多移动机器人的目标跟踪研究

孟　静，陈　罡，高晓丁

(1.浙江纺织服装学院 机电学院，浙江 宁波　315200； 2.西安工程大学 机电工程学院，西安　710000)



多移动机器人的目标跟踪研究

孟静1，2，陈罡1，高晓丁2

(1.浙江纺织服装学院 机电学院，浙江 宁波315200； 2.西安工程大学 机电工程学院，西安710000)

针对多移动机器人对固定目标和动态目标跟踪问题进行了研究；首先基于l-φ闭环控率对多移动机器人进行刚性编队，建立运动学模型，采用leader-follower协调策略算法，实现了多移动机器人的协调合作；然后采用SURF算法识别目标，通过路径规划实现对固定目标的跟踪以及利用卡尔曼滤波器实现对运动目标的跟踪，设计了基于Backstepping方法的控制器，使多移动机器人能稳定跟踪目标；最后用MATLAB进行仿真；仿真结果表明，所设计的控制器和算法能使多移动机器人的跟踪误差快速收敛于零，适用于多移动机器人对目标的跟踪。

多移动机器人；目标跟踪；路径规划；卡尔曼滤波器；Backstepping

0　前言

目标跟踪是机器人研究领域的一个重点，在人机交互、太空探索以及灾区救援等方面已有广泛的应用[1]。在固定目标跟踪中，需先对机器人的路径进行规划，对路径规划问题，主要有PRM(probabilistic roadmap method)算法、模拟退火算法以及人工势场法和蚁群优化算法相结合。Yilmaz等提出了一种基于互联网的移动机器人平台，平台采用单个网络摄像头，用户可以通过网络远程控制机器人实时跟踪路线[2]。对动态目标点跟踪主要采用帧间差分法、自适应背景消除法、匹配块法以及光流法等。Suluh等同时使用线性视觉轨迹算法用于跟踪拦截在三维空间内运动的气球。随着气球上升，机器人利用自适应控制算法自动调整摄像头转动角度，保证气球质心始终出现在图像中心[3-4]。文献[5]提出了一种扩展式卡尔曼滤波的估计算法，解决了机器人在未知环境下的动态目标追踪问题，提高了目标状态估计的准确性；文献[6]将CMUcam3控制算法应用于多种机动模式的移动机器人目标跟踪控制的研究，保证了机器人在运动过程中保持低功耗。但以上的目标跟踪多是基于单机器人系统及少数多机器人系统对固定目标的跟踪，多机器人系统对运动目标跟踪的研究情况仍处于萌芽阶段，而多机器人系统由于其时间、空间、功能、信息和资源的分布性特点，能够完成单机器人系统无法实现的复杂任务，所以，对于多移动机器人对运动目标的跟踪问题很有必要进行详细的研究。本文就多机器人系统对固定目标和运动目标的跟踪进行了研究，通过对协调策略和目标跟踪算法进行设计，最后通过仿真进行验证，结果表明多机器人系统能够实现对目标的稳定跟踪。

1　运动学模型

本文在跟随领航者法[7]思想的基础上建立机器人的运动学模型。当一个机器人群体由多个机器人组成时，需指定某个机器人为领航机器人，即 leader，其余机器人则作为它的跟随者，也就是followers，要使跟随机器人能准确地跟踪领航机器人的位置和方向，则需对领航机器人和跟随机器人之间的距离和相对位置角度进行合理的计算，跟随机器人通过得到的距离和角度跟踪到领航机器人，根据领航机器人与跟随机器人之间的相对位置关系，可以形成所需队形。其运动学模型如图1所示。

图1　运动学模型

Leader机器人的运动学方程为：

(1)

其中：vi,wi分别表示机器人的线速度与角速度。

Follower机器人的运动学方程为:

(2)

其中:

对于 follower机器人的控制输出(v2,w2)为：

(3)

其中:

则l-φ的闭环控制率可以表示为:

(4)

其中:a1,a2为比例控制系数。

2　协调控制策略和算法

在得到多移动机器人的运动学模型后，还需要设计合适的协调控制策略和算法来实现对多移动机器人的队形控制。本文采用Leader-follower编队方法对多移动机器人的队形进行控制[8-9]。Leader的运动遵循基于势场的目标吸引力原则，引力势场为：

(5)

对上式求导得：

(6)

Leader机器人受到目标的引力势场向目标位置移动。

ri=(xi,yi)为机器人i的位置矢量。

(7)

式中，Ld为机器人j和机器人i之间的期望距离，A为常量参数。

对(7)式求导，得到关于l的势场力函数：

(8)

(9)

对式(9)求导，得到φ的势场力函数为：

(10)

式中，Φd为机器人j和机器人i之间的期望夹角,B为常量参数。

由式(7)、式(9)得到关于队形控制的总势场函数为：

(11)

对式(11)求导，得出队形控制总势场力函数为：

(12)

3　控制器的设计

为了使多机器人系统中的leader快速而准确地跟踪确定目标，本文采用基于Backstepping控制方法[10]设计的控制器。移动机器人对确定目标的跟踪可描述为寻求一个控制矩阵K。

(13)

使得v(t)和w(t)的控制：

(14)

对位姿误差方程pe求微分，可得移动机器人位姿误差模型为:

(15)

(16)

对式(16)求导可得：

xe(wye-v+vrcosθe)+ye(-wxe+

(17)

设计控制律为：

(18)

将式(18)代入式(17)得：

(19)

4　目标跟踪

4.1目标识别

在SURF特征点的匹配过程中，采用欧氏距离对两个特征点的相似性度量进行计算，对于Ms中的任一特征点bi，先求出Fs中与bi的欧氏距离最小的特征点aj,j∈[1,p]，则对应的欧氏距离为：

对选出的所有匹配特征点求平均得：

(20)

则式(20)就是所要求的目标位置。

4.2固定目标跟踪

当移动机器人对固定目标进行跟踪时,需先进行路径规划,找到到固定目标的最优路径,再进行目标跟踪，本文采用遗传算法进行路径规划，假设在路径规划过程中不存在障碍物。设路径点的坐标为(xi,yi),路径最短的适应度函数[13]为：

(21)

其算法的具体步骤为：

1)对于初始种群的形成，可随机地在工作空间中垂直于x1,x2,...,xn的垂线上选择，本文中将种群规模取为40；

2)按式(21)可得到群体内各个体的适应度，得到相应的累计值为Si，最后一个累计值记为Sn；

3)在区间[0,Sn]内产生均匀分布的随机数R；

4)依次用Si与R相比较，当出现第一个Si大于或等于R的个体i将会被选为复制对象；

5) 重复3)、4)，直至满足需要的个体数目；

6)对于复制得到的个体，首先进行两两随机配对，然后按杂交概率Pc，采用双点交叉对匹配的个体进行交叉繁殖，产生一对新的个体；

7)对得到的新的个体加入随机扰动，本文取[-0.15,0.15]范围内的零均值高斯白噪声作为扰动，找出最优解；

8) 终止条件。

4.3动态目标跟踪

卡尔曼滤波器[14-15]能快速而准确地估计运动目标的运动参数。系统的状态方程为：

(22)

观测方程为:

(23)

Ak为状态转移矩阵，ξk为系统动态噪声矢量，Xk为系统状态矢量; Ck为观测系数矩阵,ηk+1为系统观测噪声矢量，Vk+1为系统观测矢量,ηk+1和ξk是零均值高斯白噪声序列，且彼此相互独立，协方差矩阵分别为Q,R。

根据已知条件，预测下一刻系统的状态，状态向量预测方程为：

(24)

状态向量协方差矩阵为：

(25)

其中：Pk和Pk+1|k分别为Xk和Xk+1|k的协方差矩阵。

根据预测值和测量值可得现在状态的最优估计值为：

(26)

(27)

Kgk为卡尔曼增益。

(28)

其中:I是单位矩阵。

通过以上公式不断进行递推运算，即可得出卡尔曼滤波的最优估计值。

当移动机器人跟踪运动目标时,假设运动目标作匀速运动，因此，运动目标的运动状态可以用其某时刻在图像中的位置和速度来表示。先对X轴方向的滤波过程进行讨论，Y轴方向同理，目标物体运动状态方程为:

(29)

式(29)中,xk为运动目标在t=k时刻时X轴方向上的位置；vk为运动目标在t=k时刻X轴方向上的速度；ak为运动目标在t=k时刻X轴方向上的加速度;T为运动目标在第k帧图像和k+1帧图像之间的时间间隔，akT和vkT为白噪声序列。将式(29)写为矩阵形式为：

(30)

则系统的状态矢量为：

(31)

状态转移矩阵为:

(32)

系统的动态噪声矢量为:

(33)

由于上述的观测值是目标在图像中的像素位置,即:

(34)

则卡尔曼滤波器系统观测矢量为:

(35)

系统的观测系数矩阵为:

(36)

通过上述系统建立的状态方程和观测方程，经过递推,就可以不断预测目标在下一帧图像中的位置。

在t=k时刻,利用SURF算法识别出运动目标在第k帧图像中的位置记为xk，当运动目标第一次出现时,根据此时运动目标的观测位置x0初始化滤波器:

(37)

在对角线上对系统初始状态向量协方差矩阵取较大值，取值根据实际测量情况来获得,本文取为:

(38)

系统的动态噪声协方差矩阵N取为:

(39)

通过计算得到运动目标在下一帧图像中的预测位置X(1|0)。在该位置附近，继续对运动目标在下一帧图像中的位置进行局部搜索,识别出的运动目标质心位置即为X(1|1)。同理，运动目标在第k+1帧图像中的预测位置位为X(k+1|k),在该预测位置附近进行局部搜索,通过SURF算法识别出的目标位置坐标为X(k+1)。

5　仿真结果

图2　固定目标跟踪　　　　　　　　图3　位姿误差

