一种基于改进A*算法的家庭服务机器人路径规划方法与流程

本发明涉及一种家庭服务机器人路径规划技术，具体为一种基于改进a*算法的家庭服务机器人路径规划方法。

背景技术：

随着社会的发展，家庭服务机器人的需求正逐步增大，而路径规划(指机器人按照某一指标要求自行搜索出一条从起点驶向终点的路径)是家庭服务机器人的基础。目前，家庭服务机器人的自主移动能力欠缺，多要在人为控制下实现，其中一个重要的原因是路径规划算法不够完善。

近年来最常用的路径规划算法是a*算法，它是一种基于面积的全局规划算法，属于启发式搜索中的最佳优先搜索方法。但是a*算法在路径规划中存在实时性差的缺陷，所选路径相比与最优路径有一定的偏差。针对a*算法的以上缺陷，论文“a*算法改进算法及其应用”(张仁平，后勤工程学院，2009)所提出的方法只考虑了对a*算法在启发代价中引入父节点的影响，但未考虑实际代价与启发代价的权重。论文“基于加权a＊算法的服务型机器人路径规划”(赵真明，东南大学，2008)考虑在a*算法实际代价和启发代价间确定最佳比例关系，但并未对启发函数做出改进。

技术实现要素：

针对现有技术中家庭服务机器人路径规划算法不够完善、所选路径相比与最优路径有一定偏差等不足，本发明要解决的问题是提供一种系统运行稳定，生成路径耗时减少、偏差小的基于改进a*算法的家庭服务机器人路径规划方法。

为解决上述技术问题，本发明采用的技术方案是：

本发明一种基于改进a*算法的家庭服务机器人路径规划方法，包括以下步骤：

1)根据实际场景构建二维占据栅格地图，将每一个栅格分别标记为障碍区域或可行区域；

2)创建open队列，保存已经生成而未被访问的节点信息；创建closed队列，保存已被访问过的节点信息；确定起始节点与目标节点，分别标记为s和g节点，将起始节点放入open队列；

3)将当前节点以及当前节点父节点到目标节点的距离之和作为启发代价h，在启发代价前乘上一个启发量权重a，将乘上一个启发量权重a后的将启发代价h和实际代价g之和f作为总代价；

4)运行改进的a*算法，在可行区域内搜寻初始节点s到目标节点g之间的最优路径，并得到算法运行时间、遍历栅格数和路径长度指标；

5)修改启发代价的启发量权重a，重复步骤4)，将得到的算法运行时间、遍历栅格数和路径长度指标进行比较分析，得到最佳的启发量权重a，即得到该环境下最佳的规划路径。

步骤4)中，运行改进的a*算法搜寻初始节点s到目标节点g之间的最优路径具体为：

4.1)从open队列中选取节点y，并将其从队列中删去；

4.2)判断节点y是否为目标节点，如果不是目标节点，则转至步骤4.3)运行；

4.3)判断节点y周围的子节点是否在closed队列中；若是，直接跳过该节点至下一个节点；

4.4)判断节点y周围的子节点是否在open队列中，若是，则将节点y周围的子节点与y节点对应的路程代价比较，若节点y周围的子节点的代价较小，则更新open队列，将该子节点更新为当前节点，将y节点加入closed队列；

4.5)循环步骤4.1至步骤4.4，直至搜寻至目标节点g。

步骤4.4)中，若节点y周围的子节点的代价较大，则将子节点加入open队列。

步骤4.4)中，如节点y周围的子节点不在open队列中，则将子节点加入open队列。

步骤4.4)中，如果节点y是目标节点，则结束步骤4)，进入步骤5)。

本发明具有以下有益效果及优点：

1.本发明提出的基于引入父节点以及改变启发代价权重的改进a*算法进行智能机器人的路径规划，对普遍的和实际的环境分别进行多次仿真与实验，结果分析表明，基于引入父节点以及改变启发代价权重的改进a*算法的路径规划系统运行稳定，生成路径耗时减少，遍历栅格数明显减少，各性能指标均明显优于普通a*算法。

附图说明

图1为本发明改进的a*算法流程图；

图2为实际场景下的a*算法仿真效果图；

图3为实际场景下引入父节点的a*算法仿真效果图；

图4为本发明改进的a*算法中时间消耗、路径消耗和遍历栅格数随启发量权重的变化图像。

具体实施方式

下面结合说明书附图对本发明作进一步阐述。

本发明一种基于改进a*算法的家庭服务机器人路径规划方法，包括以下步骤：

1)根据实际场景构建二维占据栅格地图，将每一个栅格分别标记为障碍或可行区域；

2)创建open队列，保存已经生成而未被访问的节点信息；创建closed队列，保存已被访问过的节点信息；确定起始节点与目标节点，分别标记为s和g节点，将起始节点放入open队列；

3)将当前节点(即正在被访问的节点)以及当前节点父节点到目标节点的距离(在曼哈顿距离、切比雪夫距离以及欧几里得距离中选取)之和作为启发代价h，在启发代价前乘上一个启发量权重a，将乘上一个启发量权重a后的将启发代价h和实际代价g(即实际已走距离，通过实时检测得到)之和f作为总代价(即启发函数)。

