Trajectory Optimization with Complex Obstacle Avoidance Constraints via Homotopy Network Sequential Convex Programming

Abstract

Space vehicles’ real-time trajectory optimization is the key to future automatic guidance. Still, the current sequential convex programming (SCP) method suffers from a low convergence rate and poor real-time performance when dealing with complex obstacle avoidance constraints (OACs). Given the above challenges, this work combines homotopy and neural network techniques with SCP to propose an innovative algorithm. Firstly, a neural network was used to fit the minimum signed distance field at obstacles’ different “growth” states to represent the OACs. Then, the network was embedded with the SCP framework, thus smoothly transforming the OACs from simple to complex. Numerical simulations showed that the proposed algorithm can efficiently deal with trajectory optimization under complex OACs such as a “maze”, and the algorithm has a high convergence rate and flexible extensibility.

1. Introduction

With the increasing complexity of aerospace missions, the autonomous online operation of the vehicle has become an essential part of advanced missions. Automatic guidance methods aim to generate guidance commands in real-time to meet various constraints [1]. Automatic guidance methods represented by real-time trajectory optimization (RTO) have attracted many scholars. For example, in the rocket recovery guidance problem, the RTO method can effectively increase the feasible region of the initial states and realize a safe landing [2]. In addition, due to the unknown environment of a wild quadrotor swarm scenario, it is vital to obtain the control variables in real-time to avoid collisions between quadrotors and obstacles [3]. RTO has become one of the most promising options for this type of task.

In recent years, the sequential convex programming (SCP) algorithm has progressed significantly as one of the RTO methods [4,5,6]. Liu first proposed SCP in [7,8]. It converts nonconvex constraints into affine constraints by successive linearization, thus constructing a series of second-order cone programming (SOCP) subproblems. The local optimal solutions satisfying the primal nonconvex constraints can be obtained by successively solving the subproblems. Since one can solve each subproblem in polynomial time using the mature interior point method solver, SCP becomes an efficient method. On this basis, Mao [9] and Bonalli [10] proposed the SCP algorithms Scvx and GuSTO, enjoying certain convergence guarantees. Unfortunately, it has been suggested that when the convergent virtual control is non-zero, the obtained solution is infeasible for the original problem [4,11].

In practical scenarios, spacecrafts and quadrotors will encounter a variety of complex obstacle avoidance constraints (OACs) [12]. Augugliaro [13] and Morgan [14] simplified the no-fly zone to a sphere, and then converted the OACs into a series of affine constraints by linearization. However, this method is limited to obstacles of simple shapes. Virgili-Llop [15] proposed the concept of the minimum signed distance (MSD) field to deal with obstacles of arbitrary shapes. Zhang [16] and Misra [17] decomposed the OACs into the sum of a convex polynomial function and a concave polynomial function via the convex–concave procedure algorithm, thus preserving the higher-order information of OACs. Richards [18,19] modeled the problem as mixed-integer linear programming (MILP) and introduced binary variables to deal with OACs. However, the MILP runtime varies exponentially with the number of binary variables. Space vehicles’ hardware often finds it hard to support such large-scale computations [11]. Szmuk [20] introduced the continuous state-triggered constraint (CSTC), applied to the obstacle avoidance flight in the powered landing phase of a 6-DOF rocket. Then, Szmuk extended the CSTC method to a compound CSTC and applied it to quadrotor trajectory planning [21]. Unfortunately, the above SCP-based algorithms still have many shortcomings. For example, the convergent solution cannot be guaranteed to be feasible. It is even tricky for traditional SCP to obtain feasible solutions under complex obstacles [22]. Besides, the selection of reference trajectories significantly impacts the optimality and feasibility of the solutions [1].

In recent years, with the development of homotopy and neural network techniques, many new methods and ideas have emerged in the field, providing new perspectives to overcome such shortcomings:

• The general idea of the homotopy technique is to solve a series of simple transition problems so that the solution of the transition problem gradually approaches the optimal solution of the primal problem. This method reduces the problem’s difficulty, thereby significantly improving the success rate of trajectory planning [4,23,24,25,26,27]. Taheri [23,24] and Saranathan [25] first used the homotopy method to convert a multi-point boundary value problem into an easier-to-solve two-point boundary value problem, thereby dealing with multiple propulsion modes and dynamic environments. Malyuta [26] combined the homotopy method with the SCP algorithm to solve trajectory planning in a rendezvous maneuver under the constraints of discrete logic.
• As an auxiliary tool for trajectory optimization, the neural network can effectively improve the performance of traditional algorithms [28,29,30,31,32,33]. Yin [28] proposed a trajectory planning method based on a neural network, thus improving the indirect method’s convergence rate. Tang [29] and Banerjee [31] used the neural network to fit the initial reference trajectory offline. The SCP algorithm iterates from the reference trajectory predicted by the network, which can effectively reduce the number of iterations. Li [30] used a neural network predictor to estimate the optimal flight time. Only one SOCP needs to be solved online, significantly improving real-time performance.

Unlike the above methods, we combined the homotopy and neural network techniques synchronously and proposed an RTO algorithm, called the homotopy network sequential convex programming algorithm (HNSCP), that can deal with general complex OACs. First, a universal obstacle “growth” method, namely the obstacle interface regression (OIR) approach, was proposed. Then, a homotopy neural network (HNN) was subsequently proposed. The inputs of the HNN are the spatial position coordinates and the homotopy parameter, and the output is the MSD. After that, Bayesian regularization was used to train the HNN. Finally, the OACs represented by the HNN were embedded into the SCP algorithm by successive linearization.

It is worth noting that, in past work, the STOMP [34] and CHOMP [35] algorithms adopted the strategy of computing the signed distance field offline and then applying it online without introducing a neural network. This strategy can theoretically be extended and applied to this study, but there are some difficulties in practice.

The STOMP and CHOMP algorithms only compute the MSD field of a single map. However, after introducing the homotopy technique, it needs to compute the MSD fields under a series of homotopy parameters, which will greatly increase the storage burden. In Example 2, the homotopy parameter increment selected was 0.02. This indicates that the MSD field information of 51 maps will need to be stored successively, and the storage burden will increase dramatically. Therefore, it is advantageous to introduce the neural network to fit the MSD field.

We still take Example 2 to illustrate. Assuming that the strategy of STOMP and CHOMP is adopted to build a sign distance field with an accuracy of 2.5 m (rather rough) offline, then the method needs to store $8\times {10}^6$ parameters. On the contrary, the proposed method only needs to store 3400 parameters. The amount of data stored is reduced to 0.0425%. Although the current algorithm only considers the three-dimensional position space constraints, the algorithm can be extended to the six-dimensional state space constraints without much effort to address more general constraints. The traditional storage strategy will lead to a sharp increase in the storage burden, and the advantage of the HNN will be more significant.

