未知雅可比建模的机器人视觉伺服自抗扰控制方法

仲训杲，仲训昱，彭侠夫，周承仙，徐 敏

(1.厦门理工学院电气工程与自动化学院，福建 厦门 361024；2.厦门大学航空航天学院，福建 厦门 361102)

传统示教编程控制技术不能满足机器人自主性要求，而机器人视觉伺服利用视觉传感器获取环境信息，对机器人实施反馈运动控制，已成为提高机器人自主操作能力的重要研究思路[1-2].当前机器人视觉伺服控制技术主要包括基于位置的视觉伺服(PBVS)和基于图像的视觉伺服(IBVS)[3]. PBVS方法以笛卡尔坐标为控制量，结合已知目标几何模型和相机标定参数，利用视觉测量原理对目标位姿进行估计，机器人根据目标姿态实施轨迹规划与运动控制[4-5].与之相比，IBVS方法在视觉空间对图像特征实施控制，把图像误差实时映射到机器人运动空间，实现机器人姿态协调. IBVS方法因无需估计目标在笛卡尔空间中的3D位姿，简化了系统计算，受到广泛关注[6-7].

然而，基于IBVS的机器人控制方法面临“视觉-运动”空间映射雅可比鲁棒求解和图像反馈控制器设计问题[8-9].其中求解雅可比要求系统标定参数和目标深度信息，导致精确雅可比计算困难，目前常用研究方法采用雅可比估计值替代精确值，例如Broyden雅可比估计法[10]、动态Broyden和动态指数加权递归最小二乘法(RLS)[11]、高斯-牛顿法和递推RLS雅可比估计法[12].Hao等[13]把Broyden-Gauss-Newton、Broyden递归RLS统一到状态空间中，研究基于状态空间的视觉伺服方法. 这一类研究方案通过在线递推得出雅可比估计值，但在控制器设计中不能完全保证雅可比可逆，机器人容易出现任务奇异.Liang等[14]研究了深度独立图像雅可比方法的视觉伺服.与之相似，Wang等[15]采用深度独立图像雅可比，建立图像误差与机器人关节之间的映射关系.王婷婷等[16]将特征点深度信息联合像素坐标，研究模型预测控制(MPC)的IBVS方法.Gao等[17]研究动态自适应神经网络伪逆雅可比估计的视觉伺服方法. 以上研究方法解决深度独立雅可比解耦控制和逆向雅可比求解问题，一定程度上改善了系统性能，有利于克服任务奇异的产生. 而本文研究思路与上述方法不同，将直接绕开雅可比复杂在线求解过程，机器人仅需在初始位姿引入试探运动，得出雅可比粗略值保证逆向雅可比存在，而在控制器设计中进一步对未建模雅可比实施反馈补偿，提高机器人的自主性能.

综上分析，机器人视觉伺服研究方法因需精确计算机器人雅可比，性能过于依赖系统标定参数和目标深度信息[18]，其标定参数变化与不确定性难免使现有研究方法应用受限.为此，本文研究一种未建模雅可比的机器人视觉伺服控制方法.研究方法借鉴无模型理论和非线性自抗扰控制器(active disturbance rejection controller，ADRC)构建机器人“视觉-运动”空间反馈控制，其中采用跟踪微分器(tracking differentiator，TD)跟踪视觉空间期望特征，扩张状态观测器(extended state observer，ESO)实现“视觉-运动”空间未建模雅可比动态补偿，最后利用非线性状态误差反馈(non-linear states error feedback，NLSEF)得出机器人运动空间控制量.

1 问题描述与方法分析

1.1 机器人视觉伺服

基于视觉伺服的机器人反馈控制，其控制器输入量为视觉特征，输出量为机器人运动量，控制目标是对视觉空间特征实施反馈控制，实现机器人运动空间任务操作.为此，定义视觉特征误差es(t)⊂Rn，伺服控制器极小化以下代价函数：

(1)

若F(t)→0，则机器人从当前位姿收敛到期望操作位姿.

针对手眼标定六自由度机器人视觉伺服控制，假设机器人运动空间控制量为κR(t)∈R6，视觉空间n维图像特征为κI(t)∈Rn.实际操作中，机器人末端轻微运动将导致视觉特征发生非线性变化，由此本文采用雅可比建立机器人“视觉-运动”空间微分关系，描述为：

