A Component-Based Navigation-Guidance-Control Architecture for Mobile Robots

Transcription

1 A Componen-Based Navigaion-Guidance-Conrol Archiecure for Mobile Robos Nicolas Gobillo Charles Lesire David Doose Onera - The French Aerospace Lab, Toulouse, France firsname.lasname a onera.fr Absrac In his paper we will analyze a well known archiecure in aerial roboics and generalize i o oher mobile robos. We will hen propose a normalized validaion procedure for his generalized archiecure poining ou some good pracices when developing componens. 1 Inroducion Robos are more and more auonomous, as hey are involved in more and more complex environmens, achieving more and more complex asks. Anyway, however heir auonomy skills are (from simple roue following capabiliies, o embedding complex AI planning algorihms), heir auonomy always rely on a navigaion archiecure. This navigaion archiecure provides boh localizaion informaion and locomoion capabiliies. In avionics, and herefore for aerial robos, his navigaion archiecure is used o be decomposed ino navigaion, guidance and conrol [1, 2]. For mobile (ground or marine) robos, his decomposiion is no so common, bu we can idenify similar componens on almos all exising archiecures [3, 4]. In his paper, we propose o formalize roboics engineering radiions ino a general paern o design navigaion, guidance, conrol (NGC) sofware archiecures for mobile robos. This design paern, along wih some complemenary bes pracices, is made o emphasize robusness and real-imeness of he archiecure. The NGC design paern is a componen-based generic sofware archiecure. I hen fis in Componen-Based Sofware Engineering (CBSE), which is an essenial design paradigm for roboic sofware developmen [5]. 1

2 2 The NGC archiecure The main NGC paern is based on hree componens: Navigaion: his componen uses he robo resources in order o locae iself in he world and o compue a coarse pah o achieve he mission s objecive. Guidance: he role of his componen is o compue rajecories o reach he nex waypoin provided by he navigaion componen; i includes reacive obsacle avoidance and local pah planning. Conrol: is goal is o generae he commands applied o he robo s acuaors in order o follow he guidance rajecories. These hree componens are composed ogeher o insaniae he NGC archiecure (Fig. 1). Figure 1: The NGC design paern archiecure The navigaion componen has wo inpus : he mission objecive and some informaion on he robo localizaion. The former is represened as a waypoin o reach. The laer gives informaion abou he robo localizaion in is environmen. For insance, i may be a map from a SLAM algorihm along wih an esimaion of he robo posiion. The navigaion componen hen oupus a local waypoin ha represens a goal poin o reach by he guidance componen. The guidance componen also akes ino accoun local elemery informaion in order o provide reacive obsacle avoidance while compuing smooh rajecories. I hen oupus he speed o apply o each DOF of he robo. Finally, he conrol componen ranslaes hese speed commands ino commands for he robo s acuaors. 3 Componen models 3.1 Mauve componen models In order o validae he componen s general behaviour, we use he Mauve modelling framework [6] which brings a powerful scheduling analysis and an inerface/codel separaion wihin he analysis. 2

3 We spli he componen concep in wo disinc pars : he inerface wih oher componens and he codels. The former provides several means of ineracion : properies: componens can be configured by changing parameers in heir properies; services: a componen can le is peers o direcly use one of is mehods; pors: when a componen needs o exchange, i will use one of is inpu or oupu pors The laer gives he componen he mehods and funcions i needs. The Mauve framework hardly relies on a formal definiion of he modelled componens. This definiion is implemened using a domain specific language which is parially shown in he following componens (lisings 1, 2 and 3). The Mauve model is made of several pars o fully describe a componen: he codel declaraion: i is done using he fun keyword followed by he codel name an is yped parameers. The codel reurn ype is also required; he componen definiion: his par is signaled by he word componen followed by he componen s name and conain several opional fields: such as pors or aribues. The former represens he componen s abiliy o communicae wih ohers. Every por has a specified ype and is oriened as an inpu or an oupu. The aribues keyword will be used o declare inernal variables o he componen. On he oher hand, he componen model has a mandaory field which can be updae or saemachine. The updae keyword indicaes where he componen will loop a every cycle whereas he saemachine word will ell Mauve ha he defaul sae machine of he componen will be redefined. 3.2 Codels The word codel sands for Elemenary Code. This code provides he funcionaliies and he algorihms o he hos componen. These codels are mean o be arge-independen and should be wrien in any analyzable language. I also allows is funcionaliies o be shareable and easily mainainable. In 3

