Математическое моделирование объектов и систем управления. Учебно-методическое пособие

Для написания программного кода в среде Matlab необходимо в главном меню выбрать вкладку File → New → Script.

Алгоритм представлен следующими шагами с комментариями. 1 Начало.

2 Ввод исходных данных: коэффициента перевода градусов в радианы deg 2rad =180π ° , времени работы алгоритма t = 1000 с (количество точек), начального вектора состояния системы X = (1° deg2rad

0° 1° deg2rad 25 м/с 0 м/с)T, вектора измерения WSNS = (25 м/c 0 м/с)Т,

СКО шума измерителя угловой скорости SKOw = 5 · 10–5 рад/с, СКО шума измерителя ускорения SKOa = 2 · 10–2 м/с2, СКО шума СНС

SKOs = 0,5 м/с.

Программный код, реализующий данный шаг алгоритма:

% Исходные данные global deg2rad deg2rad = pi/180; t=1000;

X=[1*deg2rad,0,1*deg2rad,25,0]’;

Wsns=[25,0]’; SKOw=5*10^(-5);SKOa=2*10^(-2);SKOs=0.5;

Все матрицы-векторы объявляются в квадратных скобках, через запятую, если это строка, через точку с запятой, для случая, когда это столбец. Транспонирование матрицы, например A, задается выражением A’.

3 Генерирование матриц белого шума системы dw и da, которые преобразуются в вектор dX = (dw, dw, –dw, da, da), и измерений dZ с нормальным законом распределения. Для этого применяется встроенная в язык программирования Matlab функция:

normrnd(<математическое ожидание>,<СКО>,<n>,<m>).

Математическое ожидание принимаем равным 0, СКО берем из исходных данных, размерность массива шума n×m. Пусть n = t, m = 1 – для dX, n = t, m = 2 – для dZ, тогда на языке Matlab это будет выглядеть следующим образом:

% Генерирование шума системы и измерений da=normrnd(0,SKOa,t,1);dw=normrnd(0,SKOw,t,1); dX =[dw,dw,-dw,da,da];

dZ =normrnd(0,SKOs,t,2);

21

4 Вычисление ковариационных матриц шума системы Q и измерений СНС R:

% Вычисление ковариационной матрицы шума системы

Q=cov(dX);

R=cov(dZ);

5 Задание начальных условий для фильтра Калмана: ковариационных матриц ошибки оценивания координат состояния Pk и ошибки предсказания Pk_, предсказанного значения оценки координат состояния Xk_, их оценки Xk, матрицы наблюдения H:

% Начальные условия для фильтра Калмана

Pk=zeros(5,5);Pk_=zeros(5,5); Xk_=X; Xk=X; H=[0,0,0,1,0;0,0,0,0,1];

При этом полагается, что на этапе выставки БПЛА начальные условия определены точно, и поэтому Pk и Pk_ нулевые, а Xk и Xk_ приравниваются к начальному вектору состояния X. Матрица, состоящая из нулей задается командой zeros(n, m) размерностью n×m.

6 Задание памяти под массивы для вывода на график координат состояния Xi, их оценок X_i, измерений СНС Zki, времени timei:

% Задание памяти под массивы для вывода на график

Xi = zeros(t,5); X_i = zeros(t,5); Zki=zeros(t,2); timei =zeros(t,1);

7 Цикл считывания времени при изменении шага i = 0…t.

8 Генерирование случайных значений измерителей угловой скорости w и ускорения a:

% Показания измерителей ускорения и угловой скорости ax=normrnd(0,SKOa);ay=ax;az=ax; wx=normrnd(0,SKOw);wy=wx;wz=wx;

9 Обновление матрицы состояния F в соответствии с выражением

(27):

% Матрица состояния системы

F=[1,wy,0,0,0;0,1,-wz,0,0;0,wz,1,0,0;-ay,0, -az,1,0;0,ay,ax,0,1];

22

10 Генерирование случайных значений СНС:

% Генерирование показаний СНС

Zk=Wsns+normrnd(0,SKOs,2,1);

11 Обновление вектора динамики системы X согласно первому уравнению (28) и матрицы шума w, где управление uk = 0:

% Динамическая модель движения БПЛА w =[wz,wx,-wy,ax,az]’;

