托盘机器人在医院中的应用

第3章 托盘机器人：在医院中运送手术器械

在本章中，我们详细介绍了一种名为托盘机器人（TrayBot）的鲁棒集成系统，该系统可自主执行资产操作；具体而言，是在医院的无菌处理中心内运输可重复使用的手术器械托盘。我们的方法基于一种认知决策机制，用于规划并协调机器人基座和机器人机械臂在特定处理位置的运动。我们开发了一种基于视觉的机械臂控制算法，使机器人能够可靠地定位并随后抓取手术器械托盘。此外，为了补偿感知与导航误差，我们设计了一种鲁棒自对准末端执行器，从而在更大工作空间中实现更高的误差容忍度。

3.1 引言

灭菌过程中的关键任务之一是拾取装有手术器械的托盘，并将其从一个位置运输到另一个位置。例如，需要从计数站拾取托盘并将其运送到清洗站。拾取任务通常要求机器人自主导航至足够靠近目标的位置，以进行后续操作。该操作包括识别托盘，并通过视觉伺服将机械臂移动到空间中的正确位置，以便末端执行器能够抓取托盘。

为此，我们开发了一款名为托盘机器人（TrayBot）的移动机器人。

具有操作能力的自主移动机器人能够扩大工作空间的灵活性，并消除为完成相同任务而需要多个固定式机器人的情况。因此，我们设计了托盘机器人（TrayBot），用于在无菌处理中心内运输装有手术器械的托盘。针对移动机器人的成功操作方案需要克服多项挑战，以实现导航与操作能力的集成。移动性要求具备在工作环境中进行定位和导航的能力，同时避免障碍物碰撞[Choset (2005)]。操作则高度依赖于搜索所需的操作点、在高维任务空间中规划运动轨迹，以及使用末端执行器抓取物体[Lewis et al.(2003)]。为了确保操作的可靠性，需要采用一种高度集成设计，将导航在运行不确定性不断增加的情况下进行导航与操作是必要的。导航与操作中的规划应共享信息，以提高成功物体操作的几率。然而，由于噪声和不确定性引起的误差使得操作任务更加困难。导航系统的误差通常比操作系统更为显著，这种不平衡的误差分布需要具备在不同层级处理误差能力的算法，并能在最终操作过程中补偿误差。

本章其余部分组织如下：第3.2节讨论移动机器人操作、基于视觉的控制和末端执行器设计方面的相关工作；第3.3节讨论硬件组件的设计；第3.4节介绍集成系统的架构，并解释系统中的主要模块和算法；第3.5节重点介绍实验设置与结果；第3.6节总结本章并提出未来工作。

3.2 相关工作

使用移动机器人的操作最近引起了机器人领域社区越来越多的关注。研究人员提出了大量架构和方法，将感知、规划和执行确定为这类操作任务的三个关键问题。在使用移动机器人的操作中，执行的一个重要特征是将移动性与操作相结合。典型的用于移动操作的机器人包括：Willow Garage的PR2[Willow Garage (2015)], 德国宇航中心的Rollin’ Justin [DLR (2016)], DFKI机器人创新中心的AILA [DFKI (2018)], PowerBot[Adept Mobilerobots (2016)], Robotnaut[美国国家航空航天局约翰逊航天中心 (2016)], HERB [Srinivasa et al.(2010)], 等。

移动操作的一项重要技术是同步定位与建图（SLAM）[Leonard和 Durrant‐Whyte（1991）],，该技术旨在构建或更新环境地图的同时，持续跟踪智能体在该环境中的位置。Klein和Murray提出了用于小型增强现实工作空间的并行跟踪与建图（PTAM）[Klein和Murray（2007）]，以实现无标记视觉 SLAM。他们的主要贡献在于将跟踪与建图并行执行，从而允许以批处理方式完成计算成本较高的优化。Mur‐Artal等人提出了一种名为ORB‐SLAM[Mur‐Artal et al.(2015)]的系统。ORB‐SLAM使用ORB特征 [Rubleeet al.(2011)]来跟踪并在环境中重建三维地图点。除了仅使用视觉传感器外，与其他传感器的融合通常可以提供更好的定位结果 [Kam et al.(1997)]。在我们的系统中，我们采用基于激光的导航与定位软件，这是PowerBot平台的一部分。

