OMMPC-Controller

• OMMPC-Controller
  • Usage
    • Prerequisites
      • Ubuntu and ROS
      • MAVROS
      • Dynamic reconfigure
      • PX4-Gazebo Simulation
      • OSQP
    • Cloning the code and compiling
    • Run the code
  • Theory and parameters
    • Reading text trajectories
    • Theory and parameters
    • Thrust Normalization
  • Acknowledgment

The TOC is automatically generated by Auto Markdown TOC.

If you feel my work has been helpful, please give it a star—thank you!

Also, the control performance is not yet optimal, so if anyone is interested in helping with debugging and parameter tuning, I would be greatly appreciated!

TODO: add parameters of rotor drag

Simulation with Gazebo and EGO-Planner-V2:

Error analysis of realworld experiment with a Fast-Drone-250 quadrotor:

Usage

Prerequisites

Ubuntu and ROS

The package has been tested on Ubuntu 20.04 and ROS noetic. The default Eigen is enough for this package to work normally.

MAVROS

Follow the guide in https://docs.px4.io/main/en/ros/mavros_installation#install-mavros.

Dynamic reconfigure

sudo apt install ros-noetic-ddynamic-reconfigure

PX4-Gazebo Simulation

First, clone the source code of PX4-Autopilot:

git clone https://github.com/PX4/PX4-Autopilot.git --recursive

If the clone process completes without any errors, proceed directly to the next step. Otherwise, you can consider the following command:

cd PX4-Autopilot/
git submodule update --init --recursive

Then run the following script to setup:

bash Tools/setup/ubuntu.sh

Reboot, and

cd PX4-Autopilot/
make px4_sitl_default gazebo

You may need to wait a few seconds for gazebo to initialize. After initialization, type commander takeoff to make the drone take off, or type commander land to make it land.

To increase the frequency of odometry, you can modify the following code in ~/PX4-Autopilot/src/modules/mavlink/mavlink_main.cpp:

configure_stream_local("LOCAL_POSITION_NED", 125.0f);  // originally 30.0f
configure_stream_local("ODOMETRY", 125.0f);  // originally 30.0f

and make px4_sitl_default gazebo again.

Add environment variables to the .bashrc file:

# >>> PX4 initialize >>>
source ~/PX4-Autopilot/Tools/simulation/gazebo-classic/setup_gazebo.bash ~/PX4-Autopilot ~/PX4-Autopilot/build/px4_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4-Autopilot
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4-Autopilot/Tools/simulation/gazebo-classic/sitl_gazebo-classic
# <<< PX4 initialize <<<

OSQP

The version of cmake on Ubuntu 20.04 is a bit old and cannot compile the latest version. Therefore, it is necessary to revert to a slightly older version.

git clone --recursive -b release-0.6.3 https://github.com/osqp/osqp.git

If the clone process completes without any errors, proceed directly to the next step. Otherwise, you can consider the following command:

cd osqp
git submodule update --init --recursive

Then, create a build directory:

cd osqp
mkdir build
cd build

Next, create the Makefiles and compile:

cmake -G "Unix Makefiles" ..
cmake --build .

Finally,

cmake --build . --target install

(May require administrator privileges)

Ensure that libosqp.so is present, then update the dynamic link library cache using the ldconfig command.

Cloning the code and compiling

sudo apt install ros-noetic-ddynamic-reconfigure
cd catkin_ws/src
git clone https://github.com/OliverWu515/ommpc_controller.git
cd ..
catkin_make

Run the code

source devel/setup.bash
roslaunch px4 mavros_posix_sitl.launch
roslaunch ommpc_controller px4_example.launch

Takeoff: First, type the following command in the terminal:

rosservice call /mavros/set_mode 0 "OFFBOARD"

Then, check the takeoff_enabled box in the rqt_reconfigure window. The drone will automatically arm and take off.

Command: Check the command_or_hover box in the rqt_reconfigure window. The drone will then execute the text trajectory or polynomial trajectory.

Landing: Check the land_enabled box in the rqt_reconfigure window. The drone will automatically land and attempt to disarm. If disarming is successful, you can type the following command in the terminal:

rosservice call /mavros/set_mode 0 "MANUAL"

to return to manual control mode, or you can proceed directly to execute other commands.

Important:

The pose of Odometry is defined as forward x, left y, upward z (ENU). The nose of the aircraft points in the positive x-axis direction, and the throttle thrust direction is along the positive z-axis. They must be strictly aligned. If the coordinate frame is NED instead of ENU, the enu_frame_ parameter in the OMMPC_EXAMPLE class must be set to false!

