Constrained Parameterized Differential Dynamic Programming for Waypoint-Trajectory Optimization

Abstract

Unmanned aerial vehicles (UAVs) are required to pass through multiple important waypoints as quickly as possible in courier delivery, enemy reconnaissance and other tasks to eventually reach the target position. There are two important problems to be solved in such tasks: constraining the trajectory to pass through intermediate waypoints and optimizing the flight time between these waypoints. A constrained parameterized differential dynamic programming (C-PDDP) algorithm is proposed for meeting multiple waypoint constraints and free-time constraints between waypoints to deal with these two issues. By considering the intermediate waypoint constraints as a kind of path state constraint, the penalty function method is adopted to constrain the trajectory to pass through the waypoints. For the free-time constraints, the flight times between waypoints are converted into time-invariant parameters and updated at the trajectory instants corresponding to the waypoints. The effectiveness of the proposed C-PDDP algorithm under waypoint constraints and free-time constraints is verified through numerical simulations of the UAV multi-point reconnaissance problem with five different waypoints. After comparing the proposed algorithm with fixed-time constrained DDP (C-DDP), it is found that C-PDDP can optimize the flight time of the trajectory with three segments to 7.35 s, 9.50 s and 6.71 s, respectively. In addition, the maximum error of the optimized trajectory waypoints of the C-PDDP algorithm is 1.06 m, which is much smaller than that (7 m) of the C-DDP algorithm used for comparison. A total of 500 Monte Carlo tests were simulated to demonstrate how the proposed algorithm remains robust to random initial guesses.

1. Introduction

Unmanned aerial vehicles (UAVs) have become the preferred platform for tasks such as reconnaissance, transportation and delivery due to their low cost, flexibility and rapid deployment characteristics. With a variety of integrated sensors, good concealment and long loiter capabilities, UAVs can be used for close-range low-altitude reconnaissance of important targets or hidden targets and greatly improve the reconnaissance efficiency and success rate [1,2,3,4,5]. However, UAVs have a short endurance due to the limitation of their energy resources, and hence, it is necessary to plan a reasonable flight trajectory to reduce the flight time and energy consumption of UAVs in missions. In addition, UAVs are required to pass through multiple necessary mission waypoints in reconnaissance, delivery and other tasks. Planning a reasonable flight trajectory to quickly pass through these points is of great economic benefit to UAV missions and can improve the performance of mission execution. Therefore, planning the flight trajectory of UAVs to reach the necessary intermediate waypoints while optimizing the flight time between waypoints is the foundation for improving the effectiveness of UAVs for reconnaissance, courier, and other missions [6,7,8].

One important piece of technology that allows UAVs to increase their autonomy is trajectory optimization [9,10,11]. Trajectory optimization aims at finding the best control commands that improve performance index such as flight endurance and utilization of energy, and enable the UAV to satisfy nonlinear constraints such as control limits and no-fly zones. Real-world trajectory optimization problems are inherently nonlinear and it is impossible to develop an analytical solution in most cases [12]. In the last few years, a wide range of numerical algorithms, including pseudo-spectral methods [13,14,15], sequential quadratic programming [16,17,18], sequential convex programming [19,20,21], and differential dynamic programming (DDP) [22,23,24] have been found to be capable of addressing nonlinear trajectory optimization problems. From the standpoint of showcasing their benefits and mathematical nature, Chai et al. [9] provide a neatly structured explanation of the understandings of several numerical optimization techniques.

In recent years, DDP has attracted a lot of attention because of its rapid convergence properties [22,24,25,26,27,28,29]. Classical dynamic programming (DP) is the foundation of DDP, which employs a second-order approximation to the value function to break down the original problem into a number of smaller-dimensional subproblems, therefore overcoming the drawbacks of DP, also known as the curse of dimensionality [30,31]. The solution process of the DDP algorithm is carried out by approximating the optimal cost function of the current nominal trajectory utilizing a second-order Taylor series and recursively updating the optimal increment of the control sequence by using the first-order optimality condition until it converges to a locally optimal solution [32]. Real-time applications are possible with DDP and its variants since they can guarantee a theoretical second-order convergence rate [33], and exhibit linear complexity throughout the prediction horizon.

However, the major problem with original DDP is its inability to handle control, state, and other constraints besides dynamics constraints, and the need for a preset flight time. These two aspects limit the application of DDP in tasks such as UAV reconnaissance and courier delivery. On the one hand, it is necessary to pass through multiple intermediate waypoints in scenarios such as reconnaissance and courier delivery; thus, it is required that the trajectory satisfies the intermediate point state constraints. In addition, presetting the flight time between multiple waypoints entails heuristic methods, complicating the manual determination of the optimal termination time. Therefore, addressing these two drawbacks with the original DDP technique is essential and necessary.

One possible solution for the problem of the flight trajectory passing through necessary intermediate waypoints is the multiple shooting method. Giftthaler et al. [34] generalized the DDP algorithm to the multiple shooting framework by introducing intermediate shooting states as initial conditions for each segment and imposing continuity constraints with the previous segment’s trajectory, and the resulting method achieves faster convergence, better local shrinkage, and a shorter runtime. For the purpose of increasing the calculation speed of the algorithm and decreasing the sensitivity of the algorithm to the initial guess, Etienne et al. [35] embedded the multiple shooting method into a DDP method for the design of spacecraft trajectories; the algorithm derived the update formula of the multiple shooting DDP method for multiple trajectory segments and used the augmented Lagrangian method to deal with continuous constraints between trajectory segments and waypoint constraints. Li et al. [36] improved the convergence performance of the multiple shooting DDP algorithm by introducing a penalty method and an adaptive evaluation function to control the algorithm step size. Expanding on previous work, Mastalli et al. [37] proposed a feasibility-driven DDP based on Goldstein’s condition and a linear search process for expected cost reductions. Although the multiple shooting method is able to improve the performance of the algorithm, this is at the cost of potential discontinuities between multiple trajectories. In addition to the multiple shooting idea, Philipp et al. [38] established the concept of waypoint proximity, which utilizes a quadratic term penalty function to constrain the trajectory to be close to the waypoint and solves the problem using a solver. The idea of waypoint proximity avoids possible discontinuities between multiple trajectories and ensures the dynamical feasibility of the whole trajectory.

In addition, a number of researchers have dealt with the free time using other numerical optimization algorithms. Maity et al. [39] incorporated changes in the final time into the variation in state variables, developed a composite cost function for the Model Predict Static Programming (MPSP) method that included both the control and terminal time increments, and utilized the final time update and the terminal time increment to produce an analytical statement with the first-order optimality condition. Hong et al. [16] solved the free time obstacle avoidance constrained guidance problem by using sequential quadratic programming based on this concept. We revisited original DDP by dynamically optimizing the final time in conjunction with the control input in our previous work [40]. Finally, Sun et al. proposed a free terminal time stochastic DDP method considering random state perturbations based on our work [41]. The work published in Science Robotics by Foehn et al. developed time-optimized quadrotor flight trajectory planning with multiple waypoints, and the proposed algorithm optimizes both the time allocation of the trajectory and the trajectory itself [38], but the work was still performed on a PC rather than on an airborne computer. Oshin et al. [42] proposed a parameterized differential dynamic programming (PDDP) method for simultaneous optimization of parameters and control inputs, which was able to fast and stable converge to the minimum cost via rigorous mathematical demonstrations. Setting the flight time as an unknown parameter can solve the free terminal time problem using PDDP. However, this algorithm does not consider the path state constraints, and it cannot deal with time optimization of multi-segment trajectories. Martinez et al. [43] developed a multiple shooting solver that solves unknown time and parameter structures via parameterized Riccati recursion for complex action estimation problems such as humanoid robot problems.

Inspired by the observations mentioned above, this article develops a novel algorithm, constrained parameterized differential dynamic programming (C-PDDP), which builds on parameterized DDP. The proposed algorithm utilizes the penalty function method to deal with the nonlinear constraints of the intermediate waypoints, and a detailed mathematical demonstration of the modification of the state–action value function is provided. Based on the concept of parameterization, the flight time between waypoints is transformed into the unknown parameters to be solved, and the optimality condition of value function at corresponding instants of the waypoints is adopted to obtain the optimal time deviation and dynamically optimize the flight time. The proposed C-PDDP algorithm solves the problem that original PDDP is not able to deal with the intermediate waypoints and flexible times for multi-segment trajectories by introducing the penalty function method as well as the idea of time parameterization. Therefore, the proposed C-PDDP algorithm provides a strong potential in UAV reconnaissance and courier and other missions.

In this paper, numerical simulations are conducted in a UAV reconnaissance mission with multiple waypoints, and the results of the simulations demonstrate that this proposed algorithm can optimize the trajectory of the UAV to reach the necessary reconnaissance mission points as well as the optimal time for each trajectory segment. Compared with the fixed-time algorithm, the proposed algorithm has a smaller margin of error and a smoother trajectory, which reduces the flight time and the cost of the trajectory.

The remainder of this paper is outlined as follows. Section 2 describes the typical form of the trajectory optimization problem and discusses the fundamental PDDP algorithm. In Section 3, the proposed C-PDDP algorithm is laid out specifically and is then numerically evaluated in Section 4. Eventually, Section 5 concludes this paper. In this paper, ${\mathbf{0}}_n$ refers to the n-dimensional zero vector; vectors are denoted by $\mathit{a}$ and matrices are denoted by $\mathbf{A}$. ${\mathbf{I}}_n$ is the n-th order unit matrix. The operators $min(a,b)$ (and $max(a,b)$) denote the process of finding the minimum (and maximum) value of a and b, respectively.

2. Preliminaries and Problem Formulation

Firstly, we model the spatial–temporal trajectory optimization problem with waypoint constraints in this section, and then introduce the PDDP algorithm for solving unknown parameters.

2.1. Modeling of the Spatial–Temporal Trajectory Optimization Problem with Multiple Waypoints

In a spatial–temporal trajectory optimization problem with multiple waypoint constraints, the goal of trajectory optimization is to find the optimal trajectory that passes through each waypoint while optimizing the flight time of each trajectory segment. Given W mission waypoints ${\mathit{p}}_{w_j}$, where $j\in [1,\dots ,W]$, the objective of the multiple-waypoint-constrained spatial–temporal trajectory optimization problem can be expressed as