X=F*X+w;

12 Реализация фильтра Калмана по формулам (36), (37), (39):

%Фильтр Калмана

Xk_=F*Xk; Pk_=F*Pk*F’+Q;

Kk=Pk_*H’*(H*Pk_*H’+R)^(-1); Xk=Xk_+Kk*(Zk-H*Xk_); Pk=(eye(5)-Kk*H)*Pk_;

Единичная матрица объявляется в Matlab командой eye(n), размерность полученного массива n×n. Обратная матрица А задается опера-

тором А^(-1).

13 Заполнение массивов вычисленными значениями координат состояния X, их оценками X_ и измерениями СНС Zki для вывода на график:

% Заполнение массивов для вывода на график timei(i) = i;

Xi(i,1:3) = (1/deg2rad).*X(1:3);X_i(i,1:3) = (1/ deg2rad).*Xk(1:3);

Xi(i,4:5) = X(4:5);X_i(i,4:5) = Xk(4:5); Zki(i,:)=Zk;

При этом обращение к массивам данных следующего вида: -- A(i) – обращение к i-му элементу массива;

-- A(i, k:l) – обращение к i-й строке и столбцам с номера k по l; -- A(i, :) – обращение к i-й строке и всем столбцам.

14 Вывод на графики.

Для данного этапа алгоритма применимы следующие команды: -- figure(N) – создает новое графическое окно N-е по счету;

-- plot (x, y) – строит график y(x);

-- plot(x1, y1, x2, y2, x3, y3, …) – строит много графиков y1(x1), y2(x2) и y3(x3) и т.д. в одном графическом окне;

23

-- plot(x1, y1,<’цвет графика y1(x1)’>, x2, y2, <’цвет графика y2(x2)’>, x3, y3, <’цвет графика y3(x3)’>, …) – команда аналогична предыдущей, но указывается дополнительно цвет линий, который пишется в одинарных кавычках:

‘y’ – желтый; ‘m’ – фиолетовый; ‘c’– голубой;

‘r’– красный; ‘g’ – зеленый; ‘b’ – синий; ‘w’– белый; ‘k’ – черный.

-- grid – показывает сетку;

-- title(<’Название графика’>) – заголовок;

-- legend(‘<текст1>‘, ‘<текст2>‘, ‘<текст3>‘, n) – добавляет к текущему графику пояснение в виде указанных текстовых строк.

Если n = –1, то пояснение размещается вне области графика. Если n = 0, то пояснение размещается в области графика, если места для этого достаточно.

Программный код для данного шаг алгоритма, представлен ниже:

% Вывод на графики figure(1)

plot(timei,Xi(:,1),’b’,timei,X_i(:,1),’r’),grid; title(‘Тангаж’)

legend(‘Вычисления из модели движения БПЛА’,’Оценка

фильтром Калмана’,0) figure(2)

plot(timei,Xi(:,2),’b’,timei,X_i(:,2),’r’),grid title(‘Крен’)

legend(‘Вычисления из модели движения БПЛА’,’Оценка

фильтром Калмана’,0) figure(3)

plot(timei,Xi(:,3),’b’,timei,X_i(:,3),’r’),grid title(‘Курс’)

legend(‘Вычисления из модели движения БПЛА’,’Оценка

фильтром Калмана’,0) figure(4)

plot(timei,Zki(:,1),’g’,timei,Xi(:,4),’b’,timei,X_i(

:,4),’r’),grid

title(‘Северная составляющая скорости’) legend(‘Измерения СНС’,’Вычисления из модели

24

движения БПЛА’,’Оценка фильтром Калмана’,0) figure(5) plot(timei,Zki(:,2),’g’,timei,Xi(:,5),’b’,timei,X_i( :,5),’r’),grid

title(‘Восточная составляющая скорости’) legend(‘Измерения СНС’,’Вычисления из модели

движения БПЛА’,’Оценка фильтром Калмана’,0)

15 Конец.

Программный код на языке Matlab, реализующий алгоритм движения БПЛА, приведен в приложении В.

Результаты моделирования, отражающие динамику тангажа, крена, курса, северной и восточной составляющих скоростей движения БПЛА, показаны на рисунках 1.4 – 1.8 соответственно. При этом углы ориентации, вычисленные из модели динамики БПЛА, отклоняются от заданных значений на большую величину, в то время как оценки фильтром Калмана указанных координат состояния намного точнее и практически не уходят от истинных показаний.

