多段化垂直泊车路径规划方法及装置与流程

本发明涉及车辆技术领域，尤其涉及一种多段化垂直泊车路径规划方法及装置。

背景技术：

目前，随着泊车空间的越来越小，泊车操作过程复杂，驾驶员不仅需要关注车辆的后方情况，还需关注侧方情况，而车辆的后方视野只能依靠后视镜观察。在泊车过程中视野狭窄，存在较大的盲区。对于经验或技术不足的驾驶员来说，很难在复杂的泊车环境和狭窄的泊车位置条件下安全、准确、快速的完成泊车操作。

现有技术中，多段泊车方法第一阶段通常采用圆弧直线组合，泊车路径曲率不连续,易造成轮胎的非均匀磨损，另外，现有的多段式泊车未考虑车辆可以一次泊入车位，但没有达到目标点的情况，因此，如何在复杂的泊车环境和狭窄的泊车位置条件下安全、准确及快速的完成泊车操作是亟待解决的技术问题。

上述内容仅用于辅助理解本发明的技术方案，并不代表承认上述内容是现有技术。

技术实现要素：

本发明的主要目的在于提供一种多段化垂直泊车路径规划方法及装置，旨在解决如何在复杂的泊车环境和狭窄的泊车位置条件下安全、准确及快速的完成泊车操作的技术问题。

为实现上述目的，本发明提供一种多段化垂直泊车路径规划方法，所述多段化垂直泊车路径规划方法包括以下步骤：

获取当前车辆信息和当前车位信息；

根据所述当前车辆信息和所述当前车位信息确定安全碰撞距离；

判断所述安全碰撞距离是否满足预设距离条件；

在所述安全碰撞距离满足所述预设距离条件时，根据所述当前车辆信息和所述当前车位信息，通过预设路径规划模型获得泊车路径。

优选地，所述获取当前车辆信息和当前车位信息的步骤之前，还包括：

获取样本车辆信息和样本车位信息；

根据所述样本车辆信息和所述样本车位信息确定样本初始坐标和样本目标坐标；

根据所述样本初始坐标和所述样本目标坐标确定样本泊车路径规划策略；

根据所述样本泊车路径规划策略建立预设路径规划模型。

优选地，所述根据所述样本初始坐标和所述样本目标坐标确定样本泊车路径规划策略的步骤，包括：

根据所述样本初始坐标和所述样本目标坐标计算样本航向角和样本行驶曲率半径；

根据所述样本航向角和所述样本行驶曲率半径计算样本极限坐标；

根据所述样本初始坐标、所述样本目标坐标和所述样本极限坐标确定样本泊车路径规划策略。

优选地，所述根据所述当前车辆信息和所述当前车位信息确定安全碰撞距离的步骤，包括：

根据所述当前车辆信息，通过预设几何算法确定起始坐标；

对所述当前车位信息进行处理，获得所述当前车辆信息对应的终点坐标；

根据所述起始坐标和所述终点坐标确定目标极限坐标；

根据所述目标极限坐标和所述当前车辆信息计算安全碰撞距离。

优选地，所述根据所述起始坐标和所述终点坐标确定目标极限坐标的步骤，包括：

根据所述起始坐标和所述终点坐标确定目标航向角；

根据所述起始坐标、所述终点坐标和所述目标航向角确定目标行驶曲率半径；

根据所述起始坐标、所述终点坐标、所述目标航向角和所述目标行驶曲率半径计算目标极限坐标。

此外，为实现上述目的，本发明还提出一种多段化垂直泊车路径规划装置，所述多段化垂直泊车路径规划装置，包括：

获取模块，用于获取当前车辆信息和当前车位信息；

确定模块，用于根据所述当前车辆信息和所述当前车位信息确定安全碰撞距离；

判断模块，用于判断所述安全碰撞距离是否满足预设距离条件；

策略模块，用于在所述安全碰撞距离满足所述预设距离条件时，根据所述当前车辆信息和所述当前车位信息，通过预设路径规划模型获得泊车路径。

优选地，所述多段化垂直泊车路径规划装置，还包括建立模块：

所述建立模块，用于获取样本车辆信息和样本车位信息；

所述建立模块，还用于根据所述样本车辆信息和所述样本车位信息确定样本初始坐标和样本目标坐标；

