路径规划基于matlab卡尔曼滤波三次插值极速赛道赛车路径规划含Matlab源码 2158期

Posted 2022-10-12 海神之光

一、卡尔曼滤波路径追踪优化简介

割草机器人通过比对当前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 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除