基于眼到手视觉伺服的移动机器人模型预测控制

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

0 引言

视觉传感器作为移动机器人的眼睛，被应用于视觉伺服、视觉导航及视觉定位等场合，是移动机器人领域运用最为广泛的传感器。因此，移动机器人的视觉伺服控制一直是移动机器人研究领域的热点。按照建模方法的不同，视觉伺服可分为基于图像的视觉伺服(Image-Based Visual Servoing，IBVS)[1]、基于位置的视觉伺服(Position-Based Visual Servoing，PBVS)[2]以及混合视觉伺服(Hybrid Visual Servoing，HVS)[3]。PBVS主要思想是利用传感器去估计机器人的位姿信息，并与给定的位姿信号进行比较得到误差量来设计反馈控制律以实现机器人的控制，其缺点在于PBVS的伺服效率取决于估计姿态的准确性。IBVS主要思想是直接在图像层对机器人进行控制，将视觉传感器获得的图像与参考图像进行对比得到图像误差来设计反馈控制率。HVS误差模型同时取决于图像特征以及相机的姿态，其通过模板图像得到期望的图像特征信息，并将图像的特征信息做差来计算单应性矩阵。但在实际中，混合视觉伺服无法保证图像始终在视野范围内，并且仍受到相机标定精度和图像质量的影响。

IBVS相比于PBVS和HVS，对相机的标定及模型误差具有鲁棒性，且不需要三维坐标，计算速度更快。因此，IBVS受到了更多学者的关注。根据相机安装的位置不同，IBVS有两种不同的模型，分别为眼到手模型和眼在手模型。根据眼在手和眼到手模型的控制方法不同，可分为自适应控制，PID控制，智能控制及预测控制等。在眼在手模型中，文献[4]提出了一种基于图像的自适应视觉伺服算法，并引入李雅普诺夫函数保证相机参数未知的情况下，系统是稳定的，移动机器人轨迹跟踪误差能够收敛到零。文献[5]设计了基于图像的视觉伺服PD控制器，并对移动机器人的动态运动采用前馈神经网络补偿，利用图像误差信息结合运动控制器，使图像平面上的轨迹跟踪误差趋向于零。但是对于眼在手模型，当移动机器人运动时，相机和变化的工作空间的几何关系不容易确定，且不容易获得全局的视觉信息。基于眼到手框架，文献[6]利用固定在天花板上的针孔相机，建立了相机透视投影模型，并采用模型预测控制方法提出了一种新的动态反馈跟踪控制器，解决视觉轨迹跟踪问题。文献[7]在相机参数及移动机器人动力学参数未知的情况下，设计了一个自适应的轨迹跟踪控制器，实现了视觉全局渐近跟踪。上述视觉伺服控制方法中，PID控制方法是利用控制输入的给定值和输出值构成偏差，来对系统进行反馈控制，但是其PID参数不易调到最优，精度不高。自适应控制对于移动机器人模型和参数的不确定性具有很好的鲁棒性，其根据实际获得的数据与期望的数据进行对比，自动调整控制器参数和控制率使系统到达最优状态，并通过李亚普诺夫函数保证系统的稳定性。但是自适应控制的参数选择没有一个具体规则，在实际运用中可能无法获得一个最优的参数。智能控制是人工智能和自动控制的结合，为了处理视觉伺服中存在的各种不确定性及提高视觉伺服系统的稳定性，许多学者将神经网络、遗传算法引入到视觉伺服系统中，其主要形成了模糊控制、神经网络控制等方法。神经网络的视觉伺服通过网络的大量训练和神经网络对于非线性映射的逼近能力，来建立机器人空间和图像空间的对应关系，其无需分析视觉模型。但是对于不同的工作环境，神经网络都需要重新训练模型，其适应普及能力较弱。

模预测控制(Model Predictive Control，MPC)能够根据系统的当前输入以及过程的历史信息，预测下一时刻输出的值[8]，并很好的用于非线性系统处理移动机器人的约束及多变量问题，且鲁棒性较好。文献[9]基于模型预测控制方法使用了视觉追踪算法提取特征点完成视觉跟踪任务，并设计了上位机实时绘制移动机器人的轨迹。文献[10]提出了一种模型预测控制方法解决了移动机器人视觉轨迹跟踪问题。因此，本文采用了模型预测控制方法设计控制器。在实际中，由于受电机转速的影响，移动机器人存在速度约束，而文献[9]和[10]所提方法无法直接处理速度约束问题。为解决带有速度约束的视觉轨迹跟踪问题，文献[11]将原始对偶神经网络和模型预测控制结合，解决了带有速度约束的视觉轨迹跟踪问题。文献[12]采用基于管道的模型预测控制及3个神经网络分别设计了线速度控制器和角速度控制器，解决了视觉点镇定问题。但模型预测控制与神经网络结合求解优化问题往往比较复杂，计算量大。

基于以上的讨论，受文献[13]和[14]的启发，本文提出了一种基于LOQO内点法的模型预测控制方法解决具有速度约束的移动机器人视觉轨迹跟踪问题。首先，结合眼到手视觉伺服模型建立移动机器人的轨迹跟踪误差模型，并对移动机器人的误差模型进行离散化，给出基于模型预测控制的跟踪代价函数。考虑到移动机器人存在的速度约束问题，采用障碍函数法[15]将不等式约束转化为等式约束并结合模型预测控制方法建立并引入优化问题。最后，使用LOQO内点法求解建立的预测控制最小化问题，得到基于视觉的轨迹跟踪控制器。通过数值仿真验证所提算法的有效性。

1 问题描述

1)基于眼到手的相机模型：

眼到手视觉伺服框架如图1所示，其中相机固定在天花板上，且相机与移动机器人平面平行。世界坐标系为X-Y-Z，相机坐标系为XC-YC-ZC，图像坐标系为XP-YP-ZP，像素坐标系为U-O-V。

图1 相机模型

在图1中，C点是相机光轴和X-Y平面的交点，它在X-Y平面上的坐标为(Ocx,Ocy)。相机光轴与图像坐标平面的交点坐标为(Opx,Opy)。机器人质心P在X-Y平面上的坐标为(x,y)，由针孔成像原理可得P在像素坐标系下的坐标P2(xr,yr)为：

(1)

2)移动机器人运动学模型:

考虑两轮差分式移动机器人，其模型如图2所示。

图2 移动机器人模型

根据图2，移动机器人的运动学模型表示为如下微分方程的形式：

(2)

式中，是移动机器人的行进方向速度，为移动机器人的角速度。对式(1)进行求导并结合式(2)可得眼到手的移动机器人视觉伺服运动学模型为：

(3)

3)轨迹跟踪误差模型：

在视觉轨迹跟踪任务中，期望的轨迹定义为：

qr(t)=(xr(t),yr(t),θr(t))T

(4)

其中:(xr,yr)为机器人质心(x,y)在像素坐标系下的参考坐标，θr为机器人在世界坐标系下的参考前进方向的角度。不失一般性，假设在像素坐标系下U轴上缩放的倍数与V轴上缩放的倍数相等，即α=β，对式(4)进行微分可以得到下式：

(5)

其中:ωr为参考机器人的角速度，vr为参考机器人的进行方向速度。基于以上分析，可得跟踪误差向量为：

(6)

对式(6)进行求导并结合式(3)和(5)可得眼到手的视觉伺服移动机器人轨迹跟踪的误差系统为：

(7)

对式(7)进行离散化，可得跟踪误差系统的离散化形式为：

x(k+1)=f(x(k))+g(x(k))u(k)

(8)

本文的目的是在eye-to-hand的框架下，针对具有速度约束的移动机器人视觉轨迹跟踪问题，采用基于LOQO内点法的模型预测控制方法设计基于视觉的轨迹跟踪控制器，实现移动机器人的轨迹跟踪控制。

2 控制器设计

模型预测控制方法被称为滚动时域控制方法(RHC)，是一种最优控制策略，其突出优点是对系统模型要求不高，可以处理控制量和状态量的约束，以及多目标优化。考虑如下的模型预测控制代价函数：

(9)

其中:m和N分别为预测时域和控制时域，x(k+j|k)和u(k+j|k)分别是x(k)和u(k)在k时刻的预测值，Q与R是权值矩阵。另一方面，考虑移动机器人的速度约束，可以将模型预测控制最小化问题描述为如下带有输入约束的性能指标优化问题：

(10)

(11)

针对具有输入约束的优化问题，首先，采用障碍函数法[15]将不等式约束转换为等式约束，并将其引入到代价函数中，则优化问题(10)可重写为：

(12)

(13)

▽uι=▽uΓ-λ1+λ2=0

(14)

▽ξι=-ηD-1e+λ1=0

(15)

▽γι=-ηE-1e+λ2=0

(16)

(17)

(18)

其中:D和E为二维的对角矩阵，其对角线上的第i个元素分别为ξi和γi，对式(15)和(16)左乘矩阵D和E，可得原始对偶方程：

▽uΓ-λ1+λ2=0

(19)

-ηe+DΛ1e=0

(20)

-ηe+EΛ2e=0

(21)

