CN117346787A - 基于多传感器融合定位的机器人定位方法及其机器人 - Google Patents

Abstract

本发明涉及一种基于多传感器融合的机器人定位方法，在构建多传感器融合SLAM定位时，加入对激光传感器所处的环境判断以及对视觉传感器所处的环境判断，使得多传感器融合算法能够更好的适应环境，从而提高对环境数据的利用。同时在根据环境判断的基础上，利用视觉图像数据信息以及激光位移数据信息进行回环检测，所有节点node的位姿以及子图的位姿，在每次检测到回环之后都会激活全局图优化，从而利用所述优化函数对机器人的位姿数据信息进行优化，矫正机器人位姿，从而提高机器人在复杂易变的环境下的定位精度。

Description

基于多传感器融合定位的机器人定位方法及其机器人

技术领域

本发明涉及机器人定位技术领域，特别是涉及一种基于多传感器融合定位的机器人定位方法及其机器人。

背景技术

随着科学技术的不断发展，涌现了多种科技产品来方便人们的工作生活，能够自主移动的机器人系统应运而生，其能够代替人类进行自主工作，例如服务性机器人如扫地机、割草机等，智能机器人如自动送餐机、自动驾驶机等。而且随着机器人性能的不断完善，机器人的应用范围不断扩展，在工业、农业、医疗、服务等行业都得到了广泛的应用，而且在城市安全、国防以及空间探测领域都得到了很好的应用。

无论是服务机器人还是智能机器人，都需要准确地获取机器人当前所处的位置，才能够进行其他任务，而SLAM(即时定位和建图)技术是机器人能够自主移动的基础，因此涌现了很多对于SLAM技术的研究来用于实现对机器人的定位。现有技术中，通常采用滤波技术对机器人的位姿进行估计，或利用多传感器数据融合将传感器的优势互补构建SLAM系统，从而提高机器人的定位精度。

然而，现有的利用多传感器融合方法只是简单的将各传感器数据进行融合，导致机器人在一些复杂场景下经常会出现定位漂移的情况，从而使得SLAM系统定位不准确，导致对机器人的定位精度低下。

发明内容

为了解决上述问题，本发明提供了一种新的基于多传感器融合定位的机器人定位方法及其机器人，在构建SLAM时加入对多传感器所处的环境因素的判断，使得多传感器融合能够更好的适应环境，提高对环境数据的利用，进一步提高机器人在复杂易变的环境下的定位精度。

在一个实施例中，本发明提供了一种基于多传感器融合定位的机器人定位方法，包括：

S1：根据视觉传感器获取的视觉图像数据信息计算所述视觉图像数据信息的视觉权重；

S2：根据激光传感器获取的激光位移数据信息计算所述激光位移数据信息的激光权重；

S3：分别根据不同的视觉权重和激光权重的关系，计算机器人的位姿数据信息；

S4：利用所述视觉图像数据信息或所述激光位移数据信息进行回环检测；

S5：在检测到回环时，对所述机器人的位姿数据信息进行全局图优化得到优化后的位姿数据信息，并根据所述优化后的位姿数据信息对机器人进行定位。

在本实施例中，进一步地，所述S1包括：

S11：在获取的视觉图像数据信息超过一初始阈值条件下，提取所述视觉图像数据信息中的特征信息；

S12：将所述视觉图像数据信息中的特征信息的数量n_orb加1进行更新，得到更新后的视觉图像数据信息中的特征信息的总数量；

S13：判断更新后的视觉图像数据信息中的特征信息的总数量是否大于一预设的提取特征信息数且所述视觉图像数据信息是否全部处理完成；若否，则返回继续执行步骤S11，直至所述特征信息的总数量不小于所述视觉图像数据信息中的预设提取特征信息数且所述视觉图像数据信息全部处理完成；若是，则进入步骤S14；

S14：计算更新后的视觉图像数据信息中的特征信息的总数量与预设的提取特征信息数N的比值，并与第一预设阈值进行比较；若计算得到的比值大于所述第一预设阈值，则判断当前环境适合所述视觉传感器，进入步骤S15a；否则判断当前环境不适合所述视觉传感器，进入步骤S15b；

S15a：将所述视觉图像数据信息的视觉权重设为1；

S15b：根据所述特征信息的总数量、预设的提取特征信息数以及第一预设阈值确定所述视觉图像数据信息的视觉权重，其计算公式如下：

其中，ω_v为视觉传感器的视觉权重，n_orb为更新后的特征信息的总数量，N为预设的提取特征信息数，T_v为第一预设阈值。

在本实施例中，进一步地，所述S2包括：

S21：在获取第K束激光位移数据信息的扫描距离小于最大扫描距离的条件下，获取该束激光击中障碍物的点的数量；其中，K为一帧激光位移数据信息中的任意一束激光束；

S22：将第K束激光位移数据信息击中障碍物的点的数量n₁加1进行更新，得到更新后的击中障碍物的点的总数量；

