CN110949374B - 基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法 - Google Patents

Abstract

本发明公开了一种基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法，该方法通过获取目标车位信息，确定泊车目标位置并以它为原点建立全局坐标系，构建泊车轨迹分段判断模型以确定规划轨迹分段数量，规划第一段轨迹结束位置范围，确定各段规划轨迹起止点及控制点位置，用二阶贝塞尔曲线进行路径规划，每隔一段时间监测并用动态调整规划轨迹法调整规划轨迹。本发明降低了对待泊车辆起始位姿的要求，提高了路径规划方法的环境适应性；可人为通过简单调整将车辆驶入所规划区域，以降低成本，可应用于中低档车中；泊车轨迹规划的控制点易确定、轨迹生成算法简单、路径曲率连续；降低了对轨迹误差的修正难度，提高了车辆避障性及泊车效率。

Description

基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法

技术领域

本发明涉及自动泊车路径规划技术领域，特别涉及一种基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法。

背景技术

自动泊车***在一定程度上降低了驾驶员的疲劳强度和车辆被损伤的风险，节约了驾驶员的时间。自动泊车轨迹规划问题是自动泊车***的重要部分，其主要任务是在泊车场景中，为智能驾驶车辆提供从起始位姿到目标位姿的一系列速度和方向盘转角，其中轨迹规划的优劣直接决定着泊车***的好坏。

目前，国内外针对泊车轨迹规划问题研究了多种方法，包括螺旋线、极样条等曲线拟合方法，还有通过简单的几何作图方法进行轨迹规划。其中用三阶贝塞尔曲线规划的泊车轨迹存在控制点难以寻找、轨迹生成算法较复杂及无法实现对移动障碍物的有效避障等问题，且当实际轨迹与规划轨迹出现误差后对实际轨迹进行动态调整较困难；用几何作图方法进行轨迹规划的方式适用范围小，不灵活，且存在曲率突变等问题。故准确灵活与简单高效不能同时在泊车轨迹规划上得到满足。

发明内容

针对上述存在的问题，本发明提出一种基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法，以降低路径规划复杂程度、对轨迹误差的修正难度，同时提高泊车效率、路径规划方法的环境适应性及对移动障碍物的有效避障性。

为了实现上述目的，本发明具体技术方案如下：一种基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法，包括如下步骤：

1)获取目标车位信息，利用车载超声波雷达、激光雷达、视觉传感器、毫米波雷达获取目标车位信息；所述目标车位信息包括车位长度L_c、车位宽度L_k和车位左上角位置Z，其中Z为待泊车辆停靠在目标车位时，以车辆正向行驶方向为正方向，车位左上角位置；

2)确定泊车目标位置D，泊车目标位置D为待泊车辆停靠在目标车位时后轴中点的目标位置，位于目标车位纵向中轴线上且在目标车位横向中轴线下方，距离目标车位横向中轴线距离为

其中L为待泊车辆轴距；

3)构建全局坐标系XOY，其中，坐标原点为泊车目标位置D，X轴为车位宽方向，以待泊车辆所在侧为正方向，Y轴为车位长方向，以车辆正向行驶方向为正方向；

4)通过车载信息采集模块、数据处理模块实时获取待泊车辆位姿和待泊车辆周边障碍物位置；所述待泊车辆位姿包括待泊车辆后轴中点位置C和待泊车辆车身姿态角α；所述车身姿态角α为从Y轴逆时针转向车身纵轴的转角；

5)构建泊车轨迹分段判断模型M；

6)根据泊车轨迹分段判断模型M判断是否需要进行第一段泊车轨迹规划，判断方法为：如果泊车轨迹分段判断模型M中射线L₁与圆S₁不相交且与射线L₂在圆S₁下侧相交，则不需要进行第一段泊车轨迹规划，转步骤17)；否则需要进行第一段泊车轨迹规划，转步骤7)；

7)确定第一段泊车轨迹结束位置区域G；

