一、卡爾曼濾波路徑追蹤優化簡介
割草機器人通過比對目前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