User Tools

Translations of this page:

Site Tools


en:mag_calibration

This is an old revision of the document!


startSectionEdit: $data "plugin_wrap_start" is NOT an array! One of your plugins needs an update.
startSectionEdit: $data "plugin_wrap_end" is NOT an array! One of your plugins needs an update.
startSectionEdit: $data "plugin_wrap_start" is NOT an array! One of your plugins needs an update.
startSectionEdit: $data "plugin_wrap_end" is NOT an array! One of your plugins needs an update.

FIXME This page is not fully translated, yet. Please help completing the translation.
(remove this paragraph once the translation is finished)

Определение угла по магнитометру
В представленном ниже видео спутник 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
en/mag_calibration.1564576587.txt.gz · Last modified: 2019/07/31 15:36 by golikov