采用内点法和图像反馈的机器人视觉伺服预测控制

滕 游，刘安东，俞 立

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

1 引 言

视觉传感器提供了丰富的外部环境测量信息.视觉伺服是一种利用视觉测量数据作为反馈信号的机器人运动控制器的设计技术，在工业机器人、无人飞行器、智能机器人等领域得到了广泛的应用[1-4].

根据选取的误差信号，视觉伺服的控制策略可以分为3类：基于位置的视觉伺服(PBVS)、基于图像的视觉伺服(IBVS)和混合视觉伺服[4].PBVS利用视觉处理算法获得的相机和物体之间的相对位姿定义系统误差，使得机器人的运动具有较好的三维轨迹，但容易受到系统标定误差、视觉测量噪声的影响.IBVS直接采用目标物体特征分别在期望图像平面和当前图像平面上的投影来定义系统误差，对相机参数和测量误差具有较好的鲁棒性，且算法运算较少，吸引了很多机器人领域的研究者的关注.混合视觉伺服同时利用相对位姿和特征的图像投影定义误差，同样容易受到模型和测量误差的影响.

传统的IBVS方法利用固定增益和雅可比矩阵的伪逆设计控制律，只能保证局部稳定性，且增益的选取会对系统性能产生重要影响，比如增益太小会影响系统收敛速度，增益过大会增加系统不稳定以及特征从图像有效区域丢失的可能性[5].特征丢失是视觉伺服的普遍问题，对于直接处理图像特征误差的IBVS，将特征点始终维持在视野内至关重要[6].文献[7]和文献[8]利用导航势函数避免特征被障碍物遮挡.文献[9]为摄像机生成了类似圆弧的轨迹，以避免大位移的视觉伺服过程中的特征丢失.但是这些方法的通用性不强，难以应用到各种复杂的不同应用中.从最优化的角度来说，维持特征不丢失是一种系统约束.模型预测控制是一种非线性最优控制策略，特别适合处理控制中存在的各种约束问题.近年来，采用模型预测控制的处理方法解决IBVS控制问题得到了广泛的关注[10,11]，但是大部分文献直接利用MATLAB中的内置优化函数解决模型预测控制中的优化问题，不利于在实际工程中实现.针对移动机器人平台，文献[11]和文献[12]采用神经网络解决QP问题，但是存在着神经网络的训练等问题.针对上述问题，本文采用模型预测控制处理方法，将控制量的饱和、特征在图像空间的坐标限制作为约束，把IBVS转化成一个有限时域内带约束的优化问题，通过障碍函数法把不等式约束转化成等式约束，给出了采用LOQO内点法[14]计算最优控制量的算法.最后通过仿真和Panda机器人上的实验验证了算法的有效性.

2 问题描述

本文采用一个六自由度的协作机器人Franka Panda和一个RGB-D相机SR300，将RGB-D相机安装在协作机器人末端夹具上，通过RGB-D相机获得当前待抓取物体的图像，驱动机器人运动，使得物体的图像最终和期望的图像一致.通过将协作机器人拖动到期望位姿进行测量，或者利用物体的CAD三维模型计算得到期望图像.

考虑上述系统在eye-in-hand框架下的IBVS控制问题如图1所示.其中，相机安装在协作机器人末端，目标物体局部坐标系为{L}，当前摄像机坐标系为{B}，期望的摄像机坐标系为{D}.

图1 基于图像的机器人控制Fig.1 Image-based robot control

为了实现通过像素控制机器人运动，首先假设摄像机已经完成标定且满足针孔投影模型，从而可以获得摄像机的内部参数矩阵.根据得到的特征点的像素坐标，利用摄像机的内部参数矩阵可以计算特征点的归一化图像平面坐标x=(x，y)T.其中，x和对应的特征点p在摄像机坐标系中的坐标X=(X，Y，Z)T的关系如下：

x=X/Z，y=Y/Z

(1)

