CN111731269A - 一种自动泊车路径规划方法及其系统 - Google Patents

Abstract

本发明提供一种自动泊车路径规划方法和系统，其中，该路径规划方法包括：S1、获取可泊平行车位的位置信息，并根据所述可泊平行车位的位置信息确定泊车路径规划的起点、起点航向、终点以及终点航向；S2、根据所述起点、起点航向、终点以及终点航向生成初步规划路径；S3、判断所述初步规划路径的可通行性，并从满足可通行性的初步规划路径中选取最终规划路径。本发明的路径规划方法通过采用分段定曲率曲线和直线组合规划平行车位泊车路径，便于车辆跟踪控制。

Description

一种自动泊车路径规划方法及其系统

技术领域

本发明涉及路径规划技术领域，尤其涉及一种自动泊车路径规划方法及其系统。

背景技术

近年来，随着国内汽车保有量不断增加，停车困难成为困扰众多车主的一大难题，一些新手停车技术不娴熟，往往造成车辆无法泊入车位或者泊车过程中与其他车辆发生剐蹭等问题。

自动泊车系统旨在解决“泊车难”问题。自动泊车系统是车辆在泊车过程中，通过车载传感器对外界环境进行感知，规划出一条合适的路径，利用控制系统和执行装置来控制车辆平稳准确的停在目标车位。

自动泊车系统主要分为感知、决策规划和运动控制三个模块，其中决策规划模块作为自动泊车系统决策大脑是自动泊车系统重要组成部分之一。决策规划模块主要是根据感知模块识别车辆周围环境的车位及障碍物信息，根据车位和障碍物信息做合理的决策，并规划一条安全可行的泊车路径。平行车位是泊车位于道路平行车位，该类泊车位作为泊车系统一种重要的泊车场景，得到广泛的关注的。现阶段针对垂直泊车位路径规划方法相对较多，针对平行车位路径规划方法相对较少，并且，所规划的路径曲线大部分都是变曲率曲线，不便于车辆跟踪。

发明内容

本发明所要解决的技术问题在于，提供一种自动泊车路径规划方法及其系统，以解决现有技术中由于规划路径曲线是变曲率不便于跟踪的问题。

为了解决上述技术问题，本发明提供一种自动泊车路径规划方法，该方法包括如下步骤：

S1、获取可泊平行车位的位置信息，并根据所述可泊平行车位的位置信息确定泊车规划路径的起点、起点航向、终点以及终点航向；

S2、根据所述泊车规划路径的起点、起点航向、终点以及终点航向基于杜宾斯曲线路径规划方法生成初步规划路径；

S3、判断所述初步规划路径的可通行性，并从满足可通行性的初步规划路径中选取对应的初步规划路径作为最终规划路径。

其中，在所述步骤S1之前还包括：

当泊车程序启动时，获取车辆当前行驶方向和车辆当前质心位置坐标；

当泊车程序启动时，获取车辆当前行驶方向和车辆后轴中心位置；

以车辆后轴中心位置为坐标原点、以车辆行驶方向为x轴，以垂直车辆行驶方向并指向车俩右侧为y轴正方向建立第一坐标系；

使车辆按照所述当前行驶方向继续行驶并搜索可泊平行车位，当搜索到可泊平行车位时，计算所述可泊平行车位在所述第一坐标系下的四点顶点的坐标。

其中，所述步骤S1具体包括：

将车辆搜索到可泊平行车位，车辆停止行驶时的车辆质心位置作为所述规划路径的起点；

获取车辆的几何尺寸，确定规划路径终点的备选位置，根据所述车辆的几何尺寸和所述可泊平行车位的四个顶点的坐标计算每一备选位置处的规划路径终点坐标；

根据所述起点坐标与任一终点坐标计算与所述任一终点对应的起点航向角和终点航向角。

其中，所述规划路径终点的备选位置包括：

