章四基于运动学模型的轨迹跟踪控制

MPC（4）基于运动学模型的轨迹跟踪控制器设计

无人驾驶车辆模型预测控制（龚建伟）第四章基于运动学模型的轨迹跟踪控制（仿真部分）

无人车辆在惯性坐标系中，车辆必须从一个给定的初始状态出发，这个初始点可以在期望轨迹上，也可以不在期望轨迹上

期望轨迹和参考控制输入可以由轨迹规划模块提供，也可以预先设定，本章中为预先设定

假设系统可提供两类有效信息：

可行驶区域的几何描述、路面特征及路面摩擦系数
车辆位置及内部状态，包括横纵向速度、加速度、轮速等参数

即轨迹跟踪控制是在周围环境及车辆内部状态完全已知的情况下进行的，不涉及环境感知和车辆状态的估计

轨迹跟踪控制器设计

车辆运动学建模

k+1时刻的状态可由k时刻状态及k时刻输入计算得到

将上式线性化可以得到新的状态空间表达式，推导可以得到系统的预测输出表达式

目标函数设计

在传统目标函数式中加入权重系数和松弛因子，并用控制增量代替控制量

不仅能对控制增量进行直接限制，同时可以防止执行过程中出现无可行解的情况

约束条件设计

主要考虑控制过程中控制量极限约束和控制增量约束

约束条件中涉及的具体数值需要通过纵横向跟踪能力测试获得

参考路径的自适应分段拟合

路网文件提供的参考路径通常是一系列不包含曲率信息的离散路点序列

需要对这些离散路点进行拟合，得到期望轨迹，主要有贝塞尔及B样条两种拟合方法

CarSim与Simulink联合仿真

Simulink模型

CarSim配置

S-Function

% 主函数
function [sys,x0,str,ts] = YDModelTrajectoryTracking(t,x,u,flag)switch flag,case 0 [sys,x0,str,ts] = mdlInitializeSizes; case 2 sys = mdlUpdates(t,x,u); case 3 sys = mdlOutputs(t,x,u); case {1,4,9} sys = [];            otherwise error(['unhandled flag = ',num2str(flag)]); end
% 初始化子函数
function [sys,x0,str,ts] = mdlInitializeSizessizes = simsizes;sizes.NumContStates  = 0;sizes.NumDiscStates  = 3;  sizes.NumOutputs     = 2;sizes.NumInputs      = 3;sizes.DirFeedthrough = 1;sizes.NumSampleTimes = 1;sys = simsizes(sizes);x0  = [0;0;0];str = [];ts  = [0.05 0];global U; U=[0;0];
% 状态更新子函数
function sys = mdlUpdates(t,x,u)sys = x;
% 输出子函数
function sys = mdlOutputs(t,x,u)% 半径为25m的圆形轨迹, 圆心为(0，35), 速度为5m/s% 轴距L=2.6,根据阿克曼转向车辆运动学模型求取参考值r(1) = 25 * sin( 0.2 * t );           % 纵向参考坐标r(2) = 35 - 25 * cos( 0.2 * t );      % 横向参考坐标r(3) = 0.2 * t;                       % 参考航向角vd1  = 5;                             % 纵向参考车速vd2  = 0.104;                         % 参考前轮偏角% 参数global a b u_piao;     global U;              global kesi;Nx  = 3;                % 状态量的个数Nu  = 2;                % 控制量的个数Np  = 80;               % 预测步长Nc  = 30;               % 控制步长T   = 0.05;             % 采样时间L   = 2.60;             % 车辆的轴距u_piao = zeros(Nu, 1);% 输入向量单位转化yaw_angle =u(3)*pi/180; % CarSim输出的角度转换为弧度%% 模型预测控制器设计% 1.误差模型a = [1    0   -vd1*sin(r(3))*T;0    1    vd1*cos(r(3))*T;0    0                  1;];b = [cos(r(3))*T                           0;sin(r(3))*T                           0;tan(vd2)*T/L       vd1*T/((cos(vd2))^2);];% 为了转化为标准二次规划问题，进行矩阵转化% 状态和控制合并为一个新的向量kesi = zeros(Nx+Nu,1);    kesi(1) = u(1) - r(1);               kesi(2) = u(2) - r(2);               heading_offset = yaw_angle - r(3);   if (heading_offset < -pi)heading_offset = heading_offset + 2*pi;endif (heading_offset > pi)heading_offset = heading_offset - 2*pi;endkesi(3) = heading_offset;kesi(4) = U(1); kesi(5) = U(2); A_cell=cell(2,2);B_cell=cell(2,1);A_cell{1,1}=a;A_cell{1,2}=b;A_cell{2,1}=zeros(Nu,Nx);A_cell{2,2}=eye(Nu);B_cell{1,1}=b;B_cell{2,1}=eye(Nu);A=cell2mat(A_cell);B=cell2mat(B_cell);C=[ 1 0 0 0 0;0 1 0 0 0;0 0 1 0 0;];% 2.预测PHI_cell=cell(Np,1);THETA_cell=cell(Np,Nc);for j=1:1:NpPHI_cell{j,1}=C*A^j;for k=1:1:Ncif k<=jTHETA_cell{j,k}=C*A^(j-k)*B;else THETA_cell{j,k}=zeros(Nx,Nu);endendendPHI=cell2mat(PHI_cell);THETA=cell2mat(THETA_cell);% 3.目标函数Row = 10;              %松弛因子Q = eye(Nx * Np, Nx * Np);    R = 5*eye(Nu * Nc);% 二次目标项H_cell=cell(2,2);H_cell{1,1}=THETA'*Q*THETA+R;H_cell{1,2}=zeros(Nu*Nc,1);H_cell{2,1}=zeros(1,Nu*Nc);H_cell{2,2}=Row;H=cell2mat(H_cell);% 线性目标项error=PHI*kesi;f_cell=cell(1,2);f_cell{1,1} = (error'*Q*THETA);f_cell{1,2} = 0;f=cell2mat(f_cell);% 4.约束A_t=zeros(Nc,Nc);for p=1:1:Ncfor q=1:1:Ncif q<=p A_t(p,q)=1;else A_t(p,q)=0;endend end A_I=kron(A_t,eye(Nu));Ut=kron(ones(Nc,1), U);% 控制量约束umin=[-0.2;  -0.436];umax=[ 0.2;   0.436]; Umin=kron(ones(Nc,1),umin);Umax=kron(ones(Nc,1),umax);A_cons_cell={A_I zeros(Nu*Nc, 1); -A_I zeros(Nu*Nc, 1)};b_cons_cell={Umax-Ut;-Umin+Ut};A_cons=cell2mat(A_cons_cell);b_cons=cell2mat(b_cons_cell);% 控制增量及松弛因子约束delta_umin = [-0.05;  -0.0082]; delta_umax = [ 0.05;   0.0082];delta_Umin = kron(ones(Nc,1),delta_umin);delta_Umax = kron(ones(Nc,1),delta_umax);lb = [delta_Umin;  0];ub = [delta_Umax; 10];% 5.优化求解options = optimset('Algorithm','interior-point-convex'); warning off all    [X, fval,exitflag]=quadprog(H, f, A_cons, b_cons,[], [],lb,ub,[],options);% 6.输出   u_piao(1)=X(1);u_piao(2)=X(2);U(1)=kesi(4)+u_piao(1);U(2)=kesi(5)+u_piao(2);u_real(1) = U(1) + vd1;u_real(2) = U(2) + vd2;sys= [u_real(1); u_real(2)];

