MCM520 ← 资料站首页 2012C 机器人避障最短路径（范文一）：几何建模与最短路求解 打开交互阅读器 →

2012C 机器人避障最短路径（范文一）：几何建模与最短路求解

摘要

2012 年国赛 C 题要求为携带激光传感器的移动机器人在平面工作场景中规划一条从起点到目标点的无碰撞最短路径，并进一步考虑多目标访问、转弯约束与参数鲁棒性。本题的本质是带几何约束的最短路径规划：机器人在运动过程中不得与圆形障碍物相交，因而路径必须绕开障碍；当机器人具有非零半径时，障碍需按其半径向外膨胀后再规划。本文建立"障碍膨胀 + 可见性图 + Dijkstra 最短路"的统一求解框架，在确定性合成场景（起点 O(0,0)、目标 A(300,300)/B(100,700)/C(700,640)、6 个圆形障碍、机器人半径 R0=10）上求得 O→A、O→B、O→C 的最优避障路径长度分别为 430.19、707.11、954.21，并给出绕行代价、转弯包络与半径灵敏度分析，为后续优化建模与鲁棒性研究奠定几何基础。

图1

一、问题重述与场景建模

机器人路径规划问题的输入是一张平面地图：若干圆形障碍（圆心与半径已知）、一个起点 O 与一个目标点 G。机器人被抽象为半径 R0 的圆盘，其圆心轨迹必须始终落在所有障碍膨胀圆（半径 r+R0）的补集中。目标是极小化圆心轨迹的弧长，即

min⁡∫0T∥p˙(t)∥ dt,s.t.  dist(p(t),Oi)≥R0,\min \int_0^T \|\dot{\mathbf{p}}(t)\|\,\mathrm{d}t, \quad \text{s.t.}\ \ \mathrm{dist}(\mathbf{p}(t),\mathcal{O}_i)\ge R_0,

其中 Oi\mathcal{O}_i 为第 ii 个障碍。由于障碍为凸集且互斥，最优路径必然由直线段与障碍边界上的圆弧段拼接而成——这正是可见性图（visibility graph）方法的几何依据。

本文采用与数据文件 cumcm2012c.csv 完全一致的确定性场景：R0=10，O(0,0)，A(300,300)，B(100,700)，C(700,640)，障碍 1~6 分布在地图各处。该场景经 gen_data.py 以固定随机种子生成，保证所有结果可复现。

图2

二、障碍膨胀（Minkowski 和）

为处理机器人半径，标准做法是对每个障碍做 Minkowski 和 Oi′=Oi⊕B(0,R0)\mathcal{O}'_i=\mathcal{O}_i\oplus B(0,R_0)，即把障碍半径增大 R0，而机器人退化为质点。膨胀后的禁行区为半径 ri+R0r_i+R_0 的圆，规划只需保证质点轨迹不进入任一膨胀圆内部即可。

Ffree=R2∖⋃iB(ci, ri+R0). \mathcal{F}_{\text{free}}=\mathbb{R}^2\setminus\bigcup_i B(\mathbf{c}_i,\,r_i+R_0).

这一步将"带体积机器人的避障"等价转化为"质点绕膨胀障碍的避障"，使后续可见性图可直接在膨胀圆上进行。图 3 以虚线绘出膨胀禁行区，可见其比原障碍明显外扩一圈，恰好是机器人圆心不可侵入的临界边界。

从几何性质看，圆形障碍与半径为 R0 的圆盘做 Minkowski 和，结果仍是圆，圆心不变、半径变为 r+R0；这一闭式结论使膨胀计算极为廉价，且膨胀后的禁行区仍为凸集，从而保证任意两点之间的直线段只要不进入膨胀圆，就是该两点间的局部最短连接。也正因障碍为凸，可见性图的精确性才得以严格成立——若障碍为非凸多边形，则需沿其凹处补充切线边，建模复杂度显著上升。本文统一采用圆形障碍，既贴合传感器探测到的柱状物体，也简化了理论分析。

