Модель БПЛА с пилотажно-навигационным комплексом

Данная статья посвящёна примеру решения задачи реализации математической модели полета автономного беспилотного летательного аппарата (БПЛА). С математической точки зрения такая модель представляет собой многомерную нелинейную систему дифференциальных уравнений, требующих программного решения, а как физическая система, летательный аппарат должен функционировать в турбулентной воздушной среде и включать в себя органы управления приводами рулей высоты, крена и направления, а также силовую установку. В составе БПЛА должны находиться система глобального спутникового навигационного позиционирования и алгоритмы автоматического управления. Разработанная программа успешно решает данные задачи.

Основная задача данной статьи - это введение в принцип работы программы моделирования полета беспилотного летательного аппарата и подробное рассмотрение её основных функций. Сразу хочется сказать, что для полноценного понимания реализации физических законов и алгортимов читатель должен быть знаком с методами преобразования координат, навигацией, аэродинамикой, ТАУ и т.д.

Большинство задач решаются удивительно просто: надо взять и сделать.

При разработке имитационной модели БПЛА использовалась архетиктура программы, представленная на рисунке 1. Рассмотрим поподробнее каждый из блоков.

                                   Рисунок 1 - Блок схема mav_automodel.slx

drawAircraft

В этой функции инициализируются системы координат, которые очень важны для описания ориентации БПЛА. Вводятся инерциальная система координат, связанная система координат и скоростная система координат. Проводится расчет матрицы вращения для преобразования координат из одной системы отсчета в координаты другой системы отсчета. Данная матрица представлена ниже в функции rotate(). Введены углы Эйлера, углы атаки и скольжения для поворота координат из инерциальной системы в связанную систему. В данной программе учтена связь между воздушной скоростью, скоростью относительно Земли, скоростью ветра, а также связь между путевым, курсовым углом, углом наклона траектории полета и углом наклона траектории относительно воздушной массы. Получено также выражение для дифференцирования вектора во вращающейся системе отсчета. В дальнейшем можно уйти от углов Эйлера в сторону кватернионов. Подробнее о кватернионах и углах Эйлера-Крылова можно почитать в лекциях (файл – архив лекций "Эйлер, кватернионы").

%Реализация функции поворота СК
function XYZ=rotate(XYZ,gamma,vartheta,psi)
  
  R_roll = [
          1, 0, 0;
          0, cos(gamma), sin(gamma);
          0, -sin(gamma), cos(gamma)];
  R_pitch = [
          cos(vartheta), 0, -sin(vartheta);
          0, 1, 0;
          sin(vartheta), 0, cos(vartheta)];
  R_yaw = [
          cos(psi), sin(psi), 0;
          -sin(psi), cos(psi), 0;
          0, 0, 1];
  R = (R_roll*R_pitch*R_yaw)';
  XYZ = R*XYZ;
end

На вход в drawAircraft функцию поступает 2 сигнала:

1) Сигнал с функции mav_dynamics, который представляет собой 12 мерный вектор-столбец пространства состояний БПЛА.

2) Сигнал со скрипта инициализации plane_vertices. Он представляет собой матрицу с координатами вершин, граней и цветов, которые используются для отрисовка в drawAircraft БПЛА.

mav_dynamics

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

 Таблица 1 - Перменные состояния системы.

%Произодные координат в ИНС
ned_dot=[...
      cos(vartheta)*cos(psi), ...
      sin(gamma)*sin(vartheta)*cos(psi)-cos(gamma)*sin(psi),...
      cos(gamma)*sin(vartheta)*cos(psi)+sin(gamma)*sin(psi);
      cos(vartheta)*sin(psi),...
      sin(gamma)*sin(vartheta)*sin(psi)+cos(gamma)*cos(psi),...
      cos(gamma)*sin(vartheta)*sin(psi)-sin(gamma)*cos(psi);
      -sin(vartheta), sin(gamma)*cos(vartheta), cos(gamma)*cos(vartheta)]...
      *[u;v;w];
