MOBILE ROBOTICS – laboratory exercises
EXERCISE 5: OCCUPANCY GRID MAP BUILDING AND EXTENDED KALMAN FILTER LOCALIZATION
WITH THE LEGO NXT 2.0 MOBILE ROBOT
I. Exercise objective: Implement algorithms for mobile robot odometry, occupancy grid map building and extended Kalman
filter localization and experimentally verify them with the Lego NXT 2.0 in a mobile robot arena.
II. Lego Mindstorms NXT 2.0 robot description:
Fig. 1 depicts an example of an assembled Lego NXT 2.0 mobile robot. It consists of Lego Mindstorms NTX 2.0 elements and
is designed for moving on flat surfaces. The locomotion mechanism of the robot consists of two actuated wheels and a single
passive wheel (castor) which serves for mobile robot stability. Such a configuration is called a differential drive mobile robot
and enables two degrees-of-freedom control which qualifies the robot as nonholonomic. In the exercise it is required to
assemble such a locomotion mechanism, while for environment perception we will use four sonars pointing in different
directions instead of a single sonar mounted on a panning motor.
Sonar
Color sensor
Sonar panning motor
NXT Brick
Actuated motor and left
wheel
Front bumper
Actuated motor and right
wheel
Touch sensor
Fig. 1. An example of a Lego NXT 2.0 mobile robot
Microcontoller
Lego NXT 2.0 robot microcontroller (NXT brick) shown in Fig. 2 consists of two microcontrollers, a 32-bit ARM7
microcontroller with 256 kB of flash memory and 64 kB of working RAM and an 8-bit AVR microcontroller with 4 kB of flash
memory and 512 B of working RAM. The NXT Brick can communicate with the computer via a USB connection which
achieves higher communication rates (up to 12 Mbit/s) and wirelessly via Bluetooth which is somewhat slower (up to 2.1
Mbit/s). It is recommended to use the USB communication in the exercise.
USB port
Output ports (A, B, C)
Screen
Speaker
Control keys
Input ports (1, 2, 3, 4)
Fig. 2. Lego NXT 2.0 microcontroller (NXT brick)
MOBILE ROBOTICS – laboratory exercises
The robot motors and sensors are connected to the NXT Brick via input and output ports. There are three output ports marked
with A, B, and C to which actuators, usually motors, or LED indicators are connected. The input ports are marked with 1, 2, 3
and 4 and are usually used for connecting the sensors. Fig. 3 shows an example of connecting motors and sensors to the NXT
Brick. On the screen, shown in Fig. 4, we can see information like power status, connection status and the currently loaded
program status.
Fig. 3. An example of connecting motors and sensors with the intelligent brick
USB connection status
USB cabel is connected and
communication is running properly
USB cabel connected, but there is
communication error
NXT Brick name
NXT Brick status
Bluetooth connection status
Bluetooth is on but the intelligent
brick is not visible to other devices
When the intelligent brick is on and working
properly, this indicator rotates. If there is an
error the indicator will stop rotating and the
NXT component needs to be reset.
Bluetooth in on and the intelligent
brick is visible to other devices
NXT Brick power status
Bluetooth is on and the intelligent
brick is connected with a device
Fig. 4. Intelligent brick screen
Actuators
The Lego NXT 2.0 robot motor shown in Fig. 5 is a DC motor controlled by the NXT Brick either by position or power. The
reference position is set in degrees and the position control loop is designed with a PID regulator. Reference power is set in
percentages from 0% to 100% of maximum power, while control itself is carried out with pulse-width modulation. Direct
velocity control is not available, since the final motor rotation depends on the load and the motor power supply. However,
despite the lack of direct velocity control, we can use precise motor rotation with the tachometer. Furthermore, we can also use
a pair of motors in “synchronized mode” which gives the effect of two motors being connected with a fixed axle, which is very
useful for straight motion of the mobile robot.
Concerning proprioceptive sensors, at our disposal we have incremental encoders which are built into the motor casing (Fig. 5).
Lego NXT 2.0 motor encoders use a pair of signals shifted in phase so that they can also detect the direction of the motor
rotation. Using the incremental encoders we can read at any time information on the relative motor rotation up to an accuracy of
±1°.
MOBILE ROBOTICS – laboratory exercises
DC motor
Incremental encoder
Hub with an axle hole
for attaching a wheel
Built-in gearing
Fig. 5. Lego NXT 2.0 motor and its schematics
Perception sensors
Concerning perception sensors shown in Fig. 6, we have at our disposal sonars, a touch sensor and a color sensor. In the
laboratory exercise only sonars are required, while the other sensors can be used as per personal choosing. Lego NXT 2.0
sonars have a range of 255 cm with a precision of ±3 cm. The color sensor enables detection of six colors: white, black, red,
yellow, green and blue. Instead of color detection this sensor can also be used to measure room luminance and can also be used
as a light indicator.
Fig. 6. Perception sensors of the Lego NXT 2.0 robot – from left to right: sonar, touch sensor and color sensor
III. Preparing for the exercise:
A) Install the RWTH Mindstorms NXT Toolbox v4.07 by following the instruction from the official website. Do not
forget to download the USB driver, and to transfer the motor control program MotorControl.rxe on the Lego NXT 2.0
Brick. This website can also be helpful for installation.
B) Assemble your Lego NXT 2.0 mobile robot – we recommend a differential drive configuration with a single passive
wheel (castor) for stability. The only condition is that the robot must be equipped with four sonars with differences in
directions of 90° (not necessarily on the same height).
C) Using the prepared graphical user interface shown in Fig. 7 test the USB communication and write the code for mobile
robot motor control.
Communication
with the
NXT Brick
Control algorithm start
and stop
Motion control of
the mobile robot
Fig. 7. Graphical user interface for the mobile robot control
MOBILE ROBOTICS – laboratory exercises
IV. Solving the exercise:
A) By reading data from motor encoders implement an odometry system of the Lego NXT 2.0 mobile robot, i.e. position
estimation by using the differential drive kinematic model. You can measure the necessary parameters like wheel
diameter and axle length after you have constructed your robot. Sample time is T= 0.25 s.
B) Using the odometry system of the mobile robot for localization and the sonar measurements build an occupancy grid
map using the algorithm from the second laboratory exercise. Depict the map graphically. We recommend the
following values for the sonar model: ρv = 180 cm, ϑ3dB = 15° te rmax = 250 cm, while the occupancy grid cell size
can be set to 3x3 cm. It is allowed to experiment with different parameter values.
C) Write the algorithm for local localization using the extended Kalman filter and the algorithm from the third laboratory
exercise. By using the collected data depict graphically on a single figure the change in position of the mobile robot as
calculated by the odometry and as estimated by the extended Kalman filter.
Some useful RWTH TOOLBOX commands for solving the exercise:
NXTMOTOR, SENDTONXT, DIRECTMOTORCOMMAND, READFROMNXT, GETULTRASONIC, USMAKESNAPSHOT