Алгоритм программного управления пространственной ориентацией КА

Предполагается написать скрипт на языке Engee который будет вычислять параметры ориентации МКА при повороте из текущей ориентации в заданную неподвижную ориентацию. Скрипт должен рассчитывать и формировать во времени программу изменения ориентации КА в виде программных кватерниона и скорости с целью разворота из текущей ориентации (в т.ч. переменной) в заданную постоянную в инерционной системе координат (ИСК). Алгоритм запускается внешним диспетчером системы автоматического управления (САУ) и должен формировать признаки начала разгона, конец разгона, начало торможения, конец торможения. Малый космический аппарат был выбран в качестве объекта исследований по следующим причинам:

· Так как его можно рассматривать как симметричный объект (как например cubesat) и его САУ управления угловым положением как правило полностью твердотельная и построена на индуктивных катушках которые позволяют управлять угловым положением МКА относительно магнитного поля земли.

· Индуктивный метод управления позволяет управлять МКА не изменяя его массу (по сравнению например с крупными спутниками которые для управления используют реактивное движение и использованием сжатого азота) что позволяет сильно упростить управления движения и как следствие упросить САУ управления космическим аппаратом.

Входные данные:

· Начальный кватернион ориентации космического аппарата в инерциальной системе координат;

· Начальная угловая скорость – любая в пределах 0,3 град/с;

· Конечный кватернион в инерциальное системе координат – любой;

· Конечная угловая скорость – 0 град/с.

Максимально допустимые значения:

· Максимально допустимое угловое ускорение космического аппарата 0,01 град/с^2;

· Максимально допустимая угловая скорость космического аппарата 0,5 град/с;

· Такт формирования программного кватерниона 0,1 с.

Основные вычисления

# подключение библиотеки построения графиков
using Plots
#компоненты начального кватерниона 
q1st = 1.0;
q2st = 0.0;
q3st = 0.0;
q4st = 0.0;
#компоненты конечного кватерниона
q1end = 0.0;
q2end = 1.0;
q3end = 0.0;
q4end = 0.0;
#Вычисление кватерниона поворота по формуле деления кватернионов 
# представленных в виде отдельных компонентов
# начальный делится на конечный
qp1 = q1st*q1end+q2st*q2end+q3st*q3end+q4st*q4end;
qp2 = q1st*q2end-q2st*q1end-q3st*q4end+q4st*q3end;
qp3 = q1st*q3end-q3st*q1end+q2st*q4end-q4st*q2end;
qp4 = q1st*q4end-q4st*q1end-q2st*q3end+q3st*q2end;
# норма кватерниона
Normp = qp1^2+qp2^2+qp3^2+qp4^2;

# норма кватерниона
Normp = qp1^2+qp2^2+qp3^2+qp4^2;

#пересчет компонент кватернионов
fip1 = 2*acosd(qp1)          
if fip1>180 # если значение угла поворота больше 180 градусов то необходим пересчет
    fip = -(360-fip1)
q1end=-q1end; # смена знака при угле поворота более 180 градусов
q2end=-q2end;
q3end=-q3end;
q4end=-q4end;
elseif fip1<-180 # для поворота на угол менее -180 аналогичная ситуация
    fip = (360+fip1)
q1end=-q1end;  # смена знака при угле поворота более 180 градусов
q2end=-q2end;
q3end=-q3end;
q4end=-q4end;
else
    fip = fip1;
end
# расчет длины ортов
e1 = qp2/sind(fip1/2); 
e2 = qp3/sind(fip1/2);
e3 = qp4/sind(fip1/2);

#Вычисление моментов времени
if fip>0
    eps_max = 0.01; # макисмальная погрешность расчета
    w_max = 0.5;  # максимальная угловая скорость
    h = 0.1; # длина шага по времени
    t1 = w_max/eps_max;# время конца ускорения 
    delta_fi = (eps_max*(t1)^2)/2;# погрешность угла поворота
    t2 = t1+((fip-2*delta_fi)/w_max); # время начала торможения 
    t3 = t1+t2;# время окончания торможения
elseif fip<0
    eps_max = -0.005; # макисмальная погрешность расчета
    w_max = -0.2;  # максимальная угловая скорость
    h = 0.1;  # длина шага по времени
    t1 = w_max/eps_max; # время конца ускорения 
    delta_fi = (eps_max*(t1)^2)/2; # погрешность угла поворота
    t2 = t1+((fip-2*delta_fi)/w_max);  # время начала торможения 
    t3 = t1+t2; # время окончания торможения
end

