Решение задачи интеграции спутниковых и инерциальных навигационных систем на основе теории нелинейной фильтрации

УДК 519.6

РЕШЕНИЕ ЗАДАЧИ ИНТЕГРАЦИИ СПУТНИКОВЫХ И ИНЕРЦИАЛЬНЫХ НАВИГАЦИОННЫХ СИСТЕМ НА ОСНОВЕ ТЕОРИИ НЕЛИНЕЙНОЙ ФИЛЬТРАЦИИ

© 2011 г. В.И. Уманский

ЗАО «ИнтехГеоТранс» «IntechGeoTrans»

Решена задача тесной интеграции спутниковой и бесплатформенной инерциальной навигационных систем (НС) в самом общем случае. Показаны особенности синтеза алгоритма нелинейного оценивания вектора состояния НС с учетом непрерывного характера автономных измерений и дискретного -спутниковых. На основе теории нелинейной фильтрации разработана методика общего решения навигационной задачи для интегрированной НС, позволяющая обеспечить высокоточное оценивание параметров движения объекта при комплексировании автономных и спутниковых измерений и устойчивое -при пропадании спутниковых сигналов.

Ключевые слова: спутниковая навигация; инерциальные системы; нелинейная фильтрация; спутниковые измерения.

The close integration problem of the satellite navigation system and strapdown inertial navigation system is solved in the most generalized case. The peculiarities of the nonlinear algorithm of the navigation system's state vector estimation in consideration of continues nature of the inertial measurements and discrete character of the satellite ones are described. The method of the navigation problem general solution based on the nonlinear filtering theory is performed for the integrated navigation system. The method provides a high-precision determination of the object navigation parameters in the integrated mode and has durable characteristics in the mode of the inertial navigation system single functioning.

Keywords: satellite navigation; inertial systems; nonlinear filtration; satellite measurements.

Постановка задачи

Решение задачи навигации подвижных объектов (ПО) с использованием спутниковых навигационных систем (СНС) в настоящее время осуществляется по двум направлениям: путем непосредственного использования навигационной информации от СНС на борту ПО и интеграцией измерительной информации от СНС с показаниями инерциальной навигационной системы (ИНС). Так как первый подход пока не в состоянии обеспечить полного решения проблемы высокоточного решения навигационной задачи ПО, то рассмотрим далее только режим тесной интеграции НС. Несмотря на то что его исследования начаты уже давно [1], проблема обеспечения заданной точности и устойчивости интегрированной НС по-прежнему остается весьма актуальной. Это обстоятельство связано с невозможностью решения данной проблемы на основе существующего математического аппарата, предполагающего использование только линеаризованных измерений СНС и линейных уравнений ошибок ИНС, устойчивых лишь на небольших интервалах времени [1 - 4]. Поэтому возникает задача разработки принципиально нового подхода, позволяющего решить задачу тесной интеграции СНС и ИНС в самом общем случае.

Математическая модель вектора состояния инерциальной НС

В качестве ИНС рассмотрим модель бесплатформенной ИНС [5 - 8] (БИНС), при синтезе которой будем использовать следующие правые системы координат (СК) [4, 5, 9]:

- приборную СК (ПСК) J 0xyz, начало которой расположено в центре масс (ЦМ) объекта, а оси направлены по ортогональным осям чувствительности приборов, входящих в состав измерителей БИНС;

- инерциальную СК (ИСК) I с началом в центре Земли;

- вращающуюся вместе с Землей гринвичскую СК (ГрСК) G 1;

- сопровождающую (ССК) OXYZ, начало которой совпадает с центром масс ПО, ось 2 совпадает с местной вертикалью, ось Y параллельна плоскости начального меридиана (с которого начинается движение), ось X дополняет систему до правой.

Считаем также, что в начальный момент времени оси ПСК и ССК (а также ИСК и ГрСК) совпадают и в измерительный комплекс БИНС входят три акселерометра и три датчика угловой скорости (ДУС).

Для синтеза вектора состояния БИНС используем далее параметры Родрига - Гамильтона. Взаимная

текущая ориентация ССК и ИСК описывается системой кинематических уравнений [4, 9]

i=2  о м

где

Фо (X) =

