CN104627175B - 一种人车交互智能泊车系统 - Google Patents

Abstract

本发明公开了一种人车交互智能泊车系统，该系统包括环境检测模块，路径规划模块以及运动控制模块；环境检测模块检测车辆和车位尺寸发送给路径规划模块；路径规划模块包括判定阶段、初始化阶段、自由规划阶段、最终规划阶段和判优阶段；路径规划模块根据车位尺寸规划出多条泊车路径，然后根据判优准则选出最优泊车路径；运动控制模块根据最优泊车路径计算车辆控制参数，完成自主泊车；本发明的泊车系统在各种泊车环境下，都能够产生泊车路径，并且能够规划出车轮转动过程的路径，使车辆在转动方向盘的过程中不需要停车，从而保证车辆行进过程的连续性。

Description

一种人车交互智能泊车系统

技术领域

本发明涉及自主泊车系统的技术领域，具体涉及一种人车交互智能泊车系统。

背景技术

众所周知，在车辆发展越来越发达的今天，车辆的智能化是车辆未来发展的一大趋势。而在城市车辆日渐增多，交通拥堵越发严重的情况下，在城市中的停车难度大大增加。很多司机都感觉很难驾驭泊车技术。因此，自主泊车系统有很强的市场需要性以及广阔的市场空间。

自主泊车系统就是车辆在人给予停车入位指令后，能够不用人工干预，自主完成停车入位的系统。该系统包括环境数据检测模块、路径规划分析模块和车辆运动控制模块三部分组成。

目前，自主泊车系统的路径规划策略主要有以下方法：

1.概率类搜索方法，例如快速搜索生成树(RRT)等。该算法针对整个环境进行路径规划，其基本思路在于以一定的指标在环境中选取路点，一段一段地规划路径，直至规划从起始状态到达终止状态的路径。该方法在各种情况下，只要存在能够倒入车位的路径就一定能规划出泊车路径。但这种方法存在一定的局限性：

(1)规划的路径唯一且为局部最优；

(2)规划时间不确定以及规划路径不确定；

(3)很少考虑车辆约束，有时会规划出不满足车辆约束的路径。

2.两分法。该方法设定停车准备位置，将泊车分为两个过程，将车辆行驶到 停车准备位置，然后再规划车辆倒车入库的路径。根据规划的路径，还可以将这一类路径规划方法细分为圆轨迹路径以及非圆轨迹路径。其中利用非圆轨迹路径规划路径主要包括对规划的圆轨迹路径进行优化，例如对圆轨迹路径进行插值优化；或者直接规划非圆轨迹路径，例如七次多项式曲线，Bezier曲线等。这一类算法简单易行速度快，但也有一定的不足：

(1)利用圆轨迹进行路径规划的算法普遍不考虑路径二阶导数连续问题，需要车辆在旋转方向盘的过程中停车，这对于车辆行驶过程中的连续性影响很大；部分算法只考虑最小转弯半径；

(2)这种算法只能生成一条路径；

(3)对于平行泊车，这种算法很少考虑狭小车位情况，对于狭小车位，该方法无法规划出路径。

3.择优法。该方法在整个停车位附近的环境中规划能够完成倒车入库的所有路径，择优选取最佳路径作为停车入位的路径。此类方法没有明确的停车准备位置，一般会设定路径单元的概念，例如将“直线-圆轨迹-直线”路径作为一个路径单元或者将“前进-后退”路径作为一个路径单元。设定最大路径单元数，然后在整个车辆行驶环境中进行路径规划，从所有路径单元数小于最大路径单元数的路径中选出一条最优路径作为泊车路径。这种算法规划的路径综合考虑各项性能指标，全局最优，较符合车辆停车入位。但现有的方法的处理速度不够快，并且利用圆轨迹进行规划的算法仍然存在一定缺陷：只考虑了最小转弯半径且不考虑路径二阶导数连续问题。

发明内容

有鉴于此，本发明提供了一种人车交互智能泊车系统，在各种泊车环境下，都能够产生泊车路径，并且能够规划出车轮转动过程的路径，使车辆在转动方 向盘的过程中不需要停车，从而保证车辆行进过程的连续性。

实现本发明的技术方案如下：

一种人车交互智能泊车系统，包括环境检测模块，路径规划模块以及运动控制模块；

环境检测模块检测停车位尺寸、车辆后轴中点的起始位置以及车辆起始航向角，然后发送给路径规划模块；

路径规划模块分为五个阶段，分别为：判定阶段、初始化阶段、自由规划阶段、最终规划阶段和判优阶段；

判定阶段：根据所述停车位尺寸、车辆后轴中点的起始位置、车辆起始航向角以及已知的车辆尺寸，自动判断当前车辆能否倒入车位，若不能倒入车位，则系统发出警告；若能倒入车位，则判断车辆是进行垂直泊车还是水平泊车；

初始化阶段：若车辆进行垂直泊车，在车位中选取至少一个车辆终点位置，以所选取的位置为起点，规划多条直线路径，并将所述多条直线路径的终点作为初始化阶段的输出位置；

