四旋翼飞行器建模（二）— Simulink实现

一、主要内容

本文旨在用 Matlab/Simulink 搭建四旋翼飞行器的动力学与运动学模型，包括以下内容：

主要内容
四旋翼飞行器动力学模型
四旋翼飞行器运动学模型
控制效率模型
模块连接
结语

二、四旋翼飞行器动力学模型

位置动力学模型

在上一篇文章四旋翼飞行器建模（一）— 动力学及运动学方程中，已经推导出了四旋翼飞行器动力学与运动学方程，这里直接给出最后的推导结果。

位置动力学方程如下：

$\ \begin{cases} {\dot{v}}_x=-\frac{{f}}{m}\left( \cos \psi \sin \theta \cos \phi +\sin \psi \sin \phi \right)\\ {\dot{v}}_y=-\frac{{f}}{m}\left( \sin \psi \sin \theta \cos \phi -\cos \psi \sin \phi \right)\\ {\dot{v}}_z={g}-\frac{{f}}{m}\cos \phi \cos \theta\\ \end{cases}$ (1)

图1 位置动力学Simulink模型

图中的Matlab Function模块内容如下：

function [v_x_dot, v_y_dot, v_z_dot] = fcn(m, g, phi, theta, psi, f)
% 函数描述
% 1.作用：本函数为四旋翼的位置动力学微分方程组，通过四旋翼所受拉力、姿态角
%         计算得到四旋翼飞行器的速度
% 2.函数输入：
%   m：四旋翼飞行器质量(kg)
%   g：重力加速度(m/s^2)
%   phi：滚转角(rad)
%   theta：俯仰角(rad)
%   psi：偏航角(rad)
%   f：螺旋桨产生的总拉力(N)
% 3.函数输出：
%   v_x_dot：地球坐标系下沿x轴的速度的导数
%   v_y_dot：地球坐标系下沿y轴的速度的导数
%   v_z_dot：地球坐标系下沿z轴的速度的导数
% 4.四旋翼构型为“X”型，螺旋桨序号如下所示：
%            3↓   1↑
%              \ /
%              / \
%            2↑   4↓
%   其中，↑表示螺旋桨逆时针旋转；↓表示螺旋桨顺时针旋转。

v_x_dot = -f * (1/m) * (cos(psi) * sin(theta) * cos(phi) + sin(psi) * sin(phi));
v_y_dot = -f * (1/m) * (sin(psi) * sin(theta) * cos(phi) - cos(psi) * sin(phi));
v_z_dot = g - f * (1/m) * cos(phi) * cos(theta);
end

2. 姿态动力学模型

姿态动力学方程如下：

$\begin{cases} \dot{p}=\frac{1}{I_{xx}}\left[ \tau _x+qr\left( I_{yy}-I_{zz} \right) -J_{RP}q\varOmega \right]\\ \dot{q}=\frac{1}{I_{yy}}\left[ \tau _y+pr\left( I_{zz}-I_{xx} \right) +J_{RP}p\varOmega \right]\\ \dot{r}=\frac{1}{I_{zz}}\left[ \tau _z+pq\left( I_{xx}-I_{yy} \right) \right]\\ \end{cases}$ (2)

图中的Matlab Function模块内容如下：

