5
Maintaining the relative positions and orientations of multiple robots using vision Ant onio Paulino * , Jorge Batista, Helder Ara ujo Department of Electrical Engineering, Institute of Systems and Robotics, University of Coimbra-Polo II, 3030 Coimbra, Portugal Abstract To keep several robots in geometric formation several issues have to be considered. This paper describes an ex- perimental system for controlling multiple robots in geometric formation. This system has a mixed control structure, where both a central coordinator is used and also each robot has a high level of autonomy. The relative locations of the robots are estimated using vision. The central coordinator is used to decide the type of formation and to inform the robots about the formation type. Each robot is autonomous to make any decisions relative to the control of its position and velocity. Any of the robots can act as the central coordinator, but at any instant of time there is only one coor- dinator. The type of geometric formation can be dynamically changed and it depends on the overall goal of the system or on dynamic changes of the environment. To keep the robots in geometric formation their relative positions and orientations have to be known and they are estimated using vision. Ó 2001 Elsevier Science B.V. All rights reserved. Keywords: Mobile robots; Vision; Pose estimation 1. Introduction Robot cooperation and coordination can be useful in dierent types of applications Werger, 1998). In most cases the cooperation of multiple robots requires that their relative positions and orientations are known Kurazume and Hirose, 1998). Some of the mobile robot cooperation ap- plications involve keeping the robots in geometric formation. Examples of such applications range from ®re ®ghting to military/security tasks and game playing Balch and Arkin, 1998). Formation is a technique that is used to ensure that an area or region are eciently covered by a set of robots, policemen, soldiers or players. For example in the MARTHA project Alami et al., 1998) several re- sults of co-operative mobile robotics were explored and each robot acts almost fully autonomously. Keeping the robots in formation may imply to keep constant their relative positions only or both the relative positions and orientations Wang, 1991). The geometric pattern can also change dy- namically as a function of the task Yamaguchi, 1997; Lawton et al., 2000). In general several sources of error prevent the relative positions and orientations from being constant. To keep for- mation it is therefore essential to be able to mea- sure deviations from the prede®ned pattern of relative positions and orientations. One important aspect of this problem is the control of the geo- metric formation Sugihara and Suzuki, 1996; Pirjanian and Mataric, 2000). One of the main www.elsevier.com/locate/patrec Pattern Recognition Letters 22 2001) 1331±1335 * Corresponding author. Tel.: +351-39-796200; fax: +351-39- 406672. E-mail address: [email protected] A. Paulino). 0167-8655/01/$ - see front matter Ó 2001 Elsevier Science B.V. All rights reserved. PII: S 0 1 6 7 - 8 6 5 5 0 1 ) 0 0 0 7 8 - 2

Maintaining the relative positions and orientations of multiple robots using vision

Embed Size (px)

Citation preview

����������� ��� ������ � ���� �� ��� ������� �� � ������ � �� ����� ���� �

���� �� ����� �� � �� �������� ���� ����� ���������� ��� ��� �� ������������ ��������� ������� ��� ���� �� ���������� ��������� ��� ���� ������� �������

��������

� ���� ����� � �� �� �� ����� � ���� � ����� ������ ���� � �� � �������� ���� ���� �������� �� � !

�������� �"���� � � �� ��� ������ � �� �� �� ����� � ���� �� ���� �"���� ��� � �� �� � �� ��������

#��� � �� � ����� � ����� �� ���� ��� �� ���� � � ��� � ���� ��� � ��� � �"� ��� ������ ���� �� � ���

� �� �� ��������� ����� ���� �� ��� ����� � ����� �� ���� � ������ ��� �"�� � � ���� � ��� � ��� � ���

� �� �� �� ��� � ���� � �"��� $��� � � �� ��� � � �� � ���� ��" ������ �� ������ � ��� � �� � ��� � ���� �

��� �� ���"� ��" � ��� � �� ��� ��� �� ��� ����� � ����� � ��� �� ��" ������� � ���� ���� �� �" �� � !

����� � ��� �"�� � �� ����� � ���� � ��� �� �"������" ������� ��� �� ������� � ��� ��� � � � ��� �"����

