SCIENCE CHINA Information Sciences, Volume 62 , Issue 5 : 052205(2019) https://doi.org/10.1007/s11432-018-9681-3

Formation building and collision avoidance for a fleet of NAOs based on optical sensor with local positions and minimum communication

More info
  • ReceivedMar 5, 2018
  • AcceptedOct 31, 2018
  • PublishedApr 3, 2019



This work was supported by National Natural Science Foundation of China (Grant Nos. 61571478, 61601428, 51709245, 51509229).


[1] Gu D. A game theory approach to target tracking in sensor networks.. IEEE Trans Syst Man Cybern B, 2011, 41: 2-13 CrossRef PubMed Google Scholar

[2] Yaochu Jin , Hongliang Guo , Yan Meng . A hierarchical gene regulatory network for adaptive multirobot pattern formation.. IEEE Trans Syst Man Cybern B, 2012, 42: 805-816 CrossRef PubMed Google Scholar

[3] Ahmad M D, Kasim M A A, Mohammed M A, et al. Multi-robot system for real-time sensing and monitoring. In: Proceedings of the 15th International Workshop on Research and Education in Mechatronics (REM), El Gouna, 2014. Google Scholar

[4] Arai T, Pagello E, Parker L E. Guest editorial advances in multirobot systems. IEEE Trans Robot Automat, 2002, 18: 655-661 CrossRef Google Scholar

[5] Li X, Zhu D, Qian Y. A Survey on Formation Control Algorithms for Multi-AUV System. Un Sys, 2014, 02: 351-359 CrossRef Google Scholar

[6] Benzerrouk A, Adouane L, Martinet P. Stable navigation in formation for a multi-robot system based on a constrained virtual structure. Robotics Autonomous Syst, 2014, 62: 1806-1815 CrossRef Google Scholar

[7] Balch T, Arkin R C. Behavior-based formation control for multirobot teams. IEEE Trans Robot Automat, 1998, 14: 926-939 CrossRef Google Scholar

[8] Sun J, Tang J, Lao S. Collision Avoidance for Cooperative UAVs With Optimized Artificial Potential Field Algorithm. IEEE Access, 2017, 5: 18382-18390 CrossRef Google Scholar

[9] Loria A, Dasdemir J, Alvarez Jarquin N. Leader-Follower Formation and Tracking Control of Mobile Robots Along Straight Paths. IEEE Trans Contr Syst Technol, 2016, 24: 727-732 CrossRef Google Scholar

[10] Oh K K, Park M C, Ahn H S. A survey of multi-agent formation control. Automatica, 2015, 53: 424-440 CrossRef Google Scholar

[11] Alonso-Mora J, Breitenmoser A, Rufli M, et al. Optimal reciprocal collision avoidance for multiple non-holonomic robots. In: Proceedings of the 10th International Symposium on Distributed Autonomous Robotic Systems, Lausanne, 2010. 203--216. Google Scholar

[12] Paulos J, Eckenstein N, Tosun T. Automated Self-Assembly of Large Maritime Structures by a Team of Robotic Boats. IEEE Trans Automat Sci Eng, 2015, 12: 958-968 CrossRef Google Scholar

[13] Xia Y, Na X, Sun Z. Formation control and collision avoidance for multi-agent systems based on position estimation.. ISA Trans, 2016, 61: 287-296 CrossRef PubMed Google Scholar

[14] Nikou A, Verginis C K, Dimarogonas D V. Robust distance-based formation control of multiple rigid bodies with orientation alignment. 2017,. arXiv Google Scholar

[15] Oh K K, Ahn H S. Distance-based control of cycle-free persistent formations. In: Proceedings of IEEE International Symposium on Intelligent Control, Denver, 2011. 816-821. Google Scholar

[16] Bartlett S L, Hampapur A, Huber M J, et al. Vision for mobile robots. In: Image Technology. Berlin: Springer, 1996. Google Scholar

