【第4期-智能驾驶汽车系列术语概念解析】第8节：基于车辆动力学模型的LQR路径跟踪算法

一、车辆二自由度动力学模型

车辆二自由度动力学模式示意图如上所示，根据《汽车理论》教材，正侧偏力产生负的侧偏角。代入符号运算，对第一期课程的二自由度动力学模型连续状态空间方程复写如下式1：

将上式改写为连续状态空间方程，即为：

二、线性二次型

	期望的系统响应特性有以下两点：1）跟踪偏差能够快速、稳定地趋近于零，并保持平衡；2）前轮转角控制输入尽可能小。
	这是一个典型的多目标优化最优控制问题，目标函数可以表示为跟踪过程累计的跟踪偏差与累计的控制输入的加权，如下式3：

可利用线性二次型调节器（Linear-Quadratic Regulator）求解。其中，Q为半正定的状态加权矩阵, R为正定的控制加权矩阵，且 两者通常取为对角阵；Q矩阵元素变大意味着希望跟踪偏差能够快速趋近于零；R矩阵元素变大意味着希望控制输入能够尽可能小。

	因此，前一项优化目标表示跟踪过程路径偏差的累积大小，第二项优化目标表示跟踪过程控制能量的损耗，这样就将轨迹跟踪控制问题转化为一个最优控制问题。
	对于式（3）目标函数的优化求解，解出的最优控制规律u是关于状态变量X的线性函数: 

其中，P是式（4）黎卡提方程的解：

三、关于矩阵增益系数K的三种求解方法

方法1

针对黎卡提方程，进行循环迭代，不断缩小等式左右的差距，当差值小于某个阈值时，认为等式左右两边相等，从而求得P矩阵，最后代入式（4）得到K；

方法2

	同样用方法1，不同点在于在线查表。由于矩阵K是由A，B，Q，R四个矩阵计算而来，可以首先在离线状态下计算四个矩阵的常见范围，并代入方法（1）进行计算，最终做成一个四维查表。

方法3

	采用MATLAB的lqr函数直接求解，如下图：

四、Matlab程序

主程序如下

clc
clear
close all
load  refPath.mat

%% 相关参数定义
dt = 0.1;
L = 2.9;
Q = 1*eye(4);
R = 1*eye(1);

%% 轨迹处理
% 定义参考轨迹
refPos = refPath_line;
refSpeed = 10;    % 参考速度

% 计算航向角
diff_x = diff(refPos(:,1)) ;
diff_y = diff(refPos(:,2)) ;
cumLength = cumsum(sqrt(diff_x.^2 + diff_y.^2));
refHeading = atan2(diff_y , diff_x);                   % 航向角
refHeading(end+1) = refHeading(end);

% 曲率
for i = 1:size(refPos,1)-2
    x = refPos(i:i+2,1);
    y = refPos(i:i+2,2);
    refCur(i) = abs(getCur(x,y));
end
refCur(i+1) = refCur(i);
refCur(i+2) = refCur(i);

% 根据阿克曼转向原理，计算参考前轮转角
refDelta = atan(L*refCur);

% 车辆初始状态定义
iniPos = refPos(1