CN102963355A - 一种智能辅助泊车方法及其实现系统 - Google Patents

Abstract

一种智能辅助泊车方法，对泊车路径分两部分进行规划，首先规划出库位内调整路径，然后规划出入库路径；并提供人机交互界面，接受驾驶员意图，显示泊车信息；一种实现所述方法的智能辅助泊车系统，该系统包括相互之间具有通信联系的环境感知单元、路径规划单元、驾驶控制单元和人机接口单元。本发明能够从任意起始位姿引导车辆泊入停车位，能够降低泊车过程中车辆原地转向和前后运动方向改变的次数；平行泊车情况下，可根据驾驶员设置的库位内允许最大调整次数选择合适的停车位，降低车辆对平行泊车库位长度的要求。

Description

一种智能辅助泊车方法及其实现系统

技术领域

本发明属于车辆控制技术领域，涉及驾驶辅助技术，特别是汽车的智能辅助泊车技术。

背景技术

由于城市汽车保有量的增加，停车位紧张的问题日益突出，泊车已经成为城市交通中的一个难点问题，由于泊车时需要驾驶员在狭小紧凑的空间中观察和判断周边环境，短时间内同时控制方向盘、油门和刹车，对其驾驶技巧和反应灵敏度提出了很高要求，因此有必要开发辅助泊车系统，帮助驾驶员完成泊车操作。

智能泊车或是辅助泊车系统已经成为国内外的研究重点，智能泊车系统一般包括感知系统，控制系统和执行系统等。而控制系统是智能泊车系统的核心，其主要是根据传感器信息以及驾驶员操纵信息，对车辆的状态进行调整，使车辆在泊车的过程中找到一条合适的路径，引导车辆完成泊车操作。

目前智能泊车的控制算法主要分为以下几类：路径规划和跟随算法，它的基本思想是路径规划模块根据起始位姿和目标位姿以及障碍物的情况规划出可行的路径，路径跟随模块控制车辆跟随规划路径，最终使车辆驶入泊车位，这种方法算法简单，容易实现且计算成本较低；基于经验的智能算法，它依据人类驾驶员的泊车经验制定控制策略，控制车辆泊车入库，一般包括模糊控制、神经网络、遗传算法及粒子群算法等，这些智能算法能够较好地解决非线性问题，但是适应性差，控制规则制定复杂，并且计算成本高；姿态稳定算法是通过稳定性分析，使车辆的姿态收敛到零，从而逐步控制车辆达到目标位姿，这种算法具有较高的非线性且计算量大。

泊车的难点可以归纳为：一、车辆在泊车过程中不能与前后的障碍物车辆发生碰撞；二、车辆在泊车过程中，车辆运动不能超过车辆本身的物理约束条件。泊车路径规划模块的主要任务是要解决这两个关键问题；另外，在泊车过程中考虑到舒适性和安全性等因素，路径规划时还要考虑车辆前后运动方向的改变次数、原地转向次数以及泊车路径长度等因素。

因此，针对辅助泊车系统，开发实用高效的路径规划方法具有重要意义。

发明内容

本发明的目的在于提供一种智能辅助泊车系统，当环境感知系统识别到可用于泊车的库位后，规划出合理的泊车路径，并控制车辆沿规划路径行驶，直到车辆完成泊车动作，并提供人机交互界面，接受驾驶员意图，显示泊车信息。

为达到以上目的，本发明所采用的解决方案是：

一种智能辅助泊车系统，包括：

环境感知单元，用于获取库位信息和车辆的位姿信息；

路径规划单元，用于规划出合适的泊车路径；

驾驶控制单元，用于控制车辆沿给定路径行驶；

人机接口单元，用于人机交互，包含驾驶员信息输入和系统信息输出。

该路径规划单元包括两个模块：

（1）库位内调整路径规划模块，规划车辆在库位内进行调整时的路径；

（2）入库路径规划模块，根据车辆的起始位姿规划入库的泊车路径；

该人机接口单元提供驾驶员意图输入接口，驾驶员可以设定平行泊车情况下允许的最大库位内调整次数；垂直泊车情况下，不允许车辆进行库位内调整。

在环境感知单元探测到库位大小后，该路径规划单元可以在平行泊车情况下计算出泊入该库位所需的库位内调整次数，当计算得到的库位内调整次数大于驾驶员设定次数时，放弃该库位。

当计算得到的库位内调整次数不大于驾驶员设定次数时，该路径规划单元首先规划出库位内调整路径，然后规划出入库路径。库位内调整路径的起点为第一目标位姿B，终点为最终目标位姿G。库位内调整路径以最小转弯半径圆弧连接第一目标位姿B和最终目标位姿G；入库路径连接车辆的起始位姿S和第一目标位姿B。

库位内调整路径规划模块由限制方程生成子模块和优化求解子模块构成，其中限制方程生成子模块能够根据库位的位置和大小生成泊车路径限制方程组，优化求解模块能够对建立的限制方程组进行最优化求解，得到最优泊车目标位姿和最优库内调整路径，保证车辆泊车过程中能够得到最大的安全距离。通过库位内调整路径的最优化，能够降低车辆的库位内调整次数，或者在相同库位内调整次数的情况下，使车辆能够泊入尽可能小的库位。

入库路径规划模块包含三个子模块：无碰撞路径生成子模块、可执行路径生成子模块和路径选择子模块，该无碰撞路径规划子模块生成无碰撞泊车路径采用扇形区域法，选择扇形上距离障碍物距离最大的点，即最不可能与障碍物发生碰撞的点，连接这些点构成无碰撞泊车路径。

可执行路径生成子模块规划的路径可能包含三种曲线类型：最短距离曲线即RS曲线（Reeds&Shepp’s curve）、曲率连续曲线即CC曲线（Continuous-Curvature curve）和半曲率连续曲线即hCC曲线（half Continuous-Curvature curve）。

