基于Simscape的四旋翼无人机物理模型仿真

Physical Model Simulation of Quadrotor UAV Based on Simscape

摘要: 针对四旋翼无人机控制问题，本文设计一种直观准确的仿真模型以提高无人机控制算法的开发速度。由于物理模型比数学模型更容易精确表现无人机在真实环境下所产生的运动学特性，故利用Solidworks软件和Matlab/Simscape工具箱搭建四旋翼无人机可视化物理仿真模型。结合Simulink下的简化电机螺旋桨模型得到完整的无人机仿真模型，在此基础上设计串级PID控制器进行仿真实验。仿真结果表明，所设计的串级PID控制器具有良好控制效果，所搭建的无人机模型能够较好模拟实际无人机的飞行表现。

Abstract: Aiming at the control problem of quadrotor UAV, this paper designs an intuitive and accurate simulation model to improve the development speed of the quadrotor control algorithm. Because the physical model is easier to accurately express the kinematic characteristics of UAV in the real environment than the mathematical model, thus we build the visual physical simulation model of quadrotor UAV by using SolidWorks software and Matlab/Simscape toolbox. Combined with the simplified motor propeller model under Simulink, a complete quadrotor simulation model is obtained. On this basis, we design a cascade PID controller for simulation experiments. The simulation results show that the cascade PID controller designed has a good control effect, and the quadrotor model can simulate well the flight performance of the actual quadrotor.

1. 引言

随着四旋翼无人机被广泛使用，人们对其稳定性和安全性的要求也进一步提高，而四旋翼无人机作为一种欠驱动的非线性系统，其在飞行过程中很容易受到外界飞行环境和自身结构参数变化的影响而失去控制。为此对无人机飞行控制算法的研究必不可少，而更贴近实际的更准确的四旋翼仿真模型能大大加速控制算法的开发和应用。

Simscape是由MathWorks公司推出的一种基于物理模型的综合建模平台，采用物理网络的方法进行模型构建，相较于公式推导建立数学模型的方式，利用Simscape所获得的物理模型更加直观和准确。目前很多领域都在利用Simscape进行建模，如电池模型 [1]、无刷直流电机模型 [2]、机器人运动控制 [3] 等，但在四旋翼无人机领域却很少被提及。文献 [4] 在四旋翼无人机建模中使用了SimMechanics进行物理建模(SimMechanics是Simscape下Multibody子库的前身)，但所建模型过于简化，没有对电机螺旋桨模型建立动态的仿真模型。文献 [5] 在四旋翼无人机建模中使用了Simscape进行物理建模，建立了动态的电机模型和螺旋桨模型，但却引入了复杂的电机控制模型，增加仿真运行时间和建模难度。并且Simscape物理模型有另外一个优势，很多研究中并没有利用，那就是能够很好反应无人机在仿真时的真实物理表现，所以当无人机搭载一些动态变化的机械结构时，可以很好模拟这些结构对无人机控制的影响，从而可以针对性的对这些扰动进行抑制。基于这种方式搭建的四旋翼无人机仿真模型，用于验证四旋翼无人机控制算法将会得到更准确的参数，在实际应用时需要进行的调整也将会更少。本文将针对搭载激光雷达的四旋翼无人机，建立数学模型和改进的可视化物理仿真模型，并在此基础上设计串级PID控制器，进行仿真分析。

2. 四旋翼无人机数学模型