一些研究人员致力于开发用于移动操作的运动学和动力学控制系统。塞拉吉提出了一种统一方法，将基座非完整约束和运动学冗余统一处理，以形成针对移动操作器的新任务约束[Seraji (1998)]。Padois 等。

al. 提出了一种用于轮式移动机械臂的反应式控制的统一建模框架，该框架特别适用于在动态环境中处理复杂任务[Padois et al.(2007)]。由于工作环境被设计为结构化良好的环境，在特定处理位置规划和协调机器人基座与机器人机械臂运动的认知决策机制已足够。

在非结构化环境中，机器人如何执行复杂抓取操作正受到越来越多的关注。例如，Papazov 等人使用 Kinect 传感器对家用物品场景进行数字化，以实现机器人抓取 [Papazov et al.(2012)]。PR2 利用激光扫描仪进行物体操作 [Chitta et al.(2012)]。Collet 和 Srinivasa 提出了一种方法，能够识别场景中的所有物体，并基于使用 SIFT 描述符学习到的物体模型估计其完整位姿 [Collet et al.(2011)]。为了提高检测精度，采用了一种配备三个摄像头的多视角方法。Saxena 等人训练了一个检测器，可以从两张以上的图像中识别出若干良好的拾取点 [Saxena et al.(2008)]。然后通过三角测量获得三维拾取位置。在我们的系统中，由于我们可以自由地对手术器械托盘进行增强以便于操作，因此仅使用安装在机器人末端执行器上的单个摄像头来定位托盘附件上的基准标记。该基准标记与抓取点相关联。

设计良好的机器人夹爪可以提高操作任务的成功率。Causey 和 Quinn[Causey and Quinn (1998)]提出了夹爪设计的一般准则。自对准夹爪因其能够增强操作可靠性而广受欢迎。Zhang 和 Goldberg 提出了一种模块化方法，用于设计梯形夹爪钳口，通过推、翻倒和固定零件，使其达到期望的方向，适用于标准平行夹爪[Zhang and Goldberg (2001)]。Sam 和 Nefi 设计了一种柔性夹爪，用于处理不同尺寸和形状的食品[Sam and Nefti (2008)]。我们还设计了一种自对准夹爪，以应对定位误差，实现手术器械托盘的快速定位与抓取。

3.3 硬件组件设计

由于我们可以自由更改或增加无菌处理过程中使用的组件，因此我们设计了一些功能以实现高效的托盘识别、抓取和运输。在本节中，我们将讨论设计细节。

手术器械在手术室使用后会被放置在托盘中，并通过推车运送至序列处理中心。

图3.1（顶部）显示了用于运输托盘的推车。在我们的系统中，推车表面的高度为 88厘米，推车表面的尺寸为90厘米（长）× 60厘米（宽）。我们设置中的托盘尺寸为28厘米（长）× 19厘米（宽），如图3.1（底部）所示。托盘在推车上放置时没有固定的位置和方向。每个托盘上都安装有一个附件，供机器人抓取。该附件还配备了视觉基准标记，以实现易于识别和定位。

该任务要求移动机器人移动到推车处并拾取一个托盘。由于PowerCube机械臂的机械限制，机器人无法到达推车表面的每个角落。根据本项目所用托盘的尺寸，我们计算出了推车表面上的操作区域，如图3.2所示。阴影区域定义了托盘应放置的位置，在此区域内，托盘可按夹爪方向限制内的不同方向放置。期望工作位置设计为前轮轴线中心与小车边缘之间距离为86cm，相当于机械手基座中心与小车边缘之间距离为56cm。

我们系统中使用的机器人是由Adept制造的Pioneer 3 PowerBotr。机器人表面安装有一个6自由度的Schunkr PowerCube机械臂，用于执行操作，如图 3.3（顶部）所示。我们在机械臂第六连杆上安装了一个摄像头，以从俯视角度获取操作区域的图像，如图3.3（底部图像，顶部圆圈）所示。图3.3（底部图像，底部椭圆）中所示的附加末端执行器由我们实验室开发，用于处理来自视觉、移动和操作过程中产生的误差。托盘上设计并安装有连接部件。夹爪和连接部件具有两组正交的

