非线性优化 python

# coding=utf-8 from scipy.optimize import minimize from scipy.optimize import NonlinearConstraint import matplotlib.pyplot as plt import numpy as np import math p_p= np.zeros((3000, 2), float) p_e= np.zeros((3000, 2), float) dis= np.zeros((3000, 1), float) X_p= np.zeros((3000, 2), float) Y_p= np.zeros((3000, 1), float) p_p[0]=[4,1] p_e[0]=[10,4] x0=[10,4] v_e=-0.7 v_p=1 r=0.4 a=1.43 t=0 dt=0.01 i=1 # 目标函数 def fun(): def v(x): return 1*x[0]+0*x[1] return v #限制条件函数 def con(a,r): def v(x): return (np.sqrt(math.pow(x[0]-p_p[i-1][0],2)+math.pow(x[1]-p_p[i-1][1],2))-a*np.sqrt(math.pow(x[0]-p_e[i-1][0],2)+math.pow(x[1]-p_e[i-1][1],2))-r) return v def dist(p1,p2): return np.sqrt(math.pow(p1[0]-p2[0],2)+math.pow(p1[1]-p2[1],2)) if __name__ == "__main__": dis[0]=dist(p_e[0],p_p[0]) cons = ({'type': 'ineq', 'fun': con(a,r)}) for i in range(1000): res = minimize(fun(), x0, constraints=cons) t=t+dt p_e[i]=[10+v_e*t,4] dx=res.x[0]-p_p[i-1][0] dy=res.x[1]-p_p[i-1][1] costh=dx/np.sqrt(math.pow(dx,2)+math.pow(dy,2)) sinth=dy/np.sqrt(math.pow(dx,2)+math.pow(dy,2)) p_p[i]=[p_p[i-1][0]+v_p*costh*dt,p_p[i-1][1]+v_p*sinth*dt] dis[i]=dist(p_e[i],p_p[i]) x0=p_e[i] if (dis[i]<0.4): break for i in range(2500): if (dis[i]!=0): print(dis[i]) # for i in range(2500): # if (p_p[i][0]!=0): # X_p[i]=p_p[i][0] # Y_p[i]=p_p[i][1] # plt.plot(X_p,Y_p) # plt.show # print(res.fun) # print(res.success) # print(res.x)