SO3控制器原理与实现(对飞行器的控制实践)

SO3控制器原理与实现

1. 概述

SO3Control是一个基于SO(3)特殊正交群的姿态控制器，用于控制四旋翼等飞行器的姿态。该控制器输入期望的位置、速度、加速度以及偏航角，输出期望的力和四元数表示的姿态。

具体应用为当有一条三维轨迹的时候，控制飞行器进行轨迹的跟踪，为轨迹优化之后具体的机器人执行器部分。

核心思想是将姿态控制问题转化为位置控制问题，即将期望力方向作为期望的Z轴方向，再结合期望偏航角求解X轴和Y轴方向，从而得到一个完整的旋转矩阵，将其转换为四元数即可。

2. 控制算法推导

2.1 位置控制器

根据牛顿第二定律，可以得到如下位置控制方程：

$$\mathbf{F}=m({\ddot{\mathbf{r}}}_d+{\mathbf{K}}_v({\dot{\mathbf{r}}}_d-\dot{\mathbf{r}})+{\mathbf{K}}_p({\mathbf{r}}_d-\mathbf{r}))+m\mathbf{g}$$

其中，$\mathbf{F}$是控制力，$m$是飞行器质量，${\mathbf{r}}_d,{\dot{\mathbf{r}}}_d,{\ddot{\mathbf{r}}}_d$分别是期望位置、速度、加速度，$\mathbf{r},\dot{\mathbf{r}}$是实际位置和速度，${\mathbf{K}}_p,{\mathbf{K}}_v$是位置和速度的控制增益，$\mathbf{g}$是重力加速度。

可以看出，该控制器是一个PD控制器，通过调节位置和速度误差来产生控制力。

2.2 姿态求解

得到控制力$\mathbf{F}$后，可以求解期望的姿态。设$\mathbf{F}$方向为body系下的$Z_c$轴，$Y_c$轴为$Z_c\times X_d$方向，$X_c$轴为$Y_c\times Z_c$方向，其中$X_d$为期望的机头方向（由偏航角决定）。则body系到惯性系的旋转矩阵为：

$$\mathbf{R}=\left[\begin{array}{ccc}{X}_c& Y_c& Z_c\end{array}\right]$$

将旋转矩阵转换为四元数，即可得到期望姿态。限制俯仰角和滚转角不超过45度，是为了保证控制的可行性。

3. 代码实现

3.1 SO3Control类

SO3Control类实现了上述控制算法。重要成员函数如下：

• void calculateControl(...): 核心函数，计算控制力和期望姿态。输入为期望位置、速度、加速度、偏航角、偏航角速度，以及控制增益。
• const Eigen::Vector3d& getComputedForce(): 返回控制力。
• const Eigen::Quaterniond& getComputedOrientation(): 返回期望姿态（四元数）。
• void setMass(const double mass): 设置飞行器质量。
• void setPosition(const Eigen::Vector3d& position): 设置实际位置。
• void setVelocity(const Eigen::Vector3d& velocity): 设置实际速度。
• void setAcc(const Eigen::Vector3d& acc): 设置实际加速度。

4. 控制器输入输出

SO(3)控制器的输入包括：

• 期望位置：${\mathbf{r}}_d\in {\mathbb{R}}^3$
• 期望速度：${\dot{\mathbf{r}}}_d\in {\mathbb{R}}^3$
• 期望加速度：${\ddot{\mathbf{r}}}_d\in {\mathbb{R}}^3$
• 期望偏航角：${\psi }_d\in \mathbb{R}$
• 期望偏航角速度：${\dot{\psi }}_d\in \mathbb{R}$
• 位置控制增益：${\mathbf{K}}_p\in {\mathbb{R}}^{3\times 3}$
• 速度控制增益：${\mathbf{K}}_v\in {\mathbb{R}}^{3\times 3}$

其中，位置、速度、加速度均在惯性系下表示。${\mathbf{K}}_p$和${\mathbf{K}}_v$通常取正定对角阵。

控制器的输出包括：

• 控制力：$\mathbf{F}\in {\mathbb{R}}^3$
• 期望姿态（四元数）：${\mathbf{q}}_d\in {\mathbb{R}}^4$

其中，控制力在body系下表示，需要转换到惯性系下作用于飞行器。${\mathbf{q}}_d$表示body系到惯性系的旋转。

5. 控制器原理

5.1 位置控制器

根据牛顿第二定律，可以得到如下位置控制方程：

$$m\ddot{\mathbf{r}}=\mathbf{F}-m\mathbf{g}$$

其中，$m$是飞行器质量，$\mathbf{g}$是重力加速度。

定义位置误差${\mathbf{e}}_p$和速度误差${\mathbf{e}}_v$为：

$$\begin{array}{rl}{\mathbf{e}}_p& ={\mathbf{r}}_d-\mathbf{r}\\ {\mathbf{e}}_v& ={\dot{\mathbf{r}}}_d-\dot{\mathbf{r}}\end{array}$$

引入PD控制律，可以得到控制力$\mathbf{F}$为：

$$\mathbf{F}=m({\ddot{\mathbf{r}}}_d+{\mathbf{K}}_v{\mathbf{e}}_v+{\mathbf{K}}_p{\mathbf{e}}_p)+m\mathbf{g}$$

可以看出，该控制器通过位置误差和速度误差的线性组合来产生控制力，以驱使飞行器跟踪期望轨迹。当误差为零时，控制力恰好抵消重力。

5.2 姿态求解

得到控制力$\mathbf{F}$后，需要求解相应的期望姿态${\mathbf{q}}_d$。这里使用如下的构建方法：

1. 令${\mathbf{z}}_c=\frac{\mathbf{F}}{\Vert \mathbf{F}\Vert }$，即令body系的$Z_c$轴与控制力同向。这样可以保证合力方向正确。

2. 令${\mathbf{x}}_d=[\cos {\psi }_d,\sin {\psi }_d,0{]}^T$，即令期望的body系$X_d$轴在XY平面内，且与$X$轴夹角为${\psi }_d$。这样可以控制偏航角。

3. 令${\mathbf{y}}_c=\frac{{\mathbf{z}}_c\times {\mathbf{x}}_d}{\Vert {\mathbf{z}}_c\times {\mathbf{x}}_d\Vert }$，即令body系$Y_c$轴与$Z_c$轴和$X_d$轴垂直。

4. 令${\mathbf{x}}_c={\mathbf{y}}_c\times {\mathbf{z}}_c$，即令body系$X_c$轴与$Y_c$轴和$Z_c$轴垂直。

这样，我们就得到了期望的body系坐标轴$({\mathbf{x}}_c,{\mathbf{y}}_c,{\mathbf{z}}_c)$，可以构建如下旋转矩阵$\mathbf{R}$：

$$\mathbf{R}=\left[\begin{array}{ccc}{\mathbf{x}}_c& {\mathbf{y}}_c& {\mathbf{z}}_c\end{array}\right]$$

再将$\mathbf{R}$转换为四元数${\mathbf{q}}_d$，即得到了期望姿态。

需要注意的是，由于${\mathbf{x}}_d$和${\mathbf{z}}_c$可能共线，此时${\mathbf{y}}_c$不能按上述方法求解。一种解决方案是在第3步中，将${\mathbf{y}}_c$初始化为$[0,0,1{]}^T$与${\mathbf{z}}_c$叉乘，再单位化。

此外，为了保证飞行器姿态在安全范围内，通常需要限制控制力与重力方向的夹角不超过某一阈值（如45°）。若超过阈值，则需要对控制力方向进行限幅。

4. 代码实现

以下是SO(3)控制器的C++实现，基于Eigen库：

void SO3Control::calculateControl(const Eigen::Vector3d& des_pos,
                                  const Eigen::Vector3d& des_vel,
                                  const Eigen::Vector3d& des_acc,
                                  const double des_yaw, 
                                  const double des_yaw_dot,
                                  const Eigen::Vector3d& kx,
                                  const Eigen::Vector3d& kv) {
  // 位置误差
  Eigen::Vector3d e_p = des_pos - pos_;
  // 速度误差  
  Eigen::Vector3d e_v = des_vel - vel_;
  // 加速度误差
  Eigen::Vector3d e_a = des_acc - acc_; 

  // 计算控制力
  force_ = mass_ * (des_acc + kv.asDiagonal() * e_v + kx.asDiagonal() * e_p) + mass_ * g_ * Eigen::Vector3d(0, 0, 1);

  // 限制控制力方向与重力方向夹角不超过45°
  double theta = M_PI / 4;
  double c = cos(theta);
  Eigen::Vector3d f = force_ - mass_ * g_ * Eigen::Vector3d(0, 0, 1);
  if (f.dot(Eigen::Vector3d(0, 0, 1)) < c * f.norm()) {
    double nf = f.norm();
    double A = c * c * nf * nf - f(2) * f(2);
    double B = 2 * (c * c - 1) * f(2) * mass_ * g_;
    double C = (c * c - 1) * mass_ * mass_ * g_ * g_;
    double s = (-B + sqrt(B * B - 4 * A * C)) / (2 * A);
    force_ = s * f + mass_ * g_ * Eigen::Vector3d(0, 0, 1);
  }

  // 计算期望姿态
  Eigen::Vector3d b1_d(cos(des_yaw), sin(des_yaw), 0);
  Eigen::Vector3d b3_c = force_.normalized();
  Eigen::Vector3d b2_c = b3_c.cross(b1_d).normalized();
  Eigen::Vector3d b1_c = b2_c.cross(b3_c);

  Eigen::Matrix3d R;
  R << b1_c, b2_c, b3_c;
  orientation_ = Eigen::Quaterniond(R); 
}

其中，pos_、vel_、acc_是当前的位置、速度和加速度，mass_是飞行器质量，g_是重力加速度。force_和orientation_是计算得到的控制力和期望姿态。

这部分代码是包括对控制力方向进行限幅。当控制力方向与重力方向夹角超过设定阈值（这里取45°）时，需要调整控制力方向，使其在限制范围内。

具体来说，设原始控制力为$\mathbf{f}$，限幅后的控制力为${\mathbf{f}}^{\prime }$，重力加速度为$\mathbf{g}$，限制角度为$\theta$。我们希望找到一个系数$s$，使得：

$${\mathbf{f}}^{\prime }=s\mathbf{f}+m\mathbf{g}$$

且${\mathbf{f}}^{\prime }$与$\mathbf{g}$的夹角恰好等于$\theta$。

设$\mathbf{f}=(f_x,f_y,f_z)$，$\mathbf{g}=(0,0,-g)$，则有：

$$\cos \theta =\frac{{\mathbf{f}}^{\prime }\cdot \mathbf{g}}{\Vert {\mathbf{f}}^{\prime }\Vert \Vert \mathbf{g}\Vert }=\frac{s{f}_z-mg}{\sqrt{s^2\Vert \mathbf{f}{\Vert }^2+m^2{g}^2-2smg{f}_z}}$$

化简可得：

$$A{s}^2+Bs+C=0$$

其中，

$$\begin{array}{rl}A& ={\cos }^2\theta \Vert \mathbf{f}{\Vert }^2-f_z^2\\ B& =2({\cos }^2\theta -1)f_{z}mg\\ C& =({\cos }^2\theta -1)m^2{g}^2\end{array}$$

解此二次方程，取较大的根，即为所求的$s$。

将$s$代入${\mathbf{f}}^{\prime }=s\mathbf{f}+m\mathbf{g}$，即可得到限幅后的控制力。

这部分推导的目的是找到一个系数$s$，使得限幅后的控制力${\mathbf{f}}^{\prime }$与重力加速度$\mathbf{g}$的夹角恰好等于预设的限制角度$\theta$。这样可以保证控制力方向不会偏离重力方向过多，从而避免飞行器出现过大的俯仰或滚转角度。

推导过程如下：

1. 假设限幅后的控制力${\mathbf{f}}^{\prime }$可以表示为原始控制力$\mathbf{f}$与重力$m\mathbf{g}$的线性组合：

   $${\mathbf{f}}^{\prime }=s\mathbf{f}+m\mathbf{g}$$

   其中，$s$是一个待求的系数。

2. 根据向量夹角公式，${\mathbf{f}}^{\prime }$与$\mathbf{g}$的夹角$\theta$满足：

   $$\cos \theta =\frac{{\mathbf{f}}^{\prime }\cdot \mathbf{g}}{\Vert {\mathbf{f}}^{\prime }\Vert \Vert \mathbf{g}\Vert }$$

