Адаптивное позиционно-траекторное управление с наблюдателем возмущения

Рассмотрим уравнения подвижного объекта вида:

                              .                         (2.61)

                                  .                             (2.62)

где  – вектор линейных координат подвижного объекта в неподвижной системе координат;  – вектор углов Эйлера;  – векторы линейных и угловых скоростей подвижного объекта в связанной системе координат;  – матрицы кинематических преобразований. Вектор  является вектором внешних переменных, а вектор  – вектором внутренних координат. Матрица  – матрица инерционных характеристик;  – результирующие векторы сил и моментов, действующих на подвижный объект [21]:

                             .                        (2.63)

                               .                          (2.64)

где  и  – соответственно главный вектор и главный момент сил тяжести;  – главный вектор плавучести (сила Архимеда);  и  – соответственно главный вектор и главный момент силы тяги, создаваемой движителями подвижного объекта;  и  – соответственно главный вектор и главный момент гидродинамических или аэродинамических сил, действующих на корпус и оперение подвижного объекта.

Синтез алгоритмов управления движением осуществляется на основе метода позиционно-траекторного управления подвижными объектами [21, 22].

Для этого сформируем требования к траектории движения в виде:

                             .                        (2.65)

где  – матрицы и вектор постоянных чисел, отражающих требования к траектории движения.

Вычислим производную по времени от выражения (2.65) с учетом уравнений кинематики (2.61)

                  .             (2.66)

Вторая производная по времени от (2.65), с учетом уравнений динамики (2.62), равна

          .     (2.67)

Введем также в рассмотрение вектор требований к скоростям подвижного объекта

                                   .                              (2.68)

Вычислим производную по времени от выражения (2.68) с учетом уравнений динамики (2.62)

           .      (2.69)

Потребуем, чтобы траекторная и скоростная ошибки (2.65) и (2.68) подчинялись уравнениям:

                          .                      (2.70)

Подставляя в систему (2.70) уравнения (2.61), (2.62), (2.66) – (2.69), получим

. (2.71)

Учитывая выражения (2.63), (2.64), из (2.71) получим выражение для управляющих сил и моментов:

   . (2.72)

Выражение (2.72) является базовым позиционно-траекторным законом управления для подвижного объекта (2.61), (2.62).

 

2.9. Проектное задание 8

1. Объект управления для всех вариантов описывается уравнениями (2.61), (2.62). При этом:

,      

                      ,                               

                       ,                               

                                    .                                             

2. В соответствии с вариантом, заданным в табл. 2.7, сформировать требования к траектории и скорости подвижного объекта в виде (2.65), (2.68).

Пусть, например, необходимо, чтобы подвижный объект двигался вдоль прямой линии, описываемой уравнением:

                                      .                                 (2.73)

Высота движения равна

                                        м.                                  (2.74)

Требования к скоростям движения:

                     м/с,  м/с,  м/с.               (2.75)

Требования к углу крена

                                       .                                  (2.76)

Требования к скорости (2.75) означают, что подвижный объект должен двигаться только носом вперед. Движение боковыми или вертикальными поверхностями вперед не разрешается в данном случае.

На основе (2.73), (2.74) и (2.76) сформируем траекторные матрицы .

Движение вдоль траектории (2.73) означает, что угол рысканья подвижного объекта в установившемся режиме должен быть равен arctg(k) градусов:

                            .                       (2.77)

Однако если подвижный объект находится вдали от прямой (2.73), то он должен двигаться перпендикулярно заданной прямой, а по мере приближения к ней уменьшать разницу в углах рысканья. В ведем расстояние , на котором подвижный объект движется перпендикулярно заданной прямой. Тогда уставку по углу рысканья можно записать в следующем виде:

                                             (2.78)

где  – текущее расстояние до заданной прямой.

В общем случае, уравнение прямой, проходящей через заданную точку , и перпендикулярной заданной прямой (2.73) имеет вид [23]:

                           .                      (2.79)

Выражение (2.74) используется формирования задания по углу тангажа:

                                .                           (2.80)

Теперь по выражениям (2.75) – (2.80) составляем матрицы  и .

В первую строку матрицы  помещаем коэффициенты перед углами Эйлера из уравнения (2.77). Во вторую строку матрицы  помещаются коэффициенты перед углами Эйлера из уравнения (2.80). В третью строку матрицы  помещаются коэффициенты перед углами Эйлера из уравнения (2.76). При этом коэффициенты перед углом рысканья помещаются в первый столбец, коэффициенты перед углом тангажа – во втором столбце, а коэффициенты перед углом крена – в третьем столбце. Тогда матрица  имеет вид:

                                  .                              (2.81)

