代码详解——《无人驾驶车辆模型预测控制》3.3.3代码详解
时间:2022-07-24
本文章向大家介绍代码详解——《无人驾驶车辆模型预测控制》3.3.3代码详解,主要内容包括其使用实例、应用技巧、基本知识点总结和需要注意事项,具有一定的参考价值,需要的朋友可以参考一下。
《无人驾驶车辆模型预测控制》3.3.3代码详解
代码下载链接:http://www.bitpress.com.cn/book/book_detail.php?id=9055
北京科技大学 白国星注释 david.gx.bai@gmail.com
clc; %清空命令空间,目的是好看
clear all; %清空工作空间,避免同名变量影响程序初始值
%% 参考轨迹生成
N=100; %生成100个点的参考路径
T=0.05; %时间间隔
% Xout=zeros(2*N,3);
% Tout=zeros(2*N,1);
Xout=zeros(N,3); %预留数组空间
Tout=zeros(N,1);
for k=1:1:N
Xout(k,1)=k*T; %横坐标
Xout(k,2)=2; %纵坐标
Xout(k,3)=0; %航向角
Tout(k,1)=(k-1)*T; %参考轨迹时间
end
%% Tracking a constant reference trajectory
Nx=3; %状态变量
Nu =2; %输入变量
Tsim =20; %仿真时间
X0 = [0 0 pi/3]; %车辆初始位置姿态
[Nr,Nc] = size(Xout); % Nr is the number ofrows of Xout
% Mobile Robot Parameters
c = [1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
L = 1;
Rr = 1;
w = 1;
% Mobile Robot variable Model
vd1 = Rr*w; % For circular trajectory %车速设置为1,参考路径中Xout(k,1)=k*T,其实省略了速度,应该是Xout(k,1)=k*T*v,vd1需要与v相等,如果不等,轨迹跟踪可能出现错误
vd2 = 0;
x_real=zeros(Nr,Nc);
x_piao=zeros(Nr,Nc);
u_real=zeros(Nr,2);
u_piao=zeros(Nr,2);
x_real(1,:)=X0;
x_piao(1,:)=x_real(1,:)-Xout(1,:); %此处与书上公式存在一些差异,式3.14、3.15中,误差为一系列预测位姿与参考路径点之间的误差,但代码中实质上是当前位姿与当前跟踪目标点之间的误差,所以公式3.15应该修订为e=φ*(x(t)-xref(t))
X_PIAO=zeros(Nr,Nx*Tsim);
XXX=zeros(Nr,Nx*Tsim);%用于保持每个时刻预测的所有状态值 %用于绘图
q=[1 0 0;0 1 0;0 0 0.5];
Q_cell=cell(Tsim,Tsim);
for i=1:1:Tsim
for j=1:1:Tsim
if i==j
Q_cell{i,j}=q;
else
Q_cell{i,j}=zeros(Nx,Nx);
end
end
end
Q=cell2mat(Q_cell);
R=0.1*eye(Nu*Tsim,Nu*Tsim); %权重系数
for i=1:1:Nr
t_d =Xout(i,3);
a=[1 0 -vd1*sin(t_d)*T;
0 1 vd1*cos(t_d)*T;
0 0 1;];
b=[cos(t_d)*T 0;
sin(t_d)*T 0;
0 T;];
A_cell=cell(Tsim,1);
B_cell=cell(Tsim,Tsim);
for j=1:1:Tsim
A_cell{j,1}=a^j;
for k=1:1:Tsim
if k<=j
B_cell{j,k}=(a^(j-k))*b;
else
B_cell{j,k}=zeros(Nx,Nu);
end
end
end
A=cell2mat(A_cell);
B=cell2mat(B_cell);
H=2*(B'*Q*B+R);
f=2*B'*Q*A*x_piao(i,:)'; %式3.16中的H和G
A_cons=[];
b_cons=[];
lb=[-1;-1];
ub=[1;1]; %约束条件
tic
[X,fval(i,1),exitflag(i,1),output(i,1)]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub); %求解
toc
X_PIAO(i,:)=(A*x_piao(i,:)'+B*X)'; %绘图用变量
if i+j<Nr
for j=1:1:Tsim
XXX(i,1+3*(j-1))=X_PIAO(i,1+3*(j-1))+Xout(i+j,1);
XXX(i,2+3*(j-1))=X_PIAO(i,2+3*(j-1))+Xout(i+j,2);
XXX(i,3+3*(j-1))=X_PIAO(i,3+3*(j-1))+Xout(i+j,3);
end
else
for j=1:1:Tsim
XXX(i,1+3*(j-1))=X_PIAO(i,1+3*(j-1))+Xout(Nr,1);
XXX(i,2+3*(j-1))=X_PIAO(i,2+3*(j-1))+Xout(Nr,2);
XXX(i,3+3*(j-1))=X_PIAO(i,3+3*(j-1))+Xout(Nr,3);
end
end %绘图用变量结束
u_piao(i,1)=X(1,1); %求解结果
u_piao(i,2)=X(2,1); %求解结果
Tvec=[0:0.05:4];
X00=x_real(i,:);
vd11=vd1+u_piao(i,1); %产生的控制输入1
vd22=vd2+u_piao(i,2); %产生的控制输入2,控制器运行结束
XOUT=dsolve('Dx-vd11*cos(z)=0','Dy-vd11*sin(z)=0','Dz-vd22=0','x(0)=X00(1)','y(0)=X00(2)','z(0)=X00(3)'); %被控车辆模型启动
t=T;
x_real(i+1,1)=eval(XOUT.x);
x_real(i+1,2)=eval(XOUT.y);
x_real(i+1,3)=eval(XOUT.z);
if(i<Nr)
x_piao(i+1,:)=x_real(i+1,:)-Xout(i+1,:);
end
u_real(i,1)=vd1+u_piao(i,1);
u_real(i,2)=vd2+u_piao(i,2); %迭代车辆下一时刻的位置,被控车辆模型结束
figure(1); %绘轨迹图
plot(Xout(1:Nr,1),Xout(1:Nr,2));
hold on;
plot(x_real(i,1),x_real(i,2),'r*');
title('跟踪结果对比');
xlabel('横向位置X');
axis([-1 5 -1 3]);
ylabel('纵向位置Y');
hold on;
for k=1:1:Tsim
X(i,k+1)=XXX(i,1+3*(k-1));
Y(i,k+1)=XXX(i,2+3*(k-1));
end
X(i,1)=x_real(i,1);
Y(i,1)=x_real(i,2);
plot(X(i,:),Y(i,:),'y')
hold on; %绘轨迹图结束
end %仿真循环结束
%% 以下为绘图部分
figure(2)
subplot(3,1,1);
plot(Tout(1:Nr),Xout(1:Nr,1),'k--');
hold on;
plot(Tout(1:Nr),x_real(1:Nr,1),'k');
%grid on;
%title('状态量-横向坐标X对比');
xlabel('采样时间T');
ylabel('横向位置X')
subplot(3,1,2);
plot(Tout(1:Nr),Xout(1:Nr,2),'k--');
hold on;
plot(Tout(1:Nr),x_real(1:Nr,2),'k');
%grid on;
%title('状态量-横向坐标Y对比');
xlabel('采样时间T');
ylabel('纵向位置Y')
subplot(3,1,3);
plot(Tout(1:Nr),Xout(1:Nr,3),'k--');
hold on;
plot(Tout(1:Nr),x_real(1:Nr,3),'k');
%grid on;
hold on;
%title('状态量-theta对比');
xlabel('采样时间T');
ylabel('theta')
figure(3)
subplot(2,1,1);
plot(Tout(1:Nr),u_real(1:Nr,1),'k');
%grid on;
%title('控制量-纵向速度v对比');
xlabel('采样时间T');
ylabel('纵向速度')
subplot(2,1,2)
plot(Tout(1:Nr),u_real(1:Nr,2),'k');
%grid on;
%title('控制量-角加速度对比');
xlabel('采样时间T');
ylabel('角加速度')
figure(4)
subplot(3,1,1);
plot(Tout(1:Nr),x_piao(1:Nr,1),'k');
%grid on;
xlabel('采样时间T');
ylabel('e(x)');
subplot(3,1,2);
plot(Tout(1:Nr),x_piao(1:Nr,2),'k');
%grid on;
xlabel('采样时间T');
ylabel('e(y)');
subplot(3,1,3);
plot(Tout(1:Nr),x_piao(1:Nr,3),'k');
%grid on;
xlabel('采样时间T');
ylabel('e(theta)');
- Hbase 学习(十一)使用hive往hbase当中导入数据
- WF追忆
- OpenCV和SVM分类器在自动驾驶中的车辆检测
- Hive Tunning(三) 最佳实践
- ambari删除脚本
- sqoop 常用命令整理(一)
- hbase 学习(十三)集群间备份原理
- hbase 学习(十二)非mapreduce生成Hfile,然后导入hbase当中
- RavenDb学习(六)查询补充特性
- hbase源码系列(一)Balancer 负载均衡
- OpenCV在车道线查找中的使用
- hbase源码系列(十五)终结篇&Scan续集-->如何查询出来下一个KeyValue
- mac系统下安装、启动、停止mongodb
- 100个Numpy练习【1】
- JavaScript 教程
- JavaScript 编辑工具
- JavaScript 与HTML
- JavaScript 与Java
- JavaScript 数据结构
- JavaScript 基本数据类型
- JavaScript 特殊数据类型
- JavaScript 运算符
- JavaScript typeof 运算符
- JavaScript 表达式
- JavaScript 类型转换
- JavaScript 基本语法
- JavaScript 注释
- Javascript 基本处理流程
- Javascript 选择结构
- Javascript if 语句
- Javascript if 语句的嵌套
- Javascript switch 语句
- Javascript 循环结构
- Javascript 循环结构实例
- Javascript 跳转语句
- Javascript 控制语句总结
- Javascript 函数介绍
- Javascript 函数的定义
- Javascript 函数调用
- Javascript 几种特殊的函数
- JavaScript 内置函数简介
- Javascript eval() 函数
- Javascript isFinite() 函数
- Javascript isNaN() 函数
- parseInt() 与 parseFloat()
- escape() 与 unescape()
- Javascript 字符串介绍
- Javascript length属性
- javascript 字符串函数
- Javascript 日期对象简介
- Javascript 日期对象用途
- Date 对象属性和方法
- Javascript 数组是什么
- Javascript 创建数组
- Javascript 数组赋值与取值
- Javascript 数组属性和方法
- 利用织梦CMS0day注入漏洞渗透测试
- nodejs中追加内容到文件
- android实现滚动文本效果
- Android实现View的拖拽
- 诊断日志知多少 | DiagnosticSource 在.NET上的应用
- Android Studio 4.0新特性及升级异常问题的解决方案
- Android Studio 4.0 正式发布在Ubuntu 20.04中安装的方法
- Android Studio 4.0 新功能中的Live Layout Inspector详解
- Android实现滑动刻度尺效果
- Android 仿微信发动态九宫格拖拽、删除功能
- android自定义等级评分圆形进度条
- Android Fragment实现底部通知栏
- Flutter实现局部刷新
- Android自定义条形对比统计图
- Android底部菜单栏(RadioGroup+Fragment)美化