# максимальное значение счетчика
N=floor(Int,round(t3/h)+1);
# инициализация выходных массивов нулями
t = zeros(N); # массив времени
Eps = zeros(N,1); # массив содержащий изменение погрешности
w = zeros(N,1); # массив содержащий угловую скорость
q = zeros(4,N);  # массив содержаний компоненты кватерниона
fi = zeros(N,1);  # массив содержаний углы ориентации космического аппарата
q_prog1 = zeros(4,N);  # компоненты программного кватерниона
for i = 2 : N
    #Увеличение времени на шаг
    t[i] = t[i-1] + h;
    #изменение углового ускорения
    if t[i]<=t1  ## диапазон времени 1
        Eps[i]=eps_max;
    elseif t[i]<=t2  # диапазон времени 2
        Eps[i]=0;
    elseif t[i]<=t3 # диапазон времени 3
        Eps[i]=-eps_max;
    else
        Eps[i]=0;
    end
    #Новые значения угловой скорости, угла и кватерниона
    w[i] = w[i-1] + h*Eps[i]; # накопление массива содержащего угловую скорость
    fi[i] = fi[i-1] + h*w[i]; # накопление массива содержащего угол поворота МКА
    q[1,i] = cosd(fi[i]/2);  # накопление массива содержащего первую компоненту кватерниона
    q[2,i] = e1*sind(fi[i]/2); # накопление массива содержащего вторую компоненту кватерниона
    q[3,i] = e2*sind(fi[i]/2); # накопление массива содержащего третью компоненту кватерниона
    q[4,i] = e3*sind(fi[i]/2); # накопление массива содержащего четвертую компоненту кватерниона
    
    #Вычисление программного кватерниона по формуле умножения кватернионов (начальный умножается на пересчитанный каждый такт)
    #        в бортовой программе нет никаких массивов, там только i-й элемент
    
    q_prog1[1,i] = q1st*q[1,i] - q2st*q[2,i] - q3st*q[3,i] - q4st*q[4,i]; # первая компонента программного кватерниона
    q_prog1[2,i] = q1st*q[2,i] + q2st*q[1,i] + q3st*q[4,i] - q4st*q[3,i]; # вторая компонента программного кватерниона
    q_prog1[3,i] = q1st*q[3,i] - q2st*q[4,i] + q3st*q[1,i] + q4st*q[2,i]; # третья компонента программного кватерниона
    q_prog1[4,i] = q1st*q[4,i] + q2st*q[3,i] - q3st*q[2,i] + q4st*q[1,i]; # четвертая компонента программного кватернионаи
end

На последнем такте значение программного кватерниона программного приравнивается к значению конечного кватерниона и тогда можно определить погрешность вычисления вычитая по модулю фактическое значение кватерниона и требуемое:

# расчет погрешности программного кватерниона в конечной точке поворота МКА
delta_prog1 = abs(q1end-q_prog1[1,N]);
delta_prog2 = abs(q2end-q_prog1[2,N]);
delta_prog3 = abs(q3end-q_prog1[3,N]);
delta_prog4 = abs(q4end-q_prog1[4,N]);
# вывод на экран значений погрешности программного кватерниона
print("delta_prog1 = ",delta_prog1,"\n");
print("delta_prog2 = ",delta_prog2,"\n");
print("delta_prog3 = ",delta_prog3,"\n");
print("delta_prog4 = ",delta_prog4,"\n");
#приравнивание к конечному кватерниону
q_prog1[1,N] = q1end;
q_prog1[2,N] = q2end;
q_prog1[3,N] = q3end;
q_prog1[4,N] = q4end;

delta_prog1 = 0.0035787899918092304
delta_prog2 = 6.403889407646801e-6
delta_prog3 = 0.0
delta_prog4 = 0.0

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

Построение графиков

На рисунке представлен график изменения углового ускорения со временем при прямом развороте на заданный угол. На графике видно что в момент начала поворота ускорение представляет собой положительный импульс (для создания положительной угловой скорости). Далее когда угловая скорость достигает заданного значения ускорение равно нулю. В момент торможения график ускорения представляет собой отрицательный импульс равный по модулю тому импульсу, который был при разгоне.

# построение параметров поворота МКА (угловое ускорение, угловая скорость, угол)
p1 = plot(t,Eps, title="Изменение углового ускорения", xlabel="Время, с", ylabel = "Угловое ускорение, град/с^2", linewidth=3);
# вывод графиков
display(p1)

# построение параметров поворота МКА (угловое ускорение, угловая скорость, угол)
p2 = plot(t,w, title="Изменение угловой скорости", xlabel="Время, с", ylabel = "Угловая скорость, град/с", linewidth=3);
# вывод графиков
display(p2)

На рисунке представлен график изменения угла со временем при прямом развороте на заданный угол.

С момента времени 0-50 секунд происходит угловое ускорение

В момент 50-360 секунд угловое ускорение равно нулю и угол поворота увеличивается линейно, угловая скорость имеет постоянное значение.

В момент 360-410 секунд угловое ускорение имеет отрицательное значение, угловая скорость линейно уменьшается до нуля, угол стремится к заданному значению (с учетом погрешности)

# построение параметров поворота МКА (угловое ускорение, угловая скорость, угол)
p3 = plot(t,fi, title="Изменение угла", xlabel="Время, с", ylabel = "Угол, град", linewidth=3);
# вывод графиков
display(p3)

# графики компонент программного кватерниона
p4 = plot(t,[q_prog1[1,:]], title="Первая компонента программного кватерниона", xlabel="Время, с", ylabel = "Компонента 1", linewidth=3);
# вывод графиков
display(p4)

# графики компонент программного кватерниона
p5 = plot(t,[q_prog1[2,:]], title="Вторая компонента программного кватерниона", xlabel="Время, с", ylabel = "Компонента 2", linewidth=3);
# вывод графиков
display(p5)

Так как МКА поворачивается в одной плоскости то 3 и 4 компоненты кватерниона будут равны нулю, что и видно по графику.

# графики компонент программного кватерниона
p6 = plot(t,[q_prog1[3,:],q_prog1[4,:]], title="3,4 компоненты программного кватерниона", xlabel="Время, с", ylabel = "Компоненты 3-4", linewidth=3);
# вывод графиков
display(p6)

Выведем времена ускорения и торможения.

## время конца участка углового ускорения
print("t1 = ",t1,"\n");
## время начала участка углового торможения
print("t2 = ",t2,"\n");
## время конца участка углового торможения
print("t3 = ",t3,"\n");

t1 = 50.0
t2 = 360.0
t3 = 410.0

Алгоритм управления пространственной ориентацией КА

Алгоритм программного управления пространственной ориентацией КА

Входные данные:

Максимально допустимые значения:

Основные вычисления

Построение графиков

Выведем времена ускорения и торможения.

Тип

Краткое описание

Связанные материалы

Теги

Категории

Языки

Форматы

Уровень

Статус

Опубликовано

Обновлено

Источник