路径跟踪

2024-07-09 05:51| 来源: 网络整理| 查看: 265

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

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

【本文地址】

路径跟踪

路径跟踪

今日新闻

推荐新闻