带约束的平行泊车路径规划方法、电子设备及存储介质技术

本发明专利技术公开了一种带约束的平行泊车路径规划方法、电子设备及存储介质，属于泊车路径规划技术领域。本发明专利技术从车辆末位置开始，通过先设定末位置迭代航向角，结合车辆以及库位信息，判断是否可以规划一条无碰撞路径，如果没有，则缓慢增大航向角，直到找到一条满足碰撞约束的路径。本发明专利技术采用圆弧+切线+圆弧的路径模板能一定程度缓和曲率的不连续和曲率变化率快的缺点；可以根据车库大小和车辆尺寸，通过迭代自适应找到泊车起始区域和最佳起始泊车点；还能保证第一次入库车辆航向角相对最小并且在满足碰撞约束的前提下深入库底，以便后续泊车动作。续泊车动作。续泊车动作。

【技术实现步骤摘要】
带约束的平行泊车路径规划方法、电子设备及存储介质


[0001]本专利技术属于泊车路径规划
，具体涉及一种带约束的平行泊车路径规划方法、电子设备及存储介质。

技术介绍

[0002]为了解决停车困难的问题，人们专利技术了自动泊车辅助系统。但现阶段的自动泊车辅助系统还存在很多不完善的地方，在狭小的空间中很容易发生车辆碰撞事故，而只依赖超声波雷达对障碍物的探测不足以完全规避问题(超声波雷达存在盲区)，还是需要有更完善的路径规划算法在一开始就规避障碍物，从而避免碰撞事故的发生。
[0003]平行泊车，一般又叫做侧向停车或侧方停车，是指车子的停放方向与道路的通行方向一致，比如在马路边看到的停车的情形。目前关于平行泊车的路径规划很少。

技术实现思路

