CN109720342B - 一种垂直泊车的路径规划设计方法 - Google Patents

Abstract

本发明公开了一种垂直泊车的路径规划设计方法，包括如下步骤：S1，根据车辆参数和环境信息计算所需最小车位长度；S2，比较实际车位长度和最小车位长度；S3，根据步骤S2比较结果选择泊车方式，如果实际车位长度小于或者等于最小车位长度，采用一次泊车到位的路径规划设计，如果实际车位长度小于或者等于最小车位长度，采用多步泊车到位的路径规划设计。本发明依据车辆在虚拟坐标系下的位置信息和通过整车CAN信号得到的车身姿态角信息，对车辆的路径规划进行具体设计，减轻驾驶员的压力。

Description

一种垂直泊车的路径规划设计方法

技术领域

本发明涉及智能驾驶技术领域，具体涉及智能停车技术。

背景技术

随着人们生活水平的不断提高和汽车产业不断的蓬勃发展，汽车数量随之逐年增加，而公路，街道及停车场显得越发拥挤狭窄，因车辆泊车引起的纠纷也逐年增加。泊车时驾驶员仅依靠后视镜与车窗玻璃观察周围车辆环境，视野受到较大限制，无法同时掌控其车辆后方的状况与兼顾周围车辆的状况，容易产生不安全的因素，而且泊车很大程度上依赖于驾驶员的驾驶经验与技巧，若是经验与技巧不够，又遇到较为狭窄的车位，泊车难度就会大大增加，因此才有了泊车辅助***。

随着泊车辅助***不断向着智能化，人性化的要求迈进，自动泊车***应运而生。开发自动泊车***的目的在于将驾驶员从复杂的泊车操作中解脱出来，提高驾驶的舒适性，缓解驾驶员泊车时的紧张程度，预防泊车事故的发生。自动泊车***包含环境数据采集***(超声波雷达***，整车CAN信号采集)，车载ECU(进行数据处理，路径规划与控制命令输出)，车辆泊车操作***(可主动控制的EPS)。

自动泊车分为侧方泊车与垂直泊车。对于垂直泊车，由于驾驶员操作时不易把握好打方向盘的时机以及方向盘的角度，而且垂直泊车可能需要进出车位数次才能停好，所以驾驶员还要兼顾换挡。因此，有必要研究垂直泊车的路径规划问题，减轻驾驶员的压力。

发明内容

本发明所要解决的技术问题就是提供一种垂直泊车的路径规划设计方法，自动进行路径规划，减轻驾驶员的压力。

为解决上述技术问题，本发明采用如下技术方案：一种垂直泊车的路径规划设计方法，包括如下步骤：

S1，根据车辆参数和环境信息计算所需最小车位长度；

S2，比较实际车位长度和最小车位长度；

S3，根据步骤S2比较结果选择泊车方式，如果实际车位长度大于最小车位长度，采用一次泊车到位的路径规划设计，如果实际车位长度小于或者等于最小车位长度，采用多步泊车到位的路径规划设计；

一次泊车到位的路径规划，以车位中线R为基准确定各坐标参数，起始位置为车后轴中心距离中线R的位置；包含以下参数：W：车辆宽度，L：车身长度， R_min：最小转弯半径，C_x：侧向距离，b：后轴中心到车尾的距离，d₃：本车在车位里距离两侧的距离，d₁：车辆后轴最内点距离车位的安全距离，Δd：一次泊车到位约束段长度，L₂：超声波识别的实际车位长度，L_min：计算得到的所需最小车位长度，上述参数具有以下几何关系：

L₂＝W+2·d₃

可选的，一次泊车到位的路径规划包括：采用最小转弯半径，根据最小侧向距离判断所需求的最小车位长度。

可选的，一次泊车到位的路径规划还包括：曲线段开始前需使用直线段过渡。

可选的，车辆两个侧后探头测到车辆进入车位后，开始对两侧车辆的车身姿态进行识别，在识别后重新规划路径，开始新的控制。

可选的，位于车辆侧边的超声波探头识别到侧向距离C_x和实际车位长度L₂后，如果L₂≥(L_min-10)，则选择一次泊车到位的路径规划设计,反之则选择多步泊车到位的路径规划设计。

可选的，一次泊车到位的路径规划设计还包括起始转弯点：设车辆的起始位置为A0，坐标为(x₀，y₀，a₀)则需要直行到位置A1后再开始沿圆弧运动，此时车辆运动轨迹为直线与相切圆弧组成，由于前一步计算最小车位长度时假设起始位置的姿态角为0，因此需要补足圆弧，设补足后虚拟的起始点为Ax，按照Ax 位置规划最小车位，此时不确定结束点姿态角是否为90°，设结束点x坐标为 0，A1状态对应的起始转弯点坐标为(x₁，y₁，a₁)，则虚拟点Ax坐标为(x_x，y_x，a_x)，根据几何关系应满足如下关系：

