CN114162113B - 一种结合多段式曲线的平行泊车路径规划方法 - Google Patents

一种结合多段式曲线的平行泊车路径规划方法 Download PDF

Info

Publication number
CN114162113B
CN114162113B CN202111623746.4A CN202111623746A CN114162113B CN 114162113 B CN114162113 B CN 114162113B CN 202111623746 A CN202111623746 A CN 202111623746A CN 114162113 B CN114162113 B CN 114162113B
Authority
CN
China
Prior art keywords
point
section
vehicle
path
parking space
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
Application number
CN202111623746.4A
Other languages
English (en)
Other versions
CN114162113A (zh
Inventor
田哲文
冯柯安
张平
汤云
潘威铮
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuhan University of Technology WUT
Original Assignee
Wuhan University of Technology WUT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Wuhan University of Technology WUT filed Critical Wuhan University of Technology WUT
Priority to CN202111623746.4A priority Critical patent/CN114162113B/zh
Publication of CN114162113A publication Critical patent/CN114162113A/zh
Application granted granted Critical
Publication of CN114162113B publication Critical patent/CN114162113B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
    • B60W30/06—Automatic manoeuvring for parking

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Traffic Control Systems (AREA)
  • Steering Control In Accordance With Driving Conditions (AREA)

Abstract

一种结合多段式曲线的平行泊车路径规划方法，先根据待泊车位、待泊车辆的信息建立坐标系，再依据该坐标系将泊车路径规划为依次连接的第一段路径、第二段路径、第三段路径，其中，第一段路径的起始点位于行车道内，第三段路径的起始点位于待泊车位内，而第二段路径为多段式曲线，其包括依次连接的AB、BC、CD、DD1、D1C1、C1B1、B1A1共七段，其中，A点位于行车道内，A1点位于待泊车位内，第一段路径引导车辆从起始位姿变为适合泊入的位姿，第二段路径引导车辆泊入车位，第三段路径引导车辆调整到车位中心。本设计不仅操作难度较小，流畅性较好，而且可扩大平行泊车初始位姿允许范围，降低平行泊车对驾驶员的操作门槛。

Description

一种结合多段式曲线的平行泊车路径规划方法
技术领域
本发明涉及一种泊车路径规划方法，属于无人车路径规划领域，尤其涉及一种结合多段式曲线的平行泊车路径规划方法。
背景技术
随着各国汽车保有量持续增加，以及城市泊车管理的规范化，无论是从操作难易程度，还是从泊车的安全方面来说，驾驶员，尤其是缺乏经验的新手驾驶员对自动泊车系统的需求越来越高。
目前，虽然国内外学者针对平行泊车问题提出了一些路径规划方法，但规划的效果都存在一些缺点，如未考虑初始位姿状态对泊车的影响，需要驾驶员将车辆停到特定的初始位姿才能进行后续的泊车操作，提升了泊车时驾驶员的操作难度，此外，大部分现有的路径规划方法还忽视了车辆泊入的流畅性，降低了停泊的效率，有损乘员的舒适性。
公开该背景技术部分的信息仅仅旨在增加对本专利申请的总体背景的理解，而不应当被视为承认或以任何形式暗示该信息构成已为本领域一般技术人员所公知的现有技术。
发明内容
本发明的目的是克服现有技术中存在的操作难度较大、流畅性较差的缺陷与问题，提供一种操作难度较小、流畅性较好的结合多段式曲线的平行泊车路径规划方法。
为实现以上目的，本发明的技术解决方案是：一种结合多段式曲线的平行泊车路径规划方法，包括以下步骤：
先以待泊车位的右后角点为坐标系原点O、以待泊车位的长度方向为X轴方向、以待泊车位的宽度方向为Y轴方向建立坐标系，其中，待泊车位的长度、宽度分别为Sl、Sw，与待泊车位相平行的行车道的宽度为Dl，且以车辆后轴中心代表待泊车辆的位置；
再规划所述平行泊车路径包括依次连接的第一段路径、第二段路径、第三段路径，所述第一段路径的起始点位于行车道内，所述第三段路径的起始点位于待泊车位内，所述第二段路径为多段式曲线，其包括依次连接的AB、BC、CD、DD1、D1C1、C1B1、B1A1共七段，其中，A点位于行车道内，A1点位于待泊车位内；所述AB、CD、D1C1、B1A1为过渡段，所述BC、C1B1为圆弧段，所述DD1为直线段，且第二段路径以DD1段的中点为旋转中心，呈中心对称；所述待泊车辆在第二段路径中的限定如下：
在A点时，车辆航向角与车位平行，方向盘转角为0；在AB段期间，车辆匀速后退，方向盘转角由0匀速变为右端最大值；在B点时，车辆航向角偏正，方向盘转角处于右端最大值；
在BC段期间，车辆匀速后退，方向盘转角保持右端最大值，并避免车身左前角点与行车道左侧边界相撞，即留有安全距离DS1，以及车身右侧与车位左前角点相撞，即留有安全距离DS2；
在CD段期间，车辆匀速后退，方向盘转角由右端最大值匀速回正到0；在D点时，车辆航向角达到整个多段式曲线的最大值，方向盘转角为0；
在DD1段期间，车辆匀速后退，方向盘转角保持为0，车辆航向角保持最大值，车辆做直线后退运动；在D1点时，车辆航向角仍为最大值，方向盘转角为0；
在D1C1段期间，车辆匀速后退，方向盘转角由0匀速变为左端最大值；在C1点时，方向盘转角达到左端最大值，车辆航向角比在D1点时小；
在C1B1段期间，车辆匀速后退，方向盘转角保持左端最大值，并避免车身右前角点与车位左前角点相撞，即留有安全距离DS3，以及车身右后角点与车位右侧边界相撞，即留有安全距离DS4；在B1点时，车辆航向角近水平方向，车辆转角仍处于左端最大值；
在B1A1段期间，车辆匀速后退，方向盘转角由左端最大值匀速回正到0；在A1点时，车辆航向角与车位平行，方向盘转角为0，多段式曲线结束。
所述A点位于待泊车位的右上方；所述A1点近待泊车位的后侧边界设置，且当待泊车辆位于A1点时，车辆后方与待泊车位的后侧边界之间沿X轴方向限定有安全距离PSx，同时，车辆左侧与待泊车位的左侧边界之间沿Y轴方向限定有安全距离PSy。
所述多段式曲线中各段的参数限定如下：
AB段：其中，θA为A点的航向角，在此按期望值0来计算；θB为B点的航向角；X1、Y1分别为AB段在X轴方向、Y轴方向上的位移；n1为AB段的路径点数目；δi为运动每一步长前的车辆前轮转角，D为路径点分辨率，L为车辆轴距；
BC段：其中，θC为C点的航向角；X2、Y2分别为BC段在X轴方向、Y轴方向上的位移；n2为BC段的路径点数目；Rm为车辆的最小转弯半径；
CD段：其中，θc为C点的航向角，θD为D点的航向角，X3、Y3分别为CD段在X轴方向、Y轴方向上的位移，n3为CD段的路径点数目，由方向盘的对称性可知n3＝n1；
DD1段：其中，X4、Y4分别为DD1段在X轴方向、Y轴方向上的位移，n4为DD1段的路径点数目；
D1C1段：与CD段中心对称；
C1B1段：与BC段中心对称；
B1A1段：与AB段中心对称。
先在所述坐标系中定义以下参数：Sl、Sw分别为待泊车位的长、宽，S1是车辆后轴中心到待泊车位的前侧边界的距离，S2是车辆后轴中心到待泊车位的左侧边界的距离，Lr为车辆后悬距离，Lf为车辆前悬距离，W为车宽，L为车辆轴距，O1为圆弧段BC的圆心，O2为圆弧段C1B1的圆心；
R0为A点到圆心O1之间的距离，计算公式如下：
R1为在圆弧段BC时，车身左前角点所经过的圆弧轨迹半径，计算公式如下：
R2为在圆弧段C1B1时，车身右后角点所经过的圆弧轨迹的半径，计算公式如下：
再定义圆心O1处的垂直线与AO1之间的夹角为α，圆心O1处的垂直线与BO1之间的夹角为θB，则：
则圆心O1的坐标表示为O1(Sl+S1-R0sinα，Sw+S2-R0cosαα)，圆心O2的坐标表示为O2(A1x+R0sinα，A1y+R0cosα)，其中，A1x、A1y分别定义为A1点的横坐标、纵坐标；
最后，得到DS1、DS2、DS3、DS4的限定分别如下：
(Sw+S2-R0cosα)+R1+DS1≤Sw+Dl
R2+DS4≤O2ymin≤O2y
所述多段式曲线中，A点所在的区域为起始区域，A1点所在的区域为终点区域，所述起始区域由BC段决定的左边界、DS1决定的上边界、DS2决定的下边界共同围成，所述终点区域由待泊车位的后侧边界、左侧边界对应决定的左边界、上边界，以及DS3决定的右边界共同围成。
当从起始区域中选定一点为A点，从终点区域中选定一点为A1点时，A点、A1点的坐标已知，多段式曲线起点、终点的X坐标之差、Y坐标之差即为已知，再将多段式曲线中每一段在X轴方向、Y轴方向上的位移分别相加，以分别得到X坐标之差、Y坐标之差的两个方程，然后联立方程求解以得到n2、n4的值，再将n2、n4代入各段的参数限定中，以求得每段的长度、车辆航向角，从而确定多段式曲线。
先从起始区域中选定一点为A点，即为第一段路径的目标终点，再采用混合A*算法规划第一段路径，具体步骤为：
先利用多种RS曲线去依次计算判断能否找到一种连接起点到目标终点的RS曲线，再将找到的曲线进行长度比较，选取代价最小的一种作为最终找到的RS曲线，然后将找到的RS曲线进行还原，并生成路径点，再判断路径点与障碍物是否有重合，若无碰撞则保留，结束寻找，视为成功找到第一段路径，若发生碰撞则舍弃，舍弃之后，先对目标终点的横、纵坐标进行一定量的偏移以依次得到其周围的8个次目标点，再重复上述寻找RS曲线的步骤，依次计算能否通过RS曲线连接起点和任意一个次目标点，若能找到一条无碰撞的RS曲线连接到次目标点，即视为成功找到第一段路径，跳出循环，若未找到，则以当前节点为根节点，按照给定步长、给定离散前轮转角依次去拓展子节点，再重复上述寻找RS曲线的步骤，依次计算能否通过RS曲线连接新的根节点和任意一个目标终点，有则罢，无则试次目标点，若仍无，则再以当前节点为根节点，按照给定步长、给定离散前轮转角依次去拓展新的子节点，继续重复上述操作，直至找到一条无碰撞的RS曲线，即视为成功找到第一段路径。
在选定一点为A点之后，先依据下述公式计算泊入车位所需的最小车位长度Slcal和最小车位宽度Swcal，判断当前的待泊车位能否进行泊车操作，若能，则再采用混合A*算法规划第一段路径，若不能，则重新寻找车位；
先从终点区域中选定一点为A1点，即为第三段路径的起始点，再采用混合A*算法规划第三段路径，以使待泊车辆泊入待泊车位的中心，且与车位平行。
与现有技术相比，本发明的有益效果为：
1、本发明一种结合多段式曲线的平行泊车路径规划方法中，主要包括依次连接的第一段路径、第二段路径、第三段路径，其中，第一段路径的起始点位于行车道内，第三段路径的起始点位于待泊车位内，第二段路径为多段式曲线，其包括过渡段、圆弧段与直线段，应用时，待泊车辆先沿第一段路径自动运行至A点，再沿第二段路径自动运行至A1点，然后沿第三段路径自动运行至待泊车位的中心，明显可见，对待泊车辆的初始位姿没有要求，降低了一般平行泊车对驾驶员的操作门槛，提高了平行泊车系统的易用性和驾驶员使用平行泊车系统的概率。因此，本发明的操作难度较小，应用范围较广。
2、本发明一种结合多段式曲线的平行泊车路径规划方法中，第二段路径为多段式曲线，其包括依次连接的AB、BC、CD、DD1、D1C1、C1B1、B1A1共七段，其中，A点位于行车道内，A1点位于待泊车位内；所述AB、CD、D1C1、B1A1为过渡段，所述BC、C1B1为圆弧段，所述DD1为直线段，第二段路径以DD1段的中点为旋转中心，呈中心对称，除此之外，对待泊车辆在每点、每段内的车辆航向角、方向盘转角都进行了限定，该种设计不仅在泊入阶段考虑到了方向盘转角在运动过程中连续变化的情况，保证了车辆泊入的流畅性，提高了乘员的舒适性，而且尽量以最小转弯半径行驶，尽可能节省泊车所需空间，此外，沿Y轴方向还具备较大的可调整范围，利于实现更准确的停车。因此，本发明不仅流畅性较好，而且停车效果较佳。
3、本发明一种结合多段式曲线的平行泊车路径规划方法中，第二段路径为多段式曲线，包括四个过渡段，一个直线段与两个圆弧段，其中，方向盘转角变化时的四个过渡段由车辆信息决定，而两段圆弧段和直线段的路径则由多段式曲线的起点和终点共同决定。因此，该多段式曲线本身具有一定的调节车辆位置误差的作用，即通过改变不同的直线段和圆弧段的长度，可以完成一定范围内的不同起点到同一终点的路径规划，增强可调性，以适用于更多的应用场景。因此，本发明的可调性较强，应用范围较广。
附图说明
图1是本发明中建立的坐标系示意图。
图2是本发明中多段式曲线的示意图。
图3是本发明中DS1的示意图。
图4是本发明中DS2的示意图。
图5是本发明中DS3的示意图。
图6是本发明中DS4的示意图。
图7是本发明中多段式曲线的起始区域、终点区域的示意图。
图8是本发明中AB段的局部放大示意图。
图9是本发明中混合A*算法路径规划示意图。
图10是本发明中第三段路径运行前后的对比示意图。
图11是本发明仿真全过程车辆轮廓扫过区域的示意图。
具体实施方式
以下结合附图说明和具体实施方式对本发明作进一步详细的说明。
参见图1—图10，一种结合多段式曲线的平行泊车路径规划方法，包括以下步骤：
先以待泊车位的右后角点为坐标系原点O、以待泊车位的长度方向为X轴方向、以待泊车位的宽度方向为Y轴方向建立坐标系，其中，待泊车位的长度、宽度分别为Sl、Sw，与待泊车位相平行的行车道的宽度为Dl，且以车辆后轴中心代表待泊车辆的位置；
再规划所述平行泊车路径包括依次连接的第一段路径、第二段路径、第三段路径，所述第一段路径的起始点位于行车道内，所述第三段路径的起始点位于待泊车位内，所述第二段路径为多段式曲线，其包括依次连接的AB、BC、CD、DD1、D1C1、C1B1、B1A1共七段，其中，A点位于行车道内，A1点位于待泊车位内；所述AB、CD、D1C1、B1A1为过渡段，所述BC、C1B1为圆弧段，所述DD1为直线段，且第二段路径以DD1段的中点为旋转中心，呈中心对称；所述待泊车辆在第二段路径中的限定如下：
在A点时，车辆航向角与车位平行，方向盘转角为0；在AB段期间，车辆匀速后退，方向盘转角由0匀速变为右端最大值；在B点时，车辆航向角偏正，方向盘转角处于右端最大值；
在BC段期间，车辆匀速后退，方向盘转角保持右端最大值，并避免车身左前角点与行车道左侧边界相撞，即留有安全距离DS1，以及车身右侧与车位左前角点相撞，即留有安全距离DS2；
在CD段期间，车辆匀速后退，方向盘转角由右端最大值匀速回正到0；在D点时，车辆航向角达到整个多段式曲线的最大值，方向盘转角为0；
在DD1段期间，车辆匀速后退，方向盘转角保持为0，车辆航向角保持最大值，车辆做直线后退运动；在D1点时，车辆航向角仍为最大值，方向盘转角为0；
在D1C1段期间，车辆匀速后退，方向盘转角由0匀速变为左端最大值；在C1点时，方向盘转角达到左端最大值，车辆航向角比在D1点时小；
在C1B1段期间，车辆匀速后退，方向盘转角保持左端最大值，并避免车身右前角点与车位左前角点相撞，即留有安全距离DS3，以及车身右后角点与车位右侧边界相撞，即留有安全距离DS4；在B1点时，车辆航向角近水平方向，车辆转角仍处于左端最大值；
在B1A1段期间，车辆匀速后退，方向盘转角由左端最大值匀速回正到0；在A1点时，车辆航向角与车位平行，方向盘转角为0，多段式曲线结束。
所述A点位于待泊车位的右上方；所述A1点近待泊车位的后侧边界设置，且当待泊车辆位于A1点时，车辆后方与待泊车位的后侧边界之间沿X轴方向限定有安全距离PSx，同时，车辆左侧与待泊车位的左侧边界之间沿Y轴方向限定有安全距离PSy。
所述多段式曲线中各段的参数限定如下：
AB段：其中，θA为A点的航向角，在此按期望值0来计算；θB为B点的航向角；X1、Y1分别为AB段在X轴方向、Y轴方向上的位移；n1为AB段的路径点数目；δi为运动每一步长前的车辆前轮转角，D为路径点分辨率，L为车辆轴距；
BC段：其中，θC为C点的航向角；X2、Y2分别为BC段在X轴方向、Y轴方向上的位移；n2为BC段的路径点数目；Rm为车辆的最小转弯半径；
CD段：其中，θC为C点的航向角，θD为D点的航向角，X3、Y3分别为CD段在X轴方向、Y轴方向上的位移，n3为CD段的路径点数目，由方向盘的对称性可知n3＝n1；
DD1段：其中，X4、Y4分别为DD1段在X轴方向、Y轴方向上的位移，n4为DD1段的路径点数目；
D1C1段：与CD段中心对称；
C1B1段：与BC段中心对称；
B1A1段：与AB段中心对称。
先在所述坐标系中定义以下参数：Sl、SW分别为待泊车位的长、宽，S1是车辆后轴中心到待泊车位的前侧边界的距离，S2是车辆后轴中心到待泊车位的左侧边界的距离，Lr为车辆后悬距离，Lf为车辆前悬距离，W为车宽，L为车辆轴距，O1为圆弧段BC的圆心，O2为圆弧段C1B1的圆心；
R0为A点到圆心O1之间的距离，计算公式如下：
R1为在圆弧段BC时，车身左前角点所经过的圆弧轨迹半径，计算公式如下：
R2为在圆弧段C1B1时，车身右后角点所经过的圆弧轨迹的半径，计算公式如下：
再定义圆心O1处的垂直线与AO1之间的夹角为α，圆心O1处的垂直线与BO1之间的夹角为θB，则：
则圆心O1的坐标表示为O1(Sl+S1-R0sinα，Sw+S2-R0cosα)，圆心O2的坐标表示为O2(A1x+R0sinα，A1y+R0cosα)，其中，A1x、A1y分别定义为A1点的横坐标、纵坐标；
最后，得到DS1、DS2、DS3、DS4的限定分别如下：
(Sw+S2-R0cosα)+R1+DS1≤Sw+Dl
R2+DS4≤O2ymin≤O2y。
所述多段式曲线中，A点所在的区域为起始区域，A1点所在的区域为终点区域，所述起始区域由BC段决定的左边界、DS1决定的上边界、DS2决定的下边界共同围成，所述终点区域由待泊车位的后侧边界、左侧边界对应决定的左边界、上边界，以及DS3决定的右边界共同围成。
当从起始区域中选定一点为A点，从终点区域中选定一点为A1点时，A点、A1点的坐标已知，多段式曲线起点、终点的X坐标之差、Y坐标之差即为已知，再将多段式曲线中每一段在X轴方向、Y轴方向上的位移分别相加，以分别得到X坐标之差、Y坐标之差的两个方程，然后联立方程求解以得到n2、n4的值，再将n2、n4代入各段的参数限定中，以求得每段的长度、车辆航向角，从而确定多段式曲线。
先从起始区域中选定一点为A点，即为第一段路径的目标终点，再采用混合A*算法规划第一段路径，具体步骤为：
先利用多种RS曲线去依次计算判断能否找到一种连接起点到目标终点的RS曲线，再将找到的曲线进行长度比较，选取代价最小的一种作为最终找到的RS曲线，然后将找到的RS曲线进行还原，并生成路径点，再判断路径点与障碍物是否有重合，若无碰撞则保留，结束寻找，视为成功找到第一段路径，若发生碰撞则舍弃，舍弃之后，先对目标终点的横、纵坐标进行一定量的偏移以依次得到其周围的8个次目标点，再重复上述寻找RS曲线的步骤，依次计算能否通过RS曲线连接起点和任意一个次目标点，若能找到一条无碰撞的RS曲线连接到次目标点，即视为成功找到第一段路径，跳出循环，若未找到，则以当前节点为根节点，按照给定步长、给定离散前轮转角依次去拓展子节点，再重复上述寻找RS曲线的步骤(拓展时，每次都是先拓展多个子节点，再在所有拓展的子节点中计算比较代价值，然后选择其中代价值最小的一个作为新的根节点，再试着用RS曲线连接这个新的根节点和目标点，后面再重复)，依次计算能否通过RS曲线连接新的根节点和任意一个目标终点，有则罢，无则试次目标点，若仍无，则再以当前节点为根节点，按照给定步长、给定离散前轮转角依次去拓展新的子节点，继续重复上述操作，直至找到一条无碰撞的RS曲线，即视为成功找到第一段路径。
在选定一点为A点之后，先依据下述公式计算泊入车位所需的最小车位长度Slcal和最小车位宽度Swcal，判断当前的待泊车位能否进行泊车操作，若能，则再采用混合A*算法规划第一段路径，若不能，则重新寻找车位；
先从终点区域中选定一点为A1点，即为第三段路径的起始点，再采用混合A*算法规划第三段路径，以使待泊车辆泊入待泊车位的中心，且与车位平行。
本发明的原理说明如下：
本发明中的RS曲线是指Reeds-Shepp曲线，一般包括9类共48种RS曲线。
本发明中在拓展单个子节点时，计算从根节点到该子节点的路径点，并判断是否发生碰撞，若发生碰撞则舍弃该子节点，若无碰撞则保留，并计算其代价值，其代价值的计算公式如下：
Gchild＝Gparent+ωtGres×1.5+kctωct+ωs|δchild|+ωcs(δchild-δparent)2，
其中，Gchild、Gparent分别为子节点和父节点的实际代价值；ωt为运动方向的代价权重系数，对于前进和后退可设置不同系数；Gres为栅格地图的分辨率，扩大1.5倍以保证每一步长都能进入新的栅格；kct为运动方向改变的系数，方向不变时为0，方向改变为1；ωct为运动方向改变时的代价权重系数；ωs为前轮转角代价权重系数；ωcs为前轮转角改变值的代价权重系数；δchild、δparent分别为拓展子节点和父节点的前轮转角值。
此外，对于新拓展的子节点，还需计算启发函数代价，即剩余预计的代价值，其包含两部分：从当前节点到栅格中心的代价，以及从当前节点所在栅格到目标点的代价，如下：
Hchild＝ωHcostmap+ωHΔS
其中，Hchild为启发代价值，即到达目标点还需的预估代价；ωH为栅格地图上单位长度的代价；costmap为混合A*算法应用时所建立的栅格地图中单个栅格中心到目标点栅格中心的代价值，此处由子节点所在栅格的检索值查阅栅格地图数组得到；ΔS为当前子节点到所在栅格中心的距离。将上述的实际的代价值Gchild和预计的代价值Hchild相加得到总的代价值。
实施例1：
一种结合多段式曲线的平行泊车路径规划方法，其特征在于：所述平行泊车路径规划方法包括以下步骤：
步骤1：初始化环境信息和车辆信息，环境信息主要包含目标车位信息(车位的长宽)以及障碍物/可通行区域信息，环境信息可以通过车载传感器，如超声波雷达、车载摄像头、激光雷达等进行采集，也可通过外部具有V2X技术的设施传入部分先验环境信息。车辆信息主要是车辆尺寸参数(定值)以及自车位姿信息等，主要通过自车传感器获取。在获得这些信息后，以待泊车位的右后角点为坐标系原点O、以待泊车位的长度方向为X轴方向、以待泊车位的宽度方向为Y轴方向建立坐标系，其中，待泊车位的长度、宽度分别为Sl、Sw，与待泊车位相平行的行车道的宽度为Dl，且以车辆后轴中心代表待泊车辆的位置；
步骤2：规划所述平行泊车路径包括依次连接的第一段路径、第二段路径、第三段路径，所述第一段路径的起始点位于行车道内，所述第三段路径的起始点位于待泊车位内，所述第二段路径为多段式曲线，其包括依次连接的AB、BC、CD、DD1、D1C1、C1B1、B1A1共七段，其中，A点位于行车道内，A1点位于待泊车位内；所述AB、CD、D1C1、B1A1为过渡段，所述BC、C1B1为圆弧段，所述DD1为直线段，第二段路径以DD1段的中点为旋转中心，呈中心对称，同时，对车辆在多段式曲线中每点、每段上的车辆航向角、方向盘转角都进行限定；
步骤3：先设定四个约束，即DS1、DS2、DS3、DS4的计算公式，再依据这四个约束去计算A点所在的区域即起始区域，以及A1点所在的区域即终点区域，然后对多段式曲线中的七段分别进行参数限定，包括每段在X轴方向、Y轴方向上的位移、车辆航向角、每段的路径点数目，再将多段式曲线中每一段在X轴方向、Y轴方向上的位移分别相加，以分别得到X坐标之差、Y坐标之差的两个方程，从而构成联立方程；
步骤4：先从起始区域中选定一点为A点，从终点区域中选定一点为A1点，再将A点、A1点的坐标值代入上述联立方程，计算求解以得到n2、n4的值，然后将n2、n4代入各段的参数限定中，以求得每段的长度、车辆航向角，从而确定多段式曲线，即第二段路径；
步骤5：以A点为第一段路径的目标终点，采用混合A*算法规划第一段路径；
步骤6：以A1点为第三段路径的目标终点，采用混合A*算法规划第三段路径；
步骤7：将三段路径组合起来，即为从初始位姿到最终理想泊车位姿的全过程泊车路径，泊车路径规划结束。
以上所述仅为本发明的较佳实施方式，本发明的保护范围并不以上述实施方式为限，但凡本领域普通技术人员根据本发明所揭示内容所作的等效修饰或变化，皆应纳入权利要求书中记载的保护范围内。

Claims (8)

1.一种结合多段式曲线的平行泊车路径规划方法，其特征在于：所述平行泊车路径规划方法包括以下步骤：
先以待泊车位的右后角点为坐标系原点O、以待泊车位的长度方向为X轴方向、以待泊车位的宽度方向为Y轴方向建立坐标系，其中，待泊车位的长度、宽度分别为Sl、Sw，与待泊车位相平行的行车道的宽度为Dl，且以车辆后轴中心代表待泊车辆的位置；
再规划所述平行泊车路径包括依次连接的第一段路径、第二段路径、第三段路径，所述第一段路径的起始点位于行车道内，所述第三段路径的起始点位于待泊车位内，所述第二段路径为多段式曲线，其包括依次连接的AB、BC、CD、DD1、D1C1、C1B1、B1A1共七段，其中，A点位于行车道内，A1点位于待泊车位内；所述AB、CD、D1C1、B1A1为过渡段，所述BC、C1B1为圆弧段，所述DD1为直线段，且第二段路径以DD1段的中点为旋转中心，呈中心对称；所述待泊车辆在第二段路径中的限定如下：
在A点时，车辆航向角与车位平行，方向盘转角为0；在AB段期间，车辆匀速后退，方向盘转角由0匀速变为右端最大值；在B点时，车辆航向角偏正，方向盘转角处于右端最大值；
在BC段期间，车辆匀速后退，方向盘转角保持右端最大值，并避免车身左前角点与行车道左侧边界相撞，即留有安全距离DS1，以及避免车身右侧与车位左前角点相撞，即留有安全距离DS2；
在CD段期间，车辆匀速后退，方向盘转角由右端最大值匀速回正到0；在D点时，车辆航向角达到整个多段式曲线的最大值，方向盘转角为0；
在DD1段期间，车辆匀速后退，方向盘转角保持为0，车辆航向角保持最大值，车辆做直线后退运动；在D1点时，车辆航向角仍为最大值，方向盘转角为0；
在D1C1段期间，车辆匀速后退，方向盘转角由0匀速变为左端最大值；在C1点时，方向盘转角达到左端最大值，车辆航向角比在D1点时小；
在C1B1段期间，车辆匀速后退，方向盘转角保持左端最大值，并避免车身右前角点与车位左前角点相撞，即留有安全距离DS3，以及避免车身右后角点与车位右侧边界相撞，即留有安全距离DS4；在B1点时，车辆航向角近水平方向，车辆转角仍处于左端最大值；
在B1A1段期间，车辆匀速后退，方向盘转角由左端最大值匀速回正到0；在A1点时，车辆航向角与车位平行，方向盘转角为0，多段式曲线结束；
所述多段式曲线中各段的参数限定如下：
AB段：
其中，θA为A点的航向角，在此按期望值0来计算；θB为B点的航向角；X1、Y1分别为AB段在X轴方向、Y轴方向上的位移；n1为AB段的路径点数目；δi为运动每一步长前的车辆前轮转角，D为路径点分辨率，L为车辆轴距；
BC段：
其中，θC为C点的航向角；X2、Y2分别为BC段在X轴方向、Y轴方向上的位移；n2为BC段的路径点数目；Rm为车辆的最小转弯半径；
CD段：
其中，θC为C点的航向角，θD为D点的航向角，X3、Y3分别为CD段在X轴方向、Y轴方向上的位移，n3为CD段的路径点数目，由方向盘的对称性可知n3＝n1；
DD1段：
其中，X4、Y4分别为DD1段在X轴方向、Y轴方向上的位移，n4为DD1段的路径点数目；
D1C1段：与CD段中心对称；
C1B1段：与BC段中心对称；
B1A1段：与AB段中心对称。
2.根据权利要求1所述的一种结合多段式曲线的平行泊车路径规划方法，其特征在于：所述A点位于待泊车位的右上方；所述A1点近待泊车位的后侧边界设置，且当待泊车辆位于A1点时，车辆后方与待泊车位的后侧边界之间沿X轴方向限定有安全距离Psx，同时，车辆左侧与待泊车位的左侧边界之间沿Y轴方向限定有安全距离Psy。
3.根据权利要求2所述的一种结合多段式曲线的平行泊车路径规划方法，其特征在于：
先在所述坐标系中定义以下参数：Sl、Sw分别为待泊车位的长、宽，S1是车辆后轴中心到待泊车位的前侧边界的距离，S2是车辆后轴中心到待泊车位的左侧边界的距离，Lr为车辆后悬距离，Lf为车辆前悬距离，W为车宽，L为车辆轴距，O1为圆弧段BC的圆心，O2为圆弧段C1B1的圆心；
R0为A点到圆心O1之间的距离，计算公式如下：
R1为在圆弧段BC时，车身左前角点所经过的圆弧轨迹半径，计算公式如下：
R2为在圆弧段C1B1时，车身右后角点所经过的圆弧轨迹的半径，计算公式如下：
再定义圆心O1处的垂直线与AO1之间的夹角为α，圆心O1处的垂直线与BO1之间的夹角为θB，则：
则圆心O1的坐标表示为O1(Sl+S1-R0sinα，Sw+S2-R0cosα)，O1x、O1y分别为圆心O1的横坐标、纵坐标，圆心O2的坐标表示为O2(Alx+R0sinα，A1y+R0cosα)，O2x、O2y分别为圆心O2的横坐标、纵坐标，其中，A1x、A1y分别定义为A1点的横坐标、纵坐标；
最后，得到DS1、DS2、DS3、DS4的限定分别如下：
(Sw+S2-R0cosα)+R1+DS1≤Sw+Dl
R2+Ds4≤O2ymin≤O2y
其中，O2ymin为O2y的最小值。
4.根据权利要求3所述的一种结合多段式曲线的平行泊车路径规划方法，其特征在于：所述多段式曲线中，A点所在的区域为起始区域，A1点所在的区域为终点区域，所述起始区域由BC段决定的左边界、DS1决定的上边界、DS2决定的下边界共同围成，所述终点区域由待泊车位的后侧边界、左侧边界对应决定的左边界、上边界，以及DS3决定的右边界共同围成。
5.根据权利要求4所述的一种结合多段式曲线的平行泊车路径规划方法，其特征在于：当从起始区域中选定一点为A点，从终点区域中选定一点为A1点时，A点、A1点的坐标已知，多段式曲线起点、终点的X坐标之差、Y坐标之差即为已知，再将多段式曲线中每一段在X轴方向、Y轴方向上的位移分别相加，以分别得到X坐标之差、Y坐标之差的两个方程，然后联立方程求解以得到n2、n4的值，再将n2、n4代入各段的参数限定中，以求得每段的长度、车辆航向角，从而确定多段式曲线。
6.根据权利要求4所述的一种结合多段式曲线的平行泊车路径规划方法，其特征在于：先从起始区域中选定一点为A点，即为第一段路径的目标终点，再采用混合A*算法规划第一段路径，具体步骤为：
先利用多种RS曲线去依次计算判断能否找到一种连接起点到目标终点的RS曲线，再将找到的曲线进行长度比较，选取代价最小的一种作为最终找到的RS曲线，然后将找到的RS曲线进行还原，并生成路径点，再判断路径点与障碍物是否有重合，若无碰撞则保留，结束寻找，视为成功找到第一段路径，若发生碰撞则舍弃，舍弃之后，先对目标终点的横、纵坐标进行一定量的偏移以依次得到其周围的8个次目标点，再重复上述寻找RS曲线的步骤，依次计算能否通过RS曲线连接起点和任意一个次目标点，若能找到一条无碰撞的RS曲线连接到次目标点，即视为成功找到第一段路径，跳出循环，若未找到，则以当前节点为根节点，按照给定步长、给定离散前轮转角依次去拓展子节点，再重复上述寻找RS曲线的步骤，依次计算能否通过RS曲线连接新的根节点和任意一个目标终点，有则罢，无则试次目标点，若仍无，则再以当前节点为根节点，按照给定步长、给定离散前轮转角依次去拓展新的子节点，继续重复上述操作，直至找到一条无碰撞的RS曲线，即视为成功找到第一段路径。
7.根据权利要求6所述的一种结合多段式曲线的平行泊车路径规划方法，其特征在于：在选定一点为A点之后，先依据下述公式计算泊入车位所需的最小车位长度Slcal和最小车位宽度Swcal，判断当前的待泊车位能否进行泊车操作，若能，则再采用混合A*算法规划第一段路径，若不能，则重新寻找车位；
8.根据权利要求4所述的一种结合多段式曲线的平行泊车路径规划方法，其特征在于：先从终点区域中选定一点为A1点，即为第三段路径的起始点，再采用混合A*算法规划第三段路径，以使待泊车辆泊入待泊车位的中心，且与车位平行。
CN202111623746.4A 2021-12-28 2021-12-28 一种结合多段式曲线的平行泊车路径规划方法 Active CN114162113B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111623746.4A CN114162113B (zh) 2021-12-28 2021-12-28 一种结合多段式曲线的平行泊车路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111623746.4A CN114162113B (zh) 2021-12-28 2021-12-28 一种结合多段式曲线的平行泊车路径规划方法

Publications (2)

Publication Number Publication Date
CN114162113A CN114162113A (zh) 2022-03-11
CN114162113B true CN114162113B (zh) 2024-04-09

Family

ID=80488274

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111623746.4A Active CN114162113B (zh) 2021-12-28 2021-12-28 一种结合多段式曲线的平行泊车路径规划方法

Country Status (1)

Country Link
CN (1) CN114162113B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115782927B (zh) * 2023-01-09 2023-04-25 广汽埃安新能源汽车股份有限公司 一种泊车路线规划方法、装置、电子设备和存储介质
CN116279436B (zh) * 2023-05-18 2023-08-08 广汽埃安新能源汽车股份有限公司 一种泊车路径规划方法及装置
CN117077884B (zh) * 2023-10-18 2024-01-23 北京木牛一心机器人科技有限公司 路径规划方法、装置、计算机设备和存储介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102007004972A1 (de) * 2007-01-26 2008-07-31 Valeo Schalter Und Sensoren Gmbh Verfahren zum rückwärtigen Einparken eines Fahrzeugs und Einparkhilfesystem hierfür
KR20100031244A (ko) * 2008-09-12 2010-03-22 주식회사 만도 평행 주차 경로 생성 방법 및 주차 보조 장치
CN108534777A (zh) * 2018-01-25 2018-09-14 江苏大学 一种平行泊车路径规划方法
CN108891412A (zh) * 2018-07-19 2018-11-27 吉林大学 一种平行泊车的路径规划方法
CN110949374A (zh) * 2019-11-14 2020-04-03 江苏大学 基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法
CN111717270A (zh) * 2019-03-20 2020-09-29 上海汽车集团股份有限公司 一种电动助力转向系统的工作状态确定方法及装置
CN111731269A (zh) * 2019-03-22 2020-10-02 广州汽车集团股份有限公司 一种自动泊车路径规划方法及其系统
CN112758084A (zh) * 2021-01-27 2021-05-07 爱驰汽车有限公司 泊车轨迹规划方法、装置、设备及存储介质

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107776570B (zh) * 2017-09-19 2020-09-01 广州汽车集团股份有限公司 全自动泊车方法及全自动泊车系统

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102007004972A1 (de) * 2007-01-26 2008-07-31 Valeo Schalter Und Sensoren Gmbh Verfahren zum rückwärtigen Einparken eines Fahrzeugs und Einparkhilfesystem hierfür
KR20100031244A (ko) * 2008-09-12 2010-03-22 주식회사 만도 평행 주차 경로 생성 방법 및 주차 보조 장치
CN108534777A (zh) * 2018-01-25 2018-09-14 江苏大学 一种平行泊车路径规划方法
CN108891412A (zh) * 2018-07-19 2018-11-27 吉林大学 一种平行泊车的路径规划方法
CN111717270A (zh) * 2019-03-20 2020-09-29 上海汽车集团股份有限公司 一种电动助力转向系统的工作状态确定方法及装置
CN111731269A (zh) * 2019-03-22 2020-10-02 广州汽车集团股份有限公司 一种自动泊车路径规划方法及其系统
CN110949374A (zh) * 2019-11-14 2020-04-03 江苏大学 基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法
CN112758084A (zh) * 2021-01-27 2021-05-07 爱驰汽车有限公司 泊车轨迹规划方法、装置、设备及存储介质

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
汽车倒车安全技术研究;马东杰;屈金龙;;上海汽车(第05期);第59-62页 *

Also Published As

Publication number Publication date
CN114162113A (zh) 2022-03-11

Similar Documents

Publication Publication Date Title
CN114162113B (zh) 一种结合多段式曲线的平行泊车路径规划方法
CN109927716B (zh) 基于高精度地图的自主垂直泊车方法
CN109866762B (zh) 基于高精度地图的自主平行泊车方法
WO2021227304A1 (zh) 一种自动泊车路径规划的避障方法及泊车路径规划系统
CN109987092B (zh) 一种车辆避障换道时机的确定方法及避障换道的控制方法
CN110298122B (zh) 基于冲突消解的无人驾驶车辆城市交叉口左转决策方法
CN109540159B (zh) 一种快速完备的自动驾驶轨迹规划方法
US9457807B2 (en) Unified motion planning algorithm for autonomous driving vehicle in obstacle avoidance maneuver
US9428187B2 (en) Lane change path planning algorithm for autonomous driving vehicle
CN113479217B (zh) 一种基于自动驾驶的变道避障方法与系统
WO2019042295A1 (zh) 一种无人驾驶路径规划方法、系统和装置
CN110239526A (zh) 一种基于准均匀b样条曲线的泊车路径规划方法
CN112606830B (zh) 一种基于混合a*算法的两段式自主泊车路径规划方法
CN107544520A (zh) 一种用于四轮载具自动驾驶的控制方法
CN103158703A (zh) 一种平行泊车的控制方法
CN113978452B (zh) 一种自动平行泊车路径规划方法
CN114610016A (zh) 基于障碍物动态虚拟膨胀的智能车辆避碰路径规划方法
CN114132305B (zh) 用于垂直与斜列泊车路径规划方法、系统、存储介质及车辆
CN115489515A (zh) 针对狭窄垂直车位的自动泊车轨迹规划方法及电子设备
CN111397623A (zh) 一种基于最佳泊车起始点的路径规划方法
CN117170377A (zh) 一种自动驾驶方法、装置及车辆
WO2021181127A1 (ja) 走行経路の設定方法、及び、走行経路の設定装置
CN116161018A (zh) 平行泊车路径规划方法及系统
CN116461522A (zh) 一种车辆变道轨迹规划方法、系统及汽车
CN115950431A (zh) 一种路径规划方法、系统、计算机设备、可读存储介质及机动车

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
GR01 Patent grant
GR01 Patent grant