CN106372758A - 一种辅助泊车系统的路径跟随方法及装置 - Google Patents

Abstract

一种智能辅助泊车系统的路径跟随方法，包括横向控制与纵向控制；所述横向控制通过时间尺度缩放及反馈线性化得到跟随误差线性信息，在此基础上对系统进行横向线性二次型最优控制；所述纵向控制包括对加速度随行驶距离连续变化的速度进行规划。一种智能辅助泊车系统的路径跟随装置，包括：输入单元、路径跟随单元和车辆控制单元；路径跟随单元包括横向控制和纵向控制两个子模块。横向控制子模块根据输入单元输入的信息，引入基于‑α稳定度设计的改进二次型性能指标，设计了改进的线性二次型最优控制器对系统进行闭环控制。纵向控制子模块采用加速度随行驶距离连续变化的算法设计合理的速度规划。保证泊车过程中的高精度、误差快速收敛性及乘客舒适性。

Description

一种辅助泊车系统的路径跟随方法及装置

技术领域

本发明属于汽车技术领域，涉及驾驶辅助技术，特别是智能泊车路径跟随装置。

背景技术

随着城市发展节奏的加快，机动车的保有量迅速增加，因此造成车位紧张、泊车困难等问题。狭小的泊车空间及驾驶员有限的视野范围给泊车入位操作增加了困难。因汽车消费者对车辆泊车辅助系统不断提出新的需求，智能辅助泊车系统成为近些年国内外研究的热点。

典型的智能辅助泊车系统是按照“感知—规划—跟踪”的分级模式来实现自动泊车功能的。该结构体系的优点在于各子系统界限清晰，次序井然，易于实现较复杂的智能行为。其中路径跟随模块是自动泊车系统中关键的一环，合理的路径跟随设计才能保证车辆按照规划的轨迹行驶，误差小且满足驾驶员舒适性要求。

目前研究路径跟随方法较多，这些方法大致归结为以下几种：1.预瞄控制，该种方法优点是结构简单，计算量小，但对于大曲率曲线跟随时，将会出现误差收敛速度慢，且跟随精度无法保证的问题；2.基于滑模变结构控制，该种方法优点是瞬态响应好，对外界变化的鲁棒性较好，但其抖振问题是滑模面选取过程中必然遇到的问题。3.反馈线性化，其优点为将车辆几何学模型精确线性化，再采用线性系统控制的方法进行控制。程昆朋在“全自动泊车系统的路径跟随”中以微分平坦理论为基础，将车辆运动学模型反馈线性化，然后采用极点配置方法使误差收敛，控制器运算量小，跟随精度高，但需要同时配置三个极点，极点的确定使得调试工作较为繁琐。以上方案各有改进，不过仍然存在不足。

因此，开发一种同时满足高精度、快速收敛性、参数易整定、乘客舒适性的泊车路径跟随装置具有重要的实际意义。

发明内容

本发明目的在于提供一种泊车辅助系统的路径跟随装置以克服现有技术中上述不足之处。当路径规划模块将规划的路线传递给路径跟随单元时，车辆可以很好的跟随规划的路线；当存在初始误差时，可以快速收敛至规划路线，且整个算法的参数易整定，同时该算法针对多段法泊车路径同样适用。

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

一种智能辅助泊车系统的路径跟随方法，包括横向控制与纵向控制；所述横向控制通过时间尺度缩放及反馈线性化得到跟随误差线性信息，在此基础上对系统进行横向线性二次型最优控制；所述纵向控制包括对加速度随行驶距离连续变化的速度进行规划。

优选地，所述横向控制的输入包括，泊车路径、实时车辆位姿、车速以及车辆方向盘转角；所述横向控制的输出包括，期望方向盘转角。

优选地，所述横向控制采用的算法包括在路径跟随线性系统基础上提出的改进线性二次型最优控制和开环辅助控制。

优选地，在改进线性二次型最优控制中引入基于-α稳定度设计的改进二次型性能指标，通过保证性能指标最小求解得到最优输入，从而计算出期望方向盘转角。

优选地，控制跟随误差以指数形式收敛至0，保证了收敛的快速性。

优选地，对于算法存在速度为零的奇异点问题，通过设置速度阈值，采取改进LQR最优控制器同时结合开环辅助控制的策略加以解决；实现多段法泊车路径的跟随工况，且跟随误差无累积。