路径选择子模块可以根据不同的原则选择合适的曲线类型（RS/hCC/CC）作为泊车路径，这些原则对应路径选择子模块的三种工作模式，其具体表述为：

1）泊车过程中前后运动方向改变的次数最少原则，对应模式1；

2）泊车过程中走过的距离最短原则，对应模式2；

3）泊车过程中原地转向的次数最少原则，对应模式3；

人机接口单元提供驾驶员意图输入接口，路径选择子模块接收驾驶员所输入的路径类型选择模式，并据此选择符合驾驶员意图的曲线作为可执行路径。

当人机接口单元接收到的驾驶员意图为模式1时，接收驾驶员设定的允许的最大方向改变次数，当规划的路径超过这个设定值，放弃泊车；当人机接口单元接收到的驾驶员意图为模式2时，系统计算泊车路径，自行选择符合驾驶员意图的曲线作为可执行路径。当人机接口单元接收到的驾驶员意图为模式3时，接收驾驶员设定的允许的最大原地转向次数，当规划的路径超过这个设定值，放弃泊车。

本发明能够从任意起始位姿引导车辆泊入停车位，能够降低泊车过程中车辆原地转向和前后运动方向改变的次数；平行泊车情况下，可根据驾驶员设置的库位内允许最大调整次数选择合适的停车位，降低车辆对平行泊车库位长度的要求。

附图说明

图1是本发明智能辅助泊车系统的系统框图。

图2是本发明实施例库位内调整路径规划模块框图。

图3是本发明实施例入库路径规划模块框图。

图4是车辆模型示意图。

图5是两次库位内调整时的实施例示意图。

图6是扇形区域法生成无碰撞路径实施例的示意图。

图7a是左转的CC曲线曲率-弧长图。

图7b是右转的CC曲线曲率-弧长图。

图8a是左转的hCC曲线曲率-弧长图。

图8b是右转的hCC曲线曲率-弧长图。

图9是RS曲线转换为CC曲线示意图。

图10是转换后CC路径曲率-弧长示意图。

图11是RS曲线转换为hCC曲线示意图。

图12是转换后hCC路径的曲率-弧长图。

图13是采样点的选取流程示意图。

图14RS/hCC/CC曲线选取模式1。

图15一种泊车过程仿真结果示意图。

具体实施方式

本发明包括感知单元，用于获取库位信息和车辆的位姿信息；路径规划单元，用于规划出合适的泊车路径；驾驶控制单元，用于控制车辆沿规划路径行驶；人机接口单元，用于人机交互，包含驾驶员信息输入和系统信息输出，例如视觉显示、语音提示和机械振动等。

路径规划单元是整个系统的中心，它接收环境感知单元发来的库位信息和车辆位姿信息，向驾驶控制单元输出车辆需要跟踪行驶的路径参数，同时把规划的路径信息输出到人机接口单元，给出驾驶员相应提示。人机接口单元是系统和驾驶员进行信息交互的窗口，它把环境感知单元、路径规划单元、驾驶控制单元等发送来的信息输出，并能够接受驾驶员的输入，允许驾驶员设定允许的最大库位内调整次数、入库路径模式等参数。

为了使车辆能够泊入更为狭小的库位，本发明允许车辆进行平行泊车时，在进入库位后做若干次姿态调整，驾驶员可以通过人机界面设定允许姿态调整的次数，当车辆泊入库位所需的库位内调整次数大于驾驶员设定次数时，系统放弃该库位。当库位内调整次数不大于驾驶员设定次数时，系统首先规划出车辆在库位内的姿态调整路径，然后规划出车辆驶入库位的路径。由于在规划库位内调整路径时进行了最优化设计，库位空间得到了充分利用。和其他辅助泊车系统相比，本发明能够在不增加库位内调整次数的情况下，使车辆泊入更小的库位。

为了使车辆能够从任意起始位姿驶入库位，在入库路径规划模块中本发明采用了两步法路径规划方法，首先生成无碰撞路径，然后根据无碰撞路径生成车辆可跟踪行驶的路径。最终生成的入库路径有三种模式，每种模式对路径的舒适性、长短等有不同的侧重。驾驶员可以根据个人喜好通过人机界面对入库路径的模式进行选择。

以下结合附图所示实施例对本发明作进一步的说明。

（1）车辆控制模型

在泊车过程中，车速很低，轮胎的侧偏特性可以忽略不计，所以可以采用以下车辆控制模型（见图4）：

$\left[\begin{array}{c}\frac{\mathrm{dx}\left(t\left(s\right)\right)}{\mathrm{ds}}\\ \frac{\mathrm{dy}\left(t\left(s\right)\right)}{\mathrm{ds}}\\ \frac{\mathrm{d\θ}\left(t\left(s\right)\right)}{\mathrm{ds}}\\ \frac{\mathrm{d\κ}\left(t\left(s\right)\right)}{\mathrm{ds}}\end{array}\right]=\left[\begin{array}{c}\cos \left(\mathrm{\θ}\left(t\left(s\right)\right)\right)\\ \cos \left(\mathrm{\θ}\left(t\left(s\right)\right)\right)\\ \mathrm{\κ}\left(t\left(s\right)\right)\\ 0\end{array}\right]\mathrm{sign}\left(v_{\max }\left(t\left(s\right)\right)\right)+\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]\frac{\mathrm{\σ}\left(\frac{s}{v_{\max }}\right)}{v_{\max }}$ 式（1-1）

其中，s是车辆所驶过的弧长，x、y是车辆在全局坐标系中的横、纵坐标，θ是车辆的航向角，κ是车辆后轴中点轨迹的曲率，σ是曲率的变化率，v_max是泊车中允许的最大速度，t是时间变量，sign(·)是符号函数

车辆的两个约束条件是：

|κ|≤κ_max,式(1-2)

|σ|≤σm_ax,式(1-3)

