自动驾驶轨迹跟踪(一)-模型预测控制(MPC)代码实现

YoungTimes

发布于 2022-04-28 20:23:19

4.7K1

发布于 2022-04-28 20:23:19

文章被收录于专栏：半杯茶的小酒杯半杯茶的小酒杯

模型预测控制(MPC)的理论推导部分见前文: 自动驾驶轨迹跟踪-模型预测控制(MPC) 这里主要用Python代码实现前文的推导结论。

先展示一下最终的效果(蓝色是目标轨迹线，红色是车辆不断靠近目标轨迹线，并最终沿着目标轨迹线行驶)：

参考轨迹的速度：5m/s

1、二次规划问题求解

我们最终将优化的目标函数转化为二次规划问题，所以这里先了解下如何用OSQP求解二次规划问题(参考材料【2】)。

1.1 什么是二次规划

\text{min}_x = \frac{1}{2} x^T P x + q^T x

满足约束:

Ax \leq b

A_{eq} x = b_{eq}

lb \leq x \leq ub

1.2 一些例子

具有线性约束的二次规划

目标函数:

f(x) = \frac{1}{2} x_1^2 + x_2^2 - x_1 x_2 - 2 x_1 - 6 x_2

需要满足以下约束

x_1 + x_2 \leq 2

- x_1 + 2 x_2 \leq 2

2 x_1 + x_2 \leq 3

整理下目标函数:

f(x) = \frac{1}{2} \begin{bmatrix} x_1 & x_2 \\ \end{bmatrix} \begin{bmatrix} 1 & -1 \\ -1 & 2 \\ \end{bmatrix} \begin{bmatrix} x_1 \\ x_2 \\ \end{bmatrix} + \begin{bmatrix} -2 & -6 \\ \end{bmatrix} \begin{bmatrix} x_1 \\ x_2 \\ \end{bmatrix}

OSQP求解二次规划:

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]

具有线性等式约束的二次规划

目标函数:

约束条件:

x_1 + x_2 = 0

OSQP求解二次规划:

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 生成直线参考轨迹

直线轨迹以参数方程的形式给出：

\begin{cases} x(t) = v_d t \\ y(t) = 5 \\ \phi(t) = 0 \\ v(t) = v_d \\ \delta(t) = 0.0 \\ \end{cases}

其中

v_d

是期望的纵向速度,

\phi

是车辆横摆角。

生成直线参考轨迹：

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】,有改动。


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 寻找参考轨迹上的最近点

tree = KDTree(ref_traj[:, :2])
nearest_ref_info = tree.query(x[:2])
nearest_ref_x = ref_traj[nearest_ref_info[1]]

2.3.2 目标函数二次型:

J = \begin{bmatrix} \Delta u & \epsilon \end{bmatrix} \begin{bmatrix} \Theta^T Q \Theta & 0 \\ 0 & \rho \\ \end{bmatrix} \begin{bmatrix} \Delta u \\ \epsilon \\ \end{bmatrix} + \begin{bmatrix} 2 E^T Q \Theta & 0 \\ \end{bmatrix} \begin{bmatrix} \Delta u \\ \epsilon \\ \end{bmatrix}

与标准的二次型一一对应：

J = \frac{1}{2} x^T H x + f^T x

得到:

x = \begin{bmatrix} \Delta u \\ \epsilon \\ \end{bmatrix}

H = \begin{bmatrix} \Theta^T Q \Theta & 0 \\ 0 & \rho \\ \end{bmatrix}

F = \begin{bmatrix} 2 E^T Q \Theta & 0 \\ \end{bmatrix}

分别计算H矩阵和F矩阵。

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 约束条件

约束条件采用控制量约束和控制增量约束，如下：

\begin{bmatrix} A_I & 0 \\ -A_I & 0 \\ \end{bmatrix} \begin{bmatrix} \Delta U \\ \epsilon \\ \end{bmatrix} \leq \begin{bmatrix} U_{max} - U_t \\ -U_{min} + U_t \\ \end{bmatrix}

\Delta U_{min} \leq \Delta U_t \leq \Delta U_{max}

分别计算

A_I

和约束条件。

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求解

# 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

本文参与腾讯云自媒体同步曝光计划，分享自微信公众号。

原始发表：2022-04-10，如有侵权请联系 cloudcommunity@tencent.com 删除

自动驾驶

无人驾驶

本文分享自半杯茶的小酒杯微信公众号，前往查看

如有侵权，请联系 cloudcommunity@tencent.com 删除。

本文参与腾讯云自媒体同步曝光计划，欢迎热爱写作的你一起参与！

自动驾驶

无人驾驶

登录后参与评论

0 条评论

热度