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

x''cos ϑsin ψ + y ''(−cos γsin ϑsin ψ +sin γcos ψ) + z ''(sin γsin ϑsin ψ +cos γcos ψ)

Вынося в выражении (1.7) отдельный вектор (x’’y’’z’’)Т, определим конечную матрицу перехода из связанной СК в навигационную:

Оператор Cbn называется матрицей направляющих косинусов (МНК), которая описывает параметры движения БПЛА в навигационной СК (обозначена как «n» – navigation – навигация) относительно связанной («b» – body – тело) с БПЛА СК. Зная углы ориентации БПЛА, можно с помощью нее получить проекции координат состояния БПЛА на оси NUE.

В целях упрощения моделирования будем рассматривать малые величины ψ, γ, ϑ в пределах –5о…5о или –0,0872…0,0872 рад. Учитывая, что при малых углах sinα ≈ α, cosα ≈ 1, перепишем МНК в более упрощенной форме:

Произведениями γψ, γϑ, ϑψ, γϑψ пренебрегаем в силу их малости. Тогда матрица Cbn будет представлена только углами поворота:

11

Обозначим вектор абсолютных ускорений БПЛА вдоль осей x, y, z через [ax ay az]T, а вектор угловой скорости вращения вокруг них как [ωx ωy ωz]T. Проекции ускорений на оси навигационной СК вычисляются по формуле

Динамика линейных скоростей в проекциях на оси NUE описывается уравнением

Подставляя правую часть (1.12) в выражение (1.13), получаем:

Для преобразования угловых скоростей вращения БПЛА вокруг осей x, y, z в проекции на оси навигационной СК, используется коссометрическая матрица вида [1]:

Динамика углового положения описывается как произведение (1.11) и (1.15) и называется уравнением Пуассона [1]:

12

Запишем выражения для динамики каждого элемента матрицы Cbn, учитывая (1.16):

Из системы (1.17) оставим только уравнения с положительными производными элементов МНК:

ϑ= ωz +ωy γ;

Из системы (1.14) получим динамическую модель линейных скоростей WN, WU, WE:

В уравнениях (1.18) и (1.19) также необходимо учитывать случайную составляющую шума параметров ax, ay, az и ωx, ωy, ωz. Сюда входят погрешности датчиков угловой скорости и ускорения, а также ветровое возмущение, турбулентные потоки воздуха и т.д. Будем обозначать ее как δax, δay, δaz иδωx, δωy, δωz соответственно для каждого из измерений.

13

Тогда выражения (1.18) и (1.19) преобразуются к виду:

=(axϑ+ ay −az γ) +(δayϑ+δaxϑ+δaz γ); WE = (ax +δax )ψ +(ay +δay )γ −(az +δaz ) =

=(axψ + ay γ + az ) +(δay γ +δaxψ +δaz ).

Вцелях упрощения моделирования исключим из рассмотрения

проекцию скорости на вертикальную ось WU = 0. Проведем исследования для случая, когда БПЛА движется с постоянными скоростями

WN, WE, а следовательно, ускорения равны нулю, углы ориентации ψ, γ, ϑ не изменяются. Колебания значений WN, WE, ψ, γ, ϑ будут обусловлены лишь шумовыми составляющими δωx, δωy, δωz и δax, δay, δaz. В качестве вектора наблюдения возьмем y = (WN WE).

Выполним замену переменных:

Будем рассматривать систему (1.20) в виде динамической модели:

y1 = x4 +v1; y2 = x5 +v2 ,

14

где v1, v2 – шум измерений скорости СНС;

w 1, w2, w3, w4, w5 – шум каждой из координат состояния x1, x2, x3, x4, x5 соответственно, определяемый из (1.20):

w4 = δax −δayϑ−δazψ; w5 = δay γ +δaxψ +δaz .

В общем виде динамическая модель движения БПЛА, исходя из (1.21) описывается уравнением:

При этом:

x =(x1 x2 x3 x4 x5 )T , u =(u1 u2 u3 u4 u5 )T , w =(w1 w2 w3 w4 w5 )T ,v =(v1 v2 )T ,

где B – матрица управления; С – матрица наблюдения; y – вектор выхода модели.

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

15

x(k +1) = x(k)(E + Ah) + Bh u(k) + h w(k),

z(k) = Hx(k) +v(k),

где x(k + 1), x(k) – координаты состояния системы, измеренные на (k + 1)-м и k-м шагах;

u (k), w(k), v(k) – вектор управления, шумов системы и измерений соответственно на k-м шаге;

E – единичная матрица размером (5×5).

Во избежание путаницы была выполнена замена вектора выхода y на z(k) и C на H в дискретной модели.

Обозначим матрицы в (26) как F = (E + Ah); N = Bh. Если принять h = 1, то матрица управления N останется без изменения, а матрица состояния F примет вид

Тогда дискретная динамическая модель движения БПЛА:

1.3 Модель оптимального фильтра Калмана для САУ БПЛА

Фильтр Калмана находит широкое применение в области автоматизации и управления навигационными системами. Он позволяет по серии неточных измерений z(k) выполнять фильтрацию зашумленных сигналов, оценивать ненаблюдаемые координаты и прогнозировать будущее состояние системы.

