一种位姿确定方法、系统、电子设备及计算机存储介质与流程

本申请涉及位姿确定技术领域，更具体地说，涉及一种位姿确定方法、系统、电子设备及计算机存储介质。

背景技术：

在机器人、无人驾驶机等无人行驶设备的运动过程中，尤其是在无人行驶设备运行在未知环境下时，需要对无人行驶设备的位姿进行确定，以此指导无人行驶设备的运行等。

现有的一种位姿确定方法是采用视觉slam(simultaneouslocalizationandmapping，同步定位与建图)方法来确定无人行驶设备在行驶过程中的旋转矩阵和平移矩阵，进而根据旋转矩阵和平移矩阵确定出无人行驶设备的位姿。

然而，现有的一种位姿确定方法中，基于视觉slam方法确定的位姿的准确度较低。

综上所述，如何提高确定的位姿的准确度是目前本领域技术人员亟待解决的问题。

技术实现要素：

本申请的目的是提供一种位姿确定方法，其能在一定程度上解决如何提高确定的位姿的准确度的技术问题。本申请还提供了一种位姿确定系统、电子设备及计算机可读存储介质。

为了实现上述目的，本申请提供如下技术方案：

一种位姿确定方法，应用于无人行驶设备，包括：

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

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

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

优选的，所述通过激光slam方法获取所述无人行驶设备在当前时刻下的平移矩阵，包括：

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

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

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

所述icp运算公式包括：

m←argmina(a-b')^tw(a-b')；

q＝[tdirq1q2]；b'←r⁰b+stdir；

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

优选的，所述基于所述特征匹配点对确定所述平移矩阵，包括：

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

优选的，所述基于所述特征匹配点对确定所述平移矩阵，包括：

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

优选的，所述基于所述特征匹配点对确定所述平移矩阵之前，还包括：

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

一种位姿确定系统，应用于无人行驶设备，包括：

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

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

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

优选的，所述第二获取模块包括：

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

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

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

所述icp运算公式包括：

m←argmina(a-b')^tw(a-b')；

q＝[tdirq1q2]；b'←r⁰b+stdir；

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

一种电子设备，应用于无人行驶设备，包括：

存储器，用于存储计算机程序；

处理器，用于执行所述计算机程序时实现如上任一所述位姿确定方法的步骤。

优选的，还包括：

与所述处理器连接的摄像设备，用于拍摄所述无人行驶设备在当前时刻下的第一帧图像以及上一时刻下的第二帧图像；

与所述处理器连接的激光扫描设备，用于扫描所述无人行驶设备在当前时刻下的第一帧点云以及上一时刻下的第二帧点云。

一种计算机可读存储介质，应用于无人行驶设备，所述计算机可读存储介质中存储有计算机程序，所述计算机程序被处理器执行时实现如上任一所述位姿确定方法的步骤。

本申请提供的一种位姿确定方法，应用于无人行驶设备，通过视觉slam方法获取无人行驶设备在当前时刻下的旋转矩阵；通过激光slam方法获取无人行驶设备在当前时刻下的平移矩阵；基于旋转矩阵和平移矩阵确定无人行驶设备在当前时刻下的位姿。本申请提供的一种位姿确定方法，通过视觉slam方法获取无人行驶设备在当前时刻下准确的旋转矩阵，通过激光slam方法获取无人行驶设备在当前时刻下准确的平移矩阵，最后再基于准确的旋转矩阵和平移矩阵获取无人驾驶设备在当前时刻下准确的位姿，与现有单独采用视觉slam方法的技术相比，准确率高。本申请提供的一种位姿确定系统、设备及计算机可读存储介质也解决了相应技术问题。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1为本申请实施例提供的一种位姿确定方法的第一流程图；

图2为本申请实施例提供的一种位姿确定方法的第二流程图；

图3为本申请提供的一种位姿确定方法的效果示意图；

图4为本申请实施例提供的一种位姿确定系统的结构示意图；

图5为本申请实施例提供的一种电子设备的结构示意图；

图6为本申请实施例提供的一种电子设备的另一结构示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

请参阅图1，图1为本申请实施例提供的一种位姿确定方法的第一流程图。

