EA041257B1

EA041257B1 - METHOD FOR COMPUTING INSTRUMENT ORIENTATION IN SPACE

Info

Publication number: EA041257B1
Application number: EA201900576
Authority: EA
Inventors: Арсений Леонидович Морозов; Равиль Рашидович Нигматуллин; Алексей Юрьевич Вдовин; Максим Леонидович Тарасов; Станислав Викторович Христофоров; Глеб Германович Масленников
Original assignee: Общество с ограниченной ответственностью "МИКС"
Filing date: 2019-12-04
Publication date: 2022-09-30

Description

Область техникиTechnical field

Настоящее техническое решение относится к области вычислительной техники, в частности, к области измерительной техники и предназначено для вычисления ориентации объекта в пространстве.The present technical solution relates to the field of computer technology, in particular, to the field of measuring technology and is intended to calculate the orientation of an object in space.

Уровень техникиState of the art

Задача трехосевых датчиков угловой скорости, а именно гироскопов, измерять изменение угла со временем. Интегрируя по времени эти измерения получают ориентацию.The task of three-axis angular velocity sensors, namely gyroscopes, is to measure the change in angle over time. By integrating over time, these measurements obtain an orientation.

Гироскопы на базе микроэлектромеханических сенсоров (МЭМС) имеют высокий уровень случайных шумов и сдвиги нуля, меняющиеся со временем. Интегрирование этих погрешностей приводит к быстрому возрастанию ошибки, что делает такой подход непригодным для большинства задач.Gyroscopes based on microelectromechanical sensors (MEMS) have a high level of random noise and zero shifts that change with time. Integration of these errors leads to a rapid increase in the error, which makes this approach unsuitable for most problems.

Для компенсации этих ошибок и уточнения измерений вводят, дополнительно к гироскопам, трехосевые акселерометры.To compensate for these errors and to refine measurements, three-axis accelerometers are introduced in addition to gyroscopes.

Акселерометр измеряет кажущееся ускорение, которое является векторной разностью ускорения движения и ускорения свободного падения. При отсутствии движения кажущееся ускорение равно ускорению свободного падения, ориентация которого известна.The accelerometer measures apparent acceleration, which is the vector difference between the acceleration of motion and the acceleration of free fall. In the absence of motion, the apparent acceleration is equal to the acceleration due to gravity, the orientation of which is known.

Таким образом, в состоянии покоя акселерометр можно использовать для вычисления ориентации, относительно ускорения свободного падения. Однако акселерометры, как и гироскопы, подвержены воздействию случайных шумов, а также во время движения кажущееся ускорение перестает быть равно ускорению свободного падения, так как появляется составляющая ускорения движения.Thus, at rest, the accelerometer can be used to calculate the orientation, relative to the gravitational acceleration. However, accelerometers, like gyroscopes, are subject to random noise, and during movement, the apparent acceleration ceases to be equal to the acceleration of free fall, since a component of the acceleration of movement appears.

Задачей вычисления ориентации на базе МЭМС акселерометров и гироскопов является оптимальная фильтрация измерений от обоих источников информации - акселерометров и гироскопов - для получения точного моментального значения ориентации. Так же, с практической точки зрения, желательно, чтобы решение данной задачи требовало минимального количества вычислений и минимальной частоты дискретизации датчиков.The goal of attitude calculation based on MEMS accelerometers and gyroscopes is to optimally filter measurements from both sources of information - accelerometers and gyroscopes - to obtain an accurate instantaneous attitude value. Also, from a practical point of view, it is desirable that the solution of this problem requires a minimum amount of calculations and a minimum sampling rate of the sensors.

Наиболее простым и очевидным способом решения данной задачи является переключение между датчиками: если угловая скорость или ускорение движения больше некоторого заданного значения, то ориентация вычисляется по гироскопу, если меньше - по акселерометру.The simplest and most obvious way to solve this problem is to switch between sensors: if the angular velocity or acceleration of movement is greater than a certain specified value, then the orientation is calculated by the gyroscope, if less - by the accelerometer.

Недостатком данного подхода является низкая точность при малых амплитудах движения, а также при длительном движении.The disadvantage of this approach is low accuracy at small amplitudes of movement, as well as during long-term movement.

Из уровня техники известно решение, которое описывает способ определения ориентации блок инерциальных измерителей (US2O16O327396A1, опубл. 10.10.2016). В данном способе угловая скорость и кажущиеся ускорения сравниваются с пороговыми значениями и на основе этого принимаются решения об участии измерений от акселерометра и гироскопа в вычислении ориентации.A solution is known from the prior art that describes a method for determining the orientation of a block of inertial meters (US2O16O327396A1, publ. 10.10.2016). In this method, the angular velocity and apparent accelerations are compared with threshold values, and based on this, decisions are made about the participation of measurements from the accelerometer and gyroscope in the calculation of attitude.

Кроме того, на текущем уровне техники наиболее распространенными являются способы на основе фильтра Калмана.In addition, at the current level of technology, the most common methods are based on the Kalman filter.

