-
+
模型预测控制在运动规划中的应用
+
Introduction
+
线性MPC解决的优化问题一般是凸的
+
非线性MPC解决的优化问题一般是非凸的
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled.1yeufe2nc6o.png)
+
+-
+
模型
+
+- 系统模型(描述系统自身)
+- 问题模型(描述优化目标)
+
+
+-
+
预测
+
+-
+
状态空间
+
+-
+
输入空间
+
+-
+
参数空间
+
+
+
+
+-
+
控制(选择最优的输入或策略)
+
+
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%201.l1yg4fu3w2.png)
+
x∈/Obstacle也相当于不等式约束,但由于该约束一般为非凸的,因此需要特殊处理
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%202.7lboaz63i28.png)
+
零阶保持器(离散)、多项式、B样条
+
数值映射的方法:Jerk limited trajectory、基于神经网络的方法
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%203.algva8vgcm.png)
+
优化的方法:
+
+- Searching
+- Convex optimization
+- Noconvex optimization
+
+- 顺序二次规划
+- 粒子群优化(适用于非凸、非线性、离散的情况)
+
+
+
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%204.4j90810t20d.png)
+
RHC(Receding Horizon Control),MPC的基础。
+
+- 建立优化问题
+- 获取当前的状态(作为优化问题的初始状态),确定优化问题
+- 求解优化问题,得到u∗
+- 将u∗直接用于短时间的状态控制
+- 循环2—4步……
+
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%205.4apu4ykq4qa.png)
+
Tube based MPC相较于RHC新增了Nominal System(相当于u∗用于输出参考轨迹x∗)和Associate controller(相当于参考轨迹x∗作为目标状态用于控制实际状态)
+
由于Nominal System只是数学模型,因此x∗是理想的;跟踪的鲁棒性问题(Model uncertainty和Disturbance)全都由Associate controller决定
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%206.h15sr8x0psc.png)
+
求解MPC的一些工具
+
Linear Model Predictive Control(MPC)
+
建立系统模型
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%207.mj030e19snh.png)
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%208.crx296bs4qj.png)
+
这里的控制量为jerk,4s内离散化为20份(通过离散,参数化J)。可以将离散的P、V、A表示为向量形式(并建立与向量J的关系)
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%209.ee39kmwkrn7.png)
+
这个表达式可以用离散状态转移方程推导得到
+
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2013.8468h4lbuve.png)
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2014.h49rwwql0yd.png)
+
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2015.3uywhtunu6c.png)
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2016.cgqfy8384g.png)
+
软约束——加入惩罚函数,优化问题数值求解(不在是二次规划问题)
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2017.t26zmo19n2r.png)
+
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2018.l5g9h3j6wla.png)
+
状态约束——一般使用软约束,状态一般受测量噪声和干扰的影响,故有可能超出预定范围
+
输入约束——一般使用硬约束
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2019.muvrv7vo3nc.png)
+
Non-linear MPC
+
Jerk Limited trajectory
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2020.omvi47ozkr.png)
+
参数空间:多项式参数空间、零阶保持器参数空间、BSCP(Boundary constrained motion primitives)——参数:系统的初末状态,通过解一个BVP问题,得到输入u(最后可以优化的参数相当于是系统的末状态)
+
BSCP中一种用得比较多的类型:JLT(Jerk Limited Trajectory)
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2021.l1tfh6vby2.png)
+
无人机内环约束(ωmax和fmax)转化为外环约束
+
先推导二阶的情况(到达目标速度)
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2022.51vaivg3x9g.png)
+
轨迹最多包含三段u=±umaxand0的情况 (详情见:On-Line Tranjectory Generation in Robotic Systems)
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2023.kut73kj6ee.png)
+
当vref>v0时,先取u=umax,当a达到amax时,取u=−umax,使at=0。a与x轴围成的面积即为Δv0,若Δv=vref−v0>Δv0,larger area——围成梯形面积,维持在amax一段时间;若Δv=vref−v0<Δv0,smaller area——实际达不到amax。进而可以得到输入u
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2024.2gfqu67gyq.png)
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2025.v4cxbh75zdd.png)
+
和只含v、a的二阶情况类似,含p、v、a的三阶情况是让v到最大速度,然后再让v回到0。根据这种情况下所围成的面积Δp0和实际的Δp大小,可以求得输入u的表达式
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2026.ce3bec4xvp5.png)
+
JLT+安全走廊——可以快速生成轨迹
+
首先生成一条从A到B的轨迹1,无人机沿着轨迹1飞行;每隔一段时间生成从当前点到C点的轨迹2,并判断轨迹2是否在安全走廊内——不在,抛弃该曲线;在,无人机跟随新的轨迹。重复
+
也可以二分法(查看从轨迹1中点到C点的轨迹是否在飞行走廊内部),一次性生成完整轨迹
+
Non-linear MPC
+
非线性MPC一般是用做局部规划器。
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2027.1qegjamt0h1.png)
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2028.udzob7upqd.png)
+
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2029.6q74hknpucj.png)
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2030.hta24eev3en.png)
+
普通的MPC会每隔一小段时间计算一次输入u∗。这往往会导致算力消耗过大,并且使生成的轨迹不平滑。而事件驱动型MPC只有当必要时才进行MPC重新计算,往往配合Tube based MPC使用(即有一个Associate controller控制轨迹)
+
必要情况:MPC生成的轨迹快跟踪完了,轨迹上有障碍,全局规划器路径变化
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2031.zmxkv7v6ydm.png)
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2032.r6nvu50zvg.png)
+
由于JLT已经天然地满足x˙=f(x,u),g(x,u)<0,h(x,u)=0,优化时仅需要考虑障碍物的约束。硬约束、软约束
+
当使用软约束(s(x)有很强的非线性)并随机散点时,目标函数往往是不具备梯度信息的。因此,需要使用不需要梯度信息的优化方法——如粒子群优化(Particle swarm optimization)
+
PSO的每轮迭代的采样点(粒子)按照一定法则运动,最终收敛到最优的点附近
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2033.x6dv8ua6r9e.png)
+
粒子每轮迭代的运动都沿速度方向
+
更新粒子的速度方向:考虑上一次迭代的速度方向,考虑朝全局最优的方向移动(全部粒子中的代价最小的粒子),考虑朝自身迭代历史中最优的方向移动
+
新的粒子速度方向为上述三个速度向量的求和(对每一项加上一个权重和随机数rand)
+
+
General BSCP
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2034.nfn73lqbavn.png)
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2035.ziy39rqcr2j.png)
+
对于General BSCP,通常需要求一个数值解(一个<x0,θ>对应一个<u^,x^,t^f>(参数为θ))。该优化问题同样为离散的形式——可以使用PSO
+
若直接使用PSO,则一次采样需要求解一次General BSCP问题(数值解)。而PSO优化又需要大量的采样,因此会导致计算速度慢,实时性差
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2036.05hoce5fu079.png)
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2037.oaio35nkq9.png)
+
给定输入<x0,θ>,通过神经网络得到<u^,x^,t^f>,不需要多次求数值解。最后得到最优的参数θ∗——可以用网络得到u∗或者求数值解保证精度
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2038.8hu1lgvt3i6.png)
+
神经网络估计梯度信息
+
![image](https://raw.githubusercontent.com/Hoshi-No-Ai/picee/main/Blog/Untitled%2039.yi61ygooyik.png)
+
使用x−xd容易陷入局部最优(可能会使轨迹卡住),因此可以采用Dijkstra等方法得到全局最优
+