基于车辆运动学的LQR轨迹跟踪控制 (matlab)

基于车辆运动学的LQR轨迹跟踪控制可以分为三个模块

车辆运动学模型、轨迹跟踪控制模块、LQR求解器

先上仿真结果

导入纽伯格林赛道数据，预先设置最优轨迹，基于自行车模型，利用LQR进行轨迹跟踪，结果如下

一、车辆运动学模型

这里使用了最基本的自行车模型

function [vehicle_new] = VehicleModel(vehicle_old,vehilce_con)

    x_old = vehicle_old(1);
    y_old = vehicle_old(2);
    theta_old = vehicle_old(3);
    v = vehilce_con(1);
    delta = vehilce_con(2);
    ts = 0.1;
    L = 1.4;

    x_new = x_old + v * cos(theta_old) * ts;
    y_new = y_old + v * sin(theta_old) * ts;
    theta_new = theta_old + tan(delta * v/L) * ts;

    vehicle_new = [x_new; y_new; theta_new];

end

在进行LQR求解的时候，需要用到线性化+离散化后的自行车模型，下面是公式推导

二、轨迹跟踪控制逻辑

3DOF vehicle model 是车辆运动学模型

LQR模块负责求解矩阵F

同时在map中匹配目标路径点（主要用于匹配前轮转角）

此部分代码如下：

LQR_tracking函数作用是 根据当前车辆状态及目标路径点生成控制量（车速、前轮转角）

VehicleModel函数就是前面提到的车辆运动学模型 

其余部分就是做一些数据更新及记录的工作

while stop_signal == 0
    
    disp(index);
    
    [v, delta, stop_signal, index_new] = LQR_tracking(vehicle_state,ref_Posx,ref_Posy,reftheta,refdelta,index);

    disp(index_new);

    vehicle_con = [v; delta];

    vehicle_state_new = VehicleModel(vehicle_state,vehicle_con);

    vehicle_state = vehicle_state_new;

    vehicle_state_record = [vehicle_state_record, vehicle_state];

    vehicle_con_record = [vehicle_con_record, vehicle_con];

    index = index_new;

end

三、LQR求解器

这部分主要参考《控制之美2》中提到的离散型LQR求解方法

此部分代码如下：

LQR_gain函数主要作用是嵌套在LQR_tracking函数中负责求解矩阵F

function [F] = LQR_gain(A,B,S,Q,R)

    tol = 1e-3;
    max_iter = 200;
    i = 0;
    diff = Inf;
    F_old = Inf;

    P = S;

    while diff > tol

        F_new = inv(transpose(B)*P*B+R) * transpose(B) * P * A;

        diff = abs(max(F_old-F_new));

        F_old = F_new;

        P_new = Q + transpose(F_old)*R*F_old + transpose(A-B*F_old)*P*(A-B*F_old);

        P = P_new;

        i = i + 1;

        if i > max_iter
            break;
        end
    end

    F = F_new;

end

四、其余工作

在前期将控制量及车辆状态量均记录在vehicle_con_record和vehicle_state_record矩阵中，最后利用matlab的画图功能将其以动画形式复现即可。