%Производные скоростей пространственных положений  
vel_dot=[r*v-q*w;p*w-r*u;q*u-p*v]+1/MAV.mass*[fx;fy;fz];
%Производные углов ореинтации БПЛА      
att_dot=[1,sin(gamma)*tan(vartheta),cos(gamma)*tan(vartheta);
            0,cos(gamma),-sin(gamma);
            0,sin(gamma)/cos(vartheta),cos(gamma/cos(vartheta)]*[p;q;r];        
%Производная от скорости изменения углов ореинтации БПЛА   
attrat_dot=[MAV.Gamma1*p*q-MAV.Gamma2*q*r;
               MAV.Gamma5*p*r-MAV.Gamma6*(p^2-r^2);
               MAV.Gamma7*p*q-MAV.Gamma1*q*r] +...
              [MAV.Gamma3*ell+MAV.Gamma4*n;
               1/MAV.Jy*m;
               MAV.Gamma4*ell+MAV.Gamma8*n];

На вход в mav_dynamics функцию поступает 2 сигнала:

1) Сигнал с функции forces_moments, в виде сил и моментов, действующих на БПЛА;

2) Сигнал со скрипта инициализации aerosonde_parameters, в котором задаются основные динамические параметры (моменты инерции, масса и т.д.)

forces_moments

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

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

Влияние гравитационного поля Земли на МБЛА может быть смоделировано в виде силы, которая действует вниз вдоль вертикальной составляющей ИСК и которая пропорциональна массе, умноженной на гравитационную постоянную.

%гравитационные силы
gForce(1) =  -MAV.mass*MAV.gravity*sin(vartheta);
gForce(2) =  MAV.mass*MAV.gravity*cos(vartheta)*sin(gamma);
gForce(3) =  MAV.mass*MAV.gravity*cos(vartheta)*cos(gamma);

Из курса аэродинамики известно, что по мере прохождения самолета через воздух, вокруг его корпуса создается определенный профиль давления (рисунок 2).

                                           Рисунок 2 - Анимация профиль давления.

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

Рисунок 3 - Графики зависимостей коэффициентов подъемной силы и силы сопротивления от угла атаки

%аэродинамические силы
aForce(1) = 0.5*MAV.rho*Va^2*MAV.S_wing*(C_X+C_X_q*MAV.c/(2*Va)*q+C_X_delta_e*delta_e);
aForce(2) = 0.5*MAV.rho*Va^2*MAV.S_wing*(MAV.C_Y_0+MAV.C_Y_beta*beta+...
        MAV.C_Y_p*MAV.b/(2*Va)*p+MAV.C_Y_r*MAV.b/(2*Va)*r+MAV.C_Y_delta_a*delta_a+...
        MAV.C_Y_delta_r*delta_r);
aForce(3) = 0.5*MAV.rho*Va^2*MAV.S_wing*(C_Z+C_Z_q*MAV.c/(2*Va)*q+C_Z_delta_e*delta_e);

%моменты сил
Torque(1) = MAV.b*(MAV.C_ell_0+MAV.C_ell_beta*beta+MAV.C_ell_p*MAV.b/(2*Va)*p+...
        MAV.C_ell_r*MAV.b/(2*Va)*r+MAV.C_ell_delta_a*delta_a+MAV.C_ell_delta_r*delta_r);
Torque(2) = MAV.c*(MAV.C_m_0+MAV.C_m_alpha*alpha+MAV.C_m_q*MAV.c/(2*Va)*q+...
        MAV.C_m_delta_e*delta_e);   
Torque(3) = MAV.b*(MAV.C_n_0+MAV.C_n_beta*beta+MAV.C_n_p*MAV.b/(2*Va)*p+...
        MAV.C_n_r*MAV.b/(2*Va)*r+MAV.C_n_delta_a*delta_a+MAV.C_n_delta_r*delta_r);
Torque(1)=Torque(1)-MAV.k_T_P*(MAV.k_Omega*delta_t)^2;

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

%сила тяги двигателя
pForce(1) = 0.5*MAV.rho*MAV.S_prop*MAV.C_prop*((MAV.K_dvig*delta_t)^2-Va^2);
pForce(2) = 0;
pForce(3) = 0;

