【路径规划】基于matlab卡尔曼滤波、三次插值极速赛道赛车路径规划【含Matlab源码 2158期】
一、卡尔曼滤波路径追踪优化简介
割草机器人通过比对当前t时刻位置、导航方程之间偏移角度θ和偏移距离d,确定t+1时刻的运动方向属于递推型路径追踪。割草机器人工作过程中受到地面起伏等环境因素影响,在采用上述追踪方法时会和预测值产生偏差,造成机器人偏离导航方程,称之为系统预测误差。采用图像处理规划导航路径时,同样也会产生偏差,称为检测误差。由于地形环境因素和割草边界线图像检测误差均为偶然性误差,符合高斯白噪声,因此采用卡尔曼滤波的方法对割草机器人路径追踪进行优化。其核心思想为:比对预测值和检测值协方差,进而采用递推的方式计算出下一时刻系统路径最优解。
由于机器人在运动过程中受地面起伏影响,因此噪声矩阵Wt均值为0,协方差为Q。
机器人检测只关心当前位置,进而和导航方程进行比对,因此系统检测模型为
结合式(11)、式(12)构建卡尔曼滤波系统预测方程为
预测值与检测值协方差为
以当前协方差为判定依据,综合考虑系统预测值和图像检测值,下一时刻最优化值为
其中,Kt+1为卡尔曼滤波增益。
协方差下一时刻更新,则
至此完成t+1时刻路径追踪优化,转向t+2时刻。
二、部分源代码
clear;
clc;
load ‘track_352.mat’;%原始赛道信息
load ‘body.mat’;%车辆信息
load ‘dynamic_constrains.mat’;%动力学约束
safe_dist = 2;%距离障碍物安全距离
%% 平滑插值赛道
tolerance = 10;%先线性插值,去掉大间隙
[track_x,track_y] = track_smooth_linear(track_x,track_y,tolerance);
tolerance = 3;%然后三次样条线插值,达到最终步长
[track_x,track_y] = track_smooth_spline(track_x,track_y,tolerance);
clear tolerance;
%% 求得与赛道长度对应的样条线参数
[c,~] = size(track_x);
[spline_A] = spline_matrix_gen©;%样条线求解矩阵,要留在内存中,加速计算
para_x = spline_A * track_x;%x三次样条参数
para_y = spline_A * track_y;%y三次样条参数
%% 单位切向量
[vector] = vector_gen(para_x,para_y,c);
%% 绘制中心线、左右边界及障碍物
draw_track(track_x,track_y,vector,track_w,obs_x,obs_y,c);
%% QP优化初始化
alpha_out = zeros(c,1);%本次次迭代结果
alpha_last = zeros(c,1);%上一次迭代结果
[lb,ub] = init_lub(track_w,body.Wb,c);%上下界初始化
[Aieq,bieq] = init_ieq(track_x,track_y,obs_x,obs_y,safe_dist,c);%有关障碍物的不等式约束初始化
stop_inter_thres = c * 0.01;%每一次迭代后所有点上横向改变平方和,每一个点上差别不超过1厘米
inter_val = 100000;%初始停止条件数值
path_cnt = 0;%迭代次数
%% QP优化
%不下降时停止迭代
tic;
while inter_val >= stop_inter_thres
[alpha_out,alpha_last,lb,ub,track_x,track_y,vector] = QP_optimization(spline_A,alpha_out,alpha_last,lb,ub,Aieq,bieq,track_x,track_y,vector,c);
inter_val = sum(alpha_out.^2);
path_cnt = path_cnt + 1;
end
[c_alpha,~] = size(alpha_out);
if c_alpha == 0
alpha_out = zeros(c,1);%以防无解
end
clear c_alpha;
path_t = toc;
clc;
clear stop_inter_thres inter_val lb ub Aieq bieq;
%% 生成最优路径及其信息
draw_optimised_path(track_x,track_y,vector,alpha_out,‘m’);%绘制最优路径
[track_x,track_y] = path_gen(track_x,track_y,vector,alpha_out);%保存最优路径
[path_distance] = path_distance_calculate(track_x,track_y,c);%沿最优路径距离
para_x = spline_A * track_x;
para_y = spline_A * track_y;
[vector] = vector_gen(para_x,para_y,c);
[phi] = phi_accum_gen(vector,c);%参考车辆航向角
[curvature_res] = get_curvature(para_x,para_y,c);%参考曲率
clear obs_x obs_y obs_w safe_dist alpha_out alpha_last;
%% 速度规划
[apex_location,apex_cnt] = get_apex(curvature_res,c);%标记Apex点
vmax = 40;%限制最高直线车速
[vel_geo] = geometry_vel_calculate(curvature_res,vmax,ay_para,c);%计算几何速度限制
[vel_forward] = forward_calculate(apex_location,apex_cnt,vel_geo,curvature_res,path_distance,acc_para,ay_para,c);%加速限制
[vel_backward] = backward_calculate(apex_location,apex_cnt,vel_geo,curvature_res,path_distance,brk_para,ay_para,c);%减速限制
vel_profile = min(vel_geo,min(vel_forward,vel_backward));%三种取最小值
[vel_flying] = init_vel_calculate(vel_profile,vel_profile(end),acc_para,ay_para,path_distance,curvature_res,c);%飞驰圈速度规划
delta_profile = atan(curvature_res * body.l / 1000);
clear apex_location apex_cnt vel_geo vel_forward vel_backward acc_para ay_para brk_para vmax vel_profile;
%% 绘制速度规划
figure(2);
clf;
subplot(2,1,1);
plot(path_distance,vel_flying,‘r’);%绘制速度-距离规划
xlabel(‘S\m’);
ylabel(‘v\m*s^-1’);
title(‘velocity profile and history’);
subplot(2,1,2);
plot(path_distance,delta_profile,‘r’);%绘制方向盘转角-距离规划
xlabel(‘S\m’);
ylabel(‘\delta \deg’);
title(‘steering profile and history’);
%% 仿真参数设置
dt = 0.1;%时间间隔
np = 20;%预测步长
nc = 10;%控制步长
nx = 3;%状态量数目
nu = 2;%控制量数目
%% 参考量设置
state_ref = [track_x,track_y,phi,curvature_res,vel_flying,path_distance];
%% 物理限制设置
u_max = [40;0.5];
u_min = [2;-0.5];
du_max = [0.2;0.2];
du_min = [-0.2;-0.2];
%% 车辆参数及状态设置
l = body.l / 1000;
target_v = vel_flying(1);%期望速度
delta = 0;%当前转向角
travel = 0;%当前里程
control_d = [0;0];%上一时刻控制偏差
control_act = [target_v;delta];%当前实际控制值
control = [control_act];%储存实际控制指令
x_d = [0;0;0];%当前状态偏差
x_act = [track_x(1);track_y(1);phi(1)];
x_res = [x_act];%储存实际状态
travel_history = [travel];%里程表历史
%% 权重矩阵及观测矩阵生成
[Qt,Rt,Ct,rou] = weight_matrix_gen(nx,nu,np,nc);
%%
index = 0;
tic;
mpc_cnt = 0;
while index < c - 1
% 矩阵生成↓↓↓
[At,Bt] = sequential_increment_matrix_gen(x_act,control_act,Ct,np,nc,body,dt);
% 求当前点偏差↓↓↓
[x_d,index] = find_state_ref_err(para_x,para_y,state_ref,x_act,travel,c);
target_v = vel_flying(index);
% 求当前约束↓↓↓
[A_eqst,b_eqst,A_ieqst,b_ieqst,lb,ub] = get_constrains(u_max,u_min,du_max,du_min,control_act,nc,nu);
% 求最优控制量偏差↓↓↓
yita = [x_d;control_d];
H = [Bt’ * Qt * Bt + Rt,zeros(size(Bt,2),1);zeros(1,size(Bt,2)),rou];
H = H + H’;%quadprog程序是求1/2H,故将其变为二倍
F = [2 * yita’ * At’ * Qt * Bt,0]';%最后一个对应松弛变量
U_out = quadprog(H,F,A_ieqst,b_ieqst,A_eqst,b_eqst,lb,ub);
% 求最优控制量↓↓↓
delta_des = delta_profile(index);%该点处期望转向角
control_act = [target_v;delta_des] + control_d + [U_out(1);U_out(2)];%用得到的控制偏差和上一步的控制偏差修正
control_act(1) = min(control_act(1),target_v);
control = [control,control_act];%储存
control_d = [U_out(1);U_out(2)];%更新当前的控制偏差
% 更新并储存状态↓↓↓
[x_act,travel] = update_state(x_act,control_act,dt,l,travel);
x_res = [x_res,x_act];%储存
mpc_cnt = mpc_cnt + 1;
travel_history = [travel_history;travel];%储存里程历史
end
三、运行结果
四、matlab版本及参考文献
1 matlab版本
2014a
2 参考文献
[1]白刚.卡尔曼滤波在割草机器人路径追踪优化中应用[J].农机化研究. 2021,43(06)
3 备注
简介此部分摘自互联网,仅供参考,若侵权,联系删除
【路径规划】基于matlab卡尔曼滤波、三次插值极速赛道赛车路径规划【含Matlab源码 2158期】相关推荐
- 【Matlab生物电信号】生物电信号仿真【含GUI源码 684期】
一.代码运行视频(哔哩哔哩) [Matlab生物电信号]生物电信号仿真[含GUI源码 684期] 二.matlab版本及参考文献 1 matlab版本 2014a 2 参考文献 [1]董兵,超于毅,李 ...
- 【Matlab语音分析】语音信号分析【含GUI源码 1718期】
一.代码运行视频(哔哩哔哩) [Matlab语音分析]语音信号分析[含GUI源码 1718期] 二.matlab版本及参考文献 1 matlab版本 2014a 2 参考文献 [1]韩纪庆,张磊,郑铁 ...
- 【Matlab身份证识别】身份证号码识别【含GUI源码 014期】
一.代码运行视频(哔哩哔哩) [Matlab身份证识别]身份证号码识别[含GUI源码 014期] 二.matlab版本及参考文献 1 matlab版本 2014a 2 参考文献 [1] 蔡利梅.MAT ...
- 【Matlab车牌识别】停车计费系统【含GUI源码 735期】
一.代码运行视频(哔哩哔哩) [Matlab车牌识别]停车计费系统[含GUI源码 735期] 二.matlab版本及参考文献 1 matlab版本 2014a 2 参考文献 [1] 蔡利梅.MATLA ...
- 【Matlab水果识别】自助水果超市【含GUI源码 594期】
一.代码运行视频(哔哩哔哩) [Matlab水果识别]自助水果超市[含GUI源码 594期] 二.matlab版本及参考文献 1 matlab版本 2014a 2 参考文献 [1]倪云峰,叶健,樊娇娇 ...
- 【Matlab路径规划】改进的遗传算法机器人避障路径规划【含GUI源码 703期】
一.代码运行视频(哔哩哔哩) [Matlab路径规划]改进的遗传算法机器人避障路径规划[含GUI源码 703期] 二.matlab版本及参考文献 1 matlab版本 2014a 2 参考文献 [1] ...
- 【Matlab验证码识别】遗传算法和最大熵优化+大津法(OTSU)+自定义阈值数字验证码识别【含GUI源码 1694期】
一.代码运行视频(哔哩哔哩) [Matlab验证码识别]遗传算法和最大熵优化+大津法(OTSU)+自定义阈值数字验证码识别[含GUI源码 1694期] 二.matlab版本及参考文献 1 matlab ...
- 【Matlab人脸识别】BP神经网络人脸识别(含识别率)【含GUI源码 891期】
一.代码运行视频(哔哩哔哩) [Matlab人脸识别]BP神经网络人脸识别(含识别率)[含GUI源码 891期] 二.matlab版本及参考文献 1 matlab版本 2014a 2 参考文献 [1] ...
- 【Matlab人脸识别】形态学教室人数统计(带面板)【含GUI源码 1703期】
一.代码运行视频(哔哩哔哩) [Matlab人脸识别]形态学教室人数统计(带面板)[含GUI源码 1703期] 二.matlab版本及参考文献 1 matlab版本 2014a 2 参考文献 [1]孟 ...
最新文章
- IP BASE对OSPF的支持版本
- 腾讯云详解宕机故障:光纤挖断后的150秒
- 《认清C++语言》---谈谈const
- 开灯变形问题(枚举法)
- 听说当今程序员很厉害?不,那是你不了解上古时期的那些神级操作
- python网络爬虫实战——实时抓取西刺免费代理ip
- 卡巴斯基携手微软MSN 卡巴斯基2010激活码免费领
- 怎么查询电脑是否支持IPV6地址?
- Linux文字游戏制作软件,小精灵美化app
- 电脑计算机网络都打不开怎么办,电脑网页打不开怎么回事的常规处理方法
- CSS实例——梦幻西游
- python爬虫爬取《战狼Ⅱ》影评
- 转眼间十年过去了,还记得曾经红遍大江南北的这首歌吗?
- 用序列到序列和注意模型实现的翻译:Translation with a Sequence to Sequence Network and Attention
- EXECL日期相减计算工龄
- 【机器学习】之 主成分分析PCA
- MATLAB地图作为底图,matlab画图——地图背景
- Codeforces Round #818 (Div. 2)
- 读《谁的青春不迷茫》摘录
- 微信小程序毕业论文题目_SSM项目源码校园辩论管理平台+后台管理系统
热门文章
- UML建模详解(1)—初识UML建模
- 【蓝桥杯Web】第十三届蓝桥杯(Web 应用开发)第一次线上模拟赛
- 【威联通Nas】安装docker版本,包含百度云,迅雷x,火狐浏览器,图形化压缩软件,文本编辑器,qb下载器,中文桌面支持
- SAP schema增强
- 理解设计模式——工厂模式
- python a股行情_用Python,tushare做一个A股每日收盘行情监测分析(含源代码)
- 深圳捷迅YL-800MT PE13管脚千万不能悬空
- 2022年大数据产业规模已超1000亿,从ChatGPT的爆火看大数据行业发展
- 计算机软件优化,如何优化计算机软件系统
- 动手学强化学习第三章(马尔可夫决策过程)