专利名称：多机器人协调队形控制方法
技术领域：
本发明涉及机器人技术领域，特别是一种多机器人协调队形控制方 法，用于多机器人系统。
背景技术：
多机器人队形控制，指的是多个机器人组成的团队在执行任务的过程 中，在适应环境约束的条件下，相互之间保持预定的几何关系。多机器人 队形在军事、航空航天等方面有广阔的应用前景，它的实现有利于缩短任 务的执行时间，提高系统的整体性能。常用的队形控制方法有领航者跟随 方法、基于行为的方法和虚拟结构法，很多研究工作致力于对这些方法进 行研究和改进以提高系统运行稳定性和环境适应性。
对于一个由N个机器人组成的系统来说，一种思路是将队形控制问题 分解为N-1组两个机器人的队形控制问题进行实现，每组的两个机器人一 个为跟随机器人、 一个为领航机器人，它们致力于保持预先设定的几何形 状。通过系统中机器人两两队形控制，整体上呈现出协调的形状。为了实 现队形控制，跟随机器人需要获取领航机器人的相关信息，常用的获取信 息的手段是通讯。考虑到通讯网络延迟、数据包丢失乃至通讯失效以及执
4行机构的误差累积等问题，设计一种无通讯、通过对其领航机器人的感知推理来实现协调队形控制的方法是有意义的。
发明内容
本发明的目的是提供一种多机器人协调队形控制方法，是基于感知推理的方法，使得跟随机器人依靠感知推理实现与领航机器人的队形控制，达到令人满意的效果。
为实现上述目的，本发明的技术解决方案是
一种多机器人协调队形控制方法，包括步骤
A) 跟随机器人依据领航机器人上的色标筒，结合视觉与码盘信息，获取当前采样时刻^与之前的A^,次采样时刻相对于领航机器人的距离和
观测方位角/1》=0，…，7V肌)；
B) 跟随机器人借助码盘信息，通过局部坐标系变换，递推得到采样时刻
*和々-W，ax时的领航机器人在跟随机器人局部坐标系《-w，o)-w，"-w^下的位置丄fc^,乂)、 W-w，(;c〗-w严，乂-w戸"，以
及当前采样时刻A:时的跟随机器人前向摄像机的光心C。pt在
《-W一《-AW"-W一坐标系下的位置《(《,乂》；
C) 基于位置^、 Z^Wp自和《，跟随机器人计算出预测的领航机器人运动方向和领航机器人到C。pt方向之间的角度;
D) 结合角度W;以及期望角度^y 、跟随机器人与领航机器人之间的距离《(即d"以及期望距离丄扭，综合视觉信息，进行运动决策，得到速度v,和转角^，实现对跟随机器人速度大小和方向的控制。所述的多机器人协调队形控制方法，其领航机器人上的色标筒为中空的圆柱形筒，由至少两种颜色上下组合而成，放于机器人上，其中心与机器人的中心保持一致。
所述的多机器人协调队形控制方法，其队形控制是在跟随机器人对领航机器人感知推理的基础上实现的，机器人之间没有通讯。
所述的多机器人协调队形控制方法，其所述B)步中的跟随机器人局部坐标系《- 臓《—w，"—w ，是与采样时刻A-A^,相对应，其原点《—w^取前向摄像机光心c。pt所在的位置， _的方向与采样时刻
& - ^薩时的机器人运动方向保持一致。
所述的多机器人协调队形控制方法，其所述c)步中的跟随机器人对
领航机器人运动方向的预测，是根据当前采样时刻A与该时刻之前第^pn^次采样时刻的领航机器人位置信息得出的，且这两个采样时刻的领航机器
人位置信息均建立在跟随机器人采样时刻A之前的第iV，M次采样时刻所
对应的局部坐标系下。
所述的多机器人协调队形控制方法，其所述D)步中的跟随机器人与
领航机器人之间的距离《为,。
所述的多机器人协调队形控制方法，其所述A^, 21 。
本发明的多机器人协调队形控制方法，适合通讯失效的情况，为多机器人在军事等方面的应用提供技术支持。
图1是本发明一种多机器人协调队形控制方法的控制框图；图2是两机器人队形控制图3是本发明一种多机器人协调队形控制方法的递推预测图4是本发明一种多机器人协调队形控制方法的跟随机器人运动决策
图5是用本发明方法的领航机器人和跟随机器人的运动轨迹。
具体实施例方式
本发明提供了一种多机器人协调队形控制方法。跟随机器人结合视觉
与自身码盘信息，获取当前采样时刻A:与之前的A^max次采样时刻相对于领
航机器人的距离和观测方位角信息。借助码盘信息，通过局部坐标系变换，递推得到采样时刻&与该时刻之前的第A^,次采样时刻时的领航机器人在跟随机器人局部坐标系(与采样时刻A:-A^,相对应)下的位置，以及
当前采样时刻^时的跟随机器人前向摄像机(它的光轴方向与机器人运动方向相一致)的光心在该局部坐标系下的位置。基于上述位置，跟随机器人计算出预测的领航机器人运动方向和领航机器人到跟随机器人前向摄像机的光心方向之间的角度，进而结合领航机器人运动方向和领航机器人到跟随机器人前向摄像机的光心方向之间的期望角度以及跟随机器人与领航机器人之间的距离和期望距离，综合视觉信息，进行运动决策，得到跟随机器人的速度和转角，实现对跟随机器人速度大小和方向的控制。多机器人协调队形控制方法的控制框图如图1所示，其中，d"， p"(s = O，. 为当前采样时刻A与之前的 隱次采样时刻相对
于领航机器人的距离和观测方位角，丄K 乂)和Zf—w一 ((w,威，乂-w,顧)分别为采样时刻A和"7Vpmax时的领航机器人在 應坐标
系(跟随机器人与采样时刻/b-iVp,相对应的局部坐标系)下的位置，为当前采样时刻*时的跟随机器人前向摄像机的光心在
x，~max0，~薩"省_坐标系下的位置，《和^分别为预测的领航机器人
运动方向和领航机器人到跟随机器人前向摄像机的光心方向之间的角度与期望角度，《.(即""和丄^分别为跟随机器人与领航机器人之间的距离和期望距离，^和 分别为跟随机器人的速度和转角。
图2为两机器人队形控制图，其中《和A分别表示跟随机器人和领航机器人，/是机器人两驱动轮中心之间的距离，IOr是世界坐标系，
z,c^^7被选取作为《的局部坐标系，oy取前向摄像机的光心c。pt所在的位置，《是c^与机器人中心之间的距离，^的方向与机器人运动方向
保持一致。当《递推预测出领航机器人A的运动时，基于丄扭,w，得到期望队形点i^，随着i ,.奔向i^，丄,—i^，^—y扭，最终建立一种协调队形关系，其中L", ^分别为A与A之间的距离和期望距离，
n#y. e
-1和 s之间行驶的距离，W"—'X"力和,+^—"分别表示跟随机器人从采样 时刻A:-1到A"做圆弧运动(妙￡ = 0)时对应的圆弧的圆心角和半径。
结合相对于领航机器人的距离和观测方位角，跟随机器人估计领航机 器人在；^— 脆0"^鹏"— 鹏坐标系下的位置
丄"(d》o,…U如下一 "一》-
=
1 一 —1
基于 _与& ，跟随机器人得到《-aw max坐标系下预 测的领航机器人运动方向，其方向角^"为
t謹"!=(《"'狀- X"『)ii 4(X卜X""…J
K-l2 3、计算领航机器人运动方向和领航机器人到跟随机器人前向摄像机
的光心c。pt方向之间的角度《
跟随机器人前向摄像机的光心C。pt在《-aw坐标系下 的位置o)K,乂/、 = 0，..., _)计算如下<formula>formula see original document page 12</formula>.左
tan, =4^(4 =f( )
于是，可得预测的领航机器人运动方向和领航机器人到跟随机器人前
向摄像机的光心C。pt方向之间的角度《如下
考虑到传感数据的不精确性，v^限制在一个与期望角度l^相关的区 间内，例如
4、运动决策 ，
由《和《(即""，结合期望的V^以及丄^，得出跟随机器人的理想
12队形点尸；，如图4所示。《是《和《之间的距离，^是《^与C^^之 间的夹角，贝U:
<formula>formula see original document page 13</formula>
记e^—《-Z^!为跟随机器人和领航机器人之间的相对距离与期望
距离的偏差，=
为领航机器人运动方向和领航机器人到跟随机 器人前向摄像机的光心方向之间的角度与期望角度的偏差。当e^《dflI e^S^，跟随机器人认为队形实现，停止运动；当^d^dn且 ,〉^， 跟随机器人沿着它到领航机器人方向的反方向倒退直到efld超过dfl为止； 当^〉d。且e^S^，跟随机器人在与领航机器人的连线上运动；当 efld 〉c^且e^ >yfl ，它的速度v,和转角 如下计算
<formula>formula see original document page 13</formula>
式中，
<formula>formula see original document page 13</formula>
Ar是采样周期，v^和
《^分别为最大速度和转角。
当跟随机器人无法预测、估计领航机器人时，若跟随机器人的非前向摄像机看到领航机器人，它应旋转尽快使得前向摄像机发现领航机器人， 如果没有发现领航机器人，它会旋转搜索，若跟随机器人前向摄像机看到 领航机器人，它会在期望距离的限制下向领航机器人运动。
本发明多机器人协调队形控制方法应用于两个机器人的队形控制中， 机器人采用中国科学院自动化研究所研制的AIM智能机器人。每个机器人
都配有CCD摄像机和码盘，单个摄像机视野范围约60。。实验中跟随机器 人采用与运动方向一致的前向摄像机Q对领航机器人进行识别、位置估 计；其它摄像机的作用在于识别出领航机器人后尽快使得C,发现领航机器 人^隨=0.2附"，0隱=20°, A^ = 5， ^隨=5， <」=55°，<-"=35、 / = 0.27w，《=5c^， dfl=0.2m， ^=20。。跟随机器人与领航机器人之间 的初始距离1.54m，初始角度189。，队形要求两机器人之间期望距离为 1.2m，期望角度是210。。采用本发明所提供的方法，能够满足需求，图5 给出了领航者和跟随者(领航机器人和跟随机器人的简称)的运动轨迹， ^和^分别表示领航者与跟随者的起始点，G,和(^分别表示领航者与跟
随者的终止点。
权利要求
1.一种多机器人协调队形控制方法，用于多机器人系统；其特征在于，包括步骤A)跟随机器人依据领航机器人上的色标筒，结合视觉与码盘信息，获取当前采样时刻k与之前的Npmax次采样时刻相对于领航机器人的距离和观测方位角dk-s， id="icf0001" file="A2008102227740002C1.tif" wi="7" he="5" top= "76" left = "63" img-content="drawing" img-format="tif" orientation="portrait" inline="yes"/>(s＝0，…，Npmax)；B)跟随机器人借助码盘信息，通过局部坐标系变换，递推得到采样时刻k和k-Npmax时的领航机器人在跟随机器人局部坐标系 id="icf0002" file="A2008102227740002C2.tif" wi="44" he="7" top= "108" left = "28" img-content="drawing" img-format="tif" orientation="portrait" inline="yes"/>下的位置Lk(xLk，yLk)、 id="icf0003" file="A2008102227740002C3.tif" wi="50" he="6" top= "107" left = "121" img-content="drawing" img-format="tif" orientation="portrait" inline="yes"/>以及当前采样时刻k时的跟随机器人前向摄像机的光心Copt在 id="icf0004" file="A2008102227740002C4.tif" wi="44" he="7" top= "131" left = "28" img-content="drawing" img-format="tif" orientation="portrait" inline="yes"/>坐标系下的位置Ofk(xofk，yofk)；C)基于位置Lk、 id="icf0005" file="A2008102227740002C5.tif" wi="13" he="4" top= "142" left = "64" img-content="drawing" img-format="tif" orientation="portrait" inline="yes"/>和Ofk，跟随机器人计算出预测的领航机器人运动方向和领航机器人到Copt方向之间的角度ψijk；D)结合角度ψijk以及期望角度ψijd、跟随机器人与领航机器人之间的距离Lijk以及期望距离Lijd，综合视觉信息，进行运动决策，得到速度vf和转角θf，实现对跟随机器人速度大小和方向的控制。
2. 如权利要求1所述的多机器人协调队形控制方法，其特征在于， 领航机器人上的色标筒为中空的圆柱形筒，由至少两种颜色上下组合而 成，放于机器人上，其中心与机器人的中心保持一致。
3. 如权利要求1所述的多机器人协调队形控制方法，其特征在于， 队形控制是在跟随机器人对领航机器人感知推理的基础上实现的，机器人之间没有通讯。
4. 如权利要求1所述的多机器人协调队形控制方法，其特征在于，所述B)步中的跟随机器人局部坐标系x"p皿o"w"吓;^,，是与采样时刻* - 瞧相对应，其原点《—w一取前向摄像机光心C。pt所在的位置， 的方向与采样时刻A - iVpmax时的机器人运动方向保持一致。
5. 如权利要求1所述的多机器人协调队形控制方法，其特征在于， 所述C)步中的跟随机器人对领航机器人运动方向的预测，是根据当前采样 时刻A与该时刻之前第A^max次采样时刻的领航机器人位置信息得出的，且这两个采样时刻的领航机器人位置信息均建立在跟随机器人采样时刻&之 前的第^p,次采样时刻所对应的局部坐标系下。
6. 如权利要求1所述的多机器人协调队形控制方法，其特征在于， 所述D)步中的跟随机器人与领航机器人之间的距离《为^ 。
7. 如权利要求l、 4或5所述的多机器人协调队形控制方法，其特征
全文摘要
本发明一种多机器人协调队形控制方法，是一种跟随机器人通过递推预测，进而实现与领航机器人的队形控制。跟随机器人结合视觉、码盘信息，获取当前采样时刻k与之前的N<sub>p max</sub>次采样时刻相对于领航机器人的距离和观测方位角d<sup>k-s</sup>，φ<sup>k-s</sup>(s＝0，…，N<sub>p max</sub>)；借助码盘信息，通过局部坐标系变换，递推得到采样时刻k和k-N<sub>p max</sub>时的领航机器人在跟随机器人局部坐标系(见式1)下的位置L<sup>K</sup>(x<sub>L</sub><sup>k</sup>，y<sub>L</sub><sup>k</sup>)、(见式2)，以及采样时刻k时的跟随机器人前向摄像机的光心C<sub>opt</sub>在(见式3)下的位置O<sub>f</sub><sup>k</sup>(x<sub>of</sub><sup>k</sup>，y<sub>of</sub><sup>k</sup>)；计算出预测的领航机器人运动方向和领航机器人到C<sub>opt</sub>方向之间的角度ψ<sub>ij</sub><sup>k</sup>，进而结合期望角度ψ<sub>ijd</sub>以及跟随机器人与领航机器人之间的距离L<sub>ij</sub><sup>k</sup>(即d<sup>k</sup>)和期望距离L<sub>ijd</sub>，综合视觉信息，进行运动决策，得到速度v<sub>f</sub>和转角θ<sub>f</sub>。本发明适合通讯失效的情况，为多机器人在军事等方面的应用提供技术支持。
文档编号G05D1/00GK101685309SQ20081022277
公开日2010年3月31日 申请日期2008年9月24日 优先权日2008年9月24日
发明者超 周, 张文文, 曹志强, 丽 杨, 瑗 袁, 民 谭, 莹 马 申请人:中国科学院自动化研究所