$$\underset{\mathit{u},t}{min}\mathsf{\Phi }\left(\mathit{x}\left(t_f\right),t_f\right)+\sum _{j=1}^W\left[{\int }_{t_{w_{j-1}}}^{t_{w_j}}{L}_j(\mathit{x}\left(t\right),\mathit{u}\left(t\right))\mathrm{d}Ø\right]$$

(1)

where $t_{w_j}$ denotes the terminal flight time of the flight trajectory segment ${\mathit{X}}_{w_{j-1}{w}_j}$ that starts at the waypoint $w_{j-1}$. The equivalent objective function of the objective function (1) in the discrete time frame can be expressed as

$$\underset{{\mathit{u}}_k,t}{min}\varphi \left({\mathit{x}}_N,t_N\right)+\sum _{j=1}^W\left[\sum _{k=t_{w_{j-1}}}^{t_{w_j}}{l}_j\left({\mathit{x}}_k,{\mathit{u}}_k\right)\right]$$

(2)

where $l_j\left({\mathit{x}}_k,{\mathit{u}}_k\right)=L_j(\mathit{x}\left(t\right),\mathit{u}\left(t\right))\delta t$, $\delta t=t_k-t_{k-1}$. It can be seen from the equation that since the trajectory needs to pass through the waypoints, the original trajectory is also partitioned into multiple segments based on the number of waypoints. If the initial point of the trajectory is also considered as a waypoint, the trajectory will be divided into W trajectory segments in the case of W waypoints.

For multiple-waypoint constraints, in order to generate a trajectory that passes through the waypoint ${\mathit{p}}_{w_j}$, it is necessary to define the cost of the distance between a specific point ${\mathit{x}}_k$ on the trajectory and the waypoint, and we consider the quadratic distance cost to be

$$D_{w_j}={\parallel {\mathit{p}}_k-{\mathit{p}}_{w_j}\parallel }_2$$

(3)

In order to require the trajectory to pass through the necessary waypoints, the above distance cost can be expressed as a state constraint on the waypoint ${\mathit{x}}_k$, written as

$${\parallel {\mathit{x}}_k-{\mathit{p}}_{w_j}\parallel }_2^2=0$$

(4)

This constraint is a special case of the general state constraint $g\left({\mathit{x}}_k,{\mathit{u}}_k\right)\le \mathbf{0}$, which constrains the point ${\mathit{x}}_k$ on the trajectory at time $t_k$ to pass through the j-th waypoint at ${\mathit{p}}_{w_j}$.

In addition to constraining the trajectory to pass through the waypoints, it is also necessary that the flight time of each trajectory segment can be dynamically optimized; i.e., the flight time within each trajectory segment is free and the optimal flight time is obtained through optimization. The free flight time within each trajectory segment can be expressed as

$$t_{w_j{w}_{j+1}}\in \mathcal{T}$$

(5)

Based on the above description of multiple waypoint constraints and the free flight time of the trajectory segment and considering the control limits of the UAV, the spatial–temporal trajectory optimization problem with multiple waypoint constraints for a UAV can be formulated as a constrained trajectory optimization problem 1, denoted as CTO₁, and has the following form

$$\begin{array}{cc}\hfill {\mathit{u}}_k^{*}=\underset{{\mathit{u}}_k,t}{min}& \varphi \left({\mathit{x}}_N,t_N\right)+\sum _{j=1}^W\left[\sum _{k=t_{w_{j-1}}}^{t_{w_j}}{l}_j\left({\mathit{x}}_k,{\mathit{u}}_k\right)\right]\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\mathit{x}}_{k+1}=\mathit{f}\left({\mathit{x}}_k,{\mathit{u}}_k\right)\hfill \\ \hfill & {\parallel {\mathit{x}}_k-{\mathit{p}}_{w_j}\parallel }_2=0\hfill \\ \hfill & {\mathit{u}}_{min}\le {\mathit{u}}_k\le {\mathit{u}}_{max}\hfill \\ \hfill & t_{w_j{w}_{j+1}}\in \mathcal{T}\hfill \end{array}$$

(6)

which is assumed to have a solution.

2.2. Parameterized Differential Dynamic Programming (PDDP)

The PDDP algorithm is a variant of the DDP algorithm that optimizes both control sequences and unknown parameters [42]. By abstracting the terminal time as an unknown parameter, PDDP is able to address the spatial–temporal trajectory optimization issue without presetting a terminal time of the UAV. In this subsection, the main ideas of PDDP are given. PDDP is designed to solve the following optimization problem:

$$\begin{array}{cc}\hfill \underset{\mathit{U};\mathit{\theta }}{\min }J(\mathit{U};\mathit{\theta })& =\sum _{k=1}^{N-1}l\left({\mathit{x}}_k,{\mathit{u}}_k;\mathit{\theta }\right)+\varphi \left({\mathit{x}}_N;\mathit{\theta }\right)\hfill \\ \hfill s.t.{\mathit{x}}_{k+1}& =\mathit{f}\left({\mathit{x}}_k,{\mathit{u}}_k;\mathit{\theta }\right)\hfill \end{array}$$

(7)

where a semicolon separates the parameter $\mathit{\theta }$ from the state $\mathit{x}$ and the control $\mathit{u}$, indicating that $\mathit{\theta }$ is a time-invariant parameter, independent of the time instant k.

The basic idea of original PDDP is to transform the trajectory optimization problem containing unknown parameters into a backward sequential optimization issue at each time instant and a parameter optimization problem between single iterations by using the Bellman optimality principle. The parameterized optimal running cost, which can also be defined as a parameterized value function, can be expressed as

$$V\left({\mathit{x}}_k;\mathit{\theta }\right)=min[\underset{Q\left({\mathit{x}}_k,{\mathit{u}}_k;\mathit{\theta }\right)}{\underbrace{l\left({\mathit{x}}_k,{\mathit{u}}_k;\mathit{\theta }\right)+V\left({\mathit{x}}_{k+1};\mathit{\theta }\right)}}]$$

(8)

where $Q\left({\mathit{x}}_k,{\mathit{u}}_k;\mathit{\theta }\right)$ denotes the action value function.

In contrast to standard DDP, which only considers the quadratic approximation of the state ${\mathit{x}}_k$ and the control ${\mathit{u}}_k$ along the nominal trajectory $\left({\mathit{x}}_k,{\mathit{u}}_k\right)$ at time $t_k$ [40], PDDP also needs to consider the quadratic approximation of the value function with respect to the parameter $\mathit{\theta }$, which yields

