Reliable Synchronization-based Navigation of Lego Minstorms NXT Robots


In competitive Lego robotics, it is often common for participants to build and program robots to solve certain tasks.  For example, participants may be required to collect and to deposit balls from one point to another, to push buttons located around the challenge playfield, or to traverse certain routes.

While the tasks possible are aplenty, it is certain that some form of robot navigation will allow most of these tasks to be completed efficiently. In order for a robot to move from one point to another, one can turn on the motors of the robot for an estimated period of time – but the traversed distance may not always be constant, as the actual speed of the non-regulated motors vary with the battery level. One can also choose to trace certain lines placed on the playfield – but such lines may not always be present and if they are, will require extra programming effort.

In this project, we develop a reliable and convenient form of navigation based on synchronizing the motors of a typical two-wheeled Lego Mindstorms NXT Robot. It allows an arbitrary robot to accurately:

  • Traverse a straight line for a specified distance
  • Traverse an arc of a specified radius and angle
  • Shift its position by a horizontal and vertical offset, while maintaining its orientation.

Motor Synchronization

The motors are synchronized using a simple P (Proportional) controller. We use an update routine to adjust the output power of each motor to maintain the speed ratio specified by TargetSpeed_\text{left} and TargetSpeed_\text{right}, the target speeds of each motor. TargetSpeed can range from -100 to 100, where the robot moves in reverse at full power and forward at full power respectively.

We now describe and construct the aforementioned update routine. Firstly, we calculate the error value based on the difference between the MeasuredSpeed \left(= \frac{RotationCount}{ElapsedTime}\right) to TargetSpeed ratio of each motor. The formula for the error value e is given by

e = \frac{MeasuredSpeed_{\text{left}}}{TargetSpeed_{\text{left}}} - \frac{MeasuredSpeed_{\text{right}}}{TargetSpeed_{\text{right}}}

=\frac{1}{ElapsedTime}\left(\frac{RotationCount_{\text{left}}}{TargetSpeed_{\text{left}}} - \frac{RotationCount_{\text{right}}}{TargetSpeed_{\text{right}}}\right)

If the above error is positive, then the left motor is running too “fast” as compared to the right motor as a proportion of their respective target speeds; if the above ratio is negative, then the right motor is running too “fast” as compared to the left motor.

Secondly, the computed error value is multiplied by a user-specified proportionality constant K_p and combined with the target speeds of each motor to give the output power to be set for each motor. This adjusted output power allows the motor to correct for the error described above. The output power for the left and right motors are respectively given by

Output_{\text{left}} = TargetSpeed_{\text{left}} - K_p \cdot e\cdot TargetSpeed_{\text{left}}

Output_{\text{right}} = TargetSpeed_{\text{right}} + K_p \cdot e\cdot TargetSpeed_{\text{right}}

Finally, the computed output powers are applied to the respective motors. One then repeats this entire process until a desired point of termination, for example, until the robot has traveled for some specified distance.

Traversing a straight line

We now look at some useful ways to navigate robots with the aid of the synchronization described above, starting with moving in a straight line for a distance of x. This problem is fairly simple. We simply set

TargetSpeed_{\text{left}} = TargetSpeed_{\text{right}}

to be the same for both motors, and then wait for the rotation count on either motor to go beyond a target value proportional to x. In fact, if we know the WheelDiameter of the wheels used by the robot, we can calculate this value exactly as

TargetRotations = \frac{360}{\pi\cdot WheelDiameter} \cdot x

The 360 appears because each complete round of the NXT motor covers 360 rotation counts. Note that the above formula assume x and WheelDiameter have the same units.

Traversing an arc

It is possible to compute TargetSpeed for each motor such that the path of the robot follows exactly the circumference of a circle with radius r. To understand how, we consider the arc maneuver demonstrated in the video above, and the paths traversed by each of the wheels relative to the robot’s path.

