基于位置的机械臂视觉伺服预测控制

刘安东，卢威威，俞 立

(浙江工业大学 信息工程学院，杭州 310023)

E-mail：lad@zjut.edu.cn

在大批量生产的需求背景下，机械臂在物流、食品、电子和医疗等领域得到了广泛应用.然而目前的机械臂仍以示教型机械臂为主，即机械臂运动的轨迹、速度、加速度等一系列变量由程序事先给定，工作场景过于单一.其主要原因在于机械臂对外部环境变化的适应能力有限，无法像人一样感知环境变化.视觉传感器在不接触目标工件的情况下能通过视觉信息反馈指导机器人工作，利用视觉伺服技术可以有效地提高机械臂的环境适应能力，扩大机械臂系统的使用范围.因此，机械臂视觉伺服控制引起了国内外学者的广泛关注[1-4].

根据建模方法的不同，视觉伺服可分为基于图像的视觉伺服(IBVS)[2]、基于位置的视觉伺服(PBVS)[3]以及混合视觉伺服(HVS)[4].IBVS需要实时计算图像雅克比矩阵，计算负荷量较大，而HVS无法确保在伺服过程中目标物始终位于相机的视野之内.相比之下，PBVS利用期望位姿与反馈位姿的差值作为视觉传感器的输入量，在三维空间中完成伺服控制任务，可避免奇异性和局部极小值等问题.因此，位姿估计和机械臂控制是基于位置的机械臂视觉伺服任务的两大重要组成部分.在位姿估计方面，文献[5]通过卡尔曼滤波方法识别和跟踪特征点运动，从而实现了多相机系统的实时位姿估计.文献[6-8]在考虑了冗余特征点信息丢失的影响下，分别提出了基于扩展卡尔曼滤波(EKF)以及无迹卡尔曼滤波(UKF)的位姿估计方法.文献[9，10]在EKF的基础上增加了迭代和自适应环节，极大提高了估计精度以及系统鲁棒性.但是这几类方法均要求噪声服从高斯分布，然而实际系统中通常是有界噪声，难以获得噪声的统计特性.

机械臂控制方面，近十年已提出了许多有效的算法，如PID控制、自适应控制、模糊控制、滑模控制、神经网络等[11-16].文献[12]针对两连杆机械臂实现了基于视觉反馈的PID位置跟踪控制.但PID算法不适用于复杂系统.文献[13，14]在视觉伺服系统中利用自适应控制算法，保证了机械臂视觉伺服系统的鲁棒性以及稳定性.然而自适应算法难以选取合适的参数.文献[15]设计了基于PBVS的二阶滑模控制器实现7自由度冗余机械臂的位姿控制，但是滑模控制在滑模面上会产生抖振现象.尽管上述算法在不同方面提高了机械臂的控制性能，但是无法有效处理机械臂的实际控制约束问题，例如控制输入饱和、任务空间受限、速度受限等.

模型预测控制(MPC)由于能够显性地处理系统约束问题，受到了广大研究者的青睐[17-19].其中文献[18]提出了一种基于PBVS的自触发非线性MPC方法，大大减少了视觉系统的测量次数，从而有效提升了处理效率.文献[19]在eye-in-hand框架下采用MPC方法实现机械臂在空间中的自动抓取.但是MPC算法中优化问题的计算负荷很大，导致伺服效率不高.iLQR/iLQG算法[20-22]是一种高效求解优化问题的方法，在保证最优解精度的同时简化了MPC的求解.针对非完整性四轮车，文献[20]在考虑控制输入计算时延的前提下，设计了基于iLQG的MPC方法有效解决了有限时域最优控制问题.文献[21]将MPC方法与iLQR算法相结合，在类人机器人上实现了平面游泳以及单腿跳跃的实验.然而，上述方法中代价函数均选择为二次形式，若将二次代价函数用于非线性系统，则系统可能会陷入局部最优状态并存在线性化误差.

