本文介绍一种针对基于冲突的路径规划问题（conflict-based search, CBS）的整数规划模型，并提供调用 OR-Tools 和 SCIP 求解器的代码。

文章目录

模型部分
代码部分
- 文件结构
- 具体代码
- - agent.py
  - model.py
  - visual.py
  - main.py
- 运行效果
彩蛋：有关禁止相邻智能体交换位置的约束

模型部分

参数

参数	含义
$I$	所有智能体
$P$	所有位置
$T$	时间限制
$t_0$	初始时刻
$s_i$	智能体 $i$ 的初始位置
$g_i$	智能体 $i$ 的目标位置
$N_p$	位置 $p$ 的所有相邻位置

决策变量

变量	类型	含义
$u_{i, p, t}$	${ 0, 1 \}$	智能体 $i$ 在时刻 $t$ 位于位置 $p$

约束条件

(1) 初始位置
$u_{i, s_i, t_0} = 1 \quad \forall i \in I$

(2) 每个智能体在每个时刻有且仅有一个位置
$\sum_{p \in P} u_{i, p, t} = 1 \quad \forall i \in I \quad \forall t \leq T$

(3) 移动方向
$u_{i, p, t} \leq \sum_{q \in N_p} u_{i, q, t + 1} \quad \forall i \in I \quad \forall t < T \quad \forall p \in P$

(4) 到达终点后不再移动
$u_{i, g_i, t} \leq u_{i, g_i, t + 1} \quad \forall i \in I \quad \forall t < T$

(5) 每个位置在每个时刻至多有一个智能体
$\sum_{i \in I} u_{i, p, t} \leq 1 \quad \forall i \in I \quad \forall p \in P$

(6) 只有一个智能体离开当前位置后，才允许另一个智能体进入
（更严谨的约束方式是禁止两个相邻的智能体对撞交换位置，但是较难线性化，且需要引入较多变量，故简化为该约束）
$u_{i, p, t} + u_{j, p, t + 1} \leq 1 \quad \forall i \in I \quad j \in I \quad \forall p \in P \quad \forall t < T$

目标函数

$\max \ \ \sum_{i \in I} \sum_{t \leq T} u_{i, g_i, t}$

代码部分

文件结构

具体代码

agent.py

from typing import Tuple


class Agent(object):
    """
    agent
    """
    def __init__(self, id: int, init_position: Tuple[int, int], destination: Tuple[int, int], colour: str = 'green') -> None:
        """
        initialise
        
        :param id:  agent ID
        :param init_position:  start point
        :param destination:  target point
        :param colour:  visual colour
        """

        self.id = id

        self.init_position = init_position
        self.destination = destination

        self.colour = colour

model.py

from datetime import datetime
import os
from typing import Tuple, List, Dict

from ortools.linear_solver import pywraplp
from pyscipopt import Model, quicksum

from agent import Agent


