[飞控]姿态控制-加速度转倾斜角和四元数转轴角函数解析(2018-10-27更新)

APM的姿态控制部分有大量的用于变换的函数，最近分析了两个常用的函数，以前看代码只看框架，觉得不就是姿态吗，不就是欧拉角，四元数，旋转矩阵吗？现在看过算法细节才知道每个算法的实现背后都是严谨的物理假设和数学推导，基础知识太重要，还有太多需要学习。

函数名：AC_PosControl::accel_to_lean_angles

位置：libraries\AC_AttitudeControl\AC_PosControl.cpp
用途：将NED坐标系下的期望加速度转换成机体坐标系下的倾斜角度(pitch,roll)
源程序：

void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float& roll_target, float& pitch_target) const
{
    float accel_right, accel_forward;

    // rotate accelerations into body forward-right frame
    accel_forward = accel_x_cmss*_ahrs.cos_yaw() + accel_y_cmss*_ahrs.sin_yaw();
    accel_right = -accel_x_cmss*_ahrs.sin_yaw() + accel_y_cmss*_ahrs.cos_yaw();

    // update angle targets that will be passed to stabilize controller
    pitch_target = atanf(-accel_forward/(GRAVITY_MSS * 100.0f))*(18000.0f/M_PI);
    float cos_pitch_target = cosf(pitch_target*M_PI/18000.0f);
    roll_target = atanf(accel_right*cos_pitch_target/(GRAVITY_MSS * 100.0f))*(18000.0f/M_PI);
}

数学推导：
假设飞行器z轴受力平衡，即在z轴上保持静止。
已知NED坐标系下期望加速度accel_x_cmss，accel_y_cmss(融合之后的加速度数据为NED坐标系下)和遥控输入的yaw，求机体坐标系下pitch角和roll角。
1.需要先将yaw角进行补偿，即只剩下倾斜角。
将NED坐标系下的水平加速度做绕z旋转，得到没有旋转只有倾斜的中间状态,该状态下的x轴方向加速度为accel_forward，y轴加速度为accel_righ。
NED坐标系转机体坐标系绕z轴旋转矩阵为：

$\left[\begin{matrix} \cos\psi & \sin\psi & 0 \\ -\sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{matrix}\right]$

2.通过accel_forward，accel_righ，计算pitch，roll。
应为飞机在z轴上受力平衡，所以机体坐标系下飞机受到的合外力将在z轴方向上产生一个-G的力才能使飞机在z轴受力平衡。
如图

即支持飞机在NED坐标系z轴保持静止需要-G，而且需要x轴上产生accel_forward，y轴上产生accel_righ，这些都是由机体坐标系下的合外力-F提供的。
所以有：

$\left[\begin{matrix} a_x \\a_y \\-G \end{matrix}\right]= R_B^N \left[\begin{matrix} 0 \\0 \\-F \end{matrix}\right]$

其中$R_B^N$是机体坐标系转到NED坐标系的旋转矩阵(之前已经进行过yaw方向的旋转了所以这个矩阵里的yaw为0)。

$\left[\begin{matrix} a_x \\a_y \\-G \end{matrix}\right]= \left[\begin{matrix} \times & \times & \cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi \\ \times & \times & \sin\psi\sin\theta\cos\phi-\cos\psi\sin\phi \\ \times & \times & \cos\psi\sin\theta \end{matrix} \right] \left[\begin{matrix} 0 \\0 \\-F \end{matrix}\right]$

可得：

$\left[\begin{matrix} accel_{forward} \\accel_{right} \\-G \end{matrix}\right] =\left[\begin{matrix} \times & \times & \sin\theta\cos\phi\\ \times & \times & -\sin\phi \\ \times & \times & \cos\phi\cos\theta \end{matrix}\right] \left[\begin{matrix} 0 \\0 \\-F \end{matrix}\right]$

所以有：

$\begin{matrix} G=\cos\phi\cos\theta*F \\ accel_{forward}=\sin\theta\cos\phi*-F=\sin\theta*\frac{G}{\cos\theta} \\ accel_{right}=\sin\phi*F=\sin\phi*\frac{G}{\cos\phi\cos\theta} \\ \end{matrix}$

所以：

$\theta=arctan(\frac{accel_{forward}}{G})$ $\phi=arctan(\frac{accel_{right}*\cos\theta}{G})$

函数名：void Quaternion::to_axis_angle

位置：libraries\AP_Math\quaternion.cpp
功能：将四元数转换成轴角形式
原函数：

