基于改进HybridA*的非完整约束机器人路径规划算法研究

田雨　林松　房殿军　江竞宇

摘要：为了实现单舵轮AGV在物流场景下的精准自主导航，针对基于HybridA*算法搜索的路径容易贴近障碍物的缺陷和算法在路径平滑后可能与障碍物冲突的问题，本文提出一种基于改进HybridA*的非完整约束轮式移动机器人路径规划方法。对于路径搜索部分，将距离场地图引入启发式函数，以提高搜索效率，并保持搜索路径远离障碍物；对于路径平滑部分，将优化问题转化为二次规划问题，通过边界约束保证避障效果。在ROS平台上仿真实验的结果表明，该路径规划方法可有效用于非完整约束系统，为单舵轮AGV自主导航的准确运动奠定了路径智能规划的基础。

关键词：非完整约束机器人；路径规划；改进HybridA*；二次规划

中图分类号：TP242.2 文献标志码：A doi：10.3969/j.issn.1006-0316.2023.03.010

文章编号：1006-0316 （2023） 03-0063-09

Path Planning of Robot with Nonholonomic Constraint based on Improved Hybrid A* algorithm

TIAN Yu1，LIN Song1，FANG Dianjun1，2，JIANG Jingyu1

（ 1.School of Mechanical Engineering， Tongji University， Shanghai 201804， China;

2.Qingdao Sino-German Institute of Intelligent Technologies， Qingdao， 266000， China ）

Abstract：In order to achieve precise autonomous driving of the single steering wheel automated guided vehicle （AGV） in logistics scene， this paper proposes a path planning method for wheeled mobile robots （WMRs） with nonholonomic constraint based on improved Hybrid A* algorithm， so that the problems of original Hybrid A* algorithm on obstacles avoidance could be solved. For the path-search part， a distance field is introduced into the heuristic function， which could improve the searching efficiency and keep the search path away from obstacles. For the path-smoothing part， a quadratic programming problem is described to guarantee the obstacle avoidance through boundary constraints. The results of simulated experiments on ROS show that the method proposed could be effectively applied to nonholonomic constrained robots， and could lay the foundation for the accurate movement of single steering wheel AGV on autonomous navigation.

Key words：robot with nonholonomic constraint；path planning；Hybrid A* algorithm；quadratic programming

AGV（Automated Guided Vehicle，自動引导车）作为一种无人操作的智能化搬运设备，在制造业生产、仓储物流等作业环节中发挥着重要作用。其中，单舵轮AGV具有底盘结构简单、驱动方式简明、成本低廉等优点，在工业物流领域被大量应用，但对该类型的AGV进行路径规划及其运动控制时，需要额外考虑车轮的非完整约束条件，让机器人在给定运动路径下始终保持车轮无滑动滚动的状态[1]，才能使其在自主导航时保证较高的可控运动精度。如果能为单舵轮AGV自主导航规划出合理、可达、无碰撞的全局路径，就可降低轨迹跟踪控制的难度，显著提高AGV的运行效率和轨迹跟踪精度，以此为AGV自主导航的准确运动提供技术支撑。

由国内外相关研究可知，考虑非完整约束系统的路径规划方法主要包括曲线插值方法、采样方法、最优控制方法和机器学习方法[2]。其中，曲线插值和采样方法由于具备较高的求解速度，是目前实际运用最为广泛的方法，也更符合AGV的低算力特点和高实时性要求，因此是本文的主要研究方向。Dolgov等[3]提出的HybridA*算法，便是此类考虑非完整约束的早期典型算法，该算法由A*算法改进而来，通过轮式底盘可实现的曲线轨迹来进行节点拓展，满足轮式车辆的非完整约束。随后，部分学者以该方法为基础算法，从不同角度提出了改进方法。任秉韬等[4]设计了可变半径的RS（Reeds- Shepp）曲线，增加了搜索灵活性；Saeid等[5]引入可见性图来引导节点扩展，大大降低了扩展节点的数量；还有其他文献[6-8]在HybridA*基础上提出不同的子节点拓展方式、启发函数或节点搜索策略，以提高搜索效率。

