Laser guided vehicles: implementing and testing a GIMnet based software platform on the electric wheelchair MICA

Full text

(1)2009:178 CIV. MASTER'S THESIS. Laser Guided Vehicles - Implementing and testing a GIMnet based software platform on the electric wheelchair MICA. Rikard Almqvist Fredrik Broström Jesper Brynolf. Luleå University of Technology MSc Programmes in Engineering Space Engineering Department of Computer Science and Electrical Engineering Division of EISLAB 2009:178 CIV - ISSN: 1402-1617 - ISRN: LTU-EX--09/178--SE.

(2) Laser Guided Vehicles: Implementing and testing a GIMnet based software platform on the electric wheelchair MICA. Rikard Almqvist, Fredrik Brostr¨ om, Jesper Brynolf Lule˚ a University of Technology Dept. of Computer Science and Electrical Engineering EISLAB. 23rd November 2009.

(3)

(4) A BSTRACT This thesis presents hardware and software implemented on the electric wheelchair MICA (Mobile Internet Connected Assistant). The work done in this project involved the whole chain from mounting sensors on MICA, developing drivers and software for the sensors and the development of algorithms for autonomous driving. The drivers and software were developed with flexibility and reuseability in mind. The backbone part of this thesis was the implementation of GIMnet (Generic Intelligent Machine Network), a communication infrastructure for robot control. A socket server and a socket client was developed in order to receive data directly into MATLAB and control MICA from MATLAB. Different tests were made to evaluate the performance of GIMnet, with regard to time delay. These tests also served to evaluate how well all the developed software, drivers and algorithms worked together. The time delay tests were made locally on the MICA computer and showed that even under high load the time delay in GIMnet stayed within acceptable margins. Tests of how well all the instruments and software were able to preform together were made by implementing the Circle Sector Expansion (CSE) algorithm. The CSE algorithm focuses on finding the largest space instead of geometry of the obstacles. CSE was implemented using MATLAB and collected data from all the sensors and equipment before making the calculations. By using the CSE algorithm MICA was able to drive autonomously with fairly good ability to avoid dynamic obstacles. A dead reckoning algorithm to calculate traversed distance was also implemented. When the precision of this was tested during a 150 m test run it was found that the accumulated dead reckoning error was 0,3 m, which is quite good for such a distance.. iii.

(5)

(6) P REFACE This thesis marks the end of our education but also the beginning of a significant interest of robots and how they work. No matter where you look you can almost always find an application for them. Even in our own field of study, space engineering, there are of much use. We have acquired a great deal of knowledge over the years here at Lule˚ a University of Technology. This has in one way or another proven useful when working with this thesis. Especially the knowledge gained in electronics, automatic control, programming and robotics. Our first real contact with robotics was in the Mechatronics course held by Professor Kalevi Hyyppä. We have also had the pleasure of having Professor Kalevi Hyyppä as a teacher in other courses. The idea of a master thesis project in the area of robotics was first presented to us by Professor ˚ Ake Wernersson during a course in Telerobotics. Where we for the first time, for real, came to develop and implement algorithms to control a robot. We would therefore like to thank Professor Kalevi Hyyppä and Professor ˚ Ake Wernersson for the inspiration and knowledge they have given us by their teaching. We would also like to give a thank to Associate Professor Johan Carlson who has created this LATEX-template and who has always helped us when we have had questions about it. At last we would like to give a great thanks to our supervisor H˚ akan Fredriksson. For all the help, guidance and inspiration you have provided us with. It is almost impossible to see how this would have been possible without you.. Rikard Almqvist, Fredrik Broström and Jesper Brynolf, nov. 2009. v.

(7)

(8) C ONTENTS Chapter 1 – Introduction 1.1 Purpose . . . . . . . . . . . . . . . . . . . . . 1.2 Goal . . . . . . . . . . . . . . . . . . . . . . . 1.3 Limitations . . . . . . . . . . . . . . . . . . . 1.3.1 Unused systems mounted on MICA . . 1.3.2 Useful systems not mounted on MICA Chapter 2 – Background 2.1 The GIMnet Software Platform . . 2.2 The MICA Wheelchair . . . . . . . 2.3 Equipment on MICA . . . . . . . . 2.3.1 The Range Scanning Lasers 2.3.2 The Fiber Optic Gyro . . . 2.3.3 The Computer . . . . . . . 2.3.4 The Touchscreen . . . . . . 2.3.5 The Router . . . . . . . . .. . . . . . . . .. . . . . . . . .. . . . . . . . .. . . . . . . . .. . . . . . . . .. . . . . . . . .. . . . . .. . . . . . . . .. . . . . .. . . . . . . . .. . . . . .. . . . . . . . .. . . . . .. . . . . . . . .. . . . . .. . . . . . . . .. . . . . .. . . . . . . . .. . . . . .. . . . . . . . .. . . . . .. . . . . . . . .. Chapter 3 – Software Interface and Driver Development 3.1 Compilers and Platforms . . . . . . . . . . . . . . . . . . . . 3.2 Implementation of GIMnet . . . . . . . . . . . . . . . . . . . 3.2.1 Design Features of the Drivers and Software . . . . . 3.3 External Network Communication Software . . . . . . . . . 3.3.1 The Socket Server . . . . . . . . . . . . . . . . . . . . 3.3.2 The Socket Client . . . . . . . . . . . . . . . . . . . .. . . . . .. . . . . . . . .. . . . . . .. . . . . .. . . . . . . . .. . . . . . .. . . . . .. . . . . . . . .. . . . . . .. . . . . .. . . . . . . . .. . . . . . .. . . . . .. . . . . . . . .. . . . . . .. . . . . .. . . . . . . . .. . . . . . .. . . . . .. 1 1 2 3 3 3. . . . . . . . .. 5 6 7 7 9 9 9 10 10. . . . . . .. 11 11 11 13 13 13 14. Chapter 4 – Development and Implementation of Algorithms and Software for Robot Steering 4.1 The “Robot” Program . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1.1 The Dog-Rabbit Algorithm . . . . . . . . . . . . . . . . . . . . . 4.2 The Virtual Wheel . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2.1 Variable Rabbit Distance . . . . . . . . . . . . . . . . . . . . . . . 4.3 Servo driver . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.3.1 Heading Calculation . . . . . . . . . . . . . . . . . . . . . . . . . 4.3.2 Dead Reckoning . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 15 17 17 17 18 18 19 19.

(9) 4.3.3. Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 22. Chapter 5 – Development and Implementation of Algorithms and Software for Robot Control 5.1 Robot Remote Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.1.1 Remote Controlling the Robot With Joystick . . . . . . . . . . . . 5.1.2 Click in a Laser Scan . . . . . . . . . . . . . . . . . . . . . . . . . 5.2 The Circle Sector Expansion (CSE) Algorithm . . . . . . . . . . . . . . . 5.2.1 Implementation of The CSE Algorithm Using MATLAB . . . . . 5.2.2 Choosing the Path . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3 The Follow-Me Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3.1 Implementation of the Follow-Me Algorithm . . . . . . . . . . . .. 31 31 31 31 32 33 35 36 36. Chapter 6 – Evaluation and performance tests 6.1 GIMnet and Time Delay . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.2 Perfomance of the Circle Sector Expansion (CSE) Algorithm . . . . . . .. 41 41 42. Chapter 7 – Discussion and Future work 7.1 Discussion . . . . . . . . . . . . . . . . . . . 7.1.1 The CSE Algorithm . . . . . . . . . 7.1.2 The Follow Me Algorithm . . . . . . 7.2 Future Work . . . . . . . . . . . . . . . . . . 7.2.1 Future Controller Work . . . . . . . 7.2.2 Future GIMnet Work . . . . . . . . . 7.2.3 Future Algorithm and Software Work 7.2.4 Further Driver Work . . . . . . . . .. . . . . . . . .. 45 45 45 46 46 46 46 46 47. . . . .. 49 49 50 50 51. . . . . .. 53 53 53 62 64 65. Appendix A – Code for the A.1 The Servo Protocol . . A.2 The Laser Protocol . . A.3 The Gyro Protocol . . A.4 The Robot Protocol . .. . . . . . . . .. GIMnet protocols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . .. . . . . . . . .. . . . . . . . .. . . . . . . . .. . . . . . . . .. . . . . . . . .. . . . . . . . .. . . . . . . . .. . . . . . . . .. . . . . . . . .. used on MICA . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . .. . . . .. Appendix B – Various information related to the controller B.1 MATLAB code for the design of the MIMO controller . . . . . . . 2.1.1 Robust Performance and Stability Test . . . . . . . . . . . 2.1.2 Generation of Controller . . . . . . . . . . . . . . . . . . . B.2 Control Loop . . . . . . . . . . . . . . . . . . . . . . . . . . . . . B.3 MIMO Controller . . . . . . . . . . . . . . . . . . . . . . . . . . .. viii. . . . . . . . .. . . . .. . . . . .. . . . . . . . .. . . . .. . . . . .. . . . . . . . .. . . . .. . . . . ..

