Question

我要根据与障碍物的距离来优化一组点，因此，如果其中一个点比某个阈值MAX_DIST更接近障碍物，我希望将其从该障碍物移开（ Dem.Dolgov撰写的论文“未知半结构化环境中自动驾驶汽车的路径规划” ）。

为此，我正在使用Scipy库及其optimize模块。因为我有一组要优化的[x，y]点，所以我将创建一个状态向量[x0，y0，x1，y1，x2，y2，...，xN，yN]并将其馈入优化器。我的成本函数定义为：

其中sigma0是简单的二次惩罚函数，而dmax是MAX_DIST。雅可比定律定义为：

| xi-oi | 0 。

我已经预先计算了一张地图，其中在地图上的每个位置我都知道到最近障碍物的距离矢量。为了进行测试，我绘制了一组接近障碍物的点，即使在每个点处获得的雅可比点都远离障碍物，优化器也不会移动我的点。我附上一张图以可视化要解决的问题（蓝色点是远离黑线的点）。

最小示例代码如下：

from scipy import optimize
import numpy as np

MAX_DIST = 20

class Control():

    def __init__(self):
        self.wObstacle = 1

    def obstacleCost(self):
        # distance_vector is an array of [x_i-x_object, y_i-y_object, distance to object]
        distance_vector = np.array([0, -15, 15])
        if distance_vector[-1] < MAX_DIST:
            return self.wObstacle * (distance_vector[-1] - MAX_DIST) ** 2
        return 0

    def jacobianObstacle(self):
        # distance_vector is an array of [x_i-x_object, y_i-y_object, distance to object]
        distance_vector = np.array([0, -15, 15])
        if distance_vector[-1] < MAX_DIST:
            return self.wObstacle * 2 * (distance_vector[-1] - MAX_DIST) * distance_vector[:-1] / distance_vector[
                -1]
        return np.array([0, 0])

    def cost_function(self, state_vector):
        cost = 0
        for i in range(2, int(len(state_vector) / 2) - 2):  # first and last <x,y> points remain untouched
            cost += self.obstacleCost()
        return cost

    def jacobian(self, state_vector):
        gradient_array = np.zeros(len(state_vector))
        for i in range(2, int(len(state_vector) / 2) - 2):  # first and last <x,y> points remain untouched
            gradient_array[2 * i:2 * (i + 1)] += self.jacobianObstacle()

        return gradient_array

    def conjugate_gradient(self):
        states_extended = np.array([-5., 28., -4.08860759, 28., -3.17721519,
                                    28., -2.26582278, 28., -1.35443038, 28.,
                                    -0.44303797, 28., 0.46835443, 28., 1.37974684,
                                    28., 2.29113924, 28., 3.20253165, 28.,
                                    4.11392405, 28., 5.02531646, 28., 5.93670886,
                                    28., 6.84810127, 28., 7.75949367, 28.,
                                    8.67088608, 28., 9.58227848, 28., 10.49367089,
                                    28., 11.40506329, 28., 12.3164557, 28.,
                                    13.2278481, 28., 14.13924051, 28., 15.05063291,
                                    28., 15.96202532, 28., 16.87341772, 28.,
                                    17.78481013, 28., 18.69620253, 28., 19.60759494,
                                    28., 20.51898734, 28., 21.43037975, 28.,
                                    22.34177215, 28., 23.25316456, 28., 24.16455696,
                                    28., 25.07594937, 28., 25.98734177, 28.,
                                    26.89873418, 28., 27.81012658, 28., 28.72151899,
                                    28., 29.63291139, 28., 30.5443038, 28.,
                                    31.4556962, 28., 32.36708861, 28., 33.27848101,
                                    28., 34.18987342, 28., 35.10126582, 28.,
                                    36.01265823, 28., 36.92405063, 28., 37.83544304,
                                    28., 38.74683544, 28., 39.65822785, 28.,
                                    40.56962025, 28., 41.48101266, 28., 42.39240506,
                                    28., 43.30379747, 28., 44.21518987, 28.,
                                    45.12658228, 28., 46.03797468, 28., 46.94936709,
                                    28., 47.86075949, 28., 48.7721519, 28.,
                                    49.6835443, 28., 50.59493671, 28., 51.50632911,
                                    28., 52.41772152, 28., 53.32911392, 28.,
                                    54.24050633, 28., 55.15189873, 28.])
        # states_extended is an array of <x_0,y_0,x_1,y_1,...,x_N,y_N>
        result= optimize.fmin_cg(self.cost_function, states_extended, fprime=self.jacobian,
                                     maxiter=50)

if __name__ == "__main__":
    conjugate_object = Control()
    conjugate_object.conjugate_gradient()

在我的原始代码中，距离矢量数组取决于点的坐标，但是在这里，我将其设置为常数。在这里，我使用optimize.fmin_cg但

result = optimize.minimize(self.cost_function, states_extended, method="BFGS",jac=self.jacobian)

提供相同的结果。

Warning: Desired error not necessarily achieved due to precision loss.
         Current function value: 1575.000000
         Iterations: 0
         Function evaluations: 81
         Gradient evaluations: 69

即使简单的问题，Scipy优化也不会优化

0 个答案: