CH-10 机器人系统与实验
机械臂准备抓起桌上的物体。相机提供目标位置,状态估计器给出当前关节状态,策略发出动作,驱动器带动夹爪靠近。前几章已经介绍了这条链路中的运动学、动力学、估计与决策;把这些算法接起来,还要弄清每个接口究竟传递什么,以及数据在什么时候生效。
同一条命令可能因惯量参数、执行器配置或观测延迟而产生不同响应。本章沿着这次抓取实验,先检查连杆、关节、惯量、执行器与传感器,再看 ROS 2 如何传递数据、调度回调和保存记录。后面的仿真与训练都将沿用这些物理和时间约定。
1. URDF 物理模型
夹爪靠近桌面时,仿真器需要判断是否发生接触;机械臂抬起物体时,又需要计算重力和惯性负载。画面中的连杆外形只解决了显示问题,碰撞形状、质量与惯量才为这些物理计算提供输入。
URDF 中的 link 描述连杆,joint 描述连杆之间的连接。只提供 visual 元素,可以描述机器人看起来是什么样;要进行碰撞检测或物理仿真,还需要 collision 与 inertial 等属性。
读 URDF 时,可以对照第 03、04 章所需的输入:joint 的轴和父子关系用于正向运动学,质心和惯量用于动能与重力计算,碰撞体用于接触检测。文件中的每一项都有具体用途,检查模型也应沿着这些用途展开。
尺寸和单位是第一步。把网格缩小到毫米尺度却仍按米解释,会同时改变碰撞尺度与力臂;相机缩放可能掩盖外观上的差异,动力学参数却不会随之纠正。因此,要先统一米、千克、弧度和秒,再确认零位下每个 joint 的轴表达在指定坐标系中。第 12 章从 URDF 计算末端位姿时,也会继承这些约定。
碰撞元素与视觉元素都位于 link 下,用 geometry 定义形状,用 origin 定义相对于连杆坐标系的位姿。碰撞计算关心接触边界,显示则关心外观,因此两者可以采用不同几何体。复杂网格之间的碰撞检测比简单几何体更昂贵,实践中可以用简单形状近似视觉网格,也可以适当扩大碰撞几何,为敏感设备留出禁止接近的区域。
惯性元素提供质量、质心位置和转动惯量。质量以千克表示;对称的三阶惯量矩阵只需六个独立元素:
计算惯量需要知道质量如何分布。缺少详细数据时,可以先假设体积内质量均匀分布,再按几何形状近似计算。origin 指定质心及惯性参考系相对于连杆参考系的关系,也必须一起检查。零或近零惯量可能使仿真表现异常;直接填入单位矩阵,则可能让惯量远大于实际值。
参考点之所以重要,可以从惯量定义直接看出。对质心坐标系中的质量微元位置 $r$,有 $I_C=\int(\|r\|^2\mathbf 1-rr^T)\,dm$,绕单位轴 $u$ 的动能为 $\tfrac12(u^TI_Cu)\omega^2$。质量离转轴越远,对惯量的贡献就越大。
具有三维质量分布的连杆,绕质心三个主轴的惯量应为正,主惯量还应满足三角不等式,例如 $I_1+I_2\ge I_3$。左侧减右侧等于某一坐标平方积分的两倍,因此对称、正定的矩阵也未必对应合理的质量分布。薄片极限可以出现等号,检查时应保留这种情况。
若 $R$ 把质心惯性轴旋转到所需轴系,$d$ 是新参考点到质心的向量,则同轴系中的平行轴关系为
URDF 的惯性数据以质心为参考点。检查关节轴负载时会用到 $I_O$,但文件中的质心惯性元素仍应保留质心惯量;把 $I_O$ 填回该元素,会使后续坐标转换重复加上平移项。第 04 章的递归动力学正是从各连杆的质心惯性出发,将其变换到递推需要的位置。
关节的 dynamics 可以描述摩擦和阻尼。移动关节的摩擦单位为牛顿,转动关节为牛顿米;对应阻尼单位分别为牛顿秒每米、牛顿米秒每弧度。未指定时,这些系数默认为零。
1.1 立方体惯量
考虑一个边长 0.1 m、质量 0.6 kg 的均匀立方体。其对角惯量为 0.001 kg·m²;同样尺寸若把对角惯量填成 1,对应质量就会达到 600 kg。
1 | side, mass = 0.1, 0.6 |
填写连杆惯量前,可以先做这样的量级核对,再根据机器人的实际质量分布确定参数。
现在将转轴平行移到距质心 $0.05\,\mathrm m$ 的位置,这个距离垂直于转轴,轴向仍平行于立方体的 $z$ 轴。平行轴项为 $0.6\times0.05^2=0.0015\,\mathrm{kg\,m^2}$,该轴惯量便增至 $0.0025\,\mathrm{kg\,m^2}$。忽略重力、摩擦和其他连杆,用 $0.01\,\mathrm{N\,m}$ 驱动,角加速度为 $4\,\mathrm{rad/s^2}$;若误用质心惯量,则会算出 $10\,\mathrm{rad/s^2}$。
两次计算中的质量完全相同,仅参考轴的位置发生变化,角加速度就相差 2.5 倍。惯量与时序复算脚本用标准库核对这些数值,也会输出后面的数据年龄案例,无需启动 ROS 或物理引擎。
2. MJCF 模型与状态
同一机械臂可以从多个初始姿态进行仿真实验。模型描述连杆、关节和参数,每次实验的状态另存于运行数据;这样才能区分“换机器人模型”和“重置一次试验”。
MuJoCo 使用 MJCF XML 描述场景,也可以加载 URDF。MJCF 会经过编译,形成用于计算的 mjModel;运行时的动态变量和中间结果保存在 mjData。前者描述模型,通常保持不变;后者是计算过程反复读取和写入的工作区域。
有了独立的运行数据,同一模型就能配合多个状态和控制实例进行仿真,重置某次试验时也不必重新定义机器人结构。模型可以保存为编译后的二进制形式;为了继续编辑、检查和维护,还应保留可读的模型源。
例如比较两种控制器时,应使用同一套质量、关节限制与碰撞参数,并为每种控制器准备独立的初始状态和运行缓存。这样,轨迹差异才有机会归因于控制输入。模型文件规定实验条件,状态快照规定实验起点,两者都要随结果保存。
URDF 主要表达机器人结构,MJCF 还可以直接组织场景、执行器和传感器。将 URDF 导入仿真器后,需要检查编译结果中的惯性、关节和限制,再确认补入的执行器与接触配置。成功加载只确认文件满足语法与编译规则,物理参数仍需按实验用途核对。
MuJoCo 用关节坐标表示状态,并把刚体组织为运动学树。无关节的子刚体固定在父刚体上;关节为子刚体相对于父刚体增加自由度。球关节、移动关节、转动关节和自由关节具有不同的自由度数量。自由关节可以表示浮动刚体,球关节和自由关节中的方向采用单位四元数。
Body 具有质量和惯性属性,附着其上的 geom 定义几何形状。一个 body 可以拥有多个 geom,因此可以用多个凸几何体表示复杂物体。geom 参与碰撞和渲染,某些情况下还可用于自动推断质量和惯量。
Site 标记刚体上需要关注的位置,例如传感器位置或肌腱路径上的点。它不参与碰撞检测或自动惯量计算。
关节数、位置坐标数和速度自由度数不一定相同。位置坐标定义系统的构型,速度位于对应构型的切空间;自由关节的四元数坐标就是一个需要注意的例子。
运动学树不能直接形成闭环;机械耦合和闭环可以通过等式约束表示。模型文件因此同时表达树状结构和额外约束,需要按其用途理解各元素。
3. 执行器与传感器
策略输出关节位置目标时,执行器把目标转换成物理作用;策略读取关节或 IMU 观测时,传感器又决定它接收到哪个时刻的数据。这两个接口连接了模型、控制与反馈。
MuJoCo 的执行器模型由传动、激活动力学和力生成三个部分组成,各部分可以独立配置。传动规定执行器与关节、肌腱、刚体等对象的连接;激活动力学描述液压、气动或肌肉式执行器的内部状态。力生成机制将标量控制信号转换成标量力,传动再将其映射为广义力。
解释策略输出之前,需要先看执行器配置。同一个数值可能表示位置目标、速度目标,也可能直接参与力的生成。控制输入和输出力还可能分别受到限幅,最终作用到机器人上的力由整条转换链决定。
以单关节为例,直接力矩模式下,输入经过传动比例产生 $\tau$;位置模式下,输入为 $q_d$,低层伺服可近似写成 $\tau=k_p(q_d-q)-k_d\dot q$,随后还可能限幅。$k_p$ 的单位是牛顿米每弧度,$k_d$ 是牛顿米秒每弧度。若 $k_p=20$、误差为 $0.1\,\mathrm{rad}$,零速度下请求力矩为 $2\,\mathrm{N\,m}$;力矩上限若为 $1$,执行器便进入饱和区。
第 05 章推导闭环增益时,通常假设执行器能够实现命令。这里的饱和和内部延迟会改变这一前提。上述单关节近似帮助我们理解接口;具体 MuJoCo 模型仍需按其传动、增益和偏置配置解释控制量。
仿真传感器包括接触、IMU、力矩、关节位置和速度、刚体坐标系、测距等类型。输出保存在 mjData.sensordata,供用户计算或数据分析使用;传感器输出本身不自动参与 MuJoCo 内部动力学计算。自定义传感器可以加入其他观测量。RGB 和深度相机还涉及渲染过程,需要通过相应程序接口获取。
传感器的计算频率也可以不同于物理仿真步长。MuJoCo 的模型文档提供历史缓冲、采样间隔和读取延迟的机制:较大的采样周期使观测只在部分仿真步更新,延迟则使读取来自更早的时间。两者可以共同存在。
读取历史数据还要考虑插值和缓冲初始化。若延迟超过已保留的历史范围,读取过程可能出现外推,得到的数据未必符合预期。这里的历史缓冲机制以文末列出的固定文档源提交为准。
3.1 历史时间戳
设采样间隔 dt=0.1 s、缓冲样本数 nsample=2。在显示时刻 0.3 s,缓冲中两个已有样本的时间戳为 0.1 s 和 0.2 s;读取 delay=0.2 s 的数据时,请求的是 0.1 s 的值。