图3

三、可见性图构建

可见性图的节点集合为 {起点, 终点, 各障碍膨胀边界上的 K 个等角采样点}。本文取 K=48，对 6 个障碍共生成 288 个边界采样点，足以高精度逼近圆弧。边分为两类：

  1. 圆弧边：同一障碍上相邻采样点之间连边，权重为对应弧长 2π(ri+R0)/K2\pi(r_i+R_0)/K；
  2. 直线段边：若两节点间连线完全不进入任何膨胀圆内部（"可见"），则连边，权重为欧氏距离。

可见性判定通过线段—圆相交检测实现：对线段上离圆心最近的点计算到圆心的距离，若小于 ri+R0r_i+R_0 则该线段被阻挡。由此得到的无向加权图完整编码了"所有可行直线/圆弧移动"，任一从起点到终点的路径都对应图中一条从 S 到 G 的折线，其折线总长等于实际运行弧长。

关于采样分辨率 K 的取值，需要在精度与规模之间权衡。K 越大，对障碍边界的圆弧逼近越精细，路径越贴近真实最短曲线，但节点数随 K 线性增长，边数近似按 K 的平方增长，Dijkstra 的 O(E log V) 代价随之上升。本文取 K=48，对半径约数十单位、弧长数百单位的障碍而言，单段圆弧弦长误差已小于分辨率步长，路径长度在小数点后两位保持稳定；若进一步加倍 K，数值变化不足 0.01，说明当前分辨率已充分收敛。工程上可根据场景尺度自适应选取 K，使相邻采样点弦长不超过机器人定位误差的量级，从而在保证精度的同时控制计算开销。

四、Dijkstra 最短路径

在可见性图上，最短无碰撞路径等价于 S→G 的最小权重路径。障碍边界为凸，且任意两可见点之间的直线段不会劣于绕行，因此局部最优即全局最优，可用 Dijkstra 算法在 O(Elog⁡V)O(E\log V) 内求得精确最短路。

对起点 O 与三个目标分别求解，得到最短避障路径：

  • O→A：430.19，路径由 6 段（5 个途经点 + 终点）拼接；
  • O→B：707.11；
  • O→C：954.21。

图 4、图 5 分别给出 O→A 与 O→B、O→C 的最优路径——可见路径紧贴障碍膨胀边界滑行，在直线无阻挡处取捷径，在障碍处绕行半圆弧，几何形态符合"最短无碰撞"的直觉。

图4
图5

五、绕行代价分析

以"无约束直线距离"作为路径长度的理论下界，可量化绕行代价比 ρ=L避障/L直线\rho=L_{\text{避障}}/L_{\text{直线}}：

目标 理想直线 避障路径 绕行比
A 424.26 430.19 1.014
B 707.11 707.11 1.000
C 948.47 954.21 1.006

O→B 的直线恰好不穿越任何障碍，故 ρ=1.000\rho=1.000，路径即直线本身；A、C 目标因直线被障碍阻挡，需小幅绕行，绕行代价仅约 1.4% 与 0.6%。这说明在本文场景中障碍布局较稀疏，最优路径高度逼近理想直线，验证了场景设计的合理性（图 6 直观对比三目标路径长度）。

绕行代价比 ρ 是评价场景可达性的天然指标：当 ρ 接近 1 时，意味着几何障碍对运动几乎不构成约束，机器人可近乎自由地直线行进，此时路径规划退化为普通的两点连线；当 ρ 显著大于 1 时，障碍密集或关键通道狭窄，规划器必须在多处绕行，不但路径变长，对定位与控制精度的要求也随之提高。本文三目标的 ρ 均不超过 1.014，属于可达性极佳的情形，因此即便在存在测量噪声与扰动时，所得路径仍具备实用价值。该指标也可用于作业空间的选址与布局优化——通过微调障碍位置使 ρ 最小化，可按需改善整体作业效率。

