An Amplitude-Phase Representation of the North-East-Down Kinematic Differential Equations

The overall goal of the paper is to derive a kinematic model for vehicle path-following systems, where the pitch and yaw angles $(\theta, \psi)$ together with the surge velocity $u$ can be treated as control inputs. Then the outer control loop can be designed as a nonlinear path-following guidance law. At the same time, the inner control loops can be stabilized using commercial autopilot systems to control the pitch and yaw angles. The main result of the paper is a novel kinematic amplitude-phase representation of the North-East-Down (NED) positional rates, which can replace the classical Euler angle rotation matrix representation. The proposed kinematic model gives equivalent NED positional rates as the rotation matrix representation for all combinations of the Euler angles and the linear velocities. The main advantage of the kinematic amplitude-phase representation is the design of vehicle guidance laws using nonlinear control theory and Lyapunov stability analysis. Furthermore, it is shown that nonlinear guidance laws can be designed for path following and that the proposed methods guarantee that the origins are uniformly semiglobally exponentially stable (USGES). A case study of an unmanned surface vehicle (USV) demonstrates how a path-following control system can be designed using the amplitude-phase representation.


I. INTRODUCTION
Te purpose of this paper is to introduce a new kinematic model for the North-East-Down (NED) positional rates, which can be used to design intuitive and efficient control systems for path following and path tracking. The proposed amplitude-phase representation is an alternative to existing models for guidance-based path following in 3-D space parametrized by rotation matrices on SO(3), see [1]. The amplitude-phase representation of the NED kinematic differential equations uses the pitch and yaw angles (θ, ψ), and the surge velocity u as control inputs, see Figure 1. Pitch and yaw angle commands can be implemented using standard aircraft and marine craft autopilot systems, see [2] and [3], while a speed controller can stabilize the surge velocity.
The associate editor coordinating the review of this manuscript and approving it for publication was Xiwang Dong.
The autopilot systems are recognized as the inner control loops in Figure 1, while the path-following guidance laws represent the outer control loops. The structural properties of the kinematic amplitude-phase equations are exploited when designing the guidance laws [3]. Lyapunov stability analysis guarantees that the line-of-sight (LOS) guidance laws for path following will render the origin of the outer control loop uniformly semiglobally exponentially stable (USGES) [4].
The main result of the paper is a novel kinematic amplitude-phase representation of the NED positional rates, which can replace the classical Euler angle rotation matrix representation in guidance, navigation, and control applications. The main advantage of the kinematic amplitude-phase representation and its control inputs is simplicity when designing and analyzing the stability of vehicle LOS path-following control systems, as shown in Figure 1.
The rest of the paper is organized as follows. Section II presents the classical kinematic equations of a moving vehicle using Euler angle rotation matrices. Theorem 1 and Corollaries 1 and 2 summarize the amplitude-phase representation of the NED kinematic differential equations. In Section III, guidance laws for path following are presented, and the effectiveness of the guidance scheme is demonstrated by simulating an unmanned surface vehicle (USV) exposed to ocean currents. The concluding remarks are drawn in Section IV.

II. KINEMATICS
For marine craft and aircraft, the six different motion components in the BODY frame {b} are conveniently defined as surge, sway, heave, roll, pitch and yaw. The NED reference frame is denoted by {n}. Consequently, the kinematic differential equations are given by [3] whereṗ n = [ẋ n ,ẏ n ,ż n ] ⊤ denotes the positional rates and R n b is the rotation matrix from {b} to {n} defined by where s · = sin(·) and c · = cos(·). It is customary to describe R n b := R z,ψ R y,θ R x,φ by three principal rotations about the z, y and x axes (zyx convention) where φ, θ and ψ denote the Euler angles. Expanding (1) yields the positional rates in component forṁ

B. AMPLITUDE-PHASE REPRESENTATION OF THE KINEMATIC DIFFERENTIAL EQUATIONS
This section presents a novel kinematic representation of the NED positional rates expressed by amplitudes and phase angles for coupled motions in 6 DOFs. As the rotation matrix representation (1), the proposed kinematic model gives equivalent NED positional rates for all combinations of the Euler angles and linear velocities. Previous work by [12] and [13] do not include the couplings between the roll, pitch, and yaw angles. Moreover, the roll dynamics has been neglected such that a 5-DOF simplified model could be derived under the assumption that φ ≡ 0. In addition, a positive surge velocity u > 0 has been assumed. Theorem 1 below is the paper's main result, and it removes the previously discussed assumptions. Theorem 1 (Kinematic Amplitude-Phase Representation): The kinematic differential equations (3)-(5) for positional rates can be expressed in 3-D amplitude-phase form according toẋ The phase angles (α c , β c ) and amplitudes (U h , U v ) are (12) where the inverse tangent function, 1 Proof: See the Appendix. For aircraft [2] and marine craft [3] moving with positive surge velocity u > 0, Corollaries 1 and 2 below can be applied.
Corollary 1 (6-DOF Model for Flying vehicles): A flying vehicle (under water and in air) with positive surge velocity u > 0 is a special case of Theorem 1 wherė for 0 < U h and 0 < u ≤ U v . where is the course over ground (COG) and is the speed over ground (SOG). The phase angle β c is recognized as the crab angle. A vehicle with nonzero β c is said to sideslip. For the vertical motion (15) reduces tȯ where U v = (u 2 + w 2 ) 1/2 is the vertical-plane speed and is the flight-path angle.