1 | from math import ceil |
配套算例打印惯量和历史时间戳,只进行数值计算。对控制器而言,一条刚收到的消息也可能来自较早的采样。判断它能否用于当前动作,需要同时查看采样时刻。
实际系统可以分别记录采样、接收、回调开始和命令生效时刻。设它们为 $t_s,t_r,t_c,t_a$,控制所用数据的年龄为 $t_a-t_s$,其中包括传输 $t_r-t_s$、排队 $t_c-t_r$ 和计算及执行 $t_a-t_c$。这几个时间差分别指向不同的改进位置。
假设相机在 $1.000\,\mathrm s$ 曝光结束,$1.006$ 收到消息,$1.020$ 开始推理,$1.032$ 命令生效。此时数据年龄为 $32\,\mathrm{ms}$,网络传输仅占 $6\,\mathrm{ms}$。若物体以恒定速度 $0.5\,\mathrm{m/s}$ 运动,它已移动 $16\,\mathrm{mm}$。这个数值是匀速条件下预测误差的量级估计,没有使用相机实测数据。
计算时间差之前还要统一时钟。源时戳和接收时戳若来自不同机器,应先估计时钟偏差,否则差值会混入时钟误差。仿真时间与墙钟时间也需分别记录,尤其在加速回放时,两者的推进速度会不同。
时延也会影响第 05 章的反馈稳定性。在线性小信号近似下,固定纯时延 $d$ 在频率 $f$ 处引入相位滞后 $-2\pi f d$。将上述 $32\,\mathrm{ms}$ 近似为固定总时延,在 $5\,\mathrm{Hz}$ 处便有 $57.6^\circ$ 的滞后;脚本也核对了这个量级。
这些滞后会消耗控制器的相位裕度,是否失稳还要结合整个开环模型判断。实际时延若有抖动,相移也会随之变化,实验记录应保留数据年龄的分布与最大值,以便检查平均值掩盖的等待。
4. ROS 2 通信接口
抓取系统中的相机、状态估计、策略推理和机械臂驱动可以分别运行。相机持续发送图像,驱动器偶尔接收配置请求,高层程序则等待一次抓取完成。ROS 2 为这些不同持续时间和交互方式提供了相应接口。
ROS 2 的节点提供模块化计算接口。节点可以同时包含发布者、订阅者、服务客户端和服务端、动作客户端和动作服务端,并通过参数改变运行时行为。节点间的连接由分布式发现机制建立。
主题 topic 采用发布与订阅方式,适合传感器、机器人状态等连续数据流。生产数据的发布者和使用数据的订阅者通过主题名发现彼此。同一主题可以有零个或多个发布者、零个或多个订阅者。订阅者通常不需要关心某条数据由哪一个发布者产生,因此部件可以替换,记录和调试工具也可以作为额外订阅者接入。
ROS 的消息具有字段类型,也具有约定的物理语义。类型检查能保证一个字段是整数或字符串,却不能自动保证它被赋予正确物理含义。例如,IMU 角速度向量具有弧度每秒的语义,不能把另一种单位的量直接塞入而保持同样解释。
服务 service 采用请求与响应式远程调用:客户端发出请求,服务端计算并返回结果。它适合能够快速返回的操作。长时间执行、需要在异常情况下中断的过程更适合动作接口。同一服务名称应只有一个服务端,但可以有多个客户端。
动作 action 用于带反馈、可取消或抢占的长时间调用。例如,高层状态机可以要求导航模块前往目标点,执行中接收进度,并在需要时取消目标。动作服务端处理执行、反馈和取消,动作客户端发起目标并等待结果。
抓取程序可以用 action 承载“完成一次抓取”的高层目标,用 topic 连续发布关节状态和相机观测,用 service 查询或修改一次配置。这里的 ROS action 指通信接口;学习策略每步输出的动作则是控制变量,两者处于不同层级。
跨节点字段还应携带坐标系与时间语义。三维点位于相机系还是基座系,决定是否需要先应用第 02 章的刚体变换;位置目标是绝对值还是增量,决定接收端如何解释。同一种消息类型可以承载不同含义,记录系统应保存这些元信息,才能在离线阶段重建策略当时看到的输入。
5. 服务质量
相机连续发送图像,推理节点却可能来不及处理每一帧。控制器需要决定是等待积压帧,还是使用最新观测;关节状态也有自己的更新时间。QoS 规定通信层如何排队、发送和处理过期消息,是建立这种行为的一部分。
ROS 2 的服务质量 QoS 用一组策略控制节点间通信。配置可以分别作用于发布者、订阅者及服务端、客户端,但不兼容的配置可能阻止数据传递。可靠性只是其中一个维度。
- History 与 Depth:保留最近若干条样本,或在资源限制内保留全部样本;队列深度只在保留最近样本时起作用。
- Reliability:尽力传输可能丢失样本;可靠传输可以通过重试提高交付保证。
- Durability:临时本地持久性让发布者为稍后加入的订阅者保留数据;易失性则不尝试保存。
- Deadline:期望相邻消息发布间隔不超过的时间。
- Lifespan:消息发布到接收之间的最大有效时间,过期消息会被丢弃。
- Liveliness 与 Lease Duration:规定实体如何表明存活,以及多长时间未表明存活会被判定为失活。
传感器场景中,最新读数及时到达通常比每一条读数都最终到达更有价值。因此,预定义传感器配置采用尽力传输与较小队列。
发布者提供一种质量,订阅者请求它能接受的最低质量。两者每项策略兼容,才能建立连接。例如,尽力传输的发布者不能满足要求可靠传输的订阅者;可靠发布者可以连接接受尽力传输的订阅者。
服务使用易失性持久性,是为了避免重启后的服务端收到旧请求,再次执行已经产生副作用的操作。因此,持久性配置也需要按程序行为选择。
考虑一个理想单消费者队列:状态每 $10\,\mathrm{ms}$ 发布一次,消费者处理一条需要 $15\,\mathrm{ms}$。长期到达率为 $100\,\mathrm{Hz}$,超过约 $66.7\,\mathrm{Hz}$ 的处理率,即使网络没有丢包,保留全部消息也会让积压持续增长。保留最近少量样本能够限制存储,处理速度仍受计算耗时限制。
反馈控制可以在应用层按源时间戳选择最新可用状态,并拒绝过龄数据;完整离线重建则需要一条能够承受全部流量的记录路径。可靠重传提高交付机会,也可能增加消息到达时间,可靠性配置本身没有给出固定延迟上界。
Deadline 约定消息间隔,Lifespan 处理消息有效期;回调完成时间还受到计算调度影响。判断关节状态是否足够新,最终要检查数据年龄。设计 QoS 时,可以先确定控制允许的最大年龄,再为采样、传输、排队和计算分配预算。超过预算时,日志应能定位发生变化的环节。
6. 回调调度
消息到达后,订阅回调、推理和计时器回调还要获得执行机会。上一节讨论了数据如何传递,这一节继续追踪它在节点内部如何变成控制更新。
Executor 使用一个或多个操作系统线程,调用订阅、计时器、服务和动作等事件的回调。最简单的情况使用当前线程执行单线程 Executor;多个节点也可以加入同一个 Executor,由同一个线程服务。

