CN113110500A - 一种自动驾驶汽车低速自动泊车横向控制方法 - Google Patents

Abstract

本发明公开了一种自动驾驶汽车低速自动泊车横向控制方法，包括：规划层规划出泊车轨迹并发送到控制层；定位层对车辆的当前位置实时定位获得定位信息并发送到控制层；底层接收控制层发出的控制指令并实时反馈车辆的当前状态信息；控制层根据车辆运动学模型得出车辆运动的状态空间方程，并建立LQR控制模型，进行求解得出泊车过程中的反馈车辆前轮转角；利用LQR控制模型计算得到实际的反馈前轮转角deltaf_feedback，并结合求得的前馈输出转角deltaf_feedforward得到最终的总输出转角，根据总输出转角控制车辆泊车过程中前向行驶和后向行驶的过程。本发明的提高了控制的稳定性，同时降低了系统的稳态误差，优化了车辆控制结构，实现全自动泊车控制。

Description

一种自动驾驶汽车低速自动泊车横向控制方法

技术领域

本发明涉及一种自动驾驶汽车低速自动泊车横向控制方法，属于自动驾驶横向控制的技术领域。

背景技术

自动驾驶在近年迎来了高速发展的契机，其中自动驾驶车的实际控制关乎车辆的安全性，稳定性以及舒适性，是自动驾驶技术落地的关键性技术。现有的自动驾驶控制技术分为横向控制技术以及纵向控制技术。横向控制关乎车辆的稳定性，现有的横向控制技术包括纯跟踪算法，Stanley算法，LQR算法，MPC算法等等。动力学LQR算法对车辆进行动力学建模，从而在中高速运动中更能符合车辆的运动特性，在横向控制效果中表现较好。

而在低速泊车场景中，车辆转弯时往往面对转角较大的场景，此时动力学模型由于对转角的线性化处理，难以实现精准的横向控制，所以现有的低速横向控制以运动学模型为主。而常用的横向控制算法以PID和纯跟踪算法为主。但PID算法难以适应泊车下多场景的变化以及存在控制参数调节难度大的问题，而纯跟踪算法设置合适的预瞄距离是个很大的挑战，往往设置不合理会导致车辆转弯过程内切或跟踪过程较慢的情况。

发明内容

针对现有自动驾驶车辆在低速运动下的控制不精确以及参数调节难度大的问题，本发明提供一种自动驾驶汽车低速自动泊车横向控制方法，结合LQR算法的优点和运动学模型在低速的良好表现，提出运动学LQR算法用于车辆泊车过程中的前进，后退等过程。

本发明具体采用以下技术方案解决上述技术问题：

一种自动驾驶汽车低速自动泊车横向控制方法，包括以下步骤：

步骤1：自动驾驶车辆的规划层根据当前环境规划出泊车轨迹并发送到控制层；自动驾驶车辆的定位层对车辆的当前位置实时定位获得定位信息并发送到控制层；自动驾驶车辆的底层接收控制层发出的控制指令并实时反馈车辆的当前状态信息；

步骤2：自动驾驶车辆的控制层根据车辆运动学模型得出车辆运动的状态空间方程，并建立LQR控制模型；

步骤3：自动驾驶车辆的控制层根据建立的LQR控制模型进行求解，推导出泊车过程中的反馈车辆前轮转角deltaf_feedback；

步骤4：自动驾驶车辆的控制层结合规划层发送的泊车轨迹、定位层发送的定位信息、底层反馈的车辆的当前状态信息，利用LQR控制模型计算得到实际的反馈前轮转角deltaf_feedback，并结合求得的前馈输出转角deltaf_feedforward得到最终的总输出转角，根据总输出转角控制车辆泊车过程中前向行驶和后向行驶的过程。

进一步地，作为本发明的一种优选技术方案，所述步骤1中自动驾驶车辆的规划层根据当前环境规划出泊车轨迹，具体为：自动驾驶车辆的规划层规划出到达泊车位的轨迹点集，由轨迹点集形成泊车轨迹，其中轨迹点集包括规划的所有轨迹点，每个轨迹点的信息包括：车辆规划的全局纵坐标x_n，横坐标y_n，航向信息fai_n，以及规划点的曲率信息Kappa_n。