通过夹爪的简单闭合实现托盘相对于机械臂的自对准斜面。这些在图3.4中进行了说明。上钳口和连接部件上部的斜面特征对X平面内的平移进行对准。下钳口和连接部件下部的斜面特征对 Y平面内的平移进行对准。夹爪完全闭合则约束了Z平面内的平移。托盘的俯仰、滚动和偏航也由这些斜面特征以及夹爪的闭合来约束。

移动机器人的软件系统运行在搭载1.8GHz Intel Core 2 CPU 4300的车载计算机上。我们使用ROS Fuerte与Ubuntu 12.04作为软件平台。

3.4 集成系统设计

本节中，我们将详细说明集成系统的架构，并解释系统中的主要模块和算法。

3.4.1 系统概述

图3.5 展示了我们托盘机器人所使用的通用系统架构。整个系统由 任务规划 模块控制，该模块负责规划机器人系统中的所有活动。

给定任务后，系统会规划机器人行进的航路点，并考虑避障。生成的航路点随后被发送用于任务规划的验证。此验证过程确保避免与障碍物发生碰撞。为了满足安全要求，在任何无法避免碰撞的情况下，系统将推迟任务。航路点的成功验证会触发机器人基座运动规划模块，以生成机器人在环境中导航的路径。反应式避障也包含在机器人基座运动规划中。在此过程中，应用了基于激光的SLAM以提高定位精度。

到达托盘存储车附近的航路点序列中的目标位置后，托盘机器人开始在环境中搜索操作点。在执行搜索任务期间，托盘机器人将机械臂移动到推车表面上方，并捕获一段图像序列。摄像头获取的图像经过处理以检测方形基准标记。如果找到标记，则将托盘ID以及该方形基准标记的位置和姿态发送至任务规划进行决策。如果托盘机器人无法找到基准标记，任务规划将生成信号通知人工操作员任务失败。

仅凭位置和姿态数据无法保证托盘机器人能够在环境中成功抓取托盘，而任务规划负责做出决策。该决策基于机器人的能力以及安全问题。例如，即使机器人能够看到物体，也可能由于机械臂的机械限制而无法执行操作任务。机器人基座的移动性可以增加完成操作任务的机会，因为它能够进行微小的位置调整。任务规划模块决定机器人基座是否需要移动以操作物体。如果即使机器人移动到其他位置也无法操作物体，则任务规划模块将拒绝该任务。

在任务规划模块确定操作任务在机器人的能力范围内后，将规划机器人基座和机械臂的运动轨迹。生成的运动轨迹被发送到导航和操作模块以完成任务。在第 3.4节的其余部分，将对每个模块进行更详细的讨论。

3.4.2 任务规划

任务规划模块是控制系统的关键部分。机器人需要决定何时控制底座或机械臂，以确保任务成功完成并最小化能耗。该决策过程基于融合来自环境、机器人、其他机器人以及先验知识的感知信息。因此，采用认知决策过程来解决导航与操作规划问题。<表3.1>展示了本系统中任务规划的伪代码。

表3.1 任务规划模块的伪代码

重复直到所有搜索点被搜索或检测到物体)
将机械臂移动到下一个搜索点
检测物体
if(检测到物体)
优化 Energy Consumption Cost Equation
if(物体在期望的操作区域内)
计算操作点
规划机械臂的运动轨迹
操作
else
规划机器人基座的运动轨迹
将底座移动到新的工作位置
检测物体
if(检测到物体)
计算操作点
规划机械臂的运动轨迹)
操作
endif()
将机器人基座移回原始工作位置
endif()
endif()
End

在导航阶段，由任务规划触发，路径点规划 生成一系列路点，以支持在工作位置处执行操作任务。在航路点生成并经过验证后，任务规划将触发机器人基座运动规划，使用基于势场的方法[克哈提布（1986）]来规划机器人底座的运动轨迹。该规划过程为机器人提供了通过所有已规划航路点的行进方案，并在每个加工位置执行相应的期望操作任务。

当机器人到达一个加工位置后，需要执行某些操作任务，例如抓取。在操作阶段，机器人有两个主要的子任务：1）确保托盘的抓取和拾取；2）最小化能耗。因此，操作阶段的规划策略是移动机器人基座以增加工作空间和操作的成功率，并优化能耗方程，例如，通过稍微移动基座，机器人可以在执行操作任务时减少关节所需的扭矩。

