Adaptive Mesh Iteration Method for Trajectory Optimization Based on Hermite-Pseudospectral Direct Transcription

An adaptive mesh iteration method based on Hermite-Pseudospectral is described for trajectory optimization. The method uses the Legendre-Gauss-Lobatto points as interpolation points; then the state equations are approximated by Hermite interpolating polynomials. The method allows for changes in both number of mesh points and the number of mesh intervals and produces significantly smaller mesh sizes with a higher accuracy tolerance solution. The derived relative error estimate is then used to trade the number of mesh points with the number of mesh intervals. The adaptive mesh iteration method is applied successfully to the examples of trajectory optimization of Maneuverable Reentry Research Vehicle, and the simulation experiment results show that the adaptive mesh iteration method has many advantages.


Introduction
There are general two kinds of methods to solve the optimal control problems, indirect method and direct method [1][2][3].The indirect method uses the Pontryagin minimum principle and the first-order optimal necessary conditions to convert the optimal problem to two or more points boundary value problem.This method could obtain the exact optimal solution.The advantages of the indirect method are obtaining the high precision solution, satisfying the optimality of the first-order necessary conditions.The disadvantages are the small convergence radius and highly sensitive costate which is difficult to estimate.In recently years, direct methods have been widely applied for the numerical solution of nonlinear optimal control problems [4,5].The state and control are discretized at a series of suitable points in a direct method, then the continuous-time optimal control is converted into a finite dimensional nonlinear programming problem (NLP), and the next procedure is using the NLP solver software to solve the NLP [6].
Among the popular direct methods are the pseudospectral method and Hermite-Simpson method [7,8].The pseudospectral method has the advantage of high rate of convergence [9][10][11], so this paper develops the theory about the Legendre pseudospectral method.There are also some considerable interest in developing theory related to Hermite-Simpson method due to its reasonable accuracy with highly sparse Hessian and constraint Jacobians matrix.Williams provides a framework for arbitrary order and arbitrary number of intervals for implementation on digital computers [12].That method allows the trading of the mesh points within each interval with the number of intervals; thus, better accuracy can be achieved by increasing mesh points for smooth problems, while increasing the number of intervals to achieve better accuracy for nonsmooth problems.However, it is a fact that smooth regions and nonsmooth regions together exist in one solution of the problem, so it is difficult to trade the number of mesh points with the number of mesh intervals when solving a complicated problem.Motived by the desire to trade the number of mesh points with the number of mesh intervals, both the number of mesh points within each mesh interval and the number of mesh intervals are allowed to vary in the method described in this paper.Furthermore, 2 Mathematical Problems in Engineering the method also can improve computational efficiency by reducing the size of the mesh.

Trajectory Optimization Problems
Without loss of generality, consider the following optimal control problems with inequality path constraints: subject to the constraints where the term () ∈    denotes the state and the term () ∈    denotes the control.In ( 1)-( 2), the time domain  ∈ [−1, +1] is transformed from the time domain  ∈ [ 0 ,   ] by the following affine transformation [12]: where the terms  0 and   represent initial time and terminal time, respectively.The basic idea of the approach in this paper is based on interpolating functions for state and costate on Legendre-Gauss-Lobatto (LGL) quadrature nodes [4,7].As the LGL nodes points are distributed over the interval [−1, 1], so it will be useful to transform the time domain.

Adaptive Mesh Iteration Methodology
The mesh points have the property −1 =  0 <  1 < ⋅ ⋅ ⋅ <   = +1.The state in the subintervals   is approximated by the Hermite interpolating polynomial with nth order [11] x Then the cost function is approximated by Gauss-Lobatto quadrature rule as follows:  ≈  (x (1)  1 ,  0 , x ()  ,   ) where the term  ()  is the same as   in [12].

Nonsmooth Solution Location.
If a mesh interval has met the accuracy tolerance, that is,  () max ≤ , where  is the desired relative tolerance, then mesh size is reduced by decrease of the number of collocation points or merging adjacent mesh intervals; otherwise the mesh size needs to be modified by increasing points or dividing that mesh interval into several subintervals.Let  () () be the curvature of the th component of the state in mesh interval , as Let  () and  () max be the mean and maximum value of  () (), respectively.Then define   as the ratio of the maximum to the mean curvature:

Increasing the Number of Mesh Points within a Mesh
Interval.When  () max >  and if   ≤  max , where  max is a userdefined parameter, the curvature is considered uniform in this mesh interval and then the number of collocation points should be increased in interval .Let  ()  and  (+1)  denote the number of collocation points in interval  at mesh  and +1, respectively, where  is the mesh refinement iteration.The number of points  (+1)  at mesh  + 1 is calculated by the formula It is noted in (12) that the ratio of the maximum to the error tolerance has a direct effect on the increases of the polynomial degree in mesh interval.An upper limit  max is set for the maximum allowable polynomial degree to make sure that the number of collocation points does not grow to an unreasonably large value.If  (+1)  >  max (i.e.,  (+1)  exceeds the maximum allowable polynomial degree), then the mesh interval   must be divided into equally spaced subintervals.

