一种车辆及其泊车路径规划方法、系统

1.本发明属于泊车路径规划技术领域，具体涉及一种车辆及其泊车路径规划方法、系统。


背景技术：

2.现在的车辆的软件及硬件水平快速发展，车载智能也发展迅速，对人们的出行方式也有着很大的改变。而现有的垂直车位路径规划通常是针对按照国家标准尺寸设置的车位设置的路径规划方法，能够在满足国家标准的车位实现自动泊车的过程。但是随着车辆越来越多，导致车位紧缺的情况出现，为了提高车位的利用率，现有的车位尺寸存在小于国家标准的情况，而车位以及停车走廊狭窄会造成泊车路径长，泊车曲线存在原地转向的问题。而现有的方法多采用几何法路径规划，几何法主要是指“圆弧-直线”路径规划方法，圆弧-直线规划方法在狭窄车位中泊车无法一次泊车须多步泊车，如图6及图7所示为几何法路径规划的过程示意图，图6中p0p1，p4p5为直线，p1p2，p2p3，p3p4为圆弧曲线，其中确定圆弧路径的具体过程为：
3.第一段圆弧路径p1p2：
4.车辆到达泊车初始位置，开始进行泊车，当车辆左侧车尾与车位左侧发生碰撞时完成第一段圆弧泊车，并给予一个0.2m的安全阈值，如图6第一段路径p1p2示意图，根据图7可得参数
5.当车辆从泊车初始位置倒车到达车辆左侧顶点与车位之间的安全阈值时(即车辆到达如图7中虚线示意的车辆位置时)，此时的左侧车位顶点坐标为d’(x
d’,y
d’)，根据d’横坐标值以及后轴中心坐标q1(x1,y1)即可得出第一段圆弧的角度，且此角度为：
[0006][0007]
第二段圆弧路径p2p3：
[0008]
由q2的坐标(x2,y2)及航向角就能求得圆心o2(x
o2
,y
o2
)的坐标，由第二段圆弧和第三段圆弧相切的几何关系，可以得到圆心o2和圆心o3的关系式：
[0009]
(x
o3-x
o2
)2+(y
o3-y
o2
)2＝(2r
min
)2；
[0010]
又由于圆弧p3p4与直线p4p5相切，则可求得o3的坐标(x
o3
,y
o3
)：
[0011]
由以上分析可以求得第二段圆弧路径p2p3对应的角度
[0012][0013][0014][0015]
由以上分析可得第三段圆弧p3p4路径对应的角度路径对应的角度进而实现基于几何法路径规划的过程，而几何法泊车路径规划在狭窄车位上应用的局限性主要包括以下
几点：车位尺寸限制：几何法泊车路径规划需要考虑车辆的尺寸和车位的尺寸，如果车位太小，车辆无法进入或者无法停放，就无法使用几何法泊车路径规划；车位位置限制：几何法泊车路径规划需要考虑车位的位置和周围环境，如果车位位置过于狭窄或者周围环境复杂，就无法使用几何法泊车路径规划；车辆类型限制：几何法泊车路径规划适用于小型车辆，对于大型车辆或者特殊车辆，如货车、客车等，就无法使用几何法泊车路径规划。因该方法存在上述局限性，在狭窄泊车环境中依旧会使得泊车曲线存在原地转向的问题，不能很好的应用在狭窄泊车环境中。因此亟需提出一种泊车路径规划方法来使得即使在狭窄泊车环境中，泊车曲线也不会存在原地转向的情况。


技术实现要素：

