一种非结构化场景自主泊车轨迹规划方法及相关装置

本发明涉及自主泊车规划领域，特别是涉及一种非结构化场景自主泊车轨迹规划方法及相关装置。

背景技术：

1、随着科技水平的进步，人们对车辆的要求除了基础的通行能力以外，还要求车辆具备一定的智能，主要体现在智能驾驶与智能泊车两方面。现阶段研究通常基于障碍物是否规则摆放将自主泊车场景分为两类：结构化场景与非结构化场景。针对结构化场景（例如地下停车场），基于泊车范式（例如倒车入库、侧方位停车等）的方法可以生成较为完备的泊车轨迹。然而，这类方法在实际应用中受到较大限制，主要原因是结构化场景很容易转化为非结构化场景，例如当随意放置的手推车或违规停放的他车存在时，地下停车场也会变为非结构化场景。针对非结构化场景下的自主泊车轨迹规划，非线性优化是当前的主流方法。然而，由于避障约束维度高、非线性强，求解非结构化场景下自主泊车优化问题耗时长，通常需要15s以上，且容易收敛于较差的局部最优。因此，如何高效、高质量地实现非结构化场景下的自主泊车，是本领域亟待解决的问题。

技术实现思路

1、本发明的目的是提供一种非结构化场景自主泊车轨迹规划方法及相关装置，可在非结构化场景下，高效地规划出高质量的自主泊车轨迹。

2、为实现上述目的，本发明提供了如下方案。

3、一方面，本发明提供了一种非结构化场景自主泊车轨迹规划方法，包括以下步骤。

4、根据路径搜索算法和插值算法，基于车辆初始位姿和车辆目标位姿，构建一初始路径曲线；初始路径曲线中包括若干个车辆位姿点；车辆位姿点包括控制变量向量和状态变量向量。

5、根据初始路径曲线，确定当前解；当前解包括自主泊车总用时、状态变量向量集、控制变量向量集和辅助变量向量集。

6、根据当前解和非线性优化方法，求解非结构化自主泊车轨迹规划问题，得到优化解。

7、基于优化解，判断是否发生碰撞，得到第一判断结果。

8、若第一判断结果为是，则基于优化解更新优化解中的辅助变量向量集并确定关键避障约束总集。

9、将辅助变量向量集更新后的优化解作为当前解，并跳转至步骤：根据当前解和非线性优化方法，求解非结构化自主泊车轨迹规划问题，得到优化解。

10、若第一判断结果为否，则基于当前解，判断是否发生碰撞，得到第二判断结果。

11、若第二判断结果为是，则将优化解作为当前解，跳转至步骤：根据当前解和非线性优化方法，求解非结构化自主泊车轨迹规划问题，得到优化解。

12、若第二判断结果为否，则将优化解作为最终解。

13、可选地，根据路径搜索算法和插值算法，基于车辆初始位姿和车辆目标位姿，构建一初始路径曲线，具体包括以下步骤。

14、根据路径搜索算法，基于车辆初始位姿和车辆目标位姿，构建一粗糙路径曲线。

15、根据插值算法，对粗糙路径曲线进行插值，得到初始路径曲线。

16、可选地，根据初始路径曲线，确定当前解，具体包括以下步骤。

17、基于初始路径曲线中车辆位姿点的数量和相邻两个车辆位姿点的时间间隔，确定自主泊车总用时。

18、基于初始路径曲线中各车辆位姿点的状态变量向量，确定状态变量向量集。

19、基于初始路径曲线中各车辆位姿点的控制变量向量，确定控制变量向量集。

20、将一个空集作为辅助变量向量集。

21、基于自主泊车总用时、状态变量向量集、控制变量向量集和辅助变量向量集，确定当前解。

22、可选地，非结构化自主泊车轨迹规划问题，如下式所示。

23、 。

24、其中，t为自主泊车总用时，x为状态变量向量集，u为控制变量向量集，λ为辅助变量向量集，x0为第一个位姿点的状态变量向量，xs为车辆初始位姿的状态变量向量，xn为最后一个位姿点的状态变量向量，xf为车辆目标位姿的状态变量向量，k为离散化时间索引，n为车辆位姿点的数量，h为离散化时间间隔，f(xk+1,uk)为隐式欧拉形式的车辆运动学模型，uk为k时刻的控制变量向量，为控制变量向量下限，为控制变量向量上限，为状态变量向量下限，为状态变量向量上限，为k时刻状态变量向量初始值，c为方形约束的大小，i,j,m均为下标，为k时刻的a类避障约束集，ai和bi为第i个障碍物的半空间表示，pj(xk)为k时刻车辆第j个顶点的全局位置，λm为第m个辅助变量向量，为k时刻的b类避障约束集，g和g为车辆的半空间表示，为k时刻车辆的相对旋转矩阵，为转置符号，pij为第i个障碍物的第j个顶点的全局位置，t(xk)为k时刻车辆的相对平移向量。

