机器人自主开门作业研究

王雪竹，杜 佳，张美玲，陈少南

（中广核研究院有限公司，广东 深圳 518031）

1 引言

随着机器人技术的发展，机器人越来越多地进入人的工作和生活环境中执行任务。对于需要在房间内外穿行的机器人，如核电站环境监测机器人、应急救援机器人、家庭服务机器人等，开门是基本功能需求。然而这些环境在建造时并未考虑机器人的需求，机器人自主开门作业面临诸多难点，包括对门的辨识、定位，以及开锁、推门、拉门过程中的轨迹规划和柔顺控制等。

研究人员提出了多种机器人自主开门方法。2009年，文献[2]针对KIST机器人，采用视觉和触觉结合的方式识别门把手位置，在抓住门把手后试探晃动，根据机器人末端位置变化识别门轴位置，并在整个开门过程中使用力控制保障机器人的安全。2010年，文献[3-4]针对PR2机器人，使用踝关节处的二维激光扫描仪进行导航，使用头部带云台的二维激光扫描仪辨识门轴和门把手的位置等信息，然后规划出一条无碰撞的运动轨迹完成开门。文献[5]针对非结构化环境中未知的门，采用基于视觉的学习方法识别图像中的门把手，然后使用立体相机获取其三维坐标。2012年，文献[6]对KUKA机器人设计笛卡尔空间下的阻抗控制，使用立体相机结合触觉获取精确的锁孔位置，插入钥匙打开门锁，然后根据触觉找到门把手，下压门把手开门。2015年，DARPA机器人挑战赛中，参赛的DRC-HUBO、CHIMP、Atlas等机器人纷纷在头部安装三维激光扫描仪和多个摄像机，用来扫描环境建立三维模型，进行开门作业[7]。

然而，更多的传感器往往意味着更大更重的机器人，影响其可达区域。尤其是对于核电作业机器人，耐辐照要求使得相机等传感器重达数公斤，受限于机器人负载能力，需要考虑使用较少的传感器来完成开门作业。

对带六自由度机械臂的履带式移动机器人进行研究，规划开门作业流程，使用安装在机械臂末端的单目相机和安装在车体上的二维激光扫描仪完成对门把手和门轴的识别定位，并设计力位混合控制方法，实现自主开门作业。

2 机器人自主开门规划

2.1 系统构成

研制的开门机器人主要由履带式移动车体和六自由度机械臂构成，如图1所示。该结构设计使得机器人具有较强的爬坡、越障、负载和多功能作业能力。在车体上安装二维激光扫描仪、在机械臂末端安装单目手眼相机，用于开门过程的导航和定位。二维激光扫描仪的功能，如图2所示。能够测量所在平面内的障碍物，用于避障行进；同时，结合相机对图像的辨识，可以得到环境的部分三维信息。

图1 机器人系统及坐标系定义Fig.1 Robot System and Its Coordinate Systems

图2 二维激光扫描仪示意图Fig.2 Illustration of the 2D Laser Scanner

2.2 机器人运动学

定义坐标系，如图1所示。世界坐标系O-xyz固定在门轴处，与车体同高。

机器人坐标系O0-x0y0z0定义在车体中心，跟随车体运动，与世界坐标系之间存在夹角θ0，其原点在O-xyz下的坐标为p0=［x0，y0，0］T，则二者之间的变换矩阵为：

末端坐标系O6-x6y6z6定义在末端工具处，机械臂各连杆之间的变换矩阵为T（i=1，…，6），则运动学方程为

二维激光扫描仪坐标系OL-xLyLzL与机器人坐标系同方向，其原点在 O0-x0y0z0下的坐标为0pL=［0xL，0yL，0zL］T，则有：

相机坐标系Oc-xcyczc定义在相机光心处，与末端坐标系存在夹角 θc，其原点在 O6-x6y6z6下的坐标为6pc=［6xc，6yc，6zc］T，则有：