优选地，所述纵向控制采用加速度随行驶距离连续变化的算法以保证舒适性；依据不同情况设置最大车速及最大加速度的变化率以提高算法的灵活性。

一种智能辅助泊车系统的路径跟随装置，包括：输入单元、路径跟随单元和车辆控制单元。输入单元：由路径规划与位姿估计两个子模块组成，路径规划计算出期望的泊车路径，作为路径跟随单元的一个输入，位姿估计模块返回车辆的实时位姿，作为路径跟随单元的另一个输入。路径跟随单元：包括横向控制和纵向控制两个子模块。横向控制：根据输入单元输入的信息，设计最优控制器对系统进行闭环控制。纵向控制：通过设计合理的速度规划，保证车辆泊车过程纵向的乘客舒适性。车辆控制单元：由路径跟随单元计算出的期望方向盘转角和期望车速首先通过执行器控制模块得到转向力矩和驱动力矩，再输入至车辆底盘，控制车辆的横向运动与纵向运动。

所述的泊车辅助系统的路径跟随单元，其输入分别为：期望泊车路径，由路径规划单元规划出的入库期望泊车路径输入到路径跟随单元；实时车辆位姿，路径跟随单元结合位姿估计单元反馈车辆的实时位姿进行实时闭环控制。

所述的泊车辅助系统的路径跟随单元，其输出分别为：期望车速，结合实际整车车速从而得到驱动力矩，从而控制泊车过程中车辆的纵向运动；期望方向盘转角，结合实际方向盘转角从而得到转向力矩，从而控制泊车过程中车辆的横向运动。

所述的泊车辅助系统的路径跟随单元的算法，包括如下步骤：(1)采用车辆运动学模型，同时将转向系统近似为一阶惯性环节，共同构成车辆模型；(2)引入新的时间变量s_d，对车辆模型进行时间尺度缩放，得到新时间域下的状态方程；(3)以同济大学程昆朋在“汽车技术”期刊发表的“全自动泊车系统的路径跟随”论文中采用的基于微分平坦理论的反馈线性化方法，得到线性系统为基础，引入二次型性能指标，设计线性二次型最优控制器(LQR)对线性系统进行闭环控制；(4)对LQR控制器进行-α稳定度设计，得到改进LQR最优控制器提高系统的稳定性及收敛速度；(5)采用开环辅助控制的方法解决算法奇异点问题，保证跟随算法可以适用于多段法泊车路径跟随(6)设计合理车速规划模块，使泊车过程中车速平缓变化，提高乘客舒适性。

由于采用了上述技术方案，本发明具有如下有益效果：采用LQR最优控制可以使跟随误差以指数形式快速收敛，保证了系统的高精度与误差快速收敛性；引入二次型性能指标，并进行后续的-α稳定度设计，可以使设计参数具有明确的物理意义，便于算法参数整定。

附图说明

图1是本发明一种智能辅助泊车系统的路径跟随装置示意图。

图2是本发明路径跟随单元示意图。

图3是本发明路径跟随算法框架图。

图4是本发明-α稳定度设计后极点位置示意图。

图5是本发明多段泊车路径示意图。

图6是本发明多段泊车路径路径跟随方法示意图。

图7是本发明路径跟随单元纵向控制示意图。

图8是本发明路径跟随单元速度规划子模块示意图。

图9是本发明路径跟随装置控制流程图。

图10是本发明平行泊车路径跟随结果示意图。

图11是本发明垂直泊车路径跟随结果示意图。

具体实施方式

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

本发明一种智能辅助泊车系统的路径跟随装置，如图1所示，包括：输入单元、路径跟随单元和车辆控制单元。

其中路径跟随单元包括横向控制和纵向控制两个子模块，如图2所示。横向控制：根据位姿估计单元返回的车辆实时位姿，与规划路径进行比较，得到实时跟踪误差，通过时间尺度缩放及反馈线性化得到新时间域下的线性系统，引入跟随误差二次型性能指标，线性二次型最优控制器(LQR)对线性系统进行闭环控制。并采用开环辅助控制的方法解决算法奇异点问题，保证跟随算法可以适用于多段法泊车路径跟随。纵向控制：通过设计合理的速度规划，保证车辆泊车过程纵向的乘客舒适性。本发明是一种辅助泊车系统的路径跟随方法，通过横向控制与纵向控制，保证了泊车过程中的高精度、误差快速收敛性和乘客舒适性。

