[发明专利]基于反向搜索的曲率连续自主泊车路径规划方法及系统

说明书

本发明公开一种基于反向搜索的曲率连续自主泊车路径规划方法及系统，方法包括如下步骤：初始化泊车的栅格地图、起点和终点；将原始的起点和终点进行节点置换；基于新的起点和终点进行CC‑HA*节点扩展，最新扩展的节点满足终点截止条件时，经过回溯得到初始路径序列；根据初始路径序列进行路径反转，得到反向路径序列；对目标点松连接处理，路径终点与目标车位重合，设置搜索截止条件，将路径规划分为反向搜索阶段和松连接阶段，得到最终泊车路径，将Hybrid A*与反向搜索相结合，保证路径的一阶曲率连续，充分考虑障碍物在行驶车道和泊车位周围的不平衡分布，通过从目标泊车位向车辆当前位置进行反向搜索，可以减少搜索过程中的无用扩展节点，提高效率。

技术领域

本发明属于无人驾驶路径规划领域，具体涉及一种基于反向搜索的曲率连续自主泊车路径规划方法及系统。

背景技术

作为自动驾驶技术的一项重要内容，自主泊车已经经历了四个阶段：APA(Autoparking Assist)，RPA(Remote Parking Assist)，HPA(Home-zone Parking Assist)和AVP(Automated Valet Parking)，相应于L1-L4级别自动驾驶。一个典型的自主泊车系统包括车位识别、路径规划、路径跟踪，车辆定位和底盘控制，本发明主要针对自主泊车规划任务。

泊车规划可以被定义为这样一个过程，在短时间内，无人车生成一条平滑无碰撞的路径，并且满足目标车位的位置和方向要求。由于无人车的非完整性约束(即由于有限的转弯半径，车辆的横向移动受到限制，在没有纵向移动的情况下无法改变方向)，之前的方法主要集中在提高路径的平滑性上面，例如路径连续、曲率连续和更高阶曲率连续。这些方法能够被分为四类：基于栅格搜索的算法、基于采样的算法、基于优化的算法和基于学习的算法。

对于栅格搜索类的算法，经典的A*算法和它的变体D*、ARA*、Theta*仅仅使用直线段作为搜索过程中的运动基元，规划出来的路径是一段折线，不能够直接作为无人车的行驶路径。Hybrid A*算法增加了圆弧段到运动基元中来提高路径的平滑程度，从而满足了无人车的非完整性运动学约束。由此出现了许多基于Hybrid A*的泊车规划算法，最近提出的CC-HA*算法增加了螺线段到运动基元中，从而进一步提高了路径的平滑程度。通过引入多种类型的运动基元能够提高路径的平滑性，但是搜索空间的维度增大，从而带来了更多的时间开销。

对于采样类的算法，经典的RRT和它的几个变体RRT-connect，Anytime RRTs，RRT*，由于非完整性约束，不能直接被用作泊车规划。Hybrid Curvature Steer算法提高了路径的平滑性，但也带来了更大的时间开销。优化类的算法将泊车规划问题当作一个优化问题，约束包括车辆运动学约束、物理约束、碰撞检测约束，但是优化类算法经常会遇到局部极小值的问题，并且优化问题的求解经常需要很长的时间开销。学习类的算法可以使用一个基于模型的强化学习算法来迭代地执行数据生成、数据评估和模型训练，然而，学习类的算法多针对仿真环境，很难进行真实泊车场景下面的实验验证。

综上，当前众多泊车规划算法难以兼顾路径平滑性、终点位姿精确性、以及计算实时性三个方面的综合性能指标，实用性仍有待提高。

发明内容

为了解决现有技术中存在的问题，本发明提供一种基于反向搜索的曲率连续自主泊车路径规划方法(RS-HA*)，将CC-HA*与反向搜索策略相结合，提出一种路径反转处理算法与一种换挡点处理算法，并提出一种松连接策略来解决初始位姿不重合问题。

为了实现上述目的，本发明采用的技术方案是：一种基于反向搜索的曲率连续自主泊车路径规划方法，具体包括如下步骤：

初始化泊车的栅格地图、起点和终点，将搜索节点通过栅格地图离散化；

将原始的起点和终点进行节点置换，得到新的起点和终点；