避开障碍物时路径优化错误

Question

我正在尝试使用平方距离法找到避开起点和终点之间障碍物的最小路径。

为此，我在起点和终点之间定义了 n 个点 - 并计算了优化路径和直线路径之间的平方距离。优化路径必须与障碍物保持最小距离。生成的优化路径是优化路径和直线路径之间的最小平方距离。

我已经实现了如下代码，但是在优化过程中，我收到以下错误：

无法将形状 (27) 中的输入数组广播到形状 (27,3)

看起来 Scipy.minimize 将数组的形状从 3 维数组更改为 1 维数组。您能否提出任何解决此问题的建议？


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点之间避开障碍物的位置和returns最小距离。

Answer 1

这是因为要最小化的输入必须使用一维数组，

来自scipynotes,

The objective function to be minimized.
 fun(x, *args) -> float
where x is an 1-D array with shape (n,) and args is a tuple of the fixed parameters needed to completely specify the function.

x0 : ndarray, shape (n,)

Initial guess. Array of real elements of size (n,), where ‘n’ is the number of independent variables.

这意味着您应该在输入中使用 starting_guess.ravel() 并更改 setDOFCoordinates 以使用一维数组。

避开障碍物时路径优化错误

Error in Path optimization while avoiding an obstacle

optimization

numpy

minimization

path-finding

scipy