Ошибка в оптимизации пути при обходе препятствия - PullRequest
Новая
       67

Ошибка в оптимизации пути при обходе препятствия

0 голосов
/ 12 февраля 2019

Я пытаюсь найти минимальный путь, чтобы избежать препятствия между начальной и конечной точкой, используя метод квадрата расстояния.

Для этого я определяю n точек между началом и концом - и вычисляю квадрат расстояния между оптимизированным и прямым путем.Оптимизированный путь должен быть на минимальном расстоянии от препятствия.Результирующий оптимизированный путь - это наименьшее квадратное расстояние между оптимизированным и прямым путем.

Я реализовал код следующим образом, но во время оптимизации я получаю следующую ошибку:

не удалось преобразовать входной массив из shape (27) в shape (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, которые избегают препятствия и возвращают минимальное расстояние.

1 Ответ

0 голосов
/ 12 февраля 2019

Это потому, что входы для минимизации должны работать с массивами 1D,

Из scipy примечания ,

Целевая функция, которую нужно минимизировать. 

 fun(x, *args) -> float

где x - это одномерный массив с формой (n,), а args - набор фиксированных параметров, необходимых для полного указания функции.

x0: ndarray, shape (n,)

Первоначальное предположение.Массив вещественных элементов размера (n,), где 'n' - количество независимых переменных.

Это означает, что вы должны использовать starting_guess.ravel() на входе и изменить setDOFCoordinates для работыс массивами, которые являются одномерными.

...