A new optimization-driven path planning method with probabilistic completeness for wheeled mobile robots

Abstract

Wheeled mobile robots are widely utilized for environment-exploring tasks both on earth and in space. As a basis for global path planning tasks for wheeled mobile robots, in this study we propose a method for establishing an energy-based cost map. Then, we utilize an improved dual covariant Hamiltonian optimization for motion planning method, to perform point-to-region path planning in energy-based maps. The method is capable of efficiently handling high-dimensional path planning tasks with non-convex cost functions through applying a robust active set algorithm, that is, non-monotone gradient projection algorithm. To solve the problem that the path planning process is locked in weak minima or non-convergence, we propose a randomized variant of the improved dual covariant Hamiltonian optimization for motion planning based on simulated annealing and Hamiltonian Monte Carlo methods. The results of simulations demonstrate that the final paths generated can be time efficient, energy efficient and smooth. And the probabilistic completeness of the method is guaranteed.

Introduction

An important technique for the exploration of extraterrestrial planets during recent years is path planning. ¹ However, there hardly exists a single path planning approach that can achieve all the exploration objectives or fulfill all the actual constraints. The main reason is that the tasks are performed in uncertain and complex environments and affected by additional hazards, such as communication time delays and limited daytime for exploiting solar power. ² Taking the task of looking for signs of past water on Mars ³ as an example, one can assume that the objective of exploration is to navigate wheeled mobile robots (WMRs) to the region of interest for further investigation while avoiding unnecessary detours. This mission-oriented characteristic makes point-to-region (PTR) path planners suitable for planetary explorations.

Point-to-point (PTP) path planning methods, such as cell decomposition methods, ⁴ potential-field-based methods ⁵ and intelligence methods, ⁶ have long been studied. Some researchers have realized that PTR problems can potentially be too complicated for PTP planners, when there are multiple goals and final states, or the goal region covers a large area.^7,8 Thus, research on PTR planners has practical significance. The research in this paper intends to develop a path planning approach for WMRs performing mission-oriented exploration tasks in extraterrestrial planets.

In general, path planning methods based on optimization methods^9,10 and probability theories such as rapidly exploring random tree (RRT)-based methods^11,12 have been shown to be more suitable for PTR problems in the environment previously mentioned. Peynot et al. ⁷ have constructed a control policy involving mobility prediction on different terrains to navigate a robot to a square goal region. This approach emphasizes the traversability of the path more than the path quality such as smoothness. Persson and Sharf ⁸ regard path planning toward a goal region as a sampling-based optimization problem, so that probabilistic completeness and fast convergence are guaranteed within a binary map. However, the method cannot work directly in non-binary maps which usually contain more information. Davoodi et al. ¹³ have developed an evolutionary algorithm to solve multi-objective path planning problems by utilizing a geometrical smoothing technique. But the smoothness is not inherent, which may affect the actual feasibility of the smooth path in real environment. Choudhury and Scherer ¹⁴ have derived a dual form of the original covariant Hamiltonian optimization for motion planning (CHOMP) method by adding a box constraint, ¹⁵ which can solve linear problems for constrained goal regions or confine the path in a specific linear search region. However, this method does not work well with non-convex cost functions, and it lacks probabilistic completeness.

The main contributions of this study are as follows:

1. We improve dual covariant Hamiltonian optimization for motion planning (Dual CHOMP) ¹⁴ to function well in ill-distributed energy-based cost maps by implementing simplified non-monotone gradient projection algorithm (NGPA). ¹⁶

2. We propose a new variant of the improved Dual CHOMP with probabilistic completeness.

3. By combining it with an energy-based cost map, the method can guide a WMR through a complicated environment while maintaining energy efficiency.

With these improvements, this novel CHOMP-based method is capable of overcoming more challenging cost maps, which are hard for the methods described in Choudhury and Scherer ¹⁴ and Zucker et al. ¹⁵ A series of simulations demonstrate that the new algorithm is efficient and that the quality of the path is much better than those obtained by other approaches in many situations.

The remainder of this paper is organized as follows. In section “Representation of the environment,” we propose a procedure for establishing an energy-based map (EMAP). In section “The definition of PTR path planning,” we define the path planning problem in the EMAP. In section “Improvement of the Dual CHOMP method,” we propose the improved Dual CHOMP method. In section “Simulation experiments for path planning tasks,” we describe the simulation experiments performed to demonstrate the performances of the new method. Finally, in section “Conclusion,” we present our conclusions.