所述建立模块，还用于根据所述样本初始坐标和所述样本目标坐标确定样本泊车路径规划策略；

所述建立模块，还用于根据所述样本泊车路径规划策略建立预设路径规划模型。

优选地，所述建立模块，还用于根据所述样本初始坐标和所述样本目标坐标计算样本航向角和样本行驶曲率半径；

所述建立模块，还用于根据所述样本航向角和所述样本行驶曲率半径计算样本极限坐标；

所述建立模块，还用于根据所述样本初始坐标、所述样本目标坐标和所述样本极限坐标确定样本泊车路径规划策略。

优选地，所述确定模块，还用于根据所述当前车辆信息，通过预设几何算法确定起始坐标；

所述确定模块，还用于对所述当前车位信息进行处理，获得所述当前车辆信息对应的终点坐标；

所述确定模块，还用于根据所述起始坐标和所述终点坐标确定目标极限坐标；

所述确定模块，还用于根据所述目标极限坐标和所述当前车辆信息计算安全碰撞距离。

优选地，所述确定模块，还用于根据所述起始坐标和所述终点坐标确定目标航向角；

所述确定模块，还用于根据所述起始坐标、所述终点坐标和所述目标航向角确定目标行驶曲率半径；

所述确定模块，还用于根据所述起始坐标、所述终点坐标、所述目标航向角和所述目标行驶曲率半径计算目标极限坐标。

本发明中，首先获取当前车辆信息和当前车位信息，然后根据所述当前车辆信息和所述当前车位信息确定安全碰撞距离，并判断所述安全碰撞距离是否满足预设距离条件，在所述安全碰撞距离满足所述预设距离条件时，根据所述当前车辆信息和所述当前车位信息，通过预设路径规划模型获得泊车路径。通过判断安全碰撞距离是否满足预设距离条件，在所述安全碰撞距离满足所述预设距离条件时，根据所述当前车辆信息和所述当前车位信息，通过预设路径规划模型获得泊车路径，从而有效提高泊车安全性和舒适性。

附图说明

图1为本发明多段化垂直泊车路径规划方法第一实施例的流程示意图；

图2为本发明多段化垂直泊车路径规划方法第一实施例的多段泊车停车示意图；

图3为本发明多段化垂直泊车路径规划方法第一实施例的泊车示意图；

图4为本发明多段化垂直泊车路径规划方法第一实施例的多段式垂直泊车路径规划第一示意图；

图5为本发明多段化垂直泊车路径规划方法第一实施例的无直线段泊车轨迹示意图；

图6为本发明多段化垂直泊车路径规划方法第一实施例的碰撞示意图；

图7为本发明多段化垂直泊车路径规划方法第一实施例的多段式垂直泊车路径规划第二示意图；

图8为本发明多段化垂直泊车路径规划方法第一实施例的s3段示意图；

图9为本发明多段化垂直泊车路径规划装置第一实施例的结构框图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

参照图1，图1为本发明多段化垂直泊车路径规划方法第一实施例的流程示意图，提出本发明多段化垂直泊车路径规划方法第一实施例。

在第一实施例中，所述多段化垂直泊车路径规划方法包括以下步骤：

步骤s10：获取当前车辆信息和当前车位信息。

需要说明的是，本实施例的执行主体是泊车系统，其中，所述泊车系统可获取当前车辆信息、当前车位信息及安全碰撞距离，并在所述安全碰撞距离满足所述预设距离条件时，根据所述当前车辆信息和所述当前车位信息，通过预设路径规划模型获得泊车路径的泊车系统，也可为其他设备，本实施例对此不做限制。

其中，所述当前车辆信息包括位置信息、车速信息和车身信息等，所述当前车位信息包括车位环境信息等。

所述当前车位信息可以通过车载环境传感器(包括机器视觉、雷达或摄像头)获取车位环境信息等。

所述获取当前车辆信息和当前车位信息的步骤之前，获取样本车辆信息和样本车位信息，根据所述样本车辆信息和所述样本车位信息确定样本初始坐标和样本目标坐标，根据所述样本初始坐标和所述样本目标坐标确定样本泊车路径规划策略，根据所述样本泊车路径规划策略建立预设路径规划模型。

