Question

我试图找到两点（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()

预期结果是一条连接起点和终点以及找到起点和终点之间满足障碍约束的点的路径（注意：它可能是弯曲的路径）。

Answer 1

如果新坐标靠近障碍物，则可以通过惩罚平方距离直接将约束添加到目标函数。

def f(DOF):
    newCoordinates = GetNewCoordinates(setDOFCoordinates(DOF))
    sumDistance = 0.0
    for coordinate in range(len(pathPositions)):
        squaredDistance = distance(newCoordinates[coordinate],pathPositions[coordinate])

        des1=distance(newCoordinates[coordinate],obstacle)

        if des1<=minimumDistanceToObstacle:

            sumDistance += squaredDistance+1000000

        else:

            sumDistance += squaredDistance

   return sumDistance

还可以如下更改最小化的初始猜测

希望有帮助

在Scipy中不遵守约束。

1 个答案: