3.10.2. Калибровка датчика

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

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

Несколько сложнее дело обстоит с магнитометром. Наилучшие условия работы магнитометра наблюдаются на экваторе. Это связано с тем, что вектор магнитного поля Земли параллелен ее поверхности. Рассмотрим визуализацию показаний магнитометра (рис. 3.8).

Рис. 3.8. Визуализация показаний магнитометра

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

Рис. 3.9

71

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

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

Рис. 3.10. Визуализация показаний магнитометра по трем осям

Искажения магнитного поля бывают жесткие и мягкие. Изображение на рис. 3.11 иллюстрирует различия между ними. Рассмотрим жесткие искажения. Интенсивность магнитного поля Земли довольно сильно зависит от текущего местоположения измерений. Так, в Кейптауне оно составляет в среднем 0.256 Гс, а в Нью-Йорке 0.52 Гс. Для сравнения магнитное поле обычного магнита на холодильник составляет порядка 50 Гс. Вследствие этого, на показания магнитометра могут влиять множество предметов вокруг. Данный вид искажений создает смещение показаний датчика. Для компенсации жестких искажений необходимо вычислить вектор смещения и просто вычесть его из результатов измерений.

Рис. 3.11. Виды искажения магнитного поля

72

В отличие от жестких искажений, искажения мягкого типа компенсировать намного сложнее. Из рис. 3.10 видно, что сфера имеет вытянутую форму (сфероид). Проекция сферы на ось YZ немного сплющена и имеет некоторый поворот. Данный вид искажений появляется вследствие воздействия на датчик находящихся рядом ферромагнитных материалов. В основном такими материалами являются различные металлы. Для компенсации данного вида искажений необходимо вычислить масштабный коэффициент для каждой из осей и умножить на него показания. Затем необходимо повернуть полученный вектор на угол, противоположный углу поворота сфероида.

Для компенсации жестких и мягких искажений удобно воспользоваться линейной алгеброй и векторной геометрией. Компенсация жестких искажений имеет следующий вид

									(3.7)
метра,						– трехкомпонентный вектор результирующих показаний магнито-
на		где
					– трехкомпонентный вектор с измеренными показаниями магнито-
метра,				– трехкомпонентный вектор смещения магнитного поля, вычисленный
	этапе калибровки датчика.
		Затем полученные данные необходимо умножить на матрицу калибровки
искажений мягкого типа
									(3.8)
		где – матрица компенсации искажений мягкого типа, полученная на
этапе		калибровки датчика.
		Матрица имеет три строки и три столбца и является с точки зрения век-
торной				математики матрицей поворота и масштабирования.
		Вычисление вектора и матрицы является достаточно трудоемкой за-
дачей. Для этих целей разработано								специализированное	программное обеспе-

чение – Magneto 1.2. Внешний вид главного окна данной программы изображен на рис. 3.12. Программа осуществляет расчет параметров калибровки ( и ) путем экстраполирования параметров сферы, изображенной на рис. 3.10. Исходными данными для программы является файл с показаниями магнитометра, полученными при вращении датчика по всем возможным осям и направлениям. После указания файла и нажатия кнопки «Calibrate» происходит вычисление параметров калибровки. Полученные данные выводятся в соответствующих полях.

Аналогичные действия происходят и при калибровке акселерометра. Сперва также происходит построение сферы из показаний датчика. Полученные точки заносятся в файл, который загружается в программу Magneto. Остается лишь вставить полученные данные в программный код и произвести вычисления согласно формулам (3.7) и (3.8).

73

Рис. 3.12. Окно программы Magneto 1.2

Таким образом, получена методика калибровки инерциального датчика для коррекции мягких и жестких искажений магнитного поля. Описана программа Magneto 1.2, позволяющая производить калибровку в полуавтоматическом режиме.

3.10.3. Преобразование данных инерциального датчика

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

В данной работе применен относительно новый способ вычисления текущей ориентации устройства – фильтр Маджвика. Данный фильтр существует в двух вариациях. Первая использует в качестве входных данных показания акселерометра и гироскопа. Вторая дополнительно учитывает еще показания

74

трехосевого магнитометра. Обе реализации на выходе выдают кватернион поворота роботизированного устройства в пространстве. Затем кватернион преобразуется в углы Эйлера, показывающие значения крена, тангажа и рысканья. Отличие в реализациях лишь в том, что при использовании первого варианта величина рысканья измеряется относительно первоначальной ориентации, в которой устройство находилось при включении, а при использовании второго – относительно направления на север. Реализация инерционной навигационной системы с магнитометром подразумевает компенсацию магнитных искажений магнитометра и дрейфа нуля гироскопа. Об этом уже было написано в предыдущих разделах, в том числе о методах калибровки датчика. Фильтр во время расчетов оперирует кватернионами. Он использует показания инерциального датчика для аналитических вычислений и оптимизации методом градиентного спуска. Перечислим главные преимущества фильтра Маджвика:

•малая требовательность к вычислительным ресурсам (около 277 арифметических операций за одну итерацию расчетов);

•эффективность при использовании низких частот дискретизации (порядка 10 Гц);

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

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

Акселерометр и магнитометр позволяют измерять гравитационные и магнитные поля планеты, вследствие чего становится возможным определение абсолютной ориентации в пространстве. Однако, акселерометр и магнитометр сильно подвержены влиянию шума. Для примера, ускорение из-за движения датчика приведет к сдвигу измеряемого вектора силы тяжести. Таким образом, задачей фильтра ориентации (фильтра Маджвика) является вычисление единой оценки ориентации путем обработки значений всех датчиков.

Кватернион представляет собой четырехмерное комплексное число, которое может быть использовано для представления тела в трехмерном пространстве. Кватернионы в фильтре Маджвика используются для того, чтобы описать ориентацию системы отсчета B относительно системы отсчета A. Данный кватернион определяется выражением (3.9).

1

2

3

4

2

2

2

2 .

(3.9)

75

Кватернион, имеющий противоположное направление имеет обозначение со знаком *. Он применяется для изменения системы отсчета и показан в выра-

жении (3.10).

		.	(3.10)

Для составных поворотов используется операция умножения кватернионов. При помощи правила Гамильтона находится результат умножения. Следует обратить внимание, что результат зависит от порядка множителей. Умножение кватернионов иллюстрируют следующие выражения

(3.11)

(3.12)

										1	2	3		4		1			2		3	4
× = [																	− 2 4			+ 3 1		− 4 2
	1			2	3			4							1	2	+ 2		1		3 4	4 3	(3.13)
															1 4		+ 2 3			− 3 2		− 4 1
Следующее выражение производит поворот вектора на кватернион
где																							(3.14)
			– поворачиваемый вектор.
Кватернион может быть преобразован в матрицу поворота при помощи
выражения (3.15), а (3.16-3.18) преобразуют кватернион в углы Эйлера.
=					12	3			1	42	2 122		−3	1 + 12 432			2( 32 4				+ 1 23) .		(3.15)
				2( 2		4	− 1			3)	2( 3		4	+ 1 2)			2 1	−1 + 2; 4					(3.16)
											2	3		1	4		2		2			2
																1		;					(3.17)
											3	4		2	4		1 3		4		.		(3.18)
														1	2	1

Рассмотрим вычисление ориентации роботизированного устройства из угловой скорости, измеренной гироскопом. Преобразуем показания гироскопа в кватернион при помощи (3.19). Угловая скорость в системе отсчета Земли по отношению к системе отсчета инерционного датчика описывается выражением (3.20). Ориентация в глобальной системе отсчета по отношению к локальной в момент времени t вычисляется путем интегрирования кватернионов производных, как показано в уравнениях (3.21) и (3.22). При этом начальная ориентация роботизированного устройства в пространстве предполагается известной.

76

									,	(3.20)
	̇=		2	× ,						(3.19)
									,	(3.21)
	,			2	,		,			(3.22)
			,−1

Акселерометр позволяет производить измерения величины и направления вектора силы тяжести в локальной системе координат датчика в совокупности с линейными ускорениями, обусловленными движением датчика. Магнитометр, в свою очередь, производит измерения величины и направления магнитного поля Земли, также в совокупности с локальными искажениями.

Если перевести вектор магнитного поля в глобальную систему координат, то измерения в локальной системе координат позволят рассчитать координаты датчика по отношению к координатам Земли. Однако, при таком подходе будет получено бесконечное множество решений, которые будут представлены всеми ориентациями, достигаемыми за счет вращения ориентации вокруг вращения, параллельного направлению поля (магнитного поля Земли либо поля гравитации). Решения можно достичь при использовании кватернионов и решения задачи оптимизации выражения (3.23).

4			.		(3.23)
Целевую функцию определяет следующее выражение
				.	(3.24)
Компоненты каждого вектора имеют вид, показанный в выражениях
(3.25-3.27).
			,		(3.25)
			,		(3.26)
			.		(3.27)

Оптимизация выполняется при помощи алгоритма градиентного спуска, описанного выражением (3.28).

+1
							̂
				, ,				.	(3.28)
Здесь – величина шага оптимизации, – число итераций.

77

Вычисление градиента поверхности решений, определяемого целевой функцией, происходит при помощи выражений (3.29-3.31).

		1																					(3.29)
	2					2			2						( 2	4		− 1		3) −
		2			− 3		− 4 + 2 ( 1 4 + 2 3) + 2																(3.30)
=					3	− 1			4)	1	2	2	+ 2		( 1	2		+ 3		4)		−
	2 ( 2									+ 2 2	− 2 − 4
	2 ( 1				3	+ 2			4)	+ 2 ( 3	4 − 1 2) + 2				1 − 22 − 32							−
								̂		−2 4	+ 2 2		3		2			2	+ 2		1
	(														4
				,				) =		2 3 −2		2	2	4	−2			1	−4 2				(3.31)
			−4 3						+ 2 2 −2 1			−4 4		+ 2 1			+ 2 2
						2 2				+ 2		−2 1 − 4 4					+ 2 3
			2 1				+ 2 4 −42 3						2 2		+ 2 3

Данные формулы описывают общий вид алгоритма, применимого к полю, ориентированному в любом направлении. Однако процесс вычислений можно упростить, если считать направление поля только в одной либо двух осях глобальной системы координат. Таким образом, направление силы тяжести предполагается направленным всегда вертикально, это показывает выражение (3.32). При подставлении кватерниона и нормализации измерений получается выражение (3.35).

								,				(3.32)
					2( 1 2				,			(3.33)
		,					+ 3 4) −				,	(3.34)
(				) = 2		21 − 22		− 32	−
( ) = 2 2						2 1		2 4		2 3 .		(3.35)
				0		−4 2		−4 3		0

Если считать, что магнитное поле Земли располагается в одной плоскости в горизонтальной и вертикальной осях, то вертикальная составляющая будет зависеть от той точки на поверхности Земли, где производятся измерения. Это представляется уравнением (3.36). При подстановке нормализованных значений измерений получим выражения (3.37-3.39).

78

																	,									(3.36)
											3								,		1 3					(3.37)
		,							2 3				4				2	4								(3.38)
												1 4					1 2			3 4
(				,		) = 2			(		−		) + 2			(			+		) −
							2 ( 1			3 + 2			4) + 2		(0.5 − 2					− 3) −
	−2 3											−4 3 − 2 1							2	4	2
																					4			2
							2 4																		.	(3.39)
= −2 4			+ 2 2				2 3	+ 2 1				2 2		+ 2 4					−2 1 + 2					3
		2 3					2 4	−4 2				2 1		− 4 3							2 2

Одни лишь измерения силы тяжести и вектора магнитного поля Земли не могут быть использованы для определения ориентации инерциального датчика. Для решения данной задачи необходимо объединение направления обоих полей, как показано в выражениях (3.40-3.41).

, (	,		,				,			)	=														(3.40)
																(		,					)		(3.41)
				(																	,
	,					,
								) =								,
												(								)

Процесс оптимизации потребует выполнения нескольких итераций. Однако, при условии скорости сходимости алгоритма больше либо равной физической скорости измерения ориентации, будет достаточно всего одной итерации. Уравнение (3.42) производит вычисление приблизительного направления ориентации на основе предыдущей ориентации.

=									,			,−1					‖ ‖ ,									(3.42)
									, −1								,−1									(3.43)
								,										,		,			,		.
	,				,−1						,					,−1

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

79

Рис. 3.13. Блок-схема фильтра Маджвика без магнитометра

Данный алгоритм можно применять при передвижении роботизированного устройства в помещениях, где искажения вектора магнитного поля Земли наиболее высокие.

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

Рис. 3.14. Блок-схема фильтра Маджвика с магнитометром

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

80