视觉伺服控制

有约束的无标定模型预测控制

在视觉伺服控制器的设计中，图像雅可比矩阵是建立运动学模型的关键。经典的IBVS采用比例控制律，它利用图像雅可比矩阵的逆(或伪逆)。然而，比

例控制器可能存在局部极小问题。也就是说，如果视觉特征数大于3，则图像雅可比矩阵不是满秩的，图像误差可能存在于图像雅可比矩阵的逆(或伪逆)的零空间中，从而导致局部收敛，使得最终的图像特征远离期望的图像特征。另外，系统约束处理困难，尤其是可见性约束。当相机的初始位置和所需位置之间的距离较大时，图像特征将不可见。在视觉伺服控制过程中，可能会违反关节的物理限制和机器人的工作空间。此外，比例控制器的主要缺点是需要知道摄像机内参数、摄像机外参数和特征点的深度参数，而这些特征点的精确值很难获得。

为了避免使用图像雅可比矩阵中元素的精确值，人们对图像雅可比矩阵的数值估计进行了广泛的研究，如神经网络、迭代学习、拟牛顿方法和模糊控

制。文献提出了许多基于深度无关交互(或图像雅可比)矩阵的自适应

控制器，以克服深度限制问题。文献首次针对摄像机参数未知且深度随时间

变化的固定摄像机构型，提出了与深度无关的交互矩阵。文献提出了眼在手

和固定眼构型的自适应视觉跟踪控制的统一设计方法。然而，这些方案没有明确考虑系统约束，而这些约束对于视觉伺服控制器的设计是至关重要的。

已经提出了许多方法来处理有约束的视觉伺服任务。例如路径规划、非线性反馈等，但大多需要给定摄像机的外部参数，并且假定摄像机的内部参数和深度信息是已知的。在IBVS中，通常采用模型预测控制(Model Predictive Control，MPC)来处理系统约束，且MPC控制器具有在未知影响和模型误差的情况下对

系统进行控制的能力。因此，MPC算法可以用来设计无标定环境下的视觉伺服控制器。本章主要提出了一种新的基于MPC的IBVS设计方法，该方法明确地考虑了系统的约束条件，能够有效地处理未知的摄像机参数和深度参数。通过模型预测控制获得控制输入，通过参数估计算法在线更新预测模型的未知参数，完成视觉伺服任务。

有约束和无标定视觉伺服的预测模型

模型预测控制被用来处理未标定环境中眼在手上和眼在手外摄像机构型的IBVS系统的控制约束。在无标定的环境中，摄像机的内外参数和特征的三维坐标是未知的。为了通过MPC获得最优控制输入，需要找到一个预测模型来描述系统的动态行为。介绍了基于深度无关交互矩阵的预测模型。在透视投影模型下，特征点的图像坐标可以描述为：

s m (t) c

其中s m (t) (u(t), v(t))T表示特征点在图像平面上的图像坐标。特征点的深度用

c

决于摄像机的内部参数和外部参数。H表示机器人正向运动学的齐次变换矩阵，

和p分别是H的旋转矩阵和平移矢量。在眼在手上摄像机构型中C BH ec，H ec 是摄像机外部参数矩阵，它表示末端执行器坐标系相对于摄像机坐标系的齐次矩阵。B是相机的内参数矩阵，由相机内部结构决定。H ( H eb )1，其中H eb为末端执行器坐标系相对于机器人基坐标系的齐次矩阵。b x是特征点在机器人基坐标

系的三维位置参数，b x是具有3个独立元素的常数向量。在眼在手外构型中，

C BH bc，其中H bc是相机外参数矩阵，它表示机器人基坐标系相对于相机坐标系的齐次矩阵的，H H eb，b x是特征点在末端执行器坐标系中的三维位置参数。

通过对式(4.1)微分，有视觉变化与关节速度的关系：

s m (t)

其中矩阵A(s m (t), q(t)) 2n是与深度无关的相互作用矩阵，形式为：

c2 v(t)c3 q(t)

其中q(t) n 1表示机器人的关节角度，n是自由度。

