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)$是终端状态的代价。
NMPC非线性模型预测控制从原理与代码实现 NMPC非线性模型预测控制详细原理推导 平行泊车轨迹规划 倒立摆Swing up控制 车辆运动学模型轨迹跟踪 四旋翼无人机轨迹跟踪 包含上述所有的文档和代码。
接下来就是求解这个优化问题啦，一般通过数值优化算法，像序列二次规划（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的魅力。