另一方面，通过原始HybridA*算法搜索出的路径存在包含不必要的转弯和曲率不连续的问题，这在实用中还有待进一步平滑优化。Dolgov[3]采用梯度下降法对HybridA*算法搜索出的路径进行平滑，此后也有文献[7-9]采用了基于梯度下降的数值优化方法来对图搜索出的路径进行平滑后处理。然而，采用梯度下降法优化得到的路径无法保证无碰撞，需要反复修改优化目标，直到通过碰撞检测[7]。此外，HybridA*算法还存在初始搜索的路径太过贴近障碍物的现象，这不仅不利于AGV的行驶，也会压缩路径平滑优化的空间，增加路径平滑的优化难度。

针对以上问题，本文在HybridA*算法的路径搜索过程中引入距离场，通过动态调整距离阈值和修改启发式函数的形式，使得初始搜索路径远离障碍物；在平滑后处理中，将优化问题描述为二次规划问题，以线性约束的形式保证优化后路径的无碰撞性，最终得到适合单舵轮AGV的自主导航路径规划方法。其规划框架如图1所示。

1 路径搜索

1.1  HybridA*算法原理

传统A*算法是一种基于栅格化地图的图搜索算法，它的目标是搜索出一条与障碍物无碰撞的距离最短路径。在栅格地图中，算法从起点出发，并以此为初始节点，不断扩展节点周围的子节点，并且选择代价函数总值最小的子节点作为下一个搜索点，不断往复，直至到达终点。公式为：

f（n）＝g（n）＋h（n）                   （1）

式中：g（n）为累计代价函数，物理意义是从起始节点到当前节点的累计距离；h（n）为启发式函数，物理意义是当前节点到目标节点的预估距；f（n）为代价函数，代表在途经当前节点这一条件

下的路径预估总距离。

核心原理为：

（1）不断寻找f（n）值最小的无碰撞节点进行扩展。如图2（a）所示，在初始节点0的周围，优先扩展无碰撞节点1；

（2）如新节点已经被扩展过，则通过比较g（n）的方式选择更优路径。如图2（b）所示，虽然2节点的f（n）值小于3节点，会被优先扩展，但0－1－3路径得到的g（n）值小于0－1－2－3的路径，因此，当3节点扩展后，2节点会被废弃。

采用这样的思路，可以将搜索方向引导向预估总路径最短的方向，而并非没有目标的依次遍历，因此可以高效率搜索到总距离最短的无碰撞路径。

不难看出，A*算法最初的目标是路径长度最短，因而代价函数主要考虑距离因素；如果在代价函数引入其他因素，则可以得到满足实际需求的路径，如时间最优、能量最优等。

HybridA*与A*的算法基本思路相似，区别点主要是拓展节点的方式和代价函数的组成。

在拓展节点部分，A*算法可以向临近栅格直接扩展，如图3（a）所示；HybridA*算法则考虑了车辆的非完整约束，子节点位置为车辆以R的转弯半径行进l的距离后所在的位置，如图3（b）所示。

在启发式函数部分，HybridA*算法对同一个节点用两种不同的方式计算，取两种计算方式结果的较大值作为启发代价。

第一种计算方式考虑非完整约束但忽略障碍物，计算当前节点与目标节点间符合运动学约束的最短路径，在车辆允许倒车时，一般通过计算RS曲线得到。RS曲线是在无障碍物情况下满足车辆运动学约束（起点位姿、终点位姿、最小转弯半径、允许倒车）的距离最优曲线，如图4所示。

第二种计算方式为考虑障碍物但忽略非完整约束，计算当前节点与目标节点间的无碰撞最短路径，一般通过A*等图搜索算法计算得到。第二种计算方式在障碍物较稠密的区域能够产生更良好的效果[3]。