8)判断车身纵轴反向延长线L₁与第一段泊车轨迹结束位置区域G是否有相交线段，若有相交线段则进行步骤9)，否则调整车辆位姿，转步骤8)；

9)通过数据处理模块确定第一控制点P₁位置，所述第一控制点P₁为待泊车辆车身纵轴反向延长线L₁与第一段泊车轨迹结束位置区域G交线段的中点；

10)通过数据处理模块确定第一段泊车规划轨迹结束点P₂位置；

11)确定初始第二控制点临近区域K；

12)根据待泊车辆后轴中点初始位置P₀、第一控制点P₁、第一段泊车规划轨迹结束点P₂生成基于二阶贝塞尔曲线的第一段泊车规划轨迹，所述第一段泊车规划轨迹方程为：

其中B_x(d)为第一段泊车规划轨迹横坐标，B_y(d)为第一段泊车规划轨迹纵坐标，P_0x为待泊车辆后轴中点初始位置横坐标，P_0y为待泊车辆后轴中点初始位置纵坐标，P_1x为第一控制点横坐标，P_1y为第一控制点纵坐标，P_2x为第一段泊车规划轨迹结束点横坐标，P_2y为第一段泊车规划轨迹结束点纵坐标；

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)确定第二控制点位置P₃，第二控制点位置P₃为过点P的车身纵轴反向延长线与Y轴正方向的交点；

19)根据第二段轨迹开始点P、第二控制点P₃、泊车目标位置点D生成基于二阶贝塞尔曲线的第二段泊车规划轨迹，所述第二段泊车规划轨迹方程为：

其中C_x(d)为第二段泊车规划轨迹横坐标，C_y(d)为第二段泊车规划轨迹纵坐标，P_x为第二段轨迹开始点横坐标，P_y为第二段轨迹开始点纵坐标，P_3x为第二控制点横坐标，P_3y为第二控制点纵坐标，D_x为泊车目标位置点横坐标，D_y为泊车目标位置点纵坐标；

20)待泊车辆按照第二段泊车规划轨迹行驶，检测并计算车辆与障碍物距离ΔX、实际行驶轨迹与规划轨迹的位置误差ΔS(t)和实际车身姿态角与规划姿态角误差Δα(t)；

21)判断是否同时满足ΔS(t)＜H、Δα(t)＜θ、ΔX＞X，其中H为轨迹位置误差阈值，θ为轨迹角度误差阈值，X为安全距离阈值，若同时满足则不需要调整规划轨迹，转步骤23)；否则进行下一步；

22)动态调整第二段泊车规划轨迹；

23)车辆沿第二段泊车规划轨迹行驶直至泊车目标位置，完成泊车。

进一步的，上述步骤5)中，所述泊车轨迹分段判断模型M构建方法包括如下步骤：

5.1)过待泊车辆后轴中点沿车身纵轴反方向作射线，记为L₁；

5.2)在Y轴正方向上距泊车目标位置D为N的点向Y轴正方向作射线，记为L₂；

5.3)以Z点为圆心作半径

的圆S₁，其中W为待泊车辆轮距。

进一步的，上述步骤7)中，所述第一段泊车轨迹结束位置区域G确定方法包括如下步骤：

7.1)过在目标车位内且在Y轴正方向上的一点，在待泊车辆所在侧作与Y轴正方向夹角为α₂且与圆s₁下半部分相切的射线L₃，该点为第一段泊车轨迹结束位置区域右上角点，记为D₀；

7.2)过D点在待泊车辆所在侧作与Y轴正方向夹角为α₁的射线L₄；射线L₃、L₄交点为第一段泊车轨迹结束位置区域左上角点，记为点Q；

7.3)D、D₀、Q三点连线构成的三角形区域为第一段泊车轨迹结束位置区域G。

进一步的，上述步骤10)中，所述第一段泊车规划轨迹结束点P₂位置确定方法包括如下步骤：

10.1)过第一控制点P₁向停车位侧作平行于G区域上下边界线的射线L₅、L₆；

