精选【自动驾驶】模型预测控制完成轨迹跟踪任务

Spgroc

个人专栏：==顶级==

发布时间 2023.07.06阅读数 2088 评论数 4

MPC（Model Predictive Control，模型预测控制）是一种先进的控制算法，广泛应用于工业控制和自动化领域。它基于数学模型对系统进行预测，并在每个控制周期内通过优化算法选择最优控制输入，以实现系统的稳定和最优性。

最优控制的基本概念：

最优控制（optimal control）是指在给定约束条件下，通过调整系统的控制参数，使系统在某种性能指标下达到最佳状态的问题。最优控制通常需要在整个时间域上进行优化，以达到最佳性能，当存在扰动的情况下，当前时刻的最优比不一定是下一时刻的最优值。最优控制在实际应用中有很广泛的应用，例如在自动驾驶汽车中，通过优化车辆的加速度和转向角度，使其在规定的时间内安全地到达目的地；在制造业中，通过调整生产线的控制参数，实现生产效率的最大化；在经济学中，通过优化投资组合，实现最大的资产回报等。

轨迹跟踪中的最优控制：

单输入单输出系统：

跟踪误差为e,控制输入为u，建立目标函数

控制目标是调节控制器输出u，使目标函数J最小，其中q,r是可调参数，表示误差及输入的权重系数，其值越大，对应指标优化的越好

多输入多输出系统：

目标函数为：

参考信号为：

得到误差为：

带入目标函数得到：

可以根据具体需求，调节参数 $q_1、q_2、r_1、r_2$

模型预测控制：

MPC利用一个已有的模型、系统当前的状态和未来的控制量，来预测系统未来的输出，然后与我们期望的系统输出做比较，得到一个损失函数（代价函数）[参考博客2]，即：

损 失 函 数 未 来 输 出 模 型 当 前 状 态 未 来 控 制 量 期 望 输 出

计算流程：

通过模型来预测系统在某一未来时间段内的表现来进优化控制。采用离散型状态空间表达式

估计/测量读取当前系统状态——可测量则测量，不可测量进行估计
基于 $u_k,u_k+1,......,u_{k+N}$ 做优化

代价函数：

$E_N^TFE_N$ 是终端误差，表示最终代价（预测时间最后一步的误差的代价）
只取当前控制输出 $u_k$ ，预测的模型很难精准的描述系统，同时可能存在扰动，故采用滚动优化。

滚动优化的核心思想是，在每个采样时刻，根据当前系统状态和测量信息，通过求解一个局部最优控制问题，得到当前时刻的最优控制策略。然后，在下一个采样时刻，重复这个优化过程。通过反复进行局部优化，控制器可以在实时环境中对系统的偏差进行校正，并应对各种复杂情况，以实现预期的控制性能。滚动优化的优点是它能够及时根据当前的系统状态和测量信息进行调整，因此具有较好的鲁棒性和适应性。它可以在实时控制中处理时变性质和扰动，同时允许控制器在每个时刻针对当前情况做出最优决策。与一次性离线优化相比，滚动优化可以更灵活地应对系统变化和不确定性。

MPC求解：

当系统模型是线性的时候，模型预测控制的设计和求解通常使用二次规划方法。

二次规划的一般形式：

对于离散系统的状态方程:

$u(k+i\mid k)$ 表示在k时刻预测控制器在k+i时刻的输出，i最大为N-1，即最大控制区间为N-1

$x(k+i\mid k)$ 表示在k时刻预测k+i时刻的系统状态，i最大为N，即最大预测区间为N

定义向量：

假设：

系 统 输 出 ： 参 考 ：

误差：

代价函数为：

其中： $x(k+i\mid k)^TQx(k+i\mid k)$ 为误差的加权和， $u(k+i\mid k)^TRu(k+i\mid k)$ 为输入的加权和， $x(k+N\mid k)^TFx(k+N\mid k)$ 为k+N时刻的误差，即终端误差。由于 $x(k+N\mid k)$ 由 $u(k+N-1\mid k)$ 决定，因此不需要 $u(k+N\mid k)$ ，即不存在终端输入项。

此时的代价函数不符合二次规划问题代价函数的形式，J 的表达式中有两个变量（输入u和状态x），要将系统状态量x消除，留下控制量u。消除x的数学推导：

整理可得：

令：

最终有：

将代价函数展开：

其中

将(14)带入得到：

由于 $x_k^TM^T\bar{Q}CU_k$ 与 $U_k^TC^T\bar{Q}Mx_k$ 互为转置且数值相等，那么：

令：

最终得到二次规划形式的代价函数：

