针对狭窄垂直车位的自动泊车轨迹规划方法及电子设备

1.本发明属于汽车相关人工智能技术中的自动驾驶领域，尤其涉及一种针对狭窄垂直车位的自动泊车轨迹规划方法及电子设备。


背景技术：

2.现如今，汽车行业发展迅速，汽车相关技术快速提高，人们对于出行工具的需求也越来越高；很多汽车产品开始推出智能辅助驾驶和无人驾驶功能，用以帮助驾驶员驾驶车辆，提高汽车的行驶安全性，同时降低长时间驾驶带来的疲劳感。
3.在停车场或园区内，环境相对固定，障碍物的种类也相对单一，因此在停车场内的自动驾驶是目前落地最多的技术环境。同时，当下机动车保有量越来越大，造成车位愈发紧张，很多停车场的面积十分有限。面对通道狭窄的垂直车位停车场环境，现存的自动泊车策略少数会无法完成泊入，多数会采用传统的多步泊入方法，通过在车位前的多次切换档位、前后腾挪的方式完成泊入，这种方法效率低下，有失败的可能性，同时多次的停车与档位切换，对于跟踪控制的精度提出了很高的要求。


技术实现要素：

4.本发明所要解决的技术问题在于针对上述存在的问题，提供一种针对狭窄垂直车位的自动泊车轨迹规划方法及电子设备，避免多次前后腾挪和档位切换，最大程度节约车位前通道空间的泊车轨迹，同时满足狭窄停车场环境下杜宇泊车过程舒适度的要求。
5.本发明为解决上述技术问题，提供如下技术方案：
6.一种针对狭窄垂直车位的自动泊车轨迹规划方法，其特征在于包括如下步骤：
7.s1：根据车辆感知信息，建立坐标系，划定能够自初始位置接管车辆完成自动泊车的初始范围；
8.s2：根据车辆初始范围与车位之间的相对位置，在坐标系中导入车辆周围环境的参数模型，计算能够一次泊入的位置；
9.s3：基于五次多项式完成第一段轨迹规划，使得车辆由初始位置行驶至能够一次泊入的位置；
10.s4：基于几何法和三次多项式耦合的方式完成第二段轨迹规划，使得车辆由所述能够一次泊入的位置一次性泊入到所述车位；
11.s5：基于五次多项式完成第三段轨迹规划，使得车辆最终停在车位正中间的位置；
12.s6：将计算得到的轨迹信息以矩阵的形式输出，为车辆控制提供数据。
13.上述技术方案中，步骤s1根据感知信息数据，依照车位初始位置建立坐标系，在车位前或上部的其中一侧划定泊车初始范围，所述初始范围为：靠近车位其中一侧的车辆纵向边界为边界低端纵坐标，自所述边界纵坐标间隔车架横梁的长度设置为边界高端纵坐标；车位前或上部的所述一侧边界点为初始范围近端横坐标，位于车位上部的车辆后轴车架横梁中心点设置为初始范围远端横坐标。
14.上述技术方案中，s1中初始范围中，车辆后轴车架横梁中心点位于车位上部，距离车位上边缘线1m至4m；由一般车辆车架横梁位置和两边纵梁位置尺寸参数设置车架横梁的长度为8m；在车位前右侧划定初始范围如下：
15.b_4＝b+1
16.b_5＝b+4
17.a_5＝a+8；
18.其中，(a,b)是车位右侧边界点，a为车位右侧边界的横坐标；b为车位右侧边界的纵坐标；
19.(a_4,b_4)是初始位置的左下边界点坐标；
20.(a_5,b_5)是初始位置的右上边界点坐标；
21.在车位前左侧划定初始范围的步骤与上述类似。
22.上述技术方案中，步骤s2基于车辆的运动学模型、右侧边角碰撞约束、和左侧边角碰撞约束，计算车辆在泊入过程中不会与车位发生碰撞的轨迹圆心(center_x，center_y)，
23.其中，右侧边角碰撞约束：
[0024][0025]
式中：
[0026]
rmin：汽车的最小转弯半径；
[0027]
w：汽车的宽度；
[0028]
safe_dis：汽车的与车位边界需要保持的最小安全距离；
[0029]
a为车位的边界点右侧边角的横坐标，b为车位的边界点右侧边角的纵坐标；
[0030]
左侧边角碰撞约束：
[0031]
center_x-r min≥a_2+w/2+safe_dis
[0032]
其中；a-2为左侧边角的横坐标，b-2为右侧边角的纵坐标；
[0033]
求得：center_x＝a_2+safe_dis+w/2+r min
[0034][0035]
上述技术方案中，步骤s3由轨迹圆心得到能够一次泊入的位置或初始位置：依据圆心轨迹纵坐标直接得到能够一次泊入的位置纵坐标：
[0036]
y_2＝center_y+rmin
[0037]
横坐标x_2与x_1的距离设置为同纵坐标y_2与y_1的距离正相关，为：
[0038]
x_2-x_1＝5*(y_2-y_1)。
[0039]
上述技术方案中，步骤s3所述第一段轨迹f为由初始位置行驶至能够一次泊入的位置这段轨迹，利用五次多项进行第一段轨迹规划，保证这段轨迹的跃度最小，即取最小值；
[0040]
其中，车辆从初始位置到能够一次泊入的位置所花费的时间t间隔如下：
[0041]
t0＝0；
[0042]
t1＝(a_2-a_1+1)/v_target；
[0043]
目标速度设定为v_target＝0.8m/s。
[0044]
上述技术方案中，步骤s4中使用几何法和三次多项式结合的方法进行第二段轨迹规划，利用直线与曲线的连接几何法规划路径，利用三次多项式规划速度，最终将二者合成规划轨迹：
[0045]
车辆的路径s(t)＝a0+a1(t-t2)+a2(t-t2)2+a3(t-t2)3,t2≤t≤t3；
[0046][0047]
式中t2是第二段轨迹的初始时间点，t3是结束时间点，a0、a1、a2、a3为多项式代数， s0代表初始位置，v0代表初始速度，h代表第二段轨迹的位移，t代表第二段轨迹所消耗的时间。
[0048]
上述技术方案中，步骤s5中第三段轨迹的规划使用同s4同样的方法进行轨迹规划，终点位置限制为车位的中心位置，保证车辆的泊车规范性。
[0049]
上述技术方案中，s4中第二段规划中的结束点的速度和s5中第三段规划的初始点速度相同，且两个点处的转向角度设置相同。这样可以保证两段轨迹的平稳衔接，能够实现平滑的过度。
[0050]
优选，s4中第二段规划中的结束点的速度和设置为0.6m/s，同时第三段规划的初始点速度同样为0.6m/s，这样可以保证两段轨迹的平稳衔接，同时二者的转向角度同样为0，完全能够实现平滑的过度。
[0051]
一种电子设备，包括：一个或多个处理器；存储器，一个或多个程序，其特征在于所述一个或多个程序被存储在所述存储器中，并且被配置成由所述一个或多个处理器直线，所述程序包括用于执行以下步骤的指令：
[0052]
s1：用于根据车辆感知信息，划定能够接管车辆完成自动泊车的初始范围的装置；
[0053]
s2：根据车辆与车位之间的相对位置，建立坐标系，导入车辆周围环境的参数模型；
[0054]
s3：基于五次多项式完成第一段轨迹规划，使得车辆行驶至能够一次泊入的位置；
[0055]
s4：基于几何法和三次多项式耦合的方式完成第二段轨迹规划，使得车辆由能够一次泊入的位置一次泊入车位；
[0056]
s5：基于五次多项式完成第三段轨迹规划，使得车辆最终停在车位正中间的位置；
[0057]
s6：将计算得到的轨迹信息以矩阵的形式输出，为车辆的控制提供数据。
[0058]
由此，本发明提供一种针对狭窄垂直车位的自动泊车轨迹规划方法及设备，使用上文中提到的策略进行重新轨迹规划之后，车辆在泊入过程中，经过测试，其对于纵向空间的节约率达到了15％，降低了泊车过程对于纵向空间的需求程度，能够更好的适应现如今越来越多的狭窄车位区域。
[0059]
步骤s2中先确定了直接泊入时的圆心位置，从而在s3中确定出第一步调整轨迹，使得车辆可以实现在泊入过程中无需换档，提高了舒适性；降低了控制的压力。在s5中增加
了车辆已经初步停入车位后基于五次多项式得横向移动过程，使得车辆既能够停在车位中央位置，又能尽量降低对车位前部通道纵向空间的需求。
[0060]
本发明的有益效果是：
[0061]
在本发明的规划策略下，车辆只在调整至一次泊入位置时切换一次档位，相比传统的泊入方法，减少了腾挪的次数，同时曲率的衔接更加平滑，没有了原地转向的过程。
[0062]
利用几何法结合车辆的与车位相对位置、车辆的运动学模型，确定车辆运动的起止点，再利用五次多项式完成车辆运动过程的轨迹规划，以获得能够最大程度节约车位前通道空间的泊车轨迹，同时满足杜宇泊车过程舒适度的要求。
[0063]
能够更少的使用车位前部通道的纵向空间，程序通过增加对最后一段路径的横向移动，使得车辆对于纵向空间的需求降低。同时在接管泊车之初先将车辆调整至能够一次完成泊入的位置，避免了车辆在车位附近的反复切换档位腾挪，提高了舒适度，降低了驾驶员心理压力。
附图说明
[0064]
图1为本发明中规定的车辆后轴中心点的初始范围
[0065]
图2为本发明针对狭窄垂直车位的自动泊车轨迹规划方法一个实施例的右侧碰撞约束图。
[0066]
图3为本发明一个实施例的左侧碰撞约束图。
[0067]
图4为本发明一个实施例的第一步轨迹目标位置。
[0068]
图5为本发明一个实施例的第一步轨迹展示。
[0069]
图6(a)为本发明一个实施例的第一步坐标x随时间的变化。
[0070]
图6(b)为本发明一个实施例的第一步速度规划输出曲线。
[0071]
图7为本发明一个实施例的第二段路径规划。
[0072]
图8为本发明一个实施例的第二段速度规划。
[0073]
图9为本发明一个实施例的第三段轨迹目标点。
[0074]
图10(a)为本发明一个实施例的第三段轨迹展示。
[0075]
图10(b)为本发明一个实施例的第三段速度规划。
[0076]
图11(a)为本发明实施例环境下采用传统规划结果。
[0077]
图11(b)为本发明一个实施例与传统规划结果的对比。
具体实施方式
[0078]
为更好地理解本发明，下面结合附图和实施例对本发明进一步的描述。
[0079]
本发明针对狭窄垂直车位的自动泊车轨迹规划方法以一个车位前通道宽度为4.5m的垂直车位停车场为实施例环境，如图1-图10(b)，本发明的技术路线及要点如下：
[0080]
步骤一：
[0081]
首先，当车辆识别的到车位后，为了提高驾驶员泊车过程中的舒适感，降低车辆在泊车过程中的腾挪的次数，在第一步准备阶段则先将车辆自初始位置调至能够一次泊入的一次泊入位置，避免在车位附近进行多步泊车。
[0082]
用如下方法计算一次泊入位置，首先，根据车辆运动学模型可以得知，汽车在低速
泊车过程中，可以视作是车辆后轴中心点的活动，当车辆保持最小转弯半径行驶时，后两侧车轮位置将会是车辆最小运动半径位置。
[0083]
因此，计算车辆在泊入过程中不会与车位发生碰撞的轨迹圆心(center_x，center_y)，即可得到车辆泊入的一次泊入位置或初始位置。
[0084][0085]
式中，
[0086]
rmin：汽车的最小转弯半径；
[0087]
w：汽车的宽度；
[0088]
safe_dis：汽车的与车位边界需要保持的最小安全距离，在计算中取0.2m；
[0089]
泊入位置的计算除了要满足最小半径不发生碰撞这个约束，同时还要满足车辆最大运动半径不与车位边界发生碰撞，如图1和2，由汽车运动学模型可知，在泊入过程中，汽车运动半径位于车辆的尾部边角，即车辆尾部的边角位置是最先发生与车位边界碰撞的。
[0090]
为了避免车辆后方角部与车位发生碰撞，车辆最大运动半径应和车位边界保持一定距离，因此，即可求得运动圆心的横坐标：
[0091]
center_x-rmin≥a_2+w/2+safe_dis
ꢀꢀꢀ
(2)
[0092]
结合图像可知，当要求center_y较小时，则center_x也应该较小，因此，为了实现节约纵向空间的目的，我们直接将车辆初始进入车位的位置尽可能靠近车位边缘，即确定：
[0093]
center_x＝a_2+safe_dis+w/2+rmin
[0094]
通过center_x的值反推，代入公式一，即可算得center_y的目标值：
[0095][0096]
步骤二：
[0097]
得到泊入时圆弧的中心点后，即可开始进行泊车，在初始状态，驾驶员停止车辆并打开自动泊车功能时，汽车首先自动调整至可以进行一步泊入的一次泊入位置。如图3和4，先调整至一次泊入位置的目的在于降低车辆在泊入过程中的换挡与前后腾挪的次数，提高泊入过程中的舒适性，并降低跟踪控制过程的压力。
[0098]
初始的x_1和y_1是由驾驶员初次停车时，根据车辆和车位之间的相对位置得出的。由于是一步泊入状态，因此y_2的数值即为泊入时圆弧圆心的纵坐标：
[0099]
y_2＝center_y+rmin
[0100]
x_2的数值和驾驶员停车位置相关，如果x_2与x_1距离过近，将会造成五次多项式路径规划出的转向角度过大，造成车辆的最大转向角无法满足规划的需要，从而造成跟踪控制的失效，因此，将x_2与x_1的距离设置为同y_2到y_1的距离正相关。
[0101]
x_2-x_1＝5*(y_2-y_1)
[0102]
得到调整阶段的始末位置坐标后，便可以开始第一步的轨迹规划，对于已知两个点参数的情况下，做它们之间的轨迹优化，我们要考虑的是优化斜率的变化率，使其值最小，也就是求解jerk最小的过程，在车辆的运动规划中，舒适度是一个非常重要的指标，在物理中衡量舒适性的物理量为跃度。
[0103]
为了保证舒适度，需要令加速度变化平缓，对于一段轨迹s＝f(t)，当最小时，则能够满足我们的需求，该段轨迹当中有如下边界条件：
[0104]
ls(0)＝s0ꢀꢀꢀ
s(t)＝sn[0105]s′
(0)＝v0ꢀꢀꢀs′
(t)＝vn[0106]s″
(0)＝a0ꢀꢀꢀs″
(t)＝an[0107]
同时满足六个边界条件，需要引入带有边界条件约束的泛函问题，要解决这个带有6个约束的泛函极值问题，需要引入五次多项式。构造矩阵：
[0108][0109]
bx＝(x0vx0ax0x1vx1ax1)
[0110]
将二者相乘：
[0111]
a＝ax*bx
[0112]
即可得到坐标x随时间t变化的曲线：
[0113]
xt＝a(1)+a(2)*dt+a(3)*dt2+a(4)*dt3+a(5)*dt4+a(6)*dt5[0114]
接下来需要求解坐标y随x的变化关系：
[0115][0116]
by＝(y0vy0ay0y1vy1ay1)
[0117]
将二者相乘：
[0118]
b＝ay*by
[0119]
即可得到坐标y随x的变化曲线，这就是车辆的路径。
[0120]
yt＝b(1)+b(2)*dt+b(3)*dt2+b(4)*dt3+b(5)*dt4+b(6)*dt5[0121]
在这个等式中，x矩阵和y矩阵的数值我们都是已知的。x0,vx0,ax0分别表示初始位置和横向坐标、速度、加速度；y0,vy0,ay0分别表示初始位置的纵向坐标，速度，加速度。其次，时间t0和t1也是已知的，分别表示初始位置和终点位置的时刻，我们利用目标速度求解两个时间点。
[0122]
v_target＝0.8m/s
[0123]
t0＝0 t1＝(a_2-a_1+1)/v_target
[0124]
这样一来x，y，t矩阵都已知，我们便容易求出两个矩阵。
[0125]
之后，我们再设置时间间隔
△
t代入t矩阵，就能利用a矩阵和t矩阵求出初始位置和终点位置之间的点的位置、速度和加速度。
[0126]
将已经得到的初始位置(x_1,y_1)，结束位置(x_2,y_2)数值代入上述公式即可得到轨迹点。图6(a)为本发明第一步坐标x随时间的变化。
[0127]
对x、y求导即可得到速度随时间变化的曲线
[0128]
dx_dt＝a(2)+2*a(3)*dt+3*a(4)*dt2+4*a(5)*dt3+5*a(6)*dt4[0129]
dy_dt＝b(2)+2*b(3)*dt+3*b(4)*dt2+4*b(5)*dt3+5*b(6)*dt4[0130][0131]
即得到速度，与上面求得的路径耦合，得到轨迹规划，如图6(b)。
[0132]
步骤三：
[0133]
完成第一段轨迹后，进行第二步规划，如图6和7，第二步由一段倒车过程和一段圆弧行驶组成，完成第二步后，车辆将会行驶至车位内竖直位置。车辆的直线后退过程保持方向盘转角为0，弧线后退过程按照最小转弯半径(最大转角)进行处理。
[0134]
由图可知，第二段路径的途径点为：
[0135]
x_3＝center_x
ꢀꢀꢀꢀꢀꢀꢀ
y_3＝center_y+rmin
[0136]
x_4＝center_x-rmin
ꢀꢀꢀꢀꢀꢀꢀ
y_4＝center_y
[0137]
该段采用速度和路径解耦的方式，利用三次多项式进行速度规划，输出速度v相对于位移s的变化数据，最终将速度与路径耦合形成规划轨迹。因为第二段轨迹和第三段轨迹并没有换挡，两端路径的结合处曲率也没有突变，因此不需要在(x_4,y_4)处停车，在速度规划的过程中，点(x_4,y_4)的速度设置为1m/s。
[0138]
该过程所需的时间：
[0139]
t3＝(a_2-a_3)/v_target t4＝(2*π*rmin/4)/v_target
[0140]
该段轨迹的速度规划使用三次多项式，在制定了初始位置的终点位置的时间后，已知各个点的坐标，将坐标转换为位移，即可得到位移相对于时间的关系，进而得到了该段轨迹的速度规划：
[0141]
s(t)＝a0+a1(t-t2)+a2(t-t2)2+a3(t-t2)3,t2≤t≤t3[0142][0143]
从(x_2,y_2)到(x_4,y_4)的过程由两段组成，因为在(x_3,y_3)处车辆并没有停止因此将此处速度规划为1m/s，将参数代入公式，得到第二段的规划速度。
[0144]
步骤四：
[0145]
由于在规划之初为了达到节约纵向空间的目的，车辆在此状态下并没有位于车位的中间，因此，需要在最后一次增加一段轨迹，使得车辆尽量的位于车位的中央，方便自车驾驶员和旁车驾驶员上下车。参照图8-图10(b)。
[0146]
已知车辆的初始位置，即(x_4,y_4)，最后停车位置如图8所示：
[0147]
x_5＝(a_2+a)/2
[0148]
y_5＝b_3+safe_dis+lr
[0149]
在规划过程中，要注意车头边缘位置与车位边界碰撞的问题，应在保证车辆不会发生碰撞的前提下满足节约纵向空间的目标；最后一段轨迹的规划和第一段轨迹相同，使用五次多项式轨迹规划方法。同上述第二段轨迹规划进行衔接，因此在初始位置(x_4,y_4)处的速度设置为1m/s。
[0150]
结合车辆泊入过程轨迹规划的输出结果可以看出：
[0151]
车辆的轨迹的衔接十分平滑，曲线的斜率没有突变，能够满足平滑舒适的需求，并且为精准的轨迹跟踪控制提供了基础。
[0152]
使用传统的规划策略后，对于采用的示例中所示的停车场环境，如图11(a)和11(b) 所示，在泊入过程中，车辆的左前侧角部的坐标位置为12.93，已经与边界发生碰撞，不能够满足目标停车场的环境。且在整个过程中，共进行了两次档位的切换，在中间的调整过程中，有两个需要原地专项的阶段，对于车辆的转向机有不良影响。使用本专利公开的优化策略进行重新规划后，优化后的轨迹中，车辆最高点降低为12.15，纵向坐标降低的幅度达到了15％，没有发生边界碰撞，同时档位切换的次数降低为1次，路径曲率变化的更加平滑，同时没有了原地转向的过程，提高了泊车的效率和乘员舒适度。很好的解决了原本的规划策略中存在的问题，更能适应当下更多的狭窄停车场环境。
[0153]
应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。