[发明专利]一种用于矿区四轮转向车辆的泊车规划方法

权利要求书

1.一种用于矿区四轮转向车辆的泊车规划方法，其特征在于，包括如下步骤：

S1，根据驶入点的初始位置和初始朝向选取两种停靠位姿，停靠位姿的参数包括车辆位置和车辆朝向，所述两种停靠位姿具有相同车辆位置和不同车辆朝向；

S2，根据驶入点以及曲率半径，按照前轮转向、后轮转向和四轮转向的三种倒车模式使用reeds-sheep曲线生成多条泊车路径作为备选集合；

S3，建立评价函数用于计算车辆采用备选集合中每条泊车路径的代价值，选取备选集合中代价值最低的为最优路径；

S4，采用螺旋建模方法对S3中得出的最优路径进行可行驶区域搜索，并判断是否出现狭窄区域，若是则在备选集合中删除此次选取的最优路径并返回S3，若不是则下一步。

S5，建立二次优化模型并使用序列二次优化算法(SQP)对无狭窄区域的最优路径进行平滑操作，并进行输出，得到最终泊车路径。

2.根据权利要求1所述的一种用于矿区四轮转向车辆的泊车规划方法，其特征在于，所述S1中的两种停靠位姿的车辆朝向相反。

3.根据权利要求1所述的一种用于矿区四轮转向车辆的泊车规划方法，其特征在于，所述S2的步骤，具体包括：

S201，分别获取四轮车辆在前轮转向、后轮转向和四轮转向对应的转向几何；

S202，在三种转向模式下分别按照两种停靠位姿，根据不同的所述驶入点和不同的所述转向半径获取相应的reeds-sheep泊车路径，并添加至备选集合中；

其中，所述转向几何包括转向半径及转向中心点。

4.根据权利要求1所述的一种用于矿区四轮转向车辆的泊车规划方法，其特征在于，在S3中所述代价值的参数，包括：距离代价、方向代价和倒车切换代价。

5.根据权利要求4所述的一种用于矿区四轮转向车辆的泊车规划方法，其特征在于，在S3中所述评价函数，具体为下述公式：

f:Min(cost_distance+cost_heading+cost_isHeadBack)；

其中，cost_distance为距离代价，由泊车路径的总长度决定、cost_heading为方向代价，由泊车路径中的圆弧路径部分决定、cost_isHeadBack为倒车切换代价，由泊车路径中的倒车行驶路径部分决定。

6.根据权利要求1所述的一种用于矿区四轮转向车辆的泊车规划方法，其特征在于，所述S4中可行驶区域搜索的步骤，具体包括：

S401，对泊车路径中的每一点，向四周进行螺旋式拓展；

S402，每次拓展后对拓展点与障碍物进行距离判断，若满足要求则继续拓展，若拓展点不满足要求，将该点的坐标记录为在该拓展方向的最大点；

S403，当四个拓展方向的最大点均被寻找到时停止拓展，四个方向上最大点围成的区域即为该路径点的可行驶区域。

7.根据权利要求6所述的一种用于矿区四轮转向车辆的泊车规划方法，其特征在于，所述S402中所述距离判断为：判断所述拓展点与所述障碍物的距离是否大于所述车辆的宽度的一半。

8.根据权利要求6所述的一种用于矿区四轮转向车辆的泊车规划方法，其特征在于，所述S4中狭窄区域的判定步骤，具体为：在搜索过程中，若前4次拓展均失败或拓展后可行驶区域的面积小于0.5m²，则判定为狭窄区域。