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

1.本发明属于泊车路径规划技术领域，具体涉及一种带约束的平行泊车路径规划方法、电子设备及存储介质。


背景技术：

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


技术实现要素：

4.本发明的目的在于，提供一种带约束的平行泊车路径规划方法、电子设备及存储介质，解决平行泊车的路径规划问题以及泊车时车辆易发生碰撞的问题。
5.为实现上述目的，本发明提供了一种带约束的平行泊车路径规划方法，包括以下步骤：
6.s1、获取车辆的长度、宽度、轴距和转弯半径；
7.s2、设定末位置迭代航向角，以及车辆分别与库后边和库底边的安全阈值；末位置迭代航向角为车身与库底边的夹角；
8.s3、根据车辆的长度、宽度、轴距、车辆分别与库后边和库底边的安全阈值、以及末位置迭代航向角，确定车辆后轴中点坐标；根据车辆后轴中点坐标、转弯半径以及末位置迭代航向角，确定末位置转弯圆及其圆心o1；根据车辆后轴中点坐标、车辆的长度、宽度、轴距、以及末位置迭代航向角，确定车辆右前角d点坐标；
9.s4、根据圆心o1坐标以及车辆右前角d点坐标，确定车辆右前角转弯半径r2；获取库位的长度和宽度，结合圆心o1坐标，确定圆心o1到库前边顶点o的距离l
o1_o
；
10.s5、判断距离l
o1_o
与车辆右前角转弯半径r2的差值是否大于等于第一安全阈值δ1，若是，则令x_value为正无穷大，执行步骤s7；若否，则执行步骤s6；
11.s6、确定圆心o1到库前边距离为r2 δ1的临界点e，进而确定线段o1e与线段o1d的夹角θ
_error
；设x轴与库底边平行，确定车辆后轴与y轴的夹角θ
p
，进而确定车辆后轴中点在车辆后轴与y轴的夹角为θ
p
θ
_error
时的横向坐标x_value；
12.s7、沿车辆后轴中心线向前迭代泊车起始位置；
13.s8、根据当前的泊车起始位置以及车辆转弯半径，确定起始位置转弯圆及其圆心o2坐标；结合末位置转弯圆，确定两圆内公切线，进而确定两圆内公切线与末位置转弯圆的切点横坐标x
k
，以及两圆内公切线与起始位置转弯圆的切点横坐标x
h
；
14.s9、判断x
k
是否小于等于x_value，若是，则执行步骤s10，若否，则执行步骤s7；
15.s10、过库前边顶点o做垂直于内公切线的直线，当垂足恰为内公切线与起始位置转弯圆的切点时，为临界点q，确定点q的横坐标x
q
；判断x
h
是否大于等于x
q
，若是则执行步骤s11，若否，则执行步骤s12；
16.s11、判断l
oq
≥car_w/2 δ2是否成立，l
oq
为库前边顶点o到临界点q的距离，car_w为车宽，δ2为第二安全阈值；若成立，则结束，若不成立，则执行步骤s13；
17.s12、判断l
o2j
≥l
o2o
δ2是否成立，l
o2j
为圆心o2到车辆边缘的距离，l
o2o
为圆心o2到库前边顶点o的距离；若成立，则结束，若不成立，则执行步骤s13；
18.s13、增大末位置迭代航向角，并判断末位置迭代航向角是否在预设范围内，若是，则执行步骤s3。
19.在一些可选的实施方案中，步骤s1中，利用超声波传感器探测库位的长度和宽度。
20.在一些可选的实施方案中，步骤s3中，根据车辆的长度、宽度、轴距、车辆分别与库后边和库底边的安全阈值、以及末位置迭代航向角，确定车辆后轴中点坐标包括：
21.设库位的库底边后顶点为原点，车辆左后角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
包括：
[0039]
计算两圆圆心连线与两圆内公切线的交点l坐标(x
l
,y
l
)，进一步得到线段l
lo1
长度，计算：
[0040]
θ
a
＝arcsin((x
l
‑
x
o1
)/l
lo1
)
[0041]
θ
b
＝arccos(r1/l
lo1
)
[0042]
θ
e
＝θ
a
‑
θ
b
[0043]
计算切点k坐标：
[0044]
x
k
＝x
o1
r1*cos(3*π/2 θ
e
)
[0045]
y
k
＝y
o1
r1*sin(3*π/2 θ
e
)
[0046]
若两次转弯半径相同，则圆o1与圆o2半径相等，因此同理可得切点h的坐标：
[0047]
x
h
＝x
o2
r2*cos(π/2 θ
e
)
[0048]
y
h
＝y
o2
r2*sin(π/2 θ
e
)
[0049]
最终可得切线方程。
[0050]
一种电子设备，包括一个或多个处理器以及存储器；一个或多个程序被存储在存储器中并被配置为由一个或多个处理器执行，一个或多个程序配置用于执行上述的带约束的平行泊车路径规划方法。
[0051]
一种计算机可读存储介质，计算机可读存储介质中存储有程序代码，其中，在程序代码运行时执行上述的带约束的平行泊车路径规划方法。
[0052]
本发明与现有技术相比，具有以下优点及有益效果：
[0053]
1、采用圆弧 切线 圆弧的路径模板能一定程度缓和曲率的不连续和曲率变化率快的缺点；
[0054]
2、根据车库大小和车辆尺寸，通过迭代自适应找到泊车起始区域和最佳起始泊车点；
[0055]
3、保证第一次入库车辆航向角相对最小并且在满足碰撞约束的前提下深入库底，以便后续泊车动作。
附图说明
[0056]
图1为车辆分别与库后边和库底边的安全阈值示意图；
[0057]
图2为碰撞点1和2的示意图；
[0058]
图3为碰撞点3、4和5的示意图；
[0059]
图4为碰撞点4和5的碰撞约束原理图；
[0060]
图5为碰撞点3、4和5的碰撞约束原理图；
[0061]
图6为沿车辆后轴中心线向前迭代泊车起始位置示意图；
[0062]
图7为碰撞点2的碰撞约束原理图；
[0063]
图8为带约束的平行泊车路径规划方法流程图。
具体实施方式
[0064]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
[0065]
本发明实施例提供了一种带约束的平行泊车路径规划方法，如图8所示，包括以下步骤：
[0066]
s1、获取车辆的长度、宽度、轴距和转弯半径；
[0067]
s2、设定末位置迭代航向角，以及车辆分别与库后边和库底边的安全阈值；末位置迭代航向角为车身与库底边的夹角；
[0068]
s3、根据车辆的长度、宽度、轴距、车辆分别与库后边和库底边的安全阈值、以及末位置迭代航向角，确定车辆后轴中点坐标；根据车辆后轴中点坐标、转弯半径以及末位置迭代航向角，确定末位置转弯圆及其圆心o1；根据车辆后轴中点坐标、车辆的长度、宽度、轴距、以及末位置迭代航向角，确定车辆右前角d点坐标；
[0069]
s4、根据圆心o1坐标以及车辆右前角d点坐标，确定车辆右前角转弯半径r2；获取库位的长度和宽度，结合圆心o1坐标，确定圆心o1到库前边顶点o的距离l
o1_o
；
[0070]
s5、判断距离l
o1_o
与车辆右前角转弯半径r2的差值是否大于等于第一安全阈值δ1，若是，则令x_value为正无穷大，执行步骤s7；若否，则执行步骤s6；
[0071]
s6、确定圆心o1到库前边距离为r2 δ1的临界点e，进而确定线段o1e与线段o1d的夹角θ
_error
；设x轴与库底边平行，确定车辆后轴与y轴的夹角θ
p
，进而确定车辆后轴中点在车辆后轴与y轴的夹角为θ
p
θ
_error
时的横向坐标x_value；
[0072]
s7、沿车辆后轴中心线向前迭代泊车起始位置；
[0073]
s8、根据当前的泊车起始位置以及车辆转弯半径，确定起始位置转弯圆及其圆心o2坐标；结合末位置转弯圆，确定两圆内公切线，进而确定两圆内公切线与末位置转弯圆的切点横坐标x
k
，以及两圆内公切线与起始位置转弯圆的切点横坐标x
h
；
[0074]
s9、判断x
k
是否小于等于x_value，若是，则执行步骤s10，若否，则执行步骤s7；
[0075]
s10、过库前边顶点o做垂直于内公切线的直线，当垂足恰为内公切线与起始位置转弯圆的切点时，为临界点q，确定点q的横坐标x
q
；判断x
h
是否大于等于x
q
，若是则执行步骤s11，若否，则执行步骤s12；
[0076]
s11、判断l
oq
≥car_w/2 δ2是否成立，l
oq
为库前边顶点o到临界点q的距离，car_w为车宽，δ2为第二安全阈值；若成立，则结束，若不成立，则执行步骤s13；
[0077]
s12、判断l
o2j
≥l
o2o
δ2是否成立，l
o2j
为圆心o2到车辆边缘的距离，l
o2o
为圆心o2到库前边顶点o的距离；若成立，则结束，若不成立，则执行步骤s13
[0078]
s13、增大末位置迭代航向角，并判断末位置迭代航向角是否在预设范围内，若是，则执行步骤s3。
[0079]
下面以实例对本发明进行说明：
[0080]
首先，车辆利用超声波传感器可以探测知库位的长度(p_h)和宽度(p_w)。
[0081]
一个原则：不管是分几步泊车，第一步就是要保证车辆在末位置不仅要满足库底边的碰撞约束，还要使航向角(heading)尽可能小(有利于后续的泊车步骤)。先设定末位置迭代航向角。如图1所示，dx，dy为相应的安全阈度。首先设置航向角为0，是否可以规划一条无碰撞路径，如果没有，缓慢增大航向角，直到找到一条满足碰撞约束的路径。
[0082]
平行泊车时可能碰撞点：
[0083]
1.车辆a点与外界的碰撞(目前暂不考虑)如图2。
[0084]
2.车辆cd边与库顶点o的碰撞，如图2。
[0085]
3.车辆点d与库右边的碰撞，如图3。
[0086]
4.车辆点c与库底边的碰撞，如图3。
[0087]
5.车辆点b与库左边的碰撞，如图3。
[0088]
路径规划算法分析及碰撞点的规避。为使车辆的行驶路径的曲率及曲率变化率较小，满足车辆运动学及eps性能要求，路径采用圆弧 切直线 圆弧模式。圆弧半径取车辆的最小转弯半径(r_min)。
[0089]
如图4所示，车辆顶点c所在的圆为车辆顶点c走过的圆弧，因车辆的末位姿确定(由满足4和5的碰撞约束、以及航向角、车辆宽度可确定)，由图可得能够满足4和5的碰撞约束。
[0090]
设库位的库底边后顶点为原点，车辆左后角b点坐标为(xb，yb)，车辆右后角c点坐标为(xc，yc)，车辆的长度、宽度、轴距依次为car_h、car_w、car_l，末位置迭代航向角为θ_h，则后轴中点p(x
p
，y
p
)坐标为：
[0091][0092]
当车辆与库后边和库底边的距离均为安全阈值时，xb＝dx，yc＝dy。在车辆与库后边和库底边的距离均大于安全阈值时，也是可以的。
[0093]
由此可得圆心o1的坐标：
[0094][0095]
由后轮轮距中点坐标可求车辆d点坐标(xd，yd)：
[0096][0097]
由此，可得车辆右前角d点所在圆的半径l
o1_d
＝r2和圆心o1到库右顶点o的距离l
o1_o
。若l
o1_o
‑
r2≥δ则满足第3点的碰撞约束(δ为安全阈值，可根据需要设置)，否则不满足。
[0098]
计算圆心o1到库右边距离为r2 δ1的点，如图5所示，假设e点恰好为此临界点。o
点坐标(x
o
,y
o
)已知，求线段o1e到与y轴平行的半径角度θ1：
[0099]
θ1＝arcsin((x
o
‑
x
o1
)/(r2 δ))
[0100]
求线段o1d到与y轴平行的半径角度θ2：
[0101]
θ2＝arcsin((x
d
‑
x
o1
)/r2)
[0102]
得到角∠eo1d，即θ_error＝θ1
‑
θ2。
[0103]
此差值映射到以后轴中心p为轨迹的圆(p点所在的圆)上，也就是说p点的轨迹圆的可行驶范围为圆心角为[θ
p
,θ
p
θ_error]。求得p点在角θ
p
θ_error的x坐标值x_value，作为判断是否满足碰撞约束的阈值：
[0104]
x_value＝x
o1
r1*cos(3*π/2 θ
p
θ_error)。
[0105]
x_value用于判断是否满足第3碰撞约束。
[0106]
接下来，确定起始点，求解切线方程，及判断是否满足第2碰撞约束。
[0107]
车辆与库右边的横向距离dh确定(y坐标确定)，沿后轴中心线向右迭代泊车起始点不同的x坐标，如图6所示。计算第一段圆弧和切线方程，判断是否满足第2碰撞约束。
[0108]
由图7可知车辆泊车起始位置后轴中心p1坐标(x
p1
,y
p1
)，由圆心o1坐标和圆心o2坐标(x
o2
,y
o2
)，得与切线相交中点l坐标(x
l
,y
l
)，可进一步得到线段l
lo1
长度。可得：
[0109]
θ
a
＝arcsin((x
l
‑
x
o1
)/l
lo1
)
[0110]
θ
b
＝arccos(r1/l
lo1
)
[0111]
θ
e
＝θ
a
‑
θ
b
[0112]
可得切点k坐标(x
k
,y
k
)：
[0113]
x
k
＝x
o1
r1*cos(3*π/2 θ
e
)
[0114]
y
k
＝y
o1
r1*sin(3*π/2 θ
e
)
[0115]
圆o1与圆o2半径相等，因此同理可得切点h的坐标(x
h
,y
h
)：
[0116]
x
h
＝x
o2
r2*cos(π/2 θ
e
)
[0117]
y
h
＝y
o2
r2*sin(π/2 θ
e
)
[0118]
最终可得切线方程。之后判断x
k
是否小于等于x_value，若x
k
小于等于x_value，则说明第3碰撞约束满足。否则，第3碰撞约束不满足，为了避免第3碰撞，需要使x
k
减小。为了使x
k
减小，如图7所示，需要使切点k向圆o1的下方移动，即使圆o2向右移动。圆o2距离圆o1越远，切点k的横坐标就越接近圆o1的横坐标，会更容易使x
k
小于等于x_value，进而满足第3碰撞约束。使圆o2距离圆o1越远，即向右迭代泊车起始点不同的x坐标，即xp1。
[0119]
第2碰撞约束分析：
[0120]
由图7分析知，第2碰撞点可能在圆弧阶段或者切线阶段，因此应当判断其临界点，经分析当过o点做垂直于切线的直线，当与切线的交点恰为切线与圆的切点时，为临界点q。易求得交点q的坐标为(x
q
,y
q
)，后轴的延长线与车身一侧的交点为j。
[0121]
当x
h
≥x
q
时，第2碰撞点在切线阶段，碰撞约束的判断条件为l
oq
≥car_w/2 δ。若满足，则路径规划成功。若不满足，则发生碰撞，为了避免该碰撞，需要增大末位置迭代航向角。末位置迭代航向角越大，车辆就更容易从库位驶出，反之就更容易驶入。
[0122]
当x
h
＜x
q
时，第2碰撞点在圆弧阶段，碰撞约束的判断条件为l
o2j
≥l
o2o
δ，若满足，则路径规划成功。若不满足，则发生碰撞，为了避免该碰撞，需要增大末位置迭代航向角。因为是平行泊车，所以末位置迭代航向角越小越好，且不得超过一定阈值。若超过一定阈值，
则泊车后，车头(a点)容易超出泊车范围。超过该阈值，则说明此库位无法完成泊车。
[0123]
规划成功的路径主要为圆o2的一段圆弧 两圆的切线 圆o1的一段圆弧。
[0124]
本发明还提供一种电子设备，包括一个或多个处理器以及存储器；一个或多个程序被存储在存储器中并被配置为由一个或多个处理器执行，一个或多个程序配置用于执行上述的带约束的平行泊车路径规划方法。
[0125]
本发明还提供一种计算机可读存储介质，计算机可读存储介质中存储有程序代码，其中，在程序代码运行时执行上述的带约束的平行泊车路径规划方法。
[0126]
需要指出，根据实施的需要，可将本技术中描述的各个步骤/部件拆分为更多步骤/部件，也可将两个或多个步骤/部件或者步骤/部件的部分操作组合成新的步骤/部件，以实现本发明的目的。
[0127]
本领域的技术人员容易理解，以上仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。