# Optimal Path-Following Guidance with Generalized Weighting Functions Based on Indirect Gauss Pseudospectral Method

Authors: Qi Chen; Xugang Wang; Jing Yang
Journal: Mathematical Problems in Engineering
 (2018)

---

## Abstract

An indirect Gauss pseudospectral method based path-following guidance law is presented in this paper. A virtual target moving along the desired path with explicitly specified speed is introduced to formulate the guidance problem. By establishing a virtual target-fixed coordinate system, the path-following guidance is transformed into a terminal guidance with impact angle constraints, which is then solved by using indirect Gauss pseudospectral method. Meanwhile, the acceleration dynamics are modeled as the first-order lag to the command. Using the receding horizon technique a closed-loop guidance law, which considers generalized weighting functions (even discontinuous) of both the states and the control cost, is derived. The accuracy and effectiveness of the proposed guidance law are validated by numerical comparisons. A STM32 Nucleo board based on the ARM Cortex-M7 processor is used to evaluate the real-time computational performance of the proposed indirect Gauss pseudospectral method. Simulations for various types of desired paths are presented to show that the proposed guidance law has better performance when compared with the existing results for pure pursuit, a nonlinear guidance law, and trajectory shaping path-following guidance and provides more degrees of freedom in path-following guidance design applications.


---

## Body

## 1. Introduction

During the last decade, there is a growing interest in unmanned aerial vehicles (UAVs) in both civilian and military applications like geological surveys, power line patrol, reconnaissance, etc. In most of these applications, the UAVs are usually required to follow a desired path accurately. The desired paths are commonly planned as straight lines or circular lines with specified constraints. To obtain a satisfactory path-following performance, a robust and efficient path-following guidance law is needed.In recent years, a variety of path-following guidance laws have been developed for UAVs. Nelson et al. [1] and Lawrence et al. [2] proposed approaches based on a notion of vector field. The vector field-based path-following approach uses vector fields to represent the desired headings to drive the UAV onto the defined path. This approach has high robustness but is complicated in construction of the vector fields and also difficult in implementation. A waypoint-based path-following law was developed by Tsourdos et al. [3], in which a number of waypoints are selected on the desired path for the vehicle to pass through. However, the results based on this approach usually have low accuracy, and, specifically, a large path-following error may occur when the path is highly curved. Another waypoint-based guidance law was proposed by Liang et al. [4] for entry vehicle. In this study, the prescribed waypoints and the expected heading angle are imposed on the vehicle as additional constraints. The proposed guidance can successfully generate a lateral trajectory that satisfies the waypoint constraint and thus guarantee that the vehicle is able to reach all the waypoints. Yang et al. [5] studied the path tracking problem for a fixed-wing unmanned aircraft using the error-regulation philosophy, in which an adaptive nonlinear model predictive controller is designed to minimize both the mean and the maximum error between the reference trajectory and the UAV, and thus provides accurate tracking performance.Virtual target-based approaches lead to another class of path-following guidance laws. The main objective of these approaches is to chase a virtual target point moving along the desired path, which is ahead of the UAV. The virtual target is initially placed at the beginning of the desired path. Once the vehicle starts to track the desired path, a virtual speed related to the vehicle’s speed and the separation between the vehicle and the virtual target is generated and imposed on the virtual target. By using the virtual speed and the curvature of the desired path, the states of the virtual target can be explicitly propagated along the desired path till the end of the engagement [6–8]. As a consequence, the position of the virtual target is always available during the entire guidance process. The line-of-sight guidance [9] and proportional navigation guidance [10] were used to drive the vehicle to chase the virtual target, which eventually drives the vehicle onto the path, and the same problem has been considered by Medagoda and Gibbens [11] using pure pursuit guidance. However, a heading error will be caused for curved paths because pure pursuit guidance consistently compels the vehicle to head toward the target. Therefore, a path-following error will occur. Further improvement to pure pursuit guidance was proposed by Cho et al. [12], in which differential geometry of space curves is used to extend the pure pursuit method for 3D path following. A nonlinear path-following guidance law adapted from pure pursuit based methods was proposed by Park et al. [13, 14]. This method is prominent due to its robustness of convergence for all initial geometries, the simple guidance command, and the so-called “look-ahead effect” which guarantees accurate following of curved paths. However, the lateral acceleration is undefined when the initial position of the vehicle is outside of the specified look-ahead distance from the desired path. Moreover, an overshoot response in the initial phase is another issue. Exploiting the concept of terminal missile guidance law with impact angle constraint, Ratnoo et al. [15] proposed a new path-following law based on trajectory shaping guidance. The advantages of this method are the fast rate of convergence, the negligible path-following error, and the strong robustness with respect to the minimum distance.However, the research works mentioned above have not considered the autopilot delay, which cannot be ignored in practice, especially for the UAVs with low control authority. Most of the existing works assume that the actual lateral acceleration is the same as the command, which means that the acceleration response is instantaneous. However, this is not practical as it always takes some time to achieve the desired guidance command in practical system. This delay may degrade the overall path-following performance and even cause instability. To obtain satisfactory path-following performance, it is of necessity to take the autopilot delay into account. In addition, previous works mentioned above generally utilize simple guidance laws to chase the virtual target and do not take the weighting functions into account to improve the path-following guidance performance. As can be shown that through appropriate selections of the weighting functions, the vehicle’s trajectory and acceleration profile can be shaped as desired for achieving different path-following objectives. Furthermore, if the weighting functions can be chosen arbitrarily, then the flexibility of the path-following guidance design could be largely enhanced. Various weighting functions, such as constant function [16], Gaussian function [17], time-to-go function [18], exponential function [19], hyperbolic tangent function [20], and sinusoidal function [21], have been used to devise terminal guidance laws for different guidance objectives. These weighting functions have their corresponding advantages, for example, reducing sensitivity with respect to initial heading error, extending the operational margin to cope with the external disturbances in the terminal phase, or alleviating the acceleration command at the initial phase. Discontinuous functions that consist of the combination of the above-mentioned weighting functions give rise to attractive and prominent types of the weighting functions, in which different weighting factors are applied to weigh the states and/or controls in different guidance phases. Consequently, discontinuous weighting functions can take advantages of several weighting functions during the entire guidance process and thus significantly enhance the guidance performance. But, meanwhile, these discontinuous weighting functions are usually intractable for most of the guidance laws in existing literatures. However, the method proposed in this paper can easily cope with these discontinuous weighting functions. The results presented in this paper are the first attempts in the literature to derive the optimal path-following guidance with generalized weighting functions using the indirect Gauss pseudospectral method. This novel approach can handle complex weighting functions (even though they are discontinuous) which are intractable for most of the guidance laws in previous studies and thus provides more degrees of freedom in path-following guidance design applications. Similar to the use of terminal missile guidance law for path following in [15], a novel guidance logic with impact angle constraint considering generalized weighting functions as well as the autopilot delay is proposed in this paper to follow the virtual target on a planar path. Detailed numerical comparisons are presented to demonstrate the high path-following performance of the proposed guidance.This paper is organized as follows. In Section2, the path-following problem based on virtual target pursuit is formulated. In Section 3, an indirect Gauss pseudospectral method based approach is derived, and then a closed-loop path-following guidance law is proposed. The validation of the Gauss pseudospectral method based approach and the performance of the proposed guidance law are presented by numerical simulations in Section 4. Conclusions are given in Section 5.


## 2. Problem Formulation