S23：判断是否扫描完成一帧激光位移数据信息中的所有激光束，若否，则返回继续执行步骤S21，继续对激光位移数据信息中击中障碍物的点的数量进行更新，直至扫描完成一帧激光位移数据信息的所有激光束；若是，则进入步骤S24；

S24：计算更新后的击中障碍物的点的总数量与一帧激光位移数据信息的激光束的数量的比例，并与第二预设阈值进行比较；若计算得到的比例大于所述第二预设阈值，则判断当前环境适合激光传感器，进入步骤S25a；否则判断当前环境不适合激光传感器，进入步骤S25b；

S25a：将所述激光位移数据信息的激光权重设为1；

S25b：根据所述激光束击中障碍物的点的总数量、一帧激光位移数据信息中激光束的数量以及所述第二预设阈值确定所述激光位移数据信息的激光权重，其计算公式如下：

其中，ω_l为激光传感器的权重信息，n₁为更新后的激光束击中障碍物的点的总数量，为一帧激光位移数据信息中激光束的数量，T_l为第二预设阈值。

在本实施例中，进一步地，所述S3中，根据以下4中不同的权重组合关系，分别进行所述机器人的位姿数据信息的计算处理：

S3a：若当前环境适合视觉传感器不适合激光传感器，构建最小化重投影误差得到第一位姿数据信息；所述第一位姿数据信息为所述机器人的位姿数据信息；

S3b：若当前环境适合激光传感器不适合视觉传感器，构建最大化栅格概率得到第二位姿数据信息；所述第二位姿数据信息为所述机器人的位姿数据信息；其计算公式如下：

其中，ξ表示相关性扫描得到的位姿估计值，T_ξ表示位姿估计值描述的坐标转换，T_ξh_k表示第k个击中障碍物的点在地图坐标系下的坐标，M_smooth()为栅格概率，用于将离散采样线性化得到该栅格的概率；

S3c：若当前环境既适合视觉传感器也适合激光传感器，则分别获取当前环境适合视觉传感器时的第一位姿数据信息T_η以及当前环境适合激光传感器的第二位姿数据信息T_ξ，并根据所述第一位姿数据信息T_η以及所述第二位姿数据信息T_ξ计算得到所述机器人的位姿数据信息；其计算公式如下：

S3d：若当前环境既不适合视觉传感器也不适合激光传感器，则分别获取视觉图像数据信息的视觉权重以及激光位移数据信息的激光权重，根据所述视觉权重以及激光权重对所述视觉图像数据信息以及激光位移数据信息进行联合优化，得到所述机器人的位姿数据信息；其计算公式如下：

在本实施例中，进一步地，所述S3a包括：

S3a1：获取重投影误差；其重投影误差计算公式如下：

r_ij＝u_ij-Π(T_CBT_i ^-1⊕x_j)

其中，r_ij表示重投影误差，u_ij表示3D路标点x_j在帧i上的观测投影，T_CB表示视觉传感器的外参，T_i ^-1⊕x_j表示将坐标x_j转换到imu坐标系下，T_CBT_i ^-1⊕x_j表示将imu坐标系下的坐标转换到视觉传感器坐标系下；Π()表示将视觉传感器坐标系下的坐标投影到平面坐标系；

S3a2：根据获取的所述重投影误差，构建最小化重投影误差的目标函数，通过所述最小化重投影误差的目标函数计算得到所述第一位姿数据信息；其目标函数表示如下：

其中，为关键帧的集合，χ为3D路标点的集合，ρ_Hub()为损失函数，其作用是为降低由于误匹配二导致的重投影误差，Σ_ij为信息矩阵，用来描述重投影误差的可信度，κ^j表示观测到第j个3D路标点的关键帧的集合。

在本实施例中，进一步地，所述S4包括：

S4a：若当前环境适合视觉传感器，则利用所述视觉图像数据信息进行回环检测；

S4b：若当前环境不适合视觉传感器，则利用所述激光位移数据信息进行回环检测。

在本实施例中，进一步地，所述S5包括：

S51：在检测到回环时，获取子图中节点之间的约束残差项；其中，子图中节点之间的约束残差项公式如下：

e_ij＝z_ij-h(pose_i,pose_j)

其中，pose_i，pose_j分别表示子图和节点的世界位姿，其子图和节点的世界位姿是根据所述视觉图像数据信息或者激光位移数据信息在考虑环境下所得到的机器人的位姿数据信息，h(pose_i,pose_j)表示第j个节点在第i个子图的坐标，h()表示坐标之间的转换关系，转换得到节点与子图之间的实际距离，z_ij表示观测得到的节点与子图之间的相对距离，e_ij表示观测距离与实际值之间的约束残差项；

S52：对所述约束残差项进行计算，得到约束误差；其中，节点之间的约束误差公式如下：