function [p_dot, q_dot, r_dot] = fcn(tau_x, tau_y, tau_z, w1, w2, w3, w4, I_xx, I_yy, I_zz, J_RP, p, q, r)
% 函数描述
% 1.作用：本函数为四旋翼的姿态动力学微分方程组，通过四旋翼所受力矩和螺旋
%         桨转速计算得到四旋翼的机体角速度
% 2.函数输入：
%   wi：四个螺旋桨的转速(rad/s)
%   tau_x：x轴反扭力矩（机体轴）(N·m)
%   tau_y：y轴反扭力矩（机体轴）(N·m)
%   tau_z：z轴反扭力矩（机体轴）(N·m)
%   I_xx：四旋翼x轴转动惯量(kg·m^2)
%   I_yy：四旋翼y轴转动惯量(kg·m^2)
%   I_zz：四旋翼z轴转动惯量(kg·m^2)
%   J_RP：整个电机转子和螺旋桨绕转轴的总转动惯量(kg·m^2)
%   p：四旋翼x轴角速度（机体轴)(rad/s)
%   q：四旋翼y轴角速度（机体轴)(rad/s)
%   r：四旋翼z轴角速度（机体轴)(rad/s)
% 3.函数输出：
%   p_dot：四旋翼x轴角加速度（机体轴)
%   q_dot：四旋翼y轴角加速度（机体轴)
%   r_dot：四旋翼z轴角加速度（机体轴)
% 4.四旋翼构型为“X”型，螺旋桨序号如下所示：
%            3↓   1↑
%              \ /
%              / \
%            2↑   4↓
%   其中，↑表示螺旋桨逆时针旋转；↓表示螺旋桨顺时针旋转。
Omega = -w1 + w2 - w3 + w4;
p_dot = (1/I_xx) * (tau_x + q * r * (I_yy - I_zz) - J_RP * q * Omega);
q_dot = (1/I_yy) * (tau_y + p * r * (I_zz - I_xx) + J_RP * p * Omega);
r_dot = (1/I_zz) * (tau_z + p * q * (I_xx - I_yy) );
end

三、四旋翼飞行器运动学模型

位置运动学模型

位置运动学方程如下：

$\left[ \begin{matrix} \dot{x}& \dot{y}& \dot{z}\\ \end{matrix} \right] ^T=\left[ \begin{matrix} v_x& v_y& v_z\\ \end{matrix} \right] ^T$ (3)

位置运动学模型的Simulink模块搭建如下图所示：

2. 姿态运动学模型

姿态运动学方程如下：

$\left[ \begin{array}{c} \dot{\phi}\\ \dot{\theta}\\ \dot{\psi}\\ \end{array} \right] =\left[ \begin{matrix} 1& \tan \theta \sin \phi& \tan \theta \cos \phi\\ 0& \cos \phi& -\sin \phi\\ 0& \sin \phi /\cos \theta& \cos \phi /\cos \theta\\ \end{matrix} \right] \left[ \begin{array}{c} p\\ q\\ r\\ \end{array} \right]$ (4)

姿态运动学模型的Simulink模块搭建如下图所示：

图中的Matlab Function模块内容如下：

function [p_dot, q_dot, r_dot] = fcn(tau_x, tau_y, tau_z, w1, w2, w3, w4, I_xx, I_yy, I_zz, J_RP, p, q, r)
% 函数描述
% 1.作用：本函数为四旋翼的姿态动力学微分方程组，通过四旋翼所受力矩和螺旋
%         桨转速计算得到四旋翼的机体角速度
% 2.函数输入：
%   wi：四个螺旋桨的转速(rad/s)
%   tau_x：x轴反扭力矩（机体轴）(N·m)
%   tau_y：y轴反扭力矩（机体轴）(N·m)
%   tau_z：z轴反扭力矩（机体轴）(N·m)
%   I_xx：四旋翼x轴转动惯量(kg·m^2)
%   I_yy：四旋翼y轴转动惯量(kg·m^2)
%   I_zz：四旋翼z轴转动惯量(kg·m^2)
%   J_RP：整个电机转子和螺旋桨绕转轴的总转动惯量(kg·m^2)
%   p：四旋翼x轴角速度（机体轴)(rad/s)
%   q：四旋翼y轴角速度（机体轴)(rad/s)
%   r：四旋翼z轴角速度（机体轴)(rad/s)
% 3.函数输出：
%   p_dot：四旋翼x轴角加速度（机体轴)
%   q_dot：四旋翼y轴角加速度（机体轴)
%   r_dot：四旋翼z轴角加速度（机体轴)
% 4.四旋翼构型为“X”型，螺旋桨序号如下所示：
%            3↓   1↑
%              \ /
%              / \
%            2↑   4↓
%   其中，↑表示螺旋桨逆时针旋转；↓表示螺旋桨顺时针旋转。
Omega = -w1 + w2 - w3 + w4;
p_dot = (1/I_xx) * (tau_x + q * r * (I_yy - I_zz) - J_RP * q * Omega);
q_dot = (1/I_yy) * (tau_y + p * r * (I_zz - I_xx) + J_RP * p * Omega);
r_dot = (1/I_zz) * (tau_z + p * q * (I_xx - I_yy) );
end

四、控制效率模型

控制效率模型的作用是将螺旋桨旋转所产生的拉力和反扭力矩计算出来。

对于十字型结构的四旋翼飞行器，其螺旋桨旋转产生的拉力和力矩与螺旋桨转速的关系如下：

$\begin{cases} f=c_T\left( \varpi _{1}^{2}+\varpi _{2}^{2}+\varpi _{3}^{2}+\varpi _{4}^{2} \right)\\ \tau _x=dc_T\left( -\varpi _{2}^{2}+\varpi _{4}^{2} \right)\\ \tau _y=dc_T\left( \varpi _{1}^{2}-\varpi _{3}^{2} \right)\\ \tau _z=c_M\left( \varpi _{1}^{2}-\varpi _{2}^{2}+\varpi _{3}^{2}-\varpi _{4}^{2} \right)\\ \end{cases}$ (5)

对于X字型结构的四旋翼飞行器，其螺旋桨旋转产生的拉力和力矩与螺旋桨转速的关系如下：

$\begin{cases} f=c_T\left( \varpi _{1}^{2}+\varpi _{2}^{2}+\varpi _{3}^{2}+\varpi _{4}^{2} \right)\\ \tau _x=dc_T\left( \frac{\sqrt{2}}{2}\varpi _{1}^{2}-\frac{\sqrt{2}}{2}\varpi _{2}^{2}-\frac{\sqrt{2}}{2}\varpi _{3}^{2}+\frac{\sqrt{2}}{2}\varpi _{4}^{2} \right)\\ \tau _y=dc_T\left( \frac{\sqrt{2}}{2}\varpi _{1}^{2}+\frac{\sqrt{2}}{2}\varpi _{2}^{2}-\frac{\sqrt{2}}{2}\varpi _{3}^{2}-\frac{\sqrt{2}}{2}\varpi _{4}^{2} \right)\\ \tau _z=c_M\left( \varpi _{1}^{2}-\varpi _{2}^{2}+\varpi _{3}^{2}-\varpi _{4}^{2} \right)\\ \end{cases}$ (6)

式中， cT 为螺旋桨拉力系数； cM 为螺旋桨转矩系数； d 为机体中心到任一电机的距离。

控制效率模型的Simulink模块搭建如下图所示：

图中的Matlab Function模块内容如下：

function [f, tau_x, tau_y, tau_z] = fcn(w1, w2, w3, w4, c_T, c_M, d)
% 函数描述
% 1.作用：本函数用来计算螺旋桨旋转产生的总拉力和反扭力矩。
% 2.函数输入：
%   wi：四个螺旋桨的转速(rad/s)
%   c_T：螺旋桨拉力系数
%   c_M：螺旋桨力矩系数
%   d：机体中心和任一电机的距离(m)
% 3.函数输出：
%   f：螺旋桨拉力（机体轴）
%   tau_x：x轴反扭力矩（机体轴）
%   tau_y：y轴反扭力矩（机体轴）
%   tau_z：z轴反扭力矩（机体轴）
% 4.四旋翼构型为“X”型，螺旋桨序号如下所示：
%            3↓   1↑
%              \ /
%              / \
%            2↑   4↓
%   其中，↑表示螺旋桨逆时针旋转；↓表示螺旋桨顺时针旋转。

f = c_T * (w1^2 + w2^2 + w3^2 + w4^2);
tau_x = d * c_T * (sqrt(2)/2) * (-w1^2 + w2^2 + w3^2 - w4^2);
tau_y = d * c_T * (sqrt(2)/2) * (w1^2 - w2^2 + w3^2 - w4^2);
tau_z = c_M * (w1^2 + w2^2 - w3^2 - w4^2);
end

五、模块连接

以上三个模型 —— 控制效率模型、动力学模型、运动学模型的关系如下图所示：

控制效率模型、动力学模型、运动学模型的 Simulink 模块连接如下图所示：

动力学模型内部：

运动学模型内部：

下面以四旋翼飞行器的悬停状态来验证模型能否正常运行。四旋翼模型的基本参数^[1]如下：

ModelParam_c_T = 1.105e-05;    % 螺旋桨拉力系数
ModelParam_c_M = 1.779e-07*2;  % 螺旋桨力矩系数
ModelParam_d = 0.225;          % 机体中心和任一电机的距离(m)
ModelParam_m = 1.4;            % 四旋翼飞行器质量(kg)
ModelParam_g = 9.8;            % 重力加速度(m/s^2)
ModelParam_I_xx = 0.0211;      % 四旋翼x轴转动惯量(kg·m^2)
ModelParam_I_yy = 0.0219;      % 四旋翼y轴转动惯量(kg·m^2)
ModelParam_I_zz = 0.0366;      % 四旋翼z轴转动惯量(kg·m^2)
ModelParam_J_RP = 0.0001287;   % 整个电机转子和螺旋桨绕转轴的总转动惯量(kg·m^2)
ModelInit_Pos_z = -100;        % 四旋翼初始高度

当四旋翼悬停在空中时，螺旋桨产生的拉力与四旋翼的重力相等。根据这个关系可以求得四旋翼的悬停转速：

$\varpi _i=\sqrt{\frac{mg}{4c_T}}=\sqrt{\frac{1.4\times 9.8}{4\times 1.105\times 10^{-5}}}=557.142\,\,rad/s$ (7)

将模型的螺旋桨转速输入设置为 557.142 的常值：

仿真时间设置为 10s，运行模型，得到以下结果：

可以看到，四旋翼飞行器在离地面 100m 处悬停。

六、结语

建模过程中未考虑空气动力、外部扰动等因素，与实际模型有一定的偏差。
下一步将对电机进行建模，并针对四旋翼设计飞行控制器。

—— 2021.02.25

本文的模型文件如下（帮忙点个赞哦~）。

链接：https://pan.baidu.com/s/18hbrMijwoluvsmg3kf6jXw

提取码：e765

参考

^全权. 多旋翼飞行器设计与控制实践[M]. 北京: 电子工业出版社, 2020.

四旋翼飞行器建模（二）— Simulink实现

玖辞丶

一、主要内容

二、四旋翼飞行器动力学模型

三、四旋翼飞行器运动学模型

四、控制效率模型

五、模块连接

六、结语

参考

为你推荐

常见移动机器人运动学模型总结

轨迹优化10. 辩证法

最优控制问题数值方法-间接法/变分法 (5)

Dijkstra算法

最优控制问题数值方法-间接法/变分法 (7)

PID控制学习笔记之二增量PID

关于作者

玖辞丶

4

0

0

1

四旋翼飞行器建模（二）— Simulink实现

四旋翼飞行器建模（一）— 动力学及运动学方程

自抗扰控制算法（三）— 控制实例

相关推荐

（七）传递函数

采用 S-function 编写机器人系统的方法总结

自抗扰控制算法（三）— 控制实例

轨迹优化12. 简化模型与内动态

无人机目标跟踪与运动控制⑤——PID轨迹跟踪控制(实验篇)

（五）线性时不变系统

热门泡泡

30积分失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 ros中启动gazebo时报错

5积分想买能用ROS2的开发套件。或者开发板

给作者打赏

四旋翼飞行器建模（二）— Simulink实现

玖辞丶

一、主要内容

二、四旋翼飞行器动力学模型

三、四旋翼飞行器运动学模型

四、控制效率模型

五、模块连接

六、结语

参考

为你推荐

常见移动机器人运动学模型总结

轨迹优化10. 辩证法

最优控制问题数值方法-间接法/变分法 (5)

Dijkstra算法

最优控制问题数值方法-间接法/变分法 (7)

PID控制学习笔记之二增量PID

评论（1）

关于作者

玖辞丶

4

0

0

1

四旋翼飞行器建模（二）— Simulink实现

四旋翼飞行器建模（一）— 动力学及运动学方程

自抗扰控制算法（三）— 控制实例

相关推荐

（七）传递函数

采用 S-function 编写机器人系统的方法总结

自抗扰控制算法（三）— 控制实例

轨迹优化12. 简化模型与内动态

无人机目标跟踪与运动控制⑤——PID轨迹跟踪控制(实验篇)

（五）线性时不变系统

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 ros中启动gazebo时报错

5积分 想买能用ROS2的开发套件。或者开发板

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板