(10) L IST 1.1 2.1 2.2 2.3 3.1 4.1 4.2 4.3 4.4 4.5 4.6 4.7 4.8 4.9 4.10 4.11 4.12 4.13 5.1 5.2 5.3 5.4 5.5 6.1 6.2 B.1. MICA with single laser . . . . . . . . . . . . . . Network structure of GIMnet . . . . . . . . . . Equipment on MICA . . . . . . . . . . . . . . . The Touchscreen . . . . . . . . . . . . . . . . . MICA equipment connections . . . . . . . . . . Wheel setup . . . . . . . . . . . . . . . . . . . . Control Loop . . . . . . . . . . . . . . . . . . . Virtual Steering Wheel . . . . . . . . . . . . . . Dead reckoning model . . . . . . . . . . . . . . Dead reckoning map . . . . . . . . . . . . . . . Dead reckoning precision . . . . . . . . . . . . . SISO block diagram . . . . . . . . . . . . . . . . Step response . . . . . . . . . . . . . . . . . . . Modeled step response . . . . . . . . . . . . . . Control-loop . . . . . . . . . . . . . . . . . . . . Simulated step response . . . . . . . . . . . . . Actual MIMO step response . . . . . . . . . . . Actual MIMO step response(low wheel pressure) CSE Example . . . . . . . . . . . . . . . . . . . Construction of realPathC array in MATLAB . Laser scan with CSE . . . . . . . . . . . . . . . Subsquare Method Figures . . . . . . . . . . . . Subsquare Method Figure With Person . . . . . GIMnet Time Delay . . . . . . . . . . . . . . . CSE Produced Map . . . . . . . . . . . . . . . . Control Loop . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . .. F IGURES. OF . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . .. 2 7 8 10 12 16 16 18 20 21 22 23 24 25 26 27 28 29 32 34 35 37 39 42 43 64.

(11)

(12) L IST 1. OF. TABLES. Table of Protocols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 13.

(13)

(14) A BBREVIATIONS 10/100 Base-T AP API BSD CAN CSE DC EEPROM DE-9 GCC GIMI GIMnet GNU GPS ID IEEE iMEMS LTU MICA MIMO PC PID RAM RJ-45 RS. 10Base-T and 100Base-T standards for Ethernet connection over copper based twisted pair. Access Point Application Programming Interface Berkeley Software Distribution Control Area Network Circle Sector Expansion Direct Current Electrically Erasable Programmable Read-Only Memory D=D-subminature, E=9 pins, 9 = number of pins in the connector GNU Compiler Collection Generic Intelligent Machine Interface Generic Intelligent Machine Network GNU’s Not Unix (a recursive acronym) Global Positioning System Identification Institute of Electrical and Electronics Engineers integrated Micro Electro-Mechanical System Lule˚ a tekniska universitet (Lule˚ a University of Technology) Mobile Internet Connected Assistant Multiple Input Multiple Output Personal Computer Proportional Integral Derivative Random Access Memory The 8 Position 8 Contact plugs usually used for network cables Robust Stability xiii.

(15) RP RS-232 SISO USB WLAN. Robust Performance Recommended Standard 232C Single Input Single Output Universal Serial Bus Wireless Local Area Network. xiv.

(16) S YMBOLS α αn αweight β θstop ρ φdir ωout ωerr ωref aweight dstop d1 d2 Fyd H L vl vlerr vlout vout verr vr. Angle between the forward vector and rabbit vector. Calculated difference in heading used for the dead reckoning. The weight factor in the subsquare method. The heading of the wheelchair. Angle to the stop point where π2 rad is straight ahead. Estimated current radius of turning used for the dead reckoning. The constant used for changing the desired heading, kept at 0 rad if desired heading is stright forward. The control-signal for the angular velocity. The error in angular velocity of the wheelchair. Angular reference velocity. The exponential used in the calculations for determining the weight factor in the subsquare method. The shortest distance to the stop point. Calculated difference in x-axis (robot coordinate frame) used for the dead reckoning. Calculated difference in y-axis (robot coordinate frame) used for the dead reckoning. Discretized MIMO controller. Distance between the two driving wheels. Projected y-axis distance between the wheel axis and the virtual wheel. Velocity of the left driving wheel. The error in velocity for the left driving wheel. The control-signal for the left driving wheel. The control-signal for the forward velocity. The error in forward velocity of the wheelchair. Velocity of the right driving wheel. xv.

(17) vref vrerr vrout V0 Vv VR VL W Ws Wt W3 xdif f xdog xn xrabbit ydif f ydog yn yrabbit. Forward reference velocity. The error in velocity for the right driving wheel. The control-signal for the right driving wheel. Forward velocity used in dead reckoning. Velocity of the virtual wheel. Velocity of the right driving wheel. Velocity of the left driving wheel. Angular velocity constant used in dead reckoning. Weighting matrix of primary sensitivity functions. Weighting matrix of complementary sensitivity functions. Weighting matrix for robust stability. Distance between the rabbit and the virtual wheel in xaxis of a wheelchair frame Cartesian coordinate system. X position of the virtual wheel. Calculated new x position in the global coordinate frame. X position of the rabbit point. Distance between the rabbit and the virtual wheel in yaxis of a wheelchair frame Cartesian coordinate system. Y position of the virtual wheel. Calculated new y position in the global coordinate frame. Y position of the rabbit point.. xvi.

(18) C HAPTER 1 Introduction This thesis was conducted under the department of Computer Science and Electrical Engineering at Lule˚ a University of Technology.. 1.1. Purpose. The purpose of this master thesis project was to develop software and algorithms for the MICA (Mobile Internet Connected Assistant) wheelchair, using GIMnet (Generic Intelligent Machine Network) as a software platform. The project includes the entire chain from mounting and connecting the equipment to the development of software and drivers for the wheelchair and also the development of algorithms in order to make the wheelchair an autonomous vehicle. The MICA electric wheelchair can be seen in Figure 1.1 on the following page.. 1.

(19) 2. Chapter One. Figure 1.1: The electric wheelchair MICA in its standard configuration.. 1.2. Goal. The goal of this project was to make the electrical wheelchair perform different tasks such as, but not limited to: • Traverse an unknown environment, avoiding any obstacle it should come across. • ”Follow me”, to follow a person. • Make the robot move by pointing at the destination in a laser scan..

(20) 1.3. Limitations. 3. • Use laser data to map the environment it traverses. • Test and evaluate the performance of GIMnet on a local computer.. 1.3 1.3.1. Limitations Unused systems mounted on MICA. There are several instruments mounted on MICA. These are all possible to use. Though in order to do so you need to connect them to the computer used in this case and develop drivers for them that implements the GIM interface. For the following instruments mounted on MICA drivers were not developed: • NDC8 Laser based reflector navigation system. • Camera based reflector navigation system. For information about these instruments, see [1].. 1.3.2. Useful systems not mounted on MICA. A GPS would have been useful on MICA for outdoor use. Though in this project MICA was mostly used indoors and therefore a GPS was not implemented..

(21)

(22) C HAPTER 2 Background. In [2] autonomous vehicles are defined as ”vehicles that are capable of intelligent motion and action without requiring either a guide to follow or teleoperator control”. In space applications autonomous vehicles are used for exploration purposes. To manually remote control vehicles that travels on the surface of other planets would be extremely difficult, if not impossible, due to the time delay. To illustrate this imagine the situation where a vehicle on the planet Mars is to be controlled from earth. Based on the mean distance between the two planets and the sun, the shortest distance between earth and Mars is 78.3 · 109 meter and the largest is 1259 · 109 meter. Presuming the signals travels with the speed of light in vacuum, 299792458 m/s, it would take about 4 minutes and 20 seconds, when the distance between the planets is at a minimum, and almost 21 minutes, when the distance between the planets is at a maximum, before the signals reaches the robot. Maneuvering under such conditions would by most people be considered an impossible task. Instead the vehicle could be equipped with different sensors that allows the vehicle to identify its surrounding by analyzing the data and use certain algorithms to come up with a way to acquire the mission objective. Autonomously driving vehicles have more application than just exploration purposes. They can be of great aid for disabled people, one example could be leading a blind person on a road and finding paths to avoid obstacles. The MICA project has served to explore these possibilities by using a robot as a medical aid. In this report the focus is on the development and testing of the different drivers for the sensors, using the GIMnet software platform. This will serve as the foundation upon which further development will be based. This thesis comprise the entire process from mounting and connecting all the sensors and equipment, to developing software interfaces and algorithms to make the MICA wheelchair become an autonomous vehicle.. 5.

(23) 6. 2.1. Chapter Two. The GIMnet Software Platform. The network communication is based upon the communication infrastructure GIMnet designed for robotics applications, which is provided under the BSD (Berkeley Software Distribution) License and can be downloaded from [4]. The backbone of this network are the hubs, seen in Figure 2.1 on the next page. There are no physical hubs but rather a Linux platform dependent program, called tcpHub, preforming the tasks of a hub. The design of GIMnet makes it possible to either just use one local tcpHub or scale it up to a large network of interconnected tcpHubs. Software modules that connect to a tcpHub can be considered clients for the hub and they are separate processes that could be running anywhere in the network. When a module connects to the tcpHub it register its name and receives an ID, which can then be used to address the module. GIMnet can be considered as the network layer. The application layer is then provided through GIMI (Generic Intelligent Machine Interface). The GIM interface provides the software module developers with a simple C++ API for networking tasks such as connecting to a tcpHub or subscribing to a service and can be used on both WindowsTM and Unix systems..

(24) 2.2. The MICA Wheelchair. 7. Figure 2.1: A figure from http://gim.tkk.fi/GIMnet showing the network structure of GIMnet.. 2.2. The MICA Wheelchair. The MICA wheelchair has been used several times in previous projects [1] [5] [6] [7], as a platform for different experiments concerning robotics. The wheelchair was constructed and built by Boden Rehab in Boden, Sweden. It is powered by two standard car batteries that supplies two electrical DC-motors and all other equipment with 24 V. The wheelchair uses a drive that is of differential type and has encoders connected to each wheel. All communication between the DC-motors controller and external equipment, e.g. the original joystick, is conducted through a CAN-bus.. 2.3. Equipment on MICA. A number of equipments were used on MICA, most of them used for navigation purposes. Navigation requires a lot of information such as where you are heading, how fast you are.

(25) 8. Chapter Two. going and some means to avoid obstacles. Listed below are the equipment used in this project. A) Range Scanning Lasers B) Fiber Optic Gyro C) Computer D) Touchscreen E) Router The mounting and placement of the different sensors and equipment can be seen in Figure 2.2. Another important measurement device that was used is the encoders, these are located inside the DC-motors and cannot be seen in the figure.. (a) Front view. (b) Rear view. Figure 2.2: A) Range Scanning Lasers, B) Fiber Optic Gyro, C) Computer, D) Touchscreen, E) Router. In this picture MICA was configured to be an experiment platform and has a rack where two range scanning lasers are mounted on top of each other..

(26) 2.3. Equipment on MICA. 2.3.1. 9. The Range Scanning Lasers. The range scanning laser of type LMS200, from the company SICK [8], is a sensor that measures the distance to objects in front of the vehicle in an angular interval that spans from 0◦ to 180◦ . It is possible to configure the LMS200 in a number of ways. The angular resolution has for this project been set to 1◦ and the unit for distance measurements was set to centimeters. The lasers were mounted on the wheelchair in two different ways. In this thesis project only one laser was used. It was mounted on top of the side mounted table, slightly aligned to the left compared to the center of the wheel axis, see Figure 1.1 on page 2. In [9] two range scanning lasers were mounted on top of each other in a metal construction that held them in place. Due to this mounting both lasers will experience very similar disturbances.. 2.3.2. The Fiber Optic Gyro. The DSP-3000 is a single-axis interferometric fiber optic gyro from KVH Industries, Inc. [10]. It measures angular rotation rate, in this case the yaw rate. The rotation rate can be integrated in order to get the turning angle. The gyro needs a 5 volts DC power supply to operate and it will return digital messages with input rates up ±375 degrees per second. The gyro is self-initializing and it takes approximately 5 seconds to become ready from that the power is applied. For more information see [11] and [12]. In Figure 2.2 on the facing page one can see the box containing the gyro. In this box there is also a DC-converter that transforms the 12 volts supply to the 5 volts the gyro needs to operate.. 2.3.3. The Computer. The computer is an UNO-2171, from Advantech [13]. It is equipped with an Intel Celeron M processor working at 1 GHz and 512 MB of RAM. The computer comes with 4 serial ports with DE-9 connectors. Two of them supports only RS-232 and the other two supports RS-232/422/485. All the ports were for this project configured to use RS-232. It also has two 10/100 Base-T RJ-45 Ports and two USB ports. The OS used on the computer is XUbuntu version 8.10. The computer has no fans and the hard drive is a CompactFlash memory. This means that the computer has no moving parts and is thus less sensitive to vibration and shock. All the different sensors and equipment were connected to the computer. The computer runs all the drivers for these and the tcpHub program that is the core of GIMnet..

(27) 10. 2.3.4. Chapter Two. The Touchscreen. The touch screen is from CARTFT [14] and of the model CTF840-C, it is a resistive type of touch screen. It can communicate with the PC through either RS-232, PS/2 or USB. It requires a 12 volts DC power supply. The touchscreen can be used to click in laser scans in order to make MICA move to that position. The touchscreen is shown in Figure 2.3.. Figure 2.3: This is a picture of the touchscreen used on MICA.. 2.3.5. The Router. The router used was an Asus Wireless Router WL-500g Deluxe. The router is completely compatible with the IEEE standards 802.11b/g for WLAN [15]..

(28) C HAPTER 3 Software Interface and Driver Development. 3.1. Compilers and Platforms. Most of the programming were made in C/C++ on computers using XUbuntu version 8.10, see [16]. The compiler used was the GNU (GNU is a recursive acronym that stands for “GNU’s Not Unix”) C++ compiler (also known as g++). This compiler is a part of the GCC (GNU Compiler Collection), see [17].. 3.2. Implementation of GIMnet. GIMnet is the mean by which all equipment communicates, a block diagram showing how everything on MICA was connected can be seen in Figure 3.1 on the following page. All the software and drivers developed uses the GIMI API in order to access the functionality that GIMnet provides. In this project all the equipment was connected to one single tcpHub. The drivers for the sensors that produces data, i.e the lasers and the gyro, connects to the tcpHub and adds a service with a Major Type Id. This makes it possible for any other software that has implemented the GIM interface to also connect to the tcpHub and subscribe to the different services and access the information they provide. The GIM interface comes with several different protocols for data transfer. The protocols defines the data that is to be sent. All the variables and their corresponding data types defined in the protocol does not need to used. In order to fully customize GIMnet for the needs of this project new protocols were developed. A new protocol was developed for each sensor providing information i.e. the gyro, the lasers, and the encoder. A protocol was also developed for data that are sent to the robot to tell it how to behave. All protocols are referred to by their name, as a text 11.

(29) 12. Chapter Three. string, and their Major Type Id, in the form of a number. The new protocols created can be seen in Table 1 on the next page. For more information regarding the protocols and what data they define see appendix A on page 49.. Figure 3.1: Block diagram showing how the sensors and equipment on MICA is connected to the GIMnet tcpHub. The Slave tcpHub was not actually used but shows how several tcpHubs can be interconnected..