其中，κ_max和σ_max是曲率和曲率变化率的最大值，它们是由车辆本身的物理特性决定的约束值。

（2）库位内位姿调整路径规划模块

1）库内调整次数的设定和判断

在本发明中，当泊车库位的尺寸确定之后，系统就能够计算出车辆泊入库位所需要的库位内姿态调整次数。系统把计算得到的库位内调整次数和驾驶员设定的最大允许调整次数进行比较，当计算得到的调整次数大于驾驶员设定的允许调整次数时，系统放弃该库位。

调整次数的设定可以采用多种方式，如采用键盘输入、触摸屏输入或者预设在系统控制器内。

根据库位尺寸计算车辆库位内调整次数的方法也可以具有多种方式，如查表法，数值计算法等。在实施例1中，系统根据公式计算调整次数，具体方法如下：

I计算泊车深度泊车深度指完成泊车动作后车辆中心线和库位外沿的距离，其确定按照以下公式：

$h=\left\{\begin{array}{cc}{W}_V/2& (W_P\&GreaterEqual;W_V+E_h)\\ W_p-W_v/2-e_h& (W_p<W_v+e_h)\end{array}\right.$ 式（2-1）

其中h表示泊车深度，W_v表示车辆宽度，W_p表示库位宽度，e_h表示泊车深度的安全距离。

II计算车辆进行n（n为非负的整数）次姿态调整时所能泊入的最小库位长度

根据以下公式计算车辆无需库位内姿态调整时所需的最小库位长度：

$L_0=\sqrt{(R^2-b^2)+{(L_v-L_r)}^2-(\sqrt{R^2-b^2}-{(h+W_v/2)}^2)}+L_r$ 式（2-2）

其中L₀表示无需库位内姿态调整时所能泊入的最小库位长度，h表示泊车深度，L_v表示车辆长度，W_v表示车辆宽度，b表示车辆轴距，L_r表示车辆后轴中点距离车辆尾端的距离，R表示车辆外侧前轮的最小转弯半径，如图4所示。

根据以下公式计算车辆进行n次库位内姿态调整时所能泊入的最小库位长度：

L_n＝-k_n arctan(n)+L₀  式（2-3）

其中L_n表示n次姿态调整所能泊入的最小库位长度，k_n表示对应于调整次数n的调节参数，由标定得来，L₀表示无需库位内姿态调整时所能泊入的最小库位长度。

III计算得到泊入现有库位所需的库位内调整次数。

根据步骤II所得的库位内调整次数和库位长度的关系，计算得到泊入现有库位所需的库位内调整次数，当调整次数大于预设值时，放弃该库位，并通过人机界面对驾驶员做出提示。

2）库位内调整路径规划

库位内调整路径规划模块利用车辆最小转弯半径圆弧规划车辆在库位内进行姿态调整的运动路径，其包含两个子模块：限制方程生成子模块和优化求解子模块，其中限制方程生成子模块能够根据库位的位置和大小生成泊车路径限制方程组，车辆在库位内运动时其轨迹必须满足限制方程组；优化求解模块能够对建立的限制方程组进行最优化求解，得到最优泊车目标位姿和最优库位内调整路径，保证车辆泊车过程中得到最大的安全距离。通过库位内调整路径的最优化，能够减少车辆在库位内的调整次数，或者在相同库位内调整次数的情况下，使车辆能够泊入尽可能小的库位。

一条典型的库位内调整路径如图5所示。

限制方程组的建立方法可以具有多种方式，即可以在线完成，也可以预设在控制器的算法里。对方程组进行优化求解的方法也可以具有多种方式，如线性规划法或者迭代求解法等。在实施例2中，路径规划算法通过计算发现车辆泊入库位需要进行2次库位内姿态调整，小于驾驶员的预设值，其库位内调整路径规划方法如下所示：

I建立限制方程组

如图5所示，库位是由障碍物1和障碍物2构成的，环境感知模块探测到库位长度为L_min，车辆需要在库位内做两次姿态调整，库位内的调整路径由两段圆弧组成，第一段是由第一目标位姿B到中间目标位姿C的向前运动，第二段是由中间目标位姿C到最终目标位姿G的向后运动。车辆沿规划路径依次到达B点、C点和G点，设B点坐标为(x_B,y_B,θ_B)，C点坐标为(x_C,y_C,θ_C),G点坐标为(x_G,y_G,θ_G),车辆的左上角点、右上角点、右后角点、左后角点依次表示为点a、b、c、d，路沿L6的纵坐标为y6，坐标轴的原点设定在前方障碍物的左后角点。车辆长度表示为L_v，车辆宽度表示为W_v，车辆轴距表示为b，车辆后轴中点距离车辆尾端的距离表示为L_r，当车辆到达G点后，要求车辆刚好和库位保持平行，并且达到所需的泊车深度，于是有

y_G＝-h  式（2-4）

θ_G＝0  式（2-5）

其中h表示泊车深度。

可以设想，为了使车辆在库位内调整过程中获得最大的安全距离，车辆在B点、C点、G点时与最近障碍物的距离应该相同。

车辆在G点时，其左后角点d距离后方障碍物最近，其横坐标为：

x_Gd＝(-L_r+x_G)式（2-6）

其中x_Gd表示车辆在G点时其左后角点d的横坐标。

车辆在B点时距离后方障碍物最近，其左后角点d点的横坐标为：

x_Bd＝x_B-L_r cosθ_B-0.5W_vsinθ_B  式（2-7）

其中x_Bd表示车辆在B点时其左后角点d点的横坐标。

车辆在C点时距离前方障碍物最近，其右前角点b的横坐标为：

x_Cb＝x_C+(L_v-L_r)cosθ_c+0.5W_vsinθ_C  式（2-8）

其中x_Cb表示车辆在C点时其右前角点b点的横坐标。

要求车辆在B点、C点、G点处与最近障碍物的距离相同，并且要求距离大于零，据此可得：

x_Bd+L_min＝-x_Cb＞0  式（2-9）

x_Gd+L_min＝x_Bd+L_min＞0  式（2-10）

根据B点和C点的位置关系可得：

R_munrcosθ_c-R_munrcosθ_B＝y_C-y_B  式（2-11）

-R_minrsinθ_c+R_minrsinθ_B＝x_C-x_B  式（2-12）

根据C点和G点的位置关系可得：

R_minlsinθ_C＝x_C-x_G  式（2-13）

R_minl-R_minlcosθ_C＝y_C-y_G  式（2-14）

其中R_minr表示车辆后轴中点向右的最小转弯半径，R_minl表示车辆后轴中点向左的最小转弯半径。

假设车辆从库外以最小转弯半径圆弧倒车到达B点，要求倒车过程中车辆右前角点b不与库位发生碰撞，右后角点c不与路沿L6发生碰撞，可得限制不等式：

$\sqrt{{(R_{\min l}+0.5W_v)}^2+{(L_v-L_r)}^2-{(y_B+R_{\min l}\cos {\mathrm{\&theta;}}_B)}^2}-R_{\min l}-\sin {\mathrm{\&theta;}}_B+x_B<0$ 式（2-15）

y_B-L_rsinθ_B-0.5W_vcosθ_B＞y6  式（2-16）

其中y6表示路沿L6的纵坐标。

公式（2-4）-（2-16）包含了使车辆在姿态调整过程中与障碍物取得最大安全距离所需要满足的条件。

II最优化求解

限制方程组中需要求解的未知量为B点的坐标(x_B,y_B,θ_B)，C点的坐标(x_C,y_C,θ_C)和G点的坐标(x_G,-h,0)，未知量有7个：x_B、y_B、θ_B、x_C、y_C、θ_C和x_G；而等式有6个：式（2-9）-（2-14）；限制不等式有4个：（2-9）、（2-10）、（2-15）和（2-16），那么可以把6个方程化简为关于θ_C的参数方程，然后让θ_C在一定范围内以一定步长取值，检测θ_C取不同值时车辆距离路沿L6的最短距离和车辆倒车到达B点过程中距离库位的最短距离。当车辆泊车过程中距离库位、路沿的最近距离都较大时，此时的θ_C取值比较合理。然后求出此时B点、C点和G点的坐标，由B点、C点和G点确定的库位内姿态调整路径可以作为最优路径。

以上计算过程提供了进行库位内姿态调整路径规划的一种方法，在其他实施例中也可以采用不同的限制方程组建立方法，或者采用不同的最优求解方法。

（3）入库路径规划模块

1)无碰撞路径的生成

