MPC控制器从入门到进阶（小车动态避障变道仿真 - Python）_mpc python

目录

• 第一章：最优化控制和基本概念
  • 1.1 引言：为什么需要MPC？
  • 1.2 最优化控制：找到\"最好\"的方案
  • 1.3 用数学表达驾驶行为
  • 1.4 定义\"好的驾驶\"
  • 1.5 模型预测控制：向前看的司机
• 第二章：最优化数学建模推导
  • 2.1 再谈\"向前看\"的控制策略
  • 2.2 为什么传统控制方法不够好？
  • 2.3 MPC的\"魔法\"：预测+优化
  • 2.4 MPC问题的\"食谱\"
  • 2.5 解决非线性问题：计算机如何\"思考\"最佳驾驶策略
  • 2.6 实际问题：计算量和实时性
  • 2.7 总结：MPC的魅力
• 第三章：一个详细的建模例子
  • 3.1 从零开始：构建我们的\"虚拟赛车手\"
  • 3.2 构建\"思考模型\"：车辆是如何运动的？
  • 3.3 定义\"好司机\"的标准：目标函数
  • 3.4 设定\"游戏规则\"：约束条件
  • 3.5 “一步一步来”：完整的MPC建模过程
  • 3.6 将模型转化为实际代码：以Python为例
  • 3.7 小结：从概念到代码
• 第四章：基于Python的MPC避障变道控制系统实现
  • 4.1 引言：从概念到实践
  • 4.2 开发环境准备
  • 4.3 基础系统架构设计
    • 4.3.1 系统参数定义
    • 4.3.2 定义系统模型
    • 4.3.3 障碍物模型与碰撞约束
  • 4.4 构建MPC优化问题
    • 4.4.1 目标函数设计
    • 4.4.2 约束条件设计
    • 4.4.3 配置求解器
  • 4.5 决策状态机设计
  • 4.6 仿真实现
    • 4.6.1 仿真循环设计
    • 4.6.2 结果可视化
  • 4.7 参数调优与性能分析
  • 4.8 小结与展望

第一章：最优化控制和基本概念

学习完本文，你将能够实现以下仿真效果：

• 本文区别于传统MPC教程（那种类似教科书的讲解，上来就是大段公式推导劝退小白）
• 本文主要用大白话讲解
• 要想进阶学习，强烈推荐 DR_CAN 在B站的教程【MPC模型预测控制器】1_最优化控制和基本概念
• 关于状态空间入门，MPC的基础知识，可参考往期文章：LQR 状态空间入门：以 “倒立摆” 仿真案例建模（ MPC 是 LQR 的泛化）

1.1 引言：为什么需要MPC？

想象你正在开车。作为一个老司机，你会怎么做？你会不断观察前方路况，预判接下来几秒钟的情况，并做出相应的控制决策——踩油门、刹车或转向。这实际上就是\"预测控制\"的基本思想。

模型预测控制（MPC）就像是一个\"会思考的司机\"，它不只看当前，还会\"向前看\"一段时间，规划出最佳的控制策略。与传统控制方法相比，MPC最大的特点就是\"有预见性\"，这使它特别适合处理复杂的控制问题，比如自动驾驶中的避障变道。

别担心如果你对控制理论不太熟悉！我们会用简单直观的方式来解释这些概念，循序渐进。

1.2 最优化控制：找到\"最好\"的方案

\"最优化控制\"听起来很高大上，但其实它就是在一堆可能的方案中，找到最符合我们期望的那个。就像是你在导航软件中，可能会有多条路线可选，但你会根据自己的需求（最快到达、最省油或最少收费）来选择最佳路线。

以自动驾驶汽车变道为例。我们有两种变道方案：

• 方案A：缓慢平滑地变道，像优雅的绅士一样
• 方案B：快速切到旁边车道，像赛车手一样激进
  
  哪个方案\"更好\"呢？这取决于情境：
• 如果你是正常驾驶，没有障碍物，可能更喜欢平滑舒适的方案A
• 如果你是在紧急避险，可能需要快速反应的方案B
  
  这就是\"最优\"的相对性——没有绝对的最好，只有最适合当前需求的方案。最优化控制就是让计算机根据我们的需求自动找到这个\"最适合\"的方案。

1.3 用数学表达驾驶行为

虽然数学看起来有些枯燥，但它能精确描述我们的控制问题。别担心，我们会用开车的例子来解释这些公式。

想象你开车时的情况。车辆的状态可以用几个关键变量描述：

• 横向位置（车道上的左右位置）
• 纵向位置（车辆前进的距离）
• 方向角（车头朝向）
• 车速

作为驾驶员，你的控制动作包括：

• 踩油门或刹车（控制加速度）
• 转动方向盘（控制转向角）

这些变量之间的关系可以用数学模型来描述：

横向位置变化 = 车速 × sin(方向角) × 时间间隔纵向位置变化 = 车速 × cos(方向角) × 时间间隔方向角变化 = (车速/轴距) × tan(转向角) × 时间间隔车速变化 = 加速度 × 时间间隔

这个模型描述了\"如果我这样踩油门和打方向盘，车会怎么走\"。虽然看起来有点复杂，但实际上就是把我们开车的经验用数学语言表达出来了。

1.4 定义\"好的驾驶\"

什么是好的驾驶呢？通常我们关心几个方面：

1. 准确性：能否精确地跟随预定路线
2. 舒适性：避免急加速、急刹车或急转弯
3. 安全性：与其他车辆和障碍物保持安全距离

用数学语言，我们可以把这些期望总结为一个\"得分函数\"（也叫目标函数）：

总得分 = 路线偏离的惩罚 + 操作不平稳的惩罚 + 安全风险的惩罚

具体来说，对于路线跟踪问题：

路线偏离的惩罚 = Q1×(横向位置偏差)² + Q2×(纵向位置偏差)² + Q3×(方向角偏差)² + Q4×(速度偏差)²操作不平稳的惩罚 = R1×(加速度)² + R2×(转向角)²

其中Q和R是权重系数，用来调整各项的重要性：

• Q1, Q2大：更注重精确跟随预定路线
• Q4大：更注重保持目标速度
• R1, R2大：更注重操作平稳，避免急加速和急转弯

自动驾驶的目标就是找到一系列控制动作（踩油门/刹车和转向），使这个总得分尽可能低。

1.5 模型预测控制：向前看的司机

现在，让我们把上面的理念组合起来，理解什么是模型预测控制（MPC）。

想象一下，你是一个正在高速公路上行驶的司机。你会怎么做？

1. 观察当前状态：我在哪个车道？当前速度是多少？
2. 预测前方情况：如果我保持当前速度和方向，未来几秒内会发生什么？
3. 评估多种方案：如果我变道呢？如果我减速呢？
4. 选择最优方案：哪个方案能最安全、最舒适地到达目的地？
5. 执行并重复：采取行动，然后在下一刻重新评估

MPC正是按照这个思路工作的智能控制算法：

1. 获取当前状态：
   通过传感器测量车辆当前的位置、速度、方向等信息。

2. 预测未来可能的状态：
   使用车辆运动模型，预测在不同控制动作下（不同的加速度和转向角），未来几秒内车辆可能的运动轨迹。

   例如：如果未来5秒内，我们打算这样踩油门和转向，车辆会沿着这条轨迹运动。

3. 优化控制方案：
   计算哪种控制方案能使总体目标函数（前面提到的\"得分\"）最小。

   MPC会同时考虑：

   • 轨迹跟踪的准确性
   • 控制动作的平稳性
   • 各种安全约束（例如不超速、不偏离车道）

4. 滚动执行：
   只执行优化方案的第一步，然后在下一个控制周期重新开始整个过程。

   这就像是司机不断调整方向盘和油门，而不是一次性决定整个行程的控制。

MPC的独特之处在于它能够：

• 看得远：考虑未来一段时间内的行为
• 考虑全面：同时兼顾多个目标
• 适应变化：随时调整控制策略
• 处理约束：明确考虑车辆和环境的物理限制

这种\"向前看\"的控制策略，使MPC特别适合处理自动驾驶这类复杂问题，尤其是需要规划轨迹并避开障碍物的场景。

接下来，我们将深入了解如何构建MPC控制器的数学模型，别担心，我们会继续用直观的例子来解释这些概念！

第二章：最优化数学建模推导

2.1 再谈\"向前看\"的控制策略

在第一章中，我们把MPC比喻成一个\"向前看\"的司机。现在，让我们更深入地了解这个司机是如何\"思考\"的。

假设你正在驾车，前方有一个障碍物。你的大脑会自动进行一系列计算：

1. 我现在在哪里？速度多快？
2. 如果我这样操作方向盘和油门，几秒后会到哪里？
3. 哪种操作方式最安全舒适？

MPC控制器的工作方式与此类似，但更加系统化和精确。接下来，我们将用更通俗的语言解释这个过程。

2.2 为什么传统控制方法不够好？

在了解MPC的细节之前，我们先思考为什么需要这么复杂的控制方法？传统的控制方法（如PID控制）有什么不足？

想象你开车跟随前车。传统的\"跟车控制\"大致是这样工作的：

