用python进行离散路径跟踪

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()

正如您所看到的，这就是我得到的结果。并不完全是人们所期望的解决方案。有关您可能期望的解决方案的参考，以下是我使用下面的成本函数得到的信息

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

然而，由于我实际上想要的是到由这些点描述的路径的最短距离，所以我期望距离更接近我想要的，因为最短的距离最有可能是一些插值点。所以我的问题有两个：

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

一种策略是将问题分解为多个优化问题，这些优化问题都是最小时间问题，其中您导航到第一个路径点，然后重新初始化问题以导航到第二个路径点，依此类推。如果您想要保持势头并预期转向，那么您将需要使用更高级的方法，如鸽/鹰跟踪问题 (参见源文件)或类似于无人机弹道优化或HALE (参见下面的参考资料)。

• 马丁，R.A.，盖茨，N.，宁，A.，赫登伦，J.D.，站位约束下高空太阳能飞机飞行轨迹的动态优化，“指导、控制和动力杂志”，2018年，doi: 10.2514/1.G003737。
• 盖茨，N.S.，摩尔，K.R.，宁，A.，赫登格伦，J.D.，太阳能再生高空长耐久无人机的组合轨迹、推进和电池质量优化，美国宇航科学院科学技术论坛(SciTech)，2019年。