基于摄像头的群体机器人的控制算法研究*

张宇超，魏洪兴，杨 博，蒋灿灿

(北京航空航天大学 机械工程及自动化学院，北京 100191)

0 引言

随着科学技术的发展，各领域对机器人应对复杂情况的多变性也提出了更高的要求，但传统机器人由于其自身结构的限制，只能完成一些目标确定、环境固定的任务。在此背景下，自组装模块化群体机器人应运而生，它是一种能够根据任务要求自动组装成不同构型的机器人[1]，能够在动态环境中完成不可预知的作业任务，并且已成为近年来机器人研究领域的热点之一。

1988年美国CMU大学研制的可重构模块化机械手系统是第一台可重构机器人的原理性样机[2]。之后在1999年，日产通商产业省工业技术院的Murate等人使用形状记忆合金研制出一种自重构系统M-TRAN[3]，该系统能用一组独立的机械装置构成不同形状。2005年之后是国外自组装与自重构模块化群体机器人的井喷时期，在这期间，Alice[4]、e-Puck[5]等各种群体机器人相继被研发出来，它们都具有自主运动和有限的感知能力。2006年出现的Swarm-Bot[6]机器人平台则是第一个具有真正意义上自组装功能的群体机器人平台。

我国在自组装模块化群体机器人方面也做了一些研究。2005年上海交大设计的自重构模块化机器人M-Cubes[7]和北京航空航天大学提出的可重构履带机器人[8]属于国内比较早的模块化群体机器人。从2010年至今，国内关于群体机器人的研究取得了大量成果，Sambot[9]、Ubot[10]、Transmote[11]、360bot[12]等相继出现，推动着群体机器人的研究发展。其中北京航空航天大学研发的Sambot机器人更是国内第一款具有完全自主移动能力的自组装模块化群体机器人。本文提出的Sambot Ⅱ机器人就是在原有的Sambot机器人基础之上，重新设计了控制系统，使用激光和摄像头作为视觉模块，从而实现更为强大的信息采集能力和自组装控制的机器人。

1 Sambot Ⅱ机器人平台

1.1 机械结构

Sambot Ⅱ机器人采用了机电集成设计方法，机器人的内部包含了控制系统、视觉模块、驱动模块等，具体参数如表1所示。

表1 Sambot Ⅱ机器人参数

如图1所示，Sambot Ⅱ机器人在结构上主要分为前后两块：前半部分的主动对接面和后半部分的移动本体。其中主动对接面上含有一对抓钩，可以与种子机器人移动本体上的前后左右四个被动对接面进行对接；当对接完成后，主动对接面还可以绕移动本体的中心进行±120°的旋转，从而实现多个Sambot Ⅱ机器人对接后的整体运动。单个Sambot Ⅱ机器人的运动依靠移动本体底部的两个差分驱动的塑料轮来实现。Sambot Ⅱ机器人主动对接面上还安装了高清摄像头和激光来实现机器人的避障和对接引导。

图1 Sambot Ⅱ机器人机械结构

1.2 控制系统

Sambot Ⅱ机器人的控制系统框图如图2所示，Intel Edison作为核心处理器，负责机器人的控制；STM32F103作为从处理器，对各个电机进行驱动控制；对接面板上的摄像头采集激光和LED灯信号发送给Edison，从而实现避障和导航等功能。

图2 Sambot Ⅱ机器人控制系统框图

2 视觉控制算法

Sambot Ⅱ机器人的一大特点就是群体自组装，为了后续能将自组装算法描述得更加精炼，我们需要引入一些概念来解释整个自组装过程。

在多个Sambot Ⅱ机器人自组装过程中，只有种子机器人内部含有目标构型的信息。种子机器人在整个自组装过程中都是静止不动的，对接机器人则处在运动状态，直到完成对接。当多个Sambot Ⅱ完成对接，但目标构型还没有完成时，该构型被称之为集合体机器人,只有完成了目标构型之后，自组装过程才算结束。

2.1 被动对接面的标识

Sambot Ⅱ机器人的移动本体包含前后左右4个被动对接面，如图3所示。其中被动对接面上相邻两个LED灯之间的水平距离为上方两个LED灯之间的距离X，相邻两个LED灯之间的垂直距离Y为左右两侧的垂直距离Y1和Y2的平均值。

