Слабосвязанный алгоритм интегрированной инерциально-спутниковой навигационной системы транспортного средства Текст научной статьи по специальности «Математика»

Слабосвязанный алгоритм интегрированной инерциально-спутниковой навигационной системы транспортного средства

И.В. Щербань, С.А. Толмачев, Д.С. Конев Введение

Известно, что относительно дешевые современные инерциальные навигационные системы (ИНС) не позволяют с достаточной точностью определять навигационные параметры движущихся объектов и, поэтому, для автотранспортных средств (ТС) требуемая точность чаще всего обеспечивается посредством комплексирования разнородных навигационных систем (НС) - недорогих бесплатформенных ИНС (БИНС) на основе MEMS-датчиков и спутниковых навигационных систем ГЛОНАСС/GPS [1,2]. Так как процессы оценивания навигационных параметров ТС осуществляются в реальном времени и должны учитывать динамику управляемых процессов, то существуют попытки построения алгоритмов интегрированных навигационных систем ТС из предположения о статистическом характере помех измерения, и, как следствие, основанных на различных методах оптимальной стохастической фильтрации [3-5]. Однако до настоящего момента существуют нерешенные проблемы практического использования подобных подходов именно применительно к автомобильной технике, так как траектории движения наземных транспортных средств заранее неизвестны, а о возможных возмущениях известны только самые общие предположения.

Во-первых, для построения универсальных математических моделей автотранспортных средств неприменим формализм Ньютона, на котором основаны процессы синтеза моделей оцениваемых объектов в классических задачах фильтрации, используемых для навигации других типов по-

движных объектов, например, летательных аппаратов или морских судов [3,5-7].

Во-вторых, в подобных задачах также неэффективны и так называемые методы тесной интеграции, базирующиеся на выводе уравнений ошибок НС [3-5]. При допущении о малости погрешностей измерителей решение подобных уравнений, являющихся, по существу, уравнениями в вариациях и получаемых из уравнений основной задачи навигации, позволяет синтезировать асимптотическую оценку ошибок навигационной системы. Следовательно, неизбежные погрешности, обусловленные утратой нелинейных взаимосвязей при линеаризации уравнений и допущениями о малости как ошибок измерений, так и погрешностей линеаризации при использовании довольно грубых измерительных МЕМБ-датчиков БИНС автомобиля, будут обуславливать непреодолимые проблемы устойчивости синтезируемых фильтров. Кроме того, на сегодняшний день практически отсутствуют адекватные математические модели погрешностей MEMS-датчиков, возможные для использования в течение длительных временных интервалов эксплуатации транспортных средств без периодических калибровок. Поэтому известные многочисленные методы оценивания, общим необходимым условием применения которых является наличие априорной информации о моделях ошибок измерителей [4,5,8], здесь также не могут использоваться.

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

гут иметь непредсказуемые "выбросы" - случайным неконтролируемым образом выдают неопознанную недостоверную информацию о местоположении ТС (например, при малом числе видимых навигационных спутников, когда автономные позиционные и скоростные спутниковые навигационные решения невозможны) [1,2,5].

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

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

2 Исходные данные для решения

Предполагается, что на транспортном средстве (ТС) используется наиболее распространенная MEMS БИНС, построенная на трех акселерометрах, трех датчиках угловой скорости и трехосевом магнитном датчике.

Для описания движения ТС используются следующие правые системы координат (СК) [9,10]:

- референцная СК, где за отсчетную поверхность принят эллипсоид Красовского (СК Пулково-42);

- геодезическая СК "Параметры Земли" (ПЗ-90), где за отсчетную поверхность принят общеземной эллипсоид;

- геодезическая СК "Мировая геодезическая система" (WGS-84);

- топоцентрическая СК OENh, где положение центра масса (ц.м.) объекта задано геодезическими координатами - высотой места h, широтой ф и долготой X, связанными с вектором у нормальной силы тяжести [5];

- СК Cxyz, связанная с объектом (ССК), совершающая движение вместе с ним и ориентированная следующим образом: ось Су направлена вдоль продольной оси автотранспортного средства, ось Cz лежит в плоскости (yz) симметрии объекта и перпендикулярна продольной оси, а ось Сх образует правый ортогональный трехгранник.

Также введены следующие ограничения и допущения. Во-первых предполагается, что автотранспортное средство движется только по автомобильным дорогам, отраженным в навигационных цифровых картах свободного доступа OpenStreetMap [2]. Во-вторых, пусть интегрированные ИНС и СНС работают в слабосвязанном режиме (Loosely Coupled Systems), когда решается задача коррекции БИНС при помощи позиционных автономных решений СНС [1,2]. При этом местоопределение центра масс движущегося объекта в СК ПЗ-90 СНС осуществляют с точностью, характеризуемой среднеквадратичным отклонением (СКО) 10 м. Также предполагается, что на временных интервалах до 5 мин могут иметь место вышеупомянутые случайные неконтролируемые и неопознанные "выбросы" СНС. В этих случаях точность определения координат ТС спутниковой системой характеризуется уже СКО, равным 200 м.