Различные модификации фильтра Калмана широко применяются в коммерческих продуктах:Various modifications of the Kalman filter are widely used in commercial products:

MTi-1 Series, MTi-10 Series, MTi-100 Series, MTi-600 Series от Xsens [1];MTi-1 Series, MTi-10 Series, MTi-100 Series, MTi-600 Series by Xsens [1];

Ellipse Micro Series, Ellipse 2 Series, Ekinox Series, Apogee Series от SBG-Systems [2];Ellipse Micro Series, Ellipse 2 Series, Ekinox Series, Apogee Series from SBG-Systems [2];

VN-100/110, VN-200/210, VN-300/310 от Vectornav [3];VN-100/110, VN-200/210, VN-300/310 from Vectornav [3];

Bosch Sensortec FusionLib от Bosch-Sensortec [4];Bosch Sensortec FusionLib from Bosch-Sensortec [4];

Embedded Motion Driver от Invensense [5] и др.Embedded Motion Driver from Invensense [5], etc.

Данный подход позволяет точно вычислить ориентацию, однако имеет следующие недостатки:This approach allows accurate orientation calculation, but has the following disadvantages:

он создает большую вычислительную нагрузку на процессор, что ведет к повышению энергопотребления и увеличению стоимости всего изделия, так как требуется достаточно мощный процессор;it creates a large computational load on the processor, which leads to an increase in power consumption and an increase in the cost of the entire product, since a sufficiently powerful processor is required;

необходима высокая частота дискретизации измерений датчиков, что также ограничивает применимость данного способа по следующим причинам:a high sampling rate of sensor measurements is required, which also limits the applicability of this method for the following reasons:

а) с увеличением частоты дискретизации увеличивается энергопотребление и датчика, и процессора, который его опрашивает;a) with an increase in the sampling rate, the power consumption of both the sensor and the processor that polls it increases;

б) некоторые, в частности, высокотемпературные датчики работают только на низкой частоте дискретизации, недостаточной для корректной работы способов на основе фильтра Калмана;b) some, in particular, high-temperature sensors, work only at a low sampling rate, which is insufficient for the correct operation of methods based on the Kalman filter;

в) с повышением частоты дискретизации увеличивается уровень шумов датчика, что ведет к снижению точности вычисления ориентации.c) as the sampling frequency increases, the noise level of the sensor increases, which leads to a decrease in the accuracy of the orientation calculation.

Известен способ вычисления ориентации с использованием комплиментарного фильтра и метода наименьших квадратов, описанный в патенте US 7089148B1, опубл. 16.10.2004. Недостатком этого способа является аппроксимация методом наименьших квадратов, что создает большую вычислительную нагрузку на процессор.A known method for calculating orientation using a complementary filter and the least squares method is described in US Pat. No. 7,089,148B1, publ. October 16, 2004. The disadvantage of this method is the least squares approximation, which creates a large computational load on the processor.

Наиболее близким к заявленному решению является способ, раскрытый в патенте US 9068843B1, опубл. 30.06.2015, а также в отчете Себастьяна О. X. Маджвика (Sebastian О. Н. Madgwick) An Efficient Orientation Filter for Inertial and Inertial/Magnetic Sensor Arrays. Report x-io and University of Bristol (UK).Closest to the claimed solution is the method disclosed in US patent 9068843B1, publ. 06/30/2015, as well as in the report by Sebastian O. X. Madgwick (Sebastian O. N. Madgwick) An Efficient Orientation Filter for Inertial and Inertial/Magnetic Sensor Arrays. Report x-io and University of Bristol (UK).

Данный способ основан на использовании метода градиентного спуска для вычисления ориентации по акселерометру и комплиментарного фильтра для объединения вычислений ориентации по акселерометру и гироскопу. Данный способ является одним из наиболее эффективных с точки зрения требований к вычислительным ресурсам.This method is based on using the gradient descent method to calculate the orientation from the accelerometer and a complementary filter to combine the calculations of the orientation from the accelerometer and the gyroscope. This method is one of the most efficient in terms of requirements for computing resources.

- 1 041257- 1 041257

Недостатком данного подхода является низкая скорость сходимости результатов вычисления ориентации в следующих случаях:The disadvantage of this approach is the low convergence rate of the orientation calculation results in the following cases:

если угловая скорость выходит за диапазон измерения датчиков;if the angular velocity is outside the measurement range of the sensors;

если реальная ориентация успевает измениться между измерениями датчиков;if the real orientation has time to change between sensor measurements;

при перезагрузке прибора.when rebooting the device.

Для того, чтобы скорость сходимости была приемлемой, ставят датчики с более высоким диапазоном и увеличивают частоту дискретизации. Повышение диапазона измерений ведет к снижению точности, увеличение частоты дискретизации ведет к повышению уровня шумов и энергопотребления.In order for the convergence rate to be acceptable, sensors with a higher range are installed and the sampling rate is increased. Increasing the measurement range leads to a decrease in accuracy, increasing the sampling frequency leads to an increase in the noise level and power consumption.

