由文献[25-26]可知,可列出(n-1)个方程,但是有(n 1)个参数,因此求解该方程组,还需添加其他约束。根据文献[27],本文选择“自由边界”约束,即:
ai、bi、ci和di,关于y(ti)和z(ti)的内插同x(ti)。解算出这些系数后,可根据视觉对应的时间戳内插出该时间阶段任意时刻的三维坐标值。1.2 多传感器约束方程的建立
为了实时进行伪卫星和VIO的融合,本文选择滑窗融合模式,窗口长度为10。伪卫星坐标系采用室内局部坐标系。为防止秩亏现象发生,首先把融合后的地图坐标系第一帧与伪卫星坐标系对齐。
由于伪卫星不能求出旋转向量,因此在优化的时候只能利用坐标向量和速度向量。总的融合约束方程可以写为
(2)
式中, rC和rS分别是VIO和伪卫星测量残差;pi,Ci 1和qi,Ci 1分别是VIO的平移和旋转测量值;pi,S是伪卫星测量值;χ是需要优化的参数。
(3)式中,pi,G、Vi,G和qi,G是优化后的全局平移、速度和旋转参数;bi,g和bi,a分别是陀螺仪和加速度计偏置;i表示滑动窗口中帧数;C、S和G代表视觉世界、伪卫星和融合后全局坐标系标志。
由于pi,CG、qi,CG和视觉VIO密切相关,而视觉VIO会随着时间有误差累积,因此,pi,CG和qi,CG也会有误差累积。为了使优化后的位姿更加平滑,本次研究选择实时优化这两个向量。
1.2.1 VIO约束
由于VIO的尺度漂移是不稳定的,直接把VIO的位姿放在全局坐标系下建立观测方程去估计尺度是不够准确的,因此,把VIO的两帧之间相对的位姿去建立观测方程