无碰撞路径是连接起始位姿S和第一目标位姿B的不与前后障碍物发生碰撞的路径，这一步的规划中，先不考虑车辆自身的物理约束。无碰撞路径的生成以路径上各个位姿到障碍物的距离最远为原则。

无碰撞路径的生成方法是，分别从起始位姿S(x_s,y_s,θ_s)和第一目标位姿B(x_B,y_B,θ_B)开始，以一定的计算步长选择某一位姿周围所有位姿中距离障碍物距离最远的点为无碰撞路径中的下一个位姿，按照这样的规律生成一条连接第一目标位姿B和起始位姿S的无碰撞路径τ。当无碰撞路径上一个位姿N确定后，下一个位姿T的确定方法采用的是扇形区域法，见图6。

以N为圆心，一个计算步长为半径做一个向前的扇形区域，扇形区域圆弧线的左右两个端点是受车辆左右转向时的物理约束条件---最大曲率确定的，其余各点对应的是曲率小于最大曲率的各点。图中T1到T7的弧上就是车辆在一个步长后的可达区域，也就是说，当车辆以不同的前轮转角运动一个步长时能够到达的区域就是扇形圆弧段上的各点。计算并比较T1到T7各点到障碍物的距离，选择到障碍物距离最大的点作为无碰撞路径上的下一个点。

无碰撞路径的生成过程共包括三个步骤：

I.生成以第一目标位姿B为起点的无碰撞路径τ_g

按照扇形区域法从第一目标位姿B起逐个步长生成下一个位姿，直至多个连续的位姿的纵坐标y和方向θ不再变化，得到一条无碰撞路径τ_g；

II.生成以起始位姿S为起点的无碰撞路径τ_s

按照扇形区域法从起始位姿起逐个步长生成下一个位姿，直至多个连续的位姿的纵坐标y和方向θ不再变化，得到一条无碰撞路径τ_s；

III.生成中间无碰撞路径τ_c，连接τ_s和τ_g

当τ_s和τ_g具有交点时（有交点指的是两条无碰撞路径存在共同点，在该点处的位姿(x,y,θ)相同），以交点为界限，抛弃交点之后的路径，交点之前的路径构成无碰撞路径。

当τ_s和τ_g不具有交点时，以两条路径开始保持平行为界限，抛弃两条路径保持平行的部分，其余部分用分布在直线上的路径τ_c连接，τ_c的两个端点分别是τ_s和τ_g的终点H和K，如图6所示。路径τ_c、τ_s和τ_g构成最终的无碰撞路径。

2）可执行路径的生成

可执行路径生成子模块首先在上一步生成的无碰撞路径上选取采样点，再用三种不同类型的曲线连接采样点生成可执行路径。车辆泊车过程中的可执行路径的规划以车辆所走过的距离最优为原则。根据不同的泊车工况，可执行路径可能包含三种类型：RS曲线、hCC曲线和CC曲线。

