| WFR, a
Dead Reckoning Robot
A practical application to understand
the theory.
By Ibrahim
Kamal
Last update:
15/4/08
 Overview
The aim of the project is simple: Build a
robot that moves in any required direction, while keeping
record of it's actual position and angle with respect to
a reference starting point but without any lines or ground
marks to guide the robot. This data will be then displayed
on an LCD display.
while this robot proved how difficult it is to archive satisfactory
results for robot positioning and mapping systems without
any ground reference except the start point, it helped to
understand many issues related to the process of dead reckoning,
including the factors that reduced precision in position
measurement, and hopefully it will help us to build a more
accurate robot taking in account all the experience achieved
from the construction of this robot. |
|
The
theory of dead reckoning
Dead
reckoning is the process of calculating your position relying
on a previously determined position. To implement such algorithms
on a robot, you have to understand the theory behind it. The mathematical
relations used for dead reckoning differ from a robot to another
due to the steering mechanism differences. For Example, the car-like
steering used in this robot, turns in a curvature of constant
and known radius, while differential drive robots can make very
narrow turns, it can even spin in its position. In this
article, we shall discuss the car-like steering used in this project.
The process of keeping track of the position deals with three
variables:
X and Y cartesian coordinates and the angle (A) of the nose
of the robot (measured from the X axis). The math behind
this process is explained with the simple example path shown
in figure 1A, where the initial conditions at the waypoint
1 are:
(X,Y) = (0,0)
A = 90°
Those three variables are updated upon reaching a waypoint.
Note that the distance 'd' from a waypoint to another in
the example is taken relatively large for
simplification of the mathematical relations, but it is
usually much smaller in order to increase accuracy.
When the robot moves by a distance 'd', it has then reached
the next waypoint (waypoint 2), and the new cartesian position
(X,Y) is updated with respect to previous waypoint's data
(waypoint 1),
|
Fig.
1A:
Example path of the robot |
according to the following relation:
And for a straight line motion, the angle remains constant, so
the angle at waypoint 1, 2 and 3 are equal to 90°
But upon reaching waypoint 3, as you can see in figure 1A, the
robot start to steer right. recalling that the steering radius
of this 'car-like steering' robot is constant, the angle of the
nose of the robot at the new waypoint 4 can be calculated with
respect to the previous waypoint, according to the following relation:
Where d is the distance from a waypoint to another,
and R the steering radius which is different
for each car and is best found by making test runs and measuring
it.
Then the new cartesian coordinates at waypoint 4 can be calculated
by the same rule that was used for waypoint 2:
While the coordinate calculations are somehow logical and can
be easily derived, the angle calculation part may be more complicated
to derive. The figure 1B shows how the angle calculation are derived
As you can see in the figure,  1
is angle of the robot at the first waypoint, 2
is angle at the second waypoint and b
is the difference between the two other angles.
With some simple geometry it can be deduced that b
= a
Finally a
is approximately calculated as if d was
equal to the dotted arc between the two waypoint which is
the real path of the robot. The errors due to this approximation
become extremely small when the distance
between the waypoints 'd' is reduced to a very small value.
Then the angle at the second waypoint is calculated with
respect |
Fig.
1B:
Angle change during steering |
to
the angle at the first waypoint.
Note that the mathematics for right and left steering differ by
a sign as shown:
steering left:
steering right: 
The following table summarizes the rules to calculate
the angle and cartesian coordinates of the robot at a required
position (2) relying on the previous known position (1)
Motion |
Angle of
the robot
Angle is in radian |
Cartesian
coordinates
Angle is in degree
|
Straight
line |
|
|
Steering
left |
|
Steering
Right |
|
Those relations can
be easily implemented as C code and applied to a robot, as you shall
see in the rest of the article.
Mechanics
As you can see in figure 2A, the chassis and all mechanical
parts were originally an RC toy car with 2 motors, one motors
is used for rear traction and the other is used to steer
the front wheels. With very little modifications I had a
chassis in hands, with all the motors and gearboxes mounted
on it, along with the steering mechanism. You may think
this was a good idea, but it wasn't; actually, it was one
of the main factors that caused errors in the position measurement.
The reason for this is the fact that with those cheap toys,
you can't make any modification to the mechanics and you
can't always add feed-back sensors as you wish, specially
in the steering mechanism. Another major inconvenience in
most of those toy cars, is the low quality of the provided
motors and the gear boxes, as they are designed to run at
full power, but are not designed to be speed controlled
to offer more precision. However, it was fairly enough to
start testing various robot navigation codes and algorithms. |
Fig.2A
|
Adding a shaft encoder
One important modification was made to the gearbox, which is to
add some mean of feedback to the microcontroller, to measure
the number of revolutions of the motor, which is an indication
of the distance traveled by the robot. To do this, a home made
shaft encoder was implemented at the first gear of the reducers
gear box, before the speed is reduced. Adding the encoder disk
at this early stage of the gearbox increase the precision of the
system, because each rotation of the wheel will correspond to
dozens of rotations of the encoder disk, or in other words, each
rotation of the encoder disk, will correspond to a very small
displacement of the robot.
The shaft encoder is constructed using two main components:
the encoder disk (Figure 2D compared to the size
of a cell phone), and the photo-couple (figure 2D).
The photo couple simply emits a beam of Infra Red (IR) light
on the encoder disk and detect any reflected IR. The encoder
disk is a piece of thick paper with a black colored portion
that wont reflect Infra-red
light. The disk is glued to the motor's output shaft, so
that it follows freely the motor rotation. the photo couple
is positioned as shown in figure 2E, so that it's
beam is directed on the encoder disk. To understand how
Infra-Red light is emitted and detected, and how to build
such devices, you can read this article
about IR proximity sensors and its applications.
When the motor turns, the encoder disk turns with it and
while the photo couple is directing IR light on the encoder,
it will generate a series of high and low pulses (1 &
0) depending on which part of the disk is facing the photo
couple
Note that this shaft encoder configuration is one among
many others. There is a lot of ways to build and assemble
shaft encoders. While not being the most efficient, This
scheme simply appeared to be the most suitable to this system,
due to the lack of space and the inability to make important
modifications.
|
Fig.2B:
Shaft
encoder assembly
|
Fig.2C:
The encoder disk
|
Fig.2D:
The photo couple
|
Fig.2E:
close up on the Shaft encoder assembly
|
Electronics
and Algorithms.
NOTE:
A full version of the c code is not provided, instead,
small software routines, algorithms., and flow charts
are provided to explain all what you need to understand
how this robot works. |
The electronics on this robot is quite standard. A main board
holds a 89C52 micro-controller that operates the traction and
steering motors via an L298D motor controller IC, while recording
the incoming pulses from the shaft encoder and making all required
dead reckoning calculations. An LCD is connected on a free port
of the micro-controller to display real time information about
the current angle and position of the robot.
The Figure 3A below shows in a very simplified fashion how does
the electronic system works. It is clear that everything is coordinated
by the micro-controller, which gives orders to the motor controller
according to a pre-programmed route of waypoints. The pulses coming
from the shaft encoder are fed back to the micro-controller and
counted into one of the two internal counters of the micro-controller.
Using an internal hardware counter instead of pure software counting
routines dramatically reduces the processing load on the micro-controller
which can sometimes suffer from the floating point calculations
required for dead reckoning, even if it is running at 24Mhz.
Fig.3A:
Simplified functional diagram of the main board of the robot.
|
Tasks handled by the micro-controller
The microcontroller execute a number of pre-programmed
moves (like going forward or going backward while steering right,
etc.) while keeping track of its position and controlling the
speed of the traction motor trying to keep it as stable and and
constant as possible. In order to implement a precise speed control
on a motor, as certain mean of feedback is required. The only
available source of feedback is the shaft encoder, which will
provide very precious data, not only for speed control, but also
for the positioning system. Figure 3B shows how the shaft
encoder
will be used for the positioning system as well as for the
speed control. The shaft encoder will provide the micro-controller's
internal counter with a sequence of pulses that correspond
to the rotation of the traction motor, hence it corresponds
to the displacement of the robot. A timer is set to execute
two software routine every 1/10 th of a second (which is
just an arbitrary value). One of those software routines
is to recalculate the actual position, taking the number
of counted pulses as "d" (the distance between
between this waypoint and the previous).
Then, another software routine is executed to control the
speed of the robot by comparing the number of counted pulses
with a fixed number which is referred to as the "required
pulses". The "required pulses" corresponds
to the desired speed, and the "counted pulses"
corresponds to the actual speed of the robot. |
Fig.3B:
Shaft encoder software implementation
|
Finally, as you can notice in
the schematic, it's all a matter of comparing those two values
and constantly adjusting the power delivered to the motor. Note
that choosing the right timing between the execution of this routine
can dramatically improve overall system stability and performance,
especially on low quality motors.
 Controlling
