一个标准的二次规划问题的目标函数可以写成
uminJ=21uHu+fTu
其中
提示
二次规划问题是一种特殊类型的最优化问题,当Q 为正定或正半定矩阵时,目标函数是凸的,此时问题有唯一的全局最优解(全局最小值)。
二次型目标函数前面加 21 主要是为了简化求导时的表达式,避免出现额外的系数,同时它也对问题的最优解没有实际影响。这种写法更多是数学推导和计算上的一种规范和习惯。
QP问题的约束条件有三种
- 不等式线性约束 MU<=b
- Mequ=beq
- 上下限约束 LB<=u<=UB
最优控制是指在一定的约束条件下,通过优化系统的控制输入,使得系统在特定性能指标上实现最优表现的一类控制方法。
一个最优控制问题包含以下几个方面
- 系统的数学模型(状态空间模型)
- 目标值(参考值)
- 性能指标(代价函数)
- 约束条件
| 特性 | LQR | MPC |
|---|
| 适用系统 | 线性系统 | 线性与非线性系统 |
| 目标函数 | 二次型、时间无穷 | 二次型、有限预测时间 |
| 优化问题的求解方式 | 一次性求解,通过黎卡提方程得到反馈矩阵后,不再进行优化 | 实时滚动优化,在每个时间步上重新求解优化问题 |
| 优化频率 | 一次性(设计时求解一次,避让设定系统期望) | 每个时间步(实时求解,每次滚动窗口预测并优化) |
| 计算复杂度 | 低,控制过程中只需简单的矩阵乘法 | 高,每个时间步需在线求解优化问题 |
| 约束处理 | 无法直接处理,需要外部技术支持 | 显式处理状态和控制输入约束 |
提示
MPC 与 LQR 的区别是增加了一个滚动优化控制(Receding Horizon Control),滚动优化控制(Receding Horizon Control)是指从当前时刻到未来一时刻这一有限时间段内,根据当前状态以及对未来的预测
在实践中MPC多为数字控制,因此使用离散系统进行分析。以一个跟踪问题为例,对于一个离散线性时不变系统的状态空间方程(status space)为
x(k+1)=Ax(k)+Bu(k)
定义其二次型代价函数为
uminJ=i=0∑N−1xk+i∣kTQxk+i∣k+uk+i∣kTRuk+i∣k
提示
现有的二次规划求解软件已经非常成熟,因此在处理模型预测控制问题时,我们将重点放在如何将问题转换为标准的二次规划形式。
mpc 接合当前状态和已有模型预测一段时间(predictive horizon)的控制结果,在实践中控制器一般是离散形式的,当预测步长为N时
将预测区间(predictive horizon)中所有的 x 写到一起
Xk=x(k∣k)x(k+1∣k)...x(k+N∣k)=x(k∣k)Ax(k∣k)+Bu(k∣k)A2x(k∣k)+ABu(k∣k)+Bu(k+1∣k)A3x(k∣k)+A2Bu(k∣k)+ABu(k+1∣k)+Bu(k+2∣k)...ANx(k∣k)+AN−2Bu(k∣k)AN−3Bu(k∣k)+...+Bu(k+N∣k)=IAA2A3...ANx(k∣k)+0Bu(k∣k)ABu(k∣k)+Bu(k+1∣k)A2Bu(k∣k)+ABu(k+1∣k)+Bu(k+2∣k)...AN−2Bu(k∣k)AN−3Bu(k∣k)+...+Bu(k+N∣k)=IAA2A3...ANx(k∣k)+0BABA2BA3BBABA2BBABBu(k∣k)u(k+1∣k)u(k+2∣k)u(k+3∣k)...u(k+N∣k)=Mxk+CUk
将控制区间(predictive horizon)中所有的 u 写到一起
Uk=[u(k∣k),u(k+1∣k),...,u(k+N∣k)]T
对于代价函数,预测区间(prediction horizon) Np和控制区间(control horizon)Nc,当 Np=Nc=N时
J=i=0∑N−1xk+i∣kTQxk+i∣k+uk+i∣kTQuk+i∣k=x(k∣k)x(k+1∣k)...x(k+N∣k)TQ0000Q000Q00...0Qx(k∣k)x(k+1∣k)...x(k+N∣k)+u(k∣k)u(k+1∣k)...u(k+N∣k)TR0000R000R00...0Ru(k∣k)u(k+1∣k)...u(k+N∣k)=(Mxk+CUk)TQ0000Q000Q00...0Q(Mxk+CUk)+UkTR0000R000R00...0RUk=(MXk+CUk)TQ(MXk+CUk)+UkTRUk=XkTMTQMXk+xkTMTQCUk+UkTCTQMXk+UkTCTQCUk+UkTRUk
提示
- 代价函数中每一项算出来都是一个实数,因此每一项的转置就等于其本身。
XkTMTQCUk=(UkTCTQMXk)T=UkTCTQMXk
- xk 时开始时刻的状态,X时是预测区间的所有状态。
令 G=MTQM, E=MTQC, H=CTQC+R 所以其二次型形式为
J=xkTGxk+2xkTEUk+UkTHUk
第一项与U 无关,省略掉,最后变为标准的二次型
J=UkTHUk+2xkTEUk
总结以下MPC控制的流程,在k时刻
估计/测量当前系统状态xk
对 U=[u(k∣k),u(k+1∣k),...,u(k+N∣k)]T 做最优化
代价函数为
J=UkTHUk+2xkTEUk
其中
- E=MTQC,
- H=CTQC+R
- M=[I,A,A2,A3,...,AN]T
- C=0BABA2BA3BBABA2BBABB
- Q 与 R 是要调整的参数,这是一个对角矩阵
- A 与 B 来自状态空间方程
只取 u(k∣k)
对于一个四旋翼飞行器跟踪轨迹的任务
x0=[pxgoal,pygoal,pzgoal,vxgoal,vygoal,vzgoal,axgoal,aygoal,azgoal]T
在一个预测区间(prediction horizon)中写成
Xr=[x0,x1,x2,...,xstep]
X0=[pxnow,pynow,pznow,vxnow,vynow,vznow,axnow,aynow,aznow]
微分平坦建立输入 u(jerk) 和 角速度的关系
在 world_frame下
pn=pn−1+Δt⋅vn−1+21Δt2⋅an−1+61Δt3⋅jn−1vn=vn−1+Δt⋅an−1+21Δt2⋅jn−1an=an−1+Δt⋅jn−1xn=[pn,vn,an]T,un=jn
写成 x(k+1)=Ax(k)+Bu(k) 的形式
即 A()
x(k+1)=[pk+1,vk+1,an]k+1=
f=((X0T×MT−XrT)×Qbar×C)T
状态(9):
- pos_x,pos_y,pos_z
- vel_x,vel_y,vel_z
- acc_x,acc_y,acc_z
MPC_HORIZON 和 step 有什么不一样
M (9 * MPC_HORIZON,9) C (9 * MPC_HORIZON,3 * MPC_HORIZON)
M=
∂t∂x(t)Y=Ax+Bu=Cx
x=(posx,posy,posz,velx,vely,velz,posy,accy,accz)T
A (9,9) B (9,3)
A=100t002t2000100t002t2000100t002t20001−Dxt0000000001−Dyt0000000001−Dzt000000t001000000t001000000t001
B=6t3002t200t0006t3002t200t0006t3002t200t
一个受约束的非线性优化问题
uNMPC=argumink=0∑N−1(∥xk−xk,r∥Q2+∥uk−uk,r∥Qu2)+∥xN−xN,r∥QN2
s.t. xk+1=f(xk,uk),x0=xinit ,ΩB∈[ΩminBΩmaxB],u∈[uminumax]