Generation of New Mesh Segment. Assume 𝑒 (𝑘)
max >  and   >  max , and then the th mesh interval should be refined.The following procedure is the strategy for mesh interval division.Firstly, the predicted polynomial determines the number of all the collocation points in the new subinterval.Secondly, the number of collocation points should be no fewer than the minimum allowable number.In other words, whenever dividing a mesh interval, each interval will contain at least  min collocation points.Third, the new number of mesh intervals,   , is given by the formula where   is a user-defined positive integer.In this process, it is ensured that the number of new intervals should be at least two.Thus, the number of new subintervals, denoted as   , can be rewritten as

Reducing the Number of Collocation Points in a Mesh
Interval.The relative error of the mesh interval is less than the desired relative tolerance, and   <  max , and then the number of collocation points should be decreased.The number of points  (+1)  at mesh  + 1 is calculated by the formula 3.7.Merging Adjacent Mesh Subintervals.Before the adjacent mesh subintervals merging, it is necessary to decrease the number of each interval according to the method in Section 3.5 and then generally estimate the number of mesh interval points.If  +1 ̸ =   , the mesh interval  +1 = [  ,  +1 ] and mesh interval   = [ −1 ,   ] cannot be merged because the highest polynomial orders of the two adjacent mesh intervals are not equal.All the matching points of the original two mesh intervals are combined, and the conditions for the merging of the two mesh subintervals are mainly three: (1) The two mesh subintervals must be adjacent.
(2) The relative error estimates of the two grid intervals are not more than .
(3) The relative error of the new mesh interval after the merger is not larger than .

Penalty Function
Method for Solving NLP.For convenience, the cost function and the path constraints are adjoined together, so that we obtain the augmented cost function where the term  represents penalty factor and  ()  = ( ()  ,  ()  , ( ()  )).When the penalty factor is greater than a certain threshold, the solution of problem is equal to the solution of original problem.The max operator has a great difference on the time spent of solving the problem, so it is time-consuming to solve the discretized NLP by using the existing mature gradient-based optimization algorithm.This paper develops the smoothing approximation function method proposed in [15].
Obviously, the function  is first-order differentiable with respect to  and has a good approximation to the max operation.When the smoothing factor  → 0, then we have  ()   (, ) = max{0,  ()  }.The optimal index is converted into The gradient-based NLP algorithm also needs to compute the gradient information of the objective function for the optimal variable.Define the Hamiltonian function of optimal control problem:  (x, u, ) =  +    +  (, ) . (19) 3.9.Mesh Refinement Method.The schematic of adaptive mesh iteration method is shown in Figure 1.The adaptive mesh iteration method is summarized as follows.Step 1. Set  = 0 and supply initial mesh,  = ⋃  =1   = [−1, +1], where ⋂  =1   = ⌀.
Step 2. Solve NLP on current mesh .
Step 5. Compute the ratio between the maximum and the mean curvature   in   , and if   ≤  max , set the number of collocation points according to (15); else divide the interval   into   subintervals, where   is given by (14).Then proceed to Step 7.
Step 6.For the single mesh interval, reduce the number of collocation points.Merge the adjacent mesh intervals if they satisfy the merger conditions.