10.2)作L₅、L₆两射线所夹角的角平分线；记该线与第一段泊车轨迹结束位置区域右边界所交点为初始第二控制点P₃′；

10.3)第一控制点P₁与初始第二控制点P₃′连线中点为第一段泊车规划轨迹结束点P₂。

进一步的，上述步骤11)中，所述初始第二控制点临近区域K确定方法如下：若P_3y′-δ＞0，则初始第二控制点临近区域K由点P₃′沿Y轴正、负方向分别加长度为δ的线段所构成，若P_3y′-δ≤0，初始第二控制点临近区域K由点P₃′到点P₃′沿Y轴正方向加长度为δ的线段所构成；其中P_3y′为初始第二控制点P₃′纵坐标。

进一步的，上述步骤16)中，所述第一段泊车规划轨迹动态调整方法包括如下步骤：

16.1)判断车辆与障碍物距离是否大于预设安全距离阈值X，如车辆与障碍物距离大于预设安全距离阈值X则转步骤16.2)，否则转16.3)；

16.2)获取待泊车辆位姿信息并覆盖原有待泊车辆初始位姿信息，转步骤5)；

16.3)待泊车辆向远离障碍物侧偏移，偏移角为α₀，转步骤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)待泊车辆向远离障碍物侧偏移，偏移角为α₀，转步骤22.1)。

本发明降低了对待泊车辆起始位姿的要求，提高了路径规划方法的环境适应性；可人为通过简单调整将车辆驶入所规划区域，以降低成本，可应用于中低档车中；泊车轨迹规划的控制点易确定、轨迹生成算法简单、路径曲率连续；降低了对轨迹误差的修正难度，提高了车辆避障性及泊车效率。

附图说明

图1是本发明自动平行泊车路径规划方法流程图。

图2是本发明第一段泊车轨迹结束位置区域规划示意图。

图3是本发明实施例完整泊车轨迹组成示意图。

具体实施方式

下面结合附图以及具体实施例对本发明作进一步的说明，需要指出的是，下面仅以一种最优化的技术方案对本发明的技术方案以及设计原理进行详细阐述，但本发明的保护范围并不限于此。

所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。

一种基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法流程图如图1所示，包括如下步骤：

1)获取目标车位信息，利用车载超声波雷达、激光雷达、视觉传感器、毫米波雷达获取目标车位信息；其中目标车位信息包括车位长度L_c、车位宽度L_k和车位左上角位置Z，其中Z为待泊车辆停靠在目标车位时，以车辆正向行驶方向为正方向，车位左上角位置；；

2)确定泊车目标位置D，泊车目标位置D为待泊车辆停靠在目标车位时后轴中点的目标位置，位于目标车位纵向中轴线上且在目标车位横向中轴线下方，距离目标车位横向中轴线距离为

其中L为待泊车辆轴距；

3)构建全局坐标系XOY，其中，坐标原点为泊车目标位置D，X轴为车位宽方向，以待泊车辆所在侧为正方向，Y轴为车位长方向，以车辆正向行驶方向为正方向；

4)通过车载信息采集模块、数据处理模块实时获取待泊车辆位姿和待泊车辆周边障碍物位置；其中，待泊车辆位姿包括待泊车辆后轴中点位置C和待泊车辆车身姿态角α，车身姿态角α为从Y轴逆时针转向车身纵轴的转角；

5)构建泊车轨迹分段判断模型M，构建方法如下：

5.1)过待泊车辆后轴中点沿车身纵轴反方向作射线，记为L₁；

5.2)在Y轴正方向上距泊车目标位置D为N的点向Y轴正方向作射线，记为L₂；在本发明具体实施例中

5.3)以Z点为圆心作半径

的圆S₁，其中W为待泊车辆轮距；

6)根据泊车轨迹分段判断模型M判断是否需要进行第一段泊车轨迹规划，判断方法为：如果泊车轨迹分段判断模型M中射线L₁与圆S₁不相交且与射线L₂在圆S₁下侧相交，则不需要进行第一段泊车轨迹规划，转步骤17)；否则需要进行第一段泊车轨迹规划，转步骤7)；

