Vision based leader-follower formation control for mobile robots

Transcription

1 Scholars' Mine Masers Theses Suden Theses and Disseraions Fall 7 Vision based leader-follower formaion conrol for mobile robos Gerard Sequeira Follow his and addiional works a: hp://scholarsmine.ms.edu/masers_heses ar of he Elecrical and Compuer Engineering Commons Deparmen: Recommended Ciaion Sequeira, Gerard, "Vision based leader-follower formaion conrol for mobile robos" (7). Masers Theses hp://scholarsmine.ms.edu/masers_heses/4589 This Thesis - Open Access is brough o you for free and open access by Scholars' Mine. I has been acceped for inclusion in Masers Theses by an auhorized adminisraor of Scholars' Mine. This work is proeced by U. S. Copyrigh Law. Unauhorized use including reproducion for redisribuion requires he permission of he copyrigh holder. For more informaion, please conac scholarsmine@ms.edu.

2

3 VISION BASED LEADER-FOLLOWER FORMATION CONTROL FOR MOBILE ROBOTS by GERARD SEQUEIRA A THESIS resened o he Faculy of he Graduae School of he UNIVERSITY OF MISSOURI-ROLLA In arial Fulfillmen of he Requiremens for he Degree MASTER OF SCIENCE IN ELECTRICAL ENGINEERING 7 Approved by Sanjeev Agarwal, Advisor R. Joe Sanley Michael Nelson

4

5 iii ABSTRACT Creaing sysems wih muliple auonomous vehicles places severe demands on he design of conrol schemes. Robo formaion conrol plays a vial role in coordinaing robos. As he number of members in a sysem rise, he complexiy of each member increases. There is a proporional increase in he quaniy and complexiy of onboard sensing, conrol and compuaion. This hesis invesigaes he conrol of a group of mobile robos consising of a leader and several followers o mainain a desired geomeric formaion. The group considered has several inexpensive sensor-limied and compuaionally limied robos ha follow a leader robo in a desired formaion over long disances. This siuaion is similar o a search, demining, or planeary exploraion siuaion in which several deployable/disposable robos are led by a more sophisicaed leader. Complex sensing and compuaion are performed by he leader, while he followers perform simple operaions under he leader s guidance. The archiecure consiss of wo main componens: (i) a model-based vision sysem and (ii) a conrol algorihm. The model-based vision sysem can recognize and relaively localize he follower robos using markers mouned on he leader robo. A Bézier rajecory based mechanism is seleced o enable a group of follower robos o follow he leader. The following conrol mehod is mahemaically simple, easy o implemen, and well suied for long disance navigaion. The algorihm only requires knowledge of he leaderfollower relaive disance and bearing angle. Boh ypes of daa are compued using measuremens from a single camera, eliminaing he need for a more sophisicaed sereo sysem.

6 iv ACKNOWLEDGMENTS I would like o express my hearfel hanks o my advisor, Dr. Sanjeev Agarwal. Wihou his suppor and guidance his hesis would no have been possible. He provided a good mixure of guidance and independence, allowing me o explore and develop my own ideas. I learned a lo of research skills from him. I am very hankful o Dr. Joe Sanley and Dr. Michael Nelson for agreeing o be he members of my hesis commiee and for aking he ime o read my work and provide valuable suggesions. I would also like o hank my colleagues who have made my say in he ARIA Lab very enjoyable. My hearfel hanks goes o my lab mae Shivakar Vulli for his immense help and paience in geing me acquained wih coding languages and Linux. My parens, Jerome and Geraldine Sequeira, and my broher Joel deserve hanks for giving me he love and suppor ha enabled me o do his work. Lasly, I wish o hank my four very good friends Shubhika, for all her loving care, Ami, whose cooking will always miss, ravin, for all his joking around, and Divya, for her care, suppor and undersanding.

7 v TABLE OF CONTENTS age ABSTRACT...iii ACKNOWLEDGMENTS... iv LIST OF ILLUSTRATIONS... vii SECTION 1.INTRODUCTION MULTI ROBOT-SYSTEMS RESEARCH MOTIVATION RELATED WORK Visual Tracking Formaion Conrol ORGANIZATION DESIGN METHODOLOGY SYSTEM OVERVIEW ROBOT HARDWARE rocessor Wireless Communicaion Camera VISUAL SYSTEM Image Capure HSV Thresholding Conour Exracion and Selecion SOFTWARE ROBOT FORMATION CONTROL ROBOT MODEL VISUAL MEASUREMENT OF TARGET OSTURE BÉZIER TRAJECTORY GENERATION The Bézier Trajecory rinciple Bézier Curve Lengh... 5

8 vi... Bézier Trajecory Generaion MULTILE ROBOT FORMATION EXERIMENTAL RESULTS EXERIMENTAL SETU FORMATION MAINTENANCE RESULTS SUMMARY AND FUTURE WORK SUMMARY FUTURE WORK... 6 AENDIX... 8 BIBLIOGRAHY... 9 VITA... 4

9 vii LIST OF ILLUSTRATIONS age Figure 1.1. Convoy of Supply Trucks... 1 Figure.1. Robo Sysem Overview... 7 Figure.. Key ars of he Robo laform Figure.. The ARM7 LC16 Conroller from hilips Figure.4. The Zigbee Radio Modem... 1 Figure.5. USB o Serial Radio Adapor Figure.6. CMOS Camera Module Figure.7. Follower View of he Leader Robo... 1 Figure.8. Follower Robo Fied Wih a CCD USB Camera Figure.9. Vision rocessing Flow Conrol... 1 Figure.1. HSV Thresholding Sequence Figure.11. Deeced Conours Figure.1. Vision rocessing Flow Diagram Figure.1. Robo Geomeric Model Figure.. osiion and Orienaion of he Leader in he Followers Frame of Reference18 Figure.. Figure Showing he Follower Robos Camera Looking a he Leaders aern Figure.4 Deecion aern Figure.5. Horizonal rojecion of he Visual Sysem.... Figure.6. rojeced aern of he Leader on he Followers Camera Image... 1 Figure.7. Disorion in he Followers Image lane when he Leader Changes Orienaion... Figure.8. A Simple Bézier Curve... Figure.9. oin J on he Bézier Curve... Figure.1. Bézier Curve Beween he Leader and Follower Robos Figure.11. Bézier Cubic Curves wih Differen Values of Scale Facor D... 6 Figure.1. Muliple Robo Formaion... 9 Figure 4.1. Overhead View of he Leader/Follower Robos... Figure 4.. Evoluion of Bézier Lengh Beween he Follower and Leader... 1