所述根据所述样本初始坐标和所述样本目标坐标确定样本泊车路径规划策略的步骤为根据所述样本初始坐标和所述样本目标坐标计算样本航向角和样本行驶曲率半径，根据所述样本航向角和所述样本行驶曲率半径计算样本极限坐标，根据所述样本初始坐标、所述样本目标坐标和所述样本极限坐标确定样本泊车路径规划策略。

步骤s20：根据所述当前车辆信息和所述当前车位信息确定安全碰撞距离。

所述根据所述当前车辆信息和所述当前车位信息确定安全碰撞距离的步骤为根据所述当前车辆信息，通过预设几何算法确定起始坐标，对所述当前车位信息进行处理，获得所述当前车辆信息对应的终点坐标，根据所述起始坐标和所述终点坐标确定目标极限坐标，根据所述目标极限坐标和所述当前车辆信息计算安全碰撞距离。

所述根据所述起始坐标和所述终点坐标确定目标极限坐标的步骤为根据所述起始坐标和所述终点坐标确定目标航向角，根据所述起始坐标、所述终点坐标和所述目标航向角确定目标行驶曲率半径，根据所述起始坐标、所述终点坐标、所述目标航向角和所述目标行驶曲率半径计算目标极限坐标。

步骤s30：判断所述安全碰撞距离是否满足预设距离条件。

步骤s40：在所述安全碰撞距离满足所述预设距离条件时，根据所述当前车辆信息和所述当前车位信息，通过预设路径规划模型获得泊车路径。

此外，为了便于理解，以下进行举例说明：

现有的多段泊车方法第一阶段通常采用圆弧直线组合，泊车路径曲率不连续,易造成轮胎的非均匀磨损，而且在第一阶段所采取的停车条件通常为车身右后顶点d与车位左侧达到安全距离，如图2所示，所述图2为本发明多段化垂直泊车路径规划方法第一实施例的多段泊车停车示意图。

但是当车身d点未进入车位时，d点已经与车位左侧达到安全距离，车辆停止，切换泊车方向。然而此时车位左侧角点1st与车辆尾部还有一段距离，可继续往后行驶。另外，现有的多段式泊车未考虑车辆可以一次泊入车位，但没有达到目标点的情况，如图3所示，所述图3为本发明多段化垂直泊车路径规划方法第一实施例的泊车示意图，其中，车辆的航向角平行与车位，但在x轴方向存在一定距离。

本方案中以车位在左侧为例，将多段式式泊车分为两种情况。

情况1：可以通过一次泊车使车辆与车位平行，但未达到目标停车点(车辆的后轴中心点)。

在车辆与车位平行后，进行库内调整。将多段式泊车划分为9个阶段。s1：直线段；s2：回旋曲线段；s3：圆弧段；s4：回旋曲线段；s5：回旋曲线段；s6：直线段；s7：回旋曲线段；s8：回旋曲线段；s9：直线段。其中，s1直线段根据不同泊车点自适应调整。s4段、s5段、s6段、s7段和s8段为库内调整阶段，s6段根据s3段的终点(q3点)的x坐标与目标停车点(q9点)的x坐标调整，如图4所示，所述图4为本发明多段化垂直泊车路径规划方法第一实施例的多段式垂直泊车路径规划第一示意图。

垂直泊车路径规划的主要步骤为：

步骤1：判断当前车位是否满足一次泊车条件，若满足进行单步泊车，如不满足执行步骤2。

已知车辆在车位中的泊车目标点(targetparkingpoint)，从而逆向推导出无s1、s5直线段的泊车路径，从而获得泊车点的极限x坐标，x'_lit。如图5所示，所述图5为本发明多段化垂直泊车路径规划方法第一实施例的无直线段泊车轨迹示意图。

泊车点的极限y坐标可通过碰撞条件求出，其中，参考如图6所示，所述图6为本发明多段化垂直泊车路径规划方法第一实施例的碰撞示意图。

碰撞条件1：当车辆即将进入车位时，汽车的后轴轴线的延长线与车身左侧边线的交点处最可能于车位角点o点出现碰撞；即要求其中，wveh为车宽。考虑到车载传感器的精度误差，增加安全距离，即d2为安全距离。

碰撞条件2：车身右后点d可能与左侧车位线发生碰撞，即临界条件为其中x′f为f点的横坐标，d1为安全距离。

