One of the core components of NavBot is the Navigator. Its job is to encapsulate the localization information of the robot, i.e. where in the world it is.
For this implementation I am only considering a two dimensional world as the bot will mostly likely only travel around my office floor or some other flat surface. At some later point if we want the bot to travel over general terrain we’d need to factor in elevation too.
But for now the navigator tracks the following data:
- Position (x,y) – The x and y location of the bot from its center, in millimeters, from some origin (0,0)
- Heading (h) – The orientation of the bot in degrees from North about its center.
- Speed (v) – The speed of motion, in millimeters per second, in the current heading from its center.
- Turn Rate – The degrees per second the bot is turning about its center.
Implementation
I’ve implemented the Navigator as it’s own C++ class and tried to make it as modular as possible so it can be deployed in any application requiring localization.
You can view the current implementation here on GitHub in Navigator.h and Navigator.cpp.
For simplicity it uses floating point math. However, as I also want to be able to change over to fixed point math Navigator abstracts out the some basic data types:
//---------------------------------------- // Base Types //---------------------------------------- typedef float nvCoord; // millimeters typedef float nvDegrees; // degrees typedef float nvRadians; // radians typedef nvDegrees nvHeading; // degrees from North typedef float nvRate; // change per second typedef float nvDistance; // millimeters typedef uint32_t nvTime; // time in milliseconds
And as you can see from the comments we are using millimeters, degrees and milliseconds as the units of measurement.
Everything is floating point except time, which is an unsigned 32 bit integer.
Each type has an “nv” prefix to avoid possible name clashes with other libraries or code.
To make the code more readable, and to facilitate possible conversion to fixed point at a later date, Navigator provides some helper macros:
//---------------------------------------- // Helper Macros //---------------------------------------- #define nvMM(D) ((nvDistance)(D)) #define nvMETERS(D) ((nvDistance)((D)*1000)) #define nvMS(MS) ((nvTime)(MS)) #define nvSECONDS(S) ((nvTime)((S)*1000)) #define nvDEGREES(D) ((nvDegrees)(D)) #define nvRADIANSS(R) ((nvRadians)(R)) #define nvNORTH nvDEGREES(0) #define nvNORTHEAST nvDEGREES(45) #define nvEAST nvDEGREES(90) #define nvSOUTHEAST nvDEGREES(135) #define nvSOUTH nvDEGREES(180) #define nvSOUTHWEST nvDEGREES(225) #define nvWEST nvDEGREES(270) #define nvNORTHWEST nvDEGREES(315)
Navigator also uses the following data abstractions:
//----------------------------------------
// nvPosition
//----------------------------------------
struct nvPosition
{
nvCoord x; // mm from origin
nvCoord y; // mm from origin
};
//----------------------------------------
// nvPose
//----------------------------------------
struct nvPose
{
nvPosition position; // mm from (0, 0)
nvHeading heading; // degrees from North
};
Using Navigator
To use Navigator we first need to create an instance of the class:
#include "Navigator.h" Navigator navigator;
Then in setup we need to initialize the instance:
// Navigator defines
#define WHEEL_BASE nvMM(83.5)
#define WHEEL_DIAMETER nvMM(35.4)
#define TICKS_PER_REV 1204
setup()
{
:
:
// set up navigation
navigator.InitEncoder( WHEEL_DIAMETER, WHEEL_BASE, TICKS_PER_REV );
:
:
}
This particular implementation uses dead reckoning via wheel encoders so we pass in the wheel diameter, distance of the wheel base and the number of encoder ticks per revolution.
With this information the Navigator can calculate the number of millimeters travelled per encoder tick by taking the circumference of the wheels and dividing it by the number of encoder ticks.
//----------------------------------------
//
//----------------------------------------
void Navigator::InitEncoder( nvDistance wheel_diameter, nvDistance wheel_base, uint16_t ticks_per_rev )
{
m_dist_per_tick = ticks_per_rev > 0 ? wheel_diameter*M_PI/ticks_per_rev : 0.0f;
m_wheel_base = wheel_base > 1.0f ? wheel_base : 1.0f;
}
We also store the distance of the wheel base as we will need it later for calculating changes in the bot’s heading.
By default the Navigator assumes that the bot is at location (0, 0) and heading (0). If we want to set a different starting location we use these two functions:
navigator.SetStartPosition( nvMM(200), nvMM(300));
navigator.SetStartHeading( nvEAST );
or
nvPose pose;
pose.position.x = nvMM(200);
pose.position.y = nvMM(300);
pose.heading = nvEAST;
navigator.SetStartPose( pose );
To enable the navigator to track the movement of the robot we need first call its Reset() method and then regularly call the UpdateTicks() method.
So first call Reset():
navigator.Reset( millis() );
passing in the current time in milliseconds. This will initialize the navigator and set its location to the starting position and heading.
After Reset() we must regularly call UpdateTicks() thus:
navigator.UpdateTicks( lticks, rticks, millis() );
passing in the number of encoder ticks for the left and right wheels since we last called UpdateTicks() and/or Reset(), and the current time in milliseconds.
Using the tick counts and time the navigator can calculate changes in the bot’s location.
We can see this better if we look at the code for both Rest() and UpdateTicks().
First Reset():
//----------------------------------------
//
//----------------------------------------
void Navigator::Reset( nvTime now )
{
m_last_ticks_time = now;
m_dt = m_lticks = m_rticks = 0.0f;
m_pose = m_init_pose;
m_heading = nvDegToRad(m_pose.heading);
m_speed = nvMM(0.0);
m_turn_rate = nvDEGREES(0.0);
}
It saves the time that it was called and sets the tick counts, m_lticks and m_rticks, and time elapsed, m_dt, to zero. It then resets the current pose to the initial pose, which is either the default pose or the one set using the SetStartPose(), or SetStartPosition() and SetStartHeading() methods.
It also makes a copy of the bots heading, converting it from degrees to radians, as it will use radians later on while doing dead reckoning calculations.
Finally it sets the speed and turn rate to zero.
When UpdateTicks() is called it does the following:
//----------------------------------------
//
//----------------------------------------
bool Navigator::UpdateTicks( int16_t lticks, int16_t rticks, nvTime now )
{
// update delta values
m_dt += nvDeltaTime( m_last_ticks_time, now );
m_lticks += lticks;
m_rticks += rticks;
// remember time for next call
m_last_ticks_time = now;
// see if we have accumulated min time delta
if ( m_dt < m_min_dt )
{
// no, so wait
return false;
}
// use ticks and time delta to update position
nvDistance dr = ((nvDistance)m_rticks)*m_dist_per_tick;
nvDistance dl = ((nvDistance)m_lticks)*m_dist_per_tick;
nvDistance dd = (dr + dl)/2;
// calc and update change in heading
nvRadians dh = (dl - dr)/m_wheel_base;
m_heading = nvClipRadians( m_heading + dh);
// update velocities
m_speed = (dd*1000.0f)/m_dt;
m_turn_rate = (nvRadToDeg(dh)*1000.0f)/m_dt;
// update pose
m_pose.heading = nvRadToDeg(m_heading);
m_pose.position.x += dd*sin(m_heading);
m_pose.position.y += dd*cos(m_heading);
// reset delta values
m_dt = 0;
m_lticks = 0;
m_rticks = 0;
return true;
}
This code is not too complicated. It first adds up the time since it was last called and keeps a running total of left and right ticks.
It then checks to see if a minimum time delta has passed. This time delta defaults to 100 milliseconds but can be changed by calling:
navigator.SetMinInterval( nvMS(50) );
Once the minimum time interval has elapsed it calculates the new location of the bot based on the tick counts and the amount of time elapsed.
Keep in mind that the calculations are an approximation. I am using the equations defined in [6] of A Tutorial and Elementary Trajectory Model for the Differential Steering System of Robot Wheel Actuators by G.W. Lucas. At some later date I plan to implement the more complete equations in section [5].
dr and dl are the distance each wheel has moved. dd is the distance that the bot as a whole has moved about its center point.
dh is the change of heading, in radians, due to the difference in dl and dr.
It adds the change in heading to the internal heading variable, m_heading, and clips the value so it is always in the range 0 to 2π.
It then calculates the rate of change of dd and dh in seconds based on the time delta of the measurement.
Finally from the new heading and the amount of displacement of the robot, i.e. dd, it calculates the changes to x and y, and updates the current pose information with the new values.
At any time we can query this information from the navigator by using various getter methods:
nvPose pose = navigator.Pose();
Serial.print("Heading: ");
Serial.println( pose.heading );
Serial.print("Position: (");
Serial.print( pose.position.x );
Serial.print(", ");
Serial.print( pose.position.y );
Serial.println(")");
Serial.print("Speed: ");
Serial.println( navigator.Speed() );
Serial.print("Turn Rate: ");
Serial.println( navigator.TurnRate() );
Notes about this Implementation
This is an initial implementation of the navigator. You can view all the code on GitHub. However, it would be extremely naive to think that this code is useable in its current form. I know for a fact that there are two major issues that need to be addressed.
The first is that the Zumo bot I am using is track based. It does not have nice thin wheels with small contact areas. The tracks will have some amount of slippage while turning and the navigator will need to compensate for this.
I also know, again from previous experience, that even when each track is turning the same number of ticks, the bot’s path is not straight. I believe this is an alignment issue but still have to explore the problem in more depth.
In any event, the navigator will need to handle any systematic biases inherent in the physical characteristics of the bot. Once the basic Pilot code is functional I can better address those issues.
But for now the navigator seems to work as expected. In my tests I discovered that my wheel diameter value was wrong. I had it at 35.4mm when it should have been 38.9mm.
Here is a short video showing the navigator in action. Keep in mind that the webcam has the video flipped from left to right which makes my -90° turn (to 270°) look like a +90° turn.
I was very surprised by the accuracy of the turn. I thought it would be way off.
Next part is to get the Pilot working. I have a lot of it done but it is much more involved piece of software than the Navigator.
solderspot 