class IPModel(object):
    """
    CBS IP model
    """
    def __init__(self, agents: List[Agent], width: int, height: int, time_limit: int, solver: str = 'scip') -> None:
        """
        model initialise
        
        :param agents:  all agents
        :param width:  map width
        :param height:  map height
        :param time_limit:  time limit of agent moving
        :param solver:  IP model solver
        """

        self.agents = agents

        self.width = width
        self.height = height
        self.time_limit = time_limit
        self.solver = solver

        self.ids = [agent.id for agent in self.agents]
        self.init_positions = {
    
    agent.id: agent.init_position for agent in self.agents}
        self.destinations = {
    
    agent.id: agent.destination for agent in self.agents}

        self.routes = {
    
    }

    def run(self):
        """
        run model
        """

        self.routes = self._run_ortools() if self.solver == 'ortools' else self._run_scip()
    
    def _run_ortools(self) -> Dict[int, Dict[int, Tuple[int, int]]]:
        """
        solve by OR-Tools

        :return:  routes dictionary, {timestep: {agent id: position}}
        """

        solver = pywraplp.Solver('MAPF', problem_type=pywraplp.Solver.CBC_MIXED_INTEGER_PROGRAMMING)

        u = {
    
    (i, x, y, t): solver.BoolVar("u_[{},{},{},{}]".format(i, x, y, t)) 
             for i in self.ids for x in range(self.width) for y in range(self.height) for t in range(self.time_limit)}
        
        # constraint 1: initial positions
        for i in self.ids:
            solver.Add(u[i, self.init_positions[i][0], self.init_positions[i][1], 0] == 1)

        # constraint 2: each time position
        for i in self.ids:
            for t in range(self.time_limit):
                solver.Add(sum(u[i, x, y, t] for x in range(self.width) for y in range(self.height)) == 1)

        # constraint 3: move direction
        for i in self.ids:
            for t in range(self.time_limit - 1):
                for x in range(self.width):
                    for y in range(self.height):
                        # neighbour cells
                        neighbours = u[i, x, y, t + 1]  # idle
                        if x > 0:
                            neighbours += u[i, x - 1, y, t + 1]
                        if x < self.width - 1:
                            neighbours += u[i, x + 1, y, t + 1]
                        if y > 0:
                            neighbours += u[i, x, y - 1, t + 1]
                        if y < self.height - 1:
                            neighbours += u[i, x, y + 1, t + 1]
                        solver.Add(u[i, x, y, t] <= neighbours)

        # constraint 4: won't move if arrive destination
        for i in self.ids:
            for t in range(self.time_limit - 1):
                solver.Add(u[i, self.destinations[i][0], self.destinations[i][1], t] 
                           <= u[i, self.destinations[i][0], self.destinations[i][1], t + 1])
                
        # constraint 5: anti-collision
        for t in range(self.time_limit):
            for x in range(self.width):
                for y in range(self.height):
                    solver.Add(sum(u[i, x, y, t] for i in self.ids) <= 1)

        # constraint 6: tile cooldown
        # too difficult to forbid neighbour transfer
        for t in range(self.time_limit - 1):
            for x in range(self.width):
                for y in range(self.height):
                    for i in self.ids:
                        for j in self.ids:
                            if j != i:
                                solver.Add(u[i, x, y, t] + u[j, x, y, t + 1] <= 1)

        # objective: arrive as soon as possible
        solver.Maximize(sum(u[i, self.destinations[i][0], self.destinations[i][1], t] 
                            for i in self.ids for t in range(self.time_limit)))
        
        dts = datetime.now()
        status = solver.Solve()
        dte = datetime.now()
        tm = round((dte - dts).seconds + (dte - dts).microseconds / (10 ** 6), 3)
        print("OR-Tools GLOP model solving time:  {} s".format(tm), '\n')

        # case 1: optimal
        if status == pywraplp.Solver.OPTIMAL:
            print("solution:")
            obj_opt = solver.Objective().Value()
            print("objective value:  {}".format(round(obj_opt, 4)))

            # result process
            u_ = {
    
    (i, x, y, t): u[i, x, y, t].solution_value() for i in self.ids 
                  for x in range(self.width) for y in range(self.height) for t in range(self.time_limit)}
            routes = {
    
    }
            for t in range(self.time_limit):
                route = {
    
    }
                for i in self.ids:
                    for x in range(self.width):
                        for y in range(self.height):
                            if u_[i, x, y, t] > 0.9:
                                route[i] = (x, y)
                routes[t] = route

        # case 2: infeasible
        elif status == pywraplp.Solver.INFEASIBLE:
            print("Model infeasible!")
    
        # case 3: others
        else:
            print("unexpected status:  {}.".format(status))

        print()
        print("advanced usage:")
        print("problem solved in {} s.".format(solver.wall_time() / 1000))
        print("problem solved in {} iterations.".format(solver.iterations()))

        return routes

    def _run_scip(self) -> Dict[int, Dict[int, Tuple[int, int]]]:
        """
        solve by SCIP

        :return:  routes dictionary, {timestep: {agent id: position}}
        """

        model = Model("MAPF")
        
        u = {
    
    (i, x, y, t): model.addVar(vtype="B", name="u_[{},{},{},{}]".format(i, x, y, t)) 
             for i in self.ids for x in range(self.width) for y in range(self.height) for t in range(self.time_limit)}
        
        # constraint 1: initial positions
        for i in self.ids:
            model.addCons(u[i, self.init_positions[i][0], self.init_positions[i][1], 0] == 1, 
                          name="cons_1_[{}]".format(i))
            
        # constraint 2: each time position
        for i in self.ids:
            for t in range(self.time_limit):
                model.addCons(quicksum(u[i, x, y, t] for x in range(self.width) for y in range(self.height)) == 1, 
                              name="cons_2_[{},{}]".format(i, t))
                
        # constraint 3: move direction
        for i in self.ids:
            for t in range(self.time_limit - 1):
                for x in range(self.width):
                    for y in range(self.height):
                        # neighbour cells
                        neighbours = u[i, x, y, t + 1]  # idle
                        if x > 0:
                            neighbours = neighbours + u[i, x - 1, y, t + 1]  # caution: += not surported
                        if x < self.width - 1:
                            neighbours = neighbours + u[i, x + 1, y, t + 1]
                        if y > 0:
                            neighbours = neighbours + u[i, x, y - 1, t + 1]
                        if y < self.height - 1:
                            neighbours = neighbours + u[i, x, y + 1, t + 1]
                        model.addCons(u[i, x, y, t] <= neighbours, name="cons_3_[{},{},{},{}]".format(i, x, y, t))

        # constraint 4: won't move if arrive destination
        for i in self.ids:
            for t in range(self.time_limit - 1):
                model.addCons(u[i, self.destinations[i][0], self.destinations[i][1], t] 
                              <= u[i, self.destinations[i][0], self.destinations[i][1], t + 1], 
                              name="cons_4_[{},{}]".format(i, t))
                
        # constraint 5: anti-collision
        for t in range(self.time_limit):
            for x in range(self.width):
                for y in range(self.height):
                    model.addCons(
                        quicksum(u[i, x, y, t] for i in self.ids) <= 1, name="cons_5_[{},{},{}]".format(x, y, t))

        # constraint 6: tile cooldown
        # too difficult to forbid neighbour transfer
        for t in range(self.time_limit - 1):
            for x in range(self.width):
                for y in range(self.height):
                    for i in self.ids:
                        for j in self.ids:
                            if j != i:
                                model.addCons(u[i, x, y, t] + u[j, x, y, t + 1] <= 1, 
                                              name="cons_6_[{},{},{},{},{}]".format(i, j, x, y, t))
                            
        # objective: arrive as soon as possible
        model.setObjective(quicksum(u[i, self.destinations[i][0], self.destinations[i][1], t] 
                                    for i in self.ids for t in range(self.time_limit)), sense="maximize")
        
        # time limit
        model.setRealParam("limits/time", 1800)

        # solve
        model.optimize()

        # status
        status = model.getStatus()
        print("model status:  {}".format(status), '\n')
        if status != "optimal":
            return {
    
    }

        # optimal objective value
        obj_value = model.getObjVal()
        print("optimal objective value:  {}".format(obj_value), '\n')

        # result process
        u_ = {
    
    (i, x, y, t): model.getVal(u[i, x, y, t]) 
              for i in self.ids for x in range(self.width) for y in range(self.height) for t in range(self.time_limit)}
        routes = {
    
    }
        for t in range(self.time_limit):
            route = {
    
    }
            for i in self.ids:
                for x in range(self.width):
                    for y in range(self.height):
                        if u_[i, x, y, t] > 0.9:
                            route[i] = (x, y)
            routes[t] = route

        return routes