Let \Delta t be some duration of time for which the robot moves along the circumference of a circle of radius r. In this duration of time, the robot would have travelled along the circumference for an angle of \Delta\theta. The left and right motor would have rotated respectively for distances of

Distance_{\text{left}} = \left(r - \frac{TrackWidth}{2}\right)\Delta\theta

Distance_{\text{right}} = \left(r + \frac{TrackWidth}{2}\right)\Delta\theta

where TrackWidth refers to the distance between tyre center to tyre center.

Since by definition Speed = \frac{Distance}{Time},  it follows that

TargetSpeed_{\text{left}} = \frac{\left(r - \frac{TrackWidth}{2}\right)\Delta\theta}{\Delta t}

TargetSpeed_{\text{right}} = \frac{\left(r + \frac{TrackWidth}{2}\right)\Delta\theta}{\Delta t}

Dividing both, we see that some cancellation occurs to give

\frac{TargetSpeed_{\text{left}}}{TargetSpeed_{\text{right}}} = \frac{r - \frac{TrackWidth}{2}}{r + \frac{TrackWidth}{2}}

With knowledge of the radius of the arc and the track width of the robot, we can set an arbitrary target speed for the left motor and work out the corresponding target speed for the right motor using the above ratio. The target speeds are then applied to both motors respectively to allow the robot to move in an arc.

It is then trivial, using some basic geometry, to work out the rotation counts required of each motor for the traversed arc to subtend some specified angle \theta (in radians):

TargetRotations_{\text{left}} = \frac{360}{\pi\cdot WheelDiameter}\cdot \left(r + \frac{TrackWidth}{2}\right)\theta

TargetRotations_{\text{right}} = \frac{360}{\pi\cdot WheelDiameter}\cdot \left(r - \frac{TrackWidth}{2}\right)\theta

Position shift

Suppose we want a robot to complete an arc maneuver such that after the maneuver, it is a distance of x away horizontally and y vertically.

With some coordinate geometry, it is fairly trivial to deduce that the the arguments for the corresponding arc maneuver can be written as

r = \frac{x^2 + y^2}{2x}

\theta = 2 \arcsin\frac{\sqrt{x^2 + y^2}}{2r}

In essence, a position shift maneuver is really just an arc maneuver parameterized differently to allow one to conveniently shift the position of the robot by an arbitrary horizontal and vertical offset.

Parallel Position shift

In spite of the convenience of the position shift maneuver, there may be some complications with the final orientation of the robot. Depending on the offsets in each directions, the robot may end up facing north, north-east, north-north-east, west, et cetera. In order to resolve this inconsistency in the final orientation of the robot, we can ensure that the robot retains its initial orientation by splitting a position shift maneuver into two sub-maneuvers.

Both maneuvers will have vertical offset of \frac{y}{2} in the same direction (for a total of \frac{y}{2} + \frac{y}{2} = y). The horizontal offset of both maneuvers will similarly be \frac{x}{2}, but the direction of the horizontal offset of each maneuver is opposite of the other. If the horizontal offset of the first maneuver is towards the left, then the horizontal offset of the second maneuver will be towards the right.

NXC Implementation and downloads

An NXC implementation of the maneuvers can be found at GitHub over here. Please drop me an email for any errors and suggestions to improve the implementation.

Results and conclusion

The accuracy of the maneuvers depends largely on the robot itself. For example, the motors used in the demonstrations have a considerable amount of backlash (\pm 4^{\circ}) and there is also considerable friction between the wheelies at the front of the robot and the playfield (which was a rather rough sheet of paper). These sources of inaccuracy together with several plausible others compound after a long sequence of maneuvers, eventually causing the final position of the robot to deviate from the desired destination. Nonetheless, the above navigation maneuvers serve as rather convenient and hassle-free (aside from its implementation) means to navigate a typical two-wheeled NXT robot from point to point.


Leave a Reply

Fill in your details below or click an icon to log in: Logo

You are commenting using your account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s