主动对接面上每个LED灯有8种颜色情况。其中4个LED灯全黑色表示该被动对接面属于对接机器人，不需要进行对接；全白色表示该面属于集合体机器人，但不需要进行对接；LED灯发红可能会对Sambot Ⅱ激光测距产生干扰，因此每个LED灯实际上只有5种颜色可供使用。

该方案最多可以同时标识625个被动对接面，我们采用被动对接面右下角的LED灯进行对接方位的标识，其他LED灯用来标识Sambot Ⅱ的ID。简化后的算法最多同时支持125个Sambot Ⅱ机器人进行对接，可满足我们的实际需要。

图3 Sambot Ⅱ机器人被动对接面

2.2 摄像头测量距离和角度

我们采用激光测距和LED灯测距两种方法相结合来得到目标对接面和对接机器人之间的距离及夹角,各元器件的安装位置如图4所示。

图4 摄像头、激光和LED灯的安装位置

2.2.1 激光测距

由于实际加工和安装的误差，摄像头的光轴和激光的光轴都可能存在一定程度上的倾斜，综合考虑了上述问题而绘制出的激光测距原理图如图5所示。

图5中,α为激光光轴与水平线的夹角，β为摄像头光轴与水平线的夹角，y为主动对接面与目标对接面之间的距离，x为激光点与摄像头采集到的图片正中心在垂直方向上的距离所占用的像素个数，f为摄像头的焦距，z为摄像头镜片与主动对接面在光轴方向上的安装误差，e为摄像头光轴和激光光轴在主动对接面上的垂直距离。

由图5中可以推出：

(1)

其中，为了表示简洁将e-a-b用c代替，c本身不具有任何物理上的意义。

化简后可以得到:

Axy+Bx+Cy+D=0.

(2)

式(2)中各个系数项为:

(3)

由此我们可以得出激光点在图像中的像素个数与实际距离满足二元二次方程的关系，通过实验标定便可得到四个未知的系数A、B、C、D。

实际标定实验中，我们测量了三组数据，每组包含14个对应点，y从5 cm处等距增加5 cm直到y为70 cm，测量每次x的数值，其中y的原点为被动对接面所在的位置。三组数据求平均得到一组平均数，利用14个平均数得到一个有14个方程的超定方程组，用最小二乘法求出它的唯一解，从而得到了A、B、C、D四个系数的最优解。在此我们将化简后得到的二元二次方程列出来仅供参考：

xy+1.259 217 7x-249.474 32y-1 631.959 2=0.

(4)

从而可以得到:

(5)

我们在实际实验中发现，当目标对接面和主动对接面之间的距离在50 cm以内的时候，误差在0 mm～5 mm之间；距离超过50 cm之后，误差最大可达到15 mm，此时测出来的距离不具有参考价值。所以实际对接过程中测距范围应当缩减在50 cm以内。

2.2.2 LED灯测距

为了进行主动对接面和目标对接面之间夹角的计算，在此引入了LED灯测距方案。我们对测距原理进行了优化，忽略了摄像头与水平方向的倾斜角度，具体原理如图6所示。图6中,d为目标对接面上相邻两个LED灯之间的距离，x为d在采集到的图像中占用的像素个数。利用相似三角形可以得到：

(6)

化简后可以得到：

A1xy+B1x+D1=0.

(7)

式(7)中各项系数为：

A1=1,B1=z,D1=-f.

(8)

由此我们可以得出同一水平面内相邻两个LED灯之间的距离在图像中的像素个数与实际距离满足二元二次方程的关系，通过实验标定便可以得到两个未知系数B1和D1。

实际标定实验中，我们测量了两组数据，每组包含8个对应点，y从15 cm处等距增加5 cm直到y为50 cm，每次记录x的数值。两组数据求平均得到一组平均数，利用8个对应点得到一个有8个方程的超定方程组，用最小二乘法求出它的唯一解，从而得到了B1和D1的最优解。现将化简后得到的二元二次方程列出来仅供参考：

xy+0.015 512 100x-3 696.935 6=0.

(9)

从而可以得到：

(10)

