车辆模型推导以及MPC控制实现

车辆预测模型（MPC）

在代码中采用如下进行实现：

def linear_mpc_control(xref, xbar, x0, dref):
    """
    linear mpc control

    xref: reference point参考点
    xbar: operational point当前点
    x0: initial state
    dref: reference steer angle
    """

    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], R)# u^T R u

        if t != 0:
            cost += cvxpy.quad_form(xref[:, t] - x[:, t], Q) # (xref-x)^T Q (xref-x)

        A, B, C = get_linear_model_matrix(
            xbar[2, t], xbar[3, t], dref[0, t])
        constraints += [x[:, t + 1] == A * x[:, t] + B * u[:, t] + C]

        if t < (T - 1):
            cost += cvxpy.quad_form(u[:, t + 1] - u[:, t], Rd) # u_{t-1}^T Rd u_{t-1}
            # 前轮方向角度小于最大角加速度×DT
            constraints += [cvxpy.abs(u[1, t + 1] - u[1, t])
                            <= MAX_DSTEER * DT]

    # 最后一个点
    cost += cvxpy.quad_form(xref[:, T] - x[:, T], Qf) # (xref-x)^T Qf (xref-x)

    constraints += [x[:, 0] == x0]
    constraints += [x[2, :] <= MAX_SPEED]
    constraints += [x[2, :] >= MIN_SPEED]
    constraints += [cvxpy.abs(u[0, :]) <= MAX_ACCEL]
    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:
        ox = get_nparray_from_matrix(x.value[0, :]) # 状态量x,
        oy = get_nparray_from_matrix(x.value[1, :]) # 状态量y
        ov = get_nparray_from_matrix(x.value[2, :]) # 状态量v
        oyaw = get_nparray_from_matrix(x.value[3, :]) # 状态量yaw
        oa = get_nparray_from_matrix(u.value[0, :]) # 控制量accel
        odelta = get_nparray_from_matrix(u.value[1, :]) # 控制量steer

    else:
        print("Error: Cannot solve mpc..")
        oa, odelta, ox, oy, oyaw, ov = None, None, None, None, None, None

    return oa, odelta, ox, oy, oyaw, ov