[发明专利]单目视觉和惯性传感器融合的远距离测距系统及方法

权利要求书

1.一种单目视觉和惯性传感器融合的远距离测距系统，包括：

单目视觉SLAM模块，包括单目相机，用于采集视觉图像数据；

IMU模块，用于获得IMU模块的IMU位姿数据；以及

尺度估计模块，将所述视觉图像数据和IMU位姿数据进行融合，实现尺度估计和定位建图，获得具备真实尺度的地图信息，完成待测目标的距离测距。

2.根据权利要求1所述的单目视觉和惯性传感器融合的远距离测距系统，通过对单目相机采集的无绝对尺度信息的视觉图像数据进行处理，获得视觉位姿数据。

3.根据权利要求2所述的单目视觉和惯性传感器融合的远距离测距系统，所述视觉位姿数据包括：单目相机位置，单目相机旋转矩阵，单目相机旋转四元数，以及单目相机旋转角速度。

4.根据权利要求1所述的单目视觉和惯性传感器融合的远距离测距系统，所述IMU模块包括陀螺仪、加速度计；所述IMU位姿数据包括：IMU加速度、IMU角速度。

5.一种单目视觉和惯性传感器融合的远距离测距方法，通过如权利要求1至4任一项所述的单目视觉和惯性传感器融合的远距离测距系统进行远距离测距，所述测距方法，包括：

步骤S1：通过单目相机采集视觉图像数据，并进行视觉定位和重投影构建地图；

步骤S2：在步骤S1所构建的地图中标注出待测距的目标物体；

步骤S3：对所构建的地图构建的地图进行尺度估计；以及

步骤S4：根据尺度估计结果，对构建的地图进行尺度缩放，从而获得具备真实尺度的地图信息，获得待测目标距离数据。

6.根据权利要求5所述的单目视觉和惯性传感器融合的远距离测距方法，步骤S1中，单目视觉SLAM模块通过单目相机观测路标点，将三维路标点投影到相机的成像平面，得到像素坐标；成像时的模型采用针孔成像模型；单目视觉SLAM模块的单个路标点按照如下方程进行观测：

ZP_uv＝K(RP_W+t)＝KTP_W；

其中，P_u，v是图像上的像素坐标，P_W＝(X Y Z)^T是路标点在相机坐标系下的三维坐标，矩阵K是相机内参数矩阵，T是相机从相机坐标系到世界坐标系的变换矩阵，R为相机的旋转矩阵，t是相机从相机坐标系到世界坐标系的平移向量，Z是路标点在相机坐标系下的Z轴坐标，也即路标点在相机坐标系下的深度值；之后将观测结果进行最小化重投影误差，π_(·)是投影函数，从而可以得到视觉观测的单目相机位姿结果，即相机的旋转矩阵R和相机的位置p：

其中，Xⁱ是第i个路标点在相机坐标系下的位置，是第i个路标点在像素平面的投影坐标，ρ()是鲁棒核函数，χ是从单目相机捕获的图像中提取的所有特征点(也即路标点)的集合，∑是求和符，是指使其后面式子达到最小值时的变量R，t的取值，p是相机的位置数据，也即相机的平移向量t。

7.根据权利要求5所述的单目视觉和惯性传感器融合的远距离测距方法，单目视觉SLAM模块使用ORB特征点和三个主要的并行线程：实时跟踪特征点的前端跟踪线程；用于构建局部集束调整的后端局部建图优化线程；以及用于消除累积漂移和全局位姿图优化的后端回环检测线程。

8.根据权利要求5所述的单目视觉和惯性传感器融合的远距离测距方法，步骤S2中，在定位和建图的过程中，当稳定的相机图像画面之中出现了待测距的目标物体，则在图像中手动选取目标物体的特征点，之后保持相机稳定继续移动一定距离，继续建图，并再次在图像中手动选取之前已选定的特征点，使包含目标物体的场景在所构建的地图中形成闭环；当构建稳定的闭环地图之后，操作结束，否则继续移动和选取目标物体特征点。