探索NMPC非线性模型预测控制：从原理到代码实践
·
nmpc非线性模型预测控制从原理到代码实践 包含4个案例 1 自动泊车轨迹优化 2 倒立摆上翻控制 3 车辆运动学轨迹跟踪 4 四旋翼无人机轨迹跟踪
在控制领域，非线性模型预测控制（NMPC）可是个强大的工具，它能处理复杂的非线性系统，在很多实际场景中都有出色的表现。今天咱就来深入了解NMPC，并且结合几个超有趣的案例，通过代码实践来感受它的魅力。
NMPC原理简单说说
NMPC的核心思想其实不难理解。它就像是一个有“远见”的决策者，会在每一个控制时刻，根据当前系统的状态，预测系统在未来一段时间内的行为。然后，基于这个预测，它会在一个有限的时间范围内寻找最优的控制输入，让系统的输出尽可能接近我们期望的目标，同时满足各种约束条件。每过一个时间步，它就会更新当前状态，重新进行预测和优化，如此循环下去。
用数学公式来表示的话，NMPC要解决的就是一个优化问题：
minimize Σ_{k=0}^{N-1} L(x_k, u_k) + F(x_N)
subject to x_{k+1} = f(x_k, u_k)
h(x_k, u_k) ≤ 0
g(x_k, u_k) = 0这里面，L(xk, uk) 是阶段成本函数，F(xN) 是终端成本函数，xk 是系统状态，uk 是控制输入，f(xk, uk) 是系统的动态方程，h(xk, uk) ≤ 0 和 g(xk, u_k) = 0 分别是不等式约束和等式约束。
案例实战
案例1：自动泊车轨迹优化
自动泊车是个很实用的功能，NMPC可以帮助车辆规划出最优的泊车轨迹。我们的目标是让车辆从初始位置平稳地停入车位，同时要避免撞到障碍物。
下面是一段简单的Python代码示例，使用 Casadi 库来实现自动泊车的NMPC优化：
import casadi as ca
import numpy as np
# 系统参数
dt = 0.1 # 时间步长
N = 20 # 预测时域
# 状态变量
x = ca.SX.sym('x')
y = ca.SX.sym('y')
theta = ca.SX.sym('theta')
states = ca.vertcat(x, y, theta)
n_states = states.numel()
# 控制变量
v = ca.SX.sym('v')
omega = ca.SX.sym('omega')
controls = ca.vertcat(v, omega)
n_controls = controls.numel()
# 动态方程
rhs = ca.vertcat(v * ca.cos(theta), v * ca.sin(theta), omega)
f = ca.Function('f', [states, controls], [rhs])
# 初始状态和目标状态
x0 = np.array([0, 0, 0])
x_ref = np.array([5, 5, np.pi/2])
# 成本函数和约束条件
Q = np.diag([1, 1, 0.1])
R = np.diag([0.1, 0.1])
U = ca.SX.sym('U', n_controls, N)
X = ca.SX.sym('X', n_states, N+1)
cost = 0
g = []
for k in range(N):
st = X[:, k]
con = U[:, k]
cost += ca.mtimes([(st - x_ref).T, Q, (st - x_ref)]) + ca.mtimes([con.T, R, con])
st_next = X[:, k+1]
f_value = f(st, con)
st_next_euler = st + dt * f_value
g.append(st_next - st_next_euler)
g = ca.vertcat(*g)
# 定义优化问题
nlp = {'x': ca.vertcat(ca.reshape(X, -1), ca.reshape(U, -1)),
'f': cost,
'g': g}
solver = ca.nlpsol('solver', 'ipopt', nlp)
# 初始猜测
X0 = np.tile(x0, (N+1, 1)).T
U0 = np.zeros((n_controls, N))
# 求解优化问题
sol = solver(x0=ca.vertcat(ca.reshape(X0, -1), ca.reshape(U0, -1)),
lbg=0, ubg=0)
X_opt = ca.reshape(sol['x'][:n_states*(N+1)], n_states, N+1).full()
U_opt = ca.reshape(sol['x'][n_states*(N+1):], n_controls, N).full()
print("Optimal states:", X_opt)
print("Optimal controls:", U_opt)代码分析
- 首先，我们定义了系统的状态变量（位置
x、y和角度theta）和控制变量（速度v和角速度omega），以及系统的动态方程f。 - 接着，我们设定了初始状态和目标状态，并且定义了成本函数和约束条件。成本函数考虑了状态误差和控制输入的代价，约束条件保证了系统的动态一致性。
- 然后，我们使用
Casadi的nlpsol函数创建了一个非线性优化求解器，使用ipopt作为求解器。 - 最后，我们给出初始猜测，求解优化问题，得到最优的状态和控制序列。
案例2：倒立摆上翻控制
倒立摆是控制领域的经典问题，NMPC可以帮助我们实现倒立摆的上翻和稳定控制。
import casadi as ca
import numpy as np
# 系统参数
dt = 0.1
N = 20
m = 1.0
l = 1.0
g = 9.81
# 状态变量
theta = ca.SX.sym('theta')
dtheta = ca.SX.sym('dtheta')
states = ca.vertcat(theta, dtheta)
n_states = states.numel()
# 控制变量
u = ca.SX.sym('u')
controls = ca.vertcat(u)
n_controls = controls.numel()
# 动态方程
d2theta = (u - m * g * l * ca.sin(theta)) / (m * l**2)
rhs = ca.vertcat(dtheta, d2theta)
f = ca.Function('f', [states, controls], [rhs])
# 初始状态和目标状态
x0 = np.array([0, 0])
x_ref = np.array([np.pi, 0])
# 成本函数和约束条件
Q = np.diag([1, 0.1])
R = np.array([[0.1]])
U = ca.SX.sym('U', n_controls, N)
X = ca.SX.sym('X', n_states, N+1)
cost = 0
g = []
for k in range(N):
st = X[:, k]
con = U[:, k]
cost += ca.mtimes([(st - x_ref).T, Q, (st - x_ref)]) + ca.mtimes([con.T, R, con])
st_next = X[:, k+1]
f_value = f(st, con)
st_next_euler = st + dt * f_value
g.append(st_next - st_next_euler)
g = ca.vertcat(*g)
# 定义优化问题
nlp = {'x': ca.vertcat(ca.reshape(X, -1), ca.reshape(U, -1)),
'f': cost,
'g': g}
solver = ca.nlpsol('solver', 'ipopt', nlp)
# 初始猜测
X0 = np.tile(x0, (N+1, 1)).T
U0 = np.zeros((n_controls, N))
# 求解优化问题
sol = solver(x0=ca.vertcat(ca.reshape(X0, -1), ca.reshape(U0, -1)),
lbg=0, ubg=0)
X_opt = ca.reshape(sol['x'][:n_states*(N+1)], n_states, N+1).full()
U_opt = ca.reshape(sol['x'][n_states*(N+1):], n_controls, N).full()
print("Optimal states:", X_opt)
print("Optimal controls:", U_opt)代码分析
这个代码和自动泊车的代码结构很相似。我们先定义了倒立摆的状态变量（角度 theta 和角速度 dtheta）和控制变量（外力 u），以及系统的动态方程。然后，同样地定义了成本函数和约束条件，使用 Casadi 求解优化问题。
案例3：车辆运动学轨迹跟踪
车辆运动学轨迹跟踪的目标是让车辆沿着给定的参考轨迹行驶。
import casadi as ca
import numpy as np
# 系统参数
dt = 0.1
N = 20
# 状态变量
x = ca.SX.sym('x')
y = ca.SX.sym('y')
theta = ca.SX.sym('theta')
states = ca.vertcat(x, y, theta)
n_states = states.numel()
# 控制变量
v = ca.SX.sym('v')
omega = ca.SX.sym('omega')
controls = ca.vertcat(v, omega)
n_controls = controls.numel()
# 动态方程
rhs = ca.vertcat(v * ca.cos(theta), v * ca.sin(theta), omega)
f = ca.Function('f', [states, controls], [rhs])
# 参考轨迹
t = np.linspace(0, dt * N, N+1)
x_ref = np.sin(t)
y_ref = np.cos(t)
theta_ref = np.zeros(N+1)
x_refs = np.vstack((x_ref, y_ref, theta_ref))
# 成本函数和约束条件
Q = np.diag([1, 1, 0.1])
R = np.diag([0.1, 0.1])
U = ca.SX.sym('U', n_controls, N)
X = ca.SX.sym('X', n_states, N+1)
cost = 0
g = []
for k in range(N):
st = X[:, k]
con = U[:, k]
cost += ca.mtimes([(st - x_refs[:, k]).T, Q, (st - x_refs[:, k])]) + ca.mtimes([con.T, R, con])
st_next = X[:, k+1]
f_value = f(st, con)
st_next_euler = st + dt * f_value
g.append(st_next - st_next_euler)
g = ca.vertcat(*g)
# 定义优化问题
nlp = {'x': ca.vertcat(ca.reshape(X, -1), ca.reshape(U, -1)),
'f': cost,
'g': g}
solver = ca.nlpsol('solver', 'ipopt', nlp)
# 初始猜测
X0 = np.zeros((n_states, N+1))
U0 = np.zeros((n_controls, N))
# 求解优化问题
sol = solver(x0=ca.vertcat(ca.reshape(X0, -1), ca.reshape(U0, -1)),
lbg=0, ubg=0)
X_opt = ca.reshape(sol['x'][:n_states*(N+1)], n_states, N+1).full()
U_opt = ca.reshape(sol['x'][n_states*(N+1):], n_controls, N).full()
print("Optimal states:", X_opt)
print("Optimal controls:", U_opt)代码分析
这里我们生成了一个参考轨迹（正弦和余弦曲线），在成本函数中考虑了车辆当前状态和参考轨迹的误差。其他部分和前面的案例类似，通过 Casadi 求解优化问题得到最优控制。
案例4：四旋翼无人机轨迹跟踪
四旋翼无人机的轨迹跟踪也是NMPC的一个重要应用场景。
import casadi as ca
import numpy as np
# 系统参数
dt = 0.1
N = 20
# 状态变量
x = ca.SX.sym('x')
y = ca.SX.sym('y')
z = ca.SX.sym('z')
vx = ca.SX.sym('vx')
vy = ca.SX.sym('vy')
vz = ca.SX.sym('vz')
states = ca.vertcat(x, y, z, vx, vy, vz)
n_states = states.numel()
# 控制变量
u1 = ca.SX.sym('u1')
u2 = ca.SX.sym('u2')
u3 = ca.SX.sym('u3')
u4 = ca.SX.sym('u4')
controls = ca.vertcat(u1, u2, u3, u4)
n_controls = controls.numel()
# 动态方程
rhs = ca.vertcat(vx, vy, vz, u1, u2, u3 - 9.81)
f = ca.Function('f', [states, controls], [rhs])
# 参考轨迹
t = np.linspace(0, dt * N, N+1)
x_ref = np.sin(t)
y_ref = np.cos(t)
z_ref = np.ones(N+1)
vx_ref = np.cos(t)
vy_ref = -np.sin(t)
vz_ref = np.zeros(N+1)
x_refs = np.vstack((x_ref, y_ref, z_ref, vx_ref, vy_ref, vz_ref))
# 成本函数和约束条件
Q = np.diag([1, 1, 1, 0.1, 0.1, 0.1])
R = np.diag([0.1, 0.1, 0.1, 0.1])
U = ca.SX.sym('U', n_controls, N)
X = ca.SX.sym('X', n_states, N+1)
cost = 0
g = []
for k in range(N):
st = X[:, k]
con = U[:, k]
cost += ca.mtimes([(st - x_refs[:, k]).T, Q, (st - x_refs[:, k])]) + ca.mtimes([con.T, R, con])
st_next = X[:, k+1]
f_value = f(st, con)
st_next_euler = st + dt * f_value
g.append(st_next - st_next_euler)
g = ca.vertcat(*g)
# 定义优化问题
nlp = {'x': ca.vertcat(ca.reshape(X, -1), ca.reshape(U, -1)),
'f': cost,
'g': g}
solver = ca.nlpsol('solver', 'ipopt', nlp)
# 初始猜测
X0 = np.zeros((n_states, N+1))
U0 = np.zeros((n_controls, N))
# 求解优化问题
sol = solver(x0=ca.vertcat(ca.reshape(X0, -1), ca.reshape(U0, -1)),
lbg=0, ubg=0)
X_opt = ca.reshape(sol['x'][:n_states*(N+1)], n_states, N+1).full()
U_opt = ca.reshape(sol['x'][n_states*(N+1):], n_controls, N).full()
print("Optimal states:", X_opt)
print("Optimal controls:", U_opt)代码分析
这个案例中，我们定义了四旋翼无人机的状态变量（位置和速度）和控制变量（四个电机的输入），以及系统的动态方程。同样地，生成参考轨迹，定义成本函数和约束条件，最后使用 Casadi 求解优化问题。
通过这四个案例，我们可以看到NMPC在不同场景下的应用。虽然代码有点复杂，但只要理解了原理，一步一步来，还是可以掌握的。希望大家能从这些案例中对NMPC有更深入的认识，也可以自己动手修改代码，探索更多的可能性。
北京人形旗下天工造物具身智能开源社区，聚焦具身天工与慧思开物两大平台
更多推荐
15
0- 0
已为社区贡献63条内容
所有评论(0)