前言

路径跟踪，轨迹跟踪的算法有基于运动学和基于动力学的，其中LQR算法基于车辆动力学网上有很多，毕竟百度开源了apollo，该算法通过简单改进后的实车效果还是非常棒的，之前通过dSPACE MicroAutoBox做了试验，横向误差都是厘米级。由于动力学模型需要估计车辆重量，轮胎侧偏刚度，还有绕Z轴转动惯量，需要估计的参数非常多，相对而言，运动学则简单了很多，不需要估计很多参数，且听某FSAC车队的大佬说，运动学模型非常稳定，于是我动手用Simulink搭了一下，跟各位分享一下建模过程，因为在网上找的基本都是MPC，感觉不用MPC都不好意思出门了，就是找不到基于运动学的LQR算法，所以动手实践下，搭起来也非常简单，并且年后找个机会实车测试一下。

一、运动学模型

运动学模型很多博客都提到过，这里分享个视频，如果不清楚运动学模型推导的可以仔细跟着做一遍。
【控制】模型预测控制 MPC 【合集】Model Predictive Control
这里，我就通过图片的方式直接呈现运动学模型，该运动学模型需要线性化和离散化，因为实际系统都是离散的，所以根据需要的采样时间进行离散化即可。

其中，Vr是车辆后轴速度，φ是车辆横摆角，δf是车辆前轮转角，l是轴距。其中基于运动学几何关系的前轮转角与转弯半径关系非常重要，通过下式就可以算出期望的前轮转角。

然后把上面方程线性化，离散化后，直接得出如下方程：

将模型变为了偏差的方程，分别是状态变量和控制变量都是偏差值：实际值减去期望的偏差值。那么可以发现，系统矩阵和控制矩阵都是时变的，因此必须在每个控制时刻都进行黎卡提方程的求解得到控制量，注意，此刻的控制量是变化量，必须加上期望值，才是最后的控制量，输入给车辆。这里和动力学模型是大不同的。
期望速度很好理解，提前给定就行。前轮转角则是通过车辆轴距和当前车辆与路径最近点的曲率值乘积的反正切值。有了上述模型，则可以进行建模与仿真。

二、建模仿真

这里给各位分享一下，我是如何搭建这个模型的，并且给出整个控制算法的代码，该代码是可以进行Code Generation的。下面就是仿真模型，该模型分为三部分。第一部分为控制算法，第二部分为车辆模型，第三部分是可视化界面，这部分可有可无。Matlab版本：R2019b。

先看一下车辆模型，为了方便用的车辆动力学工具箱的3dof 车辆模型搭建。

模型输入：前轮转角(rad); 车辆纵向速度(m/s)
模型输出：车辆后轴位置横坐标x(m);车辆后轴位置横坐标 y(m);车辆横摆角(rad)。
其余输出为可视化界面使用，有纵向速度vx,侧向速度vy，以及横摆角速度φdot。
rate trasition模块是因为整个模型使用变步长求解，因为定步长求解这个车辆模型很慢。但是控制算法是原子子系统，10ms运行一次。因为运动学离散时间为10ms。memory模块是为了避免代数环。
控制算法部分如图所示，控制算法通过m函数编写,将该子系统设置为原子子系统采样时间10ms。

该控制算法的输入有车辆当前后轴横坐标x，纵坐标y以及横摆角φ。车辆期望的路径通过离散点的形式存入Basic/Model workspace并通过Constant模块给出，包括路径的横纵坐标，每个坐标的航向角以及每个坐标的曲率。车辆期望速度通过Constant模块给出。
模型输出控制车速以及前轮转角(rad)。该车速是期望车速，按理说应该通过纵向控制输出加速度，后反算成转矩给车辆发动机或者电机。这里省略纵向控制直接将车速给到车辆模型。
下面直接给出M函数的代码。如果对代码有疑问的同学，可以私聊我进行讨论。