4 he general case i is a good pracice o wrie reenran and hread-safe code wihou any unbounded loop. 3.3 NGC componens We will now pu more deails on wha is really in he Navigaion, Guidance and Conrol Mauve models. Firs, he navigaion componen is made of he defaul sae machine (lising 1). Lising 1: Mauve model of he Navigaion componen 1 fun updae_plan ( posiion : Pose, mission : Pose ): Plan 2 fun compue_nex_waypoin ( posiion : Pose, plan : Plan )): Pose 3 4 componen Navigaion { 5 pors { 6 inpu currenposepor : Pose 7 inpu missionpor : Pose 8 oupu nexpor : Pose 9 } 10 aribues { 11 mission : Pose 12 plan : PoseLis 13 posiion : Pose 14 nexpos : Pose 15 } 16 updae { 17 i f ( missionpor. hasnewdaa ) hen { 18 mission = read missionpor ; 19 plan = updae_plan ( posiion, mission ); 20 } 21 i f ( currenposepor. hasnewdaa ) hen { 22 posiion = read currenposepor ; 23 nexpos = compue_nex_waypoin ( posiion, plan ); 24 wrie nexpos in nexpos_por 25 } 26 } 27 } The Navigaion componen reacs o he currenposepor and missionpor inpu pors which respecively ges he local posiion of he robo and a mission objecive (in his example, he mission is a posiion on a map). If a new mission has been given, i will call he updae plan codel o build a new plan. On he oher hand, when he localizaion of he robo has changed, i will check if he oupu waypoin has been reached and if i needs o be changed using he compue nex waypoin codel. Lasly i will oupu a waypoin by he nexpor oupu por. The second componen in he archiecure, he Guidance componen, has a user-defined saemachine (lising 2) o bring he necessary behaviour. Lising 2: Mauve model of he Guidance componen 4

5 1 fun sop_robo (): Speed 2 fun nexpoin ( currenpose : Pose, argepose : Pose, localmap : Scan ): Pose 3 fun guidance ( nexpose : Pose, currenpose : Pose ): Speed 4 5 componen Guidance { 6 pors { 7 inpu argeposepor : Pose 8 inpu localmappor : Scan 9 oupu speedpor : Speed 10 } 11 aribues { 12 speedpr : Speed 13 currenpose : Pose 14 argepose : Pose 15 localmap : Scan 16 nexpose : Pose 17 } 18 saemachine { 19 i n i i a l sae Arrived { 20 run { 21 i f ( isarrived ( currenpose, argepose )) hen { 22 speedpr = sop_robo () 23 } 24 } 25 handle { 26 wrie speedpr in speedpor 27 } 28 ransiion omoving -> Moving 29 } 30 sae Moving { 31 run { 32 i f ( argeposepor. hasnewdaa ) hen { 33 argepose = read argeposepor 34 } 35 i f ( localmappor. hasnewdaa ) hen { 36 localmap = read localmappor ; 37 nexpose = nexpoin ( currenpose, argepose, localmap ); 38 speedpr = guidance ( nexpose, currenpose ); 39 } 40 } 41 } 42 handle { 43 wrie speedpr in speedpor 44 } 45 ransiion oarrived [ isarrived ( currenpose, argepose )] -> Arrived 46 } 47 } 48 } Every sae of he sae machines have five pars: enry: his par is execued when he sae machine arrives in a new sae; run: his par is execued afer he enry par has finished is execuion; 5

6 ransiion: afer he execuion of he run par, if a ransiion is validaed, his sep will call he exi par and he nex sae enry is execued; handle: his sep will be execued afer run if no ransiion has occurred; exi: his par is execued afer a ransiion. The Guidance cusom saemachine has wo saes: he Arrived sae which is a sae when he robo is jus sopped by he sop robo codel. The iniial keyword indicaes ha when he componen will be sared for he firs ime, he sae machine will begin in his sae; he Moving sae: i will check if a new local map has arrived on he localmappor inpu por and call he nexpoin codel in order o compue a local pah avoiding poenial obsacles. I will also check if a new waypoin has been given by he Navigaion componen. In boh cases, he Guidance oupus he arge s DOF speeds o he nex componen. And he las componen of he base archiecure is he Conrol (lising 3). Like he Navigaion model i uses he defaul saemachine. Lising 3: Mauve model of he Conrol componen 1 fun TTRKconrol ( speedvecor : Speed, acuaorcommand : Acuaor ): in 2 3 componen Conrol { 4 pors { 5 inpu speedpor : Speed 6 oupu acuaorcommandpor : Acuaor 7 } 8 aribues { 9 speedvecor : Speed 10 acuaorcommand : Acuaor 11 } 12 updae { 13 i f ( speedpor. hasnewdaa ) hen { 14 speedvecor = read speedpor ; 15 TTRKconrol ( speedvecor, acuaorcommand ); 16 wrie acuaorcommand in acuaorcommandpor 17 } 18 } 19 } 6