• 如果与前车距离太近，就踩刹车
• 如果与前车距离太远，就踩油门
• 控制力度与距离误差成正比

这种方法简单直接，但有几个明显缺点：

1. 没有预见性：只看当前状态，不考虑未来
2. 难以处理约束：比如最大速度、加速度限制
3. 不易优化多目标：难以同时兼顾安全、舒适和效率

这就像是一个只盯着前车尾灯的新手司机，而MPC则像是能够看到远处路况的老司机，可以提前规划和调整。

2.3 MPC的\"魔法\"：预测+优化

MPC的核心思想可以概括为两个关键词：预测和优化。

预测：使用车辆模型，根据当前状态和控制动作，预测未来几秒内车辆可能的行驶轨迹。

想象你手里有一个汽车模拟器，可以快速模拟\"如果我这样踩油门和打方向盘，车会怎么走\"。MPC就内置了这样一个模拟器，不过是用数学方程而不是3D图形

这个\"模拟器\"基于车辆的物理模型：

下一时刻的前进距离 = 当前位置 + 当前速度×时间间隔×方向角的余弦值下一时刻的横向位置 = 当前横向位置 + 当前速度×时间间隔×方向角的正弦值下一时刻的方向角 = 当前方向角 + (当前速度/轴距)×时间间隔×转向角的正切值下一时刻的速度 = 当前速度 + 加速度×时间间隔

注意这个模型是非线性的——转向角对车辆运动的影响不是简单的加减乘除关系。这就是为什么我们需要特殊的非线性优化方法。

优化：在众多可能的控制方案中，找到能使目标函数（舒适性、安全性、效率的综合评分）最优的那一个。

这就像是在众多可能的驾驶操作中，找到最\"完美\"的那个操作序列。

2.4 MPC问题的\"食谱\"

如果把MPC问题比喻成一道菜，那么其\"食谱\"包含以下核心元素：

1. 状态和控制变量：

   • 状态变量：描述车辆情况（位置、速度、方向等）
   • 控制变量：驾驶员的操作（加速度、转向角）

2. 预测模型：
   告诉我们\"如果执行这些控制操作，车辆将如何运动\"。就像是一个简化的物理引擎。

3. 目标函数：
   评价\"驾驶质量\"的打分标准，通常包括：

   • 路径跟踪精度（车辆是否按预定路线行驶）
   • 控制平滑度（操作是否平稳舒适）
   • 终点状态（最终是否到达目标位置和速度）

4. 约束条件：

   • 车辆物理约束：速度不能超过限制，转向角不能过大
   • 道路边界约束：车辆必须在道路范围内
   • 安全约束：与障碍物保持安全距离

2.5 解决非线性问题：计算机如何\"思考\"最佳驾驶策略

现在到了关键问题：计算机如何找到最佳的控制序列？

传统的优化问题往往是线性的或二次型的，可以用标准方法高效求解。但车辆控制是非线性问题——转向角对位置的影响不是简单的线性关系。这就像是一个复杂的导航问题，不能用简单的直线距离来计算。

有两种主要方法来处理这种非线性问题：

1. 线性化方法：
   我们可以在当前工作点附近，将非线性模型近似为线性模型。这就像是用很多小的直线段来近似一条曲线。

   例如，我们可以将模型简化为：

   下一状态 ≈ 当前状态 + 状态变化率×控制输入

   这种方法计算快速，但在大幅度转向等情况下可能不够准确。

2. 直接非线性优化：
   现代计算机已经强大到可以直接处理非线性优化问题。使用专业的优化工具（如IPOPT）和自动微分技术，可以高效地求解这类问题。

   这就像是用一个强大的导航系统，可以考虑道路的弯曲、坡度等复杂因素，而不仅仅是直线距离。

实际应用中，MPC控制器会在几十毫秒内完成一次优化计算，找到从当前状态到目标状态的最佳控制序列。

2.6 实际问题：计算量和实时性

在理想世界中，我们可以考虑无限远的未来，评估无数种可能的控制策略。但在现实中，计算资源是有限的，特别是在需要实时响应的自动驾驶场景。

为了保证MPC的实时性，工程师们采用了一系列\"聪明的偷懒\"方法：

1. 有限预测时域：
   只看未来几秒（而不是整个旅程），这就像是开车时只关注前方100米的路况，而非整条路线。

2. 控制序列简化：
   可以假设后半段控制是恒定的，减少优化变量。

3. 热启动：
   使用上一步的优化结果作为当前步骤的初始猜测，加速收敛。这就像是基于之前的驾驶经验来调整当前的驾驶策略。

4. 并行计算：
   利用现代计算机的多核特性，并行处理不同方面的计算任务。

2.7 总结：MPC的魅力

MPC控制器的魅力在于它能够像一个富有经验的司机一样\"思考\"和\"规划\"：

1. 预见性：通过模型预测未来可能的状态
2. 全局优化：综合考虑多种因素，找到整体最优方案
3. 约束处理：能够明确处理速度限制、道路边界等约束
4. 适应性：不断重新规划，适应变化的环境

在下一章中，我们将通过一个具体的例子，一步步展示如何构建和实现一个用于车辆避障变道的MPC控制器。别担心数学公式，我们会用通俗易懂的语言和生动的比喻来解释核心概念！

第三章：一个详细的建模例子

3.1 从零开始：构建我们的\"虚拟赛车手\"

在前两章中，我们学习了MPC的基本概念和工作原理。现在，让我们动手构建一个实际的MPC控制器，就像是打造一个\"虚拟赛车手\"，能够自动驾驶车辆完成避障变道任务。

首先，我们需要明确这个\"虚拟赛车手\"需要知道什么信息，能做什么操作：

状态信息（赛车手能看到的）：

• 车辆的位置坐标（x,y）
• 车辆的航向角（ψ）
• 车辆的速度（v）

控制动作（赛车手能操作的）：

• 加速度（a）：踩油门或刹车
• 转向角（δ）：方向盘转角

这两组变量，分别构成了我们的状态向量和控制向量。

3.2 构建\"思考模型\"：车辆是如何运动的？

我们的\"虚拟赛车手\"需要有一个\"思考模型\"——了解车辆在不同控制动作下将如何运动。这就是车辆的数学模型。

想象一辆自行车：前轮可以转向，后轮固定。这种简化的\"自行车模型\"足以描述大多数车辆的基本运动特性：

下一时刻的x位置 = 当前x位置 + 当前速度 × 时间间隔 × cos(航向角)下一时刻的y位置 = 当前y位置 + 当前速度 × 时间间隔 × sin(航向角)下一时刻的航向角 = 当前航向角 + (当前速度/轴距) × 时间间隔 × tan(转向角)下一时刻的速度 = 当前速度 + 加速度 × 时间间隔

如果你学过一点物理，会发现这就是基本的运动学方程。如果没学过也没关系，你可以把它理解为：

• 车辆会沿着航向角方向前进（x和y方程）
• 转向时，航向角会根据转向角度和车速变化（航向角方程）
• 加速或刹车会改变车速（速度方程）

3.3 定义\"好司机\"的标准：目标函数

现在，我们需要定义什么是\"好的驾驶\"。在变道避障任务中，我们通常关心：

1. 轨迹跟踪：车辆应该尽量靠近目标车道
2. 速度控制：车辆应该保持适当的巡航速度
3. 平稳操作：避免急转弯和急刹车
4. 安全性：与障碍物保持安全距离

这些期望可以转化为数学上的\"目标函数\"。想象我们给司机的驾驶表现打分：

总分 = 轨迹跟踪分数 + 速度控制分数 + 操作平稳分数 + 安全分数

具体来说：

轨迹跟踪分数 = -Q1×(当前y位置-目标y位置)² // 负号表示\"越小越好\"速度控制分数 = -Q2×(当前速度-目标速度)²操作平稳分数 = -R1×(加速度)² - R2×(转向角)²安全分数 = -大数×(如果接近障碍物)

其中Q1、Q2、R1、R2是权重系数，用来调整各项指标的重要性：

• Q1大：更注重精确跟踪车道
• Q2大：更注重保持目标速度
• R1大：更注重加速平稳
• R2大：更注重转向平稳

3.4 设定\"游戏规则\"：约束条件

除了追求高分，我们的\"虚拟赛车手\"还必须遵守一些\"游戏规则\"，这些在数学上称为\"约束条件\"：

1. 物理约束：

   速度范围：0 ≤ 速度 ≤ 20 m/s加速度范围：-5 m/s² ≤ 加速度 ≤ 3 m/s²转向角范围：-30° ≤ 转向角 ≤ 30°

2. 道路约束：

   车辆必须在道路范围内：道路左边界 ≤ y位置 ≤ 道路右边界

3. 安全约束：

   与障碍物距离必须大于安全距离：距离 ≥ 安全距离

这些约束条件就像是\"红线\"，不允许被踩踏。在数学优化中，这些约束条件会限制可行解的范围。

3.5 “一步一步来”：完整的MPC建模过程

让我们梳理一下构建MPC控制器的完整过程，就像是制作一道精美菜肴的步骤：

1. 定义问题参数：

   # 时间参数Ts = 0.1 # 控制周期（秒）N = 10 # 预测步数# 车辆参数L = 2.5 # 轴距（米）# 约束参数v_min, v_max = 0, 20  # 速度范围（米/秒）a_min, a_max = -5, 3  # 加速度范围（米/秒²）delta_min, delta_max = -30, 30 # 转向角范围（度，需转换为弧度）# 目标参数lane1_y = 0 # 第一车道中心线y坐标lane2_y = 3.5 # 第二车道中心线y坐标target_v = 10 # 目标速度（米/秒）

2. 构建车辆模型：

   # 状态方程（简化表示）def vehicle_model(x, y, psi, v, a, delta, Ts, L): next_x = x + Ts * v * cos(psi) next_y = y + Ts * v * sin(psi) next_psi = psi + Ts * (v / L) * tan(delta) next_v = v + Ts * a return next_x, next_y, next_psi, next_v

3. 定义目标函数：

   def objective_function(states, controls, target_y, target_v): total_cost = 0 for i in range(N): # 轨迹跟踪代价 y_error = states[1, i] - target_y total_cost += Q1 * y_error**2 # 速度跟踪代价 v_error = states[3, i] - target_v total_cost += Q2 * v_error**2 # 控制平滑代价 total_cost += R1 * controls[0, i]**2 # 加速度惩罚 total_cost += R2 * controls[1, i]**2 # 转向角惩罚 return total_cost

4. 设置约束条件：

   # 状态约束for i in range(N+1): # 速度约束 v_min <= states[3, i] <= v_max # 道路边界约束 road_left <= states[1, i] <= road_right# 控制约束for i in range(N): # 加速度约束 a_min <= controls[0, i] <= a_max # 转向角约束 delta_min <= controls[1, i] <= delta_max# 障碍物避让约束（如果存在障碍物）for i in range(N+1): distance_to_obstacle(states[:, i]) >= safe_distance

5. 构建优化问题：

   # 决策变量states = 变量(4, N+1) # [x, y, psi, v] 在未来N+1个时刻的值controls = 变量(2, N) # [a, delta] 在未来N个时刻的值# 目标函数minimize(objective_function(states, controls, target_y, target_v))# 约束条件subject_to: # 初始状态约束 states[:, 0] = 当前状态 # 系统动力学约束 for i in range(N): states[:, i+1] = vehicle_model(states[:, i], controls[:, i]) # 状态和控制约束 ... (如第4步所述)

3.6 将模型转化为实际代码：以Python为例

在实际应用中，我们通常使用专业的优化库来构建和求解MPC问题。以Python和CasADi库为例：

import numpy as npimport casadi as ca# 定义问题参数Ts = 0.1 # 采样时间（秒）N = 10 # 预测步数L = 2.5 # 车辆轴距（米）# 定义状态和控制变量x = ca.SX.sym(\'x\')y = ca.SX.sym(\'y\')psi = ca.SX.sym(\'psi\')v = ca.SX.sym(\'v\')states = ca.vertcat(x, y, psi, v)n_states = states.numel()a = ca.SX.sym(\'a\')delta = ca.SX.sym(\'delta\')controls = ca.vertcat(a, delta)n_controls = controls.numel()# 定义系统模型rhs = ca.vertcat( v * ca.cos(psi), v * ca.sin(psi), (v / L) * ca.tan(delta), a)f = ca.Function(\'f\', [states, controls], [states + Ts * rhs])# 创建优化问题opti = ca.Opti()X = opti.variable(n_states, N+1)U = opti.variable(n_controls, N)P = opti.parameter(n_states + 2) # [当前状态, 目标y, 目标速度]# 构建目标函数obj = 0Q = np.diag([0.1, 10, 0.1, 0.5]) # 状态权重R = np.diag([0.1, 1.0])  # 控制权重for k in range(N): # 计算状态误差 st_err = X[:, k] - ca.vertcat(X[0, k], P[n_states], 0, P[n_states+1]) obj += ca.mtimes([st_err.T, Q, st_err]) # 计算控制代价 obj += ca.mtimes([U[:, k].T, R, U[:, k]])# 终端代价st_err_N = X[:, N] - ca.vertcat(X[0, N], P[n_states], 0, P[n_states+1])obj += 10 * ca.mtimes([st_err_N.T, Q, st_err_N]) # 终端代价权重更大opti.minimize(obj)# 添加约束条件opti.subject_to(X[:, 0] == P[:n_states]) # 初始状态for k in range(N): opti.subject_to(X[:, k+1] == f(X[:, k], U[:, k])) # 系统动力学# 控制约束a_min, a_max = -5, 3 # m/s²delta_min, delta_max = -np.pi/6, np.pi/6 # ±30度opti.subject_to(opti.bounded(a_min, U[0, :], a_max))opti.subject_to(opti.bounded(delta_min, U[1, :], delta_max))# 状态约束v_min, v_max = 0, 20 # m/sy_min, y_max = -1.75, 5.25 # 道路边界（假设两车道，每车道3.5米宽）opti.subject_to(opti.bounded(v_min, X[3, :], v_max)) # 速度约束opti.subject_to(opti.bounded(y_min, X[1, :], y_max)) # 道路边界约束

这看起来有点复杂，但这就是一个基本的MPC控制器框架。在下一章中，我们会进一步讲解如何将这个框架应用到实际的变道控制问题中。

3.7 小结：从概念到代码

在本章中，我们以自动驾驶车辆变道为例，详细展示了如何构建一个MPC控制器：

1. 定义状态（车辆位置、方向、速度）和控制（加速度、转向角）变量
2. 构建车辆运动模型，描述状态如何随控制输入变化
3. 设计目标函数，定义什么是\"好的驾驶\"
4. 添加约束条件，限定可行的操作范围
5. 构建优化问题，将其转化为数学形式
6. 使用优化工具求解得到最优控制序列

你可能会觉得这些数学看起来很复杂，但实际上它们只是把我们开车时的思考过程用数学语言表达出来了。如果理解了基本概念，编写MPC控制器代码其实并不那么困难。

在下一章中，我们将基于本章构建的框架，实现一个完整的车辆变道MPC控制器，并通过仿真测试其性能。

第四章：基于Python的MPC避障变道控制系统实现

4.1 引言：从概念到实践

在前面几章中，我们学习了MPC的基本原理和建模方法。现在让我们动手实现一个完整的车辆避障变道控制系统，将理论知识应用到实际中。我们将使用Python和CasADi库来构建和求解MPC优化问题。

本章将实现一个智能避障变道控制系统，它能够根据环境中障碍物的位置自主决定何时变道、如何安全避障，并在超越障碍物后返回原车道。这个系统将展示MPC在处理复杂约束和多目标优化方面的强大能力。

与传统控制方法相比，MPC方法在这类场景中具有明显优势：

1. 预见性控制：能够提前预测和规划避障路径
2. 约束处理：自然地处理车辆物理限制和安全约束
3. 多目标优化：同时考虑安全性、舒适性和效率

4.2 开发环境准备

首先，我们需要安装和导入必要的Python库：

# 可以通过pip安装这些库# pip install casadi numpy matplotlibimport casadi as ca # 符号计算和优化import numpy as np # 数值计算import matplotlib.pyplot as plt # 可视化from matplotlib.patches import Rectangle # 绘制车辆和障碍物from matplotlib.transforms import Affine2D # 处理旋转变换from matplotlib.animation import FuncAnimation # 创建动画import time # 计时# 中文字体设置try: plt.rcParams[\'font.sans-serif\'] = [\'SimHei\'] # 指定显示中文的字体 plt.rcParams[\'axes.unicode_minus\'] = False # 解决负号显示问题except Exception as e: print(f\"中文字体设置警告: {e}\")

4.3 基础系统架构设计

4.3.1 系统参数定义

首先定义系统的各种参数：

# 时间参数Ts = 0.1 # 采样时间 (s)Np = 20 # 预测时域步数# 车辆参数L = 2.5 # 车辆轴距 (m)vehicle_length = 4.0 # 车辆长度 (m)vehicle_width = 1.8 # 车辆宽度 (m)# 约束参数v_max = 20.0 # 最大速度 (m/s)v_min = 0.0 # 最小速度 (m/s)a_max = 3.0 # 最大加速度 (m/s^2)a_min = -5.0 # 最大减速度 (m/s^2)delta_max = np.deg2rad(30) # 最大转向角 (rad)delta_min = -delta_max # 最小转向角 (rad)# 道路参数lane_width = 3.5 # 车道宽度 (m)lane1_y = 0.0 # 第一车道中心线 y 坐标lane2_y = lane_width # 第二车道中心线 y 坐标# 状态和控制维度nx = 4 # 状态量数量: [x, y, psi, v]nu = 2 # 控制量数量: [a, delta]# 权重参数 - 这些参数对MPC性能有重要影响Q = np.diag([1.0, 100.0, 0.5, 1.0]) # 状态权重 [x, y, psi, v]R = np.diag([0.1, 1000.0])  # 控制权重 [a, delta]P = Q * 0.01 # 终端权重# 航向角速度变化惩罚权重 - 使转向更平滑w_psi_dot_change = 18000.0# 目标速度target_v_val = 10.0 # 目标巡航速度 (m/s)

