zoukankan      html  css  js  c++  java
  • 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_tDelta 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_tDelta t + frac{1}{2}a_{t,w}^{'}Delta t ^2 ]

    具体代码见VINS_MONO的predict()函数。

  • 相关阅读:
    geoserver 地图性能和缓存
    geoserver PostGIS的安装和使用
    geoserver 数据图层输出格式
    Centos7Yum安装配置指定版本nginx
    Centos7Yum安装PHP7.2
    CentOS7 中把默认yum源更换成163源
    安装配置Elasticserch的方法
    redis的持久化(RDB&AOF的区别)
    LayerDate渲染多个class出现闪现问题的解决
    explain分析SQL语句详解
  • 原文地址:https://www.cnblogs.com/glxin/p/9851909.html
Copyright © 2011-2022 走看看