Shortest path search while preserving vehicle orientation


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.