Алгоритмы системы ориентации и навигации транспортных роботов Текст научной статьи по специальности «Физика»

УДК 531.383

В.Б. Никишин, В.С. Шорин, Д.Л. Лисицкий АЛГОРИТМЫ СИСТЕМЫ ОРИЕНТАЦИИ И НАВИГАЦИИ ТРАНСПОРТНЫХ РОБОТОВ

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

Ориентация, навигация, транспортный робот, инерциальный датчик V.B. Nikishin, V.S. Shorin, D.L. Lisitsky

ALGORITHMS FOR ORIENTATION AND NAVIGATION SYSTEMS OF TRANSPORT ROBOTS

The article discusses problems of creating algorithms for orientation and navigation systems of transport robots using integrating data of satellite navigation systems, inertial sensors, magnetometers and odometers. The developed algorithms can significantly improve a reliability and an accuracy of information about the state of the robot.

Orientation, navigation, the transport robot, the inertial gage

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

В основе алгоритмов функционирования бесплатформенной системы ориентации (БСО) находятся кинематические уравнения, определяющие связь параметров ориентации с угловыми скоростями подвижного объекта. Как известно, особое влияние на точность выработки оценок параметров ориентации оказывают систематические и случайные низкочастотные составляющие скоростей дрейфов гироскопических измерителей угловой скорости (ГИУС). Систематические составляющие скоростей дрейфов могут быть определены при предстартовой подготовке на неподвижном основании. Влияние случайных низкочастотных составляющих скоростей дрейфа компенсируется коррекцией. В условиях земного тяготения горизонтальная коррекция осуществляется по сигналам датчиков кажущихся ускорений. Азимутальная коррекция БСО на подвижном основании реализуется, как правило, на основе сигналов неинерциальных датчиков информации: астровизиров, магнитометров и т. п. Однако наличие в выходном сигнале ГИУС медленно 30

изменяющейся составляющей скорости дрейфа приводит к появлению в оценках углов ориентации соответствующей ошибки. Поэтому оценивание и компенсация случайных низкочастотных составляющих скоростей дрейфа ГИУС средней и низкой точности в составе БСО подвижного объекта является

важным моментом.

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

Часто для описания низкочастотных составляющих скоростей дрейфа ГИУС используют модели в виде марковских процессов:

f (О = ~w°xi (t)/txi + &xi-j2r~ • w(t), (i = 1,..., 3), (1)

где w(t) - центрированный порождающий «белый шум» единичной интенсивности; txi -время корреляции; oxi - среднеквадратическое отклонение.

При наличии измерений по трем каналам (курс, тангаж, крен) обеспечивается полная наблюдаемость скоростей дрейфов ГИУС. При отсутствии измерений в азимутальном канале, как утверждается в [2], сохраняется возможность оценивания только двух скоростей дрейфов ГИУС. В [1] отмечается полная наблюдаемость всех скоростей дрейфов ГИУС и смещений нулей акселерометров, а также ряда других погрешностей датчиков первичной информации в составе бесплатформенной инерциальной навигационной системы, интегрированной с помощью фильтра Калмана с неинерциальными измерительными средствами, даже при отсутствии измерений в азимутальном канале. На наш взгляд, и первое и второе утверждения нуждаются в существенных уточнениях.

В качестве основы алгоритмов оценивания параметров ориентации используются кинематические уравнения Эйлера в следующем виде:

У = (wx2 cos g- fx3 sin g)/ cos в; в = fx2 sin g+ fx3 cos g g = fi - (fx2 cos g-fx3 sin g) tgв, (2)

где fxi - формируемые по сигналам ГИУС с учетом переносных угловых скоростей оценки скорости вращения робота относительно системы координат, жестко связанной с Землей; у, в, g - углы курса, тангажа и крена.

С учетом малости скоростей поступательного движения робота под переносными угловыми скоростями будем понимать проекции Wxi скорости вращения Земли на оси чувствительности соответствующих гироскопов.

Представим ошибки оценок f в виде суммы низкочастотных составляющих cocxi и шумов wf ГИУС. Составив для системы (2) уравнения ошибок и дополнив их выражениями (1), получим модель наблюдаемой динамической системы в следующем виде:

X = A(o,a) X + GW , (3)

где X = [Aa,oc]T , Aa = [Ду,Дв,Ag]T , of = [fcxl,fcx2,fcx3>]T - вектор состояния; G - матрица шумов; W - вектор нормированных случайных воздействий, представляющих собой процессы типа «белый шум»; a = [у, в, g]T ;

A(o,a) =

ду ду дуг дуг дуг дуг

ду д в дg д0х1 д fx2 д fx3

дв д в д в д в д в д в

ду д в дg д0х1 д fx2 д fx3

dg дg дg дg дg д^&

ду д в дg дох1 д fx2 д fх3

0 0 0 1/tx1 0 0

0 0 0 0 1/t, 2 0

0 0 0 0 0 1/г„

(4)

Полученная система (3) линейна относительно входящих в нее погрешностей. К ней можно применить процедуру оптимальной фильтрации Калмана (ОФК), принимая в качестве измерений разности соответствующих оценок параметров ориентации, сформированных по сигналам ГИУС на основе выражений (2), акселерометров

ж*=к; ж* ж*]т (магнитометров т=[т; т*л т*]т).