κI(t)=Jκ(t)·κR(t).

(2)

上式雅可比Jκ(t)∈Rn×6可表示为

(3)

(4)

式中，λ为相机焦距长度,zi(t)为第i个特征点的深度信息，ui(t)、vi(t)为特征点i在图像平面中的投影坐标. 从式(4)可看出构建机器人视觉伺服控制首先需要介入系统标定和估算目标深度信息，从而求解式(3)雅可比映射矩阵，并在满足微分方程(2)条件下设计有效的控制器，最终达到式(1)的控制性能.

1.2 ADRC控制思想

ADRC基于TD安排系统期望输入，ESO估计系统未建模动态和外扰总作用，进而实施NLSEF控制，其原理框架如图1所示.

图1 ADRC原理框架Fig.1 The principle structure of the ADRC

假设被控对象为非线性n阶不确定系统：

x(n)(t)=f(x(t),x′(t),…,x(n-1)(t))+b0u(t)+

w(t).

(5)

式中:w(t)为未知干扰，u(t)为控制量，b0为常量，系统扩展状态向量为P(t)=[x1,x2…,xn+1]T=[x(t),…,x(n-1)(t),a(t)]T.其中a(t)=f(x(t),x′(t),…,x(n-1)(t))+w(t)，记为f(x(t),x′(t),…,x(n-1)(t))和w(t)在系统运行时的实时值.则ESO输出值z2,i(t)对系统状态变量实施跟踪，即：

z2,i(t)→xi(t),i=1,2,…,n+1.

(6)

TD是信号处理环节，主要作用是安排系统参考输入P*(t)微分信号的过渡，满足：

(7)

式(6)中t→∞，TD输出量z1,1(t)将跟踪P*(t);而z1,j(t)，j=2,3,…,n跟踪P*(t)的j阶广义微分信号.

此时，NLSEF将状态误差信号进行非线性配置，实现状态误差反馈，状态误差为：

el(t)=z1,l(t)-z2,l(t),l=1,2,…,n.

(8)

将误差信号进行如下非线性NLSEF配置，得到系统控制：

(9)

式中bl、αl、δl为可调参数，fal(·)为非线性运算规则.

2 基于ADRC的机器人视觉伺服任务建模

假设机器人伺服系统采样周期为h，则方程(2)可写成离散ADRC控制器适用形式，有：

κI(k+1)-κI(k)=h·Jκ(k)·κR(k).

(10)

令视觉空间期望图像特征κI*(k)∈Rn为伺服系统的参考输入，设计跟踪微分器TD安排输入信号过渡，实现跟踪κI*(k)，为此定义跟踪误差e1(k)∈Rn：

e1(k)=Z1(k)-κI*(k).

(11)

式中Z1(k)∈Rn为TD的输出矢量，有

Z1(k+1)=Z1(k)-h·b1·fal(e1(k),α1,δ1).

(12)

上式非线性运算规则fal采用如下形式：

(13)

其中a、b、c为参数矢量.

考虑机器人系统误差和外界干扰作用，进一步设计扩展状态观测器，定义ESO跟踪误差：

e2(k)=Z2(k)-κI(k),

(14)

式中Z2(k)∈Rn为ESO的输出矢量，用于跟踪当前时刻图像特征κI(k)∈Rn，有

Z2(k+1)=Z2(k)+h·(Z3(k)-

b2·fal(e2(k),α2,δ2)+Jκ(k)κR(k)).

(15)

上式Z3(k)∈Rn表示系统未建模动态和外界干扰的总作用，给出：

Z3(k+1)=Z3(k)-h·b3·fal(e2(k),α3,δ3).

(16)

基于以上TD，ESO规划，进一步设计机器人视觉控制器，首先给出机器人伺服系统反馈误差：

e3(k)=Z1(k)-Z2(k).

(17)

此时采用NLSEF控制规则，可得机器人控制量κR(k)为：

Z3(k)),

(18)

式中，

κo(k)=h·b4·fal(e3(k),α4,δ4).

(19)

以上各式参数bi、αi、δi，i=1,2,…,4为常量，统称为ADRC参数.

3 雅可比未建模伺服控制器设计

