IMU的加速度补偿、祛除向心力

1. 简介

1. 一般，我们期望用IMU测量某个Target坐标系的加速度、角速度信息，然而IMU的坐标系与Target 坐标系一般存在位姿关系，此时IMU测量的加速度不能直接代表Target左坐标系的加速度。比如Target 坐标系绕Z轴自转动，Target的XY轴的加速度应该为0；然而IMU的加速度，包含了向心力，XY的加速度发生变化，不能直接用于Target坐标系。
2. 基于上述问题，如果需要Target坐标系的加速度，需要对IMU的加速度进行向心力的补偿。
3. 参考：CSND-浅谈加速度计旋转补偿【3D个人觉得不合理】

2. 仅有XY偏移的修正过程

0. 简介：假如IMU的坐标系与Target坐标系只有平移关系，没有旋转关系，也就是坐标系对齐，且只考虑XY。
   

1. 向心力$F_{向}=m{a}_{向}=m{w}^{2}r$，其中 $w$ 为Target坐标系自转角速度(弧度)。可以得到在Target坐标系下，IMU的向心加速度为
   $$a_{向心}=w^2\cdot r=w^2\cdot \sqrt{△x^2+△y^2}$$

2. IMU的X轴上，向心力叠加量为，这一部分需要IMU的X轴加速度减去
   $$X_a=a_{向心}\cdot cos\theta =a_{向心}\cdot \frac{△x}{r}=w^2\cdot △x$$

3. IMU的Y轴上，向心力叠加量为，这一部分需要IMU的Y轴加速度减去
   $$Y_a=a_{向心}\cdot sin\theta =a_{向心}\cdot \frac{△y}{r}=w^2\cdot △y$$

3. 3D修正过程

0. 首先将IMU的坐标系与Target坐标系的先旋转修正，此时IMU坐标系与Target的坐标系朝向对齐，问题就可以简化问题

1. 对IMU的数据进行旋转

   • 设IMU坐标系在Target坐标系朝向的旋转关系为 R ,即Target到IMU的过渡旋转，或者叫在Target坐标系下，IMU的姿态为R
     $$Target\cdot R=IMU$$
   • 那么对齐后的IMU数据为
     $$\begin{gathered} acc=R\cdot ac{c}_{imu} \\ gyro=R\cdot gyr{o}_{imu} \end{gathered}$$

2. 此时问题和第2节中类似，不过此时我们区别是，需要考虑Z轴的偏移影响

   • 在第2节可以看到，Z轴旋转 只会 增加X轴、Y轴的向心力，不会增加Z轴。同理，X轴旋转也会增加Y轴、Z轴向心力，Y轴旋转会增加X轴、Z轴向心力。
   • 所以，每个轴的向心力会受到另外两个轴的旋转影响。这里角速度为Target坐标轴角速度，不过它和旋转后的IMU角速度一致。
   • 那么向心力在每个轴的分量为，需要注意的是，这是原始测量值需要减去的值
     $$\begin{gathered} a_x=w_z\cdot △x+w_y\cdot △x \\ {a}_y=w_z\cdot △y+w_x\cdot △y \\ {a}_z=w_y\cdot △z+w_x\cdot △z \end{gathered}$$

3. 代码实现

   def IMU_Centripetal_force_correction(self, imu_acc, imu_gyro, rotation, position):
       '''
           Target坐标系作为参考坐标系,IMU的姿态为rotation、position,进行向心力去除
           imu_acc、imu_gyro 为IMU的测量值
       '''
       # 0. 得到偏移量
       delta_x, delta_y, delta_z = position[0], position[1], position[2]
       
       # 1. 将IMU的坐标系朝向与Target坐标系对齐
       acc  =  np.asarray(rotation,dtype=np.float32) @ np.asarray(imu_acc, dtype=np.float32)
       gyro =  np.asarray(rotation,dtype=np.float32) @ np.asarray(imu_gyro, dtype=np.float32)
       
       # 2. 向心力的叠加量计算
       wx, wy, wz = gyro[0], gyro[1], gyro[2]
       delta_ax = wz*delta_x + wy*delta_x
       delta_ay = wz*delta_y + wx*delta_y
       delta_az = wy*delta_z + wx*delta_z
       
       # 3. 修正加速度，陀螺仪不需要再修正
       acc[0] = acc[0] - delta_ax
       acc[1] = acc[1] - delta_ay
       acc[2] = acc[2] - delta_az 
       
      return acc, gyro
   