本申请实施例提供的一种位姿确定方法，应用于无人行驶设备，可以包括以下步骤：

步骤s101：通过视觉slam方法获取无人行驶设备在当前时刻下的旋转矩阵。

实际应用中，可以先通过视觉slam方法获取无人行驶设备在当前时刻下的旋转矩阵，无人行驶设备指的是无需人工驾驶即可行驶的设备，比如机器人、无人驾驶机等。

具体应用场景中，通过视觉slam方法获取旋转矩阵的详细过程可以参阅现有技术，比如在通过视觉slam方法获取无人行驶设备在当前时刻下的旋转矩阵时，可以先通过摄像设备拍摄无人行驶设备在当前时刻下的第一帧图像以及上一时刻下的第二帧图像，通过预设的描述子对第一帧图像和第二帧图像进行特征匹配，确定出第一帧图像和第二帧图像中的特征匹配点；之后采用对及约束方法和ransac(randomsampleconsensus)方法对特征匹配点进行处理，得到无人行驶设备在当前时刻下的旋转矩阵，比如，通过对极约束方法根据特征匹配点的像素位置确定出本质矩阵和基础矩阵，再根据本质矩阵和基础矩阵求解出旋转矩阵，再者，在根据本质矩阵和基础矩阵求解旋转矩阵时，可以采用8点法和奇异值分解(singularvaluedecomposition，svd)方法来求解旋转矩阵。当然，也可以通过无人行驶设备在当前时刻下的平移矩阵，但是通过视觉slam方法获得的平移矩阵只有两个自由度，精度不够，本申请并不采用视觉slam方法获取平移矩阵。

步骤s102：通过激光slam方法获取无人行驶设备在当前时刻下的平移矩阵。

实际应用中，在获取旋转矩阵之后，便可以通过激光slam方法获取无人行驶设备在当前时刻下的平移矩阵。

具体应用场景中，可以通过icp算法来获取平移矩阵，比如，通过激光扫描设备扫描无人行驶设备在当前时刻下的第一帧点云以及上一时刻下的第二帧点云；确定初始旋转矩阵和初始平移矩阵，比如通过sampleconsensusinitalalignment函数或者transformationestimationsvd函数确定初始旋转矩阵和初始平移矩阵；对于第一帧点云中的每一个点，用当前的旋转矩阵和平移矩阵在第二帧点云中寻找与第一帧点云中的点距离最近的点，将这两个点作为一对匹配点，直至得到所有的匹配点，点与点间的距离可以为欧式距离等；对于每一对匹配点，列一个经过该对匹配点的方程，通过误差公式确定出最优的平移矩阵值作为最终的平移矩阵值；误差公式可以为：

其中，e(x,y)表示误差，x、y分别表示第一帧点云和第二帧点云；r表示旋转矩阵，t表示平移矩阵；m表示匹配点的总个数。

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

实际应用中，在获取到旋转矩阵和平移矩阵后，便可以基于旋转矩阵和平移矩阵确定无人行驶设备在当前时刻下的位姿，具体过程可以参阅现有技术，本申请在此不再赘述。

本申请提供的一种位姿确定方法，应用于无人行驶设备，通过视觉slam方法获取无人行驶设备在当前时刻下的旋转矩阵；通过激光slam方法获取无人行驶设备在当前时刻下的平移矩阵；基于旋转矩阵和平移矩阵确定无人行驶设备在当前时刻下的位姿。本申请提供的一种位姿确定方法，通过视觉slam方法获取无人行驶设备在当前时刻下准确的旋转矩阵，通过激光slam方法获取无人行驶设备在当前时刻下准确的平移矩阵，最后再基于准确的旋转矩阵和平移矩阵获取无人驾驶设备在当前时刻下准确的位姿，与现有单独采用视觉slam方法的技术相比，准确率高。

请参阅图2，图2为本申请实施例提供的一种位姿确定方法的第二流程图。

本申请实施例提供的一种位姿确定方法可以包括以下步骤：

步骤s201：通过视觉slam方法获取无人行驶设备在当前时刻下的旋转矩阵。

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

步骤s203：通过icp运算公式对第一帧点云及第二帧点云进行特征匹配，得到特征匹配点对；