约束视觉伺服大多是采用传统的图像雅可比矩阵，但是传统的图像雅可比矩

阵依赖于特征点的二维图像坐标、摄像机内部参数和图像中未实际测量的深度参数。在以前的基于传统图像雅可比矩阵的预测模型中，深度呈现非线性。

这里，基于深度无关交互矩阵的预测模型中的深度可以由未知的摄像机参数

和特征点的三维笛卡尔坐标线性表示。深度c Z (t)可以由未知参数表示如下：

c

基于预测控制的控制律设计应采用离散时间模型，而不是连续时间模型(4.2)。

每个特征点的离散状态空间模型可写为：

x(k 1) Fx(k ) G(k )u c (k )

其中F是2 2常数矩阵，G(k )是时变变量和未知常数参数的函数：

G(k )

时变变量是特征点和机器人关节的二维图像坐标，且两者都可以测量。未知

常数参数包括摄像机内部参数、摄像机外部参数和特征点的三维位置参数。T e为

采样周期。基于离散化的深度无关交互矩阵的预测模型用于设计预测控制律。在

(4.5)中，s m可作为系统状态和输出，k为当前采样时间，u c (k ) n 1表示控制输入，即关节速度q(k )。

给定静态或动态的期望轨迹s d。当s d为静态时，视觉伺服定位任务是将特征

点从图像平面上的初始位置带到期望位置。当s d为随时间变化的动态轨迹时，视

觉伺服跟踪任务是使特征点在图像平面上跟踪期望的轨迹。

Z (t ) c 3T H s m (k ) x (k

)

模型预测控制与在线参数估计

为了实现有约束和无标定环境下的视觉伺服控制任务，需要确定控制输入， 并在线更新预测模型的未知参数。此外，MPC 算法是在自适应控制的基础上发 展起来的，能够保持自适应控制利用过去的输入输出辨识系统模型的特点。根据 控制论中的分离原理，分别计算控制输入和模型参数，更新系统模型，使系统状 态更接近实际。在本节中，控制由 MPC 确定，并从模型参数的初始估计开始。 模型参数的估计值是在线更新的。视觉伺服系统主要由最优控制模块和参数估计 模块两部分组成。最优控制模块为机器人系统生成控制输入 u c ，其将当前图像特 征 s m 引导到期望的特征 s d 。在参数估计模块中，基于特征 s m 的二维图像坐标和

关节角度的测量值来更新模型参数 。机器人视觉系统的控制方案如图 4.1 所示。

s m m (k ) F i 1 G (k m i )u (k m i ) (k m ) F s U c (k )

{U c c max }

,U

视觉伺服

约束

s d (k )

s m (k )

有约束无标定 IBVS 的模型预测控制

通过使用 MPC ，可以通过如下步骤实现控制：

(1) 采用模型对 IBVS 系统的未来行为进行预测。根据预测模型(4.5)，图像

状态的预测可以根据最后 m (m

N p ) 个状态和控制输入来定义：

m

其中 N p 是预测范围。该动态预测模型在每个采样时刻以有限的 N p 步预测特征点

s m 。用 N c 表示控制水平，它表示控制输入的自由度数目[79]。此外，N c N p 减少

了最优问题的自由度数目，提高了在线最优控制问题数值解的计算速度。从 u (k

N c

1) 到 u (k

N p

1) ，控制输入等于 u (k N c ) [80]。更新模型参数将在下一

小节中提及。预测模型在每个采样时间 k 在线更新。

(2) 求解有限时间开环约束最优控制问题。优化的目的是计算最优控制信号， 使图像特征达到期望的轨迹。同时，满足了系统的约束条件。IBVS 系统约束包 括机器人约束和图像约束。机器人约束主要包含机器人的物理约束，如关节边界、

关节速度饱和或关节加速度限制等，可视为系统的输入约束：

min

此外，还将工作空间限制和深度范围视为与机器人关节测量相关的机器人约 束。

q(k ) {q min , q max }

图像约束是将特征保持在视场中或在图像平面中设置范围的可见性约束。可 见性约束可以看作是系统的输出约束：

