CN113587933B - 一种基于分支定界算法的室内移动机器人定位方法 - Google Patents

Abstract

本发明提供一种基于分支定界算法的室内移动机器人定位方法，通过将深度相机和激光雷达采集的数据进行融合，采用分支定界的方法在离线地图中搜索全局初始位姿，再通过迭代最近点的方法对全局初始位姿进行优化，最后将该全局位姿作为激光雷达扫描匹配的全局初始位姿，之后通过相关性扫描匹配的方法不断计算机器人的当前位姿，该方式能够保证机器人在行进过程中的定位精度，减小机器人的累积误差。另外在激光雷达扫描匹配算法的运行过程中，采用分支定界的方法加速回环检测进行全局优化，当回环检测算法未能及时校正累积的误差时，采用重定位判断方法有效检测误差的累积状态，并通过调用全局定位算法校正累积误差，提高定位过程的准确性。

Description

一种基于分支定界算法的室内移动机器人定位方法

技术领域

本发明属于移动机器人定位技术领域，具体涉及一种基于分支定界算法的室内移动机器人定位方法。

背景技术

现有的定位技术可分为两大类，绝对定位和相对定位；绝对定位需要预先确定好环境模型或通过传感器直接向机器人提供外接位置信息，计算机器人在全局坐标系中的位置；通常采用的方法是概率定位法、地图匹配法、信标定位法和GPS定位。相对定位则假定机器人初始位姿已知，采用相邻时刻传感器信息对机器人位置进行跟踪估计，相对定位分为里程计法和惯性导航法，该过程会随着机器人行进距离的增加而不断累积误差，需要绝对定位的方式进行修正。

分支定界算法是求解整数规划问题的常用方法，是一种搜索与迭代的方法。其主要流程分为分支和定界两个步骤。分支即把整个可行解空间不断分割成越来越小的子集；定界则是对每个子集内的解集计算一个目标下界(对于最小值问题)。在分支定界算法中，最重要的是剪枝的过程，也就是在每次分枝后，凡是界限超出已知可行解集目标值的那些子集不再进一步分枝。在机器人定位过程中，通过该方法能加快扫描匹配的过程，较快地寻找到机器人位姿的最佳匹配。

在机器人定位研究过程中，人们逐渐采用激光雷达扫描匹配的方式来修正机器人的累积误差，这种方式通过当前帧和前一帧或者几帧的扫描结果相匹配，从而确定机器人在环境中的位姿，能有效减小定位过程的累积误差，但该过程与激光雷达的精度有着很大的关系，另外就是环境的特征需要足够丰富，才能准确的进行匹配，但参与匹配的特征越多，则会导致算法时间复杂度提高。

发明内容

基于上述问题，本发明旨在提出一种基于分支定界算法的室内移动机器人定位方法，解决单一传感器定位精度受限的问题，提高室内移动机器人全局定位的能力和相对定位的准确性。

为实现上述目的，本发明提供的一种基于分支定界算法的室内移动机器人定位方法，包括：

步骤1：采集机器人周围环境的点云数据，并将点云数据转换到机器人本体坐标系下；

步骤2：将预先构建好的整个环境的离线地图转换为多层不同分辨率的栅格地图；

步骤3：将融合后的点云数据作为分支定界算法的输入，通过深度优先搜索的方式，在搜索树中查找具有最高得分的叶子节点作为机器人的全局初始位姿；

步骤4：将当前时刻得到的点云数据作为输入点云，离线地图作为目标点云，以机器人的全局初始位姿作为迭代最近点算法的初始位姿，通过迭代最近点算法得到准确的机器人全局位姿；

步骤5：采集机器人当前时刻的位姿信息作为位姿估计值，利用相关性扫描匹配算法得到机器人前进过程中的相对位姿；

步骤6：通过分支定界算法以深度优先搜索的方式加快回环检测的速度，在构建完成的所有子图中寻找当前机器人的相似匹配，采用非线性优化库优化位姿图；

步骤7：每间隔一定的时间判断机器人累积误差的大小，如果超过设定阈值，则返回执行步骤3来校正累积误差，并输出校正之后的机器人位姿。

所述步骤1包括：

步骤1.1：分别通过深度相机、激光雷达采集当前时刻机器人所在位置周围环境的两组点云数据；

步骤1.2：将深度相机采集到的点云数据进行去噪处理；

步骤1.3：在去噪后的深度相机点云数据中截取具有一定高度的数据点，并将该点云数据与激光雷达采集到的点云数据分别投影到机器人本体坐标系下，实现点云数据的融合。

