随着人类太空探索活动的不断深入，单臂系统的不断成熟为多臂协调操作系统的发展提供了基础，而在复杂空间环境中进行精细操作的需求也使得多臂协调操作系统拥有更大的发展空间。美国NASA Johnson空间中心已经研制出可以替代或帮助宇航员出舱活动的舱外灵巧机器人Robonaut(机器人宇航员)。该机器人由2个七自由度的机械臂、2个12自由度的多指灵巧手、一个4自由度的立体视觉平台、一个六自由度的尾巴和一个可以让遥操作者融入机器人工作环境的遥现系统组成。北京航空航天大学机器人所丁希仑等^[1]对冗余度双臂机器人的协调操作进行了初步研究，建立了孔轴装配条件下的双臂机器人协调运动学方程，并完成了简单孔轴装配操作。东南大学芦俊等^[2]对视觉引导下的双臂装配机器人最佳装配位置的确定问题进行了研究，并对基于图像定位的轴与轴孔装配中的误差进行了分析。

本文的双臂协调操作系统中，给出一个基于六自由度机器人的控制子系统，该子系统通过CCD摄像机获取图像，传入图像采集卡中，再存入到计算机内存对图像进行处理，获取目标的位置信息，通过设计好的控制算法得出能够控制机器人运动的命令，机器人控制器通过串口接收指令，从而驱动机器人相应的运行，完成基于图像的视觉伺服。此外还有一个七自由度机械臂控制子系统，其中进行图像获取和目标检测的是微软公司研制的Kinect视觉传感器，Kinect不仅价格低廉，而且能够同时获取深度信息和彩色信息，被研究者们广泛的应用在机器人领域，如机器人的导航与定位^[3]、目标的跟踪^[4]、障碍物的检测^[5]等。利用基于工作空间的RRT算法对七自由度机械臂末端进行路径规划完成目标的自主抓取。通过这2套机械臂子系统最终完成双臂协作实现物体交接的任务。

1 系统结构

本文的双臂系统是由视觉传感器、机械臂系统以及主控计算机组成，其中，视觉传感器包括挂在顶上的Kinect和安装在六自由度机械臂末端的CCD摄像机，机械臂系统包括一台模块化七自由度机械臂和一台六自由度机械臂以及灵巧手。整个系统的构建模型如图 1所示。

双臂协作系统的工作流程如下：通过安装在机械臂末端的摄像机获取目标信息，通过图像误差控制六自由度机械臂抓取目标并送至指定位置，根据Kinect获取的环境信息选取目标特征点，七自由度机械臂通过路径规划控制末端位姿，自主抓取目标，将目标物放入指定位置，完成双臂交接任务。整个双臂系统主要分为视觉单元、控制单元和执行单元3个部分。视觉单元由Kinect视觉传感器、CCD摄像机、图像采集卡、一台高性能计算机和局域网组成，摄像机以10帧/s的速率采集640×480大小的图像，采集图像的过程中计算机完成对目标的检测，得出目标中心点在图像平面的坐标。

控制单元是由伺服放大器、交流伺服电机及配电组件等构成，其功能是执行接受到的控制指令，完成对摄像机的操纵，实现对目标的跟踪。

执行单元由Motoman机械臂、模块化机械臂、主控计算机和运动控制器构成，此单元可以实时地获取机器人的位置信息，并根据图像误差生成对机器人各关节的控制指令，控制指令可以根据图像实时刷新，通过主控计算机和运动控制器控制机器人完成指令任务。

2 目标检测与跟踪算法 2.1 深度信息的背景相减法

本系统采用微软公司研发的Kinect视觉传感器作为深度摄像机。Kinect的硬件结构包括一个普通的低分辨率RGB摄像机、一个红外发射器和一个红外摄像头以及一组麦克风阵列，如图 2所示。

普通RGB摄像机能够以30帧/s的速率传送640×480分辨率的RGB图像，而红外发射器和摄像机则是用来采集相应的深度信息，通过内部芯片转换并传送320×240分辨率的深度图像^[6-7]。

文献[14]利用基于深度信息的背景相减法，将当前帧与背景模型进行差值运算，从而得出环境中目标的位置与大小等信息特征，最终能够准确地提取物品桌上的目标，实现目标检测。由于本系统为双臂协调操作系统，物体位于六自由度机械臂末端的手爪上，这对灵巧手的精确抓取提出了更高的要求，对目标检测的位置信息需更加精确。

2.2 基于颜色的粒子滤波跟踪算法