1、横向控制

1)跟随误差线性系统

(1)车辆模型

在车辆泊车过程中，车速很低，车速要控制在2km/h-8km/h之间，因此可以忽略轮胎的侧向动力学的影响。可用如下公式表示：

为简化起见，将车辆的转向机构可看作一阶惯性环节，如公式(1.2)所示：

其中T是系统的时间常数，u₁是目标前轮转角。

针对车辆运动学模型，引入新的时间变量s_d，其物理意义为车辆的目标行驶距离，并引入时间尺度缩放输入系数u₂(t)，其具体时间尺度缩放函数为：

$\frac{{\mathrm{ds}}_d\left(t\right)}{dt}=\frac{v\left(t\right)}{u_2\left(t\right)}---\left(1.3\right)$

对式(1.1)与式(1.2)中的各式进行链式法则转换，并用与作为替代，可得到新的时间域s_d的状态空间表达式如公式(1.4)所示：

(2)反馈线性化

根据文献同济大学程昆朋在“汽车技术”期刊发表的“全自动泊车系统的路径跟随”论文中以及Bernhard Mueller,J D.等人在文章“Orbital tracking control for carparking via control of the clock using a nonlinear reduced order steering-angle observer”所述，系统(1.4)是一个典型的微分平坦系统，可以通过动态反馈、静态反馈和坐标变换转化为一个线性系统，在得到线性系统后设计合适的线性控制器从而实现系统的闭环控制，具体的控制器框图如图3所示。

反馈线性化后的系统L，定义z＝[x,x′,x″,y,y′,y″]^T，其具体形式如式(1.5)

$\begin{array}{ccc}{z}_1^{\′}=z_2,& z_2^{\′}=z_3,& z_3^{\′}=w_1\\ z_4^{\′}=z_5,& z_5^{\′}=z_6,& z_6^{\′}=w_2\end{array}---\left(1.5\right)$

这里引入两个新的输入w₁和w₂，因此路径跟随算法的核心转变为设计合适的线性控制器控制新的输入w₁和w₂，从而使跟踪系统闭环收敛。

(3)路径跟随误差线性系统

本节首先通过车辆运动学转换得到的线性系统推导出路径跟随误差的线性系统，再通过设计合适的LQR最优控制器，从而使跟随误差快速收敛。

设e表示跟随的误差，其中x方向的跟随误差可表示如下：

$\begin{array}{c}{e}_x=\left(\begin{array}{ccc}{e}_x& e^{\′}& e^{\′\′}\end{array}\right)\\ =\left(\begin{array}{ccc}x-x_d& x^{\′}-x_d^{\′}& x^{\′\′}-x_d^{\′\′}\end{array}\right)\end{array}---\left(1.6\right)$

其中x为车辆在x方向的实时坐标，x_d为路径规划单元输入的期望路径在x方向的坐标。

由式(1.5)可得x方向与y方向的跟随误差线性系统：

$\begin{array}{c}{e_x}^{\′}={\mathrm{Ae}}_x+{\mathrm{Bw}}_1\\ {e_y}^{\′}={\mathrm{Ae}}_y+{\mathrm{Bw}}_2\end{array}---\left(1.7\right)$

其中

为对误差进行闭环控制，采用状态反馈方法，从而实现跟随误差的快速收敛性。

$\begin{array}{c}{w}_1=-{\mathrm{Ke}}_x\\ w_2=-{\mathrm{Ke}}_y\end{array}---\left(1.8\right)$

状态反馈后的状态方程为：

$\begin{array}{c}{e_x}^{\′}=(A-BK)e_x\\ {e_y}^{\′}=(A-BK)e_y\end{array}---\left(1.9\right)$

2)LQR最优控制器设计

(1)LQR控制在路径跟随中的应用

在得到误差的线性系统后，可采用极点配置或者LQR最优控制等经典方法进行控制，因为LQR最优控制的权重函数有具体的物理意义，便于设计，因此采用LQR最优控制对状态反馈的系数K进行设计。

给定完全可控线性定常系统

$\stackrel{\·}{x}\left(t\right)=Ax\left(t\right)+Bu\left(t\right)---\left(1.10\right)$

使得系统的性能指标函数J达到最小值：

