matlab-plan-robot-motion

机械臂运动规划

为机械臂规划无碰撞路径，并将其转换为符合速度和加速度限制的时间最优轨迹。

适用场景

• 在关节配置或笛卡尔位姿之间移动机械臂——无论用户说的是“轨迹”“运动”“路径”还是“规划”
• 生成任意航点之间的轨迹（时间最优、恒定速度、梯形速度、最小跃度）
• 规划配置之间的无碰撞路径
• 将障碍物建模为用于规划的碰撞环境
• 在运动规划器（RRT、CHOMP、dlCHOMP）之间进行选择
• 验证轨迹是否无碰撞
• 任何机器人在空间中移动且工作空间存在几何体的任务——即使用户没有提及，碰撞安全也是默认要求

不适用场景

• 构建、加载或导入机器人模型——使用 /matlab-model-robot-kinematics
• 求解 IK/FK 或验证解——使用 /matlab-model-robot-kinematics
• 安装夹爪或配置末端执行器——使用 /matlab-model-robot-kinematics
• 移动机器人导航（轮式平台、地面车辆）
• 动力学仿真（inverseDynamics、forwardDynamics）
• 规划器的代码生成 / MEX 部署

前置条件

规划前，请确认满足以下条件：

0. 强制要求：先调用 /matlab-model-robot-kinematics——在 MATLAB 工作区中存在机器人模型且该模型由此技能设置完成之前，不得执行以下任何步骤。运动规划工作流中，绝不要内联调用 loadrobot、importrobot 或构建 rigidBodyTree，即使看起来很简单也不行。没有例外。建模技能会处理碰撞网格验证、夹爪选择、末端执行器坐标系对齐以及已知问题（例如，带关节的夹爪会导致规划器发生自碰撞）。跳过此步骤会造成不必要的反复尝试。

1. 机器人具有碰撞网格——没有碰撞几何体的机器人会导致规划器将实际发生碰撞的路径报告为“无碰撞”。请通过以下方式确认：

bodies = robot.Bodies;
hasCollision = any(cellfun(@(b) ~isempty(b.Collisions), bodies));
assert(hasCollision, "Robot has no collision meshes — planning results will be invalid.");

2. DataFormat="row"——所有规划函数都要求使用行向量配置。请验证：

assert(strcmp(robot.DataFormat, "row"), "Set DataFormat to 'row' before planning.");

工作流

步骤 0：评估碰撞风险

在选择轨迹工具之前，请确定工作空间中是否存在机器人可能发生碰撞的几何体。默认假设：如果机器人附近存在任何物体（桌子、工件、夹具、工具），就必须进行碰撞检测——即使用户只要求“轨迹”或“插值”也一样。

Does the workspace contain any geometry (workpiece, table, fixture, tool)?
├── YES → Model it as collision objects (Step 1) and either:
│         ├── Use a planner (Steps 2–3) if configs are far apart or path is non-trivial
│         └── Interpolate directly (Step 4) BUT verify collision-free (Step 5) — if collisions
│             are found, go back and use a planner
└── NO (free space, no objects) → Skip to Step 4 (trajectory generation)

绝不要假设关节空间直线插值是安全的。 即使配置之间很接近，中间位姿也可能进入工件内部，尤其是靠近表面作业的 6 自由度机械臂（焊接、加工、装配）。检测成本很低，碰撞代价却很高。

步骤 1：对环境建模

根据现有数据和计划使用的规划器选择表示方式：

表示方式	使用以下方式创建	适用对象	最适合
碰撞对象元胞数组	collisionBox、collisionCylinder、collisionSphere、collisionMesh	manipulatorRRT	已知几何障碍物（桌子、墙壁、料箱）
occupancyMap3D	occupancyMap3D + insertPointCloud	manipulatorRRT（通过 Map 属性）	点云或传感器数据
meshtsdf	根据 geom2struct 输出创建 meshtsdf	manipulatorCHOMP	需要平滑代价的网格环境
SphericalObstacles	N×4 矩阵 [x y z radius]	manipulatorCHOMP	简单的球体近似

碰撞对象模式：

% Table top surface at z = -0.01 (center at z = -0.02 for a 0.02 m thick box)
table = collisionBox(1.2, 0.8, 0.02);
table.Pose = trvec2tform([0.5, 0, -0.02]);

pillar = collisionCylinder(0.05, 0.6);
pillar.Pose = trvec2tform([0.4, 0.0, 0.3]);

env = {table, pillar};

occupancyMap3D 模式：

omap = occupancyMap3D(20);
sensorPose = [0 0 0 1 0 0 0];
insertPointCloud(omap, sensorPose, pointCloudData, 5.0);