其中，分别表示子图和节点的转换矩阵，/>表示节点在子图中的相对位姿在世界坐标系下的表示，/>表示子图的局部坐标系在世界坐标系下的表示，通过求逆得到的/>则是世界坐标系相对于子图的局部坐标系的姿态变换矩阵，/>表示节点在子图中的相对位姿，/>分别表示节点和子图的姿态角，因此/>表示通过节点和子图的绝对位姿计算得到节点和子图之间的相对位姿，ξ_i,j表示通过视觉传感器或激光传感器测量得到的相对位姿。

S53：对上述约束误差进行加和得到整体误差，并利用代价函数以及优化函数对所述整体误差进行优化，得到优化后的位姿数据信息；其计算公式如下：

其代价函数如下：

其中，ρ()是损失函数；

其优化函数如下：

在另一实施例中，本发明还提供了一种基于多传感器融合定位的机器人，包括：

一视觉传感器，用于获取视觉图像数据信息；

一激光传感器，用于获取激光位移数据信息；

一定位处理器，分别与所述视觉传感器以及所述激光传感器进行连接，用于基于上述所提供的机器人定位方法，根据视觉传感器获取的视觉图像数据信息和激光传感器获取的激光位移数据信息对机器人进行定位。

在另一实施例中，本发明还提供了一种机器人定位装置，包括定位处理器、存储器以及存储在所述存储器上并可被所述定位处理器执行的计算机程序，其中所述计算机程序被所述处理器执行时，实现上述所提供的机器人定位方法。

在另一实施例中，本发明还提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，其中所述计算机程序被定位处理器执行时，实现上述所提供的机器人定位方法。

本发明提供的基于多传感器融合的机器人定位方法，在构建多传感器融合SLAM定位时，加入对激光传感器所处的环境判断以及对视觉传感器所处的环境判断，使得多传感器融合算法能够更好的适应环境，从而提高对环境数据的利用。同时在根据环境判断的基础上，利用视觉图像数据信息以及激光位移数据信息进行回环检测，所有节点的位姿以及子图的位姿，在每次检测到回环之后都会激活全局图优化，从而利用所述优化函数对机器人的位姿数据信息进行优化，矫正机器人位姿，从而提高机器人在复杂易变的环境下的定位精度。

为了更清晰的理解本发明，以下将结合附图说明阐述本发明的具体实施方式。

附图说明

图1为本发明中一种基于多融合传感器的机器人的结构示意图；

图2为本发明中一种基于多融合传感器定位的机器人定位方法的流程示意图；

图3为本发明中上述图2机器人定位方法的全局优化示意图。

具体实施方式

现有技术中，基于多传感器融合定位的机器人及其定位方法中，多传感器融合算法只是简单的将各传感器数据进行融合，并未考虑到环境变化的因素，导致机器人在一些复杂场景下经常会出现定位漂移的情况，使得SLAM系统定位以及建图的精度低下。本申请在构建多传感器融合SLAM定位时，加入对激光传感器所处的环境判断以及对视觉传感器所处的环境判断，使得多传感器融合算法能够更好的适应环境，从而提高对环境数据的利用，进一步提高机器人在复杂易变的环境下的定位精度。

请参照图1，本发明的机器人1包括设置在机器人内的一视觉传感器、一激光传感器和一定位处理器。其中，所述视觉传感器用于获取视觉图像数据信息，所述激光传感器用于获取激光位移数据信息，所述定位处理器分别与所述视觉传感器以及所述激光传感器进行连接，用于根据视觉传感器获取的视觉图像数据信息和激光传感器获取的激光位移数据信息共同判断机器人所处的当前位置。

具体地，请参照图2，其为所述定位处理器的定位方法流程图。其总体流程设计构思为，先分别执行步骤S1根据视觉传感器获取的视觉图像数据信息计算所述视觉图像数据信息的视觉权重，和步骤S2根据激光传感器获取的激光位移数据信息计算激光位移数据信息的激光权重；再执行步骤S3分别根据不同的视觉权重和激光权重的关系，计算机器人的位姿数据信息；并在计算得到机器人的位姿数据信息中，进一步执行步骤S4利用所述激光位移数据信息或视觉图像数据数据信息进行回环检测以及步骤S5在检测到回环时，对所述机器人的位姿数据信息进行全局图优化得到优化后的位姿数据信息，并根据所述优化后的位姿数据信息对机器人进行定位。

以下进行详细说明：

步骤S1：根据视觉传感器获取的视觉图像数据信息计算所述视觉图像数据信息的视觉权重；

当视觉传感器所处的环境为纹理较少或者黑暗环境时，视觉传感器所能获取的有用信息较少，则不利于视觉传感器获取对应的视觉图像数据信息，说明此时当前环境是不利于视觉传感器的。基于此，在本实施例中，采用提取ORB(Oriented FAST+Rotated BRIEF)特征信息来进行视觉图像数据信息的关键帧匹配以及后续的位姿数据信息的计算。而在提取ORB特征信息时，通常采用的是自适应阈值的方法，目的是为了提取得到更多的ORB特征信息，然而采用自适应阈值的方法使得在降低阈值后提取得到的ORB特征信息的质量较差。因此在本申请中，为了提高得到的ORB特征信息的质量，先设定一个初始阈值T₁和第一预设阈值T_v，在视觉传感器环境判断中，以初始阈值T₁为界限，在该初始阈值T₁下提取视觉图像数据信息中的特征信息作为环境因素判断的基础。