x_x＝x₁+R_min·sin(a₁)

y_x＝y₁+R_min·[1-cos(a₁)]

其中f表示后轴至车头的距离，也是车辆进入车位后车后轴中心距离车位水平线的距离，即车位障碍点的y坐标；

建立直行段的直线方程：

y₀-y₁＝tan(a₀)·(x₀-x₁)，

根据下式：

其中L_act为实际车位长度，将前一步的y_x代入此式即可求得起始转弯点A1的坐标x₁和y₁。

可选的，多步泊车到位的路径规划设计包括第一段路径规划，第一段路径规划包括上述的最小车位长度和起始转弯点，还包括：

右后碰撞点：当车辆的右后端点进入到车位水平线时，直角三角形B1和B2有公共的斜边，其中b为车辆后轴到车尾的距离即三角形B1的一个直角边，B1的另一条直角边为车身转弯圆圆心与车内侧后轮中心连线，B2的直角边由车身转弯圆圆心向车位水平线作垂线得到，该垂线与水平线的交点到车身右后端点的连线即为B2水平方向的直角边，令x为B2水平方向的直角边，根据几何关系可以推算得到x值:

判断x-(R_min-L_min/2)是否大于0，若大于0则右后碰撞点不会撞上障碍车；

左后碰撞点：当车辆的左后端点进入到车位水平线时，直角三角形B1和B2有公共的斜边，其中b为车辆后轴到车尾的距离即三角形B1的一个直角边，令x 为B2水平方向的直角边，根据几何关系可以计算得到x值

此时判断x-(R_min-L_min/2)是否大于实际车位长度，若大于则左后端点可进入车位且姿态角可以达到临界值，左后端点进入车位且姿态角可以达到临界值：若姿态角恰好可以到达临界值α，设定为75°，则可依据以下几何关系求出对应的最小车位：

α_B＝tan^-1(b/(R_min+W/2))

L_s1＝R_B·sin(α+α_B)+d₁-(R_min-L_min/2)，

其中，α_B为车身转弯圆心和后轴连线与圆心和车左后端点连线的夹角，R_B为车左后端点到圆心的距离，L_s1为姿态角刚好达到临界值需要的最小车位，若结束点对应的姿态角无法达到临界值，则可以根据已知的车位长度按下式求出对应的姿态角α：

α＝sin^-1[(L_s1-d₁+x_s)/R_B]-α_B,

其中x_s为起始停车点到车位边沿的距离。

可选的，多步泊车到位的路径规划设计还包括第二段路径规划，设第二段开始(即第一段路径结束时车停止的位置)时车身后轴中心在虚拟坐标系中的x坐标为x_s,y坐标为y_s，a_s为第二段路径开始时车身的姿态角，即此时车身坐标为(x_s，y_s，a_s)，假设x_s≤0，第二段路径的目标结束点坐标为(x_e，y_e，a_e)，其中x_e＝0，a_e＝90°，

如果采用一段圆弧路径就到达目标位置，则该圆弧b必须满足与车位中线相切且转弯半径不得过大，过大会导致纵向的距离过大，此时计算圆弧半径 R＝(-x_s)/[1-sin(a_s)]，会有如下两种情况：

1)R＜R_min

按照R_min半径规划路径，如果上到姿态角等于90°处车辆的位置没有超出车位，则停止点为姿态角等于90°的位置，如果超过了，则以x＝(车位长度-W)/2处的点为停止点；

2)R≥R_min

采用直线+圆弧法路径需要的纵向长度Y_se1可按照几何关系计算：

Y_se1＝R_min·cos(a_s)+tan(a_s)·[-x_s-R_min·(1-sin(a_s))]

采用两段圆弧法路径需要的纵向长度Y_se2可由几何关系计算：

若Y_se2大于Y_se1则选取直线+圆弧法的方式规划路径，反之则选取两段圆弧的方法规划路径。

可选的，多步泊车到位的路径规划设计还包括第三段路径规划，开始规划第三段时，先判断当前车身姿态角α，再以当前姿态角过车辆后轴中点做轨迹延长直线，该直线与y＝0的交点横坐标，定为x_d，开始规划第三段路径时车身后轴中心在虚拟坐标系中的x坐标为x_s,y坐标为y_s，a_s为第三段路径开始时车身的姿态角；