RS曲线由最小转弯半径的圆弧和直线构成，它可以使得泊车路径最短，同时所需的库位长度最小，但是如果不进行平滑化处理，会导致规划路径的曲率不连续；CC曲线是曲率线性变化的曲线（曲率如图7a、图7b所示），它由直线、圆弧和直线与圆弧之间的过渡回旋曲线构成，它可以由RS曲线转换而来从而对路径进行平滑化处理，CC路径可以保证泊车全过程曲率连续，无停车转向，但是会造成泊车过程车辆前后运动方向改变次数过多，且所需的库位长度值较大；hCC曲线是一种变异的CC曲线，也是由直线、圆弧和回旋曲线构成的，其曲率随弧长的变化如图8a、图8b所示，在曲线的起始或终止点处，曲率不是零，车辆需要原地转向。与CC曲线相比，采用hCC曲线时，车辆前后运动方向改变次数较少，与障碍物发生碰撞的风险小，构成的路径适用于更小的泊车位长度。CC曲线和hCC曲线可以由RS曲线转换而来。

在生成可执行路径时，利用RS曲线连接两点的方法已经成熟，不再赘述，下面介绍利用CC曲线和hCC曲线近似RS曲线的方法。

I．RS曲线转换为CC曲线

各种类型的RS曲线转换为CC曲线，是指利用CC曲线对RS曲线进行近似，使得路径的曲率连续变化，从而解决泊车过程中停车转向的问题。CC路径是由三部分组成的：1）一段回旋曲线弧段，曲率的变化率为σ_max（曲率变化率最大值），曲率从0到±κ_max（曲率最大值）；2）一段曲率为±κ_max的圆弧；3）一段回旋曲线弧段，曲率的变化率为σ_max，曲率从±κ_max到0。

根据回旋曲线的特性，CC曲线的曲率为零的点位于一个轮廓圆上，轮廓圆半径为

$r_{\mathrm{cc}}=\sqrt{{(\sqrt{\frac{\mathrm{\&pi;}}{{\mathrm{\&sigma;}}_{\max }}}{C}_f\left(\frac{{\mathrm{\&kappa;}}_{\max }}{\sqrt{{\mathrm{\&sigma;}}_{\max }\mathrm{\&pi;}}}\right)-\frac{\sin \frac{{\mathrm{\&kappa;}}_{\max }^2}{{2\mathrm{\&sigma;}}_{\max }}}{{\mathrm{\&kappa;}}_{\max }})}^2+{(\sqrt{\frac{\mathrm{\&pi;}}{{\mathrm{\&sigma;}}_{\max }}}{S}_f\left(\frac{{\mathrm{\&kappa;}}_{\max }}{\sqrt{{\mathrm{\&sigma;}}_{\max }\mathrm{\&pi;}}}\right)+\frac{\cos \frac{{\mathrm{\&kappa;}}_{\max }^2}{{2\mathrm{\&sigma;}}_{\max }}}{{\mathrm{\&kappa;}}_{\max }})}^2}$ 式（3-1）

其中，C_f和S_f是法赛尔积分：

