基于Simulink与FlightGear联合建模并仿真多旋翼无人机在平衡态的动态控制

基于Simulink与FlightGear联合建模并仿真多旋翼无人机在平衡态的动态控制

本次实验中主要关注的是对无人机的动态控制进行建模并仿真，即探究计算主板输出PWM波到整个无人机系统，该系统输出变量的变化（例如：位置、速度、位姿、角速度等）。为什么要选择对平衡态的无人机进行建模呢？因为当俯仰角和滚转角过大时，计算主板对俯仰角和滚转角的控制模型从线性控制器变为非线性控制，控制难度增大，解析计算难度增大。当然并不是无法对无人机的高速高机动建模，比如SE（3）微分平坦算法就可以解决该类问题。但非线性控制不是本文主题（我太菜，所以只能在平衡态建模）。

贴个图说明一下当姿态角幅度过大时，线性控制器已经无法良好跟踪信号。

一、建模

这部分内容参考了北航可靠飞行组的实验代码，我从头梳理一遍之后，自己按照物理模型一步一步画simulink框图。这样自己走一遍代码更有体会。

贴一下资料网站

代码与实验课件：
http://rfly.buaa.edu.cn/course.html#ZhCourse

控制平台FlightGear与PX4工具包：https://rflysim.com/zh/3_Using/ControllerDesignAndSimulationPlatform.html#id2

首先我们打开动力系统模拟网站http://flyeval.com/paper/，选择无人机的元器件型号，随后计算生成该系统的物理参数

根据悬停时力平衡的关系
$$mg=4c_T{\omega }^2$$

可以解出悬停时电机的转速，我们将它作为电机的初始转速。

将这些物理参数作为初始化值，写成Matlab脚本:

clear;

%电机相关参数
% 油门——电机稳定转速关系式：wss = Cr * σ + wb
MotorInit_RPM = 557.1420;   % 给定电机初始转速
Motor_Cr = 737.24;          % 油门——电机稳定转速曲线斜率
Motor_wb = 161.87;          % 油门——电机稳定转速曲线截距
Motor_Tm = 0.0895;          % 电机惯性时间常数
Motor_Jm = 2.13e-4;         % 电机螺旋桨转动惯量

%螺旋桨相关参数
Rotor_CT = 1.051e-5;         % 螺旋桨推力系数
Rotor_CM = 1.789e-7;         % 螺旋桨力矩系数 

%机体气动参数
uav_Cd =  0.228;             % 阻力系数
uav_mass = 1.5;              % 多旋翼总体质量
uav_R = 0.225;               % 多旋翼机架半径
uav_Jxx = 1.663e-2;          % x轴转动惯量（单位： kg.m^2）
uav_Jyy = 1.663e-2 ;         % y轴转动惯量（单位： kg.m^2）
uav_Jzz = 3.033e-2;          % z轴转动惯量（单位： kg.m^2）
uav_J = [uav_Jxx,0,0;0,uav_Jyy,0;0,0,uav_Jzz];
uav_GravityAcc = 9.8;        % 机体在重力场所受加速度

%计算悬停时所需油门
%公式: mg = 4*CT*ωss^2
%     ωss = CR*σ + ωb
THR = (sqrt(uav_mass*uav_GravityAcc/4/Rotor_CT)-Motor_wb)/Motor_Cr;

得到机体自身的物理参数后，我们打开Simulink环境，开始搭建动态控制模型。按照如下的控制流程框图，我更偏向于按照从底层到顶层的顺序来搭建模型。

首先我们先搭建动力单元模型。主板输出的PWM波范围为[1000,2000]，我们需要提前对PWM波进行归一化处理，转换成油门值。

接下来我们对动力单元进行建模，根据动力单元公式：

$${\omega }_{ss}=C_r\sigma +{\omega }_b$$

$${\omega }_i=\frac{1}{T_{m}s+1}{\omega }_{ss}$$

先对第一条公式进行建模，输入为油门 输出为目标稳定转速

我们可以很明显的发现第二个公式实际上是一个一阶动态系统，为了控制的方便，我们将其化为状态空间方程式的形式。

$$x\prime =Ax+Bu$$

$$y=Cx+D$$

设ωi作为输出变量，Tmω为状态变量，套进表达形式里推一推可以得到

