如何在CVXPY中定义非线性约束？无人机MPC路径规划求助

问题根源分析 #

1. 无效的动力学约束：原代码中尝试用solve_ivp生成状态转移约束，但在CVXPY优化问题求解前，变量X[:,k].value和U[:,k].value均为None，导致生成的x_next无意义，动力学约束根本没有被纳入优化问题。优化器只能选择输出0控制量来最小化控制代价。
2. 动力学方程笔误：四旋翼角速度更新的交叉项计算错误，即使约束正确也无法生成合理的控制输入。
3. 代价函数设计缺陷：缺少终端状态代价，优化器没有足够的动力推动状态向目标位置移动。

修正方案 #

1. 显式离散化非线性动力学约束 #

采用欧拉前向离散化方法，将连续时间动力学转换为CVXPY可识别的非线性等式约束：
$$x_{k+1} = x_k + dt \cdot f(x_k, u_k)$$
直接用CVXPY的表达式描述该关系，确保优化器能处理非线性项。

2. 修正动力学方程 #

修正角速度更新的交叉项，匹配四旋翼的标准动力学模型：

• 俯仰角速度$\dot{q}$的交叉项应为$(I_z - I_x) \cdot p \cdot r$
• 偏航角速度$\dot{r}$的交叉项应为$(I_x - I_y) \cdot p \cdot q$

3. 优化代价函数 #

• 增加终端状态代价，强化目标位置跟踪的权重
• 微调控制代价权重，避免控制量被过度压制

4. 指定非线性求解器 #

CVXPY默认求解器不支持非线性问题，显式指定scipy.SLSQP或SCS求解器处理非线性约束。

修正后的完整代码 #

import numpy as np
import matplotlib.pyplot as plt
import cvxpy as cp
from scipy.integrate import solve_ivp

# 四旋翼参数定义
m = 1.0  # 质量
g = 9.81  # 重力加速度
Ix = 0.1  # x轴转动惯量
Iy = 0.1  # y轴转动惯量
Iz = 0.2  # z轴转动惯量
dt = 0.05  # 时间步长
N = 20  # 预测时域

# 修正后的非线性动力学模型
def f(x, u):
    F = np.zeros_like(x)
    # 位置导数（速度）
    F[0] = x[3]
    F[1] = x[4]
    F[2] = x[5]
    # 线加速度（牛顿第二定律）
    phi, theta, psi = x[6], x[7], x[8]
    F[3] = -u[0]/m * (np.sin(psi)*np.sin(phi) + np.cos(psi)*np.cos(phi)*np.sin(theta))
    F[4] = -u[0]/m * (np.cos(phi)*np.sin(psi) - np.cos(psi)*np.sin(phi)*np.sin(theta))
    F[5] = g - u[0]/m * (np.cos(psi)*np.cos(theta))
    # 姿态角导数（角速度）
    F[6] = x[9]
    F[7] = x[10]
    F[8] = x[11]
    # 角加速度（欧拉方程，修正交叉项）
    p, q, r = x[9], x[10], x[11]
    F[9] = ((Iy - Iz)*q*r)/Ix + u[1]/Ix
    F[10] = ((Iz - Ix)*p*r)/Iy + u[2]/Iy
    F[11] = ((Ix - Iy)*p*q)/Iz + u[3]/Iz
    return F

# 用于仿真的动力学包装函数
def f_wrapper(t, x, u):
    return f(x, u)

# 修正后的MPC控制器
def mpc_control(x0, pf, N, Q, R, Qf):
    # 定义优化变量：状态序列（12维，N+1步）、控制序列（4维，N步）
    X = cp.Variable((12, N + 1))
    U = cp.Variable((4, N))

    cost = 0
    constraints = []
    # 初始状态约束
    constraints += [X[:, 0] == x0]

    # 遍历预测时域，构建代价和约束
    for k in range(N):
        # 阶段代价：位置跟踪代价 + 控制代价
        state_cost = cp.quad_form(X[:3, k] - pf, Q)
        control_cost = cp.quad_form(U[:, k], R)
        cost += state_cost + control_cost

        # 非线性动力学约束：欧拉离散化
        x_k = X[:, k]
        u_k = U[:, k]
        # 用CVXPY表达式重构动力学方程
        phi, theta, psi = x_k[6], x_k[7], x_k[8]
        p, q, r = x_k[9], x_k[10], x_k[11]
        
        # 计算状态导数的CVXPY表达式
        x_dot = cp.hstack([
            x_k[3], x_k[4], x_k[5],
            -u_k[0]/m * (cp.sin(psi)*cp.sin(phi) + cp.cos(psi)*cp.cos(phi)*cp.sin(theta)),
            -u_k[0]/m * (cp.cos(phi)*cp.sin(psi) - cp.cos(psi)*cp.sin(phi)*cp.sin(theta)),
            g - u_k[0]/m * (cp.cos(psi)*cp.cos(theta)),
            x_k[9], x_k[10], x_k[11],
            ((Iy - Iz)*q*r)/Ix + u_k[1]/Ix,
            ((Iz - Ix)*p*r)/Iy + u_k[2]/Iy,
            ((Ix - Iy)*p*q)/Iz + u_k[3]/Iz
        ])
        # 离散状态转移约束
        constraints += [X[:, k+1] == x_k + dt * x_dot]

        # 控制输入约束：总推力和力矩的范围
        constraints += [u_k[0] >= 0, u_k[0] <= 20]  # 总推力需大于重力（m*g=9.81）
        constraints += [u_k[1] >= -5, u_k[1] <= 5]
        constraints += [u_k[2] >= -5, u_k[2] <= 5]
        constraints += [u_k[3] >= -5, u_k[3] <= 5]

    # 终端代价：强化目标位置跟踪
    cost += cp.quad_form(X[:3, N] - pf, Qf)

    # 求解优化问题：指定SLSQP求解器处理非线性约束
    prob = cp.Problem(cp.Minimize(cost), constraints)
    prob.solve(solver=cp.SCS, max_iters=10000)  # 或用cp.SLSQP，需确保scipy安装

    # 返回第一个控制输入
    return U[:, 0].value if U.value is not None else np.zeros(4)

# 初始状态：位置[0,0,0]，速度[0,0,0]，姿态角[0,0,0]，角速度[0,0,0]
x0 = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
# 目标位置
pf = np.array([10, 10, 10])

# 代价矩阵
Q = np.eye(3) * 100  # 位置跟踪权重
R = np.eye(4) * 0.1  # 控制代价权重（降低权重避免压制控制量）
Qf = np.eye(3) * 500  # 终端状态权重

# 仿真参数
num_steps = 100
x = x0
positions = [x[:3]]  # 存储位置轨迹

# 仿真循环
for i in range(num_steps):
    u = mpc_control(x, pf, N, Q, R, Qf)
    print(f"Step {i}: Control input = {u}")
    # 用solve_ivp进行实际状态更新（更准确的积分）
    sol = solve_ivp(f_wrapper, [0, dt], x, args=(u,), t_eval=[dt])
    x = sol.y[:, -1]
    positions.append(x[:3])

# 绘制轨迹
positions = np.array(positions)
fig = plt.figure(figsize=(10,8))
ax = fig.add_subplot(111, projection='3d')
ax.plot(positions[:,0], positions[:,1], positions[:,2], 'b-', label='MPC跟踪轨迹')
ax.scatter(pf[0], pf[1], pf[2], c='r', marker='*', s=200, label='目标位置')
ax.set_xlabel('X(m)')
ax.set_ylabel('Y(m)')
ax.set_zlabel('Z(m)')
ax.legend()
plt.title('四旋翼MPC路径规划轨迹')
plt.show()

关键说明 #

• 用CVXPY的原生表达式重构动力学，确保优化器能识别非线性约束
• 修正后的动力学模型符合四旋翼的标准欧拉方程
• 调整控制输入范围：总推力下限设为0（实际需大于重力以维持悬停），避免无效控制
• 降低控制代价权重，让优化器更愿意输出控制量推动状态变化

内容的提问来源于stack exchange，提问作者Seyed Alawi