Consider the path-following guidance geometry shown in Figure1. Here,M denotes a vehicle; the curve is the desired path;T is a virtual target moving along the path and governed by the curvature of the desired path. The vehicle pursues the virtual target and reduces the distanceR to converge to the desired path.Vm, γm, and γt denote the vehicle velocity, vehicle heading angle, and the virtual target heading angle, respectively.am is the vehicle acceleration perpendicular to the velocity vector to change the heading angle γm. Motivated by [11, 15], the speed of the virtual target is chosen as a function of the vehicle speedvm and the closing distanceR as follows:(1)vt=vmR∗Rwhere R∗ is a design parameter within the guidance algorithm and represents the minimum allowed separation between the vehicle and the virtual target, because the virtual target’s speed is inversely proportional to the closing distance. Therefore, the speed of the virtual target increases as the vehicle approaches the virtual target, which makes the vehicle always in pursuit of the virtual vehicle. This constraint links the dynamics of the vehicle and the virtual target and implies that the vehicle can never be closer to the virtual target than the minimum separation R∗ (e.g., R≥R∗). The choice of R∗ and the guidance law of the vehicle affect the vehicle following performance. In order to achieve better following performance, the vehicle is expected to approach the virtual target in tail chase. To this end, the guidance law of the vehicle had better maintain a capacity of impact angle control (the expected impact angle is the virtual target heading angle γt). Trajectory shaping guidance law [16] is analyzed in [15] and is proven to be an effective logic for virtual target following on a planar path. However, when taking the autopilot dynamics into account, this method will have some limitation (for instance, oscillation and instability). Moreover, weighting functions are not considered in the previous studies. To eliminate this limitation and enhance the flexibility of the design of the path-following guidance, a new guidance law with impact angle constraint is proposed in this paper.Figure 1
Path-following guidance geometry.For convenience, a virtual target-fixed coordinateTxdyd is defined in Figure 1. The coordinate Txdyd is fixed with the virtual target and rotates with the virtual target heading angle γt from the inertial reference coordinate xIyI. The engagement kinematics and proposed guidance are derived in Txdyd. The equations of motion for the engagement defined in Txdyd are given by(2)y˙t=Vmtsin⁡γt,y0=y0γ˙t=amtVmt,γ0=γ0where y and γ are the vehicle cross-range and heading angle defined in Txdyd, respectively. Under the assumption that Vm is constant and γ is small, (2) can be linearized as(3)y˙t=Vmtγt,γ˙t=amtVmtSuppose that the vehicle autopilot model is a first-order delay system:(4)a˙mt=1tτact-amtwhere ac and tτ denote the guidance command and autopilot time delay constant, respectively. Equations (3) and (4) can be rewritten in a compact form as follows:(5)x˙t=Axt+But,x0=x0where(6)
x
=
y
γ
a
m
T
,
u
=
a
c
,
A
=
0
V
m
0
0
0
1
V
m
0
0
-
1
t
τ
,
B
=
0
0
1
t
τNote that this linearized kinematics has been widely used to devise optimal guidance laws with diverse constraints from many researchers in [17, 18, 21–23]. It should be pointed out that the existing work among these studies mainly concentrates on various weighting functions only on the control energy cost. To enhance the design flexibility, we consider the generalized weighting functions, even if they are discontinuous, on both the states and control to construct a novel design framework of path-following guidance law.It can be observed from Figure1 that if the speed vector coincides with the axisTxd, namely,y = 0 and γ = 0, then the impact angle constraint will be satisfied. Therefore, we consider the following finite-time optimal control problem P: find u(t) in the time interval [t0,tf] that minimize the cost function(7)J=12xTtfSfxtf+12∫t0tfxTtQxt+uTtRutdtsubject to (5).Here,t0 and tf are the initial and final time of flight, respectively. Sf is used to control the terminal state of the vehicle. Q and R are used to weight the state variables and the control effort and are considered as the weighting functions.


## 3. Design of the Optimal Path-Following Guidance Law

### 3.1. Optimal Solution Based on Indirect Gauss Pseudospectral Method

