Инструменты пользователя

Перевод этой страницы:

Инструменты сайта


Боковая панель

Для чего нужен ОрбиКрафт

Как с этим работать

Подсистемы конструктора

Полезная нагрузка на базе Arduino

Уроки

Лабораторная оснастка

Обратная связь

Новости

mag_calibration

Определение угла по магнитометру
В представленном ниже видео спутник OrbiCraft разворачивается Web-камерой в сторону макета Земли. Информацию об ориентации спутник получает по датчику угловой скорости и магнитометру. Информация об угле нужна для того чтобы разворачивать спутник в необходимую сторону, а информация об угловой скорости необходима для того чтобы стабилизировать спутник, т.е. погасить угловое вращение. Управляющий момент создается с помощью двигателя маховика.

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

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

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

#include "libschsat.h"
 
void control(void)
{
	int i;
	const int num = 1;	/* magnetometer #1 */  
	printf("Enable magnetometer #%d\n", num);  
	magnetometer_turn_on(num);  
	printf("Get RAW data from magnetometer #%d\n", num);  
	for (i = 0; i < 1000; i++) {
		int16_t x, y, z;
		if (LSS_OK == magnetometer_request_raw(num, &x, &y, &z)) {
			printf("%d: x=%d y=%d z=%d\n", i, x, y, z);
		} else {
			puts("Fail!");
		}
		mSleep(100);
	}
	printf("Disable magnetometer #%d\n", num);
  	magnetometer_turn_off(num);
}

Полученные данные измерений необходимо сохранить в файл с расширением .txt Для этого необходимо нажать Ctrl+A, и затем Ctrl+C, вставить в блокнот и сохранить.

Скачать пример файла с данными.

Кроме данных в файле будут «лишние» строки с отладочной информацией. Эти лишние строки удалять не надо. Программа калибровки автоматически найдет строки с данными и вытащить необходимые значения.

Скачать Anaconda Navigator для запуска программы калибровки

Необходимо выбрать версию Python не ниже 3.6

Anaconda является мощной бесплатной интерактивной средой для Jupiter Notebook (iPython). В этом примере нам понадобится установить Anaconda для того чтобы запустить программу калибровки магнитометра. Перед скачиванием программы калибровки можно ознакомиться с ее содержимым

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

def mag_calibrated(magx,magy,magz):
	# эти 3 строки необходимо заменить на строки из программы калибровки
	magx_cal = 1.00*magx + 0.00*magy + 0.00*magz + 0.00
	magy_cal = 0.00*magx + 1.00*magy + 0.00*magz + 0.00
	magz_cal = 0.00*magx + 0.00*magy + 1.00*magz + 0.00
	return magx_cal, magy_cal, magz_cal
Выше представлены калибровочные коэффициенты идеального магнитометра. Вместо них необходимо использовать коэффициенты, которые выдаст программа калибровки.

По проекциям магнитного поля можно вычислить угол между осью x магнитометра и проекцией магнитного поля на плоскость OXY магнитометра.

import math
 
# Для того чтобы положительное направление угла определялось против часовой стрелки,
# необходимо перейти из левой системы координат, которая изображена на магнитометре,
# в правую систему координат, для этого меняем знак измерения вдоль оси Y "magy_cal" на противоположный
 
magx_cal, magy_cal, magz_cal = mag_calibrated(magx_raw,magy_raw,magz_raw)
magy_cal = - magy_cal
mag_alpha = math.atan2(magy_cal, magx_cal)/math.pi*180

Для определения команды управления для маховика важно знать не угол разворота спутника относительно магнитного поля, а угловую ошибку - разницу между текущим углом, и целевым углом alpha_goal По умолчанию угол определяется в диапазоне от -180 до 180 градусов Функция angle_transformation изменяет диапазона измерения угла. Для решения задачи ориентации угол необходимо измерять в диапазоне от (alpha_goal-180) до (alpha_goal+180), где alpha_goal - это целевой угол, между осью x магнитометра и проекцией вектора магнитного поля на горизонтальную плоскость.

def angle_transformation(alpha, alpha_goal):
	if alpha<(alpha_goal - 180):
		alpha = alpha + 360
	elif alpha>(alpha_goal +180):
		alpha = alpha - 360
	return alpha

Полученный угол можно использовать для управления спутником по правилу PD-регулятора. Новая скорость маховика равна текущей скорости маховика + приращение скорости. Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости. Коэффициенты kp и kd предлагается подобрать самостоятельно.

#mtr_speed - текущая скорость маховика
#alpha, omega - текущие угол и угловая скорость спутника
#alpha_goal, omega_goal - целевые угол и угловая скорость спутника
 
def motor_new_speed_PD(mtr_speed, alpha, alpha_goal, omega, omega_goal):
	mtr_new_speed = int(mtr_speed + kp*(alpha-alpha_goal) + kd*(omega-omega_goal))
	return mtr_new_speed
mag_calibration.txt · Последние изменения: 2018/08/30 12:48 — zhumaev

Инструменты страницы