为什么用于避障的MILP求解器仍然通过障碍物？

问：

我必须扩展 build_solver 函数以接受障碍物列表：build_solver（umax， x0， T， num_steps， obstacles=[]）。这要求我需要添加额外的约束，以确保机器人不会进入障碍物（始终如此）。我必须通过添加一个额外的“松弛变量”来做到这一点。此外，我必须确保在每个时间步骤都需要避开障碍物;我需要为 xs 中的每个 x 添加约束。当障碍物列表为空时，不应添加任何约束。

所以，我做了下面的build_solver函数（其余函数不应该改变），但为什么它仍然通过障碍物呢？

# Starter Code: 2D Trajectory Optimization
import matplotlib.patches as patches
import matplotlib.pyplot as plt
import numpy as np
from ortools.linear_solver import pywraplp


def vec_add(vec_a, vec_b):
    return [ai + bi for ai, bi in zip(vec_a, vec_b)]


def dot(vec_a, vec_b):
    return sum([av * bv for av, bv in zip(vec_a, vec_b)])


def matmul(mat, vec):
    return [dot(mat_r, vec) for mat_r in mat]


def get_propagation_matrices(dt):
    # Define the matrices (matrices from class)
    Ad = np.array([[1, 0, dt, 0],
         [0, 1, 0, dt],
         [0, 0, 1, 0],
         [0, 0, 0, 1]])
    dt2 = dt*dt/2
    Bd = np.array([

        [dt2, 0, -dt2, 0],
        [0, dt2, 0, -dt2],
        [dt, 0, -dt, 0],
        [0, dt, 0, -dt]])
    return Ad, Bd



    
def build_solver(umax, x0, T, num_steps, obstacles=[]):
    solver = pywraplp.Solver.CreateSolver('SCIP')
    infinity = solver.infinity()
    dt = T / num_steps
    Ad, Bd = get_propagation_matrices(dt)
    
    # Define the control variables
    us = [[solver.NumVar(0, umax, f'u_{step}_{side}')
            for side in range(4)] for step in range(num_steps)]
    
    # Define the slack variables for obstacle avoidance
    slack_vars = [[solver.NumVar(0, infinity, f'slack_{step}_{obs}') 
                   for obs in range(len(obstacles))]
                  for step in range(num_steps)]
    
    # Compute each state from the initial state and the thrust values
    x = x0
    xs = [x]
    for step in range(num_steps):
        x_next = vec_add(matmul(Ad, x), matmul(Bd, us[step]))
        xs.append(x_next)
        
        # Constraints for obstacle avoidance
        for obs_idx, obstacle in enumerate(obstacles):
            x_obs = obstacle[0]  # x-coordinate of the obstacle
            y_obs = obstacle[1]  # y-coordinate of the obstacle
            width_obs = obstacle[2]  # width of the obstacle
            height_obs = obstacle[3]  # height of the obstacle
            
            # Ensure the robot stays outside the obstacle region using slack variables
            for dim in range(2):  # Loop through x and y dimensions
                solver.Add(xs[step][dim] + slack_vars[step][obs_idx] >= x_obs + width_obs)
                solver.Add(xs[step][dim] - slack_vars[step][obs_idx] <= x_obs)
                solver.Add(xs[step][dim + 2] + slack_vars[step][obs_idx] >= y_obs + height_obs)
                solver.Add(xs[step][dim + 2] - slack_vars[step][obs_idx] <= y_obs)
    
        x = x_next
    
    # Add the targets for the final state and thrust
    for dim in range(4):
        solver.Add(xs[-1][dim] == 0)
    
    # Constraints on the amount of thrust and non-negativity
    for step in range(num_steps):
        for side in range(4):
            solver.Add(us[step][side] <= umax)
            solver.Add(us[step][side] >= 0)  # Ensure non-negativity
    
    solver.Minimize(dt * sum([sum(u) for u in us]))
    
    return solver, us


def replace_and_plot(x0, us, solver, T, num_steps, obstacles=[]):
    """Substitute solved values and plot."""
    x = np.array(x0)
    dt = T / num_steps
    Ad, Bd = get_propagation_matrices(dt)
    xsv = [x]
    usv =  [[us[step][side].solution_value() for side in range(4)] for step in range(num_steps)]
    for uv in usv:
        x = vec_add(matmul(Ad, x), matmul(Bd, uv))
        xsv.append(x)

    objective_value = solver.Objective().Value()
    print(f'Objective value = {objective_value}')

    fig, ax = plt.subplots()
    xsv = np.array(xsv)
    ax.plot(xsv[:, 0], xsv[:, 1], 'k', alpha=0.1)
    ax.scatter(xsv[:, 0], xsv[:, 1], c=list(range(num_steps+1)), s=1)
    ax.set_aspect('equal', 'box')
    
    for ob in obstacles:
        ax.add_patch(patches.Rectangle(
            (ob[0], ob[2]), ob[1] - ob[0], ob[3] - ob[2],
            linewidth=1, edgecolor='k', facecolor='none'))

    return objective_value

# Trajectory Solving (with obstacles): Results and Plotting

num_steps = 50
T = 25
umax = 0.4
x0_withV = [2, 3, 1, -2]
x0_noV = [2, 3, 0, 0]

# obstacle = [xmin, xmax, ymin, ymax]
obstacles = [[1, 2, -2.5, 2.5]]

solver, us = build_solver(umax, x0_withV, T, num_steps, obstacles)
status = solver.Solve()
replace_and_plot(x0_withV, us, solver, T, num_steps, obstacles)
plt.title("V not 0")

solver, us = build_solver(umax, x0_noV, T, num_steps, obstacles)
status = solver.Solve()
replace_and_plot(x0_noV, us, solver, T, num_steps, obstacles)
plt.title("V = 0")

solver, us = build_solver(umax, x0_withV, T, num_steps, obstacles=[])
status = solver.Solve()
replace_and_plot(x0_withV, us, solver, T, num_steps, obstacles=[])
plt.title("V not 0")

solver, us = build_solver(umax, x0_noV, T, num_steps, obstacles=[])
status = solver.Solve()
replace_and_plot(x0_noV, us, solver, T, num_steps, obstacles=[])
plt.title("V = 0")

None

o 在此处输入图像描述 在此处输入图像描述