void Quaternion::to_axis_angle(Vector3f &v)
{
    float l = sqrtf(sq(q2)+sq(q3)+sq(q4));
    v = Vector3f(q2,q3,q4);
    if (!is_zero(l)) {
        v /= l;
        v *= wrap_PI(2.0f * atan2f(l,q1));
    }
}

单位四元素可以表示为：

$V=\left[\begin{matrix} \cos\theta/2 \\ n_1\sin\theta/2 \\ n_2\sin\theta/2 \\ n_3\sin\theta/2 \end{matrix}\right]= \left[\begin{matrix} v_1 \\ v_2 \\ v_3 \\ v_4 \end{matrix}\right]$

单位四元数有：

$v_1^2+v_2^2+v_3^2+v_4^2=1$

又因为：

$v_1=\cos\frac{\theta}{2}$

可以得到：

$\begin{aligned} 1-\cos^2\theta/2&=\sin^2\theta/2\\&=1-v_1\\&=v_2^2+v_3^2+v_4^2 \end{aligned}$ $l=\sqrt{v_2^2+v_3^2+v_4^2}=\sin\theta/2$

所以函数中的$l$就是$\sin\theta/2$,通过$v/l$得到的是轴角的轴：

$v=\left[\begin{matrix} n_1 \\ n_2 \\ n_3 \end{matrix}\right]$

接下来求解轴角的角度$\theta$:

$\begin{aligned} \arctan\theta/2&=\frac{\sin\theta/2}{\cos\theta/2}\\&=\frac{\sqrt{v_2^2+v_3^2+v_4^2}}{v_1} \end{aligned}$

所以最后得到的轴角$v$的形式为

$v=\left[\begin{matrix} n_1*\theta \\ n_2*\theta \\ n_3*\theta \end{matrix}\right]$

如果函数里的四元素不是单位四元素所以要进行单位化

$Q=\left[\begin{matrix} q_1 \\ q_2 \\ q_3 \\ q_4 \end{matrix}\right]\underrightarrow{\text{单位化}} \left[\begin{matrix} q_1/(q_1^2+q_2^2+q_3^2+q_4^2) \\ q_2/(q_1^2+q_2^2+q_3^2+q_4^2) \\ q_3/(q_1^2+q_2^2+q_3^2+q_4^2) \\ q_4/(q_1^2+q_2^2+q_3^2+q_4^2) \end{matrix}\right]=\left[\begin{matrix} v_1 \\ v_2 \\ v_3 \\ v_4 \end{matrix}\right]$

令:

$q_1^2+q_2^2+q_3^2+q_4^2=A$

得到

$V=\left[\begin{matrix} \cos\theta/2 \\ n_1\sin\theta/2 \\ n_2\sin\theta/2 \\ n_3\sin\theta/2 \end{matrix}\right]= \left[\begin{matrix} q_1/A \\ q_2/A \\ q_3/A\\ q_4/A \end{matrix}\right]$

轴角的轴为：

$v=\left[\begin{matrix} n_1 \\ n_2 \\ n_3 \end{matrix}\right]=\left[\begin{matrix} q_2/A \\ q_3/A\\ q_4/A \end{matrix}\right]/\frac{\sin\theta}{2}$ $l=\sqrt{\large(\frac{q_2}{A})^2+(\frac{q_3}{A})^2+(\frac{q_4}{A})^2}=\sin\theta/2$

轴角的角为：

$\begin{aligned} \arctan\theta/2&=\frac{\sqrt{v_2^2+v_3^2+v_4^2}}{v_1}\\ &=\frac{\sqrt{q_2^2+q_3^2+q_4^2}}{A*\frac{q1}{A}}\\ &=\frac{\sqrt{q_2^2+q_3^2+q_4^2}}{q_1} \end{aligned}$

所以无论是不是单位四元数，都可以通过这个函数转换成轴角：

$v=\left[\begin{matrix} n_1*\theta \\ n_2*\theta \\ n_3*\theta \end{matrix}\right]$

四元数形式把欧拉角的三次旋转等效成了了一次旋转（最优化），转成轴角后，因为转轴是单位向量，相当于把这一次旋转投影到了三个轴上（解耦），用这样三个数字作为控制器的输入比直接输入欧拉角更有优势。
（与角速度积分直接得到的三个独立角度相比，积分角度在于无法计算旋转误差，只能直接相减，而轴角投影是得到误差后的投影解耦。） -2018-10-27 更新

以上两个函数反复运用在姿态控制部分，深入理解能够帮助你了解姿态的本质，和姿态控制的思想。
附上我画的姿态控制流程图可以帮助理解：