机器人视觉伺服现实应用中，系统标定和雅可比求解困难，而现有雅可比估计方法影响机器人的控制性能.为此，本小节结合ADRC状态反馈补偿优势，构建未建模雅可比的机器人实用视觉伺服控制方法.

不失一般性，假设目标特征点i在图像平面位置为fi(t)=(ui(t),vi(t))，n个特征点组成图像特征，记为视觉空间控制任务f(t)=[f1(t),f2(t),…,fn(t)]T.机器人运动空间控制量选取为U(t)=[V(t),W(t)]T，其中V(t)=(vx,vy,vz)为机器人末端线速度，W(t)=(wx,wy,wz)为角速度，根据式(2)建立微分方程：

f(t)=Jκ(t)·U(t).

(20)

考虑雅可比Jκ(t)估计值带来的误差为ζ(t)∈Rn，则式(20)改写为

(21)

式中,

(22)

(23)

即

(24)

其中：

(25)

式(25)表示雅可比估计误差和外界总扰动量.

图2 基于ADRC的未知雅可比机器人视觉伺服控制框架Fig.2 The structure of ADRC-based unknown Jacobian robot visual servoing control

此时，根据上一小节ADRC控制器设计，以期望图像特征f*(k)为系统参考输入，TD实现对其跟踪，对于机器人手眼抓取定位系统，期望图像特征f*(k)为定值，因此TD模型相当于单位矩阵E.根据式(14)～(16)，二阶ESO模型设计为：

(26)

由式(17)～(19)，NLSEF模型为：

(27)

机器人视觉伺服系统框架如图2所示，可看出ADRC是对所有特征点的控制相互耦合在一起，完成机器人末端操作器定位控制.值得注意的是本文视觉伺服控制器的设计沿用了ADRC思想理论，其中TD、ESO、NLSEF的收敛性已被有效证明，具体可以参考文献[19]. 而本文从工程应用角度出发，以机器人实际操作实验证明提出方法的有效性，至于整个系统的稳定收敛性则进行如下分析.

不失一般性，把式(27)代入式(21)，建立图像运动与机器运动之间的系统方程，如下：

(28)

由式(27)，上式化简可得：

f(t)=U0(t)+ε(t)-Z3(t)=h·b4·

fal(f*(k)-Z2(k),α4,δ4)+ε(t)-Z3(t).

(29)

如图2所示，Z2(k)跟踪当前时刻图像特征f(t)，Z3(k)跟踪未建模雅可比误差和外界干扰总作用ε(t)，已知ESO收敛，那么有限时间内Z2(t)→f(t)，Z3(t)→ε(t)，则式(29)写成：

f(t)≈μ·fal(f(t)-f*(t),α4,δ4)，

(30)

此时控制器可以看着一阶跟踪微分器形式，选择合理的参数，可以得出整个系统收敛稳定.

4 实验分析

如图3所示，相机安装在机器人末端，组成手眼标定视觉反馈实验平台.其中机器人选用UR5六自由度工业机械臂，机器人控制器作为下位机完成机器人运动学运算，同时驱动机器人各个关节.上位机选用Intel Corei5 2.67 GHz CPU, 4 GB RAM计算机，用于图像处理和运算视觉伺服控制算法.计算机与机器人控制器通过TCP/IP连接，构成视觉伺服闭环系统.

图3 眼在手视觉伺服实验平台Fig.3 The eye-in-hand visual servoing platform

以机器人抓取定位为测试实验，图3(a)为机器人初始姿态及观测图像特征，图3(b)为机器人抓取姿态及期望图像特征，实验设定控制率λ=0.25，系统采样间隔为0.1 s，机器人控制量U(k)∈R6×1为末端线速度和角速度，为简化特征提取过程，以4个圆点作为抓取定位特征，因此特征向量f(k)=[f1(k),f2(k),f3(k),f(k)4]T∈R8×1，式中fi=(uivi)为第i个特征点图像坐标，在机器人初始姿态采用试探运动方法确定粗略雅可比为：

Jκ(0)=