车辆质心与可泊平行车位中心重合时车辆后轴中心所在的第一位置、车辆泊入车位后车辆尾部与车辆尾部后侧的车位边界保持最小安全距离时车辆后轴中心所在的第二位置、车辆质心由第二位置向所述可泊平行车位外侧平移四分之一所述车辆宽度的距离后车辆质心所在的第三位置。

其中，所述每一备选位置处的规划路径终点坐标为：

其中，x_s1为第一位置处的第一终点的x轴坐标，y_s1为第一位置处的第一终点的y轴坐标，x_s2为第二位置处的第二终点的x轴坐标，y_s2为第二位置处的第二终点的y轴坐标，x_s3为第三位置处的第三终点的x轴坐标，y_s3为第三位位置处的第三终点的y轴坐标，x_p1为可泊平行车位左上顶点的x轴坐标，y_p1为可泊平行车位左上顶点的y轴坐标，x_p2为可泊平行车位右上顶点的x轴坐标，y_p2为可泊平行车位右上顶点的y轴坐标，x_p3为可泊平行车位左下顶点的x轴坐标，y_p3为可泊平行车位左下顶点的y轴坐标，x_p4为可泊平行车位右下顶点的x轴坐标，y_p4为可泊平行车位右下顶点的y轴坐标，x_p0为可泊平行车位中心点的x轴坐标，y_p0为可泊平行车位中心点的y轴坐标，x_pr是可泊平行车位左边边界的中点的x轴坐标，y_pr是可泊平行车位左边边界的中点的y轴坐标，L_b表示后悬长度，L_r表示车辆质心至车尾长度，S_safe表示尾至车位边界最小安全距离。

其中，采用下式计算与所述任一终点对应的起点航向角和终点航向角：

其中，x_s为所述任一终点的x坐标值，y_s为所述任一终点y轴坐标，α为与所述任一终点对应的起点航向角，β为与所述任一终点对应的终点航向角。

其中，所述步骤S2具体包括：

以所述规划路径起点为坐标原点，以所述规划路径起点到所述任一终点的方向为x轴正方向，以所述x轴按逆时针方向旋转90度的方向为y轴正方向建立与所述任一终点对应的第二坐标系；

通过平移和旋转将所述第一坐标系下的所述规划路径起点坐标、所述任一终点坐标以及与所述任一终点坐标对应的起点航向角和终点航向角转换到与所述任一终点对应的第二坐标系，获得第二坐标系下的起点坐标、起点航向角、终点坐标和终点航向角；

根据每一第二坐标系下的起点坐标、起点航向角、终点坐标和终点航向角，以及最小转弯半径采用杜宾斯曲线路径规划方法生成初步规划路径。

其中，所述根据每一第二坐标系下的起点坐标、起点航向角、终点坐标和终点航向角，以及最小转弯半径采用杜宾斯曲线路径规划方法生成初步规划路径具体包括：

根据每一第二坐标系下的起点坐标、起点航向角、终点坐标和终点航向角、最小转弯半径列出LSR曲线路径方程组为：

求解上述方程组可得：

其中，L表示在路径规划起点沿逆时针方向进行圆弧运动，R表示沿顺时针方向进行圆弧运动到路径规划终点，S表示L和R圆弧运动中间的直线运动，D为路径规划起点和路径规划终点之间的距离，r为最小转弯半径，α'为第二坐标系下的路径规划起点航向角，β'为第二坐标系下的路径规划终点航向角，t为初步规划路径中车辆沿逆时针方向运动的弧长，p为初步规划路径中车辆直线运动的距离，q为初步规划路径中车辆沿顺时针方向运动的弧长。

其中，所述步骤S3具体包括：

判断所述初步规划路径是否满足可通行性，若满足，则加入备选路径集合中，否则删除所述初步规划路径；

根据设定的规划路径终点优先级从所述备选路径集合中选出优先级最高的规划路径终点对应的初步规划路径作为最终规划路径。