所述步骤2具体表述为：从原始分辨率的栅格地图开始，每层对应一个高度值h，原始分辨率地图对应高度h＝0，逐层递增该高度值，在高度为h的栅格地图中每个栅格概率值为原始分辨率地图对应栅格周围2^h*2^h个栅格概率的最大值。

所述步骤3包括：

步骤3.1：确定节点选择策略，采用深度优先搜索方式确定节点选择策略；

步骤3.2：确定分支规则，对于树中每个节点用一个整数数组c＝(x，y，θ，c_h)表示，节点所在树中的高度用c_h表示，将树中满足c_h＞1的节点c分成4个高度为c_h-1的子节点：

其中，表示设置一定搜索窗口形成的有限集合：

其中，s_x、s_y、s_θ分别代表平移方向和旋转方向的边界搜索范围，S_x、S_y、S_α分别表示x方向线性搜索窗口、y方向线性搜索窗口和角度搜索窗口，r表示栅格地图的分辨率，ε_θ表示角度步长因子，ε_θ表示为：

其中，h_max表示一帧激光雷达数据所有击中点中测量得到的距离最大值；

步骤3.3：计算分支定界算法的上界：

式中，表示提前构建的由离线地图转换成的对应高度c_h的栅格地图；score(c)为将当前激光雷达数据投影至栅格/>上的概率值之和，/>为节点c对应的位姿，s_i为激光雷达扫描得到的击中点，K为一帧激光雷达数据中击中点的数量；

步骤3.4：通过深度优先搜素的方式在每层中递归搜索每个节点的上界，将具有最佳得分的叶子节点作为机器人位姿的最佳匹配，从而得到机器人全局初始位姿：

ξ_global＝ξ₀+(rx，ry，ε_θθ)

式中，ξ₀表示开始搜索时的初始位姿，ξ_global表示搜索结束得到的全局初始位姿。

所述步骤4包括：

步骤4.1：将融合后的点云数据转换到世界坐标系下；

步骤4.2：在世界坐标系下计算两组点云数据的质心：

式中，μ_p、μ_q分别表示通过融合后的点云数据P的质心、离线地图的点云数据Q的质心，N_p、N_q分别表示点云数据P中的点数、点云数据Q中的点数，p_i、q_j分别表示点云数据P中第i个点的坐标、点云数据Q中第j个点的坐标；

步骤4.3：设计迭代最近点算法中的误差函数E(R，t)；

式中，R为两组点云集变换对应的旋转矩阵，t为平移矩阵，N为参与点云变换的点数；

步骤4.4：利用迭代最近点算法迭代求解R和t，当达到收敛条件时停止迭代运算，输出的最优解作为准确的机器人全局位姿。

所述步骤5包括：

步骤5.1：通过惯性传感器采集机器人的线加速度和角速度，通过轮式里程计采集机器人的速度，将两组数据通过扩展卡尔曼滤波融合之后的数据作为机器人当前位置的位姿估计值；

步骤5.2：根据机器人当前位姿估计值，将当前时刻激光雷达数据转换到当前正在构建的子图中；

步骤5.3：设计相关性扫描匹配算法中的误差函数为：

式中，H_smooth是双三次插值函数，用于对子图中栅格的概率值进行平滑操作，T_ε表示位姿估计对应的变换矩阵，s_i′表示通过激光雷达采集的第i′帧数据，K为第i′帧激光雷达数据包含的点数；

步骤5.4：利用相关性扫描匹配算法迭代优化误差函数，当满足收敛条件时停止迭代计算，输出值作为当前位姿估计的最优解；

步骤5.5：更新子图中栅格的概率值：

H_new(x)＝clamp(F^-1(F(H_old(x))*F(p)))

式中，clamp为区间限定函数，p为栅格被占据的概率，F(p)为栅格占据概率与空闲概率的比值，H_old(x)为更新之前的栅格概率值，H_new(x)为更新之后的栅格概率值。

所述步骤6包括：

步骤6.1：将当前位姿估计的最优解作为分支定界算法的输入，与构建完成的所有子图进行搜索匹配，通过深度优先搜索的方式得到机器人在所有子图中的相似匹配；

步骤6.2：将当前机器人位姿的相似匹配作为回环约束加入位姿图中优化，全局位姿图优化的误差函数设计为：

式中，为第i个子图对应的位姿，/>为第j帧激光数据对应的位姿，∑_ij为第i个子图位姿与第j帧激光雷达位姿之间的协方差，ξ_ij为第i个子图与第j帧激光雷达的相对位姿，δ为鲁棒核函数。

所述步骤7包括：

步骤7.1：根据优化过后的机器人相对位姿，将融合点云数据转换至地图坐标系；