进一步地，作为本发明的一种优选技术方案，所述步骤1中自动驾驶车辆的定位层获得的定位信息包括车辆当前的全局纵坐标x_a，全局横坐标y_a，航向信息fai_a。

进一步地，作为本发明的一种优选技术方案，所述步骤1中自动驾驶车辆的底层反馈车辆的当前状态信息包括车速和档位，以及车辆当前的前轮转角。

进一步地，作为本发明的一种优选技术方案，所述步骤2中建立LQR控制模型，包括：

假设在点(xr,yr,fai_r)处满足车辆运动学模型，在该点处对运动学模型进行线性化并据此构造得到状态空间矩阵：X_dot＝A*X+B*U；

其中，X_dot＝[x_dot-xr_dot；y_dot-yr_dot；fai_dot-fai_r_dot]；

此处x,y,fai为目标轨迹点的坐标和横摆角，xr,yr,fai_r分别为车辆的后轴轴心位置的横和纵坐标、后轴航向角，x_dot,y_dot,fai_dot为x,y,fai的导数，xr_dot,yr_dot,fai_r_dot为xr,yr,fai_r的导数，X_dot是自车与目标轨迹参数的导数的误差矩阵；

并且，A＝[0,0,-vr*sin(fai_r)；

0,0,vr*cos(fai_r)；

0,0,0；]；

X＝[x-xr；y-yr；fai-fai_r]；

B＝[0；0；vr/L/cos(deltaf)/cos(deltaf)]；

U＝delta；

公式中，L表示车辆轴距；vr表示后轴速度；X为跟踪误差矩阵；U为控制输入矩阵，代表最优前轮转角；A与B为线性矩阵；

将状态空间矩阵离散化获得LQR控制模型：X(k+1)＝At*X(k)+Bt*U(k)，其公式中At＝I+A*T；Bt＝B*T；I为单位矩阵，T为单位时间；X(k+1)为k+1时刻的跟踪误差矩阵，X(k)为k时刻的跟踪误差矩阵；U(K)为k时刻控制输入矩阵；

然后，定义如下的评价函数：

J(n)＝X(0)_T*Q*X(0)+...+X(n-1)_T*Q+X(n-1)+U(0)_T*R*U(0)+...+U(n-1)_T*R*U(n-1)

其中，X(i)为车辆在i处的状态空间矩阵，U(i)为车辆在i处的控制输入矩阵；X(i)_T和U(i)_T分别为X(i)和U(i)的矩阵转置；Q和R分别为状态量和控制量的权重矩阵。

进一步地，作为本发明的一种优选技术方案，所述步骤4中自动驾驶车辆的控制层利用LQR控制模型计算得到实际的反馈前轮转角deltaf_feedback，并结合求得的前馈输出转角deltaf_feedforward得到最终的总输出转角，具体为：

根据定位层所发送车辆当前位置的定位信息预测下一时刻车辆的定位信息；

根据预测的下一时刻车辆的定位信息和规划层发送的泊车轨迹，计算出当前自车预测位置点到泊车轨迹中距离最近的轨迹点，获得该距离最近的轨迹点的信息；

根据距离最近的轨迹点的信息更新控制层中的参数，并由更新参数后的LQR控制模型求得实际的反馈前轮转角deltaf_feedback；

根据车辆的轴距和跟踪目标点的曲率求得前馈输出转角deltaf_feedforward；

将求得的反馈前轮转角deltaf_feedback和前馈输出转角deltaf_feedforward结合，得到最终的总输出转角deltaf_output。

本发明采用上述技术方案，能产生如下技术效果：

本发明的方法设计在低速泊车工况下，采用运动学模型，适用于转弯较大以及考虑平滑切换前进与后退的场景，由于结合了LQR算法，将最优控制代入了横向控制的过程中，提高了控制的稳定性，同时降低了系统的稳态误差。本发明提出运动学LQR控制模型用于车辆泊车过程中的前进，结合运动学模型在低速的良好表现，优化了车辆控制结构，无需驾驶员参与，实现全自动泊车控制。

因此，本发明方法在实际泊车过程中，可以快速准确地达到自由泊车的目的，稳定性较高。

附图说明

图1为本发明方法采用的车辆运动学模型原理示意图。

图2为本发明方法构建的LQR模型原理示意图。