(30) 3.3. External Network Communication Software. 13. Table 1: Table showing name of equipment or service, protocol name and major type id of the created protocols.. Equipment or Service Name Protocol Name Major Type Id Wheelchair Interface MICA PROTOCOL SERVO 122 Laser Scanner MICA PROTOCOL LASER 123 124 Gyro MICA PROTOCOL GYRO Robot Command MICA PROTOCOL ROBOT 125. If there are two services providing data using the same protocol, i.e. having the same Major Type Id, the way to distinguish between them is to use the Minor Type Id. This was used with the range scanning lasers where the first one was given Minor Type Id 1 and the second a Minor Type Id 2.. 3.2.1. Design Features of the Drivers and Software. When implementing the GIM interface, special consideration was taken to make the code as reusable as possible. The different steps in the initializing process was kept in separate threads. The first step in the initializing process is to connect to the tcpHub. The way all the different drivers and software do this is identical. If there are problems connecting to the tcpHub the software and drivers cannot continue with the initializing process but instead it tries to reconnect until a connection has been established. When connection to the tcpHub has been established services and subscriptions are added. The way a program begins to subscribe to or adds a service is the same for all software and drivers. The only thing that differs between them is what service they subscribe to or provides.. 3.3 3.3.1. External Network Communication Software The Socket Server. A socket server was implemented and used in this project, the main reason for doing this was to have a link between GIMnet and software not capable of direct communication with GIMnet. Sockets provide the means for accepting incoming data packets and delivering these to the appropriate source. A socket server is a program that accepts and listens to incoming connections over sockets. It can handle several clients at the same time. In the socket server program developed for this project the GIMI was implemented and the socket server connected to the tcpHub. It subscribes to all the services that provided data and passes this data on to all clients that connected to the server. A socket.

(31) 14. Chapter Three. server has the advantage that the clients that connects is not bound to have been written in same programming language as the server nor does it matter what platform they are running on. The server creates a new thread for each new client that connects. In this thread all the available data is written to the dedicated socket for each individual client. The server also creates a thread that is suppose to receive information from a client. The client that has the privilege to send data back to the server is the client that was last to connect to the server. In order to avoid problems with time delay the software program called “robot”, described in 4.1 on page 17, needs five different parameters. • The X-position of MICA in global coordinates when the calculations begun. • The Y-position of MICA in global coordinates when the calculations begun. • The heading of MICA when the calculations begun. • An array of X coordinates to aim for in the MICA’s local coordinate system. • An array of Y coordinates to aim for in the MICA’s local coordinate system. When the socket server has all these things it sends them as a package to the “robot” program.. 3.3.2. The Socket Client. A client application for the socket can be developed in any programming language, but it was decided to develop the client in Java [18] for easy interaction with MATLABTM [19]. MATLAB has built in support for calling and using Java objects from the command window. The client listens to all the data from the socket server and sorts this in arrays which can be handled in MATLAB. The client can be connected to the socket server from MATLAB, and once this is done any data from the socket can be obtained by calling a method in the client application from the command window. MATLAB has many built in functions, especially for matrix manipulation, these are of great use when performing calculations on the data. It is also possible to send data back to the socket server directly from MATLAB by calling methods in the Java socket client. This makes it possible to control MICA directly from MATLAB by sending control data back to the socket. Both the Circle Sector Expansion and Follow-Me algorithms discussed later used this approach. Another useful thing with this is that it’s possible to perform time consuming calculations on a machine where processing power is available, thus lightening the load of MICA’s on-board computer..

(32) C HAPTER 4 Development and Implementation of Algorithms and Software for Robot Steering. When working with a wheelchair it is important to consider the wheel placement. The two driving wheels in the front and the two caster wheels at the back (see Figure 4.1 on the following page), gives the wheelchair the ability to turn around it’s own axis. The caster wheels, without any way of measuring their angle compared to the wheelchair, are hard to account for when making a model of the wheelchairs movement. However, to save time in this project, the caster wheels were not accounted for when creating the model, instead an advanced controller was used, which was developed to have extensive error compensation due to deviations from the model.. 15.

(33) 16. Chapter Four. Figure 4.1: The left the picture shows the setup of the caster wheels and to the right the setup of the driving wheels.. In order to prepare the reader for the different sections of this chapter, this paragraph offers a brief explanation to the feedback control used in the project. The control loop is divided into two parts, an inner and an outer control loop. Where as the name implies the inner-control loop controls the process, which is the actual uncontrolled movement patterns of the wheelchair. The controller for the inner-control loop can be found in the Servo driver and will be discussed in 4.3.3 on page 22. The outer-control loop controls the inner-control loop and is discussed in 4.1 on the facing page. The overall structure of the control-loop is shown graphically in Figure 4.2 and a larger version can be found in Figure B.1 on page 64.. Figure 4.2: Block diagram of the control-loop. Input can be from for example CSE, first output is forward and angular velocity and output2 is the position of the virtual wheel..

(34) 4.1. The “Robot” Program. 4.1. 17. The “Robot” Program. The program is called “robot” because the algorithms in it shapes the movement patterns of the wheelchair when attempting to drive autonomously, thus makes it our outer controlloop layer. It should not be confused with the inner control-loop which is implemented in the servo driver. The main goal of the robot program is to take input in the form of one or more points in Cartesian coordinates, calculate the forward and angular velocity needed to reach its goal and then send the calculated data to the Servo driver for further computations. The functions provided in this program can be hard to distinguish from each other, but the intended design is described in the following sections.. 4.1.1. The Dog-Rabbit Algorithm. The dog-rabbit algorithm provides the most basic way of movement. The vehicle, which in this concept is a dog, moves towards a point, which is the rabbit. By placing the rabbit conveniently an angle for the virtual wheel, α (see Figure 4.3 on the next page), can be calculated. Considering the rabbit and wheelchair in global Cartesian coordinates, with β as the heading, gives the system of equations as    xdif f = cos −β − sin −β xrabbit − ydog ydif f sin −β cos −β yrabbit − ydog . (4.1)   α = arctan −xdif f ydif f. 4.2. The Virtual Wheel. This algorithm provides a steering method which includes placing a virtual wheel arbitrarily in front of the wheelchair. This is a basic way of controlling a vehicle such as a wheelchair, as it does not have a real steering wheel and can turn around its own axis. The virtual wheel can then simulate other driving restrictions, such as a minimum radius of turning by placing boundaries on the steering angle. In other words; a steering wheel, virtual or not, can simply be aimed towards a point and the wheelchair follows. If the virtual wheel is placed in front of the wheelchair and equidistantly from each of the driving wheels, the two equations can be described as v0 = Vv cos α and. (4.2). Vv sin α, (4.3) L where Vv is the velocity of the virtual wheel and L is the distance from the center of the wheel axis to the virtual wheel. See Figure 4.3 on the following page. w0 =.

(35) 18. Chapter Four. Figure 4.3: Graphical representation of the virtual steering wheel.. 4.2.1. Variable Rabbit Distance. The list of points received from the Circle Sector Expansion program is points located in the middle of open spaces. This gives rise to a less than optimal path, especially when considering a straight corridor with walls littered of objects. The middle of the open space is not a straight line which will result in oscillating movement. So if the input is more than one point, the robot program checks the laser-measurements for potential obstacles to each point, and aims for the last point found with no obstacles in the way.. 4.3. Servo driver. This program was designed to be the inner control loop. Implemented in the program is a controller, but it also handles communication over the MICA CAN-bus (BoreCan) and calculates MICA’s global position vector. Hence, it includes a few functions which could have been handled by separate programs, but in order to save processing power and minimize the time delay, the functions were compressed into one program..

(36) 4.3. Servo driver. 4.3.1. 19. Heading Calculation. The heading of MICA in the global coordinate frame, can be calculated from the rate gyro or the wheel encoder but there are, however, pros and cons with both approaches. Optical Rate Gyro: • High precision (PRO) • Temperature dependent internal BIAS causes drift (CON) Encoder: • No drift (PRO) • Moderate precision, noisy input (CON) The approach used on the MICA wheelchair was to receive a heading from the gyro, and in case of gyro failure perform the heading calculations from encoder data, until gyro connection could be re-established.. 4.3.2. Dead Reckoning. Dead reckoning is an algorithm which calculates the position of the wheelchair based on encoder data. In this project the wheel-encoder returns a number of pulses, which translates into distance traveled for each wheel. Distance can then be converted to a velocity by division of time. From encoder data the forward and angular velocities of the wheelchair can be calculated as VR + VL V0 = (4.4) 2 and W =. VR − VL , H. (4.5). where VR is the velocity of the right wheel, VL is the velocity of the left wheel and H is the distance between the two wheels. In this project, instead of calculating W, the rate gyro provided this information when possible. For deriving the model the following assumptions were made:.

