CN110949374B - 基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法 - Google Patents
基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法 Download PDFInfo
- Publication number
- CN110949374B CN110949374B CN201911113684.5A CN201911113684A CN110949374B CN 110949374 B CN110949374 B CN 110949374B CN 201911113684 A CN201911113684 A CN 201911113684A CN 110949374 B CN110949374 B CN 110949374B
- Authority
- CN
- China
- Prior art keywords
- parking
- track
- vehicle
- planning
- 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 58
- 230000011218 segmentation Effects 0.000 claims abstract description 5
- 238000012545 processing Methods 0.000 claims description 10
- 238000004364 calculation method Methods 0.000 claims description 6
- 230000000007 visual effect Effects 0.000 claims description 3
- 238000012937 correction Methods 0.000 abstract description 3
- 230000007613 environmental effect Effects 0.000 abstract description 3
- 238000012544 monitoring process Methods 0.000 abstract 1
- 238000010586 diagram Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
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
-
- 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
- B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
- B60W50/0098—Details of control systems ensuring comfort, safety or stability not otherwise provided for
-
- 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
- B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
- B60W2050/0001—Details of the control system
- B60W2050/0043—Signal treatments, identification of variables or parameters, parameter estimation or state estimation
Landscapes
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Human Computer Interaction (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
Abstract
本发明公开了一种基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法,该方法通过获取目标车位信息,确定泊车目标位置并以它为原点建立全局坐标系,构建泊车轨迹分段判断模型以确定规划轨迹分段数量,规划第一段轨迹结束位置范围,确定各段规划轨迹起止点及控制点位置,用二阶贝塞尔曲线进行路径规划,每隔一段时间监测并用动态调整规划轨迹法调整规划轨迹。本发明降低了对待泊车辆起始位姿的要求,提高了路径规划方法的环境适应性;可人为通过简单调整将车辆驶入所规划区域,以降低成本,可应用于中低档车中;泊车轨迹规划的控制点易确定、轨迹生成算法简单、路径曲率连续;降低了对轨迹误差的修正难度,提高了车辆避障性及泊车效率。
Description
技术领域
本发明涉及自动泊车路径规划技术领域,特别涉及一种基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法。
背景技术
自动泊车***在一定程度上降低了驾驶员的疲劳强度和车辆被损伤的风险,节约了驾驶员的时间。自动泊车轨迹规划问题是自动泊车***的重要部分,其主要任务是在泊车场景中,为智能驾驶车辆提供从起始位姿到目标位姿的一系列速度和方向盘转角,其中轨迹规划的优劣直接决定着泊车***的好坏。
目前,国内外针对泊车轨迹规划问题研究了多种方法,包括螺旋线、极样条等曲线拟合方法,还有通过简单的几何作图方法进行轨迹规划。其中用三阶贝塞尔曲线规划的泊车轨迹存在控制点难以寻找、轨迹生成算法较复杂及无法实现对移动障碍物的有效避障等问题,且当实际轨迹与规划轨迹出现误差后对实际轨迹进行动态调整较困难;用几何作图方法进行轨迹规划的方式适用范围小,不灵活,且存在曲率突变等问题。故准确灵活与简单高效不能同时在泊车轨迹规划上得到满足。
发明内容
针对上述存在的问题,本发明提出一种基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法,以降低路径规划复杂程度、对轨迹误差的修正难度,同时提高泊车效率、路径规划方法的环境适应性及对移动障碍物的有效避障性。
为了实现上述目的,本发明具体技术方案如下:一种基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法,包括如下步骤:
1)获取目标车位信息,利用车载超声波雷达、激光雷达、视觉传感器、毫米波雷达获取目标车位信息;所述目标车位信息包括车位长度Lc、车位宽度Lk和车位左上角位置Z,其中Z为待泊车辆停靠在目标车位时,以车辆正向行驶方向为正方向,车位左上角位置;
3)构建全局坐标系XOY,其中,坐标原点为泊车目标位置D,X轴为车位宽方向,以待泊车辆所在侧为正方向,Y轴为车位长方向,以车辆正向行驶方向为正方向;
4)通过车载信息采集模块、数据处理模块实时获取待泊车辆位姿和待泊车辆周边障碍物位置;所述待泊车辆位姿包括待泊车辆后轴中点位置C和待泊车辆车身姿态角α;所述车身姿态角α为从Y轴逆时针转向车身纵轴的转角;
5)构建泊车轨迹分段判断模型M;
6)根据泊车轨迹分段判断模型M判断是否需要进行第一段泊车轨迹规划,判断方法为:如果泊车轨迹分段判断模型M中射线L1与圆S1不相交且与射线L2在圆S1下侧相交,则不需要进行第一段泊车轨迹规划,转步骤17);否则需要进行第一段泊车轨迹规划,转步骤7);
7)确定第一段泊车轨迹结束位置区域G;
8)判断车身纵轴反向延长线L1与第一段泊车轨迹结束位置区域G是否有相交线段,若有相交线段则进行步骤9),否则调整车辆位姿,转步骤8);
9)通过数据处理模块确定第一控制点P1位置,所述第一控制点P1为待泊车辆车身纵轴反向延长线L1与第一段泊车轨迹结束位置区域G交线段的中点;
10)通过数据处理模块确定第一段泊车规划轨迹结束点P2位置;
11)确定初始第二控制点临近区域K;
12)根据待泊车辆后轴中点初始位置P0、第一控制点P1、第一段泊车规划轨迹结束点P2生成基于二阶贝塞尔曲线的第一段泊车规划轨迹,所述第一段泊车规划轨迹方程为:
其中Bx(d)为第一段泊车规划轨迹横坐标,By(d)为第一段泊车规划轨迹纵坐标,P0x为待泊车辆后轴中点初始位置横坐标,P0y为待泊车辆后轴中点初始位置纵坐标,P1x为第一控制点横坐标,P1y为第一控制点纵坐标,P2x为第一段泊车规划轨迹结束点横坐标,P2y为第一段泊车规划轨迹结束点纵坐标;
13)待泊车辆按照第一段泊车规划轨迹行驶,每隔Δt时间检测车辆与障碍物距离ΔX、检测并计算实际行驶轨迹与规划轨迹的位置误差ΔS(t)和实际车身姿态角与规划姿态角误差Δα(t),所述实际行驶轨迹与规划轨迹的位置误差ΔS(t)的计算公式如下:
其中,其中ΔS(t)为t时刻车辆实际泊车轨迹与规划轨迹的位置误差,x(t)s为t时刻实际泊车位置的横坐标,y(t)s为t时刻实际泊车位置的纵坐标,x(t)g为t时刻规划泊车位置的横坐标,y(t)g为t时刻规划泊车位置的纵坐标;所述实际车身姿态角与规划姿态角误差Δα(t)计算公式如下:
Δα(t)=|α(t)s-α(t)g|
其中Δα(t)为t时刻车辆实际车身姿态角与规划姿态角的误差,α(t)s为t时刻实际车身姿态角,α(t)g为t时刻规划车身姿态角;
14)判断是否可以进行第二段泊车轨迹规划,判断方法为:若车辆沿第一段泊车轨迹行驶至某点处时车身纵轴反向延长线与初始第二控制点临近区域K有交点,则可以进行第二段泊车轨迹规划,转步骤17),否则进行下一步;
15)判断是否同时满足ΔS(t)<H、Δα(t)<θ、ΔX>X,其中H为轨迹位置误差阈值,θ为轨迹角度误差阈值,X为安全距离阈值,若同时满足则不需要调整规划轨迹,转步骤13);否则进行下一步);
16)动态调整第一段泊车规划轨迹;
17)确定第二段轨迹开始点位置P,确定方法为:若进行了第一段泊车轨迹规划,则第二段轨迹开始点位置P为车身纵轴反向延长线与初始第二控制点临近区域K有交点时待泊车辆后轴中点所在位置,否则第二段轨迹开始点位置P为待泊车辆初始后轴中点所在的位置;
18)确定第二控制点位置P3,第二控制点位置P3为过点P的车身纵轴反向延长线与Y轴正方向的交点;
19)根据第二段轨迹开始点P、第二控制点P3、泊车目标位置点D生成基于二阶贝塞尔曲线的第二段泊车规划轨迹,所述第二段泊车规划轨迹方程为:
其中Cx(d)为第二段泊车规划轨迹横坐标,Cy(d)为第二段泊车规划轨迹纵坐标,Px为第二段轨迹开始点横坐标,Py为第二段轨迹开始点纵坐标,P3x为第二控制点横坐标,P3y为第二控制点纵坐标,Dx为泊车目标位置点横坐标,Dy为泊车目标位置点纵坐标;
20)待泊车辆按照第二段泊车规划轨迹行驶,检测并计算车辆与障碍物距离ΔX、实际行驶轨迹与规划轨迹的位置误差ΔS(t)和实际车身姿态角与规划姿态角误差Δα(t);
21)判断是否同时满足ΔS(t)<H、Δα(t)<θ、ΔX>X,其中H为轨迹位置误差阈值,θ为轨迹角度误差阈值,X为安全距离阈值,若同时满足则不需要调整规划轨迹,转步骤23);否则进行下一步;
22)动态调整第二段泊车规划轨迹;
23)车辆沿第二段泊车规划轨迹行驶直至泊车目标位置,完成泊车。
进一步的,上述步骤5)中,所述泊车轨迹分段判断模型M构建方法包括如下步骤:
5.1)过待泊车辆后轴中点沿车身纵轴反方向作射线,记为L1;
5.2)在Y轴正方向上距泊车目标位置D为N的点向Y轴正方向作射线,记为L2;
进一步的,上述步骤7)中,所述第一段泊车轨迹结束位置区域G确定方法包括如下步骤:
7.1)过在目标车位内且在Y轴正方向上的一点,在待泊车辆所在侧作与Y轴正方向夹角为α2且与圆s1下半部分相切的射线L3,该点为第一段泊车轨迹结束位置区域右上角点,记为D0;
7.2)过D点在待泊车辆所在侧作与Y轴正方向夹角为α1的射线L4;射线L3、L4交点为第一段泊车轨迹结束位置区域左上角点,记为点Q;
7.3)D、D0、Q三点连线构成的三角形区域为第一段泊车轨迹结束位置区域G。
进一步的,上述步骤10)中,所述第一段泊车规划轨迹结束点P2位置确定方法包括如下步骤:
10.1)过第一控制点P1向停车位侧作平行于G区域上下边界线的射线L5、L6;
10.2)作L5、L6两射线所夹角的角平分线;记该线与第一段泊车轨迹结束位置区域右边界所交点为初始第二控制点P3′;
10.3)第一控制点P1与初始第二控制点P3′连线中点为第一段泊车规划轨迹结束点P2。
进一步的,上述步骤11)中,所述初始第二控制点临近区域K确定方法如下:若P3y′-δ>0,则初始第二控制点临近区域K由点P3′沿Y轴正、负方向分别加长度为δ的线段所构成,若P3y′-δ≤0,初始第二控制点临近区域K由点P3′到点P3′沿Y轴正方向加长度为δ的线段所构成;其中P3y′为初始第二控制点P3′纵坐标。
进一步的,上述步骤16)中,所述第一段泊车规划轨迹动态调整方法包括如下步骤:
16.1)判断车辆与障碍物距离是否大于预设安全距离阈值X,如车辆与障碍物距离大于预设安全距离阈值X则转步骤16.2),否则转16.3);
16.2)获取待泊车辆位姿信息并覆盖原有待泊车辆初始位姿信息,转步骤5);
16.3)待泊车辆向远离障碍物侧偏移,偏移角为α0,转步骤16.1)。
进一步的,上述步骤22)中,所述第二段泊车规划轨迹动态调整方法包括如下步骤:
22.1)判断车辆与障碍物距离是否大于预设安全距离阈值X,如车辆与障碍物距离大于预设安全距离阈值X则转步骤22.2),否则转22.5);
22.2)车身纵轴反向延长线与第二控制点生成区域有交点则进行步骤22.3),否则转22.4);所述第二控制点生成区域为距零点长度为E的Y轴正方向上一点与距零点长度为F的Y轴正方向上一点所构成的线段;
22.3)获取待泊车辆位姿信息并覆盖原有P点位姿信息,转步骤18);
22.4)调整待泊车辆位姿,转步骤22.1);
22.5)待泊车辆向远离障碍物侧偏移,偏移角为α0,转步骤22.1)。
本发明降低了对待泊车辆起始位姿的要求,提高了路径规划方法的环境适应性;可人为通过简单调整将车辆驶入所规划区域,以降低成本,可应用于中低档车中;泊车轨迹规划的控制点易确定、轨迹生成算法简单、路径曲率连续;降低了对轨迹误差的修正难度,提高了车辆避障性及泊车效率。
附图说明
图1是本发明自动平行泊车路径规划方法流程图。
图2是本发明第一段泊车轨迹结束位置区域规划示意图。
图3是本发明实施例完整泊车轨迹组成示意图。
具体实施方式
下面结合附图以及具体实施例对本发明作进一步的说明,需要指出的是,下面仅以一种最优化的技术方案对本发明的技术方案以及设计原理进行详细阐述,但本发明的保护范围并不限于此。
所述实施例为本发明的优选的实施方式,但本发明并不限于上述实施方式,在不背离本发明的实质内容的情况下,本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。
一种基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法流程图如图1所示,包括如下步骤:
1)获取目标车位信息,利用车载超声波雷达、激光雷达、视觉传感器、毫米波雷达获取目标车位信息;其中目标车位信息包括车位长度Lc、车位宽度Lk和车位左上角位置Z,其中Z为待泊车辆停靠在目标车位时,以车辆正向行驶方向为正方向,车位左上角位置;;
3)构建全局坐标系XOY,其中,坐标原点为泊车目标位置D,X轴为车位宽方向,以待泊车辆所在侧为正方向,Y轴为车位长方向,以车辆正向行驶方向为正方向;
4)通过车载信息采集模块、数据处理模块实时获取待泊车辆位姿和待泊车辆周边障碍物位置;其中,待泊车辆位姿包括待泊车辆后轴中点位置C和待泊车辆车身姿态角α,车身姿态角α为从Y轴逆时针转向车身纵轴的转角;
5)构建泊车轨迹分段判断模型M,构建方法如下:
5.1)过待泊车辆后轴中点沿车身纵轴反方向作射线,记为L1;
6)根据泊车轨迹分段判断模型M判断是否需要进行第一段泊车轨迹规划,判断方法为:如果泊车轨迹分段判断模型M中射线L1与圆S1不相交且与射线L2在圆S1下侧相交,则不需要进行第一段泊车轨迹规划,转步骤17);否则需要进行第一段泊车轨迹规划,转步骤7);
7)确定第一段泊车轨迹结束位置区域G,确定方法如下:
7.1)过在目标车位内且在Y轴正方向上的一点,在待泊车辆所在侧作与Y轴正方向夹角为α2且与圆s1下半部分相切的射线L3,该点为第一段泊车轨迹结束位置区域右上角点,记为D0;
7.2)过D点在待泊车辆所在侧作与Y轴正方向夹角为α1的射线L4;射线L3、L4交点为第一段泊车轨迹结束位置区域左上角点,记为点Q;
7.3)D、D0、Q三点连线构成的三角形区域为第一段泊车轨迹结束位置区域G;
该方法可根据停车位周围环境的实际情况调整α1及α2的大小,且α1≤α2,两者差值越小则第一段泊车轨迹结束位置区域越大;在本发明具体实施例中α1=80°,α2=90°;
8)通过数据处理模块判断车身纵轴反向延长线L1与第一段泊车轨迹结束位置区域G是否有相交线段,若有相交线段则进行步骤9),否则调整车辆位姿,转步骤8);
9)通过数据处理模块确定第一控制点P1位置,其中,第一控制点P1为待泊车辆车身纵轴反向延长线L1与第一段泊车轨迹结束位置区域G交线段的中点;
10)通过数据处理模块确定第一段泊车规划轨迹结束点P2位置,第一段泊车规划轨迹结束点P2的确定方法包括如下步骤:
10.1)过第一控制点P1向停车位侧作平行于G区域上下边界线的射线L5、L6;
10.2)作L5、L6两射线所夹角的角平分线;记该线与第一段泊车轨迹结束位置区域右边界所交点为初始第二控制点P3′;
10.3)第一控制点P1与初始第二控制点P3′连线中点为第一段泊车规划轨迹结束点P2;
11)确定初始第二控制点临近区域K,其中,若P3y′-δ>0,则初始第二控制点临近区域K由点P3′沿Y轴正、负方向分别加长度为δ的线段所构成,若P3y′-δ≤0,初始第二控制点临近区域K由点P3′到点P3′沿Y轴正方向加长度为δ的线段所构成;其中P3y′为初始第二控制点P3′纵坐标;在本发明具体实施例中
12)根据待泊车辆后轴中点初始位置P0、第一控制点P1、第一段泊车规划轨迹结束点P2生成基于二阶贝塞尔曲线的第一段泊车规划轨迹,所述第一段泊车规划轨迹方程为:
其中Bx(d)为第一段泊车规划轨迹横坐标,By(d)为第一段泊车规划轨迹纵坐标,P0x为待泊车辆后轴中点初始位置横坐标,P0y为待泊车辆后轴中点初始位置纵坐标,P1x为第一控制点横坐标,P1y为第一控制点纵坐标,P2x为第一段泊车规划轨迹结束点横坐标,P2y为第一段泊车规划轨迹结束点纵坐标;
13)待泊车辆按照第一段泊车规划轨迹行驶,每隔Δt时间检测车辆与障碍物距离ΔX、检测并计算实际行驶轨迹与规划轨迹的位置误差ΔS(t)和实际车身姿态角与规划姿态角误差Δα(t),其中,实际行驶轨迹与规划轨迹的位置误差ΔS(t)的计算公式如下:
其中,其中ΔS(t)为t时刻车辆实际泊车轨迹与规划轨迹的位置误差,x(t)s为t时刻实际泊车位置的横坐标,y(t)s为t时刻实际泊车位置的纵坐标,x(t)g为t时刻规划泊车位置的横坐标,y(t)g为t时刻规划泊车位置的纵坐标;所述实际车身姿态角与规划姿态角误差Δα(t)计算公式如下:
Δα(t)=|α(t)s-α(t)g|
其中Δα(t)为t时刻车辆实际车身姿态角与规划姿态角的误差,α(t)s为t时刻实际车身姿态角,α(t)g为t时刻规划车身姿态角;
14)判断是否可以进行第二段泊车轨迹规划,判断方法为:若车辆沿第一段泊车轨迹行驶至某点处时车身纵轴反向延长线与初始第二控制点临近区域K有交点,则可以进行第二段泊车轨迹规划,转步骤17),否则进行下一步;
15)判断是否同时满足ΔS(t)<H、Δα(t)<θ、ΔX>X,其中H为轨迹位置误差阈值,θ为轨迹角度误差阈值,X为安全距离阈值,若同时满足则不需要调整规划轨迹,转步骤13);否则进行下一步;
16)动态调整第一段泊车规划轨迹,动态调整第一段泊车规划轨迹的具体方法如下:
16.1)判断车辆与障碍物距离是否大于预设安全距离阈值X,如车辆与障碍物距离大于预设安全距离阈值X则转步骤16.2),否则转16.3);
16.2)获取待泊车辆位姿信息并覆盖原有待泊车辆初始位姿信息,转步骤5);
16.3)待泊车辆向远离障碍物侧偏移,偏移角为α0,转步骤16.1);
17)确定第二段轨迹开始点位置P,确定方法为:若进行了第一段泊车轨迹规划,则第二段轨迹开始点位置P为车身纵轴反向延长线与初始第二控制点临近区域K有交点时待泊车辆后轴中点所在位置,否则第二段轨迹开始点位置P为待泊车辆初始后轴中点所在的位置;
18)确定第二控制点位置P3,第二控制点位置P3为过点P的车身纵轴反向延长线与Y轴正方向的交点;
19)根据第二段轨迹开始点P、第二控制点P3、泊车目标位置点D生成基于二阶贝塞尔曲线的第二段泊车规划轨迹,所述第二段泊车规划轨迹方程为:
其中Cx(d)为第二段泊车规划轨迹横坐标,Cy(d)为第二段泊车规划轨迹纵坐标,Px为第二段轨迹开始点横坐标,Py为第二段轨迹开始点纵坐标,P3x为第二控制点横坐标,P3y为第二控制点纵坐标,Dx为泊车目标位置点横坐标,Dy为泊车目标位置点纵坐标;
20)待泊车辆按照第二段泊车规划轨迹行驶,检测并计算车辆与障碍物距离ΔX、实际行驶轨迹与规划轨迹的位置误差ΔS(t)和实际车身姿态角与规划姿态角误差Δα(t);
21)判断是否同时满足ΔS(t)<H、Δα(t)<θ、ΔX>X,其中H为轨迹位置误差阈值,θ为轨迹角度误差阈值,X为安全距离阈值,若同时满足则不需要调整规划轨迹,转步骤23);否则进行下一步;
22)动态调整第二段泊车规划轨迹,动态调整第二段泊车规划轨迹的具体方法如下:
22.1)判断车辆与障碍物距离是否大于预设安全距离阈值X,如车辆与障碍物距离大于预设安全距离阈值X则转步骤22.2),否则转22.5);
22.2)车身纵轴反向延长线与第二控制点生成区域有交点则进行步骤22.3),否则转22.4);其中,第二控制点生成区域为距零点长度为E的Y轴正方向上一点与距零点长度为F的Y轴正方向上一点所构成的线段;
22.3)获取待泊车辆位姿信息并覆盖原有P点位姿信息,转步骤18);
22.4)调整待泊车辆位姿,转步骤22.1);
22.5)待泊车辆向远离障碍物侧偏移,偏移角为α0,转步骤22.1);
23)车辆沿第二段泊车规划轨迹行驶直至泊车目标位置,完成泊车。
Claims (8)
1.一种基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法,其特征在于包括如下步骤:
1)获取目标车位信息,利用车载超声波雷达、激光雷达、视觉传感器、毫米波雷达获取目标车位信息;所述目标车位信息包括车位长度Lc、车位宽度Lk和车位左上角位置Z,其中Z为待泊车辆停靠在目标车位时,以车辆正向行驶方向为正方向,车位左上角位置;
3)构建全局坐标系XOY,其中,坐标原点为泊车目标位置D,X轴为车位宽方向,以待泊车辆所在侧为正方向,Y轴为车位长方向,以车辆正向行驶方向为正方向;
4)通过车载信息采集模块、数据处理模块实时获取待泊车辆位姿和待泊车辆周边障碍物位置;所述待泊车辆位姿包括待泊车辆后轴中点位置C和待泊车辆车身姿态角α;所述车身姿态角α为从Y轴逆时针转向车身纵轴的转角;
5)构建泊车轨迹分段判断模型M;
6)根据泊车轨迹分段判断模型M判断是否需要进行第一段泊车轨迹规划,如果需要进行第一段泊车轨迹规划,转步骤7);否则转步骤17);
7)确定第一段泊车轨迹结束位置区域G;
8)判断车身纵轴反向延长线L1与第一段泊车轨迹结束位置区域G是否有相交线段,若有相交线段则进行步骤9),否则调整车辆位姿,转步骤8);
9)通过数据处理模块确定第一控制点P1位置,所述第一控制点P1为待泊车辆车身纵轴反向延长线L1与第一段泊车轨迹结束位置区域G交线段的中点;
10)通过数据处理模块确定第一段泊车规划轨迹结束点P2位置;
11)确定初始第二控制点临近区域K;
12)根据待泊车辆后轴中点初始位置P0、第一控制点P1、第一段泊车规划轨迹结束点P2生成基于二阶贝塞尔曲线的第一段泊车规划轨迹,所述第一段泊车规划轨迹方程为:
其中Bx(d)为第一段泊车规划轨迹横坐标,By(d)为第一段泊车规划轨迹纵坐标,P0x为待泊车辆后轴中点初始位置横坐标,P0y为待泊车辆后轴中点初始位置纵坐标,P1x为第一控制点横坐标,P1y为第一控制点纵坐标,P2x为第一段泊车规划轨迹结束点横坐标,P2y为第一段泊车规划轨迹结束点纵坐标;
13)待泊车辆按照第一段泊车规划轨迹行驶,每隔Δt时间检测车辆与障碍物距离ΔX、检测并计算实际行驶轨迹与规划轨迹的位置误差ΔS(t)和实际车身姿态角与规划姿态角误差Δα(t),所述实际行驶轨迹与规划轨迹的位置误差ΔS(t)的计算公式如下:
其中,其中ΔS(t)为t时刻车辆实际泊车轨迹与规划轨迹的位置误差,x(t)s为t时刻实际泊车位置的横坐标,y(t)s为t时刻实际泊车位置的纵坐标,x(t)g为t时刻规划泊车位置的横坐标,y(t)g为t时刻规划泊车位置的纵坐标;所述实际车身姿态角与规划姿态角误差Δα(t)计算公式如下:
Δα(t)=|α(t)s-α(t)g|
其中Δα(t)为t时刻车辆实际车身姿态角与规划姿态角的误差,α(t)s为t时刻实际车身姿态角,α(t)g为t时刻规划车身姿态角;
14)判断是否可以进行第二段泊车轨迹规划,判断方法为:若车辆沿第一段泊车轨迹行驶至某点处时车身纵轴反向延长线与初始第二控制点临近区域K有交点,则可以进行第二段泊车轨迹规划,转步骤17),否则进行下一步;
15)判断是否同时满足ΔS(t)<H、Δα(t)<θ、ΔX>X,其中H为轨迹位置误差阈值,θ为轨迹角度误差阈值,X为安全距离阈值,若同时满足则不需要调整规划轨迹,转步骤13);否则转步骤16);
16)动态调整第一段泊车规划轨迹;
17)确定第二段轨迹开始点位置P,确定方法为:若进行了第一段泊车轨迹规划,则第二段轨迹开始点位置P为车身纵轴反向延长线与初始第二控制点临近区域K有交点时待泊车辆后轴中点所在位置,否则第二段轨迹开始点位置P为待泊车辆初始后轴中点所在的位置;
18)确定第二控制点位置P3,第二控制点位置P3为过点P的车身纵轴反向延长线与Y轴正方向的交点;
19)根据第二段轨迹开始点P、第二控制点P3、泊车目标位置点D生成基于二阶贝塞尔曲线的第二段泊车规划轨迹,所述第二段泊车规划轨迹方程为:
其中Cx(d)为第二段泊车规划轨迹横坐标,Cy(d)为第二段泊车规划轨迹纵坐标,Px为第二段轨迹开始点横坐标,Py为第二段轨迹开始点纵坐标,P3x为第二控制点横坐标,P3y为第二控制点纵坐标,Dx为泊车目标位置点横坐标,Dy为泊车目标位置点纵坐标;
20)待泊车辆按照第二段泊车规划轨迹行驶,检测并计算车辆与障碍物距离ΔX、实际行驶轨迹与规划轨迹的位置误差ΔS(t)和实际车身姿态角与规划姿态角误差Δα(t);
21)判断是否同时满足ΔS(t)<H、Δα(t)<θ、ΔX>X,其中H为轨迹位置误差阈值,θ为轨迹角度误差阈值,X为安全距离阈值,若同时满足则不需要调整规划轨迹,转步骤23);否则转步骤22);
22)动态调整第二段泊车规划轨迹;
23)车辆沿第二段泊车规划轨迹行驶直至泊车目标位置,完成泊车。
3.如权利要求1所述的基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法,其特征在于所述步骤6)中,所述是否需要进行第一段泊车轨迹规划的判断方法为:如果泊车轨迹分段判断模型M中射线L1与圆S1不相交且与射线L2在圆S1下侧相交,则不需要进行第一段泊车轨迹规划,否则需要进行第一段泊车轨迹规划。
4.如权利要求1所述的基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法,其特征在于所述步骤7)中,所述第一段泊车轨迹结束位置区域G确定方法包括如下步骤:
7.1)过在目标车位内且在Y轴正方向上的一点,在待泊车辆所在侧作与Y轴正方向夹角为α2且与圆s1下半部分相切的射线L3,该点为第一段泊车轨迹结束位置区域右上角点,记为D0;
7.2)过D点在待泊车辆所在侧作与Y轴正方向夹角为α1的射线L4;射线L3、L4交点为第一段泊车轨迹结束位置区域左上角点,记为点Q;
7.3)D、D0、Q三点连线构成的三角形区域为第一段泊车轨迹结束位置区域G。
5.如权利要求1所述的基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法,其特征在于所述步骤10)中,所述第一段泊车规划轨迹结束点P2位置确定方法包括如下步骤:
10.1)过第一控制点P1向停车位侧作平行于G区域上下边界线的射线L5、L6;
10.2)作L5、L6两射线所夹角的角平分线;记该线与第一段泊车轨迹结束位置区域右边界所交点为初始第二控制点P3′;
10.3)第一控制点P1与初始第二控制点P3′连线中点为第一段泊车规划轨迹结束点P2。
6.如权利要求1所述的基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法,其特征在于所述步骤11)中,所述初始第二控制点临近区域K确定方法如下:若P3y′-δ>0,则初始第二控制点临近区域K由点P3′沿Y轴正、负方向分别加长度为δ的线段所构成,若P3y′-δ≤0,初始第二控制点临近区域K由点P3′到点P3′沿Y轴正方向加长度为δ的线段所构成;其中P3y′为初始第二控制点P3′纵坐标。
7.如权利要求1所述的基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法,其特征在于所述步骤16)中,所述第一段泊车规划轨迹动态调整方法包括如下步骤:
16.1)判断车辆与障碍物距离是否大于预设安全距离阈值X,如车辆与障碍物距离大于预设安全距离阈值X则转步骤16.2),否则转16.3);
16.2)获取待泊车辆位姿信息并覆盖原有待泊车辆初始位姿信息,转步骤5);
16.3)待泊车辆向远离障碍物侧偏移,偏移角为α0,转步骤16.1)。
8.如权利要求1所述的基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法,其特征在于所述步骤22)中,所述第二段泊车规划轨迹动态调整方法包括如下步骤:
22.1)判断车辆与障碍物距离是否大于预设安全距离阈值X,如车辆与障碍物距离大于预设安全距离阈值X则转步骤22.2),否则转22.5);
22.2)车身纵轴反向延长线与第二控制点生成区域有交点则进行步骤22.3),否则转22.4);所述第二控制点生成区域为距零点长度为E的Y轴正方向上一点与距零点长度为F的Y轴正方向上一点所构成的线段;
22.3)获取待泊车辆位姿信息并覆盖原有P点位姿信息,转步骤18);
22.4)调整待泊车辆位姿,转步骤22.1);
22.5)待泊车辆向远离障碍物侧偏移,偏移角为α0,转步骤22.1)。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911113684.5A CN110949374B (zh) | 2019-11-14 | 2019-11-14 | 基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911113684.5A CN110949374B (zh) | 2019-11-14 | 2019-11-14 | 基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110949374A CN110949374A (zh) | 2020-04-03 |
CN110949374B true CN110949374B (zh) | 2021-06-18 |
Family
ID=69977290
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911113684.5A Active CN110949374B (zh) | 2019-11-14 | 2019-11-14 | 基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110949374B (zh) |
Families Citing this family (26)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111688679B (zh) * | 2020-06-01 | 2021-05-11 | 湖南大学 | 一种基于区域叠加构建三维可行泊车起始空间的方法 |
CN111752277A (zh) * | 2020-06-24 | 2020-10-09 | 浙江大华技术股份有限公司 | Agv的位姿调整方法、装置、存储介质及电子装置 |
CN111923902B (zh) * | 2020-08-10 | 2022-03-01 | 华人运通(上海)自动驾驶科技有限公司 | 一种泊车控制方法、装置、电子设备及存储介质 |
CN112092802A (zh) * | 2020-09-25 | 2020-12-18 | 北京罗克维尔斯科技有限公司 | 车辆控制方法、装置及车辆 |
CN112172797B (zh) * | 2020-09-27 | 2021-12-17 | 华人运通(上海)自动驾驶科技有限公司 | 泊车控制方法、装置、设备和存储介质 |
CN112256023B (zh) * | 2020-09-28 | 2022-08-19 | 南京理工大学 | 基于贝塞尔曲线的机场巡界机器人局部路径规划方法及*** |
CN112277931B (zh) * | 2020-10-20 | 2022-04-15 | 上海欧菲智能车联科技有限公司 | 垂直泊车轨迹生成方法、装置、车辆及存储介质 |
CN112339747A (zh) * | 2020-10-30 | 2021-02-09 | 上海欧菲智能车联科技有限公司 | 自动泊车轨迹的生成方法、装置、电子设备及存储介质 |
CN112356827A (zh) * | 2020-10-30 | 2021-02-12 | 的卢技术有限公司 | 自动泊车控制方法、装置、计算机设备和存储介质 |
CN112562411B (zh) * | 2020-12-04 | 2022-04-22 | 中国联合网络通信集团有限公司 | 获取自主代客泊车视频的方法和*** |
CN112537294B (zh) * | 2020-12-23 | 2022-05-17 | 上汽通用汽车有限公司 | 自动泊车控制方法及电子设备 |
CN112783161B (zh) * | 2020-12-29 | 2023-04-25 | 广东嘉腾机器人自动化有限公司 | 一种基于贝塞尔曲线的agv避障方法 |
CN112363413B (zh) * | 2020-12-31 | 2021-04-02 | 奥特酷智能科技(南京)有限公司 | 一种生成仿真车自动行驶路径的方法 |
CN112793562B (zh) * | 2021-02-03 | 2023-02-28 | 武汉理工大学 | 一种自动泊车路径的规划和跟踪控制方法、规划装置、存储介质和计算机设备 |
CN113138598A (zh) * | 2021-03-31 | 2021-07-20 | 深圳市优必选科技股份有限公司 | 一种智能小车的控制方法及智能小车 |
CN113085840B (zh) * | 2021-04-21 | 2023-03-07 | 中国第一汽车股份有限公司 | 一种泊车路径的确定方法、装置、车辆及存储介质 |
CN113335270B (zh) * | 2021-07-01 | 2022-05-03 | 湖南大学 | 一种泊车路径规划方法和装置 |
CN113419534B (zh) * | 2021-07-01 | 2022-03-08 | 湖南大学 | 一种基于贝塞尔曲线的转向路段路径规划方法 |
CN113626985B (zh) * | 2021-07-13 | 2023-10-24 | 的卢技术有限公司 | 电机转子结构的生成方法、装置、计算机设备和存储介质 |
CN113701756B (zh) * | 2021-08-04 | 2024-05-31 | 东南大学 | 一种新型自适应无人车辆倒车入库路径规划与跟踪方法 |
CN113581167B (zh) * | 2021-09-01 | 2023-03-31 | 华域汽车***股份有限公司 | 一种基于直线圆弧路径的全自动泊车控制方法及控制器 |
CN114162113B (zh) * | 2021-12-28 | 2024-04-09 | 武汉理工大学 | 一种结合多段式曲线的平行泊车路径规划方法 |
CN114791288B (zh) * | 2022-06-27 | 2022-09-30 | 青岛慧拓智能机器有限公司 | 矿区车辆的行驶路径规划方法、装置及计算机设备 |
CN116653931B (zh) * | 2023-08-01 | 2024-02-23 | 禾昆科技(北京)有限公司 | 车辆泊车控制方法、装置、电子设备和计算机可读介质 |
CN117184060B (zh) * | 2023-11-08 | 2024-01-30 | 新石器慧通(北京)科技有限公司 | 轨迹修正方法和装置、无人车和存储介质 |
CN118072555B (zh) * | 2024-04-19 | 2024-06-18 | 北京工业大学 | 车辆横向异常轨迹的风险分析方法、装置、设备和产品 |
Family Cites Families (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE102006062390A1 (de) * | 2006-12-19 | 2008-06-26 | Valeo Schalter Und Sensoren Gmbh | Verfahren zum rückwärtigen Einparken eines Fahrzeugs und Einparkhilfesystem hierfür |
US10427676B2 (en) * | 2017-05-31 | 2019-10-01 | GM Global Technology Operations LLC | Trajectory planner for autonomous driving using bézier curves |
CN103158703B (zh) * | 2013-03-26 | 2016-04-06 | 北京经纬恒润科技有限公司 | 一种平行泊车的控制方法 |
US10304335B2 (en) * | 2016-04-12 | 2019-05-28 | Ford Global Technologies, Llc | Detecting available parking spaces |
US10145953B2 (en) * | 2017-03-31 | 2018-12-04 | Ford Global Technologies, Llc | Virtual steerable path |
DE102017215737B4 (de) * | 2017-09-07 | 2022-01-27 | Ford Global Technologies, Llc | Fahrerassistenzsystem |
CN110254422B (zh) * | 2019-06-19 | 2021-02-12 | 中汽研(天津)汽车工程研究院有限公司 | 一种基于多目标增强学习及贝塞尔曲线的汽车避障方法 |
-
2019
- 2019-11-14 CN CN201911113684.5A patent/CN110949374B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN110949374A (zh) | 2020-04-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110949374B (zh) | 基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法 | |
CN113306549B (zh) | 一种自动泊车轨迹规划算法 | |
CN109720342B (zh) | 一种垂直泊车的路径规划设计方法 | |
CN105197010A (zh) | 辅助泊车***以及辅助泊车控制方法 | |
CN106515722B (zh) | 一种垂直泊车轨迹规划方法 | |
US9746854B2 (en) | System and method for controlling a vehicle | |
CN105764773B (zh) | 用于控制车辆的方法 | |
US8095273B2 (en) | Autonomous parking strategy of the vehicle with rear steer | |
CN109270933A (zh) | 基于圆锥曲线的无人驾驶避障方法、装置、设备及介质 | |
CN111016886B (zh) | 一种基于b样条理论的自动泊车路径规划方法 | |
CN110696819B (zh) | 一种自动泊车路径规划方法及*** | |
CN103158703A (zh) | 一种平行泊车的控制方法 | |
CN112141091B (zh) | 解决车位偏移和定位偏移的二次泊车方法、***及车辆 | |
CN113689735B (zh) | 一种车辆换道碰撞预警方法及装置 | |
CN108759829B (zh) | 一种智能叉车的局部避障路径规划方法 | |
CN113978452B (zh) | 一种自动平行泊车路径规划方法 | |
CN113525509B (zh) | 一种铰接车辆转向控制方法及装置 | |
CN113419534B (zh) | 一种基于贝塞尔曲线的转向路段路径规划方法 | |
CN112596521B (zh) | 一种双舵轮磁导航agv纠偏方法及装置 | |
CN109859528B (zh) | 一种基于v2x车联网的弯道处车辆位置分类方法 | |
CN113335270B (zh) | 一种泊车路径规划方法和装置 | |
CN114701488B (zh) | 基于横纵向解耦的自动泊车路径跟踪方法 | |
CN115903828A (zh) | 一种基于车辆四轮转向的自动泊车路径规划方法 | |
CN114889643A (zh) | 一种运动障碍物的三元素自主避障方法 | |
CN113701756B (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 | ||
TR01 | Transfer of patent right | ||
TR01 | Transfer of patent right |
Effective date of registration: 20240528 Address after: 230000 b-1018, Woye Garden commercial office building, 81 Ganquan Road, Shushan District, Hefei City, Anhui Province Patentee after: HEFEI WISDOM DRAGON MACHINERY DESIGN Co.,Ltd. Country or region after: China Address before: 212013 No. 301, Xuefu Road, Zhenjiang, Jiangsu Patentee before: JIANGSU University Country or region before: China |