$$x=T_m{\omega }_{ss}$$

$$x\prime =\frac{T_m}{T_{m}s+1}{\omega }_{ss}\prime =\frac{T_{m}s}{T_{m}s+1}{\omega }_{ss}$$

$$\frac{T_{m}s}{T_{m}s+1}{\omega }_{ss}=(\frac{A{T}_m}{T_{m}s+1}+B){\omega }_{ss}$$

$$A{T}_m+B(T_{m}s+1)=T_{m}s$$

解得

$$\{\begin{array}{c}A=-\frac{1}{T_m}\\ B=1\\ C=\frac{1}{T_m}\\ D=0\end{array}$$

在Simulink上对稳定转速与当前转速的传递函数进行建模，并结合之前建立的油门与稳定转速的模型，由此我们就可以得到以油门为输入，以螺旋桨转速为输出动力单元的整体模型。

接着我们根据X字型四旋翼无人机的控制效率公式搭建模型

$$\left[\begin{array}{c}{F}_{拉力}\\ {\tau }_x\\ {\tau }_y\\ {\tau }_z\end{array}\right]=\left[\begin{array}{cccc}{c}_T& c_T& c_T& c_T\\ \frac{\sqrt{2}}{2}d{c}_T& -\frac{\sqrt{2}}{2}d{c}_T& -\frac{\sqrt{2}}{2}d{c}_T& \frac{\sqrt{2}}{2}d{c}_T\\ \frac{\sqrt{2}}{2}d{c}_T& \frac{\sqrt{2}}{2}d{c}_T& -\frac{\sqrt{2}}{2}d{c}_T& -\frac{\sqrt{2}}{2}d{c}_T\\ c_M& -c_M& c_M& -c_M\end{array}\right]\left[\begin{array}{c}{\omega }_1^2\\ {\omega }_2^2\\ {\omega }_3^2\\ {\omega }_4^2\end{array}\right]$$

一开始我打算直接在模型里插入一个矩阵相乘的模块，但这样不好把4*1矩阵中的拉力和力矩分离出来以供刚体控制模型使用。可以在simulink中插入“Matlab Function函数”来自己编写函数脚本，自动生成函数自变量为输入、因变量为输出的模块。

%自定义函数模块
function [Fb,Mb] = efficiency_control(w,R,Cm,Ct)
 %%叉字型
 %3  1
 %2  4
 M_rctcm = [-sin(pi/4)*R*Ct,sin(pi/4)*R*Ct,sin(pi/4)*R*Ct,-sin(pi/4)*R*Ct;
            sin(pi/4)*R*Ct,-sin(pi/4)*R*Ct,sin(pi/4)*R*Ct,-sin(pi/4)*R*Ct;
            Cm,Cm,-Cm,-Cm];
 Mb = M_rctcm*(w.^2);%式(6.24),力矩
 
 Fb = [0; 0; -sum(Ct*(w.^2))]; %(6.23),螺旋桨拉力

end

接下来，我们搭建气动阻力模型。根据阻力公式

$$f_{阻}=-C_d{v}_b,其中C_d为阻力系数（常数），v_b为机体坐标系下的速度$$

我们可以把气动模型在simulink里封装为函数

function f_r = resistance_model(Vb,Cd)

f_r = Cd*Vb.*abs(Vb)*0.5;

end

最后我们建立最上层的刚体控制模型，这部分要分为动力学和运动学两个部分。根据刚体动力学的两个公式：

$$\{\begin{array}{c}{}^{b}v\prime =-[{}^b\omega ]\times {}^{b}v+g{R}^T{e}_3-\frac{f}{m}{e}_3\\ J\cdot {}^b\omega \prime =-{}^b\omega \times (J\cdot {}^b\omega )+G_a+\tau \end{array}$$

将机体速度、机体角速度、受力f、陀螺力矩、力矩作为输入，可以解得机体速度微分和角速度微分输出，再经过一个积分环节便可以得到速度、角速度。很明显前验速度的微分需要后验速度的参与才可以计算出来，因此还需要将输出量再返回给输入量。

先对第一条公式进行建模将地球固联坐标系的重力加速度变换到机体坐标系后再加上拉力与阻力的影响并减去速度与角速度的叉乘便可以的到机体速度的微分 （注意vb的反馈）

再对第二条公式进行建模，根据公式将力矩、陀螺力矩与一大坨矩阵叉乘点乘加起来后再除以转动惯量便得到了机体角速度的微分。

计算陀螺力矩并封装为函数

function Ga = fcn(Jrp,MotorRads,pqr)
Ga=zeros(3,1);
Ga(1)=Jrp*pqr(2)*(-MotorRads(1)-MotorRads(2)+MotorRads(3)+MotorRads(4));
Ga(2)=Jrp*pqr(1)*( MotorRads(1)+MotorRads(2)-MotorRads(3)-MotorRads(4));

最终搭建的模型如下：

得到了机体速度的微分与机体角速度的微分，我们通过积分环节便可以得到机体速度与机体角速度，机体速度经过坐标变换便可以的到地球固联坐标系的速度，机体角速度则可以通过一下公式转换为四元数、旋转矩阵和欧拉角的导数。

$$\{\begin{array}{c}R\prime =R[{}^b\omega {]}_x\\ \Theta \prime =W\cdot {}^b\omega \\ q_v\prime =\frac{1}{2}(q_0{I}_3+[q_v{]}_x){}^b\omega \end{array}$$

在进行姿态表示的转换时，我们要注意旋转矩阵和欧拉角存在解的奇异性问题，因此选用四元数表示和计算才是最佳方法，先将机体角速度转换成四元数

function qdot = fcn(pqr,q)

qdot=0.5*[0,-pqr(1),-pqr(2),-pqr(3);pqr(1),0,pqr(3),-pqr(2);pqr(2),-pqr(3),0,pqr(1);pqr(3),pqr(2),-pqr(1),0]*q;

得到四元数导数后再进行积分得到四元数，当然不能忘记对四元数进行归一化处理，得到四元数后便可以转成欧拉角和旋转矩阵了。

对机体速度和地球固联坐标系下的速度进行积分处理便得到了机体坐标系下的位移和地球固联坐标系下的位移

最后的最后，我们完成了对无人机的动态建模，最终模型如下（麻了）

二、仿真

仿真联合了FlightGear软件进行，因此需要提前下载该软件并在Matlab中安装相关工具包。仿真前先进行配置，编写myownUAV-set.xml配置文件和myownUAV.xml文件

myownUAV-set.xml配置文件如下：

    
        myownUAV
        network
        
            Aircraft/myownUAV/Models/myownUAV.xml
        
         -40 
        
            2
        
    

myownUAV.xml配置文件如下：

 myownUAV.ac



  spin
  propeller1
  /engines/engine[0]/rpm
  -1
  
    0
    7.5
    1
  
  
    0.0
    0.0
    1.0
  




  spin
  propeller2
  /engines/engine[1]/rpm
  1
  
    7.5
    0
    1
  
  
    0.0
    0.0
    1.0
  




  spin
  propeller3
  /engines/engine[2]/rpm
  -1
  
    0
    -7.5
    1
  
  
    0.0
    0.0
    1.0
  





  spin
  propeller4
  /engines/engine[3]/rpm
  1
  
    -7.5
    0
    1
  
  
    0.0
    0.0
    1.0
  



  

在运行FlightGear之前，要先双击GEN FG RUN模块，打开后设置脚本名称、FlightGear位置、模型名称、端口、飞行机场背景等以后，点Generate Script在MATLAB工作目录下生成一个脚本。

使用文本编辑器打开脚本，做如下修改：

• start-date-lat后面的时间改为2004:06:01:01:00:00
• 找到freeze,将前面的enable
  改为disable

运行FlightGear。运行生成的.bat文件，然后对
Matlab进行仿真即可实现Matlab动力学模
型驱动FlightGear 3D模型飞行。最终仿真时的模型框图为：

先令油门值为0.7，经过3s后再令油门值为0.5，观察FlightGear中无人机的运动，示波器中显示高度的变化值

感悟

Simulink建模是研究机器人控制模型的有利工具，应当系统认真学习。在建模中要注意以下几点：

• 矩阵相乘模块要注意左乘和右乘的区别
• 积分模块中可能存在运算时序导致的错误，应当尽量减少延迟的加入，并记得设置微小初始值量
• 解决代数环问题是也要注意记得设置微小初始量