Navigation Using Model Predictive Control with Turtlebot3

I tend to watch all publications coming out of Boston Dynamics because they are really cool! In this video in particular Scott Kuindersma refers to Model Predictive Control (MPC) as the technique used to solve grasping tasks carried out by Atlas. While my the goal is not to explain MPC, I was very interested in the subject and decided create a MPC controller for Turtlebot3.

MPC

While the aim of the article is not to explain MPC I’ll try to explain the insights behind it. Let’s imagine you’re playing a board game, for example chess. Before making your move, you might think several steps ahead, considering different possible actions your opponent might take. Based on your predictions of these future scenarios, you make your current move. This is somewhat similar to how Model Predictive Control (MPC) works in the world of robotics and control systems.

Model Predictive Control is a type of control algorithm that uses a model of the system (how the robot’s state changes) to predict its future behavior and make optimal control decisions based on these predictions. Model refers to a mathematical representation of the system (Turtlebot3 in our case) that we’re trying to control. This model describes how the system behaves or reacts under different conditions or inputs. Future refers to a set period of time ahead for which we make predictions about the state of our robot. This future time period is often called the “prediction horizon” or “control horizon”. Finally, Optimal means that we will choose the best or most desirable control action to input to the robot. The optimal control action is the one that will best achieve the goals (in our case moving Turtlebot3 from one place to another) for the system while adhering to certain given constraints.

Turtlebot3 Model

By model I mean the Kinematic Model. A kinematic model is a mathematical description of the motion of a robot. It describes the relationship between the robot’s joint angles and the position and orientation of its end effector.

First we define the origin of the robot’s local coordinate frame located at position (𝑥(𝑡), 𝑦(𝑡)) in the global coordinate frame at time 𝑡. The full robot’s state is completed with the heading angle angle 𝜃(𝑡) at time 𝑡 Fig 1. The control inputs that we will use to navigate are the robot’s linear velocity 𝑣(𝑡) and angular velocity 𝑤(𝑡) at time t. We define the state vector 𝐱 ∈ ℝ_3 as 𝐱 ∶= [𝑥 𝑦 𝜃] and the control input vector 𝐮 ∈ ℝ_2 as 𝐮 ∶= [𝑣 𝑤].

Fig. 1 Turtlebot3 local frame and heading angle.

Following the previous definitions for state and control input the equation for the kinematic model is defined as shown in Fig. 2

Fig. 2 Turtlbot3 state-space form kinematic model definition.

From the previous definition we observe that f(x, u) is a nonlinear function. Meaning we will be implementing a Nonlinear MPC since we will use a nonlinear model of the robot to predict its future behavior.

Future Predictions

Using the kinematic model we can estimate future states from current state as follows:

𝑇𝑠 is a parameter that we need to set that controls the sampling time for the future predicted states. Each future predicted state can be computed as in the prediction horizon is:

Where k represents one time step 𝑡 = 𝑘𝑇𝑠 in the control horizon (how far we look into the future make make the estate estimations).

Optimization

To perform navigation under a Model Predictive Control (MPC) approach, the future positions of the robot are determined over a set time period known as the prediction horizon. An optimization process is performed to minimize a specific objective function, which enables the identification of a series of ideal control responses throughout this horizon. The objective function to minimize if define as:

Xref is the goal (final state) that we are trying to reach. Since we do not wish any control input to have influence in driving the robot Uref is set to 0. Q and R represent positive definite weighing matrices that in our initial case can be set to the identity matrix I.

Now we need to solve for the optimization problem by defining it in the following way:

While this might look scary we will leverage a sophisticated library for solving optimization problems called CasADi. I have added pointers indicating the main components of the MPC in the equation (Model and Future).

Armed with all this tooling navigating Turtlebot3 around can be summarized in the following way:

Here’s how it works:

1. Look Ahead: MPC looks at where the robot is at time t (state X(t)) and predicts future states in the horizon prediction.
2. Optimize: It then solves the optimization problem by computing a series of moves that will get the robot to where it wants to go.
3. Make a Move: At every iteration we will be closer to the goal by applying the control input u(0), first future state in the prediction horizon.
4. Repeat: We keep doing this over and over until the robot gets to its goal.

Implementation

The implementation is done as a ROS package in C++. The repository can be found here. Here is a video of the library working exactly like Atlas! No just kidding!

LIVE Demo

As with most videos from Boston Dynamics is better to see a video of the robot reaching a goal using MPC!

As can be seen from the video MPC offers great advantages over other technique, for example PID Controller. First, MPC is well-suited for controlling complex systems with multiple inputs, outputs, and constraints (we are controlling to variable here angular velocity and linear velocity). It can handle systems with non-linear dynamics, time delays, and interactions between different variables. Second, it can handle constraints on inputs, outputs, and states of the system. This is particularly useful in practical applications where constraints such as physical limits, safety constraints, or operational limits need to be enforced. It’s worth noting that something to improve in the current implementation is the fact that no obstacles are being taken into account but formulating the obstacles as part of the optimization should be easy to improve. Finally, MPC can be easily reconfigured and adapted to changing system dynamics or control objectives. It allows for real-time adjustments of control actions based on new measurements and changing conditions.

In summary, MPC while complex it’s a technique that can handle very complex tasks and formulate into a unique problem which might the reason it’s so trendy and Boston Dynamics uses it! Happy MPCing!!!

References

[1] Nonlinear MPC for Collision-Free and Deadlock-Free Navigation of
Multiple Nonholonomic Mobile Robots.

[2] CasADi.

[3] ROS.