NXT Programming

Lesson 10, Localization Part 1

In this lab session we investigate how the method of odometry, [6, Chapter 1, page 19], and the tacho counter of the NXT motor can be used to keep track of the position of a LEGO car with so called differential drive where two motors are used independently to move and steer a car as shown in Figure 1. An example of such a car is the base vehicle of Lesson 6. We will investigate how the leJOS classes DifferentialPilot and OdometryPoseProvider uses odometry to keep track of the position of the base vehicle. Furthermore, we will determind the accuracy of the odometry estimates of the position provided by DifferentialPilot and OdometryPoseProvider. The different interfaces and classes available in leJOS for localization and navigation e.g. DifferentialPilot are described in the turorial : Controlling Wheeled Vehicles, [4].

Figure 1 A car with differential drive that can turn in place, [2].

Odometry

In [6, Chapter 1, page 19] it is described how to perform dead reckoning, [3], for a differential driven car by using simple geometric equations to compute the momentary position of the car relative to a known starting position, Figure 2. These equations have been used in the leJOS classes DifferentialPilot and OdometryPoseProvider. These classes implement methods to control car movements and methods to keep track of the Pose of the car, i.e. its location and heading.

Figure 2 The odometry equations for a car with differential drive, [6, Chapter 1, page 20].

The methods travel and rotate from DifferentialPilot can be used to make e.g. the base vehicle drive a simple route like a square. This has been done in the program PilotSquare.java, [1].

Figure 3 The base vehicle with a pointer tool
(the orange brick) that can be used to measure how
close to its starting position the vehicle stops
after having driven in a square.

Make the base vehicle drive a few times in a square and figure out how accurate the vehicle drives by observing how close to its starting position the vehicle stops after its tour. Does the vehicle stop at the same position each time?

Calibrate the wheel diameter and the track width

In [7] causes for different odometry errors are described, both systematic errors e.g. caused by unequal wheel diameters and non-systematic errors e.g. caused by wheel-slippage due to irregularities of the surface or over-acceleration. According to [7] the non-systematic errors can be reduced significantly by adjusting the wheel diameters and track width used in the software. In [7] a method is given that reduces the systematic odometry errors. We will, however, in the following use a more ad hoc method.

In [4] it is described how the different constructors for DifferentialPilot all require two parameters called wheelDiameter and trackWidth. These are the parameters we need to adjust to reduce the systematic odometry errors:

  • Start with wheelDiameter. Make the vehicle travel e.g. 50 cm and adjust the wheel diameter until the vehicle travels as close to 50 cm as possible. If the vehicle will not run in a straight line on a smooth surface, there may be a small difference in the diameters of the two wheels. Then the constructor with different diameters for the left and right wheels should be used.

  • After having adjusted the wheel diameters then make the vehicle rotate a given angle e.g. 180 degrees and adjust the trackWidth until the vehicle rotates as close to the given angle as possible.

In [4] it is described that "With proper adjustment of these parameters, errors in distance traveled and angle of rotation can be held to 2% or perhaps less". See if this can be achieved.

After the calibration try with the square again maybe this time with a longer distance. Try a few times and use this to determind if the only odometry errors left is the non-systematic errors, i.e. errors in the stopping position that are randomly distributed around the starting position.

Position tracking by means of particle filters

To estimate the influence of non-systematic errors on the position of the vehicle obtained by odometry we will use the method of Monte Carlo localization or particle filter localization, [9]: "The algorithm uses a particle filter to represent the distribution of likely states, with each particle representing a possible state, i.e. a hypothesis of where the robot is". We will use use the algorithm in the special case where only the movements of the vehicle will be modelled in the motion update step of the algorithm, [9] and all particles are initially set to the known starting position. An example of the resulting particle set after each move of a non-sensing vehicle can be seen in Figure 4. The details of the stochastic motion model used in Figure 4 can be found in [9].

Figure 4 The distribution of likely positions for a non-sensing vehicle after moving for several steps, [9].

Inspired by the leJOS MCLParticleSet a particle filter position tracking has been implemented in the program PilotMonitor,[1], running on a PC while monitoring the movements of a base vehicle running the program PilotRoute, [1], that make the base vehicle drive a route of travel or rotate steps, Figure 5.

Figure 5 The distribution of likely positions for a non-sensing vehicle after the route
travel(100), rotate(90), travel(100), rotate(-90) and travel(100).

In the PilotMonitor program the motion model in the class Particle is the same as used in the leJOS class MCLParticle with distanceNoiseFactor = 0.05, angleNoiseFactor = 2: 



 /**
   * Apply the robot's move to the particle with a bit of random noise.
   * Only works for rotate or travel movements.
   * 
   * @param move the robot's move
   */
  public void applyMove(Move move, float distanceNoiseFactor, float angleNoiseFactor) 
  {
    
    float ym = (move.getDistanceTraveled() * ((float) Math.sin(Math.toRadians(pose.getHeading()))));
    float xm = (move.getDistanceTraveled() * ((float) Math.cos(Math.toRadians(pose.getHeading()))));


    pose.setLocation(new Point(
                     (float) (pose.getX() + xm + (distanceNoiseFactor * xm * rand.nextGaussian())),
                     (float) (pose.getY() + ym + (distanceNoiseFactor * ym * rand.nextGaussian()))));
    pose.setHeading(
       (float) (pose.getHeading() + move.getAngleTurned() + (angleNoiseFactor  * rand.nextGaussian())));
    pose.setHeading((float) ((int) (pose.getHeading() + 0.5f) % 360));
    
  }
Now we are going to investigate if this model of the non-systematic random odometry errors also model the errors of the base vehicle driven in a route of travel and rotate steps. This can be done in two steps:
  • Estimate the two noise factors distanceNoiseFactor and angleNoiseFactor for the base vehicle by repeating a single travel step and a single rotate step several times to measure the extent of the randomly distributed errors around the intended stopping position.

  • Look into the motion model used in [9] to get inspiration for experimenting with the model used in applyMove.

Position tracking while avoiding objects

How can you track the position of the vehicle moving in a fixed route by means of travel and rotate steps while the vehicle avoids objects in front of it ? Mount an ultrasonic sensor in front of the vehicle to detect objects.

References

Last update: 16-5-15