s m (k ) {s m m max }

, s min J (

(k ), U c (k ))

U c (k ) [

u c (k ) , ,

u c (k

N p

1) ]T

J ( (k ),

U c (k ))

(s d (k i ) s m (k i ))T Q (s d (k i ) s m (k i ))

(k i 1) R u

( u c c (k i 1))

min

通过考虑 IBVS 系统的输入和输出约束的 MPC 策略可以写成：

U c

( k ) 其中， U c (k ) 表示控制输入变化的最优序列，为：

动态预测模型为：

s m (k 1) s m (k )

(k

i ) s d (k i ) s m (k i )

u c (k )

u c (k

1)

u c (k )

其中， s m 是可以测量的模型状态和模型输出， s d 是期望状态， (k ) 是在时间

i [k

1, k

N p ]时期望状态和预测状态之间的差。

关节速度约束：

q (k )

{q min , q max }

关节速度约束的变化，可以有效地抑制关节速度的突变：

q

{

q min ,

q max }

式子(4.16)和(4.17)都是 IBVS 系统的输入约束。

s m (k )

{s m min , s m max }

式子(4.18)表示可见性约束，它是 IBVS 系统的输出约束。输入和输出约束 可以表示为控制输入

U c 的变化的最优序列的函数。

要最小化的二次成本函数包括预测误差和控制输入变化的顺序，其描述为：

N p

i

1

N c i

1

其中第一项最小化有限预测范围 N p 上的预测状态 s m 和期望状态 s d 之间的误差。

第二项最小化有限控制范围 N c 上控制输入的变化。 Q 和 二项的对称加权矩阵。

对于在线约束优化问题，需要采用一种有效的算法进行求解。利用序列二次 规划(SQP)算法计算约束条件下的可行解，得到 N c 上控制输入

U c 变化的最优序

Z (t)

T

H

Z (t)

c i 3 b x 1, c i1 b x 2 , c i 2 b x 2 , c i 3 b x 2 , c i1 b x 3, c i 2 b x 3, c i 3 b x 3, c i1, c i 2 , c i 3 )T ， i

(

iT

, c i 4 ) (i 1, 2,3) 。

1

列。

(3) 获取每个采样时间的控制输入。由于模型的不确定性和摄像机标定误差、 图像噪声、未知效应等干扰，最优控制序列 U c 中的所有元素都不能作用于动态 系统。实际上，(4.12)中的最优控制序列

U c 的第一个元素是

u c (k ) ， u c (k ) 被

定义为当前时刻 k 的控制输入的最优变化，在下一个采样时刻，水平向前移动一 步，并且在更新输出度量和未知动态效应之后，整个优化过程重新开始[80]。与最 优控制的全局优化策略不同，MPC 算法采用滚动时域策略，对工业实际中的模 型失配和扰动进行补偿是实用和有效的。

此外，在(4.19)中，未来参考输入 s d (k

i) 是

(k

i)

[k

1; k

N p ] 时刻期望

的特征， s m (k ) 是当前时刻的可测量状态，这分别作用于预测控制律的前馈补偿 和反馈补偿。因此，可以使用 MPC 来控制系统以实现高效的性能。

4.3.2 参数估计

预测模型(4.5)是参数未知的不确定线性变参数(LPV)模型。预测模型可以通 过基于机器人关节测量和图像特征的系统辨识算法进行辨识。MPC 选择控制输 入，以便系统根据期望的运动渐近地运行。由于假设了特定的预测模型结构，因 此系统辨识问题被简化为参数估计问题[81]。

预测模型(4.5)中的未知参数集是透视投影模型(4.1)中的未知参数子集。然 后，可以基于机器人关节和图像特征的测量值来估计透视投影模型(4.1)中的未

知参数，用于识别预测模型(4.5)。透视投影模型可以重写为：

c

如(4.4)所述，深度 c Z (t) 可以由未知参数重写为线性形式：

c

其中

(r 11, r 21, r 31, r 12 , r 22 , r 32 , r 13, r 23, r 33, p 1, p 2 , p 3 )T ，

