Для обеспечения более точного следования траектории воспользуемся управлением (24), при условии, что . Соответствующие переходные процессы показаны на рис. 7-8. Как и ожидалось, введение интегральных слагаемых в управление и виртуальную скорость способствует тому, что аппарат более точно следует заданной траектории и действие возмущений подавляется.
Теперь учтём, что не все переменные вектора состояния измеряются. Будем считать, что вектор доступен измерению. Используем его для построения оценок всего вектора состояния с помощью расширенного фильтра Калмана (33)-(35). Выберем в качестве матриц Q и R следующие:
.
Simulink-модель системы, где используется расширенный фильтр Калмана, приведена на рис. 9. При этом считаем, что на систему не действуют никакие внешние возмущения.
В подсистеме Kalman filter задаётся расширенный фильтр Калмана в соответствии с (33)-(35). На рис. 10-11 приведены графики переходных процессов при управлении (36) с использованием в контуре обратной связи расширенного фильтра Калмана. Видно, что в системе с использованием расширенного фильтра Калмана в контуре обратной связи также обеспечивается движение вдоль траектории с достаточно хорошими характеристиками динамических процессов.
Теперь введём в систему ступенчатые возмущения вида (21).
Поскольку возмущения не измеряются и, соответственно, не учитываются в фильтре Калмана, фактическая траектория будет смещена относительно желаемой. Для того, чтобы обеспечить более точное следование траектории и подавить действие возмущений, модифицируем фильтр Калмана путём введения в него оценок вектора возмущения.
Уравнения, которые описывают модифицированный фильтр Калмана, описываются формулами (41)-(45).
Общая структура Simulink-модели осталась прежней, изменилась только внутренняя часть подсистемы Kalman filter, а именно изменились размеры входных векторов и внутренняя часть M-функции, описывающей правую часть системы, задающей расширенный фильтр Калмана. Внутреняя часть подсистемы Kalman filter приведена на рис. 12, программный код указанной m-функции приведён в листинге 1. Также здесь при моделировании добавлен аддитивный шум.
На рис. 13-16 приведены графики процессов , а на рис. 17 - график оценки возмущений . Видно, что оценка вектора возмущений сходится к тестовым возмущениям , а переменные - к соответствующим компонентам желаемой траектории .
Листинг 1
имитационный моделирование подводный аппарат
function d_K = kalman_F(u,Q,R)
P = u(1:144); XX = u(145:152); tau_d = u(153:156); x = u(157:164); tau = u(165:168);
P = reshape(P,12,12);
C_y = [1 0 0 0 0 0 0 0; 0 1 0 0 0 0 0 0; 0 0 1 0 0 0 0 0; 0 0 0 1 0 0 0 0];
y = C_y*x;
y = y + rand(length(y), 1) * 1 - rand(length(y), 1) * 1;
H = [C_y zeros(4)];
x_K = XX(1); y_K = XX(2); z_K = XX(3); psi_K = XX(4); u_K = XX(5); v_K = XX(6); w_K = XX(7); r_K = XX(8);
J = [cos(psi_K) -sin(psi_K) 0 0;
sin(psi_K) cos(psi_K) 0 0;
0 0 1 0;
0 0 0 1];
q = [u_K;v_K;w_K;r_K];
f1 = J*q;
m = 10; Iz = 30; Xud = 34; Yvd = 75; Zwd = 33; Nrd = 62;
Xu = 6; Yv = 10; Zw = 7; Nr = 14; Xuu = 18; Yvv = 4;
Zww = 4; Nrr = 14;
c12 = -m*r_K; c21 = -m*r_K; c41 = m*v_K; c44 = -m*u_K;
C = [0 c12 0 0; c21 0 0 0; 0 0 0 0; c41 0 0 c44];
g = [-Xuu*u_K*abs(u_K) -Yvv*v_K*abs(v_K) -Zww*w_K*abs(w_K) -Nrr*r_K*abs(r_K)].';
D = diag([-Xu -Yv -Zw -Nr]);
m11 = m - Xud; m22 = m - Yvd; m33 = m - Zwd; m44 = Iz - Nrd;
M = diag([m11 m22 m33 m44]);
f2 = inv(M)*[tau - C*q - D*q - g]; %+tau
f = [f1;f2+inv(M)*tau_d;0;0;0;0];
df1 = [0, 0, 0, - v_K*cos(psi_K) - u_K*sin(psi_K), cos(psi_K), -sin(psi_K), 0, 0;
0, 0, 0, u_K*cos(psi_K) - v_K*sin(psi_K), sin(psi_K), cos(psi_K), 0, 0;
0, 0, 0, 0, 0, 0, 1, 0;
0, 0, 0, 0, 0, 0, 0, 1];
df15 = -(Xu + Xuu*abs(u_K) + Xuu*u_K*sign(u_K))/(Xud - m);
df25 = -(m*r_K)/(Yvd - m);
df26 = -(Yvv*abs(v_K) + Yvv*v_K*sign(v_K) + 10)/(Yvd - m);
df45 = (m*r_K - m*v_K)/(Iz - Nrd);
df16 = -(m*r_K)/(Xud - m);
df37 = -(Zww*abs(w_K) + Zww*w_K*sign(w_K) + 7)/(Zwd - m);
df46 = -(m*u_K)/(Iz - Nrd);
df28 = -(m*u_K)/(Yvd - m);
df18 = -(m*v_K)/(Xud - m);
df48 = (m*u_K + Nrr*abs(r_K) + Nrr*r_K*sign(r_K) + 14)/(Iz - Nrd);
df2 = [zeros(4,4) [df15 df16 0 df18; df25 df26 0 df28; 0 0 df37 0; df45 df46 0 df48]];
F = [df1 zeros(4);df2 inv(M);zeros(4, 12)];
K = P*H.'*inv(R);
dP = F*P + P*F.' - K*H*P + Q;
dXX_K = f + K*(y - C_y*XX);
dP = dP(:);
d_K = [dP; dXX_K];
Заключение
В процессе выполнения научно-исследовательской практики была достигнута поставленная цель - реализован известный подход в соответствии со статьёй [1], на его основе реализован модифицированный вариант управления путём введения в систему управления расширенного фильтра Калмана для оценивания переменных состояния, а также обеспечена возможность подавления кусочно-постоянных возмущений, выполнен анализ результатов. Анализ вычислительных результатов показывает, что представленные варианты управлений для разных условий функционирования обеспечивают движение вдоль траектории с учётом поставленных требований - в случае, когда вектор состояния измеряется не полностью, а также на объект действуют кусочно-постоянные возмущения.
Таким образом, в работе получены следующие результаты:
1) Реализован известный подход для синтеза управления на траектории, основанный на использовании техники backstepping, для конкретной модели подводного объекта;
2) Предложены способы для модификации указанного подхода для подавления кусочно-постоянных возмущений, в том числе с использованием расширенного фильтра Калмана в качестве наблюдателя;
3) Проверка работоспособности предложенных законов управления выполнена в среде MATLAB-Simulink, для этого сформированы необходимые Simulink-модели, на основе которых выполнены вычислительные расчёты.