【技术实现步骤摘要】
本专利技术涉及自主泊车规划领域，特别是涉及一种非结构化场景自主泊车轨迹规划方法及相关装置。
技术介绍
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、另一方面，本专利技术提供了一种计算机设备，包括：存储器、处理器以存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计本文档来自技高网...
【技术保护点】
1.一种非结构化场景自主泊车轨迹规划方法，其特征在于，包括：
2.根据权利要求1所述的一种非结构化场景自主泊车轨迹规划方法，其特征在于，根据路径搜索算法和插值算法，基于车辆初始位姿和车辆目标位姿，构建一初始路径曲线，具体包括：
3.根据权利要求1所述的一种非结构化场景自主泊车轨迹规划方法，其特征在于，根据所述初始路径曲线，确定当前解，具体包括：
4.根据权利要求1所述的一种非结构化场景自主泊车轨迹规划方法，其特征在于，所述非结构化自主泊车轨迹规划问题，如下式所示：
5.根据权利要求1所述的一种非结构化场景自主泊车轨迹规划方法，其特征在于，所述碰撞包括A类碰撞和B类碰撞；所述A类碰撞为车辆顶点与障碍物接触；所述B类碰撞为障碍物顶点与车辆接触；所述关键避障约束总集包括A类避障约束总集和B类避障约束总集；所述A类避障约束总集包括各离散化时刻的A类避障约束集；所述B类避障约束总集包括各离散化时刻的B类避障约束集；
6.根据权利要求1所述的一种非结构化场景自主泊车轨迹规划方法，其特征在于，在基于所述优化解更新所述优化解中的辅助变量
7.根据权利要求1所述的一种非结构化场景自主泊车轨迹规划方法，其特征在于，所述路径搜索算法为HA搜索算法；所述插值算法为基于速度梯形曲线的三次样条插值算法。
8.一种计算机装置，包括：存储器、处理器以存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序以实现权利要求1-7中任一项所述的一种非结构化场景自主泊车轨迹规划方法的步骤。
9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该计算机程序被处理器执行时实现权利要求1-7中任一项所述的一种非结构化场景自主泊车轨迹规划方法的步骤。
10.一种计算机程序产品，包括计算机程序，其特征在于，该计算机程序被处理器执行时实现权利要求1-7中任一项所述的一种非结构化场景自主泊车轨迹规划方法的步骤。
...【技术特征摘要】
1.一种非结构化场景自主泊车轨迹规划方法，其特征在于，包括：
2.根据权利要求1所述的一种非结构化场景自主泊车轨迹规划方法，其特征在于，根据路径搜索算法和插值算法，基于车辆初始位姿和车辆目标位姿，构建一初始路径曲线，具体包括：
3.根据权利要求1所述的一种非结构化场景自主泊车轨迹规划方法，其特征在于，根据所述初始路径曲线，确定当前解，具体包括：
4.根据权利要求1所述的一种非结构化场景自主泊车轨迹规划方法，其特征在于，所述非结构化自主泊车轨迹规划问题，如下式所示：
5.根据权利要求1所述的一种非结构化场景自主泊车轨迹规划方法，其特征在于，所述碰撞包括a类碰撞和b类碰撞；所述a类碰撞为车辆顶点与障碍物接触；所述b类碰撞为障碍物顶点与车辆接触；所述关键避障约束总集包括a类避障约束总集和b类避障约束总集；所述a类避障约束总集包括各离散化时刻的a类避障约束集；所述b类避障约束总集包括各离散化时刻的b类避障约束集；
6.根据权...
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。