� �"����� ������� � ��� ���� ������ � ���� ��� � �� �� �� ����� � ���� � ���� ������ � ���� �� ���

������� �� ���� � �� �� #� ��� ���" �� ��������� ����� ���� �� � %&&' $����� (������ ��)� � ����� �������

��!���" � ��� � ��* )��� �* � �� �������� �

� ���� ������

+ � � � ����� � ��� � ������ � ��� ������� �� ��,���� �"��� � �������� �� -.����'//01� 2� � �� ����� ��� � ����� � � ������ � �� �3���� ���� ���� ������ � ���� �� ��� ������� �� �� �� #� -4��5��� ��� �� ���'//01� ( �� � ��� � ��� � � � ����� � ��!������ �� ��� �� ������� ��� � �� �� �� ������ ���� �� $ ����� � ���� �������� �� ����� � 6� 6������ � �����"7������" ����� ������� ��"��� -���� ��� ����� '//01� 8 ���� ��� � ������3�� ���� �� ���� � ����� ���� �� ��� ��� � �� �9�����" � ���� �" � ��� � � ���

� ������� � ���� ��"��� 8 � ���� �� �����+��� � ���� -���� �� ��� '//01 ����� �!���� � � ! ������� � ��� � ���� #�� � � ����� ���� � � ���� �� �� ��" ��� � � ��"�4������ ��� � �� �� � ���� � ��" ���" � ���� � ������ ���� ������ � ���� �� �" � ����� ������ � ���� �� ��� ������� �� -.����'//'1� ��� �� ����� ������ ��� �� ������ �"!������" �� � ������ � � ��� ���� -:���������'//;* <�#� � �� ��� %&&&1� 2� ����� ������ ���� � � ������ ��� ������ � ���� �� ��� ������� �� � � ����� � ������� � ���� � !���� � �� �� ����� � �������� � �� ��� � ���!��� ������� �� � � ��� ����6��� ������ ������� � ���� �� ��� ������� ��� =�� ��� ���������� � ���� � ��� �� ��� � �� � ��� �� !����� � ���� � -(������ ��� (�5���� '//>*�������� ��� ������� %&&&1� =�� � ��� ����

###��������� �7 ����7�����

������ +�� ����� � <����� %% -%&&'1 '??'@'??A

�B ��� ����� ���� � ���C D?A'!?/!;/>%&&* �� C D?A'!?/!

E&>>;%�

������ �������" ����� F�������� -�� ����� 1�

&'>;!0>AA7&'7G ! ��� � �� ����� � %&&' $����� (������ ��)� � ����� �������

