CC BY
61
ИСПОЛЬЗОВАНИЕ ОДОМЕТРИИ ДЛЯ ПОВЫШЕНИЯ ТОЧНОСТИ ИНЕРЦИАЛЬНОЙ НАВИГАЦИОННОЙ СИСТЕМЫ ТРАНСПОРТНОГО СРЕДСТВА
Ахмедов Даулет Шафигулович
канд. техн. наук,
директор ДТОО «Институт космической техники и технологий»,
Республика Казахстан, г. Алматы
Ибраев Айдос Саятулы
докторант PhD,
Казахского Национального Университета им. Аль-Фараби, Республика Казахстан, г. Алматы E-mail: [email protected]
USE OF ODOMETRY FOR INCREASING THE ACCURACY OF THE INERTIAL NAVIGATION SYSTEM OF A VEHICLE
Daulet Akhmedov
doctor of Technical Sciences, Director of the "Institute of Space Engineering and Technology"
Kazakhstan, Almaty
Aidos Ibrayev
PhD student of Al-Farabi Kazakh National University,
Kazakhstan, Almaty
АННОТАЦИЯ
Статья посвящена разработке алгоритма интеграции ИНС с помощью одометрических датчиков с целью повышения точности навигационных данных.
Данная статья содержит подробное описание процесса определения координат с помощь одометрии. Также приведена схема слабосвязанной итеграции ИНС/Одометрии с использованием слабосвязанной фильтрации Калмана. В результате такой гибридизации мы получаем относительно дешевую систему навигации транспортного средства, позволяющую определять местоположение транспортного средства достаточно долго (до 3 часов), в интервалах недоступных для GPS сигналов.
www.sibac.info
ABSTRACT
The article is devoted to the development of the INS/Odometry integration algorithm in order to improve the accuracy of navigational data. This article contains a detailed description of the process of determining the coordinates using odometry. The scheme of INS / Odometry integration using a weakly coupled Kalman filtering is also given. As a result of this hybridization, we get a relatively cheap vehicle navigation system that allows us to locate the vehicle long enough (up to 3 hours) while GPS signals are not aviable.
Ключевые слова: гибридная навигация, ориентация, инерциальная навигация, гироскоп, акселерометр, одометр, фильтр Калмана.
Keywords: hybrid navigation, orientation, localization, inertial navigation, gyroscope, accelerometer, odometer, Kalman filter.
Одометрия
Определению местоположения объектов с помощью спутниковой и инерциальной навигации посвящены многие работы. Поэтому, авторы решили не углубляться в эту тематику и перейти на описание уравнении оценки положения транспортного средства с помощью одометрических данных, которые являются отличным дополнением к данным ИНС.
Одометрия позволяет измерить вектор скорости х,у,6 и
интегрировать его в конфигурацию (х,у,в). Прикрепленные к колесам
автомобиля, одометры измеряют количество оборотов колес в заданном промежутке времени.
В приведенных ниже моделях мы не будем рассматривать скольжение колес.
Модель транспорта двухколесного типа
Рассмотрим двухколесного транспортного средства показанной на рисунке 1. Кинематические уравнения для такого автомобиля будут в виде:
и = г {ап +ал) / 2
v = 0 (1)
в = г (соп - соп) / 2d
Здесь:
С0п , С0Л - угловые скорости правого и левого колеса соответственно;
www.sibac.info
ВВ - угловая скорость автомобиля вокруг вертикальной оси в плоскости x-y;
в - угол между главной осью автомобиля и оси x; г - радиус колес (предполагаем, что одинаковые); 2d - расстояние колес.
Рисунок 1. Модель транспортного средства двухколесного типа
Неголономные ограничения как следствие пренебрежения скольжением, приводит к тому, что поперечная скорость равна нулю ^ = 0). Если разложить этот компонент скорости на оси x и у, получим:
-х sin в +v cos 0=0
(2)
А для продольной скорости:
X cos в + у sin в = и
(3)
www.sibac.info
Дискретная форма приведенных выше уравнений:
Х+1 = Хк + иС08вк+ Ук+1 =Ук +11этвк+1Ж
вк+1=вк+вл
(4)
Здесь:
хк и ук - центр оси вала колес;
А t - период дискретизации.
Модель транспортного средства типа трицикла (рулевое колесо)
Состояние транспортного средства изображенной на рисунке 2 можно написать в виде (х, у, в, ф, V) Где: х и у - положение оси задних колес;
в - угол который образуемый корпусом автомобиля с осью х;
ф - угол поворота относительно кузова автомобиля (угол поворота колеса, определяющего мгновенный центр вращения О);
Ь - расстояние между валами колес автомобиля (на рисунке, расстояние Я и К).
Мгновенный радиус кривизны кривого у (Д следует за этой кривой) равен l/tanф, и его верхний предел формулируется в следующем виде [2]:
1 1
■ = - Шп<рт
Рт
(5)
Где: рт1П - минимальный радиус вращения;
Рисунок 2. Модель транспортного средства типа трицикла
Транспортные средства данного типа подвергаются ограничению Мм , которое означает, что угол поворота должна быть
ограниченной и не превышать максимального значения.
11 п I ч м Ограничение \<р\ < сртах < — можно написать в виде |6>| <- [1, 4].
2
Рт
Где, м = ик - скорость точки Я по продольной оси автомобиля. Кинематическая модель данной модели:
Если одометр расположен на задних колесах автомобиля, то кинематическая модель будет выражаться следующим образом:
(х^ ' С05 0 ^ Г о ^
У я/п в 0
— м1 +
в 1ст ср /1 0
, 0 ) V 1 ,
Где: м = Мд - продольные скорости задних колес;
www.sibac.info
У2 - продольная скорость рулевого колеса.
Если одометрические датчики прикреплены на передних колесах автомобиля, то кинематическая модель будет характеризоваться:
cos 0 cos ср^ (0 ^
У sin в sin ср 0
= v1 + v2
в sin ср /1 0
j>J , 0 У V1V
Где: Vj = vp - продольные скорости передних колес
угловая скорость в соответствует скорости инерциального блока по оси z.
Компьютер выдает нам число вращения правого и левого заднего колеса с частотой равной 10 Гц. Для каждого измерения в момент времени к, скорость левого и правого колеса может быть оценена, как:
v = сс х METERS PER TOP 0 0 - - (8) v =d x METERS PER TOP
С С — —
В качестве vl можно взять среднее значение этих двух колес (при наличии двух энкодеров).
v ^ ( 1 / 2 1 / 2 Y V ,
' , ,, ' (9)
ю' /W -1 /WД^ '
Где: И7- расстояние задних колес, а со = в.
Обозначим измерения одометра на верхушках колес как а, а равна а разделенная на период обновления информации, единица измерения - оборот/с. МБТБК8_РБЯ_ТОР - является коэффициентом преобразования, который дает горизонтальное расстояние, пройденное за один оборот [6].
Для реализации этих уравнений в алгоритме мы должны использовать дискретную форму. Дискретная форма указанных уравнений (для движения задних колес) имеет вид:
www.sibac.info
x (к +1) = x (к) + Tv (к) cos {в (к)) y (к +1) = y (к) + Tv (к) sin (в (к))
(10)
'(к +1) =
[а(к)-а(к -1)]
METER PER TOP
И
в( к + 1) = в( к )+ T1 v ( к ) tan (<р( к ))
(11)
Или,
в( к + 1) = в( к) + Ta2
Где: Т - период обновления данных [3, 4].
(12)
Интеграция ИНС/Одометрия
Вспомогательную информацию к инерциальной навигации можно получить не только от внешних датчиков, таких как СНС [2]. Если для движения автомобиля имеются какие-либо ограничения, то альтернативным способом может быть использование ограничивающих условий для инерциальных уравнений.
Схема работы интегрирования ИНС с помощью одометрии
показан на рисунке 3 (Где Ш "ъ - матрица перехода от системы координат
связанной с телом (Ь) к навигационной системе отсчета (п)).
T
www.sibac.info
Рисунок 3. Интеграция ИНС/Одометрия
Так как мы использовали неголономные ограничения, считая, что транспортное средство не выпрыгивает и не скользит в течение проведения экспериментов, скорость транспортного средства в плоскости перпендикулярной к направлению вперед (ось х) будет равняться нулю [7]. Данный факт можно использовать в качестве виртуального наблюдения для ограничения накопления ошибок по одному из направлении (боковой либо вертикальном).
Если транспортное средство чрезмерно скользит, или производит движение в вертикальном направлении, то требуется более подробная модель. Тем не менее, использование ограничении в виде виртуальных ограничении остаются в силе.
В идеальных условиях, нет скольжения в направлении боковой оси. Так же, мы предполагаем, что нет движения в направлении нормальной к поверхности дороги:
V
V - 0
V.4 - о
(13)
V'Мо - скорость обеспечиваемый одометром,
www.sibac.info
'V Л (vbxcosUcos^
,(k ) =
0
V У
vx cos в sin у -vb sin в
V
(14)
n
v
Таким образом, две неголономные ограничения могут быть использованы в качестве дополнительных наблюдении для обновления матрицы фильтра Калмана.
Szk+1 =(Zk+1 )odo - h (x"°m/k ) = v"MS - v
n ,,n
Odo
(15)
Матрица наблюдения:
HOdo (03x3 73x3 03x3)
А ковариационная матрица измерения:
(16)
( R 0 0 ^
R Speed Odo 0 0
R =
Ry 0
0 R,
(17)
Так как вектор скорости преобразуется от системы координат связанной с телом (Ь) к навигационной системе отсчета, ковариация шумов наблюдения тоже должна быть преобразована:
Чее* = Ш * К Ш (18)
где: Ш1 - матрица перехода между указанными выше системами координат.
Алгоритм фильтра Калмана рекурсивно проходит в два этапа:
• Прогноз (оценка): Первый алгоритм производит прогнозирование для оценки состояния и его ковариации (в инерциальной системе) в момент к.
• Коррекция: после прогнозирования, получив данные одометра, мы имеем скорость движения транспортного средства по оси Х. Это наблюдение, выраженное в локальной системе координат связанной с автомобилем, можно преобразовать в навигационную систему координат с помощью уравнения (14). Затем, с помощью уравнения (15)
www.sibac.info
вычисляются вектор измерения (наблюдения) и ее ковариационная матрица R. После этого, производиться обновление состояния и соответствующей ей ковариации.
Анализ результатов
В качестве базы данных для тестирования мы выбрали данные из датчиков автоматизированой платформы AnnieWAY проекта KITTI Vision Benchmark Suite (Karlsruhe Institute of technology, Германия).
Началом координат мы выбрали точку начала движения (измерении). Направление ОХ - с запада на восток, OY - с юга на север.
1) Город:
При условиях движения по ровным городским улицам, точность инерциальной навигации и одометрии достаточно высока, в то время, как из-за помех высоких здании точность GPS значительно снижена. На рисунке 4 отображены координаты расчитанные по алгоритмам ИНС, одометрии, интегрированной ИНС/Одометрия системы, GPS, ИНС/СНС/Одометрии.
Н Figure 3 Fils Edit View Insert Tools Desktop window Help □ X
oe a Э fl iiffls □ Ю) I □ 0
120
100 SO
GO <| 40 о 20
0
-20
40
E60 50 100 150 OX. m? 200 250 3D0
а)
mwv.sibacinfo
б)
Рисунок 4. а) Координаты по Одометрии(зеленый), ИНС(красный) и реальные координаты (шкала в метрах) б) Реальные координаты (синяя), по одометрии+ИНС, показания GPS приемника и результаты калмановской фильтрации (ИНС/СНС интеграция) (шкала в метрах)
Из графиков видно, что для гладких немецких улиц, точность определения местоположения по одометрическим датчикам (приблизительно 2 метра за час) значительно превосходит точность по ИНС (примерно 40 метров за час). Отклонение от истинного значения интеграции ИНС/Одометрии составил 1,7544 метров за час.
2) Трасса
Когда автомобиль двигаеться по трассе, ошибки ИНС и особенно Одометрии знаыительно больше по сравнению с движением по городу (во первых, из за гладкости и ровности дороги, и во вторых из-за дальности дороги и высокой скорости передвижения). Точность GPS, в свою очередь значительно выше.
www.sibac.info
а)
б)
Рисунок 5. а) Координаты по Одометрии+ИНС(зеленая) и реальные координаты (синяя) б) Реальные координаты (синяя), показания GPS приемника (красная), Координаты по Одометрии+ИНС (голубая), и результаты Калмановской фильтрации (ИНС/СНС) (зеленая)
Накопление ошибок ИНС при движении на трассе составил 127 метров в три часа, в то время как ошибка Одометрии в течение такого же периода времени составил всего 6 метров. Результат интегрированной ИНС/Одометрии примерно равна 4,175 метров за три часа.
Заключение
Результаты интеграции ИНС с помошью одометрии позволили улучшить инерциальные данные о местоположении транспортного средства в десятки раз. Такая система может обеспечивать автомобиль навигационной информацией хорошей точности при недоступности GPS достаточно долгое время. Также, такая модель может быть использована для улучшения интегрированной ИНС/СНС системы, позволяя получить высокоточную гибридную навигационную систему состоящую из более дешевых датчиков.
Список литературы:
1. Ибраев А.С., Ибраева А.С., Ахмедов Д.Ш. «Автоматтандырылган келжтерге арналган инерциалды-спутник'п навигацияльщ жуйесшщ математикальщ-багдарламальщ кешенш жасау», Вестник Жезказганского университета имени О.А. Байконурова. Жезказган -2016. Стр. 363- 368.
2. Cannon M.E., Lachapelle G., and Sun H. "Development and Testing of an Integrated INS/GPS Cross-Linked System for Sub-Meter Positioning of A CF-188 Jet Fighter". Institute of Navigation Annual Meeting/Cambridge/June 28-30, 1999.
3. Kelly A., "Some Useful Results for Closed-Form Propagation of Error in Vehicle Odometry", report CMU-RI-TR-00-20, Robotics Institute, Carnegie Mellon University, Dec 1, 2000.
4. Milanés V., Naranjo J.E., González C., Alonso J., de Pedro T., "Autonomous Vehicle Based in Cooperative GPS and Intertial Systems", in Robotica, vol. 26, n.5, pp. 627- 633, Oct. 2008.
5. Noureldin A., Karamat T.B., Georgy J., "Fundamentals of Inertial Navigation, Satellite-based Positioning and their Integration", Springer Science & Business Media, 28.10.2012
6. Samson C., "Feedback control of a Nonholonomic Car-Like robot", A book presents results from the ESPRIT Basic Research Project, INRIA, 1997.
7. Sukkarieh S., Eduardo M., "A High Integrity IMU/GPS Navigation Loop for Autonomous Land Vehicle Applications", IEEE Transactions on robotics and automation, Vol. 15, No. 3, June 1999.
