Optical Tracking Control of a Differentially- Driven Wheeled Robot

Transcription

1 Opticl Trcking Control of Differentilly- Driven Wheeled Robot A thesis submitted in fulfilment of the requirements for the degree of Doctor of Philosophy Loren Yeo B. Eng. (Hons.) School of Aerospce, Mechnicl nd Mnufcturing Engineering College of Science, Engineering nd Helth RMIT University Bundoor, Victori December 01

2 To my mother nd in memory of my fther i

3 Declrtion I certify tht except where due cknowledgement hs been mde, the work is tht of the uthor lone; the work hs not been submitted previously, in whole or in prt, to qulify for ny other cdemic wrd; the content of the thesis is the result of work which hs been crried out since the officil commencement dte of the pproved reserch progrm; ny editoril work, pid or unpid, crried out by third prty is cknowledged; nd, ethics procedures nd guidelines hve been followed. Loren Yeo 1 December 01 ii

4 Abstrct Mobile robotics hs become n incresingly ubiquitous technology in modern times. A typicl exmple is the wheeled mobile robot (WMR). In order for WMR to function effectively, it must demonstrte excellent trcking control nd loclistion cpbilities. This is chieved by hving ccurte nd responsive control lgorithms s well s high-precision sensor systems. However, this often requires complicted lgorithms nd expensive equipment. This thesis proposes system to show tht good trcking performnce cn be chieved with modertely simple control lgorithm nd reltively inexpensive hrdwre. The pltform used in this reserch ws differentilly-driven wheeled robot constructed using the Lego Mindstorms NXT system. Positionl trcking ws provided by two Avgo opticl lser sensors commonly found on the computer mouse. The min progrmming environments were MATLAB nd Simulink, long with severl other open-source pplictions. In the first prt of the thesis, PID-bsed system is presented long with the two control schemes. The first is purely kinemtic model nd the second includes dynmic constrints. For both versions, cscded PID design ws employed nd settings were mnully tuned. The finl mthemticl models were computtionlly simulted nd their respective results were nlysed nd compred. Hrdwre vlidtion ws not conducted for this phse of the reserch s the simultion results suggested tht the PID-bsed system my not produce the desired level of trcking performnce. The second prt of the thesis explores model reference dptive control system. Lypunov's direct method ws used to chieve stbility nd convergence in the system. In contrst to the PID-bsed model, vstly more ccurte geometric loclistion technique ws pplied. The reserch identified number of shortcomings in current geometric loclistion methods nd suggested wys to mitigte them. In ddition, novel pproch for detecting fulty sensor redings ws introduced in conjunction with the development of semiredundnt system. The eventul theoreticl model ws tested using computer simultion, nd the outcome ws contrsted with the results of the PID-bsed system. This ws followed by the construction of prototype in order to verify the vlidity of the proposed model. Vrious configurtions of the dptive model were tested nd compred: the two loclistion methods, use of single nd dul sensors, nd ppliction of semi-redundncy. The thesis concludes with the nlysis of results of the prototype testing. The theoreticl propositions in the thesis were shown to be mply vlidted. Suggestions for future reserch work re lso presented. iii

5 Acknowledgements A reserch endevour is kin to long nd chllenging journey. Without the ssistnce, motivtion nd support provided by others long the wy, the tsk would be immensely hrder, if not impossible. To primry supervisor, Prof Sbu John, I would like to express my deepest grtitude for his invluble insight, guidnce nd support throughout the course of my reserch. I would lso like to extend my sincere thnks to my secondry supervisor, Prof John Mo, for the input nd help tht he hs provided me. Specil cknowledgement goes to Prof Jmes Peyton Jones of Villnov University for his technicl ssistnce with the Villnov University LEGO Rel Time Trget (VU-LRT) softwre. Lst but not lest, I would like to offer my hertfelt thnks to my mother nd fmily for ll the love, encourgement, support nd ptience tht they hve shown me over the yers. I would be nothing without them. iv

6 Tble of Contents Abstrct iii Acknowledgements iv List of Figures ix List of Tbles xii Nomenclture xii Chpter 1: Generl Introduction Introduction 1 1. Bckground Motivtion, Objectives nd Reserch Questions Thesis Outline List of Publictions 5 Chpter : Literture Review 6.1 Mthemticl Modelling Methods Kinemtic Modelling 6.1. Dynmic Modelling 7. Nvigtion nd Trcking 7.3 Common Mobile Sensor Technologies nd Methodologies Ded Reckoning Sensors Opticl Encoders Opticl Mouse Sensors 1.3. Rnge Sensors Lser Rnge Finders Other Types of Rnge Finders Lser Scnners Rnge Determintion by Time-of-Flight Method Rnge Determintion by Tringultion Method Rnge Determintion by Interferometry Method Signl Becon Systems 7.4 Control Systems 6.5 Summry 8 Chpter 3: Generl System Anlysis nd Design Mthemticl Modelling Generl Assumptions nd Limittions Kinemtic Modelling Assumptions nd Limittions 31 v

7 3.. Kinemtic Equtions DC Motor Modelling Dynmic Modelling Assumptions nd Limittions Dynmic Equtions Friction Modelling Dynmic Constrints Summry 4 Chpter 4: Computtionl Design of the PID-Bsed Model Generl Outline of the Computtionl Model Assumptions nd Limittions of the Simultion Model Design Detils of the Simultion Model Trjectory Genertor Dul PID Controller Inverse Kinemtics About-Turn Algorithm for Countering Reverse Motion DC Motors Motor Controller Inverse Dynmics Forwrd Kinemtics (Sensor) Summry 58 Chpter 5: Simultion Results of the PID-Bsed Model Results nd Anlysis Discussion nd Conclusion Further Work 74 Chpter 6: Anlysis nd Design of the Adptive System DC Motor, Ger Assembly nd Vehiculr Dynmics Model Reference Adptive Control Feedbck Lineristion Liner Prmeteristion nd Adptive Dynmics Kinemtics nd Geometry of Vehiculr Motion Limittions of the Geometric Approch Euler's Method Revisited nd Hybrid Technique Error Detection nd Correction for Prtil Redundncy Summry 111 vi

8 Chpter 7: Computtionl Design of the Adptive Model Generl Outline of the Computtionl Model Assumptions nd Limittions of the Simultion Model Design Detils of the Simultion Model Trjectory Genertor Inverse Kinemtics Gin Subsystem Inertil Mtrix Centrifugl Mtrix D-Inverse Mtrix Prmeter Updte Subsystem Grouped Prmeter Initilistion Regressor Mtrix Symmetric Positive Definite Mtrix Function System Dynmics Forwrd Kinemtics (Sensor) Design Detil of Deployed Model Prmeter Updte (Deployed Model) System Interfce Dul Opticl Sensors Sensor Processing Hybrid Geometric Loclistion Kinemtic Loclistion Summry 130 Chpter 8: Test Prototype Hrdwre Pltform Lego Mindstorms NXT NXT Device Communictions NXT Direct Current Servo Motor JED Microprocessors AVR00 Single Bord Computer JED AVR00 Bord Modifictions Atmel AVR ATmeg3 Microcontroller SPI on the ATmeg3 in Mster Mode Mnging Two SPI Devices on Single ATmeg3 SPI Port I C Bus on the ATmeg3 in Slve Mode USART on the ATmeg Avgo ADNS-6010 LserStrem Lser Mouse Sensor Write nd Red Opertions on the ADNS-6010 Sensor Sensitivity to Chnges Between Sensor Assembly nd Surfce 149 vii

9 Construction of the ADNS-6010 Dul-Sensor Subsystem Clibrtion of the ADNS-6010 Sensors Construction of Vehiculr Prototype Mesurement of Sensor Position on Trcking Surfce Softwre Pltform Summry 163 Chpter 9: Simultion nd Vlidtion Results of the Adptive Model Simultion Results nd Anlysis Hrdwre Vlidtion Results nd Anlysis Discussion nd Conclusion 184 Chpter 10: Generl Conclusions Future Work 190 References 191 Appendix A 196 A.1 Enlrged version of Adptive Model of WMR (Fig. 7.1) 196 A. Embedded MATLAB Code for Trjectory Genertor 197 A.3 Embedded MATLAB Code for About-Turn Algorithm 199 A.4 Simulink S-Function Code for Symmetric Positive Definite Mtrix 04 A.5 Embedded MATLAB Code for Pck function in System Interfce Subsystem 08 A.6 Embedded MATLAB Code for c1 function in Hybrid Geometric Subsystem 10 A.7 Embedded MATLAB Code for c function in Hybrid Geometric Subsystem 10 A.8 Embedded MATLAB Code for c3 function in Hybrid Geometric Subsystem 10 A.9 Embedded MATLAB Code for c4 function in Hybrid Geometric Subsystem 10 A.10 Embedded MATLAB Code for c5 function in Hybrid Geometric Subsystem 11 A.11 Embedded MATLAB Code for c6 function in Hybrid Geometric Subsystem 11 A.1 Embedded MATLAB Code for cpsi function in Hybrid Geometric Subsystem 11 Appendix B 1 B.1 Code for Atmel ATmeg3 Microcontroller - MouseController.c 1 B. Code for Atmel ATmeg3 Microcontroller - twi_slve.c B.3 Code for Atmel ATmeg3 Microcontroller - twi_slve.h 8 Appendix C 30 C.1 Circuit Schemtics of JED AVR00 Single Bord Computer 30 C. Circuit Schemtics of Core nd Power of JED AVR00 31 C.3 Circuit Schemtics Port A of JED AVR00 3 viii

10 C.4 Circuit Schemtics Port B of JED AVR00 33 C.5 Circuit Schemtics Port C of JED AVR00 34 C.6 Circuit Schemtics Port D of JED AVR00 35 List of Figures Fig..1 Difference in imge contrsts using illumintion by lser nd LED 13 Fig.. Recommended sensor height for ADNS Fig..3 Recommended sensor height for ADNS Fig..4 Time-wlk due to vrition in signl intensity 18 Fig..5 Fixed threshold triggering versus constnt frction triggering 18 Fig..6 Phse shift between trnsmitted nd reflected signls 0 Fig..7 Bet frequency of FMCW system 0 Fig..8 Distnce mesurement by tringultion Fig..9 Opticl obstruction Fig..10 Dul-sensor tringultion Fig..11 Uneven bem focus spot 3 Fig..1 Speckle 3 Fig. 3.1 Differentilly-steered WMR 30 Fig. 3. Simple geometric representtion of sensor positions 34 Fig. 3.3 Approximtion error due to discretistion 35 Fig. 3.4 Block digrm of DC motor system 36 Fig. 3.5 Curved pth segment of WMR 40 Fig. 4.1 PID-Bsed Trcking Control System of the WMR 4 Fig. 4. Pth Genertor Subsystem 44 Fig. 4.3 Dul PID Controller Subsystem 45 Fig. 4.4 Inverse Kinemtics Subsystem 47 Fig. 4.5 Reltive positions of WMR wheels nd primry sensor 49 Fig. 4.6 About-turn mnoeuvre of the WMR 49 Fig. 4.7 Angulr displcement of the bout-turn mnoeuvre 51 Fig. 4.8 DC Motors Subsystem 5 Fig. 4.9 Motor Controller Subsystem 54 Fig Inverse Dynmics Subsystem 55 Fig Inverse Dynmics Algorithm Subsystem 56 Fig. 4.1 Forwrd Kinemtics (Sensor) Subsystem 58 Fig. 5.1 Simultion Test No. 1: Stright trjectory with offset strting point 59 Fig. 5. Simultion Test No. : Stright trjectory with offset strting point 60 Fig. 5.3 Simultion Test No. 3: Sinusoidl trjectory with offset strting point 60 Fig. 5.4 Contrst of reference, kinemtic nd dynmic pths 61 Fig. 5.5 Detils of turning pths t selected timefrmes 6 ix

11 Fig. 5.6 Simultion Test No. 4: Squre trjectory 64 Fig. 5.7 Simultion Test No. 4: Squre trjectory error nlysis 64 Fig. 5.8 Simultion Test No. 5: Squre trjectory with offset strting point 65 Fig. 5.9 Simultion Test No. 6: Sinusoidl trjectory 67 Fig Simultion Test No. 6: Sinusoidl trjectory error nlysis 67 Fig Simultion Test No. 7: Sinusoidl trjectory 68 Fig. 5.1 Simultion Test No. 7: Sinusoidl trjectory error nlysis 69 Fig Simultion Test No. 8: Circulr trjectory (r = m, 1 rev) 70 Fig Simultion Test No. 8: Circulr trjectory (r = m, revs) 71 Fig Simultion Test No. 8: Circulr trjectory (r = m, revs) error nlysis 71 Fig. 6.1 Model Reference Adptive Control System 80 Fig. 6. Feedbck Lineristion 81 Fig. 6.3 Two distinct pths with identicl sensor redings 89 Fig. 6.4 Another exmple of two distinct pths with identicl sensor redings 89 Fig. 6.5 Geometry of sensor movement during vehicle motion 90 Fig. 7.1 Adptive Trcking Control System of the WMR 110 Fig. 7. Trjectory Genertor Subsystem 11 Fig. 7.3 Inverse Kinemtics Subsystem 113 Fig. 7.4 Gin Subsystem 114 Fig. 7.5 Inertil Mtrix Subsystem 114 Fig. 7.6 Centrifugl Mtrix Subsystem 115 Fig. 7.7 D-Inverse Mtrix Subsystem 115 Fig. 7.8 Prmeter Updte Subsystem 116 Fig. 7.9 Grouped Prmeter Initilistion Subsystem 117 Fig Regressor Mtrix Subsystem 114 Fig System Dynmics Subsystem 118 Fig. 7.1 Forwrd Kinemtics (Sensor) Subsystem 10 Fig Adptive Trcking Control System of the WMR (deployed) 11 Fig Prmeter Updte Subsystem (Deployed Model) 1 Fig System Interfce Subsystem 13 Fig Dul Opticl Sensors Subsystem 13 Fig Sensor Processing Subsystem 14 Fig Hybrid Geometric Loclistion Subsystem 16 Fig Kinemtic Loclistion Subsystem 17 Fig. 8.1 Lego Mindstorms NXT 1.0 robotics system (courtesy of Lego) 19 Fig. 8. Lego Mindstorms NXT system rchitecture 130 Fig. 8.3 I C bus interconnection 131 Fig. 8.4 I C dt trnsmission 13 Fig. 8.5 Lego Mindstorms NXT servo motor 133 Fig. 8.6 JED AVR00 Single Bord Computer 135 x

12 Fig. 8.7 JED AVR00 Single Bord Computer PCB lyout 135 Fig. 8.8 SPI trnsmission with vrious clock phse nd polrity settings 138 Fig. 8.9 Avgo ADNS-6010 opticl mouse sensor 143 Fig Avgo ADNS-6010 cross-section of sensor ssembly 143 Fig Timing digrm for dt smpling during write opertions 144 Fig. 8.1 Timing digrm for dt smpling during red opertions 144 Fig Timing digrm for write nd red opertions nd delys 145 Fig Recommended sensor height for ADNS Fig Recommended sensor height for ADNS Fig Circuit schemtic of ADNS-6010 opticl mouse sensor subsystem 149 Fig Two opticl sensors mounted on polycrbonte plte 149 Fig Underside of sensor mounting plte 150 Fig Perspective view of fully-ssembled vehicle 155 Fig. 8.0 Front view of fully-ssembled vehicle 155 Fig. 8.1 Rer view of fully-ssembled vehicle 156 Fig. 8. Side view of fully-ssembled vehicle 156 Fig. 8.3 NXT brick removed to show bttery comprtment 157 Fig. 8.4 Sensor subsystem exposed 157 Fig. 8.5 Underside of vehicle 158 Fig. 8.6 VU-LRT librry blockset for Simulink 159 Fig. 9.1 Simultion Test No. 1: Squre trjectory (m x m) 161 Fig. 9. Adptive Simultion Test No. 1: Squre trj. (m x m) error nlysis 16 Fig. 9.3 Adptive Simultion Test No. : Squre trj. (4m x 4m) 163 Fig. 9.4 Adptive Simultion Test No. : Squre trj. (4m x 4m) error nlysis 163 Fig. 9.5 Adptive Simultion Test No. 3: Squre trj. 1 with offset strt point 164 Fig. 9.6 Adptive Simultion Test No. 4: Squre trj. with offset strt point 165 Fig. 9.7 Adptive Simultion Test No. 5: Sinusoidl trjectory 166 Fig. 9.8 Adptive Simultion Test No. 5: Sinusoidl trjectory error nlysis 166 Fig. 9.9 Adptive Simultion Test No. 6: Sinusoidl trjectory 167 Fig Adptive Simultion Test No. 6: Sinusoidl trjectory error nlysis 168 Fig Adptive Simultion Test No. 7: Tringulr trjectory 169 Fig. 9.1 Adptive Simultion Test No. 7: Tringulr trjectory error nlysis 169 Fig Adptive Simultion Test No. 8: Circulr trjectory (r = m, 1x) 170 Fig Adptive Simultion Test No. 8: Circulr trj. (r = m, x) 171 Fig Adptive Simultion Test No. 8: Circulr trj. (r = m, x) error nlysis 17 Fig Adptive Simultion Test No. 9: Circulr trj. (r = 3m, x) 173 Fig Adptive Simultion Test No. 9: Circulr trj. (r = 3m, x) error nlysis 173 Fig. A.1 Adptive Trcking Control System of the WMR 194 Fig. C.1 Circuit Schemtics of JED AVR00 Single Bord Computer 8 Fig. C. Circuit Schemtics of Core nd Power of JED AVR00 9 xi

13 Fig. C.3 Circuit Schemtics of Port A of JED AVR00 30 Fig. C.4 Circuit Schemtics of Port B of JED AVR00 31 Fig. C.5 Circuit Schemtics of Port C of JED AVR00 3 Fig. C.6 Circuit Schemtics of Port D of JED AVR00 33 List of Tbles Tble 5.1 Summry of simultion test results of PID model 7 Tble 8.1 Lego Mindstorms NXT motor chrcteristics 134 Tble 8. Sensor clibrtion dt 151 Tble 8.3 Clibrted sensitivity of both sensors 15 Tble 8.4 Dt correltion nd surfce qulity of test surfce 15 Tble 8.5 Dimensions of wheeled robot 154 Tble 9.1 Summry of simultion test results of dptive model 174 Tble 9. Trcking errors in dul-sensor configurtion 175 Tble 9.3 Trcking errors in single-sensor configurtion 176 Tble 9.4 Trcking errors in squre trjectory (clockwise) 177 Tble 9.5 Trcking errors in squre trjectory (nticlockwise) 178 Tble 9.6 Trcking errors with nd without error compenstion 180 Nomenclture A nb, nl, nr mx tl, tr x y B b B m, B L C C c D d E E b Loction of first (rer) sensor Distnce between rer sensor nd wheel bseline Norml ccelertion t centre of wheel bseline, left or right wheel Mximum llowble ccelertion without slippge Tngentil ccelertion of left or right wheel Longitudinl ccelertion of vehicle Lterl ccelertion of vehicle Centre of wheel bseline Distnce between centre of grvity nd wheel bseline Viscous-friction coefficient of motor or lod xle Point of contct of the front wheel with the ground Mtrix contining Coriolis nd centrifugl terms Distnce between front wheel nd wheel bseline Coefficient mtrix Trck width of vehicle Applied voltge Bck-EMF xii

14 e crit e t F mx F x F x l, F x r F y F y l, F y r G h q i I z J m, J L K k K b K r K t L L L M m N n g P Q q q ~ q d R r R r crit r pth, l, r pth, r r s RMS RMSE s l, s r Bse- exponent tht corresponds to the criticl rdius Trcking error Mximum llowble force without slippge Longitudinl force exerted by driving wheels Longitudinl forces exerted by left or right driving wheel Lterl force exerted by driving wheels Lterl forces exerted by left or right driving wheel Centre of grvity of vehicle New input for linerised system Armture current Moment of inerti of vehicle Inerti of motor's rotor or lod xle Loction of second (front) sensor Distnce between front sensor nd wheel bseline Bck-EMF constnt Regressor mtrix Torque constnt Distnce between the two sensors Point of contct of the left wheel with ground Armture inductnce Inertil mtrix Mss of vehicle Norml force on vehicle Ger rtio Symmetric positive definite constnt mtrix Strictly positive nd symmetric mtrix Trcking vector Difference between current nd desired vlues of trcking vector Desired vlues of trcking vector Point of contct of the right wheel of vehicle with the ground Wheel (including tyre) rdius Armture resistnce Criticl rdius t which computing precision exceeds sensor resolution Pth rdii for left or right wheel Rdius of circulr pth formed by the centre of the wheel bseline Root men squre Root men squre error Distnce covered by left or right wheel s 1, s Arc length formed by Sensor 1 or SQUAL Surfce qulity xiii

15 T m, T L Motor or lod torque T L, l, T L,r Lod torque on left or right driving wheel u Longitudinl velocity of vehicle u Longitudinl ccelertion of vehicle U i ULP V v v x x' x x, x b, x l, x r x p Δx G Δx 1, Δx, Δx A, Δx K Δx 1 ', Δx ' y y' y y, y b, y l, y r y p Δy G Δy 1, Δy, Δy A, Δy K Δy 1 ', Δy ' α α l, α r β Γ γ ε x, ε y θ m, θ L θ p m L Input vector for dynmic system Unit in the lst plce Lypunov function cndidte Lterl velocity of vehicle Lterl ccelertion of vehicle Position of vehicle on X-xis Position of vehicle on locl X-xis Velocity of vehicle on X-xis X-coordinte of rer sensor, centre of wheel bseline, left or right wheel X-coordinte of pivot wheel Displcement of centre of grvity with respect to X-xis Displcement detected by sensor on X-xis Displcement detected by sensor on locl X-xis Position of vehicle on Y-xis Position of vehicle on locl Y-xis Velocity of vehicle on Y-xis Y-coordinte of rer sensor, centre of wheel bseline, left or right wheel Y-coordinte of pivot wheel Displcement of centre of grvity with respect to Y-xis Displcement detected by sensor on Y-xis Displcement detected by sensor on locl Y-xis Yw (turning) ccelertion of vehicle Angulr ccelertion of left or right wheel Angle between locl X-xis nd tngent to the turning rc Symmetric positive definite constnt mtrix Angle between turning rdii Trcking error on X- or Y-xis Angulr displcement of motor or lod xle Prmeter vector, Angulr velocity of motor or lod xle, Angulr ccelertion of motor or lod xle m L λ Strictly positive number µ Coefficient of friction of tyres σ Stndrd devition ψ Yw (turning) ngle of vehicle xiv

16 ω Yw (turning) velocity of vehicle Yw (turning) ccelertion of vehicle ω l, ω r, Angulr velocity of left or right wheel, Angulr ccelertion of left or right wheel l r xv

17 Chpter 1: Generl Introduction 1.1 Introduction The importnce of robotic devices is reflected in their ever widening use. In mobile robotics, wheeled types re most commonly seen. Of the mny vrieties of wheeled mobile robots (WMRs), one of the populr configurtions is tht of the differentil-drive system. This sort of robot is fvoured for its simplicity in design nd lso for its mnoeuvrbility. Key to the opertion of n utonomous WMR is loclistion the bility to locte itself within frme of reference. An exmple of device tht is ble to trck its position within defined spce is the ubiquitous computer mouse. The principle behind its opticl trcking system cn esily be pplied to utonomous WMRs. Since this trcking system hs strightforwrd design tht requires miniml computtionl power, it would be idel for use on simple nd inexpensive WMR. Indeed, such set-up hs been tested on smll-scle pltforms (O'Hr nd Ky 003; Singh nd Wldron 004). Aprt from cpble trcking system, WMR lso requires n effective system to control nd drive it. Underpinning this control lgorithm is mthemticl model of the behviour nd properties of the WMR. The two min wys in defining mthemticl model re kinemtic nd dynmic modelling (Sh nd Angeles 1989; Cmpion et l. 1996). Due to better prcticl relism, dynmic modelling hs been incresing fvoured over the kinemtic pproch. 1. Bckground The potentilly unlimited possibilities for mobile robotic pplictions hve kept reserchers excited for mny yers. While trditionlly, robots hve seen their biggest deployment in the mnufcturing sector, they re grdully mking inrods into other res such s trnsporttion, medicine nd even consumer products. The dvent of mobile robotics hs opened up new fields of pplictions where conventionl fixed-bse robots would hve been imprcticl. From utomted guided vehicles in industril settings to nscent self-driving crs, the field continues to grow rpidly. Furthermore, mobile robots re especilly useful in hzrdous pplictions such s plnetry, deep-se nd mining explortions, s well s bomb disposl, lndmine detection nd clernce, etc. A crucil fctor tht ffects the performnce of WMR is its bility to keep trck of its current loction. To ccomplish this, wide rnge of methods hve been employed. One of the oldest 1

18 nd most rudimentry pproches is bsed on the ide of reltive positioning nd is clled ded-reckoning. This is usully ccomplished with the use of wheel encoders, but inertil mesurement units (IMUs) such s ccelerometers nd gyroscopes hve seen incresing rtes of doption in recent yers. Another populr technique is depth sensing. This usully involves rnge finders tht emit lser, infrred or sonr. In recent yers, due to the rpid dvncement in computer processing power, vision-bsed positioning using single or dul cmers systems hve lso become incresingly common. Also in use re tringultion, triltertion nd multiltertion techniques vi the use of signl becons or trnsponders. The fixed reference points provide instntneous loction on grid nd re bsed on the principle of bsolute positioning. A lrge-scle exmple of this would be the Globl Positioning System (GPS). However, it hs indequte ccurcy s well s limited use within buildings. Additionl sensors my not necessrily improve robot performnce. Indeed, they my slow processing time, increse heft nd rise energy consumption. A widely-known exmple of trcking system is found on the computer mouse. The opticl lser sensors instlled in the ltest computer mice work on the principle of opticl flow, nd re chep, highly ccurte nd require very miniml computtionl power to operte. Also, they re not susceptible to errors cused by tyre slippge or discrepncy in wheel sizes. This opticl sensor technology hs existed for quite some time now, but there hs not been ny commercil or lrge-scle deployment of such it for nvigtion or trcking purposes. In one of the first ded-reckoning systems tht introduced the use of opticl mouse sensor to replce wheel encoders, n nlogue compss ws lso employed to provide yw redings (Silv et l. 00). Unfortuntely, the reltively low precision of the compss ffected the overll ccurcy of the system. Lter proposls (Bonrini et l. 004; Bonrini et l. 005) employed the use of two opticl sensors tht fully seprted the robot's loclistion from its kinemtics. Despite the high level of precision offered by the sensors, the finl outcome ws still pprecibly ffected by systemtic nd non-systemtic errors. Further studies (Minoni nd Signorini 006; Plcin et l. 006) looked into the fctors tht impct upon the ccurcy of opticl mouse sensors in order to understnd why they hve not been ble to deliver the ccurcy expected of them. In the following yers, there hve been continuing ttempts to improve the trcking ccurcy of robotic vehicle tht use mouse sensors for nvigtion. Nturlly, there is lso interest in extending the ppliction of mouse sensors to wider rnge of conditions - such s the outdoors (Jckson et l. 007). These sensors hve been suggested for use in robot swrms (Gustfson et l. 005) s well s unmnned flying vehicles (Thkoor et l. 004). Some hve dded other types of sensors or devices to work longside mouse sensors in order to supplement or enhnce their performnce. A number of these sensor-fusion pltforms hve dded inertil mesurement units to provide better verstility especilly when the mouse sensors re deployed in conditions where the trcking surfce is not uniformly even (Hyun et

19 l. 009; Prk et l. 009; Dille et l. 010). Other systems employ dditionl cmers tht re not ttched to the vehicles but plced t certin loctions (Sekimori nd Miyzki 005; 007). However, the prcticl limittions of such systems re obvious. Aprt from the precision of the sensors, the effectiveness of WMR s trcking control system lso depends very much on the soundness of its underlying theoreticl model. In recent yers, dynmic-bsed modelling (Zho nd BeMent 199; Zhng et l. 1998; 003) hs become incresingly populr s techniques nd technology continue to improve. The kinemtic pproch (Oriolo et l. 00) is well-estblished nd retins much populrity due to its computtionl simplicity. However, by not ccounting for dynmic forces, this method fces severe prcticl limittions even t reltively low speeds (Boyden nd Velinsky 1994; Hong et l. 1999). As result, under- or over-steer often occurs due to significnt wheel/tyre slippge. While it is very difficult to tke every dynmic effect into ccount, dynmic modelling is generlly more relistic solution thn purely kinemtic pproch. 1.3 Motivtion, Objectives nd Reserch Questions The issue of trcking control precision is highly importnt in the study of utonomous robots. Without relible solutions, pplictions will remin limited. Over the yers, ccurcy in trcking control hs improved mrkedly. Numerous methodologies nd technologies hve been introduced nd tested. However, costs, complexity nd required processing power hve esclted with incresed precision. At present, trcking sensor systems used on WMR pltforms rnge from light nd chep encoders to ultrsonic sensors s well s bulkier nd more expensive lser scnners nd cmer set-ups. The chllenge is to produce n ccurte trcking system without the exorbitnt costs normlly ssocited with high-precision pltforms. A prospective lterntive is the opticl mouse sensor. Bsed on its extremely high scle of precision, lbeit with known limittions, it ws decided this could be vible option to investigte. Currently-vilble mouse sensors cn detect tiny movements rnging from 1.7 x 10 - mm (000 dpi) (Avgo 009b) to 4.48 x 10-3 mm (5670 dpi) (Avgo 011), nd re more tolernt thn previous models of slight height chnges between sensor nd surfce. Yet, despite such high level of sensor precision, studies hve repetedly shown tht WMRs using such sensors hve tendency to devite by mrgin of tens to hundreds of millimetres from their rel-world position fter trvelling distnce of only few metres. It remins to be thoroughly explined why the mgnitude of the trcking error grows so quickly. It is cler tht such rtes of error ccumultion plce serious limittions on the pplicbility of this prticulr type of technology for nvigtionl purposes. Thus, it is importnt to find out 3

20 why mobile robots re unble to deliver the level of ccurcy commensurte with their highprecision opticl mouse sensor systems. Ultimtely, it is the im of this reserch to explore the fesibility of developing n ccurte trcking system tht is inexpensive, hs low processing demnds, requires no ssistnce from externl reference devices, nd is esily deployble. To chieve this gol, it is vitl to investigte why others hve not chieved the optiml results s expected when using such high-precision opticl sensors. Also, vrious trcking control methods would be nlysed nd compred, nd the most effective one dopted for use. The reserch would be guided by the following questions: 1. Is the trcking control lgorithm robust enough for its purpose?. How does the performnce of kinemtic nd dynmic models compre? 3. Could reltively poor trcking ccurcy be ttributed to inherent sensor limittions? 4. How ccurte re the odometric clcultions for loclistion? 5. Are there ny other yet unknown fctors tht hve n effect on trcking performnce? 6. Wht could be done to improve the performnce nd vibility of systems tht use this technology? It is hoped tht the finl reserch outcome would be ble to nswer these questions nd deliver n ccurte trcking control pltform without the high cost or complexity usully ssocited with high-precision systems. This would be demonstrted by using off-the-shelf components to build n inexpensive vehiculr pltform coupled with pir of opticl mouse sensors to provide feedbck informtion with level of redundncy. 1.4 Thesis Outline This chpter introduces the min ide of the thesis nd outlines the motivtion nd objectives of the reserch. The rest of the thesis is orgnised in the following mnner: Chpter provides literture review of ll the slient topics tht re discussed in this thesis. This includes kinemtic nd dynmic modelling techniques, nvigtion nd trcking methods, common sensor technologies nd methodologies, nd vrious types of control systems. Chpter 3 explores in detil the theoreticl model of the wheeled robot. Kinemtic nd dynmic models re nlysed mthemticlly, nd friction modelling is introduced. 4

21 Chpter 4 describes the computtionl design of PID-bsed WMR. All components nd lgorithms re modelled in softwre. Chpter 5 presents the simultion results of the PID model. The outcome directly ffects the decision on whether to proceed to hrdwre vlidtion or dopt new or revised mthemticl model. Chpter 6 exmines the mthemticl model of n dptive system in detil. Two techniques for clculting odometry re explored nd prtilly-redundnt system is proposed. Chpter 7 explins the computtionl design of n dptive system. All components nd lgorithms re modelled in softwre. Chpter 8 illustrtes the construction of the test prototype. All hrdwre components nd their usge re described in detil. The softwre used for the reserch is lso outlined. Chpter 9 presents both the simultion nd vlidtion results of the dptive model. Single- nd dul-sensor configurtions re compred, nd the prtilly-redundnt system is put to the test. Chpter 10 consists of the overll conclusions of the reserch nd proposes dditionl work for future considertion. 1.5 List of Publictions Yeo, L., S. John, et l. (008), Simultion of n Opticl Trcking Control System of Differentilly-Driven Wheeled Mobile Robot, Proceedings of the Interntionl Conference on Modeling, Simultion & Visuliztion Methods (MSV 008), pp Yeo, L., S. John, et l. (009), Simultion of n Opticl Trcking Control System of Differentilly-Driven Wheeled Robot, Interntionl Journl of Computer Aided Engineering nd Technology, vol. (1): pp

22 Chpter : Literture Review Every reserch project begins with comprehensive review of the ccumulted knowledge within the field. Only with thorough understnding of the subject cn reserch then be undertken. The literture review will include topics such s mthemticl modelling methods s well s sensor technologies nd methodologies..1 Mthemticl Modelling Methods The most logicl method for the modelling of properties nd behviour of WMRs is mthemticlly-bsed. The process usully begins with the derivtion of kinemtic equtions to describe the movement of the WMR. This is followed by the formultion of dynmic equtions tht tke forces into ccount. Nturlly, the ltter pproch provides more relistic model. Currently, mny modelling methods for differentil drive WMRs ignore the presence of longitudinl or lterl wheel/tyre slippge in order for pth plnning to be bsed purely on kinemtics. Even with the smoothest of trjectories, this model is highly idelistic nd prone to inccurcies in prcticl pplictions. More recent proposls hve included dynmic considertions to minimise longitudinl slip due to excess torque, s well s lterl slip due to turning mnoeuvres. There hs lso been suggestion on the use of Guss-Newton lgorithm to predict positionl errors (Seyr nd Jkubek 005). It remins to be seen how well predictive methods perform when unforeseen situtions rise..1.1 Kinemtic Modelling The most bsic mthemticl technique for describing the movement of n utonomous robot is bsed on kinemtics. Hence, some form of kinemtic modelling is employed in the motion control of most mobile robots. With knowledge of wheel dimeter, trck width, nd other slient vehiculr dimensions trcking-sensor informtion, the moving velocity nd ccelertion of the WMR cn be derived from the ngulr velocity of its wheels nd vice vers. Additionlly, the rottionl displcement nd turning velocity of the WMR cn be obtined by nlysing the difference between the ngulr velocities of its wheels nd vice vers. For the mobile robot to get to destintion, route is first defined nd then divided into numerous checkpoints. The llotted time for the WMR to get from one checkpoint to the next, s well s the loctionl informtion of the checkpoint reltive to neighbouring ones will 6

23 determine the required trvelling speed nd direction for the prticulr segment of the route. This is essentilly how the motion of typicl WMR is kinemticlly controlled. By ignoring dynmic forces, purely kinemtic pproch hs been shown to produce unrelistic results t elevted lods nd speeds (Boyden nd Velinsky 1994; Hong et l. 1999). As consequence, there re significnt limittions to the pplicbility of solely kinemtic systems (Boyden nd Velinsky 1994b). Hence, in recent yers there hs been trend towrds the ppliction of dynmic modelling to mobile robotics..1. Dynmic Modelling Currently, there re severl mthemticl pproches to dynmic modelling of WMRs. Newton-Euler (Zhng et l. 1998) nd Euler-Lgrnge (d'andre-novel et l. 1991) formultions of mechnics re both well-estblished nd re nturlly most commonly dopted. However, more recent method proposed by Kne nd Levinson (1983) hs lso been well-received by reserchers (Thnjvur nd Rjgopln 1997; Nukulwuthiops et l. 00). Essentilly, ll three pproches re simply vrints of clssicl mechnics. Lgrngin mechnics is re-formultion of Newtonin mechnics while Kne s method is n dpttion of Lgrngin mechnics. Wheres the Newtonin pproch concentrtes on the blnce of forces, the Lgrngin technique focuses on the conservtion of energy. As for Kne s method, it ws developed primrily for nlysing non-holonomic systems such s multi-body objects. Considering tht the WMR used in this study is of reltively simple design, strightforwrd nlysis ws initilly chieved by dopting the Newtonin pproch. The Euler-Lgrnge method is used in lter model incorporting dptive control.. Nvigtion nd Trcking The most fundmentl requirement for the successful nvigtion of mobile robot is its bility to know its position with n cceptble degree of certinty t ny given time (Borenstein et l. 1997). This concept is clled loclistion. One of the methods tht hs been used for long time is tht of ded-reckoning. It is populr becuse of the simplicity of the technique. Essentilly, the current position of n object of interest is determined by how fr it hs moved in ll pertinent xes from its lst known 7

24 loction. This cn be derived from informtion such s distnce, speed, heding nd length of time between mesurements. Without priori knowledge of previous reference point, this pproch will not work. A mjor drwbck of this method is tht inccurcies in the mesuring these vribles would result in positionl errors tht ccumulte over time. There re two kinds of errors inherent in ded-reckoning: systemtic nd non-systemtic. The first type is directly relted to the properties nd chrcteristics of the vehicle. Fctors such s unequl wheel sizes, inccurte mesurements of wheel dimeter, trck, or wheel bse, would result in the erroneous clcultion of vehicle's position. These bised errors cn be reduced by creful clibrtion of the system. The second type of error is often hrder to identify nd minimise. Non-systemtic errors re usully rndom nd thus impossible to predict. Exmples of such errors re sensor noise, environmentl fluctutions, etc. When mobile robot is ble to loclise itself, the next step would be to develop trcking lgorithm where it is ble to follow prescribed route or hed towrds prticulr destintion. The type of control strtegy employed is determined by the objective of the opertion. The methods most commonly seen re pth following, trjectory trcking nd point-to-point stbilistion. If it is necessry for the WMR to dhere to desired pth, then route within the reference frme must be mpped out beforehnd. This type of control strtegy is clled pth following. In this cse, the WMR is llowed to set its own speed s long s it stys on the pth. However, if the WMR were to be required to not only stick to given course but lso be t prticulr loctions long the course t specific times, then its speed will be regulted by time constrints. This form of control strtegy is termed trjectory trcking. In the cse where the WMR is only required to trvel to trget loction without pre-defined route or time limit, it is essentilly free to decide its own pth to the destintion s well s its speed long the wy. This mode of control strtegy is clled point-to-point stbilistion. An importnt cvet regrding the ltter pproch is tht the WMR must be ble to detect nd void obstcles nd hzrds while on its wy to its finl destintion. This would require dditionl sensors nd complex lgorithms which would led to incresed computtionl demnds. If there re no mps or guides vilble from the outset nd the WMR is required to chrt its own mps, then this is the only logicl option. In contrst, if cler nd sfe route cn be specified from the beginning, s required by the first two strtegies, then such problems my be voided. Depending on the opertionl requirements, ech of these schemes hs its dvntges nd disdvntges (Srkr et l. 1993, 1994). 8

25 As no system is perfect, it is inevitble tht trcking errors will occur t some point. During the mp-building phse, trcking errors cn led to serious distortions nd would require numerous psses nd clever lgorithms to rrive t some sort of convergence. If the reference mps re poorly chrted due to trcking errors, it would be impossible for the WMR to lter follow its intended pth. Insted, the WMR would ttempt to dhere to distorted mp nd end up somewhere it is not supposed to be. At present, common pproch to loclistion nd mp-building is the employment of probbilistic mens (Leonrd nd Durrnt- Whyte 1991; Durrnt-Whyte nd Biley 006) to mtch nd merge visully-detected fetures. Using this method, WMR is ble to determine its own loction nd t the sme time build up mp s it goes long. This technique is clled Simultneous Loclistion nd Mpping (SLAM) (Montemerlo et l. 00, 003). The sitution with trcking error is less criticl if n cceptbly ccurte mp hs lredy been pre-progrmmed in robot s memory. The mp could hve been input mnully or generted utonomously from previous surveys. Current imge nlysing technology is quite cpble in iding the loclistion nd orienttion of robot using comprisons of visul fetures, such s lndmrks or contours, with its stored mps. Similrly, sonr systems re ble to discern notble fetures by exmining signl echoes. Another populr method is the mesurement of distnce trvelled by ech wheel vi the use of wheel encoders. These imge, sonic or odometric feedbck systems help to keep the robot on desired pth. Awy from simulted settings, it remins to be seen how well current methods cope with chllenging rel-life situtions such s environments with few noticeble fetures, or surfces tht re severely uneven or slippery. A simple rel-world test for the trcking ccurcy of WMR is the clibrtion procedure clled the UMBmrk (Borenstein nd Feng 1995). It would one of the tests included in this study lthough the clibrtion process of UMBmrk would not be used..3 Common Mobile Robot Sensor Technologies nd Methodologies The nvigtionl bility of n utonomous WMR depends criticlly on how well it senses its surroundings. Not only must the robot be ble to see where it is going, it must lso be ble to know its current loction within specified frme of reference. Without some form of sensor, the WMR will simply not be ble to find its wy round. Before decision is mde in regrds to the type of nvigtionl sensor to be dopted, n exmintion of common sensor technologies nd methodologies must first be conducted. The inherent dvntges nd disdvntges of these technologies will be compred with one nother in order to mke the most pproprite selection to meet the objectives of this study. 9

26 .3.1 Ded Reckoning Sensors In the rt of nvigtion, one of the oldest wys of determining the current loction of trveller or vessel is method clled ded reckoning. Essentilly, with known previous position nd well-documented course leding from tht position to the current one, the present loction cn be deduced from the distnce between the two points. If the distnce trvelled is not known directly, tht informtion cn often be derived lterntively from the speed nd time of trvel. This technique is strightforwrd nd well-tested. Hence, it is widelydopted for nvigtionl purposes including mobile robotic nvigtion. A bsic form of ded reckoning is method clled odometry. The word is derived from odometer - the Americn term for device tht mesures vehiculr displcement, especilly tht of motor cr. This is direct pproch where trvelled distnce is determined solely by using instrumenttion bord the object of interest. In contrst, indirect methods, such s tringultion or multiltertion, obtin displcement informtion vi derivtion Opticl Encoders Mobile robots come in mny forms. Some re irborne while others my hve legs. However, the most common types employ wheels or continuous trcks for locomotive purposes. Most of these wheeled robots use rotry encoders to mesure trvelling speed nd distnce. Rotry encoders re generlly used for mesuring the ngulr velocities of rotting shfts nd xles. Knowing the ngulr velocity s well s the overll dimeter of prticulr wheel nd its tyre, the speed nd distnce trvelled by ny wheeled vehicle cn be esily determined. For wheeled robots, encoders re often used for monitoring the rottion of pir of wheels, ech locted on opposite sides of the robot. When the robot is trvelling in stright line, the ngulr speed of equl-sized wheels on opposite sides re identicl, sve for equipment tolernces. If the robot is negotiting turn, the rottionl velocities of both wheels will nturlly differ from ech other. Using this informtion, the robot's direction cn be obtined in ddition to its moving speed. When the robot opertes over known period of time, the trvel route s well s velocity, displcement nd direction cn ll be determined quite esily. Mgnetic nd opticl encoders re the most common kinds of rotry encoders encountered nowdys. For mobile robotics, opticl encoders re generlly preferred. The opertion of n opticl encoder typicl involves focused ry of light intercting with ptterned disc ffixed to rotting shft. The result is pulsed signl which is then picked up by photosensor. This signl is digitlly nlysed by microprocessor to obtin informtion on ngulr velocity nd position. 10

27 The purpose of the coded pttern is to produce binry output from the interction with the light ry. This outcome will depend on where the light lnds on the disc. The design of the encoder will determine whether the emitter nd photodetector re locted on the sme side or opposite sides of the ptterned disc. In the former cse, the disc pttern is rrnged in such wy where there re contrsting regions of high nd low reflectivity. Consequently, the intensity of the light tht bounces off the ptterns cn be sorted into two levels: high nd low. This reflected signl is then picked up by the sensors nd interpreted s binry output. In the ltter cse, binry signl is generted by the selective obstruction of the light ry. In order for light to pss through intermittently, the disc is either trnsprent of perforted in certin regions specified by the coded pttern. The light ry is thus trnsformed into n on-off signl which cn be detected by sensors on the other side of the wheel nd then trnslted into binry dt. Opticl encoders hve two min vrieties: incrementl nd bsolute. For incrementl encoders, ngulr velocity is mesured directly while reltive position is derived. In contrst, for bsolute encoders, ngulr position is mesured directly while velocity is derived. Due to the fct tht incrementl encoders contin fewer sensors, they re usully much cheper thn bsolute types. The min disdvntge of rotry encoders is tht they re prone to odometric errors due to their inbility to detect wheel slippge. An encoder will report the rotry speed of wheel (or xle) tht is being monitored regrdless of whether there is full trction between the wheel (or tyre) nd trvelling surfce. If there is slippge during vehicle ccelertion, onbord encoders will likely report the vehicle speed (nd distnce trvelled) to be higher thn in relity. On the other hnd, if slippge occurs during vehicle decelertion, the speed of the vehicle (nd distnce trvelled) s determined by onbord encoders will tend to be lower thn it ctully is. Odometric errors ccumulte nd re unbounded. Thus, even smll but consistent mount of slippge cn quickly dd up to level where it begins to hve substntil impct on opertionl ccurcy. WMRs tht rely on wheel encoders for positionl trcking require quite some effort to clibrte (Borenstein nd Feng 1995). Furthermore, it is essentil for slippge to be minimised, so vehicle speeds re often kept reltively low. 11

28 .3.1. Opticl Mouse Sensors The computer mouse ws invented by Dougls Engelbrt t the Stnford Reserch Institute (now clled SRI Interntionl) in It ws designed s n input device with two wheels perpendiculr to ech other nd ttched to potentiometers llowing it to trck its movement long the horizontl nd verticl xes. Engelbrt s chief engineer, Bill English, who helped build the first mouse t SRI lter moved to Xerox PARC nd developed the bll mouse in 197. Using combintion of opticl encoders nd mechnicl rollers ctuted by bll, this design becme the de-fcto stndrd until the 1990s when it begn to be supplnted by fully opticl systems. The opticl mouse sensors seen tody come in two types: light-emitting diode (LED) nd lser. LED sensors tend to be older nd re slowly being replced by lser versions. While the source of illumintion my differ in both vrints, the principle behind how they work is identicl. The purpose of the LED or lser is to light up smll re of surfce to the extent where n imge cn be clerly cptured by tiny cmer within the sensor. This process is repeted continuously s the mouse is moved cross surfce. Using digitl signl processing, the imges re compred to one nother to determine the distnce nd direction trvelled. This principle is clled opticl flow (Beuchemin nd Brron 1995). The mjor dvntge of lser over LED is light coherence. Specificlly, coherent illumintion llows imges to be cquired with higher contrsts compred to non-coherent light source like LED. The obvious benefits re better sensitivity nd precision. This mens tht surfces tht hve in the pst proved to be tricky for LED sensors, such s low-detil or glossy plnes, no longer pose problem s shown in Fig..1. Mnufcturers hve climed tht lser sensors cn provide contrst improvement of up to 0 times over LED counterprts (Agilent Technologies 004; Teo 006). The bility of n opticl mouse sensor in detecting movement is clled its sensitivity. This is usully mesured in counts (sometimes clled dots) per inch, i.e. cpi or dpi. The typicl sensitivity of sensors tht re currently deployed rnge from 000 dpi to 5670 dpi. This trnsltes to resolution tht rnges from 1.7 x 10 - mm to 4.48 x 10-3 mm respectively. This level of precision is virtully unmtched by most other types of sensors. Nowdys, some of the high performnce sensors cn rech sensitivity of up to 800 dpi (Avgo 01). One of the drwbcks of opticl mouse sensors is the tendency for reding errors to occur if there is just bit of fluctution in the gp between the sensor nd trcking surfce. Compred to previous models, newer sensors re generlly more tolernt of such height vrition. Refer 1

29 to Figs.. nd.3 for comprison of Avgo sensors ADNS-051 (relesed in 005) nd ADNS-6010 (relesed in 006). Fig..1 Difference in imge contrsts using illumintion by lser nd LED (Logitech 004) Fig.. Recommended sensor height for ADNS-051 (Avgo 009) 13

30 Fig..3 Recommended sensor height for ADNS-6010 (Avgo 009b).3. Rnge Sensors Rnge (or depth) sensors hve long been used in mobile robotics for obstcle voidnce, lndmrk detection, loclistion nd mp building. The generl principle behind the workings of these sensors is quite simple. An initil signl is emitted. This signl will trvel until it mkes contct with n obstcle nd reflect bck to the sensor. The totl elpsed time is then processed nd the distnce between the sensor nd obstcle cn be determined. By nlysing series of emitted nd reflected signls, pproximte profiles of the obstcles within the sensor's view cn be shped. At the sme time, the robot will be ble to scertin its loction with respect to the surveyed environment. As the technique known s simultneous loclistion nd mpping (SLAM) becomes incresingly populr, so hs the doption of depth sensors Lser Rnge Finders In recent yers, lser is incresingly used in rnge-finding pplictions. This is due to its high ccurcy nd fst response. Also, dvncement in lser technology hs gretly improved mesuring distnce to rnge tht ws previously possible only with the use of rdio or sound wves. In vcuum nd ir, the propgtion cpbilities of lser re excellent. Hence, the chievble rnge of mesurement is very good. On the other hnd, the trnsmission of high frequency wves, such s light, is severely inhibited by bsorption effects of denser medium like wter. Therefore, underwter pplictions re limited to those tht require short opertion rnge. It is importnt to note tht lser light cn fll within the visible rnge or within invisible spectrums such s infrred or ultrviolet. 14

31 Lser is essentilly form of highly-concentrted light. Hence, its speed of trvel is identicl to tht of light nd is similrly influenced by physicl properties of the propgtion medium, such s its composition, density, temperture, pressure nd humidity. For light nd other electromgnetic wves trvelling through medium like ir, the effects of vrition in temperture, pressure nd humidity re very smll. When distnces involved re short, they re essentilly negligible. In contrst, the speed of sound in ir is noticebly ffected by temperture. For exmple, t tmospheric pressure, the speed of sound t 0 C is m/s while tht t 30 C is 349. m/s. When the speed of trvelling wve is ffected by chnges in properties of the propgtion medium, the phenomenon is clled refrction. In pure vcuum, refrction does not occur nd its refrctive index is nominlly set t the vlue of 1. The refrctive indices of ll other mediums re compred to this bseline. Using the modified Edlén eqution (Edlén 1966), the refrctive index of ir is clculted to be t stndrd temperture nd pressure nd with reltive humidity of 50%. This figure is hrdly different from tht of vcuum nd shows the wek refrctive effect of ir. A chnge of 10 C in temperture results in vrition of less thn 0.001% in the refrctive index. A chnge of 30% in reltive humidity results in vrition of bout 0.000% in the refrctive index. Thus, it is quite evident tht chnges in properties of ir hve little effect on refrction. For short-rnge mesurements in ir, refrction cn sfely be ignored. For very long distnces, refrction cn distort the direction of lser bem s well s contribute to phenomenon clled scintilltion. Scintilltion occurs when pockets of tmospheric turbulence or therml fluctutions led to loclised vritions in the refrctive index. A lser pssing through such region my undergo constructive nd destructive interference. Ultimtely, this cn give rise to chnge in signl intensity nd phse. As indicted previously, the refrctive property of ir is very smll. Thus, scintilltion is only n issue when distnces re lrge. Aside from tmospheric disturbnces, suspended prticles in the ir cn lso hinder the effective propgtion of lser bem by scttering it. Aerosols, such s dust nd fog, cn gretly ffect the clrity of ir nd hence limit the opertionl distnce of lser rnge finder. Under such circumstnces, the ctul trget surfce my be obscured, thus leding to erroneous depth redings. The cpbilities of lser rnge finders re not limited by the chrcteristics of the trnsmission medium lone. Surfce ttributes of trgeted objects re nother significnt fctor. An lmost perfectly-blck surfce will bsorb most of the energy of n incident lser bem nd result in hrdly ny detectble reflection. On the other hnd, mximum reflectivity my not be desirble either. An incident lser bem rriving t smooth nd mirror-like surfce will reflect in nrrowly-defined direction without losing much focus. This phenomenon is clled 15

32 speculr reflection. So, unless the sensor is in the pth of the reflected bem, no signl would be recorded. Indeed, slight roughness on the surfce of n object ids the detection of lser on trgeted spot. This is becuse the diffuse reflection of lser bem in multiple directions substntilly increses the chnces of detecting reflected signl. Diffuse reflection is lso known s Lmbertin reflection. Another serious problem is tht of trnsprent surfces. Lser bems trvel right through them with little or no reflection. If the medium is thick, the lser bem my lso undergo considerble refrction. As such, redings would be seriously flwed. The shpe nd externl fetures of trgeted object nd its environment cn lso ffect the ccurcy of lser rnge finder. From certin ngles, surfces my be obscured. Furthermore, echoes cused by multiple deflections of lser bem cn produce completely misleding results. This problem is clled cross-tlk nd is much less severe with lser thn with rdr or sonr due to the highly-focused bem of lser. For lser-bsed systems, opticl cross-tlk would only pose serious issue in n environment full of highly polished surfces. Indeed, more likely to occur is electronic cross-tlk, which is the interference of signls within the circuitry. Other surfce fetures such s discontinuities cn lso cuse reding uncertinties. If lser bem is trgeted directly on surfce edge, there is possibility tht prt of the bem would be reflected bck to the sensor while the remining portion would continue until it reches nother surfce before n echo is detected. This mens multiple return signls would be received for one originl trnsmission. This problem of conflicting rnge redings is known s the mixed pixel effect s explined by Hebert nd Krotkov (1991). As such, it is importnt to keep the bem-width s nrrow s possible. The inevitble divergence of lser bem mkes the problem more pronounced t long distnces. Some filtering techniques hve been proposed to del with this problem, but their computtionl requirements render them too slow for rel-time pplictions s explined by Tuley, Vndpel et l. (005). Environmentl fctors side, the ccurcy of lser rnge finders is ultimtely dependent on the integrity of the electronic circuitry nd the trnsmission stbility of the lser diode. In generl, for depth-sensing purposes, the current view holds tht the benefits of using lser outweigh its drwbcks. This explins the populrity of the technology. Currently, there re numerous types of lser rnge finders vilble. Most of them re bsed on one of the following three min principles: time-of-flight (TOF), tringultion nd interferometry. 16

33 .3.. Other Types of Rnge Finders Some other forms of depth-sensing systems employ similr principles to those behind the lser rnge finder. The min difference is in the trnsmitted signl. Insted of coherent light found in lser, norml signls within the electromgnetic (EM) spectrum, such s infrred, microwve or rdio wves, re emitted. The use of rdio wves for rnge determintion is commonly known s rdr n cronym for rdio detection nd rnging. Another populr depth detection technique uses mechnicl wves in the form of sound. This ltter method is lso clled sonr short for sound nvigtion nd rnging (Lnger nd Thorpe 199). To prevent the sound signls from being n nnoynce to humn observers nd bystnders, the emitted frequencies re usully in the ultrsonic rnge Lser Scnners Rnge finders re found t the core of mny lser scnners. Essentilly, scnner directs rnge finder to sweep cross trget re in order to gther multitude of distnce dt points (Curless 1999). The type of illumintion cn be ctegorised into three brod groups. In one-dimensionl illumintion, point bem is projected on the surfce of n object. The bem is then swept in two dimensions cross the trget re. For two-dimensionl illumintion, the projected imge is often line. This requires scnning to be crried out in one dimension. Three-dimensionl illumintion employs structured light techniques, such s Moire ptterns. No scnning is required s trget re is illuminted ll t once Rnge Determintion by Time-of-Flight Method In time-of-flight systems, the key mesurement is the time tken for trnsmitted signl to be reflected bck from trget. Knowing the speed of light nd compensting for the effects of temperture, pressure nd humidity on the trnsmission medium, the distnce of trget cn esily be determined from the mesured time-lg. TOF rnge finders tend to hve good operting rnge, nd it is not uncommon to find one tht cn mesure over kilometre. TOF systems cn be further ctegorised into two generl sub-types: pulse modultion nd continuous wve (CW) modultion.. Pulse Modultion This is the sme time-tested principle behind the rdr nd sonr. When lser is employed s the trnsmitted signl, it is clled lidr (Light Detection nd Rnging) or ldr (Lser Detection 17

34 nd Rnging). The ide behind pulse modultion is simple. A short signl pulse is trnsmitted while detector wits the echo rebounding from trget. While this technique is strightforwrd, some inherent difficulties exist. The speed of light is pproximtely 300x10 6 m/s. For TOF system, light hs trvel twice the distnce between the sensor nd object. So, for distnce of 1 m, the time tken for pulse to be emitted nd received is bout 6.66x10-9 s. Such short timings require pulsed TOF systems to hve highly precise electronic circuitry. Even n error of nnosecond would led to enormous inccurcies. The problem becomes less cute s distnce increses. Another form of uncertinty rises from vritions in the intensity of detected signls. The rise time of return signl is dependent on signl strength, i.e. the greter the signl intensity, the quicker the pek is reched. Thus, for identicl distnces, systems tht employ fixed threshold triggering would record stronger signl erlier thn weker one s demonstrted in Fig..4. Since more reflective surfces produce stronger return signls, they will pper to be nerer thn less reflective ones. This discrepncy is clled time-wlk. A common remedy for this problem is the use of constnt-frction discrimintors. Fig..5 shows how detection is triggered on these devices when predefined frction of the pek mplitude is reched. However, s the vlue of the constnt-frction is set rbitrrily, this method cn only be seen s n incomplete solution. The time-wlk problem is compounded by the mixed pixel effect if there is shrp trnsition between res with lrge difference in reflectivity. Fig..4 Time-wlk due to vrition in signl intensity (Donti 004) 18

35 Fig..5 Fixed threshold triggering versus constnt frction triggering (Dschwen 006) Ambiguity in signl detection cn rise if pulse is emitted before the return signl of previous pulse is received. In such cses, it would be impossible to distinguish which return signl corresponds to which originl pulse. If both return signls overlp, the resulting interference my render the redings useless. The time required for signl to return depends on the distnce. Hence, the pulse repetition intervl must be djusted to suit the rnge of the opertionl environment. If the time intervl between consecutive pulses is too short, it would led to mbiguous redings. On the other hnd, long pulse intervl will result in low rte of mesurement. b. Continuous Wve Modultion In continuous wve (CW) modultion, the lser is trnsmitted vi crrier signl usully of lower frequency within the RF (rdio frequency) bnd. When this modulted signl is reflected bck from trget, it exhibits shift in either its phse or frequency. This shift trnsltes to time-lg, which in turn is used to determine distnce. Phse shift is mesured using mplitude-modulted continuous wve (AMCW), while frequency shift is mesured using frequency-modulted continuous wve (FMCW). CW systems re considered prt of the TOF fmily becuse they ultimtely mesure the time intervl between signl trnsmission nd reception, lbeit indirectly vi phse or frequency shifts. As CW systems do not directly mesure the trvel time of signl, they do not require the complicted high-speed electronics of pulsed systems. i. Continuous Wve vi Amplitude Modultion AMCW systems trnsmit t the constnt frequency of crrier wve. The lser signl informtion is conveyed by vritions in the signl strength of the crrier wve. As the 19

36 frequency of the crrier wve is lredy predetermined, phse shift between the emitted nd reflected signls cn esily be converted to corresponding time intervl s shown in Fig..6. However, constnt phse shift is repeted every cycle. Therefore, mesurement of ny distnce beyond hlf the wvelength of the crrier frequency would led to mbiguities. The mbiguity intervl is hlf the crrier wvelength becuse the distnce between the rnge finder nd the trget is hlf of the trvel distnce of the lser. In light of this importnt fct, it must be ensured tht the rnge finder does not operte in n environment where there is possibility of distnces exceeding the mbiguity intervl. To be certin, the crrier wvelength should be set in such wy where there would be plenty of llownce. Fig..6 Phse shift between trnsmitted nd reflected signls (Everett 1995) ii. Continuous Wve vi Frequency Modultion In FMCW systems, the lser signl informtion is trnsmitted by vritions in the instntneous frequency of crrier wve while the crrier mplitude remins constnt. Generlly, the crrier wve is of the sw-toothed or tringulr types. When reflected signl is mixed with the originl signl, the resulting frequency shift cretes bet frequency tht is proportionl to the rnge s illustrted in Fig..7. Unlike AMCW systems, bsolute distnce cn be mesured with no mbiguity. However, FMCW systems re not without problems. Due to the fct tht the lser signl is conveyed by vritions in the crrier frequency, ny fluctution in the lser frequency itself cn clerly ffect the frequency of the modulted wve. As the response of lser diode to modultion frequency is seldom liner, nd lser frequency is highly sensitive to temperture chnges in the lser diode, signl stbility is thus difficult to chieve. Even with the use of therml control over the lser diode, signl drift cnnot be completely eliminted. Difficulties in ttining linerity nd design complexities explin why FMCW systems re quite uncommon. 0

37 Fig..7 Bet frequency of FMCW system (Hncock 1999).3..5 Rnge Determintion by Tringultion Method The premise behind the tringultion technique is the utilistion of simple geometry to determine distnce s opposed to mesurement of elpsed time used by TOF systems. Using this principle, wide vriety of rnge-sensing techniques hs been developed. A key drwbck of the tringultion method is tht mesurement resolution decreses s rnge increses. Hence, this method is unsuitble for long rnge pplictions. Tringultion methods cn be pssive or ctive. Pssive systems rely on mbient light nd do not emit ny rdition. Active systems re more verstile under different lighting conditions becuse such systems emit their own rdition. An exmple of pssive method is stereoscopic photogrmmetry. Imges from two different ngles of single scene re cptured nd compred. Complex lgorithms re then used to correlte imge informtion nd determine ctul distnce. Detection is difficult under dim lighting conditions nd correltion is impossible if trget surfces hve low-detil fetures or none t ll. While such systems cn chieve good ccurcy, its complexity results in slow computtion time nd high cost. In generl, this technique is more suited for limited-rnge pplictions tht re sttionry or slow-moving. Other types of pssive tringultion methods mke use of focusing or defocusing effects. With fixed focl length, the rnge is determined either by relying on ctive focusing or mesuring the mount of blur on the focl plne. Since the focus/defocus techniques follow the lws of optics nd hve little resemblnce to the conventionl tringultion set-up, it my not seem obvious strightwy tht the two principles re in ny wy relted. However, closer inspection will show tht these methods re geometriclly similr s demonstrted by Pentlnd (1987), nd Schechner nd Kiryti (1998). The performnce of rnge-from-focus systems rests on fctors such s the opticl uniformity of the lens nd focusing cpbilities. 1

38 Also, rnge resolution cn be improved by using lrger lenses, though tht would dd unnecessry weight. While the defocus method suffers from mbiguity problems, the focus technique does not. Unlike trditionl tringultion rrngements where the emitter nd detector re on seprte xes, the lck of n emitter ensures tht focus/defocus methods do not suffer from occlusion problems cused by the shding (lso clled shdowing) effect. This phenomenon will be explined little lter. For ctive tringultion systems, the dvntges of lser mke it populr choice in terms of pplictions. In typicl set-up, the bseline is the fixed distnce between the lser emitter nd detector. A virtul tringle is formed by connecting the bseline to the pth from the trnsmitter to the trgeted re, nd from the trgeted re to the receiver. The rnge from the bseline to the trget is then esily worked out by using the lw of sines s demonstrted in Fig..8. Unlike pulse-modulted TOF systems, there is no necessity for the electronic circuitry to hve cpbilities to del with very high smpling rtes. Also, the stbility of the emitted lser frequency is not n issue t ll, in contrst to continuous wve techniques. Fig..8 Distnce mesurement by tringultion (Siegwrt nd Nourbkhsh 004) A common problem with the tringultion method is the shding or shdowing effect. The non-coxil nture of the trnsmitter nd receiver mens tht there will be occsions where fetures on trget surfce cn prevent the lser from illuminting certin res. Similrly, fetures on trget surfce cn block the return pth of lser. These opticl obstructions result in shded res tht cnnot be detected, such s shown in Fig..9. The probbility of shding occurrence cn be reduced by nrrowing the distnce between the emitter nd detector. However, this will come t the expense of mesuring sensitivity nd rnge. To llevite this problem, Yoshid nd Hirose (1988) hve suggested using two detectors t the bseline with lser trnsducer situted in between. Fig..10 shows such set-up. This provides some redundncy in cse either of the detectors experiences occlusion. Other forms of multiple-detector systems hve lso been proposed (Pieper et l. 1995).

39 Fig..9 Opticl obstruction Fig..10 Dul-sensor tringultion (Blis 004) The geometric dependency of the tringultion method mens tht the ccurte detection of the loction of trget spot is criticl. When lser bem lnds on highly uneven or extremely ngled surfce, the resulting spot imge could be severely distorted. An exmple of such sitution is shown in Fig..11. The difficulty in determining the centre of the bem would led to mesurement uncertinties not dissimilr to the mixed pixel problem. Distortion in the spot imge cn lso rise from rndom intensity fluctutions. This phenomenon is clled speckle, nd is cused by interference within highly coherent signl such s lser. Fig..1 shows n exmple of speckle. Numerous filtering lgorithms hve been proposed to mitigte speckle noise. Fig..11 Uneven bem focus spot (Center for Advnced Sptil Technologies.) Fig..1 Speckle (Bilenc) 3

40 The limited rnge of tringultion systems is not solely due to the fct tht there is loss in mesurement sensitivity with incresing distnce. The non-coxil rrngement of the trnsmitter nd detector mens tht lser spot beyond certin rnge my not be visible within the detector s field of view. For most current sensors, the field of view is sufficiently wide for medium-rnge spot detection. However, it is restricting fctor in lser scnning opertions tht utilises D or 3D illumintion. For short rnges, rnge finders operting on the tringulr technique generlly tend to provide higher mesuring ccurcy thn TOF systems Rnge Determintion by Interferometry Method Conventionl lser interferometry shres the sme principle with modulted continuous wve techniques. The key difference is the lck of crrier signl in conventionl interferometry. In this technique, single lser bem is split into reference bem nd mesuring bem. The reference bem is sent immeditely to fringe detector while the mesuring bem is projected onto trget nd is then reflected bck to the detector. The two bems re then opticlly recombined. The resulting is series of interference fringes. A chnge in object distnce would led to new interference pttern. The reltive distnce between the current nd previous positions of n object cn be determined by counting the interference fringes (Ytsenko et l. 004). The extremely short wvelength of light provides high mesuring resolution. However, this method suffers from mbiguity problems similr to the AMCW method. Due to the exceedingly minute wvelength of light, the bsolute mesuring rnge is imprcticlly smll. Nevertheless, this technique could be used to determine reltive distnce with tremendous ccurcy, provided tht the device components possess high stbility..3.3 Signl Becon or Trnsponder Systems Other thn the sensor systems mentioned bove, there re numerous other kinds in use. One reltively common type is the signl becon system. Trnsmitted signls re often rdio wves, infrred or lser. For ground-bsed rdio-frequency (RF) systems, there re two min types: ctive nd pssive. Active becon systems work very differently from their pssive counterprts. For the initil clibrtion, the subject nd vrious trnsponders re situted t known loctions. The vehicle then sends out interrogting signls to three or more reference trnsponders which in turn reply respectively. This is to determine the time needed for ech trnsponder to reply fter receiving signl from the vehicle. This response lg is lso clled the turn-round dely 4

41 (TAD). After the clibrtion process is complete, the clcultion of the bsolute distnce between the vehicle nd prticulr trnsponder is simple mtter. The totl elpsed time between n initil signl brodcst nd the reception of response from trnsponder cn be redily documented. Subtrct the TAD from the totl elpsed time, nd the result is the time required for signl to trvel from the vehicle to trnsponder nd bck. Along with the knowledge tht the propgtion speed of rdio wves is no different from tht of light, the distnce between the vehicle nd trnsponder cn esily be clculted. This will give circulr locus of possible positions of the subject. With three or more trnsponders, the intersection of the corresponding loci will provide the loction of the vehicle. This method is clled triltertion, nd it mesures the bsolute time of rrivl (TOA) of vrious signls in order to determine subject s loction. However, it will be shown lter tht triltertion cn pply to pssive systems s well. Pssive becon systems wit signls brodcst simultneously from known trnsmitter sites with ccurte loctions. The vrious distnces between the subject nd the trnsmitters will result in the signls rriving t different times. Since the time tken from brodcst to reception is not known, the bsolute distnces between the vehicle nd the trnsmitter sites cnnot be directly estblished. Only the time difference of rrivl (TDOA) cn be determined. This is done by noting the dely in the reception of the vrious signls reltive to one nother or nlysing the phse differences mong them. The result is hyperboloid locus of possible positions. Using four trnsmitter sttions, three sets of TDOA dt will be vilble, nd the resulting intersection of the loci will provide the position of the vehicle. This method is lso known s multiltertion or hyperbolic positioning. A lrge-scle exmple of triltertion positioning system is the Globl Positioning System (GPS) developed by the US Deprtment of Defence. This system hs 31 (formerly 4) stellites tht orbit the erth roughly every 1 hours t n elevtion of bout 10,900 nuticl miles. The positions of the stellites re trcked by ground-bsed sttions t ll times. This system differs from typicl ground-bsed counterprts in tht the stellites nd receivers ll hve synchronised internl clocks. Signls sent by ech stellite contin the time of trnsmission. When signl is received, the receiver notes the time of reception nd clcultes the totl elpsed time for the signl to trvel between the stellite nd itself. The distnce cn then be determined by using the triltertion technique. Since the receivers only receive signls nd do not trnsmit, the system is pssive. Mny fctors cn ffect the ccurcy of GPS. Atmospheric effects cn led to refrction or dispersion of signls. As result, signl speeds my be retrded nd led to erroneous clcultion of distnces. Multi-pth effects re nother potentil source of error. A receiver 5

42 cn be confused by direct nd reflected signls. Synchronistion errors mong the stellites nd receivers re lso possible cuse of inccurte redings. For civilin purposes, the nominl precision of GPS is limited to bout 15 m. According to the most recent report by the US Deprtment of Defense regrding GPS, its globl verge ccurcy rnges from 6.0 m to 1.8 m bsed on 95% of reported dt (USAF 008). This ccurcy cn be enhnced to rnge of less thn 1 m by relying on dditionl ground-bsed reference sttions or supplementry GPS receiver units positioned t precisely-known loctions. However, in order to tke dvntge of the ugmented GPS precision, expensive equipment is required. Moreover, the ccurcy of systems tht use dditionl ground-bsed becon sttions deteriorte s distnce increses between the bse sttions nd receiver unit (Monteiro et l. 005). For lrge opertionl re, this level of precision my be sufficient. However, for most smller-scle pplictions, the vilble degree of ccurcy is often indequte. Furthermore, unless there re signl repeters instlled within building, GPS cnnot function indoors or in other sheltered or enclosed plces such s tunnels. Another well-known positioning method is tringultion. Unlike triltertion, where the loction of position is derived from direct mesurement of distnces, tringultion depends on the determintion of ngles mde between points to known bselines. Exmples include becons situted t known loctions which emit signls such s infrred light. When within rnge, the scnning sensors on robot will ech detect different ngle of incidence reltive to predetermined bseline. Using simple geometry, the distnce of the vehicle with respect to ech becon cn be determined. The biggest drwbck of signl becon systems is the need for setting up becons s well s mesuring their loctions precisely before ny trcking ctivity cn commence. Also, the effective rnge of the becons nd the corresponding plcement issues often limit the flexibility such systems..4 Control Systems The proportionl-integrl-derivtive (PID) controller is the oldest nd most common type of control-loop feedbck system used for industril control (Bennett 001). For decdes, PID controllers, long with PI nd PD vrints, re the de fcto mechnisms used in process control. Generlly esy to tune, especilly with the id of estblished methods, it provides reltively quick nd convenient wy to utomte control process. Hence, the PID controller remins populr choice to this dy. For the sme reson, PID controllers re lso regulrly found on robotic systems. 6

43 Numerous tuning techniques for PID controllers hve been introduced over the yers, such s those bsed on heuristics, root-locus, pole-plcement, frequency response, step response, integrl error optimistion, etc. (Åström nd Hägglund 1995; Tn et l. 1999; Tn et l. 006). Out of ll the methods, the heuristic pproch of Ziegler-Nichols is one of the most populr. Another well-known empiricl pproch is the Cohen-Coon method, lthough it is not s widely-used s Ziegler-Nichols. For cscded PID systems, there re no stndrd or widelyrecognised tuning techniques (Lee et l. 1998; Sdsivro nd Chidmbrm 006; Lev nd Mrinelli 009). So, the tuning procedure is generlly quite bit more complicted thn non-cscded systems. Nturlly, the complexity is incresed when multiple vribles re involved. The min purposes of the proportionl ction re to reduce rise time nd stedy-stte error. If the vlue of this prmeter is set too high, overshoot would result nd the system response my oscillte. On the other hnd, if this prmeter is set too low, the controller my not be sufficiently responsive to lrge errors. The integrl ction reflects the ccumulted error over time nd is mostly used for eliminting stedy-stte error. Thus, it hs correltion with pst error vlues. It lso helps in reducing rise time without the need for lrger proportionl gin. If the gin setting for this prmeter is excessive, it would led to overshoot nd incresed oscilltion. The derivtive ction exhibits dmping effect nd is used for slowing the rte of chnge of the response. This helps reduce overshoot nd settling time. By vrying the rte of chnge of the signl, this prmeter hs predictive effect on the resulting output. This is in contrst to the rective nture of the integrl ction. This prmeter is highly sensitive to noise. If set too ggressively, the dmping effect would be lost nd instbility would ensue. The effectiveness of PID controllers cn be extended by hving set of tuned settings for ech different operting rnge - s determined by the observble vribles. This pproch is clled gin scheduling, nd ensures tht control is lmost lwys optiml. In generl, conventionl PID controllers re unsuitble for systems tht re highly non-liner nd unstble. In ddition to PID controllers, there re mny other types of mechnisms tht provide some form of utomted control, such s self-tuning regultors (Åström nd Wittenmrk 1973; Åström et l. 1977), robust (Zhou nd Doyle 1998) nd dptive (Sstry nd Bodson 1994; Dumont nd Huzmezn 00; Pourboghrt nd Krlsson 00) controllers. While the generl ides of robust nd dptive control re similr, there is key difference between the two. In robust control, the limits of uncertinties in the prmeter or frequency domins must be 7

44 specified priori. In contrst, there re no such restriction in dptive control (Lndu et l. 011). All the generl pproches in control theory, such s ones mentioned bove, utilise vrious methods to chieve their gols. These include sliding mode (vrible structure) control, liner qudrtic regultor, Lypunov's method, etc. The ltter theory (sometimes spelt Lipunov) ws proposed ner the end of the 19th century, nd ws lrgely overlooked for the next 50 yers (Prks 199). The stbility principle tht rose from the theory hs now become n indispensible prt of the knowledge bse in field of control systems. There re two methods proposed by Lypunov. The first method exmines the locl stbility of system tht hs been linerised nd requires the solution of differentil equtions of the system (Murry et l. 1994). It is lso clled the indirect method. The second method checks the symptotic or globl stbility of n equilibrium point without the need for solving differentil equtions of the system (Slotine nd Li 1991; Khlil 00). An dditionl dvntge over the first method is tht it cn be used on systems tht cnnot be linerised. This pproch is lso clled the direct method nd is used much more widely thn the first method. This thesis shll dopt the technique of model reference dptive control (MRAC) bsed on the direct Lypunov stbility method (Gholipour nd Yzdnpnh 003). MRAC is lso known s MRAS, which stnds for model reference dptive system. This is concept where the output of system is constntly compred to the expected response of reference model. The updtble prmeters of its closed-loop controller re then djusted ccordingly to mtch the reference model's response (Slotine nd Li 1991)..5 Summry After considering the vrious types of sensor technologies nd methodologies, it ws decided tht trcking system bsed on the opticl mouse sensor ws the best option for the purpose of this reserch. This type of sensor hs extremely high precision nd requires very little processing power nd virtully no mintennce. Furthermore, it is chep nd widely vilble. A WMR tht uses the opticl mouse sensor s trcking device would not require the use of ny externl equipment (e.g. trnsponders), so deployment would be reltively simple. It will lso be ble to function indoors. As for the choice of the control system, both PID nd dptive systems would be explored. 8

45 Chpter 3: Generl System Anlysis nd Design The overrching ide behind most utonomous mobile robots is simple. First nd foremost, the robot must be ble to locte its own position within pre-defined frme of reference. This positionl determintion process is clled loclistion. Next, desired pth or destintion is defined within the sme reference frme. Once the robot is ble to scertin its own current loction, ttempts will be mde to propel it towrds its trget course or position. As the robot moves towrds its gol, its loction will be constntly compred with tht of its objective, nd djustments in speed nd heding will be mde s nd when necessry. This process will be repeted until the finl objective is chieved. In this chpter, the generl mthemticl model of the differentilly-driven wheeled robot will be developed nd nlysed. This will include the explortion of both kinemtic nd dynmic systems long with motor nd friction modelling. 3.1 Mthemticl Modelling For this study, frme of reference must first be defined before the WMR cn crry out its tsk. This reference frme is bsed on the Crtesin coordinte system nd should tke into ccount the entire re of opertion. Once this is complete, the initil positionl dt from the trcking sensors cn be initilised with respect to the reference frme. One of the min objectives of this study is the design of WMR tht is reltively uncomplicted. This effectively rules out the use of point-to-point stbilistion s control strtegy. The trjectory-trcking pproch ws selected in fvour of the pth-following scheme becuse it llowed for more control over the speed of the WMR. The bility of the WMR to determine its own loction within reference frme lrgely depends on pir of opticl sensors. These opticl sensors re similr to the ones typiclly found in computer mice. Here, the sensors essentilly ct s the eyes of the WMR by looking t the surfce of trvel nd recording the distnce moved from one time smple to the next. Both sensors pick up positionl dt in the sme mnner. However, one is used for loclistion while the other serves to estblish the WMR s heding - the direction in which it is pointed. This informtion is determined by nlysing the reltionship between the positionl dt provided by the two sensors. 9

46 In this thesis, the mthemticl modelling of the WMR will be ddressed in two brod sections: kinemtics nd dynmics. The kinemtic equtions define the motion of the WMR while the dynmic equtions outline its opertionl limits. The system is fundmentlly non-liner. The pproximte mthemticl model of the differentilly-steered WMR used here is rectngulr box with two independently-driven rer wheels nd single front cstor wheel s shown in Fig For simultion purposes, the nominl overll dimensions of the WMR model sns wheels re 0. m, 0.15 m, 0.1 m for length, width nd height respectively. The rdius of the wheels is 0.03 m. The totl weight of the WMR is 1.5 kg. For positioning nd nvigtion, the x-y globl coordinte system hs its origin lbelled O, while the x -y locl coordinte system hs its origin locted t the centre of grvity, G, of the WMR. Fig. 3.1 Differentilly-steered WMR The key points of the model re the centre of the wheel bseline, centre of grvity, loctions of dul sensors, nd points of contct of the left, right nd front wheels with the ground. These re represented by B, G, A, K, L, R nd C respectively Generl Assumptions nd Limittions Wheel (or tyre) slippge often contributes significntly to the discrepncy between the expected nd ctul positions of moving vehicle. This form of trcking error is considered to be non-systemtic nd difficult to model. As mentioned in erlier chpters, the opticl trcking sensors used here hve the bility to trck the loction of the WMR ccurtely - regrdless of slippge. The control lgorithm simply fctors ny positionl error into the clcultions, nd 30

47 necessry djustments re then mde ccordingly. In other words, no-slip conditions cn be ssumed in the clcultions while ctul slippge is treted s prt of the feedbck error. Hence, while frictionl limits of the wheels re n importnt considertion for keeping potentil slippge to minimum, the modelling of wheel slippge is not crucil for this study. The trcking sensors on the WMR re ccurte nd efficient if used within opertionl limits. In regrds to the imges cptured by the sensors, it is importnt tht the rottion from one frme to the next be kept below the mximum llowble rte. Otherwise, it could led to erroneous pixel-mtching of sensor imges between frmes (Singh nd Wldron 004). There re other fctors tht cn lso prevent the proper opertion of the trcking sensors. Undulting terrins ffect the ngles of incidence nd reflection of the lser bem, nd cn result in inccurte loction redings. Hence, these sensors work best on flt trvelling surfces. Additionlly, completely fetureless or mirror-like surfces cn prevent these sensors from trcking properly. Also, puddles of liquid on the ground cn form ripples tht distort the trcking lser. A dusty or smoky tmosphere my diffuse or even obstruct the trcking lser s well. 3. Kinemtic Modelling The behviour of WMR is fundmentlly governed by kinemtics. The distnce between checkpoints nd the time llocted for trvel determine the velocity of the WMR nd, by extension, its wheel speed. In contrst, dynmics ply less direct lbeit still importnt role in this study. Its function is to ensure tht the WMR does not exceed opertionl limits Assumptions nd Limittions Dynmic fctors re not tken into ccount in this section. Thus, inertil effects such s weight trnsfer, s well s friction do not ply prt in the clcultions. 3.. Kinemtic Equtions Referring bck to Fig. 3.1, the wheel rdius, trck width, nd distnce between the wheel bseline nd centre of mss re respectively identified s r, d nd b. The left nd right wheel ngulr velocities re ω l nd ω r, respectively. The longitudinl, lterl nd yw (turning) velocities re respectively lbelled u, v nd ω, nd the yw ngle is ψ. 31

48 Tking the centre of mss s the representtive point of the WMR, the equtions of motion in terms of the ngulr velocity of the driving wheels cn be described s follows: l r u x ' r (3.1) br v y ' r l d (3.) r v r l d b (3.3) Eqs (3.1) nd (3.) cn be rerrnged to obtin the following: And u l r r r l dv br 1 dv l u (3.4) r b 1 dv r u (3.5) r b The reference point of the WMR s position is t sensor A, nd not t mss centre G. In the globl coordinte system, the equtions of motion with respect to the trcking sensor re: x ucos v bsin v bcos (3.6) (3.7) y usin (3.8) But v b, (3.9) x ucos sin (3.10) y usin cos By resolving Eqs (3.6) nd (3.7), the locl velocities of the WMR in the globl coordinte system cn be written s follows: u x cos y sin (3.11) 3

49 v xsin y cos (3.1) b x sin y cos (3.13) b v xsin y cos (3.14) The positionl dt of the second sensor K is processed in similr wy to sensor A by swpping the vrible for k. For WMR tht uses one opticl mouse sensor, the clcultions of positionl nd velocity informtion re bsed on kinemtic equtions tht hve non-holonomic constrints. While the sensor my be ble to pick up lterl movement cused by slippge, the dt could be incorrectly processed if the kinemtic equtions re used. This is becuse the equtions re bsed on the possible rnge of motion of non-holonomic vehicle without ny slippge. The ddition of second sensor llows the clcultions to be bsed on ctul geometry nd be free of non-holonomic limits present in the kinemtic equtions. This mens tht full rnge of motion cn be considered nd ccurtely ccounted for. Since both sensors re situted t fixed distnce from ech other long the longitudinl xis of the vehicle, they will cover the sme distnce long tht xis. Hence, x ' ' (3.15) 1 x Using simple geometry s shown in Fig. 3., the yw ngle bsed on dt from two sensors cn be determined by: sin 1 y ' y L 1 ' (3.16) L k (3.17) It must be noted tht the opticl mouse sensor does not distinguish between stright line nd circulr rc of the sme length. Since chnge in vehicle heding requires ngulr (yw) movement tht follows curved pth, the stright-line distnces represented by Δy 1 nd Δy in Fig. 3. re ctully respective rcs of identicl lengths. This is explined in more detil in Chpter 6. Therefore, Eq (3.16) should be rewritten in more ccurte polr form s: y ' y1 ' (3.18) L 33

50 The turning velocity of Eq (3.13) cn now be revised s such: d y ' y1' (3.19) dt L Fig. 3. Simple geometric representtion of sensor positions It is obvious tht the simplified model shown in Fig. 3. is n pproximtion nd not totlly ccurte depiction of the ctul geometric motion of the WMR. Due to the fct tht the sensors' positions cn only be mesured t prescribed intervls, discretistion errors re thus unvoidble. However, these errors re usully mngeble nd often insignificnt when the time period between smples tken is very smll, i.e. smpling frequency is high. Figure 3.3 illustrtes pir of sensors in two poses beyond the initil position s well s their respective pproximted positions. These poses represent the sensors' positions fter one smple period with two different smpling rtes used. The first shows the sensors being smpled t shorter time intervl nd is ssocited with ngle ψ 1. The second shows the sensors being smpled t lter time compred to the first due to the use of longer smpling period, nd is ssocited with ngle ψ. This is n exmple of first-order Euler pproximtion. In the digrm, the discrepncy between ctul nd pproximte positions ssocited with the shorter smpling period is not mrked out becuse the error is quite smll. In the cse of the longer smpling period, the discretistion error is quite significnt nd cn be clerly seen. It is evident from the illustrtion tht discretistion error increses s the smpling period lengthens, i.e. smple frequency decreses. Conversely, discretistion error pproches zero s the smpling period shrinks. Being tht ded-reckoning errors re cumultive, it is 34

51 impertive tht the smpling frequency used should be s high s computtionlly fesible in order to preserve n cceptble level of ccurcy. Fig. 3.3 Approximtion error due to discretistion 3.3 DC Motor Modelling The propulsion of the WMR is lrgely dependent on pir of DC motors one for ech wheel. Both the velocity nd direction of the WMR re controlled by simply vrying the speed of ech motor. To work out the necessry voltge to drive the motors t the required rottionl speeds, inverse kinemtic clcultions re used to simulte the motor controller. Referring to Kuo (1995), let T m, K t nd i represent the motor torque, torque constnt nd rmture current respectively. Since the motor torque is proportionl to the current flowing through the rmture, their reltionship cn be estblished s: t t Tm Kt i (3.0) The ngulr displcement of the rotor θ m, bck-emf E b, nd bck-emf constnt K b re relted to ech other in the following mnner: E b t t d m Kb (3.1) dt 35

52 Additionlly, the rmture resistnce, rmture inductnce, pplied voltge, rotor inerti, viscous-friction coefficient nd lod torque re denoted s R, L, E, J m, B m nd T L respectively. They cn be written in terms of Kirchhoff s nd Newton s lws s such: L J m di dt t d m dt t E t E t R i (3.) t B m dm dt t T m b t T L t (3.3) The trnsfer function between the pplied voltge nd rotor speed is cquired vi the use of Lplce Trnsforms: E m s Kt s Ls R J ms Bm Kt Kb (3.4) The bove eqution cn be represented s block digrm s shown in Fig It is importnt to note tht in SI units, the vlues of K t nd K b re exctly equl. Fig. 3.4 Block digrm of DC motor system 3.4 Dynmic Modelling Modelling WMR s behviour on kinemtics lone will likely led to inccurcies especilly t incresing mss nd velocity. To prevent slippge, dynmic forces hve to be tken into ccount. For the WMR modelling employed in this study, dynmic constrints re imposed on kinemtic solutions in order to produce relistic results. 36

53 3.4.1 Assumptions nd Limittions The point of reference for dynmic considertions is usully the centre of mss. However, uneven mss distribution cn often contribute to significnt dynmic effects for which re unccounted. Hence, neglecting substntil mss concentrtion cn severely ffect the ccurcy of mthemticl model. In this study, the WMR is not deemed to hve ny res of significnt mss concentrtion. Friction is n importnt dynmic fctor. Rolling friction without slippge is essentilly sttic friction. When slippge occurs, dynmic friction replces sttic friction. Since the coefficient of dynmic friction is usully lower thn tht of sttic friction, the former coefficient is used here s conservtive estimte for clculting ccelertion limits before the onset of slippge. All forms of friction, whether sttic, dynmic or viscous, re ssumed to be constnt in this report. Also, the frictionl chrcteristics of the wheels (tyres) re tken to be uniform in ll directions. It hs been mentioned in n erlier section tht the trvelling surfce hs to be reltively flt in order for the opticl trcking sensors to work ccurtely. Also, non-level surfce cn increse the tendency of the WMR to topple. In such sitution, no ccelertion djustments cn be mde to prevent it from tipping over s there re no sensors on the WMR to mesure its verticl posture Dynmic Equtions The motion of the WMR model is restricted to three degrees of freedom. Tking the WMR s reference, forces occur in the plne consisting of both lterl nd longitudinl xes. A turning moment cuses rottion in the perpendiculr xis. The mss nd moment of inerti re lbelled m nd I z respectively, while forces exerted by the driving wheels re denoted by F x l, F x r, F y l nd F y r. F x' l F x' r m u v 1 x' l x' r (3.5) m u F F v F y' l F y' r m v u 1 y' l y' r (3.6) m v F F u 37

54 1 d Fx ' l Fx ' r Fy ' l Fy ' r b I (3.7) z Revisiting Eqs (3.1) to (3.3), the bove ccelertion Eqs (3.5) to (3.7) cn lso be written in terms of the rottionl ccelertion of the driving wheels. The yw (turning) ccelertion of the WMR is known here s α. l r u r (3.8) br v r l d (3.9) r v r l d b (3.30) (3.31) Friction Modelling To prevent slippge, the mximum llowble force on ech wheel is its frictionl limit. This mens tht for ny given moment, the resultnt of forces on ech wheel in both lterl nd longitudinl xes cnnot exceed the mximum llowble force without slippge. F mx mx y m F x F (3.3) The bove eqution is the bsis of simple nd populr friction model. It lso hppens to describe circulr locus nd is therefore commonly referred s the friction circle. The eqution cn be rewritten in terms of ccelertion by fctoring out the mss. mx y x (3.33) Denoting N s the force tht is norml to the frictionl plne, nd µ s the friction coefficient, frictionl force is generlly defined s: Fmx N (3.34) On level surfce, the norml force is none other thn the weight of n object itself. 38

55 N mg (3.35) The mximum force tht cn be pplied without slippge would be: Fmx mg (3.36) Referring to Beckmn (1991), let p(i) be the frction of the WMR s mss supported by prticulr wheel t ny given time. The frictionl limit on the wheel would now be: p i mmx pimg (3.37) This eqution reduces to: mx x y g (3.38) The bove clerly demonstrtes tht mss is not fctor in the determintion of the mximum llowble ccelertion without slippge. This mens tht weight trnsfer hs no bering on ccelertion limits. Only the coefficient of friction mtters in this cse Dynmic Constrints The friction model explored in the previous section is crucil to the determintion of the dynmic constrints. Essentilly, the velocity nd ccelertion of the wheels of the WMR re constntly checked to ensure tht the resultnt forces on the WMR will not cuse it to exceed frictionl limits. A typicl curved pth segment of the WMR is shown in Fig Trvelling long the pth, the WMR mkes n ngulr displcement θ. At the sme time, the left nd right wheels cover distnces s l nd s r respectively. The pth rdii for the left nd right wheels re respectively represented by r pth, l nd r pth, r, nd the trck width of the WMR is denoted by d. 39

56 Fig. 3.5 Curved pth segment of WMR The reltionship between the two pth rdii is defined by: r pth, r rpth, l d (3.39) Displcements for both wheels re: s l r pth, l (3.40) s r r pth, r (3.41) The left nd right wheels, ech of rdius r, rotte t ngulr velocities ω l nd ω r. For given time t, the wheel displcements cn lso be written s: s s l r r t (3.4) l r t (3.43) r By resolving Eqs (3.36) to (3.39), the pth rdius of ech wheel cn be represented in terms of the wheel ngulr velocity: l r l r r r r pth, l pth, r r pth, l pth, l d And r r pth l pth r l d, (3.44) r r d r l, (3.45) l 40

57 For WMR turning t ngulr velocity ω, the norml ccelertion nb t the centre of the wheel bseline is equl to the centripetl ccelertion. nb r r pth, l pth, r (3.46) This norml ccelertion is lso the sme s tht t the left nd right wheels, respectively denoted s nl nd nr. nl (3.47) nr nb For WMR turning t ngulr ccelertion α, the tngentil ccelertion tl of the left wheel would be: tl r pth, l (3.48) For the left wheel with rdius r nd ngulr ccelertion α l, the bove cn lso be written s: tl r (3.49) l Similrly, for the right wheel with ngulr ccelertion α r, the tngentil ccelertion tr would be: tr r pth, r (3.50) Or tr r (3.51) r The resultnt ccelertions l nd r respectively t the left nd right wheels re: l nl tl (3.5) r nr tr (3.53) 41

58 3.5 Summry A detiled mthemticl model of the wheeled robot hs been developed nd presented in this chpter. Both kinemtic nd dynmic systems were exmined, nd the respective ssumptions nd limittions were duly noted. For the ltter system, simple friction model ws lso introduced. The Euler pproch ws the chosen method for loclistion here. Its shortcomings re well known nd this ws clerly illustrted long with n explntion on how the issue cn be mnged. This theoreticl model will serve s bsis for the computtionl model to be implemented in the following chpter. 4

59 Chpter 4: Computtionl Design of the PID-Bsed Model Before prototype is built, computer simultion is first performed to verify the vlidity of the mthemticl model s well s the control lgorithm. This step helps to reduce the mount of construction nd ltertions of prototypes needed, nd thus sves time nd resources. Bsed on the theoreticl model developed previously, computtionl representtion will be formulted in this chpter to include both kinemtic nd dynmic lgorithms s well s PID control system. The entire simultion will be conducted by using the MATLAB progrm nd its grphicl modelling tool, Simulink. 4.1 Generl Outline of the Computtionl Model The ide behind the overll design is quite bsic s cn be seen in Fig A set of checkpoints forms desired pth for the WMR. It then hs to rrive t ech checkpoint t preset time. The two sensors provide the heding nd coordintes of the current loction. With the knowledge of the current position nd next checkpoint, s well s the llotted time for the WMR to trvel between the two loctions, the direction nd velocity cn be determined. Using inverse kinemtics, the instntneous ngulr velocity of the driving wheels cn now be clculted. Fig. 4.1 PID-Bsed Trcking Control System of the WMR The rottionl speeds of the wheels re directly dependent on how fst the motors re spinning. The opertion of the motors is in turn governed by motor controller. Knowing the required ngulr velocity of the wheels, the motor controller cn compute the necessry voltge for ech motor. Before the motor controller sends voltge signl to ech motor, check hs to be mde to ensure tht the motors do not turn overly fst to the point where the wheels lose trction longitudinlly or lterlly. With the ppliction of inverse dynmics, the mximum llowble 43

60 velocity for ech wheel cn be determined from the frictionl limits of the wheels. If limits re exceeded, motor speeds on both sides will hve to be scled bck proportionlly, nd the motor controller will need to reduce voltges ccordingly. In purely kinemtic model under no-slip conditions, the procedure for the verifiction of dynmic limits is bsent. At every time step of the simultion, the trcking sensors tke reding of the coordintes of the current loction. As it is quite impossible to mimic the opticl trcking process, wheel odometry is used s substitute method for loclistion. Since non-slip conditions re ssumed in the simultion, the use of this lterntive technique is deemed justified. Using this pproch, the positionl dt is generted with the help of forwrd kinemtics. Essentilly, the current coordintes re clculted by integrting the velocities of the driving wheels within time step to determine the distnce trvelled by ech wheel. However, to simulte the dulsensor opticl trcking system s relisticlly s possible, the WMR s heding is not directly determined from wheel odometry, but from the reltionship between the positionl dt from both sensors. By clculting the distnce between the current position of the WMR nd the next checkpoint, course cn be chrted long with the pproprite velocity. This completes one simultion cycle. Additionlly, severl PID (proportionl-integrl-differentil) controllers re used throughout the system to fine-tune the overll performnce. 4. Assumptions nd Limittions of the Simultion Model The generl ssumptions nd limittions in the mthemticl model pply here s well. Also, the trcking dt generted by the use of forwrd kinemtics re ssumed to be resonbly ccurte simultion of ctul dt from the trcking sensors. In ddition, the simulted trcking dt ssume tht the WMR is operting within frictionl limits t ll times, nd hence no slippge hs occurred. 4.3 Design Detils of Simultion Model Computer modelling usully involves the feeding of dt into set of mthemticl equtions with the results collected fterwrds. This process is repeted over certin number of cycles nd the output is compred with the predicted outcome. The development of simultion model using mthemticl equtions is generlly strightforwrd process. However, dditionl lgorithms re often required to complement the 44

61 min mthemticl equtions nd lso to hndle specil conditions. These essentil lgorithms will be explined in the following sections Trjectory Genertor The objective of the study is the design of WMR with the bility of following predefined pth or trjectory. In this simultion, the required route cn be plotted vi series of points extrcted from dt file, or it cn be generted from n eqution or series of equtions. For demonstrtionl purposes, the ltter pproch ws dopted. The MATLAB code used for generting the reference pth cn be found in Appendix A. To synchronise the pth points with simultion time steps, clock ws introduced to the system s shown in Fig. 4.. The imposition of time constrints on the desired pth results in the cretion of trjectory - s defined in erlier sections. Alterntively, signl genertor cn be used to provide commonlyencountered signls such s sinusoids. For the ske of dt portbility, the generted pth points long with their ssocited time steps re sved in mtrix formt in dt file. Fig. 4. Trjectory Genertor Subsystem 4.3. Dul PID Controller A PID (proportionl-integrl-derivtive) controller is form of feedbck control device used for tuning the response of system. It works by nlysing the difference between the current nd desired sttes of prticulr process vrible, nd then producing suitble response to reduce, nd if possible, eliminte the error. As cn be seen in Fig. 4.1, there re two sets of dul PID controllers used in the simultion model. Ech set hs pir of PID controllers becuse there re two prmeters to mnge. 45

62 The first set controls the response to the discrepncy between the present ctul loction nd the desired position on the preset route. The positionl error is split into two portions to be hndled seprtely in reltion to the x nd y xes. The second set is responsible for the response to the difference between the current nd required ngulr velocities of ech wheel. The wheels re controlled independently of ech other. A schemtic of the dul PID controller is presented in Fig The proportionl prmeter ffects the response to the current error. It is ment for reducing rise time nd overll error. While stedy-stte error is lessened long with the reduction of overll error, it cnnot be fully eliminted. An indequte proportionl gin cn cuse the system to tke long time to reduce ny error. This results in n unresponsive system. On the other hnd, n overly ggressive setting cn led to overshoot nd oscilltion, nd thus cuse instbility in the system. The integrl prmeter ffects the response bsed on the sum of errors over time. It is used for eliminting stedy-stte error, nd will id in pushing overll error to zero s result. If the integrl gin is too low, stedy-stte error will be reduced but not eliminted. However, if the setting is too high, it cn result in overshoot s well s poor trnsient response, i.e. long settling time. At worst, it my even produce n oscillting nd unstble system. The derivtive prmeter ffects the response to the rte of chnge of error. It is primrily intended for reducing overshoot nd settling time. An insufficient derivtive gin cn result in the system being sluggish in curbing overshoot s well s ineffective in improving trnsient response. In contrst, n excessive setting cn led to instbility due to noise mplifiction. Fig. 4.3 Dul PID Controller Subsystem 46

63 4.3.3 Inverse Kinemtics In the simultion, the inverse kinemtics lgorithm is no more thn computtionl mnifesttion of the mthemticl equtions on which it is bsed. The process begins with the input of informtion on the discrepncy between the current loction of the WMR nd the desired position t given point in time. This positionl error is mesured with respect to both the X nd Y xes. The Inverse Kinemtics subsystem is shown in Fig In order for the WMR to reduce the positionl error nd get to the next destintion point on the preset route within specified timefrme, the velocity of ech wheel hs to be precisely controlled. This tsk is chieved vi the use of kinemtic equtions described in Chpter 3. 47

64 Fig. 4.4 Inverse Kinemtics Subsystem 48

65 About-Turn Algorithm for Countering Reverse Motion There re situtions where the forwrd motion of the WMR would result in its wheels trvelling further distnce to the next checkpoint thn it would if the WMR moved in reverse. This condition rises when the next checkpoint is locted t more thn 90 clockwise or nticlockwise from the current heding of the WMR. Without ny form of restriction, the control lgorithm would tke the shortest possible route. This could result in the WMR frequently trvelling in reverse. The simplest wy to detect ny potentil reverse motion is to check whether the control lgorithm is ssigning negtive ngulr velocity to either of the DC motors. In order to ensure tht the WMR only trvels forwrd, the first step is to restrict the DC motors from ny reverse rottion. Then, process is initited to turn the WMR round nd drive it towrds the trget checkpoint. Before proceeding on to the lgorithm, the positions of key points of the WMR hve to be first estblished. Fig. 4.5 shows the position of sensor A reltive to tht of the bseline centre B, s well s tht of the left nd right wheels L nd R. From the loctionl dt provided by sensor A, the respective positions B, L nd R cn be clculted geometriclly. The coordintes re denoted respectively such tht those for A re x nd y, those for B re x b nd y b, those for L re x l nd y l, nd those for R re x r nd y r. The distnce between A nd B is, the trck width is d nd the ngle of orienttion is ψ. x y b b x cos (4.1) y sin (4.) d 0 xl xb cos 90 (4.3) d 0 yl yb sin 90 (4.4) d 0 xr xb cos 90 (4.5) d 0 yr yb sin 90 (4.6) 49

66 Fig. 4.5 Reltive positions of WMR wheels nd primry sensor The bout-turn lgorithm ssigns one wheel s sttionry pivot while the WMR is turned s quickly s possible until its heding is directly in line with the next checkpoint. Before this turning lgorithm is pplied, the ngulr velocities of the two motors re compred with ech other, nd the wheel on the side with the lower bsolute velocity is scribed s the pivot. As cn be seen from Fig. 4.6, the turning pth of the WMR is bsiclly circulr rc followed by direct pth to the next checkpoint. The points B, A, P, B 1 nd Q respectively denote the centre of the wheel bseline, sensor, pivot wheel, position of the bseline centre fter the WMR hs turned round, nd the loction of the next checkpoint. Note tht pivot P cn be either the left or right wheel depending on the sitution. In this cse, P is the position of the left wheel. Fig. 4.6 About-turn mnoeuvre of the WMR 50

67 Two tngents cn be drwn from the next checkpoint to the circulr pth formed by the centre of the wheel bseline. One of these tngents is the pth to the next checkpoint fter the WMR hs turned round. The normls from the pivot wheel to the two tngents re P-P 1 nd P-P respectively. The point B 1 is t the intersection of the tngent pth nd its corresponding norml. Let r s be the rdius of the circulr pth formed by the centre of the wheel bseline. Pivot wheel P hs coordintes x p nd y p. As mentioned erlier, P could be either the position of the left or right wheel depending on which one is selected s the pivot. The eqution for this pth is: x x y y r (4.7) p p s The tngent to the circulr pth is simply its derivtive with respect to x: dy dx x x p (4.8) y y p The coordintes of intersection point B 1 re x 1 nd y 1. The prmetric equtions describing B 1 re thus: x t 1 (4.9) y t x 1 y p rs p (4.10) The eqution of the tngent pth fter the bout-turn is: dy y y1 x x 1 (4.11) dx Resolving Eqs (4.8) to (4.11), x x p y y p t... rs x x p x p x x p x p y y p t... 4 x x x x y y r x x x r y y r 0 p p p p s p p s p s (4.1) To solve for t, MATLAB is used to find the roots. This qudrtic eqution will produce two sets of results. If the solution is complex, only the rel prt is considered. The longer of the chord 51

68 lengths between point B nd ech set of solutions will determine the position of point B 1. The chord length is defined s: x x y y s (4.13) 1 b 1 b When point B 1 hs been determined, the norml vectors P-P 1 nd P-P cn be found. Following on, the ngle θ 1 between the two vectors cn be estblished s shown in Fig A norml vector P-P 3 is then creted to bisect the ngle formed between P-P 1 nd P-P. This bisected ngle is denoted s θ. Next, the vector P-P 4 is projected long the wheel bseline. The ngle θ 3 between P-P 3 nd P-P 4 cn then be clculted. Finlly, θ 4, the totl ngulr displcement required to turn the WMR round cn be computed by summing ngles θ nd θ 3. (4.14) 4 3 Fig. 4.7 Angulr displcement of the bout-turn mnoeuvre 5

69 The ngulr velocity of the non-pivoting wheel cn now be clculted to turn the WMR s quickly s possible in circulr rc spnning n ngle of θ 4. Once this step is complete, the WMR only needs to follow stright-line pth to the next checkpoint. The About-Turn lgorithm is progrmmed s n embedded MATLAB function within the Inverse Kinemtics subsystem in the Simulink model shown in Fig The detils of the corresponding MATLAB code for the turn function block re locted in Appendix A DC Motors The locomotion of the WMR depends on pir of DC motors independently driving wheel ech. Underpinning the simultion of the DC motors re the equtions found in Chpter 3. The lod torque is n estimtion of the turning resistnce due to fctors such s Coulomb friction. With properties of the motors lredy estblished, the ngulr velocity of ech motor cn be esily determined for ny given input voltge. The DC Motors subsystem is illustrted in Fig Fig. 4.8 DC Motors Subsystem 53

70 4.3.5 Motor Controller The speed of ech driving motor of the WMR vries ccording to situtionl requirements. For this purpose, motor controller is used to regulte the speed of ech motor independently. Hving known the motor properties, the required voltge for ech motor cn be scertined for ny desired speed. In the simultion, the underlying equtions for the lgorithm of the motor controller re simply the reverse rrngement of the equtions tht describe the behviour of the DC motor. Fig. 4.9 shows the Motor Controller subsystem. For further elbortion, the DC Motor Modelling section of Chpter 3 is revisited. Eq (3.18) cn be rerrnged s: J i m d t dt m 1 K t t J B m m d d dt m m dt t t K B m t i d t m dt t T L T t L t (4.15) In the sme mnner, Eq (3.17) cn be rewritten s follows: L E di t dt t R i t t E t K b d m dt t t di d m L R i t Kb (4.16) dt dt Remembering tht the motor velocity is: t d m i where i l or r (4.17) dt Thus, the necessry voltge for ttining ny desired motor speed is simply obtined vi the use of Eqs (4.15) to (4.17). 54

71 Fig. 4.9 Motor Controller Subsystem Inverse Dynmics The control system of WMR is bsed fundmentlly on kinemtic model. However, s explined in erlier chpters, purely kinemtic system hs significnt indequcies. In order to obtin more relistic results, dynmic considertions re necessry in defining the physicl limits of the kinemtic model. In the simultion, the purpose of the inverse dynmics lgorithm is to constntly monitor the WMR's movement for ny potentil brech of dynmic boundries tht could result in wheel slippge. If positive detection is mde, the overll velocity of the WMR is reduced until it no longer exceeds the dynmic threshold. The lgorithm determines which of the wheels is likely to be responsible for the lrgest contribution to the overll error nd proceeds to slow it down to the llowed limit. The velocity of the other wheel is reduced concurrently by the sme proportion. The preservtion of the velocity rtio between the wheels will ensure tht the WMR mintins its heding even if its overll velocity is decresed. Ultimtely, the reduction in the speed of both wheels is brought bout by lowering the motor voltge by the sme fctor. The Inverse Dynmics subsystem in the Simulink model hs two components s shown in Fig The first module is simultion of the behviour of the pir of DC motors in response to 55

72 voltge input tht is regulted by kinemtic control system. Hence, the design of the module is identicl to tht of the ctul DC motors subsystem. The second module consists of the inverse dynmics lgorithm which clcultes the predicted motion of the WMR due to motor (or wheel) speed vlues from the previous module. It lso computes the necessry speed reduction fctor if dynmic constrints re deemed to hve been exceeded. The equtions used for the tsk re found in the Dynmic Limits section of Chpter 3. The Simulink subsystem for the inverse dynmics lgorithm is shown in Fig The inverse dynmics subsystem includes switch tht llows dynmic constrints to be bypssed, thus leving purely kinemtic system. The process for this portion of the generl model my initilly seem unnecessrily convoluted. A more intuitive nd strightforwrd design would be to plce simple subsystem consisting of just the inverse dynmics lgorithm right fter the subsystem for inverse kinemtics. After ll, the objective of the inverse dynmics subsystem is to nlyse the potentil behviour of the WMR resulting from the motor (or wheel) speed vlues put out by the inverse kinemtics subsystem. However, s the immedite response of the DC motor to ny voltge chnge is not liner, the resulting behviour of the WMR is likewise ffected. Hence, the current design ws dopted to tke into ccount the non-liner response of the DC motor. Fig Inverse Dynmics Subsystem 56

73 Fig Inverse Dynmics Algorithm Subsystem 57

74 4.3.7 Forwrd Kinemtics (Sensor) The Forwrd Kinemtics subsystem is simulted nlogue for the opticl trcking sensor. Since it is not possible to directly simulte positionl dt from n opticl sensor, n odometric method vi the use of forwrd kinemtics is dopted insted. This pproch is justified since both the opticl nd simulted trcking systems employ similr ded reckoning techniques. However, it is importnt to note tht this method is only pplicble in non-slip conditions. As the nme of the forwrd kinemtics subsystem suggests, the equtions used within re just reverse rrngement of those found in the inverse kinemtics subsystem. Likewise, the kinemtic equtions used here re lso described in the sme section of Chpter 3. In the simultion, the ngulr velocities of the wheels driven by the DC motors re used to determine the direction nd distnce trvelled by the WMR over specified time period. The content within the simultion subsystem is illustrted in Fig The positionl nd orienttion dt re fithfully recorded. At the sme time, the vehicle yw tht hs been tweked by the PID controller is sent bck to the feedbck loop. The single PID controller used here is of stndrd type. 4.4 Summry A computtionl model hs been constructed by using the mthemticl blueprint proposed in the previous chpter. It is comprised of both kinemtic nd dynmic spects nd hs incorported slient components such s PID controllers, motors nd sensors. It hs lso tken into considertion the effects of tyre friction. With this stge completed, the behviour of the wheeled robot in response to vrious ssigned trjectories cn now be simulted in softwre. The dvntge of using softwre simultion is tht trjectories nd other physicl prmeters cn be ltered with ese in order to guge the rection of the robot. The results of the simultion will be presented in the next chpter. 58

75 Fig. 4.1 Forwrd Kinemtics (Sensor) Subsystem 59

76 Chpter 5: Simultion Results of the PID-Bsed Model The purpose of the computtionl model developed in the preceding chpter is to simulte the behviour of theoreticl WMR with certin physicl chrcteristics in response to vrious trjectory stimuli. In this chpter, severl different trjectories will be used to evlute the performnce of the model. The kinemtic nd dynmic versions of the model will lso be tested nd compred. The results from the simultion tests will provide good indiction of the vlidity of the model s well s the effectiveness of the PID controller. 5.1 Results nd Anlysis The WMR model ws tested in severl simulted scenrios of which three initil bsic trjectories re shown here lbelled Figs. 5.1 to 5.3. In ech of these simultions, the WMR ws ble to quickly propel itself from its strting point towrds to the pre-defined pth locted t certin distnce wy. Once the WMR hs negotited itself onto the pth, it will follow it fithfully until the end of the simultion period. Fig. 5.1 Simultion Test No. 1: Stright trjectory with offset strting point 60

77 Fig. 5. Simultion Test No. : Stright trjectory with offset strting point Fig. 5.3 Simultion Test No. 3: Sinusoidl trjectory with offset strting point An importnt tsk in this study is to verify how well the kinemtic nd dynmic models perform ginst ech other. For this undertking, opertionl conditions must be chosen such tht cler contrst cn be seen between the routes tken by both simultion models. A winding pth ws deemed to be most suitble becuse it requires the WMR to constntly vry its speed. Ultimtely, the pth chosen ws sinusoid s shown in Fig

78 Fig. 5.4 Contrst of reference, kinemtic nd dynmic pths The bove plot shows three complete pths but without ny informtion on how they progressed through time. As the dynmic model is expected to slow down t turn more thn the kinemtic one, significnt difference in speed would be expected between the two models t such loction. For this reson, few prts of the plot were selected to be redrwn for clrity s illustrted in Fig These segments correspond to the timefrmes for the periods just before nd right fter the turns. 6

79 Fig. 5.5 Detils of turning pths t selected timefrmes It is interesting to note from Fig. 5.5 tht the pth progression of the dynmic model consistently lgs tht of the kinemtic model. It is evident tht slow-down occurs t every turn. Yet, the finl outcome shows tht the dynmic model is ble to ctch up with the kinemtic route dvncement. This must indicte tht the dynmic model speeds up during the reltively stright segments of the course. It is importnt not only for controller to be ble to follow prescribed trjectory, but it hs to be ble do it with level of ccurcy tht is within cceptble limits. In order to verify the trcking cpbilities of the PID controller, further simultion runs were conducted. The trjectories include rnge of speeds s well s turns of vrying cuteness. For these tests, the gin settings of the PID controller re tuned with much more precision thn the previous runs. This is to ensure tht mximum ttinble performnce is mesured nd tht errors re unlikely to be due to sloppy tuning. An error nlysis is conducted fter ech run to quntify the results. The performnce of the PID controller over the sme course shll be compred to tht of n dptive controller in lter chpter. There re number of methods for judging trcking errors. Among them, the most commonly used ones re men bsolute error (MAE), men bsolute percentge error (MAPE), men squred error (MSE) nd root men squred error (RMSE). The MSE nd RMSE re considered unbised estimtors of vrince (σ ) nd stndrd devition (σ) respectively. Tht 63

80 mens the MSE nd RMSE re equivlent respectively to the vrince nd stndrd devition of smple popultion if it exhibits no bis. This thesis shll dopt RMS error s the min nlyticl metric for trcking error. For trcking vrible x in smple size of N, RMS error is defined s: RMSE 1 N N x i x i1 (5.1) where x is the desired vlue nd trcking error x x It should be noted tht the desired position of the WMR is lwys one time step hed of the current position. Therefore, the trcking error is defined s the discrepncy between the current position of the WMR nd the position of prescribed trjectory in the previous time step. Thus, the trcking errors in the X- nd Y-xes re respectively: t xt x t where x is the desired vlue (5.) x d 1 t yt y t where y is the desired vlue (5.3) y d 1 d d The resulting RMS error equtions re: RMSE x 1 N N i1 x, i (5.4) RMSE y 1 N N i1 y, i (5.5) In ddition to clculting the RMS error, it is lso informtive to determine the error distribution nd probbility density function in order to be id the visulistion of the error spred. The error PDF is plotted with the use of kernel density estimtion. The first result of the second series of simultion runs is shown Fig The course included three 90-degree turns, so the WMR hs to slow considerbly t these points nd then ccelerte fterwrds in order to ctch up to the prescribed trjectory. This pth is sme s the one recommended in the UMBmrk clibrtion test (Borenstein nd Feng 1995). However, it ws not used here for clibrtion purposes becuse the WMR used in this reserch is clibrted very differently. Nevertheless, its trcking performnce in this test could still be 64

81 used to compre with the results from other reserch. Judging from the outcome, the controller hs successfully mnged to follow the prescribed pth. Fig. 5.6 PID Simultion Test No. 4: Squre trjectory. The distribution nd probbility density function (PDF) of the trcking error re shown in Fig The results confirm tht the WMR lrgely styed on course nd did not devite much throughout the test. This fct ws lredy visully evident in Fig The resulting RMS errors re mm in the X-xis nd 4.69 mm in the Y-xis. For course tht consisted of only stright lines, the performnce ws not spectculr but certinly quite cceptble. Since the controller ws forbidden to engge in reverse motion for ny of the wheels (for resons described in previous section), the turning rdius of the WMR ws incresed. This ws especilly noticeble when negotiting shrp turns. As result, the WMR veered quite bit from the expected pth while turning 90-degree corners. This behviour my hve hd significnt impct on the clcultion the RMS errors. 65

82 Fig. 5.7 PID Simultion Test No. 4: Squre trjectory error nlysis The simultion shown in Fig. 5.8 ws run with the sme course s the previous test. The only difference ws tht the WMR nd trjectory hd different strting positions. This ment tht right t the strt, the WMR ws lredy lgging the trjectory nd hd to speed up in order to ctch up. With this chnge of circumstnce, the gin settings hd to be re-tuned. Bsed on the results, it cn be concluded tht the controller mnged to follow the prescribed pth successfully. The lg offset strting point tested the bility of the controller to ctch up to the desired trjectory in the lest mount of time, but doing so without being overly ggressive to the point where the WMR would struggle to settle on the ctul pth when it finlly got there. As the course included three 90-degree turns, the WMR hd to slow considerbly t these points. Once gin, this presented chllenge to n ggressively-tuned controller. On the other hnd, controller tht is too conservtively tuned my never be ble to ctch up to the trjectory given the discrepncy between the strting positions of the trjectory nd WMR. A blnced tuning is key to the success of controller's performnce. However, it is possible tht the opertionl prmeters my not llow n optiml result no mtter how the controller is tuned. An error nlysis of the trcking ccurcy ws not crried out for this test s the dt from the initil offset would skew the results. The preceding test with the sme strting point 66

83 for both the trjectory nd WMR would be more ccurte reflection of the controllers trcking bility. The outcome of this test ws chieved fter some pinstking tuning of the PID gin settings. It took only slight tweks to the settings to hve noticebly negtive effects on the results. Vrying the WMR strting point by more thn 10 cm in ny direction would lso require significnt re-djustment of the gin settings. While the PID controller hs demonstrted its bility to hndle the tsk, it is evident tht it hs significnt limittions. It tkes fir mount of effort to tune the controller nd its locus of pplicbility is rther smll. Thus, its verstility is questionble. Fig. 5.8 PID Simultion Test No. 5: Squre trjectory with offset strting point At first glnce, bsed on the trjectory plot of Fig. 5.9, the trcking performnce for the sinusoidl test seemed to be superior to tht of the squre test. While the squre trjectory hd stright lines tht were esier to trck thn the curves of sinusoid, the ltter lcked the shrp turns seen in the former. However, the error nlysis s depicted in Fig revels little difference between the two scenrios. The RMS errors in the X- nd Y-xes re.76 mm nd mm respectively. So, the mgnitude of the devitions in the X- nd Y-xes re virtully reversed between the two tests. The error spreds of both tests re lso similr. This mens tht the controller ws ble to trck the sinusoidl pth s well s the squre one, but not distinctly better. It ppers tht the dvntge of not hving to negotite shrp corners ws negted by the incresed difficulty in trcking curved trjectory. 67

84 Fig. 5.9 PID Simultion Test No. 6: Sinusoidl trjectory Fig PID Simultion Test No. 6: Sinusoidl trjectory error nlysis 68

85 The sinusoidl trjectory ws run gin with slightly different prmeters s shown in Fig The turns involved were more cute nd the pth ws little more thn twice the length of previous one, nd the time llotted for completion of the course ws doubled. This ment tht lthough the WMR hd to slow down more drsticlly thn previously to negotite tighter turns, it ws not given ny more time to regin speed fter every turn. Once gin, the gin settings hd to be ltered substntilly. Once gin, the plot shows tht the controller ws ble to ccomplish its tsk. A closer look t the error distribution nd density shown in Fig. 5.1 indictes tht lthough the WMR hd mostly followed the expected trjectory, it veered off the trck more often nd by greter mrgin thn the previous sinusoidl test. Given tht the course ws more demnding for resons mentioned bove, the outcome ws not unexpected. The RMS errors in the X- nd Y-xes re 4.6 mm nd 6.94 mm respectively. The discrepncy in the X-xis is mrginlly greter thn in the preceding test, but for Y-xis it is lmost doubled. If the WMR is required to negotite very nrrow course (tht my be wlled like mze), this controller my struggle to perform with such error mrgins. Fig PID Simultion Test No. 7: Sinusoidl trjectory 69

86 Fig. 5.1 PID Simultion Test No. 7: Sinusoidl trjectory error nlysis For the lst simultion run, circulr trjectory ws chosen. In ddition to testing trcking performnce of the controller, it checked the bility of the WMR to return to its strting point - just like in the squre trck scenrio. There were no shrp turns or sudden chnges in speed or heding in this course. So, the results were expected to be quite good. As illustrted in Fig 5.13, the controller ws quite up to the tsk. During the tuning of the PID controller's gin settings, it ws noticed tht the WMR would often follow the trck quite fithfully until just moment before it rrived bck t the strting point. At tht instnce, some slight oscilltions becme noticeble. Extending the course time reveled tht further oscilltions occurred spordiclly lthough they were quite minor. The gin settings were then djusted such tht the controller could perform relibly over two revolutions of the course. When tht ws chieved, the test ws performed for even long distnces, i.e. up to 10 revolutions. It ws confirmed for this test tht s long s the controller could function predictbly over two revolutions, the sme gin settings would pply for much longer runs. So, the test ws re-run to cover two revolutions of the sme trck just to see how well the WMR could hold its course over greter distnce. Since n overlpping plot hd tendency to obscure undultions nd minor detils of the WMR's pth, the grph for single revolution of the trck is included here (Fig. 5.13) for visul clrity. 70

87 Fig PID Simultion Test No. 8: Circulr trjectory (1 rev) It is cler from Fig tht the controller ws ble to keep the WMR on trck for t lest two revolutions of the circulr trjectory. While the error spred shown in Fig suggests tht the WMR closely followed the pth most of the time, there re times when it did not trck its trjectory well t ll. This could men tht it either stryed from the course by quite significnt mrgin or it mintined its course but lgged behind the desired position t certin points. Judging from Fig. 5.14, the ltter is the more likely explntion. The resulting error nlysis does not point to good controller performnce. While the RMS error in the X-xis is respectble 1.85 mm, the discrepncy in the Y-xis is mm. This lst result is rther poor considering tht the prescribed trjectory ws not very chllenging. Mny further ttempts were mde to improve the gin settings, but better outcome proved elusive. 71

88 Fig PID Simultion Test No. 8: Circulr trjectory ( revs) Fig PID Simultion Test No. 8: Circulr trjectory ( revs) error nlysis 7

89 5. Discussion nd Conclusion The results prove tht the dynmic lgorithm will slow down the simulted WMR when situtions rise tht could cuse it to exceed friction limits. Nevertheless, it is responsive enough to be ble to speed up when required to mtch the reference trjectory. In generl, the PID model performs resonbly well in simulted environment nd demonstrtes the fesibility of the ide. However, it is not without shortcomings. The PID control lgorithm used in this study is reltively bsic. Bsed on the error nlyses, its trcking performnce ws not uniform cross vrious situtions. A summry of results is displyed in Tble 5.1. In some scenrios, the controller trcks with very good ccurcy while in others it struggles to sty on course. It is certinly possible tht the chosen gin settings were not optiml. However, s stted before, there is no commonly-dopted method for tuning cscded PID systems, nd the pplicbility of PID control to highly non-liner system is questionble. So, mnul tuning hd to be relied upon to obtin the most suitble system response for ny given condition. Trjectory Type RMS Error in X-Axis (mm) RMS Error in Y-Axis (mm) Squre: 4m x 4m Sinusoidl: y = sin(x) Sinusoidl: y = sin(x) Circulr: rdius = m Tble 5.1 Summry of simultion test results of PID model It ws discovered during the simultion tht the gin settings pplied to only smll locus of opertionl prmeters. This mens tht every time there is minor chnge in the scenrio, such s the desired trjectory, required verge speed or different strting offset, re-tune of the gin settings would be needed. Furthermore, minor tweks to the settings often seemed to hve noticeble effects on the finl outcome. So, the djustments hd to be done in very fine increments or decrements. This mens tht the tuning procedure requires lot of time nd effort. Thus, not only do the gin settings of the controller require frequent djustments, the djustment process itself is lso pinstking. These chrcteristics suggest tht the controller in its current form hs limited prcticl use. It would be necessry for n improved version or n entirely new controller to be designed before robotic vehicle with dul opticl sensors could be built nd competently tested. 73

90 For now, it must be remembered tht without n ctul working prototype, how well these results mtch up to relity remins to be seen. 5.3 Further Work After review of current literture nd creful considertion, it is decided tht new controller would be the best wy forwrd. This new controller hs to be ble to cope with greter rnge of opertionl situtions without the need for constnt djustments. Indeed, it would be preferble if it could even tune itself while on the move. Bsed on these preferences, n dptive lgorithm is the nturl choice for the tsk t hnd. If nd when the new controller hs proved its superior cpbilities in simultion, working prototype would be constructed in order to vlidte the theoreticl results. 74

91 Chpter 6: Anlysis nd Design of the Adptive System In the erlier chpters of this thesis, mthemticl model of the wheeled robot ws nlysed nd developed. It provided the bsis for the computtionl representtion tht ws designed subsequently. While the PID-bsed model ws ble to perform dequtely in simultion, it lso reveled number of inherent deficiencies tht could not be overcome by introducing minor ltertions. This outcome hs led to the need for mjor overhul or even new model. In this chpter, n extensively-modified model with completely different control system will be developed to ddress the problems fced by the previous model. This will be fullydynmic system with n dptive controller. In ddition, the unique chrcteristics of the opticl mouse sensor will be explined, nd two methods for clculting odometry will be nlysed nd compred. A technique for chieving prtil redundncy will lso be introduced. 6.1 DC Motors, Ger Assembly nd Vehiculr Dynmics The dynmics of the DC motor nd vehicle hve lredy been discussed in Chpter 3. However, they re revisited here in more detil. The equtions of the DC motor nd ger ssembly re now revised to include ger rtio, n g. These equtions re expressed in terms of Kirchhoff s nd Newton s lws s shown in the following. Note tht subscripts m nd L refer to vribles ssocited with the motor nd xle lods respectively. The ger rtio is defined s: n g m L where n g 1 (6.1) Rewriting Eq (3.), J J m L L L L TL Tm t B t J t B t 1 m m m n m m g TL t T t B t J t B t Or, n J t B t g m m n 1 g L m 1 TL t J t B t T m L m L n g L L L m L L m m n g n g (6.) (6.3) 75

92 76 Substituting Eq (3.19) into Eqs (6.3) nd (6.4), g L t m L m L g m m m m n t T t K i t B t J n t B t J 1 (6.4) g L t L L L L g L m L m g n t T t K i t B t J n t B t J n 1 (6.5) From Eqs (3.0) nd (3.1), dt t d K t E t i R dt t di L m b (6.6) Using Lplce Trnsforms on Eq (6.6), m b R s L s s K s E s I (6.7) Considering inertil lod only (i.e. T L = 0), nd pplying Eq (6.7) to Eq (6.4), m b t m L L g m m R s L s s K s E K s B s J n B s J s 1 s E s s K s B s J n B s J K R s L s m b m L L g m m t 1 b t L L n m m t m K K B s J B s J R s L s K s E s g 1 (6.8) Or, b t L L n m m g t L K K B s J B s J R s L s n K s E s g 1 (6.9) Evluting Eq (6.9) in the time domin, E K n t K K n B R B R n t J R J R n B L B L n t J L J L n t g L b t g L m g L L m g L m g L L m g (6.10)

93 77 Tking Eq (6.7) this time with E = 0, nd pplying to Eq (6.4), m b R s L s s K s I (6.11) g L m b t m L L g m m n s T R s L s s K K s B s J n B s J s 1 g L m b t L L g m m n s T s R s L K K B s J n B s J s 1 b t L L n m m L m k k B s J B s J R s L s R s L s T s g 1 (6.1) Or, b t L L n m m g L L k k B s J B s J R s L s n R s L s T s g 1 (6.13) Evluting Eq (6.13) in the time domin, t T R t T L t K K n B R B R n t J R J R n B L B L n t J L J L n L L L b t g L m g L L m g L m g L L m g (6.14) Equtions (6.10) nd (6.14) clerly describe third-order system. The third-order term is the derivtive of the ngulr ccelertion of the lod ttched to the motor. Although the term exists in the idel mthemticl model, it hs little physicl mening nd dds mbiguity nd complexity to the development of control system. Hence, it is dvntgeous to eliminte the term if possible. In order to remove the third-order term, the following coefficients hve to be ignored, but only if they re justifibly insignificnt: L m J L nd J L In generl, the electricl time constnt of motor is often very smll nd my be ignored. It will be shown in lter chpter tht this ssumption holds true ccording to the specifictions of the motors used in the prototype. Thus, 0 R L (6.15)

94 78 Bsed on this simplifiction, the third-order term vnishes nd the equtions cn now be reduced to second-order system. The vlidity of this simplifiction nd the control lgorithm bsed on these equtions will lter be verified by computtionl simultion s well s hrdwre testing. Dividing Eqs (6.10) nd (6.14) by R, t E R K n t R K K n B B n t J J n t g L b t g L m g L L m g (6.16) t T t R K K n B B n t J J n L L b t g L m g L L m g (6.17) For convenience of clcultion, some prmeters my be grouped together, L m g eq J J n J (6.18) L m g eq B B n B (6.19) Now, Eqs (6.16) nd (6.17) my be written s: t T n t R K K n B t J m g L b t g eq L eq (6.0) And t T t R K K n B t J L L b t g eq L eq (6.1) Using the principle of superposition, t T t T n t R K K n B t J L m g L b t g eq L eq (6.) Or, t T t E R K n t R K K n B t J L t g L b t g eq L eq (6.3)

95 79 Let θ = θ L, nd subscript i = l or r, for left or right wheel respectively, i b t g eq i eq i t g i L R K K n B J E R K n T, (6.4) Or, i b t g eq i eq i t g i L R K K n B J E R K n T, (6.5) Lod torque on ech wheel is r F T i x i L, ', (6.6) Substituting Eqs (6.5) nd (6.6) into (3.4), v F F m u r x l x, ' ', 1 v R K K n B J E R K n R K K n B J E R K n mr u r b t g eq r eq r t g l b t g eq l eq l t g 1 But b v, 1 b R K K n B J E E R K n mr u r l b t g eq r l eq r l t g (6.7) From Eq (3.1), r l r u r u r l Substituting into Eq (6.7), 1 b r u R K K n B r u J E E R K n mr u b t g eq eq r l t g

96 80 1 b E E mr R K n u R K K n B mr u mr J r l t g b t g eq eq r l t g b g t g eq eq t g E E K n mrb R u K n K n B R r u r J mr K n R (6.8) Substituting Eqs (6.5), (6.6) nd (3.5) into (3.6), u v m F F r y l y ' ' u b mb d F F I r x l x z 1 ' ' u b mb R K K n B J E R K n R K K n B J E R K n r d I l b t g eq l eq l t g r b t g eq r eq r t g z 1 u b mb R K K n B J E E R K n r d I l r b t g eq l r eq l r t g z 1 (6.9) From Eq (3.), l r d br v l r d br b r d l r Substituting the bove into Eq (6.9), l r t g b t g eq eq z E E r R d K n mbu R K K n B r d mb r d J I l r t g b g t g eq eq z t g E E u d K n mrb R K n K n B R r d d mrb r d J d ri K n R (6.30) 6. Model Reference Adptive Control Adptive control is brod rnge of techniques tht employ utomtic tuning to help controllers sustin the performnce levels of systems with prmeters tht vry over time or

97 re not fully known (Spong et l. 006). The wy tht dptive controllers work is bsed on the fundmentl concept of online prmeter estimtion in which estimtes of unknown or uncertin prmeters re constntly fed bck to the system to be used in the computtion of the control input. The bility to cope with complex systems with prmeter uncertinties mkes this type of control nturl choice in the field of robotics. Adptive control is inherently non-liner. There re mny types of dptive control schemes, but they lrgely follow either of two pproches: indirect or direct (Lndu 003). In the first pproch, lso clled the explicit method, the plnt prmeters re estimted online from mesured dt nd then used to clculte the controller prmeters. It strives towrds the convergence of estimted nd uncertin prmeter vlues. In the second pproch, lso clled the implicit method, controller prmeters re estimted directly without ny prmeter estimtion of the plnt model. This mens tht there is no requirement for ny explicit system identifiction. Thus, the direct pproch leds to more strightforwrd designs s well s better performnce in generl. Nevertheless, ech pproch hs its own dvntges nd disdvntges. The control scheme dopted in this reserch is model reference dptive control (MRAC). While MRAC cn be of either the indirect of direct type, it is usully ssocited with the ltter, nd this will be the pproch used in the reserch. In MRAC systems, reference model tht describe the plnt chrcteristics is used in the design of the controller (Slotine nd Li 1991; Ionnou nd Sun 1996). Bsiclly, control input is pssed into both the reference model nd plnt, nd the difference between the idel nd ctul outcomes re used to mke djustments in the next set of inputs. A generlised MRAC system is shown in Fig Fig. 6.1 Model Reference Adptive Control System 81

98 In order to design the reference model, it is necessry to hve n intimte understnding of the plnt s well the level of performnce required. Nturlly, this involves detiled mthemticl modelling. To ensure the stbility of the system, Lypunov's second (direct) method is used in the nlysis. The bsic ide of Lypunov's direct method is tht if the totl energy of system is being dissipted continuously, it will eventully rrive t n equilibrium point nd remin there (Freemn nd Kokotović 1996). The generl procedure requires suitble sclr energy-like function (lso clled the Lypunov function) to be chosen nd its derivtive long system trjectories to be evluted. If the derivtive is demonstrted to be decresing long the system trjectories s time is incresing, it cn be concluded tht the system's energy is dissipting nd will thus settle eventully t equilibrium (Khlil 00). If symptotic stbility cn be proven using Lypunov's method, there is no longer ny necessity to solve for the system's differentil equtions to determine its stbility. Hence, this pproch is often clled the direct method Feedbck Lineristion The rtionle behind feedbck lineristion is to trnsform complicted non-liner systems into simpler but equivlent liner forms. This is ccomplished by trnsformtion of stte nd input, nd with non-liner feedbck. Liner systems re well-understood, extensively documented nd reltively esier to design. So, there re cler dvntges of being ble to linerise system. In the model used here, the feedbck control is orgnised in two prts s illustrted in Fig. 6.. The first computes ll the non-linerities within the plnt so tht the second cn be designed to be bsed on liner nd decoupled system. Fig. 6. Feedbck Lineristion 8

99 A few common symbols tht will be used from here on shll be defined now: 1. The subscript "d" is the desired vlue of n ssocited vrible. The tilde (~) ccent is the difference between current/nominl nd desired vlues 3. The circumflex (^) ccent is the nominl (estimted) vlue of vrible The im of this WMR is to trck trjectory, so it would seem intuitive to choose the coordintes [x, y] s the trcking vector. However, the control is more direct if it mtches the physicl behviour of the non-holonomic WMR. As the WMR cnnot move lterlly to prescribed position but rther hs to steer towrds it, more direct trcking vector would include vribles for longitudinl nd turning (yw) motion. The kinemtics of the system s described in Eqs (3.11) nd (3.13) cn be expressed in mtrix form: u cos sin sin x cos y (6.31) Now, let the trcking vector be defined s: q 1 u q (6.3) q Following the bove, q 1 u q q (6.33) q1 x' q (6.34) q The dynmic equtions in Eqs (6.8) nd (6.30) cn now be expressed in the form: Mq C q q DU (6.35) i where M is the inertil mtrix, C is the mtrix contining the Coriolis nd centrifugl terms, D is coefficient mtrix, nd U i is the input vector, i.e. 83

100 84 d mrb r d J d ri K n R r J mr K n R M eq z t g eq t g 0 0 b g t g eq t g t g b g t g eq K n K n B R r d q d K n mrb R q K n mrb R K n K n B R r q C D r l i E E U (6.36) Choosing the control lw with h q s new input tht is yet to be defined, q q C Mh DU q i q q C Mh D U q i 1 (6.37) Substituting Eq (6.41) into Eq (6.39), q C q Mh DD q C q q M q 1 q q h (6.38) By choosing the input shown in Eq (6.37), the result is system tht is liner nd decoupled s illustrted in Eq (6.38). This shows tht the input h q cn now be crefully selected to control SISO (single-input single-output) liner system. Since there is second order system involved, let the input h q be: q q q h d q ~ ~ (6.39) where q q q d ~ q q q d ~

101 nd λ is strictly positive number. q d q d h q ~ q q~ 0 ~ q q q~ 0 q ~ q~ q~ 0 (6.40) The bove is the typicl response of criticlly-dmped liner second order system with nturl frequency of λ. It shows tht the error dynmics hve two rel poles nd is thus exponentilly convergent. The nturl frequency is responsible for the speed of response of the system nd lso the rte of decy of the trcking error. To solve for the ccelertion of the robot, q 1 q M DUi C q (6.41) Substituting from Eq (6.38), h q M 1 DU i C q q (6.4) Solving Eq (6.46), Mh U i q DU i Cq q q D 1 Mh C q (6.43) q The result is the sme s Eq (6.37) nd illustrtes how the initil input ws trnsformed within the system. 6.. Liner Prmeteristion nd Adptive Dynmics In order to pply the equtions of motion of ny dynmic model for purpose, the system prmeters hve to be identified nd determined. These prmeters could include msses, sptil dimensions, moments of inerti, etc. To help simplify nlysis nd solution, ttempts re often mde to linerise the equtions of motion. 85

102 For exmple, the inertil, Coriolis nd centrifugl terms of Eq (6.35) my be expressed in the following mnner: q K r q q p Mq C q, (6.44) where K r is the regressor mtrix θ p is the prmeter vector It is cler tht the originl expression hs been trnsformed, nd is now liner in the prmeters. In inverse-dynmics, the vlues of system prmeters re often uncertin or not known t ll. This my be due to vrious kinds of errors or simply lck of informtion. Thus, nominl (estimted) vlues of prmeters re used insted. Revising the control lw from Eq (6.37), U i q 1 D Mh ˆ Cˆ q (6.45) q where ~ M M Mˆ ~ C C Cˆ However, ccording to Eq (6.39), h q q d q~ q~ Substituting Eq (6.39) into (6.45) nd the result into Eq (6.35), 1 Mq Cq q DD M ˆ q d q q Cˆ q q M ~ Mˆ q Cq q Mˆ q d q~ q~ Cˆ q q ~ Mq Cqq Cˆ qq Mˆ q d q q~ q~ ~ ~ M q Cqq Mˆ q~ q~ q~ q ~ q~ q~ Mˆ ~ ~ Mq Cq q 1 ~ ~ (6.46) 86

103 87 Applying the technique of liner prmeteristion s explined erlier, the expression consisting of the inertil, Coriolis nd centrifugl terms in Eq (6.46) becomes: p r q q K q q C q M ~, ~ ~ (6.47) where , q q dq q q q q q q K d r (6.48) nd t g b g t g eq eq z t g eq t g p K n mrb R K n K n B R r d mrb r d J d ri K n R r J mr K n R p p p p 1 ~ ~ ~ ~ ~ (6.49) Thus, Eq (6.46) cn be expressed s such: p r q q K M q q q ~, ˆ ~ ~ ~ 1 (6.50) Now, let the trcking error be defined s: q q q q q q e d d t ~ ~ (6.51) The system equtions cn now be expressed in the stndrd stte-spce representtion: p r t t K B M Ae e ~ ˆ 1 (6.5) where A (6.53)

104 nd B (6.54) Consider the Lypunov function cndidte: V ~ T ~ T 1 ~ e t, p et Pe p p (6.55) where P nd Γ re symmetric positive definite constnt mtrices nd T PA A P Q (6.56) where Q is strictly positive nd symmetric, i.e. Q Q T 0 (6.57) Tking the derivtive of Eq (6.55), T T T V ~ 1 ~ 1 e tqet p p K r Mˆ B Pet (6.58) For gurnteed stbility, the lst term is equted to zero. Hence, ~ T 1 p K r Mˆ B T Pe t (6.59) By definition: ~ p ˆ (6.60) p p But since θ p is constnt, ~ p ˆ p (6.61) 88

105 Thus, the prmeter updte (dpttion) lw is selected s such: ˆ p K T r Mˆ 1 B T Pe t (6.6) Applying the bove prmeter updte lw to Eq (6.58), V e t Qe 0 (6.63) t Since Q is strictly positive, it cn be concluded tht V e t, ~ p e ~ t p is negtive semi-definite in the, spce, which mens tht V(t) V(0) for ll t 0. Therefore, ccording to Brblt's lemm, the position trcking errors, e t, will converge to zero symptoticlly nd prmeter errors will remin bounded. The detiled proof for the ppliction of Brblt's lemm hs been described in existing literture (Popov 1973; Slotine nd Li 1991; Fontes nd Mgni 004) nd is not shown here. 6.3 Kinemtics nd Geometry of Vehiculr Motion The nvigtionl nd trcking bility of mobile robot to determine its loction is not only dependent on the precision of its sensors. Eqully importnt re the underlying lgorithms tht turn rw sensor dt into useful nd ccurte informtion. In the PID model, n Eulerin discretistion pproch ws used. When the smpling period is very short, the pproximted results re ner exct. It is the sme method tht is used when robot hs only one sensor nd hs to depend on kinemtics to clculte its movement. In the dptive model, truer pproch ws dopted by utilising geometriclly ccurte nlysis. Before n nlyticl frmework cn be formulted, it is impertive to gin detiled understnding of the nture of how the sensors work. Although the type of opticl mouse sensor used in this reserch hs incredibly high precision, it lso hs its quirks nd limittions. When the sensor detects movement, it reports two sets of motion-relted dt: Δx nd Δy. These figures correspond to the locl displcement with respect to the sensor itself. Note tht in this reserch, the defult coordintes of the sensors re swpped to mtch those of the vehicle. 89

106 When the vehicle is turning, the sensor would move through continuous series of rcs. Consider segment tht the sensor hs covered over rndom smpling period. Being tht the pth is n rc, turning rdius nd centre must exist s shown in Fig The forwrd motion of ny object in circulr motion is the tngent to the circle. The longitudinl (locl x') xis of the sensor ligns with its forwrd motion, nd therefore with the tngent s well. This mens tht its lterl (locl y') xis coincides with the rdius of the turning rc. Since the rdius remins constnt during the prticulr time period, it mens tht its displcement in the lterl (locl y') direction is effectively zero. This implies tht sensor will not be ble to distinguish between motion in stright line nd perfect rc (or circle). Fig. 6.3 Two distinct pths with identicl sensor redings A simple test will illustrte this prticulr behviour of the sensor. The cord of computer mouse is held down t ny chosen position. Tht point on the cord being pressed down would represent turning centre nd the cord length between tht point nd the mouse would serve s rdius. This rrngement llows the mouse to move long the pth of n rc. Insted of the pointer/cursor on the computer screen trvelling long similrly circulr pth, it will in fct move in n pproximtely stright line. Bsed on the forementioned chrcteristic, the sme principle pplies if there is n ngulr offset. As illustrted in Fig. 6.4, even if the longitudinl nd lterl xes re not ligned with the tngent nd rdius respectively, the sensor cn still be insensitive to turning motion. In this cse, the constnt offset ngle contributes to constnt lterl (locl y') displcement. This cn be construed s constnt lterl devition nd be treted s constnt grdient to stright line. Therefore, ny turning motion could still be potentilly missed. 90

107 Fig. 6.4 Another exmple of two distinct pths with identicl sensor redings The use of two opticl mouse sensors effectively negtes the quirk of the sensor not being ble to distinguish stright pth from curved one. The different positions of the sensors men tht the ech sensor will experience different turning rdius, nd the corresponding tngents will not be equl. This is illustrted in Fig Using this piece of knowledge, the overll ngulr displcement of the vehicle cn be worked out. Following tht, the longitudinl nd lterl displcements of ech sensor cn be derived geometriclly. So, there is no longer ny mbiguity in discerning stright line from curved pth bsed on two-sensor system. If both sensors hve the sme redings nd ngle between them is zero, the only conclusion is tht the vehicle is trvelling is stright line in the longitudinl (locl x') direction. There is bsolutely no mistking it for curved trjectory. Fig. 6.5 Geometry of sensor movement during vehicle motion 91

108 In order to work out the geometry of the vehicle's pth of trvel, the known quntities hve to be indentified first. As the sensors min purpose is to chrt their own movement, it is only nturl tht distnce trvelled by ech sensor is obtinble from their dt output. The other known prmeters re the vehicle's dimensions nd how they relte to the sensors. The geometricl clcultion of the vehicle's pth is strightforwrd if it is moving in stright line. In ny turning motion, there will be common turning centre for the sensors nd vehicle s whole, but different rdius for ech sensor becuse of how they re positioned. Since the pth nd direction trvelled by ech sensor cn be derived from the output dt, the mgnitude of the turning rdius cn lso be clculted - s will be shown in the equtions below. As the distnce between the sensors is fixed constnt, the tringle of three known sides cn be formed s illustrted in Fig The obvious pproch would be to use the Cosine Rule. This is similr to the method dopted by n erlier publiction (Bonrini et l. 004) for WMR with different sensor configurtion. However, there re some key differences in the pproch dopted in this thesis. It hs been estblished erlier tht two-sensor configurtion removes ny doubt in the strightness or curvture of vehicle's trjectory. Although the shpe of the vehicle's trjectory is no longer in question, the inherent nture of the individul sensor remins the sme. This mens tht no mtter whether the trjectory is stright line or n rc, the Δx nd Δy redings of ech sensor will indicte the sme displcement mgnitudes. Refer to Figs. 6.3 nd 6.4. Therefore, the length of the pth reported by ech sensor cn be clculted s stright line: s x' y ' (6.64) The ngle between locl x' xis nd tngent to the turning rc is: y' tn 1 x',, x' 0 x' 0 (6.65) The locl longitudinl nd lterl displcements re respectively: x' scos y' ssin (6.66) (6.67) 9

109 The fixed distnce between sensors is: L k (6.68) where is the distnce between Sensor 1 nd the wheel bseline k is the distnce between Sensor nd the wheel bseline Using the Cosine Rule, L r 1 r r 1 r cos (6.69) where the ngle between the turning rdii is: (6.70) 1 Knowing the rc length, the rdius of the turning rc cn be determined: s (6.71) 1 r 1 s r (6.7) 1 1 s (6.73) r s r (6.74) By substituting Eqs (6.7) nd (6.74) into the Cosine Rule of Eq (6.69), the chnge in heding (yw) cn be found: s1 s s1s cos, y1 y L s1 s s1s cos, y1 y (6.75) L 0, y1 y 93

110 For computtionl purposes, s r 1 1, signed s (6.76) r, signed (6.77) It is cler tht in Eqs (6.7), (6.74), (6.76) nd (6.77), if the yw ngle Δψ is zero, the turning rdii r 1 nd r will become infinitely long. From physicl point of view, it indictes tht the vehicle is moving in stright line purely long its longitudinl x-xis. Since the vehicle is not turning in ny wy, there is no turning rdius. Thus, the mgnitudes of the two turning rdii present in the geometric equtions will become mthemticlly infinite. Hence, cre must be tken to formulte the equtions in such wy s to ccommodte this scenrio. As ngle Δψ tends towrds zero nd the turning rdii grow infinitely lrge, there will come point when computtionl limits re reched in terms of vrible size nd processing power. Thus, decision must be mde on the precision level governing when to round off Δψ s zero s it becomes infinitesimlly smll. Specil-cse lgorithms would then be invoked to hndle the scenrio of Δψ being zero. Nturlly, smll quntistion nd rounding errors will occur s result. x r1, x cos 1 cos signed 1 1' A,, 0 0 r1, signed sin 1 sin 1, 0 x 1' (6.78) x A, 0 y r1, 0 sin 1 sin signed 1 1',, 0 0 r1, signed cos 1 cos 1, 0 y 1' (6.79) 0, 0 x r, x cos cos signed ' K,, 0 0 r, signed sin sin, 0 x ' (6.80) xk, 0 94

111 y r 0 sin sin, signed ',, 0 0 r, signed cos cos, 0 y ' (6.81) 0, 0 Note tht Δx A, Δy A, Δx K nd Δy K re the reported displcement dt long the locl xis s reported by the Sensors 1 nd t positions A nd K respectively. These numbers re not equivlent to ctul locl displcements Δx 1 ', Δy 1 ', Δx ' nd Δy ' respectively, except when there is no turning motion, i.e. Δψ is zero. As discussed previously, since the sensors re insensitive to turning motion, the perceived displcement vlues my not reflect the ctul pth tken. The true locl displcements, Δx 1, Δy 1, Δx nd Δy, hve to be determined by geometric clcultions. Using the similrity principle, the locl displcement of the centre of grvity cn be found: b k x ' b x ' k b x ' k b x G ' 1 1 b k (6.8) y ' b y ' k b y ' k b y G ' 1 1 (6.83) The locl velocities t the WMR's centre of mss re simply derived from Eqs (6.8) nd (6.83): dxg u ' dt (6.84) dyg v ' dt (6.85) With heding ngle of ψ known from the previous smpling period, the globl displcements of the sensors cn be clculted s thus: sin r1, signed sin 1 1, 0 x 1, globl (6.86) x A cos, 0 cos r1, signed cos 1 1, 0 y 1, globl (6.87) 0, 0 95

112 sin r, signed sin, 0 x, globl (6.88) xk cos, 0 cos r, signed cos, 0 y, globl (6.89) 0, 0 Since Sensor is the designted reference sensor for the WMR's position, t 1 xt x globl x, t 1 yt y globl y, (6.90) (6.91) new (6.9) 6.4 Limittions of the Geometric Approch The nlysis of the WMR's motion presented in the previous section is the idel geometric model. It is certinly more fithful mthemticl representtion of vehicle's motion thn the liner pproximtion pproch of Euler s discussed in Chpter 3. However, the idel method is not without its limittions - especilly when pplied computtionlly. As pointed out in the previous section, there re inevitble trunction nd rounding errors s well s constrints on vrible size nd processing cpbilities. In the lter chpter describing the prototype, the documenttion will show tht the min progrm uses double-precision (64-bit) floting point numbers for storge nd clcultions. This is necessry becuse the high precision of the sensor leds to very smll vlues during clcultions, nd the turning rdius' tendency to pproch infinity mens tht numbers cn lso get very lrge. Indeed, simultions confirm tht the use of single-precision computtion ws indequte for the progrm to execute without significnt loss of precision in hndling either very smll or very lrge numbers. A mjor shortcoming of the min microcontroller used by the WMR is tht it hs no floting point unit. This is quite typicl of microcontrollers used in embedded systems. As such, every non-integer number hs to be hndled vi emultion. This inevitbly leds to significnt loss of processor performnce. Depending on the opertion, it could be slower thn similr integer clcultions by severl orders of mgnitude. Aside from the processor limittions, there is nother notble concern. The direct consequence of very smll numbers being clculted is the lrge mgnitude of the turning rdius. Smll fluctutions in the rw sensor dt, whether cused by detection error or sensor 96

113 noise, my cuse similrly smll error percentge in the clculted vlue of the turning rdius. However, the lrge mgnitudes involved men tht even tiny percentge shift cn led to significnt devition in the finl clculted vlues in bsolute terms. In essence, errors re mgnified. This ws clerly noticed during prototype testing. During the prototype testing phse, the clibrtion of sensors reveled tht there ws slight difference between them. On typicl test surfce, the sensor with the lower sensitivity mnged x 10-5 m/count. The minimum detectble vehicle turning ngle is when either (but not both) of the sensors registers single count in the lterl (Δy') direction. Using either Eq (3.18) or Eq (6.79), the minimum turning resolution of the vehicle cn be determined rd (6.93) min 10 So, it is theoreticlly impossible for the progrm to return vlue smller thn the bove. However, this is not lwys the cse ccording to experimentl dt. Regrdless, s Δψ pproches zero nd the progrm needs to round off to zero in order to invoke the specilcse lgorithm which presumes the vehicle to be trvelling in n bsolutely stright trjectory. A cut-off point is required, nd the minimum turning resolution serves s good criterion for it. By including generous mrgin of error, the cut-off point ws set t: rd (6.94) min 10 Once this minimum point ws set, the turning rdii of both sensors will no longer continue to pproch infinity. Yet, dt collected from ll the test runs revel tht both rdii routinely exceed x m nd occsionlly even surpss 3 x m. These re indeed very lrge numbers when compred to the size of the vehicle nd typicl trvelling distnce. It is known fct tht rel numbers re not represented exctly in binry system tht is typiclly used in modern computing (Goldberg 1991). Their pproximte representtions re clled floting point numbers, nd there is tendency for loss of precision s numbers get very lrge. This is especilly true if those lrge numbers re derived from very smll vlues which could led to loss of significnce. The processors nd softwre used in the development of this prototype ll dhere to the IEEE Stndrd for Floting-Point Arithmetic (IEEE 754). The stndrd mesure of precision is the unit in the lst plce (ULP). This is the smllest possible difference between two consecutive numbers tht cn be represented. It is kin to resolution or grnulrity metric, nd is reltive to the mgnitude of number. The smllest possible precision is mesured t vlue of "1" nd is clled the mchine epsilon. 97

114 The determintion of the ULP t given numericl rnge is quite strightforwrd. MATLAB lso hs built-in function to clculte the ULP which is clled eps(). Since the ntive computing formt is in binry, the clcultions will use bse of two. For given number, n, e( p1) ULP( n) (6.95) where β is the bse e is the integer exponent p is the number of precision bits In double-precision (64-bit) number, there re 53 (5 stored) significnd bits, 11 exponent bits, nd 1 sign bit. The rnge of numbers from x to 3 x cn be represented in bse s to For ny number somewhere within this sid rnge, the ULP is: ULP (10 13 ) 44(531) 13 3 ULP (10 ) (6.96) As mentioned before, x m is vlue frequently exceeded in clcultions of the rdius length. This rdius is directly used to determine the coordintes of the sensors nd by extension the vehicle. While the sensors hve precision levels t bout 3 x 10-5 m/count, the lrge rdii vlues used in the clcultions cn only mnge precision of just under 4 x 10-3 m. This loss of precision mounts to more thn two orders of mgnitude (i.e. over 100 times) nd is thus considered very significnt. This is certinly serendipitous discovery. The problem is compounded by the fct tht it hppens quite frequently when the vehicle is trvelling in nerly stright line. Furthermore, ded-reckoning errors re cumultive. Indeed, if the sensor resolution were set higher thn the nominl 800 cpi, the minimum detectble ngle would be smller, thus llowing the mximum turning rdius to rech fr lrger mgnitude before it hs to be truncted. For exmple, if the mximum rdius were to be incresed by fctor of 10, the precision of the clculted coordintes would be in the rnge of well over 15 mm. ULP ( ) As is clerly illustrted here, bigger numbers led to greter loss of precision. Ironiclly, in order to chieve the highest precision possible, ccurcy is ctully lost in the process. Thus, it is cler tht lthough the geometric model is mthemticlly ccurte, it is not quite so in prcticl sense. Furthermore, there re other issues in ddition to the limittions in 98

115 floting-point precision. To void situtions leding to infinity, very smll numbers hve to be rounded to zero. At the other end of the scle, extremely lrge vlues hve to be truncted. So, the idel model is not s perfect s expected. Nevertheless, the geometric pproch is still more ccurte method thn Euler's liner pproximtion in generl. The rounding nd quntistion errors s well s noise sensitivity issues of the geometric method only occur within smll rnge of motion, i.e. when the turning ngle Δψ is ner or equl to zero. Other thn tht, the forementioned concerns do not pply. However, vehicles do generlly move forwrd in somewht of stright line much of the time. So, lthough the problem is confined to nrrow rnge of motion, it occurs quite frequently. In contrst, Euler's method suffers from discretistion errors throughout the entire rnge of motion except when the vehicle is trvelling in n bsolutely stright line. 6.5 Euler's Method Revisited nd Hybrid Technique It is cler tht s the geometric method pproches point where its precision begins to deteriorte, it is lso when Euler's method tends to be most ccurte. The logicl conclusion would be to combine the two methods. The geometric pproch would cover nerly the entire rnge of motion, while Euler's method would pply to situtions where turning is nerly or totlly bsent. The key question would be when to switch between methods. An obvious choice for the switch-over point is just before the turning rdius reches mgnitude where the numericl precision of the processor flls below the sensor's resolution. For rounding purposes, the criticl number should be no more thn hlf the resolution of the sensor with the higher sensitivity. Let r crit be the criticl rdius t which computing precision exceeds sensor resolution, σ s. Thus, the minimum llowble computing precision is: ULP( ) 0.5 (6.97) r crit s But β = for ny binry system. Hence, e crit ( p1) 0. 5 e e crit ( p1) crit 0.5 s p log 0.5 s p 1 s ecrit log s (6.98) 99

116 where e crit is the bse- exponent tht corresponds to the criticl rdius However, the ULP is clculted using rounded-down integer exponents. This mens tht ny number up to but not including the next higher integer is rounded down to the next lower integer. So, the exclusive limit of e crit is the next higher integer. A common function in C nd MATLAB used for performing this rounding-up opertion is clled ceil(). Thus, Eq (6.10) should be revised s such: ecrit ceil log 0.5 s p 1 (6.99) The criticl rdius cn now be determined: r e crit crit (6.100) According to the clibrtion results in Tble 8.3, the more sensitive sensor of the two hs resolution of.87 x 10-5 m. To clculte the criticl rdius of the system, e crit e crit ceil 36 5 log r crit 36 r crit The criticl rdius will thus be set t: r crit m (6.101) The corresponding computer (mchine) precision t this numericl rnge is confirmed to be well beneth the sensor's resolution: 10 6 ULP (6.810 ) (6.10) 100

117 Bsed on the bove figures, the switch-over from the geometric technique to Euler method ws ssigned to be the point when either of the rdii exceeds 6.8 x m. The trcking equtions cn now be revised ccordingly. For convenience, Eqs (6.64) to (6.77) will be reproduced here lbeit without their originl descriptions nd explntions: s x' y ' (6.103) y' tn 1 x' x' scos y' ssin,, x' 0 x' 0 (6.104) (6.105) (6.106) L k (6.107) L r 1 r 1 r 1 r cos (6.108) (6.109) s (6.110) 1 r 1 s r (6.111) 1 1 s (6.11) r s r (6.113) s1 s s1s cos, y1 y L s1 s s1s cos, y1 y (6.114) L 0, y1 y s r 1 1, signed s (6.115) r, signed (6.116) 101

118 Referring bck to Fig. 6.5, the revised equtions re thus: Or, Or, Or, Or, r1, signed sin 1 sin 1 x1 ' s1 cos 1 r1, x1 ' x signed A sin sin r1, signed cos 1 cos 1 y1 ' s1 sin 1 r1, y1 ' y r x1 ' s x ' signed A, signed r x 1 cos cos cos, signed K 1 sin sin sin sin r, signed cos cos y1 ' s sin y r, ' y signed K 1 1 cos cos,,,,,,,,,,,,, r r 1 r r 1 r r,, crit crit r r 1 1 crit crit r r r r 1 1 r r, crit crit r r 1 r 1 r r r r r crit crit r r r r crit crit r crit r r r crit r crit crit r crit crit (6.117) (6.118) (6.119) (6.10) The equtions for the locl displcement of the centre of grvity remin the sme s Eqs (6.8) nd (6.83): b k x ' b x ' k b x ' k b x G ' 1 1 b k (6.11) y ' b y ' k b y ' k b y G ' 1 1 (6.1) Similrly, the locl velocities t the WMR's centre of mss re unchnged from Eqs (6.84) nd (6.85): dxg u ' dt (6.13) dyg v ' dt (6.14) 10

119 The revised globl displcements of the sensors re: Or, Or, Or, x x y y x x 1, globl 1, globl 1, globl 1, globl, globl, globl r1, signed sin s1 cos 1 r1, x signed A sin 1 sin1 sin 1 cos y r1, signed cos s1 sin 1 r1, x r, s signed A cos sin y signed r, x cos signed K sin sin A sin 1 cos 1 cos 1 A cos sin sin cos y K 1 1,,,,,,,,,,, sin, r r r r 1 1 r r crit crit r r 1 1 crit crit r r r r 1 1 crit crit r r 1 r r r r 1 r r r r r crit crit crit crit r crit crit (6.15) (6.16) (6.17) (6.18) (6.19) (6.130) Or, y y, globl, globl r, s r, x signed cos sin signed K cos sin y cos cos K cos,,,, r r r r r r r crit crit r crit crit (6.131) (6.13) The equtions for the WMR's position re still the sme s Eqs (6.90) to (6.9). Assigning Sensor s the reference sensor, t 1 xt x globl x, t 1 yt y globl y, (6.133) (6.134) new (6.135) The bove equtions do not include ny ccommodtion for sensor mislignment. These mislignment ngles re obtined through clibrtion nd cn simply be included lter. The next section provides n exmple of how this is done. With minor modifictions, the generl principle of the method described bove is pplicble to other systems tht employ multiple opticl mouse sensors. 103

120 6.6 Error Detection nd Correction for Prtil Redundncy The ccurcy of the nvigtion nd trcking of WMR depends on the precision of its sensors s well s the loclistion lgorithm used. The previous section underlined the prcticl limittions of both method nd computtion precision, nd how they cn be ccurtely ddressed. However, without relible sensor dt, ll other efforts will mount to nothing. This section will discuss the identifiction of sensor errors nd the mens to cope with them while mintining n cceptble level of trcking ccurcy. It is commonly understood tht even under the best experimentl circumstnces, sensors will occsionlly report inccurte dt. Presume tht either of the sensors hs provided erroneous redings which cn be identified nd corrected (or discrded). If the relible dt from the other sensor could be used s some sort of reference for correction, then some level of sensor redundncy could be chieved. It is impossible for dt from single sensor to fully compenste for the loss of dt from the other sensor. Fctors such s slippge cnnot be ccounted for. Thus, the redundncy level cn only be considered prtil. Mny WMRs pst nd present hve only relied on the use of one sensor for nvigtion vi ded-reckoning. These types of systems generlly depend on kinemtic pproch for coordinte clcultion. The inherent quirks of the opticl mouse sensor s well s the inbility to fully ccount for slippge mke it unsuitble for precise loclistion. While the kinemtic pproch my not possess the best level of ccurcy, it is still widely used in robotics for situtions where slippge miniml. So, it is postulted tht the kinemtic method could still be used springly s contingency lgorithm when dt from one of the sensors is fulty. It is certinly preferble to using questionble dt or stopping the vehicle completely until its position cn be reset. There re severl cuses leding to dt vrince nd error in sensor, such s surfce texture, clernce between sensor nd surfce, sensor velocity, nd rndom noise (Minoni nd Signorini 006; Plcin et l. 006). Sensor noise here is collective term for unpredictble errors such electricl fluctutions s well s other uncertinties. It is importnt to identify nd ccount for this type of error, but it cnnot be eliminted. In the reserch, only one type of surfce is used per series of tests. The surfce is crefully selected nd clened to ensure tht it is quite uniform nd visibly free defects or debris. While not perfect, the verge chrcteristics of the surfce cn be considered to be rther consistent. Thus, ny significnt chnge in sensor sensitivity due to this reson would be infrequent, loclised nd limited to n extremely short spn of time. 104

121 In regrds to the effect of sensor speed, there is smll vribility s long s the sensor opertes within the mnufcturer's recommendtion. Since the WMR used in this reserch is only cpble of very low ccelertion nd the smpling time is quite smll (i.e. 0.0 s), the vehicle speed cn be seen s lmost constnt in between polling. Thus, the effect of sensor speed is presumed to be mrginl. Severl initil stright-line tests t different speeds (30% to 100% of mximum) detected no discernible difference in mesured distnce. There is one fctor tht contributes by fr the biggest vribility in sensor sensitivity nd thus hs the most influence over the dt vlues. It is the distnce between the sensor nd the surfce tht it is trcking. This is known from the hrdwre chrts provided by the mnufcturer s well s from the experience gined during the clibrtion process. Although the recommended operting height rnge is very nrrow, newer sensors hve wider mrgin outside tht rnge where they still perform ner optimum level. Beyond the extended rnge, sensitivity chnges drsticlly nd generlly drops off precipitously. Tht is why it is extremely importnt to ensure tht the test surfce is reltively flt nd void of defects. In order to mintin constnt ground clernce, some reserchers hve ffixed the mouse (in its originl enclosure) to the WMR such tht it is in constnt contct with the ground. Nturlly, this rrngement cuses lot of friction nd my obstruct the vehicle's movement if there is even the slightest surfce protrusion. If the mouse mnges to scle the obstruction, then it hs lifted off the surfce nd hs thus ltered the distnce between sensor nd ground. The sme could hppen if ny of the wheels were to roll over smll object or debris. This negtes the benefit of hving the mouse slide on the ground. The only cler dvntge of this design is tht minimum distnce between the sensor nd surfce is constrined by the bottom thickness of the mouse, nd thus there is cler lower limit. Generlly, mny of WMRs tht use opticl mouse sensors hve smll ground clernce so tht the sensor (or its enclosure) would not rub long the ground nd thus llow for smoother running. This reserch uses the sme set-up. However, the trde-off for this sort of configurtion is tht there is greter fluctution in the distnce between sensor nd surfce, nd thus greter potentil for inccurte redings. In the clcultion of sensor dt to determine the vehicle's position, the distnce between the two sensors is known nd constnt prmeter. So, this serves s n excellent reference in ny clcultion for detecting dt vrince. Now, the two sensors on this WMR hve fixed distnce between them on the sme longitudinl xis, so their displcement redings in the longitudinl direction should idelly be identicl. If there is ny difference in their redings on the shred xis, their reltive positions to ech other on tht xis will chnge. This mens tht 105

122 the distnce between the sensors is no longer the sme s before. Since this is physiclly impossibility, the originl ssumption holds true. Hence, for Sensor 1 locted t position A, nd Sensor locted t position K, their locl displcements re: x (6.136) A x K Including djustments for minor mislignment, x cos x cos (6.137) A A K K where ε is the mislignment ngle with respect to the longitudinl xis, x' So, if Eq (6.137) does not hold true, one or both redings my be incorrect. Nturlly, certin mrgin of error must be llowed. If the redings in the x-xis re questionble, the redings in the y-xis from the sme smple must lso be treted similrly. It is perfectly possible for both redings to be erroneous nd still pss the Eq (6.137) condition. However, this is coincidentl nd unlikely to hppen often. If this sitution does indeed rise, it would be quite impossible to detect. Furthermore, there would be no mens to resolve the problem becuse this method requires t lest one sensor to report relible redings t ny given time. A previous study (Bonrini et l. 005) ssumed tht the errors encountered in the uthors' reserch were due to under-reporting tht is cused by decrese in sensitivity levels. This implies tht displcement vlues should never exceed wht re expected. In tht reserch, two mice in their originl shells were ffixed to the WMR in such wy tht they were forced to mke contct with the ground. So, the height between the sensor nd trcking surfce cn never be less thn the bottom thickness of the mouse housing. According to the mnufcturer's hrdwre chrts (Fig. 8.14), the sensitivity levels decrese when the sensor height exceeds the optimum rnge. So, for tht reserch, the hypothesis seems perfectly resonble. Unfortuntely, these ssumptions do not pply to other WMRs tht mintin gp between sensor nd ground. In this reserch, the sensor's ground clernce cn vry such tht it cn be more or less thn the optimum height. According to the mnufcturer's dt (Fig. 8.15) of the mouse used in this reserch, sensitivity levels cn be higher of lower thn the optimum vlues outside the recommended height rnge. The principle behind the workings of this type of sensors suggests tht once out of the optimum/working zone, sensitivity levels will generlly decrese s height increses. In this cse, s the sensitivity levels cn vry both wys, it mens tht erroneous redings could be more or less thn the ctul displcements. Ultimtely, the sensitivity levels re expected to decrese s height increses. 106

123 Now, even if dt inconsistency cn be detected using Eq (6.137), it still remins to be determined which sensor is responsible for the fult. Fortuntely, in ddition to displcement vlues, the sensor lso reports the number of fetures trcked by the sensor in ech frme. This metric is clled "surfce qulity" or SQUAL for short. Generlly, sensor trcking improves with greter number of identifible fetures. Conversely, if surfce is perfectly smooth nd fetureless, or if the distnce between sensor nd surfce is beyond the sensor's focl rnge, there will be no identifible fetures. In such cses, the sensor will report no displcement redings nd the SQUAL vlue would be zero. A low SQUAL vlue my not necessrily men tht the dt is unrelible. The SQUAL vlues of sensor would likely be reltively low if the trcking surfce hs texture tht is rther homogeneous. However, the sensor my still be ble identify enough fetures to trck properly. On the other hnd, if the sensor is unble to trck with ny consistency, other thn hrdwre mlfunction, it is lmost certin tht the SQUAL vlues would be very low or ner zero. With this knowledge in mind, the SQUAL dt could be used in conjunction with Eq (6.137) in order to design test for identifying erroneous dt. Firstly, the SQUAL dt for ech sensor on given test surfce re recorded during sensor clibrtion. Like the sensitivity vlues, the SQUAL dt is lso verged over the number of smples s shown in Tble 8.4. The results show tht the SQUAL redings re quite consistent over the sme surfce. This will serve s benchmrk for good qulity redings. The two sensors report slightly different SQUAL vlues over the sme surfce. So, it is meningless to use direct comprison between the two s test for dt relibility. Insted, ech sensor's dt should be weighed ginst its clibrted reference. In sttistics, 99.74% of smple set is contined within three stndrd devitions (σ). The common prctice is to consider ny dt tht flls outside this region s n outlier. So, this is the convention will be followed here. Due to experimentl error, it is unrelistic to use Eq (6.137) s test without including mrgin of error. Agin, the dt from the clibrtions cn be used to determine the verge difference between the redings of both sensors s shown in Tble 8.4. This will be used s reference for judging dt relibility. Similr to the SQUAL dt, the error mrgin used here will be three stndrd devitions. Restting the fundmentl test, x cos x cos 3 (6.138) A A K K x 107

124 If this condition is met within the designted llownce, the clcultions will simply follow the procedure described in Section 6.5. However, the moment the condition fils, the immedite tsk would be to identify the sensor with the presumbly fulty redings. The following equtions re to be used until the next set of dt is smpled nd put through the test gin. The SQUAL test used to determine whether dt is considered relible is: SQUAL Sensor SQUAL 3 SQUAL (6.139) where μ SQUAL is the reference men SQUAL vlue for the sensor σ SQUAL is the reference stndrd devition for the sensor Consider the scenrio where the dt from the front (second) sensor hs filed the SQUAL test nd is deemed unrelible. Accounting for ny mislignment by including correctionl ngle, ε, the locl nd globl displcements cn be determined in the equtions below. The locl displcements of Sensor 1 re: x x A cos y sin (6.140) 1' A 1' A A A A y x A sin y cos (6.141) A As explined before, the sensor mkes no distinction between stright-line or curved trjectory. Ignoring ny potentil slippge, the turning centre cn only fll somewhere long the wheel bseline nd its outwrd projections on both ends. The turning ngle nd velocity re thus: ' y 1 y 1' (6.14) (6.143) t where Δt is the smpling period The dt from Sensor hs been discrded for this smple period, so the dt from Sensor 1 must be extrpolted to estimte the coordintes t the loction of Sensor. It hs lredy been demonstrted tht the displcement in the locl x-xis re the sme for both sensors. 108

125 Also, the turning ngle is common for the entire vehicle. Using the principle of similrity, the displcement in the locl y-xis cn be projected. x ' 1' (6.144) x y ' k (6.145) To clculte dynmic forces cting on the WMR, the locl displcements nd velocities t its centre of grvity must be determined. The sme method for projecting the dt for Sensor is pplied here. x G ' x1' (6.146) y G ' b (6.147) xg ' u t (6.148) yg ' v t (6.149) Since Sensor is the designted reference point, the globl displcements will be clculted with respect to this sensor's loction. x x cos y sin (6.150) y x sin y cos (6.151) Finlly, the equtions for the WMR's globl position re: x y t 1 xt x, (6.15) t 1 yt y (6.153) new (6.154) Now, consider the lterntive cse where the rer (first) sensor's dt hs filed the SQUAL condition nd is considered uncceptble. The clcultions for the locl displcements of Sensor re nlogous to the previous scenrio. x x K cos y sin (6.155) ' K ' K K K K y x K sin y cos (6.156) K 109

126 Similrly, the turning ngle nd velocity cn be determined like before: ' k y y ' k (6.157) t (6.158) Since Sensor is the designted reference point, there is no need to clculte the locl displcements or velocities of Sensor 1. As before, the locl displcements nd velocities t the WMR's centre of grvity cn be determined by using the principle of similrity. x G ' x' (6.159) y G ' b (6.160) xg ' u t (6.161) yg ' v t (6.16) The globl displcements bsed on Sensor s the designted reference point is the sme s in the previous cse. x x cos y sin (6.163) y x sin y cos (6.164) Like before, the equtions for the WMR's globl position re: x y t 1 xt x, (6.165) t 1 yt y (6.166) new (6.167) The shortcomings of the kinemtic pproch hve lredy been extensively documented. So, this lgorithm should only used springly s bck-up option when dt from one of the sensors is deemed unrelible. The moment the dt from both sensors hve pssed the fitness test, the trcking lgorithm should be switched bck to the hybrid pproch tht ws discussed in the preceding section. 110

127 6.7 Summry A hevily-revmped mthemticl model of the wheeled robot hs been developed nd presented in this chpter. While it hs relied on some of the theoreticl foundtion estblished in prior chpters, the bulk of this model differs from the previous version to the extent tht it could lmost be considered new design. The current model is dynmic system tht employs model reference dptive control in contrst to the PID controllers used previously. In theory, the self-tuning cpbilities of the dptive controller provide superior performnce nd verstility. Thus, it will be expected to ddress some of the problems encountered by the erlier model. A crucil prt of the wheel robot is the sensor system. Hence, it ws necessry to highlight the peculir trits of the opticl mouse sensors tht re plnned for use on the prototype. Another importnt re to investigte is the different methods for clculting odometry. Both kinemtic nd geometric pproches were nlysed, nd their respective dvntges nd disdvntges were identified. By combining the best ttributes of both odometric methods, new hybrid technique ws formulted. Lstly, novel prtil-redundncy system ws devised to compenste for momentry sensor error or loss of trcking. With this new model complete, it will now need to be converted into computtionl representtion so tht it cn be tested in simulted environment. 111

128 Chpter 7: Computtionl Design of the Adptive Model The theoreticl underpinnings of the dptive model re fr removed from tht of the erlier PID system. Thus, the resulting model hs lmost been entirely reworked nd resembles little of the previous system. Using the mthemticl foundtion estblished in the Chpters 3 nd 6, new computtionl representtion will be developed in this chpter. Once gin, the simultion is performed with the help of the MATLAB progrm nd its grphicl modelling tool, Simulink. When the simultion results re stisfctory, modifictions would be mde in order for it to be compiled for the processor of the WMR nd work with its hrdwre components. 7.1 Generl Outline of the Computtionl Model The bsic ide of this design is similr to tht of the previous PID model nd other systems in generl. A trjectory genertor issues number of checkpoints long desired pth for the WMR to follow. The sensors detect the displcements t every smpling time which re then used to determine the coordintes nd heding t the present loction. Knowing the distnce between the current position nd the next checkpoint s well s the time llowed for the WMR to trvel to the next loction, its speed nd direction cn be clculted. The full model of the system is shown in Fig 7.1. For greter clrity, n enlrged version is plced in Appendix A. Fig. 7.1 Adptive Trcking Control System of the WMR 11

129 7. Assumptions nd Limittions of the Simultion Model The generl ssumptions nd limittions in the mthemticl model in Chpters 3 nd 6 lso pply here. Rw sensor dt cnnot be fully mimicked, so the required trnsltionl nd turning (yw) speeds re fed bck into subsystem contining motor nd vehicle prmeters in order to clculte the projected displcements. Sensor noise is not included in the simultion. Also, since this is n idel condition, there is no slippge involved. Thus, this is n idel scenrio tht tests the controller's performnce nd not those of the sensors. 7.3 Design Detils of the Simultion Model The simultion process of the dptive model is similr to tht of the PID system. The lgorithms tht describe theoreticl model re converted into computtionl code nd run over nd over gin to produce dt. This output dt is then compred with the predicted outcome to see how well they mtch up. There re few differences in the pproch here compred to the previous model. Initilly, the dptive model ws formulted s continuous system ccording to the theoreticl equtions. After it ws constructed nd simulted tests hd been run, it ws then converted into discrete model. As the ctul WMR is discrete system, this model is more relistic. When the discrete model hd been fully tested in simultion, it ws then modified such tht the simulted dt ws replced by n interfce for the sensors Trjectory Genertor The purpose of trjectory genertor is to discretise predetermined pth into series of points tht coincide with every smpling instnce. As the WMR is required to cover the distnce between points t ech smple, the element of time is thus introduced. As such, the creted pth becomes trjectory. The Trjectory Genertor subsystem s shown in Fig 7. is the only subsystem tht is tken directly from the PID model lmost unchnged. Further elbortion cn be found in Chpter 4. Additionl trjectories re included for the dptive model. The scope, grph nd mtrix-file-output blocks re present in the simultion version of the model. However, in order to conserve memory, they re removed from the finl model compiled for the WMR's microcontroller. 113

130 Fig. 7. Trjectory Genertor Subsystem 7.3. Inverse Kinemtics In order for the controller to determine the required speed nd direction to get to the next trjectory point, it requires informtion on the discrepncy between the current loction of the WMR nd the desired position t tht prticulr time. This is the purpose of the Inverse Kinemtics subsystem shown in Fig The theory behind the equtions used in the clcultions here re described in Chpter 3. In the PID system, the required displcements in the X- nd Y-xes re used to clculte how fst ech wheel would need to rotte. In the dptive model, those displcement vlues re used to clculte the trcking vribles, which re the longitudinl nd turning velocities. The determintion of wheel speed is not clculted in this subsystem but determined lter in the model. 114

131 Fig. 7.3 Inverse Kinemtics Subsystem 115

132 7.3.3 Gin Subsystem The Gin subsystem consists minly of strictly positive number tht is ssocited with the system's nturl frequency λ. In vrious combintions, this vlue mkes up the positive definite mtrices for the positionl nd velocity gins. The schemtic of the subsystem is illustrted in Fig Fig. 7.4 Gin Subsystem Inertil Mtrix The Inertil Mtrix subsystem s shown in Fig. 7.5 consists of the inertil terms of the dynmic equtions of the system s described by Eqs (6.35) nd (6.36). The coefficients of the inertil terms re linerly prmeterised ccording to Eqs (6.47) to (6.49) nd re constntly updted vi the dptive mechnism. Fig. 7.5 Inertil Mtrix Subsystem 116

133 7.3.5 Centrifugl Mtrix The Centrifugl Mtrix subsystem s shown in Fig. 7.6 contins both the centrifugl nd Coriolis terms of the system's dynmic equtions s described by Eqs (6.35) nd (6.36). Like the inertil mtrix, the coefficients of the both terms re lso linerly prmeterised ccording to Eqs (6.47) to (6.49) nd re updted continuously by the dptive lgorithm. Fig. 7.6 Centrifugl Mtrix Subsystem D-Inverse Mtrix The D mtrix s described by Eqs (6.35) nd (6.36) consists of nothing more thn the coefficients of the voltge inputs of the linerised system. The D- Inverse mtrix (or D -1 ) is just mthemtic mnipultion of the equtions for deriving the input vector s shown in Eq (6.37). The subsystem is illustrted in Fig Fig. 7.7 D-Inverse Mtrix Subsystem 117

134 7.3.7 Prmeter Updte One of the defining chrcteristics of dptive control is the updtble nture of its prmeters. These prmeters re tuned with the use of Lypunov stbility nlysis in response to the discrepncy between the ctul nd desired outcomes of the system. As illustrted in Eqs (6.47) to (6.49), the dynmic equtions of the system cn be linerly prmeterised nd rrnged in vector form. The resulting prmeters re dynmiclly tuned in the Prmeter Updte subsystem s shown in Fig The dptive gin settings re configured within symmetric positive definite mtrix s represented by the Constnt Digonl Mtrix block. Fig. 7.8 Prmeter Updte Subsystem Grouped Prmeter Initilistion At the strt of the progrm, the updtble control prmeters need to hve initil vlues. Ech prmeter is itself mde up of combintion of severl other system chrcteristics. Some of these properties cn be ccurtely mesured or clculted, while others my be pproximted. Together, they provide nominl or estimted figures tht re used for prmeter initilistion. These vlues do not hve to be exct since the dptive lgorithm 118

135 will correct them s the progrm is running. However, good estimtes help speed up the convergence process. All the initil prmeter vlues re grouped together s vector in single subsystem s shown in Fig Fig. 7.9 Grouped Prmeter Initilistion Subsystem Regressor Mtrix It hs been shown previously tht the dynmic equtions of the system cn be linerly prmeterised into regressor function nd prmeter vector s demonstrted in Eqs (6.47) to (6.49). The Regressor Mtrix subsystem is shown in Fig Fig Regressor Mtrix Subsystem 119

136 Symmetric Positive Definite Mtrix (spd_mt) Function The clcultion of symmetric positive definite constnt mtrix, P, is required in the derivtion of the Lypunov function cndidte s depicted in Eqs (6.55) to (6.63). This mtrix is determined with the help of MATLAB's lyp() function which is embedded in n S-Function block. The relevnt code is plced in Appendix A. Unfortuntely, the function cnnot be ported to the compiled progrm for the WMR's microcontroller. Therefore, the vlues generted by the simultion model re mnully entered into hrdwre model before compiltion. Despite the slight inconvenience, this pproch works becuse the numbers in this mtrix remin constnt throughout the progrm's execution System Dynmics The System Dynmics subsystem s shown in Fig 7.11 represents the non-liner model of the wheeled robot. In the simultion version, voltge inputs re used in the theoreticl derivtion of the liner nd yw ccelertions of the WMR. In turn, these ccelertions re used in the simultion of the displcements tht re expected from hypotheticl sensor. With the displcements known, the finl coordintes cn be determined. The Grouped Prmeters, Inerti Mtrix nd Centrifugl Mtrix subsystems re identicl to the similrlynmed ones described in preceding sections. In the version compiled for the WMR's microcontroller, ll the forementioned subsystems long with the simulted sensor re replced with subsystems tht interfce with ctul hrdwre components like the DC motors nd opticl mouse sensors. Fig System Dynmics Subsystem 10

137 Forwrd Kinemtics (Sensor) Just s in the PID system, the Forwrd Kinemtics subsystem in the dptive model represents simulted nlogue for the opticl mouse sensor. As it is impossible to simulte the displcement dt from n opticl sensor, forwrd kinemtics is used to clculte the odometry of the WMR. This simultion technique relies on ded reckoning, which is the sme method used by the WMR to process ctul sensor dt. Since the smpling period is very smll nd slippge is not fctor during simultion, this pproch is deemed vlid. In this subsystem s shown in Fig. 7.1, loclistion is ccomplished by pplying forwrd kinemtics to the lterl nd yw ccelertions generted by the simulted system dynmics. The results re then used to determine the displcement of the WMR with respect to the globl coordintes during the smpling period. 11

138 Fig. 7.1 Forwrd Kinemtics (Sensor) Subsystem 1

139 7.4 Design Detils of Deployed Model The deployed model s shown in Fig is the version of the progrm tht ctully interfces with hrdwre components such s the DC motors nd opticl mouse sensors. Its design is mostly similr to the simulted version with one key exception. As the progrm is ctully interfcing with rel hrdwre peripherls, it no longer needs to simulte sensor dt or the WMR's movement. Hence, the System Dynmics subsystem, which is used for simultion clcultions, is no longer needed. In its plce is the System Interfce subsystem tht contins blocks which communicte with the hrdwre components. Additionlly, slight chnge hd to be mde to the Prmeter Updte subsystem. Other thn tht, ll other subsystems from the simultion model re crried over to the deployed version unchnged. Fig Adptive Trcking Control System of the WMR (Deployed Model) Prmeter Updte (Deployed Model) It ws explined in the Subsection tht MATLAB's lyp() function could not be compiled for the WMR's microcontroller. Hence, slight modifiction hd to be mde to the Prmeter Updte subsystem s shown in Fig In plce of the function for clculting the necessry symmetric positive definite constnt mtrix, constnt block (spd) is introduced. The vlues clculted in the simultion model re mnully trnsferred into this constnt block. 13

140 Fig Prmeter Updte Subsystem (Deployed Model) 7.4. System Interfce The System Interfce subsystem s shown in Fig consists of interfces with the DC motors nd the opticl mouse sensors. Conceptully, the input signls of the subsystem re the voltge levels required to drive the motors t the desired speeds. However, the motors re driven by PWM (pulse-width modultion) signls nd not by direct voltge levels. Furthermore, the WMR's min microcontroller does not ctully generte the voltge needed to power the motors. Hence, the voltge levels re only represented s numericl mgnitudes in the signls processed by the microcontroller. These signls re converted into corresponding duty-cycle signls which re then trnsmitted to the motors. The interfce blocks for the DC Motors re prt of the Villnov University Lego Rel Time Trget (VU-LRT) librry tht is designed for use with the Lego Mindstorms NXT system nd Simulink. At the sme time s the control signls re being sent to the motors, the WMR is receiving dt from the opticl mouse sensors. The rw dt from the sensors re unpcked nd repcked into usble dt pckges by the Pck function before being sent to the Sensor Processing subsystem for further processing. The embedded code for the Pck subsystem is plced in Appendix A. 14

141 Fig System Interfce Subsystem Dul Opticl Sensors The Dul Opticl Sensors subsystem s depicted in Fig consists of borrowed S- Function tht forms the bsis of severl other sensor blocks within the VU-LRT librry. Here, it hs been dpted to interfce with both opticl mouse sensors. Fig Dul Opticl Sensors Subsystem Sensor Processing The Sensor Processing subsystem s shown in Fig is responsible for computing the sensor dt to determine the displcement of the WMR during the lst smpling period, nd its current position with respect to the globl coordintes. The relibility of the sensor dt is first checked ccording to the method described in Section 6 of Chpter 6. If the dt from both sensors re deemed cceptble, the Hybrid Geometric Loclistion subsystem is ctivted. Otherwise, the Kinemtic Loclistion subsystem is used insted. 15

142 Fig Sensor Processing Subsystem 16

143 Hybrid Geometric Loclistion There re two techniques used for odometric clcultions s discussed in Chpter 6. Of the two, the hybrid geometric method offers better ccurcy over the purely kinemtic pproch. Hence, the former method is employed for loclistion s nd when possible. However, it is not pplicble to single-sensor configurtion. Although the WMR hs two opticl sensors, this scenrio cn rise when either of the sensors report fulty redings. This leves only one set of relible dt nd would necessitte switch to the kinemtic loclistion method until the sensor redings re bck to norml. There re severl embedded functions in the Hybrid Geometric Loclistion subsystem s shown in Fig One of these functions sets the turning ngle to zero once the clculted vlue dips below the minimum detectble turning (yw) ngle bsed on sensor sensitivity. This would evoke the specil cse scenrio where the turning ngle is perfectly zero. The other embedded functions chnge the geometric clcultions to n Eulerin one when the criticl rdius hs been reched. The codes of these embedded functions re plced in Appendix A Kinemtic Loclistion The kinemtic odometry method is bsed on first-order Eulerin pproximtion technique nd cnnot mtch the ccurcy of the geometric pproch except when there is little or no turning. Hence, it would not be utilised unless it is necessry. This could hppen when one of the sensors is producing erroneous redings. In tht cse, this is the bck-up loclistion method tht would be used. The Kinemtic Loclistion subsystem is shown in Fig When this subsystem is ctivted due to unrelible redings being detected, the lgorithm will compre the reported SQUAL (surfce qulity) figures with clibrted vlues to see which of the sensors is most likely to be t fult. The sensor whose SQUAL redings fll within the cceptble rnge would hve its dt processed while the other's dt would be discrded. If both sensors report poor SQUAL redings, the outcome would be highly questionble. This is becuse the system requires t lest one set of relible dt from either sensor for odometric computtion. In this cse, dt would be red from Sensor 1 by defult. The lterntive is to terminte the progrm nd hlt the WMR. 17

144 Fig Hybrid Geometric Loclistion Subsystem 18

145 Fig Kinemtic Loclistion Subsystem 19

146 7.5 Summry Bsed on the theoreticl model proposed in the preceding chpter, computtionl representtion hs been constructed here. Included in the model re ll key components such s the dptive controller, hybrid odometric system, prtil-redundncy cpbilities, motors nd sensors. To be precise, there re ctully two versions of this new computtion model. The first version is designed to run in purely simulted environment. This will be test for the soundness nd stbility of the model. It lso llows the system's performnce to be esily tweked. Due to the nture of the simultion, the responses of ctul hrdwre components, like the motors nd sensors, hd to be simulted. Once the model's vlidity hd been confirmed, second version ws developed. This ltter version is ment to operte in rel-world scenrio. It mens the progrm is intended to be run on the CPU of the wheeled robot. As the motors nd sensors no longer need to be simulted, the computtionl model hd to be mended to replce the simulted components with hrdwre-interfce modules. This sets the stge for the construction of the prototype. 130

147 Chpter 8: Test Prototype 8.1 Hrdwre Pltform Theoreticl propositions cn often be vlidted by crrying out rel-world tests. Sometimes, observtion will suffice if the hypothesis involves nturl phenomenon. Other times, test prototype my hve to be built in order to verify the soundness of proposed ide. One of the objectives of this reserch is to develop n ccurte trcking system using lowcost components. Off-the-shelf prts re often (but not lwys) cheper thn custom-built devices. In this cse, the Lego Mindstorms NXT robotics system ws deemed suitble cndidte for the purpose of this study. It hs progrmmble embedded processor tht comes long with plenty of components (i.e. construction prts, motors nd sensors) tht cn be used to build fully-functioning test pltform. Furthermore, the stndrd Lego NXT kit is reltively inexpensive nd hs proven cpbilities in robotics for hobbyists. The opticl trcking sensors used for this reserch re typiclly found on computer mice. Unfortuntely, they re not vilble in form where they cn be plugged directly into the Lego robotics system without first undergoing some customistion. Also, since the opticl sensors nd the Lego NXT utilise very different communictions protocols, n dditionl microcontroller hs to be enlisted s n intermediry trnsltion device Lego Mindstorms NXT The Lego Mindstorms NXT 1.0 robotics system ws first unveiled in July 006. It ws relesed s successor pltform to the highly populr Mindstorms Robotic Invention System (RIS). With n upgrded processor, servo motors with much more torque nd wider rnge of vilble sensors, the NXT is much more dvnced pltform thn its predecessor. Fig. 8.1 shows the NXT system connected to mximum of three servo motors nd selection of vilble sensors. 131

148 Fig. 8.1 Lego Mindstorms NXT 1.0 robotics system (courtesy of Lego) Not long fter its introduction, Lego published the firmwre code with n open source licence. Also mde vilble to the public re the softwre developer kit (SDK), hrdwre developer kit (HDK) nd schemtics for ll the stndrd hrdwre components (Lego 006). Due to the open nture of the NXT system, it is now supported by well over dozen progrmming lnguges nd softwre development pltforms in ddition to the officil NXT-G grphicl progrmming environment. There re lso plenty of third-prty sensors vilble to complement those mde Lego. Thus, it is not surprising tht since its debut six yers go, the Lego Mindstorms NXT hs become widely embrced by the public, nd is even used s teching tool (Greg nd Pilt 008; Bobtsov et l. 011; Cuéllr nd Pegljr 011; Cruz-Mrtín et l. 01) for engineering courses in some universities. A minor revision (version.0) ws relesed in August 009 with upgrded softwre nd sensors. However, the min processor block remined unchnged. The physicl block tht contins the NXT's processing circuitry is clled the Intelligent Brick. Within the Brick, the min processor is n Atmel AT91SAM7S56 CPU bsed on the 3-bit ARM7 microrchitecture. The processor runs t 48 MHz, nd hs 56 kb of flsh memory nd 64 kb of RAM onbord. It does not hve floting-point unit (FPU) nd hs to rely on emultion to hndle floting-point numbers. For purposes tht require lot of floting-point opertions, the lck of n FPU could severely ffect the performnce of the processor. The ARM7 processor is widely deployed s n inexpensive nd low-power microcontroller for embedded systems. This clss of processor ws first introduced in 1996, so its processing cpbilities re nemic in comprison to the ltest genertion of microcontrollers - not to mention microprocessors used in computers. Nevertheless, these processors continue to be used in current devices nd hve proven cpbilities s demonstrted by the NXT. 13

149 The NXT is typiclly progrmmed to crry out simple to modertely complicted tsks. So, complex progrms tht lso require plenty of floting-point opertions my pose chllenge for the min processor. How well the NXT's processor fres will be reveled when it is finlly put to the test. In ddition to the min processor, there is lso n Atmel ATmeg48 co-processor bsed on the 8-bit AVR microrchitecture. The ATmeg48 runs t 8 MHz nd hs 4 kb of flsh memory nd 51 B or RAM. The min functions of the co-processor is to hndle power mngement, perform nlogue-to-digitl (A/D) conversion of input signls, nd crete pulse width modultion (PWM) signls for the motors. An illustrtion of the NXT system rchitecture is shown in Fig. 8.. Fig. 8. Lego Mindstorms NXT system rchitecture (Chikms 007) The NXT cn process multiple types of signls, such s PWM for the motors, digitl I C (Inter- Integrted Circuit) nd nlogue signls from the sensors, USB.0, nd Bluetooth. The wireless Blutetooth interfce is convenient mens for dt cquisition especilly during testing nd troubleshooting. However, it ws discovered during initil tests tht the mximum 9.6 kb/s trnsfer rte ws not only rther slow but ws further hmpered by significnt ltency. Thus, the 1 Mb/s USB.0 connection is preferred over Bluetooth despite the necessity of using cord. The NXT cn be powered by six AA rechrgeble NiMH or non-rechrgeble lkline btteries for nominl operting voltge of 7. V or 9 V. Alterntively, rechrgeble 7. V Li-ion bttery pck cn be used. The Li-ion bttery pck is the preferred power option of this reserch. 133

150 NXT Device Communictions The min ARM7 processor hs one hrdwre I C chnnel nd uses it for communicting with the AVR co-processor vi high-speed mode (380 kb/s). The design of the NXT system does not nticipte the dt trnsmission rtes of connected I C peripherls to come nywhere ner the high-speed rnge. To prevent slow device communictions from bogging down the only I C chnnel nd ffecting the trnsmission speed between the two processors, externl I C device connections re offloded onto different port. However, since the ARM7 processor hs only one ntive I C port, externl I C communictions hve to be implemented using softwre emultion on generl purpose input/output (I/O) port. This is lso known s "bit-bng" pproch. The fctory-defult mximum trnsmission rte of the low-speed I C port is 9.6 kb/s. This hs since been boosted to 15 kb/s by third-prty softwre developers (Shw 011). The four input ports on the NXT into which sensors or other devices re plugged cter for both nlogue nd digitl signls. Since the opticl mouse sensor opertes on digitl bsis, this report shll therefore explore only the digitl I C communiction interfce. I C is multi-mster single-ended seril bus first introduced by Philips Electronics in the 1980s. The division (Philips Semiconductors) responsible for the design of I C ws spun off in 006 into seprte compny clled NXP Semiconductors. NXP wived licensing fees for the protocol soon fter the compny's cretion, but continues to oversee the development of the technology. Some mnufcturers refer to I C s TWI (two-wire interfce). There re only two bi-directionl lines used for communictions in the I C protocol in ddition to power nd ground lines. One is for the seril clock, usully lbelled SCL, SCK or SCLK (DIGIAI0 on the NXT), nd the other is for seril dt, usully lbelled SDA or SDIO (DIGIAI1 on the NXT). A generlised illustrtion of n I C system is shown in Fig On typicl I C bus, every connected device is ssigned unique 7-bit bus ddress nd cn ssume either mster or slve mode. The design of the NXT mndtes tht no other externl device shring the I C bus cn ssume the role of the mster other thn the NXT's min processor itself. This ensures tht the NXT controls both the clock nd signl flow. 134

151 Fig. 8.3 I C bus interconnection (Atmel 010) In order to communicte with slve device on the I C bus, the NXT mster hs to initite series of commnds. For exmple, if the NXT is polling prticulr sensor for dt, the procedure would be s follows: 1. The NXT (mster) issues START commnd. If the trget sensor (slve) uses internl registers to store its dt, continue with Step 3, otherwise skip to Step 6 3. The NXT issues the 7-bit device (bus) ddress of the sensor plus 1-bit WRITE flg 4. The NXT sends the 8-bit internl register ddress where the dt is stored on the sensor 5. The NXT issues STOP commnd followed by repeted START commnd 6. The NXT issues the 7-bit device (bus) ddress of the sensor plus 1-bit READ flg 7. The sensor trnsmits s much dt s requested by the NXT for one polling cycle 8. The NXT issues STOP commnd when it hs received the required mount of dt 9. The procedure is repeted for every polling cycle Fig. 8.4 I C dt trnsmission (NXP 01) 135

152 Note tht both mster nd slve devices hve to issue n cknowledgement t every step of the forementioned procedure before the next ction cn tke plce. A grphicl representtion of typicl I C dt trnsmission for devices tht do not use internl registers is shown in Fig Both the min nd co-processors hve ntive SPI (Seril Peripherl Interfce) hndling cpbilities. However, the interfce is not enbled on the NXT nd no circuitry hs been provided for it. Thus, it is somewht of n inconvenience tht the defult communiction protocol of the opticl mouse sensor is SPI. The originl microcontroller tht ccompnies the opticl mouse sensor converts the signl from SPI to USB so tht it cn be used with modern computers. In this reserch, the sensor long with the lser diode nd cermic resontor re removed from the originl circuit bord nd redeployed with custom-mde circuitry tht is connected to different microcontroller. This SPI signl from the sensor is then trnslted into I C by the custom-progrmmed microcontroller so tht sensor cn communicte with the NXT NXT Direct Current Servo Motor The NXT servo motor shown in Fig. 8.5 is the ltest ddition to the Mindstorms fmily of PWM-driven DC motors. It is the first to sport n encoder nd hs one-degree resolution. This motor lso delivers fr more torque thn ny previous models in the product rnge. It powered by the output port of the NXT t 4.3V. Fig. 8.5 Lego Mindstorms NXT servo motor (courtesy of Lego) Lego hs not relesed the technicl specifictions for the motor. However, numerous reserchers nd hobbyists hve crried out their own testing nd relesed their results. The most extensive nlyses (Hurbin 007, b) hve been conducted by the co-uthor of prominent book on the Lego Mindstorms NXT (Gsperi nd Hurbin 009). Using the dt provided by Hurbin, Prof Ryo Wtnbe of Wsed University ws ble to clculte the motor chrcteristics (Wtnbe 007). Mny hve since used those prmeters in their 136

153 projects (Ymmoto 009, b) nd reserch. Some hve lso used Hurbin's dt to complement their own results (Mxim 011), while others hve conducted their own experiments (Dlsger et l. 006; Gonçlves et l. 009; Mrtinec nd Hurk 011) to figure out the prmeters of the NXT motor. There is quite bit of vrince in the results nd it is not known whether the discrepncy in figures is due to different testing methodologies or mnufcturing vribility. Tking the verge of the published results of the NXT motor's electromechnicl chrcteristics, the prmeters used in this reserch is summrised in the tble below: Motor Prmeter Unit Vlue Torque constnt, K t N.m/A 0.34 Bck-EMF constnt, K b V.s/rd Armture resistnce, R Ω 5.63 Armture inductnce, L H 4.7e-3 Viscous-friction coefficient, B m N.m.s 6. 00e-4 Rotor inerti, J m N.m.s or kg.m 1.31e-3 Tble 8.1 Lego Mindstorms NXT motor chrcteristics The vlues of torque constnt (K t ) nd bck-emf constnt (K b ) in SI units should be identicl in n idel sitution. However, due to unvoidble experimentl error, this is clerly not the cse. It is noted tht some reserchers only tested for the torque constnt nd presumed the bck-emf vlue to be of equl mgnitude. Ultimtely, the ccurcy of these figures my not be of gret importnce due to the dptive controller's bility to updte the vlue of the WMR's prmeters JED Microprocessors AVR00 Single Bord Computer The JED AVR00 is single-bord microcontroller with n Atmel ATmeg3 processor t its hert. The bord essentilly hosts ll the circuitry required to support the functioning of the microcontroller, such s providing power connections, clock genertor, input/output (I/O) terminls, dditionl memory, etc. The AVR00 cn be considered self-contined processing system sns power supply. A picture of the bord nd digrm of its component lyout re shown in Figs. 8.6 nd 8.7 respectively. The full circuit schemtics of the bord re locted in Appendix C. 137

154 Fig. 8.6 JED AVR00 Single Bord Computer (JED 008) Fig. 8.7 JED AVR00 Single Bord Computer PCB lyout (JED 008) 138

155 The ATmeg3 microcontroller opertes t 5 V while the AVR00 controller bord ccepts DC inputs between 5.5 V nd 18 V (Schoell 004). It is tolernt of trnsient voltge spikes rnging from -50 V to 60V. On the NXT, there re three different voltge output levels vilble t ech port, nmely, 3.3 V, 4.3 V nd 9 V (7. V if using rechrgeble btteries). There were initil ttempts to use 9 V output from n unused NXT port to power the AVR00 bord together with the two opticl mouse sensors lbeit indirectly. The sensors would be powered through regulted 5 V output terminl on the AVR00 bord, but the voltge would hve to be stepped down further to 3.3 V for the sensors. However, regulr signl drop-outs suggested tht the supply current ws unstble t the required level or simply indequte. Thus, it ws decided tht the AVR00 bord nd two sensors should employ seprte power supplies. After switching the power source for the microcontroller bord nd opticl sensors, the use of n enclosure for six AA NiMH btteries ws initilly plnned. However, it ws discovered tht the NiMH btteries could sustin n effective voltge of 1.45 V under lod for good length of time, even though their nominl voltge is rted t 1. V. This mens tht the only four btteries would be needed. Indeed, this hs been verified during extended testing JED AVR00 Bord Modifictions In order for the AVR00 bord to work with the NXT, some minor ltertions hd to be mde. Firstly, the AVR00 hs buffered I C bus vi the use of Philips 8B715 I C bus extender chip (JED 005). The NXT does not hve similr chip within its I C circuitry, so 8B715 chip hd to be desoldered nd removed. Secondly, the NXT hs no internlly-mounted pull-up resistors. Lego recommends the use of 8 kω resistors on both the clock nd dt lines of n externl device (Lego 006). On the AVR00, the clock nd dt lines ech hs 330 Ω nd 1.5 kω resistor connected to Vcc (JED 005). They hd to be removed nd replced with single 8 kω resistor on ech line Atmel AVR ATmeg3 Microcontroller The single most importnt component on controller bord is the microcontroller itself. Without it, the bord hs no reson to exist. On the AVR00 bord sits the Atmel ATmeg3 microcontroller. It is n 8-bit processor within the AVR microrchitecture fmily (Atmel 011b). This prticulr microcontroller is regulted by n externl crystl oscilltor to operte t MHz, nd hs 3 kb of progrmmble flsh memory, 1 MB of EEPROM (electricllyersble progrmmble red-only memory) nd kb of SRAM (sttic rndom ccess 139

156 memory). It hs ntive support for ll the communiction protocols required for the testing nd opertion of the prototype vehicle, such s SPI, I C (clled TWI by Atmel) nd USART (Universl Synchronous/Asynchronous Receiver/Trnsmitter). The ATmeg3 opertes t 5 V. However, both the NXT's AT91SAM7S56 processor nd the ADNS-6010 sensors operte t 3.3 V. The AT91SAM7S56 is tolernt of 5 V I/O signls (Atmel 011), so it cn hve direct signl lines with the ATmeg3. However, the sensors re not 5V-tolernt, so the 5 V signls from the ATmeg3 to ADNS-6010 hve to be stepped down. The only signl tht goes from the ADNS-6010 to ATmeg3 is crried by the MISO line. The ADNS-6010 outputs high signl within the rnge of.64 V to 3.3 V, while the ATmeg3 registers minimum of 3 V s high signl. It is cler tht the ATmeg3 cnnot detect the entire high signl rnge of the ADNS Therefore, logic level shifter/converter ws used to bring the 3.3 V signls to 5 V level. There re four min communiction ports on the ATmeg3, i.e. Ports A to D. In ddition to hndling generl I/O functions, most port pins lso hve lterntive uses. For exmple, Port B serves n lterntive role s n SPI port, Port C doubles s n I C (TWI) port, nd Port D lso functions s USART port. The opticl mouse sensors used in the prototype hs ntive SPI connection while the NXT uses I C insted. In order for the NXT nd the sensors to communicte with one nother, the signls hve to be converted from I C to SPI. This is the purpose of the ATmeg3 microcontroller Seril Peripherl Interfce (SPI) on the ATmeg3 in Mster Mode The Seril Peripherl Interfce is synchronous seril dt communiction protocol. It opertes in full duplex, which mens dt cn flow in both directions concurrently, unlike the hlf-duplex I C. Thus, it is generlly much fster interfce thn I C. As in I C, devices ssume mster nd slve modes in SPI. However, no more thn two devices cn communicte with ech other t ny given time. Shown in Fig. 8.8 re timing digrms of SPI trnsmissions reflecting the effects of clock phse polrity. 140

157 Fig. 8.8 SPI trnsmission with vrious clock phse nd polrity settings (Atmel 011b) The ATmeg3 cts s mster device (Atmel 008) when communicting with the opticl sensors. It sets the resolution of the sensors nd lso determines how often they re polled for dt. Aside from power nd ground lines, the four min signl lines on the SPI bus re: 1. Seril clock (SCK, SCLK or CLK). Mster input, slve output (MISO) 3. Mster output, slve input (MOSI) 4. Slve (or chip) select (SS, CS or NCS) On the ATmeg3, the forementioned logic signls re ssigned to Pins 7, 6, 5 nd 4 respectively on Port B. The min SPI configurtions re found on the SPI Control Register (SPCR) (Atmel 011b). The following is brief description of their functions nd respective settings on the ATmeg3. Other settings re described in the microcontroller progrm itself. 141

158 1. Bit 7 - SPIE (SPI Interrupt Enble): Set to high for interrupt to be executed when seril trnsfer is completed. [Remins 0 by defult]. Bit 6 - SPE (SPI Enble): Set to high to enble SPI. [Set to 1] 3. Bit 5 - DORD (Dt Order): Set to high to send LSB first nd low to send MSB first. [Remins 0 by defult] 4. Bit 4 - MSTR (Mster/Slve Select): Set to high to configure AVR s mster nd low to configure it s slve. [Set to 1] 5. Bit 3 - CPOL (Clock Polrity): If set to high, SCK is high when idle; if set to low, SCK is low when idle. [Set to 1] 6. Bit - CPHA (Clock Phse): Set to high to smple dt on triling edge nd low to smple dt on leding edge. [Set to 1] 7. Bits 1 nd 0 - SPR1 nd SPR0 (SPI Clock Rte Select): Set frequency of clock signl. [SPR0 remins 0 nd SPR0 set to 1] The settings for the clock polrity nd phse re determined by opertionl chrcteristics of the opticl mouse sensor (Avgo 009b). The SPI clock rte governs how fst the opticl sensor is polled nd is lso configured to mtch the sensor's dt updte speed. The full progrm tht is loded on the microcontroller is found in Appendix B Mnging Two SPI Devices on Single ATmeg3 SPI Port When the NXT requests for dt from the opticl sensors, it expects the dt to be trnsmitted from both sensors t the sme time. However, there is only one ntive SPI chnnel on the ATmeg3. So, it cn only communicte with one SPI sensor t time. This presents significnt problem. It is possible to emulte the SPI function on nother port. However, the emulted SPI would be fr too slow to keep up with the hrdwre-bsed SPI. Other lterntives would be to cquire n dditionl identicl microcontroller bord or use different type of microcontroller tht hs two ntive SPI ports, such s those in the Atmel AVR XMEGA fmily. Ultimtely, solution to this problem ws found tht did not require ny dditionl components. Both sensors would be connected to the sme physicl terminls of the SPI port (Port B) on the controller bord except for the slve-select (SS) line. The SS wire of the first sensor would remin connected to the SS terminl of the SPI port. For the second sensor, the SS line would be routed to n unused pin on the sme port nd would be ctivted seprtely. Bsiclly, the ide is to poll both sensors with minuscule dely in between such tht it would not hve n dverse effect on the overll outcome. 14

159 When the sensors re polled for dt, the SS line of the first sensor would be ctivted in order for the microcontroller to communicte with it nd request for dt. After the first sensor hs been successfully polled, its SS line is set to low nd dt trnsmission will end. The procedure is then repeted for the second sensor. When switching between sensors, there is nturlly dely. According to the hrdwre specifictions of the sensors, there is 10 ns dely between SS being set high nd the SCK becoming ctive on sensor, i.e. the sensor is redy for dt trnsmission. The SPI polling rte is set by the microcontroller t 1/16th of the oscilltor frequency which equtes to 03.4 khz. This mens its smpling period is 4.34 μs. It is evident tht the switching dely is extremely short compred to the smpling period. Effectively, it would pper to be lmost instntneous. Furthermore, ech sensor hs dt buffer tht cn store t lest 16 dt smples, so ny slight dely or synchronistion issue would not hve ny noticeble effect on the dt polling process Inter-Integrted Circuit (I C) Bus on the ATmeg3 in Slve Mode The Inter-Integrted Circuit bus is lso clled the Two-Wire Interfce (TWI) by Atmel, the mnufcturer of the ATmeg3 microcontroller, s well s by mny other chip mkers. For this prototype, the ATmeg3 receives dt from the opticl sensors vi SPI, then repckges nd trnsfers the sme dt to the NXT vi I C. While the ATmeg3 cts s n SPI mster to the opticl sensors, it functions s n I C slve (Atmel 009) to the NXT. Thus, the NXT controls the dt trnsfer rte between itself nd the ATmeg3. Detils of its workings on the NXT re described in Section The I C portion of the progrm is interrupt-driven s opposed to the SPI section which is pollbsed. Although the NXT softwre specifies the use of n internl register ddress for ccessing dt, the ATmeg3 is progrmmed to ignore the commnd nd send dt from its defult dt register. Regrding the min I C configurtions, they re found on the TWI Control Register (TWCR) (Atmel 011b). The following is brief description of their functions nd respective settings on the ATmeg3. Other settings re described in the microcontroller progrm s well s supplementry source nd heder files. The microcontroller is ssigned device/bus ddress of 0x Bit 6 - TWEA (TWI Enble Acknowledge Bit): Set to high to enble ACK pulse when slve ddress or dt is received. [Set to 1]. Bit 5 - TWSTA (TWI START Condition Bit): Set to high for device to become mster. [Remins 0 by defult] 143

160 3. Bit 4 - TWSTO (TWI STOP Condition Bit): Set to high to generte STOP condition in mster or for error recovery in slve. [Remins 0 by defult] 4. Bit - TWEN T(WI Enble Bit): Set to high to enble TWI opertion nd ctivte TWI interfce. [Set to 1] 5. Bit 0 - TWIE (TWI Interrupt Enble): Set to high to enble interrupt request. [Set to 1] The mximum I C trnsfer rte is limited to 9.6 kb/s (9.6 khz) by the originl operting system (OS) tht cme with the system. A few third-prty softwre pltforms for the NXT hve been ble to increse the I C speed to 15 kb/s. The prototype uses n open-source third-prty OS tht employs custom-designed driver softwre to mnge the I C communictions. Unfortuntely, the drivers used by the open source OS is n older version tht is still restricted to 9.6 kb/s. Newer versions of the driver provide huge performnce boost, but they re incomptible with the OS currently used by the prototype. Since the dt smpling rte of the opticl sensors fr exceeds the mximum trnsfer speed of the NXT, dt loss is rel risk. Indeed, t 03.4 khz, the sensors smple t more thn 1 times fster thn the NXT is ble to receive dt. The problem is further compounded if the control progrm of the NXT smples t n even lower rte thn is possible. A solution for this problem hd to be found. A study of the how dt is stored nd trnsmitted must first be conducted. An 8-bit vrible cn hold ny number between 0 nd 55 for totl of 56 possible vlues. A 16-bit (two-byte) vrible cn store numbers rnging from 0 to for totl of possible vlues. This mens tht 16-bit vrible could hold minimum ccumultion of 56 8-bit numbers. Nturlly, the 16-bit vrible cn store more tht 56 8-bit numbers if the individul 8-bit vlues re smller thn the mximum of 55. Bsed on how the dt trnsfer process works, 16-bit number cn be sent in two 8-bit chunks. So, lthough 16-bit vrible cn hold t lest 56 8-bit numbers dded together, it only requires twice the length of time to trnsmit compred to n 8-bit vrible. Using this method, the rte of dt trnsfer cn be drsticlly reduced. Ech pcket of sensor dt corresponding to motion in the longitudinl nd lterl directions is eight bits (i.e. one byte) long respectively. By creting 16-bit buffer on the microcontroller for ech set of coordinte dt from the sensors, the vstly different dt trnsfer rtes between the sensors nd NXT re no long n issue. Through extensive troubleshooting, it ws discovered tht there ws flw in the wy tht the I C driver communicted with the NXT. Dt loss often occurred when ny of the dt bytes in trnsmission held vlue tht exceeded 17. Tht number lso hppens to represent the mximum vlue tht cn be stored in seven bits. So, if the dt bytes were insted cut up into 7-bit chunks (with null eighth bit tcked on) before trnsmission, no dt loss would occur. 144

161 However, this would men tht the 16-bit buffer vrible would hve to be trnsmitted in three prts. In the end, it ws unnecessry to trnsmit the buffer vrible in three portions. During testing, the high smpling rte of the sensors nd reltively slow speed of the prototype ment tht vlue of ech individul dt pcket ws ctully very smll. The vlue held in the buffer never cme close to exceeding 9-bit mximum before being trnsmitted. So, the lst two bits of the 16-bit buffer could be sfely ignored nd not trnsmitted. The code for the I C functionlity of the microcontroller is rrnged seprtely from the min progrm nd is locted in Appendix B USART on the ATmeg3 The Universl Synchronous/Asynchronous Receiver/Trnsmitter (USART) is communictions protocol tht is used for trnsmitting dt from the microcontroller to the computer during testing nd troubleshooting phses. Although it is not used in the ctul opertion of the system, it plyed vitl role in getting the prototype up nd running. The hrdwre connector for the USART port on the AVR00 bord is of type RS-3/DE-9. During the initil testing of the sensor nd microcontroller, the involvement of the NXT ws unnecessry. The first step of progrmming the microcontroller is to set up the SPI connection so tht it cn communicte with the sensors. To check whether the microcontroller hs successfully cquired dt from the sensors, the dt on the microcontroller is trnsmitted vi seril cble to computer for nlysis. This dt is trnsmitted through the USART port on the microcontroller nd displyed on terminl emultor progrm on computer. The USART code hs been left intct in the min microcontroller progrm found in Appendix B nd cn be used for troubleshooting purposes when needed Avgo ADNS-6010 LserStrem Lser Mouse Sensor The Avgo (formerly Agilent) ADNS-6010 ws considered high-performnce sensor when it ws introduced in 005. Since then, opticl mouse sensor technology hs progressed significntly nd delivery of the ADNS-6010 cesed in Jnury 01. The ADNS-6010 sensor is ble to detect motion up to speed of 45 in/s (1.14 m/s) nd n ccelertion of 0 g (Avgo 009b). It cn process over 7080 frmes (surfce imges) per second to provide high precision trcking. In ddition, it hs selectble resolution/sensitivity of 400, 800, 1600 nd 145

162 000 counts/in (cpi) - equivlent respectively to 15.7, 31.5, 63.0, 78.7 counts/mm. The sensors used in the reserch were crefully desoldered from two brnd-new mice. A picture of the sensor chip is shown in Fig Fig. 8.9 Avgo ADNS-6010 opticl mouse sensor (Avgo 009b) The opertion of typicl opticl mouse sensor is bsed on the principle of opticl flow. The ADNS-6010 consists of two min components: the imge cquisition system (IAS) nd digitl signl processor (DSP). The IAS comprises of 716 μw lser diode, opticl lens nd imge sensor. Essentilly, the lser diode illumintes the trcking surfce nd the resulting imge is reflected through the lens nd cptured by the 30-by-30-pixel imge sensor. A continuous series of surfce imges (frmes) re then processed by the DSP in order determine the displcements in the longitudinl nd lterl directions with respect to the sensor. A crosssection digrm of the sensor, lens nd lser diode is illustrtion in Fig Fig Avgo ADNS-6010 cross-section of sensor ssembly (Avgo 009b) The ADNS-6010 sensor opertes t 3.3 V level, so it hs to use voltge regultor to reduce the 5 V supply from the AVR00 bord. For resons unknown, the resolution of the sensors could not be set beyond 800 cpi. In ny cse, this is very high level of resolution, so it is not concern. The sensors ssume the role of SPI slve devices while the ATmeg3 microcontroller cts s the mster. This mens tht the sensors' settings s well s their smpling rte re ll dictted by the microcontroller. 146

163 Write nd Red Opertions on the ADNS-6010 Sensor The ADNS-6010 sensor is the slve SPI device while the ATmeg3 microcontroller tkes on the role of the mster. Thus, ll red nd write opertions cn only be initited by the ATmeg3. A write opertion is chrcterised by trnsmission of dt from the microcontroller to the sensor vi the MOSI line. The first byte in every trnsmission sequence comprises of 7-bit ddress plus n MSB (most significnt bit) of "1". Dt is contined in subsequent bytes. Dt bits crried on the MOSI signl re smpled on rising edges of SCLK by the sensor. The timing digrm is illustrted in Fig Fig Timing digrm for dt smpling during write opertions (Avgo 009b) A red opertion is chrcterised by trnsmission of dt from the sensor to the microcontroller vi the MISO line. The first byte in every trnsmission sequence comprises of 7-bit ddress plus n MSB (most significnt bit) of "0". Dt is contined in subsequent bytes. The sensor smples the MISO signl on rising edges of SCLK nd outputs dt bits on flling edges of SCLK. The timing digrm is illustrted in Fig Fig. 8.1 Timing digrm for dt smpling during red opertions (Avgo 009b) 147

164 The timings of the dt smpling for both write nd red opertions, s shown in Figs nd 8.1, re used to configure the SPI clock phse nd polrity on the ATmeg3 microcontroller. In ddition to the timings of the smpling process, the timings of write nd red opertions s well s ny delys in between hve to be fully tken into ccount when writing the microcontroller's progrm. These timings re shown in Fig The delys inserted into the progrm reflect this considertion. Fig Timing digrm for write nd red opertions nd delys (Avgo 009b) For the successful opertion of the sensor, not every register hs to be ccessed. The following re ddresses of 8-bit registers tht hve configurtion bits which need to be set. This is ccomplished by issuing relevnt commnds from the microcontroller to the sensor. 1. 0x0 - Configurtion_bits: Sets resolution, lser mode, etc.. 0xc - LP_CFG0: Sets reltive lser current level 3. 0xd - LP_CFG0: Complements 0xC to set reltive lser current level For write nd red opertions in SPI, the following re ddresses of 8-bit registers tht re required to be ccessed. 148

165 1. 0x0 - Motion: Reports ny motion since lst poll nd ny buffer overflows. 0x03 - Delt_X: Displcement in locl X-xis (lterl xis of sensor) in counts 3. 0x04 - Delt_Y: Displcement in locl Y-xis (longitudinl xis of sensor) in counts 4. 0x05 - SQUAL: Qulity of surfce imge cptured by sensor bsed on number of fetures detected Sensitivity to Chnges Between Sensor Assembly nd Surfce The distnce between the sensor ssembly nd trcking surfce is crucil fctor in trcking ccurcy. So, it is importnt to mintin this distnce within opertionl limits. Nturlly, sensor with greter opertionl rnge is better ble to ccommodte uneven surfces, nd thus less likely to lose trcking ccurcy. One of the improvements of the ADNS-6010 over sensors from n erlier genertion is its greter opertionl rnge of distnce between the sensor ssembly nd trcking surfce. A good exmple is the populr ADNS-051 model which predted the ADNS-6010 by bout three yers, nd ws contemporneous model with the ltter in mnufcturer's product rnge for severl yers. Not only ws the ADNS-051 deployed in wide rry of retil mice, it hs lso been used in quite few reserch endevours s well s projects published on the World Wide Web. As is evident from their respective performnce chrcteristics shown in Figs nd 8.15, the ADNS-6010 is significntly more tolernt to chnge in height of the gp between the sensor ssembly nd trcking surfce thn the ADNS-051. The ADNS-6010 hs recommended operting region of.4 ± 0.3 mm nd mximum effective rnge extending to ± 0.8 mm. The ADNS-051 hs similr recommended operting region to the ADNS However, its mximum effective rnge only extends to brely ± 0.4 mm before the onset of mrked deteriortion of ccurcy. This is one of the key fctors influencing the selection of the ADNS for this reserch. 149

166 Fig Recommended sensor height for ADNS-051 (Avgo 009) Fig Recommended sensor height for ADNS-6010 (Avgo 009b) Construction of the ADNS-6010 Dul-Sensor Subsystem The circuitry of the sensor is bsed on the documenttion provided by the mnufcturer. However, ltertions hd to be mde becuse of the use of different type of microcontroller. Also, extrneous components relted to the buttons, scroll wheel nd USB port were eliminted. 150

167 The sensors operte t 3.3 V but receive 5 V power from the AVR00 bord. Thus, voltge regultor hd to be used to reduce the voltge to suitble level. Similrly, the 5 V signls from the ATmeg3 microcontroller hd to be reduced. This ws ccomplished by the use of resistors nd the principle of voltge division. For signls going from the ADNS-6010 to ATmeg3, logic level shifter/converter ws used on the MISO line to step up the 3.3 V signl to 5 V level. The level shifter unit lso cme with voltge-divider circuitry to step down 5 V signls to 3.3 V levels. However, the resistors used did not hve n optimum rtio nd hlved the 5 V level insted. Thus, the voltge-divider circuitry provided by the level shifter bord ws not used. According to the schemtics of the AVR00 bord, ech of the four signl lines of the SPI port hs 4.7 kω resistor connected in series. The principle of voltge division sttes tht: V R (8.1) out V in R1 R Thus, the required resistnce to step 5 V down to 3.3 V is: R1 R V (8.) in 1 V R out R 9. 1 k 3 Ech sensor nd ll its supporting components nd wiring re then mounted on blnk printed circuit bord (PCB). The two sensor circuit bords re in turn mounted on polycrbonte plte tht hs pir of precision-cut holes for the lens ssembly of the sensors. The resulting circuit digrm for the sensor is illustrted in Fig The fully-ssembled sensor circuitry nd mounting plte is shown in Figs nd

168 Fig Circuit schemtic of ADNS-6010 opticl mouse sensor subsystem Fig Two opticl sensors mounted on polycrbonte plte 15

169 Fig Underside of sensor mounting plte Clibrtion of the ADNS-6010 Sensors Before n opticl mouse sensor cn be used for trcking, it hs to be clibrted first. The displcement dt reported by the sensor is mesured in counts. Without known correltion with ctul mesurement units, the dt is quite useless. The selected resolution of 800 counts-per-inch (cpi) cnnot be relied upon becuse it is only nominl figure. The first step is to ffix the sensors long with their mounting plte to the prototype vehicle. The gp between the sensor plte nd trcking surfce ws mesured to be between mm nd 3 mm. There ws not prcticl method vilble to mesure the distnce to precision of 0.1 mm. However, ccording to the hrdwre specifictions s shown in Fig 8.15, there is no problem if the gp is within the rnge of mm to 3 mm. This mesurement ws determined by the use of smll steel pltes of mm nd 3 mm thickness. The thinner plte ws ble to just slide through the gp, but thicker one ws not. As the vehicle's motors re gered, it is quite difficult to push it smoothly long test surfce. It is much esier to slide the vehicle long if there is thin film under the tyres. A 0.13 mm plstic trnsprency ws deemed to be suitble mteril becuse it is very thin nd lso durble enough withstnd the sliding motion for some time. To hold the vehicle in plce while 153

170 it sits on the trnsprency, wooden frme ws constructed with steel corner brces, nd ligned with the id of squre ruler. The trnsprency ws stpled cross the frme. Holes were cut out in lignment with the sensors. A test surfce ws found tht hd stright steel guide nd distnce of m ws mrked out on the guide. For the clibrtion test, the frme with the vehicle firmly held within ws slid long the guide from strting to ending mrkers. This process ws repeted 10 times. When the clibrtion runs were conducted, dt hd to be collected. The rw dt from the sensors ws first uploded to the NXT nd re-trnsmitted to lptop nd recorded. The distnce for ech run ws divided by the totl number of reported counts. The resulting number represents the sensor's sensitivity nd hs unit of m/count, or lterntively mm/count. The dt ws verged over the 10 runs. After clibrtion, when the sensor reports displcement of certin number of counts, the figure cn be multiplied by the sensitivity fctor to give the ctul distnce trvelled in metres or millimetres. The clibrtion results re shown in Tbles 8. to 8.4. Idelly, the redings in the lterl xis (x-xis) of the sensor should be zero since there is no motion in tht direction. However, it is impossible to chieve perfect lignment. Thus, smll mount of error is present, which hs to be fctored into the clcultions. The results indicte tht the resolutions for the two sensors re pproximtely 884 cpi nd 834 cpi. Prt of the clibrtion process includes survey of the surfce texture s seen by the sensors. This is clled "surfce qulity" or SQUAL by the mnufcturer. Also, comprison of displcement redings from the two sensors ws conducted to see how well they gree with ech other. Sensor 1 X (count) Sensor 1 Y (count) Sensor X (count) Sensor Y (count) Averge Tble 8. Sensor clibrtion dt 154

171 Sensor 1 Sensor Sensor sensitivity (m/count).87180e E-05 Sensor sensitivity (mm/count).87180e E-0 Sensor offset ngle (rd) E E-04 Sensor offset ngle (deg) Tble 8.3 Clibrted sensitivity of both sensors Sensor 1 Sensor Men (x1 - x) σ (x1 - x) (using 3σ [m] SQUAL SQUAL [m] zero-djusted men) [m] e e e e e e e e e e e e e e e e e e e-04.99e e e e e e-04.63e e e e e-04 Averge e e e-04 σ σ Tble 8.4 Dt correltion nd surfce qulity of test surfce Construction of Vehiculr Prototype The vehiculr prototype ws differentilly-driven wheeled robot with single front cstor wheel. Other thn the microcontroller bord nd sensor subsystem, the chssis of the vehicle ws entirely composed of prts from the Lego Mindstorms NXT 1.0 kit. The verstility of the NXT system ment tht no custom mnufcturing ws required to build the vehicle chssis. Furthermore, the NXT kit ws reltively inexpensive compred to most other commercillyvilble robotic pltforms. This trnslted into svings in time nd cost. An dditionl set 155

172 ws purchsed t the sme time in cse spre prts were needed. Six pieces from the second set were included in the current vehicle. The overll dimensions of the vehicle were prtly limited by the size of the individul prts. A lrger vehicle my require more joints nd result in structurlly weker frme. If the vehicle were too smll, it my not be ble to contin the microcontroller bord nd sensor subsystem. While the microcontroller bord ws of fixed size, the sensor subsystem nd vehicle chssis were not. So, there ws some flexibility in deciding on their finl dimensions. The custom construction of the sensor subsystem nd vehicle chssis lso ment tht they hd to be designed somewht concurrently nd with the other in mind. The design of the vehicle ws quite strightforwrd. It hd to conform to the design of vehicle with differentilly-driven rer wheels nd lone free spinning front cstor wheel. Also, it hd to be ble to hold the NXT brick s well s dditionl components such s the microcontroller bord nd sensor subsystem. A finl design ws produced by dhering to these simple requirements. Firstly, the two servo motors tht drove the rer wheels were integrted s prt of the structurl frme. The gp between the two motors formed chnnel tht would fit the sensor subsystem. As the frme ws built, spce ws llocted for the microcontroller bord nd lso the NXT brick. As the chssis consisted mostly of bems (clled lift rms by Lego) tht were held together by connector pins, there ws quite bit of flexibility in the frme. Thus, in order to stiffen the frme, numerous digonl pieces were dded for brcing purposes. The complete prototype is shown in Fig The vehicle in vrious stges of ssembly is shown in Figs. 8.0 to 8.5. The finl dimensions of the vehicle re displyed in Tble 8.5. To clculte the mss moment of inerti, the vehicle ws pproximted to rectngulr box with the rottionl xis situted t the rer end Mesurement of Sensor Position on Trcking Surfce The designted sensor tht defined the WMR's position ws the front sensor, so it ws only necessry to mrk the position of this sensor nd not the one in the rer. However, since the ground clernce between sensors nd trcking surfce ws tiny, it ws quite impossible to mrk the position of the sensor directly on the ground. The solution ws to form lines tht intersect t the loction of sensor on the vehicle nd extend them to the periphery. The end points of these intersecting lines would be clerly mrked on the vehicle s shown in Figs. 8.0, 8.1, 8. nd 8.5. A verticl piece t the fore of the vehicle s well s outriggers on 156

173 both sides were dded solely for this purpose. The ground clernce of outriggers ws well under 1 mm nd could brely fit piece of pper in between. In order to situte the vehicle t strting loction with certin orienttion, two lines tht intersect the strting point were first drwn on the ground nd extended just beyond the mximum dimensions of the vehicle. The vehicle ws then moved into plce by ligning its mrkings with these lines. Similrly, in order to determine the ending position, dots were drwn on the ground where the vehicle mrkings were locted. The vehicle ws then removed nd intersecting lines were drwn by connecting the dots. The intersecting point would be the position of the ending point s identified by the front sensor. Overll length (including rer wheels) [m] 0.6 Overll width (including outriggers) [m] 0.18 Height (including NXT brick) [m] Weight [kg] Trck width [m] 0.16 Rdius of tyre [m] 0.08 Distnce between wheel bseline nd first (rer) sensor [m] Distnce between wheel bseline nd second (front) sensor [m] Centre of grvity (longitudinl blnce point) from wheel bseline [m] 0.10 Approximte mss moment of inerti [kg.m ] 0.0 Tble 8.5 Dimensions of wheeled robot 157

174 Fig Perspective view of fully-ssembled vehicle Fig. 8.0 Front view of fully-ssembled vehicle 158

175 Fig. 8.1 Rer view of fully-ssembled vehicle Fig. 8. Side view of fully-ssembled vehicle 159

176 Fig. 8.3 NXT brick removed to show bttery comprtment Fig. 8.4 Sensor subsystem exposed 160

177 Fig. 8.5 Underside of vehicle 8. Softwre Pltform The min progrmming environments were MATLAB nd Simulink with the inclusion of Rel- Time Workshop Embedded Coder. The PID model ws developed using MATLAB 007 while the dptive model ws formulted using version 010b. The Simulink blocks for interfcing with the Lego Mindstorms NXT re included in n open-source dd-on module clled the Villnov University Lego Rel Time Trget (VU-LRT) librry blockset s shown in Fig It provides wy for the progrm to interct with the motors nd sensors. The opticl sensor block in the progrm ws ctully customised block with dpted code borrowed from the I C block of the VU-LRT blockset. The VU-LRT blockset is derived from the Embedded Coder Robot NXT librry blockset originlly developed by Tkshi Chikms. Prt of the Embedded Coder Robot NXT librry blockset is included in Simulink since version

178 Fig. 8.6 VU-LRT librry blockset for Simulink For the NXT brick, the originl firmwre ws initilly replced by modified version clled the Enhnced NXT firmwre. It is developed nd mintined by John Hnsen. This firmwre dds fetures to the originl nd lso llows both stndrd nd non-stndrd Lego progrms to run on the NXT. In order for Simulink progrms to run on the NXT, they must be compiled for the nxtosek pltform. As the progrm size becme quite lrge in ltter stges of the reserch, the Enhnced NXT firmwre ws replced by the nxtosek NXT BIOS. This mode llowed the usge of flsh memory nd SRAM to be mximised. This ment tht lrger progrms were llowed, nd more vribles could be stored in the RAM. However, only one progrm could be stored in the flsh memory t ny time. The nxtosek pltform is n open-source rel-time operting system (RTOS) tht consists of lejos device drivers, TOPPERS/ATK (formerly known s TOPPERS/OSEK), nd TOPPERS/JSP. lejos is n lterntive open-source firmwre tht includes Jv virtul mchine which llows the NXT to be progrmmed in the Jv progrmming lnguge. 16

179 TOPPERS/ATK is n open-source kernel with rel-time multi-tsking cpbilities tht is used in the utomotive industry. TOPPERS/JSP is rel-time kernel tht conforms to µitron4.0 (Jpnese industry stndrd) specifictions. nxtosek is developed nd mintined by Tkshi Chikms. In order for Simulink to compile progrm for the nxtosek pltform, it must utilise dditionl tools such s the GNU ARM toolchin which includes GCC (GNU C Compiler), nd Cygwin, which provides Unix-like environment within the Windows operting system. Both tools re open source. The ATmeg3 microcontroller on the AVR00 bord ws progrmmed using the Atmel AVR Studio 4 integrted development environment. This is proprietry progrm vilble from Atmel t no cost. The required dditionl tools re WinAVR, which includes AVR-GCC (GNU C Compiler for AVR core), the AVR-Libc librry, nd the Procyon AVRlib C function librry. All the ncillry tools re open source. 8.3 Summry A wheeled robot prototype hs been successfully built by using the Lego Mindstorms NXT system. The NXT pltform provided the construction pieces, "brick" contining the CPU nd sensor interfces, nd pir of servo motors for driving the wheels. Additionl components include two Avgo opticl mouse sensors, nd n AVR00 processor bord equipped with n Atmel ATmeg3 microcontroller tht provided the necessry interfce for communictions between the NXT's CPU nd mouse sensors. Although ll the components re stock items, severl modifictions hd to be mde in order to get them to operte s unified system. As for the softwre, while MATLAB nd Simulink were collectively the primry progrmming environment, number of other open source nd free proprietry softwre were required to work in conjunction with the min progrm in order for the whole system to function. While the prototype ws being ssembled, simultion tests were crried out concurrently. Now tht the construction of the wheeled robot is complete, hrdwre vlidtion will be the next logicl step. The results of the both simultion nd rel-world tests will be presented in the following chpter. 163

180 Chpter 9: Simultion nd Vlidtion Results of the Adptive Model The trcking ccurcy of WMR does not only depend only on the precision its sensor. Without competent control system, the WMR would be rther useless. So, the computtionl model developed in Chpter 7 will hve to undergo extensive testing in order to ssess its suitbility for the intended purpose. The first test phse will be crried out completely in simultion. Vrious trjectories will be ssigned to the progrm, nd the resulting simulted response will be used to twek the model to improve its performnce. The second test phse will involve rel-world testing of the ctul prototype. The finl results will be presented nd nlysed in this chpter. 9.1 Simultion Results nd Anlysis The trjectories in this chpter re selected to mtch the ones used in the PID model in order for comprisons to be mde. However, there re few dditionl simultions here tht do not hve mtching counterprts in the PID model. They re included in order to provide more comprehensive testing regime. In couple of the extr tests, the PID model could not perform the simultions to n cceptble stndrd, so they were not included in the report. It is recommended tht Chpter 5 be revisited for comprison purposes. Looking t the trjectory plot in Fig. 9.1, it is cler tht the controller successfully mnged to follow the designted pth. This is confirmed by the results error distribution results shown in Fig 9.. The resulting RMS errors re 5.97 mm in the X-xis nd 31.8 mm in the Y-xis. This is quite cceptble, considering tht the WMR ws frequently lgging. The reson for this is tht the speed of the prescribed trjectory ws set to be ner the mximum level llowed for tht the WMR. So, whenever the WMR hd to slow down when negotiting corners, it fced mighty tsk in ctching up fterwrds. Despite this, it mnged to complete its test without ny issue. Another thing is tht the controller ws forbidden to engge in reverse motion for ny of the wheels (for resons described previously). This cused the turning rdius of the WMR to be incresed nd hd noticeble effect when going round shrp corners. This is the reson tht the WMR ws seen to veer off temporrily from the expected pth while negotiting 90- degree turns. Needless to sy, this hd n impct on the clcultion the RMS errors. 164

181 Fig. 9.1 Simultion Test No. 1: Squre trjectory (m x m) Fig. 9. Adptive Simultion Test No. 1: Squre trjectory (m x m) error nlysis 165

182 The plot shown in Fig. 9.3 is n enlrged version of the previous test. It is lso the sme pth s used in the UMBmrk clibrtion test s mentioned in the chpter on PID simultion results. The outcome shows tht tht the controller ws ble to follow the designted pth very well. The error distribution results re shown in Fig 9.4. The resulting RMS errors re mm in the X-xis nd mm in the Y-xis. The longer stretches of stright lines compred to the preceding test ment tht the WMR hd more time to pick up speed fter being slowed down by shrp corners. These error vlues re clerly superior to those in the PID test. Also, only slight twek to one of the gin settings ws necessry to obtin these results. Indeed, the controller performed quite well even without the djustment. So, the twek ws more like bit of fine-tuning. Fig. 9.3 Adptive Simultion Test No. : Squre trjectory (4m x 4m) 166

183 Fig. 9.4 Adptive Simultion Test No. : Squre trjectory (4m x 4m) error nlysis The plots shown in Figs. 9.5 nd 9.6 shre the sme course s the preceding test. However, the strting positions of the WMR nd trjectory re different. This ment tht the WMR lgged the trjectory from the beginning nd hd to ply ctch-up. This ws good chllenge for the controller just like it ws in the similr PID test. The end result is excellent s demonstrted by the WMR's bility to steer towrd the pth nd mintin its trjectory fterwrds. In contrst to the PID test, there ws no need t ll to re-tune the controller's gin settings from the previous test. The verstility of the dptive controller is clerly evident. As in the similr test in the PID simultion, there ws no error nlysis conducted for these scenrios becuse the offsets would skew the results. Besides, the bility of the controller to trck this prticulr trjectory hd lredy been tested in the preceding simultion. 167

184 Fig. 9.5 Adptive Simultion Test No. 3: Squre trjectory 1 with offset strting point Fig. 9.6 Adptive Simultion Test No. 4: Squre trjectory with offset strting point 168

185 The sinusoidl trjectory lcked the shrp turns tht were present in the squre pths. However, stright lines re esier to trck thn curves. In the end, the gentle curvture of the sinusoid did not turn out to be much of chllenge to the controller's bility t ll. As is evident from the plot in Fig. 9.7, the WMR trcked its ssigned trjectory with ese. This observtion ws bcked up by the error nlysis chrts in Fig The RMS errors in the X- nd Y-xes re.63 mm nd 5.60 mm respectively. These figures re simply outstnding nd fr surpssed the results in the equivlent PID test. To obtin the best results, slight djustment ws mde to one of the gin settings from the previous 4m-squre test. Indeed, the finl settings ended up being identicl to the ones used in the m-squre simultion - which were only mrginlly different from those used in 4m-squre test nywy. Yet gin, even under vrying situtions, the controller continued to demonstrte its remrkble dptive qulities. Fig. 9.7 Adptive Simultion Test No. 5: Sinusoidl trjectory 169

186 Fig. 9.8 Adptive Simultion Test No. 5: Sinusoidl trjectory error nlysis The trjectory from the previous test ws run gin using sinusoid of double the mgnitude nd frequency s shown in Fig Just s in the similr PID test, the length of this course ws slightly over twice tht of the preceding sinusoidl pth, nd the ssigned completion time ws doubled. This would prove to be quite chllenge for the controller becuse the more cute turns will slow the WMR quite drmticlly, but no dditionl time ws given for it to recover its speed fter negotiting those turns. Despite the demnding circumstnces, the controller ws ble to complete the tsk without ny problem s is illustrted by the trjectory plot. Looking t the error distribution nd density chrts in Fig 9.10, it is cler tht the results were substntilly worse thn the previous sinusoidl test. Given the more rduous tsk, the outcome is to be expected. The RMS errors in the X- nd Y-xes re 1.75 mm nd 60.9 mm respectively. The performnce is still unmistkebly superior to the PID model, lthough the difference is not s substntil s in other tests. This probbly indictes tht even though the dptive controller performed better thn the PID model, it still found the tsk quite chllenging. 170

187 Fig. 9.9 Adptive Simultion Test No. 6: Sinusoidl trjectory Fig Adptive Simultion Test No. 6: Sinusoidl trjectory error nlysis 171

188 The tringulr trjectory show in Fig 9.11 ws probbly one of the more chllenging tests for the WMR's controller. The shrp nd cute ngles of the turns mde the negotition round them rther tricky nd slow. It ws not helped by the fct tht the WMR ws not llowed to reverse its wheels, so the turning rdius ws reltive lrge. All this ment tht the WMR often lgged behind the trjectory. In spite of ll these fctors, the controller still mnged to get the WMR to stick to the trjectory quite well s evidenced by the trjectory plot. Due to the wide turns tken by the WMR, quite bit of pth devition ws expected in the finl outcome. Indeed, the results of the error nlysis confirmed the prediction. The RMS errors in the X- nd Y-xes re mm nd mm respectively s shown in Fig Compred to the results from previous simultions with less demnding trjectories, these numbers re not too bd t ll. The stright lines probbly helped the sitution becuse they re esier to follow nd lso llow the WMR to pick up speed much quicker. This trjectory ws the first to require tweking of more thn one gin setting in order to get the best results. However, the djustment process took only short time to complete s opposed to the typicl PID tuning routine. Indeed, this test ws lso crried out for PID model, but no mount of tuning could get the controller to perform dequtely. So, the results were left out. Fig Adptive Simultion Test No. 7: Tringulr trjectory 17

189 Fig. 9.1 Adptive Simultion Test No. 7: Tringulr trjectory error nlysis The circulr trjectory s shown in Fig 9.13 shres one obvious chrcteristic with the squre pth, which is they ech strt nd end t the sme plce. The devition from the originl point provides good visul indiction of much trcking drift or error hs been ccumulted. Although there re no shrp corners to negotite, tests hve shown tht stright lines re much esier to trck nd nvigte thn curves. In order for direct comprisons with the PID model, the test ws conducted for both one nd two revolutions of the circle. Unlike the PID test, there ws no instnce where the WMR ws ble to perform dequtely for one revolution but suffered from oscilltions s the test continued. In order to prevent n overlpping plot from obscuring ny undultion or minor detil of the WMR's trjectory, the plot for one revolution will lso be shown here. 173

190 Fig Adptive Simultion Test No. 8: Circulr trjectory (r = m, 1 rev) The plot in Fig shows tht the controller ws fully cpble of mintining circulr trjectory for two revolutions. Some imperfections in the WMR's trjectory were noticeble but not very significnt. A clerer picture of the controller's performnce is provided by the error nlysis in Fig The RMS error of mm in the X-xis is quite comprble to those of previous simultions with different trjectories. Wheres, the RMS error of mm in the Y-xis is fr poorer thn ny of the dptive simultion results collected thus fr. However, since the error spred is quite smll nd the trjectory plot indictes very little pth devition, most of the error could be ttributed to lg. This is somewht expected becuse curved trjectory is hrder to trck thn one with stright lines. But the mount of lg is bit more thn nticipted. However, considering tht the totl distnce trvelled in the Y-xis ws 4 m, the error mrgin is still quite cceptble. The performnce is clerly superior to the PID-bsed model, but not quite s much s in other tests. Another point is tht the prescribed trjectory speed ws lmost 90% of the WMR's mximum velocity. Tht ment tht if there were ny minor trcking difficulties, the WMR would struggle to keep up with the trjectory. This might be possible explntion for the unexceptionl results. Despite the issues with lg, it is importnt to note tht the controller ws ble to keep the WMR on trck throughout the test without much devition. 174

191 Fig Adptive Simultion Test No. 8: Circulr trjectory (r = m, revs) Fig Adptive Simultion Test No. 8: Circulr trjectory (r = m, revs) error nlysis 175

192 The simultion shown in Fig is bsiclly the sme s the preceding test except tht the trjectory is lrger circle. Just s before, the visul results indicte tht the controller performed well in mintining its ssigned trjectory. The error nlysis chrts in Fig will provide better guge of the ctul performnce of the controller. Compred to the previous test, the results here were even less stellr. An RMS error of mm in the X-xis is still quite cceptble. However, discrepncy of mm in the Y-xis is quite poor. Since there ws little devition of the WMR from the ssigned trjectory, the errors must be due to lg once gin. The error distribution shows tht the WMR mnged to keep up with the trjectory for the most prt, but there re occsions when it lgged by quite significnt mrgin. It is postulted tht if the prescribed trjectory speed is lowered, the lg issues would be reduced or eliminted. In ny cse, this is good wy to test the limits of controller's bilities. Since the WMR ws ble to follow its preset trjectory without veering off, the controller must be considered to hve done its job - lbeit not quite perfectly. Fig Adptive Simultion Test No. 9: Circulr trjectory (r = 3m, revs) 176

193 Fig Adptive Simultion Test No. 9: Circulr trjectory (r = 3m, revs) error nlysis At the conclusion of the bttery of simultion tests, summry of the results is presented in Tble 9.1 for ese of reference. Trjectory Type RMS Error in X-Axis (mm) RMS Error in Y-Axis (mm) Squre: m x m Squre: 4m x 4m Sinusoidl: y = sin(x) Sinusoidl: y = sin(x) Tringulr Circulr: rdius = m Circulr: rdius = 3m Tble 9.1 Summry of simultion test results of dptive model 177

194 9. Hrdwre Vlidtion Results nd Anlysis When the lgorithm of trcking controller hs been thoroughly tested in simultion, the logicl steps tht follow would be to build prototype to verify the computed predictions. The vehiculr prototype is bsed on the Lego Mindstorms NXT pltform. Detils of the vehicle nd its min components re described in Chpter 8. The WMR used here hs only two opticl mouse sensors. It hs no ccess to dditionl onbord or externl sensors tht my provide ny kinds of positionl or inertil informtion. Due to this restriction, it is impossible to trck the ctul movement of the vehicle. However, there is no problem in physiclly mesuring its finl resting coordintes becuse it position cn be mrked on the floor. Indeed, this is the most importnt point. During opertion, WMR generlly hs some llownce for minor devition from its ssigned trjectory. However, the min objective is for it to get s close s possible to its destintion coordintes. If the WMR is ble to consistently rech its ssigned destintion coordintes ccurtely, it mens tht it is ble to trck its position very well. This infers tht the WMR is lso ble to follow its prescribed trjectory with good precision. It is highly unlikely tht WMR could repetedly rrive t the finl coordintes but not be ble ccurtely trck ny other point long its trjectory. Noting the lessons lernt from the simultion tests, the ssigned trjectories used in the ctul tests would only be bout 70% of the mximum speed of the WMR. This will llow it the opportunity to ctch up fter it hs performed mnoeuvre tht hs reduced its speed. Otherwise, it my not be ble to reduce or eliminte the lg cused by slowdown, nd these lgs could ccumulte if the prescribed trjectory is quite convoluted. Since the WMR did not encounter ny scenrio where mking trjectory correction would risk the vehicle running bckwrds for prolonged periods of time, the restrictions on reverse motion ws lifted in order to llow tighter turning mnoeuvres. The initil set of tests is bsic evlution of the trcking performnce of the WMR over stright line of m, i.e. x = m nd y = 0 m. It lso includes comprison of the two loclistion methods used, i.e. kinemtic nd hybrid geometric. Judging from the outcome of the first round of tests shown in Tble 9., the discrepncy between the two pproches were quite smll considering the distnce trvelled. However, the differences were not totlly insignificnt. The hybrid method displyed smller men error nd RMS devition thn the purely kinemtic technique. So, the hybrid method is clerly better - t lest for this prticulr test. 178

195 Dul-Sensor Trcking (Trjectory: x = m; y = 0 m) Hybrid Geometric Loclistion Kinemtic Loclistion Error in X [mm] Error in Y [mm] Error in X [mm] Error in Y [mm] Men error [mm] RMS error [mm] Tble 9. Trcking errors in dul-sensor configurtion The objective of the next set of tests is to evlute the trcking performnce if only one sensor is utilised. The sme trjectory would be used for comprison with the dul-sensor system. In this scenrio, the WMR cn only use the kinemtic method for loclistion purposes. The results of the single-sensor test re displyed in Tble 9.3. It is cler tht the performnce of the WMR when dopting this configurtion ws noticebly poorer thn when employing the dul-sensor system no mtter which loclistion method ws used in the ltter. The men error in the direction of trvel is very smll, but there is quite bit of vrince in dt. As for the lterl direction, the men error is significntly greter thn in the longitudinl direction nd the vrince is even lrger. The devition of WMR becme slightly noticeble towrds to end of the trjectory. This ws most likely due to the errors tht hd been ccumulted. The inferior results compred to the previous tests re not entirely unexpected. After ll, this is the rtionle for using two sensors insted of one, nd the prediction hs been confirmed. It should be kept in mind this is the method used s contingency when dt from one of the sensor is deemed unrelible. As discussed in erlier chpters, this pproch should not be 179

196 relied upon for loclistion purposes, nd ny usge should be infrequent nd only for very short periods of time. Single-Sensor Trcking (Trjectory: x = m; y = 0 m) Error in X [mm] Error in Y [mm] Men error [mm] RMS error [mm] Tble 9.3 Trcking errors in single-sensor configurtion The third set of tests puts the WMR through squre trjectory with dimensions of 4 m on ll sides. As mentioned previously, this is the sme course used in the UMBmrk clibrtion test. It must be noted tht this WMR uses n opticl sensor system tht is clibrted very differently from those using wheel encoders. However, it is not the clibrtion procedure tht is of interest here. The UMBmrk test hs been used quite frequently by other reserchers, so their finl results cn be used s comprison to the ones obtined in this test. This test would be run five times in the clockwise direction nd five times in the nticlockwise direction. The results shown in Tbles 9.4 nd 9.5 indicte tht the trcking nd loclistion cpbilities of the WMR re very good. The lrgest men error in ny direction is only 13. mm. However, its error spred is quite bit bigger, with mximum of t mm. In ny cse, this is still n excellent result. 180

197 Clockwise Direction in Squre Trjectory (4m x 4m) Error in X [mm] Error in Y [mm] Men error [mm] RMS error [mm] RMS error [%] Tble 9.4 Trcking errors in squre trjectory (clockwise) Anticlockwise Direction in Squre Trjectory (4m x 4m) Error in X [mm] Error in Y [mm] Men error [mm] RMS error [mm] RMS error [%] Tble 9.5 Trcking errors in squre trjectory (nticlockwise) The method used by UMBmrk for error quntifiction is the mgnitude of the offset from the men error. The lrger of the clockwise nd nticlockwise results would be the finl defining vlue. r x err y err (9.1) where x err is the men error between computed nd ctul positions in x y err is the men error between computed nd ctul positions in y 181

198 Using the metric describe bove, the error mgnitude for both the clockwise nd nticlockwise tests re: r cw mm (9.) r cw mm (9.3) E mx mx R cw ; R cw (9.4) Emx mm (9.5) The outcome ccording to this metric is very good in comprison to other ded-reckoning systems. The result chieved by nother dul-sensor system for the sme trjectory is n error of 114 mm (Bonrini et l. 004). The best results ttined by the developer of the UMBmrk over eight sets of tests re errors tht rnged from n verge of 1 mm to 35 mm with stndrd error of the men of 11. mm (Borenstein nd Feng 1996). However, the WMR ws pinstkingly clibrted (nd sometimes hd to be re-clibrted between sets) nd could only run t slow pce in order to void tyre slippge. Tht is becuse it relied on wheel encoders for odometric mesurements nd thus could not compenste for ny slippge. For reference purposes, the performnce results of nother WMR tht used multiple mouse sensors will be mentioned here (Sekimori nd Miyzki 007). The trjectory used in tht cse ws.5 m long nd included four right-ngle turns. Even though the use of different trjectory mens tht the results re not exctly comprble to the ones dopted in this reserch, it is nonetheless useful to cite them here for some sort of comprison. Using twosensor configurtion, it produced n verge trnsltion error of mm. Using foursensor configurtion, it produced n verge trnsltion error of mm. The sensor clibrtion process used by the WMR in this reserch ws simple nd strightforwrd. The procedure only needed to be performed once for prticulr type of trcking surfce. Without needing further clibrtion or djustment, the WMR ws ble to repet its good performnce over nd over gin. The effectiveness of the trcking nd loclistion system used by the WMR in this reserch ws thus confirmed. The lst round of tests is n ssessment of how well the redundnt system works. The ide is to disrupt the trcking of either of the sensors (but not both) for very short period of time. The sensor cnnot focus properly if the gp between itself nd the trcking surfce is too smll. A thin object tht could slide underneth the sensor mounting plte would be good choice t first thought. However, since the both sensors re lined up behind the front cstor wheel, it mens tht cstor wheel would encounter the object s well. If this hppens, the front of the vehicle would be lifted, nd the trcking of both sensors would be ffected s 18

199 result. The only wy to do this is to introduce the item right behind the cstor wheel before the WMR begin moving. However, it would be limited to one ttempt per test run. It hs been sid tht opticl mouse sensors work poorly on mirror-like surfces. Since mirrors re not pper thin, luminium foil ws used in test run insted. Somehow, the sensor hd no problem coping with it. The next ide ws to slide ruler under the vehicle quickly nd pull out before it mkes contct with the rer wheels. It proved to be quite impossible to crry out becuse the rer sensor is very close to the rer wheels nd the ruler kept getting run over by one of wheels before it could be completely pulled out. Finlly, the solution ws found in 15cm-long fether. It ws light nd flexible, nd could be flicked in nd out under the vehicle very quickly. There were still plenty of times tht it got cught under the rer wheel, but no lterntive hd been found t the time of experiment. Due to the inexct mnner of this method, every test run should be nlysed individully. This set of tests is stright-line trjectory tht runs for m with sensor obstruction ffecting front nd rer sensors lterntely. The obstruction mnoeuvre would occur twice during the run nd for bout one to two seconds ech time. The results in Tble 9.6 clerly illustrte the mssive mount of errors tht cn occur if inccurte dt is not properly identified nd ccounted for. Indeed, when the sensors were obstructed for longer periods of time, the errors were even greter. As stted before, the inexct test conditions mens tht the individul test runs cnnot be collectively nlysed. However, on n individul bsis, the results re very cler. For the runs where the error compenstion lgorithm ws ctivted in the min progrm, the outcome shows tht it works quite well. The results re comprble to those from the single-sensor test s shown erlier in Tble 9.3. Sensors Momentrily Obstructed (Trjectory: x = m; y = 0 m) Without Error Detection With Error Compenstion Error in X [mm] Error in Y [mm] Error in X [mm] Error in Y [mm] Tble 9.6 Trcking errors with nd without error compenstion 183

200 9.3 Discussion nd Conclusion The simultion results re very good nd show tht the controller's lgorithm ws cpble of hndling the job. It mnged to cope with every trjectory tht ws ssigned to it. Even in situtions where it could brely keep up becuse of speed limittions, it still kept trcking the prescribed pth with very little devition. Unlike the PID model, the dptive system only required very minor djustments to its gin settings in order to obtin optimum results in vrious test scenrios. After the controller hs proven its bilities, the next step ws to build wheeled robot s test prototype for verifying the outcomes predicted by the simultions. The hrdwre vlidtion results were excellent throughout. For odometry comprison, the hybrid geometric method proved to be superior to tht of the Euler-bsed kinemtic pproch. For the UMBmrk test, the men errors were quite smll nd better thn expected. However, the mrgin of RMS error ws between two nd three times tht of the men error. This reflects the mount of error tht is rndom or unccounted for. Considering tht men error is smll, even with this level of vrince the overll performnce is still very good compred to other WMRs tht re bsed on ded-reckoning system. An importnt cvet regrding the results is tht the trcking surfce used in the tests ws reltively smooth, nd the surfce texture ws quite uniform. The results would certinly be poorer if there hd been pronounced undultions in the trcking surfce or lrge vritions in the surfce texture. It must lso be noted tht the ctul trcking ccurcy still cnnot mtch the high level of precision provided by the sensors. It is suspected tht the outriggers hd plyed n indvertent role in contributing to the stbility of the vehicle. They were only ment for mrking out the sensor's position on the ground. However, since they were lmost touching the ground, they ensured tht during vehicle motion minimum clernce between the sensors nd ground ws mintined regrdless of ny minor flexing of the chssis or tyres. The results fter the ddition of the outriggers seem to indicte slight but noticeble improvement. The outcome of the redundncy test ws quite stisfctory. It showed tht the system s ble to detect reding errors nd the controller ws ble to compenste for them. The results were quite comprble to those of the single-sensor test, but perhps not quite s good. In contrst, the performnce of the WMR without ny error detection ws inferior to the single-sensor test by wide mrgin. The disruption of the sensors' trcking ws infrequent nd only momentry, but it ws enough to distort the loclistion of the WMR. As ded-reckoning errors re cumultive, the outcome would obviously be worse if the sensors were constntly losing their trcking. However, due to the inherent nture of this method, the results would be much improved if sensor disruptions were only limited to very short periods, e.g. well under 1 s. 184

201 Although the WMR hs demonstrted its bility to detect nd ccount for sensor error, the contingency lgorithm cnnot provide the sme ccurcy offered by dul-sensor system. So, the level of redundncy must strictly be considered s only prtil. However, s the evidence hs shown, hving prtil redundncy is clerly preferble to none t ll. 185

202 Chpter 10: Generl Conclusions At the culmintion of this reserch endevour, some slient points cn be mde in regrds to the work is presented in this thesis. The reserch questions tht were posed t the beginning of this study will lso be ddressed in reltion to the work tht hs been conducted. This reserch explored two fundmentlly different trcking control systems. The initil system employed PID-bsed control lgorithm which used Euler's pproximtion for odometric clcultions. Two models were designed for this system: one tht ws purely kinemtic nd the other tht hd dynmic constrints imposed. When the dynmic lgorithm ws ctivted in the model during simultion, it ws cler tht the simulted motion slowed down in certin situtions. This ws not surprising becuse the purely kinemtic model did not hve to tke into ccount ny dynmic fctors tht would ffect vehiculr behviour, such s ccelertion round bend. Thus, without ny dynmic considertions, purely kinemtic model is not quite relistic nd is only pplicble to slow-moving vehicles. This ddresses Reserch Question. In generl, the simultion results were mostly cceptble nd sometimes quite good. On occsions, they were little less thn stisfctory. The mjor drwbck of the PID-bsed system is tht the gin settings pply only to rther limited rnge of opertionl situtions. This mens tht whenever there is smll chnge in conditions, such s the ssigned trjectory, required speed or different strting point, the gin settings would hve to be redjusted. Furthermore, this system hs shown itself to be very sensitive to even minor tweks to the gin settings. As result, the djustments must be crried out using very fine grdtions. Therefore, without the help of n estblished technique for tuning this cscded PID system, the mnul tuning procedure would require plenty of time nd effort. Thus, the prcticl pplictions of this system re questionble t best. Due to the obvious limittions of the PID-bsed system, n lterntive dptive model ws developed. The simultion results of the dptive system were generlly very stisfctory nd clerly superior to those of the PID model. Also, in contrst to the PID model, optimum results in vrious test conditions were ttined with only very slight djustments mde to the gin settings of the dptive system. This indictes tht the system is verstile nd reltively esy to use. In the vlidtion tests, the good experimentl results of the ctul prototype confirmed the performnce level s predicted by the simulted model. The WMR lso performed very well in the widely-cited UMBmrk test when compred to published results of other ded-reckoning systems tht hve undergone the sme test. Furthermore, the dvntge of using two 186

203 sensors over one is clerly demonstrted theoreticlly nd experimentlly. This outcome provides cler nswer to Reserch Question 1. Despite the excellent results in comprison to similr systems, the trcking ccurcy is still unble to mtch the precision level of the sensors. The sensor dt hs proved to be remrkble consistent during clibrtion tests in ner-idel test conditions, i.e. very smooth trcking surfce, similr surfce texture throughout, stedy sensor height due to WMR being fixed in sturdy frme nd slid long without tyres rotting, etc. Even though electricl noise is n unvoidble chrcteristic of ll electronic components, the results indicte tht it is not significnt issue here. Thus, it is hrd to doubt the relibility or stbility of the sensor's redings. The conditions during regulr experimentl runs re less uniform or idel thn the short clibrtion tests. So, the sources nd mgnitudes of errors re lmost certin to be greter thn those seen in the clibrtion tests. It is well-estblished fct tht the sensors re highly sensitive to chnges in opertionl conditions. They lso hve very nrrow optiml opertionl rnge nd my suffer severe loss of precision when operting outside its limits. Therefore, it is very likely tht non-optiml opertionl conditions re more significnt fctor in the loss of trcking ccurcy thn intrinsic rndom error emnting from the sensors themselves. This nswers Reserch Question 3. An essentil fctor in the ccurcy of WMR's loclistion is the determintion of its odometry. In the PID-bsed model, first-order Euler pproximtion is used for odometric clcultions, while the dptive model dopted hybrid geometric pproch. In the pst, most WMRs used the Euler method becuse most of them employed single sensor for trcking. In recent yers, more reserchers hve begun using more thn one sensor for their WMRs, nd re thus ble to dopt geometric pproch for odometry. In the theoreticl nlysis, the Euler method is shown to be cceptbly ccurte when smpling periods re very smll, i.e. high smpling frequencies. Also, its ccurcy increses s the WMR's turning ngle pproches zero. In contrst, the geometric pproch is n idel model tht is theoreticlly exct nd not ffected by smpling frequency or turning ngle. It provides much greter ccurcy thn Euler-bsed systems. However, purely geometric pproch hs serious limittions when it comes to prcticl pplictions. An in-depth exmintion of the geometric model revels severl fundmentl issues tht would gretly ffect the ccurcy of the odometry when pplied to rel system. Firstly, rounding errors occur when trying to void divide-by-zero scenrios. Secondly, trunction errors re inevitble s vlues of system vribles pproch infinity. Lstly, clcultion ccurcy is limited by the computer (mchine) precision ccording to the IEEE Stndrd for 187

204 Floting-Point Arithmetic (IEEE 754). Computer precision decreses s numericl mgnitude increses. In combintion, these fctors gretly ffect the odometric ccurcy of the WMR's loclistion process. These problems hve been fully ddressed in the theoreticl sections of the thesis nd unique hybrid geometric loclistion model is proposed. The results of the hrdwre testing clerly demonstrte the superiority of the hybrid method over the Euler-bsed pproch. Furthermore, the results lso show tht the hybrid geometric method outperformed other similr systems tht dopted the purely geometric pproch. This vlidtes the soundness of the new method. The nswers to Reserch Questions 4 nd 5 re tht the current odometric methods hve significnt shortcomings due to previously undocumented fctors which hve now been ddressed in this reserch. However, the possibility cnnot be ruled out tht there re still some unknown fctors tht contribute to the loss of trcking ccurcy other thn purely rndom errors. All the recognised issues tht ffect the trcking ccurcy of WMR hve been thoroughly discussed in this thesis nd solutions hve been proposed for some of them. As mentioned previously, other unknown fctors my exist. While every issue contributes to loss of ccurcy to certin extent, some problems hve fr more significnt effect thn others. Most notbly, it is lmost impossible for the WMR to keep its sensors operting within the optiml ground clernce rnge of 0.6 mm on typicl trcking surfce. Even the mximum llowed rnge is only 1.6 mm. Thus, repeted degrdtion in sensor precision cnnot be voided. This is certinly one of the most significnt concerns of ll. The nrrow optiml rnge of the gp between sensor nd trcking surfce hs proven to be the biggest known fctor ffecting trcking ccurcy. In order to llevite the problem, it is suggested tht the sensor be mounted t higher position nd its existing lens be swpped for nother with longer focl length. With this set-up, chnges in the sensor elevtion due to surfce undultions would be proportionlly smller in comprison with the extended focl length. This mens tht the loss of focus is reduced, nd precision cn thus be mintined t cceptble levels. This could result in big improvement in terms of trcking ccurcy. The downside of this pproch is tht the sensor redings would be more ffected by ny tilting motion of the WMR. Until this modifiction is implemented, there re still other improvements to trcking ccurcy nd relibility tht cn be introduced in the mentime. In this reserch, the dvntge of using two sensors over one is clerly demonstrted theoreticlly nd experimentlly. In ddition, if either of the sensors is producing unrelible redings, new method is proposed to detect the error nd mke necessry djustments in the odometric clcultions. This provides the trcking control system some form of redundncy. Without this feture, even short spells of reding error would severely ffect the 188

205 ccurcy of the WMR's loclistion. Since this pproch relies on using one sensor for odometry if the other fils, it cnnot fully compenste for the loss of dt from the other sensor. Hence, it cn only be considered prtilly-redundnt system. Even though this is not perfect solution, it helps mintin certin level of trcking ccurcy nd thus leds to greter overll relibility of the system. The results of hrdwre testing clerly show the promise of this method. Reserch Question 6 is ddressed by this prtilly-redundnt technique s well s the proposl for chnges in the sensor mounting height nd focl length of its lens. To sum up, it is useful to restte the ccomplishments nd novelties of this reserch. This study ws ble to successfully combine the use of n dptive controller nd dul opticl mouse sensor system to develop n ccurte trcking system for wheeled robot. In the process, severl fctors were identified tht hve significnt collective impct on the odometric precision of purportedly idel loclistion method. These errors hve not been documented in existing literture until now. They hve been ddressed in this thesis nd unique solution hs been provided. In ddition, new error-detection nd correction method hs been proposed to provide prtil redundncy for the system in cse one of the sensors experiences temporry dt fult. As envisioned, the use of reltively inexpensive off-theshelf components mde it possible to deliver prototype tht is low-cost nd requires low processing power, but is still ble to perform its tsk effectively. Finlly, even though ded reckoning hs fllen out of fvour in recent times, this study shows tht there is plenty of usefulness left in this nvigtionl pproch. 189

206 10.1 Future Work One of the most notble problems encountered when using n opticl mouse sensor (OMS) for robot trcking is the susceptibility to reding errors due to fluctutions in the distnce between the sensor nd trcking surfce. The optiml gp size between sensor nd surfce is typiclly bout mm or so. Thus, even slight vrition in the height of the gp cused by surfce imperfections could led to significnt divergence from the norm in terms of proportion. If the current sensor lenses were swpped for ones with longer focl length, the sensors could be mounted t much higher position. In such configurtion, minor chnges in the gp distnce will no longer mount to lrge proportionl devition. This mens tht the sensor cn mintin focus more esily nd its height sensitivity would be reduced. As result, the sensor would be more tolernt of surfce undultions nd trcking performnce would be gretly improved. This effect hs been demonstrted previously (Jckson et l. 007) where n OMS mounted on the bumper of stndrd rod vehicle produced n verge error rte of just 1.07% over distnce of.5 m. The dvntge of hving the sensors relocted to higher position is cler. However, there is n obvious drwbck. Distortions in the sensors' redings cused by tilting would be mgnified s result. In order to llevite this problem, the sensors could be mounted on gyro-stbilised gimbl system. This would ensure tht the sensors re lwys pointed verticlly downwrds. Unfortuntely, this option would dd significnt expense nd complexity to the system. For low-cost pltforms, this would not be fesible. A more relistic pproch would be to find the optimum sensor mounting height where there is blnced trde-off between sensitivity to gp distnce nd vehicle tilting. A potentil enhncement to the current prototype is n obstcle voidnce system. Since the trcking sensors fce downwrds nd cnnot detect ny obstruction in its pth, forwrdlooking sensors hve to be dded, such s infrred or ultrsonic ones. Nturlly, the necessry lgorithms would hve to be dded to the min control system. Aside from looking t wys to improve the existing system, potentil pplictions in other relted reserch res should lso be investigted. One of them is the use of the OMS trcking system s loclistion subsystem for mpping robot. For systems employing Simultneous Loclistion nd Mpping (SLAM) techniques, their loclistion ccurcy is nowhere ner the level provided by systems using OMS. This is mostly due to methodologicl or technologicl limittions. Once the mpping phse is complete, the OMS trcking cn be turned off or kept running in the bckground for redundncy purposes. 190

207 References Agilent Technologies (004). Agilent 0x Surfce Trcking Improvement for Lser-Bsed Mice. Snt Clr, CA, USA, Agilent Technologies. Åström, K. J., Borisson, U., et l. (1977). Theory nd Applictions of Self-Tuning Regultors. Automtic 13(5): Åström, K. J. nd Hägglund, T. (1995). PID Controllers - Theory, Design, nd Tuning (nd Edition). ISA. Åström, K. J. nd Wittenmrk, B. (1973). On Self Tuning Regultors. Automtic 9(): Atmel (008). AVR151: Setup nd use of the SPI on tinyavr nd megavr device (Revision C). Atmel Corportion. From: Atmel (009). AVR311: Using the TWI module s IC slve on tinyavr nd megavr devices (Revision D). Atmel Corportion. From: Atmel (010). AVR315: Using the TWI module s IC mster on tinyavr nd megavr devices (Revision C). Atmel Corportion. From: Atmel (011). AT91SAM ARM-bsed Flsh MCU (Revision L). Atmel Corportion. From: Atmel (011b). ATmeg3(L) Complete (Revision Q). Atmel Corportion. From: Avgo (009). ADNS-051 Opticl Mouse Sensor - Dt Sheet. Avgo Technologies/Agilent Technologies, 00. From: Avgo (009b). ADNS-6010 Lser Mouse Sensor - Dt Sheet. Avgo Technologies/Agilent Technologies, 005. From: Avgo (011). ADNS-9500 LserStrem Gming Sensor - Dt Sheet. Avgo Technologies, 009. From: Avgo (01). ADNS-9800 LserStrem Gming Sensor - Dt Sheet. Avgo Technologies, 01. From: Beuchemin, S. S. nd Brron, J. L. (1995). The Computtion of Opticl Flow. ACM Comput. Surv. 7(3): Beckmn, B. (1991). The Physics of Rcing, Prt 7: The Trction Budget. The Physics of Rcing. From: Bennett, S. (001). The Pst of PID Controllers. Annul Reviews in Control 5(0): Bilenc, A. Lser Speckle. S. imging.gif, Wellmn Center for Photomedicine, Msschusetts Generl Hospitl. Blis, F. (004). Review of 0 Yers of Rnge Sensor Development. Journl of Electronic Imging 13(1): Bobtsov, A. A., Pyrkin, A. A., et l. (011). Using of LEGO Mindstorms NXT Technology for Teching of Bsics of Adptive Control Theory. 18th Interntionl Federtion of Automtic Control (IFAC) World Congress. Miln, Itly, Interntionl Federtion of Automtic Control (IFAC). Bonrini, A., Mtteucci, M., et l. (004). Ded Reckoning for Mobile Robots Using Two Opticl Mice. First Interntionl Conference on Informtics in Control, Automtion nd Robotics (ICINCO 004). H. Arújo, A. Vieir, J. Brz, B. Encrnção nd M. Crvlho. Setúbl, Portugl, INSTICC Press: Bonrini, A., Mtteucci, M., et l. (005). Automtic Error Detection nd Reduction for n Odometric Sensor bsed on Two Opticl Mice. Proceedings of the 005 IEEE Interntionl Conference on Robotics nd Automtion (ICRA 005). Borenstein, J., Everett, H. R., et l. (1997). Mobile Robot Positioning: Sensors nd Techniques. Journl of Robotic Systems 14(4): Borenstein, J. nd Feng, L. (1995). UMBmrk - A Benchmrk Test for Mesuring Odometry Errors in Mobile Robots: 14. Borenstein, J. nd Feng, L. (1996). Mesurement nd Correction of Systemtic Odometry Errors in Mobile Robots. IEEE Trnsctions on Robotics nd Automtion 1(6):

208 Boyden, F. D. nd Velinsky, S. A. (1994). Dynmic Modeling of Wheeled Mobile Robots for High Lod Applictions. Proceedings of the 1994 IEEE Interntionl Conference on Robotics nd Automtion, Sn Diego, CA. Boyden, F. D. nd Velinsky, S. A. (1994b). Limittions of Kinemtic Models for Wheeled Mobile Robots. Advnces in Robot Kinemtics nd Computtionl Geometry. A. J. Lenrcic nd B. B. Rvni. Kluwer Acdemic Publishers, Amsterdm, the Netherlnds: Cmpion, G., Bstin, G., et l. (1996). Structurl Properties nd Clssifiction of Kinemtic nd Dynmic Models of Wheeled Mobile Robots. IEEE Trnsctions on Robotics nd Automtion 1(1): Center for Advnced Sptil Technologies. Uneven bem focus spot, University of Arknss. Chikms, T. (007). LEGO MINDSTORMS NXT system rchitecture. NXT_system_rchitecture.jpg, nxtosek. Cruz-Mrtín, A., Fernández-Mdrigl, J. A., et l. (01). A LEGO Mindstorms NXT Approch for Teching t Dt Acquisition, Control Systems Engineering nd Rel-Time Systems Undergrdute Courses. Computers & Eduction 59(3): Cuéllr, M. P. nd Pegljr, M. C. (011). Design nd Implementtion of Intelligent Systems with LEGO Mindstorms for Undergrdute Computer Engineers. Computer Applictions in Engineering Eduction: n/-n/. Curless, B. (1999). From Rnge Scns to 3D Models. SIGGRAPH Computer Grphics 33(4): d'andre-novel, B., Bstin, G., et l. (1991). Modelling nd Control of Non-Holonomic Wheeled Mobile Robots. Proceedings of the IEEE Interntionl Conference on Robotics nd Automtion, Dlsger, H. W., Esbensen, T., et l. (006). Cmer Controlled Robot. Fculty of Engineering, Science nd Medicine, Deprtment of Electronic Systems. Alborg, Denmrk, Alborg University: 14. Dille, M., Grocholsky, B., et l. (010). Outdoor Downwrd-Fcing Opticl Flow Odometry with Commodity Sensors Field nd Service Robotics. A. Howrd, K. Ignemm nd A. Kelly. Springer Berlin / Heidelberg. 6: Donti, S. (004). Electro-Opticl Instrumenttion: Sensing nd Mesuring with Lsers, 1. Prentice Hll. Dschwen (006). Constnt Frction. Constnt_frction_1.svg.png, Wikimedi Commons. Dumont, G. A. nd Huzmezn, M. (00). Concepts, Methods nd Techniques in Adptive Control. Americn Control Conference, 00. Proceedings of the 00. Durrnt-Whyte, H. nd Biley, T. (006). Simultneous locliztion nd mpping: prt I. Robotics & Automtion Mgzine, IEEE 13(): Edlén, B. (1966). The Refrctive Index of Air. Metrologi : Everett, H. R. (1995). Sensors for Mobile Robots: Theory nd Appliction. A K Peters, Ltd. Fontes, F. A. C. C. nd Mgni, L. (004). A Generliztion of Brblt's Lemm with Applictions to Robust Model Predictive Control. Proceedings of the Sixteenth Interntionl Symposium on Mthemticl Theory of Networks nd Systems (MTNS 004). Louvin, Belgium. Freemn, R. A. nd Kokotović, P. (1996). Robust Nonliner Control Design Stte-Spce nd Lypunov Techniques. Birkhäuser, Boston. Gsperi, M. nd Hurbin, P. E. (009). Extreme NXT: Extending the LEGO MINDSTORMS NXT to the Next Level, Second Edition. Apress, Inc., New York, NY 10013, USA. Gholipour, A. nd Yzdnpnh, M. J. (003). Dynmic Trcking Control of Nonholonomic Mobile Robot with Model Reference Adpttion for Uncertin Prmeters. 38th Europen Control Conference (ECC 003) Cmbridge, U.K. Goldberg, D. (1991). Wht Every Computer Scientist Should Know About Floting-Point Arithmetic. ACM Computing Surveys 3(1): Gonçlves, J., Lim, J., et l. (009). Relistic Simultion of Lego Mindstorms NXT Bsed Robot. Third IEEE Multi-conference on Systems nd Control (MSC 009) consisting of 18th IEEE Interntionl Conference on Control Applictions (CCA) & 4th IEEE Interntionl Symposium on Intelligent Control (ISIC), Sint Petersburg, Russi. Greg, W. nd Pilt, A. (008). Rel-Time Control Teching Using LEGO MINDSTORMS NXT Robot. Interntionl Multiconference on Computer Science nd Informtion Technology (IMCSIT 008). 19

209 Gustfson, E. H., Lollini, C. T., et l. (005). Swrm Technology for Serch nd Rescue Through Multi-Sensor Multi-Viewpoint Trget Identifiction. Proceedings of the 37th Southestern Symposium on System Theory, 005 (SSST '05). Hncock, J. A. (1999). Lser Intensity-Bsed Obstcle Detection nd Trcking, Crnegie Mellon University, The Robotics Institute. Hebert, M. nd Krotkov, E. (1991). 3-D Mesurements from Imging Lser Rdrs: How Good re They? Proceedings of the IEEE/RSJ Interntionl Workshop on Intelligent Robots nd Systems, 1991, 'Intelligence for Mechnicl Systems'. Hong, D., Velinsky, S. A., et l. (1999). Verifiction of Wheeled Mobile Robot Dynmic Model nd Control Rmifictions. Journl of Dynmic Systems, Mesurement nd Control - Trnsctions of the ASME 11(1): Hurbin, P. E. (007). LEGO 9V Technic Motors compred chrcteristics. From: Hurbin, P. E. (007b, Nov 008). NXT motor internls. From: Hyun, D., Yng, H. S., et l. (009). Differentil opticl nvigtion sensor for mobile robots. Sensors nd Actutors A: Physicl 156(): Ionnou, P. A. nd Sun, J. (1996). Robust Adptive Control. Prentice Hll PTR, Upper Sddle River, New Jersey, USA. Jckson, J. D., Cllhn, D. W., et l. (007). A Rtionle for the Use of Opticl Mice Chips for Economic nd Accurte Vehicle Trcking. Third IEEE Interntionl Conference on Automtion Science nd Engineering (CASE 007). JED (005). AVR00 Schemtic (colour). JED Microprocessors. From: JED (008, 3 Nov). JED AVR00 Single Bord Computer using the ATmeg3 CPU. From: Kne, T. R. nd Levinson, D. A. (1983). The use of Kne's Dynmicl Equtions in Robotics. Interntionl Journl of Robotics Reserch : 3-1. Khlil, H. (00). Nonliner Systems, 3rd Ed. Prentice Hll. Kuo, B. C. (1995). Automtic Control Systems, 7. Prentice-Hll, Upper Sddle River, NJ. Lndu, I. D. (003). Controls, Adptive Systems. Encyclopedi of Physicl Science nd Technology (Third Edition). R. A. Meyers. Acdemic Press, New York: Lndu, I. D., Lozno, R., et l. (011). Adptive Control - Algorithms, Anlysis nd Applictions, nd Ed. Springer. Lnger, D. nd Thorpe, C. (199). Sonr Bsed Outdoor Vehicle Nvigtion And Collision Avoidnce. Proceedings of the 199 leee/rsj Interntionl Conference on Intelligent Robots nd Systems. Lee, Y., Prk, S., et l. (1998). PID Controller Tuning To Obtin Desired Closed Loop Responses for Cscde Control Systems. Industril & Engineering Chemistry Reserch 37(5): Lego (006). LEGO MINDSTORMS NXT Hrdwre Developer Kit (Version 1.00). Lego Group. From: Leonrd, J. J. nd Durrnt-Whyte, H. F. (1991). Simultneous Mp Building nd Locliztion for n Autonomous Mobile Robot. Proceedings of the IEEE/RSJ Interntionl Workshop on Intelligent Robots nd Systems, 1991 (IROS '91), 'Intelligence for Mechnicl Systems', Osk, Jpn. Lev, A. nd Mrinelli, A. (009). Comprtive Anlysis of Some Approches to the Autotuning of Cscde Controls. Industril & Engineering Chemistry Reserch 48(1): Logitech (004). Lser Technology Brief, Logitech. Mrtinec, D. nd Hurk, Z. (011). Vehiculr Pltooning Experiments with LEGO MINDSTORMS NXT. Control Applictions (CCA), 011 IEEE Interntionl Conference on. Mxim (011, 14 Jn). Motor controller with feed-forwrd for Lego NXT. From: Minoni, U. nd Signorini, A. (006). Low-Cost Opticl Motion Sensors: An Experimentl Chrcteriztion. Sensors nd Actutors A: Physicl 18(): Monteiro, L. S., Moore, T., et l. (005). Wht is the ccurcy of DGPS? The Journl of Nvigtion 58():

210 Montemerlo, M., Thrun, S., et l. (00). FstSLAM: A Fctored Solution to the Simultneous Locliztion nd Mpping Problem. Proceedings of the Eighteenth AAAI Ntionl Conference on Artificil Intelligence, 00. Edmonton, Albert, Cnd, Americn Assocition for Artificil Intelligence. Montemerlo, M., Thrun, S., et l. (003). FstSLAM.0: An Improved Prticle Filtering Algorithm for Simultneous Locliztion nd Mpping tht Provbly Converges. Proceedings of the Sixteenth Interntionl Joint Conference on Artificil Intelligence (IJCAI 003), Acpulco, Mexico, Morgn Kufmnn Publishers, Sn Frncisco, USA. Murry, R. M., Li, Z., et l. (1994). A Mthemticl Introduction to Robotic Mnipultion. CRC Press. Nukulwuthiops, W., Lowttn, S., et l. (00). Dynmic Modeling of One-Wheel Robot by Using Kne's Method. IEEE Interntionl Conference on Industril Technology, 00. NXP (01). IC-Bus Specifiction nd User Mnul (Rev. 4). NXP Semiconductors. From: O'Hr, K. J. nd Ky, J. S. (003). Investigting Open Source Softwre nd Eductionl Robotics. Journl of Computing Sciences in Colleges 18(3): Oriolo, G., De Luc, A., et l. (00). WMR Control vi Dynmic Feedbck Lineriztion: Design, Implementtion, nd Experimentl Vlidtion. IEEE Trnsctions on Control Systems Technology 10(6): Plcin, J., Vlgñon, I., et l. (006). The Opticl Mouse for Indoor Mobile Robot Odometry Mesurement. Sensors nd Actutors A: Physicl 16(1): Prk, H. R., Hyun, D. J., et l. (009). A Ded Reckoning Sensor System nd Trcking Algorithm for Mobile Robot. ICCAS-SICE, 009. Prks, P. C. (199). A. M. Lypunov's Stbility Theory Yers On. IMA Journl of Mthemticl Control nd Informtion 9(4): Pentlnd, A. P. (1987). A New Sense for Depth of Field. IEEE Trnsctions Pttern Anlysis nd Mchine Intelligence 9(4): Pieper, R., Cooper, A., et l. (1995). Dul Bseline Tringultion. Proceedings of the Twenty- Seventh Southestern Symposium on System Theory, Popov, V. M. (1973). Hyperstbility of Control Systems, Revised Ed. Editur Acdemiei Bucrest / Springer-Verlg. Pourboghrt, F. nd Krlsson, M. P. (00). Adptive Control of Dynmic Mobile Robots with Nonholonomic Constrints. Computers & Electricl Engineering 8(4): Sdsivro, M. V. nd Chidmbrm, M. (006). PID Controller Tuning of Cscde Control Systems Using Genetic Algorithm. Journl of the Indin Institute of Science 86(4): Sh, S. K. nd Angeles, J. (1989). Kinemtics nd Dynmics of Three-Wheeled -DOF AGV. Proceedings of the IEEE Interntionl Conference on Robotics nd Automtion, Srkr, N., Yun, X., et l. (1993). Dynmic Pth Following: A New Control Algorithm for Mobile Robots. Proceedings of the 3nd IEEE Conference on Decision nd Control, 1993, Sn Antonio, Texs, USA. Srkr, N., Yun, X., et l. (1994). Control of Mechnicl Systems With Rolling Constrints: Appliction to Dynmic Control of Mobile Robots. The Interntionl Journl of Robotics Reserch 13(1): Sstry, S. nd Bodson, M. (1994). Adptive Control: Stbility, Convergence, nd Robustness Prentice Hll. Schechner, Y. Y. nd Kiryti, N. (1998). Depth from Defocus vs. Stereo: How Different Relly Are They? Proceedings of the Fourteenth Interntionl Conference on Pttern Recognition, Schoell, E. (004). AVR00 Bord technicl description. JED Microprocessors. From: pdf. Sekimori, D. nd Miyzki, F. (005). Self-Locliztion for Indoor Mobile Robots Bsed on Opticl Mouse Sensor Vlues nd Simple Globl Cmer Informtion. 005 IEEE Interntionl Conference on Robotics nd Biomimetics (ROBIO). Sekimori, D. nd Miyzki, F. (007). Precise Ded-Reckoning for Mobile Robots using Multiple Opticl Mouse Sensors Informtics in Control. Automtion nd Robotics II. J. Filipe, J.-L. Ferrier, J. A. Cetto nd M. Crvlho. Springer Netherlnds:

211 Seyr, M. nd Jkubek, S. (005). Mobile Robot Predictive Trjectory Trcking. Proceedings of the Second Interntionl Conference on Informtics in Control (ICINCO 005), 'Automtion nd Robotics', Brcelon, Spin. Shw, A. (011, 0 Feb). ic.c: Added higher speed ic operting mode (15KHz rther thn 9.6KHz) (Revision 441). From: 41&view=mrkup. Siegwrt, R. nd Nourbkhsh, I. R. (004). Autonomous Mobile Robots. MIT Press. Silv, V., Sntos, F., et l. (00). A Robust Self-Locliztion System for Smll Mobile Autonomous Robot. Proceedings of the 3rd ANIRob/IEEE Int. Symposium on Robotics nd Automtion (ISRA 00). Toluc, Mexico. Singh, S. P. N. nd Wldron, K. J. (004). Design nd Evlution of n Integrted Plnr Locliztion Method for Desktop Robotics. Proceedings of the IEEE Interntionl Conference on Robotics nd Automtion, 004 (ICRA 004), New Orlens, LA, USA. Slotine, J.-J. nd Li, W. (1991). Applied Nonliner Control. Prentice Hll. Spong, M. W., Hutchinson, S., et l. (006). Robot Modeling nd Control. John Wiley & Sons, Hoboken, NJ. Tn, K. K., Wng, Q.-G., et l. (1999). Advnces in PID Control. Springer-Verlg. Tn, W., Liu, J., et l. (006). Comprison of Some Well-Known PID Tuning Formuls. Computers & Chemicl Engineering 30(9): Teo, C. M. (006). Understnding Opticl Mice. Avgo Technologies. From: Thkoor, S., Morookin, J. M., et l. (004). BEES: Exploring Mrs with Bioinspired Technologies. Computer 37(9): Thnjvur, K. nd Rjgopln, R. (1997). Ese of Dynmic Modelling of Wheeled Mobile Robots (WMRs) using Kne's Approch. Proceedings of the IEEE Interntionl Conference on Robotics nd Automtion, Tuley, J., Vndpel, N., et l. (005). Anlysis nd Removl of Artifcts in 3-D LADAR Dt. Proceedings of the IEEE Interntionl Conference on Robotics nd Automtion, 005: USAF (008). GPS Stndrd Positioning Service (SPS) Performnce Stndrd 4th Edition. United Sttes Air Force Globl Positioning Systems Directorte. From: Wtnbe, R. (007). NXT Motor. From: Holidy/NXT Motor.html. Ymmoto, Y. (009, 8 Apr). NXT Bllbot (Self-Blncing Robot On A Bll) Controller Design. From: Ymmoto, Y. (009b, 1 My). NXTwy-GS (Self-Blncing Two-Wheeled Robot) Controller Design. From: Ytsenko, L. P., Shore, B. W., et l. (004). Rnging nd Interferometry with Frequency Shifted Feedbck Lser. Optics Communictions 4(4 6): Yoshid, K. nd Hirose, S. (1988). Lser Tringultion Rnge Finder Avilble under Direct Sunlight. Proceedings of the IEEE Interntionl Conference on Robotics nd Automtion, Zhng, Y., Chung, J. H., et l. (003). Vrible Structure Control of Differentilly Steered Wheeled Mobile Robot. Journl of Intelligent nd Robotic Systems 36(3): Zhng, Y., Hong, D., et l. (1998). Dynmic Model Bsed Robust Trcking Control of Differentilly Steered Wheeled Mobile Robot. Proceedings of the Americn Control Conference, Zho, Y. nd BeMent, S. L. (199). Kinemtics, Dynmics nd Control of Wheeled Mobile Robots. Proceedings of the IEEE Interntionl Conference on Robotics nd Automtion, 199. Zhou, K. nd Doyle, J. C. (1998). Essentils Of Robust Control. Prentice Hll. 195

212 Fig. A.1 Adptive Trcking Control System of the WMR Appendix A A.1 Enlrged version of Adptive Model of WMR (Fig. 7.1) 196

213 A. Embedded MATLAB Code for Trjectory Genertor Trjectory Genertor function block s seen in Fig. 4. of Section nd Fig.7. of Section function [x, y] = ref_pth(t) %Reference trjectory % **** Stright line **** x = 0. * t; y = 0; % y = 0.5; % y = 0.5 * x + 0.5; %{ % **** Sinusoidl wveform **** x = 0. * t; y = sin(x); % y = * sin( * x); %} %{ % **** Tringulr wveform **** x = 0.05 * t; period = ; phse = 0; rtio = 0.5; mp = ; u1 = rem(x + phse * period / ( * pi) + period, period); y = (u1 <= period * rtio).* u1 * mp / (period * rtio) + (u1 > period * rtio)....* (period - u1) * mp / (period * (1 - rtio)); %} %{ % **** Circulr wveform **** % Note: Rtio of dimeter to chord is /pi x_offset = ; y_offset = 0; r = ; d = * r; x = mod((0.1 * t), ( * d)) + (x_offset - r); if x > (d + x_offset - r) x = * (d + x_offset - r) - x; y = y_offset - sqrt(r^ - (x - x_offset)^); else y = y_offset + sqrt(r^ - (x - x_offset)^); end %} %{ % **** Squre trjectory **** t_fctor = 0.; length = 4; t_length = length / t_fctor; x_offset = 0; y_offset = 0; if (t <= t_length) x = t_fctor * t + x_offset; y = 0 + y_offset; elseif ((t > t_length) && (t <= ( * t_length))) x = length + x_offset; y = t_fctor * (t - t_length) + y_offset; elseif ((t > * t_length) && (t <= 3 * t_length)) x = length - t_fctor * (t - * t_length) + x_offset; y = length + y_offset; elseif ((t > 3 * t_length) && (t <= 4 * t_length)) 197

214 x = 0 + x_offset; y = length - t_fctor * (t - 3 * t_length) + y_offset; else x = 0 + x_offset; y = 0 + y_offset; end %} %{ % **** Squre trjectory with chmfer **** t_fctor = 0.; length = 4; t_length = length / t_fctor; x_offset = 0; y_offset = 0; cnr_offset = 0.04; t_cnr = cnr_offset / t_fctor; if (t <= (t_length - t_cnr)) x = t_fctor * t + x_offset; y = 0 + y_offset; elseif ((t > (t_length - t_cnr)) && (t <= t_length)) x = t_fctor * t + x_offset; % y = x - (t_fctor * (t_length - t_cnr) + y_offset); y = x - length + cnr_offset + y_offset; elseif ((t > t_length) && (t <= ( * (t_length - t_cnr)))) x = length + x_offset; % y = t_fctor * (t - t_length + t_cnr) + y_offset; y = t_fctor * t - length + cnr_offset + y_offset; elseif ((t > ( * (t_length - t_cnr))) && (t <= ( * t_length - t_cnr))) % y = t_fctor * (t - t_length + t_cnr) + y_offset; y = t_fctor * t - length + cnr_offset + y_offset; x = length - (y - (length - cnr_offset + y_offset)) + x_offset; elseif ((t > ( * t_length - t_cnr)) && (t <= (3 * (t_length - t_cnr)))) x = length - cnr_offset - (t_fctor * t - ( * length - cnr_offset)) + x_offset; y = length + y_offset; elseif ((t > (3 * (t_length - t_cnr))) && (t <= (t_length + * (t_length - t_cnr)))) x = length - cnr_offset - (t_fctor * t - ( * length - cnr_offset)) + x_offset; y = x + (length - cnr_offset) + y_offset; elseif ((t > (t_length + * (t_length - t_cnr))) && (t <= (t_length + 3 * (t_length... - t_cnr)))) x = 0 + x_offset; y = length - cnr_offset - (t_fctor * t - (3 * length - * cnr_offset)) + y_offset; else x = 0 + x_offset; y = 0 + y_offset; end %} 198

215 A.3 Embedded MATLAB Code for About-Turn Algorithm About-Turn function block s seen in Fig. 4.4 of Sections nd function [w_dj, thet] = turn(w, err, pos, cons, wmx) % Algorithm for resolving reverse motion nd sturtion % Initilistion of vribles xq = 0; yq = 0; xb = 0; yb = 0; xl = 0; yl = 0; xr = 0; yr = 0; xp = 0; yp = 0; x = 0; y = 0; coeff1 = 0; coeff = 0; coeff3 = 0; eqn = [0, 0, 0]; xt = [0 + 0i; 0 + 0i]; x1 = 0; y1 = 0; x = 0; y = 0; ch1 = 0; ch = 0; xtn1 = 0; ytn1 = 0; xtn = 0; ytn = 0; norm1 = [0; 0]; norm = [0; 0]; norm3 = [0; 0]; unit_norm1 = [0; 0]; unit_norm = [0; 0]; unit_norm3 = [0; 0]; ng_normls1 = 0; ng_normls = 0; ng_normls3 = 0; wheel_vect = [0; 0]; unit_wheel_vect = [0; 0]; ng_disp = 0; circ_disp = 0; thet_l = 0; thet_r = 0; omeg_l = 0; omeg_r = 0; % Wheel ngulr velocity [rd/s] % omeg_l = w(1); % omeg_r = w(); % Positionl error [m] del_x = err(1); del_y = err(); % Current sensor position [m] x = pos(1); y = pos(); 199

216 % Current orienttion [rd] psi = pos(3); % Wheel rdius [m] r = cons(1); % Distnce between wheels [m] d = cons(); % Distnce between sensor nd centre of wheel bseline [m] = cons(3); % Mximum ngulr velocity of motor [rd/s] omeg_mx = wmx; % **** Mthemticl ide-memoire [Strt] **** % Eqution of circle with centre t (xp, yp) % (x - xp)^ + (y - yp)^ = rs^ % y = yp + sqrt(rs^ - (x - xp)^) % Derivtive % dy/dx = -(x -xp) / (y - yp) % Arc length % s = int(sqrt(1 + (dy/dx)^), x) % Eqution of tngent % y - y1 = dy/dx * (x - x1) % **** Mthemticl ide-memoire [End] **** if (w(1) < 0) && (w() < 0) % Turns WMR round using one wheel s sttionry pivot % Rdius of circle (distnce between pivot wheel & bseline centre [m] rs = d / ; % Reference sensor position [m] xq = x + del_x; yq = y + del_y; % Determintion of position of wheel bseline centre [m] % tn(psi) = (y - yb) / (x - xb) % ((x - xb)^ + (y - yb)^ = ^ xb = x - * cos(psi); yb = y - * sin(psi); % Determintion of positions of wheels [m] xl = xb + rs * cos(psi + pi /); yl = yb + rs * sin(psi + pi /); xr = xb + rs * cos(psi - pi /); yr = yb + rs * sin(psi - pi /); % Determintion of positions of wheels [m] (Alterntive) % if (psi >= 0) && (psi < pi / ) % xl = xb - rs * (-cos(psi + pi / )); % yl = yb + rs * (sin(psi + pi / )); % xr = xb + rs * (cos(psi - pi / )); % yr = yb - rs * (-sin(psi - pi / )); % elseif (psi >= pi / ) && (psi < pi) % xl = xb - rs * (-cos(psi + pi / )); % yl = yb - rs * (-sin(psi + pi / )); % xr = xb + rs * (cos(psi - pi / )); % yr = yb + rs * (sin(psi - pi / )); 00

217 % elseif (psi >= pi) && (psi < 3 / * pi) % xl = xb + rs * (cos(psi + pi / )); % yl = yb - rs * (-sin(psi + pi / )); % xr = xb - rs * (-cos(psi - pi / )); % yr = yb + rs * (sin(psi - pi / )); % else % xl = xb + rs * (cos(psi + pi / )); % yl = yb + rs * (sin(psi + pi / )); % xr = xb - rs * (-cos(psi - pi / )); % yr = yb - rs * (-sin(psi - pi / )); % end % Determintion of pivoting wheel if bs(w(1)) <= bs(w()) xp = xl; yp = yl; else xp = xr; yp = yr; end % Points of intersection of tngent nd circle [m] x = xq; y = yq; co1 = (x - xp)^ + (y - yp)^; co = - * rs^ * (x - xp) - * xp * (x - xp)^ - * xp * (y - yp)^; co3 = xp^ * ((x - xp)^ + (y - yp)^) + rs^ * ( * xp * (x - xp)... - (y - yp)^) + rs^4; eqn = [co1, co, co3]; xt = roots(eqn); x1 = rel(xt(1)); x = rel(xt()); y1 = yp + sqrt(rs^ - (x1 - xp)^); y = yp + sqrt(rs^ - (x - xp)^); % Chord length [m] ch1 = sqrt((x1 - xb)^ + (y1 - yb)^); ch = sqrt((x - xb)^ + (y - yb)^); % Determintion of trget tngent point if ch1 > ch xtn1 = x1; ytn1 = y1; xtn = x; ytn = y; else xtn1 = x; ytn1 = y; xtn = x1; ytn = y1; end % Norml vectors from centre of circle to tngent points norm1 = [(xtn1 - xp); (ytn1 - yp)]; norm = [(xtn - xp); (ytn - yp)]; % Angle between norml vectors (the lrger ngle > 180 deg) % Note: The complex function is used for fixing MATLAB precision errors ng_normls1 = bs(rel(cos(complex(dot(norm1, norm)./... (norm(norm1).* norm(norm)))))); ng_normls1 = * pi - ng_normls1; % Norml vector bisecting ngle between two other norml vectors 01

218 unit_norm1 = norm1./ (norm(norm1)); unit_norm = norm./ (norm(norm)); norm3 = -(unit_norm1 + unit_norm); unit_norm3 = norm3./ (norm(norm3)); % Vector long wheel bseline (orthogonl to orienttion) wheel_vect = [(xb - xp); (yb - yp)]; unit_wheel_vect = wheel_vect./ (norm(wheel_vect)); % Angle between first norml vector nd bisecting norml vector ng_normls = ng_normls1 / ; % Angle between wheel vector nd bisecting norml vector % (Note: Both re unit vectors, so no need to divide by mgnitude) ng_normls3 = bs(cos(dot(unit_wheel_vect, unit_norm3))); % Totl ngulr displcement of sensor [rd] ng_disp = ng_normls + ng_normls3; % Circumferentil displcement of outer wheel [m] circ_disp = d * ng_disp; % Angulr displcement of wheels [rd] if bs(w(1)) <= bs(w()) thet_l = 0; thet_r = circ_disp / r; else thet_l = circ_disp / r; thet_r = 0; end % Angulr velocity of wheels (clculted outside function) omeg_l = 0; omeg_r = 0; elseif (w(1) < 0) && (w() >= 0) yw_vel = r / d * (w() - w(1)); omeg_l = 0; omeg_r = omeg_l + d * yw_vel / r; if omeg_r > omeg_mx omeg_r = omeg_mx; end thet_l = 0; % Angulr displcement of wheel not clculted here thet_r = 0; % Angulr displcement of wheel not clculted here elseif (w(1) >= 0) && (w() < 0) yw_vel = r / d * (w() - w(1)); omeg_r = 0; omeg_l = omeg_r - d * yw_vel / r; if omeg_l > omeg_mx omeg_l = omeg_mx; end thet_l = 0; % Angulr displcement of wheel not clculted here thet_r = 0; % Angulr displcement of wheel not clculted here elseif (w(1) >= 0) && (w() >= 0) if (w(1) > omeg_mx) (w() > omeg_mx) yw_vel = r / d * (w() - w(1)); if w(1) <= w() omeg_r = omeg_mx; omeg_l = omeg_r - d * yw_vel / r; else omeg_l = omeg_mx; omeg_r = omeg_l + d * yw_vel / r; end else omeg_l = w(1); omeg_r = w(); 0

219 end end thet_l = 0; thet_r = 0; % Angulr displcement of wheel not clculted here % Angulr displcement of wheel not clculted here % Output w_dj = [omeg_l, omeg_r]; thet = [thet_l, thet_r]; 03

220 A.4 Simulink S-Function Code for Symmetric Positive Definite Mtrix Symmetric Positive Definite Mtrix function block s seen in Fig. 7.8 of Sections nd Note: Simulink code supplied by MthWorks nd modified by Loren Yeo for the project function spd_mt(block) % Stbility nlysis using Lypunov function cndidte % Determintion of symmetric positive definite (SPD) mtrix % Defult Simulink S-Function code by MthWorks modified by Loren Yeo where stted %% %% The setup method is used to setup the bsic ttributes of the %% S-function such s ports, prmeters, etc. Do not dd ny other %% clls to the min body of the function. %% setup(block); %endfunction %% Function: setup =================================================== %% Abstrct: %% Set up the S-function block's bsic chrcteristics such s: %% - Input ports %% - Output ports %% - Dilog prmeters %% - Options %% %% Required : Yes %% C-Mex counterprt: mdlinitilizesizes %% function setup(block) % Register number of ports block.numinputports = 1; block.numoutputports = 1; % Setup port properties to be inherited or dynmic block.setprecompinpportinfotodynmic; block.setprecompoutportinfotodynmic; % Override input port properties block.inputport(1).dimensions = ; block.inputport(1).dttypeid = 0; % double block.inputport(1).complexity = 'Rel'; block.inputport(1).directfeedthrough = true; % Override output port properties block.outputport(1).dimensions = [4 4]; block.outputport(1).dttypeid = 0; % double block.outputport(1).complexity = 'Rel'; % Register prmeters block.numdilogprms = 0; % Register smple times % [0 offset] : Continuous smple time % [positive_num offset] : Discrete smple time % % [-1, 0] : Inherited smple time % [-, 0] : Vrible smple time block.smpletimes = [-1 0]; 04

221 %% %% The M-file S-function uses n internl registry for ll %% block methods. You should register ll relevnt methods %% (optionl nd required) s illustrted below. You my choose %% ny suitble nme for the methods nd implement these methods %% s locl functions within the sme file. See comments %% provided for ech function for more informtion. %% % Required % Required %end setup %% %% PostPropgtionSetup: %% Functionlity : Setup work res nd stte vribles. Cn %% lso register run-time methods here %% Required : No %% C-Mex counterprt: mdlsetworkwidths %% function DoPostPropSetup(block) block.numdworks = 1; block.dwork(1).nme = 'x1'; block.dwork(1).dimensions = 1; block.dwork(1).dttypeid = 0; % double block.dwork(1).complexity = 'Rel'; % rel block.dwork(1).usedasdiscstte = true; %% %% InitilizeConditions: %% Functionlity : Clled t the strt of simultion nd if it is %% present in n enbled subsystem configured to reset %% sttes, it will be clled when the enbled subsystem %% restrts execution to reset the sttes. %% Required : No %% C-MEX counterprt: mdlinitilizeconditions %% function InitilizeConditions(block) %end InitilizeConditions %% %% Strt: %% Functionlity : Clled once t strt of model execution. If you %% hve sttes tht should be initilized once, this %% is the plce to do it. %% Required : No %% C-MEX counterprt: mdlstrt %% function Strt(block) block.dwork(1).dt = 0; %endfunction %% %% Outputs: 05

222 %% Functionlity : Clled to generte block outputs in %% simultion step %% Required : Yes %% C-MEX counterprt: mdloutputs %% function Outputs(block) % **** Custom Code by Loren Yeo (Strt) **** % **** Aide-Memoire **** % For liner system of the form % dx = Ax % or x_dot = Ax % Consider qudrtic Lypunov function cndidte % V = x'px % where P is symmetric positive definite mtrix % Tking the derivtive % dv = dx'px + x'pdx = -x'qx % where A'P + PA = -Q % For the system to be strictly stble, P must be SPD if Q is SPD % ********************** % Position gin (lmb^) Kp = block.inputport(1).dt(1); % Velocity gin ( * lmbd) Kv = block.inputport(1).dt(); % Hurwitz gin mtrix A = [zeros() eye(); -Kp*eye() -Kv*eye()]; % For computtionl simplicity, let Q = I Q = eye(4); % Output P = lyp(a', Q); block.outputport(1).dt = P; % **** Custom Code by Loren Yeo (End) **** %end Outputs %% %% Updte: %% Functionlity : Clled to updte discrete sttes %% during simultion step %% Required : No %% C-MEX counterprt: mdlupdte %% function Updte(block) % block.dwork(1).dt = block.inputport(1).dt; %end Updte %% %% Derivtives: %% Functionlity : Clled to updte derivtives of %% continuous sttes during simultion step %% Required : No %% C-MEX counterprt: mdlderivtives 06

223 %% function Derivtives(block) %end Derivtives %% %% Terminte: %% Functionlity : Clled t the end of simultion for clenup %% Required : Yes %% C-MEX counterprt: mdlterminte %% function Terminte(block) %end Terminte 07

224 A.5 Embedded MATLAB Code for Pck function in System Interfce Subsystem Pck function block s seen in Fig of Section 7.4. function out = pck(in) % First define the size of the pcket % using nullcopy to void generting unnecessry code out = eml.nullcopy(zeros(6, 1, 'int16')); % IC mster hs driver flw tht prevents received unsigned integer % vlue from exceeding 17 (mximum 7-bit unsigned integer vlue is 17) % Workround: Send 7 bits t time insted of 8 bits % Buffer is limited to combined 14-bit mxiumum unsigned vlue of % Mximum buffer vlue is scertined to not exceed % Therefore, two highest-order bits re unused nd cn be discrded %{ % Sensor 1 sensor1x = typecst(in(1), 'uint8'); % Lower 7-bit byte of X-coordinte sensor1xb = typecst(in(), 'uint8'); % Upper 7-bit byte of X-coordinte sensor1y = typecst(in(3), 'uint8'); % Lower 7-bit byte of Y-coordinte sensor1yb = typecst(in(4), 'uint8'); % Upper 7-bit byte of Y-coordinte % Sensor sensorx = typecst(in(5), 'uint8'); % Lower 7-bit byte of X-coordinte sensorxb = typecst(in(6), 'uint8'); % Upper 7-bit byte of X-coordinte sensory = typecst(in(7), 'uint8'); % Lower 7-bit byte of Y-coordinte sensoryb = typecst(in(8), 'uint8'); % Upper 7-bit byte of Y-coordinte %} % Sensor 1 sensor1x = in(1); sensor1xb = in(); sensor1y = in(3); sensor1yb = in(4); sensor1sq = in(5); sensor1sqb = in(6); % Sensor sensorx = in(7); sensorxb = in(8); sensory = in(9); sensoryb = in(10); sensorsq = in(11); sensorsqb = in(1); % Lower 7-bit byte of X-coordinte % Upper 7-bit byte of X-coordinte % Lower 7-bit byte of Y-coordinte % Upper 7-bit byte of Y-coordinte % Lower 7-bit byte of surfce qulity % Upper 7-bit byte of surfce qulity % Lower 7-bit byte of X-coordinte % Upper 7-bit byte of X-coordinte % Lower 7-bit byte of Y-coordinte % Upper 7-bit byte of Y-coordinte % Lower 7-bit byte of surfce qulity % Upper 7-bit byte of surfce qulity % Recst type for conctention of upper nd lower bytes sensor1x = cst(sensor1x, 'uint16'); sensor1xb = cst(sensor1xb, 'uint16'); sensor1y = cst(sensor1y, 'uint16'); sensor1yb = cst(sensor1yb, 'uint16'); sensor1sq = cst(sensor1sq, 'uint16'); sensor1sqb = cst(sensor1sqb, 'uint16'); sensorx = cst(sensorx, 'uint16'); sensorxb = cst(sensorxb, 'uint16'); sensory = cst(sensory, 'uint16'); sensoryb = cst(sensoryb, 'uint16'); sensorsq = cst(sensorsq, 'uint16'); sensorsqb = cst(sensorsqb, 'uint16'); % Bit shift left logicl for upper 7-bit bytes sensor1xb = bitsll(sensor1xb, 7); sensor1yb = bitsll(sensor1yb, 7); 08

225 sensor1sqb = bitsll(sensor1sqb, 7); sensorxb = bitsll(sensorxb, 7); sensoryb = bitsll(sensoryb, 7); sensorsqb = bitsll(sensorsqb, 7); % Bit-wise OR to combine upper nd lower 7-bit bytes sensor1x_uint14 = bitor(sensor1x, sensor1xb); sensor1y_uint14 = bitor(sensor1y, sensor1yb); sensor1sq_uint14 = bitor(sensor1sq, sensor1sqb); sensorx_uint14 = bitor(sensorx, sensorxb); sensory_uint14 = bitor(sensory, sensoryb); sensorsq_uint14 = bitor(sensorsq, sensorsqb); % Convert to signed 16-bit integer if sensor1x_uint14 >= 819 sensor1x = int16(sensor1x_uint14) ; else sensor1x = int16(sensor1x_uint14); end if sensor1y_uint14 >= 819 sensor1y = int16(sensor1y_uint14) ; else sensor1y = int16(sensor1y_uint14); end if sensorx_uint14 >= 819 sensorx = int16(sensorx_uint14) ; else sensorx = int16(sensorx_uint14); end if sensory_uint14 >= 819 sensory = int16(sensory_uint14) ; else sensory = int16(sensory_uint14); end % Coordinte system of vehicle hs xes swpped with respect to sensor % Vehicle xes re 90 degrees clockwise with respect to sensor xes sensor1x_veh = sensor1y; sensor1y_veh = -sensor1x; sensor1sq = sensor1sq_uint14; sensorx_veh = sensory; sensory_veh = -sensorx; sensorsq = sensorsq_uint14; out = [sensor1x_veh sensor1y_veh sensor1sq sensorx_veh sensory_veh... sensorsq]'; 09

226 A.6 Embedded MATLAB Code for c1 function in Hybrid Geometric Loclistion Subsystem c1 function block s seen in Fig of Section function dx1 = c1(r1, dx1, dxa, dya, dj_1) %#eml r_crit = 6.8e10; if bs(r1) > r_crit dx1 = (dxa * cos(dj_1)) - (dya * sin(dj_1)); else dx1 = dx1; end A.7 Embedded MATLAB Code for c function in Hybrid Geometric Loclistion Subsystem c function block s seen in Fig of Section function dy1 = c(r1, dy1, dxa, dya, dj_1) %#eml r_crit = 6.8e10; if bs(r1) > r_crit dy1 = (dxa * sin(dj_1)) + (dya * cos(dj_1)); else dy1 = dy1; end A.8 Embedded MATLAB Code for c3 function in Hybrid Geometric Loclistion Subsystem c3 function block s seen in Fig of Section function dx = c3(r, dx, dxk, dyk, dj_) %#eml r_crit = 6.8e10; if bs(r) > r_crit dx = (dxk * cos(dj_)) - (dyk * sin(dj_)); else dx = dx; end A.9 Embedded MATLAB Code for c4 function in Hybrid Geometric Loclistion Subsystem c4 function block s seen in Fig of Section function dy = c4(r, dy, dxk, dyk, dj_) %#eml 10

227 r_crit = 6.8e10; if bs(r) > r_crit dy = (dxk * sin(dj_)) + (dyk * cos(dj_)); else dy = dy; end A.10 Embedded MATLAB Code for c5 function in Hybrid Geometric Loclistion Subsystem c5 function block s seen in Fig of Section function dx = c5(r, psi, dx, dxk, dyk, dj_) %#eml r_crit = 6.8e10; if bs(r) > r_crit dx = (dxk * cos(psi + dj_)) - (dyk * sin(psi + dj_)); else dx = dx; end A.11 Embedded MATLAB Code for c6 function in Hybrid Geometric Loclistion Subsystem c6 function block s seen in Fig of Section function dy = c6(r, psi, dy, dxk, dyk, dj_) %#eml r_crit = 6.8e10; if bs(r) > r_crit dy = (dxk * sin(psi + dj_)) + (dyk * cos(psi + dj_)); else dy = dy; end A.1 Embedded MATLAB Code for cpsi function in Hybrid Geometric Loclistion Subsystem cpsi function block s seen in Fig of Section function dpsi = cpsi(dpsi) %#eml psi_min = 4e-4; if bs(dpsi) < psi_min dpsi = 0; else dpsi = dpsi; end 11

228 Appendix B B.1 Code for Atmel ATmeg3 Microcontroller - MouseController.c Microcontroller progrm pertining to Sections to /**** Aide Memoire for Atmel AVR ATmeg3 Microcontroller ***** ** Bit-msking nd bit-shifting in registers ** Set bit to high (1): PORTB = PORTB (1 << bit position); PORTB = (1 << bit position); Reset bit to low (0): PORTB = PORTB & ~(1 << bit position); PORTB &= ~(1 << bit position); Flip bit vlue: PORTB = PORTB ^ (1 << bit position); PORTB ^= (1 << bit position); Multiple settings 0 nd 0: PORTB &= ~((1<<PORTB0) (1<<PORTB1)); 0 nd 1: PORTB = (PORTB & ~(1<<PORTB1)) (1<<PORTB0); 1 nd 0: PORTB = (PORTB & ~(1<<PORTB0)) (1<<PORTB1); 1 nd 1: PORTB = (1<<PORTB0) (1<<PORTB1); ** Port B (SPI) ** * Port B Register * PINB (Input pins): Red-only. Register where dt is red from pins DDRB (Dt direction): Set to high to configure pin s output nd low to configure it s input PORTB (Dt): Register where dt is written to pins * Port B Pin Assignment * PB7: SCK (SPI Bus Seril Clock) PB6: MISO (SPI Bus Mster Input/Slve Output) PB5: MOSI (SPI Bus Mster Output/Slve Input) PB4: SS' (SPI Slve Select Input) PB3: AIN1 (Anlog Comprtor Negtive Input) / OC0 (Timer/Counter0 Output Compre Mtch Output) PB: AIN0 (Anlog Comprtor Positive Input) / INT (Externl Interrupt Input) PB1: T1 (Timer/Counter1 Externl Counter Input) PB0: T0 (Timer/Counter0 Externl Counter Input) / XCK (USART Externl Clock Input/Output) * SPI Control Register (SPCR) * Bit 7 - SPIE (SPI Interrupt Enble): Set to high for interrupt to be executed when seril trnsfer is completed Bit 6 - SPE (SPI Enble): Set to high to enble SPI Bit 5 - DORD (Dt Order): Set to high to send LSB first nd low to send MSB first Bit 4 - MSTR (Mster/Slve Select): Set to high to configure AVR s mster nd low to configure it s slve Bit 3 - CPOL (Clock Polrity): If set to high, SCK is high when idle; if set to low, SCK is low when idle Bit - CPHA (Clock Phse): Set to high to smple dt on triling edge nd low to smple dt on leding edge Bits 1 nd 0 - SPR1 nd SPR0 (SPI Clock Rte Select): Set frequency of clock signl SPR1; SPR0: SCK frequency 0; 0: fosc/4 0; 1: fosc/16 1; 0: fosc/64 1; 1: fosc/18 where fosc: oscilltor frequency ( MHz) CPOL; CPHA: Smpling point 1

229 0; 0: Leding (Rising) Edge 0; 1: Triling (Flling) Edge 1; 0: Leding (Flling) Edge 1; 1: Triling (Rising) Edge * SPI Sttus Register (SPSR) * Bit 7 - SPIF (SPI Interrupt Flg): Red-only. Set when dt trnsfer is complete nd clered when interrupt is executed Bit 6 - WCOL (Write Colision Flg): Red-only. Set if the SPDR register is written to during dt trnsfer Bit 0 - SPIx (Double SPI Speed): Set to high to double SCK frequency when in mster mode * SPI Dt Register (SPDR) * Bits 7 to 0 - MSB to LSB: Writing initites trnsmission; reding cuses Receive Shift Register buffer to be red ** Port C (IC/TWI) ** * TWI Bit Rte Register (TWBR) * Bits 7 to 0 - TWBR7 to TWBR0 (TWI Bit Rte Register): Set SCL clock frequency long with TWPS in mster mode * TWI Control Register (TWCR) * Bit 7 - TWINT (TWI Interrupt Flg): Set by hrdwre when trnsmission is complete; flg clered by setting to high Bit 6 - TWEA (TWI Enble Acknowledge Bit): Set to high to enble ACK pulse when slve ddress or dt is received Bit 5 - TWSTA (TWI START Condition Bit): Set to high for device to become mster Bit 4 - TWSTO (TWI STOP Condition Bit): Set to high to generte STOP condition in mster or for error recovery in slve Bit 3 - TWWC (TWI Write Collision Flg): Red-only. Set when ttempting to write to TWDR when TWINT is low Bit - TWEN T(WI Enble Bit): Set to high to enble TWI opertion nd ctivte TWI interfce Bit 1 - Res (Reserved Bit): Red-only. Alwys set to low Bit 0 - TWIE (TWI Interrupt Enble): Set to high to enble interrupt request * TWI Sttus Register (TWSR) * Bits 7 to 3 - TWS7 to TWS3 (TWI Sttus): Red-only. Contin codes tht reflect sttus of the TWI logic Bit - Res (Reserved Bit): Red-only. Alwys set to low Bits 1 nd 0 - TWPS1 nd TWPS0 (TWI Prescler Bits): Set SCL clock frequency long with TWBR in mster mode * TWI Dt Register (TWDR) * Bits 7 to 0 - TWD7 to TWD0 (TWI Dt Register): Contin next byte to be trnsmitted or lst byte received * TWI (Slve) Address Register (TWAR) * Bits 7 to 1 - TWA6 to TWA0 (TWI (Slve) Address Register): Contin 7-bit slve ddress Bit 0 - TWGCE (TWI Generl Cll Recognition Enble Bit): Set to high to enble recognition of Generl Cll ** Port D (USART) ** * USART Control nd Sttus Register A (UCSRA) * Bit 7 - RXC (USART Receive Complete): Red-only. Set when there is unred dt in receive buffer nd clered when empty Bit 6 - TXC (USART Trnsmit Complete): Set when trnsmit shift register nd trnsmit buffer (UDR) re empty Bit 5 - UDRE (USART Dt Register Empty): Red-only. Set if trnsmit buffer (UDR) is empty nd redy to be written Bit 4 - FE (Frme Error): Red-only. Set when first stop bit of the next chrcter in receive buffer is zero Bit 3 - DOR (Dt OverRun): Red-only. Set when the receive buffer is full (two chrcters) Bit - PE (Prity Error): Red-only. Set if next chrcter in the receive buffer hd prity error when received Bit 1 - UX (Double the USART Trnsmission Speed): For synchronous opertion only 13

230 Bit 0 - MPCM (Multi-processor Communiction Mode) : Set to high to ignore incoming frmes without ddress informtion * USART Control nd Sttus Register B (UCSRB) * Bit 7 - RXCIE (RX Complete Interrupt Enble): Set to high to enble interrupt on the RXC Flg Bit 6 - TXCIE (TX Complete Interrupt Enble): Set to high to enble interrupt on the TXC Flg Bit 5 - UDRIE (USART Dt Register Empty Interrupt Enble): Set to high to enble interrupt on the UDRE Flg Bit 4 - RXEN (Receiver Enble): Set to high to enble the USART receiver Bit 3 - TXEN (Trnsmitter Enble): Set to high to enble the USART trnsmitter Bit - UCSZ (Chrcter Size): Sets the number of dt bits in frme the receiver nd trnsmitter use Bit 1 - RXB8 (Receive Dt Bit 8): Red-only. RXB8 is the ninth dt bit of the received chrcter Bit 0 - TXB8 (Trnsmit Dt Bit 8): TXB8 is the ninth dt bit in the chrcter to be trnsmitted * USART Control nd Sttus Register C (UCSRC) * Bit 7 - URSEL (Register Select): Set to high to ccess UCSRC nd low to ccess UBRRH (USART Bud Rte Register - High) Bit 6 - UMSEL (USART Mode Select): Set to high for synchronous opertion nd low for synchronous opertion Bit 5 - UPM1 (Prity Mode): Enble nd set type of prity genertion nd check Bit 4 - UPM0 (Prity Mode): Enble nd set type of prity genertion nd check Bit 3 - USBS (Stop Bit Select): Set to high for two stop bits to be inserted by trnsmitter nd low for one stop bit Bit - UCSZ1 (Chrcter Size): Sets the number of dt bits in frme the receiver nd trnsmitter use Bit 1 - UCSZ0 (Chrcter Size): Sets the number of dt bits in frme the receiver nd trnsmitter use Bit 0 - UCPOL (Clock Polrity): Sets the reltionship between output chnge, input smple nd synchronous clock (XCK) UPM1; UPM0: Prity Mode 0; 0: Disbled 0; 1: Reserved 1; 0: Enbled, Even Prity 1; 1: Enbled, Odd Prity UCSZ; UCSZ1; UCSZ0: Chrcter Size 0; 0; 0: 5-bit 0; 0; 1: 6-bit 0; 1; 0: 7-bit 0; 1; 1: 8-bit 1; 0; 0: Reserved 1; 0; 1: Reserved 1; 1; 0: Reserved 1; 1; 1: 9-bit UCPOL: Trnsmitted Dt Chnged (Output of TxD Pin); Received Dt Smpled (Input on RxD Pin) 0: Rising XCK Edge; Flling XCK Edge 1: Flling XCK Edge; Rising XCK Edge **** End of Aide Memoire ****/ // #include <io.h> #include <stdio.h> #include <stdint.h> #include <stdlib.h> #include <ctype.h> #include <string.h> #include <vr/io.h> #include <vr/common.h> #include <vr/interrupt.h> #include <vr/pgmspce.h> #include <vr/sfr_defs.h> #include <vr/iom3.h> #include <vr/portpins.h> #include <util/tomic.h> #include <util/dely.h> #include <util/twi.h> 14

231 #include "spi.h" #include "twi_slve.h" #include "conf/globl.h" #define fosc UL frequency in Hz // #define F_CPU UL #define usrt_bud_rte second #define ubbr_vlue (((fosc / (usrt_bud_rte * 16UL))) - 1) vlue (UL: unsigned long int) // Crystl oscilltor // USART bud rte in bits per // USART Bud Rte Register sttic int USART_putchr(unsigned chr c, FILE *strem); // Sends chr/string to USART port // sttic int USART_putchr(chr c, FILE *strem); // Sends chr/string to USART port sttic FILE usrt_out = FDEV_SETUP_STREAM(USART_putchr, NULL, _FDEV_SETUP_WRITE); // Set up buffer s n stdio strem /* extern voltile unsigned int ISR_sttus; extern voltile unsigned int ISR_sttus_1; extern voltile unsigned int ISR_sttus_; extern voltile unsigned int ISR_sttus_3; extern voltile unsigned int ISR_sttus_4; extern voltile unsigned int ISR_sttus_5; extern voltile unsigned int ISR_sttus_6; extern voltile unsigned int ISR_sttus_7; extern voltile unsigned int ISR_sttus_8; extern voltile unsigned int ISR_sttus_9; extern voltile unsigned int ISR_sttus_10; extern voltile unsigned int ISR_sttus_11; extern voltile unsigned int ISR_sttus_1; */ // ISR sttus flg // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter extern voltile unsigned int TWI_buf_out[]; unsigned int TWI_slveAddress = 0x01; unsigned int TWI_reg_ddr = 0x44; sttic int buffer_x_1 = 0; sttic int buffer_y_1 = 0; sttic int buffer_x_ = 0; sttic int buffer_y_ = 0; sttic int buffer_squl_1 = 0; sttic int buffer_squl_ = 0; // sttic unsigned int counter = 1; // Own TWI slve ddress // Slve register ddress specified by TWI mster // Initilise USART port on microcontroller void USART_init(void) { UCSRB = ((1<<RXEN) (1<<TXEN)); UCSRC = ((1<<URSEL) (1<<UCSZ1) (1<<UCSZ0)); } UBRRL = ubbr_vlue; UBRRH = (ubbr_vlue>>8); // Enble USART receiver nd trnsmitter // Enble ccess to UCSRC nd configure // 8-bit chrcter size // Sends chrcter/string to USART port int USART_putchr(unsigned chr text, FILE *strem) // int USART_putchr(chr text, FILE *strem) { while ((UCSRA & (1<<UDRE)) == 0); UDR = text; return 0; } // Initilise SPI port in mster mode on microcontroller void SPI_mster_init(void) { // PORTB = 0x00; // Set ll vlues to low on Port B for initilistion DDRB = ((1<<PB7) (1<<PB5) (1<<PB4) (1<<PB3)); // Set SCK, MOSI, SS_1 nd SS_ s // outputs on Port B // DDRB &= ~(1<<PB6); // Set MISO s input on Port B PORTB = ((1<<PB4) (1<<PB3)); // Set SS_1 nd SS_ to high when SPI is not trnsmitting SPCR = ((1<<SPE) (1<<MSTR)); // Enble SPI nd configure mster sttus 15

232 } SPCR = (1<<CPOL); // Set clock polrity of MOSI SPCR = (1<<CPHA); // Set clock phse of MOSI // SPCR = (1<<SPR1); // Set clock rte in conjunction with SPR0 SPCR = (1<<SPR0); // Set clock rte in conjunction with SPR1 // SPSR = (1<<SPIX); // Set high to double clock rte // Trnsmit dt vi SPI port unsigned int SPI_trnsmit(unsigned int sddrdt) { SPDR = sddrdt; while (!(SPSR & (1<<SPIF))); return SPDR; } // Wit for trnsmission to complete // Red dt from SPI Receive Shift Register unsigned int SPI_red(unsigned int sddr) { // sddr &= ~(1<<7); // Set MSB to "0" unsigned int dt1 = SPI_trnsmit(sddr); _dely_us(150); // Set dely for 150 microseconds unsigned int dt = SPI_trnsmit(0x00); _dely_us(0); // Set dely for 0 microseconds return dt; } // Write dt to SPI Trnsmit Shift Register void SPI_write(unsigned int sddr, unsigned int sdt) { sddr = (1<<7); // Set MSB to "1" unsigned int dt1 = SPI_trnsmit(sddr); unsigned int dt = SPI_trnsmit(sdt); _dely_us(100); // Set dely for 100 microseconds } // Activte SPI port on Sensor 1 void SPI_1_ctivte(void) { PORTB &= ~(1<<PB4); _dely_us(50); } // Dectivte SPI port on Sensor 1 void SPI_1_dectivte(void) { PORTB = (1<<PB4); _dely_us(50); } // Activte SPI port on Sensor void SPI ctivte(void) { PORTB &= ~(1<<PB3); _dely_us(50); } // Dectivte SPI port on Sensor void SPI dectivte(void) { PORTB = (1<<PB3); _dely_us(50); } // Set SS_1 to low on Port B // Set dely for 50 microseconds // Set SS_1 to high on Port B // Set dely for 50 microseconds // Set SS_ to low on Port B // Set dely for 50 microseconds // Set SS_ to high on Port B // Set dely for 50 microseconds // Check for negtive vlues nd djust ccordingly (dt rnge: -18 to 17) int check_negtive(unsigned int rw_dt) { int djusted_dt; if (rw_dt >= 18) { djusted_dt = rw_dt - 56; // djusted_dt = (reg_0x0 ^ 0xff) - 1; // Binry conversion using two's // complement method } else 16

233 } djusted_dt = rw_dt; return djusted_dt; // Convert deciml vlue into binry rry unsigned int *dec_to_bin(unsigned int dec_num) { sttic unsigned int bits_0_to_7[8]; memset(bits_0_to_7, 0, sizeof(bits_0_to_7)); // Reset rry vlues to 0 for (int i = 0; dec_num!= 0; i++) { bits_0_to_7[i] = dec_num % ; dec_num = dec_num / ; } } return bits_0_to_7; // Print binry dt in reverse order void print_rry(unsigned int *bin_rry) { for (int j = 7; j >= 0; j--) { printf("%u", bin_rry[j]); } printf("\n\n\r"); } // Print Bits 7 to 0 from left to right // Configure settings for opticl sensor void sensor_config(void) { unsigned int reg_0x0 = SPI_red(0x0); printf("set resoulution to 800 counts per inch:\n\r"); // reg_0x0 = (1<<); // Set Bit to configure resolution reg_0x0 = (1<<4); // Set Bit 4 to configure resolution // reg_0x0 &= ~(1<<6); // Set Bit 6 to low to turn shutter mode off (lser lwys on) SPI_write(0x0, reg_0x0); reg_0x0 = SPI_red(0x0); printf("configurtion_bits (0x0): 0x%x\n\r", reg_0x0); printf("configurtion_bits (0x0): 0b"); unsigned int *reg_0x0_bin; reg_0x0_bin = dec_to_bin(reg_0x0); print_rry(reg_0x0_bin); printf("set reltive lser current to 100%%:\n\r"); SPI_write(0xc, 0x00); // Set Bits 0 to 6 to configure reltive lser current unsigned int reg_0xc = SPI_red(0xc); printf("lp_cfg0 (0xc): 0x%x\n\r", reg_0xc); printf("lp_cfg0 (0xc): 0b"); unsigned int *reg_0xc_bin; reg_0xc_bin = dec_to_bin(reg_0xc); print_rry(reg_0xc_bin); SPI_write(0xd, 0xff); // Set Bits 0 to 6 s complement of 0xc register unsigned int reg_0xd = SPI_red(0xd); printf("lp_cfg1 (0xd): 0x%x\n\r", reg_0xd); printf("lp_cfg1 (0xd): 0b"); unsigned int *reg_0xd_bin; reg_0xd_bin = dec_to_bin(reg_0xd); print_rry(reg_0xd_bin); } SPI_write(0x1, 0xff); // Cler motion registers Delt_X nd Delt_Y // Query sensor for delt x dt int red_delt_x(void) { unsigned int reg_0x03 = SPI_red(0x03); int reg_0x03_djusted = check_negtive(reg_0x03); return reg_0x03_djusted; // return reg_0x03; } 17

234 // Query sensor for delt y dt int red_delt_y(void) { unsigned int reg_0x04 = SPI_red(0x04); int reg_0x04_djusted = check_negtive(reg_0x04); return reg_0x04_djusted; // return reg_0x04; } // Query sensor for surfce qulity dt unsigned int red_squl(void) { unsigned int reg_0x05 = SPI_red(0x05); return reg_0x05; } // Convert signed 16-bit number to unsigned 14-bit (dt rnge: 0 to 16383) unsigned int convert_uint14(int dt_int16) { unsigned int dt_uint14; if (dt_int16 < 0) { dt_uint14 = dt_int ; } else dt_uint14 = dt_int16; return dt_uint14; } void TWI_buffer_lod(void) { // IC mster hs driver flw tht prevents trnsmitted uint8 vlue from exceeding 17 // Workround: Send 7 bits t time insted of 8 bits (mximum 7-bit uint8 vlue is 17) // Buffer does not exceed 14-bit mxiumum unsigned vlue of // Therefore, two highest-order bits re unused nd cn be discrded // Convert buffer vlue from signed 16-bit to unsigned 14-bit unsigned int buffer_x_1_uint14 = convert_uint14(buffer_x_1); unsigned int buffer_y_1_uint14 = convert_uint14(buffer_y_1); unsigned int buffer_x uint14 = convert_uint14(buffer_x_); unsigned int buffer_y uint14 = convert_uint14(buffer_y_); // Split 14-bit buffer vlue into two 7-bit chunks; eighth bit pdded with zero TWI_buf_out[0] = (buffer_x_1_uint14 & 0x7F); TWI_buf_out[1] = (buffer_x_1_uint14 >> 7); TWI_buf_out[] = (buffer_y_1_uint14 & 0x7F); TWI_buf_out[3] = (buffer_y_1_uint14 >> 7); TWI_buf_out[4] = (buffer_squl_1 & 0x7F); TWI_buf_out[5] = (buffer_squl_1 >> 7); TWI_buf_out[6] = (buffer_x uint14 & 0x7F); TWI_buf_out[7] = (buffer_x uint14 >> 7); TWI_buf_out[8] = (buffer_y uint14 & 0x7F); TWI_buf_out[9] = (buffer_y uint14 >> 7); TWI_buf_out[10] = (buffer_squl_ & 0x7F); TWI_buf_out[11] = (buffer_squl_ >> 7); } // Reset cummultive buffer buffer_x_1 = 0; buffer_y_1 = 0; buffer_x_ = 0; buffer_y_ = 0; buffer_squl_1 = 0; buffer_squl_ = 0; // counter = 1; int min(void) { // Initilise SPI port in mster mode on microcontroller printf("initilising SPI mster... \n\r"); SPI_mster_init(); 18

235 // Initilise TWI module for slve opertion. Include ddress nd/or enble Generl Cll. printf("initilising TWI slve... \n\r"); TWI_Slve_Initilise((unsigned int)((twi_slveaddress<<twi_adr_bits) (1<<TWI_GEN_BIT))); // Strt the TWI trnsceiver to enble reception of the first commnd from the TWI Mster printf("strting TWI slve trnsceiver... \n\r"); TWI_Strt_Trnsceiver(); // Initilise USART port on microcontroller USART_init(); stdout = &usrt_out; // Initilise both sensors (sensors shre ll pins with the exception of Slve Select) DDRB = (1<<PB1); // Set s output on PORT B to provide RESET input on sensor DDRB = (1<<PB0); // Set s output on PORT B to provide NPD input on sensor PORTB = (1<<PB0); // Set NPD to high on Port B printf("resetting both Avgo ADNS-6010 sensors...\n\r"); PORTB = (1<<PB1); // Set RESET to high on Port B _dely_ms(1); // Set dely for 1 millisecond PORTB &= ~(1<<PB1); // Set RESET to low on Port B _dely_ms(00); // Set dely for 00 milliseconds printf("sensor reset complete.\n\n\r"); // Configure settings for Sensor 1 printf("configuring settings for Sensor 1...\n\n\r"); SPI_1_ctivte(); sensor_config(); SPI_1_dectivte(); // Configure settings for Sensor printf("configuring settings for Sensor...\n\n\r"); SPI ctivte(); sensor_config(); SPI dectivte(); // Enble IC/TWI globl interrupts sei(); // Commence reding of sensors printf("reding coordintes...\n\n\r"); /* Note on sensors' motion detection: The physicl loctions of the two sensors mke it impossible for the vehicle to move without both sensors registering motion. Hence, it is only necessry to query either sensor for its motion sttus before reding dt from both. */ // Poll motion register nd red SPI dt only when motion is detected SPI_1_ctivte(); // unsigned int no_ctivity_count = 0; // unsigned int ISR_sttus_1 = 1; // unsigned int cycle = 0; while (1) { // Red motion register on Sensor 1; freeze coordinte dt unsigned int reg_0x0 = SPI_red(0x0); unsigned int *reg_0x0_bin = dec_to_bin(reg_0x0); // if (no_ctivity_count > 1000) // { // brek; // } if (reg_0x0_bin[7] == 1) // Check whether motion is detected { int delt_x_1 = red_delt_x(); int delt_y_1 = red_delt_y(); unsigned int squl_1 = red_squl(); // unsigned int delt_x_1_rw = red_delt_x(); // int delt_x_1 = check_negtive(delt_x_1_rw); 19

236 // unsigned int delt_y_1_rw = red_delt_y(); // int delt_y_1 = check_negtive(delt_y_1_rw); SPI_1_dectivte(); SPI ctivte(); // Red motion register on Sensor ; freeze coordinte dt reg_0x0 = SPI_red(0x0); int delt_x_ = red_delt_x(); int delt_y_ = red_delt_y(); unsigned int squl_ = red_squl(); // unsigned int delt_x rw = red_delt_x(); // int delt_x_ = check_negtive(delt_x rw); // unsigned int delt_y rw = red_delt_y(); // int delt_y_ = check_negtive(delt_y rw); SPI dectivte(); ATOMIC_BLOCK(ATOMIC_FORCEON) { buffer_x_1 += delt_x_1; buffer_y_1 += delt_y_1; buffer_squl_1 += squl_1; buffer_x_ += delt_x_; buffer_y_ += delt_y_; buffer_squl_ += squl_; } // counter++; // printf(" Delt X Delt Y SQul\n\r"); // printf("sensor 1: %7i %11i %9i\n\r", delt_x_1, delt_y_1, squl_1); // printf("sensor : %7i %11i %9i\n\n\r", delt_x_, delt_y_, squl_); // printf(" Delt X Delt Y Counts\n\r"); // printf("sensor 1: %7i %11i\n\r", delt_x_1, delt_y_1); // printf("sensor : %7i %11i %10i\n\n\r", delt_x_, delt_y_, counter); // printf("%i\n", delt_x_1); // printf("%i\n", delt_y_1); // printf("%i\n", delt_x_); // printf("%i\n", delt_y_); SPI_1_ctivte(); // no_ctivity_count = 0; } // no_ctivity_count++; /* if (ISR_sttus_1 > 150) { printf("isr_sttus_1: %i\n\r", ISR_sttus_1); printf("isr_sttus_: %i\n\r", ISR_sttus_); printf("isr_sttus_3: %i\n\r", ISR_sttus_3); printf("isr_sttus_4: %i\n\r", ISR_sttus_4); printf("isr_sttus_5: %i\n\r", ISR_sttus_5); printf("isr_sttus_6: %i\n\r", ISR_sttus_6); printf("isr_sttus_7: %i\n\r", ISR_sttus_7); printf("isr_sttus_8: %i\n\r", ISR_sttus_8); printf("isr_sttus_9: %i\n\r", ISR_sttus_9); printf("isr_sttus_10: %i\n\r", ISR_sttus_10); printf("isr_sttus_11: %i\n\r", ISR_sttus_11); printf("isr_sttus_1: %i\n\r", ISR_sttus_1); printf("cycle count: %i\n\r", cycle); exit(0); } if (ISR_sttus == 1) { printf("isr sttus: %i (%i)\n\r", ISR_sttus, ISR_sttus_1); ISR_sttus_1++; } 0

237 else { printf("isr sttus: %i\n\r", ISR_sttus); ISR_sttus_1 = 1; } cycle++; */ } // printf("\n\n\rprogrm stopped due to lck of ctivity."); /* // Constnt dt polling while (1) { SPI_1_ctivte(); */ } // Red motion register on Sensor 1; freeze coordinte dt unsigned int reg_0x0 = SPI_red(0x0); int delt_x_1 = red_delt_x(); int delt_y_1 = red_delt_y(); // unsigned int delt_x_1_rw = red_delt_x(); // int delt_x_1 = check_negtive(delt_x_1_rw); // unsigned int delt_y_1_rw = red_delt_y(); // int delt_y_1 = check_negtive(delt_y_1_rw); SPI_1_dectivte(); SPI ctivte(); // Red motion register on Sensor 1; freeze coordinte dt reg_0x0 = SPI_red(0x0); int delt_x_ = red_delt_x(); int delt_y_ = red_delt_y(); // unsigned int delt_x rw = red_delt_x(); // int delt_x_ = check_negtive(delt_x rw); // unsigned int delt_y rw = red_delt_y(); // int delt_y_ = check_negtive(delt_y rw); SPI dectivte(); ATOMIC_BLOCK(ATOMIC_FORCEON) { buffer_x_1 += delt_x_1; buffer_y_1 += delt_y_1; buffer_x_ += delt_x_; buffer_y_ += delt_y_; } // printf(" Delt X Delt Y\n\r"); // printf("sensor 1: %7i %11i\n\r", delt_x_1, delt_y_1); // printf("sensor : %7i %11i\n\n\r", delt_x_, delt_y_); // printf("%i\n", delt_x_1); // printf("%i\n", delt_y_1); // printf("%i\n", delt_x_); // printf("%i\n", delt_y_); SPI_1_dectivte(); SPI dectivte(); } 1

238 B. Code for Atmel ATmeg3 Microcontroller - twi_slve.c I C/TWI source file for microcontroller progrm pertining to Section Note: Code supplied by Atmel, edited by Bernhrd Wlle for GNU C comptibility, nd modified by Loren Yeo for the project /***************************************************************************** * * Atmel Corportion * * File : TWI_Slve.c * Compiler : IAR EWAAVR.8/3.10c * Revision : $Revision: 1.7 & 475 $ * Dte : $Dte: Thursdy, Aug 05, ::50 UTC & :00: $ * Updted by : $Author: lholsen & mlrsson $ * * Support mil : vr@tmel.com * * Supported devices : All devices with TWI module cn be used. * The exmple is written for the ATmeg16 * * AppNote : AVR311 - TWI Slve Implementtion * * Description : This is smple driver to AVRs TWI module. * It is interupt driven. All functionlity is controlled through * pssing informtion to nd from functions. See min.c for smples * of how to use the driver. * * Note : Revision 1.7 edited by Bernhrd Wlle to be comptible with the GNU C * Compiler nd AVR-libc. * Additionl chnges to both Revisions 1.7 nd 475 mde by Loren Yeo. * ****************************************************************************/ #include <vr/io.h> #include <stdbool.h> #include <stdio.h> #include <vr/interrupt.h> #include <vr/pgmspce.h> #include "globl.h" #include "twi_slve.h" sttic unsigned int TWI_buf[TWI_BUFFER_SIZE]; sttic unsigned int TWI_msgSize = 0; sttic unsigned int TWI_stte = TWI_NO_STATE; enum {TWI_dt_size = 1}; voltile unsigned int TWI_buf_in; voltile unsigned int TWI_buf_out[TWI_dt_size]; extern unsigned int TWI_reg_ddr; /* voltile unsigned int ISR_sttus = 0; voltile unsigned int ISR_sttus_1 = 0; voltile unsigned int ISR_sttus_ = 0; voltile unsigned int ISR_sttus_ = 0; voltile unsigned int ISR_sttus_3 = 0; voltile unsigned int ISR_sttus_4 = 0; voltile unsigned int ISR_sttus_5 = 0; voltile unsigned int ISR_sttus_6 = 0; voltile unsigned int ISR_sttus_7 = 0; voltile unsigned int ISR_sttus_8 = 0; voltile unsigned int ISR_sttus_9 = 0; voltile unsigned int ISR_sttus_10 = 0; voltile unsigned int ISR_sttus_11 = 0; voltile unsigned int ISR_sttus_1 = 0; */ // Trnsceiver buffer (outgoing) // Number of bytes to be trnsmitted // Stte byte; defult set to TWI_NO_STATE // Size of one set of dt in bytes // Trnsceiver buffer (incoming) // Trnsceiver buffer (outgoing) // Register ddr specified by TWI mster // ISR sttus flg // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter // ISR sttus flg counter

239 /* This is true when the TWI is in the middle of trnsfer nd set to flse when ll bytes hve been trnsmitted/received Also used to determine how deep we cn sleep. */ // sttic unsigned chr TWI_busy = 0; // (Rev 475) union TWI_sttusReg_t TWI_sttusReg = {0}; // TWI_sttusReg defined in TWI_Slve.h (R 475) // union TWI_sttusReg TWI_sttusReg = {0}; // TWI_sttusReg defined in TWI_Slve.h (R 1.7) /**************************************************************************** Cll this function to set up the TWI slve to its initil stndby stte. Remember to enble interrupts from the min ppliction fter initilizing the TWI. Pss both the slve ddress nd the requrements for triggering on generl cll in the sme byte. Use e.g. this nottion when clling this function: TWI_Slve_Initilise( (TWI_slveAddress<<TWI_ADR_BITS) (TRUE<<TWI_GEN_BIT) ); The TWI module is configured to NACK on ny requests. Use TWI_Strt_Trnsceiver function to strt the TWI. ****************************************************************************/ void TWI_Slve_Initilise(unsigned int TWI_ownAddress) { TWAR = TWI_ownAddress; // Set own TWI slve ddress. Accept TWI Generl Clls // TWDR = 0xFF; // Defult content = SDA relesed (Rev 1.7) TWCR = (1<<TWEN) // Enble TWI-interfce nd relese TWI pins (0<<TWIE) (0<<TWINT) // Disble TWI Interupt (0<<TWEA) (0<<TWSTA) (0<<TWSTO) // Do not ACK on ny requests, yet (0<<TWWC); // TWI_busy = 0; // (Rev 475) } /**************************************************************************** Cll this function to test if the TWI_ISR is busy trnsmitting. ****************************************************************************/ unsigned int TWI_Trnsceiver_Busy(void) { // If TWI interrupt is enbled then the Trnsceiver is busy // return (TWCR & (1<<TWIE)); // (Rev 1.7) // return TWI_busy; // (Rev 475) return (TWCR & (1<<TWINT)); } /**************************************************************************** Cll this function to fetch the stte informtion of the previous opertion. The function will hold execution (loop) until the TWI_ISR hs completed with the previous opertion. If there ws n error, then the function will return the TWI Stte code. ****************************************************************************/ unsigned int TWI_Get_Stte_Info(void) { while (TWI_Trnsceiver_Busy()); // Wit until TWI hs completed trnsmission return (TWI_stte); // Return error stte } /**************************************************************************** Cll this function to send prepred messge, or strt the Trnsceiver for reception. Include pointer to the dt to be sent if SLA+W is received. The dt will be copied to the TWI buffer. Also include how mny bytes tht should be sent. Note tht unlike the similr Mster function, the Address byte is not included in the messge buffers. The function will hold execution (loop) until the TWI_ISR hs completed with the previous opertion, then initilize the next opertion nd return. ****************************************************************************/ void TWI_Strt_Trnsceiver_With_Dt(unsigned int *msg, unsigned int msgsize) { unsigned int temp; while (TWI_Trnsceiver_Busy()); TWI_msgSize = msgsize; // Wit until TWI is redy for next trnsmission // Number of dt to trnsmit 3

240 } for (temp = 0; temp < msgsize; temp++) { // TWI_buf[temp] = msg[temp]; TWI_buf_out[temp] = msg[temp]; } TWI_sttusReg.ll = 0; TWI_stte = TWI_NO_STATE; TWCR = (1<<TWEN) (1<<TWIE) (1<<TWINT) (1<<TWEA) (0<<TWSTA) (0<<TWSTO) (0<<TWWC); // TWI_busy = 1; // (Rev 475) // Copy dt tht my be trnsmitted if the TWI // Mster requests dt // TWI Interfce enbled // Enble TWI Interupt nd cler the flg // Prepre to ACK next time Slve is ddressed /**************************************************************************** Cll this function to fill buffer with dt to be trnsmitted. ****************************************************************************/ void TWI_lod_buffer(unsigned int *msg, unsigned int msgsize) { while (TWI_Trnsceiver_Busy()); // Wit until TWI is redy for next trnsmission } TWI_msgSize = msgsize; for (int idx = 0; idx < msgsize; idx++) { TWI_buf_out[idx] = msg[idx]; } // TWCR = (1<<TWINT); // Number of dt to trnsmit // Copy dt tht my be trnsmitted if the TWI // Mster requests dt /**************************************************************************** Cll this function to strt the Trnsceiver without specifing new trnsmission dt. Useful for restrting trnsmission, or just strting the trnsceiver for reception. The driver will reuse the dt previously put in the trnsceiver buffers. The function will hold execution (loop) until the TWI_ISR hs completed with the previous opertion, then initilize the next opertion nd return. ****************************************************************************/ void TWI_Strt_Trnsceiver(void) { while (TWI_Trnsceiver_Busy()); // Wit until TWI is redy for next trnsmission TWI_sttusReg.ll = 0; TWI_stte = TWI_NO_STATE; TWCR = (1<<TWEN) // TWI Interfce enbled (1<<TWIE) (1<<TWINT) // Enble TWI Interupt nd cler the flg (1<<TWEA) (0<<TWSTA) (0<<TWSTO) // Prepre to ACK next time Slve is ddressed (0<<TWWC); // TWI_busy = 0; // (Rev 475) } /**************************************************************************** Cll this function to red out the received dt from the TWI trnsceiver buffer. I.e. first cll TWI_Strt_Trnsceiver to get the TWI Trnsceiver to fetch dt. Then Run this function to collect the dt when they hve rrived. Include pointer to where to plce the dt nd the number of bytes to fetch in the function cll. The function will hold execution (loop) until the TWI_ISR hs completed with the previous opertion, before reding out the dt nd returning. If there ws n error in the previous trnsmission the function will return the TWI Stte code. ****************************************************************************/ unsigned int TWI_Get_Dt_From_Trnsceiver(unsigned int *msg, unsigned int msgsize) { unsigned int i; while (TWI_Trnsceiver_Busy()); // Wit until TWI is redy for next trnsmission if(twi_sttusreg.slv.lsttrnsok) // Lst trnsmission completed successfully { for (i = 0; i < msgsize; i++) // Copy dt from Trnsceiver buffer } { msg[i] = TWI_buf[i]; // printf("dt %d: %d\n", i, msg[i]); } TWI_sttusReg.slv.RxDtInBuf = FALSE; // Slve Receive dt hs been red from buffer 4

241 } return TWI_sttusReg.slv.lstTrnsOK; /**************************************************************************** This function is the Interrupt Service Routine (ISR), nd clled when the TWI interrupt is triggered; tht is whenever TWI event hs occurred. This function should not be clled directly from the min ppliction. ****************************************************************************/ // SIGNAL(SIG_WIRE_SERIAL) ISR(TWI_vect) { unsigned int TWSR_sttus = (TWSR & TWSR_STATUS_MASK); sttic unsigned int TWI_bufPtr; switch (TWSR_sttus) { // Own SLA+R hs been received; ACK hs been returned cse TWI_STX_ADR_ACK: // ISR_sttus = 1; // ISR_sttus_1++; // Arbitrtion lost in SLA+R/W s Mster; own SLA+R hs been received; ACK returned // cse TWI_STX_ADR_ACK_M_ARB_LOST: TWI_bufPtr = 0; // Set buffer pointer to first dt loction TWI_buffer_lod(); // Dt byte in TWDR hs been trnsmitted; ACK hs been received cse TWI_STX_DATA_ACK: // ISR_sttus = ; // ISR_sttus_++; /* if ((TWI_buf_in == TWI_reg_ddr) && (TWI_bufPtr < TWI_msgSize)) { TWDR = TWI_buf_out[TWI_bufPtr++]; } */ TWDR = TWI_buf_out[TWI_bufPtr++]; TWCR = (1<<TWEN) // TWI Interfce enbled (1<<TWIE) (1<<TWINT) // Enble TWI Interrupt nd cler flg to send byte (1<<TWEA) (0<<TWSTA) (0<<TWSTO) (0<<TWWC); // ISR_sttus_++; // TWI_busy = 1; // (Rev 475) brek; // Dt byte in TWDR hs been trnsmitted; NACK hs been received. // I.e. this could be the end of the trnsmission. cse TWI_STX_DATA_NACK: // ISR_sttus = 3; // ISR_sttus_3++; /* if (TWI_bufPtr == TWI_msgSize) // Hve we trnsceived ll expected dt? { TWI_sttusReg.slv.lstTrnsOK = TRUE; // Set sttus bits to completed } else // Mster hs sent NACK before ll dt were sent { TWI_stte = TWSR_sttus; // Store TWI Stte s error messge } */ TWCR = (1<<TWEN) // Enble TWI-interfce nd relese TWI pins (1<<TWIE) (1<<TWINT) // Keep interrupt enbled nd cler the flg (R 475) (1<<TWEA) (0<<TWSTA) (0<<TWSTO) // Answer on next ddress mtch (R 475) (0<<TWWC); /* // Put TWI Trnsceiver in pssive mode (Rev 1.7) TWCR = (1<<TWEN) // Enble TWI-interfce nd relese TWI pins (0<<TWIE) (0<<TWINT) // Disble Interrupt (Rev 1.7) (0<<TWEA) (0<<TWSTA) (0<<TWSTO) // Do not cknowledge on ny new // requests (Rev 1.7) (0<<TWWC); */ // TWI_busy = 0; // Trnsmit is finished; not busy nymore (Rev 475) 5

242 brek; // Generl cll ddress hs been received; ACK hs been returned cse TWI_SRX_GEN_ACK: // ISR_sttus = 4; // ISR_sttus_4++; // Arbitrtion lost in SLA+R/W s Mster; Generl cll ddress received; ACK returned // cse TWI_SRX_GEN_ACK_M_ARB_LOST: // TWI_sttusReg.slv.genAddressCll = TRUE; // Own SLA+W hs been received ACK hs been returned cse TWI_SRX_ADR_ACK: // ISR_sttus = 5; // ISR_sttus_5++; // Arbitrtion lost in SLA+R/W s Mster; own SLA+W hs been received; ACK returned // cse TWI_SRX_ADR_ACK_M_ARB_LOST: // Dont need to cler TWI_S_sttusReg.genAddressCll due to it being defult stte // TWI_sttusReg.slv.RxDtInBuf = TRUE; TWI_bufPtr = 0; // Set buffer pointer to first dt loction // Reset the TWI Interupt to wit for new event TWCR = (1<<TWEN) // TWI Interfce enbled (1<<TWIE) (1<<TWINT) // Enble TWI Interrupt nd cler flg to send byte (1<<TWEA) (0<<TWSTA) (0<<TWSTO) // Expect ACK on this trnsmission (0<<TWWC); // TWI_busy = 1; // (Rev 475) brek; // Previously ddressed with own SLA+W; dt hs been received; ACK hs been returned cse TWI_SRX_ADR_DATA_ACK: // ISR_sttus = 6; // ISR_sttus_6++; // Previously ddressed with generl cll; dt hs been received; ACK returned cse TWI_SRX_GEN_DATA_ACK: // ISR_sttus = 7; // ISR_sttus_7++; // TWI_buf[TWI_bufPtr++] = TWDR; TWI_buf_in = TWDR; // printf("received dt: %i\n\r", TWI_buf[TWI_bufPtr - 1]); // TWI_sttusReg.slv.lstTrnsOK = TRUE; // Set flg trnsmission successful // Reset the TWI Interupt to wit for new event. TWCR = (1<<TWEN) // TWI Interfce enbled (1<<TWIE) (1<<TWINT) // Enble TWI Interrupt nd cler flg to send byte (1<<TWEA) (0<<TWSTA) (0<<TWSTO) // Send ACK fter next reception (0<<TWWC); // TWI_busy = 1; // (Rev 475) brek; // A STOP or repeted START condition hs been received while still ddressed s Slve cse TWI_SRX_STOP_RESTART: // ISR_sttus = 8; // ISR_sttus_8++; // Enter not ddressed mode nd listen to ddress mtch (Rev 475) TWCR = (1<<TWEN) // Enble TWI-interfce nd relese TWI pins (1<<TWIE) (1<<TWINT) // Enble interrupt nd cler the flg (R 475) (1<<TWEA) (0<<TWSTA) (0<<TWSTO) // Wit for new ddress mtch (R 475) (0<<TWWC); /* // Put TWI Trnsceiver in pssive mode (Rev 1.7) TWCR = (1<<TWEN) // Enble TWI-interfce nd relese TWI pins (0<<TWIE) (0<<TWINT) // Disble Interrupt (Rev 1.7) (0<<TWEA) (0<<TWSTA) (0<<TWSTO) // Do not cknowledge on ny new // requests (Rev 1.7) (0<<TWWC); */ // TWI_busy = 0; // Witing for new ddress mtch, so is not busy (Rev 475) brek; // Previously ddressed with own SLA+W; dt hs been received; NOT ACK returned cse TWI_SRX_ADR_DATA_NACK: // ISR_sttus = 9; // ISR_sttus_9++; 6

243 // Previously ddressed with generl cll; dt hs been received; NOT ACK returned cse TWI_SRX_GEN_DATA_NACK: // ISR_sttus = 10; // ISR_sttus_10++; // Lst dt byte in TWDR hs been trnsmitted (TWEA = 0); ACK hs been received cse TWI_STX_DATA_ACK_LAST_BYTE: // ISR_sttus = 11; // ISR_sttus_11++; // No relevnt stte informtion vilble; TWINT = 0 // cse TWI_NO_STATE: // Bus error due to n illegl START or STOP condition cse TWI_BUS_ERROR: // ISR_sttus = 1; // ISR_sttus_1++; TWI_stte = TWSR_sttus; // Recover from TWI_BUS_ERROR TWCR = ((1<<TWSTO) (1<<TWINT)); brek; // Store TWI Stte s error messge, // opertion lso clers noerrors bit // Relese the SDA nd SCL pins thus // enbling other devices to use the bus defult: // ISR_sttus = 13; // TWI_stte = TWSR_sttus; // Store TWI Stte s error messge, // opertion lso clers the Success bit TWCR = (1<<TWEN) // Enble TWI-interfce nd relese TWI pins (1<<TWIE) (1<<TWINT) // Keep interrupt enbled nd cler flg (R 475) (1<<TWEA) (0<<TWSTA) (1<<TWSTO) // Acknowledge on new requests (R 475) (0<<TWWC); /* TWCR = (1<<TWEN) // Enble TWI-interfce nd relese TWI pins (0<<TWIE) (0<<TWINT) // Disble Interrupt (Rev 1.7) (0<<TWEA) (0<<TWSTA) (0<<TWSTO) // Do not cknowledge on ny new // requests (Rev 1.7) (0<<TWWC); */ // TWI_busy = 0; // Unknown sttus; wit for new ddress // mtch tht cn be hndled (Rev 475) } } 7

244 B.3 Code for Atmel ATmeg3 Microcontroller - twi_slve.h I C/TWI heder file for microcontroller progrm pertining to Section Note: Code supplied by Atmel, edited by Bernhrd Wlle for GNU C comptibility, nd modified by Loren Yeo for the project /***************************************************************************** * * Atmel Corportion * * File : TWI_Slve.h * Compiler : IAR EWAAVR.8/3.10c * Revision : $Revision: 1.6 & 475 $ * Dte : $Dte: Mondy, My 4, :3:18 UTC & :00: $ * Updted by : $Author: ltw & mlrsson $ * * Support mil : vr@tmel.com * * Supported devices : All devices with TWI module cn be used. * The exmple is written for the ATmeg16 * * AppNote : AVR311 - TWI Slve Implementtion * * Description : Heder file for TWI_slve.c * Include this file in the ppliction. * * Note : Revision 1.6 edited by Bernhrd Wlle to be comptible with the GNU C * Compiler nd AVR-libc. * Additionl chnges to both Revisions 1.6 nd 475 mde by Loren Yeo. * ****************************************************************************/ #ifndef TWI_SLAVE_H #define TWI_SLAVE_H /**************************************************************************** TWI Sttus/Control register definitions ****************************************************************************/ #define TWI_BUFFER_SIZE 6 including // Reserves memory for the drivers trnsceiver buffer. // Set this to the lrgest messge size tht will be sent // ddress byte. /**************************************************************************** Globl definitions ****************************************************************************/ union TWI_sttusReg_t // Sttus byte holding flgs (Revision 475) // union TWI_sttusReg // Sttus byte holding flgs (Revision 1.6) { unsigned int ll; struct { unsigned int lsttrnsok:1; unsigned int RxDtInBuf:1; unsigned int genaddresscll:1; // TRUE = Generl cll, FALSE = TWI Address; unsigned int unusedbits:5; } slv; }; extern union TWI_sttusReg_t TWI_sttusReg; // (Revision 475) // extern union TWI_sttusReg TWI_sttusReg; // (Revision 1.6) /**************************************************************************** Function definitions ****************************************************************************/ void TWI_Slve_Initilise(unsigned int); unsigned int TWI_Trnsceiver_Busy(void); 8

245 unsigned int TWI_Get_Stte_Info(void); void TWI_Strt_Trnsceiver_With_Dt(unsigned int*, unsigned int); void TWI_Strt_Trnsceiver(void); unsigned int TWI_Get_Dt_From_Trnsceiver(unsigned int*, unsigned int); void TWI_lod_buffer(unsigned int*, unsigned int); void TWI_buffer_lod(void); /**************************************************************************** Bit nd byte definitions ****************************************************************************/ #define TWI_READ_BIT 0 // Bit position for R/W bit in "ddress byte" #define TWI_ADR_BITS 1 // Bit position for LSB of the slve ddress bits in init byte #define TWI_GEN_BIT 0 // Bit position for LSB of the generl cll bit in init byte /**************************************************************************** TWI Stte codes ****************************************************************************/ // Generl TWI Mster sttus codes #define TWI_START 0x08 // START hs been trnsmitted #define TWI_REP_START 0x10 // Repeted START hs been trnsmitted #define TWI_ARB_LOST 0x38 // Arbitrtion lost // TWI Mster Trnsmitter sttus codes #define TWI_MTX_ADR_ACK 0x18 // SLA+W hs been trnsmitted nd ACK received #define TWI_MTX_ADR_NACK 0x0 // SLA+W hs been trnsmitted nd NACK received #define TWI_MTX_DATA_ACK 0x8 // Dt byte hs been trnsmitted nd ACK received #define TWI_MTX_DATA_NACK 0x30 // Dt byte hs been trnsmitted nd NACK received // TWI Mster Receiver sttus codes #define TWI_MRX_ADR_ACK 0x40 // SLA+R hs been trnsmitted nd ACK received #define TWI_MRX_ADR_NACK 0x48 // SLA+R hs been trnsmitted nd NACK received #define TWI_MRX_DATA_ACK 0x50 // Dt byte hs been received nd ACK trnsmitted #define TWI_MRX_DATA_NACK 0x58 // Dt byte hs been received nd NACK trnsmitted // TWI Slve Trnsmitter sttus codes #define TWI_STX_ADR_ACK 0xA8 // Own SLA+R hs been received; ACK returned #define TWI_STX_ADR_ACK_M_ARB_LOST 0xB0 // Arbitrtion lost in SLA+R/W s Mster; own // SLA+R hs been received; ACK hs been returned #define TWI_STX_DATA_ACK 0xB8 // Dt byte in TWDR hs been trnsmitted; ACK // hs been received #define TWI_STX_DATA_NACK 0xC0 // Dt byte in TWDR hs been trnsmitted; NOT ACK // hs been received #define TWI_STX_DATA_ACK_LAST_BYTE 0xC8 // Lst dt byte in TWDR hs been trnsmitted // (TWEA = 0); ACK hs been received // TWI Slve Receiver sttus codes #define TWI_SRX_ADR_ACK 0x60 // Own SLA+W hs been received ACK returned #define TWI_SRX_ADR_ACK_M_ARB_LOST 0x68 // Arbitrtion lost in SLA+R/W s Mster; own // SLA+W hs been received; ACK hs been returned #define TWI_SRX_GEN_ACK 0x70 // Generl cll ddress hs been received; ACK hs // been returned #define TWI_SRX_GEN_ACK_M_ARB_LOST 0x78 // Arbitrtion lost in SLA+R/W s Mster; Gen cll // ddress hs been received; ACK returned #define TWI_SRX_ADR_DATA_ACK 0x80 // Previously ddressed with own SLA+W; dt hs // been received; ACK hs been returned #define TWI_SRX_ADR_DATA_NACK 0x88 // Previously ddressed with own SLA+W; dt hs // been received; NOT ACK hs been returned #define TWI_SRX_GEN_DATA_ACK 0x90 // Previously ddressed with generl cll; dt // hs been received; ACK hs been returned #define TWI_SRX_GEN_DATA_NACK 0x98 // Previously ddressed with generl cll; dt // hs been received; NOT ACK hs been returned #define TWI_SRX_STOP_RESTART 0xA0 // A STOP or repeted START condition hs been // received while still ddressed s Slve // TWI Miscellneous sttus codes #define TWI_NO_STATE 0xF8 // No relevnt stte informtion vilble; TWINT = 0 #define TWI_BUS_ERROR 0x00 // Bus error due to illegl START or STOP condition // Defines nd constnts #define TWCR_CMD_MASK #define TWSR_STATUS_MASK 0x0F 0xF8 #endif /* TWI_SLAVE_H */ 9

246 Appendix C C.1 Circuit Schemtics of JED AVR00 Single Bord Computer Circuit digrm pertining to Section 8.1. (including Subsections to ) 30

247 C. Circuit Schemtics of Core nd Power of JED AVR00 Circuit digrm pertining to Section 8.1. (including Subsections to ) 31

248 C.3 Circuit Schemtics Port A of JED AVR00 Circuit digrm pertining to Section 8.1. (including Subsections to ) 3

249 C.4 Circuit Schemtics Port B of JED AVR00 Circuit digrm pertining to Section 8.1. (including Subsections to ) 33

250 C.5 Circuit Schemtics Port C of JED AVR00 Circuit digrm pertining to Section 8.1. (including Subsections to ) 34

251 C.6 Circuit Schemtics Port D of JED AVR00 Circuit digrm pertining to Section 8.1. (including Subsections to ) 35