Skip to main content

07 Урок. Ориентация Орбикрафт с помощью магнитометра

Программа ориентации по магнитометру​

Создайте следующую программу. Коэффициенты для функции def mag_calibrated возьмите из программы Magneto.

Коэффициенты

Picture 1. Коэффициенты

Код на Python:​

import math
 
time_step = 0.1     # Временной шаг работы алгоритма, с
omega_goal = 0.0    # Целевая угловая скорость спутника, град/с. Для режима стабилизации равна 0.0.
alpha_goal = 0      # Целевой угол поворота
mtr_max_speed = 5000    # Максимально допустимая скорость маховика, об/мин
mtr_num = 1     # Номер маховика
hyr_num = 1     # Номер AVS
mag_num = 1     # Номер магнитометра
 
 
errora = 0              # Ошибка по углу
Pa = 0              # Воздействие пропорционального звена
Da = 0              # Воздействие дифференциального звена
Ia = 0              # Воздействие интегрального звена
Kpa = 200       # Пропорциональный коэфициент ошибки по углу
Kda = 0.02              # Дифференциальный коэффициент
Kia = 0.5               # Интегральный коэффициент
lastErrora = 0          # Прошлая ошибка по углу
Integratora = 0         # Интеграл (сумма всех ошибок по углу)
PID_alpha = 0       # Величина управляющего воздействия
Integrator_maxa = 10    # Ограничение максимального значения интергатора
Integrator_mina = -10   # Ограничение минимального значения интергатора
 
error = 0           # Ошибка по угловой скорости
P = 0               # Воздействие пропорционального звена
D = 0               # Воздействие дифференциального звена
I = 0               # Воздействие интегрального звена
Kp = 200                # Пропорциональный коэффициент
Kd = 0.02               # Дифференциальный коэффициент
Ki = 0.5                # Интегральный коэффициент
lastError = 0           # Прошлая ошибка по угловой скорости
Integrator = 0          # Интеграл (сумма всех ошибок по угловой скорости)
PID_omega = 0           # Величина управляющего воздействия
Integrator_max = 10     # Ограничение максимального значения интергатора
Integrator_min = -10    # Ограничение минимального значения интергатора
 
# Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов
def mag_calibrated(magx,magy,magz):
    magx_cal = 1.451626*(magx + 47.687257) + 0.036309*(magy + -1468.312497) + 0.008968*(magz + 48.449050)
    magy_cal = 0.036309*(magx + 47.687257) + 0.408031*(magy + -1468.312497) + 0.074774*(magz + 48.449050)
    magz_cal = 0.008968*(magx + 47.687257) + 0.074774*(magy + -1468.312497) + 1.429262*(magz + 48.449050)
    return magx_cal, magy_cal, magz_cal
 
# Функции для определение новой скорости маховика.
# Новая скорость маховика складывается из
# текущей скорости маховика mtr_speed и приращения скорости PID
# Приращение скорости получено путем сложения трех коэффициентов
# P - пропорциональный коэффициент регулятора
# I - интегральный коэффициент регулятора
# D - дифференциальный коэффициент регулятора
# Integrator накапливает суммарную ошибку
# Сатуратор ограничивает максимальную величину суммарной ошибки
# mtr_speed - текущая угловая скорость маховика, об/мин
# omega - текущая угловая скорость спутника, град/с
# omega_goal - целевая угловая скорость спутника, град/с
# mtr_new_speed - требуемая угловая скорость маховика, об/мин
def motor_new_speed_PD(mtr_speed, alpha, alpha_goal, omega, omega_goal):
    global Integrator
    global lastError
    global Integratora
    global lastErrora   
 
    error = omega - omega_goal                  # Вычисление ошибки
    P = Kp * error                              # Вычисление воздействия пропорционального звена
    D = Kd * (error - lastError) / time_step    # Вычисление воздействия дифференциального звена
    lastError = error                           # Запоминаем ошибку
    Integrator = Integrator + error * time_step # Накапливаем суммарную ошибку
    if Integrator > Integrator_max:             # Сатурация (Ограничиваем максимальное значение накапливаемой ошибки)
        Integrator = Integrator_max
    elif Integrator < Integrator_min:
        Integrator = Integrator_min
    I = Integrator * Ki             # Вычисление воздействия интегрального звена
    PID_omega = P + I + D           # Вычисление суммарного управляющего воздействия
 
    errora = alpha - alpha_goal                 # Вычисление ошибки
    Pa = Kpa * errora                               # Вычисление воздействия пропорционального звена
    Da = Kda * (errora - lastErrora) / time_step    # Вычисление воздействия дифференциального звена
    lastErrora = errora                         # Запоминаем ошибку
    Integratora = Integratora + errora * time_step # Накапливаем суммарную ошибку
    if Integratora > Integrator_maxa:           # Сатурация (Ограничиваем максимальное значение накапливаемой ошибки)
        Integratora = Integrator_maxa
    elif Integratora < Integrator_mina:
        Integratora = Integrator_mina
    Ia = Integratora * Kia             # Вычисление воздействия интегрального звена
    PID_alpha = Pa + Ia + Da            # Вычисление суммарного управляющего воздействия    
 
    mtr_new_speed = int(mtr_speed + PID_omega + PID_alpha)
    if mtr_new_speed > mtr_max_speed:
        mtr_new_speed = mtr_max_speed
    elif mtr_new_speed < -mtr_max_speed:
        mtr_new_speed = -mtr_max_speed
    print "P= ", P, "I= ", I, "D= ", D, "PID_omega= ", PID_omega, "PID_alpha=", PID_alpha, "mtr_new_speed= ", mtr_new_speed  
    print "Pa= ", Pa, "Ia= ", Ia, "Da= ", Da, "PID_alpha=", PID_alpha
    return mtr_new_speed
 
 
