基于机器人平台的四象限追击算法研究

黄 伟 1袁2袁 杨 卫 1袁2袁 张文栋 1袁2（1. 中北大学电子测试技术国家重点实验室，山西 太原 030051；

2. 中北大学仪器科学与动态测试教育部重点实验室，山西 太原 030051）

摘 要院该文基于轮式机器人平台，通过建立数学模型并求解，设计一种机器人追击算法，对入侵轨迹进行分析和总

结，从而实现对平面四个象限内各个方向入侵目标的全方位追击。实验验证该算法实时性好，简单易行，具有重要的

实用价值。

关键词院机器人；四象限；全方位；追击算法

中图分类号院TP24曰TP301.6曰TP391.9曰TN911.7 文献标志码院A 文章编号院1674-5124渊2014冤03-0075-05

Study of four quadrant pursuit algorithm based on robot platform

HUANG Wei1，2，YANG Wei1，2，ZHANG Wen-dong1，2

（1. National Key Laboratory for Electronic Measurement Technology，North University of ChinaTaiyuan 030051，China；

2. Key Laboratory of Instrumentation Science and Dynamic Measurement，North University of ChinaTaiyuan 030051，China）

Abstract: This paper introduced an algorithm based on the platform of wheeled mobile robot，which is about all-round pursuit of the invading target in the four quadrants of the plane. Inestablishing the mathematical model， solving the model， a robot pursuit algorithm is designed.The algorithm achieves all -round pursuit of the invading target in the four quadrants of theplane，through analyzing and summarizing the intrusion paths. The experimental results show thatthe algorithm is real-time，simple and easy，and it has very important practical value.Key words: robot；four-quadrant；full direction；pursuit algorithm

收稿日期院2013-08-26曰收到修改稿日期院2013-10-17作者简介：黄 伟（1986-），男，河北辛集市人，硕士研究生，

专业方向为嵌入式电子电路系统。

0 引 言随着社会发展和科技进步，机器人在生产生活中

得到越来越多的应用，轮式移动机器人作为其中的

一个重要分支，以其控制方便、行走速度快、工作效

率高等优点在工业、农业、反恐防爆、家庭、空间探测

等各个领域得到广泛的应用[1]。运动目标的追击与拦

截问题在实际生活中有很多应用：如在军事上，导弹

攻击、拦截来袭的敌方运动目标；在自动生产线上，

机器人从传送带抓取物体；在机器人足球赛中，机器

人需要追击运动的足球、对方机器人等。

目前很少有专门的文献介绍和研究机器人追击

问题。机器人追击问题和拦截问题类似，文献[2]中介

绍了有关“追逃游戏”（pursuit-evasion game）模型的

导弹制导律的研究，文献[3]中提出了一种鲁棒几何

算法来解决导弹制导中的目标拦截问题，文献[4-5]中介绍了自适应视觉跟踪和高斯网络算法在机器人抓

取中的应用，文献[6]中阐述了机器人运用理想比例

导航技术进行运动目标的拦截问题，文献[7]中对空

间航天器作战中的拦截问题进行了讨论。更多的研

究则集中于机器人足球赛中截球动作，文献[8-12]中分别通过改进的 PEE 算法、专家 PID 算法、推算定

位（RD）算法、提前角导引算法等对机器人足球赛中

的截球策略进行了研究。

中国测试CHINA MEASUREMENT & TEST Vol.40 No.3May，2014第 40 卷第 3 期2014 年 5 月

doi院10.11857/j.issn.1674-5124.2014.03.021

中国测试 2014 年 5 月中国测试

图 2 电机的运行范围

12000

8000

4000

50

1.0 2.0 3.0 4.0电流/A

100 150 200 250转矩/N窑m

连续运行范围

短时运行范围

电机可以短时

过载运行

电机额定功率曲线

100W本文通过对移动机器人平台追击模型的建立，

提出了一种对平面四个象限内的入侵目标实现全方

位有效追击的新算法，并通过实验验证了该算法的可

行性。

1 机器人平台介绍1.1 机器人平台结构

机器人平台的行走机构采用差速驱动原理，将

两个电机前后放置。单一的驱动轮本身没有转向功

能，只能前进、停止、后退和变速，当两个主动轮同时

驱动时，通过控制两主动轮不同转速下的转速差，实

现行走机构的直行和转弯。结合轮式机构的特点，此

种驱动方式会对机器人的通过性产生较大影响。在

遇到不平整路面或者有障碍物的情况下，很容易出现

一个、两个甚至三个轮子悬空的情况，如果这其中包

含一个或者两个主动轮时，机器人将无法按照要求

前进或者不能继续移动。因此在机器人平台上加装

了同步带装置，将四个轮子全都转变为主动轮，这样

