一种基于二次规划与并行迭代的自动泊车路径规划方法技术

本发明专利技术公开了一种基于二次规划与并行迭代的自动泊车路径规划方法，包括：(1)通过hybrid A*算法生成符合车辆运动学模型且与障碍物不发生碰撞的初始路径；(2)按照初始路径中的速度方向切换点将初始路径进行分解，得到分段初始路径集合；(3)根据分段初始路径集合中的分段路径元素构造碰撞避免的二次规划问题，通过在并行迭代中动态调整路径点权重的方法得到分段初始路径集合对应的分段无碰撞连续路径集合；(4)构造碰撞避免且曲率优化的二次规划问题，然后通过在并行迭代中动态调整优化目标中部分目标函数权重的方法得到无碰撞且满足曲率要求的泊车路径。利用本发明专利技术，可以提高车辆在自动泊车过程中的安全性、稳定性和舒适性。舒适性。舒适性。

【技术实现步骤摘要】
一种基于二次规划与并行迭代的自动泊车路径规划方法


[0001]本专利技术属于车辆自动泊车领域，尤其是涉及一种基于二次规划与并行迭代的自动泊车路径规划方法。

技术介绍

[0002]自动泊车包含感知、规划和控制等众多模块。感知模块通过图像、激光和超声波等工具对车辆运动状态以及车辆所处环境进行建模。规划模块结合感知模块提供的车辆所处的环境信息和车辆当前运动状态信息计算出符合车辆动力学且无碰撞的轨迹信息。控制模块根据规划模块输出的车辆轨迹点信息控制车辆进行运动。
[0003]如公开号为CN109606354A的中国专利文献公开了一种基于分层规划的自动泊车方法，包括感知模块、HMI显示模块、路径规划模块、控制跟踪模块，通过设于车辆四周的感知模块获取周围环境障碍物信息，推算泊车位大小、类型以及库位内是否存在障碍物，当库位尺寸符合且库位内部不存在障碍物时，进行基于数值优化的初始规划，当初始规划不满足泊车需求时，根据当前自车位姿、库位信息与环境障碍物信息，进行一次A*搜索规划和二次数值优化规划，规划成功后，将轨迹控制点发送车载控制器，车载控制器控制车辆方向盘、油门踏板和制动踏板，将车辆泊入目标库位。
[0004]规划模块是自动泊车系统的重要组成部分。规划模块输出路径信息和速度信息，这两种信息合成为轨迹信息。规划模块直接进行轨迹规划和路径
‑
速度解耦规划是当前两个研究较多的方向，轨迹规划得到的路径具有较好最优性，但计算复杂度较高；路径
‑
速度解耦规划方法的计算复杂度较低，但是会损失最终轨迹的最优性。
[0005]路径
‑
速度解耦规划在工程实践中使用较多，其中路径规划可以分为基于采样搜索的方法和基于优化的方法。基于采样搜索的方法产生的路径通常不够平滑或者不能够完全满足车辆动力学的要求。基于优化的方法根据车辆信息与环境信息进行数学建模，然后在相应的约束条件下最小化代价函数得到最优路径。
[0006]现有路径
‑
速度解耦规划的技术中，基于质点模型的路径规划需要考虑车辆最小转弯半径的约束条件，因此最终路径大多使用基于优化的方法。因为车辆最小转弯半径不能体现在质点模型中，所以曲率约束进行转换，但是大部分现有方案对曲率问题的近似过于理想，这导致在某些情况下路径规划得到路径的曲率半径小于车辆最小转向半径，这不利于下游的控制器模块对车辆进行稳定控制。

技术实现思路

