约束在 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 

同时更改最小化的初始猜测，如下所示

希望对你有帮助