一种路径规划方法及路径规划系统与流程

本发明涉及智能汽车技术领域，尤其涉及一种路径规划方法及路径规划系统。

背景技术：

作为一种方便、快捷的交通工具，汽车已成为人们生活和工作的重要组成部分。随着汽车数量的逐年增加，有限的城市空间显得日趋拥挤，车辆平均分配到的停放空间也日趋缩小，车辆泊车入位困难问题在人们生活中逐渐显现。人们对车辆使用轻便性及安全性要求促使越来越多汽车生产商、科研机构及高校对泊车系统进行研究。

技术实现要素：

本发明实施例提供了一种路径规划方法及路径规划系统，用于在满足预置条件下，进行路径规划，提高车辆行驶的准确度。

有鉴于此，本发明第一方面包括一种路径规划方法，可以包括：

当满足预置条件时，确定车辆的起点位姿；

确定目标位姿，所述目标位姿为待规划路径的终点位姿；

使用目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第一规划路径，其中，所述目标连接方法为直线连接方法和圆弧连接方法中的至少一种。

可选的，在本发明的一些实施例中，所述确定车辆的起点位姿之前，所述方法还包括：

获取所述车辆的已规划路径；

获取所述车辆的第一当前位姿；

所述当满足预置条件时，确定车辆的起点位姿，包括：

确定所述车辆的第一当前位姿与最近路径点的位姿的第一跟踪误差，所述已规划路径包括所述最近路径点的位姿；

当所述第一跟踪误差大于第一预置阈值时，确定所述车辆的起点位姿为所述第一当前位姿；

所述确定目标位姿，包括：

确定目标位姿为所述已规划路径的终点位姿。

可选的，在本发明的一些实施例中，所述确定车辆的起点位姿之前，所述方法还包括：

获取所述车辆的已规划路径；

获取所述车辆的第一当前位姿；

获取发生变化的终点位姿；

所述当满足预置条件时，确定车辆的起点位姿，包括：

当所述发生变化的终点位姿与所述车辆的已规划路径的终点位姿的距离大于第二预置阈值时，确定所述车辆的起点位姿为所述第一当前位姿；

所述确定目标位姿，包括：

确定目标位姿为所述发生变化的终点位姿。

可选的，在本发明的一些实施例中，所述确定车辆的起点位姿之前，所述方法还包括：

获取所述车辆的已规划路径；

获取所述车辆的第一当前位姿；

所述当满足预置条件时，确定车辆的起点位姿，包括：

当在所述已规划路径上检测到所述车辆的第一当前位姿与障碍物的距离小于第三预置阈值时，控制所述车辆朝远离所述已规划路径的终点位姿的方向行驶，获取所述车辆的第二当前位姿；

确定所述车辆的起点位姿为所述第二当前位姿；

所述确定目标位姿，包括：

确定目标位姿为所述已规划路径的终点位姿。

可选的，在本发明的一些实施例中，所述使用目标连接方法连接所述起点位姿和所述目标位姿，若失败之后，所述方法还包括：

将所述起点位姿朝着所述目标位姿的方向以第一转弯半径进行移动，得到第三当前位姿，所述第一转弯半径大于等于所述车辆的最小转弯半径，所述第三当前位姿与所述目标位姿平行；

计算所述第三当前位姿与所述目标位姿的第二跟踪误差；

若所述第二跟踪误差小于第四预置阈值，则直线连接所述第三当前位姿和所述目标位姿，确定第二规划路径，所述第二规划路径包括所述起点位姿与所述第三当前位姿之间的路径，以及所述第三当前位姿与所述目标位姿之间的路径。

可选的，在本发明的一些实施例中，所述方法还包括：

若所述第二跟踪误差大于等于所述第四预置阈值，则将所述起点位姿朝着所述目标位姿的方向以直线移动预置长度，得到第四当前位姿；

使用所述目标连接方法连接所述第四当前位姿和所述目标位姿，若成功，则确定第三规划路径，所述第三规划路径包括所述起点位姿与所述第四当前位姿之间的路径，以及所述第四当前位姿与所述目标位姿之间的路径。

可选的，在本发明的一些实施例中，所述方法还包括：

将所述起点位姿朝着所述目标位姿的方向以第二转弯半径进行移动，得到第五当前位姿，所述第二转弯半径大于等于所述车辆的最小转弯半径；

使用所述目标连接方法连接所述第五当前位姿和所述目标位姿，若成功，则确定第四规划路径，所述第四规划路径包括所述起点位姿与第五当前位姿之间的路径，以及所述第五当前位姿与所述目标位姿之间的路径。

可选的，在本发明的一些实施例中，所述方法还包括：

根据所述起点位姿确定第一圆弧，所述第一圆弧与所述起点位姿相切；

根据所述目标位姿确定第二圆弧，所述第二圆弧与所述终点位姿相切；