Исходной информацией навигационной системы являются, таким образом, измерения БИНС: составляющие пх, пу, п2 вектора п кажущегося

ускорения автомобиля на оси ССК; составляющие шх, шу, ш2 вектора ю угловой скорости его вращения; угол курса К0, рассчитываемый по измерениям магнитометра, а также координаты ТС в СК ПЗ-90 ХСНС, УСНС и 2СНС,., формируемые по измерениям СНС.

Выходной информацией навигационной системы являются широта ф и долгота X положения автомобиля на поверхности Земли в СК Пулково-42.

3 Итерационный алгоритм решения навигационной задачи

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

Такт работы медленного контура

Тш = ВД

равен интервалу времени получения навигационных сообщений СНС. Здесь Т0 - такт работы быстрого контура; км>1 - коэффициент, определяемый из трудоемкости задач и производительности бортового вычислителя ТС.

В быстром контуре выполняется км итерационных циклов решения навигационной задачи БИНС, где вычисляются текущие значения координат ТС ф, X и к, восточная УЕ, северная Уы и вертикальная Уь составляющие линейной скорости ТС относительно Земли, а также углы пространственной ориентации ТС К, 0 и у - курса, продольных и боковых наклонов

соответственно. На схеме этой функциональной части решения задачи соответствует "Навигационный блок БИНС".

Каждый цикл навигационного блока БИНС включает следующую последовательность действий. Для первого такта итерационного цикла Т0 в качестве начальных данных используются выходные параметры медленного контура, остальные же исходные данные для (км-1) последующих тактов формируются в ходе решения итерационных циклов классическим образом. Дискретность съема измерений датчиков БИНС равна при этом Т0.

Вначале определяется матрица МС^Е(0,г) (г = 1,2,..., км) перехода из ССК в геодезическую СК БИН

М

С^Е

со б&о + 8тЛ"со со - усо

- 8тЛ"со80 + со^т^т0 со^со80 - (япЛз1п0 + со^1пусо80) - со8^Ш0 со8усо80

(1)

по начальным данным К, 0, у(0,г), и элементы кососимметрической матрицы ¿5 у(0, г)

ю у =

0 -Юк Ю —

Юк 0 -ЮЕ , (2)

-Ю— Ю Е 0

V,

V,

V*

где юе =—— ; ю— =Осо8ф+Vе ; = О1пф+Vеtgф; О - угловая скорость суточ

Я

я

ного вращения Земли; Яф =

а( - е 2)

(л 2-2 V/2

(1 - е 81п ф

Я

+к; як =

(1 - е 281п2ф)'

1/2

+к; а, е - пара-

рГ ' (1

метры эллипсоида Красовского; УЕ, Ум, УН, ф, Н, Х(0,г) - входные параметры.

Далее выполняется интегрирование матричного дифференциального уравнения Пуассона [5,8] для определения компонентов матрицы

мс^е г):

м,

с^е

= Мс^еЮь - ю,М

^мС^Е :

(3)

а

где ó b =

0 -юг юy юг 0 -их

-и y Ю х 0

кососимметрическая матрица, формируемая по

текущим измерениям ДУСов.

При вычислении текущих значений компенсационных ускорений

аЕ, aN, ah

(t,i) пренебрегаем изменением вектора гравитационного ускорения

на такте интегрирования и в расчетах используются значения ф, X, УЕ, Яф Я^^А), полученные на предыдущем шаге. Для этого вначале вычисляется величина нормальной силы тяжести на поверхности эллипсоида у:

ауecos2ф + Ьу sin 2ф

у = —- , (4)

/22,72-2

•у/а cos ф + b sin ф

где уe, уp - величины нормальной силы тяжести на земном экваторе и поЛ

люсе, равные уe = 9,780 , уp = 9,832 (м/с ).

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

gE = -у^ ; gN =-у^ ; gh =у+д?, (5)

где , \ - восточная и северная составляющие уклонения отвесной линии;

Ag - аномалия силы тяжести.

Тогда окончательно получаем:

аЕ = Vh (2Q + À)cosф - VN (2Q + À)>шф + gE ; aN = Ve (2Q + À)тф + ^ф + gN ; (6)