基于以上讨论，本文着重于解决基于位置的机械臂视觉伺服中的位姿估计和目标跟踪控制问题.首先在PBVS方案下建立了关节机械臂运动学模型.再根据相机投影模型进行特征点变换，并利用MHE方法求解代价函数优化问题，从而实现有界噪声约束下的机械臂位姿估计.然后提出了基于iLQR的MPC方法，利用动态规划原理以及对偶梯度上升方法求解控制输入约束下的预测控制优化问题，得到最优控制序列.为了克服估计误差和线性化误差引起的扰动问题，采用极点配置方法设计了ESO对扰动进行前馈补偿，并对观测误差有界性和闭环系统稳定性进行了证明.最后通过数值仿真与经典PBVS控制方法进行对比，验证了本文所提算法的有效性.

考虑如图1所示基于位置的视觉伺服系统，主要可分为位姿估计以及控制器设计两部分.位姿估计部分是将相机特征点的运动转化为三维空间中末端执行器的运动，从而实现二维到三维空间中的信息转换.控制器设计部分则是利用估计得到的位姿计算机械臂的控制变量，并驱动机械臂逐渐达到理想位姿以实现对象的定位或跟踪.

图1 PBVS系统Fig.1 PBVS system

关节机械臂运动学方程可写为如下形式：

q·=J(q2)u

(1)

其中，q=[qxqyqzqφqθqψ]T为位姿向量，位姿向量包含位置向量q1=[qxqyqz]T以及姿态(欧拉角)向量q2=[qφqθqψ]T，qφ、qθ、qψ分别表示滚动角(Roll)，俯仰角(Pitch)以及偏航角(Yaw)，u=[vxvyvzwφwθwψ]T为速度矢量.J(q2)∈R6×6是用欧拉角定义的变换矩阵，即：

J(q2)=J1(q2)0
0J2(q2)

(2)

从而可将式(1)改写为如下离散时间形式：

qk+1=f(qk，uk)=qk+T·J(qk)·uk

(3)

其中，T为采样时间.

本文的目的是在针对基于位置的机械臂视觉伺服系统(3)，采用MHE方法估计机械臂3维位姿信息；
根据当前估计位姿与期望位姿之差，采用基于iLQR的MPC算法设计视觉伺服控制器；
进一步利用ESO方法解决扰动问题，从而实现伺服控制并驱动机械臂达到期望位姿.

本节将采用MHE方法实现机械臂位姿估计.机械臂位姿包括机械臂末端和目标物之间的相对位姿以及其相对速度：

xk=[qkq·k]

(4)

相机投影模型如图2所示，其中涉及的坐标系包括：图像坐标系Oi-(Xi，Yi)，像素坐标系Opixel-(m，n)，相机坐标系Oc-(Xc，Yc，Zc)，物体坐标系Oob-(Xob，Yob，Zob)，Xi和Yi轴分别与XC和YC轴平行.

图2 相机投影模型Fig.2 Camera projection model

为了估计姿态xk，本文选取4个特征点.根据图2，以第j个特征点为例描述该点映射到像平面上的像素坐标与状态量之间的关系.该特征点在图像坐标系和像素坐标系下的坐标分别为(qix，j，qiy，j)和(mj，nj).则像素坐标与图像坐标之间的关系为：

mj
nj
1=1/dx0m0
01/dyn0
001qix，j
qiy，j
1

(5)

其中，1/dx，1/dy分别表示像素点的长和宽，(m0，n0)为图像坐标系原点在像素坐标下的坐标.

相机坐标系与物体坐标系之间关系为：

Pcj=cRobctob
01×31Pobj=cTobPobj

(6)

其中，Pobj=(qobx，j，qoby，j，qobz，j，1)T表示物体坐标系中第j个特征点的齐次坐标，Pcj=(qcx，j，qcy，j，qcz，j，1)T表示相机坐标系中第j个特征点的齐次坐标，cTob=cRobctob

01×31为物体坐标系到相机坐标系的变换矩阵，cRob为3×3旋转矩阵，ctob表示物体坐标系原点在相机坐标系下的坐标.

由相机模型可得相机坐标系与图像坐标系之间的投影关系为：

qix，j=Fqcx，jqcz，j，qiy，j=Fqcy，jqcz，j

(7)

综合以上转换关系，可得到像素坐标系与物体坐标系之间的关系为：

mjk
njk=fj(xk)=HccTobPobjGccTobPobj

(8)

其中，Hc=F/dx0m00

0F/dyn00表示相机内参矩阵，F为焦距，Gc=[0 0 1 0].

