Motion planning for an Ackerman-steered rover

Despite progress in technology and the development of microelectronics, the task of finding an optimal path remains very challenging for modern computers—whether CPU or GPU. The planning horizon for many local algorithms (e.g., DWA, TEB, MPPI on CPU) typically does not exceed a few meters, and sometimes even decimeters. However, planning over a longer time interval and distance allows algorithms to better find paths in obstacle-dense environments and is a crucial element of a system for avoiding moving, dynamic obstacles. To address the challenge of creating a pathfinding module with a long planning horizon, this article will examine the MPPI-Generic local planner package, which runs on the GPU. It can work in conjunction with the State Lattice Planner from ROS NAV2, but in this demonstration, it will be used independently—as a "universal planner." Testing of both planning packages will be conducted in a custom-built Qt-OpenGL car (pickup) simulator written in C++ for tighter integration with the simulator and convenient access to the MPPI-Generic CUDA kernels.

It is worth starting with the fact that in an ideal planning system, there should be no division into global and local planners. The planner should be universal: immediately calculate all system states during planning and find the optimal path. Such systems are found in unmanned vehicles driving on roads - usually, it is either a massively parallel planner or a neural network one.

The "global + local planner" bundle is a forced measure due to the limitation of computational power of modern computers and the complexity of creating a massively parallel global planner. There are also caveats to the term "global". A true global planner involves planning from your current location to the destination. In terms of robotics and ROS, a global planner is one that can chart a path from one point to another in a warehouse, office, or at home, i.e., with a planning horizon of tens of meters. Otherwise, the planning time starts to go beyond realtime and reaches tens of seconds or even minutes.

However, in outdoor robotics, planning for just a few meters is often insufficient due to the high speeds of street traffic. To recognize a dangerous collision in time, it is necessary to predict the trajectory of all vehicles visible to the rover at least linearly (based on their current velocity vector), as well as to know its own trajectory several tens of seconds ahead. At the same time, it must be recalculated at the frequency of data reception from the main source of information about obstacles—the lidar, which is typically 10–20 Hz, to be able to react in time.

Unfortunately, many algorithms in the open source do not fit into such strict constraints. For example, the runtime of hybrid A* algorithms from random GitHub repositories on a desktop:

I got approximately similar results when trying to create my own solution. It was only faster over short distances:

Simple naive approaches, relying on the computer's processing power and solving the problem via so-called "brute force," do not work here. Complex optimizations are required—those that involve sacrificing something for the sake of speed, but in a way that does not significantly affect the quality of trajectories. Heuristic search algorithms are needed that cut off obviously poor options and choose from several most probable ones, rather than enumerating everything indiscriminately. And this is a decidedly non-trivial task.

Everything affects the pathfinding time: route length, number of obstacles, target orientation (nose or tail relative to the start), distance from the target position to obstacles, shape of obstacles, etc. This increases planning time and makes it sometimes unpredictable.

However, it is the global planner that can guaranteed find the shortest and kinematically feasible path on the map (if it exists for your robot) by enumerating all trajectory options. In ROS NAV2, the fastest global planner is State Lattice (state lattice). This is an A* family algorithm that iterates over a prepared set of primitives generated in advance by a script. Primitives are arcs, turns that are, so to speak, overlaid on the traversability map, going from the cell with the rover's current position to neighboring cells and sometimes through 1-2 cells ahead:

By combining it with a kinematic feasibility check of the move (to reduce body rotation), the global planner builds a path across the passability map (costmap). As a result of its work, the global planner returns the shortest path—a trajectory that the controller should follow. The controller's task is to transform the path into clear control commands for the motors or another lower-level controller. Thus, the controller can be of any type, for example, a simple geometric calculation of the direction to a point (azimuth calculation) that determines the course for a steering servo, more complex path-following algorithms like LOS/PLOS (LOS with Markov chain, analogous to Pure Pursuit and Vector Pursuit in ROS NAV2), or a local planner could play its role.

The local planner, unlike the global one, operates very quickly:

It calculates thousands of trajectories in parallel on the GPU, computes the control signal for the motors or a lower-level (reactive) controller, but very often cannot find a path to the goal:

the local planner cannot find a path to the target position (marked by the same pickup) through the forbidden (purple) zone on the passability map and is stuck in a trap.

Especially MPPI-Generic, thanks to massive parallelism on GPU unlike the CPU implementation of MPPI ROS NAV2, allows for fast operation even with heavy neural network models for vehicle motion such as AutoRally, balancing at the limit of tire-road adhesion capability:

Specifically, for such fast and aggressive driving, additional training on the track and creation of tire-road adhesion maps are required, which necessitates multiple passes. Building such maps is a separate complex task; for their on-the-fly construction, one needs to train a neural network model or compile it while driving and repeatedly traversing the same route, saving and accumulating current states from the model. For outdoor environments, implementing such a complex solution quickly and reliably will be very difficult. Therefore, I decided to make a controller that stabilizes angular velocity using a gyroscope. It won't be able to accelerate the rover to extreme speeds and balance at the limit of adhesion. But thanks to the fast response of the gyroscope in an emergency situation, it will help catch the vehicle and allow driving fast enough even with a low update frequency of the setpoint.

I also decided to test MPPI-Generic without combining both planning approaches (global + local planner) as is usually accepted. Because such a solution has its drawbacks, it increases planning time, creating a delay in control and loading the CPU, which already has many tasks. To make the global planner (for example, State Lattice) find a path faster, it first builds a simple rough path using A* 2D, then uses this path as a distance heuristic (to iterate over fewer primitives and make fewer steps on the map), and then already builds a kinematically feasible path. The local planner will be the third in terms of re-planning of the same route, all this time adds up to the total delay:

As an example — the planning time for a random route in the State Lattice Planner:

In this screenshot, the shortest path to the goal (red dot) through obstacles (black) and the safe zone/inflation layer around them (blue) is marked in green, purple expansions i.e. cells and path options checked by the algorithm

Therefore, there is a desire to use only one universal controller — as fast as possible.

To test MPPI-Generic as a universal planner, I decided to conduct tests in a custom simulator creating an environment similar to outdoor with large open spaces:

To begin with, let's discuss how planning works in the simulator. There are two planners here: State Lattice Planner and MPPI-Generic. Currently, they operate in parallel, meaning both receive the target coordinates and search for a path to it.

The machine is controlled through a low-level (reactive) controller that accepts angular and linear velocity setpoints (w_cmd and v_cmd). Such control can be implemented on a real rover with a microcontroller, gyroscope, and encoder-driven wheels on board.

The controller is equipped with fuses that limit the wheel turning angle at high speeds depending on the centrifugal force. If the centrifugal force exceeds a threshold, the controller reduces the linear velocity setpoint so that the angular velocity of the body matches the setpoint exactly. In other words, the controller prioritizes angular velocity over linear velocity.

This is a necessary limitation: if it is not introduced, the opposite will happen - the linear velocity will be prioritized over the angular velocity, and the machine will either be unable to fit into the trajectory (and crash, but at full speed!), or simply flip over due to a sharp turn of the steering wheels at speed. A similar controller is implemented in ROS NAV2 Regulated Pure Pursuit (RPP) - but it requires odometry and a trajectory from a global planner, as well as in Ardupilot (acro mode / turn rate mode for rovers) - where it usually works through GPS.

In my controller implementation, control is built through drive wheel encoders. This allows the MCU firmware to do without a complex EKF filter — a simple Mahony filter is sufficient to obtain data on the body's angular velocity (yaw) from the gyroscope and tilt compensation, as well as data on drive wheel speeds from encoders. Even if the wheels slip into skidding, this will not cause the algorithm to diverge. On the contrary, due to excessive speed, the steering wheel angle will be more strongly limited. With a sudden return of traction, this will prevent overturning; in other cases, body inertia will compensate for skidding.

At the same time, except for the limitation on wheel slip (centrifugal force), the controller does not limit anything else. If the local planner sends commands (w_cmd and v_cmd) to the MCU in accordance with this simple rule, the limitations will never be applied and will not interfere with rover control, except in case of an error.

This control scheme somewhat resembles ESP/ESC (Electronic Stability Program) but controlling the steering wheel instead of brakes. The local planner/controller running on Jetson/miniPC steers the vehicle body's turning speed via setpoints w_cmd and v_cmd, which sort of hides the chassis under a layer of abstraction; the controller takes on all imperfections of body geometry and road, and also handles (once, unlike the angular position controller) sudden jerks from wheel impacts when running over small obstacles — directly on the MCU.

The controller can operate at a high frequency (100 - 400 Hz) with minimal possible delays (based on IMU interrupts and DMA), does not require building complex wheel-ground adhesion maps on the go, allows the local and global planner to work at a low lidar frequency (10-20 Hz). This also provides greater time intervals for long-term planning. For greater realism in the simulation, the controller's current measurements of angular velocity and encoder speed are noised (random walk + white noise + drift, encoders white noise), which better meets outdoor requirements.

The global State Lattice planner in the simulator currently works in conjunction with State Lattice + Regulated Pure Pursuit Controller; this is a simple geometric controller that allows State Lattice to follow the route by "chasing" a marker located ahead on the path to align along the route line. It resembles PLOS, in particular, as it seems to me, it inherited its sensitivity to noise and even not-so-significant route reconfigurations from State Lattice, but it performs its job.

