月面车载机械臂的无标定视觉伺服控制方法

王腾飞，王 牛

(重庆大学自动化学院，重庆400044)

1 引言

月球是人类太空探测的重点对象[1]。月球车作为月面探测的重要工具，通常需要搭载机械臂完成设备搬运、定位、安装、月壤采样等操作[2]。月面车载机械臂的控制方式主要有2种，一种是地面控制中心发送控制命令进行远程遥控，一种是车载控制系统自主决策实现自主控制[3]。自主控制可以对机械臂进行实时操纵，有效提高月面作业的工作效率，但由于月面作业环境复杂，机械臂运动控制的稳定性和快速性较差，如何优化机械臂的运动轨迹，同时提高机械臂运动控制的适应性和鲁棒性是目前研究的重点[4]。

机械臂对外界环境的感知能力是影响机械臂控制性能的重要因素[5]。视觉伺服控制利用视觉传感器获取环境信息，具有信息量大、适用范围广泛、非接触性等优点[6]，逐渐被应用于机械臂的伺服控制。视觉伺服控制分为基于标定的视觉伺服和无标定视觉伺服。基于标定的视觉伺服反馈信号是估计的机械臂位姿参数，由于需要精确标定摄像机的内外参数和机械臂运动学模型，标定难度大、适应性和稳定性差，特别是无法应用于一些无法进行标定的环境[7]。机械臂无标定视觉伺服方法由Hosoda等[8-9]在上世纪90年代提出，不需要标定摄像机内外参数和机械臂运动学模型，其直接在图像空间设计控制器，将图像特征变化映射为机械臂的关节角度变化，驱动机械臂运动。

将无标定视觉应用到机械臂的伺服控制中，需要对双目视觉进行合理的配置，合适的双目视觉配置可以将笛卡尔空间的姿态信息转换为2个图像平面中的图像特征信息。目前常用的双目视觉配置是正交配置。Wang等[10]将机械臂运动分解成2个摄像机平面中的位置参数，机械臂能够成功跟踪圆形路径；Chen等[11]将具有正交视觉配置的自动标记控制系统成功应用于气瓶自动标记；Qian等[12]实现了机械臂在“眼在手”和“眼固定”配置下的移动物体精确跟踪。

尽管正交视觉配置避免了从图像特征空间估计目标的深度信息，但是摄像机的安装需要严格正交，这在实际的使用环境中具有较大的局限性。针对此问题，本文提出一种双摄像头双轴平行视觉配置，不需要两个摄像机严格正交，能有效提高双目无标定视觉伺服系统的鲁棒性。

2 伺服控制系统

机械臂无标定视觉伺服控制结构如图1所示，具体控制过程如下：系统通过摄像机实时获取当前机械臂操作工具的图像信息，提取得到当前图像特征fc，通过与期望目标点的图像特征fd做偏差运算，得到当前时刻的图像特征偏差fe，fe经过控制器后输出Δf，Δf与视觉映射模型运算后，得到机械臂各关节角度的控制变化量Δq，接着机械臂关节控制器控制各关节运动到目标关节角度q，机械臂末端随着关节运动发生位姿变化，然后双目摄像机继续获取最新时刻的机械臂末端图像信息，产生新的图像特征，重复上述控制过程，直到机械臂操作工具到达期望的目标位姿。

无标定视觉伺服是一次性视觉伺服，即机械臂完成空间点到点运动任务的运动轨迹是一次性生成的。在控制过程中，系统实时比较当前图像特征与期望到达位姿的图像特征，当其误差值小于设置的允许误差，表示机械臂到达了期望的空间位姿，完成了一次空间定位任务。

图1 机械臂无标定视觉伺服控制框图Fig.1 Block diagram of uncalibrated visual servo control in manipulator

2.1 双摄像头双轴平行视觉配置

理想情况下，2个摄像机使用正交视觉配置，如图2所示，摄像机1和摄像机2在笛卡尔空间的x、y平面中相互正交。但是正交视觉配置在一些工程应用中具有一定的局限性，例如，2个摄像机在同一平面上不能完全正交。因此，本文提出了一种双摄像头双轴平行视觉配置，如图3所示。相机1与相机2的x轴分别与所提出的视觉配置中的笛卡尔空间的x、y轴平行，相机1绕x轴旋转角度ψ1，相机2绕y轴旋转角度ψ2。 需要指出的是，所提出的视觉配置不是为了重建笛卡尔空间中机械臂的姿态，而是为了补偿目标的深度信息，并将笛卡尔空间中的机械臂运动分解为两个图像平面的图像特征信息。与正交视觉配置相比，所提出的视觉配置仍然能够实现机械臂无标性视觉伺服控制任务，并具有更好的控制效果。