令量测输出yk=[m1k，n1k，…，m4k，n4k]T，非线性量测函数f(xk)=[f1(xk)，…，f4(xk)]T.由此建立系统状态方程和量测方程如下：

xk+1=Cxk+ξk

(9)

yk=Dxk+ηk

(10)

其中，C=diag{a，a，a，a，a，a}为状态矩阵，a=1T

01，ξk为k时刻的有界系统噪声，ηk为k时刻的有界量测噪声，D为与特征点像素坐标(mjk，njk)T相关的量测矩阵，可通过f(xk)在平衡点x0附近泰勒展开得到.

根据滚动优化策略，MHE的主要步骤可分为优化，预测和传输3个部分：

优化：根据代价函数进行优化，从而估计xk-M在k时刻的估计值k-M，k，即：

k-M，k=arg minJMHE，k

(11)

其中，M为滚动时域窗口长度，代价函数定义如下：

JMHE，k=σ‖k-M，k-k-M‖2+∑ki=k-M‖yi-Di，k‖2

(12)

其中，σ为正常数.

预测：根据预测方程(13)和估计值k-M，k得到预测值k-M+1为：

k-M+1=Ck-M，k，k=0，1，…

(13)

传输：k-M+1，k，…，k，k可通过估计值k-M，k来表示：

i+1，k=Ci，k，i=k-M，…，k-1

(14)

根据上述步骤，则位姿估计问题描述如下：

问题1.给定k-M时刻的预测值k-M和从k-M到k时刻的量测序列ykk-M，通过最小化代价函数(12)，可得系统(9)、(10)的最优位姿估计值*k-M，k为：

*k-M，k=argmink-M，kJMHE，k
s.t.k-M，k∈X
-rξ≤ξk≤rξ
-rη≤ηk≤rη

(15)

预测更新方程为：

*k-M+1=C*k-M，k

(16)

注1.MHE是预测控制的对偶问题，其主要思想是利用有限个测量输出来估计当前系统状态.所以，MHE优化问题本质上与MPC是一致的，为此优化问题(15)的求解过程在此省略，详见第3节MPC的求解过程.

本节的目的是根据当前估计位姿与期望位姿之差，设计视觉伺服控制器.由于PBVS系统可测量状态为二维平面上的图像像素坐标，无法直接获得笛卡尔空间三维坐标，因此可根据最优估计值*k，t中的位姿*k，t与期望位姿qd之差定义在k时刻的位姿误差ek，t：

ek+1，t=*k+1，t-qd

(17)

由文献[23]可知，滚动时域估计器所获得的估计误差是有界的，因此采用估计位姿来定义位姿误差会引入一定的干扰.对式(3)在平衡点附近进行线性化(qk=qd，uk=0)并带入式(17)可得：

ek+1，t=*k+1，t-qd
=*k，t+Buk，t+vk-qd
=ek，t+Buk，t+vk

(18)

其中，B=T·J(qd)，vk为估计误差和系统线性化误差引入的干扰且有界.为此，本文采用基于ESO的MPC方法解决具有外部扰动和约束的视觉伺服控制问题，控制结构图如图3所示.

图3 预测控制结构图Fig.3 Predictive control structure diagram

4.1 基于iLQR的MPC

为了解决具有约束的视觉伺服控制问题，首先定义如下的MPC的性能指标：

JMPC，t=∑t+N-1k=tL(ek，t，uk，t)+L(eN)

(19)

其中，L(ek，t，uk)=‖ek，t‖2Q+‖uk，t‖2R为代价函数，L(eN)=‖et+N，t‖2W为终端代价函数，N是预测步长，Q，R是正定权重矩阵，W是需要设计的终端权重矩阵.

由于ESO采用前馈补偿控制方法，预测控制器设计只需考虑名义系统如下：

k+1，t=k，t+Bk，t

(20)

则预测控制优化问题描述如下：

问题2.在k时刻给定名义位姿误差k，t，通过最小化MPC性能指标，可得系统(20)的最优名义控制输入*k，t为：

*k，t=argmink，tJMPC，t
s.t.umin≤k，t≤umax

(21)

为了处理控制输入约束，利用对偶梯度上升方法引入对偶变量ck和dk可得如下性能指标：