7)确定第一段泊车轨迹结束位置区域G，确定方法如下：

7.1)过在目标车位内且在Y轴正方向上的一点，在待泊车辆所在侧作与Y轴正方向夹角为α₂且与圆s₁下半部分相切的射线L₃，该点为第一段泊车轨迹结束位置区域右上角点，记为D₀；

7.2)过D点在待泊车辆所在侧作与Y轴正方向夹角为α₁的射线L₄；射线L₃、L₄交点为第一段泊车轨迹结束位置区域左上角点，记为点Q；

7.3)D、D₀、Q三点连线构成的三角形区域为第一段泊车轨迹结束位置区域G；

该方法可根据停车位周围环境的实际情况调整α₁及α₂的大小，且α₁≤α₂，两者差值越小则第一段泊车轨迹结束位置区域越大；在本发明具体实施例中α₁＝80°，α₂＝90°；

8)通过数据处理模块判断车身纵轴反向延长线L₁与第一段泊车轨迹结束位置区域G是否有相交线段，若有相交线段则进行步骤9)，否则调整车辆位姿，转步骤8)；

9)通过数据处理模块确定第一控制点P₁位置，其中，第一控制点P₁为待泊车辆车身纵轴反向延长线L₁与第一段泊车轨迹结束位置区域G交线段的中点；

10)通过数据处理模块确定第一段泊车规划轨迹结束点P₂位置，第一段泊车规划轨迹结束点P₂的确定方法包括如下步骤：

10.1)过第一控制点P₁向停车位侧作平行于G区域上下边界线的射线L₅、L₆；

10.2)作L₅、L₆两射线所夹角的角平分线；记该线与第一段泊车轨迹结束位置区域右边界所交点为初始第二控制点P₃′；

10.3)第一控制点P₁与初始第二控制点P₃′连线中点为第一段泊车规划轨迹结束点P₂；

11)确定初始第二控制点临近区域K，其中，若P_3y′-δ＞0，则初始第二控制点临近区域K由点P₃′沿Y轴正、负方向分别加长度为δ的线段所构成，若P_3y′-δ≤0，初始第二控制点临近区域K由点P₃′到点P₃′沿Y轴正方向加长度为δ的线段所构成；其中P_3y′为初始第二控制点P₃′纵坐标；在本发明具体实施例中

12)根据待泊车辆后轴中点初始位置P₀、第一控制点P₁、第一段泊车规划轨迹结束点P₂生成基于二阶贝塞尔曲线的第一段泊车规划轨迹，所述第一段泊车规划轨迹方程为：

其中B_x(d)为第一段泊车规划轨迹横坐标，B_y(d)为第一段泊车规划轨迹纵坐标，P_0x为待泊车辆后轴中点初始位置横坐标，P_0y为待泊车辆后轴中点初始位置纵坐标，P_1x为第一控制点横坐标，P_1y为第一控制点纵坐标，P_2x为第一段泊车规划轨迹结束点横坐标，P_2y为第一段泊车规划轨迹结束点纵坐标；

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)待泊车辆向远离障碍物侧偏移，偏移角为α₀，转步骤16.1)；

17)确定第二段轨迹开始点位置P，确定方法为：若进行了第一段泊车轨迹规划，则第二段轨迹开始点位置P为车身纵轴反向延长线与初始第二控制点临近区域K有交点时待泊车辆后轴中点所在位置，否则第二段轨迹开始点位置P为待泊车辆初始后轴中点所在的位置；

18)确定第二控制点位置P₃，第二控制点位置P₃为过点P的车身纵轴反向延长线与Y轴正方向的交点；

19)根据第二段轨迹开始点P、第二控制点P₃、泊车目标位置点D生成基于二阶贝塞尔曲线的第二段泊车规划轨迹，所述第二段泊车规划轨迹方程为：

