探索NMPC：从原理到多场景代码实现的奇妙之旅

yrZXiZUdP

118人浏览 · 2026-03-13 19:45:00

yrZXiZUdP · 2026-03-13 19:45:00 发布

NMPC非线性模型预测控制从原理与代码实现 NMPC非线性模型预测控制详细原理推导平行泊车轨迹规划倒立摆Swing up控制车辆运动学模型轨迹跟踪四旋翼无人机轨迹跟踪包含上述所有的文档和代码。

嘿，各位技术小伙伴们！今天咱们一头扎进NMPC（非线性模型预测控制）的神秘世界，看看它的原理到底是啥，又怎么在各种酷炫的场景里通过代码实现。

NMPC详细原理推导

NMPC的核心思想就是基于系统的非线性模型，预测系统未来的行为，然后通过求解一个优化问题来确定当前时刻的最优控制输入。想象一下，你开着车，不仅要考虑当下怎么操作方向盘、油门刹车，还要预测接下来几百米车子的行驶轨迹，然后做出最合理的操作。

从数学角度看，我们首先得有个描述系统动态的非线性模型，比如常见的离散时间模型可以写成：$x{k + 1} = f(xk, uk)$，这里$xk$是系统状态，$uk$是控制输入。然后呢，我们要定义一个性能指标，一般是最小化预测轨迹和期望轨迹之间的偏差，再加上控制输入的变化量等项，比如：$J = \sum{k = 0}^{N - 1} [l(xk, uk) + \rho \Delta uk^2] + F(xN)$，其中$l(xk, uk)$衡量状态和控制输入的代价，$\rho$是权重系数，$F(x_N)$是终端状态的代价。

接下来就是求解这个优化问题啦，一般通过数值优化算法，像序列二次规划（SQP）等。这个过程其实就是在每个时间步，不断调整控制输入，让系统尽可能地沿着期望轨迹走。

平行泊车轨迹规划

平行泊车可是个技术活，NMPC能帮我们优雅地实现。先看代码（以Python为例，这里简化代码仅展示核心思路）：

import numpy as np
# 定义车辆运动学模型参数
L = 2.5  # 车辆轴距
dt = 0.1  # 时间间隔

def vehicle_model(x, u):
    theta = x[2]
    new_x = x[0] + u[0] * np.cos(theta) * dt
    new_y = x[1] + u[0] * np.sin(theta) * dt
    new_theta = theta + u[1] * dt
    return np.array([new_x, new_y, new_theta])

# 假设期望的泊车终点位置
goal = np.array([5, 2, np.pi / 2])
# 初始状态
x0 = np.array([0, 0, 0])
# 这里简单设定预测时域N
N = 20
# 迭代求解控制输入
for i in range(N):
    # 这里应该有具体的优化求解过程，为简化省略
    u_opt = np.array([1, 0.1])  # 假设优化得到的控制输入
    x0 = vehicle_model(x0, u_opt)
    print(f"当前位置: {x0}")

代码分析：这里我们先定义了车辆运动学模型vehiclemodel，它根据当前状态x和控制输入u来更新车辆位置。在实际应用中，我们要在每个时间步通过优化算法找到uopt，让车辆逐步驶向目标位置goal。这个优化过程就用到前面说的NMPC原理，最小化车辆当前位置和目标位置的偏差等代价函数。

倒立摆Swing up控制

倒立摆可是控制领域的经典案例，用NMPC来控制它摇摆上升超有趣。

import sympy as sp
# 定义符号变量
t = sp.Symbol('t')
theta = sp.Function('theta')(t)
omega = sp.Derivative(theta, t)
g = 9.81
l = 1
m = 0.1
# 建立倒立摆动力学方程
eq = m * l**2 * sp.Derivative(omega, t) + m * g * l * sp.sin(theta)
# 离散化处理（简化示意）
# 这里可以通过Euler法等进行离散化
# 假设已经离散化得到离散模型
def inverted_pendulum_discrete(x, u):
    new_theta = x[0] + x[1] * dt
    new_omega = x[1] + (-m * g * l * np.sin(x[0]) + u) / (m * l**2) * dt
    return np.array([new_theta, new_omega])

# 初始状态
x_init = np.array([0, 0])
# 控制目标：让摆直立，即theta接近pi
goal_theta = np.pi
# 预测时域
N = 30
for i in range(N):
    # 同样省略具体优化求解
    u_opt = 1  # 假设优化得到的控制输入
    x_init = inverted_pendulum_discrete(x_init, u_opt)
    print(f"当前theta: {x_init[0]}")

代码分析：首先用符号运算库sympy建立倒立摆的动力学方程，然后进行离散化得到离散模型invertedpendulumdiscrete。在循环里，每次通过优化找到合适的控制输入u_opt，让倒立摆从初始状态摇摆上升到直立状态，这个过程也是基于NMPC最小化摆的角度和目标角度偏差等代价函数。

车辆运动学模型轨迹跟踪

车辆在行驶过程中要精准跟踪预设轨迹，NMPC大显身手。

