一种基于冲突的路径搜索的整数规划模型
本文介绍一种针对基于冲突的路径规划问题（conflict-based search, CBS）的整数规划模型，并提供调用 OR-Tools 和 SCIP 求解器的代码。
文章目录
- 模型部分
-
- 参数
- 决策变量
- 约束条件
- 目标函数
- 代码部分
-
- 文件结构
- 具体代码
-
- agent.py
- model.py
- visual.py
- main.py
- 运行效果
- 彩蛋：有关禁止相邻智能体交换位置的约束
-
- 建模
- 代码
- 运行效果
模型部分
参数
| 参数 | 含义 |
|---|---|
| I I I | 所有智能体 |
| P P P | 所有位置 |
| T T T | 时间限制 |
| t 0 t_0 t0 | 初始时刻 |
| s i s_i si | 智能体 i i i 的初始位置 |
| g i g_i gi | 智能体 i i i 的目标位置 |
| N p N_p Np | 位置 p p p 的所有相邻位置 |
决策变量
| 变量 | 类型 | 含义 |
|---|---|---|
| u i , p , t u_{i, p, t} ui,p,t | { 0 , 1 } \{ 0, 1 \} {0,1} | 智能体 i i i 在时刻 t t t 位于位置 p p p |
约束条件
(1) 初始位置
u i , s i , t 0 = 1 ∀ i ∈ I u_{i, s_i, t_0} = 1 \quad \forall i \in I ui,si,t0=1∀i∈I
(2) 每个智能体在每个时刻有且仅有一个位置
∑ p ∈ P u i , p , t = 1 ∀ i ∈ I ∀ t ≤ T \sum_{p \in P} u_{i, p, t} = 1 \quad \forall i \in I \quad \forall t \leq T p∈P∑ui,p,t=1∀i∈I∀t≤T
(3) 移动方向
u i , p , t ≤ ∑ q ∈ N p u i , q , t + 1 ∀ i ∈ I ∀ t < T ∀ p ∈ P 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 ui,p,t≤q∈Np∑ui,q,t+1∀i∈I∀t<T∀p∈P
(4) 到达终点后不再移动
u i , g i , t ≤ u i , g i , t + 1 ∀ i ∈ I ∀ t < T u_{i, g_i, t} \leq u_{i, g_i, t + 1} \quad \forall i \in I \quad \forall t < T ui,gi,t≤ui,gi,t+1∀i∈I∀t<T
(5) 每个位置在每个时刻至多有一个智能体
∑ i ∈ I u i , p , t ≤ 1 ∀ i ∈ I ∀ p ∈ P \sum_{i \in I} u_{i, p, t} \leq 1 \quad \forall i \in I \quad \forall p \in P i∈I∑ui,p,t≤1∀i∈I∀p∈P
(6) 只有一个智能体离开当前位置后，才允许另一个智能体进入
（更严谨的约束方式是禁止两个相邻的智能体对撞交换位置，但是较难线性化，且需要引入较多变量，故简化为该约束）
u i , p , t + u j , p , t + 1 ≤ 1 ∀ i ∈ I j ∈ I ∀ p ∈ P ∀ t < T 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 ui,p,t+uj,p,t+1≤1∀i∈Ij∈I∀p∈P∀t<T
目标函数
max ∑ i ∈ I ∑ t ≤ T u i , g i , t \max \ \ \sum_{i \in I} \sum_{t \leq T} u_{i, g_i, t} max i∈I∑t≤T∑ui,gi,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)
运行效果
以 3 × 3 3 \times 3 3×3 的地图为例，红色智能体和蓝色智能体需要互换位置（对面互换），时间限制为 10，初始状态如下图所示：
求解结果如下，即各个时刻每个智能体的位置：
结果动图如下：
如果选用 OR-Tools 求解器，求解时间大概在 35-40s 左右；
如果选用 SCIP 求解器，求解时间约 8s。
如果把规模增加，地图改为 4 × 4 4 \times 4 4×4，时间限制为 16，目标位置逻辑不变，
当选用 OR-Tools 求解器时，求解时间大概在 150-300s 左右；
当选用 SCIP 求解器时，求解时间大概在 25s 左右。
求解结果如下：
实际上，当地图变大时，由于阻塞减少，只需 6 个时间步数即可完成全部移动过程，若把时间限制减少为 10，则运行时间：
OR-Tools 约 70s；SCIP 在 10s 以内。
结果动图如下：
彩蛋：有关禁止相邻智能体交换位置的约束
建模
增加一组辅助变量，用于标识某时刻两个智能体位于相邻位置：
| 变量 | 类型 | 含义 |
|---|---|---|
| v i , j , p , q , t v_{i, j, p, q, t} vi,j,p,q,t | { 0 , 1 } \{ 0, 1 \} {0,1} | 智能体 i i i 和 j j j 在时刻 t t t 分别位于位置 p p p 和 q q q |
则禁止相邻智能体交换位置的约束为：
u i , p , t + u j , q , t ≥ 2 − v i , j , p , q , t ⋅ M u i , p , t + u j , q , t ≤ 1 + ( 1 − v i , j , p , q , t ) ⋅ M u i , q , t + 1 + u j , p , t + 1 ≤ 1 + v i , j , p , q , t ⋅ M ∀ i ∈ I ∀ j ∈ I i ≠ j ∀ p ∈ P ∀ q ∈ N p ∀ t < T 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 ui,p,t+uj,q,t≥2−vi,j,p,q,t⋅Mui,p,t+uj,q,t≤1+(1−vi,j,p,q,t)⋅Mui,q,t+1+uj,p,t+1≤1+vi,j,p,q,t⋅M∀i∈I∀j∈Ii=j∀p∈P∀q∈Np∀t<T
其中， M M 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))
运行效果
运行 3 × 3 3 \times 3 3×3 的地图算例，可在 25s 内完成求解，求解结果如下：
可见，约束放宽后，6 步移动即可完成全部移动过程，而之前需要 9 步。
动图效果：
运行 4 × 4 4 \times 4 4×4 的地图算例，时间限制设置为 10（大地图且放宽约束使移动步数更少），求解时间约 80s，结果如下：
可见，约束放宽后，移动步数由 6 步缩短为 5 步。
动图效果：
