04 Урок. Стабилизация спутника

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

Как реализовать режим стабилизации

Изменять угловую скорость спутника можно с помощью маховиков, реактивных двигателей, электромагнитных катушек, двигателей - гиродинов. В этом примере мы рассмотрим управление управляющим моментом с помощью маховика. Действие этого устройства основано на законе сохранения момента импульса. Например, когда двигатель-маховик раскручивается в одну сторону, то космический аппарат (КА), соответственно, начинает вращаться в другую сторону под действием такого же раскручивающего момента, но направленного в противоположную сторону в соответствии с третьим законом Ньютона. Если под влиянием внешних факторов КА начал разворачиваться в определённом направлении, достаточно увеличить скорость вращения маховика в ту же сторону и нежелательный поворот КА прекратится, вместо спутника вращательный момент «вберет» в себя маховик. Получать информацию об угловой скорости спутника будем с помощью датчика угловой скорости. В этом примере мы рассмотрим как по показаниям датчика угловой скорости и данных о частоте вращения маховика рассчитать управляющие команды для маховика, чтобы спутник был стабилизирован или поддерживал требуемую угловую скорость.

Теория

Аналогии между поступательным и вращательным движением

Аналогом закона сохранения импульса для вращательного движения является закон сохранения момента импульса или закон сохранения кинетического момента:

\sum\limits_{i=1}^{n}{{{J}_{i}}\cdot {{\omega }_{i}}}=const

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

Законы движения

Законы движения также выглядят аналогичным образом

Законы движения

Вывод соотношения для требуемой угловой скорости маховика

Запишем закон сохранения кинетического момента системы спутник+маховик для моментов времени «1» и «2»:

{{J}_{s}}\cdot {{\omega }_{s1}}+{{J}_{m}}\cdot {{\omega }_{m1}}={{J}_{s}}\cdot {{\omega }_{s2}}+{{J}_{m}}\cdot {{\omega }_{m2}}

Абсолютная скорость маховика, т.е. скорость маховика в инерциальной системе координат, например, связанной с Землей, представляет собой сумму угловой скорости спутника и угловой скорости маховика относительно спутника, т.е. относительной угловой скорости маховика:

{{\omega }_{mi}}={{\omega }_{si}}+{{{\omega }'}_{mi}}

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

Выразим угловую скорость маховика, которую необходимую задать

{{J}_{s}}\cdot {{\omega }_{s1}}+{{J}_{m}}\cdot \left( {{\omega }_{s1}}+{{{{\omega }'}}_{m1}} \right)={{J}_{s}}\cdot {{\omega }_{s2}}+{{J}_{m}}\cdot \left( {{\omega }_{s2}}+{{{{\omega }'}}_{m2}} \right)

\left( {{J}_{s}}+{{J}_{m}} \right)\left( {{\omega }_{s1}}-{{\omega }_{s2}} \right)=-{{J}_{m}}({{\omega }_{m1}}-{{\omega }_{m2}})

{{\omega }_{m2}}={{\omega }_{m1}}+\frac{{{J}_{s}}+{{J}_{m}}}{{{J}_{m}}}\left( {{\omega }_{s1}}-{{\omega }_{s2}} \right)

Обозначим отношение $\frac{{{J}_{s}}+{{J}_{m}}}{{{J}_{m}}}$ как $k_d$ .

Для работы алгоритма необязательно знать точное значение $\frac{{{J}_{s}}+{{J}_{m}}}{{{J}_{m}}}$ , т.к. маховик не может мгновенно установить требуемую угловую скорость. Также в процесс управления вмешиваются шумы измерений: угловая скорость спутника измеренная с помощью датчика угловой скорости не является точной, т.к. в измерениях всегда есть постоянная ошибка и шум измерений. Следует учесть, что измерения угловой скорости и выдача команд маховику происходят с некоторым минимальным шагом во времени. Все эти ограничения приводят к тому, что $k_d$ подбирается экспериментальным путем или строятся подробные компьютерные модели, которые учитывают все вышеописанные ограничения. В нашем случае коэффициент $k_d$ будем подбирать экспериментально.

${{\omega }_{m2}}={{\omega }_{m1}}+{{k}_{d}}\left( {{\omega }_{s1}}-{{\omega }_{s2}} \right)$

Угловая скорость $\omega_{s2}$ в момент времени «2» является целевой угловой скоростью, обозначим ее $\omega_{s\_goal}$ . Таким образом, если необходимо что спутник поддерживал угловую скорость $\omega_{s\_goal}$ , то зная текущую угловую скорость спутника и текущую угловую скорость маховика возможно рассчитать желаемую скорость маховика для поддержания режима «вращения с постоянной скоростью»:

${{\omega }_{m2}}={{\omega }_{m1}}+{{k}_{d}}\left( {{\omega }_{s1}}-{{\omega }_{{s\_goal}}} \right)$

Используя режим вращения с постоянной скоростью можно заставить спутник повернуться на любой угол если вращать спутник с постоянной скоростью определенное время. Тогда время, которое необходимо вращать спутник с постоянной скоростью $\omega_{s\_goal}$ , чтобы развернуться на требуемый угол $\alpha$ определяется делением этих величин:

$t=\frac{\alpha}{\omega_{{s\_goal}}}$

Если требуется чтобы спутник был застабилизирован, то $\omega_{s\_goal}=0$ и выражение приобретает более простой вид:

${{\omega }_{m2}}={{\omega }_{m1}}+{{k}_{d}}\cdot {{\omega }_{s1}}$

Реализация на языке Python

Пример использование формулы в примере кода на Python:

# запрос данных угловой скорости спутника (ДУС) и маховика
hyro_state, gx_raw, gy_raw, gz_raw = hyro_request_raw(hyr_num)
mtr_state, mtr_speed = motor_request_speed(mtr_num)
 
# перевод значений угловой скорости в градусы/сек
gx_degs = gx_raw * 0.00875
 
# если ДУС установлен осью z вверх, то угловая скорость
# спутника совпадает с показаниями ДУС по оси z, иначе
# необходимо изменить знак: omega = - gz_degs
omega = gz_degs
 
mtr_new_speed = int(mtr_speed+ kd*(omega-omega_goal))

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

# Коэффициент дифференциальной обратной связи.
# Коэффициент положительный, если маховик расположен осью z вверх
# и ДУС расположен осью z также вверх.
# Коэффициент подбирается экспериментально в зависимости от формы
# и массы вашего спутника.
kd = 200.0
# Временной шаг работы алгоритма, с
time_step = 0.1
# Целевая угловая скорость спутника, град/с.
# Для режима стабилизации равна 0.0.
omega_goal = 0.0
# Номер маховика
mtr_num = 1
# Максимально допустимая скорость маховика, об/мин
mtr_max_speed = 5000
# Номер ДУС (датчика угловой скорости)
hyr_num = 1
 
# Функции для определение новой скорости маховика.
# Новая скорость маховика складывается из
# текущей скорости маховика и приращения скорости.
# Приращение скорости пропорционально ошибке по углу
# и ошибке по угловой скорости.
# mtr_speed - текущая угловая скорость маховика, об/мин
# omega - текущая угловая скорость спутника, град/с
# omega_goal - целевая угловая скорость спутника, град/с
# mtr_new_speed - требуемая угловая скорость маховика, об/мин
def motor_new_speed_PD(mtr_speed, omega, omega_goal):
    mtr_new_speed = int(mtr_speed + kd*(omega-omega_goal))
    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
    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)
    sleep(1)
 
 
# Функция отключает все приборы,
# которые будут использоваться в основной программе.
def switch_off_all():
    print "Finishing..."
    print "Disable angular velocity sensor №", hyr_num
    hyro_turn_off(hyr_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
        omega_goal = 0.0
 
    for i in range(1000):
        print "i = ", i
 
        # Опрос датчика угловой скорости и маховика.
        hyro_state, gx_raw, gy_raw, gz_raw = hyro_request_raw(hyr_num) 
        mtr_state, mtr_speed = motor_request_speed(mtr_num)
 
        # Обработка показаний датчика угловой скорости,
        # вычисление угловой скорости спутника по показаниям ДУС.
        # Если код ошибки ДУС равен 0, т.е. ошибки нет
        if not hyro_state:
            gx_degs = gx_raw * 0.00875
            gy_degs = gy_raw * 0.00875
            gz_degs = gz_raw * 0.00875
            # если ДУС установлен осью z вверх, то угловая скорость
            # спутника совпадает с показаниями ДУС по оси 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,omega,omega_goal)
            motor_set_speed(mtr_num, mtr_new_speed)
 
        sleep(time_step)
 
    switch_off_all()

Реализация на языке C

Пример полного кода программы стабилизации спутника на языке С:

#include "libschsat.h"
const float kd = 200.0;
 
// Временной шаг работы алгоритма, с
const float time_step = 0.2;
 
// Целевая угловая скорость спутника, град/с.
// Для режима стабилизации равна 0.0.
const float omega_goal = 0.0;
 
// Номер маховика
const int mtr_num = 1;
 
// Максимально допустимая скорость маховика, об/мин
const int mtr_max_speed = 5000;
 
// Номер ДУС (датчика угловой скорости)
const  uint16_t hyr_num = 1;
int16_t mtr_new_speed;
int motor_new_speed_PD(int mtr_speed, int omega, float 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);
    Sleep(1);
 }
 
void switch_off_all(void){
/* Функция отключает все приборы,которые будут использоваться в основной программе.*/
    printf("Finishing...");
    int16_t new_speed = mtr_new_speed; //
    printf("\nDisable angular velocity sensor №%d\n", hyr_num);
    hyro_turn_off(hyr_num);
    motor_set_speed(mtr_num, 0, &new_speed);
    Sleep (1);
    motor_turn_off(mtr_num);
    printf("Finish program\n");
}
 
void control(void){
    initialize_all();
    // Инициализируем статус маховика
    int16_t mtr_state;
    // Инициализируем статус ДУС    
    int16_t hyro_state = 0;
    int16_t pRAW_dataX = 0;
    int16_t pRAW_dataY = 0;
    int16_t pRAW_dataZ = 0;
    int16_t *gx_raw = &pRAW_dataX; 
    int16_t *gy_raw = &pRAW_dataY; 
    int16_t *gz_raw = &pRAW_dataZ;
    int16_t speed = 0;
    int16_t *mtr_speed = &speed;
    int16_t omega;
    int i;
    for(i = 0; i < 1000; i++){
        printf("i = %d\n", i);
        // Опрос датчика угловой скорости и маховика.
        hyro_state = hyro_request_raw(hyr_num, gx_raw, gy_raw, gz_raw);     
        mtr_state = motor_request_speed(mtr_num, mtr_speed);
 
        // Обработка показаний датчика угловой скорости,
        // вычисление угловой скорости спутника по показаниям ДУС.
        // Если код ошибки ДУС равен 0, т.е. ошибки нет
        if (!hyro_state){
            float gx_degs = *gx_raw * 0.00875;
            float gy_degs = *gy_raw * 0.00875;
            float gz_degs = *gz_raw * 0.00875;
            // если ДУС установлен осью z вверх, то угловая скорость
            // спутника совпадает с показаниями ДУС по оси z, иначе
            // необходимо изменить знак: omega = - gz_degs
            omega = gz_degs;
            printf("gx_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");
        }
 
        int mtr_new_speed;
        //Обработка показаний маховика и установка требуемой угловой скорости.
        if (!mtr_state) {
            // если код ошибки 0, т.е. ошибки нет
            int16_t mtr_speed=0;
            motor_request_speed(mtr_num, &mtr_speed);
            printf("Motor_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();
}

Задание для самостоятельной работы

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

стабилизация в течении 10 секунд;
разворот на 180 градусов за время 30 секунд;
снова стабилизация в течении 10 секунд.

Перепишите программу на языке C и добейтесь ее работоспособности.

04 Урок. Стабилизация спутника

Как реализовать режим стабилизации​

Теория​

Аналогии между поступательным и вращательным движением​

Вывод соотношения для требуемой угловой скорости маховика​

Реализация на языке Python​

Пример использование формулы в примере кода на Python:​

Пример полного кода программы стабилизации спутника на языке Python:​

Реализация на языке C​

Пример полного кода программы стабилизации спутника на языке С:​

Задание для самостоятельной работы​