4.3.2 定义系统模型

使用CasADi定义车辆的运动学模型：

# 定义符号变量x_sym = ca.SX.sym(\'x\') # x位置y_sym = ca.SX.sym(\'y\') # y位置psi_sym = ca.SX.sym(\'psi\') # 航向角v_sym = ca.SX.sym(\'v\') # 速度states_sym = ca.vertcat(x_sym, y_sym, psi_sym, v_sym)a_sym = ca.SX.sym(\'a\') # 加速度delta_sym = ca.SX.sym(\'delta\') # 转向角controls_sym = ca.vertcat(a_sym, delta_sym)# 定义系统动力学方程 - 自行车模型rhs = ca.vertcat( v_sym * ca.cos(psi_sym),  # x方向速度 v_sym * ca.sin(psi_sym),  # y方向速度 (v_sym / L) * ca.tan(delta_sym), # 航向角变化率 a_sym # 速度变化率(加速度))# 创建离散时间模型函数 - 使用前向欧拉法f = ca.Function(\'f\', [states_sym, controls_sym], [states_sym + Ts * rhs])

4.3.3 障碍物模型与碰撞约束

为了实现避障功能，我们需要定义障碍物模型和碰撞约束：

# 障碍物参数obs_length = vehicle_length # 障碍物长度 (m)obs_width = vehicle_width # 障碍物宽度 (m)obs_x_initial = 35.0 # 障碍物初始x位置 (m)obs_y_fixed = lane1_y # 障碍物固定在第一车道obs_speed_x = 4.0 # 障碍物速度 (m/s)，低于车辆目标速度# 碰撞检测参数veh_r = vehicle_length / 2.5 # 车辆等效碰撞半径obs_r = obs_length / 2.0 # 障碍物等效碰撞半径safe_dist = 0.05  # 额外安全距离# 道路边界参数，考虑车辆宽度road_y_min = lane1_y - lane_width / 2 # 道路左边界road_y_max = lane2_y + lane_width / 2 # 道路右边界vehicle_y_min = road_y_min + vehicle_width / 2 # 车辆中心最小y坐标vehicle_y_max = road_y_max - vehicle_width / 2 # 车辆中心最大y坐标

4.4 构建MPC优化问题

4.4.1 目标函数设计

使用CasADi的Opti接口构建MPC优化问题，首先定义决策变量和参数：

# 创建优化器实例opti = ca.Opti()# 定义决策变量X = opti.variable(nx, Np + 1) # 状态变量，包括初始状态U = opti.variable(nu, Np) # 控制变量# 定义参数（当前状态和参考值）# [当前状态(4), 目标y位置(1), 目标速度(1), 障碍物x(1), 障碍物y(1), 上一时刻航向角速度(1)]P = opti.parameter(nx + 5)

然后构建目标函数，包括状态误差惩罚、控制输入惩罚和平滑性惩罚：

# 构建目标函数obj = 0# 状态追踪误差和控制输入惩罚for k in range(Np): # 参考状态（只关心y位置和速度跟踪，x方向自由前进） x_ref_k = ca.vertcat(X[0, k], P[nx], 0, P[nx+1]) # 状态误差代价 state_error = X[:, k] - x_ref_k obj += ca.mtimes([state_error.T, Q, state_error]) # 控制输入代价 obj += ca.mtimes([U[:, k].T, R, U[:, k]])# 终端代价x_ref_N = ca.vertcat(X[0, Np], P[nx], 0, P[nx+1])terminal_error = X[:, Np] - x_ref_Nobj += ca.mtimes([terminal_error.T, P, terminal_error])# 添加航向角速度变化惩罚，使转向更平滑psi_dot_list = []for k in range(Np): current_psi_dot = (X[3, k] / L) * ca.tan(U[1, k]) psi_dot_list.append(current_psi_dot)# 惩罚与上一控制周期航向角速度的变化obj += w_psi_dot_change * (psi_dot_list[0] - P[nx+4])**2# 惩罚预测时域内的航向角速度变化for k in range(1, Np): obj += w_psi_dot_change * (psi_dot_list[k] - psi_dot_list[k-1])**2# 设置优化目标opti.minimize(obj)

4.4.2 约束条件设计

添加系统的各种约束条件：

# 初始状态约束opti.subject_to(X[:, 0] == P[:nx])# 系统动力学约束for k in range(Np): opti.subject_to(X[:, k+1] == f(X[:, k], U[:, k]))# 控制输入约束opti.subject_to(opti.bounded(a_min, U[0, :], a_max)) # 加速度约束opti.subject_to(opti.bounded(delta_min, U[1, :], delta_max)) # 转向角约束# 状态约束opti.subject_to(opti.bounded(v_min, X[3, :], v_max)) # 速度约束# 道路边界约束opti.subject_to(opti.bounded(vehicle_y_min, X[1, :], vehicle_y_max))# 碰撞避免约束for k in range(1, Np + 1): # 从k=1开始，因为k=0是当前状态 # 计算车辆与障碍物之间的距离平方 dist_sq = (X[0, k] - P[nx+2])**2 + (X[1, k] - P[nx+3])**2 # 要求距离大于安全距离 min_dist_sq = (veh_r + obs_r + safe_dist)**2 opti.subject_to(dist_sq >= min_dist_sq)

4.4.3 配置求解器

配置优化求解器并为热启动做准备：

# 求解器设置opts = { \'ipopt.print_level\': 0, # 抑制详细输出 \'print_time\': 0, # 不打印求解时间 \'ipopt.max_iter\': 6000, # 最大迭代次数 \'ipopt.tol\': 1e-5, # 收敛容差 \'ipopt.acceptable_tol\': 1e-4, # 可接受收敛容差 \'ipopt.mu_init\': 1e-2, # 初始互补松弛变量 \'ipopt.constr_viol_tol\': 1e-4 # 约束违反容差}opti.solver(\'ipopt\', opts)

4.5 决策状态机设计

为了实现智能避障变道，我们需要一个决策状态机来决定何时变道、何时返回原车道：

# 状态机状态定义class ManeuverState: DRIVING_LANE_1 = 0 # 在第一车道正常行驶 CHANGING_TO_LANE_2 = 1 # 正在向第二车道变道 DRIVING_LANE_2 = 2 # 在第二车道行驶 RETURNING_TO_LANE_1 = 3 # 正在返回第一车道 COMPLETED = 4  # 完成整个避障变道任务# 状态转换参数evade_trigger_distance = 25.0 # 开始变道的触发距离 (m)pass_obstacle_distance = obs_length / 2 + vehicle_length / 2 + 3.0 # 通过障碍物的距离阈值return_trigger_x_offset = vehicle_length / 2 + 6.0 # 触发返回车道的距离阈值# 状态机逻辑def update_maneuver_state(state, x_vehicle, y_vehicle, x_obstacle): \"\"\"根据当前状态和车辆与障碍物的相对位置更新状态和目标车道\"\"\" if state == ManeuverState.DRIVING_LANE_1: # 如果接近障碍物，开始变道 distance_to_obstacle = x_obstacle - x_vehicle if 0 < distance_to_obstacle < evade_trigger_distance: return ManeuverState.CHANGING_TO_LANE_2, lane2_y return state, lane1_y elif state == ManeuverState.CHANGING_TO_LANE_2: # 如果已经接近第二车道中心线，认为变道完成 if abs(y_vehicle - lane2_y) < 0.3: return ManeuverState.DRIVING_LANE_2, lane2_y return state, lane2_y elif state == ManeuverState.DRIVING_LANE_2: # 如果已经超过障碍物一定距离，开始返回第一车道 if x_vehicle > x_obstacle + pass_obstacle_distance: return ManeuverState.RETURNING_TO_LANE_1, lane1_y return state, lane2_y elif state == ManeuverState.RETURNING_TO_LANE_1: # 如果已经接近第一车道中心线，认为返回完成 if abs(y_vehicle - lane1_y) < 0.3: return ManeuverState.COMPLETED, lane1_y return state, lane1_y else: # COMPLETED return state, lane1_y

这个状态机根据车辆与障碍物的相对位置，自动决定何时开始变道、何时返回原车道，实现了自主决策功能。与简单的基于时间的变道相比，这种方法更加智能和安全。

4.6 仿真实现

4.6.1 仿真循环设计

下面是实现仿真循环的代码，包括状态机更新、MPC求解和系统状态更新：