The Hamiltonian function of the problemP is(8)H=12xtTQxt+utTRut+λtTAxt+Butwhere λ(t) is the costate vector that satisfies the dynamics(9)λ˙t=-∂H∂xt=-Qxt+ATλtwith the transversality condition(10)λtf=SfxtfAccording to the minimum principle, the necessary optimality condition is(11)∂H∂ut=0which yields the optimal control(12)u∗t=-R-1BTλtSubstituting (12) into (5), and combining (9), the linear two-point boundary-value problem can be described by(13a)x˙t=∂H∂λt=Axt-BR-1BTλt,xt0=x0(13b)λ˙t=-∂H∂xt=-Qxt+ATλt,λtf=SfxtfFor some complex or discontinuous weighting functions, it is intractable to solve (13a) and (13b) analytically. A conventional method to solve problem ((13a), (13b)) is the backward sweep method. In this method, it is assumed that λ(t)=P(t)x(t), which is substituted into (12), where P(t) is determined by solving the differential Riccati matrix equation. It is well-known that this method needs iterations and is numerically intensive and potentially unstable. To overcome these disadvantages, a novel approach based on indirect Gauss pseudospectral method that can deal with arbitrary weighting functions is derived to tackle problem ((13a), (13b)). Because the computation interval used in the Gauss pseudospectral method is [-1,1], it is necessary to transfer the time interval [t0,tf] to [-1,1] via the following transformation:(14)τ=2tf-t0t-tf+t0tf-t0By applying (14), ((13a), (13b)) is transferred to the following form:(15a)x˙τ=tf-t02Axτ-BR-1BTλτ,x-1=x0(15b)λ˙τ=-tf-t02Qxτ+ATλτ,λ1=Sfx1Next, ((15a), (15b)) is transformed into a set of algebraic equations based on the approximated state variable x(τ) and costate variable λ(τ) using interpolating polynomials. It should be noted that the initial value is known for the state variable x(τ) in (15a), whereas the terminal value is known for the costate variable λ(τ) in (15b). Therefore, there is a slight difference between approaches used to approximate x(τ) and λ(τ). For x(τ), the boundary point, -1, and theN Gauss points [24], τk, k=1,…,N, which are all in the interior of the interval [-1,1], are used as the interpolation points to form the interpolating polynomials(16)xτ≈Xτ=x-1L0τ+∑k=1NxτkLkτwhere Li(τ) (i=0,…,N) are N+1 Lagrange interpolating polynomials and defined as(17)Liτ=∏j=0,j≠iNτ-τjτi-τjDifferentiating (16) yields(18)x˙τ≈X˙τ=x-1L˙0τ+∑k=1NxτkL˙kτDenote D-i=L˙0(τi) and Dik=L˙k(τi); (18) can be written as(19)x˙τi≈X˙τi=x-1D-i+∑k=1NxτkDikfor i=1,2,…,N, where D-∈RN and D∈RN×N are differential approximation matrices.Because the terminal value is known forλ(τ), the boundary point, 1, and theN Gauss points, τk, k=1,…,N, are used as the interpolation points to form the interpolating polynomials(20)λ˙τi≈Λ˙τi=λ1D-i∗+∑k=1NλτkDik∗for i=1,2,…,N, where D-∗∈RN and D∗∈RN×N are adjoint differential approximation matrices. According to (15b), λ(1)=Sfx(1), where x(1) is the terminal value of state and can be determined via Gauss quadrature [25]:(21)x1=x-1+tf-t02∑k=1NwkAxτk-BR-1BTλτkwherewk are Gauss weights for k=1,2,…,N. Thus the terminal value of λ is(22)λ1=Sfx-1+Sftf-t02∑k=1NwkAxτk-BR-1BTλτkSubstituting (22) into (20) and including (19), it follows that(23a)D-i∗Sfx-1+D-i∗Sftf-t02∑k=1NwkAxk-BR-1BTλk+∑k=1NDik∗λk=-tf-t02Qxi+ATλi(23b)D-ix-1+∑k=1NDikxk=tf-t02Axi-BR-1BTλiwhere i=1,2,…,N, xk≡x(τk), λk≡λ(τk) for k=1,2,…,N.As proven in [24], the differential approximation matrices D- and D, and the adjoint differential approximation matrices D-∗ and D∗ have the following relationships:(24a)D-i=-∑k=1NDik(24b)D-i∗=-∑k=1NDik∗(24c)Dik∗=-wkwiDkiIt can be seen from (24a), (24b), and (24c) that as long as the matrix D is determined via differentiating the Lagrange interpolating polynomials Lk(t), the other matrices D-, D-∗, and D∗ can be directly calculated by employing (24a), (24b), and (24c). Furthermore, the matrix D and the Gauss weights wk can be computed offline, and thus all the other matrices can be also computed offline.DenoteΛ=[λ1T,λ2T,…,λNT]T∈RnN, X=[x1T,x2T,…,xNT]T∈RnN, X0=x(-1)∈Rn. Equations (23a) and (23b) can be rearranged as(25a)D-~∗X0+D~+∗Λ+Q~X=0(25b)D-~X0+D~-X+G~Λ=0where (26)D-~∗=D-1∗SfD-2∗Sf⋮D-N∗Sf∈RnN×n,D-~=D-1InD-2In⋮D-NIn∈RnN×n,(27)D~+∗=D11∗In-tf-t02D-1∗Sfw1BR-1BT-ATD12∗In-tf-t02D-∗Sfw2BR-1BT⋯D1N∗In-tf-t02D-1∗SfwNBR-1BTD21∗In-tf-t02D-2∗Sfw1BR-1BTD22∗In-tf-t02D-2∗Sfw2BR-1BT-AT⋯D2N∗In-tf-t02D-2∗SfwNBR-1BT⋮⋮⋱⋮DN1∗In-tf-t02D-N∗Sfw1BR-1BTDN2∗In-tf-t02D-N∗Sfw2BR-1BT⋯DNN∗In-tf-t02D-N∗SfwNBR-1BT-AT∈RnN×nN(28)D~-=D11In-tf-t02AD12In⋯D1NInD21InD22In-tf-t02A⋯D2NIn⋮⋮⋱⋮DN1InDN2In⋯DNNIn-tf-t02A∈RnN×nNQ~=tf-t02D-1∗Sfw1A+Qtf-t02D-1∗Sfw2A⋯tf-t02D-1∗SfwNAtf-t02D-2∗Sfw1Atf-t02D-2∗Sfw2A+Q⋯tf-t02D-2∗SfwNA⋮⋮⋱⋮tf-t02D-N∗Sfw1Atf-t02D-N∗Sfw2A⋯tf-t02D-N∗SfwNA+Q∈RnN×nNG~=tf-t02BR-1BT0n⋯0n0ntf-t02BR-1BT⋯0n⋮⋮⋱⋮0n0n0ntf-t02BR-1BT∈RnN×nNIn the preceding matrices, In and 0n are the n×n identity and zero matrices, respectively. Next, we transform (25a) and (25b) into a concise form as(29)D-~∗D-~X0+D~+∗Q~G~D~-ΛX=0DenoteD-Aug=D-~∗D-~∈R2nN×n, DAug=D~+∗Q~G~D~-∈R2nN×2nN. It follows that(30)D-AugX0+DAugΛX=0By solving (30), the following solution to the two-point boundary-value problem of equations (15a) and (15b) is obtained:(31)ΛX=-DAug-1D-AugX0It is important to note from (31) that once the initial state value is given, the states and costates at all the Gauss points can be simultaneously calculated without any explicit integration process. However, (31) cannot provide the values of the states and costates at the boundary points because Gauss points do not include the boundary points. Since the initial value of x has been given, its terminal value can be calculated via (21), and the terminal value of λ can be calculated via (22). Regarding the initial value of λ, the following equation can be used: (32)λ-1=λ1-tf-t02∑k=1NwkQxk+ATλkNow, all the costates at the discretization points (Gauss points and the boundary points) have been obtained from (21), (22), (31), and (32); the optimal control can be thus determined via the following equation:(33)ui∗=-R-1BTλii=0,1,2,…,N+1


### 3.2. Implementation of Path-Following Guidance

Note that the final time of the flighttf is necessary in the computation of matrices D~+∗, D~-, Q~, and G~. In order to obtain an approximation of tf, the following equation is employed:(34)tf=t0+tgo≈t0+RVmAs the proposed guidance with impact angle constraint is derived in the new coordinateTxdyd, the vehicle cross-range y and heading angle γ in Txdyd are computed as(35a)y=-xm-xtsin⁡γt+ym-ytcos⁡γt(35b)γ=γm-γtwhere (xm,ym) and (xt,yt) are the coordinates of the vehicle and the virtual vehicle in the inertial coordinate OxIyI, respectively.Note that the optimal solution to the problemP described previously is open-loop. To obtain a closed-loop solution, the receding horizon technique is used in this study. Finally, the procedure for implementing the proposed guidance law with impact angle constraint for the path following can be summarized as follows.(1)
Initialize the number of Gauss pointN. Sett0=0.(2)
Solve the problemP:
(a)
Computetf from (34) and X0 from (35a) and (35b).(b)
Compute the matricesD-Aug and DAug, and solve (31).(c)
Get the optimal controlui(i=0,1,…,N+1) using (33).(3)
Apply the optimal control at the first point,u0, to the vehicle’s dynamics and update the states of the vehicle.(4)
Update the states of the virtual vehicle according to (1) and the curvature of the desired path.(5)
Updatet0 byt0+Δt (Δt is the guidance time step).(6)
Repeat steps(2)-(5) until the vehicle reaches the end of the desired path.


## 3.1. Optimal Solution Based on Indirect Gauss Pseudospectral Method