4)运行改进的a*算法，在可行区域内搜寻初始节点s到目标节点g之间的最优路径，并得到算法运行时间、遍历栅格数和路径长度指标；

5)修改启发代价的启发量权重a，重复步骤4)，将得到的算法运行时间、遍历栅格数和路径长度指标进行比较分析，得到最佳的启发量权重a。即得到该环境下最佳的规划路径。

关于步骤1)，本实施例针对移动式服务机器人工作的典型场景，选取了一个典型的室内家居环境，构建室内地图。并考虑房间户型(其中包括卧室、客厅、卫生间、厨房、衣帽间、活动室以及阳台)以及房间内部的桌椅、家具、杂物等障碍(将每一个栅格分别标记为障碍区域或可行区域来区分是否有物体摆放)，使地图更加符合实际室内情况。在该环境下，对不同的算法进行仿真实验，并对比分析实验结果。其中栅格地图设置为50*80格。步骤2)中，创建open队列主要保存已经生成而未被访问的节点信息，创建closed队列主要保存已被访问过的节点信息。确定起始节点与目标节点，为了充分评估算法的有效性，选取场景西北角(左上角)亮点标记为起始点，东南角(右下角)亮点标记为目标点，将其标记为s和g节点，将起始节点放入open队列。

步骤3)中，将当前节点以及当前节点父节点到目标节点的欧几里得距离(在曼哈顿距离、切比雪夫距离以及欧几里得距离中选取)之和作为启发代价h。曼哈顿距离和切比雪夫距离不能直接体现两点间的距离，而欧几里德距离则更加符合实际，所以本实施例中选用欧几里德距离，具体公式为：

在启发代价前乘上一个启发量权重a，将启发代价h和实际代价g之和f作为总代价，具体公式为：f(n)＝g(n)+a[h(n)+h(p)]。

步骤4)中，运行改进的a*算法搜寻初始节点s到目标节点g之间的最优路径，并得到算法运行时间、遍历栅格数和路径长度等指标；

步骤4.1)从open队列中选取总估计代价最小的节点y，作为当前节点进行研究，并将其从open队列中删去；

步骤4.2)判断节点y是否为目标节点，若是则说明算法已经搜寻到目标点g，结束步骤4)，并可返回搜寻到的路径，否则向下继续运行步骤4.3)；

步骤4.3)判断当前节点y周围的四个子节点(也可以选取八个字节点)是否在closed队列中，若是，则说明该节点已经被遍历过，直接跳过该节点；

步骤4.4)判断节点y周围的四个子节点是否在open队列中，若是，则将其与y节点对应的路程代价比较，若子节点的代价较小，则更新open队列，将该子节点更新为当前节点，将y节点加入closed队列；若非，则将子节点加入open队列。

步骤4.5)循环运行步骤4.1)至步骤4.4)，一步步从当前节点向目标节点逼近，直至搜寻至目标节点g。

关于步骤5)，一般来说，启发代价与实际代价的权重不能相差过大，启发量过大会使搜索路径落入局部最优解的可能性变的很大，使得规划的路径与最优路径差别很大。因此，启发量权重a一般在1左右。在此，令a∈[0.5,2]，对引入了父节点和启发函数权重的a*算法进行实验，重复步骤4)，将得到的算法运行时间、遍历栅格数和路径长度等指标进行比较分析，得到最佳的启发量权重a。即可确定该环境下最佳的规划路径。

针对该场景下父节点的影响，从算法的快速性，准确性，计算复杂度三个方面分别选取搜寻时间、路径代价、遍历栅格数三个指标。由图2和图3以及实验结果的对比，可以清晰地看出引入了父节点的a*算法(a为0.5)相比于a*算法遍历的栅格数显著减少，而从实验结果来看，两者花费的时间分别是10.1362秒和63.7900秒，但是路径代价上，两者分别是126.1070和121.9172。可以看出该算法相比a*算法该算法在牺牲了较小路程代价的情况下，可以极大地提高算法的实时性。

在引入启发量权重之后，通过对合理范围(如启发量权重为权0.5-1.2)内的启发量权重进行研究，对不同启发代价权重的情况进行仿真后，将得到的搜寻时间、路径代价、遍历栅格数等指标进行分析，绘制出这三个指标随启发量权重变化而变化的规律，如图4所示。

从图4中可以看出，针对该场景下的路径规划问题，搜寻时间、路径代价、遍历栅格数三个指标有如下的规律：

(1)时间消耗

当a在0.7～0.9之间时，搜寻到最佳路径所消耗的时间较短；当a增大或减小至该范围外后，消耗时间都会增大；相比于a*算法，所消耗时间仅为其15％左右。

(2)路径代价

路径代价在a为0.6左右时最小，达到最优的状态；当a增大或减小，路径代价均会快速上升；路径代价与a*算法相差约3％。

(3)遍历栅格数

遍历栅格数与a大致呈线性反相关，a越小，遍历栅格数越多，算法的计算量和存储量越大；与a*算法相比，遍历栅格数仅为其12.4％。

从各项指标的与原算法对比中，可以看出该方法在各性能指标上，均明显优于普通a*算法以及前文提到的只考虑父节点和只考虑实际代价和启发代价间最佳比例关系的两种算法。