步骤7.2：计算融合后的点云数据在对应原始分辨率的栅格地图上的索引：

I_x＝(x_trans-x_origin-0.5*r)/r+1

I_y＝(y_trans-y_origin-0.5*r)/r+1

式中，I_x为地图坐标系x轴方向的索引，I_y为地图坐标系y轴方向的索引，x_trans为经过变换的融合点云x方向平移量，y_trans为经过变换的融合点云y方向平移量，x_origin为栅格地图的原点x坐标，y_origin为栅格地图的原点y坐标，r为栅格地图分辨率；

步骤7.3：计算融合后的点云数据与栅格地图的匹配概率P_occupy；

P_occupy＝i_c/(i_c+i_e)

式中，i_c为所有点云数据中匹配正确的个数，i_e为所有点云数据中匹配错误的个数；

步骤7.4：根据设定阈值判断是否进行重定位，当P_occupy小于等于设定阈值时，则认为机器人当前时刻的位姿已经有了一定程度的累积误差，因此需要返回执行步骤3来校正累积误差；

步骤7.5：每间隔一定时间，重复执行步骤7.1～步骤7.5来判断误差累积状态，保证机器人在前进过程中的定位准确性。

本发明的有益效果是：

本发明提出了一种基于分支定界算法的室内移动机器人定位方法，1)通过分支定界方法加快全局定位收敛速度，并通过迭代最近点算法实现机器人全局位姿的准确估计，整体方案通过两阶段的匹配算法不仅实现全局定位的实时性，还能保证算法的定位精度；2)采用多种传感器融合进行定位，解决单一传感器定位失效的问题，利用多种传感器之间的互补特性，减小定位误差的累积，增加算法的定位精度，此外，本发明采用的传感器成本较低，定位算法对硬件平台要求较低；3)在机器人相对定位算法中增加机器人重定位判断方法，能够及时校正机器人累积误差，提高机器人定位精度；4)采用绝对定位与相对定位结合的方式，形成完整的室内移动机器人定位方案，能够实现机器人在环境的初始定位以及位置跟踪，提高算法整体的***性。

附图说明

图1为本发明中基于分支定界算法的室内移动机器人定位方法流程图；

图2为发明中基于分支定界算法的室内移动机器人定位方法原理图；

图3为本发明中采用的分支定界算法流程图；

图4为本发明中相对定位流程图；

图5为本发明中全局定位流程图。

具体实施方式

下面结合附图和具体实施实例对发明做进一步说明。本发明提供了一种基于分支定界算法的室内移动机器人定位方法，机器人安装的传感器包括：深度相机、2D激光雷达、惯性传感器、轮式里程计，其中深度相机安装在机器人的正前方，用来采集机器人正前方的环境图像；2D激光雷达安装在机器人的正上方，用来采集周围环境的点云数据；惯性传感器安装在机器人的正上方，用来采集机器人的线加速度和角速度；轮式里程计安装在机器人的车轮上，用来采集机器人的速度；将深度相机和激光雷达采集的数据进行融合，采用分支定界的方法在离线地图中搜索全局初始位姿，再通过迭代最近点的方法对全局初始位姿进行优化，该方法不仅保证了全局定位的准确性，而且能够有效减小全局定位的定位时间。最后将该全局位姿作为激光雷达扫描匹配的全局初始位姿，之后通过相关性扫描匹配的方式不断计算机器人的当前位姿，该方式能够保证机器人在行进过程中的定位精度，减小机器人的累积误差。另外在激光雷达扫描匹配算法的运行过程中，采用分支定界的方法加速回环检测进行全局优化，当回环检测算法未能及时校正累积误差时，采用误差判断方法有效检测误差的累积状态，及时重定位减小误差，提高定位过程的准确性。

如图1～2所示，一种基于分支定界算法的室内移动机器人定位方法，包括：

获取深度相机点云数据，并截取高度z＝0的点云数据投影至机器人本体坐标系，获取激光雷达扫描数据，转换为点云数据投影至机器人本体坐标系，将以上两种机器人本体坐标系上的点云数据进行融合；

选取具有一定高度的点云数据，截取在该高度平面内的点云，根据深度相机和机器人本体坐标系之间的静态坐标变换能够得到相机坐标系到机器人本体坐标系的变换矩阵，将截取的点云通过该变换矩阵，转换到机器人本体坐标系下；

2D激光雷达数据通常以极坐标表示，而点云数据为三维数据，因此需要将激光雷达数据转换成空间直角坐标系下的坐标表示，并把z轴坐标设置成0，将转换之后的点云数据投影至机器人本体坐标系。

步骤1：采集机器人周围环境的点云数据，并将点云数据转换到机器人本体坐标系下；包括：

