8基于模型预测控制的路径规划

文章目录

  • Introduction
    • Model
  • Parameter space
    • Optimization
    • Control
    • Tube based MPC
    • Convenient sources
  • Linear Model Predictive Control
    • hard constraints
    • soft constraints
    • Design rule
    • limitation
    • Parameter space: Boundary constrained motion primitives (BSCP)
      • Parameter space: Jerk limited trajectory
  • JLT:Non-linear MPC
    • environment perception
    • Event Manager
    • Parameter space: General BSCP
      • PSO
    • Unicycle model
    • Dynamic window approach
    • Second order unicycle
    • compare DWA with our approach
  • Homework
    • 作业二

Introduction

  • Model
    • System model(质点本身的运动)
    • Problem model(一般需要非线性方法来解决)
  • Prediction
    • State space
    • Input space
    • Parameter space
  • Control
    • The process of choosing the best policy
      8基于模型预测控制的路径规划_第1张图片

Model

模型预测控制通常的描述
8基于模型预测控制的路径规划_第2张图片
final cost,tf关于系统最后的状态的函数
min ⁡ C F ( x ( t f ) ) \min C_F\left( x\left( t_f \right) \right) minCF(x(tf))
running cost,t0-tf之间的cost,关于系统状态的函数
+ ∫ t = t 0 t f C R ( x , u ) d t +\int_{t=t_0}^{t_f}{C_R\left( x,u \right) dt} +t=t0tfCR(x,u)dt

约束分别为:
动力学约束(微分约束)
不等式约束(关于功率,加速度和速度等限制)
等式约束(从系统的一个状态到另一个状态的边界约束)
障碍物约束(特殊,通常非凸的)

Parameter space

在预测轨迹有作用.

min ⁡ C F ( x ( t f ) ) + ∫ t = t 0 t f C R ( x , u ) d t \min C_F\left( x\left( t_f \right) \right) +\int_{t=t_0}^{t_f}{C_R\left( x,u \right) dt} minCF(x(tf))+t=t0tfCR(x,u)dt
目的:找寻最佳的输入 u u u
需要将无限维度的输入转化到有限维度的输入.
方法:

  • 零阶保持器(直接采样离散化),如下图有13段调整的输入
  • 多项式(将输入转化为多项式,调整a,b,c,d参数)
  • B样条
  • 数值解方法
    • 有限Jerk轨迹
    • 基于神经网络的方法
      8基于模型预测控制的路径规划_第3张图片

Optimization

  • 搜索:
    • 图搜索
    • 基于随机抽样的搜索
  • 凸优化:
    • 二次规划,解决线性优化
  • 非凸优化:
    • 序列二次规划
    • 粒子群优化解决非线性,不连续的优化

Control

8基于模型预测控制的路径规划_第4张图片