有关求解器的调用方法参见：

调用 OR-Tools GLOP 求解器的简单模型实例
 Python 调用 SCIP 求解器的选址模型代码示例
注意：Python 调用 SCIP 求解器时，不能使用 += 运算符

visual.py

可视化的代码，最终生成视频和动图，生成后可将中间生成的图片删除。

import os
from typing import Tuple, List, Dict

from matplotlib import pyplot as plt
from matplotlib.patches import Rectangle
import cv2
import glob
import imageio

from agent import Agent


def get_image_video(map_size: int, routes: Dict[int, Dict[int, Tuple[int, int]]], agents: List[Agent], 
                    delete_images: bool = False):
    """
    get images and video
    
    :param map_size:  map size
    :param routes:  routes dictionary, {timestep: {agent id: position}}
    :param agents:  all agents
    :param delete_images:  if delete images after video generated

    :return: nothing
    """

    path = './outputs/'

    # get images
    timesteps = sorted(list(routes.keys()))
    for t in timesteps:
        # initial map
        plt.figure(figsize=(5, 5))
        ax = plt.gca()
        ax.set_xlim([0, map_size])
        ax.set_ylim([0, map_size])
        for i in range(map_size):
            for j in range(map_size):
                rectangle = Rectangle(
                    xy=(i, j), width=1, height=1, edgecolor='gray', facecolor='white')
                ax.add_patch(rectangle)

        # draw agents positions
        route = routes[t]
        for i in route.keys():
            x, y = route[i]
            rectangle = Rectangle(xy=(x, y), width=1, height=1, color=agents[i - 1].colour)
            ax.add_patch(rectangle)
            plt.text(x=x + 0.4, y=y + 0.4, s=str(i), fontsize=20)

            # direction
            if t and route[i] != routes[t - 1][i]:
                coef_len = 0.75
                dx = (x - routes[t - 1][i][0]) * coef_len if x - routes[t - 1][i][0] != 0 else 0
                dy = (y - routes[t - 1][i][1]) * coef_len if y - routes[t - 1][i][1] != 0 else 0
                plt.arrow(x=routes[t - 1][i][0] + 0.5, y=routes[t - 1][i][1] + 0.5, dx=dx, dy=dy, head_width=0.05, facecolor='black')
        
        plt.title(label="timestep: {}".format(t))

        image_name = path + str(t) + '.png'
        plt.savefig(image_name)

    # get image files
    image_files = []
    file_names = []
    for file_name in glob.glob('./outputs/*.png'):
        file_names.append(file_name)
        image = cv2.imread(filename=file_name)
        height, width, layers = image.shape
        size = (width, height)
        image_files.append(image)

    # generate video
    video_path = f'./outputs/result.avi'
    fourcc = cv2.VideoWriter_fourcc(*'DIVX')
    video = cv2.VideoWriter(video_path, fourcc, 0.5, size)  # fps: 0.5
    for image in image_files:
        video.write(image)
    video.release()

    # generate gif
    gif_path = f'./outputs/result.gif'
    imageio.mimsave(uri=gif_path, ims=image_files, format='GIF', duration=0.5)

    # delete original image files
    if delete_images:
        for file in file_names:
            os.remove(file)