Применив процедуру ОФК, получим следующие выражения в матричной форме:

С-

у = сх + V ;

1 0 0 0 0 0'

0 1 0 0 0 0

0 0 1 0 0 0

(5)

(6)

X = Л(а>,а) X + иф + К (у - СХ), (7)

где У - вектор измерения; С - матрица измерения; V - вектор измерительных шумов; Я - ковариационная матрица, характеризующая вектор V; К - матрица коэффициентов передачи фильтра Калмана; иф - вектор управления.

Для оценки эффективности описанных алгоритмов проведено математическое моделирование работы БСО при угловом движении основания: при 0<г<300 с: ^=10°, #=0, 7=0, ух1=0, при 300<<800 с: у = 10 + 5 • біп(0,46 -ж- (г - 300))°, #=0, у=0, при г>400 с ух1=5 м/с. При этом учитывались погрешности датчиков, описанные в паспорте прибора ЛБ1816405. Некоторые результаты моделирования приведены на рис. 1 - 4.

Для счисления координат робота сигналы датчиков пересчитываются к осям горизонтального географического сопровождающего трехгранника 0^2С3 с началом координат в точке старта робота:

(8)

0 0]Т; Л = Аг Лв Л у.

После этого интегрируются уравнения движения центра масс (точки установки трехкомпонентного измерителя кажущегося ускорения) робота с введенными в них членами коррекции, формируемыми по сигналам ЄР^/ГЛОНАСС и датчиков пути и скорости (ДПС):

с1£/Л = V + кх(д* -V), (9)

г * г *

где ту и V - векторы координат местоположения объекта, определяемые по сигналам

ДПС и приемника глобальных систем позиционирования, соответственно; V - вектор оценок скоростей поступательного движения робота. При наличии в БЦВМ электронной карты местности и сигналы соответствующих датчиков, опознающих маркеры на местности, оценки координат корректируются в результате привязки траектории движения робота

к электронной карте.

Рис. 1. Погрешность оценивания угла курса с ОФК в контуре коррекции

Рис. 2. Погрешность оценивания угла курса без ОФК в контуре коррекции

Рис. 3. Погрешность оценивания угла тангажа с ОФК в контуре коррекции

Рис. 4. Погрешность оценивания угла тангажа без ОФК в контуре коррекции

Широта и долгота местоположения робота определяются по выражениям:

р = р0 + £/ Я; 1 = 1 + £3/(Я*соз р), (10)

где рь и 10 - широта и долгота точки старта;

^1 и - декартовы координаты этой же точки.

Результаты математического

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

Как показывает математическое моделирование, применение алгоритмов

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

Работа выполнена при поддержке гранта МОН РФ №2 02.740.11.0482.

Рис. 5. Погрешности измерения координат центра масс

ЛИТЕРАТУРА

1. Андропов А.В. Повышение точности определения местоположения внутритрубных инспекционных снарядов за счет использования спутниковых радионавигационных систем : автореф. дис. канд. техн. наук / А.В. Андропов. Красноярск, 2006.

2. Кортунов В.И. Анализ процессов коррекции БИНС / В.И. Кортунов, Г.А. Проскура // Сб. тр. XIV С-Петерб. Междунар. науч. конф. по интегрированным навигационным системам. СПб., 2007. С.223-225.

3. Плотников П.К. Об устойчивости алгоритмов определения углов поворотов объекта по сигналам гироскопической бесплатформенной системы ориентации / П.К. Плотников, С.А. Лючев // Изв. вузов СССР. Приборостроение. 1991. №10. С43-49.

Никишин Владимир Борисович -

кандидат технических наук, доцент кафедры «Приборостроение» Саратовского государственного технического университета

Шорин Владимир Сергеевич -

аспирант кафедры «Приборостроение» Саратовского государственного технического университета

Лисицкий Денис Леонтьевич -

аспирант кафедры «Системы искусственного интеллекта» Саратовского государственного технического университета

Статья поступила в редакцию 05.10.10, принята к опубликованию 20.10.10