Кроме того, не все результаты вычисления ориентации, которые появляются на выходе алгоритма, соответствуют реальной ориентации, так как при каждом изменении реальной ориентации требуется какое-то количество итераций алгоритма и, соответственно, отсчетов, чтобы обеспечить сходимость вычисляемой ориентации.In addition, not all the results of the orientation calculation that appear at the output of the algorithm correspond to the real orientation, since with each change in the real orientation, a certain number of iterations of the algorithm and, accordingly, samples are required to ensure the convergence of the calculated orientation.

Другим недостатком данного способа является то, что коэффициент β регулирует две характеристики - разрешающую способность и скорость сходимости вычислений - что ведет к невозможности удовлетворить требования к обеим характеристикам сразу.Another disadvantage of this method is that the coefficient β regulates two characteristics - the resolution and the rate of convergence of calculations - which leads to the inability to satisfy the requirements for both characteristics at once.

Сущность изобретенияThe essence of the invention

Технической проблемой, на решение которой направлено заявленное техническое решение, является создание компьютерно-реализуемого способа вычисления ориентации прибора в пространстве, который охарактеризован в независимом пункте формулы.The technical problem to be solved by the claimed technical solution is the creation of a computer-implemented method for calculating the orientation of the device in space, which is described in an independent claim.

Технический результат заключается в разработке такого способа вычисления ориентации на основе измерений микроэлектромеханических датчиков акселерометров и гироскопов, который преодолеет недостатки известных из уровня техники решений, т.е. потребует меньших вычислительных ресурсов; обеспечит сходимость вычислений за один отсчет; точность результата вычислений не будет зависеть от диапазона измерений и частоты дискретизации датчиков.The technical result consists in the development of such a method for calculating the orientation based on measurements of microelectromechanical sensors of accelerometers and gyroscopes, which will overcome the shortcomings of the solutions known from the prior art, i.e. will require less computing resources; ensure the convergence of calculations in one reading; the accuracy of the calculation result will not depend on the measurement range and sampling frequency of the sensors.

В предпочтительном варианте реализации заявлен компьютерно-реализуемый способ вычисления ориентации прибора в пространстве, включающий в себя этапы, на которых:In a preferred embodiment, a computer-implemented method for calculating the orientation of a device in space is claimed, including the steps of:

а) получают измерения акселерометра и гироскопа;a) get accelerometer and gyroscope measurements;

б) осуществляют вычисление кватерниона, описывающего скорость изменения ориентации в земной системе координат;b) carry out the calculation of the quaternion describing the rate of change of orientation in the earth's coordinate system;

в) осуществляют вычисление оценки текущей ориентации путем интегрирования кватерниона;c) calculating the estimate of the current orientation by integrating the quaternion;

г) выполняют нормировку оценки текущей ориентации;d) normalize the estimation of the current orientation;

д) осуществляют вычисление градиента целевой функции, причем при первой корректировке для вычисления градиента используется нормированная оценка ориентации, полученная в пункте г) при последующих корректировках, для вычисления градиента используется нормированная ориентация, вычисленная в пункте и) при предыдущих корректировках;e) calculating the gradient of the objective function, wherein during the first correction, the normalized orientation estimate obtained in step d) is used to calculate the gradient, with subsequent corrections, the normalized orientation calculated in step i) is used for previous corrections to calculate the gradient;

е) выполняют нормировку градиента;f) performing gradient normalization;

ж) осуществляют вычисление корректировочной ориентации путем умножения нормированного градиента на шаговый коэффициент;g) calculating corrective orientation by multiplying the normalized gradient by a step factor;

з) выполняют корректировку ориентации путем вычитания из вычисленной ориентации корректировочной ориентации, причем при первой корректировке в качестве вычисленной ориентации используется нормированная оценка ориентации, полученная в пункте г) при последующих корректировках, в качестве вычисленной ориентации используется нормированная ориентация, вычисленная в пункте и) при предыдущих корректировках;h) the orientation is corrected by subtracting the corrective orientation from the calculated orientation, and during the first correction, the normalized orientation estimate obtained in paragraph d) is used as the calculated orientation, during subsequent adjustments, the normalized orientation calculated in paragraph i) is used as the calculated orientation, at the previous adjustments;

и) выполняют нормировку откорректированной ориентации;i) normalizing the corrected orientation;

к) проверяют условие превышения заранее заданного порогового значения корректировочной ориентации;j) checking a condition for exceeding a predetermined correction orientation threshold;

л) осуществляют проверку условия превышения номера текущей ориентации максимального количества корректировок;k) checking the condition for exceeding the number of the current orientation of the maximum number of adjustments;

м) выдают нормированное значение ориентации на выход алгоритма в случае выполнения хотя бы одного из условий к) или м); повтор пунктов с д) по л) до тех пор, пока не выполнится хотя бы одно из условий.l) give a normalized value of the orientation to the output of the algorithm in case at least one of the conditions j) or m); repeat steps e) to k) until at least one of the conditions is met.

Описание чертежейDescription of drawings