优化的输入 u ∗ u^* 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段
8基于模型预测控制的路径规划_第5张图片
变为矩阵描述
8基于模型预测控制的路径规划_第6张图片
预测模型
T p , T v , T a T_p,T_v,T_a Tp,Tv,Ta参考如下代码,大致思路就是便是将这几个量(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:**平滑轨迹。优化函数在这里插入图片描述
w n w_n 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,4s20个时间段,每段0.2s
log记录系统轨迹
调节四个权重的值
8基于模型预测控制的路径规划_第7张图片
J 的解析解,不去解QP问题,可能效率会更高些
J=-inv(H)*F';%J的解析解
8基于模型预测控制的路径规划_第8张图片在这里插入图片描述

hard constraints

8基于模型预测控制的路径规划_第9张图片
由于许多求解器使用的都是小于形式,所以需要化为四组约束条件,写成代码 A J < b AJAJ<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');

8基于模型预测控制的路径规划_第10张图片
预测时域只有4s,所以无法在如此短的时间内位置从10m达到0m,这是由于限制了速度和加速度 1 m / s 1m/s 1m/s,无法求得解.
所以将目标放到优化函数,这样至少有一个解.
note:模型预测控制受限于预测时域,但如果要扩大模型预测时域,会使得系统的计算负担加大,而太短则会使得系统无法镇定,从而发散.

soft constraints

如果速度和加速度不可避免超出约束限制呢 ?比如刮风
8基于模型预测控制的路径规划_第11张图片
那么求解器会没有解,控制器也不知道该如何去控制?
此时需要将惩罚函数( S ( V ) S(V) S(V))添加到优化目标,不过这样不能使用二次规划问题.
一般使用指数函数,但这样在转角位置不可导,
8基于模型预测控制的路径规划_第12张图片

8基于模型预测控制的路径规划_第13张图片
M是一个很大的正数
L要足够大,使得等式成立,又不能太大,不然会超过约束条件太多,所以加入 w s ∗ L T ∗ L w_s*L^T*L wsLTL
这样能够用二次优化问题来求解.

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');

8基于模型预测控制的路径规划_第14张图片

Design rule

设计状态时考虑软约束,以免没有解

  • 它们受到测量噪声和干扰的影响
    输入考虑硬约束
  • 它们可以任意变化,不受噪音干扰,超过约束可能会损害物理系统

limitation

  • 通常需要线性模型,或者模型可以合理的线性化(自适应MPC) ,参考MATLAB
  • 障碍约束通常具有非凸性。如下图所示,障碍物为蓝色,在外面为或的关系,是非凸的.

8基于模型预测控制的路径规划_第15张图片

Parameter space: Boundary constrained motion primitives (BSCP)

  • 多项式参数空间
    在这里插入图片描述

  • 零阶保持参数空间
    8基于模型预测控制的路径规划_第16张图片

  • BSCP
    8基于模型预测控制的路径规划_第17张图片

Parameter space: Jerk limited trajectory

我们不仅需要有限的冲击jerk,而且还有有限的加速度和速度。
将称其为JLT。
8基于模型预测控制的路径规划_第18张图片

8基于模型预测控制的路径规划_第19张图片

8基于模型预测控制的路径规划_第20张图片
第二步中需要内环约束映射到外环的原因是由于运动规划器针对外环操作

下图上部是三阶积分器模型
使用时最优控制
不仅能对输入量进行控制,也能状态量进行控制
8基于模型预测控制的路径规划_第21张图片
8基于模型预测控制的路径规划_第22张图片
结果表明,轨迹主要由 u = − u j m a x , u = u j m a x u=-u_{jmax},u=u_{jmax} u=ujmax,u=ujmax和0三段组成,[论文](On-Line Trajectory Generation in Robotic Systems)

如何求解?
通过观察,加速度图像要么是三角形,要么是梯形,目标是使得加速度图像的面积等于 要求的速度变化量
8基于模型预测控制的路径规划_第23张图片
8基于模型预测控制的路径规划_第24张图片
容易求出一个二阶积分器,接下来是三阶积分器

从任意状态到期望点
速度,位置曲线越平滑越有利于飞行
8基于模型预测控制的路径规划_第25张图片
例子
8基于模型预测控制的路径规划_第26张图片
此时对应的是位置的变化
先加速到最大,然后减速,需要求解加速时间
8基于模型预测控制的路径规划_第27张图片

限制在安全走廊问题
8基于模型预测控制的路径规划_第28张图片8基于模型预测控制的路径规划_第29张图片

  • 一种高效的三维速度、加速度和限jerk轨迹生成算法,对硬件要求低
  • 能对变化的即时反应(环境/任务)
    8基于模型预测控制的路径规划_第30张图片

JLT:Non-linear MPC

environment perception

Pixelized environment
●占据地图
〇像素是否被占用
●成本地图

  • 进入某个像素的代价
  • 离最近障碍物的距离信息(EDT)
    8基于模型预测控制的路径规划_第31张图片
    8基于模型预测控制的路径规划_第32张图片
    未知区域代价在两者之间

映射过程

  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的更新频率有依据,便于降低计算量
●轨迹调节
〇 持续监控当前轨迹是否安全。
●事件处理程序
〇如果当前轨迹即将结束,触发一个重新规划事件
〇如果当前轨道不安全,则触发重新规划应急事件
●节省计算能力,飞行更平稳

对比图
8基于模型预测控制的路径规划_第33张图片

cost function and constraints(由于本来就满足前三个约束,只需要关注最后一个)
8基于模型预测控制的路径规划_第34张图片
硬约束: x ∉ O b s t a c l e x\notin Obstacle x/Obstacle
软约束(非线性)
s ( x ) = { 0 , i f x ∉ O b s t a c l e M , o t h e r w i s e s\left( x \right) =\begin{cases} 0,ifx\notin Obstacle\\ M,otherwise\\ \end{cases} s(x)={0,ifx/ObstacleM,otherwise
running cost
组成:轨迹到目标点距离,轨迹平滑(惩罚过大的输入),障碍物约束
在这里插入图片描述
end cost
在这里插入图片描述
x 0 , x f − > x ( t ) x_0,x_f->x(t) x0,xf>x(t)
x_0,固定,确定x_f,但却很难得到关于 x f x_f xf的梯度,所以这是一个缺乏梯度的优化函数(由于非线性软约束的存在),而且在对其轨迹进行评分时函数是不连续

粒子群优化

8基于模型预测控制的路径规划_第35张图片
8基于模型预测控制的路径规划_第36张图片
在地图中散 x f x_f xf点,对到达的终点 x f x_f xf的轨迹进行评分,最终收敛到一个点,其自身存在速度,关乎下一次到达的点
8基于模型预测控制的路径规划_第37张图片

8基于模型预测控制的路径规划_第38张图片
通过不断的迭代,得到最优的final point
8基于模型预测控制的路径规划_第39张图片
由于 θ ∗ \theta^* θ的牵引到达最优点
每一次MPC通过PSO找到最优的终点.
飞行效果
8基于模型预测控制的路径规划_第40张图片
JLT仅仅适应于三阶积分器,不适应于汽车模型,固定翼等等,那么需要更一般的算法

Parameter space: General BSCP

一般的BVP问题:
8基于模型预测控制的路径规划_第41张图片
终止条件,第二个,
动力学约束 x . = f ( x , u ) x^{.}=f(x,u) x.=f(x,u)

通常需要数值优化(数值解)。
有时可以通过控制器来达到相同效果.
8基于模型预测控制的路径规划_第42张图片

8基于模型预测控制的路径规划_第43张图片

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

8基于模型预测控制的路径规划_第44张图片
问题说这个系统是一个黑箱的系统?

对于这种黑箱系统,需要PSO这类算法,但是需要采样许多点才能够收敛,系统实时性慢,
那么可以将
8基于模型预测控制的路径规划_第45张图片
数值优化解用神经网络来近似参数 ,不需要梯度的方法.

S : < x 0 , θ > → < u ∧ ( t ) , x ∧ ( t ) , t ∧ f > S:\rightarrow <\overset{\land}{u}\left( t \right) ,\overset{\land}{x}\left( t \right) ,\overset{\land}{t}_f> S:<x0,θ>→<u(t),x(t),tf>
更新 x 0 x_0 x0.
优化在这里插入图片描述
得到 θ ∗ \theta^* θ
使用NN估计输入u,作为参考量,输出轨迹,不准,
θ ∗ \theta^* θ带入原来的BVP,解出来,

  • List item

不需要费时的数值解

  • 直接用输入

    网络的大小和训练细节
    8基于模型预测控制的路径规划_第46张图片
    估测梯度

8基于模型预测控制的路径规划_第47张图片

PSO

PSO: 10个粒子,10次迭代。VS均匀采样:100个粒子1次迭代
8基于模型预测控制的路径规划_第48张图片
PSO相对于均匀采样轨迹更平滑

8基于模型预测控制的路径规划_第49张图片

Unicycle model

8基于模型预测控制的路径规划_第50张图片
对于更大的差速小车,需要有线加速度和角加速度,二阶系统。

Dynamic window approach

8基于模型预测控制的路径规划_第51张图片

  • 给定一组期望的线速度,角速度
  • 预测的未来轨迹是一段圆狐
  • 根据障碍物距离,无碰撞距离,与目标的相对距离等计算轨迹。
  • 给每个轨迹评分
  • 选择分数较高的轨迹
    详见博客

每次DWA算法都需要在(红色的窗口,但不能在深灰色区域)找到一个最优的速度和角速度,限制选择范围是因为电机速度不能大的跳变
8基于模型预测控制的路径规划_第52张图片

Second order unicycle

8基于模型预测控制的路径规划_第53张图片
8基于模型预测控制的路径规划_第54张图片
对线性加速度和角加速度有额外的限制。
给定一对预期的线速度和角速度,未来的轨迹不再是一个圆弧。
需要一个底层控制器用于调节线速度/角速度到期望的值。

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).8基于模型预测控制的路径规划_第55张图片
向下速度减弱一些,防止失速

作业二

二阶模型
评价函数已经给出了,任务从任意的初始状态到地图的中心。8基于模型预测控制的路径规划_第56张图片

你可能感兴趣的:(Motion,Planning,自动驾驶)