自动泊车路径优化方法及装置制造方法及图纸

本发明专利技术公开了一种自动泊车路径优化方法及装置，该方法包括：获取障碍物信息，并生成初始路径规划轨迹，将初始路径规划轨迹的每一段路径转换为若干个离散的初始轨迹点P

【技术实现步骤摘要】
自动泊车路径优化方法及装置


[0001]本专利技术涉及一种自动驾驶领域，尤其涉及自动泊车的路径优化。

技术介绍

[0002]在自动泊车路径规划之后，需要对路径进行平滑处理，使曲率无突变。目前主要有曲线过渡法以及优化方法，曲线过渡一般只对曲率凸变处进行光滑处理，而无梯度优化算法无法保证求解效率，并且目前大部分平滑算法可能存在局部轨迹与目标点偏差较大，过度平滑，导致车辆与障碍物存在碰撞的风险。由于路径的曲率约束为非线性，二次规划无法添加曲率约束，故现有优化后的规划路径往往具有曲率突变、车辆过度平滑导致车辆与障碍物碰撞的问题。
[0003]为此，在专利公开文件CN112277932A中，公开了一种自动驾驶的泊车路径规划方法，将路径曲率约束引入优化函数的约束条件和优化目标中，使得优化后的泊车轨迹和曲率连续，车辆沿轨迹行驶时方向盘变化连续，不需要停车。然而，这种优化方法只是对初始路径的少量点进行优化，不能保证整条路径满足约束条件和只局限于车辆初始位置限制一定范围的平行泊车规划问题，同时只能针对该专利中的路径规划方法(几何法)再进行优化，不具有通用性，难以在实际泊车路径规划中使用，且优化时，并不能解决车辆静止时大幅度打方向盘的问题。
[0004]故，急需一种可解决上述问题的自动泊车路径规划方法及装置。

技术实现思路

