IMU数据积分获得当前位姿

从两帧IMU数据中获得当前位姿的预测思路非常简单，无非是求出当前时刻\(t\)与下一时刻\(t+1\)加速度的均值， 把它作为\(\Delta t\)时间内的平均加速度，有了这个平均加速度及当前时刻的初始速度和初始位置，就可以近似的求出\(t+1\)时刻的速度和位置。求出当前时刻\(t\)与下一时刻\(t+1\)角速度的均值， 把它作为\(\Delta t\)时间内的平均角速度，有了这个平均角速度及当前时刻的姿态，就可以近似的求出\(t+1\)时刻的姿态。
但是由于IMU的数据存在着坐标系、bias和重力加速度的问题需要额外的一些处理。
首先对于加速度，因为imu的加速度数据是在Body坐标系下表示的，所以要利用对应时刻的姿态将其转换到世界坐标系下，转换之前要减去bias，转化之后要减去重力加速度（世界坐标系下的重力加速度恒等于9.8）：
\[ a_{t,w}=Q_t(a_{t,b}-B_a)-g\\ a_{t+1,w}=Q_{t+1}(a_{t+1,b}-B_a)-g \]
\(Q_{t+1}\)是\(t+1\)时刻的姿态，需要用角速度的数据来近似计算：
\[ \omega_t^{'} = \frac{1}{2}(\omega_t+\omega_{t+1})-B_g\\ Q_{t+1}=Q_t(\omega_t^{'}\Delta t) \]
有了\(t,t+1\)时刻的加速度，就可以求出\(t+1\)时刻的速度和位置：
\[ a_{t,w}^{'}=\frac{1}{2}(a_{t,w}+a_{t+1,w})\\ V_{t+1}=V_t+a_{t,w}^{'}\Delta t\\ P_{t+1}=P_t+ V_t\Delta t + \frac{1}{2}a_{t,w}^{'}\Delta t ^2 \]

按照计算的顺序整理一下整个积分的流程：
\[ a_{t,w}=Q_t(a_{t,b}-B_a)-g\\ \omega_t^{'} = \frac{1}{2}(\omega_t+\omega_{t+1})-B_g\\ Q_{t+1}=Q_t(\omega_t^{'}\Delta t)\\ a_{t+1,w}=Q_{t+1}(a_{t+1,b}-B_a)-g\\ a_{t,w}^{'}=\frac{1}{2}(a_{t,w}+a_{t+1,w})\\ V_{t+1}=V_t+a_{t,w}^{'}\Delta t\\ P_{t+1}=P_t+ V_t\Delta t + \frac{1}{2}a_{t,w}^{'}\Delta t ^2 \]
具体代码见VINS_MONO的predict()函数。

转载于:https://www.cnblogs.com/glxin/p/9851909.html