碰撞条件3：车辆在倒车过程中前右顶点c与侧边障碍发生碰撞，即临界条件为oc＝l+r+y'f-d3，其中y'f为f点的纵坐标，d3为安全距离。

通过碰撞条件1和2可以求出单步泊车起始泊车点y坐标的上限y_lit1，通过碰撞条件3可以求出起始泊车点y坐标的下限y_lit2。单步泊车的位置条件为起始泊车点(s1段的起点)坐标(x1,y1)，满足条件y_lit2≤y1≤y_lit1和x1≥x_lit。

步骤2：判断当前位置是否满足情况1的条件。

当s3圆弧段终点时，车辆与车位平行，与车位左侧达到安全距离，即lo1d＝x'f+s-d1。通过可求出起始泊车点极限坐标(x_lit3,y_lit3)。

情况1的满足条件为：y_lit1<y1≤y_lit3和x1≥x_lit3。

步骤3：当起始泊车点满足情况1的条件时，开始泊车。专利只针对多段式泊车，对车位满足单步泊车工况，本专利不作考虑。当满足步骤1和步骤2的工况，输出所有关键点的坐标信息，该信息主要包括：

1、关键点坐标序号

2、泊车坐标系下关键点x坐标点

3、泊车坐标系下关键点y坐标点

4、泊车坐标系下关键点的航向角

5、泊车坐标系下推荐行驶速度

6、泊车坐标系下的期望行驶距离

7、泊车坐标系下的行驶曲率半径

8、当前所处的阶段

其中，关于垂直泊车路径规划数学建模：

以组成车位的2nd角点为坐标系原点，车辆前进方向为x轴方向，车身左侧方向为y轴方向，建立泊车坐标系。在泊车坐标系下垂直泊车路径规划数学建模：

s1段：

起始泊车点(parkingpoint)的状态量为泊车目标点的状态量为x、y为以车位第二角点为坐标原点下的坐标，为车辆航向角，k为曲率。其中，k0＝0，k5＝0。

在泊车坐标系下，q1点的状态方程为：

s2段：

以q1为相对坐标系原点，q2点的相对坐标为(x'2,y'2)。

令a＝k1，b＝k2，

在泊车坐标系下，q2点的状态方程为：

其中，kmax为曲率的最大值，为最小转弯半径的倒数；k_spd为曲率变化速度。

s3段：

以q2为相对坐标系原点，q3点的相对坐标为(x'3,y'3)；

在泊车坐标系下，q3点的状态方程为：

其中，l3为s3段的长度，r3为s3段终点的曲率半径。

s4段：

此阶段方向盘向左打，以q3为相对坐标系原点，q4点的相对坐标为(x'4,y'4)，计算方法同s2段。

在泊车坐标系下，q4点的状态方程为：

其中，l4为s4段的长度，r4为s4段终点的曲率半径。

s5段：

此阶段方向盘回正，以q4为相对坐标系原点，q5点的相对坐标为(x'5,y'5)，计算方法同s2段。

在泊车坐标系下，q5点的状态方程为：

其中，l5为s5段的长度，r5为s5段终点的曲率半径。

s6段：

以q5为相对坐标系原点，q6点的相对坐标为(x'6,y'6)。

在泊车坐标系下，q6点的状态方程为：

s7段：

以q6为相对坐标系原点，q7点的相对坐标为(x'7,y'7)，计算方法同s2段。

在泊车坐标系下，q7点的状态方程为：

其中，l7为s7段的长度，r7为s7段终点的曲率半径。

s8段：

以q7为相对坐标系原点，q8点的相对坐标为(x'8,y'8)，计算方法同s2段。

在泊车坐标系下，q8点的状态方程为：

情况2：当车辆与车位左侧达到安全距离，无法继续后退，此时车辆与车位不平行，有前进空间。

通过后退挡与前进挡切换使用，使车辆的位姿达到水平位置。将多段式泊车划分为6个阶段。s1：直线段；s2：回旋曲线段；s3：圆弧段；s4：圆弧段；s5：圆弧段；s6：直线段。其中，s1直线段根据不同泊车点自适应调整。s4段、s5段为调整阶段，如图7所示，所述图7为本发明多段化垂直泊车路径规划方法第一实施例的多段式垂直泊车路径规划第二示意图。

