A new local path planning approach based on improved dual covariant Hamiltonian optimization for motion planning method

Abstract

We propose a new local path planning approach based on optimization methods with probabilistic completeness in this article. This approach adds a linear constraint to the original covariant Hamiltonian optimization for motion planning problem with a new cost function. By deducing the dual form, the path planning problem is described as a box-constrained quadratic programming problem. The nonmonotone gradient projection algorithm is introduced to solve the dual problem, which makes the algorithm adaptable to non-convex cost functions. In order to prevent early convergence at local minima that can occur when applying optimization methods, this article introduces Hamiltonian Monte Carlo to the modification, which constantly forces the initial path to jump out of the local extremum, thus improving the robustness and success rate of the path planning approach. Compared with other methods through simulations, this approach is proven to provide balanced planning efficiency and path quality. The feasibility in a real environment is experimentally validated by applying the approach to a wheeled mobile robot.

Introduction

Path planning has long been a research topic in the development of mobile robots. It is highly related to robot autonomy, as well as their performance and completion of motion tasks. In the field of space exploration, almost all missions involving exploration of other planets use wheeled mobile robots (WMRs).^1,2 For WMRs with limited steering ability such as planetary rovers, the path cannot be well tracked if not smooth enough. Meanwhile, planetary exploration missions usually involve regions of research interest instead of a single target position. Therefore, research into smooth and mission-directed path planning methods is practically significant for these situations.

Some researchers consider general path planning research to involve researching point-to-point (PTP) locomotion mostly, because a single complicated path planning problem can be decomposed into a series of continuous PTP tasks.^3,4 For low-dimensional path planning problems, there are some popular approaches: cell decomposition methods like A*-based ⁵ and D*-based ⁶ methods, potential field methods like the artificial potential field method and its variants, ⁷ graphical methods like the Voronoi diagram, ⁸ and other intelligent methods. ⁹ These methods stress the instant success prior to quality of paths. Effective methods for solving high-dimensional path planning problems are randomization-based and optimization-based approaches, such as rapidly-exploring random trees (RRT), ¹⁰ covariant Hamiltonian optimization for motion planning (CHOMP), ¹¹ and stochastic trajectory optimization for motion planning (STOMP). ¹² Optimization-based methods can usually maintain a balance between the success rate and path quality.

In recent years, researchers have realized that point-to-region (PTR) path planning problems are difficult to solve with traditional PTP planners. Optimization and probabilistic methods were shown to be more suitable for solving PTR problems such as mission-directed explorations previously mentioned, due to their efficiency in addressing high-dimensional tasks. T Peynot et al. ¹³ constructed a probability-based control policy involving mobility prediction in various terrain types for robot navigation toward a square goal region, which emphasizes path traversability rather than path quality. S Persson and I Sharf ¹⁴ regard path planning toward a goal region as a sampling-based optimal problem, thus probabilistic completeness and fast convergence are guaranteed. However, the path obviously lacks smoothness. M Davoodia et al. ¹⁵ used an evolutionary algorithm to solve multi-objective path planning problems using a geometric smoothing technique. However, the post-smoothing technique can actually distort the original paths to some extent, leading to uncertainty regarding the feasibility of a forced-smoothed path in a real environment. S Choudhury and S Scherer ¹⁶ derived the dual form of the original CHOMP problem by adding a box constraint to the original CHOMP problem, ¹¹ which can effectively solve linear goal region–constrained problems. However, this method and the original CHOMP do not work well with non-convex cost functions.

In this article, we take a step further to improve dual CHOMP by redesigning the cost function and introducing a nonmonotonic gradient searching approach. After these improvements, it becomes capable of local PTR path planning missions within ill-distributed cost maps, which are difficult for traditional CHOMP-based methods. We propose using the Hamiltonian Monte Carlo (HMC) method to improve the dual CHOMP method in order to guarantee the probabilistic completeness. Thus, it can solve local minima problems frequently encountered in the practical utilization of CHOMP-based method.

The remainder of this article is organized as follows. The map representation of environment and the kinematic model for a WMR are presented in “Map representation and kinematic model for a WMR” section. In “The path planning method based on improved dual CHOMP” section, the original dual CHOMP method is improved by modifying the cost function and introducing a nonmonotonic gradient searching approach for path planning missions in ill-distributed cost maps. The modified method based on HMC in Riemannian space is applied for probabilistic completeness. In “Simulations and analysis” section, simulations are performed to demonstrate the performance of the method. In “Experiment in a real environment” section, an experiment with our planetary rover in a real environment is performed to test the actual feasibility of the method. Finally, conclusions are presented in the “Conclusion” section.