7 This componen reacs o is speedpor inpu por o compue he orders given o he arge acuaors using he TTRKconrol funcion. This design paern has several good poins: i is made of numerous small componens ha are easy o mainain upon ime; is i generic, and can hen adap o several robo plaforms (aerial robos, ground vehicles, boas or submarines) by only adaping ypes and codels (i.e. algorihms) o he argeed plaform; each componen is mean o have a real-ime behaviour and keep a relaive reaciveness depending on heir hierarchical level. To build a coheren and robus NGC archiecure, we are proposing some complemenary bes pracices: as soon as i is possible, creae unified and normalized small srucures; use a limied number of funcions on each componen, as i improves modulariy and mainainabiliy of he archiecure; regarding execuion, creae independen hread for each componen, and use periodic aciviies insead of causal links: i provides more robusness o he archiecure as a componen can coninue o work even if anoher componen has failed; i is hen easier o include recovery or safey sraegies in he archiecure. 3.4 Examples The whole NGC archiecure has been implemened on wo arges: he firs one is a TTRK robo wih an ARM powered Gumsix Waer 1 running a real-ime Linux kernel. The second is a simulaed Pioneer-3DX on a x86 compuer running he MORSE simulaor [7]. The only hings ha changed beween he wo implemenaions were he Conrol s Codel and he adapers o and from arge-specific ypes. In boh cases, all he componens were deployed using he Orocos middleware [8] and logged by he LTTng race oolki 2. 1 hps:// 2 hps://lng.org 7

8 Deployer GPS_LEA_xT sae sae Telemeer AdaperLocalPos NewObj hokuyo_scan Pose_local Pose_local Pose_obj hokuyo_scan Pose_local Pose_obj adaperscan Navigaion generic_scan Pose_local Pose_nex Pose_ask generic_scan Pose_nex Pose_plan Pose_ask Joysick Guidance Planner OnOff SpeedSruc_ele SpeedSruc_guid Pose_plan OnOff SpeedSruc_ele SpeedSruc_guid Swich SpeedSruc_con SpeedSruc_con Conrol Acuaor Acuaor AdaperToCICAS CICAS_UGV_TC CICAS_UGV_TC cicasugv The sofware archiecure is based on he main NGC paern which was compleed by adapers o conver some of he non-generic ypes, a joysick and a swich o keep manual conrol on he sysem if desired. A planner is also plugged ono he Navigaion componen o provide poenial parallel planning wih differen algorihms (Fig. 2). Figure 2: Archiecure embedded on he TTRK and Pioneer robos 4 Real-Time analysis The previously described componen models are mean o be run on Real- Time operaing sysems, even if i will sill work fine on non Real-Time sysems. In order o ensure a good and safe behaviour of he componen-based roboic sysem, we need o validae he real-ime consrains of he embedded componens. Therefore we have defined an analysis proocol for ha purpose. 4.1 Mehodology The very firs sep of he analysis is he formal validaion of he componen models by verifying for example he non-locking aspec of he ask. Once he model has been checked, an insance of he componen can be generaed using he code generaion feaure of he Mauve framework in order o ensure he validiy of he running code. All he Codels mus be developed keeping in mind ha WCETs have o be compued. In his paper we will be using he C language in order o use ools such as Frama-C 3 and OTAWA [9] for respecively validae he properies of he algorihms and compue heir Wors-Case Execuion Time (WCET). The las sep in his procedure is o deploy he whole se of componens on he arge in order o log is execuion and compare he heoreical resuls wih he real ones. 3 hp://frama-c.com 8

9 4.2 Tracing example The LTTng racing oolchain gives us he real execuion imes of each componen. We will for example focus on he Conrol componen (Fig. 3). This plo represens he probabiliy o ge execuion imes for he Conrol componen running on he TTRK s Gumsix. I shows hree main peaks which can be se in wo groups: he firs one on he lef represens he sae when he componen does no receive any new ; i herefore has nohing new o compue and reurns rapidly; he second group is made of he middle and righ peaks on he plo and corresponds o he case when he codel is called; hese wo peaks are wo branching possibiliies wihin he codel. This kind of informaion is ineresing because i will ell us if he wors case execuion ime will be reached frequenly or no and poenially opimize pars of he componen or codels. Figure 3: Disribuion of he execuion ime of he Conrol componen The deploymen procedure is se on hree pars: he firs par is he imporaion of he componen models previously described using he impor command; 9