meshtsdf 模式（用于 CHOMP）：

meshStructs = geom2struct(env);
mTSDF = meshtsdf(meshStructs, Resolution=20, TruncationDistance=0.2);

TruncationDistance 必须大于机器人最大碰撞球半径，否则 CHOMP 无法正确检测邻近情况。

第 2 步：选择规划器

首要决策——环境是什么类型？

What is the environment representation?
├── Simple primitives (boxes, cylinders, spheres) → manipulatorRRT
├── Dense mesh / CAD STLs → manipulatorCHOMP (meshtsdf is its native input)
├── Point cloud / sensor data → manipulatorRRT (via occupancyMap3D)
└── Just a few spheres → manipulatorCHOMP (SphericalObstacles, lightweight)

次要决策——路径拓扑：

Does the path require going "around" obstacles (large joint-space excursion)?
├── YES → Favor manipulatorRRT (global search won't get stuck in local minima)
└── NO (small adjustment to avoid nearby obstacle) → Either works; CHOMP may be faster

默认选择：manipulatorRRT——它适用于所有环境类型，碰撞检查稳健，且其输出可顺利传递给 contopptraj。请注意，shorten 可以减少绕行，contopptraj 会遵循速度／加速度限制，但两者都不会平滑路径几何中的急剧方向变化。如果平滑的关节空间路径很重要（例如，为了减少冲击或机械磨损），则 CHOMP 更合适——它会联合优化平滑度代价和避障代价，从而生成本质上更平滑的轨迹。

有关详细的调优参数和性能特征，请参阅 references/planner-comparison.md。

第 3 步：规划路径

使用 manipulatorRRT 规划之前： 预先确定自碰撞跳过对，以避免浪费规划迭代：

1. [isColl,sepDist,witPts] = checkCollision(robot, startConfig, {}, SkippedSelfCollisions="parent")——如果 isColl(1)==0，则使用 "parent"。
2. 否则，通过实测构建跳过列表——请参阅 references/self-collision-skip-procedure.md。

manipulatorRRT：

rrt = manipulatorRRT(robot, env, SkippedSelfCollisions=skippedSelfCollisions);
rrt.MaxConnectionDistance = 0.3;
rrt.ValidationDistance = 0.05;
rrt.MaxIterations = 5000;
rrt.EnableConnectHeuristic = true;

rng(0, "twister");
path = plan(rrt, startConfig, goalConfig);
shortenedPath = shorten(rrt, path, 20);

调用 plan 后务必调用 shorten——RRT 路径本身就很曲折。第二个参数是缩短迭代次数（20 是一个不错的默认值；复杂环境下可增加）。

使用 occupancyMap3D 的 manipulatorRRT：

rrt = manipulatorRRT(robot, {}, Map=omap);
rrt.MaxConnectionDistance = 0.2;
rrt.ValidationDistance = 0.02;

仅使用地图时，碰撞对象应传入空单元格 {}。

manipulatorCHOMP：

chomp = manipulatorCHOMP(robot, MeshTSDF=mTSDF, ...
    SkippedSelfCollisions="parent");
chomp.SmoothnessOptions.Weight = 100;
chomp.CollisionOptions.CollisionCostWeight = 10;
chomp.SolverOptions.MaxIterations = 200;

trajectory = optimize(chomp, startConfig, goalConfig);

必须在创建 MeshTSDF 时传入——创建后它是只读的。

自碰撞处理： 在创建时传入 SkippedSelfCollisions。创建后再设置可能导致类型不匹配（尤其是在代码生成环境中）。

切勿使用 clearCollision 来规避自碰撞问题。 移除碰撞网格会在不发出提示的情况下禁用这些刚体的碰撞检查——规划器会将实际发生碰撞的路径报告为“无碰撞”。

策略： 首先尝试 SkippedSelfCollisions="parent"。如果规划器仍报告某个有效配置存在自碰撞，则按照 references/self-collision-skip-procedure.md 中的步骤，通过实测构建跳过列表。

关于 checkCollision 的关键信息（很容易被误解）：

• separationDist 中的刚体顺序：第 1 到 m 行／列依次对应 robot.Bodies{1:m}；索引 m+1 对应 robot.BaseName。基座在最后，而不是最前。
• NaN = 该刚体对发生碰撞。Inf = 已成功跳过该刚体对。
• 如果矩阵中存在任意 NaN，isColliding 就会返回 true。跳过列表必须消除所有 NaN 项——某个刚体对虽然出现在跳过列表中却仍显示为 NaN，说明跳过未生效（请检查名称拼写以及 char 与 string 类型）。
• {" "," "}（一个 1×2 单元格中的空格字符）是一个虚设的跳过对，可抑制默认的父级跳过行为，但不会实际跳过任何真实的刚体对。
• 在所有采样中始终为 NaN 的方法可以捕获父子重叠（约 100% 的碰撞率），但并联连杆夹爪（例如 robotiq2F85）还存在其他碰撞率约为 70%–90% 的刚体对，也需要跳过。完成第一轮后，重新检查 home——如果仍有碰撞，则迭代添加剩余的 NaN 刚体对。

第 4 步：生成轨迹（为路径设置时间参数）

根据应用需求进行选择：

需求	工具	关键属性
满足关节限制的最短时间	contopptraj	TOPPRA 算法——真正的时间最优
工作区域内保持恒定速度（焊接、切割、点胶）	trapveltraj	保证存在零加速度的巡航阶段
平滑运动并最大限度减小加加速度（无障碍物）	minjerkpolytraj	将 ∫jerk² 代价降至最低

contopptraj（时间最优）：

velLimits = [-2*ones(numJoints,1), 2*ones(numJoints,1)];
accelLimits = [-5*ones(numJoints,1), 5*ones(numJoints,1)];
[q, qd, qdd, tSamples] = contopptraj(shortenedPath', velLimits, accelLimits);

trapveltraj（恒定速度巡航）：

[q, qd, qdd, tSamples] = trapveltraj(shortenedPath', 500, AccelTime=0.4);

AccelTime 设置加速段的持续时间——两段加速之间的部分都保持恒定速度。使用 PeakVelocity 设置明确的巡航速度。

minjerkpolytraj（平滑插值）：

tPoints = linspace(0, 3, size(shortenedPath', 2));
[q, qd, qdd, ~, tSamples] = minjerkpolytraj(shortenedPath', tPoints, 500);

重要提示：

• 三者都要求输入为 numJoints×numWaypoints——对 RRT 输出进行转置（shortenedPath'）
• 三者都返回 numJoints×numSamples
• 切勿使用手动梯形代码或 interp1 + gradient——这些工具可取代它们
• contopptraj 不保证恒定速度；trapveltraj 不会将时间降至最低——请正确选择

第 5 步：验证轨迹无碰撞

规划器已验证各个路点，但 contopptraj 会在路点之间进行插值。请验证完整轨迹：

% q from contopptraj is numJoints×numSamples — transpose for checkCollision (expects row vectors)
qRows = q';
inCollision = false;
for i = 1:size(qRows, 1)
    result = checkCollision(robot, qRows(i,:), env, SkippedSelfCollisions="parent");
    % result is 1×2: [selfCollision, worldCollision] — check BOTH
    if any(result)
        warning("Collision at trajectory sample %d (t=%.3f s), self=%d world=%d", ...
            i, tSamples(i), result(1), result(2));
        inCollision = true;
        break;
    end
end
if ~inCollision
    fprintf("Trajectory is collision-free (%d samples verified).\n", size(qRows,1));
end

如果在插值后的轨迹中发现碰撞，请增大规划器的 ValidationDistance（数值越小，规划期间检查越精细），然后重新规划。

第 6 步：可视化

figure;
show(robot, q(1,:), Frames="off");
hold on;
for k = 1:numel(env)
    show(env{k});
end
title("Motion Plan — Start Configuration");
axis auto;
view(45, 30);

为轨迹制作动画：

figure;
ax = show(robot, q(1,:), Frames="off");
hold on;
for k = 1:numel(env)
    show(env{k});
end
axis auto;
view(45, 30);
title("Motion Plan Animation");

for i = 1:10:size(q,1)
    show(robot, q(i,:), Frames="off", PreservePlot=false, Parent=ax);
    drawnow;
end

关键函数

函数	用途	工具箱	起始版本
manipulatorRRT	基于采样的无碰撞规划器	Robotics System Toolbox	R2021b
manipulatorCHOMP	基于优化的平滑规划器	Robotics System Toolbox	R2023a
contopptraj	时间最优轨迹（TOPPRA）	Robotics System Toolbox	R2023b
trapveltraj	梯形速度曲线（恒定速度巡航）	Robotics System Toolbox	R2019a
minjerkpolytraj	最小加加速度多项式轨迹	Robotics System Toolbox	R2022a
checkCollision	检查机器人与环境的碰撞	Robotics System Toolbox	R2020b
collisionBox	盒体碰撞基元	Robotics System Toolbox	R2019b
collisionCylinder	圆柱体碰撞基元	Robotics System Toolbox	R2019b
collisionSphere	球体碰撞基元	Robotics System Toolbox	R2019b
collisionMesh	网格碰撞基元	Robotics System Toolbox	R2019b
occupancyMap3D	从点云创建 3D 体素地图	Navigation Toolbox	R2019b
meshtsdf	从网格创建截断有符号距离场	Robotics System Toolbox	R2023a
geom2struct	将碰撞几何体转换为网格结构体	Robotics System Toolbox	R2023a
manipulatorStateSpace	用于规划器的自定义状态空间	Robotics System Toolbox	R2021b
plannerBiRRT	双向 RRT（使用状态空间）	Navigation Toolbox	R2020b

常见错误

错误	错误原因	正确做法
需要时间最优时使用 trapveltraj	梯形速度曲线并非时间最优——会分配过多时间	使用 contopptraj（TOPPRA）。若目标是保持恒定速度，则应使用 trapveltraj。
需要恒定速度时使用 contopptraj	TOPPRA 会持续改变速度以缩短时间——没有稳定的巡航阶段	对焊接、切割和点胶应用，使用带有 AccelTime 的 trapveltraj
手动计算梯形速度曲线时间	这是对已有解决方案的错误重复实现；20 多行代码可由一次调用取代	根据需求使用 contopptraj 或 trapveltraj
使用 interp1 + gradient 生成轨迹	数值微分会引入噪声，且不会遵守限制	使用合适的轨迹函数——它们都会以解析方式返回 q、qd、qdd
因为插值「简单」而跳过碰撞检查	即使是相邻的构型，其中间位姿也可能与工件碰撞，尤其是在靠近表面时	始终使用 checkCollision 检查所有轨迹采样点（步骤 5）
使用 clearCollision 修复自碰撞错误	这会彻底移除这些刚体的碰撞检查——规划器会悄然忽略实际碰撞	通过实测找出正确的跳过碰撞对（见上文「自碰撞处理」）
未在机器人上设置碰撞网格就进行规划	对实际会发生碰撞的路径，规划器会报告「无碰撞」	规划前确认 robot.Bodies 具有碰撞几何体
在构造后设置 SkippedSelfCollisions	可能导致类型不匹配；在代码生成中也不可靠	在构造规划器时将其作为名称-值参数传入
在构造 CHOMP 后设置 MeshTSDF	创建后该属性为只读	在构造时传入 MeshTSDF=mTSDF
RRT 执行 plan 后未调用 shorten	RRT 路径曲折，包含不必要的绕行	始终调用 shorten(rrt, path, 20)
仅在规划器航点处验证碰撞	航点之间的插值轨迹点仍可能发生碰撞	使用 checkCollision 检查所有 contopptraj 输出采样点
未转置 RRT 路径就直接传给 contopptraj	manipulatorRRT/plan 返回 N×numJoints（每行一个航点），而 contopptraj 需要 numJoints×N（每列一个航点）	转置：contopptraj(path', velLimits, accelLimits)
将 contopptraj 输出 q 直接用于 checkCollision	contopptraj 返回 numJoints×numSamples，而 checkCollision 需要行向量	转置：checkCollision(robot, q(:,i)', env, ...)，或使用 q'
在规划器中使用 DataFormat="column"	manipulatorRRT/plan 需要行向量	规划前将机器人的 DataFormat 设为 "row"
将 checkCollision 输出视为标量	存在世界物体时，checkCollision 返回 1×2：[selfCollision, worldCollision]。将其用于 if checkCollision(...) 时，只会检查 selfCollision（元素 1）——世界碰撞会被悄然忽略	始终捕获结果：result = checkCollision(...)，然后检查 any(result)，或专门检查世界碰撞时查看 result(2)
meshtsdf 的 TruncationDistance 过小	若截断距离小于机器人碰撞球半径，CHOMP 就无法检测邻近情况	将 TruncationDistance 设为大于最大碰撞球半径的值（通常至少为 0.2）
放置地面/地板碰撞物体时，将其顶面设在 z=0	机器人的底座碰撞网格可能延伸到 z=0 以下最多 5 mm（最坏情况为 universalUR5，低于 5 mm；大多数其他机器人约为 1 mm）。此外，许多 loadrobot 的 home 配置会使机械臂水平伸展，导致远端连杆低于 z=0。这两种情况都会在规划开始前立即造成世界碰撞。	将地面顶面放在 z ≤ −0.01 处（避开所有 loadrobot 底座网格）。仅在任务需要时才添加地面。调用 plan 前，始终使用 checkCollision 确认起点/目标构型无碰撞——若发生碰撞，应调整 IK 初始猜测或抬高目标位置，而不是降低地面。

---

Copyright 2026 The MathWorks, Inc.

---