3.4.3 导航与定位

机器人基座在环境中的导航需要完成两个部分的工作：定位和运动控制。在我们的系统中，我们使用了激光传感器来提高定位的精度。避障功能通过在系统中集成一种反应式的基于势场的运动控制方法实现。例如，每次机器人在从A到B的运动过程中发现其运动路径上存在障碍物时，它会重新规划路径以避开该障碍物。

3.4.4 基于视觉的机械臂控制

为了实现对放置在推车上的工具托盘的识别与定位，我们将一个奇利标签放置在附件的小块水平表面区域上，如图3.6所示。Chilitag是一种用于增强现实应用中的方形黑白基准标记[Bonnard et al.(2013)]。每个奇利标签提供托盘ID，同时还提供托盘的位置和方向信息。安装在夹爪上方15cm处的摄像头用于检测奇利标签。该摄像头向下观察推车表面的水平面。标准图像

在进行Chilitag检测之前，应用去畸变来校正径向畸变。

3.4.4.1 搜索托盘

系统通知存在带有标识号的托盘后，将采用一种搜索策略在工作空间内查找目标 Chilitag。由于机械臂长度有限且安装的摄像头高度受限，我们开发了一种M步搜索流程。在每一步中，机器人带动其末端执行器及摄像头在N条直线路径上从左到右或从右到左移动。这些直线路径由近及远分布，在每条路径上定义了L个搜索点。在每个搜索点，托盘机器人通过摄像头采集图像，并判断目标Chilitag是否存在。完成从左到右N条线的搜索后，若仍未找到目标Chilitag，托盘机器人将底座向前移动距离d，并在接下来的N条直线路径上继续搜索。这些路径的几何布局与前一步保持相同。整个过程共执行M步，在最坏情况下的搜索点总数为MxNxP。图 3.7展示了我们的搜索策略示意图。经过M步搜索后，若仍未找到目标Chilitag，机器人将通知人工操作员搜索失败；否则，机器人将计算该Chilitag的位置和方向。

第3章 托盘机器人：在医院中运送手术器械（续）

3.4.4.2 确定操作点

在标定阶段，我们使用摄像头采集夹爪即将完全抓取托盘附件时的图像帧，并将 Chilitag中心的图像坐标存储为参考点C0。在实时操作过程中，托盘机器人持续采集一系列图像，直到检测到中心位于图像坐标C1的Chilitag。由于摄像头的图像平面与推车表面平行，仅需二维平移即可将夹爪移动至目标位置进行抓取。该平移向量 T可通过公式 T = C1 - C0 简单计算得出。然后，我们根据Chilitag的实际与观测尺寸，将平移向量 T从图像坐标缩放至世界坐标。Chilitag的方向可根据其检测到的四个角计算得到。Chilitag的世界坐标反映了操作点的位置。

如果操作点距离基座连接件太远，机械臂将过度伸展，导致控制系统不稳定性以及末端执行器处的振动。如果操作点过于靠近基座连接件，则逆运动学解的求解失败率可能会增加。因此，我们将操作点限制在期望区域内。

为了找到操作和机器人基座的最佳运动，我们采用了一种利用机器人移动性的决策机制。能耗函数定义如公式3.1所示：

$$
L = f(J, Q_J) + D^T Q_d D \quad (3.1)
$$

其中，$ J $ 是表示机械臂关节角度的六维向量，$ D $ 是改变机器人基座与推车之间距离的位移值，$ Q_J $ 和 $ Q_d $ 是权重矩阵。当机器人通过改变 $ D $ 处于不同位置时，可计算出抓取托盘上附件所需的运动学构型。我们通过采用如公式3.2所示的线性方程简化了消耗函数：

$$
L = J^T Q_J J + D^T Q_d D \quad (3.2)
$$

通过最小化公式3.2，可以计算出期望的位移运动距离 $ D $。如果 $ D $ 小于预定义阈值 $ D_{th} $，机器人将向前或向后移动，位移量为 $ D - D_{th} $。如果机器人移动其底座，则需要重新执行搜索过程以确定Chilitag的世界坐标；如果无需移动其底座，则进入任务的操作阶段。