令p在期望图像平面上的坐标为x*=(x*，y*)T，则可以定义图像平面上的系统偏差为e=x-x*.

由于摄像机安装在运动的机器人末端上，摄像机坐标系中的特征点坐标与机器人末端的速度紧密相关.根据机器人末端的运动关系，特征点在摄像机坐标系中的坐标和摄像机的运动速度之间的关系如下所示：

(2)

其中，v和w分别为摄像机自身的平动速度和转动速度.对式(1)取微分，并代入式(2)，可以得到归一化图像特征点与摄像机运动速度之间的运动学方程：

(3)

其中，u=(vT，wT)T，

由式(3)可知不同的控制量u可以产生不同的系统状态轨迹.控制律设计的目标即利用特征点的图像误差产生合适的控制序列，使得摄像机到达合适的位姿.由于摄像机安装在机器人末端上，摄像机的运动是通过机器人的关节电机驱动的，电机具有最大转速的限制，从而使得控制量中的线速度和角速度需要满足电机转速约束，即式(3)中控制输入u的约束问题.另一方面，在运动过程中必须保持特征点始终位于摄像机的视野内，因此在设计视觉伺服控制器的过程中有必要考虑摄像机视野约束问题，即式(3)中的状态x的约束问题.

本文的目的是针对具有摄像机视野约束和控制输入约束的IBVS控制问题，利用目标在当前图像平面和期望图像平面上的特征偏差，采用MPC方法设计视觉伺服控制器，驱动摄像机到达期望的位姿.

3 基于预测控制的机器人视觉伺服控制器设计

MPC是一种处理约束控制问题的有效方法.在MPC框架中，控制问题通常被转化为一个带约束的最小化问题.因此，本节针对具有视野约束和控制量约束的视觉伺服问题，设计基于MPC的IBVS控制器，并采用LOQO内点法求解控制输入，实现点镇定控制.

3.1 模型预测控制

为了获得预测模型，首先以采样周期T将目标特征点的式(3)离散化，表示成如下的离散时间状态空间模型：

xk+1=xk+LkukT

(4)

为了驱动机器人到达期望目标位姿，采用图像平面上的偏差量e作为状态量，从而可进一步写出有限时域内的预测误差和输入之间的关系为：

(5)

另外，在实际物理系统中，机器人运动都是通过电机驱动的，电机具有最大转速的限制，从而使得摄像机的线速度和角速度需要满足最大转速的限制.由式(3)可知，线速度和角速度约束即为控制量约束，因此控制量需要满足约束条件：

umin

(6)

其中umin和umax分别为相机速度的最小值和最大值，通常具有关系umin=-umax.更重要的是，为了保证可视性约束，系统状态必须满足约束条件：

xmin

(7)

其中xmin和xmax分别为归一化图像平面坐标的最小值和最大值.本文考虑镇定问题，使用有限时域内的状态偏差和控制量以及终端惩罚函数构造代价函数，从而将镇定问题转化成如下的最小化问题：

(8)

其中，Qj和Rj表示权重矩阵，F(ek+N)为终端惩罚项.因此，IBVS系统的控制器设计问题转化为一个带约束的二次优化问题.

为了计算出N个时域的最优控制序列，重新定义以下向量：

从而，可以将公式(7)表示成：

(9)

其中，

进一步，系统性能指标函数可改写为：

(10)

其中，Q和R分别为由Qj和Rj组成的权重矩阵.式(10)中的最后一项是终端惩罚项，将其展开后可得：

(11)

(12)

(13)

3.2 内点法

公式(12)中的不等式约束的数量和特征点的数量与系统的预测时域有关.针对单个时域，关于控制输入的约束不等式有12个，关于可视性的不等式约束数量为特征点数量的4倍.令N个时域的所有约束数量为M，采用障碍函数法将所有约束转化成等式约束，优化问题转化成：

(14)

(15)

其中y为引入的拉格朗日乘子.由最优性原理可知，上述拉格朗日方程的最优值满足一阶KKT条件：