此外，HybridA*算法还会在每拓展n个节点时计算该节点到目标位姿的RS曲线，如果曲线与环境障碍物无碰撞，则直接采纳该路径作为剩余路径，结束搜索过程。这可以减少拓展的节点数量，大大提升搜索效率。完整的Hybrid A*算法流程如图5所示。

1.2 启发式函数改进

为使搜索出的路径远离障礙物，引入距离场地图[10]思想，其原理是将栅格地图基于广度优先搜索的思想进行遍历，得到每个栅格点和最近障碍物的距离，被障碍物占据的栅格数值为1，距离障碍物越远则数值越高。原始栅格地图和由此生成的距离场地图对比如图6所示。

本文所涉及的物流场景一般不具备开阔空间，在该种环境下只考虑障碍物但忽略非完整约束的启发函数往往效果更优，因此本文舍弃了计算RS曲线作为启发式函数的部分，而是只对障碍物进行考虑。引入了距离场系数后的启发式函数可表示为：

式中：Xi为A*算法搜索得到的从当前节点n到目标节点所途径的总共k个节点中的第i个节点； 为与该节点对应的障碍物距离代价函数； 为该节点对应的距离场数值；K为大于1的安全距离阈值。

式（3）可理解为，节点与障碍物的距离小于阈值时，赋较大的代价值；节点与障碍物的距离大于阈值时，代价值随距离的增大而减小。

为了验证启发式函数的改进效果，本文在ROS Rviz平台上建立仿真障碍地图，分别以路径搜索中较常见三种情况（S型连续弯、U型弯、长直线避障）设定初始位姿和目标位姿进行路径搜索实验，如图7所示。其中，完整障碍地图尺寸为30 m×20 m，搜索分辨率0.1 m，因而搜索栅格尺寸为300×200；三次实验的始末位姿已在图中标出。可以看出，改进后的路径能够相较原路径离障碍物更远。

在此基础上，为定量分析改进算法的性能，本文在该地图上随机设定起始位姿和目标位姿进行50次重复实验，箱型图结果如图8所示。箱型图是一种体现数据分布情况的统计图，其中箱体部分的红线代表数据中位数（Q2）位置，箱体实线上界与下界分别代表数据上四分位数（Q3）和下四分位数（Q1）；箱体外部的蓝色圆圈代表大于Q3－1.5×（Q3－Q1）或小于Q1－1.5×（Q3－Q1）的值，被称作离群值；箱体虚线上界和下界分别为未离群的最大值和最小值。

图8中表征两种算法结果区别的规划路径相对长度差为：

式中： 为两种算法规划路径的长度差； 为原算法规划出的路径长度； 为改进算法规划出的路径长度。

由图8可以看出，相较于原始HybridA*算法，改進算法在搜索路径时拓展节点的个数更少、算法总用时更短，在考虑了距离场地图生成的额外开销下，总体计算效率仍更优。需要注意的是，为保证显示尺度，图8（a）（b）中出现的离群值仅用虚线代替，未标注正确位置，但离群数据中改进算法的结果仍相较原算法效率更高。此外，改进算法搜索的路径长度在大多数情况下会略大于原始算法的搜索结果，这是考虑了安全避障、放弃距离最优的合理结果，与预期相符。

2 路径平滑

上述路径搜索算法能够粗略得到一条符合非完整约束的可达路径，但存在不必要的拐弯、路径曲率变化不够连续等问题。本文将其描述为标准二次规划问题，借以来对路径作进一步平滑处理：以轨迹的距离代价和曲率代价作为优化目标，把原始离散点与障碍物的距离引入约束条件，以保证优化后的轨迹不会与障碍物发生碰撞，最后进行优化求解。

二次规划问题是一种特殊的非线性规划问题，其数学模型可表示为：

式中：X为由所要优化的变量构成的矩阵；Q为二次型代价构成的矩阵；c为线性代价构成的矩阵；D为X的可行域。