将所述第一圆弧朝着所述目标位姿的方向移动，得到第三圆弧，所述第三圆弧与所述第二圆弧相切；

若所述第三圆弧与所述第二圆弧相切的切点在所述起点位姿与所述目标位姿之间，则确定第五规划路径，所述第五规划路径包括所述起点位姿与所述切点之间的路径，以及所述切点与所述目标位姿之间的路径。

本发明第二方面提供一种路径规划系统，可以包括：

确定模块，用于当满足预置条件时，确定车辆的起点位姿；确定目标位姿，所述目标位姿为待规划路径的终点位姿；

处理模块，用于使用目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第一规划路径。

可选的，在本发明的一些实施例中，所述路径规划系统还包括：获取模块；

所述获取模块，用于获取所述车辆的已规划路径；获取所述车辆的第一当前位姿；

所述确定模块，具体用于确定所述车辆的第一当前位姿与最近路径点的位姿的第一跟踪误差，所述已规划路径包括所述最近路径点的位姿；当所述第一跟踪误差大于第一预置阈值时，确定所述车辆的起点位姿为所述第一当前位姿；确定目标位姿为所述已规划路径的终点位姿。

可选的，在本发明的一些实施例中，所述路径规划系统还包括：获取模块；

所述获取模块，用于获取所述车辆的已规划路径；获取所述车辆的第一当前位姿；获取发生变化的终点位姿；

所述确定模块，具体用于当所述发生变化的终点位姿与所述车辆的已规划路径的终点位姿的距离大于第二预置阈值时，确定所述车辆的起点位姿为所述第一当前位姿；确定目标位姿为所述发生变化的终点位姿。

可选的，在本发明的一些实施例中，所述路径规划系统还包括：获取模块；

所述获取模块，用于获取所述车辆的已规划路径；获取所述车辆的第一当前位姿；

所述确定模块，具体用于当在所述已规划路径上检测到所述车辆的第一当前位姿与障碍物的距离小于第三预置阈值时，控制所述车辆朝远离所述已规划路径的终点位姿的方向行驶，获取所述车辆的第二当前位姿；确定所述车辆的起点位姿为所述第二当前位姿；确定目标位姿为所述已规划路径的终点位姿。

可选的，在本发明的一些实施例中，

所述处理模块，还用于将所述起点位姿朝着所述目标位姿的方向以第一转弯半径进行移动，得到第三当前位姿，所述第一转弯半径大于等于所述车辆的最小转弯半径，所述第三当前位姿与所述目标位姿平行；计算所述第三当前位姿与所述目标位姿的第二跟踪误差；若所述第二跟踪误差小于第四预置阈值，则直线连接所述第三当前位姿和所述目标位姿，确定第二规划路径，所述第二规划路径包括所述起点位姿与所述第三当前位姿之间的路径，以及所述第三当前位姿与所述目标位姿之间的路径。

可选的，在本发明的一些实施例中，

所述处理模块，还用于若所述第二跟踪误差大于等于所述第四预置阈值，则将所述起点位姿朝着所述目标位姿的方向以直线移动预置长度，得到第四当前位姿；使用所述目标连接方法连接所述第四当前位姿和所述目标位姿，若成功，则确定第三规划路径，所述第三规划路径包括所述起点位姿与所述第四当前位姿之间的路径，以及所述第四当前位姿与所述目标位姿之间的路径。

可选的，在本发明的一些实施例中，

所述处理模块，还用于将所述起点位姿朝着所述目标位姿的方向以第二转弯半径进行移动，得到第五当前位姿，所述第二转弯半径大于等于所述车辆的最小转弯半径；使用所述目标连接方法连接所述第五当前位姿和所述目标位姿，若成功，则确定第四规划路径，所述第四规划路径包括所述起点位姿与第五当前位姿之间的路径，以及所述第五当前位姿与所述目标位姿之间的路径。

可选的，在本发明的一些实施例中，

所述处理模块，还用于根据所述起点位姿确定第一圆弧，所述第一圆弧与所述起点位姿相切；根据所述目标位姿确定第二圆弧，所述第二圆弧与所述终点位姿相切；将所述第一圆弧朝着所述目标位姿的方向移动，得到第三圆弧，所述第三圆弧与所述第二圆弧相切；若所述第三圆弧与所述第二圆弧相切的切点在所述起点位姿与所述目标位姿之间，则确定第五规划路径，所述第五规划路径包括所述起点位姿与所述切点之间的路径，以及所述切点与所述目标位姿之间的路径。

本发明第三方面提供一种车辆，可以包括如本发明第二方面及第二方面任一可选方式中所述的路径规划系统。

本发明第四方面提供一种计算机可读存储介质，其存储计算机程序，其中，所述计算机程序使得计算机执行本发明实施例第一方面公开的一种路径规划方法。

