Path Optimization for Ground Vehicles in Off-Road Terrain

Many methods to get optimal paths that satisfy all constraints are slow. This is important due to the relationship between processing rate, vehicle speed, and the minimum reaction radius. The minimum reaction radius is the minimum distance at which the vehicle is capable of reacting to new information. It can be thought of as the total system latency multiplied by the vehicle speed. Since path planning (along with sensor segmentation) takes up the bulk of this time, this radius can be significantly reduced by improved planning methods. The ideal situation is a planner that is both fast and satisfies all constraints. However, this is hard to achieve [1][2]. Another solution is to take a quickly generated path and apply constraints after generation through an optimization process. By doing this you can gain some of the benefits of both fast and accurate planners. Although it should be noted that the constraints may significantly change the optimal path such that a path optimizer, if given this initial path, will converge to some locally optimal path rather than the global optimal. Thus, a distinction should be made between an optimized path and an optimal path.

As vehicle speed increases the kinematic, and even dynamic, constraints can restrict the free space of the vehicle even more than the physical obstacles present in the environment. Therefore, it becomes advantageous to start thinking in a framework where these constraints can be nativity represented. Typical world space representation are good at representing obstacles but have no native way to represent kinematic constraints such minimum turning radius. Additionally, path properties like smoothness are harder to quantify.

Representing the path in the actuator space switches this problem around. Now, provided with a good vehicle model, kinematic and smoothness constraints can be represented as obstacles in the actuator space. However, as vehicles inevitably operate in the real world, and typically aren’t interested in purely achieving some actuator state, we must still consider the world space. Thus, requiring us to transform between representations when checking for obstacles. This leads to a trade-off between the ease of representing kinematic constraints and world constraints.

This concept of actuator space planning has a long history in manipulator planning. In fact, it’s typical for manipulator planning to be done entirely within the actuator space with obstacles transformed into it from the world space. However, manipulators have the advantage that their actuator space and state space are identical. Typically, they are trying to achieve given state in the actuator space so, aside from the initial obstacle transform, transforming states isn’t necessary. This leads to an ideal environment for optimization with several works being done [3][4][5].

Recently, the concept of actuator space has had some applications in planning for air vehicles, typically small quadcoptor Unmanned Air Systems (UAS). However, this transform will often only be used to check that the path is within the limits of the actuator[6][7]. Due to the kinematics of these UAS, scaling the path speed around offending segments is typically all that’s needed to satisfy these constraints. So the path can be transformed into the actuator space and rescaled without the need to directly plan in the space.

From an optimization point of view, UAS are another good candidate for path optimization [8][9]. They typically operate in binary space, that is, the space is either free or has an obstacle and there is no preference for one set of free space over another. Additionally, due to their higher speed, path smoothness is highly desired. Thus making kinematic constraints the dominating factor in path planning.

For ground vehicles, thinking in the actuator space isn’t a new idea. Model predictive control can be thought of as operating in this context. Additionally, many methods of trajectory sampling or path primitive generation fall into this context. However, due to the larger set of constraints on ground vehicles and the comparatively complex set of world space obstacles, longer path planning is typically done in the world space. Although there have been some examples of actuator space planning for short trajectories [10].

Optimization has also seen comparatively less literature than in the other domains. Most of what exists is used as a path smoother for planners such as the RRT family [11]. The history of path optimizers can be thought of as beginning with the application of Dubins [12] (and later Reed-Sheep’s paths) to methods such as A* [13][14] and it’s derivatives, such as D* [15] and D* Lite [16], and RRT [17]. Although these paths can be thought of as satisfying all the kinematic constraints they are discontinuous in steering angle. A solution to this is the clothoid path. Clothoid guarantee continuous steering angle and limits such as max steering angle and max steering angle rate are natural to the path formulation[18]. However, there is no closed form solution to the clothoid path. Additionally, clothoids can only represent a subset of all feasible paths. Most recent optimization work has represented the paths as polynomial splines [19][20][21][22]. These splines trade some of the benefits of clothoids for the closed form nature of polynomials. Additionally, they can represent a much larger segment of the set of all feasible paths.

