章四 基于运动学模型的轨迹跟踪控制
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] = mdlInitializeSizes
sizes = 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;
end
if (heading_offset > pi)
heading_offset = heading_offset - 2*pi;
end
kesi(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:Np
PHI_cell{j,1}=C*A^j;
for k=1:1:Nc
if k<=j
THETA_cell{j,k}=C*A^(j-k)*B;
else
THETA_cell{j,k}=zeros(Nx,Nu);
end
end
end
PHI=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:Nc
for q=1:1:Nc
if q<=p
A_t(p,q)=1;
else
A_t(p,q)=0;
end
end
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)];
仿真结果如下,其中红色为参考轨迹,蓝色为实际轨迹
实际轨迹与期望轨迹跟踪误差较小,跟踪效果良好,但因只考虑运动学,故仅适用于低速场景
文章来源:https://www.toymoban.com/news/detail-445019.html
文章来源地址https://www.toymoban.com/news/detail-445019.html
到了这里,关于基于运动学模型的轨迹跟踪控制的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!