我试图在避开障碍物的同时找到两点(A,B)之间的最小路径。为了得到这个,我试图找到连接A和B之间n个点的最小平方距离
我设计最小化函数的方法是找到a和b之间所有n个点的最佳位置,这些点返回最小平方距离并满足约束条件
下面显示的代码使用Scipy.minimize,但例程似乎不满足障碍约束。下面的代码显示最小化成功收敛,但我可以看到结果通过了障碍
非常感谢你的帮助
import numpy as np
import matplotlib.pyplot as plt
import random
from mpl_toolkits.mplot3d import Axes3D
from scipy.optimize import minimize
fig = plt.figure()
ax = fig.add_subplot(111)
## Setting Input Data:
startPoint = np.array([0,0])
endPoint = np.array([8,8])
obstacle = np.array([4,4])
## Get degree of freedom coordinates based on specified number of segments:
numberOfPoints = 10
pipelineStraightVector = endPoint - startPoint
normVector = pipelineStraightVector/np.linalg.norm(pipelineStraightVector)
stepSize = np.linalg.norm(pipelineStraightVector)/numberOfPoints
pointCoordinates = []
for n in range(numberOfPoints-1):
point = [normVector[0]*(n+1)*stepSize+startPoint[0],normVector[1]*(n+1)*stepSize+startPoint[1]]
pointCoordinates.append(point)
DOFCoordinates = np.array(pointCoordinates)
def initialGuess(DOFCoordinates):
numberOfDofCoordinates = len(DOFCoordinates)
vecLength = 2 * numberOfDofCoordinates
dofs = np.zeros(vecLength)
dofs[:numberOfDofCoordinates] = DOFCoordinates[:,0]
dofs[numberOfDofCoordinates:2*numberOfDofCoordinates] = DOFCoordinates[:,1]
return dofs
## function to calculate the squared residual:
def distance(a,b):
dist = ((a[0]-b[0])**2 + (a[1]-b[1])**2 )
return dist
## Get Straight Path Coordinates:
def straightPathCoordinates(DOF):
allCoordinates = np.zeros((2+len(DOF),2))
allCoordinates[0] = startPoint
allCoordinates[1:len(DOF)+1]=DOF
allCoordinates[1+len(DOF)]=endPoint
return allCoordinates
pathPositions = straightPathCoordinates(DOFCoordinates)
## Set Degree of FreeDom Coordinates during optimization:
def setDOFCoordinates(DOF):
numberOfDofCoordinates = len(DOFCoordinates)
dofCoordinates = np.zeros((numberOfDofCoordinates,2))
dofCoordinates[:,0] = DOF[:numberOfDofCoordinates]
dofCoordinates[:,1] = DOF[numberOfDofCoordinates:2*numberOfDofCoordinates]
return dofCoordinates
def GetNewCoordinates(DOF):
numberOfDofCoordinates = len(DOFCoordinates)
allCoordinates = np.zeros((2+numberOfDofCoordinates,2))
allCoordinates[0] = startPoint
allCoordinates[1:len(DOF)+1]=DOF
allCoordinates[1+len(DOF)]=endPoint
return allCoordinates
## Objective Function: Set Degree of FreeDom Coordinates and Get Square Distance between optimized and straight path coordinates:
def f(DOF):
newCoordinates = GetNewCoordinates(setDOFCoordinates(DOF))
sumDistance = 0.0
for coordinate in range(len(pathPositions)):
squaredDistance = distance(newCoordinates[coordinate],pathPositions[coordinate])
sumDistance += squaredDistance
return sumDistance
minimumDistanceToObstacle = 2
## Constraints: all coordinates need to be away from an obstacle with a certain distance:
constraint = []
for coordinate in range(len(DOFCoordinates)+2):
cons = {'type': 'ineq', 'fun': lambda DOF: np.sqrt((obstacle[0] - GetNewCoordinates(setDOFCoordinates(DOF))[coordinate][0])**2 +(obstacle[1] - GetNewCoordinates(setDOFCoordinates(DOF))[coordinate][1])**2) - minimumDistanceToObstacle}
constraint.append(cons)
## Get Initial Guess:
starting_guess = initialGuess(DOFCoordinates)
## Run the minimization:
objectiveFunction = lambda DOF: f(DOF)
result = minimize(objectiveFunction,starting_guess,constraints=constraint, method='COBYLA')
newLineCoordinates = GetNewCoordinates(setDOFCoordinates(result.x))
print newLineCoordinates
print pathPositions
print result
ax.plot([startPoint[0],endPoint[0]],[startPoint[1],endPoint[1]],color='grey')
ax.scatter(obstacle[0],obstacle[1],color='red')
for coordinate in range(len(newLineCoordinates)-1):
firstPoint = newLineCoordinates[coordinate]
secondPoint = newLineCoordinates[coordinate+1]
ax.plot([firstPoint[0],secondPoint[0]],[firstPoint[1],secondPoint[1]],color='black',linewidth=2)
ax.scatter(firstPoint[0],firstPoint[1])
ax.text(firstPoint[0],firstPoint[1],str(firstPoint[0])+','+str(firstPoint[1]))
plt.show()
预期结果是一条连接起点和终点的路径(注意:它可能是一条曲线路径),以及找到满足障碍物约束的起点和终点之间的点
如果新坐标靠近障碍物,可以通过惩罚平方距离将约束直接添加到目标函数
还可以按如下方式更改最小化的初始猜测
希望能有帮助
相关问题 更多 >
编程相关推荐