# 仿真设置sim_time = 12.0 # 仿真总时间 (s)t_history = np.arange(0, sim_time, Ts)n_steps = len(t_history)# 初始化历史记录数组x_history = np.zeros((nx, n_steps)) # 状态历史u_history = np.zeros((nu, n_steps-1)) # 控制输入历史obs_history = np.zeros((2, n_steps)) # 障碍物位置历史state_history = np.zeros(n_steps, dtype=int) # 状态机状态历史target_y_history = np.zeros(n_steps) # 目标车道历史# 初始状态设置x_current = np.array([0.0, lane1_y, 0.0, target_v_val * 0.8]) # 初始速度略低于目标x_history[:, 0] = x_currentobs_x = obs_x_initialobs_y = obs_y_fixedobs_history[:, 0] = [obs_x, obs_y]# 状态机初始化maneuver_state = ManeuverState.DRIVING_LANE_1target_y = lane1_ytarget_y_history[0] = target_ystate_history[0] = maneuver_state# 控制器变量psi_dot_prev = 0.0 # 上一时刻航向角速度，用于平滑控制# 为热启动准备初始猜测值U_guess = np.zeros((nu, Np))X_guess = np.tile(x_current, (Np + 1, 1)).Tprint(\"开始仿真...\")for i in range(n_steps - 1): # 当前时间 current_time = t_history[i] # 更新障碍物位置 obs_x = obs_x_initial + obs_speed_x * current_time obs_history[:, i+1] = [obs_x, obs_y] # 更新状态机和目标车道 maneuver_state, target_y = update_maneuver_state( maneuver_state, x_current[0], x_current[1], obs_x ) state_history[i+1] = maneuver_state target_y_history[i+1] = target_y # 设置MPC参数 current_param = np.concatenate([ x_current, # 当前状态 [x, y, psi, v] [target_y, target_v_val], # 目标y位置和速度 [obs_x, obs_y], # 障碍物位置 [psi_dot_prev] # 上一时刻航向角速度 ]) opti.set_value(P, current_param) # 设置初始猜测值（热启动） opti.set_initial(X, X_guess) opti.set_initial(U, U_guess) # 求解MPC优化问题 try: sol = opti.solve() # 提取结果 X_optimal = sol.value(X) U_optimal = sol.value(U) # 保存历史数据 u_history[:, i] = U_optimal[:, 0] # 只应用第一个控制输入 # 更新系统状态（使用车辆模型而非简单欧拉积分） x_next = f(x_current, U_optimal[:, 0]).full().flatten() x_current = x_next x_history[:, i+1] = x_current # 更新初始猜测（热启动） X_guess = np.hstack((X_optimal[:, 1:], X_optimal[:, -1].reshape(-1, 1))) U_guess = np.hstack((U_optimal[:, 1:], U_optimal[:, -1].reshape(-1, 1))) # 更新上一时刻航向角速度 psi_dot_prev = (x_current[3] / L) * np.tan(U_optimal[1, 0]) except Exception as e: print(f\"优化求解失败: {e}\") breakprint(\"仿真完成!\")

4.6.2 结果可视化

实现静态图表和动态动画的可视化：

以上代码讲解比较碎片化，
完整代码如下（含可视化代码）