(37) 20. Chapter Four. Figure 4.4: Graphical representation of the dead reckoning model.. . V0 = const. W = const.. (4.6). Assuming no slip condition gives V0 , (4.7) W where ρ is the radius of the circle. The relative position change of the wheelchair can then be calculated as d2 = ρ sin(an − β) (4.8) ρ=. and. d1 =. ρ(1 − cos(an − β)) if − π2 < an − β < 0 −ρ(1 − cos(an − β)) if 0 < an − β < π2. (4.9). The new position of the wheelchair can then be calculated by adding the rotation of the relative position change, with respect to β, with the old coordinates. xn cos β − sin β d1 x = + 0 . (4.10) yn sin β cos β d2 y0 To show the precision of this algorithm, a map was plotted by using the coordinates generated over a distance of about 150 meters. The map can be seen in Figure 4.5 on the next page. The accumulated dead reckoning error during this test run can be seen in Figure 4.6 on page 22..

(38) 4.3. Servo driver. 21. Figure 4.5: An indoor test area, scale is in meter. Blue dots are laser measurements, red are generated rabbit points and green is the path of the wheelchair..

(39) 22. Chapter Four. Figure 4.6: Showing the enlargement of the square in Figure 4.5 on the preceding page, scale is in meter. The circle in the figure marks the dead reckoning error. This error is approximately 0.3 m.. 4.3.3. Controller. In the beginning of the project it was first though that a software implemented controller was not required, as the electronics for the DC-motors had a built-in hardware controller. It was however noted that the safety margins on the present controller resulted in extremely low acceleration and limited movement patterns, which were hard to account for when making efficient algorithms. Single Input Single Output (SISO) Controllers SISO controllers are as their name implies controllers with a single input and a single output, included in this group of controllers are P-, PI-, PD- and PID-controllers. It was decided to use one SISO controller for each wheel to represent a diagonal MIMO.

(40) 4.3. Servo driver. 23. (Multiple Input Multiple Output) controller. The starting design of the control loop is represented in the block diagram in Figure 4.7, more about SISO and MIMO controllers can be found in [20].. Figure 4.7: Block diagram of a diagonal SISO-controller loop.. The equations for the “Conversion 1”- and “Conversion 2”- blocks are verr vrerr 1 H2 , = vlerr 1 − H2 werr. (4.11). 1 1 vout vr 2 = 21 1 wout − vl H H. (4.12). . and the equation used for the “Model”-block is a simple diagonal ramp such as 1 0 vrout vr sT1 +1 = 0 − sT21+1 vlout vl. (4.13). where T1 and T2 are the time-constants for the ramp. For the “Controller” block the P-, PI-, PD- and PID-controllers are denoted as C. In general this gives vrout C1 0 vrerr = . (4.14) vlout 0 C2 vlerr This means that when a step in forward or angular velocity is applied the controllers calculate the new control signal independently of each other. In the start of this project a PID controller was used, integral part for disturbances such as low and/or different wheel pressure and a derivative part to decrease the overshoot produced by the integral part. But since all the SISO controllers have the same fundamental flaw, that it does not take into account the cross-linking, it was decided to try a more advanced controller..

(41) 24. Chapter Four. Multiple Input Multiple Output (MIMO) Controller. The controller chosen was “H∞ controller with mixed sensitivity” and the design of the controller is based upon chapter 9.3.5 in the book ”Multivariable Feedback Control”[20]. The MATLAB code used to make numerical calculations during the development of the MIMO controller, such as checking for RP (Robust Performance) and RS (Robust Stability), can be found in appendix B on page 53. In order to find a suitable multivariable controller a modeled step response was required. This was done by logging encoder data while sending predefined velocity steps for each wheel, it should be noted that no driver was in the seat of the wheelchair during the measurements. Logged data was then plotted and can be seen in Figure 4.8.. Figure 4.8: Plot of measured step response without a controller. Blue is measured data, red is forward reference velocity and green is angular reference velocity.. Modeling of measured data was performed by ident, a built in function in MATLAB. The numerical model returned by ident, is in Equation 4.15 on the facing page represented.

(42) 4.3. Servo driver. 25. in the Laplace domain. " 0,91102 exp−0,025s vout 2 = 1+0,2213s+0,0539s wout 0. 0,30228 exp−0,025s 1+0,68966s 0,66386 exp−0,025s 1+0,099601s. #. vref . wref. (4.15). Graphical representation of the modeled step response as a function of time can be seen in Figure 4.9.. Figure 4.9: Plot of Equation 4.15 as a function of time, without a controller.. Weights were chosen to minimize the cross-coupling and to ensure fast angular velocity traceability. It was also naturally aimed for to have as small steady state error as possible, especially in the angular velocity. The three weighting matrices needed for this controller: Ws (Primary Sensitivity Matrix), Wt (Complementary Sensitivity Matrix) and W3 (Identity Matrix) were first chosen by loop shaping, which is described in “Multivariable Feedback Control” [20]. In order to reach the design parameters for the controller the amplitude of the weights were modified by trial and error to   s ws1,1 ( 0.6 +1) 0 s +1  Ws =  0.04 (4.16) s ws2,2 ( 0.6 +1) 0 s +1 0.04. , . s wt1,1 ( 100 +1).  s +1 s +1 Wt =  ( 10 )( 10000 ) 0. . 0.  . s wt2,2 ( 100 +1). (. s +1 10. )(. s +1 10000. ). (4.17).

(43) 26. Chapter Four. and . 1 0 W3 = 0 1. (4.18). where the constants (that were chosen by trial and error) are ws1,1 = 30, ws2,2 = 80 and wt1,1 = wt2,2 = 2. Solving the Riccati equation with γ = 3.3 yields the controller as a numerical continuous model. Discretized with zero order hold and a time constant of 50 ms gives the discrete controller Fyd in state-space form as. Fyd.   a1,1      .   A =  .. =  a1,8   c1,1    C= c1,2.    a8,1 b1,1 b2,1 ..  , B =  .. ..   . .  .  , . . . a8,8 b1,8 b2,8 . . . c8,1 0 0 , D= . . . c8,2 0 0 ... .... (4.19). where the matrices A, B and C is found in Equation B.1, Equation B.2 and Equation B.3 on page 65. The implementation of the controller in the control loop used for simulation, in Simulink [21], and on the MICA wheelchair is described by the block diagram in Figure 4.10. It should be noted that for on board purposes the “Continuous Model”-block is the wheelchairs actual behavior.. Figure 4.10: Block diagram of the MIMO control loop.. Resulting simulation of the MIMO control loop can be seen in Figure 4.11 on the facing page..

(44) 4.3. Servo driver. 27. Figure 4.11: Plot of simulated step response as a function of time.. A small amount of cross coupling can be seen as well as a fast response to a step in angular velocity reference. A step in forward velocity reference results in a slower response and a larger steady state error. After implementation of the controller, real time logging of encoder data was done and the resulting measured step response can be seen in Figure 4.12 on the next page. This measurement was performed on a flat surface with the caster wheels randomly aligned. It was noted that the simulation and on board measurement did not match, hence leading to the conclusion that the model is not perfect. The biggest flaws with the controller as such is the overshoot generated from a step in angular velocity and a larger then expected cross-coupling. The overshoot in angular velocity can lead to a nervous behavior when the angular velocity reference is changed frequently..

(45) 28. Chapter Four. Figure 4.12: Measured step response as a function of time when using the MIMO controller. The green line is the reference signal, the red line is the control signal and the blue line is measured data.. The robust stability and performance of the controller was tested with a flat wheel, a measured step response plot of this can be seen in Figure 4.13 on the facing page. However, except for the flaws already noted due to model deviations, the controller did not have any problem compensating for these kind of errors. In fact, even though the resulting cross-coupling increased, the generated overshoot decreased. It should be noted that the gyro was used in order to have any error compensation in angular velocity. However, no error compensation in forward velocity was possible without an inertial sensor such as an accelerometer..

(46) 4.3. Servo driver. 29. Figure 4.13: Measured step response as a function of time when using the MIMO controller to compensate for low wheel pressure in the left wheel. The green line is the reference signal, the red line is the control signal and the blue line is measured data..

(47)

