四旋翼飞行器数学模型详解及代码实现

最新推荐文章于 2025-03-10 12:00:00 发布

jiayoushijie-泽宣

最新推荐文章于 2025-03-10 12:00:00 发布

阅读量2.6k

点赞数 21

文章标签：人工智能机器人算法自动驾驶

本文链接：https://blog.csdn.net/jiayoushijie/article/details/139380565

版权

四旋翼飞行器数学模型详解及代码实现

1. 概述

本文将介绍了四旋翼飞行器的仿真系统（四旋翼无人机的数学模型）及其SO(3)控制器的实现。该系统以模拟四旋翼的飞行动力学,并实现其姿态控制。

系统主要包含两个模块:

模型: 四旋翼动力学模型,负责仿真四旋翼的飞行状态。
控制器: SO(3)控制器,负责计算四旋翼的控制输入,实现姿态控制，控制器可以参考上一篇我写的博客。

下面将分别介绍这两个模块的原理和实现。

2. 四旋翼动力学模型

四旋翼是一种下垂式飞行器,通过控制4个旋翼的转速差异,可以产生俯仰、滚转、偏航三个方向的力矩,从而控制其姿态。同时,通过控制4个旋翼的总推力,可以控制其垂直升降运动。

2.1 运动学模型

设四旋翼在惯性系下的位置为 $\mathbf{p} = [x, y, z]^T$ ,速度为 $\mathbf{v} = [v_x, v_y, v_z]^T$ ,姿态为 $\mathbf{R} \in SO(3)$ ,角速度为 $\mathbf{\omega} = [\omega_x, \omega_y, \omega_z]^T$ 。则其运动学方程为（细节推导可以看我的四元数动力学系列课程）:

$\begin{aligned} \dot{\mathbf{p}} &= \mathbf{v} \\ \dot{\mathbf{R}} &= \mathbf{R}\hat{\mathbf{\omega}} \end{aligned}$

其中, $\hat{\mathbf{\omega}}$ 是 $\mathbf{\omega}$ 的反对称矩阵:

$\hat{\mathbf{\omega}} = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix}$

2.2 动力学模型

根据牛顿-欧拉方程,四旋翼的动力学方程为:

$\begin{aligned} m\dot{\mathbf{v}} &= \mathbf{F} - mg\mathbf{e}_3 \\ \mathbf{J}\dot{\mathbf{\omega}} &= \mathbf{M} - \mathbf{\omega} \times \mathbf{J}\mathbf{\omega} \end{aligned}$

其中, $m$ 是四旋翼质量, $\mathbf{J}$ 是其转动惯量矩阵, $\mathbf{F}$ 和 $\mathbf{M}$ 分别是作用在质心的合力和力矩, $\mathbf{e}_3 = [0, 0, 1]^T$ 。

设第 $i$ 个旋翼的转速为 $\omega_i$ ,则其产生的推力 $f_i$ 和反扭矩 $\tau_i$ 为:

$\begin{aligned} f_i &= k_f\omega_i^2 \\ \tau_i &= k_m\omega_i^2 \end{aligned}$

其中, $k_f$ 和 $k_m$ 分别为推力系数和反扭矩系数,与旋翼的几何参数有关。

设四旋翼的臂长(转轴到质心的距离)为 $l$ ,则合力 $\mathbf{F}$ 和力矩 $\mathbf{M}$ 可表示为:

$\mathbf{F} = \begin{bmatrix} 0 \\ 0 \\ \sum_{i=1}^4 f_i \end{bmatrix}, \quad \mathbf{M} = \begin{bmatrix} l(f_2 - f_4) \\ l(f_3 - f_1) \\ \sum_{i=1}^4 (-1)^{i+1}\tau_i \end{bmatrix}$

此外,还需考虑旋翼动力学,即转速变化的响应过程。通常可用一阶惯性环节来描述:

$\dot{\omega}_i = \frac{1}{k_{\omega}}(u_i - \omega_i)$

其中, $u_i$ 是第 $i$ 个旋翼的控制输入(通常是电机电压或PWM信号), $k_{\omega}$ 是时间常数。

3. SO(3)控制器

SO(3)控制器是一种基于李代数的非线性控制器,可以实现四旋翼的全局指数稳定的姿态跟踪控制。其基本思想是:将四旋翼的姿态误差转化为SO(3)群上的误差,然后设计李代数反馈律,使误差在SO(3)群上指数收敛到零。

3.1 控制律设计

设四旋翼的期望姿态为 $\mathbf{R}_d \in SO(3)$ ,期望角速度为 $\mathbf{\omega}_d$ ,则姿态误差可定义为:

$\mathbf{e}_R = \frac{1}{2}(\mathbf{R}_d^T\mathbf{R} - \mathbf{R}^T\mathbf{R}_d)^{\vee}$

其中, $(\cdot)^{\vee}$ 表示反对称矩阵的逆运算,即取上三角元素组成向量。

类似地,角速度误差可定义为:

$\mathbf{e}_{\omega} = \mathbf{\omega} - \mathbf{R}^T\mathbf{R}_d\mathbf{\omega}_d$

根据李代数理论,可设计如下控制律:

$\mathbf{M} = -k_R\mathbf{e}_R - k_{\omega}\mathbf{e}_{\omega} + \mathbf{\omega} \times \mathbf{J}\mathbf{\omega}$

其中, $k_R$ 和 $k_{\omega}$ 为正的控制增益。可证明,在该控制律作用下,姿态误差 $\mathbf{e}_R$ 和角速度误差 $\mathbf{e}_{\omega}$ 都能全局指数收敛到零。

此外,还需要考虑推力 $f$ 的设计。通常根据期望的z轴加速度 $a_{des}$ 来设计:

$a_{des})/(\mathbf{R}_d\mathbf{e}_3) \cdot (\mathbf{R}\mathbf{e}_3)$

这样可以保证推力方向与期望z轴一致。

3.2 控制器实现

so3_control中的主要函数是static Control getControl(const Quadrotor& quad, const Command& cmd),其输入是当前四旋翼状态quad和控制指令cmd,输出是4个旋翼的转速control。

该函数的主要步骤如下:

计算参数,如惯量矩阵I,当前姿态R和角速度omega等。
根据指令cmd中的四元数计算期望姿态Rd。
计算推力force和姿态误差eR,角速度误差eOm。
计算力矩M(公式见3.1节)。
根据推力force和力矩M,计算4个旋翼的转速平方w_sq。这需要求解一个4元一次方程组。
开方得到转速control。注意要进行合法性检查,如不小于0等。

4 代码示例如下：

Quadrotor.cpp该文件实现了四旋翼动力学模型的仿真。主要包括以下几个部分:

4.1 类成员变量

class Quadrotor {
public:
  struct State {
    Eigen::Vector3d x; // 位置
    Eigen::Vector3d v; // 速度
    Eigen::Matrix3d R; // 姿态(旋转矩阵)
    Eigen::Vector3d omega; // 角速度
    Eigen::Array4d motor_rpm; // 电机转速
  };

  // 构造函数
  Quadrotor();

  // 积分一个时间步长
  void step(double dt); 

  // 常微分方程右端项  
  void operator()(const State& x, State& dx, const double t);

  // 设置控制输入(电机转速)  
  void setInput(double u1, double u2, double u3, double u4);

  // 获取状态
  const State& getState() const; 

  // 其他成员函数,设置/获取各种参数

private:
  State state_; // 状态
  Eigen::Array4d input_; // 控制输入
  double mass_; // 质量
  Eigen::Matrix3d J_; // 转动惯量
  double kf_, km_; // 电机参数
  double arm_length_; // 臂长
  double motor_time_constant_; // 电机时间常数
  double min_rpm_, max_rpm_; // 电机转速范围
  // ...
};

这里定义了Quadrotor类,表示一个四旋翼。其中State结构体表示四旋翼的状态,包括位置、速度、姿态(旋转矩阵)、角速度和电机转速。

类的主要成员变量包括状态state_,控制输入input_(4个电机的转速),以及一些物理参数如质量mass_、转动惯量J_、电机参数kf_和km_、臂长arm_length_等。

4.2 动力学模型

四旋翼的动力学模型可以用以下常微分方程组来描述:

$\begin{aligned} \dot{\mathbf{p}} &= \mathbf{v} \\ m\dot{\mathbf{v}} &= \mathbf{F} - mg\mathbf{e}_3 \\ \dot{\mathbf{R}} &= \mathbf{R}\hat{\mathbf{\omega}} \\ \mathbf{J}\dot{\mathbf{\omega}} &= \mathbf{M} - \mathbf{\omega} \times \mathbf{J}\mathbf{\omega} \\ \dot{\omega}_i &= \frac{1}{k_{\omega}}(u_i - \omega_i) \end{aligned}$

其中, $\mathbf{p},\mathbf{v},\mathbf{R},\mathbf{\omega}$ 分别为位置、速度、姿态、角速度, $\omega_i$ 为第 $i$ 个电机的转速, $\mathbf{F}$ 和 $\mathbf{M}$ 为合力和合力矩:

$\mathbf{F} = \begin{bmatrix} 0 \\ 0 \\ \sum_{i=1}^4 k_f\omega_i^2 \end{bmatrix}, \quad \mathbf{M} = \begin{bmatrix} l(f_2 - f_4) \\ l(f_3 - f_1) \\ \sum_{i=1}^4 (-1)^{i+1}k_m\omega_i^2 \end{bmatrix}$

下面我将从基本原理出发,详细推导四旋翼的动力学模型,并解释相关的数学符号和对应的代码实现。

一个四旋翼系统通常可以看作一个刚体,受到重力、推力、气动力等外力作用。为了描述其运动,我们需要建立合适的坐标系和数学模型。

4.3. 坐标系定义

首先定义两个坐标系:

惯性坐标系(Inertial Frame):{I}。通常选择地面上的一个固定点作为原点,x轴指向前,y轴指向左,z轴指向上。
机体坐标系(Body Frame):{B}。原点在四旋翼质心,x轴指向前,y轴指向左,z轴指向上。

两个坐标系的关系可以用一个旋转矩阵R来描述,即:

P_I = R * P_B

其中,P_I和P_B分别是惯性系和机体系下的同一个点的坐标。

4.4 运动学模型

设四旋翼在惯性系下的位置为p,速度为v,姿态(即机体系相对于惯性系的旋转)用四元数q或旋转矩阵R表示,角速度为ω。则其运动学方程为:

dp/dt = v
dq/dt = 0.5 * q * [0; ω]

或者用旋转矩阵形式:

dR/dt = R * ω^

其中,ω^表示角速度的反对称矩阵:

ω^ = [  0   -ω_z  ω_y
        ω_z   0   -ω_x
       -ω_y  ω_x   0  ]

对应的C++代码为:

Eigen::Vector3d p;  // 位置
Eigen::Vector3d v;  // 速度
Eigen::Matrix3d R;  // 旋转矩阵  
Eigen::Vector3d ω;  // 角速度

// 运动学方程
Eigen::Vector3d p_dot = v;
Eigen::Matrix3d ω_hat;
ω_hat <<     0,   -ω(2),    ω(1),
            ω(2),    0,    -ω(0),
           -ω(1),  ω(0),     0;  
Eigen::Matrix3d R_dot = R * ω_hat;

4.5 动力学模型

根据牛顿第二定律,四旋翼的平移动力学方程为:

m * dv/dt = F_thrust + F_drag + F_gravity

其中,m为四旋翼质量,F_thrust为推力,F_drag为空气阻力,F_gravity为重力。

假设空气阻力与速度成正比,即F_drag = -k_d * v,重力在惯性系z轴方向上,即F_gravity = -m * g * e_3,其中e_3 = [0; 0; 1],则有:

m * dv/dt = R * [0; 0; F_thrust_B] - k_d * v - m * g * e_3

其中,F_thrust_B是机体系下的推力。

对应的C++代码为:

double m;               // 质量
double g = 9.8;         // 重力加速度
double k_d;             // 阻力系数
Eigen::Vector3d F_thrust_B; // 机体系推力

// 动力学方程  
Eigen::Vector3d v_dot = 1/m * (R*F_thrust_B - k_d*v - m*g*Eigen::Vector3d(0,0,1));

根据欧拉方程,四旋翼的转动动力学方程为:

J * dω/dt = τ - ω × (J * ω)

其中,J为四旋翼绕质心的转动惯量矩阵,τ为合外力矩。

假设转动惯量矩阵为对角阵,即J = diag(J_x, J_y, J_z),则有:

J_x * dω_x/dt = τ_x - (J_z - J_y) * ω_y * ω_z
J_y * dω_y/dt = τ_y - (J_x - J_z) * ω_z * ω_x  
J_z * dω_z/dt = τ_z - (J_y - J_x) * ω_x * ω_y

对应的C++代码为:

Eigen::Matrix3d J;  // 转动惯量矩阵
Eigen::Vector3d τ;  // 合外力矩

// 动力学方程
Eigen::Vector3d ω_dot;
ω_dot(0) = (τ(0) - (J(2,2)-J(1,1))*ω(1)*ω(2)) / J(0,0); 
ω_dot(1) = (τ(1) - (J(0,0)-J(2,2))*ω(2)*ω(0)) / J(1,1);
ω_dot(2) = (τ(2) - (J(1,1)-J(0,0))*ω(0)*ω(1)) / J(2,2);

4.6 推力和力矩模型

