模型预测控制(MPC)的理论推导部分见前文: 自动驾驶轨迹跟踪-模型预测控制(MPC) 这里主要用Python代码实现前文的推导结论。
先展示一下最终的效果(蓝色是目标轨迹线，红色是车辆不断靠近目标轨迹线，并最终沿着目标轨迹线行驶)：
参考轨迹的速度：5m/s
1、二次规划问题求解
我们最终将优化的目标函数转化为二次规划问题，所以这里先了解下如何用OSQP求解二次规划问题(参考材料【2】)。
1.1 什么是二次规划
满足约束:
1.2 一些例子
具有线性约束的二次规划
目标函数:
需要满足以下约束
整理下目标函数:
OSQP求解二次规划:
代码语言：javascript复制import numpy as np
from scipy import sparse
import osqp
P = np.array([[1, -1], [-1, 2]])
q = np.array([-2, -6])
A = np.array([[1, 1], [-1, 2], [2, 1]])
l = np.array([-np.inf, -np.inf, -np.inf])
u = np.array([2, 2, 3])
prob = osqp.OSQP()
Q = sparse.csc_matrix(Q)
A = sparse.csc_matrix(A)
prob.setup(Q, p, A, l, u)
res = prob.solve()
if res.info.status != 'solved':
raise ValueError('OSQP did not solve the problem!')
print(res.x)
[0.66650027 1.33321806]
具有线性等式约束的二次规划
目标函数:
约束条件:
OSQP求解二次规划:
代码语言：javascript复制import numpy as np
from scipy import sparse
import osqp
Q = np.array([[1, -1], [-1, 2]])
p = np.array([-2, -6])
A = np.array([1, 1])
u = np.array([0])
l = np.array([0])
prob = osqp.OSQP()
Q = sparse.csc_matrix(Q)
A = sparse.csc_matrix(A)
prob.setup(Q, p, A, l, u)
res = prob.solve()
if res.info.status != 'solved':
raise ValueError('OSQP did not solve the problem!')
print(res.x)
[-0.8000016 0.80000272]
2、轨迹跟踪MPC问题求解
2.1 生成直线参考轨迹
直线轨迹以参数方程的形式给出：
其中
是期望的纵向速度,
是车辆横摆角。
生成直线参考轨迹：
代码语言：javascript复制v_d = 5.0
dt = 0.05
sim_steps = 2000
def load_ref_traj():
ref_traj = np.zeros((sim_steps, 5))
for i in range(sim_steps):
ref_traj[i, 0] = v_d * i * dt
ref_traj[i, 1] = 5.0
ref_traj[i, 2] = 0.0
ref_traj[i, 3] = v_d
ref_traj[i, 4] = 0.0
return ref_traj
2.2 实现简单的仿真环境
基于车辆运动学模型实现一个简单的仿真环境。代码来源【1】,有改动。
代码语言：javascript复制
class UGV_model:
def __init__(self, x0, y0, theta0, L, T): # L:wheel base
self.x = x0 # X
self.y = y0 # Y
self.theta = theta0 # headding
self.l = L # wheel base
self.dt = T # decision time periodic
def update(self, vt, deltat): # update ugv's state
self.v = vt
dx = self.v * np.cos(self.theta)
dy = self.v * np.sin(self.theta)
dtheta = self.v * np.tan(deltat) / self.l
self.x = dx * self.dt
self.y = dy * self.dt
self.theta = dtheta * self.dt
def plot_duration(self):
plt.scatter(self.x, self.y, color='r')
plt.axis([-5, 100, -6, 6])
plt.pause(0.008)
2.3 实现MPC控制器
详细的理论推导部分见自动驾驶轨迹跟踪-模型预测控制(MPC)
2.3.1 寻找参考轨迹上的最近点
代码语言：javascript复制tree = KDTree(ref_traj[:, :2])
nearest_ref_info = tree.query(x[:2])
nearest_ref_x = ref_traj[nearest_ref_info[1]]
2.3.2 目标函数二次型:
与标准的二次型一一对应：
得到:
分别计算H矩阵和F矩阵。
代码语言：javascript复制a = np.array([
[1.0, 0, -nearest_ref_x[3] * math.sin(nearest_ref_x[2]) * self.T],
[0, 1.0, nearest_ref_x[3] * math.cos(nearest_ref_x[2]) * self.T],
[0, 0, 1]
])
b = np.array([
[math.cos(nearest_ref_x[2]) * self.T, 0],
[math.sin(nearest_ref_x[2]) * self.T, 0],
[math.tan(nearest_ref_x[4]) * self.T / self.L, nearest_ref_x[3] * self.T / (self.L * math.pow(math.cos(nearest_ref_x[4]), 2.0))]
])
A = np.zeros([self.Nx self.Nu, self.Nx self.Nu])
A[0 : self.Nx, 0 : self.Nx] = a
A[0 : self.Nx, self.Nx : ] = b
A[self.Nx :, self.Nx :] = np.eye(self.Nu)
B = np.zeros([self.Nx self.Nu, self.Nu])
B[0 : self.Nx, :] = b
B[self.Nx :, : ] = np.eye(self.Nu)
C = np.array([
[1, 0, 0, 0, 0],
[0, 1, 0 ,0 ,0],
[0, 0, 1, 0, 0]
])
theta = np.zeros([self.Np * self.Nx, self.Nc * self.Nu])
phi = np.zeros([self.Np * self.Nx, self.Nu self.Nx])
tmp = C
for i in range(1, self.Np 1):
phi[self.Nx * (i - 1) : self.Nx * i] = np.dot(tmp, A)
tmp_c = np.zeros([self.Nx, self.Nc * self.Nu])
tmp_c[ :, 0 : self.Nu] = np.dot(tmp, B)
if i > 1:
tmp_c[ :, self.Nu :] = theta[self.Nx * (i - 2) : self.Nx * (i - 1), 0 : -self.Nu]
theta[self.Nx * (i - 1) : self.Nx * i, :] = tmp_c
tmp = np.dot(tmp, A)
Q = np.eye(self.Nx * self.Np)
R = 5.0 * np.eye(self.Nu * self.Nc)
rho = 10
H = np.zeros((self.Nu * self.Nc 1, self.Nu * self.Nc 1))
H[0 : self.Nu * self.Nc, 0 : self.Nu * self.Nc] = np.dot(np.dot(theta.transpose(), Q), theta) R
H[-1 : -1] = rho
kesi = np.zeros((self.Nx self.Nu, 1))
diff_x = x - nearest_ref_x[:3]
diff_x = diff_x.reshape(-1, 1)
kesi[: self.Nx, :] = diff_x
kesi[self.Nx :, :] = delta_u_pre.reshape(-1, 1)
F = np.zeros((1, self.Nu * self.Nc 1))
F_1 = 2 * np.dot(np.dot(np.dot(phi, kesi).transpose(), Q), theta)
F[ 0, 0 : self.Nu * self.Nc] = F_1
2.3.3 约束条件
约束条件采用控制量约束和控制增量约束，如下：
分别计算
和约束条件。
代码语言：javascript复制A_t = np.zeros((self.Nc, self.Nc))
for row in range(self.Nc):
for col in range(self.Nc):
if row >= col:
A_t[row, col] = 1.0
A_I = np.kron(A_t, np.eye(self.Nu))
A_cons = np.zeros((self.Nc * self.Nu, self.Nc * self.Nu 1))
A_cons[0 : self.Nc * self.Nu, 0 : self.Nc * self.Nu] = A_I
U_t = np.kron(np.ones((self.Nc, 1)), delta_u_pre.reshape(-1, 1))
umin = np.array([[-0.2], [-0.54]])
umax = np.array([[0.2], [0.332]])
LB = U_min - U_t
UB = U_max - U_t
delta_Umin = np.kron(np.ones((self.Nc, 1)), delta_umin)
delta_Umax = np.kron(np.ones((self.Nc, 1)), delta_umax)
delta_Umin = np.vstack((delta_Umin, [0]))
delta_Umax = np.vstack((delta_Umax, [10]))
A_1_cons = np.eye(self.Nc * self.Nu 1, self.Nc * self.Nu 1)
A_cons = np.vstack((A_cons, A_1_cons))
LB = np.vstack((LB, delta_Umin))
UB = np.vstack((UB, delta_Umax))
2.3.4 使用OSQP求解
代码语言：javascript复制# Create an OSQP object
prob = osqp.OSQP()
H = sparse.csc_matrix(H)
A_cons = sparse.csc_matrix(A_cons)
# Setup workspace
prob.setup(H, F.transpose(), A_cons, LB, UB)
res = prob.solve()
if res.info.status != 'solved':
raise ValueError('OSQP did not solve the problem!')
2.4 运行效果
参考轨迹的速度：3m/s
参考轨迹的速度：5m/s
参考轨迹的速度：10m/s