icp运算公式可以包括：

m←argmina(a-b')^tw(a-b')；

q＝[tdirq1q2]；b'←r⁰b+stdir；

其中，a表示第一帧点云；b表示第二帧点云；(m,b)表示特征匹配点对，m表示第一帧点云中与第二帧点云中特征相匹配的点；r⁰表示第一帧点云与第二帧点云的相对位置；tdir表示第一帧点云与第二帧点云间的距离；s表示tdir的尺度；α表示y轴和z轴组成的惩罚因子，α＞＞1；q1、q2、tdir相互垂直。

步骤s204：基于特征匹配点对确定平移矩阵。

具体应用场景中，在基于特征匹配点对确定平移矩阵时，可以判断||m-b'||的值是否小于预设值，若是，则将(m,b)作为目标匹配点对，基于目标匹配点对确定平移矩阵。

具体应用场景中，为了提高本申请提供的位姿确定方法的运行效率，在基于特征匹配点对确定平移矩阵时，可以通过ransac算法，基于特征匹配点对确定平移矩阵。

ransac算法的运算公式如下：

ttrue＝snewtdir；当tdir为点到点的距离时，当tdir为点到面的距离时，

具体应用场景中，为了满足用户等外界对位姿确定过程的管控，在基于特征匹配点对确定平移矩阵之前，还可以接收自身上传端口传输的预设值。

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

请参阅图3，图3为本申请提供的一种位姿确定方法的效果示意图，其中实线表示本申请，虚线表示现有icp方法，rotation表示旋转矩阵，translation表示平移矩阵，由图3可知，本申请提供的位姿确定方法的准确率较高。

本申请还提供了一种位姿确定系统及计算机可读存储介质，其均具有本申请实施例提供的一种位姿确定方法具有的对应效果。请参阅图4，图4为本申请实施例提供的一种位姿确定系统的结构示意图。

本申请实施例提供的一种位姿确定系统，应用于无人行驶设备，可以包括：

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

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

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

本申请实施例提供的一种位姿确定系统，应用于无人行驶设备，第二获取模块可以包括：

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

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

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

icp运算公式包括：

m←argmina(a-b')^tw(a-b')；

q＝[tdirq1q2]；b'←r⁰b+stdir；

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

本申请实施例提供的一种位姿确定系统，应用于无人行驶设备，第一确定子模块可以包括：

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

本申请实施例提供的一种位姿确定系统，应用于无人行驶设备，第一确定子模块可以包括：

第一确定单元，用于通过ransac算法，基于特征匹配点对确定平移矩阵。

本申请实施例提供的一种位姿确定系统，应用于无人行驶设备，还可以包括：

第一接收模块，用于第一确定子模块基于特征匹配点对确定平移矩阵之前，接收自身上传端口传输的预设值。

本申请还提供了一种电子设备及计算机可读存储介质，其均具有本申请实施例提供的一种位姿确定方法具有的对应效果。请参阅图5，图5为本申请实施例提供的一种电子设备的结构示意图。

本申请实施例提供的一种电子设备，应用于无人行驶设备，包括存储器201和处理器202，存储器201中存储有计算机程序，处理器202执行存储器201中存储的计算机程序时实现如下步骤：

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

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

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

本申请实施例提供的一种电子设备，应用于无人行驶设备，包括存储器201和处理器202，存储器201中存储有计算机程序，处理器202执行存储器201中存储的计算机程序时具体实现如下步骤：获取激光扫描设备扫描到的当前时刻下的第一帧点云以及上一时刻下的第二帧点云；通过icp运算公式对第一帧点云及第二帧点云进行特征匹配，得到特征匹配点对；基于特征匹配点对确定平移矩阵；

icp运算公式包括：

m←argmina(a-b')^tw(a-b')；

q＝[tdirq1q2]；b'←r⁰b+stdir；

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

本申请实施例提供的一种电子设备，应用于无人行驶设备，包括存储器201和处理器202，存储器201中存储有计算机程序，处理器202执行存储器201中存储的计算机程序时具体实现如下步骤：判断||m-b'||的值是否小于预设值，若是，则将(m,b)作为目标匹配点对，基于目标匹配点对确定平移矩阵。

