加速度计和陀螺仪模型(imu元件)分析

一、先分析加速度

**
1、3自由度：3个轴方向的加速度/力的模型很好理解，前后X，左右Y，上下Z；
2、3自由度: 沿前后轴X方向的滚动，左右轴Y方向的俯仰，上下轴Z方向的偏航；
这三个自由度的正方向可参考飞机航向模型规定；
3、接下来给正方体盒子模型一个任意方向的力F，不垂直于XYZ三平面，Fx,Fy,Fz为三个轴上的分力；
F合 ^2 = Fx ^2 + Fy ^2 + Fz ^2;①
假设F合只有重力(1g=9.8m/s^2)，那么作用于该模型就是：(1g) ^2 = 0 ^2 + 0 ^2 + (1g) ^2;
同理: 假设模型向右滚动了45°，那么关系(提取g)：1 ^2 = 0 ^2 + (SQRT(1/2))^2 + (SQRT(1/2)) ^2 ;
4、一般情况下模型(IMU器件)可能通过物理量转换电信号的方式进行采样测量，最后模拟量转换为数字量输出；
通过i2c、spi等通信方式与MCU进行数据交互，假设是通过12位ADC进行转换的，那么数字量范围0~4095；
5、具体数值参考文章实例过程；
注意IMU器件说明书中零加速度时的电压值，加速度计灵敏度值；
将实际工况实测电压值跟其比较，然后得出该方向上的相对加速度电压值；
将相对加速度电压值通过与灵敏度值换算，可将电压单位v转换为加速度单位g；
假设值为ax，ay，az；
**

二、再分析偏转角度

**
1、F合与XYZ三轴的夹角；
假设分别为θx，θy，θz，并且与Fx,Fy,Fz与F合各自满足余弦关系：cosθx = Fx/F合；
若已知F合,Fx,Fy,Fz力大小，通过反余弦关系可求出θx，θy，θz各角度值；
同理：角度关系满足 1 ^2 = cosθx ^2 + cosθy ^2 + cosθz ^2;①
2、我们所关心的是分别绕三轴转动的偏转角度：
F合在XY平面，XZ平面，YZ平面的投影Fxy，Fxz，Fyz分别与z，y，x轴的夹角永远是直角；
同理，在二维平面上满足关系：Fxy ^2 = Fx ^2 + Fy ^2；
同理，满足关系：F合 ^2 = Fxy ^2 + Fz ^2；很容易得出；
假设Fxz，Fyz与Z轴的夹角是θxz-Y，θyz-X;
θxz-Y就是绕Y轴旋转角度，θyz-X就是绕X轴旋转角度并都以Z轴为基准线(参考线)；
3、陀螺仪测量角度的变化率(角加速度)
假设以绕Y轴为例：上面中的θxz-z得出，简化为θxz；
t0~t1时刻的变化率：aωy = (θxz1 - θxz0)/(t1-t0)，单位即度/秒，这就是陀螺仪检测的物理量；其它轴同理可得；
4、在实际工况下，假设同样是通过模拟量转换为数字量的方式，将ADC数字量数值通过固有的公式换算成单位度/秒的值；
假设仍是12位ADC：
aωy = （adcGyroXZ *Vref /4095 -Vzero）/ Sensitivity_Value ;
adcGyroXZ：可由IMU传感器直接读取到MCU，等价于绕Y轴转动的过程；
Vzero：表示零变化率电压值，即陀螺仪不受任何转动影响时的输出值；
Vref：是ADC采样范围参考电压(画板子的硬件工程师会告诉你)；
Sensitivity_Value：陀螺仪灵敏度，单位mv/度/秒；具体看参考文章；
**

三、加速度+偏转角度加权平均值结合