[17] Fredslund J, Mataric M J. A general algorithm for robot formations using local sensing and minimal communication. IEEE Trans Robot Automat, 2002, 18: 837-846 CrossRef Google Scholar

[18] Gouaillier D, Hugel V, Blazevic P, et al. Mechatronic design of NAO humanoid. In: Proceedings of IEEE International Conference on Robotics and Automation, Kobe, 2009. 769-774. Google Scholar

[19] Wang X M, Zerr B, Thomas H, et al. Pattern formation for a fleet of auvs based on optical sensor. In: Proceedings of OCEANS-17, Aberdeen, 2017. Google Scholar

[20] Kim Y, Mesbahi M. On Maximizing the Second Smallest Eigenvalue of a State-Dependent Graph Laplacian. IEEE Trans Automat Contr, 2006, 51: 116-120 CrossRef Google Scholar

[21] Alonso-Mora J, Breitenmoser A, Rufli M, et al. Multi-robot system for artistic pattern formation. In: Proceedings of IEEE International Conference on Robotics and Automation, Shanghai, 2011. 4512-4517. Google Scholar

[22] Yu S, Barca J C. Autonomous formation selection for ground moving multi-robot systems. In: Proceedings of IEEE International Conference on Advanced Intelligent Mechatronics, Busan, 2015. 54--59. Google Scholar

[23] Benozzi L. A detection strategy for line formation of a team of humanoid robots. Dissertation for Master Degree. Florence: University of Florence, 2017. Google Scholar

[24] Aldebaran: Nao software 1.14.5 documentation/hardware/nao technical overview/joints. 2013. http://doc.aldebaran.com/1-14/family/robots/joints~robot.html. Google Scholar

[25] Aldebaran: Nao software 1.14.5 documentation/reference/naoqi api/naoqi vision/alvisualcompass. 2013. http://doc.aldebaran.com/1-14/naoqi/vision/alvisualcompass.html?highlight=compass. Google Scholar

[26] Fujinaga N, Yamauchi Y, Ono H. Pattern Formation by Oblivious Asynchronous Mobile Robots. SIAM J Comput, 2015, 44: 740-785 CrossRef Google Scholar

[27] Aldebaran: Nao software 1.14.5 documentation/hardware/nao technical overview/contact and tactile sensors. 2013. http://doc.aldebaran.com/1-14/family/robots/contact-sensors~robot.html. Google Scholar

