IMU与轮速计联合初始化

2023-12-28 15:40:23

? ? ? ? 最近在做一个有意思的项目,在地库里面实现多传感器融合里程计,采用的是滤波算法。由于地库里没有gnss信号,缺乏绝对观测,也不想用视觉特征等比较慢的算法,只采用轮速计与IMU做融合。

????????公司其他项目在地库中是主要是直接用轮速计做积分,用IMU做pitch角和roll角的更新。因此,在xy平面上实际上就是轮速计的积分,轮速计积分虽然比较稳定,但是并不是很好,有时候也会飘,而缺乏绝对观测的原因,一旦飘了一点点就再也回不来了,以至于回环的时候有一个很大的偏差。说到IMU,这真是个难搞的器件。如果直接用它做积分,立马就要飞掉,甚至你都看不出一点点趋势,发散的非常快,以至于还搞出速度用轮速计,角度用IMU这样的不伦不类的融合方式。地库里程计也是个细活,差一点点结果就千差万别,前面的一个小小偏差对后面的轨迹就是大范围的偏差。

? ? ? ? 言归正传,IMU与轮速计做融合最重要的是要做初始化,即估计IMU的角速度随机游走bg、加速度随机游走ba、初始速度v以及初始重力方向g^0。这些量要是估计不好,IMU立马死给你看。以前在有gnss信号的情况下,根本就没做初始化,因为绝对观测会把错误位姿纠正回来,但是在地库中就不能这么搞了,所以也让我认识到初始化的重要性,还是不得不做的。我们知道,重力方向都是朝下的,大小为g^w={(0,0, -9.81)}^T,这里说的初始重力方向g^0指的是重力方向在以第一帧位姿坐标系下的表示,就是R_0^Tg^w,R_0是第一帧位姿的旋转,因此如果重力方向采用了g^w,那么初始方向就要用R_0,相应的初始速度v也要乘以R_0,即R_0v,如果重力方向采用g^0,那么初始方向就可以设为单位阵I,初始速度为v。

1. 对于bg的初始化来说,陀螺仪有角速度观测w_I,轮速计也有角速度观测w_o。因此,b_g=w_I-R_{Io}w_o, 累计一定量的观测,取平均就可以了。

2. 对于g^0的初始化来说,可以截取两个时刻的轮速计时间段ts-te,将这两个时刻内的IMU观测找出来,对不齐的就进行插值。IMU的加速度计累计量就是这两个时刻的速度差,轮速计有两个时刻速度的观测v_i,v_j,由此,建立公式,R_{0i}\beta_{ij}=R_{0j}v_j-R_{0j}v_i+g^0\Delta{t}\beta_{ij}为两时刻加速度计的积分\beta_{ij}=\sum_{i}^{j}(v_i+R_{i,i+1}a_i)

? ? ? ? 这里得到的g^0只是初值,没有限定g^0的模长,实际上g^0只有两个自由度,它的模长始终为9.81。这里采用vins里面的做法,采用球面坐标,球的半径是9.81,在g^0的初值处张成一个切平面,在切平面处进行微调,因此可限制g^0的模长为9.81,公式为g^0=||g||*norm(g^0)+w_1b_1+w_2b_2, 其中w_1,w_2为待优化量,b_1,b_2为切矢量

b_1=\left\{\begin{matrix} g^0\times (1, 0, 0)^T, g^0\neq (1, 0, 0)^T\\ g^0\times(0, 0, 1)^T, otherwise \end{matrix}\right.

b_2=g^0\times{b_1}

3. 对于ba的初始化来说,再次利用公式R_{0i}\beta_{ij}=R_{0j}v_j-R_{0j}v_i+g^0\Delta{t},这里\beta_{ij}忽略了ba,由于ba通常非常小,所以这里忽略掉它以求g^0,当求出g^0之后就可以把它加回来,即\beta_{ij}=\sum_{i}^{j}(v_i+R_{i,i+1}(a_i-b_a)),再次建立多时间戳观测,求得b_a

mark一个题外话,已经被它搞晕好几次了:

T_{wj}=T_{wi}*T_{i,j}

通常,R_i*x+t_i看起来像是让位姿从i变到了j,应该是T_{j,i},但是它实际上表示的是T_{i,j},不是T_{j,i},切记。

文章来源:https://blog.csdn.net/qingcaichongchong/article/details/135260525
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。