�22C ( & ' > ; ! 0 > A A - & ' 1 & & & ; 0 ! %

������ �� ���� � ��� �� ��� � ������� � � ��������� � ���� 2� �" ��� ������ � ���� �� ��� ������� ����� ���� � ��� �� ���� ����"H�(� ������ ���� �� 2� � �� ������ � ���� ����� ������� �� �� � �� ���� � ������ ����� �������� �" ������ ���7 ���� � ���� � ��� ������� � #��� ������ ���� � #��� � ����� �������" � #��� � ���� � � �� ���� � � !������ ��� �� � ���� �� ��" � �������� � #� �� ��������� �� ��� ��� � ���� � ���� �� 2� ������� � ��� �"���� �������� �� ���� ���� #� ������� � �� ��� ���� � ��� ���������� � ��� � �� � ��� ��� � ���� ���

� ��������� �������

��� � �� � � ���� � ��� �� ��� �������� ������5�� � �� � ���������� � �� �������� � �� ���� ���������� ��� ��������!����� ��� �� � ���� ��� � �� ������� ���� ��� ���� �� � ����� � ����� ���� ��� ��������� �"������" ��� ���� � � ��� � ��������� � ��� � �"� 2� ��� ����� � � � ���7���� � ��� ����� � ����� �� ��� � � #���� �� ��� ����� � ����� � 2� ���� � ����� �� � ����� � ����� � ��������� ������ �� BB� �� ��" ������� � ���� �" �� BB ��������� ��� � � �� ��� ��� � ���� � �� �� ���!����" � � � ��� ������� 8 ���� ��� �� ���BB ��6��� ��� ������ � ���� � � ���� � � ��#� �� ��� ��3����� � � ���� � �������� 2� �!������ ��� � ���� �� � ���� �� � ��� � �� ���� ������� � ��� � ���� �� � � ��� � ��� 2����� �� �� ������ �� ���� � �� ��� �� ��" ������� � ���� �" �� � ���� ���� �� ��� � ����� ���� # ��� � �� �� ��������5�� -���� � ���� ��� #� ���� � � �� � �� � ���� �1�

I���� ��� �������5��� � � ��� �"����� ���� � � ��6��� ��� #� # � � ������ �"��������� # � � ������ �"���� �� ���� �" ���� !� � � ���� ����� ����� � ���"� ��� �� � ���� ��� � �� � ��� � �� -����� ���� �1�4� #��� ��� � ���� �� � ��� � ��� ��� ��������� ������� ������ ��6��� �" ��� BB� ���� !� � ��� � ����� ��� ������� � ���� �� �������� � ��� � ���� � � � J���� ���� ��� �������� � � �� � �����" ����������� � ��� �����

#���� �� ����� �� ���� � � ����� #� �� ���� ��������� � �"�����

� � �� � ���� � �� �� �������" � � ��������� � � � �� � � B ����� ��� � #� � ��� 2�� � �� � � ��� �� �� �� #� �� ��� ���� � ���� ��< � ��<� �<� �<�� ��� ��� ������� � ���� ���� � � ���� � � ��� ��� ������ � ��� � ���� � ������� �� #�� ��� � ���� �� ����" ��� ��,�!���� ��� � �< � ��� � � � � � ����� ���� � ���� #� �� 8��� ' ��� �� �������� �� ��� � �# � � ������ �"����� ��� � �� ����"������������ ��� �� ���" � ������ � ����� ��� �! ��� � ��� ������� �� ��� ���� �� � ���� ��� ��� �� ���� � ����� ��� � ��� ������������ �� ��� ���� �� �� ������� �� ��� �������� �� ������ �� ���� � �� �� ����� � �2I�� � ����� �� ��� ��� �� �� �������" � � �������� �� ��� � � � ������ ����� �" ����� � � ���� � � �� � ��� ��� � � # � ��C

�+�� � � ������� � ��������� ��+�� � � ��������� � � ������� �

�'�

��� ������� �� ��� ���� �� � � ��� ���� ��� ������C

�� ������������������������������������+�� �

+��

� �+�� �+��

��

�� � ������+���+��

��%�

� �2I �� ���� � � �� ��� ������� �� ��� ���� �� �� ������� ����� �� ������ �� � �!

8��� '� )��� � ���� � � ���� � ������������

'??% $% ������ �� ��% & ������� �� ������ '������ (( )(��*+ *��*,*��-

����� ��� ��� � � � ��� 2� ��� ���� � �����"����� �� �� � �# !#��� ��,������ ���� � ���2� ��� ���� � ��� -��� .���� '//'1 ���� ������6���� � � ��� � ���� ���� � � ����� ���� �� ��� �"����� ����� � ��� � �� �#�"�� ����� � ��� ������ � ���� � �� ��� � ���� ��2� ��� ���� � � � ������ ��� ���� � ���� ��������� ��� �"���� ��� ��� ��� -� ���" ����������1 � ���� � � ��� � �� � ��� �"���������� � � � �� #� ��� ������� � ���� � � ��� ��� � ���

� ��������� �������� ��������� �� ������������

$��� � � ��������� ��� � �� ������ � � ����!�� ������ � � ���� ������ �� ���� � ����� � � ������6���� � ��� �� � �� �������� �� 8 ���� ��� �� ���� � � ����� � ���� ����������� � ��� ������ �������� �� 8��� %� � ��!� � � �� �������� � ��� � � ������� � ��������� ���� � �� ���� ��" ��������� ���" ���������� ����� ��� ���� � �������� �� -��������� ��� '///1� � �� �������� � ����� � ��� ����� �� �3���� ��� ���������� � � ��� ������������ ��� ���� ������� ���� ��" �� ���������� � �� ����� �� ���� ������ ���� ����� ��!���5��� �� � �� � ������� � �� ��� ��� �� ���� ������ � ��� � � ��� � �� �� �� ��� ����� ������ ���������� ��� ������ � �� ��� #���� �������� -��� ���� ������ �� ���� �� � ��������� � � #���� ����� ���1 �� �� �� ���

� ������ � ��� � �� ��� ��� ���������� � ������� � ����!�3���� ����� 6������ ����� ��� �!� ���� �������� �� -8��5���� � �� ��� '///1� ���� ������� � � ��� ������ �������� �� ����� ���� � ��� � � � ������5�� ���������� � �������� ��� ����� 6����� � ����� ��� ���������� �� �� �� � ������ ��� ��� ����� � ���� ���"�� � ��!������� ������� ��� ����� � �� ��� ����� ����� � 6��� �� ��������� ������ ���� � � �� ��� �� � ��� ���� ������� ���� ���5������ ������ �� ��� ������ � �� ��� �� � ������� 2����� �������� � #� ������ �" �� ����� ����� � �� ���� �� ������� � ����� ��� ���������� � ���� � �� ��� ������� �� #���� ��!������ ��� ���� � � ��� ����� � ���������� ��� ��� ������� � ������ ����� ���� ���������� ����� ���� � � ��� �" ���� ��� ��� ��� � � ./ ����� 2� #� ��6�� ��� �������� ������ ���� �� ���� � #�" ���� ��� ./ ������ ���� � ��� K � #���� ��� � � � ���� �" �� ���� � ���� ����� � �� � ������������ ���� � ���� � ��� ��� � ��� ���� �� ��� ��� # � ��� 2� ���� ����� ��� ���� � ���� ���#��� ��� ����� � ������ ���� ��� ���������� ���� �� �������� �" �" �� �����!��� ��� ����� � ����� � �3���� � ������ ������ ������� #��� ��� � ��� ����� ?I � ��!����� ��

� �� �� ' �� � � � �� � ' ��� �?�#��� � �� � ? E ���� ���� ���� � �� � � � � � ����� #��� � �� � ���� ���� � � �� ������� � ��� ����� �������� ��������� � �� ���� ����� � ���� � � �� ��� ���� � ���� ��� � ����� ������� � ���� � �� ��� ����� � ����������� �������� ���� ��� � ���� � ��� ���������� ������� �� ����� �� ��� � � & �������� � ����� � �3���� � ��� �� �#����� ��

� � � �� �� ' �� � � � � � ' ��� �E�#��� ���� � �� � ? ? ���� � 2� � ���� -�������� ��� ����� �� ��������1 ���� ���� �� ������� � � ��� ���� ������� � �������� ������ #! ���� � ���� �� �����" ��� ���� ����� #�� �� � � � ���� � �� � � � �� J�!��� � ����!�3���� ��� ��� ��� 6�� ���� #�� ��� ������� ��� ����� ��������� � ��� ������� �

8��� %� (������ � ��� ���� ������ ���� � ���� ���

�������" ��� � ���

$% ������ �� ��% & ������� �� ������ '������ (( )(��*+ *��*,*��- '???

���� ���� � � ������" ��� �������"� � #������ ����� ��������� � � �� � �� ��������� ����������� 2� ����� #��� � �� � �� � 5� � ��� ����!����� �� ���� � � �,����� �" � ��� ���� � ��" ��� ���� � ������ (���� ����� � � ���!��6����" �,��� ��� ��� ����� � ��� � ���� �� �� �� ������ �" ��� ����� � ������ � ������� � �� ����� ��� ������� � ���� �� � ��,����� �" ��� ���� ���#��� ��� � ��� ��� ����!� � ��� ��������� ��� �� ���� � ��� � ����� ����!���� � ��� ����� (���� ��� ������� ������ ���� #�� ��� �� #���� � ��� ������� � ���� � #� ��� � ������� � � ��� � ������� � ���������� ������ � ���� �� ��� ����� � ����������� B �������� �# � ���� � ��� ������������� ��� ���� ����� � ������� �'� ��� �%� �� ��� �� � ������ �"

� � ������%�� � �'��

�%� � �'� � �A�

��� ��������� � � ������� � � ��� ������� ���������� �� � � ����� ��� ���� ���� �� �������" ��������� � � ��� ����� � ����� ��3���� ��

2� ��� ���� � ��� � ���� � � �� �������� ��(���� � %� #� ���� ��� ����������� ������ � ��� � � # �� ������� � ��� ?I � ���� � ��� ������� � ������ � ��� ������ ����� �" ���� �� �������� � ���� �� #���� ����� ��� ���������������� ���� �� �������� � ���� ������������ (���� #� �� # ��� � � � ���� � -�� ��� � ���" �������1� #� ���� 6�� � ����� ������������ � �� �� � � � �������� 2� � �"�!���� ���� � � ��� ���� ������� � ����� ����� � �� �� �# ������ ������� $��� �������� �� �������� #��� � � �� � � � �� �� � �� � ��������� � ��� � �� 4� #��� ���� ��� ���������� �� �� � ���� ��� ��� �� ������� #��� ����� �� ������� ��������� � �� ��� �� � � � �������C

�+� � � ��� ���� � ����� ���� � � ��

�+� � � ����� ���� � � ��� ���� � � �� �>��+� � �� � � ��

� ����� � ������ ����� ���� � �� ���� ����� ���� �+

� � ��� # � ����� ������ �� ��� �<�������� �� (���� � %�

� ������������ �������

��� #� � ���������� #�� ���������� ���������� �� � � � ������ ��� �� ��� 6��� ��������� � ������� � �� ����� � ����� ����� ��!� ��� #��� L ����� (����� �� � ��� ���������� ������� ��� ��� ����� � �� ����������"���� ���� ������� ��� �������� � � ��� � �� ��� -��� ����� � � ��� ������� ���� � ��6�!��� � ���� �1 �� #� �� ��� � ���� � � ��� � ��� ��� ���� � ��������� ��� ������ � ���� �� ��� ������� �� � ��� � ��� 8��� ? �� #� ����� �� " ��� ��� (����� ���� 2� ����� ������ ���������� ������ #�� ����� � ��� ����� ���� �� � #�� � ����� ��� ����M� � ��� ������ � ���� #� # �� �� � ������� �� (���� � %� 2� ����#�"� ��� �" � ������� �� ���� � ��� � #�� ��������#��� ����. � �� ���� � ���� ���� 8��� E�� #� �# � ��� ����� ��� ��� #��� ��� �"�����

��� 6�� ���� #�� � �� ����� �" �� � �� #��� ��� ����� ��� ������ � ������� � ��!�� � � ��'E&� '&�� �� ��� ���� � ������ �"������� �� ��� �� ����� ��� � #� ��� ���� ��� ����#��� � � ������"� �������������� �� �� �� ��� � 2� ��� ��� �� ����� �� ��� � � #�� ��� !������ 2�� ������� � ���� � ���� � �� ��� ����������� ����� ��'E&� &�� ����� ��� � �� ���5� � � #���� �� ����� ������ ��� ��� ���� ���������� ��� �� (���� � ? #�� � � ����� 2������� � %7?��

BBI ������ #��� � >�A �� � �� ����� #�� �����"������ �� � 6�� � ���# � �� �� >&��

� �����������

2� ���� ���� �� �"���� � � �� ��� �� !����� � � � ���� � #�� ��������� + � �

8��� ?� � � ���# � ��� �� ��� ��� � ���

'??E $% ������ �� ��% & ������� �� ������ '������ (( )(��*+ *��*,*��-

� ���� �� ��� ������� �� �� ��������� ��������� �� ��� �"���� �������� ��� ��� �����" � ������ �"������" ��� ������ � � ���� ��I���� ��� � ��� � ����� � ��� ��� � � ����� �"������" ������ ��� ������ � � ���� ���������� � ���� ������ � ����� �� ����!�� �� � ��� ���� ���� � ���� #�� ���� #���C� �� � � ���� �� �"����� ������ � � ��!�� � #��� �� ����� ���!� ��� � ��� � � ��!�� � ����� ���� �� B���� � ����� �� � ���� # ������� ��� ��� � ��� ���� ������ � ���������� ������� ���� � ��� � ���

�������� �������

��� ���� � ������" ���� #���� ��� ���� � � � ����� �+�N2(7%7%�'7���+7%&;E7/A� ����+�N2(7�7$$27'&%A%7'//0 ������ �" ��� � ��!����� 8 ������ � � (������ ��� ����� �"����� �� ����� #�� ���� ��� �" � �����+�N2(7�7$$27'&%A%7'//0�

�!�������

����� +�� 8��"� (�� ���� ��� 2������ 8�� + ���� 8�� '//0�

����! � � � ����� � �� ��� ��+��� � ����� 2$$$

+ � �� ��� ���� ����� ?>@E;�

����� ��� ����� +�B�� '//0� ������ !����� � ���� � � ��

� ����! � � ������ 2$$$ ����� + � �� ��� ���� 'E ->1�

�������� ��� ����� � ��� ������� ����� '///� 2������� ��������

� ����� ����� ������� �� 2$$$ ����� + � �� ��� ����

'A -A1�

8��5���� �� ��� ���� ��� 8����� +�� '///� I���� ���� �3����

6����� � ������� 2�C 2$$$ ����� � ������ ���"��� ���

������� 2���������� ) � %'� L� A� ��" '///* '?� L� E�

��� '//'�

4��5���� +�� �� ��� (�� '//0� (���" � � ������� � ��!

�� ���� �"����! ������ � ���� ��������� � B�(!222� 2�C

� �� 2B+�M/0 @ 2$$$ 2������� B ��� � + � ���� ���

��� ���� �� ��" '>@%&� <������ ������� ��� %0/>@%/&?�

<�#� �� ��+�� : ���� ����� ����� +�.�� %&&&� � ��������5��

��� ��� � �������" � ���� � ��� ����� 2�C � ��

%&&& 2$$$ 2������� B ��� � + � ���� ��� ��� ���� ��

(�� 8������ � B�� ��� %&&&� ��� %;%0@%;??�

��������� ��� ������� ��� %&&&� ����! � � ����� ��3������ �

����� ������ �������� ������ � ������ �� 2�C � ��

%&&& 2$$$ 2������� B ��� � + � ���� ��� ��� ���� ��

(�� 8������ � B�� ��� %&&&� ��� %>/>@%;&%�

(������� 4�� (�5���� 2�� '//>� I��������� �� ����� �

� ���� � � �� ����� ������� #��� ���" � ��� � ���

�� + � �� ("����� '? -?1� '%;@'?/�

.���� ��4�B�� '//'� L������� � ��������� � ������ ��� �!

� �� � ��� � �� � ���� �� � ���� �� �� + � ��

("����� 0 -%1� ';;@'/A�

.���� ����� '//0� �������� � ������ � �� � � ���!

������� ���� ������ � 2�C � �� � ��� 2B+�M/0@2$$$

2������� B ��� � + � ���� ��� ��� ���� �� ��" '>@%&�

<������ ������� ��� ?A&E@?A&/�

:��������� ��� '//;� �������� � ���� � � �� � ������!

���� ��� � � �� � ��� � � � ���� 2�C � �� '//; 2$$$

2������� B ��� � + � ���� ��� ��� ���� �� ���3��3���

L�# �� �� � ��� '//;� ��� %?&&@%?&A�

8��� E� +����� #��� (����� ���C -�1 . ��� / � ���� �� -�� ��� � � ������ ����1 � ��� ���� ��� � #� � ��* -�1 . ��� /

� ���� �� -�� ��� � � ������ ����1 � ��� ���� ��� ��� �# � #� � ���

$% ������ �� ��% & ������� �� ������ '������ (( )(��*+ *��*,*��- '??A