autonomous monocycle robot conducted by gps …vbn.aau.dk/files/43401370/project.pdf · autonomous...
TRANSCRIPT
Autonomous Monocycle Robot Conductedby GPS Positioning
By Group 948Iulian BatrosValerio Ricci
Danish GPS Center
GPS Technology
Fredrik Bajers Vej 7C
http://gps.aau.dk
Abstract
Title
Autonomous Monocycle Robot
Conducted by GPS Positioning
Project period
Autumn semester 2010
Project group
GPS09gr948
Participants
Iulian Batros
Valerio Ricci
Supervisors
Kai Borre
Copies: 3
Date of completion: January 5 t h , 2011
Currently tasks on construction sites
and supply of big stores are
performed by human operators
(moving crates, activating other
devices, maintenance tasks). This
project would like to highlight the
use of a robot capable of performing
these tasks using the GPS signals
and to identify strengths and
weaknesses of this study work.
Several advantages in terms of
manufacturing and maintenance
costs are to be considered when
analyzing the benefits of this project
work.
In th is project, we propose the
local izat ion algori thm of a
monocyc le robot us ing di fferential
GPS and a hybr id strategy for a
rel iable mobi le robot in outdoor
environment.
4
Table of contents
Preface............................................................................. .................................5Aknowledgements.......................................................... ..................................5List of Figures................................................................ ..................................6
1 Introduction.................................................................................................7
2 Robotics2.1 Robot used....................................................................... ...................92.2 Parameters..........................................................................................112.3 Methods......................................................................... .....................14
3 GPS3.1 Overview............................................................................................153.2 GPS Structure................................................................ .....................163.3 Differential GPS.................................................................................19
4 Our Solution4.1 Problem Statement..............................................................................234.2 Approach.......................................................................... ...................24
5 Tests and Results5.1 DGPS..................................................................... ..............................295.2 Robot...................................................................................................315.3 Platform................................................................. ..............................34
6 Conclusions...................................................................................... ............35
Work Process..................................................................................................36
AppendixAppendix A GPS Code................................................................. .............37Appendix B Roomba.................................................................................39Appendix C Robotics Code.......................................................................42
References.......................................................................................... .............51
5
Preface
This document presents the work of group 948 on the 9th semester Master GPS Techology.It includes six chapters. The first chapter gives an introduction and background of thisproject work. The second and third chapters are devoted to GPS and monocycle robotoverviews. Problem statement is described in the fourth chapter followed byimplementation of our solution. Results and discussion are presented in the ongoing fifthchapter. Conclusions are given in the last sixth chapter.
On the Appendix section valuable technical information about the robot used can be foundas well as the code used throughout this project.References are given in square brackets that correspond to entries in the bibliographysection at the end of the document. A list of figures and nomenclature are also provided.
Acknowledgements
We would like to thank our supervisor Professor Kai Borre for guiding and advising usthrough the course of the project, as well as to Postdoctoral Researcher Darius Plausinaitisfor providing valuable information and advice on many difficulties and issues that aroseduring the course of this project. We would also like to express our gratitude to HenrikSchioeler for the guidance and help on the robotics part of our project.
Aalborg, January 5, 2011
Iulian Batros Valerio Ricci
___________ ___________
6
List of Figures
Figure 1 iRobot Create....................................................................................9
Figure 2 GPS Structure..................................................................................15
Figure 3 GPS Space Segment........................................................................16
Figure 4 GPS Satellite....................................................................................17
Figure 5 Position of the monitor stations and the master control station.......18
Figure 6 Differential GPS...............................................................................19
Figure 7 Topcon equipment............................................................................25
Figure 8 Topcon receivers used......................................................................31
Figure 9 ZigBee Modules...............................................................................33
Figure 10 iRobot Command Module..............................................................41
Figure 11 iRobot Serial Interface....................................................................42
Nomenclature
DGPS Differential GPSGNSS Global Navigation Satellite SystemsGPS Global Positioning SystemGRIL GPS Receiver InterfaceIEEE Institute of Electrical and Electronics EngineersMCS Master Control StationRTK Real Time KinematicUTM Universal Transverse MercatorWGS84 World Geodetic System 84
7
Chapter 1
Introduction
Currently tasks on construction sites and supply of big stores are performed by humanoperators (moving crates, activating other devices, maintenance tasks). This project would liketo highlight the use of a robot capable of performing these tasks using the GPS signals and toidentify strengths and weaknesses of this study work.
Several advantages in terms of manufacturing and maintenance costs are to beconsidered when analyzing the benefits of this project work. Due to this great potential wedecided to base this project around its study and implementation. This study work aims toopen doors to new areas of research by providing a platform for new applications and futureaugmentations of these applications. The GPS provides milimeter accuracy that is perfect forthis kind of application.
Our goal is to develop an autonomous mobile robot which can perform tasks in outdoorenvironment aided by GPS. In the last decades there have been remarkable developments inlocalization and mapping algorithm of navigation system for indoor/outdoor mobile robots.In indoor cases, personal service robots perform the missions of guiding tourists inmuseum, cleaning room and nursing the elderly. In outdoor cases, mobile robots have beenused for the purpose of patrol, reconnaissance, surveillance and exploring planets, etc.Many kinds of navigation algorithm of a mobile robot have been proposed. Most of thealgorithms, however, are developed for autonomous navigation. Therefore, navigationalgorithms for the teleoperated mobile robot field are necessary. A hybrid strategy for areliable mobile robot consists of a mobile robot and a control station.[6]
In this project, we propose the localization algorithm of a monocycle robot usingdifferential GPS. Further on, we present a hybrid strategy for a reliable mobile robot inoutdoor environment.
This was accomplished by starting with a standard iRobot Roomba Create andmodifying it to accomplish the tasks set out for the project.
The participants of this project are Valerio Ricci from Rome, Italy and Iulian Batros fromBucharest, Romania. Both participants have a background of electronics engineering. This
8
project was conducted under the master programme of GPS Technology at ElectronicsDepartment of Aalborg University under the supervision of Prof. Kai Borre and DariusPlausinaitis. The timeline of this project was from September 2010 to end of January 2011.
After analyzing the whole task of the project, we divided the work to project definitionand specification, preliminary research, development and troubleshooting, initial testing, writingof project and final testing
Although we initially created a timetable, due to constantly having temporary technicalissues and changing the tasks of the project, we had to adapt and to restructure our timetable inorder to avoid delays. Group meetings with our supervisor were appointed whenever necessaryand related to our current progress of the project.
We aimed at excellent efficiency per member, therefore we divided and also shared ourwork in professional manner so tasks of the project are fully achieved and concluded in adequatetime. As a side note to this project we had a good team spirit and we feel that this attitudedetermined us to break many technical obstacles we encountered and also boosted our skills.
9
Chapter 2
Robotics
2.1 Monocycle Robot used
“The term monocycle/unicycle is often used in robotics and control theory to mean a
generalised cart or car moving in a two-dimensional world; these are also often called
"unicycle-like" or "unicycle-type" vehicles. This usage is distinct from the literal sense of
"one wheeled robot bicycle".
These vehicles are typically shown as having two parallel driven wheels, one mounted on
each side of their centre, and (presumably) some sort of offset castor to maintain balance;
although in general they could be any vehicle capable of simultaneous arbitrary rotation and
translation. An alternative realization uses a single driven wheel with steering, and a pair of
idler wheels to give balance and allow a steering torque to be applied.
The Roomba is an autonomous robotic
vacuum cleaner created by iRobot
Corporation. Under normal operating
conditions, it is able to navigate a living
space and its obstacles while vacuuming
the floor.” [3]
Figure 1 iRobot Create
„The Roomba robotic vacuum cleaner is a
physical embodiment of iRobot subsumption
architecture. Roomba has no room map or
route plan. It has no overall view of what it is
doing. Instead it functions much more like an
insect: going toward things it likes (dirt,
power) and away from things it dislikes (walls, stairs), moving in predefined movement
10
routines while occasionally and randomly jumping out of a predefined routine.
This random walk feature of the Roomba algorithm is perhaps what confuses people the most
at first. It will seem to be going along doing the right thing when it suddenly takes off in a
different direction to do something else. But for every time it moves from the right place to
the wrong place, it has moved from the wrong place to the right place. On average (and if
left for a long enough time), Roomba covers the entire area. In terms of time efficiency, Roomba
is not the most effective, as it takes several times longer for it to fully cover a region than it
would for a person with a normal vacuum cleaner.” [1]
The iRobot Create is a reprogrammable version of the Roomba robot vacuum cleaner for roboticshobbyists, educators and researchers. Out of the package, users can use their PC to control therobot by sending numerical sentences over a serial connection.[1]
„Roomba is organized in three sections:
Sensor front: Virtually all of the sensors (bump, wall, cliff, and home base contac ts) are up
front. In fact, almost all the sensors are mounted on the movable front bumper. This movable
bumper both enables a mechanical means to measure contact (the give triggers a switch) and
absorbs shock to minimize damage. The Roomba firmware is desi gned to always travel
forward, so it places its most sensitive foot forward, as it were. When programming the
Roomba, you can subvert this tendency and make the Roomba drive backward, but doing so
makes it difficult for the Roomba to “see” anything.
Motor middle: The main drive motors, vacuum motors, vacuum brushes, side cleaning
brush, and battery are all in the center. This gives the Roomba a center -of-mass very close to
the center of its body, making it very stable when moving.
Vacuum back: Just like a normal vacuum cleaner, the entire back third contains the vacuum
and vacuum bag for holding dirt. The back can be removed when in ROI mode, which
slightly unbalances the Roomba and gives it more of a “hot rod” type of movement.
In December 2005, iRobot Corporation, the maker of the Roomba, recognized the growing
hacking community and released documentation describing the Serial Command Interface
(SCI) present on third-generation Roombas. In mid-2006 iRobot renamed the SCI to be the
Roomba Open Interface (ROI), a name that better fits its role. The ROI allows you to take full
control of the Roomba and its behavior. This is no simple remote control interface, but instead
a protocol that allows complete sensor readout and full actuator control.
Since the release of the SCI/ROI specification, there has been an explosion of new Roomba
hacks. The Roomba hacking community has blossomed to include not just professional
11
hardware engineers, but people from all experience levels, from normal people looking to play
with their Roomba in a new way to academics experimenting with low-cost robotics. The ROI
turns the Roomba into a true robotics platform. And because these are all reversible hacks, it’s
easy to try someone else’s hacks without breaking Roomba out.”[1]
Two obstacles to use, especially for educational purposes, are: (1) the cryptic nature of the
command interface; and (2) the difficulty in establishing a software serial port connection to the
robot. [5]
Two relevant sensor outputs for this project are the distance travelled by the robot and the
angle of turn.
The distance and angle values in the sensor data are odometry data, just like the car’s
odometer. Unlike a car odometer, the values can be either positive or negative. For distance,
that’s forward and backward; for angle, that’s counter-clockwise and clockwise.[5]
2.2 Parameters
Distance
The distance that Roomba has traveled in millimeters since the distance it was last requested.
This is the same as the sum of the distance traveled by both wheels divided by two. Positive
values indicate travel in the forward direction; negative in the reverse direction. [4]
The distance is obtained from the optical interrupter sensor on the wheels. The value comes
from counting the number of beam interruptions caused from the toothed interrupter disc. In
Roomba the disc has 32 slots and the gearing provides a 25:1 reduction between wheel and
disc. Thus the sensor’s angular and distance resolution of the turning wheel is:
360 degrees /(32 teeth x 25 gearing reduction) = 0.45 degrees
(68 mm diameter x pi) / (32 teeth x 25 gearing reduction) = 0.27 mm [4]
The Roomba Open Interface specification gives a distance resolution of 1 mm, and that’s
probably because 1 mm is the standard distance unit inside the Roomba and the distance value
has been averaged to give a more stable (but lower resolution) value. [1]
In any case, getting actually 1mm of precision from any real-world electromechanical device
is tough, let alone a consumer- grade vacuuming robot moving on variable surfaces. [1]
12
This mm accuracy is suitable to use alongside DGPS accuracy and to the specification stated
in the project description.
Angle
The angle that Roomba has turned since the angle was last requested. The angle is
expressed as the difference in the distance traveled by Roomba’s two wheels in millimeters,
specifically the right wheel distance minus the left wheel distance, divided by two. This makes
counter-clockwise angles positive and clockwise angles negative. [4] „This can be used to
directly calculate the angle that Roomba has turned through since the last request. Since the
distance between Roomba’s wheels is 258mm, the equations for calculating the angles in
familiar units are:
Angle in radians = (2 * difference) / 258
Angle in degrees = (360 * difference) / (258 * Pi).
Although the distance value is a straightforward measurement like a normal odometer, the
angle value is an odometrical difference. Roomba has a distance sensor on each wheel, and
the angle value in the sensor data is the difference in the distance traveled by each wheel.
Specifically, it is the right wheel difference minus the left wheel difference.” [1]
This difference describes a rotation around the center point between the two wheels. It’s as if
the wheels are moving partially along a circumference determined by the distance between
the wheels.[1]
Velocity
Both the distance and angle values returned in the sensor data are based on the last time the
sensors were read. Each time they are read, they are reset to zero. This makes the values relative
measurements (as opposed to absolute measurements). It also makes velocity calculations
easier. By reading the sensors at a time interval, you can determine the velocity (both
translational and angular) by simply dividing the distance and angle by the time interval. [1]
For commanding the robot a Bluetooth connection was further developed between the robot and
a laptop. This acts like a virtual serial interface and at some extent has the capabilities of a
13
normal serial interface, allowing users to override some autonomous tasks of the robot.
In order to talk to the Roomba Bluetooth adapter, the Bluetooth has to be configured
properly on the laptop and after paired up with the Roomba, so that the robot open interface
can be used. These actions involve pairing of the Bluetooth device on robot with the Bluetooth
dongle of the computer. Further on the virtual serial interface is set up and tested for possible
malfunctions before use.
Other two possible connections tested in this project are direct serial interface through the
iRobot Command Module and a ZigBee connection between robot and computer. The serial
connection responds much better than Bluetooth when trying to command the robot with our
code, possibly because this robot model is setup for predefined tasks through direct serial
connection and that a wireless medium adds additional issues. Additionally between the two
wirelles medias, ZigBee responds better than Bluetooth when initializing the robot. Bluetooth
seems to respond slow and to be uncertain at this point for further use.
„Because Bluetooth is a wireless protocol, it can suffer from interference, dropouts, and signal
strength issues like any other wireless protocol. This is reflected in various pauses in data
transfer between the computer and the Bluetooth device. Normally it should not really be an
issue, but it is something to be aware of.
When Bluetooth fails on a task, further tests are done with cable connection to rule out
possible code errors or other related issues. As testing usually tests a lot of time because of the
unforseen obstacles we have devoted most of our time for yhis project to troubleshooting some
of the problems that arose and at the same time on finding and adapting new solutions to our
current task.
The most noticeable difference between using a cable and using Bluetooth to control the
Roomba is the initial connect time can be longer if the computer has to re-connect and re-pair
with the Bluetooth dongle. It can be several seconds if the Bluetooth adapter has been idle or
turned off.”[1]
The MATLAB code used for the reading of the relevant Roomba sensors is shown in theAppendix section of this project.
„The Roomba Open Interface is a subset of the Create Open Interface. That means that all thecommands for Roomba work on Create; but not all the commands for Create work on theRoomba.
Timing: The PC and Matlab communicate at 57600 baud. That means there is a limit as tohow quickly the robot can send, receive and respond to commands.”[5]
14
2.3 Methods
„There are two ways sensors can be read. Method 1 is to use the AllSensorsReadRoomba function. This returns the values of all
the sensors in one call.
Method 2 is to use the individual sensor commands (AngleSensorRoomba,BatteryVoltgeRoomba, etc).
Method 1 AllSensorsReadRoomba (Advantage) is faster than multiple calls to individualsensors. (Disadvantage) Each time the distance and angle sensor are read they return the changesince the last time they were read, and then Create resets the encoder values. Therefore if weonly want to read the bumper values, calling AllSenorsReadRoomba will inadvertently resetthe distance and angular measurements. We need to track those values in your code if we aretrying to navigate via dead reckoning.
Method 2 individual calls (Advantages) is less cumbersome to call. And there is no risk ofresetting the distance and angle measurements. (Disadvantages) It is slow if we need to read manysensors. Also, with this approach we are not reading the sensors simultaneously. For exampleif we call DistanceSensor, then AngleSensor while the robot is driving, we need to accountfor the fact that the two measurements were not taken at the same time and the robot moved inthe time between the two calls.” [5]
15
Chapter 3
GPS
3.1 Overview
The Global Positioning System (GPS) is a satellite-based positioning system made up of a
constellation of 31 satellites placed into orbit by the U.S. Department of Defense. GPS was
originally intended for military applications, but in the 1980s, the government made the
system available for civilian use. GPS works in any weather conditions, anywhere in the world,
24 hours a day. There are no subscription fees or setup charges to use GPS. GPS comprises
three major system segments, Space, Control, and User.
Figure 2 GPS Major Segments
The Space Segment consists of a nominal constellation of 31 satellites. Each satellite
broadcasts RF ranging codes and a navigation data message. „The Control Segment consists of
a network of monitoring and control facilities which are used to manage the satellite
constellation and update the satellite navigation data messages. The User Segment consists of a
variety of radio navigation receivers specifically designed to receive, decode, and process
the GPS satellite ranging codes and navigation data messages.
The ranging codes broadcast by the satellites enable a GPS receiver to measure the transit
time of the signals and thereby determine the range between each satellite and the
16
receiver. The navigation data message enables a receiver to calculate the position of each
satellite at the time the signals were transmitted. The receiver then uses this information to
determine its own position, performing calculations similar to those performed by other
distance-measuring navigation equipment. Conceptually, each range measurement defines a
sphere centered on a satellite. The common intersection point of the spheres on or near the
earth's surface defines the receiver position.
For GPS positioning, a minimum of four satellites are normally required to be simultaneously
"in view" of the receiver, thus providing four range measurements. This enables the
receiver to calculate the three unknown parameters representing its (3-D) position, as
well as a fourth parameter representing the user clock error. Treating the user clock error as
an unknown enables most receivers to be built with an inexpensive crystal oscillator rather than
an expensive precision oscillator or atomic clock. Precise time estimates are required for
precise positioning, since a time error of 3 nanoseconds is roughly equivalent to a range error
of 1 metre. Less than four satellites can be used by a receiver if time or altitude is precisely
known or if these parameters are available from an external source.” [2]
3.2 GPS Structure
Space Segment
The GPS Space Segment consists of 31
satellites in semi-synchronous (approximately
12- hour) orbits. The satellites are arranged in
six orbital planes with four satellites in each
plane. The orbital planes have an inclination
angle of 55 degrees relative to the earth's
equator. The satellites have an average orbit
altitude of 20200 kilometres above the surface
of the earth.
Figure 3 Space Segment
„The satellites complete one orbit in approximately 11 hours and 58 minutes. Since the earth
is rotating under the satellites, the satellites trace a track over the earths surface which repeats
every 23 hours and 56 minutes. A user at a fixed location on the ground will observe the same
17
satellite each day passing through the same track in the sky, but the satellite will rise and set
four minutes earlier each day, due to the 4 minute difference between the rotational period of
the earth and two orbital periods of a satellite. The satellites are positioned in the orbital planes
so that four or more satellites, with a good geometric relationship for positioning, will normally
be observable at every location on earth.
Figure 4 GPS Satellite
The satellites transmit ranging signals on two L-band frequencies: Ll at 1575.42 MHz and L2
at 1227.6 MHz. The satellite signals are transmitted using spread-spectrum techniques,
employing two different ranging codes as spreading fictions, a 1.023 MHz
coarse/acquisition code (C/A- code) on L1 and a 10.23 MHz precision code (P-code) on both
L1 and L2. Either the C/A-code or the P-code can be used to determine the range between the
satellite and the user, however, the P-code is normally encrypted and available only to
authorized users. When encrypted, the P-code is known as the Y-code. A 50 Hz navigation
message is superimposed on both the P(Y)-code and the C/A-code. The navigation message
includes satellite clock-bias data, satellite ephemeris (precise orbital) data for the
transmitting satellite, ionospheric signal-propagation correction data, and satellite almanac
(coarse orbital) data for the entire constellation.” [2]
18
Control Segment
The control segment is composed of a master control station (MCS), an alternate master
control station, four dedicated ground antennas and more than ten dedicated monitor
stations. The MCS can also access U.S. Air Force Satellite Control Network (AFSCN)
ground antennas (for additional command and control capability) and NGA (National
Geospatial- Intelligence Agency) monitor stations. The flight paths of the satellites are
tracked by dedicated U.S. Air Force monitoring stations in Hawaii, Kwajalein, Ascension
Island, Diego Garcia, Colorado Springs, Colorado and Cape Canaveral, along with shared
NGA monitor stations operated in England, Argentina, Ecuador, Bahrain, Australia and
Washington DC. The MCS is the central processing facility for the Control Segment and
is responsible for monitoring and managing the satellite constellation. The MCS functions
include control of satellite station- keeping maneuvers, reconfiguration of redundant
satellite equipment, regularly updating the navigation messages transmitted by the satellites,
and various other satellite health monitoring and maintenance activities. The monitor stations
passively track all GPS satellites in view, collecting ranging data from each satellite. This
information is transmitted to the MCS where the satellite ephemeris and clock parameters
are estimated and predicted. The MCS uses the ground antennas to periodically upload the
ephemeris and clock data to each satellite for retransmission in the navigation message. [8]
19
Figure 5 Position of the monitor stations and the master control station
User Segment
The User Segment consists of receivers specifically designed to receive, decode, and process
the GPS satellite signals. Receivers can be stand-alone, integrated with or embedded into
other systems. GPS receivers can vary significantly in design and function, depending on
their application for navigation, accurate positioning, time transfer, surveying and attitude
reference. [2]
3.3 Differential GPS
Differential GPS was developed to meet the needs of positioning and distance measuring
applications that required higher accuracies than stand -alone GPS could deliver. A
typical differential GPS architecture consists of a reference receiver located at a
surveyed, known location, and one or more DGPS user receivers. The user receivers are
often called rover receivers because they are not confined to a fixed location like the
reference receiver. The fixed receiver antenna, differential correction processing
system, and data link equipment (if used) are collectively called the master station.
Both sets of receivers either collect and store the necessary data for later processing, or
send them to the desired location in real time via the data link.[2]
20
Figure 6 Differential GPS
„DGPS is based on the principle that receivers in the same vicinity will simultaneously
experience common errors on a particular satellite ranging signal. In general, the rover receivers
use measurements from the master receiver to remove the common errors. In order to
accomplish this, the rover receivers must simultaneously use a subset or the same set of
satellites as the master station. The DGPS positioning equations are formulated so that the
common errors cancel. Various DGPS techniques are employed depending on the accuracy
desired, where the data processing is to be performed, and whether real-time results are
required. If real-time results are required then a data link is also required. For applications
without a real-time requirement, the data can be collected and processed later. The accuracy
requirements usually dictate which measurements are used and what algorithms are
employed.
Carrier-Phase Differential
The carrier-phase measurement technique uses the difference between the carrier phases
measured at the master receiver and rover receiver. A double-differencing technique is used to
remove the satellite and receiver clock errors. The first difference is the difference between
the phase measurements at the user receiver and the reference receiver for a single satellite.
This eliminates the satellite clock error which is common to both measurements. This
process is then repeated for a second satellite. A second difference is then formed by
subtracting the first difference for the first satellite from the first difference for the second
satellite. This eliminates both receiver clock errors which are common to the first difference
equations. This process is repeated for two other pairs of satellites resulting in three double-
21
differenced measurements that can be solved for the difference between the reference station
and user receiver locations. This is inherently a relative positioning technique, therefore the
rover receiver must know the master station location to determine its absolute position.
The "raw" phase measurements are essentially a count of the number of carrier cycles between
the satellite and receiver positions. The number of cycles times the carrier wavelength is a
range measurement. The receivers can directly measure the fractional portion of the phase
measurement and can track phase shifts including whole cycles, but they must calculate the
initial whole number of cycles between the receiver and the satellite. This is referred to as the
integer cycle ambiguity.
If carrier track or phase lock on a satellite is interrupted and the integer count is lost, then the
initialization process must be repeated for that satellite (known as cycle clip). Output data
flow may also be interrupted if the receiver is not collecting redundant measurements from
extra satellites to maintain the position solution. If a precise position solution is
maintained, reinitialization for the "lost" satellite can be almost immediate. Developing a
robust and rapid method of initialization and reinitialization is the primary challenge facing
designers of real-time systems that have a safety critical application.
22
DGPS can also be implemented in several different ways depending on the type of data link
used. The simplest data link is no data link at all. For non-real-time applications, the
measurements can be stored in the receiver or on suitable media and processed at a later
time. In most cases to achieve surveying accuracies, the data must be post-processed using
precise ephemeris data that is only available after the survey data has been collected.
Similarly, for some test applications the cost and effort to maintain a real-time data link may
be unnecessary. Real-time outputs can be useful to confirm that a survey or test is
progressing properly even if the accuracy of the results will be enhanced later.
Differential corrections or measurements can be uplinked in real-time from the master
station to the rovers. This is the most common technique where a large number of users must
be served in real-time. If the rover receivers are passive as in GPS itself, an unlimited number
of users can be served. For military purposes and proprietary commercial services, the uplink
can be encrypted to restrict the use of the DGPS signals to a select group of rover receivers.
The accuracy of a DGPS solution developed using a single master station will degrade with
distance from the master station site. This is due to the increasing difference between the
master and rover receiver ephemeris, ionospheric, and tropospheric errors.
DGPS does more than increase positioning accuracy, it also enhances GPS integrity by
compensating for anomalies in the satellite ranging signals and navigation data message.”[2]
In order to maintain the integrity of a DGPS position solution, corrections must be provided
by the same master station or by time-synchronized reference stations. This is because the
correction values are dependent on the reference station clock. Mixing corrections from
unsynchronized reference stations can generate unpredictable clock and range errors of rover.
23
Chapter 4
Our Solution
4.1 Problem Statement
Currently tasks on construction sites and supply of big stores are performed by human operators
(moving crates, activating other devices, maintenance tasks). A robot capable of performing
these tasks using the GPS signals would be excellent. Due to this great potential we decided to
base this project around its study and implementation. The GPS provides milimeter accuracy
that is perfect for this kind of application.
Is GPS suitable to use for this kind of application? Additionally, what would be the strengths
and weaknesses of this approach?
Below objectives are to be accomplished.
1. Milimeter GPS accuracy from Topcon receivers used in differential mode
2. Robot command and control from MATLAB
3. Platform testing and troubleshooting
Our goal is to develop an autonomous mobile robot which can perform tasks in outdoor
environment aided by GPS. In this project, we propose the localization algorithm of a
monocycle robot using differential GPS. Further on, we present a hybrid strategy for a reliable
mobile robot in outdoor environment.
This was accomplished by starting with a standard iRobot Roomba Create and modifying it to
accomplish the tasks set out for the project.
24
4.2 Approach
Our solution is based on the use of RTK (Real time Kinematic), between a single reference
station and a rover station (positioned on the monocylce robot), to determine a better position of
the rover due to the real time corrections of differential GPS given by the reference station,
whose coordinates are well-known.
The RTK is crucial for our purpose because we need a method that gives us the very precise
position over time, because the robot moves continuosly, and a static method is not proper in
this case, in which all is dynamic.
The radio link between the Master station and the robot, is given by two radiomodems, and the
RTK is implemented in Matlab and GRIL (GPS Receiver Interface Language) language. So
with RTK we know the position of the moving robot with better accuracy. We calculate the
position of the rover in UTM, because we need a coordinates system that fits well for a 2D
problem.
The first step was to establish the links between the Master, the Rover, and the laptop. So we
have connected the Master to the laptop by serial connection, and Master with Rover by
radiolink using two SATEL radiomodems. We set the baudrate of the communication between
the Master and the Rover to the value of 38400, in Matlab code, the most suitable for the
frequency of the communication furnished by the radiomodems (433 MHz).
The second step, was the configuration of the two receivers, Master and Rover, for working in
RTK mode. We used Matlab code and GRIL language to obtain this, choosing the CMR
standard protocol for the communication messages between Master and Rover.
This choice was done to reduce the amount of data. Once established the configuration of the
system, we started to do real measurements of the Rover position, putting the Rover outside in a
known place, with known UTM coordinates.
25
Figure 7 Topcon equipment
To realize the measurements outside, we used a Topcon equipment for the structural support,
including a main vertical bar for the Rover antenna support, two other adjustable bars used like
„feet” for the balance, and two pockets, one for the Rover receiver and one for the radiomodem.
It was important to fix the main bar, in vertical direction, to do this we used a visual indicator
included in the equipment.
The position obtained from the measurements was compared with the known coordinates, and
the error estimated. The first error computed was very big, approximately 3km on the Northing
coordinate and 200m on the Easting coordinate.
We made a comparison between the measured coordinates of the Rover in UTM and the same
coordinates computed using Google Earth. Actually we did a conversion from geodetic
coordinates to UTM coordinates because Google Earth gives the result in geodetic coordinates.
In a first moment, we though that the reference ellipsoid used by Google Earth was different
respect than the our one, but was the same, WGS84.
Then we have tried to analyze the RTK code in MATLAB and some parameter of the receivers
by GRIL command, like the offset of the antenna, but we haven't found relevant errors in both
26
cases.
As a final check, to verify is the problem was the RTK, we tried to compute the position of the
Master station, with single positioning mode, so without using RTK, but the error on the
position obtained, compared with the one obtained with Google Earth, was almost the same.
After this we decided to focus the attention not in the absolute position of the Master and Rover
receivers, but in their baseline (the distance between them), and to fix the position of the master
as the origin of the coordinates system. In this way, we turned into another approach,
considering not the absolute position but the relative position between Master and Rover, that is
most relevant for our purpose.
To check the reliability of the RTK for the relative position computation of the Rover, we did
the measurements of the Rover position first by putting it in a fixed position and then by moving
it for 3 meters in East direction. The accuracy of the position computation was at the millimeter
level. This was an important goal, we obtained a very good accuracy on the position, crucial for
the future development of our project.
ROBOT
The robot used in our project is Roomba from iRobot ® and it includes the Rover receiver, the
antenna, and the radiomodem. Our aim is to guide the robot trajectory from a point A to a point
B, in a predetermined path, with an accuracy of the position of the final goal B, at the centimeter
or hopefully millimeter level. To do this, we divide the path in several intermediate nodal points,
in which the robot does a comparison between its real coordinates, computed using the GPS, and
the goal coordinates.
In each step the robot moves a certain distance, stops and computes the distance and azimuth
between its coordinates and the goal coordinates, makes the corrections on the trajectory, and
then, continues to move.
In each step the distance the robot has to run, decreases according to the distance from the goal.
So at the beginning we will have longer distance for each step because the robot is far away
from the goal, but when it becomes closer the distance for each step becomes shorter. The same
for the velocity, it changes every step depending on the distance from the goal, shorter the
27
distance, lower the velocity.
So we want to use azimuth as a feedback for the rotation of the wheels to get the correct
trajectory, and the distance from the goal as a feedback for the speed of the robot and the
distance to run.
Our solution includes the use of a Bluetooth connection between the computer and the robot, to
give and receive commands via computer. We use a first class Bluetooth device, with a range of
100m. Using the Bluetooth, the computer creates a connection, seen as a serial connection,
between computer and robot. In this way we can use MATLAB to give commands to the robot
and receive answer from the robot. Other two possible connections tested in this project are
direct serial interface through the iRobot Command Module and a ZigBee connection between
robot and computer. The serial connection responds much better than Bluetooth when trying to
command the robot with our code, possibly because this robot model is setup for predefined
tasks through direct serial connection and that a wireless medium adds additional issues.
Additionally between the two wireless medias, ZigBee responds better than Bluetooth when
initializing the robot. Bluetooth seems to respond slow and to be uncertain at this point for
further use. The ZigBee has a range of 500m, has the advantage to have lower probability of
interference than Bluetooth. The technical integration of Zigbee module into the platform was
kind of difficult though, because compared to the Bluetooth device which was already a build-in
module ready to use, the Zigbee modules were difficult to connect to the Command Module of
the robot or directly to the robot. First, because the module won’t get sufficient power directly
from the robot and second there were lots of issues regarding connection to the robot and several
conversions were done.
We received a lot of help from Henrik Schioeler regarding the ZigBee modules and pin
configuration and also from Peter Boye and Ben from the Electronics Workshop of Aalborg
University with the ZigBee modules, additional connections and whole technical integration and
successful building of the robot. We got spare parts and components from the Electronics
Components place of Aalborg University.
29
Chapter 5
Testing and results
5.1 DGPS Tests
First practical task of this project was represented by obtaining the milimeter accuracy with two
GPS receivers used in differential mode. One of the receivers is used as a master and the other
one represents the rover. RTK was used for the correction. This task was pursued with Topcon
receivers with the master placed inside our study space at the university somewhere close to the
window and the rover placed on a tripod outside on the place we decided to determine the
distance with milimeter accuracy. Initially we had several issues with the development of the
MATLAB code as this task was new to both of us and we took the time the combine the code
with the GRIL commands for RTK mode in order to succesfully use the receivers for our task.
We compared the position we obtained with the position we got from Google Maps and Google
Earth. Strangely we got a kilometer error and we decided to further investigate this issue as the
accuracy obtained is key to the final task of this project.
Several scenarios were considered and analyzed at this point. First was that we neglected some
ellipsoid characteristics when comparing both positions or while making the transphormation of
coordinates from one system to another. Second was the we used incorrectly GRIL commmands
and did the RTK setup not in a proper way.
Further steps were to determine the position with single point positioning, meaning we only
used one receiver to determine the position. We modified the code to proceed to this task but we
got the same error. In the meantime we determined also that the error is not related to ellipsoid
characterstics used and we tried to rule this out also because of time constraints.
We got after this an advice from our supervisor, Professor Kai Borre, that we should proceed
and neglect this obstacle as in the long run it is not relevant onto accomplishing our objective.
The idea is that we would use the „shifted” position of the rover, shifted because it is not the
30
correct on compared to the one from Google Earth, in order to determine if we have or not
milimeter accuracy so we can proceed with the platform testing (robot plus rover).
Initial tests have determined that we are close to the required accuracy and further test
determined that we obtained this accuracy. We checked this by moving the tripod with the rover
exactly by a predetermined distance measured with a ruler. We checked on several directions
and we got excellent results.
Needless to say future work should be devoted on finding the error on the receivers, but at this
point we can use the accurate information we got so far. We would use this accuracy as a solid
base of development off the hybrid platform. The next step on our testing plan was the tesing of
the robot. This is represented by the independent tesing of the robot, independent from the GPS
rover.
31
Figure 8 Topcon receivers used
5.2 Robot Tests
We seriously considered at the beginning using an Arduino platform in order to control the
robot. By researching the advantages and disadvantages of this approached we realized that we
would further add unnecesary obstacles and we would deviate from our main goal if we would
choose this path. The microcontroller of the robot should be enough for our current task.
First step of this testing part was determined by the method that we would further use to
32
command the robot from MATLAB. As the next step represented by the localization algorithm
depends a lot on the current part of testing we started this testing part from earlier times in the
project. Several scenarios of transmission media were considered – Serial connection using a
cable from robot to computer, and two wireless media – Bluetooth and Zig Bee.
First one that we tried was Bluetooth as it seemed to be the moost easier and intelligent way to
go. We proceeded with the pairing of the two devices in use and then creation of the virtual
serial interface that we would further use. We used MATLAB code in order to initialise,
command the robot and read sensor information in order to further integrate this with the code
used for position determination. First steps with Bluetooth surprisingly were very hard and we
encountered issues from delayed connection and failures on initialisation of robot. At this point
we thought that the code used is not suitable for Bluetooth, although it should be because of the
virtual serial interface. RealTerm is one of the softwares that we tried to determine with whether
we can solve these issues. We got several incompatibility issues of this software with the
Windows platform and also we had issues with the Bluetooth software used (mainly on
Windows Vista). These problems determined us to consider other means of connecting the
robot to the computer.
The next medium was the serial connection. Things went smoothly while using this method, but
because it is almost impossible to use a cable connection on a mobile application we had to go
with another method. At this point we got a helping hand from Henrik Schioeler and Peter Boye.
Henrik Schioeler gave us the idea of using ZigBee and borrowed us two devices to pair and use
for our project. Peter Boye helped us on integrating the device on the robot so we can do
fuurther tests. This activity is ongoing and is needless to say that results are expected from this
method.
33
Figure 9 ZigBee modules
„ZigBee is a wireless technology developed as an open global standard to address the unique
needs of low-cost, low-power wireless M2M networks. The ZigBee standard operates on the
IEEE 802.15.4 physical radio specification and operates in unlicensed bands including 2.4 GHz,
900 MHz and 868 MHz.
The 802.15.4 specification upon which the ZigBee stack operates gained ratification by
the IEEE in 2003. The specification is a packet-based radio protocol intended for low-cost,
battery-operated devices. The protocol allows devices to communicate in a variety of network
topologies and can have battery life lasting several years.
The ZigBee protocol has been created and ratified by member companies of the Zigbee
Alliance. Over 300 leading semiconductor manufacturers, technology firms, OEMs and service
companies comprise the ZigBee Alliance membership. The ZigBee protocol was designed to
provide an easy-to-use wireless data solution characterized by secure, reliable wireless network
architectures.”[7]
34
5.3 Platform Tests
The final step of the testing and whole project is marked by the testing of the integrated
platform. This platformed contains the robot as a base, a radiomodem, the rover we used so far
and a battery to power the rover. Integration is ongoing and also further results and findings are
to be determined.
The main idea behind this is that we have as task to adapt the MATLAB code used for
controlling serially the robot to the position of the robot. By having three predetermined
locations we would drive accurately the robot from point to point. All informations requested
from the sensors regarding travelelled distance and angle measurements play a key role in the
success of the testing.
35
Chapter 6
Conclusions
GPS is excellent for obtaining milimeter accuracy on the positioning of any type of robot inthe outdoor environment. Additionally setting up DGPS in the required area is very easy todo and also financially effective compared to other solutions. Some issues may appaerwhen using the robot indoors, where GPS signals cannot propagate, although lots ofsurfaces allow GPS signals to pass and can be further analyzed on future work as a solutionto this minor drawback.
Future work on this subject should be devoted to integration of the Kalman filter andspecific application testing for possible troubleshooting. The optimization of this projectwork would definetely polish the current work. Future work should also incorporatescenarios and troubleshooting for obstacle avoidance for keeping the robot trajectory assmooth as possible.
Customers will always aim for excellence and safety. RTK DGPS without a doubt cansustain this challenge.
36
Work process
The participants of this project are Valerio Ricci from Rome, Italy and Iulian Batros fromBucharest, Romania. Both participants have a background of electronics engineering. Thisproject was conducted under the master programme of GPS Technology at ElectronicsDepartment of Aalborg University under the supervision of Prof. Kai Borre and DariusPlausinaitis. The timeline of this project was from September 2010 to end of January 2011.
After analyzing the whole task of the project, we divided the work to project definitionand specification, preliminary research, development and troubleshooting, initial testing, writingof project and final testing. Although we divided the work, at the end of each task we shared andfinished the work together.
We initially created a timetable, but due to constantly having temporary technical issuesand changing the tasks of the project, we had to adapt and to restructure our timetable in order toavoid delays. Group meetings with our supervisor were appointed whenever necessary andrelated to our current progress of the project.
We aimed at excellent efficiency per member, therefore we divided and also shared ourwork in professional manner so tasks of the project are fully achieved and concluded in adequatetime. As a side note to this project we had a good team spirit and we feel that this attitudedetermined us to break many technical obstacles we encountered and also boosted our skills.
WBS Tasks Start End Dura
tion
(Day
s)
% Com
ple
te
Work
ing
Day
s
1 Project definition/Analysis 9.27.10 10.10.10 14 100% 102 Preliminary research 10.11.10 10.17.10 7 100% 53 Development DGPS 10.18.10 10.31.10 14 100% 104 Trounleshooting DGPS 11.01.10 11.07.10 7 100% 55 Development robotics 11.08.10 11.21.10 14 100% 106 Troubleshooting robotics 11.22.10 11.28.10 7 100% 57 Mixed troubleshooting 11.29.10 12.12.10 14 100% 10
8Transmission mediaanalysis 12.13.10 12.19.10 7 100% 5
8.1Development hybridplatform 12.20.10 1.12.11 24 75% 18
8.2 Testing 11.22.10 1.25.11 65 60% 47
37
Appendix
Appendix A
GPS Code
%RTK between Master and Rover receiver
ports={ 'COM4'};
period='40';
commM=openPorts (ports,period);
% Set up reference station (Master)
fprintf (commM, 'set, /par/rover/mode/,off'); fprintf (commM, 'set,
/par/base/mode/, off'); fprintf (commM, 'set, /par/pos/mode/cur, sp');
fprintf(commM, 'set,/par/ref/pos//geo,{W84,N57d00m53.030560s,E009d59m09.254910s
,59.998 } ');
fprintf(commM, 'dm,/dev/ser/c');
fprintf(commM, 'set, /par/dev/ser/c/imode,none');
fprintf(commM,'em,/dev/ser/c,cmr{/10:1,/0:1,/1:10}');
%RTK Rover configuration ports={ 'COM3'};
commR=openPorts (ports, '40'); %
Set up Rover station
38
fprintf (commR, 'set, /par/rover/mode/,off');
fprintf (commR, 'set, /par/base/mode/, off');
fprintf(commR, 'dm,/dev/ser/c');
% Enabling Rover to receive rtcm,cmr or jps message on c port
%fprintf(commM,'set,/par/dev/ser/c/imode,rtcm'); %non so se tutti o uno solo fprintf
(commR, 'set, /par/dev/ser/c/imode, cmr');
%fprintf (commM, 'set, /par/dev/ser/c/imode, jps');
%Selecting RTK for position computation
fprintf (commR, 'set, /par/pos/mode/cur,pd');
%Computing Rover coordinates in UTM coordinates system
fprintf (commR, 'em,, /msg/jps/{PG} :1');
a=input('How many registrations of position you want?');
i=0;
fid=fopen('C:\Users\Valerio\parsing Topcon\registrations.txt', 'w+t');
while (i<a)
PG=getPG(commR);
format long g
[NN,E] = geo2utm(PG.lat,PG.lon,32);
i=i+1;
y = [NN;E];
39
fprintf(fid,'NN = %6.2f E = %12.8f\n',y);
end
function [PG] = getPG(comm)%pag.8 GRIL
fprintf(comm,'em,,/msg/jps/{PG}:1');PG=[];
if fread(comm,1)==80disp('Receiving receiver position in geodetic coordinates');
else disp('error on receiver message'); return;endchar1=fread(comm,1);if char1==71
disp('correct message');else disp('incorrect message'); return;end
len= fread(comm,3);lat=fread(comm,1,'double');lon=fread(comm,1,'double');alt=fread(comm,1,'double');sigma=fread(comm,1,'single');type=fread(comm,1,'uint8');cs=fread(comm,1,'int8');line=fread(comm,1);
PG.len=len;PG.lat=(360*lat)/6.28;PG.lon=(360*lon)/6.28;PG.alt=alt;PG.sigma=sigma;PG.type=type;PG.cs=cs;
end
Appendix B
Technical Specifications iRobot
„The Roomba operates with internal nickel-metal hydride batteries (NiMH) and must be recharged
regularly from a wall plug, First-generation models needed to be told the size of the room via
three room size buttons (Small, Medium, and Large), but this is no longer required with second
and third-generation models.
The contact bumper detects bumping into walls and furniture, and the Virtual Walls limit the
Roomba to the areas that the owner desires with an infrared signal. Special Scheduler Virtual
40
Walls can be programmed to turn on at the same time the Scheduler-enabled Roomba is
activated. Four infrared sensors on the bottom of the unit prevent it from falling off ledges.
Second- and third-generation models have additional dirt sensors that allow them to detect
particularly dirty spots and focus on those areas accordingly.” [3]
Unlike the Electrolux Trilobite vacuuming robots, Roombas do not map out the rooms they
are cleaning. Instead, they rely on a few simple algorithms such as spiral cleaning (spiraling),
room crossing, wall-following and random walk angle-changing after bumping into an object or
wall. This design is based on MIT researcher and iRobot CTO Rodney Brooks' philosophy that
robots should be like insects, equipped with simple control mechanisms tuned to their
environments. The result is that although Roombas are effective at cleaning rooms, they take
several times as long to do the job as a person would. The Roomba will cover some areas many
times, and other areas only once or twice. [1]
„Accesories include Easy Clean Brush, Remote Control, iRobot Scheduler, Homebase, Virt ual
Wall, Virtual Wall Lighthouse, Advanced Power System (APS) Battery, Roomba Serial Control
Interface (Roomba SCI), BAM - A Bluetooth module that converts the RoomBa to Bluetooth
control from any bluetooth device, a Bluetooth dongle to allow control from BlueTooth enabled
devices, Roo232: allows programming input through a serial port connector, RoombaFX - a C#
class by RoboDynamics that implements the entire Roomba SCI command set.
Roomba comes with a Mini-DIN TTL serial interface, which is incompatible with standard
PC/Mac serial ports and cables, both electrically and physically. However, third -party adapters are
available to access the Roomba's computer via Bluetooth or RS -232 (PC/Mac serial). The
Roomba Open Interface (formerly "Roomba Serial Command Interface") API allows
programmers and roboticists to create their own enhancements to Roomba. Several projects are
described on Roomba hacking sites. In response to this interest, the company manufactured the
iRobot Create, with the vacuum cleaner motor r eplaced by a "cargo bay" for mounting devices
like TV cams, lasers, and even non-mobile robots. The Create provides a greatly enhanced, 25-pin
interface providing both analog and digital bidirectional communication with the hosted device.
Thus, it can then be used as the mobile base and wireless interface for completely new robots.
iRobot Create is explicitly designed for robotics development, rather than simply hacking the
Roomba. The Create also possesses a serial port through which sensor data can be rea d and motor
commands can be issued using the iRobot Roomba Open Interface protocol.
The platform accepts virtually all accessories designed for iRobot's domestic robots and can also
be programmed with the addition of a small "command module" (a microcontro ller with a USB
connector and four DE-9 expansion ports).
Due to the limitations in storage space and processing power of the iRobot Command Module,
41
many choose to utilize an external computer in controlling the Create robot. Since the built -in
serial port supports the transmission of sensor data and can receive actuation commands, any
embedded computer that supports serial communication can be used as the control computer.
A number of robot interface server / simulators support the iRobot Create. Most nota bly,
the Player Project have long included a device interface for the Roomba, and has recently
developed a Create interface in Player 2.1. The Universal Real -time Behavior Interface (URBI)
environment also contains a Create interface. The Microsoft Robotic s Studio simulation
environment contains a Create model.”[3]
Figure 10 iRobot Command Module
„The iRobot Command Module works with iRobot Create, providing a way to write
own programs in C or C++ to control Create and to add custom hardware to expand
Create’s capabilities without tethering the robot to a PC.
The Command Module plugs into Create’s
Cargo Bay Connector, and it has four
expansion ports that provide additional inputs
and outputs for your custom hardware. Three
of the connectors are on the top surface, spaced
to provide easy attachment points for an array
of sensors, and one connector is on the back
for easy access from the cargo bay.
Figure 11 iRobot Serial Interface
42
The Command Module is powered by an Atmel AVR ATMega1 68 microcontroller which
can be reprogrammed by downloading programs from your the computer with the included USB
cable.
The Command Module program can access all of Create’s sensors through the Open
Interface with the sensor command. When the command is sent, Create will reply
with all of the latest sensor information, in the byte order specified in the Open
Interface documentation, sending the result out of its serial port.
The Command Module comes with a bootloader pre -installed in the Flash memory.
The bootloader allows the download of own programs to the Command Module using
Programmers Notepad and avrdude and is capable of updating the rest of the
Command Module’s memory (Flash and EEPROM).
(0 = button not pressed, 1 = button pressed).”[4]
Appendix C
Robotics Code
„function [serPort] = RoombaInit(my_COM);% initializes serial port for use with Roomba% COMMport is the number of the comm port% ex. RoombaInit(1) sets port = 'COM1'% note that it sets baudrate to a default of 57600% can be changed (see SCI).% An optional time delay can be added after all commands% if your code crashes frequentlyglobal tdtd = 0.01;% This code puts the robot in CONTROL(132) mode, which means does NOT stop% when cliff sensors or wheel drops are true; can also run while plugged into chargerContrl = 132;
warning off
%% set up serial comms,% output buffer must be big enough to take largest message sizecomm = strcat('COM', num2str(my_COM));
43
a = instrfind('port',comm);if ~isempty(a)
disp('That com port is in use. Closing it.')fclose(a)delete(a)
end
disp('Establishing connection to Roomba...');
% defaults at 57600, can changeserPort = serial(comm,'BaudRate', 57600);set(serPort,'Terminator','LF')set(serPort,'InputBufferSize',100)set(serPort, 'Timeout', 1)set(serPort, 'ByteOrder','bigEndian');set(serPort, 'Tag', 'Roomba')
disp('Opening connection to Roomba...');fopen(serPort);
%% Confirm two way connumicationdisp('Setting Roomba to Control Mode...');% Start! and see if its aliveStart=[128];fwrite(serPort,Start);pause(.1)
fwrite(serPort,Contrl);pause(.1)% light LEDSfwrite(serPort,[139 25 0 128]);
% set songfwrite(serPort, [140 1 1 48 20]);pause(0.05)% sing itfwrite(serPort, [141 1])
disp('I am alive if my two outboard lights came on')
confirmation = (fread(serPort,4))pause(.1)
function [BumpRight, BumpLeft, BumpFront, Wall, virtWall, CliffLft, ...CliffRgt, CliffFrntLft, CliffFrntRgt, LeftCurrOver, RightCurrOver, ...DirtL, DirtR, ButtonPlay, ButtonAdv, Dist, Angle, ...Volts, Current, Temp, Charge, Capacity, pCharge] =
AllSensorsReadRoomba(serPort);% Reads Roomba Sensors% [BumpRight (0/1), BumpLeft(0/1), BumpFront(0/1), Wall(0/1), virtWall(0/1),CliffLft(0/1), ...% CliffRgt(0/1), CliffFrntLft(0/1), CliffFrntRgt(0/1), LeftCurrOver (0/1),RightCurrOver(0/1), ...
44
% DirtL(0/1), DirtR(0/1), ButtonPlay(0/1), ButtonAdv(0/1), Dist (meters since lastcall), Angle (rad since last call), ...% Volts (V), Current (Amps), Temp (celcius), Charge (milliamphours), Capacity(milliamphours), pCharge (percent)]% Can add others if you like, see code% Esposito 3/2008% initialize preliminary return valuesBumpRight = nan;BumpLeft = nan;BumpFront = nan;Wall = nan;virtWall = nan;CliffLft = nan;CliffRgt = nan;CliffFrntLft = nan;CliffFrntRgt = nan;LeftCurrOver = nan;RightCurrOver = nan;DirtL = nan;DirtR = nan;ButtonPlay = nan;ButtonAdv = nan;Dist = nan;Angle = nan;Volts = nan;Current = nan;Temp = nan;Charge = nan;Capacity = nan;pCharge = nan;
try
%Flush bufferN = serPort.BytesAvailable();while(N~=0)fread(serPort,N);N = serPort.BytesAvailable();end
warning offglobal tdsensorPacket = [];% flushing bufferconfirmation = (fread(serPort,1));while ~isempty(confirmation)
confirmation = (fread(serPort,26));end
%% Get (142) ALL(0) data fieldsfwrite(serPort, [142 0]);
%% Read data fieldsBmpWheDrps = dec2bin(fread(serPort, 1),8); %
BumpRight = bin2dec(BmpWheDrps(end)) % 0 no bump, 1 bumpBumpLeft = bin2dec(BmpWheDrps(end-1))
45
if BumpRight*BumpLeft==1BumpRight =0;BumpLeft = 0;BumpFront =1;
elseBumpFront = 0;
endWall = fread(serPort, 1) %0 no wall, 1 wall
CliffLft = fread(serPort, 1) % no cliff, 1 cliffCliffFrntLft = fread(serPort, 1)CliffFrntRgt = fread(serPort, 1)CliffRgt = fread(serPort, 1)
virtWall = fread(serPort, 1)%0 no wall, 1 wall
motorCurr = dec2bin( fread(serPort, 1),8 );Low1 = motorCurr(end); % 0 no over curr, 1 over CurrLow0 = motorCurr(end-1); % 0 no over curr, 1 over CurrLow2 = motorCurr(end-2); % 0 no over curr, 1 over CurrLeftCurrOver = motorCurr(end-3) % 0 no over curr, 1 over CurrRightCurrOver = motorCurr(end-4) % 0 no over curr, 1 over Curr
DirtL = fread(serPort, 1)DirtR = fread(serPort, 1)
RemoteCode = fread(serPort, 1); % coudl be used by remote or to communicate withsendIR commandButtons = dec2bin(fread(serPort, 1),8);ButtonPlay = Buttons(end)ButtonAdv = Buttons(end-2)
Dist = fread(serPort, 1, 'int16')/1000 % convert to Meters, signed, average distwheels traveled since last time called...caps at +/-32Angle = fread(serPort, 1, 'int16')*pi/180 % convert to radians, signed, since lasttime called, CCW positive
ChargeState = fread(serPort, 1);Volts = fread(serPort, 1, 'uint16')/1000Current = fread(serPort, 1, 'int16')/1000 % neg sourcing, pos chargingTemp = fread(serPort, 1, 'int8')Charge = fread(serPort, 1, 'uint16') % in mAhoursCapacity = fread(serPort, 1, 'uint16')pCharge = Charge/Capacity *100 % May be inaccurate%checksum = fread(serPort, 1)
pause(td)catch
disp('WARNING: function did not terminate correctly. Output may be unreliable.')end
function [AngleR] = AngleSensorRoomba(serPort);% Displays the angle in radians and degrees that Create has turned since the angle waslast requested.% Counter-clockwise angles are positive and Clockwise angles are negative.
46
%Initialize preliminary return valuesAngleR = nan;
try
%Flush BufferN = serPort.BytesAvailable();while(N~=0)fread(serPort,N);N = serPort.BytesAvailable();end
warning offglobal td
fwrite(serPort, [142]); fwrite(serPort,20);
AngleR = fread(serPort, 1, 'int16')*pi/180;pause(td)catch
disp('WARNING: function did not terminate correctly. Output may be unreliable.')end
function [Distance] = DistanceSensorRoomba(serPort);% Gives the distance traveled in meters since last requested. Positive% values indicate travel in the forward direction. Negative values indicate% travel in the reverse direction. If not polled frequently enough, it is% capped at its minimum or maximum of +/- 32.768 meters.
%Initialize preliminary return valuesDistance = nan;
try
%Flush BufferN = serPort.BytesAvailable();while(N~=0)fread(serPort,N);N = serPort.BytesAvailable();end
warning offglobal tdfwrite(serPort, [142]); fwrite(serPort,19);
Distance = fread(serPort, 1, 'int16')/1000;if (Distance > 32) | (Distance <-32)
disp('Warning: May have overflowed')end
pause(td)catch
47
disp('WARNING: function did not terminate correctly. Output may be unreliable.')end
function [] = SetFwdVelAngVelCreate(serPort, FwdVel, AngVel )% Specify forward velocity in meters/ sec% [-0.5, 0.5]. Specify Angular Velocity in rad/sec. Negative velocity isbackward/Clockwise. Caps overflow.% Note that the wheel speeds are capped at .5 meters per second. So it is possibleto% specify speeds that cannot be acheived. Warning is displayed.% Only works with Create I think...not Roombatry
%Flush BufferN = serPort.BytesAvailable();while(N~=0)fread(serPort,N);N = serPort.BytesAvailable();end
global tdd = .258; % wheel baselinewheelVel = inv([.5 .5; 1/d -1/d])*[FwdVel; AngVel];rightWheelVel = min( max(1000* wheelVel(1), -500) , 500);leftWheelVel = min( max(1000* wheelVel(2), -500) , 500);if ( abs(rightWheelVel) ==500) | ( abs(leftWheelVel) ==500)
disp('Warning: desired velocity combination exceeds limits')endfwrite(serPort, [145]); fwrite(serPort,rightWheelVel, 'int16');fwrite(serPort,leftWheelVel, 'int16');pause(td)catch
disp('WARNING: function did not terminate correctly. Output may be unreliable.')end
function [] = SetFwdVelRadiusRoomba(serPort, FwdVel, Radius);
% Moves Roomba by setting forward vel and turn radius% serPort is a serial port object created by Roombainit% FwdVel is forward vel in m/sec [-0.5, 0.5],% Radius in meters, postive turns left, negative turns right [-2,2].% Special cases: Straight = inf% Turn in place clockwise = -eps% Turn in place counter-clockwise = eps
try
%Flush BufferN = serPort.BytesAvailable();while(N~=0)fread(serPort,N);N = serPort.BytesAvailable();end
warning offglobal td
48
%% Convert to millimetersFwdVelMM = min( max(FwdVel,-.5), .5)*1000;if isinf(Radius)
RadiusMM = 32768;elseif Radius == eps
RadiusMM = 1;elseif Radius == -eps
RadiusMM = -1;else
RadiusMM = min( max(Radius*1000,-2000), 2000);end
%fwrite(serPort, [137, 255, 56, 1, 244])
fwrite(serPort, [137]); fwrite(serPort,FwdVelMM, 'int16'); fwrite(serPort,RadiusMM,'int16');disp('moving!')pause(td)catch
disp('WARNING: function did not terminate correctly. Output may be unreliable.')end
function travelDist(serPort, roombaSpeed, distance);%Moves the Create the distance entered in meters. Positive distances move the%Create foward, negative distances move the Create backwards.%roombaSpeed should be between 0.025 and 0.5 m/s
try
%Flush BufferN = serPort.BytesAvailable();while(N~=0)fread(serPort,N);N = serPort.BytesAvailable();end
warning offglobal tdif (roombaSpeed < 0) %Speed given by user shouldn't be negative
disp('WARNING: Speed inputted is negative. Should be positive. Taking the absolutevalue');
roombaSpeed = abs(roombaSpeed);end
if (abs(roombaSpeed) < .025) %Speed too lowdisp('WARNING: Speed inputted is too low. Setting speed to minimum, .025 m/s');roombaSpeed = .025;
end
if (distance < 0) %Definition of SetFwdVelRAdius Roomba, speed has to be negative togo backwards. Takes care of this case. User shouldn't worry about negative speeds
roombaSpeed = -1 * roombaSpeed;end
SetFwdVelRadiusRoomba(serPort, roombaSpeed, inf);distanceMM = distance * 1000;
49
fwrite(serPort, [156]); fwrite(serPort,distanceMM, 'int16');pause(td)SetFwdVelRadiusRoomba(serPort, 0, 0);pause(td)fwrite(serPort, [154]);while( serPort.BytesAvailable() ==0)
%disp('waiting to finish')enddisp('Done travelDist.')pause(td)catch
disp('WARNING: function did not terminate correctly. Output may be unreliable.')end
function turnAngle(serPort, roombaSpeed, turnAngle);%Turns the Create by turnAngle degrees or the shortest equivalent degree. PositiveturnAngle%up to 180 degrees and negative turnAngle from -180 to -360 degrees turns the Createcounterclockwise.%Negative turnAngle up to -180 degrees and positive turnAngle from 180 to 360 degreesturns the Create%clockwise. turnAngle > 360 or < -360 will be scaled to fit within +/- 360 degrees foroptimal performance.%turnAngle should be between +/- 360 degrees%roombaSpeed should be between 0 and 0.2 m/s
try
%Flush BufferN = serPort.BytesAvailable();while(N~=0)fread(serPort,N);N = serPort.BytesAvailable();end
warning offglobal td
if (roombaSpeed < 0) %Speed shouldn't be negativedisp('WARNING: Speed inputted is negative. Should be positive. Taking the absolute
value');roombaSpeed = abs(roombaSpeed);
end
if (abs(roombaSpeed) < .025) %Speed inputted is too lowdisp('WARNING: Speed inputted is too low. Setting speed to minimum, .025 m/s');roombaSpeed = .025;
end
if (turnAngle > 360 || turnAngle < -360) %sets range to +/- 360 degrees to avoidexcess turning
disp('Setting angle to be between +/- 360 degrees');if (turnAngle > 360)
while (turnAngle > 360)turnAngle = turnAngle - 360;
50
endelse
while (turnAngle < -360)turnAngle = turnAngle + 360;
endend
end
if (turnAngle > 180) %Sets the degrees to the shortest pathdisp('Setting turn path to shortest route. Going to turn clockwise');turnAngle = turnAngle - 360;
elseif (turnAngle < -180)disp('Setting turn path to shortest route. Going to turn counterclockwise');turnAngle = turnAngle + 360;
end
if (turnAngle < 0 ) % Makes sure the robot turns in the right directionturnDir = -eps;
elseturnDir = eps;
endSetFwdVelRadiusRoomba(serPort, roombaSpeed, turnDir);fwrite(serPort, [157]); fwrite(serPort,turnAngle, 'int16');pause(td)SetFwdVelRadiusRoomba(serPort, 0, 0);pause(td)fwrite(serPort, [154]);while( serPort.BytesAvailable() ==0)
%disp('waiting to finish')enddisp('Done turnAngle')pause(td)catch
disp('WARNING: function did not terminate correctly. Output may be unreliable.')end
“
51
References
1. Tod E. Kurt - Hacking Roomba, ExtremeTech, 20072. Navstar GPS User Equipment Introduction – Public Release, 19963. Wikipedia input on iRobot Roomba Create4. Roomba Open Interface manual5. Joel Esposito, Owen Barton, Joshua Koehler – Roomba.matlab – Matlab
Toolbox for the iRobot Create, 20096. Yutaka Erikawa, Yoshinobu Ando, Makoto Mizukawa – Using GPS Outdoor
Autonomous Driving System for Robot Development, International Conferenceon Control, Automation and Systems, Seoul 2008
7. http://www.digi.com/technology/rf-articles/wireless-zigbee.jsp
8. Wikipedia input on GPS