如何在GEKKO中优化固定翼无人机的多航点轨迹？

我的目标是计算固定翼无人机的最优轨迹，使其到达一系列指定的x坐标（例如[150, 0]）。

我尝试了多种单航点约束施加方法：

• 单航点场景下，使用m.fix_final(x, val = 150.0)可成功实现，但会同时固定x的导数为0；
• 使用m.Minimize(final*10**5*(x - 150)**2)也能生效，但扩展到多航点时，因航点到达时间不明确，难以指定对应节点；
• 使用waypoint = m.Var(value=0)结合m.Equation(waypoint == m.if3(x - 150, 0, 1))和m.fix_final(waypoint, val=1)，虽不限制导数，但实现繁琐且对初始条件敏感。

扩展到双航点时上述方法均失效，我参考相关方案采用多阶段优化实现，代码如下：

import numpy as np
import matplotlib.pyplot as plt
from gekko import GEKKO
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt

waypoints = [150, 0]

# create GEKKO model
m = GEKKO(remote=False)

num_time_steps = 31
# scale 0-1 time with tf
m.time = np.linspace(0,1,num_time_steps)

# options
m.options.NODES = 4 # collocation nodes
m.options.SOLVER = 1 # solver APOPT # 3 # solver (IPOPT)
m.options.IMODE = 6
m.options.MAX_ITER = 1000
m.options.MV_TYPE = 0
m.options.DIAGLEVEL = 1

# final time
tf = [m.FV(value=3.0, lb = 0.1) for i in range(len(waypoints))] # lb on time to prevent time-travel

# Manipulated Variables
CL = [m.MV(value=1, lb = 0, ub = 10) for i in range(len(waypoints))] # lift coefficient
phi = [m.MV(value=0, lb = np.deg2rad(-45), ub = np.deg2rad(45)) for i in range(len(waypoints))] # roll angle

# set STATUS to 1 to allow optimizer to change
for i in range(len(waypoints)): 
    tf[i].STATUS = 1
    CL[i].STATUS = 1
    phi_dot[i].STATUS = 1

mass = m.Const(value=9.5)
g = m.Const(value=9.81)
CD0 = m.Const(value=0.01)
rho = m.Const(value=1.225)
S = m.Const(value=0.65)
f_max = m.Const(value=40)

# State Variables
V = [m.Var(value=100, lb = 0, ub = 200, fixed_initial=False) for i in range(len(waypoints))] # velocity
psi = [m.Var(value=0, fixed_initial=False) for i in range(len(waypoints))] # heading angle
gamma = [m.Var(value=0, lb = np.deg2rad(-10), ub = np.deg2rad(10), fixed_initial=False) for i in range(len(waypoints))] # flight path angle

x = [m.Var(value=0, fixed_initial=False) for i in range(len(waypoints))] # x position
y = [m.Var(value=0, fixed_initial=False) for i in range(len(waypoints))] # y position
z = [m.Var(value=0, fixed_initial=False) for i in range(len(waypoints))] # z position


# specify initial conditions
m.fix_initial(x[0], val=0.0)
m.fix_initial(y[0], val=0.0)
m.fix_initial(z[0], val=150.0)
m.fix_initial(V[0], val=60.0)
m.fix_initial(psi[0], val=0.0)
m.fix_initial(gamma[0], val=0.0)

# Aerodynamic Model
k = m.Intermediate(4 * f_max ** 2 * CD0)
CD = [m.Intermediate(CD0 + CL[i] ** 2 / k) for i in range(len(waypoints))]

D = [m.Intermediate(0.5 * rho * V[i] ** 2 * S * CD[i]) for i in range(len(waypoints))]
L = [m.Intermediate(0.5 * rho * V[i] ** 2 * S * CL[i]) for i in range(len(waypoints))]

for i in range(len(waypoints)):
    # differential equations scaled by tf
    m.Equation(V[i].dt()==tf[i]*(- D[i] / mass - g*m.sin(gamma[i])))
    m.Equation(gamma[i].dt()== tf[i] * (L[i] * m.cos(phi[i]) - mass * g * m.cos(gamma[i])) / (mass * V[i]))
    m.Equation(psi[i].dt()==tf[i]*(L[i] * m.sin(phi[i]) + m.cos(psi[i])) / (mass * V[i] * m.cos(gamma[i])))
    m.Equation(x[i].dt()==tf[i]*(V[i] * m.cos(gamma[i]) * m.cos(psi[i])))
    m.Equation(y[i].dt()==tf[i]*(V[i] * m.cos(gamma[i]) * m.sin(psi[i])))
    m.Equation(z[i].dt()==tf[i]*(V[i] * m.sin(gamma[i])))

for i in range(len(waypoints) - 1):
        m.Connection(phi[i+1], phi[i], pos1 = 1, pos2 = 'end', node1 = 1, node2 = 'end')
        m.Connection(x[i+1], x[i], pos1 = 1, pos2 = 'end', node1 = 1, node2 = 'end')
        m.Connection(y[i+1], y[i], pos1 = 1, pos2 = 'end', node1 = 1, node2 = 'end')
        m.Connection(z[i+1], z[i], pos1 = 1, pos2 = 'end', node1 = 1, node2 = 'end')
        m.Connection(psi[i+1], psi[i], pos1 = 1, pos2 = 'end', node1 = 1, node2 = 'end')
        m.Connection(gamma[i+1], gamma[i], pos1 = 1, pos2 = 'end', node1 = 1, node2 = 'end')
        m.Connection(V[i+1], V[i], pos1 = 1, pos2 = 'end', node1 = 1, node2 = 'end')

        m.Connection(phi[i+1],'calculated', pos1=1, node1=1)
        m.Connection(x[i+1],'calculated', pos1=1, node1=1)
        m.Connection(y[i+1],'calculated', pos1=1, node1=1)
        m.Connection(z[i+1],'calculated', pos1=1, node1=1)
        m.Connection(psi[i+1],'calculated', pos1=1, node1=1)
        m.Connection(gamma[i+1],'calculated', pos1=1, node1=1)
        m.Connection(V[i+1],'calculated', pos1=1, node1=1)


f = np.zeros(num_time_steps); f[-1]=1; final=m.Param(f)

# minimize final time while meeting waypoints
for i in range(len(waypoints)):
        m.Minimize(final*10**5*(x[i] - waypoints[i])**2)

m.Minimize((m.sum(tf))**2)


m.solve()

但该方案无法同时到达两个航点，仅将阶段终点置于两目标中点附近。

请问是否有推荐的多航点问题解决方法？或我的动力学模型是否存在阻碍转向的错误？

编辑：
调整初始条件和无人机质量后，模型已可实现转向并到达所有航点（含新增的第三个航点），但仍希望了解更规范的实现方案。

一、规范的多航点轨迹优化实现方案 #

1. 多阶段优化的正确约束方式 #

你的多阶段思路是对的，但之前的航点约束施加有误，应该直接固定每个阶段的终点状态值，而非用软约束（最小化平方项）。具体调整：

• 对于第i个阶段（对应第i+1个航点），直接使用m.fix_final(x[i], val=waypoints[i])，明确要求该阶段结束时x坐标到达目标值。
• 去掉之前的软约束项m.Minimize(final*10**5*(x[i] - waypoints[i])**2)，改用硬约束确保航点被严格到达。

2. 简化多阶段连接逻辑 #

你的Connection调用存在冗余，只需确保相邻阶段的状态变量连续即可，无需重复设置'calculated'连接。正确的连接代码应为：

for i in range(len(waypoints)-1):
    # 确保阶段i的终点等于阶段i+1的起点
    m.Connection(x[i+1], x[i], pos1=0, pos2='end')
    m.Connection(y[i+1], y[i], pos1=0, pos2='end')
    m.Connection(z[i+1], z[i], pos1=0, pos2='end')
    m.Connection(V[i+1], V[i], pos1=0, pos2='end')
    m.Connection(psi[i+1], psi[i], pos1=0, pos2='end')
    m.Connection(gamma[i+1], gamma[i], pos1=0, pos2='end')
    m.Connection(phi[i+1], phi[i], pos1=0, pos2='end')

注：pos1=0表示阶段i+1的起点（时间=0处），pos2='end'表示阶段i的终点（时间=1处），这样能保证状态连续。

3. 可选：单阶段+时间节点约束方案 #

如果不想用多阶段，也可以在单阶段中引入航点到达时间变量，通过约束指定时刻的x坐标来实现多航点：

• 定义航点到达时间t_wp = [m.FV(value=..., lb=0) for _ in waypoints]，确保时间递增（t_wp[i+1] > t_wp[i]）。
• 使用m.time的缩放特性，将航点时间映射到0-1的模型时间轴，然后约束对应时刻的x值：

# 假设总时间为tf_total（单个FV）
for i, wp in enumerate(waypoints):
    # 计算航点对应的模型时间点
    t_wp_norm = t_wp[i] / tf_total
    # 约束该时刻的x等于航点值
    m.Equation(x == wp).at_time(t_wp_norm)

这种方式无需拆分阶段，但需要处理时间变量的递增约束，适合航点数量不多的场景。

二、动力学模型检查 #

你之前的模型中，偏航角（psi）的微分方程存在错误：

m.Equation(psi[i].dt()==tf[i]*(L[i] * m.sin(phi[i]) + m.cos(psi[i])) / (mass * V[i] * m.cos(gamma[i])))

其中+ m.cos(psi[i])属于多余项，正确的固定翼偏航动力学方程应为：

m.Equation(psi[i].dt()==tf[i]*(L[i] * m.sin(phi[i])) / (mass * V[i] * m.cos(gamma[i])))

这个错误会导致偏航力矩异常，阻碍正常转向，也是之前难以到达航点的原因之一。

另外，代码中phi_dot[i].STATUS = 1属于笔误，应该是phi[i].STATUS = 1，否则会报错未定义phi_dot。

三、额外优化建议 #

• 对操纵变量（CL、phi）添加变化率约束，避免控制量突变，提升轨迹平滑性：

for i in range(len(waypoints)):
    CL[i].DMAX = 0.5  # 最大变化率
    phi[i].DMAX = np.deg2rad(5)  # 最大滚转角变化率（每秒5度）

• 若追求最短时间，目标函数直接用m.Minimize(m.sum(tf))即可，无需平方项，平方项会放大长阶段的权重，可能导致非最优解。

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