Реализация изобретения будет описана в дальнейшем в соответствии с прилагаемыми чертежами, которые представлены для пояснения сути изобретения и никоим образом не ограничивают область изобретения.The implementation of the invention will be described hereinafter in accordance with the accompanying drawings, which are presented to explain the essence of the invention and in no way limit the scope of the invention.

К заявке прилагаются следующие чертежи:The following drawings are attached to the application:

Фиг. 1 иллюстрирует оси измерений;Fig. 1 illustrates the measurement axes;

фиг. 2 иллюстрирует упрощенную схему заявленного способа.fig. 2 illustrates a simplified diagram of the claimed method.

Детальное описание изобретенияDetailed description of the invention

В приведенном ниже подробном описании реализации изобретения приведены многочисленные детали реализации, призванные обеспечить отчетливое понимание настоящего изобретения. Однако квалифицированному в предметной области специалисту будет очевидно, каким образом можно использоIn the following detailed description of the implementation of the invention, numerous implementation details are provided to provide a clear understanding of the present invention. However, it will be obvious to a person skilled in the subject area how to use

- 2 041257 вать настоящее изобретение, как с данными деталями реализации, так и без них. В других случаях хорошо известные методы, процедуры и компоненты не были описаны подробно, чтобы не затруднять излишне понимание особенностей настоящего изобретения.- 2 041257 the present invention, both with and without these implementation details. In other instances, well-known methods, procedures, and components have not been described in detail so as not to unnecessarily obscure the features of the present invention.

Кроме того, из приведенного изложения будет ясно, что изобретение не ограничивается приведенной реализацией. Многочисленные возможные модификации, изменения, вариации и замены, сохраняющие суть и форму настоящего изобретения, будут очевидными для квалифицированных в предметной области специалистов.Furthermore, it will be clear from the foregoing that the invention is not limited to the present implementation. Numerous possible modifications, changes, variations and substitutions that retain the spirit and form of the present invention will be apparent to those skilled in the subject area.

Для достижения указанного выше технического результата в качестве настоящего изобретения предлагается способ обработки измерений инерциальных микроэлектромеханических акселерометров и гироскопов для вычисления ориентации в пространстве.To achieve the above technical result, as the present invention, a method for processing measurements of inertial microelectromechanical accelerometers and gyroscopes to calculate orientation in space is proposed.

Первым этапом в заявленном решении является получение измерений акселерометра и гироскопа.The first step in the claimed solution is to obtain accelerometer and gyroscope measurements.

После чего на основе измерений гироскопа вычисляется оценка текущей ориентации. С помощью оценки текущей ориентации и измерений акселерометра вычисляется корректировочная ориентация, которая является произведением шагового коэффициента и градиент целевой функции. Целевая функция более подробно будет описана в заявленном решении ниже.Then, based on the gyroscope measurements, an estimate of the current orientation is calculated. Using the current orientation estimate and accelerometer measurements, a corrective orientation is calculated, which is the product of the step factor and the gradient of the objective function. The objective function will be described in more detail in the claimed solution below.

Далее полученная ранее оценка текущей ориентации корректируется путем вычитания корректировочной ориентации. После этого проверяются условия прекращения корректировки путем проверки выполнения двух условий:Next, the previously obtained estimate of the current orientation is corrected by subtracting the corrective orientation. After that, the conditions for stopping the adjustment are checked by checking whether two conditions are met:

меньше ли корректировочная ориентация заранее заданного порогового значения (первое условие); и превысило ли количество корректировок некоторое заранее заданное максимальное количество корректировок (второе условие).whether the correction orientation is less than a predetermined threshold value (first condition); and whether the number of adjustments exceeded some predetermined maximum number of adjustments (second condition).

Если ни одно из условий не выполнилось, то вычисление корректировочной ориентации и корректировка повторяются снова.If none of the conditions is met, then the calculation of the corrective orientation and the correction are repeated again.

На всех корректировках, кроме первой, вместо оценки ориентации, полученной с помощью измерений гироскопа, используется откорректированная ориентация, вычисленная на предыдущей корректировке.On all but the first adjustments, instead of the orientation estimate obtained from the gyroscope measurements, the corrected orientation calculated on the previous adjustment is used.

Если выполнилось хотя бы одного из условий прекращения корректировки, то корректировка завершается. Полученный результат поступает на выход алгоритма.If at least one of the conditions for terminating the adjustment is met, then the adjustment ends. The result obtained is fed to the output of the algorithm.

Особенностью заявленного решения является то, что в данном алгоритме отсутствуют требования к частоте дискретизации датчиков, поэтому датчик может подбираться исходя из других, более важных характеристик, таких как, например: точность, размер, стоимость и т.д.A feature of the proposed solution is that this algorithm does not require the sampling rate of the sensors, so the sensor can be selected based on other, more important characteristics, such as, for example: accuracy, size, cost, etc.