# Функция включает все приборы,
# которые будут использоваться в основной программе.
def initialize_all(): # Функция инициализации всех систем
    print "Enable motor №", mtr_num 
    motor_turn_on(mtr_num)
    sleep(1)
    print "Enable angular velocity sensor №", hyr_num 
    hyro_turn_on(hyr_num) # Включаем AVS
    sleep(1)
    print "Enable magnetometer", mag_num
    magnetometer_turn_on(mag_num)
    sleep(1)
 
def switch_off_all(): # Функция выключения всех систем
    print "Finishing..."
    print "Disable angular velocity sensor №", hyr_num
    hyro_turn_off(hyr_num)   # Выключаем AVS
    print "Disable magnetometer", mag_num
    magnetometer_turn_off(mag_num)
    motor_set_speed(mtr_num, 0)
    sleep (1)
    motor_turn_off(mtr_num)
    print "Finish program"
 
 
# Основная функция программы, в которой вызываются остальные функции.
def control():
    initialize_all()
    mtr_state = 0       # Инициализируем статус маховика
    hyro_state = 0      # Инициализируем статус AVS
    mag_state = 0       # Инициализируем статус магнитометра
    alpha_goal = 90     # Целевой угол
    omega_goal = 0      # Целевая угловая скорость
    mag_alpha = 0
 
    for i in range(200):
        #print "i = ", i
 
        # Опрос датчика угловой скорости и маховика.
        mag_state, magx_raw, magy_raw, magz_raw = magnetometer_request_raw(mag_num)
        hyro_state, gx_raw, gy_raw, gz_raw = hyro_request_raw(hyr_num) 
        mtr_state, mtr_speed = motor_request_speed(mtr_num)     
 
        if not mag_state: # если магнитометр вернул код ошибки 0, т.е. ошибки нет
            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
            #mag_alpha = angle_transformation(mag_alpha, alpha_goal)
            #print "magx_cal =", magx_cal, "magy_cal =", magy_cal, "magz_cal =", magz_cal # Вывод откалиброванных значений магнитометра
            print "mag_alpha atan2= ", mag_alpha
        elif mag_state == 1:
            print "Fail because of access error, check the connection"
        elif mag_state == 2:
            print "Fail because of interface error, check your code"
 
        # Обработка показаний датчика угловой скорости,
        # вычисление угловой скорости спутника по показаниям AVS.
        # Если код ошибки AVS равен 0, т.е. ошибки нет
        if not hyro_state:
            gx_degs = gx_raw * 0.00875
            gy_degs = gy_raw * 0.00875
            gz_degs = gz_raw * 0.00875
            # если AVS установлен осью z вверх, то угловая скорость
            # спутника совпадает с показаниями AVS по оси z, иначе
            # необходимо изменить знак: omega = - gz_degs
            omega = gz_degs
            #print "gx_degs =", gx_degs, "gy_degs =", gy_degs, "gz_degs =", gz_degs
        elif hyro_state == 1:
            print "Fail because of access error, check the connection"
        elif hyro_state == 2:
            print "Fail because of interface error, check your code"
 
        #Обработка показаний маховика и установка требуемой угловой скорости.
        if not mtr_state:   # если код ошибки 0, т.е. ошибки нет
            print "Motor_speed: ", mtr_speed
            # установка новой скорости маховика
            mtr_new_speed = motor_new_speed_PD(mtr_speed, mag_alpha, alpha_goal, omega, omega_goal)
            motor_set_speed(mtr_num, mtr_new_speed)
 
        sleep(time_step)
 
    switch_off_all()

