Для чего нужен ОрбиКрафт
Подсистемы конструктора
Инструкции по работе с ОрбиКрафт
Уроки
Лабораторная оснастка
Знакомство с Arduino
Полезная нагрузка на базе Arduino
Обратная связь
Новости
Для чего нужен ОрбиКрафт
Подсистемы конструктора
Инструкции по работе с ОрбиКрафт
Уроки
Лабораторная оснастка
Знакомство с Arduino
Полезная нагрузка на базе Arduino
Обратная связь
Новости
Определение угла по магнитометру
В представленном ниже видео спутник 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 для запуска программы калибровки
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