若车辆进行水平泊车，则根据停车位尺寸以及车辆尺寸，判断是否存在第一路径，所述第一路径为使车辆不接触停车位的边沿并且能够在车辆行进的过程中进行车轮转向，最终到达车辆终点位置的路径；其中，在车轮转向中车辆的行进过程为过渡过程；

若存在第一路径，则在车位中寻找满足第一路径的车辆终点位置，并将其作为初始化阶段的输出位置；

若不存在第一路径，则在车位中选取车辆终点位置，以该位置为起点，规划多条行车路径，并将所述行车路径的终点作为初始化阶段的输出位置；

自由规划阶段：以初始化阶段的输出位置为起点，规划由若干段圆轨迹和过渡过程路径组成的多条路径，并将该规划路径的终点作为输出位置；

最终规划阶段：以自由规划阶段的输出位置为起点，以车辆起点位置为终点，规划包含过渡过程的多条路径；

判优阶段：根据判优准则从由初始化阶段、自由规划阶段及最终规划阶段所形成的、从车辆终点位置到车辆起点位置的多条路径中选出最优泊车路径，进入运动控制模块；

运动控制模块，根据最优泊车路径计算车辆控制参数，完成自主泊车。

进一步地，判定阶段判断车辆不能倒入车位的条件为：

(1)、L_lot＜(L_car+L_safe)且d_lot＜(L_car+L_safe)；

(2)、L_lot＜(d_car+d_safe)；

(3)、d_lot＜(d_car+d_safe)；

其中，L_lot为车位沿道路的长度，L_car为车辆长，L_safe为车尾与停车位边线之间的最短距离，d_lot为车位垂直于道路的长度，d_car为车辆宽，d_safe为车辆靠近路的一侧与停车位边线之间的最短距离；

满足以上三式中任意一个即可判定车辆不能倒入车位。

进一步地，判定阶段判断车辆进行垂直/水平泊车的条件为：

若d_lot≥L_lot，车辆进行垂直泊车；

若d_lot＜L_lot，车辆进行水平泊车。

进一步地，存在第一路径的条件为：

其中，L_rear为后悬，Δx为车辆起始状态为(0,0,0)，车轮与车辆夹角从0°旋转到最大转角θ_steer,max所产生的x方向的位移，Δy为车辆起始状态为(0,0,0)，车轮与车辆夹角从0°旋转到最大转角θ_steer,max所产生的y方向的位移，Δα^*为车辆起始状态为(0,0,0)，车轮与车辆夹角从0°旋转到最大转角θ_steer,max所产生的航向角的变化量；

r_i为车辆以最小转弯半径旋转过程中内侧后轮的半径，r_e为车辆以最小转弯半径旋转过程中车体外侧前顶点的半径；

其中，r为车辆后车轴中点的最小转弯半径，L_front为前悬，v为车辆的行驶速度大小，w为车轮与车辆夹角的角速度大小。

进一步地，满足第一路径条件的终点位置为：当车停在车辆终点位置处，车尾与停车位边线之间的距离为ΔL，车辆靠近路的一侧与停车位边线之间的距离为Δd；ΔL≥L_safe，Δd≥d_safe；当L_lot满足以下条件时，该终点位置即为满足第一路径条件的终点位置：

进一步地，本发明若不存在第一路径时，所进行的规划多条行车路径的过程为：

判断是否存在第二路径，所述第二路径为需要车辆停车进行车轮转向，但是不需要车辆在车位内进行前后调整来达到车辆终点位置的路径；若存在第二路径，则寻找所有满足第二路径的车辆终点位置，以所寻找的车辆终点位置为起点，规划最小转弯半径圆轨迹，以距离车辆终点位置最近的车轮切换位置为圆轨迹的终点，并将该终点位置作为初始化阶段的输出位置；

若不存在第二路径，选取ΔL＝L_safe，Δd＝d_safe的车辆位置作为车辆终点位置，以车辆终点位置为起点规划由若干段前进或后退的最小转弯半径圆轨迹组成的 曲线路径，且曲线路径的终点为车辆刚好驶出车位的位置，并将曲线路径的终点作为初始化阶段的输出位置。

进一步地，本发明所述最终规划阶段所规划的路径为由两个直线路径、两个过渡过程及一个圆轨迹组成的路径F点—E点—D点—C点—B点—A点，其中，F点为自由规划阶段的输出位置，A点为车辆起点位置，F、E点之间和B、A点之间的路径为直线路径，E、D点之间和C、B点之间的路径为过渡过程路径，D、C点之间的路径为圆轨迹；圆轨迹的半径大于等于最小转弯半径，选取不同半径的圆轨迹进行规划，对于不同的圆轨迹，其过渡过程不同。

进一步地，所述最终规划阶段圆轨迹圆心坐标(x_c,y_c)的计算方法为：

其中，假设A点与B点重合，C点的坐标为(x_st1,y_st1)，E点与F点重合，D点的坐标为(x_st2,y_st2)，根据确定的航向角和圆轨迹半径，(x_stc1,y_stc1)为以(x_st1,y_st1)为终点进行旋转的圆心，(x_stc2,y_stc2)为以(x_st2,y_st2)为起点进行旋转的圆心，车辆在A点处航向角的正切值为K₁，车辆在F点处航向角的正切值为K₂；

B点坐标(x₁,y₁)与E点坐标(x₂,y₂)的计算方法为：