—X2 —X3 —X4

Xi —X 4 X3

4

—X

X1 —X2 X2 X1

Xi (0) = Xl0 , X2 (о) = X2

X3 (0) = X30, X4 (0) = X4

1 т

к = Х1 X 2 Х3 X 4 - вектор параметров Родрига

т

Гамильтона; roS = |ю X

JY шZ

J = Zd— md— Wd =

сивностей Dd ; md =

1LI = 2Ф0 (ц)(Zd — md — Wd ) .

Для окончательного синтеза вектора состояния навигационной системы ПО необходимо представить в замкнутой форме правые части исходных систем уравнений (1), (4). При этом далее учтем, что проекции угловой скорости , юг трехгранника связаны с проекциями линейной скорости объекта Ух ,УУ на оси ССК линейными соотношениями [3, 9]:

vx =rS (r+h);

VY = —ЮXS (r + h ) =

Ю., i = X, Y, Z -

1' ' '

проекции абсолютной угловой скорости ССК на ее оси, равные: юх = ю+ 2О(Х2Х3 +Х1Х4),

юУ = ю^ + о(2Х2 + 2Х2 -1) , ю2 = 2О(Х3Х4 -Х1Х2) , ю. , i = X, У - проекции угловой скорости ССК на

ее оси, обусловленные движением ПО относительно Земли; О - скорость вращения Земли.

В свою очередь, текущую ориентацию трехгранника J ПСК относительно трехгранника I ИСК можно задать следующим образом:

I = 1 Фо

Ц (°) = Нч0 , Ц2 (0) = Ц20 ,

Ц3 (°) = Цз° , Ц4 (°) = Ц4° ,

1 т I т

I = |Ц1 Ц2 Цз ц^ , ^ = |юх юу юг| - вектор

абсолютной угловой скорости вращения приборного трехгранника, который может быть получен по

I т

показаниям 2Л = \2х 2 у 2Л трёх ортогональных

ДУСов, расположенных на ПО

где г - радиус Земли; к - высота объекта над уровнем моря.

Для синтеза выражений проекций Ух, УУ обратимся к основному уравнению инерциальной навигации [5, 9]:

1 = Vs +(2^s + )х Vs + gs

где х - знак векторного произведения; gS - вектор ускорения силы тяжести; а - вектор ускорения, измеряемого акселерометрами; У8 = |Ух УУ У2 |т -вектор скорости объекта относительно Земли;

S =KS roYS 0

движения

- вектор угловой скорости объекта относительно Земли;

где Wd = |Жх Жу - вектор аддитивных помех

измерения ДУСов, описываемый белым гауссовским шумом (БГШ) с нулевым средним и матрицей интен-

вектор ма-

тематического ожидания смещения нуля ДУСов.

С учетом (3) угловое движение БИНС (2) относительно ИСК может быть представлено в векторном виде следующим образом:

= Ох ОУ О2 - вектор угловой скорости

вращения Земли, проекции которого на оси ССК имеют вид:

Ох = 2О(Х2Х3 +Х1Х4) , ОУ =О(2Х? + 2Х2 -1),

О2 = 2О(Х3Х4 - Х1Х2 ) .

Для выбранной ориентации осей ССК проекции

т

вектора gS = gУ gZ | на оси ССК определяются как

gх = 4О2 (г + к)(Х2Х3 +Х1Х4)(Х3Х4 -Х1Х2), gУ = 2О2 (г + к) (2Х? + 2Х2 -1) (Х3Х4 - Х1Х2), gZ = -О2 (г + к) ^2 (Х2Х3 +Х1Х4 )2 + (2Х? + 2Х2 -1)2 ^ -

-g0 (г,k, ф(Х1 -Х4 )) ,

где g0 - гравитационное ускорение, рассматриваемое как функция [5]; высоты к и широты ф(Х1 ^Х4) = arcsin2 (Х3Х4 -Х1Х2) + Ы , k = 0,да, [4, 9].

Вектор выходных сигналов акселерометров

т

Za = 21 22 23\ может быть представлен следующим образом:

Za = Са + Жа ,

S

0

0

0

T

T

mj m

m

d

d

d

x

У

z

где Wa =

WWW

ax ay az