main.py

import rich

from agent import Agent
from model import IPModel
from visual import get_image_video


SIZE = 3
TIME_LIMIT = 10


# agents info
agents = []
for i in range(SIZE):
    agent = Agent(id=i + 1, init_position=(i, 0), destination=(i, SIZE - 1), colour='red')
    agents.append(agent)
for i in range(SIZE):
    agent = Agent(id=i + SIZE + 1, init_position=(i, SIZE - 1), destination=(i, 0), colour='blue')
    agents.append(agent)

# IP model
# solver = 'ortools'
solver = 'scip'
ip_model = IPModel(agents=agents, width=SIZE, height=SIZE, time_limit=TIME_LIMIT, solver=solver)
ip_model.run()
routes = ip_model.routes
print()
rich.print(routes)
print()

# visualise
get_image_video(map_size=SIZE, routes=routes, agents=agents, delete_images=True)

运行效果

以 $\times 3$ 的地图为例，红色智能体和蓝色智能体需要互换位置（对面互换），时间限制为 10，初始状态如下图所示：

求解结果如下，即各个时刻每个智能体的位置：
求解结果 1

结果动图如下：
结果动图 1

如果选用 OR-Tools 求解器，求解时间大概在 35-40s 左右；
如果选用 SCIP 求解器，求解时间约 8s。

如果把规模增加，地图改为 $\times 4$ ，时间限制为 16，目标位置逻辑不变，
当选用 OR-Tools 求解器时，求解时间大概在 150-300s 左右；
当选用 SCIP 求解器时，求解时间大概在 25s 左右。
求解结果如下：
求解结果 2
实际上，当地图变大时，由于阻塞减少，只需 6 个时间步数即可完成全部移动过程，若把时间限制减少为 10，则运行时间：
OR-Tools 约 70s；SCIP 在 10s 以内。

结果动图如下：
结果动图 2

彩蛋：有关禁止相邻智能体交换位置的约束

建模

增加一组辅助变量，用于标识某时刻两个智能体位于相邻位置：