Our work has two major innovations:

• A universal obstacle regression method, namely OIR, is proposed. To the best of our knowledge, this is the first attempt to apply OIR to the trajectory planning field.
• The concept of the HNN is proposed for the first time. Compared with [28,29,30,31,32,33], training the HNN does not require solving optimal control problems. Only the MSD field is needed so that adequate samples can be generated efficiently and stably.

Our algorithm also has the following three major advantages:

• After introducing OIR and the HNN, the algorithm supported the modeling of arbitrary complex OACs, thus greatly expanding the application scenarios of SCP.
• The homotopy parameter can be flexibly updated by changing the input parameters of the HNN, thereby realizing the smooth transition from simple obstacles to complex ones. The convergence and optimality of the SCP algorithm were greatly improved. Numerical simulations showed that the HNSCP algorithm converges well, even with the most straightforward linear interpolated initial reference trajectory.
• After introducing the HNN, MSD fields with different homotopy parameters could be fitted with only a small amount of data. The data storage was significantly reduced, which is vital for online applications.

The structure of this paper is as follows. Section 2 presents a mathematical description of the optimal control problem. Section 3 proposes the HNSCP algorithm, which can be divided into two procedures: online and offline. Section 4 describes the details of the online procedure, including convexification and the successive iteration algorithm. Section 5 introduces the details of the offline procedure, including the OIR and HNN training. In Section 6, a simulation of two specific examples is conducted. The first example compared the proposed algorithm with state-of-the-art algorithms, verifying that the proposed algorithm had a better performance. The second example increased the complexity of the obstacle. A complex “maze” was designed to demonstrate the algorithm’s adaptability in complex environments. Section 7 presents the main conclusions.

2. Problem Statement

This work focuses on dealing with complex constraints, and therefore does not establish an accurate dynamic model of the vehicle and does not limit the type of the vehicle. Readers may consider the “vehicle” as a rocket, quadrotor, or planetary lander in the following passage. First, a reference coordinate system was established, as shown in Figure 1. The origin of this coordinate system is stationary relative to the ground. The y-axis points to the zenith, and the x-axis and z-axis are parallel to the ground and form a right-handed rectangular coordinate system, where the x-axis is in the drawing plane. The z-axis is perpendicular to the drawing plane.

Secondly, the dynamic equations are listed. The vehicle was regarded as a mass point and only the thrust and gravity were considered. The 3-DOF dynamic equations are as follows:

$$\{\begin{array}{l}\dot{p}=v\\ \dot{v}=u+g\end{array}$$

(1)

where $p:R_{++}\to R^3$ represents the position vector of the vehicle, $v:R_{++}\to R^3$ represents the velocity vector, and $u:R_{++}\to R^3$ represents the thrust acceleration vector, which was also the control vector. $g\in R^3$ is the gravity acceleration vector and satisfies $g={[0,-g,0]}^T$, where $g=9.8065{\mathrm{m}/\mathrm{s}}^2$. Although this work only considered the thrust and gravity, the proposed method can be extended and is applicable to more accurate dynamic models or 6-DOF models. This is beyond the scope of the study and will be left to future publications.

Then, two kinds of process constraints were imposed.

(1) Due to the limitations of the vehicle’s own propulsion system, it was necessary to impose limits on the magnitude of the thrust acceleration:

$$u_{\min }\le \Vert u\Vert \le u_{\max }$$

(2)

where $\Vert u\Vert$ is the Euclidean norm of vector $u$, which represents the thrust acceleration magnitude. In the following text, $\Vert u\Vert$ will be abbreviated as u. $u_{\min }$ and $u_{\max }$ are the lower and upper bounds of the thrust acceleration, respectively, as shown in Figure 1.

(2) OACs were considered. For the convenience of description, we divided the research region into two parts. The impenetrable obstacle was denoted by ${\Omega }_{\mathrm{o}}$, as shown in the blue part of Figure 1. The traversable cavity was represented by ${\Omega }_{\mathrm{v}}$, as shown in the white part of Figure 1. The boundary between the obstacle and the cavity was denoted by $\partial \Omega$. The boundary between the research region and the outside region was denoted by $\partial \tilde{\Omega }$. Then, the OACs can be written as:

$$p\in {\Omega }_{\mathrm{v}}$$

(3)

The necessary initial state constraints and terminal state constraints were imposed:

$$\{\begin{array}{l}p\left(t_0\right)=p_0\\ p\left(t_{\mathrm{f}}\right)=p_{\mathrm{f}}\\ v\left(t_0\right)=v_0\\ v\left(t_{\mathrm{f}}\right)=v_{\mathrm{f}}\end{array}$$

(4)

where $t_0$ and $t_{\mathrm{f}}$ represent the initial and terminal times, respectively. $p_0$ and $v_0$ are the position and velocity vectors at the initial time, respectively. $p_{\mathrm{f}}$ and $v_{\mathrm{f}}$ are the position and velocity vectors at the terminal time, respectively. $t_{\mathrm{f}}$ is treated as an unknown variable, indicating that the optimal control problem with free terminal time is solved.

Finally, the velocity increment was selected as the cost function. Minimizing this cost function can save the fuel consumption of a rocket or the energy consumption of a quadrotor:

$$J={\displaystyle \underset{t_0}{\overset{t_{\mathrm{f}}}{\int }}u\mathrm{d}t}$$

(5)

The mathematical description of the optimal control problem is shown in Problem 1.

Problem 1:

Cost function: $\underset{t_{\mathrm{f}},u(t)}{\mathrm{minimize}}$ (5)

Subject to: (1)~(4)

3. HNSCP Algorithm

Traditional sequential convex programming (TSCP) is widely used in [13,14,15]. Unfortunately, the TSCP algorithm can only deal with obstacles with simple geometric shapes. Reference [22] shows that when the shape of an obstacle is complex, sub-SOCP may be infeasible, and the iteration will be interrupted. Eventually, the convergence rate of the algorithm will drop significantly. The convergence rate of TSCP in the simulation of reference [22] is only 50%, and Example 1 in Section 6 also shows that the convergence rate is only 22%. Therefore, it is necessary to introduce a better method to deal with complex obstacles.

A flow chart of the proposed HNSCP algorithm is shown in Figure 2.

For the convenience of explanation, Figure 2 takes the “maze” in Section 6 as an example. First, mature modeling software (SolidWorks, Dassault Systemes, the United States; AutoCAD, Autodesk, the United States) was used to build the maze and obtain two boundaries, $\partial \tilde{\Omega }$ and $\partial \Omega$.

Given $\partial \tilde{\Omega }$ and $\partial \Omega$, the OIR approach proposed in Section 5 calculates the position coordinates of the obstacle boundary in different “growth” states from $\partial \tilde{\Omega }$ to $\partial \Omega$ with a constant growth speed. We denoted the “growth” state of the obstacle with $\epsilon \in [0,1]$, also known as the homotopy parameter. The obstacle boundary, cavity, and obstacle at homotopy parameter $\epsilon$ are denoted as $\partial {\Omega }^{\epsilon }$, ${\Omega }_{\mathrm{v}}^{\epsilon }$, and ${\Omega }_{\mathrm{o}}^{\epsilon }$, respectively. The OIR will output $\partial {\Omega }^{\epsilon }$, ${\Omega }_{\mathrm{v}}^{\epsilon }$, and ${\Omega }_{\mathrm{o}}^{\epsilon }$ under a series of $\epsilon$ values. The schematic diagram is shown in the lower-left corner of Figure 2. The black lines are the contours of $\epsilon$, representing the $\partial {\Omega }^{\epsilon }$ when the homotopy parameter is $\epsilon$. Areas with a reddish color suggest that $\epsilon$ is close to 1. Areas with a bluish color indicate that $\epsilon$ is close to 0.

Without a loss of generality, cavity ${\Omega }_{\mathrm{v}}^{\epsilon }$ is a nonconvex area. The MSD was defined as $d^{\epsilon }$.

$$\begin{array}{l}{d}^{\epsilon }=d^{\epsilon +}-d^{\epsilon -}k=1,2,\dots ,N\\ d^{\epsilon +}=\inf \left\{\Vert d\Vert |p+d\in {\Omega }_{\mathrm{v}}^{\epsilon }\right\}\\ d^{\epsilon -}=\inf \left\{\Vert d\Vert |p+d\notin {\Omega }_{\mathrm{v}}^{\epsilon }\right\}\end{array}$$

(6)

The absolute value of $d^{\epsilon }$ is the shortest distance from the given position p to the obstacle boundary. The MSD is negative if the coordinates are inside the cavity ($p\in {\Omega }_{\mathrm{v}}^{\epsilon }$), while the MSD is positive if the coordinates are inside the obstacle ($p\in {\Omega }_{\mathrm{o}}^{\epsilon }$).

Subsequently, the HNN was trained using the sample generation and training method proposed in Section 5. The inputs of HNN are the position coordinates and the homotopy parameter $\epsilon$, and the output is the estimated value of the MSD $\tilde{d}=\mathrm{Net}(p,\epsilon )$. The superscript “~” indicates that the value is the prediction. The schematic diagram of the fitting result is shown at the bottom of Figure 2. It is worth noting that Figure 2 only shows the fitting result at $\epsilon =1$. The black line is the contour line that satisfies $\mathrm{Net}(p,1)=0$, the yellowish color indicates that the predicted d is larger, and the blue color indicates that the predicted d is smaller. Finally, the neural network function $\mathrm{Net}(p,\epsilon )$, which can predict the MSD with any homotopy parameter, was obtained.

Then, the trained HNN was embedded into SCP, proposed in Section 4, as OACs. A different $\epsilon$ was selected in the iteration to reduce the difficulty of solving and improve the convergence. In the first iteration, $\epsilon$ was set to 0. $\epsilon$ increased linearly with a step size of $\Delta \epsilon$ as the number of iterations increased, until $\epsilon$ was equal to 1. The bottom-right corner of Figure 2 shows the trajectory optimization results for different initial conditions.

4. Online Procedure

4.1. Discretization and Convexification

4.1.1. Discretization

The whole flight process was divided into N − 1 segments according to equal time intervals; then, the discrete time step was as follows:

$$\Delta t=\frac{t_{\mathrm{f}}-t_0}{N-1}$$

(7)

Without a loss of generality, take $t_0=0$. Given the constant discrete number N, the discrete step size $\Delta t$ is only related to the terminal time $t_{\mathrm{f}}$. To address the free final time problem, $\Delta t$ was regarded as a decision variable in this study. The discrete state variables and control variables with serial number k are defined as:

$$\begin{array}{l}{p}_k=p\left(k\Delta t-\Delta t\right)\\ v_k=v\left(k\Delta t-\Delta t\right)\\ u_k=u\left(k\Delta t-\Delta t\right)\\ k=1,2,\dots ,N\end{array}$$

(8)

To avoid the “artificial infeasibility” in the first few iterations, the virtual control variables $a_{\mathrm{v}k}\in R^3$ were augmented to control the variables [9]. Using the trapezoidal discrete method, Equation (1) can be discretized and written as follows:

$$\begin{array}{l}{p}_{k+1}-p_k-\frac{1}{2}\Delta t{v}_k-\frac{1}{2}\Delta t{v}_{k+1}=\mathbf{0}\\ v_{k+1}-v_k-\frac{1}{2}\Delta t{u}_k-\frac{1}{2}\Delta t{u}_{k+1}-\frac{1}{2}\Delta t{a}_{\mathrm{v}}{}_k-\frac{1}{2}\Delta t{a}_{\mathrm{v}}{}_{(k+1)}-\Delta tg=0\\ k=1,2,\dots ,N-1\end{array}$$

(9)

4.1.2. Convexification of Dynamical Equations

Equation (1) was converted into 6 (N − 1) algebraic equations. The linearized equations can be obtained by taking the first-order approximation of the Taylor expansion. The coefficient matrices can be simplified as follows:

$$\begin{array}{l}{A}_k\left[\begin{array}{c}{p}_k\\ p_{k+1}\end{array}\right]+B_k\left[\begin{array}{c}{v}_k\\ v_{k+1}\end{array}\right]+C_k\left[\begin{array}{c}{u}_k\\ u_{k+1}\end{array}\right]+D_k\left[\begin{array}{c}{a}_{\mathrm{v}k}\\ a_{\mathrm{v}(k+1)}\end{array}\right]+E_k\Delta t=F_{k}k=1,2,\dots ,N-1\\ A_k=\left[\begin{array}{cc}-I_{3\times 3}& I_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\right]B_k=\left[\begin{array}{cc}-\frac{1}{2}\Delta \widehat{t}{I}_{3\times 3}& -\frac{1}{2}\Delta \widehat{t}{I}_{3\times 3}\\ -I_{3\times 3}& I_{3\times 3}\end{array}\right]\\ C_k=\left[\begin{array}{cc}{\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ -\frac{1}{2}\Delta \widehat{t}{I}_{3\times 3}& -\frac{1}{2}\Delta \widehat{t}{I}_{3\times 3}\end{array}\right]D_k=\left[\begin{array}{cc}{\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ -\frac{1}{2}\Delta \widehat{t}{I}_{3\times 3}& -\frac{1}{2}\Delta \widehat{t}{I}_{3\times 3}\end{array}\right]\\ E_k=\left[\begin{array}{c}-\frac{1}{2}\left({\widehat{v}}_k+{\widehat{v}}_{k+1}\right)\\ -\frac{1}{2}\left({\widehat{\mathit{u}}}_k+{\widehat{\mathit{u}}}_{k+1}+{\widehat{\mathit{a}}}_{\mathrm{v}k}+{\widehat{\mathit{a}}}_{\mathrm{v}(k+1)}+2g\right)\end{array}\right]F_k=\left[\begin{array}{c}-\frac{1}{2}\left({\widehat{v}}_k+{\widehat{v}}_{k+1}\right)\Delta \widehat{t}\\ -\frac{1}{2}\left({\widehat{\mathit{u}}}_k+{\widehat{\mathit{u}}}_{k+1}+{\widehat{\mathit{a}}}_{\mathrm{v}k}+{\widehat{\mathit{a}}}_{\mathrm{v}(k+1)}\right)\Delta \widehat{t}\end{array}\right]\end{array}$$

(10)

where the variables with the superscript “^” represent reference variables and are regarded as known variables. All coefficient matrices in (10) are constant, and all constraints are converted into affine constraints.

The transformation from (9) to (10) introduces linearized approximation. Trust region constraints were added to ensure that the linearized constraints were still valid [9]. The new variables ${\sigma }_{uk}$ and ${\sigma }_{\Delta t}$ were introduced to represent the trust regions of the control variables and the discrete time step, respectively:

$$\Vert u_k-{\widehat{\mathit{u}}}_k\Vert \le {\sigma }_{uk}k=1,2,\dots ,N$$

(11)

$$\Vert \Delta t-\Delta \widehat{t}\Vert \le {\sigma }_{\Delta t}$$

(12)

The above trust regions were augmented into the cost function in the form of penalty terms. In addition, it is crucial to introduce the new variables ${\sigma }_{ak}$ and augment them to the cost function in the form of penalty terms to constrain the magnitude of virtual control:

$$\Vert a_{\mathrm{v}k}\Vert \le {\sigma }_{ak}k=1,2,\dots ,N$$

(13)

4.1.3. Convexification of Obstacle Avoidance Constraints

According to (6), the OACs are then denoted by nonlinear inequations:

$$d_k^{\epsilon }\le 0k=1,2,\dots ,N$$

(14)

where $d_k^{\epsilon }$ is only related to the position coordinate $p_k$ when cavity ${\Omega }_{\mathrm{v}}^{\epsilon }$ is determined. Replacing $d_k^{\epsilon }$ with the HNN leads to the following inequality:

$$\mathrm{Net}(p_k,\epsilon )\le 0k=1,2,\dots ,N$$

(15)

Since the HNN is continuously differentiable, Equation (15) can be simplified by using the Taylor expansion, and only the first-order linear terms are retained:

$$\left[\frac{\partial }{\partial {\widehat{\mathit{p}}}_k^T}\mathrm{Net}\left({\widehat{\mathit{p}}}_k,\epsilon \right)\right]p_k\le \left[\frac{\partial }{\partial {\widehat{\mathit{p}}}_k^T}\mathrm{Net}\left({\widehat{\mathit{p}}}_k,\epsilon \right)\right]{\widehat{\mathit{p}}}_k-\mathrm{Net}\left({\widehat{\mathit{p}}}_k,\epsilon \right)-d_{\mathrm{mar}}k=1,2,\dots ,N$$

(16)

One can obtain ${\widehat{\mathit{p}}}_k$ from the result of the previous iteration. The collision safety margin $d_{\mathrm{mar}}\ge 0$ was embedded, which avoids the increased risk of collision due to fitting errors of the HNN. The numerical finite difference method was adopted to approximate the differential operation. It is worth noting that the method proposed in this study cannot guarantee obstacle avoidance between discrete time steps. However, the numerical simulation shows that in most cases, there were only finite isolated points on the trajectory making the constraints (16) tight, and there is the possibility of violating the constraint near these isolated points. We addressed this problem by choosing the appropriate discrete number N and safety margin $d_{\mathrm{mar}}$. For Example 1, the simulation shows that the OACs are satisfied between discrete time steps when $N\ge 30$ and $d_{\mathrm{mar}}\ge 3\mathrm{m}$. For Example 2, it is suitable to choose $N\ge 100$ and $d_{\mathrm{mar}}\ge 5\mathrm{m}$.

4.1.4. Convexification of Control Magnitude Constraints

When the lower bound of the thrust acceleration $u_{\min }$ is non-zero, the feasible set of the thrust acceleration is non-convex. New slack variables were introduced to transform the thrust magnitude constraints (2) in the primal problem into convex ones:

$$\Vert u_k\Vert \le {\mathsf{\Gamma }}_k$$

(17)

$$u_{\min }\le {\mathsf{\Gamma }}_k\le u_{\max }$$

(18)

where ${\mathsf{\Gamma }}_k$ represents the slack variable. Reference [36] and a large number of numerical simulations show that although the feasible region of the problem has changed after relaxation, the optimal solutions are equivalent.

4.1.5. Convexification of Cost Function

After augmenting the penalty terms, the modified linear cost function is listed as follows:

$$J={\displaystyle \sum _{k=1}^N\left({\widehat{\mathsf{\Gamma }}}_k\Delta t+\Delta \widehat{t}{\mathsf{\Gamma }}_k+{\omega }_a{\sigma }_{ak}+{\omega }_u{\sigma }_{uk}\right)}+{\omega }_{\Delta t}{\sigma }_{\Delta t}$$

(19)

where ${\omega }_a$, ${\omega }_u$, and ${\omega }_{\Delta t}$ are fixed penalty coefficients that can be set manually by users. The selection of these parameters follows two principles. First, ${\omega }_a$ needs to be a large positive number to ensure that the magnitude of the virtual control variable converges to zero. Second, ${\omega }_u$ and ${\omega }_{\Delta t}$ need to be selected as small positive numbers to ensure the convergent solution is near the reference one.

The convexified problem is summarized in Problem 2. Problem 2 is an SOCP and can be solved efficiently using the prime-dual interior point method.

Problem 2:

Cost function: $\underset{\Delta t,p_k,v_k,u_k,a_{\mathrm{v}k},{\sigma }_{uk},{\sigma }_{ak},{\sigma }_{\Delta t},{\mathsf{\Gamma }}_k}{\mathrm{minimize}}$ (19)

Subject to: (4), (10–13), (16–18) with given $\epsilon$

Table 1. Variables and constraints in Problem 2.

Category	Item	Number	Dimension of the Cone	Total Number
Variables to be optimized	Time step $\Delta t$	1	-	15 N + 2
	Position $p_k$	3 N
	Velocity $v_k$	3 N
	Control $u_k$	3 N
	Virtual control $a_{\mathrm{v}k}$	3 N
	Control trust regions ${\sigma }_{uk}$	N
	Virtual control magnitude relaxation variables ${\sigma }_{ak}$	N
	Time trust region ${\sigma }_{\Delta t}$	1
	Control magnitude relaxation variables ${\mathsf{\Gamma }}_k$	N
Affine equality constraints	Boundary value conditions, Equation (4)	12	-	6 N + 6
	Dynamic constraints, Equation (10)	6 (N − 1)
Affine inequality constraints	OACs, Equation (16)	N	-	3 N
	Control magnitude constraints, Equation (18)	2 N
Second order cone constraints	Control trust region constraints, Equation (11)	N	4	3 N + 1
	Time trust region constraint, Equation (12)	1	2
	Virtual control constraints, Equation (13)	N	4
	Control magnitude constraints, Equation (17)	N	4

Assume N = 100. According to Table 1, there are 1502 variables, 606 affine equations, 300 affine inequations, and 301 s-order cones. Therefore, Problem 2 is a medium-scale SOCP with thousands of variables.

lists the number of variables, affine equations, affine inequalities, and second-order cones.

4.2. Successive Iterative Algorithm

The successive iterative algorithm solving the primal problem is shown below.

Algorithm 1 required an initial reference trajectory. Unfortunately, it is difficult to generate high-quality initial reference trajectories in real time due to the complexity and variability of the problem [1]. In this work, Algorithm 2 was adopted to generate rough initial reference trajectories efficiently.

Algorithm 1: Online Procedure of Homotopy Network Sequential Convex Programming (HNSCP)

Step 1: Algorithm initialization. Set the maximum number of iterations $i_{\max }$. Set the penalty coefficient ${\omega }_a$, ${\omega }_u$ and ${\omega }_{\Delta t}$. Set the iteration convergence criterion ${\epsilon }_{\max ,u}$, ${\epsilon }_{\max ,t}$, ${\epsilon }_{\max ,a}$. Set the increment step of the homotopy variable $\Delta \epsilon$. Set the homotopy variable $\epsilon =0$; Step 2: Set initial reference variables, $\Delta {\widehat{t}}^{(0)}$, ${\widehat{\mathit{p}}}_k{}^{(0)}$, ${\widehat{v}}_k{}^{(0)}$, ${\widehat{\mathit{u}}}_k{}^{(0)}$, ${\widehat{\mathit{a}}}_{\mathrm{v}k}{}^{(0)}$, according to Algorithm 2; For i = 0: $i_{\max }$

If $\epsilon <1.0$

$\epsilon =\epsilon +\Delta \epsilon$

Else

$\epsilon =1.0$

End    Step 3: Solve Problem 2, and the obtained solution is used to update the reference variables $\Delta {\widehat{t}}^{(i+1)}$, ${\widehat{\mathit{p}}}_k{}^{(i+1)}$, ${\widehat{v}}_k{}^{(i+1)}$, ${\widehat{\mathit{u}}}_k{}^{(i+1)}$, ${\widehat{\mathit{a}}}_{\mathrm{v}k}{}^{(i+1)}$;

If ${\displaystyle \sum _{k=1}^N{\sigma }_{uk}{}^{(i)}}\le {\epsilon }_{\max ,u}$, ${\sigma }_{\Delta t}{}^{(i)}\le {\epsilon }_{\max ,t}$, ${\displaystyle \sum _{k=1}^N{\sigma }_{ak}{}^{(i)}}\le {\epsilon }_{\max ,a}$ and $\epsilon =1.0$

A convergent solution $\Delta {\widehat{t}}^{(i)}$, ${\widehat{\mathit{p}}}_k{}^{(i)}$, ${\widehat{v}}_k{}^{(i)}$, ${\widehat{\mathit{u}}}_k{}^{(i)}$, ${\widehat{\mathit{a}}}_{\mathrm{v}k}{}^{(i)}$ is obtained;      Return;

End

End

Step 4: Fail to converge; Return;

Algorithm 2: Initialization of HNSCP’s Reference Variables

Step 1: Obtain the initial states and the terminal states $p_0,v_0,p_{\mathrm{f}},v_{\mathrm{f}}$ Step 2: Set the reference speed constant $v_{\mathrm{ref}}$; Step 3: Set the linear reference variables; $\Delta {\widehat{t}}^{(0)}=\Vert p_{\mathrm{f}}-p_0\Vert /v_{\mathrm{ref}}$; For k = 1: N

${\widehat{\mathit{p}}}_k{}^{(0)}=p_0+\left(k-1\right)\left(p_{\mathrm{f}}-p_0\right)/\left(N-1\right)$; ${\widehat{v}}_k{}^{(0)}=\left(p_{\mathrm{f}}-p_0\right)/\Delta {\widehat{t}}^{(0)}$; ${\widehat{\mathit{u}}}_k{}^{(0)}={[0,u_{\max },0]}^T$; ${\widehat{\mathit{a}}}_{\mathrm{v}k}{}^{(0)}={[0,0,0]}^T$;

End

Return;

5. Offline Procedure

5.1. Obstacle Interface Regression Approach

The eikonal equation, a typical hyperbolic partial differential equation, was first applied to wave propagation. The position of the wavefront at different time steps can be obtained by solving the partial differential equation. Inspired by this, this section extends the method to describe the growth and construction of obstacles and proposes a universal method to address this issue. There are mature methods and commercial software for solving boundary value problems of partial differential equations. Therefore, the proposed method enjoys simple programming and strong universality. Consequently, it is more competitive than other methods.

5.1.1. Basic Principles

The obstacle’s “growth” process is a kind of interface regression phenomenon. This phenomenon has been extensively studied in optics [37], combustion [38,39], and image processing [40]. Sethian [41,42] proposed the level-set method and the fast marching method. These two methods solve the initial value problem of the level-set equation and the boundary value problem of the eikonal equation, respectively. The solution describes the interface at the given regression state. Although these two methods have been widely used, there are still many difficulties in solving hyperbolic partial differential equations. Mokrý [37] and Li [39] proposed a method to solve an ellipse-form eikonal equation, which can be directly embedded in the finite element software. The original eikonal equation is as follows:

$$\nabla T\left(p\right)\cdot \nabla T\left(p\right)=\frac{1}{r{\left(p\right)}^2}$$

(20)

where T is a scalar field. Its physical meaning is the moment when the interface propagates to the given position, so T’s contour line is the interface. r is the regression speed.

It is hard to obtain a reliable solution for (20) by using existing numerical methods, so (20) needs to be approximated to an ellipse form:

$$\alpha r{\nabla }^{2}T=r^2\nabla T\cdot \nabla T-1$$

(21)

where $\alpha$, a small-enough positive number, is the diffusion term coefficient set manually by users. It is worth noting that (21) degenerates into (20) if $\alpha =0$.

Equation (21) is a nonlinear Poisson equation. Solving (21) requires a series of boundary conditions. The initial interface adopts the Dirichlet condition T = 0, and the rest of the boundaries adopt the “zero flux” condition. The nonlinear Poisson equation can then be transformed into sparse quadratic equations using a finite element method. The system of algebraic equations can be efficiently solved using the Newton–Raphson method. Powerful commercial finite element software (COMSOL Multiphysics, Comsol Inc, Zürich, Switzerland; ANSYS, Ansys Inc, Champaign, IL, USA) can be used for mesh generation, equation resolving, and post-processing.

It is worth noting that the proposed method can only solve the approximate solution. The value of $\alpha$ determines the performance of the algorithm. If $\alpha$ is large, the approximate error of (21) is large, which will eventually lead to a large error in the regression result. If $\alpha$ is small, it will be difficult for the Newton–Raphson method to converge. To take into account the convergence and error, with the aid of a large number of numerical simulations, a reasonable range of $\alpha$ was concluded [39]:

$$\alpha \in \left[0.1h,0.5h\right]$$

(22)

where h is the scale of the local mesh element.

5.1.2. Implementation

Firstly, the Dirichlet boundary condition was set as:

$$T\left(p\right)=0,p\in \partial \tilde{\Omega }$$

(23)

The regression speed r was used to describe spatial distribution of the obstacle:

$$r\left(p\right)=\{\begin{array}{l}\kappa r_{0}p\notin {\Omega }_{\mathrm{o}}\hfill \\ r_{0}p\in {\Omega }_{\mathrm{o}}\hfill \end{array}$$

(24)

where $\kappa$ is a number much less than 1 and $r_0$ is the unit regression speed, which can be taken as 1 m/s. Its practical implication is as follows: the regression speed of the interface in the obstacle ${\Omega }_{\mathrm{o}}$ is $r_0$, while the regression speed in the cavity ${\Omega }_{\mathrm{v}}$ is so small that it can be ignored numerically. Since the interface will preferentially pass through the obstacle region ${\Omega }_{\mathrm{o}}$, the T contours describe the “growth” boundaries of the obstacle from $\partial \tilde{\Omega }$ to the given $\partial \Omega$, as shown in Figure 3.

The normalized scalar field $\epsilon$ was defined as:

$$\epsilon =\frac{T{r}^2}{{\left(T{r}^2\right)}_{\max }}$$

(25)

The subscript “max” means obtaining the maximum value of the whole field, and the $\epsilon$ field is a scalar field ranging between 0 and 1. The $\epsilon$ field’s contours represent the obstacles’ boundaries in different “growth” states. For example, the contour line $\epsilon =0$ indicates no obstacles, the contour line $\epsilon =0.45$ suggests that the obstacle has grown to 45%, and the contour line $\epsilon =1$ indicates that the obstacle has been restored to ${\Omega }_{\mathrm{o}}$.

Figure 4 shows the flow charts and results for the two examples in Section 6, respectively. First, finite element software or CAD software was used to model the obstacle; the blue area represents the cavity, and the yellow area represents the obstacle. Then, the interface regression speed was set according to (24), and the boundary conditions were set according to (23). After dividing the uniform triangular second-order element or the tetrahedral second-order element, the Newton iteration method was used to solve (21) to obtain the T field. Finally, the $\epsilon$ field was obtained according to (25). The method can return the “growth” boundary coordinates for a given $\epsilon$, laying the foundation for the sample generation of the HNN in the next section.

5.2. Homotopy Neural Network

5.2.1. Structure

The feedforward neural network (FNN) was the first artificial neural network invented. The units in each layer can receive signals from the units in the previous layer and transmit signals to the next layer. According to the universal approximation theorem, for an FNN with a linear output layer and at least one hidden layer using a nonlinear activation function, as long as the number of units in the hidden layer is large enough, the FNN can approximate any real function defined in a bounded closed set [43,44].

Considering the training time cost and fitting accuracy, an FNN composed of three hidden layers was used to predict the MSD field. The input layer was denoted as layer 0, the hidden layers were denoted as layers 1 to 3, and the output layer was denoted as layer 4.

Table 2. The meaning of the associated notation.

Notation	Explanation
$M_l$	Number of units in layer l
$f_l(\cdot )$	Activation function of the units in layer l
$W^{(l)}\in R^{M_l\times M_{l-1}}$	Weight matrix from layer l − 1 to layer l
$b^{(l)}\in R^{M_l}$	Bias vector from layer l − 1 to layer l
$z^{(l)}\in R^{M_l}$	Net input (net activation) of units in layer l
$a^{(l)}\in R^{M_l}$	Output (activation) of units in layer l

gives the relevant notation used in this section.

Setting the input of the neural network as the position coordinates and homotopy parameters, the following equation exists:

$$a^{(0)}={\left(p^T,\epsilon \right)}^T={\left(p_x,p_y,p_z,\epsilon \right)}^T$$

(26)

Then, the output of the other layers can be calculated using the following propagation formula:

$$\begin{array}{cc}\begin{array}{l}{z}^{(l)}=W^{(l)}{a}^{(l-1)}+b^{(l)}\\ a^{(l)}=f_l(z^{(l)})\end{array}& l=1,2,3,4\end{array}$$

(27)

The neural network will recursively obtain the output according to the following process. First, the net activation of layer l − 1 is calculated from the activation of layer l − 1 and the weight matrix and bias vector of layer l. The nonlinear activation function is used to calculate the activation of the l layer. This process is repeated until reaching the output layer. The activation of the output layer satisfies the following equation:

$$a^{(4)}=\tilde{d}$$

(28)

where d represents the MSD and its superscript “~” indicates that the value is the prediction. The schematic diagram of the HNN’s structure is shown in Figure 5.

Generally speaking, to reduce the fitting error, the number of units in each layer should be appropriately increased if the obstacles are more complex. The two examples in Section 6 used different unit scales to obtain better performance, and the specific parameters are shown in

Table 3. Selection of the HNN structure in different examples.

Parameter	Example 1	Example 2
$M_0$	4	4
$M_1$	10	40
$M_2$	20	40
$M_3$	10	40
$M_4$	1	1

. It is worth noting that the activation function was selected similarly. The hidden layers adopted the sigmoid activation function, and the output layer adopted the linear activation function.

5.2.2. Sample Generation

The traditional method of fitting control variables and trajectories using supervised learning requires many samples [28,29,30,31,32,33], and the only way to obtain samples is to solve complex optimal control problems. Therefore, acquiring a large number of samples has become a challenge in neural network training. Training the HNN proposed in this study only required calculating the MSD. Therefore, a large number of samples can be generated efficiently. The specific algorithm is given below.

5.2.3. Training

First, a loss function called the mean square error (MSE) was defined to evaluate the fitting accuracy of the HNN:

$${\sigma }_{\mathrm{MSE}}=\frac{{\displaystyle \sum _{i=1}^n{\left(d_i-{\tilde{d}}_i\right)}^2}}{n\max \left(d_i^2\right)}=\frac{{\displaystyle \sum _{i=1}^n{\left[d_i-\mathrm{Net}\left(p_i^T,{\epsilon }_i\right)\right]}^2}}{n\max \left(d_i^2\right)}$$

(29)

where n is the total number of samples, $d_i$ is the target value generated by Algorithm 3, and $d_i$ is the estimated value of the HNN. A total of 85% of the data set was used for training and 15% was used for test. The Bayesian regularization training method [45] was used to train the HNN. This method can effectively avoid overfitting and has a better fitting performance for complex problems. The training results of the two examples in Section 6 are shown in

Table 4. Statistics of training results.

Parameter	Example 1	Example 2
Number of samples n	54,270	77,920
Mean square error (MSE)	$\begin{array}{l}9.0688\hfill \end{array}\times {10}^{-6}$	$3.0411\times {10}^{-5}$
MSE of the training set	$8.9248\times {10}^{-6}$	$2.9356\times {10}^{-5}$
MSE of the test set	$9.8816\times {10}^{-6}$	$3.6390\times {10}^{-5}$
Training time	30 min 14 s	3 h 21 min 10 s

.

Algorithm 3: Sample Generation
Step 1: Obtain the output of the OIR algorithm: $\partial {\Omega }^{\epsilon }$, ${\Omega }_{\mathrm{v}}^{\epsilon }$ and ${\Omega }_{\mathrm{o}}^{\epsilon }$. Step 2: Generate n random samples ${(p_i^T,{\epsilon }_i)}^T$, where $p_i\in \partial {\Omega }^{\epsilon }\cup {\Omega }_{\mathrm{v}}^{\epsilon }\cup {\Omega }_{\mathrm{o}}^{\epsilon }$ and ${\epsilon }_i\in [0,1]$; For i = 1: n
	Step 3: Compute the MSD according to (6); Step 4: Store the data in the sample set;
End
Return;

The MSE of the training set and the sample set in the two examples were close, which indicates that the training did not overfit. In addition, all test set errors were at a small level, showing that the HNN had an excellent fitting performance.

6. Numerical Examples

This section presents the application of the algorithm in two scenarios with complex obstacles. It is worth noting that although a 3-DOF dynamic model was established, only the trajectory in the x − y plane was drawn in the results for the convenience of display. All numerical simulations were performed on a PC with an Intel Core i7-11700k at 3.5 GHz and 16 GB of RAM. We adopted ECOS [46] as the interior point solver, and the programming language was C.

6.1. Example 1: Comparison with State-of-Art Algorithms

This section compares the results between HNSCP and two other advanced SCP-based algorithms (continuous state-triggered constraint (CSTC) method [21] and homotopy (HOMO) method [4]). The comparison items included real-time, convergence, and optimality performance.

First, the obstacle and cavity were manually set. Then, the OIR method was used to compute the “growth” of the obstacle boundary, and Algorithm 3 was used to generate a large number of samples for HNN training. The fitting performance of the trained HNN is shown in Figure 6. Ultimately, Algorithm 1 and Algorithm 2 can be used to generate the optimal trajectory in real-time.

Figure 6 shows the prediction results of the HNN when the homotopy parameter $\epsilon$ increased from small to large. It can be seen that with an increase in $\epsilon$, the MSD field predicted by the HNN changed from simple to complex and gradually approached the MSD field of the primal problem. When $\epsilon =1$ (see Figure 6d), the recovery of the MSD field was complete. The vehicle’s initial and terminal states were set, and then the HNSCP, HOMO, CSTC, and TSCP algorithms were adopted to solve the optimal trajectory. The specific parameter settings of the algorithm are shown in

Table 5. Parameter setting of Example 1.

Parameter	Value	Parameter	Value
$p_0$	${[50,10,0]}^T$ m	${\omega }_a$	100,000.0
$v_0$	${[0,0,0]}^T$ m/s	${\omega }_u$	0.1
$p_{\mathrm{f}}$	${[950,10,0]}^T$ m	${\omega }_{\Delta t}$	0.1
$v_{\mathrm{f}}$	${[0,0,0]}^T$ m/s	$\Delta \epsilon$	0.2
$u_{\max }$	20 m/s2	$v_{\mathrm{ref}}$	30 m/s
$u_{\min }$	5 m/s2	$d_{\mathrm{mar}}$	5 m

, and the results are shown in

Table 6. Comparison between different SCP-based algorithms (N = 100).

Algorithm	Velocity Increment (m/s)	Number of SCP Iterations	Elapsed Time (s)	Terminal Time (s)
HNSCP	490.80 (Best)	19 (Best)	0.487 (Best)	44.93
HOMO	509.07	19 (Best)	1.710	47.80
CSTC	609.48	29	0.920	56.35
TSCP	-	Infeasible after 6	-	-

and Figure 7.

It can be seen from Table 6 that under the same parameter settings, HNSCP, HOMO, and CSTC could all obtain the local optimal solution. Among them, the performance index obtained by HNSCP was the smallest, the number of iterations was the smallest, and the elapsed time was also the shortest. Therefore, under these boundary conditions, the HNSCP algorithm was better. It is worth noting that the subproblem was infeasible after six iterations using the TSCP algorithm.

Figure 7a shows the iterative details of the HNSCP algorithm. It was found that in the first few iterations, the trajectory traversed the obstacle. As the number of iterations increased, the trajectory gradually avoided obstacles and converged to a feasible solution. Figure 7b compares the trajectories solved by different algorithms. The CSTC results were quite different from those of the other two algorithms, and the trajectory was more curved, which significantly increased the consumption of fuel. Figure 7c shows the thrust acceleration profile in the “bang-bang” form, which is also the basic feature of the fuel-optimal trajectory. In order to compare the performance of different algorithms more comprehensively, a Monte Carlo simulation was conducted with 100 samples under different discrete number conditions. Given the discrete number, the initial positions of the vehicle were randomly selected to solve the optimal trajectories. The statistical results are shown in

Table 7. Monte Carlo simulation of Example 1.

Algorithm	N	Convergence Rate	Average Elapsed Time (s)	Solver’s Average Consumed Time (s)	Optimality Performance	Can it be Extended to Exp. 2
HNSCP	30	100%	0.1087	0.0950	1.0	Easy
	60	100%	0.2546	0.2129	1.0
	100	100%	0.5249	0.4124	1.0
	150	100%	1.0450	0.7658	1.0
HOMO (Poor real-time performance)	30	96%	0.3218	0.2782	0.9912	Hard
	60	98%	0.7077	0.5451	1.0001
	100	100%	1.6043	1.1096	1.0058
	150	100%	3.6225	2.0957	1.0153
CSTC (Poor optimality performance)	30	99%	0.1175	0.1050	1.1052	Hard
	60	99%	0.3441	0.2980	1.1559
	100	100%	0.8270	0.6561	1.2239
	150	98%	1.6957	1.2685	1.2217
TSCP (Poor convergence performance)	30	22%	0.1093	0.0968	1.0234	Easy
	60	18%	0.2488	0.2117	1.0001
	100	18%	0.4781	0.3856	1.0001
	150	18%	0.8242	0.6172	1.0000

. The optimality performance was evaluated with $I_{\mathrm{opt}}$, and its expression is as follows:

$$I_{\mathrm{opt}}={\displaystyle \sum _{i=1}^m\frac{J^{\ast }}{m{J}_{\mathrm{HNSCP}}^{\ast }}}$$

(30)

where m is the number of Monte Carlo test samples. $J^{\ast }$ is the velocity increment solved by the algorithm to be evaluated, and $J_{\mathrm{HNSCP}}^{\ast }$ is the velocity increment solved by HNSCP.

For the convergence rate, the HNSCP algorithm could achieve 100% convergence, which was slightly higher than that of the HOMO and CSTC algorithms and much higher than that of the TSCP algorithm. Therefore, HNSCP has certain advantages in ensuring the safe and reliable flight of the vehicle. For real-time performance, HNSCP’s and TSCP’s elapsed times were relatively short. The CSTC algorithm needed more iterations. Therefore, the real-time performance was slightly inferior. The HOMO algorithm introduced more auxiliary variables and auxiliary cone constraints, making each iteration longer. Therefore, the real-time performance was the worst. In fact, assuming that the cavity is composed of a union of $n_{ss}$ cuboid spaces, the HOMO algorithm needs $n_{ss}N$ extra auxiliary variables. As the number of obstacles increases, the scale of the problem will expand dramatically. The relationship between the elapsed time and the discrete number is shown in Figure 8. For optimality, HNSCP, HOMO, and TSCP were similar, while the trajectory planned by the CSTC algorithm was more curved, and the performance index was usually 1.1–1.25 times larger than that of the other algorithms.

In summary, each algorithm had some shortcomings. HOMO had poor real-time performance, the CSTC algorithm had a poor performance index, and the TSCP algorithm had poor convergence. However, the HNSCP algorithm could balance various evaluation indicators, and its performance was relatively good. In addition, CSTC and HOMO were also limited because the cavity must be abstracted as a union of multiple cuboids, which cannot well-simulate real and complex environments and has poor extendibility. Conversely, HNSCP can be applied to a wide range of scenarios, which will be shown in Example 2.

6.2. Example 2: Application in a Complex Maze

This section increases the obstacles’ complexity and verifies the HNSCP algorithm’s adaptability. First, a flat “maze” was constructed using modeling software. Then, the steps in Figure 2 were followed to solve the optimal trajectory. The fitting performance of the trained HNN is shown in Figure 9. As the homotopy parameter increased, the signed distance field predicted by the HNN gradually recovered the primal one.

The test was divided into a single test and a Monte Carlo test. The settings of the algorithm parameters in a single test were slightly different from those in Table 5. The initial position coordinate was set to ${[90,80,0]}^T$, the terminal coordinate was ${[970,80,0]}^T$, and $\Delta \epsilon =0.02$. The other parameters were the same as those in Table 5. The Monte Carlo test was still randomly selecting the initial position coordinates to solve the trajectories. The results obtained for both tests are shown in

Table 8. The result of Example 2.

Single Test	Velocity increment (m/s)	Number of Iterations	Elapsed Time (s)	Terminal Time (s)
Results	751.21	59	1.728	60.179
Monte Carlo Test	Sample Quantity	Convergence Rate	Average Elapsed Time (s)	Solver’s Average Consumed Time (s)
Results	100	100%	1.945	1.487

and Figure 10.

We chose a small Δε to ensure that the algorithm had better convergence (100%). This adjustment significantly increased the number of iterations and, thus, the elapsed time. However, it can be seen from Table 8 that the algorithm could still converge within 2 s and had the potential for online applications. It was noted that in the single test, the TSCP algorithm was interrupted after the 24th iteration and failed to find a feasible solution.

From Figure 10a,e, it can be seen that the thrust acceleration profile calculated by HNSCP was the “bang-bang” form, and a total number of 12 maximum thrust arcs were found. Figure 10b shows the trajectory details, where the planned trajectory approached the constraint boundary 14 times (see from A to N), but none violated the constraint. The above results show that the algorithm had a strong adaptability and could still find a locally optimal feasible solution even in an extremely complex environment. Figure 10c shows the iterative details of the HNSCP algorithm. In the first few iterations, the trajectory traversed the obstacle. As the number of iterations increased, the trajectory gradually avoided all obstacles and converged to a feasible solution. Figure 10d shows the Monte Carlo test results. Starting from different positions, the HNSCP algorithm could find optimal feasible trajectories from the linear interpolation reference trajectory.

Due to the fitting error of the HNN, there may be the possibility of collision with real obstacles. Three strategies can be adopted to avoid such phenomena:

• Set and increase the safety margin $d_{\mathrm{mar}}$ (adopted in this work);
• Increase the scale of the neural network to improve the fitting accuracy;
• Take the HNSCP result as the initial value, and use the TSCP algorithm to refine the solution.

7. Conclusions

This work combines the sequential convex optimization algorithm with homotopy and neural network techniques. Compared with traditional methods, the proposed algorithm can deal with much more complex OACs and has significant advantages in convergence, real-time, and optimality. Numerical simulations show that for complex “maze” obstacle constraints, the proposed algorithm can achieve 100% convergence and find the local optimal solution within 2 s (N = 100). Consequently, it has the potential for onboard applications. Adding homotopy and neural network technologies has dramatically improved convergence in practice, but lacks theoretical explanation. Future work will address this issue.