步骤1.1：分别通过深度相机、激光雷达采集当前时刻机器人所在位置周围环境的两组点云数据；

通过深度相机获得的点云需要进行去噪处理，由于深度相机获取的原始点云数据包含部分噪声、离群点，存在点云密度不规则等问题，需要对原始点云数据进行体素滤波，体素滤波会对输入点云创建一个三维体素网格，每个体素内用体素中所有点的重心来近似表示体素中其他点，这样该体素内所有点都用一个重心点最终表示，该方法能在下采样的过程中保留原始点云的形状。

步骤1.2：将深度相机采集到的点云数据(简称深度相机点云数据)进行去噪处理；具体采用体素滤波方式进行去噪处理；

步骤1.3：在去噪后的深度相机点云数据中截取具有一定高度(z＝0)的数据点，并将该点云数据与激光雷达采集到的点云数据分别投影到机器人本体坐标系下，实现点云数据的融合；

根据深度相机坐标系和机器人坐标系之间的静态坐标变换能够得到相机坐标系到机器人坐标系的变换矩阵，将截取的点云通过该变换矩阵，转换到机器人本体坐标系下；转换的公式如下：

P_b＝R_bc*P_c+t_bc

其中，P_b为变换到机器人坐标系下的点云，R_bc为相机坐标系到机器人坐标系的旋转矩阵，P_c为相机坐标系下的点云，t_bc为相机坐标系到机器人坐标系的平移矩阵。

然后，将激光雷达获得的数据(简称激光雷达数据)转换成点云数据，并投影至机器人坐标系下和深度相机获得的点云数据进行融合；

2D激光雷达获得的数据通常以极坐标表示，而点云数据为三维数据，因此需要将2D激光雷达获得的数据转换成空间直角坐标系下的坐标表示，并把z轴坐标设置成0，转换公式如下：

θ_cur＝θ_min+i*Δθ

x＝R*cosθ_cur

y＝R*sinθ_cur

z＝0

其中，θ_min为激光雷达开始扫描的角度，在本实施例中取θ_min＝0，Δθ表示激光雷达旋转的角分辨率，在本实施例中取Δθ＝0.33°，i表示激光雷达从扫描开始到当前位置的第i个点，R表示当前位置激光雷达扫描点返回的距离值，θ_cur表示当前位置和开始扫描的位置相差的角度，x、y和z分别表示转换成点云数据的三个维度的坐标。

将转换之后的点云数据投影至机器人坐标系，投影的公式如下所示：

P_b＝R_bl*P_l+t_bl

其中，P_b为变换到机器人坐标系下的点云，R_bl为激光雷达坐标系到机器人坐标系的旋转矩阵，P_l为激光雷达坐标系下的点云，t_bl为激光雷达坐标系到机器人坐标系的平移矩阵。

步骤2：将预先构建好的整个环境的离线地图转换为多层不同分辨率的栅格地图；具体表述为：从原始分辨率的栅格地图开始，每层对应一个高度值h，原始分辨率地图对应高度h＝0，逐层递增该高度值，在高度为h的栅格地图中每个栅格概率值为原始分辨率地图对应栅格周围2^h*2^h个栅格概率的最大值。

将融合后的点云数据作为分支定界算法的输入，与离线建立的地图进行搜索匹配，分支定界算法将离线地图转换成六层不同分辨率的栅格地图，通过深度优先搜索的方式得到机器人的全局初始位姿。如图3所示，具体来说，分支定界算法的原理是将问题所有可能的解集表示为树中的节点，其中根节点表示为所有可能的解，每个节点的子节点构成父节点的一个子集，叶子节点为单体结构，表示问题的一个可行解。在本发明中通过深度优先搜索快速评估很多叶子节点，将得分低于最优分数的节点进行剪枝，快速找到最优匹配。

步骤3：将融合后的点云数据作为分支定界算法的输入，通过深度优先搜索的方式，在搜索树中查找每一层具有最高得分的节点，并将具有最高得分的叶子节点作为最佳匹配结果，根据该结果可以得到机器人的全局初始位姿；包括：

步骤3.1：确定节点选择策略，采用深度优先搜索方式确定节点选择策略；该算法能够快速评估很多节点，并对不满足条件的节点快速剪枝，提高分支定界算法的运算效率。

步骤3.2：确定分支规则，对于树中每个节点用一个整数数组c＝(x，y，θ，c_h)表示，节点所在树中的高度用c_h表示，将树中满足c_h＞1的节点c分成4个高度为c_h-1的子节点：

其中，表示设置一定搜索窗口形成的有限集合，取S_x＝30S_y＝30S_θ＝180°：