从以上技术方案可以看出，本发明实施例具有以下优点：

在本发明实施例中，当满足预置条件时，确定车辆的起点位姿；确定目标位姿，所述目标位姿为待规划路径的终点位姿；使用目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第一规划路径，其中，所述目标连接方法为直线连接方法和圆弧连接方法中的至少一种。即当满足预置条件时，可以根据车辆的起点位姿和目标位姿，进行路径规划，提高车辆行驶到目标位姿的准确度。

附图说明

为了更清楚地说明本发明实施例技术方案，下面将对实施例和现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，还可以根据这些附图获得其它的附图。

图1a所示，为本发明实施例中起点位姿和目标位姿在y轴上的投影示意图；

图1b为本发明实施例中使用目标连接方法连接起点位姿和目标位姿，得到第一规划路径的第一个示意图；

图1c为本发明实施例中使用目标连接方法连接起点位姿和目标位姿，得到第一规划路径的第二个示意图；

图1d为本发明实施例中使用目标连接方法连接起点位姿和目标位姿，得到第一规划路径的第三个示意图；

图1e为本发明实施例中使用目标连接方法连接起点位姿和目标位姿，得到第一规划路径的第四个示意图；

图2为本发明实施例中路径规划方法的第一个实施例示意图；

图3为本发明实施例中路径规划方法的第二个实施例示意图；

图4a为本发明实施例中路径规划方法的第三个实施例示意图；

图4b为本发明实施例中得到第一规划路径的一个示意图；

图5a为本发明实施例中路径规划方法的第四个实施例示意图；

图5b为本发明实施例中得到第二规划路径的一个示意图；

图6a为本发明实施例中路径规划方法的第五个实施例示意图；

图6b为本发明实施例中得到第三规划路径的一个示意图；

图7a为本发明实施例中路径规划方法的第六个实施例示意图；

图7b为本发明实施例中得到第四规划路径的一个示意图；

图8a为本发明实施例中路径规划方法的第七个实施例示意图；

图8b为本发明实施例中得到第五规划路径的一个示意图；

图9a为本发明实施例中路径规划系统的第一个实施例示意图；

图9b为本发明实施例中路径规划系统的第二个实施例示意图；

图10为本发明实施例中路径规划系统的第三个实施例示意图。

具体实施方式

本发明实施例提供了一种路径规划方法及路径规划系统，用于在满足预置条件下，进行路径规划，提高车辆行驶的准确度。

为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，都应当属于本发明保护的范围。

本发明实施例可以应用在车辆正常行驶，或者倒车行驶，或者垂直泊车或者其他的场景中，具体不做限定。

下面先对本发明实施例中所涉及的目标连接方法做一个简要的说明，其中，所述目标连接方法为直线连接方法和圆弧连接方法中的至少一种。如下所示：

假设起点位姿用点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作与ax轴平行的辅助线，得到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、获取车辆的已规划路径和所述车辆的第一当前位姿。

在本发明实施例中，路径规划系统获取车辆的已规划路径和所述车辆的第一当前位姿。其中，车辆的已规划路径包括至少两个路径点的位姿，位姿可以包括位置坐标和航向角信息。

车辆上可以装设有惯性测量单元(inedrtialmeasurementunit，imu)、轮脉冲计数器等传感器，这些传感器可以作为车辆的定位模块(如车身里程计)，来获取车辆的第一当前位姿。

202、当满足预置条件时，确定车辆的起点位姿。

当满足预置条件时，路径规划系统确定车辆的起点位姿，可以包括：路径规划系统确定所述车辆的第一当前位姿与最近路径点的位姿的第一跟踪误差，所述已规划路径包括所述最近路径点的位姿；当所述第一跟踪误差大于第一预置阈值时，路径规划系统确定所述车辆的起点位姿为所述第一当前位姿。可以理解的是，第一预置阈值的设置一般是根据经验值得到的，如果对路径跟踪的可靠性要求比较高，则可以将第一预置阈值设置的小一些，如果对路径跟踪的可靠性要求稍微低一些，则可以将第一预置阈值设置的大一些。

示例性的，计算第一跟踪误差的方式如下所示：

(1)车辆的第一当前位姿为(xr，yr，θ)，寻找已规划路径上距离车辆的最近路径点的位姿为(xref，yref，θref)；

(2)计算横向跟踪误差(crosstrackerror，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)

(4)控制律设计：

δ＝-(k1cte+k2θe)(公式5)

其中，δ为前轮侧偏角，也可以计算eps转角，由于前轮侧偏角和eps转角二者之间是比例关系，因此只是一个比例系数的关系。k1、k2为反馈比例系数，根据不同的控制律有不同的计算方法，由于控制律有很多，且与本申请内容无关，此处不再赘述，无论采用何种控制律，只要保证cte和θe可以收敛到0即可。

203、确定目标位姿。