在速度和通过性上都有很大提高。具体结构如图 1所示。

驱动电机选用查瑞士 MAXON 公司的 EC-pow原ermax 30 型石墨无刷电机，额定功率为 100 W，图 2为 EC-powermax 30 型电机的运行范围。该电机具

有体积小、效率高、惯性低、寿命长等优势，并具有无

齿槽效应、加速性能高、电磁干扰小、线性电压/速度

曲线、线性负载/电流曲线、转矩波动小、可短时过

载、结构紧凑的技术特点。

为获得更大的转矩和相对低的速度，本行走机构

选用了与驱动电机 EC-powermax 30 相配套的行星

轮减速箱GP42C。该减速箱具有传递转矩大、性能高、

尺寸小、输入齿轮与输出齿轮同轴的特点。

移动机构需要进行闭环控制，所以在电机的末端

安装一个光电编码器，构成伺服控制电机，从而实现

对系统的精确控制。按照 MAXON 公司的配套规格，

选用 MAXON 公司的光电编码器 HEDS5540，规格为

500 线，3 通道。

为了将主动轮的动力传递至被动轮，实现四轮驱

动，设计出了同步带。但是由于结构体积的限制，无

法增加多余的装置来安装同步带。本设计将联轴器

进行了改装，将联轴器和带轮合二为一，既减少了传

动的损失，又节省了空间。

根据以上介绍，最终设计完成的机器人平台实物

如图 3 所示。

1.2 惯性导航技术

机器人所处的位置是操作人员非常关心的问

题，陀螺仪是一种能够精确确定运动物体方位的仪

器，它在现代航空、航海、航天等国防工业及汽车、转

台、工业生产流水线、机器人，以及消费电子领域，如

摄像机、数码相机的图像防抖等，都得到了广泛的应

用。现代陀螺仪的种类众多，MEMS 陀螺是近年出现

并得到了广泛应用的新型陀螺。本设计中采用了

图 1 传动机构三维模型图

图 3 机器人平台实物图

76

第 40 卷第 3 期

图 4 机器人平台追击模型

O目标入侵方向

感知圆

A渊x1袁y1冤B渊x2袁y2冤割线中点C

追击点D渊x袁y冤

入侵角机器人追击路径

目标入侵轨迹

MEMS 陀螺 16255 导航定位系统来实时的获得机器

人的精确航向。ADIS16255 为完整的角速率测量系

统，采用 ADI 公司的 iSensor 集成技术制造，全部功

能均集成于一个紧凑的封装中。提供适当格式的工

厂校准及可调数字传感器数据，从而利用简单的 SPI串行接口即可方便地访问数据。ADIS16255 还提供

扩展的温度校准。通过 SPI 接口可以访问陀螺仪的

测量结果、温度、电源和一个辅助模拟输入。由于可

以轻松访问校准的数字传感器数据，因此开发者能够

获得可立即供系统使用的器件，使开发时间、成本和

编程风险得以减少。

2 数学模型的建立本研究的目的是利用轮式机器人平台，对值守范

围内的入侵人员和车辆目标进行检测，实现对平面

4 个象限内全方位入侵目标的追击。文中的机器人平

台通过红外热释电传感器对入侵的人员和车辆目标

进行检测，通过对采集信号的分析处理，确定入侵目

标的坐标，对入侵目标的运动轨迹进行预推，确定机

器人的最佳追击点，机器人根据最佳追击点对入侵

目标进行追击。机器人平台采用单体“两点式”导引

控制律进行控制，即根据目标的运动轨迹（定位到的

两点坐标）计算机器人的初始角度和行驶距离以保证

两者在值守范围内相遇即可。假设机器人和目标都

是做匀速直线运动，因此系统不存在根据目标轨迹修

正机器人运动轨迹的反馈，是一个开环控制系统。假设

红外热释电传感器检测到的目标第一点 A 的坐标为

（x1，y1），第二点 B 的坐标为（x2，y2），C 点为机器人感

知圆与目标入侵轨迹相交线段的中点，机器人追击目

标的相遇点 D 的坐标为（x，y），线段 OB 与目标入侵

方向的夹角为 A，机器人平台的追击模型如图4所示。

对照模型，按照求解未知数最少、所列方程最简

单、未知系数耦合率最低的原则，选取入侵点、攻击

点与坐标原点构成的三角形来求解，假设机器人平台

最大出击速度为 4m/s，人员和车辆目标分别为 2m/s和 8m/s，则根据几何关系可得：

cosA = L2+（V目标T）2-（4T）22伊L伊（V目标T）

（1）T=cosA伊L伊V目标依L伊 （cosA伊V目标）2-V目标

2+16姨V目标

2-16（2）

L= x22+ y22姨 （3）其中 T 为机器人平台出动至行走到追击点时所