图3为本发明方法中横向控制的原理示意图。

具体实施方式

下面结合说明书附图对本发明的实施方式进行描述。

本发明涉及一种自动驾驶汽车低速自动泊车横向控制方法，该方法适用于自动驾驶车的低速泊车系统，其中低速泊车系统包括规划层、定位层、底层、控制层。本方法运用的系统下自动驾驶车辆的规划层和定位层均正常工作，且底层能实时发送反馈，控制层实时进行控制。具体地，本方法包括以下步骤：

步骤1：自动驾驶车辆的规划层根据当前环境规划出泊车轨迹并发送到控制层；自动驾驶车辆的定位层对车辆的当前位置实时定位获得定位信息并发送到控制层；自动驾驶车辆的底层接收控制层发出的控制指令并实时反馈车辆的当前状态信息；具体如下：

步骤(1.1)规划层规划出泊车轨迹

当进入低速泊车系统后，自动驾驶车辆的规划层根据用户或决策需求或当前环境规划出到达泊车位的轨迹点集，由轨迹点集形成泊车轨迹，并将泊车轨迹发送到控制层。轨迹点集包括规划的所有轨迹点，每个轨迹点的信息包括：车辆规划的全局纵坐标x_n，横坐标y_n，航向信息fai_n，以及规划点的曲率信息Kappa_n。

步骤(1.2)定位层对车辆的当前位置实时定位获得定位信息

自动驾驶车辆的定位层负责实时高精度给出车辆的当前定位信息，且发送频率要高，本发明中定位层发送的定位信息包括车辆当前的全局纵坐标x_a，全局横坐标y_a，航向信息fai_a。

步骤(1.3)自动驾驶车辆的底层实时反馈车辆的当前状态信息

自动驾驶车辆的底层接收控制层发出的控制指令，并反馈车辆的当前状态信息。本方法对车辆进行横向控制，故假设车辆纵向已控，只发送车辆前轮转角信号给车辆底层。底层反馈车辆的当前车速和档位，以及车辆当前的前轮转角。

步骤2：自动驾驶车辆的控制层根据车辆运动学模型得出车辆运动的状态空间方程，并建立LQR控制模型，具体如下：

步骤(2.1)获取车辆运动学方程，具体为：

如图1，车辆的后轴轴心位置可由坐标(xr，yr)表示，后轴速度为vr，前轴轴心位置可由(xf，yf)表示，前轴速度为vf，车辆的后轴航向角表示为fai_r，前轮转角表示为deltaf，车辆轴距为L。此时，车辆的后轴轴心速度可表示为：

vr＝xr_dot*cos(fai_r)+yr_dot*sin(fai_r) (1)

其中，xr_dot和yr_dot分别为xr和yr的导数；

以及，前后轴的运动学约束为：

xf_dot*sin(fai_r+deltaf)-yf_dot*cos(fai_r+deltaf)＝0 (2)

xr_dot*sin(fai_r)-yr_dot*cos(fai_r)＝0 (3)

由式(1)(2)(3)可得：

xr_dot＝vr*cos(fai_r) (4)

yr_dot＝vr*sin(fai_r) (5)

根据前后轮的几何关系可知：

xf＝xr+L*cos(fai_r) (6)

yf＝yr+L*sin(fai_r) (7)

将式(4)(5)(6)(7)代入(2)(3)，可解得：

fai_r_dot＝vr*tan(deltaf)/L (8)

联合(4)(5)(8)得车辆的运动学模型：

[xr_dot；yr_dot；fai_r_dot]＝[cos(fai_r)；sin(fai_r)；tan(deltaf)/L]*vr (9)

步骤(2.2)根据车辆运动学模型得出车辆运动的状态空间方程，并建立LQR控制模型，其原理如图2所示，具体为：

假设在点(xr,yr,fai_r)处满足上述车辆运动学模型，在该点处对运动学模型进行线性化，并注意由于是横向控制，故控制量只有一个deltaf，vr可视为常量，线性化的结果为：

[x_dot-xr_dot；y_dot-yr_dot；fai_dot-fai_r_dot]＝

[0,0,-vr*sin(fai_r)；

0,0,vr*cos(fai_r)；

0,0,0；]*

