Trajectory Optimization for the Nonholonomic Space Rover in Cluttered Environments Using Safe Convex Corridors

Abstract

Due to the limitation of space rover onboard computing resources and energy, there is an urgent need for high-quality drive trajectories in complex environments, which can be provided by delicately designed motion optimization methods. The nonconvexity of the collision avoidance constraints poses a significant challenge to the optimization-based motion planning of nonholonomic vehicles, especially in unstructured cluttered environments. In this paper, a novel obstacle decomposition approach, which swiftly decomposes nonconvex obstacles into their constituent convex substructures while concurrently minimizing the proliferation of resultant subobstacles, is proposed. A safe convex corridor construction method is introduced to formulate the collision avoidance constraints. The numerical approximation methods are applied to transfer the resulting continuous motion optimization problem to a nonlinear programming problem (NLP). Simulation experiments are conducted to illustrate the feasibility and superiority of the proposed methods over the rectangle safe corridor method and the area method.

1. Introduction

Planetary surface exploration is a captivating and critical field of research [1,2,3]. Planetary surface exploration is fundamental to space missions, and space rovers play a vital role in exploring challenging environments on the Moon, Mars, and other celestial bodies. The success of these missions relies on generating feasible, safe, and high-quality motion trajectories for space rovers, ensuring driving flexibility, energy efficiency, and prolonged rover service life.

Motion planning is a crucial aspect of planetary surface exploration [4], especially in environments such as Mars with complex terrains, unstable landscapes, and unknown obstacles. The objective of motion planning is to enable rovers to move safely and efficiently, accomplishing scientific investigations and mission objectives. Careful trajectory planning minimizes collisions with obstacles, reduces energy consumption, extends rover lifespan, and ensures successful arrival at designated target points, allowing for the collection of valuable scientific data.

Over the past few decades, substantial progress has been made in motion planning methods for nonholonomic robots, including space rovers. These methods encompass the polynomial interpolation method [5,6], adaptive state lattices [7], homotopy-based methods [8], probabilistic search methods such as rapidly exploring random tree [9], informed RRT* [10], fast marching trees [11,12], reinforcement learning method [13,14,15], numerical optimization methods [16,17,18,19], and others.

However, despite the numerous methods available, research regarding trajectory planning in cluttered environments remains an open field, particularly for nonholonomic rovers. The polynomial interpolation method connects data points with straight lines or polynomial curves to generate trajectories, which can constrain the flexibility of robot motion planning, especially in complex, nonlinear, or obstacle-avoiding scenarios. Probabilistic search algorithms often lack state constraints and usually do not consider the vehicle’s kinematics and dynamics. Learning methods often require prolonged training times and may produce unstable or inaccurate results.

On the other hand, local trajectory generation methods based on numerical optimization models can effectively incorporate the rover’s dynamic characteristics and environmental information, resulting in collision-free and kinodynamically feasible trajectories that meet various driving performance requirements [20,21], such as minimum energy consumption or shortest distance. However, these methods inevitably face a trade-off between trajectory optimality and computational efficiency. Effectively and concisely modeling collision avoidance constraints in cluttered environments is the core challenge in addressing such issues.

1.1. Related Works

The key to constructing collision avoidance constraints lies in efficiently and accurately modeling the environment, particularly the geometric representation of space rovers, irregular obstacles, and safe convex corridors. To incorporate collision avoidance constraints into the trajectory optimization model, it is essential to derive closed-form mathematical descriptions of these constraints. Much research focuses on describing the distance between the robot and obstacles, including signed distance [22], differentiable collision detection algorithms [23], hierarchical optimization-based collision avoidance (H-OBCA) [24], and others.

However, in cluttered environments, the collision avoidance constraints constructed by these methods often exhibit relatively high dimensions, leading to a decrease in the computational efficiency of the trajectory optimization model. Consequently, a method based on safe convex corridors becomes more suitable for addressing such complex scenarios. This method typically converts the conventional obstacle avoidance problem into convex constraints representing the robot’s presence within convex polygons or polyhedra, significantly simplifying the computational process. This method drastically reduces the computation time compared with the original nonlinear and nonconvex obstacle avoidance constraints [19], as it utilizes simple linear constraints as shown in Figure 1. Deits et al. [25] presented IRIS (Iterative Regional Inflation by Semidefinite programming), a novel approach that efficiently computes large obstacle-free regions through convex optimizations for robot manipulator optimization. On the other hand, Chen et al. [26] introduced a safe corridor algorithm, designed to generate collision-free trajectories for autonomous quadrotor flight in cluttered environments. Another noteworthy contribution comes from Liu et al., who proposed the convex feasible set algorithm (CFS) [27], capable of solving nonconvex optimization problems with convex costs and nonconvex constraints, with a primary focus on achieving real-time computation in motion planning for robotics. In addition, Li et al. introduced safe travel corridors (STCs) [28] to simplify collision avoidance constraints in automatic parking maneuver planning, thereby enhancing efficiency and robustness in complex environments. The aforementioned safe corridor algorithms, while ensuring robot safety by constraining the robot within a convex region of free space, may result in the loss of some free space and potential suboptimal trajectory planning or infeasible solutions, as discussed in detail in Section 5.

In the realm of trajectory planning problems, collision detection for the robot often involves employing bounding boxes, such as spheres, oriented rectangles, convex hulls, or overlapping spheres [29] (see Figure 2). Meanwhile, obstacles are commonly represented as convex polygons or polyhedra. As illustrated in Figure 3, three-dimensional obstacle sets can be transformed into two-dimensional convex polygon obstacle sets. When confronted with irregular obstacles, they are typically decomposed into multiple subconvex polygons to facilitate collision detection. Notable convex decomposition methods include Keil’s algorithm [30], Bayazit’s algorithm [31], improved Bayazit’s algorithm [32], and approximate convex decomposition algorithms [33], among others. Nevertheless, it should be noted that these algorithms still encounter issues, such as decomposition failure or the generation of an excessive number of subconvex polygons during segmentation.

1.2. Trajectory Planning Framework Based on Safe Convex Corridors

Considering the challenges identified in previous research, we introduce a specialized trajectory planning framework for space rovers, designed to navigate unstructured environments with a high density of obstacles. This framework makes use of the safe convex corridors method, as illustrated in Figure 4. During the “convex decomposition and discretization of obstacles” stage, the input comprises polygonal obstacles. The path planning stage employs a hybrid $A^{★}$ [34], though other path planning algorithms can also be employed. The “sample waypoints” stage involves secondary sampling of the path points obtained from the path planning output to facilitate the construction of safe convex corridors. The “path planning” and “velocity planning” stages handle the initial solutions for trajectory optimization. In this context, waypoints refer to the discrete path points generated by path planning algorithms, such as $A^{★}$ [35], hybrid $A^{★}$ [34], and jump point search [36], and then processed further in the “sample waypoints” stage. The specific implementation details will be introduced later in this paper.

The contributions of this paper can be summarized as follows:

1

An efficient method for generating safe convex corridors (SCCs) is introduced. The proposed method outperforms the existing methods, such as the rectangle safe corridor method [28] and the area method [18], in some distinct scenarios. Moreover, it achieves superior optimality compared with some similar safe corridor algorithms, specifically the rectangle safe corridor method.

2

A novel convex decomposition algorithm, which can be applied to break down concave polygonal obstacles into multiple subconvex polygons, is proposed. This decomposition method reduces the number of resulting subpolygons significantly, when compared with Bayazit’s algorithm [31] and Keil’s algorithm [30]. This improvement contributes to the acceleration of the SCC generation process.

3

An adaptive sampling algorithm is established to ensure appropriate spacing between adjacent waypoints used in constructing SCCs, while avoiding collisions with obstacles. This adaptive sampling algorithm guarantees suitable distances and nonintersecting lines between waypoints, thus ensuring the effective generation of SCCs.

4

A strategy for discretizing only the boundary of obstacles, rather than the entire obstacle area, is given. This approach greatly expedites the SCCs’ generation process, while still providing accurate and reliable obstacle representation.

1.3. Organization

The paper is structured as follows: In Section 2, we present the Bolza-type optimization formulation for the trajectory generation problem. Accompanied by illustrative examples showcasing its superiority, Section 3 introduces our tailored convex decomposition algorithm for nonconvex obstacles. The process of preparing to construct safe convex corridors is elaborated in Section 4. In Section 5, we delve into the method for constructing these safe convex corridors. To rigorously assess the effectiveness of our proposed methods, we meticulously design simulations and compare them with other state-of-the-art approaches in Section 6. Finally, we draw conclusions in Section 7.