考虑到由线性约束构成的可行域一定是凸集；而当Q矩阵为半正定矩阵时，目标函数为凸函数。同时满足上述两个条件时，该问题为凸二次规划问题，问题的局部最优解即为全局最优解，因此可以较方便地通过数值方法求得最优解。

2.1 约束条件

二次规划的目标是对路径搜索得到的路径离散点进行进一步优化，对于每一个离散点Pi，用三个变量{xi， yi， di}进行描述，因此对于初始搜索路径上的n个离散点，应共有3n个变量。如图9所示。

di为优化后的位置相对原始位置在路径法线方向上的偏移距离；（xi0， yi0）为离散点原始坐标； 为离散点的朝向，即笛卡尔坐标系中该离散点指向下一离散点的射线与x轴的夹角。

有位置约束：

式中：（ ，  ）为离散点Pi优化后的坐标。

为保证偏移后的离散点不会与障碍物发生碰撞，对偏移量 增加线性约束，其中 的约束上下界 和 根据与障碍物的实际距离计算得到，如图10所示，满足：

2.2 优化目标

对离散轨迹点的优化目标有两个，一是使其减少不必要的拐弯，二是保证路径的平滑性，避免曲率突变。因此代价函数应包含两部分，即距离代价和离散曲率代价，分别对应两个优化目标。

对于第一目标，一般来说，离散点间距之和越小，说明路径轨迹越直，拐弯越少，同时路径总长度越短，因此描述为距离代价，建立函数为：

式中： 为距离代价函数； 为距离代价权重； 为离散点间距。

对于第二目标，主要考虑轨迹离散点的离散曲率和曲率的变化率，其中，离散曲率之和越小，说明路径的平均离散曲率越小；离散曲率的变化率之和越小，说明曲率越连续，两者都会对路径的平滑性产生影响。

首先考虑离散曲率的计算方式。如图11所示，对于在圆弧上的三个离散点，假设离散点间距s近似相等，构造Pi+1相对PiPi+2对称的辅助点P'i+1，则可由△Pi+1OPi+2和△P'i+1Pi+2Pi+1的相似关系推导得到：

式中： 为Pi处的离散曲率。

在优化函数中为了保持二次形式，将其简化，得到：

式中： 为离散曲率代价函数； 为离散点处的曲率： 为曲率的变化率； 和 分别为曲率和曲率变化率对应的权重系数。

综合式（9）和式（13），得到：

式中： 为优化问题的代价函数。

综上，便将路径平滑问题描述成一个标准的二次规划问题，本文将约束条件和优化目标以稀疏矩阵的形式表达，采用OSQP开源求解器进行求解。

2.3 平滑效果

本文针对图7（b）II所示的初步搜索路径，采用不同的权重系数进行路径平滑，对应的路径离散曲率折线图如图12所示，平滑效果如图13所示。

从图12可以看出，初始搜索路径中包含大量折线尖点，说明初始路径的离散曲率突变剧烈；而二次规划优化后的路径有效改善了初始搜索路径的形状，减少了不必要的拐弯，并尽可能拉直，大大降低了路径的离散曲率。

此外，对比三种不同权重的二次规划优化结果也可以看出，权重系数的取值会影响路径的特性。利用这一优化算法，本文在该地图上随机设定起始位姿和目标位姿，选用不同的平滑权重系数进行50次重复实验。

系数选取如表1所示，箱型图结果如图14所示。

图14（a）（b）中，分布越靠下方，说明路径越平滑；图14（c）为平滑后路径与初始搜索路径的长度差，距离差在大于0的前提下越大，说明平滑后路径的距离越短，“拉直”效果越好，其表达方式为：

式中： 为平滑后路径与初始搜索路径的长度差； 为初始路径长度（对照组）； 为平滑路径长度。

通过对比s1、s2两组权重的实验结果可以看出，Wsmo2的增大可以使路径更平滑；结合图13的实验结果分析，Wsmo2的增大还可以使曲率的变化更加连续。而s3/s4与s1/s2相比的区别在于引入了Wdis，从图14可以看出，Wdis的引入会缩短路径的长度，对路径的“拉直”效果更好，但随着Wdis的增加，路径曲率最大值会明显增大，这会导致局部曲率偏大。

