Trajectory Tracking Using ACADO
Trajectory tracking ROS package written using ACADO-Toolkit
code: https://github.com/TianxiaoYe-Shawn/ACADO_NMPC_ROS
The following is the effect based on the ARCLab Mocap System:
The following is the effect based on the JACKAL gazebo simulator:
Build ACADO NMPC ROS package from scratch
1. Ubuntu and ROS Versions
This ROS package is built on Ubuntu 20.04 and ROS Noetic full desktop version. It is recommended to follow this version, as the JACKAL simulator is also built on it.
2. Creating a ROS Workspace
Copy the following code into the terminal to create a workspace:
1 | mkdir -p ~/NMPC_ACADO_ws/src/ |
You can rename NMPC_ACADO_ws to your preferred workspace name.
3. Downloading ROS Packages
Navigate to the /src directory in your workspace and download the package:
1 | cd ~/NMPC_ACADO_ws/src |
Move the package to the /src directory and remove other files:
1 | mv ACADO_NMPC_ROS/acado_mpc . |
4. Installing ACADO
Install dependencies:
1 | sudo apt-get install gcc g++ cmake git gnuplot doxygen graphviz |
Choose any working directory (I use ~/) to download the ACADO source code:
1 | cd |
Install:
1 | cd ACADOtoolkit |
Configure the environment variable:
1 | echo "source ~/ACADOtoolkit/build/acado_env.sh" >> ~/.bashrc |
5. Compiling ROS Packages
ACADO’s advantage is that it can generate efficient C code through a symbolic language. First, modify your own MPC model in the file symbolic_mpc.cpp inside acado_mpc/acado_export_code (it’s suggested not to modify it on the first run).
Then generate the C code package:
1 | cd ~/NMPC_ACADO_ws/src/acado_mpc/acado_export_code |
Move the generated code and build the static library:
1 | mv symbolic_mpc_export/* ../../acado_mpc_export/ |
add include folder in your ROS package:
1 | cd ~/NMPC_ACADO_ws/src/acado_mpc/ |
Compile the entire ROS package:
1 | cd ~/NMPC_ACADO_ws |
If everything is successful, your package is now fully set up. If you do not have any testing environment, the JACKAL simulator is a good choice next.
6. Setting Up JACKAL Simulator
Install the JACKAL simulator with one command:
1 | sudo apt-get install ros-noetic-jackal-simulator ros-noetic-jackal-desktop ros-noetic-jackal-navigation |
Modify the simulator environment, we need a spacious area:
1 | cd /opt/ros/noetic/share/jackal_gazebo/launch/ |
Press i to enter edit mode.
Change the line <arg name="world_name..." to <arg name="world_name" default="$(find gazebo_ros)/launch/empty_world.launch" />
Then press ESC, type :wq to save and exit.
The JACKAL simulator is now fully set up.
7. Start Running!
(For each of the following rosrun and roslaunch, you need to open a new terminal)
First, open the JACKAL simulator:
1 | roslaunch jackal_gazebo jackal_world.launch |
You should see the JACKAL vehicle parked in Gazebo.
Then open the tracking test environment (including publishing trajectory, configuring rviz):
1 | roslaunch acado_mpc tracking_env.launch |
You should see a green circular trajectory in rviz (you can customize your trajectory in the trajectory_publisher.cpp file).
Then open the control input monitor:
1 | rosrun acado_mpc plot_control_input.py |
You should see the monitor displaying messages on two control input topics, which are static since no messages are published yet.
Configure the MPC weight parameters:
1 | roslaunch acado_mpc set_weight.launch |
Then directly ctrl+c to cancel, at this point the custom weight parameters have been passed.
Finally, in this terminal, open the MPC control node:
1 | rosrun acado_mpc mpc_node |
At this point, you should see the JACKAL vehicle start moving and following the trajectory.
Trajectory Tracking Using CasADi
Wrote an MPC controller using CasADi to control the car of the Jackal Gazebo simulator to track a circular trajectory.
code: https://github.com/wisc-arclab/JACKAL_UGV/tree/CasADi-NMPC
The MPC solution frequency is 10HZ.
The system only uses the kinematic model:
$$
\begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{bmatrix} = \begin{bmatrix} v \cos(\theta) \\ v \sin(\theta) \\ w \end{bmatrix}
$$
The MPC optimal control model is here:
$$
\begin{aligned}
\text{minimize} \quad & \sum_{k=0}^{N-1} \left( (x_k - x_{\text{ref},k})^T Q (x_k - x_{\text{ref},k}) + u_k^T R u_k \right) + (x_N - x_{\text{ref},N})^T Q (x_N - x_{\text{ref},N}) \\
\text{subject to} \quad & x_{k+1} = x_k + \Delta t \cdot f(x_k, u_k), \quad \forall k \in {0, \ldots, N-1} \\
& x_0 = x_{\text{initial}}, \\
& 0 \leq v \leq u_{\text{max}}, \\
& w_{\text{min}} \leq w \leq w_{\text{max}}.
\end{aligned}
$$
Objective of Minimization: The goal is to make the system’s behavior (its state at various points in time) closely follow a predefined desired path or set of states, known as the reference trajectory. This involves:
- Reducing State Errors: The system tries to keep its states (like position, velocity) as close as possible to these reference states at each step of the prediction. The closer the system’s actual state is to the reference, the better the performance.
- Controlling Input Usage: At the same time, the system seeks to use its control inputs (like thrust or steering angle) efficiently. High input usage can mean higher energy consumption and wear and tear, which the system tries to minimize.
Adding a Final State Check: At the end of the prediction horizon, there’s an additional emphasis on making sure the system’s final state is as close as possible to the final desired state. This helps in ensuring that the system not only performs well throughout its operation but also ends in a state that’s optimal for future actions.
Following Dynamics: The system must adhere to its natural dynamics, which describe how its states evolve over time based on the inputs applied. This ensures that the predictions and control actions are realistic and feasible given the physical nature of the system.
Starting From Current State: The optimization begins from the current, actual state of the system. This makes the whole control process relevant and grounded in the system’s real-world situation at the start of control.
Respecting Input Limits: The control inputs are kept within practical limits. For instance, the system cannot accelerate or rotate beyond what it’s physically capable of, ensuring safety and avoiding damage.
Trajectory Tracking Using CppAD & Ipopt
Wrote an MPC controller using CppAD and Ipopt to solve the non-linear optimal control problem and let the car of the Jackal Gazebo simulator to track a figure-8 trajectory.
code: https://github.com/wisc-arclab/JACKAL_UGV/tree/CppAD/Ipopt-NMPC
Real world:
Simulation:
State Parameters and Control Vectors
State Parameter Vector
$$
\mathbf{x}_t = \begin{bmatrix}
x_t \\
y_t \\
\theta_t \\
v_t \\
cte_t \\
e\theta_t
\end{bmatrix}
$$
Where:
- $x_t$ and $y_t$ represent the vehicle’s position coordinates.
- $\theta_t$ is the vehicle’s orientation or heading angle.
- $v_t$ is the vehicle’s velocity.
- $cte_t$ (Cross Track Error) measures the lateral distance between the vehicle and the reference trajectory.
- $e\theta_t$ (Error in Theta) represents the orientation error relative to the trajectory’s tangent at the vehicle’s current position.
Control Vector
$$
\mathbf{u}_t = \begin{bmatrix}
\omega_t \\
a_t
\end{bmatrix}
$$
Where:
- $\omega_t$ is the angular velocity, which affects the vehicle’s heading.
- $a_t$ is the acceleration, controlling the speed increase or decrease.
These control inputs are used to predict and optimize the vehicle’s trajectory over the prediction horizon, ensuring adherence to the desired path while maintaining dynamic stability and safety.
Total Cost Function
The total cost function $J$ is defined as:
$$
J = \sum_{i=0}^{N-1} \left( w_{cte} (cte_i - ref_{cte})^2 + w_{\theta} (e\theta_i - ref_{\theta})^2 + w_v (v_i - ref_v)^2 \right) +\\ \sum_{i=0}^{N-2} \left( w_{angvel} angvel_i^2 + w_{accel} accel_i^2 \right) +\\ \sum_{i=0}^{N-3} \left( w_{angvel_d} (angvel_{i+1} - angvel_i)^2 + w_{accel_d} (accel_{i+1} - accel_i)^2 \right)
$$
Where:
- $cte_i$, $e\theta_i$, and $v_i$ represent the cross-track error, heading error, and velocity at step $i$.
- $ref_{cte}$, $ref_{\theta}$, and $ref_v$ are the target values for these states.
- $w_{cte}$, $w_{\theta}$, $w_v$, $w_{angvel}$, $w_{accel}$, $w_{angvel_d}$, and $w_{accel_d}$ are weights indicating the importance of each component.
- $angvel_i$ and $accel_i$ are the control inputs at each step for angular velocity and acceleration.
Constraint Formulations
Initial State Constraints
$$
\begin{aligned}
x[1] &= x_{start}, \\
y[1] &= y_{start}, \\
\theta[1] &= \theta_{start}, \\
v[1] &= v_{start}, \\
cte[1] &= cte_{start}, \\
e\theta[1] &= e\theta_{start}.
\end{aligned}
$$
Vehicle Dynamics and Control Constraints
$$
\begin{aligned}
x_{t+1} &= x_t + v_t \cos(\theta_t) \cdot dt, \\
y_{t+1} &= y_t + v_t \sin(\theta_t) \cdot dt, \\
\theta_{t+1} &= \theta_t + \omega_t \cdot dt, \\
v_{t+1} &= v_t + a_t \cdot dt, \\
cte_{t+1} &= cte_t + (v_t \sin(e\theta_t) \cdot dt), \\
e\theta_{t+1} &= e\theta_t - (\theta_t - \text{atan}(f’(x_t))) + \omega_t \cdot dt.
\end{aligned}
$$
where $ f’(x_t) $ represents the derivative of the polynomial trajectory fit evaluated at $ x_t $.
Actuator Limitations
To ensure smooth control actions and avoid abrupt vehicle behavior, actuator limitations are imposed:
$$
\begin{aligned}
|\omega_t| &\leq \omega_{max}, \\
|a_t| &\leq a_{max}.
\end{aligned}
$$
Actuator Change Constraints
Minimizing the change in control inputs to promote smoother transitions:
$$
\begin{aligned}
|\Delta \omega_t| &\leq \Delta \omega_{max}, \\
|\Delta a_t| &\leq \Delta a_{max}.
\end{aligned}
$$