DP Adaptive Backstepping Controller — Quaternion Formulation

This package implements a dynamic positioning (DP) adaptive backstepping controller for the Nautilus AUV. It uses a unit-quaternion error-state formulation for the attitude kinematics, which eliminates the gimbal-lock singularity that occurs at ±90° pitch in the Euler-angle version. The adaptive terms estimate the linear and quadratic damping online and compensate for unmodelled disturbances, in the same spirit as a model reference adaptive controller. Stability and convergence are proven via a composite Lyapunov function.

Overview

• Uses the backstepping control method for position and orientation control
• Replaces the Euler-angle Jacobian $J(\eta)$ with the quaternion error-state Jacobian $J_e(\eta)$ (called L in the code), which is square, smooth, and invertible for all attitude errors smaller than 180°
• Includes adaptive terms to estimate linear and nonlinear (quadratic) damping and external disturbances

Model for AUV

Kinematics (error-state form):

\dot{z}_1 = J_e(\eta)\,\nu

where the error state $z_1 \in \mathbb{R}^6$ is defined below, and the error-state Jacobian is:

J_e(\eta) =
\begin{bmatrix}
R(q) & 0_{3\times 3} \\
0_{3\times 3} & T_e(q_e)
\end{bmatrix}, \quad
T_e(q_e) = \eta_e I_3 + S(\varepsilon_e)

Here $R(q) \in SO(3)$ is the rotation matrix from NED to body, $q_e = q_d^* \otimes q$ is the error quaternion (scalar part $\eta_e$, vector part $\varepsilon_e$), and $S(\cdot)$ is the skew-symmetric (cross-product) matrix.

Dynamics (Newton–Euler, body frame):

M\dot{\nu} + C(\nu)\,\nu - F(\nu,\Theta^\star) = \tau + d^\star

• $\nu \in \mathbb{R}^6$: body-fixed velocity (linear and angular)
• $M$: constant, symmetric, positive-definite mass-inertia matrix
• $C(\nu)$: Coriolis and centripetal matrix
• $F(\nu, \Theta^\star) = Y(\nu)\Theta^\star$: damping in regressor form (linear and quadratic per DOF)
• $\tau$: control wrench
• $d^\star$: lumped disturbances and unmodelled dynamics

Notation note: The symbol $\eta_e$ denotes the scalar part of the error quaternion (not the pose vector $\eta$); this follows Fossen’s standard overloading.

File overview

1. dp_adapt_backs_controller.cpp/hpp
   • Core controller implementation: computes L, L_inv, L_dot, the error state $z_1$, $z_2$, $\alpha$, $\dot\alpha$, and the full control wrench $\tau$.
   • Integrates the adaptive parameters online.
2. dp_adapt_backs_controller_utils.cpp/hpp
   • Utility functions: calculate_L_inv, calculate_R_dot, calculate_Q_dot, calculate_L_dot, calculate_coriolis, calculate_Y_v.
3. dp_adapt_backs_controller_ros.cpp/hpp
   • ROS 2 node wrapper: subscribes to odometry, killswitch, and reference topics; publishes thrust commands.
4. adapt_params_nautilus.yaml / adapt_params_nautilus_sim.yaml
   • Tunable controller parameters (K1, K2, adapt_gain, d_gain, r_b_bg, time_step).
5. CMakeLists.txt
   • Build configuration, ROS 2 dependencies, executable generation, and installation setup.

Tuning Parameters

• K1: Outer loop gain matrix (position and orientation errors $z_1$).
• K2: Inner loop gain matrix (velocity error $z_2$).
• adapt_gain: Diagonal adaptation rate for the 12 damping parameters ($\Gamma_\theta$).
• d_gain: Diagonal adaptation rate for the 6 disturbance estimates ($\Gamma_d$).
• r_b_bg: Vector from body origin to centre of gravity (used in the Coriolis matrix).

Backstepping Controller

Error state and backstepping variables

The orientation error quaternion is formed by left-multiplication with the desired quaternion conjugate:

q_e = q_d^* \otimes q = \begin{bmatrix} \eta_e \\ \varepsilon_e \end{bmatrix}

The tracking error in $\mathbb{R}^6$ is then:

z_1 = \begin{bmatrix} p - p_d \\ 2\varepsilon_e \end{bmatrix}

The factor of 2 on the vector part of the error quaternion ensures $\dot{z}_{1,\text{ori}} = T_e(q_e)\,\omega$ and makes the LFC derivative tractable. The velocity error is:

z_2 = \nu - \alpha

where $\alpha$ is the virtual control law defined below.

Adaptive parameters

\tilde{\Theta} = \hat{\Theta} - \Theta^\star, \qquad \tilde{d} = \hat{d} - d^\star

File truncated at 100 lines see the full file