10 viii Figure 4.. A Follower Tracing a Sraigh Line ah Defined by he Leader... Figure 4.4 Separaion vs Frame Number for Sraigh Line Formaion... Figure 4.5. Follower Tracing a Curve Generaed by he Leader.... Figure 4.6. Separaion vs Frame Number wih he Follower Tracing a Curve Generaed by he Leader... Figure 4.7. Overhead View of he Follower Robo Tracking a Virual oin Figure 4.8. Separaion vs Frame Number wih Virual oin Formaion... 4 Figure 4.9. Overhead View of he Follower Robo Tracking a Virual oin Along a Curve... 5 Figure 4.1. Separaion vs Frame Number wih he Follower Robo Tracking a Virual oin Along a Curve... 5

11 1. INTRODUCTION 1.1. MULTI ROBOT-SYSTEMS The prospecs of muli-robo sysems have been increasing in recen years. This is because he advanages such sysems offer over a single robo including greaer flexibiliy, adapabiliy, scalabiliy, and affordabiliy. Having a group of robos move in formaion would be beneficial in many real world applicaions, such as search and rescue, demining in miliary missions, ransporing large objecs, and convoying. I is possible for one user o conrol an enire group of robos wihou having o specify explicily he commands for each one. Figure 1.1 shows a siuaion in which only he lead ruck in a supply convoy needs o be manned. In an alernaive scenario, he lead robo may be endowed wih more sophisicaed sensors and compuaional capabiliies for overall planning and navigaion, while oher robos in formaion are simple and specialized. The de-emphasis of one large and expensive robo reduces he chance of a caasrophic mission failure. The use of several mobile robos in a coordinaed manner enables he achievemen of complex asks. The robos do no have o be very complex in srucure, since each one can be specialized for a paricular ask. Reducing he number of sensors on each robo in a muli-robo sysem plays a major role in reducing he complexiy and cos. Figure 1.1. Convoy of Supply Trucks. 1 1 icure couresy: hp://

12 One poenial advanage of muli-robo sysems is he reducion of human involvemen in dangerous siuaions including search and rescue, mining areas, balefields, and planeary missions. Such applicaions subjec he robos o high damage and failure raes. Single unis fied wih expensive equipmen raise unaccepable economic concerns. The use of muliple robos equipped wih low cos componens, among which asks are disribued are more suiable for such siuaions. Task disribuion includes sensing, compuaion, and conrol for he group as a whole. Simple robos can be considered disposable o he overall mission, so loosing a few unis does no resul in mission failure. In a leader-follower framework, he leader is given he ask of navigaion, including pah planning, and obsacle avoidance, whereas he followers asks involve racking he leader, gahering daa, and handling communicaion. These groups can be easily expandable o accommodae more unis for a larger sensor coverage or roop movemen. This hesis addresses he problem of designing a leader-follower framework wih sensor-limied robos. Given a leader robo ha moves abou in an unknown rajecory, an aemp was made o mainain he robos following he leader a a cerain disance behind, by using only visual informaion abou he posiion of he leader robo. The robos were designed o be as simple as possible in srucure. The unis are no equipped wih expensive sensors such as laser or sonar rangefinders. Global osiioning Sysem (GS) receivers are subjec o saellie availabiliy and someimes lose a signal if here is no clear view of he sky. Sensors for keeping rack of robo movemen, such as wheel encoders, are subjec o accuracy issues and drif, making informaion gahered from such sensors unreliable. For hese reasons, in he curren work, all he sensing was done via vision. The eam consised of hree or more unis equipped wih only a forward facing camera for gahering daa. 1.. RESEARCH MOTIVATION Tradiionally, he conrol design for mobile robos relies on measuremen from dead reckoning sensors such as wheel encoders which provide odomeer or posiion daa. However, hese measuremens render hemselves compleely unreliable afer a few meers of navigaion due o he encoder's low accuracy and drif. Wheel slippage, uneven

13 surfaces, and elecrical noise render readings imprecise. Acousic sensors such as radars and sonars are expensive and heir readings are suscepible o sray reflecions. They are also unable o recognize and disinguish beween objecs of similar shape, ype and color. However, due o he effeciveness and low cos of vision sensors and heir relaive cheapness in compuing power, he curren rend is o design sysems ha use vision as heir primary sensor. Image processing in sofware obviaes he need for complex sysems using odomery, sonar or laser sensors. In his hesis, an effor was made o acquire localizaion and sensory informaion hrough vision. Alhough his mehod resuls in increased compuaion, beer algorihms can be applied o reduce his concern. This research enailed geing a group of mobile robos wih inexpensive sensors o follow a leader wih a desired geomeric formaion. A single forward facing camera was he only sensor being used for seing up and mainaining formaion. These robos are significanly less powerful and complex in comparison o oher roboic sysems in erms of sensors and compuaional power. 1.. RELATED WORK Visual Tracking. Localizaion using vision sensors in robo formaions is also known as he visual racking problem. Fiala, (4) presened a vision based sysem for conrolling muliple robo plaforms in real ime using imagery from a op view video camera. Lowe, (4) described a mehod ha involved exracing disincive feaures by maching individual deeced feaures o a feaure daabase of known objecs using a fas neares neighbor algorihm. Chen e al. (5) developed a monocular-camera based visual servo racking conroller for a mobile robo subjec o nonholonomic moion consrains using Lyapunov-based echniques. By comparing corresponding objec arge poins from wo differen camera images, geomeric relaionships are exploied o derive a ransformaion ha relaes he acual posiion and orienaion of he mobile robo o a reference posiion and orienaion. This ransformaion is used o synhesize a roaion and ranslaion error sysem from he curren posiion and orienaion o he fixed reference posiion. Han and Hahn, (5) used a single forward facing camera o deermine he relaive posiion of a robo wih respec o anoher robo or objec. Since he shape of he arge in he image frame varies due o roaion and ranslaion of he

