我试图找到避免了启动和使用方距离法的终点之间有障碍物的最小路径。
要做到这一点,我定义了起点和终点之间的n个点 - 和计算最佳的路径和直线路径之间的平方距离。优化的路径必须是在距障碍物的最小距离。将得到的优化的路径是最优化的和直线路径之间的最小平方距离。
我已经实现的代码如下,但优化过程中,我收到以下错误:
不能从广播形状(27)进入形状输入阵列(27.3)
它看起来像Scipy.minimize改变从3-d阵列的阵列到1D阵列的形状。能否请您提出任何建议,以解决这个问题?
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, projection='3d')
## Setting Input Data:
startPoint = np.array([1,1,0])
endPoint = np.array([0,8,0])
obstacle = np.array([5,5,0])
## 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],normVector[2]*(n+1)*stepSize+startPoint[2]]
pointCoordinates.append(point)
DOFCoordinates = np.array(pointCoordinates)
## Assign a random z value for the DOF coordinates - change later:
for coordinate in range(len(DOFCoordinates)):
DOFCoordinates[coordinate][2] = random.uniform(-1.0, -0.0)
##ax.scatter(DOFCoordinates[coordinate][0],DOFCoordinates[coordinate][1],DOFCoordinates[coordinate][2])
## function to calculate the squared residual:
def distance(a,b):
dist = ((a[0]-b[0])**2 + (a[1]-b[1])**2 + (a[2]-b[2])**2)
return dist
## Get Straight Path Coordinates:
def straightPathCoordinates(DOF):
allCoordinates = np.zeros((2+len(DOF),3))
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):
print 'DOF',DOF
allCoordinates = np.zeros((2+len(DOF),3))
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 = setDOFCoordinates(DOF)
print DOF
sumDistance = 0.0
for coordinate in range(len(pathPositions)):
squaredDistance = distance(newCoordinates[coordinate],pathPositions[coordinate])
sumDistance += squaredDistance
return sumDistance
## Constraints: all coordinates need to be away from an obstacle with a certain distance:
constraint = []
minimumDistanceToObstacle = 0
for coordinate in range(len(DOFCoordinates)+2):
cons = {'type': 'ineq', 'fun': lambda DOF: minimumDistanceToObstacle-((obstacle[0] - setDOFCoordinates(DOF)[coordinate][0])**2 +(obstacle[1] - setDOFCoordinates(DOF)[coordinate][1])**2+(obstacle[2] - setDOFCoordinates(DOF)[coordinate][2])**2)}
constraint.append(cons)
## Get Initial Guess:
starting_guess = DOFCoordinates
## Run the minimization:
objectiveFunction = lambda DOF: f(DOF)
result = minimize(objectiveFunction,starting_guess,constraints=constraint, method='COBYLA')
print result.x
print DOFCoordinates
ax.plot([startPoint[0],endPoint[0]],[startPoint[1],endPoint[1]],[startPoint[2],endPoint[2]])
ax.scatter(obstacle[0],obstacle[1],obstacle[2])
所期望的结果是一组点和点A和点B之间它们的位置能避免障碍物,并返回的最小距离。
这是因为输入,降低与一维数组工作,
从SciPy的notes,
目标函数最小化。
fun(x, *args) -> float
其中x是与形状(N)和args的1-d阵列是完全指定的函数所需要的固定的参数的元组。
X0:ndarray,形状(N,)
最初的猜测。大小的实际元件的阵列(N),其中“n”是独立变量的数目。
这意味着你应该输入使用starting_guess.ravel()
并更改setDOFCoordinates
与作为1维数组工作。