当不满足情况1以及单步泊车时，采用情况2的多段式泊车。输出所有关键点的坐标信息与情况1相同。

垂直泊车路径规划数学建模：

以组成车位的2nd角点为坐标系原点，车辆前进方向为x轴方向，车身左侧方向为y轴方向，建立泊车坐标系。在泊车坐标系下垂直泊车路径规划数学建模：

s1段：

起始泊车点(parkingpoint)的状态量为泊车目标点的状态量为x、y为以车位第二角点为坐标原点下的坐标，为车辆航向角，k为曲率。其中，k0＝0，k5＝0。

在泊车坐标系下，q1点的状态方程为：

s2段：

以q1为相对坐标系原点，q2点的相对坐标为(x'2,y'2)。

令a＝k1，b＝k2，

在泊车坐标系下，q2点的状态方程为：

其中，kmax为曲率的最大值，为最小转弯半径的倒数；k_spd为曲率变化速度。

s3段：

s3段分两种情况考虑：①当车身d点进入车位，以d点与车位左侧的距离为s3段停车的判断条件；②当车身d点未进入车位时，以车位角点与车身尾部距离作为判断条件，当车位角点1st与车身尾部ad的距离达到安全距离停车。如图8所示，所述图8为本发明多段化垂直泊车路径规划方法第一实施例的s3段示意图。

以q2为相对坐标系原点，q3点的相对坐标为(x'3,y'3)；

在泊车坐标系下，q3点的状态方程为：

其中，l3为s3段的长度，r3为s3段终点的曲率半径。

s4段：

以q3为相对坐标系原点，q4点的相对坐标为(x'4,y'4)。

在泊车坐标系下，q4点的状态方程为：

其中，l4为s4段的长度，r4为s4段终点的曲率半径。

s5段：

以q4为相对坐标系原点，q5点的相对坐标为(x'5,y'5)。

在泊车坐标系下，q5点的状态方程为：

其中，l5为s5段的长度，r5为s5段终点的曲率半径。

由上可知，本申请提出一种多段式垂直泊车单步路径规划算法,在满足车辆运动学原理和车辆运动约束的基础上，规划出一条合理的垂直泊车轨迹，从而增强泊车路径规划算法适应性，从而进一步改善和规避现有市场技术方案的安全风险，其中，路径规划是自动泊车系统的重要组成部分，一条合理的泊车轨迹对自动泊车的安全性、可靠性和有效性将会起到不可估量的作用。

在本实施例中，首先获取当前车辆信息和当前车位信息，然后根据所述当前车辆信息和所述当前车位信息确定安全碰撞距离，并判断所述安全碰撞距离是否满足预设距离条件，在所述安全碰撞距离满足所述预设距离条件时，根据所述当前车辆信息和所述当前车位信息，通过预设路径规划模型获得泊车路径。通过上述方式判断安全碰撞距离是否满足预设距离条件，在所述安全碰撞距离满足所述预设距离条件时，根据所述当前车辆信息和所述当前车位信息，通过预设路径规划模型获得泊车路径，从而有效提高泊车安全性、可靠性及有效性。

此外，参照图9，本发明实施例还提出一种多段化垂直泊车路径规划装置，所述多段化垂直泊车路径规划装置，包括：获取模块9001，用于获取当前车辆信息和当前车位信息；确定模块9002，用于根据所述当前车辆信息和所述当前车位信息确定安全碰撞距离；判断模块9003，用于判断所述安全碰撞距离是否满足预设距离条件；策略模块9004，用于在所述安全碰撞距离满足所述预设距离条件时，根据所述当前车辆信息和所述当前车位信息，通过预设路径规划模型获得泊车路径。

所述获取模块9001，用于获取当前车辆信息和当前车位信息的操作。

需要说明的是，本实施例的执行主体是泊车系统，其中，所述泊车系统可获取当前车辆信息、当前车位信息及安全碰撞距离，并在所述安全碰撞距离满足所述预设距离条件时，根据所述当前车辆信息和所述当前车位信息，通过预设路径规划模型获得泊车路径的泊车系统，也可为其他设备，本实施例对此不做限制。

其中，所述当前车辆信息包括位置信息、车速信息和车身信息等，所述当前车位信息包括车位环境信息等。