(48) C HAPTER 5 Development and Implementation of Algorithms and Software for Robot Control 5.1 5.1.1. Robot Remote Control Remote Controlling the Robot With Joystick. The joystick driver was one of the first robot controlling programs that was developed in this project. The program starts by trying to connect to the tcpHub. When connected it adds a service with Major Type Id and Minor Type Id. Then the program will then try to figure out what kind of joystick you have connected to your computer. When the program has determined what joystick that is currently in use, the axis position of the joystick is sampled. For safety reasons the joystick will send the value 0 on all axis if the “dead man’s handle”, a button on the joystick, is not pressed down. If the “dead man’s handle” is pressed down then the position of the joystick axis is send every time the joystick is sampled. The servo driver then translates this information so that MICA follows the command of the joystick.. 5.1.2. Click in a Laser Scan. The laser scan program was originally a program made to display laser scan data that was sent over GIMnet, in an early stage of the project. When a touchscreen was added as method of input to the robot it came naturally that one should be able to click in the laser scan to control MICA. To the existing code, of subscribing to laser scan data and show it on the screen, was added the possibility of recognize a mouse click on the screen. The clicked point in laser scan was sent to the software program called “robot”, described in 4.1 on page 17, as the point to aim for. 31.

(49) 32. 5.2. Chapter Five. The Circle Sector Expansion (CSE) Algorithm. The path finding algorithm for autonomous driving used in this project was CSE. It has previously been used on the MICA wheel chair when Sven Rönnbäck used it on MICA in his doctoral thesis, see [22]. The circle sector expansion algorithm focuses on free space and places where the vehicle is able to traverse, instead of geometry and properties of the obstacles. The general idea is to use circle sectors to search for free space. The circle sector expands until it hits something that is not free space. This point binds the circle sector and splits it up into two new circle sectors and the process is then repeated, see Figure 5.1.. Figure 5.1: This shows an example of a circle sector expansion. The middle point in the figure is the first binding point of the circle sector expansion. It splits up in two new expansions.. If the radius of the expanded circle sector exceeds the defined maximum radius then the expansion comes to an end. The expansions indicates the direction of free space and if the length of a chord in the circle sector is larger then the width of the vehicle with some safety margin then it indicates a possible way to traverse..

(50) 5.2. The Circle Sector Expansion (CSE) Algorithm. 5.2.1. 33. Implementation of The CSE Algorithm Using MATLAB. When one uses CSE as it was implemented in this project. There are certain things that needs to be defined. The vehicle needs to be defined with a radius, inside which the vehicle is able to fit. Then one defines a safety margin. The third thing to define is how big the expanded circle sector is allowed to be. These three things has a great impact in the behavior of the vehicle. If one uses data from a prerecorded map where all the walls and obstacles are known, it would be possible to have a rather large radius for the expanded circle sectors. Though if one tries to do the same without well defined data of walls and such the CSE will find paths that is not possible to traverse. An example of this is the left path in Figure 5.3 on page 35, as it will head straight into a wall not seen by the laser scanner. The CSE algorithm was used both with two and one laser mounted on the wheelchair. The laser data is transformed into a Cartesian x,y-coordinate system. In each calculation cycle a new set of data is collected from the lasers. In each expansion the the center point of the circles are saved in a temporary array, tempPathC. A possible path to traverse is a path where the chord is larger then 2 × (rvehicle + safetymargin), where rvehicle is the radius of the wheelchair. If both chords fulfills this requirement then the larger of the two are chosen for further expansion. Though the necessary information to continue an expansion from the other chord is saved in a temporary break point array, tempBreakP. When no further expansion is possible then the temporary array are checked for loops. If it passes this test then the temporary arrays, tempPathC and tempBreakP, is added to the “real” arrays. Additional information regarding, such as what kind of stop it was and what index it had in the realPathC array, is stored in the stop array. When a stop point is reached the algorithm checks the realBreakP array for new points to expand from. If a point is found in realBreakP then a new expansion will begin from that point. The coordinate of the center point of the circle sector where both chords were big enough to pass as possible paths to traverse, i.e. the coordinate of the central point for the circle sector where a break point was created, will become the starting point for this expansion branch. The expansion will continue until it reaches a stop. The algorithm will then again go back to the realBreakP array and check for new points to expand from. When the algorithm reaches a stop point and there are no more points in the realBreakP array to expand from the calculations are done. All the possible paths has then be calculated and the realPathC array will have a tree like structure containing all the center points of all expanded circle sectors. The Figure 5.2 on the following page gives a visual representation of how the realPathC array is created and in Figure 5.3 on page 35 on can see the all possible paths plotted in a laser scan..

(51) 34. Chapter Five. Figure 5.2: Shows an example of how the realPathC array is constructed when CSE was implemented using MATLAB. Each new branch starts from a break point that has been stored in the realBreakP array during previous expansions. The end of each branch, the stop point, is stored in the stop array..

(52) 5.2. The Circle Sector Expansion (CSE) Algorithm. 35. Figure 5.3: A laser scan plot with the tree structure and the circle sectors of the CSE included where the squares marks the chosen path.. 5.2.2. Choosing the Path. The CSE gives several possible paths as output and it is up to the user to specify exactly what path to traverse. For this project a weighted algorithm was used. As shown in Equation 5.3 the function takes into account the angle θstop and the distance dstop . Where θstop is the angle from the positive x-axis to the stop point and dstop is shortest distance to the stop point, see Equation 5.1 and Equation 5.2. The constant φdir is used in order to change the preferred heading. With φdir set to 0 rad the preferred heading will be straight ahead, which is represented by the path marked with squares in Figure 5.3. One can change the exponential in order to set how the function should prioritize distance compared to heading, though the power of the sine functions does always need to be an odd number. The most commonly used value for φdir during autonomous driving tests conducted with MICA was − π2 rad. This made MICA try to go left whenever it was possible. dstop. q 2 2 = Xstop + Ystop . θstop = arctan. Ystop Xstop. (5.1). . f (dstop , θstop ) = dstop · sin3 (θstop + φdir ). (5.2). (5.3).

(53) 36. 5.3. Chapter Five. The Follow-Me Algorithm. The Follow-Me algorithm has been used in many other autonomous vehicle projects and applications. It can be implemented in a number of different ways, but the general idea is the same. The autonomous vehicle should follow a person or an object, which will be referred to as the Follow-Me object in this text. In the case of MICA, the range scanning lasers is used to detect the Follow-Me object inside a predefined area within the laser scan. Once the object is detected, MICA shall lock onto it and start following it once it moves.. 5.3.1. Implementation of the Follow-Me Algorithm. The Follow-Me algorithm is implemented in MATLAB, where data from the laser is acquired from the socket server. First a square with 2 meter sides is placed on the laser scan, it’s center at a point 1 meters in front of MICA. This square defines where the Follow-Me object should be searched for, any object outside this square will never be detected as the Follow-Me object. This square is then divided into a number of smaller subsquares creating a grid, in this case each subsquare has the size of 1dm2 . Each subsquare has an index, the index of the subsquare in the lower left corner is (1, 1) and the subsquare in the upper right corner is (20, 20). After this it needs to be determined how many laser scan points each square contains. Then a 20 × 20 matrix is created where the value of each index is the number of laser scan points found within the subsquare with the same index. This method will be referred to as the Subsquare method and the result of this is visualized in Figure 5.4 on the facing page..

(54) 5.3. The Follow-Me Algorithm. 37. Figure 5.4: The upper left figure shows the laser scan and the square where the Follow-Me object is searched for, the scale of the plot is in meters. The upper right figure shows the enlarged square with each subsquare, where the number on the sides represent their index number. The red square marks the subsquare where the most number of scanned points appeared. The lower figure shows a 3-d representation of the square where the peaks represent the number of scanned points within each subsquare. The numbers on the x and y axis represent the subsquares index number and the number on the z axis represent the number of points found within the subsquare.. Before the follow-me is initiated, a movement within the square has to be detected. A movement is defined as an increase of laser scan points within a subsquare, from one set of collected data to the next, that is above a given threshold value. The subsquare where this movement was detected is set as the subsquare to initially aim for. The subsquare.

(55) 38. Chapter Five. method is then initiated. New data is then collected and subsquare matrix is calculated for the new data. The new matrix is then weighted. Each element in the matrix is multiplied with a weight factor, see Equation 5.4, to created a weighted matrix according to Equation 5.5. !aweight 1 αweight = p (5.4) (iprevious − icurrent )2 + (jprevious − jcurrent )2 + 1 The exponential of the weight factor, aweight , is depending on if the subsquare method has been initiated or not. When the method is uninitiated the exponential, aweight , is kept low (aweight = 0), and when the method is initiated the exponential is kept high (aweight = 4) in order to create a stable lock on the target it follows. Bweighted = Bi,j · αweight. (5.5).

(56) 5.3. The Follow-Me Algorithm. 39. Figure 5.5: The upper left figure shows a scan where a person stands within the square where the follow-me object is searched for, scale is in meters. The upper right and the lower figure shows an enlargement of the square and the 3-d representation of the legs obtained from the subsquare method..

