matlab-model-robot-kinematics

机器人建模与运动学

正确构建机械臂模型，并验证每个运动学解。

适用场景

• 任何需要在 MATLAB 中使用机器人模型的任务（即使最终目标是动画、抓取与放置、轨迹或运动规划）
• 用户提到某个具体机械臂的名称（UR5e、KUKA iiwa14、Panda、Kinova Gen3 等）
• 加载、构建或导入机器人模型
• 仿真、制作动画或可视化机械臂
• 涉及抬升、伸手、拾取、放置或抓取的任务
• 添加视觉网格或碰撞几何体
• 将夹爪连接到机械臂
• 在 inverseKinematics 与 generalizedInverseKinematics 之间进行选择
• 验证 IK/GIK 解（退出标志、位置误差、关节连续性）

不适用场景

• 无碰撞运动规划（RRT、CHOMP、路径规划）——使用运动规划技能
• 轨迹生成（contopptraj、cubicpolytraj、trapveltraj）——使用轨迹技能
• 闭环并联机构（Delta、Stewart）——请参阅 references/parallel-robot-guidance.md
• 移动机器人导航或轮式平台——不在本文范围内
• 动力学仿真（inverseDynamics、forwardDynamics）——属于单独领域

工作流

第 1 步：确定模型来源

选择可用的最高保真度来源：

优先级	来源	使用场景	原因
1	loadrobot	机器人存在于库中	包含碰撞网格、惯性和视觉几何体
2	importrobot	用户有 URDF/Xacro/SDF 文件	保留网格引用
3	根据 DH 参数构建	仅有 DH 参数	除非手动添加，否则不包含碰撞网格

**重要：**没有碰撞网格的机器人会导致下游运动规划悄无声息地报告“无碰撞”路径，而这些路径实际上会发生碰撞。始终优先选择包含网格的来源。

如果用户说“构建机器人”，但提到的是已知机器人（UR5e、KUKA iiwa、Panda 等），请先建议使用 loadrobot。

**从 URDF、Simscape 或 CAD 导入后：**检查是否存在碰撞网格。Simscape Multibody 模型以及许多 URDF/CAD 导出文件只提供视觉几何体，不包含碰撞网格。如果缺少碰撞网格，请阅读 references/import-mesh-handling.md，了解如何根据视觉几何体生成碰撞网格的决策树。

第 2 步：加载或构建模型

始终设置 DataFormat="row"——规划和动力学工作流都需要此设置。

选项 A——loadrobot：

robot = loadrobot("universalUR5e", DataFormat="row");

选项 B——importrobot（URDF/Xacro/SDF）：

robot = importrobot("myRobot.urdf", DataFormat="row");

如果 URDF 只有视觉几何体（没有 <collision> 标签），请在导入时添加 VHACD 碰撞分解：

opts = vhacdOptions("RigidBodyTree");
opts.SourceMesh = "VisualGeometry";
robot = importrobot("myRobot.urdf", DataFormat="row", ...
    MeshPath="path/to/meshes", CollisionDecomposition=opts);

选项 C——根据 DH 参数构建：

robot = rigidBodyTree(DataFormat="row");

body = rigidBody("link1");
jnt = rigidBodyJoint("joint1", "revolute");
setFixedTransform(jnt, [a alpha d 0], "dh");
jnt.PositionLimits = [-pi, pi];
body.Joint = jnt;

addVisual(body, "Cylinder", [0.04, 0.3]);
addCollision(body, "Cylinder", [0.05, 0.3]);

addBody(robot, body, "base");

第 3 步：确保存在 tool0，且 Z 轴朝外

连接夹爪之前，机器人必须有一个 tool0 刚体，其局部 Z 轴沿机械臂朝外的方向（远离腕部）。大多数 loadrobot 模型都满足这一条件——但 KUKA iiwa 模型不满足。

请查看 references/ee-frame-alignment.md，了解完整的兼容性表和诊断流程。

如果机器人已具有 Z 轴朝外的 tool0（所有 UR、ABB、Techman、FANUC、Kinova、Yaskawa）：直接连接夹爪。

如果机器人缺少 tool0，或末端执行器的 Z 轴未对齐（KUKA iiwa 7/14）：添加一个用于校正的 tool0 坐标系：

% KUKA iiwa: iiwa_link_ee Z is perpendicular to arm — fix with Ry(90°)
tool0 = rigidBody("tool0");
tool0Joint = rigidBodyJoint("tool0_joint", "fixed");
R_correction = [0 0 1; 0 1 0; -1 0 0]; % Ry(90°)
setFixedTransform(tool0Joint, rotm2tform(R_correction));
tool0.Joint = tool0Joint;
addBody(robot, tool0, "iiwa_link_ee");

**对于未知机器人（URDF 导入）：**在连接任何部件之前，运行 references/ee-frame-alignment.md 中的诊断检查，以验证对齐情况。

第 4 步：配置模型

视觉外观与碰撞几何——用途不同，二者都不可或缺：

功能	用途	用于
addVisual(body, shape, dims)	显示外观	show 用于可视化
addCollision(body, shape, dims)	规划几何	checkCollision、运动规划器