The Hamiltonian function of the problemP is(8)H=12xtTQxt+utTRut+λtTAxt+Butwhere λ(t) is the costate vector that satisfies the dynamics(9)λ˙t=-∂H∂xt=-Qxt+ATλtwith the transversality condition(10)λtf=SfxtfAccording to the minimum principle, the necessary optimality condition is(11)∂H∂ut=0which yields the optimal control(12)u∗t=-R-1BTλtSubstituting (12) into (5), and combining (9), the linear two-point boundary-value problem can be described by(13a)x˙t=∂H∂λt=Axt-BR-1BTλt,xt0=x0(13b)λ˙t=-∂H∂xt=-Qxt+ATλt,λtf=SfxtfFor some complex or discontinuous weighting functions, it is intractable to solve (13a) and (13b) analytically. A conventional method to solve problem ((13a), (13b)) is the backward sweep method. In this method, it is assumed that λ(t)=P(t)x(t), which is substituted into (12), where P(t) is determined by solving the differential Riccati matrix equation. It is well-known that this method needs iterations and is numerically intensive and potentially unstable. To overcome these disadvantages, a novel approach based on indirect Gauss pseudospectral method that can deal with arbitrary weighting functions is derived to tackle problem ((13a), (13b)). Because the computation interval used in the Gauss pseudospectral method is [-1,1], it is necessary to transfer the time interval [t0,tf] to [-1,1] via the following transformation:(14)τ=2tf-t0t-tf+t0tf-t0By applying (14), ((13a), (13b)) is transferred to the following form:(15a)x˙τ=tf-t02Axτ-BR-1BTλτ,x-1=x0(15b)λ˙τ=-tf-t02Qxτ+ATλτ,λ1=Sfx1Next, ((15a), (15b)) is transformed into a set of algebraic equations based on the approximated state variable x(τ) and costate variable λ(τ) using interpolating polynomials. It should be noted that the initial value is known for the state variable x(τ) in (15a), whereas the terminal value is known for the costate variable λ(τ) in (15b). Therefore, there is a slight difference between approaches used to approximate x(τ) and λ(τ). For x(τ), the boundary point, -1, and theN Gauss points [24], τk, k=1,…,N, which are all in the interior of the interval [-1,1], are used as the interpolation points to form the interpolating polynomials(16)xτ≈Xτ=x-1L0τ+∑k=1NxτkLkτwhere Li(τ) (i=0,…,N) are N+1 Lagrange interpolating polynomials and defined as(17)Liτ=∏j=0,j≠iNτ-τjτi-τjDifferentiating (16) yields(18)x˙τ≈X˙τ=x-1L˙0τ+∑k=1NxτkL˙kτDenote D-i=L˙0(τi) and Dik=L˙k(τi); (18) can be written as(19)x˙τi≈X˙τi=x-1D-i+∑k=1NxτkDikfor i=1,2,…,N, where D-∈RN and D∈RN×N are differential approximation matrices.Because the terminal value is known forλ(τ), the boundary point, 1, and theN Gauss points, τk, k=1,…,N, are used as the interpolation points to form the interpolating polynomials(20)λ˙τi≈Λ˙τi=λ1D-i∗+∑k=1NλτkDik∗for i=1,2,…,N, where D-∗∈RN and D∗∈RN×N are adjoint differential approximation matrices. According to (15b), λ(1)=Sfx(1), where x(1) is the terminal value of state and can be determined via Gauss quadrature [25]:(21)x1=x-1+tf-t02∑k=1NwkAxτk-BR-1BTλτkwherewk are Gauss weights for k=1,2,…,N. Thus the terminal value of λ is(22)λ1=Sfx-1+Sftf-t02∑k=1NwkAxτk-BR-1BTλτkSubstituting (22) into (20) and including (19), it follows that(23a)D-i∗Sfx-1+D-i∗Sftf-t02∑k=1NwkAxk-BR-1BTλk+∑k=1NDik∗λk=-tf-t02Qxi+ATλi(23b)D-ix-1+∑k=1NDikxk=tf-t02Axi-BR-1BTλiwhere i=1,2,…,N, xk≡x(τk), λk≡λ(τk) for k=1,2,…,N.As proven in [24], the differential approximation matrices D- and D, and the adjoint differential approximation matrices D-∗ and D∗ have the following relationships:(24a)D-i=-∑k=1NDik(24b)D-i∗=-∑k=1NDik∗(24c)Dik∗=-wkwiDkiIt can be seen from (24a), (24b), and (24c) that as long as the matrix D is determined via differentiating the Lagrange interpolating polynomials Lk(t), the other matrices D-, D-∗, and D∗ can be directly calculated by employing (24a), (24b), and (24c). Furthermore, the matrix D and the Gauss weights wk can be computed offline, and thus all the other matrices can be also computed offline.DenoteΛ=[λ1T,λ2T,…,λNT]T∈RnN, X=[x1T,x2T,…,xNT]T∈RnN, X0=x(-1)∈Rn. Equations (23a) and (23b) can be rearranged as(25a)D-~∗X0+D~+∗Λ+Q~X=0(25b)D-~X0+D~-X+G~Λ=0where (26)D-~∗=D-1∗SfD-2∗Sf⋮D-N∗Sf∈RnN×n,D-~=D-1InD-2In⋮D-NIn∈RnN×n,(27)D~+∗=D11∗In-tf-t02D-1∗Sfw1BR-1BT-ATD12∗In-tf-t02D-∗Sfw2BR-1BT⋯D1N∗In-tf-t02D-1∗SfwNBR-1BTD21∗In-tf-t02D-2∗Sfw1BR-1BTD22∗In-tf-t02D-2∗Sfw2BR-1BT-AT⋯D2N∗In-tf-t02D-2∗SfwNBR-1BT⋮⋮⋱⋮DN1∗In-tf-t02D-N∗Sfw1BR-1BTDN2∗In-tf-t02D-N∗Sfw2BR-1BT⋯DNN∗In-tf-t02D-N∗SfwNBR-1BT-AT∈RnN×nN(28)D~-=D11In-tf-t02AD12In⋯D1NInD21InD22In-tf-t02A⋯D2NIn⋮⋮⋱⋮DN1InDN2In⋯DNNIn-tf-t02A∈RnN×nNQ~=tf-t02D-1∗Sfw1A+Qtf-t02D-1∗Sfw2A⋯tf-t02D-1∗SfwNAtf-t02D-2∗Sfw1Atf-t02D-2∗Sfw2A+Q⋯tf-t02D-2∗SfwNA⋮⋮⋱⋮tf-t02D-N∗Sfw1Atf-t02D-N∗Sfw2A⋯tf-t02D-N∗SfwNA+Q∈RnN×nNG~=tf-t02BR-1BT0n⋯0n0ntf-t02BR-1BT⋯0n⋮⋮⋱⋮0n0n0ntf-t02BR-1BT∈RnN×nNIn the preceding matrices, In and 0n are the n×n identity and zero matrices, respectively. Next, we transform (25a) and (25b) into a concise form as(29)D-~∗D-~X0+D~+∗Q~G~D~-ΛX=0DenoteD-Aug=D-~∗D-~∈R2nN×n, DAug=D~+∗Q~G~D~-∈R2nN×2nN. It follows that(30)D-AugX0+DAugΛX=0By solving (30), the following solution to the two-point boundary-value problem of equations (15a) and (15b) is obtained:(31)ΛX=-DAug-1D-AugX0It is important to note from (31) that once the initial state value is given, the states and costates at all the Gauss points can be simultaneously calculated without any explicit integration process. However, (31) cannot provide the values of the states and costates at the boundary points because Gauss points do not include the boundary points. Since the initial value of x has been given, its terminal value can be calculated via (21), and the terminal value of λ can be calculated via (22). Regarding the initial value of λ, the following equation can be used: (32)λ-1=λ1-tf-t02∑k=1NwkQxk+ATλkNow, all the costates at the discretization points (Gauss points and the boundary points) have been obtained from (21), (22), (31), and (32); the optimal control can be thus determined via the following equation:(33)ui∗=-R-1BTλii=0,1,2,…,N+1


## 3.2. Implementation of Path-Following Guidance

Note that the final time of the flighttf is necessary in the computation of matrices D~+∗, D~-, Q~, and G~. In order to obtain an approximation of tf, the following equation is employed:(34)tf=t0+tgo≈t0+RVmAs the proposed guidance with impact angle constraint is derived in the new coordinateTxdyd, the vehicle cross-range y and heading angle γ in Txdyd are computed as(35a)y=-xm-xtsin⁡γt+ym-ytcos⁡γt(35b)γ=γm-γtwhere (xm,ym) and (xt,yt) are the coordinates of the vehicle and the virtual vehicle in the inertial coordinate OxIyI, respectively.Note that the optimal solution to the problemP described previously is open-loop. To obtain a closed-loop solution, the receding horizon technique is used in this study. Finally, the procedure for implementing the proposed guidance law with impact angle constraint for the path following can be summarized as follows.(1)
Initialize the number of Gauss pointN. Sett0=0.(2)
Solve the problemP:
(a)
Computetf from (34) and X0 from (35a) and (35b).(b)
Compute the matricesD-Aug and DAug, and solve (31).(c)
Get the optimal controlui(i=0,1,…,N+1) using (33).(3)
Apply the optimal control at the first point,u0, to the vehicle’s dynamics and update the states of the vehicle.(4)
Update the states of the virtual vehicle according to (1) and the curvature of the desired path.(5)
Updatet0 byt0+Δt (Δt is the guidance time step).(6)
Repeat steps(2)-(5) until the vehicle reaches the end of the desired path.