由此可以看出x与y近似满足反比关系。该方程在实际算法中有利于快速估计实际距离，为角度测量做了铺垫。

2.2.3 角度的测量

由于绝大多数情况下对接机器人和目标对接面都是存在一定夹角的，所以就需要先进行角度的测量，才能顺利完成对接任务,角度测量原理如图7所示。图7中，θ为对接机器人与目标对接面之间的夹角，L为对接机器人与目标对接面之间的实际距离，d1为目标对接面上相邻两个LED灯之间的水平距离，s为d1在对接机器人方向上的投影。

图5 激光测距的原理图 图6 LED灯测距原理(俯视图) 图7 角度测量的原理

从图7中我们可以看出，水平距离d1由于存在投影的原因，所以θ值越小，s值越小。利用上述现象，我们可以通过测量目标对接面上相邻两个LED灯之间的垂直距离并利用公式(10)来快速估算出实际距离L。从而再次利用公式(10)反推出在此距离下水平距离的理论值d1，而测量的水平距离的实际值s与d1的比值则为sinθ。因此就可以较为快速地估算出θ的大小，应用于对接算法中。

实验验证的时候，我们测量了θ为75°、60°、45°和30°时4组数据，如表2所示，从表2可以看出，利用目标对接面上相邻两个LED灯之间的垂直距离计算出来的角度比实际角度偏大，最大误差达到11°左右，而且θ的值越大，误差也就越大。

总的来说，该算法只能粗略地提供角度信息，但是由于Sambot Ⅱ可以在一定角度误差范围内实现对接，所以实际的对接算法中还是用到了该角度测量的算法，具体运用将在2.3节中进行详细叙述。

表2 角度测量的实验结果

2.3 对接算法的设计

对接环境：Sambot Ⅱ机器人处在封闭式围栏环境中，该环境中除了Sambot Ⅱ机器人本身的LED灯和照明的日光灯以外，不存在任何其他光源。

我们将两个Sambot Ⅱ机器人的对接过程分为三个阶段进行规划，分别是漫游、导航和对接。

漫游的目的是让对接机器人找到种子机器人或者集合体机器人。将对接机器人放入实验环境中，打开摄像头，寻找LED光源。如果发现，就朝光源前进，直到机器人通过激光测距发现自己和种子机器人的距离在50 cm以内。没有发现，则随机旋转一定的角度，前进一定距离后继续旋转360°寻找光源，如此往复直到找到光源。一旦机器人找到光源并且距离在50 cm以内，就会执行LED灯测量角度算法，估计当前的夹角θ。然后机器人逆时针旋转θ，直线前进一段距离后，顺时针旋转直到重新找到光源，计算此时的距离和夹角。如此往复直到机器人移动到正对着被动对接面的直线轨迹上，检测该面是否为目标对接面，是目标对接面则进入对接算法，否则进入导航算法。

导航的目的是让对接机器人找到需要进行对接的目标对接面。当进入导航算法后，对接机器人会直线前进到达距离光源15 cm左右的位置。之后对接机器人会围绕着种子机器人进行逆时针运动，直到找到目标对接面。实现方法是对接机器人先顺时针旋转直到光源在图像中消失，前进一个体长，接着逆时针旋转直到重新发现光源并检测是否符合要求。如果符合要求就是发现了目标对接面，进入对接算法。否则继续顺时针旋转直到光源消失，进入下一次运动检测判断，如此往复直到找到目标对接面进入对接算法。

对接的目的是让对接机器人成为集合体机器人的一部分，从而实现完整的目标构型。当进入对接算法后，对接机器人会实时检测目标对接面上的4个LED灯，保证激光打在目标对接面的中心线上。当距离足够近的时候，对接机器人不再处理图像信息，而是保持方向不变，匀速直线运动直到机械触碰开关被压下，触发对接抓钩的锁紧行为，完成自主对接。当对接机器人完成对接并锁紧之后，会向种子机器人发送确认信号。种子机器人接收到该信号后会更新自身的节点信息，确认是否完成目标构型，从而判断自组装过程是否结束。

3 自主对接实验

