拓展A*算法的机器人室内三维地图路径规划

王 硕，周海波，张建军※，刘 彪

(1.天津理工大学天津市先进机电系统设计与智能控制重点实验室，天津 300384；2.天津理工大学机电工程国家级实验教学示范中心，天津 300384；3.广州高新兴机器人有限公司，广东 广州 510530)

1 引言

机器人由于自身计算能力的不足，往往不能如人类一样实时的对自身周围环境进行精准的建模与处理，因此在运动之前规划出行走路径，是大多数机器人采用的方法。机器人按照自身定位能力和地图信息，利用路径规划算法，计算地图中两点之间最短无碰撞路线，是实现自主机器人自动化运行的基础条件。目前已经已存在若干较为成熟的二维地图的路径搜索方法，广泛应用于各行各业，包括A*[1]、D*[2]、Dijkstra[3]、遗传[4]、蚁群[5]等。但是二维地图无法表示空间的高程信息，在一个有限高度的空间范围内，二维地图也无法表示范围内的通过性，使不同高度机器人的决策产生了错误。而三维地图对环境信息的表现则更为丰富，可以准确告知机器人通道高度能否通过。

三维地图在计算机内存中最基本的储存格式为点云地图，但是由于点云地图数据量较大且处理效率低下，难以在实际场景中应用点云地图进行路径规划。而八叉树地图与点云地图相比是一种数据量较小，且易于更新的地图格式，它是一个正方体沿X、Y、Z轴各分两半得到的8个小正方体，每一个小正方体又能继续细分下去。根据细分程度，可以有效减少数据量。

本文首先使用三维深度相机对室内环境进行建模形成点云地图，然后再将其转换成八叉树格式用于处理，使环境信息的数据量和节点数量有效减少。在此基础上，为解决原始的A*算法不讨论机器人体积的缺点，本文定义移动机器人为一个正方体包围盒而非一个点，通过将包围盒实际尺寸计算在障碍物及边界周围区域内，从而实现了三维地图环境中机器人物理体积在A*算法上的设计。

2 A*算法

A*算法是在最佳优先搜索思想和Dijkstra算法[6-7]的基础上开发的。与Dijkstra算法在各个方向上随机扩张搜索相比，A*算法计算预估成本，将扩张趋向于朝向目标方向进行，从而使搜索效率更高，成本函数f(M)的说明如下

f(M)=g(M)+h(M)

(1)

式中的M可以代表地图中某一地点，f(M)表示规划的路线需要经过点M时付出的总成本，g(M)表示从出发点移动至M点时所付出的成本，h(M)表示从M点移动至目标点预计要付出的成本，其中h(M)由曼哈顿距离计算：

h(M)=D(n，m)=|xn-xm|+|yn-ym|

(2)

这里n和m代表了地图中的两个不同的位置点，(xn，ym)和(yn，ym)代表了这二点各自在地图中的位置。

2)若M节点为无障碍节点且没有超出地图之外时，选取M作为A的子节点，将计算过成本的节点进行标记，以后的规划过程中不再重复计算；

3)以M为父节点继续执行步骤1)和2)，并只考虑M的右上、右、右下、下、左下节点计算成本，对于其周围的其他三个节点，在上一步骤已经计算过成本，属于M的同级子节点，则不再重复计算；

4)重复迭代，直到规划至B点时搜索结束，A点到B点的路径如图1所示。

图1 A*算法路径搜索

3 拓展的A*算法

A*算法假设移动机器人没有体积，在不考虑机器人体积的情况下进行路径规划，机器人有可能会和环境发生碰撞，从而造成安全危害。所以，本文将移动机器人的体积引入A*算法讨论范围，假设移动机器人是标准正方体的包围盒，以包围盒的外接圆作为移动机器人自身动作的最大边界。如果满足了包围盒的外接圆边界和障碍物之间不产生接触，则可以保证移动机器人的原地旋转不发生碰撞。假设机器人的移动中枢位于包围盒正中央，如图2所示。

图2 机器人的二维实际尺寸模型

设栅格的边长是1，若包围盒边长为栅极边长的n≥1倍，则包围盒的外接圆半径可以表示为

(3)

如图1，由于机器人存在实际体积，因此在进行路径规划时，障碍物的边缘区域是机器人中心点无法到达的，需要排除在路径规划区域之外。因此，定义障碍物和边界附近的一定范围为机器人中心点禁入区域，或称为安全区域，路径规划的搜索范围中需要排除掉安全区域，这样可将机器人重新简化为用一个无体积的点来表示，从而可以运用A*算法对机器人中心点进行路径规划。对安全区域的范围可以进行如下探究：

1)当机器人直行时，安全区域范围的表示如图3所示，即在障碍物周围和边界朝里部分的安全范围最小宽度L1一满足下式时，可保证机器人不与障碍与边界接触

图3 机器人直行时安全区域

L1=(n-1)/2+l1

(4)

其中l1表示机器人直行时包围盒一条边的中点与包围盒外接圆之间的距离

(5)

为避免碰撞，L1值向上取整。

2)机器人斜向移动示意如图4所示。首先，使安全区域的最小宽度L2=L1，因为当机器人移动时，倾斜方向上两个相邻节点的中点与障碍物顶点之间的距离是最近的，即图4中红色节点交点处，所以其最小间距为