C0：α接近90°且x_d几乎与车位中心重合，按照轨迹延长直线直接规划路径；

C1：α接近90°、|x_d|较大且x_s>0，起始点位于x＝0的右侧，并且姿态角较大，此时可以规划为两段圆弧，根据几何关系计算转弯半径，公式如下：

此时会出现以下两种情况：

1)R＞R_min

为了保证车辆在车位中的姿态角不要小于70°需要对结束点x坐标重新规划，假设整段路径中车身姿态角最大处的角度为a_M，按下式计算该角度：

若a_M大于70°则按照原规划结束点(0，0，90°)；

若小于90°，则令最大姿态角为70°重新规划第三段路径结束点的x坐标x_e：

x_e＝x₀-2·R·(1-sin(a_M))

此处x₀为开始规划第三段路径时车身后轴中心在虚拟坐标系中的x坐标；

2)R≤R_min

取R＝R_min，此时无法保证规划到x＝0，设规划到y＝0，尽可能避免规划路径中间出现车身姿态角过小的情况；

C2：α接近90°、|x_d|较大且x_s≤0

起始点位于x＝0的左侧，并且姿态角较大，此时姿态角为负值，为了计算方便，依然取正值，此时可以规划为两段圆弧，根据下式计算转弯半径

此时会出现以下两种情况：

1)R＞R_min

为了保证车辆在车位中的姿态角不要小于70°，需要对结束点x坐标重新规划，假设整段路径中车身姿态角最大处的角度为a_M：

若a_M大于70°则按照原规划结束点(0，0，90°)；

若小于90°，则令最大姿态角为70°重新规划第三段路径结束点的x坐标x_e：

x_e＝x₀-2·R·(1-sin(a_M))

此处x₀为开始规划第三段路径时车身后轴中心在虚拟坐标系中的x坐标；

2)R≤R_min

取R＝R_min，此时无法保证规划到x＝0，设规划到y＝0，确保车辆在车位的姿态角不要小于70°；

C3:α较小且x_s>0

首先选取一段圆弧，判断是否可以达到目标点(0,0,90°)，可以通过几何关系求得一段到位所需要的转弯半径R，根据R值的大小分为以下两种情况：

1)R＞R_min

d_y为沿规划路径运动时y方向的运动距离，

如果满足d_y<y_s则按照一段圆弧规划，圆弧结束后接直线；

否则判断直线+圆弧的方案是否可行，按照最小转弯半径，计算所需的y方向距离，规划到结束点(0,0,90°)

设圆弧直线相切点为Z点，坐标为(x_z，y_z，a_z)，通过几何关系可求得直线+圆弧所需的纵向距离Y_se，计算方式如下式：

Y_se＝R_min·cos(a_s)+tan(a_s)·[x_s-R_min·(1-sin(a_s))]

若Y_se＜y_s，则直线+圆弧可行，否则则采用两段圆弧法，方法同C1；

2)R≤R_min

采用最小转弯半径，规划到y＝0，此时判断终点的x坐标，若超过单侧车位间隙的范围，则将终止点规划到车位间隙的范围内；

C4:α小于80°且x_s<0，直接采用最小转弯半径R_min进行路径规划。

本发明采用的技术方案，比较当前环境信息下计算出的所需最小车位长度和实际检测到的车位长度来选择一次泊车到位或多次泊车到位。随后依据车辆在虚拟坐标系下的位置信息和通过整车CAN信号得到的车身姿态角信息，对车辆的路径规划进行具体设计，减轻驾驶员的压力。

本发明的具体技术方案及其有益效果将会在下面的具体实施方式中结合附图进行详细的说明。

附图说明

下面结合附图和具体实施方式对本发明作进一步描述：

图1为本发明整体流程图；

图2为一次泊车到位示意图；

图3为一次泊车到位具体实例计算得到所需最小车位长度的示意图；

图4为一次泊车到位约束段Δd与侧向距离的关系示意图；

图5为多次泊车到位第一段路径规划最小车位长度示意图；

图6为多次泊车到位第一段路径规划右后碰撞点示意图；

图7为多次泊车到位第一段路径规划左后碰撞点示意图；

图8为多次泊车到位第一段路径规划左后端点进入车位且姿态角可以达到临界值示意图；

图9为多次泊车到位第一段路径规划左后端点无法进入车位的临界姿态角示意图；

图10为多次泊车到位第二段路径规划示意图1；

图11为多次泊车到位第二段路径规划示意图2；

图12为多次泊车到位第二段路径规划示意图3；

图13为多次泊车到位第三段路径规划示意图；

图14为多次泊车到位第三段路径规划C2示意图；