所述当前车位信息可以通过车载环境传感器(包括机器视觉、雷达或摄像头)获取车位环境信息等。

所述获取当前车辆信息和当前车位信息的步骤之前，获取样本车辆信息和样本车位信息，根据所述样本车辆信息和所述样本车位信息确定样本初始坐标和样本目标坐标，根据所述样本初始坐标和所述样本目标坐标确定样本泊车路径规划策略，根据所述样本泊车路径规划策略建立预设路径规划模型。

所述根据所述样本初始坐标和所述样本目标坐标确定样本泊车路径规划策略的步骤为根据所述样本初始坐标和所述样本目标坐标计算样本航向角和样本行驶曲率半径，根据所述样本航向角和所述样本行驶曲率半径计算样本极限坐标，根据所述样本初始坐标、所述样本目标坐标和所述样本极限坐标确定样本泊车路径规划策略。

所述确定模块9002，用于根据所述当前车辆信息和所述当前车位信息确定安全碰撞距离的操作。

所述根据所述当前车辆信息和所述当前车位信息确定安全碰撞距离的步骤为根据所述当前车辆信息，通过预设几何算法确定起始坐标，对所述当前车位信息进行处理，获得所述当前车辆信息对应的终点坐标，根据所述起始坐标和所述终点坐标确定目标极限坐标，根据所述目标极限坐标和所述当前车辆信息计算安全碰撞距离。

所述根据所述起始坐标和所述终点坐标确定目标极限坐标的步骤为根据所述起始坐标和所述终点坐标确定目标航向角，根据所述起始坐标、所述终点坐标和所述目标航向角确定目标行驶曲率半径，根据所述起始坐标、所述终点坐标、所述目标航向角和所述目标行驶曲率半径计算目标极限坐标。

所述判断模块9003，用于判断所述安全碰撞距离是否满足预设距离条件的操作。

所述策略模块9004，用于在所述安全碰撞距离满足所述预设距离条件时，根据所述当前车辆信息和所述当前车位信息，通过预设路径规划模型获得泊车路径的操作。

此外，为了便于理解，以下进行举例说明：

现有的多段泊车方法第一阶段通常采用圆弧直线组合，泊车路径曲率不连续,易造成轮胎的非均匀磨损，而且在第一阶段所采取的停车条件通常为车身右后顶点d与车位左侧达到安全距离，如图2所示，所述图2为本发明多段化垂直泊车路径规划方法第一实施例的多段泊车停车示意图。

但是当车身d点未进入车位时，d点已经与车位左侧达到安全距离，车辆停止，切换泊车方向。然而此时车位左侧角点1st与车辆尾部还有一段距离，可继续往后行驶。另外，现有的多段式泊车未考虑车辆可以一次泊入车位，但没有达到目标点的情况，如图3所示，所述图3为本发明多段化垂直泊车路径规划方法第一实施例的泊车示意图，其中，车辆的航向角平行与车位，但在x轴方向存在一定距离。

本方案中以车位在左侧为例，将多段式式泊车分为两种情况。

情况1：可以通过一次泊车使车辆与车位平行，但未达到目标停车点(车辆的后轴中心点)。

在车辆与车位平行后，进行库内调整。将多段式泊车划分为9个阶段。s1：直线段；s2：回旋曲线段；s3：圆弧段；s4：回旋曲线段；s5：回旋曲线段；s6：直线段；s7：回旋曲线段；s8：回旋曲线段；s9：直线段。其中，s1直线段根据不同泊车点自适应调整。s4段、s5段、s6段、s7段和s8段为库内调整阶段，s6段根据s3段的终点(q3点)的x坐标与目标停车点(q9点)的x坐标调整，如图4所示，所述图4为本发明多段化垂直泊车路径规划方法第一实施例的多段式垂直泊车路径规划第一示意图。

垂直泊车路径规划的主要步骤为：

步骤1：判断当前车位是否满足一次泊车条件，若满足进行单步泊车，如不满足执行步骤2。

已知车辆在车位中的泊车目标点(targetparkingpoint)，从而逆向推导出无s1、s5直线段的泊车路径，从而获得泊车点的极限x坐标，x'_lit。如图5所示，所述图5为本发明多段化垂直泊车路径规划方法第一实施例的无直线段泊车轨迹示意图。