设第i个旋翼的转速为ω_i,则其产生的推力f_i和反扭矩τ_i分别为:

f_i = k_f * ω_i^2
τ_i = k_m * ω_i^2

其中,k_f和k_m分别为推力系数和反扭矩系数,由旋翼的几何形状和空气动力学特性决定。

假设四个旋翼分别位于机体系x轴和y轴的正负方向上,且旋翼1、3逆时针转,旋翼2、4顺时针转。设旋翼距质心的距离为l,则总推力F_thrust_B和合外力矩τ_B在机体系下的分量为:

F_thrust_B = [0; 0; k_f * (ω_1^2 + ω_2^2 + ω_3^2 + ω_4^2)]
τ_B = [ l*k_f * (ω_2^2 - ω_4^2);
        l*k_f * (ω_3^2 - ω_1^2);
        k_m  * (ω_1^2 - ω_2^2 + ω_3^2 - ω_4^2)]

对应的C++代码为:

double k_f;  // 推力系数 
double k_m;  // 反扭矩系数
double l;    // 旋翼距质心距离
Eigen::Array<double,4,1> ω;  // 旋翼转速

Eigen::Vector3d F_thrust_B;
F_thrust_B << 0, 0, k_f * ω.square().sum(); 

Eigen::Vector3d τ_B;  
τ_B << l * k_f * (ω(1)*ω(1) - ω(3)*ω(3)), 
       l * k_f * (ω(2)*ω(2) - ω(0)*ω(0)),
       k_m * (ω(0)*ω(0) - ω(1)*ω(1) + ω(2)*ω(2) - ω(3)*ω(3));

4.7 电机动力学

实际中,电机转速不能瞬间达到指令值,而是存在一个响应过程。通常可以用一阶惯性环节来描述:

dω_i/dt = (ω_i_cmd - ω_i) / τ_m

其中,ω_i_cmd是第i个电机的转速指令,τ_m是电机的时间常数。

对应的C++代码为:

double τ_m;  // 电机时间常数
Eigen::Array<double,4,1> ω_cmd;  // 转速指令  

// 电机动力学方程
Eigen::Array<double,4,1> ω_dot = (ω_cmd - ω) / τ_m;

4.8 完整模型

综上所述,四旋翼的完整动力学模型可以用以下微分方程组表示:

dp/dt = v
dv/dt = g*e_3 + R*F_thrust_B/m - k_d/m*v
dR/dt = R*ω^ 
dω/dt = J^(-1) * (τ_B - ω×(J*ω))
dω_i/dt = (ω_i_cmd - ω_i) / τ_m,  i=1,2,3,4

其中,状态变量为[p; v; R; ω; ω_1; ω_2; ω_3; ω_4],输入变量为[ω_1_cmd; ω_2_cmd; ω_3_cmd; ω_4_cmd],系统参数为[m; g; k_d; k_f; k_m; l; J; τ_m]。

对应的C++代码可以参考前文给出的片段,这里不再赘述。需要注意的是,在实现过程中,我们通常使用四元数而不是旋转矩阵来表示姿态,因为四元数更加紧凑和高效。此外,还需要加入一些额外的代码来处理数值积分、坐标系转换等问题。

这个模型在Quadrotor::operator()函数中实现:

void Quadrotor::operator()(const State& x, State& dx, const double t) {
  // 提取状态变量
  Eigen::Vector3d pos = x.x;
  Eigen::Vector3d vel = x.v;  
  Eigen::Matrix3d R = x.R;
  Eigen::Vector3d omega = x.omega;
  Eigen::Array4d motor_rpm = x.motor_rpm;

  // 计算转速平方
  Eigen::Array4d motor_rpm_sq = motor_rpm * motor_rpm;

  // 计算合力
  double thrust = kf_ * motor_rpm_sq.sum();
  Eigen::Vector3d force(0, 0, thrust);

  // 计算合力矩  
  Eigen::Vector3d moments;
  moments << kf_ * (motor_rpm_sq[1] - motor_rpm_sq[3]) * arm_length_,
             kf_ * (motor_rpm_sq[2] - motor_rpm_sq[0]) * arm_length_,
             km_ * (motor_rpm_sq[0] - motor_rpm_sq[1] 
                    + motor_rpm_sq[2] - motor_rpm_sq[3]);

  // 计算姿态变化率(so(3))
  Eigen::Matrix3d omega_hat; 
  omega_hat <<     0, -omega.z(),  omega.y(),
               omega.z(),      0, -omega.x(),
              -omega.y(), omega.x(),      0;
  Eigen::Matrix3d R_dot = R * omega_hat;

  // 计算角加速度
  Eigen::Vector3d omega_dot = J_.inverse() * (moments - omega.cross(J_ * omega));

  // 计算电机转速变化率
  Eigen::Array4d motor_rpm_dot = (input_ - motor_rpm) / motor_time_constant_;

  // 填充dx
  dx.x = vel;
  dx.v = force / mass_ - Eigen::Vector3d(0, 0, 9.81);
  dx.R = R_dot;
  dx.omega = omega_dot;
  dx.motor_rpm = motor_rpm_dot;  
}