图15为多次泊车到位第三段路径规划C3示意图1；

图16为多次泊车到位第三段路径规划C3示意图2。

具体实施方式

如图1所示，一种垂直泊车的路径规划设计方法，包括如下步骤：

S1，根据车辆参数和环境信息计算所需最小车位长度；

S2，比较实际车位长度和最小车位长度；

S3，根据步骤S2比较结果选择泊车方式，如果实际车位长度大于最小车位长度，采用一次泊车到位的路径规划设计，如果实际车位长度小于或者等于最小车位长度，采用多步泊车到位的路径规划设计。

一次泊车到位

如图2所示:

采用最小车辆的转弯半径，为车自身的最小转弯半径，由车厂提供；

根据最小侧向距离判断所需求的最小车位长度，最小侧向距离可以设置，这里设为固定值50厘米；

小于等于最小侧向距离对应的最小车位长度时，停在中间；大于时按照最小侧向距离对应的最小车位长度靠前车停(前车为寻找车位过程中后测到的车)；

曲线段开始前需使用直线段过渡，由于找到车位后再停车时，可能会停在离车位较远的地方，如果此时直接转向，会有较高概率撞上车位一侧的车，因此需要先以直线段的方式直着往后倒一段距离后再开始转向；

车辆两个侧后探头测到车辆进入车位后，通过安装在车身两侧后方的雷达对两侧的车进行探测，根据探测结果结合车在虚拟坐标系中的坐标与车身姿态对两侧车辆的车身姿态进行识别，根据识别出的角度与车在车位中与两侧车的距离，修正初始找车位时定下的车位中心点(虚拟坐标系中的原点)与初始设定的停止位置目标角度，重新规划路径，开始新的控制。

一次泊车到位的情况以车位中线为基准确定各坐标参数，起始位置为距离中线R的位置；

图2中各参数的意义为：W(车辆宽度)，L(车身长度)，R_min(最小转弯半径)，c_x(侧向距离)，b(后轴中心到车尾的距离)，d₃(试验车在车位里距离两侧的距离)，d₁(车辆后轴最内点距离车位的安全距离)，Δd(一次泊车到位约束段长度)，L₂(超声波识别的实际车位长度)，L_min(计算得到的所需最小车位长度)。

上述参数具有以下几何关系：

L₂＝W+2·d₃

取参数R＝400cm，W＝180cm，d₁＝20cm，C_x取50～200cm，b＝97cm，L＝457cm，计算L_min。结果如图3所示。约束段Δd与侧向距离的关系如图4所示。

起始转弯点：参考图5所示，设车辆的起始位置为A0，坐标为(x₀，y₀，a₀)，则需要直行到位置A1后再开始沿圆弧运动，此时车辆运动轨迹为直线与相切圆弧组成，由于前一步计算最小车位长度时假设起始位置的姿态角为0，因此需要补足圆弧，设补足后虚拟的起始点为Ax，按照Ax位置规划最小车位，此时不确定结束点姿态角是否为90°，设结束点x坐标为0，A1状态对应的起始转弯点坐标为(x₁，y₁，a₁)，则虚拟点Ax坐标为(x_x，y_x，a_x)，根据几何关系应满足如下关系：

x_x＝x₁+R_min·sin(a₁)

y_x＝y₁+R_min·[1-cos(a₁)]

其中f表示后轴至车头的距离，也是车辆进入车位后车后轴中心距离车位水平线的距离，即车位障碍点的y坐标；建立直行段的直线方程：

y₀-y₁＝tan(a₀)·(x₀-x₁)，根据下式：

其中L_act为实际车位长度，将前一步的y_x代入此式即可求得起始转弯点A1的坐标x₁和y₁。

位于车辆侧边的超声波探头识别到侧向距离C_x和实际车位长度L₂后，如果 L₂≥(L_min-10)，则选择一次泊车到位的算法。考虑到车辆进入车位后会采用侧后方两个探头修正，故留10cm裕量；反之则选择多步泊车到位算法。

多次泊车到位

第一段路径规划

最小车位长度

如图5所示，按照最小转弯半径进行规划，假设起始时车辆位置为Ax，设此位置时侧向距离为c_x，车辆宽为W，车辆后轴最内点距离车位的安全距离为d₁，车辆的最小转弯半径为R_min，根据几何关系即可求得最小车位长度L_min，具体计算方法参考一次泊车到位计算方法。

起始转弯点

