[发明专利]一种基于HybridA*算法的拖挂牵引车泊车路径规划方法

权利要求书

1.一种基于Hybrid A*算法的拖挂牵引车泊车路径规划方法，其特征在于，包括：

获取拖挂牵引车车辆参数：牵引车后轴中心距牵引车前端、后端、鞍点的距离，牵引车宽度、轴距；挂车前端、后端距鞍点距离，挂车宽度、轴距；牵引车车轮最大转向角；

获取起始点及目标点位姿信息；所述位姿信息由牵引车位置坐标，牵引车横摆角，挂车横摆角组成；

定义规划仿真参数：牵引车行驶步长，转向步长，栅格地图步长，横摆角保留步长，各惩罚成本系数；

坐标转换：将牵引车起始点、目标点坐标根据所述栅格地图步长转换到栅格地图对应坐标上；

ReedsShepp连接：使用ReedsShepp曲线连接当前运动节点和目标点，并对连接曲线进行碰撞检测，保留有效连接线；设置连接线第一惩罚成本；在保留的有效连接线中选择第一惩罚成本最小的连接线，路径规划完成。

2.根据权利要求1所述的基于Hybrid A*算法的拖挂牵引车泊车路径规划方法，其特征在于，所述第一惩罚成本包括车辆行驶方向变化成本、车轮转向变化成本、车辆行驶距离。

3.根据权利要求1所述的基于Hybrid A*算法的拖挂牵引车泊车路径规划方法，其特征在于，所述路径规划方法还包括：如果无有效连接线，则继续更新节点。

4.根据权利要求3所述的基于Hybrid A*算法的拖挂牵引车泊车路径规划方法，其特征在于，所述更新节点的过程具体包括：

以当前位姿为拖挂牵引车运动始点，基于拖挂牵引车运动学模型，对所述拖挂牵引车进行转向及运动方向的遍历，获得车辆可到达节点位姿；

对可到达节点的路径进行碰撞检测，保留有效节点；对所述有效节点进行第二惩罚成本计算后进行筛选、合并，并根据所述第二惩罚成本递增进行排序。

5.根据权利要求4所述的基于Hybrid A*算法的拖挂牵引车泊车路径规划方法，其特征在于，所述拖挂牵引车运动学模型中的运动学公式为：

(1)牵引车运动轨迹：

更新牵引车横摆角：

更新牵引车后轴中心点坐标：

(2)挂车运动状态：

更新挂车曲率：

更新挂车横摆角：

其中参数表示牵引车横摆角、表示挂车横摆角、s表示仿真步长、ρ_i表示牵引车曲率、ρ_j表示挂车曲率、λ_i表示鞍点到牵引车后轴的长度、L_j表示挂车长度。

6.根据权利要求1或4所述的基于Hybrid A*算法的拖挂牵引车泊车路径规划方法，其特征在于，所述碰撞检测包括：拖挂牵引车自身运动时牵引车与拖挂是否碰撞；拖挂牵引车与已知障碍点是否碰撞；拖挂牵引车与停车位包络线是否发生碰撞。

7.根据权利要求4所述的基于Hybrid A*算法的拖挂牵引车泊车路径规划方法，其特征在于，所述第二惩罚成本＝累加成本+启发成本；

所述累加成本包括：车辆行驶方向变化成本，车轮转向成本，车轮转向变化成本，车辆行驶距离，上一节点累加成本；

所述启发成本包括：牵引车和拖挂距目标点牵引车和拖挂距离，牵引车和拖挂横摆角与目标点牵引车和拖挂横摆角差值，牵引车与拖挂夹角。