Upload
others
View
9
Download
0
Embed Size (px)
Citation preview
Tobias Looker, Keegan Brown,
Kade Turner and Mark Walbran
University of Auckland
Faculty of Engineering
SLAM Robot Project
Title 1
Date
Executive Summary This report details the design, development and testing of a Mecanum wheeled Simultaneous
localization and mapping robot. This robot was developed for the purpose of autonomous
vacuum cleaning of a room, whilst avoiding obstacles. Highlights of our design include its
simplicity, minimal hardware usage, modular software design and efficiency.
Unfortunately on the morning of assessment there were some hardware issues with the robot
and the robot did not perform as desired during demonstration. However, videos and camera
mapping of the robot have been provided, proving satisfactory performance of the robot the
day before demonstration and the night after.
Title 2
Date
Contents Executive Summary ........................................................ 1
Contents ......................................................................... 2
Figures ............................................................................ 4
Introduction .................................................................... 6
Robot Control ................................................................. 7
Motor Control ................................................................. 7
Trajectory Control .......................................................... 9
Wall Follow Control ........................................................ 9
Yaw Control .................................................................. 10
Control System Integration .......................................... 11
Position Control ............................................................ 12
Odometry ..................................................................... 12
Collision Detection ....................................................... 15
Blockage Identification ................................................. 16
Obstacle Detection ....................................................... 16
Scanning ....................................................................... 16
Geometry calculations .................................................. 17
Obstacle Avoidance ...................................................... 19
X Movement ................................................................. 19
Y Movement ................................................................. 20
Wall obstacle case ........................................................ 20
Movement arrays ......................................................... 20
Finite State Machine ..................................................... 22
Title 3
Date
Initializing ..................................................................... 23
Start .............................................................................. 23
Running ......................................................................... 25
Collision Detected......................................................... 25
Avoid Collision .............................................................. 25
Stop ............................................................................... 25
Code Structure .............................................................. 25
Sensors ......................................................................... 27
MPU .............................................................................. 27
Infrared Proximity Sensor ............................................. 29
Sonar ............................................................................. 30
Scanning ....................................................................... 31
Sensor Class .................................................................. 31
3D Design ...................................................................... 32
Mapping........................................................................ 33
Overview ....................................................................... 33
Algorithm ...................................................................... 33
Conclusions ................................................................... 35
References ...................................................................... 0
Title 4
Date
Figures Figure 1 - All motors RPM to PWM relationship ............................................................................. 7
Figure 2 - Linear speed to RPM relationship ................................................................................... 8
Figure 3 - Full forward and reverse PWM to RPM relationship ...................................................... 9
Figure 4 - Spiral algorithm path ..................................................................................................... 10
Figure 5 - Wall following control system diagram ......................................................................... 10
Figure 6 - Yaw control system diagram ......................................................................................... 11
Figure 7 - Control system integration depiction............................................................................ 11
Figure 8 - Position control system diagram ................................................................................... 12
Figure 9 - Global Vs Local Co-ordinate system diagram ................................................................ 13
Figure 10 - Odometry without complementary filter ................................................................... 14
Figure 11 - Odometry without corner creep adjustment .............................................................. 15
Figure 12 - Odomety with complementary filter and creep adjustment ...................................... 15
Figure 13 Obstacle Scan ................................................................................................................ 16
Figure 14: Obstacle diameter diagram .......................................................................................... 17
Figure 15: Local X and Y position offset diagram .......................................................................... 18
Figure 16: Local X and Y obstacle avoidance path ......................................................................... 21
Figure 17 - FSM Diagram ............................................................................................................... 22
Figure 18: 4 stages of initial positioning in the start state ............................................................ 24
Figure 19 - Class Diagram .............................................................................................................. 26
Figure 20 SMD Decoupling Capacitor ............................................................................................ 29
Figure 21 Correct Sensor Orientation ........................................................................................... 29
Figure 22 Long Range IR Calibration .............................................................................................. 30
Figure 23 Short Range IR Calibration ............................................................................................. 30
Figure 24 Scanning Head Photo Interrupter.................................................................................. 31
Figure 25 Photo Interrupter Circuit ............................................................................................... 31
Figure 26 Stepper mount Design Revisions ................................................................................... 32
Title 5
Date
Figure 27 FStepper Mount CAD ..................................................................................................... 32
Figure 28 Scanning Head Design Iterations ................................................................................... 32
Figure 29 Final CAD Design ............................................................................................................ 33
Figure 30 Final Robot Hardware Design ........................................................................................ 33
Figure 31 Final Robot Path When Operational.............................................................................. 36
Title 6
Date
Introduction This project aimed to develop an autonomous robot capable of mapping its environment and
plotting its path in this environment; commonly known as a SLAM robot. This problem is a
difficult one to solve, since reliable position information requires good map data, and vice versa.
Since we begin with neither, any errors in measurements can easily be compounded and a
positive feedback situation can occur, with increasingly poor data.
Our robot uses two IR sensors (one mounted facing to the right and one mounted to a stepper
motor shaft) to receive information about the robot’s surroundings. Position information is
calculated from odometry of wheels, and the orientation of the robot measured by a 9-axis
MPU. This information is processed by the Arduino sketch, and instructs the robot to explore its
surroundings, covering all open ground and avoiding obstacles.
The critical tasks were identified (motor control and movement, sensor management and
scanning, map generation and presentation). These tasks were then split between the members
of the group, and were constructed separately during the early stages. Some of the big problems
that arose came from code execution timing, keeping data current across all modules, and
dealing with measured data and the noise associated with sensor readings.
Title 7
Date
Robot Control
Motor Control During the initial stages of testing the provided robot, it became clear that there were significant
differences between each of the motors speeds when provided with the same PWM signal.
Furthermore the speed changes of the motors appeared non-linear to the increases in the PWM
signal for each motor which caused further complications with the initial motion of the device.
To account for these discrepancies and investigate the actual relationship between PWM and
the motors speed a series of tests were conducted using a hand held tachometer. From the
results an equation for the relationship between PWM and motor speed for each motor in both
directions was obtained and as suspected the relationships came out as an exponential as
shown below.
Figure 1 - All motors RPM to PWM relationship
To form a precise representation of the motor characteristics a 4th order polynomial was used
instead of the exponential. After obtaining a relationship for the forward and backward motion
of each motor, the equations were implemented into the code. Instead of directly writing PWM
values to the each of the motors, speed values were used. These speed values through the
equations obtained yielded PWM values to achieve the desired speed. This resulted in more
predictable and desired motion of the robot and the ability to vary the robots speed linearly.
To prove that the motor equations obtained resulted in the linear speed change of the robot
another series of tests were conducted. The robot was driven in all 4 main orientations for 1.5
meters at a series of speeds for each motor ranging from 35 to 115 RPM. The time to complete
these paths was recorded and used to obtain the cars average velocity over the trajectory. The
resulting data showed a linear relationship for each direction of travel verifying that the 4th
order motor equations work as required.
0
100
200
300
400
500
600
0 20 40 60 80 100 120 140
All Motors
Title 8
Date
Figure 2 - Linear speed to RPM relationship
As shown above this testing verified the initial linearization from PWM to RPM was correct. The
equation obtained through this testing was essentially the same as what would be obtained
through theoretical RPM to linear velocity conversion. However creating this relationship
through theoretical means would mean the slip and other correction factors would be left out
so it was determined that a more direct means of calculation would yield more accurate results.
Once the motor speeds of the robot were converted to the required linear speeds inverse
kinematics were applied to the overall X and Y velocity vectors to equate the components
required from each motor. The resulting equations are shown below.
𝑉𝑀𝑜𝑡𝑜𝑟_𝑇𝑜𝑝_𝐿𝑒𝑓𝑡 = 𝑌𝑠𝑝𝑒𝑒𝑑 + 𝑋𝑠𝑝𝑒𝑒𝑑 − 𝑌𝑎𝑤𝑠𝑝𝑒𝑒𝑑
𝑉𝑀𝑜𝑡𝑜𝑟_𝑇𝑜𝑝_𝑅𝑖𝑔ℎ𝑡 = −𝑌𝑠𝑝𝑒𝑒𝑑 + 𝑋𝑠𝑝𝑒𝑒𝑑 − 𝑌𝑎𝑤𝑠𝑝𝑒𝑒𝑑
𝑉𝑀𝑜𝑡𝑜𝑟_𝐵𝑜𝑡𝑡𝑜𝑚_𝑅𝑖𝑔ℎ𝑡 = 𝑌𝑠𝑝𝑒𝑒𝑑 − 𝑋𝑠𝑝𝑒𝑒𝑑 − 𝑌𝑎𝑤𝑠𝑝𝑒𝑒𝑑
𝑉𝑀𝑜𝑡𝑜𝑟_𝐵𝑜𝑡𝑡𝑜𝑚_𝐿𝑒𝑓𝑡 = −𝑌𝑠𝑝𝑒𝑒𝑑 − 𝑋𝑠𝑝𝑒𝑒𝑑 − 𝑌𝑎𝑤𝑠𝑝𝑒𝑒𝑑
The structure of the resulting motor control system consisted of two main classes, Motor and
MotorController. For each of the 4 robot Motors, a Motor class was instantiated. This class
contained the equation representing the motion of the motor in the forward and reverse
direction. A desired speed is sent to this class which is converted by the equation into the
required PWM signal. The MotorController class then sent the desired speed commands to the 4
instances of the motor class, Motorcontroller received commands from the higher level control
in the form of an X, Y and Yaw speed. These commands were then converted into the required
speeds from each motor.
y = 0.0026x + 0.017
0
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0 20 40 60 80 100 120 140
Forward Velocity
Title 9
Date
Figure 3 - Full forward and reverse PWM to RPM relationship
Trajectory Control This controller was the first implemented above the motor controller, the purpose of this layer
of control was used to instruct the robot on how to move. The inputs to this controller were X
and Y speeds. These commands were handed down to the next level of control called motor
controller which converted the speeds into the required speeds from each motor. A speed
factor from this level of control was handed back, this factor scaled the speeds to ensure that
over saturation of one or more of the motors didn’t occur. This controller was integrated with
wall follow control and yaw control explained below through a prioritization system.
Wall Follow Control
The chosen approach for the robot to complete the map was through a spiral pattern starting
from a corner and spiraling inward. To accomplish this method a combination of a wall following
control system using a side mounted medium range IR sensor and a yaw controller using the
MPU was implemented. Once the robot was re-initialized after orientating in a corner the yaw
and wall follow control systems were enabled. The original yaw set point was 0 which held the
robot straight, the wall follow set point was 50 which held the robot far enough away from the
wall that it was at no risk of hitting the wall. Using the collision detection system with the
permanently front mounted long range IR sensor and the rotatable long range IR sensor. When
the sensors detected something in front the system could distinguish if it was look at a wall or
an obstacle (explained later). With this ability when the robot reached what it identified as a
wall the yaw set point would decrease by 90 degrees. After rotating a total of 270 degrees
-150
-100
-50
0
50
100
150
-600 -400 -200 0 200 400 600
Tacho RPM
Title 10
Date
clockwise when the robot next identified a wall not only would the yaw set point adjust but the
wall follow set point would increase by 1 width of the robot.
Figure 4 - Spiral algorithm path
Figure 5 - Wall following control system diagram
Yaw Control
To ensure the robot moved in straight lines as required a yaw control system was implemented,
this proportional controller received a set point and a reference signal in the form of the MPU’s
current yaw value. These two provided the error signal which was then when processed through
a proportional controller that outputed the required yaw speed of the robot. Problems initially
arose when the robot was travelling and correcting dynamically because sometimes motors at
the lowest control level were being sent the product of a yaw speed and x or y speed that when
Start
Position
End
Position
+
-
Kp Motors Wall Setpoint Wall Error Local X speed Robot X speed
IR sensor
sensor
Measured wall distance
Title 11
Date
added was unachievable by the motor. This resulted in the oversaturation of the motor and
therefore would cause the motor to stop. To correct this problem a speed factor was introduced
at this level which meant when a motor was sent an invalid speed it would return a scale factor
that reduced the speed to an obtainable level. This factor was then applied to the other 3
motors to ensure the instructed overall motion was still achieved.
Figure 6 - Yaw control system diagram
Control System Integration
During the main execution of the robot through the map a combination of the yaw controller,
wall follow controller and trajectory control was required like in the instance shown below.
Figure 7 - Control system integration depiction
+
-
Kp Motors Yaw Setpoint Yaw Error Yaw speed Robot Yaw
MPU Measured Yaw
Yaw
Setpoint
Wall
Setpoint
MPU
Heading
Right IR
Reading
Forward
Trajectory
Title 12
Date
When these two controllers were initially combined problems arose because the control
systems attempted to compete with each other. To prevent this the following priority system for
the controllers was established.
1. Yaw Controller
2. Wall Follow Controller
3. Trajectory Controller
When the error of the yaw controller was greater than 10 degrees it disabled the trajectory and
wall follow control system until the error was reduced sufficiently. When the yaw error was
within a tolerable range but the wall follow controller had an error greater than 10. The
trajectory controller was disabled until this error was reduced. The results of this integration
meant the robot moved in a much more controlled fashion.
Position Control Once the three controllers explained above were combined another level of control was added
to enable the robot to move to instructed positions. This functionality was achieved through an
additional method in direction controller which called the existing method that combined the
three other controllers. Shown through the control system diagram below the method reported
back to what state called it either achieving set point or set point achieved. This distinction
allowed the logic calling this method to know when to send the robot a new positional set point.
Figure 8 - Position control system diagram
Odometry Above the motor control logic is the DirectionController class, this object takes in the desired
speeds in the X and Y and the desired direction. The class then instructs MotorController on how
to achieve the required trajectory by using the MPU as a sensor. In addition to this the
DirectionController class also returns the current position each time it is called. The class
estimates the current position by first using the equations obtained in the second part of testing
to convert the motor RPM speeds into the current x and y velocity of the robot. This resulting
figure is then integrated with time to gain the required positional information. Through tuning
+
-
Kp Motors Position
Setpoint
Position Error Direction Speed Robot Speed
Odometry Estimated Position
Title 13
Date
the equations with a correction factor, odometery within 2cm was achieved. In order for this
information to be useful to the map function the position information being generated needed
to be converted to a global co-ordinate system rather that a set of axes set to the robot.
Figure 9 - Global Vs Local Co-ordinate system diagram
To achieve this the direction control class was expanded to take in the current MPU reading, by
having this information the function could then determine how the local x and y position change
components belonged to global position vectors. To ensure the global co-ordinate axis were
orientated with the map correctly, after the initiation of the robot to find a corner of the map
the MPU was re-initiated setting the heading to zero.
Without limiting the possible speeds in the x and y of the robot unnecessarily the
implementation of a speed factor was used to ensure none of the motors saturated and
therefore stopped but also allowed the action to be performed at the max possible speed if
desired. When the x, y and yaw speed are passed to the MotorController class they are
converted into the required speeds of each motor to achieve the motion. Before converting this
RPM value to PPM, the value is checked to ensure it is not higher than the max RPM speed
allowed by the motor (100RPM). If the required speed of the motor is above this, a factor is
calculated to scale the motors value to 100. This factor is then applied to the other 3 motors to
ensure that the resulting motion is what was instructed. This factor is then passed back out of
the MotorController class to instruct the DirectionController class what the actual x, y and yaw
speeds achieved were to ensure the estimate of position performed by this class is accurate.
As shown in the above motor characteristic equations each motor featured a dead band of
around 25 RPM. To prevent creep from this source of error the positional estimating module
ignored any speeds below 25 RPM treating them effectively as zero speed.
Y
X
X
Y
Global coordinates defined by start position
Local coordinates fixed to robot
Title 14
Date
After the raw tunning of the odometry results such as that shown below were achieved where a
moderate amount of creep occurred which accumulated as the robot ran. To cure this problem
and improve the odometry a complimentary filter was added to position estimation using the IR
sensors. Every time the direction control class updated the robots position in a particular
direction the robot checked the relevant IR sensor. If this sensor was in range and sampled a
valid position for both the current and previous iteration the difference in the sensor reading
was combined via a complementary filter with the linear speed integration.
Figure 10 - Odometry without complementary filter
The results of the addition of a complementary filter eliminated the creep in the odometry as
the robot ran in straight lines along a wall. Below is the results the robot provided from a full run
of the map without obstacles. The final problem that was identified was creep occurring at the
corners as the robot rotated to re aligned with a wall and continue wall following. This error is
shown in the graph below through the corners showing a shift in the y when in reality the robot
was rotating on the spot. To eliminate the error the controller prioritization discussed in the
control system integration section was linked to the position estimation module. This allowed
the position estimation method the ability to know definitively the robots current motion and
consequently eliminated this problem.
𝑃𝑜𝑠 𝐶ℎ𝑎𝑛𝑔𝑒 = (∫ 𝑉(𝑡) ∗ 𝑑𝑡 𝑡
𝑡−1
) ∗ 𝑉𝑒𝑙𝑜𝑐𝑖𝑡𝑦𝑊𝑒𝑖𝑔ℎ𝑡𝑖𝑛𝑔 + (𝐼𝑅(𝑡) − (𝐼𝑅(𝑡−1)) ∗ (1 −
∗ 𝑉𝑒𝑙𝑜𝑐𝑖𝑡𝑦𝑊𝑒𝑖𝑔ℎ𝑡𝑖𝑛𝑔)
Where
𝑉𝑒𝑙𝑜𝑐𝑖𝑡𝑦𝑊𝑒𝑖𝑔ℎ𝑡𝑖𝑛𝑔 < 1
-200
0
200
400
600
800
1000
1200
-2500 -2000 -1500 -1000 -500 0
Odometry without complementary filter
Title 15
Date
Figure 11 - Odometry without corner creep adjustment
The final odometry is shown below with the robot completing the map including the avoidance
of an obstacle. The small amount of creep that is visble of the final loop of the robot was due to
the gyro on the MPU drifiting which accumulated and by the time the robot was in the center of
the map the motion of the robot was slightly offset.
Figure 12 - Odomety with complementary filter and creep adjustment
Collision Detection The approach to collision control involved using the stepper motor located on the center of the
robot that feature a sonar and IR sensor mounted to it. During execution of the main running
state a function inside the CollisionDetection class called handlecollision was called each cycle to
check for anything in the robots immediate path. If something was detected the robots FSM
then transitioned into a state called collision detected, once in this state another function inside
-1200
-1000
-800
-600
-400
-200
0
-100 400 900 1400 1900
Odometry without corner creep adjustment
-1100
-900
-700
-500
-300
-100-50 450 950 1450 1950
Odomety with complementary filter and creep adjustment
Title 16
Date
the CollisionDetection class was called to determine if what is blocking the robots path is a wall
or an obstacle. To make this distinction the function reviewed the data from the sonar and IR’s
last scan with the stepper motor. If the distances in the array showed uniform change for
example there were no large changes in distance values then the function deduced that it was
looking at a wall. If the detection was deemed to be a wall and the robot was in the
mapping/running mode as opposed to initialize. The robot would then call a function from the
collisiondetection class to determine how to avoid the object. The strategy to make this
determination required 3 key pieces of information that could be extracted from the data array
received from the sweep of the sonar and IR sensor.
Blockage Identification
Once a blockage had been detected through the constant scanning of the stepper-mounted
Infrared sensor, it needed to be determined whether that blockage was an obstacle or a wall. To
accomplish this a function called IdentifyBlockage was called whenever a blockage had been
detected. This function performed a scan facing directly forward and then at ±15 steps (±27˚)
from forward.
If the difference between either the +27˚ and 0˚ or -27 ˚ the 0˚ distances is greater than the
obstacle tolerance (defined at 100mm) than the scanned blockage must be an obstacle. If any of
distances is out of range of the Infrared sensor (>1000mm) than the blockage must also be an
obstacle, as a blockage would not have been detected over 1000mm away from a wall.
Obstacle Detection
Scanning
Once the three point scan has determined that the blockage is indeed an obstacle the sensor
class is called and the stepper motor does a ±22 step (±40˚) scan with the infrared sensor
mounted on top. An example output from such a scan is shown as a radar plot in Figure 13
below. Distances are represented on the radial axis (in mm) with scan angle being represented
on the angular axis.
Figure 13 Obstacle Scan
Title 17
Date
The start and end positions of the obstacle (StartStep and EndStep) were determined through
searching the data and obtaining the array indexes where the positive and negative jumps in
distance occur.
There were initially issues when the walls behind the obstacle were out of range of the IR
sensors. This problem was fixed by counting a start point as either a negative jump in distance
or a jump from zero to a positive integer, the last start point detected was then taken as the
start point of the obstacle. End points were counted as either positive jumps in distance or a
jump from a positive integer to zero, the first end point detected was used to ensure that it was
the actual end point of the obstacle.
They were then saved and used later in the calculations along with the distance values obtained.
Geometry calculations
With the array of scan data available and the start and end positions of the obstacle recorded,
the next step was to calculate the diameter of the obstacle. Start Dist. was obtained as the
distance value at array index [StartStep] and with this information the obstacle diameter was
calculated. This can be seen demonstrated in Figure 14 and equations below.
Figure 14: Obstacle diameter diagram
𝜃 = (𝑆𝑡𝑎𝑟𝑡𝑆𝑡𝑒𝑝 − 𝐸𝑛𝑑𝑆𝑡𝑒𝑝) ∗𝜋
100
𝐷 = 2 ∗ 𝑆𝑡𝑎𝑟𝑡𝐷𝑖𝑠𝑡 ∗ sin (𝜃
2)
X
Y
θ/2
D
θ/2
Title 18
Date
Once the diameter (D) of the obstacle had been calculated, this could then be used along with
the offset angle (Ф) to calculate the X and Y offsets of the obstacle in the local co-ordinate
system (centred on the robot). This can be seen represented graphically in Figure 15 below.
Figure 15: Local X and Y position offset diagram
An array of distance values was passed back by the sensor class from the stepper-mounted IR
sensor. These values represented the distance from the centre of the robot to the nearest
blockage in millimetres. The IR reading were taken at intervals of 1.8˚ (1 step of the stepper
motor). The distance to the closest edge of the obstacle could be calculated by iteratively
searching the array of distance values and recording the minimum value found. Once this was
determined, the distance to the centre of the obstacle could be determined as shown below.
𝐶𝑒𝑛𝑡𝑟𝑒𝐷𝑖𝑠𝑡 = 𝑚𝑖𝑛𝑉𝑎𝑙𝑢𝑒 +𝐷
2
The angular offset of the obstacle relative to the robot (Ф) was required in radians. This could be
calculated from the start/end positions of the obstacle and size of the scan array through the
following equations.
𝑂𝑓𝑓𝑠𝑒𝑡𝑆𝑡𝑒𝑝 = (𝑆𝑡𝑎𝑟𝑡𝑆𝑡𝑒𝑝 + 𝐸𝑛𝑑𝑆𝑡𝑒𝑝)/2 −𝐴𝑟𝑟𝑎𝑦𝑆𝑖𝑧𝑒
2
Ф (𝑟𝑎𝑑) = 𝑂𝑓𝑓𝑠𝑒𝑡𝑆𝑡𝑒𝑝 ∗𝜋
100
X
Y L
W D
Wall D
ist.
Local Y Offset
Local X
Offset
Ф
Title 19
Date
Once the angle of offset and distance to the centre of the obstacle were determined these could
be converted between polar and Cartesian co-ordinates using basic trigonometry and the
diagram shown below.
𝐿𝑜𝑐𝑎𝑙𝑋𝑂𝑓𝑓𝑠𝑒𝑡 = 𝐶𝑒𝑛𝑡𝑟𝑒𝐷𝑖𝑠𝑡 ∗ 𝑠𝑖𝑛(Ф)
𝐿𝑜𝑐𝑎𝑙𝑌𝑂𝑓𝑓𝑠𝑒𝑡 = 𝐶𝑒𝑛𝑡𝑟𝑒𝐷𝑖𝑠𝑡 ∗ cos (Ф)
Obstacle Avoidance
X Movement
If the robot was to the right of the obstacle and there was sufficient distance between the wall
and obstacle for the robot to fit through then the robot would move right around the obstacle.
These two conditions are represented through the equations below.
𝐿𝑜𝑐𝑎𝑙𝑋𝑂𝑓𝑓𝑠𝑒𝑡 < 0
𝑊𝑎𝑙𝑙𝐷𝑖𝑠𝑡 > 𝐿𝑜𝑐𝑎𝑙𝑋𝑂𝑓𝑓𝑠𝑒𝑡 + 𝑊 + 𝐷 + 2 ∗ 𝐶𝑙𝑒𝑎𝑟𝑎𝑛𝑐𝑒
Movement right around the obstacle was accomplished by setting the local X movement as
below.
𝐿𝑜𝑐𝑎𝑙𝑋𝑀𝑜𝑣𝑒 = 𝐿𝑜𝑐𝑎𝑙𝑋𝑂𝑓𝑓𝑠𝑒𝑡 +𝑊
2+
𝐷
2+ 𝐶𝑙𝑒𝑎𝑟𝑎𝑛𝑐𝑒
If the robot was either to the left of the obstacle or obstacle was too close to the wall to fit the
robot then the robot would move left. This was accomplished as follows.
𝐿𝑜𝑐𝑎𝑙𝑋𝑀𝑜𝑣𝑒 = 𝐿𝑜𝑐𝑎𝑙𝑋𝑂𝑓𝑓𝑠𝑒𝑡 −𝑊
2−
𝐷
2− 𝐶𝑙𝑒𝑎𝑟𝑎𝑛𝑐𝑒
Ф
Local Y Offset
Local X
Offset
Title 20
Date
Y Movement
The y movement was broken up into two stages. Firstly, the robot had to move until it was close
to the obstacle (as scanning did not take place right in front of the obstacle. This was done as
follows.
𝐿𝑜𝑐𝑎𝑙𝑌𝑀𝑜𝑣𝑒 = 𝐿𝑜𝑐𝑎𝑙𝑌𝑂𝑓𝑓𝑠𝑒𝑡 −𝐿
2−
𝐷
2− 𝐶𝑙𝑒𝑎𝑟𝑎𝑛𝑐𝑒
Then, once the robot had moved to the side of the obstacle it had to move forward far enough
so that the rear of the robot had cleared the obstacle before re-initiating wall-following. This
was done as follows.
𝐿𝑜𝑐𝑎𝑙𝑌𝑀𝑜𝑣𝑒𝑇𝑤𝑜 = 𝐿𝑜𝑐𝑎𝑙𝑌𝑂𝑓𝑓𝑠𝑒𝑡 +𝐿
2+
𝐷
2+ 𝐶𝑙𝑒𝑎𝑟𝑎𝑛𝑐𝑒
Wall obstacle case
If a start position is detected for the obstacle but no end position then it must be against a wall.
If this is the case then a slightly different approach is taken for robot movement.
The obstacle is assumed to be directly in front of the robot and since no end position can be
obtained all calculations are based on the centre position and the start position. An offset angle
of zero is assumed and everything is calculated as such. The other difference is that the robot
always moves left around the obstacle, and its Local X Offset is assumed to be zero.
Movement arrays
Once the x and y movements had all been determined the final step was to place them in arrays
to determine the exact robot path. Two arrays were created, with 3 cells in each, to represent
the changes in local X and local Y co-ordinates respectively. The path of robot, and population of
the arrays is best represented graphically as seen in Figure 16 below.
Title 21
Date
Figure 16: Local X and Y obstacle avoidance path
X
Y
Local Y Move
Local X
Mo
ve
Local Y Move Two –
Local Y Move
Title 22
Date
Finite State Machine The highest level of intelligence for the robot is the FSM, this determined the different modes of
operation for the robot. This FSM is defined in the main sketch of the program, each state call
functions from the set of classes required to run the robot.
Figure 17 - FSM Diagram
Start
Running
Collision
Detected
Avoid
Collision
Stop
Title 23
Date
Initializing
This state runs the initialization code for the robot at first run, this is where the setup functions
for the lower level classes were called.
Start
This state is the first to be called after Initializing which instructs the robot to move, the purpose
of this state is to home the robot to a corner by finding a wall becoming parallel to it then
moving along the wall until a corner is found. Because there are two phases to the start state in
terms of the robots orientation (Finding a wall and finding a corner), if the state exits to handle
collision detection it must know where it got to when it re-enters the start state. For example if
the robot became parallel to a wall, detected an obstacle and consequently transitioned to the
collision detection state then when the collision had been avoided the robot transitioned back
to the start state. The state would need to skip to half way through i.e. not instructing the robot
to find a wall, instead find a corner.
When the robot first starts up there are 3 basic steps it completes. Throughout all these steps,
obstacle detection and avoidance is run. During the start state, when a blockage is identified as
an obstacle, the robot rotates 20˚ and rescans, this is done continuously until an orientation is
found in which there are no obstacles blocking the wall.
First, it drives straight to the closest wall at a moderate. It continues doing this until it is within a
set distance of that wall, deemed a suitable distance for scanning and judging perpendicular
angles.
First, it drives straight to the closest wall at a moderate speed. It continues doing this until it is
within a set distance of that wall, deemed a suitable distance for scanning and judging
perpendicular angles.
The final stage of movement for the robot is to drive forwards until a corner is reached. Once
the robot is within an acceptable distance of the corner it rotates 90˚ counter clockwise once
more, placing it in the back right corner of the room. The robot can now begin coverage of the
room.
The 4 stages of initial positioning are represented graphically in Figure 18 below.
Title 24
Date
Figure 18: 4 stages of initial positioning in the start state
1
2 3
4
Title 25
Date
Running
This state is the main portion of the code, it runs the robot through the spiral wall following
algorithm. For each run of this state it updates the robots position checks for a collision and
handles any changes in direction based on a collision information. If the collision detection
algorithm “identifyblockage” returns its found a wall then the running state runs code to handle
the wall following behavior. If “identifyblockage” returns it has found a wall then the running
state exits and returns to the FSM to transition to the collision detected state.
Collision Detected
If this state is called then “identifyblockage” called from either the start state or running state
has indicated that an object has been detected. The purpose of this state is to run the required
code needed to calculate how to avoid the object. The output of this state is the generation of
an array of absolute positions the robot must travel to avoid. This state returns to the FSM to
transition to the avoid collision state.
Avoid Collision
This state is called from collision detected, it runs the robot through a position array of points to
avoid an obstacle. Once finished based on the state where the collision was originally detected
the FSM transitions back to that state for example either the running or start state.
Stop
Once the main running state determines that the map is complete it trasitions the FSM to this
state. The purpose of this stop state is to disable the control systems and therefore stop the
robot.
Code Structure The robot is run through multiple classes, each designed for a specific task. All the classes belong
to a hierarchy, with the top level controllers declared in the main sketch. Each top level
controller may contain other minor classes to assist with even more specific tasks. A brief
rundown of these controllers is given, along with how they operate in conjunction with each
other.
DirectionController
o This controller is used to move the robot to set points, control the speed of the
robot, correct the yaw orientation, and control the wall following algorithm.
This class receives instructions based on the Sensor data, and contains the
Motor controller class.
SENSORS
o This class handles taking measurements for the two IR sensors, the MPU, and
moving the stepper motor to perform sweeping scans. It populates a global
array with scan data to be used by other classes.
Title 26
Date
MapMaker
o This class handles the construction and addition of data to a map of the robot’s
surroundings in memory. It reads data from the SENSORS class, and position
information from the DirectionController class.
CollisionDetection
o This class handles all the logic surrounding the identification of any blockage to
the robot’s path (wall or obstacle), and calculates the path required to avoid
obstacles. It used the SENSORS scan functions.
Motor
o There is an instance of this class for each of the 4 motors on the robot each
instance contains an equation that represents the motors PWM to RPM
relationship. When a motor is instructed to move it is sent a RPM speed which is
then converted to the required PWM.
MotorController
o This class takes X Y and Yaw speeds from the DirectionController class and
converts them into the speeds required from each motor to achieve the desired
motion. The class also instantiates the four instances of the motor class
therefore knowing the characteristics of each motor.
Figure 19 - Class Diagram
Title 27
Date
Sensors
MPU The 9 axis motion processing unit (MPU) is used to provide accurate information about the yaw
heading of the robot, this enables the direction controller to control the heading of the robot
using a closed loop controller. The MPU used is the InvenSense MPU-9050, which communicates
over the I2C bus with the Arduino MEGA
The MPU contains 3 axis gyro for sensing the angular rate of change of the robot, a 3 axis
accelerometer which detects acceleration and a 3 axis magnetometer which can detect the
strength of the surrounding magnetic field. The MPU contains an on-board digital motion
processor (DMP) capable of performing advance data fusion usually completed on the
microprocessor connected to the MPU. This offloading of the data fusion to the DMP allows
increased operation speed of the Arduino MEGA as it is not being slowed down by complex
algorithms.
To program the MPU a set of internal registers require setting on each start-up of the robot, a
further set of data has to be written to the MPU to enable the DMP functionality. To help with
this setup routine a library was created by Jeff Rowberg [1], this library uses data from the
InvenSense MPU-6050 register map [2] which is an almost identical MPU but lacks the
magnetometer.
The DMP data is accessed over the I2C bus through a set of registers forming a First-In-First-Out
(FIFO) queue, which contains individual sensor readings and DMP quaternion data. The MPU has
an interrupt pin which can be used to inform the Arduino MEGA that the FIFO queue has been
filled i.e. new data is available. Upon using this interrupt problems were encountered with the
interrupt adversely affecting the operation of other portions of code which should not be
interrupted. To get around this the interrupt pin is not utilised and instead the FIFO queue is
constantly polled checking if new data is available. A problem encountered with polling the FIFO
queue is that if it is not polled at a high enough rate the queue can overflow which causes the
MPU to give errors out and proceed to stop running. To ensure the queue is read often enough
the update rate of the MPUs DMP was decreased to 50𝐻𝑧 and the queue is read every loop of
the main FSM loop.
The DMPs quaternion data requires re-orientating to a more useable format for use with the
robot, this was achieved by converting the quaternion to roll-pitch-yaw rotations using functions
built into the MPU library.
The re-orientated DMP data only fuses the gyro and accelerometer data and does not include
the magnetometer data. On the Arduino MEGA the data was fused with the magnetometer for
just the yaw orientation. This was achieved by using a complimentary filter which through
experimentation was found to require a 0.7 weighting for the magnetometer heading and a thus
a 0.3 weighting on the DMP data, the equation used is shown below:
𝑌𝑎𝑤𝐹𝑖𝑙𝑡𝑒𝑟𝑒𝑑 = 0.3 × 𝑌𝑎𝑤𝐷𝑀𝑃 + 0.7 × 𝐻𝑒𝑎𝑑𝑖𝑛𝑔𝑀𝑎𝑔
Title 28
Date
The filtered yaw now showed absolute heading relative to north, but it was noted that when
motors were operated near the MPU the heading became unreadable, this was believed to be
due to electromagnetic interference (EMI). Mounting of the MPU at differing locations was
investigated to minimise the EMI but it was decided that the risk associated with fusing the
magnetometer data was not worth the slight increase in performance.
The MPUs DMP requires axis offsets to be calculated for the gyro and accelerometer, these
offsets allow for a much faster initialisation of the MPU as it has a starting point for the
calibration procedure. The calculation of these offsets was performed using Jeff Rowbergs
MPU6050_calibration.ino sketch which waits for the MPUs values to stabilise and returns the
required offsets. It was found that every time the MPU was remounted these offsets required
recalculating as the MPUs orientation changes very slightly. The final offsets used are shown
below:
𝑋𝐺𝑦𝑟𝑜𝑂𝑓𝑓𝑠𝑒𝑡 = 109
𝑌𝐺𝑦𝑟𝑜𝑂𝑓𝑓𝑠𝑒𝑡 = −9
𝑍𝐺𝑦𝑟𝑜𝑂𝑓𝑓𝑠𝑒𝑡 = −8
𝑋𝐴𝑐𝑐𝑒𝑙𝑂𝑓𝑓𝑠𝑒𝑡 = −5363
𝑌𝐴𝑐𝑐𝑒𝑙𝑂𝑓𝑓𝑠𝑒𝑡 = −788
𝑍𝐴𝑐𝑐𝑒𝑙𝑂𝑓𝑓𝑠𝑒𝑡 = 1058
These offsets improved the DMPs initialisation time from approximately 8 𝑠𝑒𝑐𝑜𝑛𝑑𝑠 to less than
1 𝑠𝑒𝑐𝑜𝑛𝑑 and completely eliminated yaw drift.
To get the most accurate MPU data the MPUs was located as close to the center of rotation, this
minimises rotation induced accelerations (centripetal and tangential) that are perceived by the
accelerometer as described by Wei, Khosla & Riviere, 2003 [3]. To further reduce any drift in the
yaw output from the DMP the MPU was soft mounted the Robot using double sided foam tape,
this acts as a high frequency vibration damper (low pass filter for the accelerometer) while also
mounting the MPU securely.
Title 29
Date
Infrared Proximity Sensor
The supplied sharp Infrared Proximity Sensors GP2Y0A21 and GP2Y0A02 both take
approximately 40 𝑚𝑠 to measure distance and require a 5𝑉 supply which requires a 10𝜇𝐹
bypass capacitor across Vcc and GND to ensure sufficient supply current [4].
Using a power supply and oscilloscope the power in Vcc line was probed to check the noise
before and after the addition of a bypass capacitor. It was
found that the 5𝑉 supply dropped as much as 30% which
would adversely affect the analog output, hence a 10𝜇𝐹
ceramic surface mount capacitor was soldered directly
between Vcc and GND on the sensor. Mounting the
capacitor directly to the sensor helps to remove losses in
the wires from the power supply and creates a smaller
package not requiring any external interface board before
connecting to the Arduino MEGA.
The supplied datasheets [4] describes that the sensor orientation relative to movement in the
perpendicular plane of the sensors direction is best in the vertical orientation depicted in Figure
21.
To make the best use of the analog output of the IR sensors the analog reference for the
Arduino MEGA was set to the 3𝑣3 reference as the output from the IR sensors ranged from
0𝑉 𝑡𝑜 3𝑉, this meant that the analog to digital converter (ADC) resolution was used fully.
To linearise the IR Sensors a plot of 1/analog voltage vs. distance was created for the sensors
range, this was because the relationship described in the sensors datasheet described an inverse
relation [4]. To gather this data an average of 100 readings taken every 50𝑚𝑠 at each distance
was recorded with an Arduino UNO communicating over the serial port, using an Arudino UNO
allowed for uninterrupted use of the robot while others focused on the sensor calibration.
The plots Figure 23 & have had a linear curve fit applied to find the equation for the line for the
long range (200-1500mm) sensor and the short range sensor (100-800mm) respectively. Both
best fit lines have very good R2 values 0f around 0.992 & 0.998 respectively.
Figure 20 SMD Decoupling Capacitor
Figure 21 Correct Sensor Orientation
Title 30
Date
The output from the IR sensors was jittery and required post filtering to smooth the output. This
was accomplished through the use of a rolling average of the 15 consecutive values, the average
was only taken if the 15 consecutive values fell within 98% of each other. This helped to create
distance outputs that were accurate to within 5mm.
Due to the highly non-linear output from the IR sensors, when the distance to be measured was
outside the range of the sensors it would report a range that was just inside either end of the
measurable distances. To help negate this problem the effective output of the sensors was
decreased to 100 − 600𝑚𝑚 for the short range sensor and 200 − 1000𝑚𝑚 for the long range
sensor, any distance outside these ranges is reported as 0𝑚𝑚. The sensors have also been
positioned such that the minimum range of the sensors is within the robots size and so no wall
or obstacle can get within this minimum range.
Sonar A large portion of time was spent on trying to get consistent and repeatable distance values
from the sonar range finder. The analog, pulse width and serial output were all used but even
with filtering method like used for the IR sensors a reliable output could not be achieved and
Figure 23 Short Range IR Calibration
y = 113462x + 34.226R² = 0.9924
0
200
400
600
800
1000
1200
1400
1600
0 0.002 0.004 0.006 0.008 0.01 0.012 0.014
GP2Y0A02 Calibration
Figure 22 Long Range IR Calibration
y = 324.31x - 40.404R² = 0.9981
0
100
200
300
400
500
600
700
800
900
1000
0 0.5 1 1.5 2 2.5 3
GP21YA02 Calibration
Title 31
Date
considering there was four IR sensors for use the robot the sonar range finder was abandoned in
favor of sweeping an IR sensor back and forth to cover
the same area as the sonar does.
Scanning
To rotate the forwards facing IR sensor a stepper motor
was implemented. A stepper motor has several
advantages over the supplied RC servo motor, including
precise and repeatable steps of 1.8° and the ability to
rotate a full 360°. To initialize the stepper motor a
known position a photo interrupter is used as a switch to
detect when the stepper faces forwards. The circuit
diagram for the photo interrupter is shown in Figure 25
[5].
To drive the stepper motor a simple dual H-bridge bi-polar
stepper driver was used. Contorlling the two H-bridges is done
using four of the ARDUINO MEGAs digital pins an dthe
AccelStepper library provided by Mike McCauley under a
GPL V2 license agreement. This library made it simple to
command the stepper to move to a set position.
Sensor Class
A sensor class was created that contains functions for accessing each sensors data via pointers
and has the following functions: start() which sets up all the sensors, irRead() which updates an
array with IR range finder readings, frontScan() which is used to scan between a set angle in
front of the robot and populate a scan array with IR distance values from every 1.8°,
checkFront() which is used while driving to sweep back and forward and detect any obstacles in
front of the robot while it is moving forwards and finally scanThreePoints() which returns the
distance to three equally spaced angle to distinguish between a wall and an obstacle.
Figure 24 Scanning Head Photo Interrupter
Figure 25 Photo Interrupter Circuit
Title 32
Date
3D Design The mechanical design of the robot went through an iterative process in which parts were
incrementally improved until the desired functionality was reached. The main manufacture
method used was 3D printing for mounting all the sensors to the supplied robot chassis.
To mount the stepper motor to the robot a mount was designed that could also mount an IR
range sensor on each side. This part went through three major revisions depicted in below in
Figure 26 & Figure 27.Figure 26 Stepper mount Design Revisions
The scanning range finder mount went through four major
revisions and ended up having a shroud to improve beam size of the sensor. The sensor ended
up being horizontal so that it would not overshoot the wall of the arena, although this is not the
ideal orientation it did not appear to affect the performance of the sensor.
The wheel hubs that were provided had excessive play and meant that during robot direction
changes the robot could jerk slightly in the wrong direction leading to errors in the odometry. To
remedy this new hubs were printed that were a press fit on the robot.
Below in Figure 30 is the finalized mountings attached to the robot, only one long range and one
short range IR range finders for the scanning and wall following respectively were required along
with the MPU.
Figure 26 Stepper mount Design Revisions Figure 27 FStepper Mount CAD
Figure 28 Scanning Head Design Iterations
Title 33
Date
Mapping
Overview
The mapping algorithm is controlled by a class object called MapMaker. This object refers to the
global data containers for the current robot position, the current IR readings and the current
yaw heading, and uses this data to construct a map of its surroundings. Since the odometry of
the motor control has been well tuned, we have found that the position data is sufficiently
accurate to give an accurate map of the surroundings.
The map is able to detect differences between walls, empty space and obstacles, and displays
them as an ASCII art drawing in the serial console when the ‘p’ character is entered.
The map is drawn by building an intensity grid of measured points. If a point on the map is
recorded more than once, its intensity is increased, and is therefore considered more reliable,
and more likely to be a true reading. This approach allows us to eliminate false readings due to
noise in sensors, or other sporadic readings.
Most of the mapping testing was done using Visual Studio, since this class only outputs to
console, and the IR and position inputs are simple enough to simulate. When ported to the
Arduino, there were a few small changes required, such as console print commands, and
adjusting of threshold values. For the most part however, the simulation in Visual Studio
provided an accurate prediction of the mapping behavior on the robot.
Algorithm The mapping class is initialized in the setup() function in the main Arduino sketch. This is done
by calling the initialise() function, and providing references to the current position coordinates,
current IR readings and current heading.
Figure 30 Final Robot Hardware Design Figure 29 Final CAD Design
Title 34
Date
While the program is running, the function addLaserReadings() is called at regular intervals.
When this is called, the mapper copies the data from the global arrays so that this data is not
affected. These values are then filtered using getFilteredData(). This function applies a number
of filters to the global data, including applying the origin offset due to the start position of the
robot; coercing data outside of the map’s allowable range due to noise, and proper alignment of
the yaw reading.
After the data has been filtered, the IR distances are added to the current position values to get
an obstacle point in the map coordinate system. The yaw orientation is used to determine
whether IR distances should be added or subtracted from the position coordinates. These
positions are stored in a 2D intensity array, and the value at the corresponding cell is
incremented, up to a value of 254.
The map used the value 255 as a special case to indicate the path that the robot has traversed.
This way the map can show the positions of walls and obstacles, and also show the path that the
robot has traversed around these obstacles.
The map can be printed at any time by pressing the ‘p’ key. When this key is pressed, the
mapper runs the printDisplayMap() function. This function cycles through each map cell, and
compares the intensity to a display threshold (calculated as a function of the highest intensity
recorded). If higher than the threshold, then the map prints a wall character in this cell.
Otherwise the point is not considered reliable enough to be considered a wall. The print
function also checks for the values to indicate the robot’s path, and prints these where
appropriate.
Title 35
Date
Conclusions This project has been very successful and a great learning experience. Unfortunately on the day of assessment there were issues with the stepper motor mounted Long-Range Infrared sensor which caused non-ideal performance in our final demonstration.
However, this problem was fixed after the assessment when the IR sensor was replaced on the afternoon after the demonstration and proof of satisfactory performance was obtained both before and after the demonstration. On the morning of Thursday 4th July (the day before the demonstration) there is a video showing the robot traversing the entire course with only one minor collision. After some minor code fixes, even this issue was fixed. There is then a computer vision map which was obtained on the evening of Friday 5th July (after the demonstration) showing successful navigation of the room with no obstacle collisions. This computer vision map can be seen on, proving successful operation of the SLAM robot.
Things that could have been improved in this project include the verification of correctly functioning hardware. This would have saved us having a poor performance in the demonstration and ensured more reliable operation of the robot. Another aspect that we would do differently next time is to ensure a consistent coding standard between group members. One difficult thing about working in a medium-large group is understanding each other’s code and ensuring outputs and inputs of various modules are matching.
Aspects of this project that did work well include the minimization of sensor usage. Our robot only required 2 IR sensor and an MPU, ensuring cheap production of the robot and less hardware components to rely on. The entire hardware of the robot was also really well contained giving a sleek outward appearance and ensuring protection of important wires and sensors. Another great aspect of the project was the modular style of the code, which allows for easy modification of the code for future robots when requirements have changed. In addition to this it allowed for easy debugging of the code in Microsoft Visual Studio during development. The final part of the project that worked really well was the orthogonality. The robot managed to easily get parallel to a wall and stay orthogonal at all times, ensuring accurate movement and complete coverage of the map.
Overall, this project has gone really well and the final performance of our robot worked out great once hardware issue were solved. There a lot of great potential applications for this robot and it could be easily transferred to other tasks if small parts of it are changed.
Title 36
Date
However, this problem was fixed after the assessment when the IR sensor was replaced on the
afternoon after the demonstration and proof of satisfactory performance was obtained both
before and after the demonstration. On the morning of Thursday 4th July (the day before the
demonstration) there is a video showing the robot traversing the entire course with only one
minor collision. After some minor code fixes, even this issue was fixed. There is then a computer
vision map which was obtained on the evening of Friday 5th July (after the demonstration)
showing successful navigation of the room with no obstacle collisions. This computer vision map
can be seen on, proving successful operation of the SLAM robot.
Figure 31 Final Robot Path When Operational
References [1] http://www.i2cdevlib.com/, Jeff Rowberg, 2010
[2] http://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf,
InvenSense
[3] http://ieeexplore.ieee.org/xpls/icp.jsp?arnumber=1241852, Wei, Khosla & Riviere, 2003
[4] https://www.sparkfun.com/datasheets/Components/GP2Y0A21YK.pdf, Sharp IR Datasheet,
Sharp Electronics
[5] http://www.martyncurrey.com/connecting-an-photo-interrupter-to-an-arduino/, Curry,
2008