$$\begin{array}{cc}\hfill V\left({\mathit{x}}_k+\delta {\mathit{x}}_k;\mathit{\theta }+\delta \mathit{\theta }\right)\approx & V\left({\mathit{x}}_k;\mathit{\theta }\right)+V_{\mathit{x},k}^{\mathrm{T}}\delta {\mathit{x}}_k+V_{\theta ,k}^{\mathrm{T}}\delta \mathit{\theta }\hfill \\ \hfill & +\frac{1}{2}{\left[\begin{array}{c}\delta {\mathit{x}}_k\\ \delta \mathit{\theta }\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{cc}{V}_{\mathit{x}\mathit{x},k}\hfill & V_{\mathit{x}\mathit{\theta },k}\hfill \\ V_{\mathit{\theta }\mathit{x},k}\hfill & V_{\mathit{\theta }\mathit{\theta },k}\hfill \end{array}\right]\left[\begin{array}{c}\delta {\mathit{x}}_k\\ \delta \mathit{\theta }\end{array}\right]\hfill \end{array}$$

(9)

where the deviations of the state, control and parameters at $t_k$ from the nominal trajectory are denoted by $\delta {\mathit{x}}_k$, $\delta {\mathit{u}}_k$ and $\delta \mathit{\theta }$. $V_{\mathit{x},k}$ and $V_{\theta ,k}$ are the gradient vectors, and $V_{\mathit{x}\mathit{x},k}$, $V_{\mathit{x}\mathit{\theta },k}$, $V_{\mathit{\theta }\mathit{x},k}$ and $V_{\mathit{\theta }\mathit{\theta },k}$ are the derivatives of $V\left({\mathit{x}}_k;\mathit{\theta }\right)$ at time instant k.

Similarly, the quadratic approximation of $Q\left({\mathit{x}}_k,{\mathit{u}}_k;\mathit{\theta }\right)$, defined in Equation (8), is approximated as

$$\begin{array}{cc}\hfill & Q({\mathit{x}}_k+\delta {\mathit{x}}_k,{\mathit{u}}_k+\delta {\mathit{u}}_k;\mathit{\theta }+\delta \mathit{\theta })\approx Q({\mathit{x}}_k,{\mathit{u}}_k;\mathit{\theta })+Q_{\mathit{x},k}^{\mathrm{T}}\delta {\mathit{x}}_k+Q_{\mathit{u},k}^{\mathrm{T}}\delta {\mathit{u}}_k+Q_{\mathit{\theta },k}^{\mathrm{T}}\delta \mathit{\theta }\hfill \\ \hfill & +\frac{1}{2}{\left[\begin{array}{c}\delta {\mathit{x}}_k\hfill \\ \delta {\mathit{u}}_k\hfill \\ \delta \mathit{\theta }\hfill \end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{Q}_{\mathit{x}\mathit{x},k}\hfill & Q_{\mathit{x}\mathit{u},k}\hfill & Q_{\mathit{x}\mathit{\theta },k}\hfill \\ Q_{\mathit{u}\mathit{x},k}\hfill & Q_{\mathit{u}\mathit{u},k}\hfill & Q_{\mathit{u}\mathit{\theta },k}\hfill \\ Q_{\mathit{\theta }\mathit{x},k}\hfill & Q_{\mathit{\theta }\mathit{u},k}\hfill & Q_{\mathit{\theta }\mathit{\theta },k}\hfill \end{array}\right]\left[\begin{array}{c}\delta {\mathit{x}}_k\hfill \\ \delta {\mathit{u}}_k\hfill \\ \delta \mathit{\theta }\hfill \end{array}\right]\hfill \end{array}$$

(10)

Combining the terms of the same order of $\delta \mathit{x}$, $\delta \mathit{u}$, and $\delta \mathit{\theta }$ obtains the derivation of $Q\left({\mathit{x}}_k,{\mathit{u}}_k;\mathit{\theta }\right)$ as

$$\begin{array}{cc}\hfill & Q_{\mathit{x},k}=l_{\mathit{x},k}+{\mathit{f}}_{\mathit{x},k}^{\mathrm{T}}{V}_{\mathit{x},k+1}\hfill \\ \hfill & Q_{\mathit{u},k}=l_{\mathit{u},k}+{\mathit{f}}_{\mathit{u},k}^{\mathrm{T}}{V}_{\mathit{x},k+1}\hfill \\ \hfill & Q_{\mathit{\theta },k}=l_{\mathit{\theta },k}+V_{\mathit{\theta },k+1}+{\mathit{f}}_{\mathit{\theta },k}^{\mathrm{T}}{V}_{\mathit{x},k+1}\hfill \\ \hfill & Q_{\mathit{x}\mathit{x},k}=l_{\mathit{x}\mathit{x},k}+{\mathit{f}}_{\mathit{x},k}^{\mathrm{T}}{V}_{\mathit{x}\mathit{x},k+1}{\mathit{f}}_{\mathit{x},k}\hfill \\ \hfill & Q_{\mathit{u}\mathit{u},k}=l_{\mathit{u}\mathit{u},k}+{\mathit{f}}_{\mathit{u},k}^{\mathrm{T}}{V}_{\mathit{x}\mathit{x},k+1}{\mathit{f}}_{\mathit{u},k}\hfill \\ \hfill & Q_{\mathit{\theta }\mathit{\theta },k}=l_{\mathit{\theta }\mathit{\theta },k}+V_{\mathit{\theta }\mathit{\theta },k+1}+{\mathit{f}}_{\mathit{\theta },k}^{\mathrm{T}}{V}_{\mathit{x}\mathit{x},k+1}{\mathit{f}}_{\mathit{\theta },k}+{\mathit{f}}_{\mathit{\theta },k}^{\mathrm{T}}{V}_{\mathit{x}\mathit{\theta },k+1}+V_{\mathit{\theta }\mathit{x},k+1}{\mathit{f}}_{\mathit{\theta },k}\hfill \\ \hfill & Q_{\mathit{x}\mathit{u},k}=l_{\mathit{x}\mathit{u},k}+{\mathit{f}}_{\mathit{x},k}^{\mathrm{T}}{V}_{\mathit{x}\mathit{x},k+1}{\mathit{f}}_{\mathit{u},k}=Q_{\mathit{u}\mathit{x},k}^{\mathrm{T}}\hfill \\ \hfill & Q_{\mathit{x}\mathit{\theta },k}=l_{\mathit{x}\mathit{\theta },k}+{\mathit{f}}_{\mathit{x},k}^{\mathrm{T}}{V}_{\mathit{x}\mathit{x},k+1}{\mathit{f}}_{\mathit{\theta },k}+{\mathit{f}}_{\mathit{x},k}^{\mathrm{T}}{V}_{\mathit{x}\mathit{\theta },k+1}=Q_{\mathit{\theta }\mathit{x},k}^{\mathrm{T}}\hfill \\ \hfill & Q_{\mathit{u}\mathit{\theta },k}=l_{\mathit{u}\mathit{\theta },k}+{\mathit{f}}_{\mathit{u},k}^{\mathrm{T}}{V}_{\mathit{x}\mathit{x},k+1}{\mathit{f}}_{\mathit{\theta },k}+{\mathit{f}}_{\mathit{u},k}^{\mathrm{T}}{V}_{\mathit{u}\mathit{\theta },k+1}=Q_{\mathit{\theta }\mathit{u},k}^{\mathrm{T}}\hfill \end{array}$$

(11)

With regard to the control deviation $\delta {\mathit{u}}_k$, the second-order expansion of $Q({\mathit{x}}_k,{\mathit{u}}_k;\mathit{\theta })$ is a quadratic function, and then one can utilize the optimality condition to find $\delta {\mathit{u}}_k^{*}$, i.e.,

$$\begin{array}{c}\hfill \delta {\mathit{u}}_k^{*}={\mathit{k}}_k+{\mathit{K}}_k\delta {\mathit{x}}_k+{\mathit{M}}_k\delta {\mathit{\theta }}_k\end{array}$$

(12)

where

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\mathit{k}}_k& =-Q_{\mathit{u}\mathit{u},k}^{-1}{Q}_{\mathit{u},k}\hfill \\ \hfill {\mathit{K}}_k& =-Q_{\mathit{u}\mathit{u},k}^{-1}{Q}_{\mathit{u}\mathit{x},k}\hfill \\ \hfill {\mathit{M}}_k& =-Q_{\mathit{u}\mathit{u},k}^{-1}{Q}_{\mathit{u}\mathit{\theta },k}\hfill \end{array}\end{array}$$

(13)

and the feedforward term ${\mathit{k}}_k$ and the feedback term ${\mathit{K}}_k$ are the same as in the original DDP method. However, an additional feedback term ${\mathit{M}}_k$ is added to optimize $\mathit{\theta }$ in PDDP.

Since the parameter $\mathit{\theta }$ is time-independent, i.e., it does not vary with time, it only needs to be updated at the time instant $k=1$. In other words, the expression for the optimal parameter increment $\delta {\mathit{\theta }}^{*}$ can be obtained under the deviation condition of $\delta {\mathit{x}}_1=0$ in Equation (9) as follows

$$\begin{array}{c}\hfill \delta {\mathit{\theta }}^{*}=-V_{\mathit{\theta }\mathit{\theta },1}^{-1}{V}_{\mathit{\theta },1}\end{array}$$

(14)

To ensure the convergence of the PDDP algorithm, the feedforward terms ${\mathit{k}}_k$ of $\delta {\mathit{u}}_k^{*}$ and $-V_{\mathit{\theta }\mathit{\theta },1}^{-1}{V}_{\mathit{\theta },1}$ of $\delta {\mathit{\theta }}^{*}$ need to be scaled by the damping coefficient ${\alpha }_l$, where ${\alpha }_l$ can be determined by the linear search method [42], and 

$$\begin{array}{cc}\hfill \delta {\mathit{u}}_k^{*}& ={\alpha }_l{\mathit{k}}_k+{\mathit{k}}_k\delta {\mathit{x}}_k+{\mathit{M}}_k\delta {\mathit{\theta }}_k\hfill \end{array}$$

(15)

$$\begin{array}{cc}\hfill \delta {\mathit{\theta }}^{*}& =-{\alpha }_l{V}_{\mathit{\theta }\mathit{\theta },1}^{-1}{V}_{\mathit{\theta },1}\hfill \end{array}$$

(16)

Substituting the optimal control variables (12) into the Taylor expansion of $Q({\mathit{x}}_k,{\mathit{u}}_k;\mathit{\theta })$ and grouping the terms of $\delta {\mathit{x}}_k$ and $\delta \mathit{\theta }$ with the same order, we can get

$$\begin{array}{cc}\hfill V\left({\mathit{x}}_k;\mathit{\theta }\right)& =Q\left({\mathit{x}}_k,{\mathit{u}}_k;\mathit{\theta }\right)-(\frac{1}{2}{\alpha }_l^2-{\alpha }_l)Q_{\mathit{u},k}^{\mathrm{T}}{Q}_{\mathit{u}\mathit{u},k}^{-1}{Q}_{\mathit{u},k}\hfill \\ \hfill V_{\mathit{x},k}& =Q_{\mathit{x},k}-Q_{\mathit{x}\mathit{u},k}{Q}_{\mathit{u}\mathit{u},k}^{-1}{Q}_{\mathit{u},k}\hfill \\ \hfill V_{\mathit{\theta },k}& =Q_{\mathit{\theta },k}-Q_{\mathit{\theta }\mathit{u},k}{Q}_{\mathit{u}\mathit{u},k}^{-1}{Q}_{\mathit{u},k}\hfill \\ \hfill V_{\mathit{x}\mathit{x},k}& =Q_{\mathit{x}\mathit{x},k}-Q_{\mathit{x}\mathit{u},k}{Q}_{\mathit{u}\mathit{u},k}^{-1}{Q}_{\mathit{u}\mathit{x},k}\hfill \\ \hfill V_{\mathit{x}\mathit{\theta },k}& =Q_{\mathit{x}\mathit{\theta },k}-Q_{\mathit{x}\mathit{u},k}{Q}_{\mathit{u}\mathit{u},k}^{-1}{Q}_{\mathit{u}\mathit{\theta },k}=V_{\mathit{\theta }\mathit{x},k}^{\mathrm{T}}\hfill \\ \hfill V_{\mathit{\theta }\mathit{\theta },k}& =Q_{\mathit{\theta }\mathit{\theta },k}-Q_{\mathit{\theta }\mathit{u},k}{Q}_{\mathit{u}\mathit{u},k}^{-1}{Q}_{\mathit{u}\mathit{\theta },k}\hfill \end{array}$$

(17)

Before running the PDDP algorithm, in order to generate the nominal trajectory, an initial guess of the control inputs should be given. Then, the terminal value function is initialized using the terminal objective function $V\left({\mathit{x}}_N;\mathit{\theta }\right)=\varphi ({\mathit{x}}_N;\mathit{\theta })$, $V_{\mathit{x},N}={\varphi }_{\mathit{x},N}$, $V_{\mathit{x}\mathit{x},N}={\varphi }_{\mathit{x}\mathit{x},N}$, $V_{\mathit{\theta },N}={\varphi }_{\mathit{\theta },N}$, $V_{\mathit{\theta }\mathit{\theta },N}={\varphi }_{\mathit{\theta }\mathit{\theta },N}$, $V_{\mathit{x}\mathit{\theta },N}={\varphi }_{\mathit{x}\mathit{\theta },N}=V_{\mathit{\theta }\mathit{x},N}^{\mathrm{T}}$. Then, the optimal control correction $\delta {\mathit{u}}_k$ is computed in reverse time using Equation (12), while the optimal parameter increment $\delta \mathit{\theta }$ is computed at time $k=1$ using Equation (14). Briefly, the one-step control update and one-iteration parameter update used to generate a new trajectory are, respectively,

$$\begin{array}{cc}\hfill {\mathit{u}}_k& ={\mathit{u}}_k+\delta {\mathit{u}}_k^{*}={\mathit{u}}_k+{\alpha }_l{\mathit{k}}_k+{\mathit{k}}_k\delta {\mathit{x}}_k+{\mathit{M}}_k\delta {\mathit{\theta }}_k\hfill \\ \hfill \mathit{\theta }& =\mathit{\theta }+\delta {\mathit{\theta }}^{*}=\mathit{\theta }-{\alpha }_l{V}_{\mathit{\theta }\mathit{\theta },1}^{-1}{V}_{\mathit{\theta },1}\hfill \end{array}$$

(18)

Once the backward process computes the optimal control update and the optimal parameter update, the forward process is triggered to generate a nominal trajectory. The PDDP method iteratively executes the backward and forward processes until the difference in the cost functions between the two iterations is below a preset amount

$$\left|J^{\left(n\right)}-J^{(n-1)}\right|<ϵ$$

(19)

in which the constant $ϵ>0$ has a relatively small value and $J^{\left(n\right)}$ denotes the objective function of the nth iteration. As long as the termination condition is fulfilled, the iterative process of the algorithm is terminated and the optimal state and control are given.

3. Constrained Parameterized Differential Dynamic Programming for Spatial–Temporal Optimization

The purpose of this section is to determine the trajectory that passes through multiple waypoints and achieves the optimal flight time between waypoints. For easy understanding, this section defines the flight time in each trajectory segment between waypoints as ${\theta }_j=t_{w_j}-t_{w_{j-1}}$, which is used to denote the unknown parameter in the PDDP algorithm. In this section, the C-PDDP algorithm that considers the spatial–temporal trajectory optimization problem with waypoint constraints in the free-time framework is derived in detail. We will first use the penalty function method based on the original PDDP algorithm to deal with the nonlinear waypoint path constraints, and then introduce how to optimize the parameterized time dynamically in an iterative approach to obtain the optimal flight time for each trajectory segment.

The PDDP algorithm is used to solve problem CTO₁ (6); then, the parameterized formulation of problem (6) is in the form of constrained trajectory optimization problem 2, denoted as CTO₂, which has the following form

$$\begin{array}{cc}\hfill ({\mathit{u}}_k^{*},{\theta }_j^{*})=\underset{\mathit{U};\Theta }{min}& \varphi \left({\mathit{x}}_N;t_N\right)+\sum _{j=1}^W\left[\sum _{k=t_{w_{j-1}}}^{t_{w_j}}{l}_j\left({\mathit{x}}_k,{\mathit{u}}_k;{\theta }_j\right)\right]\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\mathit{x}}_{k+1}=\mathit{f}\left({\mathit{x}}_k,{\mathit{u}}_k;{\theta }_j\right)\hfill \\ \hfill & {\parallel {\mathit{x}}_k-{\mathit{p}}_{w_j}\parallel }_2=0\hfill \\ \hfill & {\mathit{u}}_{min}\le {\mathit{u}}_k\le {\mathit{u}}_{max}\hfill \\ \hfill & {\theta }_j\in \mathcal{T}\hfill \end{array}$$

(20)

where $\mathit{U}={\left[{\mathit{u}}_1,\dots ,{\mathit{u}}_i,\dots ,{\mathit{u}}_N\right]}^{\mathrm{T}}$, $\Theta ={\left[{\theta }_1,\dots ,{\theta }_j,\dots ,{\theta }_W\right]}^{\mathrm{T}}$.

The multiple waypoint constraint ${\parallel {\mathit{x}}_k-{\mathit{p}}_{w_j}\parallel }_2^2=0$ in problem (20) only requires this path constraint to be considered at the corresponding time instant $t_{w_j}$ of the waypoint. This constraint is an equational constraint that represents forcing the trajectory to pass through the waypoint ${\mathit{p}}_{w_j}$ and tolerate a 0 error, which is usually impractical in numerical optimization algorithms. Therefore, in this section, the penalty function method is used to impose path constraints at each waypoint and the waypoint constraints are considered in the cost function so that the trajectory is able to pass through waypoints. The PDDP algorithm with path constraints is derived according to Bellman’s principle of optimality, which gives the updated expression of $V\left({\mathit{x}}_k;\mathit{\theta }\right)$ and $Q\left({\mathit{x}}_k,{\mathit{u}}_k;\mathit{\theta }\right)$, thus constraining the trajectory to pass through the waypoints. On this basis, for the problem of a non-optimal preset flight time, the updating formula of unknown parameters in PDDP is utilized at the corresponding instants of the waypoints in the trajectory according to the value function and its derivatives, which derives the optimal deviation of flight times for each trajectory segment and obtains the optimal flight time for each segment by iterative calculation.

The idea of solving this problem can be illustrated by Figure 1.

3.1. Constrained Parameterized Differential Dynamic Programming

In this subsection, the PDDP is extended to nonlinear path constraints ${\left({\mathit{x}}_k-{\mathit{p}}_{w_j}\right)}^{\mathrm{T}}$ by the penalty function method [44,45,46], after which control clamping ${\mathit{u}}_{min}\le {\mathit{u}}_k\le {\mathit{u}}_{max}$ is utilized to deal with the control constraints. The flight time of each trajectory segment is fixed in this subsection.

By adding the quadratic penalty function of the equality constraints ${\left({\mathit{x}}_k-{\mathit{p}}_{w_j}\right)}^{\mathrm{T}}$ at each waypoint to the original cost function (7), the augmented parameterized cost function can be obtained as

$$J=\sum _{k=0}^{N-1}l\left({\mathit{x}}_k,{\mathit{u}}_k;\mathit{\theta }\right)+\varphi ({\mathit{x}}_N;\mathit{\theta })+\sum _{j=0}^w\frac{1}{2}{\left({\mathit{x}}_{w_j}-{\mathit{p}}_{w_j}\right)}^{\mathrm{T}}{\mu }_k^j\left({\mathit{x}}_{w_j}-{\mathit{p}}_{w_j}\right)$$

(21)

where ${\mu }_k^j$, which satisfies the condition ${\mu }_k^j>0$, denotes the penalty coefficient for the j-th waypoint constraint.

By generalizing the cost function (21), the value function is modified as

$$\begin{array}{cc}\hfill \widehat{V}\left({\mathit{x}}_k;\mathit{\theta }\right)=& \varphi ({\mathit{x}}_N;\mathit{\theta })+\sum _{i=k}^{N-1}l\left({\mathit{x}}_k,{\mathit{u}}_k^{*};\mathit{\theta }\right)\hfill \\ \hfill & +\sum _{j=0}^w\frac{1}{2}{\left({\mathit{x}}_{w_j}-{\mathit{p}}_{w_j}\right)}^{\mathrm{T}}{\mu }_k^j\left({\mathit{x}}_{w_j}-{\mathit{p}}_{w_j}\right)\hfill \end{array}$$

(22)

According to Bellman’s optimality principle, the action value function at the corresponding time instant $t_{w_j}$ for each waypoint is modified to be

$$\begin{array}{cc}\hfill \widehat{Q}\left({\mathit{x}}_{w_j},{\mathit{u}}_{w_j};{\theta }_j\right)=& \widehat{V}\left({\mathit{x}}_{w_j+1};{\theta }_j\right)+l\left({\mathit{x}}_{w_j},{\mathit{u}}_{w_j};{\theta }_j\right)\hfill \\ \hfill & +\frac{1}{2}{\left({\mathit{x}}_{w_j}-{\mathit{p}}_{w_j}\right)}^{\mathrm{T}}{\mu }_{w_j}^j\left({\mathit{x}}_{w_j}-{\mathit{p}}_{w_j}\right)\hfill \end{array}$$

(23)

Similar to original PDDP, we consider small perturbations around the nominal trajectory and then we approximate the value function and action value function with a second-order Taylor series. The second-order Taylor expansion of the value function and action value function at the corresponding time instant $t_{w_j}$ at the waypoint is given by