Ещё одной особенностью заявленного решения является то, что при превышении угловой скорости диапазона измерений гироскопа не потребуется дополнительных отсчетов, чтобы обеспечить сходимость вычислений. Сходимость обеспечивается за счет итеративных корректировок внутри алгоритма.Another feature of the proposed solution is that when the angular velocity exceeds the measurement range of the gyroscope, no additional readings are required to ensure the convergence of the calculations. Convergence is ensured by iterative adjustments within the algorithm.

Ещё одной особенностью этого способа является то, что шаговый коэффициент задается как константа. Значение этой константы выбирается эмпирически исходя из того, позволяет ли выбранный коэффициент добиться требуемой точности.Another feature of this method is that the step factor is given as a constant. The value of this constant is chosen empirically based on whether the selected coefficient allows achieving the required accuracy.

Ещё одной особенностью заявленного решения является то, что максимально возможное количество итераций градиентного спуска выбирается исходя из вычислительной мощности процессора и из скорости сходимости алгоритма с выбранным шаговым коэффициентом.Another feature of the proposed solution is that the maximum possible number of gradient descent iterations is selected based on the processing power of the processor and on the convergence rate of the algorithm with the selected step coefficient.

Ещё одной особенностью этого способа является то, что пороговое значение условия завершения корректировок выбирается исходя из требуемого разрешения по ориентации.Another feature of this method is that the threshold value of the completion condition for corrections is selected based on the required orientation resolution.

Опционально допускается исключение проверки на превышение количества совершенных корректировок некоторого заранее заданного максимального количества корректировок. В таком случае корректировка будет происходить до тех пор, пока корректировочная ориентация не станет меньше некоторого заранее заданного порогового значения.Optionally, it is allowed to exclude the check for exceeding the number of completed adjustments of some predetermined maximum number of adjustments. In such a case, the adjustment will continue until the adjustment orientation is less than some predetermined threshold value.

Опционально допускается, чтобы шаговый коэффициент задавался переменной, которая увеличивается, если корректировочная ориентация больше некоторого дополнительного порога (для увеличения скорости вычислений); уменьшается, если корректировочная ориентация меньше некоторого другого дополнительного порога (для увеличения точности вычисленной ориентации).Optionally, it is allowed that the step factor be given by a variable that increases if the corrective orientation is greater than some additional threshold (to increase the speed of calculations); decreases if the corrective orientation is less than some other additional threshold (to increase the accuracy of the computed orientation).

В заявленном решении для представления ориентации используются углы Эйлера, матрицы поворота и кватернионы.The claimed solution uses Euler angles, rotation matrices, and quaternions to represent orientation.

При вычислении углов Эйлера и матриц поворота используются прямые и обратные тригонометрические функции, которые требуют большое количество математических операций.When calculating Euler angles and rotation matrices, direct and inverse trigonometric functions are used, which require a large number of mathematical operations.

Вычисление кватерниона, описывающего ориентацию в пространстве, требует примерно в 100 раз меньшее количество математических операций.Computing a quaternion describing orientation in space requires about 100 times fewer mathematical operations.

Многие существующие способы для вычисления ориентации используют кватернионы. При необходимости, после вычисления кватерниона, описывающего ориентацию, этот кватернион может быть преобразован в углы Эйлера или матрицу поворота.Many existing methods for calculating orientation use quaternions. If necessary, after calculating the quaternion describing the orientation, this quaternion can be converted into Euler angles or a rotation matrix.

В заявленном решении, с целью минимизации требований к вычислительным ресурсам, для вычисления ориентации также используются кватернионы. Математика кватернионов хорошо известна и не рассматривается в данном изобретении. Поэтому известные формулы, связанные с операциями междуIn the claimed solution, in order to minimize the requirements for computing resources, quaternions are also used to calculate the orientation. The mathematics of quaternions is well known and is not covered in this invention. Therefore, the well-known formulas associated with operations between

- 3 041257 кватернионами, даны без вывода.- 3 041257 quaternions, given without output.

Ориентация прибора показывает, на сколько оси ортогонального базиса прибора (далее оси прибора) отклонены от осей земной системы координат. Если развернуть прибор так, чтобы его оси совпали с осями земной системой координат, то ось Z прибора будет направлена вертикально вверх, от Земли, а оси X и Y будут направлены туда, куда были направлены оси X и Y прибора в момент начала измерений. При этом необязательно, чтобы на момент начала измерений ось Z прибора совпадала с осью Z земной системной координат (начальное положение осей X и Y прибора определяет направление осей X и Y земной системы координат).The orientation of the device shows how much the axes of the orthogonal basis of the device (hereinafter referred to as the axes of the device) deviate from the axes of the earth's coordinate system. If you turn the device so that its axes coincide with the axes of the Earth's coordinate system, then the Z axis of the device will be directed vertically upwards, from the Earth, and the X and Y axes will be directed to where the X and Y axes of the device were directed at the moment the measurements were started. At the same time, it is not necessary that, at the time of the start of measurements, the Z axis of the device coincides with the Z axis of the earth system coordinates (the initial position of the X and Y axes of the device determines the direction of the X and Y axes of the earth coordinate system).

