多旋翼无人机理论 | 四旋翼动力学数学模型与Matlab仿真

力的来源

无人机的动力系统：电调-电机-螺旋桨 。

给人最直观的感受就是 电机带动螺旋桨转，产生升力。

螺旋桨旋转产生升力的原因，在很多年前伯努利就给出了解释，简单说就是流速大，压强小；流速小，压强大，也就是伯努利定理。


可以看到螺旋桨的桨面并不是平的，旋转时桨面上下的空气流速不一直，会产生向上的推力。

数学模型

对于四旋翼无人机刚开始的输入可以简化成四个电机的油门，每个电机的油门归一化到0-1区间。

四旋翼动力学数学模型就是： 根据电机的油门算出和升力和各轴的力矩。

这里有三个模型近似：

1. 对于每个电机，电机稳态转速与油门成线性关系（0%油门产生0%转速，10%油门产生10%转速，100%油门产生100%转速），电机近似一阶系统逐渐达到稳态转速
   实际在0油门也会有点转速，有一点截距
   
   电机近似一阶系统逐渐达到稳态转速，不会是阶跃那种（下面曲线把一阶加速过程放大了，实际不会这么慢达到稳态转速）
   
   其中电机转速和油门的计算公式如下：
   
   给电机的油门越大，电机转速越快，油门与电机稳态转速之间接近线性关系。但是给电机一个油门之后，电机并不能立即达到对应的转速，可以把电机近似为一阶系统。
   其中$C_m$为电机转速斜率，定义为油门增加1，电机转速增加量；${\bar{\omega }}_m$为电机转速截距，定义为油门为0时，电机的转速，则公式右边为电机的稳定转速，与油门线性相关
   公式左边为动态过程，随着电机转速$\bar{\omega }(t)$的增加，电机转速增量$\dot{\bar{\omega }}(t)$逐渐减小，电机转速逐渐达到给定转速。$T_m$为电机时间常数，越大则每次转速的增量越小，达到稳定转速的时间越长。

2. 对于每个螺旋桨，产生的升力与转速平方成正比
   大概曲线是这样：
   
   计算公式如下
   
   $C_T$就是升力系数，T就是无人机的合升力

3. 四个螺旋桨的合力共同作用在机体系z轴；四个螺旋桨力的差异在机体系三个轴产生力矩。
   其中x和y轴靠升力的不平衡来产生力矩；z轴力矩的产生靠反扭矩。
   力矩（Torque）是一个物理量，它描述了力对物体产生旋转效果的能力。力矩的公式为：$\tau =r\times F$，
   $\tau$是力矩（单位通常是牛顿米，N·m）;F 是作用在物体上的力（单位通常是牛顿，N）;r 是力臂的矢量（单位通常是米，m）。
   在无人机飞行控制中，力矩是一个非常重要的概念。无人机的姿态控制（如俯仰、滚转、偏航）都是通过调整电机产生的力矩来实现的。通过改变电机的转速，可以改变电机产生的力矩，从而控制无人机的姿态和飞行方向。
   在四旋翼中，力、力臂、力矩，用如下视图表示：
   
   其中绿色F1、F2、F3、F4为各电机产生的力，方向为垂直xy平面向上，黄色d为力臂矢量，则力矩Mi的方向通过右手定则可以得到方向，橙色M1、M2、M3、M4则为各电机所产生的力矩。
   以电机1为例，其产生的力矩M1在机体系x,y轴的分量为：
   
   同理可得到四个电机产生的合力矩，在x和y轴为：
   
   在z轴方向上，螺旋桨旋转，空气给螺旋桨一个反方向的阻力，例如逆时针旋转的1号电机，
   

黑色v为螺旋桨线速度方向，绿色f1为等效空气阻力，黄色r为力臂矢量，则通过力矩计算公式得到该力矩橙色M1z为垂直向下，大小为：

同理可得到其它螺旋桨旋转产生的z轴方向的力矩M2z（垂直朝上）、M3z（垂直朝下）、M4z（垂直朝上）
但是等效反扭力矩f1难以得到，通过实验得出，反扭力矩也和螺旋桨的转速平方成正比

其中$C_M$为反扭力矩系数，代表单个螺旋桨转速增加1rad/s，反扭力矩增加的大小
那么可以得到四个螺旋桨产生的反扭力矩为：

数学模型总结

油门和电机转速的计算公式：

转速和升力的计算公式：

转速和力矩的计算公式：

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