16

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

Корреляция – взаимосвязь двух или более случайных величин, которые с некоторой допустимой степенью точности можно считать таковыми. При этом, если нет никакой связи между параметрами измеренными в момент времени t и t + t, t + t и t + 2Δt и т.д., то полагают, они некоррелированы по времени.

Математическое ожидание (μ) – среднее значение случайной величины.

Пусть имеется выборка из n измерений угловой скорости и ускоре-

ния [ωx1, ωx2, …, ωxn], [ωy1, ωy2, …, ωyn], [ωz1, ωz, …, ωzn],[ax1, ax2, …, axn], [ay1, ay2, …, ayn], [az1, az2, …, azn]. У каждого из них есть шумовая составляющая [δωx1, δωx2, …, δωxn], [δωy1, δωy2, …, δωyn], [δωz1, δωz, …, δωzn],[δax1, δax2, …, δaxn], [δay1, δay2, …, δayn], [δaz1, δaz2, …, δazn].

Найдем математическое ожидание шума, например, для измерителя угловой скорости в проекции на ось x:

При отсутствии дрейфа или смещения нуля у приборов, как прави-

ло, μ = 0.

Среднеквадратичное отклонение, СКО (σ) – показатель рассеи-

вания случайной величины относительно ее математического ожидания.

Параметр σ, в частности, в условиях предыдущего примера, вычисляется по формуле

Дисперсия (σ2) – мера разброса случайной величины относительно ее математического ожидания. Для ее вычисления достаточно СКО возвести в квадрат:

По формулам (1.29) – (1.31) аналогично определяются μ, σ, σ2 для остальных измерителей.

17

Спектральная мощность сигнала – функция, описывающая рас-

пределение мощности в зависимости от частоты.

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

Нормальное распределение (распределение Гаусса) – распределе-

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

Существует ошибочное предположение, что данный вид шума является эквивалентным понятию «белый шум». Однако термин «белый» имеет отношение к корреляции сигнала в два различных момента времени, которая не зависит от распределения амплитуды шума. Белый шум может иметь не только распределение Гаусса, но и Пуассона, Коши и т.д.

Ковариация (cov) – показатель линейной зависимости двух величин, представленных выборкой случайных значений. Определим данный параметр для вектора шумов измерений [v11, v12, …, v1n], [v21, v22, …, v2n]:

где µv1 , µv2 – математические ожидания шумов v1 и v2.

Обычно два канала СНС независимы, поэтому обычно выраже-

ния cov(v1, v2) и cov(v2, v1) близки к нулю, как и µv1 , µv2 а cov(v1, v1) и cov(v2, v2), как видно из (1.33), являются частным случаем диспер-

сии σv21 , σv22 величин v1 и v2.

18

Ковариационная матрица – квадратная симметрическая неотрицательно определенная матрица, главная диагональ которой представлена дисперсиями компонент случайного вектора, а остальные элементы – ковариациями между двумя компонентами.

Так, ковариационная матрица шумов измерений определяется из выражения

Для шумов системы она примет вид

Алгоритм работы фильтра Калмана состоит из двух этапов [6]:

1 Предсказание состояния системы. Оно заключается в вычисле-

нии оценки координат xˆk =(xˆ1 xˆ2 xˆ3 xˆ4 xˆ5 )T по результатам предыдущих измерений xˆk −1 :

Управление uk– = 0, в нем присутствует лишь только шумовая составляющая (δωz δωx –δωy δax δaz)Т без координат состояния соглас-

но (1.23).

Прогноз ошибок оценивания координат состояния, которые определяются ковариационной матрицей Pk :

Матрица Pk–1 представляет из себя ковариационную матрицу оши-

бок оценивания координат состояния δxˆk =(δxˆ1 δxˆ2 δxˆ3 δxˆ4 δxˆ5 )T , вычисленную на предыдущем такте и состоящую из попарных ковариа-

ций двух компонент данного вектора:

19

2 Корректировка вектора состояния. На данном этапе вычисляется коэффициент усиления фильтра Калмана kk, затем пересчитываются обновленные значение xˆk и Pk:

Вектор измерений в k-й момент времени обозначен через zk и представляет собой данные СНС о скоростях WN и WE.

1.4 Разработка модели движения БПЛА и фильтра Калмана в Matlab

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

-- тангаж ϑ = 1°; -- крен γ = 0°; -- курс ψ = 1°;

-- начальные скорости движения WN = 25 м/с, WE = 0 м/с; -- СКО шума измерителя угловой скорости σω = 5·10–5 рад/с; -- СКО шума измерителя ускорения σa = 2·10–2 м/с2;

-- СКО шума СНС σснс = 0,5 м/с.

Перед прочтением данного раздела рекомендуется предварительно ознакомиться с основами программирования в среде Matlab в литературе [2, 4, 5, 7], а также с обозначением переменных, используемых в данной программе и их соответствием с параметрами расчетных формул настоящих методических указаний (приложение А, таблица А.1).

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

20