Известно, что для моделирования турбулентной атмосферы достаточно использовать постоянные и случайные значения скорости ветра. Для реализации случайных значений, составляющих ветра достаточно отфильтровать белый шум через фильтр. В данной программе я использую фильтр Драйдена [3], который представляет собой обычные передаточные функции для северной, восточной и высотной составляющей ветра. После фильтрации был добавлен блок дискретизации сигнала с определённым временным интервалом. И сумматор соответственно для суммирования постоянной и случайной составляющей сигнала. Стоит сказать, что существует различные методы для реализации турбулентной атмосферы, например, в отраслевом ОСТе 102514-84 рассмотрена подробнейший алгоритм реализации математической модели турбулентной атмосферы. На рисунке 4 показан результат фильтрации белого шума.

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

На вход в forces_moments функцию поступает 3 сигнала:

1) Сигнал с функции autopilot, в которой реализован основной алгортим автоматического управления;

2) Сигнал со скрипта инициализации aerosonde_parameters, в котором задаются основные динамические параметры (моменты инерции, масса и т.д.);

3) Сигнал со скрипта инициализации wind_parameters, в котором задаются параметры атмосферы.

compute_trim и mavsim_trim.mdl

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

Целью compute_trim и mavsim_trim является расчет состояний балансировки и входных воздействий, при которых самолет одновременно выполняет следующие три условия [4]:

1) Перемещается с постоянной скоростью Va;

2) Набирает высоту с постоянным углом наклона траектории полета;

3) Находится на постоянной орбите заданного радиуса R.

Проблема найти такие состояния системы, при которых она бы сводилась к решению системы нелинейных алгебраических уравнений. Существует большое число численных методов решения подобных систем уравнений. Simulink использует встроенную процедуру для расчета условий балансировки. Формат команды trim Simulink имеет вид:

%расчет балансировочных значений
[x_trim,u_trim,~,dx_trim] = trim('mavsim_trim',x0,u0,y0,ix,iu,iy,dx0,idx);

где x_trim является рассчитанным сбалансированным состоянием, u_trim является рассчитанным входным параметром сбалансированного управления, а dx_trim является рассчитанной производной состояния.Также необходимо ввести начальные условия. Эта система определяется моделью mavsim_trim.mdl в Simulink (рисунок 4). Входными параметрами системы являются команды сервопривода delta_e, delta_a, delta_r и delta_t, а выходными параметрами являются рассчитанные балансировочные углы атаки, скольжения, а также рассчитанная воздушная скорость.

                                          Рисунок 4 - модель в Simulink (mavsim_trim.mdl)

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

autopilot

Пилотажно-навигационный комплекс (автопилот) является системой, используемой для управления полетом без помощи пилота. Для пилотируемого самолета автопилот является системой полного управления полетом, которая регулирует положение (высоту, широту, долготу) и ориентацию в пространстве (крен, тангаж, рыскание) вовремя различных фаз полета (например, при взлете, при наборе высоты, в горизонтальном полете, спуске, при заходе на посадку и при посадке). Для БПЛА автопилот является системой полного управления летательного аппарата во время всех фаз полета. Для проектирования автопилота бокового и продольного движений используется метод, который носит название последовательного замыкания контуров обратных связей. Подробнее с данным методом и алгоритмами расчета с помощью ЛАЧХ можно ознакомиться в архиве "Лекции".

Для примера разберем последовательность действий для расчета коэффициентов обратных связей автопилота бокового движения (для углов курса и крена).

1) в файле aerosond_parameters задать значения коэффициентов передаточной функции по углу крена (апериодическое звено);

%----------Обратная связь по крену-------------
%Коэффициенты передаточной функции
MAV.C_p_p=MAV.Gamma3*MAV.C_ell_p+MAV.Gamma4*MAV.C_n_p;
MAV.C_p_delta_a=MAV.Gamma3*MAV.C_ell_delta_a+MAV.Gamma4*MAV.C_n_delta_a;
MAV.a_phi1=-.5*MAV.rho*MAV.Va0^2*MAV.S_wing*MAV.b*MAV.C_p_p*MAV.b/2/MAV.Va0;
MAV.a_phi2=.5*MAV.rho*MAV.Va0*2*MAV.S_wing*MAV.C_p_delta_a;

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

%демпфер крена
MAV.omega_n_phi=sqrt(abs(MAV.a_phi2)*MAV.delta_a_max/MAV.e_phi_max);
MAV.zeta_phi= 4*0.707;
%----------Обратная связь по курсу-------------
%коэф для размещения частоты среза
MAV.W_x=15;
%расположение полосы пропускания для курса
MAV.omega_n_chi=MAV.omega_n_phi/MAV.W_x;
%выбор коэффициента демпфирования
MAV.zeta_chi=.707*1.5;

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

