SSC San Diego has been involved in the development of autonomous vehicles for over twenty-five years. Autonomous UGVs have become highly sophisticated and capable of robust navigation in very complex environments. Examples of this include the Defense Advanced Research Projects Agency (DARPA) Grand Challenge, the Army Research Laboratory (ARL) Demo III vehicle, and the National Aeronautics and Space Administration (NASA) Mars Rovers.
It is the intent of SSC San Diego to leverage technology developed in the UGV arena and adapt and apply the results in the USV domain. The rational for this is not simply financial, but also because there are many corollaries between the UGV and USV environments. These include a mostly planar surface with potentially complex features such as hazardous terrain areas, pre-defined navigation ways and driving rules, dynamic obstacle environments, through-the-air sensing, etc. The SSC San Diego development approach for USV autonomous navigation capabilities is the same as for all of its unmanned vehicle programs. That is to develop basic, robust, transition-ready capabilities, and then build upon those capabilities with progressively more sophisticated and enabling functionality. The point is to transition and field technologies incrementally rather than spend years trying to develop the entire system all at once.
Figure 2. Basic capability blocks of an autonomous vehicle
A simple example of this is shown in Figure 2, where the base functionality is waypoint navigation, which has been well demonstrated on the SSC San Diego USV and many other unmanned vehicles. Waypoint navigation without obstacle avoidance (OA), however, provides only limited capabilities to the warfighter in a real-world mission. In order to provide more functionality and reduce the reliance on operator oversight, a robust obstacle avoidance capability must be added. Once the obstacle avoidance piece has been sufficiently demonstrated, then more advanced behaviors can be added, such as autonomous recovery in the case of lost communications, target tracking and/or interception, etc. SSC San Diego is currently focused on the obstacle avoidance block which is the subject of the following sections.
OBSTACLE AVOIDANCE ARCHITECTURE
A two-tiered obstacle avoidance approach has been adopted, consisting of a near-field or reactive OA component and a far-field or deliberative OA component that operate simultaneously and in conjunction with one another. The primary function of the deliberative component is to continuously modify the existing waypoint route to plan around obstacles detected with the long-range sensors. The reactive OA component is responsible for avoiding obstacles in close proximity to the vehicle regardless of the vehicle’s mode of operation or mission. Figure 3 attempts to depict this approach graphically. The deliberative OA component or path planning function attempts to modify the existing route from the outer horizon of the reactive component and beyond. Meanwhile, the reactive OA component is only concerned with obstacles between the reactive/deliberative boundary and the vehicle.
Obstacle Avoidance Software Architecture
To help set a frame of reference, the USV obstacle avoidance software architecture will be addressed, but only at a high level. Figure 4 is a basic block diagram of the OA software architecture as it exists on the USV. An attempt has been made to modularize the OA software into distinct functional components.
The deliberative OA component consists primarily of the path planner, which interfaces directly with the navigator. The path planner receives data from both chart server and radar server. The reactive component intercepts the tele-operation or driving commands from the navigator and modifies them before forwarding them to the driver for execution at the actuators. The reactive component also interfaces with the radar and chart server but receives different types of data than is received by the path planner. The reactive component receives data from the other near-field sensors as well, such as the vision and ladar systems. It’s worth noting that all of the interconnecting lines in the block diagram are Joint Architecture for Unmanned Systems (JAUS) messages. In the future, the use of standard messages for the component interfaces should allow the unmanned systems community to exchange and share basic navigation components.
Figure 4. Block diagram of existing USV OA software architecture
DELIBERATIVE OBSTACLE AVOIDANCE
The task of the deliberative obstacle avoidance component is to plan a path in the far-field that follows the original path as much as possible and avoids obstacles, both moving and stationary. It does this with the help of a path planner using a two-dimensional (2D) obstacle map. The obstacle map is essentially an occupancy grid2, which is created by dividing the environment into a discrete grid and assigning each cell location a value representing the probability of being occupied or not occupied by an obstacle. The map for the deliberative OA component is filled with stationary obstacles from the chart server and moving obstacles provided by the radar in the form of Automated Radar Plotting Aid (ARPA) contacts.
Besides stationary obstacle data from the chart server, users may add their own obstacles to the map through operation or exclusion zones. Operation zones are sets of points that make up polygonal areas within which the USV should operate, and exclusion zones represent areas the USV should avoid.
Planning around stationary obstacles is rather trivial as proven best-path search algorithms have been used to plan optimal paths around stationary objects for years. The underlying search technique for the USV’s deliberative obstacle avoidance is the A* (A Star) search algorithm. A* was chosen because it can find an optimal solution in a short amount of time. Also, since A* uses a cost analysis at each step, inserting an added cost for proximity to obstacles was a natural process. This cost allows the USV to set a safety barrier around obstacles, which can also be adjusted for different obstacles. This cost analysis also can be extended for other costs such as direction, shipping lanes, “soft” obstacles, route time, etc.
Path planning around moving obstacles, however, is not a trivial task. Canny and Reif 3 showed that motion planning for a point in a plane with bounded velocity in the presence of moving obstacles is Non-deterministic Polynomial-time hard (NP-hard) or, in other words, very difficult. No research has been found which produces optimal solutions with computations in near-real-time as required by a USV in motion. Aggarwal and Fujimura4 show that a more optimal solution can be found by adding a third dimension of time and plotting the location of the moving obstacles along that three-dimensional (3D) structure, but it is not close to meeting the time constraints of a USV planning 200 or 300 yards ahead of its current position while traveling 20 knots. Fujimura and Samet5 provide yet another solution representing obstacles in time through a quadtree-type hierarchical structure, but even they admit the solution is best with few moving obstacles. The requirements for this project were that the solution not necessarily be optimal, but could allow for many obstacles and be completed within seconds. To keep the amount of time to search for a valid path manageable, the path planner has translated the third dimension of time of a moving obstacle to a 2D projected area. This truncation from three dimensions to two means that a solution can be found within seconds, but there is no guarantee the solution will be optimal nor always completely acceptable. For this reason, there still needs to be an extremely fast reactive obstacle avoidance system. Still, the deliberative obstacle avoidance will increase the likelihood of avoiding collisions and make the job of the reactive component much easier, resulting in less violent obstacle avoidance maneuvers. This idea is similar to an algorithm used for UAV path planning by Rathbun, Kragelund, Pongpunwattana, and Capozzi.6
To avoid moving obstacles and maintain the waypoint path set by the user, the path planner determines safe velocity ranges using the Velocity Obstacle method.7 This algorithm transforms a moving obstacle into a stationary one by considering the relative velocity and trajectory of the USV with respect to the obstacle. After producing a collision area called the Velocity Obstacle, defined using the relative velocity vector, the algorithm returns a set of USV velocity vectors guaranteeing collision avoidance. This transformation and collision area detection, when applicable, reduces the complexity of the path planning among moving obstacles problem to linear time. This is used as a first pass to avoid moving obstacles. However, in the case that changing velocity doesn’t avoid collisions, the path planner changes the path by creating projected obstacle areas for each obstacle and determining a safe alternative route using the A* search.
Projected Obstacle Areas
To achieve a more realistic real-time planner, a projected obstacle area (POA) was created, which is the area a moving obstacle could occupy in the future: a translation from 3D to 2D. The possible area a moving obstacle could occupy during the entire planning stages of a path could fill up the map with too many obstacles and hinder the movement of the USV. Therefore, it was necessary to identify a particular moment where the moving obstacle would pose the greatest threat (when the obstacle is at its closest point to the USV). This greatest threat is found using the closest point of approach (CPA), which is the minimum distance between the two objects in time along their respective paths. Since a moving obstacle can pose a threat to the USV along multiple stretches of the path, it was necessary to calculate the CPA of every obstacle along each path segment. This results in a single POA for each obstacle for each path segment, similar to snapshots of the moving obstacles in time, thus providing an accurate representation of the areas the USV will want to avoid.
COLREGS (Rules of the Road)
Because the projected obstacle area is an estimate of the location of the obstacle in the future, and because moving objects in water very rarely continue along the same heading at the same velocity, the POAs are made to handle uncertainty. Each POA contains uncertainty values to modify the associated area, simulating a moving obstacle that might speed up, slow down, and/or change direction, as shown in Figure 5.
Figure 5. Projected obstacle areas display results of different uncertainty values (dashed lines represent the average POA and the solid lines represent the new POA. (a) An average POA (b) An increase in port angle (c) An increase in starboard angle (d) An increase in ahead distance (e) An increase in astern distance
Through the use of these uncertainty values, the deliberative obstacle avoidance component can mimic the International Regulations for Avoiding Collisions at Sea, COLREGS, or rules of the road. Increasing the uncertainty angle on one side of a POA might mean that the lower cost path passes on the other side. The path planner addresses three rules of the road: 1) when meeting head on, pass port-to-port; 2) continue on course when meeting port-to-port or starboard-to-starboard; 3) when crossing, vessels found in your danger zone (on your right) have the right of way. An example of rule 1 is shown in Figure 6.
Figure 6. The projected obstacle area of the moving obstacle with an increased starboard uncertainty angle forces the USV to plan around the obstacle port-to-port.
The POA of a moving obstacle is calculated from the current path of the USV and the time taken to traverse that path. As that path changes, there is a need to update the POA and recalculate. For this reason, more than one iteration of the planner could be needed to account for updated POAs and changes to the path. Usually the program requires no more than 2-3 iterations to return a successful obstacle avoidance route.
The autonomy of the USV is still in its early stages, which requires that the user stays in the control loop at all times in the deliberative obstacle avoidance process. Any route change made by the path planner has to be accepted or rejected by the user. For this to occur, SSC San Diego’s own Multi-robot Operator Control Unit (MOCU) needs to be able to communicate with the obstacle avoidance component and receive updated routes from it. Currently the process starts with the user creating a route for the USV to follow. This route is sent to the USV’s navigator and immediately executed. The path planner then begins to plan an obstacle-free path. If any significant part of the route has changed, the path planner notifies the user of an alternative path (Figure 7), giving up all control to the user and setting its planning on hold until a response is received. That path is displayed simultaneously with the current route in MOCU and the user can accept, accept and edit, or reject that path. If the user accepts the new path, those changes are sent to the USV’s navigator as the current route. If the user accepts the path but makes a few changes, that path is sent back to the path planner to be planned again, which then is returned to the user for acceptance. If the user rejects the path, the path planner repeats its planning process.
Figure 7. Screen capture of MOCU with an obstacle avoidance route displayed in pink dashed lines. The previous route is displayed as solid green lines. This particular OA route avoids the buoys (solid yellow circles) and ARPA contacts (yellow quatrefoil).
When a user specifies a set of waypoints for the USV to follow, it is assumed they have a specific purpose for creating each one of those waypoints. Therefore, every time the path planner runs, it attempts to maintain as much of that user-defined route as possible. During each iteration, the deliberative obstacle avoidance component plans from the original route. If the USV has already deviated from the original route to avoid obstacles, then the planner attempts to return to the original route as soon as feasible. This removes any obsolete paths caused by volatile moving obstacles or unreliable ARPA contacts. An example is demonstrated in Figure 8.
Figure 8. Maintaining the original route (a) The original route (b) OA route planned around 3 moving obstacles (c) OA route looks more like the original route when the headings of two of the moving obstacles have changed (d) The OA route completely returns to the original route when the changes from c are kept and the third moving obstacle is removed
REACTIVE OBSTACLE AVOIDANCE
The deliberative OA component or path planner attempts to modify the route so that it is as clear of obstacles as possible. The deliberative OA component does not guarantee obstacle avoidance, however, for multiple reasons: 1) the USV may inadvertently deviate from the planned path if GPS is jammed or the Inertial Navigation Unit (INU) drifts; 2) the long-range sensors are not capable of detecting small low-profile obstacles such as very small personal boats; 3) the deliberative OA component is only useful if the USV is in waypoint navigation mode. In a manual or tele-operation mode, auto-heading mode, target-tracking mode, etc., there is no preplanned path to modify. Because of these limitations, a reactive component has been included in the overall OA architecture.
The reactive OA component is responsible for avoiding obstacles that come in close proximity to the USV, regardless of the mode of operation or current mission assignment. It does so by intercepting any tele-operation or driving commands before they reach the actuators and modifying them in real time so as to avoid collisions.
The near-field component of the two-tiered approach, also known as the reactive OA, reacts to short-range obstacles, somewhere within 400m of the USV. The USV avoids these obstacles by modifying the throttle and steering commands in real-time based on the combined votes of each behavior. The SSC San Diego implementation of reactive OA is a behavior-based common world-model approach. That is to say, all of the near-field sensors are fused into a common local world-model, and individual behaviors vote on specific navigation solutions within that model. For instance, the obstacle avoidance behavior votes for actions that avoid or turn away from potential hazards while the path-following behavior votes for actions that will keep the vehicle on the planned path.
This approach is not novel but has a long history of applications in real-world systems (including the Mars Rovers) and has its lineage back to the Carnegie Mellon University Morphin algorithm8 and Distributed Architecture for Mobile Navigation (DAMN). As applied here, a number of arcs are projected in front of the vehicle over the local world-model obstacle map (Figure 9). The number of arcs considered is a function of the map size and grid spacing, with the arcs spaced such that one arc passes through each of the outer cells. This approach guarantees that each cell in the grid is covered by at least one arc so that all navigable paths are considered. Each of the arcs is related to the vehicle velocity and turn rate by where R is the radius of the arc, V is the vehicle velocity, and is the vehicle turn rate.
For the OA behavior, each arc is given a weight or vote based on the distance the robot could travel along that arc before it encountered an obstacle.9 The longer arcs are weighted more favorably than the shorter arcs or arcs that are blocked near the robot. The votes are scaled from 0 to -1 so that they can be combined with votes from other navigation behaviors by the arbiter (Figure 11, top right).
One common navigation behavior is waypoint navigation or route following. The waypoint navigation function produces a commanded turn rate and velocity based on the heading error between the robot’s current heading and the heading toward the goal point along the path (Figure 10). At each cycle these turn-rate and velocity commands from the waypoint navigation function are converted into an arc. To form votes for the entire array of arcs, the primary arc is given the maximum value and votes for arcs on either side of it are linearly decreased until they reach a value of zero (Figure 11, top left). The path following behavior does not vote against any arcs.
Figure 9. Obstacle grid with arcs shown
Other arc-voting behaviors have also been added to help navigate around obstacles. These include a free-space behavior that votes (from 1 to 0) for large continuous sections of arcs that are unblocked, essentially open areas in the obstacle map (Figure 11, bottom left). This behavior also votes for open arcs that fall between obstacles so that the robot won’t always favor going around groups of obstacles.
Weighting factors are applied to the votes from each of the behaviors prior to combining them so that some behaviors contribute more to the final outcome than others. In the current implementation the OA behavior is weighted almost three times heavier than any of the other behaviors. The combined votes are shown in the bottom right plot in Figure 11.
Figure 10. Depiction of heading error between robot’s heading and heading to the goal point.10
Figure 11. Example votes from behaviors
While implementing this reactive obstacle avoidance component on the USV, there were a few issues that needed to be addressed. The following describe some of these and their solutions.
Initially there existed a disconnect between the commands that were being sent and the actual steering of the USV because the commands did not account for environmental factors such as wind, current, or even an unbalancing of the boat. A feedback control loop was implemented to ensure the selected arc on the reactive OA was executed accurately regardless of the environment or other physical conditions. A KVH gyroscope (maximum reporting angular rate at 100 Hz) was added to report turn-rates, which were then integrated into a PID loop for steering auto-correction.
Test results showed that occasionally the output from the arbiter, the decision-maker of the reactive component, fluctuated between steering to the left and right side of an obstacle, approaching a near-collision. This was due to the combination of votes from a path-following behavior and an obstacle avoidance behavior; a turn to the left would move the path-following vote to the right, to return to the path, and increase the overall vote to the right, which could eventually earn the highest vote and change the course of the USV. A simple solution of decreasing the overall vote for all possible vote bins on the opposing side of an obstacle, relative to the USV, made it more difficult for the vehicle to switch directions and cross over in front of obstacles.
During testing, it was also discovered that as obstacles moved past the vehicle and out of the obstacle grid, the USV would at times turn sharply in an effort to return to the route, turning directly into the obstalce. The obstacle arc grid used by the reactive obstacle avoidance program uses only the forward-looking half of the environment (i.e. that which is in front of the vehicle), leaving out obstacles sometimes only meters away, just because they were behind the vehicle. For extremely quick turns however, it is important to have knowledge of obstacles that are close behind the vehicle. In the near future a more complete near-field world model will be developed but in the interim, to mitigate problems, the data in the rows just behind the USV are mirrored up to the current horizontal plane, eliminating those sharp turns which would lead into obstacles.
Figure 12. Reactive world model showing the placement of an obstacle off the map and behind the vehicle and the resulting mirrored obstacle onto the map.
Because of the drag properties on the sea surface and the variability in the control of a USV's rotational speed, it was not uncommon for the USV to slightly drift sideways into the obstacle area (or the grown area representing an obstacle), causing the USV to evoke emergency procedures to return to open water. This was addressed by modifying the characteristics of the free space voting behavior by decreasing the values of the votes near the obstacles, creating a buffer around obstacles, and giving more priority and more voting value to the arcs that are slightly farther away from the obstacle areas.
Figure 13. Free space voting behavior with decrease in value for near-obstacles and the corresponding obstacle map.