其中C_x(d)为第二段泊车规划轨迹横坐标，C_y(d)为第二段泊车规划轨迹纵坐标，P_x为第二段轨迹开始点横坐标，P_y为第二段轨迹开始点纵坐标，P_3x为第二控制点横坐标，P_3y为第二控制点纵坐标，D_x为泊车目标位置点横坐标，D_y为泊车目标位置点纵坐标；

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)待泊车辆向远离障碍物侧偏移，偏移角为α₀，转步骤22.1)；

23)车辆沿第二段泊车规划轨迹行驶直至泊车目标位置，完成泊车。

Claims (8)

1.一种基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法，其特征在于包括如下步骤：

1)获取目标车位信息，利用车载超声波雷达、激光雷达、视觉传感器、毫米波雷达获取目标车位信息；所述目标车位信息包括车位长度L_c、车位宽度L_k和车位左上角位置Z，其中Z为待泊车辆停靠在目标车位时，以车辆正向行驶方向为正方向，车位左上角位置；

2)确定泊车目标位置D，即待泊车辆停靠在目标车位时后轴中点的目标位置，所述泊车目标位置D位于目标车位纵向中轴线上且在目标车位横向中轴线下方，距离目标车位横向中轴线距离为

其中L为待泊车辆轴距；

3)构建全局坐标系XOY，其中，坐标原点为泊车目标位置D，X轴为车位宽方向，以待泊车辆所在侧为正方向，Y轴为车位长方向，以车辆正向行驶方向为正方向；

4)通过车载信息采集模块、数据处理模块实时获取待泊车辆位姿和待泊车辆周边障碍物位置；所述待泊车辆位姿包括待泊车辆后轴中点位置C和待泊车辆车身姿态角α；所述车身姿态角α为从Y轴逆时针转向车身纵轴的转角；

5)构建泊车轨迹分段判断模型M；

6)根据泊车轨迹分段判断模型M判断是否需要进行第一段泊车轨迹规划，如果需要进行第一段泊车轨迹规划，转步骤7)；否则转步骤17)；

7)确定第一段泊车轨迹结束位置区域G；

8)判断车身纵轴反向延长线L₁与第一段泊车轨迹结束位置区域G是否有相交线段，若有相交线段则进行步骤9)，否则调整车辆位姿，转步骤8)；

9)通过数据处理模块确定第一控制点P₁位置，所述第一控制点P₁为待泊车辆车身纵轴反向延长线L₁与第一段泊车轨迹结束位置区域G交线段的中点；

10)通过数据处理模块确定第一段泊车规划轨迹结束点P₂位置；

11)确定初始第二控制点临近区域K；

12)根据待泊车辆后轴中点初始位置P₀、第一控制点P₁、第一段泊车规划轨迹结束点P₂生成基于二阶贝塞尔曲线的第一段泊车规划轨迹,所述第一段泊车规划轨迹方程为：

其中B_x(d)为第一段泊车规划轨迹横坐标，B_y(d)为第一段泊车规划轨迹纵坐标，P_0x为待泊车辆后轴中点初始位置横坐标，P_0y为待泊车辆后轴中点初始位置纵坐标，P_1x为第一控制点横坐标，P_1y为第一控制点纵坐标，P_2x为第一段泊车规划轨迹结束点横坐标，P_2y为第一段泊车规划轨迹结束点纵坐标；

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)确定第二控制点位置P₃，第二控制点位置P₃为过点P的车身纵轴反向延长线与Y轴正方向的交点；

19)根据第二段轨迹开始点P、第二控制点P₃、泊车目标位置点D生成基于二阶贝塞尔曲线的第二段泊车规划轨迹,所述第二段泊车规划轨迹方程为：

其中C_x(d)为第二段泊车规划轨迹横坐标，C_y(d)为第二段泊车规划轨迹纵坐标，P_x为第二段轨迹开始点横坐标，P_y为第二段轨迹开始点纵坐标，P_3x为第二控制点横坐标，P_3y为第二控制点纵坐标，D_x为泊车目标位置点横坐标，D_y为泊车目标位置点纵坐标；