2.3 力位混合控制

为实现机器人与环境之间的柔顺接触，避免硬碰撞，同时保证非接触方向的位置运动精度，对机械臂设计力位混合控制算法如下：

式中：τ6×1—关节力矩控制量；x6×1=［x，y，z，rx，ry，rz］—机械臂末端的位置和姿态角；xd6×1—其期望值；J6×6—雅可比矩阵，用于将末端广义力变换到关节力矩；对角阵 Kp6×6、Kd6×6—位置控制的比例系数矩阵和微分系数矩阵；g6×1—重力补偿项；Fd6×1—末端广义力的期望值；I6×6—单位阵；对角阵 Λ6×6为控制模式选择矩阵，某行为1则代表对应的位置或姿态角选择位置控制，某行为0则代表选择力控制模式。

2.4 开门作业规划和控制

由于履带式移动车体的精度远低于机械臂，尤其是在转弯运动中，车体的运动误差容易导致机械臂受力过大损坏。因此在开门时尽量令车体静止，必须运动时则令其匀速直线运动以减少误差，同时机械臂配合车体运动。

自主开门流程设计如下：

（1）行进到门前

车体行进到门前停下，且门把手在相机视野内。

（2）门把手定位

使用手眼相机和二维激光扫描仪，获取机器人和门把手之间的相对位置姿态。

（3）接近门把手

机械臂在位置控制模式下，移动到门把手正上方，并令末端工具垂直于门。

（4）开锁

对机械臂使用力位混合控制，使末端工具下压门把手，并跟随门把手转动，从而打开门锁。

力位混合控制设计如下：令平行于门板的z自由度处于力控制模式，该方向的期望力竖直向下，用于下压门把手；令垂直于门板旋转的rx自由度处于力控制模式，该方向的期望旋转力矩为零，即处在自由转动状态，以跟随门把手的旋转；其余4个自由度处于位置控制模式，期望位置（姿态）保持不变。

（5）轻微打开门并完成机器人定位

对机械臂使用力位混合控制，使机械臂轻微向前（推门）或向后（拉门）打开门。同时，使用二维激光扫描仪辨识门轴位置，完成机器人和门把手在世界坐标系下的定位。

力位混合控制设计如下：令绕竖直方向旋转的rz自由度处于力控制模式，该方向的期望旋转力矩为零，即处在自由转动状态，以跟随门的旋转；其余5个自由度处于位置控制模式，向前或向后运动。

（6）开门

根据门把手在世界坐标系下的坐标，计算门的运动轨迹。推门情况下，机械臂放开门把手，改用位置控制模式，根据机械臂自身运动范围规划出一条轨迹，推开门板；拉门情况下，令车体匀速后退，对机械臂使用和步骤（5）相同的力位混合控制，令机械臂沿门把手轨迹运动，打开门。

3 自主定位

自主定位用于在上述自主开门流程中完成对x0、y0和兹0的辨识。

3.1 门所在直线的辨识

对于二维激光扫描仪而言，门是一条线段，通过识别门所在直线（也即世界坐标系y轴），可得出机器人与门之间的距离x0和夹角 兹0。

二维激光扫描仪的数据是极坐标（r，琢）形式，如图2所示。将其变换到直角坐标系OL-xLyL下，有：

对各个点进行如下Hough变换：

式中：兹—斜率；籽—截距。从而将数据从xLyL平面中的一个点投影成参数坐标系籽兹平面中的一条曲线。

在 籽兹平面内，找到各条曲线的聚集点（籽*，兹*），则Lxcos兹*+Lysin兹*=籽*即为门所在直线在OL-xLyL坐标系下的表示。

根据式（1）和式（2），可得：

值得注意的是，为获取更准确的直线方程，在投影计算时采用较小的斜率间隔以提高分辨率。然而由于测量数据自身存在抖动误差，较小的斜率间隔会导致籽兹平面内数据的分散，易受周围环境干扰。为解决这一问题，令籽兹平面内各个点的值等于其周围一定范围内的点的值之和，使数据变得更凝聚，然后再计算最大值点作为聚集点。