本申请实施例提供的一种电子设备，应用于无人行驶设备，包括存储器201和处理器202，存储器201中存储有计算机程序，处理器202执行存储器201中存储的计算机程序时具体实现如下步骤：通过ransac算法，基于特征匹配点对确定平移矩阵。

本申请实施例提供的一种电子设备，应用于无人行驶设备，包括存储器201和处理器202，存储器201中存储有计算机程序，处理器202执行存储器201中存储的计算机程序时具体实现如下步骤：基于特征匹配点对确定平移矩阵之前，接收自身上传端口传输的预设值。

本申请实施例提供的一种电子设备，应用于无人行驶设备，还可以包括：

与处理器连接的摄像设备，用于拍摄无人行驶设备在当前时刻下的第一帧图像以及上一时刻下的第二帧图像；

与处理器连接的激光扫描设备，用于扫描无人行驶设备在当前时刻下的第一帧点云以及上一时刻下的第二帧点云。

请参阅图6，本申请实施例提供的另一种电子设备中还可以包括：与处理器202连接的输入端口203，用于传输外界输入的命令至处理器202；与处理器202连接的显示单元204，用于显示处理器202的处理结果至外界；与处理器202连接的通信模块205，用于实现电子设备与外界的通信。显示单元204可以为显示面板、激光扫描使显示器等；通信模块205所采用的通信方式包括但不局限于移动高清链接技术(hml)、通用串行总线(usb)、高清多媒体接口(hdmi)、无线连接：无线保真技术(wifi)、蓝牙通信技术、低功耗蓝牙通信技术、基于ieee802.11s的通信技术。

本申请实施例提供的一种计算机可读存储介质，应用于无人行驶设备，计算机可读存储介质中存储有计算机程序，计算机程序被处理器执行时实现如下步骤：

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

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

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

本申请实施例提供的一种计算机可读存储介质，应用于无人行驶设备，计算机可读存储介质中存储有计算机程序，计算机程序被处理器执行时具体实现如下步骤：获取激光扫描设备扫描到的当前时刻下的第一帧点云以及上一时刻下的第二帧点云；通过icp运算公式对第一帧点云及第二帧点云进行特征匹配，得到特征匹配点对；基于特征匹配点对确定平移矩阵；

icp运算公式包括：

m←argmina(a-b')^tw(a-b')；

q＝[tdirq1q2]；b'←r⁰b+stdir；

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

本申请实施例提供的一种计算机可读存储介质，应用于无人行驶设备，计算机可读存储介质中存储有计算机程序，计算机程序被处理器执行时具体实现如下步骤：判断||m-b'||的值是否小于预设值，若是，则将(m,b)作为目标匹配点对，基于目标匹配点对确定平移矩阵。

本申请实施例提供的一种计算机可读存储介质，应用于无人行驶设备，计算机可读存储介质中存储有计算机程序，计算机程序被处理器执行时具体实现如下步骤：通过ransac算法，基于特征匹配点对确定平移矩阵。

本申请实施例提供的一种计算机可读存储介质，应用于无人行驶设备，计算机可读存储介质中存储有计算机程序，计算机程序被处理器执行时具体实现如下步骤：基于特征匹配点对确定平移矩阵之前，接收自身上传端口传输的预设值。

本申请所涉及的计算机可读存储介质包括随机存储器(ram)、内存、只读存储器(rom)、电可编程rom、电可擦除可编程rom、寄存器、硬盘、可移动磁盘、cd-rom、或技术领域内所公知的任意其它形式的存储介质。

本申请实施例提供的一种位姿确定系统、电子设备及计算机可读存储介质中相关部分的说明请参见本申请实施例提供的一种位姿确定方法中对应部分的详细说明，在此不再赘述。另外，本申请实施例提供的上述技术方案中与现有技术中对应技术方案实现原理一致的部分并未详细说明，以免过多赘述。

还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

对所公开的实施例的上述说明，使本领域技术人员能够实现或使用本申请。对这些实施例的多种修改对本领域技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本申请的精神或范围的情况下，在其它实施例中实现。因此，本申请将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。