路径规划系统确定目标位姿，可以包括：路径规划系统确定目标位姿为所述已规划路径的终点位姿。

204、使用目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第一规划路径，其中，所述目标连接方法为直线连接方法和圆弧连接方法中的至少一种。

路径规划系统使用目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第一规划路径。

需要说明的是，使用目标连接方法连接起点位姿和目标位姿，得到第一规划路径，可以参考上述图1a至图1b的说明，此处不再赘述。

在本发明实施例中，获取所述车辆的已规划路径；获取所述车辆的第一当前位姿；确定所述车辆的第一当前位姿与最近路径点的位姿的第一跟踪误差，所述已规划路径包括所述最近路径点的位姿；当所述第一跟踪误差大于第一预置阈值时，确定所述车辆的起点位姿为所述第一当前位姿；确定目标位姿为所述已规划路径的终点位姿。使用目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第一规划路径。即当车辆的第一当前位姿和最近路径点的位姿的第一跟踪误差大于第一预置阈值时，可以根据车辆的第一当前位姿和已规划路径的终点位姿，使用目标连接方法重新进行路径规划，进行误差消除，保证车辆行驶的准确性。

如图3所示，为本发明实施例中路径规划方法的第二个实施例示意图，可以包括：

301、获取所述车辆的已规划路径、所述车辆的第一当前位姿和发生变化的终点位姿。

在本发明实施例中，路径规划系统获取车辆的已规划路径和所述车辆的第一当前位姿。其中，车辆的已规划路径包括至少两个路径点的位姿，位姿可以包括位置坐标和航向角信息。

车辆上可以装设有惯性测量单元(inedrtialmeasurementunit，imu)、轮脉冲计数器等传感器，这些传感器可以作为车辆的定位模块(如车身里程计)，来获取车辆的第一当前位姿。

路径规划系统还获取车辆发生变化的终点位姿，该终点位姿可以由用户输入，也可以由其他设备发送过来的，具体获取车辆发生变化的终点位姿的方式不做限定。

302、当满足预置条件时，确定车辆的起点位姿。

当满足预置条件时，路径规划系统确定车辆的起点位姿，可以包括：当所述发生变化的终点位姿与所述车辆的已规划路径的终点位姿的距离大于第二预置阈值时，路径规划系统确定所述车辆的起点位姿为所述第一当前位姿。

其中，第二预置阈值的设置一般是根据经验值得到的。示例性的，按照车辆的已规划路径可以行使到发生变化后的终点位姿，以及，按照车辆的已规划路径不能行驶到发生变化后的终点位姿，这两种情况的临界值对应的发生变化后的终点位姿与已规划路径的终点位姿的距离可以作为第二预置阈值。

可以理解的是，位姿可以包括位置坐标、航向角等信息。路径规划系统可以根据更新的位置与车辆的已规划路径的终点位置来计算两者之间的距离，当该距离大于第二预置阈值时，证明发生变化的终点位姿与已规划路径的终点位姿的距离比较远，需要为车辆重新规划路径。路径规划系统确定车辆的第一当前位姿为起点位姿。

303、确定目标位姿。

路径规划系统确定目标位姿，可以包括：路径规划系统确定目标位姿为所述发生变化的终点位姿。

304、使用目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第一规划路径。

路径规划系统使用目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第一规划路径。

需要说明的是，使用目标连接方法连接起点位姿和目标位姿，得到第一规划路径，可以参考上述图1a至图1b的说明，此处不再赘述。

在本发明实施例中，获取所述车辆的已规划路径、所述车辆的第一当前位姿和发生变化的终点位姿。当所述发生变化的终点位姿与所述车辆的已规划路径的终点位姿的距离大于第二预置阈值时，路径规划系统确定所述车辆的起点位姿为所述第一当前位姿；路径规划系统确定目标位姿为所述发生变化的终点位姿；使用目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第一规划路径。即当车辆的终点位姿发生变化时，确定发生变化后的终点位姿与已规划路径的终点位姿的距离，如果该距离大于第二预置阈值时，可以确定起点位姿为车辆的第一当前位姿，目标位姿为发生变化的终点位姿，可以根据车辆的第一当前位姿和发生变化的终点位姿，使用目标连接方法重新进行路径规划，保证车辆行驶的准确性。

如图4a所示，为本发明实施例中路径规划方法的第三个实施例示意图，可以包括：

401、获取所述车辆的已规划路径和所述车辆的第一当前位姿。

在本发明实施例中，路径规划系统获取车辆的已规划路径和所述车辆的第一当前位姿。其中，车辆的已规划路径包括至少两个路径点的位姿，位姿可以包括位置坐标和航向角信息。

车辆上可以装设有惯性测量单元(inedrtialmeasurementunit，imu)、轮脉冲计数器等传感器，这些传感器可以作为车辆的定位模块(如车身里程计)，来获取车辆的第一当前位姿。

