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

权利要求书

1.一种基于反向搜索的曲率连续自主泊车路径规划方法，其特征在于，具体包括如下步骤：

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

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

基于所述新的起点和终点进行CC-HA*节点扩展，最新扩展的节点满足终点截止条件时，经过回溯得到初始路径序列

根据初始路径序列进行路径反转，得到反向路径序列

对于反向路径序列中的每一个换挡点，对其布尔变量id_back进行取反处理，最终得到换挡点路径

基于换挡点路径对目标点松连接处理，路径终点与目标车位重合，设置搜索截止条件，将路径规划分为反向搜索阶段和松连接阶段，得到最终泊车路径。

2.根据权利要求1所述的基于反向搜索的曲率连续自主泊车路径规划方法，其特征在于，初始化栅格地图、起点和终点，搜索节点经过栅格地图离散化后记为：

S＝[id_x，id_y，id_theta，id_kappa，id_moton，id_back]^T，

其中id_kappa是一个有限离散数值变量，用公式表示为：

其中id_moton是一个枚举变量，表示相邻节点曲率的增减，用公式表示为：

其中id_back是一个布尔变量，表示当前节点前进或者后退方向，用公式表示为：

搜索最终得到的路径记为将车辆当前位置作为起点S₀，目标车位作为终点S_g。

3.根据权利要求2所述的基于反向搜索的曲率连续自主泊车路径规划方法，其特征在于，将原始的起点和终点进行节点置换具体为：将原始的起点S₀和终点S_g进行节点置换，即把目标车位的位置和朝向作为搜索的起点，车辆当前位置和朝向作为搜索的终点，得到新的起点S_n0＝S_g和终点S_ng＝S₀。

4.根据权利要求1所述的基于反向搜索的曲率连续自主泊车路径规划方法，其特征在于，基于所述新的起点和终点进行CC-HA*节点扩展，最新扩展的节点满足终点截止条件时，经过回溯得到初始路径序列具体如下：

从链表OPEN_LIST中选出f值最小的节点作为当前节点，f值的计算包括代价函数和启发函数两部分，

f(n)＝g(n)+h(n)

其中g(n)表示当前节点与搜索起点代价函数，使用距离起点的代价进行度量，h(n)表示与目标节点的启发函数，使用如下公式计算：

启发函数包括两部分，第一部分h_R-S是从车辆当前位置到终点的Reeds-Shepp曲线长度，表示只考虑车辆的运动学约束，不考虑周围的障碍物；第二部分h_2D是2D A*的路径长度，表示只是考虑周围的障碍物，而不考虑生成路径是否可行驶；

判断最新扩展的节点是否满足终点截止条件，如果不是，将其从OPEN_LIST删除，加入CLOSED_LIST；如果满足终点截止条件，便结束搜索过程，从该节点依次向前回溯父节点得到初始路径序列

5.根据权利要求1所述基于反向搜索的曲率连续自主泊车路径规划方法，其特征在于，根据初始路径序列进行路径反转，得到反向路径序列具体为：对于路径中的每一个节点进行翻转操作，将描述车辆运动方向的布尔变量id_back取反，离散的数值变量id_kappa绝对值保持不变，改变符号，描述曲率变化的枚举变量id_moton也取反，id_x，id_y，id_theta保持不变，得到反向路径序列

6.根据权利要求1所述的基于反向搜索的曲率连续自主泊车路径规划方法，其特征在于，反向搜索阶段具体为：从起点到终点运行反向搜索算法；当到达路径中途适当区域中任何一个已经搜索到的节点时，提前结束该阶段的反向搜索过程，并将该路径中点作为反向搜索阶段的最后一个节点；

松连接阶段具体为：重新规划一条从当前节点到目标位置的路径，并将其与反向搜索阶段得到的路径首尾相接，最终形成完整的泊车规划路径。