消耗的时间，L 为定位第二点与坐标原点的欧式距

离。由方程可知，目标定位的第一点不影响方程的求

解，追击时间 T 只由控制变量入侵角的余弦来决定，

而入侵角的余弦的正负取决于定位第二点与目标入

侵割线中点的相对位置，若 y2>ymiddle，则入侵角为钝

角，反之为锐角，由此讨论可根据入侵的线型、入侵

的方向、定位第二点与割线中点的关系将追击模型

细化为 16 种情况，如图 5 所示。

根据以上分析，设计机器人追击算法如图 6所示。

3 追击算法的仿真分析假设机器人平台的值守范围是以机器人平台当

前位置为圆心半径为 50m 的圆，计算可得机器人平

台的出击时间 T臆12.5 s。通过 Matlab 对以上算法进

行仿真分析，分析结果如图 6中三维空间关系图所示，

图 7（a）是入侵目标为人员时的仿真结果，图 7（b）是入侵目标为车辆时的仿真结果。图中 X 轴为目标

（人员和车辆）的入侵角度 a，Y 轴为定位第二点坐标

与坐标原点的欧式距离 L；Z轴为机器人平台出动至

行走到追击点时所消耗的时间 T。由仿真结果分析可知入侵目标为人员或车辆时

有以下规律：

（1）入侵角度 a越大，定位第二点坐标与坐标原

点的欧式距离 L 越长，所需追击时间 T 就越长。

（2）目标为车辆时，可以看到只有当入侵角小于

30毅的情况下追击时间 T 才有效；目标为人员时入侵

角无限制，追击时间始终有效。

（3）目标为人员时，定位两点的距离必须小于目

标入侵割线长度的 2/3；对于目标为车时，定位两点

的距离必须小于目标入侵割线长度的 1/3，而且车辆

目标的入侵轨迹不能只经过一个象限。

（4）目标为人员时，定位第二点未经过割线中

点，追击时间 T 均可求出两个可行解，分别对应拦截

模式和追击模式；若目标定位第二点正好位于割线

中点处，则追击时间可被唯一确定。

黄 伟等：基于机器人平台的四象限追击算法研究 77

中国测试 2014 年 5 月中国测试

OBC D O D

CBA A

AD

BC ACB

D

OO

OABC D

OABC

O

ABCD O

CBD

DD

A

入侵角度琢琢<90毅 琢>90毅纵坐标

y1>y2

y1<y2

y1>y2

y1<y2

象限

第一象限

第二象限

入侵角度琢琢<90毅 琢>90毅纵坐标

y1>y2

y1<y2

y1>y2

y1<y2

象限

第三象限

第四象限

OO

OO

OO

OO

AA

A A

A A

A A

BB

BB

BB

BB

C琢

C

C C

CC

CC

DD

D D

DD

DD

琢

琢

图 5 机器人追击模型细化情况

图 6 追击算法程序流程图

唤醒红外

监测目标

计算K尧B与Y_MIDDLE

红外定位信息渊x1袁y1冤渊x2袁y2冤

求解追击时间

y2<Y_MIDDLEy2<Y_MIDDLE

N

人员

是否满足所有判据

Y车辆

人员或车辆

计算K尧B与Y_MIDDLE

N

Y

N

Y求解追击时间解集袁并比较选取最小解

求解追击时间求解追击时间解集袁并比较选取最小解

得出控制参数渊出击角

度和出击距离冤

图 7 不同入侵目标的仿真结果

渊a冤入侵目标为人员时的仿真结果

长度/m23 1 0 0 102030

入侵角/渊60毅冤

15

10

5

0

目标为人时追击时间

渊b冤入侵目标为车辆时的仿真结果

目标为车时追击时间

7.55.02.50.0

-2.5-5.030

2010

0 32

10

入侵角/渊60毅冤长度/m

78

第 40 卷第 3 期

4 实验验证根据以上分析，在机器人平台上编写了相应追击

算法程序，分别对人员和车辆目标进行了实验验证。

表 1 为入侵目标为人员时机器人平台的决策情况，

表 2 为入侵目标为车辆时机器人平台的决策情况，

实验数据与 Matlab 仿真分析结果相符，验证了以上

4 条规律。由实验数据可知该算法对人员目标具有

普遍的适用性，对车辆目标，由于机器人平台自身速

度以及值守范围的限制，具有一定的局限性。

5 结束语文中给出了一种基于轮式机器人为平台的机器

人追击算法，实现了对平面 4 个象限内入侵目标的

全方位追击。首先对目标入侵轨迹进行分析和综合，

构建了机器人平台追击数学模型，通过对模型的求解，

设计了机器人追击算法，最后通过仿真分析总结了

该算法的适用条件和局限性。并通过实验数据验证

了这些规律，实验结果表明该算法实时性好，简单易