其中，所述判断初步规划路径是否满足可通行性具体包括：

使生成的初步规划路径膨胀一设定宽度，判断膨胀后的区域内是否障碍物，若有，则生成的初步规划路径不满足可通行性，否则满足可通行性。

其中，所述设定的规划路径终点优先级具体为：

所述第一终点的优先级高于所述第二终点的优先级，所述第二终点的优先级高于所述第三终点的优先级。

其中，所述设定宽度为

其中，B为车辆宽度。

本发明还提供一种自动泊车路径规划系统，包括：

规划路径信息确认单元，用于获取可泊平行车位的位置信息，根据所述可泊平行车位的位置信息确定泊车规划路径的起点、起点航向、终点以及终点航向；

初步规划路径生成单元，用于根据所述起点、起点航向、终点以及终点航向生成初步规划路径；

最终规划路径选取单元，用于判断所述初步规划路径的可通行性，按照设定的规则从满足可通行性的初步规划路径中选取对应的初步规划路径作为最终规划路径。

本发明实施例的有益效果在于：本发明的平行车位泊车路径规划方法，根据启动泊车程序时的车辆质心位置和行驶方向建立第一坐标系，并在第一坐标系下获得可泊平行车位的位置信息、路径规划起点、终点信息，并建立第二坐标系，基于路径规划起点、终点坐标在第二坐标系下基于杜宾斯曲线路径规划方法获得规划路径。本发明采用杜宾斯曲线的组合计算平行泊车路径，该方法不用分类考虑，能自动搜索最有解，适应性较强，并且通过采用分段定曲率曲线和直线组合规划平行车位泊车路径，便于车辆跟踪控制。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例的一种自动泊车路径规划方法的流程示意图。

图2是本发明实施例的一种自动泊车路径规划方法中建立第一坐标系的示意图。

图3是本发明实施例的一种自动泊车路径规划方法中规划路径的终点的设定位置的示意图。

图4是本发明实施例的一种自动泊车路径规划方法中车位中心点和车位左侧边界线的终点的示意图。

图5是本发明实施例的一种自动泊车路径规划方法中车辆几何参数示意图。

图6是本发明实施例的一种自动泊车路径规划方法中第二坐标系的示意图。

图7是本发明实施例的一种自动泊车路径规划方法中车辆右转一定弧长的位置示意图。

图8是本发明实施例的一种自动泊车路径规划方法中车辆左转一定弧长的位置示意图。

图9是本发明实施例的一种自动泊车路径规划方法中车辆直行一定距离的位置示意图。

图10是本发明实施例的一种自动泊车路径规划方法中按照第二终点生成的初步路径曲线示意图。

具体实施方式

以下各实施例的说明是参考附图，用以示例本发明可以用以实施的特定实施例。

以下参照图1进行说明，本发明实施例一提供一种自动泊车路径规划方法，该方法包括如下步骤：

S1、获取可泊平行车位的位置信息，并根据所述可泊平行车位的位置信息确定泊车路径规划的起点、起点航向、终点以及终点航向。

具体地，在所述步骤S1之前还包括：当车辆驾驶员下车启动泊车程序时，获取车辆后轴中心所在位置和车辆当前行驶方向，以车辆后轴中心所在位置为坐标原点，以车辆行驶方向为x轴正向，以垂直车辆行驶方向指向车辆右侧为y轴正向建立第一坐标系，该第一坐标系如图2所示。车辆继续行驶，并通过车载环视和超声波雷达搜索可泊平行车位，当搜索到可泊平行车位时，获取可泊平行车位的位置信息。具体地，可泊平行车位的位置信息可以是该第一坐标系下的可泊平行车位的四个顶点的坐标值。如图3所示，x_p1为可泊平行车位左上顶点的x轴坐标，y_p1为可泊平行车位左上顶点的y轴坐标，x_p2为可泊平行车位右上顶点的x轴坐标，y_p2为可泊平行车位右上顶点的y轴坐标，x_p3为可泊平行车位左下顶点的x轴坐标，y_p3为可泊平行车位左下顶点的y轴坐标，x_p4为可泊平行车位右下顶点的x轴坐标，y_p4为可泊平行车位右下顶点的y轴坐标。