如图5所示，设车辆的起始位置为A0，坐标为(x₀，y₀，a₀)，则需要直行到位置 A1后再开始沿圆弧运动，此时车辆运动轨迹为直线与相切圆弧组成，由于前一步计算时假设起始位置的姿态角为0，因此需要补足圆弧，设补足后虚拟的起始点为Ax，按照Ax位置规划最小车位。此时不确定结束点姿态角是否为90°，设结束点x坐标为0，A1状态对应的起始点(后轴中点)坐标为(x₁，y₁，a₁)，则虚拟点Ax坐标为(x_x，y_x，a_x)，根据几何关系即可求得起始转弯点的坐标值，具体计算方法参考一次泊车到位计算方法。

右后碰撞点

如图6所示，当车辆的右后端点进入到车位水平线时(右后端点与车位水平线在同一直线上时)，直角三角形B1和B2有公共的斜边(车身转弯圆的圆心到车尾右后端点)，其中b为车辆后轴到车尾的距离即图中三角形B1的一个直角边，B1的另一条直角边为车身转弯圆圆心与车内侧后轮中心连线，B2的直角边由车身转弯圆圆心向车位水平线作垂线得到，该垂线与水平线的交点到车身右后端点的连线即为B2水平方向的直角边。令x为B2水平方向的直角边，根据几何关系可以推算得到x值:

判断x-(R_min-L_min/2)是否大于0，若大于0则右后碰撞点不会撞上障碍车。

左后碰撞点

如图7所示，当车辆的左后端点进入到车位水平线时，直角三角形B1和B2 有公共的斜边，其中b为车辆后轴到车尾的距离即图中三角形B1的一个直角边，令x为B2水平方向的直角边，根据几何关系可以计算得到x值

此时判断x-(R_min-L_min/2)是否大于实际车位长度，若大于则左后端点可进入车位且姿态角可以达到临界值。

如图8所示，左后端点进入车位且姿态角可以达到临界值：若姿态角恰好可以到达临界值α(设定为75°)，则可依据以下几何关系求出对应的最小车位：

α_B＝tan^-1(b/(R_min+W/2))

L_s1＝R_B·sin(α+α_B)+d₁-(R_min-L_min/2)，

如图9所示，若结束点对应的姿态角无法达到临界值，则可以根据已知的车位长度按下式求出对应的姿态角α：

α＝sin^-1[(L_s1-d₁+x_s)/R_R]-α_B，

此处x_s为起始停车点到车位边沿的距离。

由于需要多段泊车到位，因此在此设置临界值作为第一段路径停止的条件，避免车一直往后倒撞到障碍车。

第二段路径规划

如图10所示，设第二段开始(即第一段路径结束时车停止的位置)时车身后轴中心在虚拟坐标系中的x坐标为x_s,y坐标为y_s，a_s为第二段路径开始时车身的姿态角，即此时车身坐标为(x_s，y_s，a_s)，假设x_s≤0，第二段路径的目标结束点坐标为(x_e，y_e，a_e)，其中x_e＝0，a_e＝90°，

如果采用一段圆弧路径就到达目标位置，则该圆弧b必须满足与车位中线相切且转弯半径不得过大，过大会导致纵向的距离过大，此时计算圆弧半径 R＝(-x_s)/[1-sin(a_s)]，会有如下两种情况：

1)R＜R_min

按照R_min半径规划路径，如果上到姿态角等于90°处车辆的位置没有超出车位，则停止点为姿态角等于90°的位置，如果超过了，则以x＝(车位长度-W)/2处的点为停止点；

2)R≥R_min

如图11所示，采用直线+圆弧法路径需要的纵向长度Y_se1可按照几何关系计算：

Y_se1＝R_min·cos(a_s)+tan(a_s)·[-x_s-R_min·(1-sin(a_s))]

如图12所示，采用两段圆弧法路径需要的纵向长度Y_se2可由几何关系计算：

若Y_se2大于Y_se1则选取直线+圆弧法的方式规划路径，反之则选取两段圆弧的方法规划路径。

第三段路径规划

如图13所示，开始规划第三段时，先判断当前车身姿态角α(车身姿态角由车上传感器返回的数据积分计算得到)，再以当前姿态角过车辆后轴中点做轨迹延长直线，该直线与y＝0的交点横坐标，定为x_d，开始规划第三段路径时车身后轴中心在虚拟坐标系中的x坐标为x_s,y坐标为y_s，a_s为第三段路径开始时车身的姿态角,即第三段路径开始时车的坐标为(x_s，y_s，a_s)；

C0：α接近90°且x_d几乎与车位中心重合，按照轨迹延长直线直接规划路径；

C1：α接近90°、|x_d|较大且x_s>0，起始点位于x＝0的右侧，并且姿态角较大，此时可以规划为两段圆弧，根据几何关系计算转弯半径，公式如下：