## 4. Hardware Experiment and Simulation Results

In this section, a hardware experimental platform based on the ARM Cortex-M7 processor will be addressed to evaluate the real-time computational performance of the proposed method. Simulation will be provided to validate the accuracy and effectiveness of the proposed method for solving problemP via comparing with GPOPS [26] which is an open-source software for solving optimal control problems, and then the performance of the proposed guidance law in a path-following problem, taken from [15], will be investigated. In all of the following simulations, unless stated otherwise, the vehicle considered in this study has a constant velocity Vm as 50 m/s, a lateral acceleration saturation ±15g, and an autopilot dynamic delay tτ=0.5s. It should be pointed out that the lateral acceleration saturation of ±15g might be unfeasible in practice for common UAVs. However, because the emphasis of this section is to demonstrate the effectiveness and the superiority of the proposed guidance law, in order to make fair comparisons with other guidance laws, here, we chose the same value of the lateral acceleration saturation that used in [15].


### 4.1. Validation

In this section, the proposed method is compared with GPOPS by assuming that the initial value of the problemP is x0=150500T and the terminal timetf is 10s. Actually, the terminal time and initial value are determined by (34), (35a), and (35b) in the path-following guidance law as stated above. The assumed value of x0 and tf is only used for validation in this section. The weighting functions are chosen as Q=diag(111), R=1, Sf=diag(1×1051×1050), R∗=20m. The number of Gauss points,N, is chosen as 15. The comparison results of the states and costates presented in Figure 2 clearly show that the proposed method is consistent with GPOPS. Specifically, the maximum errors in the three components of the states are less than 3×10−6 and the maximum errors in the three components of the costates are less than 1×10−5. These results demonstrate the high accuracy of the proposed method.Figure 2
Comparisons of results between the proposed method and GPOPS.
(a)
Time histories of states (b)
Error of states (c)
Time histories of costates (d)
Error of costatesTable1 summarizes the computational time of the two methods performing on a 2.66GHz Core 2 Duo personal computer (PC) with 2 GB RAM running Windows 7. As can be seen, the proposed method is computationally superior to GPOPS. This is mainly because GPOPS must invoke an optimization algorithm (for instance, ipopt or snopt) to iteratively find the optimal solution, which is time consuming and results in a serious computational burden. However, the proposed method obtains the optimal solution by just solving algebraic equations and does not need any iterative optimization process, which makes the proposed method has high computational efficiency.Table 1
Computational time of the proposed method and GPOPS performing on PC.
No. Computational time t 2/t1 Proposed Method GPOPS t 1/(ms) t 2/(ms) 1 2.39 462.04 193.32 2 2.50 467.73 187.09 3 2.46 463.50 188.41To further evaluate the real-time computational capacity of the proposed method on a real digital processor, the guidance law is performed on a STM32 Nucleo-F767ZI development board. The hardware experimental platform is shown in Figure3. The microcontroller (STM32F767ZIT6U) used in the development board is based on the high-performance ARM Cortex-M7 32-bit RISC core operating at up to 216MHz frequency and features a floating-point unit (FPU) which supports ARM double-precision and single-precision processing instructions. To implement the guidance law on the hardware platform, the STM32Cube MX software is used to generate the HAL-based embedded project of IAR compiler. All the matrix operations needed in the guidance law are carried out using the corresponding C functions provided by CMSIS DSP library, which is built-in suite of common signal processing functions. An oscilloscope is used to measure the computational time in the following procedure: keep the first port of GPIOB at a high level during the whole computation process, while set the port to a low level at other times. Hence the computation time can be captured by measuring the duration time of the high level. The single-precision FPU and the compiler’s code optimization are turned on and off, respectively, in the test. Table 2 summarizes the computational time of the proposed guidance law running one loop (e.g., step 2-step 5 in the receding horizon procedure). It is seen that the computational time increases as the discretization point increases. When turning on FPU, the computational time is far less than that of the case with FPU OFF. In addition, the compiler optimization for the embedded code is also helpful to reduce the computation time. When we use the compiler optimization and turn on FPU at the same time, the computational time can be reduced to a satisfactory level, which means the proposed method is computationally viable for onboard implementation.Table 2
Computational time of the proposed guidance law performing on hardware.
C/C++ CompilerOptimization FPU: VFPv5 single precision Computation time (ms) N=5 N=10 N=15 High level with speed mode Turn ON 3 18 87 Turn OFF 40 303 1070  None Turn ON 7 46 128 Turn OFF 46 357 1183Figure 3
Hardware experimental platform based on STM32F767ZIT6U microcontroller.


### 4.2. Straight-Line Following

A straight-line path is considered in this section for path-following performance analysis. The proposed guidance law is compared with trajectory shaping guidance in [15], nonlinear path-following guidance in [13, 14], and pure pursuit in [11]. The value of the autopilot delay tτ varies as tτ=0.1s, 0.3s, 0.5s, and 0.7s. Comparisons of trajectories and lateral command acceleration are presented in Figure 4. Trajectories for tτ=0.1s, as shown in Figure 4(a), present good path following for all four guidance laws. However, the proposed guidance converges faster to the desired path than the other work and does not cause drastic control effort against the large initial control requirement of trajectory shaping guidance as shown in Figure 4(b). For tτ=0.3s, the proposed guidance still obtains good path-following performance as shown in Figure 4(c), whereas pure pursuit has an overshoot; nonlinear guidance and trajectory shaping guidance start to oscillate, which cause path-following errors and slow convergences. Moreover, it is clear in Figure 4(d) that the control effort of trajectory shaping guidance is increased with oscillations until it is increased to the saturation. For an increased tτ=0.5s as shown in Figures 4(e) and 4(f), the overshoot of pure pursuit and the oscillation of nonlinear guidance start to degenerate, and the corresponding control efforts increase to saturation. However, the proposed guidance still follows the desired path with negligible errors. While increasing the dynamic delay to 0.7s, the path-following performances for trajectory shaping guidance, nonlinear guidance, and pure pursuit deteriorate drastically, as shown in Figure 4(g). Nonlinear guidance and trajectory shaping guidance cannot even converge to the path because of oscillations. Figure 4(h) indicates that the proposed guidance also causes control saturation during the initial phase, but as the vehicle approaches the desired path, the control effort can return to reasonable values. However, the control efforts of nonlinear guidance and trajectory shaping guidance are approximately switching between their upper and lower bounds.Figure 4
Straight-line following withR∗=20m.
(a)
Trajectories withtτ=0.1s (b)
Lateral command acceleration withtτ=0.1s (c)
Trajectories withtτ=0.3s (d)
Lateral command acceleration withtτ=0.3s (e)
Trajectories withtτ=0.5s (f)
Lateral command acceleration withtτ=0.5s  (g)
Trajectories withtτ=0.7s (h)
Lateral command acceleration withtτ=0.7sThe initial heading angle of the vehicle could be different for different missions, a robust path-following guidance law should achieve good path following and be unaffected by the value of initial heading angle. Next, we analyze the path-following performance with different initial heading angles. For fair comparison, the autopilot lag of the vehicle is set astτ =0.1s and R∗ is 20m, where trajectory shaping, pure pursuit, and nonlinear guidance law could achieve a satisfactory performance without oscillation as shown in Figure 4(a). Trajectories for γm(t0)=-90deg, 0deg, and 180deg, as shown in Figure 5, present good path following for the proposed guidance, trajectory shaping, and nonlinear guidance. However, the proposed guidance converges faster to the desired path with negligible errors, whereas the pure pursuit fails to converge to the desired path. Moreover, it can be clearly seen that the trajectory shaping guidance always drives the vehicle to first move backward and then approach the desired path during the engagement. This is mainly because the trajectory shaping guidance always utilizes a tail-chase approach to chase the virtual target, which degrades the rate of convergence. Results show that the proposed guidance can achieve satisfactory path-following performances under different initial heading angles γm(t0) and is very robust with respect to the value of γm(t0).Figure 5
Straight-line following with different initial heading angles.
(a)
Trajectories withγm(t0)=-90deg (b)
Lateral command acceleration withγm(t0)=-90deg (c)
Trajectories withγm(t0)=0deg (d)
Lateral command acceleration withγm(t0)=0deg (e)
Trajectories withγm(t0)=180deg (f)
Lateral command acceleration withγm(t0)=180deg


### 4.3. Straight-Line Following with Different Weighting Functions

Since we do not impose any constrained conditions on the weighting functions during the whole derivation of the guidance law, the proposed method can easily and directly tackle different weighting functions without any modifications, which means different weighting functions make no difference to the structure of the proposed method. This property makes the path-following guidance design very flexible to accomplish the specified objective. In this section, we take the straight-line path-following scenario as an example and utilize different weighting functions of both states and control cost to demonstrate the path-following performance of the proposed guidance law. Three cases with different weighting functions, showed in (36)–(39), are investigated in the simulation. Case 1 considers simple weighting functions, in which both Q and R are constants. Note that one special case of case 1, Q=0, R=1, is frequently used to derive the optimal guidance, as studied in [15, 16, 27, 28]. In the second case, Q is chosen as an exponential function with respect to the time-to-go, while R is chosen as a Gaussian weighting function. Case 3 presents hybrid weighting functions that are of discontinuous form, and switches between those in Cases 1 and 2 by the path-following error d. Note that discontinuous weighting functions are intractable for most of the guidance laws in existing literatures.Case 1.
(36) Q t = 1 0 0 0 1 0 0 0 1 , R t = 1Case 2.
(37) Q t = 10 e 0.2 t g o 0 0 0 10 e 0.2 t g o 0 0 0 10 e 0.2 t g o , R t = e - t g o 2 / 0.5 t + t g o / 8 2Case 3.
(38)
Q
t
=
10
e
0.2
t
g
o
0
0
0
10
e
0.2
t
g
o
0
0
0
10
e
0.2
t
g
o
i
f
d
>
50
m
1
0
0
0
1
0
0
0
1
e
l
s
e
(39)
R
t
=
e
-
t
g
o
2
/
0.5
t
+
t
g
o
/
8
2
i
f
d
>
50
m
1
e
l
s
eAs shown in Figure6, the proposed guidance law can successfully drive the vehicle converge to the desired path for all cases. However, different weighting functions lead to different convergence rate and thus obtain different path-following performance. For case 1, the constants Q and R do not make any special considerations on shaping the vehicle’s path and acceleration profile during different phases in the transition and result in a moderate path-following performance. Case 2 performs a faster convergence during the forepart of the transition, but a slower convergence when the vehicle is approaching the desired path and an obvious overshoot. It is clearly observed that case 3 possesses both the advantages of Cases 1 and 2 and performs the best path-following performance not only in the convergence rate but also in the accuracy. Figure 7 depicts the time history of the command acceleration of each case. The switching point of the weighting functions in case 3 is clearly observed in this figure. The results of simulations show that the path-following performance of the proposed method can be flexibly designed by adjusting the weighting functions, which provides more degrees of freedom in designing a path-following guidance law for various guidance objectives.Figure 6
Straight-line following trajectories with different weighting functions.Figure 7
Lateral command acceleration with different weighting functions.


### 4.4. Composite Path Following

Next, a composite path consisting of counterclockwise semicircle, clockwise semicircle, and straight line is considered. The radius of both the semicircles is 100m. The performance of composite path following for different guidance laws is shown in Figure8. For small values of tτ, all the guidance laws can provide satisfactory path-following performances. However, as the value of tτ increases, trajectory shaping guidance and nonlinear guidance start to oscillate and have considerable path-following errors. This phenomenon is similar to that of straight-line following. Pure pursuit is not very sensitive to tτ and performs better than trajectory shaping guidance and nonlinear guidance, but it also has a large path-following error in curve following for large values of tτ. The proposed guidance law is independent of tτ and can follow the path with negligible errors all the time. Simulation results show that the proposed guidance law is more effective than the other guidance laws in both straight-line following and curve following.Figure 8
Composite path following withR∗=20m.
(a)
t τ=0.1s (b)
t τ=0.3s (c)
t τ=0.5s (d)
t τ=0.7s


### 4.5. Effect of ParameterR∗

As pointed out in [15], a robust path-following law should be fairly unaffected by the choice of R∗. In this study, we consider the composite path given in the previous section again and vary the value of R∗ as R∗=15m, 45m, 75m, and 105m to evaluate the path-following performance of each guidance law. For R∗=15m, as shown in Figure 9(a), trajectory shaping guidance and nonlinear guidance cannot converge to the desired path and have drastic oscillations around the desired path, whereas the proposed guidance and pure pursuit have negligible path-following errors. Figure 9(b) shows that the oscillations of trajectory shaping guidance and nonlinear guidance are alleviated for R∗=45m, but the proposed guidance still has the least errors. For R∗=75m, as shown in Figure 9(c), trajectory shaping guidance and nonlinear guidance provide slow convergence, and pure pursuit performance starts to deteriorate with obvious path-following errors. However, the proposed guidance still has a satisfactory path-following performance. When further increasing R∗ to 105m as shown in Figure 9(d), the proposed guidance still follows the desired path with high accuracy, whereas both pure pursuit and nonlinear guidance have considerable path-following errors, and trajectory shaping guidance converges slowly. These results demonstrate that the proposed guidance law is fairly unaffected by the choice of R∗.Figure 9
Trajectories with differentR∗ when tτ=0.5s.
(a)
R ∗=15m (b)
R ∗=45m (c)
R ∗=75m (d)
R ∗=105m


### 4.6. Convergence Rate Comparison

To further analyze the path-following performance of the proposed guidance, consider a circular path following and focus on the convergence rate. The radius of the circle is chosen as 100m, andR∗=100m. Trajectories with tτ=0.5s are plotted in Figure 10(a), and the corresponding position errors are shown in Figure 10(b). As can be seen in Figure 10(a), the proposed guidance presents a very good following performance, whereas trajectory shaping guidance and nonlinear guidance converge to the path slowly. Moreover, pure pursuit has a steady following error because of a higher R∗ and the curvature of the path. This is the inherent property of pure pursuit as shown in [15]. The settling time (1%), as shown in Figure 10(b), for the proposed guidance, trajectory shaping guidance, and nonlinear guidance is 2.68s, 5.12s, and 14.00s, respectively. Thus, the proposed guidance possesses the fastest convergence to the path compared with the other guidance laws. A similar study is carried out for a larger dynamic delay, tτ=0.7s. The trajectories and corresponding position errors are presented in Figures 10(c) and 10(d), respectively. The settling time for the proposed guidance, trajectory shaping guidance, and nonlinear guidance is 2.70s, 15.05s, and 16.88s, respectively. As can be seen, an increased tτ deteriorates the convergence rates for all of the four guidance laws. However, the proposed guidance still provides the fastest convergence to the path and is slightly affected by tτ. Results further highlight the significant advantage of the proposed guidance law.Figure 10
Circular path following withR∗=100m for convergence rate comparison.
(a)
Trajectories withtτ=0.5s (b)
Position errors withtτ=0.5s (c)
Trajectories withtτ=0.7s (d)
Position errors withtτ=0.7s


