CN110440823B - 一种路径规划方法及路径规划系统 - Google Patents
一种路径规划方法及路径规划系统 Download PDFInfo
- Publication number
- CN110440823B CN110440823B CN201910797675.6A CN201910797675A CN110440823B CN 110440823 B CN110440823 B CN 110440823B CN 201910797675 A CN201910797675 A CN 201910797675A CN 110440823 B CN110440823 B CN 110440823B
- Authority
- CN
- China
- Prior art keywords
- pose
- target
- path
- current
- starting point
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 114
- 238000012545 processing Methods 0.000 claims description 13
- 238000004590 computer program Methods 0.000 claims description 5
- 238000010586 diagram Methods 0.000 description 22
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000004898 kneading Methods 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 239000013598 vector Substances 0.000 description 2
- 238000013500 data storage Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62D—MOTOR VEHICLES; TRAILERS
- B62D15/00—Steering not otherwise provided for
- B62D15/02—Steering position indicators ; Steering position determination; Steering aids
- B62D15/027—Parking aids, e.g. instruction means
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62D—MOTOR VEHICLES; TRAILERS
- B62D15/00—Steering not otherwise provided for
- B62D15/02—Steering position indicators ; Steering position determination; Steering aids
- B62D15/027—Parking aids, e.g. instruction means
- B62D15/028—Guided parking by providing commands to the driver, e.g. acoustically or optically
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
- G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
Abstract
本发明实施例公开了一种路径规划方法及路径规划系统，用于通过车辆当前位姿与目标位姿的距离的判断，选择对应的重新规划的连接方式，进行泊车的路径重新规划，进行误差消除，实现车辆平行泊车的准确性。本发明实施例方法包括：获取起点位姿和目标位姿；确定起点位姿和目标位姿的距离；若距离大于第一预置阈值，则将目标位姿朝着起点位姿的方向以第一转弯半径进行移动，得到第一当前位姿，第一转弯半径大于等于车辆的最小转弯半径；使用目标连接方法连接第一当前位姿和起点位姿，若成功则得到第一规划路径，第一规划路径包括起点位姿与第一当前位姿之间的路径，以及第一当前位姿与目标位姿之间的路径。
Description
技术领域
本发明涉及智能汽车技术领域，尤其涉及一种路径规划方法及路径规划系统。
背景技术
作为一种方便、快捷的交通工具，汽车已成为人们生活和工作的重要组成部分。随着汽车数量的逐年增加，有限的城市空间显得日趋拥挤，车辆平均分配到的停放空间也日趋缩小，车辆泊车入位困难问题在人们生活中逐渐显现。人们对车辆使用轻便性及安全性要求促使越来越多汽车生产商、科研机构及高校对泊车系统进行研究。
发明内容
本发明实施例提供了一种路径规划方法及路径规划系统，用于通过车辆当前位姿与目标位姿的距离的判断，选择对应的重新规划的连接方式，进行泊车的路径重新规划，进行误差消除，实现车辆平行泊车的准确性。
有鉴于此，本发明第一方面提供了一种路径规划方法及路径规划系统，可以包括：
获取起点位姿和目标位姿；
确定所述起点位姿和所述目标位姿的距离；
若所述距离大于第一预置阈值，则将所述目标位姿朝着所述起点位姿的方向以第一转弯半径进行移动，得到第一当前位姿，所述第一转弯半径大于等于所述车辆的最小转弯半径；
使用所述目标连接方法连接所述第一当前位姿和所述起点位姿，若成功则得到第一规划路径，其中，所述目标连接方法为直线连接方法和圆弧连接方法中的至少一种，所述第一规划路径包括所述起点位姿与所述第一当前位姿之间的路径，以及所述第一当前位姿与所述目标位姿之间的路径。
可选的，在本发明的一些实施例中，所述方法还包括：
若所述距离小于等于所述第一预置阈值，使用所述目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第二规划路径。
可选的，在本发明的一些实施例中，所述根据所述起点位姿和所述目标位姿，使用所述目标连接方法，若失败之后，所述方法还包括：
将所述起点位姿朝着所述目标位姿的方向以直线移动预置长度，得到第二当前位姿；
使用所述目标连接方法连接所述第二当前位姿和所述目标位姿，若成功，则确定第三规划路径，所述第三规划路径包括所述起点位姿与所述第二当前位姿之间的路径，以及所述第二当前位姿与所述目标位姿之间的路径。
可选的，在本发明的一些实施例中，所述根据所述起点位姿和所述目标位姿，使用所述目标连接方法，若失败之后，或者，使用所述目标连接方法连接所述第二当前位姿和所述目标位姿，若失败之后，所述方法还包括：
将所述起点位姿朝着所述目标位姿的方向以第二转弯半径进行移动，得到第三当前位姿，所述第二转弯半径大于等于所述车辆的最小转弯半径，所述第三当前位姿与所述目标位姿平行；
计算所述第三当前位姿与所述目标位姿的跟踪误差；
若所述跟踪误差小于第二预置阈值，则直线连接所述第三当前位姿和所述目标位姿，确定第四规划路径，所述第四规划路径包括所述起点位姿与所述第三当前位姿之间的路径，以及所述第三当前位姿与所述目标位姿之间的路径。
本发明第二方面提供一种路径规划系统，可以包括：
获取模块，用于获取起点位姿和目标位姿；
处理模块，用于确定所述起点位姿和所述目标位姿的距离；若所述距离大于第一预置阈值，则将所述目标位姿朝着所述起点位姿的方向以第一转弯半径进行移动，得到第一当前位姿，所述第一转弯半径大于等于所述车辆的最小转弯半径；使用所述目标连接方法连接所述第一当前位姿和所述起点位姿，若成功则得到第一规划路径，其中，所述目标连接方法为直线连接方法和圆弧连接方法中的至少一种，所述第一规划路径包括所述起点位姿与所述第一当前位姿之间的路径，以及所述第一当前位姿与所述目标位姿之间的路径。
可选的，在本发明的一些实施例中，
所述处理模块，还用于若所述距离小于等于所述第一预置阈值，使用所述目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第二规划路径。
可选的，在本发明的一些实施例中，
所述处理模块，还用于将所述起点位姿朝着所述目标位姿的方向以直线移动预置长度，得到第二当前位姿；使用所述目标连接方法连接所述第二当前位姿和所述目标位姿，若成功，则确定第三规划路径，所述第三规划路径包括所述起点位姿与所述第二当前位姿之间的路径，以及所述第二当前位姿与所述目标位姿之间的路径。
可选的，在本发明的一些实施例中，
所述处理模块，还用于将所述起点位姿朝着所述目标位姿的方向以第二转弯半径进行移动，得到第三当前位姿，所述第二转弯半径大于等于所述车辆的最小转弯半径，所述第三当前位姿与所述目标位姿平行；计算所述第三当前位姿与所述目标位姿的跟踪误差；若所述跟踪误差小于第二预置阈值，则直线连接所述第三当前位姿和所述目标位姿，确定第四规划路径，所述第四规划路径包括所述起点位姿与所述第三当前位姿之间的路径，以及所述第三当前位姿与所述目标位姿之间的路径。
本发明第三方面提供一种车辆，可以包括如本发明第二方面及第二方面任一可选方式中所述的路径规划系统。
本发明第四方面提供一种计算机可读存储介质，其存储计算机程序，其中，所述计算机程序使得计算机执行本发明实施例第一方面公开的一种路径规划方法。
从以上技术方案可以看出，本发明实施例具有以下优点：
在本发明实施例中，获取起点位姿和目标位姿；确定所述起点位姿和所述目标位姿的距离；若所述距离大于第一预置阈值，则将所述目标位姿朝着所述起点位姿的方向以第一转弯半径进行移动，得到第一当前位姿，所述第一转弯半径大于等于所述车辆的最小转弯半径；使用所述目标连接方法连接所述第一当前位姿和所述起点位姿，若成功则得到第一规划路径，其中，所述目标连接方法为直线连接方法和圆弧连接方法中的至少一种，所述第一规划路径包括所述起点位姿与所述第一当前位姿之间的路径，以及所述第一当前位姿与所述目标位姿之间的路径。通过车辆当前位姿与目标位姿的距离的判断，选择对应的重新规划的连接方式，进行泊车的路径重新规划，进行误差消除，实现车辆平行泊车的准确性。
附图说明
为了更清楚地说明本发明实施例技术方案，下面将对实施例和现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，还可以根据这些附图获得其它的附图。
图1A所示，为本发明实施例中起点位姿和目标位姿在y轴上的投影示意图；
图1B为本发明实施例中使用目标连接方法连接起点位姿和目标位姿，得到第一规划路径的第一个示意图；
图1C为本发明实施例中使用目标连接方法连接起点位姿和目标位姿，得到第一规划路径的第二个示意图；
图1D为本发明实施例中使用目标连接方法连接起点位姿和目标位姿，得到第一规划路径的第三个示意图；
图1E为本发明实施例中使用目标连接方法连接起点位姿和目标位姿，得到第一规划路径的第四个示意图；
图2为本发明实施例中路径规划方法的一个实施例示意图；
图3A为本发明实施例中得到第二规划路径的一个示意图；
图3B为本发明实施例中得到第三规划路径的一个示意图；
图3C为本发明实施例中得到第四规划路径的一个示意图；
图4为本发明实施例中路径规划系统的一个实施例示意图；
图5为本发明实施例中路径规划系统的另一个实施例示意图。
具体实施方式
本发明实施例提供了一种路径规划方法及路径规划系统，用于通过车辆当前位姿与目标位姿的距离的判断，选择对应的重新规划的连接方式，进行泊车的路径重新规划，进行误差消除，实现车辆平行泊车的准确性。
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，都应当属于本发明保护的范围。
本发明实施例可以应用在车辆正常行驶，或者倒车行驶，或者平行泊车等场景中，具体不做限定。
下面先对本发明实施例中所涉及的直线圆弧连接的方法做一个简要的说明，其中，所述目标连接方法为直线连接方法和圆弧连接方法中的至少一种。如下所示：
假设起点位姿用点A(xA,yA,θA)表示，目标位姿用点B(xB,yB,θB)表示，简单直线圆弧连接方法是指找到从点A到点B的由直线、圆弧组成的完整路径，该路径在起点处与点A相切，在终点处与点B相切。
如图1A所示，为本发明实施例中起点位姿和目标位姿在y轴上的投影示意图。在图1A所示中，以车辆的当前位姿朝向为x轴，右手定则得到y轴，分别过点A、点B作彼此的x轴垂线得到y方向上的投影分别为ry，fy(带符号)。
其中，
(1)如图1B所示，为本发明实施例中使用目标连接方法连接所述起点位姿和所述目标位姿，得到第一规划路径的第一个示意图。在图1B所示中，如果点A、点B共线，则连接点A和点B，第一规划路径为AB构成的直线线段。
(2)如图1C所示，为本发明实施例中使用目标连接方法连接所述起点位姿和所述目标位姿，得到第一规划路径的第二个示意图。
如果点A、点B不共线，首先确定是否可以找到一个圆弧把点A、点B连接起来。如图1C所示，具体操作为分别过点A、点B作其垂线相交于O点，若OA≠OB，则无解转到(3)；如果OA＝OB且圆弧半径R≥Rmin(车辆最小转弯半径)，则找到了单圆弧路径AB，即第一规划路径为单圆弧路径AB；否则不存在这样的圆弧，转到(3)。
(3)如图1D所示，为本发明实施例中使用目标连接方法连接所述起点位姿和所述目标位姿，得到第一规划路径的第三个示意图。
假设一圆弧与点A相切，圆心为点O，如图1D所示，过点A作与点B在x轴平行的辅助线，得到P点，假设该圆弧半径为R，则OP＝R-|fy|，圆心角为dθ＝|θA-θB|，在ΔOPA中由余弦定理可得cos(dθ)＝OP/OA＝(R-|fy|)/R，可以解得圆弧半径为：
R＝|fy|/(1-cos(dθ))
如果R≥Rmin(车辆最小转弯半径)，则找到了从A到B的圆弧-直线路径。即第一规划路径包括圆弧路径AM和直线路径MB。
(4)如图1E所示，为本发明实施例中使用目标连接方法连接所述起点位姿和所述目标位姿，得到第一规划路径的第四个示意图。
假设圆弧与B相切，圆心为点O，如图1E所示，过B作与A x轴平行的辅助线，得到P点，设圆弧半径为R，则OP＝R-|ry|，圆心角为dθ＝|θA-θB|，在ΔOPB中由余弦定理可得cos(dθ)＝OP/OB＝(R-|ry|)/R，可以解得圆弧半径为：
R＝|ry|/(1-cos(dθ))。
如果R≥Rmin，则找到了从A到B的直线-圆弧路径。即第一规划路径包括直线路径AM和圆弧路径MB。
下面以实施例的方式，对本发明技术方案做进一步的说明，如图2所示，为本发明实施例中路径规划方法的一个实施例示意图，可以包括：
201、获取起点位姿和目标位姿。
在本发明实施例中，路径规划系统获取起点位姿和目标位姿的方式，可以是由用户输入起点位姿和目标位姿，或者，由路径规划系统检测得到起点位姿和目标位姿。其中，可以理解的是，起点位姿即为车辆的当前位姿。
车辆上可以装设有惯性测量单元(Inedrtial measurement unit，IMU)、轮脉冲计数器等传感器，这些传感器可以作为车辆的定位模块(如车身里程计)，来获取车辆的当前位姿。
202、确定所述起点位姿和所述目标位姿的距离。
路径规划系统确定所述起点位姿和所述目标位姿的距离。位姿一般可以包括位置、航向角、车速等信息，起点位姿包括起点位置，目标位姿包括目标位置，可以根据起点位置和目标位置，计算起点位姿和目标位姿的距离。
203、若所述距离大于第一预置阈值，则将所述目标位姿朝着所述起点位姿的方向以第一转弯半径进行移动，得到第一当前位姿，所述第一转弯半径大于等于所述车辆的最小转弯半径。
若所述距离大于第一预置阈值，则路径规划系统将所述目标位姿朝着所述起点位姿的方向以第一转弯半径进行移动，得到第一当前位姿，所述第一转弯半径大于等于所述车辆的最小转弯半径。
其中，车辆的最小转弯半径是车辆转向机构决定的，每辆车都有唯一特性，即方向盘打死转弯时的半径。
204、使用目标连接方法连接所述第一当前位姿和所述起点位姿，若成功则得到第一规划路径，其中，所述目标连接方法为直线连接方法和圆弧连接方法中的至少一种，所述第一规划路径包括所述起点位姿与所述第一当前位姿之间的路径，以及所述第一当前位姿与所述目标位姿之间的路径。
示例性的，假设需要重新规划的起点位姿为stpos，目标位姿为finpos。如果当前位姿与目标位姿较远，从目标位姿finpos处开始进行出库操作(前后揉库)，每次揉库得到一个中间位姿midpos1，尝试从midpos1到stpos进行直线圆弧连接，成功则对完整轨迹进行逆序并结束，得到第一规划路径。其中，第一规划路径包括所述起点位姿与所述第一当前位姿之间的路径，以及所述第一当前位姿与所述目标位姿之间的路径。
需要说明的是，使用目标连接方法连接第一当前位姿位姿和起点位姿，得到第一当前位姿与目标位姿之间的路径，可以参考上述图1A至图1B的说明，此处不再赘述。
205、若所述距离小于等于所述第一预置阈值，使用所述目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第二规划路径。
可以理解的是，如果起点位姿和目标位姿的距离小于等于该第一预置阈值，即起点位姿与目标位姿较近，尝试从stpos到finpos进行直线圆弧连接，若成功，则得到第二规划路径，该第二规划路径包括起点位姿与目标位姿之间的路径。如图3A所示，为本发明实施例中得到第二规划路径的一个示意图。其中，所述目标连接方法为直线连接方法和圆弧连接方法中的至少一种。
需要说明的是，使用所述目标连接方法连接所述起点位姿和所述目标位姿，若失败之后，可以执行步骤206和207，或者，执行步骤208-步骤210。
206、将所述起点位姿朝着所述目标位姿的方向以直线移动预置长度，得到第二当前位姿。
路径规划系统将所述起点位姿朝着所述目标位姿的方向以直线移动预置长度，得到第二当前位姿。即将起点位姿stpos朝着目标位姿的方向直线移动特定长度，得到第二当前位姿midpos2。
207、使用所述目标连接方法连接所述第二当前位姿和所述目标位姿，若成功，则确定第三规划路径，所述第三规划路径包括所述起点位姿与所述第二当前位姿之间的路径，以及所述第二当前位姿与所述目标位姿之间的路径。
路径规划系统使用所述目标连接方法连接所述第二当前位姿和所述目标位姿，若成功，则确定第三规划路径。示例性的，尝试从第二当前位姿midpos2到stpos进行简单直线圆弧连接，成功则结束，得到第三规划路径。如图3B所示，为本发明实施例中得到第三规划路径的一个示意图。
需要说明的是，使用所述目标连接方法连接所述第二当前位姿和所述目标位姿，若失败之后，执行步骤208-步骤210。
208、将所述起点位姿朝着所述目标位姿的方向以第二转弯半径进行移动，得到第三当前位姿，所述第二转弯半径大于等于所述车辆的最小转弯半径，所述第三当前位姿与所述目标位姿平行。
路径规划系统将所述起点位姿朝着所述目标位姿的方向以第二转弯半径进行移动，得到第三当前位姿，所述第二转弯半径大于等于所述车辆的最小转弯半径，所述第三当前位姿与所述目标位姿平行。
其中，车辆的最小转弯半径是车辆转向机构决定的，每辆车都有唯一特性，即方向盘打死转弯时的半径。
示例性的，将起点位姿stpos朝着目标位姿finpos的方向以第二转弯半径进行移动，直到与目标位姿平行得到第三当前位姿midpos3。
209、计算所述第三当前位姿与所述目标位姿的跟踪误差。
路径规划系统计算所述第三当前位姿与所述目标位姿的跟踪误差。该跟踪误差可以是上述的横向跟踪误差，也可以是车辆航向角误差。
示例性的，计算跟踪误差的方式如下所示：
(1)车辆的第三当前位姿为(xr，yr，θ)，目标位姿为(xref，yref，θref)；
(2)计算横向跟踪误差(cross track error，CTE)
其中，跟踪误差向量：d＝(xr-xref，yr-yref) (公式1)
(xref，yref)对应的法向量为：n＝(sinθref，-cosθref) (公式2)
横向跟踪误差为：
CTE＝d·n＝(xr-xref)sinθref-(yr-yref)cosθref (公式3)
(3)计算车辆航向角误差：
θe＝θ-θref (公式4)
210、若所述跟踪误差小于第二预置阈值，直线连接所述第三当前位姿和所述目标位姿，确定第四规划路径，所述第四规划路径包括所述起点位姿与所述第三当前位姿之间的路径，以及所述第三当前位姿与所述目标位姿之间的路径。
如果该跟踪误差小于第二预置阈值时，可以直线连接第三当前位姿和目标位姿，那么第四规划路径就可以包括起点位姿到第三当前位姿的圆弧路径，以及第三当前位姿到目标位姿的直线路径。即如果midpos3与finpos的第二跟踪误差小于第四预置阈值，则认为规划成功，然后补充从midpos3到finpos的一段直线，得到完整规划路径。如图3C所示，为本发明实施例中得到第四规划路径的一个示意图。
在本发明实施例中，获取起点位姿和目标位姿；确定所述起点位姿和所述目标位姿的距离；若所述距离大于第一预置阈值，则将所述目标位姿朝着所述起点位姿的方向以第一转弯半径进行移动，得到第一当前位姿，所述第一转弯半径大于等于所述车辆的最小转弯半径；根据所述第一当前位姿和所述起点位姿，使用目标连接方法，若成功则得到第一规划路径，所述第一规划路径，其中，所述第一规划路径包括所述起点位姿与所述第一当前位姿之间的路径，以及所述第一当前位姿与所述目标位姿之间的路径。通过车辆当前位姿与目标位姿的距离的判断，选择对应的重新规划的连接方式，进行泊车的路径重新规划，进行误差消除，实现车辆平行泊车的准确性。
如图4所示，为本发明实施例中路径规划系统的一个实施例示意图，可以包括：
获取模块401，用于获取起点位姿和目标位姿；
处理模块402，用于确定所述起点位姿和所述目标位姿的距离；若所述距离大于第一预置阈值，则将所述目标位姿朝着所述起点位姿的方向以第一转弯半径进行移动，得到第一当前位姿，所述第一转弯半径大于等于所述车辆的最小转弯半径；使用所述目标连接方法连接所述第一当前位姿和所述起点位姿，若成功则得到第一规划路径，其中，所述目标连接方法为直线连接方法和圆弧连接方法中的至少一种，所述第一规划路径包括所述起点位姿与所述第一当前位姿之间的路径，以及所述第一当前位姿与所述目标位姿之间的路径。
可选的，在本发明的一些实施例中，
处理模块402，还用于若所述距离小于等于所述第一预置阈值，使用所述目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第二规划路径。
可选的，在本发明的一些实施例中，
处理模块402，还用于将所述起点位姿朝着所述目标位姿的方向以直线移动预置长度，得到第二当前位姿；使用所述目标连接方法连接所述第二当前位姿和所述目标位姿，若成功，则确定第三规划路径，所述第三规划路径包括所述起点位姿与所述第二当前位姿之间的路径，以及所述第二当前位姿与所述目标位姿之间的路径。
可选的，在本发明的一些实施例中，
处理模块402，还用于将所述起点位姿朝着所述目标位姿的方向以第二转弯半径进行移动，得到第三当前位姿，所述第二转弯半径大于等于所述车辆的最小转弯半径，所述第三当前位姿与所述目标位姿平行；计算所述第三当前位姿与所述目标位姿的跟踪误差；若所述跟踪误差小于第二预置阈值，则直线连接所述第三当前位姿和所述目标位姿，确定第四规划路径，所述第四规划路径包括所述起点位姿与所述第三当前位姿之间的路径，以及所述第三当前位姿与所述目标位姿之间的路径。
可选的，本发明实施例中还提供一种车辆，该车辆包括如图4所示的路径规划系统。
如图5所示，为本发明实施例中路径规划系统的另一个实施例示意图。
可以包括：
存储有可执行程序代码的存储器501；
与存储器501耦合的处理器502；
处理器502调用存储器501中存储的可执行程序代码，执行如下步骤：
获取起点位姿和目标位姿；
确定所述起点位姿和所述目标位姿的距离；
若所述距离大于第一预置阈值，则将所述目标位姿朝着所述起点位姿的方向以第一转弯半径进行移动，得到第一当前位姿，所述第一转弯半径大于等于所述车辆的最小转弯半径；
使用所述目标连接方法连接所述第一当前位姿和所述起点位姿，若成功则得到第一规划路径，其中，所述目标连接方法为直线连接方法和圆弧连接方法中的至少一种，所述第一规划路径包括所述起点位姿与所述第一当前位姿之间的路径，以及所述第一当前位姿与所述目标位姿之间的路径。
可选的，在本发明的一些实施例中，处理器502调用存储器501中存储的可执行程序代码，执行如下步骤：
若所述距离小于等于所述第一预置阈值，使用所述目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第二规划路径。
可选的，在本发明的一些实施例中，处理器502调用存储器501中存储的可执行程序代码，执行如下步骤：
将所述起点位姿朝着所述目标位姿的方向以直线移动预置长度，得到第二当前位姿；
使用所述目标连接方法连接所述第二当前位姿和所述目标位姿，若成功，则确定第三规划路径，所述第三规划路径包括所述起点位姿与所述第二当前位姿之间的路径，以及所述第二当前位姿与所述目标位姿之间的路径。
可选的，在本发明的一些实施例中，处理器502调用存储器501中存储的可执行程序代码，执行如下步骤：
将所述起点位姿朝着所述目标位姿的方向以第二转弯半径进行移动，得到第三当前位姿，所述第二转弯半径大于等于所述车辆的最小转弯半径，所述第三当前位姿与所述目标位姿平行；
计算所述第三当前位姿与所述目标位姿的跟踪误差；
若所述跟踪误差小于第二预置阈值，则直线连接所述第三当前位姿和所述目标位姿，确定第四规划路径，所述第四规划路径包括所述起点位姿与所述第三当前位姿之间的路径，以及所述第三当前位姿与所述目标位姿之间的路径。
在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时，可以全部或部分地以计算机程序产品的形式实现。
所述计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行所述计算机程序指令时，全部或部分地产生按照本发明实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一计算机可读存储介质传输，例如，所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(DSL))或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。所述计算机可读存储介质可以是计算机能够存储的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质，(例如，软盘、硬盘、磁带)、光介质(例如，DVD)、或者半导体介质(例如固态硬盘Solid State Disk(SSD))等。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在本发明所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (7)
1.一种路径规划方法，其特征在于，包括：
获取起点位姿和目标位姿；
确定所述起点位姿和所述目标位姿的距离；
若所述距离大于第一预置阈值，则将所述目标位姿朝着所述起点位姿的方向以第一转弯半径进行移动，得到第一当前位姿，所述第一转弯半径大于等于车辆的最小转弯半径；
使用目标连接方法连接所述第一当前位姿和所述起点位姿，若成功则得到第一规划路径，其中，所述目标连接方法为直线连接方法和圆弧连接方法中的至少一种，所述第一规划路径包括所述起点位姿与所述第一当前位姿之间的路径，以及所述第一当前位姿与所述目标位姿之间的路径；
若所述距离小于等于所述第一预置阈值，使用所述目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第二规划路径；
若失败，则将所述起点位姿朝着所述目标位姿的方向以第二转弯半径进行移动，得到第三当前位姿，所述第二转弯半径大于等于所述车辆的最小转弯半径，所述第三当前位姿与所述目标位姿平行；
计算所述第三当前位姿与所述目标位姿的跟踪误差；
若所述跟踪误差小于第二预置阈值，则直线连接所述第三当前位姿和所述目标位姿，确定第四规划路径，所述第四规划路径包括所述起点位姿与所述第三当前位姿之间的路径，以及所述第三当前位姿与所述目标位姿之间的路径。
2.根据权利要求1所述的方法，其特征在于，所述根据所述起点位姿和所述目标位姿，使用所述目标连接方法，若失败之后，所述方法还包括：
将所述起点位姿朝着所述目标位姿的方向以直线移动预置长度，得到第二当前位姿；
使用所述目标连接方法连接所述第二当前位姿和所述目标位姿，若成功，则确定第三规划路径，所述第三规划路径包括所述起点位姿与所述第二当前位姿之间的路径，以及所述第二当前位姿与所述目标位姿之间的路径。
3.根据权利要求2所述的方法，其特征在于，所述使用所述目标连接方法连接所述第二当前位姿和所述目标位姿，若失败之后，所述方法还包括：
将所述起点位姿朝着所述目标位姿的方向以第二转弯半径进行移动，得到第三当前位姿，所述第二转弯半径大于等于所述车辆的最小转弯半径，所述第三当前位姿与所述目标位姿平行；
计算所述第三当前位姿与所述目标位姿的跟踪误差；
若所述跟踪误差小于第二预置阈值，则直线连接所述第三当前位姿和所述目标位姿，确定第四规划路径，所述第四规划路径包括所述起点位姿与所述第三当前位姿之间的路径，以及所述第三当前位姿与所述目标位姿之间的路径。
4.一种路径规划系统，其特征在于，包括：
获取模块，用于获取起点位姿和目标位姿；
处理模块，用于确定所述起点位姿和所述目标位姿的距离；若所述距离大于第一预置阈值，则将所述目标位姿朝着所述起点位姿的方向以第一转弯半径进行移动，得到第一当前位姿，所述第一转弯半径大于等于车辆的最小转弯半径；使用目标连接方法连接所述第一当前位姿和所述起点位姿，若成功则得到第一规划路径，其中，所述目标连接方法为直线连接方法和圆弧连接方法中的至少一种，所述第一规划路径包括所述起点位姿与所述第一当前位姿之间的路径，以及所述第一当前位姿与所述目标位姿之间的路径；
所述处理模块，还用于若所述距离小于等于所述第一预置阈值，使用所述目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第二规划路径；
所述处理模块，还用于将所述起点位姿朝着所述目标位姿的方向以第二转弯半径进行移动，得到第三当前位姿，所述第二转弯半径大于等于所述车辆的最小转弯半径，所述第三当前位姿与所述目标位姿平行；计算所述第三当前位姿与所述目标位姿的跟踪误差；若所述跟踪误差小于第二预置阈值，则直线连接所述第三当前位姿和所述目标位姿，确定第四规划路径，所述第四规划路径包括所述起点位姿与所述第三当前位姿之间的路径，以及所述第三当前位姿与所述目标位姿之间的路径。
5.根据权利要求4所述的路径规划系统，其特征在于，
所述处理模块，还用于将所述起点位姿朝着所述目标位姿的方向以直线移动预置长度，得到第二当前位姿；使用所述目标连接方法连接所述第二当前位姿和所述目标位姿，若成功，则确定第三规划路径，所述第三规划路径包括所述起点位姿与所述第二当前位姿之间的路径，以及所述第二当前位姿与所述目标位姿之间的路径。
6.一种车辆，其特征在于，包括如权利要求4或5所述的路径规划系统。
7.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储计算机程序，所述计算机程序被处理器执行时实现如权利要求1-3中任一项所述的路径规划方法。
Priority Applications (3)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910797675.6A CN110440823B (zh) | 2019-08-27 | 2019-08-27 | 一种路径规划方法及路径规划系统 |
| EP19942652.9A EP3922949A4 (en) | 2019-08-27 | 2019-12-18 | METHOD AND SYSTEM FOR PATHWAY PLANNING |
| PCT/CN2019/126414 WO2021036107A1 (zh) | 2019-08-27 | 2019-12-18 | 一种路径规划方法及路径规划系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910797675.6A CN110440823B (zh) | 2019-08-27 | 2019-08-27 | 一种路径规划方法及路径规划系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110440823A CN110440823A (zh) | 2019-11-12 |
| CN110440823B true CN110440823B (zh) | 2021-04-13 |
Family
ID=68438010
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910797675.6A Active CN110440823B (zh) | 2019-08-27 | 2019-08-27 | 一种路径规划方法及路径规划系统 |
Country Status (3)
| Country | Link |
|---|---|
| EP (1) | EP3922949A4 (zh) |
| CN (1) | CN110440823B (zh) |
| WO (1) | WO2021036107A1 (zh) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110440823B (zh) * | 2019-08-27 | 2021-04-13 | 广州小鹏汽车科技有限公司 | 一种路径规划方法及路径规划系统 |
| CN111750859B (zh) * | 2020-05-29 | 2021-11-05 | 广州极飞科技股份有限公司 | 过渡路径规划方法及相关装置 |
| CN111766870B (zh) * | 2020-05-29 | 2021-11-05 | 广州极飞科技股份有限公司 | 过渡路径和作业路径的规划方法及相关装置 |
| US20220397914A1 (en) * | 2021-06-04 | 2022-12-15 | Ubtech North America Research And Development Center Corp | Fast continuous regulation of nonholonomic mobile robots |
| CN113561968B (zh) * | 2021-08-27 | 2022-12-30 | 上海欧菲智能车联科技有限公司 | 一种车辆水平泊出轨迹规划方法及装置、车辆、存储介质 |
| CN114355925B (zh) * | 2021-12-29 | 2024-03-19 | 杭州海康机器人股份有限公司 | 路径规划方法、装置、设备及计算机可读存储介质 |
| CN114038203A (zh) * | 2022-01-12 | 2022-02-11 | 成都四方伟业软件股份有限公司 | 一种交通仿真中两点路口车道的曲线拟合方法及装置 |
Family Cites Families (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101203897B1 (ko) * | 2011-02-25 | 2012-11-23 | 동국대학교 산학협력단 | 이동체（ｍｏｂｉｌｅ ｂｏｄｙ）를 위한 셀?기반 경로 계획 장치 및 방법 |
| CN102963355B (zh) * | 2012-11-01 | 2016-05-25 | 同济大学 | 一种智能辅助泊车方法及其实现系统 |
| CN105197010B (zh) * | 2014-06-04 | 2018-03-27 | 长春孔辉汽车科技股份有限公司 | 辅助泊车系统以及辅助泊车控制方法 |
| JP6642820B2 (ja) * | 2015-10-21 | 2020-02-12 | 日立オートモティブシステムズ株式会社 | 駐車経路算出装置、駐車支援装置および駐車経路算出方法 |
| JP6594736B2 (ja) * | 2015-10-27 | 2019-10-23 | クラリオン株式会社 | 駐車支援装置 |
| CN107672585B (zh) * | 2017-08-29 | 2019-09-17 | 广州小鹏汽车科技有限公司 | 一种自动泊车路径规划方法及系统 |
| CN108534777A (zh) * | 2018-01-25 | 2018-09-14 | 江苏大学 | 一种平行泊车路径规划方法 |
| CN109703553B (zh) * | 2019-01-30 | 2020-08-04 | 长安大学 | 一种基于牵引点跟踪的自动泊车方法 |
| CN109927717B (zh) * | 2019-03-22 | 2020-07-17 | 湖北亿咖通科技有限公司 | 泊车轨迹的确定方法、装置及智能终端 |
| CN110440823B (zh) * | 2019-08-27 | 2021-04-13 | 广州小鹏汽车科技有限公司 | 一种路径规划方法及路径规划系统 |
| CN110440824A (zh) * | 2019-08-27 | 2019-11-12 | 广州小鹏汽车科技有限公司 | 一种路径规划方法及路径规划系统 |
-
2019
- 2019-08-27 CN CN201910797675.6A patent/CN110440823B/zh active Active
- 2019-12-18 EP EP19942652.9A patent/EP3922949A4/en active Pending
- 2019-12-18 WO PCT/CN2019/126414 patent/WO2021036107A1/zh unknown
Also Published As
| Publication number | Publication date |
|---|---|
| WO2021036107A1 (zh) | 2021-03-04 |
| EP3922949A4 (en) | 2022-05-25 |
| CN110440823A (zh) | 2019-11-12 |
| EP3922949A1 (en) | 2021-12-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110440823B (zh) | 一种路径规划方法及路径规划系统 | |
| WO2021036108A1 (zh) | 一种路径规划方法及路径规划系统 | |
| JP3316842B2 (ja) | 無人搬送車システムと無人搬送車の誘導方法 | |
| US10168150B2 (en) | Apparatus and method for auto-leveling steering wheel | |
| CN111369066A (zh) | 路径规划方法、装置、电子设备及可读存储介质 | |
| EP2976934B1 (en) | Vehicle guidance system | |
| US20130265175A1 (en) | Parking control apparatus and method for providing an alarm thereof | |
| US11721109B2 (en) | Apparatus and method for compensating for heading angle | |
| CN107421545B (zh) | 一种机器人的位置偏差检测方法、装置及机器人 | |
| CN111238520A (zh) | 变道路径规划方法、装置、电子设备和计算机可读介质 | |
| CN108928390B (zh) | 自动转向控制装置以及转向控制方法 | |
| CN111231942A (zh) | 停车辅助装置 | |
| CN111413968A (zh) | 车辆的控制方法、装置及电子设备 | |
| CN110576848A (zh) | 动态路径规划的方法及装置 | |
| JP2022013609A (ja) | バック軌跡追跡方法、装置、電子設備、記憶媒体、及びプログラム | |
| CN112478540B (zh) | 控制托盘转动的方法和装置 | |
| CN110221313B (zh) | Agv小车的激光雷达安装位置修正方法以及agv小车 | |
| CN115469667A (zh) | 车辆航迹的确定方法、装置、电子设备和存储介质 | |
| CN113126607A (zh) | 一种机器人及其运动控制方法和装置 | |
| CN116300955A (zh) | 路径跟踪方法、装置、终端设备及计算机可读存储介质 | |
| JP6675354B2 (ja) | 車両用操舵制御装置 | |
| JP2017007409A (ja) | 車両用情報処理装置、車両用情報処理システム、および車両用情報処理プログラム | |
| KR20110104681A (ko) | 차선유지 제어 방법 및 그 시스템 | |
| CN112577503B (zh) | 车辆起点区域的路径规划方法、装置、设备 | |
| JP2017167925A (ja) | 駐車列特定システムおよび駐車列特定プログラム |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| CB02 | Change of applicant information | ||
| CB02 | Change of applicant information |
Address after: 510000 No.8 Songgang street, Cencun, Tianhe District, Guangzhou City, Guangdong Province Applicant after: GUANGZHOU XPENG AUTOMOBILE TECHNOLOGY Co.,Ltd. Address before: Room 245, No. 333, jiufo Jianshe Road, Zhongxin Guangzhou Knowledge City, Guangzhou City, Guangdong Province Applicant before: GUANGZHOU XPENG AUTOMOBILE TECHNOLOGY Co.,Ltd. |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| EE01 | Entry into force of recordation of patent licensing contract | ||
| EE01 | Entry into force of recordation of patent licensing contract |
Application publication date: 20191112 Assignee: Zhaoqing Xiaopeng New Energy Investment Co.,Ltd. Assignor: GUANGZHOU XIAOPENG MOTORS TECHNOLOGY Co.,Ltd. Contract record no.: X2021990000645 Denomination of invention: A path planning method and path planning system Granted publication date: 20210413 License type: Common License Record date: 20211018 |