自动泊车路径规划方法、设备及计算机可读存储介质与流程

1.本发明涉及自动泊车技术领域，尤其涉及一种自动泊车路径规划方法、设备及计算机可读存储介质。


背景技术：

2.随着科技的快速发展，汽车的智能化需求也越来越来高。由于市场需求和技术发展，自动泊车技术的应用越来越普遍，但当前自动泊车技术还存在很多问题。avp(automated valet parking，自动泊车)能有效减少由于驾驶员操作失误造成的刮擦，能提高停车场利用效率，解决驾驶员找车位难等问题。但是由于停车场结构复杂，停车位数量多，道路较窄，也就造成avp系统中路径规划比较难的问题，路径规划速度慢等问题，影响车辆自动泊车功能的使用。


技术实现要素：

3.本发明实施例提供一种自动泊车路径规划方法、设备及计算机可读存储介质，其能有效提高路径规划速度、降低路径规划难度。
4.第一方面，本发明实施例提供一种自动泊车路径规划方法，其包括：
5.获得待规划路径所在场所的平面环境信息，并基于所述平面环境信息确定第一规划路线；其中，所述平面环境信息包括车辆的当前位置和目标车位；
6.对所述第一规划路线中的转弯路段依所述车辆的转向能力进行优化，使得优化后的转弯路段满足曲率连续；
7.将所述优化后的第一规划路线中的各路线元素的存储顺序依车辆行进方向进行调整，得到用于所述车辆自动泊车的第二规划路线。
8.作为上述方案的改进，所述对所述第一规划路线中的转弯路段依所述车辆的转向能力进行优化，使得优化后的转弯路段满足曲率连续之前，还包括：
9.对所述第一规划路线进行道路中心约束，使得约束后的第一规划路线位于所在道路的中线。
10.作为上述方案的改进，所述基于所述平面环境信息确定第一规划路线，包括：基于所述平面环境信息，调用cad以所述车辆的当前位置为起点，选定的空闲的目标车位为终点，绘制所述第一路线规划。
11.作为上述方案的改进，所述对所述第一规划路线中的转弯路段依所述车辆的转向能力进行优化，使得优化后的转弯路段满足曲率连续，包括：
12.对所述第一规划路线的转弯路段进行倒圆弧角设置，使得所述第一规划路线中相邻的直线路段通过圆弧连接；其中，相邻的直线路段之间圆弧的半径大于所述车辆的最小转完半径；
13.将所述第一规划路线中的圆弧替换为过渡曲线；其中，所述第一规划路线中相邻的直线路段与过渡曲线之间曲率连续，且所述过渡曲线的曲率半径大于所述车辆的最小转
完半径。
14.作为上述方案的改进，所述过渡曲线为b样条曲线或者b样条曲线与所述圆弧的组合。
15.作为上述方案的改进，所述第一规划路线以cad格式文件存储；
16.则，所述将优化后的第一规划路线中的各路线元素的存储顺序依车辆行进方向进行调整，得到用于所述车辆自动泊车的第二规划路线，包括：
17.将优化后的第一规划路线转换为dxf格式，并按照图形类型存储为多个路线元素；其中，所述路线元素包括直线路段元素、过渡曲线元素以及参考圆弧元素；
18.分别对所述路线元素进行插值处理；
19.对插值后的所述路线元素的各点的存储顺序进行调整，得到所述第二规划路线。
20.作为上述方案的改进，所述对插值后的所述路线元素的各点的存储顺序进行调整，得到所述第二规划路线，包括：
21.以所述第一规划路线的起点作为当前检测点，将所述第一规划路线中的所有路线元素添加到工作队列中；
22.遍历所述工作队列中的路线元素，检查每个所述路线元素的两端点与当前检测点之间的距离，直至找到最小距离对应的端点；
23.判断当前找到的端点是否为对应的路线元素的起点；
24.若否，则反转当前找到的端点对应的路线元素的所有点；
25.若是或者完成反转当前找到的端点对应的路线元素的所有点后，将当前找到的端点对应的路线元素的终点作为下一个检测点，并将当前找到的端点对应的路线元素移动到结果队列，直至所述规划路线中所有路线元素移动到所述结果队列；
26.将所述结果队列中的路线元素按照存储顺序依次连接，得到所述第二规划路线。
27.作为上述方案的改进，所述第一规划路线中的过渡曲线的点在cad格式文件中按照逆时针方向存储。
28.第二方面，本发明实施例提供了一种自动泊车路径规划设备，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时实现如第一方面中任意一项所述的自动泊车路径规划方法。
29.第三方面，本发明实施例提供了一种计算机可读存储介质，其特征在于，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时所述计算机可读存储介质所在设备执行如第一方面中任意一项所述的自动泊车路径规划方法。
30.相对于现有技术，本发明实施例的有益效果在于：基于车辆的当前位置和目标车位，初步确定由当前位置到目标车位的粗略的第一规划路线；然后对所述第一规划路线进行转弯路段曲率约束，优化第一规划路线；最后将优化后的第一规划路线中的各路线元素的存储顺序进行调整，得到用于所述车辆自动泊车的第二规划路线；本发明实施例的路径规划过程中约束简单，能有效提高路径规划速度、降低路径规划难度。
附图说明
31.为了更清楚地说明本发明的技术方案，下面将对实施方式中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施方式，对于本领域普
通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
32.图1是本发明实施例提供的一种自动泊车路径规划方法的流程图；
33.图2是本发明实施例提供曲率优化前的第一规划路线示意图；
34.图3是本发明实施例提供的中心约束后的第一规划路线示意图；
35.图4是本发明实施例提供的曲率优化后的第一规划路线示意图；
36.图5是本发明实施例提供的优化后的第一规划路线的路线元素示意图；
37.图6是本发明实施例提供的路线元素存储顺序调整流程示意图；
38.图7是本发明实施例提供的一种自动泊车路径规划设备的示意图。
具体实施方式
39.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
40.实施例一
41.请参见图1，是本发明实施例提供的一种自动泊车路径规划方法的流程示意图，所述自动泊车路径规划方法具体包括：
42.s11：获得待规划路径所在场所的平面环境信息，并基于所述平面环境信息确定第一规划路线；其中，所述平面环境信息包括车辆的当前位置和目标车位；
43.进一步，所述基于所述平面环境信息确定第一规划路线，包括：
44.基于所述平面环境信息，调用cad以所述车辆的当前位置为起点，选定的空闲的目标车位为终点，绘制所述第一路线规划。
45.示例性的，以停车库场景为例，在cad系统预先导入或绘制停车场的平面环境图，以所述车辆的当前位置为起点，选定的空闲的目标车位为终点，粗略地手动绘制第一规划路线，如图2所示。
46.s12：对所述第一规划路线中的转弯路段依所述车辆的转向能力进行优化，使得优化后的转弯路段满足曲率连续；
47.所述车辆的转向能力根据其最小转完半径确定。
48.进一步，所述对所述第一规划路线中的转弯路段依所述车辆的转向能力进行优化，使得优化后的转弯路段满足曲率连续，包括：
49.对所述第一规划路线的转弯路段进行倒圆弧角设置，使得所述第一规划路线中相邻的直线路段通过圆弧连接；其中，相邻的直线路段之间圆弧的半径大于所述车辆的最小转完半径；
50.将所述第一规划路线中的圆弧替换为过渡曲线；其中，所述第一规划路线中相邻的直线路段与过渡曲线之间曲率连续，且所述过渡曲线的曲率半径大于所述车辆的最小转完半径。
51.进一步，所述过渡曲线为b样条曲线或者b样条曲线与所述圆弧的组合。
52.由于粗略绘制的第一规划路线是直角转角，而车辆无法按照直角转角进行转弯，因此，在本发明实施例中，需要对所述第一规划路线的转弯路段进行曲率约束，以保证第一
规划路线曲率连续，使得车辆可以安全地按照第一规划路线行进；具体的曲率约束包括两个阶段，第一阶段是基于车辆的最小转弯半径，对所述第一规划路线的转弯路段设置倒圆弧角，倒圆角的半径大于所述车辆的最小转弯半径；第二阶段是对所述第一规划路线中的直线路段与其相连的圆弧进行曲率连续调整，例如采用b样条曲线替代所述规划路线中的圆弧，又或者在直线路段与其相连的圆弧之间插入b样条曲线，从而得到与直线路段曲率连续的过渡曲线，保证所述车辆可以沿着优化后的第一规划路线行进，如图4所示。具体的，可通过检查过渡曲线的最大曲率圆和曲率梳确定该过渡曲线是否与相连的直线元素曲率连续。
53.进一步，所述第一规划路线中的过渡曲线的点在cad格式文件中按照逆时针方向存储。
54.s13：将所述优化后的第一规划路线中的各路线元素的存储顺序依车辆行进方向进行调整，得到用于所述车辆自动泊车的第二规划路线。
55.进一步，所述第一规划路线以cad格式文件存储；
56.则，所述将所述优化后的第一规划路线中的各路线元素的存储顺序依车辆行进方向进行调整，得到用于所述车辆自动泊车的第二规划路线，包括：
57.将所述第一规划路线转换为dxf格式，并按照图形类型存储为多个路线元素；其中，所述路线元素包括直线路段元素、过渡曲线元素以及参考圆弧元素；
58.分别对所述路线元素进行插值处理；
59.对插值后的所述路线元素的各点的存储顺序进行调整，得到所述第二规划路线。
60.在本发明实施例中，由于所述第一规划路线中的过渡曲线的点在cad文件中是逆时针存储，此时，需要按照车辆的行进方向，即由所述第一规划路线的起点到终点的方向对所述第一规划路线的各个路线元素进行存储顺序调整。其中，所述路线元素指的是第一规划路线中不同图形类型的路段，如图5所示的直线路段元素、过渡曲线元素，又或者为了增加参考元素增设的圆弧元素等。将cad文件格式的第一规划路线导出为dxf文件格式，由于dxf文件格式是由ascii码组成的纯文本格式，具有易处理的优点，然后通过对各个路线元素的点按照车辆的行进方向存储顺序进行调整，得到所述第二规划路线。
61.本发明实施例在初步规划自动泊车的第一规划路线后，只需进行曲率连续约束，然后调整路线中各个路线元素的存储顺序，保证得到第二规划路线规划与车辆行进方向一致，整个路径规划过程中约束简单，能有效提高路径规划速度、降低路径规划难度。规划的轨迹不符合人类驾驶员驾驶习惯等问题。
62.在一种可选的实施例中，所述对所述第一规划路线中的转弯路段依所述车辆的转向能力进行优化，使得优化后的转弯路段满足曲率连续之前，还包括：
63.对所述第一规划路线进行道路中心约束，使得约束后的第一规划路线位于所在道路的中线。
64.其中，所述平面环境信息还包括：道路边界。基于场所的道路边界，可在cad中预先设定规划的路线到道路边界的距离，使得第一规划路线到两侧道路边界的距离相等，即所述第一规划路线位于道路中线上，如图3所示。其中，还可以设定第一规划路线到两侧道路边界的距离大于设定距离阈值；所述距离阈值根据所述车辆的车宽和道路宽度得到，保证所述车辆按照所述第一规划路线行进时与道路边界保持一定的安全距离，避免与周边障碍
物(例如墙面、其他车位停放的车辆等)发生碰撞。
65.在一种可选的实施例中，所述对插值后的所述路线元素的各点的存储顺序进行调整，得到所述第二规划路线，包括：
66.以所述第一规划路线的起点作为当前检测点，将所述第一规划路线中的所有路线元素添加到工作队列中；
67.遍历所述工作队列中的路线元素，检查每个所述路线元素的两端点与当前检测点之间的距离，直至找到最小距离对应的端点；
68.判断当前找到的端点是否为对应的路线元素的起点；
69.若否，则反转当前找到的端点对应的路线元素的所有点；
70.若是或者完成反转当前找到的端点对应的路线元素的所有点后，将当前找到的端点对应的路线元素的终点作为下一个检测点，并将当前找到的端点对应的路线元素移动到结果队列，直至所述规划路线中所有路线元素移动到所述结果队列；
71.将所述结果队列中的路线元素按照存储顺序依次连接，得到所述第二规划路线。
72.示例性的，导出后的dxf文件格式中按照图形类型存储第一规划路线的不同路段为不同路线元素，例如：
73.过渡曲线元素spline：控制点；
74.参考圆弧元素arc：圆心、半径、起始角度、终止角度(仅用于为后续插值和存储顺序调整环节提供参考)；
75.直线路段元素line：起点终点；
76.以arc为例，给出文件存储示例如下：
77.arc#图形类型
[0078]5[0079]
210
[0080]
330
[0081]
16
[0082]
100
[0083]
acdbentity
[0084]8[0085]
path#图层名
[0086]
62
[0087]
30
[0088]
420
[0089]
16292126
[0090]
370
[0091]-3
[0092]
100
[0093]
acdbcircle
[0094]
10
[0095]
66.76303500452#圆心x坐标
[0096]
20
[0097]
153.997091606571#圆心y坐标
[0098]
30
[0099]
0.0
[0100]
40
[0101]
7.0#半径
[0102]
100
[0103]
acdbarc
[0104]
50
[0105]
125.088088266287#圆弧起始角度(deg)
[0106]
51
[0107]
179.042654569302#圆弧终止角度(deg)
[0108]
0。
[0109]
如图6所示，步骤1：将第一规划路线的起点设为当前检测点，所有路线元素加入工作队列；
[0110]
步骤2：遍历所有工作队列中的路线元素，对于每个所述路线元素的起点和终点，检查与当前检测点之间的距离，当找到最小距离对应的点时停止；
[0111]
步骤3：判断当前找到的点是否为对应的路线元素的起点；
[0112]
步骤4：若否，则反转当前找到的点对应的路线元素的所有点；
[0113]
步骤5：若是或者完成反转当前找到的点对应的路线元素的所有点后，将当前找到的点对应的路线元素的终点作为下一个检测点，并将当前找到的点对应的路线元素移动到结果队列；
[0114]
步骤6：判断是否所有路线元素移动到所述结果队列；若是，则结束，若否，则返回步骤1。
[0115]
由于基于cad系统规划路线，其存储过程可能无序，本发明实施例通过调整路线元素先后顺序，构建最终第二规划路线，保证第二规划路线与车辆行进方向一致。
[0116]
在一种可选的实施例中，所述分别对所述路线元素进行插值处理，包括：
[0117]
对过渡曲线元素进行插值处理，具体过程如下：
[0118]
1、获取过渡曲线元素控制点数量n和曲率k；
[0119]
2、节点向量设置为u＝zeros(1，n+k+1)；其中，zeros(n，m)代表n行m列全零矩阵；
[0120]
3、u(1:k)＝0；
[0121]
4、u(n+2，n+k+1)＝1；
[0122]
5、u(k+1，n+1)＝linspace(0，1，n-k+1)；其中，a＝linspace(b1，b2，n)表示生成n个点，这些点的间距为(b
2-xb1)/(n-1)；
[0123]
6、使用节点向量和控制点进行b样条线构建，可以得到任意t∈[0，1]区间曲线以及其导数的表达式x(t)，y(t)，x
′
(t)，y
′
(t)，x
″
(t)，y
″
(t)；
[0124]
7、对于任意参数t，其航向角φ为atan2(y
′
(t)，x
′
(t))；
[0125]
8、对于任意参数t，其曲率k为：
[0126][0127]
上述任意参数t即为过渡曲线元素的插值。
[0128]
对参考圆弧元素进行插值处理，具体过程如下：
[0129]
1、任意i∈[0，1]区间，θ(i)＝lerp(θs，θe，i)，其中，θs为起始角度，θe为终止角度；
[0130]
2、x(θ)＝x0+r
·
cos(θ)；
[0131]
3、y(θ)＝y0+r
·
sin(θ)；其中，(x0，y0)为起始坐标；
[0132]
4、φ(θ)＝θ+π/2；
[0133]
5、k(θ)＝1/r；其中，r为圆弧半径。
[0134]
上述任意参数i即为参考圆弧元素的插值。
[0135]
对直线路段元素进行插值处理，具体过程如下：
[0136]
任意j∈[0，1]区间，起点为(xs，ys)，终点为(xe，ye)；
[0137]
1、x(j)＝lerp(xs，xe，t)；
[0138]
2、y(j)＝lerp(ys，ye，t)；
[0139]
3、φ(j)＝atan2(y
e-ys，x
e-xs)；
[0140]
4、k(θ)＝0；
[0141]
其中，p＝atan2(y，x)表示返回y和x的四象限反正切(tan-1)，该值必须为实数；lerp(p，b，j)＝p+j(q-p)。
[0142]
上述任意参数j即为直线路段元素的插值。
[0143]
相对于现有技术，本发明实施例的有益效果在于：基于车辆的当前位置和目标车位，初步确定由当前位置到目标车位的粗略的路线规划；然后对第一规划路线进行转弯路段曲率连续约束，优化第一规划路线；最后将优化后的第一规划路线中的各路线元素的存储顺序进行调整，得到用于所述车辆自动泊车的第二规划路线；本发明实施例的路径规划过程中约束简单，能有效提高路径规划速度、降低路径规划难度。
[0144]
实施例二
[0145]
参见图7，是本发明第实施例提供的自动泊车路径规划设备的示意图。如图5所示，该自动泊车路径规划设备包括：至少一个处理器11，例如cpu，至少一个网络接口14或者其他用户接口13，存储器15，至少一个通信总线12，通信总线12用于实现这些组件之间的连接通信。其中，用户接口13可选的可以包括usb接口以及其他标准接口、有线接口。网络接口14可选的可以包括wi-fi接口以及其他无线接口。存储器15可能包含高速ram存储器，也可能还包括非易失存储器(nvm，non-volatile memory)，例如至少一个磁盘存储器。存储器15可选的可以包含至少一个位于远离前述处理器11的存储装置。
[0146]
在一些实施方式中，存储器15存储了如下的元素，可执行模块或者数据结构，或者他们的子集，或者他们的扩展集:
[0147]
操作系统151，包含各种系统程序，用于实现各种基础业务以及处理基于硬件的任务；
[0148]
程序152。
[0149]
具体地，处理器11用于调用存储器15中存储的程序152，执行上述实施例所述的自动泊车路径规划方法，例如图1所示的步骤s11-s14。
[0150]
示例性的，所述计算机程序可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器中，并由所述处理器执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述自动泊车路径规划设备中的执行过程。
[0151]
所述自动泊车路径规划设备可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述自动泊车路径规划设备可包括，但不仅限于，处理器、存储器。本领域技术人员可以理解，所述示意图仅仅是自动泊车路径规划设备的示例，并不构成对自动泊车路径规划设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件。
[0152]
所称处理器11可以是中央处理单元(central processing unit，cpu)，还可以是其他通用处理器、数字信号处理器(digital signal processor，dsp)、专用集成电路(application specific integrated circuit，asic)、现成可编程门阵列(field-programmable gate array，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器11是所述自动泊车路径规划设备的控制中心，利用各种接口和线路连接整个自动泊车路径规划设备的各个部分。
[0153]
所述存储器15可用于存储所述计算机程序和/或模块，所述处理器11通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述自动泊车路径规划设备的各种功能。所述存储器15可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器15可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(smart media card，smc)，安全数字(secure digital，sd)卡，闪存卡(flash card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
[0154]
其中，所述自动泊车路径规划设备集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、u盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(rom，read-only memory)、随机存取存储器(ram，random access memory)、电载波信号、电信信号以及软件分发介质等。
[0155]
需要说明的是，本发明实施例所述的自动泊车路径规划设备与实施例一所述的自动泊车路径规划方法的实现原理和技术效果相同，在此不再详细说明。
[0156]
实施例三
[0157]
本发明实施例提供了一种计算机可读存储介质，其特征在于，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行如实施例一中任意一项所述的自动泊车路径规划方法。
[0158]
需说明的是，以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。另外，本发明提供的装置实施例附图中，模块之间的连接关系表示它们之间具有通信连接，具体可以实现为一条或多条通信总线或信号线。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
[0159]
以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出多台改进和润饰，这些改进和润饰也视为本发明的保护范围。