当BLE遇到MEMS——姿态解算
<div class='showpostmsg'> 本帖最后由 lb8820265 于 2019-1-16 23:13 编辑前面介绍了惯性系统的基础知识，这里来介绍如何使用MEMS传感器进行姿态解算，姿态解算就是使用陀螺仪加速度计算出物体当前的姿态。姿态解算姿态解算是惯性导航基础，还是用提问的方式循序渐进的学习。1. MEMS加速度计输出的是什么？ 先来看看MEMS加速度计的工作原理，加速度计的工艺不同内部结构也不同，但是简单来说可以用一个正方体盒子里面装一个小球的模型来模拟，然后6个面都能检测到压力产生形变，进而转化为电信号，然后压力除以质量就是加速度。 将盒子平放在水平面上（左图），底面就承受着重力加速度G。如果让盒子自由落体（中图），那么底面的加速度就为零。是不是很奇怪呢？不动有加速度G，自由下落却没有加速度，因此加速度输出的就是三个方向的加速度与重力加速度和和。由于有重力加速度的参与，想要直接拿MEMS加速度计的输出值积分来惯性导航的小伙伴要失望了。同样会发现，当盒子45度角放置的时候（右图），两个轴的加速度值相等，因此可是用输出的加速度值来算出物体的角度，但是由于也会测加速度，所以小伙伴也要失望了。但是也不是没有办法，下面会介绍。2. MEMS陀螺仪输出的是什么？ MEMS陀螺仪输出的是旋转角速度。先来看看MEMS陀螺仪的工作原理，不同的陀螺仪有不同的结构，但是总的来说都是使用科里奥利力。就是旋转体系中进行直线运动的质点受到一种使其偏离直线运动的力。产生这样的力有两个要求，一个是载体要旋转，一个是物体需要相对旋转载体运动。当相对运动不变时，力与载体旋转速度成正比。MEMS陀螺仪为了实现不断地产生运动，普遍使用了类似音叉的结构，像音叉一样来回震动来产生相对运动，当旋转时候音叉将会受到科里奥利力进而转化为电势差。如下图所示。 因此MEMS陀螺仪需要有旋转才会产生科里奥利力，转速越快力越大，静止或者往某个方向运动都不会产生力。因此MEMS陀螺仪输出的是以三个坐标轴为旋转轴的旋转速度。由于静态偏差的存在，想要直接拿陀螺仪输出值积分来计算物体角度的小伙伴可能要失望了，但也不是没有办法，下面会介绍。3. 陀螺仪加速度计如何融合？ 前面介绍了使用静态时的加速度计两个轴加速度值的关系可以得出物体的角度，同样使用陀螺仪积分也可以得到角度，它们得到的角度值都各有优缺点。使用加速度计可以通过两个轴的受到的重力加速度值很准确的得出长时间静态时候物体的姿态角度，但是当物体动态时将会受到较大的干扰；使用陀螺仪可以通过积分较为准确的计算出短时间内物体的姿态角度，但是长时间积分的会导致误差积累，导致角度不准确。两者刚好可以长处互补，两者融合后就是玉女剑法双剑合璧了。 将两者进行长处互补就叫做数据融合了，陀螺仪加速度的数据融合就是同时使用两个量计算出角度，只是分配给每个量的权值不同，权值可以不变也可以动态改变。在惯性导航中，这样的类似的融合情况非常的多，例如： 陀螺仪加速度计和磁力计的融合。使用陀螺仪加速度得出的偏航角（Yaw）长时间会漂移，但短时间内响应迅速，磁力计得出的偏航角长时间准确，但是短时间响应慢。 IMU惯导系统和GPS的融合，IMU惯性导航长时间会漂移，但是短时间响应速度，GPS长时间稳定但是短时间响应慢。 IMU惯导系统和GPS和气压计的融合，在高度上GPS测量并不是很准确，所以通常会辅助气压计，气压计响应快但是会受天气等的影响，所以通常会将三者进行融合。 融合算法可以简单的用如下公式表示：Rest(n)= (Racc(n-1) * w1 + Rgyro(n-1) * w2 ) / (w1 + w2)+ (Rest(n-1)- Racc(n-1))*w3 其中Rest表示使用递归的方式获得角度的估计值，Racc表示使用加速度计算出的角度值，Rgyro表示使用陀螺仪计算出的角度值，w1表示对加速计算结果的信任程度（权重），w2表示对陀螺仪计算结果的信任程度。由于加速度计是长时间可靠的，所以通常用加速度计的值与估计值的差积分来使结果准确。这里的w3相当于积分系数。 理论上来说，当转动速度快的时候陀螺仪的积分得出的角度比较可信，当转速比较慢时加速度计得出的角度比较可信，也就是速度快陀螺仪的权重要大，速度慢加速度计的权重要大，这就是变权重的算法，卡尔曼滤波就是这个思路。权重不变的典型融合算法有互补滤波和梯度下降法。4. 如何姿态解算？ 前面简单的介绍了使用陀螺仪和加速度计融合可以得较为准确的角度，但这是一轴的，要得到三轴的角度，就需要一定的技巧了，专业名词叫做姿态解算。需要了解旋转矩阵和四元数。旋转矩阵在前文已经介绍了，使用一个三维的矩阵可以表示任意姿态的变化。四元数用在姿态解算上可以说是一个创新，就像电力系统中的DQ变换，只是用另一种形式表示罢了，但是却有了简少了计算量，避免了万向节，方便差值等优点，这里不做详细介绍。欧拉角和四元数组成的旋转矩阵公式分别如下： 姿态解算的算法非常的多，这里用最简单的mahony互补滤波算法为例，介绍姿态解算的通常流程。 代码摘录自原点博士： void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float halfvx, halfvy, halfvz;
float halfex, halfey, halfez;
float qa, qb, qc;
如果加速计各轴的数均是0，那么忽略该加速度数据。否则在加速计数据归一化处理的时候，会导致除以0的错误。
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
把加速度计的数据进行归一化处理。其中invSqrt是平方根的倒数，使用平方根的倒数而不是直接使用平方根的原因是使得下面的ax，ay，az的运算速度更快。通过归一化处理后，ax，ay，az的数值范围变成-1到+1之间。
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
根据当前四元数的姿态值来估算出各重力分量。用于和加速计实际测量出来的各重力分量进行对比，从而实现对四轴姿态的修正。
halfvx = q1 * q3 – q0 * q2;
halfvy = q0 * q1 + q2 * q3;
halfvz = q0 * q0 – 0.5f + q3 * q3;
使用叉积来计算估算的重力和实际测量的重力这两个重力向量之间的误差。
halfex = (ay * halfvz – az * halfvy);
halfey = (az * halfvx – ax * halfvz);
halfez = (ax * halfvy – ay * halfvx);
把上述计算得到的重力差进行积分运算，积分的结果累加到陀螺仪的数据中，用于修正陀螺仪数据。积分系数是Ki，如果Ki参数设置为0，则忽略积分运算。
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * (1.0f / sampleFreq);
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f;
integralFBy = 0.0f;
integralFBz = 0.0f;
}
把上述计算得到的重力差进行比例运算。比例的结果累加到陀螺仪的数据中，用于修正陀螺仪数据。比例系数为Kp。
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}
通过上述的运算，我们得到了一个由加速计修正过后的陀螺仪数据。接下来要做的就是把修正过后的陀螺仪数据整合到四元数中。
gx *= (0.5f * (1.0f / sampleFreq));
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx – qc * gy – q3 * gz);
q1 += (qa * gx + qc * gz – q3 * gy);
q2 += (qa * gy – qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy – qc * gx);
把上述运算后的四元数进行归一化处理。得到了物体经过旋转后的新的四元数。
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
将四元数转化为欧拉角
roll =atan2f(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1)*57.3;
pitch =asinf(2*q1*q3 - 2*q0*q2)*57.3;
yaw=-atan2f(2*q1*q2 + 2*q0*q3, -2*q2*q2 -2*q3*q3 + 1)*57.3;
}
整个代码的流程图如下： 代码的流程图结合代码可以很清晰的看懂代码，这里对流程图进行一些解释。1. 加速度计值的归一化 所谓的归一化就是将每个轴的值都除以三个轴数据的平方和，将三维向量转化为单位向量，作用后面介绍。2. 四元数获取重力分量 首先是需要知道如果获取重力分量，重力分量的获取实际上就是四元数组成的旋转矩阵乘以重力分量也就是旋转矩阵的第三列， 代码内容就是这么来的。3. 归一化后的重力加速度和四元数的重力分量进行叉乘 首先要明白叉乘的意义， 将矩阵相乘展开就是代码中的内容。4. 叉乘的结果为何可以用来补偿陀螺仪的角速度 两个向量的叉积得到的结果是两个向量的模与他们之间夹角正弦的乘积a×v=|a||v|sinθ，加速度计测量得到的重力向量和预测得到的机体重力向量已经经过单位化，因而它们的模是1，也就是说它们向量的叉积结果仅与sinθ有关，当角度很小时，叉积结果可以近似与角度成正比。感叹，这里真是太巧妙了！ 叉乘的结果就是加速度的重力分量与预测角度的重力分量间的角度差。因为加速度计是在长时间下是可靠的，所以长时间下预测的角度肯定要与加速度计的重力分量重合，这样才是准确的。通过陀螺仪的角速度加上对叉乘结果的比例积分运算进行修正，最后得出正确的角速度。所以互补滤波法得出的角度曲线越靠近正确结果修正的越慢。5. 转化为四元数 前面得到了修正后的角速度值，这里需要将修正后的角度数据积分然后再次变成四元数，思路就是四元数自加然后加上与角速度相关的四元数微分方程。 这里直接给出四元数与角速度相关的微分运算公式： 矩阵运算后就是代码中的内容。6. 四元数转化为欧拉角 由于四元数并不直观，通常还是会将四元数转化为欧拉角，前文中给出了有欧拉角和四元数组成的旋转，将四元数旋转矩阵第三列前两行元素相除可以得到α 角，将矩阵第三行前两列元素相除可以得到γ角，第三行第三列元素可以直接得到β角。所以四元数转化为欧拉角的公式如下： 也就是代码中的内容，乘以57.3是因为公式得出的是弧度值，需要乘180/π转为角度。
此内容由EEWORLD论坛网友lb8820265原创，如需转载或用于商业用途需征得作者同意并注明出处
</div><script> var loginstr = '<div class="locked">查看本帖全部内容，请<a href="javascript:;" style="color:#e60000" class="loginf">登录</a>或者<a href="https://bbs.eeworld.com.cn/member.php?mod=register_eeworld.php&action=wechat" style="color:#e60000" target="_blank">注册</a></div>';
if(parseInt(discuz_uid)==0){
(function($){
var postHeight = getTextHeight(400);
$(".showpostmsg").html($(".showpostmsg").html());
$(".showpostmsg").after(loginstr);
$(".showpostmsg").css({height:postHeight,overflow:"hidden"});
})(jQuery);
} </script><script type="text/javascript">(function(d,c){var a=d.createElement("script"),m=d.getElementsByTagName("script"),eewurl="//counter.eeworld.com.cn/pv/count/";a.src=eewurl+c;m.parentNode.insertBefore(a,m)})(document,523)</script> <p>不说啥了，给大佬磕一个</p>
页:
[1]