Код на С:​

#include <stdio.h>
#include <stdint.h>
#include "libschsat.h"
#define LSS_OK 0 
#define LSS_ERROR 1 
#define LSS_BREAK 2
#include <math.h>
 
const int kd = 200; /* К-т дифференциальной обратной связи. 
Если угловая скорость спутника положительна, то спутник надо раскручивать
 по часовой стрелки, т.е. маховик надо разгонять по часовой стрелке.*/
 
const int kp = -50; /* К-т пропорциональной обратной связи. 
Если текущий угол больше целевого, то спутник надо вращать против 
часовой стрелки, соответственно маховик надо разгонять против часовой стрелки.*/
const float time_step = 0.05; // Временной шаг работы
const uint16_t mtr_num = 1;         // Номер маховика
const uint16_t hyr_num = 1;                 // Номер AVS
const uint16_t mag_num = 1;         // Номер магнитометра
// Максимально допустимая скорость маховика, об/мин
const int16_t mtr_max_speed = 5000;
int16_t mtr_new_speed;
 
int mag_calibrated(int16_t *magx, int16_t *magy, int16_t *magz ){
//Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов
    float magx_cal;
    magx_cal = 1.06*(*magx + -7.49) + -0.01*(*magy + -23.59) + 0.07*(*magz + -108.24);
    float magy_cal;
    magy_cal = -0.01*(*magx + -7.49) + 1.11*(*magy + -23.59) + 0.09*(*magz + -108.24);
    float magz_cal;
    magz_cal = 0.07*(*magx + -7.49) + 0.09*(*magy + -23.59) + 1.00*(*magz + -108.24);
    *magx = (int16_t) magx_cal;
    *magy = (int16_t) magy_cal;
    *magz = (int16_t) magz_cal; 
    return 0;
}
 
int motor_new_speed_PD(int mtr_speed, int omega, int omega_goal){   
    /* Функция для определения новой скорости маховика.
 Новая скорость маховика складывается из
 текущей скорости маховика и приращения скорости.
 Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости.
 mtr_speed - текущая угловая скорость маховика, об/мин
 omega - текущая угловая скорость спутника, град/с
 omega_goal - целевая угловая скорость спутника, град/с
 mtr_new_speed - требуемая угловая скорость маховика, об/мин*/
 
    mtr_new_speed = (int)(mtr_speed + kd * (omega - omega_goal));   
    if (mtr_new_speed > mtr_max_speed)
    {
        mtr_new_speed = mtr_max_speed;
    }
    else if (mtr_new_speed < -mtr_max_speed)
    {
        mtr_new_speed = -mtr_max_speed;
    }
    return mtr_new_speed;
}
 
void initialize_all(void){
/*   Функция инициализации всех систем,
    включает все приборы,которые будут использоваться в основной программе.*/
    printf ("Enable motor №%d\n", mtr_num);
    motor_turn_on(mtr_num);
    Sleep(1);
    printf("Enable angular velocity sensor №%d\n", hyr_num); 
    hyro_turn_on(hyr_num); // Включаем AVS
    Sleep(1);
    printf("Enable magnetometer %d\n", mag_num);
    magnetometer_turn_on(mag_num);
    Sleep(1);
}
 
int switch_off_all(){
/*Функция отключает все приборы,которые будут использоваться в основной программе.*/
    printf("Finishing...");
    int16_t new_speed = mtr_new_speed; 
    printf("\nDisable angular velocity sensor №%d\n", hyr_num);
    hyro_turn_off(hyr_num);
    printf("Disable magnetometer %d\n", mag_num);
    magnetometer_turn_off(mag_num);
    motor_set_speed(mtr_num, 0, &new_speed);
    Sleep (1);
    motor_turn_off(mtr_num);
    printf("Finish program\n");
    return 0;
}
 
