[发明专利]一种自动驾驶汽车低速自动泊车横向控制方法

权利要求书

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

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

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

步骤(2.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)

其中，fai_r_dot为fai_r的导数；再联合(4)(5)(8)获得车辆的运动学模型：

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

步骤(2.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]；

并且，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为目标轨迹点的坐标和横摆角；x_dot,y_dot,fai_dot为x,y,fai的导数；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分别为状态量和控制量的权重矩阵；

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

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