Для осуществления заявленного способа необходимы измерения трех-осевого акселерометра и трех-осевого гироскопа или с набора акселерометров и гироскопов, обеспечивающих измерения по трем ортогональным осям. Измерения акселерометра и гироскопа должны быть приведены к осям прибора. Взаимное расположение осей акселерометра и гироскопа показано на фиг. 1.To implement the claimed method, it is necessary to measure a three-axis accelerometer and a three-axis gyroscope or from a set of accelerometers and gyroscopes that provide measurements along three orthogonal axes. Measurements of the accelerometer and gyroscope must be brought to the axes of the instrument. The mutual arrangement of the axes of the accelerometer and the gyroscope is shown in Fig. 1.

Кватернион, описывающий текущее значение ориентации, обозначается ^D^'. Верхний индекс в обозначении указывает систему координат, относительно которой задан кватернион, нижний индекс указывает опорную систему координат. Индекс S обозначает систему координат прибора, а индекс D земную систему координат.The quaternion describing the current orientation value is denoted by ^D ^'. The superscript in the notation indicates the coordinate system relative to which the quaternion is specified, the subscript indicates the reference coordinate system. Index S denotes the instrument coordinate system, and index D denotes the earth coordinate system.

В заявленном решении способ начинается с получения измерений акселерометра и гироскопа. Вектора измерений акселерометра и гироскопа, выраженные в форме кватернионов ma и т_ю соответственно, выглядят следующим образом:In the claimed solution, the method begins with obtaining accelerometer and gyroscope measurements. The measurement vectors of the accelerometer and gyroscope, expressed in the form of quaternions ma and t _u , respectively, are as follows:

т_а = [0 ах ay az] т_о = [0 ωχ ωγ ωζ]t _a = [0 ah ay az] t _o = [0 ωχ ωγ ωζ]

Следующим этап осуществляют вычисление оценки текущей ориентации прибора в пространстве с помощью измерений гироскопа. Для этого необходимо вычислить кватернион °^, описывающий скорость изменения ориентации в земной системе координат. Точка над обозначением кватерниона показывает, что данный кватернион описывает скорость.The next step is to calculate an estimate of the current orientation of the device in space using gyroscope measurements. To do this, it is necessary to calculate the quaternion °^, which describes the rate of change in orientation in the earth's coordinate system. A dot above a quaternion indicates that the quaternion describes speed.

**

Кватернион вычисляется согласно известному выражению:The quaternion is calculated according to the well-known expression:

₍₂₎ где - это кватернион, описывающий текущую ориентацию прибора в пространстве. ^sa ^sa ₍₂₎ where is a quaternion describing the current orientation of the device in space. ^s a ^s a

Оценка ориентации в момент времени t вычисляется путем интегрирования по времени:The orientation estimate at time t is calculated by integrating over time:

йшл = Йе-I + Й △ t = DQt~i +1 Йг-ι 0 A t (3)yshl \u003d Ye-I + Y △ t \u003d DQt~i +1 Yg-ι 0 A t (3)

Вычисленная оценка ориентации о⁹®,' нормируется к единице.The calculated estimate of the orientation about ⁹ ®,' is normalized to one.

Акселерометр измеряет кажущееся ускорение, которое является разностью ускорения движения и ускорения свободного падения.The accelerometer measures apparent acceleration, which is the difference between the acceleration of motion and the acceleration of free fall.

В моменты, когда прибор неподвижен, акселерометр измеряет вектор ускорения свободного падения. Координаты вектора ускорения свободного падения известны в земной системе координат, таким образом можно вычислить положение системы координат прибора относительной земной системы коор динат.When the device is stationary, the accelerometer measures the gravitational acceleration vector. The coordinates of the gravitational acceleration vector are known in the terrestrial coordinate system, thus it is possible to calculate the position of the instrument coordinate system relative to the terrestrial coordinate system.

В предложенном способе это решается задачей оптимизации. Известная ориентация ускорения свободного падения в земной системе координат, выраженная через кватернион преобразуется в систему координат прибора с помощью искомой ориентации прибора оЧ, что должно быть равно измерению акселерометра ^т°In the proposed method, this is solved by an optimization problem. The known orientation of the gravitational acceleration in the earth's coordinate system, expressed in terms of the quaternion, is converted into the instrument's coordinate system using the desired orientation of the instrument oCh, which should be equal to the accelerometer measurement ^t °

D? ®^Eg® D<i-m _α ^χβ(4) *g = [0 ^{0 0} Ч(5) ^sa является решением уравнения minf^q^g^a)(6)D? ® ^E g® D<im _α ^χ β(4) *g = [0 ^{0 0} P(5) ^s a is the solution of the equation minf^q^g^a)(6)

Для решения задачи оптимизации используется метод градиентного спуска, так как является одним из наиболее эффективных. Функция (4) выступает в качестве целевой функции. Согласно этому методу ориентация вычисляется с помощью уравнения:To solve the optimization problem, the gradient descent method is used, as it is one of the most effective. Function (4) acts as an objective function. According to this method, the orientation is calculated using the equation:

