[제4호 - 지능형 운전 자동차 시리즈 용어 및 개념 해석] 8장: 차량 동역학 모델 기반 LQR 경로 추적 알고리즘

1. 차량 2자유도 동역학 모델

차량의 2자유도 동역학 모델의 개략도는 위에 나와 있습니다. "자동차 이론" 교과서에 따르면 양의 코너링 힘은 음의 측면 슬립 각도를 생성합니다. 기호 연산을 대입하여 첫 번째 과정의 2자유도 동적 모델의 연속 상태공간 방정식을 다음 수식 1과 같이 다시 작성합니다. 위의 수식을

연속 상태공간 방정식으로 다시 작성합니다.

2. 선형 이차 형태

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

이는 Linear-Quadratic Regulator를 사용하여 해결할 수 있습니다. 그 중 Q는 반양정부호 상태 가중치 행렬이고, R은 양정부호 제어 가중치 행렬이며, 둘은 일반적으로 대각행렬로 취해진다. Q 행렬의 요소가 클수록 추적 편차가 신속하게 0에 접근합니다. R 행렬의 요소 변경이 크다는 것은 제어 입력이 가능한 한 작아야 함을 의미합니다.

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

그 중 P는 식 (4)의 Riccati 방정식의 해이다:

3. 행렬 이득 계수 K를 해결하는 세 가지 방법

방법 1

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

방법 2

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

방법 3

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

4. 매트랩 프로그램

주요 프로그램은 다음과 같습니다

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,:);
iniSpd = 1;
iniHeading = refHeading(1);
%% 主程序
% 当前车辆信息
currentPos = iniPos;
currentSpd = iniSpd;
currentHeading = iniHeading;

% 赋初值
Delta = 0;

% 轨迹跟踪实际量
posSet = currentPos;
headingSet = currentHeading;
spdSet = currentSpd;
latErrSet = [];

latError_last = 0;
yaw_error_last = 0;
omega = 0;
actualCur = 0.0001;
targetIdx = 1;
% 循环
while true
     % 寻找预瞄距离范围内最近路径点
     if size(refPos,1) - targetIdx> 20
         dist = sqrt((refPos(targetIdx:targetIdx+20,1)-currentPos(1)).^2 + ...
             (refPos(targetIdx:targetIdx+20,2)-currentPos(2)).^2);
     else
         dist = sqrt((refPos(targetIdx:end,1)-currentPos(1)).^2 + ...
             (refPos(targetIdx:end,2)-currentPos(2)).^2);
     end
     [~,targetIdx_temp] = min(dist);
     targetIdx = targetIdx + targetIdx_temp - 1;

    
    % 判断是否超出索引
    if targetIdx >= size(refPos,1)
        break
    end
    
    % 速度P控制器
    a = 0.8* (refSpeed-currentSpd)/dt;
    
    % LQR控制器
    [Delta,delta_r,latError,yaw_error] =  LQR_controller(targetIdx,currentPos,currentSpd,currentHeading,refPos,refHeading,refDelta,refSpeed,refCur,L,Q,R,dt,latError_last,yaw_error_last,actualCur,omega);    
   
    % 如果误差过大，退出循迹
    if abs(latError) > 3
        disp('误差过大，退出程序!\n')
        break
    end
    
    % 更新状态
    [currentPos,currentHeading,currentSpd,omega] = update(currentPos,currentHeading,currentSpd, a,Delta, dt,L);
    
    % 保存每一步的实际量
    posSet(end+1,:) = currentPos;
    headingSet(end+1,:) = currentHeading;
    spdSet(end+1,:) = currentSpd;
    latErrSet(end+1,:) = [cumLength(targetIdx),latError];
    
    %
    latError_last = latError;
    yaw_error_last = yaw_error;
    
    % 计算实际路径曲率
    if size(posSet,1) >= 3
        actualCur = getCur(posSet(end-2:end,1), posSet(end-2:end,2));
    else
        actualCur = 0.00001;
    end