图2 正交视觉配置Fig.2 Configuration of orthogonal vision

图3 双轴平行视觉配置Fig.3 Configuration of bi-axial parallel vision

2.2 视觉映射模型

机械臂无标定视觉伺服控制的本质是找到合适的视觉映射模型，视觉映射模型将摄像机像平面的图像特征变化映射成机械臂在笛卡尔空间的关节角度变化。选取不同的图像特征会产生不同的视觉映射模型。本文采用图像雅克比矩阵作为视觉映射模型，并利用Kalman滤波算法在线估计图像雅克比矩阵[13]，将机械臂图像特征变化与机械臂关节角度变化之间的非线性关系进行近似线性化处理。

假定机械臂关节角度的变化量为(Δq1、Δq2、Δq3……Δqn)时，机械臂末端对应的图像特征变化量为 (Δf1、Δf2、Δf3……Δfm)， 于是可得到当前时刻的图像雅克比矩阵，如式(1)所示：

式中m为选取的图像特征个数，n为机械臂空间运动自由度。根据上式建立起机械臂末端图像特征变化与机械臂关节角度变化之间的映射关系，如式(2)所示：

当视觉映射模型采用图像雅可比矩阵时，无标定视觉伺服控制通常需要求取图像雅克比矩阵的逆。当m=n时，雅克比矩阵非奇异，图像雅克比矩阵的逆J-1f存在。当m≠n时，雅克比矩阵的奇异性使图像雅克比矩阵不可逆，会影响机械臂无标定视觉伺服控制系统的稳定性。

2.2.1 图像特征提取

图像雅克比矩阵的具体形式与图像特征的选取方式和选取数目有关，本文采用双目视觉来获取机械臂末端的图像特征。在双目视觉系统中，点特征可以确定目标物体的位置信息，但对于机械臂无标定视觉伺服控制系统，需要确定机械臂末端的位姿信息，所以需要采用线特征。线特征可以由2个点特征构成。图像特征的选取应该在能够反映机械臂位姿信息的情况下，方便系统的设计与分析，所以图像特征的选取数目应该根据系统的具体情况而定。基本的规则是：机械臂末端的位姿与所选取的图像特征应该具有一对一或一对多的映射关系，所以图像特征的选取数目应该等于或者大于机械臂的运动自由度。在此基础上，还要注意摄像机的安装位姿，避免图像雅克比矩阵出现奇异。

根据以上分析，针对本文采用的六轴机械臂，图像特征的提取方法如图4所示。为了便于提取图像特征，机械臂末端带有红色和黄色标记，在机械臂起始位置，假设红色和黄色两个色块的中心点在世界坐标系中的三维坐标分别为P1和P2。其中机械臂末端P1点在摄像机Cam1图像平面上的像素坐标为 P11(u11，v11)，P2点在摄像机Cam1图像平面上的像素坐标为 P12(u12，v12)。机械臂末端P1点在摄像机Cam2图像平面上的像素坐标为P21(u21，v21)，P2点在摄像机Cam2图像平面上的像素坐标为P22(u22，v22)。 假设机械臂末端两个标记点的期望目标点为则它们在摄像机Cam1图像平面上的像素坐标分别为，在摄像机Cam2图像平面的像素坐标分别为和

图4 图像特征提取方法Fig.4 Image feature extraction method

假设Cam1图像平面上的点P11和P12所组成的直线与Cam1图像平面上的u轴的夹角为θ1，Cam2图像平面上的点P21和P22所组成的直线与Cam2图像平面上的u轴的夹角为θ2。θ1和θ2的计算公式如式(3)：

根据上文图像特征提取规则，六轴机械臂无标定视觉伺服控制系统图像特征的数目应该不少于6，则可以选取Cam1图像平面上的点P11(u11，v11)、Cam2图像平面上的点P21(u21，v21)以及θ1和θ2组成图像特征向量f，如式(4)所示。其中点特征能够反映机械臂末端在笛卡尔空间中的位置，而由线特征获得的夹角θ1和θ2能够反映机械臂末端在空间中的姿态。

根据选取的图像特征，可得到如式(5)所示的图像雅克比矩阵：

2.2.2 基于Klman滤波算法的图像雅克比矩阵在线估计

