8基於模型預測控制的路徑規划

Introduction

• Model
  • System model(質點本身的運動)
  • Problem model(一般需要非線性方法來解決)

• Prediction
  • State space
  • Input space
  • Parameter space
• Control
  • The process of choosing the best policy

Model

模型預測控制通常的描述

final cost,tf關於系統最后的狀態的函數

running cost,t0-tf之間的cost,關於系統狀態的函數

 

約束分別為:
動力學約束(微分約束)
不等式約束(關於功率,加速度和速度等限制)
等式約束(從系統的一個狀態到另一個狀態的邊界約束)
障礙物約束(特殊,通常非凸的)

Parameter space

在預測軌跡有作用.

目的:找尋最佳的輸入u uu
需要將無限維度的輸入轉化到有限維度的輸入.
方法:

• 零階保持器(直接采樣離散化),如下圖有13段調整的輸入
• 多項式(將輸入轉化為多項式,調整a,b,c,d參數)
• B樣條
• 數值解方法
  • 有限Jerk軌跡
  • 基於神經網絡的方法

Optimization

• 搜索:
  • 圖搜索
  • 基於隨機抽樣的搜索
• 凸優化:
  • 二次規划,解決線性優化
• 非凸優化:
  • 序列二次規划
  • 粒子群優化解決非線性,不連續的優化

Control

優化的輸入u ^∗ ,只在0.1s內(假定)去求解,不斷重新解的u,這個過程被稱為RHC(Receding Horizon Control)

Tube based MPC

為了解決需要快速響應的系統問題,控制頻率要求高
多了Nominal system ,也就是標稱名義系統模型,不考慮外界干擾,能以較低的頻率解優化問題,較高的頻率控制.應用廣泛。

之所以叫Tube based MPC,是因為Associate controller可以使我們的real state 鎮定在reference trajectory附近,形成一個管道。

[論文](Tube-Based MPC: a Contraction Theory Approach)

Convenient sources

• Matlab MPC toolbox:
內置教學,可以作為代碼生成.
• μAO-MPC:
可以作為代碼生成.

• Acado toolkit:
可以作為代碼生成.解決非線性問題
• YANE:
大量的非線性算子
• Multi-Parametric Toolbox 3:
顯示MPC,不同於前面的使用數值優化,查表法,較高效

Linear Model Predictive Control

4s為prediction horizon
4s分成20段

變為矩陣描述

預測模型
解T_p , T_v , T_a參考如下代碼,大致思路就是便是將這幾個量(p,v,a)化為關於j的表達式

function [Tp, Tv, Ta, Bp, Bv, Ba] = getPredictionMatrix(K,dt,p_0,v_0,a_0) Ta=zeros(K); Tv=zeros(K); Tp=zeros(K); for i = 1:K Ta(i,1:i) = ones(1,i)*dt; end for i = 1:K% K為i for j = 1:i Tv(i,j) = (i-j+0.5)*dt^2;% end end for i = 1:K for j = 1:i Tp(i,j) = ((i-j+1)*(i-j)/2+1/6)*dt^3; end end Ba = ones(K,1)*a_0; Bv = ones(K,1)*v_0; Bp = ones(K,1)*p_0; for i=1:K Bv(i) = Bv(i) + i*dt*a_0; Bp(i) = Bp(i) + i*dt*v_0 + i^2/2*a_0*dt^2; end

問題模型:
假設
**目標1:**最終要到達零位置，零速度，零加速度。優化函數

 **目標2:**平滑軌跡。優化函數

 wn​為權重
整體優化目標:

 

 

 其中

通過上兩式的變化有

這是一個凸優化問題,使用MATLAB 的quadprog()求解二次規划問題