其中，s_x、s_y、s_θ分别代表平移方向和旋转方向的边界搜索范围，S_x、S_y、S_θ分别表示x方向线性搜索窗口、y方向线性搜索窗口和角度搜索窗口，r表示栅格地图的分辨率，r＝0.05，ε_θ表示角度步长因子，ε_θ表示为：

其中，h_max表示一帧激光雷达数据所有击中点中测量得到的距离最大值；

步骤3.3：计算分支定界算法的上界：

式中，N(x_i，y_i)是双三次插值函数，表示提前构建的由离线地图转换成的对应高度c_h的栅格地图；score(c)为将当前激光雷达数据投影至栅格/>上的概率值之和，/>为节点c对应的位姿，s_i为激光雷达扫描得到的击中hit点，K为一帧激光雷达数据中击中点的数量；栅格/>存储的是大小为2^h×2^h的像素框中的栅格概率最大值，通过预先计算栅格的方式可以快速计算激光扫描匹配的得分。

步骤3.4：通过深度优先搜素的方式在每层中递归搜索每个节点的上界，将具有最佳得分的叶子节点作为机器人位姿的最佳匹配，从而得到机器人全局初始位姿：

ξ_global＝ξ₀+(rx，ry，ε_θθ)

式中，ξ₀表示开始搜索时的初始位姿，ξ_global表示搜索结束得到的全局初始位姿。

步骤4：将当前时刻得到的点云数据作为输入点云，离线地图作为目标点云，以机器人的全局初始位姿作为迭代最近点算法的初始位姿，通过迭代最近点算法优化该初始位姿，得到准确的机器人全局位姿；包括：

步骤4.1：将融合后的点云数据转换到世界坐标系下，变换公式如下所示：

P_w＝R_wb*P_b+t_wb

其中，P_w为变换到世界坐标系下的点云，R_wb为机器人坐标系到世界坐标系的旋转矩阵，P_b为机器人坐标系下的点云，t_wb为机器人坐标系到世界坐标系的平移矩阵。

步骤4.2：在世界坐标系下计算两组点云数据的质心：

待转换的点云构成点集：

P＝{p₁，p₂，…，p_M}

目标点云构成点集：

Q＝{q₁，q₂，…，q_N}

假设P和Q中点数均为N_p，则两组点集的质心分别为：

式中，μ_p、μ_q分别表示通过融合后的点云数据P的质心、离线地图的点云数据Q的质心，N_p、N_q分别表示点云数据P中的点数、点云数据Q中的点数，p_i、q_j分别表示点云数据P中第i个点的坐标、点云数据Q中第j个点的坐标；

步骤4.3：设计迭代最近点算法中的误差函数E(R，t)；

式中，R为两组点云集变换对应的旋转矩阵，t为平移矩阵，N为参与点云变换的点数；

步骤4.4：利用迭代最近点算法迭代求解R和t，当达到收敛条件时停止迭代运算，输出的最优解作为准确的机器人全局位姿；当达到下列收敛条件之一时停止迭代运算：

1)当迭代次数达到最大迭代次数，则认为算法已经收敛；

2)当前一次变换矩阵与此次变换矩阵之间的差值小于设定阈值，则认为算法已经收敛；

3)当前后两次迭代的点对的欧氏距离均方误差之和小于设定阈值，则认为算法已经收敛。

此时得到的旋转矩阵和平移矩阵能够作为机器人准确的全局初始位姿，当机器人进行持续的位置跟踪时，会将其作为相对定位的初始位姿，整个全局定位算法由步骤3和步骤4组成，如图5所示。

根据得到的机器人全局位姿确定机器人在环境中的绝对位置，之后以轮式里程计和惯性传感器(简称imu)采集到的数据通过扩展卡尔曼滤波之后的数据作为机器人当前位姿估计值，通过相关性扫描匹配算法得到机器人前进过程的相对位姿。

步骤5：采集机器人当前时刻的位姿信息作为位姿估计值，利用相关性扫描匹配算法得到机器人前进过程中的相对位姿；包括：

步骤5.1：通过惯性传感器采集机器人的线加速度和角速度，通过轮式里程计采集机器人的速度，将两组数据通过扩展卡尔曼滤波融合之后的数据作为机器人当前位置的位姿估计值；

步骤5.2：根据机器人当前位置的位姿估计值，将当前时刻激光雷达获取的数据转换到当前正在构建的子图中；

转换公式如下所示：

其中，p_m表示转换到子图中的激光雷达数据，p_s表示当前时刻激光雷达数据，θ表示位姿估计的旋转角，x和y分别表示位姿估计的x方向偏移量和y方向偏移量。