此时会出现以下两种情况：

1)R＞R_min

为了保证车辆在车位中的姿态角不要小于70°需要对结束点x坐标重新规划，假设整段路径中车身姿态角最大处的角度为a_M，按下式计算该角度：

若a_M大于70°则按照原规划结束点(0，0，90°)；

若小于90°，则令最大姿态角为70°重新规划第三段路径结束点的x坐标x_e：

x_e＝x₀-2·R·(1-sin(a_M))

此处x₀为开始规划第三段路径时车身后轴中心在虚拟坐标系中的x坐标；

2)R≤R_min

取R＝R_min，此时无法保证规划到x＝0，设规划到y＝0，尽可能避免规划路径中间出现车身姿态角过小的情况；

C2：α接近90°、|x_d|较大且x_s≤0

起始点位于x＝0的左侧，并且姿态角较大，此时姿态角为负值，为了计算方便，依然取正值，此时可以规划为两段圆弧，根据图14示几何关系计算转弯半径，根据下式计算转弯半径

此时会出现以下两种情况：

1)R＞R_min

为了保证车辆在车位中的姿态角不要小于70°，需要对结束点x坐标重新规划，假设整段路径中车身姿态角最大处的角度为a_M：

若a_M大于70°则按照原规划结束点(0，0，90°)；

若小于90°，则令最大姿态角为70°，重新规划第三段路径结束点结束点的x 坐标x_e：

x_e＝x₀-2·R·(1-sin(a_M))

此处x₀为开始规划第三段路径时车身后轴中心在虚拟坐标系中的x坐标；

2)R≤R_min

取R＝R_min，此时无法保证规划到x＝0，设规划到y＝0，确保车辆在车位的姿态角不要小于70°；

C3:α较小且x_s>0

首先选取一段圆弧，判断是否可以达到目标点(0，0，90°)，可以通过以图 15所示几何关系求得一段到位所需要的转弯半径R。

根据R值的大小分为以下两种情况：

1)R＞R_min

d_y为沿规划路径运动时y方向的运动距离。

如果满足d_y<y_s则按照一段圆弧规划，圆弧结束后接直线；

否则判断直线+圆弧的方案是否可行，按照最小转弯半径，计算所需的y方向距离，规划到结束点(0,0,90°)

设圆弧直线相切点为Z点，坐标为(x_z，y_z，a_z)，通过几何关系可求得直线+圆弧所需的纵向距离Y_se，计算方式如下式：

Y_se＝R_min·cos(a_s)+tan(a_s)·[x_s-R_min·(1-sin(a_s))]

若Y_se＜y_s，则直线+圆弧可行，否则则采用两段圆弧法，方法同C1；

2)R≤R_min

采用最小转弯半径，规划到y＝0，此时判断终点的x坐标，若超过单侧车位间隙的范围，则将终止点规划到车位间隙的范围内；

C4:α较小且x_s<0

直接采用最小转弯半径R_min进行路径规划。

本领域技术人员可以理解是，本发明中本车在虚拟坐标系中的位置，以车后轴中心在虚拟坐标系中的位置为准。另外，本发明涉及的长度单位均为厘米。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，熟悉该本领域的技术人员应该明白本发明包括但不限于上面具体实施方式中描述的内容。任何不偏离本发明的功能和结构原理的修改都将包括在权利要求书的范围中。

Claims (9)

1.一种垂直泊车的路径规划设计方法，其特征在于包括如下步骤：

S1，根据车辆参数和环境信息计算所需最小车位长度；

S2，比较实际车位长度和最小车位长度；

S3，根据步骤S2比较结果选择泊车方式，如果实际车位长度大于最小车位长度，采用一次泊车到位的路径规划设计，如果实际车位长度小于或者等于最小车位长度，采用多步泊车到位的路径规划设计；

一次泊车到位的路径规划，以车位中线R为基准确定各坐标参数，起始位置为车后轴中心距离中线R的位置；包含以下参数：W：车辆宽度，L：车身长度，R_min：最小转弯半径，C_x：侧向距离，b：后轴中心到车尾的距离，d₃：本车在车位里距离两侧的距离，d₁：车辆后轴最内点距离车位的安全距离，Δd：一次泊车到位约束段长度，L₂：超声波识别的实际车位长度，L_min：计算得到的所需最小车位长度，

上述参数具有以下几何关系：

L₂＝W+2·d₃

2.根据权利要求1所述的一种垂直泊车的路径规划设计方法，其特征在于：一次泊车到位的路径规划包括：采用最小转弯半径，根据最小侧向距离判断所需求的最小车位长度。