具体地，将车辆搜索到可泊平行车位时车辆停止向前行驶时车辆后轴中心的当前位置设定为规划路径的起点(x₀，y₀)。设定规划路径的终点的备选位置，如图4所示，规划路径的终点的备选位置包括车辆质心与可泊平行车位中心重合时车辆后轴中心所在的第一位置、车辆泊入车位后车辆尾部与车辆尾部后侧的车位边界保持最小安全距离时车辆后轴中心所在的第二位置、车辆质心由第二位置向所述可泊平行车位外侧平移四分之一所述车辆宽度的距离后车辆质心所在的第三位置。获取车辆的几何信息，如图5所示，车辆的几何信息包括车身长度M、车辆后悬长度L_b、车辆质心至车尾长度L_r、设定尾至车位边界最小安全距离S_safe，计算上述三个位置处对应的终点坐标，具体如下：

其中，x_s1为第一位置处的第一终点的x轴坐标，y_s1为第一位置处的第一终点的y轴坐标，x_s2为第二位置处的第二终点的x轴坐标，y_s2为第二位置处的第二终点的y轴坐标，x_s3为第三位置处的第三终点的x轴坐标，y_s3为第三位置处的第三终点的y轴坐标，x_p1为可泊平行车位左上顶点的x轴坐标，y_p1为可泊平行车位左上顶点的y轴坐标，x_p2为可泊平行车位右上顶点的x轴坐标，y_p2为可泊平行车位右上顶点的y轴坐标，x_p3为可泊平行车位左下顶点的x轴坐标，y_p3为可泊平行车位左下顶点的y轴坐标，x_p4为可泊平行车位右下顶点的x轴坐标，y_p4为可泊平行车位右下顶点的y轴坐标，x_p0为可泊平行车位中心点的x轴坐标，y_p0为可泊平行车位中心点的y轴坐标，x_pr是可泊平行车位左边边界的中点的x轴坐标，y_pr是可泊平行车位左边边界的中点的y轴坐标，L_b表示后悬长度，L_r表示车辆质心至车尾长度，S_safe表示尾至车位边界最小安全距离。

在获得了终点坐标后，根据起点坐标和任一终点坐标计算获得与该任一终点坐标对应的起点航向角和终点航向角。例如路径规划终点为第一终点，则通过起点坐标和第一终点坐标计算获得与第一终点对应的起点航向角和终点航向角。具体的计算公式为：

其中，x_s为所述任一终点的x坐标值，y_s为所述任一终点y轴坐标，α为与所述任一终点对应的起点航向角，β为与所述任一终点对应的终点航向角。

S2、根据所述泊车路径规划的起点、起点航向、终点以及终点航向基于杜宾斯曲线路径规划方法生成初步规划路径。

在确定了规划路径起点坐标、起点航向角、终点坐标和终点航向角后，以规划路径起点为坐标原点，以规划路径起点到任一终点方向为x轴正方向，以所述x轴按顺时针方向旋转90度为y轴正方向建立与所述任一终点对应的第二坐标系x’oy’，因而可以建立三个第二坐标系，如图6所示。将第一坐标系下的规划路径起点坐标(x₀，y₀)、任一终点坐标(x_si,y_si)其中i＝1,2,3、与该任一终点坐标对应的起点航向角α和终点航向角β通过平移和旋转转换到与该任一终点对应的第二坐标系下x’oy’，获得每一第二坐标系下对应的起点坐标(x’₀，y’₀)、起点航向角α'、终点坐标(x'_si,y'_si)、终点航向角β'。例如，假设根据第一坐标系下的起点、第一终点和与第一终点对应的起点航向角、终点航向角建立的第二坐标系记为规划路径坐标系一，则将第一坐标系下的起点、第一终点和与第一终点对应的起点航向角、终点航向角转换到所述规划路径坐标系一。