粒子滤波^[8]是一种序贯蒙特卡洛滤波方法，其本质是用一系列随机抽取的样本代替状态的后验概率分布。粒子滤波的基本思想是：首先建立一个概率模型或随机过程，使它的参数等于问题的解，然后通过对模型或过程的观察或抽样试验来计算所求参数的统计特征，最后给出所求解的近似值。粒子滤波包括2个步骤：预测和更新。

已知系统在k时刻之前的所有观测z_1:k－1=z₁, z₂, …, z_k－1(其中z_k为系统在k时刻的观测值)，那么系统在k时刻的状态x_k可由式(1)给出的模型进行预测^[9-11]：

当得到系统在k时刻的观测z_k后，就可以根据贝叶斯公式得到

式中：p(z_k|x_k)由事先选择的观测模型给出，当前时刻的状态p(z_k|x_k)由N个权值为w_kⁱ 的样本x_kⁱ 表示，这N个样本就是粒子滤波器中的粒子，权值代表该粒子与所跟踪目标的相似程度。样本x_kⁱ通常是从一个已知的分布q(x_k|x_1:k－1, z_1:k)中获得，其权值计算公式为

权值就是观测概率p(z_k|x_k)，因此输出的状态估计可表示为

通过计算图像的RGB颜色直方图建立目标的颜色模型。假设建立的直方图共有N个灰度级，则颜色直方图表示为q^*={q^*(n)}, n=1, 2, …, N，且。

在初始时刻(k₁时刻)手工选取目标位置x_k1^*，得到颜色参考模型为q^*，k时刻得到某一粒子的颜色直方图为q_k(x)。本文需要在RGB颜色分布空间上选择一个距离D，该距离使得每一次估计均值时的方差最小。通常使用Bhattacharyya相似性系数如式(5)，这种度量概率分布之间差别的距离值介于[0, 1]，且忽略了那些不存在的灰度等级。

文中用式(5)表示的Bhattacharyya距离^[14]统计跟踪窗口区域内的颜色直方图，并将其与参考直方图比较，求解似然估计。可以取

此时计算的就是p(z_k|x_k)就是k时刻该粒子权值。

根据上文论述，基于颜色的粒子滤波跟踪算法分为以下几个过程：

1)初始化过程

在观测图像中手工标注感兴趣的目标区域，以该区域中心为原点，产生粒子集{x_k1ⁿ}，其中n=1, 2, …, N, 每个粒子代表一个目标可能存在的区域。

2)粒子传播过程

根据目标运动的动态模型，每个粒子都遵循一个状态转移过程独立地进行自身传播，从而得到下一时刻的新粒子集。即k时刻的粒子集{x_kⁿ}通过状态转移，得到k+1时刻的粒子集{x_k+1ⁿ}。

3)观测过程

对传播后的新粒子进行观测，即计算每个新粒子所代表的状态与初始目标状态的相似度。也即计算每个新粒子区域的颜色直方图与参考直方图的Bhattacharyya距离(每个粒子应该赋予的权值)，距离越大，说明该粒子越接近参考模型，所赋予的权值就越大，反之权值就越小。

4)估计过程

对M≤N个权值较大的粒子计算后验概率，求出期望值：

式中：w_k+1是对每个粒子区域按式(6)求得的权值(概率)。用式(7)计算的期望值作为目标k+1时刻状态的最优估计。

3 机械臂轨迹规划 3.1 七自由度机械臂子系统

本子系统利用Schunk七自由度机械臂和Barrett四自由度灵巧手，融合Microsoft Kinect和触觉传感器阵列的信息，共同完成目标的稳定抓取。在Kinect的深度信息的引导下，机械臂接近目标物体，当到达灵巧手的工作空间后，在触觉信息的配合下，进行精确稳定抓取。

七自由度机械臂的路径规划算法利用著名的RRT算法。RRT(Rapidly-exploring random trees)是一种随机规划算法，是由Steven M. Lavalle和James J. Kuffner^[12-13]首先提出来的一种路径规划算法。RRT方法被广泛应用在机器人轨迹优化和路径规划中，不仅计算快速，而且形式简单、概率完全。文献[14]中详细介绍了基于工作空间RRT算法的工作原理，通过构建RRT树的方法实现逆运动学轨迹优化，从而获取相应的关节运动，完成了机械臂自主抓取任务。七自由度机械臂控制系统的轨迹规划由2部分组成，首先机械臂抓取目标的前提是将机械臂零位设置为初始位置，将抓取目标的位置设置为目标点，从而进行轨迹优化；其次如何利用六自由度机械臂将目标物准确放到框内在本系统中也是非常重要的，采用将抓取点和投掷点分别作为初始点和目标点的方法来完成。通过RRT算法进行数据解算，机械臂获取到RRT解算出的关节角度并运行到指定位置进行目标抓取，实现双臂协调操作。同样的道理，机械臂将抓取的目标物投掷到指定位置的框中，以此来完成整改双臂协调操作任务。

