Motion Planning with Feedback
Collaborated in a team of three to develop a motion planning system to navigate around predefined obstacles using a robust feedback controller. I learned the difficulty of non-holonomic control and the underlying technologies behind navigation and self driving cars.
Project Goals & Background
As part of an Intro to Robotics course, I collaborated with two teammates on a lab focused on Feedback and Feedforward Control. Our objective was to develop a MATLAB script that could interface with a motion capture system and control LIMO robots. The project was split into two phases: the first involved implementing a feedforward control system to follow a Dubins car path, while the second added a feedback control system using more complex paths generated by Rapidly-exploring Random Trees (RRTs).
What Are LIMOs and What Are The Challenges?
-
LIMO robots are four-wheeled vehicles that, like cars, trucks, and most ground-based vehicles, follow non-holonomic motion constraints.
-
This means they cannot move freely in every direction at any given moment. For example, they can't instantly move sideways, they can only drive forward, backward, and turn gradually by steering.
-
The most famous example of this motion constraint is parallel parking.
Limo Robot (Right) & Parallel Parking Motion (Left)
Phase 1a: Dubins Cart
To work around these motion constraints, a commonly used method is Dubins path planning. A Dubins path is a route made up of three consecutive motion options:
-
Straight (S)
-
Left turn at maximum steer (L)
-
Right turn at maximum steer (R)
There are six possible combinations (RSR, LSL, RSL, LSR, RLR, LRL). To find the best path, the algorithm calculates the total length of all six paths and then selects the shortest one. An example path may be to turn left, go straight, and finally turn right (LSR).
Sample Dubins Cart Paths
Phase 1b: Feedforward Implementation
The goal of the model is to generate control inputs {specifically, speed and turning commands) that guide the robot along a precomputed path using pure feedforward control, without relying on real-time feedback. To implement this Dubins car model on the LIMO robot, we first needed two key pieces of information:
-
Accurate Turning Radius: The Dubins model assumes the robot turns with a constant minimum radius. To determine this for the Limo, we had it drive in a complete circle while marking its path. Measuring this circle gave us the robot’s actual turning radius.
-
Known Velocity: Since Dubins paths give distances for each segment (turn or straight), we need to know the robot's speed to convert those distances into time durations. This lets us control how long the robot should drive straight or turn.
Using this information, we generated a hard-coded path for the LIMO by simply sending left, straight, or right commands for specific time durations based on the turning radius and speed. Below is a comparison of the robot’s actual trajectory with the theoretical Dubins path under pure feedforward control.
As seen, the actual path deviated from the planned trajectory. We hypothesize the following reasons which show why its important to have a feedback controller that can adjust for errors:
-
Sensitivity to initial conditions: Small errors in the Limo’s starting orientation or steering angle can lead to compounding deviations along the path.
-
Non-instantaneous steering transitions: Delays in switching between steering directions prevent the Limo from precisely following the planned path.
Actual position is derived from an Optitrack Motion Capture System
Phase 2a: Feedback Controller
Clearly feedforward results in a poor path because there are several compounding errors. This is where a feedback controller that can continuously adjust the steering angle in a closed loop system comes into play.
We decided to create a MISO (multiple input, single output) system, in which the heading (angle of the limo) and position are used to determine the steering angle.
-
Intuitively the vehicle must turn right if positioned left of the desired path (Case A)
-
Or turn right if angled counterclockwise to the desired path (Case B)
Based on the deviation/error from each state: Proportional (P), Integral (I), and Derivative (D), terms are then applied to determine the corresponding turning angle.
Absolute Positional and Heading Difference Illustration
Phase 2b: Advanced Path Generation
The Rapidly exploring Random Tree algorithm (RRT) was chosen for more advanced path planning around an obstacle. Documentation and how it works can be found here. This was chosen due to the fact that obstacles can easily be defined in an occupancy map variable. Additionally, it allows the state space to be defined in terms of Dubin's carts, meaning that the path planner accounts for the non-holonomic motion of the LIMO and the limitations associated with the turning radius.
Feedback Controller Results
Before testing more advanced paths, we re-ran the original Dubins path as a baseline for comparison. With the feedback controller enabled, the LIMO followed the path much more accurately and smoothly. It also traveled the correct distance and reached the expected final orientation.
The next three tests involve different occupancy maps, each increasing in complexity. The results were mixed: the LIMO was able to follow the simpler paths successfully, but its performance declined on more complex maps. For instance on Map 3 with frequent turns and tighter navigation, the LIMO occasionally deviated from the intended path.
Conclusion
To conclude, we learned a lot from this lab: both in terms of theory (applying PID and Dubin's path planning) and also practical skills such as troubleshooting code and the Mocap system. We were overall very satisfied by the robustness of the feedback controller