402、当满足预置条件时，确定车辆的起点位姿。

当满足预置条件时，确定车辆的起点位姿，可以包括：当在所述已规划路径上检测到所述车辆的第一当前位姿与障碍物的距离小于第三预置阈值时，路径规划系统控制所述车辆朝远离所述已规划路径的终点位姿的方向行驶，获取所述车辆的第二当前位姿；路径规划系统确定所述车辆的起点位姿为所述第二当前位姿。

可以理解的是，第三预置阈值的设置一般是根据经验值得到的。如果车辆的第一当前位姿与障碍物的距离小于第三预置阈值时，可以认为车辆处于危险范围，即车辆与障碍物相撞的可能性比较大。如果车辆的第一当前位姿与障碍物的距离大于等于第三预置阈值时，可以认为车辆还处于安全范围，车辆有足够的距离和时间来绕开障碍物，或者，将障碍物清除等。

示例性的，如果泊车过程中遇到障碍物迫使泊车暂停，可包括后方避障，首先换前进挡离开障碍物一段距离，停车，然后换倒挡，找到一条避开障碍物的倒车路径连通目标位姿；前方避障，从避障卡住的当前位姿直接找到一条连通目标位姿的倒车路径。

可以理解的是，如图4b所示，为本发明实施例中得到第一规划路径的一个示意图。在图4b所示中，控制车辆从起点位姿stpos朝着远离目标位姿finpos的方向移动，移动方式可以包括但不限于：左转、直行、右转等，设置一定长度的移动距离，例如每隔0.1m产生一个第二当前位姿midpos。

403、确定目标位姿。

路径规划系统确定目标位姿，可以包括：路径规划系统确定目标位姿为所述已规划路径的终点位姿。

404、使用目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第一规划路径。

路径规划系统使用目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第一规划路径。例如：从midpos到finpos使用目标连接方法，如果连接成功，则路径规划成功，得到第一规划路径。需要说明的是，使用目标连接方法连接起点位姿和目标位姿的具体实现方式参考上述图1a至图1b的说明，此处不再赘述。

在本发明实施例中，获取所述车辆的已规划路径和所述车辆的第一当前位姿；当在所述已规划路径上检测到所述车辆的第一当前位姿与障碍物的距离小于第三预置阈值时，路径规划系统控制所述车辆朝远离所述已规划路径的终点位姿的方向行驶，获取所述车辆的第二当前位姿；路径规划系统确定所述车辆的起点位姿为所述第二当前位姿；确定目标位姿为所述已规划路径的终点位姿；使用目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第一规划路径，保证车辆行驶的准确性。

可选的，上述图2或者图3所示实施例中，使用目标连接方法连接所述起点位姿和所述目标位姿，若失败，则执行图5a所示实施例的步骤，进行路径的重新规划。如图5a所示，为本发明实施例中路径规划方法的第四个实施例示意图，可以包括：

501、将所述起点位姿朝着所述目标位姿的方向以第一转弯半径进行移动，得到第三当前位姿，所述第一转弯半径大于等于所述车辆的最小转弯半径，所述第三当前位姿与所述目标位姿平行。

在本发明实施例中，路径规划系统将所述起点位姿朝着所述目标位姿的方向以第一转弯半径进行移动，得到第三当前位姿。

第一转弯半径大于等于所述车辆的最小转弯半径，车辆的最小转弯半径是车辆转向机构决定的，每辆车都有唯一特性，即方向盘打死转弯时的半径。

示例性的，将起点位姿stpos朝着目标位姿的方向以第一转弯半径进行移动，直到与目标位姿平行得到midpos1。

502、计算所述第三当前位姿与所述目标位姿的第二跟踪误差。

可以理解的是，路径规划系统计算所述第三当前位姿与所述目标位姿的第二跟踪误差。该第二跟踪误差的计算，可以参考上述公式1至公式5所示，此处不再赘述。该第二跟踪误差可以是上述的横向跟踪误差，也可以是车辆航向角误差，也可以是前轮侧偏角，也可以是eps转角，具体不做限定。

503、若所述第二跟踪误差小于第四预置阈值，则直线连接所述第三当前位姿和所述目标位姿，确定第二规划路径，所述第二规划路径包括所述起点位姿与所述第三当前位姿之间的路径，以及所述第三当前位姿与所述目标位姿之间的路径。

如果第二跟踪误差小于第四预置阈值时，可以直线连接第三当前位姿和目标位姿，那么第二规划路径就可以包括起点位姿到第三当前位姿的圆弧路径和第三当前位姿与目标位姿的直线路径。即如果midpos1与finpos的第二跟踪误差小于第四预置阈值，则认为规划成功，然后补充从midpos1到finpos的一段直线，得到完整规划路径。如图5b所示，为本发明实施例中得到第二规划路径的一个示意图。