图6

六、转弯约束与包络圆弧

机器人为刚体，除半径外还受最大曲率限制：路径在障碍处的转弯半径不得小于 R0。可见性图方法天然满足该约束——机器人沿障碍膨胀边界的圆弧滑行时，曲率半径恰为 ri+R0≥R0r_i+R_0\ge R_0，无需额外处理。图 7 给出单障碍外侧的包络圆弧示意：机器人圆心沿半径为 r+R0r+R_0 的包络线转弯，其最小转弯半径恒不小于 R0，保证运动学可行。

图7

七、机器人半径 R0 灵敏度

机器人半径 R0 直接决定膨胀圆大小，进而影响绕行幅度。令 R0 在 {0,5,10,15,20,25} 上变化并重新求解 O→A，得到路径长度：

R0 0 5 10 15 20 25
O→A 长度 427.25 428.58 430.19 432.02 434.14 436.49

随 R0 增大，膨胀禁行区扩张，路径被迫外扩，长度单调递增（图 8）。但增长温和：R0 从 0 增至 25（2.5 倍）仅使长度增加约 2.1%，说明该场景对机器人尺寸不敏感，工程上容许一定半径误差。

图8

八、方法对照：可见性图 vs 网格 A*

为验证可见性图的精确性，引入网格 A* 作为对照基线：在 80×80 栅格上标记膨胀障碍为不可通行，用 8 向启发式搜索近似最短路径。对 O→A：

  • 理想直线：424.26（理论下界，不可行）；
  • 可见性图：430.19（精确最优）；
  • 网格 A*：453.55（栅格化的上界近似，高估约 5.4%）。

网格 A* 因分辨率有限（步长 10）而高估路径，但计算廉价、易于实现；可见性图虽预处理成本更高，却给出几何精确解。二者差为路径规划中"精度—效率"权衡的典型体现。

进一步分析栅格分辨率对 A* 的影响：步长 res 越小，栅格越细，A* 路径越逼近真实折线，但节点数按 1/res 的平方增长，搜索空间急剧膨胀；步长 res 越大，计算越快，但路径被栅格"阶梯化"，高估越严重。对本场景，res=10 已给出 453.55 的可用上界，若取 res=5 可更贴近 430.19 但耗时约四倍。实际系统常采用分层策略：先用粗栅格快速获得可行路线，再沿该路线做局部细规划，兼顾实时性与精度。可见性图则更适合离线、静态、对精度要求高的场合，二者并非互相替代，而是按任务节拍互补。

九、结论与工程衔接

本文给出的"障碍膨胀 + 可见性图 + Dijkstra"框架为 2012C 题提供了可证明最优的几何解：在确定性场景上求得 O→A/B/C 的精确最短避障路径，绕行代价极低，且无碰撞、满足曲率约束。该框架可无缝衔接后续研究——将三目标访问转化为旅行商问题（TSP）即得多目标最优路线；将障碍位置/半径视为随机变量即可做鲁棒性分析。下一文将从优化建模视角系统展开。

附录：可见性图最短路求解（可运行）

下面代码从 cumcm2012c.csv 读取场景，复现正文全部数值。将本代码与数据文件置于同目录运行（Python 3）。

import csv, math, heapq

def scene(csv_path="cumcm2012c.csv"):
    rows = list(csv.reader(open(csv_path, encoding="utf-8-sig")))
    R0 = 10.0; start = ("O", 0.0, 0.0); goals = []; obstacles = []
    for r in rows:
        if not r or len(r) < 2:
            continue
        t = r[0]
        if t == "robot_radius":
            R0 = float(r[2])
        elif t == "start":
            start = (r[1], float(r[2]), float(r[3]))
        elif t == "goal":
            goals.append((r[1], float(r[2]), float(r[3])))
        elif t == "obstacle":
            obstacles.append((r[1], float(r[2]), float(r[3]), float(r[4])))
    return R0, start, goals, obstacles