- вектор помех аксе-

лерометров, описываемый БГШ с нулевым мато-жиданием и матрицей интенсивностей Da (/);

С(ц,Х) = D(ц)Вт (Х), С(ц,Х) - матрица направляющих косинусов, определяющая ориентацию ПСК относительно ССК; D (ц) - матрица поворота 2-го рода [4, 9], определяющая ориентацию ПСК относительно ИСК; В = D (X) - матрица 2-го рода, определяющая ориентацию ССК относительно ИСК.

Тогда с учетом (8) вектор ускорений, измеряемых акселерометрами, можно представить в следующем виде:

а = Ст

Выражение

V + 20х^х -(2^х + х5 V + & ;

aF = I

а7 = V7 - ( 2Qf + ю

(2Q7 +ra7s )VX +(

+ ( 2QK +ю

X XS I Y

)VY + sz =

1 =1 Фо (X).

2

—Vy

(r + h) 1 Vx +

0

(2Xi + 2X3

20

Q

откуда, подставив в (10) выражения (5), (6) и (9), получим:

^х = С(1)(ц,Х)(2 а -wa) + 20V --(20у + Кх (Г + h)-1 уг -Ях;

^ = С(2)(ц,Х)(2а - wa)-2Пг¥х +

+ (20х - vy (Г + и)-1 Ух - яy;

^2 = С(3) (ц, Х)(2а - wa ) + (20Y + ^х (Г + и)- ) ^х --(20х - vy (г + И)-1 V -ях,

где через с^)(ц,Х)(г = 1,..,3) обозначена г-я строка

матрицы СТ(ц,Х).

Входящая в приведенные выше уравнения высота объекта над уровнем моря и определяется вертикальной составляющей его скорости

и = Кх,

что позволяет замкнуть все полученные уравнения в единую систему.

В окончательном виде уравнения стохастического вектора состояния БИНС на базе трех ДУСов и трех акселерометров имеют вид:

20

А = 2ф0 (ц)(2т -тй - ^ ) ; Кх = С(1) (ц, Х)(2a - Wa ) + 40(Х3Х4 - Х1Х2 ) VY --(20(2Х2 + 2Х2 -1) + Vх (г + И)-1 )к2 --402 (г + И)(Х2Х3 + Х1Х4) (Х3Х4 - Х1Х2); VY = С(2) (ц, Х) (2a - Wa ) - 40 (Х3Х4 - Х1Х2 V + + (40(Х2Х3 +Х1Х4(г + И)-1 )к2 --202 (г + И) (2Х2 + 2Х2 -1) (Х3Х4 - Х1Х 2); К2 = С(3) (ц,Х)(2а - Wa)+(20(2Х2 + 2Х32 -1)+Vх (г + И)-1 )х хКх -(40(Х2Х3 +Х1Х4) - VY (г + И)-1 у:г +

+02 (г + И)(2(Х2Х3 +Х1Х4)2-(2Х2 + 2Х2 -1)2^ +

+Я0 (Г, h, ф(Х1 -Х 4 )), И = ^ .

Или в каноническом виде - векторной форме Лан-жевена:

Y = Е

где Y = |ХТ цт Vх VY V2 И\ ; £ = ^ТТ WJ\ .

Принципиальными особенностями уравнений (11) являются, во-первых, их общий характер, а во-вторых, возможность использования на их основе при интеграции БИНС и СНС методов нелинейной фильтрации, обеспечивающих оптимальность навигационных оценок.

С целью использования данной возможности необходимо получить, следуя [3, 10], уравнение наблюдателя за вектором Y (т.е. аналитическую модель сигнала, несущего информацию о компонентах вектора

Y).

Математическая модель автономного наблюдателя стохастического вектора состояния БИНС

Для решения этой задачи рассмотрим возможность комплексирования БИНС с доплеровскими датчиками скорости (ДДС).

Для использования информации ДДС в алгоритмах оценивания полагаем далее, что вектор выходных

Т

сигналов ZD = 2Г)Х 2^ 2Dz . ДДС, оси которых ортогональны и направлены по осям ПСК, имеет вид:

ZD = V+WD + UD:

где V = VxVyV2\ - вектор относительной скорости

T

движения ПО в ПСК; WD - вектор марковских помех на выходе ДДС; ^ - БГШ с нулевым средним и матрицей интенсивностей DU.

В общем случае вектор WD описывается стохастическим уравнением:

WD = fD (WD , 0+ fD0 ^ , t)£0,

где - известные векторная и матричная функ-

ции; - БГШ с нулевым средним и матрицей интенсивностей DD.

Так как проекции относительной линейной скорости ПО на оси ПСК Ух, Уу и У определяются по проекциям скорости Ух,УУ, и У2 на оси ССК как

У = С (ц, Х)У5,

то уравнение наблюдателя за вектором состояния БИНС имеет вид:

ZD = С(Ц, Х)У5 + Жо + ио , или в более общем (каноническом) виде

ZD = Н

где Н (У, t) = С(ц, Х)^ + .

(При этом вектор WD должен быть включен в состав всего вектора состояния БИНС Y, а вектор ^ -

в состав ее вектора шумов: = |wd Wj

Y =

F

K

t ) = R

dHT

ÖY

D

R ( Yt )=-

ÖF

R

ÖFT (Y, t

ÖY v ' v ' ÖY

T

+^0 (¥,t)^Г (^t)-К(^t)ОиКт (^t) , (14) У = М (У0 ) , Я0 = М {(у, -У )(У0 - У )т },

где Y - вектор текущей оценки вектора состояния НС У(1).

Полученные оценки навигационных параметров БНС позволяют обеспечить устойчивое оценивание навигационных параметров ПО даже при отсутствии спутниковых измерений. В то же время наличие последних может существенно повысить точность оценивания, в связи с чем рассмотрим возможность интеграции спутниковых и автономных измерений более подробно.

Математическая модель сигналов спутниковых измерений

В стандартном (автономном) режиме информационный сигнал кодовых измерений (псевдодальность) может быть записан (учитывая алгоритмическую компенсацию [11, 12] погрешностей, обусловленных прохождением радиосигнала через ионосферу и тропосферу, погрешностей часов приемника и спутника) как:

Zr =v (1с — I)2 + (л —п)2+(Сс—о2 +

W7

Представление уравнений движения БНС в форме «объект - наблюдатель» открывает принципиальную возможность оптимальной оценки параметров движения ПО.

Синтез алгоритмов нелинейной фильтрации навигационных параметров автономной БНС

В теории нелинейной фильтрации для получения оценок процессов вида (11) используют различные приближенные (субоптимальные) методы, наиболее востребованным из которых является нелинейный (гауссовский) фильтр Калмана - Бьюси, обеспечивающий требуемый компромисс между точностью оценивания и объемом вычислительных затрат.

Уравнения (11), (12) в форме «объект - наблюдатель» легко позволяют, следуя [10], записать нелинейный фильтр для исследуемой НС:

где £с, ,с, Сс - координаты спутника в гринвичской СК, вычисляемые на борту спутника и передаваемые на объект в навигационном сообщении, £, ,, С - текущие координаты объекта в гринвичской СК; Ж2 - БГШ с нулевым средним и известной дисперсией О2к (0 .

Аналогично информационный сигнал доплеров-ских измерений 2У может быть представлен следующим образом:

2у =[& ч)у -у )+(, -л)(У,с -У,)+(Сс -С)(Усс-ус )]х

<(>/(Iс —I)2 + (Лс —п)2 + (Сс —С)2 ) 1 +

w7

где У=с ,У,с ,У?с - проекции скорости спутника на оси гринвичской СК; у= ,У, у - проекции скорости объекта на оси гринвичской СК; Ж2у - БГШ с нулевым средним и известной дисперсией ^).

Сигналы кодовых и доплеровских измерений несут информацию как о текущих координатах ПО, так и о его скорости, т.е. могут быть использованы как сигналы наблюдения вектора состояния объекта. Но для этого их необходимо представить в соответствующей ССК. В исследуемом случае имеем для координат объекта:

—1

¡ = 2(г + И) (Х 2Х4 +Х1Х3);

^ = 2(г + И)(Х3Х4 -Х1Х2);

С = (г + И)(2Х12 + 2Х4 -1).

При определении связи вектора скорости объекта в гринвичской СК Ус с вектором скорости V в ССК необходимо учитывать, что данная связь определяется не только матрицей В(Х1, Х 2, Х3, Х 4) ориентации ССК относительно ИСК, приведенной выше, но и требует дополнительного использования матрицы поворота О гринвичской СК относительно ИСК:

G =

cos Qt 0 -sin Qt 0 1 0 sin Qt 0 cos Qt

В этом случае представление вектора Уа имеет следующий вид:

^ = О(0t)BT (Х1, Х2, Х3, Х4) Vs = ОВТ (X)V5 .

Таким образом, информационные модели сигналов кодовых и доплеровских измерений окончательно можно представить следующим образом:

= с -V' '" /\'^2Х 4 +Х1Х 3 /

zr = wZ^ +

Zv =

(¡с -2(г + И)(Х2Х4 +Х1Х3)) + + (^с - 2(Г + И)(Х3Х4-Х1Х2))2 +

+(С с - (Г + И) (2X2 + 2Х4 -1))2 ]12 = Нк (X, И) + W2R;

(¡и - 2 (Г+И ) (Х2Х4 +Х1Х3 ))(^ -[ОВТ (Х)](1) )+

+ (^с - 2 (Г + И)(Х3Х4 -Х1Х2 ))(^с - [ОВТ (Х)](2) ) +

+ (Сс - (Г + И) (2Х? + 2Х2 -1)) ^ - [ОВТ (Х)](3) Vs )] х х[(^ -2(г + И)(Х2Х4 +Х1Х3))2 + -2(г + И)х

х(Х3Х4 - Х1Х2 ))2 + (Сс - (г + И) (2Х12 + 2Х4 -1))2 )-1

+W2V = ^

где [ОВТ (Х)](г) - г-я строка матрицы ОВТ (X).

Важнейшей особенностью спутниковых наблюдений, существенно затрудняющей высокоточное оценивание непрерывных навигационных параметров объекта - особенно для высокоскоростных ПО, является их дискретный характер (в ГЛОНАСС интервал времени между строками - навигационными сообщениями - составляет 2 с [12]). Подобная задача относится уже к случаю непрерывно-дискретной фильтрации и может быть, как известно [10], решена на основе совместного использования двух видов оценок: непрерывной - на интервале между спутниковыми

наблюдениями, и дискретной - в момент приема навигационного сообщения.

Решение навигационной задачи по комплексированным измерениям интегрированной НС

Итак, рассмотрим схему оценивания вектора состояния интегрированной НС, комплексированный наблюдатель которой формируется как по автономным измерениям БНС, так и по спутниковым измерениям. При этом уравнения спутниковых измерений (15) с целью упрощения дальнейших построений представим в векторном виде:

= Hc

где £ - белый гауссов вектор-шум с нулевым средним и матрицей интенсивностей

^ 0

Zr Hr (X, h) WZR

Z = = +

Zv Hv (X, h,Vs) WZv

D.

CHC

'ZR 0

Так как данные наблюдения являются дискретными, то уравнения спутниковых измерений (16) необходимо представить в следующем виде:

2к = Не (X, к) + С к ,

где к = 1, 2, ... - номер такта приема навигационного сообщения.

Общая схема интеграции НС в данном случае будет следующей: на временном интервале между спутниковыми измерениями для оценки навигационных параметров используется непрерывный нелинейный фильтр (13), (14), а при обработке спутниковой навигационной информации - дискретный гауссовский фильтр [10]. При этом следует иметь в виду, что непрерывный фильтр используется только на временных интервалах [/к_ь /к], k = 1, 2, ..., между дискретными спутниковыми измерениями, поэтому начальные условия У^к-1), R(tK-1) уравнений непрерывной фильтрации вида (13), (14) на интервале [/к_ь tк] формируются как результат дискретного оценивания Ук-1 = У^к-1 + 0), Rк-1 = R(tк-1 + 0) вектора Yв момент времени /к-1: У^к-1) = Ук-1 = У^к-1 + 0), R(tк-1) = Rк-1 = R(tк-1 + 0).

Результат интегрирования У^к), R(tк) уравнений (13), (14) в конце временного интервала [/к-1, tк] является начальным условием - 0) = Ук 0,