[x-xr；y-yr；fai-fai_r]+[0；0；vr/L/cos(deltaf)/cos(deltaf)]*(delta-deltaf) (10)

其中，x_dot,y_dot,fai_dot为x,y,fai的导数。x,y,fai为目标轨迹点的坐标和横摆角。则(10)式可用下面的状态空间矩阵表达：

X_dot＝A*X+B*U； (11)

X_dot＝[x_dot-xr_dot；y_dot-yr_dot；fai_dot-fai_r_dot]；

其中，A＝[0,0,-vr*sin(fai_r)；

0,0,vr*cos(fai_r)；

0,0,0；]；

X＝[x-xr；y-yr；fai-fai_r]；

B＝[0；0；vr/L/cos(deltaf)/cos(deltaf)]；

U＝delta；

上述公式中，x,y,fai为目标轨迹点的坐标和横摆角，xr,yr,fai_r分别为车辆的后轴轴心位置的横和纵坐标、后轴航向角，x_dot,y_dot,fai_dot为x,y,fai的导数，xr_dot,yr_dot,fai_r_dot为xr,yr,fai_r的导数，X_dot是自车与目标轨迹参数的导数的误差矩阵；以及；X为跟踪误差矩阵；U为控制输入矩阵，代表最优前轮转角；A与B为抽象出的线性矩阵。在上述跟踪误差矩阵X中，x-xr、y-yr、fai-fai_r为目标轨迹与当前自车轨迹的误差，为实际LQR算法需要控制收敛至0的误差参数，分别对应位置误差，朝向误差。

由于在实际控制器中处理LQR时为离散的信号，所以需要对理论的LQR模型做离散化，即将(11)式离散化，可得：

X(k+1)＝At*X(k)+Bt*U(k) (12)

At＝I+A*T；

Bt＝B*T；

其中，I为单位矩阵，T为单位时间。X(k+1)为k+1时刻的跟踪误差矩阵，X(k)为k时刻的跟踪误差矩阵；U(K)为k时刻控制输入矩阵，代表k时刻最优前轮转角。

然后，定义如下的评价函数：

J(n)＝X(0)_T*Q*X(0)+...+X(n-1)_T*Q+X(n-1)+U(0)_T*R*U(0)+...+U(n-1)_T*R*U(n-1)

其中，X(i)为车辆在i处的状态空间矩阵，U(i)为车辆在i处的控制输入矩阵；X(i)_T和U(i)_T分别为X(i)和U(i)的矩阵转置；Q和R分别为状态量和控制量的权重矩阵。

步骤3：自动驾驶车辆的控制层根据建立的LQR控制模型进行求解，推导出泊车过程中的反馈车辆前轮转角deltaf_feedback，具体如下：

令U(k)＝-K*X(k) (13)

则(12)式可变为：X(k+1)＝(At-Bt*K)*X(k) (14)

通过设计矩阵K，可以使得闭环系统的极点达到期望的状态。本发明中的LQR控制的目标是设计反馈矩阵K使得评价函数J最小。

为了找到矩阵K，先假设存在一个常量矩阵P，使得：

为求解P矩阵，令K＝-Bt_T*P/R (16)

得到Racatti方程P*At+At_T*P-P*Bt/R*Bt_T*P＝-Q (17)

由此求得P矩阵，代入(16)式求取K矩阵；将K矩阵代入(13)式，可解得U(k)，在步骤4中，代入实际参数计算出U(k),赋值给deltaf_feedback，即可得到当前的反馈车辆前轮转角deltaf_feedback。

步骤4：自动驾驶车辆的控制层结合规划层发送的泊车轨迹、定位层发送的定位信息、底层反馈的车辆的当前状态信息，利用LQR控制模型计算得到实际的反馈前轮转角deltaf_feedback，并结合求得的前馈输出转角deltaf_feedforward得到最终的总输出转角，根据总输出转角控制车辆泊车过程中前向行驶和后向行驶的过程，其原理如图3所示，具体如下：

步骤(4.1)控制层根据定位层所发送车辆当前位置的定位信息预测下一时刻车辆的定位信息，具体为：