$J=\frac{1}{2}{\∫}_0^{\mathrm{\∞}}\[x^T\left(t\right)Qx\left(t\right)+u^T\left(t\right)Ru\left(t\right)\]dt---\left(1.11\right)$

式中，状态变量的加权阵Q为3×3维正半定对称阵；输入变量的加权阵R为常数。通过LQR最优控制寻找输入量u^*(t)，使输出跟踪输入，并保证性能指标J最小的同时使系统达到平衡。u^*(t)为最优控制的充要条件为

u^*(t)＝-Kx(t),K＝R^-1B^TP (1.12)

式中：P为下述矩阵Riccati代数方程的正定对称解

-PA-A^TP+PBR^-1B^TP-Q＝0 (1.13)

x方向误差线性系统经过状态反馈后的状态方程为：

$\begin{array}{c}{e_x}^{\′}=(A-BK)e_x\\ =(A-{\mathrm{BR}}^{-1}{B}^{T}P)e_x\end{array}---\left(1.14\right)$

其系统的性能指标为：

$\begin{array}{c}J=\frac{1}{2}{\∫}_0^{\mathrm{\∞}}\[{e_x}^T\left(s_d\right){\mathrm{Qe}}_x\left(s_d\right)+{w_1}^T\left(s_d\right){\mathrm{Rw}}_1\left(s_d\right)\]{\mathrm{ds}}_d\\ =\frac{1}{2}{\∫}_0^{\mathrm{\∞}}\[\left(\begin{array}{ccc}{e}_x& e_x^{\′}& e_x^{\′\′}\end{array}\right)\left(\begin{array}{ccc}{Q}_{11}& & \\ & Q_{22}& \\ & & Q_{22}\end{array}\right)\left(\begin{array}{c}{e}_x\\ e_x^{\′}\\ e^{\′\′}\end{array}\right)+{w_1}^T\left(s_d\right){\mathrm{Rw}}_1\left(s_d\right)\]{\mathrm{ds}}_d\\ =\frac{1}{2}{\∫}_0^{\mathrm{\∞}}\[\[Q_{11}{e_x}^2+Q_{22}{e}_x^{\′2}+Q_{33}{e}_x^{\′\′2}\]+{{\mathrm{Rw}}_1}^2\left(s_d\right)\]{\mathrm{ds}}_d\end{array}---\left(1.15\right)$

根据式(1.14)(1.15)所知，LQR最优控制器设计的关键是选择合适的加权矩阵Q和R，并根据式(1.12)(1.13)求出反馈增益K。

针对LQR最优控制在路径跟随中的应用，式(1.15)表征了跟随系统的性能指标。由式(1.15)可知，权矩阵Q＝diag(Q₁₁,Q₂₂,Q₃₃)对角线上各权系数分别代表各项指标的相对重要性；R代表了能量损耗的相对重要性，其作用在于限制控制器的输出不至于太大而导致难以控制。Q和R的选取对闭环系统动态性能的影响很大。一般情况下，Q中某个元素数值增加时，其对应的状态变量的响应速度加快；R增加时，对控制输入的约束增加。因此根据Q和R的影响，设计路径跟随LQR最优控制器，使得跟随误差快速收敛。

(2)指数收敛特性分析

对于路径跟随系统，跟踪误差的快速收敛特性是保证车辆能够很好的跟随规划轨迹的重要指标。本节将对该算法的收敛特性进行分析。

选取二次型函数作为状态方程(1.9)的李雅普诺夫函数，即

V(x)＝e_x ^TMe_x (1.16)

其中矩阵M满足如下李雅普诺夫方程：

(A-BK)^TM+M(A-BK)＝-I (1.17)

I为单位矩阵，由李雅普诺夫第二法可知，当M为正定实对称矩阵时，则该系统为大范围渐近稳定的。

记矩阵M的最大特征值为λ_max(M)，比率为γ＝λ(I)/λ_max(M)，可以有如下推导：

$\begin{array}{c}{V}^{\′}=-{e_x}^T{\mathrm{Ie}}_x\\ =-\frac{\mathrm{\λ}\left(I\right)}{{\mathrm{\λ}}_{\max }\left(M\right)}{e_x}^T\[{\mathrm{\λ}}_{\max }\left(M\right)I\]e_x\\ \≤-{{\mathrm{\γe}}_x}^T{\mathrm{Me}}_x\end{array}---\left(1.18\right)$

由于γe_x ^TMe_x＝γV,从而可得：

V′≤-γV(1.19)

对式(1.19)进行积分可知

$V\≤V\left(0\right)e^{-{\mathrm{\γs}}_d}---\left(1.20\right)$

又因为V＝e_x ^TMe_x≥λ_min(M)||e_x||²

$\left|\right|e_x\left(s_d\right)\left|\right|\≤\sqrt{\frac{V\left(0\right)}{{\mathrm{\λ}}_{\min }\left(M\right)}}\·e^{-\frac{\mathrm{\γ}}{2}{s}_d}---\left(1.21\right)$

由式(1.21)可知，状态反馈后的跟随系统在新的时间s_d域下，X方向的跟随误差e_x与Y方向的跟随误差e_y至少以γ/2的指数收敛速度进行收敛。

(3)改进LQR控制器设计

当在设计LQR控制器时，当希望所有的系统闭环极点均位于s-平面的s＝-α线左侧，其中α＞0，这样得到基于-α稳定度设计的改进二次型性能指标，新的指标函数定义为如下：

$J={\∫}_0^{\mathrm{\∞}}{e}^{2\mathrm{\α}t}(x^{T}Qx+u^{T}Ru)dt---\left(1.22\right)$

引入一个新的状态变量ξ(t)，使得ξ(t)＝e^αtx(t)，且新的控制量为v(t)＝e^αtu(t)，则原系统的指标函数改写成为：

$J={\∫}_0^{\mathrm{\∞}}({\mathrm{\ξ}}^{T}Q\mathrm{\ξ}+v^{T}Rv)dt---\left(1.23\right)$

原系统的状态方程改写成：

$\stackrel{\·}{\mathrm{\ξ}}=(A+\mathrm{\α}I)\mathrm{\ξ}+Bv---\left(1.24\right)$

从而改进的Riccati代数方程为：

-P(A+αI)-(A+αI)^TP+PBR^-1B^TP-Q＝0 (1.25)

新的最优控制策略变成：

$\begin{array}{c}{v}^{*}\left(t\right)=-{\mathrm{K\ξ}}^{*}\left(t\right)\\ =-R^{-1}{B}^T{\mathrm{P\ξ}}^{*}\left(t\right)\end{array}---\left(1.26\right)$

反馈后的系统为：

$\begin{array}{c}\stackrel{\·}{\mathrm{\ξ}}=(A+\mathrm{\α}I)\mathrm{\ξ}+B(-K\mathrm{\ξ})\\ =\[(A-BK)+\mathrm{\α}I\]\mathrm{\ξ}\end{array}---\left(1.27\right)$

LQR算法可以保证所有的极点均位于左半平面，因此，因此反馈系数K可以保证所有的极点位于左半平面，而实际系统为可以保证极点均位于x＝-α的左侧，如图4所示：位于AB的左侧。

经过简单变换，可得

u^*(t)＝-R^-1B^TPx^*(t) (1.28)

3)开环辅助控制

由式(1.3)可知，为了使式(1.3)成立，要求

u₂(t)≠0 (1.29)

即系统在此处存在一个奇异点。对于车辆来说，u₂(t)＝0意味着车速为0，所以车速在趋近于0时控制效果变差，当车速为0时，系统将失去控制。在泊车过程中，以下情况下将会出现车速为0的情况：1)车辆起步阶段，初始车速为0；2)车辆停止阶段，最终车速为0；3)当泊车路径由多段组成，如图5所示，车辆由第一段倒车行驶切换至下一段前进行驶时，车速必然会出现车速为0状态。

针对奇异点问题，采用的解决方案为闭环控制为主+辅助开环控制的策略。闭环控制即本发明提出的基于LQR最优控制的路径跟随方法其输出的期望方向盘转角为而开环控制为根据规划的路径各点的曲率计算出车辆方向盘的对应转角，设为其具体求解方法如式(1.30)(1.31)所示。

$R=\frac{1}{\mathrm{\κ}}---\left(1.31\right)$

式中i为车辆转向传动比，L为车辆轴距，R为路径转弯半径，κ为路径各点对应的曲率。

当车速足够小时，由于奇异点的存在，闭环控制算法将控制效果变差，这里设定v_critical代表临界车速，可设为0.1m/s，则闭环控制为主+辅助开环控制的策略如式(1.32)所示：