具体地，所述步骤S1包括：

S11：在获取的视觉图像数据信息超过一初始阈值条件下，提取所述视觉图像数据信息中的特征信息；

在该步骤中，具体地，提取视觉图像数据信息中的ORB特征信息。

S12：将所述视觉图像数据信息中的特征信息的数量n_orb加1进行更新，得到更新后的视觉图像数据信息中的特征信息的总数量；

S13：判断更新后的视觉图像数据信息中的特征信息的总数量n_orb是否大于一预设的提取特征信息数N，且所述视觉图像数据信息是否全部处理完成；若否，则返回继续执行步骤S11，直至所述特征信息的总数量n_orb不小于所述视觉图像数据信息中的预设提取特征信息数N且所述视觉图像数据信息全部处理完成；若是，则进入步骤S14；

S14：计算更新后的视觉图像数据信息中的特征信息的总数量n_orb与预设的提取特征信息数N的比值得到并与第一预设阈值T_v进行比较；若/>判断当前环境适合所述视觉传感器，进入步骤S15a；否则判断当前环境不适合所述视觉传感器，进入步骤S15b。

在上述步骤中，如前所述，在视觉传感器环境判断中，在初始阈值T₁下提取的ORB特征信息才是真正的特征信息，因此当更新后的所述ORB特征信息的总数量不小于所述视觉图像数据信息中的预设提取特征信息数N且所述视觉图像数据信息全部处理完成时，计算ORB特征信息的总数量n_orb与预设提取特征信息数N的比例并与第一预设阈值T_v进行比较，若/>说明在当前环境下，所提取的视觉图像数据信息中的ORB特征信息的质量较高，则判断当前环境适合所述视觉传感器；否则，则判断当前环境不适合所述视觉传感器。

S15a：将所述视觉传感器的视觉权重ω_v设为1；

S15b：根据所述ORB特征信息的总数量、预设的提取特征信息数N以及第一预设阈值T_v确定所述视觉传感器的视觉权重，其计算公式如下：

其中，ω_v为视觉传感器的视觉权重，n_orb为更新后的ORB特征信息的数量，N为预设提取特征信息数，T_v为第一预设阈值。

步骤S2：根据激光传感器获取的激光位移数据信息计算所述激光位移数据信息的激光权重；

当激光传感器所处的环境为障碍物很少的环境时，激光传感器所能获取的有用信息较少，此时说明该环境不适合激光传感器。在该环境下，激光传感器扫描一帧获取的激光位移数据信息中，击中障碍物的点的数量与一帧中击中和未击中障碍物的点的总数量的比例也会越来越小。因此，在本实施例中，我们设置一个第二预设阈值T_l，当击中障碍物的点的数量的比例小于第二预设阈值T_l时，说明在当前环境下系统所获取的激光数据过少，利用激光数据所提取的特征信息的数量也过少且质量低下，此时说明当前环境不适合激光传感器。优选地，所述第二预设阈值T_l通常设置为0.3。

具体地，所述步骤S2包括：

S21：在获取第K束激光位移数据信息的扫描距离Z_k小于最大扫描距离d的条件下，获取该束激光击中障碍物的点的数量n₁；其中，K为一帧激光位移数据信息中的任意一束激光束；

具体地，在激光传感器扫描所获取的一帧激光位移数据信息中，会获取到多束激光位移数据信息，在该步骤中，获取其中任意一束激光位移数据信息击中障碍物的点的数量n₁以及激光位移数据信息的扫描距离，方便起见，此处获取第K束激光位移数据信息的扫描距离Z_k；

S22：将第K束激光位移数据信息击中障碍物的点的数量n₁加1进行更新，得到更新后的击中障碍物的点的总数量。

S23：判断是否扫描完成一帧激光位移数据信息中的所有激光束，若否，则返回继续执行步骤S21，对激光位移数据信息中击中障碍物的点的数量进行更新，直至扫描完成一帧激光位移数据信息的所有激光束；若是，则进入步骤S24；

具体地，设置θ为两束激光束之间相隔的角度，激光扫描一圈为π，则为一帧激光位移数据信息的激光束数量，因此若判断/>则返回步骤S31继续执行，对激光位移数据信息中击中障碍物的点的数量进行更新，直至扫描完成一帧激光位移数据信息的所有激光束。

S24：计算更新后的击中障碍物的点的总数量n₁与一帧激光位移数据信息的激光束的数量的比例，得到/>并与第二预设阈值T_l进行比较；若/>则判断当前环境适合激光传感器，进入步骤S25a；否则判断当前环境不适合激光传感器，进入步骤S25b；