可以理解的是，第四预置阈值的设置一般是根据经验值得到的，如果对路径跟踪的可靠性要求比较高，则可以将第四预置阈值设置的小一些，如果对路径跟踪的可靠性要求稍微低一些，则可以将第四预置阈值设置的大一些。

在本发明实施例中，上述图2或者图3所示实施例中路径规划失败，则执行图5a所示实施例的步骤，将所述起点位姿朝着所述目标位姿的方向以第一转弯半径进行移动，得到第三当前位姿，所述第三当前位姿与所述目标位姿平行；计算所述第三当前位姿与所述目标位姿的第二跟踪误差；若所述第二跟踪误差小于第四预置阈值，则直线连接所述第三当前位姿和所述目标位姿，确定第二规划路径，所述第二规划路径包括所述起点位姿与所述第三当前位姿之间的路径，以及所述第三当前位姿与所述目标位姿之间的路径。从而进行路径重新规划，提供了一种路径重新规划的实现方式，保证车辆行驶的准确性。

可选的，上述图2或者图3所示实施例中，使用目标连接方法连接所述起点位姿和所述目标位姿，若失败，或者，在图5a所示的实施例中，若第二跟踪误差大于等于所述第四预置阈值，则可以执行图6a所示实施例的步骤，进行路径的重新规划。如图6a所示，为本发明实施例中路径规划方法的第五个实施例示意图，可以包括：

601、将所述起点位姿朝着所述目标位姿的方向以直线移动预置长度，得到第四当前位姿。

路径规划系统先将起点位姿朝着目标位姿的方向以直线移动预置长度，得到第四当前位姿，这里的预置长度可以是经验值，也可以是由用户输入的值。示例性的，将起点位姿stpos朝着目标位姿的方向直线移动特定长度，得到第四当前位姿midpos2。

602、使用所述目标连接方法连接所述第四当前位姿和所述目标位姿，若成功，则确定第三规划路径，所述第三规划路径包括所述起点位姿与所述第四当前位姿之间的路径，以及所述第四当前位姿与所述目标位姿之间的路径。

路径规划系统使用所述目标连接方法连接所述第四当前位姿和所述目标位姿，若成功，则确定第三规划路径，所述第三规划路径包括所述起点位姿与所述第四当前位姿之间的直线路径，以及所述第四当前位姿与所述目标位姿之间的圆弧路径。示例性的，尝试从midpos2到stpos的简单直线圆弧连接。如图6b所示，为本发明实施例中得到第三规划路径的一个示意图。

在本发明实施例中，上述图2或者图3所示实施例中路径规划失败，在图5a所示的实施例中，若第二跟踪误差大于等于所述第四预置阈值，则执行图6a所示实施例的步骤，将所述起点位姿朝着所述目标位姿的方向以直线移动预置长度，得到第四当前位姿；使用所述目标连接方法连接所述第四当前位姿和所述目标位姿，若成功，则确定第三规划路径，所述第三规划路径包括所述起点位姿与所述第四当前位姿之间的路径，以及所述第四当前位姿与所述目标位姿之间的路径；从而进行路径重新规划，提供了一种路径重新规划的实现方式，保证车辆行驶的准确性。

可选的，上述图2或者图3所示实施例中，使用目标连接方法连接所述起点位姿和所述目标位姿，若失败，或者，在图5a所示的实施例中，若第二跟踪误差大于等于所述第四预置阈值，或者，在图6a所示的实施例中，使用所述目标连接方法连接所述第四当前位姿和所述目标位姿，若失败，则可以执行图7a所示实施例的步骤，进行路径的重新规划。如图7a所示，为本发明实施例中路径规划方法的第六个实施例示意图，可以包括：

701、将所述起点位姿朝着所述目标位姿的方向以第二转弯半径进行移动，得到第五当前位姿，所述第二转弯半径大于等于所述车辆的最小转弯半径。

路径规划系统将所述起点位姿朝着所述目标位姿的方向以第二转弯半径进行移动，得到第五当前位姿，所述第二转弯半径大于等于所述车辆的最小转弯半径。其中，车辆的最小转弯半径是车辆转向机构决定的，每辆车都有唯一特性，即方向盘打死转弯时的半径。

示例性的，将起点位姿stpos朝着目标位姿finpos的方向以第二转弯半径进行移动，每隔一定的步长得到一个midpos3。

702、使用所述目标连接方法连接所述第五当前位姿和所述目标位姿，若成功，则确定第四规划路径，所述第四规划路径包括所述起点位姿与第五当前位姿之间的路径，以及所述第五当前位姿与所述目标位姿之间的路径。