实验分别采用文献[20]中基于卡尔曼滤波(Kalman filtering，KF)的雅可比估计IBVS控制策略、经典PBVS控制策略[3]，以及本文研究的ADRC雅可比未建模视觉伺服控制策略进行对比，伺服任务为控制机器人末端从任意初始姿态定位到期望抓取姿态，并且抓取过程要求机器人大范围运动.为了验证各方法的鲁棒性能，实验设定特征点靠近图像边缘区域，因此初始特征为f(0)=(390.1,238.2,426.7,312.9,352.1,352.2,314.5,277.6)，期望特征为f*=(47.67,47.67,464.3,47.67,464.3,464.3,47.67,464.3)，3种方法实验结果分析如下.

实验1 采用本文基于ADRC雅可比未建模的视觉伺服控制策略.式(26)和(27)中，对其中一个特征点设置如下ADRC控制参数(参数采用经验法设定，其他特征点ADRC参数相同)：α2=α3=0.12，α4=0.53，δ2=δ3=0.5，δ4=1，b2=b3=1.43，b4=1.25.实验结果如图4所示：其中图4(a)为图像特征运动轨迹，图中圆点代表期望特征位置，可以看出图像空间特征轨迹平滑，且稳定在相机的视场范围内，特征点没有偏离视场；图4(b)为机器人末端抓取定位运动轨迹，可知笛卡尔空间机器人接近以直线的运动方式从初始位姿定位到期望位姿，运动轨迹保持平滑稳定，机器人各关节无冲突，并且没有绕行回退现象发生；图4(c)为图像特征收敛误差，图4(d)为机器人末端运动速度，结果表明图像特征收敛到期望特征，图像误差一致收敛趋向于0，同时机器人在笛卡尔空间位姿收敛，运动速度无震荡，因此采用ADRC补偿未建模雅可比视觉伺服控制效果良好.

实验3 采用传统PBVS的伺服控制方法.PBVS方法需要相机标定参数，参数包括图像中心u0=v0=256，相机焦距λku=λkv=1 000，其中ku、kv分别为图像u、v轴方向上的比例因子.测试结果如图6所示：其中图6(a)为图像特征运动轨迹，可以看出图像运动轨迹扭曲，特征点严重偏离相机视场范围；图6(b)为机器人末端抓取定位运动轨迹，由于PBVS方法直接对机器人运动实施控制，所以笛卡尔空间机器人运动轨迹以直线的方式定位到期望位姿，但是与4(b)相比，机器人运动在初始阶段数据点密集，说明震荡较大，而且直线运动存在关节受限的风险；图6(c)为图像特征收敛误差，图6(d)为机器人末端运动速度，结果可看出，与基于KF方法相似，收敛效果不及本文提出的视觉伺服控制方案.

图4 本文提出的视觉伺服方法实验结果Fig.4 The results of the proposed visual servoing method

图5 基于KF雅可比估计的IBVS方法实验结果Fig.5 The results of the KF-based Jacobian estimation IBVS method

图6 传统PBVS方法实验结果Fig.6 The results of the traditional PBVS method

综上实验结果，与传统KF和PBVS相比，本文采用基于ADRC补偿的雅可比未建模视觉伺服控制方法性能优越，图像特征运动轨迹平滑稳定在相机视场范围内，且机器人末端运动轨迹平稳无震荡回退，机器人在视觉空间中的稳态误差在10个像素范围内，定位精度高.这主要原因有两个：一方面ADRC降低了由雅可比计算精度带来的系统内扰；另一方面雅可比计算误差通过ESO和NLSEF实现了非线性状态反馈补偿.本文采用的状态误差反馈补偿方案是一种改善图像特征运动轨迹和机器人末端运动轨迹、提高伺服控制系统性能的有效手段.

5 结 论

本文研究一种基于ADRC思想的机器人视觉伺服方法. 在未建模雅可比条件下，利用无模型理论和非线性自抗扰技术建立机器人“视觉间-运动”空间映射，算法通过TD跟踪视觉空间期望特征，ESO实现未建模雅可比动态补偿，最后利用NLSEF控制规则建立机器人伺服控制律. 本文构建的视觉伺服控制方案，避免了机器人系统标定和雅可比在线求解难的问题. 最后在手眼标定六自由度机器人抓取定位比较的实验结果表明，图像空间特征轨迹与笛卡尔空间机器人末端运动轨迹平稳，无震荡回退，定位精度高. 研究方法实现了未知标定模型的机器人自主抓取定位操作.