为了验证算法的可行性，我们在原有的一代Sambot机器人的基础上进行了改装，安装了相应的传感设备从而制作成了Sambot Ⅱ机器人样品，如图8所示。由于一代Sambot的主动对接面板内空间有限，无法同时安装抓钩和摄像头，因此我们撤掉了抓钩，用主动对接面和目标对接面的重合度来表明机器人自主对接实验的成败。图9为两个Sambot机器人对接偏差示意图,因为Sambot的对接机构可以在如表3所示的误差范围内实现对接[13]，所以当重合度≥75%的时候都可以认为自主对接实验成功。

图8 Sambot Ⅱ机器人样品 图9 两个Sambot机器人对接偏差示意图

表3 两个Sambot机器人的对接允许偏差

实验在一个1 m×1 m的实验平台上进行。首先将种子机器人放在一侧中间的位置，然后放入一个对接机器人实现自组对接。该实验共重复10次，以估计Sambot Ⅱ机器人自主对接实验的成功率，实验的结果统计如表4所示。图10为其中一次实验的全过程，该过程共耗时46 s，最后对接完成时两个面的重合度达到了100%。

表4 自主对接实验结果统计

从表4中我们可以看出，利用摄像头、激光和LED灯进行信息交互的Sambot Ⅱ机器人在现有算法的控制下，一次对接的成功率在80%左右。证明了现有的对接控制算法的可行性。

4 结论

本研究基于一代Sambot机器人平台，重新设计出了采用摄像头、激光和LED灯进行信息交互的Sambot Ⅱ机器人，提出了一种用于群体机器人自组装控制的视觉算法，并用此方法成功地进行了2个Sambot Ⅱ机器人的自主对接实验。研究表明基于摄像头的视觉算法是可以用于群体机器人的控制的。

下一步的工作将制作出完整的配有对接抓钩的Sambot Ⅱ机器人，并改进现有算法，提高对接成功率。同时将开展多个Sambot Ⅱ机器人进行自组装实验的研究，从而实现真正意义上的群体机器人自组装控制。

图10 两个Sambot Ⅱ机器人自主对接实验过程

参考文献：

[1] 张海英,刘祚时,林桂娟.群体机器人研究的现状和发展[J].电子技术应用,2004,30(2):38-41.

[2] 王兵,蒋蓁.模块化重构机器人技术的现状与发展综述[J].机电工程,2008,25(5):1-4.

[3] Murata S,Kurokawa H.Self-reconfigurable robots[J].IEEE Robotics & Amp Amp Automation Magazine,2007,14(1):71-78.

[4] Caprari G,Siegwart R.Mobile micro-robots ready to use:Alice[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems.[s.l.]:IEEE,2005:3295-3300.

[5] Cianci C M,Raemy X,Pugh J,et al.Communication in a swarm of miniature robots:the e-Puck as an educational tool for swarm robotics[C]//International Conference on Swarm Robotics.[s.l.]:Springer-Verlag,2006:103-115.

[6] Gross R,Bonani M,Mondada F,et al.Autonomous self-assembly in swarm-bots[J].IEEE Transactions on Robotics,2006,22(6):1115-1130.

[7] 夏平,朱新坚,费燕琼.一种新型自重构模块机器人的机构设计[J].机械设计与研究,2005,21(6):40-42.

[8] 王田苗,邹丹,陈殿生.可重构履带机器人的机构设计与控制方法实现[J].北京航空航天大学学报,2005,31(7):705-708.

[9] Wei H,Chen Y,Tan J,et al.Sambot:A self-assembly modular robot system[J].IEEE/ASME Transactions on Mechatronics,2011,16(4):745-757.

[10] 赵杰,唐术锋,朱延河,等.基于万向式关节的模块化自重构机器人[J].机器人,2010,32(5):608-613.

[11] Qiao G,Song G,Zhang J,et al.Design of transmote:A modular self-reconfigurable robot with versatile transformation capabilities[C]//IEEE International Conference on Robotics and Biomimetics.[s.l.]:IEEE,2012:1331-1336.

[12] 曹燕军,葛为民,张华瑾.一种新型模块化自重构机器人结构设计与仿真研究[J].机器人,2013,35(5):568-575.

[13] 魏洪兴,刘淼,李德忠,等.一种新的自组装模块化群体机器人——对接机构设计与自组装控制[J].机器人,2010,32(5):614-621.