泊车点的极限y坐标可通过碰撞条件求出，其中，参考如图6所示，所述图6为本发明多段化垂直泊车路径规划方法第一实施例的碰撞示意图。

碰撞条件1：当车辆即将进入车位时，汽车的后轴轴线的延长线与车身左侧边线的交点处最可能于车位角点o点出现碰撞；即要求其中，wveh为车宽。考虑到车载传感器的精度误差，增加安全距离，即d2为安全距离。

碰撞条件2：车身右后点d可能与左侧车位线发生碰撞，即临界条件为其中x′f为f点的横坐标，d1为安全距离。

碰撞条件3：车辆在倒车过程中前右顶点c与侧边障碍发生碰撞，即临界条件为oc＝l+r+y'f-d3，其中y'f为f点的纵坐标，d3为安全距离。

通过碰撞条件1和2可以求出单步泊车起始泊车点y坐标的上限y_lit1，通过碰撞条件3可以求出起始泊车点y坐标的下限y_lit2。单步泊车的位置条件为起始泊车点(s1段的起点)坐标(x1,y1)，满足条件y_lit2≤y1≤y_lit1和x1≥x_lit。

步骤2：判断当前位置是否满足情况1的条件。

当s3圆弧段终点时，车辆与车位平行，与车位左侧达到安全距离，即通过可求出起始泊车点极限坐标(x_lit3,y_lit3)。

情况1的满足条件为：y_lit1<y1≤y_lit3和x1≥x_lit3。

步骤3：当起始泊车点满足情况1的条件时，开始泊车。专利只针对多段式泊车，对车位满足单步泊车工况，本专利不作考虑。当满足步骤1和步骤2的工况，输出所有关键点的坐标信息，该信息主要包括：

1、关键点坐标序号

2、泊车坐标系下关键点x坐标点

3、泊车坐标系下关键点y坐标点

4、泊车坐标系下关键点的航向角

5、泊车坐标系下推荐行驶速度

6、泊车坐标系下的期望行驶距离

7、泊车坐标系下的行驶曲率半径

8、当前所处的阶段

其中，关于垂直泊车路径规划数学建模：

以组成车位的2nd角点为坐标系原点，车辆前进方向为x轴方向，车身左侧方向为y轴方向，建立泊车坐标系。在泊车坐标系下垂直泊车路径规划数学建模：

s1段：

起始泊车点(parkingpoint)的状态量为泊车目标点的状态量为x、y为以车位第二角点为坐标原点下的坐标，为车辆航向角，k为曲率。其中，k0＝0，k5＝0。

在泊车坐标系下，q1点的状态方程为：

s2段：

以q1为相对坐标系原点，q2点的相对坐标为(x'2,y'2)。

令a＝k1，b＝k2，

在泊车坐标系下，q2点的状态方程为：

其中，kmax为曲率的最大值，为最小转弯半径的倒数；k_spd为曲率变化速度。

s3段：

以q2为相对坐标系原点，q3点的相对坐标为(x'3,y'3)；

在泊车坐标系下，q3点的状态方程为：

其中，l3为s3段的长度，r3为s3段终点的曲率半径。

s4段：

此阶段方向盘向左打，以q3为相对坐标系原点，q4点的相对坐标为(x'4,y'4)，计算方法同s2段。

在泊车坐标系下，q4点的状态方程为：

其中，l4为s4段的长度，r4为s4段终点的曲率半径。

s5段：

此阶段方向盘回正，以q4为相对坐标系原点，q5点的相对坐标为(x'5,y'5)，计算方法同s2段。

在泊车坐标系下，q5点的状态方程为：

其中，l5为s5段的长度，r5为s5段终点的曲率半径。

s6段：

以q5为相对坐标系原点，q6点的相对坐标为(x'6,y'6)。

在泊车坐标系下，q6点的状态方程为：

s7段：

以q6为相对坐标系原点，q7点的相对坐标为(x'7,y'7)，计算方法同s2段。

在泊车坐标系下，q7点的状态方程为：

其中，l7为s7段的长度，r7为s7段终点的曲率半径。

s8段：

以q7为相对坐标系原点，q8点的相对坐标为(x'8,y'8)，计算方法同s2段。