其中，n₁为激光束击中障碍物的点的数量，π为激光位移数据信息扫描一圈的角度，为一帧激光位移数据信息中激光束的数量。

S25a：将所述激光传感器的激光权重ω_l设为1；

S25b：根据所述激光束击中障碍物的点的总数量n₁、一帧激光位移数据信息中激光束的数量以及所述第二预设阈值T_l确定所述激光传感器的激光权重，其计算公式如下：

其中，ω_l为激光传感器的权重信息，n₁为更新后的激光束击中障碍物的点的数量，为一帧激光位移数据信息中激光束的数量，T_l为第二预设阈值。

步骤S3：根据不同的视觉权重和激光权重的关系，计算机器人的位姿数据信息；

具体地，根据以下4中不同的权重组合关系，分别进行机器人的位姿数据信息的计算处理：

S3a：若当前环境适合视觉传感器不适合激光传感器，构建最小化重投影误差得到第一位姿数据信息；所述第一位姿数据信息为所述机器人的位姿数据信息；

进一步地，所述S3a具体包括：

S3a1：获取重投影误差；其重投影误差计算公式如下：

其中，r_ij表示重投影误差，u_ij表示3D路标点x_j在帧i上的观测投影，T_CB表示视觉传感器的外参，表示将坐标x_j转换到imu坐标系下，/>表示将imu坐标系下的坐标转换到视觉传感器坐标系下。Π()表示将视觉传感器坐标系下的坐标投影到平面坐标系。

通过上述重投影误差计算公式，可得到观测投影u_ij与计算投影之差，进而得到重投影误差r_ij。

S3a2：根据获取的所述重投影误差，构建最小化重投影误差的目标函数，通过所述最小化重投影误差的目标函数计算得到所述第一位姿数据信息；其目标函数表示如下：

其中，δ_k为关键帧的集合，χ为3D路标点的集合，ρ_Hub()为损失函数，其作用是为降低由于误匹配二导致的重投影误差，Σ_ij为信息矩阵，用来描述重投影误差的可信度，κ^j表示观测到第j个3D路标点的关键帧的集合。

S3b：若当前环境适合激光传感器不适合视觉传感器，构建最大化栅格概率得到第二位姿数据信息；所述第二位姿数据信息为所述机器人的位姿数据信息；其计算公式如下：

其中，ξ表示相关性扫描得到的位姿估计值，T_ξ表示位姿估计值描述的坐标转换，T_ξh_k表示第k个击中障碍物的点在地图坐标系下的坐标，M_smooth()为栅格概率，用于将离散采样线性化得到该栅格的概率。

在该步骤中，通过构建最小二乘函数，即最大化栅格概率M_smooth(T_ξh_k)即可得到第二位姿优化数据信息。

S3c：若当前环境既适合视觉传感器也适合激光传感器，则分别获取当前环境适合视觉传感器时的第一位姿数据信息T_η以及当前环境适合激光传感器的第二位姿数据信息T_ξ，并根据所述第一位姿数据信息T_η以及所述第二位姿数据信息T_ξ计算得到所述机器人的位姿数据信息；其计算公式如下：

在该步骤中，如果当前环境既适合视觉传感器也适合激光传感器，则将分别获取的第一位姿数据信息T_η以及当前环境适合激光传感器的第二位姿数据信息T_ξ求平均值，从而得到机器人的位姿数据信息。

S3d：若当前环境既不适合视觉传感器也不适合激光传感器，则分别获取视觉图像数据信息的视觉权重以及激光位移数据信息的激光权重，根据所述视觉权重以及激光权重对所述视觉图像数据信息以及激光位移数据信息进行联合优化，得到所述机器人的位姿数据信息；其计算公式如下：

步骤S4：利用所述视觉图像数据信息或所述激光位移数据信息或进行回环检测；

在该步骤中，由于机器人在定位建图的过程中，可能有累积误差，因此机器人从起始点出发绕一圈再起始点时通常会由于误差的关系，在定位建图中，机器人所处的位置并不在起始点。为了减少累积误差，优化机器人的位姿数据信息，本实施例中利用视觉图像数据信息或激光位移数据信息进行回环检测。具体地，根据对激光传感器以及视觉传感器的环境判断，选择相应的传感器数据进行回环检测。在实际环境中，往往包含的纹理信息比较丰富，采用视觉图像数据信息进行回环检测的精确度要比利用激光位移数据信息进行回环检测的精确度更高，因此优选地，只要当前环境适合视觉传感器，则利用视觉传感数据信息进行回环检测。

具体地，若当前环境适合视觉传感器，则执行步骤S4a，利用所述视觉图像数据信息进行回环检测；可选地，利用视觉图像数据信息进行回环检测时，采用DBOW2词袋法进行回环检测。若当前环境不适合视觉传感器，则执行步骤S4b，则利用激光位移数据信息进行回环检测；可选地，利用激光位移数据信息进行回环检测时，采用分支定界法进行回环检测。