图 1:Executor、客户端库与中间件的关系。Executor 调度可执行的回调。
接收到的消息保留在中间件中,直到回调获取它,而不是另外在客户端库建立消息队列。这有助于保持中间件 QoS 的语义。等待集合用标志表示队列是否有可读取的数据,也用于检测计时器到期。
多线程 Executor 可以并行处理多个消息或事件,能否实际并行还取决于回调组。互斥组不允许组内回调并行,Reentrant 组则允许。如果控制和估计回调并发读写同一模型的运行缓存,应建立同步或使用独立缓存。
线程增多之后,同一互斥组中的回调仍需等待,数据竞争也仍需处理。可以将耗时推理、可视化与短控制回调分组,测量各自最坏执行时间,再判断是否需要独立 Executor。传统等待集合只报告某队列有数据,没有提供所有队列的完整积压次序,因此不能普遍把其调度视为全局 FIFO。
Casini 等人研究 ROS 2 处理链响应时间时,将传感器到控制输出建模为一条计算链。论文分析的是 Crystal 版本及特定调度假设,其结论不能直接用作 Rolling 程序的实时保证,但测量整条链的方法有助于定位等待。
前面的 $32\,\mathrm{ms}$ 案例中,排队时间长于传输时间,优先优化网络未必能取得最大的改善。将第 08 章的估计器输入、第 09 章的策略推理与驱动输出放在同一条时间线上,才能估计反馈落后当前物理状态多久。
在仿真中追踪这条时间链,还需要知道物理状态怎样随步长推进。较大的仿真步长可以提高运行速度,也可能使积分不稳定。若状态快速发散,应同时检查步长、积分器,以及初始穿透等可能产生巨大排斥力的模型条件。下一章会用一个小模型展开这些检查。
7. 记录回放与系统辨识
保存抓取过程中的命令与观测,可以在离线阶段检查控制器使用了哪些状态,也可以回放输入、比较模型响应。先用一个移动机器人小例子熟悉记录过程,再将同样的工作流用于机械臂。
ros2 bag 将系统中指定接口的数据保存为记录,之后可以检查、分享和回放。最基础的练习是记录 turtlesim 的速度命令与位置。以下命令按 ROS 2 Rolling 文档核对,需要先完成所用发行版的安装,并在每个终端加载 ROS 环境。
在两个终端分别运行:
1 | ros2 run turtlesim turtlesim_node |
在第三个终端检查主题,并开始记录:
1 | ros2 topic list |
操作键盘终端使小海龟移动,结束时用 Ctrl-C 停止记录。-o 指定记录目录,多个主题由空格分开。查看记录及回放的命令为:
1 | ros2 bag info subset |
回放前停止键盘遥操作,保留 turtlesim。记录信息包括时长、消息数量、主题类型和各主题数量。命令主题只在输入时发布,位置主题可以持续发布,因此二者的消息数量和记录时长不应被简单看作一样。
回放路径可能受细小时序变化影响,并不一定与第一次执行逐点相同。连续记录还可以按时长或文件大小切分,避免单个记录过大。
延迟参数可以用于系统辨识。为此,先选最大可能延迟 $delay_{max}$,让缓冲样本数满足 $nsample=\lceil delay_{max}/timestep\rceil$;只要不超过该最大延迟,就可以在运行或辨识时修改执行器或传感器的延迟字段。
有了命令和观测,可以将第 04 章的动力学用于参数辨识。以低速单转轴为例,忽略库仑摩擦,设 $\tau(t-d)=I\ddot q(t)+b\dot q(t)+g(q(t))$。先按时戳对齐候选延迟 $d$;对每个 $d$,用 $\ddot q,\dot q$ 作为回归量估计 $I,b$,再比较未参与拟合的轨迹残差。
试验轨迹决定能够辨识哪些参数。匀速运动几乎不能激励惯量项,难以可靠地区分惯量与偏置,需要改变速度的试验提供信息。处理测量数据时,直接差分位置噪声以求二阶导数会放大高频噪声;采用积分形式或滤波可以改变这一问题,但滤波引入的相移也必须记录。
以水平转轴为例,扣除重力后的两组数据为 $(\ddot q,\dot q,\tau)=(2,1,0.015)$ 与 $(-1,2,0.0175)$。解两条线性方程,得到 $I=0.0025\,\mathrm{kg\,m^2}$、$b=0.01\,\mathrm{N\,m\,s/rad}$,恢复了前面移轴后的惯量。配套脚本核对了这个解析解。
真实试验中的力矩标定、噪声和延迟都有不确定性,还需要用独立轨迹检查预测误差。使用 bag 数据时,应保留物理采样时戳和初始条件:消息的到达时序不能代替采样时序,回放消息也不会自动恢复原来的接触状态。下一章将实验缩小为一个仿真模型,从状态和时间的检查开始,再逐步接入控制与学习。
参考资料
ROS 2 Documentation Contributors. Adding Physical and Collision Properties to a URDF Model. Collision; Inertia; Joint Dynamics. ROS 2 Documentation Contributors;中文节选重排、命令精简,Executor 原图保留;CC BY 4.0。
Google DeepMind. MuJoCo Overview; Modeling. Model and Data; Kinematic Tree; Actuator; Sensor; Divergence; Delays. © Google DeepMind / MuJoCo Contributors;中文节选,合并模型与传感器定义;文档采用 CC BY 4.0。
ROS 2 Documentation Contributors. Nodes; Topics; Services; Actions. Interface definitions.
ROS 2 Documentation Contributors. Quality of Service Settings; Executors. Policies; Profiles; Basic Use.
ROS 2 Documentation Contributors. Recording and Playing Back Data. Managing Topic Data.
Casini, Daniel, Tobias Blaß, Ingo Lütkebohle, and Björn B. Brandenburg. Response-Time Analysis of ROS 2 Processing Chains Under Reservation-Based Scheduling. ECRTS, 2019. End-to-end response-time analysis; the paper studies ROS 2 Crystal under its stated scheduling assumptions. 处理链分析及其适用边界;惯量、时间预算和辨识数值为独立教学推导。