The cost map process is a continuation on the methods developed in [23]. A lidar scan is taken over a local grid map. From this scan, obstacles are extracted by taking the heigh difference between the minimum and maximum height return in each cell. If this difference is larger than a given threshold then the cell is marked as containing an obstacle. Here, we consider two types of obstacles, hard obstacles (trees, rocks, people, etc) are those that should never be driven through, and soft obstacles (bushes, tall grass, etc) that should be avoided but are traversable. This is determined by our segmentation layer, each lidar return is marked as either a hard or soft object. If there are soft object returns above the obstacle threshold but no hard returns then the cell is a soft obstacle. Otherwise, if there’s a hard return above the threshold it’s a hard obstacle.

Both the hard and soft obstacle maps share a common process, shown in figure 3. As they are binary maps, rather than a continuous function, they can be expanded with a dilate operation by the radius of the robot. The area inside this expanded region is then converted into a distance region. Where the value of each pixel represents the distance to the edge of the region. This is done to ensure that there is always a strong gradient in the costmap to push the solution out of obstacles. Finally, a similar process is done to create a repulsive halo around around obstacles. This, once again, helps the solver avoid obstacles but also prevents discontinuities in the cost map.

The final costmap is the simple per-pixel sum of both of these sub-maps. Thus, we maintains both the strong gradients and continuity in the final costmap.

Before the optimizer can run it needs to be given a base path. For our implementation we chose to use A* [13]. It was chosen due to its guarantee of optimality and fast run time. Here, we used 8 way connectivity and the euclidean distance as a heuristic. The vehicle’s current position is used as the start of the search. If the goal point is within the local map it is used as the goal for A*. Otherwise, the closest point within the map to the goal is used for A*. This search is done over the same costmap as the optimization. Despite the guarantee of optimality of A* it doesn’t take into account the kinematic constraints of the vehicle. Thus the final optimized path will deviate from the A* path.

Once the base path is generated it needs to be converted into the actuator space. The typical path representation in the world space is a list of $(x,y,\theta)$ representing the position and orientation of the vehicle on a 2D plane. An actuator space path is represented by a list of $(\phi,dr)$ along with the initial $(x_{0},y_{0},\theta_{0})$. Here $\phi$ is the current steering angle and $dr$ is the distance traveled over some fixed time step. We assume a constant path speed. Figure 4 shows an example path represented in both world and actuator space.

To convert the path from world space to actuator space we need to have an inverse model of the vehicle. Note that this method isn’t tied to a particular model but does require an inverse model. Here we use a 2D model although a 3D model could be used at the cost of increased dimensionality. Equations 1 and 2 describe the inverse model. If the input path doesn’t contain heading information then we can use equation 3 to back calculate it. Although this will leave the heading at the final point undefined, in practice it can be assumed to be the same as that of the next to last point.

Transforming from the actuator space to world space is easier. It can be thought of as simply integrating the forward model with respect to some starting point $(x_{0},y_{0},\theta_{0})$. Equations 4, 5, and 6 describe this process.

Finally, we define the cost function to optimize against (equation 7). Here $P$ represents the path and $p$ is a point in the path. We have already talked about the costmap but to fully capture the cost of a path we must also consider it’s kinematic properties. This leads to a cost function with both a map cost and a kinematic cost.

Breaking down this function further leads to the next two equations. The costmap cost (equation 8) is simply the sum of all the cost map cells through which the path moves.

Next, we are concerned with the constraints imposed on the vehicle by our kinematic model. Here we get two main constraints on the path, the steering angle $\phi$ must lie between some maximum and minimum values (equation 9). Typically, we can assume that $-\phi_{min}=\phi_{max}$ so this constraint reduces to equation 10.

Similarly, the rate of change in the steering angle is also of interest to us. This leads to another set of constraints in equation 11 . Once again we can assume that $-\dot{\phi}_{min}=\dot{\phi}_{max}$ so this constraint reduces to equation 12.

