四旋翼无人机动力学数学模型与Matlab仿真
四旋翼无人机动力学数学模型与Matlab仿真
四旋翼无人机作为一种常见的无人机类型，其动力学特性是理解无人机飞行控制的基础。本文将从力的来源、数学模型到Matlab仿真，全面介绍四旋翼无人机的动力学特性。
力的来源
无人机的动力系统由电调-电机-螺旋桨组成。直观来看，电机带动螺旋桨旋转产生升力。螺旋桨旋转产生升力的原理，早在伯努利时代就被解释清楚：流速大，压强小；流速小，压强大，这就是伯努利定理。螺旋桨的桨面设计使得旋转时桨面上下的空气流速不同，从而产生向上的推力。
数学模型
对于四旋翼无人机，可以将输入简化为四个电机的油门，每个电机的油门归一化到0-1区间。四旋翼动力学数学模型的目标是根据电机的油门计算出升力和各轴的力矩。这里涉及三个模型近似：
- 电机转速与油门的关系：
电机稳态转速与油门成线性关系，但实际中0油门也会有点转速，存在截距。
电机近似为一阶系统逐渐达到稳态转速。
公式如下：
$$\dot{\bar{\omega}}(t) = \frac{C_m \sigma + \bar{\omega}_m - \bar{\omega}(t)}{T_m}$$
其中，$C_m$是电机转速斜率，$\bar{\omega}_m$是电机转速截距，$T_m$是电机时间常数。
- 升力与转速的关系：
- 升力与转速平方成正比。
- 计算公式为：$T = C_T \sum_{i=1}^{4} \omega_i^2$
- 力矩的产生：
四个螺旋桨的合力共同作用在机体系z轴；四个螺旋桨力的差异在机体系三个轴产生力矩。
x和y轴靠升力的不平衡来产生力矩；z轴力矩的产生靠反扭矩。
力矩的计算公式为：$\tau = r \times F$
在四旋翼中，力、力臂、力矩的关系如下图所示：
其中绿色F1、F2、F3、F4为各电机产生的力，方向为垂直xy平面向上，黄色d为力臂矢量，则力矩Mi的方向通过右手定则可以得到方向，橙色M1、M2、M3、M4则为各电机所产生的力矩。
以电机1为例，其产生的力矩M1在机体系x,y轴的分量为：
$$M_{1x} = d \cdot F_1 \cdot \sin(\theta)$$
$$M_{1y} = d \cdot F_1 \cdot \cos(\theta)$$同理可得到四个电机产生的合力矩，在x和y轴为：
$$M_x = \sum_{i=1}^{4} d \cdot F_i \cdot \sin(\theta_i)$$
$$M_y = \sum_{i=1}^{4} d \cdot F_i \cdot \cos(\theta_i)$$在z轴方向上，螺旋桨旋转，空气给螺旋桨一个反方向的阻力，例如逆时针旋转的1号电机，黑色v为螺旋桨线速度方向，绿色f1为等效空气阻力，黄色r为力臂矢量，则通过力矩计算公式得到该力矩橙色M1z为垂直向下，大小为：
$$M_{1z} = r \cdot f_1$$同理可得到其它螺旋桨旋转产生的z轴方向的力矩M2z（垂直朝上）、M3z（垂直朝下）、M4z（垂直朝上）。但是等效反扭力矩f1难以得到，通过实验得出，反扭力矩也和螺旋桨的转速平方成正比。
其中$C_M$为反扭力矩系数，代表单个螺旋桨转速增加1rad/s，反扭力矩增加的大小。那么可以得到四个螺旋桨产生的反扭力矩为：
$$M_z = C_M \sum_{i=1}^{4} \omega_i^2$$
数学模型总结
油门和电机转速的计算公式：
$$\dot{\bar{\omega}}(t) = \frac{C_m \sigma + \bar{\omega}_m - \bar{\omega}(t)}{T_m}$$转速和升力的计算公式：
$$T = C_T \sum_{i=1}^{4} \omega_i^2$$转速和力矩的计算公式：
$$M_x = \sum_{i=1}^{4} d \cdot F_i \cdot \sin(\theta_i)$$
$$M_y = \sum_{i=1}^{4} d \cdot F_i \cdot \cos(\theta_i)$$
$$M_z = C_M \sum_{i=1}^{4} \omega_i^2$$
Matlab 仿真
油门与电机转速模型仿真
仿真代码如下，反应了电机转速响应油门的变化曲线：
%% 油门与电机转速模型测试
global dt Tm Cm varpim
dt = 1e-3; % 仿真时间步长
Cm = 706.01; % 油门增大1，电机转速变化(RPM)
varpim = 170.47; % 零占空比时电机转速(RPM)
Tm = 0.260; % 电机时间常数
N = 2000;
t = 0:dt:dt*(N-1);
sigma = [0.7; 0.6; 0.5; 0.4];
varpi = zeros(N, 4);
k=1;
for tt=0:dt:(N-2)*dt
k = k+1;
% 动力单元模型
varpi(k, 1) = motor(sigma(1), varpi(k-1, 1)); % 电机1转速
varpi(k, 2) = motor(sigma(2), varpi(k-1, 2)); % 电机2转速
varpi(k, 3) = motor(sigma(3), varpi(k-1, 3)); % 电机3转速
varpi(k, 4) = motor(sigma(4), varpi(k-1, 4)); % 电机4转速
end
figure(1);plot(t, varpi(:,1), 'LineWidth', 1.5); hold on
plot(t, varpi(:,2), 'LineWidth', 1.5);
plot(t, varpi(:,3), 'LineWidth', 1.5);
plot(t, varpi(:,4), 'LineWidth', 1.5); hold off
legend(['\sigma_1=' num2str(sigma(1))], ['\sigma_2=' num2str(sigma(2))],['\sigma_3=' num2str(sigma(3))],['\sigma_4=' num2str(sigma(4))]);
xlabel('时间 t (s)');ylabel('转速 \varpi (rad/s)');title('电机模型测试'); grid on; grid minor
%% 电机模型
% 输入：油门大小 sigma（0-1）
% 电机上一时刻的转速（rad/s）
% 输出：此时刻电机转速（rad/s）
function varpi = motor(sigma, varpi_)
global dt Tm Cm varpim;
dvarpi = (Cm * sigma + varpim - varpi_) / Tm * dt;
varpi = varpi_ + dvarpi;
end
油门与升力、力矩的关系仿真
仿真代码如下：
%% 油门与升力、力矩模型测试
global dt Tm Cm varpim d cT cM
dt = 1e-3; % 仿真时间步长
Cm = 706.01; % 油门增大1，电机转速变化(RPM)
varpim = 170.47; % 零占空比时电机转速(RPM)
Tm = 0.260; % 电机时间常数
d = 0.225; % 450mm/2
cT = 1.201e-5; % 升力系数
cM = 1.574e-7; % 反扭力系数
N = 2000;
t = 0:dt:dt*(N-1);
sigma = [0.7; 0.6; 0.5; 0.4];
varpi = zeros(N, 4);
T = zeros(N, 1);
tau = zeros(N, 3);
k=1;
for tt=0:dt:(N-2)*dt
k = k+1;
% 电机模型
varpi(k, 1) = motor(sigma(1), varpi(k-1, 1)); % 电机1转速
varpi(k, 2) = motor(sigma(2), varpi(k-1, 2)); % 电机2转速
varpi(k, 3) = motor(sigma(3), varpi(k-1, 3)); % 电机3转速
varpi(k, 4) = motor(sigma(4), varpi(k-1, 4)); % 电机4转速
[T(k), tau(k,:)] = power_mix(varpi(k, :));
end
figure(1);subplot(211); plot(t, T, 'linewidth', 1.5); title('动力合成模型');ylabel('升力 (N)');
subplot(212);plot(t, tau(:,1), 'linewidth', 1.5);hold on
plot(t, tau(:,2),'linewidth', 1.5);plot(t, tau(:,3),'linewidth', 1.5);hold off
ylabel('力矩 (N\cdotm)');xlabel('时间 (t)'); legend('\tau_x', '\tau_y', '\tau_z');
%% 电机模型
% 输入：油门大小 sigma（0-1）
% 电机上一时刻的转速（rad/s）
% 输出：此时刻电机转速（rad/s）
function varpi = motor(sigma, varpi_)
global dt Tm Cm varpim;
dvarpi = (Cm * sigma + varpim - varpi_) / Tm * dt;
varpi = varpi_ + dvarpi;
end
%% 动力合成模型
% 输入：四个电机转速
% 输出：合升力与三轴力矩
function [T, tau] = power_mix(varpi)
global cT cM d;
T = cT * sum(varpi.^2);
tau(1) = sqrt(2)/2 * d * cT * (-varpi(1)^2 + varpi(2)^2 + varpi(3)^2 - varpi(4)^2);
tau(2) = sqrt(2)/2 * d * cT * ( varpi(1)^2 + varpi(2)^2 - varpi(3)^2 - varpi(4)^2);
tau(3) = cM * (varpi(1)^2 - varpi(2)^2 + varpi(3)^2 - varpi(4)^2);
end