[0005]本专利技术的目的是提供一种自动泊车路径优化方法及装置，可有效减小优化后的规划路径过度平滑导致车辆与障碍物存在碰撞的风险。
[0006]为了实现上述目的，本专利技术公开了一种自动泊车路径优化方法，包括：获取障碍物信息，并生成初始路径规划轨迹，将初始路径规划轨迹的每一段路径转换为若干个离散的初始轨迹点P
iref
(x
iref
,y
iref
)，i＝1、2、3
…
n；以路径规划轨迹的每一段路径中各个轨迹点坐标P
i
(x
i
，y
i
)为优化变量，以限制最大曲率、起始地与目的地车辆位姿固定、各个轨迹点不与障碍物碰撞为优化约束条件，以路径的平滑度、路径长度和偏离初始轨迹点距离为优化目标，构建序列二次规划问题，对序列二次规划问题进行迭代求解以生成轨迹点信息；依据所述轨迹点信息生成优化后的规划路径；其中，所述优化目标的数值为路径平滑度、路径长度以及偏离初始轨迹点距离的权重求和，且路径平滑度中，起始地坐标P1(x1，y1)和目的地坐标P
n
(x
n
，y
n
)的权重均大于起始地坐标和目的地坐标之间轨迹点坐标P2(x2，y2)到P
n
‑1(x
n
‑1，y
n
‑1)的权重。
[0007]较佳地，限制最大曲率的优化约束条件包括：
[0008][0009]其中，向量C
max
为曲率限制值，Δl为相邻轨迹点之间
间距的均值。
[0010]较佳地，对序列二次规划问题的迭代求解时，将限制最大曲率的优化约束函数进行线性展开为：
[0011][0011]为序列二次规划上一步优化迭代得到的解；对序列二次规划问题的迭代求解时，还限制所述优化变量的运动极限。
[0012]较佳地，各个轨迹点不与障碍物碰撞的约束条件为：起始地坐标P1(x1，y1)和目的地坐标P
n
(x
n
，y
n
)不变，轨迹点坐标P2(x2，y2)到P
n
‑1(x
n
‑1，y
n
‑1)与初始轨迹点P
iref
(x
iref
,y
iref
)之间X坐标和Y坐标的偏差值均小于等于di为路径规划轨迹的每一段路径中各个初始轨迹点处对应车辆与障碍物最近的距离。
[0013]较佳地，航向角约束条件为：
[0014]yaw1和yaw2分别为依据起始地车辆位姿和目的地车辆位姿设定的起始地和目的地处的航向角给定值，P0(x0，y0)为约束车辆起始地航向角不变添加的辅助点，P
n+1
(x
n+1
，y
n+1
)为约束车辆目的地航向角不变添加的辅助点。
[0015]较佳地，路径平滑度代价为：
[0016][0017]路径长度代价为：
[0018][0019]偏离初始轨迹点距离代价为：
[0020][0021]P
smooth
、P
length
和P
ref
分别为平滑度、长度和相对初始轨迹点距离的惩罚因子，为轨迹点P
i
平滑度的权重，中为轨迹点P
i
偏离初始轨迹点距离的权重；
[0022]优化目标为：
[0023][0024]H是半正定对称矩阵，g为梯度向量，x＝{x0,y0,x1,y1,
…
，x
n+1
,y
n+1
}
T
。
[0025]较佳地，对序列二次规划问题进行迭代求解具体包括：给定所述优化变量一个运动极限并进行优化近似问题构造，对所述优化近似问题进行序列二次规划求解以获得新的优化变量，依据所述新的优化变量更新优化变量，计算优化目标的数值和优化约束条件的约束值，依据所述优化目标的数值判断是否收敛，依据所述优化约束条件的约束值判断优
化约束条件合格时是否合格，并在收敛且优化约束条件合格时输出优化变量，在未收敛且优化约束条件合格时进行下一迭代序列二次规划问题求解，在优化约束条件未合格时缩减运动极限并进行下一迭代序列二次规划问题求解。
[0026]具体地，缩减运动极限具体为：对违犯约束的优化约束条件进行预设倍率的缩减。
[0027]更佳地，在依据所述优化约束条件的约束值判断优化约束条件合格时是否合格时，还判断多次迭代中优化变量是否均处于优化变量运动区间边缘，若是则扩大运动极限增大，以提高求解效率。
[0028]具体地，扩大运动极限具体为：对多次迭代优化中优化变量处于边缘时对预设倍率的扩大。
[0029]具体地，判断所述优化目标和优化约束条件是否合格时，如果违犯限制最大曲率条件，则对优化目标中，增加路径平滑度代价的权重。
[0030]具体地，采用Hybrid A*算法进行路径规划以生成所述初始路径规划轨迹，并将初始路径规划轨迹的每一段路径分解为n+2个离散的初始轨迹点P
iref
(x
iref
,y
iref
)，i＝0、1、2、3
…
n、n+1，P
0ref
(x
0ref
,y
0ref
)为路径规划轨迹的每一段路径中约束车辆起始地航向角不变添加的辅助点，P
n+1ref
(x
n+1ref...

【技术保护点】

【技术特征摘要】
1.一种自动泊车路径优化方法，其特征在于：包括：获取障碍物信息，并生成初始路径规划轨迹，将初始路径规划轨迹的每一段路径转换为若干个离散的初始轨迹点P
iref
(x
iref
,y
iref
)，i＝1、2、3
…
n；以路径规划轨迹的每一段路径中各个轨迹点坐标P
i
(x
i
，y
i
)为优化变量，以限制最大曲率、起始地与目的地车辆位姿固定、各个轨迹点不与障碍物碰撞为优化约束条件，以路径的平滑度、路径长度和偏离初始轨迹点距离为优化目标，构建序列二次规划问题，对序列二次规划问题进行迭代求解以生成轨迹点信息；依据所述轨迹点信息生成优化后的规划路径；其中，所述优化目标的数值为路径平滑度、路径长度以及偏离初始轨迹点距离的权重求和，且路径平滑度中，起始地坐标P1(x1，y1)和目的地坐标P
n
(x
n
，y
n
)的权重均大于起始地坐标和目的地坐标之间轨迹点坐标P2(x2，y2)到P
n
‑1(x
n
‑1，y
n
‑1)的权重。2.如权利要求1所述的自动泊车路径优化方法，其特征在于：限制最大曲率的优化约束条件包括：其中，向量C
max
为曲率限制值，Δl为相邻轨迹点之间间距的均值。3.如权利要求1所述的自动泊车路径优化方法，其特征在于：对序列二次规划问题的迭代求解时，将限制最大曲率的优化约束函数进行线性展开为：代求解时，将限制最大曲率的优化约束函数进行线性展开为：为序列二次规划上一步优化迭代得到的解；对序列二次规划问题的迭代求解时，还限制所述优化变量的运动极限。4.如权利要求1所述的自动泊车路径优化方法，其特征在于：各个轨迹点不与障碍物碰撞的约束条件为：起始地坐标P1(x1，y1)和目的地坐标P
n
(x
n
，y
n
)不变，轨迹点坐标P2(x2，y2)到P
n
‑1(x
n
‑1，y
n
‑1)与对应初始轨迹点P
iref
(x
iref
,y
iref
)之间X坐标和Y坐标的偏差值均小于等于di为路径规划轨迹的每一段路径中各个初始轨迹点处对应车辆与障碍物最近的距离。5.如权利要求1所述的自动泊车路径优化方法，其特征在于：航向角约束条件为：yaw1和yaw2分别为依据起始地车辆位姿和目的地车辆位姿设定的起始地和目的地处的航向角给定值；P0(x0，y0)为路径规划轨迹的每一段路径中约束车辆起始地航向角不变添加的辅助点，P
n+1
(x
n+1
，y
n+1
)为路径规划轨迹的每一段路径中约束车辆目的地航向角不变添加的辅助点。6.如权利要求1所述的自动泊车路径优化方法，其特征在于：路径平滑度代价为：
路径长度代价为：偏离初始轨迹点距离代价为：P
smooth
、P
length
和P
ref
分别为平滑度、长度和相对初始轨迹点距离的惩罚因子，为轨迹点P
i
平滑度的权重，中为轨迹点P
i
偏离初始轨迹点距离的权重；优化目标为：H是半正定对称矩阵，g为梯度向量，x＝{x0,y0,x1,y1,
…
，x
n+1
,y
n+1
}
T
。7.如权利要求1所述的自动泊车路径优化方法，其特征在于：对序列二次规划问题进行迭代求解具体包括：给定所述优化变量一个运动极限并进行优化近似问题构造，对所述优化近似问题进行序列...

【专利技术属性】
技术研发人员：李坤，
申请(专利权)人：远峰科技股份有限公司，
类型：发明
国别省市：