$\left\{\begin{array}{c}{C}_f\left(x\right)={\&Integral;}_0^x\cos \left(\frac{\mathrm{\&pi;}}{2}{\mathrm{\&xi;}}^2\right)\mathrm{d\&xi;}\\ S_f\left(x\right)={\&Integral;}_0^x\sin \left(\frac{\mathrm{\&pi;}}{2}{\mathrm{\&xi;}}^2\right)\mathrm{d\&xi;}\end{array}\right.$

曲率为零点处的轮廓圆的切线方向和车辆的运动方向的夹角为：

$\mathrm{\&mu;}=\arctan \frac{\sqrt{\frac{\mathrm{\&pi;}}{{\mathrm{\&sigma;}}_{\max }}}{C}_f\left(\frac{{\mathrm{\&kappa;}}_{\max }}{\sqrt{{\mathrm{\&sigma;}}_{\max }\mathrm{\&pi;}}}\right)-\frac{\sin \frac{{\mathrm{\&kappa;}}_{\max }^2}{2{\mathrm{\&sigma;}}_{\max }}}{{\mathrm{\&kappa;}}_{\max }}}{\sqrt{\frac{\mathrm{\&pi;}}{{\mathrm{\&sigma;}}_{\max }}}{S}_f\left(\frac{{\mathrm{\&kappa;}}_{\max }}{\sqrt{{\mathrm{\&sigma;}}_{\max }\mathrm{\&pi;}}}\right)+\frac{\cos \frac{{\mathrm{\&kappa;}}_{\max }^2}{2{\mathrm{\&sigma;}}_{\max }}}{{\mathrm{\&kappa;}}_{\max }}}$ 式（3-2）

回旋曲线弧段所转过的角度为：

${\mathrm{\&delta;}}_{\min }={\mathrm{\&kappa;}}_{\max }^2{\mathrm{\&sigma;}}_{\max }^{-1}$ 式（3-3）

对于给定的起始位姿，终止位姿只会随着中间段圆弧的对应的转角不同而发生变化。不同的终止位姿都位于该轮廓圆上。

如图9所示，以第一种类型RS路径

（L、R和S分别代表的是向左、向右的最小转弯半径圆弧和直线，上标表示运动的方向，+表示先前运动，-表示向后运动，下标代表的是各段的长度）为例说明。q_s和q_B分别代表起点S和终点B的位姿(x_S,y_S,θ_S)和(x_B,y_B,θ_B)。转换前后要保证车辆的起始位姿和终止位姿不变。R_minl和R_minr分别表示车辆后轴中点左转和右转的最小转弯半径。r_ccl和r_ccr分别表示左转和右转时两轮廓圆的半径。q_1f和q₂₀分别表示第一个轮廓圆的驶出点和第二个轮廓圆的驶入点的位姿(x_1f,y_1f,θ_1f)和(x₂₀,y₂₀,θ₂₀)。(x_o1,y_o1)和(x_o2,y_o2)是两轮廓圆圆心的坐标。起点方向角ψ_s和圆心O1坐标分别是

${\mathrm{\&psi;}}_s=\mathrm{mod}({\mathrm{\&theta;}}_s-\frac{\mathrm{\&pi;}}{2}-\mathrm{\&mu;},2\mathrm{\&pi;})$ 式(3-4)

终点方向角ψ_B和圆心O2坐标分别是

${\mathrm{\&psi;}}_B=\mathrm{mod}({\mathrm{\&theta;}}_B+\frac{\mathrm{\&pi;}}{2}-\mathrm{\&mu;},2\mathrm{\&pi;})$ 式(3-6)

$\left\{\begin{array}{c}{x}_{o2}=x_B-r_{\mathrm{ccr}}\cos \left({\mathrm{\&psi;}}_B\right)\\ y_{o2}=y_B-r_{\mathrm{ccr}}\sin \left({\mathrm{\&psi;}}_B\right)\end{array}\right.$ 式(3-7)

由此可求出两个圆心之间的距离|O₁O₂|。

中间一段的直线路径的长度为：

$l_3=\sqrt{{\left|O_1{O}_2\right|}^2-{(r_{\mathrm{ccl}}+r_{\mathrm{ccr}})}^2+{\left((r_{\mathrm{ccl}}+r_{\mathrm{ccr}})\sin \left(\mathrm{\&mu;}\right)\right)}^2}-(r_{\mathrm{ccl}}+r_{\mathrm{ccr}})\sin \left(\mathrm{\&mu;}\right)$ 式(3-8)

则两个圆心连线与水平方向所成的角度为φ＝∠o₁o₂。

第一个轮廓圆和第二个轮廓圆上的驶出和驶入点的方向角：

${\mathrm{\&theta;}}_{1f}={\mathrm{\&theta;}}_{20}=\mathrm{mod}(\mathrm{\&phi;}+\frac{\mathrm{\&pi;}}{2}-\mathrm{\&mu;},2\mathrm{\&pi;})$ 式(3-9)

然后可以求出

各段的弧长。转换后的路径就是从q_s开始以CC路径到q_1f，接下来直线运动到q20，最后一段是CC曲线路径至qB。三段对应的弧长分别为：

$\left\{\begin{array}{c}{S}_1=\frac{2{\mathrm{\&kappa;}}_{\max }}{{\mathrm{\&sigma;}}_{\max }}+\frac{{\mathrm{\&theta;}}_{1f}-{\mathrm{\&theta;}}_s-{\mathrm{\&delta;}}_{\max }}{{\mathrm{\&kappa;}}_{\max }}\\ S_2=l_3\\ S_3=\frac{2{\mathrm{\&kappa;}}_{\max }}{{\mathrm{\&sigma;}}_{\max }}+\frac{{\mathrm{\&theta;}}_B-{\mathrm{\&theta;}}_{20}-{\mathrm{\&delta;}}_{\min }}{{\mathrm{\&kappa;}}_{\max }}\end{array}\right.$ 式（3-10）

所以可求出规划的路径上各点对应的曲率：

$\mathrm{\&kappa;}=\left\{\begin{array}{cc}{\mathrm{\&sigma;}}_{\max }\&CenterDot;s,& 0\&le;s<\frac{{\mathrm{\&kappa;}}_{\max }}{{\mathrm{\&sigma;}}_{\max }}\\ {\mathrm{\&kappa;}}_{\max },& \frac{{\mathrm{\&kappa;}}_{\max }}{{\mathrm{\&sigma;}}_{\max }}\&le;s<\frac{{\mathrm{\&kappa;}}_{\max }}{{\mathrm{\&sigma;}}_{\max }}+\frac{{\mathrm{\&theta;}}_{1f}-{\mathrm{\&theta;}}_s-{\mathrm{\&delta;}}_{\min }}{{\mathrm{\&kappa;}}_{\max }}\\ {\mathrm{\&sigma;}}_{\max }\&CenterDot;(s_1-s),& \frac{{\mathrm{\&kappa;}}_{\max }}{{\mathrm{\&sigma;}}_{\max }}+\frac{{\mathrm{\&theta;}}_{1f}-{\mathrm{\&theta;}}_s-{\mathrm{\&delta;}}_{\min }}{{\mathrm{\&kappa;}}_{\max }}\&le;S<S_1\\ 0,& s_1\&le;s<s_1+s_2\\ -{\mathrm{\&sigma;}}_{\max }\&CenterDot;(s-s_1-s_2),& S_1+S_2\&le;S<\frac{{\mathrm{\&kappa;}}_{\max }}{{\mathrm{\&sigma;}}_{\max }}+s_1+s_2\\ -{\mathrm{\&kappa;}}_{\max },& S_1+S_2\&le;S<\frac{{\mathrm{\&kappa;}}_{\max }}{{\mathrm{\&sigma;}}_{\max }}+s_1+s_2+\frac{{\mathrm{\&theta;}}_B-{\mathrm{\&theta;}}_{20}-{\mathrm{\&delta;}}_{\min }}{{\mathrm{\&kappa;}}_{\max }}\\ -{\mathrm{\&sigma;}}_{\max }\&CenterDot;(s_1+s_2+s_3-s),& s_1+s_2+\frac{{\mathrm{\&kappa;}}_{\max }}{{\mathrm{\&sigma;}}_{\max }}+\frac{{\mathrm{\&theta;}}_B-{\mathrm{\&theta;}}_{20}-{\mathrm{\&delta;}}_{\min }}{{\mathrm{\&kappa;}}_{\max }}\&le;s\&le;s_1+s_2+s_3\\ 0,& s_1+s_2+s_3\end{array}\right.$ 式（3-11）

生成路径的曲率随弧长变化的示意图见图10。

II．RS曲线转换为hCC曲线

hCC路径由两部分组成的：1）一段曲率为±κ_max的圆弧；2）一段回旋曲线弧段，曲率的变化率为σ_max，曲率可以是从0到±κ_max，也可以是从±κ_max到0。hCC曲线构成的轨迹的特点是要求车辆在起始位置和结束位置时允许在车辆停车时原地转向，但在运动过程中不需要停车转向。hCC的求法与CC转换的求法类似，只是轮廓圆圆心的求法不同，见图11。

两个轮廓圆的圆心的求法是：

$\left\{\begin{array}{c}{x}_{o1}=x_s+R_{\min l}(-\sin \left({\mathrm{\&theta;}}_s\right))\\ y_{o1}=y_s+R_{\min l}\left(\cos \left({\mathrm{\&theta;}}_s\right)\right)\end{array}\right.$ 式(3-12)

$\left\{\begin{array}{c}{x}_{o2}=x_B-R_{\min r}(-\sin \left({\mathrm{\&theta;}}_B\right))\\ y_{o2}=y_B-R_{\min r}\left(\cos \left({\mathrm{\&theta;}}_B\right)\right)\end{array}\right.$ 式(3-13)

然后可以求出

各段的弧长：

$\left\{\begin{array}{c}{S}_1=\frac{{\mathrm{\&kappa;}}_{\max }}{{\mathrm{\&sigma;}}_{\max }}+\frac{{\mathrm{\&theta;}}_{1f}-{\mathrm{\&theta;}}_s-{\mathrm{\&delta;}}_{\min }/2}{{\mathrm{\&kappa;}}_{\max }}\\ S_2=l_3\\ S_3=\frac{{\mathrm{\&kappa;}}_{\max }}{{\mathrm{\&sigma;}}_{\max }}+\frac{{\mathrm{\&theta;}}_B{\mathrm{\&theta;}}_{20}{\mathrm{\&delta;}}_{\min }/2}{{\mathrm{\&kappa;}}_{\max }}\end{array}{}_{}\right.$ 式（3-14）

所以可求出规划的路径上各点对应的曲率：

$\mathrm{\&kappa;}=\left\{\begin{array}{cc}{\mathrm{\&kappa;}}_{\max },& 0\&le;s<\frac{{\mathrm{\&theta;}}_{1f}-{\mathrm{\&theta;}}_s-{\mathrm{\&delta;}}_{\min }/2}{{\mathrm{\&kappa;}}_{\max }}\\ {\mathrm{\&sigma;}}_{\max }\&CenterDot;(s_1-s),& \frac{{\mathrm{\&theta;}}_{1f}-{\mathrm{\&theta;}}_s-{\mathrm{\&delta;}}_{\min }/2}{{\mathrm{\&kappa;}}_{\min }}\&le;s<s_1\\ 0,& s_1\&le;s<s_1+s_2\\ {\mathrm{\&sigma;}}_{\max }\&CenterDot;(s_1+s_2-s),& s_1+s_2\&le;s<\frac{{\mathrm{\&kappa;}}_{\max }}{{\mathrm{\&sigma;}}_{\max }}+s_1+s_2\\ -{\mathrm{\&kappa;}}_{\max },& \frac{{\mathrm{\&kappa;}}_{\max }}{{\mathrm{\&sigma;}}_{\max }}+s_1+s_2\&le;s\&le;s_1+s_2+s_3\\ 0,& s>s_1+s_2+s_3\end{array}\right.$ 式（3-15）

生成路径的曲率随弧长的示意图见图12。

驾驶控制单元根据路径规划单元求出的目标曲率和目标曲率变化率控制车辆跟随该路径。

III.采样点的选取思路是在无碰撞路径上成对地选择点，首先选择的一对点是无碰撞路径的起点和终点，利用可执行路径连接两两点，如果判断当前两点之间的可执行路径与前后障碍物发生碰撞，就再增加一个两点之间的中间点，继续用可执行路径连接两两点直至不与前后障碍物发生碰撞，最后这些无碰撞的点就选作采样点。

在无碰撞路径上选取采样点的流程图见图13。图中各符号的含义是，τ表示无碰撞路径，N_cand N_g分别表示当前采样候选点和当前过渡目标点的序号，P表示τ上的点的总数。q_sam是选出的采样点的位姿，q_B是无碰撞路径上的最后一个位姿，即库位内位姿调整阶段规划得到的第一目标位姿。运算符floor(·)表示取小于等于某数的最小整数。

3）路径选择

不同的驾驶员对泊车过程舒适性的要求不同，该路径规划单元中路径选择子模块可以根据驾驶员输入的不同模式进行选择，三个模式分别是：泊车过程前后运动方向改变次数最少模式；泊车过程走过路径最短模式；泊车过程原地转向次数最少模式；根据前面描述的RS、hCC和CC曲线的特点，RS曲线可能会使得整个泊车过程走过的路径最短，但泊车过程经常需要停车转向，同CC曲线相比，hCC曲线较适合作为前后运动方向改变次数最少的模式，CC曲线则能够保证全程无原地转向。实施例3中给出了路径选取子模块中RS/hCC/CC曲线选取为模式1时的情况。见图14。

在该模式下，车辆前后运动方向改变的次数越少越好，且最好不出现RS路径。结合泊车的实际情况，选用RS/hCC/CC路径以前后运动方向改变次数最少为原则。当无论选择哪一种曲线都会造成车辆前后运动方向改变次数较多，超过驾驶员设定的最大次数时，放弃泊车。

i.首先计算hCC和CC路径，不计算RS路径；

ii  当hCC和CC路径中有一个存在，且方向改变次数较少时，选用存在的曲线类型，路径规划成功，退出；

iii.当hCC和CC路径都存在且前后运动方向改变次数都较少时，

a)hCC方向改变的次少于CC，选用hCC，路径规划成功，退出；

b)否则，选用CC，路径规划成功，退出；

iv.当hCC和CC路径都不存在或是虽然两者中至少有一个存在但方向改变次数过多时，计算RS路径

a)RS路径不存在或是方向改变次数过多时，路径规划不成功，退出；

b)RS路径存在且方向改变次数不多时，路径规划成功，选用RS路径，退出。

对上所述路径规划算法进行仿真，仿真结果如图15所示。仿真设定的工况是：最多允许两次库位内调整，选定的入库路径模式为前后运动方向改变次数最少。由于仿真中库位长度接近两次调整要求的最小库位长度，并且车辆的起始位姿较为苛刻，为q_s(x_s,y_s,θ_s)=（2m,1.4m,20°），所以车辆需要在库内调整两次，在库外运动方向改变三次。

上述的对实施例的描述是为便于该技术领域的普通技术人员能理解和应用本发明，本发明中所述方法也可应用于垂直泊车辅助系统。熟悉本领域技术的人员显然可以容易地对这些实施例做出各种修改，并把在此说明的一般原理应用到其他实施例中而不必经过创造性的劳动。因此，本发明不限于这里的实施例，本领域技术人员根据本发明的揭示，不脱离本发明的范畴所做出的改进和修改都应该在本发明的保护范围之内。

Claims (10)

1.一种智能辅助泊车方法，其特征在于：探测感知车辆周围环境，识别到可用于泊车的库位后，规划出合理的泊车路径，并控制车辆沿规划路径行驶，直到车辆完成泊车动作；并提供人机交互界面，接受驾驶员意图，显示泊车信息；其中，对泊车路径分两部分进行规划，首先规划出库位内调整路径，然后规划出入库路径。

2.根据权利要求1所述的智能辅助泊车方法，其特征在于：

规划库位内调整路径时，采用最小转弯半径圆弧构成路径，首先建立库位边界对库位内姿态调整路径的限制方程，然后对方程进行优化求解，据此求出最优泊车目标位姿和最优库位内调整路径；

规划入库路径时采用两步法路径规划方法：首先采用扇形区域法生成无碰撞路径，然后根据无碰撞路径生成车辆可跟踪行驶的路径，最后从多条可执行的路径中选择合适类型的路径。

3.根据权利要求2所述的智能辅助泊车方法，其特征在于：提供人机界面，允许驾驶员对泊车路径进行个性化设定；

（1）依据库位尺寸计算出车辆需要的库位内姿态调整的次数，由驾驶员根据个人喜好通过人机界面对库位内调整次数上限值进行设定，当所需调整次数不大于驾驶员设定的上限值时，开始路径规划；

（2）依据路径的舒适性、长短区分出不同的入库路径模式，由驾驶员根据个人喜好通过人机界面对入库路径的模式进行选择。

4.实现权利要求1至3中任一所述方法的智能辅助泊车系统，其特征在于：该系统包括相互之间具有通信联系的环境感知单元、路径规划单元、驾驶控制单元和人机接口单元。

5.根据权利要求4所述的智能辅助泊车系统，其特征在于：该路径规划单元包括两个模块：

（1）库位内调整路径规划模块，规划车辆在库位内进行调整时的路径；

（2）入库路径规划模块，根据车辆的起始位姿规划入库的泊车路径。

6.根据权利要求4所述的智能辅助泊车系统，其特征在于：该人机接口单元提供驾驶员意图输入接口，由驾驶员设定平行泊车情况下允许的库位内调整次数上限值；垂直泊车情况下，不允许车辆进行库位内调整。

7.根据权利要求4所述的智能辅助泊车系统，其特征在于：在环境感知单元探测到库位大小后，该路径规划单元计算出泊入该库位所需的库位内调整次数，当计算得到的库位内调 整次数大于驾驶员设定次数时，放弃该库位；当计算得到的库位内调整次数不大于驾驶员设定次数时，该路径规划单元首先规划出库位内调整路径，然后规划出入库路径；库位内调整路径以最小转弯半径圆弧连接第一目标位姿B和最终目标位姿G；入库路径连接车辆的起始位姿S和第一目标位姿B。

8.根据权利要求5所述的智能辅助泊车系统，其特征在于：该库位内调整路径规划模块由限制方程生成子模块和优化求解子模块构成，其中限制方程生成子模块能够根据库位的位置和大小生成泊车路径限制方程组，优化求解模块能够对建立的限制方程组进行最优化求解，得到最优泊车目标位姿G和最优库位内调整路径；

或者，该入库路径规划模块包含三个子模块：无碰撞路径生成子模块、可执行路径生成子模块和路径选择子模块，该无碰撞路径规划子模块生成无碰撞泊车路径时采用扇形区域法，选择扇形上距离障碍物车辆距离最大的点，即最不可能与障碍物发生碰撞的点；连接这些点构成无碰撞泊车路径；

或者，该可执行路径生成子模块规划的路径包含三种曲线类型：RS曲线（Reeds & Shepp’s curve）、CC曲线（continuous-Curvature curve）和hCC曲线（half Continuous-Curvature curve）；

或者，该路径选择子模块可以根据不同的原则选择合适的曲线类型（RS/hCC/CC）作为泊车路径，这些原则对应路径选择子模块的三种工作模式，包括：

1）泊车过程中前后运动方向改变的次数最少原则，对应模式1；

2）泊车过程中走过的距离最短原则，对应模式2；

3）泊车过程中原地转向的次数最少原则，对应模式3。

9.根据权利要求8所述的智能辅助泊车系统，其特征在于：人机接口单元提供驾驶员意图输入接口，路径选择子模块接收驾驶员所输入的路径类型选择模式，并据此选择符合驾驶员意图的曲线作为可执行路径。

10.根据权利要求9所述的智能辅助泊车系统，其特征在于：当人机接口单元接收到的驾驶员意图为模式1时，接收驾驶员设定的允许的最大前后运动方向改变次数，当规划的路径超过这个设定值，放弃泊车；当人机接口单元接收到的驾驶员意图为模式2时，系统计算泊车路径，自行选择符合驾驶员意图的曲线作为可执行路径；当人机接口单元接收到的驾驶员意图为模式3时，接收驾驶员设定的允许的最大原地转向次数，当规划的路径超过这个设定值，放弃泊车。 

Images