J′MPC，t=∑t+N-1k=t[L(k，t，k，t)+ck(umin-k，t)+
dk(k，t-umax)]+L(N)

(22)

针对系统(20)，假定在t时刻存在一组轨迹：

{t，t，…，t+N-1，t，t+N-1，t，t+N，t}

(23)

使得优化问题(21)存在最优解，并由此根据最优性原理设置值函数：

V(k，t)=minUtJ′MPC，t

(24)

其中，Ut={t，t+1，t，…，t+N-1，t}.

利用贝尔曼最优性原理将式(24)整个控制序列的最小化改写为单个控制信号最小化的迭代形式：

V(i，t)=mini，t[L(i，t，i，t)+V(g(i，t，i，t))]

(25)

其中，i∈[t，t+N-1].

根据式(25)定义ΔP为围绕(i，t，i，t)的扰动函数：

ΔP(δi，t，δi，t)

=P(i，t+δ，i，t+δ，i)-P(i，t，i，t，i)

=L(i，t+δi，t，i，t+δi，t)-L(i，t，i，t)+

V(g(i，t+δi，t，i，t+δi，t))-V(g(i，t，i，t))

=12[1δi，tδi，t]0PTPT

PPP

PPP1

δi，t

δi，t

(26)

其中，泰勒系数P如下所示：

P=L+gTVi+1

P=L+gTVi+1

P=L+gTVi+1g+Vi+1g

P=L+gTVi+1g+Vi+1g

P=L+gTVi+1g+Vi+1g

利用一阶KKT条件可得最小化式(26)的解析解δ*i，t为：

δ*i，t=arg minδiΔP(δi，t，δi，t)
=ri+Riδi，t

(27)

其中，ri=-P-1P，Ri=-P-1P.

将式(27)代入式(26)，可得：

ΔVi=12rTiPri+rTiP

(28)

Vi=P+RTiPri+RTiP+PTri

(29)

Vi=P+RTiPRi+RTiP+PTRi

(30)

通过计算ri，Ri，ΔVi，Vi以及Vi的值，从而完成向后遍历步骤.然后利用向后遍历步骤得到的序列{rt，Rt，…，rt+N-1，Rt+N-1}实现系统名义位姿误差序列和名义控制序列的更新，完成向前遍历.

t←t
i，t←i，t+ri+Riδi，t
i+1，t←i，t+Bi

(31)

可通过反复执行向后遍历以及向前遍历步骤，得到最优名义控制序列U*=[*t，…，*t+N-1，t]T，并利用文献[24]中的拉格朗日乘子更新规则，更新式(22)中的ck和dk：

ck=ck+α(umin-k，t)
dk=dk+α(k，t-umax)

(32)

其中，α是更新率.

注2.由文献[25]可知，iLQR算法是一种递归优化算法，具有二次收敛特性，因此可以确保能得到式(27)中的最优解δ*i，t.

4.2 扩张状态观测器

由式(17)可知，vk为估计误差和系统线性化误差引入的干扰且有界.不失一般性，假设误差vk满足以下增量形式：

vk+1=vk+Δvk

(33)

结合式(18)，可得到扩张状态方程：

Xk+1=A1Xk+B1uk，t+S1Δvk

(34)

其中，Xk=ek，t

vk，A1=II

0I，B1=B

0，S1=0

I.为了对有界干扰进行估计，将Xk表示为式(34)的输出信号，从而得到如下的观测器方程：

k+1=A1k+B1uk，t+KobS2(Xk-k)

(35)

令Ek+1=Xk+1-k+1，并结合式(34)以及式(35)得到观测误差系统：

Ek+1=(A1-KobS2)Ek+S1Δvk

(36)

根据ESO方法，设计复合控制器：

uk，t=*k，t-B-1k

(37)

并将其代入式(18)，从而得到闭环系统模型：

ek+1，t=ek，t+B*k，t+vk-k

(38)

4.3 稳定性分析

基于以上分析，本文将分别对观测误差模型有界性以及闭环系统模型稳定性进行证明.首先，针对观测误差模型(36)可得以下定理.

定理1.若存在合适的观测器增益矩阵Kob使得：

α=‖A1-KobS2‖<1

(39)

成立，则观测误差有界且满足以下不等式：

‖Ek‖≤εk

(40)

序列εk是指数衰减的：

