一、主要内容

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

  1. 主要内容
  2. 四旋翼飞行器动力学模型
  3. 四旋翼飞行器运动学模型
  4. 控制效率模型
  5. 模块连接
  6. 结语

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

  1. 位置动力学模型

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

位置动力学方程如下：

[公式] (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. 姿态动力学模型

姿态动力学方程如下：

[公式] (2)

图2 姿态动力学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

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

  1. 位置运动学模型

位置运动学方程如下：

[公式] (3)

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

图3 位置运动学Simulink模型

2. 姿态运动学模型

姿态运动学方程如下：

[公式] (4)

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

图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

四、控制效率模型

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

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

[公式] (5)

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

[公式] (6)

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

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

图5 控制效率矩阵的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

五、模块连接

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

图6 控制效率模型、动力学模型、运动学模型的关系

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

图7 控制效率模型、动力学模型、运动学模型 Simulnk 模块连接

动力学模型内部：

运动学模型内部：

下面以四旋翼飞行器的悬停状态来验证模型能否正常运行。四旋翼模型的基本参数[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;        % 四旋翼初始高度

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

[公式] (7)

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

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

图8 四旋翼飞行器的悬停

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

六、结语

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

—— 2021.02.25

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

链接：pan.baidu.com/s/18hbrMi

提取码：e765

参考

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