[发明专利]一种基于SLAM技术的自主位姿测量方法

权利要求书

1.一种基于SLAM技术的自主位姿测量方法，基于视觉传感器、IMU惯导传感器和GPS传感器实现，所述方法包括：

步骤1)接收待建场景的点云数据、图像信息、姿态信息和目标位置信息；

步骤2)基于改进的极几何约束3D点云运动分割算法对高动态场景进行对象级运动场景分割；

步骤3)对点云数据、图像信息、姿态信息和目标位置信息采用多源数据定位定姿算法，对静态部分的图像信息采用2D-3D匹配算法，对动态目标采用动态目标与视点位置关联估计算法，实现定位；

步骤4)对场景中的静态部分进行三维点云场景构建得到精确匹配的点云数据，对场景中的动态目标进行动态物体构建，并对定位定姿结果辅助进行全局优化，进而完成位姿测量；

所述步骤2)具体包括：

步骤2-1)对前后帧的图像进行特征点提取；

步骤2-2)根据提取的特征点与环境3D点云是否符合极几何约束条件，进行动静态物体的区分，对不同运动物体的3D点云场景进行初步分割；

步骤2-3)通过条件随机场的优化，得到对象级别的运动场景分割数据；

所述步骤2-2)具体包括：

对前后两帧的特征点与环境3D点云间的几何关系进行判断，如果符合极几何约束条件，则对前后两帧图像上每个特征点做一次动静态判断，然后在三维空间中，计算特征点x_i和x_j之间的相对距离d_ij＝‖x_i-x_j‖，判断相对距离d_ij是否稳定，如果稳定，为同一个运动物体，否则，为不同的运动物体；

所述步骤3)具体包括：

步骤3-1)在2D环境中，对点云数据、图像信息、姿态信息和目标位置信息采用“视觉+IMU+GPS”多源数据定位定姿算法；

步骤3-2)在3D静态环境中，对图像信息采用2D-3D匹配算法，通过引入环境信息加强定位解算；

步骤3-3)在3D动态环境中，采用动态目标与视点位置关联估计算法，将分割后的动态目标运动信息进行建模，建立运动物体运动模型及与视点间的数据关联，从动态物体提取出有利于提高视点定位精度的相关信息，实现定位；

所述步骤3-2)具体为：

采用2D-2D方法对相机定位：

其中，x_ij为特征点，E_i(R′,t′)是本质矩阵；

通过前后两帧图像特征点的8点法计算得出本质矩阵，通过SDV分解得到四组解，通过判断得到其中的唯一解，从而得到相机姿态；

对于含有3D环境点云数据，将第一帧观测到的3D点重投影到第二帧上面，得到最小误差的相机姿态为：

其中，R为第一帧图像到相机坐标系下的旋转矩阵，R′为第二帧图像到相机坐标系下的旋转矩阵，t_i为第一帧图像到相机坐标系下的平移变量，t′为第二帧图像到相机坐标系下的平移变量，P为重投影矩阵，x_ij为第二帧特征点，为第一帧特征点；i表示第一帧图像的横坐标，j表示第一帧图像的纵坐标，m表示第一帧图像的横坐标像素点，n表示第一帧图像的纵坐标像素点；

当2D与3D分割不一致时，结合两种定位方法联合估计相机位置，表达公式如下：

条件满足：

对于被动态物体遮挡的静态点云，在目标函数上增加权重，属于动态物体及其遮挡的点云权重设置为零，其他静态点权重设置为1。

2.根据权利要求1所述的基于SLAM技术的自主位姿测量方法，其特征在于，所述步骤3-1)具体为：

当GPS状态正常时，以GPS和IMU组合导航算法进行定位定姿计算；

在GPS状态异常但未失锁时，以GPS、视觉和IMU组合导航算法进行定位定姿计算，引入优化变量的边缘化处理；

当GPS为失锁状态，以视觉和IMU组合导航算法进行定位定姿计算。