[发明专利]一种位姿确定方法、系统、电子设备及计算机存储介质

权利要求书

1.一种位姿确定方法，其特征在于，应用于无人行驶设备，包括：

通过视觉SLAM方法获取所述无人行驶设备在当前时刻下的旋转矩阵；

通过激光SLAM方法获取所述无人行驶设备在当前时刻下的平移矩阵；

基于所述旋转矩阵和所述平移矩阵确定所述无人行驶设备在当前时刻下的位姿。

2.根据权利要求1所述的方法，其特征在于，所述通过激光SLAM方法获取所述无人行驶设备在当前时刻下的平移矩阵，包括：

获取激光扫描设备扫描到的当前时刻下的第一帧点云以及上一时刻下的第二帧点云；

通过ICP运算公式对所述第一帧点云及所述第二帧点云进行特征匹配，得到特征匹配点对；

基于所述特征匹配点对确定所述平移矩阵；

所述ICP运算公式包括：

m←arg min_a(a-b')^TW(a-b')；

Q＝[t_dir q₁ q₂]；b'←R⁰b+st_dir；

其中，a表示所述第一帧点云；b表示所述第二帧点云；(m,b)表示所述特征匹配点对，m表示所述第一帧点云中与所述第二帧点云中特征相匹配的点；R⁰表示所述第一帧点云与所述第二帧点云的相对位置；t_dir表示所述第一帧点云与所述第二帧点云间的距离；s表示t_dir的尺度；α表示惩罚因子。

3.根据权利要求2所述的方法，其特征在于，所述基于所述特征匹配点对确定所述平移矩阵，包括：

判断||m-b'||的值是否小于预设值，若是，则将(m,b)作为目标匹配点对，基于所述目标匹配点对确定所述平移矩阵。

4.根据权利要求2或3所述的方法，其特征在于，所述基于所述特征匹配点对确定所述平移矩阵，包括：

通过RANSAC算法，基于所述特征匹配点对确定所述平移矩阵。

5.根据权利要求4所述的方法，其特征在于，所述基于所述特征匹配点对确定所述平移矩阵之前，还包括：

接收自身上传端口传输的所述预设值。

6.一种位姿确定系统，其特征在于，应用于无人行驶设备，包括：

第一获取模块，用于通过视觉SLAM方法获取所述无人行驶设备在当前时刻下的旋转矩阵；

第二获取模块，用于通过激光SLAM方法获取所述无人行驶设备在当前时刻下的平移矩阵；

第一确定模块，用于基于所述旋转矩阵和所述平移矩阵确定所述无人行驶设备在当前时刻下的位姿。

7.根据权利要求6所述的系统，其特征在于，所述第二获取模块可以包括：

第一获取子模块，用于获取激光扫描设备扫描到的当前时刻下的第一帧点云以及上一时刻下的第二帧点云；

第一匹配子模块，用于通过ICP运算公式对所述第一帧点云及所述第二帧点云进行特征匹配，得到特征匹配点对；

第一确定子模块，用于基于所述特征匹配点对确定所述平移矩阵；

所述ICP运算公式包括：

m←arg min_a(a-b')^TW(a-b')；

Q＝[t_dir q₁ q₂]；b'←R⁰b+st_dir；

其中，a表示所述第一帧点云；b表示所述第二帧点云；(m,b)表示所述特征匹配点对，m表示所述第一帧点云中与所述第二帧点云中特征相匹配的点；R⁰表示所述第一帧点云与所述第二帧点云的相对位置；t_dir表示所述第一帧点云与所述第二帧点云间的距离；s表示t_dir的尺度；α表示惩罚因子。