Position Estimation

Position estimation allows determining the location of the robot at any given time using sensors. The Lego NXT motor contains optical encoders that can be used to implement basic odometry. By combining this information with accurate gyroscope readings, a robot can achieve great position accuracy. In this section we also explain how to make the Robot Navigation software select the different position estimation options.
Odometry
This is the most used and least expensive position estimation technique. In most cases, optical encoders are used to measure wheel rotation. In a skid/steering robot platform the encoder readings can determine the wheel (robot) displacement as well as relative rotation of the robot. In a robot with known wheel separation (or track), the instantaneous robot displacement and rotation can be approximated using:

DISPLACEMENT = (LEFT_ENCODER_COUNT + RIGHT_ENCODER_COUNT) * ENCODER_SCALE_FACTOR / 2
ROTATION = (LEFT_ENCODER_COUNT – RIGHT_ENCODER_COUNT) * ENCODER_SCALE_FACTOR / TRACK

The procedure to estimate the encoder scale factor constant is discussed below. The position and direction (heading) of a robot can be now approximated by adding up (or integrating) these values:
POS_X = POS_X + DISPLACEMENT * cos(HEADING + ROTATION / 2)
POS_Y = POS_Y + DISPLACEMENT * sin(HEADING + ROTATION / 2)
HEADING = HEADING + ROTATION

It is common to initialize the robot’s position and heading to zero. The Robot Navigation software uses this approach when the Robot class is defined as type EV3:
Ev3 robot(PERIOD, TRACK, ENCODER_SCALE_FACTOR, motor_aux_info);

Gyroscope Enhanced Odometry
The Odometry position solution deteriorates quickly as the robot moves. In particular, when the robot turns, odometry can accumulate significant heading angle errors, which lead to large positioning errors. Therefore, applications requiring accurate positioning demand other sensors to measure orientation. Gyroscopes offer an elegant solution for this problem, as they can measure the robot rates of rotation independent of the terrain conditions or external conditions. The gyroscope can determine the instantaneous rotation for a given period of time (or sampling period), as follows:
ROTATION = RATE * PERIOD

The resulting computed rotation can be plugged directly in the odometry equations presented above. Some gyroscopes such as the Lego-EV3 and Microinfinity XG13000L perform the heading integration internally. In that case the direct output from the gyroscope (heading) can be used to compute position:
POS_X = POS_X * cos(HEADING)
POS_Y = POS_Y * sin(HEADING)

This method is widely used in all the applications presented in this site. The Robot Navigation software will use this approach when the Robot class uses a gyroscope. For long-term accurate localization, users will need an accurate orientation reference, such as the XG1300L gyroscope. In this case, the program will need to be instructed to create a Xg1300lGyro class that is capable of handling this extra sensor:
Xg1300lGyro robot(PERIOD, TRACK, ENCODER_SCALE_FACTOR, motor_aux_info, (char *)&GYRO_PORT);


Important:
The XG1300L gyroscope must be turned on making sure that the device is completely stationary. This is quite challenging in the case of the LEGO robot, since pressing the button to turn the robot on can easily move it. According to the gyro data sheet, it can be reset in software, but I have not found a good way of sending the appropriate I2C commands from a C/C++ program. As an alternative, I found that the program that reads the sensor data created to run using the LEGO EV3 software handles very well the device and resets it properly every time the program is started. Users only need to run this program once before running the C/C++ programs.

Notice that the class Xg1300lGyro uses an extra parameter (GYRO_PORT), which defines the port location of the gyroscope. Users that have access to the LEGO gyroscope may also use it as the orientation reference. However, the results may not be as good (see the LEGO vs. Microinfinity gyro comparison and the Tic-Tac-Toe demo).

LegoGyro robot(PERIOD, TRACK, ENCODER_SCALE_FACTOR, motor_aux_info, (char *)&GYRO_PORT);


Encoder Scale Factor
In order to use Odometry, we need to determine the relationship between the number of encoder counts and the linear displacement of the wheels. For a motor with a known number of counts per wheel revolution (360 for the Lego-NXT motors) and a known wheel diameter, the encoder scale factor can be determined using:
ENCODER_SCALE_FACTOR = PI * WHEEL_DIAMETER / COUNTS_PER_REVOLUTION

Alternatively, if the wheel diameter is unknown or difficult to measure (i.e. tracked vehicles), it is still possible to determine the encoder scale factor by making the robot move a fixed distance along a straight line. The encoder scale factor is related to the final encoder count at the end of the motion by:
ENCODER_SCALE_FACTOR = DISTANCE / FINAL_ENCODER_COUNT

Prev << Robot Platform
Next >> Control