为视觉外观着色：addVisual(body, "Mesh", stlFile, tform, FaceColor=[1 0.8 0])

loadrobot 加载的机器人已经包含这两者。从头构建时，请显式添加两者。

安装夹爪（操作任务必需）：

如果任务涉及物体交互（拾取、放置、抬起、抓取、操纵），则必须使用夹爪——不要跳过此步骤。仅在纯可视化或没有物体接触的可达性研究中才可省略。

在构建任何自定义夹爪之前，必须先阅读 references/gripper-models.md。 可用模型包括平行夹爪（robotiq2F85，开口宽度为 85mm）和真空夹爪（适用于大型／扁平物体的 robotiqEPick 变体）。不要假设 robotiq2F85 是唯一选项。

1. 阅读 references/gripper-models.md
2. 按照该文件中的决策树操作（检查兼容性 → 检查物体尺寸 → 选择抓取策略）
3. 仅当没有合适的现成模型时才构建自定义夹爪（操纵器不兼容，且物体不适合真空吸取）

gripper = loadrobot("robotiq2F85", DataFormat="row");
addSubtree(robot, "tool0", gripper, ReplaceBase=false);

重要：始终使用 ReplaceBase=false。 默认值（true）会将夹爪的基座连杆合并到父体中，从而悄然丢弃基座视觉网格（夹爪外壳／连接件）。所有现成夹爪都有基座视觉网格，不使用此标志就会丢失。

安装夹爪后，在夹爪的接触点添加一个接触坐标系（固定关节，不增加 DOF），然后对此坐标系求解 IK。这样可以避免混淆工具尖端坐标系的方向，也无需手动计算偏移量。有关各夹爪的偏移量和完整模式，请参阅 references/gripper-models.md。

contactFrame = rigidBody("contact_point");
contactJoint = rigidBodyJoint("contact_joint", "fixed");
setFixedTransform(contactJoint, trvec2tform([0, 0, gripperOffset]));
contactFrame.Joint = contactJoint;
addBody(robot, contactFrame, "tool0");

添加自定义末端执行器坐标系（未安装夹爪或需要特定偏移量时）：

ee = rigidBody("tool_tip");
eeJoint = rigidBodyJoint("tool_tip_joint", "fixed");
setFixedTransform(eeJoint, trvec2tform([0.1, 0, 0]));
ee.Joint = eeJoint;
addBody(robot, ee, "tool0");

第 5 步：通过运动学验证模型

使用 FK 和 IK 确认模型构建正确，包括连杆长度、关节轴、限位以及末端执行器坐标系的位置。

使用 FK 验证： 移动到已知配置，并检查末端执行器是否到达预期位置。

q = homeConfiguration(robot);
tform = getTransform(robot, q, "tool0");
fprintf("Home position: [%.3f, %.3f, %.3f] m\n", tform(1:3,4));

直观确认姿态是否合理：

figure;
show(robot, q, Frames="off");
axis auto;
title("Home configuration");

使用 IK 验证： 选择一个已知可达的目标，并确认求解器能够收敛。

选择——IK 还是 GIK：

用途	适用情况
inverseKinematics	单个目标姿态，无额外约束
generalizedInverseKinematics	多个约束（位置 + 瞄准、关节边界、方向等）

所有可用的 GIK 约束类型，请参阅 references/gik-constraints.md。

ik = inverseKinematics(RigidBodyTree=robot);
weights = [0.25 0.25 0.25 1 1 1];
targetPose = trvec2tform([0.4, 0.1, 0.3]) * eul2tform([0 pi 0], "ZYX");
[qSol, solnInfo] = ik("tool0", targetPose, weights, homeConfiguration(robot));

第 6 步：验证每个 IK/GIK 解（必需）

绝不要跳过此步骤。 Agent 经常跳过验证，交付悄然求解失败的结果。

在 inverseKinematics 之后：

if solnInfo.ExitFlag <= 0
    warning("IK did not converge. ExitFlag: %d, Status: %s", ...
        solnInfo.ExitFlag, solnInfo.Status);
end

tformActual = getTransform(robot, qSol, "tool0");
posError = norm(tformActual(1:3,4)' - targetPose(1:3,4)');
if posError > 1e-3
    warning("IK position error %.4f m exceeds threshold.", posError);
end

在 generalizedInverseKinematics 之后：

if solInfo.ExitFlag <= 0
    warning("GIK did not converge. ExitFlag: %d, Status: %s", ...
        solInfo.ExitFlag, solInfo.Status);
end

for i = 1:numel(solInfo.ConstraintViolations)
    cv = solInfo.ConstraintViolations(i);
    if cv.Violation > 1e-3
        warning("Constraint %d (%s) violated: %.4f", i, cv.Type, cv.Violation);
    end
end

对于连续航点——检查关节连续性：

for i = 2:size(qAll, 1)
    maxJump = max(abs(qAll(i,:) - qAll(i-1,:)));
    if maxJump > deg2rad(30)
        warning("Joint jump of %.1f deg between waypoints %d and %d.", ...
            rad2deg(maxJump), i-1, i);
    end