-μe+WYe=0

(16)

其中，W=diag(ωi)，Y=diag(yi).

进一步令式(15)的Hessian矩阵和式(13)的Jacobian矩阵为：

(17)

从而得到式(16)对应的牛顿方程为：

(18)

(19)

(20)

(21)

进而采用消元法可得式(18)的解为：

(22)

其中，

ω(j+1)=ω(j)+α(j)Δω(j)

y(j+1)=y(j)+α(j)Δy(j)

其中，α(j)为迭代步长.

综上所述，将LOQO内点法解决IBVS的优化计算过程总结如下：

3)根据公式(18)计算σ，-γ，ρ；

7)如果满足要求，则得到最优值，否则跳转到步骤 2).

4 实 验

本节通过数值仿真和机器人实验对提出的方法进行验证.仿真周期T设定为40 ms，采用一个边长为50cm的正方形的4个顶点作为特征点，实验中，把物体坐标系的原点设置在正方形中心，XY平面设置在正方形所在平面.摄像机相对于物体坐标系的真实期望位姿为(0，0，-0.5，0，0，0)，其中前3个参数为摄像机坐标系相对于目标物体坐标系的位置，后3个参数为摄像机坐标系相对于目标物体坐标系的方位角.在期望状态下，4个特征点的归一化参考图像坐标为x*=(-0.5，0.5，0.5，0.5，0.5，-0.5，-0.5，-0.5).

本文比较了两种不同的初始位姿情况下，传统IBVS和本文提出的方法的实验结果.第1种情况下，初始位姿和期望位姿之间的偏差为绕z轴旋转180°，传统的IBVS会导致摄像机沿着z轴后退，最终停留在一个导致雅可比矩阵失效的状态，无法到达期望状态.采用本文的方法虽然也会产生后退情况，但在运行过程中会产生旋转运动，且会改变摄像机平动方向，使得摄像机沿着z轴前进，最终可以到达期望状态.传统IBVS控制器和本文提出的控制器的控制效果分别如图2(a)和图2(b)所示.

图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)所示.从图中可以看出，为传统IBVS控制器设置合适的控制器增益系数后，控制器的输出能较长时间维持在最大值，但是当接近目标状态时，控制量的输出会缓慢下降，收敛速度将变得非常缓慢，在22.1s时到达期望状态.而本文提出的控制器的输出，在接近目标状态时，也可以维持在最大值，在13.9s时到达期望状态.

图3 第2种初始位姿下控制器的输出Fig.3 Outputs of controllers under the second initial pose

使用六自由度的协作机器人Franka Panda和RGB-D相机SR300对本文提出的方法进行了验证.将目标物体随意放置在相机的视野内，IBVS控制器和本文提出的控制器都能够驱动相机到达期望姿态和位置，两种控制器作用下，特征点在图像平面上的轨迹分别如图4(a)和图4(b)所示，控制器的输出速度分别如图4(c)和图4(d)所示，从图中可以看出，本文提出的控制器的收敛速度更快.在实际应用中，本文提出的方法通过求解一个带约束的非线性优化过程获得当前时刻的控制量，避免了传统IBVS方法中存在的特征丢失问题、局部最小问题，并极大地改善了临近期望状态时的收敛速度，但是算法的实现更加复杂，对计算能力的要求更高.

图4 在Panda机器人上运行的实验结果Fig.4 Results running on a Panda robot

5 结束语

本文将IBVS问题表示成一个有限时域模型预测控制问题，采用非线性优化方法计算所需的控制量.显著优点是在计算控制量的过程中可以考虑2D图像平面的视野约束和物理执行部件输出的控制量约束.实验表明，和传统的IBVS方法相比，本文提出的方法有效地考虑了可视性约束，避免特征从图像平面的有效视野中消失，同时考虑了控制量约束，可以避免产生过大的控制量，同时使得控制量尽可能长时间地维持在最大值，解决了传统IBVS中存在的特征丢失和增益无法调节的问题.