A Random Sampling-Based Method via Gaussian Process for Motion Planning in Dynamic Environments

Abstract

Motion planning is widely applied to industrial robots, medical robots, bionic robots, and smart vehicles. Most work environments of robots are not static, which leads to difficulties for robot motion planning. We present a dynamic Gaussian local planner (DGLP) method to solve motion planning problems in dynamic environments. In a dynamic environment, dynamic obstacles sometimes make part of the global path invalid, so the local invalid path needs to be local re-planned online. Compared with the node sampling-based methods building large-scale random trees or roadmaps, the Gaussian random path sampling (GRPS) module integrated in the DGLP directly samples smooth random paths discretized into sparse nodes to improve the local path re-planning efficiency. We also provide the path end orientation constraint (PEOC) method for the local re-planning paths in order to merge them smoothly into the global paths. In the robot experiments, the average planning time of the DGLP is 0.04s, which is at least 92.31% faster than the test methods, and its comprehensive evaluation scores, which consider the consuming time, path quality, and success rate of local re-planning, are at least 44.92% higher than the test methods. The results demonstrate that the proposed DGLP method is able to efficiently provide high-quality local re-planning paths in dynamic environments.

1. Introduction

An autonomous robot system usually needs to consider mechanical design [1], visual perception [2], kinematics [3], dynamics [4], motion planning and control [5], etc. Motion planning is an essential tool for autonomous robots, as it enables the description and execution of motion as high-level goals rather than lower-level primitives [6]. It is a fundamental task for motion planning to provide a collision-free path guiding a robot from an initial node to a goal region in an environment full of obstacles [7]. For example, spray-painting robots based on vision guidance have to follow reasonable paths given by motion planning for spraying [8]. Sometimes robots face more difficult situations, such as performing tasks in dynamic environments, which involves dynamic motion planning problems. For solving these problems, various methods have been developed, of which graph-based and sampling-based methods are the most effective and popular.

For low-dimensional motion planning, such as path planning of smart vehicles, graph-based methods (e.g., A* [9] and Dijkstra [10] methods) always rapidly provide high-quality feasible paths. The feasible paths refer to collision-free paths and the high-quality paths are the low-cost paths. However, motion planning is a PSPACE-hard problem [11], with complexity growing with the number of a robot’s degrees of freedom (DoF). The graph-based methods are no longer applicable due to the excessive computational resources they require in 6 DoF, 7 DoF, and even higher DoF cases.

Sampling-based motion planning methods, which randomly sample valid robot configurations and form graphs of valid motions to find collision-free paths, can avoid the explicit construction of planning scenarios by random sampling and collision detection. Because of high-planning efficiency, they are suitable for solving both low- and high-dimensional motion planning problems [12,13]. In a dynamic environment, dynamic obstacles sometimes make part of the global path invalid, so the invalid partial path needs to be local re-planned online [14]. Sampling-based methods with anytime technology, such as informed anytime fast marching tree (IAFMT*) [15] and informed rapidly exploring random trees (informed RRT*) [16], find multiple feasible paths in the allowed time. The * indicates that the methods are asymptotically optimal (AO) methods. However, these methods only solve static motion planning problems because the algorithms will not return solutions until all of the allowed time runs out. Methods based on probabilistic roadmaps, such as probabilistic roadmaps (PRM) [17] and its variants [18,19], usually build probabilistic roadmaps offline and plan feasible paths online. If environments change, the probabilistic roadmaps have to be rebuilt offline, so these methods are unsuitable for solving dynamic motion planning problems. Offline motion planning refers to robot paths or trajectory planning when movement ceases. Methods based on random trees without anytime technology, such as rapidly exploring random trees (RRT) [20] and its variants [21,22], can be directly run online and can be used for motion planning in dynamic environments. However, there is strong randomness for the paths planned by these methods, so the path quality is not stable. Compared with these methods, some asymptotically optimal methods, such as fast marching trees (FMT*) [23] and variants [24,25], can give high-quality paths in online local re-planning; however, their planning efficiency needs to be further improved. A summary of sampling-based methods is presented in

Table 1. A summary of sampling-based methods.

