Driving a Robot Fastest Path from A to B

Frank Naccarato, Ph.D., P.Eng.

Mentor, FRC Team 907: East York Cybernetics

12 September 2013

Two driving strategies for a typical FRC tank robot are compared: (1) a turn-straight-
turn path in which the robot moves in a straight line with discrete heading adjustments
at the start and end points, and (2) a generalized curvilinear path. The analysis shows
that the turn-straight-turn method minimizes drive time in all cases.

Introduction

A question that might be of some interest to FRC teams is

Whats the fastest way of getting a robot from Point A to Point B?

It is tempting to skip the math and jump right to an intuitive answer: Everyone knows that the shortest
path between two points is a straight line.

True, but is this the answer to our question?

(Aside: Archimedes made this statement to define a straight line, so in his view it is true by definition.
Interested readers might want to look into calculus of variations for an in-depth proof.)

We need to recognize that shortest path is not always the same thing as fastest path
[Brachistochrone Problem]. To find the actual answer, we have to frame our question mathematically.

A similar question was asked by L. E. Dubins for a vehicle whose rate of turn was limited (e.g., an
automobile). His analysis gave us the Dubins Path a curve consisting of circular arc segments and
straight lines [Dubins 1957].

Kinematic Representation of the Robot

We first need to describe a tank-drive robot in kinematic terms. Referring to Figure 1, we will make the
following qualifying assumptions about the robot:

1. During pure rotations, the robot turns about its geometric centre, C, half-way between the
middle wheels. The point C has coordinates (x,y).
2. The wheel base is w (i.e., centre-distance from left bank of wheels to right bank).
3. The right bank of wheels runs at linear speed uR while the left bank runs at uL.
4. The maximum linear speed either bank of wheels can reach is umax.
5. The robots heading is defined as the unit vector h parallel to the wheels direction of motion,
with its origin at (x,y). The heading (or azimuth) angle is formed between the heading vector
and the x-axis of the reference frame.
6. The instantaneous centre of rotation is point R, which is a distance r from C, such that

1
 Figure 1: Kinematic representation of robot.

Based on these assumptions, it is possible to relate the parameters in the following ways:

Unit heading vector, h: [ ] | | [1]

Speed, u, of the robot (measured at C): ( ) [2]

Velocity, v, of the robot: [3]

Angular speed, , of the robot: [4]

( )
Expressing uR in terms of uL: [5]
( )

Given the nature of tank drives, during a turn either the left or right bank of wheels can move at umax but
not both. In Figure 1, the robot is turning right (i.e., the instantaneous centre of rotation is on the right
hand side). Therefore, the maximum linear speed when turning is achieved when uL = umax.

2
From [4]: ( )

( )
From [5]: ( )
( )

( )
Re-arranging: ( ) ( ) [6]
( )

From [6], we see that if r = 0 then u = 0. This is what we expect because r = 0 is true when the robot is
executing a pure rotation with no translation.

Also, if r then u umax. Again, this agrees with expectations because this corresponds to translation
in a perfectly straight line with no rotation.

Robot Pose

The position of the robot is described by the displacement vector d:

[ ]

If we didnt care about which way the robot was pointing, we could stop here. However, for path
planning, we are definitely interested in the robots heading, which is described by the angle .
Therefore, it takes three coordinates (x,y,) to completely define the robots position and orientation in
the plane.

To save us having to write position and orientation all the time, lets define this set of numbers more
compactly as the robots pose, q:

[ ]

(Aside: If it takes three coordinates to fully describe a two-dimensional rigid body, how many
coordinates are needed to define a three-dimensional rigid body?)

Time Minimization Problem

Given the concept of robot pose, we need to modify our original question slightly:

Whats the fastest way of getting a robot from Point Pose A to Point Pose B?

Referring to Figure 2, we can restate the question as a path-planning problem:

Find the path that minimizes the time to go from qA to qB.

3
 Figure 2: Path planning goal get from Pose A to Pose B in minimum time.

Let P represent an arbitrary path that takes the robot from A to B. Consider an infinitesimal fragment of
the path that has an arc length represented by ds. If the robot is traveling at speed u across that
fragment, then the time dt to traverse that fragment is

Therefore, the total time to traverse the entire path P is

Expressed mathematically, we are looking for a path P that minimizes T.

Scenario 1: Turn-Straight-Turn

Figure 3 illustrates the first driving strategy we wish to consider, the so-called Turn-Straight-Turn path.
This path can be broken down into three discrete steps:

1. A pure rotation through A from initial heading vector a to intermediate heading vector hAB
2. A pure translation (i.e., straight line motion) through a distance L
3. A pure rotation through B from intermediate heading vector hAB to final heading vector b

4
 Figure 3: Scenario 1: Turn-Straight-Turn.

Therefore, the total time consists for three components