MAV.roll_kp = MAV.delta_a_max/MAV.e_phi_max*sign(MAV.a_phi2);
MAV.roll_kd = (2*MAV.zeta_phi*MAV.omega_n_phi-MAV.a_phi1)/MAV.a_phi2;
%статический коэффициент 
MAV.course_kp = 2*MAV.zeta_chi*MAV.omega_n_chi*MAV.Va0/MAV.gravity;
%астатический коэффициент 
MAV.course_ki = MAV.omega_n_chi^2*MAV.Va0/MAV.gravity;

4) Все значения коэффициентов передаются в функцию autopilot, в которой может быть реализован стандартный алгоритм ПИД регулятора [5];

delta_a = roll_hold(gamma_c, gamma, p, MAV);
    
function delta_a = roll_hold(gamma_c, gamma, p, MAV)
    error=gamma_c-gamma;
    differentiator=p;
    kp=MAV.roll_kp;
    kd=MAV.roll_kd;
    delta_a=sat(kp*error+kd*differentiator,45*pi/180,-45*pi/180);
end

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

1) стабилизации угла тангажа;

2) выдерживания высоты полета с помощью тангажа;

3) выдерживания воздушной скорости с помощью тангажа;

4) выдерживания воздушной скорости с помощью заслонки.

Эти управляющие режимы могут быть объединены при создании конечного автомата управления высотой полета. В области набора высоты дроссельная заслонка устанавливается на максимальное отклонение, и воздушная скорость выдерживается с помощью управляющих сигналов тангажа, тем самым обеспечивая условия, позволяющие БПЛА исключать срыв потока. Проще говоря, это заставляет МБЛА набирать высоту с максимально возможной скоростью набора высоты, пока он не достигнет заданной высоты полета. Аналогичным образом, в зоне снижения дроссельная заслонка устанавливается на минимальное открытие, и воздушная скорость выдерживается с помощью управляющих сигналов угла тангажа.

persistent altitude_state;
  
    if h<=MAV.altitude_take_off_zone     
        altitude_statet = 1;
    elseif h<=h_c-MAV.altitude_hold_zone 
        altitude_statet = 2;
    elseif h>=h_c+MAV.altitude_hold_zone 
        altitude_statet = 3;
    else
        altitude_statet = 4;
    end
    if t==0
        initialize_integrator = 1;
    elseif altitude_state~=altitude_statet
        initialize_integrator=1;
    else
        initialize_integrator=0;
    end
    altitude_state=altitude_statet;
    

    switch altitude_state
        case 1  
            delta_t=1;
            theta_c=MAV.climb_pitch;
            
        case 2
            delta_t=1;
            theta_c=airspeed_with_pitch_hold(Va_c, Va, initialize_integrator, MAV);
             
        case 3
            delta_t=0;
            theta_c=airspeed_with_pitch_hold(Va_c, Va, initialize_integrator, MAV);

        case 4
            delta_t=airspeed_with_throttle_hold(Va_c, Va, initialize_integrator, MAV);
            theta_c=altitude_hold(h_c, h, initialize_integrator, MAV);
    end
    delta_e = pitch_hold(theta_c, theta, q, MAV);

    delta_t = sat(delta_t,1,0);

На вход в autopilot функцию поступает 3 сигнала:

1) Сигнал с функции compute_trim, в которой реализован алгоритм расчета балансировочных значений;

2) Сигнал со скрипта инициализации aerosonde_parameters, в котором задаются основные динамические параметры (моменты инерции, масса и т.д.);

3) Сигнал с функции true_state, в которой селектируются состояния БПЛА.

Таким образом, мы получили программную реализацию модели беспилотного летательного аппарата в среде MATALAB/Simulink. Давайте рассмотрим основные плюсы и минусы:

+ Программа не использует функциональные встроенные блоки Simulink;

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

+ Данная программа может быть интегрирована в пилотажный стенд-тренажер для выполнения различных исследовательских работ (как показано в видео).

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

- Получение аэродинамических коэффициентов для конкретного самолета достаточно проблематично.

Спасибо за внимание!

Добравшись до конца, начинаешь думать о начале.