3.2 六自由度机械臂运动控制

如图 3所示为六自由度机器人系统的结构图。图中O₁、O₂、O₃分别为肩、大臂、小臂关节，主要负责机器人的移动；O₄、O₅、O₆为手腕的3个关节，主要负责手爪的摇摆、俯仰和偏转；θ₁、θ₂、θ₃、θ₄分别为O₁、O₂、O₃、O₄机器人从初始位置到期望位置的旋转角度，方向如箭头所示，手爪和摄像机安装在机械臂的末端，因此该子系统也被称为“眼在手上”视觉伺服控制系统。视觉控制器采用比例控制器的方法控制机器人关节运动，假设目标在机器人基坐标系下的位置为P₀=[x₀ y₀]^T，目标在图像平面的位置为f₀=[u₀ v₀]^T，图像平面的中心位置为f_g=[u_g v_g]^T，这也是目标在图像上的期望位置。比例控制器的反馈控制率为=kφ()，为了使运动目标始终保持在图像的中心区域，需将目标在图像上的当前位置与期望位置之间的误差传到控制器中，控制机器人运动，改变机器人在基坐标系下的位置。

由上述可知，期望图像与当前图像的误差为

将图像误差作为控制器的输入，控制器的输出为机器人各关节在基坐标系下的位移增量变化值，其计算公式为

控制策略流程如图 5所示。

根据图像误差控制机器人具体步骤如下：

1)记录机器人期望位置下图像中目标的中心点在图像上的坐标(x₀, y₀)、矩形框的宽和高(w₀, h₀)以及期望位置下的关节角；

2)设定限制条件，当目标在图像中心位置的2个方向上的误差绝对值在设定范围内，并且摄像机与目标的距离适中时，机器人的关节角不变；

3)当目标超出设定范围时，将图像分为4个区域，并分别计算当前图像与期望图像间的误差和机器人当前位置与期望位置间的关节角度差值；

4)运动控制过程中，根据期望值=当前值+Δθ，控制机器人在循环中根据图像误差进行相应的运动。

4 实验

为了完成多臂协作物体交接的任务，验证上述研究内容的可行性，本文设计了一套双臂协作的实验系统，实验场景如图 6所示。

4.1 实验相关说明

实验系统包括双机械臂、Kinect、机械臂末端摄像机、灵巧手、控制器、交接目标物。实验任务：利用六自由度机械臂抓取目标，通过视觉伺服控制机械臂将目标递送到七自由度机械臂附近区域，并通过上方Kinect视觉传感器对目标进行检测，七自由度通过检测的目标点位置信息(抓取点的三维坐标)对目标进行自主抓取，最终实现目标物交接后将其投掷到指定位置。

4.2 实验过程

图 7为七自由度机械臂2次轨迹优化的解算结果。其中，图 7(a)~(d)为以机械臂末端当前位置为初始点，以目标物抓取点为目标点进行轨迹优化的仿真结果，包括RRT树、机械臂末端轨迹、机械臂各关节轨迹和末端坐标分量轨迹；图 7(e)~(h)为以目标的抓取点为初始位置，以投掷点为目标位置进行轨迹优化的结果。双臂协作实验效果如图 8所示。

本次实验的目标物为矩形条和圆桶，通过Kinect目标检测可以得出目标物轮廓信息，通过轮廓点的位置计算出抓取点的坐标为(0.132, -0.631, 0.598)，投掷点的坐标为(-0.683, 0.562, 0.257)，这2个点为七自由度机械臂进行目标抓取的点和投掷的点，也是路径规划的起始点和终点。从图 8中可以看出，本次实验共进行了2次连续的物体交接实验。(1)~(8)显示为以矩形条为目标实现第1次双臂协调操作，并完成物体交接任务，(9)~(17)为以圆桶为目标实现第2次双臂协调操作，两次操作是连续进行的。

5 结束语

本文通过处理由Kinect获取的图像信息来确定目标抓取点的位置，并根据基于工作空间的RRT算法实现机械臂路径规划，与此同时另一台机械臂通过视觉伺服实现了目标抓取与递送。文章通过构建一套双臂协作实验系统完成了多臂协作物体交接的任务，并验证了本文研究内容的可行性。