То же самое можно сказать и о скоростях движения. Их оценки фильтром Калмана намного меньше, чем максимальное значение шума измерений СНС.

25

26

Рисунок 1.8 – Динамика восточной составляющей скорости БПЛА

27

2. Требования к курсовой работе

2.1 Примеры заданий на курсовую работу

Примерные варианты заданий на курсовую работу приведены в таблице 2.1.

Тема курсовой работы: «Разработка модели фильтра Калмана для системы управления беспилотным летательным аппаратом».

Задание: произвести синтез модели фильтра Калмана для САУ БПЛА с заданными параметрами движения:

ϑ – тангаж, град; γ – крен, град; ψ – курс, град;

WN_СНС – северная составляющая скорости, измеренная СНС, м/с; WE_СНС – восточная составляющая скорости, измеренная СНС, м/с; σω – СКО шумов угловой скорости, рад/с;

σa – СКО шумов ускорений, м/с2;

σснс – СКО шумов СНС, м/с.

Значения γ и WE_СНС принять равными 0.

Таблица 2.1 – Примерные варианты заданий на курсовую работу

28

2.2 Требования к выполнению курсовой работы

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

1 Структурная схема модели САУ БПЛА.

2 Блок-схема алгоритма программы, реализующей модель движения БПЛА и фильтра Калмана.

3 Графики изменения параметров движения БПЛА, полученных из динамической модели и оцененные фильтром Калмана.

4 Программный код в Matlab, описывающий динамику координат состояния БПЛА и фильтр Калмана.

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

Общими требованиями к оформлению курсовой работы являются:

1)обоснованность рекомендаций и предложений;

2)убежденность аргументации;

3)краткость и точность формулировок;

4)корректность изложения результатов работы;

5)четкостьилогическаяпоследовательностьизложенногоматериала. Курсовая работа сдается в печатном виде.

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

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

2.3 Требования к оформлению курсовой работы

Курсовую оформляют в соответствии с ГОСТ 7.32–2001 [3] на страницах формата А4, текст печатается на компьютере, гарнитурой Times New Roman, шрифт не менее 12 пт., через 1,5 интервала, с выравниванием по ширине. Титульный лист оформляется в соответствие с приложением Г.

Страницы нумеруются арабскими цифрами. В общую нумерацию входят все листы. Число иллюстраций должно быть достаточным для пояснения излагаемого текста. Нумерация страниц ставится сквозная, рисунки в пределах раздела. Например, рис. 1.1, 1.2 соответствует первой части работы, рис. 2.1, 2.2 – второй и т.д. Рисунок помещается после первого упоминания в тексте.

Таблицы в тексте могут иметь заголовки, их нумеруют в пределах раздела. На все таблицы и рисунки, помещенные в тексте курсовой работы, необходимо указывать ссылки.

29

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

Примерные контрольные вопросы к защите курсовой работы

1 Перечислите основные параметры движения БПЛА.

2 Каковы основные задачи управления БПЛА?

3 Какие системы координат могут быть использованы при составлении модели движения БПЛА?

4 Какие допущения вводятся при составлении модели движения БПЛА?

5 Что такое коссометрическая матрица угловых скоростей?

6 Дайте пояснения теореме Пуассона.

7 Назовите координаты состояния в модели движения БПЛА.

8 Как формируется матрица шумов системы?

9 От каких параметров движения БПЛА зависит матрица состояния?

10 Как осуществляется переход от непрерывной модели движения БПЛА к дискретной?

11 Объясните назначение фильтра Калмана.

12 Что такое «белый» шум системы?

13 Что из себя представляет нормальный (гауссовский) закон распределения случайной величины?

14 Дайте определения понятиям корреляция, ковариация.

15 В чем отличие СКО от дисперсии?

16 Что такое математическое ожидание случайной величины?

17 В чем отличие ковариационных матриц шума системы и шума измерений?

18 Что такое ковариационная матрица ошибок оценивания координат состояния системы?

19 Перечислите этапы калмановской фильтрации.

20 Как вычислить коэффициент усиления фильтра Калмана?

30