3.4.4.3 抓取托盘

在操作阶段，机器人规划末端执行器的运动轨迹以操作物体。运动规划应避免机器人与环境之间的碰撞。因此，我们在运动规划中设置了两个约束：一是避免自碰撞，二是避免与环境中任何物体发生碰撞。

3.5 实验设置和结果

所提出的集成导航与操作系统已得到验证。对于搜索部分，我们选择 M = 2、N = 4 和 L = 3。每条N线的长度均为30厘米。线间距离为5厘米，我们选择 d = 20 cm。这些数值的选择是因为从相机获取的图像最多可覆盖16 cm宽的区域。我们的参数设置使机器人能够覆盖 50 cm × 46 cm 面积，以满足项目要求。对于第3节中的操作规划，我们选择 $ D_{th} = 40 $ cm。

根据上述要求，我们设计了两个实验：
- 确定成功拾取托盘的抓取操作的成功率。
- 确定托盘方向的最大角度，该角度仍可使自对准夹爪成功抓取托盘。

3.5.1 成功率

托盘被随机放置在推车的操作区域内（图3.2），且方向固定为 θ = 0°。

图3.8 显示了在一次实验运行期间获得的图片。开始时，托盘机器人移动到推车附近的路点（a）。托盘机器人随后搜索托盘（b）。托盘机器人在N次线搜索后仍未检测到托盘。然后，托盘机器人向前移动以搜索接下来的N行。在搜索过程中，托盘机器人检测到了托盘。随后，规划并执行运动轨迹以到达托盘上的附件位置（c）。接着，托盘机器人通过闭合两个夹爪来抓取附件（d），抬起托盘（e），并移动到下一个位置（f）。

我们将托盘放置在推车表面工作区域内的20个不同位置，测试了托盘机器人拾取托盘的能力。托盘机器人成功拾取托盘的次数为100%。由于相机畸变和/或关节角度限制所产生的误差，机器人计算出的抓取点可能存在高达1 cm的误差。由于这些误差，托盘机器人能够抓取托盘上的附件，但精度不够高。然而，这一问题通过我们的自对准设计得到了缓解。

3.5.2 夹爪的角度容差

如图3.9所示，我们将托盘以三种不同的方向 θ = 0°、θ = −15° 和 θ = 15° 放置。在这三种情况下，托盘机器人均能成功抓取托盘，并可利用其定制夹持器将托盘的方向与夹爪对齐。
θ=0° (d–f) θ=−15° (g–i) θ=15°)

我们将托盘放置在四个不同位置，并测试最大可容忍角度，如图3.10所示。在每个位置，托盘的方向从 θ = −30° 变化到 θ = 30°。然后我们计算抓取操作的成功率以及机器人能够使用其末端执行器处理的最大角度。本实验旨在测试使用我们定制的误差容忍度。

当托盘位于左上位置时，最大可容忍误差角度约为 ±25°。对于其余三个位置，最大可容忍误差角度为 ±30°。这是合理的，因为左上位置是机械臂能够到达的最远点。不同的关节对关节角度有不同的限制。某些关节可能无法达到期望操作器构型所需的角度，但这不会影响整体系统性能。基于这些实验结果，我们可以得出结论：我们开发集成导航与操作系统的方法能够满足项目的需求。

3.6 结论与未来工作

本章介绍了我们的集成导航与操作系统——托盘机器人（TrayBot），该系统能够从推车上拾取托盘，并在无菌处理环境中进行导航中心，并将托盘放置在指定的桌面上。该系统集成了规划、决策、定位、导航和基于视觉的机械臂控制。末端执行器的特殊设计增强了系统的误差处理能力。实验结果表明，我们的方法是有效的，能够满足项目需求。托盘机器人可轻松适应其他需要机器人进行导航与操作的运输项目。

我们希望在未来的研究中进一步探讨几个问题。目前，我们的系统使用奇利标签，这实现了快速的图像处理和检测。然而，在某些情况下，在物体上添加额外的基准标记是不合理的。我们计划分析物体的特征，例如形状和纹理，以提升视觉检测模块的性能。此外，当前系统中的决策过程是确定性的。未来，我们希望研究在不同层次上整合任务约束和环境约束，以增强规划与决策模块的灵活性和鲁棒性。