- 0) = ЯК0 для выполнения алгоритма дискретного оценивания в момент времени IК: Y(tк - 0) = Yк0 = Y(tк), Щк - 0) = Як0 = ).

Подобная связь начальных и конечных условий алгоритмов дискретного и непрерывного оценивания является одним из основных условий режима тесной интеграции автономной БНС и СНС.

При синтезе дискретного нелинейного фильтра необходимо учитывать, что в отличие от случая непрерывной фильтрации дискретное оценивание осуществляется при расширенном векторе наблюдения:

ZD = 2<Р = Н (У ,к) + ио, Zк = Нс (У, к) + С к, который далее с целью упрощения записи представим как

Z;

ИНТ

7 (1) ^к H (Y, к ) Ud

= +

7K Hc (Y,x) С к

= H

ИНТ

(Y ,к ) + С,

ИНТ

где СКИНТ - БГШ с нулевым средним и матрицей интенсивности

D

D,

и

0 D,

0

СНС

Алгоритм дискретного оценивания при подобном расширенном наблюдателе в соответствии с [10] имеет вид:

ÖH И

Y(tK + 0) = YK о + R(tK + 0) +-

dY

xD^t-1 |>Г "HИНТ

, 1 dH— {YK о, к) 1

R-\tK + 0) = R-0 +-4-Dинт - +