(22)

(23)

(24)

对式(24)两边同时左乘得：

(25)

图3 系统状态误差 图4 系统输入误差

其中:ru=▽uι，rξ=▽ξι，rγ=▽γι，rλ=▽λι。

图5 速度跟踪曲线 图6 实际轨迹与真实轨迹

通过计算可得Δξ，Δγ：

(26)

(27)

将式(26)～(27)代入式(25)，可得如下降维系统：

(28)

ui+1=ui+ρ(i)Δu(i)

(29)

ξ(i+1)=ξ(i)+ρ(i)Δξ(i)

(30)

γ(i+1)=γ(i)+ρ(i)Δγ(i)

(31)

λ1(i + 1)=λ1(i)+ρ(i)Δλ1(i)

(32)

λ2(i + 1)=λ2(i)+ρ(i)Δλ2(i)

(33)

其中:ρ(i)为搜索方向，每次迭代都要求解式(28)得到Δu，Δλ1，Δλ2；Δξ和Δγ可求解式(26)～(27)得到。

定理2.1：当Ζ=-Hu-D-1Λ1-E-1Λ2，如果矩阵Ζ是非奇异的，则式(28)具有唯一的解。

Δu=Ζ-1(▽uΓ+D-1Λ1rλ1-E-1Λ2rλ2-ηD-1e+ηE-1e)

(34)

Δλ1=-D-1Λ1(rλ 1+Δu)-rξ

(35)

Δλ2=-E-1Λ2(rλ2-Δu)-rγ

(36)

证明：对式(28)采用消元法可得到式(34)，(35)，(36)。证毕。

注：在式(10)中，权重矩阵Q和R为正定矩阵，因此，可知Hessian矩阵Hu也是正定矩阵。松弛变量ξ，γ和拉格朗日乘子λ1，λ2都为正的，即对角矩阵D，E，Λ1和Λ2也都是正定的。所以保证了矩阵Z是非奇异的。

3 实验仿真分析

在仿真中，搜索方向ρ(i)选取为：

ρ(i)=0.95*

期望跟踪的轨迹描述为：

仿真结果如图3～6所示。

图3表示系统的状态误差变化图，从中可以看到，系统状态在4秒后趋于零，表明系统的状态误差具有良好的收敛性，轨迹跟踪的横坐标、纵坐标及角度误差能够收敛为零。图4表示系统的输入误差，在图中可以看到短时间内输入误差能够快速收敛到零。图5表示移动机器人的实际线速度和实际角速度，在图5中可以看到，移动机器人的实际线速度和角速度能够在短时间内跟踪上期望的线速度和角速度，并能够保持稳定。图6是像素坐标系中移动机器人的实际轨迹与真实轨迹图，从图6中可以看到，对于一个给定的期望轨迹，当初始位置偏离实际轨迹时，移动机器人仍可快速跟踪期望的轨迹，完成对一个圆的轨迹跟踪任务。

从上述仿真结果可以看出，当跟踪问题中含有不等式约束问题时，基于LOQO内点法的预测控制方法设计的控制器能够很好地保证视觉轨迹跟踪系统的稳定性，系统的状态误差及输入误差都能够较快的收敛到零，移动机器人的线速度和角速度也能很快的保持稳定，并且仿真中移动机器人能够快速的跟上期望的参考轨迹，完成视觉轨迹跟踪任务。从表1中可以看出，在仿真中，移动机器人第10秒的最终线速度为0.249 99 m/s，最终角速度为0.312 50 rad/s，都能够跟上期望的线速度和角速度。对于第10秒的系统输入误差，误差u1和u2都能够收敛于零。从表1中可以看到，移动机器人的系统误差e1，e2，e3都能够收敛于零，表明了移动机器人的横轴误差、纵轴误差及角速度误差都能够无限趋向于零。根据上述分析，基于LOQO内点法的模型预测控制方法能够精确的完成带速度约束的移动机器人轨迹跟踪任务。

表1 第10秒跟踪误差仿真数据表

4 结束语

为了解决具有速度约束的移动机器人视觉轨迹跟踪问题，本文提出了一种基于LOQO内点法的模型预测控制方法。首先在眼到手框架下建立了移动机器人系统误差模型并给出基于模型预测控制的移动机器人视觉伺服跟踪的代价函数。同时考虑移动机器人速度约束，采用障碍函数法将不等式约束转化为等式约束，并引入到代价函数中。进而，采用LOQO内点法对代价函数进行求解，得到基于视觉的轨迹跟踪控制器。最后，仿真结果表明，本文所提出的算法能够很好地完成视觉轨迹跟踪任务。