一种基于连续时间模型的多相机视觉惯性里程计优化方法

专利主权项内容

1.一种基于连续时间模型的多相机视觉惯性里程计优化方法，其特征在于包括以下步骤：S1、收集传感器数据，包括惯性测量单元输出的加速度和角速度数据、简称IMU数据，以及多相机同步采集的多张图像数据、简称多相机图像帧，并缓存在缓存队列中；S2、传感器数据预处理，构造当前多相机图像帧和前一多相机图像帧帧间状态量约束；S3、基于深度学习神经网络对当前多相机图像帧进行图像感知，得到移动物体的语义感知结果，并生成移动物体图像掩膜，所述移动物体图像掩膜用于特征点提取和跟踪时剔除移动物体区域的特征点；S4、取出当前多相机图像帧和前一多相机图像帧进行特征点提取和跟踪，得到前后多相机图像帧特征点关联数据以及当前多相机图像帧内多相机间特征点关联数据；S5、如果所述多相机视觉惯性里程计没有初始化，则进行所述多相机视觉惯性里程计初始化，构建初始特征地图，在任意时间将多相机图像帧和IMU数据进行关联，对齐IMU坐标系和视觉局部坐标系，同时基于B样条曲线的连续时间轨迹模型，对多相机运动轨迹进行参数化，执行步骤S2；S6、如果所述多相机视觉惯性里程计已经初始化，则当前多相机图像帧利用IMU积分得到位姿初值，并加入滑窗大小为N的滑动窗口内进行非线性优化，以滑窗内的所有轨迹控制点位姿，特征逆深度，每个相机的行延迟以及相机和IMU传感器间的时移作为估计状态量，并以IMU预积分误差和特征点重投影误差为误差项，迭代估计最优的位姿，执行步骤S2。