Representation of the environment

For mobile robots such as legged robots ¹⁷ and WMRs, ¹⁸ it is hard to designate some regions to be definitely unreachable or untraversable in a certain map. Usually cost maps are capable of solving this problem. By penalizing the traversing costs, a WMR can traverse regions with relatively high elevation, which are usually considered as obstacles in binary maps, as illustrated in Figure 1.

Figure 1.

(a) The WMR navigates around a region considered as an obstacle in a binary map. (b) The WMR traverses through the smooth and low-cost side of the same region, while keeping away from the steep side with a high cost in a cost map.

The WMR is assumed to move uniformly in this paper, in order to focus the discussion on the tendency of traverse energy change brought by the different elevations of continuous terrains. The energy consumption required to traverse a given region is obviously positively associated with the slope angle of that region, whether the WMR with the uniform velocity goes uphill or downhill. Thus, we propose the following procedure to establish an EMAP:

• Step 1. Compute the slope angles from the elevations of adjacent nodes along the positive directions of the X- and Y-axes. The process is shown in Figure 2 taking X-axis as an example.

• Step 2. Compute the energy-based changes corresponding to the slopes by

  $$E_{i,j}=\{\begin{array}{c}{c}_{\mathrm{up}}(m\tan \theta \Delta s+E_{\mathrm{dis}}),\theta ⩾0\hfill \\ \multicolumn{1}{c}{c_{\mathrm{down}}(m\tan \theta \Delta s+E_{\mathrm{dis}}),\theta <0}\end{array}$$

  (1)

  where m is the mass of the WMR, $\theta$ is the slope angle, $\Delta s$ is the granularity, $E_{\mathrm{dis}}$ is the inevitable dissipative energy and $c_{\mathrm{up}},c_{\mathrm{down}}\in [1,\infty )$ are the coefficients.

• Step 3. Let the traversing energy at the grid with the coordinate $(i,j)$ be $E_{i,j}^{\Delta s}=\sqrt{{E_{i,j}^{\Delta X}}^2+{E_{i,j}^{\Delta Y}}^2}$ , where $E_{i,j}^{\Delta X}$ and $E_{i,j}^{\Delta Y}$ are the traversing energy values calculated along the X- and Y-axes, respectively.

• Step 4. By comparing $S_{i,j}^{\Delta X}$ and $S_{i,j}^{\Delta Y}$ with the threshold slope angle $S_{\mathrm{thr}}$ , the new costs filling in grids are calculated by

$$C_{i,j}^{\Delta s}=\{\begin{array}{c}{\mathrm{wE}}_{i,j}^{\Delta s},S_{i,j}^{\Delta X}⩽S_{\mathrm{thr}}\mathrm{or}{S}_{i,j}^{\Delta Y}⩽S_{\mathrm{thr}}\hfill \\ \multicolumn{1}{c}{C_{max},S_{i,j}^{\Delta X}>S_{\mathrm{thr}}\mathrm{or}{S}_{i,j}^{\Delta Y}>S_{\mathrm{thr}}}\end{array}$$

(2)

where $w\in [0,1)$ is a weight coefficient for further combination with other costs and $C_{max}$ is the limited peak cost.

Figure 2.

Let $\alpha$ and $\beta$ be the slope angles along the X-axis here. The slope distribution space $S_{i,j}^X$ along the X-axis can then be obtained. Thus, the traversing energy space $E_{i,j}^X$ can be obtained as in Step 2.

This procedure is proposed according to our preliminary research about the total traversing cost change while a real six-wheeled rover is climbing different slopes.^18,19 For a path of a given length, the lower the total energy-based cost accumulated along the path, the less the actual energy consumed by a real WMR.

There are some regions which are impossible or unallowable to be traversed in a real map. These obstacle regions can be from certain rules of constructing a cost map or manual interference to designate regions to be untraversable. We set these regions as containing high costs in EMAPs. The cost function of an obstacle region is defined as

$$c(x)=\{\begin{array}{c}{\epsilon }_{o1}D(x)+{\epsilon }_{o2},\mathrm{if}D(x)⩽0\hfill \\ \multicolumn{1}{c}{0,\mathrm{otherwise}}\end{array}$$

(3)

where x is any possible position of the WMR in the map. The function $D(x)$ is the distance between the WMR and the obstacle region. The constant ${\epsilon }_{o1}$ adjusts the linear amplitude and ${\epsilon }_{o2}$ denotes a base cost value.

The overall cost map $C_{\mathrm{MAP}}$ is obtained by combining the prior EMAP and the obstacle costs together, as shown in Figure 3. We treat cost maps as images and dilate all the regions regarded as untraversable to a certain distance for safer planning. The dilated regions contain relatively high costs but not deemed as untraversable.

Figure 3.

(a) The original digital elevation map (DEM) of a given terrain. The color indicates the elevations in meters. (b) The energy-based cost map obtained from (a) before weighting. (c) A binary representation of artificial obstacle regions and regions with high-cost grids filled by penalized high slope angles. (d) The complete EMAP $(100\times 100\mathrm{grids})$ with weighted and dilated regions.

The definition of PTR path planning

The purpose of path planning in this paper is to safely navigate a WMR to a target region $T\subset W$ , where $W\subset {\mathbb{R}}^3$ is the workspace. Let $O\subset W$ be the region’s non-traversable obstacles or penalized slope angles. After establishing a complete EMAP, let the function $C_{\mathrm{ij}}=C(i,j)$ be the cost of the grid point $(i,j)$ in it. The path of a WMR $\xi$ , that is, the trajectory of the WMR observed from a top view, can be discretized within a time span $t\in [0,1]$ . The entire path is assumed to represent an infinite-dimensional point in the following sections. Let $\xi (t)$ be the waypoint with a cost of $C(\xi (t))$ in W. Let $P_s$ and $P_g$ be the starting and goal positions of the path in the two-dimensional (2D) space.

We consider that the goal region T is a square to simplify the discussion. Once an optimal path connecting $P_s$ to $T\backslash O$ is discovered, the path planning is deemed to be successful. Now the path planning problem can be abstracted as the problem of minimizing the cost along the whole path

$$\begin{array}{c}\underset{\xi }{min}\underset{0}{\overset{1}{\int }}C(\xi (t))\mathrm{dt}\hfill \\ \mathrm{subject}\mathrm{to}\xi (0)=P_s\in T,\xi (1)=P_g\in T,\xi (t)\in W\backslash O\hfill \end{array}$$

(4)

Improvement of the Dual CHOMP method

In this section, we improve the original Dual CHOMP method to solve path planning problems in general cost maps which usually involve non-convex cost functions. A new variant based on the improved deterministic Dual CHOMP which can guarantee probabilistic completeness is proposed.

Implementing NGPA to improve Dual CHOMP

The objective functional is defined as follows ¹⁵

$$\mathit{U}[\xi ]={\lambda }_o{\mathit{F}}_{\mathrm{obs}}[\xi ]+{\lambda }_s{\mathit{F}}_{\mathrm{smooth}}[\xi ]$$

(5)

where ${\mathit{F}}_{obs}[\xi ]$ is the cost functional for obstacles, ${\mathit{F}}_{smooth}[\xi ]$ is the smoothness cost functional for the locomotion of a WMR and ${\lambda }_o,{\lambda }_s\in (0,\infty ]$ are two weighting coefficients instead of only one as in Zucker et al. ¹⁵ for a more flexible adjustment in complicated cost maps.

Let $q_0$ and $q_n$ be the start and end points of the planned trajectory, respectively. $\xi$ can be rewritten as $\xi \approx (q_0^T,q_1^T,\dots ,q_n^T{)}^T$ with $n$ interpolation points. Let $\Delta t$ be the virtual time interval. ${\mathit{F}}_{\mathrm{smooth}}[\xi ]$ is then written as

$${\mathit{F}}_{\mathrm{smooth}}[\xi ]=\frac{1}{2}\Vert K\xi +e{\Vert }^2=\frac{1}{2\Delta t^2}\Vert \begin{array}{c}{q}_1-q_0\hfill \\ \multicolumn{1}{c}{\begin{array}{c}{q}_2-q_1\hfill \\ \multicolumn{1}{c}{⋮}\end{array}}\\ \multicolumn{1}{c}{q_n-q_{n-1}}\end{array}{\Vert }^2$$

(6)

Let Hessian $A=K^{T}K$ be the metric tensor of a Riemannian space. The discretization expression of equation (6) in the $k\mathrm{th}$ step is

$$\overline{\nabla }{\mathit{F}}_{\mathrm{smooth}}[{\xi }_k]\approx \sum _{t\in [0,1]}(A{\xi }_k(t)+K^{T}e)$$

(7)

The continuous obstacle functional can be written as ${\mathit{F}}_{\mathrm{obs}}[\xi ]={\int }_0^{1}C({\xi }_k(t))\mathrm{dt}$ . And the discretization expression of ${\mathit{F}}_{\mathrm{obs}}[\xi ]$ in the $k\mathrm{th}$ step is

$$\overline{\nabla }{\mathit{F}}_{\mathrm{obs}}[{\xi }_k]=\sum _{t\in [0,1]}(\Vert {\xi \prime }_k\Vert )((I-{\hat{\xi }\prime }_k{{\hat{\xi }\prime }_k}^T)\nabla C-C\kappa )$$

(8)

where $\kappa =(1/\Vert {\xi \prime }_k{\Vert }^2)(I-\hat{\xi }{\prime }_k{\hat{\xi }{\prime }_k}^T){\xi ″}_k$ , denoting the curvature of ${\xi }_k$ , $\xi {\prime }_k$ and ${\xi ″}_k$ are the first and second differential, respectively, and $\hat{\xi }\prime$ denotes the norm of $\xi {\prime }_k$ .

A linear inequality constraint, representing the constraint of the goal region T, to the original optimization problem in iteration k is written as ¹⁴

$$\begin{array}{cc}\multicolumn{1}{c}{{\xi }_{k+1}=}& \arg \underset{\xi }{min}\mathit{U}[{\xi }_k]+(\xi -{\xi }_k{)}^T{\overline{\nabla }}_A\mathit{U}[{\xi }_k]\hfill \\ \multicolumn{1}{c}{}& +\frac{{\eta }_k}{2}\Vert \xi -{\xi }_k{\Vert }_A\mathrm{s}.\mathrm{t}.M\xi -n⩾0\hfill \end{array}$$

(9)

where ${\overline{\nabla }}_{A}U[\xi ]$ is the functional gradient of $U[\xi ]$ within a Riemannian space with the metric tensor A and ${\eta }_k$ is a regularization coefficient. M and n here are general parameter matrices for a linear constraint.

Letting u be a Lagrange multiplier, we can obtain the primary solution from the Lagrange dual function $\mathit{G}[u]=\underset{\xi }{min}\mathit{L}[\xi ,u]$ .

When applying the original Dual CHOMP on an EMAP, the Hessian $M^T\mathrm{AM}$ is often singular because the digital elevation map (DEM) is usually not inherently smooth enough. And for a global map lacking further information, it will distort the map if performing interpolation forcefully. Therefore, the original Dual CHOMP, using Newton’s projection method, ¹⁴ cannot solve problems for our cases. Thus, we here implement the NGPA to solve the problem of minimizing u

$$u_k=\underset{u}{\arg min}-\mathit{G}[u]\mathrm{s}.\mathrm{t}.u⩾0$$

(10)

We simplify the NGPA here to improve computational efficiency. Let $\Omega \subset R^n$ be a non-empty and closed convex set, representing the box constraint. For $u,v\in \Omega$ , let $\mathit{B}$ be the projection of point $(u,v)$ onto $\Omega$ , written as $\mathit{B}(u)=\underset{v}{\arg min}\Vert u-v\Vert$ . Let the reference function be $f_k^r=\mathit{G}[u_k]$ . The algorithm we use to compute the dual variable $u_{k+1}$ from a single iteration k is shown in Algorithm 1, where ${\overline{\alpha }}_k\in [{\alpha }_{min},{\alpha }_{max}]$ is the initial step size, $\delta$ and $\eta$ are the preset parameters of an Armijo line search, j is a large enough positive integer, $g_k$ is the gradient and $d_k$ is the difference during the line search.

Algorithm 1. Single step of the simplified NGPA.
Inputs: $[{\alpha }_{\min },{\alpha }_{\max }]\in (0,\infty )$ , $j\in (0,\infty )$ , $\delta ,\eta \in (0,1)$
1 Choose ${\overline{\alpha }}_k\in [{\alpha }_{min},{\alpha }_{max}]$ ;
2 $g_k=\overline{\nabla }\mathit{G}[u_k]$
3 $d_k=\mathit{B}(u_k-{\overline{\alpha }}_k{g}_k)-u_k$ ;
4 If $f(u_k+d_k)⩽f_k^r+\delta g_k^T{d}_k$ Then
5   ${\alpha }_k=1$ ;
6 Else
7  While $f(u_k+{\eta }^j{d}_k)⩽f_k^r+{\eta }^j\delta g_k^T{d}_k$ do
8    $j=j-1$ ;
9    ${\alpha }_k={\eta }^{j+1}$ ;
10   $u_{k+1}=u_k+{\alpha }_k{d}_k$
11 Return $u_{k+1}$ ;

The complete deterministic variant of the improved Dual CHOMP for path planning is shown in Algorithm 2, where ${\epsilon }_C$ is a small positive constant, $u_1$ and ${\xi }_1$ are the initial guesses and $C_{{\xi }_{k+1}}$ is the overall cost of the path for further possible usage. The algorithm will output the path when it fulfills the terminating conditions or reaches the limited trying times. Through these modifications, the improved deterministic Dual CHOMP can now solve path planning problems with non-convex functions.

Algorithm 2. Deterministic Dual CHOMP.
Inputs: DEM of workspace, $P_s$ , O, T, $u_1$ , ${\xi }_1$ , ${\epsilon }_C\in (0,\infty )$ , ${\lambda }_o,{\lambda }_s\in (0,\infty )$ ;
1 $C_{\mathrm{MAP}}=\mathrm{BuildCostMap}(\mathrm{DEM},O)$ ;
2 For $k=1,\dots ,N$ do
3  If $(|{\mathit{F}}_{\mathrm{obs}}[{\xi }_{k+1}]-{\mathit{F}}_{\mathrm{obs}}[{\xi }_k]|>{\epsilon }_C)$ & ${\xi }_k(1)\in T$ Then
4    $\mathit{U}[\xi ]={\lambda }_o{\mathit{F}}_{\mathrm{obs}}[\xi ]+{\lambda }_s{\mathit{F}}_{\mathrm{smooth}}[\xi ]$ ;
5    $u_{k+1}\leftarrow \mathbf{Algorithm}1$ ;
6    ${\xi }_{k+1}\leftarrow {\xi }_{k+1}={\xi }_k-\frac{1}{{\eta }_k}{A}^{-1}\overline{\nabla }U[{\xi }_k]-\frac{1}{{\eta }_k}{A}^{-1}{M}^T{u}_{k+1}$ ;
7 Return: ${\xi }_{k+1}$ , $C_{{\xi }_{k+1}}$ ;

Applying simulated annealing and Hamiltonian Monte Carlo to Dual CHOMP

As pointed out in Zucker et al., ¹⁵ the initial configuration of the trajectory can greatly affect the performance of CHOMP, which also happens while applying the dual solution. We chose Hamiltonian Monte Carlo (HMC)^15,20 to modify the initial path and simulated annealing ²¹ to handle global optimization. Meanwhile, the improved deterministic Dual CHOMP still functions as a key step.

Algorithm 3 shows the simulated annealing HMC Dual CHOMP (SHD CHOMP). RRT-connect with a large step size is performed ahead of Dual CHOMP for efficiently generating an initial rough configuration ${\xi }_{\mathrm{init}}$ . And HMC is performed to achieve new modified configuration ${\xi }_{\mathrm{init}}^{\mathrm{HMC}}$ . Let $t_e$ denote the terminal temperature and $t_0$ be the initial temperature in simulated annealing. Let $\alpha$ be the cooling rate and p be the equivalent probability for the path to be accepted in the current iteration. Let ${\epsilon }_S$ be a small positive constant. Let $a,b\in [1,\infty )$ be the limited attempt times. The algorithm indicates that, after failing to converge to a strong minimum by gently altering the initial path with HMC, it can be more probable to find a valid solution by switching to a new starting configuration. The costs of history paths are stored. The best path ${\xi }_b$ with the lowest cost $C_{{\xi }_b}$ is deemed as the best output.

Algorithm 3. SHD CHOMP.
Inputs: $P_s$ , T, ${\epsilon }_S\in (0,\infty )$ , $\alpha$ , $t_e$ , $t_0$ , $i=j=1$ , a, b;
1 While $t>t_e$ do
2   $P_g=\mathrm{RandomGoal}(T\backslash O)$ ;
3   ${\xi }_{\mathrm{init}}\leftarrow \mathbf{RRT}\mathbf{-}\mathbf{connect}$ ;
4  While $i<a$ or $j<b$ do
5    ${\xi }_{\mathrm{init}}^{\mathrm{HMC}}\leftarrow \mathbf{HMC}$ ;
6    ${\xi }_k^{\mathrm{HMC}},C_{{\xi }_k}\leftarrow \mathrm{Algorithm}2$ ;
7    $\Delta C=C_{{\xi }_k}-C_{{\xi }_{k-1}}$ ;
8    $p=\exp \{-\Delta C/t\}$ ;
9   If $\Delta C⩽{\epsilon }_S$ or $p<\mathrm{Random}(0,1)$ Then
10     ${\xi }_k={\xi }_k^{\mathrm{HMC}}$ ;
11     $i=i+1$ ;
12   Else
13     $j=j+1$ ;
14   $t=t(1-\alpha )$ ;
15 Return: ${\xi }_b$ with the lowest $C_{{\xi }_b}$ ;

Let ${\gamma }_k$ denote a high-dimensional momentum in the kth iteration. The total energy of the path in HMC is $\mathcal{H}[{\xi }_k,{\gamma }_k]=\mathcal{U}[{\xi }_k]+\mathcal{K}[{\gamma }_k]$ , where $\mathcal{K}[{\gamma }_k]=\frac{1}{2}{\gamma }_k^{T}A{\gamma }_k$ is the sum of kinetic energies of all path nodes and $\mathcal{U}[{\xi }_k]$ is the potential energy of the entire path. The potential energy can be deemed to be the same as the accumulated cost of the path achieved from equation (5). The key step of HMC is the leapfrog procedure. We consider that the path ${\xi }^{*}$ in the Dual CHOMP method and the momentum ${\gamma }^{*}$ in HMC iterations are both A-inner products in the original 2D space as follows ¹⁵

$$\{\begin{array}{c}{{\xi }^{*}}^{\prime }=A^{\frac{1}{2}}\xi \prime \hfill \\ \multicolumn{1}{c}{{{\gamma }^{*}}^{\prime }=A^{\frac{1}{2}}\gamma \prime }\\ \multicolumn{1}{c}{{\overline{\nabla }}_{{\xi }^{*}}U({\xi }^{*})=A^{-\frac{1}{2}}{\overline{\nabla }}_{\xi }U(\xi )}\end{array}$$

(11)

where $\xi \prime$ and $\gamma \prime$ are the differentials of the path $\xi$ and the momentum $\gamma$ in the original 2D space, respectively. The symmetric matrix A accords with the Jordan canonical form.

Thus, the kth step of the common leapfrog scheme in the metric space A based on equation (11) can be written as

$$\{\begin{array}{c}{\gamma }_{k+\epsilon /2}^{*}={\gamma }_k^{*}-\frac{\epsilon }{2}{A}^{-\frac{1}{2}}{\overline{\nabla }}_{{\xi }_k}U({\xi }_k)\hfill \\ \multicolumn{1}{c}{{\xi }_{k+\epsilon }^{*}={\xi }_k^{*}+\epsilon {\gamma }_{k+\epsilon /2}^{*}}\\ \multicolumn{1}{c}{{\gamma }_{k+\epsilon }^{*}={\gamma }_{k+\epsilon /2}^{*}-\frac{\epsilon }{2}{A}^{-\frac{1}{2}}\overline{\nabla }{U}_{{\xi }_{k+\epsilon }}({\xi }_{k+\epsilon })}\end{array}$$

(12)

where $\epsilon$ is a temporary time interval for the leapfrog step.

The HMC procedure can encumber the computational efficiency when the path has too many nodes. A practical approach to boost the computational efficiency is to uniformly sample some nodes from the previously invalid path at the first iteration in Algorithm 3, until a rough path is determined. Then, after performing the leapfrog step with the rough path, the current path is interpolated for enough nodes before step 6 in Algorithm 3.

Simulation experiments for path planning tasks

The virtual simulation workspace is based on a square piece of the satellite map of Devon Island (around ${75}^{°}26\prime 36″\mathrm{N},{89}^{°}54\prime 10″\mathrm{W}$ ), with a resolution of 30 m. The island has been used as a Moon/Mars analog site for testing wheeled rovers.^22,23 To increase the complexity of the workspace, we added some artificial obstacle regions. Including different starting positions, goal regions, threshold slope angles and obstacle distributions, we have tested a problem set containing 1250 different conditions. All the simulation experiments have been run with MATLAB R2016b on a platform with an Intel (R) Pentium G4600 CPU.

Comparison of the deterministic Dual CHOMP and RRT-connect

We tested the improved deterministic Dual CHOMP against RRT-connect for comparison, referring to the comparisons of the original CHOMP. ¹⁵ These methods are popular and practical techniques for high-dimensional path planning problems. What we are trying to show is that Dual CHOMP can have significantly better performance under some circumstances, which provides us another choice dealing with practical problems.

The goal of RRT-connect was selected randomly in $T\backslash O$ . And we compare the average performances after multiple runs. The step size of RRT-connect was set at 2 grid spacings. We also tested the original CHOMP as a PTP planner, with the same straight-line starting configuration as Dual CHOMP. As pointed out above, the original Dual CHOMP ¹⁴ suffers from the frequent occurrence of a large singular matrix in nearly all problems, which makes it unsuitable to be transplanted into an autonomous WMR with limited computing resources. Therefore, it is not included in these comparisons. We tested the three algorithms on 1250 simulation problems five times each, for a total of 6250 runs of each algorithm.

It is important to inspect the performances without applying parameter sets specifically trained in advance for solving each problem for better outputs, as done in Zucker et al. ¹⁵ This means quite a high bias toward the randomized planner in our simulations. Optimization-based methods tend to be locked in local minima if the parameters and testing problems remain unchanged, which does not happen to randomized methods. Table 1 lists the test parameters for each algorithm. The improved Dual CHOMP and the original CHOMP shared the same parameters. The untraversable regions were dilated with 2 grid points around their outline for a safer planning. The total cost was accumulated according to the costs of grids traversed in the EMAP. The path length was a scalar value when the side length of the square cost map was set at 1. The time budget was set at 10 s.

Table 1.

The parameters for the algorithms.

EMAP	Improved Dual CHOMP and original CHOMP	SHD CHOMP
$c_{\mathrm{up}}=1.5$	${\overline{\alpha }}_k=0.1$	$t_e=1$
$c_{\mathrm{down}}=1.2$	$j=100$	$t_0=10$
$\mathrm{mg}=10$	$\delta =0.2$	$\alpha =0.5$
$E_{\mathrm{dis}}=1$	$\eta =0.3$	$a=b=2$
$w=0.1$	${\epsilon }_C=0.001$	$max(\gamma )\in [-5,5]$
$C_{max}={\epsilon }_{o2}=10$	${\lambda }_o=0.03$
$S_{\mathrm{thr}}\in \{9^{°},{30}^{°}\}$	${\lambda }_s=1$
${\epsilon }_{o1}=1$

EMAP: energy-based map; CHOMP: covariant Hamiltonian optimization for motion planning; SHD CHOMP: simulated annealing HMC Dual CHOMP.

Figure 4 shows the final results of the comparison of all the three methods mentioned above. For PTP planners, the original CHOMP does outperform RRT-connect when it succeeds. As a PTR planner, the improved Dual CHOMP achieves a success rate nearly two times higher than the original CHOMP, given the nature of being region constrained and robust to ill-distributed costs.

Figure 4.

The comparison results of the three methods. From left to right: the success rates of three methods within a time limit of 10 s, the average running time, the average path cost, the average path length of the three methods considering only successful runs and the time distributions of successful runs with improved Dual CHOMP and RRT-connect only.

For CHOMP-based methods, the optimization-driven characteristic and the untrained parameters limit the performance in success rate. When the instant success rate is a prior issue, that could be a potential problem. And that is why an approach with probabilistic completeness is necessary, which will be tested and discussed in the next section. Nevertheless, the improved Dual CHOMP significantly outperforms the randomized planner in all other aspects we are interested in. Figure 5 shows an illustration of two successful runs of Dual CHOMP and RRT-connect in the same test problem in the EMAP and the real word map. The original CHOMP fails to find a feasible path in this case. As we can see, Dual CHOMP can usually promise a shorter and smoother path, while RRT-connect usually generates some unnecessary detours encumbering its performance.

Figure 5.

(a) The final paths of two successful runs with improved Dual CHOMP (green) and RRT-connect (magenta). (b) A 3D illustration of the paths in the real world. The landscape color indicates the elevation (unit: m).

Another prominent advantage of CHOMP-based methods is that the number of trajectory nodes can be controlled without significant impact on the performance. The WMR can simply follow the path without further interpolation which may cause path distortion. Table 2 presents a comparison to show how much the number of nodes affects the performances of different methods. In contrast, after the step size was down-sized 10 times as well, RRT-connect failed to find a any feasible path within the time budget in nearly all conditions.

Table 2.

Comparison of the three methods.

Method	Original CHOMP (100 nodes)	Dual CHOMP (100 nodes)	Dual CHOMP (1000 nodes)	RRT-connect (average 63 nodes)
Success rate (%)	40.96	79.6	65.12	93.84
Average time (s)	0.0234	0.1255	1.3628	1.0141
Average cost	294.3228	304.5022	316.427	534.0613
Average path length	0.7544	0.6562	0.69954	1.1999

CHOMP: covariant Hamiltonian optimization for motion planning.

Comparison of SHD CHOMP and RRT*

We tested RRT*, ²⁴ an optimization-driven variant of RRT, against SHD CHOMP to make a fair comparison, since they are both approaches of combining optimization and randomization. During the tests, we implemented this method using the same parameters as for the deterministic version. Table 1 lists the parameters we used for simulated annealing. The step size for RRT* was 2 grid spacings.

The two methods can both yield feasible and optimal results if kept running. The comparison here is mainly focused on evaluating the performance of the first feasible path. We used them to test 255 problems which were previously unsolvable for improved deterministic Dual CHOMP. The time budget was set at 10 s. The results shown in Table 3 are from 1275 runs with each algorithm. Figure 6 shows an example of two successful runs of SHD CHOMP and RRT* for the same test problem in an EMAP.

Table 3.

Simulation results of previously unsolvable problems.

Method	SHD CHOMP	RRT*
Success rate (%)	88.63	44.7
Average time (s)	1.8996	3.8803
Average cost	494.7993	480.94
Average path length	1.1013	1.0555

SHD CHOMP: simulated annealing HMC Dual CHOMP; RRT: rapidly exploring random tree.

Figure 6.

(a) The final paths of two successful runs with SHD CHOMP (green) and RRT* (magenta). (b) A 3D illustration of the paths in the real world. The landscape color indicates the elevation (unit: m).

Through the implementation of simulated annealing and HMC, SHD CHOMP significantly outperforms RRT* in success rate and average planning time. If a larger step size is chosen for initial path generator, that is, RRT-connect, the run time can be shortened while maintaining a high success rate according to our research (with a step size of 4 grid spacings, the success rate is 89.65% and the mean run time is 0.93 s). Smoothness is not well guaranteed by SHD CHOMP in the current tests. The primary reason is that the parameters of improved Dual CHOMP have remained unchanged in our study as mentioned in the previous section. This limits its flexibility to balance the smoothness and obstacle cost functions, leading to weak minima, that is, non-smooth paths.

Conclusion

In this paper, we propose the improved Dual CHOMP method, which is an optimization-driven PTR path planning method. It is designed to smoothly navigate a planetary WMR from its current position to a region of research interests from a global view. The method is composed of two variants: the improved deterministic Dual CHOMP can achieve a path of quality efficiently; SHD CHOMP can guarantee the probabilistic completeness on the basis of the deterministic variant. By switching to the randomized variant when the deterministic one fails, we can achieve an overall success rate of 97.68% in our problem set, without training different preset sets of parameters for each problem in advance or online adjusting parameters specifically.

By establishing an energy-based cost map with estimated energy consumption, the optimal result from the improved Dual CHOMP method can be energy saving. The generated path is of high quality for mission-oriented planetary exploration using a WMR. By designing other types of cost maps, the method can solve similar optimization problems.

The main weakness of the method is that the deterministic variant has a relatively lower success rate against other randomized planners. This makes the approach more suitable for planning tasks placing a priority on path quality instead of an instant success. Also it cannot deal with path planning problems in maze-like maps directly. One has to segment maze-like maps before applying the method. SHD CHOMP, the randomized variant, suffers from bad smoothness on account of the unchanged parameters in this research. On the other hand, this indicates that it can function better in our further research of developing a task-directed parameter adjustment technique with a real WMR. The method proposed is not designed to replace other path planning approaches in all practical situations, but rather provide a better alternative when available.