其中，C点航向角的正切值为K_st1，D点航向角的正切值为K_st2；

通过过渡过程计算出C点与D点的坐标。

进一步地，所述判优准则为包括行驶过程中车辆与路缘的最短距离，旋转方向盘次数，停车次数，切换档位次数以及行驶距离的代价函数。

有益效果：

1、本发明所提供的智能泊车系统能根据车位情况，既可以规划车辆在行驶时转动方向盘的过渡路径，也可以选择在停车过程中旋转方向盘而不规划过渡路径。

2、可以智能判断垂直/水平泊车，并且可以根据判优函数选出最优路径。

3、规划的路径包括直线，半径大于等于最小转弯半径的圆轨迹，不同圆轨迹之间的过渡路径，比以前的路径规划方法更全面。

附图说明

图1为自主泊车系统示意图；

图2为车辆转向原理示意图；

图3为路径规划方法的流程图；

图4为车位尺寸以及车辆起始状态的参数示意图；

图5为车辆尺寸参数示意图；

图6为垂直泊车的路径初始化阶段示意图；

图7为水平泊车第一路径初始化阶段示意图；

图8为水平泊车第二路径初始化阶段示意图；

图9为第二路径初始化阶段车轮切换位置示意图；

图10为水平泊车第三路径初始化阶段示意图；

图11为图10所示路径的自由路径规划阶段示意图；

图12为图11所示路径的最终规划阶段示意图；

图13为最终规划阶段计算过程示意图；

图14为垂直泊车效果图；

图15为水平泊车效果图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明提供了一种人车交互智能泊车系统，图1为该系统示意图，该系统包括环境检测模块，路径规划模块以及运动控制模块；

路径规划模块分为五个阶段，分别为：判定阶段、初始化阶段、自由规划阶段、最终规划阶段和判优阶段；路径规划模块的规划顺序与车辆实际泊车顺序是相反的，路径规划模块规划的是从车辆终点位置到车辆起点位置的路径，而车辆实际泊车顺序是从车辆起点位置到车辆终点位置。

路径规划模块的流程图如图3所示，用户下达泊车命令以及泊车模式后，环境检测模块检测停车位尺寸、车辆后轴中点的起始位置以及车辆起始航向角；

当路径规划模块接收到环境检测信息后，返回一个表示接收到信息的标志位，判定阶段：路径规划模块根据环境检测模块输出的停车位尺寸、车辆后轴中点的起始位置、车辆起始航向角以及已知的车辆尺寸，自动判断当前车位能否进行停车泊位，车位参数以及车辆起始状态示意图如图4所示，车位沿道路的长度为L_lot，车位垂直于道路的长度为d_lot，可用于车辆进行自主泊车的道路宽度为d_road，车辆尺寸参数示意图如图5所示，车辆长为L_car，车辆宽为d_car，前悬为L_front，后悬为L_rear。车尾与停车位边线之间的最短距离为L_safe，车辆靠近路的一侧与停车位边线之间的最短距离为d_safe，判定阶段判断车辆不能倒入车位的条件为：

(1)、L_lot＜(L_car+L_safe)且d_lot＜(L_car+L_safe)；

(2)、L_lot＜(d_car+d_safe)；

(3)、d_lot＜(d_car+d_safe)；

满足以上三式中任意一个即判定车辆不能倒入车位。

在车辆能够倒入车位的情况下，若d_lot≥L_lot，车辆进行垂直泊车；

在车辆能够倒入车位的情况下，若d_lot＜L_lot，车辆进行水平泊车；

初始化阶段：若车辆进行垂直泊车，则初始化阶段规划一段直线路径，该直线路径的起点为车辆终点位置，终点为初始化阶段的输出位置，选取能使车停入车位的点(如车位中点)作为车辆终点位置，所述车辆终点位置即为车辆后轴中点的终点位置，选取车辆直线行驶能够到达的位置作为初始化阶段的输出位置；存在一个以上的初始化阶段的输出位置；然后进入自由规划阶段；

定义车辆的状态为(x,y,θ)，其中y和x为车辆后轴中点的坐标，θ为车辆航向角；

以图6所绘制的路径为例，系统根据车位信息，选定终点状态为(1000,1050,-90)，经过直线路径后，车辆位置为一个集合，其横坐标为1000，纵坐标从(L_car-L_rear+L_safe)到终点纵坐标，航向角为-90°，图6中车辆起点状态为(1000,750,-90)经过直线路径后，行驶至终点状态(1000,1050,-90)。

若车辆进行水平泊车，则初始化阶段根据停车位尺寸以及车辆尺寸，判断是否存在第一路径，所述第一路径即车位足够长，使车辆不接触停车位的边沿并且能够在车辆行进的过程中进行车轮转向，最终到达车辆终点位置的路径；其中，在车轮转向中车辆的行进过程为过渡过程；

过渡过程车辆的路径推导为：

图2为车辆转向原理示意图。定义车辆的位置为车辆后轴中点的坐标(x,y)，最小转弯半径如图r_min是指当转向盘转到极限位置，汽车以最低稳定车速转向行 驶时，外侧转向轮的中心平面在支承平面上滚过的轨迹圆半径。根据r_min可以计算车辆后车轴中点的最小转弯半径r，车辆以最小转弯半径旋转过程中内侧后轮的半径r_i以及车辆以最小转弯半径旋转过程中车体外侧前顶点的半径r_e为：