14 4 arge, hey suggesed a racking scheme which uses he exended snake algorihm o exrac he conour of he arge and updaes he emplae in every sep of he maching process. In his work, he follower robos forward facing camera has an unobsruced view of he leader robo. The leader has a colored paern mouned behind i. This helps in deermining he posiion and orienaion of he leader robo wih he follower robo as a reference. erspecive geomery is used o ransform real world D coordinaes o a D image plane Formaion Conrol. A variey of approaches have been proposed for robo formaion and conrol. One of he firs approaches was developed by Balch and Arkin, (1994) who proposed a behavior-based approach for formaion of a eam of miliary unmanned ground vehicles as scou unis equipped wih GS, sonars, and vision sensors. Tan and Lewis, (1996) applied he concep of virual rigid srucure for formaion mainenance. Their algorihm assumed ha all robos had global knowledge and ieraively fi he virual srucure o he curren robo posiions, displaced he virual srucure in some desired direcion, and updaed he robos posiions. Vidal e al. () ranslaed he formaion conrol problem ino a separae visual servoing ask for each follower, which is also he same approach used in his hsis. The follower robo uses vision o esimae he posiion and velociies of is leader. However, his was accomplished using omni-direcional cameras. Alhough using disribued camera sensors in hese approaches requires inense compuaion a each robo, implemening beer algorihms for vision processing reduces he need for higher processing power. Chiem and Cervera, (4) proposed an efficien mehod o conrol robo formaions using Bézier rajecories. Each follower robo uses a forward facing color-racking vision camera o esimae he relaive pose of he leader. Then, a local Bézier rajecory is creaed and followed. The work presened in his hesis uses Bézier curves for rajecory generaion on simple lines, because i is compuaionally simple and is well suied o he simple follower robos. Work by boh Fredslund and Maaric, () and Michaud e al. () involved he followers panning heir cameras o cener he leader in he camera's field of view. Their works also proposed mehods in which he robos iniialize and deermine heir own posiions in he formaion, in addiion o formaion conrol. However each member of he eam of homogeneous robos, is equipped wih sonar, laser, a camera,

15 5 and a radio link for communicaing wih he ohers. arker e al. (4) presened a conrol approach for heerogeneous robos in which a more capable leader assiss simpler follower robos ha have no onboard capabiliies for obsacle avoidance or localizaion and only minimal capabiliies for kin recogniion. The leader conrols navigaion using a chaining formaion mehod, forming a sensor nework. Das e al. (1) presened a paradigm for swiching beween cenralized and decenralized conrollers ha allows for changes in formaion. Like he leader, followers can also ake par in obsacle avoidance. A single omni-direcional camera is used in all robos and a hos compuer is used as a cenralized processing uni. The hos compuer receives video from all robos and calculaes relaive velociy and pose beween each follower and is leaders. Desai e al. (1998) used mehods of feedback linearizaion for conrolling formaions ha uilize only local sensor-based informaion in a leader-follower moion. Firs, he lead robo is given a moion plan. Each robo has he abiliy o measure he relaive posiion of oher robos ha are immediaely adjacen o i. Once he moion for he lead robo is given, he remainder of he formaion is governed by local conrol laws based on he relaive dynamics of each of he follower robos and he relaive posiions of he robos in he formaion. Akella and Huchinson, () address he ask of coordinaing he moions of muliple robos when heir rajecories (defined by boh he pah and velociy along he pah) are specified, used primarily o avoid collisions beween unis. In Huchinson s work, each follower robo is equipped wih a forward facing camera. Visual racking he leader robo gives is orienaion and posiion wih he follower robo as a reference. A Bézier curve is hen generaed beween he wo robos, enabling he follower o rack he leader robo using velociy conrol ORGANIZATION This hesis is organized as as described below. Secion 1 provides an inroducion o research opics relaed o his work. Muli-robo sysems are inroduced, hen relaed work on various visual racking and formaion conrol mehods is reviewed. Finally, he moivaion behind his work is discussed. In Secion, he whole sysem seup is described in deail. The physical and elecrical configuraion of he robos is presened and he visual capure algorihm is

16 6 discussed. The echnique used o measure and esimae he robo posiion and orienaion using parameers from he visual algorihm is discussed, followed by a descripion of he sofware used. Secion describes he rajecory racking conrol algorihm, which is based on Bézier Trajecories. Secion 4 presens he experimenal resuls of he formaion conrol law, using he mobile plaforms in he laboraory. The experimenal seup is described in deail followed by resuls wih differen formaions. Secion 5 summarizes he conribuions of his work and idenifies areas in which fuure work could improve and exend he mehods developed here.

17 7. DESIGN METHODOLOGY.1. SYSTEM OVERVIEW Each robo in a eam is capable of localizing and following he leader robo using vision. The sysem srucure of such a robo is shown in Figure.1. I consiss of six unis: main uni, power uni, locomoion, sensing, communicaion and he hos compuer unis. The main uni, an ARM7 CU, is described in Secion..1. The power uni is a single 8V lihium-polymer baery along wih.v and 5V volage regulaors for powering he main conrol board. The drive moors run direcly on he 8V. An onboard CMOS camera capures JEg images and sends hem over serially o he ARM7 conroller. However, in he research for his hesis, a USB camera was used due o camera firmware issues. A Zigbee radio handles communicaion beween he ARM7 conroller and he hos compuer. All he image processing is accomplished by he hos compuer and conrol commands are sen back o he robos locomoion module. Figure.1. Robo Sysem Overview.

18 8.. ROBOT HARDWARE The robos, based on Surveyor Corporaions SRV-1 plaform, are palm sized and feaure wo independenly driven, differenial-seered ank-syle reads run via wo DC gear moors. An ARM7 conroller handles processing and manages onboard peripherals. The plaforms are equipped wih infrared sensors for deecing near ranged obsacles and a forward facing camera. All communicaion beween he robos and he hos compuer is conduced serially via Zigbee wireless radios. Figure. illusraes key plaform pars. The sysem is powered by an 8V lihium polymer baery. Aside from he drive moors, he plaform runs a.v o 5V. Figure.. Key ars of he Robo laform...1. rocessor. The plaforms are powered by a 6MIS bi ARM7TDMI-S LC16 processor from hilips NX wih 64kB on chip saic RAM and 18kB on chip flash memory for soring code. The ARM7TDMI-S core is a synhesizable embedded RISC processor ha provides sysem designers wih he flexibiliy necessary o build embedded devices requiring small size, low power, and high performance. The processor employs a unique archiecural sraegy known as Thumb, which makes i ideally suied o high-volume applicaions wih memory resricions or applicaions in which code Surveyor Corporaion, hp://