[28] Aldebaran: Nao software 1.14.5 documentation/hardware/nao technical overview/sonars. 2013. http://doc.aldebaran.com/1-14/family/robots/sonar~robot.html. Google Scholar

  • Figure 1

    (Color online) One NAO humanoid robot.

  • Figure 2

    (Color online) Sketch of a planar pyramid pattern.

  • Figure 3

    (Color online) The examples of the same NAO's visual features captured with different relative orientations $\theta$. (a) $\theta~=~0$, $\varphi~=~0$; (b) $\theta~=~90$, $\varphi~=~0$; (c) $\theta~=~120$, $\varphi~=~0$; (d) $\theta~=~180$, $\varphi~=~0$; (e) $\theta~=~180$, $\varphi~=~90$; (f) $\theta~=~320$, $\varphi~=~0$. $\theta$ is the orientation, the angle between the torso and the north directions, while $\varphi$ is the heading, the angle of head yaw.

  • Figure 4

    (Color online) Image segmentation for recognizing an NAO: detection of the orange tuples. (a), (c) The origin color images; (b), (d) the corresponding segmented images.

  • Figure 5

    (Color online) The generation of the M-base in the software “V-rep".

  • Figure 6

    Some masks of the M-base corresponding to the (a)–(d), (f) of Figure 3. (a) $\theta=0$; (b) $\theta=90$; (c) $\theta=120$;protect łinebreak (d) $\theta=180$; (e) $\theta=320$.

  • Figure 7

    (Color online) The tuples extracted from one image. (a) One image got from the forehead camera; (b) the binary image after segmentation of image with orange color in the HSV space; (c) enhanced image with one Gaussian filter and the closing and opening operations of morphological operations.

  • Figure 8

    The corresponding feature points in two adjacent images. Left image stands for the current image, while the right one is the latest image.

  • Figure 9

    (Color online) The NAO frame (NAO-$i$), the neck frame (neck-$i$) (a) and the local frame (local-$i$) (b). In the NAO-$i$, the $x$ axis is looking forward in the horizon plane, $y$ axis is perpendicular to the $x$ axis in the horizon plane, the origin point is the mass center, and $z$ axis is perpendicular to the $xy$ plane. The neck-$i$ is parallel to the NAO frame. In the local-$i$, the origin point and its $y$ axis coincide with the origin point and $x$ axis of the NAO-$i$ respectively, and its $x$ axis is along the inverse direction of the $y$ axis of the NAO-$i$.

  • Figure 10

    (Color online) Camera model. $o_{i}u_{i}v_{i}$ is the image frame, $u$ axis and $v$ axis are along the width and height of the image respectively. $o_{ci}x_{ci}y_{ci}z_{ci}$ is the camera frame of forehead camera of the NAO-$i$, where $o_{ci}$ is the optic center. Its $z$ axis is along the direction of optic axis and the $z$ axis is parallel to the $x$ axis of the NAO-$i$. $x$ axis and $y$ axis are parallel to the $u$ axis and $v$ axis, respectively. $P_{ij}$ is the mass center of NAO-$j$ in the NAO-$i$. $f$ is the focus of forehead camera of NAO-$i$. $A_{ij}$ and $A_{uv}$ are the projections of one aspect of NAO-$j$ in the plane $o_{ci}x_{ci}y_{ci}$ and in the CCD $o_{i}u_{i}v_{i}$. The distance between NAO-$i$ and NAO-$j$ is $d_{ij}$.

  • Figure 11

    (Color online) The relationship between the origin points of the frames fixed-$i$ ($o_{fi}x_{fi}y_{fi}$) and fixed-$j$ ($o_{fj}x_{fj}y_{fj}$).

  • Figure 12

    (Color online) The illustration of Lemma 1. If the points choose the short distances as the edges, the edges will be never intercrossing. (a) The general situation; (b) the situation with all the points are alligned. In this case, $d_{e_{1}}+d_{e&apos;_{1}}=~d_{e_{2}}+d_{e&apos;_{2}}$, but if ${\rm~max}(d_{e_{1}},d_{e&apos;_{1}})~<~{\rm~max}(d_{e_{2}},d_{e&apos;_{2}})$, we regard the $e_{1}$ and $e&apos;_{1}$ are not intercrossing.

  • Figure 13

    (Color online) Common frame ($o_{c}x_{c}y_{c}$) and pyramid frame ($o_{p}x_{p}y_{p}$). NAO-$n$ is assumed to be the robot with the maximum number of neighbors. $o_{c}x_{c}y_{c}$ is the frame fixed-$n$. Here, $o_{c}x_{c}y_{c}$ presents the orientation but not the real position, while $o_{p}x_{p}y_{p}$ is located at the correct position. Its $y$ axis $o_{p}y_{p}$ is the principal axis. Origin point $o_{p}$ is the average value of all the positions in the $o_{c}x_{c}y_{c}$. $\beta$ is the angle between the $o_{p}y_{p}$ and the $o_{c}x_{c}$.

  • Figure 14

    (Color online) The generation of straight and non-intercrossing trajectories in the pyramid frame. Except the leader (the NAO nearest to the first place in the pyramid pattern), the positions in the initial distribution and in the pyramid pattern are divided into the combined sub-groups with one or two positions respectively according to their subscriptions ordered by their coordinations (a), (b). Then in each combined sub-group, they match up depending on Lemma 1. The straight and non-intercrossing trajectories are planned, noted as $(\eta_{i},~d_{i})$ (c).

  • Figure 15

    (Color online) The evaluation of the relative distance errors and relative angle errors between each follower and the followed robot at the convergent state of pyramid built by 10 robots with the relative distance noises $\delta_{d_{ij}}\in~\pm~0.1d_{e}$, and the planned trajectory noises $\delta_{d_{i}}\in~\pm~0.1d_{e}$ and $\delta_{\eta_{i}}\in~\pm~0.05\theta_{e}$. (a) Relative distance errors; (b) relative angle errors.

  • Figure 16

    (Color online) The examples of building pyramids with 6, 10, 15 robots respectively with the relative distance noises $\delta_{d_{ij}}\in~\pm~0.1d_{e}$, and planned trajectory noises $\delta_{d_{i}}\in~\pm~0.1d_{e}$ and $\delta_{\eta_{i}}\in~\pm~0.05\theta_{e}$. (a), (c), (e) are the initial distributions (the red points) with 6, 10, 15 robots respectively, while (b), (d), (f) are the corresponding trajectories (red lines) and the built pyramid patterns (the points with black edges). The only one green point with black edge is the leader in the pyramid patterns.

  • Figure 17

    (Color online) One example of building a pyramid pattern in the static obstacle environment (8 robots and 7 obstacles) with relative distance noises $\delta_{d_{ij}}\in~\pm~0.1d_{e}$, planned trajectories noises $\delta_{d_{i}}\in~\pm0.1d_{e}$, $\delta_{\eta_{i}}\in~\pm~0.05\theta_{e}$, and translation trajectory noises $\delta_{d_{ti}}\in~\pm0.05d_{e}$, $\delta_{\eta_{ti}}\in~\pm0.05\theta_{e}$. The red points are the robots, while the blue points are the obstacles. (a) The initial distribution of robots and obstacles; (b) the basic pyramid built by considering the obstacles as the robots, where the black edge circles and dotted black edge circles are the positions in the pyramid pattern corresponding to robots and obstacles respectively; (c) the basic pyramid moving out of the obstacle area along the planned obstacle-avoided translation trajectories; (d) the basic pyramid at the obstacle-free area moved out from (c); (e), (f) the process of correcting and building a final pyramid pattern by the followers moving to the positions in the pyramid pattern corresponding to the obstacles layer by layer.

  • Figure 18

    (Color online) Experiments environment with enough space and many features. (a) Experiment environment taken from front to back; (b) experiment environment taken from back to front.

  • Figure 19

    (Color online) An example of fitting curve of distance model with $\theta~=~180$. Here, $f(x)$, $x$, $p_{1}$, $p_{2}$, represent $d$, $\frac{1}{r_{g}}$, $a$ and $b$, respectively.

  • Figure 20

    Two masks with “side" posture got in the real environment under two different kinds of lighting. The posture parameters are: the heading $\varphi=90^\circ$, the distance $d_{s}=0.6$ m, and the orientation $\theta=270^\circ$.

  • Figure 21

    (Color online) Comparison of the rotation behavior performed by the NAO itself and by the vision compass. (a) The initial position; (b) and (c) are good and bad rotation results by the NAO itself; (d) and (e) are the rotation results by the visual compass.

  • Figure 22

    The situations of leading to wrong estimations of orientations by visual compass. (a) Few features in one or two images; (b) similar features in two images; (c) large rotation step length ($40^\circ$).

  • Figure 23

    (Color online) The results of NAOs moving $0.8$ m along a line (a chalk line, rewritten with red dashed line). (a)–(d) The movement results performed by the NAO itself; (e)–(h) the movement results performed by the distance control. (a) and (e) are the initial postures of both tests; (b) is the posture of moving forward 20 cm; (c) and (d) show the deviation of the movement by the NAO itself; (f) is the posture of moving forward 40 cm after the second step with $d_{\rm~stp}=20$ cm; (g) presents the deviation of the orientation even moving forward 20 cm; (h) is the correction result of (g) by using the orientation feedback of distance control at the end of this step.

  • Figure 24

    (Color online) An example of pyramid building. (a) Initial distribution of NAOs; (b) the NAOs search neighbors at $\theta~=0^\circ$ with the $\varphi~\in~[0^\circ,119^\circ]$; (c) the NAOs search neighbors at $\theta=0^\circ$ with the $\varphi~\in~[-119^\circ,0^\circ]$; (d) the NAOs are rotating to $\theta~=180^\circ$ from $\theta~=0^\circ$ with the rotation control; (e) the NAOs search neighbors at $\theta~=180^\circ$; (f) NAOs are exchanging information among NAOs by WiFi through the PC, the leader and followers are identified, and the trajectories are planned well with the proposed method; (g) the leader is rotating to the destination with the rotation control; (h) the leader has arrived at its destination with the distance control; (i) the leader rotates to the pyramid orientation and the followers begin rotating to face to their destinations with the rotation control; (j) follower_2 arrives at its destination, while follower_1 still rotates to its destination; (k) followers_2 has rotated to the pyramid orientation and follower_1 has arrived at its destination; (l) after follower_1 rotates to the pyramid orientation, the pyramid building is achieved.

  • Table 1   Part of coefficient pairs of D-base with different orientations
    $a$ $b$ $\theta$
    0.5044 0.1095 0
    0.5167 0.09141 5
    0.5146 0.09774 10
    0.5068 0.1037 15
    0.5032 0.1053 20
    0.4908 0.1258 25
    vdots vdots vdots

    Algorithm 1 Recognition of NAOs with all the tuple groups in one image


    is the evaluation of an NAO existence or not in this image.

    for $g_{j}[x,y]$ in tuple groups

    for mask in M-base

    for scale ($r_{g_{j}}$) in range $[0.01,1.5]$ with a step $0.01$

    dim = size of mask $\times$ $r_{g_{j}}$;

    if the size of dim is smaller than the size of the tuple group $g_{j}[x,y]$ then


    end if

    end for

    mask_new = mask $\times$ dim, to change the size of mask to become smaller than the size of $g_{j}[x,y]$;

    $\lambda_{g_{j}}$ is calculated by comparing the mask_new with the tuple group $g_{j}[x,y]$, and is stored in the list $L_{\lambda}$;

    end for

    $\lambda_{g_{j}}~=~{\rm~max}\{L_{\lambda}\}$ is the evaluation of $g_{j}[x,y]$ and is stored in the list $L_{\lambda_{\rm~max}}$;

    end for


    Algorithm 2 Identification of the IDs of neighbors with the neighbor-check method

    for $i$ in all the NAOs

    for $j$ in all the neighbors of NAO-$i$

    for $k$ in all the NAOs

    if $k!=i$ then

    for $n$ in neighbors of NAO-$k$

    if $\varepsilon_{xjn}$, $\varepsilon_{yjn}$, $\varepsilon_{djn}$ meet (9) and $\varepsilon_{djn}$ is the minimum value then

    ID of neighbor-$j$ of NAO-$i$ is $k$;

    ID of neighbor-$n$ of NAO-$k$ is $i$;

    end if

    end for

    end if

    end for

    end for

    end for

  • Table 2   The moving distances and associated orientation errors
    Distance (m)$e_{\rm~orien}$ (radian) Distance (m)$e_{\rm~orien}$ (radian)
    0.2 $-$0.0211 0.5 0.1698
    0.2 0.0957 0.7 0.5292*
    0.3 0.3734* 0.7 0.2274
    0.3 $-$0.0379 0.8 0.334
    0.4 0.0125 0.8 0.4465
    0.4 $-$0.1579 1.0 0.4493
    0.5 0.1374 1.0 0.4062