路径规划系统使用所述目标连接方法连接所述第五当前位姿和所述目标位姿，若成功，则确定第四规划路径，所述第四规划路径包括所述起点位姿与第五当前位姿之间的圆弧路径，以及所述第五当前位姿与所述目标位姿之间的路径。其中，第五当前位姿与目标位姿之间的路径可能是直线路径，也可能是圆弧路径，也可能是直线路径+圆弧路径，也可能是圆弧路径+直线路径。

示例性的，尝试从midpos3到finpos的简单直线圆弧连接。如图7b所示，为本发明实施例中得到第四规划路径的一个示意图。

在本发明实施例中，将所述起点位姿朝着所述目标位姿的方向以第二转弯半径进行移动，得到第五当前位姿，所述第二转弯半径大于等于所述车辆的最小转弯半径。使用所述目标连接方法连接所述第五当前位姿和所述目标位姿，若成功，则确定第四规划路径，所述第四规划路径包括所述起点位姿与第五当前位姿之间的路径，以及所述第五当前位姿与所述目标位姿之间的路径。从而进行路径重新规划，提供了一种路径重新规划的实现方式，保证车辆行驶的准确性。

可选的，上述图2或者图3所示实施例中，使用目标连接方法连接所述起点位姿和所述目标位姿，若失败，或者，在图5a所示的实施例中，若第二跟踪误差大于等于所述第四预置阈值，或者，在图6a所示的实施例中，使用所述目标连接方法连接所述第四当前位姿和所述目标位姿，若失败，或者，在图7a所示中使用所述目标连接方法连接所述第五当前位姿和所述目标位姿，若失败，则可以执行图8a所示实施例的步骤。

如图8a所示，为本发明实施例中路径规划方法的第七个实施例示意图，可以包括：

801、根据所述起点位姿确定第一圆弧，所述第一圆弧与所述起点位姿相切。

路径规划系统根据起点位姿确定第一圆弧，第一圆弧与所述起点位姿相切。其中，第一圆弧的半径为第三转弯半径，第三转弯半径大于等于所述车辆的最小转弯半径。

802、根据所述目标位姿确定第二圆弧，所述第二圆弧与所述目标位姿相切。

路径规划系统根据所述目标位姿确定第二圆弧，所述第二圆弧与所述目标位姿相切。其中，第二圆弧的半径为第四转弯半径，第四转弯半径大于等于所述车辆的最小转弯半径。

需要说明的是，步骤801与步骤802的时序不做限定。

803、将所述第一圆弧朝着所述目标位姿的方向移动，得到第三圆弧，所述第三圆弧与所述第二圆弧相切。

路径规划系统将所述第一圆弧朝着所述目标位姿的方向移动，得到第三圆弧，所述第三圆弧与所述第二圆弧相切。示例性的，如图8b所示，为本发明实施例中得到第五规划路径的一个示意图。设stpos与finpos处分别生成第一圆弧和第二圆弧，先后尝试沿着stpos朝finpos的方向移动第一圆弧，得到第三圆弧。

804、若所述第三圆弧与所述第二圆弧相切的切点在所述起点位姿与所述目标位姿之间，则确定第五规划路径，所述第五规划路径包括所述起点位姿与所述切点之间的路径，以及所述切点与所述目标位姿之间的路径。

若所述第三圆弧与所述第二圆弧相切的切点在所述起点位姿与所述目标位姿之间，则路径规划系统确定第五规划路径，所述第五规划路径包括所述起点位姿与所述切点之间的路径，以及所述切点与所述目标位姿之间的路径。示例性的，如果第一圆弧移动后的第三圆弧与第二圆弧相切，得到切点tanpt，如果切点tanpt在stpos于finpos之间，则认为路径规划成功。

在本发明实施例中，根据所述起点位姿确定第一圆弧，所述第一圆弧与所述起点位姿相切；根据所述目标位姿确定第二圆弧，所述第二圆弧与所述目标位姿相切；将所述第一圆弧朝着所述目标位姿的方向移动，得到第三圆弧，所述第三圆弧与所述第二圆弧相切。若所述第三圆弧与所述第二圆弧相切的切点在所述起点位姿与所述目标位姿之间，则确定第五规划路径，所述第五规划路径包括所述起点位姿与所述切点之间的路径，以及所述切点与所述目标位姿之间的路径。从而进行路径重新规划，提供了一种路径重新规划的实现方式，保证车辆行驶的准确性。

如图9a所示，为本发明实施例中路径规划系统的第一个实施例示意图，可以包括：

确定模块901，用于当满足预置条件时，确定车辆的起点位姿；确定目标位姿，所述目标位姿为待规划路径的终点位姿；

处理模块902，用于使用目标连接方法连接所述起点位姿和所述目标位姿，若成功则得到第一规划路径，其中，所述目标连接方法为直线连接方法和圆弧连接方法中的至少一种。

可选的，在本发明的一些实施例中，如图9b所示，为本发明实施例中路径规划系统的另一个实施例示意图，所述路径规划系统还包括：获取模块903；