end

主要功能

Function	Purpose	Toolbox	Available From
loadrobot	加载带网格的内置机器人	Robotics System Toolbox	R2019b
importrobot	从 URDF/Xacro/SDF/Simscape 导入	Robotics System Toolbox	R2017a
rigidBodyTree	创建空机器人模型	Robotics System Toolbox	R2016b
setFixedTransform	设置关节变换（DH 或齐次变换）	Robotics System Toolbox	R2016b
addVisual	添加用于显示的可视几何体	Robotics System Toolbox	R2019a
addCollision	添加用于规划的碰撞几何体	Robotics System Toolbox	R2019b
addSubtree	将子树（夹爪）附加到刚体	Robotics System Toolbox	R2016b
getTransform	计算某个配置的正向运动学（FK）	Robotics System Toolbox	R2016b
inverseKinematics	求解单个目标位姿的逆向运动学（IK）	Robotics System Toolbox	R2016b
generalizedInverseKinematics	使用多个约束求解逆向运动学（IK）	Robotics System Toolbox	R2017a
show	可视化机器人配置	Robotics System Toolbox	R2016b

模式

可视化

figure;
show(robot, q, Frames="off");
axis auto;
view(45, 30);
title("Robot at configuration q");

对于多个配置，请使用 tiledlayout/nexttile：

figure;
tiledlayout(1, 3);
for i = 1:3
    nexttile;
    show(robot, qAll(i,:), Frames="off", PreservePlot=false);
    axis auto;
    title(sprintf("Waypoint %d", i));
end

使用初始猜测值提升 IK 收敛性

为一系列连续航点求解 IK 时，请将上一次的解用作初始猜测值：

qPrev = homeConfiguration(robot);
for i = 1:numWaypoints
    [qSol, solnInfo] = ik("tool0", targetPoses(:,:,i), weights, qPrev);
    % ... validate ...
    qPrev = qSol;
end

约定

• 创建或导入时，始终将 DataFormat="row" 设为数据格式
• 关节限位使用弧度（以角度指定时使用 deg2rad）
• 使用 Frames="off" 和 axis auto，获得简洁的可视化效果
• 使用 tiledlayout/nexttile，而不是 subplot
• 为末端执行器坐标系使用描述性名称（例如，"tool_tip"、"bucket_tip"）
• inverseKinematics 的权重格式为 [orientation(3) position(3)]——对于位置优先任务，应设置较高的位置权重

常见错误

错误	错误原因	正确做法
明明 loadrobot 中已有机器人，却从 DH 模型构建	没有碰撞网格 → 规划会静默失败	先检查 loadrobot 库
使用 addCollision 设置视觉外观	碰撞几何经过简化，不会由 show 渲染	显示时使用 addVisual，规划时使用 addCollision
跳过 IK 验证	解可能无法收敛；ExitFlag <= 0 表示失败	始终检查 ExitFlag，并通过 FK 验证位置误差
使用 DataFormat="column" 或 "struct"	与要求使用行向量的规划／轨迹函数不兼容	始终使用 DataFormat="row"
操作任务中省略夹爪	与物体交互（拾取、抬起、抓取）需要模型上装有夹爪	任务涉及接触物体时，始终安装夹爪
在没有接触坐标系时对 "tool0" 或末端本体求解 IK	"tool0" 是法兰（与接触点有偏移）；末端本体坐标系存在令人困惑的 90° 旋转	在夹爪偏移处添加固定的 "contact_point" 坐标系，并以该坐标系为目标求解 IK
从头开始构建自定义夹爪	提供的模型几何精确，可直接安装	查看 references/gripper-models.md，了解可用的夹爪
对尺寸超过 85 mm 的物体使用 robotiq2F85	夹爪开口不足以抓取该物体	将物体尺寸与夹爪最大开口进行比较；大型物体请使用真空吸盘
需要多个约束时使用 IK	inverseKinematics 只能处理单个位姿目标	使用带约束对象的 generalizedInverseKinematics
未检查航点序列的关节连续性	解之间的大幅跳变会导致轨迹不安全	比较连续的解，并标记超过 30 deg 的跳变
使用 addSubtree 时未设置 ReplaceBase=false	夹爪基座视觉网格会被静默丢弃；可视化中的外壳会消失	安装夹爪时始终传入 ReplaceBase=false
将夹爪安装到 KUKA iiwa 时未进行坐标系校正	iiwa_link_ee 的 Z 轴与机械臂垂直；夹爪会朝向侧面	安装前添加带 Ry(90°) 校正的 tool0 — 请参阅 references/ee-frame-alignment.md
假设所有 loadrobot 模型的 tool0 都以 Z 轴朝外	某些机器人（KUKA iiwa）没有 tool0，或末端执行器坐标系未对齐	查看 references/ee-frame-alignment.md 中的表格，或在安装夹爪前运行诊断
对闭环并联机器人使用 rigidBodyTree	无法表示闭合运动链	请参阅 references/parallel-robot-guidance.md

---

Copyright 2026 The MathWorks, Inc.

---