## 4.1. Validation

In this section, the proposed method is compared with GPOPS by assuming that the initial value of the problemP is x0=150500T and the terminal timetf is 10s. Actually, the terminal time and initial value are determined by (34), (35a), and (35b) in the path-following guidance law as stated above. The assumed value of x0 and tf is only used for validation in this section. The weighting functions are chosen as Q=diag(111), R=1, Sf=diag(1×1051×1050), R∗=20m. The number of Gauss points,N, is chosen as 15. The comparison results of the states and costates presented in Figure 2 clearly show that the proposed method is consistent with GPOPS. Specifically, the maximum errors in the three components of the states are less than 3×10−6 and the maximum errors in the three components of the costates are less than 1×10−5. These results demonstrate the high accuracy of the proposed method.Figure 2
Comparisons of results between the proposed method and GPOPS.
(a)
Time histories of states (b)
Error of states (c)
Time histories of costates (d)
Error of costatesTable1 summarizes the computational time of the two methods performing on a 2.66GHz Core 2 Duo personal computer (PC) with 2 GB RAM running Windows 7. As can be seen, the proposed method is computationally superior to GPOPS. This is mainly because GPOPS must invoke an optimization algorithm (for instance, ipopt or snopt) to iteratively find the optimal solution, which is time consuming and results in a serious computational burden. However, the proposed method obtains the optimal solution by just solving algebraic equations and does not need any iterative optimization process, which makes the proposed method has high computational efficiency.Table 1
Computational time of the proposed method and GPOPS performing on PC.
No. Computational time t 2/t1 Proposed Method GPOPS t 1/(ms) t 2/(ms) 1 2.39 462.04 193.32 2 2.50 467.73 187.09 3 2.46 463.50 188.41To further evaluate the real-time computational capacity of the proposed method on a real digital processor, the guidance law is performed on a STM32 Nucleo-F767ZI development board. The hardware experimental platform is shown in Figure3. The microcontroller (STM32F767ZIT6U) used in the development board is based on the high-performance ARM Cortex-M7 32-bit RISC core operating at up to 216MHz frequency and features a floating-point unit (FPU) which supports ARM double-precision and single-precision processing instructions. To implement the guidance law on the hardware platform, the STM32Cube MX software is used to generate the HAL-based embedded project of IAR compiler. All the matrix operations needed in the guidance law are carried out using the corresponding C functions provided by CMSIS DSP library, which is built-in suite of common signal processing functions. An oscilloscope is used to measure the computational time in the following procedure: keep the first port of GPIOB at a high level during the whole computation process, while set the port to a low level at other times. Hence the computation time can be captured by measuring the duration time of the high level. The single-precision FPU and the compiler’s code optimization are turned on and off, respectively, in the test. Table 2 summarizes the computational time of the proposed guidance law running one loop (e.g., step 2-step 5 in the receding horizon procedure). It is seen that the computational time increases as the discretization point increases. When turning on FPU, the computational time is far less than that of the case with FPU OFF. In addition, the compiler optimization for the embedded code is also helpful to reduce the computation time. When we use the compiler optimization and turn on FPU at the same time, the computational time can be reduced to a satisfactory level, which means the proposed method is computationally viable for onboard implementation.Table 2
Computational time of the proposed guidance law performing on hardware.
C/C++ CompilerOptimization FPU: VFPv5 single precision Computation time (ms) N=5 N=10 N=15 High level with speed mode Turn ON 3 18 87 Turn OFF 40 303 1070  None Turn ON 7 46 128 Turn OFF 46 357 1183Figure 3
Hardware experimental platform based on STM32F767ZIT6U microcontroller.


## 4.2. Straight-Line Following

A straight-line path is considered in this section for path-following performance analysis. The proposed guidance law is compared with trajectory shaping guidance in [15], nonlinear path-following guidance in [13, 14], and pure pursuit in [11]. The value of the autopilot delay tτ varies as tτ=0.1s, 0.3s, 0.5s, and 0.7s. Comparisons of trajectories and lateral command acceleration are presented in Figure 4. Trajectories for tτ=0.1s, as shown in Figure 4(a), present good path following for all four guidance laws. However, the proposed guidance converges faster to the desired path than the other work and does not cause drastic control effort against the large initial control requirement of trajectory shaping guidance as shown in Figure 4(b). For tτ=0.3s, the proposed guidance still obtains good path-following performance as shown in Figure 4(c), whereas pure pursuit has an overshoot; nonlinear guidance and trajectory shaping guidance start to oscillate, which cause path-following errors and slow convergences. Moreover, it is clear in Figure 4(d) that the control effort of trajectory shaping guidance is increased with oscillations until it is increased to the saturation. For an increased tτ=0.5s as shown in Figures 4(e) and 4(f), the overshoot of pure pursuit and the oscillation of nonlinear guidance start to degenerate, and the corresponding control efforts increase to saturation. However, the proposed guidance still follows the desired path with negligible errors. While increasing the dynamic delay to 0.7s, the path-following performances for trajectory shaping guidance, nonlinear guidance, and pure pursuit deteriorate drastically, as shown in Figure 4(g). Nonlinear guidance and trajectory shaping guidance cannot even converge to the path because of oscillations. Figure 4(h) indicates that the proposed guidance also causes control saturation during the initial phase, but as the vehicle approaches the desired path, the control effort can return to reasonable values. However, the control efforts of nonlinear guidance and trajectory shaping guidance are approximately switching between their upper and lower bounds.Figure 4
Straight-line following withR∗=20m.
(a)
Trajectories withtτ=0.1s (b)
Lateral command acceleration withtτ=0.1s (c)
Trajectories withtτ=0.3s (d)
Lateral command acceleration withtτ=0.3s (e)
Trajectories withtτ=0.5s (f)
Lateral command acceleration withtτ=0.5s  (g)
Trajectories withtτ=0.7s (h)
Lateral command acceleration withtτ=0.7sThe initial heading angle of the vehicle could be different for different missions, a robust path-following guidance law should achieve good path following and be unaffected by the value of initial heading angle. Next, we analyze the path-following performance with different initial heading angles. For fair comparison, the autopilot lag of the vehicle is set astτ =0.1s and R∗ is 20m, where trajectory shaping, pure pursuit, and nonlinear guidance law could achieve a satisfactory performance without oscillation as shown in Figure 4(a). Trajectories for γm(t0)=-90deg, 0deg, and 180deg, as shown in Figure 5, present good path following for the proposed guidance, trajectory shaping, and nonlinear guidance. However, the proposed guidance converges faster to the desired path with negligible errors, whereas the pure pursuit fails to converge to the desired path. Moreover, it can be clearly seen that the trajectory shaping guidance always drives the vehicle to first move backward and then approach the desired path during the engagement. This is mainly because the trajectory shaping guidance always utilizes a tail-chase approach to chase the virtual target, which degrades the rate of convergence. Results show that the proposed guidance can achieve satisfactory path-following performances under different initial heading angles γm(t0) and is very robust with respect to the value of γm(t0).Figure 5
Straight-line following with different initial heading angles.
(a)
Trajectories withγm(t0)=-90deg (b)
Lateral command acceleration withγm(t0)=-90deg (c)
Trajectories withγm(t0)=0deg (d)
Lateral command acceleration withγm(t0)=0deg (e)
Trajectories withγm(t0)=180deg (f)
Lateral command acceleration withγm(t0)=180deg


## 4.3. Straight-Line Following with Different Weighting Functions

Since we do not impose any constrained conditions on the weighting functions during the whole derivation of the guidance law, the proposed method can easily and directly tackle different weighting functions without any modifications, which means different weighting functions make no difference to the structure of the proposed method. This property makes the path-following guidance design very flexible to accomplish the specified objective. In this section, we take the straight-line path-following scenario as an example and utilize different weighting functions of both states and control cost to demonstrate the path-following performance of the proposed guidance law. Three cases with different weighting functions, showed in (36)–(39), are investigated in the simulation. Case 1 considers simple weighting functions, in which both Q and R are constants. Note that one special case of case 1, Q=0, R=1, is frequently used to derive the optimal guidance, as studied in [15, 16, 27, 28]. In the second case, Q is chosen as an exponential function with respect to the time-to-go, while R is chosen as a Gaussian weighting function. Case 3 presents hybrid weighting functions that are of discontinuous form, and switches between those in Cases 1 and 2 by the path-following error d. Note that discontinuous weighting functions are intractable for most of the guidance laws in existing literatures.Case 1.
(36) Q t = 1 0 0 0 1 0 0 0 1 , R t = 1Case 2.
(37) Q t = 10 e 0.2 t g o 0 0 0 10 e 0.2 t g o 0 0 0 10 e 0.2 t g o , R t = e - t g o 2 / 0.5 t + t g o / 8 2Case 3.
(38)
Q
t
=
10
e
0.2
t
g
o
0
0
0
10
e
0.2
t
g
o
0
0
0
10
e
0.2
t
g
o
i
f
d
>
50
m
1
0
0
0
1
0
0
0
1
e
l
s
e
(39)
R
t
=
e
-
t
g
o
2
/
0.5
t
+
t
g
o
/
8
2
i
f
d
>
50
m
1
e
l
s
eAs shown in Figure6, the proposed guidance law can successfully drive the vehicle converge to the desired path for all cases. However, different weighting functions lead to different convergence rate and thus obtain different path-following performance. For case 1, the constants Q and R do not make any special considerations on shaping the vehicle’s path and acceleration profile during different phases in the transition and result in a moderate path-following performance. Case 2 performs a faster convergence during the forepart of the transition, but a slower convergence when the vehicle is approaching the desired path and an obvious overshoot. It is clearly observed that case 3 possesses both the advantages of Cases 1 and 2 and performs the best path-following performance not only in the convergence rate but also in the accuracy. Figure 7 depicts the time history of the command acceleration of each case. The switching point of the weighting functions in case 3 is clearly observed in this figure. The results of simulations show that the path-following performance of the proposed method can be flexibly designed by adjusting the weighting functions, which provides more degrees of freedom in designing a path-following guidance law for various guidance objectives.Figure 6
Straight-line following trajectories with different weighting functions.Figure 7
Lateral command acceleration with different weighting functions.


## 4.4. Composite Path Following

Next, a composite path consisting of counterclockwise semicircle, clockwise semicircle, and straight line is considered. The radius of both the semicircles is 100m. The performance of composite path following for different guidance laws is shown in Figure8. For small values of tτ, all the guidance laws can provide satisfactory path-following performances. However, as the value of tτ increases, trajectory shaping guidance and nonlinear guidance start to oscillate and have considerable path-following errors. This phenomenon is similar to that of straight-line following. Pure pursuit is not very sensitive to tτ and performs better than trajectory shaping guidance and nonlinear guidance, but it also has a large path-following error in curve following for large values of tτ. The proposed guidance law is independent of tτ and can follow the path with negligible errors all the time. Simulation results show that the proposed guidance law is more effective than the other guidance laws in both straight-line following and curve following.Figure 8
Composite path following withR∗=20m.
(a)
t τ=0.1s (b)
t τ=0.3s (c)
t τ=0.5s (d)
t τ=0.7s


## 4.5. Effect of ParameterR∗

As pointed out in [15], a robust path-following law should be fairly unaffected by the choice of R∗. In this study, we consider the composite path given in the previous section again and vary the value of R∗ as R∗=15m, 45m, 75m, and 105m to evaluate the path-following performance of each guidance law. For R∗=15m, as shown in Figure 9(a), trajectory shaping guidance and nonlinear guidance cannot converge to the desired path and have drastic oscillations around the desired path, whereas the proposed guidance and pure pursuit have negligible path-following errors. Figure 9(b) shows that the oscillations of trajectory shaping guidance and nonlinear guidance are alleviated for R∗=45m, but the proposed guidance still has the least errors. For R∗=75m, as shown in Figure 9(c), trajectory shaping guidance and nonlinear guidance provide slow convergence, and pure pursuit performance starts to deteriorate with obvious path-following errors. However, the proposed guidance still has a satisfactory path-following performance. When further increasing R∗ to 105m as shown in Figure 9(d), the proposed guidance still follows the desired path with high accuracy, whereas both pure pursuit and nonlinear guidance have considerable path-following errors, and trajectory shaping guidance converges slowly. These results demonstrate that the proposed guidance law is fairly unaffected by the choice of R∗.Figure 9
Trajectories with differentR∗ when tτ=0.5s.
(a)
R ∗=15m (b)
R ∗=45m (c)
R ∗=75m (d)
R ∗=105m


## 4.6. Convergence Rate Comparison

To further analyze the path-following performance of the proposed guidance, consider a circular path following and focus on the convergence rate. The radius of the circle is chosen as 100m, andR∗=100m. Trajectories with tτ=0.5s are plotted in Figure 10(a), and the corresponding position errors are shown in Figure 10(b). As can be seen in Figure 10(a), the proposed guidance presents a very good following performance, whereas trajectory shaping guidance and nonlinear guidance converge to the path slowly. Moreover, pure pursuit has a steady following error because of a higher R∗ and the curvature of the path. This is the inherent property of pure pursuit as shown in [15]. The settling time (1%), as shown in Figure 10(b), for the proposed guidance, trajectory shaping guidance, and nonlinear guidance is 2.68s, 5.12s, and 14.00s, respectively. Thus, the proposed guidance possesses the fastest convergence to the path compared with the other guidance laws. A similar study is carried out for a larger dynamic delay, tτ=0.7s. The trajectories and corresponding position errors are presented in Figures 10(c) and 10(d), respectively. The settling time for the proposed guidance, trajectory shaping guidance, and nonlinear guidance is 2.70s, 15.05s, and 16.88s, respectively. As can be seen, an increased tτ deteriorates the convergence rates for all of the four guidance laws. However, the proposed guidance still provides the fastest convergence to the path and is slightly affected by tτ. Results further highlight the significant advantage of the proposed guidance law.Figure 10
Circular path following withR∗=100m for convergence rate comparison.
(a)
Trajectories withtτ=0.5s (b)
Position errors withtτ=0.5s (c)
Trajectories withtτ=0.7s (d)
Position errors withtτ=0.7s


## 5. Conclusions

In this paper, a novel indirect Gauss pseudospectral method based path-following guidance law with generalized weighting functions by pursuing a virtual target is presented, where the autopilot dynamic delay is considered to compensate for the vehicle’s response to the guidance command. The accuracy and effectiveness of the proposed guidance are validated by comparing with GPOPS. The real-time computational performance of the proposed method is validated on a hardware platform based on the ARM Cortex-M7 processor. Simulation results show the oscillations and instability caused by the autopilot delay and demonstrate that the proposed guidance possesses a very robust path-following ability and a fast convergence rate for different autopilot delays. Simulation also shows that the proposed approach has high performance when compared with the trajectory shaping guidance, nonlinear guidance, and pure pursuit. Additionally, according to selections of the weighting functions (even discontinuous), the proposed guidance law can flexibly shape the path-following trajectories and the acceleration commands, providing more degree of freedoms for the whole path-following guidance design process.



---

*Source: 3104397-2018-08-09.xml*