Й = Й-, -тй-р = 1,2,3.JV(7) где β является шаговым коэффициентом, регулирующим скорость сходимости и разрешение по ориентации, - градиент целевой функции, N - максимальное количество итераций. Градиент W вычисляется согласно уравнениям:J = J-, -ty-r = 1,2,3.JV(7) where β is a step factor that regulates the rate of convergence and orientation resolution, is the gradient of the objective function, N is the maximum number of iterations. Gradient W is calculated according to the equations:

- 4 041257 mil, ^Eg,ma) = J^TCDl, ^Eg)f(ll, ^Eg,m_a) (8) где ^1^ &- 4 041257 mil, ^E g,ma) = J ^T CDl, ^E g)f(ll, ^E g,m _a ) (8) where ^1^ &

2(ад₄-ад)-«^х 2 (hell ₄ -hell) - " ^x fdi, ^Eg,m_a)=fdi, ^E g,m _a )= 2(ад₂+ад₄)-«Т2 (hell ₂ + hell ₄ ) - "T 2(|-q² ₂-qb- az2(|-q ² ₂ -qb-az 2^з 2^₄ 2^ -2^3 2^ ₄ 2^ - mil^Eg)= 2q₂ 2q_t 2q> :mil ^E g)= 2q ₂ 2q _t 2q> : I I 0 -4^₂ -4^з0 -4^ ₂ -4^3 - Якобиан целевой - Jacobian Target функции, functions,

2q₂ (9) (10), q₁, q₂, q₃, q₄ являются элементами кватерниона (И) нормируется. Выражение (12) является корректировочной ориентацией2q ₂ (9) (10), q ₁ , q ₂ , q ₃ , q ₄ are elements of the quaternion (AND) is normalized. Expression (12) is a correction orientation