然后基于杜宾斯曲线路径规划方法的LSR曲线组合，以泊车规划路径起点坐标、任一终点坐标、与任一终点对应的起点航向角和终点航向角、最小转弯半径生成初步规划曲线。

在一具体实施方式中，基于杜宾斯曲线路径规划方法具体为：在倒车过程中，以倒车方向为车辆行驶正方向，车辆逆时针旋转定义为向左运动，车辆顺时针旋转定义为向右运动，以车辆倒车方向为车辆航向，车辆航向角以第二坐标系的x'轴为基准，绕逆时针旋转航向角为正，绕顺时针旋转航向角为负，采用L_δ表示车辆当前航向为θ时，车辆向左运动δ弧长后的位置、S_δ表示车辆完成直线行驶后的位置、L_δ分别表示在右转后的位置。根据图7-9所示的几何关系可得到L_δ、R_δ、S_δ表达式如下式所示:

为简化计算，将泊车路径规划起点和终点距离除以所规划的杜宾斯曲线的半径，即d＝D/r进行单位化，所求得曲线弧长等于路径长度。设t、p和q分别表示三段曲线运动弧长。根据上式，LSR曲线路径方程组为：

求解上述方程组可得：

所求得的t、p和q分别为规划路径起点坐标、起点航向角为(x’₀，y’₀，α')、终点坐标、终点航向角为(x'_si,y'_si，β')、最小转弯半径为r条件下初步规划路径左转的弧度t，直行距离p，右转的弧度q。因此路径曲线长度为：r(t+q)+p。通过LSR曲线路径规划方法可以获得第一终点、第二终点以及第三终点各自对应的初步规划路径。

S3、判断所述初步规划路径的可通行性，并按照设定的规则从满足可通行性的初步规划路径中选取对应的初步规划路径作为最终规划路径。

具体地，判断所述初步规划路径是否满足可通行性，若满足，则加入备选路径集合中，否则删除所述初步规划路径；根据设定的规划路径终点优先级从所述备选路径集合中选出优先级最高的规划路径终点对应的初步规划路径作为最终规划路径。

在一具体实施方式中，设定的规划路径终点优先级具体为：第一终点的优先级高于第二终点的优先级，第二终点的优先级高于第三终点的优先级。

在一具体实施方式中，所述判断初步规划路径是否满足可通行性具体包括：使生成的初步规划路径膨胀一设定的宽度，判断膨胀后的区域内是否障碍物，若有，则生成的初步规划路径不满足可通行性，否则满足可通行性。其中，设定的宽度为

B为车辆宽度。

本发明的平行车位自动泊车方法，根据启动泊车程序时的车辆质心位置和行驶方向建立第一坐标系，并在第一坐标系下获得可泊平行车位的位置信息、路径规划起点、终点信息，并建立第二坐标系，基于路径规划起点、终点坐标在第二坐标系下基于杜宾斯曲线路径规划方法获得规划路径。本发明采用杜宾斯曲线的组合计算平行泊车路径，该方法不用分类考虑，能自动搜索最有解，适应性较强。通过采用分段定曲率曲线和直线组合规划平行车位泊车路径，便于车辆跟踪控制。