**
1、统一坐标系，明确采用的IMU元件XYZ轴方向；
这一块注意是确认IMU元件输出数据是否对应自己想要的物理量数值，有可能方向相反(需要取反)，有可能对应错位等问题
需要实际调试一下方位，正反等；
2、调整后得出3个方向的加速度ax，ay，az；得出绕3个轴的角加速度aωx，aωy，aωz；
具体为什么结合测量更准确，参考文章描述；
3、卡尔曼滤波方式优化处理测量值
首先由加速度模型单独得出的加速度值ax，ay，az，以及陀螺仪数据的修正作为卡尔曼滤波处理的输入；
会得到值：ax估，ay估，az估；
所以下一次的估值(输出)会由上一次系统的估值+陀螺仪数据的修正(输入)得出；
【注意】每个时刻t下：ax，ay，az是有变化的(有干扰和噪声)，ax估，ay估，az估也会随着ax，ay，az以及陀螺仪数据的变化而变化；
例如以x方向为例，时刻t=n时，ax估n-1：是n-1刻的估算值也就是上一次估算值，axn：是n时刻的当前实际测量值；
现在我们要计算ax估n；
以绕Y轴旋转角度为例:
反正切： θxz-Y = arctan(Fx/Fz)
那么n-1时刻的θxz-Y(n-1) = arctan(Fx(n-1)/Fz(n-1)) = arctan(ax估(n-1)/ay估(n-1));
n-1~n时间为T：aωy(n) = ( θxz-Y(n) - θxz-Y(n-1) )/ T ;
【注意】因为我们使用IMU元件，其实际测量值：aωx(n)，aωy(n)，aωz(n)是已知的(这个测量值是由传感器中陀螺仪部分实际测量出来的)；
所有我们可根据IMU元件陀螺仪实测的角加速度得到相应的角度值 θyz-X(n)，θxz-Y(n)，θxy-Z(n)；
【注意】这个不是加速度计得出的值；
4、现在需要得出由陀螺仪产生的在XYZ轴上的加速度分量ax-gyro，ay-gyro，az-gyro；
a-gyro^2 = ax-gyro ^2 +ay-gyro ^2 + az-gyro ^2;②
加速度单位：g；②式等式左右同提取单位和数值大小；a-gyro，ax-gyro,ay-gyro,az-gyro提取公因式后可等价简化为1，x，y，z；
1^2 = x^2 +y ^2 + z ^2 ；
那么可根据文中模型得出: x = x/1 = sin(aωy) / SQRT(1+cos(aωy))^2 +tan(aωx) ^2);
ax-gyro =ax-gyro/1 = sin(aωy) / SQRT(1+cos(aωy))^2 +tan(aωx) ^2); 根据几个公式换算得来；
然后文中又根据公式② 的变形，得出：
az-gyro = Sign(az-gyro)*SQRT(1 – ax-gyro^2 – ay-gyro^2)；
其中，当 az-gyro>=0时，Sign(az-gyro) = 1 , 当 az-gyro<0时，Sign(az-gyro) = -1 ；
5、我们已知了ax，ay，az完全由加速度计得出的XYZ轴加速度分量；
和第4步得出的ax-gyro，ay-gyro，az-gyro完全由陀螺仪产生的在XYZ轴上的加速度分量；
两者肯定都要采用，会使用一个加权平均值：
ax估(n-1) = （w1ax+w2 ax-gyro )/（w1+w2）；
另w1 = w2 =w；
ax估(n-1) = （ax+w ax-gyro )/（1+w）；
在上面的公式中，w表示我们对加速度计和陀螺仪的相信程度。
这个值可以通过测试确定，根据经验值5-20之间会得到一个很好的结果。
此算法和卡尔曼滤波最主要的差别是它的权重w是相对固定的，而卡尔曼滤波中的权重会随着加速度计读取的噪声而改变。
卡尔曼滤波注重给你一个“最好”的理论结果(优化估算的算法，进过系统处理，可以使输出值实测值更接近于理想值理论值)；
你可以实现一个算法，它能根据测量的噪声而改变w值，但对大部分应用来说固定的权重也能工作的很好；
ax估(n) = （ax+w ax-gyro )/（1+w）；这样估算值就得出来(精度有待商榷)；
【tips】公式推导可参考文章中构画的模型；
参考文献：
1、http://wjhsh.net/ne-zha-p-7390032.html
2、https://www.geek-workshop.com/forum.php?mod=viewthread&tid=1695&reltid=676&pre_thread_id=0&pre_pos=1&ext=
3、http://starlino.com/imu_kalman_arduino.html
4、注：关于此算法的具体实现和测试，请阅读这篇文章：http://starlino.com/imu_kalman_arduino.html
加速度计和陀螺仪IMU融合的其他资源：
http://www.mikroquad.com/pub/Res … ryFilter/filter.pdf
http://stackoverflow.com/questio … -accelerometer-data
http://www.dimensionengineering.com/accelerometers.htm