О^ = [91 4з q^·O^ = [91 43 q^

Таким образом, полное выражения для всех элементовSo the complete expression for all elements

V/ = [sl 52 53 54]:V/ = [sl 52 53 54]:

^J1 = 4^² + 2q3ax + 4^² - 2q2ay ^2 = 4^² - 2q,ax + 4^¾ - 2qxay - 4q2 + 8^² + 8^² + 4^2az ^3 = ^q₃ + 2q_xax + 4^₄ ² - 2^4ay - 4#3 + 8^² + 8^3^ + 4q₃az 54 = 4^²^ + 2q₂ax + 4<^₄ - 2^₃oy ^J 1 = 4^ ² + 2q3ax + 4^ ² - 2q2ay ^2 = 4^ ² - 2q,ax + 4^¾ - 2qxay - 4q2 + 8^ ² + 8^ ² + 4^2az ^3 = ^q ₃ + 2q _x ax + 4^ ₄ ² - 2^4ay - 4#3 + 8^ ² + 8^3^ + 4q ₃ az 54 = 4^ ² ^ + 2q ₂ ax + 4<^ ₄ - 2^ ₃ oy

Шаговый коэффициент β может задаваться как переменная: большее значение, если т велико, для ускорения сходимости, меньшее значение, если Vf мало, для увеличения разрешения вычисляемой ориентации.The step factor β can be set as a variable: a larger value, if m is large, to speed up convergence, a smaller value, if Vf is small, to increase the resolution of the calculated orientation.

В предложенном способе при первой итерации градиентного спуска вместо предыдущего значения ориентации используется оценка ориентации, полученная с помощью измерений гироскопа:In the proposed method, at the first iteration of gradient descent, instead of the previous orientation value, the orientation estimate obtained using gyroscope measurements is used:

(13)(13)

В заявленном решении с помощью уравнения (12) вычисляется корректировочная ориентация, а с помощью уравнения (14) осуществляется корректировка (уравнение (14) является следствием подстановки (12) в (7). Таким образом, одна корректировка ориентации является одной итерацией градиентного спуска):In the claimed solution, using equation (12), an orientation correction is calculated, and using equation (14), an adjustment is made (equation (14) is a consequence of substituting (12) into (7). Thus, one orientation adjustment is one iteration of gradient descent) :

D^i “ D^i-l D^errj^ ~ 1,2,3. JV_dIq — _ΰΙ_ω (14)D^i “ D^il D^errj^ ~ 1,2,3. JV _d Iq — _ΰ Ι _ω (14)

Откорректированная ориентация нормируется.The corrected orientation is normalized.

Корректировка будет происходить до тех пор, пока i не станет равно N, где N - максимальное количество корректировок. Для минимизации вычислений введено дополнительное условие, которое позволяет прекратить корректировку ориентации, прежде чем i станет равно N. Этим дополнительным условием прекращения корректировки является требование того, чтобы было меньше некоторого заранее заданного порогового значения ε:The adjustment will continue until i equals N, where N is the maximum number of adjustments. To minimize the calculations, an additional condition is introduced that allows the orientation correction to be stopped before i becomes equal to N. This additional condition for the termination of the correction is the requirement that it be less than some predetermined threshold value ε:

(15)(15)

Корректировка будет происходить до тех пор, пока не выполнится одно из условий (16):The adjustment will take place until one of the conditions (16) is met:

либо количество совершенных корректировок станет равно максимальному количеству корректировок N, либо корректировочная ориентации станет меньше порогового значения ε . ε задается исходя из необходимой точности вычисления ориентации.either the number of adjustments made will become equal to the maximum number of adjustments N, or the adjustment orientation will become less than the threshold value ε . ε is set based on the required accuracy of the orientation calculation.

(> = 5)ν(Χ,,<_ε) (16)(> = 5)ν(Χ,,< _ε ) (16)

Упрощенная блок-схема предложенного компьютерно-реализуемого способа вычисления ориентации в пространстве (200) представлена на фиг. 2:A simplified block diagram of the proposed computer-implemented method for calculating orientation in space (200) is shown in FIG. 2:

На этапе (201) получают измерения акселерометра и гироскопа. После чего на этапе (202) осуществляют вычисление оценки текущей ориентации на основе измерений гироскопа.In step (201), accelerometer and gyroscope measurements are obtained. Then, at step (202), the estimation of the current orientation is calculated based on the measurements of the gyroscope.

Далее на этапе (203) выполняют вычисление корректировочной ориентации с помощью измерений акселерометра. После чего на этапе (204) выполняют корректировку ориентации.Next, in step (203), corrective orientation calculation is performed using accelerometer measurements. Thereafter, at step (204), an orientation correction is performed.

И на этапе (205) проверяют условие прекращение корректировки и выдают результат откорректированной ориентации (206).And, at step (205), the correction stop condition is checked and the corrected orientation result is output (206).

Заявленный способ не ограничивается компьютерной реализацией аппаратной части и может быть реализован как с использованием компьютеров, так и в средствах микропроцессорной или иной вычислительной техники, главным требованием к которым будет являться способность ввода измерений акселерометров и гироскопов и произведение описанных вычислений.The claimed method is not limited to computer implementation of the hardware and can be implemented both using computers and in microprocessor or other computer equipment, the main requirement for which will be the ability to enter measurements of accelerometers and gyroscopes and perform the described calculations.

В настоящих материалах заявки было представлено предпочтительное раскрытие осуществление заявленного технического решения, которое не должно использоваться как ограничивающее иные, частные воплощения его реализации, которые не выходят за рамки испрашиваемого объема правовой охраныIn these application materials, a preferred disclosure of the implementation of the claimed technical solution was presented, which should not be used as limiting other, private embodiments of its implementation that do not go beyond the requested scope of legal protection.

- 5 041257 и являются очевидными для специалистов в соответствующей области техники.- 5 041257 and are obvious to specialists in the relevant field of technology.

СсылкиLinks

[1] Сайт компании Xsens - https://www.xsens.com/.[1] Xsens website - https://www.xsens.com/.

[2] Сайт компании SBG-Systems - https://www.sbg-systems.com/.[2] SBG-Systems website - https://www.sbg-systems.com/.

[3] Сайт компании Vectornav - https://www.vectomav.com.[3] Vectornav website - https://www.vectomav.com.

[4] Сайт компании Bosch-sensortec - https://www.bosch-sensortec.com/.[4] Bosch-sensortec website - https://www.bosch-sensortec.com/.

[5] Сайт компании Invensense - https://www.invensense.com/.[5] Invensense website - https://www.invensense.com/.

[6] Sebastian О. Н. Madgwick An Efficient Orientation Filter for Inertial and Inertial/Magnetic Sensor Arrays. Report x-io and University of Bristol (UK), 30.04.2010.[6] Sebastian O. N. Madgwick An Efficient Orientation Filter for Inertial and Inertial/Magnetic Sensor Arrays. Report x-io and University of Bristol (UK), 30.04.2010.

Claims

CLAIM

A computer-implemented method for calculating orientation in space, executing on a computing device, containing a processor, through which the steps are carried out, at which:

a) get accelerometer and gyroscope measurements;

b) determine the quaternion describing the rate of change of orientation in the earth's coordinate system by measuring the gyroscope;

c) determining the assessment of the current orientation by integrating the quaternion;

d) normalize the estimation of the current orientation;

e) determine the gradient of the objective function by measuring the accelerometer, and at the first correction, the normalized orientation estimate obtained in item d) is used to determine the gradient, with subsequent adjustments, the normalized orientation obtained in step i) is used for the previous corrections to determine the gradient;

f) performing gradient normalization;

g) determining corrective orientation by multiplying the normalized gradient by a step factor;

h) the orientation is corrected by subtracting the corrective orientation from the obtained orientation, and during the first correction, the normalized orientation estimate obtained in paragraph d) is used as the received orientation, during subsequent adjustments, the normalized orientation obtained in paragraph i) is used as the received orientation, in the previous adjustments;

i) normalizing the corrected orientation;

j) checking a condition for exceeding a predetermined correction orientation threshold;

l) give a normalized value of the orientation to the output of the algorithm in case at least one of the conditions j) or m); repeat steps e) to k) until at least one of the conditions is met.