由于图像雅可比矩阵仅在当前时刻的领域内有效，实际使用中常采用估计算法在线更新图像雅可比矩阵，以提高机械臂无标定视觉伺服控制的精度。本文将待估计的图像雅可比矩阵参数作为系统状态变量，在考虑噪声对系统状态观测的影响情况下，基于Kalman滤波算法来对系统状态变量进行估计。将公式(2)离散化后得到公式(6)：

假定无标定视觉伺服系统中图像雅可比矩阵的观测向量x(k)为系统状态变量，机械臂末端图像特征变化为系统输出，即z(k)=f(k+1)-f(k)，则有状态方程如式(7)：

式中，η(k)和v(k)分别是状态噪声和图像输出噪声，假设都是高斯白噪声，则运动矩阵C(k)如式(8)所示：

依据Kalman滤波算法，建立观测向量x(k)的递推公式如式(9)：

其中，Rη和Rv是噪声方差阵，P(k)是状态估计的协方差阵，其初始值可以根据具体需要进行设置。系统状态估计的初始值^x(0)，即J^(0)，通过让机械臂先进行试探运动获取递推数据，再利用最小二乘法拟合获得。让机械臂末端在工作空间做任意给定的6步线性无关的试探运动，获得机械臂关节角度变化Δq1、Δq2…Δq6，相应的图像特征变化为Δf1、Δf2…Δf6，图像雅克比矩阵估计的初始值J^(0)为式(10)：

这样就可以用估计的图像雅克比初始值J^(0)构造卡尔曼滤波状态的初始值 ^x(0)，然后利用卡尔曼滤波递推算法，在线递推估计下一时刻的图像雅克比矩阵。

3 地面模拟试验

3.1 MATLAB仿真

在MATLAB R2016b仿真软件平台下，利用Peter Corke开发的开源机器人工具箱Robotics Toolbox 9.8和机器视觉工具箱Machine Vision Toolbox 3.3，以常用的六自由度Puma560机械臂为控制对象。Puma560机械臂的各连杆D-H参数如表1所示。其中j表示机械臂的关节编号，θ表示关节角，d表示关节之间的距离，a表示关节长度，α表示关节角的角度值。

表1 六轴Puma560机械臂D-H参数Table 1 D-H parameters of 6-axis Puma560 manipulator

摄像机Cam1和Cam2的内部参数设置为：图像分辨率都是1024×1024，摄像机焦距f=0.008 m，像素的物理尺寸dx=dv=1.0×10-5m。在仿真环境中，设置摄像机的固定位置时，应该使摄像机能够最大范围的观察到机械臂末端的运动，并且保证2个摄像机的视觉范围之间不会出现相互遮挡的情况。摄像机Cam1和Cam2相对于基坐标系的平移旋转矩阵Tcam1和Tcam2如式(11)和(12)所示：

设置机械臂末端的初始位姿和期望位姿为pres=[0.6217-0.2767-0.5142-2.6101-2.3307-2.6565]，pdes= [0.7177-0.4390-0.2194 0.8460 1.3667 0.4868]。仿真初始时刻和仿真结束后机械臂末端在2个摄像机中的图像特征如图5所示，其中直线连接的为当前图像特征，另外两点为期望图像特征。

仿真结束后机械臂末端在笛卡尔空间的运动轨迹图、运动坐标偏差图、图像平面点特征偏差图以及角度特征偏差图，分别如图6～9所示。

图5 起始时刻和结束时刻的Cam1和Cam2的图像特征Fig.5 Image features of Cam1 and Cam2 at the start and end moment

图6 运动轨迹Fig.6 Motion trajectory

图7 运动坐标偏差Fig.7 Deviation of motion coordinates

图8 点特征偏差Fig.8 The point features deviation

从仿真实验图像可以看出，利用本文所提方法控制的机械臂以一条接近最优轨迹的运动路径从笛卡尔空间的起始点到达期望目标点，终止时刻图像特征基本与期望图像特征重合，机械臂运动轨迹的坐标偏差和图像特征偏差均以较快的速度收敛到零点处。

图9 角度特征偏差Fig.9 Deviation of angle features

下面再通过具体实验数据分析机械臂的无标定视觉伺服控制性能。表2为机械臂末端的位姿数据，表3为机械臂末端的图像特征数据，表4为机械臂末端运动轨迹的性能指标，分别为运动轨迹的平均误差、均方误差和最大误差。

表2 机械臂末端位姿数据Table 2 Pose data of the manipulator end

表3 机械臂末端图像特征数据Table 3 Image feature data of the manipulator end

表4 机械臂末端运动轨迹性能指标Table 4 Performance indices of the manipulator end motion trajectory