19 9 densiy is an issue. The key idea behind Thumb is a super-reduced insrucion se. Essenially, he ARM7TDMI-S processor has wo insrucion ses including he he sandard -bi ARM se and he 16-bi THUMB se. The Thumb ses 16-bi insrucion lengh allows i o approach wice he densiy of sandard ARM code while reaining mos of he ARM s performance advanage over a radiional 16-bi processor using 16-bi regisers. This is possible because Thumb code operaes on he same -bi regiser se as ARM code. Due o he huge code size of his work, all code has been wrien in Thumb mode. eripherals for he LC16 include wo Universal Asynchronous Receivers Transceivers (UARTs). One UART provides a full modem conrol handshake inerface; he oher provides only ransmi and receive daa lines. The implemened IC-bus suppors bi raes up o 4 Kbi/s (Fas IC-bus). Six single edges and/or double edge conrolled WM oupus are available for moor conrol. As shown in Figure., he processor is based around a developmen board from Embedded Ariss. Figure.. The ARM7 LC16 Conroller from hilips. The board runs a a volage of.v and has all of he processor's pins roued o header for easy inerface. Downloading of new firmware ono he chip is handled by he chip's onboard boo loader using a serial channel.

20 1... Wireless Communicaion. Each SRV1 communicaes wih he base saion via Zigbee complian wireless radios. These are full fledged radio modems capable of speeds up o 5,bps. They have a ransmiing power of 1mW wih an indoor range of abou a 1m each. Daa is ransferred beween he robos and he base saion a 115bps. The radios are used for elemery, as well as for remoely downloading code ono he robos. All SRV-1 daa and conrol commands, including camera images are sen via hese radios. Figure.4 shows one such radio module. Figure.5 shows he base saion, which consiss of a similar radio conneced via Universal Serial Bus. Figure.4. The Zigbee Radio Modem. Figure.5. USB o Serial Radio Adapor.... Camera. The SRV-1 robos are equipped wih a C8 JEG compression module which performs as a video camera or a JEG compressed sill camera (OV764 sensor). The included lens has a focal lengh of 4.6mm wih a 57 degree field of view.

21 11 As show in Figure.6, he camera consiss of a lens, an image sensor (Omnivision s OV764), and a compression/serial-bridge chip (Omnivision s OV58). The OV764 is a low-volage CMOS image sensor ha suppors various image resoluions (VGA, CIF, SIF, QCIF, 16 18, 8 64) as well as various color formas (4 gray/16 gray/56 gray/1-bi RGB/16-bi RGB). I provides complee user conrol of image qualiy, formaing, and oupu daa ransfer. The OV58 Serial Bridge is a conroller chip ha implemens boh a JEG compression engine and a serial (RS-) inerface o a hos conroller. The OV58 implemens a se of 11 iniializaion commands, including aking a snapsho, geing a picure, and seing he packe size. Figure.6. CMOS Camera Module. Commands o he camera are issued via a serial RS- inerface a 91,bps. Snapsho commands from he ARM7 conroller capure a full resoluion single-frame sill picure. The picure is hen compressed by he JEG engine (OV58) and ransferred o he hos which is furher relayed o he base saion for processing. The camera is capable of aking JEG snapshos a 8x64, 16x18, x4, 64x48 resoluions. The 16x18 resoluion is used due o radio bandwidh issues. The camera provides a frame rae of abou 1 o frames per second. Figure.7 shows a JEG image as seen by one of he robos.

22 1 Figure.7. Follower View of he Leader Robo Due o firmware problems of he camera module, however, i was no possible o obain he required frame rae of 15- frames per second. This reduced frame rae resuled in a lo of lag and miscalculaions in sending commands o he robo. Hence, in his research, a low cos CCD camera was fixed and used insead of he onboard camera. The webcam is a hilips 9NC model capable of grabbing images a frames per second. Figure.8 shows one of he modified robos fied wih he webcam. Images are capured and analyzed a a rae of 15 frames per second by he vision processing program. Figure.8. Follower Robo Fied Wih a CCD USB Camera.

23 1.. VISUAL SYSTEM The visual racking problem is divided ino arge deecion and pose esimaion. Targe deecion, explained in his secion, involves capuring an image from he camera and processing i o deec feaures of ineres. Esimaion of pose is explained in Secion.. The vision processing can be divided ino he asks illusraed in Figure.9. Image Capure Color Image HSV Thresholding Binary Image Conour Deecion oins/markers Soring / Labeling deeced poins osiion/ Orieaion esimaion on of arge Figure.9. Vision rocessing Flow Conrol...1. Image Capure. The CCD camera capures images of he leader robo in a JEG forma. Capured Images are he convered ino OpenCVs IplImage forma for processing. Images are capured a he rae of 15 frames per second.

24 14... HSV Thresholding. One of he main asks for he vision sysem is o deec he leader using colored blobs mouned on he leader robo. Deecion is done hrough color segmenaion in Hue Sauraion Value (HSV) space. While he Red Blue Green (RGB) color sysem is used widely by mos digial and capuring devices, i is no suiable for use in recogniion asks. When he value of a separae channel (red, blue, green) changes, he color presenaion of he enire image is affeced grealy whereas in he HSV space, he Hue value represens color while sauraion indicaes inensiy of he color and value conains informaion abou how brigh he pixel is. Unlike he RGB space where colors are mixed up from hree differen color channels, using he HSV color sysem o deec a feaure of a paricular color is much saisfacory. Figure.1 shows he HSV hresholding sequence where a hree channel IplImage capured ino OpenCV which is convered from RGB image space o HSV as shown in Figure.1b. The HSV image is furher sripped ino hree individual hue, sauraion and value channels. The hue channel is hen individually hresholded o deec a specific color. The sauraion and inensiy values are hresholded depending on lighing condiions. Since he HSV hresholded IplIimage as shown in Figure.1c is a one channel image, i does no show any color. (a) (b) (c) Figure.1. HSV Thresholding Sequence (a) Color Image, (b)he HSV Image, and (c)he HSV Thresholded Image.