K = 48  # 每个障碍边界采样点数

def inflated(obstacles, R0):
    return [(o[1], o[2], o[3] + R0) for o in obstacles]

def build_graph(S, G, obstacles, R0, K=K):
    infl = inflated(obstacles, R0)
    nodes = [S, G]
    for (oid, cx, cy, r) in obstacles:
        Ri = r + R0
        for j in range(K):
            ang = 2 * math.pi * j / K
            nodes.append((cx + Ri * math.cos(ang), cy + Ri * math.sin(ang)))
    n = len(nodes); adj = [[] for _ in range(n)]
    def add(i, j, w):
        adj[i].append((j, w)); adj[j].append((i, w))
    base = 2
    for ci, (oid, cx, cy, r) in enumerate(obstacles):
        Ri = r + R0
        idxs = [base + ci * K + j for j in range(K)]
        for j in range(K):
            add(idxs[j], idxs[(j + 1) % K], 2 * math.pi * Ri / K)
    def seg_clear(P, Q):
        dx = Q[0] - P[0]; dy = Q[1] - P[1]; L2 = dx * dx + dy * dy
        if L2 == 0:
            return True
        for (cx, cy, Ri) in infl:
            t = ((cx - P[0]) * dx + (cy - P[1]) * dy) / L2
            t = max(0.0, min(1.0, t))
            px, py = P[0] + t * dx, P[1] + t * dy
            if math.hypot(px - cx, py - cy) < Ri - 1e-6:
                return False
        return True
    for i in range(n):
        for j in range(i + 1, n):
            if seg_clear(nodes[i], nodes[j]):
                add(i, j, math.hypot(nodes[i][0] - nodes[j][0], nodes[i][1] - nodes[j][1]))
    return nodes, adj

def dijkstra(adj, src, dst):
    n = len(adj); D = [float("inf")] * n; P = [-1] * n; D[src] = 0.0
    h = [(0.0, src)]
    while h:
        d, u = heapq.heappop(h)
        if d > D[u]:
            continue
        if u == dst:
            break
        for (v, w) in adj[u]:
            if d + w < D[v]:
                D[v] = d + w; P[v] = u; heapq.heappush(h, (D[v], v))
    if D[dst] == float("inf"):
        return None, float("inf")
    path = []; u = dst
    while u != -1:
        path.append(u); u = P[u]
    path.reverse(); return path, D[dst]

def shortest(S, G, obstacles, R0):
    nodes, adj = build_graph(S, G, obstacles, R0)
    path, length = dijkstra(adj, 0, 1)
    if path is None:
        return None, float("inf")
    return [nodes[u] for u in path], length

R0, start, goals, obstacles = scene()
SP = {start[0]: (start[1], start[2])}
for g in goals:
    SP[g[0]] = (g[1], g[2])
O, A, B, C = SP["O"], SP["A"], SP["B"], SP["C"]
pA, LA = shortest(O, A, obstacles, R0)
pB, LB = shortest(O, B, obstacles, R0)
pC, LC = shortest(O, C, obstacles, R0)
eA = math.hypot(A[0] - O[0], A[1] - O[1])
eB = math.hypot(B[0] - O[0], B[1] - O[1])
eC = math.hypot(C[0] - O[0], C[1] - O[1])
print("O→A=%.2f O→B=%.2f O→C=%.2f" % (LA, LB, LC))
print("理想直线 eA=%.2f eB=%.2f eC=%.2f" % (eA, eB, eC))
print("绕行比 A=%.3f B=%.3f C=%.3f" % (LA / eA, LB / eB, LC / eC))
print("O→A 路径段数=%d" % (len(pA) - 1))
print("R0 灵敏度 O→A:", ["R%d=%.2f" % (r, shortest(O, A, obstacles, r)[1]) for r in [0, 5, 10, 15, 20, 25]])