Complexity

Volume 2018, Article ID 4358747, 14 pages

https://doi.org/10.1155/2018/4358747

## Robot Motion Planning Method Based on Incremental High-Dimensional Mixture Probabilistic Model

^{1}State key Laboratory of Robotics and Systems, Harbin Institute of Technology, Harbin, China^{2}Shenzhen Academy of Aerospace Technology, Shenzhen, China^{3}Istituto Italiano di Tecnologia, Via Morego 30, Genova, Italy

Correspondence should be addressed to Xin Wang; moc.liamg@s70tihgnawnix and Fei Chen; ti.tii@nehc.ief

Received 11 June 2018; Revised 18 August 2018; Accepted 19 September 2018; Published 1 November 2018

Guest Editor: Andy Annamalai

Copyright © 2018 Fusheng Zha et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

#### Abstract

The sampling-based motion planner is the mainstream method to solve the motion planning problem in high-dimensional space. In the process of exploring robot configuration space, this type of algorithm needs to perform collision query on a large number of samples, which greatly limits their planning efficiency. Therefore, this paper uses machine learning methods to establish a probabilistic model of the obstacle region in configuration space by learning a large number of labeled samples. Based on this, the high-dimensional samples’ rapid collision query is realized. The influence of number of Gaussian components on the fitting accuracy is analyzed in detail, and a self-adaptive model training method based on Greedy expectation-maximization (EM) algorithm is proposed. At the same time, this method has the capability of online updating and can eliminate model fitting errors due to environmental changes. Finally, the model is combined with a variety of sampling-based motion planners and is validated in multiple sets of simulations and real world experiments. The results show that, compared with traditional methods, the proposed method has significantly improved the planning efficiency.

#### 1. Introduction

In recent years, as robots play an increasingly important role in industrial production and daily life, the issue of motion planning has received extensive attention. Although modern robots have significant differences in configuration, size, perception, driving methods, and application scenes, autonomous navigation and planning in complex environments are common problems faced by almost all robots [1].

The motion planning problem refers to, given the related description of robot and environment, initial state and goal region, seeking a series of control inputs to drive the robot to complete the movement from the initial state to the goal region while satisfying the environmental constraints (without colliding with the obstacles). However, for some robots with high planning dimensions, their configuration space’s obstacle region cannot be explicitly described. In response to this problem, sampling-based motion planning algorithms have developed rapidly and received widespread attention [2–4]. This type of methods do not explicitly describe obstacles. Instead, they rely on the collision query module to provide feasibility information of the candidate trajectories and connect a series of collision-free samples to generate a feasible path from the initial state to the goal region. The collision query module is generally implemented by the robot kinematics calculation and the space bounding box principle [5], which requires a large computational overhead to make the module a major bottleneck for limiting the efficiency of the sampling-based motion planning algorithms [6]. Sampling-based motion planning algorithms can generate a large number of labeled samples with spatial collision information in planning instances, which provides a necessary condition for the implementation of machine learning methods. If the robots can learn from past planning experience to guide the future planning tasks, more efficient motion planning can be achieved. Therefore, how to use machine learning methods to break the efficiency bottleneck of motion planning algorithms has become a research focus in this field in recent years.

A large amount of research work is devoted to performing adaptive sampling and guiding the planning algorithm to explore certain areas in the configuration space with machine learning methods. Dalibard et al. [7] use the principal component analysis (PCA) method for online analysis of samples to estimate the direction and position of the local narrow passage in collision-free space and increase the sampling density in the direction of the passage axis. Similar methods include bridge test [8] and retraction strategy [9, 10]. These methods can significantly improve the efficiency of the planning algorithm at the local narrow passage. Brock and Burns [11] propose an exploration strategy based on entropy guidance, which can guide the planning algorithm to sample the areas that maximize information gain. However, because of the high computational cost of this method, it is more suitable for the off-line path graph construction of multiqueue query algorithms like PRM. Arslan and Tsiotras [12] use the kernel function to learn the feasibility and heuristics of samples generated in the previous planning instances and increase the sampling density of the areas that may make current path cost lower. This method can improve the convergence rate of asymptotically optimum motion planning algorithms [13]. Bialkowski et al. [14] propose to store empirically observed estimates of collision-free space in a point proximity data structure and then use kd-tree algorithm to generate future valid samples.

