基于平滑A*人工势场法的机器人动态路径规划
任玉洁+付丽霞+张勇+毛剑琳
摘要：针对动态环境下移动机器人的路径规划问题，提出了平滑A*人工势场法的路径规划方法。首先采用平滑A*算法在静态障碍物环境中进行全局路径规划；其次，在机器人遇到动态障碍物时采用A*人工势场法进行局部动态路径规划，并以此调整全局路径规划结果；最后对路径规划结果进行平滑处理。将平滑A*人工势场法应用于机器人动态路径规划，并与D*算法进行对比。实验结果表明，该算法能够在动态环境下规划出一条更为优化的路径，有效缩短了路径长度，提高了规划效率。
关键词：A*算法；人工势场法；路径规划；机器人
DOIDOI：10.11907/rjdk.172192
中图分类号：TP301
文献标识码：A文章编号文章编号：16727800（2018）001000803
Abstract：A path planning method based on smoothed A* artificial potential field is proposed aiming the path planning problem of mobile robot in dynamic environment， In order to improve the planning efficiency， Firstly a smoothed A* algorithm was used to perform the global path planning in the static obstacle environment；Secondly the A* artificial potential field method was applied to perform the local dynamic path planning and adjusted then the global path planning results when the robot encounters a dynamic obstacle；Finally the path planning results were smoothed.The smoothed A* artificial potential field method is applied to the robot dynamic path planning and compared with the D*algorithm. Experimental results have shown that the proposed algorithm can be worked out a more optimized path in dynamic environment， effectively shorten the path length， improve the planning efficiency.
Key Words：A* algorithm； artificial potential field method； path planning； robot
0引言
路径规划是移动机器人控制领域的一个重要研究方向，它是指依据某些指标在一个障碍物环境下搜寻一条从起始点到目标点的最优或近似最优无碰撞路径[12]。依据环境信息，大体上可将路径规划分为环境信息已知的全局路径规划和基于传感器感知的局部路径规划[3]。其中，全局路径规划能处理障碍物信息已知情况下的路径规划，但当出现动态障碍物时，规划效果不理想；局部路径规划能够通过传感器反馈的信息，规避环境中的动态障碍物。
A*算法是一种经典的启发式搜索算法，适用环境已知的全局路径规划，它具备最优性、完备性和高效性[4]，利用A*算法可以得到较好的路径规划效果。但是由于A*算法的计算特点，规划出的路径点一般存在折线多、累计转折角度大等问题。平滑A*[5]通过建立平滑模型，有效降低了路径中的转折点与转折角度。D*算法[6]是基于A*算法提出的机器人动态路径规划算法，其思想是在向目标点移动过程中，若检测到环境中的动态障碍物，则搜寻障碍物附近节点的变化情况，重新规划路径。人工势场法[7]是一种比较成熟的局部路径规划算法，其由于具有计算速度快、适用于实时控制的优点而得到广泛应用。但由于该方法依据的是局部环境信息，缺乏全局自我调节能力，易陷入局部最优[8]。
本文提出平滑A*算法和改进人工势场法结合的动态路径规划算法。首先，为提高算法搜索效率，采用文献[5]中的平滑A*算法对障碍物环境进行全局路径规划，再引入改进的人工势场法对动态障碍物进行避障。
1A*算法
1.1传统A*算法
A*算法是一种经典的启发式算法，在静态环境中能高效地求出从起始点到目标点的路径，其估价函数定义为：
其中，X是当前节点，f（x）是当前节点的总启发代价，g（x）是初始点到当前点的实际路径代价，h（x）是当前点到目标点的估计路径代价。
h（x）的取值对算法效率具有决定性作用。本文采用当前点与目标点之间的欧几里得距离作为估计代价，如下所示：
其中，xt为目标点横坐标，yt为目标点纵坐标，xx为当前点横坐标，yx为当前点纵坐标。
A*算法寻径首先要创建两张表：open list和close list。其中，open list用来保存拓展节点，close list用來保存障碍物和已使用过的节点。搜寻过程中，先从open list找出总启发代价最小的点设为当前节点，将其放入close list，并对其进行扩展；将扩展后的节点更新到open list，再从open list选取总启发代价最小的点设为当前节点；重复过程，直到找到目标点。
1.2平滑A*算法
文献[5]中提出，传统A*算法规划出的路径存在冗余转折点，为了避免不必要的转折，对求得的路径进行平滑处理。从当前点开始，依次连接之后的节点，判断是否与障碍物有交点，如果无交点，舍弃中间节点，求出平滑路径。具体过程如下：①取出传统A*算法规划出的路径点，将起点设为pcurrent，下一节点依次设为pnext、pdnext；②执行check（pcurrent，pdnext，barrier list）方法获得返回值，如果为0则继续执行，否则跳转至第4步；③pnext赋值给pcurrent，pdnext赋值给pnext，pdnext下一节点赋值给pdnext，跳转至第5步；④路径点中删除pnext节点，将pdnext赋值给pnext，pdnext下一节点赋值给pdnext；⑤若pdnext为目标点则继续执行，否则跳转至第2步；⑥输出平滑后的路径。
其中，barrier list是路径规划环境中的障碍物列表，函数check（pcurrent，pdnext，barrier list）用来检测pcurrent和pdnext连线与障碍物边线是否相交，如果不相交则返回1，否则返回0。
2人工势场法
2.1传统人工势场法
人工势场法在机器人运动空间构造虚拟的力势场，包括目标点产生的引力势场和障碍物产生的斥力势场。目标点引力与当前点到目标间距离成正比，障碍物产生斥力与当前点到障碍物距离成反比，通过引力和斥力的作用，寻找一个从初始点到目标点的安全无碰撞路径。其引力势场函数和斥力势场函数分别定义为：
式中，X是当前位置，Xg是目标点位置，X0是障碍物位置，Katt是引力场常量，Krep是斥力场常量，ρ0是障碍物最大威胁距离。
根据引力势场和斥力势场的负梯度可以得到引力函数和斥力函数：
通过人工势场法可以在较简单的障碍物环境中规划出一条从初始点到目标点的安全无碰撞路径。当环境中障碍物变多，则可能存在人工势场法的局部极小值点问题。局部极小值点是机器人运动环境中某些点由于受多个力的共同作用，造成了斥力和引力相互抵消，从而使合力为0。随着障碍物增多，寻径过程中陷入局部极小值点的机率随之增大。
2.2A*人工势场法
针对人工势场法局部极小值点问题，提出A*人工势场法。主要包括局部极小值检测和A*人工势场法避障。
本文人工势场法局部极小值检测采用一定间隔内机器人行进距离和阈值进行比较的方法。该方法首先根据机器人移动步长和障碍物影响范围大小设置一个阈值T，然后检测当前节点前一个节点与后一个节点之间的距离并与T进行比较。若距离小于阈值T，可以认为机器人陷入了局部极小值点，反之则没有陷入。
检测到移动机器人陷入局部极小值点时，对局部极小值点用A*算法进行规划从而将其避开，脱离局部极小值点后回到人工势场法。传统A*算法路径点为栅格中点，对障碍物的判断为检测拓展节点是否为障碍物点。但此时路径点不再局限于栅格中点，在寻径过程中，连接当前节点与拓展节点，通过判断连线与障碍物边线是否有交点判断是否与障碍物相交。图1是传统人工势场法的路径规划结果，图2是A*人工势场法的路径规划结果。
3平滑A*人工势场法
将平滑A*算法与A*人工势场法结合。首先用栅格法[9]进行建模，将机器人移动区域划分为相同的小栅格。静态障碍物标记为黑色，可通行区域标记为白色。由于障碍物形状不确定，为了简化操作，当障碍物不足一个栅格时，将其补全至一个栅格；然后采用平滑A*算法对障栅格法处理后的障碍物环境进行全局路径规划，当遇到动态障碍物时，将全局规划路径点中障碍物的前一节点设为初始节点，障碍物后一节点设为目标点，采用A*人工势场法进行动态路径规划；再对A*人工势场法规划的路径进行平滑处理，消除冗余节点，最后输出路径。具体流程如图3所示。
4实验研究
为了验证动态规划性能，在Matlab2014上进行仿真，设人工势场法初始参数引力场常量Katt=100，斥力场常量Krep=3，斥力影响范围ρ0=3，步长l=0.5；图中矩形网格为动态障碍，沿y轴负方向作速度为1栅格/s的匀速运动。图4为在20×20栅格环境下采用平滑A*算法预规划路径图，图5为未碰撞时的路径图，图6是动态障碍物碰撞时D*算法规避路线图，图7是动态障碍物碰撞时平滑A*人工势场法算法规避路线图。表1给出了D*算法和平滑A*人工势场法路径规划参数对比。
实验结果表明，当传感器检测到动态障碍物时，采用A*人工势场法能够有效避障并快速回到预规划路径，与D*算法相比，路径长度减少了7.001%，累计转折次数减少了23.077%，累计转折角度减少了37.973%。
5结语
本文将平滑A*算法与改进人工势场法相结合，首先用平滑A*算法进行全局路径规划，再用A*人工势场法引导机器人进行动态避障，最后对求得的路径点进行平滑处理。仿真实验结果表明，本文算法在动态环境中能较好地满足移动机器人的路径规划要求。
参考文献：
[1]朱大奇，颜明重.移动机器人路径规划技术综述[J].控制与决策，2010，25（7）：961967.
[2]ZAMIRIAN M， KAMYAD A V， FARAHI M H.A novel algorithm for solving optimal path planning problems based on parametrization method and fuzzy aggregation[J].Physics Letters A，2009，373（38）：34393449.
[3]杨世良.动态环境下机器人混合型路径规划算法[D].郑州：郑州大学，2011.
[4]李冲，张安，毕文豪.单边矩形扩展A*算法[J].机器人，2017，39（1）：4656.
[5]王红卫，马勇，谢勇，等.基于平滑A～*算法的移动机器人路径规划[J].同济大学学报：自然科学版，2010，38（11）：16471650.
[6]FERGUSON D， STENTZ A. The field D* algorithm for improved path planning and replanning in uniform and nonuniform cost environments[R].Technical Report CMUTRRI0519，2005.
[7]KHATIB O. Realtime obstacle avoidance for manipulators and mobile robots[M]. Sage Publications， Inc.1986.
[8]張殿富，刘福.基于人工势场法的路径规划方法研究及展望[J].计算机工程与科学，2013，35（6）：8895.
[9]王娟娟，曹凯.基于栅格法的机器人路径规划[J].农业装备与车辆工程，2009（4）：1417.
（责任编辑：黄健）
摘要：针对动态环境下移动机器人的路径规划问题，提出了平滑A*人工势场法的路径规划方法。首先采用平滑A*算法在静态障碍物环境中进行全局路径规划；其次，在机器人遇到动态障碍物时采用A*人工势场法进行局部动态路径规划，并以此调整全局路径规划结果；最后对路径规划结果进行平滑处理。将平滑A*人工势场法应用于机器人动态路径规划，并与D*算法进行对比。实验结果表明，该算法能够在动态环境下规划出一条更为优化的路径，有效缩短了路径长度，提高了规划效率。
关键词：A*算法；人工势场法；路径规划；机器人
DOIDOI：10.11907/rjdk.172192
中图分类号：TP301
文献标识码：A文章编号文章编号：16727800（2018）001000803
Abstract：A path planning method based on smoothed A* artificial potential field is proposed aiming the path planning problem of mobile robot in dynamic environment， In order to improve the planning efficiency， Firstly a smoothed A* algorithm was used to perform the global path planning in the static obstacle environment；Secondly the A* artificial potential field method was applied to perform the local dynamic path planning and adjusted then the global path planning results when the robot encounters a dynamic obstacle；Finally the path planning results were smoothed.The smoothed A* artificial potential field method is applied to the robot dynamic path planning and compared with the D*algorithm. Experimental results have shown that the proposed algorithm can be worked out a more optimized path in dynamic environment， effectively shorten the path length， improve the planning efficiency.
Key Words：A* algorithm； artificial potential field method； path planning； robot
0引言
路径规划是移动机器人控制领域的一个重要研究方向，它是指依据某些指标在一个障碍物环境下搜寻一条从起始点到目标点的最优或近似最优无碰撞路径[12]。依据环境信息，大体上可将路径规划分为环境信息已知的全局路径规划和基于传感器感知的局部路径规划[3]。其中，全局路径规划能处理障碍物信息已知情况下的路径规划，但当出现动态障碍物时，规划效果不理想；局部路径规划能够通过传感器反馈的信息，规避环境中的动态障碍物。
A*算法是一种经典的启发式搜索算法，适用环境已知的全局路径规划，它具备最优性、完备性和高效性[4]，利用A*算法可以得到较好的路径规划效果。但是由于A*算法的计算特点，规划出的路径点一般存在折线多、累计转折角度大等问题。平滑A*[5]通过建立平滑模型，有效降低了路径中的转折点与转折角度。D*算法[6]是基于A*算法提出的机器人动态路径规划算法，其思想是在向目标点移动过程中，若检测到环境中的动态障碍物，则搜寻障碍物附近节点的变化情况，重新规划路径。人工势场法[7]是一种比较成熟的局部路径规划算法，其由于具有计算速度快、适用于实时控制的优点而得到广泛应用。但由于该方法依据的是局部环境信息，缺乏全局自我调节能力，易陷入局部最优[8]。
本文提出平滑A*算法和改进人工势场法结合的动态路径规划算法。首先，为提高算法搜索效率，采用文献[5]中的平滑A*算法对障碍物环境进行全局路径规划，再引入改进的人工势场法对动态障碍物进行避障。
1A*算法
1.1传统A*算法
A*算法是一种经典的启发式算法，在静态环境中能高效地求出从起始点到目标点的路径，其估价函数定义为：
其中，X是当前节点，f（x）是当前节点的总启发代价，g（x）是初始点到当前点的实际路径代价，h（x）是当前点到目标点的估计路径代价。
h（x）的取值对算法效率具有决定性作用。本文采用当前点与目标点之间的欧几里得距离作为估计代价，如下所示：
其中，xt为目标点横坐标，yt为目标点纵坐标，xx为当前点横坐标，yx为当前点纵坐标。
A*算法寻径首先要创建两张表：open list和close list。其中，open list用来保存拓展节点，close list用來保存障碍物和已使用过的节点。搜寻过程中，先从open list找出总启发代价最小的点设为当前节点，将其放入close list，并对其进行扩展；将扩展后的节点更新到open list，再从open list选取总启发代价最小的点设为当前节点；重复过程，直到找到目标点。
1.2平滑A*算法
文献[5]中提出，传统A*算法规划出的路径存在冗余转折点，为了避免不必要的转折，对求得的路径进行平滑处理。从当前点开始，依次连接之后的节点，判断是否与障碍物有交点，如果无交点，舍弃中间节点，求出平滑路径。具体过程如下：①取出传统A*算法规划出的路径点，将起点设为pcurrent，下一节点依次设为pnext、pdnext；②执行check（pcurrent，pdnext，barrier list）方法获得返回值，如果为0则继续执行，否则跳转至第4步；③pnext赋值给pcurrent，pdnext赋值给pnext，pdnext下一节点赋值给pdnext，跳转至第5步；④路径点中删除pnext节点，将pdnext赋值给pnext，pdnext下一节点赋值给pdnext；⑤若pdnext为目标点则继续执行，否则跳转至第2步；⑥输出平滑后的路径。
其中，barrier list是路径规划环境中的障碍物列表，函数check（pcurrent，pdnext，barrier list）用来检测pcurrent和pdnext连线与障碍物边线是否相交，如果不相交则返回1，否则返回0。
2人工势场法
2.1传统人工势场法
人工势场法在机器人运动空间构造虚拟的力势场，包括目标点产生的引力势场和障碍物产生的斥力势场。目标点引力与当前点到目标间距离成正比，障碍物产生斥力与当前点到障碍物距离成反比，通过引力和斥力的作用，寻找一个从初始点到目标点的安全无碰撞路径。其引力势场函数和斥力势场函数分别定义为：
式中，X是当前位置，Xg是目标点位置，X0是障碍物位置，Katt是引力场常量，Krep是斥力场常量，ρ0是障碍物最大威胁距离。
根据引力势场和斥力势场的负梯度可以得到引力函数和斥力函数：
通过人工势场法可以在较简单的障碍物环境中规划出一条从初始点到目标点的安全无碰撞路径。当环境中障碍物变多，则可能存在人工势场法的局部极小值点问题。局部极小值点是机器人运动环境中某些点由于受多个力的共同作用，造成了斥力和引力相互抵消，从而使合力为0。随着障碍物增多，寻径过程中陷入局部极小值点的机率随之增大。
2.2A*人工势场法
针对人工势场法局部极小值点问题，提出A*人工势场法。主要包括局部极小值检测和A*人工势场法避障。
本文人工势场法局部极小值检测采用一定间隔内机器人行进距离和阈值进行比较的方法。该方法首先根据机器人移动步长和障碍物影响范围大小设置一个阈值T，然后检测当前节点前一个节点与后一个节点之间的距离并与T进行比较。若距离小于阈值T，可以认为机器人陷入了局部极小值点，反之则没有陷入。
检测到移动机器人陷入局部极小值点时，对局部极小值点用A*算法进行规划从而将其避开，脱离局部极小值点后回到人工势场法。传统A*算法路径点为栅格中点，对障碍物的判断为检测拓展节点是否为障碍物点。但此时路径点不再局限于栅格中点，在寻径过程中，连接当前节点与拓展节点，通过判断连线与障碍物边线是否有交点判断是否与障碍物相交。图1是传统人工势场法的路径规划结果，图2是A*人工势场法的路径规划结果。
3平滑A*人工势场法
将平滑A*算法与A*人工势场法结合。首先用栅格法[9]进行建模，将机器人移动区域划分为相同的小栅格。静态障碍物标记为黑色，可通行区域标记为白色。由于障碍物形状不确定，为了简化操作，当障碍物不足一个栅格时，将其补全至一个栅格；然后采用平滑A*算法对障栅格法处理后的障碍物环境进行全局路径规划，当遇到动态障碍物时，将全局规划路径点中障碍物的前一节点设为初始节点，障碍物后一节点设为目标点，采用A*人工势场法进行动态路径规划；再对A*人工势场法规划的路径进行平滑处理，消除冗余节点，最后输出路径。具体流程如图3所示。
4实验研究
为了验证动态规划性能，在Matlab2014上进行仿真，设人工势场法初始参数引力场常量Katt=100，斥力场常量Krep=3，斥力影响范围ρ0=3，步长l=0.5；图中矩形网格为动态障碍，沿y轴负方向作速度为1栅格/s的匀速运动。图4为在20×20栅格环境下采用平滑A*算法预规划路径图，图5为未碰撞时的路径图，图6是动态障碍物碰撞时D*算法规避路线图，图7是动态障碍物碰撞时平滑A*人工势场法算法规避路线图。表1给出了D*算法和平滑A*人工势场法路径规划参数对比。
实验结果表明，当传感器检测到动态障碍物时，采用A*人工势场法能够有效避障并快速回到预规划路径，与D*算法相比，路径长度减少了7.001%，累计转折次数减少了23.077%，累计转折角度减少了37.973%。
5结语
本文将平滑A*算法与改进人工势场法相结合，首先用平滑A*算法进行全局路径规划，再用A*人工势场法引导机器人进行动态避障，最后对求得的路径点进行平滑处理。仿真实验结果表明，本文算法在动态环境中能较好地满足移动机器人的路径规划要求。
参考文献：
[1]朱大奇，颜明重.移动机器人路径规划技术综述[J].控制与决策，2010，25（7）：961967.
[2]ZAMIRIAN M， KAMYAD A V， FARAHI M H.A novel algorithm for solving optimal path planning problems based on parametrization method and fuzzy aggregation[J].Physics Letters A，2009，373（38）：34393449.
[3]杨世良.动态环境下机器人混合型路径规划算法[D].郑州：郑州大学，2011.
[4]李冲，张安，毕文豪.单边矩形扩展A*算法[J].机器人，2017，39（1）：4656.
[5]王红卫，马勇，谢勇，等.基于平滑A～*算法的移动机器人路径规划[J].同济大学学报：自然科学版，2010，38（11）：16471650.
[6]FERGUSON D， STENTZ A. The field D* algorithm for improved path planning and replanning in uniform and nonuniform cost environments[R].Technical Report CMUTRRI0519，2005.
[7]KHATIB O. Realtime obstacle avoidance for manipulators and mobile robots[M]. Sage Publications， Inc.1986.
[8]張殿富，刘福.基于人工势场法的路径规划方法研究及展望[J].计算机工程与科学，2013，35（6）：8895.
[9]王娟娟，曹凯.基于栅格法的机器人路径规划[J].农业装备与车辆工程，2009（4）：1417.
（责任编辑：黄健）
