首页 > 专利查询

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

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

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

⽜顿迭代法

迭代初值选取

万向节锁

C++代码⽰例

初始姿态解算原理

导航坐标系选为东北天，则重⼒加速度和磁⼒计在导航坐标系中分别表⽰为，。假设机⾝坐标系下，加速度计和磁⼒计输出分别为，。若飞机加速度为0，且加速度计安装位置在飞机重⼼（原因打算后⾯的博客再写），则：

其中，。可以⽤四元数表⽰形式为：

这⾥的表⽰形式和上⼀篇博客中的不⼀样，是为了和保持⼀致。的欧拉⾓形式可参见上⼀篇博客。

选其中四个⽅程如下：

⽜顿迭代法

将元⾮线性⽅程组写成如下形式：

简写为：，

其中：

若⽅程组存在解，在的某个开邻域内可微，设是⽅程组的第个近似解。由⼀阶泰勒公式可得：

可以近似写为：

写成矩阵形式：

则迭代公式为：

算法设计：

在附近选取，给定精度⽔平和最⼤迭代次数。

对于执⾏

计算和

求解关于的线性⽅程组：

若，取，并停⽌计算，否则继续向下执⾏

计算

若，继续迭代，否则停⽌计算。

迭代初值选取

迭代初值的选取会影响迭代次数和迭代结果。⾸先，初值离真值越近，迭代次数越少。此外，如果⽅程有多个解，选取不同的初值会得到不同的解，⽽正确的解只有⼀个，这就需要我们选取的初值必须距离正确的解最近。

上⾯我们选取了四个⽅程，理论上有很多个解。最开始时，我将初值固定为，某些情况下解算出来的偏航⾓会差180°。这意味着，最终的结果满⾜选取的四个⽅程，却不满⾜剩下的两个。那为什么俯仰⾓和滚转⾓解算不会出错呢？这个也是有原因的：

⽤欧拉⾓表⽰，并将(1)式展开可以得到：

可以发现，当时，仅根据加速度计的输出便可以得到俯仰⾓和滚转⾓：

我们选取的四个⽅程包括加速度计的三个⽅程，迭代结果⼀定满⾜(1)式，⽽(1)式⼜唯⼀确定俯仰⾓和滚转⾓，因此⼆者解算结果永远不会出错。

那问题来了，我们该怎样选取合适的初值呢？这个也不难！如下：

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

万向节锁

笔者解决了初值选取的问题之后，发现在俯仰⾓为的时候，不仅迭代次数剧增，⽽且迭代结果偏航⾓和滚转⾓都会出错。这⼜是为什么呢？想了⼜想，想了⼜想，发现了传说中的万向节锁！

我在上⾯特意提到：当时，根据加速度计可以正确得到俯仰⾓和滚转⾓，是因为如果，是⽆意义的。况且实际中加速度计都存在误差，如果俯仰⾓过分接近，加速度计和轴的理论值会被误差淹没，也就⽆法得到正确的滚转⾓和偏航⾓。⽬前，笔者只有⼀个解决办法，对初始姿态解算时的俯仰⾓进⾏限制，笔者设置在之间。

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;

maxstepif (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);

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 (() > 1)

quat += dquat;

} while (curStep <= maxStep && () > 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⽅法。

本文发布于:2024-09-24 03:23:04，感谢您对本站的认可！

本文链接：https://www.17tex.com/tex/4/350512.html

上一篇：量子化学计算中帮助几何优化收敛的常用方法

下一篇：微分方程在MATLAB中的实现

标签：迭代俯仰选取初值解算

留言与评论（共有 0 条评论）