从上面3个表数据可以看出机械臂无标定视觉伺服控制具有较高的定位精度，其中机械臂末端最终定位在笛卡尔空间中的坐标误差绝对值均在0.001 m以内，其图像特征中的点特征偏差绝对值均满足在1个像素以内，角度特征偏差绝对值均小于0.03 rad，定位精度满足实际应用误差要求。机械臂末端运动轨迹的各项误差均较小，表明其接近最优运动轨迹。仿真试验表明，采用无标定视觉伺服方法控制的机械臂具有较优的笛卡尔空间运动轨迹，收敛速度快，控制精度高。

3.2 地面实物模拟

六轴机械臂无标定视觉伺服控制平台如图10所示，其中机械臂是Denso机器人公司生产的VS-6556G六轴工业机械臂。机械臂控制箱是RC7M-VSG6BA控制箱，摄像机是大恒水星系列的MER-125-30UM高速工业CCD摄像机。此外还包括机械臂示教器、末端工具控制电路、机械臂末端工具和PC计算机等其他组成部分。其中示教器用来对机械臂进行参数设置、手动调试和校正，末端工具控制电路用来控制末端盘式直流电机的启停和操作工具的装卸，其由机械臂控制箱的IO接口控制，而控制命令是上位机通过以太网与机械臂控制箱通信发送的。

图10 机械臂无标定视觉伺服控制平台Fig.10 Uncalibrated visual servo control platform of manipulator

本文采用的Denso机械臂共有6个轴关节，各轴关节运动范围分别如表5所示。

表5 Denso机械臂各关节运动范围Table 5 Motion range of Denso manipulator joints

通常在机械臂的工作任务中，需要在机械臂末端安装相应的操作工具，在本文中，操作工具使用的是螺丝卸载工具。如图11所示，操作工具由固定支架、盘式直流电机、电磁线圈和螺丝卸载工具组成。在螺丝卸载工具的末端有红色和黄色的标记，其作用与仿真试验中相同，便于提取机械臂末端工具的图像特征。

图11 机械臂末端工具Fig.11 Manipulator end tool

机械臂无标定视觉伺服采用双目视觉系统，摄像机布局方式如图12所示。

图12 双目视觉硬件系统构成Fig.12 Hardware system composition for binocular vision

设机械臂末端的初始位姿和期望位姿分别为pres=[480.60 - 76.70 520.40 84.30 22.10 85.90]，pdes=[595.00-144.80 392.60 91.20 22.10 75.80]。实验结束后机械臂末端在笛卡尔空间的运动轨迹图、运动坐标偏差图、图像平面点特征偏差图以及角度特征偏差图，分别如图13～16所示。

从试验图像可以看出，机械臂末端运动轨迹接近最优轨迹，机械臂运动轨迹坐标偏差和图像特征偏差快速收敛到零点处。

试验的具体实验数据如表6～8所示，其中表6为机械臂末端的位姿数据，表7为机械臂末端的图像特征数据，表8为机械臂末端运动轨迹的性能指标。

图13 运动轨迹Fig.13 Motion trajectory

图14 运动坐标偏差Fig.14 Deviation of motion coordinates

图15 点特征偏差Fig.15 Deviation of point features

表6 机械臂末端位姿数据Table 6 Pose data of the manipulator end

图16 角度特征偏差Fig.16 Deviation of angle features

表7 机械臂末端图像特征数据Table 7 Image feature data of the manipulator end

表8 机械臂末端运动轨迹性能指标Table 8 Performance indices of the manipulator end motion trajectory

从试验数据可以看出机械臂无标定视觉伺服控制在实物环境中同样具有较高的定位精度，其中机械臂末端最终定位在笛卡尔空间中的坐标误差绝对值均在1 mm以内，其图像特征中的点特征偏差绝对值均在1个像素以内，角度特征偏差绝对值均小于0.8°，定位精度满足实际应用需求。机械臂末端运动轨迹的各项误差均较小，表明其接近最优运动轨迹。

试验表明，机械臂无标定视觉伺服控制方法在实物环境中具有较好的控制性能，系统收敛速度较快，控制精度也较高。

4 结论

1)采用双轴平行视觉配置方案，通过选择图像特征空间的点特征和线特征设计控制器，能有效提高机械臂对复杂控制环境的适应能力。

2)采用图像雅克比矩阵反映无标定视觉伺服系统的手眼映射关系，并利用Kalman滤波算法对其进行实时在线估计，能在系统发生微小变化时，对机械臂关节角度变化和相应图像特征变化之间的非线性关系进行近似线性化的处理。

3)仿真和实物试验验证了本文所提方法的有效性。