Planning Space	Method	Advantages	Disadvantages
Statics	Sampling-based methods with anytime technology	They find multiple feasible paths.	They do not return solutions until all the allowed time runs out
	PRM-based methods	They plan online efficiently.	They rebuild roadmaps offline if the environment changes.
	RRT-based methods	They are very flexible and can directly run online.	Planning path quality is not stable.
Dynamics	RRT-based methods	They are very flexible and can directly run online.	Planning path quality is not stable.
	AO methods without anytime technology	They plan high-quality paths online.	Planning efficiency needs to be improved.

.

Most sampling-based methods construct probabilistic roadmaps or random trees by sampling many random nodes in configuration-spaces (C-spaces) to find feasible paths. In order to obtain high-quality paths, even optimal paths, the probabilistic roadmaps or random trees have to be further expanded by adding random sampling nodes. For global motion planning that allows offline planning, these sampling-based methods perform well. However, for local motion re-planning that has to perform efficient online planning, it is difficult for the methods based on random node sampling to balance planning efficiency and quality. This paper presents a new sampling-based method for local motion re-planning, namely, dynamic Gaussian local planner (DGLP), to fast search for high-quality smooth local paths in dynamic environments. Different from the traditional methods based on random node sampling, the DGLP utilizes our Gaussian random path sampling (GRPS) method to define a probability distribution over random paths and samples finite random paths represented by sparse nodes to improve the planning efficiency of high-quality paths. The high-quality collision-free local paths, replacing invalid partial paths, can be selected quickly using the lazy collision detection technique from the sampling paths. We also provide the path end orientation constraint (PEOC) method for smoothly merging the local re-planning paths into global paths. The paper evaluates the performance of the DGLP in simulations and experiments and the results show that the proposed method, DGLP, can perform re-planning local high-quality paths efficiently.

Note that the proposed method does not consider task constraints (e.g., an end effector maintains contact with a plane), and the DGLP samples only in a C-space, not in an implicit space which is a subset of a C-space and consists of all configurations satisfying the constraint.

Our contribution can be summarized as follows.

• This paper presents a sampling-based local re-planning method, namely, DGLP, which can fast converge to high-quality solutions in dynamic environments.
• We provide the GRPS and PEOC methods to generate candidates for high-quality local paths and smoothly merge local paths into global paths, respectively.
• The paper evaluates the performance of DGLP via simulations and experiments.

The paper is organized as follows. Section 2 proposes the DGLP, including the GRPS and PEOC methods. The results and discussion of the experiments are given in Section 3. Section 4 concludes the paper and presents prospects for future work.

2. Dynamic Gaussian Local Planner

The DGLP is described in the pseudo code from Algorithm 1.

Algorithm 1 DGLP
Require: Query ($x_{\mathrm{init}}$, $x_{\mathrm{goal}}$), ($v_{\mathrm{init}}$, $v_{\mathrm{goal}}$), Path sample count $n_p$,                                 Node count $n_s$, Amplitude factor $v_a$
1	$\left\{\mathcal{P}=\varnothing \right\}\leftarrow \mathrm{Initialize}\left(n_p,n_s\right)$;
2	$\mathcal{P}\leftarrow \mathrm{SampleConstraintPaths}\left(x_{\mathrm{init}}, x_{\mathrm{goal}},{\mathit{v}}_{\mathrm{init}}, {\mathit{v}}_{\mathrm{goal}},n_p,n_s,v_a\right);$
3	$\mathcal{J}\leftarrow \mathrm{Cos}\mathrm{t}\left(\mathcal{P}\right);$
4	${\delta }^{†}\leftarrow \mathrm{SelectPath}\left(\mathcal{P},\mathcal{J},n_p\right);$
5	If ${\delta }^{†}=\varnothing$ then
6	return failed
7	return ${\delta }^{†}$

2.1. Overview

