使用 python gekko 进行离散路径跟踪

Question

我有一些代表路径的离散数据点，我想最小化对象轨迹与这些路径点之间的距离以及其他一些约束。我正在尝试将 gekko 作为解决此问题的工具，为此我通过从抛物线制作数据点和对路径的约束来解决一个简单的问题。我尝试解决它是

from gekko import GEKKO
import numpy as np
import matplotlib.pyplot as plt
import time

#path data points
x_ref = np.linspace(0, 4, num=21)
y_ref = - np.square(x_ref) + 16

#constraint for visualization purposes
x_bound = np.linspace(0, 4, num=10)
y_bound = 1.5*x_bound + 4

def distfunc(x,y,xref,yref,p):
    '''
    Shortest distance from (x,y) to (xref, yref)
    '''

    dtemp = []
    for i in range(len(xref)):
        d = (x-xref[i])**2+(y-yref[i])**2
        dtemp.append(dtemp)
    min_id = dtemp.index(min(dtemp))
    if min_id == 0:
        next_id = min_id+1
    elif min_id == len(x_ref):
        next_id = min_id-1
    else:
        d2 = (x-xref[min_id-1])**2+(y-yref[min_id-1])**2
        d1 = (x-xref[min_id+1])**2+(y-yref[mid_id+1])**2
        d_next = [d2, d1]
        next_id = min_id + 2*d_next.index(min(d_next)) - 1
    n1 = xref[next_id] - xref[min_id]
    n2 = yref[next_id] - yref[min_id]
    nnorm = p.sqrt(n1**2+n2**2)
    n1 = n1 / nnorm
    n2 = n2 / nnorm
    difx = x-xref[min_id]
    dify = y-yref[min_id]
    dot = difx*n1 + dify*n2
    deltax = difx - dot*n1
    deltay = dify - dot*n2
    return deltax**2+deltay**2

v_ref = 3
now = time.time()

p = GEKKO(remote=False)
p.time = np.linspace(0,10,21)
x = p.Var(value=0)
y = p.Var(value=16)
vx = p.Var(value=1)
vy = p.Var(value=0)
ax = p.Var(value=0)
ay = p.Var(value=0)
p.options.IMODE = 6
p.options.SOLVER = 3
p.options.WEB = 0

x_refg = p.Param(value=x_ref)
y_refg = p.Param(value=y_ref)

x_refg = p.Param(value=x_ref)
y_refg = p.Param(value=y_ref)
v_ref = p.Const(value=v_ref)


p.Obj(distfunc(x,y,x_refg,y_refg,p))
p.Obj( (p.sqrt(vx**2+vy**2) - v_ref)**2 + ax**2 + ay**2)

p.Equation(x.dt()==vx)
p.Equation(y.dt()==vy)
p.Equation(vx.dt()==ax)
p.Equation(vy.dt()==ay)
p.Equation(y>=1.5*x+4)
p.solve(disp=False, debug=True)
print(f'run time: {time.time()-now}')

plt.plot(x_ref, y_ref)
plt.plot(x_bound, y_bound)
plt.plot(x1.value,x2.value)
plt.show()

This is the result that I get. 如您所见，它并不完全是人们所期望的解决方案。有关您可能期望的解决方案的参考，here is what I get using the cost function below

p.Obj((x-x_refg)**2 + (y-y_refg)**2 + ax**2 + ay**2)

但是，由于我真正想要的是到这些点描述的路径的最短距离，我希望 distfunc 更接近我想要的，因为最短距离最有可能到某个插值点。所以我的问题是双重的：

1. 这是目标函数的正确 gekko 表达式/公式吗？
2. 我的另一个目标是解决问题的速度，那么有没有更有效的方式来为 gekko 表达这个问题？

Answer 1

除非您插入连续可微分的逻辑条件，例如if2 或if3 函数，否则您无法定义根据条件变化的目标函数。 Gekko 对符号模型进行一次评估，然后将其传递给可执行文件以进行求解。它只调用一次 Python 模型构建，因为它将模型编译为高效的字节码以供执行。您可以看到使用p.open_folder() 创建的模型。模型文件以apm 扩展名结尾：gk_model0.apm。

Model
Constants
    i0 = 3
End Constants
Parameters
    p1
    p2
    p3
    p4
End Parameters
Variables
    v1 = 0
    v2 = 16
    v3 = 1
    v4 = 0
    v5 = 0
    v6 = 0
End Variables
Equations
    v3=$v1
    v4=$v2
    v5=$v3
    v6=$v4
    v2>=(((1.5)*(v1))+4)
    minimize (((((v1-0.0)-((((((v1-0.0))*((0.2/sqrt(0.04159999999999994))))+(((v2-16.0))&
             *((-0.03999999999999915/sqrt(0.04159999999999994))))))*&
             ((0.2/sqrt(0.04159999999999994))))))^(2))+((((v2-16.0)&
             -((((((v1-0.0))*((0.2/sqrt(0.04159999999999994))))+(((v2-16.0))&
             *((-0.03999999999999915/sqrt(0.04159999999999994))))))&
             *((-0.03999999999999915/sqrt(0.04159999999999994))))))^(2)))
    minimize (((((sqrt((((v3)^(2))+((v4)^(2))))-i0))^(2))+((v5)^(2)))+((v6)^(2)))
End Equations

End Model

一种策略是将您的问题拆分为多个优化问题，这些问题都是minimal time problems，您可以在其中导航到第一个航路点，然后重新初始化问题以导航到第二个航路点，依此类推。如果您想保持动量并预测转向，则需要使用更高级的方法，例如 Pigeon / Eagle tracking problem（参见 source files）或类似于 trajectory optimization with UAVs 或 HALE 无人机（参见下面的参考资料）。

• Martin, R.A., Gates, N., Ning, A., Hedengren, J.D., Dynamic Optimization of High-Altitude Solar Aircraft Trajectories Under Station-Keeping Constraints, Journal of Guidance, Control, and Dynamics, 2018, doi: 10.2514/1.G003737。
• Gates, N.S., Moore, K.R., Ning, A., Hedengren, J.D., Combined Trajectory, Propulsion and Battery Mass Optimization for Solar-Regenerative High-Altitude Long Endurance Unmanned Aircraft, AIAA 科技论坛 (SciTech), 2019。