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

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

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

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

初始姿态解算原理

导航坐标系选为东北天，则重力加速度和磁力计在导航坐标系中分别表示为 [0,0,−g]T ， [0,mny,mnz]T 。假设机身坐标系下，加速度计和磁力计输出分别为 [ax,ay,az]T ， [mbx,mby,mbz]T 。若飞机加速度为0，且加速度计安装位置在飞机重心（原因打算后面的博客再写），则：

⎡⎣⎢axayaz⎤⎦⎥=Rbn⎡⎣⎢00−g⎤⎦⎥(1)

⎡⎣⎢0mnymnz⎤⎦⎥=Rnb⎡⎣⎢mxmymz⎤⎦⎥(2)

其中， Rnb=(Rbn)T 。 Rbn 可以用四元数表示形式为：

⎡⎣⎢⎢q20+q21−q22−q232(q1q2−q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q20−q21+q22−q232(q2q3−q0q1)2(q1q3−q0q2)2(q2q3+q0q1)q20−q21−q22+q23⎤⎦⎥⎥

这里的表示形式和上一篇博客中的不一样，是为了和 Matlab 保持一致。 Rbn 的欧拉角形式可参见上一篇博客。
选其中四个方程如下：

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪2(q1q3−q0q2)+axg=02(q2q3+q0q1)+ayg=0q20−q21−q22+q23+azg=0mbx(q20+q21−q22−q23)+mby(2(q1q2−q0q3))+mbz(2(q1q3+q0q2))=0

---

牛顿迭代法

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

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪f1(x1,x2,...,xn)=0f2(x1,x2,...,xn)=0⋮fn(x1,x2,...,xn)=0

简写为： F(x)=0 ，
其中： x=(x1,x2,…,xn)T,F(x)=(f1(x),f2(x),…,fn(x))T
若方程组存在解 x∗∈int(D) ，在 x∗ 的某个开邻域 D0={x∣∥x−x∗∥<δ,δ>0}⊂D 内 F(x) 可微，设 x(k)∈D0 是方程组的第 K 个近似解。由一阶泰勒公式可得：
fi(x)≈fi(x(k))+∑nj=1∂fi(x(k))∂xj(xj−x(k)j);(i=1,2,...,n)
可以近似写为：
fi(x(k))+∑nj=1∂fi(x(k))∂xj(xj−x(k)j)=0;(i=1,2,...,n)
写成矩阵形式：
F′(x(k))(x−x(k))=−F(x(k))
则迭代公式为：
x(k+1)=x(k)−F′(x(k))−1F(x(k));(k=0,1,2...)
算法设计：

• 在 x∗ 附近选取 x(0)∈D ，给定精度水平 ϵ>0 和最大迭代次数 M 。
• 对于 k=0,1,...M 执行
  
  • 计算 F(x(k)) 和 F′(x(k))
  • 求解关于 Δx(k) 的线性方程组： F′(x(k))Δx(k)=−F(x(k))
  • 若 ∥Δx(k)∥/∥x(k)∥≤ϵ ，取 x∗≈x(k) ，并停止计算，否则继续向下执行
  • 计算 x(k+1)=x(k)+Δx(k)
  • 若 k<M ，继续迭代，否则停止计算。

---

迭代初值选取

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

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪sin(θ)=axgcos(θ)sin(ϕ)=−aygcos(θ)cos(ϕ)=−azg

可以发现，当 cos(θ)≠0 时，仅根据加速度计的输出便可以得到俯仰角和滚转角：

{θ=asin(ax/g)ϕ=atan(−ay,−az)(3)

我们选取的四个方程包括加速度计的三个方程，迭代结果一定满足(1)式，而(1)式又唯一确定俯仰角和滚转角，因此二者解算结果永远不会出错。
那问题来了，我们该怎样选取合适的初值呢？这个也不难！如下：

⎡⎣⎢axayazmbxmbymbz⎤⎦⎥=⎡⎣⎢R12R22R32R13R23R33⎤⎦⎥[0−gmnymnz](4)

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

---

万向节锁

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

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方法。

---