获取模块903，用于获取所述车辆的已规划路径；获取所述车辆的第一当前位姿；

确定模块901，具体用于确定所述车辆的第一当前位姿与最近路径点的位姿的第一跟踪误差，所述已规划路径包括所述最近路径点的位姿；当所述第一跟踪误差大于第一预置阈值时，确定所述车辆的起点位姿为所述第一当前位姿；确定目标位姿为所述已规划路径的终点位姿。

可选的，在本发明的一些实施例中，如图9b所示，为本发明实施例中路径规划系统的第二个实施例示意图，所述路径规划系统还包括：获取模块903；

获取模块903，用于获取所述车辆的已规划路径；获取所述车辆的第一当前位姿；获取发生变化的终点位姿；

确定模块901，具体用于当所述发生变化的终点位姿与所述车辆的已规划路径的终点位姿的距离大于第二预置阈值时，确定所述车辆的起点位姿为所述第一当前位姿；确定目标位姿为所述发生变化的终点位姿。

可选的，在本发明的一些实施例中，如图9b所示，为本发明实施例中路径规划系统的另一个实施例示意图，所述路径规划系统还包括：获取模块903；

获取模块903，用于获取所述车辆的已规划路径；获取所述车辆的第一当前位姿；

确定模块901，具体用于当在所述已规划路径上检测到所述车辆的第一当前位姿与障碍物的距离小于第三预置阈值时，控制所述车辆朝远离所述已规划路径的终点位姿的方向行驶，获取所述车辆的第二当前位姿；确定所述车辆的起点位姿为所述第二当前位姿；确定目标位姿为所述已规划路径的终点位姿。

可选的，在本发明的一些实施例中，

处理模块902，还用于将所述起点位姿朝着所述目标位姿的方向以第一转弯半径进行移动，得到第三当前位姿，所述第一转弯半径大于等于所述车辆的最小转弯半径，所述第三当前位姿与所述目标位姿平行；计算所述第三当前位姿与所述目标位姿的第二跟踪误差；若所述第二跟踪误差小于第四预置阈值，则直线连接所述第三当前位姿和所述目标位姿，确定第二规划路径，所述第二规划路径包括所述起点位姿与所述第三当前位姿之间的路径，以及所述第三当前位姿与所述目标位姿之间的路径。

可选的，在本发明的一些实施例中，

处理模块902，还用于若所述第二跟踪误差大于等于所述第四预置阈值，则将所述起点位姿朝着所述目标位姿的方向以直线移动预置长度，得到第四当前位姿；使用所述目标连接方法连接所述第四当前位姿和所述目标位姿，若成功，则确定第三规划路径，所述第三规划路径包括所述起点位姿与所述第四当前位姿之间的路径，以及所述第四当前位姿与所述目标位姿之间的路径。

可选的，在本发明的一些实施例中，

处理模块902，还用于将所述起点位姿朝着所述目标位姿的方向以第二转弯半径进行移动，得到第五当前位姿，所述第二转弯半径大于等于所述车辆的最小转弯半径；使用所述目标连接方法连接所述第五当前位姿和所述目标位姿，若成功，则确定第四规划路径，所述第四规划路径包括所述起点位姿与第五当前位姿之间的路径，以及所述第五当前位姿与所述目标位姿之间的路径。

可选的，在本发明的一些实施例中，

处理模块902，还用于根据所述起点位姿确定第一圆弧，所述第一圆弧与所述起点位姿相切；根据所述目标位姿确定第二圆弧，所述第二圆弧与所述终点位姿相切；将所述第一圆弧朝着所述目标位姿的方向移动，得到第三圆弧，所述第三圆弧与所述第二圆弧相切；若所述第三圆弧与所述第二圆弧相切的切点在所述起点位姿与所述目标位姿之间，则确定第五规划路径，所述第五规划路径包括所述起点位姿与所述切点之间的路径，以及所述切点与所述目标位姿之间的路径。

可选的，本发明实施例中还提供一种车辆，该车辆包括如图9a或者图9b所示的路径跟踪系统。

如图10所示，为本发明实施例中路径规划系统的第三个实施例示意图。

可以包括：

存储有可执行程序代码的存储器1001；

与存储器1001耦合的处理器1002；

处理器1002调用存储器1001中存储的可执行程序代码，执行如图2-图8所示实施例中的步骤。

在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时，可以全部或部分地以计算机程序产品的形式实现。

所述计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行所述计算机程序指令时，全部或部分地产生按照本发明实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一计算机可读存储介质传输，例如，所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(dsl))或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。所述计算机可读存储介质可以是计算机能够存储的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质，(例如，软盘、硬盘、磁带)、光介质(例如，dvd)、或者半导体介质(例如固态硬盘solidstatedisk(ssd))等。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在本发明所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read-onlymemory)、随机存取存储器(ram，randomaccessmemory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。