ah = g h - VE (2Q + À - VN ф 5

• VN ■ VE где ф = ; à = —. Яф ràcOSФ

По измеряемым текущим значениям акселерометров nx, ny, nz вычисляются составляющие

пЕ пх

пы = М с^е (, г) пу

пк

и, окончательно, находятся ускорения ТС в геодезической СК на /-м шаге итерационного цикла:

Уе = пЕ - аЕ ; ум = пм - ам; ук = - ак . (8)

Выходные параметры навигационного блока БИНС - текущие скорости и координаты ТС, определяются из уравнений:

Уе(г,г) = Уе(0,0+ |Уе(х)йх ; Уы(г,г) = Уы(0,г)+ |Уы(х)йх ;

Ук (г, г) = Ук (0, г) + | Ук (х)йх

(9)

ф(г, г ) = Ф(0, г)+ | УШ йх;

Т ^Ф(х)

, г ) = х(0, г)+ |

Уе (х)

йх;

^^Х(х)С08ф(х) '

к(г, г) = к(0, г)+ | Ук (х)йх .

(10)

Также в быстром контуре рассчитывается среднее значение приращения модуля скорости на интервале кмТ0 по измеренным акселерометрами ускорениям во всех км тактах итерационного цикла:

ДУ(70, г = км ) = -^ 5 ДУ(г, г); (11)

км г=1

где ДУ(г, г)=т0 ^пх(г, г)2 + пу(г, г)2 + п2(г, г)2 ; у - номер такта медленного контура.

Формируемое таким образом значение приращения модуля скорости характеризуется только погрешностями акселерометров и не содержит погрешности, обусловленной ошибками ДУСов.

Рассчитанные в км-м цикле по уравнениям (9-11) текущие переменные вектора состояния ТС являются исходными данными для у-го такта медленного контура:

ДУ ,Уе ,Ум ,Ук, Ф, X, к (Т0, км ) = Уе У ,Ук, Ф, X, к (0, ]),

т

т

0

0

т

0

т,

0

где также используются усредненные результаты измеренного магнитометром БИНС угла курса K на интервале кмТ0:

Kср(?о, ^ ) =^ ZКи (t, i) = Ки (0, j) . (12)

км i =1

В медленном контуре в первую очередь выполняются преобразования систем координат. Преобразование выходных значений быстрого контура - фазовых переменных ТС, из СК Пулково-42 в СК WGS-84 выполняется в три этапа. Вначале выполняется переход из геодезических координат в декартовые координаты СК Пулково-42 в соответствии с уравнениями X П (о, j) = (W + h) cos9cosA; Y П (0, j) = (W + h) coscpsin A;

Z П (0, j) = (w(l - e2 )+ h) sin9 ,

где W (о, j )= . a == - радиус кривизны первого вертикала,

- e2sin9

а затем выполняется переход из декартовых координат Пулково-42 в декартовы координаты ПЗ-90 и, далее, в СК WGS-84 по известным уравнениям [10].

Одновременно выполняется преобразование из СК ПЗ-90 также в СК WGS-84 декартовых координат ТС, восстанавливаемых по измерениям СНС. В результате получаем две группы координат ТС в СК WGS-84 в одноименные моменты времени, но формируемые по результатам работы разнородных навигационных систем - БИНС XW , УИ , ZW (тм, j) и СНС

XCW , Ус , Z(W (тм, j).

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

соответствующую информацию инерциальной системы [1,2]. Поэтому далее выполняется прогноз местоположения ТС на автомобильной дороге.

Для этого вначале уточняется направление движения ТС по значению к ср(о, j) из уравнения (12). Затем угол курса корректируется по цифровой карте - по координатам дороги, что позволяет получить его точное текущее значение кист (о, j). За счет последнего появляется возможность расчета прогнозируемого перемещения ТС за прошедший интервал времени Тм:

Д^Тм, j) = ГмАГ(Гм, j) . (13)

Исходя из значений модуля скорости дг(о, j) (11), Д^(тм, j) (13) и скорректированного угла курса определяется окрестность возможного положения ТС на карте и выполняется выборка группы из т пар координат ,7^ )(х^ )...(хт )}, описывающих траекторию дороги в окрестности данной точки.

Решение задачи идентификации на основе минимизации невязок осуществляется в статистической (апостериорной) постановке - по результатам полученных наборов измерений относительно базового решения. В результате находятся координаты местоположения ТС (хТС (тм, j) 7ТС (тм, j)). При выборе конкретного алгоритма идентификации основным является требование к его вычислительной эффективности [11].

С целью обеспечения возможности последующей работы итерационной схемы выполняется расчет начальных условий для следующего, 7+1-го такта. Для этого осуществляется обратный перевод полученных координат ТС из СК WGS-84 в декартовые координаты Пулково-42, а затем и в геодезические координаты, для чего необходимо проведение итераций при вычислении геодезической широты и геодезической высоты [10].