在程序运行过程中以不同半径旋转是由车轮与车辆所形成的夹角不同造成的，在车辆转向的过程中，外侧车轮形成的角度较小为θ_steer2，半径大；内侧车轮形成的角度较大为θ_steer1，半径小。将其转换为前轮轴中点的角度θ_steer，其大小为：

通过不同的θ_steer来达到不同旋转半径的目的。当车轮与车辆所成夹角由θ_steerA变为θ_steerB的过程中，车辆航向角以及车辆位置会发生变化。设定车辆起始状态为(x_ini,y_ini,θ_ini)，过渡过程车辆所行驶的路程为S_total，当车轮与车辆所成夹角由θ_steerA变为θ_steerB后，车辆航向角的变化量为Δα，车辆的新位置为(x,y)，即车辆经过过渡过程之后的状态为(x,y,θ_ini+Δα)。

为了方便讨论假定车轮夹角变化为匀速，车辆行驶速度也为匀速，即有

其中，t为时间，s为路程，C为常值，由(4)可知

解微分方程(7)可得：

其中，r₀为车辆在进行车轮转向前的转弯半径，即在式(8)以及式(9)中，当F(s)＝C·s＝0时，可见r＝r₀。F(s)为的原函数， 则有

接下来给出x,y的计算过程，(cx,cy)为圆心，(Dx,Dy)为终点坐标，(Px,Py,θ₁)，(Dx,Dy,θ₂)为车辆行驶前以及车辆旋转一个小角度后的车辆状态，根据微积分思路，在一个小角度旋转过程中，认为半径r不变。当θ_steer不变时，有

其中，θ₁＝θ_ini+Δα，将Dx,Dy式中的cx,cy替换，代入θ₁,θ₂可得

积分可得

其中，(ini_x,ini_y)为进行方向盘转动前的位置。

由此，可以获得车辆航向角的变化量为Δα，车辆的新位置为(x,y)。

当车位足够长，存在第一路径的条件为：

其中，Δx为车辆起始状态为(0,0,0)，车轮与车辆夹角从0°旋转到最大转角θ_steer,max所产生的x方向的位移，Δy为车辆起始状态为(0,0,0)，车轮与车辆夹角从0°旋转到最大转角θ_steer,max所产生的y方向的位移，Δα^*为车辆起始状态为(0,0,0)，车轮与车辆夹角从0°旋转到最大转角θ_steer,max所产生的航向角的变化量；

Δx，Δy，Δα^*应用式(10)以及式(13)进行推导如下：

根据式(5)：假设车轮与车辆夹角的角速度的大小为w，车辆行驶速度大小为v，有车轮与车辆夹角从0°旋转到θ_steer,max所行驶的路程为s_total0；

已知r₀＝+∞，由式(5)可计算出将C与s_total0带入式(10)中可得：

带入式(13)中可得：

图7为第一路径示意图，图中, 其主要思路在于求出车辆旋转圆心与车辆最终停车位置以及车位的相对位置。

在车辆行进中，车辆能够不接触停车位的边沿并且能够在车辆行进的过程中进行车轮转向由C₇点旋转到B₇点，弧B₇C₇为一段圆弧，弧A₇B₇为车辆由B₇点过渡行驶到A₇点，最终到达车辆终点位置。

若存在第一路径，则说明至少存在一个车辆终点位置可以使车辆能够不接 触停车位的边沿并且能够在车辆行进的过程中进行车轮转向而到达。然后，在车位中寻找满足第一路径条件的车辆终点位置，当车停在车辆终点位置处，车尾与停车位边线之间的距离为ΔL，车辆靠近路的一侧与停车位边线之间的距离为Δd；ΔL≥L_safe，Δd≥d_safe；当L_lot满足以下条件时，该终点位置即为满足第一路径条件的车辆终点位置：

存在一个以上满足第一路径条件的车辆终点位置，每一个车辆终点位置均作为初始化阶段的输出位置，进入自由规划阶段；

若不存在第一路径，则判断是否存在第二路径，所述第二路径即车位长度中等，需要车辆停车进行车轮转向，但是不需要车辆在车位内进行前后调整来达到车辆终点位置的路径；

如图8所示，L_B＝L_rear，L_A＝L_safe。当车位长度满足时，存在第二路径，即车辆在停车状态下进行车轮转向，车辆能够不接触停车位的边沿，最终到达车辆终点位置。

若存在第二路径，则在车位中寻找满足第二路径条件的车辆终点位置，存在一个以上满足第二路径条件的车辆终点位置，对每个车辆终点位置均规划最小转弯半径圆轨迹，每个规划出的圆轨迹的起点为不同的车辆终点位置，终点为不同的距离车辆终点位置最近的车轮切换位置，距离车辆终点位置最近的车轮切换位置为初始化阶段的输出位置，然后进入自由路径规划阶段。

以下为结合附图对第二路径进行详细说明：

根据图8，由于车辆向车位中间靠拢，相当于d_safe扩大到Δd，L_safe(即图中的L_A)扩大到ΔL。第二路径其判断条件如下：

