2024年5月8日发(作者：万俟心远)
小型微型计算机系统
Journal
of
Chinese
Computer
Systems
2021年1月第1期
Vol
.42
No
. 1 2021
采用内点法和图像反馈的机器人视觉伺服预测控制
滕游，刘安东，俞立
(浙江工业大学信息工程学院，杭州310023)
：
***********.cn
摘要：针对具有控制量约束和可视性约束的六自由度机器人的视觉伺服系统，研究了基于图像空间局部模型的预测控制器
的设计问题.首先对特征点的投影图像的运动学方程进行离散化，得到系统的误差预测模型.然后通过选取合适的性能指标函
数，将视觉伺服控制器设计问题转化为一个具有控制量约束和可视性约束的最优化问题.进一步，利用对数障碍函数处理约束，
得到系统的牛顿方程，获得控制量的迭代求解公式.最后，利用数值仿真和实验验证了所提方法的有效性.
关键词：视觉伺服;机器人;模型预测控制；内点法
中图分类号：
TP
391 文献标识码
：A
文章编号：1000-1220(2021)0丨•0丨9645
IBVS Predictive Control for Robot Systems with Interior Point Method
TENG
You,LIU
An
-
dong,YU
Li
(College of Information Engineering,Zhejiang University of Technology .Hanzhou )
Abstract
：
This
paper
deals
with
the
design
of
a
predictive
controller
of
a
6
-DOF
robot
based
on
a
local
model
in
image
space
,
subject
to
input
constraints
and
visibility
constraints
.
First,in
order
to
obtain
a
systematic
error
prediction
model,the
kinematics
equations
of
the
feature
points
of
the
projected
image
are
discretized
.
Then
,
a
suitable
cost
function
is
selected
,
and
the
design
of
visual
servo
con
troller
is
transformed
into
a
nonlinear
optimization
problem
,
in
which
the
input
constraints
and
visibility
constraints
on
the
system
are
considered
.
Furthermore
,
these
constraints
are
processed
by
using
the
logarithmic
barrier
function
.
The
Newton
^
equation
of
the
system
is
obtained
and
solved
for
the
iterative
equations
of
control
inputs
.
Finally,the
effectiveness
of
the
proposed
method
is
verified
by
nu
merical
simulation
and
experiments
.
Key
words
：
visual
servo
；
robot
；
model
predictive
control
；
interior
point
method
i引言
视觉传感器提供了丰富的外部环境测量信息.视觉伺服
会增加系统不稳定以及特征从图像有效区域丢失的可能
性[51.特征丢失是视觉伺服的普遍问题，对于直接处理图像
特征误差的
IBVS
，将特征点始终维持在视野内至关重要
文献[7]和文献[8]利用导航势函数避免特征被障碍物遮挡.
文献[9]为摄像机生成了类似圆弧的轨迹，以避免大位移的
视觉伺服过程中的特征丢失.但是这些方法的通用性不强，难
以应用到各种复杂的不同应用中.从最优化的角度来说，维持
特征不丟失是一种系统约束.模型预测控制是一种非线性最
优控制策略,特别适合处理控制中存在的各种约束问题.近年
来，采用模型预测控制的处理方法解决
IBVS
控制问题得到
了广泛的关注[1° "]，但是大部分文献直接利用
MATLAB
中
的内置优化函数解决模型预测控制中的优化问题，不利于在
实际工程中实现.针对移动机器人平台，文献[11]和文献
[12]采用神经网络解决
QP
问题，但是存在着神经网络的训
练等问题.针对上述问题，本文采用模型预测控制处理方法，
将控制量的饱和、特征在图像空间的坐标限制作为约束，把
IBVS
转化成一个有限时域内带约束的优化问题，通过障碍函
数法把不等式约束转化成等式约束，给出了采用
LOQO
内点
法[
M
]计算最优控制量的算法.最后通过仿真和
Panda
机器人
上的实验验证了算法的有效性.
是一种利用视觉测量数据作为反馈信号的机器人运动控制器
的设计技术，在工业机器人、无人飞行器、智能机器人等领域
得到了广泛的应用1M].
根据选取的误差信号，视觉伺服的控制策略可以分为3
类:基于位置的视觉伺服（
PBVS
)、基于图像的视觉伺服（
IB
-
VS
)和混合视觉伺服[4].
PBVS
利用视觉处理算法获得的相
机和物体之间的相对位姿定义系统误差，使得机器人的运动
具有较好的三维轨迹，但容易受到系统标定误差、视觉测量噪
声的影响.
IBVS
直接采用目标物体特征分别在期望图像平面
和当前图像平面上的投影来定义系统误差，对相机参数和测
量误差具有较好的鲁棒性，且算法运算较少，吸引了很多机器
人领域的研究者的关注.混合视觉伺服同时利用相对位姿和
特征的图像投影定义误差，同样容易受到模型和测量误差的
影响.
传统的
IBVS
方法利用固定增益和雅可比矩阵的伪逆设
计控制律，只能保证局部稳定性,且增益的选取会对系统性能
产生重要影响，比如增益太小会影响系统收敛速度，增益过大
收稿日期:202(
M
)
I
>09收修改稿日期:202(
M
)2-20基金项目：NSFC-浙江两化融合联合基金项目（
U
1709213)资助.作者简介：滕游,
男，1985年生，硕士，助理研究员，研究方向为图像处理、机器视觉、机器人控制；刘安东，男，1985年生，博士，讲师，研究方向为机器人控制、预测
控制；俞立，男，1961年生，博士，教授,研究方向为运动控制、控制理论.
期滕游等:采用内点法和图像反馈的机器人视觉伺服预测控制
197
2问题描述
本文采用一个六自由度的协作机器人
Franka
Panda
和一
个
RGB
-
D
相机
SR
300,将
RGB
-
D
相机安装在协作机器人末
端夹具上，通过
RGB
-
D
相机获得当前待抓取物体的图像，驱
动机器人运动，使得物体的图像最终和期望的图像一致.通过
将协作机器人拖动到期望位姿进行测量，或者利用物体的
CAD
三维模型计算得到期望图像.
考虑上述系统在
eye
-
in
-
hand
框架下的
IBVS
控制问题如
图1所示.其中，相机安装在协作机器人末端，目标物体局部
坐标系为,当前摄像机坐标系为，期望的摄像机坐标
系为
IDI
.
图1基于图像的机器人控制
Fig
. 1
Image-based
robot
control
为了实现通过像素控制机器人运动，首先假设摄像机已
经完成标定且满足针孔投影模型，从而可以获得摄像机的内
部参数矩阵.根据得到的特征点的像素坐标，利用摄像机的内
部参数矩阵可以计算特征点的归一化图像平面坐标
x
= (
JC
,
其中，
X
和对应的特征点
p
在摄像机坐标系中的坐标
X
= (
x
,
y
,
z
，的关系如下:
x
=
X
/
Z,y
=
Y/Z
(1)
令
P
在期望图像平面上的坐标为
X
’
==(，，，）'则可以定
义图像平面上的系统偏差为
e
=
x
-
x
'
由于摄像机安装在运动的机器人末端上,摄像机坐标系
中的特征点坐标与机器人末端的速度紧密相关.根据机器人
末端的运动关系,特征点在摄像机坐标系中的坐标和摄像机
的运动速度之间的关系如下所示：
_
v-wxX
(2)
其中,
V
和
W
分别为摄像机自身的平动速度和转动速度.对式
(1)取微分，并代人式（2),可以得到归一化图像特征点与摄
像机运动速度之间的运动学方程：
x
=
L
(
x)u
(3)
其中
,u
= (
v
7',
w
7')7'，
-(1+
Jc
2) >
L
(
x
)=
0
f
^
〇 -+
f
(
l
”2)
-xy
IBVS
是像素层面的控制，虽然能够驱动单个特征点到达
期望状态，但无法保证摄像机具有唯一确定的方位.通常情况
下使用4个及以上不共线的特征点构造系统误差，以保证当
系统误差为零时，摄像机具有唯一确定的期望位姿.假设设置
的有效特征点个数为~则可将式(3)中的
x
和
L
重新定义为
x
= (
x
,(
L
【，-”,
L
；：)
T
•其中
,L
通常被称
为雅可比矩阵或者交互矩阵.
由式（3)可知不同的控制量
II
可以产生不同的系统状态
轨迹.控制律设计的目标即利用特征点的图像误差产生合适
的控制序列,使得摄像机到达合适的位姿.由于摄像机安装在
机器人末端上，摄像机的运动是通过机器人的关节电机驱动
的，电机具有最大转速的限制，从而使得控制量中的线速度和
角速度需要满足电机转速约束，即式（3)中控制输人
u
的约
束问题.另一方面，在运动过程中必须保持特征点始终位于摄
像机的视野内，因此在设计视觉伺服控制器的过程中有必要
考虑摄像机视野约束问题，即式(3)中的状态
X
的约束问题.
本文的目的是针对具有摄像机视野约束和控制输人约束
的
IBVS
控制问题，利用目标在当前图像平面和期望图像平
面上的特征偏差，采用
MPC
方法设计视觉伺服控制器，驱动
摄像机到达期望的位姿.
3基于预测控制的机器人视觉伺服控制器设计
MPC
是一种处理约束控制问题的有效方法.在
MPC
框
架中,控制问题通常被转化为一个带约束的最小化问题.因
此，本节针对具有视野约束和控制量约束的视觉伺服问题，设
计基于
MPC
的
IBVS
控制器，并采用
LOQO
内点法求解控制
输入，实现点镇定控制.
3.1
模型预测控制
为了获得预测模型，首先以采样周期
T
将目标特征点的式
(3)离散化，表示成如下的离散时间状态空间模型：
为了驱动机器人到达期望目标位姿，采用图像平面上的偏差
量
e
作为状态量，从而可进一步写出有限时域内的预测误差
和输人之间的关系为：
ek*i
= +
t/_i
ut
+
y
_|
T,j
= 1,2 ,•••
,N
(5)
另外，在实际物4系统中，机器人运动都是通过电机驱动
的，电机具有最大转速的限制，从而使得摄像机的线速度和角
速度需要满足最大转速的限制.由式（3)可知，线速度和角速
度约束即为控制量约束，因此控制量需要满足约束条件：
u—
〈化
+
卜
,<11
順
，_/ = 1
，
2,…，W (6)
其中11_和111„„分别为相机速度的最小值和最大值,通常具
有关系.更重要的是，为了保证可视性约束，系统
状态必须满足约束条件：
X— 腿， y=0 ， l,… ， W-1 (7) 其中和*_分别为归一化图像平面坐标的最小值和最大 值.本文考虑镇定问题，使用有限时域内的状态偏差和控制量 以及终端惩罚函数构造代价函数，从而将镇定问题转化成如 下的最小化问题： B (*),… m , in J (8) s . t . (5)、(6)、(7)和•/ = ……+ e【+(V ^ et*N 其中, G 和弋表示权重矩阵,为终端惩罚项.因此， IBVS 系统的控制器设计问题转化为一个带约束的二次优化 问题. 为了计算出 W 个时域的最优控制序列，重新定义以下向量： e 〇(*) =[ e [,-, e[]r 198 小型微型计算机系统 2021 年 e ( k ) = [ e[+l ,--*, e[+w]r 5⑷=[ u * r ，.", uI+AM]r 从而，可以将公式(7)表示成： e ( k ) =~ e 0( k ) ^ Gu { k)T (9) 其中， Lk 0 G L “1 ■ + I Lk + N-l - 进一步，系统性能指标函数可改写为： J = eTQe + ut R u + eTk + NFek,N =( e 〇 + GuT ) t Q ( e 〇 + GuT ) + uTRu + eTk + NFek+N = e 〇 rGe 〇 ^ eT 0 QGuT ^ iTGTQe0T + uTGTQGnt + ut R u + eTk + NFek+N (10) 其中，(2和/?分别为由 G ;和义组成的权重矩阵.式（ l 〇)中的 最后一项是终端惩罚项，将其展开后可得： e* + ^e* + = ( e* + I S 1 + U* + i-l + I s 1 + Uik + i-l = Is I S ^ ik +<-| Ujk + i-l T ') +( Is I X ^* + i-|UJk + i-l Fe *+( j | Li * i .1 utt (.1 r ) rF ( j |£, t + i .1 utti . lr )( ll ) 通过上述转化，最小化问题（8)转^成了一个关于控制 量 u ( Jt )的带约束的二次优化问题： m i ( m *) f _ ( u ( k )) s . t . h( u ( k)) ^0 ( 12 ) 其中 j ( a ( k )) =•/, -•*(*) u (*) - umin LtutT h (^ k )) a - xt (13) 乙 *utr n _LL“N_iuttN_lT. 3.2内点法 公式（12)中的不等式约束的数量和特征点的数量与系 统的预测时域有关.针对单个时域，关于控制输入的约束不等 式有12个，关于可视性的不等式约束数量为特征点数量的4 倍.令 yv 个时域的所有约束数量为 A #,采用障碍函数法将所 有约束转化成等式约束，优化问题转化成： s . t . / i ( u t) -〇> = 0 (14) 其中 u 4为11(幻的简化表示,《〇和M分别为引入的惩罚项和 惩罚因子.为了求解上述优化问题,采用拉格朗日乘子法，得 到下面的拉格朗日方程： L ( ut ,(〇, ytAi ) =/( u t) -^ I 1〇 g ( w ,) - yr ( A ( u t) - c 〇) (15) 其中 y 为引入的拉格朗日乘子.由最优性原理可知，上述拉格 朗日方程的最优值满足一阶 KKT 条件： Vf(ut) -Vh(ut)Ty=0 -/ie + WYe = 0 / i ( u *) - a ) =0 (16) 其中 ,W = dfaS ( o 〇 ,F = d /邓(兄）. 进一步令式（15)的 Hessian 矩阵和式（13)的 Jacobian 矩 阵为： ff(5t,y)=V2/(S1)-i 1 = v 1 jV2A((it) A ( S *) =▽/!(!〇 (17) 从而得到式（16)对应的牛顿方程为： - ~ H ( ukyy ) 0 A ( u k)T- _ Au ， 0- W~lY - / A 〇) - A ( i 〇 -I 0 - -Ay - - v /( i t) - A(ijV a - fiW~le + y = -y (18) - (0 -/ i ( u A) ■ p 其中〇■〇»和 P 为引人的中间变量，▽/(54)、扒，>〇和/1(5*) 的具体表达式为： V /( u t) =2 TGTQe 0 +2 TtGTQGut +2 Rut + ■ L Ftk - LlFLkuk 27+ 2 T 1(19) -^ k * N - l ^ ek ~ - Lk + N _' FL “ N _' i “N_'- ~^6 N A ( u *) = V / i ( u t)= hN - diag ( Lk — Lk + N_、、 ( 20 ) - diag ( LfL “N_l') ■ -~ H = 2 tGTQG ^2 R ^2 t i (21) - L “ n - FL “N-- 进而采用消元法可得式（18)的解为： Au t= - N -( T - ATy - ATm - xp ) ^ y = y - YW-l^ia (22) Aco = -p + AAu * 其中， Mu*,y,« 〇 ) =H(ut,y) + A ( u 4) ryW ' A ( u t) 由于/ V ( it , y , w )是非奇异的，所以，式（18)的解（22)是 唯一的.从而可以利用式（22),采用迭代策略获得优化问题 (14)的解： i «+l) = i tu) + au ) AS t0) 〇,»*'> =«〇«> + a (;, Aw u) y 0»» = yU ) +〇( U ) Ay «) 其中, aw >为迭代步长. 综上所述，将 LOQO 内点法解决 IBVS 的优化计算过程 总结如下： 2) 计算/!,+ (〇, ▽/(5 t ), A /， A ( i 〇; 3) 根据公式（18)计算〇■, - ip ; 4) 计算 W ( i ,， y ， w ); 5) 根据公式(22)计算 ASt , Ay , Ato ; i 期 6) 7) 计算 滕游等:采用内点法和图像反馈的机器人视觉伺服预测控制 199 所示.从图中可以看出，为传统 IBVS 控制器设置合适的控制 如果满足要求，则得到最优值，否则跳转到步骤2). 器增益系数后，控制器的输出能较长时间维持在最大值，但是 上述优化算法获得的是一组控制序列 SU )，将控制序列 中的第1组值作为当前时刻的机器人控制量.按照滚动优化 的原则，当新的控制量作用到机器人系统后，可以获得下一个 时刻的图像测量值，再次应用上述优化过程，计算控制量，从 而实现整个系统的实时反馈控制. 4实验 本节通过数值仿真和机器人实验对提出的方法进行验 证.仿真周期 r 设定为40 ms ,采用一个边长为50 cm 的正方 形的4个顶点作为特征点，实验中，把物体坐标系的原点设置 在正方形中心, XY 平面设置在正方形所在平面.摄像机相对 于物体坐标系的真实期望位姿为(〇,〇, -0.5,0,0,0),其中前 3个参数为摄像机坐标系相对于目标物体坐标系的位置，后3 个参数为摄像机坐标系相对于目标物体坐标系的方位角.在 期望状态下,4个特征点的归一化参考图像坐标为5^=(- 0. 5,0. 5,0.5 ,0. 5 ,0.5, —0.5, —0.5, —0.5). 在仿真中，平动速度限制在*25 cm / s 范围内，旋转速度 限制在±0.25 rad / s 范围内.对于本文采用 LOQO 内点法的 IBVS ,还需要考虑以下几个参数设置问题.首先，在初始化 时，令严为单位对角矩阵= max ( A ,(«!°>),其中 0.5.在计算过程中，令弘 =S ^ m ,5 =0. 5 ,aw =0. 95 :|’ = 1，2,3,4》一，当某次迭代计算出 的控制量和上一次迭代计算出的控制量之间的偏差足够小 时，结束当前迭代过程，将计算结果中的第1组控制量作为当 前时刻的控制输人. 本文比较了两种不同的初始位姿情况下，传统 IBVS 和 本文提出的方法的实验结果.第1种情况下，初始位姿和期望 位姿之间的偏差为绕 z 轴旋转180°,传统的 IBVS 会导致摄 像机沿着 z 轴后退，最终停留在一个导致雅可比矩阵失效的 状态，无法到达期望状态.采用本文的方法虽然也会产生后退 情况，但在运行过程中会产生旋转运动,且会改变摄像机平动 方向，使得摄像机沿着 z 轴前进，最终可以到达期望状态 .传 统 IBVS 控制器和本文提出的控制器的控制效果分别如图2 ( a )和图2( b )所示. ( a ) IBVS 控制器 （ b ) LOQO 预测控制 的实验结果 器的实验结果 图2第1种初始位姿下控制器运行结果 Fig . 2 Results of controllers under the first initial pose 第2种初始位姿为（1,1，-3，0,0,0. 6),传统 IBVS 控制器 产生的输出和本文提出的控制器的输出如图3( a )和图3( b ) 当接近目标状态时，控制量的输出会缓慢下降，收敛速度将变 得非常缓慢，在22. Is 时到达期望状态.而本文提出的控制器 的输出，在接近目标状态时，也可以维持在最大值，在13.9 s 时到达期望状态. 10 15 20 25 0 5 10 15 20 25 Time (s) Time (s) ( a ) IBVS 控制器的输出 ( s / p 0 . V, 0.2 b V ， 0.1 j ) 0 . v: ^_ — 3 ^ 〇 . 3 ^ -. 0 -. 13 0 a 0 2 4 6 8 10 1214 16 2 4 6 8 1012 14 16 Time (s) Time (s) ( b ) LOQO 预测控制器的输出 图3第2种初始位姿下控制器的输出 Fig . 3 Outputs of controllers under the second initial pose 使用六自由度的协作机器人 Franka Panda 和 RGB-D 相 机 SR 300对本文提出的方法进行了验证.将目标物体随意放 置在相机的视野内, IBVS 控制器和本文提出的控制器都能够 驱动相机到达期望姿态和位置，两种控制器作用下，特征点在 图像平面上的轨迹分别如图4( a )和图4( b )所示，控制器的 ( b ) LOQO 预测控制 的实验结果 器的实验结果 -3 0.2 — V. 0.1 ’、. 上 v, -.-V, P 。 | g g - -0.2 〇• 1 0 2 4 6 8 1012141618 Time (s) ( c ) IBVS 控制器的输出 uou (s/PBJ)XJPO13> 0 V . * v 0 U ea 0 . I S -. 0 y V 3 -C U B 0 J I l -. 0 I (s/PBi)XJPOP> 2 3 4 5 6 0 1 2 3 4 5 6 7 Time (s) Time (s) ( d ) LOQO 预测控制器的输出 图4在 Panda 机器人上运行的实验结果 Fig . 4 Results running on a Panda robot 输出速度分别如图4( c )和图4( d )所示，从图中可以看出，本 文提出的控制器的收敛速度更快.在实际应用中，本文提出的 200 小型微型计算机系统 2021 年 方法通过求解一个带约束的非线性优化过程获得当前时刻的 控制量，避免了传统 IBVS 方法中存在的特征丢失问题、局部 最小问题，并极大地改善了临近期望状态时的收敛速度，但是 算法的实现更加复杂，对计算能力的要求更高. 5结束语 本文将 IBVS 问题表示成一个有限时域模型预测控制问 题，采用非线性优化方法计算所需的控制量.显著优点是在计 算控制量的过程中可以考虑2 D 图像平面的视野约束和物理 执行部件输出的控制量约束.实验表明，和传统的 IBVS 方法 相比，本文提出的方法有效地考虑了可视性约束，避免特征从 图像平面的有效视野中消失，同时考虑了控制量约束,可以避 免产生过大的控制量，同时使得控制量尽可能长时间地维持 在最大值，解决了传统 IBVS 中存在的特征丢失和增益无法 调节的问题. References ： [1 ] Ma X M,Wang C L,Cao L. Visual servo-based control of mobile robots for opening doors[ M]. Wearable Sensors and Robots,Singa pore ：Springer ,2017. [2 ] Schenck C,Fox D. Visual closed-loop control for pouring liquids [CJ//IEEE International Conference on Robotics and Automation (ICRA) Singapore,2017 ： 2629-2636. [3 ] Xu De. A tutorial for monocular visual servoing[ J]. Acta Automati- ca Sinica,2018,44(10) ： 1729-1746. [4 ] Jia Bing-xiyLiu Shan,Zhang Kai-xiang,et al. Survey on robot visu al servo control ： vision system and control strategies[ J]. Acta Auto- matica Sinica,2015,41 (5) ： 861-873. [5 ] Malis E. Improving vision-based control using efficient second-order minimization techniques [ C ]//IEEE International Conference on Robotics and Automation (ICRA) ,New Orleans, US A, 2004 ： 1843- 1848. [6 ] Mezouar Y, Chaumette F. Path planning for robust image-based control[ J]. IEEE Transactions on Robotics and Automation,2002, 18(4) ： 534-549. [7 ] Cowan N J, Weingarten J D, Koditschek D E. Visual servoing via navigation functions[ J]. IEEE Transactions on Robotics,2002,18 (4) ： 521-533. [8 ] Chen J,Dawson D M,Dixon W E,et al. Navigation function-based visual servo control [J]. Automatica,2007,43(7) ： 1165-1177. [9 ] Chesi G, Vicino A. Visual servoing for large camera displacements [J]. IEEE TransacUons on Robotics,2004,20(4) ： 724-735. [10] Allibert G,Courtial E, Chaumette F. Predictive control for con strained image-based visual servoing[ J]. IEEE Transactions on Ro botics,2010,26(5) ： 933-939. [11 ] Heshmati-Alamdari s G K,Eqtami A,et al. Robustness a- nalysis of model predictive control for constrained image-based vis ual servoing[ C]//DBEE International Conference on Robotics and Automation (ICRA )，Hong Kong ， 2014:4469 4474. [12] Li Z, Yang C,Su C Y,et al. Vision-based model predictive control for steering of a nonholonomic mobile robot[ J]. IEEE Transactions on Control Systems Technology ,2016,24(2) ： 553-564. [13] Ke F,Li Z,Xiao H,et al. Visual servoing of constrained mobile ro bots based on model predictive control [ J ]. IEEE Transactions on Systems,Man,and Cybernetics ： Systems,2017,47(7) ： 1428-1438. [14] Vanderbei, Robert J. LOQO：an interior point code for quadratic programming [ J ]. Optimization Methods and Software ,1999,11(1- 4) .451484. 附中文参考文献： [3] 徐德 . 单目视觉伺服研究综述 [J]. 自动化学报 ,2018,44(10): 1729-1746. [4] 贾丙西，刘山，张凯祥，等 . 机器人视觉伺服研究进展：视觉系 统与控制策略 [J] • 自动化学报， 2015，41 (5) :861473.
2024年5月8日发(作者：万俟心远)
小型微型计算机系统
Journal
of
Chinese
Computer
Systems
2021年1月第1期
Vol
.42
No
. 1 2021
采用内点法和图像反馈的机器人视觉伺服预测控制
滕游，刘安东，俞立
(浙江工业大学信息工程学院，杭州310023)
：
***********.cn
摘要：针对具有控制量约束和可视性约束的六自由度机器人的视觉伺服系统，研究了基于图像空间局部模型的预测控制器
的设计问题.首先对特征点的投影图像的运动学方程进行离散化，得到系统的误差预测模型.然后通过选取合适的性能指标函
数，将视觉伺服控制器设计问题转化为一个具有控制量约束和可视性约束的最优化问题.进一步，利用对数障碍函数处理约束，
得到系统的牛顿方程，获得控制量的迭代求解公式.最后，利用数值仿真和实验验证了所提方法的有效性.
关键词：视觉伺服;机器人;模型预测控制；内点法
中图分类号：
TP
391 文献标识码
：A
文章编号：1000-1220(2021)0丨•0丨9645
IBVS Predictive Control for Robot Systems with Interior Point Method
TENG
You,LIU
An
-
dong,YU
Li
(College of Information Engineering,Zhejiang University of Technology .Hanzhou )
Abstract
：
This
paper
deals
with
the
design
of
a
predictive
controller
of
a
6
-DOF
robot
based
on
a
local
model
in
image
space
,
subject
to
input
constraints
and
visibility
constraints
.
First,in
order
to
obtain
a
systematic
error
prediction
model,the
kinematics
equations
of
the
feature
points
of
the
projected
image
are
discretized
.
Then
,
a
suitable
cost
function
is
selected
,
and
the
design
of
visual
servo
con
troller
is
transformed
into
a
nonlinear
optimization
problem
,
in
which
the
input
constraints
and
visibility
constraints
on
the
system
are
considered
.
Furthermore
,
these
constraints
are
processed
by
using
the
logarithmic
barrier
function
.
The
Newton
^
equation
of
the
system
is
obtained
and
solved
for
the
iterative
equations
of
control
inputs
.
Finally,the
effectiveness
of
the
proposed
method
is
verified
by
nu
merical
simulation
and
experiments
.
Key
words
：
visual
servo
；
robot
；
model
predictive
control
；
interior
point
method
i引言
视觉传感器提供了丰富的外部环境测量信息.视觉伺服
会增加系统不稳定以及特征从图像有效区域丢失的可能
性[51.特征丢失是视觉伺服的普遍问题，对于直接处理图像
特征误差的
IBVS
，将特征点始终维持在视野内至关重要
文献[7]和文献[8]利用导航势函数避免特征被障碍物遮挡.
文献[9]为摄像机生成了类似圆弧的轨迹，以避免大位移的
视觉伺服过程中的特征丢失.但是这些方法的通用性不强，难
以应用到各种复杂的不同应用中.从最优化的角度来说，维持
特征不丟失是一种系统约束.模型预测控制是一种非线性最
优控制策略,特别适合处理控制中存在的各种约束问题.近年
来，采用模型预测控制的处理方法解决
IBVS
控制问题得到
了广泛的关注[1° "]，但是大部分文献直接利用
MATLAB
中
的内置优化函数解决模型预测控制中的优化问题，不利于在
实际工程中实现.针对移动机器人平台，文献[11]和文献
[12]采用神经网络解决
QP
问题，但是存在着神经网络的训
练等问题.针对上述问题，本文采用模型预测控制处理方法，
将控制量的饱和、特征在图像空间的坐标限制作为约束，把
IBVS
转化成一个有限时域内带约束的优化问题，通过障碍函
数法把不等式约束转化成等式约束，给出了采用
LOQO
内点
法[
M
]计算最优控制量的算法.最后通过仿真和
Panda
机器人
上的实验验证了算法的有效性.
是一种利用视觉测量数据作为反馈信号的机器人运动控制器
的设计技术，在工业机器人、无人飞行器、智能机器人等领域
得到了广泛的应用1M].
根据选取的误差信号，视觉伺服的控制策略可以分为3
类:基于位置的视觉伺服（
PBVS
)、基于图像的视觉伺服（
IB
-
VS
)和混合视觉伺服[4].
PBVS
利用视觉处理算法获得的相
机和物体之间的相对位姿定义系统误差，使得机器人的运动
具有较好的三维轨迹，但容易受到系统标定误差、视觉测量噪
声的影响.
IBVS
直接采用目标物体特征分别在期望图像平面
和当前图像平面上的投影来定义系统误差，对相机参数和测
量误差具有较好的鲁棒性，且算法运算较少，吸引了很多机器
人领域的研究者的关注.混合视觉伺服同时利用相对位姿和
特征的图像投影定义误差，同样容易受到模型和测量误差的
影响.
传统的
IBVS
方法利用固定增益和雅可比矩阵的伪逆设
计控制律，只能保证局部稳定性,且增益的选取会对系统性能
产生重要影响，比如增益太小会影响系统收敛速度，增益过大
收稿日期:202(
M
)
I
>09收修改稿日期:202(
M
)2-20基金项目：NSFC-浙江两化融合联合基金项目（
U
1709213)资助.作者简介：滕游,
男，1985年生，硕士，助理研究员，研究方向为图像处理、机器视觉、机器人控制；刘安东，男，1985年生，博士，讲师，研究方向为机器人控制、预测
控制；俞立，男，1961年生，博士，教授,研究方向为运动控制、控制理论.
期滕游等:采用内点法和图像反馈的机器人视觉伺服预测控制
197
2问题描述
本文采用一个六自由度的协作机器人
Franka
Panda
和一
个
RGB
-
D
相机
SR
300,将
RGB
-
D
相机安装在协作机器人末
端夹具上，通过
RGB
-
D
相机获得当前待抓取物体的图像，驱
动机器人运动，使得物体的图像最终和期望的图像一致.通过
将协作机器人拖动到期望位姿进行测量，或者利用物体的
CAD
三维模型计算得到期望图像.
考虑上述系统在
eye
-
in
-
hand
框架下的
IBVS
控制问题如
图1所示.其中，相机安装在协作机器人末端，目标物体局部
坐标系为,当前摄像机坐标系为，期望的摄像机坐标
系为
IDI
.
图1基于图像的机器人控制
Fig
. 1
Image-based
robot
control
为了实现通过像素控制机器人运动，首先假设摄像机已
经完成标定且满足针孔投影模型，从而可以获得摄像机的内
部参数矩阵.根据得到的特征点的像素坐标，利用摄像机的内
部参数矩阵可以计算特征点的归一化图像平面坐标
x
= (
JC
,
其中，
X
和对应的特征点
p
在摄像机坐标系中的坐标
X
= (
x
,
y
,
z
，的关系如下:
x
=
X
/
Z,y
=
Y/Z
(1)
令
P
在期望图像平面上的坐标为
X
’
==(，，，）'则可以定
义图像平面上的系统偏差为
e
=
x
-
x
'
由于摄像机安装在运动的机器人末端上,摄像机坐标系
中的特征点坐标与机器人末端的速度紧密相关.根据机器人
末端的运动关系,特征点在摄像机坐标系中的坐标和摄像机
的运动速度之间的关系如下所示：
_
v-wxX
(2)
其中,
V
和
W
分别为摄像机自身的平动速度和转动速度.对式
(1)取微分，并代人式（2),可以得到归一化图像特征点与摄
像机运动速度之间的运动学方程：
x
=
L
(
x)u
(3)
其中
,u
= (
v
7',
w
7')7'，
-(1+
Jc
2) >
L
(
x
)=
0
f
^
〇 -+
f
(
l
”2)
-xy
IBVS
是像素层面的控制，虽然能够驱动单个特征点到达
期望状态，但无法保证摄像机具有唯一确定的方位.通常情况
下使用4个及以上不共线的特征点构造系统误差，以保证当
系统误差为零时，摄像机具有唯一确定的期望位姿.假设设置
的有效特征点个数为~则可将式(3)中的
x
和
L
重新定义为
x
= (
x
,(
L
【，-”,
L
；：)
T
•其中
,L
通常被称
为雅可比矩阵或者交互矩阵.
由式（3)可知不同的控制量
II
可以产生不同的系统状态
轨迹.控制律设计的目标即利用特征点的图像误差产生合适
的控制序列,使得摄像机到达合适的位姿.由于摄像机安装在
机器人末端上，摄像机的运动是通过机器人的关节电机驱动
的，电机具有最大转速的限制，从而使得控制量中的线速度和
角速度需要满足电机转速约束，即式（3)中控制输人
u
的约
束问题.另一方面，在运动过程中必须保持特征点始终位于摄
像机的视野内，因此在设计视觉伺服控制器的过程中有必要
考虑摄像机视野约束问题，即式(3)中的状态
X
的约束问题.
本文的目的是针对具有摄像机视野约束和控制输人约束
的
IBVS
控制问题，利用目标在当前图像平面和期望图像平
面上的特征偏差，采用
MPC
方法设计视觉伺服控制器，驱动
摄像机到达期望的位姿.
3基于预测控制的机器人视觉伺服控制器设计
MPC
是一种处理约束控制问题的有效方法.在
MPC
框
架中,控制问题通常被转化为一个带约束的最小化问题.因
此，本节针对具有视野约束和控制量约束的视觉伺服问题，设
计基于
MPC
的
IBVS
控制器，并采用
LOQO
内点法求解控制
输入，实现点镇定控制.
3.1
模型预测控制
为了获得预测模型，首先以采样周期
T
将目标特征点的式
(3)离散化，表示成如下的离散时间状态空间模型：
为了驱动机器人到达期望目标位姿，采用图像平面上的偏差
量
e
作为状态量，从而可进一步写出有限时域内的预测误差
和输人之间的关系为：
ek*i
= +
t/_i
ut
+
y
_|
T,j
= 1,2 ,•••
,N
(5)
另外，在实际物4系统中，机器人运动都是通过电机驱动
的，电机具有最大转速的限制，从而使得摄像机的线速度和角
速度需要满足最大转速的限制.由式（3)可知，线速度和角速
度约束即为控制量约束，因此控制量需要满足约束条件：
u—
〈化
+
卜
,<11
順
，_/ = 1
，
2,…，W (6)
其中11_和111„„分别为相机速度的最小值和最大值,通常具
有关系.更重要的是，为了保证可视性约束，系统
状态必须满足约束条件：
X— 腿， y=0 ， l,… ， W-1 (7) 其中和*_分别为归一化图像平面坐标的最小值和最大 值.本文考虑镇定问题，使用有限时域内的状态偏差和控制量 以及终端惩罚函数构造代价函数，从而将镇定问题转化成如 下的最小化问题： B (*),… m , in J (8) s . t . (5)、(6)、(7)和•/ = ……+ e【+(V ^ et*N 其中, G 和弋表示权重矩阵,为终端惩罚项.因此， IBVS 系统的控制器设计问题转化为一个带约束的二次优化 问题. 为了计算出 W 个时域的最优控制序列，重新定义以下向量： e 〇(*) =[ e [,-, e[]r 198 小型微型计算机系统 2021 年 e ( k ) = [ e[+l ,--*, e[+w]r 5⑷=[ u * r ，.", uI+AM]r 从而，可以将公式(7)表示成： e ( k ) =~ e 0( k ) ^ Gu { k)T (9) 其中， Lk 0 G L “1 ■ + I Lk + N-l - 进一步，系统性能指标函数可改写为： J = eTQe + ut R u + eTk + NFek,N =( e 〇 + GuT ) t Q ( e 〇 + GuT ) + uTRu + eTk + NFek+N = e 〇 rGe 〇 ^ eT 0 QGuT ^ iTGTQe0T + uTGTQGnt + ut R u + eTk + NFek+N (10) 其中，(2和/?分别为由 G ;和义组成的权重矩阵.式（ l 〇)中的 最后一项是终端惩罚项，将其展开后可得： e* + ^e* + = ( e* + I S 1 + U* + i-l + I s 1 + Uik + i-l = Is I S ^ ik +<-| Ujk + i-l T ') +( Is I X ^* + i-|UJk + i-l Fe *+( j | Li * i .1 utt (.1 r ) rF ( j |£, t + i .1 utti . lr )( ll ) 通过上述转化，最小化问题（8)转^成了一个关于控制 量 u ( Jt )的带约束的二次优化问题： m i ( m *) f _ ( u ( k )) s . t . h( u ( k)) ^0 ( 12 ) 其中 j ( a ( k )) =•/, -•*(*) u (*) - umin LtutT h (^ k )) a - xt (13) 乙 *utr n _LL“N_iuttN_lT. 3.2内点法 公式（12)中的不等式约束的数量和特征点的数量与系 统的预测时域有关.针对单个时域，关于控制输入的约束不等 式有12个，关于可视性的不等式约束数量为特征点数量的4 倍.令 yv 个时域的所有约束数量为 A #,采用障碍函数法将所 有约束转化成等式约束，优化问题转化成： s . t . / i ( u t) -〇> = 0 (14) 其中 u 4为11(幻的简化表示,《〇和M分别为引入的惩罚项和 惩罚因子.为了求解上述优化问题,采用拉格朗日乘子法，得 到下面的拉格朗日方程： L ( ut ,(〇, ytAi ) =/( u t) -^ I 1〇 g ( w ,) - yr ( A ( u t) - c 〇) (15) 其中 y 为引入的拉格朗日乘子.由最优性原理可知，上述拉格 朗日方程的最优值满足一阶 KKT 条件： Vf(ut) -Vh(ut)Ty=0 -/ie + WYe = 0 / i ( u *) - a ) =0 (16) 其中 ,W = dfaS ( o 〇 ,F = d /邓(兄）. 进一步令式（15)的 Hessian 矩阵和式（13)的 Jacobian 矩 阵为： ff(5t,y)=V2/(S1)-i 1 = v 1 jV2A((it) A ( S *) =▽/!(!〇 (17) 从而得到式（16)对应的牛顿方程为： - ~ H ( ukyy ) 0 A ( u k)T- _ Au ， 0- W~lY - / A 〇) - A ( i 〇 -I 0 - -Ay - - v /( i t) - A(ijV a - fiW~le + y = -y (18) - (0 -/ i ( u A) ■ p 其中〇■〇»和 P 为引人的中间变量，▽/(54)、扒，>〇和/1(5*) 的具体表达式为： V /( u t) =2 TGTQe 0 +2 TtGTQGut +2 Rut + ■ L Ftk - LlFLkuk 27+ 2 T 1(19) -^ k * N - l ^ ek ~ - Lk + N _' FL “ N _' i “N_'- ~^6 N A ( u *) = V / i ( u t)= hN - diag ( Lk — Lk + N_、、 ( 20 ) - diag ( LfL “N_l') ■ -~ H = 2 tGTQG ^2 R ^2 t i (21) - L “ n - FL “N-- 进而采用消元法可得式（18)的解为： Au t= - N -( T - ATy - ATm - xp ) ^ y = y - YW-l^ia (22) Aco = -p + AAu * 其中， Mu*,y,« 〇 ) =H(ut,y) + A ( u 4) ryW ' A ( u t) 由于/ V ( it , y , w )是非奇异的，所以，式（18)的解（22)是 唯一的.从而可以利用式（22),采用迭代策略获得优化问题 (14)的解： i «+l) = i tu) + au ) AS t0) 〇,»*'> =«〇«> + a (;, Aw u) y 0»» = yU ) +〇( U ) Ay «) 其中, aw >为迭代步长. 综上所述，将 LOQO 内点法解决 IBVS 的优化计算过程 总结如下： 2) 计算/!,+ (〇, ▽/(5 t ), A /， A ( i 〇; 3) 根据公式（18)计算〇■, - ip ; 4) 计算 W ( i ,， y ， w ); 5) 根据公式(22)计算 ASt , Ay , Ato ; i 期 6) 7) 计算 滕游等:采用内点法和图像反馈的机器人视觉伺服预测控制 199 所示.从图中可以看出，为传统 IBVS 控制器设置合适的控制 如果满足要求，则得到最优值，否则跳转到步骤2). 器增益系数后，控制器的输出能较长时间维持在最大值，但是 上述优化算法获得的是一组控制序列 SU )，将控制序列 中的第1组值作为当前时刻的机器人控制量.按照滚动优化 的原则，当新的控制量作用到机器人系统后，可以获得下一个 时刻的图像测量值，再次应用上述优化过程，计算控制量，从 而实现整个系统的实时反馈控制. 4实验 本节通过数值仿真和机器人实验对提出的方法进行验 证.仿真周期 r 设定为40 ms ,采用一个边长为50 cm 的正方 形的4个顶点作为特征点，实验中，把物体坐标系的原点设置 在正方形中心, XY 平面设置在正方形所在平面.摄像机相对 于物体坐标系的真实期望位姿为(〇,〇, -0.5,0,0,0),其中前 3个参数为摄像机坐标系相对于目标物体坐标系的位置，后3 个参数为摄像机坐标系相对于目标物体坐标系的方位角.在 期望状态下,4个特征点的归一化参考图像坐标为5^=(- 0. 5,0. 5,0.5 ,0. 5 ,0.5, —0.5, —0.5, —0.5). 在仿真中，平动速度限制在*25 cm / s 范围内，旋转速度 限制在±0.25 rad / s 范围内.对于本文采用 LOQO 内点法的 IBVS ,还需要考虑以下几个参数设置问题.首先，在初始化 时，令严为单位对角矩阵= max ( A ,(«!°>),其中 0.5.在计算过程中，令弘 =S ^ m ,5 =0. 5 ,aw =0. 95 :|’ = 1，2,3,4》一，当某次迭代计算出 的控制量和上一次迭代计算出的控制量之间的偏差足够小 时，结束当前迭代过程，将计算结果中的第1组控制量作为当 前时刻的控制输人. 本文比较了两种不同的初始位姿情况下，传统 IBVS 和 本文提出的方法的实验结果.第1种情况下，初始位姿和期望 位姿之间的偏差为绕 z 轴旋转180°,传统的 IBVS 会导致摄 像机沿着 z 轴后退，最终停留在一个导致雅可比矩阵失效的 状态，无法到达期望状态.采用本文的方法虽然也会产生后退 情况，但在运行过程中会产生旋转运动,且会改变摄像机平动 方向，使得摄像机沿着 z 轴前进，最终可以到达期望状态 .传 统 IBVS 控制器和本文提出的控制器的控制效果分别如图2 ( a )和图2( b )所示. ( a ) IBVS 控制器 （ b ) LOQO 预测控制 的实验结果 器的实验结果 图2第1种初始位姿下控制器运行结果 Fig . 2 Results of controllers under the first initial pose 第2种初始位姿为（1,1，-3，0,0,0. 6),传统 IBVS 控制器 产生的输出和本文提出的控制器的输出如图3( a )和图3( b ) 当接近目标状态时，控制量的输出会缓慢下降，收敛速度将变 得非常缓慢，在22. Is 时到达期望状态.而本文提出的控制器 的输出，在接近目标状态时，也可以维持在最大值，在13.9 s 时到达期望状态. 10 15 20 25 0 5 10 15 20 25 Time (s) Time (s) ( a ) IBVS 控制器的输出 ( s / p 0 . V, 0.2 b V ， 0.1 j ) 0 . v: ^_ — 3 ^ 〇 . 3 ^ -. 0 -. 13 0 a 0 2 4 6 8 10 1214 16 2 4 6 8 1012 14 16 Time (s) Time (s) ( b ) LOQO 预测控制器的输出 图3第2种初始位姿下控制器的输出 Fig . 3 Outputs of controllers under the second initial pose 使用六自由度的协作机器人 Franka Panda 和 RGB-D 相 机 SR 300对本文提出的方法进行了验证.将目标物体随意放 置在相机的视野内, IBVS 控制器和本文提出的控制器都能够 驱动相机到达期望姿态和位置，两种控制器作用下，特征点在 图像平面上的轨迹分别如图4( a )和图4( b )所示，控制器的 ( b ) LOQO 预测控制 的实验结果 器的实验结果 -3 0.2 — V. 0.1 ’、. 上 v, -.-V, P 。 | g g - -0.2 〇• 1 0 2 4 6 8 1012141618 Time (s) ( c ) IBVS 控制器的输出 uou (s/PBJ)XJPO13> 0 V . * v 0 U ea 0 . I S -. 0 y V 3 -C U B 0 J I l -. 0 I (s/PBi)XJPOP> 2 3 4 5 6 0 1 2 3 4 5 6 7 Time (s) Time (s) ( d ) LOQO 预测控制器的输出 图4在 Panda 机器人上运行的实验结果 Fig . 4 Results running on a Panda robot 输出速度分别如图4( c )和图4( d )所示，从图中可以看出，本 文提出的控制器的收敛速度更快.在实际应用中，本文提出的 200 小型微型计算机系统 2021 年 方法通过求解一个带约束的非线性优化过程获得当前时刻的 控制量，避免了传统 IBVS 方法中存在的特征丢失问题、局部 最小问题，并极大地改善了临近期望状态时的收敛速度，但是 算法的实现更加复杂，对计算能力的要求更高. 5结束语 本文将 IBVS 问题表示成一个有限时域模型预测控制问 题，采用非线性优化方法计算所需的控制量.显著优点是在计 算控制量的过程中可以考虑2 D 图像平面的视野约束和物理 执行部件输出的控制量约束.实验表明，和传统的 IBVS 方法 相比，本文提出的方法有效地考虑了可视性约束，避免特征从 图像平面的有效视野中消失，同时考虑了控制量约束,可以避 免产生过大的控制量，同时使得控制量尽可能长时间地维持 在最大值，解决了传统 IBVS 中存在的特征丢失和增益无法 调节的问题. References ： [1 ] Ma X M,Wang C L,Cao L. Visual servo-based control of mobile robots for opening doors[ M]. Wearable Sensors and Robots,Singa pore ：Springer ,2017. [2 ] Schenck C,Fox D. Visual closed-loop control for pouring liquids [CJ//IEEE International Conference on Robotics and Automation (ICRA) Singapore,2017 ： 2629-2636. [3 ] Xu De. A tutorial for monocular visual servoing[ J]. Acta Automati- ca Sinica,2018,44(10) ： 1729-1746. [4 ] Jia Bing-xiyLiu Shan,Zhang Kai-xiang,et al. Survey on robot visu al servo control ： vision system and control strategies[ J]. Acta Auto- matica Sinica,2015,41 (5) ： 861-873. [5 ] Malis E. Improving vision-based control using efficient second-order minimization techniques [ C ]//IEEE International Conference on Robotics and Automation (ICRA) ,New Orleans, US A, 2004 ： 1843- 1848. [6 ] Mezouar Y, Chaumette F. Path planning for robust image-based control[ J]. IEEE Transactions on Robotics and Automation,2002, 18(4) ： 534-549. [7 ] Cowan N J, Weingarten J D, Koditschek D E. Visual servoing via navigation functions[ J]. IEEE Transactions on Robotics,2002,18 (4) ： 521-533. [8 ] Chen J,Dawson D M,Dixon W E,et al. Navigation function-based visual servo control [J]. Automatica,2007,43(7) ： 1165-1177. [9 ] Chesi G, Vicino A. Visual servoing for large camera displacements [J]. IEEE TransacUons on Robotics,2004,20(4) ： 724-735. [10] Allibert G,Courtial E, Chaumette F. Predictive control for con strained image-based visual servoing[ J]. IEEE Transactions on Ro botics,2010,26(5) ： 933-939. [11 ] Heshmati-Alamdari s G K,Eqtami A,et al. Robustness a- nalysis of model predictive control for constrained image-based vis ual servoing[ C]//DBEE International Conference on Robotics and Automation (ICRA )，Hong Kong ， 2014:4469 4474. [12] Li Z, Yang C,Su C Y,et al. Vision-based model predictive control for steering of a nonholonomic mobile robot[ J]. IEEE Transactions on Control Systems Technology ,2016,24(2) ： 553-564. [13] Ke F,Li Z,Xiao H,et al. Visual servoing of constrained mobile ro bots based on model predictive control [ J ]. IEEE Transactions on Systems,Man,and Cybernetics ： Systems,2017,47(7) ： 1428-1438. [14] Vanderbei, Robert J. LOQO：an interior point code for quadratic programming [ J ]. Optimization Methods and Software ,1999,11(1- 4) .451484. 附中文参考文献： [3] 徐德 . 单目视觉伺服研究综述 [J]. 自动化学报 ,2018,44(10): 1729-1746. [4] 贾丙西，刘山，张凯祥，等 . 机器人视觉伺服研究进展：视觉系 统与控制策略 [J] • 自动化学报， 2015，41 (5) :861473.