εk=αεk-1+β

(41)

其中，β=supk{‖S1Δvk‖}.

证明1.首先将算子应用于观测误差方程(36)并结合三角不等式，可得：

‖Ek+1‖≤‖(A1-KobS2)Ek‖+‖S1Δvk‖
≤‖A1-KobS2‖·‖Ek‖+‖S1Δvk‖

(42)

由式(39)已知‖A1-KobS2‖<1，同时S1为确定的矩阵，vk为有界线性化误差，因此‖S1Δvk‖必有界，则式(42)可改写为如下形式：

‖Ek+1‖≤α‖Ek‖+β

(43)

根据式(41)以及式(43)可知，当k→+∞时，在满足α<1的条件下，εk指数衰减于渐近值ε∞=β1-α.因此结合式(40)可确保观测误差模型(36)是有界的.

然后针对闭环系统模型(38)的稳定性结果证明可以表示如下.

定理2.针对性能指标(19)以及闭环系统(38)，在给定权重矩阵Q和R的情况下，若存在合适的矩阵K以及终端权重矩阵W满足以下不等式：

-W-10W-1+KTBTKTW-1
*-II00
**-W-100
***-R-10
****-Q-1≤0

(44)

则闭环系统(38)是稳定的，同时J*MPC，t为李雅普诺夫函数.

证明2.首先假定超过预测步长N的控制序列如下所示：

*k，t=Kek，t，k≥t+N

(45)

J*MPC，t=∑t+N-1k=t[‖ek，t‖2Q+‖*k，t‖2R]+‖et+N，t‖2W

(46)

根据最优性原理可知，t+1时刻的性能指标满足下式：

J*MPC，t+1=∑t+Nk=t+1[‖ek，t+1‖2Q+‖*k，t+1‖2R]+‖et+N+1，t+1‖2W
≤∑t+Nk=t+1[‖ek，t‖2Q+‖*k，t‖2R]+‖et+N+1，t‖2W
=J*MPC，t+Φt-‖et‖2Q-‖*t‖2R

(47)

其中：

Φt=‖et+N+1，t‖2W-‖et+N，t‖2W+‖et+N，t‖2Q+‖*t+N，t‖2R
=‖(I+BK)et+N+1，t+vt-t‖2W-‖et+N，t‖2W+
‖et+N，t‖2Q+‖Ket+N，t‖2R

(48)

根据式(44)并通过舒尔定理可知Φt≤0，从而得到如下不等式：

J*MPC，t+1≤J*MPC，t-‖et‖2Q-‖*t‖2R
≤J*MPC，t

(49)

由式(49)可知，J*MPC，t为一李雅普诺夫函数，从而保证闭环系统稳定.

证毕.

基于位置的视觉伺服控制算法如下所示.

算法1.

Step 1.在k=t时刻，利用MHE方法得到的估计位姿*k，t定义名义位姿误差k，t，并随机初始化名义控制序列uin=[k，…，k+N-1，k]T；

Step 2.利用uin以及k生成轨迹{k，k，…，k+N-1，k，k+N-1，k，k+N，k}；

Step 3.在j=1，2，…，g2情况下iLQR算法进行向后遍历以及向前遍历，当j=g2时迭代结束，跳转到Step 4；

Step 4.在i=1，2，…，g1情况下iLQR算法利用式(32)更新对偶变量，当i=g1时迭代结束，跳转到Step 5，否则返回Step 3；

Step 5.得到最优名义控制序列U*=[*k，…，*k+N-1，k]T，则将U*中第一个值与ESO方法结合，通过设计合适的观测器增益矩阵Kob确保α<1，从而对扰动vk进行估计；

Step 6.根据式(37)利用估计扰动k设计复合控制器，并完成机械臂控制；

Step 7.令k=k+1，返回Step 1继续执行.

为了验证本文所提算法的有效性，本节考虑针对单关节机械臂利用MATLAB进行数值仿真算例，因此变换矩阵(2)可写为如下形式：

J1(q2)=cqψcqθcqψsqθsqφ-sqψcqφcqψsqθcqφ+sqψsqφ

sqψcqθsqψsqθsqφ+cqψcqφsqψsqθcqφ-cqψsqφ

-sqφcqθsqφcqθcqφ