По формируемым таким образом значениям ф, И, Х(0,7+1) находятся и остальные фазовые переменные из уравнений:

VE (0, у + 1) = ДХ

( \

a ,

+ h

2 • 2 ^ - 81П т/ ,

7(1 -е2 81п2 ф)

С08ф ;

V, (0, у + 1) = Дф

^ л

2

\з + Н

1 - б2 81и2 ф)2

V (0, у +1) = У-Ш

где Ал=Х(°У +1)-Х((),У) ; Аф = Ф(о,У + 1)-Ф(0,У)

т т

м м

Для окончательного формирования начальных данных для быстрого контура следующего у+1-го такта итерационной схемы необходимо найти оставшиеся углы, используемые для расчета начальной матрицы Мс^Е(0,/):

(

к (0, у +1)=кист (0, у); у(0, у +1) =

агс81и

Vн

^Е2 + V,,2 + Vh2

(15)

Заключение

Рассмотренный алгоритм решения навигационной задачи автомобиля обеспечивает возможность эффективной интеграции БИНС на основе МЕМБ-технологии и СНС. Такое построение вычислений позволяет избежать накопления ошибок интегрирования и увеличения погрешностей местоположения ТС со временем. Как показало моделирование, этот факт обеспечивает достаточную - субметровую, точность работы схемы, причем, без привлечения аппарата оптимальной фильтрации.

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

Результаты исследований, изложенные в данной статье, получены при финансовой поддержке Минобрнауки РФ в рамках реализации проекта «Создание высокотехнологичного производства для изготовления ком-

плексных реконфигурируемых систем высокоточного позиционирования объектов на основе спутниковых систем навигации, локальных сетей лазерных и СВЧ маяков и МЭМС технологии» по постановлению правительства №218 от 09.04.2010 г. Исследования проводились в ФГАОУ ВПО ЮФУ.

Список литературы:

1. Пешехонов, В.Г. Интегрированные инерциально-спутниковые системы навигации: сб. ст. и докл. [Текст] - СПб.: Электроприбор, 2001. - 235 с.

2. Sukkarieh, S. Low Cost, High Integrity Aided Inertial Navigation Systems For Autonomous Land Vehicles [текст]: Ph.D. Thesis, Univ. of Sydney, 2000. - 136 p.

3. Голован А.А., Парусников Н.А. Математические основы навигационных систем. Ч. I. Математические модели инерциальной навигации [Текст] -М.: МГУ, 2010. - 126 с.

4. Демидов, О.В. Задача тесной интеграции систем ГЛОНАСС и GPS с ИНС разных классов точности [Текст]: Дисс. на соиск. степ. к.ф.-м.н.: 01.02.01 : защищена 11.12.2009 / Голован Андрей Андреевич. - М.: МГУ, 2009. - 139 с.

5. Анучин О.Н., Емельянцев Г.И. Интегрированные системы ориентации и навигации для морских подвижных объектов [Текст] - СПб.: Электроприбор, 2003. - 390 с.

6. Weston, J. L. Basic Principles Of Strapdown Inertial Navigation Systems [Текст] // J. L. Weston // Strapdown Inertial Navigation Technology - 2nd Edition. - Radar, sonar, navigation and avionics, 2004. - Chapter 3. - P. 1759.

7. Середа А. Ю., Детюк К. В. Бортовой информационно-навигационный комплекс КА «ГЛОНАСС-К» [Электронный ресурс] // «Инженерный вестник Дона», 2012, №3. - Режим доступа:

http://ivdon.ru/magazine/archive/n3y2012/906 (доступ свободный)- Загл. с экрана. - Яз. рус.

8. Красовский, А.А. Справочник по теории автоматического управления [Текст] - М.: Наука, 1987. - 712 с.

9. Бранец В.Н., Шмыглевский И.П. Введение в теорию бесплатформенных инерциальных навигационных систем [Текст] - М.: Наука, 1979. - 320 с.

10. ГОСТ Р 51794-2001. Аппаратура радионавигационная глобальной навигационной спутниковой системы и глобальной системы позиционирования. Системы координат. Методы преобразования координат определяемых точек. - 12 с.

11. Клевцов С.И. Моделирование алгоритма краткосрочного прогнозирования изменения быстроизменяющейся физической величины в реальном времени [Электронный ресурс] // «Инженерный вестник Дона», 2012, №3. - Режим доступа: http://ivdon.ru/magazine/archive/n3y2012/920 (доступ свободный)- Загл. с экрана. - Яз. рус.