(57)

(58) C HAPTER 6 Evaluation and performance tests Different tests were conducted to evaluate the performance of MICA. These include time delay tests locally over GIMnet, evaluating the effect of time delay in control loops and more. This chapter discusses how these tests were conducted and the results gained.. 6.1. GIMnet and Time Delay. Sending data over GIMnet will introduce a time delay for certain. It was assumed that this delay would increase both with the amount of data sent and the number of services sending data, as well as the number of subscribers receiving data. To test the time delay when running locally a time stamp is added to the data on the service side, this is compared to the time of arrival at the subscriber. This is an easy way to measure time delay when both service and subscriber are running locally, since no synchronization is needed. First a test was conducted with GIMnet running under light load, both lasers were sending data, however only data from one of them was subscribed to. The results when receiving 3000 samples of laser data were a mean value of 647µS with a standard deviation of 163µS. Next a test under normal, that is all the necessary services and subscribers communicating over GIMnet, was done. As with the first test, 3000 samples of laser data were taken. The resulting mean value was 838µS with a 312µS standard deviation. It’s noted that there’s an increase, albeit slight, in time delay from running with light load. A larger increase in randomness of the delays is also noted. See Figure 6.1 on the next page for a plot of the time delays under normal load. Finally a test was run under high load, as with the previous two tests, the time delay of 3000 samples of laser data were analyzed. This time however, 20 extra subscribers were connected to GIMnet subscribing to data from all services. This gave rise to a mean 41.

(59) 42. Chapter Six. Figure 6.1: The time delay for 3000 samples of laser data when running GIMnet under normal load. The dashed lined shows mean time delay.. time delay of 821µS, with a standard deviation of 441µS. Increasing the number of subscribers will not increase the time delay by much. Increasing the number of services, or the amount of data each service sends over GIMnet will. The time delays shown from these tests is nothing to worry about when running MICA under normal conditions. And should the need to add more services or subscribers arise later, GIMnet should be able to handle this, without time delay getting out of hand.. 6.2. Perfomance of the Circle Sector Expansion (CSE) Algorithm. When CSE was used together with variable “rabbit” distance, variable speed and variable safety margin in the program called “robot”, see 4.1 on page 17. MICA was able to drive autonomously through the corridors in the basement of the A-house in Lule˚ a university of technology. The positions of the wheels from the encoder and the laser scans was recorded during this trip and can be seen in Figure 6.2 on the next page. Where the blue dots are laser scan points, the green squares marks the path the wheelchair has traversed and the red dots is the path CSE said the wheel chair should take..

(60) 6.2. Perfomance of the Circle Sector Expansion (CSE) Algorithm. 43. Figure 6.2: This is a map produced from recorded data. The blue dots are laser scan points and the green squares are encoder data to indicate where MICA has been driving. This was recorded while MICA was driving autonomously using the CSE algorithm. MICA was driving in the corridors in the basement of the A-house in LTU.. The features in the robot program allows the CSE algorithm to send data with a relatively high frequency. The CSE algorithm can therefore recalculate the path several times a second which allows for something close to “real time” obstacle avoidance..

(61)

(62) C HAPTER 7 Discussion and Future work 7.1. 7.1.1. Discussion. The CSE Algorithm. CSE was implemented in order to test the GIMnet based software and drivers on MICA while driving autonomously. It collects data from the lasers, the gyro and the encoder for different purposes. This means that many different components are tested at the same time. This gives a good indication of how well the entire system is working together. The CSE was used in order to create an autonomous vehicle, in this case it means that once started the vehicle had no destination. Choosing the path to traverse was made in a rather simple manner. By simply using a weighted function, see Equation 5.3 on page 35, that looked at the location of the stop points at the end of each branch. The weighted function was designed so that the angle to the destination, the stop point, weighs more heavily then the distance. The angle to aim for can be changed though the angle mostly used was − π2 rad, with 0 rad being straight forward, this means that the robot preferred going left if it could. If there existed multiple paths to the left then the one with a stop point furthest to the left was most likely to be chosen. In Sven Rönnbäcks doctor thesis [22], concerning CSE, the desired destination was included as a part of the equations. The result from CSE is a list of center points in the local X,Y-coordinate system. These were sent as soon as a new calculation had been made. By trying to execute the algorithm as many times as possible one gets a rather good avoidance of non static obstacles, such as people walking in front of the robot, but the robot becomes slightly oscillating. If the algorithm were to be executed less then the robot would drive by using old data and it would become more stable, though the avoidance capabilities would decrease. 45.

(63) 46. Chapter Seven. 7.1.2. The Follow Me Algorithm. The subsquare method used in the Follow Me algorithm locks on to the target well. It is sensitive to disturbances because it lacks prediction of where the follow-me object will be in the next laser scan. It only searches for objects close to the previous position of the follow me object. By adding prediction of where the object will be at the next time instant, the performance could be improved.. 7.2 7.2.1. Future Work Future Controller Work. • The model of the wheelchair should be recreated using a person as extra weight. This would result in less compensation needed by the controller, as it would not see the extra weight as a deviation from the original model. After changing the model, the weights of the controller would have to be redesigned. • Further develop the controller to take into account the behavior of the caster wheels.. 7.2.2. Future GIMnet Work. • Some of the equipment mounted on MICA was not used. In further development of MICA these systems should be integrated by building drivers using GIMnet.. 7.2.3. Future Algorithm and Software Work. • The touchscreen has not been used to its full potential, clicking in a laser scan is limited to short distances. What could be done is to integrate CSE, touch screen, GPS, odometer (encoder data) and remote control, in order to create a Mobile Internet Connected Assistant that would be able to perform many different tasks. The CSE would be used in order to avoid obstacles. The GPS and the odometer would both be used in order to navigate both indoors and outdoors. A graphical user interface, similar to the ones found in todays GPS based navigation equipment, could be used in order to control the vehicle. The person sitting in the wheelchair would then have the option to either click in a laser scan, for short range navigation, or click in a map, for long range navigation. These options could also be implemented for remote control. • The MICA wheelchair is an experiment platform that is very well suited for testing of different algorithms. The interface between MATLAB and GIMnet is a contributing factor to this. The performance of the socket server, that is a major part in this interface, needs to be improved. Mainly it needs to be made more flexible..

(64) 7.2. Future Work. 47. • One thing that is important to know when testing algorithms is the time delay. It would therefore be good to develop and implement algorithms for precise time delay measurements. • To compensate for loss in precision of the gyro due to a tilted wheelchair, extra gyros could be added in order to measure the roll and pitch of the vehicle. • Dead reckoning could be improved in a variety of ways. The first way would be to take into account the time difference of measurement between the different sensors used when calculating the difference in position. Secondly, the precision could be improved significantly when moving on slippery surfaces by adding more inertial sensors such as accelerometers, as the dead reckoning fails completely if the no slip condition is not fulfilled. • Included in the robot program is a deactivated algorithm which output is a variable speed of the virtual wheel. This algorithm would be useful in the way that it can decrease the speed when a turn is coming up and increase the speed in a straight line. However, the current algorithm increases the speed during the end of a turn and if the target speed is high enough it will most certainly loose the grip to the surface. To make this work, a general way to prevent it from increasing its speed until the turn is finished, would be required.. 7.2.4. Further Driver Work. • The driver for each instrument has been developed in order to provide basic functionality. Though implementing further features into them would allow for the user to more easily customize the setup of the instruments to the specific needs of the user. An example of this is the drivers for the laser scanner. It is possible to send several configuration options to the lasers such as angular resolution, unit of the distance scanning and sensitivity etc. when the driver is initialized. What could be done is to allow for such options to be remotely changed, one example would be to allow such modifications to be done through the socket client in MATLAB. Though one should be very careful of enabling the possibility of modifying options that has to write to the EEPROM on the laser scanner. Because this memory has a limited amount of writes. Further development of all drivers and software should include making a graphical user interface for configuration purposes. • Develop the drivers further so that they can better compensate for errors such as abnormalities in the laser scan or the error in the gyro due to tilt..

(65)

(66) A PPENDIX A Code for the GIMnet protocols used on MICA A.1. The Servo Protocol. /** * * $Id: MICA_PROTOCOL_SERVO * */ #ifndef TMICA_PROTOCOL_SERVO_H #define TMICA_PROTOCOL_SERVO_H #include <inttypes.h> struct TMICA_PROTOCOL_SERVO{ public: double double double double double double double. velocity; rotation; heading; x; y; rwp; lwp;. //Timestamp double time; }; 49.