4. 位置的旋转

1. 设IMU的一点为$P_I$，IMU在Word的旋转关系为$R_{WI}$，则该点在Word坐标系下的点为$R_{WI}\cdot P_I$

5. 速度的旋转

1. 结论：该量与位置无关，不考虑位置影响；不同坐标系下的速度，并非直接旋转得到
2. 同一点 $P$ 在IMU坐标系的坐标为$P_i$，在Word坐标系下为 $P_w$，二者的关系满足
   $$P_w=R_{wi}\cdot P_i$$
3. 对向量求导，可以得到每个轴的速度。因此对该点进行求导，得到该点的速度。 注意: $[{]}_{\times }$ 是一个反对程矩阵算子, $w_i$是IMU坐标系下的角速度， $w_w\times P_w$ 是对应元素相乘
   $$\begin{array}{rl}{V}_w=\dot{P_w}& =R_{wi}\cdot \dot{P_i}+\dot{R_{wi}}\cdot P_i\\ & =R_{wi}\cdot V_i+R_{wi}\cdot [w_i{]}_{\times }\cdot P_i\\ & =R_{wi}\cdot V_i+[R_{wi}\cdot w_i{]}_{\times }\cdot R_{wi}\cdot P_i\\ & =\underset{旋转后的速度}{\underbrace{R_{wi}\cdot V_i}}+\underset{坐标系之间的相对速度}{\underbrace{w_w\times P_w}}\end{array}$$
4. 结论：IMU在Word坐标系下的速度，等于速度进行旋转，与相对速度$w_w\cdot P_w$的和

6. 加速度的旋转

1. 同一点 $P$ 在IMU坐标系的坐标为$P_i$，在Word坐标系下为 $P_w$，二者的关系满足
   $$P_w=R_{wi}\cdot P_i$$
2. 对该速度向量求导，可以得到每个轴的加速度大小， 需要注意$w_w=R_{wi}\cdot w_i$, 并且 $\dot{R_{wi}}=[R_{wi}\cdot w_i{]}_{\times }\cdot R_{wi}$。
   $$\begin{array}{rl}{a}_w=\dot{V_w}& =R_{wi}\cdot \dot{V_i}+\dot{R_{wi}}\cdot V_i+w_w\times \dot{P_w}+\dot{w_w}\times P_w\\ & =R_{wi}\cdot a_i+\dot{R_{wi}}\cdot V_i+w_w\times (V_w+w_w\times P_w)+[\dot{R_{wi}}\cdot w_i+R_{wi}\cdot \dot{w_i}]\times P_w\\ & =R_{wi}\cdot a_i+w_w\times V_w+w_w\times V_w+w_w\times (w_w\times P_w)+[[R_{wi}\cdot w_i{]}_{\times }\cdot R_{wi}\cdot w_i+R_{wi}\cdot \dot{w_i}]\times P_w\\ & =R_{wi}\cdot a_i+2w_w\times V_w+w_w\times (w_w\times P_w)+(w\times w+\dot{w_w})\times P_w\\ & =\underset{旋转后的加速度}{\underbrace{R_{wi}\cdot a_i}}+\underset{科氏力}{\underbrace{2w_w\times V_w}}+\underset{离心力}{\underbrace{w_w\times (w_w\times P_w)}}+\underset{欧拉力}{\underbrace{\dot{w_w}\times P_w}}\end{array}$$
3. 需要注意的是，上面加速度的只需要 IMU与world 坐标系之间的关系$R_{wi}$，以及$P$点的位置，IMU测量得到的$w_i、a_i$，最后是world坐标系下的$\dot{w_w}$