import matplotlib.pyplot as plt

# 定义车辆运动学模型
def kinematic_model(x, u):
    v = u[0]
    delta = u[1]
    Lf = 2.0
    new_x = x[0] + v * np.cos(x[2]) * dt
    new_y = x[1] + v * np.sin(x[2]) * dt
    new_theta = x[2] + v / Lf * np.tan(delta) * dt
    return np.array([new_x, new_y, new_theta])

# 期望轨迹，简单假设为直线
def desired_trajectory(t):
    return np.array([t, 0, 0])

# 初始状态
x_start = np.array([0, 0, 0])
T = 10
time = np.arange(0, T, dt)
x_track = np.zeros((len(time), 3))
x_track[0] = x_start

for i in range(len(time) - 1):
    # 这里应该调用优化算法找u_opt
    u_opt = np.array([1, 0])  # 假设优化得到的控制输入
    x_track[i + 1] = kinematic_model(x_track[i], u_opt)
    desired = desired_trajectory(time[i])
    # 计算偏差等，可用于优化
    error = np.linalg.norm(x_track[i + 1][:2] - desired[:2])

plt.plot(x_track[:, 0], x_track[:, 1], label='跟踪轨迹')
plt.plot([desired_trajectory(t)[0] for t in time], [desired_trajectory(t)[1] for t in time], label='期望轨迹')
plt.legend()
plt.show()

代码分析：kinematicmodel定义了车辆运动学模型，根据控制输入更新车辆状态。desiredtrajectory设定了期望轨迹，这里简单设为直线。在循环中，每次通过优化得到u_opt来跟踪期望轨迹，最后通过绘图展示跟踪效果。

四旋翼无人机轨迹跟踪

四旋翼无人机在空中灵活飞舞，NMPC让它能精准跟踪轨迹。

import numpy as np
import matplotlib.pyplot as plt

# 四旋翼动力学模型简化（仅展示核心部分）
def quadcopter_dynamics(x, u):
    # x: [x, y, z, vx, vy, vz, phi, theta, psi, p, q, r]
    # u: [T1, T2, T3, T4]
    g = 9.81
    m = 0.5
    new_x = x[0] + x[3] * dt
    new_y = x[1] + x[4] * dt
    new_z = x[2] + x[5] * dt
    new_vx = x[3] + (u[0] * np.cos(x[6]) * np.sin(x[7]) * np.cos(x[8]) + u[1] * np.cos(x[6]) * np.sin(x[7]) * np.cos(x[8]) + u[2] * np.cos(x[6]) * np.sin(x[7]) * np.cos(x[8]) + u[3] * np.cos(x[6]) * np.sin(x[7]) * np.cos(x[8])) / m * dt
    new_vy = x[4] + (u[0] * np.cos(x[6]) * np.cos(x[7]) * np.sin(x[8]) + u[1] * np.cos(x[6]) * np.cos(x[7]) * np.sin(x[8]) + u[2] * np.cos(x[6]) * np.cos(x[7]) * np.sin(x[8]) + u[3] * np.cos(x[6]) * np.cos(x[7]) * np.sin(x[8])) / m * dt
    new_vz = x[5] + (-g + (u[0] + u[1] + u[2] + u[3]) / m) * dt
    # 姿态角和角速度更新省略部分复杂计算
    return np.array([new_x, new_y, new_z, new_vx, new_vy, new_vz])

# 期望轨迹，例如螺旋线
def desired_spiral(t):
    R = 1
    h = 0.5
    x = R * np.cos(t)
    y = R * np.sin(t)
    z = h * t
    return np.array([x, y, z])

x0 = np.array([0, 0, 0, 0, 0, 0])
T = 10
dt = 0.1
time = np.arange(0, T, dt)
x_traj = np.zeros((len(time), 6))
x_traj[0] = x0

for i in range(len(time) - 1):
    # 省略优化求解u_opt过程
    u_opt = np.array([1, 1, 1, 1])  # 假设优化得到的控制输入
    x_traj[i + 1] = quadcopter_dynamics(x_traj[i], u_opt)
    desired = desired_spiral(time[i])
    error = np.linalg.norm(x_traj[i + 1][:3] - desired)

plt.plot(x_traj[:, 0], x_traj[:, 1], label='实际轨迹')
plt.plot([desired_spiral(t)[0] for t in time], [desired_spiral(t)[1] for t in time], label='期望轨迹')
plt.legend()
plt.show()

代码分析：quadcopterdynamics构建了四旋翼无人机的动力学模型，根据控制输入u更新无人机状态。desiredspiral设定了期望的螺旋线轨迹。循环中通过优化得到u_opt来让无人机跟踪期望轨迹，绘图展示跟踪情况。

以上就是NMPC在不同场景下从原理到代码实现的精彩内容啦，大家可以进一步深入研究，在实际项目中发挥NMPC的强大威力！完整的文档和代码都在相应的项目里，大家可以自行探索调试，感受NMPC的魅力。