dY

HИНТ

dY '

Таким образом, предложенная схема совместного использования алгоритмов оценивания (13), (14) и (17) позволяет принципиально и в самом общем случае - без каких-либо упрощающих допущений - решить задачу тесной интеграции автономной БНС и СНС.

Литература

1. Демидов О.В. Задача тесной интеграции систем

ГЛОНАСС и GPS с ИНС разных классов точности : дис. ... канд. физ.-мат. наук. М., 2009.

2. Красовский А.А. Основы теории акселерометрических бесплатформенных инерциальных систем // Изв. РАН. Теория и системы управления. 1994. № 4. С. 135 - 146.

3. Кузовков Н.Т., Салычев О.С. Инерциальная навигация и оптимальная фильтрация. М., 1983. 216 с.

4. Онищенко С. М. Применение гиперкомплексных чисел в теории инерциальной навигации. Автономные системы. Киев, 1983. 208 с.

5. Андреев В.Д. Теория инерциальной навигации. Автоном-

ные системы. М., 1966. 580 с.

6. Канащенков А.И. Формирование облика авионики перспективных летательных аппаратов // Изв. РАН. Теория и системы управления. 2002. № 6. С. 128 - 138.

7.Sitomer J., Ha J., Kourepenis A., Connelly J. HighPerformance MEMS Inertial Measurement Unit (MMIMU) for Tactical Applications // IEEE PLANS 2002, Palm Springs, CA, April 2002.