[0016]
本发明的目的在于提供一种车辆及其泊车路径规划方法、系统，用以解决现有基于几何法实现路径规划的方式会使得泊车曲线存在原地转向的问题。
[0017]
为解决上述技术问题，本发明提供了一种车辆泊车路径规划方法，包括如下步骤：
[0018]
1)根据车辆的泊车起点与泊车终点，生成泊车起点至泊车终点的泊车环境代价地图；
[0019]
2)在泊车环境代价地图中增加方向盘转动角度代价以及车辆航向角代价后，生成新的代价地图；
[0020]
3)在新的代价地图中运用rs曲线作为启发函数，生成从泊车起点至泊车终点的泊车路径，并判断此泊车路径是否与车辆所处环境中的障碍物存在碰撞点；
[0021]
4)若存在碰撞点，则运用rs曲线扩展搜索，至形成的泊车路径与车辆所处环境中的障碍物无碰撞点。
[0022]
其有益效果为：因车辆在狭窄车位和狭窄停车走廊泊车过程中需要多次转动方向盘，并且车辆的航向角在狭窄泊车环境中会多次变化，若仅按照原有的路径规划方式规划一条从起点到终点的路径仅是完成了理论上的泊车规划，而会存在因方向盘以及车辆航向角的不断改变导致泊车曲线原地转向的问题，因此本发明的方法通过在生成泊车环境代价地图后，通过增加方向盘转角代价以及车辆航向角代价生成新的代价地图的过程，来避免由代价地图生成的泊车曲线存在原地转向的问题。并且本发明的方法在新的代价地图中运用rs曲线作为启发函数来生成泊车路径，并在生成后对泊车路径进行判断，并基于rs曲线扩展搜索的方式使得最终生成的泊车路径无碰撞的情况存在，保证了车辆在泊车过程中的安全性。
[0023]
进一步地，步骤1)中，通过采用代价函数计算公式生成泊车起点至泊车终点的泊车环境代价地图。
[0024]
通过代价函数计算公式生成代价地图的方式，能够使得生成的代价地图每个栅格的代价值能被显示。
[0025]
进一步地，步骤2)中，通过将方向盘转动角度以及车辆航向角增加至代价函数中，生成新的代价函数，以此新的代价函数将泊车环境代价地图生成新的代价地图。
[0026]
进一步地，所述新的代价函数公式为：其中，dis表示当前节点扩展到下一节点的距离大小；为方向盘转角；系数k为方向盘与前轮转角的比例；δφ表示航向角变化；cost(δφ)表示从当前节点航向角扩展到下一节点航
向角的代价；方向盘转角的变化量。
[0027]
本发明的方法通过将方向盘转角以及车辆航向角增加至代价函数中，形成新的代价函数，使得通过该新的代价函数生成的新的代价地图中的每个栅格代价值均能被显示，并且本发明的方法考虑到不同类型的车辆方向盘与前轮转角的比例不同，因此在方向盘转角前用系数k做约束，及不同类型的车辆运用对应车辆的k值，以使本发明的方法能够适用于各种类型的车辆。
[0028]
进一步地，步骤3)中，通过rs曲线先进行节点搜索，获取新的代价地图中泊车起点与泊车终点之间的各个目标节点，连接泊车起点、目标节点以及泊车终点形成泊车路径。
[0029]
本发明的方法通过使用rs曲线进行节点搜索，以通过新的代价地图指引扩展节点的搜索方向，使其向代价值小的方向扩展，且此rs曲线搜索的方法将车辆的二自由度模型应用于扩展搜索中，来计算点到点间的距离，更加符合车辆运动特性。
[0030]
进一步地，步骤3)中，通过以车辆后轴中心为基准建立车辆坐标系，在此坐标系下计算rs曲线。
[0031]
本发明的方法通过以车辆后轴中心为基准建立车辆坐标系，使得车辆的泊车起点位置在坐标系中为原点位置，即(x，y)＝(0，0)，此设置相较于泊车起点为其他位置时，能够简便后续的计算过程。
[0032]
进一步地，步骤3)中，通过检测车辆在泊车路径中车身线与车辆所处环境中的障碍物线是否存在交点的方式，判断泊车路径是否与车辆所处环境中的障碍物存在碰撞点；所述车身线为：将车辆作为矩形后，所述矩形的各个边为所述车身线。
[0033]
在碰撞测试中，把车辆理想化为一个矩形，传统的栅格法碰撞检测中理想矩形化的车辆在栅格中占据的空间比较大，在狭窄空间内不容易实现泊车规划，因此，本发明使用车身线与障碍物线碰撞检测方法。当前泊车环境中，障碍物和车位都是固定的，此场景下的障碍物及车位都可以简化成障碍线。
[0034]
进一步地，所述rs曲线的计算公式为：
[0035][0036]
其中，t为时间，s为直线距离，l为左圆弧的圆弧长度，r为右圆弧的圆弧长度，φ为航向角。
[0037]
为解决上述技术问题，本发明还提供了一种车辆泊车路径规划系统，包括控制装置，所述控制装置包括存储器和处理器，处理器用于执行指令实现上述介绍的车辆泊车路径规划方法，并达到与该方法相同的有益效果。
[0038]
为解决上述技术问题，本发明还提供了一种车辆，包括车体，还包括存储器和处理器，处理器用于执行指令实现上述介绍的车辆泊车路径规划方法，并达到与该方法相同的有益效果。
附图说明
[0039]
图1是本发明的泊车路径规划方法的流程图；
[0040]
图2是reeds-shepp曲线搜索路径原理图；
[0041]
图3是车辆泊车位置及狭窄车位标注示意图；
[0042]
图4是本实施例中车身与障碍物碰撞检测中第一种碰撞情况示意图；
[0043]
图5是本实施例中车身与障碍物碰撞检测中第二种碰撞情况示意图；
[0044]
图6是现有的几何法路径规划过程的示意图；
[0045]
图7是现有的几何法路径规划确定第一段圆弧路径的示意图。
具体实施方式
[0046]
为了使本发明的目的、技术方案及优点更加清楚明了，以下结合附图及实施例，对本发明进行进一步详细说明。
[0047]
车辆泊车路径规划方法实施例：
[0048]
本实施例的方法通过增加方向盘旋转角度代价和航向角代价后，生成代价地图，来避免由代价地图生成的泊车曲线存在原地转向的问题，并且基于rs曲线由代价地图指引rs曲线扩展节点的搜索方向，使其向代价值小的方向扩展，以生成由泊车起点至泊车终点的泊车路径。
[0049]
本实施例的方法使得即使在狭窄车位及现在走廊中，也能够实现泊车的过程，狭窄车位分为一般狭窄与极窄程度，将车位宽度是车身宽度的130％-136％时，判断为一般狭窄，车位宽度是车身宽度的124％-130％时，判断为极窄程度，具体的方法流程如图1所示：
[0050]
1)根据泊车起点以及泊车终点，生成狭窄泊车环境代价地图；
[0051]
本实施例中根据本车位置点作为起点，泊车目标点位置作为终点，使用代价函数计算公式生成代价地图。
[0052]
2)增加方向盘转动角度代价以及航向角代价，生成新的代价地图；
[0053]
在狭窄环境内泊车需要考虑车位尺寸的狭窄还要考虑停车走廊的狭窄，方向盘转动是在此环境下泊车不可缺少的操作并且车辆的航向角在狭窄泊车环境中会多次变化，因此，增加方向盘转动角度代价和航向角代价的代价函数来对生成的狭窄泊车环境代价地图进行改进，以生成如图2所示的新的代价地图，其中代价函数为：其中，dis表示当前节点扩展到下一节点的距离大小；为方向盘转角；系数k为方向盘与前轮转角的比例，此系数会根据不同类型的汽车变化；δφ表示航向角变化；cost(δφ)表示从当前节点航向角扩展到下一节点航向角的代价；方向盘转角的变化量。其中前轮转角的最大值由车辆单车模型获取，车辆单车模型公式为：其中，θ为最大前轮转角，l为汽车轴距，r为转弯半径。
[0054]
3)在新的代价地图中，通过rs曲线搜索节点，生成由泊车起点至泊车终点的泊车路径；
[0055]
改进后的代价地图(即新的代价地图)用于hybrid a*中，指引扩展点的搜索方向，使其往代价值小的方向扩展，其中对hybrid a*算法的搜索扩展节点进行优化，提出在hybrid a*算法中使用reeds-shepp曲线朝着代价小的方向扩展搜索路径。每选定一个扩展点，就把这个选定的扩展点当作起点，目标点当作终点，利用reeds-shepp曲线搜索当前起点到终点的reeds-shepp路径。reeds-shepp曲线(即rs曲线)的应用为：
[0056]
(1)以车辆后轴中心为基准，建立车辆坐标系(如图3所示)，在此坐标系下计算reeds-shepp曲线；
[0057]
(2)把起点和终点的坐标从世界坐标转换为车辆坐标；
[0058]
(3)车辆的转弯半径，起点和终点的坐标进行归一化处理；
[0059]
(4)计算reeds-shepp曲线路径。
[0060]
其中，reeds-shepp曲线计算公式为：
[0061][0062]
其中，t为时间；s为直线距离；l，r表示圆弧长度；l表示左圆弧；r表示右圆弧；φ为航向角。其中若x方向速率的变化率大于y方向速率的变化率时为本实施例的左圆弧，若y方向速率的变化率大于x方向速率的变化率时为本实施例的右圆弧。
[0063]
其中，在使用reeds-shepp曲线前做归一处理，简化计算。其中归一化处理是指：将圆弧的半径定为单位1，假设车辆的初始姿态为q1＝(x1，y1，φ1)，目标姿态为qg＝(x2，y2，φ2)，车辆的最小转弯半径为r＝ρ。归一化的过程就是向量的平移和旋转的过程，使得变换后的起始姿态q1＝(0，0，0)，目标姿态为车辆的转弯半径r＝1。
[0064]
其中，
[0065][0066]
不同车辆有着不同的转弯半径，归一处理后可以让reeds-shepp曲线通用性更强，能够作用于不同车辆。
[0067]
4)检测泊车路径是否存在车辆与环境障碍物碰撞的情况，若存在碰撞的情况，则通过扩展搜索节点的方式，生成物碰撞情况存在的泊车路径。
[0068]
本实施例的碰撞检测是针对狭窄泊车环境的车身线与障碍线碰撞检测的方法，按照生成的路径，每隔一段距离就进行一次碰撞检测，计算车身的边框，检测车身四条边框线和障碍线有无交点来判断车辆与障碍物是否发生碰撞(即判断是否存在如图4或图5的情况发生)，由此来判断是否需要对泊车路径进行再规划。
[0069]
其中，计算车辆边框线，只需计算出车辆的四个顶点位置。在狭窄环境下泊车，车位尺寸狭窄，停车走廊狭窄，此环境下泊车面临着车辆与车位左侧停放车辆碰撞，与车位右侧停放车辆碰撞(如图4所示的情况)，与停车走廊顶部墙面碰撞或者顶部停放车辆碰撞(如图5所示的情况)，由此判断，在碰撞测试中设定每隔一个栅格进行一次障碍线碰撞检测。
[0070]
具体操作如下：使用reeds-shepp曲线寻找到狭窄车位泊车终点位置后即刻开始碰撞检测，离散reeds-shepp曲线，应用车辆坐标系计算每个点的下(x，y，φ)检测当前坐标连线是否与障碍线碰撞，即车身线是否与障碍线相交。
[0071][0072]
其中，tz为碰撞时刻，yz碰撞时刻的障碍线。
[0073]
如果规划的路径无发生碰撞则完成规划，如果发生了碰撞则对泊车路径再规划，使用reeds-shepp曲线扩展搜索节点，生成新的泊车路径再进行碰撞测试，直至最后无碰撞，生成最终路径，至此规划完成。
[0074]
本实施例的方法通过增加方向盘转角代价和航向角代价的代价函数来改进代价地图并提出使用reeds-shepp曲线作为启发函数，使得在狭窄的泊车环境下泊车路径更平滑，缩短泊车路径，减小路径搜索过程中的迭代次数并且提出使用车身与障碍物线与线碰撞检测方法，在狭窄的泊车环境中更易实现，提高规划效率。且本实施例的方法通过增加方向盘转角代价和航向角代价且使用rs曲线生成路径，来减少路径搜索的迭代次数，且提出的狭窄泊车环境下车身线与障碍物线碰撞检测方法，判断泊车路径是否需要再规划，来提高规划效率。
[0075]
车辆泊车路径规划系统实施例：
[0076]
本实施例中的车辆泊车路径规划系统，包括控制装置，该控制装置包括存储器和处理器，用于实现车辆泊车路径规划方法，该方法已经在车辆泊车路径规划方法实施例中详细介绍，此处不再赘述。
[0077]
车辆实施例：
[0078]
本实施例中的车辆包括车体，以及存储器和处理器，用于实现车辆泊车路径规划方法，该方法已经在车辆泊车路径规划方法实施例中详细介绍，此处不再赘述。
[0079]
以上所述，仅为本发明的较佳实施例，并不用以限制本发明，本发明的专利保护范围以权利要求书为准，凡是运用本发明的说明书及附图内容所作的等同结构变化，同理均应包含在本发明的保护范围内。

技术特征：
1.一种车辆泊车路径规划方法，其特征在于，包括如下步骤：1)根据车辆的泊车起点与泊车终点，生成泊车起点至泊车终点的泊车环境代价地图；2)在泊车环境代价地图中增加方向盘转动角度代价以及车辆航向角代价后，生成新的代价地图；3)在新的代价地图中运用rs曲线作为启发函数，生成从泊车起点至泊车终点的泊车路径，并判断此泊车路径是否与车辆所处环境中的障碍物存在碰撞点；4)若存在碰撞点，则运用rs曲线扩展搜索，至形成的泊车路径与车辆所处环境中的障碍物无碰撞点。2.根据权利要求1所述的车辆泊车路径规划方法，其特征在于，步骤1)中，通过采用代价函数计算公式生成泊车起点至泊车终点的泊车环境代价地图。3.根据权利要求1所述的车辆泊车路径规划方法，其特征在于，步骤2)中，通过将方向盘转动角度以及车辆航向角增加至代价函数中，生成新的代价函数，以此新的代价函数将泊车环境代价地图生成新的代价地图。4.根据权利要求3所述的车辆泊车路径规划方法，其特征在于，所述新的代价函数公式为：cost＝dis+dis[kθ+cost(δφ)+nδθ]；其中，dis表示当前节点扩展到下一节点的距离大小；θ为方向盘转角；系数k为方向盘与前轮转角的比例；δφ表示航向角变化；cost(δφ)表示从当前节点航向角扩展到下一节点航向角的代价；δθ方向盘转角的变化量。5.根据权利要求4所述的车辆泊车路径规划方法，其特征在于，步骤3)中，通过rs曲线先进行节点搜索，获取新的代价地图中泊车起点与泊车终点之间的各个目标节点，连接泊车起点、目标节点以及泊车终点形成泊车路径。6.根据权利要求5所述的车辆泊车路径规划方法，其特征在于，步骤3)中，通过以车辆后轴中心为基准建立车辆坐标系，在此坐标系下计算rs曲线。7.根据权利要求6所述的车辆泊车路径规划方法，其特征在于，步骤3)中，通过检测车辆在泊车路径中车身线与车辆所处环境中的障碍物线是否存在交点的方式，判断泊车路径是否与车辆所处环境中的障碍物存在碰撞点；所述车身线为：将车辆作为矩形后，所述矩形的各个边为所述车身线。8.根据权利要求7所述的车辆泊车路径规划方法，其特征在于，所述rs曲线的计算公式为：其中，t为时间，s为直线距离，l为左圆弧的圆弧长度，r为右圆弧的圆弧长度，φ为航向角。9.一种车辆泊车路径规划系统，包括控制装置，其特征在于，所述控制装置包括存储器和处理器，处理器用于执行指令实现如权利要求1～8任一项所述的车辆泊车路径规划方法。10.一种车辆，包括车体，其特征在于，还包括存储器和处理器，处理器用于执行指令实现如权利要求1～8任一项所述的车辆泊车路径规划方法。

技术总结
本发明属于泊车路径规划技术领域，具体涉及一种车辆及其泊车路径规划方法、系统，该方法通过在生成的泊车环境代价地图中增加方向盘转动角度代价以及车辆航向角代价后，生成新的代价地图，运用RS曲线作为启发函数，生成从泊车起点至泊车终点的泊车路径，判断此泊车路径是否与车辆所处环境中的障碍物存在碰撞点，在存在碰撞点的情况下，运用RS曲线扩展搜索，至形成的泊车路径与车辆所处环境中的障碍物无碰撞点。进而通过增加方向盘转角代价以及车辆航向角代价生成新的代价地图的过程，来避免由代价地图生成的泊车曲线存在原地转向的问题，并基于RS曲线扩展搜索方式使得最终生成的泊车路径无碰撞的情况存在，保证了车辆在泊车过程中的安全性。过程中的安全性。过程中的安全性。


技术研发人员：高建平 李炫 吴延峰 史振宁 姚晨豪
受保护的技术使用者：河南科技大学
技术研发日：2023.04.14
技术公布日：2023/6/28