文章目录
参考资料
1. 基本概念
-
模型预测控制(MPC)的核心思想就是以优化方法求解最优控制器,其中优化方法大多时候采用二次规划(Quadratic Programming)。
-
MPC控制器优化得到的控制输出也是系统在未来有限时间步的控制序列。 当然,由于理论构建的模型与系统真实模型都有误差,所以,实际上更远未来的控制输出对系统控制的价值很低,故MPC仅执行输出序列中的第一个控制输出。
模型(Model)
分为机理模型和基于数据的模型(例如用神经网络训练的一个model)使用基于数据的模型的MPC可以结合model based RL使用。
预测(Predict)
模型就是用来预测的,预测的目的是为了更好的决策
控制(Control)
控制即决策,根据预测来作出决策。
MPC利用一个已有的模型、系统当前的状态和未来的控制量,来预测系统未来的输出,然后与我们期望的系统输出做比较,得到一个损失函数(代价函数),即:
由于上式中模型、当前状态、期望输出都是已知的,因此只有未来控制量一个自变量。采用二次规划的方法求解出某个未来控制量,使得损失函数最小,前面提到,这个未来控制量的第一个元素就是当前控制周期的控制量。
1.1 MPC vs optimal control
最优控制(optimal control)指的是在一定的约束情况下达到最优状态的系统表现,其中约束情况通常是实际环境所带来的限制,例如汽车中的油门不能无限大等。
最优控制强调的是“最优”,一般最优控制需要在整个时间域上进行求优化(从0时刻到正无穷时刻的积分),这样才能保证最优性,这是一种很贪婪的行为,需要消耗大量算力。同时,系统如果是一个时变系统,或者面临扰动的话,前一时刻得到的最优并不一定是下一时刻的最优值。
最优控制常用解法有: 变分法,极大值原理,动态规划,LQR(LQR可以参考博客)。
MPC仅考虑未来几个时间步,一定程度上牺牲了最优性。
1.2 MPC优点
-
MPC善于处理多输入多输出系统(MIMO);
-
MPC可以处理约束,如安全性约束,上下阈值;
-
MPC是一种向前考虑未来时间步的有限时域优化方法(一定的预测能力)
最优控制要求在整个时间优化
实际上MPC采用了一个折中的策略,既不是像最优控制那样考虑整个时域,也不是仅仅考虑当前,而是考虑未来的有限时间域。
2. MPC整体流程
2.1 预测区间与控制区间
对于一般的离散化系统(因为实际计算机实现的控制系统都是离散的系统,连续系统可以进行离散化操作),在k时刻,我们可以测量出系统的当前状态,再通过计算得到的
得到系统未来状态的估计值
;
将预测状态估计的部分称为预测区间(Predictive Horizon),指的是一次优化后预测未来输出的时间步的个数。
将控制估计的部分称为控制区间(Control Horizon),在得到最优输入之后,我们只施加当前时刻的输入u(k),即控制区间的第一位控制输入。
如下图 范围为控制区间,之后的红色部分称为 held constant,其中控制区间是要通过优化器来进行优化的参数。
过小的控制区间,可能无法做到较好的控制,而较大的控制区间,比如与预测区间相等,则会导致只有前一部分的控制范围才会有较好的效果,而后一部分的控制范围则收效甚微,而且将带来大量的计算开销。
2.2 约束
对于约束,一般分为Hard约束和Soft约束,Hard约束是不可违背必须遵守的,在控制系统中,输入输出都可能会有约束限制,但是在设计时不建议将输入输出都给予Hard约束,因为这两部的约束有可能是有重叠的,导致优化器会产生不可行解。
Hard约束不能违反,Soft约束可以;比如Hard约束是刹车踩的幅度;Soft约束是速度
建议输出采用Soft约束,而输入的话建议输入和输入参数变化率二者之间不要同时为Hard约束,可以一个Hard一个Soft。
2.3 MPC流程
模型预测控制在k时刻共需三步;
-
第一步:获取系统的当前状态;
-
第二步:基于
进行最优化处理;
离散系统的代价函数可以参考
其中
表示误差的终值,也是衡量优劣的一种标准。
-
第三步:只取
作为控制输入施加在系统上。
在下一时刻重复以上三步,在下一步进行预测时使用的就是下一步的状态值,我们将这样的方案称为滚动优化控制(Receding Horizon Control)。
预测控制的优化不是一次离线进行,而是随着采样时刻的前进反复地在线进行,故称为
滚动优化
。这种滚动优化虽然得不到理想的全局最优解,但是反复对每一采样时刻的偏差进行优化计算,将可及时地校正控制过程中出现的各种复杂情况。
2.4 MPC vs. LQR
从以下几个方面进行阐述:
- 研究对象:是否线性化
- 状态方程:是否离散化
- 目标函数:误差和控制量的极小值
- 工作时域:预测时域,控制时域,滚动优化,求解一次
- 求解方法:QP求解器,变分法求解黎卡提方程
- LQR和MPC的优缺点:滚动优化,求解时域,实时性,算力,工程常用方法
具体可参考博客
3. MPC设计
当模型是线性的时候(非线性系统可以线性化),MPC的设计求解一般使用二次规划方法。
设线性模型为以下形式:
假定未来 步的控制输入已知,为
,根据以上模型与输入,我们可以计算未来
步的状态:
将上面步写成矩阵向量形式:
其中,
上式中的下三角形式,直接反映了系统在时间上的因果关系,即
时刻的输入对
时刻的输出没有影响,
时刻的输入对
和
时刻没有影响。
假定参考轨迹为 ,则MPC的一个简单的目标代价函数如下:
其中,
这一项是为了让控制输入不会太大,因此代价函数中添加了一项对控制量的约束。
将式(2)代入式(3),则优化变量仅剩 。以上最优化问题可用二次规划方法求解,得到满足目标代价函数的最优控制序列
。
当转换成式(3)后,可以利用凸优化库进行二次规划求解,例如python的cvxopt,OSQP: An Operator Splitting Solver for Quadratic Programs,Casdi等。
4. MPC应用——无人车轨迹跟踪
4.1 MPC建模
设车辆的状态量偏差和控制量偏差如式 ( 4 ) 所示:
基于先前的运动学模型的离散状态空间方程如下,
为了表示控制系统达到稳定控制所付出的代价,MPC控制的代价函数定义如下:
其中函数参数 ,并且矩阵
为正定矩阵,即
Q | Q_(f) | R |
---|---|---|
给定状态代价矩阵 | 最终状态代价矩阵 | 输入代价矩阵 |
: 时间范围(Time Horizon)
: 分别设定状态偏差和输入的相对权重
: 意味着任何非零输入都增加
的代价
: 衡量状态偏差
: 衡量输入大小
: 衡量最终状态偏差
对于公式(6),它需要服从的约束条件包括
4.2 python代码实现
完整程序见GitHub仓库
4.2.1 参数
# mpc parameters
NX = 3 # x = x, y, yaw
NU = 2 # u = [v,delta]
T = 8 # horizon length
R = np.diag([0.1, 0.1]) # input cost matrix
Rd = np.diag([0.1, 0.1]) # input difference cost matrix
Q = np.diag([1, 1, 1]) # state cost matrix
Qf = Q # state final matrix
#车辆
dt=0.1 # 时间间隔,单位:s
L=2 # 车辆轴距,单位:m
v = 2 # 初始速度
x_0=0 # 初始x
y_0=-3 #初始y
psi_0=0 # 初始航向角
MAX_STEER = np.deg2rad(45.0) # maximum steering angle [rad]
MAX_DSTEER = np.deg2rad(45.0) # maximum steering speed [rad/s]
MAX_VEL = 2.0 # maximum accel [m/s]
4.2.2 运动学模型
import math
class KinematicModel_3:
"""假设控制量为转向角delta_f和加速度a
"""
def __init__(self, x, y, psi, v, L, dt):
self.x = x
self.y = y
self.psi = psi
self.v = v
self.L = L
# 实现是离散的模型
self.dt = dt
def update_state(self, a, delta_f):
self.x = self.x+self.v*math.cos(self.psi)*self.dt
self.y = self.y+self.v*math.sin(self.psi)*self.dt
self.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dt
self.v = self.v+a*self.dt
def get_state(self):
return self.x, self.y, self.psi, self.v
def state_space(self, ref_delta, ref_yaw):
"""将模型离散化后的状态空间表达
Args:
ref_delta (_type_): 参考的转角控制量
ref_yaw (_type_): 参考的偏航角
Returns:
_type_: _description_
"""
A = np.matrix([
[1.0, 0.0, -self.v*self.dt*math.sin(ref_yaw)],
[0.0, 1.0, self.v*self.dt*math.cos(ref_yaw)],
[0.0, 0.0, 1.0]])
B = np.matrix([
[self.dt*math.cos(ref_yaw), 0],
[self.dt*math.sin(ref_yaw), 0],
[self.dt*math.tan(ref_delta)/self.L, self.v*self.dt /(self.L*math.cos(ref_delta)*math.cos(ref_delta))]
])
C = np.eye(3)
return A, B, C
4.2.3 参考轨迹
class MyReferencePath:
def __init__(self):
# set reference trajectory
# refer_path包括4维:位置x, 位置y, 轨迹点的切线方向, 曲率k
self.refer_path = np.zeros((1000, 4))
self.refer_path[:,0] = np.linspace(0, 100, 1000) # x
self.refer_path[:,1] = 2*np.sin(self.refer_path[:,0]/3.0)+2.5*np.cos(self.refer_path[:,0]/2.0) # y
# 使用差分的方式计算路径点的一阶导和二阶导,从而得到切线方向和曲率
for i in range(len(self.refer_path)):
if i == 0:
dx = self.refer_path[i+1,0] - self.refer_path[i,0]
dy = self.refer_path[i+1,1] - self.refer_path[i,1]
ddx = self.refer_path[2,0] + self.refer_path[0,0] - 2*self.refer_path[1,0]
ddy = self.refer_path[2,1] + self.refer_path[0,1] - 2*self.refer_path[1,1]
elif i == (len(self.refer_path)-1):
dx = self.refer_path[i,0] - self.refer_path[i-1,0]
dy = self.refer_path[i,1] - self.refer_path[i-1,1]
ddx = self.refer_path[i,0] + self.refer_path[i-2,0] - 2*self.refer_path[i-1,0]
ddy = self.refer_path[i,1] + self.refer_path[i-2,1] - 2*self.refer_path[i-1,1]
else:
dx = self.refer_path[i+1,0] - self.refer_path[i,0]
dy = self.refer_path[i+1,1] - self.refer_path[i,1]
ddx = self.refer_path[i+1,0] + self.refer_path[i-1,0] - 2*self.refer_path[i,0]
ddy = self.refer_path[i+1,1] + self.refer_path[i-1,1] - 2*self.refer_path[i,1]
self.refer_path[i,2]=math.atan2(dy,dx) # yaw
# 计算曲率:设曲线r(t) =(x(t),y(t)),则曲率k=(x'y" - x"y')/((x')^2 + (y')^2)^(3/2).
# 参考:https://blog.csdn.net/weixin_46627433/article/details/123403726
self.refer_path[i,3]=(ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) # 曲率k计算
def calc_track_error(self, x, y):
"""计算跟踪误差
Args:
x (_type_): 当前车辆的位置x
y (_type_): 当前车辆的位置y
Returns:
_type_: _description_
"""
# 寻找参考轨迹最近目标点
d_x = [self.refer_path[i,0]-x for i in range(len(self.refer_path))]
d_y = [self.refer_path[i,1]-y for i in range(len(self.refer_path))]
d = [np.sqrt(d_x[i]**2+d_y[i]**2) for i in range(len(d_x))]
s = np.argmin(d) # 最近目标点索引
yaw = self.refer_path[s, 2]
k = self.refer_path[s, 3]
angle = normalize_angle(yaw - math.atan2(d_y[s], d_x[s]))
e = d[s] # 误差
if angle < 0:
e *= -1
return e, k, yaw, s
def calc_ref_trajectory(self, robot_state, dl=1.0):
"""计算参考轨迹点,统一化变量数组,便于后面MPC优化使用
参考自https://github.com/AtsushiSakai/PythonRobotics/blob/eb6d1cbe6fc90c7be9210bf153b3a04f177cc138/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
Args:
robot_state (_type_): 车辆的状态(x,y,yaw,v)
dl (float, optional): _description_. Defaults to 1.0.
Returns:
_type_: _description_
"""
e, k, ref_yaw, ind = self.calc_track_error(robot_state[0], robot_state[1])
xref = np.zeros((NX, T + 1))
dref = np.zeros((NU, T)) # 参考控制量
ncourse = len(self.refer_path)
xref[0, 0] = self.refer_path[ind,0]
xref[1, 0] = self.refer_path[ind, 1]
xref[2, 0] = self.refer_path[ind, 2]
# 参考控制量[v,delta]
ref_delta = math.atan2(L*k, 1)
dref[0, :] = robot_state[3]
dref[1, :] = ref_delta
travel = 0.0
for i in range(T + 1):
travel += abs(robot_state[3]) * dt
dind = int(round(travel / dl))
if (ind + dind) < ncourse:
xref[0, i] = self.refer_path[ind + dind,0]
xref[1, i] = self.refer_path[ind + dind,1]
xref[2, i] = self.refer_path[ind + dind,2]
else:
xref[0, i] = self.refer_path[ncourse - 1,0]
xref[1, i] = self.refer_path[ncourse - 1,1]
xref[2, i] = self.refer_path[ncourse - 1,2]
return xref, ind, dref
4.2.4 矩阵拍平
def get_nparray_from_matrix(x):
return np.array(x).flatten()
4.2.5 角度归一化到[-pi,pi]
def normalize_angle(angle):
"""
Normalize an angle to [-pi, pi].
:param angle: (float)
:return: (float) Angle in radian in [-pi, pi]
copied from https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/stanley_control/stanley_control.html
"""
while angle > np.pi:
angle -= 2.0 * np.pi
while angle < -np.pi:
angle += 2.0 * np.pi
return angle
4.2.6 MPC控制实现
def linear_mpc_control(xref, x0, delta_ref,ugv):
"""
linear mpc control
xref: reference point
x0: initial state
delta_ref: reference steer angle
ugv:车辆对象
"""
x = cvxpy.Variable((NX, T + 1))
u = cvxpy.Variable((NU, T))
cost = 0.0 # 代价函数
constraints = [] # 约束条件
for t in range(T):
cost += cvxpy.quad_form(u[:, t]-delta_ref[:,t], R)
if t != 0:
cost += cvxpy.quad_form(x[:, t] - xref[:, t], Q)
A, B, C = ugv.state_space(delta_ref[1,t], xref[2,t])
constraints += [x[:, t + 1]-xref[:, t+1] == A @ (x[:, t]-xref[:, t]) + B @ (u[:, t]-delta_ref[:,t]) ]
cost += cvxpy.quad_form(x[:, T] - xref[:, T], Qf)
constraints += [(x[:, 0]) == x0]
constraints += [cvxpy.abs(u[0, :]) <= MAX_VEL]
constraints += [cvxpy.abs(u[1, :]) <= MAX_STEER]
prob = cvxpy.Problem(cvxpy.Minimize(cost), constraints)
prob.solve(solver=cvxpy.ECOS, verbose=False)
if prob.status == cvxpy.OPTIMAL or prob.status == cvxpy.OPTIMAL_INACCURATE:
opt_x = get_nparray_from_matrix(x.value[0, :])
opt_y = get_nparray_from_matrix(x.value[1, :])
opt_yaw = get_nparray_from_matrix(x.value[2, :])
opt_v = get_nparray_from_matrix(u.value[0, :])
opt_delta = get_nparray_from_matrix(u.value[1, :])
else:
print("Error: Cannot solve mpc..")
opt_v, opt_delta, opt_x, opt_y, opt_yaw = None, None, None, None, None,
return opt_v, opt_delta, opt_x, opt_y, opt_yaw
4.2.7 主函数
from celluloid import Camera # 保存动图时用,pip install celluloid
# 使用随便生成的轨迹
def main():
reference_path = MyReferencePath()
goal = reference_path.refer_path[-1,0:2]
# 运动学模型
ugv = KinematicModel_3(x_0, y_0, psi_0, v, L, dt)
x_ = []
y_ = []
fig = plt.figure(1)
# 保存动图用
camera = Camera(fig)
# plt.ylim([-3,3])
for i in range(500):
robot_state = np.zeros(4)
robot_state[0] = ugv.x
robot_state[1] = ugv.y
robot_state[2]=ugv.psi
robot_state[3]=ugv.v
x0 = robot_state[0:3]
xref, target_ind, dref = reference_path.calc_ref_trajectory(robot_state)
opt_v, opt_delta, opt_x, opt_y, opt_yaw = linear_mpc_control(xref, x0, dref, ugv)
ugv.update_state(0, opt_delta[0]) # 加速度设为0,恒速
x_.append(ugv.x)
y_.append(ugv.y)
# 显示动图
plt.cla()
plt.plot(reference_path.refer_path[:,0], reference_path.refer_path[:,1], "-.b", linewidth=1.0, label="course")
plt.plot(x_, y_, "-r", label="trajectory")
plt.plot(reference_path.refer_path[target_ind,0], reference_path.refer_path[target_ind,1], "go", label="target")
# plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
# camera.snap()
# 判断是否到达最后一个点
if np.linalg.norm(robot_state[0:2]-goal)<=0.1:
print("reach goal")
break
# animation = camera.animate()
# animation.save('trajectory.gif')
if __name__=='__main__':
main()
跟踪效果如下:
(跟踪效果不是很好,我并没有进一步调整,就先这样吧···。)
5. MPC开源库/程序
文章出处登录后可见!