簡介

該節主要說明模型預測控制的原理，本節的MATLAB代碼是對優達學城MPC方法的復現

jeremy-shannon/CarND-MPC-Project?

github.com圖標ksakmann/CarND-MPC-Project?

github.com圖標

1、運動學模型

使用自行車模型作為運動學約束

2、優化問題建模

上圖為模型預測控制的一個示意圖，在當前位姿下，使用運動學模型對後續n個時刻的位姿進行估計，使用非線性優化器求解出cost最小的控制量v和delta

該模型假設速度恆定，即只進行橫向的控制，需要進行縱向控制時，請參考優達學城上的方法

目標函數

其中：

P,Q,R 為權重矩陣

cte為無人車的橫向誤差

phi_errror為無人車的姿態誤差

等式約束項

不等式約束項

3、單步的MATLAB程序

主程序

clc
clear all

NP = 10; %預測周期數
dt = 0.05; %預測周期

x_start = 0;
y_start = x_start + NP;
phi_start = y_start + NP;
cte_start = phi_start + NP;
ephi_start = cte_start + NP;
delta_start = ephi_start + NP;
% 狀態變數和控制變數的總個數
m = 6 * NP ;
% 向量var前10個表示10個時刻x_start，第11到20個表示10個時刻y_start，第21到30個表示10個時刻phi_start，後續以此類推
var = zeros(m,1);

% 損失函數
func = @(var)cost_function( var,NP,dt );
% 約束條件
nlcon = @(var)nonlconstrict( var,NP,dt );
% 各個約束的上下界
cl = zeros( 6*NP+1,1 );
cu = zeros( 6*NP+1,1 );
% 方向盤轉角的上下界（弧度）
for i = (5*NP+1):6*NP
cl(i) = -0.45;
cu(i) = 0.45;
end
% 求解的初始化
state_intial = zeros(m,1);
% 求解器的設置
opts = optiset(solver,IPOPT,display,iter,maxiter,50,maxtime,0.5);
% Create OPTI Object
Opt = opti(fun,func,nl,nlcon,cl,cu,x0,state_intial,options,opts);
% Solve the NLP problem
[A,fval,exitflag,info] = solve(Opt,state_intial)
% 向量A為所求的解

目標函數

function cost = cost_function(var,NP,dt)

 x_start = 0;
y_start = x_start + NP;
phi_start = y_start + NP;
cte_start = phi_start + NP;
ephi_start = cte_start + NP;
delta_start = ephi_start + NP;

cte = 0;
ephi = 0;
delta = 0;
for i = 1:1:NP
cte = cte + var( cte_start+i )^2;
ephi = ephi + var( ephi_start+i)^2;
end
for i = 1:1:NP-1
delta = delta + ( var( delta_start+i+1 ) - var( delta_start+i ) )^2;
end

cost = 1*cte + 1 * ephi + 100*delta;
end

約束函數

function c = nonlconstrict( var,NP,dt )

 x_start = 0;
y_start = x_start + NP;
phi_start = y_start + NP;
cte_start = phi_start + NP;
ephi_start = cte_start + NP;
delta_start = ephi_start + NP;

x_0 = 0;
y_0 = 0;
phi_0 = 0;
v = 5;
delta_0 = 0;
lf = 3;

c = zeros( 6*NP+1,1 );
for i = 1:1:NP
if i == 1
c( x_start + i ) = var(x_start + i ) - x_0 - v * cos( phi_0 ) * dt;
c( y_start + i ) = var(y_start + i ) - y_0 - v * sin( phi_0 ) * dt;
c( phi_start + i ) = var(phi_start + i ) - phi_0 - v /lf * tan( delta_0 ) * dt;
c( cte_start + i ) = var(cte_start + i ) - 3 + v * sin(phi_0) * dt;
c( ephi_start + i ) = var(ephi_start + i) - 0 + v/lf * delta_0*dt;
c( delta_start + i )= var(delta_start + i);
c( delta_start + i + NP)= var(delta_start + i) - delta_0 ;

else
c( x_start + i ) = var(x_start + i ) - var(x_start + i-1 ) - v * cos( var(phi_start + i-1) ) * dt;
c( y_start + i ) = var(y_start + i ) - var(y_start + i-1) - v * sin( var(phi_start + i-1) ) * dt;
c( phi_start + i ) = var(phi_start + i ) - var(phi_start + i-1) - v/lf * tan( var(delta_start + i-1) ) * dt;
c( cte_start + i ) = var(cte_start + i ) - 3 + v * sin( var(phi_start+i-1) ) * dt;
c( ephi_start + i ) = var(ephi_start + i) - 0 + v/lf * var(delta_start + i-1) * dt;
c( delta_start + i )= var(delta_start + i);
end
end
end

推薦閱讀：

相关文章