变量	类型	含义
$v_{i, j, p, q, t}$	${ 0, 1 \}$	智能体 $i$ 和 $j$ 在时刻 $t$ 分别位于位置 $p$ 和 $q$

则禁止相邻智能体交换位置的约束为：
$u_{i, p, t} + u_{j, q, t} \geq 2 - v_{i, j, p, q, t} \cdot M \\ u_{i, p, t} + u_{j, q, t} \leq 1 + (1 - v_{i, j, p, q, t}) \cdot M \\ u_{i, q, t + 1} + u_{j, p, t + 1} \leq 1 + v_{i, j, p, q, t} \cdot M \\ \forall i \in I \quad \forall j \in I \quad i \neq j \quad \forall p \in P \quad \forall q \in N_p \quad \forall t < T$

其中， $M$ 为一个充分大的数。
增加上述约束后，去掉前面模型的约束 (6) 即可。

代码

由于 OR-Tools 求解效率低，使用 SCIP 求解器。
在变量声明的代码后面增加：

# assistant variables
position_pairs = set()
for x_1 in range(self.width):
    for y_1 in range(self.height):
        for x_2 in range(self.width):
            for y_2 in range(self.height):
                if x_2 == x_1 and y_2 == y_1:
                    continue
                if abs(x_2 - x_1) <= 1 and abs(y_2 - y_1) <= 1:
                    if ((x_2, y_2), (x_1, y_1)) not in position_pairs:
                        position_pairs.add(((x_1, y_1), (x_2, y_2)))
v = {
    
    }
for i in self.ids:
    for j in self.ids:
        if j == i:
            continue
        for pos_pairs in position_pairs:
            (x_1, y_1), (x_2, y_2) = pos_pairs
            for t in range(self.time_limit):
                v[i, j, x_1, y_1, x_2, y_2, t] = model.addVar(
                    vtype="B", name="v_[{},{},{},{},{},{},{}]".format(i, j, x_1, y_1, x_2, y_2, t))

注释掉约束 (6)，在后面添加新约束：

# constraint 7: forbid neighbour transfer
big_m = 1e4
for i in self.ids:
    for j in self.ids:
        if j == i:
            continue
        for pos_pairs in position_pairs:
            (x_1, y_1), (x_2, y_2) = pos_pairs
            for t in range(self.time_limit - 1):
                model.addCons(
                    u[i, x_1, y_1, t] + u[j, x_2, y_2, t] >= 2 - v[i, j, x_1, y_1, x_2, y_2, t] * big_m, 
                    name="cons_7_[{},{},{},{},{},{},{}]_I".format(i, j, x_1, y_1, x_2, y_2, t))
                model.addCons(
                    u[i, x_1, y_1, t] + u[j, x_2, y_2, t] <= 1 + (1 - v[i, j, x_1, y_1, x_2, y_2, t]) * big_m, 
                    name="cons_7_[{},{},{},{},{},{},{}]_II".format(i, j, x_1, y_1, x_2, y_2, t))
                model.addCons(
                    u[i, x_2, y_2, t + 1] + u[j, x_1, y_1, t + 1] <= 1 + v[i, j, x_1, y_1, x_2, y_2, t] * big_m, 
                    name="cons_7_[{},{},{},{},{},{},{}]_III".format(i, j, x_1, y_1, x_2, y_2, t))

运行效果

运行 $\times 3$ 的地图算例，可在 25s 内完成求解，求解结果如下：
求解结果 encore 1
可见，约束放宽后，6 步移动即可完成全部移动过程，而之前需要 9 步。

动图效果：
结果动图 encore 1

运行 $\times 4$ 的地图算例，时间限制设置为 10（大地图且放宽约束使移动步数更少），求解时间约 80s，结果如下：
求解结果 encore 2
可见，约束放宽后，移动步数由 6 步缩短为 5 步。

动图效果：
结果动图 encore 2

一种基于冲突的路径搜索的整数规划模型

文章目录

模型部分

参数

决策变量

约束条件

目标函数

代码部分

文件结构

具体代码

agent.py

model.py

visual.py

main.py

运行效果

彩蛋：有关禁止相邻智能体交换位置的约束

建模

代码

运行效果

猜你喜欢