步骤5.3：设计相关性扫描匹配算法中的误差函数为：

式中，H_smooth是双三次插值函数，用于对子图中栅格的概率值进行平滑操作，T_ε表示位姿估计对应的变换矩阵，s_i′表示通过激光雷达采集的第i′帧数据，K为第i′帧激光雷达数据包含的点数；

步骤5.4：利用相关性扫描匹配算法迭代优化误差函数，当满足收敛条件时停止迭代计算，输出值作为当前位姿估计的最优解；当达到下列收敛条件之一时停止迭代计算：

1)当达到设定的最大迭代次数时，认为算法已经收敛；

2)当前后两次变换矩阵之间的差值小于设定阈值时，认为算法已经收敛。

步骤5.5：更新子图中栅格的概率值：当一帧激光雷达数据***子图中时，对之前没有观察过的网格点会分配一个概率值，而之前观察过的网格点则会更新其概率值；

H_new(x)＝clamp(F^-1(F(H_old(x))*F(p)))

式中，clamp为区间限定函数，p为栅格被占据的概率，F(p)为栅格占据概率与空闲概率的比值，H_old(x)为更新之前的栅格概率值，H_new(x)为更新之后的栅格概率值。

步骤6：通过分支定界算法以深度优先搜索的方式加快回环检测的速度，在构建完成的所有子图中寻找当前机器人的相似匹配，最后将该相似匹配加入闭环约束中，采用非线性优化库优化位姿图；包括：

步骤6.1：将当前位姿估计的最优解作为分支定界算法的输入，与构建完成的所有子图进行搜索匹配，通过深度优先搜索的方式得到机器人在所有子图中的相似匹配；

步骤6.2：将当前机器人位姿的相似匹配作为回环约束加入位姿图中优化，全局位姿图优化的误差函数设计为：

式中，为第i个子图对应的位姿，/>为第j帧激光数据对应的位姿，∑_ij为第i个子图位姿与第j帧激光雷达位姿之间的协方差，ξ_ij为第i个子图与第j帧激光雷达的相对位姿，δ为鲁棒核函数，E(·)表示残差函数，其计算公式如下所示：

步骤7：每间隔一定的时间，判断机器人累积误差的大小，如果超过设定阈值，则返回执行步骤3来校正累积误差，并输出校正之后的机器人位姿；判断机器人累积误差的过程，主要原理是将融合点云数据通过坐标变换至世界坐标系下，并计算变换之后的激光雷达数据中每个点对应于具体栅格的索引，查找离线地图相应位置的概率值，如果离线地图中该位置概率值表示该位置未知或者未占据，则更新不匹配点的个数，否则更新匹配点个数；具体包括：

步骤7.1：根据优化过后的机器人相对位姿，将融合点云数据转换至地图坐标系，转换公式如下所示：

P_w＝R_wb*P_b+t_wb

其中，P_w为变换到世界坐标系下的点云，R_wb为机器人坐标系到世界坐标系的旋转矩阵，P_b为机器人坐标系下的点云，t_wb为机器人坐标系到世界坐标系的平移矩阵；

步骤7.2：计算融合后的点云数据在对应原始分辨率的栅格地图上的索引，加载离线地图时会将地图每个位置的索引以及对应的概率值保存在一个数组中，以供匹配过程查找，而融合点云在栅格地图索引计算公式如下所示：

I_x＝(x_trans-x_origgin-0.5*r)/r+1

I_y＝(y_trans-y_origin-0.5*r)/r+1

式中，I_x为地图坐标系x轴方向的索引，I_y为地图坐标系y轴方向的索引，x_trans为经过变换的融合点云x方向平移量，y_trans为经过变换的融合点云y方向平移量，x_origin为栅格地图的原点x坐标，y_origin为栅格地图的原点y坐标，r为栅格地图分辨率，r＝0.05；

步骤7.3：计算融合后的点云数据与栅格地图的匹配概率P_occupy；融合后的点云数据落在栅格地图正确位置上时，会递增匹配正确计数值，而当融合后的点云数据与栅格地图原始栅格占据情况不一致时，则会递增错误匹配计数值，最后匹配概率计算公式如下所示：

P_occupy＝i_c/(i_c+i_e)

式中，i_c为所有点云数据中匹配正确的个数，i_e为所有点云数据中匹配错误的个数；

步骤7.4：根据设定阈值判断是否进行重定位，当P_occupy小于等于设定阈值时，则认为机器人当前时刻的位姿已经有了一定程度的累积误差，因此需要返回执行步骤3来校正累积误差；