Аналогично, в строки матрицы  помещаются коэффициенты перед переменными  из уравнений (2.77), (2.80) и (2.76):

                                  .                             (2.82)

В вектор  помещаются уставки из уравнений (2.77), (2.80) и (2.76):

                                     .                                 (2.83)

Аналогичным образом, на основе уравнений (2.75) формируются матрицы :

                                  .                              (2.84)

                                      .                                 (2.85)

3. В Matlab необходимо промоделировать полученные алгоритмы управления. Пример программы моделирования представлен ниже.

Запускаемый файл имеет вид.

clc

clear all

close all

 

M=diag([1;1;1;10;100;100]);

x0=[1;1;3;0;0;0;0;0;0;0;0;0];

tk=200;

k=1; T1=2; T2=1; T3=1; y_0=10; ky=0.03; Vx_0=1; Vy_0=0; Vz_0=0; L=10;

 

[t,y]=ode45('position_path_function',[0 tk],x0,[],M,k,T1,T2,T3,y_0,ky,Vx_0,Vy_0,Vz_0,L);

 

figure(1); hold on; grid on; plot(t,y(:,2),'*b');

figure(2); hold on; grid on; plot(y(:,1),y(:,3),'*b');

Интегрируемый ode-файл имеет вид.

function y=position_path_function(t,x,flag,M,k,T1,T2,T3,y_0,ky,Vx_0,Vy_0,Vz_0,L)

x0=x(1);

y0=x(2);

z0=x(3);

psi=x(4);

upsilon=x(5);

gamma=x(6);

Vx=x(7);

Vy=x(8);

Vz=x(9);

wx=x(10);

wy=x(11);

wz=x(12);

 

V=[Vx;Vy;Vz];

omega=[wx;wy;wz];

 

A=[cos(psi)*cos(upsilon), -cos(psi)*sin(upsilon)*cos(gamma)+sin(psi)*sin(gamma), cos(psi)*sin(upsilon)*sin(gamma)+sin(psi)*cos(gamma);

sin(upsilon),      cos(upsilon)*cos(gamma),                           -cos(upsilon)*sin(gamma);

-sin(psi)*cos(upsilon), cos(psi)*sin(gamma)+sin(psi)*sin(upsilon)*cos(gamma), cos(psi)*cos(gamma)-sin(psi)*sin(upsilon)*sin(gamma)];

Aw = [0, cos(gamma)/cos(upsilon), -sin(gamma)/cos(upsilon);

0, sin(gamma)           , cos(gamma)        ;

1, -cos(gamma)*tan(upsilon), sin(gamma)*tan(upsilon) ];

 

%-----------controller------------------------

r0=[x0;y0;z0];

Theta=[psi;upsilon;gamma];

psi_0=atan(k)-pi*abs(k*x0-z0)/2/L;

gamma_0=0;

if(abs(k*x0-z0)>L)

psi_0=atan(k)-pi/2;

end

A1=[0 0 0; 0 ky 0; 0 0 0];

A2=[1 0 0; 0 1 0; 0 0 1];

A3=[-psi_0; -ky*y_0; -gamma_0];

psi_tr=A1*r0+A2*Theta+A3;

A4=[1 0 0; 0 1 0; 0 0 1];

A5=[-Vx_0; -Vy_0; -Vz_0];

psi_v=A4*V+A5;

dpsi_tr=A1*A*V+A2*Aw*omega;

dA=zeros(3,3);

dAw=zeros(3,3);

u=-M*[A4 zeros(3,3); A1*A A2*Aw]^(-1)*[T3*psi_v; A1*dA*V+A2*dAw*omega+T2*psi_tr+T1*dpsi_tr];

 

y=[[A zeros(3,3); zeros(3,3) Aw]*[V; omega]; M^(-1)*u];

Для простоты в ode-файле матрицы  приняты нулевыми.

Результаты моделирования – высота и траектория полета – представлены на рис. 2.12 и 2.13.

Рисунок 2.12 – Высота полета

Рисунок 2.13 – Траектория полета

4. Изменяя коэффициент  в пределах 50 %, определить его влияние на качество переходных процессов.

5. Уменьшая и увеличивая коэффициент L в 3 раза, определить его влияние на качество переходный процессов.

6. Варианты заданий. Во всех вариантах угол крена равен нулю. Траектория задается уравнением . Вертикальная и боковая скорости равны нулю.

Таблица 2.7 – Варианты заданий для системы позиционно-траекторного управления

Вар.	Траектория	Высота	Скорость
1
2
3
4
5
6
7
8
9
10

 

Продолжение таблицы 2.7

11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30