In addition, by learning the past experience of motion planning, the prediction of the new samples’ feasibility can be achieved. Burns et al. [15] present a model-based motion planning method. This method utilizes the local weighted regression to incrementally learn the approximate model of the configuration space, and the classification of new samples is implemented. Compared with the traditional collision query module, the method has a smaller computational complexity. Yang et al. [16] propose a neural networks framework to achieve robot automatic collision avoidance. By exploiting the joint space redundancy, the human operator would be able to only concentrate on motion of robots end effector without concern over possible collision. Pan et al. [17] report that the collision query results produced in previous motion planning tasks are stored in a data set. When the new sample is generated, the KNN algorithm is used to search the nearest neighbors of the sample in the data set, and the probability of its feasibility is estimated according to the neighbors’ collision information. Yang et al. [18] combine the flexibility of the Gauss process (GP) with the efficiency of the RRT algorithm and establish a motion model to predict the movement of obstacles, so that the robot can find a safe path in the dynamic environment constraints. Huh et al. [19] propose that the Gaussian mixture model can be used to fit the probability distribution of the high-dimensional space obstacle region, so as to quickly predict the feasibility of the new samples. However, the influence of the number of Gaussian components on the model prediction accuracy has not been analyzed and considered. Besides, some other learning methods like conditional variational autoencoder (CVAE) [20], neural learning [21], experience graphs (E-Graphs) [22], and dynamic movement primitives (DMPs) [23] are also used in robot motion planning problem.

The above methods improve the efficiency of motion planning to some extent, but there are the following problems:

(1) Some lazy learning methods such as kd-tree and KNN require data sets with large sample size. In the motion planning problem of high-dimensional space, this will lead to great spatial complexity which is hard to realize.

(2) Models like PCA, CVAE, E-Graphs, and local weighted regression have poor flexibility. Even if the environment or the base of robot changes slightly, the model needs to be retrained, which greatly increases the computational overhead.

Therefore, in this article, we propose to use Gaussian mixture model (GMM) as a prior model of robot configuration space to improve the efficiency of robot motion planning. GMM can represent the unknown model by the linear combination of Gaussian probability density functions. It has the ability to fit the continuous probability density distribution in any dimensional space with small spatial complexity. On the other hand, the Greedy EM algorithm utilized in this paper can realize incremental training of GMM, so that the model can be updated according to the environment changes without retraining.

In general, the main contributions of this paper include the following: (1) The influence of number of components on the prediction accuracy is analyzed, and a method for adaptively training GMM of collision region in robot high-dimensional configuration space based on the convergence of log-likelihood function is proposed. (2) The Greedy EM algorithm is used to update the GMM online with real-time 3D map to adapt to environmental changes. (3) The above method is applied to a variety of sampling-based motion planning algorithms, and the planning efficiency is significantly improved.

The paper is organized as follows: Section 2 introduces the main motivation of the research work; Section 3 introduces the incremental training process of GMM using Greedy EM clustering algorithm, and the integration with motion planning algorithms; Section 4 shows the simulations and the real world experiments’ results; Section 5 summarizes the results and provides directions for future work.

#### 2. Motivations and Problem Statement

The Gaussian mixture model [24] is a mixture probabilistic model that can fit the probability density distribution of arbitrary dimensions and arbitrary shapes by weighting the probability density functions of multiple Gaussian distributions. where is the component weights and satisfied . is the probability density function of the th Gaussian distribution, and is the parameter vector of the distribution: where is the dimension. and are the mean and covariance matrix of the Gaussian component, respectively.

By using the sample set with sample size , the parameters of the Gaussian mixture model can be estimated by the basic EM algorithm. The basic EM algorithm process is as follows:

Set the number of components and the parameters’ initial value properly, and start iterating.

*Step E*. According to the current model parameters, calculate the expected probability of each Gaussian component to each observation data .

*Step M*. The weight of each Gaussian component , the mean , and the covariance matrix are adjusted by maximum likelihood estimate method.

The E step and the M step are repeated to maximize the log-likelihood function and improve the fitting ability of GMM to the sample set.

For simplicity, first, the above basic EM clustering algorithm is used to train the GMMs with different number of components to fit the obstacle area in two-dimensional map (Figure 1(a)). This process will naturally extend later to multiple dimensions. The trained GMM is used to calculate the collision probability of the new samples, so as to guide the [1] (a RRT-like motion planning algorithm with asymptotically optimal) search for the feasible path from the starting point to the end point.