3.根据权利要求2所述的一种垂直泊车的路径规划设计方法，其特征在于：一次泊车到位的路径规划还包括：曲线段开始前需使用直线段过渡。

4.根据权利要求2所述的一种垂直泊车的路径规划设计方法，其特征在于：车辆两个侧后探头测到车辆进入车位后，开始对两侧车辆的车身姿态进行识别，在识别后重新规划路径，开始新的控制。

5.根据权利要求1所述的一种垂直泊车的路径规划设计方法，其特征在于：位于车辆侧边的超声波探头识别到侧向距离C_x和实际车位长度L₂后，如果L₂≥(L_min-10)，则选择一次泊车到位的路径规划设计，反之则选择多步泊车到位的路径规划设计。

6.根据权利要求5所述的一种垂直泊车的路径规划设计方法，其特征在于：一次泊车到位的路径规划设计还包括起始转弯点：设车辆的起始位置为A0，坐标为(x₀，y₀，a₀)则需要直行到位置A1后再开始沿圆弧运动，此时车辆运动轨迹为直线与相切圆弧组成，由于前一步计算最小车位长度时假设起始位置的姿态角为0，因此需要补足圆弧，设补足后虚拟的起始点为Ax，按照Ax位置规划最小车位，此时不确定结束点姿态角是否为90°，设结束点x坐标为0，A1状态对应的起始转弯点坐标为(x₁，y₁，a₁)，则虚拟点Ax坐标为(x_x，y_x，a_x)，根据几何关系应满足如下关系：

x_x＝x₁+R_min·sin(α₁)

y_x＝y₁+R_min·[1-cos(α₁)]

其中f表示后轴至车头的距离，也是车辆进入车位后车后轴中心距离车位水平线的距离，即车位障碍点的y坐标；

建立直行段的直线方程：

y₀-y₁＝tan(a₀)·(x₀-x₁)，

根据下式：

其中L_act为实际车位长度，将前一步的y_x代入此式即可求得起始转弯点A1的坐标x₁和y₁。

7.根据权利要求6所述的一种垂直泊车的路径规划设计方法，其特征在于：多步泊车到位的路径规划设计包括第一段路径规划，第一段路径规划包括上述的最小车位长度和起始转弯点，还包括：

右后碰撞点：当车辆的右后端点进入到车位水平线时，直角三角形B1和B2有公共的斜边，其中b为车辆后轴到车尾的距离即三角形B1的一个直角边，B1的另一条直角边为车身转弯圆圆心与车内侧后轮中心连线，B2的直角边由车身转弯圆圆心向车位水平线作垂线得到，该垂线与水平线的交点到车身右后端点的连线即为B2水平方向的直角边，令x为B2水平方向的直角边，根据几何关系可以推算得到x值：

判断x-(R_min-L_min/2)是否大于0，若大于0则右后碰撞点不会撞上障碍车；

左后碰撞点：当车辆的左后端点进入到车位水平线时，直角三角形B1和B2有公共的斜边，其中b为车辆后轴到车尾的距离即三角形B1的一个直角边，令x为B2水平方向的直角边，根据几何关系可以计算得到x值

此时判断x-(R_min-L_min/2)是否大于实际车位长度，若大于则左后端点可进入车位且姿态角可以达到临界值，

左后端点进入车位且姿态角可以达到临界值：若姿态角恰好可以到达临界值α，设定为75°，则可依据以下几何关系求出对应的最小车位：

α_B＝tan^-1(b/(R_min+W/2))

L_s1＝R_B·sin(α+α_B)+d₁-(R_min-L_min/2)，

其中，α_B为车身转弯圆心和后轴连线与圆心和车左后端点连线的夹角，R_B为车左后端点到圆心的距离，L_s1为姿态角刚好达到临界值需要的最小车位，

若结束点对应的姿态角无法达到临界值，则可以根据已知的车位长度按下式求出对应的姿态角α：

α＝sin^-1[(L_s1-d₁+x_s)/R_B]-α_B，

此处x_s为起始停车点到车位边沿的距离。

8.根据权利要求7所述的一种垂直泊车的路径规划设计方法，其特征在于：多步泊车到位的路径规划设计还包括第二段路径规划，

设第二段开始时车身后轴中心在虚拟坐标系中的x坐标为x_s，y坐标为y_s，a_s为第二段路径开始时车身的姿态角，即此时车身坐标为(x_s，y_s，a_s)，假设x_s≤0，第二段路径的目标结束点坐标为(x_e，y_e，a_e)，其中x_e＝0，a_e＝90°，

