欢迎学习非线性模型预测控制（NMPC）算法。NMPC 因其能直接处理非线性动力学模型和复杂的约束条件，在自动驾驶、无人机及机器人控制领域有着极高的应用价值。

下面我将为大家详细拆解 NMPC 的理论框架，并结合 Python + CasADi（目前工业界和学术界最主流的 NMPC 实现工具）给出完整的代码实现指导。

一、 核心数学模型：移动机器人运动学

在 NMPC 中，我们首先需要一个数学方程来描述“按一下按钮，机器人怎么动”。对于常见的差速或类车机器人，最简模型是：

• 状态量：位置和朝向。
• 控制量：线速度和角速度。

连续时间方程：

离散化（欧拉法）：为了在计算机上计算，我们将  时间后的状态预测为：

二、 NMPC 的求解逻辑

NMPC 的本质是在每一个采样时刻，求解一个约束优化问题：

1. 目标： 让预测的轨迹与参考轨迹的偏差最小，且控制量不要剧烈波动。
2. 约束：
• 必须符合物理规律（运动学方程）。
• 速度不能超过物理限制（如 ）。
• 角速度不能转得太快（如 ）。

三、 完整 Python 代码实现

这段代码可以直接运行。它模拟了一个机器人追踪圆周轨迹的过程。

依赖库安装：pip install casadi numpy matplotlib

import casadi as ca
import numpy as np
import matplotlib.pyplot as plt

defrun_nmpc_simulation():
# 1. 仿真参数设置
    T = 0.1# 采样时间 (s)
    N = 20# 预测时域 (Prediction Horizon)
    sim_time = 20.0# 总仿真时间 (s)

# 2. 定义状态和控制限制
    v_max = 0.6# 最大线速度
    omega_max = np.pi / 4# 最大角速度 (45度)

# 3. 准备参考轨迹 (例如：一个圆圈)
    t_range = np.arange(0, sim_time, T)
    x_ref = 2.0 * np.cos(0.3 * t_range)
    y_ref = 2.0 * np.sin(0.3 * t_range)
    theta_ref = 0.3 * t_range + np.pi/2

# 4. 初始化状态
    x0 = np.array([2.0, 0.0, np.pi/2]) # 初始位置
    state_history = [x0]
    u_history = []

# --- NMPC 控制器核心循环 ---
    curr_state = x0

for i in range(len(t_range) - N):
# 创建 CasADi Opti 实例
        opti = ca.Opti()

# 定义决策变量
        X = opti.variable(3, N+1) # 状态预测序列 (x, y, theta)
        U = opti.variable(2, N)   # 控制输入序列 (v, omega)

# 定义目标函数 (Cost Function)
        obj = 0
        Q = np.diag([10.0, 10.0, 0.1]) # 位置权重
        R = np.diag([0.1, 0.01])       # 控制权重

for k in range(N):
# 状态偏差代价
            st_err = X[:, k] - np.array([x_ref[i+k], y_ref[i+k], theta_ref[i+k]])
            obj += ca.mtimes([st_err.T, Q, st_err])
# 控制量代价
            obj += ca.mtimes([U[:, k].T, R, U[:, k]])

        opti.minimize(obj)

# 运动学约束 (Multiple Shooting)
for k in range(N):
            x_next = X[0, k] + U[0, k] * ca.cos(X[2, k]) * T
            y_next = X[1, k] + U[0, k] * ca.sin(X[2, k]) * T
            th_next = X[2, k] + U[1, k] * T
            opti.subject_to(X[0, k+1] == x_next)
            opti.subject_to(X[1, k+1] == y_next)
            opti.subject_to(X[2, k+1] == th_next)

# 边界约束 (物理限制)
        opti.subject_to(opti.bounded(-v_max, U[0, :], v_max))
        opti.subject_to(opti.bounded(-omega_max, U[1, :], omega_max))

# 初始状态约束
        opti.subject_to(X[:, 0] == curr_state)

# 设置求解器 (IPOPT)
        opts = {'ipopt.print_level': 0, 'print_time': 0}
        opti.solver('ipopt', opts)

# 求解
try:
            sol = opti.solve()
            u_optimal = sol.value(U[:, 0]) # 取第一个控制指令
except:
            print("求解失败，保持上一步控制")
            u_optimal = u_history[-1] if u_history else [0, 0]

# 5. 更新系统 (在真实环境或仿真环境执行一步)
        v_cmd, w_cmd = u_optimal
        next_state = np.array([
            curr_state[0] + v_cmd * np.cos(curr_state[2]) * T,
            curr_state[1] + v_cmd * np.sin(curr_state[2]) * T,
            curr_state[2] + w_cmd * T
        ])

        curr_state = next_state
        state_history.append(curr_state)
        u_history.append(u_optimal)

if i % 20 == 0:
            print(f"Step {i}: x={curr_state[0]:.2f}, y={curr_state[1]:.2f}")

# 6. 可视化结果
    state_history = np.array(state_history)
    plt.figure(figsize=(8, 8))
    plt.plot(x_ref, y_ref, 'r--', label='Reference Path')
    plt.plot(state_history[:, 0], state_history[:, 1], 'b-', label='NMPC Trajectory')
    plt.legend()
    plt.title("NMPC Trajectory Tracking")
    plt.xlabel("X (m)")
    plt.ylabel("Y (m)")
    plt.grid(True)
    plt.show()

if __name__ == "__main__":
    run_nmpc_simulation()

四、 代码中的关键技术细节

1. Opti 堆栈： 与传统的 SX 或 MX 符号定义相比，opti.variable 和 opti.subject_to 更接近人类语言，极大降低了编写 NMPC 的出错率。
2. 多重射击法 (Multiple Shooting)： 我们不仅把  作为变量，也把每个时刻的状态  作为变量，并通过 opti.subject_to(X[:, k+1] == x_next) 将它们耦合。这种方法比单次射击（Single Shooting）更稳定，更容易处理非线性。
3. 预测时域 ： 在代码中设置为 20。如果  太小，机器人看路太短，容易开出弯道；如果  太大，计算量会成倍增加，导致无法实时。
4. 权重 Q 和 R：
• 如果你希望跟踪更精准，增大 Q。
• 如果你希望机器人行驶平稳、省油、保护电机，增大 R。

五、 学习建议

如果你想进阶，可以尝试在代码中加入以下功能：

1. 动态障碍物： 增加一个约束 (X[0,k]-obs_x)^2 + (X[1,k]-obs_y)^2 > r^2。
2. 热启动 (Warm Start)： 每次求解后，用当前的 sol.value(X) 和 sol.value(U) 作为下一次求解的初始值，这能将计算时间缩短 50% 以上。
3. 从 Python 迁移到 C++： 当你对逻辑熟悉后，可以使用 CasADi 生成 C 代码，或者直接使用 Acados（目前工业界最快的 NMPC 库）。

这个 Demo 涵盖了 NMPC 的完整闭环，是理解非线性控制最直观的起点。