2. Problem Formulation

This section presents a comprehensive overview and specific formulation of the trajectory planning problem for space rovers. The main objective of trajectory planning is to determine a trajectory $\zeta \left(t\right)\in {\mathbb{R}}^{N_{\zeta }}$ that connects the initial state ${\zeta }_0$ to the terminal state ${\zeta }_f$. The system’s control variables are denoted as $u\left(t\right)\in {\mathbb{R}}^{Nu}$. The general formulation of the space rover’s trajectory planning problem, as proposed in [17], is as follows:

$$\begin{array}{l}\mathrm{Minimize}\hfill \\ J={\int }_{t_0}^{t_f}F(\zeta \left(t\right),u\left(t\right),t)dt+W_e·E({\zeta }_0,{\zeta }_f,t_0,t_f)\hfill \\ \mathrm{Subject}\mathrm{to}\hfill \\ \left\{\begin{array}{c}\dot{\zeta }\left(t\right)=f(\zeta \left(t\right),u\left(t\right),t)\hfill \\ {\zeta }_{min}\le \zeta \left(t\right)\le {\zeta }_{max}\hfill \\ u_{min}\le u\left(t\right)\le u_{max}\hfill \\ h_{min}\le h(\zeta \left(t\right),u\left(t\right),t)\le h_{max}\hfill \\ e({\zeta }_0,{\zeta }_f,t_0,t_f)=0\hfill \\ q_{min}\le q(x_0,{\zeta }_f,t_0,t_f)\le q_{max}\hfill \\ t\in [t_0,t_f]\hfill \end{array}\right.\hfill \end{array}$$

(1)

Problem (1) is a Bolza-type optimal control problem, where the cost function $J(·)$ quantifies the desired performances. ${\int }_{t_0}^{t_f}F(·)dt$ denotes the integral performance metrics, $E(·)$ represents the terminal performance metrics, and $W_e$ is the non-negative weighting coefficient. The constraints of optimal control problem (1) consist of the following terms: the differential equation $\dot{\zeta }\left(t\right)=f(\zeta \left(t\right),u\left(t\right),t)$ that describes the kinematic constraints of the space rover, the state and control limits ${\zeta }_{min}\le \zeta \left(t\right)\le {\zeta }_{max}$ and $u_{max}\le u\left(t\right)\le u_{max}$, the collision avoidance constraints $h_{min}\le h(\zeta \left(t\right),u\left(t\right),t)\le h_{max}$, and the boundary conditions $e({\zeta }_0,{\zeta }_f,t_0,t_f)=0$ and $q_{min}\le q(x_0,{\zeta }_f,t_0,t_f)\le q_{max}$. The planning horizon is denoted as $T=[t_0,t_f]$.

2.1. Kinematic Constraints

This paper focuses on the motion planning of space rovers in unstructured planetary surface environments, where they operate at low speeds and encounter numerous cluttered polygonal obstacles (see Figure 3c). It is worth noting that the height information of obstacles is not taken into consideration in this study. To navigate such challenging scenarios, the rover’s nonholonomic constraints must be carefully considered. For high-speed motion scenarios, alternative dynamic models are available, such as the high-dimensional double-track dynamics model and the whole-body dynamics model [37], which can be employed accordingly based on the specific requirements. The kinematic model of the space rover consists of front wheels acting as steering wheels and rear wheels serving as driving wheels. A clear representation of this configuration is depicted in Figure 5. The motion of the space rover can be effectively described by the following single-track bicycle model [19]:

$$\frac{d}{dt}\left[\begin{array}{c}x\left(t\right)\\ y\left(t\right)\\ \theta \left(t\right)\\ v\left(t\right)\\ a\left(t\right)\\ \varphi \left(t\right)\\ \omega \left(t\right)\end{array}\right]=\left[\begin{array}{c}v\left(t\right)·cos\theta \left(t\right)\\ v\left(t\right)·sin\theta \left(t\right)\\ v\left(t\right)·tan\varphi \left(t\right)/l_w\\ a\left(t\right)\\ \dot{a}\left(t\right)\\ \omega \left(t\right)\\ \dot{\omega }\left(t\right)\end{array}\right]$$

(2)

In this equation, $\left(x\left(t\right),y\left(t\right)\right)$ represents the coordinates of the midpoint of the rover’s rear axle, which corresponds to point p in Figure 5. The variables $v\left(t\right)$, $a\left(t\right)$, and $\dot{a}\left(t\right)$ denote the longitudinal velocity, acceleration, and rates of acceleration changes of the rover, respectively. $\varphi \left(t\right)$ denotes the steering angle of the front wheels, while $\omega \left(t\right)$ and $\dot{\omega }\left(t\right)$ represent the angular rate and the angular acceleration of the rover. The yaw angle of the rover is denoted as $\theta \left(t\right)$. Additionally, the front suspension length, wheelbase, rear suspension length, and width between the left and right wheels are represented by $l_1$, $l_2$, $l_3$, and $l_w$, respectively. In this model, the control variable is denoted as $u={[\dot{\omega }\left(t\right),\dot{a}\left(t\right)]}^T$, which influences the rover’s angular rate and acceleration.

2.2. Path Constraints

The motion of the space rover is governed by a multitude of constraints arising from its mechanical configuration and the surrounding environment. These constraints remain applicable throughout the planning time horizon and are commonly known as path constraints. Specifically, the following limitations are imposed on certain state and control variables:

$$\left\{\begin{array}{c}\left|v\left(t\right)\right|⩽v_{max}\\ \left|a\left(t\right)\right|⩽a_{max}\\ \left|\dot{a}\right|⩽{\dot{a}}_{max}\\ \left|\omega \left(t\right)\right|⩽{\omega }_{max}\\ \left|\dot{\omega }\left(t\right)\right|⩽{\dot{\omega }}_{max}\\ \left|\varphi \left(t\right)\right|⩽{\varphi }_{max}\\ t\in \left[0,t_f\right]\end{array}\right.$$

(3)

2.3. Boundary Constraints

The boundary constraint is the setting of the initial state and terminal state of the space rover, and also includes the control variables of the space rover system.

$$\begin{array}{c}\hfill \zeta \left(t_0\right)={\zeta }_0,u\left(t_0\right)=u_0\\ \hfill \zeta \left(t_f\right)={\zeta }_f,u\left(t_f\right)=u_f\end{array}$$

(4)

2.4. Cost Function

In this paper, the optimization problem’s cost function is formulated with two criteria: minimizing the shortest time and the smallest energy consumption. The cost function is represented as follows:

$$\begin{array}{c}\hfill J=W_{\mathrm{time}}{t}_f+\underset{0}{\overset{t_f}{\int }}v{\left(t\right)}^2+\omega {\left(t\right)}^2+\dot{\mathrm{a}}{\left(t\right)}^{2}dt\end{array}$$

(5)

Here, $W_{\mathrm{time}}$ represents the weight assigned to the time factor. Now, let us elaborate on each component of the cost function:

1

$W_{\mathrm{time}}{t}_f$: This term quantifies the cost associated with the total time of the trajectory, where $W_{\mathrm{time}}$ serves as the weight factor for the time component, and $t_f$ denotes the final time of the trajectory. By incorporating this term, the objective function aims to minimize the overall time required to traverse the planned trajectory. The weight factor $W_{\mathrm{time}}$ provides flexibility in balancing time optimization with other motion-related objectives.

2

$\underset{0}{\overset{t_f}{\int }}v{\left(t\right)}^2+\omega {\left(t\right)}^2+\dot{\mathrm{a}}{\left(t\right)}^{2}dt$: This term represents integral-type performance metrics, seeking to minimize speed, angular velocity, and energy consumption throughout the entire trajectory.

The objective function can be tailored to meet specific requirements. For instance, if the primary objective is to minimize the rover’s planning time, the objective function may only include the first term. Further discussions on this matter will be presented in Section 6.

2.5. Optimal Control Formulation for the Trajectory Planning Problem

The optimal control for trajectory planning in complex and unstructured environments can be summarized as follows:

$$\begin{array}{c}\mathrm{Minimize}\begin{array}{c}\end{array}\mathrm{MinimumTimeAndEnergy}\left(5\right)\hfill \\ \mathrm{s}.\mathrm{t}.\begin{array}{c}\end{array}\mathrm{Kinematic}\mathrm{Constraints}\left(2\right)\hfill \\ \begin{array}{c}\end{array}\begin{array}{c}\end{array}\begin{array}{c}\end{array}\mathrm{PathConstraints}\left(3\right)\hfill \\ \begin{array}{c}\end{array}\begin{array}{c}\end{array}\begin{array}{c}\end{array}\mathrm{CollisionAvoidanceConstraints}(11,20)\hfill \\ \begin{array}{c}\end{array}\begin{array}{c}\end{array}\begin{array}{c}\end{array}\mathrm{BoundaryConstraints}\left(4\right)\hfill \end{array}$$

(6)

Let us explore the intricacies of the obstacle avoidance constraints, a crucial aspect in ensuring the space rover’s collision-free trajectory. To simplify the representation of the space rover’s shape, we employ a bounding rectangle and approximate surrounding obstacles with convex polygons (refer to Figure 5). This transforms the collision avoidance problem between the rover and obstacles into a nonoverlapping scenario, similar to the approach described in [18], known as the area method. However, in scenarios with numerous obstacles, this avoidance method can be inefficient, with the optimization problem solving speed decreasing dramatically as the number of obstacles increases. In Section 6, we will conduct experiments to demonstrate the time-related disadvantages of the area method. Therefore, we opt to use the safe convex corridors (SCCs) method to represent the avoidance constraints, with specific implementation details provided in Section 5.

Directly solving the optimal control problem (6) is highly challenging. To approximate the trajectory planning problem, one may apply a difference method or a direct collocation method [38]. As a result, the trajectory planning problem is transformed into a corresponding nonlinear programming problem, which can be effectively addressed using numerical optimization methods.

3. Convex Decomposition of Obstacles

Convex decomposition of obstacles is a prerequisite for many collision avoidance algorithms, including the safe convex corridors’ generation algorithm discussed in this paper. In practice, the algorithm is required to efficiently decompose as few convex polygons as possible, which can effectively improve the time efficiency of the subsequent generation of a safe convex corridor. It is also possible to wrap each obstacle with only one convex polygon. An efficient algorithm for the convex decomposition of nonconvex obstacles is presented in this section.

3.1. The Convex Decomposition Algorithm

Without loss of generality, let us examine the nonconvex obstacle depicted in Figure 6a. The vertices of this obstacle can be classified into two categories: convex vertices and concave vertices. A convex vertex of a polygon is a vertex within the polygon where its corresponding internal angle is less than 180 degrees. For an arbitrary vertex $V_i$, we define the dot product of its adjacent vectors as [32]

$$F=\overrightarrow{V_{i-1}{V}_i}·\overrightarrow{V_{i+1}{V}_i},$$

(7)

and the vertex is convex for $F>0$ and concave for $F<0$. The core idea of the convex decomposition algorithm is to find some line segments that can transfer all the concave vertexes to convex vertexes. In order to ensure the effectiveness of the convex decomposition algorithm, the selected line segments should meet the following requirements:

1

The line segments need to be inside the concave polygon, and should not intersect with the edge and other line segments. Otherwise, more convex polygons and more vertices will be produced.

2

In the case where the line segment connects two concave points, if the two concave points can be eliminated at the same time, this connection is prioritized so that fewer convex polygons will be generated.

The detailed convex decomposition processes are given in Algorithm 1.

The set of vertex coordinates $V_{\mathrm{in}}=\left\{\left(x_1,y_1\right),\dots ,\left(x_n,y_n\right)\right\}$ represents the vertices of the polygon, and n represents the number of the vertices. Define the geometric center of the polygon as

$$V_c(x_c,y_c)=\left(\frac{1}{n}\sum _{i=1}^n{x}_i,\frac{1}{n}\sum _{i=1}^n{y}_i\right)$$

(8)

In Algorithm 1, the first line rearranges the vertices of the given irregular polygon according to the azimuth angle ${\beta }_i$ and the Euclidean distance $d_i$ between the vertices and their geometric center $V_c$, which are defined as

$${\beta }_i=arctan\left(\frac{y_i-y_c}{x_i-x_c}\right)$$

(9)

and

$$d_i={∥V_c-V_i∥}_2$$

(10)

The function $\mathtt{findConcavePoints}(·)$ in the second line of Algorithm 1 is used to identify the concave vertices and collect them in the set $\Omega$. As shown in Figure 6a, for an arbitrary concave point $V_i$, denote the intersection points of the polygon edge and the extension line of the vectors $\overrightarrow{V_{i-1}{V}_i}$ and $\overrightarrow{V_{i+1}{V}_i}$ as $V_{M_i}$ and $V_{N_i}$. $\mathrm{{\rm Y}}$ indicates the set of vertices within the ray region $\stackrel{\wedge }{V_{M_i}{V}_i{V}_{N_i}}$. Denote G and $\mathrm{\Psi }$ as the set of concave and convex vertices in the proposed ray region. Below, we classify the scenarios into three types:

1

Scenario 1: When G is nonempty, it indicates the presence of concave points within the ray region $\stackrel{\wedge }{V_{M_i}{V}_i{V}_{N_i}}$. To address this scenario, $\mathtt{findDividingLineInG}(·)$ (line 6, Algorithm 1) is utilized, and the detailed procedure can be found in Algorithm 2.

2

Scenario 2: When G is empty, but $\mathrm{\Psi }$ is nonempty, it indicates the existence of only convex points within $\stackrel{\wedge }{V_{M_i}{V}_i{V}_{N_i}}$. In this scenario (lines 7–9, Algorithm 1), where either there are convex points or all connections between concave vertices intersect with the edges in E, $\mathtt{findDividingLineIn}\mathrm{\Psi }(·)$ is employed to handle this scenario, and the detailed subsequent implementations are presented in Algorithm 3.

3

Scenario 3: When $\mathrm{{\rm Y}}$ is empty, it indicates the absence of vertices within $\stackrel{\wedge }{V_{M_i}{V}_i{V}_{N_i}}$. This scenario implies that there are no vertices in the ray region or all connections intersect the edges in E (lines 10–13, Algorithm 1). In such cases, a connection is established between $P_i$ and the midpoint $V_{H_i}$ of $V_{M_i}$ and $V_{N_i}$.

Algorithm 1 Convex decomposition

Require: Vertices of the input polygon $V_{\mathrm{in}}$, polygon edges, and added dividing lines E 1: $V_{\mathrm{in}}\leftarrow \mathtt{sort}\left(V_{\mathrm{in}}\right)$ 2: $\Omega \leftarrow \mathtt{findConcavePoints}\left(V_{\mathrm{in}}\right)$ 3: while$\Omega \ne \varnothing$do 4:    $V_i\leftarrow \Omega .\mathrm{pop}-\mathrm{front}\left(\right)$ 5:    $\mathrm{{\rm Y}}\leftarrow \mathtt{findPoints}(\overrightarrow{V_{i-1}{V}_i},\overrightarrow{V_i{V}_{i+1}})$, $G\leftarrow \mathrm{{\rm Y}}\cap \Omega$, $\mathrm{\Psi }\leftarrow \mathrm{{\rm Y}}\setminus G$ 6:    $[\Omega ,\mho ,E]\leftarrow \mathtt{findDividingLineInG}(G,V_i,E,\Omega )$ 7:    if ℧ then 8:      $[\Omega ,\mho ,E]\leftarrow \mathtt{findDividingLineIn}\mathrm{\Psi }(\mathrm{\Psi },V_i,E,\Omega )$ 9:    end if 10:    if ℧ then 11:      $[V_{H_i},V_{M_i},V_{N_i}]\leftarrow \mathtt{rayIntersection}(\overrightarrow{V_{i-1}{V}_i},\overrightarrow{V_i{V}_{i+1}},E)$ 12:      Remove $V_i$ from $\Omega$ and add $\overrightarrow{V_i{V}_{H_i}}$ to E. 13:    end if 14: end while 15: $[V_{\mathrm{out}},m]\leftarrow \mathtt{separation}(E,V_{\mathrm{in}})$ 16: return  $[V_{\mathrm{out}},m]$

Below, we provide a comprehensive explanation of the detailed implementations for Algorithms 2 and 3. In Algorithm 2, the second line contains the function $\mathtt{closestPoint}(·)$, which is utilized to find $V_j$ in G that is closest to $V_i$. In line 3, the function $\mathtt{segmentIntersection}(·)$ checks whether the connecting line segment $\overrightarrow{V_i{V}_j}$ intersects with any edges in the set E. If an intersection is found, the vertex $V_j$ is removed from the set G. The function $\mathtt{convexify}(·)$ in line 6 examines whether the line segment $\overrightarrow{V_i{V}_j}$ separates the angle $\angle V_{j-1}{V}_j{V}_{j+1}$ into two angles less than 180°. If this condition is met, the two concave vertices $V_i$ and $V_j$ are eliminated from the set $\Omega$. Otherwise, connect the concave vertex closest to $V_i$, as further described in line 13 of Algorithm 2. The return result “$\mho =$ true” indicates that all the concave vertices in the set G have been traversed, and all the dividing lines intersect with the line segments in E. Since the concave vertex $V_i$ has not been eliminated, it is necessary to find the dividing line in the set $\mathrm{\Psi }$ and continue the operation in the function $\mathtt{findDividingLineIn}\mathrm{\Psi }(·)$, which is detailed in Algorithm 3. Given the similarity between the functions in Algorithms 2 and 3, further elaboration is omitted to maintain conciseness and clarity.

Algorithm 2$[\Omega ,\mho ,E]=\mathtt{findDividingLineInG}(G,V_i,E,\Omega )$

Require: $\mho \leftarrow \mathrm{true}$, $\mathrm{tmp}\leftarrow 0$ 1: while$G\ne \varnothing$do 2:    $V_j\leftarrow \mathtt{closestPoint}(V_i,G)$ 3:    if $\mathtt{segmentIntersection}(E,\overrightarrow{V_i{V}_j})$ then 4:      Remove $V_j$ from G, Continue 5:    end if 6:    if $\mathtt{convexify}(\overrightarrow{V_i{V}_j},\angle V_{j-1}{V}_j{V}_{j+1})$ then 7:      Remove $V_i$ and $V_j$ from $\Omega$, add $\overrightarrow{V_i{V}_j}$ to E, $\mho \leftarrow \mathrm{false}$, Break 8:    else if $\mathrm{tmp}=0$ then 9:      $\mathrm{tmp}\leftarrow \overrightarrow{V_i{V}_j}$ 10:    end if 11: end while 12: if  $\mathrm{tmp}\ne 0$then 13:    Remove $V_i$ from $\Omega$, add tmp to E, $\mho \leftarrow \mathrm{false}$, Break 14: end if 15: return  $[\Omega ,\mho ,E]$

Algorithm 3$[\Omega ,\mho ,E]=\mathtt{findDividingLineIn}\mathrm{\Psi }(\mathrm{\Psi },V_i,E,\Omega )$

Require:  $\mho \leftarrow \mathrm{true}$ 1: while  $\mathrm{\Psi }\ne \varnothing$do 2:    $V_j\leftarrow \mathtt{closestPoint}(V_i,\mathrm{\Psi })$ 3:    if segmentIntersection(E, $\overrightarrow{V_i{V}_j}$) then 4:      Remove $V_j$ from $\mathrm{\Psi }$, Continue 5:    else 6:      Remove $V_i$ from $\Omega$, add $\overrightarrow{V_i{V}_j}$ to E, $\mho \leftarrow \mathrm{false}$, Break 7:    end if 8: end while 9: return  $[\Omega ,\mho ,E]$

3.2. Examples

As shown in Figure 6a, the set of all vertices $V_{in}=\{V_1,V_2,\cdots ,V_{13}\}$, and the set of concave vertices is $\Omega =\{V_3,V_5,V_9,V_{11}\}$. For the concave vertex $V_3$, the corresponding sets $\mathrm{{\rm Y}}$, G, and $\mathrm{\Psi }$ can be obtained as ${\mathrm{{\rm Y}}}_3={\mathrm{\Psi }}_3=\{V_{12},V_{13}\}$ and $G=\varnothing$. Since $\left|\overrightarrow{V_3{V}_{12}}\right|<\left|\overrightarrow{V_3{V}_{13}}\right|$, connect the concave vertex $V_3$ and the convex vertex $V_{12}$ and remove $V_3$ from $\Omega$. For $V_5$, the set $G_5=\left\{V_{11}\right\}$. Connecting $V_5$ and $V_{11}$, both $\angle V_5{V}_{11}{V}_{12}$ and $\angle V_5{V}_{11}{V}_{10}$ are less than 180°. Then the vertices $V_5$ and $V_{11}$ are removed from the set $\Omega$. For vertex $V_9$, both the sets ${\mathrm{\Psi }}_9$ and $G_9$ are empty. Therefore, $V_9$ is connected to the midpoint of $V_{M9}$ and $V_{N9}$, i.e., $V_{H9}$. The convex decomposition procedures are listed in

Table 1. Decomposition procedures of the proposed irregular polygon.

Concave Point	Types of Scenarios	${\mathbf{\Psi }}_{\mathit{i}}$	${\mathit{G}}_{\mathit{i}}$	Processing Method
$V_3$	Scenario 2	$\left\{V_{12,}{V}_{13}\right\}$	∅	connect $\overrightarrow{V_3{V}_{12}}$
$V_5$	Scenario 1	$\left\{V_{10,}{V}_{12,}{V}_{13}\right\}$	$\left\{V_{11}\right\}$	connect $\overrightarrow{V_5{V}_{11}}$
$V_9$	Scenario 3	∅	∅	connect $\overrightarrow{V_9{V}_{H9}}$
$V_{11}$	Connecting $V_5$ can eliminate the concave point condition without the need for processing

, and the result is shown in Figure 6b.

To illustrate the superiority of the proposed convex decomposition algorithm, it is compared with Keil’s algorithm [30] and Bayazit’s algorithm [31]. The results are shown in Figure 7. It is evident that the proposed algorithm results in a lower number of generated subpolygons for a given irregular polygon compared with Keil’s algorithm and Bayazit’s algorithm.

4. Preparation for Safe Convex Corridors’ Construction

This section encompasses the preparatory tasks preceding the construction of safe convex corridors (SCCs), involving both path planning and adaptive sampling methodologies. These crucial processes are employed to generate a comprehensive set of points necessary for the subsequent construction of SCCs. Additionally, they incorporate obstacle inflation and boundary discretization techniques to effectively exclude regions occupied by obstacles during the SCCs’ construction process.

4.1. Path Planning

In the path planning stage, mainstream path planning algorithms, such as rapidly exploring random trees, A*, and informed RRT*, are used to generate path points. The generated path points serve two main purposes: first, to assist the velocity planning module in generating the initial solution for the optimal control problem (6), and second, to be used in constructing safe convex corridors (SCCs).

Compared with a traditional A* algorithm, the hybrid A* algorithm has the following advantages:

1

Better suited for continuous state space: The hybrid A* algorithm efficiently handles continuous state spaces, enabling more effective searches in high-dimensional state spaces.

2

Consideration of kinematics: The hybrid A* algorithm takes into account kinematic constraints, such as the maximum turning radius and maximum velocity of the space rover, resulting in the generation of more reasonable and feasible paths.

Considering these advantages, we have chosen to use the hybrid A* algorithm for generating path planning points. For the velocity planning stage, please refer to [28].

4.2. Adaptive Sampling of Waypoints

The discrete path points generated directly by the path planning stage may not be suitable for SCC. These points may have small interpoint spacing and an excessive quantity, resulting in prolonged construction times and SCC with limited spatial coverage. Therefore, it is imperative to employ adaptive sampling to obtain waypoints. The waypoints utilized in SCC construction must satisfy two requirements: first, the points should be adequately spaced apart, and second, the line segments connecting these waypoints should be free from obstacle collisions.

Below, we present the specific steps involved in our implementation process. We begin by performing a sampling process on the path points acquired from the path planning stage. To achieve this, we set a maximum length, denoted as $L_{max}$, and then proceed to calculate the cumulative distance along the path points starting from the start point. Once the cumulative distance surpasses the specified $L_{max}$ threshold, we incorporate the previous point into the set $\mathcal{P}$ and proceed to re-evaluate the cumulative length from the current point. This iterative process continues until we reach the end point. However, it is crucial to bear in mind that the sequence of waypoints mentioned above does not guarantee obstacle-free connections between neighboring line segments, as illustrated in Figure 8. Therefore, in situations where the adjacent waypoints $p_i$ and $p_{i+1}$ are linked by line segments intersecting with obstacles, we introduce an intermediary point $p_m$ through further sampling. Subsequently, we assess whether the line segments formed by the newly inserted point $p_m$ with the points $p_i$ and $p_{i+1}$ intersect with any obstacles. If there are no intersections, we can proceed to check the subsequent line segments. Nonetheless, should there still be line segments that intersect with obstacles, we persist in sampling additional intermediate points between these segments to find feasible connections. By diligently following this approach, we ultimately derive a new sequence of waypoints denoted by $\mathcal{P}$. This newly obtained path maximizes the distance between adjacent points while ensuring that it remains within the bounds of $L_{max}$. Moreover, the waypoints in this sequence are meticulously adjusted to avoid any consecutive intersections with obstacles along the connecting line segments.

4.3. Obtain Discrete Obstacle Points

Unlike in previous works, such as [28,39], we adopt a different approach for discretization. Instead of discretizing the entire region occupied by obstacles, we concentrate solely on discretizing the boundaries of these obstacles. By doing so, we significantly expedite the generation of safe convex corridors (SCCs). To elucidate, our process commences with the discretization of the edges of the polygons resulting from the convex decomposition of the obstacles. The specific steps are as follows:

1

We discretize the boundaries of the convex polygons forming the obstacles by sampling points along their edges. The Euclidean distance between the sampled points is denoted as ${\epsilon }_1$, and it is imperative to include the vertices of the polygon obstacles in the sampling. The resulting discrete points constitute a set denoted as $\mathcal{O}$.

2

Utilizing the points in the set $\mathcal{O}$ as centers and r as the radius, we construct a series of circles, where r represents the space rover’s coverage circle. As depicted in Figure 5, both covering circles have an identical radius.

3

Subsequently, we discretize the circles, with the Euclidean distance between the discrete points denoted as ${\epsilon }_2$. These discrete points are collected into a set denoted as $ob{s}^{*}$. Referring to Figure 9a, the red points represent the discretized obstacle point set $ob{s}^{*}$.

The calculation for the radius r is outlined below:

$$\begin{array}{cc}\hfill O_f(x_f,y_f)& \hfill =\left(x+(0.75l-l_3)cos\theta ,y+(0.75l-l_3)sin\theta \right)\hfill \\ \hfill O_r(x_r,y_r)& \hfill =\left(x+(0.25l-l_3)cos\theta ,y+(0.25l-l_3)sin\theta \right)\hfill \\ \hfill r& =\sqrt{{\left(0.25l\right)}^2+{\left(0.5l_w\right)}^2}\hfill \end{array}$$

(11)

5. Safe Convex Corridor Construction

The construction of a safe convex corridor is based on the global waypoints, denoted as $\mathcal{P}=\{p_1,p_2,\cdots ,p_{N_p}\}$. The start point is denoted as $p_1$, while the end point is represented by $p_{N_p}$. Each directed line segment $\overrightarrow{L_i}=p_i\to p_{i+1}$ corresponds to a segment in the free space, achieved through adaptive sampling. To establish the safe convex corridor, we create a convex polygon $\mathrm{S}{\mathrm{C}}_{\mathrm{i}}$ around each directed line segment $\overrightarrow{L_i}$. By utilizing these convex polygons, the safe convex corridor $SCC\left(\mathcal{P}\right)={\bigcup }_{i=1}^{N_p-1}S{C}_i$ can be obtained. Here, $S{C}_i$ denotes the ith convex polygon constructed around $\overrightarrow{L_i}$. Figure 10 visually demonstrates the constructed safe convex corridor, effectively eliminating a significant amount of redundant obstacles. The intersection between adjacent convex polygons is guaranteed to be nonempty, with $p_i$ being a part of $S{C}_{i-1}\cup S{C}_i$ for all $i=2,3,\cdots ,N_p-1$, ensuring the connectivity of the SCC. Failure to maintain connectivity would compromise the safety of the trajectory, as shown in Figure 11. This is why we generate ${\mathrm{SC}}_{\mathrm{i}}$ using directed line segments $\overrightarrow{L_i}$ instead of individual points.

We will create two safe convex corridors, denoted as SCCf and SCCr, for the centers $O_f\left(t\right)=(x_f\left(t\right),y_f\left(t\right))$ and $O_r\left(t\right)=(x_r\left(t\right),y_r\left(t\right))$, respectively. By using Equation (11), we determine the corresponding $O_f$ and $O_r$ from $\mathcal{P}$, denoted as ${\mathcal{P}}_f\left(O_{f1},O_{f2},\dots ,O_{fNp}\right)$ and ${\mathcal{P}}_r\left(O_{r1},O_{r2},\dots ,O_{rNp}\right)$, respectively. We then utilize these two sets of centers to construct the convex corridors SCCf and SCCr. Below, we eliminate the subscripts “f” and “r” since the construction process for both safe convex corridors is identical. Therefore, we will only describe it once.

Next, we will illustrate the process of determining the constraints within the corresponding safe convex corridor using the directed line segment $\overrightarrow{L_i}$ as an example. This process consists of four steps: creating a bounding rectangle (Section 5.1), establishing an initial ellipse (Section 5.2), forming a convex polygon (Section 5.3), and defining the constraints within the safe convex corridor (Section 5.4). The complete process can be referred to Figure 9 and Algorithm 4.

Algorithm 4 SCC = creatSCC(p,$ob{s}^{*}$)

1: i ← 1 2: while i $\le {\mathrm{N}}_{\mathrm{p}}-1$ do 3:    $O_{remain}$←$ob{s}^{*}$ 4:    $\mathrm{P}{r}_i\left(T_1{T}_2{T}_3{T}_4\right)$←findPr ($\overrightarrow{L_i},h,\Delta s$) 5:    $O_{remain}$←removeObstacles($O_{remain}$,Pr ) 6:    $O{V}_0\left(E_0,d\right)=\left\{x=E_0\overline{x}+p_q\mid \parallel \overline{x}\parallel \le 1\right\}$ 7:    j ← 1 8:    while $O_{remain}\ne \varphi$ do 9:      $ob{s}_{min}$←closest($O{V}_j$,$O_{remain}$) 10:      $O{V}_j(E_{new},p_q)\leftarrow$ellipseScaling$(O{V}_{j-1},O_{remain})$ 11:      $H_{s_j}^a\leftarrow 2E_{new}^{-T}·E_{new}^{-1}·(ob{s}_{min}-d^T)$ 12:      $H_{s_j}^b$←${H_{s_j}^a}^T·ob{s}_{min}$ 13:      $O_{remain}$←removeObstacles($O_{remain},H_{s_j}$) 14:      j ← j+1 15:    end while 16:    $H_{s_i}$$\leftarrow {\cap }_{j=1}^m{H}_{s_j}$ 17:    $S{C}_i\left(A_i,B_i\right)\leftarrow \mathrm{P}{r}_i\cap H_{s_i}$ 18:    i ← i+1 19: end while 20: SCC $\leftarrow {\sum }_{i=1}^{N_p-1}S{C}_i$ 21: return SCC

5.1. Creating a Bounding Rectangle

To ensure the connectivity of the SCC and prevent excessive computation time, we define a bounding rectangle $Pr\left(T_1{T}_2{T}_3{T}_4\right)$. If there are no obstacle points from the set $ob{s}^{*}$ inside this bounding rectangle, we skip the next two steps (Section 5.2 and Section 5.3). The length of the adjacent sides of the bounding rectangle is denoted as $L={∥\overrightarrow{L_i}∥}_2+2\Delta s$ and $2h$ (refer to Figure 12). The redundancy length $2\Delta s$ serves to enhance the continuity of the safe convex corridor while ensuring that it remains considerably small to prevent any potential interference with obstacles. The side aligned with the direction of $\overrightarrow{L_i}$ is inclined at an angle ${\delta }_i$.

$${\delta }_i=\left\{\begin{array}{cc}arctan\frac{y_{i+1}-y_i}{x_{i+1}-x_i},\hfill & \mathrm{if}{x}_{i+1}-x_i>0\hfill \\ \pi +arctan\frac{y_{i+1}-y_i}{x_{i+1}-x_i}\hfill & \mathrm{else}\hfill \end{array}\right.$$

(12)

The upper half of the rectangle Pr is denoted as the rectangle ${Pr}_{\mathrm{up}}\left(T_1{T}_2{M}_2{M}_1\right)$, while the lower half of Pr is denoted as rectangle ${Pr}_{\mathrm{low}}\left(M_1{M}_2{T}_3{T}_4\right)$. The boundary between them is defined by the directed line segment $\left|\overrightarrow{p_i{p}_{i+1}}\right|$. $M_1$ represents the midpoint of the line segment $\overline{T_1{T}_4}$, and $M_2$ represents the intersection point of the line segment $\overline{T_2{T}_3}$. The coordinates of the six points of the bounding rectangle are as follows:

$$M_1:\left\{\begin{array}{c}{x}_{m_1}=x_i-\Delta scos\left({\delta }_i\right)\\ y_{m_1}=y_i-\Delta ssin\left({\delta }_i\right)\end{array}\right.$$

(13)

$$M_2:\left\{\begin{array}{c}{x}_{m_2}=x_{i+1}+\Delta scos\left({\delta }_i\right)\\ y_{m_2}=y_{i+1}+\Delta ssin\left({\delta }_i\right)\end{array}\right.$$

(14)

$$\mathrm{Pr}:\left\{\begin{array}{c}(x_{T_1},y_{T_1})=(x_{m_1}-hsin\left({\delta }_i\right),y_{m_1}+hcos\left({\delta }_i\right))\\ (x_{T_2},y_{T_2})=(x_{m_2}-hsin\left({\delta }_i\right),y_{m_2}+hcos\left({\delta }_i\right))\\ (x_{T_3},y_{T_3})=(x_{m_2}+hsin\left({\delta }_i\right),y_{m_2}-hcos\left({\delta }_i\right))\\ (x_{T_4},y_{T_4})=(x_{m_1}+hsin\left({\delta }_i\right),y_{m_1}-hcos\left({\delta }_i\right))\end{array}\right.$$

(15)

The aforementioned process corresponds to the function $\mathtt{findPr}(·)$ in Algorithm 4, specifically in the fourth line.

5.2. Establishing an Initial Ellipse

This step involves the determination of a suitable initial ellipse, denoted as $O{V}_0$. The major axis of the ellipse corresponds to the directed line segment $\overrightarrow{L_i}$, while the minor axis is perpendicular to $\overrightarrow{L_i}$. Mathematically, the ellipse is represented by the equation $OV(E,d)=\left\{x=E\overline{x}+d|∥\overline{x}∥\le 1\right\}$, where the unit circle with the center at the origin has $|\overline{x}|\le 1$. The matrix $E\in {\mathbb{R}}^2$ captures the rotation and scaling transformation of the ellipse, and it can be decomposed as $E=R^{T}SR$, where $S=\mathrm{diag}(a,b)$ is a diagonal matrix with a and b representing the lengths of the ellipse’s semiaxes. Specifically, a corresponds to the semiaxis length in the direction of $\overrightarrow{L_i}$, while b represents the semiaxis length perpendicular to $\overrightarrow{L_i}$. The matrix R denotes the rotation matrix, and d represents the translation of the ellipse.

For the initial ellipse $O{V}_0(E_0,d)$, its radius is determined as $r_o={∥\overrightarrow{L_i}∥}_2/2$, and its center is located at the midpoint $p_q$ between $p_i$ and $p_{i+1}$. It is important to note that the major and minor axes of the ellipse are of equal length. The coordinates of d are given by $p_q$, as illustrated in Figure 9a.

$$\left\{\begin{array}{c}{E}_0=\left[\begin{array}{cc}cos\left(\delta \right)& sin\left(\delta \right)\\ -sin\left(\delta \right)& cos\left(\delta \right)\end{array}\right]\left[\begin{array}{cc}a& 0\\ 0& b\end{array}\right]\hfill \\ d=\left(p_i+p_{i+1}\right)/2\hfill \end{array}\right.$$

(16)

The process described above corresponds to the step in Algorithm 4, specifically in the sixth line, with the green ellipses in Figure 9a representing the initial ellipses created by our method.

5.3. Forming a Convex Polygon

The process involves identifying the obstacle point, denoted as $ob{s}_{min}\in ob{s}^{*}$, which is closest to the center point $p_q$ of the ellipse. This obstacle point is used to construct a new ellipse, denoted as $O{V}_{new}$. The current half-space $H_{s_j}$ that contains the ellipse $O{V}_{new}$ at the point $ob{s}_{min}$ is retained, while excluding the portions of $ob{s}^{*}$ that do not belong to the half-space $H_{s_j}$. This operation is repeated using the new ellipse $O{V}_{new}$ until there are no obstacles within the ellipse, indicated by $O{V}_{new}\cap ob{s}^{*}=\varnothing$. By obtaining the set of half-spaces $H_{s_i}={\cap }_{j=1}^m{H}_{s_j}$ and the bounding rectangle P$r_i\left(T_1{T}_2{T}_3{T}_4\right)$, we can construct the convex polygon $S{C}_i=H_{s_i}\cap P{r}_i$. It is worth noting that $S{C}_i$ is guaranteed to be convex because the intersection of convex sets is always convex. The specific steps are outlined as follows.

First, we identify the obstacle points within the bounding rectangle Pr and denote them as $O_{remain}$. The obstacles outside Pr are not considered, which increases the corridor generation process. Next, we need to find the coordinates of the obstacle point $ob{s}_{min}$ in $O_{remain}$ that is closest to the center point $p_q$ of the ellipse. To accomplish this, we transform the coordinates of $ob{s}_{min}$ into the coordinate system of the ellipse. This is achieved by applying a transformation that maps $OV(E,d)$ to $∥\overline{x}∥\le 1$. In the coordinate system of the ellipse, the transformed coordinates of $obs$ are represented as $ob{s}_{ov}=E^{-1}obs-p_q$. In this coordinate system, the distance from $ob{s}_{ov}$ to the center $p_q$ of the ellipse is equivalent to the distance from $ob{s}_{ov}$ to the origin, denoted as $dis={∥ob{s}_{ov}∥}_2$. Based on the input $ob{s}_{ovmin}$, we generate a new ellipse $O{V}_{new}$. The new ellipse $O{V}_{new}$ retains the same a-axis and center $p_q$ as the original ellipse, while adjusting the b-axis. This is achieved by using a scaling matrix $E_{st}$ to stretch or shrink the b-axis. The transformation matrix $E_{new}=E·E_{st}$ represents the expression for the new ellipse $O{V}_{new}$.

$$O{V}_{new}(E_{new},d)=\left\{x=E·E_{st}\overline{x}+d|∥\overline{x}∥\le 1\right\}$$

(17)

At the point $ob{s}_{ovmin}$, there exists a tangent line that separates the new ellipse $O{V}_{new}$ from the obstacle $O_{remain}$. The half-space where the ellipse lies is the desired half-space $H{s}_j$.

$$H{s}_j:\left\{\begin{array}{c}{H_{s_j}^a}^T=2E_{new}^{-T}·E_{new}^{-1}·(ob{s}_{min}-d^T)\hfill \\ H_{s_j}^b={H_{s_j}^a}^T·ob{s}_{min}\hfill \\ H_{s_j}^a·p_q<H_{s_j}^b\hfill \end{array}\right.$$

(18)

Next, we use the hyperplane $H_{s_j}\left(H_{s_j}^a,H_{s_j}^b\right)$ to exclude the obstacle $O_{remain}$. If $O_{remain}\ne \varnothing$, we continue to stretch or shrink the ellipse until there are no more obstacles within the ellipse. By following the steps above, we obtain a series of half-spaces $H_{s_1}\sim H_{s_m}$, whose union is the set $H_{s_i}=H_{s_1}\cap H_{s_2}\cap \dots \cap H_{s_m}$. The set $H_{s_i}$ is convex because the intersection of convex sets is convex. A complete description can be found in Algorithm 4; $\mathtt{removeObstacles}(·)$ represents the use of a hyperplane constraint to exclude obstacles on the other side of the ellipse. Repeat the above process along the adjacent points of the waypoints $\mathcal{P}$ until reaching the end point, completing the generation of the SCC.

5.4. Defining the Constraints within the Safe Convex Corridor

The previous steps focused on creating safe convex corridors. In this step, our objective is to convert the collision avoidance constraints for the space rover into linear constraints within the safe convex corridor. Specifically, we have generated safe convex corridors, denoted as SCCf and SCCr, corresponding to the center points $O_f=(x_f,y_f)$ and $O_r=(x_r,y_r)$, respectively. It is important to note that we are not constructing a convex polygon for each adjacent pair of waypoints, but rather for a series of path points lying within the convex polygon (refer to Figure 13).

For instance, a path point $p_k$ within the i-th convex polygon can be represented as follows:

$$A_i·p_k\le B_i$$

(19)

In this equation, the matrices A and B are derived from the edges of the convex polygon. Here, $i=1,2,\dots ,N_p-1$ and $k=k_1,k_2,\dots ,k_g$, where $p_{k_1}=p_i$ and $p_{k_g}=p_{i+1}$. Here, g represents the number of path points within the i-th convex polygon. The position of the space rover’s coverage circle center within the corresponding safe convex corridor (SCC) can be represented by the following equation:

$$\left\{\begin{array}{c}{A}_{fi}·O_{fj}\le B_{fi}\\ A_{ri}·O_{rj}\le B_{ri}\end{array}\right.$$

(20)

In the above formula, j represents the index of the discrete path points generated by path planning, with $j=1,\cdots ,N(N>N_p)$. The first row indicates that the front circle center $O_f\left(t\right)=(x_f\left(t\right),y_f\left(t\right))$ is within the safe convex corridor SCCf, and the second row indicates that the rear circle center $O_r\left(t\right)=(x_r\left(t\right),y_r\left(t\right))$ is within the safe convex corridor SCCr. Lastly, the obstacle avoidance constraints in the trajectory planning problem 6 have been transformed into Equations (11) and (20).

6. Experimental Analysis and Discussion

The feasibility and effectiveness of the proposed trajectory planning framework have been verified in a typical cluttered planetary surface environment. The impact of parameters on the algorithm has been discussed, and a comparison has been made between the proposed SCC algorithm and other state-of-the-art obstacle avoidance algorithms. In our simulation experiments, IPOPT [40] and MA57 were selected as the solvers for the optimization problem.

6.1. Simulation Platform and Parameters

The simulations are conducted in MATLAB 2020b and run on a desktop with AMD Ryzen 5 3600X 6 Core CPU @3.8 GHZ and 16 G RAM. The parameters used in the simulations are listed in

Table 2. Basic parameter settings for the rover in the simulation environment.

Parameter	Description	Value	Unit
l	Space rover length	$4.375$	m
$l_w$	Space rover width	$1.805$	m
$l_1$	Front suspension length	$0.874$	m
$l_3$	Rear suspension length	$0.986$	m
$l_2$	Front and rear wheelbase	$2.875$	m
$v_{max}$	Maximum speed	$1.6$	$\mathrm{m}/\mathrm{s}$
$a_{max}$	Maximum acceleration	$1.0$	$\mathrm{m}/{\mathrm{s}}^2$
${\omega }_{max}$	Maximum angular velocity	$0.35$	$\mathrm{rad}/\mathrm{s}$
${\dot{a}}_{max}$	Maximum jerk	$4.0$	$\mathrm{m}/{\mathrm{s}}^3$
${\dot{\omega }}_{max}$	Maximum angular jerk	$0.8$	$\mathrm{rad}/{\mathrm{s}}^2$
${\varphi }_{max}$	Maximum steering angle	$0.75$	$\mathrm{rad}$
r	Space rover coverage circle radius	1.5	$\mathrm{m}$
N	Number of discrete points	100	-
${\Delta }_s$	Construction parameter of Pr	0.1	$\mathrm{m}$
${\epsilon }_1$	Discretization accuracy of obstacle boundaries	0.1	$\mathrm{m}$
${\epsilon }_2$	Discretization accuracy of inflated circles	0.1	$\mathrm{m}$
$W_{\mathrm{time}}$	Weighting factor for time in the objective function	10	-

.

6.2. Simulation Results

The simulations are conducted in a cluttered space measuring $40\mathrm{m}\times 40\mathrm{m}$. In this space, 7 irregular obstacles are randomly positioned. By employing the proposed convex decomposition algorithm described in Section 3, a total of 10 subconvex polygons are obtained. Among these polygons, the largest obstacle occupies an area of $27.763{\mathrm{m}}^2$, while the smallest one covers an area of $2.488{\mathrm{m}}^2$. The initial and terminal states of the simulation are defined as $x_0=8.551\mathrm{m}$, $y_0=7.672\mathrm{m}$, ${\theta }_0=0.785\mathrm{r}\mathrm{a}\mathrm{d}$ and $x_{t_f}=30.736\mathrm{m}$, $y_{t_f}=34.608\mathrm{m}$, ${\theta }_{t_f}=0.070\mathrm{r}\mathrm{a}\mathrm{d}$, respectively.

The resulting safe convex corridors SCCf and SCCr, along with the optimized trajectory, are depicted in Figure 14. In Figure 14a,b, the inflated boundaries of the obstacles (represented by orange dots) are utilized for collision detection. The expansion of the convex corridor halts upon encountering surrounding obstacles, and in some cases, it takes the shape of a rectangle due to the absence of obstacle points within the $\mathrm{Pr}$ region. Consequently, no further steps are executed. Figure 14c displays the resulting trajectory, which surpasses the initially generated path by the hybrid A* algorithm. The discrepancy in results is due to the hybrid A* algorithm’s lack of consideration for the upper bounds of state and control variables, and its focus solely on partial kinematics. Additionally, its objective function is designed to find the shortest path, leading to differences in trajectory outcomes when compared with our comprehensive trajectory planning framework. Statistical analysis of the SCCf/SCCr areas and the number of ellipse reshaping steps are presented in Figure 15. In the figure, it is evident that the ellipse can undergo a maximum of 9 adjustments, whereas the minimum number of adjustments is 0. A value of 0 indicates the absence of any obstacles within the safe corridor. Additionally, Figure 16 illustrates the state and control variables of the resulting trajectory, all of which remain within their respective limits.

6.3. Method Comparison for Optimality and Time Efficiency

To compare the proposed method with state-of-the-art collision avoidance methods, namely, the rectangle safe corridor method [28] and the area method [18], we kept the objective function, constraints, and initial solution in the trajectory optimization problem consistent, except for the collision avoidance constraints. The simulations were conducted in four different unstructured environments, as depicted in Figure 17. The parameters of the obstacles, as well as the initial and terminal poses, are listed in

Table 3. Information of the obstacles.

Case ID	Number of Obstacles	Area$/{\mathbf{m}}^2$	Vertices	Initial State $({\mathit{x}}_0/\mathbf{m},{\mathit{y}}_0/\mathbf{m},{\mathit{\theta }}_0/\mathbf{rad})$	Terminal State $({\mathit{x}}_{\mathbf{tf}}/\mathbf{m},{\mathit{y}}_{\mathbf{tf}}/\mathbf{m},{\mathit{\theta }}_{\mathbf{tf}}/\mathbf{rad})$
Case 1	30	1.092–13.418	4–8	25.601, 2.874, 1.047	24.656, 33.610, 0.785
Case 2	25	1.266–7.207	4–7	13.872, 14.086, 1.047	22.423, 31.805, 0
Case 3	20	4.482–17.977	4–6	30.119, 7.910, 2.443	25.938, 35.748, 1.222
Case 4	8	6.633–25.328	5–6	32.922, 17.933, 1.571	29.216, 36.651, 3.142

. The simulation results of the three methods are presented in

Table 4. Comparison of experimental results of different methods.

	Rectangle Safe Corridor					Our Work					Area Method
Case ID	Cost	CPU Time/s	${\Delta }_{\mathbf{obj}}$	${\mathbf{L}}_{\mathbf{obj}}$	${\Delta }_{\mathbf{t}}$	Cost	CPU Time/s	${\Delta }_{\mathbf{obj}}$	${\mathbf{L}}_{\mathbf{obj}}$	${\Delta }_{\mathbf{t}}$	Cost	CPU Time/s
Case 1	51.227	1.905	8.715	17.013%	103.434	42.881	0.838	0.369	0.861%	236.408	42.512	198.948
Case 2	250.977	2.916	218.41	87.024%	31.040	32.641	2.226	0.074	0.227%	40.971	32.567	93.428
Case 3	59.749	1.578	12.720	21.289%	67.118	42.505	2.238	1.668	3.924%	47.029	40.837	107.490
Case 4	44.457	0.843	7.036	26.408%	31.126	33.117	1.057	0.400	1.208%	48.548	32.717	52.372
Mean	101.602	1.811	64.444	63.428%	54.838	37.784	1.590	0.626	1.657%	62.901	37.158	101.124

Optimality loss: ${\Delta }_{obj}$ = cost of current method − cost of area method, rate of optimality loss: $L_{obj}$ = ${\Delta }_{obj}$/cost of current method, ${\Delta }_t$ = (CPU time of area method − CPU time of current method)/CPU time of current method.

.

It is evident that both the proposed method and the rectangle safe corridor method outperform the area method in terms of time efficiency. Moreover, the proposed method demonstrates slightly superior performance compared with the rectangle safe corridor method. In case 1, which consists of the highest number of irregularly distributed obstacles, the area method exhibits the poorest performance. The time efficiency of the area method is closely tied to the number of obstacles and vertices. As the number of obstacles surpasses a certain threshold, the area method may fail due to the algorithm taking longer to execute than the predefined tolerance time.

The aforementioned results clearly illustrate the significant reduction in computation time achieved by the trajectory optimization formulation based on our method. This improvement is attributed to the elimination of redundant obstacles and the utilization of linear constraints for the safe corridor. In contrast, the area method relies on nonlinear constraints and is unable to eliminate redundant obstacles. Consequently, it encounters difficulties in solving the optimization problem, particularly when dealing with a large number of obstacles and vertices.

The convergence efficiency of trajectory optimization models using three different collision avoidance methods in case 1 is illustrated in Figure 18. The figure shows the optimal value and constraint violation ($inf\_pr$ in the IPOPT solver). It is evident that the proposed safe convex corridor method outperforms the other two methods in terms of performance.

The two types of safe corridor methods (including our proposed method and the rectangle safe corridor method) may unavoidably sacrifice some free space during the corridor construction process, which could potentially result in the loss of the optimal solution. Despite the lower computational efficiency of the area method, it is able to make full use of the available free space. This explains why the area method exhibits the smallest loss function among the four cases (refer to Table 4).

According to the experimental results, our method excels in both time efficiency compared with the area method and optimality loss rate compared with the rectangle safe corridor method. This indicates that our method achieves superior efficiency while minimizing the impact on optimality, making it a more suitable choice compared with the other two methods.

6.4. Impact of Parameters of the Bounding Rectangle Pr on the Optimization Problem

Moreover, we discussed the impact of the Pr parameters, namely ${\Delta }_s$, h, and ${\left|\overrightarrow{L_i}\right|}_2$. Since ${\left|\overrightarrow{L_i}\right|}_2$ has already been determined, we will only discuss the other two parameters. It is important to note that ${\Delta }_s$ should not be excessively large, as it may result in the presence of obstacle points inside the ellipse. This limitation arises because we have fixed the axis of the ellipse along $\overrightarrow{L_i}$.

Moving on, we examine the influence of the parameter h on the optimization problem. As depicted in Figure 19a, with the increase in h, the area of the safe convex corridor expands, and the value of the optimization objective function decreases rapidly until it eventually stabilizes. Figure 19b demonstrates that the computation time decreases rapidly and eventually reaches a steady state as h increases, which aligns with the evolution pattern of the objective function. The initial computation time is longer due to the challenge of finding a feasible or optimal solution when the area of the safe corridor is relatively small. It is therefore crucial to select an appropriate value for h to avoid wasting computation time on SCC generation if h is too large, or risking the loss of the optimal solution if h is too small. In practice, h is defined as $h=max({\lambda }_h{L}_c,h_{max})$, where $L_c$ denotes the length of the current oriented path segment, ${\lambda }_h$ is a non-negative constant, and $h_{max}$ is equal to twice the length of the rover.

Figure 20 illustrates five optimized trajectories generated with different values of h. This outcome is consistent with the results shown in Figure 19, where the trajectories for the initial three values of h differ, while the trajectories for the last two values of h overlap. This phenomenon occurs because, at this stage, increasing the value of h does not lead to a further increase in the area of the safe convex corridor. Due to the current obstacle environment, the area of the safe convex corridor can only reach a certain maximum. Additionally, it is possible that the optimal solution satisfying the current optimization problem is already within the feasible region, rendering the increase in the area of the safe convex corridor irrelevant to the optimal solution.

6.5. Discussion on Applicability

The trajectory planning framework proposed in this study is versatile and can be applied not only to the specific problem we have presented but also to various other problem variations. By customizing the objective functions and constraints to match the specific scenario, remarkable results can still be achieved. The effectiveness of the proposed trajectory optimization framework is demonstrated in Figure 21, which showcases optimized trajectories with different objectives and constraints. For further formulation details, please refer to

Table 5. Description of different trajectory optimization formulations.

Problem ID	Objective and Constraints Details
Problem 1	$J=t_f$
Problem 2	$J=t_f+20{\int }_0^{t_f}v{\left(t\right)}^2+\omega {\left(t\right)}^2+\dot{\mathrm{a}}{\left(t\right)}^{2}dt$
Problem 3	$J=t_f+40{\int }_0^{t_f}v{\left(t\right)}^2+\omega {\left(t\right)}^2+\dot{\mathrm{a}}{\left(t\right)}^{2}dt$
Problem 4	$J=t_f+0.5{\int }_0^{t_f}v{\left(t\right)}^{2}dt$
Problem 5	$J=t_f+0.5{\int }_0^{t_f}\omega {\left(t\right)}^{2}dt$
Problem 6	$J=t_f+0.5{\int }_0^{t_f}\dot{\omega }{\left(t\right)}^2+\dot{a}{\left(t\right)}^{2}dt$
Problem 7	Problem 2 with unconstraint $\theta \left(t_f\right),\varphi \left(t_f\right)$
Problem 8	Problem 2 with unconstraint $\omega ,\dot{\omega }$
Problem 9	Problem 2 with unconstraint $v,a,\theta$

. For example, if the primary objective is to minimize the time required for the space rover’s trajectory, one could consider using the framework provided in Problem 1. If the objective is to optimize both time and energy constraints, Problem 2 or 3 formulations would be suitable options. Alternatively, if the focus is solely on guiding the space rover from random points to a designated target without considering its pose, i.e., no specific requirements for the pose at the end point, Problem 7 could be used. For scenarios where certain constraints, such as velocity or angular velocity, are not a concern for the space rover, considering the frameworks presented in Problem 8 or 9 would be appropriate.

7. Conclusions

This paper presents a trajectory optimization method of the nonholonomic rover based on safe convex corridors. The collision avoidance modeling problem in cluttered environments is transformed into a safe convex corridor construction problem, and a simple linear programming model describing the collision avoidance constraints in cluttered environments is presented. Detail implementations of the convex decomposition algorithm and the safe convex corridor, i.e., SCCf and SCCr, construction algorithm are introduced. The main conclusions of this paper are summarized as follows:

(1)

The trajectory planning framework, which is based on the proposed safety convex corridor method, outperforms two other advanced methods in terms of timeliness, while ensuring a significantly low rate of optimal solution loss. It enables the space rover to achieve a collision-free arrival from the starting point to the destination while maintaining trajectory optimality.

(2)

A novel convex decomposition algorithm is introduced, which generates significantly fewer subconvex polygons compared with the alternative methods. This enhances the efficiency of the entire trajectory planning framework.

(3)

Furthermore, the paper discusses the strategies for parameter selection in these algorithms. The effectiveness and superiority of the proposed methods are validated through extensive simulations conducted in various meticulously designed typical environments.

While the trajectory planning framework presented in this paper assumes a fully known global map, it can be extended to handle scenarios where obstacles are not known a priori. Specifically, as the space rover explores the environment, it continuously senses its surroundings and generates real-time obstacle maps. This online localization and mapping process allows the rover to create a local map and estimate its position relative to encountered obstacles. Seamlessly integrated with the trajectory planning framework, the online localization and mapping modules enable the rover to adapt to changing environments. As the rover navigates, it simultaneously senses and updates its map information in real time. Leveraging up-to-date map information, our algorithm dynamically recalculates trajectories to efficiently circumvent newly detected obstacles.