CC BY
2Труды МАИ. Выпуск № 90 www.mai.ru/science/trudy/_
УДК 527.6
Влияние гармонических колебаний блока инерциальных измерителей на погрешность работы алгоритма счисления пути пешеходной навигационной системы
Купоросова Е.С.
Казанский национальный исследовательский технический университет имени А.Н.
Туполева, КНИТУ-КАИ, ул. К. Маркса, 10, Казань, 420111, Татарстан
e-mail: kuporos_lena@mail.ru
Аннотация
С целью исследования точности работы алгоритма инерциального счисления пути с помощью пешеходной навигационной системы (ПНС) выполнено моделирование движения робота, имитирующего походку человека. Для оценки количественных и качественных характеристик модели движения робота проведен эксперимент с движением человека по замкнутому прямоугольному маршруту с блоком инерциальных измерителей (БИИ) ADIS16405, закрепленным на поясе пользователя. При моделировании движения робота основное внимание уделено работе алгоритма обработки входной информации с выходов трех гироскопов и трех акселерометров БИИ, характеристики моделей которых приняты идеальными. Моделирование работы алгоритма обработки входной информации выполнено без внешней коррекции. Исследовано влияние амплитуд угловых колебаний БИИ на погрешность счисления пути при заданном периоде дискретности ПНС Т=0,025 с.
Ключевые слова: акселерометр, датчик угловой скорости, угловые колебания, траектория, система координат, кинематическая модель движения, кажущееся ускорение, преобразование Фурье.
Введение
Автономные ПНС содержат инерциальный измерительный модуль с тремя гироскопами, измеряющими компоненты вектора угловой скорости, и с тремя акселерометрами, измеряющими компоненты вектора кажущегося ускорения [1]. В качестве датчиков ПНС применяют микроэлектромеханические приборы, имеющие малые габариты и низкое энергопотребление. Однако, автономной работе ПНС свойственны погрешности, обусловленные в основном наличием шумовых помех в выходных сигналах микроэлектромеханических датчиков и погрешностями вычислений при обработке поступающей с датчиков информации [2].
Цель данной работы - исследование влияния угловых колебаний БИИ, закрепленного на поясе человека, на погрешность определения координат местоположения БИИ при моделировании движения человека по замкнутому прямоугольному маршруту. Основное внимание уделено точности работы алгоритма дискретной обработки входной информации, полагая, что датчики идеально измеряют компоненты векторов угловой скорости и кажущегося ускорения БИИ.
Постановка задачи
Для определения местоположения человека в условиях отсутствия сигналов
спутниковой навигации, прежде всего необходимо непрерывно отслеживать
изменение направлений осей чувствительности инерциальных датчиков относительно неподвижной системы координат. Для вычисления значений угловых координат осей чувствительности БИИ относительно неподвижной системы координат требуется некоторое время Т=^1+Д^, где Дtl - интервал дискретности поступления исходной информации с выходов датчиков БИИ, Д^ - время, необходимое на обработку исходной информации и вычисление угловых координат БИИ. Время запаздывания Т является одним из факторов, приводящих к погрешности счисления пути ПНС, т.к. к моменту приема новой (текущей) информации с БИИ его действительное угловое положение не совпадет с положением, вычисленным по предыдущим значениям проекций угловой скорости. Чем больше амплитуды угловых колебаний БИИ, тем существеннее при том же значении Т будет погрешность в определении его углового положения, которая определяет точность преобразования проекций параметров движения БИИ из связанной системы координат в навигационную. Поставим задачу исследовать влияние амплитуд угловых колебаний БИИ при заданном Т на погрешность счисления пути в процессе движения человека по заданному маршруту.
Модель движения человека с БИИ построим как кинематическую модель движения робота, имитирующего походку человека. Для получения опорных данных о характере моделируемого движения проведем реальный эксперимент с записью данных, поступающих с БИИ при движении человека по замкнутому прямоугольному маршруту. Чтобы при моделировании движения робота с БИИ исключить влияние погрешностей инерциальных датчиков, их передаточные
функции примем равными единице. Чтобы оценить погрешность алгоритма счисления пути, основанного на преобразовании проекций векторов кинематических параметров движения БИИ из связанной системы координат в неподвижную (навигационную) с последующим их интегрированием [3], модель работы ПНС построим без применения внешней коррекции.
Запись и обработка выходных сигналов БИИ А0/£16405
Конструктивно БИИ ADIS16405 представляет собой куб с длиной ребра 23 мм, показанный на рисунке 1 [4]. При закреплении БИИ на поясе пользователя направления осей чувствительности БИИ (оси ax, ay, az) могут быть произвольными. При этом важно определить начальные значения углов ориентации осей чувствительности относительно осей неподвижной системы координат O0x0y0z0, в которой происходит определение местоположения человека. Полюс O0 поместим в вершине прямоугольника, задающего линию пути. Оси x0 и z0 направлены по сторонам прямоугольника, ось у0 направлена вертикально вверх. Человек начал движение из точки O0 в направлении оси z0 и, двигаясь по замкнутой линии пути, трижды прошел по ней, разворачиваясь в угловых точках на 90о против часовой стрелки. Начальное положение осей чувствительности БИИ приближенно соответствовало направлениям одноименных осей неподвижной системы координат. Обозначим датчики, измеряющие ускорения, символами Ax, Ay, Az, а датчики угловых скоростей - символами Bx, By, Bz.
7 I-
Рис. 1. Внешний вид и ориентация осей БИИ АЛ/516405 Для определения угловых координат осей чувствительности БИИ относительно неподвижной системы Оох0у020 выполним три последовательных поворота БИИ от совмещенного положения двух систем вокруг осей: 1) ау (на угол рысканья у), 2) ах (на угол тангажа 0), 3) а2 (на угол крена у).
В процессе эксперимента выполнена запись показаний БИИ в виде массивов данных с шагом дискретности по времени А! ~ 0,025 с. Сложность анализа полученной информации состоит в том, что в сигналах датчиков смешана информация о характере продольного и бокового движений БИИ. Можно допустить, что информация с выходов датчиков Ау, А2, Вх в большей степени характеризует продольное движение пользователя, в котором изменяются координаты положения БИИ в вертикальной плоскости 2, у и угол тангажа 0, а информация с выходов датчиков Ах, Ву, В2 - боковое движение пользователя, в котором изменяются углы рысканья у, крена у и боковое отклонение х человека от заданной вертикальной
плоскости движения, что было учтено при обработке выходной информации с датчиков БИИ. Троекратный обход заданного маршрута занял по времени примерно 40 с. Одна из сторон прямоугольника преодолевалась за 5 шагов, другая - за 3-4 шага. Массивы показаний датчиков обработаны с применением библиотеки программ сглаживания случайных сигналов Curve Fitting Tool (MatLab) методом скользящего среднего (Moving Average) с размером окна (Span) 15 дискрет [5]. К сглаженным сигналам применено преобразование Фурье с построением частотного спектра распределения амплитуд гармоник ряда Фурье (рисунки 2, 3) [6].
Из графиков видно, что:
- основная частота колебаний параметров продольного движения /1—1,55 Гц, период этих колебаний равен 0,645 с и соответствует времени выполнения шага одной ногой при установившейся ходьбе;
- основная частота колебаний параметров бокового движения юг и ax /2—/1/2—0,77 Гц, период этих колебаний равен 1,3 с и соответствует времени выполнения двух шагов;
- в частотном спектре сигнала по угловой скорости юу доминирующая частота /3—0,36 Гц, что соответствует частоте поворотов пользователя в угловых точках заданной линии пути. Частота /3 является параметром, харакеризующим среднее время движения вдоль одной стороны прямоугольника tc=1//3—2,8 с, и не связана с характером походки человека.
Частотный спектр ш
^лМдЛ^!^__
0.5
1 1.5
Гц
Частотный спектр а
у
(В
0.5
1 1.5
Гц
Частотный спектр а
0.04
Й 0.02
0.5
1.5
I Гц
2.5
2.5
2.5
Рис. 2. Частотные спектры сглаженных сигналов, характеризующих продольное
движение пользователя
I
10
N с;
£3 5
40
I 20
\
0.04
3 0.02
0.5
0.5
0.5
Частотный спектр ш_
1 1.5
I Гц
Частотный спектр ш
1 1.5
Гц
Частотный спектр а
1 1.5
I Гц
2.5
2.5
2.5
Рис. 3. Частотные спектры сглаженных сигналов, характеризующих боковое
движение пользователя
2
0
0
2
0
0
2
0
0
1
2
0
0
2
0
0
2
0
0
2
Пиковые значения угловой скорости юу при поворотах на 90о изменяются от 90 о/с до 170 о/с. Средняя скорость движения человека, выполняющего 5 шагов за 3,22 с на длине пути 3 м, равна 0,93 м/с или 3,35 км/ч. Средняя длина шага Ь1, измеренная между следами пяток разных ног, равна 0,6 м.
Математическая модель движения робота с БИИ
Допустим, что в момент времени t=0 начальные значения углов у, 0, у равны нулю. Заданная траектория движения робота - прямоугольник О0АВС со сторонами
О0Л=ВС=Ь2 =5 м, АВ=СО0=ЬХ =3 м (рисунок 4).
х0
<- в А
-> ->
Рис. 4. Схема замкнутого маршрута движения робота из точки О0 Робот начинает движение из точки О0 к точке А со средней скоростью = V* = 0,93 м/с. В точке А он останавливается и поворачивается на 900 влево. Время поворота At=1с. Допустим, что угловая скорость поворота постоянная, т.е. \\1* = 1,57 с-1. После поворота робот продолжает движение к точке В с той же скоростью, но в направлении оси Х0, т.е. со скоростью = V * = 0,93 м/с. В точке В робот останавливается и выполняет второй разворот влево на 900, после чего движение продолжается в отрицательном направлении оси 20 со скоростью ^ = -V*
до точки очередной остановки и поворота - точки С. Выполнив в точке С поворот
влево на 900, робот продолжает движение в отрицательном направлении оси X0 со скоростью Vxo = -V * до точки начала движения O0.
При установившемся режиме движения БИИ, закреплённый на поясе робота, совершает вертикальные колебания от некоторой высоты Hmin до высоты Hmax. Высота БИИ Hmin соответствует моменту начала или окончания шага, а высота Hmax определяет положение БИИ в верхней точке траектории, когда линия опорной ноги вертикальна.
Изменение вертикальной координаты БИИ h(t) = H(t) - Ноти, где H(t) - текущая высота БИИ над опорной поверхностью, смоделируем уравнением:
h(t) = |y(t)|, y(t) = ym cos (at), (1)
где ym = Hmax - Hmm - диапазон вертикальных колебаний БИИ;
ю - круговая частота. По данным эксперимента время выполнения одного шага при установившейся ходьбе Atm=0,645 c. За это время одна нога делает двойной шаг (stride) длиной L2=1,2 м, а БИИ перемещается по горизонтали на величину L1=L2/2=0,6 м. Средняя скорость движения БИИ 0,93 м/с. Частота fh колебаний координаты h(t) равна 1,55 Гц, а круговая частота юй = 2пfh = 9,73 с 1. Очевидно, что круговая частота ю в
формуле (1) вдвое меньше частоты ®h , т.е. ю = 1 юй = 4,86c 1.
Модель изменения вертикальной скорости БИИ в соответствии с выражением (1) запишем в виде:
Г-у ю-sin coi при y(t)>0; h(t) = \ ■Ут v ■УУ) (2) I ymю • sinюt при y(t) < 0
Величина ym зависит от длины шага L1, длины ноги l и может быть приближенно оценена по формуле ут = l-ф2 -0,25L2 [1]. Для l=1 м, Li=0,6 м получим ym~0,06 м.
При движении робота БИИ совершает угловые колебания по тангажу, крену и рысканью. Колебания по тангажу происходят с частотой вертикальных колебаний БИИ, т.е. f& = fh = 1,55 Гц, ю& = 2f = 9,73 c-1 = 2ю.
Допустим, что при установившемся движении БИИ совершает гармонические колебания по тангажу в диапазоне от 00 до 2&т. Значение & = 0 соответствует положению БИИ по высоте при h = уи, а значение & = 2&и соответствует положению h=0, т.е. в начале каждого шага и в момент его завершения. Таким образом, при завершении каждого шага корпус робота наклоняется вперед.
Уравнение гармонических колебаний по тангажу, согласованное по фазе с колебаниями h(t), имеет вид:
&(t) = &m -К cos(2Qt) (3)
При симметричной ходьбе колебания БИИ по крену должны быть симметричны относительно нуля, а по рысканью - симметричны относительного заданного направления движения. Доминирующая частота колебаний БИИ по крену и рысканью равна ю = 4,86c-1. Примем, что максимальный по модулю крен будет в момент завершения каждого шага в сторону шагающей ноги. Значение у=0 соответствует положению БИИ, когда h = у . Такая же схема колебаний будет и по рысканью. Значения Ду = Дут, y = ут соответствуют завершению шага с правой ноги, а Ду = -Дут, у = -ут - с левой.
Уравнения изменения углов y(t) и Ay(t), согласованные по фазе с уравнением (1), имеют вид:
Y(t) = Ym sin (rot), Ay(t) = A^m sin (rot). (4)
В соответствии с выражениями (3), (4) модель изменения во времени угловых скоростей A\j/(7), $(0 и j(t) имеет вид:
A\jt(t) = A\|/mro • cos (roí),
d(0 = 2dmG)-sin(2G)i), (5)
Y(0 = Y„®-cos (coi).
Угол курса у изменяется с угловой скоростью
y(í) = \j/*(í) + Ay(0, (6)
где \\í (/) - угловая скорость поворота робота в угловых точках маршрута.
БИИ совершает также линейные колебания в направлении осей z и x. При продольных колебаниях профиль скорости на каждом шаге включает колебания около 0,3 м/с относительно средней скорости ходьбы V* [1]. Модель изменения скорости движения запишем в виде
Vz = V* +AV cos (2ot), (7)
где AV=0,3 м/с.
При боковых колебаниях БИИ его смещение Ax относительно вертикальной плоскости будет наибольшим в момент окончания шага в сторону шагающей ноги. При h=ym смещение Лх=0, а скорость Ai имеет экстремум:
^=-A*»cos(arf). (8)
Среднеквадратичные отклонения сглаженных сигналов с выходов датчиков Вх и Вг равны соответственно 6,7 о/с и 5,5 о/с. Тогда приближенная оценка амплитуд
6 7 6 7
колебаний углов 0 и у будет соответственно = 6- = —:— = 0,69° и
2ш 9, -3
уи =5,5 = = 1,13°. Оценка амплитуды колебаний скорости у при значении ш 4,86 *
( л „ ^ , 2 л • 0,72 0,72 п .. .
а } = 0,72 м/с дает величину Ахт = —— = —— = 0,15 м/с.
ш 4,83
Результаты моделирования
Блок-схема программы моделирования движения робота с БИИ и вычисления координат его движения приведена на рисунке 5.
вычисление
меток
маршрута
3 I
к=1:Ы7
4 \ /
управление движением по маршруту
5 ф
6 ф
Л',;| - начало движения,
N1 - начало 1 поворота,
N2 - начало движения после 1 поворота,
Л'з - начало 2 поворота,
N4 - начало движения после 2 поворота,
N5 - начало 3 поворота,
- начало движения после 3 поворота, N1 — конец пути.
оформление/ результатов/
конец
<--
Расчет параметров углового и поступательного движения БИИ, формирование массивов сигналов ДУС _и акселерометров.
Формирование массивов угловых и линейных координат движения БИИ по алгоритму ИНС.
Рис. 5. Блок - схема алгоритма моделирования движения робота
Алгоритм формирования сигналов датчиков и выходных сигналов ПНС построен по схеме:
1. Определение проекций вектора угловой скорости БИИ на оси связанной системы координат
соб & Бт у соб у 0
= соб & соб у - Бт у 0
- Бт & 0 1
У* +А\\1 У
(9)
где переменные в правой части определяются по формулам (3) - (6).
2. Определение проекций кажущегося ускорения на оси, параллельные осям неподвижной системы координат
«Х0 (0 = (0 -УХо (г ~ 1)) / Л/, (/) = (У2о (0 -У2о (г -1)) / Л/,
aYí¡{i) = {h{i)-h(i-\))l^t + g.
(10)
3. Определение сигналов на выходах датчиков угловых скоростей (ДУС)
= ^ДУС (Р)■ С • Ю ,
® *
(11)
где WдУc(p) - передаточная функция ДУС (для идеальных ДУС WдyС ^)=1);
С - матрица, формирующая входную информацию ДУС (для идеальных ДУС С - единичная матрица).
4. Определение проекций кажущегося ускорения на оси связанной системы координат
ах а Х0
ау = В (у, &, у) аг 10
а ¿0
«х «х
= Ккс (р)
я*
где В (у, Э, у) - матрица направляющих косинусов между осями неподвижной и
связанной систем координат;
у=у* +ДуДу - кинематические переменные, формируемые в модели движения робота.
5. Определение сигналов на выходах акселерометров
(13)
где Жакс(р) - передаточная функция акселерометра (для идеальных датчиков
^акс(р)=1).
6. Вычисление углов рысканья, тангажа, крена (у (0) = О, Э( 0) = 0, у( 0) = 0.
(14)
7. Определение проекций выходных сигналов акселерометров на оси неподвижной системы координат
у( ') у(/ 1) СОБ Э(/ -1) у( 1 1) СОБ у(/ -1) 0 -1 (0
Э( /) = Э(/ 1) + СОБ Э( 1 -1) СОБ у(/ -1) - Бт у( 1 -1) 0 й,0') ■Дг
у(') у(/ -1) - бШ Э( 1 -1) 0 1 (0
а
■X,
,(0 (О
аг
К (0
втЫ0Шу(0)
«*(0 йу{ О
(О
(15)
8. Определение проекций вектора путевой скорости на оси неподвижной системы координат
9. Определение горизонтальных координат местоположения БИИ в неподвижной системе отсчета
Хг,
0БИИ (/•)= Х0БИИ (• -1) + *Г (• ) = *0БИИ (•-1) + )■* . (17)
В идеальном случае при отсутствии угловых и линейных гармонических колебаний БИИ траектория движения робота, построенная по выходным данным ПНС, имеет наименьшие искажения по сравнению с действительной линией пути (рисунок 6).
Рис. 6. Траектория движения робота при отсутствии гармонических колебаний БИИ Оценим погрешность ПНС по отклонению R конечной точки пути от начальной в процентах от общей протяженности маршрута S, равной в рассмотренном примере 16 м. Как видно из рисунка 6, отклонение R-0,57 м -3,5% &
Расчет координат траектории при моделировании движения БИИ с учетом его угловых и линейных гармонических колебаний с амплитудами, найденными по результатам реального эксперимента, естественно, приводит к неудовлетворительным результатам, т.к. в модели ПНС отсутствует по начальному допущению внешняя коррекция, использующая, например, 2ЦРГ-технологию [7].
Наименьшее влияние на погрешность счисления пути оказывают колебания БИИ по рысканью. При колебаниях по крену и тангажу погрешности заметно возрастают. При моделировании движения робота с учетом колебаний БИИ только по крену или тангажу погрешность определения координат конечной точки пути линейно зависит от амплитуд этих колебаний, что отражено на рисунке 7.
Рис. 7. Зависимость погрешности определения координат конечной точки пути от амплитуды колебаний БИИ по крену и тангажу
При линеаризации зависимостей Я (ут), Я (&и) методом наименьших квадратов
получены уравнения
Я (Ут ) = 6,806уот + 0,1155, Я (&т ) = 45,85&т + 0,335. (9)
Как видно из рисунка погрешность счисления пути в большей степени зависит от амплитуды колебаний БИИ по тангажу, чем от амплитуды колебаний БИИ по крену. Так при равных амплитудах колебаний, например, 0,5о, погрешность при колебаниях БИИ по тангажу равна 23,5 м, а при колебаниях по крену - 3,5 м.
Полученные результаты могут быть учтены при синтезе алгоритма ПНС, в которой для повышения точности счисления пути должна быть предусмотрена также внешняя коррекция, использующая дополнительную информацию о движении пешехода [7].
Выводы
1. Гармонические колебания БИИ как угловые, так и линейные, вызывают существенные погрешности в работе ПНС без коррекции при определении траектории БИИ даже при идеальных характеристиках датчиков.
2. Наибольшее влияние из угловых колебаний на погрешность определения местоположения человека оказывают колебания БИИ относительно плоскости горизонта, при этом величина погрешности определения координат местоположения линейно зависит от амплитуд колебаний БИИ по тангажу и крену.
3. При равных амплитудах угловых колебаний БИИ по тангажу и крену колебания по тангажу вызывают погрешность определения координат
местоположения человека в семь раз больше, чем колебания по крену.
Библиографический список
1. Дэвидсон П., Такала Я. Разработка алгоритмов инерциальной навигационной системы с учетом особенностей походки человека // Гироскопия и навигация. 2013. № 1(80). С. 86-94.
2. Нгуен Ньы Ман. Разработка математической модели погрузочно-разгрузочного устройства с всенаправленными колесами // Труды МАИ, 2012, №58: http://www.mai.ru/science/trudy/published.php?ID=33459
3. Матвеев В.В., Распопов В.Я. Основы построения бесплатформенных инерциальных навигационных систем. - СПб.: Электроприбор, 2009. - 280 с.
4. Официальный интернет-сайт компании «Analog devices». Техническое описание блока инерциальных измерителей ADIS16405, URL: http://www.analog.com/ru/index.html
5. MATLAB & Simulink Release Notes for R2008a, URL: http:www.mathworks.com
6. Бендат Д., Пирсол А. Прикладной анализ случайных данных. - М.: Мир, 1989. - 540 с.
7. Маринушкин П.С., Бахтина В.А., Подшивалов И.А., Стукач О.В. Вопросы разработки инерциальных пешеходных навигационных систем на основе МЭМС -датчиков // Наука и Образование. 2015. № 6. С. 157-173.