Map representation and kinematic model for a WMR

A grid cost map is established to represent obstacles that may interfere the locomotion of a WMR, such as areas occupied by walls and rocks. We assume the obstacles are stationary relative to the WMR from a local view within limited time. The following cost of a grid is defined to prevent a WMR from being too close to obstacles

$$c\left(x\right)=\{\begin{array}{c}-D\left(x\right)+\alpha \mathrm{if}D\left(x\right)⩽0\hfill \\ \frac{1}{\alpha }{(D\left(x\right)-\alpha )}^2\mathrm{if}0<D\left(x\right)⩽\alpha \hfill \\ 0\mathrm{others}\hfill \end{array}$$

(1)

where x is any grid in the map, $D(x)$ is the distance between the grid and the boundary of the nearest obstacle, $\alpha$ is an adjusting coefficient, and $c(x)$ is the cost of the grid. Thus, the cost map of a distance field filled by these distance-involving costs can be established.

A WMR system can generally be classified as a nonholonomic system, which has under-actuated characteristics. In order to focus the discussion on path planning problems, the locomotion of a WMR is assumed to be performed in a two-dimensional space. The model of the six-wheel robot we are using is simplified to a two-wheel kinematic model regardless of slip and skid, as shown in Figure 1. The pose of the entire WMR in a global coordinate system is defined as $q_c=[x_c{y}_c{\theta }_c{]}^T$ with respect to the WMR’s geometrical center. The linear velocity of the WMR is $v_c$ , the steering angular velocity is $w_c$ , and the target pose of the WMR is $q_r=[x_r{y}_r{\theta }_r{]}^T$ . The distance between the centers of the two wheels is $2L$ and the diameter of a wheel is $2r$ .

Figure 1.

The kinematic model of the wheeled mobile robot.

The pose error of the WMR can be written as

$$q_e=\left[\begin{array}{c}{x}_e\hfill \\ \multicolumn{1}{c}{y_e}\\ \multicolumn{1}{c}{{\theta }_e}\end{array}\right]=\left[\begin{array}{ccc}\cos {\theta }_c\hfill & \sin {\theta }_c\hfill & 0\hfill \\ \multicolumn{1}{c}{-\sin {\theta }_c}& \cos {\theta }_c\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 1\hfill \end{array}\right]\left[\begin{array}{c}{x}_r-x_c\hfill \\ \multicolumn{1}{c}{y_r-y_c}\\ \multicolumn{1}{c}{{\theta }_r-{\theta }_c}\end{array}\right]$$

(2)

Thus, the following differential equation for the pose error can be derived

$$q{\prime }_e=\left[\begin{array}{c}x{\prime }_e\hfill \\ \multicolumn{1}{c}{y{\prime }_e}\\ \multicolumn{1}{c}{\theta {\prime }_e}\end{array}\right]=\left[\begin{array}{c}{y}_e{\omega }_c-v_c+v_r\cos {\theta }_e\hfill \\ \multicolumn{1}{c}{v_r\sin {\theta }_e-x_e{\omega }_c}\\ \multicolumn{1}{c}{{\omega }_r-{\omega }_c}\end{array}\right]$$

(3)

We can see that the connection between the control input $[\begin{array}{cc}{v}_c\hfill & {\omega }_c\hfill \end{array}]$ and the pose of the WMR has been defined in equations (2) and (3). Therefore, the remaining navigation problem for the WMR is path planning, which will be discussed in the following sections.

The path planning method based on improved dual CHOMP

CHOMP-based methods treat an overall path $\xi$ as a high-dimensional point. We slightly alter the original description of the trajectory cost as follows

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

(4)

where ${\mathcal{F}}_{\mathrm{obs}}[\xi ]$ is the obstacle cost functional, ${\mathcal{F}}_{\mathrm{smooth}}[\xi ]$ is the smoothness cost functional, and ${\lambda }_o$ and ${\lambda }_s$ are weight coefficients. In a certain Riemannian metric space M, the functional gradient ¹¹ is ${\overline{\nabla }}_M\mathcal{U}[\xi ]=M^{-1}\overline{\nabla }\mathcal{U}[\xi ]$ .

Improved dual CHOMP

The start and goal points in a two-dimensional space can be seen as the locations of WMR at times $t=0$ and $t=1$ . Thus, the nodes of the planned path for WMR are distributed uniformly within a time span $t=[0,1]$ . Thus, we can write the cost functionals as

$$\{\begin{array}{c}{\mathcal{F}}_{\mathrm{obs}}\left[\xi \right]=\underset{0}{\overset{1}{\int }}C\left(\xi \left(t\right)\right)\mathrm{dt}\hfill \\ \multicolumn{1}{c}{{\mathcal{F}}_{\mathrm{smooth}}\left[\xi \right]={\sigma }_k\underset{0}{\overset{1}{\int }}{\Vert \xi \prime \left(t\right)\Vert }^2\mathrm{dt}+{\sigma }_c\underset{0}{\overset{1}{\int }}\Vert {\omega }_p\xi ″\left(t\right)\Vert \mathrm{dt}}\end{array}$$

(5)

where $\xi (t)$ is the coordinate of the path node at time t, $C(\xi (t))$ is the cost value of a node in the distance field map, $\xi \prime (t)$ is the virtual velocity, ${\omega }_p=\xi \prime (t)\rho$ is the virtual angular velocity, $\rho$ is the curvature of the path at the node, and ${\sigma }_k$ and ${\sigma }_c$ are adjustment coefficients. The obstacle functional is the accumulated cost of the entire path in the cost map. The smoothness functional is a scalar expression of the virtual inherent kinetic energy for the path and the overall effect of the virtual Coriolis force for the path.

The smoothness functional we propose in this article is different from that used in the original CHOMP method. ¹¹ Intuitively, the virtual Coriolis force applied to the current path node changes along with the steering angle and linear velocity while the WMR follows the planned path. This accounts for radial and tangential changes. On the contrary, the original CHOMP method only considers changes in the tangential value along the path, which we find will cause the planning procedure to be too sensitive to the smoothness functional during our research. After applying this modification, the value of the smoothness cost functional will change gently even when the nodes of path is sparse. On the contrary, the original CHOMP method tends to either diverge quickly when path nodes become sparse or converge too fast due to the fast growth of the virtual kinetic energy. The following theorem and proof are given regarding this point.

Theorem

The change in kinetic energy between two nodes of the path calculated from the original CHOMP will be larger than defined in equation (5) if the two path nodes are far away enough and the curvature of the path during a time interval $\Delta t$ is small enough.

Proof

Assume the time interval $\Delta t$ is certain. The virtual kinetic energy of the original CHOMP method during $\Delta t$ is ¹¹

$${\mathcal{F}}_{\mathrm{smooth}}^{*}\left[\xi \left(\Delta t\right)\right]=\Vert \xi \prime \left(\Delta t\right){\Vert }^2$$

(6)

Considering an extreme situation where ${\sigma }_k=0$ and ${\sigma }_c=1$ in equation (5), we can derive

$${\mathcal{F}}_{\mathrm{smooth}}\left[\xi \left(\Delta t\right)\right]=\Vert \xi \prime \left(\Delta t\right)\rho \xi ″\left(\Delta t\right)\Vert$$

(7)

where the curvature is $\rho =|\xi \prime (\Delta t)|/(1+\xi ″(\Delta t{)}^2{)}^{3/2}$ . Considering two adjacent path nodes $q_a$ and $q_b$ in two dimensions, between which the curvature is small, we can derive $\xi \prime (\Delta t)\approx (q_b-q_a)/\Delta t$ and $\Vert \xi \prime (\Delta t){\Vert }^2\approx \xi \prime (\Delta t{)}^2$ . Therefore, we let

$$\begin{array}{cc}\multicolumn{1}{c}{\kappa =\left|\frac{{\mathcal{F}}_{\mathrm{smooth}}^{*}\left[\xi \left(\Delta t\right)\right]}{{\mathcal{F}}_{\mathrm{smooth}}\left[\xi \left(\Delta t\right)\right]}\right|}& =\frac{{\Vert \xi \prime \left(\Delta t\right)\Vert }^2}{(\Vert \xi \prime \left(\Delta t\right)\xi ″\left(\Delta t\right)\rho \Vert )}\hfill \\ =\frac{{\Vert \xi \prime \left(\Delta t\right)\Vert }^2}{\Vert \frac{\xi \prime \left(\Delta t\right)\xi ″\left(\Delta t\right)|\xi \prime \left(\Delta t\right)|}{{(1+\xi ″{\left(\Delta t\right)}^2)}^{\frac{3}{2}}}\Vert }\hfill \\ =\left|\frac{\xi \prime {\left(\Delta t\right)}^2}{\left(\frac{\xi \prime {\left(\Delta t\right)}^2\xi ″\left(\Delta t\right)}{{(1+\xi ″{\left(\Delta t\right)}^2)}^{\frac{3}{2}}}\right)}\right|\hfill \\ =\left|\frac{{(1+\xi ″{\left(\Delta t\right)}^2)}^{\frac{3}{2}}}{\xi ″\left(\Delta t\right)}\right|\hfill \end{array}$$

(8)

Here, we consider two different extreme conditions when the virtual acceleration $|\xi ″(\Delta t)|$ during $\Delta t$ is at its upper and lower limits, where $\xi \prime (\Delta t)$ is changing continuously and smoothly.

1. If $|\xi ″(\Delta t)|\to 0$ , which is the lower limit, we obtain $\kappa \to +\infty$ .

2. If $|\xi ″(\Delta t)|=|(\xi \prime (\Delta t)-0)/\Delta t|=|(q_b-q_a)/\Delta t^2|$ , which is the upper limit, we obtain

$$\begin{array}{c}\multicolumn{1}{c}{\kappa =\left|\frac{\frac{1}{2}{(1+\frac{{(q_b-q_a)}^2}{\Delta t^4})}^{\frac{3}{2}}}{\left(\frac{(q_b-q_a)}{\Delta t^2}\right)}\right|}\\ =\left|\frac{1}{2\Delta t^4}{(1+\frac{\Delta t^4}{{(q_b-q_a)}^2})}^{\frac{3}{2}}{(q_b-q_a)}^2\right|\hfill \\ >\frac{1}{2\Delta t^4}{(q_b-q_a)}^2\hfill \end{array}$$

(9)

Thus, $\kappa >1$ when $|q_b-q_a|>\sqrt{2}\Delta t^2$ .

Under the above assumptions, we know that the inequality $\kappa >1$ will hold as long as $|q_b-q_a|>\sqrt{2}\Delta t^2$ is satisfied, which means $|{\mathcal{F}}_{\mathrm{smooth}}^{*}[\xi (\Delta t)]|>|{\mathcal{F}}_{\mathrm{smooth}}[\xi (\Delta t)]|$ . End.

Normally, $\Delta t$ is quite small, which means the conditions to hold this theorem is usually easy to be satisfied.

After applying a first-order Taylor expansion to the cost functional $\mathcal{U}[\xi ]$ in metric space M, a simplified form can be written for the equation about the path $\xi$

$$\begin{array}{c}\multicolumn{1}{c}{{\xi }_{k+1}=\underset{\xi }{\arg \min }\mathcal{U}\left[{\xi }_k\right]+{(\xi -{\xi }_k)}^T{\overline{\nabla }}_M\mathcal{U}\left[{\xi }_k\right]}\\ +\frac{{\eta }_k}{2}\Vert \xi -{\xi }_k{\Vert }_M\hfill \end{array}$$

(10)

where ${\eta }_k$ is the weight coefficient.

The equation (10) is actually a non-convex programming problem without constraints, which usually must be solved by deducing its dual form. Therefore, referring to the literature, ¹⁶ a general linear constraint $C\xi ⩽d$ is added to equation (10), where C and d are linear expressions for the constrained region. After applying Lagrangian linearization to the dual form, we can derive a quadratic programming equation for the dual factor u ¹⁶

$$\begin{array}{c}{u}_{k+1}=\underset{u}{\arg \min }\mathit{G}\left(u\right)=\underset{u}{\arg \min }\frac{1}{2{\eta }_k}{u_k}^{T}C{M}^{-1}{C}^T{u}_k\hfill \\ -{u_k}^T(C{\xi }_k-d-\frac{1}{{\eta }_k}C{M}^{-1}\overline{\nabla }\mathcal{U}\left[{\xi }_k\right])\hfill \\ \mathrm{Subject}\mathrm{to}u⩾0\hfill \end{array}$$

(11)

The primal solution is recovered from equation (11) and is written as

$${\xi }_{k+1}={\xi }_k-\frac{1}{{\eta }_k}{M}^{-1}\overline{\nabla }\mathcal{U}\left[{\xi }_k\right]-\frac{1}{{\eta }_k}{M}^{-1}{C}^T{u}_{k+1}$$

(12)

The dual factor u can be updated by iteratively applying equation (11). The original problem can then be solved. This equation by nature is a box-constrained quadratic convex programming problem. However, during daily exploration missions, we do not usually apply strong interpolation to the map in case unpredictable map distortion occurs. Therefore, the original dual CHOMP method cannot work well in a non-smooth cost map by generating non-convex cost functions.

We introduce nonmonotone gradient projection algorithm (NPGA) to amend this weak point brought by equation (11) and solve the dual problem. We simplify NGPA for an easy application in a real robot computing platform. Let $\Omega \subset {\mathbf{R}}^n$ be a non-empty convex closed set, which represents the linear constraint $C\xi ⩽d$ . The gradient projection step ¹⁷ is shown in Figure 2, where ${\overline{u}}_k$ is the result of a single iteration, $u_k$ is the original value, ${\overline{\alpha }}_k$ is the step length, and $d_k$ is the gradient direction. This yields the gradient projection $\mathit{B}({\overline{u}}_k)$ of the point $(u_k,v_k)$ onto $\Omega$ . The reference function is chosen as $f_k^r=\mathit{G}[u_k]$ , where $\mathit{G}[u_k]$ has been defined in equation (11).

Figure 2.

Gradient projection step.

The path planning problem can now be divided into two major steps. First, the dual factor $u_k$ is obtained after inputting the old path ${\xi }_k$ . Then, the new path ${\xi }_{k+1}$ with potentially lower cost is obtained by substituting $u_k$ into equation (12).

The complete improved dual CHOMP method for non-convex cost functions is shown in Algorithm 1, where $P_s$ is the starting point of the path, O is the space containing obstacle information, T is the linear target region confined by $C\xi ⩽d$ , $u_0$ and ${\xi }_0$ are the initial values, ${\epsilon }_c\in (0,\infty )$ is a small positive number defining the terminal condition, ${\overline{\alpha }}_k$ is the initial attempting step length, and $\delta$ , $\eta$ , and j are the parameters for Armijo–Goldstein line search scheme. ¹⁸

Algorithm 1. Improved dual CHOMP algorithm
Input: $P_s$ , O, T, $u_0$ , ${\xi }_0$ , ${\epsilon }_c$ , ${\overline{\alpha }}_k$ , $\delta$ , $\eta$ , j
1 $\mathrm{BuildCostMap}(O)$ ;← Establish distance field map
2 For $k=1\dots \mathrm{N}$ do
3  While $(|{\mathcal{F}}_{\mathrm{obs}}[{\xi }_{k+1}]-{\mathcal{F}}_{\mathrm{obs}}[{\xi }_k]|>{\epsilon }_c)$ & ${\xi }_k(1)\in T$ do
4    $\mathcal{U}[\xi ]={\lambda }_o{\mathcal{F}}_{\mathrm{obs}}[\xi ]+{\lambda }_s{\mathcal{F}}_{\mathrm{smooth}}[\xi ]$ ;←equation (4)
5    $g_k=\overline{\nabla }\mathit{G}[u_k]$
6    $d_k=\mathit{B}(u_k-{\overline{\alpha }}_k{g}_k)-u_k$ ;
7   If $f(u_k+d_k)⩽f_k^r+\delta g_k^T{d}_k$ Then
8     ${\alpha }_k=1$ ;
9   Else
10    While $f(u_k+{\eta }^j{d}_k)⩽f_k^r+{\eta }^j\delta g_k^T{d}_k$ do
11      $j=j-1$ ;
12     ${\alpha }_k={\eta }^{j+1}$ ;
13    $u_{k+1}=u_k+{\alpha }_k{d}_k$ ;
14   ${\xi }_{k+1}$ ←equation (12)
15 Output: ${\xi }_{k+1}$

HMC dual CHOMP algorithm

The improved dual CHOMP uses gradient descent scheme to find minima through iterations. This may also make the algorithm be locked to local minima causing failures in path planning, which commonly occur in optimization-based path planners. Therefore, it is necessary to find an approach that guarantees probabilistic completeness of the method in this article, thus increasing the success rate of finding valid paths.

The initial path chosen for path searching has a great influence on the result when other parameters are held constant. Here, we propose introducing HMC ¹⁹ to improve dual CHOMP in order to increase the path searching ability autonomously without requiring training in advance as done in He et al. ¹¹

When the algorithm detects that the final path generated by Algorithm 1 is locked to a local minimum, that is, obstacles interfere with the path, the algorithm will determine a new initial path using HMC in the metric space M. After repeating this procedure, the algorithm will eventually find an available path for a WMR.

The key step of HMC is the leapfrog procedure. According to the theory presented in He et al., ¹¹ we consider that the path ${\xi }^{*}$ in the improved dual CHOMP method and the momentum ${\gamma }^{*}$ are both M-inner products in the original two-dimensional space, shown as follows

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

(13)

Thus, the leapfrog scheme of HMC in metric space M can be rewritten as

$$\{\begin{array}{c}{\gamma }_{t+\frac{\epsilon }{2}}={\gamma }_t-\frac{\epsilon }{2}{M}^{-\frac{1}{2}}{\overline{\nabla }}_{{\xi }_t}\mathcal{U}\left({\xi }_t\right)\hfill \\ \multicolumn{1}{c}{{\xi }_{t+\epsilon }={\xi }_t+\epsilon {\gamma }_{t+\frac{\epsilon }{2}}}\\ \multicolumn{1}{c}{{\gamma }_{t+\epsilon }={\gamma }_{t+\frac{\epsilon }{2}}-\frac{\epsilon }{2}{M}^{-\frac{1}{2}}{\overline{\nabla }}_{{\xi }_{t+\epsilon }}\mathcal{U}\left({\xi }_{t+\epsilon }\right)}\end{array}$$

(14)

where $\epsilon$ is a temporary time interval for the leapfrog step, $\xi$ is the path, and $\gamma$ is the momentum.

Since we treat the path as a high-dimensional point, the kinetic energy of WMR in HMC is set as $\mathcal{K}(\gamma )=\frac{1}{2}{\gamma }^{\mathrm{T}}M\gamma$ , which is the sum of kinetic energies of all path nodes. The potential energy of path $\xi$ is $\mathcal{U}(\xi )$ , which is the overall cost of the path. Thus, the total energy of the path is $\mathcal{H}({\varsigma }_t)=\mathcal{U}({\xi }_t)+\mathcal{K}({\gamma }_t)$ , where ${\zeta }_t$ is short for $\{{\xi }_t,{\gamma }_t\}$ .

The entire algorithm is shown in Algorithm 2, where ${\gamma }_1$ is the initial momentum applied to the path, ${\gamma }_t$ is a matrix defining Gaussian distributed random noise applied to the momentum that corrupts the current path at time t, $P_{\mathrm{leap}}^t$ is the probability of the current leapfrog step, $\beta$ is the parameter of the leapfrog method, and $t_{\max }$ is a large enough integer. During practical application, we would like to confine the number of elements in $\xi$ when performing leapfrog in order to increase the efficiency of the algorithm.

Algorithm 2. HMC dual CHOMP
Input: ${\gamma }_1$ , ${\lambda }_o$ , ${\lambda }_s$ , $\beta$ , $t_{\max }$
1 For $t=1,2,\dots ,t_{\max }$
2  Ifstuck in local minima Then
3     $\mathcal{U}[\xi ]={\lambda }_o{\mathcal{F}}_{\mathrm{obs}}[\xi ]+{\lambda }_s{\mathcal{F}}_{\mathrm{smooth}}[\xi ]\leftarrow$ equation (4)
4   If $t=1$ Then
5     ${\xi }_1$ $\leftarrow {\gamma }_1$
6   Else
7     ${\xi }_t$ $\leftarrow {\gamma }_t$
8    $P_{\mathrm{leap}}^t=\min (1,\exp \{-\beta (\mathcal{H}({\zeta }_t)-\mathcal{H}({\zeta }_{t-1}))\})$ ;
9   If $t=1$ Then
10     $P_{\mathrm{leap}}^1=P_{\mathrm{leap}}^t$ ;
11   Else
12    If $P_{\mathrm{leap}}^t>P_{\mathrm{leap}}^{t-1}$ Then
13      ${\zeta }_{t+1}={\zeta }_{t+\epsilon }$ ;
14     Perform Algorithm 1 with ${\xi }_{t+1}$ as initial path;
15    Else
16      ${\zeta }_{t+1}={\zeta }_t$ ;
17  Else
18   Break For;
19 Output: ${\xi }_{k+1}$

Thus, the entire path searching progress functions via the following two major steps:

1. Perform Algorithm 1 to determine an optimal path for WMR for the first time.

2. Perform Algorithm 2 when Algorithm 1 fails and repeat Algorithm 2 until a feasible path is found.

A practical approach to boost the computational efficiency is to uniformly sample from the previously invalid path at the first iteration in Algorithm 2 until a rough path is determined. Then, after the leapfrog step with the rough path, we interpolate it linearly with enough nodes just before performing Algorithm 1.

Simulations and analysis

We performed a series of path planning simulations that consider problems which may occur in real environments. To test the performance of the HMC dual CHOMP method, we compared it with the original CHOMP and RRT* algorithm. ²⁰ We compared HMC dual CHOMP with RRT* because they are integrated path planning methods that perform both random searching and optimization-based searching.

The method in this article and RRT* both have probabilistic completeness, which means they can autonomously find feasible solutions after enough attempts and suitable parameter adjustments under most situations. Therefore, we test their performances on the following strict conditions:

1. The preset parameters remain unchanged in all simulations.

2. We only compare the performance of the first feasible solution regardless of all the other feasible solutions that may be found after further attempts.

3. Solutions must be found within a certain time budget, or else, the solutions are seen as invalid.

Algorithm 2 will uniformly sample 10 nodes out of the 100 nodes from the current path ${\xi }_t$ as the input of HMC to decrease the computational time. After HMC finds a new rough path with higher probability, Algorithm 2 will interpolate the rough path to 100 nodes and use this path as the initial path in Algorithm 1. The search step length of RRT* is 2 grids.

Although RRT* is a PTP planner, it can also be seen as a PTR path planner by sampling from the goal region in the HMC dual CHOMP method (Table 1). We let RRT* sample five available goal points from the goal region and calculated the average performance for comparison. All the initial points of the three path planners were the same and the time budget for path searching was set to 20 s.

Table 1.

HMC dual CHOMP and original CHOMP parameters.

Parameters	Values
${\overline{\alpha }}_k$	0.1
j	100
$\delta$	0.2
$\eta$	0.3
${\epsilon }_c$	0.01
${\lambda }_o$	20
${\lambda }_s$	1
${\sigma }_k$	0.5
${\sigma }_c$	0.05
$\beta$	10

HMC: Hamiltonian Monte Carlo; CHOMP: covariant Hamiltonian optimization for motion planning.

We performed simulations with the three methods in the same preset distance field cost map on the same computer with a G4600 CPU. The start points were selected from the left edge of the map. The linear goal regions were selected from the right edge of the map. This yielded a total of 90 sets of start–goal combinations. We performed five simulations for each start-goal set with the three path planning methods. The results from the simulations are shown in Table 2. The length of each path is a scalar value when the scalar length of the map border is set to $1\times 1$ .

Table 2.

The comparison of simulation results from the three algorithms.

Method	HMC dual CHOMP	Original CHOMP	RRT*
Success rate (%)	88.2	22.2	23.78
Avg. path nodes	100	100	34
Avg. planning time(s)	0.2459 (±0.035)	0.02551 (±0.0048)	12.27 (±4.41)
Avg. path length	0.9047 (±0.1)	0.9582 (±0.054)	1.1664 (±0.15)

HMC: Hamiltonian Monte Carlo; CHOMP: covariant Hamiltonian optimization for motion planning.

The simulation results show that the overall performance of the HMC dual CHOMP method significantly outperforms the other methods in most aspects. One should note that we found $C{M}^{-1}{C}^T$ was often singular in these tests, which does not satisfy the key condition in the original dual CHOMP method. ¹⁶ Therefore, it was not tested for comparison in this article. The original CHOMP method is fast, but it suffers from a low path planning success rate when dealing with non-convex cost functions.

The number of path nodes from CHOMP-based methods were preset, while that from RRT* were post-calculated. Normally, the number of path nodes cannot be accurately controlled while performing RRT-based methods. The computational burden will grow exponentially with RRT-based methods when the size of the searching step decreases, while it only grows linearly with the CHOMP-based method. In a practical application, one can directly use a path with enough nodes as the tracking target for a WMR. Post interpolation may cause great distortion of the original path if a low number of path nodes is used.

The paired successful path searching results from HMC dual CHOMP and RRT* are shown in Figure 3. The cost value of the cost field map is shown in grayscale. The starting point is marked with a circle on the left edge. The final reached goal points are marked with a star or a triangle. The target goal region is confined by a dashed line. Obviously, the path generated with HMC dual CHOMP is smoother and the WMR can track it more easily. On the contrary, the RRT* path has sharp turns that may impede smooth locomotion.

Figure 3.

HMC dual CHOMP and RRT* results in the same path planning problem.

The searching history of the HMC dual CHOMP method when Algorithm 1 is successful is shown in Figure 4. The overall cost of the path will push the trajectory away from the high cost regions as they are inhibited by obstacles. The changing history of the overall path cost is shown in Figure 5. The obstacle cost functional initially drives the path away from the obstacles. Then, the smoothness cost functional plays the major role in smoothing the path until the path searching terminal conditions are fulfilled.

Figure 4.

Searching history with the HMC dual CHOMP method in a distance field map.

Figure 5.

Overall path cost history.

The progress of the HMC dual CHOMP method as it drives the path away from local minima, that is, encountering collisions, is shown in Figure 6. We can see that Algorithm 2 will continuously search for valid paths after adding random momentum to the path when Algorithm 1 fails.

Figure 6.

Progress of the algorithm as it jumps out of local minima.

It should be mentioned that HMC dual CHOMP, which places greater emphasis on optimization, is shown to be an effective approach for daily exploration tasks while maintaining high path qualities that we are interested in. However, the RRT-based method and other methods, which place greater emphasis on randomization, will obviously have more advantages for the maze-like map. By segmenting large maps into smaller local maps, the method proposed in this article can still intuitively function well. This is also the reason we emphasize that HMC dual CHOMP is a strong candidate for addressing local path planning problems.

Experiment in a real environment

We built an experimental environment for a planetary WMR in order to verify the feasibility of the algorithm in this study in a real environment. The hardware configuration of the entire system is shown in Figure 7. The host PC is responsible for managing the visual measurement information of OptiTrack, which contains the positions of obstacles and current locomotive status of the WMR. With this information, the host PC can perform path planning with the HMC dual CHOMP. The slave PC is in charge of commanding the WMR to track the planned path.

Figure 7.

Hardware configuration in the experiment.

The layout of the experimental site is shown in Figure 8. We chose the far corner region as the target region. The WMR moves with a uniform linear velocity of $v_C=8\mathrm{mm}/\mathrm{s}$ . The path planning test results in this small map are shown in Figure 9. It shows that the path planned with the method presented in this article can be easily followed by the WMR. We also find it very difficult for RRT* to function under these conditions, for example, in a confined experimental site, the planned path produced with RRT* is likely to have sharp turns. It is impossible for a WMR with limited locomotive ability to directly track sharp turns such as the one used here, which usually causes unexpected collisions.

Figure 8.

Experimental site.

Figure 9.

Comparison of the planned path and the actual path (unit: mm).

Conclusion

We propose using a new PTR path planning method based on the original dual CHOMP method to address local exploration tasks for a WMR. After improving the structure of the cost functions and introducing a simplified NGPA, the method can now sufficiently handle ill-distributed cost maps containing non-convex cost functions. We went a step further toward enhancing the improved dual CHOMP method with probabilistic completeness by proposing the HMC dual CHOMP method. The method can provide an inherently smooth path for WMRs with limited steering ability such as a planetary rover performing local exploration missions. The generated path can usually be directly used to guide a WMR. The success rate is significantly higher than that provided by other algorithms. The path quality is also guaranteed by emphasizing optimization in our method, for example, the path is smoother and the path length is reduced by 22.4% at most. The average planning time in the simulations is 0.246 s with a CPU of low frequency, which can be used for real-time path planning in a real environment.

This method currently cannot sufficiently address path planning problems in large maps and maze-like maps. Although the simulations have produced some positive results, more thorough and complicated experiments in a real complex environment should be performed in future because the size of the experimental site is currently confined. The method can be modified to provide better performance for our WMR by specifically considering the structure of the planetary six-wheel robot we are using to meet the nonholonomic constrain. The real-time performance can be boosted by improving the computational efficiency with state-of-the-art nonlinear fractional calculus methods^21,22 in our further work. The results from the simulation and experiment indicate that the method presented in this article can be modified specifically and applied to other common mobile robots when performing PTR path planning tasks.