3. 将${\mathbf{f}}^{\prime }=s\mathbf{f}+m\mathbf{g}$代入上式，得：

   $$\cos \theta =\frac{(s\mathbf{f}+m\mathbf{g})\cdot \mathbf{g}}{\Vert s\mathbf{f}+m\mathbf{g}\Vert \Vert \mathbf{g}\Vert }$$

4. 设$\mathbf{f}=(f_x,f_y,f_z)$，$\mathbf{g}=(0,0,-g)$，代入上式，化简得：

   $$\cos \theta =\frac{s{f}_z-mg}{\sqrt{s^2\Vert \mathbf{f}{\Vert }^2+m^2{g}^2-2smg{f}_z}}$$

5. 为了求解$s$，我们需要消除根号。先将分母移到左边，然后两边平方，得：

   $${\cos }^2\theta (s^2\Vert \mathbf{f}{\Vert }^2+m^2{g}^2-2smg{f}_z)=(s{f}_z-mg{)}^2$$

6. 展开并合并同类项，得到一个关于$s$的二次方程：

   $$({\cos }^2\theta \Vert \mathbf{f}{\Vert }^2-f_z^2)s^2+2({\cos }^2\theta -1)f_{z}mgs+({\cos }^2\theta -1)m^2{g}^2=0$$

7. 令$A={\cos }^2\theta \Vert \mathbf{f}{\Vert }^2-f_z^2$，$B=2({\cos }^2\theta -1)f_{z}mg$，$C=({\cos }^2\theta -1)m^2{g}^2$，则上式可以简写为：

   $$A{s}^2+Bs+C=0$$

8. 求解这个二次方程，取较大的根作为$s$的值。

9. 将求得的$s$代回${\mathbf{f}}^{\prime }=s\mathbf{f}+m\mathbf{g}$，即可得到限幅后的控制力${\mathbf{f}}^{\prime }$。

这样，我们就通过调节系数$s$的大小，使得限幅后的控制力方向与重力方向的夹角恰好等于预设值$\theta$，从而实现了控制力方向的限幅。

这种限幅方法的优点是计算简单，且能保证控制力大小和方向的连续性，避免了硬限幅可能引起的控制抖动。但它也有一定局限性，如当原始控制力与重力方向夹角过大时，限幅后的控制力大小可能不足，影响控制性能。因此，在实际应用中，还需要根据具体需求，权衡限幅角度的选取。

具体的控制过程

控制器计算出期望控制力和期望姿态后，需要将其传递给飞行控制系统，转化为具体的动力输出。这一过程通常分为两步：

1. 动力分配：根据期望控制力，计算出每个旋翼的推力。常用的方法有：

   • 矩阵分配法：假设控制力与各旋翼推力之间是线性关系，可以建立一个分配矩阵，将控制力映射到推力。
   • 优化分配法：设计一个优化问题，以控制力为约束，以均衡负载、能耗最小等为目标，求解出最优推力。

2. 姿态控制：根据期望姿态和当前姿态，计算出姿态误差，再通过PID控制器，得到角速度控制量。再利用动力学模型，将角速度控制量转化为各旋翼的差动力矩。

一个具体的案例是PX4飞控中的多旋翼姿态控制器，其基本流程如下：

1. 位置控制器根据位置、速度误差，计算出期望加速度。

2. 将期望加速度转化为期望控制力，并根据期望偏航角，计算出期望姿态（四元数）。

3. 将期望控制力传递给动力分配模块，计算出各旋翼的参考推力。

4. 将期望姿态传递给姿态控制器，与当前姿态比较，得到姿态误差。

5. 姿态控制器通过PID控制，将姿态误差转化为角速度控制量。

6. 角速度控制量与推力一起，通过动力学模型，计算出各旋翼的实际转速。

7. 将转速指令输出到电调，驱动电机，完成飞行控制。

综上，SO(3)控制器通过将位置控制与姿态控制解耦，大大简化了控制器设计。期望姿态作为中间量，承接了位置控制器的输出，也为下层姿态控制提供了参考输入，是飞行控制系统的重要组成部分。

下面是PX4中多旋翼姿态控制器的部分代码,位于文件 src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp 中。

