[发明专利]一种基于分支定界算法的室内移动机器人定位方法

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

3.根据权利要求1所述的一种基于分支定界算法的室内移动机器人定位方法，其特征在于，所述步骤2具体表述为：从原始分辨率的栅格地图开始，每层对应一个高度值h，原始分辨率地图对应高度h＝0，逐层递增该高度值，在高度为h的栅格地图中每个栅格概率值为原始分辨率地图对应栅格周围2^h*2^h个栅格概率的最大值。

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

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

步骤3.2：确定分支规则，对于树中每个节点用一个整数数组c＝(x,y,θ,c_h)表示，节点所在树中的高度用c_h表示，将树中满足c_h1的节点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表示搜索结束得到的全局初始位姿。