在代价函数设定完成后，添加系统约束，即可进行二次规划求解。

MPC完成轨迹跟踪问题：

汽车运动学模型：

其中： $v$ 为汽车的速度， $\dot{x}$ 为汽车在X轴方向上的分速度， $\dot{y}$ 为汽车在Y轴方向上的分速度， $\phi$ 为汽车航向角， $\delta$ 为汽车前轮转向角，L为车辆的轴距，写成离散的形式为：

系统的输入为 $u=[v,\delta]$ ，状态为 $[x_k,y_k,v_k,\phi_k]$ ，写成线性模型离散系统为：

状态转移矩阵 A:

输入矩阵 B:

常数矩阵 C:

其中，P.dt 表示时间步的时间间隔，P.WB 是车辆的轴距，v 是速度， $\phi$ 是车辆的航向角， $\delta$ 是转向角。cos 和 sin 表示余弦和正弦函数。

以 $u=[v,\delta]$ 表示系统输入，以 $z=[x,y,v,\phi]$ 表示系统状态， $z_{ref}=[x,y,v,\phi]$ 表示参考轨迹，那么跟踪误差为 $z_{ref}-z$ ,将跟踪问题写成二次规划形式：

最小化目标函数：

其中，

$u_t$ 是系统的输入向量， $R$ 是输入的权重矩阵。
$z_t$ 是系统的状态向量， $Q$ 是状态误差的权重矩阵。
$z_{\text{ref}, t}$ 是参考轨迹的状态向量。
$z_{\text{ref}, T}$ 是最终时间步的参考轨迹状态向量。
$Q_f$ 是最终时间步状态误差的权重矩阵。

满足约束条件：

其中，

$A$ , $B$ , $C$ 是根据系统的离散模型计算得到的状态转移矩阵和输入矩阵。
$z_0$ 是初始状态向量。
$\text{speed}_{\text{max}}$ , $\text{speed}_{\text{min}}$ 是速度的上下限。
$\text{acceleration}_{\text{max}}$ 是加速度的最大限制。
$\text{steer}_{\text{max}}$ 是转向角的最大限制。

程序实现：

画小车


def draw_car(x, y, yaw, steer, C, color='black'):
    # 车辆的位置(x, y)、航向角yaw、前轮转角steer以及一些车辆参数C作为输入
    # 定义车辆的轮廓和车轮的形状
    car = np.array([[-C.RB, -C.RB, C.RF, C.RF, -C.RB],
                    [C.W / 2, -C.W / 2, -C.W / 2, C.W / 2, C.W / 2]])

    wheel = np.array([[-C.TR, -C.TR, C.TR, C.TR, -C.TR],
                      [C.TW / 4, -C.TW / 4, -C.TW / 4, C.TW / 4, C.TW / 4]])
    # 复制车轮的形状
    rlWheel = wheel.copy()
    rrWheel = wheel.copy()
    frWheel = wheel.copy()
    flWheel = wheel.copy()
    # 定义旋转矩阵
    Rot1 = np.array([[math.cos(yaw), -math.sin(yaw)],
                     [math.sin(yaw), math.cos(yaw)]])

    Rot2 = np.array([[math.cos(steer), math.sin(steer)],
                     [-math.sin(steer), math.cos(steer)]])
    # 对前轮形状进行旋转
    frWheel = np.dot(Rot2, frWheel)
    flWheel = np.dot(Rot2, flWheel)
    # 调整前轮位置
    frWheel += np.array([[C.WB], [-C.WD / 2]])
    flWheel += np.array([[C.WB], [C.WD / 2]])
    # 调整后轮位置
    rrWheel[1, :] -= C.WD / 2
    rlWheel[1, :] += C.WD / 2
    # 对轮廓和车轮形状进行旋转
    frWheel = np.dot(Rot1, frWheel)
    flWheel = np.dot(Rot1, flWheel)
    rrWheel = np.dot(Rot1, rrWheel)
    rlWheel = np.dot(Rot1, rlWheel)
    car = np.dot(Rot1, car)
    # 平移车辆和车轮的位置
    frWheel += np.array([[x], [y]])
    flWheel += np.array([[x], [y]])
    rrWheel += np.array([[x], [y]])
    rlWheel += np.array([[x], [y]])
    car += np.array([[x], [y]])
    # 绘制车辆形状和车轮
    plt.plot(car[0, :], car[1, :], color)


    if steer > 0:
        plt.plot(frWheel[0, :], frWheel[1, :], "red")
        plt.plot(flWheel[0, :], flWheel[1, :], "black")
        plt.fill(frWheel[0, :], frWheel[1, :], color="yellow", alpha=0.5)
    elif steer < 0:
        plt.plot(flWheel[0, :], flWheel[1, :], "red")
        plt.plot(frWheel[0, :], frWheel[1, :], "black")
        plt.fill(flWheel[0, :], flWheel[1, :], color="yellow", alpha=0.5)
    else:
        plt.plot(flWheel[0, :], flWheel[1, :], "black")
        plt.plot(frWheel[0, :], frWheel[1, :], "black")


    plt.plot(rrWheel[0, :], rrWheel[1, :], "blue")
    plt.plot(rlWheel[0, :], rlWheel[1, :], "blue")
    # 绘制方向箭头
    Arrow(x, y, yaw, C.WB * 0.6, color)