function [Spd,Str,Stop_Flag]  = fcn(x,y,phi,xr,yr,thetar,kappar,GoalSpd)
%****初始化参数矩阵************%
global index;
T = 0.01;%程序离散控制周期，配合原子子系统0.01ms周期
L = 1.6;%车辆轴距
PN_1 = zeros(3,3); %该矩阵用于黎卡提方程迭代
n=length(xr);      %路径点个数
Stop_Flag = 0;     %仿真结束标志位
I = eye(3);        %初始化3*3单位矩阵
K =zeros(2,3);     %初始化反馈增益矩阵维度
%coder.extrinsic('dlqr'); %若用自带库函数，请用外部声明
min=1;
%*******寻找车辆后轴与当前车辆最近点，仅查找距上一次车辆位置最近点前30个点if index == 0  %%未开始d_min=(x-xr(1))^2+(y-yr(1))^2;min=1;for i=1:30 %% search 1-30 points when car is running.d=(x-xr(i))^2+(y-yr(i))^2;if d<d_mind_min=d;min=i;endenddmin=min;index = dmin; %% 当前点索引分配给index;elsed_min = (x-xr(index))^2+(y-yr(index))^2;min = index;if index <= n - 30for i = index:index+30d=(x-xr(i))^2+(y-yr(i))^2;if d<d_mind_min=d;min=i;endenddmin=min;index = dmin;else for i = index:nd=(x-xr(i))^2+(y-yr(i))^2;if d<d_mind_min=d;min=i;if min == nStop_Flag =1;%路径点跑完了仿真停止endendenddmin=min;index = dmin; endend
%*****状态变量计算******%
kesix = x -xr(dmin);
kesiy = y -yr(dmin);
kesir =phi - thetar(dmin);%******防止偏差数据溢出***********%if kesir < -3.14kesir = kesir+6.28;endif kesir >3.14kesir = kesir -6.28;endxstate = [kesix;kesiy;kesir];%状态变量
vd1 = GoalSpd; %期望速度
t_d = thetar(dmin);%当前和车辆最近点的航向角
k = kappar(dmin);%当前和车辆最近点的曲率
vd2=atan(L*k);%期望前轮转角Ad=[1    0   -vd1*sin(t_d)*T;0    1   vd1*cos(t_d)*T;0    0    1;];  %线性离散后的系统矩阵Bd=[cos(t_d)*T     0;sin(t_d)*T     0;tan(vd2)*T/L   vd1*T/(L*(cos(vd2)^2))]; %线性离散后的控制矩阵Q = [10 0 0;0 10 0; 0 0 10];%初始化LQR的权重矩阵R =[5 0;0 5];%初始化LQR的权重矩阵%*********黎卡提方程求解**********%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>40000 %防止迭代次数过长，该值根据控制器实际性能设置break;endendP =PN_1;K = (R+Bd'*P*Bd)\Bd'*P*Ad;%计算状态反馈增益K%***************************************%%K =dlqr(Ad,Bd,Q,R); %如果使用官方求解库，配合外部函数引用才有效，否则报错。U  = - K * xstate; %线性反馈控制量deltav = U(1);deltastr =U(2);Spd = GoalSpd+deltav; %m/sStr = vd2+deltastr; %rad
end

若您在模型中已经做了规划层的算法，可以直接将唯一的规划点传入控制算法，就省去了搜索路径点最近点的一系列程序。程序反而更加简洁。上述程序直接将所有路径点以行向量的形式传入了函数中。
注意index是全局变量，需要在Edit Data中声明该变量是data store memory。然后在Simulink中添加data store memory模块，并填写数据类型。
接下来可以进行仿真，如果没有可视化界面的同学或者不会用的同学就将xy输出到matlab界面画图对比期望路径。

从动画界面可以看出车辆完全可以跟踪期望路径，这里感谢一下北理工车队提供FSAC2020赛季的高速循迹赛道数据库。该算法的全部过程如上，希望各位有兴趣的朋友可以动手搭一下，最核心的控制算法已经全部给出，可以联合Carsim进行仿真。也可以直接进行Code Gen部署到ROS或者直接刷写进Speedgoat或者AutoBox，前提是不能使用自带dlqr函数，因自带lqr函数无法代码生成。这里简单生成下C代码：

总结

动力学模型的LQR就不介绍了，网上有很多，并且后续会进行运动学和动力学模型的实车试验进行对比，结合控制器性能和算法稳定性综合评价选出最合适的算法。如果您对本次算法有任何不解，欢迎私信讨论。感兴趣的同学建议您自己动手实践一下，最核心的代码已经完全给出，实现起来还是非常简单的，有问题，请直接打扰。

更新

如果仅想获取模型的朋友，可以通过下面链接下载，原创不易，谢谢支持。Matlab版本请更新到2019b及以上，并安装控制系统工具箱以及车辆动力学工具箱。
Matlab/Simulink基于运动学的LQR轨迹跟踪控制算法

路径跟踪—基于车辆运动学方程的离散时间LQR控制仿真相关推荐

【飞行器】基于matlab四旋翼飞行器PID控制仿真【含Matlab源码 1277期】
一.获取代码方式获取代码方式1: 完整代码已上传我的资源:[飞行器]基于matlab四旋翼飞行器PID控制仿真[含Matlab源码 1277期] 点击上面蓝色字体,直接付费下载,即可. 获取代码方式 ...
基于 stm32f103 芯片的直流电机驱动控制仿真系统
西电2022年A测(线上)题目文章目录前言一.Keil5的安装 1.获取安装包 2.安装Keil5 3.运行注册机 4.安装器件库二,运行工程文件,获取HEX可执行文件 1,获取工程文件 2. ...
【控制】《多无人机协同控制技术》周伟老师-第12章-基于 Multi-Agent 的多无人机协同控制仿真平台的设计与实现
第11章回到目录无第12章-基于 Multi-Agent 的多无人机协同控制仿真平台的设计与实现 12.1 多无人机协同控制视景仿真系统设计 12.1.1 多无人机协同控制视景仿真系统功能描述 ...
基于单片机的饮水机温度控制系统、基于单片机继电器及双向可控硅控制照明设备、基于单片机的液位自动检测控制仿真设计、基于单片机家用厨房天然气火灾报警器系统-设计全套资料
1347基于单片机的饮水机温度控制系统-电设资料随着社会的发展和科技的不断进步,人们的生活水平也在逐渐提高.与之而来的问题也很多,例如能源危机与环境污染,绿色环保理念因而开始深入人心.太阳能作为新兴 ...
西电A测|基于Arduino uno的温度检测控制仿真系统
西电A测|基于Arduino uno的温度检测控制仿真系统温度检测控制仿真系统 Proteus仿真电路图 Arduino IDE编写程序结果验证功能拓展温度检测控制仿真系统功能要求:Ardu ...
matlab仿真直流电机,[转载]基于Matlab/Simulink的无刷直流电机控制仿真研究
摘要: 基于Matlab/Simulink,本文设计了一个无刷直流电机的控制方案,详细阐述了无刷直流电机的运行原理,并用Matlab/Simulink对其进行了仿真.实验证明,用Matlab/Simu ...
基于simulink的无人机姿态飞行控制仿真
目录 1.算法描述 2.仿真效果预览 3.MATLAB核心程序 4.完整MATLAB 1.算法描述无人机是无人驾驶飞机的简称(Unmanned Aerial Vehicle),是利用无线电遥控设备和 ...
斯坦福大学：极限工况下的无人驾驶路径跟踪｜厚势汽车
来源: 同济智能汽车研究所责任编辑:啜小雪文章译自 2017 年美国控制年会的会议论文原标题:Path-Tracking for Autonomous Vehicles at the Limit ...
电气simulink常用模块_基于Simulink的永磁电机（PMSM）控制仿真
基于Simulink的永磁电机(PMSM)控制仿真引言与传统的电励磁同步电机相比,永磁同步电机具有结构简单.运行可靠.体积小.质量轻.损耗小.尺寸形状灵活多样等特点. 控制目前常用且简单的控制方 ...

路径跟踪—基于车辆运动学方程的离散时间LQR控制仿真

文章目录

前言

一、运动学模型

二、建模仿真

总结

更新

路径跟踪—基于车辆运动学方程的离散时间LQR控制仿真相关推荐

最新文章

热门文章