行，具有重要的实用价值。

参考文献[1] 朱磊磊,陈军援 轮式移动机器人研究综述[J]援 机床与液压，

2009，37（8）：242-247.[2] Turetsky V，Shinar J援 Missile guidance laws based on

pursuit-evasion game formulations[J]援 Automatica，2003（39）：607-618援

[3] 张友安，胡云安，林涛援 导弹制导的鲁棒几何方法[J]援 控制

理论与应用，2003，20（1）：14-16.[4] Kimura H，Mukai N，Slotine J E援 Adaptive visual

tracking and gaussian network algorithm for roboticcatching[J]援 ASME，Advances in Robust and NonlinearControl System，1992（43）：67-74援

[5] 林瑾.机械手臂抓取轨迹精度测试仿真分析[J].中国测试，

2013，39（3）：125-128.[6] Mehrandezh M，Sela M N，Fenton R G援 Robotic inter原

ception of moving objects using ideal proportional navi原gation guidance technique[J]援 Robotics and AutonomousSystems，1999（28）：285-310援

[7] 徐晓静，张恒源，李智 援 空间作战中的轨道拦截建模与

仿真[J]援 装备指挥技术学院学报，2005，16（5）：60-63.[8] 王湘中，贺超英，吴舒辞援一种改进的机动目标拦截算法[J]援

中南林业科技大学学报，2008，28（5）：136-139.[9] 邓本再，王江银，张中景，等援 基于专家 PID 控制的足球

机器人截球的研究[J]援工业控制计算机，2010，23（8）：63-64.[10] 白冰洋，沈阳援推算定位算法在拦截策略中的研究与应用[J]援

计算机仿真，2009，26（3）：169-171援[11] 邢云冰，史浩山，赵洪钢，等援 基于足球机器人截球动作的

提前角导引算法研究[J]援系统仿真学报，2007，19（2）：393-395.[12] 朱群峰，宋亚男，杨宜民，等援 机器人足球比赛最优拦截问题

建模与求解[J]援计算机工程与应用，2010，46（5）：217-220.

第一点坐标/m 第二点坐标/m 入侵角/渊毅冤 静网决策情况备注

行驶距离/m 初始角度/渊毅冤 前进后退 旋转方向

渊-45袁5冤 渊-40袁10冤 59.03 35.32 50.83 前进 逆时针 第一象限渊中点前冤渊-45袁5冤 渊-20袁30冤 101.31 46.54 3.73 前进 逆时针 第一象限渊中点后冤渊45袁5冤 渊40袁10冤 59.03 35.32 50.83 前进 顺时针 第二象限渊中点前冤渊45袁5冤 渊20袁30冤 101.31 46.54 3.73 前进 顺时针 第二象限渊中点后冤渊-45袁-5冤 渊-35袁-15冤 68.20 35.47 39.17 后退 顺时针 第三象限渊中点前冤渊-45袁-5冤 渊-20袁-30冤 101.31 46.54 3.73 后退 顺时针 第三象限渊中点后冤渊45袁-5冤 渊40袁-10冤 59.03 35.32 50.83 后退 逆时针 第四象限渊中点前冤渊45袁-5冤 渊20袁-30冤 101.31 46.54 3.73 后退 逆时针 第四象限渊中点后冤渊5袁45冤 渊10袁40冤 59.03 35.32 50.83 前进 顺时针 第一象限反向渊中点前冤渊5袁45冤 渊20袁30冤 78.69 37.13 64.13 前进 顺时针 第一象限反向渊中点后冤渊-5袁45冤 渊-10袁40冤 59.03 35.32 39.17 前进 逆时针 第二象限反向渊中点前冤渊-5袁45冤 渊-20袁30冤 78.69 37.13 64.13 前进 逆时针 第二象限反向渊中点后冤渊-30袁-5冤 渊-15袁10冤 78.69 18.56 26.57 前进 逆时针 跨一二象限

表 1 目标为人员时机器人平台决策情况

第一点坐标/m 第二点坐标/m 入侵角/渊毅冤 静网决策情况备注

行驶距离/m 初始角度/渊毅冤 前进后退 旋转方向

渊-45袁5冤 渊-44袁6冤 52.76 不满足

渊-30袁-5冤 渊-25袁0冤 45.00 不满足

渊-35袁-10冤 渊-30袁-5冤 35.53 不满足

渊-40袁-15冤 渊-35袁-10冤 29.05 18.12 30.96 前进 逆时针 第二三象限渊中点前冤渊-35袁-6冤 渊-30袁-3冤 25.25 12.87 34.99 前进 逆时针 第二三象限渊中点前冤

表 2 目标为车辆时机器人平台决策情况

黄 伟等：基于机器人平台的四象限追击算法研究 79