在泊车坐标系下，q8点的状态方程为：

情况2：当车辆与车位左侧达到安全距离，无法继续后退，此时车辆与车位不平行，有前进空间。

通过后退挡与前进挡切换使用，使车辆的位姿达到水平位置。将多段式泊车划分为6个阶段。s1：直线段；s2：回旋曲线段；s3：圆弧段；s4：圆弧段；s5：圆弧段；s6：直线段。其中，s1直线段根据不同泊车点自适应调整。s4段、s5段为调整阶段，如图7所示，所述图7为本发明多段化垂直泊车路径规划方法第一实施例的多段式垂直泊车路径规划第二示意图。

当不满足情况1以及单步泊车时，采用情况2的多段式泊车。输出所有关键点的坐标信息与情况1相同。

垂直泊车路径规划数学建模：

以组成车位的2nd角点为坐标系原点，车辆前进方向为x轴方向，车身左侧方向为y轴方向，建立泊车坐标系。在泊车坐标系下垂直泊车路径规划数学建模：

s1段：

起始泊车点(parkingpoint)的状态量为泊车目标点的状态量为x、y为以车位第二角点为坐标原点下的坐标，为车辆航向角，k为曲率。其中，k0＝0，k5＝0。

在泊车坐标系下，q1点的状态方程为：

s2段：

以q1为相对坐标系原点，q2点的相对坐标为(x'2,y'2)。

令a＝k1，b＝k2，

在泊车坐标系下，q2点的状态方程为：

其中，kmax为曲率的最大值，为最小转弯半径的倒数；k_spd为曲率变化速度。

s3段：

s3段分两种情况考虑：①当车身d点进入车位，以d点与车位左侧的距离为s3段停车的判断条件；②当车身d点未进入车位时，以车位角点与车身尾部距离作为判断条件，当车位角点1st与车身尾部ad的距离达到安全距离停车。如图8所示，所述图8为本发明多段化垂直泊车路径规划方法第一实施例的s3段示意图。

以q2为相对坐标系原点，q3点的相对坐标为(x'3,y'3)；

在泊车坐标系下，q3点的状态方程为：

其中，l3为s3段的长度，r3为s3段终点的曲率半径。

s4段：

以q3为相对坐标系原点，q4点的相对坐标为(x'4,y'4)。

在泊车坐标系下，q4点的状态方程为：

其中，l4为s4段的长度，r4为s4段终点的曲率半径。

s5段：

以q4为相对坐标系原点，q5点的相对坐标为(x'5,y'5)。

在泊车坐标系下，q5点的状态方程为：

其中，l5为s5段的长度，r5为s5段终点的曲率半径。

由上可知，本申请提出一种多段式垂直泊车单步路径规划算法,在满足车辆运动学原理和车辆运动约束的基础上，规划出一条合理的垂直泊车轨迹，从而增强泊车路径规划算法适应性，从而进一步改善和规避现有市场技术方案的安全风险，其中，路径规划是自动泊车系统的重要组成部分，一条合理的泊车轨迹对自动泊车的安全性、可靠性和有效性将会起到不可估量的作用。

在本实施例中，首先获取当前车辆信息和当前车位信息，然后根据所述当前车辆信息和所述当前车位信息确定安全碰撞距离，并判断所述安全碰撞距离是否满足预设距离条件，在所述安全碰撞距离满足所述预设距离条件时，根据所述当前车辆信息和所述当前车位信息，通过预设路径规划模型获得泊车路径。通过上述方式判断安全碰撞距离是否满足预设距离条件，在所述安全碰撞距离满足所述预设距离条件时，根据所述当前车辆信息和所述当前车位信息，通过预设路径规划模型获得泊车路径，从而有效提高泊车安全性、可靠性及有效性。

本发明多段化垂直泊车路径规划装置的其他实施例或具体实现方式可参照上述各方法实施例，此处不再赘述。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者系统所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者系统中还存在另外的相同要素。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。在列举了若干装置的单元权利要求中，这些装置中的若干个可以是通过同一个硬件项来具体体现。词语第一、第二、以及第三等的使用不表示任何顺序，可将这些词语解释为名称。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如只读存储器镜像(readonlymemoryimage，rom)/随机存取存储器(randomaccessmemory，ram)、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。

以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。