当车位的长度满足上式时，即存在第二路径，如果存在第二路径，则初始化阶段规划的路径为车辆终点位置—最小转弯半径圆轨迹—距离车辆终点位置最近的车轮切换位置，然后进入自由路径规划阶段。其中，车轮切换位置为车辆在该点处停车向相反方向打轮的位置。车轮切换位置的确定如图9所示，其中，在倒入至车辆终点位置过程的圆轨迹E₉F₉，车辆的内侧顶点D₉可能与车位顶点A₉相碰，在车辆旋转过程中，车体可以看做由无数个点组合而成的，每个点都是以同一个圆心、不同半径进行旋转，以C₉为圆心，D₉半径为r＝r_i2，对于圆轨迹D₉E₉，圆心为点O₉，顶点D₉半径为r＝r_i1+d_car。其中r_i2≥r_i，r_i1＝r_i。如图9所示，求出交点C₉，连接O₉C₉，与过D₉点以O为圆心，半径为r_i1+d_car的圆轨迹的交点即为车轮切换位置。注意到，A₉C₉＝C₉E₉＝r_i2，因此该位置(图9的E₉点)为距离车辆终点位置最近的车轮切换位置。在停车的状态下，将车轮角度旋转到指定角度后，可继续沿以C₉为圆心，半径为r_i2的圆轨迹E₉F₉行驶。

图9所示的情况是两圆交点位于车位顶点A₉的右侧，两圆交点C₉即为圆轨迹E₉F₉段的圆心，过D₉点以O₉为圆心，半径为r_i1+d_car的圆轨迹与直线O₉C₉的交点E₉即为车辆车轮的切换位置。当两圆交点位于车位顶点的左侧，或者两圆不存在交点时，则车辆车轮切换位置如下：

当无交点或交点在顶点左侧时，则有图9所示的F₉点并不在顶点的右侧，因此只需要两段路径在垂直于道路方向上的位移大于车辆宽度与车辆边沿与路边的距离之和就可以保证车辆能够倒入车位且不需要考虑车辆能否与车位顶点A₉相碰。

即(r_i1+d_car+r_i2)·(1-cosθ_min)＝d_car+Δd，

综上所述，根据图9，可以看出，若|O₉A₉|＞r_i1+d_car+2r_i2，则无交点；若|B₉A₉|≥r_i2，则交点在顶点右侧；若|B₉A₉|＜r_i2，则交点在顶点左侧。

两圆交点位于车位顶点A₉的右侧，车辆车轮切换位置即为两圆交点C₉，O₉C₉与过D₉点以O为圆心，半径为r_i1+d_car的圆轨迹的交点。当两圆交点位于车位顶点的左侧或者两圆不存在交点时，

当车位比较狭窄，不存在第二路径时，则规划第三路径，所述第三路径为车辆需在车位内进行前后调整才能达到车辆终点位置的路径，所规划的第三路径为由若干段前进或后退的最小转弯半径圆轨迹组成的圆轨迹组，圆轨迹组的起点为车辆终点位置，圆轨迹组的终点为车辆刚好驶出车位的位置；选取ΔL＝L_safe，Δd＝d_safe的车辆位置作为车辆终点位置，车辆刚好驶出车位的位置为初始化阶段的输出位置；

下面结合附图对第三路经进行说明：

如图10所示，车位较为狭窄需要车辆在车位内进行前进/后退调整过程，在规划时，车辆从A₁₀点前进到B₁₀点，然后退至C₁₀点，最后前进到D₁₀点。

初始化阶段的输出位置D₁₀点的确定准则如下：由车辆终点位置起开始以最小转弯半径向车位外旋转；当车辆距离车位小于必要的安全距离时，停车切换为倒车档，并将车轮反向打满进行倒退；当车辆距离车位小于必要的安全距离时，停车切换为前进档。如此循环，直至当车辆前进旋转的外顶点(图10中点)已经移出车位。

自由规划阶段规划由若干段圆轨迹和过渡过程路径组成的多条路径，该路径的起点为初始化阶段的输出位置，终点为自由规划阶段的输出位置，然后进入最终规划阶段；所述若干段圆轨迹由于半径不同、方向不同而存在多种组合，相邻两段圆轨迹之间规划过渡过程路径，其中，最后一段圆轨迹的终点位置即为自由规划阶段的输出位置；由于存在一个以上的初始化阶段的输出位置以及 多种自由规划路径，自由规划阶段的输出位置为一个以上。

自由规划阶段利用曲线路径规划，利用半径较大的圆轨迹(车轮与车辆夹角为1°)来近似直线路径。该阶段中每旋转一次方向盘或切换一次前进/后退档位，迭代次数+1，每次迭代结束时的车辆位置为一个以上，从第一次迭代到最后一次迭代所得到的车辆位置均作为自由规划阶段的输出位置，当迭代次数等于设定的最大迭代次数时(即完成最后一次迭代时)，结束自由规划阶段，进入最终规划阶段。

在两次相邻的迭代过程中，两次的方向盘转角不同，两次车轮与车辆夹角不同，两次不同半径的路径之间是否存在过渡过程的路径都需要进行规划。即既可以在车辆行驶的过程中转动方向盘，也可以先停车，然后在停车状态下转动方向盘。