III. APPLICATIONS TO GUIDANCE AND CONTROL
The main advantage of the amplitude-phase representation (6)- (8) is that the pitch and yaw angles (θ, ψ) can be treated as control inputs for the cross-and vertical-track errors, respectively during path following, see Figure 1. Consider a 2-D straight-line segment in the horizontal plane specified by two waypoints (x n i , y n i ) and (x n i+1 , y n i+1 ) expressed in {n}. Let the origin of the path-tangential coordinate system {p} be located at (x n i , y n i ) such that the x p -axis is pointing towards the next waypoint (x n i+1 , y n i+1 ). The along-and cross-track errors (x where and Time differentiation of (28) and Application of Corollary 1 yieldsẋ Note that the vertical position error z n e = z n −z n d , where z n d is the constant desired depth/altitude, is expressed in {n}. The path-following control objective is to regulate the cross-track error to zero (y p e = 0) and the vertical position to the desired depth/altitude (z n = z n d ). In addition, the surge velocity u can be used to control the speed U h in (31) through (11)- (12). This is known as path tracking.

A. HORIZONTAL-PLANE LOS GUIDANCE LAW FOR PATH FOLLOWING
Path-following control systems for marine craft, aircraft, and small autonomous vehicles can be designed using LOS guidance laws. A surface vehicle is usually controlled using a heading or a course autopilot. The cross-track error can be regulated to zero (y p e = 0) by designing a course autopilot such that χ = χ d where χ d is the desired course angle. Since χ = ψ + β c , we can apply the proportional LOS guidance law Lyapunov stability theory guarantees that the equilibrium point y p e = 0 of (35) is USGES if U h > 0, see [4]. Alternatively, the vector field guidance law [16] can be applied. In some cases, it is advantageous to let the look-ahead distance h vary with time, e.g. by using optimization techniques [17] or the explicit formula by [18].
Next, consider the NTNU Autonaut shown in Figure 3, which is a long-endurance USV propelled by the motion of the waves [19]. The USV is controlled by a single rudder δ r . The yaw angle transfer function is [3] ψ δ r (s) = K s(Ts + 1) For the Autonaut, K = 0.25 s −1 and T = 3.0 s, see [21]. Hence, the North-East positions are obtained by integrating (22) and (23). Assume that the USV is exposed to stochastic ocean currents with initial speed V c = 0.2 m/s and direction β V c = 150 deg. Hence, it follows that the ocean current velocities are u c = V c cos(β V c ) and v c = V c sin(β V c ), respectively. Furthermore, we assume that the USV moves at forward speed U = 1.0 m/s such that the velocities become Since the USV dynamics is well damped, a proportionalintegral (PI) controller is used to regulate the course angle where the numerical values K p = 1.25 and K i = 0.02 are based on [21]. The function, ssa(x) = mod(x + π, 2π) − π, is the smallest-signed angle confining the argument to the interval [−π, π). Here mod(·) denotes the modulo operation or the signed remainder of a division. The closed-loop system is shown in Figure 2 where the inner control loop (41) regulates χ to χ d . Note that the guidance law (34) represents the outer control loop. Consequently, the inner control loop can be replaced by a commercial autopilot system. The guidance VOLUME 11, 2023 The desired vehicle path shown in Figure 4 consists of N straight-line segments specified by waypoints (x n i , y n i ) where i = 1, 2, . . . , N . Switching between the waypoints is an important feature when implementing straight-line path following control systems. The next waypoint (x n i+1 , y n i+1 ) is selected based on whether or not the USV lies within a circle of acceptance with radius R around (x n i+1 , y n i+1 ). In other words, if the USV's North-East positions (x n , y n ) at time t satisfy [3] the next waypoint (x n i+1 , y n i+1 ) is selected. The desired course angle and its true value are shown in Figure 5, confirming that χ ≈ χ d during path following. At the same time, the magnitude of the cross-track error is less than 5 meters. The waypoint switching algorithm causes the steps. Hence, the accuracy will be much better, typically in the centimeter range, during straight-line path following.
The LOS guidance law (34) can also be implemented using a heading autopilot which guarantees that the yaw angle ψ ≈ ψ d , where ψ d is the desired yaw angle. This approach requires knowledge of the crab angle β c . The crab angle is uncertain but nearly constant during the straight-line path following, see Figure 5. Hence, the preferred solution to this problem is to apply an integral LOS algorithm [22] to compensate for β c . Alternatively, adaptive LOS guidance laws can be used to estimate β c as shown by [14] and [23]. It is also possible to construct observers for estimation and compensation of β c , see [24] and [25].
This is based on the assumption that the inner control loop regulates γ to γ d . Again Lyapunov stability theory guarantees that the origin z n = z n d of (45) is USGES if U v > 0 [4]. Alternatively, a pitch angle command can be computed using θ = γ − α c . This requires knowledge of the signal α c , which is uncertain but nearly constant during straight-line path following. However, integral LOS can be used to overcome this problem [3].

IV. CONCLUSION
This paper has presented a novel kinematic amplitude-phase representation of the North-East-Down (NED) positional rates, which can replace the classical Euler angle rotation matrix representation in guidance, navigation, and control applications. Several authors have discussed proportional line-of-sight (LOS) guidance laws for path following using approximate amplitude-phase models where couplings to the roll motion are neglected. The results in the paper generalize this to six degrees of freedom (DOFs) coupled motions by including the roll mode. The proposed kinematic model has three control inputs; the pitch and yaw angles (θ, ψ), and the surge velocity u. The main advantage of the kinematic amplitude-phase representation and its control inputs is simplicity when designing and analyzing the stability of vehicle LOS path-following control systems. A unmanned surface vehicle (USV) case study demonstrated how a path-following control system could be designed using the amplitude-phase representation.