本文研究的四旋翼无人机结构为X型结构，其结构示意图如图1所示。图中两个右手坐标系分别是地面坐标系 $O_e{x}_e{y}_e{z}_e$ 和机体坐标系 $O_b{x}_b{y}_b{z}_b$，其中 $z_e$ 方向为竖直向上， $z_b$ 方向为垂直螺旋桨平面向上，机头与 $x_b$ 轴正方向重合。 $\varphi$ 、 $\theta$ 、 $\psi$ 分别表示四旋翼无人机的滚转角、俯仰角、偏航角。 ${\omega }_1$ 、 ${\omega }_2$ 、 ${\omega }_3$ 、 ${\omega }_4$ 为四个旋翼的转速，俯视角下旋翼1、3逆时针旋转为正桨，产生垂直螺旋桨平面竖直向上的推力F1、F3和顺时针方向的力矩M1、M3，旋翼2、4顺时针旋转为反桨，产生垂直螺旋桨平面竖直向上的推力F2、F4和逆时针方向的力矩M2、M4。以机头方向为前方向，旋翼2、3转速较高会使左边推力较大，滚转角 $\varphi$ 增加，产生右滚动作，旋翼1、4的转速较高则反之；旋翼3、4转速较高会使后方推力较大，俯仰角 $\theta$ 增加，产生前倾动作，旋翼1、2的转速较高则反之；旋翼2、4转速较高会使逆时针方向力矩较大，偏航角 $\psi$ 增加，产生左转动作，旋翼1，3的转速较高则反之。

在假设四旋翼无人机是刚体、结构完全对称、质量和转动惯量不变、重心与机体坐标系重合、只受沿 $z_e$ 轴负方向的重力和沿 $z_b$ 轴正方向的螺旋桨拉力、低速且小角度(使机体的角速度可以等于相应欧拉角的变化率)、忽略空气阻力和陀螺效应的情况下可以得到其基于欧拉角的简化六自由度非线性数学模型 [6]：

Figure 1. Quadrotor structural diagram

图1. 四旋翼无人机结构示意图

$\{\begin{array}{l}m\ddot{x}=\left(\cos \varphi \sin \theta \cos \psi +\sin \varphi \sin \psi \right)f\hfill \\ m\ddot{y}=\left(\cos \varphi \sin \theta \sin \psi -\sin \varphi \cos \psi \right)f\hfill \\ m\ddot{z}=\left(\cos \varphi \cos \theta \right)f-mg\hfill \\ J_x\ddot{\varphi }={\tau }_x\hfill \\ J_y\ddot{\theta }={\tau }_y\hfill \\ J_z\ddot{\psi }={\tau }_z\hfill \end{array}$ (1)

式中m、g分别为无人机总质量和重力加速度。 $J_x$ 、 $J_y$ 、 $J_z$ 分别为无人机在三轴上的转动惯量。f为作用在机体上的总拉力。 ${\tau }_x$ 、 ${\tau }_y$ 、 ${\tau }_z$ 为无人机分别在三轴上所受的合力矩。

总拉力及力矩与四个电机转速的关系为 [7]：

$\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]$ (2)

式中 $c_T$ 为单桨综合拉力系数，单位为N/(rad/s)²，其与拉力系数 $C_T$ 的关系 [8] 为 $c_T=\left(1/4{\pi }^2\right)\rho D_p^4{C}_T$。 $c_M$ 为单桨综合力矩系数，单位为N∙m/(rad/s)²，其与力矩系数 $C_M$ 的关系为 $c_M=\left(1/4{\pi }^2\right)\rho D_p^5{C}_M$。d为无人机半轴距。

3. 仿真模型搭建

3.1. 可视化物理仿真模型搭建

利用SolidWokes软件搭建四旋翼无人机三维模型，如图2所示，忽略电机和机架的零件细节，保留机架、螺旋桨和激光雷达的结构特征，分别建立六个连杆零件图，在同一模型文件中进行装配，并设置好各个零件之间的配合关系。

Figure 2. Quadrotor three-dimensional model

图2. 四旋翼无人机三维模型

通过Simscape Multibody Link插件将装配好的四旋翼无人机三维模型文件导出为XML文件和可视化的STEP文件，在Matlab命令空间使用“smimport”命令建立Simscape Multibody模型。调整所建模型并补充地面模块、输出无人机姿态信息和位置信息的六自由度关节模块、输入电机四个控制量的电机螺旋桨模块后，得到四旋翼无人机仿真模型如图3所示。

Figure 3. Quadrotor simulation model

图3. 四旋翼无人机仿真模型

3.2. 电机螺旋桨模型

电机螺旋桨模型可以通过一个惯性环节进行描述 [9]，其传递函数为：

${\omega }_k=\frac{1}{T_{m}s+1}\left(C_R{D}_k+{\omega }_b\right)$ (3)

其中 ${\omega }_k,k=1,2,3,4$ 为电机转速， $D_k,k=1,2,3,4$ 为输入的控制量也即油门量， $T_m$ 为电机响应时间常数， $C_R$ 和 ${\omega }_b$ 分别为油门–转速曲线的线性拟合后的斜率和截距。

基于(2)式和(3)式搭建电机螺旋桨仿真模型如图4所示。

Figure 4. Motor-propeller simulation model

图4. 电机螺旋桨模型

3.3. 仿真模型参数设置

基于实际所搭样机的硬件信息，计算仿真模型的参数。实际样机如图5所示。

Figure 5. Quadrotor prototype

图5. 四旋翼无人机样机

各硬件信息如表1所示。参考北航可靠飞行控制研究组的飞行测评软件 [10]，得到仿真所需参数如表2所示。

Table 1. Brand and model of hardware in the prototype

表1. 样机所用硬件品牌及型号表

Table 2. Parameters of simulation model

表2. 仿真模型参数表

4. 控制器设计

4.1. 四旋翼无人机控制框架

采用内外环控制策略，控制框图如图6所示，其中内环对四旋翼无人机姿态角进行控制，外环对四旋翼无人机位置进行控制。

Figure 6. Control block diagram of quadrotor

图6. 四旋翼无人机控制框图

4.2. 串级PID控制器设计

假设四旋翼无人机处于定高悬停状态，拉力抵消重力 $f\approx mg$。俯仰角和横滚角非常小 $\theta \approx \varphi \approx \text{0}$，此时有 $\sin \varphi \approx \varphi$， $\cos \varphi \approx 1$， $\sin \theta \approx \theta$， $\cos \theta \approx 1$，可将(1)式的水平通道模型线性化为：