图11为图10所示的路径继续进行自由规划阶段所规划的路径，如图11所示，图中的A₁₁点即为图10中的D₁₀点，首先车辆以车轮与车辆夹角17°从C₁₁点进行旋转，旋转到B₁₁位置，在B₁₁位置停车旋转方向盘直至车轮与车辆夹角呈-33°，再由B₁₁点旋转到A₁₁点与第一阶段即图10中的D₁₀点重合。本例由于道路比较狭窄，因此选择了停车进行车轮旋转。

最终规划阶段规划车辆从自由规划阶段的输出位置到车辆起点位置的路径，该路径为由两个直线路径、两个过渡过程及一个圆轨迹组成的路径F点—E点—D点—C点—B点—A点，其中，F点为自由规划阶段的输出位置，A点为车辆起点位置，F、E点之间和B、A点之间的路径为直线路径，E、D点之间和C、B点之间的路径为过渡过程路径，D、C点之间的路径为圆轨迹；图12为图11所示路径继续进行最终规划阶段所规划的路径，如图12所示，车辆先由C₁₂点经过直线路径—过渡路径—旋转路径—过渡路径—直线路径到达B₁₂点，然后采用行进中旋转车轮的方法，将车轮旋转到17°，车辆经过过渡路径达到第二阶段 即图11中的的C₁₁点。对于直线路径-过渡路径-旋转路径-过渡路径-直线路径部分，图13为最终规划阶段路径示意图，如图13所示，车辆的起始位置位于A点，而第三阶段将车轮与车辆夹角变为0°之后的车辆状态为F点，车辆从A点经过直线路径到达B点，经过过渡过程路径到达C点，经过旋转路径到达D点，经过过渡过程路径到达E点，最后经过直线路径到达F点。其中，C点可以和B点重合，E点也可以和F点重合，即可以不规划过渡过程路径。当旋转路径的半径确定时，一定存在上述路径，计算B点以及E点的坐标方法如下：

设车辆在A点处航向角为θ₁，斜率为K₁＝tanθ₁；车辆在F点处航向角为θ₂，斜率为K₂＝tanθ₂。经过过渡过程路径后C点方向的斜率为K_st1，旋转后的D点的方向的斜率为K_st2。A点的坐标为(x_p,y_p)，从A点行驶转动车轮后的C₁坐标为(x_st1,y_st1)，可以看出直线CC₁与直线AB平行；从F点行驶转动车轮后的D₁坐标为(x_st2,y_st2)，F点的坐标为(x_d,y_d)，可以看出直线DD₁与直线EF平行。要求出圆心O点的坐标为(x_c,y_c)，B点坐标(x₁,y₁)，E点坐标(x₂,y₂)。

其中，B点和C点分别为第一段过渡过程的起点和终点，D点和E点分别为第二段过渡过程的起点和终点；圆轨迹的半径大于等于最小转弯半径，选取不同半径的圆轨迹进行规划。对于不同的圆轨迹，其过渡过程不同。

圆轨迹圆心坐标(x_c,y_c)的计算方法为：

可以看出直线O₁C₁与直线OC平行，直线O₂D₁与直线OD平行。求两直线交点可得圆心O坐标：

其中，对于某一半径圆轨迹，其过渡过程中位置以及航向角的偏移量是确定的，因此，C点和D点的航向角为确定的，假设A点与B点重合，C点的坐标为(x_st1,y_st1)，E点与F点重合，D点的坐标为(x_st2,y_st2)，根据确定的航向角和圆轨迹半径，O₁(x_stc1,y_stc1)为以(x_st1,y_st1)为终点进行旋转的圆心，O₂(x_stc2,y_stc2)为以(x_st2,y_st2)为起点进行旋转的圆心。

由直线CC₁与直线AB平行，直线DD₁与直线EF平行，经过过渡路径后C点方向与直线OC垂直，旋转后的D点的方向与直线OD垂直，B点坐标以及E点坐标计算方法如下式所示：

解上式可得，B点的坐标(x₁,y₁)与E点的坐标(x₂,y₂)；

其中，C点航向角的正切值为K_st1，D点航向角的正切值为K_st2；

通过过渡过程计算出C点与D点的坐标；

当选择不规划BC段过渡路径时，有x_st1＝x_p，y_st1＝y_p，K_st1＝K₁，带入上式计算即可获得路径。

当选择不规划DE段过渡路径时，有x_st2＝x_d，y_st2＝y_d，K_st2＝K₂，带入上式 计算即可获得路径。

判优阶段：根据判优准则从由初始化阶段、自由规划阶段及最终规划阶段所形成的、从车辆终点位置到车辆起点位置的多条路径中选出最优泊车路径，进入运动控制模块；

运动控制模块根据最优泊车路径计算车辆控制参数，完成自主泊车。

所述判优准则为包括行驶过程中与路缘的最短距离，旋转方向盘次数，停车次数，切换档位次数以及行驶距离的代价函数。该代价函数为路径规划指标的加权和，表达式如下：

