专利名称：一种基于可行通道的机器人目标追踪方法
技术领域：
本发明涉及移动机器人技术领域，特别是一种基于可行通道的机器人 目标追踪方法。
背景技术：
从世界上第一台机器人诞生以来，机器人技术得到了迅速的发展。机 器人的应用范围也已经从工业制造领域扩展到军事、核工业、航空航天、 服务业、医疗、人类日常生活等多个领域。其中具有广阔应用前景和军事 价值的移动机器人受到了普遍关注。
目标追踪是移动机器人的一个典型研究内容，在军事、反恐、安保等 任务中具有重要的应用前景。在获取目标的位置后，需要解决的问题是如
何躲避周围的障碍并保持对目标的追踪。目前有一些避障、奔向目标的方 法，诸如人工势场法以及改进、基于行为的方法、智能控制方法的应用等， 取得了一些研究进展，但是考虑到机器人的实际尺寸、实际环境的不确定 性、复杂性等因素，机器人避障追踪目标仍需要深入的研究。
发明内容
本发明的目的是提供一种基于可行通道的机器人目标追踪方法，使机器人通过自身的视觉传感器、超声传感器、红外传感器和码盘提供的信息 实现对目标的无碰追踪，达到令人满意的效果。
为实现上述目的，本发明的技术解决方案是
一种基于可行通道的机器人目标追踪方法，其特征在于，包括步骤
A) 在目标上设置色标筒；
B) 机器人用摄像机搜寻目标上的色标筒以确定目标，综合视觉测量与
码盘信息，得到目标在机器人坐标系下的估计位置(p:，《)；
C) 将超声和红外传感器信息统一到机器人坐标系下，结合估计的目标 位置，产生机器人的决策空间，同时获取障碍物分布点集
D) 建立机器人通道集/^'(f = O,l,...,尺,-1);
E) 结合决策空间中的目标信息和障碍物分布信息，选取出合适的机器人 通道；
F) 根据决策空间中的红外传感器信息以及机器人通道选择情况，确定机 器人的转角^与速度大小v，实现对机器人的控制。
所述的一种基于可行通道的机器人目标追踪方法，其所述目标上的色 标筒为中空的圆柱形筒，由至少两种颜色上下组合而成，色标筒的中心与 目标的中心保持一致。
所述的一种基于可行通道的机器人目标追踪方法，其所述机器人坐标 系为以机器人中心为极点，当前运动方向为极轴的极坐标系。
所述的一种基于可行通道的机器人目标追踪方法，其所述障碍物分布 点集的数目^为4的倍数。所述的一种基于可行通道的机器人目标追踪方法，其所述机器人通道
为一有向矩形，且机器人中心坐落在该矩形上。
所述的一种基于可行通道的机器人目标追踪方法，其所述机器人通道 集的数目/^为4的倍数，且7^为尺j的倍数。
本发明只根据自身感知信息做出决策，可以实现实时的控制，为移动 机器人在安全防御、安保等方面的应用提供技术支持。
图1是本发明的一种基于可行通道的机器人目标追踪方法的控制框图； 图2是障碍物分布(Xrf=16)示意图3是机器人对静态目标的追踪轨迹；
图4是机器人追踪动态目标的轨迹。
具体实施例方式
本发明提供了一种基于可行通道的机器人目标追踪方法，在机器人通 过视觉和码盘信息估算出目标在自身坐标系中的位置后，将超声传感器和 红外传感器信息统一到机器人坐标系下，结合估计的目标位置，产生机器
人的决策空间，同时获取障碍物分布点集；建立机器人通道集，结合决策 空间中的目标信息和障碍物分布信息，选取出合适的机器人通道，进而综 合决策空间中的红外传感器信息，确定出机器人的转角与速度大小，实现 对机器人的控制，使得机器人在避开障碍物的同时追踪目标。控制框图如 图1所示，其中，(",《)为目标在机器人坐标系下的估计位置，《(^: = 0,1，...,7^-l)为障碍物分布点集，F尸》-0,1,...,/^-l)为机器人通 道集合，0为机器人转角，v为机器人速度大小。
1.目标位置估计
建立以机器人中心为极点，当前方向为极轴的机器人坐标系S。跟随 机器人有四个摄像机&(/)(/ = 1,2,3,4)，从机器人中心到这四个摄像机光心
的连线方向分别与各自的光轴方向相一致，且分别与机器人当前运动方向
右转工方向、当前运动方向、当前运动方向左转工方向、当前运动方向的 2 2
反方向相一致。
目标上配备一个色标筒，色标筒为中空的圆柱形筒，由至少两种颜色 上下纟早合而成，色标筒的中心与目标的中心保持一致。机器人通过对色标 筒的视觉识别完成对目标的识别，结合视觉标定，进而获取估计的相对信 息。令(p:,《)为目标在机器人坐标系Z中的估计位置。
定义(",力是图像坐标系上一点，(x，》z)是其在世界坐标系上的坐标，
有
<formula>formula see original document page 7</formula>
其中#是摄像机的内参数矩阵，由摄像机标定得到。
记("pVl)和(m2,v2)分别为色标筒顶端和底端的点，(A，乂，A)和 (A,h，z》是它们所对应的世界坐标系的坐标，",="2， z,=z2， 乂是 色标筒的实际高度，则"估算如下<formula>formula see original document page 8</formula>
式中"，由#得到，v2-v,是色标筒在图像中的像素高度差，《是摄像机与
机器人中心之间的距离。
记( ，v》是色标筒的中心，~，《分别为图像的宽度和视野的宽度， 则《估算如下
《=
，
arc tan( A tan(《/ 2)) + (/ — 2). - (/ = 1， 2， 3)2w.
2
置tan(~^. tan(《/ 2)) + ;r (/ = 4 a wr < 0)
arc tan(j tan(《/ 2》—丌(>'=4 a wr》0)
如果机器人无法发现目标，此时，视觉测量失效，可以采用码盘信息
估计目标位置。如果机器人在7;时间内仍无法发现，它不得不搜索。记
(p二,《一)为前一采样时刻的目标坐标，A,A分别为上一决策周期左轮和 右轮行使的距离。从而，机器人转角 和机器人中心行使的距离《估算
<formula>formula see original document page 8</formula>
其中，《为左轮和右轮之间的距离。
目标在机器人坐标系S中的坐标可如下估计:
P"7 = J +《2 - 2C0S(d - A)
2.决策空间在实际的机器人系统中，不同传感器分布于机器人的不同位置。为此， 有必要将传感信息统一到机器人坐标系S下，得到机器人的决策空间，便 于机器人进行基于传感的决策。基于超声传感器信息，将机器人周围的环 境等分成K,等份，^/为4的倍数。机器人周围障碍物分布可描述为点集 《^ = 0,1,.，"^^-1)， 《用描述，定义集合
;r，"为决
;={《("，《)(A = o，i,...，A:rf — i)}，见图2，其中，《=
策空间中与《相对应的超声传感器信息。
3.机器人通道
考虑到机器人具有一定的物理尺寸，机器人通道定义为一有向矩形， 宽度为2>;,，长度为/>,，且机器人中心坐落在该矩形上。
设定机器人共有^,=^个通道可以选择，i^为4的倍数，《d为尺
/
的C^倍，C^〉0为一自然数。这些通道组成通道集/^"^ = 0,1,...,&-1)，
2;r
每个通道对应一个方向;r + :/ ，可用集合《表示为
《=
2;r
F《& =-兀+ 二W = 0,1,...,《y ， 1) }> ，与机器人当前运动方向相对应
尺/
的通道为前向通道F/f"2 。定义障碍物分布点集f的子集
4 5 4
定义《描述点《与通道/^'的关系，当该点在通道内时，《=1。于
是有<formula>formula see original document page 10</formula>
当Q,中所有的点都不在F《中，意味着通道F《是安全的，机器人可
以沿着通道/^'的方向运动，记^=0。
<formula>formula see original document page 10</formula>
4.选取合适的机器人通道
<formula>formula see original document page 10</formula>
为了实现机器人对目标的有效追踪，需要从所有安全的通道中选取出 合适的一个。选择标准是机器人尽量朝着目标方向运动，定义合适的机器 人通道为Fif 。
定义机器人运动方向到目标方向的角度为《，《=-《+2&;r ， 、=0(" = 0),6>0g=—《。其中，《e[-7T,;r)， 6^e(-oo,Qo)。当《在-;r或者 ;r周围跳变时，、的值发生改变。《从负到正，、加1;《从正到负，&减1。
首先计算出目标所在的通道wf ， r =
2;r
.3
若(一7T <《:< 一;r / 2) a O / 2 < 6>j < 7T)， 贝lj有、=、+1 ; 若 (;r/2〈(9么<;r)a(-;r<《<-;r/2)，贝'J ^=、-1。
(1)如果《S0，判断当前方向的相应通道是否可行，即F/一是否为 0 。若F，/2=0 ，搜寻/ = 0,1,...直至U (尺,/2 —Omodi^^r ，使得 F，/2_')m°", =1，于是，"-(i^/^-Z + OmodX^如果Ff卢—')m。^始终
为o， p二r。若/^//2 = 1，搜寻/ = 0，1,...，直至^(/://2 + 0010(1^:/ = 7，使得
'=0，于是p二(尺,/2 + 0mod^;如果/^//2+''— 始终为1，表明没有发现合适的通道。
(2)如果《<0，判断当前方向的相应通道是否可行，即F/一是否为
0 。若/^,/2 = 0 ，搜寻/ = 0，1,...直至lj (^y/^ + ^modi^^r ，使得 《V2+,)m。"， = i ，于是p = / 2 +,—丄)mod《,，如果《V2+')加^始终为o，
p = r。若尸，/2 = 1，搜寻z'=o,u.直到(尺"2 —/)mod^:r ，使得
/^〃2—')m。W =0'于是/7:(i^/2 —/)m0dX/;如果7^卢—')m。",始终为1，
表明没有合适的通道。
5.机器人决策
一旦机器人获取尸/T，机器人计算出转角^二+ 如果机器人
《/
当前运动方向所对应的红外传感器探测到障碍，机器人速度大小v设为0， 否则，v = V(/。当机器人无法发现合适的通道时，v取0。
本发明所提供的一种基于可行通道的机器人目标追踪方法应用于中科院 自动化研究所研制的智能机器人A頂上，该直径48cm，由双轮差动驱动， 安装有4个摄像机，16路超声传感器，16路红外传感器。每个摄像机视 野约60° 。当机器人和目标之间的距离小于60c奶时，认为追踪成功。 《rf =160 ， C# = 4 ， = 40 , D。 = 100cw ， Z>w = 60cm ， = 5 ，
《=0.27m，《=5cm， ^=20cm"。采用本发明所提供的目标追踪方法， 效果令人满意，图3和图4分别给出了目标静止和目标运动时，机器人的
无碰追踪轨迹。
权利要求
1.一种基于可行通道的机器人目标追踪方法，其特征在于，包括步骤A)在目标上设置色标筒；B)机器人用摄像机搜寻目标上的色标筒以确定目标，综合视觉测量与码盘信息，得到目标在机器人坐标系下的估计位置(ρnT，θnT)；C)将超声和红外传感器信息统一到机器人坐标系下，结合估计的目标位置，产生机器人的决策空间，同时获取障碍物分布点集Pnk(k＝0，1，...，Kd-1)；D)建立机器人通道集FPvt(t＝0，1，...，Kf-1)；E)结合决策空间中的目标信息和障碍物分布信息，选取出合适的机器人通道；F)根据决策空间中的红外传感器信息以及机器人通道选择情况，确定机器人的转角θ与速度大小v，实现对机器人的控制。
2. 如权利要求1所述的一种基于可行通道的机器人目标追踪方法，其 特征在于，所述目标上的色标筒为中空的圆柱形筒，由至少两种颜色上下 组合而成，色标筒的中心与目标的中心保持一致。
3. 如权利要求1所述的一种基于可行通道的机器人目标追踪方法，其 特征在于，所述机器人坐标系为以机器人中心为极点，当前运动方向为极 轴的极坐标系。
4. 如权利要求1所述的一种基于可行通道的机器人目标追踪方法，其 特征在于，所述障碍物分布点集的数目尺,为4的倍数。
5. 如权利要求1所述的一种基于可行通道的机器人目标追踪方法，其特征在于，所述机器人通道为一有向矩形，且机器人中心坐落在该矩形上。
6.如权利要求1所述的一种基于可行通道的机器人目标追踪方法，其 特征在于，所述机器人通道集的数目尺，为4的倍数，且^为^的倍数。
全文摘要
本发明一种基于可行通道的机器人目标追踪方法，涉及机器人技术，机器人用视觉与码盘信息得到目标在机器人坐标系下的位置估计(ρ<sub>n</sub><sup>T</sup>，θ<sub>n</sub><sup>T</sup>)，并将超声和红外传感器信息统一到该坐标系下，产生决策空间，同时获取障碍物分布点集P<sub>n</sub><sup>k</sup>(k＝0，1，…，K<sub>d</sub>-1)；建立机器人通道集FP<sub>v</sub><sup>t</sup>(t＝0，1，…，K<sub>f</sub>-1)，结合决策空间中的目标信息和障碍物分布信息，选取合适的机器人通道，进而综合决策空间中的红外传感器信息，确定机器人的转角θ与速度大小v，实现对机器人的控制，使机器人在避开障碍物的同时追踪目标。本发明方法使机器人通过自身视觉、超声、红外传感器和码盘提供的信息实现对目标的无碰追踪。该方法只根据自身感知信息做出决策，可以实现实时的控制，为移动机器人在安全防御、安保等方面的应用提供技术支持。
文档编号G05D1/12GK101667037SQ20081011958
公开日2010年3月10日 申请日期2008年9月3日 优先权日2008年9月3日
发明者侯增广, 超 周, 张文文, 曹志强, 旭 王, 瑗 袁, 民 谭 申请人:中国科学院自动化研究所