在Scipy.minimize中没有遵守约束

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 

还要将最初的猜测更改为最小化，如下所示

希望能帮助到你