在开环控制与闭环控制切换瞬间，由于不可避免的带来角度的突变，但由于处于车辆起步阶段或者停止阶段，车速极低，对应的方向盘转角也会很小，极小的角度突变不会带来整体跟随误差及乘客舒适性的影响。

对于泊车路径由多段曲线组成时，如图5所示的三段泊车路径AB→BC→CD,对应的车速方向为“倒车→前进→倒车”。当车辆跟随AB段结束后，由于B点车速为0，因此B点附近采用开环控制，当车辆再次启动开始对BC段进行跟随时，此时需要进行信息重置，才能进一步进行闭环路径跟随，如图6所示。

2、纵向控制

通过前文所述的时间尺度缩放，车辆的横向运动控制与纵向运动控制进行了解耦，横向运动的新的时间变量为s_d，其物理意义为车辆的目标行驶距离，而纵向运动控制是保证车辆能够精确地跟随每一段规划路径，对车辆的行驶距离s进行精确控制，二者之间通过时间尺度缩放输入u₂进行相关转换。纵向运动控制器的框图如图7所示。

由纵向运动控制器框图可以看出，纵向运动存在两个闭环控制，外层闭环控制是根据实际行驶距离与期望行驶距离的差值规划出合理的目标车速，内层闭环控制是根据实际车速与期望车速的差值设计车速控制器，使车辆可以按照期望的车速行驶。

车速规划模块是设计合理的目标车速，这里的设计原则是保证车辆在泊车过程中，避免发生急加速或者急刹车等影响乘客舒适性的情况发生。纵向舒适性可由加速度加以判断，当纵向加速度连续变化无突变时，车辆纵向不会产生阶跃力，因此满足舒适性要求，正是基于这样原则，规划车速如图8所示。

图中v为期望车辆车速，a为期望车辆加速度，τ为期望车辆加加速度。从开始泊车到泊车终止整个过程，车辆会经过加速-匀速-减速的过程，在加速与减速阶段，车辆的加速度均为连续变化。其对应的数学表达式如下所示：

