约束运动规划：客厅里的大沙发

可视化演示

展示机器人约束路径规划、构型空间映射与轨迹优化过程，含障碍避让与约束边界。

基础概念：先画出客厅户型图

搬沙发前，你要先看清客厅的门、走廊和转角在哪儿。机器人也一样，规划前必须知道它自身有哪些“可动部位”、每个部位能转到什么角度。全部关节角度或者位置参数放在一起，就是机器人的配置。所有可能的配置堆叠起来，形成一个抽象空间，我们叫它配置空间（C-space）。

在配置空间里，每个点都代表沙发的一种姿态。那些会撞到墙、卡在门框的姿态，就被排除在外，剩下的点构成“可行配置集合”。约束运动规划的任务，就是在这个高维集合中找一条连续的曲线连接起点和终点。

有了这张“户型图”，机器人才能知道哪些姿态是被允许的。很多初学者习惯在真实工作空间里直接画线避障，但真正的规划发生在配置空间中——因为机器人的每个姿态，都要在配置空间中用坐标去描述和验证。

约束分类：有的像墙，有的像重力

搬沙发时，走廊宽度是“尺寸限制”，拉不动沙发是因为“摩擦力不足”，沙发不能穿墙是“几何限制”。约束运动规划里的约束，同样可以分门别类。

完整约束（holonomic）只限制位置，可以写成只含位置变量的等式，像“墙”一样划定区域范围。非完整约束（nonholonomic）则与速度有关，无法通过积分消去速度变量。最经典的例子是小汽车：它不能横向平移，只能前进或后退并转向——车的位置坐标并不是被墙拦住，而是被“转向规则”限制了移动方向。

除了完整与非完整这对核心分类，约束还可以按是否涉及不等式分为等式约束与不等式约束。等式约束要求严格等于某个值，好比插座必须对准才能插进去；不等式约束则给出安全范围，好比关节不能转过某个极限角度。

采样规划：黑暗中投飞镖

配置空间往往高维且形状复杂，直接像网格搜索那样遍历，维度一高就会爆炸。想象你在没有灯的房间里搬沙发：你不会把地板每一寸都摸一遍，而是用手电筒随意照几个位置，试试能不能通过——这就是采样规划的直觉。

PRM（概率路线图）像先在屋里到处掷飞镖，每枚飞镖落在可行区域就记为一个节点，再把彼此够得着且不撞墙的节点连起来，形成一张“可行路线网络”。RRT（快速随机树）则从起点长出一棵树，每次随机朝一个方向长一小段，碰壁就换个方向，直到枝丫够到终点。

实际使用时，采样点不是完全随机——会有一定概率直接瞄准目标点（目标偏置采样），这就像你虽然黑暗中看不清路，但会大致朝门的方向投飞镖。而在有约束的环境里，采样点如果落在约束外，则会被拒绝或投影回可行区域，这是约束采样策略的核心。

约束处理：被门卡住怎么办

好不容易找到一条可行路径，但路径上的节点可能并不严格满足所有约束。这就像沙发推过了走廊一半，但角度不对，卡在门框上。此时我们需要“处理”约束，把节点调整到可行区域内。

投影策略是最直观的方法：采样点如果不在约束流形上，就在流形上找一个离它最近的点，把它“拽回来”。这个流形就是由约束定义的子空间——沙发能通过的所有姿态构成的那个“弯弯曲曲的通道面”。后退规避则是在扩展失败时，把树枝缩回去一点或换一个方向重新尝试，好比沙发卡住后先退半步再换角度。

在 RRT 中，投影策略既可以用于采样阶段（采样点投影后再连接），也可以用于扩展阶段（新节点投影回流形再连边）。投影与后退配合，能显著提高窄通道环境的规划成功率。

轨迹优化：搬得稳还要搬得快

找到一条无碰撞路径只是第一步，搬沙发时你还会希望动作尽量平稳、少反复调整。这就从“路径规划”进入“轨迹优化”阶段——给路径加上时间轴，同时优化速度、加速度和能量消耗。

最常见的做法是把约束写进目标函数里。惩罚函数法把约束违反量乘以一个很大的权重，加到原始目标函数上：谁违反约束，谁就受到“罚款”。权重越大，越不敢违反约束。但权重过大会导致数值不稳定，因此后来发展出增广拉格朗日方法，引入拉格朗日乘子迭代调整，用乘子逐渐逼近对偶变量，避免惩罚系数无限增大。