import casadi as ca # 导入 CasADi 用于符号计算和自动微分，是构建NMPC的核心库import numpy as np # 导入 NumPy 用于数值计算，特别是数组操作import matplotlib.pyplot as plt # 导入 Matplotlib.pyplot 用于绘图import time # 导入 time 模块，用于计时等操作from matplotlib.patches import Rectangle # 从 Rectangle 用于绘制车辆和障碍物形状from matplotlib.transforms import Affine2D # Affine2D 用于车辆的旋转和平移变换# FuncAnimation 用于创建动画from matplotlib.animation import FuncAnimation# --- 中文字体设置 ---# 尝试设置 Matplotlib 的字体以支持中文显示try: # 设置 sans-serif 字体为 \'SimHei\' (黑体)，这是一种常用的中文字体 plt.rcParams[\'font.sans-serif\'] = [\'SimHei\'] # 设置坐标轴负号显示为 False，以便正确显示负号 (在某些中文字体配置下，负号可能显示为方框) plt.rcParams[\'axes.unicode_minus\'] = Falseexcept Exception as e: # 如果字体设置失败，打印警告信息 print(f\"中文字体设置警告: {e}。如果中文显示不正确，请确保已安装如 SimHei 等中文字体，并正确配置 Matplotlib。\")# --- NMPC (非线性模型预测控制) 参数 ---Ts = 0.1 # 采样时间 (s)：控制器更新和系统离散化的时间间隔Np = 20 # 预测时域：控制器向前预测的步数L = 2.5 # 车辆轴距 (m)：前轮和后轮轴之间的距离，用于运动学模型v_max = 20.0 # 最大速度 (m/s)v_min = -2.0 # 最小速度 (m/s) (允许倒车)a_max = 3.0 # 最大加速度 (m/s^2)a_min = -5.0 # 最大减速度 (m/s^2) (即负向加速度的最小值)delta_max = np.deg2rad(30) # 最大转向角 (rad)：将角度从度转换为弧度delta_min = -delta_max # 最小转向角 (rad)：通常与最大转向角对称# 车辆绘图尺寸vehicle_plot_length = 4.0 # 绘图用车辆长度 (m)vehicle_plot_width = 1.8 # 绘图用车辆宽度 (m)nx = 4 # 状态量数量：[x (全局坐标系X位置), y (全局坐标系Y位置), psi (车辆航向角), v (车辆速度)]nu = 2 # 控制量数量：[a (加速度), delta (前轮转向角)]# --- 权重矩阵调整 ---# 状态权重矩阵 Q: 对应状态 [x, y, psi, v] 的误差惩罚# Q矩阵参数说明：# Q[0,0] = 1.0: x方向位置权重# - 增大：车辆会更严格地跟踪x方向的参考轨迹# - 减小：车辆在x方向有更大的自由度，可以更灵活地调整# Q[1,1] = 100.0: y方向位置权重# - 增大：车辆会更严格地保持在车道中心，减少横向偏移# - 减小：车辆在y方向有更大的自由度，可以更灵活地变道# Q[2,2] = 0.5: 航向角权重# - 增大：车辆会更严格地保持期望航向角，转向更平滑# - 减小：车辆可以更灵活地改变航向，但可能导致转向不够平滑# Q[3,3] = 1.0: 速度权重# - 增大：车辆会更严格地跟踪目标速度# - 减小：车辆在速度控制上更灵活，但可能导致速度波动Q = np.diag([1.0, 100.0, 0.5, 1.0])# 控制权重矩阵 R: 对应控制量 [a, delta] 的变化惩罚# R矩阵参数说明：# R[0,0] = 0.1: 加速度权重# - 增大：加速度变化更平滑，但响应可能变慢# - 减小：加速度变化更灵活，但可能导致控制不够平滑# R[1,1] = 1000.0: 转向角权重# - 增大：转向更平滑，但可能导致转向响应不够及时# - 减小：转向更灵活，但可能导致转向不够平滑R = np.diag([0.1, 1000.0])# 终端权重因子：用于增强终端状态的跟踪精度# 增大：更注重终端状态的准确性，但可能导致中间过程不够平滑# 减小：更注重整个过程的平滑性，但终端状态可能不够准确P_factor = 0.01P = Q * P_factor# 航向角速度变化惩罚权重# 增大：转向更平滑，但可能导致转向响应不够及时# 减小：转向更灵活，但可能导致转向不够平滑w_psi_dot_change = 18000.0# --- 道路参数 ---lane_width = 3.5 # 车道宽度 (m)lane1_y = 0.0 # 第一车道中心线 y 坐标 (m)lane2_y = lane_width # 第二车道中心线 y 坐标 (m)# --- 道路边界参数 ---road_y_min_abs = lane1_y - lane_width / 2 # 道路绝对下边界road_y_max_abs = lane2_y + lane_width / 2 # 道路绝对上边界# 车辆中心Y坐标的约束，考虑车辆宽度vehicle_y_constraint_lower = road_y_min_abs + vehicle_plot_width / 2vehicle_y_constraint_upper = road_y_max_abs - vehicle_plot_width / 2# --- 障碍物参数 ---obs_x_initial = 35.0 # 障碍物初始 x 坐标 (m) (中心点)obs_y_fixed = lane1_y # 障碍物 y 坐标 (m) (中心点, 假设障碍物在第一车道中心)obs_speed_x = 4.0 # 障碍物向右移动的速度 (m/s)obs_length = vehicle_plot_length # 障碍物长度 (m)obs_width = vehicle_plot_width # 障碍物宽度 (m)obs_color = \'orange\'  # 障碍物颜色# 碰撞检测参数veh_r = vehicle_plot_length / 2.5 # 车辆等效碰撞半径 (m)obs_effective_collision_radius = obs_length / 2.0 # 障碍物等效碰撞半径 (m)# 安全距离参数说明：# 增大：车辆会与障碍物保持更大的距离，更安全但可能导致变道不够及时# 减小：车辆可以更接近障碍物，变道更及时但安全性降低safe_dist = 0.05target_v_val = 10.0 # 目标巡航速度 (m/s)# --- CasADi 符号变量与模型 ---# 定义状态变量x_sym = ca.SX.sym(\'x_s\') # x位置y_sym = ca.SX.sym(\'y_s\') # y位置psi_sym = ca.SX.sym(\'psi_s\') # 航向角v_sym = ca.SX.sym(\'v_s\') # 速度states_sym = ca.vertcat(x_sym, y_sym, psi_sym, v_sym)# 定义控制变量a_sym = ca.SX.sym(\'a_s\') # 加速度delta_sym = ca.SX.sym(\'delta_s\') # 转向角controls_sym = ca.vertcat(a_sym, delta_sym)# 定义车辆运动学模型rhs = ca.vertcat(v_sym * ca.cos(psi_sym), # x方向速度  v_sym * ca.sin(psi_sym), # y方向速度  (v_sym / L) * ca.tan(delta_sym), # 航向角变化率  a_sym) # 加速度f_discrete = ca.Function(\'f_discrete\', [states_sym, controls_sym], [states_sym + Ts * rhs])# --- NMPC 问题构建 ---opti = ca.Opti()X_dv = opti.variable(nx, Np + 1) # 状态变量U_dv = opti.variable(nu, Np) # 控制变量P_param = opti.parameter(nx + 5) # 参数：[x_curr (nx), target_y, target_v, obs_x_center, obs_y_center, prev_psi_dot]# 构建目标函数obj = 0for k in range(Np): # 目标x设为当前预测的x，不强制x方向的跟踪，让车辆自由前进 x_ref_k = ca.vertcat(X_dv[0, k], P_param[nx], 0, P_param[nx+1]) # 目标航向角为0 obj += ca.mtimes([(X_dv[:, k] - x_ref_k).T, Q, (X_dv[:, k] - x_ref_k)]) # 状态误差惩罚 obj += ca.mtimes([U_dv[:, k].T, R, U_dv[:, k]]) # 控制量惩罚# 终端状态惩罚terminal_state_ref = ca.vertcat(X_dv[0, Np], P_param[nx], 0, P_param[nx+1])terminal_state_error = X_dv[:, Np] - terminal_state_refobj += ca.mtimes([terminal_state_error.T, P, terminal_state_error])# 航向角速度变化惩罚psi_dot_preds_list = []for k_pd in range(Np): current_psi_dot_expr = (X_dv[3, k_pd] / L) * ca.tan(U_dv[1, k_pd]) psi_dot_preds_list.append(current_psi_dot_expr)if Np > 0: obj += w_psi_dot_change * (psi_dot_preds_list[0] - P_param[nx+4])**2for k_cost in range(1, Np): obj += w_psi_dot_change * (psi_dot_preds_list[k_cost] - psi_dot_preds_list[k_cost-1])**2opti.minimize(obj)# 添加约束条件opti.subject_to(X_dv[:, 0] == P_param[:nx]) # 初始状态约束for k in range(Np): opti.subject_to(X_dv[:, k+1] == f_discrete(X_dv[:, k], U_dv[:, k])) # 系统动力学约束# 控制量约束opti.subject_to(opti.bounded(a_min, U_dv[0, :], a_max)) # 加速度约束opti.subject_to(opti.bounded(delta_min, U_dv[1, :], delta_max)) # 转向角约束opti.subject_to(opti.bounded(v_min, X_dv[3, :], v_max)) # 速度约束# 碰撞约束与道路边界约束for k in range(1, Np + 1): # 从 k=1 开始，因为 k=0 是当前状态 # 车辆中心预测位置: X_dv[0, k], X_dv[1, k] # 1. 碰撞约束 dist_sq_obs = (X_dv[0, k] - P_param[nx+2])**2 + \\  (X_dv[1, k] - P_param[nx+3])**2 min_dist_sq_allowed = (veh_r + obs_effective_collision_radius + safe_dist)**2 opti.subject_to(dist_sq_obs >= min_dist_sq_allowed) # 2. 道路边界约束 opti.subject_to(X_dv[1, k] >= vehicle_y_constraint_lower) opti.subject_to(X_dv[1, k] <= vehicle_y_constraint_upper)# 求解器设置opts = { \'ipopt.print_level\': 0, \'print_time\': 0, \'ipopt.max_iter\': 6000, \'ipopt.tol\': 1e-5, \'ipopt.acceptable_tol\': 1e-4, \'ipopt.mu_init\': 1e-2, \'ipopt.constr_viol_tol\': 1e-4}opti.solver(\'ipopt\', opts)# --- 仿真循环 ---sim_time = 12.0 # 可以适当延长仿真时间观察返回车道行为t_history = np.arange(0, sim_time, Ts)x_history = np.zeros((nx, len(t_history)))u_history = np.zeros((nu, len(t_history)-1))target_y_history = np.zeros(len(t_history))obs_x_center_history = np.zeros(len(t_history))obs_y_center_history = np.zeros(len(t_history))# 初始状态设置x_current = np.array([0.0, lane1_y, 0.0, target_v_val * 0.8])x_history[:, 0] = x_currenttarget_y_history[0] = lane1_yobs_x_center_history[0] = obs_x_initialobs_y_center_history[0] = obs_y_fixedpsi_dot_at_last_interval = 0.0U_guess_current = np.zeros((nu, Np))X_guess_current = np.tile(x_current, (Np + 1, 1)).T# 初始控制猜测值设置num_accel_steps = Np // 2if x_current[3] < target_v_val and num_accel_steps > 0: desired_avg_accel = (target_v_val - x_current[3]) / (num_accel_steps * Ts) applied_accel = np.clip(desired_avg_accel, a_min, a_max) U_guess_current[0, :num_accel_steps] = applied_accelelif x_current[3] > target_v_val and num_accel_steps > 0: desired_avg_decel = (target_v_val - x_current[3]) / (num_accel_steps * Ts) applied_decel = np.clip(desired_avg_decel, a_min, a_max) U_guess_current[0, :num_accel_steps] = applied_decel# 初始状态预测temp_x_guess = x_current.copy()X_guess_current[:, 0] = temp_x_guessfor k_rollout in range(Np): u_rollout = U_guess_current[:, k_rollout] temp_x_next_full = f_discrete(temp_x_guess, u_rollout) temp_x_guess = temp_x_next_full.full().flatten() X_guess_current[:, k_rollout+1] = temp_x_guess# 状态机参数设置maneuver_state = \"DRIVING_LANE_1\"current_target_y = lane1_yevade_trigger_distance = 25.0 # 触发变道的距离阈值pass_obstacle_distance = obs_length / 2 + vehicle_plot_length / 2 + 3.0 # 通过障碍物的距离阈值return_trigger_x_offset = vehicle_plot_length / 2 + 6.0 # 触发返回车道的距离阈值stabilize_distance_after_return = 15.0 # 返回车道后的稳定距离print(\"开始仿真...\")for i in range(len(t_history) - 1): vehicle_x_position = x_current[0] current_sim_time_step = t_history[i] # 更新障碍物位置 current_obs_x_center = obs_x_initial + obs_speed_x * current_sim_time_step current_obs_y_center = obs_y_fixed obs_x_center_history[i] = current_obs_x_center obs_y_center_history[i] = current_obs_y_center obstacle_front_x = current_obs_x_center - obs_length / 2 vehicle_front_x_ref = vehicle_x_position # 状态机逻辑 if maneuver_state == \"DRIVING_LANE_1\": current_target_y = lane1_y if vehicle_front_x_ref > obstacle_front_x - evade_trigger_distance and \\  vehicle_front_x_ref < current_obs_x_center + obs_length : maneuver_state = \"CHANGING_TO_LANE_2\" print(f\"t={t_history[i]:.1f}s: State -> CHANGING_TO_LANE_2 (veh_x={vehicle_x_position:.1f}m, obs_front_x={obstacle_front_x:.1f}m)\") elif maneuver_state == \"CHANGING_TO_LANE_2\": current_target_y = lane2_y if vehicle_front_x_ref > current_obs_x_center + pass_obstacle_distance: maneuver_state = \"DRIVING_LANE_2\" print(f\"t={t_history[i]:.1f}s: State -> DRIVING_LANE_2 (veh_x={vehicle_x_position:.1f}m, obs_center_x={current_obs_x_center:.1f}m)\") elif maneuver_state == \"DRIVING_LANE_2\": current_target_y = lane2_y if vehicle_front_x_ref > current_obs_x_center + return_trigger_x_offset: maneuver_state = \"RETURNING_TO_LANE_1\" print(f\"t={t_history[i]:.1f}s: State -> RETURNING_TO_LANE_1 (veh_x={vehicle_x_position:.1f}m, obs_center_x={current_obs_x_center:.1f}m)\") elif maneuver_state == \"RETURNING_TO_LANE_1\": current_target_y = lane1_y if vehicle_front_x_ref > current_obs_x_center + return_trigger_x_offset + stabilize_distance_after_return and \\  abs(x_current[1] - lane1_y) < 0.2 and \\  abs(x_current[3] - target_v_val) < 1.5 : maneuver_state = \"COMPLETED_MANEUVER\" print(f\"t={t_history[i]:.1f}s: State -> COMPLETED_MANEUVER (veh_x={vehicle_x_position:.1f}m, y={x_current[1]:.2f}m, obs_center_x={current_obs_x_center:.1f}m)\") elif maneuver_state == \"COMPLETED_MANEUVER\": current_target_y = lane1_y target_y_history[i] = current_target_y if i == len(t_history) - 2: target_y_history[i+1] = current_target_y # 设置NMPC求解器参数 current_obstacle_center_for_nmpc = np.array([current_obs_x_center, current_obs_y_center]) param_psi_dot_val = psi_dot_at_last_interval opti.set_value(P_param, np.concatenate([x_current,  [current_target_y],  [target_v_val],  current_obstacle_center_for_nmpc,  [param_psi_dot_val]])) opti.set_initial(X_dv, X_guess_current) opti.set_initial(U_dv, U_guess_current) try: # 求解NMPC问题 sol = opti.solve() u_optimal_sequence = sol.value(U_dv) x_predicted_sequence = sol.value(X_dv) u_apply = u_optimal_sequence[:, 0] U_guess_current = np.hstack((u_optimal_sequence[:, 1:], u_optimal_sequence[:, -1].reshape(-1,1))) X_guess_current = np.hstack((x_predicted_sequence[:, 1:], x_predicted_sequence[:, -1].reshape(-1,1))) except Exception as e: print(f\"求解器在步骤 {i} (t={t_history[i]:.1f}s) 失败: {e}。应用零控制或前一控制。\") u_apply = np.array([0.0, 0.0]) if i == 0 else u_history[:, i-1] # 当求解失败时，重置猜测值 U_guess_current = np.zeros((nu, Np)) temp_x_guess_fail = x_current.copy() current_X_for_reset = temp_x_guess_fail.copy() X_guess_reset = np.zeros_like(X_guess_current) X_guess_reset[:,0] = current_X_for_reset for k_reset in range(Np): u_reset = U_guess_current[:, k_reset] temp_f_val = f_discrete(current_X_for_reset, u_reset) if isinstance(temp_f_val, ca.DM): current_X_for_reset = temp_f_val.full().flatten() else: current_X_for_reset = ca.evalf(f_discrete(current_X_for_reset, u_reset)).full().flatten() X_guess_reset[:, k_reset+1] = current_X_for_reset X_guess_current = X_guess_reset # 更新控制历史 u_history[:, i] = u_apply # 计算航向角速度 current_v_for_psi_dot = x_current[3] current_delta_for_psi_dot = u_apply[1] psi_dot_at_last_interval = (current_v_for_psi_dot / L) * np.tan(current_delta_for_psi_dot) # 更新系统状态 x_next_full = f_discrete(x_current, u_apply) x_next = x_next_full.full().flatten() x_next[2] = np.arctan2(np.sin(x_next[2]), np.cos(x_next[2])) # 归一化角度 x_next[3] = np.clip(x_next[3], v_min, v_max) # 限制速度 x_history[:, i+1] = x_next x_current = x_next# 更新最后一个时间步的障碍物位置last_time_step = t_history[-1]obs_x_center_history[-1] = obs_x_initial + obs_speed_x * last_time_stepobs_y_center_history[-1] = obs_y_fixed# 更新最后一个时间步的目标车道if maneuver_state == \"COMPLETED_MANEUVER\" or maneuver_state == \"RETURNING_TO_LANE_1\": target_y_history[-1] = lane1_yelif maneuver_state == \"DRIVING_LANE_2\": target_y_history[-1] = lane2_yelse: target_y_history[-1] = current_target_yprint(\"仿真数据生成完毕，开始生成动画...\")# --- 动画与绘图 ---fig, axs = plt.subplots(4, 1, figsize=(12, 15), sharex=True, gridspec_kw={\'height_ratios\': [3, 1, 1, 1]})plt.subplots_adjust(hspace=0.3)# 设置第一个子图（车辆轨迹）axs[0].set_xlabel(\'x 坐标 (m)\')axs[0].set_ylabel(\'y 坐标 (m)\')axs[0].set_title(\'NMPC 车辆避障与换道仿真动画 (参数优化后)\')# 绘制道路边界和车道线axs[0].axhline(road_y_min_abs, color=\'k\', linestyle=\'-\', lw=1.5, label=\'道路下边界\')axs[0].axhline(road_y_max_abs, color=\'k\', linestyle=\'-\', lw=1.5, label=\'道路上边界\')axs[0].axhline(lane1_y, color=\'lightgray\', linestyle=\':\', lw=1, label=\'第一车道中心\')axs[0].axhline(lane2_y, color=\'lightgray\', linestyle=\':\', lw=1, label=\'第二车道中心\')# 创建障碍物图形obstacle_bottom_left_x_init = obs_x_center_history[0] - obs_length / 2obstacle_bottom_left_y_init = obs_y_center_history[0] - obs_width / 2obstacle_shape_anim = Rectangle((obstacle_bottom_left_x_init, obstacle_bottom_left_y_init), obs_length, obs_width, facecolor=obs_color, alpha=0.7, label=\'障碍物\', edgecolor=\'black\', lw=0.5, zorder=5)axs[0].add_artist(obstacle_shape_anim)# 设置网格和坐标轴axs[0].grid(True, linestyle=\':\', alpha=0.7)axs[0].axis(\'equal\')# 设置显示范围min_x_traj, max_x_traj = np.min(x_history[0,:]), np.max(x_history[0,:])max_obs_x_final = obs_x_initial + obs_speed_x * t_history[-1] + obs_length / 2min_obs_x_initial = obs_x_initial - obs_length / 2axs[0].set_xlim(min(min_x_traj, min_obs_x_initial) - 10, max(max_x_traj, max_obs_x_final) + 20)axs[0].set_ylim(road_y_min_abs - lane_width*0.5, road_y_max_abs + lane_width*0.5)# 创建车辆图形vehicle_rect_anim = Rectangle((-vehicle_plot_length / 2, -vehicle_plot_width / 2), vehicle_plot_length, vehicle_plot_width, facecolor=\'cornflowerblue\', alpha=0.6, edgecolor=\'black\', lw=0.7, zorder=10)arrow_anim_line, = axs[0].plot([], [], lw=1.5, color=\'darkblue\', zorder=11)rear_axle_path_line, = axs[0].plot([], [], color=\'royalblue\', linestyle=\'-\', lw=1.0, alpha=0.7, label=\'车辆轨迹 (参考点)\')axs[0].plot([],[], color=\'lime\', linestyle=\'--\', lw=1.0, alpha=0.9, label=\'目标Y轨迹\')axs[0].legend(loc=\'upper left\', fontsize=\'small\')# 创建时间文本显示time_text = axs[0].text(0.02, 0.95, \'\', transform=axs[0].transAxes, fontsize=\'medium\', bbox=dict(boxstyle=\"round,pad=0.3\", fc=\"wheat\", ec=\"black\", lw=0.5, alpha=0.7))# 设置速度子图axs[1].set_ylabel(\'速度 (m/s)\')axs[1].axhline(target_v_val, color=\'green\', linestyle=\'--\', label=\'目标速度\')line_v, = axs[1].plot([], [], lw=1.5, color=\'purple\', label=\'实际速度\')axs[1].legend(fontsize=\'small\', loc=\'lower right\')axs[1].grid(True, linestyle=\':\', alpha=0.7)axs[1].set_ylim(min(v_min, np.min(x_history[3, :])) -1, max(v_max, np.max(x_history[3, :])) + 1)# 设置航向角子图axs[2].set_ylabel(\'航向角 (度)\')line_psi, = axs[2].plot([], [], lw=1.5, color=\'teal\', label=\'实际航向角\')axs[2].legend(fontsize=\'small\', loc=\'lower right\')axs[2].grid(True, linestyle=\':\', alpha=0.7)min_psi_deg = np.rad2deg(np.min(x_history[2, :])) if len(x_history[2,:]) > 0 else -10max_psi_deg = np.rad2deg(np.max(x_history[2, :])) if len(x_history[2,:]) > 0 else 10margin_psi = 10if abs(max_psi_deg - min_psi_deg) < 1e-6 : min_psi_deg -= margin_psi max_psi_deg += margin_psiaxs[2].set_ylim(min_psi_deg - margin_psi, max_psi_deg + margin_psi)# 设置加速度和转向角子图axs[3].set_xlabel(\'时间 (s)\')axs[3].set_ylabel(\'加速度 (m/s^2)\', color=\'orangered\')line_a, = axs[3].plot([], [], lw=1.5, color=\'orangered\', label=\'加速度\')axs[3].tick_params(axis=\'y\', labelcolor=\'orangered\')axs[3].legend(fontsize=\'small\', loc=\'upper left\')axs[3].grid(True, linestyle=\':\', alpha=0.7)axs[3].set_ylim(a_min - 1, a_max + 1)# 创建双y轴显示转向角ax3_twin = axs[3].twinx()ax3_twin.set_ylabel(\'转向角 (度)\', color=\'darkgoldenrod\')line_delta, = ax3_twin.plot([], [], lw=1.5, color=\'darkgoldenrod\', linestyle=\'--\', label=\'转向角\')ax3_twin.tick_params(axis=\'y\', labelcolor=\'darkgoldenrod\')ax3_twin.legend(fontsize=\'small\', loc=\'upper right\')ax3_twin.set_ylim(np.rad2deg(delta_min) - 5, np.rad2deg(delta_max) + 5)# 绘制目标轨迹axs[0].plot(x_history[0,:len(target_y_history)], target_y_history, color=\'lime\', linestyle=\'--\', lw=1.0, alpha=0.9, label=\'目标Y轨迹\')axs[0].legend(loc=\'upper left\', fontsize=\'small\')# 动画初始化函数def init(): axs[0].add_patch(vehicle_rect_anim) obs_bl_x_init = obs_x_center_history[0] - obs_length / 2 obs_bl_y_init = obs_y_center_history[0] - obs_width / 2 obstacle_shape_anim.set_xy((obs_bl_x_init, obs_bl_y_init)) arrow_anim_line.set_data([], []) rear_axle_path_line.set_data([], []) time_text.set_text(\'\') line_v.set_data([], []) line_psi.set_data([], []) line_a.set_data([], []) line_delta.set_data([], []) return [vehicle_rect_anim, arrow_anim_line, rear_axle_path_line, time_text, line_v, line_psi, line_a, line_delta, obstacle_shape_anim]# 动画更新函数def update(frame): # 更新车辆位置和姿态 veh_center_x = x_history[0, frame] veh_center_y = x_history[1, frame] psi_rad = x_history[2, frame] # 更新车辆图形变换 transform = Affine2D().rotate_around(0, 0, psi_rad) + Affine2D().translate(veh_center_x, veh_center_y) + axs[0].transData vehicle_rect_anim.set_transform(transform) # 更新车辆方向箭头 arrow_length = vehicle_plot_length * 0.4 arrow_end_x = veh_center_x + arrow_length * np.cos(psi_rad) arrow_end_y = veh_center_y + arrow_length * np.sin(psi_rad) arrow_anim_line.set_data([veh_center_x, arrow_end_x], [veh_center_y, arrow_end_y]) # 更新车辆轨迹 rear_axle_path_line.set_data(x_history[0, :frame+1], x_history[1, :frame+1]) # 更新障碍物位置 current_frame_obs_x_center = obs_x_center_history[frame] current_frame_obs_y_center = obs_y_center_history[frame] obstacle_bottom_left_x = current_frame_obs_x_center - obs_length / 2 obstacle_bottom_left_y = current_frame_obs_y_center - obs_width / 2 obstacle_shape_anim.set_xy((obstacle_bottom_left_x, obstacle_bottom_left_y)) # 更新状态显示 current_display_maneuver_state = \"UNKNOWN\" target_y_idx = min(frame, len(target_y_history) - 1) current_target_y_for_display = target_y_history[target_y_idx] veh_x_pos_frame = x_history[0, frame] frame_obs_x_center = obs_x_center_history[frame] frame_obs_front_x = frame_obs_x_center - obs_length / 2 # 更新状态显示文本 actual_y_pos = x_history[1,frame] if abs(actual_y_pos - lane1_y) < 0.25 and \\ veh_x_pos_frame > frame_obs_x_center + return_trigger_x_offset + stabilize_distance_after_return * 0.8 and \\ abs(current_target_y_for_display - lane1_y) < 0.1 :  current_display_maneuver_state = \"完成机动\" elif abs(current_target_y_for_display - lane1_y) < 0.1: if veh_x_pos_frame < frame_obs_front_x - evade_trigger_distance * 0.5 : current_display_maneuver_state = \"第一车道行驶\" elif veh_x_pos_frame > frame_obs_x_center + pass_obstacle_distance : current_display_maneuver_state = \"返回第一车道\" else: current_display_maneuver_state = \"第一车道 (近障)\" elif abs(current_target_y_for_display - lane2_y) < 0.1: if veh_x_pos_frame < frame_obs_x_center + pass_obstacle_distance * 0.9 : current_display_maneuver_state = \"变道至第二车道\" else: current_display_maneuver_state = \"第二车道行驶\" # 更新状态文本 time_text.set_text(f\'时间: {t_history[frame]:.1f} s\\n速度: {x_history[3, frame]:.2f} m/s\\n状态: {current_display_maneuver_state}\\n目标Y: {current_target_y_for_display:.1f}m\\n实际Y: {actual_y_pos:.2f}m\') # 更新速度曲线 line_v.set_data(t_history[:frame+1], x_history[3, :frame+1]) # 更新航向角曲线 line_psi.set_data(t_history[:frame+1], np.rad2deg(x_history[2, :frame+1])) # 更新控制量曲线 if frame < len(u_history[0,:]): time_for_controls = t_history[:frame+1] control_slice_a = u_history[0, :frame+1] control_slice_delta = u_history[1, :frame+1] line_a.set_data(time_for_controls, control_slice_a) line_delta.set_data(time_for_controls, np.rad2deg(control_slice_delta)) elif frame > 0 : time_for_controls = t_history[:frame] control_slice_a = u_history[0, :frame] control_slice_delta = u_history[1, :frame] line_a.set_data(time_for_controls, control_slice_a) line_delta.set_data(time_for_controls, np.rad2deg(control_slice_delta)) else: line_a.set_data([], []) line_delta.set_data([], []) return [vehicle_rect_anim, arrow_anim_line, rear_axle_path_line, time_text, line_v, line_psi, line_a, line_delta, obstacle_shape_anim]# 创建动画num_frames = len(t_history)ani = FuncAnimation(fig, update, frames=num_frames,  init_func=init, blit=True, interval=Ts*1000, repeat=False)# 保存动画animation_filename = \'NMPC_避障变道仿真动画_v6_优化边界.gif\'try: ani.save(animation_filename, writer=\'pillow\', fps=int(1/Ts)) print(f\"动画已保存为: {animation_filename}\")except Exception as e: print(f\"保存动画失败: {e}\") print(\"请确保已安装 Pillow (for GIF) 或 FFmpeg (for MP4)，并将其添加到系统路径。\") print(\"如果问题持续，尝试在 FuncAnimation 中设置 blit=False。\")plt.show()