以下举例说明，假设车辆感知系统识别平行车位四个顶点坐标分别为：(x_p1，y_p1)＝(5,1.1)、(x_p2，y_p2)＝(10,1.1)、(x_p3，y_p3)＝(10,3.1)、(x_p4，y_p4)＝(5,3.1)。车辆沿x轴向前行驶直至车尾超过库位边界1m，确定该点为车辆泊车路径规划起点(x₀,y₀)＝(11.87,0)，根据车位信息计算泊车位中心点和泊车位左边线中心点(x_p0,y_p0)＝(7.5,2.1)，(x_pr,y_pr)＝(5,2.1)。根据泊车位中心点和泊车位边线中点计算路径规划终点(x_s1,y_s1)＝(6.20,2.1)，(x_s2,y_s2)＝(6.02,2.1)，(x_s3,y_s3)＝(6.02,1.6)，根据三个终点对应建立三个第二坐标系，泊车路径规划起点(x₀,y₀)转换至第二座标系下坐标(x'₀,y'₀)＝(0,0)。泊车路径规划终点(x_si,y_si)转换至对应的第二坐标系下的对应坐标为(x'_s1,y'_s1)＝(6.04,0)，(x'_s2,y'_s2)＝(6.22,0)和(x'_s3,y'_s3)＝(6.06,0)。以(x'₀,y'₀)为泊车路径起点，(x'_s1,y'_s1)为泊车路径终点的起点航向和终点航向角度α₁＝339.6769度，由于泊车起点车身与车位平行，故β₁＝α₁＝339.6769度；同理，求得分别以(x'_s2,y'_s2)和(x'_s3,y'_s3)为泊车终点时，起点和终点航向角度β₂＝α₂＝340.2532度，β₃＝α₃＝344.7035度。采用选用LSR组合的杜宾斯曲线，取最小半径r＝4.5，计算泊车路径起点(x'₀,y'₀)和终点(x_si,y_si)之间的杜宾斯曲线。计算求得基于杜宾斯曲线分段组合结果为：

(1)泊车路径规划起点坐标(x'₀,y'₀)＝(0,0)，泊车路径规划终点(x'_s1,y'_s1)＝(6.04,0)，起点航向角和终点航向角为β₁＝α₁＝339.6769度，最小转弯半径r＝4.5时所求解的杜宾斯曲线无解。

(2)泊车路径规划起点为(x'₀,y'₀)＝(0,0)，泊车路径规划终点(x'_s2,y'_s2)＝(6.22,0)，起点航向角和终点航向角为β₂＝α₂＝340.2532度，最小转弯半径r＝4.5时，所求解的杜宾斯曲线组合为：t＝0.6022,p＝0.2028,q＝0.6022；例点(x'_s2,y'_s2)＝(6.22,0)所规划的路径如图10所示。

(3)泊车路径规划起点为(x'₀,y'₀)＝(0,0)，泊车路径规划终点为(x'_s3,y'_s3)＝(6.06,0)，起点航向角和终点航向角为β₃＝α₃＝344.7035度，最小转弯半径r＝4.5时，所求得的杜宾斯曲线组合为：t＝0.3648，p＝0.6279，q＝0.3648。

分别判定两条泊车路径曲线膨胀

的宽度，其中，B为车辆宽度，判断膨胀后的范围内是否存在障碍物，如果存在障碍物表明所规划的该条泊车路径不可通行，如果两条路径均可通行，则优先选择(x'_s2,y'_s2)＝(6.22,0)作为终点。其中泊车路径终点优先级为第一终点优先级高于第二终点优先级，第二终点优先级高于第三终点优先级。

选择第二终点(x'_s2,y'_s2)＝(6.22,0)作为泊车终点，车辆按照分段定曲率跟踪泊车路径曲线，至泊车路径终点之后，车辆根据前后车距或者与前后车位边线向前调整车位直至车辆质心至车位中心。如果选择的是(x'_s3,y'_s3)＝(6.06,0)作为泊车终点，车辆按照分段定曲率跟踪泊车路径曲线，至泊车路径终点之后，根据前期标定的结果，车辆在车位内调整位置直至车辆停止车位中心。

基于本发明实施例一，本发明实施例二提供一种平行车位自动泊车路径规划系统，具体包括：

规划路径信息确认单元，用于获取所述可泊平行车位的位置信息，并根据所述可泊平行车位的位置信息确定泊车路径规划的起点、起点航向、终点以及终点航向；

