CC BY
100
УДК 656.6+523
А.С. Конаков, В.В. Шаврин, В.И. Тисленко, А.А. Савин
Влияние начальных условий на среднеквадратическое отклонение оценок координат в бесплатформенной инерциальной навигационной системе при кватернионном методе перехода между базисами
Исследованы статистические зависимости СКО оценок координат от СКО начальных условий и абсолютных значений угловых скоростей и кажущихся ускорений, произведено математическое моделирование алгоритма оценки координат в бесплатформенной инерциальной навигационной системе при различных СКО начальных условий.
Ключевые слова: бесплатформенная инерциальная навигационная система, кватернионы, погрешность, среднеквадратическое отклонение.
Введение и постановка задачи. Необходимость в точном автономном решении навигационной задачи характерна для многих практических приложений. Одна из класса подобных систем - бесплатформенная инерциальная навигационная система (БИНС).
Среди существенных преимуществ БИНС, по сравнению с другими системами, можно выделить малые габариты, низкое энергопотребление, невысокую стоимость, отсутствие ограничений на угловые маневры, возможность работы в любом базисе. Главным же недостатком всех систем, основанных на принципе инерции тел, является накопление погрешностей с течением времени [1], обусловленное численным решением системы стохастических дифференциальных уравнений.
Для инициализации процесса определения координат необходимо знание начальных условий, которые в силу ряда причин являются случайными. Начальные условия по положению и скорости могут быть получены с помощью спутниковой радионавигационной системы. Сложнее обстоит дело с заданием угловой ориентации тела. Для инициализации начальных значений кватерниона необходимо знать координаты положения локальной (топоцентрической) системы координат (СК) относительно навигационной и ориентацию связанной системы координат (осей датчика) относительно локальной СК. Результат решения должен быть представлен в навигационной СК. Взаимную ориентацию связанной и локальной систем координат можно определить как разницу между показаниями БИНС (с произвольно заданной ориентацией) и СРНС, выполняя постоянную коррекцию показаний БИНС.
Решение задачи. Определение координат и скорости во всех инерциальных навигационных системах основано на численном решении дифференциального уравнения сложного движения [1]
рость неинерциальной системы отсчета (НСО) относительно (ИСО) в неинерциальной системе отсчета; Vй () - линейная скорость тела относительно неинерциальной системы отсчета; ю - угловая скорость вращения НСО относительно ИСО; И - радиус-вектор, соединяющий центр масс тела с
вычисленное в ИСО; ай - линейное ускорение тела относительно НСО; е - угловое ускорение НСО
Для всех бесплатформенных систем, получающих первичную информацию в базисе, жестко связанном с телом, необходим алгоритм перехода между этим базисом и навигационным, в котором и необходимо определить координаты, интересующие потребителя. В данной статье был использо-
- линеиная ско-
(1)
центром НСО; v(?) - ускорение тела в ИСО; а0 - линейное ускорение НСО относительно ИСО,
относительно ИСО; 2 шх Vп (?) - ускорение Кориолиса; [шх[шх R]] - добавочное ускорение.
ван кватернионный алгоритм перехода между базисами, как обладающий рядом важных преимуществ, описанных в [1]:
XN = vect
QN vb f\— 1N
b 0 X 0 Q b
(2)
tN
где X - произвольный вектор (скорость, ускорение, местоположение и т.д.), координаты которого заданы в навигационном базисе (N); Xb - произвольный вектор, координаты которого заданы в базисе, жестко связанном с телом (b); QN - нормированный кватернион перехода между базисом b и
N; о - знак умножения кватернионов (в данной статье для обозначения базисов используются сокращения от соответствующих английских слов Navigation, body).
В матричном представлении формула (2) имеет вид [2]
гЬ
XN = c(qN )
Xb
где матрица С) , зависящая от кватерниона, имеет следующий вид:
з)
К о ))■
N 2. N 2 N 2 N 2
qb 0 + qb 1 - qb 2 - qb 3
ll„N qN + qN qN \
Aqb iqb 2 + qb оqb зі
li„N qN qN qN \
Цqb iqb з-qb оqb 2
qb 02 - q^ 12 + q^22 - q£з2 2q(з+qü0qbNi)
2(qN qN + qN qN \
2\qb iqb з + qb 0qb 21
l(qN qN qN qN \
2\qb 2qb з - qb 0qb l]
N 2 N 2 N 2. N 2
qb 0 -qb 1 -qb 2 +qb з
где д^1 = д^1 (?) - элементы кватерниона.
При вращательном движении объекта ориентация между связанным базисом и инерциальным изменяется. Эволюция кватерниона, связывающего базисы N и Ь, описывается кинематическим уравнением [1]
2
В итоге определение координат в БИНС при традиционном подходе предполагает решение следующей СДУ:
Q b-
(з)
Г (t)
v (t) =
^ -о О
v(0
-2[ra(t)x v(t)] + g+С( (t))a(t) 0,5 QN (t) ю()
(4)
где а(?) и ш(?) - векторы линейных ускорений и угловых скоростей в связанной СК; g - вектор гравитационного ускорения.
В реальной БИНС при интегрировании СДУ (4) в качестве а(?) и ш(?) используются соответствующие сигналы га (?) и (?) с выходов датчиков МЭМС. Математическая модель этих сигналов в непрерывном времени определена в [2,3] и имеет вид
z a (t)
_zro (t)_
Saa(?) + a(t)+ba + na (t) Srow(t) + w(t)+Ью + пю (t)
(5)
где Sa и Sro - диагональные матрицы (3x3) с элементами, определяющими масштаб сигналов датчиков по осям связанной СК, которые суммируются с величиной измеряемых параметров a(t) и ш(?) ; ba и bffl - векторы смещения нулей; na (t) и пю (?) - векторы аддитивных гауссовских возмущений.
Наличие неизвестных смещений и масштабных коэффициентов, их зависимость от температуры и времени являются, наряду с аддитивным шумом, источником дополнительной погрешности БИНС. Для ее компенсации обычно используют различные методы калибровки МЭМС [3].
В данной работе исследуется влияние СКО задания начальных условий для СДУ (4) на погрешность оценок координат, скорости и пространственной ориентации (кватерниона) объекта в условиях, когда они формируются на основе методов теории нелинейной Марковской фильтрации [5, 6]. В зарубежной литературе обычно используют термин «байесовская фильтрация» [6]. В данном случае
T
нелинейная СДУ (4) определяет динамику вектора состояния [г(?) v(t) Q(?)] , уравнения (5) при
этом задают 6-мерный вектор наблюдений. Анализ наблюдаемости динамической системы (3), (4) с использованием [5, 6] показал, что она не наблюдаема. Таким образом, даже при отсутствии аддитивных возмущений в (4) восстановление вектора состояния невозможно, что по существу ясно, исходя из структуры уравнений (4). Таким образом, применение методов теории фильтрации в лучшем случае может лишь замедлить процесс расходимости при формировании оценок координат на основе БИНС.
В качестве алгоритма нелинейной фильтрации координат и скорости объекта использовался квазиоптимальный алгоритм «сигма-точечного» фильтра Калмана [6], который имеет известные преимущества по сравнению с широко используемым алгоритмом расширенного фильтра Калмана [6]. Результаты моделирования показывают, что алгоритм чувствителен к угловым рассогласованиям. Это обусловлено тем, что последнее уравнение в (4) является автономным, при этом погрешность оценок кватерниона непосредственно зависит только от возмущений во втором уравнении (5). Также эти погрешности оказывают влияние на погрешность вычисления кажущегося ускорения в базисе N и далее после двукратного интегрирования значительно увеличивают погрешность оценок координат. Таким образом, неточность знания углового положения приводит к значительному возрастанию СКО оценок координат и скорости.
Для их уменьшения использовались разные численные методы интегрирования: метод Эйлера 1-го порядка для ускорения и скорости с шагом 10 мс и метод Рунге-Кутты 5-го порядка для решения кинематических уравнений (2).
Известно, что в случае автономной работы БИНС, состоящей из одного двухосного акселерометра и одного гироскопа, погрешность определения местоположения в локальной (топоцентриче-ской) системе координат, с учетом всех возможных источников, определена соотношением [3]
2 3 2 2
5r(?) - 5/0) + 5voA? + ЬЪоа A-+Sbog ^+50og+И)z • VAt + Soa • a^-+Sog • 5Aoz • VAt, (6)
где 5/) - ошибка определения координат в начальный момент времени ?о ; 5vo - ошибка скорости в момент ?o ; At - интервал времени с момента получения последних данных от СРНС; 5boa - ошибка смещения нуля акселерометра в момент ?o ; 5bog - ошибка смещения нуля гироскопа в момент ?o ; 50o - ошибка несовмещения осей БИНС по углам крена и тангажа с осями локальной системы координат; 5Aoz -VA? - ошибка несовмещения БИНС по углу азимута с локальной системы координат, умноженная на пройденное расстояние; Soa - масштабный коэффициент для акселерометра; Sog - масштабный коэффициент для гироскопа.
Анализ уравнения (5) приводит к выводу, что закон изменения погрешности определения координат от абсолютной величины ускорения - квадратичный. Этот вывод подтверждается моделированием. Также погрешность определения координат изменяется во времени как кубическая парабола, что тоже подтверждается результатами моделирования.
Исследование влияния СКО начальных условий на СКО погрешности определения местоположения выполнялось при малом уровне шума наблюдений (порядка io-6 град/с для угловой скорости и io-6 м/с2 для линейного ускорения). Методика моделирования состояла в том, что при изучении влияния величины СКО погрешности по заданному параметру, значения СКО по остальным параметрам фиксировались на достаточно малом уровне (1 ppm от минимального уровня СКО исследуемого параметра для относительных значений).
На рис. 1 приведены результаты моделирования. Стоит отметить, что для перехода между базисами используется нормированный кватернион, таким образом, его максимальное значение не превышает 1 и СКО начальных условий по кватерниону безразмерно. Как и ожидалось, СКО оценок погрешностей увеличивается при увеличении СКО начальных условий. Также увеличивается скорость расхождения оценок координат во времени. В каждой отдельной реализации оценки положения на выходе фильтра изменяются от заданных начальных значений и с течением времени либо остаются практически неизменными (при малом значении СКО начальных условий), либо расходятся. При СКО начальных условий переменных кватерниона больше o,o785 алгоритм не обеспечивает получения решения (ковариационная матрица перестает быть положительно определенной, из-за
чего невозможно произвести разложение Холецкого, без результатов которого «сигма-точечный» алгоритм не функционирует).
"1 —1—
■
1
1,2 1,4 1,6
СКО
Он у
10 15 20 25 0 0,4 0,8 1,2 1,4 1,6
СКОящ,, м СКОсну, м/с
а б в
Рис. 1. Зависимость среднеквадратической погрешности определения координат (в метрах) от среднеквадратических ошибок начальных условий по перемещению (а), скорости (б), нормированному значению кватерниона (в)
На практике представляет интерес зависимость СКО ошибки местоположения от СКО начальных условий по угловой ориентации (углам Эйлера) объекта. Функциональная зависимость углов Эйлера от элементов кватерниона имеет следующий вид [4]:
=f (40, 41,42, 43 ) =
arctan2(^2(q243 + 404l)],[2(43 -4041)]) ( \ 2(4143 - 4042)
- arctan
arctan2
-[2(4143 - 4042)] ([2(4142 + 4043)]
2 . 2 2 2 40 + 41 - 42 - 43
где ф - рыскание; 0 - тангаж; у - курс; arctan2(y,x) - обратный тангенс с четырьмя квадрантами;
41, 42, 43, 44 - элементы кватерниона.
Ковариационные матрицы в линейном приближении преобразуются по следующему закону:
K = FK4FT ,
где F - матрица Якоби нелинейной функции f (40,41,42,43) вычисляется в точке математического ожидания оценки кватерниона; K4 - ковариационная матрица элементов кватерниона; K - ковариационная матрица углов Эйлера (ф, 0, у).
Полученные результаты приведены в таблице.
Зависимость СКО ошибки местоположения от СКО начальных условий по углам Эйлера
СКО Q (н.у.) 0,000025 0,00005 0,0001 0,0005 0,001 0,005
СКО ф (н.у.)1 1,45E-07 2,89E-07 5,79E-07 2,80E-06 5,79E-06 2,80E-05
СКО 0 (н.у.)2 5,73E-03 0,011 0,023 0,115 0,229 1,146
СКО у (н.у.)3 5,73E-03 0,001 0,023 0,115 0,229 1,146
СКО R, м 7,857 8,1767 9,8586 30,5672 61,6265 269,8528
Из-за нелинейной связи кватерниона и углов Эйлера, при одинаковой СКО всех переменных кватерниона СКО углов Эйлера различна. Таким образом, для достижения желаемого уровня точности оценок координат необходимо налагать требования на СКО начальных условий угловой ориентации для тех переменных, которые используются в алгоритме перехода между базисами. По результатам проведенного моделирования можно считать приемлемой величиной для СКО начальных условий по углам Эйлера 0,5 градуса для тангажа, крена, рыскания.
Заключение. В статье приведены результаты исследования влияния СКО начальных условий на СКО погрешности определения местоположения. Полученные зависимости позволяют сформулировать требования к точности задания начальных условий при определенном ограничении на СКО погрешности местоположения на определенном временном интервале. При предельном допустимом
значении СКО 20 м на интервале работы 60 с СКО начальных условий должно быть не более 5 м по положению, 0,045 м/с по скорости, 0,001 по кватерниону.
Литература
1. Бранец В.Н. Введение в теорию бесплатформенных инерциальных систем / В.Н. Бранец, И.П. Шмыглевский. - М.: Наука, 1992. - 280 с.
2. Nebot E. Initial calibration and alignment of low cost inertial navigation units for land vehicle applications / E. Nebot, H. Duran-Whyte // Journal of Robotics Systems. - 1999. - Vol. 16, № 2. - P. 81-92.
3. Farrell J.A. Real-Time Differential Carrier Phase GPS-Aided INS / J.A. Farrell, T.D. Givargis // IEEE Transactions on Control Systems Technology. - 2000. - Vol. 8, № 4. - P. 709-720.
4. Gao J. Development of a Precise GPS/INS/On-Board Vehicle Sensors Integrated Vehicular Positioning System [Электронный ресурс]. - Режим доступа: http://www.ucalgary.ca/engo_webdocs/GL/07.20 251.JianningQiu.pdf, свободный, (дата обращения: 20.03.2010).
5. Квакернаак Х. Линейные оптимальные системы управления / Х. Квакернаак, Р. Сиван. - М.: Мир, 1977. - 638 с.
6. Julier S. J. A new method for nonlinear transformation of means and covariances in filters and estimators / S.J. Julier, J.K. Ulhmann // IEEE Transactions on Automatic Control. - 2000. - Vol. 45. - P. 472478.
Конаков Алексей Сергеевич
Студент каф. радиотехнических систем ТУСУРа
Тел.: 8-(382-2) 41-36-70
Эл. почта: [email protected]
Шаврин Вячеслав Владимирович
Магистрант каф. радиотехнических систем ТУСУРа
Тел.: 8-(382-2) 41-36-70
Эл. почта: [email protected]
Тисленко Владимир Ильич
Д-р техн. наук, профессор каф. радиотехнических систем ТУСУРа
Тел.: 8-(382-2) 41-36-70
Эл. почта: [email protected]
Савин Александр Александрович
Канд. техн. наук, доцент каф. радиотехнических систем ТУСУРа Тел.: 8-(382-2) 41-36-70 Эл. почта: [email protected]
Konakov A.S., Shavrin V.V., Tislenko V.I., Savin A.A.
Effect of initial conditions on the RMS estimates of coordinates in strapdown inertial navigation system with quaternion method of transition between the bases
The statistical estimates based RMS coordinate deviation from the initial conditions and the absolute values of angular velocity and the apparent acceleration produced mathematical modeling to determine the coordinates of strapdown inertial navigation system with different standard deviation of the initial conditions.
Keywords: strapdown inertial navigation system, quaternions, the error standard deviation.