参考轨迹

ax = [0.0, 15.0, 30.0, 45.0, 60.0, 45.0, 30.0, 15.0]
ay = [0.0, 10.0, 0.0, -10.0, 0.0, 10.0, 0.0, -10.0]
cx, cy, cyaw, ck, s = cs.calc_spline_course(
    ax, ay, ds=P.d_dist)  # 返回路径点、航向角、曲率和s值的列表

def calc_spline_course(x, y, ds=0.1):
    sp = Spline2D(x, y)  # 使用给定的x和y坐标创建Spline2D对象
    s = np.arange(0, sp.s[-1], ds)  # 根据给定的步长ds生成s值的数组

    rx, ry, ryaw, rk = [], [], [], []  # 初始化存储路径点、航向角、曲率和s值的列表
    for i_s in s:  # 遍历每个s值
        ix, iy = sp.calc_position(i_s)  # 计算给定s值处的路径点坐标
        # 将路径点坐标添加到相应的列表中
        rx.append(ix)
        ry.append(iy)
        # 计算给定s值处的航向角
        ryaw.append(sp.calc_yaw(i_s))
        # 计算给定s值处的曲率
        rk.append(sp.calc_curvature(i_s))
        # 返回路径点、航向角、曲率和s值的列表
    return rx, ry, ryaw, rk, s

线性MPC求解

def linear_mpc_control(z_ref, z0, a_old, delta_old):  # 线性MPC控制
    """
    linear mpc controller
    :param z_ref: reference trajectory in T steps T步之内的参考轨迹
    :param z0: initial state vector 初始状态数组
    :param a_old: acceleration of T steps of last time T步之内上一时刻的加速度
    :param delta_old: delta of T steps of last time T步之内上一时刻的转向角度
    :return: acceleration and delta strategy based on current information 基于当前信息的加速度与转向角策略
    """

    if a_old is None or delta_old is None:  # 上一次信息为空，赋值为0
        a_old = [0.0] * P.T
        delta_old = [0.0] * P.T

    x, y, yaw, v = None, None, None, None  # 系统状态

    for k in range(P.iter_max):  # 开始迭代计算
        z_bar = predict_states_in_T_step(z0, a_old, delta_old, z_ref)  # 预测车辆状态
        # 将 a_old 和 delta_old 的值分别复制给 a_rec 和 delta_rec，并创建新的列表对象
        a_rec, delta_rec = a_old[:], delta_old[:]
        # 求解线性MPC
        a_old, delta_old, x, y, yaw, v = solve_linear_mpc(z_ref, z_bar, z0, delta_old)

        du_a_max = max([abs(ia - iao) for ia, iao in zip(a_old, a_rec)])
        du_d_max = max([abs(ide - ido) for ide, ido in zip(delta_old, delta_rec)])

        if max(du_a_max, du_d_max) < P.du_res:
            break

    return a_old, delta_old, x, y, yaw, v

