路径跟踪—Matlab LQR库函数无法代码生成，那就手写一个

Matlab环境下无法对lqr,dlqr,care,dare函数进行代码生成，如果您想用在Simulink环境下使用该函数，会告诉您无法进行代码生成，这时您加上外部函数是可以在Simulink环境下使用的。

// An highlighted block
coder.extrinsic(function)

但是如果您想将在线lqr函数部署到您的实际控制器中，比如车辆路径跟踪dlqr算法的时候，您又不想事先求解好反馈增益K，或者您需要时变的Q和R矩阵，那么就无法将lqr函数代码生成部署到快速控制原型或者工控机ROS系统。Matlab给出的建议是手工编码求解黎卡提方程。具体原理不阐述了，直接给出Matlab M函数。编写原理也很简单，求解迭代黎卡提方程即可，迭代步数较多，是否会Overrun看您的控制器性能了。下面基于车辆路径跟踪动力学模型的dlqr算法给出求解反馈增益K代码。

// An highlighted block
function [K,i] = fcn(a, b, m, cr, cf, Iz, vx)
dt = 0.005; %采样时间
PN_1 =zeros(4,4);
P=zeros(4,4);
I =eye(4);
i=0;%该变量用于观察迭代步数
vx =double( uint8(vx*10/10));
if vx>0.1
    A=[0,1,0,0;0,(cf+cr)/(m*vx),-(cf+cr)/m,(a*cf-b*cr)/(m*vx);
        0,0,0,1;0,(a*cf-b*cr)/(Iz*vx),-(a*cf-b*cr)/Iz,(a*a*cf+b*b*cr)/(Iz*vx)];
    B=[0; -cf/m; 0; -a*cf/Iz];
    
    Ad = (eye(4)-A*dt/2)\(eye(4)+A*dt/2); %离散化A矩阵
    Bd = B*dt;%离散化B矩阵
    
    Q=[20 0 0 0;0 1 0 0;0 0 20 0;0 0 0 1 ];
    R=10;
    PN =Q;
    err =10^(-1);%该精度根据需要调整，误差越小迭代次数越多
    error = 1;
    while(error>err)
        PN_1 = Q+Ad'*PN/(I+Bd/R*Bd'*PN)*Ad;%黎卡提方程
        error = norm(PN-PN_1);
        PN =PN_1;
        i = i+1;%观察迭代步数
        if i>400
            break;
        end
    end
    P =PN_1;
    K = (R+Bd'*P*Bd)\Bd'*P*Ad;
else
    K = [0 0 0 0];
end
 
end

该模型就是基于车辆动力学的误差方程。推导过程参考随处可见的文档。离散化过程是前向欧拉和中点欧拉，有兴趣您可以推导一下。