| | | |

Assuming the robot reaches top speed very quickly (i.e., very steep acceleration, which is generally true)
we can use [4] to replace the angular velocities (remembering that r = 0 for pure rotations):

(| | | |)

The first term represents the time required to drive in a straight line while the second represents the
time needed to execute the turns at both ends.

Expression [8] represents the general case, where the robots heading can turn in either direction. There
is an important subset of cases were the robot can get from A to B by always turning in one direction. In
such a case, the terms have the same sign, which means:

| | | | | |

From Figure 3 we know that and , which implies that

| | | |

So, in the special case where the robot only turns in one direction, expression [8] becomes:

| |

5
 Figure 4: Scenario 2: Curvilinear Path.

Scenario 2: Curvilinear Path

Figure 4 illustrates how the trip from Pose A and Pose B may be achieved using a curvilinear path. In this
case, path P is an arbitrary two-dimensional parametric curve that satisfies the following boundary
conditions:

1. The curve starts at Point A

2. At Point A, the curve is tangent to heading vector a
3. The curve ends at Point B
4. At Point B, the curve is tangent to heading vector b

For a curvilinear path, we calculate the time using [7]:

We use [6] to substitute for the speed u:

( )

Factoring r out of the integrand:

( )

We can now break the integral into the sum of two parts:

6
If LP is the total arc length of path P, the first part may be replaced as follows:

Remembering that r is the instantaneous radius of curvature, it is possible to express the infinitesimal
arc length ds as:

| |

The absolute value operator is necessary because ds and r are positive quantities, but d can change
signs depending on whether the incremental change in heading is to the left or the right. Substituting for
ds in the integral:

| |

Note that expression [10] is identical in form to expression [8], which allows a term-by-term comparison.

To evaluate the integral in the second term, there are two cases that span all possible situations:

1. The curve P has no inflection points, meaning that the robot always changes heading in one
direction only (i.e., either in the + or - sense, but not both).

2. The curve has one or more inflection points, so that the robot changes heading in both the +
and - sense over different intervals.

Case 1

In this special case, the robot always changes heading in one direction only. This implies that the
integrand never changes sign, making it possible to move the absolute value operator outside the
integral, which can then be evaluated in a straightforward way:

| |

Case 2

There will be sets of boundary conditions that can be satisfied by curves with one or more inflection
points. However, paths with an increasing number of inflection points are, by definition, more curvy
and therefore less time-efficient. Therefore, we only have to concern ourselves with curves with one
infection point (since all others will take a longer time to traverse).

In fact, the lowest order of parametric curve that can satisfy our four boundary conditions is a cubic. Any
interval of a cubic can have either have no inflection points or exactly one inflection point. So, a cubic is
the simplest type of parametric curve that describes the robot path.

7
 Figure 5: Curve with an inflection point.

Figure 5 shows an arbitrary curve with an inflection point at I. This effectively divides the curve into two
intervals, AI and IB, where has opposite signs. We can therefore evaluate the integral in [10] over
these two intervals:

[ | | | |]

The heading vector at the inflection point, hI, corresponds to the unit tangent vector at that point. Over
the interval AI, the robot executes a cumulative angle change of while over interval IB the angle
change is . The two integrals can be evaluated to give:

(| | | |)

Comparisons of Scenarios

To compare the curvilinear scenario to the turn-straight-turn scenario, we have to evaluate the
quantity for all cases. Using expressions [8], [9], [11] and [12], it is possible to construct the
following comparisons:

Case 1 Always Turning in Same Direction

From [9] and [11]:

8
This is clearly true because the total arc length for a curve between any two points is always greater
than the linear distance between the same two points.

Case 2 Inflection Point

From [8] and [12]:

(| | | | | | | |)

From [13], we know that the first term will always be positive. We now examine the second term.

Referring back to Figure 3, it is possible to make geometric arguments to support this statement:

If this statement were not true, then at point I, the destination point B would lie to the right of the
heading vector instead of the left. If the robot started turning left at this point, it would never reach the
destination. (Based on symmetry, the same argument applies if left and right were reversed.) Therefore
the statement must be true. This also implies the following:

From [14] and [15], we can say

(| | | | | | | |)

Using [13] and [16], we can state that for Case 2 as well.

Conclusions

Since for both Case 1 and Case 2, and since these cases span all potential situations, we can
conclude that there is no benefit to following a smooth curve between two poses. Rather, a turn-
straight-turn approach to path planning is the best approach for tank-like FRC robots.

References

Dubins, L.E. (July 1957). On Curves of Minimal Length with a Constraint on Average Curvature,
and with Prescribed Initial and Terminal Positions and Tangents. American Journal of
Mathematics, 79 (3).

Brachistochrone Problem mathworld.wolfram.com/BrachistochroneProblem.html