[发明专利]一种基于地平面假设的视觉惯导SLAM方法

说明书

本发明涉及一种基于地平面假设的视觉惯导SLAM方法，从图像中提取特征点进行IMU预积分，建立相机投影模型，并进行相机内参标定、IMU和相机之间的外参标定；系统初始化，将视觉观测到的点云和相机位姿对齐到IMU预积分上，恢复地面方程和相机位姿；对地面初始化得到地面方程，确定当前相机位姿下的地面方程，并反投影到图像坐标系上，获取更加准确的地面区域；基于状态估计，推导出各传感器观测模型，将相机观测、IMU观测和地面特征观测融合在一起做状态估计，使用图优化模型来做状态估计，使用稀疏图优化和梯度下降法来实现整个优化。本发明相较之前的算法有较大的精度上的提升，可在全局限制相机位姿的估计，使得准确度大大提升。

技术领域

本发明涉及一种定位及建图技术，具体为一种基于地平面假设的视觉惯导SLAM方法。

背景技术

SLAM全称是同时定位和地图构建，由摄像头实时采集图像，通过一帧帧的图像估计相机的运动轨迹，并重建相机运动场景的地图。传统视觉SLAM使用场景中颜色变化明显的点、线作为地图的路标点，没有任何实际含义，也没有任何的上下文语义，在商场环境中受光照和行人遮挡影响严重。为了让机器人在室内外环境自由移动，为了AR应用能够更加真实的与场景融为一体，SLAM成为近年研究的热点，而单目相机体积小、成本低，能够方便的嵌入其他设备中等优点逐渐成为关注的焦点。但是传统的基于纯视觉的SLAM受到光照变化和遮挡后精度和连贯性上面都会明显下降。

发明内容

针对现有技术中基于纯视觉的SLAM受到光照变化和遮挡后精度和连贯性明显下降等不足，本发明要解决的问题是提供一种定位鲁棒性、连续性更强，可恢复真实尺度的基于地平面假设的视觉惯导SLAM方法。

为解决上述技术问题，本发明采用的技术方案是：

本发明一种基于地平面假设的视觉惯导SLAM方法，包括以下步骤：

1)从一图像中提取特征点，对上述特征点进行IMU预积分，建立相机投影模型，并进行相机内参标定、IMU和相机之间的外参标定；

2)基于地面标识物的Visual-Inertial SLAM系统的初始化，将视觉观测到的点云和相机位姿对齐到IMU预积分上，并且用地面上的已知尺寸、坐标的标识物来恢复地面方程和相机位姿；

3)通过Visual-Inertial Odometry得到相机相对地面的旋转和高度，对步骤2)中的地面进行初始化，得到地面方程，确定当前相机位姿下的地面方程，并把这个地面方程反投影到7图像坐标系上，使用图像处理方法获取更加准确的地面区域；

4)基于先验地面特征点深度的Visual-Inertial状态估计，详细推导出各种传感器的观测模型，将相机观测、IMU观测和地面特征观测融合在一起来做状态估计，使用图优化模型来做状态估计，使用稀疏图优化和梯度下降法来实现整个优化。

步骤1)中建立相机投影模型为：

101)世界坐标系和相机坐标系转换公式为

其中为相机的外参，描述相机相对世界原点旋转和平移的量，X_c、Y_c、Z_c为相机坐标系，X_w、Y_w、Z_w为世界坐标系；

图像坐标系到像素坐标系转换公式为

X、Y、Z代表相机坐标系，x、y代表图像平面坐标系，f表示一个线性倍数；

102)将相机坐标在u轴上放大f_x倍，在v轴上放大f_y倍，原点平移c_x，c_y，得到像素坐标系：