加速度计和陀螺仪模型(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