根据定位层发送的定位信息包括车辆当前的全局纵坐标x_a，全局横坐标y_a，航向信息fai_a，并且根据车辆规划层轨迹点集生成的泊车轨迹。根据当前的定位信息预测下一时刻控制时车辆的定位。假设车辆的控制延迟时间为t_delay,车速为v，那么预测出的下一时刻车辆位置为：

x_p＝x_a+v*t_delay*cos(fai_a)；

y_p＝y_a+v*t_delay*sin(fai_a)；

fai_p＝fai_a+v*t_delay*tan(fai_a)/L；

步骤(4.2)根据预测的下一时刻车辆的定位信息和规划层发送的泊车轨迹，计算出当前自车预测位置点到泊车轨迹中距离最近的轨迹点，获得该距离最近的轨迹点的信息，具体为：

根据预测的下一时刻车辆的定位信息(x_p，y_p，fai_p)和轨迹点集(x,y)可计算出当前自车预测位置点到轨迹点集中距离最近的轨迹点,该轨迹点的信息为在轨迹点集中的序列号为m，则全局纵坐标x_m，横坐标y_m，航向信息fai_m，以及规划点的曲率信息Kappa_m。

步骤(4.3)根据距离最近的轨迹点的信息更新控制层中的参数，并由更新参数后的LQR控制模型求得实际的反馈前轮转角deltaf_feedback，具体为：

得到距离最近的轨迹点的信息后，可以更新LQR控制器中的参数，将其中的vr用当前的车速v代替，x用x_m代替，y用y_m代替，fai用fai_m代替，P矩阵可初始化为Q矩阵。

由Q和R分别为状态量和控制量的权重矩阵，Q矩阵可设为：[Qx,0,0；0,Qy,0；0,0,Qfai],其中的Qx，Qy和Qfai为常数，可根据具体实际情况进行标定。由于只有一个控制量，所以R为1*1矩阵，设为标量1即可。

将上述更新后的控制层参数输入LQR控制模型，根据步骤3求解的K矩阵代入LQR控制模型，最终LQR控制模型计算得到并输出U(k)，即最优前轮转角，将其值赋给反馈前轮转角deltaf_feedback，进而得到实际的反馈前轮转角deltaf_feedback。

步骤(4.4)由前馈模型根据车辆的轴距L和跟踪目标点的曲率Kappa_m求得前馈输出转角deltaf_feedforward，即deltaf_feedforward＝atan(L*Kappa_m)。

步骤(4.5)将求得的实际反馈前轮转角deltaf_feedback和前馈输出转角deltaf_feedforward结合，得到最终的总输出转角deltaf_output，表示为：

deltaf_output＝deltaf_feedback+deltaf_feedforward；

根据LQR控制器输出的最终的总输出转角deltaf_output控制车辆前进，当泊车过程中需要倒车时，将速度变为负值，其他参数保持不变，即可完成泊车中对车辆的横向控制。

由此，本发明的方法提出的LQR算法用于车辆泊车过程中的前进并结合运动学模型在低速的良好表现，将最优控制代入了横向控制的过程中，提高了控制的稳定性，同时降低了系统的稳态误差，优化了车辆控制结构，无需驾驶员参与，实现全自动泊车控制。

上面结合附图对本发明的实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。

Claims (6)

1.一种自动驾驶汽车低速自动泊车横向控制方法，其特征在于，包括以下步骤：

步骤1：自动驾驶车辆的规划层根据当前环境规划出泊车轨迹并发送到控制层；自动驾驶车辆的定位层对车辆的当前位置实时定位获得定位信息并发送到控制层；自动驾驶车辆的底层接收控制层发出的控制指令并实时反馈车辆的当前状态信息；

步骤2：自动驾驶车辆的控制层根据车辆运动学模型得出车辆运动的状态空间方程，并建立LQR控制模型；

步骤3：自动驾驶车辆的控制层根据建立的LQR控制模型进行求解，推导出泊车过程中的反馈车辆前轮转角deltaf_feedback；

步骤4：自动驾驶车辆的控制层结合规划层发送的泊车轨迹、定位层发送的定位信息、底层反馈的车辆的当前状态信息，利用LQR控制模型计算得到实际的反馈前轮转角deltaf_feedback，并结合求得的前馈输出转角deltaf_feedforward得到最终的总输出转角，根据总输出转角控制车辆泊车过程中前向行驶和后向行驶的过程。