(

T

,1)T ，

i

(c i1 b x 1, c i 2 b x 1,

T

r ij (i 1,2,3, j 1,2,3) 表示旋转矩阵

的第 h 个元素。 c ij 表示 C 的 (i, j) 元素， b x i 是向量 b x 的第 i 个元素。则(4.20)中的 常数未知参数个数为 39 个，分别是(4.20)左右侧未知摄像机参数和特征位置参数 的 13 个和 26 个独立乘积。

分析了过程为参数线性估计的情况下的参数估计问题。也就是说，可以如下 重写模型(4.20)：

Y (k )

T

(k )

其中 (k )

l 2

是矩阵，Y (k )

21

是向量。(k ) 和 Y (k ) 都依赖于可测量的机器

(k )P (k 1)(k )

P (k 1)

T 0113

u (k ) T

01 v (k )

人关节 q (k ) 和可测量的图像特征 s m (k ) 。

l

1

是一个未知的参数向量，它依赖

于摄像机参数和特征位置参数的乘积。

利用辨识算法对(4.20)中的未知参数进行估计，需要保证 39 个未知参数中有 一个非零参数，并选择非零参数重新定义剩余的 38 个未知参数。末端效应器坐 标系和摄影机坐标系之间沿 z 轴的平移向量是 C 的(3,4)元素，通常 c 34 0 。未知 参数的数目 l 满足条件 l 38 。当 l 38 时，设

(

1T

,

2T

,

3T

)T ，选择非零参数 c 34

重新定义剩余的 38 个未知参数 ，记为 c 34 ，Y (k ) [u (k ), v (k )]T 。可以得 到：

(k )

T T

如果不存在零元素 r ij (i

1,2,3, j 1,2,3) 和 p h (h

1,2,3) ，则 l 等于 38。如果

r ij

0 ，则可测量矩阵

(k ) 可以通过从

T

和 T 中去掉 r ij 而得到。如果 p h 0 ，则

可测量矩阵

(k ) 可以通过从

T

和

T 中去掉 p h 而得到，从 中去除 c ei (e 1,2,3)

可以得到 。如果元素 r ij (i 1,2,3

j 1,2,3) 或 p h (h 1,2,3) 彼此成线性比例，则可

以通过将依赖于 r ij 或 p h 的线性比例列组合为一列来获得可测量矩阵 。因此，

中要估计的参数个数满足 l

38 。

可以采用多种参数估计方法来解决参数估计问题。由于参数要在线更新，所 以使用递归方法。采用递归最小二乘算法更新参数 。可以通过最小化时间 N 处

的误差函数来确定参数：

N

k 0

其中，遗忘因子

在范围 (0,1] 内。通常， 在范围 [0.95,1] 中选择，则赋

予最新数据更多权重[82]。 的估计值更新如下：

其中， L (k ) 的值为：

其中， P (k ) 的值为：

P (k )

其中 L (k ) 是权重因子 矩阵 ， P (k ) 是正定 协方差 矩阵 。一般地，

?(0)=0l

1

，

P (0)

2

I l l ，其中 是一个足够大的正数，I n n 是一个单位矩阵。在每个采样时

刻，需要得到依赖于可测量机器人关节和图像特征的可测量矩阵

(k ) 。然后，

加权因子矩阵L(k )、估计 ?(k )和协方差矩阵P(k )可以分别由(4.26)、(4.25)和(4.27) 获得。预测控制从模型参数 ?(0)的初始估计开始，并基于图像特征和机器人关节

的测量值来更新模型参数。

稳定性分析

本节分析了在所提出的方法控制下的机器人视觉系统的稳定性。

定理1：考虑视觉伺服系统(4.5)服从系统的输入输出约束(4.16)-(4.18)，如果

应用控制器(4.11)和参数估计算法(4.25)-(4.27)，则系统输出{s m (k )}和输入{u c (k )}

有界，特征点的图像误差收敛到零：

lim[s m (k ) s d (k )] 0

k

证明：参数估计模型(4.22)可以表示为：

s m (k ) T (k )

其中s m (k ) Y (k )。将图像误差定义为：

e(k ) s m (k ) s d (k ) T (k ) s d (k )

在成本函数收敛到零之前， u c (k )满足(4.29)。如果成本函数收敛到零，则

u c (k )满足：

T (k ) s d (k )

显然，跟踪误差等于零。如果 u c (k )进入饱和状态，则(4.29)有效。由于 u c (k ) 存在饱和，当系统参数发生较大变化时，控制输入不会发生较大变化。此外，还

保证了输出{s m (k )}和输入{u c (k )}的有界性。

由于 未知，我们用以下公式替换(4.31)：

T (k )? s d (k )

那么(4.31)可以重写为：

Ti (k )? s di (k ) 1 i 2

其中 Ti (k )和s di (k )分别是可测量矩阵 T (k )和期望输出s d (k )的第i行。

由于元素r ij (i 1,2,3, j 1,2,3)与p h (h 1,2,3)是有界的，可以得到：

1(k ) m1 m2 u(k )

2 (k ) m1 m2 v(k )

其中0 m1 ，0 m2 。假设期望轨迹是先验已知的，并且在所有时间k中s d (k ) m3 ，可以得到：

e i (k )

lim[e i (k )] 0

lim[s m (k ) s d (k )] 0

s m (k )

max 964

min 0

e i (k ) s mi (k ) s di (k ) s mi (k ) m3

其中e i (k )和s mi (k )分别是e(k )和s m (k )的第i 行。显然，s m1(k ) u(k )，s m2 (k ) v(k )。

因此，建立了线性有界性条件：

1(k ) m1 m2 ( e1(k ) m 3 )

M 1 M 2 e1(k )

2 (k ) m1 m 2 ( e2 (k ) m

3 )

M 1 M 2 e2 (k )

式中0 M 1 ，0 M 2 。从[83]中，有：

lim

k

其中 max [P(k 1)]是P(k 1)的最大特征值。建立了一致有界性条件：

0 b1(k ) 1

0 b2 (k ) 2max [P (k 1)]

考虑(4.39)，在一致有界条件(4.40)，(4.41)和线性有界条件(4.37)，(4.38)下，

有：

k

并且 i (k )是有界的[83]。假设在图像误差收敛到零之后，控制输入u c (k )、

控制输入 u c (k )的变化和系统输出s m (k )不会进入饱和，则(4.31)是有效的。从

(4.42)中，可以得出结论：

k

仿真结果

在本节，为了验证所提出的方法，仿真基于2自由度平面机器人，分别针对

眼在手上和眼在手外摄像机构型来执行视觉定位和跟踪任务。

第一和第二机械臂的长度分别为0.12m和0.10m，摄像机内参数如表4.1所

示，图像分辨率为1292×964。为了将特征点保留在视场中，可见性约束定义为

以下不等式：

u min 0 u max 1292

H ? ec

0 1 0 0.03 0

0.02

H e

摄像机内部参数

Table4.1 Camera internal parameters methods

参数 焦距(m)

u 轴图像平面偏移(pixel) v 轴图像平面偏移(pixel) 图像 u 轴中的比例因子(pixel/m) 图像 v 轴中的比例因子(pixel/m)

关节速度的最大控制输入为 0.25rad/s ，关节速度的最大变化为 0.05rad/s 。由 于摄像机以 25 帧/秒的帧速率拍摄图像，控制器的采样时间为 40ms ，采用 SQP 优化算法（MATLAB 优化工具箱中的 fmincon 函数）求解在线优化问题。为了减 少计算时间，通过分别设置 N p 和 N c 为 3 和 2 对快速响应和精度之间进行了权 衡。初始协方差矩阵为 P(0) 1010 I ，遗忘因子选择为

0.995 。摄像机内参数

的初始估计为 u ?0

500 pixel ，v ?0 500 pixel ，

u

250000 pixel ，v

250000 pixel 。

为了本章方法，采用经典的比例 IBVS [84]、视觉预测控制方法[85]和本章提出 的方法进行对比来完成视觉定位和跟踪任务。对于眼在手上执行眼在手上仿真， 对于眼在手外执行眼在手外仿真。

眼在手上仿真

在眼在手上构型中，机器人末端执行器相对于摄像机的变换矩阵为：

c 摄像机外部参数的初始粗略估计为：

1 0 0.3 0.01

0 0 0.95 0.01

0 0 0 1

在第一个仿真中，控制四个特征点来完成视觉定位任务。假设初始的 4 个特 征点相对于机器人基坐标系的坐标分别为 (0.01,0.01,0.02)T ， (0.01,0.01,0.01)T ，

(0.01,0.01,0.01)T ， (0,0,0)T 法 。 机 器 人 的 初 始 关 节 和 期 望 关 节 分 别 设 置 为 q 0

[

12

12]T 和 q d

[ 3 6]T ， 加 权 矩 阵 为 Q 0.0001 I 8

8

和

R 0.001 I 22

。三种方法的比较结果如图 4.2-4.4 所示。

(a)图像轨迹

(b)位置误差

(a)图像轨迹

(a)图像轨迹

在第二个仿真中，执行视觉跟踪任务。控制一个特征点的投影来跟踪期

望的轨迹。特征点相对于机器人基坐标系的初始估计坐标是

(0.01,0.01,0.02)T。机器人机械手的初始关节被设置为q(0) [6 12]T。特征点s d在图像平面上的期望轨迹为椭圆轨迹：

s d (t) (800+40cos(0.4t),300 25sin(0.4t))T，加权矩阵为Q 0.0001 I 22和R 0.0001 I 22。三种方法的比较结果如图4.5-4.7所示。

(a)图像轨迹

(b)位置误差

图4.5经典比例控制器的仿真结果

Fig.4.5 Simulation results of classical proportional chapter

(a)图像轨迹

(b)位置误差

(a)图像轨迹

(b)位置误差

图4.7本章控制器的仿真结果

Fig.4.7 Simulation results of the controller proposed in this chapter

眼在手外仿真

眼在手外摄像机构型中，机器人基坐标系相对于摄像机坐标系的变换矩阵为：

c 0 0.05

H b

0 0.98 0 0.01 H ? bc

摄像机外部参数的初始估计为：

1

0.2 0.3 0.01

0 0 0.95 0.01

0 0 0 1

在第一个仿真中，和眼在手上构型类似，首先执行视觉定位任务。控制器用 于将四个特征点调整到所需位置。4 个特征点相对于末端效应器坐标系的初始估 计 坐 标 分 别 为 (0.01,

0.01,0.01)T ， (0.3,

0.1,0.042)T ， (0.01,0.003,0)T ，

(0.049,0,0.1)T ， 机 器 人 机 械 手 的 初 始 关 节 和 期 望 关 节 分 别 设 置 为

q 0

[

3

6]T 和 q d

[

12

12]T ， 加 权 矩 阵 为 Q

0.0001

I 8

8

× 8 和

R 0.001 I 2

2

。三种方法的比较结果如图 4.8-4.10 所示。

(a) 图像轨迹

(b)位置误差

图4.8经典比例控制器的仿真结果

Fig.4.8 Simulation results of classical proportional controller

(a)图像轨迹

(b)位置误差

图4.9视觉预测控制器的仿真结果

Fig.4.9 Simulation results of visual predictive controller

(a)图像轨迹

在第二个仿真中，同样地与4.5.1类似，执行视觉跟踪任务。控制一个

特征点的投影来跟踪期望的轨迹。特征点相对于机器人基坐标系的初始估计

坐标是(0.01,0.025,0.02)T。为了将眼在手外和眼在手上相比较，特征点的初始位置和期望轨迹与眼在手上的视觉跟踪任务中设置的位置和期望轨迹相

同。然后将机械手的初始关节设为q0 [57100 2950]T，加权矩阵为

Q 0.0001 I 22和R 0.001I 22。三种方法的比较结果如图所示。

(a)图像轨迹