8. Savage P.G. Strapdown System Performance Analysis // Advances In Navigation Sensors and Integration Technology, NATO RTO Lecture Series No. 232, October 2003, Section 4.3 - 28 RTO-LS-232 (2004) Pre-Prints.

9. Ишлинский А.Ю. Ориентация, гироскопы и инерциальная навигация. М., 1976. 672 с.

10. Тихонов В.И., Харисов В.Н. Статистический анализ и синтез радиотехнических устройств и систем. М., 1991. 608 с.

11. Долганюк С.И. Методы и алгоритмы обработки информации для позиционирования мобильных промышленных объектов на базе ГЛОНАСС/GPS : дис. .канд. техн. наук. М., 2010.

12. Интерфейсный контрольный документ ГЛОНАСС (5 редакция), 2002.

Поступила в редакцию 23 марта 2011 г.

Уманский Владимир Ильич - канд. техн. наук, генеральный директор ЗАО «ИнтехГеоТранс», советник генерального директора ОАО «НИИАС». Тел./факс 8(499)967-77-02. E-mail : umanvi@yandex.ru

Umanskiy Vladimir Iljich - Candidate of Technical Sciences, Director general closed company «IntechGeoTrans», the adviser of the general director JSC NIIAS, PhD Engineering. Ph./fax. 8(499)967-77-02. E-mail: umanvi@yandex.ru