步骤7.5：每间隔一定时间(ΔT＝5s)，重复执行步骤7.1～步骤7.5来判断误差累积状态，保证机器人在前进过程中的定位准确性，步骤5～步骤7是机器人行进过程中相对位姿的定位过程，其设计原理图如图4所示。

为了验证本发明的有效性，对该发明的定位效果进行了仿真实验和室内实际环境测试实验，主要进行全局定位和相对定位两部分测试实验，主要测试机器人在已知先验地图的情况下，对其在地图中任意位置的准确估计能力，测试环境分为仿真环境和实际环境，分别进行20次相同环境的测试，测试评估算法的运行效率和准确性，测试结果如下，表中，trans_x、trans_y、trans_z分别表示x方向平移误差，y方向平移误差和z方向平移误差，ang_x、ang_y、ang_z分别表示x方向旋转角度误差，y方向旋转角度误差和z方向旋转角度误差：

表1全局定位精度对比表格

表2全局定位效率对比表格

表3相对定位精度对比表格

根据以上实验数据说明，本发明提出的基于分支定界算法的室内机器人定位方法在全局定位和相对定位两个方面均取得不错的精度；出于实际工程需要，机器人定位算法必须能够进行持续定位，因此实时性是必要条件，因此未对相对定位算法的运行效率进行评估，本实验涉及的算法均能够进行相对局部定位。

Claims (5)

1.一种基于分支定界算法的室内移动机器人定位方法，其特征在于，包括：

步骤1：采集机器人周围环境的点云数据，并将点云数据转换到机器人本体坐标系下；

步骤2：将预先构建好的整个环境的离线地图转换为多层不同分辨率的栅格地图；

步骤3：将融合后的点云数据作为分支定界算法的输入，通过深度优先搜索的方式，在搜索树中查找具有最高得分的叶子节点作为机器人的全局初始位姿；

步骤4：将当前时刻得到的点云数据作为输入点云，离线地图作为目标点云，以机器人的全局初始位姿作为迭代最近点算法的初始位姿，通过迭代最近点算法得到准确的机器人全局位姿；

步骤5：采集机器人当前时刻的位姿信息作为位姿估计值，利用相关性扫描匹配算法得到机器人前进过程中的相对位姿；

步骤6：通过分支定界算法以深度优先搜索的方式加快回环检测的速度，在构建完成的所有子图中寻找当前机器人的相似匹配，采用非线性优化库优化位姿图；

步骤7：每间隔一定的时间判断机器人累积误差的大小，如果超过设定阈值，则返回执行步骤3来校正累积误差，并输出校正之后的机器人位姿；

所述步骤2具体表述为：从原始分辨率的栅格地图开始，每层对应一个高度值h，原始分辨率地图对应高度h＝0，逐层递增该高度值，在高度为h的栅格地图中每个栅格概率值为原始分辨率地图对应栅格周围2^h*2^h个栅格概率的最大值；

所述步骤3包括：

步骤3.1：确定节点选择策略，采用深度优先搜索方式确定节点选择策略；

步骤3.2：确定分支规则，对于树中每个节点用一个整数数组c＝(x,y,θ,c_h)表示，节点所在树中的高度用c_h表示，将树中满足c_h>1的节点c分成4个高度为c_h-1的子节点：

其中，表示设置一定搜索窗口形成的有限集合：

其中，s_x、s_y、s_θ分别代表平移方向和旋转方向的边界搜索范围，S_x、S_y、S_θ分别表示x方向线性搜索窗口、y方向线性搜索窗口和角度搜索窗口，r表示栅格地图的分辨率，ε_θ表示角度步长因子，ε_θ表示为：

其中，h_max表示一帧激光雷达数据所有击中点中测量得到的距离最大值；

步骤3.3：计算分支定界算法的上界：

式中，表示提前构建的由离线地图转换成的对应高度c_h的栅格地图；score(c)为将当前激光雷达数据投影至栅格/>上的概率值之和，/>为节点c对应的位姿，s_i为激光雷达扫描得到的击中点，K为一帧激光雷达数据中击中点的数量；

步骤3.4：通过深度优先搜素的方式在每层中递归搜索每个节点的上界，将具有最佳得分的叶子节点作为机器人位姿的最佳匹配，从而得到机器人全局初始位姿：

ξ_global＝ξ₀+(rx,ry,ε_θθ)

式中，ξ₀表示开始搜索时的初始位姿，ξ_global表示搜索结束得到的全局初始位姿；

所述步骤6包括：

步骤6.1：将当前位姿估计的最优解作为分支定界算法的输入，与构建完成的所有子图进行搜索匹配，通过深度优先搜索的方式得到机器人在所有子图中的相似匹配；