进一步地，在利用视觉图像数据信息或激光位移数据信息进行回环检测时，若检测到回环，说明当前点的位置和起始点的位置是一样的，此时会进行全局图优化，调整机器人在当前点以及在整个地图轨迹上的位姿数据信息，使得轨迹闭合，从而有效减少累积误差，能够对得到的机器人的位姿数据信息进行优化，提高机器人定位精度。基于此，在所述步骤S4之后，所述定位方法还包括：

步骤S5：在检测到回环时，对所述机器人的位姿数据信息进行全局图优化，得到优化后的位姿数据信息，并根据所述优化后的位姿数据信息对机器人进行定位。

在该步骤中，当检测到回环后，触发后端全局图优化。请参考图3，在全局图优化中以子图和关键帧为节点，以节点之间的约束为边，其全局图优化如图3所示。

具体地，所述S5包括：

S51：在检测到回环时，获取子图中节点之间的约束残差项；其中，节点node之间的约束残差项公式如下：

e_ij＝z_ij-h(pose_i,pose_j)

其中，pose_i，pose_j分别表示子图submap和节点node的世界位姿，其子图submap和节点node的位姿是根据前述机器人定位时由视觉图像数据信息或者激光位移数据信息在考虑环境下所得到的机器人位姿数据信息，其根据环境的不同而得到，h(pose_i,pose_j)表示第j个node在第i个submap的坐标，h()表示坐标之间的转换关系，转换得到node与submap之间的实际距离，z_ij表示观测得到的node与submap之间的相对距离，e_ij表示观测距离与实际值之间的约束误差。

通常情况下，我们认为观测距离z_ij是正确的，因此通过调整pose_i和pose_j的值，从而使得e_ij最小，进而调整后得到的位姿优化数据信息也就更加准确，提高对机器人定位的精确度。

S52：对所述约束残差项进行计算，得到节点之间的约束误差；其中，节点node之间的约束误差计算公式如下：

其中，分别表示子图submap和节点node的转换矩阵，/>表示node在submap中的相对位姿在世界坐标系下的表示，/>表示submap的局部坐标系在世界坐标系下的表示，通过求逆得到的/>则是世界坐标系相对于submap局部坐标系的姿态变换矩阵，表示node在submap中的相对位姿，/>分别表示node和submap的姿态角，因此/>表示通过node和submap的绝对位姿计算得到node和submap之间的相对位姿，ξ_i,j表示通过视觉传感器或激光传感器测量得到的相对位姿，通过调整/>和/>能够使得约束误差/>最小。

在本实施例中，通过对submap的相对位姿和node的相对位姿，能够使得约束误差项最小，即计算得到的相对位姿尽可能地更靠近测量得到的相对位姿，从而消除累积误差。

S53：对上述约束误差进行加和得到整体误差，并利用代价函数以及优化函数对所述整体误差进行优化，得到优化后的位姿数据信息；其计算公式如下：

其代价函数如下：

其中，ρ()是损失函数，避免异常点导致的错误匹配的约束带来的影响。

进一步地，利用优化函数对上述代价函数进行优化，其优化函数如下：

本实施例中，在进行全局图优化之后，所有节点node的位姿以及子图的位姿，在每次检测到回环之后都会激活全局图优化，从而利用所述优化函数对机器人的位姿数据信息进行优化，矫正机器人位姿，从而进一步提高机器人的定位精度，实现对机器人的精准定位。

基于上述机器人定位方法，本申请还提供了一种基于多传感器融合的机器人定位装置，所述机器人定位装置包括定位处理器、存储器以及存储在所述存储器上并可被所述处理器执行的计算机程序，其中所述计算机程序被所述处理器执行时，实现上述基于多传感器融合的机器人定位方法的步骤。

基于上述机器人定位方法，本申请还提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，其中所述计算机程序被处理器执行时，实现上述基于多传感器融合的机器人定位方法的步骤。

(一)验证分析

为了验证在复杂环境下本申请所构建的SLAM算法性能更优于其他算法，我们采用euroc数据集作为实验数据。euroc数据集主要录制于室内工厂中，分为简单、中等、困难三个等级。同时，该数据集采用手持设备进行录制，采用的数据轨迹没有规律可循，符合复杂环境的场景。为了更好的测试本申请所构建的SLAM算法的性能，我们采用三个场景，每个场景采用简单、中等、困难三个场景序列。

请参阅表1，采用本申请所构建的SLAM系统进行测试，比较了我们构建的SLAM以及其他三种算法的预测实验，其结果如表1所示

表1四种算法在三个场景序列中的约束误差评估