(67) 50. Appendix A. #endif. A.2. The Laser Protocol. /** * * $Id: MICA_PROTOCOL_LASER * */ #ifndef TMICA_PROTOCOL_LASER_H #define TMICA_PROTOCOL_LASER_H #include <inttypes.h> struct TMICA_PROTOCOL_LASER{ public: //Integer array for range measurements int laserData[181]; //Timestamp double timeStamp; //Displacement variables to compensate for non centered laser placement. double x_displacement; double y_displacement; double z_displacement; }; #endif. A.3. The Gyro Protocol. /** * * $Id: MICA_PROTOCOL_GYRO * */ #ifndef TMICA_PROTOCOL_GYRO_H.

(68) A.4. The Robot Protocol #define TMICA_PROTOCOL_GYRO_H #include <inttypes.h> struct TMICA_PROTOCOL_GYRO{ public: double yaw; double heading; double error; int index; //Timestamp double time; }; #endif. A.4. The Robot Protocol. /** * * $Id: MICA_PROTOCOL_ROBOT * */ #ifndef TMICA_PROTOCOL_ROBOT_H #define TMICA_PROTOCOL_ROBOT_H #include <inttypes.h> struct TMICA_PROTOCOL_ROBOT{ public: //Desired x,y Position double xpos[1000]; double ypos[1000]; int pos; //Desired x,y Orientation double xpos_d; double ypos_d;. 51.

(69) 52. Appendix A. // Return values for CSE double cseX; double cseY; double cseH; //Timestamp double timeStamp; }; #endif.

(70) A PPENDIX B Various information related to the controller B.1. MATLAB code for the design of the MIMO controller. 2.1.1. Robust Performance and Stability Test. s = tf(’s’); %%%% %11% %%%% Kp = 0.91102; %+-0.0016567 Tw = 0.23216; %+-0.0063727 Zeta = 0.47665; %+-0.015733 Td = 0.025; %+-0.025 G11 = Kp*exp(-Td*s)/(1+2*Zeta*Tw*s+(Tw*s)^2); % Plot transferfunction with uncertainties hold off; w = logspace(-4,3,500); bla(1:length(w))=0; for k = 0.90902:0.004:0.91302 for tw = 0.22516:0.014:0.23916 for zeta = 0.45665:0.04:0.49665 for td = 0.00:0.05:0.05 for n=1:length(w) 53.

(71) 54. Appendix B Go=Kp*exp(-Td*i*w(n))/(1+2*Zeta*Tw*i*w(n)+(Tw*i*w(n))^2); Gp=k*exp(-td*i*w(n))/(1+2*zeta*tw*i*w(n)+(tw*i*w(n))^2); bla(n)=abs((Gp-Go)/Go); end semilogx(w,bla);grid; hold on; end end end. end Wi(1:length(w))=0; for n=1:length(w) Wi(n) = 0.03*(i*w(n)/0.14+1)/(i*w(n)/10^1+1); end semilogx(w, Wi, ’--r’); % Loop-shaping of Liw w = logspace(-3,4); hold off; B1(1:length(w))=0; B2(1:length(w))=0; Liw(1:length(w))=0; for n = 1:length(w) Wi = abs(0.03*(i*w(n)/0.14+1)/(i*w(n)/10^1+1)); Gd = 1/(i*w(n)/0.0001+1); Wp = Gd; B1(n) = (1+Wp)/(1-Wi); B2(n) = (1-Wp)/(1+Wi); Liw(n) = 15/(i*w(n)/0.1+1); end loglog(w,B1);grid; hold on; loglog(w,B2);grid; hold on; loglog(w,Liw,’--r’);grid; % Robust Performance check. hold off; w=logspace(-3,6,1000); RP(1:length(w))=0;.

(72) B.1. MATLAB code for the design of the MIMO controller. 55. S11(1:length(w))=0; TT11(1:length(w))=0; for di = -1:2:1 for n = 1:length(w); Gd = 1/(i*w(n)/0.0001+1); Wp = Gd; Kiw = 15/(i*w(n)/0.1+1)*inv(Kp*exp(-Td*i*w(n))/(1+2*Zeta*Tw*i*w(n)+(Tw*i*w( Giw = Kp*exp(-Td*i*w(n))/(1+2*Zeta*Tw*i*w(n)+(Tw*i*w(n))^2); Wi = abs(0.03*(i*w(n)/0.14+1)/(i*w(n)/10^1+1)); Liw = Kiw*Giw*(1+di*Wi); Liw = 15/(i*w(n)/0.1+1)*(1+di*Wi); Siw=abs(1/(1+Liw)); Tiw=abs(Liw/(1+Liw)); RP(n)=abs(Wp*Siw)+abs(Wi*Tiw); S11(n)=Siw; TT11(n)=Tiw; end loglog(w,S11); hold on; loglog(w,TT11); loglog(w,RP,’-r’); end % Illustrate robustness. hold off; for di = -1:2:1 Gd = 1/(s*0.0001+1); Wi = 0.03*(s/0.14+1)/(s/10^1+1); Ls = 15/(s/0.1+1)*(1+di*Wi); Ge = -Gd/(1+Ls); step(Ge);grid; hold on; end %%%% %12% %%%% Kp = 0.30228; %+-0.00080213 Tp1 = 0.68966; %+-0.051718 Td = 0.025; %+-0.025.

(73) 56 G12 = -Kp*exp(-Td*s)/(1+Tp1*s); % Plot transferfunction with uncertainties hold off; w = logspace(-4,3,500); bla(1:length(w))=0; for k = -0.24:0.12:-0.12 for tp1 = 0.19:0.18:0.37 for td = 0.05:0.15-0.05:0.15 for n=1:length(w) Go=Kp*exp(-Td*i*w(n))/(1+Tp1*i*w(n)); Gp=k*exp(-td*i*w(n))/(1+tp1*i*w(n)); bla(n)=abs((Gp-Go)/Go); end semilogx(w,bla);grid; hold on; end end end Wi(1:length(w))=0; for n=1:length(w) Wi(n) = 0.4*(i*w(n)/0.8+1)/(i*w(n)/10^0.8+1); end semilogx(w, Wi, ’--r’); % Loop-shaping of Liw w = logspace(-3,4); hold off; B1(1:length(w))=0; B2(1:length(w))=0; Liw(1:length(w))=0; for n = 1:length(w) Wi = 0.4*(i*w(n)/0.8+1)/(i*w(n)/10^0.8+1); Gd = 1/(i*w(n)/0.0001+1); Wp = Gd; B1(n) = (1+Wp)/(1-Wi); B2(n) = (1-Wp)/(1+Wi); Liw(n) = 14/(i*w(n)/0.1+1); end loglog(w,B1);grid;. Appendix B.

(74) B.1. MATLAB code for the design of the MIMO controller. 57. hold on; loglog(w,B2);grid; hold on; loglog(w,Liw,’--r’);grid; % Robust Performance check. hold off; w=logspace(-3,6,1000); RP(1:length(w))=0; S12(1:length(w))=0; TT12(1:length(w))=0; for di = 1 for n = 1:length(w); Gd = 1/(i*w(n)/0.0001+1); Wp = Gd; Kiw = 14/(i*w(n)/0.1+1)*inv(Kp*exp(-Td*i*w(n))/(1+Tp1*i*w(n))); Giw = Kp*exp(-Td*i*w(n))/(1+Tp1*i*w(n)); Wi = abs(0.4*(i*w(n)/0.8+1)/(i*w(n)/10^0.8+1)); Liw = Kiw*Giw*(1+di*Wi); % Liw = 14/(i*w(n)/0.1+1)*(1+di*Wi); Siw=abs(1/(1+Liw)); Tiw=abs(Liw/(1+Liw)); RP(n)=abs(Wp*Siw)+abs(Wi*Tiw); S12(n)=Siw; TT12(n)=Tiw; end loglog(w,S12); hold on; loglog(w,TT12); end % Illustrate robustness. hold off; for di = -1:2:1 Gd = 1/(s*0.0001+1); Wi = 0.4*(s/0.8+1)/(s/10^0.8+1); Ls = 14/(s/0.1+1)*(1+di*Wi); Ge = -Gd/(1+Ls); step(Ge);grid; hold on; end.

No results found