25 15... Conour Exracion and Selecion. The hresholded images are run hrough an OpenCV implemenaion of conour exracion based on he algorihm developed by Suzuki and Abe, (1985) due o is simpliciy, robusness and fas speed. The Suzuki-Abe algorihm rerieves conours from he binary image by raser scanning he image o look for border poins. Once a poin ha belongs o a new border is found, a border following procedure is applied o rerieve and sore he border in he Freeman chain forma. During he border following procedure, visied border poins are marked wih a special value. The algorihm oupus a lis of conours using Freeman chain code. Figure.11 shows he deeced conours. However, since he algorihm deecs conours of any shape and size, a few false deecions are generaed, as illusraed in Figure.11a. Since he markers on he leader robo are of a known area range, he deeced conours are area hresholded. Figure.11b shows he deeced markers afer filering. (a) (b) Figure.11. Deeced Conours (a)false Deecion, (b)false Deecions Filered. OpenCV Reference Manual hp://

26 16.4. SOFTWARE All code used in his work was developed using GNU open source ools. Code for he ARM7 microconroller was wrien in C/C++ using an ARM por of GCC, which is a GNU compiler. OpenCV, an open source compuer vision library developed by Inel was used o process images. This library provides funcions for image capure and racking, as well as processing. OpenCV is an image processing library developed by Inel specifically for heir processors. I makes use of boh he mulimedia and sreaming Single Insrucion, Muliple Daa (SIMD) exensions (MMX and Sreaming SIMD Exensions) ha Inel have inroduced ino heir enium range, resuling in image manipulaion speeds of up o 5fps. Figure.1 is a flow diagram illusraing how capured images are sen wirelessly o a hos compuer for processing. Feaures are hen exraced and used by he visual servoing algorihm for generaing velociy feedback commands. These commands are hen sen back o he robos via he same radio link. Figure.1. Vision rocessing Flow Diagram.

27 17. ROBOT FORMATION CONTROL.1. ROBOT MODEL The wo readed robo can be described by he following kinemaics equaions:. x = v cosϕ (1). y = vsinϕ () & ϕ = ω () where (x,y) are he Caresian coordinaes of robo posiion, ϕ is he robo orienaion angle, and v and ω are he robo linear and angular velociies. and v denoe linear v1 speeds of he lef and he righ wheels, respecively. The linear ( v ) and angular ( ω ) velociies are expressed below: ( v ) 1 + v v = (4) ( v 1 v ) ω = L (5) where L represens he disance beween he wo driving wheels. Figure.1 illusraes he geomeric model of he robo. Figure.1. Robo Geomeric Model.

28 18.. VISUAL MEASUREMENT OF TARGET OSTURE This secion presens a vision-based framework for mobile robo deecion and racking using off-he-shelf cameras mouned on he robo. Targe deecion and pose esimaion are performed from single frames using markers as key elemens. The mehod consiss of racking a recangular shaped srucure behind each robo. Deerminaion of he posiion and orienaion of he leader can be achieved by esimaing is disance and relaive orienaion wih regard o he followers. As shown in Figure., only he posiion (x, y) and orienaion θ needs o be esimaed. Figure.. osiion and Orienaion of he Leader in he Followers Frame of Reference. ose and orienaion esimaion refers o he issue of obaining relaive posiion and orienaion beween wo or more mobile robos. The camera capures a paern mouned on he leader robo, as shown in Figure..

29 19 Figure.. Figure Showing he Follower Robos Camera Looking a he Leaders aern. The paern as shown in Figure.4, feaures four circles a each corner of a square of known lengh E(mm). The wo segmens AC and BD provide an esimae o he disance beween he follower and he leader based on heir perceived and real heighs. The difference beween he perceived heighs of he wo segmens gives an esimae of he orienaion of he paern wih respec o he follower robo. The suggesed paern on he leader robo is illusraed in Figure.4a. Figure.4b. Illusraes he paern as observed in he follower robos image plane (X image,y image ). h L and h R are he heighs of he wo segmens AC and BD. The posiions of he paern's marks on he image are expressed in pixels as (X i, Y i ) where i = A,B,C and D. Figure.4. Deecion aern (a) Leader Robo, (b) aern on he Followers Camera Image.

30 Figure.5 illusraes he horizonal projecion of he vision sysem, showing he posure of he leader vehicle on a coordinae sysem (X C, Z C ) associaed wih he camera. f is he focal lengh of he camera being used. oins A, C and B, D represen segmens AC and BD when viewed from he op. Figure.5. Horizonal rojecion of he Visual Sysem. The projeced paern on he robo's camera image appears wih a projecion disorion as represened in Figure.6. The heighs h L and h R now change as he leader changes orienaion. From hese image feaures, i is possible o compue he posure of he arge vehicle (x T, z, θ). The reverse perspecive model is used o projec a D poin in he D camera image plane, resuling in he following parameers, which correspond o Figure.5. x L ( z f f ) x ( z f f ) x L R = A, xr = B (6)

31 1 + = 1 L L h E f z, + = 1 R L h E f z (7) Figure.6. rojeced aern of he Leader on he Followers Camera Image. L R T x x x + =, L R T z z z + = (8) + + = L R L R x x z z 1 an θ (9) Figure.7 shows frames capured by he follower robo when he leader robo is sraigh in fron of he follower a a disance of 7mm in differen orienaions. Figure.7a shows he leader oriened a a degree angle o he lef whereas Figure.7b shows he leader wih he same orienaion o he righ. Heighs h L and h R change in value as he paern orienaion is alered.

32 (a) (b) Figure.7. Disorion in he Followers Image lane when he Leader Changes Orienaion, (a) Lef and (b) Righ... BÉZIER TRAJECTORY GENERATION..1. The Bézier Trajecory rinciple. A Bézier curve in is mos common form is a simple cubic equaion used for curve fiing. Originally developed by ierre Bézier, who used i o design he Body of a Renaul Car in he 197s, i has only recenly been used in roboics. Figure.8 illusraes a simple Bézier curve. Figure.8. A Simple Bézier Curve.

33 A Bézier curve is a curve which is exacly deermined by a se of conrol poins. Each poin of he curve is calculaed from a parameric mahemaical funcion which uses he coordinaes of he conrol poins as parameers. Given he four poins A, B, C, and D, and a value beween and 1, he poins E, F, and G are consruced a -fracion of he way along he segmens AB, BC, and CD, respecively; poins H and I are hen placed a -fracion of he way along he segmens EF and FG; and finally, J is consruced a -fracion of he way along he segmen HI. The locus generaed by J as goes from o 1 is he generaed curve. oins A and D are he end poins, poins B and C are he conrol poins. The curve can be changed in shape by changing he disance beween he segmens AB and DC. In reference o he Leader- Follower model, poin A represens he follower and D represens he leader robo. oin J on he curve varies as varies from o 1. As illusraed in Figure.9a J lies a he midpoin of he Bézier curve wih a value of =1/ whereas in Figure.9b J moves down o poin A as approaches. (a) (b) Figure.9. oin J on he Bézier Curve (a) = 1, (b).

34 4 Given N+1 conrol poins wih k= o N, he Bézier parameric curve is described by as follows: k () () 1 ) (1 )!!(! = = k N k N N k k N k k (1) Bézier curves are parameric curves ha, when applied independenly o he x and y coordinaes for a D curve, give: () 1 ) (1 )!!(! ). (. = = k N k N x x N k k N k k (11) () 1 ) (1 )!!(! ). (. = = k N k N y y N k k N k k The equaion for a Bézier curve is a polynomial of degree N (one less han he number of conrol poins). As he number of conrol poins increases, degree N rises, so i becomes expensive in erms of processing power o draw he curve. However, mos curves can be drawn uilizing only four conrol poins. The polynomial degree is hen hree (hus he name "Cubic Bézier" curve). Given four conrol poins-, 1,, and, he mahemaical formula for a Cubic Bézier curve is as follows: () 1 ) (1 )!!(! = = k k k k k k (1) 1 ) (1 ) (1 ) (1 ) ( = ) ( ) 6 ( ) ( ) ( = ) ( ) ( ) ) ( ( ) ( = Le, } 1 ) ( = d c b a (1)

35 5 where, c = ( 1 ) (14) b = ( ) c a c b = d = inegral is:... Bézier Curve Lengh. The lengh of he Bézier curve s, which is an s = x + y s = s = x + y 1 = ( a + a + a ) + ( b + b + b ) s = d (15) 1 1 The lengh of s is calculaed by inegraion from = o =1. a a1, a,, a and b, b b1, b, for x and y can be calculaed from Eqn. (14).... Bézier Trajecory Generaion. The follower robos use a Bézier curve o generae a rajecory o he leader robo. This curve is he weighed sum of four conrol poins (, 1,, and ), as shown in Fig..1. D is he disance beween poins 1 - and -. Figure.1. Bézier Curve Beween he Leader and Follower Robos.

36 6 Endpoins and define he posiions of he follower and leader, respecively. Conrol poins 1 and are deermined by he orienaion of he robos. The four poins are defined in he follower robo's reference frame: D x D cosθ =, 1 =, =, y Dsinθ x = (16) y The cubic Bézier curve from Eqn. (1) given by: ( ) = a + b + c + d, [,1] (17) where he vecors a, b, c are defined as in Eqn. (14). Values of x, y and θ are obained from he vision sysem. The heory of Bézier curves saes wo properies regarding he endpoins: The curve passes hrough he endpoins hemselves, and The curve is angen o he vecors 1 - and - a he endpoins The conrol poins can be arbirarily chosen anywhere in he space beween he wo robos. The shape of he curve can be easily deformed by modifying he disance D beween poins 1 - and -. However, placing he poins in differen locaions resuls in differen curves or rajecories, as illusraed in Figure.11. Figure.11. Bézier Cubic Curves wih Differen Values of Scale Facor D.

37 7 The follower robos need o keep a se disance away from he leader robo. This is done by selecing an appropriae value of D. This D is proporional o he disance beween he robos, and he same D is used for boh pairs of endpoins and conrol poins such ha D = (18) 1 = By experimenal experience as shown by Chiem and Cervera, (4), and from Figure.11.d, he value of D which makes he Bézier curve exacly he same as a 9- degree arc is: ( 1) D = (19) = ( 1) x + y =.9* x + y Alhough he leader robo is se on a predefined rajecory wih a consan velociy v, he velociy may change by small amouns. The follower robo has o ake ino accoun hese changes in velociies. This is done by keeping rack of he lengh of he Bézier curve, s. The conroller is proporional in naure, increasing he followers speed if i is oo far from he leader and decreasing speed if i is oo near. The change in linear velociy Δv is given by: ( s ) Δ v () s where s is he desired lengh of he curve. Also, he follower robos need o vary heir angular velociy, ω, which is compued as follows, o keep up wih he leader robo. This angular velociy mus correspond o he curvaure of he Bézier rajecory a : v ω = = vκ (1) R

38 8 where R and κ are he radius and curvaure of he rajecory, respecively. The curvaure of any parameric curve is: xy &&& yx &&& κ = () ( x& + y& ) From Figure.1, since he follower robo is a he origin (,) and i is only necessary o compue he curvaure a =, x & = v () y& = so ha he curvaure a his poin is: & y κ = x & = ( y Dsinθ ) d (4) where D is obained from Eqn. (19) and he desired angular velociy is compued from Eqn. (1). Individual wheel velociies are hen calculaed from Eqn. (4) and Eqn. (5). The camera runs a a frame rae of 5 frames per second. When a frame is capured, he new posiion of he leader and he lengh of he Bézier curve is esimaed. Using hese parameers, he Bézier poins are calculaed and he new linear velociy v and angular velociy ω are compued..4. MULTILE ROBOT FORMATION In his research, he follower robo mainains a posiion relaive o a leader robo. Using he same leader-follower philosophy, muliple robos can be made o follow he same leader wih he help of virual poins. These poins are he posiion of he leader displaced a cerain disance away. The only drawback of such a formaion is ha every follower robo has o have he leader in view. Virual desinaions are assigned o each follower o mainain a geomeric formaion. These poins are he posiion of he leader robo moved perpendicular o and a cerain disance away from he leaders y-axis. Then, cubic Bézier rajecories, as described in Secion.., are defined beween he follower and hese virual desinaions

39 9 o allow he robos o follow he leader. The rajecory is updaed in real-ime because he virual desinaion varies as he leader robo moves. Figure.1 illusraes wo follower robos racking virual poins V1 and V displaced by cerain amoun in differen direcions from he leader. Figure.1. Muliple Robo Formaion.

40 4. EXERIMENTAL RESULTS 4.1. EXERIMENTAL SETU The seup consised of an overhead camera covering a 7. x 5. sq. fee area. The camera was a low-cos CMOS vision sensor conneced via USB o a compuer mouned on he laboraory ceiling. The leader and he follower robos had colored markers mouned on heir op. These served as robo idenificaion for he program analyzing he area. The area was covered wih whie boards o ensure minimum disurbances from oher colors and easy deecion for he image processing program. Colors for he wo robos were chosen so ha hey sand ou agains he background. Figure 4.1 shows he seup wih he leader colored red and he follower colored blue. Figure 4.1. Overhead View of he Leader/Follower Robos.

41 1 OpenCV was used o capure and filer images and deec he markers. The colors were deeced using he HSV hreshold algorihm as described in Secion. For a paricular run he camera capured a video involving he leader and follower robos a a 64 x 48 resoluion a 5 frames per second. The video was hen processed hrough OpenCV for analysis. Verificaion of he follower robos mainaining a desired formaion wih he leader was done by making sure ha he robos mainain a consan predefined Bezier lengh beween hem and visually verifying heir rajecories. 4.. FORMATION MAINTENANCE RESULTS This experimen demonsraes how he robos aain formaion afer a cerain period of ime. The leader and follower robos are separaed a a disance of abou 1mm a sarup. The required disance beween he robos is 5mm. This disance is mainained by calculaing he lengh of he Bézier curve. Figure 4. Illusraes resuls for hree independen runs. I is seen ha he separaion beween he wo robos converges o he desired value (5mm) afer a cerain period measured in frames. Figure 4.. Evoluion of Bézier Lengh Beween he Follower and Leader.

42 Figure 4. shows he follower keeping up righ behind he leader. The red rail represens he leader while he blue is he follower. The follower sared is run a an angle o he leader. Afer a cerain period, he follower mainained is posiion righ behind he leader a he desired disance. Figure 4.. A Follower Tracing a Sraigh Line ah Defined by he Leader. Figure 4.4 shows he wo robos mainain a separaion of 5mm beween hem. The oscillaions from he desired value are due o a purely proporional conroller. Figure 4.4 Separaion vs Frame Number for Sraigh Line Formaion.

43 In anoher experimen, a follower robo followed he leader in a circular arc moion as in Figure 4.5. During arc moions, alhough he follower go behind he leader, he vision sysem akes some amoun of ime before i could ge he follower righ behind he leader. Figure 4.6 shows he how he follower ried o mainain a disance of 5mm behind he leader. During curve moions here is a larger deviaion from he desired separaion as compared o linear moions. This discrepancy is due o a poor velociy conroller model. Figure 4.5. Follower Tracing a Curve Generaed by he Leader. Figure 4.6. Separaion vs Frame Number wih he Follower Tracing a Curve Generaed by he Leader.

44 4 However, on curves wih a larger radius, hese deviaions seem o reduce. Figure 4.7 Illusraes he follower robo racking a virual poin o verify muliple robo formaions. Figure 4.8 shows he separaion beween he wo robos for wo independen runs. Figure 4.9 is he same experimen, bu along a curve. As discussed before here is a much more deviaion in he separaion values when he robos move along a curve. The separaion beween he wo robos is illusraed in Figure 4.1. Figure 4.7. Overhead View of he Follower Robo Tracking a Virual oin. Figure 4.8. Separaion vs Frame Number wih Virual oin Formaion.

45 5 Figure 4.9. Overhead View of he Follower Robo Tracking a Virual oin Along a Curve. Figure 4.1. Separaion vs Frame Number wih he Follower Robo Tracking a Virual oin along a Curve.

46 6 5. SUMMARY AND FUTURE WORK 5.1. SUMMARY This hesis presens a framework for vision-based conrol for muli-vehicle coordinaion using he leader-follower approach. Unlike he leader, he followers are designed o be equipped wih low cos sensors, primarily vision sensors. The leader is a more sophisicaed vehicle wih poenial navigaion, ranging and obsacle avoidance capabiliies. For curren work, he leader is programmed wih a predefined rajecory. This framework consiss of a vision based model and a formaion conrol algorihm for he follower robos. The follower requires only informaion abou he posiion and orienaion of he leader o follow. The vision sysem uses markers for idenificaion of he leader robo. I can esimae he relaive pose (disance and angle) beween wo robos from single images. The vision sysem is divided in wo main componens, image processing and pose esimaion. A HSV hresholding approach is used for processing he capured images for markers deecion. Using he model of perspecive geomery, he posiion and orienaion of he leader robo is deduced. The formaion conrol algorihm is based on generaing a Bézier rajecory beween he leader and follower. The Bézier rajecory is defined according o he relaive configuraion beween he leader and he follower and is updaed in real-ime as he leader navigaes. This ype of conrol mehod is simple and does no require complicaed compuaion in he followers. Experimenal resuls show ha he algorihm performs well wih boh sraigh line and virual poin following. While negoiaing curves, he follower robos find i a lile difficul o keep up wih he leader. This can be solved by developing a beer higher order velociy conroller for he followers. 5.. FUTURE WORK The vision based pose esimaion is precise enough so ha he followers do no need accurae posiioning sysems. Only he leader robo carries such a sysem o command he enire formaion. The proposed formaion mehod is limied in ha he followers need o mainain line-of-sigh conac wih he leader. If for any reason a

47 7 follower looses sigh of he leader, a search for leader mechanism is run. Once visibiliy is recovered, he follower can cach up wih he leader. A beer soluion would be o use a wide angle camera or an omni-direcional camera. An omni-direcional vision sensor consiss of a conical lens and a camera which give a 6 view of he environmen. The follower would no need o search for he leader because i would always be in view. The robos in his research ry o keep he leader in view by modifying heir angular and linear velociies. However, since his work sresses he need for low cos and he smalles number of sensors, velociy feedback could be calculaed from he vision sensor using opical flow algorihms. Formaion swiching beween he robos and obsacle avoidance is also imporan. There are imes when he formaion may need o form a single line. In such a scenario, each follower robo becomes a leader o he one behind i. Fuure work could focus on how o produce a cenralized conrol sraegy for eams of follower robos ha swich beween differen formaions in reacion o environmenal changes or o avoid obsacles. However, such formaions may increase communicaion bandwidh beween he robos. Using omni-direcional cameras as described above could also provide flexibiliy in achieving differen formaions.

48 8 AENDIX COEFFICIENTS OF A CUBIC BEZIER CURVE A Cubic Bézier curve, in a -D plane, in parameric erms is : x = a + + a1 + a a y = b + + b1 + b b The four conrol poins, 1,, and can be expressed as: x, y ),( x, y ),( x, y ),( x, ). ( 1 1 y x = and y = when = x y Hence, x = = a, y b x = and y = when = 1 x y (A.1) Hence, x = a + a1 + a + a, y = b + b1 + b + b (A.) The oher wo conrol poins 1 and require he derivaive of he curve. As in Figure.9, derivaive along angen AB: dx d dx d = a dy b d 1+ a + a, = b1 + b + dy = ( x 1 x ) and = ( y 1 y ) when = d Hence ( x 1 x ) = a1,( y1 y ) = b1 (A.) Derivaive along angen DC: dx d dy = ( x x ) and = ( y y ) when =1 d Hence ( x x ) = a1 + a + a (A.4) ( y + b y ) = b1 + b Solving for a a1, a, a, b, b1, b,, b from Equaions (A.1), (A.), (A.), (A.4) a = =, a = ( x x ), b = ( y ) x, b y y a = ( x x1 + x ), b = ( y y1 + y ) a = x x y + ( x1 x ), b = y y + ( y1 )

49 9 BIBLIOGRAHY Agrawal M., and Konolige K. (6), Real-ime Localizaion in Oudoor Environmens using Sereo Vision and Inexpensive GS, The 18h Inernaional Conference on aern Recogniion, vol., pp Akella S., and Huchinson S. (), Coordinaing he Moions of Muliple Robos wih specified Trajecories, roc. of he IEEE Inernaional Conference on Roboics and Auomaion, vol. 1, pp Armeso L., Tornero J. (6), Robus and Efficien Mobile Robo Self-Localizaion using Laser Scanner and Geomerical Maps, IEEE/RSJ Inernaional Conference on Inelligen Robos and Sysems, vol. 1, pp Balch T., and Arkin R. (1994), Moor Schema-based Formaions Conrol for Muliagen Robo Teams, roceedings of he Firs Inernaional Conference on Muliagen Sysems, pp Balch T., and Arkin R. (1998), Behavior-based Formaion Conrol for Muli-Robo Teams, IEEE Transacions on Roboics and Auomaion, vol. 14, pp Bela, C., and Kumar, V. (1), Moion Generaion for Formaions of Robos: A Geomeric Approach, IEEE Inernaional Conference on Roboics and Auomaion, Korea, vol, pp Brooks R. (1986), A Robus Layered Conrol Sysem for A Mobile Robo, IEEE Journal of Roboics and Auomaion, vol. RA-, No. 1, pp Bruce J., Balch T., and Veloso M. (), Fas and Inexpensive Color Image Segmenaion for Ineracive Robos, roceedings of Inernaional Conference on Inelligen Robos and Sysems, vol., pp Carelli R., Soria C., Nasisi O., and Freire E. (), Sable AGV Corridor Navigaion wih Fused Vision-Based Conrol Signals, Conference of he IEEE Indusrial Elecronics Sociey, Sevilla, Spain, vol., pp Chen J., Dawson M., Dixon W., and Behal A. (5), Adapive Homography Based Visual Servo Tracking for Fixed and Camera-in-Hand Configuraions, IEEETransacions on Conrol Sysems Technology, vol 1, pp Chiem S., and Cervera E. (4), Vision-Based Robo Formaions wih Bezier Trajecories, roceedings of Inelligen and Auonomous Robos, pp

50 4 Das A., Fierro R., Kumar V., Osrowski J., Splezer J., and Taylor C. (), A Visionbased Formaion Conrol Framework, IEEE Transacions on Roboics and Auomaion, Vol. 18, pp Das V., Fierro R., Kumar V., Souhall B., Splezer J.,Taylor C. (1), Real-ime Vision-Based Conrol of a Nonholonomic Mobile Robo, Inernaional Conference on Roboics and Auomaion, Seoul, Korea, pp Desai J., Osrowski J., and Kumar V. (1998), Conrolling Formaions of Muliple Mobile Robos, roceedings of he IEEE Inernaional Conference on Roboics and Auomaion, Leuven, Belgium, vol. 4, pp Fredslund J., and Maaric J. (), A General Algorihm for Robo Formaions using Local Sensing and Minimal Communicaion, IEEE Transacions on Roboics and Auomaion, Vol. 18, No. 5, pp Fredslund J., and Maaric J. (1), Robo Formaions Using Only Local Sensing and Conrol, IEEE Inernaional Symposium on Compuaional Inelligence in Roboics and Auomaion (CIRA-1), Banff, Albera, Canada, pp Fiala M. (4), Vision Guided Conrol of Muliple Robos, roceedings of he Firs Canadian Conference on Compuer and Robo Vision, Washingon DC, USA, pages Forsyh, and once, erspecive rojecion, in Compuer Vision : A Modern Approach. renice Hall,, pp Han Y., and Hahn H. (5), Visual Tracking of a moving Targe using Acive Conour based SSD Algorihm, Roboics and Auonomous Sysems. Huang J., Farrior S., Qadi A., and Goddard S. (5), Localizaion and Follow-he- Leader Conrol of Heerogeneous Groups of Mobile Robos, ASME Inernaional Mechanical Engineering Congress and Exposiion. Jiangyang H. (7), Localizaion and Follow-he-Leader Conrol of a Heerogeneous Group of Mobile Robos, h.d. disseraion, Universiy of Nebraska. Larsen T., Bak M., Andersen N., and Ravn O. (1998), Locaion Esimaion for Auonomously Guided Vehicle using an Augmened Kalman Filer o Auocalibrae he Odomery, Fusion 98, Firs Inernaional Conference on Informaion Fusion, Las Vegas, USA. L. E. arker, B. Kannan, F. Tang, and M. Bailey (4), Tighly-Coupled Navigaion Assisance in Heerogeneous Muli-Robo Teams, in roc. IEEE In. Conference Inelligen Robos Sys., Sendai, Japan, vol. 1, pp