考虑上述三个系数对路径形状的影响后，本文在该尺度地图上最终选择的权重系数比值为Wdis：Wsmo1：Wsmo2＝0.04：1：10，选取不同目标点进行全局路径规划实验，最终效果如图15所示。可以看出，本文所提出的基于改进HybridA*的非完整约束机器人路径规划方法在各种障碍场景中均能规划出平滑且无碰撞的全局路径。

3 结论

本文针对传统HybridA*算法在应用于非完整约束机器人全局路径规划时路径容易贴近障碍物的缺陷，和算法在路径平滑后可能与障碍物冲突的问题，将全局路径规划分为路徑搜索和路径平滑两部分进行算法改进。

其中，在路径搜索部分，采用HybridA*图搜索方法，在搜索的启发式函数部分引入距离场地图，使搜索路径远离障碍物并提高搜索效率；在路径平滑部分，将平滑问题构建成二次规划问题进行求解，通过约束优化后路径离散点的极限位置以保证路径的无碰撞性，并基于向前差分的思想引入表征曲率变化率的优化代价，进一步提升路径的平滑程度。

在基于ROS平台的仿真实验结果表明，本文提出的全局路径规划算法可有效应用于非完整约束AGV，能够同时保证避障和路径平滑性，并基本满足系统的实时性要求，为AGV自主导航的精准运动奠定了基础。

参考文献：

[1]谭跃刚. 非完整机器人的原理与控制[M]. 北京：科学出版社，2011.

[2]李柏，张友民，邵之江. 自动驾驶车辆运动规划方法综述[J]. 控制与信息技术，2018（6）：1-6.

[3]Dolgov D，Thrun S，Montemerlo M，et al. Path Planning for Autonomous Vehicles in Unknown Semi-structuredEnvironments[J]. The International Journal of Robotics Research，2010，29（5）：485-501.

[4]任秉韬，王淅淅，邓伟文，等. 基于混合A～*和可变半径RS曲线的自动泊车路径优化方法[J]. 中国公路学报，2022，35（7）：317-327.

[5]Saeid S，Duong-Van N，Klaus-Dieter K. Guided Hybrid A-Star Path Planning Algorithm for ValetParking Applications[C]. Beijing， China：2019 5th International Conference on Control， Automation and Robotics，2019：570-575.

[6]Tu K.，Yang S.，Zhang H.，et al. Hybrid A* Based Motion Planning for Autonomous Vehicles inUnstructured Environment[C]. Sapporo， Japan：2019 IEEE International Symposium on Circuits and Systems，2019：1-4.

[7]陈鑫鹏，徐彪，胡满江，等. 一种基于等步长分层拓展的混合A～*路径规划方法[J]. 控制与信息技术，2021（1）：17-22，29.

[8]齐尧，徐友春，李华，等. 一种基于改进混合A～*的智能车路径规划算法[J]. 军事交通学院学报，2018，20（8）：85-90.

[9]Kurzer K. Path Planning in Unstructured Environments： A Real-time Hybrid A* Implementation for Fast and Deterministic Path Generation for the KTH Research Concept Vehicle[D]. Stockholm：KTH Royal Institute of Technology，2016.

[10]Lau B，Sprunk C，Burgard W. Improved updating of Euclidean distance maps and Voronoi diagrams[C]. IEEE/RSJ International Conference on Intelligent Robots & Systems. IEEE，2010.

收稿日期：2022-12-15

作者简介：田雨（1997－），男，天津人，硕士，主要研究方向为机械制造及其自动化，E-mail：tianyu9741@163.com。*通讯作者：林松（1957－），男，四川广元人，工学博士（德），主要研究方向为产品研发方法及其智能设计、虚拟产品生产及其数字孪生、智能装置及其人机协调、技术系统可靠性及其安全设计，E-mail：slin@tongji.edu.cn。