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)控制器仍不失为姿态控制的入门和基础算法，对深入理解四旋翼控制具有重要意义。