[0007]本专利技术提供了一种基于二次规划与并行迭代的自动泊车路径规划方法，能够实现生成无碰撞且符合车辆运动学模型的自动泊车的路径规划，提高了车辆在自动泊车过程中的安全性、稳定性和舒适性。
[0008]一种基于二次规划与并行迭代的自动泊车路径规划方法，包括以下步骤：
[0009](1)初始路径生成阶段：在泊车场景的可行域中，通过hybrid A*路径规划算法生
成符合车辆运动学模型且与障碍物不发生碰撞的初始路径，进入初始路径分解阶段；
[0010](2)初始路径分解阶段：按照初始路径中的速度方向切换点将初始路径进行分解，得到分段初始路径集合，进入分段无碰撞连续路径生成阶段；
[0011](3)分段无碰撞连续路径生成阶段：根据分段初始路径集合中的分段路径元素构造碰撞避免的二次规划问题，然后通过在并行迭代中动态调整路径点权重的方法得到分段初始路径集合对应的分段无碰撞连续路径集合，进入曲率优化阶段；
[0012](4)曲率优化阶段：根据分段无碰撞连续路径集合中的元素构造碰撞避免且曲率优化的二次规划问题，然后通过在并行迭代中动态调整优化目标中部分目标函数权重的方法得到无碰撞且满足曲率要求的泊车路径。
[0013]步骤(1)中，与障碍物不发生碰撞的初始路径指的是车辆轮廓的地面垂直投影的顶点与障碍物地面垂直投影区域没有重叠部分；其中，障碍物指除待泊车车位与车道外的任何物体。
[0014]步骤(2)中，速度方向切换点的序列表示为：
[0015][0016]T＝{p1，Q，p
n
}
[0017]P＝{p1，...，p
n
}，n∈N
+
[0018]其中，R
min
是车辆的最小转弯半径，P是hybrid A*生成的离散路径点序列，p
k
是速度切换点，∠(p
k
‑1p
k+1
p
k
)是hybrid A*算法生成的序列离散路径点序列中第k
‑
1，k，k+1这三个路径点构成的三角形中第k点对应顶点的角度；Q是除了hybrid A*算法生成的序列离散路径起点与终点外速度方向切换点序列；T是包含了hybrid A*算法生成的序列离散路径起点与终点的速度方向切换点序列，n是hybrid A*算法生成的离散路径点数量。
[0019]步骤(2)中，分段初始路径集合表示为：
[0020]Pt＝{Pe1，...，Pe
m
‑1}
[0021]Pe是分段初始路径序列，分段初始路径序列为序列P中两个速度方向切换点之间的路径段序列，m是序列Q中元素的数量。
[0022]步骤(3)中，碰撞避免的二次规划问题构造过程如下：
[0023](3
‑
1)利用五次多项式f
x
(s)表征无碰撞连续路径在世界坐标系中的横坐标序列x与无碰撞连续路径长度序列s的关系，函数解析式表示为：
[0024]f
x
(s)＝a5s5+a4s4+a3s3+a2s2+a1s1+a0[0025]其中，a0～a5为五次多项式的系数；序列s中的元素表示车辆从分段初始路径起点运动到分段初始路径中其他各个路径点的路径长度；
[0026](3
‑
2)利用五次多项式f
y
(s)表征无碰撞连续路径在世界坐标系中的纵坐标y与无碰撞连续路径长度s的关系，函数解析式表示为：
[0027]f
y
(s)＝b5s5+b4s4+b3s3+b2s2+b1s1+b0[0028]其中，b0～b5为五次多项式的系数；
[0029](3
‑
3)根据(3
‑
1)和(3
‑
2)构建二次函数的目标函数中的J1部分：
[0030][0031]其中，t是分段初始路径中路径点的数量，序列c表示分段初始路径中各个路径点的权重系数，初始数值为单位向量[1...1]1×
t；x
kref
，y
kre
表示分段初始路径中各个路径点在世界坐标系中的横坐标和纵坐标；s
k
即为车辆从分段初始路径起点运动到分段初始路径的第k个路径点的路径长度；
[0032](3
‑
4)根据(3
‑
1)和(3
‑
2)构建二次函数的目标函数中的J2部分：
[本文档来自技高网...

【技术保护点】

【技术特征摘要】
其中，a0～a5为五次多项式的系数；序列s中的元素表示车辆从分段初始路径起点运动到分段初始路径中其他各个路径点的路径长度；(3
‑
2)利用五次多项式f
y
(s)表征无碰撞连续路径在世界坐标系中的纵坐标y与无碰撞连续路径长度s的关系，函数解析式表示为：f
y
(s)＝b5s5+b4s4+b3s3+b2s2+b1s1+b0其中，b0～b5为五次多项式的系数；(3
‑
3)根据(3
‑
1)和(3
‑
2)构建二次函数的目标函数中的J1部分：其中，t是分段初始路径中路径点的数量，序列c表示分段初始路径中各个路径点的权重系数，初始数值为单位向量[1
…
1]1×
t
；x
kref
,y
kref
表示分段初始路径中各个路径点在世界坐标系中的横坐标和纵坐标；s
k
即为车辆从分段初始路径起点运动到分段初始路径的第k个路径点的路径长度；(3
‑
4)根据(3
‑
1)和(3
‑
2)构建二次函数的目标函数中的J2部分：其中，f
x(1)
(s)表示f
x
(s)对变量s的一阶导数，f
y(1)
(s)表示f
y
(s)对变量s的一阶导数；(3
‑
5)根据(3
‑
1)和(3
‑
2)构建二次函数的目标函数中的J3部分：其中，f
x(2)
(s)表示f
x
(s)对变量s的二阶导数，f
y(2)
(s)表示f
y
(s)对变量s的二阶导数；(3
‑
6)根据(3
‑
3)～(3
‑
5)构建碰撞避免的二次规划目标函数表示为J：J＝w1*J1+w2*J2+w3*J3其中，w1，w2，w3为各个子目标函数的权重系数；(3
‑
7)碰撞避免的二次规划问题表示为：min J＝w1*J1+w2*J2+w3*J3s.t. f
x
(s1)＝x
1ref
，f
y
(s1)＝y
1ref
f
x
(s
t
)＝x
tref
，f
y
(s
t
)＝y
tref
f
x
(s2)＝x
2ref
，f
y
(s2)＝y
2ref
f
x
(s
t
‑1)＝x
t
‑
1ref
，f
y
(s
t
‑1)＝y
t
‑
1ref
。6.根据权利要求5所述的基于二次规划与并行迭代的自动泊车路径规划方法，其特征在于，步骤(3)中，并行迭代中动态调整路径点权重的方法具体为：并行指的是根据分段初始路径集合中的分段初始路径元素通过多线程并行计算的方式并行构造碰撞避免的二次规划问题并求解；迭代指的是在每个线程里建立单重循环逻辑，循环中进行碰撞检测，如果碰撞避免的二次规划问题生成的泊车路径没有发生碰撞则退出循环，如果发生碰撞则增加
序列c中碰撞路径点及其附近路径点的权重数值，然后重新进行循环判断，直至找到无碰撞的泊车路径。7.根据权利要求1所述的基于二次规划与并行迭代的自动泊车路径规划方法，其特征在于，步骤(3)和步骤(4)中，碰撞的检测方法为：将二次规划生成的泊车路径进行离散采样，然后计算车辆在各个离散路径点的姿态与障碍物的碰撞情况。8.根据权利要求1所述的基于二次规划与并行迭代的自动泊车路径规划方法，其特征在于，步骤(4)中，构造碰撞避免且曲率优化的二次规划问题具体为：(4
‑
1)利用五次多项式g
x
(d)表征泊车路径在世界坐标系中的横坐标x与泊车路径长度d的关系，函数解析式表示为：g
x
...

【专利技术属性】
技术研发人员：谢磊，李周衡，胡铖，多然，周肖铃，翁汪佳，苏宏业，
申请(专利权)人：浙江大学，
类型：发明
国别省市：