[0004]本专利技术的目的在于，提供一种带约束的平行泊车路径规划方法、电子设备及存储介质，解决平行泊车的路径规划问题以及泊车时车辆易发生碰撞的问题。
[0005]为实现上述目的，本专利技术提供了一种带约束的平行泊车路径规划方法，包括以下步骤：
[0006]S1、获取车辆的长度、宽度、轴距和转弯半径；
[0007]S2、设定末位置迭代航向角，以及车辆分别与库后边和库底边的安全阈值；末位置迭代航向角为车身与库底边的夹角；
[0008]S3、根据车辆的长度、宽度、轴距、车辆分别与库后边和库底边的安全阈值、以及末位置迭代航向角，确定车辆后轴中点坐标；根据车辆后轴中点坐标、转弯半径以及末位置迭代航向角，确定末位置转弯圆及其圆心O1；根据车辆后轴中点坐标、车辆的长度、宽度、轴距、以及末位置迭代航向角，确定车辆右前角D点坐标；
[0009]S4、根据圆心O1坐标以及车辆右前角D点坐标，确定车辆右前角转弯半径R2；获取库位的长度和宽度，结合圆心O1坐标，确定圆心O1到库前边顶点O的距离L
O1_O
；
[0010]S5、判断距离L
O1_O
与车辆右前角转弯半径R2的差值是否大于等于第一安全阈值Δ1，若是，则令X_value为正无穷大，执行步骤S7；若否，则执行步骤S6；
[0011]S6、确定圆心O1到库前边距离为R2+Δ1的临界点E，进而确定线段O1E与线段O1D的夹角θ
_error
；设X轴与库底边平行，确定车辆后轴与Y轴的夹角θ
P
，进而确定车辆后轴中点在车辆后轴与Y轴的夹角为θ
P
+θ
_error
时的横向坐标X_value；
[0012]S7、沿车辆后轴中心线向前迭代泊车起始位置；
[0013]S8、根据当前的泊车起始位置以及车辆转弯半径，确定起始位置转弯圆及其圆心O2坐标；结合末位置转弯圆，确定两圆内公切线，进而确定两圆内公切线与末位置转弯圆的切点横坐标X
k
，以及两圆内公切线与起始位置转弯圆的切点横坐标X
H
；
[0014]S9、判断X
k
是否小于等于X_value，若是，则执行步骤S10，若否，则执行步骤S7；
[0015]S10、过库前边顶点O做垂直于内公切线的直线，当垂足恰为内公切线与起始位置转弯圆的切点时，为临界点Q，确定点Q的横坐标X
Q
；判断X
H
是否大于等于X
Q
，若是则执行步骤S11，若否，则执行步骤S12；
[0016]S11、判断L
OQ
≥car_w/2+Δ2是否成立，L
OQ
为库前边顶点O到临界点Q的距离，car_w为车宽，Δ2为第二安全阈值；若成立，则结束，若不成立，则执行步骤S13；
[0017]S12、判断L
O2J
≥L
O2O
+Δ2是否成立，L
O2J
为圆心O2到车辆边缘的距离，L
O2O
为圆心O2到库前边顶点O的距离；若成立，则结束，若不成立，则执行步骤S13；
[0018]S13、增大末位置迭代航向角，并判断末位置迭代航向角是否在预设范围内，若是，则执行步骤S3。
[0019]在一些可选的实施方案中，步骤S1中，利用超声波传感器探测库位的长度和宽度。
[0020]在一些可选的实施方案中，步骤S3中，根据车辆的长度、宽度、轴距、车辆分别与库后边和库底边的安全阈值、以及末位置迭代航向角，确定车辆后轴中点坐标包括：
[0021]设库位的库底边后顶点为原点，车辆左后角B点坐标为(Xb，Yb)，车辆右后角C点坐标为(Xc，Yc)，车辆的长度、宽度、轴距依次为car_h、car_w、car_l，车辆分别与库后边和库底边的安全阈值为dx和dy，末位置迭代航向角为θ_h，则后轴中点P(X
p
，Y
p
)坐标为：
[0022][0023]当车辆与库后边和库底边的距离均为安全阈值时，Xb＝dx，Yc＝dy。
[0024]在一些可选的实施方案中，步骤S3中，根据车辆后轴中点坐标、转弯半径以及末位置迭代航向角，确定末位置转弯圆及其圆心O1包括：
[0025]设圆心O1的坐标为(X
O1
，Y
O1
)，则有：
[0026][0027]式中，r_min为车辆最小转弯半径。
[0028]在一些可选的实施方案中，步骤S3中，根据车辆后轴中点坐标、车辆的长度、宽度、轴距、以及末位置迭代航向角，确定车辆右前角D点坐标包括：
[0029][0030]式中，(Xd，Yd)为D点坐标。
[0031]在一些可选的实施方案中，步骤S6中，确定圆心O1到库前边距离为R2+Δ1的临界点E，进而确定线段O1E与线段O1D的夹角θ
_error
包括：
[0032]计算线段O1E与Y轴的角度θ1：θ1＝arcsin((x
o
‑
x
o1
)/(R2+Δ1))；X
O
为库前边顶点O的横坐标；
[0033]计算线段O1D与Y轴的角度θ2：θ2＝arcsin((x
d
‑
x
o1
)/R2)；
[0034]得到夹角θ_error＝θ1
‑
θ2。
[0035]在一些可选的实施方案中，步骤S6中，确定车辆后轴中点在车辆后轴与Y轴的夹角为θ
P
+θ
_error
时的横向坐标X_value包括：
[0036]X_value＝X
o1
+R1*cos(3*π/2+θ
p
+θ_error)
[0037]式中，R1为车辆后轴中心到圆心O1的距离。
[0038]在一些可选的实施方案中，确定两圆内公切线与末位置转弯圆的切点横坐标X
k
，以及两圆内公切线与起始位置转弯圆的切点横坐标X
H
...

【技术保护点】