图4～图6为多移动机器人对运动目标的跟踪。假设动态目标的初始位置为(-2 000，500)，运动速度为v=10m/s,扫描周期T=5s，且运动轨迹为一条直线，则由图4可以看出，移动机器人根据卡尔曼滤波器得出的估计轨迹跟踪动态目标，由图5和图6可以看出根据所设计的控制器位移误差逐渐趋近于零，最终稳定地跟踪动态目标。

图4　动态目标跟踪

图5　横坐标位姿误差

图6　纵坐标位姿误差

6　结语

本文讨论了多移动机器人对固定目标和运动目标的跟踪问题，利用SURF算法识别目标，对固定目标的跟踪，先进行路径规划，找出最优路径，机器人根据找出的最优路径跟踪固定目标。对运动目标的跟踪则采用卡尔曼滤波器得出运动目标的最优估计值，移动机器人根据得出的估计值进行路径规划，跟踪动态目标，最后通过MATLAB仿真，仿真结果表明，通过所设计的控制器和算法能实现多移动机器人对固定目标与运动目标的跟踪。该研究对多移动机器人对运动目标的跟踪具有一定的参考价值，以及为实际的救援工作提供了模拟平台，在后续的工作中，将对多移动机器人协调的稳定性做进一步的研究。

[1]ChwaD.Sliding-modetrackingcontrolofnonholonomicwheeledmobilerobotsinpolarcoordinates[J].IEEETransactionsonControlSystemsTechnology，2004，12(4)：637-644.

[2] 戴博，肖晓明，蔡自兴.移动机器人路径规划技术的研究现状与展望[J].控制工程，2005，12(3)：198-202.

[3]LeeTL,LaiLC，WuCJ.Afuzzyalgorithmfornavigationofmobilerobotsinunknownenvironments[A].ProceedingsoftheIEEEInternationalSymposiumonCircuitsandSystems，Piscataway，NJ,USA：IEEE，2005：3039-3042.

[4] 许伟. 基于激光雷达环境信息处理的机器人定位导航技术研究[D]. 南京：南京理工大学，2006.

[5] 伍明，孙继银.基于扩展式卡尔曼滤波的移动机器人未知环境下动态目标跟踪[J].机器人，2010，32(3).

[6] 沈义平.多机动模式下移动机器人目标跟踪控制研究[D].哈尔滨：哈尔滨工程大学，2013.

[7] 彭林.多移动机器人群集运动控制研究[D].武汉：华中科技大学，2006.

[8] 胡玮韬.多机器人编队及运动控制研究[D].西安:西安电子科技大学,2010.

[9] 王佳，吴晓蓓，徐志良.一种基于势能函数的多智能体编队控制新方法[J].信息与控制，2008,37(3):263-268.

[10] 王家军.非完整控制系统的非线性控制策略研究[D].天津:天津大学，2003.

[11] 贾伟，张清，席庆彪，等.基于SURF的UAV快速目标识别算法[J].计算机工程与应用，2013,49(23)：132-136.

[12] 李凤英.移动机器人目标识别及跟踪搬运策略研究[D].大连：大连海事大学，2009.

[13] 刘玲.基于智能计算的移动机器人路径规划方法研究[D].湖南：湖南大学，2007.

[14] 刘士荣，孙凯，张波涛，等.基于改进Camshift算法的移动机器人运动目标跟踪[J].华中科技大学学报(自然科学版)，2011,39(2)：224-226.

[15]ChuiCK,ChenGR. 卡尔曼滤波及其实时应用[M]. 第4版.北京：清华大学出版社，2013.

Research on Target Tracking of Multiple Mobile Robots

Meng Jing1,2，Chen Gang1，Gao Xiaoding2

(1.Institute of Electromechanics,Zhejiang Textile Vocational Technology College,Ningbo315200，China; 2.Institute of Electrical and Mechanical Engineering,Xi′an Polytechnic University,Xi′an710000，China)

Aiming at the fixed target and dynamic target tracking problem of the multiple mobile robots is studied. Firstly,the rigid formation and kinematics model of the multiple mobile robots is established that is based on the l-φ closed loop control,the coordination of the multiple mobile robots is achieved under the leader - followers coordination strategy algorithm; And then the target is identified by using SURF algorithm,through the path planning and Kalman filter,the fixed target and dynamic target can be tracked. The controller is designed based on Backstepping,the target can be tracked stably by the multiple mobile robots. Finally,Matlab is used to simulate,the results shows that the tracking errors of the multiple mobile robots is converged to zero quickly under the designed controller and the algorithm,which can be used for the target tracking of the multiple mobile robots.

multiple mobile robots；target tracking；path planning；Kalman filter；Backstepping

1671-4598(2016)04-0141-05DOI：10.16526/j.cnki.11-4762/tp.2016.04.042

TP242

A

2015-09-29；

2015-11-02。

国家自然科学基金(61203068);宁波市重大科技攻关项目(2013B10045);智能纺织机电技术研发(2015B11009)。

孟静(1988-)，女，陕西榆林人，硕士研究生，主要从事机器人控制方向的研究。

陈罡(1974-)，男，浙江宁波人，教授，主要从事智能控制方向的研究。