The definition of velocity in Odometry differs from the ROS official documentation. The ROS official definition expresses velocity in the body frame, but most open-source odometry systems define velocity relative to the world frame. This controller also defaults to defining velocity in the world frame. If the velocity you are using is defined in the body frame, set vel_in_body_ to true in the init method of the OMMPC_EXAMPLE class (for example, when using Gazebo for simulation). Otherwise, set it to false (for example, when using open-source odometry for realworld experiments).

Theory and parameters

Reading text trajectories

If reading text trajectories for testing is enabled, after entering command control mode, the file stored at the path ref_txt/ref_filename will be read line by line with a fixed step size ref_txt/time_step. The format of each line is:

x-position y-position z-position x-velocity y-velocity z-velocity yaw yaw-rate

In the traj folder, several scripts are stored for generating text trajectories as needed.
Note: The text trajectory must not contain empty lines at the end, otherwise a memory error may occur and lead to a crash!

Related parameters:
ref_txt:
  enable: Allows the use of text trajectories. If set to false, commands from the planner are received instead.
  time_step: The sampling step size for the text trajectory. Special attention: The step size for MPC and the text reference trajectory must be consistent. This requires ensuring that this parameter, MPC_params/step_T (the MPC step size), and the step in the script generating the text trajectory are the same!
  ref_filename: The path to the text trajectory file, specified as a relative path relative to this project.

Theory and parameters

For the derivation with respect to the dynamic of quadrotors, refer to the file OMMPC_derivation.pdf in this repo.

For more general conclusions, refer to G. Lu, W. Xu and F. Zhang, "On-Manifold Model Predictive Control for Trajectory Tracking on Robotic Systems," in IEEE Transactions on Industrial Electronics, vol. 70, no. 9, pp. 9192-9202, Sept. 2023.

There are three ways to set the reference trajectory and control inputs:

• Set a hover reference. Related function: setHoverReference. This essentially sets the reference pose for the next several steps to the current pose and the velocity to 0.
• Set the reference from a text trajectory. Related functions: setTextReference, get_txt_des. This function will also be used while setting references during takeoff and landing.
• Set the reference from a polynomial trajectory. Related functions: setTrajectoryReference, feed_from_traj_utils. If it is necessary to read polynomial trajectories in other formats, you can implement them yourself.

We adopt Hopf fibration to calculate the full state from the flat outputs.

Related parameters:
use_fix_yaw: Whether to fix the yaw angle
use_trajectory_ending_pos: Whether to ensure reaching the trajectory endpoint, generally set to true
MPC_params:
  step_T: MPC step size. When using text trajectories, note that the text trajectory step size must be consistent with this.
  Q_pos_xy, Q_pos_z: Weight for horizontal and vertical error, respectively. Should be set 1-3 orders of magnitude larger than other weights.
  Q_attitude_rp, Q_attitude_yaw: Attitude error weight.
  Q_velocity: Velocity error weight.
  R_thrust: Thrust input penalty.
  R_pitchroll, R_yaw: Angular velocity input penalty. These penalties should be relatively smaller, otherwise they affect control accuracy; but cannot be too small, otherwise the smoothness will be affected.
  state_cost_exponential, input_cost_exponential: State/input error weight discount factor. The weight at step k is multiplied by exp{-k/total_steps * discount_factor}.
  max_bodyrate_xy, max_bodyrate_z: Maximum angular rate. It is recommended to set the z-component relatively smaller.
  min_thrust/max_thrust: Minimum/Maximum thrust (actually acceleration, unit: m/s^2)

Thrust Normalization

Assume that

$$ t_{cmd}=\frac{a_{z,d}}{T_a} $$

where $T_a$ is the normalization constant, which is determined by the physical characteristics of the quadrotor, and can be estimated by Kalman filtering

$$ \begin{aligned} x_k&=T_{a,k} \\ z_k&=a_z=t_{cmd}\ T_{a,k} \end{aligned} $$

Then

$$ \begin{aligned} \breve{P}_k&=1/\rho \\ K_k&=\frac{\breve{P}_k\cdot t_{cmd}}{t_{cmd} \breve{P}_k\cdot t_{cmd}+\rho} \\ \hat{T}_{a,k}&=\breve{T}_{a,k}+K_k(a_{z,imu}-t_{cmd} \breve{T}_{a,k}) \\ P_k&=(1-K_k\cdot t_{cmd})\cdot \breve{P}_k \\ \end{aligned} $$

Related parameters
hover_percentage: Used to set the initial value of Ta

Acknowledgment

• Huizhe Li and Yuhao Fang for instruction of parameter tuning and basic structures of code
• Autotrans and RPG MPC for providing examples of setting reference for MPC
• SUPER for providing supporting material including derivation of OMMPC
• GCOPTER and SE(3) Controller for providing hints of differential flatness with Hopf fibration
• Fast-Drone-250 for providing examples of using Finite State Machine (FSM) to manage states of quadrotors