$$\begin{array}{cc}\hfill \widehat{V}\left({\mathit{x}}_{w_j}+\delta {\mathit{x}}_{w_j};{\theta }_j+\delta {\theta }_j\right)\approx & \widehat{V}\left({\mathit{x}}_{w_j};{\theta }_j\right)+\left[\begin{array}{cc}{\widehat{V}}_{\mathit{x},w_j}^{\mathrm{T}}\hfill & {\widehat{V}}_{\theta ,w_j}^{\mathrm{T}}\hfill \end{array}\right]\left[\begin{array}{c}\delta {\mathit{x}}_k\hfill \\ \delta {\theta }_j\hfill \end{array}\right]\hfill \\ \hfill & +\frac{1}{2}{\left[\begin{array}{c}\delta {\mathit{x}}_{w_j}\\ \delta {\theta }_j\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{cc}{\widehat{V}}_{\mathit{x}\mathit{x},w_j}\hfill & {\widehat{V}}_{\mathit{x}\theta ,w_j}\hfill \\ {\widehat{V}}_{\theta \mathit{x},w_j}\hfill & {\widehat{V}}_{\theta \theta ,w_j}\hfill \end{array}\right]\left[\begin{array}{c}\delta {\mathit{x}}_{w_j}\\ \delta {\theta }_j\end{array}\right]\hfill \end{array}$$

(24)

$$\begin{array}{cc}\hfill & \widehat{Q}({\mathit{x}}_k+\delta {\mathit{x}}_k,{\mathit{u}}_k+\delta {\mathit{u}}_k;{\theta }_j+\delta {\theta }_j)\approx \widehat{Q}({\mathit{x}}_k,{\mathit{u}}_k;{\theta }_j)+\left[\begin{array}{ccc}{\widehat{Q}}_{\mathit{x},w_j}^{\mathrm{T}}\hfill & {\widehat{Q}}_{\mathit{u},w_j}^{\mathrm{T}}\hfill & {\widehat{Q}}_{\theta ,w_j}^{\mathrm{T}}\hfill \end{array}\right]\left[\begin{array}{c}\delta {\mathit{x}}_k\hfill \\ \delta {\mathit{u}}_k\hfill \\ \delta {\theta }_j\hfill \end{array}\right]\hfill \\ \hfill & +\frac{1}{2}{\left[\begin{array}{c}\delta {\mathit{x}}_k\hfill \\ \delta {\mathit{u}}_k\hfill \\ \delta {\theta }_j\hfill \end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{\widehat{Q}}_{\mathit{x}\mathit{x},w_j}\hfill & {\widehat{Q}}_{\mathit{x}\mathit{u},w_j}\hfill & {\widehat{Q}}_{\mathit{x}\theta ,w_j}\hfill \\ {\widehat{Q}}_{\mathit{u}\mathit{x},w_j}\hfill & {\widehat{Q}}_{\mathit{u}\mathit{u},w_j}\hfill & {\widehat{Q}}_{\mathit{u}\theta ,w_j}\hfill \\ {\widehat{Q}}_{\theta \mathit{x},w_j}\hfill & {\widehat{Q}}_{\theta \mathit{u},w_j}\hfill & {\widehat{Q}}_{\theta \theta ,w_j}\hfill \end{array}\right]\left[\begin{array}{c}\delta {\mathit{x}}_k\hfill \\ \delta {\mathit{u}}_k\hfill \\ \delta {\theta }_j\hfill \end{array}\right]\hfill \end{array}$$

(25)

Then, we can calculate derivatives of the modified action value function, i.e., the gradient and Hessian matrix as

$$\begin{array}{cc}\hfill {\widehat{Q}}_{\mathit{x},w_j}& =l_{\mathit{x},w_j}+{\mu }_k^j\left({\mathit{x}}_k-{\mathit{p}}_{w_j}\right)+{\mathit{f}}_{\mathit{x},w_j}^{\mathrm{T}}{V}_{\mathit{x},w_j+1}\hfill \\ \hfill {\widehat{Q}}_{\mathit{u},w_j}& =l_{\mathit{u},w_j}+{\mathit{f}}_{\mathit{u},w_j}^{\mathrm{T}}{V}_{\mathit{x},w_j+1}\hfill \\ \hfill {\widehat{Q}}_{\theta ,w_j}& =l_{\theta ,w_j}+V_{\theta ,w_j+1}+{\mathit{f}}_{\theta ,w_j}^{\mathrm{T}}{V}_{\mathit{x},w_j+1}\hfill \\ \hfill {\widehat{Q}}_{\mathit{x}\mathit{x},w_j}& =l_{\mathit{x}\mathit{x},w_j}+{\mu }_k^j{\mathbf{I}}_n+{\mathit{f}}_{\mathit{x},w_j}^{\mathrm{T}}{V}_{\mathit{x}\mathit{x},w_j+1}{\mathit{f}}_{\mathit{x},w_j}\hfill \\ \hfill {\widehat{Q}}_{\mathit{u}\mathit{u},w_j}& =l_{\mathit{u}\mathit{u},w_j}+{\mathit{f}}_{\mathit{u},w_j}^{\mathrm{T}}{V}_{\mathit{x}\mathit{x},w_j+1}{\mathit{f}}_{\mathit{u},w_j}\hfill \\ \hfill {\widehat{Q}}_{\theta \theta ,w_j}& =l_{\theta \theta ,w_j}+V_{\theta \theta ,w_j+1}+{\mathit{f}}_{\theta ,w_j}^{\mathrm{T}}{V}_{\mathit{x}\mathit{x},w_j+1}{\mathit{f}}_{\theta ,w_j}\hfill \\ \hfill & +{\mathit{f}}_{\theta ,w_j}^{\mathrm{T}}{V}_{\mathit{x}\theta ,w_j+1}+V_{\theta \mathit{x},w_j+1}{\mathit{f}}_{\theta ,w_j}\hfill \\ \hfill {\widehat{Q}}_{\mathit{x}\mathit{u},w_j}& =l_{\mathit{x}\mathit{u},w_j}+{\mathit{f}}_{\mathit{x},w_j}^{\mathrm{T}}{V}_{\mathit{x}\mathit{x},w_j+1}{\mathit{f}}_{\mathit{u},w_j}=Q_{\mathit{u}\mathit{x},w_j}^{\mathrm{T}}\hfill \\ \hfill {\widehat{Q}}_{\mathit{x}\theta ,w_j}& =l_{\mathit{x}\theta ,w_j}+{\mathit{f}}_{\mathit{x},w_j}^{\mathrm{T}}{V}_{\mathit{x}\mathit{x},w_j+1}{\mathit{f}}_{\theta ,w_j}+{\mathit{f}}_{\mathit{x},w_j}^{\mathrm{T}}{V}_{\mathit{x}\theta ,w_j+1}=Q_{\theta \mathit{x},w_j}^{\mathrm{T}}\hfill \\ \hfill {\widehat{Q}}_{\mathit{u}\theta ,w_j}& =l_{\mathit{u}\theta ,w_j}+{\mathit{f}}_{\mathit{u},w_j}^{\mathrm{T}}{V}_{\mathit{x}\mathit{x},w_j+1}{\mathit{f}}_{\theta ,w_j}+{\mathit{f}}_{\mathit{u},w_j}^{\mathrm{T}}{V}_{\mathit{u}\theta ,w_j+1}=Q_{\theta \mathit{u},w_j}^{\mathrm{T}}\hfill \end{array}$$

(26)

From Equation (26), it can be seen that since the multiple waypoint state constraints are only related to the state ${\mathit{x}}_{w_j}$; the derivatives of the action value functions ${\widehat{Q}}_{\mathit{x},w_j}$ and ${\widehat{Q}}_{\mathit{x}\mathit{x},w_j}$ are therefore modified.

After obtaining the updated derivatives of the action value function, the control correction $\delta {\mathit{u}}_{w_j}$ is updated to

$$\begin{array}{c}\hfill \delta {\widehat{\mathit{u}}}_{w_j}^{*}={\widehat{\mathit{k}}}_{w_j}+{\widehat{\mathit{k}}}_{w_j}\delta {\mathit{x}}_{w_j}+{\widehat{\mathit{M}}}_{w_j}\delta {\mathit{\theta }}_{w_j}\end{array}$$

(27)

where

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\widehat{\mathit{k}}}_{w_j}& =-{\widehat{Q}}_{\mathit{u}\mathit{u},w_j}^{-1}{\widehat{Q}}_{\mathit{u},w_j}\hfill \\ \hfill {\widehat{\mathit{k}}}_{w_j}& =-{\widehat{Q}}_{\mathit{u}\mathit{u},w_j}^{-1}{\widehat{Q}}_{\mathit{u}\mathit{x},w_j}\hfill \\ \hfill {\widehat{\mathit{M}}}_{w_j}& =-{\widehat{Q}}_{\mathit{u}\mathit{u},w_j}^{-1}{\widehat{Q}}_{\mathit{u}\mathit{\theta },w_j}\hfill \end{array}\end{array}$$

(28)

The derivatives of the original value function (17) ought to be substituted with the modified values obtained in Equation (26); then, the value function and its derivatives should be corrected to

$$\begin{array}{cc}\hfill \widehat{V}\left({\mathit{x}}_{w_j};{\theta }_j\right)& =\widehat{Q}\left({\mathit{x}}_{w_j},{\mathit{u}}_{w_j};{\theta }_j\right)-(\frac{1}{2}{\alpha }_l^2-{\alpha }_l){\widehat{Q}}_{\mathit{u},w_j}^{\mathrm{T}}{\widehat{Q}}_{\mathit{u}\mathit{u},w_j}^{-1}{\widehat{Q}}_{\mathit{u},w_j}\hfill \\ \hfill {\widehat{V}}_{\mathit{x},w_j}& ={\widehat{Q}}_{\mathit{x},w_j}-{\widehat{Q}}_{\mathit{x}\mathit{u},w_j}{\widehat{Q}}_{\mathit{u}\mathit{u},w_j}^{-1}{\widehat{Q}}_{\mathit{u},w_j}\hfill \\ \hfill {\widehat{V}}_{\theta ,w_j}& ={\widehat{Q}}_{\theta ,w_j}-{\widehat{Q}}_{\theta \mathit{u},w_j}{\widehat{Q}}_{\mathit{u}\mathit{u},w_j}^{-1}{\widehat{Q}}_{\mathit{u},w_j}\hfill \\ \hfill {\widehat{V}}_{\mathit{x}\mathit{x},w_j}& ={\widehat{Q}}_{\mathit{x}\mathit{x},w_j}-{\widehat{Q}}_{\mathit{x}\mathit{u},w_j}{\widehat{Q}}_{\mathit{u}\mathit{u},w_j}^{-1}{\widehat{Q}}_{\mathit{u}\mathit{x},w_j}\hfill \\ \hfill {\widehat{V}}_{\mathit{x}\theta ,w_j}& ={\widehat{Q}}_{\mathit{x}\theta ,w_j}-{\widehat{Q}}_{\mathit{x}\mathit{u},w_j}{\widehat{Q}}_{\mathit{u}\mathit{u},w_j}^{-1}{\widehat{Q}}_{\mathit{u}\theta ,w_j}={\widehat{V}}_{\theta \mathit{x},w_j}^{\mathrm{T}}\hfill \\ \hfill {\widehat{V}}_{\theta \theta ,w_j}& ={\widehat{Q}}_{\theta \theta ,w_j}-{\widehat{Q}}_{\theta \mathit{u},w_j}{\widehat{Q}}_{\mathit{u}\mathit{u},w_j}^{-1}{\widehat{Q}}_{\mathit{u}\theta ,w_j}\hfill \end{array}$$

(29)

It is important to point out again that the above update to the value function and the action value function occurs only at the time instants corresponding to the waypoints, and at other time instants, the gradient and Hessian matrix of the action value function are the same as in Equation (17).

For ${\mathit{u}}_{min}\le {\mathit{u}}_k\le {\mathit{u}}_{max}$ of each time instant, the control is set in the region as

$$\begin{array}{cc}\hfill {\mathit{u}}_k& =max(min({\mathit{u}}_k+{\widehat{\mathit{k}}}_k+{\widehat{\mathit{k}}}_k\delta {\mathit{x}}_k+{\widehat{\mathit{M}}}_k\delta {\mathit{\theta }}_k,{\mathit{u}}_{max}),{\mathit{u}}_{min})\hfill \end{array}$$

(30)

3.2. Constrained Parameterized Differential Dynamic Programming for Spatial–Temporal Optimization

In the presence of multiple waypoint constraints, the flight trajectory can be constrained to the necessary waypoints through the penalty function method. However, the use of predefined fixed flight times between multiple trajectory segments usually results in a non-optimal trajectory. According to Equation (14), the original PDDP algorithm obtains the optimal parameter variations by applying the first-order optimality condition to the value function $V_1$ at the initial time instant. In the case of multiple waypoints, the flight time needs to be optimized separately for each trajectory segment to acquire the time-optimal flight trajectory. Therefore, C-PDDP uses the optimality condition of $V\left({\mathit{x}}_k;\mathit{\theta }\right)$ at each corresponding time instant of waypoints. Then, we can acquire the optimal flight time deviation of the trajectory segment, thus achieving spatial–temporal trajectory optimization between multiple waypoints through iterative optimization.

In this subsection, the constrained PDDP algorithm from the previous subsection is incorporated into the free-time framework, and an algorithm that will completely solve the problem CTO₁ is derived. Considering the existence of multiple trajectory segments to optimize the flight time for each segment, the flight time parameter ${\theta }_j$ is updated at the intersection of trajectory segments, which is the discrete time instant of the trajectory corresponding to the waypoint, to optimize the flight time of each trajectory segment. The core idea of solving the free-time problem is to update the flight time by determining the time increment at each iteration.

The second-order Taylor expansion of the value function at the waypoint $w_j$ is given in Equation (24); thus, the deviation in the value function at waypoint $w_j$ can be defined as $\Delta V\left(x_{w_j};{\theta }_j\right)=\widehat{V}\left({\mathit{x}}_{w_j}+\delta {\mathit{x}}_{w_j};{\theta }_j+\delta {\theta }_j\right)-\widehat{V}\left({\mathit{x}}_{w_j};{\theta }_j\right)$, i.e.,

$$\begin{array}{cc}\hfill \Delta V\left(x_{w_j};{\theta }_j\right)=& {\widehat{V}}_{\mathit{x},\theta }^{\mathrm{T}}\delta {\mathit{x}}_{w_j}+{\widehat{V}}_{\theta ,w_j}^{\mathrm{T}}\delta {\theta }_j\hfill \\ \hfill & +\frac{1}{2}{\left[\begin{array}{c}\delta {\mathit{x}}_{w_j}\\ \delta {\theta }_j\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{cc}{\widehat{V}}_{\mathit{x}\mathit{x},w_j}\hfill & {\widehat{V}}_{\mathit{x}\theta ,w_j}\hfill \\ {\widehat{V}}_{\theta \mathit{x},w_j}\hfill & {\widehat{V}}_{\theta \theta ,w_j}\hfill \end{array}\right]\left[\begin{array}{c}\delta {\mathit{x}}_{w_j}\\ \delta {\theta }_j\end{array}\right]\hfill \end{array}$$

(31)

This function is quadratic with respect to the flight time increment $\delta {\theta }_j$. According to its definition, the optimal flight time increment $\delta {\theta }_j^{*}$ meets the first-order optimality condition

$$\delta {\theta }_j^{*}=\underset{\delta {\theta }_j}{min}\Delta V\left({\mathit{x}}_{w_j};{\theta }_j\right)$$

(32)

Therefore, the optimal flight time variation $\delta {\theta }_j^{*}$ can be obtained as

$${\nabla }_{\delta {\theta }_j}V\left(x_{w_j};{\theta }_j\right)=V_{\theta ,w_j}+\delta {{\mathit{x}}_{w_j}}^{\mathrm{T}}{V}_{\mathit{x}\theta ,w_j}+\delta {\theta }_j{V}_{\theta \theta ,w_j}=0$$

(33)

Solving Equation (33) yields the following expression for the optimal flight time increment $\delta {{\theta }_j}^{*}$ of the trajectory segment:

$$\begin{array}{c}\hfill \delta {\theta }_j^{*}=-V_{\theta \theta ,w_j}^{-1}(V_{\theta ,w_j}+V_{\theta \mathit{x},w_j}^{\mathrm{T}}\delta {\mathit{x}}_{w_j}^{*})\end{array}$$

(34)

The above equation shows that the increment deviation $\delta t_N^{*}$ includes a feedforward term $-V_{\theta \theta ,w_j}^{-1}{V}_{\theta ,w_j}$ and a feedback term $-V_{\theta \theta ,w_j}^{-1}{V}_{\theta \mathit{x},w_j}^{\mathrm{T}}\delta {\mathit{x}}_{w_j}$, where $V_{\theta ,w_j}$, $V_{\theta \mathit{x},w_j}$ and $V_{\theta \theta ,w_j}$ are given by Equation (29).

From Equation (34), it can be seen that unlike the condition $\delta {\mathit{x}}_1=0$ in the unknown parameter solution for a single-segment trajectory, $\delta {\mathit{x}}_{w_j}$ is not 0 at the intermediate waypoint in the multiple trajectory segments problem, and thus the update of $\delta {\theta }_j$ is related to $\delta {\mathit{x}}_{w_j}$. However, at the time instant $k=w_j$, the optimal updates of the three variables $\delta {\theta }_j$, $\delta {\mathit{u}}_{w_j}$, and $\delta {\mathit{x}}_{w_j}$ are coupled to each other, and the computation of $\delta {\mathit{x}}_{w_j}$ is determined by $\delta {\mathit{u}}_{w_j}$ via dynamic equations with a strong nonlinear nature, and therefore a closed-form solution between these three variables cannot be found. Consequently, $\delta {\mathit{x}}_{w_j}$ from the previous iteration is used in the actual computation, denoted as $\delta {\mathit{x}}_{w_j}^{\mathrm{old}}$, to approximate the solution of $\delta {\theta }_j$, i.e.,

$$\begin{array}{c}\hfill \delta {\theta }_j^{*}=-V_{\theta \theta ,w_j}^{-1}(V_{\theta ,w_j}+V_{\theta \mathit{x},w_j}^{\mathrm{T}}\delta {\mathit{x}}_{w_j}^{\mathrm{old}})\end{array}$$

(35)

Finally, the optimal time variation $\delta {\theta }_j^{*}$ is calculated at the time instants of each waypoint $k=w_j$ by Equation (35). That is, the time update used for one iteration at each trajectory segment intersection point is

$$\begin{array}{cc}\hfill {\theta }_j& ={\theta }_j+\delta {\theta }_j^{*}\hfill \\ \hfill & ={\theta }_j-V_{\theta \theta ,w_j}^{-1}{V}_{\theta ,w_j}-V_{\theta \theta ,w_j}^{-1}{V}_{\theta \mathit{x},w_j}^{\mathrm{T}}\delta {\mathit{x}}_{w_j}^{\mathrm{old}}\hfill \end{array}$$

(36)

To avoid a poor quality of the initial guesses for the flight time of each trajectory segment, which leads to C-PDDP divergence and a failure to converge to the local optimum, and to make sure that the time parameter is non-negative, we use the box constraint [47] to constrain the update of the flight time of each segment in a bounded region to ensure numerical stability

$${\theta }_j=min(max({\theta }_j+\delta {\theta }_j^{*},t_{min}),t_{max})$$

(37)

where $t_{min}$ and $t_{max}$ denote the lower and upper bounds of each flight time.

The C-PDDP algorithm uses the same initialization method as the PDDP algorithm and will not be repeated here. The updates of the value function and the state–action value function at other time instants in the C-PDDP algorithm are the same as in Equations (26) and (29), except that the time parameter needs to be updated at the waypoint $k=w_j$.

Remark 1.

Although Equation (35) provides a feasible solution scheme for $\delta {\theta }_j^{*}$, its calculation only uses an approximation of $\delta {\mathit{x}}_{w_j}$, i.e., $\delta {\theta }_j^{*}$ is not an exact solution to the original problem, and thus the equation does not guarantee that the updated time parameter is an optimal solution.

3.3. Scaling the Flight Time of Trajectory Segments

By constraining the distance cost between waypoint ${\mathit{p}}_{w_j}$ and the specific point ${\mathit{x}}_{w_j}$ of the trajectory through the penalty function method and updating the flight time ${\theta }_j$ as an unknown parameter at the time instant of the waypoint, the C-PDDP algorithm is able to find the spatial–temporal optimal trajectory under the constraint of multiple waypoints. However, it should be noted that the C-PDDP algorithm obtains the optimal control update and the optimal time update after the calculation of the value function and its derivatives at each algorithm iteration, and it is also necessary to obtain a new nominal trajectory through the control update and the time update by utilizing the integration of the dynamic equations in the forward process of the algorithm. After obtaining the updated flight time ${\theta }_j$ for each segment of the trajectory, an update of the objective function and the time parameter ${\theta }_j$ contained in the dynamic equation in problem (20) is required.

The objective function and dynamics in problem (20) need to be scaled and reformulated on the time horizon to ensure that the dynamics equations and objective function in each trajectory segment are described in terms of the most recent flight time parameters obtained from the calculation. The variable ${\tau }_j=\frac{t_{w_j}-t_{w_{j-1}}}{{\theta }_j}+j-1$ is adopted to express the time deviation after scaling; then, the objective function and dynamics in problem (20) can be reformulated over a fixed time interval of unit length as

$$\begin{array}{cc}\hfill \underset{\mathit{U};\Theta }{\min }J(\mathit{U};\Theta )& =\sum _{k=1}^{N-1}{\theta }_j·l\left({\mathit{x}}_k\left({\tau }_k\right),{\mathit{u}}_k\left({\tau }_k\right)\right)+\varphi \left({\mathit{x}}_N\left({\tau }_N\right)\right)\hfill \\ \hfill s.t.{\mathit{x}}_{k+1}& ={\theta }_j\mathit{f}\left({\mathit{x}}_k\left({\tau }_k\right),{\mathit{u}}_k\left({\tau }_k\right)\right)\hfill \end{array}$$

(38)

During the solution of the problem using the C-PDDP algorithm, each iteration of the algorithm will generate a new time parameter ${\theta }_j$ for each trajectory segment.

Remark 2.

From the definition of the scaled time variable ${\tau }_j$, it can be seen that after extracting the time parameter ${\theta }_j$ of each trajectory segment, the scaled flight time length is in the range of unit length $[0,1]$. To ensure the numerical stability of the C-PDDP algorithm, a fixed number of time instant points are set in each trajectory segment, i.e., a fixed, scaled time step h is used within the deflation time interval $[0,1]$ during the actual operation of the algorithm, and thus the discrete-time dynamics can be expressed as

$$\begin{array}{cc}\hfill {\mathit{x}}_{k+1}& ={\mathit{x}}_k+{\theta }_j\mathit{f}\left({\mathit{x}}_k\left({\tau }_k\right),{\mathit{u}}_k\left({\tau }_k\right)\right)h\hfill \end{array}$$

(39)

Further, the actual time step within each trajectory segment can be dynamically adjusted according to the change in the time parameter ${\theta }_j$ during the optimization of the algorithm, which avoids using an interpolation operation due to the change in the time parameter, thus making the algorithm difficult to converge.

Remark 3.

The Hessian matrices $Q_{\mathit{u}\mathit{u},k}$ and $V_{\mathit{\theta }\mathit{\theta },w_j}$ are assumed to remain positive in the C-PDDP algorithm throughout the optimization process. Unlike the direct LM approach on $Q_{\mathit{u}\mathit{u},k}$ in the original DDP algorithm [48], the authors of [49] present a more robust approach that places the regularization on the state variables instead of the control variables:

$$\begin{array}{cc}\hfill {\widehat{V}}_{\mathit{x}\mathit{x},k}& ={\widehat{V}}_{\mathit{x}\mathit{x},k}+{\rho }_{\mu }{\mathbf{I}}_n\hfill \\ \hfill {\widehat{V}}_{\mathit{\theta }\mathit{\theta },k}& ={\widehat{V}}_{\mathit{\theta }\mathit{\theta },k}+{\rho }_{\nu }\mathbf{I}\hfill \end{array}$$

(40)

in which ${\rho }_v$ and ${\rho }_q$ are small and constant with regularization.

Remark 4.

The penalty factor is incrementally increased in the classical penalty function method to make sure that the solution converges to the optimal solution in the feasible state domain. Therefore, in every iteration, we update ${\mu }_k^j>0$ to

$${\mu }_{k,i}^j=\alpha {\mu }_{k,i-1}^j,{\mu }_{k,0}^j=\mu$$

(41)

in which ${\mu }_k^j$ in i-th iteration is ${\mu }_{k,i}^j$ in the C-PDDP method. $\alpha >1$ denotes the update rate.

Remark 5.

It should be noted in particular that the proposed algorithm still cannot handle cases where there are obstacles in the environment, and further development of the algorithm for obstacle constraints is needed, which will be a part of our future research.

We provide the pseudo-code of C-PDDP in Algorithm 1 by considering the results of the preceding subsections. The algorithm is based on the PDDP algorithm and constraining the cost of the distance of the waypoint at the corresponding time instant and updating the time parameter. The algorithm terminates when the difference in cost functions between two iterations is below a given constant.

Algorithm 1 Constrained parameterized differential dynamic programming

1: Initialize ${\mathit{U}}_0,\mu ,{\theta }_j$ 2: for $k=0,...,N-1$ do 3:    ${\mathit{x}}_{k+1}=f({\mathit{x}}_k,{\mathit{u}}_k;{\theta }_j)$ 4: end for 5: while $\left|J_i-J_{i-1}\right|\ge ϵ$ do 6:    J← (2) 7:    $V\left({\mathit{x}}_N;\mathit{\theta }\right)=\varphi ({\mathit{x}}_N;\mathit{\theta })$, $V_{\mathit{x},N}={\varphi }_{\mathit{x},N}$, $V_{\mathit{x}\mathit{x},N}={\varphi }_{\mathit{x}\mathit{x},N}$, $V_{\mathit{\theta },N}={\varphi }_{\mathit{\theta },N}$, $V_{\mathit{\theta }\mathit{\theta },N}={\varphi }_{\mathit{\theta }\mathit{\theta },N}$, $V_{\mathit{x}\mathit{\theta },N}={\varphi }_{\mathit{x}\mathit{\theta },N}=V_{\mathit{\theta }\mathit{x},N}^{\mathrm{T}}$ 8:    for $k=N-1,...,0$ do 9:      Compute derivatives of $Q\left({\mathit{x}}_k,{\mathit{u}}_k;{\theta }_j\right)$ by (11) 10:      if $k=w_j$ then 11:         Compute derivatives of modified $\widehat{Q}\left({\mathit{x}}_{w_j},{\mathit{u}}_{w_j};{\theta }_j\right)$ by (26) 12:      end if 13:      ${\widehat{\mathit{k}}}_k,{\widehat{\mathit{k}}}_k,{\widehat{\mathit{M}}}_k$← (28) 14:      Compute derivatives of $\widehat{V}\left({\mathit{x}}_k,{\mathit{u}}_k;{\theta }_j\right)$ by (29) 15:      if $k=w_j$ or $k=1$ then 16:         $\delta {\theta }_j^{*}=-V_{\theta \theta ,w_j}^{-1}(V_{\theta ,w_j}+V_{\theta \mathit{x},w_j}^{\mathrm{T}}\delta {\mathit{x}}_{w_j}^{\mathrm{old}})$ 17:      end if 18:    end for 19:    ${\theta }_j$← (37) 20:    ${\mu }_{k,i}^j=\alpha {\mu }_{k,i-1}^j$ 21:    for $k=0,...,N-1$ do 22:      ${\delta \mathit{x}}_k^{\mathrm{old}}={\mathit{x}}_k^{\mathrm{new}}-{\mathit{x}}_k$ 23:      ${\mathit{u}}_k$← (30) 24:      ${\mathit{u}}_k=max(min({\mathit{u}}_k+{\widehat{\mathit{k}}}_k+{\widehat{\mathit{k}}}_k\delta {\mathit{x}}_k+{\widehat{\mathit{M}}}_k\delta {\mathit{\theta }}_k,{\mathit{u}}_{max}),{\mathit{u}}_{min})$ 25:      ${\mathit{x}}_{k+1}^{\mathrm{new}}=f({\mathit{x}}_k,{\mathit{u}}_k;{\theta }_j)$ 26:    end for 27:    $\mathit{X}={\mathit{X}}^{\mathrm{new}}$ 28: end while

4. Application Example

In order to verify the effectiveness of the proposed C-PDDP algorithm, this section investigates a spatial–temporal optimization problem with multiple waypoint constraints and control limits in a UAV reconnaissance scenario, and compares the proposed algorithm with the fixed-time constrained DDP algorithm (C-DDP) to demonstrate the superiority of the spatial–temporal optimization of the C-PDDP algorithm. All simulations were conducted on a personal computer with i7-8570H and 8 GB RAM.

4.1. Mission Scenario and Simulation Setup

Consider a multi-point reconnaissance mission of a UAV in the horizontal plane, where the velocity of the UAV is assumed to be constant. The dynamics equations can be simplified to

$$\left\{\begin{array}{cc}\hfill \dot{x}& =Vcos\psi \hfill \\ \hfill \dot{y}& =Vsin\psi \hfill \\ \hfill \dot{\psi }& =\frac{a_y}{V}\hfill \end{array}\right.$$

(42)

The multi-point reconnaissance mission requires the UAV to pass through multiple necessary intermediate mission points ${\mathit{p}}_{w_j}$ and arrive at the end point ${\mathit{x}}_d$ while optimizing the flight time ${\theta }_j$ between the multiple waypoints that is limited to $t_{min}\le {\theta }_j\le t_{max}$. We assume that the UAV has a minimum flight energy consumption while accomplishing the mission objectives, and therefore, the quadratic cost function is defined as follows:

$$\begin{array}{cc}\hfill J& =\frac{1}{2}{\left({\mathit{x}}_N-{\mathit{x}}_d\right)}^{\mathrm{T}}{\mathbf{W}}_N\left({\mathit{x}}_N-{\mathit{x}}_d\right)\hfill \\ \hfill & +\sum _{k=0}^{N-1}\left[\frac{1}{2}{\mathit{u}}_k^{\mathrm{T}}\mathbf{R}{\mathit{u}}_k+\frac{1}{2}{({\mathit{x}}_k-{\mathit{x}}_d)}^{\mathrm{T}}{\mathbf{W}}_s({\mathit{x}}_k-{\mathit{x}}_d)\right]\hfill \end{array}$$

(43)

in which ${\mathbf{W}}_N$, $\mathbf{R}$ and ${\mathbf{W}}_s$ are the weighted diagonal matrices and ${\mathit{x}}_d={\left[x_T,y_T,{\psi }_d\right]}^T$ refers to the intended terminal state vector of the UAV.

To accelerate the convergence rate of the algorithm, we employ a penalty factor that increases with the number of iterations, i.e., ${\mu }_{k,i}^j=\alpha {\mu }_{k,i-1}^j$, ${\mu }_{k,0}^j=\mu$.

Table 1. C-PDDP algorithm parameter settings.

Parameter	Value
Terminal weighting matrix ${\mathbf{W}}_i^N$	$\mathrm{diag}\left(15{\mathbf{I}}_3\right)$
Control weighting matrix ${\mathbf{R}}_i$	1
State weighting matrix ${\mathbf{W}}_i^s$	$\mathbf{0}$
LM parameter ${\rho }_v$	${10}^{-5}$
LM parameter ${\rho }_q$	${10}^{-5}$
Damping coefficient ${\alpha }_l$	0.4
Penalty factor $\mu$	50
Number of discrete nodes N	270
Penalty factor growth rate $\alpha$	$1.2$
Stopping threshold $ϵ$	${10}^{-4}$

summarizes the parameters required for the implementation of the C-PDDP algorithm.

4.2. Performance of the C-PDDP Algorithm

In this subsection, the effectiveness of the proposed C-PDDP algorithm in handling spatial–temporal optimization with multiple waypoints is illustrated by a typical UAV reconnaissance scenario algorithm. We consider that the UAV passes through five different intermediate waypoints, and the scenario setup conditions, such as the specific state values of the UAV, the locations of the five waypoints, and the initial values and boundaries of the flight time of the trajectory segments, are listed in

Table 2. Scenario 1 parameter settings.

Parameter	Value
UAV initial parameters $(x_0,y_0,{\psi }_0)$	$(0\mathrm{m},0\mathrm{m},0^{\circ })$
Target point parameters $(x_N,y_N,{\psi }_N)$	$(450\mathrm{m},450\mathrm{m},{45}^{\circ })$
UAV flight speed V	$30\mathrm{m}/\mathrm{s}$
Lateral acceleration limitation $a_{y_{max}}$	$30{\mathrm{m}/\mathrm{s}}^2$
Waypoint 1 position ${\mathit{p}}_{w_1}$	$(80\mathrm{m},250\mathrm{m})$
Waypoint 2 position ${\mathit{p}}_{w_2}$	$(150\mathrm{m},250\mathrm{m})$
Waypoint 3 position ${\mathit{p}}_{w_3}$	$(250\mathrm{m},250\mathrm{m})$
Waypoint 4 position ${\mathit{p}}_{w_4}$	$(350\mathrm{m},250\mathrm{m})$
Waypoint 5 position ${\mathit{p}}_{w_5}$	$(420\mathrm{m},250\mathrm{m})$
Flight time initial guess for each segment ${\theta }_j$	$13.5\mathrm{s}$
Segment flight time limitations $[t_{min},t_{max}]$	$[5\mathrm{s},20\mathrm{s}]$

. The initial approximate trajectories in all five scenarios are uncontrolled trajectories, i.e., the initial guess is $a_y=0$. The UAV-related parameters and settings used in the simulation were taken from a U.S. SwitchBlade 300 UAV [50].

The results of the spatial–temporal trajectory optimization of the UAV passing through the intermediate waypoints under five waypoint scenarios are shown in Figure 2, which includes the optimized trajectories, lateral commanded acceleration, convergence of the flight time for each trajectory segment and the time history of flight path angles. As shown in the optimized trajectory results in Figure 2a, C-PDDP is able to optimize the UAV flight trajectory from an uncontrolled trajectory to pass through five different intermediate waypoints and arrive at the terminal point, which satisfies the waypoint constraints and the terminal constraints while guaranteeing the dynamic constraints of the UAV. The UAV control command, i.e., lateral acceleration, is shown in Figure 2b, from which it shows that in order to reach the waypoint, the lateral acceleration command optimized by the algorithm in all five cases undergoes one large deflection. From Figure 2d, it is evident that the C-PDDP algorithm optimizes the flight time of the trajectory segments to different results in all five cases, and realizes the flight time optimization of multiple trajectory segments. The simulation results of the above five cases show that the C-PDDP algorithm is capable of accomplishing the task of multi-segment free-time trajectory optimization with the constraints of the waypoints.

4.3. Comparison with C-DDP

In this subsection, we consider a scenario where the UAV passes through two necessary intermediate waypoints of the trajectory. The flight trajectory is divided into three trajectory segments if the initial and terminal position points of the trajectory are also accounted for, and the flight time of the UAV is free within the three trajectory segments. The initial guess is $a_y=0$, meaning that the initial nominal trajectory is uncontrolled. To illustrate the superiority of the proposed C-PDDP algorithm for spatial–temporal optimization, we perform a simulation comparison with the fixed time C-DDP algorithm in the same simulation scenario. The simulation conditions such as the initial guess and the number of discrete points are set the same for both algorithms, so that the comparison is fair. Scenario setting conditions such as the location of the waypoints and the initial value of the flight time of the trajectory segment are listed in

Table 3. Scenario 2 parameter settings.

Parameter	Value
Waypoint 1 position ${\mathit{p}}_{w_1}$	$(90\mathrm{m},180\mathrm{m})$
Waypoint 2 position ${\mathit{p}}_{w_2}$	$(360\mathrm{m},270\mathrm{m})$
Initial flight time guess for each segment ${\theta }_j$	$9\mathrm{s}$

.

The optimization results of the spatial–temporal trajectory of the UAV passing through two intermediate waypoints under the two algorithms are shown in Figure 3, which include the optimized trajectory, lateral commanded acceleration, convergence of the flight time for each trajectory segment, time history of flight path angles, and cost function convergence results. As shown in the optimized trajectory results in Figure 3a, the two algorithms are able to optimize the UAV flight trajectory from an uncontrolled trajectory to pass through two intermediate waypoints in two different directions and arrive at the terminal point, which satisfies the waypoint constraints and the terminal point constraints while guaranteeing the dynamics constraints of the UAV. However, the figure shows that the trajectory optimized by the C-PDDP algorithm is smoother, while the trajectory optimized by the C-DDP algorithm is more tortuous due to its inability to optimize the flight time between the waypoints. The UAV control command, i.e., lateral acceleration, is shown in Figure 3c, from which it shows that both algorithms optimize the lateral acceleration command with two large deflections in order to reach the waypoints. The trajectory optimized by the C-DDP algorithm has a larger lateral acceleration than the one optimized by the C-PDDP algorithm due to the long flight time, and this leads to a larger trajectory cost. From Figure 3d, it can be seen that the flight times of the three trajectory segments are, respectively, optimized from the initial value of $9\mathrm{s}$ to $7.35\mathrm{s}$, $9.50\mathrm{s}$ and $6.71\mathrm{s}$ by the C-PDDP algorithm, which completes the task of free-time optimizing.

In conclusion, the trajectory optimized by the C-PDDP algorithm has a smaller error distance between the waypoints, a smaller total flight time and a lower trajectory cost; differences in the optimization results of the two algorithms are listed in

Table 4. Comparison results of C-PDDP and C-DDP.

Algorithm	Error at the Waypoint	Total Flight Time	Energy Consumption
C-PDDP	1.06 m	23.67 s	2099.0
C-DDP	7.34 m	27.00 s	4901.9

. The results in the table indicate that the proposed C-PDDP is able to optimize the UAV trajectory passing through the necessary intermediate points in reconnaissance scenarios with multiple waypoints and optimize the flight time of each trajectory segment at the same time to ensure the spatial–temporal optimality of the trajectory.

4.4. Monte Carlo Simulation Study

In order to demonstrate the robustness of the C-PDDP algorithm to initial control sequence guesses, we generated lateral acceleration commands for each time instant randomly from a uniform distribution. A total of 500 Monte Carlo simulations were conducted. The lateral acceleration command guesses for each time instant are generated within the upper and lower limits of the lateral acceleration limit, i.e., $a_y\sim U\left(a_{y_{min}},a_{y_{max}}\right)$. The simulation setup in this subsection is the same as that of the C-PDDP algorithm in the previous subsection.

The Monte Carlo simulation results of the C-PDDP algorithm are shown in Figure 4. Figure 4a gives the initial guessed trajectories and optimal flight trajectories for all converged solutions. It can be seen that the C-PDDP algorithm is able to converge to the optimal trajectory even under such complicated initial guess conditions, while completing the task of passing the waypoints and optimizing the flight time of each trajectory segment. The distribution of the optimal flight time for each trajectory segment is shown in Figure 4b, which indicates that the calculated flight time averages converge to $7.35\mathrm{s}$, $9.50\mathrm{s}$ and $6.71\mathrm{s}$, which is same as the results obtained in the previous subsection. The number of iterations of the C-PDDP algorithm shown in Figure 4c shows that C-PDDP can converge to the optimal result within 400 algorithmic iterations in most cases under an initial guess with strong randomness. Figure 4d shows the distribution of cost function values of the algorithm obtained from 500 Monte Carlo simulations, from which it is shown that most of the simulations are able to converge to the optimal cost value; however, some results failed to converge. According to the statistics, the algorithm fails to converge to the optimal value in three simulations, and so the convergence rate of the algorithm in 500 Monte Carlo simulations is $99.4\%$. The Monte Carlo simulations demonstrate that the C-PDDP algorithm is stable with random initial control guesses under given conditions. Meanwhile, the calculation time of the given examples to converge to the optimal solution is about 1 s, which also proves the computational efficiency of the algorithm.

5. Conclusions

This paper proposes a C-PDDP algorithm capable of solving spatial–temporal trajectory optimization problems with multiple intermediate waypoints. This method handles the waypoint path constraints through a penalty function method and parameterizes the flight times of multiple trajectory segments to dynamically optimize multiple segment times. The detailed procedure of the algorithm is derived in this paper, and the optimal control variations and optimal time variations at the waypoints are given.

In UAV reconnaissance mission scenarios, numerical simulations are conducted for five different waypoint scenarios to verify the effectiveness of the spatial–temporal trajectory optimization achieved by the C-PDDP algorithm. In a typical UAV reconnaissance scenario with two intermediate waypoints, the proposed algorithm is compared with the fixed-time constrained DDP algorithm. The proposed algorithm is able to optimize the trajectories with distance errors of $1.06\mathrm{m}$ and flight times of $7.35\mathrm{s}$, $9.50\mathrm{s}$ and $6.71\mathrm{s}$ for the spatially and temporally optimized trajectories. For the C-DDP algorithm, which cannot optimize the time between trajectory segments, the trajectories are optimized with errors above $7\mathrm{m}$ and the trajectories are more tortuous. The above simulation results demonstrate the advantages of the proposed C-PDDP algorithm for spatial–temporal optimization, and the Monte Carlo simulations show the robustness of the algorithm to random initial guesses.

The limitations of the proposed algorithm in this paper include the following. The C-PDDP algorithm proposed in this paper uses the penalty function method to deal with waypoint constraints, and when the number of waypoints is large, poor initial guesses may affect the convergence of the algorithm. In addition, the implementation of the penalty function method requires manual adjustment of some algorithm-related parameters, and the algorithm does not consider the presence of obstacles in the environment. Future work includes providing better initial guesses, eliminating the parameter tuning problem of the penalty function method using the primal-dual interior point method, and exploring the proposed method using a 3D model of a UAV in a 3D environment with obstacles.