其中，d_{min_obs}为行驶过程中车辆距离路缘的最短距离，N_steer为行驶过程中前进/后退切换次数，N_stop为行驶过程中停车的次数，D_total为行驶的总距离，N_manu为行驶过程中方向盘转动的次数，α、β、μ、λ和σ均为权重，选取代价函数值最小的路径作为最优路径。图14为垂直泊车路径效果，图15为水平泊车路径效果。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种人车交互智能泊车系统，其特征在于，包括环境检测模块，路径规划模块以及运动控制模块；

环境检测模块检测车位尺寸、车辆后轴中点的起始位置以及车辆起始航向角，然后发送给路径规划模块；

路径规划模块实现的功能分为五个阶段，分别为：判定阶段、初始化阶段、自由规划阶段、最终规划阶段和判优阶段；

判定阶段：根据所述车位尺寸、车辆后轴中点的起始位置、车辆起始航向角以及已知的车辆尺寸，自动判断当前车辆能否倒入车位，若不能倒入车位，则系统发出警告；若能倒入车位，则判断车辆是进行垂直泊车还是水平泊车；

初始化阶段：若车辆进行垂直泊车，在车位中选取至少一个车辆终点位置，以所选取的位置为起点，规划多条直线路径，并将所述多条直线路径的终点作为初始化阶段的输出位置；

若车辆进行水平泊车，则根据车位尺寸以及车辆尺寸，判断是否存在第一路径，所述第一路径为使车辆不接触车位的边沿并且能够在车辆行进的过程中进行车轮转向，最终到达车辆终点位置的路径；其中，在车轮转向中车辆的行进过程为过渡过程；

若存在第一路径，则在车位中寻找满足第一路径的车辆终点位置，并将其作为初始化阶段的输出位置；

若不存在第一路径，则在车位中选取车辆终点位置，以该位置为起点，规划多条行车路径，并将所述行车路径的终点作为初始化阶段的输出位置；

自由规划阶段：以初始化阶段的输出位置为起点，规划由若干段圆轨迹和过渡过程路径组成的多条路径，并将该规划路径的终点作为输出位置；

最终规划阶段：以自由规划阶段的输出位置为起点，以车辆起点位置为终点，规划包含过渡过程的多条路径；

判优阶段：根据判优准则从由初始化阶段、自由规划阶段及最终规划阶段所形成的、从车辆终点位置到车辆起点位置的多条路径中选出最优泊车路径，进入运动控制模块；

运动控制模块，根据最优泊车路径计算车辆控制参数，完成自主泊车。

2.如权利要求1所述的一种人车交互智能泊车系统，其特征在于，判定阶段判断车辆不能倒入车位的条件为：

(1)、L_lot＜(L_car+L_safe)且d_lot＜(L_car+L_safe)；

(2)、L_lot＜(d_car+d_safe)；

(3)、d_lot＜(d_car+d_safe)；

其中，L_lot为车位沿道路的长度，L_car为车辆长，L_safe为车尾与车位边线之间的最短距离，d_lot为车位垂直于道路的长度，d_car为车辆宽，d_safe为车辆靠近路的一侧与车位边线之间的最短距离；

满足以上三式中任意一个即判定车辆不能倒入车位。

3.如权利要求1所述的一种人车交互智能泊车系统，其特征在于，判定阶段判断车辆进行垂直/水平泊车的条件为：

若d_lot≥L_lot，车辆进行垂直泊车；

若d_lot＜L_lot，车辆进行水平泊车。

4.如权利要求1所述的一种人车交互智能泊车系统，其特征在于，存在第一路径的条件为：

$L_{lot}>L_{rear}+L_{safe}+\mathrm{\Δ}x-\frac{d_{car}}{2}\·{\mathrm{sin\Δ\α}}^{*}-r_i\·{\mathrm{sin\Δ\α}}^{*}+\sqrt{r_e^2-{\left(r_i\·{\mathrm{cos\Δ\α}}^{*}+\mathrm{\Δ}y+\frac{d_{car}}{2}\·\left({\mathrm{cos\Δ\α}}^{*}-1\right)-d_{safe}\right)}^2}$

其中，L_rear为后悬，Δx为车辆起始状态为(0,0,0)，车轮与车辆夹角从0°旋转到最大转角θ_steer,max所产生的x方向的位移，Δy为车辆起始状态为(0,0,0)，车轮与车辆夹角从0°旋转到最大转角θ_steer,max所产生的y方向的位移，Δα^*为车辆起始状态为(0,0,0)，车轮与车辆夹角从0°旋转到最大转角θ_steer,max所产生的航向角的变化量；r_i为车辆以最小转弯半径旋转过程中内侧后轮的半径，r_e为车辆以最小转弯半径旋转过程中车体外侧前顶点的半径；

${\mathrm{\Δ\α}}^{*}=\∫\frac{1}{r}ds=-\frac{v}{(L_{car}-L_{front}-L_{rear})w}ln\left|\sin (90-{\mathrm{\θ}}_{steer,max})\right|$