/**
 * Control attitude rates ( e.g. angular velocity)
 * @param rates_sp desired angular velocity  
 * @param thrust_body thrust_body 
 * @param dt sampling time
 * @param reset_integral if true reset integral terms
 * @return [rad/s] body frame 3D angular rate setpoint 
 */
Vector3f
AttitudeControl::update(const Vector3f &rates_sp, const float thrust_body, const float dt, const bool reset_integral)
{
    Vector3f rates_error = rates_sp - _rates;

    /* 
        计算角速度误差积分项
        如果重置积分项或者换向,则不累加
        如果饱和,停止积分
     */
    if (!reset_integral && !_vehicle_status.in_transition_mode) {
        _rates_int += rates_error * _rate_i.emult(Vector3f(_wheighting_matrix_inv(0,0),_wheighting_matrix_inv(1,1),_wheighting_matrix_inv(2,2))) * dt;

        // 积分项限幅
        for (int i = 0; i < 3; i++) {
            if ((_rate_i(i) > FLT_EPSILON || _rate_i(i) < -FLT_EPSILON) &&
                    (fabsf(_rates_int(i)) > _rate_int_lim(i))) {
                _rates_int(i) = math::sign(_rates_int(i)) * _rate_int_lim(i);
            }
        }
    }

    // 角速度PID控制,计算扭矩
    Vector3f torque = _rate_p.emult(rates_error) + _rate_d.emult(_rates_prev - _rates) / dt + _rates_int;

    // 扭矩限幅
    torque = math::constrain(torque, -_lim_rates, _lim_rates);

    // 扭矩归一化
    Vector3f torque_norm = torque.normalized();
    
    /* 扭矩按推力缩放
      归一化扭矩乘以按油门缩放后的扭矩幅值
      该缩放使得扭矩随着油门的减小而减小,当油门低于悬停油门 8% 时,扭矩为零
        (这可以改善降落时的扰动) */
    torque = Vector3f(torque_norm(0), torque_norm(1), torque_norm(2)) * math::min(thrust_body, _hover_thrust) / _hover_thrust;

    _rates_prev = _rates;

    return torque;
}

这段代码实现了角速度的PID控制,主要步骤如下:

1. 计算角速度误差 rates_error,即期望角速度与实际角速度之差。

2. 计算角速度误差积分项 _rates_int,并进行限幅。注意,如果重置积分项、处于模式切换状态或者积分饱和,则不累加积分。

3. 根据角速度误差、误差积分项以及误差微分项(通过后向差分近似),计算控制扭矩 torque。其中,_rate_p、_rate_i、_rate_d 分别为 PID 控制器的三个增益。

4. 对控制扭矩进行限幅,确保其不超过预设的限制 _lim_rates。

5. 对控制扭矩进行归一化,得到扭矩方向 torque_norm。

6. 将扭矩方向乘以按油门缩放后的扭矩幅值,得到最终的控制扭矩输出。其中,_hover_thrust 为悬停油门,用于扭矩缩放。

7. 更新上一时刻的角速度 _rates_prev,用于下一步的微分项计算。

8. 返回控制扭矩。

该控制器的输入为期望角速度 rates_sp 和当前推力 thrust_body,输出为三轴控制扭矩。控制器通过 PID 算法,不断调整扭矩输出,使得飞行器的实际角速度跟踪期望角速度。同时,它还引入了一些特殊处理,如积分项限幅、扭矩缩放等,以提高控制性能和鲁棒性。

这只是 PX4 姿态控制器的一部分,完整的控制流程还包括姿态误差计算、控制分配等步骤。但这段代码展示了 PX4 中姿态控制器的核心思路,即基于角速度的 PID 反馈控制,并辅以一系列的条件处理和限幅措施,以适应多旋翼飞行控制的需求。

5. 总结

SO(3)控制器通过位置-速度-加速度的PD反馈，计算出合适的控制力，再根据控制力和期望偏航角求解出期望姿态，实现了四旋翼的姿态控制。

该控制器优点是原理简单，计算高效，适合实时控制。但在实际应用中，还需要考虑更多因素，如外部扰动、执行器动力学、状态估计等，才能获得理想的控制性能。

尽管如此，SO(3)控制器仍不失为姿态控制的入门和基础算法，对深入理解四旋翼控制具有重要意义。