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

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


stabilization

Различия

Здесь показаны различия между двумя версиями данной страницы.

Ссылка на это сравнение

stabilization [2019/05/13 16:34]
golikov [Пример полного кода программы стабилизации спутника на языке Python:]
stabilization [2020/03/25 16:28]
Строка 1: Строка 1:
-===== 04 Урок. Стабилизация спутника===== 
  
-Режим стабилизации спутника означает поддержание нулевой угловой скорости. Такой режим необходим,​ например для получения четких снимков или их передачи на наземный пункт приема,​ когда время передачи данных продолжительно и не допустимо отклонение антенны спутника от направления на наземный пункт приема данных. Также описанная в этом уроке теория подходит для режима поддержания любой требуемой угловой скорости,​ а не только нулевой,​ для таких задач как отслеживание подвижного объекта. 
- 
-===== Как реализовать режим стабилизации ===== 
- 
-Изменять угловую скорость спутника можно с помощью маховиков,​ реактивных двигателей,​ электромагнитных катушек,​ двигателей - гиродинов. В этом примере мы рассмотрим управление управляющим моментом с помощью [[ru:​wheel_subsys|маховика]]. Действие этого устройства основано на законе сохранения момента импульса. Например,​ когда двигатель-маховик раскручивается в одну сторону,​ то космический аппарат (КА), соответственно,​ начинает вращаться в другую сторону под действием такого же раскручивающего момента,​ но направленного в противоположную сторону в соответствии с третьим законом Ньютона. Если под влиянием внешних факторов КА начал разворачиваться в определённом направлении,​ достаточно увеличить скорость вращения маховика в ту же сторону и нежелательный поворот КА прекратится,​ вместо спутника вращательный момент "​вберет"​ в себя маховик. Получать информацию об угловой скорости спутника будем с помощью [[ru:​w_subsys|датчика угловой скорости]]. В этом примере мы рассмотрим как по показаниям датчика угловой скорости и данных о частоте вращения маховика рассчитать управляющие команды для маховика,​ чтобы спутник был стабилизирован или поддерживал требуемую угловую скорость. 
- 
-===== Теория ===== 
- 
-==== Аналогии между поступательным и вращательным движением ==== 
-Аналогом закона сохранения импульса для вращательного движения является закон сохранения момента импульса или закон сохранения кинетического момента:​ 
- 
-$\sum\limits_{i=1}^{n}{{{J}_{i}}\cdot {{\omega }_{i}}}=const \label{eq:​1}$ 
- 
-Вообще вращательное движение спутника описывается законами схожими с законами известными для поступательного движения. Так, например,​ для каждого параметра в поступательном движении есть аналогичный параметр для вращательного движения:​ 
- 
-^ Поступательное движение ^ Аналогия ^ Вращательное движение ^ 
-| Сила | $F\leftrightarrow M$ | Момент | 
-| Расстояние | $S\leftrightarrow \alpha$ | Угол | 
-| Скорость | $V\leftrightarrow\omega$ | Угловая скорость | 
-| Ускорение | $a\leftrightarrow\epsilon$ | Угловое ускорение | 
-| Масса | $m\leftrightarrow J$ | Момент инерции | 
- 
-Законы движения также выглядят аналогичным образом 
- 
-^ Название закона ^ Поступательное движение ^ Вращательное движение ^ 
-| второй закон Ньютона | $F=m\cdot a$| $M=J\cdot \epsilon$| 
-| кинетическая энергия | $E=\frac{m\cdot {{V}^{2}}}{2}$| $E=\frac{J\cdot {{\omega}^{2}}}{2}$ | 
-| закон сохранения количества движения| $\sum\limits_{i=1}^{n}{{{m}_{i}}\cdot {{V }_{i}}}=const$ | $\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: ==== 
-<code 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)) 
-</​code>​ 
- 
-==== Пример полного кода программы стабилизации спутника на языке Python: ==== 
- 
-<file python stab.py> 
-# Коэффициент дифференциальной обратной связи. 
-# Коэффициент положительный,​ если маховик расположен осью 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() 
-</​file>​ 
- 
-==== Пример полного кода программы стабилизации спутника на языке С: ==== 
- 
-<file с stab.с> 
-#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();​ 
-} 
-</​file>​ 
-===== Задание для самостоятельной работы ===== 
- 
-  - Измените программу так, чтобы спутник вращался с постоянной скоростью. 
-  - Измените программу так, чтобы спутник работал по следующей циклограмме:​ 
-    * стабилизация в течении 10 секунд 
-    * разворот на 180 градусов за время 30 секунд 
-    * снова стабилизация в течении 10 секунд 
-  - Перепишите программу на языке C и добейтесь ее работоспособности. 
stabilization.txt · Последние изменения: 2020/03/25 16:28 (внешнее изменение)

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