$\{\begin{array}{l}\ddot{x}=g\left(\theta \cos \psi +\varphi \sin \psi \right)\\ \ddot{y}=g\left(\theta \sin \psi -\varphi \cos \psi \right)\end{array}$ (4)

高度通道模型和姿态模型在(1)式中已简化到线性模型，不需要再进行线性化。根据(1)式和(4)式设计串级PID控制器为：

$\{\begin{array}{l}{v}_{ld}=K_{lp}{e}_l\\ {\Theta }_{ld}=g^{-1}{A}^{-1}\left(K_{vp}{e}_v+K_{vi}{\displaystyle \int e_v+K_{vd}{\stackrel{\dot{}}{e}}_v}\right)\\ {\stackrel{\dot{}}{z}}_d=k_{pz}{e}_z\\ f_d=m\left(g+k_{\stackrel{\dot{}}{z}p}{e}_{\stackrel{\dot{}}{z}}+k_{\stackrel{\dot{}}{z}i}{\displaystyle \int e_{\stackrel{\dot{}}{z}}}+k_{\stackrel{\dot{}}{z}d}{\stackrel{\dot{}}{e}}_{\stackrel{\dot{}}{z}}\right)\\ {\stackrel{\dot{}}{\Theta }}_d=K_{\Theta p}{e}_{\Theta }\\ {\tau }_d=K_{\stackrel{\dot{}}{\Theta }p}{e}_{\stackrel{\dot{}}{\Theta }}+K_{\stackrel{\dot{}}{\Theta }i}{\displaystyle \int e_{\stackrel{\dot{}}{\Theta }}+K_{\stackrel{\dot{}}{\Theta }d}{\stackrel{\dot{}}{e}}_{\stackrel{\dot{}}{\Theta }}}\end{array}$ (5)

其中水平通道期望速度 $v_{ld}=\left[\begin{array}{c}{\stackrel{\dot{}}{x}}_d\\ {\stackrel{\dot{}}{y}}_d\end{array}\right]$，水平通道位置环参数 $K_{lp}=\left[\begin{array}{cc}{k}_{xp}& 0\\ 0& k_{yp}\end{array}\right]$，水平通道位置误差 $e_l=\left[\begin{array}{c}{x}_d-x\\ y_d-y\end{array}\right]$。水平通道期望姿态角 ${\Theta }_{ld}=\left[\begin{array}{c}{\varphi }_d\\ {\theta }_d\end{array}\right]$，偏航角影响矩阵 $A=\left[\begin{array}{cc}\sin \psi & \cos \psi \\ -\cos \psi & \sin \psi \end{array}\right]$，水平通道速度环参数 $K_{vp}=\left[\begin{array}{cc}{k}_{\stackrel{\dot{}}{x}p}& 0\\ 0& k_{\stackrel{\dot{}}{y}p}\end{array}\right]$， $K_{vi}=\left[\begin{array}{cc}{k}_{\stackrel{\dot{}}{x}i}& 0\\ 0& k_{\stackrel{\dot{}}{y}i}\end{array}\right]$， $K_{vd}=\left[\begin{array}{cc}{k}_{\stackrel{\dot{}}{x}d}& 0\\ 0& k_{\stackrel{\dot{}}{y}d}\end{array}\right]$，水平通道速度误差 $e_v=\left[\begin{array}{c}{\stackrel{\dot{}}{x}}_d-\stackrel{\dot{}}{x}\\ {\stackrel{\dot{}}{y}}_d-\stackrel{\dot{}}{y}\end{array}\right]$。期望姿态角速度 ${\stackrel{\dot{}}{\Theta }}_d=\left[\begin{array}{c}{\stackrel{\dot{}}{\varphi }}_d\\ {\stackrel{\dot{}}{\theta }}_d\\ {\stackrel{\dot{}}{\psi }}_d\end{array}\right]$，姿态角度环参数 $K_{\Theta p}=\left[\begin{array}{ccc}{k}_{\varphi p}& 0& 0\\ 0& k_{\theta p}& 0\\ 0& 0& k_{\psi p}\end{array}\right]$，姿态角误差 $e_{\Theta }=\left[\begin{array}{c}{\varphi }_d-\varphi \\ {\theta }_d-\theta \\ {\psi }_d-\psi \end{array}\right]$。期望力矩 ${\tau }_d=\left[\begin{array}{c}{\tau }_{xd}\\ {\tau }_{yd}\\ {\tau }_{zd}\end{array}\right]$，姿态角速度环参数 $K_{\stackrel{\dot{}}{\Theta }p}=\left[\begin{array}{ccc}{k}_{\stackrel{\dot{}}{\varphi }p}& 0& 0\\ 0& k_{\stackrel{\dot{}}{\theta }p}& 0\\ 0& 0& k_{\stackrel{\dot{}}{\psi }p}\end{array}\right]$， $K_{\stackrel{\dot{}}{\Theta }i}=\left[\begin{array}{ccc}{k}_{\stackrel{\dot{}}{\varphi }i}& 0& 0\\ 0& k_{\stackrel{\dot{}}{\theta }i}& 0\\ 0& 0& k_{\stackrel{\dot{}}{\psi }i}\end{array}\right]$， $K_{\stackrel{\dot{}}{\Theta }d}=\left[\begin{array}{ccc}{k}_{\stackrel{\dot{}}{\varphi }d}& 0& 0\\ 0& k_{\stackrel{\dot{}}{\theta }d}& 0\\ 0& 0& k_{\stackrel{\dot{}}{\psi }d}\end{array}\right]$，姿态角速度误差 $e_{\stackrel{\dot{}}{\Theta }}=\left[\begin{array}{c}{\varphi }_d-\varphi \\ {\theta }_d-\theta \\ {\psi }_d-\psi \end{array}\right]$。

基于上述串级PID控制器，搭建控制器的仿真模型，得到完整的四旋翼无人机控制系统仿真模型如图7所示。

5. 仿真结果分析

通过对最内层姿态角角速度环的阶跃响应曲线进行分析，可得知控制器的动态性能表现。现给定高度0 m，分别给定偏航角速度、横滚角速度、俯仰角速度1 rad/s进行仿真。总仿真时间1 s，在0.5 s时，启动机械式激光雷达以10 Hz扫描频率进行工作，由于机械式激光雷达的旋转组件重心并不在无人机z轴上，当其以固定频率进行旋转时会对无人机产生周期性干扰，使无人机总体重心产生周期性偏移，对控制表现产生影响。在上述条件下进行Simulink仿真得到各姿态角角速度阶跃响应曲线如图8所示。

主要针对图8中俯仰角速度响应曲线进行分析，图中俯仰角速度能无误差稳定到给定值上，上升时间为0.027 s，峰值时间0.048 s，超调量64.2%，调整时间0.185 s。除超调量较大，其他性能指标均非常好，由于姿态角速度环为最内环，其响应速度非常重要，以超调量较大来换取更快的响应时间很有必要，满足控制要求。在t = 0.5 s时，俯仰角速度产生周期为0.1s的正弦波动，反应了启动激光雷达对控制系

Figure 7. Simulation model of quadrotor control system

图7. 四旋翼无人机控制系统仿真模型

Figure 8. Angular velocity step response curve

图8. 角速度阶跃响应曲线图

统产生的周期干扰，说明物理仿真模型能够准确模拟机械结构的动态变化，验证了仿真模型在物理结构表现上的准确性。

通过轨迹跟踪仿真，可验证物理仿真模型的准确性和所设计控制器的整体控制性能。给定一条螺旋上升轨迹为参考轨迹，通过Simulink仿真得到四旋翼无人机仿真模型运动产生的三维轨迹曲线，同时计算得到位置误差曲线以及姿态角误差曲线，分别如图9中各图所示。

由图9可以看出，四旋翼无人机控制系统能达到稳定状态，能跟踪参考轨迹，证明仿真模型具有可控性。对比文献 [11] 所做的基于数学模型的四旋翼无人机串级PID轨迹跟踪仿真，跟踪相同的螺旋上升曲线，文献 [11] 的位置跟踪的稳态误差大约在 ± 0.5 m范围内且一直存在，与图9位置跟踪误差曲线趋势一致。文献 [11]

Figure 9. Trajectory tracking simulation

图9. 轨迹跟踪仿真

献 [11] 的姿态角误差逐渐收敛到0，与图9姿态跟踪误差曲线趋势一致，表明所搭建的仿真模型与数学仿真模型具有相同的飞行控制表现，验证了基于Simscape四旋翼无人机仿真模型的可控性和飞行表现上准确性。

6. 结束语

本文利用Solidworks软件和Matlab/Simscape工具箱搭建了四旋翼无人机可视化物理仿真模型，给出了对应的数学模型，设计了该仿真模型的串级PID控制系统，根据实际样机计算了仿真模型的具体参数并进行姿态角速度响应仿真和轨迹跟踪仿真。仿真结果表明，所搭建的物理仿真模型与数学仿真模型在飞行表现上一致，但前者搭建更为快速直观，能更好模拟自身机械结构的动态变化，仿真出更复杂的干扰影响，在算法研究上为四旋翼无人机控制算法开发提供了更接近真实的验证仿真模型，在工程应用上为四旋翼无人机功能延展夯实仿真基础。如四旋翼无人机搭载机械臂，四旋翼无人机搭载物流箱等情况，这些情况下，四旋翼无人机会随着飞行过程中姿态或任务的改变导致自身重心的变化，从而对其自身的控制产生干扰，在数学模型上很难进行描述，但在Simscape平台上只需要在约束内连接相应的机械结构，即可直观准确地仿真出这种情形下控制器所面临的具体干扰，从而可以使控制算法在仿真阶段就考虑对这些干扰的抑制，进一步加速控制算法的开发和应用。

参考文献