基金项目:国家自然科学基金(61703356); 福建省自然科学基金(2018J05114); 福建省中青年教师教育科研项目(JAT160363); 厦门理工学院高层次人才项目(YKJ15020R)
通信作者:xumin@xmut.edu.cn
(1.厦门理工学院电气工程与自动化学院,福建 厦门 361024; 2.厦门大学航空航天学院,福建 厦门 361102)
(1.School of Electrical Engineering and Automation,Xiamen University of Technology,Xiamen 361024,China; 2.School of Aerospace Engineering,Xiamen University,Xiamen 361102,China)
robot visual positioning; global state space; visual feedback control; unite learning
DOI: 10.6043/j.issn.0438-0479.201703033
针对机器人非标定全局定位问题,研究Kalman滤波(Kalman filtering,KF)算法联合反馈型Elman神经网络(Elman neural network,ENN)学习机器人图像空间与运动空间非线性映射关系,从而建立基于图像的视觉反馈控制方法.首先利用ENN学习得到机器人全局定位的次优状态,以此为系统状态向量构建伺服系统状态方程与观测方程,进而利用KF估计得到机器人图像雅可比矩阵.其次,采用KF对ENN网络权重进行在线微调,KF联合ENN满足机器人全局定位稳定收敛的要求,并对环境干扰具有一定的自适应性.最后在摄像机参数未标定条件下,进行六自由度机器人“眼在手”(eye-in-hand)定位比较试验,结果验证了提出的非标定视觉伺服控制方法的有效性.
To address the robotic uncalibration global positioning problem,we studied a Kalman filtering(KF)unite feedback Elman neural network(ENN)for learning nonlinear mapping between robot image-space and movement-space,then propose an image-based visual feedback control method.First,suboptimum states were obtained by ENN global learning for robot global positioning to build the system state equation and observation equation,and further use KF to estimate the image Jacobin matrix.Second,KF also fine-tuning the ENN's weights in real time,the KF cooperative working with ENN not only meets the global stability of the robot global positioning,but also exhibits a certain adaptability to the dynamic environment.Finally,under conditions of uncalibrate the camera parameters,many positioning comparison experiments had been carried out with six degrees of freedom "eye-in-hand" robotic to verify the effectiveness of the proposed uncalibration image visual serving method.
随着机器人应用的不断推广,机器人必将承担多样的任务,面临多变的工作环境,而机器人利用视觉信息调整自身位形,由此构成的视觉伺服反馈控制方式,有利于提高机器人操作的灵活性和环境自适应性,在机器人控制中具有不可替代的作用[1].
根据视觉反馈信息的不同,机器人视觉伺服控制可分为基于位置的视觉伺服(position-based visual servoing,PBVS)和基于图像的视觉伺服(image-based visual servoing,IBVS)[2].PBVS方法以目标3D笛卡尔坐标为反馈信号,由目标几何模型及摄像机标定参数估计目标位姿,控制器根据机器人相对目标位姿进行轨迹规划并实施接近目标控制.而IBVS方法直接以2D图像为反馈信息,以当前图像特征与期望特征之间的图像误差计算机器人控制量,并实施运动控制[3-5].相比于PBVS,IBVS因无需估计目标在笛卡尔空间中的3D位姿,精简了三维重建而得到广泛关注[6-8].然而,基于标定的经典IBVS控制方法依赖于图像雅可比矩阵将图像误差函数映射到机器人运动中,该伺服方法一方面需要标定摄像机参数,另一方面需要特征点的深度信息[2],在非结构环境中难以发挥其优势.为此,在非结构、非标定环境中,建立新的非标定IBVS控制方法成为当前的研究热点.非标定IBVS的研究,其关键问题是在未知标定参数、深度信息条件下实时求解机器人“图像空间-运动空间”之间非线性映射雅可比矩阵[9-14].
本文中针对机器人全局定位问题,研究一种基于图像的非标定视觉伺服控制方法.首先采用反馈型Elman神经网络(Elman neural network,ENN),对机器人“图像空间-运动空间”之间非线性映射关系进行全局学习,得到机器人末端定位的次优状态.在线测试阶段,进一步采用卡Kalman滤波(Kalman filtering,KF)算法对ENN输出状态实施最优滤波,得到机器人定位的雅可比精确估计值.同时,KF状态对ENN权重进行在线微调,保证ENN网络下一时刻状态输出收敛稳定.该研究方法在线调节ENN网络权值,保证机器人大范围定位的稳定性,并使伺服系统具有一定的环境自适应性.此外,研究方法通过在线估计雅可比矩阵,不需要摄像机内部参数和目标深度信息,避免了摄像机和手眼标定.最后进行“眼在手”(eye-in-hand)六自由度机器人定位比较实验,验证了提出的非标定视觉伺服方法的可行性与优越性.
以在板摄像机机器人视觉反馈为对象,视觉伺服系统以图像特征为反馈信息,控制机器人从初始位形定位到期望位形.为此,在图像平面中定义图像误差:
e(k)=S(k)-Sd,(1)
式中,S(k)∈Rn×1为n维当前图像特征向量,Sd∈Rn×1为n维期望特征向量,二者由摄像机投影模型得到.
摄像机投影模型如图1所示,C{Oc-XcYcZc}为摄像机坐标系,I{OI-UV}为图像平面坐标系.假设目标点在摄像机坐标系C中的笛卡尔坐标矩阵为P=[xc,yc,zc],摄像机坐标原点Oc与目标点P的连线与摄像机成像平面I相交一点S,则在小孔成像条件下,S即为P的成像点,S=[u,v]可表示为:
(ST
1)=1/(zc)(~overω)(PT
1),(2)
式中,(~overω)=[Π3×3 03×1]∈R3×4为摄像机投影矩阵,Π为摄像机内参数矩阵.
机器人视觉反馈控制通常以速度为控制量,因此通过计算机器人运动速度即可确定机器人位形.考虑六自由度机器人“眼在手”系统,并令机器人控制量
U(k)=[v(k),w(k)]T,
其中v(k)=[vx(k),vy(k),vz(k)],w(k)=[wx(k),wy(k),wz(k)]分别为机器人末端在基坐标系中的线速度和角速度,则机器人运动速度U(k)与图像特征S(k)的变化量满足以下映射关系[2]:
(·overS)T(k)=J(k)U(k),(3)
式中J(k)∈Rn×6为机器人位形与图像特征的函数,定义为图像雅可比矩阵.
例如,采用二维图像点控制六自由度机器人运动,由摄像机投影模型(2)可知,若图像无畸变,即图像平面中U轴垂直V轴,则式(3)转变为点特征图像雅可比矩阵J'(k),有如下显式:
J'(k)=
(η/(zc)0 -u/(zc)-(uv)/η η+(u2)/η -v
0 η/(zc)-((v))/(zc)-η-(v2)/η -(uv)/η u)∈
R2×6,(4)
由式(4)可知,图像雅可比矩阵包含了摄像机内部参数η和特征点深度信息zc,所以机器人位形对应的图像特征与摄像机参数以及目标点深度信息相关,这意味着视觉反馈系统需要介入标定摄像机内部参数,即为机器人基于标定的经典IBVS工作方式.
而在摄像机参数和目标深度信息未知环境中实现机器人视觉反馈控制,其关键问题是图像雅可比在线估计.本文把雅可比矩阵视为机器人“图像空间-运动空间”非线性动态映射关系,并转化为状态估计问题.式(3)对应n维图像特征向量雅可比矩阵不显式表示为:
J(k)=[j11 … j16
jn1 … jn6]∈Rn×6,(5)
再把式(5)雅可比矩阵的6n个元素构建一个列向量X(k),形式如下:
X(k)=[j11…j12 … jn1…jn6]T ∈R6n×1,(6)
式中jik代表雅可比J(k)的第i行和第k列元素.
不失一般性,把式(6)X(k)作为机器人伺服系统的状态向量,并考虑离散动态系统:
X(k)=φ(k/(k-1))X(k-1)+F(k-1),(7)
Z(k)=h(k)X(k)+V(k),(8)
式中,φ(k/k-1)为状态转移矩阵,F(k)为过程噪声,V(k)为观测噪声,方差分别为Q和R,Z(k)为观测向量,根据式(3)定义:
Z(k)=S(k)-S(k-1)=J(k)U(k),(9)
那么观测矩阵h(k)可写成:
h(k)=[U(k)… 0
0 … U(k)]∈R6×6n.(10)
KF算法作为最小方差状态估计器,实用于离散系统的状态估计,若系统(7)和(8)中过程噪声F(k)和观察噪声V(k)为高斯白噪声序列,那么应用KF算法对系统(7)和(8)进行状态预测,即可得到雅可比估计值.然而,实际环境中,观测噪声V(k)为CCD传感器的测量噪声,不满足白噪声条件,再加上摄像机畸变和图像处理算法精确性等因素,传统KF效果不佳,在大范围空间内由于递推积累误差的影响容易导致滤波器发散,从而机器人末端定位不准,甚至失败.为此,本文中研究一种基于KF联合反馈型ENN网络的非标定全局空间机器人视觉伺服方法.
首先采用反馈ENN[15]对机器人“图像空间-运动空间”非线性动态映射进行全局学习,得到伺服系统的次优状态,使机器人末端能够在大范围内定位到期望位形的邻域内.
反馈型ENN是一种典型的动态递归网络,除了和普通神经网络一样包含隐含层和输出层,还包含关联层,所以ENN最大优点是关联层具备一定的记忆功能,能够记忆隐含层的激励输出,这使得ENN广泛用于动态系统的辨识和预测控制等领域.
本文中采用的ENN网络拓扑结构如图2所示,网络输入为视觉特征向量S(k),输出为伺服系统次优状态向量X'(k),网络数学模型描述如下:
输入层输入输出关系式为:
OI(k)=f(Wl1S(k)+Wl2OC(k)-αi),(11)
式中,OI(k)为输入层的输出量,S(k)为输入图像特征,OC(k)为关联层的输出量,Wl1、Wl2分别为输入量和关联层与输入层之间的连接权值,αi为输入层的偏置向量,f(x)为输入层激励函数,采用S型函数,如下式:
f(x)=1/(1+e-x),(12)
关联层输入输出关系式为:
OC(k)=OI(k-1),(13)
输出层的输入输出关系式为:
X'(k)=Wl3OI(k)-αj,(14)
式中,Wl3为输入层与输出层之间的连接权值,αj输出层的偏置向量.
网络各层连接权值Wli(i=1,2,3)通过离线训练获取.本文中选用四个图像点组成八维特征向量,实现六自由度机器人运动控制,首先采用示教法控制机器人遍历全局工作范围,稀疏采样600个样本训练ENN,输入样本为图像特征集,即S=(S1(k),S2(k),…,S600(k))∈R8×600,其中Si(k)∈R8×1 为第i个特征向量.输出样本为系统状态向量集,即X=(X1(k),X2(k),…,X600(k))∈R48×600,其中Xi(k)∈R48×1为第i个特征向量对应的系统状态.学习算法采用梯度下降法[15],网络学习迭代110步,达到收敛稳定,训练最小平方和误差为1.3,说明网络测试输出大致接近训练输出,这意味着在全局空间范围内,机器人末端能够大致定位到期望位形的邻域内.为实现机器人精确定位,下一步是对ENN输出的次优状态进行KF滤波,实现图像雅可比矩阵在线精确估计.
在非标定环境中,本研究将KF联合反馈ENN,构建机器人全局状态空间视觉伺服控制方案.框架如图3所示,主要由控制律、KF联合ENN雅可比在线估计、ENN权值更新组成.
首先设计一个可靠的伺服控制律对整个机器人视觉伺服系统至关重要.式(1)期望特征Sd为已知定值,对式(1)求导可得:
(·overe)(k)=(·overS)(k),(15)
考虑非零常数λ使以下等式成立:
(·overe)(k)=-λe(k),(16)
将式(15)代入式(16)可得:
(·overS)(k)=-λe(k),(17)
把式(17)代入式(3)可得:
-λe(k)=J(k)U(k),(18)
上式变形得到伺服控制律:
U(k)=-λJ+(k)e(k),(19)
式中λ为控制系数,J+(k)=J(k)T(J(k)J(k)T)-1为雅可比矩阵J(k)的广义逆.
控制器式(19)中雅可比矩阵J(k)未知,本文中采用雅可比估计值(^overJ)(k)代替J(k).雅可比精确估计值由KF算法联合反馈ENN网络得到,算法具体步骤为:
1)首先视觉传感器结合图像处理算法提取图像特征S(k),ENN网络根据式(14)输出系统次优状态X'(k).
2)令X(k-1)=X'(k),代入状态转移方程式(7)获取伺服系统状态更新值X(k).
3)X(k)带入观测方程(8)得到k时刻状态观测值Z(k).
4)应用KF最小方差递推算法获取k时刻最优状态估计值(^overX)(k),并把状态向量恢复成雅可比矩阵形式,算法如下:
初始化:J(0)∈R8×6; J(0)→X(0)∈R48×1
Q∈R48×48; R∈R8×8.
1)(^overX)(k/(k-1))=φ(k/(k-1))(^overX)(k-1).
2)P(k/(k-1))=φ(k/(k-1))P(k-1)φ(k/(k-1))T+Q.
3)K(k)=P(k/(k-1))hT(k)(h(k)P(k/(k-1))h(k)T+R)-1.
4)(^overX)(k)=(^overX)(k/(k-1))+K(k)(Z(k)-h(k)
(^overX)(k/(k-1))).
5)P(k)=(E-K(k)h(k))P(k/(k-1))(E-
K(k)h(k))T+K(k)RK(k)T.
6)(^overJ)(k)←(^overX)(k).
以上得到k时刻雅可比精确估计值(^overJ)(k),为了确保下一时刻机器人定位的稳定性,此时有必要对k时刻ENN权值进行在线微调.权值更新以图像特征S(k)为ENN的输入量,ENN在线输出系统次优状态X'(k),然后采用梯度下降法微调ENN权值(如图3虚线所示),使网络输出值X'(k)跟踪KF最优估计值(^overX)(k),即通过更新网络权值使以下代价函数最小:
σ(k)=argmin(1/2ΔXT(k)ΔX(k)),(20)
式中ΔX(k)=((^overX)(k)-X'(k)).网络权值W(k)更新规则如下:
W(k+1)=W(k)+γΔX(k)(X'(k))/(W),(21)
式中γ为学习率.
机器人非标定视觉伺服框架如图3所示,基本步骤为,首先摄像机获取图像特征,ENN输出系统次优状态; 在此基础上利用KF算法实现最优状态估计,得到雅可比精确估计值; 其次ENN权值在线更新确保下一时刻系统稳定; 最后机器人控制律以时间为指标,即在额定的采样时间内评判图像误差,若达到额定采样时间,则机器人定位结束,定位误差值为像素.
如图4所示,采用微软公司Kinect摄像机,并将摄像机固定在机器人末端,组成“眼在手”机器人实验平台.摄像机通过USB接口与个人PC相连,PC 通过 RS232 串口与机器人控制器相连接,构成机器人视觉伺服闭环系统.PC作为上位机主要完成图像采集与图像处理,并执行雅可比估计算法和伺服控制算法,机器人控制器作为下位机完成机器人运动学运算,同时驱动机器人各个关节.定位试验以六自由度机器人末端线速度和角速度为控制量,即U(k)∈R6×1,以A4纸打印4个黑点为定位特征,那么特征向量S(k)为:
S(k)=[s1 s2 s3 s4]T∈R8×1,(22)
式中si=(ui vi)为第i个图像点,所以图像雅可比矩阵大小为8×6,初始化为:
J(0)=
[-0.66 0 0.11 -0.02 -1.02 -0.16
0 -0.66 -0.10 1.02 0.02 -0.16
-0.66 0 0.11 0.02 -1.02 0.16
0 -0.66 0.11 1.02 -0.02 -0.16
-0.66 0 -0.11 -0.02 -1.02 0.16
0 -0.66 0.11 1.02 0.02 0.16
-0.66 0 -0.10 0.02 -1.02 -0.16
0 -0.66 -0.11 1.02 -0.02 0.16],(23)
由式(6)可知,系统状态向量X(k)大小为48×1,令状态转移矩阵φ(k)为单位矩阵,大小为48×48,控制率由实验经验选定为λ=0.15,采样间隔为0.1 s.
为了验证本文中非标定视觉伺服方法(简称本文方法)的有效性,以经典PBVS和IBVS方法[2]为比较对象,进行机器人大范围定位试验.试验中PBVS和IBVS需要摄像机内部参数(本文中方法不需要摄像机参数),摄像机参数有:图像中心u0=v0=256,摄像机焦距ηku=ηkv=1 000,ku、kv分别为图像U、V轴方向上的比例因子.为了体现机器人大范围定位,机器人初始位姿与期望位姿分别设定在机器人运动范围的最大边缘和最小边缘处,对应初始图像特征为S(0)=[378,235,429,301,363,354,311,289]T,期望图像特征为Sd=[47,47,464,45,460,463,40,459]T.
PBVS方法试验结果如图5所示(图中小方块代表特征点期望位置),由图5(a)可知,虽然PBVS方法机器人末端从初始位姿定位到期望位姿,机器人运动轨迹几乎接近直线,震动小,但是图5(b)图像特征运动轨迹不佳,特征点容易超出摄像机视场范围.
IBVS方法试验结果如图6所示,图6(a)机器人运动轨迹不稳定,机器人在起始段产生较大震动.由图6(b)可知,虽然IBVS方法图像特征轨迹以直线方式从初始位置收敛到期望位置,特征点保持在摄像机视场范围内,
本文方法试验结果如图7所示,由图7(a)可知,本文方法机器人几乎以直线方式从初始位姿定位到期望位姿,机器人运动轨迹稳定,震动小; 同时由图7(b)可知,图像特征轨迹以近直线方式从初始位置收敛到期望位置,并且特征点保持在摄像机视场范围内.图8为机器人定位误差,3种伺服方法图像误差收敛趋向于0,机器人定位成功.
以上比较试验可知,PBVS方法机器人运动轨迹良好,但图像特征轨迹容易偏离摄像机视场范围; IBVS方法图像特征轨迹良好,但机器人运动轨迹初始段不稳定,产生较大震动; 而本文中方法机器人运动轨迹稳定无震动,图像特征轨迹保持在摄像机视场范围内,综合性能较好.
为了进一步验证本文方法的稳定性,考虑外界干扰,通过与传统KF方法比较说明本文方法的稳定性能.机器人真实环境操作过程,系统过程噪声和传感器观测噪声的摄动,都将导致系统不稳定.不失一般性,试验中假设过程噪声F(k)和观测噪声V(k)为零均值,方差分别为Q=1.2×[1 … 0
0 … 1]∈R48×48,R=0.1×[1 … 0
0 … 1]∈R8×8的白噪声序列.设定初始图像特征S(0)=[272,336,315,420,230,465,186,380]T,期望图像特征Sd=[89,89,422,80,420,423,85,425]T.
传统KF方法试验结果如图9所示,图9(a)为3D笛卡尔空间机器人运动轨迹,图9(b)为2D图像平面特征点运动轨迹.由图9(a)可知,在噪声干扰影响下机器人末端出现较大幅度的绕行回退运动,进而导致图像特征轨迹扭曲,特征点偏离摄像机视场范围(见图9(b)).
而对于相同干扰条件下的机器人定位任务,本文中KF联合ENN方法试验结果如图 10所示,图 10(a)为3D笛卡尔空间机器人运动轨迹,图 10(b)为2D图像平面特征点运动轨迹.由图 10(a)可知,机器人运动轨迹稳定,无绕行回退现象产生; 同时由图 10(b)可知,图像特征轨迹平滑,特征点保持在摄像机视场范围内.
另外,由图 11可以看出,在额定的采样时间内,本文中KF联合ENN方法图像误差收敛速度较KF方法快.以上结果其主要原因是首先由ENN得到机器人全局定位的次优状态,在这基础上KF实施雅可比精确估计,并实时调整ENN网络权重,这种联合工作方式有利于保证机器人全局空间运动的稳定性.
综上可知,在未知摄像机参数及目标深度信息条件下,与经典PBVS和IBVS方法相比,本文中研究的非标定图像视觉伺服控制方法性能良好.在考虑外界噪声干扰条件下,和传统KF方法相比,本文KF联合ENN方法改善了机器人运动的稳定性能,并具有一定的自适应能力.说明本文研究的KF联合ENN网络的图像雅可比动态估计方法,及构建的非标定视觉伺服控制方案真实有效.
针对非标定六自由度机器人全局空间定位问题,研究了KF联合反馈ENN网络学习的非标定图像视觉伺服控制方案.ENN网络首先对机器人“图像空间-运动空间”非线性映射关系进行全局学习,获取机器人定位的次优状态,进而采用KF进行最优状态估计,解决图像雅可比在线估计问题.KF同时对ENN权重进行实时微调,有利于保证机器人定位全局稳定,并对环境干扰具有一定的自适应性.最后本文方法与经典PBVS、IBVS以及传统KF方法进行六自由度机器人定位比较试验,实验结果验证了本文方法的有效性.