int control(){
    float mtr_new_speed;
    //данные магнитометра
    int16_t mgx_cal=0;
    int16_t mgy_cal=0;
    int16_t mgz_cal=0;
    int16_t *magx_raw = &mgx_cal;
    int16_t *magy_raw = &mgy_cal;
    int16_t *magz_raw = &mgz_cal;
    //данные AVS
    int16_t gx_raw;
    int16_t gy_raw;
    int16_t gz_raw;
    int16_t *hyrox_raw=&gx_raw;
    int16_t *hyroy_raw= &gy_raw;
    int16_t *hyroz_raw = &gz_raw;
    int16_t mtr_speed;
    int16_t omega;
    int16_t omega_goal=0;  // Целевая угловая скорость
 
    initialize_all();
 
    int mtr_state = 0;      // Инициализируем статус маховика
    int hyro_state = 0;         // Инициализируем статус AVS
    int mag_state = 0;      // Инициализируем статус магнитометра
//  int alpha_goal = 0;     // Целевой угол     
    double mag_alpha = 0;
    int i;
    for (i = 0; i < 500; i++)   {
        mag_state = magnetometer_request_raw(mag_num, magx_raw, magy_raw, magz_raw);
        hyro_state = hyro_request_raw(hyr_num,hyrox_raw,hyroy_raw,hyroz_raw); 
        gx_raw = *hyrox_raw; 
        gy_raw = *hyroy_raw; 
        gz_raw = *hyroz_raw; 
        mtr_state = motor_request_speed(mtr_num, &mtr_speed);
        if (!mag_state){
            mag_calibrated(magx_raw,magy_raw,magz_raw);
            *magy_raw = - *magy_raw;/* переходим из левой системы координат,
            которая изображена на магнитометре в правую, для того чтобы 
            положительное направление угла было против часовой стрелки*/
            mag_alpha = atan2(mgy_cal, mgx_cal)/M_PI*180;
            printf("magx_cal = %d", mgx_cal);
            printf(" magy_cal = %d", mgy_cal);
            printf(" magz_cal = %d", mgz_cal);
            printf(" mag_alpha atan2 = %f\n", mag_alpha);
        }
        else if (mag_state == 1){
            printf("Fail because of access error, check the connection\n");
           }
        else if (mag_state == 2){
            printf("Fail because of interface error, check your code\n");
          }
 
    if (!hyro_state){
            float gx_degs = gx_raw * 0.00875;
            float gy_degs = gy_raw * 0.00875;
            float gz_degs = gz_raw * 0.00875;
            /* если AVS установлен осью z вверх, то угловая скорость
            // спутника совпадает с показаниями AVS по оси z, иначе
             необходимо изменить знак: omega = - gz_degs */
            omega = gz_degs;
            printf("\ngx_degs = %f", gx_degs);
            printf(" gy_degs = %f", gy_degs);
            printf(" gz_degs = %f\n", gz_degs);
        }
        else if (hyro_state == 1){
            printf("Fail because of access error, check the connection\n");
        }
        else if (hyro_state == 2){
            printf("Fail because of interface error, check your code\n");
        }
 
        //Обработка показаний маховика и установка требуемой угловой скорости.
        if (!mtr_state) {
            // если код ошибки 0, т.е. ошибки нет
            int16_t mtr_speed=0;
            motor_request_speed(mtr_num, &mtr_speed);
            printf("\nMotor_speed: %d\n", mtr_speed);   
            // установка новой скорости маховика
            mtr_new_speed = motor_new_speed_PD(mtr_speed,omega,omega_goal);
            motor_set_speed(mtr_num, mtr_new_speed, &omega);
        }   
        Sleep(time_step);   
    }
    switch_off_all();
    return 0;
}

Анализ работы программы​

В программе используются следующие функции для работы с Орбикрафт:

magnetometer_turn_on(num)

– функция включения магнитометра, где num – это номер магнитометра.

magnetometer_turn_off(num)

– функция выключения магнитометра, где num – это номер магнитометра.

magnetometer_request_raw(num)

– функция возвращающая сырые данные измеренные магнитометром с номером num, представляющие собой список из 4 числовых значений.

hyro_turn_on(hyr_num)

– функция включения AVS, где hyr_num – это номер AVS.

hyro_turn_off(hyr_num)

– функция выключения AVS, где hyr_num – это номер AVS.

hyro_request_raw(hyr_num) – функция возвращающая сырые данные измеренные AVS с номером hyr_num, представляющие собой список из 4 числовых значений.

motor_turn_on(num)

– функция включения маховика, где num – это номер маховика.

motor_turn_off(num)

– функция выключения маховика, где num – это номер маховика.

motor_set_speed(mtr_num, speed)

– функция устанавливающая скорость вращения маховика с номером mtr_num, в значение speed.

Тест программы​

Протестируйте работу программы при следующих значениях целевого угла alpha_goal. 0, 90, -90, 180. При повороте на целевой угол в 180 градусов программа будет работать неправильно. Для устранения бага в работе в программу необходимо добавить функцию трансформации угла angle_transformation. Для определения команды управления для маховика важно знать не угол разворота спутника относительно магнитного поля, а угловую ошибку - разницу между текущим углом, и целевым углом alpha_goal. По умолчанию угол определяется в диапазоне от -180 до 180 градусов с помощью функции atan2. Функция 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

Тест скорректированной программы​

Протестируйте работу программы при следующих значениях целевого угла alpha_goal: 0, 90, -90, 180. Теперь при повороте на целевой угол в 180 градусов программа будет работать правильно.