$\left\{\begin{array}{c}\mathrm{\Δ}x={\∫}_0^{\frac{{\mathrm{\θ}}_{steer,\max }\·v}{w}}\cos \left({\∫}_0^s\frac{1}{s}ds\right)ds\\ \mathrm{\Δ}y={\∫}_0^{\frac{{\mathrm{\θ}}_{steer,\max }\·v}{w}}\sin \left({\∫}_0^s\frac{1}{s}ds\right)ds\end{array}\right.$

其中，r为车辆后车轴中点的最小转弯半径，L_front为前悬，v为车辆的行驶速度大小，w为车轮与车辆夹角的角速度大小，s为路程。

5.如权利要求1所述的一种人车交互智能泊车系统，其特征在于，满足第一路径条件的终点位置为：当车停在车辆终点位置处，车尾与车位边线之间的距离为ΔL，车辆靠近路的一侧与车位边线之间的距离为Δd；ΔL≥L_safe，Δd≥d_safe；当L_lot满足以下条件时，该终点位置即为满足第一路径条件的终点位置：

$L_{lot}>L_{rear}+\mathrm{\Δ}x-\frac{d_{car}}{2}\·{\mathrm{sin\Δ\α}}^{*}-r_i\·{\mathrm{sin\Δ\α}}^{*}+\sqrt{r_e^2-{\left(r_i\·{\mathrm{cos\Δ\α}}^{*}+\mathrm{\Δ}y+\frac{d_{car}}{2}\·\left({\mathrm{cos\Δ\α}}^{*}-1\right)-\mathrm{\Δ}d\right)}^2}+\mathrm{\Δ}L.$

6.如权利要求1所述的一种人车交互智能泊车系统，其特征在于，若不存在第一路径，所进行的规划多条行车路径的过程为：

判断是否存在第二路径，所述第二路径为需要车辆停车进行车轮转向，但是不需要车辆在车位内进行前后调整来达到车辆终点位置的路径；若存在第二路径，则寻找所有满足第二路径的车辆终点位置，以所寻找的车辆终点位置为起点，规划最小转弯半径圆轨迹，以距离车辆终点位置最近的车轮切换位置为圆轨迹的终点，并将该终点位置作为初始化阶段的输出位置；

若不存在第二路径，选取ΔL＝L_safe，Δd＝d_safe的车辆位置作为车辆终点位置，以车辆终点位置为起点规划由若干段前进或后退的最小转弯半径圆轨迹组成的曲线路径，且曲线路径的终点为车辆刚好驶出车位的位置，并将曲线路径的终点作为初始化阶段的输出位置。

7.如权利要求1所述的一种人车交互智能泊车系统，其特征在于，所述最终规划阶段所规划的路径为由两个直线路径、两个过渡过程及一个圆轨迹组成的路径F点—E点—D点—C点—B点—A点，其中，F点为自由规划阶段的输出位置，A点为车辆起点位置，F、E点之间和B、A点之间的路径为直线路径，E、D点之间和C、B点之间的路径为过渡过程路径，D、C点之间的路径为圆轨迹；圆轨迹的半径大于等于最小转弯半径，选取不同半径的圆轨迹进行规划。

8.如权利要求7所述的一种人车交互智能泊车系统，其特征在于，所述最终规划阶段圆轨迹圆心坐标(x_c,y_c)的计算方法为：

$\left\{\begin{array}{c}{x}_c=\frac{-y_{stc2}+K_2\·x_{stc2}+y_{stc1}-K_1\·x_{stc1}}{K_2-K_1}\\ y_c=\frac{K_2\·(y_{stc1}-K_1\·x_{stc1})-K_1\·(y_{stc2}-K_2\·x_{stc2})}{K_2-K_1}\end{array}\right.$

其中，假设A点与B点重合，C点的坐标为(x_st1,y_st1)，E点与F点重合，D点的坐标为(x_st2,y_st2)，根据确定的航向角和圆轨迹半径，(x_stc1,y_stc1)为以(x_st1,y_st1)为终点进行旋转的圆心，(x_stc2,y_stc2)为以(x_st2,y_st2)为起点进行旋转的圆心，车辆在A点处航向角的正切值为K₁，车辆在F点处航向角的正切值为K₂；

B点坐标(x₁,y₁)与E点坐标(x₂,y₂)的计算方法为：

$\left\{\begin{array}{c}{x}_1=\frac{(x_c+y_c\·K_{st1}-y_{st1}\·K_{st1}+x_{st1}\·K_1\·K_{st1})}{1+K_1\·K_{st1}}\\ x_2=\frac{(x_c+y_c\·K_{st2}-y_{st2}\·K_{st2}+x_{st2}\·K_2\·K_{st2})}{1+K_2\·K_{st2}}\\ y_1=\frac{(y_{st1}+x_c\·K_1-x_{st1}\·K_1+y_c\·K_1\·K_{st1})}{1+K_1\·K_{st1}}\\ y_2=\frac{(y_{st2}+x_c\·K_2-x_{st2}\·K_2+y_c\·K_2\·K_{st2})}{1+K_2\·K_{st2}}\end{array}\right.$

其中，C点航向角的正切值为K_st1，D点航向角的正切值为K_st2；

通过过渡过程计算出C点与D点的坐标。

9.如权利要求1所述的一种人车交互智能泊车系统，其特征在于，所述判优准则为包括行驶过程中车辆与路缘的最短距离，旋转方向盘次数，停车次数，切换档位次数以及行驶距离的代价函数。