增广拉格朗日更进一步，在惩罚项基础上加入乘子项，使迭代过程中约束渐近满足，同时避免惩罚系数失控。轨迹优化输出的不仅是一条几何路径，而是一条带时间信息的平滑轨迹，机器人沿着它执行时动作更优雅。

应用场景：客厅里的实战

搬沙发的道理，用在真实机器人系统里，就成了工程实践。机械臂装配中，机械臂要将齿轮、插头等零件精确送入指定位置，必须同时满足姿态对准、插入深度、摩擦力等多重约束。约束运动规划让机械臂在狭小装配空间中绕开障碍、精确到达目标姿态。

移动机器人（如仓库AGV）在货架间穿梭，受非完整约束（只能前进后退转向）和通道宽度的双重限制；手术机器人则在狭小的人体解剖空间中操作，约束更严厉，精度要求更高。这些场景都依赖约束运动规划来保证安全性和成功率。

挑战展望：更大的房子与更多的家具

住房越来越大，家具越来越多，搬沙发也越发困难。运动规划面对的核心挑战是维度灾难：机器人自由度越高，配置空间维度越大，网格法搜索量呈指数增长。即使采样法能绕开维度爆炸，窄通道、动态障碍物仍然考验算法能力。

动态避碰要求在机器人运动时实时感知并躲避移动的障碍物，这对规划器的实时性提出极高要求。近年来学习规划（如强化学习、模仿学习）开始与经典采样规划融合，用数据驱动的方式预测高价值采样区域，让算法在复杂环境中更聪明地分配算力。未来的约束运动规划，正从“被动躲避约束”走向“主动利用约束”。

AI 应用场景

Python 演示

import mathimport random  def dist(a, b):    return math.hypot(a[0] - b[0], a[1] - b[1])  def collide(a, b, obstacles):    for ox, oy, r in obstacles:        dx, dy = b[0] - a[0], b[1] - a[1]        L2 = dx * dx + dy * dy        if L2 < 1e-9:            return True        t = max(0.0, min(1.0, ((ox - a[0]) * dx + (oy - a[1]) * dy) / L2))        cx, cy = a[0] + t * dx, a[1] + t * dy        if math.hypot(cx - ox, cy - oy) < r:            return True    return False  def rrt_plan(start, goal, obstacles, n_iter=3000, step=25):    nodes = [start]    parent = [-1]    solved = False    for _ in range(n_iter):        if random.random() < 0.25:            target = goal        else:            target = (random.uniform(0, 400), random.uniform(0, 300))        best_idx = 0        best_dist = 1e18        for i, node in enumerate(nodes):            d = dist(node, target)            if d < best_dist:                best_dist = d                best_idx = i        near = nodes[best_idx]        d = dist(near, target)        if d > step:            target = (near[0] + (target[0] - near[0]) * step / d,                      near[1] + (target[1] - near[1]) * step / d)        if collide(near, target, obstacles):            continue        nodes.append(target)        parent.append(best_idx)        if dist(target, goal) < 30:            solved = True            break    if not solved:        return None    path = []    cur = len(nodes) - 1    while cur != -1:        path.append(nodes[cur])        cur = parent[cur]    path.reverse()    return path  if __name__ == '__main__':    obstacles = [(200, 160, 40), (200, 250, 40), (120, 200, 45)]    path = rrt_plan((25, 25), (375, 275), obstacles)    if path:        print('约束RRT规划成功，共 %d 个路径点' % len(path))    else:        print('未找到可行路径')

1. def collide(a, b, obstacles):判断线段 ab 是否与任何圆形障碍物相交，这是“无碰撞约束”的检查函数。
2. t = max(0.0, min(1.0, ...))计算线段上离圆心的最近参数 t，若最近距离小于半径则碰撞。
3. if random.random() < 0.25: target = goal目标偏置采样，25% 概率直接瞄准终点。
4. best_dist = 1e18初始化最近邻搜索的最小距离为非常大的值。
5. if d > step:若采样点太远，则沿方向截断为固定步长。
6. nodes.append(target); parent.append(best_idx)新节点入树，并记录父节点索引用于回溯。
7. cur = len(nodes) - 1从目标节点开始回溯，沿着 parent 链找路径。
8. path.reverse()将回溯得到的倒序路径反转成从起点到终点的顺序。

常见误区

面试题