Управление движением подводного аппарата по заданной траектории

Размещено на http://www.allbest.ru/

Санкт-Петербургский государственный университет

Кафедра компьютерных технологий и систем

Выпускная квалификационная работа

Направление 010400 - Прикладная информатика

Управление движением подводного аппарата по заданной траектории

Большаков Георгий Михайлович

Санкт-Петербург

2018

Введение

В настоящее время для решения различных исследовательских задач в водных просторах, таких как исследование дна, рельефа, различных подводных сооружений, инспекция и обслуживание разного рода технических сооружений, видео и фото съемка, интенсивно используются подводные автономные необитаемые аппараты (АНПА). Поскольку АНПА существуют различных типов, конструкций, разрабатываются во многих организациях самостоятельно, актуальным является изучение различных вопросов, связанных с управлением АНПА. Особое внимание интересно уделить вопросам управления сложными движениями в трёхмерном пространстве, необходимость в которых может возникнуть, например, при обхождении препятствий. Основной целью данной работы являлась разработка законов управления на траектории, задаваемых желаемыми координатами объекта и курсом, с учётом возможного действия на объект кусочно-постоянных возмущений, в том числе на основе не полностью измеренного вектора состояния. Для этого в работе выполнена реализация известного подхода к управлению АНПА на траектории в предположении, что известен весь вектор измерений, предложенного в работе [1] и основанного на технике backstepping. В работах [2], [3] предложены иные варианты управления АНПА на траектории с теми же предположениями, в том числе обсуждается возможность использования расширенного фильтра в контуре обратной связи. В данной работе предложены модификации управления, представленного в работе [1] для подавления действия кусочно-постоянных возмущений, в том числе использован расширенный фильтр Калмана для оценки вектора состояния в условиях, когда не все компоненты вектора состояния доступны измерению.

1. Постановка задачи

1.1 Математическая модель аппарата

Основные принципы построения математических моделей морских подвижных объектов представлены в [4]. В данной работе в качестве математической модели подводного аппарата принимается модель, общая структура которой описана в [1]-[3], там же приведены параметры модели.

Кинематическая модель

Для построения кинематической модели используются две системы отсчёта в трёхмерной прямоугольной системе координат, показанные на рис. 1: инерциальная система отсчёта , и система отсчёта, связанная с телом . Также на рис. 1 показано, как введены основные переменные состояния, характеризующие динамику подводного аппарата.

Рис. 1. Системы координат, используемые в модели

Кинематическая модель аппарата может быть выражена уравнением



где - положение и ориентация в инерциальной системе отсчёта; - вектор скоростей в системе отсчёта, связанной с телом; - матрица перехода между системами отсчёта.

Динамическая модель

Динамическая модель может быть выражена следующим уравнением:

где - инерциальная матрица, обозначает матрицу Кориолиса, - матрица, содержащая гидродинамические коэффициенты демпфирования, обозначает силу тяжести и силу плавучести, - управляющие силы и моменты, которые действуют на центр тяжести аппарата.

В данной работе предполагается, что , то есть рассматриваются только четыре степени свободы. Это обусловлено тем, что они в достаточной степени описывают основные характеристики всей системы, и из-за сложности полной системы. Следовательно, вышеупомянутые векторы и матрицы могут быть упрощены как

В качестве управления будем принимать вектор управляющих сил и моментов , вырабатываемый управляющими органами, которыми оборудован подводный аппарат и удовлетворяющий соотношению

Параметры

Здесь используются следующие параметры конкретного подводного аппарата, приведённые в [1]-[3]:

.

В матрице M в верхних трёх диагональных элементах используются разность массы аппарата и добавочной массы, в четвёртом - момента инерции относительно оси z и добавочной массы, в матрице C - массы аппарата и элементов вектора q, в матрице D - отрицательные линейные параметры сопротивления, g - квадратичные параметры сопротивления.

1.2 Математическая постановка задачи

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

(9)

При построении управления следует учитывать, что не все переменные могут быть доступны измерению, а также, возможно влияние на объект внешних воздействий от ветра и течения, которые математически описываются кусочно-постоянными функциями.

Будем считать, что вектор управлений формируется как

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

В случае, если управление строится на основе оценок вектора состояния, то есть учитывается, что не весь вектор состояния доступен измерению, вектор управления формируется как

,

где - оценки векторов и , сформированные на основе данных об измеряемых переменных :

,

где , а матрица постоянна.

2. Синтез управления

2.1 Управление на основе техники Backstepping

В качестве базового подхода, на котором будем основывать синтез управления, используется подход, который предложен в статье [1], основанный на использовании техники Backstepping. Техника Backstepping предполагает решение задачи синтеза в несколько этапов для последовательности упрощённых моделей, и позволяет учитывать известную нелинейную часть динамики системы.

Указанный подход предполагает синтез управления обеспечивающий выполнение соотношений

где - реальная текущая траектория, - желаемая траектория, - реальный текущий вектор скоростей, - желаемый вектор скоростей (виртуальное управление):

Вектор управлений при этом формируется в следующем виде:

Подставим (16) в (2) и получим уравнение относительно ошибки :

Теперь, считая, что , получим уравнение относительно ошибки

Отсюда видно, что если выбрать K, диагональными матрицами с положительными коэффициентами, соотношения, указанные выше (15), (16), будут выполняться.

В статье [1] показано, что при таком выборе управления и виртуального вектора желаемых скоростей ошибки и будут равномерно ограничены, при этом замкнутая система будет обладать робастными свойствами, то есть при некоторых ограниченных отклонениях в матрицах модели будет сохраняться движение вдоль траектории, но с некоторыми отклонениями от желаемых значений.

Матрицы и позволяют влиять на характеристики переходных процессов, путём увеличения их диагональных элементов можно увеличить быстродействие по сходимости конкретных компонент ошибок.

Теперь будем считать, что на подводный аппарат действуют внешние воздействие, представленные в виде



Вектор вводится в правую часть системы (2) аддитивно. Тогда модель может быть переписана в виде

Указанные внешние силы и момент при движении будут смещать аппарат с заданной траектории, и описанное управление (16) не обеспечивает движение объекта по заданной траектории. Для подавления влияния внешнего возмущения на динамику объекта можно модифицировать структуру управления путём введения в уравнения (15) и (16) интегральных слагаемых для ошибок и .

В обновлённом варианте вектор виртуальных желаемых скоростей и вектор примут вид

В этом случае уравнение относительно ошибки примет вид



Введение интегрального слагаемого будет означать, что в модель добавлены вспомогательные уравнения относительно вектора

Тогда выберем матрицы и диагональными с положительными коэффициентами, и если наличие возмущений (21) приведёт к постоянному смещению в системе с управлением (16), то при управлении (24) в системе в этом случае образуется решение по вектору то есть вектор должен совпасть с .

Аналогично уравнение относительно ошибки примет вид

Здесь также выберем матрицы и диагональными с положительными коэффициентами, в результате чего вектор должен совпасть с .

2.2 Управление с использованием наблюдателя

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

Наблюдателем для системы вида

где - вектор состояния системы - вектор измерений, принято называть устройство, которое описывается системой вида

И позволяет построить оценку вектора состояния , используя измеряемые переменные , так, что

Вопрос синтеза наблюдателей для линейных систем хорошо изучен, в частности, большой популярностью пользуется фильтр Калмана. Для нелинейных систем, когда считается, что следует использовать не линейное приближение, а учитывать нелинейные составляющие, ситуация не так однозначна, в основном наблюдатели строятся для систем специального вида. Часто оказывается возможным использование для нелинейных систем так называемого расширенного фильтра Калмана. Воспользуемся этой идеей для рассматриваемой системы.

Объединяя уравнения (1) и (12) в одну систему и разрешая систему (2) относительно , можно представить математическую модель подводного аппарата, записывая её совместно с системой измерений в виде:

.

Считая также, что на объект действуют некоторые внешние возмущения , а в систему измерений аддитивно входят некоторые функции, задающие погрешности в измерениях датчиков, , систему можно представить в виде:

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

Уравнения расширенного фильтра Калмана выглядят следующим образом:



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

Матрицы отражают возможные погрешности в модели, и в условиях отсутствия информации о таких погрешностях конкретный их вид, который соответствовал бы реальным условиям, задать невозможно. Для конкретного объекта их можно настраивать в процессе экспериментов для тестовых режимов. В силу отмеченных обстоятельств в данной работе эти матрицы выбраны диагональными с постоянными элементами.

Введём наблюдатель указанного вида в контур обратной связи системы, тогда итоговое управление формируется в виде

При наличии кусочно-постоянных возмущений в системе (22) оценка вектора состояния при помощи фильтра Калмана (33)-(35) оказывается неточной, поскольку при его построении данные возмущения не учтены (используемая модель задана не полностью). Для уточнения уравнений наблюдателя введём в него также оценки постоянных возмущений, изменяя структуру фильтра Калмана.

В качестве модели возмущений на каждом интервале постоянства компонент вектора возмущений примем следующую систему дифференциальных уравнений: каждое решение которой представляется постоянным вектором

В этом случае, объединяя модель объекта и модель возмущений, получаем общую систему дифференциальных уравнений

Расширенный фильтр Калмана для (36) будет представляться следующей системой уравнений:

Здесь все матрицы имеют тот же смысл, что и в системе (33)-(35).

3. Численное моделирование

Практическая реализация рассмотренного подхода выполнена в среде MATLAB-Simulink. Для выполнения имитационного моделирования построены Simulink-модели, соответствующие разным режимам движения.

При выполнении численных расчётов используется явный метод Эйлера для решения системы обыкновенных дифференциальных уравнений с постоянным шагом .

Проведение расчётов выполнено для следующей тестовой траектории:

Проиллюстрируем динамику объекта при различных условиях. Вначале будем считать, что все переменные вектора состояния измеряются. В этом случае аппарат будет двигаться под воздействием (backstepping numbers) при обеспечении в соответствии с (15). В качестве конкретных матриц в управлении и в выражении для виртуальной скорости выбраны

Общая схема Simulink-модели, соответствующей указанным условиям, отображена на рисунке 2.

На рисунке 3 показан процесс перехода на заданную траекторию в пространстве Oxyz под воздействием управления (). На рис. 4 показан соответствующий процесс для .

Теперь введём в систему ступенчатые возмущения следующего вида:

Динамические процессы при том же управлении (16) под действием возмущений (21) показаны на рис. 5-6. Видно, что аппарат сместился с траектории.

Для обеспечения более точного следования траектории воспользуемся управлением (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-модели, на основе которых выполнены вычислительные расчёты.

Литература

[1] Hsiu-Ming Wu and Mansour Karkoub. Hierarchical Backstepping Control for Trajectory-Tracking of Autonomous Underwater Vehicles Subject to Uncertainties // 2014 14th International Conference on Control, Automation and Systems (ICCAS 2014)

[2] Amir Valibeygi, M. Hadi Balaghi I., and Krishna Vijayaraghavan. A Comparative Study of Extended Kalman Filter and an Optimal

Nonlinear Observer for State Estimation // 2017 American Control Conference

[3] Bing Sun, Daqi Zhu, Weichong Li. An Integrated Backstepping and Sliding Mode Tracking Control Algorithm for Unmanned Underwater Vehicles // 2012 UKACC International Conference on Control

[4] Thor I. Fossen. Guidance and Control of Ocean Vehicles // 1994

Размещено на Allbest.ru