$v\left(s\right)=\left\{\begin{array}{cc}\frac{1}{2}{\mathrm{\&tau;}}_{max}{s}^2& 0\&le;s<\frac{l_a}{2}\\ 0.5*{\mathrm{\&tau;}}_{max}*\&lsqb;2l_{a}s-s^2-\frac{{l_a}^2}{2}\&rsqb;& \frac{l_a}{2}\&le;s<l_a\\ v_{max}& l_a\&le;s<l-l_a\\ v_{max}+{\mathrm{\&tau;}}_{max}\&lsqb;s(l-l_a)-0.5{(l-l_a)}^2-0.5s^2\&rsqb;& l-l_a\&le;s<l-\frac{l_a}{2}\\ {\mathrm{\&tau;}}_{max}\&lsqb;\frac{s^2}{2}-ls+\frac{l^2}{2}\&rsqb;& l-\frac{l_a}{2}\&le;s<l\end{array}\right.---\left(2.1\right)$

$a\left(s\right)=\left\{\begin{array}{cc}{\mathrm{\&tau;}}_{\max }s& 0\&le;s<\frac{l_a}{2}\\ {\mathrm{\&tau;}}_{\max }{l}_a-{\mathrm{\&tau;}}_{\max }s& \frac{l_a}{2}\&le;s<l_a\\ 0& l_a\&le;s<l-l_a\\ -{\mathrm{\&tau;}}_{\max }s+{\mathrm{\&tau;}}_{\max }(l-l_a)& l-l_a\&le;s<l-\frac{l_a}{2}\\ {\mathrm{\&tau;}}_{\max }s-{\mathrm{\&tau;}}_{\max }l& l-\frac{l_a}{2}\&le;s<l\end{array}\right.---\left(2.2\right)$

$\mathrm{\&tau;}\left(s\right)=\left\{\begin{array}{cc}{\mathrm{\&tau;}}_{max}& 0\&le;s<\frac{l_a}{2}\\ -{\mathrm{\&tau;}}_{max}& \frac{l_a}{2}\&le;s<l_a\\ 0& l_a\&le;s<l-l_a\\ -{\mathrm{\&tau;}}_{\max }& l-l_a\&le;s<l-\frac{l_a}{2}\\ {\mathrm{\&tau;}}_{max}& l-\frac{l_a}{2}\&le;s<l\end{array}\right.---\left(2.3\right)$

式中，s表示车辆的行驶距离，τ_max为最大加加速度，v_max为最大车速，l为该段泊车路径的长度。l_a表示车辆的加速距离。其中v_max与τ_max可根据乘客驾驶员需求而设定，增加了设计的灵活度。

3、算法验证

图9为路径跟随单元输入与输出的示意图。路径跟随单元输出期望方向盘转角与期望车速，输入到执行器控制单元，得到转向力矩与驱动力矩，分别输入给车辆的EPS与驱动电机，控制泊车过程的横向运动与纵向运动。

图10为平行泊车路径跟随效果示意图。

图11为三段法垂直泊车路径跟随效果示意图。

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

Claims (10)

1.一种智能辅助泊车系统的路径跟随方法，包括横向控制与纵向控制；其特征在于：所述横向控制通过时间尺度缩放及反馈线性化得到跟随误差线性信息，在此基础上对系统进行横向线性二次型最优控制；所述纵向控制包括对加速度随行驶距离连续变化的速度进行规划。

2.根据权利要求1所述的智能辅助泊车系统的路径跟随方法，其特征在于：所述横向控制的输入包括，泊车路径、实时车辆位姿、车速以及车辆方向盘转角；所述横向控制的输出包括，期望方向盘转角。

3.根据权利要求2所述的智能辅助泊车系统的路径跟随方法，其特征在于：所述横向控制采用的算法包括在路径跟随线性系统基础上提出的改进线性二次型最优控制和开环辅助控制。

4.根据权利要求3所述的智能辅助泊车系统的路径跟随方法，其特征在于：在改进线性二次型最优控制中引入基于-α稳定度设计的改进二次型性能指标，通过保证性能指标最小求解得到最优输入，从而计算出期望方向盘转角。

5.根据权利要求4所述的智能辅助泊车系统的路径跟随方法，其特征在于：控制跟随误差以指数形式收敛至0，保证了收敛的快速性。

6.根据权利要求2所述的智能辅助泊车系统的路径跟随方法，其特征在于：对于算法存在速度为零的奇异点问题，通过设置速度阈值，采取改进线性二次型最优控制器同时结合开环辅助控制的策略加以解决；实现多段法泊车路径的跟随工况，且跟随误差无累积。

7.根据权利要求1所述的智能辅助泊车系统的路径跟随方法，其特征在于：所述纵向控制采用加速度随行驶距离连续变化的算法以保证舒适性；依据不同情况设置最大车速及最大加速度的变化率以提高算法的灵活性。

8.一种实现权利要求1至7中任一所述方法的智能辅助泊车系统的路径跟随装置，包括输入单元、路径跟随单元和车辆控制单元，路径跟随单元包含横向控制与纵向控制两个子模块，其特征在于：所述横向控制子模块通过时间尺度缩放及反馈线性化得到跟随误差线性系统，在此基础上采用二次型性能指标对系统进行横向线性二次型最优控制；所述纵向控制子模块中包括加速度随行驶距离连续变化的速度规划分模块。

9.根据权利要求8所述的智能辅助泊车系统的路径跟随装置，其特征在于：所述横向控制子模块的输入包括期望泊车路径、实时车辆位姿；由路径规划子模块规划出的所述泊车路径输入到路径跟随单元；其输出包括期望方向盘转角，输出至车辆控制单元从而控制泊车过程中车辆的横向运动；所述的横向控制算法包括在路径跟随线性系统基础上设计的改进线性二次型最优控制器和开环辅助控制两个子模块。

10.根据权利要求9所述的智能辅助泊车系统的路径跟随装置，其特征在于：所述改进线性二次型最优控制器子模块中包含基于-α稳定度设计的改进二次型性能指标，通过保证性能指标最小求解得到最优输入，从而计算出期望方向盘转角；优选地，该控制器使跟随误差以指数形式收敛至0，保证了收敛的快速性；优选地，通过设置速度阈值，采取改进线性二次型最优控制器同时结合开环辅助控制的方法解决算法存在速度为零的奇异点问题；优选地，速度规划模块采用加速度随行驶距离连续变化的算法以保证纵向乘客舒适性，依据不同情况设置最大车速及最大加速度的变化率以提高算法的灵活性。