仿真结果如下，其中红色为参考轨迹，蓝色为实际轨迹

实际轨迹与期望轨迹跟踪误差较小，跟踪效果良好，但因只考虑运动学，故仅适用于低速场景

基于运动学模型的轨迹跟踪控制相关推荐

基于MPC的移动机器人轨迹跟踪控制qpOASES例程
参考了一个模型预测控制(MPC)的简单实现 https://www.cnblogs.com/zhjblogs/p/13880682.html 与基于MPC的移动机器人轨迹跟踪控制matlab例程 ...
ROS功能包|mav_control_rw（基于MPC的无人机轨迹跟踪控制）---gazebo仿真测试
ROS功能包|mav_control_rw(基于MPC的无人机轨迹跟踪控制)---gazebo仿真测试 gazebo仿真测试 gazebo仿真测试启动gazebo并加载无人机模型 $ roslaun ...
使用webots的MPC的移动机器人轨迹跟踪控制
上一篇文章中使用MPC对机器人的一个方向自由度进行了控制, 基于MPC的移动机器人轨迹跟踪控制qpOASES例程现在使用速度与角速度对机器人进行平面运动控制. 所以机器人的控制量为U=[v ;w], ...
基于CarSim和Simulink的车辆预瞄轨迹跟踪控制模型+carsim设置文件
基于CarSim和Simulink的车辆预瞄轨迹跟踪控制模型+carsim设置文件可选模型说明文件和操作说明 ID:2180642445175030
【Carsim Simulink自动驾驶仿真】基于MPC的轨迹跟踪控制
如果对Carsim的基础使用还不了解,可以参考:[Carsim Simulink自动驾驶仿真]基于MPC的速度控制如果对MPC算法原理不清楚,可以参考:如何理解MPC模型预测控制理论项目介绍: 教 ...
基于模糊补偿的液压机械臂轨迹跟踪控制
摘要: 针对具有摩擦和负载等不确定性的液压机械臂轨迹跟踪控制问题,建立机械臂机械系统.液压系统的数学模型,基于李亚普洛夫函数,提出一种模糊补偿策略进行控制,构造模糊系统逼近摩擦力和负载,应用流量补偿器 ...
无人驾驶车辆路径规划及轨迹跟踪控制学习笔记（2）
目录汇总学习笔记汇总在关键交通场景中,轨迹规划和轨迹跟踪控制是自动驾驶车辆避免碰撞的两个关键.它不仅需要系统功能,而且需要强大的实时性. 我们集成了自动驾驶汽车的轨迹规划器和跟踪控制器,通过轨 ...
无人驾驶车辆规划+轨迹跟踪控制学习笔记（1）
综述自动驾驶汽车有一个很有前途的未来,它可以使运输时间变得轻松,并使驾驶员能够参与其他活动,从而改变世界各地的日常生活.它们有可能大大减少由驾驶员的过失造成的撞车事故,包括驾驶积极.补偿过度.经验不 ...
无人驾驶汽车系统入门（十）——基于运动学模型的模型预测控制
无人驾驶汽车系统入门(十)--基于运动学模型的模型预测控制在前面的第五篇博客中,我们介绍了两种常见的车辆模型--运动学自行车模型和动力学自行车模型,本节我们基于运动学车辆模型引入一种新的控制理论-- ...

基于运动学模型的轨迹跟踪控制

章四基于运动学模型的轨迹跟踪控制