Skip to content

Tuning Adaption

Jump to bottom
Philipp Föhn edited this page Apr 8, 2019 · 2 revisions

Tuning

Our implementation allows to be tuned by using the ROS parameters in the parameter file /parameters/default.yaml. State costs are denoted with Q_, while input/actuation costs are denoted with R_. All inputs are bounded and their bounds are respected in the MPC over the full lookahead horizon. The thrust input is bounded by the positive bounds min_thrust <= thrust <= max_thrust, while the bodyrates are bounded by their absolute bound |bodyrate_xy| <= max_bodyrate_xy for pitch/roll and separately for yaw around z.

For the perception cost, the parameters p_B_C = [x, y, z] and q_B_C = [w, x, y, z] represent the position and quaternion rotation of the camera with respect to the body in the body frame.

Additionally, the cost on states and inputs in the lookahead horizon can be scaled exponentially to lower the weight on states further in the future. It is implemented as a exponential weighting W_i = W * exp(-i/N * cost_scaling) for the i-th state on the horizon (i=0 for actual state, i=20 for the last/furthest state). It can be set individually for states (state_cost_exponential) and inputs (input_cost_exponential).

Rule of Thumb for MPC tuning

First of all, boundaries and camera-to-body calibration should be set according to your platform and should not be changed for tuning reasons.

The MPC is tuned by setting the weights on quadratic state/input deviations/errors. These weights result in relative trade-off. Example: if the weight on position is 4 times higher than the weight on velocity, it means that reducing the error in position is twice as important as reducing an error of equal magnitude in velocity.

A good starting point to tune a controller is to focus on the expected state error and input energy cost of the model. This is neither a scientific approach nor guarantees stability, but it should serve as a starting point for tuning in simulation or on a secured (save and crash-protected) real system.

Since the perception cost is an additional factor, it should be set to 0 for initial tuning. The initial weight can be set as the inverse to the variance of a state, or as a rule of thumb: Set the weight on a state to the inverse of the quadratic tolerated error.

Example: A positional error of 0.1m is tolerated, velocity is not important, so we tolerate 0.3m/s. This results in Q_pos = 1/(0.1^2) = 100 and Q_vel = 1/(0.3^2) ~= 10. Due to the dynamic model of a quadrotor, the orientation is coupled to the translational behaviour, so we have to treat it accordingly. We don't want an aggressive in this example so we expect a quaternion deviation of magnitude 0.2 (roughly ~20 degrees) with respect to the defined position error, resulting in Q_ori = 1/(0.2^2) = 25.

For the inputs we can do the same and estimate a thrust deviation of 1 m/s^2 and bodyrate deviation of 0.3 rad/s leading to R_thrust = 1, R_pitchroll = 10, R_yaw = 10.

At this point we could simulate the system or try it on a safe test bench system. One would intuitively increase positional cost until the tracking is accurate and use velocity cost to damp oscillation behaviour. The cost on orientation would intuitively be used to restrict big deviations from the setpoint (such as hover orientation), while the costs on inputs can be used to tune the "aggressiveness" of the system.

Model Adaption

The description of the underlying model resides in the folder /model. To adapt the model, the ACADO framework must be installed. The model is described in /model/quadrotor_model_thrustrates.cpp. After adapting the description in this file, it needs to be built and executed to generate the code files used in the MPC. This will update the files in /model/quadrotor_mpc_codegen/. If your changes affect dimensionality or structure of the MPC, you will also have to adapt the wrapper and controller to handle additional states, inputs or other variables.

Adaption can be done following these steps:

  1. Adapt the model as you wish
  2. Build the new model description: cd rpg_mpc/model or roscd rpg_mpc/model and then make
  3. Execute the new model description to generate the adapted files by executing ./quadrotor_model_codegen
  4. Rebuild the ROS package catkin build rpg_mpc
  5. If this fails, you likely have changed the dimensions of variables used in the MPC and need to adapt the wrapper and controller.

Wrapper and controller can be adapted by changing the concerned variables in the following files:

Clone this wiki locally