clear all; close all; clc; p_0 = 10; v_0 = 0; a_0 = 0; K=20; dt=0.2; log=[0 p_0 v_0 a_0]; w1 = 100; w2 = 1; w3 = 1; w4 = 1; for t=0.2:0.2:10
    %% Construct the prediction matrix [Tp, Tv, Ta, Bp, Bv, Ba] = getPredictionMatrix(K,dt,p_0,v_0,a_0); %% Construct the optimization problem H = w4*eye(K)+w1*(Tp'*Tp)+w2*(Tv'*Tv)+w3*(Ta'*Ta);
    F = w1*Bp'*Tp+w2*Bv'*Tv+w3*Ba'*Ta;
    
    %% Solve the optimization problem J = quadprog(H,F,[],[]); %% Apply the control j = J(1); p_0 = p_0 + v_0*dt + 0.5*a_0*dt^2 + 1/6*j*dt^3; v_0 = v_0 + a_0*dt + 0.5*j*dt^2; a_0 = a_0 + j*dt; %% Log the states log = [log; t p_0 v_0 a_0]; end %% Plot the result plot(log(:,1),log(:,2:4)); grid on; xlabel('t(s)'); legend('p','v','a');

其中:
K = 20, 表示20個時間段,每段0.2s，共計預測未來4s內輸出
log記錄系統軌跡
調節四個權重的值

J 的解析解,不去解QP問題,可能效率會更高些
J=-inv(H)*F';%J的解析解

hard constraints

由於許多求解器使用的都是小於形式,所以需要化為四組約束條件,寫成代碼A J < b 

clear all; close all; clc; p_0 = 10; v_0 = 0; a_0 = 0; K=20; dt=0.2; log=[0 p_0 v_0 a_0]; w1 = 100; w2 = 1; w3 = 1; w4 = 1; for t=0.2:0.2:20
    %% Construct the prediction matrix [Tp, Tv, Ta, Bp, Bv, Ba] = getPredictionMatrix(K,dt,p_0,v_0,a_0); %% Construct the optimization problem H = w4*eye(K)+w1*(Tp'*Tp)+w2*(Tv'*Tv)+w3*(Ta'*Ta);
    F = w1*Bp'*Tp+w2*Bv'*Tv+w3*Ba'*Ta;
 A = [Tv;-Tv;Ta;-Ta]; b = [ones(20,1)-Bv;ones(20,1)+Bv;ones(20,1)-Ba;ones(20,1)+Ba];%[-1,1] %% Solve the optimization problem J = quadprog(H,F,A,b); %% Apply the control j = J(1); p_0 = p_0 + v_0*dt + 0.5*a_0*dt^2 + 1/6*j*dt^3; v_0 = v_0 + a_0*dt + 0.5*j*dt^2; a_0 = a_0 + j*dt; %% Log the states log = [log; t p_0 v_0 a_0]; end %% Plot the result plot(log(:,1),log(:,2:4)); grid on; xlabel('t(s)'); legend('p','v','a');

預測時域只有4s,所以無法在如此短的時間內位置從10m達到0m,這是由於限制了速度和加速度1 m / s 1m/s1m/s,無法求得解.
所以將目標放到優化函數,這樣至少有一個解.
note:模型預測控制受限於預測時域,但如果要擴大模型預測時域,會使得系統的計算負擔加大,而太短則會使得系統無法鎮定,從而發散.

soft constraints

如果速度和加速度不可避免超出約束限制呢 ?比如刮風
如

那么求解器會沒有解,控制器也不知道該如何去控制?
此時需要將懲罰函數(S ( V ) )添加到優化目標,不過這樣不能使用二次規划問題.
一般使用指數函數,但這樣在轉角位置不可導,

 

M是一個很大的正數

L要足夠大,使得等式成立,又不能太大,不然會超過約束條件太多,所以加入 w _s ∗ L ^T ∗ L 這樣能夠用二次優化問題來求解.

clear all; close all; clc; p_0 = 10; v_0 = -3; a_0 = 0; K=20; dt=0.2; log=[0 p_0 v_0 a_0]; w1 = 10; w2 = 1; w3 = 1; w4 = 1; w5 = 1e4; for t=0.2:0.2:20
    %% Construct the prediction matrix [Tp, Tv, Ta, Bp, Bv, Ba] = getPredictionMatrix(K,dt,p_0,v_0,a_0); %% Construct the optimization problem H = blkdiag(w4*eye(K)+w1*(Tp'*Tp)+w2*(Tv'*Tv)+w3*(Ta'*Ta),w5*eye(K));%note w5->M
    F = [w1*Bp'*Tp+w2*Bv'*Tv+w3*Ba'*Ta zeros(1,K)];
 A = [Tv zeros(K);-Tv -eye(K);Ta zeros(K); -Ta zeros(K); zeros(size(Ta)) -eye(K)];%-eye(K)->L b = [ones(20,1)-Bv;ones(20,1)+Bv;ones(20,1)-Ba;ones(20,1)+Ba; zeros(K,1)]; %% Solve the optimization problem J = quadprog(H,F,A,b); %% Apply the control j = J(1); p_0 = p_0 + v_0*dt + 0.5*a_0*dt^2 + 1/6*j*dt^3; v_0 = v_0 + a_0*dt + 0.5*j*dt^2; a_0 = a_0 + j*dt; %% Log the states log = [log; t p_0 v_0 a_0]; end %% Plot the result plot(log(:,1),log(:,2:4)); grid on; xlabel('t(s)'); legend('p','v','a');

 

Design rule

設計狀態時考慮軟約束,以免沒有解

• 它們受到測量噪聲和干擾的影響
  輸入考慮硬約束
• 它們可以任意變化，不受噪音干擾,超過約束可能會損害物理系統

limitation

• 通常需要線性模型，或者模型可以合理的線性化(自適應MPC) ,參考MATLAB
• 障礙約束通常具有非凸性。如下圖所示,障礙物為藍色,在外面為或的關系,是非凸的.

Parameter space: Boundary constrained motion primitives (BSCP)

• 多項式參數空間

• 零階保持參數空間

• BSCP

Parameter space: Jerk limited trajectory

我們不僅需要有限的沖擊jerk，而且還有有限的加速度和速度。
將稱其為JLT。

第二步中需要內環約束映射到外環的原因是由於運動規划器針對外環操作

下圖上部是三階積分器模型
使用時最優控制
不僅能對輸入量進行控制,也能狀態量進行控制

結果表明,軌跡主要由u=−ujmax​,u=ujmax和0三段組成,[論文](On-Line Trajectory Generation in Robotic Systems)

如何求解?
通過觀察,加速度圖像要么是三角形,要么是梯形,目標是使得加速度圖像的面積等於 要求的速度變化量

 

容易求出一個二階積分器,接下來是三階積分器

從任意狀態到期望點
速度,位置曲線越平滑越有利於飛行

 例子

此時對應的是位置的變化
先加速到最大,然后減速,需要求解加速時間

限制在安全走廊問題

• 一種高效的三維速度、加速度和限jerk軌跡生成算法,對硬件要求低
• 能對變化的即時反應(環境/任務)

JLT:Non-linear MPC

environment perception

Pixelized environment
●占據地圖
〇像素是否被占用
●成本地圖

• 進入某個像素的代價
• 離最近障礙物的距離信息(EDT)

未知區域代價在兩者之間

映射過程

1. 讀取傳感器數據
2. 更新占用地圖
   a.添加新的障礙
   b.清潔LOS區域
   3.執行EDT
3. 執行成本分配

Two level guidance
把問題分成一系列的TPBVP(Two-point boundary value problem)。
•全局規划
提供一系列相互連接的線段
•軌跡規划
求解jerk有限軌跡TPBVP。
該軌跡將車輛導向線段路徑上的第一個拐點Xf(每個Xf對應一條軌跡)。
它為評估提出了多種軌跡。
•評估者
評估軌跡的質量

Evaluator

檢查每個軌跡的質量,不根據采樣點來評分(點與點之間可能有障礙物)
•使用connected表示原始軌跡線段。
•執行DDA marching，檢查每個像素覆蓋的軌跡。
•找到具有最高成本價值的像素。
•根據時間評估軌跡
持續時間和清潔度

 

Event Manager

(事件驅動MPC!,Y確定何時更新軌跡)搭配tube 來使用,可以使得MPC的更新頻率有依據,便於降低計算量
●軌跡調節
〇 持續監控當前軌跡是否安全。

●事件處理程序
〇如果當前軌跡即將結束，觸發一個重新規划事件
〇如果當前軌道不安全，則觸發重新規划應急事件
●節省計算能力，飛行更平穩

對比圖

cost function and constraints(由於本來就滿足前三個約束,只需要關注最后一個)

硬約束:

 軟約束(非線性)

running cost
組成:軌跡到目標點距離,軌跡平滑(懲罰過大的輸入),障礙物約束

 end cost

x0​,xf​−>x(t)

x_0,固定,確定x_f,但卻很難得到關於x_f 的梯度,所以這是一個缺乏梯度的優化函數(由於非線性軟約束的存在),而且在對其軌跡進行評分時函數是不連續粒子群優化

在地圖中散x_f 點,對到達的終點x _f ​的軌跡進行評分,最終收斂到一個點,其自身存在速度,關乎下一次到達的點

通過不斷的迭代,得到最優的final point
 

由於θ ^∗的牽引到達最優點

每一次MPC通過PSO找到最優的終點.

飛行效果

JLT僅僅適應於三階積分器,不適應於汽車模型,固定翼等等,那么需要更一般的算法

Parameter space: General BSCP

一般的BVP問題:

終止條件,第二個,
動力學約束

通常需要數值優化(數值解)。
有時可以通過控制器來達到相同效果.

[論文](Model Predictive Local Motion Planning With Boundary State Constrained Primitives)

問題說這個系統是一個黑箱的系統?

對於這種黑箱系統,需要PSO這類算法,但是需要采樣許多點才能夠收斂,系統實時性慢,
那么可以將

數值優化解用神經網絡來近似參數 ,不需要梯度的方法.

更新x0 。

優化

得到θ ^∗
使用NN估計輸入u,作為參考量,輸出軌跡,不准,
將θ ^∗帶入原來的BVP,解出來,

•  List item

不需要費時的數值解

• 直接用輸入

網絡的大小和訓練細節

估測梯度

PSO

PSO: 10個粒子，10次迭代。VS均勻采樣:100個粒子1次迭代

PSO相對於均勻采樣軌跡更平滑

Unicycle model

對於更大的差速小車，需要有線加速度和角加速度，二階系統。

Dynamic window approach

• 給定一組期望的線速度，角速度
• 預測的未來軌跡是一段圓狐
• 根據障礙物距離，無碰撞距離，與目標的相對距離等計算軌跡。
• 給每個軌跡評分
• 選擇分數較高的軌跡
  詳見博客

每次DWA算法都需要在（紅色的窗口，但不能在深灰色區域）找到一個最優的速度和角速度，限制選擇范圍是因為電機速度不能大的跳變。

Second order unicycle

對線性加速度和角加速度有額外的限制。
給定一對預期的線速度和角速度，未來的軌跡不再是一個圓弧。
需要一個底層控制器用於調節線速度/角速度到期望的值。

compare DWA with our approach

DWA	MPC
限制在一階小車	處理二階也可以
軌跡預測為一個圓弧，對於高階可以用forward simulation	對於高階可以用forward simulation或者神經網絡
找到優化軌跡方法，也就是最優的速度，如前文所述	粒子群優化，比DWA均勻采樣的更優

Homework

Previously, we have discussed how to design a quadratic programming based MPC to allow a single-axis triple integrator to travel from an arbitrary state to the centre of the state space, a.k.a with zero position,velocity and acceleration.Now please design a quadratic programming based MPC to track conical spiral for a 3-axis triple integrator (basically a quadrotor model).

 

向下速度減弱一些,防止失速

作業二

二階模型
評價函數已經給出了，任務從任意的初始狀態到地圖的中心。