图4 机器人转弯时安全区域

(6)

在正整数n≥3时，d2>0，且因实际情景中机器人的实际体型远大于八叉树地图中最小体素尺寸，所以机器人在斜向移动时的安全区域宽度要求，和直行时保持一致即可以达到不碰撞要求。

综上所述，应用扩展的A*算法搜索A点到B点的路线情况如图5所示。通过计算安全区域，在地图精度最低即n=3的情况下，也可以有效避碰，从而可以确保机器人自主移动过程的安全。

图5 拓展的A*算法路径搜索

4 八叉树地图构建及其应用

由于机器人导航需求的提高，二维地图不能达到实际场景需要，三维地图构建的重要性愈发受到重视。即时定位与建图(SLAM)[8-10]技术，让机器人能够在完全陌生的空间中构建地图。通常情形下，用SLAM技术直接扫描出的地图一般是点云地图。因为点云地图存储容量大，运算性能较差，所以在规划机器人的路径时通常不使用点云地图，因此Armin H等人[11]选择使用八叉树结构来减少计算量。通过改变空间分辨率，可以进行自主的控制环境地图数据规模，根据机器人自身算力，达到计算效率和地图精度的平衡。

八叉树以正方体中心为切分点，沿X、Y、Z轴分两半得到的8个小正方体，每一个小正方体又能继续细分下去，形成每个叶子节点有8个分支的树状数据结构。如图6是八叉树的空间分割示意图，每个体素的色彩深度代表了该位置空间已被占有的程度，黑色代表该体素所在位置的空间已被充分占有，白色则代表空间完全空置，而灰色则代表该节点空间存在部分被占用。灰色体素将会被继续分割，直至宽度达到设定最小值，此最小值即为八叉树的分辨率。

图6 八叉树空间划分示意图

移动机器人的包围盒尺寸以八叉树地图分辨率为基本单位，且为奇数值。移动机器人边界框的外接球为移动机器人自身运动所能到达的最大边界。机器人在固定位置不移动时的任何动作都不应超出球体范围之外。包围盒三维尺寸模型如图7所示。

图7 包围盒三维尺寸模型

类似的，同样分别讨论机器人直行与斜行时三维地图的安全区域：

1)当机器人直行时，安全范围最小宽度L3满足下式时，可保证机器人不与障碍发生碰撞

L3=(n-1)/2+l2

(7)

其中l2为机器人的包围盒一个面中心点与外接球之间的距离：

(8)

为避免碰撞，L3值向上取整。

2)当机器人斜向移动时，斜方向相邻二个小立方体的交点距障碍物顶点的间距为最小值，令斜行时的安全区域最小长度L4=L3，机器人最短安全距离为

(9)

在n≥5时，d3>0。且因实际情景中机器人的实际体型远大于八叉树地图中最小体素尺寸，因此，需要机器人斜向移动的安全区域长度和直行时保持一致，即可以达到不碰撞要求。

5 实验分析

实验环境为实验室部分室内地图，如图8所示。图8(a)为移动机器人实物照片，机器人的尺寸为620mm×500mm×950mm，则移动机器人的包围盒边长可以指定为950mm，机器人配备Kinect摄像头传感器和底盘伺服电机驱动系统，可以实现自主移动。深度相机采集完成的三维室内点云数据如图8(b)所示，三维地图采集了实验室拐角的环境位置信息，便于验证通过本文算法控制机器人在地图中实现避障与转弯的过程，图中设置了路径规划时的起点A和终点B。将点云地图转换成分辨率为0.01m和0.1m的八叉树地图如图8(c)和图8(d)所示，采用改进的A*算法在进行路径规划实验展示，两种分辨率规划出的室内路径。

图8 室内三维地图及路径规划

对不同分辨率下的算法计算时间比对如图9所示。经过运算后可以看出，物理分辨率由0.01m上升到了0.02m，而栅格数量也下降了447.2%，计算时间减少了213.8%。伴随分辨率的不断提高，栅格数量和计算工作时间也越来越小，因此通过提高八叉树地图的分辨率可以有效提高算法运行效率。但分辨率的提高会造成地图的细节的丢失，如图8(c)和图8(d)所示。若地图细节丢失严重，可能会降低路径规划的精度，甚至因为某些本能通过的通道由于分辨率的提高而变窄，导致路径规划失败。所以应该在确保实时性的前提下，选用合适的分辨率以提升路径规划准确度。

图9 不同分辨率路径规划时间对比

6 结论

1)为解决室内三维地图场景中移动机器人的路径规划与避障问题，将机器人实际体积纳入考虑范畴，充分讨论机器人物理体积在路径规划中的影响，并按照机器人实际尺度设定了障碍物以及边界附近的安全范围，将机器人重新以质点的方式进行A*路径规划，从而实现机器人的避障。

2)针对三维点云地图由于节点数量过多难以进行路径规划计算的问题，将点云结构转换成八叉树结构，并在八叉树地图上建立最优路径搜索方法，减少了计算耗时，保证了机器人运动时算法的实时性。

3)最后，在实验中通过SLAM技术采集了室内环境三维地图，并转化为八叉树地图，通过本文算法规划了机器人的运动路径，验证了该方法的实用价值。