通过表1可以直观地看出，通过本申请构建的SLAM系统，其在各个方面的性能都优于其他几种算法，而且相对于LVI-SAM算法，越是复杂的环境，本申请构建的SLAM性能越好，能够有效的提升定位精度。通过加入对环境的判断，能够在环境改变的情况下也能够提取到质量较高的特征信息，例如在视觉传感器突然转变方向导致图片模糊的情况下，利用本申请的方法同样能够提取到质量高的特征信息，从而在视觉传感器突然转变方向时也能够很好的贴合真实轨迹，减少累积误差，提高机器人在复杂环境下的定位精度。

(二)评估指标

为了评估定位轨迹的精确度，本申请中利用均方根误差(RMSE)以及评级误差(MEAN)进行计算，其具体计算公式如下：

其中，T_i分别表示机器人在时间i的优化位姿和真实位姿。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (10)

1.一种基于多传感器融合定位的机器人定位方法，其特征在于，包括：

S1：根据视觉传感器获取的视觉图像数据信息计算所述视觉图像数据信息的视觉权重；

S2：根据激光传感器获取的激光位移数据信息计算所述激光位移数据信息的激光权重；

S3：分别根据不同的视觉权重和激光权重的关系，计算机器人的位姿数据信息；

S4：利用所述视觉图像数据信息或所述激光位移数据信息进行回环检测；

S5：在检测到回环时，对所述机器人的位姿数据信息进行全局图优化得到优化后的位姿数据信息，并根据所述优化后的位姿数据信息对机器人进行定位。

2.根据权利要求1所述的机器人定位方法，其特征在于，所述S1包括：

S11：在获取的视觉图像数据信息超过一初始阈值条件下，提取所述视觉图像数据信息中的特征信息；

S12：将所述视觉图像数据信息中的特征信息的数量加1进行更新，得到更新后的视觉图像数据信息中的特征信息的总数量；

S13：判断更新后的视觉图像数据信息中的特征信息的总数量是否大于一预设的提取特征信息数且所述视觉图像数据信息是否全部处理完成；若否，则返回继续执行步骤S11，直至所述特征信息的总数量不小于所述视觉图像数据信息中的预设提取特征信息数且所述视觉图像数据信息全部处理完成；若是，则进入步骤S14；

S14：计算更新后的视觉图像数据信息中的特征信息的总数量与预设的提取特征信息数的比值，并与第一预设阈值进行比较；若计算得到的比值大于所述第一预设阈值，则判断当前环境适合所述视觉传感器，进入步骤S15a；否则判断当前环境不适合所述视觉传感器，进入步骤S15b；

S15a：将所述视觉图像数据信息的视觉权重设为1；

S15b：根据所述特征信息的总数量、预设的提取特征信息数以及第一预设阈值确定所述视觉图像数据信息的视觉权重，其计算公式如下：

其中，ω_v为视觉传感器的视觉权重，n_orb为更新后的特征信息的总数量，N为预设的提取特征信息数，T_v为第一预设阈值。

3.根据权利要求1所述的机器人定位方法，其特征在于，所述S2包括：

S21：在获取第K束激光位移数据信息的扫描距离小于最大扫描距离的条件下，获取该束激光击中障碍物的点的数量；其中，K为一帧激光位移数据信息中的任意一束激光束；

S22：将第K束激光位移数据信息击中障碍物的点的数量n₁加1进行更新，得到更新后的击中障碍物的点的总数量；

S23：判断是否扫描完成一帧激光位移数据信息中的所有激光束，若否，则返回继续执行步骤S21，继续对激光位移数据信息中击中障碍物的点的数量进行更新，直至扫描完成一帧激光位移数据信息的所有激光束；若是，则进入步骤S24；

S24：计算更新后的击中障碍物的点的总数量与一帧激光位移数据信息的激光束的数量的比例，并与第二预设阈值进行比较；若计算得到的比例大于所述第二预设阈值，则判断当前环境适合激光传感器，进入步骤S25a；否则判断当前环境不适合激光传感器，进入步骤S25b；

S25a：将所述激光位移数据信息的激光权重设为1；

S25b：根据所述激光束击中障碍物的点的总数量、一帧激光位移数据信息中激光束的数量以及所述第二预设阈值确定所述激光位移数据信息的激光权重，其计算公式如下：

其中，ω_l为激光传感器的权重信息，n₁为更新后的激光束击中障碍物的点的总数量，为一帧激光位移数据信息中激光束的数量，T_l为第二预设阈值。

4.根据权利要求1所述的机器人定位方法，其特征在于，所述S3中，根据以下4中不同的权重组合关系，分别进行所述机器人的位姿数据信息的计算处理：

S3a：若当前环境适合视觉传感器不适合激光传感器，构建最小化重投影误差得到第一位姿数据信息；所述第一位姿数据信息为所述机器人的位姿数据信息；

S3b：若当前环境适合激光传感器不适合视觉传感器，构建最大化栅格概率得到第二位姿数据信息；所述第二位姿数据信息为所述机器人的位姿数据信息；其计算公式如下：

