quaternion_pd_controller
Written by Jaehyeong Hwang
explaining theory behind quaternion_pd_controller.cpp
First written on 19.10.2020
m_r_B(r_B) : the center of buoyancy, expressed in body frame
m_W(W) : the weight of ROV
m_B(B) : the Buoyancy of ROV
m_c : the orientation gain. It will be used in proportional gain matrix
m_c_i : the orientation integral gain. It will be used in integral gain matrix.
m_K_d : the derivative gain matrix
m_K_x : the position error gain matrix
m_K_i : the integral gain matrix
m_K_d, m_K_i will be used calculation of final pid gain.
Rotation matrix R converts from inertial frame to body frame. In quaternion parameters, rotation matrix is like this.
returns restoringForceVector(R), which is basically restoring force vector g. In underwater, gravitational force and buoyancy force acts through center of gravity and center of buoyancy for each. It is calculated in line 110 ~ 115 and described in the book of Fossen, .
Integral windup is a phenomenon that can be happened when large changes in setpoint occurs and the integral term accumulates a significant error during the rise (windup), thus overshooting and continuing to increase as this accumulated error is unwound(Wikipedia). Integrator anti-windup should be implemented in order to avoid that the integrator integrates up beyond the saturation limits of the actuators. maxPoseGain is maximum position gain value to saturate position. maxAttGain is to saturate attitude. Integral is the integral gain matrix which is multiplication between integral gain matrix K_i and error vector z.
Lyapunov function is used to decide if the control system is stable or not. Lyapunov function, the sum of potential and kinetic energy was used for this PID controller.
By applying Lasalle's invariant theorem the controller achieves LAS(Local Asymptotically Stable) state.
Basically, this code is based on nonlinear pd quaternion controller, from the thesis of Fossen, Fjellstad, O.-E. and T. I. Fossen (1994). Quaternion Feedback Regulation of Underwater Vehicles, Proceedings
of the 3rd IEEE Conference on Control Applications (CCA'94), Glasgow, August 24-26, 1994, pp. 857-862
https://www.fossen.biz/TTK4190/papers/FjellstadFossen%20CCA94.pdf
however Kristoffer Rakstad Solberg who wrote this code added integral action to diminish disturbance caused by wind and waves. So the final PID controller looks like this.
Let’s look into the error vector z which is described in line from 97 to 108.
Proportional gain matrix is described in this line.
Integral gain matrix is described in this line.
Integral windup prevention function is described. The code is explained in line 32 ~ 62
The purpose of attitude control is to make current state reach to the desired state. The body frame should conincide with the desired frame. It can be expressed as
Quaternion error is given by the product of desired state \bar{q_d} and current state q. error_body is difference between current position x and desired position x_d. errorVector function will return a 6*1 vector consisted of error of position and error of attitudes.
sgn function returns the sign of value.
circleOfAcceptance function returns acceptable distance between current position and desired position which is lower than pre-defined value R. A new setpoint is released whenever the vehicle is within the boundaries of the sphere of acceptance
Tustin's method was used for transform continuous time system to discrete time system in reference model. I won't go into too much in this theory. But by choosing the damping ratio as 1.0 and natural frequency as 0.1 we can get a critically damped and rapid reference model. By using Matlab, we can get the transfer function as
As we can see in the code the reference model can be calculated.