4.7 参数调优与性能分析

MPC控制器的性能很大程度上取决于参数的选择。下面分析一些关键参数及其影响：

1. 权重矩阵 Q、R 的影响：

   • Q[1,1]（横向位置权重）：增大可使车辆更精确地跟踪目标车道，但可能导致转向过激
   • R[1,1]（转向角权重）：增大可使转向更平滑，但可能降低车道跟踪精度
   • 平衡Q和R是关键，我们的实现中使用较大的转向角权重（R[1,1]=1000.0）确保平滑转向

2. 航向角速度变化惩罚 w_psi_dot_change：

   • 这个参数对转向平滑性至关重要
   • 增大会使转向更平滑，但可能降低响应速度
   • 减小会使转向更灵活，但可能出现转向抖动

3. 预测时域 Np：

   • 增大可提高预见性，使变道更平滑，但计算量增加
   • 减小可降低计算负担，但可能导致近视行为
   • 我们选择了适中的Np=20，在预见性和计算效率之间取得平衡

4. 安全距离参数：

   • 增大安全距离会使避障更加保守，安全裕度更大
   • 减小安全距离允许更激进的避障，但安全裕度降低
   • 我们的实现使用较小的安全距离（safe_dist=0.05），依靠碰撞半径保证安全

5. 状态机参数：

   • evade_trigger_distance：决定何时开始变道，增大会提前启动变道
   • pass_obstacle_distance：决定何时可以返回原车道，增大会延迟返回
   • 这些参数需要根据车辆速度和障碍物速度进行调整

通过仿真实验，我们可以观察到以下结果：

1. 避障效果：控制器能够成功检测到前方障碍物，并在合适的时机启动变道
2. 平滑变道：由于航向角速度变化惩罚，变道过程平滑自然
3. 速度控制：车辆能够在变道过程中维持接近目标速度的行驶状态
4. 返回原车道：超越障碍物后，车辆能够安全地返回原车道

4.8 小结与展望

在本章中，我们实现了一个基于MPC的智能避障变道控制系统，它展示了MPC在处理复杂自动驾驶场景中的优势：

1. 预见性控制：MPC能够提前预测车辆和障碍物未来的运动轨迹，规划最优避障路径
2. 约束处理：MPC自然地处理了道路边界、车辆物理限制和碰撞避免等约束
3. 多目标优化：同时兼顾了车道跟踪、速度控制、操作平滑和安全避障等多个目标
4. 自主决策：结合状态机，实现了基于环境感知的自主变道决策

这个控制系统还有以下改进空间：

1. 多障碍物避让：扩展系统以处理多个动态障碍物
2. 不确定性处理：考虑测量噪声和预测误差，增强鲁棒性
3. 计算优化：改进算法以降低计算负担，实现更高的控制频率
4. 复杂道路环境：处理弯道、交叉路口等更复杂的道路场景

MPC作为一种强大的预见性控制框架，在自动驾驶领域具有广阔的应用前景。它能够平衡安全性、舒适性和效率，适应复杂多变的驾驶环境，是实现高级自动驾驶的关键技术之一。

通过本教程，我们从MPC的基本概念出发，经过数学建模，最终实现了一个实用的避障变道控制系统。希望这个案例能帮助读者理解MPC的工作原理和应用方法，为进一步探索和应用MPC技术奠定基础。