磁力计和加速度计初始姿态解算

磁力计和加速度计初始姿态解算

四旋翼的初始姿态是通过磁力计和加速度计来解算的，缺一不可。学习过程中遇到了不少问题，想通了总结如下：

• 初始姿态解算原理
• 牛顿迭代法
• 迭代初值选取
• 万向节锁
• C++代码示例

初始姿态解算原理

导航坐标系选为东北天，则重力加速度和磁力计在导航坐标系中分别表示为$[0,0,-g]^T$，$[0,m_y^n,m_z^n]^T$。假设机身坐标系下，加速度计和磁力计输出分别为$[a_x,a_y,a_z]^T$，$[m_x^b,m_y^b,m_z^b]^T$。若飞机加速度为0，且加速度计安装位置在飞机重心（原因打算后面的博客再写），则：

$$\begin{equation} \left[ \begin{array}{ccc} a_x\\a_y\\a_z \end{array} \right]= R_n^b \left[ \begin{array}{ccc} 0\\0\\-g \end{array} \right](1) \end{equation}$$
$$\begin{equation} \left[ \begin{array}{ccc} 0\\m_y^n\\m_z^n \end{array} \right]= R_b^n \left[ \begin{array}{ccc} m_x\\m_y\\m_z \end{array} \right](2) \end{equation}$$
其中，$R_b^n=(R_n^b)^T$。$R_n^b$可以用四元数表示形式为：
$$\begin{equation} \left[ \begin{array}{ccc} q_0^2+q_1^2-q_2^2-q_3^2&&2(q_1q_2+q_0q_3)&&2(q_1q_3-q_0q_2)\\ 2(q_1q_2-q_0q_3)&&q_0^2-q_1^2+q_2^2-q_3^2&&2(q_2q_3+q_0q_1)\\ 2(q_1q_3+q_0q_2)&&2(q_2q_3-q_0q_1)&&q_0^2-q_1^2-q_2^2+q_3^2 \end{array} \right] \end{equation}$$
这里的表示形式和上一篇博客中的不一样，是为了和$Matlab$保持一致。$R_n^b$的欧拉角形式可参见上一篇博客。
选其中四个方程如下：
$$\begin{eqnarray} \left\{ \begin{array}{ccc} 2(q_1q_3-q_0q_2)+\frac{a_x}{g}=0\\ 2(q_2q_3+q_0q_1)+\frac{a_y}{g}=0\\ q_0^2-q_1^2-q_2^2+q_3^2+\frac{a_z}{g}=0\\ m_x^b(q_0^2+q_1^2-q_2^2-q_3^2)+m_y^b(2(q_1q_2-q_0q_3))+m_z^b(2(q_1q_3+q_0q_2))=0\\ \end{array} \right. \end{eqnarray}$$

---

牛顿迭代法

将$n$元非线性方程组写成如下形式：

$$\begin{eqnarray} \left\{ \begin{array}{ccc} f_1(x_1,x_2,...,x_n)=0\\ f_2(x_1,x_2,...,x_n)=0\\ \vdots\\ f_n(x_1,x_2,...,x_n)=0\\ \end{array} \right. \end{eqnarray}$$
简写为：$F(x)=0$，
其中：$x=(x_1,x_2,\ldots,x_n)^T,F(x)=(f_1(x),f_2(x),\ldots,f_n(x))^T$
若方程组存在解$x^*\in int(D)$，在$x^*$的某个开邻域$D_0=\{x\mid \|x-x^*\|<\delta,\delta>0\}\subset D$内$F(x)$可微，设$x^{(k)}\in D_0$是方程组的第$K$个近似解。由一阶泰勒公式可得：
$f_i(x)\approx f_i(x^{(k)})+\mathop{\sum_{j=1}^{n}}\frac{\partial f_i(x^{(k)})}{\partial x_j}(x_j-x_j^{(k)});(i=1,2,...,n)$
可以近似写为：
$f_i(x^{(k)})+\mathop{\sum_{j=1}^{n}}\frac{\partial f_i(x^{(k)})}{\partial x_j}(x_j-x_j^{(k)})=0;(i=1,2,...,n)$
写成矩阵形式：
$F'(x^{(k)})(x-x^{(k)})=-F(x^{(k)})$
则迭代公式为：
$x^{(k+1)}=x^{(k)}-F'(x^{(k)})^{-1}F(x^{(k)});(k=0,1,2...)$
算法设计：

• 在$x^*$附近选取$x^{(0)}\in D$，给定精度水平$\epsilon >0$和最大迭代次数$M$。
• 对于$k=0,1,...M$执行
  
  • 计算$F(x^{(k)})$和$F'(x^{(k)})$
  • 求解关于$\Delta x^{(k)}$的线性方程组：$F'(x^{(k)})\Delta x^{(k)}=-F(x^{(k)})$
  • 若$\|\Delta x^{(k)}\|/\|x^{(k)}\|\leq\epsilon$，取$x^*\approx x^{(k)}$，并停止计算，否则继续向下执行
  • 计算$x^{(k+1)}=x^{(k)}+\Delta x^{(k)}$
  • 若$k<M$，继续迭代，否则停止计算。

---

迭代初值选取

迭代初值的选取会影响迭代次数和迭代结果。首先，初值离真值越近，迭代次数越少。此外，如果方程有多个解，选取不同的初值会得到不同的解，而正确的解只有一个，这就需要我们选取的初值必须距离正确的解最近。
上面我们选取了四个方程，理论上有很多个解。最开始时，我将初值固定为$[1,0,0,0]$，某些情况下解算出来的偏航角会差180°。这意味着，最终的结果满足选取的四个方程，却不满足剩下的两个。那为什么俯仰角和滚转角解算不会出错呢？这个也是有原因的：
用欧拉角表示$R_n^b$，并将(1)式展开可以得到：

$$\begin{eqnarray} \left\{ \begin{array}{ccc} sin(\theta)=\frac{a_x}{g}\\ cos(\theta)sin(\phi)=\frac{-a_y}{g}\\ cos(\theta)cos(\phi)=\frac{-a_z}{g} \end{array} \right. \end{eqnarray}$$
可以发现，当$cos(\theta)\neq0$时，仅根据加速度计的输出便可以得到俯仰角和滚转角：
$$\begin{eqnarray} \left\{ \begin{array}{ccc} \theta=asin(a_x/g)\\ \phi=atan(-a_y,-a_z)\\ \end{array} \right.(3) \end{eqnarray}$$
我们选取的四个方程包括加速度计的三个方程，迭代结果一定满足(1)式，而(1)式又唯一确定俯仰角和滚转角，因此二者解算结果永远不会出错。
那问题来了，我们该怎样选取合适的初值呢？这个也不难！如下：
$$\begin{equation} \left[ \begin{array}{ccc} a_x&&m_x^b\\ a_y&&m_y^b\\ a_z&&m_z^b\\ \end{array} \right]= \left[ \begin{array}{ccc} R_{12}&&R_{13}\\ R_{22}&&R_{23}\\ R_{32}&&R_{33}\\ \end{array} \right] \left[ \begin{array}{ccc} 0&&m_y^n\\ -g&&m_z^n\\ \end{array} \right](4) \end{equation}$$

$R_{ij}$是旋转矩阵的后两列，等式右边的2x2方阵是可逆的，并且是个常数矩阵，这样也就可以求出这两列元素。俯仰角和滚转角都已知的情况下，根据旋转矩阵第二列便可以求出偏航角的正弦和余弦，进而得到偏航角。把得到的欧拉角转化为四元数来作为迭代的初值，就一定可以得到正确的解。

---

万向节锁

笔者解决了初值选取的问题之后，发现在俯仰角为$\pm90^\circ$的时候，不仅迭代次数剧增，而且迭代结果偏航角和滚转角都会出错。这又是为什么呢？想了又想，想了又想，发现了传说中的万向节锁！
我在上面特意提到：当$cos(\theta)\neq 0$时，根据加速度计可以正确得到俯仰角和滚转角，是因为如果$cos(\theta)=0$，$\phi=atan(0,0)$是无意义的。况且实际中加速度计都存在误差，如果俯仰角过分接近$\pm90^\circ$，加速度计$a_y$和$a_z$轴的理论值会被误差淹没，也就无法得到正确的滚转角和偏航角。目前，笔者只有一个解决办法，对初始姿态解算时的俯仰角进行限制，笔者设置在$\pm75^\circ$之间。

C++代码示例

int NewtonIteration(cVector<3> &acc, cVector<3> &mag, cQuaternion &quat)
{
    int maxStep = 10;
    int curStep = 0;
    double error = 1E-8;
    cMatrix<4, 4> dF;
    cVector<4>  F;
    cQuaternion dquat;
    cEulerAngle angle;
    if (fabs(acc(3))<9.5)
    {
        double sin_yaw, cos_yaw, sin_pitch, cos_pitch, sin_roll, cos_roll;
        sin_pitch = acc(1) / g;
        cos_pitch = sqrt(1 - sin_pitch*sin_pitch);
        sin_roll = -acc(2) / (g*cos_pitch);
        cos_roll = -acc(3) / (g*cos_pitch);
        sin_yaw = (mag(1)*g + acc(1)*mnz) / (g*mny*cos_pitch);
        cos_yaw = ((mag(2)*g + acc(2)*mnz) / (g*mny) - sin_roll*sin_pitch*sin_yaw) / cos_roll;
        angle(1) = atan2(sin_roll, cos_roll);
        angle(2) = asin(sin_pitch);
        angle(3) = atan2(sin_yaw, cos_yaw);
        angle.toQuat(quat);
        do{
            curStep++;
            dF(1, 1) = -quat(3);
            dF(1, 2) = quat(4);
            dF(1, 3) = -quat(1);
            dF(1, 4) = quat(2);
            dF(2, 1) = quat(2);
            dF(2, 2) = quat(1);
            dF(2, 3) = quat(4);
            dF(2, 4) = quat(3);
            dF(3, 1) = 2 * quat(1);
            dF(3, 2) = -2 * quat(2);
            dF(3, 3) = -2 * quat(3);
            dF(3, 4) = 2 * quat(4);
            dF(4, 1) = 2 * (mag(1) * quat(1) - mag(2) * quat(4) + mag(3) * quat(3));
            dF(4, 2) = 2 * (mag(1) * quat(2) + mag(2) * quat(3) + mag(3) * quat(4));
            dF(4, 3) = 2 * (-mag(1) * quat(3) + mag(2) * quat(2) + mag(3) * quat(1));
            dF(4, 4) = 2 * (-mag(1) * quat(4) - mag(2) * quat(1) + mag(3) * quat(2));
            F(1) = -(quat(2) * quat(4) - quat(1) * quat(3) + acc(1) / (2 * g));
            F(2) = -(quat(3) * quat(4) + quat(1) * quat(2) + acc(2) / (2 * g));
            F(3) = -(quat(1) * quat(1) - quat(2) * quat(2) - quat(3) * quat(3) + quat(4) * quat(4) + acc(3) / g);
            F(4) = -(mag(1) * (quat(1) * quat(1) + quat(2) * quat(2) - quat(3) * quat(3) - quat(4) * quat(4))
                + 2 * mag(2) * (quat(2) * quat(3) - quat(1) * quat(4)) + 2 * mag(3) * (quat(2) * quat(4) + quat(1) * quat(3)));
            LinearEquationSolve(dF, F, dquat);
            if (dquat.norm() > 1)
                dquat.normalize();
            quat += dquat;
            quat.normalize();
        } while (curStep <= maxStep && dquat.norm() > error);
        std::cout << "*************************************" << std::endl;
        std::cout << "step : " << curStep << std::endl;
        std::cout << "result : " << std::endl;
        std::cout << quat << std::endl;
        cEulerAngle ans(quat);
        std::cout << ans*57.3 << std::endl;
    }
    return curStep;
}

LinearEquationSolve()解线性方程组采用全主元Doolittle方法。

---