25、可选地，碰撞包括a类碰撞和b类碰撞；a类碰撞为车辆顶点与障碍物接触；b类碰撞为障碍物顶点与车辆接触；关键避障约束总集包括a类避障约束总集和b类避障约束总集；a类避障约束总集包括各离散化时刻的a类避障约束集；b类避障约束总集包括各离散化时刻的b类避障约束集。

26、基于优化解更新优化解中的辅助变量向量集并确定关键避障约束总集，具体包括以下步骤。

27、将优化解的辅助变量向量集、a类避障约束集和b类避障约束集初始化为空集。

28、判断碰撞是否为a类碰撞，得到第三判断结果。

29、若第三判断结果为是，则基于优化解，生成一辅助变量向量，并将辅助变量向量添加到优化解的辅助变量向量集中。

30、基于碰撞和辅助变量向量生成a类避障约束，将a类避障约束添加到发生碰撞的离散化时刻的a类避障约束集中；a类避障约束中包括碰撞中的障碍物的索引、碰撞中的车辆顶点的索引和辅助变量向量的索引。

31、若第三判断结果为否，则判断碰撞是否为b类碰撞，得到第四判断结果。

32、若第四判断结果为是，则基于优化解，生成一辅助变量向量，并将辅助变量向量添加到优化解的辅助变量向量集中。

33、基于碰撞和辅助变量向量生成b类避障约束，将b类避障约束添加到发生碰撞的离散化时刻的b类避障约束集中；b类避障约束中包括碰撞中的障碍物的索引、碰撞中的障碍物的顶点的索引和辅助变量向量的索引。

34、可选地，在基于优化解更新优化解中的辅助变量向量集并确定关键避障约束总集之后，还包括以下步骤。

35、基于关键避障约束总集，确定一中间避障约束总集。

36、针对中间避障约束总集中的任一离散化时刻的避障约束集，将待传导避障约束添加到避障约束集中，得到更新后的中间避障约束总集；待传导避障约束为关键避障约束总集中除离散化时刻以外的其他离散化时刻的避障约束集中的避障约束；避障约束集包括a类避障约束集和b类避障约束集。

37、将更新后的中间避障约束总集，作为关键避障约束总集。

38、可选地，路径搜索算法为ha搜索算法；插值算法为基于速度梯形曲线的三次样条插值算法。

39、另一方面，本发明提供了一种计算机设备，包括：存储器、处理器以存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序以实现上述任一项所述的一种非结构化场景自主泊车轨迹规划方法的步骤。

40、另一方面，本发明提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现上述任一项所述的一种非结构化场景自主泊车轨迹规划方法的步骤。

41、另一方面，本发明提供了一种计算机程序产品，包括计算机程序，该计算机程序被处理器执行时实现上述任一项所述的一种非结构化场景自主泊车轨迹规划方法的步骤。

42、根据本发明提供的具体实施例，本发明公开了以下技术效果。

43、本发明提供了一种非结构化场景自主泊车轨迹规划方法及相关装置，方法包括以下步骤：先基于车辆初始位姿和车辆目标位姿确定一初始路径曲线；随后根据初始路径曲线，确定当前的自主泊车总用时、状态变量向量集、控制变量向量集和辅助变量向量集，作为当前解；再基于非线性优化方法，求解非结构化自主泊车轨迹规划问题，得到相应的优化解；在判断采用优化解发生碰撞时，更新优化解中的辅助变量向量集并确定关键避障约束总集，然后重新求解规划问题并判断是否发生碰撞，不断迭代，直到连续两次的优化解未发生碰撞时，将最后一次的优化解作为最终解，其中包括每一时刻的状态变量和对应的控制变量向量集，以及在自主泊车期间避障需要用到的辅助变量向量集，根据最终解进行自主泊车。本发明通过求解加累积的迭代过程，不断生成新的优化解，直到连续两次求解结果是无碰撞的，期间仅累积发生碰撞时的关键约束，不考虑对泊车轨迹不产生影响的无关约束，能够降低避障约束的非线性与维度，提高了自主泊车轨迹规划效率及自主泊车轨迹的质量。