3.2 门把手定位

为抓握门把手打开门锁，需要辨识门把手的三维坐标。由于相机在深度方向的辨识精度不够高，因此仅使用相机对门把手进行图像平面内的辨识，然后结合二维激光扫描仪获取的深度信息，得到更精确的门把手的三维坐标。

具体方法是：令机械臂带着相机在世界坐标系yz平面移动，辨识得到相机在不同位置时门把手在图像坐标系下的坐标，然后进行拟合，并联合对门所在直线的辨识结果，得到门把手与机器人的相对位姿。为提高拟合精度，令机械臂移动过程中门把手逐渐靠近图像中心。流程如下：

（1）不妨令y0=0，也即将世界坐标系沿y轴进行平移；

（2）按照3.1节方法，通过激光扫描仪数据计算x0和兹0，得到

（3）为提高图像识别精度，在门和车体的夹角兹0已测得的情况下，根据兹0和兹c调整机械臂末端姿态，使相机正对门，即zc∥x且 xc∥y；

（5）使用尺度不变的模板匹配方法，从相机图像中辨识门把手，得到门把手中心在图像坐标系（原点在图像中心）下的坐标（wh，hh）；

（6）在世界坐标系下平移机械臂末端，平移量为驻p6=（k1-k2i）［0，-sgn（wh），-sgn（hh）］T，其中 i表示第 i次执行本步，k1和 k2为系数；

（7）重复步骤（4）～步骤（6）若干次；

（8）根据得到的一系列 yc、wh、zc和 hh，进行最小二乘拟合，得到 yc=kywh+by，zc=kzhh+bz。（by，bz）即为门把手在图像中心时的相机坐标，则门把手中心在世界坐标系下的坐标为ph=［0，by，bz］T；

3.3 门轴识别和机器人定位

在机械臂轻微打开门过程中，门所在直线发生旋转，通过对直线旋转过程的拟合，可以得出转轴（即门轴）位置，计算出y0，从而完成机器人和门把手在世界坐标系下的定位，用于门打开过程中对机器人进行轨迹规划。

在门运动时，按照3.1节方法，通过二维激光扫描仪数据多次辨识门所在直线，得到OL-xLyL坐标系下的一系列直线…，n 表示第 i次辨识结果。

据此修正 y0：y0=-yh，修正门把手的坐标：ph=ph+［0，yh，0］T。

4 实验

为验证提出的机器人自主开门规划控制和定位方法的有效性，进行自主开门实验，如图3所示。

在此实验过程中的自主定位结果如下：首先使用3.1节方法得到：兹0=-3.5°，x0=-1006mm；然后使用 3.2 节方法得到：ph=［0，-8.5mm，803mm］；最后使用3.3节方法得到：yh=-y0=-474mm，据此修正：ph=［0，465.5mm，803mm］。对机器人和门进行实际测量可得，门把手坐标应为：ph=［0，459mm，800mm］，初始时机器人与门之间距离为1002mm。鉴于门把手坐标和机器人与门的距离分别反映了定位算法在门平面和法线方向的精度，对比实测值和辨识结果，可以看出所提方法能够实现自主定位，且辨识误差小于1cm。

图3 开门实验照片Fig.3 Pictures of the Door-Opening Experiment

5 结论

（1）为实现机器人在房间内外穿行所需的开门功能，提出一种基于力位混合控制的机器人自主开门规划方法，该方法能够降低接触力，保障开门过程机器人和环境的安全。实验表明该方法能够完成自主开门。（2）为完成上述开门规划所需的自主定位，设计一种使用单目手眼相机和二维激光扫描仪的门把手和门轴辨识方法，能够在开门作业中完成对门的识别和机器人自主定位。实验验证该方法辨识精度优于1cm，满足开门需求。