Rather than writing these as hard constraints, we instead represent them by large, steep cost gradients outside of the boundaries. However, even within the boundaries imposed by our constraints we are still interested in smooth paths. Equation 13 is the final kinematic cost function with $\alpha,\beta$ being the in and out of bounds cost factors respectively.

With the cost function now defined the path can be sent to the solver. Here, we cause perturbations to the path at each point such that each perturbation decreases the cost of the path. However, perturbations at the start of the path will have a much larger total effect on the path than perturbations later in the path. Additionally, the actuator space does not fix the goal point of the path. To solve both these issues we dampen the perturbation over the next N time steps. This is done by using an S curve to interpolate between the perturb path and the original path in world space then transforming this path back into actuator space. Figure 5 shows this process for a perturbation at step 5 with a damping length of 25 time steps.

The solver itself is a simple implementation of gradient descent. It is described by algorithm 1. In brief, the cost for the perturbed segment is checked for both left and right perturbations over the perturbation distance. This, along with the current cost of that segment, defines the gradient. Then, a final perturbation is chosen for that point according to the gradient and a scaling factor. This process is repeated for each point along the path. When there exist no more cost reducing perturbations along the path, or the maximum iteration limit is reached, the path is returned.

So far we have only been concerned with the steering angle of the vehicle and any obstacles the path might run into. However, we also want the vehicle to traverse this path as fast as possible. Thus the need for a speed planner that will maintain a high speed while respecting the constraints of the vehicle. Speed control is done in two passes. First, the maximum speed at each point is determined based on that point’s steering angle (equation 14). Here $v_{min}$ and $v_{max}$ are the minimum and maximum speeds, $\gamma$ is the steering angle at which we want the minimum speed, and $p_{v},p_{\phi}$ are the steering angle and speed for the point $p$. Additionally, the goal point is assigned a speed of zero.

Next, backwards and forwards passes are done over the path such that all the path speeds lie within the maximum and minimum acceleration limits of the vehicle.

After this speed control pass the path is converted back into world space and then published to the vehicle controller.

We tested this system on a Clearpath robotics Warthog. The system architecture is described in figure 6. We equipped it with an Ouster OS1-64 lidar and a Vectornav VN-300 INS. Data from the IMU and wheel odometry was fused using an extended kallman filter to provide fused odometry data. Our map had a resolution of 20 cm with a size of 512x512 and updated at a rate of 2 Hz. The A* planner was ran at 4 Hz with the optimization module running at 10 Hz. The tests were done with a speed of 5 m/s, the vehicle’s maximum speed, and a minimum turning speed of 2 m/s.

The following experiments were done at the Texas A&M RELLIS campus. This first test was done around a large rock pile (figure 7). Figure 8 shows the test on the map. The goal point was given on the other side of the pile. The line in red shows the initial A* path, the blue line shows the resulting optimized path. Note that the resulting path is similar to the input path but avoids the sharp turns in the base path. The right figure shows the final driven path in yellow. We can see that the driven path is initially similar to the first plan. However, once the vehicle can see the other side of the rocks the path is re-planned.

In test 2 we go around a large hill (figure 9). Note that the optimized path is significantly different from the base path (figure 10). Unlike the first experiment, the terrain around the hill is largely empty. Here, the planned path didn’t significantly change as the robot moved, but controller errors led to an overshoot of the first turn. However, this was smoothly corrected and the resulting path was still collision free.

In addition to the real testing we also did several runs in simulation. The simulator was created in the Unity game engine and is integrated with the Robot Operating System (ROS). The simulation vehicle is a replica of the Warthog used for the physical testing. Figure 11 shows the simulated test environment. First we ran the system with the A* planner as a baseline (figure 12). Next, the same path was ran with our optimizer running on top of the A* path (figure 13). All other settings were left unchanged. A maximum speed of 5 m/s and a minimum speed of 2 m/s were used. The goal point was set so the vehicle would go the small gap between the fence and boat. With only the A* path the vehicle would overshoot the path and get stuck in the gap. Adding the optimizer allowed the vehicle to successfully navigate the obstacles.