Simulation Analysis
The order and intervals of the method in [12] are fixed in each simulation, while those of the method described in this paper are variable.For the convenience of narration, the mesh refinement method in [12] is called FMRM (fixed mesh refinement method), and the method in Section 3 is called AMIM (adaptive mesh iteration method).The term  denotes the mesh refinement iteration, and  = 0 means the mesh initialization, and the term  and term  denote the total collocation points and interval number, respectively.The number of collocation points within each interval of the two methods is at least 2. The maximum of all mesh interval allowable error values is , where  = 10 −6 .When the mesh is initialization, the whole mesh is divided into 10 intervals, and each interval has a number of 2 collocation points.The value of term  max is 1.2, and the maximum number of collocation points with each interval is 12.The penalty factor  is 100 [15].The simulation results of this paper were performed on a 3.4 GHz Intel Core i7 CPU computer and MATLAB Version R2013.
Consider the following trajectory optimization problem taken from [16], and the state equations for a threedimensional point mass vehicle which is commonly used in midcourse guidance systems of a Maneuverable Research Reentry Vehicle (MaRRV) are listed as follows: where  is the radial position and the terms  and  are the latitude and longitude, respectively.The term V is the total velocity, and the terms  and  denote the flight path angle and azimuth angle, respectively.The symbol  represents the vehicle mass and  is the gravity (/ 2 with  being the gravitational parameter).The atmospheric density with altitude is assumed to be exponential model as where  0 and  are constants in Table 1.
The lift and drag are defined as where The objective is to maximize the range, so the cost function is given by Figures 2(a), 2(b), and 2(c) show the final solutions of the optimal control problem.It can be seen from Figures 2(a), 2(b), and 2(c) that the solution to the trajectory is relatively smooth, though there seems to be a rapid change in the attack angle in Figure 2(c) near  = 1000 s.It is noted that the control variable is limited to [−5 deg, 15 deg] and that the solutions showed in Figure 2(c) satisfy that constraint.The results demonstrate the ability of approaches described in this paper for solving optimal control problems with inequality path constraints.In particular, it can be seen that the attack angle remains of 15 deg near  = 1000 s.One might suppose that if the attack angle is not constrained, its value would likely be larger than 15 deg.
Figure 3 shows the collocation points distribution of the solutions obtained using AMIM.When  = 0, the initial mesh is constructed.From  = 0 to  = 2, the mesh points are added largely, while the increment is relatively small from   there is a great difference from mesh initialization to Mesh Iteration 1 and Mesh Iteration 2 of the trajectories; moreover, the gap between Mesh Iteration 2 and Mesh Iterations 3-4 is very small, and the situation of attack angle iteration process shown in Figure 4(b) is the same as the trajectories process, which means that the solutions are gradually converged on the actual solution with each mesh refinement iteration.It is noted that a higher number of mesh points are needed to ensure the convergence of the solutions, and that explains why the increments in Figure 3 from mesh initialization to Mesh Iteration 2 and from Mesh Iteration 2 to Mesh Iteration 4 are different.
A comparison of the implementation of the AMRM and FMRM with different higher-order and intervals solutions are given in Table 2. Comparisons are made in terms of computation time, the number of total mesh points  and the number of mesh intervals  and the number of mesh refinement iterations , and the cost function values.In each case, the initial guesses are randomized controls, with randomized state.A total of 100 samples are used to produce the results in this paper.The terminology AMRM (2,11) refers to the AMRM where the number of mesh points within each interval can vary between 2 and 11; furthermore, the number of mesh intervals in AMRM can vary as well.All the simulations parameters are shown in black roman font in Table 2, and all the results are shown in black italics.As can be seen from the results listed in Table 2, the AMRM result in the smallest overall times compared with other cases.The reason is that the AMRM has the properties of reducing the unnecessary points and intervals, while FMRM (other cases) does not have this kind of property but only keeps the number of mesh points and intervals fixed until the simulation terminated.It is a fact that computation times mostly depend on the number of mesh refinement iterations and mesh size, while the growth of computation time for cases 4, 7, and 10 is due mostly to the increment number of mesh points and mesh intervals.Interestingly, the number of mesh intervals in case 1 is not set parameter but the result from the simulation, where the numbers of mesh intervals in cases 2-10 are set parameters.Case 7 using 7 mesh points with 55 intervals gives a terminal cost function of −4163.4km, which is the most optimality one in cases 2-9.The results show, as expected, that using larger mesh size results in improvements in accuracy at the expense of more runtime, due to the denser Jacobians.For FMRM, better accuracy can be achieved by increasing mesh points for smooth problems, while increasing the number of intervals to achieve better accuracy for nonsmooth problems [12].However, it is a fact that smooth regions and nonsmooth regions together exist in one solution of the problem, so it is difficult to trade the number of mesh points and the number of mesh intervals when solving a complicated problem.The simulations show that the AMEM can trade the number of mesh points within intervals with the number of mesh intervals and obtain an accurate solution with a relatively small mesh size.

Conclusions
This paper develops an adaptive mesh iteration method for solving optimal control problems.The method has the ability to trade the number of mesh points with number of mesh intervals compared to other mesh refinement methods.The mesh size is guided by a previously derived maximum relative error and the ratio of the maximum to the mean curvature of the mesh intervals.The number of mesh intervals is increased in regions where solution is nonsmooth, while the mesh points increased in regions where the solution is smooth.Furthermore, the mesh size can be decreased either by reducing the mesh points or by combining adjacent mesh intervals which share the same number of mesh points.The method applied successfully trajectory optimization of MaRRV from the open literature and compared with other mesh refinement methods.The simulation results demonstrate the superiority of the AMRM.

Table 1 :
Hypersonic boost-glide vehicle date and physical constants.

Table 2 :
Compare results using VOIand FOI methods.