def solve_linear_mpc(z_ref, z_bar, z0, d_bar):
    """
    求解二次规划问题
    solve the quadratic optimization problem using cvxpy, solver: OSQP
    :param z_ref: reference trajectory (desired trajectory: [x, y, v, yaw]) 参考轨迹
    :param z_bar: predicted states in T steps t步之内预测的状态
    :param z0: initial state 初始状态
    :param d_bar: delta_bar 转向角度
    :return: optimal acceleration and steering strategy 加速度与转向角度
    """

    z = cvxpy.Variable((P.NX, P.T + 1))  # 要优化的状态量
    u = cvxpy.Variable((P.NU, P.T))  # 系统输入量

    cost = 0.0  # 损失
    constrains = []  # 约束

    for t in range(P.T):  # 计算T步之内的数据
        cost += cvxpy.quad_form(u[:, t], P.R)  # 输入的损失
        cost += cvxpy.quad_form(z_ref[:, t] - z[:, t], P.Q)  # 参考轨迹误差的损失

        # 计算系统的离散模型，系统状态[x, y, v, yaw]，系统输入[a,delta]
        A, B, C = calc_linear_discrete_model(z_bar[2, t], z_bar[3, t], d_bar[t])

        constrains += [z[:, t + 1] == A @ z[:, t] + B @ u[:, t] + C]  # 将系统状态更新的约束条件添加到约束列表中

        if t < P.T - 1:  # 在最后一个时间步之前执行这段代码，保证t + 1时刻有效
            cost += cvxpy.quad_form(u[:, t + 1] - u[:, t], P.Rd)  # 输入损失
            constrains += [cvxpy.abs(u[1, t + 1] - u[1, t]) <= P.steer_change_max * P.dt]  # 保证系统输出在可控的变化范围内

    cost += cvxpy.quad_form(z_ref[:, P.T] - z[:, P.T], P.Qf)  # 计算最终时间步 P.T 时刻系统输出 z 和参考轨迹 z_ref 之间的二次型损失

    constrains += [z[:, 0] == z0]  # 初值
    constrains += [z[2, :] <= P.speed_max]  # 最大速度
    constrains += [z[2, :] >= P.speed_min]  # 最小速度
    constrains += [cvxpy.abs(u[0, :]) <= P.acceleration_max]  # 加速度约束
    constrains += [cvxpy.abs(u[1, :]) <= P.steer_max]  # 转向角约束

    cost = cvxpy.sum(cost)  # 将累加的表达式求和，得到标量值
    prob = cvxpy.Problem(cvxpy.Minimize(cost), constrains)  # 求解二次规划问题
    prob.solve(solver=cvxpy.OSQP)  # 使用OSQP求解方法

    a, delta, x, y, yaw, v = None, None, None, None, None, None  # 系统状态量

    if prob.status == cvxpy.OPTIMAL or \
            prob.status == cvxpy.OPTIMAL_INACCURATE:  # 二次规划问题成功求解
        # 状态量赋值
        x = z.value[0, :]
        y = z.value[1, :]
        v = z.value[2, :]
        yaw = z.value[3, :]
        # 转化后的数据列表
        a = u.value[0, :]  # 加速度，力
        delta = u.value[1, :]  # 转向角
    else:
        print("Cannot solve linear mpc!")

    return a, delta, x, y, yaw, v

总结

对比LQR与MPC控制：

LQR：对模型精度要求很高，要求线性系统，在无限长的时间域内做优化控制，只求解一次。

MPC：需要模型，线性和非线性系统均可，在每一步都需要进行一次最优化计算，根据预测的状态计算离散模型，会考虑系统的约束，对控制器计算能力要求较高。

参考

[DR_CAN讲解的MPC模型预测控制器]https://www.bilibili.com/video/BV1cL411n7KV
[模型预测控制(MPC)实现轨迹跟踪]https://blog.csdn.net/weixin_42301220/article/details/124566369

自动驾驶 MPC 轨迹跟踪

原创文章作者： Spgroc 。如若转载，请注明出处：古月居 https://www.guyuehome.com/43723

打赏 1

上一篇：【控制算法】ADRC自抗扰控制

下一篇：【机器人】PyBullet四足机器狗仿真

精选【自动驾驶】模型预测控制完成轨迹跟踪任务

Spgroc

最优控制的基本概念：

轨迹跟踪中的最优控制：

模型预测控制：

计算流程：

MPC求解：

MPC完成轨迹跟踪问题：

程序实现：

画小车

参考轨迹

线性MPC求解

总结

参考

为你推荐

KITTI结果领先地位！Progressive LiDAR Adaptation for Road Detection——PLARD算法

精选Intel深度摄像头RealSense D435（实感双目摄像头）和目标检测结合使用

经典文献阅读之--NeRF-SLAM（单目稠密重建）

精选三维目标检测之ROS可视化

精选Apollo control之PID算法

激光雷达点云操作的工程化分享（二）——PCA

评论（4）

关于作者

Spgroc

24

0

399

6

【自动驾驶】强化学习DDPG玩赛车游戏

精选【电机控制】FOC电机控制

【自动驾驶】Carsim无人车横向LQR控制仿真

相关推荐

Apollo planning之RuleBasedStopDecider

ROS2学习笔记（二）-- 多机通讯原理简介及配置方法

精选C++ 之Google benchmark测试与分析

无人驾驶小车调试笔记（五）-- 命令行通信

Apollo星火计划学习笔记——Control 专项讲解(PID)

自动驾驶路径跟踪控制——车辆动力学建模基本概念

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

TA的专栏

控制算法

强化学习

自动驾驶

机器人

专栏导航

谢谢大爷打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板