J2(q2)=1tqθsqφtqθcqφ

0cqφ-sqφ

0sqφ/cqθcqφ/cqθ

其中，s、c和t分别表示三角函数sin，cos以及tan.

仿真参数设置如下：

初始位姿参数q0=-0.50.10.3π5π4π3，期望位姿参数qd=[-1.20.550.9000]，预测步长N=10，采样时间T=0.01s，控制输入最大值以及最小值分别为：

umax=-umin=[0.4 0.8 0.5 0.6 0.7 0.5]

算法1中参数g1=2，g2=5，更新率α=1，正定权重矩阵Q和R分别为：

Q=diag(5，4.5，3，5，2，4)
R=diag(0.01，0.01，0.01，0.01，0.01，0.01)

终端权重矩阵W设计如下：

W=139.84 -5.57 -5.48-4.23 37.43 11.91
-5.57 34.55-27.97-22.44-22.25 18.30
-5.48-27.97 84.57-8.04-16.17 20.24
-4.23-22.44 -8.04114.91 131.79-59.96
37.43-22.25-16.17131.79 229.02-79.30
11.91 18.30 20.24-59.96-79.30 64.60

图4为MHE方法的估计误差曲线，其中图4(a)为位置估计误差，图4(b)为姿态估计误差.从中可以明显看出，位置估计误差在-0.06cm～0.05cm之间，姿态估计误差则是保持在-0.05deg～0.05deg之间，MHE方法估计精度很高，因此可以将该位姿信息作为估计目标的真实位姿.

图4 MHE估计误差Fig.4 MHE estimation error

图5 ESO扰动估计Fig.5 ESO disturbance estimation

图5为实际存在扰动以及ESO对于实际扰动的估计，其中图5(a)为位置的扰动估计，图5(b)为姿态的扰动估计.从图5中可以明显看出，位置扰动保持在0.1以内，姿态扰动则是稳定在0.15以内，而ESO对于扰动的估计误差范围基本处于-0.01～0.01之间，估计效果非常好，有效解决了估计误差和系统线性化误差扰动问题，提高了系统的跟踪性能.

图6则为基于iLQR的MPC方法在预测步长N=10情况下的状态轨迹变化曲线，其中图6(a)为位置状态轨迹，图6(b)则为姿态状态轨迹，并与经典PBVS控制方法以及未加上扩张状态观测器的算法轨迹进行对比.首先将iLQR-MPC算法与经典PBVS控制方法进行对比，可以明显看出虽然两者最后均收敛到了期望位姿qd附近，但是经典PBVS控制方法基本是在15步左右才能够达到收敛，而根据图6中信息可知，iLQR-MPC算法在5步左右就达到收敛，控制效果良好，相比经典PBVS控制方法所需时间更短.

图6 状态轨迹Fig.6 State trajectory

然而由图6亦可知，iLQR-MPC算法存在扰动误差问题，在位置轨迹方面，扰动误差较小，x，y以及z方向上的误差均处于0.02m～0.03m之间，然而在姿态轨迹方面，扰动误差较大，Pitch误差达到了0.04rad，而Roll以及Yaw的误差更是达到0.09rad.从图5中可以明显看出，ESO具有良好的扰动估计精度，可以很好地消除扰动误差的影响.因此相比单纯的iLQR-MPC算法而言，加入ESO后可以减弱估计误差以及线性化误差的影响，从而大大减小了扰动误差，状态轨迹完全收敛到了期望位姿qd附近，误差接近于0，控制效果良好.

图7中显示的则是算法在预测步长N=10情况下的控制输入变化曲线，根据上文设置的仿真参数umax以及umin可以看出，控制输入没有发生超出约束条件的情况.

图7 控制输入Fig.7 Control input

本文着重于解决基于位置的机械臂视觉伺服中的位姿估计和目标跟踪控制问题.首先根据PBVS方案建立机械臂运动学模型，再基于相机投影模型利用MHE方法实现有界噪声约束条件下的机械臂位姿估计，然后通过基于iLQR的MPC算法并结合对偶梯度上升方法处理控制输入约束问题，后续利用ESO方法有效解决了估计误差和系统线性化误差引入的干扰问题.最后，仿真结果表明，本文所提算法相比经典PBVS方法能够更快达到期望位姿.