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

这是我得到的结果。如您所见，它不完全是人们应该期望的解决方案。为了参考您可能期望的解决方案，这是我使用下面的成本函数得到的

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

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

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

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无人机的轨迹优化(见下面的参考资料(。

• Martin， R.A.， Gates， N.， Ning， A.， Hedengren， J.D.， 高空太阳能飞机轨迹在站保持约束下的动态优化， 制导、控制和动力学杂志， 2018， doi： 10.2514/1.G003737.
• 盖茨，N.S.，摩尔，K.R.，宁，A.，Hedengren，法学博士，太阳能再生高空长航时无人机的组合轨迹，推进和电池质量优化，AIAA科学技术论坛(SciTech(，2019年。