专利名称：一种智能辅助泊车方法及其实现系统的制作方法
技术领域：
本发明属于车辆控制技术领域，涉及驾驶辅助技术，特别是汽车的智能辅助泊车技术。
背景技术：
由于城市汽车保有量的增加，停车位紧张的问题日益突出，泊车已经成为城市交通中的一个难点问题，由于泊车时需要驾驶员在狭小紧凑的空间中观察和判断周边环境，短时间内同时控制方向盘、油门和刹车，对其驾驶技巧和反应灵敏度提出了很高要求，因此有必要开发辅助泊车系统，帮助驾驶员完成泊车操作。智能泊车或是辅助泊车系统已经成为国内外的研究重点，智能泊车系统一般包括 感知系统，控制系统和执行系统等。而控制系统是智能泊车系统的核心，其主要是根据传感器信息以及驾驶员操纵信息，对车辆的状态进行调整，使车辆在泊车的过程中找到一条合适的路径，引导车辆完成泊车操作。目前智能泊车的控制算法主要分为以下几类路径规划和跟随算法，它的基本思想是路径规划模块根据起始位姿和目标位姿以及障碍物的情况规划出可行的路径，路径跟随模块控制车辆跟随规划路径，最终使车辆驶入泊车位，这种方法算法简单，容易实现且计算成本较低；基于经验的智能算法，它依据人类驾驶员的泊车经验制定控制策略，控制车辆泊车入库，一般包括模糊控制、神经网络、遗传算法及粒子群算法等，这些智能算法能够较好地解决非线性问题，但是适应性差，控制规则制定复杂，并且计算成本高；姿态稳定算法是通过稳定性分析，使车辆的姿态收敛到零，从而逐步控制车辆达到目标位姿，这种算法具有较高的非线性且计算量大。泊车的难点可以归纳为一、车辆在泊车过程中不能与前后的障碍物车辆发生碰撞；二、车辆在泊车过程中，车辆运动不能超过车辆本身的物理约束条件。泊车路径规划模块的主要任务是要解决这两个关键问题；另外，在泊车过程中考虑到舒适性和安全性等因素，路径规划时还要考虑车辆前后运动方向的改变次数、原地转向次数以及泊车路径长度等因素。因此，针对辅助泊车系统，开发实用高效的路径规划方法具有重要意义。
发明内容
本发明的目的在于提供一种智能辅助泊车系统，当环境感知系统识别到可用于泊车的库位后，规划出合理的泊车路径，并控制车辆沿规划路径行驶，直到车辆完成泊车动作，并提供人机交互界面，接受驾驶员意图，显示泊车信息。为达到以上目的，本发明所采用的解决方案是一种智能辅助泊车系统，包括环境感知单元，用于获取库位信息和车辆的位姿信息；路径规划单元，用于规划出合适的泊车路径；
驾驶控制单元，用于控制车辆沿给定路径行驶；人机接口单元，用于人机交互，包含驾驶员信息输入和系统信息输出。该路径规划单元包括两个模块(I)库位内调整路径规划模块，规划车辆在库位内进行调整时的路径；(2)入库路径规划模块，根据车辆的起始位姿规划入库的泊车路径；该人机接口单元提供驾驶员意图输入接口，驾驶员可以设定平行泊车情况下允许的最大库位内调整次数；垂直泊车情况下，不允许车辆进行库位内调整。
在环境感知单元探测到库位大小后，该路径规划单元可以在平行泊车情况下计算出泊入该库位所需的库位内调整次数，当计算得到的库位内调整次数大于驾驶员设定次数时，放弃该库位。当计算得到的库位内调整次数不大于驾驶员设定次数时，该路径规划单元首先规划出库位内调整路径，然后规划出入库路径。库位内调整路径的起点为第一目标位姿B，终点为最终目标位姿G。库位内调整路径以最小转弯半径圆弧连接第一目标位姿B和最终目标位姿G ;入库路径连接车辆的起始位姿S和第一目标位姿B。库位内调整路径规划模块由限制方程生成子模块和优化求解子模块构成，其中限制方程生成子模块能够根据库位的位置和大小生成泊车路径限制方程组，优化求解模块能够对建立的限制方程组进行最优化求解，得到最优泊车目标位姿和最优库内调整路径，保证车辆泊车过程中能够得到最大的安全距离。通过库位内调整路径的最优化，能够降低车辆的库位内调整次数，或者在相同库位内调整次数的情况下，使车辆能够泊入尽可能小的库位。入库路径规划模块包含三个子模块无碰撞路径生成子模块、可执行路径生成子模块和路径选择子模块，该无碰撞路径规划子模块生成无碰撞泊车路径采用扇形区域法，选择扇形上距离障碍物距离最大的点，即最不可能与障碍物发生碰撞的点，连接这些点构成无碰撞泊车路径。可执行路径生成子模块规划的路径可能包含三种曲线类型最短距离曲线即RS曲线(Reeds&Shepp，s curve)、曲率连续曲线即 CC 曲线(Continuous-Curvature curve)和半曲率连续曲线即 hCC 曲线(half Continuous-Curvature curve)。路径选择子模块可以根据不同的原则选择合适的曲线类型(RS/hCC/CC)作为泊车路径，这些原则对应路径选择子模块的三种工作模式，其具体表述为I)泊车过程中前后运动方向改变的次数最少原则，对应模式I ;2)泊车过程中走过的距离最短原则，对应模式2 ；3)泊车过程中原地转向的次数最少原则，对应模式3 ；人机接口单元提供驾驶员意图输入接口，路径选择子模块接收驾驶员所输入的路径类型选择模式，并据此选择符合驾驶员意图的曲线作为可执行路径。当人机接口单元接收到的驾驶员意图为模式I时，接收驾驶员设定的允许的最大方向改变次数，当规划的路径超过这个设定值，放弃泊车；当人机接口单元接收到的驾驶员意图为模式2时，系统计算泊车路径，自行选择符合驾驶员意图的曲线作为可执行路径。当人机接口单元接收到的驾驶员意图为模式3时，接收驾驶员设定的允许的最大原地转向次数，当规划的路径超过这个设定值，放弃泊车。
本发明能够从任意起始位姿引导车辆泊入停车位，能够降低泊车过程中车辆原地转向和前后运动方向改变的次数；平行泊车情况下，可根据驾驶员设置的库位内允许最大调整次数选择合适的停车位，降低车辆对平行泊车库位长度的要求。
图I是本发明智能辅助泊车系统的系统框图。图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曲线选取模式I。图15—种泊车过程仿真结果示意图。
具体实施例方式本发明包括感知单元，用于获取库位信息和车辆的位姿信息；路径规划单元，用于规划出合适的泊车路径；驾驶控制单元，用于控制车辆沿规划路径行驶；人机接口单元，用于人机交互，包含驾驶员信息输入和系统信息输出，例如视觉显示、语音提示和机械振动
坐寸ο路径规划单元是整个系统的中心，它接收环境感知单元发来的库位信息和车辆位姿信息，向驾驶控制单元输出车辆需要跟踪行驶的路径参数，同时把规划的路径信息输出到人机接口单元，给出驾驶员相应提示。人机接口单元是系统和驾驶员进行信息交互的窗口，它把环境感知单元、路径规划单元、驾驶控制单元等发送来的信息输出，并能够接受驾驶员的输入，允许驾驶员设定允许的最大库位内调整次数、入库路径模式等参数。为了使车辆能够泊入更为狭小的库位，本发明允许车辆进行平行泊车时，在进入库位后做若干次姿态调整，驾驶员可以通过人机界面设定允许姿态调整的次数，当车辆泊入库位所需的库位内调整次数大于驾驶员设定次数时，系统放弃该库位。当库位内调整次数不大于驾驶员设定次数时，系统首先规划出车辆在库位内的姿态调整路径，然后规划出车辆驶入库位的路径。由于在规划库位内调整路径时进行了最优化设计，库位空间得到了充分利用。和其他辅助泊车系统相比，本发明能够在不增加库位内调整次数的情况下，使车辆泊入更小的库位。为了使车辆能够从任意起始位姿驶入库位，在入库路径规划模块中本发明采用了两步法路径规划方法，首先生成无碰撞路径，然后根据无碰撞路径生成车辆可跟踪行驶的路径。最终生成的入库路径有三种模式，每种模式对路径的舒适性、长短等有不同的侧重。驾驶员可以根据个人喜好通过人机界面对入库路径的模式进行选择。以下结合附图所示实施例对本发明作进一步的说明。(I)车辆控制模型
在泊车过程中，车速很低，轮胎的侧偏特性可以忽略不计，所以可以采用以下车辆控制模型(见图4)
其中，s是车辆所驶过的弧长，x、y是车辆在全局坐标系中的横、纵坐标，Θ是车辆的航向角，K是车辆后轴中点轨迹的曲率，O是曲率的变化率，Vmax是泊车中允许的最大速度，t是时间变量，sign( ·)是符号函数车辆的两个约束条件是I K I ^ K max,式(1-2)I σ I ^ σ max,式(1-3)其中，Kmax和是曲率和曲率变化率的最大值，它们是由车辆本身的物理特性决定的约束值。(2)库位内位姿调整路径规划模块I)库内调整次数的设定和判断在本发明中，当泊车库位的尺寸确定之后，系统就能够计算出车辆泊入库位所需要的库位内姿态调整次数。系统把计算得到的库位内调整次数和驾驶员设定的最大允许调整次数进行比较，当计算得到的调整次数大于驾驶员设定的允许调整次数时，系统放弃该库位。调整次数的设定可以采用多种方式，如采用键盘输入、触摸屏输入或者预设在系统控制器内。根据库位尺寸计算车辆库位内调整次数的方法也可以具有多种方式，如查表法，数值计算法等。在实施例I中，系统根据公式计算调整次数，具体方法如下I计算泊车深度泊车深度指完成泊车动作后车辆中心线和库位外沿的距离，其确定按照以下公式
\WJ2(Wp>Wv+eh)A = ^P式(2-1)
\W -WJ2-eh (Wp<Wv+eh)
其中h表示泊车深度，Wv表示车辆宽度，Wp表示库位宽度，eh表示泊车深度的安全距离。II计算车辆进行η (η为非负的整数)次姿态调整时所能泊入的最小库位长度根据以下公式计算车辆无需库位内姿态调整时所需的最小库位长度
其中Ltl表示无需库位内姿态调整时所能泊入的最小库位长度，h表示泊车深度，LV表示车辆长度，Wv表示车辆宽度，b表示车辆轴距，Lr表示车辆后轴中点距离车辆尾端的距离，R表示车辆外侧前轮的最小转弯半径，如图4所示。根据以下公式计算车辆进行η次库位内姿态调整时所能泊入的最小库位长度Ln = -kn arctan (n) +L0 式(2-3)其中Ln表示η次姿态调整所能泊入的最小库位长度，kn表示对应于调整次数η的调节参数，由标定得来，L0表示无需库位内姿态调整时所能泊入的最小库位长度。III计算得到泊入现有库位所需的库位内调整次数。根据步骤II所得的库位内调整次数和库位长度的关系，计算得到泊入现有库位所需的库位内调整次数，当调整次数大于预设值时，放弃该库位，并通过人机界面对驾驶员做出提示。2)库位内调整路径规划库位内调整路径规划模块利用车辆最小转弯半径圆弧规划车辆在库位内进行姿态调整的运动路径，其包含两个子模块限制方程生成子模块和优化求解子模块，其中限制方程生成子模块能够根据库位的位置和大小生成泊车路径限制方程组，车辆在库位内运动时其轨迹必须满足限制方程组；优化求解模块能够对建立的限制方程组进行最优化求解，得到最优泊车目标位姿和最优库位内调整路径，保证车辆泊车过程中得到最大的安全距离。通过库位内调整路径的最优化，能够减少车辆在库位内的调整次数，或者在相同库位内调整次数的情况下，使车辆能够泊入尽可能小的库位。一条典型的库位内调整路径如图5所示。限制方程组的建立方法可以具有多种方式，即可以在线完成，也可以预设在控制器的算法里。对方程组进行优化求解的方法也可以具有多种方式，如线性规划法或者迭代求解法等。在实施例2中，路径规划算法通过计算发现车辆泊入库位需要进行2次库位内姿态调整，小于驾驶员的预设值，其库位内调整路径规划方法如下所示I建立限制方程组如图5所示，库位是由障碍物I和障碍物2构成的，环境感知模块探测到库位长度SLmin,车辆需要在库位内做两次姿态调整，库位内的调整路径由两段圆弧组成，第一段是由第一目标位姿B到中间目标位姿C的向前运动，第二段是由中间目标位姿C到最终目标位姿G的向后运动。车辆沿规划路径依次到达B点、C点和G点，设B点坐标为(xB，yB, θ Β)，C点坐标为(Xd y。Θ J, G点坐标为(xe，y(；，Θ G),车辆的左上角点、右上角点、右后角点、左后角点依次表示为点a、b、c、d,路沿L6的纵坐标为y6,坐标轴的原点设定在前方障碍物的左后角点。车辆长度表示SLv，车辆宽度表示为Wv，车辆轴距表示为b，车辆后轴中点距离车辆尾端的距离表示为Lp当车辆到达G点后，要求车辆刚好和库位保持平行，并且达到所需的泊车深度，于是有yG = ~h 式(2—4)
Θ G = O 式(2-5)其中h表示泊车深度。可以设想，为了使车辆在库位内调整过程中获得最大的安全距离，车辆在B点、C点、G点时与最近障碍物的距离应该相同。车辆在G点时，其左后角点d距离后方障碍物最近，其横坐标为xGd = (-Lr+xG)式(2-6 )其中xed表示车辆在G点时其左后角点d的横坐标。车辆在B点时距离后方障碍物最近，其左后角点d点的横坐标为xBd = xB-Lr cos Θ B-0. 5ffvsin θ Β 式(2—7)其中xBd表示车辆在B点时其左后角点d点的横坐标。车辆在C点时距离前方障碍物最近，其右前角点b的横坐标为xcb = xc+(Lv-Lr) cos Θ c+0. 5Wvsin Θ c 式(2—8)其中表示车辆在C点时其右前角点b点的横坐标。要求车辆在B点、C点、G点处与最近障碍物的距离相同，并且要求距离大于零，据此可得xBd+Lmin =-Xcb > O 式(2-9)xGd+Lmin = xBd+Lmin > O 式(2-10)根据B点和C点的位置关系可得RmunrCOS Θ「R—COS θ β = yc_yB 式(2-11)-RminrSin Θ C+Rminrsin θ B = xc-xB 式(2-12)根据C点和G点的位置关系可得RminlSinQc = Xc-XG 式(2-13)Rminl-RminlCOS Θ c = H 式(2-14)其中Rmim表示车辆后轴中点向右的最小转弯半径，Rfflinl表示车辆后轴中点向左的最小转弯半径。假设车辆从库外以最小转弯半径圆弧倒车到达B点，要求倒车过程中车辆右前角点b不与库位发生碰撞，右后角点c不与路沿L6发生碰撞，可得限制不等式
权利要求
1.一种智能辅助泊车方法，其特征在于探测感知车辆周围环境，识别到可用于泊车的库位后，规划出合理的泊车路径，并控制车辆沿规划路径行驶，直到车辆完成泊车动作；并提供人机交互界面，接受驾驶员意图，显示泊车信息；其中，对泊车路径分两部分进行规划，首先规划出库位内调整路径，然后规划出入库路径。
2.根据权利要求I所述的智能辅助泊车方法，其特征在于 规划库位内调整路径时，采用最小转弯半径圆弧构成路径，首先建立库位边界对库位内姿态调整路径的限制方程，然后对方程进行优化求解，据此求出最优泊车目标位姿和最优库位内调整路径； 规划入库路径时采用两步法路径规划方法首先采用扇形区域法生成无碰撞路径，然后根据无碰撞路径生成车辆可跟踪行驶的路径，最后从多条可执行的路径中选择合适类型的路径。
3.根据权利要求2所述的智能辅助泊车方法，其特征在于提供人机界面，允许驾驶员对泊车路径进行个性化设定； (O依据库位尺寸计算出车辆需要的库位内姿态调整的次数，由驾驶员根据个人喜好通过人机界面对库位内调整次数上限值进行设定，当所需调整次数不大于驾驶员设定的上限值时，开始路径规划； (2)依据路径的舒适性、长短区分出不同的入库路径模式，由驾驶员根据个人喜好通过人机界面对入库路径的模式进行选择。
4.实现权利要求I至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 曲线(halfContinuous-Curvature curve)； 或者，该路径选择子模块可以根据不同的原则选择合适的曲线类型(RS/hCC/CC)作为泊车路径，这些原则对应路径选择子模块的三种工作模式，包括 1)泊车过程中前后运动方向改变的次数最少原则，对应模式I; 2)泊车过程中走过的距离最短原则，对应模式2； 3)泊车过程中原地转向的次数最少原则，对应模式3。
9.根据权利要求8所述的智能辅助泊车系统，其特征在于人机接口单元提供驾驶员意图输入接口，路径选择子模块接收驾驶员所输入的路径类型选择模式，并据此选择符合驾驶员意图的曲线作为可执行路径。
10.根据权利要求9所述的智能辅助泊车系统，其特征在于当人机接口单元接收到的驾驶员意图为模式I时，接收驾驶员设定的允许的最大前后运动方向改变次数，当规划的路径超过这个设定值，放弃泊车；当人机接口单元接收到的驾驶员意图为模式2时，系统计算泊车路径，自行选择符合驾驶员意图的曲线作为可执行路径；当人机接口单元接收到的驾驶员意图为模式3时，接收驾驶员设定的允许的最大原地转向次数，当规划的路径超过这个设定值，放弃泊车。
全文摘要
一种智能辅助泊车方法，对泊车路径分两部分进行规划，首先规划出库位内调整路径，然后规划出入库路径；并提供人机交互界面，接受驾驶员意图，显示泊车信息；一种实现所述方法的智能辅助泊车系统，该系统包括相互之间具有通信联系的环境感知单元、路径规划单元、驾驶控制单元和人机接口单元。本发明能够从任意起始位姿引导车辆泊入停车位，能够降低泊车过程中车辆原地转向和前后运动方向改变的次数；平行泊车情况下，可根据驾驶员设置的库位内允许最大调整次数选择合适的停车位，降低车辆对平行泊车库位长度的要求。
文档编号B60W30/06GK102963355SQ20121042972
公开日2013年3月13日 申请日期2012年11月1日 优先权日2012年11月1日
发明者陈慧, 张野, 程昆朋 申请人:同济大学