Waypoint heading controller

Basic PI-heading controller for waypoint guidance.

Collaboration diagram for Waypoint heading controller:

Author

Mats Nåvik Hval

Date

12.08.2011 MNH: Initial version.

A PI heading controller (with a yaw rate term in the rudder angle) that follows a desired heading. The heading error is \( e = \psi - \psi_d \) wrapped to (-pi, pi], with \( \psi \) the third component of StateRotation. Its integral is a state. Its rate is \( e \), except that it is 0 when the magnitude of the integral is at least pi and \( e \) has the sign of the integral: the integral stops growing but can still shrink. The outputs are

\[ \delta = \frac{s}{K_\delta}\left(-K_p e - K_d r - K_i \int e\right),\qquad \tau_z = s\left(-K_p e - K_i \int e\right) \]

with \( r \) the third component of StateOmega and \( s \) = directionparameter. The rudder angle \( \delta \) is limited to plus or minus pi/8 (22.5 degrees). The angles are in radians. When both Kp and Ki are 0 the constructor sets \( K_p = m\,\omega_n^2 \) and \( K_i = (\omega_n/10) K_p \) with

\[ \omega_n = \frac{\omega_b}{\sqrt{1 - 2\zeta^2 + \sqrt{4\zeta^4 - 4\zeta^2 + 2}}} \]

from the bandwidth \( \omega_b \) (controlBandwidth) and zeta. Units of the torque are not stated in the code.

Note

Kdelta must not be 0: the rudder angle divides by it, so 0 or a missing value is a parameter error.

Input ports

Name	Width	Description
StateRotation	3	Euler angles of the ship; the third component is the heading
StateOmega	3	Angular velocity of the ship; the third component is the yaw rate
DesiredHeading	1	Desired heading

Output ports

Name	Width	Description
RudderAngle	1	Rudder angle command in radians
headingerror	1	The heading error \( e \). Updated when the derivatives are calculated
TorqueBODY	1	Yaw torque \( \tau_z \) (the component in the body frame, as the name says; the port has size 1)

Configuration parameters

The gain names start with a capital letter, unlike Ship::HeadingPIDController.

Name	Width	Description
Kp	1	Proportional gain. Default 0
Ki	1	Integral gain. Default 0
Kd	1	Gain on the yaw rate. Default 0
Kdelta	1	Rudder coefficient that divides the rudder angle. Required, must not be 0
directionparameter	1	Factor \( s \) on the rudder angle and the torque, 1 or -1. Default 1
m	1	Ship coefficient Iz - Nrdot (Fossen 12.128). Default 0. Used when Kp and Ki are 0
d	1	Ship coefficient -Nr (Fossen 12.128). Default 0. Read but not used
controlBandwidth	1	Control bandwidth \( \omega_b \) in rad/s. Default 1. Used when Kp and Ki are 0\; then it must be finite and greater than 0
zeta	1	Relative damping used in the default-gain calculation. Default 0. Used when Kp and Ki are 0

States

Name	Width	Description
headingErrorIntegral	1	Integral of the heading error. Initial value 0