10 he second sep is he insaniaion of he needed componens by using he keyword insance followed by an insance name and he name of he componen. When insaniaing he componens, we also seup he connecions beween he inpu and oupu pors; he las sep conains he properies of he componens and is signalled by he deploymen command. Wihin his par we specify he codels bes and wors case execuion imes and he insaniaed componens aciviies. The componen aciviies are se using he aciviy keyword and conains he componens prioriy, is period and is deadline. An exrac of he deploymen paern is shown on he lising 4. Lising 4: Deploymen of he NGC componens in Morse 1 impor " Navigaion. xma " 2 impor " Guidance. xma " 3 impor " Conrol. xma " 4 5 // Archiecure 6 insance Navigaion : Navigaion {} 7 insance Guidance : Guidance { 8 por argeposepor Navigaion. nexpor 9 } 10 insance Conrol : Conrol { 11 por speedpor Guidance. speedpor 12 } // Deploymen 16 deploymen { 17 // Navigaion 18 Navigaion_codel = aciviy Navigaion { 20 prioriy = 7 21 period = deadline = } 24 // Guidance 25 Guidance_codel = aciviy Guidance { 27 prioriy = 4 28 period = deadline = } 31 // Conrol 32 Conrol_codel = aciviy Conrol { 34 prioriy = 2 35 period = deadline = } } The deploymen in Mauve needs imes expressed in inegers, in he above example he imes are wrien in microseconds. We have o provide he bes 10

11 and wors case execuion imes for every codel of every componen deployed. The periods and deadlines of he componens are required as well as heir prioriy: he lower he value, he higher he prioriy. We can hen simulae he deploymen in order o compue imelines of he scheduled asks (Fig. 4). On he diagram, he asks are ordered in decreasing prioriy and we can clearly see ha here is no much ime lef for he high level asks such as he Planner. In his paricular example, he planner has nohing o compue and herefore he ask se is schedulable. AdaperT ocicas Conrol Swich Guidance Joysick AdaperLocalP os Navigaion P lanner Figure 4: Task scheduled by a Rae-Monoonic algorihm 5 Conclusion In his paper, we have presened he Navigaion-Guidance-Conrol design paern. I formalizes a well known archiecure in aerial roboics o mobile robos in general. This design paern brings o ligh bes pracices for embedded sofware engineering in roboics. In he fuure work, we plan o implemen his archiecure on differen robos, no only he racked TTRK or he Pioneer-3DX bu also on an aerial robo such as a quadroor. I will allow us o check he muli-arge feaure he NGC design paern brings. Anoher work in progress is he validaion of behavioural properies such as he faul olerance when a componen or a sensor fails. 11

12 References [1] F. Kendoul, Survey of Advances in Guidance, Navigaion, and Conrol of Unmanned Roorcraf Sysems, Journal of Field Roboics, vol. 29, no. 2, [2] J.-H. Kim, S. Sukkarieh, and S. Wishar, Real-Time Navigaion, Guidance, and Conrol of a UAV Using Low-Cos Sensors, in Field and Service Roboics (FSR 2003), (Lake Yamanaka, Japan), [3] S. Lacroix, A. Malle, D. Bonnafous, G. Bauzil, S. Fleury, M. Herrb, and R. Chaila, Auonomous Rover Navigaion on Unknown Terrains, Inernaional Journal on Roboic Research, vol. 21, no , [4] C. McGann, F. Py, K. Rajan, H. Thomas, R. Henhorn, and R. McEwen, A deliberaive archiecure for AUV conrol, in Inernaional Conference on Roboics and Auomaion (ICRA 2008), (Pasadena, CA, USA), [5] D. Brugali and A. Shakhimardanov, Componen-Based Roboic Engineering (Par II), IEEE Roboics and Auomaion Magazine, vol. 17, no. 1, [6] C. Lesire, D. Doose, and H. Cassé, MAUVE: a Componen-based Modeling Framework for Real-ime Analysis of Roboic Applicaions, in Workshop on Sofware Developmen and Inegraion in Roboics (SDIR 2012), (Sain-Paul, MN, USA), [7] G. Echeverria, S. Lemaignan, A. Degrooe, S. Lacroix, M. Karg, P. Koch, C. Lesire, and S. Sinckwich, Simulaing complex roboic scenarios wih morse, in Inernaional Conference on Simulaion, Modeling, and Programming for Auonomous Robos (SIMPAR), (Tsukuba, Japan), [8] P. Soeens and H. Bruyninckx, Realime hybrid ask-based conrol for robos and machine ools, in Inernaional Conference on Roboics and Auomaion (ICRA), (Barcelona, Spain), [9] C. Rochange and P. Sainra, OTAWA: An Open Toolbox for Adapive WCET Analysis, IFIP Workshop on Sofware Technologies for Fuure Embedded and Ubiquious Sysems (SEUS), pp ,