步骤6.2：将当前机器人位姿的相似匹配作为回环约束加入位姿图中优化，全局位姿图优化的误差函数设计为：

式中，为第i个子图对应的位姿，/>为第j帧激光数据对应的位姿，∑_ij为第i个子图位姿与第j帧激光雷达位姿之间的协方差，ξ_ij为第i个子图与第j帧激光雷达的相对位姿，δ为鲁棒核函数，E(·)表示残差函数。

2.根据权利要求1所述的一种基于分支定界算法的室内移动机器人定位方法，其特征在于，所述步骤1包括：

步骤1.1：分别通过深度相机、激光雷达采集当前时刻机器人所在位置周围环境的两组点云数据；

步骤1.2：将深度相机采集到的点云数据进行去噪处理；

步骤1.3：在去噪后的深度相机点云数据中截取具有一定高度的数据点，并将该点云数据与激光雷达采集到的点云数据分别投影到机器人本体坐标系下，实现点云数据的融合。

3.根据权利要求1所述的一种基于分支定界算法的室内移动机器人定位方法，其特征在于，所述步骤4包括：

步骤4.1：将融合后的点云数据转换到世界坐标系下；

步骤4.2：在世界坐标系下计算两组点云数据的质心：

式中，μ_p、μ_q分别表示通过融合后的点云数据P的质心、离线地图的点云数据Q的质心，N_p、N_q分别表示点云数据P中的点数、点云数据Q中的点数，p_i、q_j分别表示点云数据P中第i个点的坐标、点云数据Q中第j个点的坐标；

步骤4.3：设计迭代最近点算法中的误差函数E(R,t)；

式中，R为两组点云集变换对应的旋转矩阵，t为平移矩阵，N为参与点云变换的点数；

步骤4.4：利用迭代最近点算法迭代求解R和t，当达到收敛条件时停止迭代运算，输出的最优解作为准确的机器人全局位姿。

4.根据权利要求1所述的一种基于分支定界算法的室内移动机器人定位方法，其特征在于，所述步骤5包括：

步骤5.1：通过惯性传感器采集机器人的线加速度和角速度，通过轮式里程计采集机器人的速度，将两组数据通过扩展卡尔曼滤波融合之后的数据作为机器人当前位置的位姿估计值；

步骤5.2：根据机器人当前位置的位姿估计值，将当前时刻激光雷达获取的数据转换到当前正在构建的子图中；

步骤5.3：设计相关性扫描匹配算法中的误差函数为：

式中，H_smooth是双三次插值函数，用于对子图中栅格的概率值进行平滑操作，T_ε表示位姿估计对应的变换矩阵，s_i，表示通过激光雷达采集的第i′帧数据，K为第i′帧激光雷达数据包含的点数；

步骤5.4：利用相关性扫描匹配算法迭代优化误差函数，当满足收敛条件时停止迭代计算，输出值作为当前位姿估计的最优解；

步骤5.5：更新子图中栅格的概率值；

H_new(x)＝clamp(F^-1(F(H_old(x))*F(p)))

式中，clamp为区间限定函数，p为栅格被占据的概率，F(p)为栅格占据概率与空闲概率的比值，H_old(x)为更新之前的栅格概率值，H_new(x)为更新之后的栅格概率值。

5.根据权利要求1所述的一种基于分支定界算法的室内移动机器人定位方法，其特征在于，所述步骤7包括：

步骤7.1：根据优化过后的机器人相对位姿，将融合点云数据转换至地图坐标系；

步骤7.2：计算融合后的点云数据在对应原始分辨率的栅格地图上的索引：

I_x＝(x_trans-x_origin-0.5*r)/r+1

I_y＝(y_trans-y_origin-0.5*r)/r+1

式中，I_x为地图坐标系x轴方向的索引，I_y为地图坐标系y轴方向的索引，x_trans为经过变换的融合点云x方向平移量，y_trans为经过变换的融合点云y方向平移量，x_origin为栅格地图的原点x坐标，y_origin为栅格地图的原点y坐标，r为栅格地图分辨率；

步骤7.3：计算融合后的点云数据与栅格地图的匹配概率P_occupy：

P_occupy＝i_c/(i_c+i_e)

式中，i_c为所有点云数据中匹配正确的个数，i_e为所有点云数据中匹配错误的个数；

步骤7.4：根据设定阈值判断是否进行重定位，当P_occupy小于等于设定阈值时，则认为机器人当前时刻的位姿已经有了一定程度的累积误差，因此需要返回执行步骤3来校正累积误差；

步骤7.5：每间隔一定时间，重复执行步骤7.1～步骤7.5来判断误差累积状态，保证机器人在前进过程中的定位准确性。