【技术特征摘要】
1.一种带约束的平行泊车路径规划方法，其特征在于，包括以下步骤：S1、获取车辆的长度、宽度、轴距和转弯半径；S2、设定末位置迭代航向角，以及车辆分别与库后边和库底边的安全阈值；末位置迭代航向角为车身与库底边的夹角；S3、根据车辆的长度、宽度、轴距、车辆分别与库后边和库底边的安全阈值、以及末位置迭代航向角，确定车辆后轴中点坐标；根据车辆后轴中点坐标、转弯半径以及末位置迭代航向角，确定末位置转弯圆及其圆心O1；根据车辆后轴中点坐标、车辆的长度、宽度、轴距、以及末位置迭代航向角，确定车辆右前角D点坐标；S4、根据圆心O1坐标以及车辆右前角D点坐标，确定车辆右前角转弯半径R2；获取库位的长度和宽度，结合圆心O1坐标，确定圆心O1到库前边顶点O的距离L
O1_O
；S5、判断距离L
O1_O
与车辆右前角转弯半径R2的差值是否大于等于第一安全阈值Δ1，若是，则令X_value为正无穷大，执行步骤S7；若否，则执行步骤S6；S6、确定圆心O1到库前边距离为R2+Δ1的临界点E，进而确定线段O1E与线段O1D的夹角θ
_error
；设X轴与库底边平行，确定车辆后轴与Y轴的夹角θ
P
，进而确定车辆后轴中点在车辆后轴与Y轴的夹角为θ
P
+θ
_error
时的横向坐标X_value；S7、沿车辆后轴中心线向前迭代泊车起始位置；S8、根据当前的泊车起始位置以及车辆转弯半径，确定起始位置转弯圆及其圆心O2坐标；结合末位置转弯圆，确定两圆内公切线，进而确定两圆内公切线与末位置转弯圆的切点横坐标X
k
，以及两圆内公切线与起始位置转弯圆的切点横坐标X
H
；S9、判断X
k
是否小于等于X_value，若是，则执行步骤S10，若否，则执行步骤S7；S10、过库前边顶点O做垂直于内公切线的直线，当垂足恰为内公切线与起始位置转弯圆的切点时，为临界点Q，确定点Q的横坐标X
Q
；判断X
H
是否大于等于X
Q
，若是则执行步骤S11，若否，则执行步骤S12；S11、判断L
OQ
≥car_w/2+Δ2是否成立，L
OQ
为库前边顶点O到临界点Q的距离，car_w为车宽，Δ2为第二安全阈值；若成立，则结束，若不成立，则执行步骤S13；S12、判断L
O2J
≥L
O2O
+Δ2是否成立，L
O2J
为圆心O2到车辆边缘的距离，L
O2O
为圆心O2到库前边顶点O的距离；若成立，则结束，若不成立，则执行步骤S13；S13、增大末位置迭代航向角，并判断末位置迭代航向角是否在预设范围内，若是，则执行步骤S3。2.根据权利要求1所述的带约束的平行泊车路径规划方法，其特征在于，步骤S1中，利用超声波传感器探测库位的长度和宽度。3.根据权利要求1所述的带约束的平行泊车路径规划方法，其特征在于，步骤S3中，根据车辆的长度、宽度、轴距、车辆分别与库后边和库底边的安全阈值、以及末位置迭代航向角，确定车辆后轴中点坐标包括：设库位的库底边后顶点为原点，车辆左后角B点坐标为(Xb，Yb)，车辆右后角C点坐标为(Xc，Yc)，车辆的长度、宽度、轴距依次为car_h、car_w、car_l，车辆分别与库后边和库底边的安全阈值为dx和dy，末位置迭代航向角为θ_h，则后轴中点P(X
p
，Y
p
)坐标为：
当车辆与库后边和库底边的距离均为安全阈值时，Xb＝dx，Yc＝dy。4.根据权利要求3所述的带约束的平行泊车路径规划方法，其特征在于，...

【专利技术属性】
技术研发人员：张敬伟，方强，胡诗晨，何思，张焕期，
申请(专利权)人：东风汽车集团股份有限公司，
类型：发明
国别省市：