初步规划路径生成单元，用于根据所述起点、起点航向、终点以及终点航向生成初步规划路径；

最终规划路径选取单元，根据所述可泊平行车位的位置信息判断所述初步规划路径的可通行性，选取满足可通行性的路径长度最小的初步规划路径作为最终泊车路径。

以上所揭露的仅为本发明较佳实施例而已，当然不能以此来限定本发明之权利范围，因此依本发明权利要求所作的等同变化，仍属本发明所涵盖的范围。

Claims (13)

1.一种自动泊车路径规划方法，其特征在于，包括如下步骤：

S1、获取可泊平行车位的位置信息，并根据所述可泊平行车位的位置信息确定泊车规划路径的起点、起点航向、终点以及终点航向；

S2、根据所述泊车规划路径的起点、起点航向、终点以及终点航向基于杜宾斯曲线路径规划方法生成初步规划路径；

S3、判断所述初步规划路径的可通行性，并从满足可通行性的初步规划路径中选取最终规划路径。

2.根据权利要求1所述的方法，其特征在于，在所述步骤S1之前还包括：

当泊车程序启动时，获取车辆当前行驶方向和车辆后轴中心所在位置；

以车辆后轴中心所在位置为坐标原点、以车辆行驶方向为x轴，以垂直车辆行驶方向并指向车辆右侧为y轴正方向建立第一坐标系；

使车辆按照所述当前行驶方向继续行驶并搜索可泊平行车位，当搜索到可泊平行车位时，计算所述可泊平行车位的四点顶点在所述第一坐标系下的坐标。

3.根据权利要求2所述的方法，其特征在于，所述步骤S1具体包括：

将车辆搜索到可泊平行车位车辆停止行驶时的车辆质心所在位置作为所述规划路径的起点；

获取车辆的几何尺寸，确定规划路径终点的备选位置，根据所述车辆的几何尺寸和所述可泊平行车位的四个顶点的坐标计算每一备选位置处的规划路径终点坐标；

根据所述规划路径起点坐标与任一终点坐标计算与所述任一终点对应的起点航向角和终点航向角。

4.根据权利要求3所述的方法，其特征在于，所述规划路径终点的备选位置包括：

车辆后轴中心与可泊平行车位中心重合时车辆后轴中心所在的第一位置、车辆后轴中心与所述第一位置在同一直线上并且车辆尾部与离车辆尾部最近的车位边界保持最小安全距离时车辆后轴中心所在的第二位置、车辆后轴中心由第二位置向所述可泊平行车位外侧平移四分之一车辆宽度的距离后车辆后轴中心所在的第三位置。

5.根据权利要求4所述的方法，其特征在于，所述每一备选位置处的规划路径终点坐标为：

其中，x_s1为第一位置处的第一终点的x轴坐标，y_s1为第一位置处的第一终点的y轴坐标，x_s2为第二位置处的第二终点的x轴坐标，y_s2为第二位置处的第二终点的y轴坐标，x_s3为第三位置处的第三终点的x轴坐标，y_s3为第三位位置处的第三终点的y轴坐标，x_p1为可泊平行车位左上顶点的x轴坐标，y_p1为可泊平行车位左上顶点的y轴坐标，x_p2为可泊平行车位右上顶点的x轴坐标，y_p2为可泊平行车位右上顶点的y轴坐标，x_p3为可泊平行车位左下顶点的x轴坐标，y_p3为可泊平行车位左下顶点的y轴坐标，x_p4为可泊平行车位右下顶点的x轴坐标，y_p4为可泊平行车位右下顶点的y轴坐标，x_p0为可泊平行车位中心点的x轴坐标，y_p0为可泊平行车位中心点的y轴坐标，x_pr是可泊平行车位左边边界的中点的x轴坐标，y_pr是可泊平行车位左边边界的中点的y轴坐标，L_b表示后悬长度，L_r表示车辆后轴中心至车尾长度，S_safe表示车尾至车位边界最小安全距离。

