I was developing a module to plan paths for an
automated vehicle when I came across a confounding hurdle. In order to complete
a set task, the vehicle needed to arrive at its destination in a specific
orientation. The automated vehicle was a pilot project for picking up and
delivering shipping containers within a shipping port terminal yard. Containers
are loaded onto ships with doors facing toward the rear of the vessel to avoid
salt water spray entering the container, hence the requirement for a specific
orientation of arrival.
At the time I was unable to find any
references dealing with the inclusion of vehicle orientation in the shortest
path problem, and to this day have not come across any material addressing it.
My colleagues suggested finding the shortest path and later inserting a flip to
correct the orientation if it was needed. It felt dirty to take an optimal path
and add a kink to it. There was also a tangible downside; using the plan and
kink approach often required the vehicle to turn, come to a complete stop, and
then reverse. Stopping to reverse often took more time than a longer route that
did not require stopping.
I was modeling the problem as a graph
representing the map of everywhere that the vehicle could travel. This fitted
nicely with the intended use because the vehicle was constrained to travel over
stacks of shipping containers and follow predefined roadways. The shortest path
algorithm (Dijkstra) is to start at some node in the graph and examine the
edges that allow travel to another node. Each edge has a predefined cost. In my
case the nodes represent possible locations the vehicle can travel to. The
edges between the nodes represent short maneuvers the vehicle can perform.
Maneuvers include traveling straight ahead, turning left, turning right,
reversing, reversing left, and reversing right. The cost is the travel
distance.
The next step in the shortest path algorithm
is to examine all the possible edges from the starting point, and select the
one with the least cost. This is the shortest possible path between the
starting point and the selected node, because all other routes require taking a
more expensive edge first. We record that route and mark it as ‘found’. The
same is not necessarily true of the other edges because there may be an even
shorter route via the above selected node. Now we need to examine all the edges
from the recently ‘found’ node. Thus we keep an ‘open set’ of edges that are
currently under examination, select the next minimal edge, mark the connected
node as ‘found’, and expand the ‘open set’ with the edges from that node. This
continues until we mark the final destination as ‘found’ and know that by
following the path we recorded back to the start we have the shortest path
between the start and finish.
There are some very interesting algorithms
which build on the shortest path algorithm. A* guides the expansion of the
search in the direction of the destination by observing that if the direct
distance added to the cost of travel to a particular node is greater than the
open paths, it cannot contain a shorter path. Floyd-Warshall is able to
calculate all shortest paths relatively quickly by storing a matrix of
discovered paths. But there is not a prescribed way to arrive with a target
orientation, which was the particular problem I was facing.
After much fruitless research, discussion and
experimentation I had an epiphany. The problem could be solved simply by
creating a virtual graph that would overlay on top of the original graph, but
represent the vehicle being in the opposite orientation. One physical location
would have two virtual nodes which were not connected to each other (because
the vehicle cannot instantaneously flip itself). These nodes were connected to
adjacent nodes only in ways that were physically possible for the vehicle to
travel. For example if the vehicle started at node A facing North, it can
travel forward North to B and arrive still facing North. So the A-North node is
connected to the B-North node with an edge, but the A-North node is not
connected to the B-South node because that is an impossible maneuver. Where
this becomes more interesting is in the turns. A-North is connected to C-East
because you can start at A facing North, travel forward and turn to arrive at C
facing East. Following enough turns you can find a path to A-South from
A-North, describing a path that goes nowhere but flips the vehicle.
The first beautiful thing here is that we
produce a connected graph that perfectly describes all the loops and reversals
that the vehicle can physically perform. All that is required to generate the
orientation preserving graph is to be able to calculate when following an edge
from a given starting orientation, what the final orientation will be. This is
simple geometry. The second beautiful thing is that identifying the reversals
can be done with the same simple geometry and pre-calculated for the graph. So while
expanding the search I add the cost of stopping the vehicle and accelerating in
the opposite direction to edge selections that are reversals.
Now we have a graph that accurately describes
all the possible maneuvers and real cost of travel. Because this is a regular
graph I apply the standard shortest path algorithm. A* and Floyd-Warshall also
work nicely. To find the optimal path I specify the starting node that has the
vehicle’s starting orientation and the finishing node that has the desired
final orientation. This calculates the truly optimal route for the vehicle to
travel, not an approximation.
I remember when I first described the
epiphany to my colleagues on a whiteboard they looked at me with baleful
skepticism, but I was so excited that it did not dull my momentum one bit. The
algorithm was very simple to implement and so I was able to quickly demonstrate
to them how well it worked in practice. It was a fantastic feeling to see the
vastly improved path selection and their skepticism was quickly replaced with
enthusiasm and interest.
After that discovery I became the principal
Traffic Management System developer and had the opportunity to implement
complex routing and scheduling to coordinate twenty five fully automated
vehicles to move shipping containers at the Port of Brisbane, which are still
in operation today. The project had a profound impact on the port operation by dramatically
increasing safety, reducing damages and increasing productivity. There were
many challenging problems which I created solutions for, but the orientation
preserving routes solution had a deep resonance in me because it combined
people, algorithms, simplicity and resulted in an elegant solution. I was
relatively new to the team at the time and the discovery made me instantly an
integral part of the team. I knew I had earned my colleagues’ respect and that
they were eager to work with me. It was a struggle to reach the solution which
made the victory sweeter. It was not something I could mechanically apply from
a book, and it brought genuine value to the project I was working on. There was
a kind of beauty to the simplicity of the solution as opposed to being an
edifice of hard work. It was a solution that composed well with other
solutions.
One thing that stuck with me from this
experience is that a seemingly impossible problem can have a simple solution. A
little bit of insight goes a long way, so you need to think and explore around
the edges of what is possible. I have since found that this is true of people
oriented challenges as well as technical problems. When faced with a difficult
team or individual I find that dramatic improvements can be achieved with
subtle adjustments. To find these adjustments you need to be truly interested
in the dynamics of what is happening with the team or individual, make the
effort to listen and understand not just what you see but also what other
people have observed, and explore ideas that might provide an improved outcome.
Discovering that simple but powerful solution
also impressed upon me that algorithms can have a very real world impact, that
software is a powerful lever which allows us to change the world for the
better, and that innovation enables us to make that impact a reality.
I am a passionate advocate of cultivating an
environment that encourages innovation and enjoy reading on the subject. One
aspect that is common in innovation is the existence of a network which allows
ideas to be transmitted and explored quickly. Looking back I can see how the
environment I was in at the time contributed to arriving at the solution. I was
learning new graph algorithms, exploring the problem with the team, discussing
the challenge with University advisors involved in the project, and coming up
to speed on the geometry of the steering control function generation being
developed by a colleague. Tailoring an environment that will nurture the kind
of innovation appropriate to the task is essential to keeping a team
interested, productive, and highly successful.