end
latErr_mean = mean(abs(latErrSet(:,2)));

%%  画图
% 画路径跟踪图
figure
hold on
grid on
plot(refPos(:,1), refPos(:,2),  'Color', 'b');
scatter(posSet(:,1),posSet(:,2),10,'MarkerEdgeColor','r',...
    'MarkerFaceColor','r')   % 坐标离散点
plot(posSet(:,1), posSet(:,2),  'Color', 'r');
xlabel('纵向坐标 / m');
ylabel('横向坐标 / m');
legend('规划车辆轨迹', '实际行驶轨迹');


% 画误差图
figure
hold on
grid on
plot(latErrSet(1:end,1), latErrSet(1:end,2),'LineWidth', 3,  'Color', 'b');
plot([0,latErrSet(end,1)], [latErr_mean,latErr_mean], 'r:','LineWidth', 3);
xlabel('距离 / m');
ylabel('横向误差 / m');
legend('横向误差', '平均横向误差');

%% LQR控制
function [Delta,delta_r,latError,yaw_error] =  LQR_controller(targetIdx,currentPos,currentSpd,currentHeading,refPos,refHeading,refDelta,refSpd,refCur,L,Q,R,dt,latError_last,yaw_error_last,actualCur,omega)
% 车辆参数
m = 1723;          % 质量
lf = 1.232;        % 质心到前轴的距离
lr = 1.468;        % 质心到后轴的距离
C_af = -66900;     % 前轮侧偏刚度
C_ar = -62700;     % 后轮侧偏刚度
Iz = 4175;         % 饶Z轴的转动惯量

% 求位置、航向角参考量
x_r = refPos(targetIdx,1);
y_r = refPos(targetIdx,2);
heading_r = refHeading(targetIdx);
delta_r = refDelta(targetIdx);

% 求位置、航向角的误差
x_error  = currentPos(1) - x_r;
y_error = currentPos(2) - y_r;
yaw_error =  currentHeading - heading_r;

% 根据百度Apolo，计算横向误差
latError = y_error*cos(heading_r) - x_error*sin(heading_r);

% 计算变化率
latError_dot = currentSpd * sin(yaw_error); 
yawError_dot = omega - currentSpd*refCur(targetIdx)*cos(yaw_error)/(1+refCur(targetIdx)*latError);

% 将误差值赋值到状态量
X(1,1) = latError; 
X(2,1) = latError_dot;  
X(3,1) = yaw_error;
X(4,1) = yawError_dot;

% 由状态方程矩阵系数，计算K
A = [0,  1,  0, 0;
    0,  2*(C_af + C_ar)/(m*currentSpd), -2*(C_af + C_ar)/m, 2*(lf*C_af - lr*C_ar)/(m*currentSpd);
     0,  0,  0, 1;
     0,  2*(lf*C_af - lr*C_ar)/(Iz*currentSpd), -2*(lf*C_af - lr*C_ar)/Iz, 2*(lf^2*C_af + lr^2*C_ar)/(Iz*currentSpd)];
B = [0; -2*C_af/m; 0; -2*lf*C_af/Iz];

K = lqr(A,B,Q,R);

% 获得控制量
u = -K * X; 
Delta = max(u,-0.44);
Delta = min(Delta,0.44);

end

%% 更新状态
function [pos_new, currentHeading_new, currentSpd, omega] = update(currentPos,currentHeading,currentSpd, a,Delta, dt,L)
currentSpd = currentSpd + a*dt;
omega =  currentSpd / L * tan(Delta);
currentHeading_new = currentHeading + currentSpd / L * tan(Delta) * dt;
pos_new(1) = currentPos(1) + currentSpd*cos(currentHeading_new)*dt;
pos_new(2) =  currentPos(2) + currentSpd*sin(currentHeading_new)*dt;
end

4. 시뮬레이션 결과

시뮬레이션 결과는 다음과 같습니다.