2.根据权利要求1所述自动驾驶汽车低速自动泊车横向控制方法，其特征在于，所述步骤1中自动驾驶车辆的规划层根据当前环境规划出泊车轨迹，具体为：自动驾驶车辆的规划层规划出到达泊车位的轨迹点集，由轨迹点集形成泊车轨迹，其中轨迹点集包括规划的所有轨迹点，每个轨迹点的信息包括：车辆规划的全局纵坐标x_n，横坐标y_n，航向信息fai_n，以及规划点的曲率信息Kappa_n。

3.根据权利要求1所述自动驾驶汽车低速自动泊车横向控制方法，其特征在于，所述步骤1中自动驾驶车辆的定位层获得的定位信息包括车辆当前的全局纵坐标x_a，全局横坐标y_a，航向信息fai_a。

4.根据权利要求1所述自动驾驶汽车低速自动泊车横向控制方法，其特征在于，所述步骤1中自动驾驶车辆的底层反馈车辆的当前状态信息包括车速和档位，以及车辆当前的前轮转角。

5.根据权利要求1所述自动驾驶汽车低速自动泊车横向控制方法，其特征在于，所述步骤2中建立LQR控制模型，包括：

假设在点(xr,yr,fai_r)处满足车辆运动学模型，在该点处对运动学模型进行线性化并据此构造得到状态空间矩阵：X_dot＝A*X+B*U；

其中，X_dot＝[x_dot-xr_dot；y_dot-yr_dot；fai_dot-fai_r_dot]；

此处x,y,fai为目标轨迹点的坐标和横摆角，xr,yr,fai_r分别为车辆的后轴轴心位置的横和纵坐标、后轴航向角，x_dot,y_dot,fai_dot为x,y,fai的导数，xr_dot,yr_dot,fai_r_dot为xr,yr,fai_r的导数，X_dot为自车与目标轨迹参数的导数的误差矩阵；

并且，A＝[0,0,-vr*sin(fai_r)；

0,0,vr*cos(fai_r)；

0,0,0；]；

X＝[x-xr；y-yr；fai-fai_r]；

B＝[0；0；vr/L/cos(deltaf)/cos(deltaf)]；

U＝delta；

公式中，L为车辆轴距；vr为后轴速度；X为跟踪误差矩阵；U为控制输入矩阵，代表最优前轮转角；A与B为线性矩阵；

将状态空间矩阵离散化获得LQR控制模型：X(k+1)＝At*X(k)+Bt*U(k)，其公式中At＝I+A*T；Bt＝B*T；I为单位矩阵，T为单位时间；X(k+1)为k+1时刻的跟踪误差矩阵，X(k)为k时刻的跟踪误差矩阵；U(K)为k时刻控制输入矩阵；

然后，定义如下的评价函数：

J(n)＝X(0)_T*Q*X(0)+...+X(n-1)_T*Q+X(n-1)+U(0)_T*R*U(0)+...+U(n-1)_T*R*U(n-1)

其中，X(i)为车辆在i处的状态空间矩阵，U(i)为车辆在i处的控制输入矩阵；X(i)_T和U(i)_T分别为X(i)和U(i)的矩阵转置；Q和R分别为状态量和控制量的权重矩阵。

6.根据权利要求1所述自动驾驶汽车低速自动泊车横向控制方法，其特征在于，所述步骤4中自动驾驶车辆的控制层利用LQR控制模型计算得到实际的反馈前轮转角deltaf_feedback，并结合求得的前馈输出转角deltaf_feedforward得到最终的总输出转角，具体为：

根据定位层所发送车辆当前位置的定位信息预测下一时刻车辆的定位信息；

根据预测的下一时刻车辆的定位信息和规划层发送的泊车轨迹，计算出当前自车预测位置点到泊车轨迹中距离最近的轨迹点，获得该距离最近的轨迹点的信息；

根据距离最近的轨迹点的信息更新控制层中的参数，并由更新参数后的LQR控制模型求得实际的反馈前轮转角deltaf_feedback；

根据车辆的轴距和跟踪目标点的曲率求得前馈输出转角deltaf_feedforward；

将求得的反馈前轮转角deltaf_feedback和前馈输出转角deltaf_feedforward结合，得到最终的总输出转角deltaf_output。

Images