Алгоритм управления пространственной ориентацией КА
goltsevАлгоритм программного управления пространственной ориентацией КА
Предполагается написать скрипт на языке 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;
Таким образом, прямой разворот произведен корректно и соответствует заданным ограничениям по угловой скорости и угловому ускорению.
Построение графиков
На рисунке представлен график изменения углового ускорения со временем при прямом развороте на заданный угол. На графике видно что в момент начала поворота ускорение представляет собой положительный импульс (для создания положительной угловой скорости). Далее когда угловая скорость достигает заданного значения ускорение равно нулю. В момент торможения график ускорения представляет собой отрицательный импульс равный по модулю тому импульсу, который был при разгоне.
# построение параметров поворота МКА (угловое ускорение, угловая скорость, угол)
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");