20)待泊车辆按照第二段泊车规划轨迹行驶，检测并计算车辆与障碍物距离ΔX、实际行驶轨迹与规划轨迹的位置误差ΔS(t)和实际车身姿态角与规划姿态角误差Δα(t)；

21)判断是否同时满足ΔS(t)＜H、Δα(t)＜θ、ΔX＞X，其中H为轨迹位置误差阈值，θ为轨迹角度误差阈值，X为安全距离阈值，若同时满足则不需要调整规划轨迹，转步骤23)；否则转步骤22)；

22)动态调整第二段泊车规划轨迹；

23)车辆沿第二段泊车规划轨迹行驶直至泊车目标位置，完成泊车。

2.如权利要求1所述的基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法，其特征在于所述步骤5)中，所述泊车轨迹分段判断模型M构建方法包括如下步骤：

5.1)过待泊车辆后轴中点沿车身纵轴反方向作射线，记为L₁；

5.2)在Y轴正方向上距泊车目标位置D为N的点向Y轴正方向作射线，记为L₂；

5.3)以Z点为圆心作半径

的圆S₁，其中W为待泊车辆轮距。

3.如权利要求1所述的基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法，其特征在于所述步骤6)中，所述是否需要进行第一段泊车轨迹规划的判断方法为：如果泊车轨迹分段判断模型M中射线L₁与圆S₁不相交且与射线L₂在圆S₁下侧相交，则不需要进行第一段泊车轨迹规划，否则需要进行第一段泊车轨迹规划。

4.如权利要求1所述的基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法，其特征在于所述步骤7)中，所述第一段泊车轨迹结束位置区域G确定方法包括如下步骤：

7.1)过在目标车位内且在Y轴正方向上的一点，在待泊车辆所在侧作与Y轴正方向夹角为α₂且与圆s₁下半部分相切的射线L₃，该点为第一段泊车轨迹结束位置区域右上角点，记为D₀；

7.2)过D点在待泊车辆所在侧作与Y轴正方向夹角为α₁的射线L₄；射线L₃、L₄交点为第一段泊车轨迹结束位置区域左上角点，记为点Q；

7.3)D、D₀、Q三点连线构成的三角形区域为第一段泊车轨迹结束位置区域G。

5.如权利要求1所述的基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法，其特征在于所述步骤10)中，所述第一段泊车规划轨迹结束点P₂位置确定方法包括如下步骤：

10.1)过第一控制点P₁向停车位侧作平行于G区域上下边界线的射线L₅、L₆；

10.2)作L₅、L₆两射线所夹角的角平分线；记该线与第一段泊车轨迹结束位置区域右边界所交点为初始第二控制点P₃′；

10.3)第一控制点P₁与初始第二控制点P₃′连线中点为第一段泊车规划轨迹结束点P₂。

6.如权利要求1所述的基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法，其特征在于所述步骤11)中，所述初始第二控制点临近区域K确定方法如下：若P_3y′-δ＞0,则初始第二控制点临近区域K由点P₃′沿Y轴正、负方向分别加长度为δ的线段所构成，若P_3y′-δ≤0，初始第二控制点临近区域K由点P₃′到点P₃′沿Y轴正方向加长度为δ的线段所构成；其中P_3y′为初始第二控制点P₃′纵坐标。

7.如权利要求1所述的基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法，其特征在于所述步骤16)中，所述第一段泊车规划轨迹动态调整方法包括如下步骤：

16.1)判断车辆与障碍物距离是否大于预设安全距离阈值X，如车辆与障碍物距离大于预设安全距离阈值X则转步骤16.2)，否则转16.3)；

16.2)获取待泊车辆位姿信息并覆盖原有待泊车辆初始位姿信息，转步骤5)；

16.3)待泊车辆向远离障碍物侧偏移，偏移角为α₀，转步骤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)待泊车辆向远离障碍物侧偏移，偏移角为α₀，转步骤22.1)。

Images