如果采用一段圆弧路径就到达目标位置，则该圆弧b必须满足与车位中线相切且转弯半径不得过大，过大会导致纵向的距离过大，此时计算圆弧半径R＝(-x_s)/[1-sin(a_s)]，会有如下两种情况：

1)R＜R_min

按照R_min半径规划路径，如果上到姿态角等于90°处车辆的位置没有超出车位，则停止点为姿态角等于90°的位置，如果超过了，则以x＝(车位长度-W)/2处的点为停止点；

2)R≥R_min

采用直线+圆弧法路径需要的纵向长度Y_se1可按照几何关系计算：

Y_se1＝R_min·cos(a_s)+tan(a_s)·[-x_s-R_min·(1-sin(a_s))]

采用两段圆弧法路径需要的纵向长度Y_se2可由几何关系计算：

若Y_se2大于Y_se1则选取直线+圆弧法的方式规划路径，反之则选取两段圆弧的方法规划路径。

9.根据权利要求8所述的一种垂直泊车的路径规划设计方法，其特征在于：多步泊车到位的路径规划设计还包括第三段路径规划，

开始规划第三段时，先判断当前车身姿态角α，再以当前姿态角过车辆后轴中点做轨迹延长直线，该直线与y＝0的交点横坐标，定为x_d，开始规划第三段路径时车身后轴中心在虚拟坐标系中的x坐标为x_s，y坐标为y_s，a_s为第三段路径开始时车身的姿态角；

C0：α接近90°且x_d几乎与车位中心重合，按照轨迹延长直线直接规划路径；

C1：α接近90°、|x_d|较大且x_s＞0，起始点位于x＝0的右侧，并且姿态角较大，此时可以规划为两段圆弧，根据几何关系计算转弯半径，公式如下：

此时会出现以下两种情况：

1)R＞R_min

为了保证车辆在车位中的姿态角不要小于70°需要对结束点x坐标重新规划，假设整段路径中车身姿态角最大处的角度为a_M，按下式计算该角度：

若a_M大于70°则按照原规划结束点(0，0，90°)；

若小于90°，则令最大姿态角为70°重新规划第三段路径结束点的x坐标x_e：

x_e＝x₀-2·R·(1-sin(a_M))

此处x₀为开始规划第三段路径时车身后轴中心在虚拟坐标系中的x坐标；

2)R≤R_min

取R＝R_min，此时无法保证规划到x＝0，设规划到y＝0，尽可能避免规划路径中间出现车身姿态角过小的情况；

C2：α接近90°、|x_d|较大且x_s≤0

起始点位于x＝0的左侧，并且姿态角较大，此时姿态角为负值，为了计算方便，依然取正值，此时可以规划为两段圆弧，根据下式计算转弯半径

此时会出现以下两种情况：

1)R＞R_min

为了保证车辆在车位中的姿态角不要小于70°，需要对结束点x坐标重新规划，假设整段路径中车身姿态角最大处的角度为a_M：

若a_M大于70°则按照原规划结束点(0，0，90°)；

若小于90°，则令最大姿态角为70°重新规划第三段路径结束点的x坐标x_e：

x_e＝x₀-2·R·(1-sin(a_M))

此处x₀为开始规划第三段路径时车身后轴中心在虚拟坐标系中的x坐标；

2)R≤R_min

取R＝R_min，此时无法保证规划到x＝0，设规划到y＝0，确保车辆在车位的姿态角不要小于70°；

C3：α较小且x_s＞0

首先选取一段圆弧，判断是否可以达到目标点(0，0，90°)，可以通过几何关系求得一段到位所需要的转弯半径R，根据R值的大小分为以下两种情况：

1)R＞R_min

d_y为沿规划路径运动时y方向的运动距离，

如果满足d_y＜y_s则按照一段圆弧规划，圆弧结束后接直线；

否则判断直线+圆弧的方案是否可行，按照最小转弯半径，计算所需的y方向距离，规划到结束点(0，0，90°)

设圆弧直线相切点为Z点，坐标为(x_z，y_z，a_z)，通过几何关系可求得直线+圆弧所需的纵向距离Y_se，计算方式如下式：

Y_se＝R_min·cos(a_s)+tan(a_s)·[x_s-R_min·(1-sin(a_s))]

若Y_se＜y_s，则直线+圆弧可行，否则则采用两段圆弧法，方法同C1；

2)R≤R_min

采用最小转弯半径，规划到y＝0，此时判断终点的x坐标，若超过单侧车位间隙的范围，则将终止点规划到车位间隙的范围内；

C4：α小于80°且x_s＜0，直接采用最小转弯半径R_min进行路径规划。

Images