6.根据权利要求5所述的方法，其特征在于，采用下式计算与所述任一终点对应的起点航向角和终点航向角：

其中，x_s为所述任一终点的x坐标值，y_s为所述任一终点y轴坐标，α为与所述任一终点对应的起点航向角，β为与所述任一终点对应的终点航向角。

7.根据权利要求5所述的方法，其特征在于，所述步骤S2具体包括：

以所述规划路径起点为坐标原点，以所述规划路径起点到所述任一终点的方向为x轴正方向，以所述x轴按逆时针方向旋转90度的方向为y轴正方向建立与所述任一终点对应的第二坐标系；

通过平移和旋转将所述第一坐标系下的所述规划路径起点坐标、所述任一终点坐标以及与所述任一终点对应的起点航向角和终点航向角转换到与所述任一终点对应的第二坐标系，获得第二坐标系下的规划路径起点坐标、起点航向角、终点坐标和终点航向角；

根据每一第二坐标系下的规划路径起点坐标、起点航向角、终点坐标、终点航向角，以及最小转弯半径采用杜宾斯曲线路径规划方法生成初步规划路径。

8.根据权利要求7所述的方法，其特征在于，所述根据每一第二坐标系下的规划路径起点坐标、起点航向角、终点坐标、终点航向角，以及最小转弯半径采用杜宾斯曲线路径规划方法生成初步规划路径具体包括：

以车辆倒车方向为车辆航向，车辆航向角以第二坐标系的x轴为基准，绕逆时针旋转航向角为正，绕顺时针旋转航向角为负，根据每一第二坐标系下的规划路径起点坐标、起点航向角、终点坐标、终点航向角、最小转弯半径列出LSR曲线路径方程组为：

求解上述方程组可得：

其中，L表示在路径规划起点沿逆时针方向进行圆弧运动，R表示沿顺时针方向进行圆弧运动到路径规划终点，S表示L和R圆弧运动中间的直线运动，D为路径规划起点和路径规划终点之间的距离，r为最小转弯半径，α'为第二坐标系下的路径规划起点航向角，β'为第二坐标系下的路径规划终点航向角，t为初步规划路径中车辆沿逆时针方向运动的弧长，p为初步规划路径中车辆直线运动的距离，q为初步规划路径中车辆沿顺时针方向运动的弧长。

9.根据权利要求8所述的方法，其特征在于，所述步骤S3具体包括：

判断所述初步规划路径是否满足可通行性，若满足，则加入备选路径集合中，否则删除所述初步规划路径；

根据设定的规划路径终点优先级从所述备选路径集合中选出优先级最高的规划路径终点对应的初步规划路径作为最终规划路径。

10.根据权利要求9所述的方法，其特征在于，所述判断初步规划路径是否满足可通行性具体包括：

使生成的初步规划路径膨胀一设定宽度，判断膨胀后的区域内是否有障碍物，若有，则生成的初步规划路径不满足可通行性，否则满足可通行性。

11.根据权利要求9所述的方法，其特征在于，所述设定的规划路径终点优先级具体为：

所述第一终点的优先级高于所述第二终点的优先级，所述第二终点的优先级高于所述第三终点的优先级。

12.根据权利要求10所述的方法，其特征在于：

所述设定宽度为

其中，B为车辆宽度。

13.一种自动泊车路径规划系统，其特征在于，包括：

规划路径信息确认单元，用于获取可泊平行车位的位置信息，根据所述可泊平行车位的位置信息确定泊车规划路径的起点、起点航向、终点以及终点航向；

初步规划路径生成单元，用于根据所述起点、起点航向、终点以及终点航向生成初步规划路径；

最终规划路径选取单元，用于判断所述初步规划路径的可通行性，按照设定的规则从满足可通行性的初步规划路径中选取最终规划路径。

Images