其中，ξ表示相关性扫描得到的位姿估计值，T_ξ表示位姿估计值描述的坐标转换，T_ξh_k表示第k个击中障碍物的点在地图坐标系下的坐标，M_smooth()为栅格概率，用于将离散采样线性化得到该栅格的概率；

S3c：若当前环境既适合视觉传感器也适合激光传感器，则分别获取当前环境适合视觉传感器时的第一位姿数据信息以及当前环境适合激光传感器的第二位姿数据信息，并根据所述第一位姿数据信息以及所述第二位姿数据信息计算得到所述机器人的位姿数据信息；其计算公式如下：

其中，T_η为当前环境适合视觉传感器时的第一位姿数据信息，T_ξ为当前环境适合激光传感器的第二位姿数据信息；

S3d：若当前环境既不适合视觉传感器也不适合激光传感器，则分别获取视觉图像数据信息的视觉权重以及激光位移数据信息的激光权重，根据所述视觉权重以及激光权重对所述视觉图像数据信息以及激光位移数据信息进行联合优化，得到所述机器人的位姿数据信息；其计算公式如下：

5.根据权利要求4所述的机器人定位方法，其特征在于，所述S3a包括：

S3a1：获取重投影误差；其重投影误差计算公式如下：

其中，r_ij表示重投影误差，u_ij表示3D路标点x_j在帧i上的观测投影，T_CB表示视觉传感器的外参，表示将坐标x_j转换到imu坐标系下，/>表示将imu坐标系下的坐标转换到视觉传感器坐标系下；Π()表示将视觉传感器坐标系下的坐标投影到平面坐标系；

S3a2：根据获取的所述重投影误差，构建最小化重投影误差的目标函数，通过所述最小化重投影误差的目标函数计算得到所述第一位姿数据信息；其目标函数表示如下：

其中，为关键帧的集合，χ为3D路标点的集合，ρ_Hub()为损失函数，其作用是为降低由于误匹配二导致的重投影误差，Σ_ij为信息矩阵，用来描述重投影误差的可信度，κ^j表示观测到第j个3D路标点的关键帧的集合。

6.根据权利要求1所述的机器人定位方法，其特征在于，所述S4包括：

S4a：若当前环境适合视觉传感器，则利用所述视觉图像数据信息进行回环检测；

S4b：若当前环境不适合视觉传感器，则利用所述激光位移数据信息进行回环检测。

7.根据权利要求1-6任一项所述的机器人定位方法，其特征在于，所述S5包括：

S51：在检测到回环时，获取子图中节点之间的约束残差项；其中，子图中节点之间的约束残差项公式如下：

e_ij＝z_ij-h(pose_i,pose_j)

其中，pose_i，pose_j分别表示子图和节点的世界位姿，其子图和节点的世界位姿是根据所述视觉图像数据信息或者激光位移数据信息在考虑环境下所得到的机器人的位姿数据信息，h(pose_i,pose_j)表示第j个节点在第i个子图的坐标，h()表示坐标之间的转换关系，转换得到节点与子图之间的实际距离，z_ij表示观测得到的节点与子图之间的相对距离，e_ij表示观测距离与实际值之间的约束残差项；

S52：对所述约束残差项进行计算，得到约束误差；其中，节点之间的约束误差公式如下：

其中，分别表示子图和节点的转换矩阵，/>表示节点在子图中的相对位姿在世界坐标系下的表示，/>表示子图的局部坐标系在世界坐标系下的表示，通过求逆得到的/>则是世界坐标系相对于子图的局部坐标系的姿态变换矩阵，/>表示节点在子图中的相对位姿，/>分别表示节点和子图的姿态角，因此/>表示通过节点和子图的绝对位姿计算得到节点和子图之间的相对位姿，ξ_i,j表示通过视觉传感器或激光传感器测量得到的相对位姿；

S53：对上述约束误差进行加和得到整体误差，并利用代价函数以及优化函数对所述整体误差进行优化，得到优化后的位姿数据信息；其计算公式如下：

其代价函数如下：

其中，ρ()是损失函数；

其优化函数如下：

8.一种基于多传感器融合定位的机器人，其特征在于，包括：

一视觉传感器，用于获取视觉图像数据信息；

一激光传感器，用于获取激光位移数据信息；

一定位处理器，分别与所述视觉传感器以及所述激光传感器进行连接，用于基于权利要求1-7任一项所述的机器人定位方法，根据视觉传感器获取的所述视觉图像数据信息和激光传感器获取的所述激光位移数据信息对机器人进行定位。

9.一种基于多传感器融合定位的机器人定位装置，其特征在于，包括定位处理器、存储器以及存储在所述存储器上并可被所述定位处理器执行的计算机程序，其中所述计算机程序被所述处理器执行时，实现上述权利要求1-7任一项所述的机器人定位方法。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，其中所述计算机程序被定位处理器执行时，实现上述权利要求1-7任一项所述的机器人定位方法。