The global planner's trajectory is marked in green, and blue lines indicate verified cells (expansions); their direction corresponds to the direction of primitives.

The local planner in the simulator also displays trajectories, more precisely a whole set of trajectories: the optimal trajectory with zero noise is shown in blue, and the red ones represent a random sample of 1024 perturbations around the current optimal control (blue trajectory). MPPI generates noise with zero mean relative to the previous solution, so all trajectories "swirl" around the blue one. This is a property of the local optimizer: the planner assumes the previous solution was good and searches for improvements nearby. There is a downside to this: if the robot is stuck, the red trajectories won't get it out of the trap—a global planner or additional heuristic is needed:

Unlike the global one, the local planner can control the machine via a controller, providing it with setpoints w_cmd and v_cmd in real time.

To demonstrate the operation of all 3 controllers visually, I made a video showing everything as it is; the task is simply to drive the route without accidents, getting stuck in difficult spots—in short, to show reliable and stable operation along the entire route.

The first video shows a "reference" traversal of the track in manual mode by setting setpoints for the low-level controller from the keyboard; this also demonstrates the capabilities of controlling the machine via a low-level controller on the MCU:

The route was completed at full speed without major issues, despite the fact that the steering wheels in the car turn quite slowly + the setpoint changes smoothly via an integrator for greater precision, which does not allow it to be set instantaneously to the required position, creating a lag for control + generally low data input frequency, but control through the controller forgives this.

In the 2nd video, the route is traversed by a combination, State Lattice + Regulated Pure Pursuit Controller, at twice the lower speed for better controllability, as I already wrote, the goal is to ensure that the controllers operate stably:

Due to the twice lower speed in the setpoint, the controller could not climb a steep hill. However, otherwise the route was traversed well. Among the shortcomings, it is worth noting that the State Lattice route is not always optimal for the car. Sometimes the planner goes its own way until it hits an obstacle, and then bends sharply. When approaching the target, it does not build the route in advance, but makes sharp bends at the last moment. This provokes the RPP into jerks and creates oscillations, which forces State Lattice to rebuild the route — a kind of resonance arises. At moments when the route goes smoothly without unnecessary bends, RPP minimally corrects the setpoint.

In this video, MPPI-Generic traverses the route with a simple model (not on neural networks like in autorally but rather an analogue of MPPI ROS NAV2 with minor modifications) and a cost function guiding it to the target position (not along the route of the global planner like MPPI from ROS NAV2), i.e., the local planner is used as a universal planner, the video is accelerated approximately 2 times:

The MPPI speed here is less because, unlike RPP, it adjusts it itself. Achieving a balance so that MPPI drives fast everywhere is quite difficult, and I wasn't able to do it from my first acquaintance with this library. In general, "configuring" MPPI-Generic, if this process can be called that (you have to constantly invent something in the model, program rather than just turning parameter knobs) is complex, and the algorithm is very capricious, but sometimes in various combinations (including incorrect ones) it does some quite interesting things with the car model.

In one of the tests, I left the simulation running for 2 hours and went to lunch. During that time, the faulty model (with coordinate system offsets), using a multitude of micropulses in place, perfectly matched its target. It seemed that the machine was adapting to its errors — unlike simple controllers, MPPI handles some problems and thereby masks them. Unlike simple controllers, which immediately show with their appearance if something is wrong. In this test, MPPI brushed against obstacles a couple of times, despite the inflation layer, and at the end parked the car poorly, somehow not using reverse at all, preferring to drive in circles, even though it knows how to. From run to run of the simulation, MPPI behaves slightly differently). Nevertheless, it completed the entire route by itself without a global planner under equal conditions and handled almost everything, except for the case of driving onto a hill where the State Lattice + Regulated Pure Pursuit Controller combination also failed, although it was driving faster, which in my opinion makes MPPI-Generic the closest candidate for the role of a universal massively parallel planner.

In the future, I do not plan to improve MPPI-Generic towards a universal planner — it is very difficult to configure. I simply want to add 10 critics from mppi_nav2 to it and make a combination of State Lattice + MPPI-Generic, possibly compare them with State Lattice + mppi_nav2. Let's see which one ends up working better. However, even now MPPI-Generic works quickly with a large planning horizon and 1024 samples (although this is far from the limit) on the GPU, thereby offloading the CPU for more priority tasks since almost all of ROS/ROS2 runs on CPU nodes and ROS NAV2 consists of code on CPU. In the future, I plan to port the controller code from the simulator to a homemade rover made from an ATW-800 quadcopter: