一种基于可行通道的机器人目标追踪方法 【技术领域】
本发明涉及移动机器人技术领域，特别是一种基于可行通道的机器人目标追踪方法。
背景技术
从世界上第一台机器人诞生以来，机器人技术得到了迅速的发展。机器人的应用范围也已经从工业制造领域扩展到军事、核工业、航空航天、服务业、医疗、人类日常生活等多个领域。其中具有广阔应用前景和军事价值的移动机器人受到了普遍关注。
目标追踪是移动机器人的一个典型研究内容，在军事、反恐、安保等任务中具有重要的应用前景。在获取目标的位置后，需要解决的问题是如何躲避周围的障碍并保持对目标的追踪。目前有一些避障、奔向目标的方法，诸如人工势场法以及改进、基于行为的方法、智能控制方法的应用等，取得了一些研究进展，但是考虑到机器人的实际尺寸、实际环境的不确定性、复杂性等因素，机器人避障追踪目标仍需要深入的研究。
【发明内容】
本发明的目的是提供一种基于可行通道的机器人目标追踪方法，使机器人通过自身的视觉传感器、超声传感器、红外传感器和码盘提供的信息实现对目标的无碰追踪，达到令人满意的效果。
为实现上述目的，本发明的技术解决方案是：
一种基于可行通道的机器人目标追踪方法，其特征在于，包括步骤：
A)在目标上设置色标筒；
B)机器人用摄像机搜寻目标上的色标筒以确定目标，综合视觉测量与码盘信息，得到目标在机器人坐标系下的估计位置(ρnT，θnT)；
C)将超声和红外传感器信息统一到机器人坐标系下，结合估计的目标位置，产生机器人的决策空间，同时获取障碍物分布点集Pnk(k＝0，1，...，Kd-1)；
D)建立机器人通道集FPvt(t＝0，1，...，Kf-1)；
E)结合决策空间中的目标信息和障碍物分布信息，选取出合适的机器人通道；
F)根据决策空间中的红外传感器信息以及机器人通道选择情况，确定机器人的转角θ与速度大小v，实现对机器人的控制。
所述的一种基于可行通道的机器人目标追踪方法，其所述目标上的色标筒为中空的圆柱形筒，由至少两种颜色上下组合而成，色标筒的中心与目标的中心保持一致。
所述的一种基于可行通道的机器人目标追踪方法，其所述机器人坐标系为以机器人中心为极点，当前运动方向为极轴的极坐标系。
所述的一种基于可行通道的机器人目标追踪方法，其所述障碍物分布点集的数目Kd为4的倍数。
所述的一种基于可行通道的机器人目标追踪方法，其所述机器人通道为一有向矩形，且机器人中心坐落在该矩形上。
所述的一种基于可行通道的机器人目标追踪方法，其所述机器人通道集的数目Kf为4的倍数，且Kd为Kf的倍数。
本发明只根据自身感知信息做出决策，可以实现实时的控制，为移动机器人在安全防御、安保等方面的应用提供技术支持。
【附图说明】
图1是本发明的一种基于可行通道的机器人目标追踪方法地控制框图；
图2是障碍物分布(Kd＝16)示意图；
图3是机器人对静态目标的追踪轨迹；
图4是机器人追踪动态目标的轨迹。
【具体实施方式】
本发明提供了一种基于可行通道的机器人目标追踪方法，在机器人通过视觉和码盘信息估算出目标在自身坐标系中的位置后，将超声传感器和红外传感器信息统一到机器人坐标系下，结合估计的目标位置，产生机器人的决策空间，同时获取障碍物分布点集；建立机器人通道集，结合决策空间中的目标信息和障碍物分布信息，选取出合适的机器人通道，进而综合决策空间中的红外传感器信息，确定出机器人的转角与速度大小，实现对机器人的控制，使得机器人在避开障碍物的同时追踪目标。控制框图如图1所示，其中，(ρnT，θnT)为目标在机器人坐标系下的估计位置，Pnk(k＝0，1，...，Kd-1)为障碍物分布点集，FPvt(t＝0，1，...，Kf-1)为机器人通道集合，θ为机器人转角，v为机器人速度大小。
1.目标位置估计
建立以机器人中心为极点，当前方向为极轴的机器人坐标系∑。跟随机器人有四个摄像机Sv(i)(i＝1，2，3，4)，从机器人中心到这四个摄像机光心的连线方向分别与各自的光轴方向相一致，且分别与机器人当前运动方向右转方向、当前运动方向、当前运动方向左转方向、当前运动方向的反方向相一致。
目标上配备一个色标筒，色标筒为中空的圆柱形筒，由至少两种颜色上下组合而成，色标筒的中心与目标的中心保持一致。机器人通过对色标筒的视觉识别完成对目标的识别，结合视觉标定，进而获取估计的相对信息。令(ρnT，θnT)为目标在机器人坐标系∑中的估计位置。
定义(u，v)是图像坐标系上一点，(x，y，z)是其在世界坐标系上的坐标，有：
zuv1=Mxyz=αx0u00αyv0001xyz]]>
其中M是摄像机的内参数矩阵，由摄像机标定得到。
记(u1，v1)和(u2，v2)分别为色标筒顶端和底端的点，(x1，y1，z1)和(x2，y2，z2)是它们所对应的世界坐标系的坐标，u1＝u2，z1＝z2，y2-y1是色标筒的实际高度，则ρnT估算如下：
ρnT=z1+dc=z2+dc=αy(y2-y1)v2-v1+dc]]>
式中αy由M得到，v2-v1是色标筒在图像中的像素高度差，dc是摄像机与机器人中心之间的距离。
记(uT，vT)是色标筒的中心，ud，θv分别为图像的宽度和视野的宽度，则θnT估算如下：
如果机器人无法发现目标，此时，视觉测量失效，可以采用码盘信息估计目标位置。如果机器人在Td时间内仍无法发现，它不得不搜索。记(ρn-1T，θn-1T)为前一采样时刻的目标坐标，Dl，Dr分别为上一决策周期左轮和右轮行使的距离。从而，机器人转角αn和机器人中心行使的距离dn估算如下：
αn=arctan(Dr-Dldrl),]]>dn=Dl+Dr2]]>
其中，drl为左轮和右轮之间的距离。
目标在机器人坐标系∑中的坐标可如下估计：
ρnT=ρn-1T2+dn2-2ρn-1Tdncos(θn-1T-αn)]]>
θnT=θn-1T-αn+sgn(θn-1T-αn)arccos(ρn-1T2+ρnT2-dn22ρnTρn-1T)]]>
2.决策空间
在实际的机器人系统中，不同传感器分布于机器人的不同位置。为此，有必要将传感信息统一到机器人坐标系∑下，得到机器人的决策空间，便于机器人进行基于传感的决策。基于超声传感器信息，将机器人周围的环境等分成Kd等份，Kd为4的倍数。机器人周围障碍物分布可描述为点集Pnk(k＝0，1，...，Kd-1)，Pnk用(ρnk，θnk)描述，定义集合见图2，其中，ρnk为决策空间中与θnk相对应的超声传感器信息。
3.机器人通道
考虑到机器人具有一定的物理尺寸，机器人通道定义为一有向矩形，宽度为Drw，长度为Drl，且机器人中心坐落在该矩形上。
设定机器人共有个通道可以选择，Kf为4的倍数，Kd为Kf的Cdf倍，Cdf＞0为一自然数。这些通道组成通道集FPvt(t＝0，1，...，Kf-1)，每个通道对应一个方向可用集合ξ表示为与机器人当前运动方向相对应的通道为前向通道定义障碍物分布点集ζ的子集Ωt={Pns(ρns,θns)(s=(tCdf+j)modKd)|j∈[-Kd4,Kd4]}.]]>
定义Stk描述点Pnk与通道FPvt的关系，当该点在通道内时，于是有，
当Ωt中所有的点都不在FPvt中，意味着通道FPvt是安全的，机器人可以沿着通道FPvt的方向运动，记
Fnt=1Stk=1,∃Pnk∈Ωt0others]]>
4.选取合适的机器人通道
为了实现机器人对目标的有效追踪，需要从所有安全的通道中选取出合适的一个。选择标准是机器人尽量朝着目标方向运动，定义合适的机器人通道为FPvp。
定义机器人运动方向到目标方向的角度为θng，kg＝0(n＝0)，其中，当θnT在-π或者π周围跳变时，kg的值发生改变。θnT从负到正，kg加1；θnT从正到负，kg减1。
首先计算出目标所在的通道FPvT，
若则有kg＝kg+1；若则kg＝kg-1。
(1)如果判断当前方向的相应通道是否可行，即是否为0。若搜寻i＝0，1，...直到(Kf/2-i)mod Kf＝T，使得于是，p＝(Kf/2-i+1)mod Kf；如果始终为0，p＝T。若搜寻i＝0，1，...，直到(Kf/2+i)mod Kf＝T，使得于是p＝(Kf/2+i)mod Kf；如果始终为1，表明没有发现合适的通道。
(2)如果判断当前方向的相应通道是否可行，即是否为0。若搜寻i＝0，1，...直到(Kf/2+i)mod Kf＝T，使得于是p＝(Kf/2+i-1)mod Kf，如果始终为0，p＝T。若搜寻i＝0，1，...直到(Kf/2-i)mod Kf＝T，使得于是p＝(Kf/2-i)mod Kf；如果始终为1，表明没有合适的通道。
5.机器人决策
一旦机器人获取FPvp，机器人计算出转角如果机器人当前运动方向所对应的红外传感器探测到障碍，机器人速度大小v设为0，否则，v＝vd。当机器人无法发现合适的通道时，v取0。
实施例
本发明所提供的一种基于可行通道的机器人目标追踪方法应用于中科院自动化研究所研制的智能机器人AIM上，该直径48cm，由双轮差动驱动，安装有4个摄像机，16路超声传感器，16路红外传感器。每个摄像机视野约60°。当机器人和目标之间的距离小于60cm时，认为追踪成功。Kd＝160，Cdf＝4，Kf＝40，Drl＝100cm，Drw＝60cm，Td＝5，drl＝0.27m，dc＝5cm，vd＝20cm/s。采用本发明所提供的目标追踪方法，效果令人满意，图3和图4分别给出了目标静止和目标运动时，机器人的无碰追踪轨迹。