这个函数计算状态变量的导数dx,即常微分方程组的右端项。具体步骤如下:

从状态x中提取各个变量。
计算电机转速平方,用于后续计算推力和力矩。
计算合力force,即z轴方向的总推力。
计算合力矩moments,包括x轴和y轴方向的差动力矩(由于电机布局),以及z轴方向的反扭矩。
计算旋转矩阵的变化率R_dot,即so(3)上的运动学方程。
根据欧拉方程计算角加速度omega_dot。
计算电机转速的变化率motor_rpm_dot,视为一阶惯性环节。
将以上变化率填充到dx中。

4.8 数值积分

Quadrotor::step()函数使用Boost.Odeint库对常微分方程组进行数值积分,更新四旋翼的状态:

void Quadrotor::step(double dt) {
  // 积分一个时间步长
  odeint::integrate(*this, state_, 0.0, dt, dt); 

  // 对旋转矩阵进行正交化
  Eigen::LLT<Eigen::Matrix3d> llt(state_.R.transpose() * state_.R);
  Eigen::Matrix3d R_ortho = state_.R * llt.matrixL().inverse();
  state_.R = R_ortho;

  // 限制高度不小于0
  if (state_.x.z() < 0.0) {
    state_.x.z() = 0.0;
    if (state_.v.z() < 0.0) {
      state_.v.z() = 0.0;
    }
  }  
}

这里使用了odeint::integrate()函数,传入常微分方程*this(通过重载operator()实现),积分时间范围[0, dt]和步长dt。积分结束后,对旋转矩阵进行正交化处理(因为数值误差可能导致其不再正交),并限制高度不小于0(模拟地面效应)。

5. quadrotor_simulator_so3 实现

该文件实现了基于Quadrotor类的四旋翼仿真,以及SO(3)控制器。主要包括以下几个部分:

5.1 控制器实现

SO(3)控制器的实现在getControl()函数中:

static Control getControl(const Quadrotor& quad, const Command& cmd) {
  // 提取状态
  const Quadrotor::State state = quad.getState();
  Eigen::Vector3d pos = state.x;
  Eigen::Vector3d vel = state.v;  
  Eigen::Matrix3d R = state.R;
  Eigen::Vector3d omega = state.omega;

  // 提取指令
  Eigen::Vector3d force_des(cmd.force[0], cmd.force[1], cmd.force[2]);
  Eigen::Quaterniond q_des(cmd.qw, cmd.qx, cmd.qy, cmd.qz);
  double kR[3] = {cmd.kR[0], cmd.kR[1], cmd.kR[2]};
  double kOm[3] = {cmd.kOm[0], cmd.kOm[1], cmd.kOm[2]};

  // 计算姿态误差  
  Eigen::Matrix3d R_des = q_des.toRotationMatrix();
  Eigen::Matrix3d e_R_mat = 0.5 * (R_des.transpose() * R - R.transpose() * R_des);
  Eigen::Vector3d e_R(e_R_mat(2,1), e_R_mat(0,2), e_R_mat(1,0));

  // 计算角速度误差
  Eigen::Vector3d e_Om = omega;

  // 计算力矩
  Eigen::Vector3d M;
  for (int i = 0; i < 3; ++i) {
    M(i) = -kR[i] * e_R(i) - kOm[i] * e_Om(i); 
  }

  // 计算推力
  double force = force_des.dot(R.col(2));

  // 计算电机转速
  double d = quad.getArmLength();
  double c = quad.getPropellerThrustCoefficient();
  Control control;  
  control.rpm[0] = sqrt((force + 2*M(1)/d - M(2)/c) / (4*c));
  control.rpm[1] = sqrt((force - 2*M(1)/d - M(2)/c) / (4*c)); 
  control.rpm[2] = sqrt((force - 2*M(0)/d + M(2)/c) / (4*c));
  control.rpm[3] = sqrt((force + 2*M(0)/d + M(2)/c) / (4*c));

  return control;
}