the power delivered to the motor to control the speed.
PWM (pulse width modulation) is the most common way to control
the power delivered to the motor, thus the speed of the motor.
It's also the most efficient method. This article does not intent
to explain PWM, but here is how in works, in very simplified way:
The microcontroller generate pulses of electricity for the motor
(instead of constant DC voltage). The frequency of
those
pulses remain constant, while the ON period of the pulse
is varied to increase or decrease the speed of the motor.
Figure 3C shows how this process work, with a series of
pulses, each pulse having an increasing "ON" period,
thus the Average voltage of the wave of pulses increases
gradually as the duty cycle increases (duty cycle is
the ratio of the 'ON' period over the total period of the
pulse). As the average voltage increases, the speed
of the motor will increase gradually.
Making a speed control program using the PWM theory |
Fig.3C
PWM motor control
|
is very simple, as shown in the example below, which
is a c code targeted for any 8051 microcontroller to run the motor
at half speed (50/100) and where the Enable pin of the motor controller
is connected to pin P1.0 of the microcontroller:
#include
<REGX52.h>
unsigned char max_pwm, pwm_counter, required_pwm;
void main(){
max_pwm = 100;
required_pwm = 50;
while(1){
pwm_counter++;
if (pwm_counter <= required_pwm){
P1_0 = 1;
//Turn
on the motor
}else{
P1_0 = 0; //Turn
off the motor
}
if (pwm_counter > max_pwm){
pwm_counter
= 0;
}
}
}
|
This piece of code is highly configurable, you can change the pulses
frequency by changing the max_pwm value and you
can change the speed from 0% to 100% by changing the value of required_pwm.
This codes does not cause any noticeable delays in the rest of the
program execution.
Software routine for calculating the position and the angle of the
robot
This function will update the position
and angle of the robot according to two variables:
1- Distance, which is the number of counted pulses
2- Direction, which indicates whether the robot is steering or not,
and in which direction the traction motor is running. The following
table shown the different moves executed by the robot, corresponding
to the value of the variable "direction"
Executed
move by the robot |
Direction |
Forward, steering to
the left |
0 |
Forward in a straight
line |
1 |
Forward, steering to
the right |
2 |
Backward, steering to
the left |
3 |
Backward
in a straight line |
4 |
Backward, steering to
the right |
5 |
One important note to be able to understand
the following code is that angle is always multiplied by 10. The
rest of the code, is just an implementation of the dead reckoning
mathematical relations explained in the beginning of this article.
update_posision(direction,distance){
if ((direction
== 0) | (direction == 5)){
angle
= angle + ((distance / steering_radius)*572.958);
//572.958 is to convert from rad
to degree,
}else if
((direction == 2) | (direction == 3)){
angle
= angle - ((distance / steering_radius)*572.958);
}
if (angle < 0){
angle
= angle + 3600;
}
if (angle > 3600){
angle
= angle - 3600;
}
if ((direction == 0) | (direction == 1)| (direction ==
2)){
x = x
+ (distance*cos(angle*0.001745));
//0.01745 is to convert from degree
to rad
y = y + (distance*sin(angle*0.001745));
//then, angle/10 = 0.001745
}else{
x = x
- (distance*cos(angle*0.001745));
y = y - (distance*sin(angle*0.001745));
}
}
|
Note: the included headers, and variable definitions
are not included in this routine
Operating the L293D motor driver
Using
the L293D motor driver, makes controlling a motor as simple
as operating a nand gate IC. It totally isolates the TTL
logic inputs from the high current outputs.
Putting a logic 1 on the pin In1 will make
Out1 pin go to Vpower (36 Volts MAX.),
while a logic 0 will make it go to 0V
Each couple of channels can be enabled and disabled using
E1 and E2 pins. When disabled a channel provide a very high
impedance (resistance) to the motor, exactly as if the motor
wasn't connected to the driver IC at all, which makes this
feature very useful for PWM control.
Figure 3D shows different ways to connect a motor
to the IC.
One way is to use 2 channels to build |
Fig.3D
Using the L293D motor driver
|
a bi-directional motor driver, another way is to
use 1 channel per motor, building a unidirectional driver. In this
project, we will be using the 4 channels to drive the 2 motors in
both directions. To get more specific information on this very useful
IC, you can always download and inspect the datasheet
Schematic of the shaft encoder
as
I said before, this article
about IR proximity sensors explains in detail how
such a device works, how it is built, and how to apply it
to a system.
This shaft encoder is connected to the main board with 4
lines. Two of them are to deliver 5V and Gnd, one is to
transmit the output of the sensor to the microcontroller,
and the last one, labeled "Thre" is the sensor's
threshold, used to adjust the sensitivity of the of sensor,
according to the ambient light conditions. That's why you
will find a potentiometer on the main board, it's to adjust
the sensitivity of the shaft encoder. |
Fig.3E
Using the L293D motor driver
|
Schematic of the main board
Here is the schematic of the main boards, which handles all the
robot. As you can see, the LCD is connected to port 0 of the 8951
microcontroller, thus requiring a pull-up resistor network.
You can notice Some leds on some of the free pins of the microcontroller,
those are used as test signals to held debug eventual errors.
The L293D motor driver makes the motor control very simple, as control
pins are directly connected from the microcontroller to the driver.
Wire connections are added for a fan to cool the L293D driver. The
power supply part is quite generic, with its filtering and decoupling
capacitors.
M2 is a connection of the shaft encoder module, while M3 is a connection
for an eventual line sensor. (As a matter of fact I didn't expect
the results of dead reckoning on such a robot to be very spectacular,
so i added this connection as a backdoor, for future use)
Fig.3F
Schematic of the Main Board |
PWM generator Module M1 (Reserved for future use):
Note that M1 is a connection to an external PWM generator module,
that can be controller to output 2 different PWM signals from a
4 bit digital input (P3.0 to P3.3). The reason for putting this
module is to allow future expansion of the robot. Actually on the
PCB, P3.0 and P3.1 are short circuited to the 2 PWM output lines,
directly controlling the 2 motors.
Conclusion
and Future development
Preview of the last 15
messages discussing this page. Messages are sorted from the newest to
the oldest. |
Posted
by:
mbosko
on:
29 May 2008 |
|
|
Posted
by:
ikalogic
on:
28 Apr 2008 |
Re: WFR, a Dead Reckoning Robot |
|
 |
Quoting ravishankar747: Sir,
Have you any circuit related to stepper motor driver using L297 and L298.
Ravi |
No, not yet. but the net is full of that, or look into the datasheet of the L297!
|
|
|
|
Posted
by:
ikalogic
on:
09 Feb 2008 |
|
Re: WFR, a Dead Reckoning Robot |
|
|
Quoting FAQ: sir,
I read the article about the robot,that was really so nice.
i have decided to do a similar project in my college, can u please guide me for that.
I would like to know what was the cost for the robot, where all the materials available in chennai or within india.
please kindly help me |
The most expensive part, is the chassis of the robot, that i bought from RadioShake for less than $50.. then the next three expensive pieces are the L298D motor driver, the LCD and the 89S52 Micro controller.. I guess those ICs are easily available in your country!
good luck, and keep us posted with your work.
|
|
|
|
|
Posted
by:
faq
on:
09 Feb 2008 |
|
Re: WFR, a Dead Reckoning Robot |
|
|
sir,
I read the article about the robot,that was really so nice.
i have decided to do a similar project in my college, can u please guide me for that.
I would like to know what was the cost for the robot, where all the materials available in chennai or within india.
please kindly help me
|
|
|
|
You have
to be a member to post replies. |
|
|
|