Some notions have to be introduced in Algorithm 1. The $x_{\mathrm{init}}$ and $x_{\mathrm{goal}}$ are the initial node and the goal node of a local re-planning path $\delta$. The $v_{\mathrm{init}}$ and $v_{\mathrm{goal}}$ are the velocities at the $x_{\mathrm{init}}$ and $x_{\mathrm{goal}}$. The $n_p$ is the number of random sampling paths in local motion re-planning. Every sampling path is represented by $n_s$ discrete nodes, including $x_{\mathrm{init}}$ and $x_{\mathrm{goal}}$. The fluctuation amplitude of the random paths is controlled by the $v_a$. The $\mathcal{P}$ is the set of the random paths, including ${\delta }_i, i=1,2,\dots ,n_p$. The $\mathcal{J}$ be the cost set of the local paths. Generally, the path cost is the path length. In addition, the ${\delta }^{†}$ is the high-quality local re-planning path with orientation constraints at the end, which can be smoothly merged into the global path.

The DGLP is presented in Algorithm 1 and Figure 1. As a dynamic obstacle invalidates a part of the global path, as shown in Figure 1a, the function $\mathrm{Initialize}\left(n_p,n_s\right)$ initializes the set $\mathcal{P}$. The function $\mathrm{SampleConstraintPaths}\left(x_{\mathrm{init}}, x_{\mathrm{goal}},{\mathit{v}}_{\mathrm{init}}, {\mathit{v}}_{\mathrm{goal}},n_p,n_s,v_a\right)$ samples $n_p$ random paths with the end orientation constraints at both the $x_{\mathrm{init}}$ and $x_{\mathrm{goal}}$ by the GRPS and EPOC methods, which returns the set $\mathcal{P}$, as shown in Figure 1b. The function $\mathrm{Cos}\mathrm{t}\left(\mathcal{P}\right)$ calculates all path costs in the $\mathcal{P}$ and enters them into the $\mathcal{J}$. The function $\mathrm{SelectPath}\left(\mathcal{P},\mathcal{J},n_p\right)$ selects the high-quality collision-free local path ${\delta }^{†}$. from the $\mathcal{P}$ via lazy collision detection [26], as shown in Figure 1c. Note that the outputs of the DGLP are robot geometric paths that do not involve time information, velocity constraints, and acceleration constraints. The geometric paths must be post-processed to add the time information and the constraints. We use the Time Parameterization module of MoveIt! to automatically add the time stamps, velocity constraints, and acceleration constraints for the geometric paths to ensure robot safety.

2.2. Gaussian Random Path Sampling (GRPS)

The function $\mathrm{SampleConstraintPaths}\left(·\right)$ in Algorithm 1 is based on the GRPS method. For a d Dof robot, it has d-dimensional C-space. The C-space and robot workspace can be transformed into each other. The paths for the robot in the C-space can be planned by sampling-based methods. In d-dimensional C-space, let $\delta$ be a local path represented by $n_s$ discrete nodes $x_i, i=1,2,\dots ,n_s$, i.e., $\mathit{\delta }=\left\{x_1,x_2,\dots x_n{}_s\right\}$. Each d-dimensional node $x_i$ consists of d 1-dimensional node component $x_i^j, j=1,2,\dots ,d$, i.e., $x_i={\left(x_i^1,x_i^2,\dots ,x_i^d\right)}^{\mathrm{T}}$. The local path component of the j’th dimension is ${\delta }^j=\left\{x_1^j,x_2^j,\dots x_{n_s}^j\right\}$. The intuitive relationship is illustrated in Figure 2. In this paper, the non-bold symbols represent variables$\in$ ${ℝ}^{1\times 1}$, ${ℝ}^{1\times \mathrm{n}}$, or ${ℝ}^{1\times \mathrm{n}}$, and the bold italicized symbols are variables$\in$ ${ℝ}^{\mathrm{n}\times \mathrm{n}}$ or ${ℝ}^{\mathrm{n}\times \mathrm{m}}$.

The DGLP employs the Gaussian process model to randomly generate the local path component ${\delta }^j$. The Gaussian process model is determined by a mean function $\mu \left(s\right)$ and a kernel function $k\left(s,s^{\prime }\right)$, where $s\in \mathrm{S}$ is path node index. In order to ensure the smoothness of random paths, we set the mean function $\mathit{\mu }\left(s\right)$ to 0 and select the Squared Exponential (SE) kernel function as the kernel function $k\left(s,s^{\prime }\right)$. The SE kernel function is

$$k\left(s,s^{\prime }\right)={\sigma }_f^2\exp \left[-{\left(s-s^{\prime }\right)}^2/2{\sigma }_l^2\right].$$

(1)

The ${\sigma }_f$ can affect the fluctuation amplitude of random paths. The ${\sigma }_f$ is determined by the sampling range $R$ and the amplitude factor $v_a$, i.e., ${\sigma }_f=v_a·R$ in the DGLP. The ${\sigma }_l$ controls the complexity of random paths.

Let a random path $\mathit{\delta }=\left\{x\left(s\right),s\in S\right\}$ be a Gaussian process, where $x\left(s\right)$ denotes the path node corresponding to the index $s$. The random path is described as

$$\mathit{\delta }~\mathcal{G}\mathcal{P}\left(\mathit{\mu }\left(s\right),k\left(s,s^{\prime }\right)\right).$$

(2)

The path probability distribution based on the Gaussian process regression model is

$$\left\{\begin{array}{c}p\left({\mathit{\delta }}_p|{\mathit{S}}_p,{\mathit{S}}_t,{\mathit{\delta }}_t\right)=N\left({\mathit{\mu }}_p,{\mathsf{\Sigma }}_p+{\sigma }_n^{2}I\right)\\ {\mathit{\mu }}_p={\mathit{k}}_{pt}{\left({\mathit{K}}_t+{\sigma }_n^{2}I\right)}^{-1}{\mathit{\delta }}_t\\ {\mathsf{\Sigma }}_p=k_p-{\mathit{k}}_{pt}{\left({\mathit{K}}_t+{\sigma }_n^{2}I\right)}^{-1}{\mathit{k}}_{pt}^{\mathrm{T}}\end{array}\right.$$

(3)

${\mathit{S}}_t=\left\{s_1,s_N\right\}$ and ${\mathit{\delta }}_t=\left\{x_{\mathrm{init}},x_{\mathrm{goal}}\right\}$ are a node index set and a given path node set. ${\mathit{S}}_p=\left\{s_i\right|i=1,2,\dots ,N\}$ and ${\mathit{\delta }}_p=\left\{x_i\right|i=1,2,\dots ,N\}$ are a node index set and path node set of a random path. Note that the $N$ is equivalent to the $n_p$ in Algorithm 1. The noise variance is represented by ${\sigma }_n^2$, and ${\sigma }_{\mathrm{n}}=0$ in this paper. ${\mathit{k}}_{pt}=k\left({\mathit{S}}_p,{\mathit{S}}_t\right)\in {ℝ}^{\mathrm{N}\times 2}$ is a kernel matrix of ${\mathit{S}}_p$ and ${\mathit{S}}_t$. $k_p=k\left({\mathit{S}}_p,{\mathit{S}}_p\right)\in {ℝ}^{\mathrm{N}\times \mathrm{N}}$ is a kernel matrix of ${\mathit{S}}_p$. ${\mathit{K}}_t=k\left({\mathit{S}}_t,{\mathit{S}}_t\right)\in {ℝ}^{2\times 2}$ is a kernel matrix of ${\mathit{S}}_t$. ${\mathit{k}}_{pt}$, $k_p$, and ${\mathit{K}}_t$ can be calculated by the SE kernel function.

Based on the singular value decomposition (SVD) of ${\mathsf{\Sigma }}_p+{\sigma }_n^{2}I$ in (3), a Gaussian process can generate a random sampling path ${\mathit{\delta }}_p=\left\{x_i\right|i=1,2,\dots ,N\}$, i.e., the local path component ${\delta }^j$, from

$${\mathit{\delta }}_p=U\sqrt{S}ℊ+{\mathit{\mu }}_p,$$

(4)

where the elements of the $ℊ=\left[g_1,g_2,\dots ,g_N\right]$ are Gaussian random variables, the $S$is the singular value matrix of ${\mathsf{\Sigma }}_p+{\sigma }_n^{2}I$, and the $U$ is the left singular value matrix of ${\mathsf{\Sigma }}_p+{\sigma }_n^{2}I$.

2.3. Path End Orientation Constraint (EPOC)

The function $\mathrm{SampleConstraintPaths}\left(·\right)$ in Algorithm 1 is also based on the EPOC method. As shown in Figure 3a, because of the randomness of the local re-planning paths sampled from the GRPS method, the end orientation of the local paths is also random. We can use the EPOC method to control the end orientation of the local paths, as illustrated in Figure 3b.

The EPOC method is shown in Figure 4. In order to obtain the local path satisfying the constraint, we add an orientation constraint node $x_{\mathrm{c}1}$ at the distance $\epsilon$ from the initial node $x_{\mathrm{init}}$ on the line of the ${\mathit{v}}_{\mathrm{init}}$. In addition, we add another orientation constraint node $x_{\mathrm{c}2}$ in the same way. As $\epsilon$ goes to zero, based on the four nodes $x_{\mathrm{c}1}$, $x_{\mathrm{c}2}$, $x_{\mathrm{init}}$, and $x_{\mathrm{goal}}$, the GRPS can sample local paths with the path end orientation constraint.

3. Experiments

3.1. Simulations

In order to evaluate the performance of the DGPL, we provide a 2-dimensional dynamic local path re-planning simulation. The simulation is run on the Matlab 2016b (Northeastern University, Shenyang, China) and a computer with a 3.7 GHz Intel Core i7-8700K CPU and 16 GB RAM. The DGPL parameters $n_p$, $n_s$, and $v_a$ are 100, 10, and 0.9, respectively. As shown in Figure 5, the dynamic obstacle moves from left to right in the environment, and as the dynamic obstacle touches the given global path, the partial invalid paths are replaced by the local re-planning paths provided online by the DGPL method.

As shown in Figure 5b–e, the DGPL can re-plan the local paths online for the partial invalid paths as the environment changes and the average planning time is 0.002 s. Because the SE kernel function is employed in the Gaussian process model, the local re-planning paths are smooth naturally. The local re-planning paths are also merged into the global path smoothly since the EPOC method constrains the orientation of both ends of the local path. As shown in Figure 5d,e, the 100 local re-planning paths from the DGPL are all smooth and their ends are added with orientation constraints, so as to merge into the global path smoothly. In addition, the length of the local re-planning paths from the DGPL is reasonable, which indicates that the DGPL performs well in local path planning quality.

As shown in Figure 5d, if the obstacle is too close to the initial and goal nodes of local re-planning, most of the random path candidates will collide with it, which leads to reduction of local re-planning efficiency and success rate. To avoid the case and guarantee the planning effect, the initial and goal nodes of local re-planning should be located far away from the obstacle.

3.2. Robot Experiments

We provide the robot simulations and experiments on a 6-DOF manipulator to evaluate the performance of the DGLP by comparing it with the sampling-based algorithms, i.e., RRT and FMT*. All algorithms are run on the Open Motion Planning Library (OMPL) [27], the Robot Operating System (ROS), the MoveIt, and a computer with a 2.3-GHz Intel Core i7-3610QE CPU and 8 GB RAM. The position precision and orientation precision are set to 0.001 m and 0.001 rad, respectively. The default OMPL settings are employed for RRT and FMT* in the OMPL. The DGLP parameters $n_p$, $n_s$, and $v_a$ are 1000, 10, and 0.9, respectively. All algorithms run 10 times for the local motion re-planning of the manipulator in the robot simulations and experiments. At the beginning of movement of the manipulator, the obstacle is placed about 10 cm in front of the elbow joint of the initial position of the manipulator as soon as possible.

We provide the same global path for all algorithms respectively in the simulations and experiments. After the manipulator starts to move, we add an obstacle in the environment to invalidate part of the global path. Every algorithm has to re-plan a local path as soon as possible to guide the manipulator clear of the obstacle in the dynamic environment. The robot simulation and experiment are shown in Figure 6 and Figure 7, respectively.

Table 2. The experimental results for dynamic local path re-planning.

Experiments	Method	Avg. Time (s)	Avg. Path Cost (m)	Success Rate
Robot simulation	RRT	1.04 (1.00)	1.0641 (1.00)	9/10 (−1.00)
	FMT*	0.43 (−0.21)	0.6969 (−1.00)	10/10 (1.00)
	DGLP	0.03 (−1.00)	0.7449 (−0.74)	10/10 (1.00)
Robot experiment	RRT	0.90 (1.00)	0.9778 (1.00)	7/10 (1.00)
	FMT*	0.52 (0.116)	0.8008 (−1.00)	9/10 (−1.00)
	DGLP	0.04 (−1.00)	0.8396 (−0.56)	8/10 (0.00)

Each column of data is normalized and displayed in brackets, where the range of the normalized values is [−1, 1].

shows the results of the robot simulations and experiments, where Avg. time is the average time of local re-planning and Avg. path cost is the average path length of the manipulator end effector. In terms of local re-planning efficiency, the DGLP is more outstanding than RRT and FMT*. The average local planning time of the DGLP in the simulation is 0.03 s, which is 97.12% and 93.02% faster than RRT and FMT*, respectively. In the experiment, the average time of the DGLP is 95.56% and 92.31% faster than RRT and FMT*, respectively. By directly sampling local re-planning paths with sparse nodes via the GRPS method, instead of building large-scale random trees, such as RRT and FMT*, the DGLP can rapidly provide feasible local paths, in addition, the lazy collision detection technique further improves the efficiency of local path re-planning. For local re-planning quality, both in the simulation and experiment, the average path costs of FMT* are minimal since FMT* is an asymptotically optimal method that is good at finding high-quality paths, even the optimal paths. The average path costs of the DGLP in the simulation and experiment are 6.89% and 4.85% higher than FMT*, which means that the DGLP has the ability to plan high-quality local paths. The DGLP based on the random sampling paths is able to conveniently sort sampling path candidates by path cost, so as to quickly select the feasible local path with the highest quality from the candidates. Compared with AO methods based on random trees, such as FMT*, although the DGLP cannot give the optimal path, it can output high-quality collision-free paths more quickly, which is especially important in dynamic environments, since there is not enough time and computational resources to plan the optimal path. Regarding the success rate of local re-planning, the DGLP performs very well. Its overall success rate in the simulation and experiment is 90%.

In dynamic environments, it is of the greatest importance for a motion planning method to provide a collide-free path as soon as possible. Then, it should ensure the local path quality and planning success rate. We use the weighted stacking method to comprehensively evaluate the performance of each method. The weighted stacking equation is

$$Y=w_1·z_1-w_2·z_2-w_3·z_3,$$

(5)

where $Y$ is the comprehensive evaluation score, the $z_1, z_2,$and $z_3$ are the normalized values of the planning success rate, computing time, and path cost, respectively, and $w_1$, $w_2$, and $w_3$ are the weighting factors. We set $w_1=0.5$ due to the computing efficiency priority principle, and set $w_2=w_3=0.25$. According to Table 2 and Equation (5), we calculate the comprehensive evaluation scores of the methods, as shown in Figure 8. In all groups of comprehensive evaluation scores, the DGLP scores the highest and its scores are at least 44.92% higher than RRT and FMT*, which demonstrates that the DGLP can well balance the efficiency and quality of the local path re-planning.

The ability of the DGLP to provide high-quality local re-planning paths smoothly and efficiently in dynamic environments has been tested by the simulations and experiments. The proposed method can be applied to motion planning for indoor service robots and smart vehicles which are often faced with unexpected situations in dynamic environments, and need to temporarily alter the original planning paths. The DGLP is able to fast plan collision-free local paths with high-quality for the robots in unexpected situations. The proposed method should be used with an offline planning method, i.e., the offline planning method gives a global path, and if part of the global path is invalid, the DGLP outputs the local re-planning paths online to replace the invalid partial path.

4. Conclusions and Future Work

In this paper, we propose a sampling-based method, the DGLP method, which can solve motion planning problems in dynamic environments. We also provide the GRPS and PEOC methods to sample smooth random paths and merge them smoothly into global paths for local motion re-planning. The random sampling paths based on Gaussian process can be represented by sparse nodes and the DGLP employs the lazy collision detection technique, so as to improve the local path re-planning efficiency. The results of the simulations and experiments show that the DGLP method can search for high-quality smooth local paths efficiently to replace the local invalid paths, and merge the local paths into the global paths smoothly.

In future work, we will extend DGLP by introducing the GPU-based parallel computing technique to further improve its real-time motion planning capacity.