robot motion planning method based on incremental high...

Research ArticleRobot Motion Planning Method Based on IncrementalHigh-Dimensional Mixture Probabilistic Model

Fusheng Zha1 Yizhou Liu 1 Xin Wang 2 Fei Chen 3 Jingxuan Li1 and Wei Guo1

1State key Laboratory of Robotics and Systems Harbin Institute of Technology Harbin China2Shenzhen Academy of Aerospace Technology Shenzhen China3Istituto Italiano di Tecnologia Via Morego 30 Genova Italy

Correspondence should be addressed to XinWang xinwanghit07sgmailcom and Fei Chen feicheniitit

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

Guest Editor Andy Annamalai

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

The sampling-based motion planner is the mainstream method to solve the motion planning problem in high-dimensional spaceIn the process of exploring robot configuration space this type of algorithm needs to perform collision query on a large numberof samples which greatly limits their planning efficiency Therefore this paper uses machine learning methods to establish aprobabilistic model of the obstacle region in configuration space by learning a large number of labeled samples Based on thisthe high-dimensional samplesrsquo rapid collision query is realized The influence of number of Gaussian components on the fittingaccuracy 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 errorsdue to environmental changes Finally the model is combined with a variety of sampling-based motion planners and is validated inmultiple sets of simulations and real world experiments The results show that compared with traditional methods the proposedmethod has significantly improved the planning efficiency

1 Introduction

In recent years as robots play an increasingly importantrole in industrial production and daily life the issue ofmotion planning has received extensive attention Althoughmodern robots have significant differences in configurationsize perception driving methods and application scenesautonomous navigation and planning in complex environ-ments are common problems faced by almost all robots[1]

The motion planning problem refers to given the relateddescription of robot and environment initial state and goalregion seeking a series of control inputs to drive the robotto complete the movement from the initial state to thegoal region while satisfying the environmental constraints(without colliding with the obstacles) However for somerobots with high planning dimensions their configurationspacersquos obstacle region cannot be explicitly described Inresponse to this problem sampling-based motion planningalgorithms have developed rapidly and received widespread

attention [2ndash4] This type of methods do not explicitlydescribe obstacles Instead they rely on the collision querymodule to provide feasibility information of the candidatetrajectories and connect a series of collision-free samplesto generate a feasible path from the initial state to the goalregion The collision query module is generally implementedby the robot kinematics calculation and the space boundingbox principle [5] which requires a large computationaloverhead to make the module a major bottleneck for lim-iting the efficiency of the sampling-based motion planningalgorithms [6] Sampling-based motion planning algorithmscan generate a large number of labeled samples with spatialcollision information in planning instances which providesa necessary condition for the implementation of machinelearning methods If the robots can learn from past planningexperience to guide the future planning tasks more efficientmotion planning can be achieved Therefore how to usemachine learning methods to break the efficiency bottleneckof motion planning algorithms has become a research focusin this field in recent years

HindawiComplexityVolume 2018 Article ID 4358747 14 pageshttpsdoiorg10115520184358747

2 Complexity

A large amount of research work is devoted to performingadaptive sampling and guiding the planning algorithm toexplore certain areas in the configuration space with machinelearning methods Dalibard et al [7] use the principalcomponent analysis (PCA) method for online analysis ofsamples to estimate the direction and position of the localnarrow passage in collision-free space and increase thesampling density in the direction of the passage axis Similarmethods include bridge test [8] and retraction strategy [9 10]These methods can significantly improve the efficiency ofthe planning algorithm at the local narrow passage Brockand Burns [11] propose an exploration strategy based onentropy guidance which can guide the planning algorithm tosample the areas that maximize information gain Howeverbecause of the high computational cost of this method itis more suitable for the off-line path graph construction ofmultiqueue query algorithms like PRM Arslan and Tsiotras[12] use the kernel function to learn the feasibility andheuristics of samples generated in the previous planninginstances and increase the sampling density of the areasthat may make current path cost lower This method canimprove the convergence rate of asymptotically optimummotion planning algorithms RRT [13] Bialkowski et al [14]propose to store empirically observed estimates of collision-free space in a point proximity data structure and then usekd-tree algorithm to generate future valid samples

In addition by learning the past experience of motionplanning the prediction of the new samplesrsquo feasibility canbe achieved Burns et al [15] present a model-based motionplanning method This method utilizes the local weightedregression to incrementally learn the approximate model ofthe configuration space and the classification of new samplesis implemented Compared with the traditional collisionquery module the method has a smaller computationalcomplexity Yang et al [16] propose a neural networksframework to achieve robot automatic collision avoidance Byexploiting the joint space redundancy the human operatorwould be able to only concentrate on motion of robots endeffector without concern over possible collision Pan et al [17]report that the collision query results produced in previousmotion planning tasks are stored in a data set When the newsample is generated the KNN algorithm is used to searchthe 119896 nearest neighbors of the sample in the data set andthe probability of its feasibility is estimated according to theneighborsrsquo collision information Yang et al [18] combine theflexibility of the Gauss process (GP) with the efficiency ofthe RRT algorithm and establish a motion model to predictthe movement of obstacles so that the robot can find a safepath in the dynamic environment constraints Huh et al[19] propose that the Gaussian mixture model can be usedto fit the probability distribution of the high-dimensionalspace obstacle region so as to quickly predict the feasibilityof the new samples However the influence of the numberof Gaussian components on the model prediction accuracyhas not been analyzed and considered Besides some otherlearning 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 motionplanning to some extent but there are the following prob-lems

(1) Some lazy learning methods such as kd-tree andKNN require data sets with large sample size In the motionplanning problem of high-dimensional space this will lead togreat spatial complexity which is hard to realize

(2) Models like PCA CVAE E-Graphs and localweighted regression have poor flexibility Even if the environ-ment or the base of robot changes slightly the model needsto be retrained which greatly increases the computationaloverhead

Therefore in this article we propose to use Gaussianmixture model (GMM) as a prior model of robot config-uration space to improve the efficiency of robot motionplanning GMM can represent the unknown model by thelinear combination of Gaussian probability density functionsIt has the ability to fit the continuous probability densitydistribution in any dimensional space with small spatialcomplexity On the other hand the Greedy EM algorithmutilized in this paper can realize incremental training ofGMM so that the model can be updated according to theenvironment changes without retraining

In general the main contributions of this paper includethe following (1) The influence of number of componentson the prediction accuracy is analyzed and a method foradaptively training GMM of collision region in robot high-dimensional configuration space based on the convergenceof log-likelihood function is proposed (2) The Greedy EMalgorithm is used to update the GMM online with real-time 3D map to adapt to environmental changes (3) Theabove method is applied to a variety of sampling-basedmotion planning algorithms and the planning efficiency issignificantly improved

Thepaper is organized as follows Section 2 introduces themain motivation of the research work Section 3 introducesthe incremental training process of GMM using GreedyEM clustering algorithm and the integration with motionplanning algorithms Section 4 shows the simulations andthe real world experimentsrsquo results Section 5 summarizes theresults and provides directions for future work

2 Motivations and Problem Statement

The Gaussian mixture model [24] is a mixture probabilisticmodel that can fit the probability density distribution ofarbitrary dimensions and arbitrary shapes by weighting theprobability density functions of multiple Gaussian distribu-tions

119875 (119909 | 120579) =119870

sum119896=1

120587119896120601 (119909 | 120579119896) (1)

where120587119896 is the component weights and satisfied 1205871+1205872+sdot sdot sdot+120587119870 = 1 120587119896 ge 0 120601(119909 | 120579119896) is the probability density function ofthe 119896th Gaussian distribution and 120579119896 is the parameter vectorof the distribution

Complexity 3

(a) Map (b) 119870 = 7 (c) 119870 = 10

(d) 119870 = 15 (e) 119870 = 25

55

50

45

40

35

30

25

20

15

10

05

Col

lisio

n Pr

obab

ility

times10minus4

(f) 119870 = 40

Figure 1 The Gaussian mixture model with different number of components is used to fit the probability distribution of the obstacle regionsin the two-dimensional map (a) and the GMM-based collision query method is used to guide the RRTlowast algorithm to plan the path from thestarting point to the end point The white lines represent the feasible edges that RRTlowast algorithm explores the green points are the startingpoint and the end point of the planning problem and the red line represents the final path

120601 (119909 | 120579119896) = (2120587)minus1198892 10038161003816100381610038161198781198961003816100381610038161003816minus12

sdot exp [(minus05) (119909 minus 119898119896)T 119878minus1119896 (119909 minus 119898119896)](2)

where 119889 is the dimension 119898119896 and 119878119896 are the mean andcovariance matrix of the Gaussian component respectively

By using the sample set 1199091 1199092 119909119873 with sample size119873 the parameters of the Gaussian mixture model can beestimated by the basic EM algorithmThe basic EM algorithmprocess is as follows

Set the number of components 119870 and the parametersrsquoinitial value 120579(0) properly and start iterating

Step E According to the current model parameters calculatethe expected probability 119875(119896 | 119909119895) of each Gaussiancomponent to each observation data 119909119895

119875 (119896 | 119909119895) =120587119896120601 (119909119895 | 120579119896)

sum119870119896=1 120587119896120601 (119909119895 | 120579119896) 119895 = 1 2 119873 (3)

StepMTheweight of eachGaussian component 120587119896 themean119898119896 and the covariance matrix 119878119896 are adjusted by maximumlikelihood estimate method

120587119896 = 1119873119873

sum119895=1

119875 (119896 | 119909119895)

119898119896 =sum119873119895=1 119875 (119896 | 119909119895) 119909119895sum119873119895=1 119875 (119896 | 119909119895)

119878119896 =sum119873119895=1 119875 (119896 | 119909119895) (119909119895 minus 119898119896) (119909119895 minus 119898119896)

T

sum119873119895=1 119875 (119896 | 119909119895)

(4)

The E step and the M step are repeated to maximize thelog-likelihood functionL119870 and improve the fitting ability ofGMM to the sample set

L119870 =119873

sum119895=1

log119875 (119909119895 | 120579) (5)

For simplicity first the above basic EM clustering algo-rithm is used to train the GMMs with different numberof components to fit the obstacle area in two-dimensionalmap (Figure 1(a)) This process will naturally extend later to

4 Complexity

5 10 15 20 25 30 35 40 450Quantity of components K

00501

01502

02503

03504

Erro

r pro

babi

lity

GMMsFitting curve

(a) Generalization error probability of GMMs-4

times 10


0

02

04

06

08

1

Aver

age c

alcu

latio

n tim

e (s)

GMMsFitting curve

(b) The average calculation time required for GMMs to performprobabilistic calculations

Figure 2 Comparison of the GMMsrsquo performance under differentnumbers of components

multiple dimensions The trained GMM is used to calculatethe collision probability of the new samples so as to guidethe RRTlowast [1] (a RRT-like motion planning algorithm withasymptotically optimal) search for the feasible path from thestarting point to the end point

Figures 1(b)ndash1(f) show the influence of number of Gaus-sian components on the model fitting effect and the motionplanning result When 119870 = 7 the fitting accuracy ofthe GMM is low resulting in a large number of feasibleregions in the two-dimensional map being blocked and theplanned path repeatedly passes through the obstacle regionObviously this kind of GMM cannot meet the needs ofmotion planning problems With the increasing number ofmodel components the fitting accuracy of the GMM to theobstacle region is continuously improved When 119870 = 15the contour of the high collision probability area is basicallyconsistent with the obstacles in the map which indicates thatthe GMM has the ability to perform collision query on newsamples If the number of Gaussian components continuesto increase the GMMrsquos fitting accuracy is not significantlyimproved

Subsequently the generalization error of GMM underdifferent number of components is further calculated and

analyzed As shown in Figure 2(a) with the increasing num-ber ofGaussian components the probability of generalizationerror on new samples is reduced After119870 = 15 if the numberof components continues to increase the performance of theGMM is not significantly improved which is basically inaccordance with the result shown in Figure 1

On the other hand the time to calculate collisionprobability of new samples under different number ofcomponents is calculated As shown in Figure 2(b) theaverage calculation time is linearly positively correlatedwith the number of Gaussian components This indicatesthat the increasing number of components will lead togreater computational overhead when performing proba-bilistic calculations on new samples and limit the effi-ciency of motion planning algorithms Therefore select-ing the appropriate number of components 119870 accordingto the actual environment can maximize the performanceof GMM in the sampling-based motion planning algo-rithm

Therefore using the basic EM algorithm to train theGMM of the collision region has the following problems

(1) The environment in which the robots are located isvery different and requires different number of componentsso it is impossible to artificially specify 119870 to suit the needs ofall environments

(2) When the robotrsquos environment changes the GMMtrained with the basic EM algorithm will fail and need to beretrained which indicates that it does not have the ability toupdate online according to the environment

3 Algorithms

To solve the above problems the Greedy EM algorithmis used to perform incremental training on the GMMand the 119870 value is adaptively selected according to theconvergence of the log-likelihood function The model hasthe online learning ability to adapt to the environmentchanges The GMM is used to perform collision queryroutine in a variety of sampling-based motion planningframeworks The algorithm framework is shown in Fig-ure 3

31e TrainingMethod of the Probabilistic Model As shownin Algorithm 1 119883119888119900119897 is the sample set of collision regionin robot configuration space 119866119888119900119897 is the GMM to describethe distribution of the collision region 119896 is the numberof components of the current GMM am S is an arrayof weights mean values and covariance matrices for allcomponents of the current GMM andL119896 is the value of theoverall log-likelihood function under the current number ofGaussian components 119896

The incremental training process of GMM is divided intotwo parts global EM iteration and binary EM iteration Theglobal EM iteration process is exactly the same as the basicEM algorithm The maximum likelihood estimation is usedto adjust theweights mean values and covariancematrices ofall Gaussian components in the current GMM to improve fit-ting ability to the sample set119883119888119900119897 as shown in (3) (4) and (5)

Complexity 5

Sampling based motion planner

Random expansion

Generate a examplein C-space

Collision Probabilitywith GMM

No YesFall in decision boundaries

Forward kinematic

Collision-checkbased on FCL

Collision-free Pathpoints

Generate a final path

Update the GMM model

Training-setprevious GMM

model

Newsamples

Add new component

Eliminate old component

Binary EM cluster

Global EM cluster

New GMM model

Perception

Newmap

RGBDCamera

Update the point-cloud

Update the octomap

Figure 3 Algorithm framework the main contributions are highlighted in red

Binary EM iteration is the key step in incrementaltraining Add a new component 120601(119909 | 120579) to the GMM 119891119896(119909)with 119896 components

119891119896+1 (119909) = (1 minus 119886119899119890119908) 119891119896 (119909) + 119886119899119890119908120601 (119909 | 120579) 119886119899119890119908 isin (0 1)

(6)

The new componentrsquos weight 119886119899119890119908 is initialized by thefollowing equation

119886119899119890119908 =

05 if 119896 = 12119896 + 1 if 119896 ge 2 (7)

The mean 119898119899119890119908 is set to a randomly selected sample andthe covariance matrix 119878119899119890119908 is initialized to 1205902119868 According to[24] 120590 depends on the dimension of the planning problem 119889sample size119873 and a fixed number 120573which is set to half of themaximum singular value of samplesrsquo covariance matrix (8)

120590 = 120573 [ 4119889 + 2119873]

1(119889+4)

(8)

To make the new GMM better fit the sample set the newcomponentrsquos weight and the parameters vector 120579 need to be

adjustedTheoptimization goal is tomaximize the binary log-likelihood function

L119896+1 =119873

sum119895=1

log119891119896+1 (119909119895)

=119873

sum119895=1

log [(1 minus 119886119899119890119908) 119891119896 (119909119895) + 119886119899119890119908120601 (119909 | 120579)](9)

According to the maximum likelihood estimation binaryEM iterations are performed on the current GMM and thenew Gaussian component The binary EM iteration processis as follows

Set the initial number of components 119896 to 1 and initializethe parameters of the component according to the abovemethod

Step E Initialize the new Gaussian componentrsquo parameters120579 and calculate its expected probability to each observationsample 119909119895

119875 (119896 + 1 | 119909119895) =119886119899119890119908120601 (119909119895 | 120579)

(1 minus 119886119899119890119908) 119891119896 (119909119895) + 119886119899119890119908120601 (119909119895 | 120579)(10)

6 Complexity

1 function BUILD GMM(119883119888119900119897)2 119866119888119900119897 larr997888INITIAL GMM(119883119888119900119897)3 while L119896L119896minus1 minus 1 gt 1119890minus3 do4 119898119899119890119908 larr997888RandomSelect(119883119888119900119897)5 119878119899119890119908 larr997888 12059021198686 119886119899119890119908 larr997888 2(119896 + 1)7 (119886119899119890119908 119898119899119890119908 119878119899119890119908) larr997888

BINARY EM(119866119888119900119897 119886119899119890119908 119898119899119890119908 119878119899119890119908)8 119866119888119900119897 larr997888 AddToGMM(119866119888119900119897 119886119899119890119908 119898119899119890119908 119878n119890119908)9 119866119888119900119897 larr997888GLOBLE EM(119866119888119900119897)10 end while11 return 11986611988811990011989712 end function13 function Initial GMM(119883119888119900119897)14 a[1] larr997888 115 m[1] larr997888 E(119883119888119900119897)16 S[1] larr997888Cov(119883119888119900119897)17 119866119888119900119897 larr997888GLOBLE EM(119866119888119900119897)18 return 11986611988811990011989719 end function20 function AddToGMM (119866119888119900119897 119886119899119890119908 119898119899119890119908 119878119899119890119908)21 for 119895 = 1 997888rarr 119896 do22 119886[119895] larr997888 119886[119895](1 + 119886119899119890119908)23 end for24 a[119896 + 1] larr997888 11988611989911989011990825 m[119896 + 1] larr997888 11989811989911989011990826 S[119896 + 1] larr997888 11987811989911989011990827 return 11986611988811990011989728 end function

Algorithm 1 The Greedy EM Algorithm

StepMMaximize the log-likelihood function (9) by adjustingthe new componentrsquos weight 119886119899119890119908 mean119898119899119890119908 and covariancematrix 119878119899119890119908

119886119899119890119908 = 1119873119873

sum119895=1

119875 (119896 + 1 | 119909119895)

119898119899119890119908 =sum119873119895=1 119875 (119896 + 1 | 119909119895) 119909119895sum119873119895=1 119875 (119896 + 1 | 119909119895)

119878119899119890119908 =sum119873119895=1 119875 (119896 + 1 | 119909119895) (119909119895 minus 119898) (119909119895 minus 119898)

T

sum119873119895=1 119875 (119896 + 1 | 119909119895)

(11)

Then as shown in line 20 to line 28 of Algorithm 1 theweights of all components in the current GMM are dilutedand the new component trained by the binary EM algorithmis added to the GMM Then the new GMMrsquos parameters areadjusted through the global EM iterations until convergence

As shown in line 3 to line 10 of Algorithm 1 the globalEM iterations and binary EM iterations are alternated toimplement the incremental training of GMM When theoverall log-likelihood function value L119896 becomes stable(which means L119896L119896minus1 minus 1 lt 1119890minus3 as shown in line 3 ofAlgorithm 1) the addition of a new Gaussian component isstopped and the training is completed In this way according

to the convergence of the overall log-likelihood functionthe number of Gaussian components 119870 can be adaptivelyadjusted without being specified in advance so as to rea-sonably weigh the accuracy and computational efficiency ofthe GMM Beside compared with the basic EM algorithmthe GMM trained by Greedy EM algorithm has better fittingaccuracy [25]

32 Online Updating of Gaussian Mixture Model The actualworking environment of the robot is dynamic and uncertainThe Gaussian mixture model trained off-line by the abovemethod may cause errors due to the changes of environmentor robot base so the GMM is required to have the abilityto update online Through the Greedy EM algorithm a newGaussian component is added to fit the new collision regiondue to environmental changes and some useless Gaussiancomponents are eliminated

As shown in line 2 to line 4 of Algorithm 2 the octreemapO119900119888119905119903119890119890 is updated according to the point cloud dataP119901119900119894119899119905119888119897119900119906119889acquired by the RGB-D camera The sample set 119883119888119900119897 of thehigh-dimensional collision region is updated according to thenew octree map and robot kinematics model K119903119900119887119900119905 Thenaccording to (1) and (2) the probability that each samplebelongs to the current GMM is calculated If the probabilityis small it indicates that the sample is a new collision samplepoint due to environmental changes In this way new samplesare screened and a sample is randomly selected as the initialmean value of the newly added Gaussian component Finallythe Greedy EM algorithm is used to train the GMM andthe new component is added continuously until the log-likelihood function converges

As shown in line 13 of Algorithm 2 119866119888119900119897 is the GMMto fit the collision region in configuration space in order toprevent the infinite increase of the number of componentsand inefficiency of calculation some useless Gaussian com-ponents in the new GMM need to be eliminated such as theweights being very small or the components with covariancematrix determinant close to 0 as shown in Figure 4 Othercomponents obtain the weight of the removed componentsaccording to their respective weights The sum of weights ofall components is always 1 This process enables the GMM toadapt quickly and accurately to environment changes

33 GMM-Based Motion Planner After the GMM of thehigh-dimensional collision region is established by theGreedy EM algorithm the probability of the new samplesbelonging to the GMM can be calculated by (1) and (2)thereby determining whether the sample is feasible In thisway the forward kinematics calculation and precise collisionquery are avoided and the total collision query time is greatlyshortened

As shown in line 7 to line 13 of Algorithm 3 if theprobability of the new sample P(119902119899119890119908) belonging to thecurrent GMM is less than the upper probabilistic boundaryof the collision-free region 120575119888119900lminus119891119903119890119890 the sample is consid-ered feasible If the probability of the new sample P(119902119899119890119908)belonging to the GMM is greater than the lower probabilisticboundary 120575119888119900119897 of the collision region the sample is considered

Complexity 7

Previous GMMs

Current GMMs

Olds samples

New samples

Add new component

Eliminate useless component

Figure 4 The schematic diagram of GMMrsquos online updating process

1 function UPDATE GMM(119866119888119900119897 119883119888119900119897P119901119900119894119899119905119888119897119900119906119889)2 O119900119888119905119903119890119890119906119901119889119886119905119890(P119901119900119894119899119905119888119897119900119906119889)3 119883119888119900119897119906119901119889119886119905119890(O119900119888119905119903119890119890K119903119900119887119900119905)4 119883119888119900119897 119899119890119908 larr997888 ScreenSamples(119883119888119900119897 119866119888119900119897)5 while L119896L119896minus1 minus 1 gt 1119890minus3 do6 119898119899119890119908 larr997888RandomSelect(119883119888119900119897 119899119890119908)7 119878119899119890119908 larr997888 12059021198688 119886119899119890119908 larr997888 2(119896 + 1)9 (119886119899119890119908 119898119899119890119908 119878119899119890119908) larr997888

BINARY EM(119866119888119900119897 119886119899119890119908 119898119899119890119908 119878119899119890119908)10 119866119888119900119897 larr997888AddToGMM(119866119888119900119897 119886119899119890119908 119898119899119890119908 119878119899119890119908)11 119866119888119900119897 larr997888GLOBLE EM(119866119888119900119897)12 end while13 119866119888119900119897 larr997888Eliminate(119866119888119900119897)14 return 11986611988811990011989715 end function

Algorithm 2 GMMrsquos Online Updating

Require 119883119888119900119897 examplars1 119866119888119900119897 larr997888BUILD GMM(119883119888119900119897)2D119894119899119894119905(119902119894119899119894119905)3 while Distrance(119902119892119900119886119897 119902119899119890119908) lt 119889119898119894119899 do4 119902119903119886119899119889 larr997888RandomSampling()5 119902119899119890119886119903 larr997888NodeSelection(D 119902119903119886119899119889)6 119902119899119890119908 larr997888NodeExpansion(D 119902119903119886119899119889 119902119899119890119886119903)7 if P(119902119899119890119908) lt 120575119888119900119897minus119891119903119890119890 then8 119902119899119890119908 is collision-free9 else if P(119902119899119890119908) gt 120575119888119900119897 then10 119902119899e119908 is collision11 else12 FCLCollisionQuery(119902119899119890119908)13 end if14 end while15 119866119888119900119897 larr997888UPDATE GMM(119866119888119900119897 119883119888119900119897P119901119900119894119899119905119888119897119900119906119889)

Algorithm 3 GMM-based Motion Planner

Figure 5 GMMrsquos training scene

unfeasible If the probability is between the 120575119888119900119897 and 120575119888119900119897minus119891119903119890119890the GMMrsquos prediction result is ambiguous Therefore theprecise collision query on these samples will be implementedby the kinematics and Flexible Collision Library (FCL) [26]120575119888119900119897 and 120575119888119900119897minus119891119903119890119890 can be determined by cross-validation

Cross-validation is a method commonly used in machinelearning to evaluate the prediction effect of a model with asmall sample set 1000 samples in the collision region andcollision-free region are collected respectively to form twocross-validation sets Through multiple tests the probabilityof GMMrsquo generalization error for two cross-validation setsunder different decision boundaries 120575 can be obtainedFor robot motion planning false positive means that thecollision-free samples are classified as collision sampleswhich may cause the motion planner to fail to find a feasiblesolution False negative means that the collision samples areclassified as collision-free samples and may cause the finalpath to pass through collision region

For example in the environment of Figure 5 the GMM istrained by Greedy EM algorithm to fit collision region in 6-DOF robotrsquos configuration spaceThen the above-mentioned

8 Complexity

Cross validation test

03

004

30102030405060708090

100

Erro

r pro

babi

lity

()

times10-3

false negative ratefalse positive rate＝ＩＦ- Ｌ

Tolerance＝ＩＦ

02 04 06 08 1 12 14 16 18 20Decision threshold

Figure 6 Cross-validation results

0 5 10 15 20 25Component quantity K

minus22

minus21

minus2

minus19

minus18

minus17

Log-

likel

ihoo

d

times104

Figure 7 The learning curve of Greedy EM algorithm

cross-validation is performed on the six-dimensional GMMto obtain the generalization error curves as shown in Figure 6In order to guarantee the safety and success rate of motionplanning the tolerable generalization error probability is setto 30 which is the empirical value obtained bymany simu-lation experimentsThe intersections with two generalizationerror curves are 120575119888119900119897 and 120575119888119900119897minus119891119903119890119890

4 Simulations and Experiments

This section provides some simulations and real worldexperimentsrsquo results to support the method proposed in thispaper All experiments are conducted on an Intel Core i5-4590 33GHz personal computer ROS and Gazebo are usedto build a simulation platform FCL is used to acquire thesample set of the collision region in robot configuration spaceand perform accurate collision query when the GMM-basedcollision query results are ambiguous OMPL [27] (Open

Motion Planning Library) provides a variety of sampling-based motion planning algorithms for comparison experi-ments

41 Learning and Online Updating In order to prove theeffectiveness of the algorithm the 6-DOF UR10 robot inFigure 5 is used for the simulation test First accordingto Algorithm 1 a six-dimensional Gaussian mixture modelunder the environment is trained By alternately using theglobal EM iterations and binary EM iterations the numberof Gaussian components 119870 gradually increases When 119870 =25 the log-likelihood function L119896 converges meaning thatthe GMM has completed training and stopped adding newGaussian components as shown in Figure 7

After the model training is completed the incrementalGMMrsquos online updating capability is tested by changing theenvironment As shown in Figure 8 the table in Figure 5 isrotated and translated to change the environment BesidesWe have also carried out simulations to change the envi-ronment through moving the robot Algorithm 2 is used toupdate the GMM online The new Gaussian components areadded and trained according to the binary EM iterations from(7) to (11) and the basic EM algorithm from (3) to (5) isused to train the GMM with new components added Thiswill allow the GMM to fit the new collision sample set Afterthe training is completed the Gaussian components thatcannot help to fit the new collision sample set are eliminatedto reduce the computational overhead of the algorithm Asshown in Figures 8(b) 8(d) 8(f) and 8(h) the projectionsof the GMM on the first joint and the third joint subspacechange according to the changes of environment and robotbase Therefore GMM has the ability to respond to theenvironmental changes online

42 6-DOF Robot Motion Planning The 6-DOF UR10robot is used to perform motion planning in the simulationenvironment of Figure 5 After the completion of GMMrsquosincremental training the false positive and false negativegeneralization error curves (Figure 6) are obtained usingthe cross-validation method in Section 33 and the decisionboundaries 120575119888119900119897 and 120575119888119900119897minus119891119903119890119890 are determined accordinglyFinally the GMM-based collision query strategy describedabove is applied to four sampling-based motion planningalgorithms RRT [3] RRT-Connect [28] BiEST [29] andKPIECE [30]

The initial state and the goal region of the planningproblem are specified and the above planners are used tosolve the planning problem repeatedly to obtain the boxplotsof the planning time (Figure 9) Besides we provide theaverage planning time and the planning success rate asshown in Table 1 The allowed planning time is set to 50s Itcan be seen that the collision query strategy based on GMMis generally applicable to various sampling-based motionplanning algorithms Compared with the basic collisionquery methods based on forward kinematics and spacegeometry the computational overhead is greatly reduced andthe overall motion planning efficiency is improved by 2-3times

Complexity 9

(a) Translating the table

25

20

15

10

05

0

minus05

minus10

minus15

25201510050minus05 30 35

3th

join

t (ra

d)

1st joint (rad)

(b) Updating GMM for tablersquo translation

(c) Rotating the table

25

20

15

10

05

0

minus05

minus10

minus15

25201510050minus05 30 35

3th

join

t (ra

d)

1st joint (rad)

(d) Updating GMM for tablersquo rotation

(e) Translating the robot

05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)

(f) Updating GMM for robotrsquo translation

Figure 8 Continued

10 Complexity

(g) Rotating the robot

05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)

(h) Updating GMM for robotrsquo rotation

Figure 8 GMMrsquos online updating

GMM-RRT Basic-RRT0

10

20

30

40

50

60

Tim

e (s)

RRT

(a) RRTGMM-RRT-Connect Basic-RRT-Connect

1

2

3

4

5

6

7

8

9

10

11

Tim

e (s)

RRT-Connect

(b) RRT-Connect

GMM-BiEST Basic-BiEST0

5

10

15

20

25

30

35

40

Tim

e (s)

BiEST

(c) BiESTGMM-KPIECE Basic-KPIECE

0

10

20

30

40

50

60

Tim

e (s)

KPIECE

(d) KPIECE

Figure 9 The boxplots of various motion planning algorithmsrsquo planning times

Complexity 11

(a) Scene 1 desk (b) Scene 2 kitchen

(c) Scene 3 microwave oven (d) Scene 4 cabinet

Figure 10 A 6-DOF UR10 robot is used to pick and place a cup in various home environments and the universality of the proposed methodis verified The green grid represents occupied space stored in the octree data structure and the blue line represents the trajectory of the endeffector when the robot performs the pick and place task

Table 1 Planning performance of the motion planning algorithms

Planning Methods RRT RRT-Connect BiEST KPIECEBasic GMM Basic GMM Basic GMM Basic GMM

Average Planning Time(s) 2204 1063 5122 2239 1805 5200 3434 1519Successful Rate() 674 953 100 100 963 100 462 931

In order to further test the effectiveness of the proposedmethod in various environments four common scenes (Fig-ure 10) are built in a simulation environment The RGB-Dcamera is used to acquire the point cloud data of each sceneand a three-dimensional octree map is created based on theOctoMap library [31] The Greedy EM algorithm is usedto train the GMMs of the robot configuration space in thefour scenes respectively and the GMM-based and the basiccollision query methods are applied to the above four kindsof motion planners respectively In the four planning scenesthe task of picking and placing a cup is accomplished bymotion planning of a robot repeatedly testing this planningtask and obtaining the average planning time and successfulrate with the GMM-based and basic collision query methodsas shown in Table 2 The allowed planning time is set to 50s

From Table 2 it can be seen that the actual performanceof the proposed method varies due to different complexityof the planning tasks and number of Gaussian componentsthe latter of which is caused by different planning scenes

However the average planning time of the GMM-basedmotion planning method is reduced by 2-4 times comparedwith the basic collision query method

43 12-DOF Robot Motion Planning In order to verify theapplication effect of this algorithm in the actual scene twoUR10 robots and RealSense camera are used to build a 12-DOF dual-arm robot experimental platform as shown inFigures 11(a) and 11(b) In order to prevent the robot obstructcamerarsquos view the ldquoEye in Handrdquo arrangement is adopted

Due to the high planning dimensions of dual-arm robotsthe successful rate under allowed planning time (50s) ofmost motion planning algorithms is too low and it isdifficult to obtain large amounts of data through multipleexperiments Therefore only the RRT-Connect algorithmis used to perform multiple experiments and the planningtimes based on the GMMmethod and the basic method arerespectively obtained (Figure 11(d)) The success rate of bothmethods is 100

12 Complexity

Table2Planning

perfo

rmance

ofthem

otionplanning

algorithm

sinfour

scenes

AverageP

lann

ingtim

e(s)SuccessfulR

ate()

RRT

RRT-Con

nect

BiES

TKP

IECE

Basic

GMM

Basic

GMM

Basic

GMM

Basic

GMM

Scene1

3227312

2747720

09711

000530100

3425860

172410

04680823

145810

0Scene2

1755902

9759987

0310100

0178100

192910

00779100

1446

100

0632100

Scene3

1632

903

4176100

060

7100

0372100

6792700

2056100

4128100

160510

0Scene4

3320686

741610

00510100

02711

003737100

0734100

3348100

182610

0

Complexity 13

(a) Initial state (b) Goal state

(c) Octree map and trajectoryGMM-RRT-Connect Basic-RRT-Connect

RRT-Connect

2

4

6

8

10

Tim

e (s)

(d) Boxplots of planning time

Figure 11 12-DOF dual-arm robot experiment(a) (b)The initial and goal state of the planning problem respectively (c)The environmentalmodel represented by octree and the lines represent the trajectory of end effectors (d) The boxplots of planning time based on GMM andbasic collision query method

Experiments show that the method proposed in thispaper is also applicable to higher-dimensional motion plan-ning problems

5 Conclusion

In order to improve the planning efficiency of the sampling-based motion planning algorithm this paper studies theinfluence of the number of components on the fitting accu-racy and proposes a self-adapting model training methodwhich achieves a rapid collision query in the robot high-dimensional configuration space Using themachine learningmethod the robot learns the collision query results gen-erated during the previous motion planning instances andthe Gaussian mixture model is incrementally established toquickly estimate the new high-dimensional samplesrsquo collisionprobability In order to eliminate the model fitting errorscaused by environmental changes the Greedy EM algo-rithm is used to adaptively select number of components ofthe GMM-based on the convergence of the log-likelihoodfunction and achieved online response to the changes of

external environment This method is applied to severalkinds of sampling-based motion planning algorithms Thesimulations and real world experiments are performed on the6-DOF and 12-DOF robots in various scenes Compared withthe basicmethods the planning time is greatly shortenedTheeffectiveness of the proposed algorithm is proved

In the future work we will continue to focus onimproving the practical performance of this method inhigher-dimensional robotmotionplanning problems such asHumanoid robots and quadruped robots The GMM will beused for guided sampling to further narrow the scope of thesearch algorithm

Data Availability

All data generated or analyzed during this study are includedin this published article

Conflicts of Interest

The authors declare that they have no conflicts of interest

14 Complexity

Acknowledgments

This work is supported by National Natural Science Founda-tion of China (NoU1713222No 61773139 andNo 61473015)Natural Science Foundation of Heilongjiang Province China(Grant No F2015008) and Shenzhen Peacock Plan (NoKQTD2016112515134654)

References

[1] S Karaman and E Frazzoli ldquoSampling-based algorithms foroptimal motion planningrdquo International Journal of RoboticsResearch vol 30 no 7 pp 846ndash894 2011

[2] L Kavraki P Svestka and M H Overmars Probabilisticroadmaps for path planning in high-dimensional configurationspaces vol 1994 Unknown Publisher 1994

[3] S M LaValle and J J Kuffner Jr ldquoRandomized kinodynamicplanningrdquo International Journal of Robotics Research vol 20 no5 pp 378ndash400 2001

[4] L Janson E Schmerling A Clark and M Pavone ldquoFastmarching tree A fast marching sampling-based method foroptimal motion planning in many dimensionsrdquo InternationalJournal of Robotics Research vol 34 no 7 pp 883ndash921 2015

[5] G v Bergen ldquoEfficient Collision Detection of ComplexDeformable Models using AABB Treesrdquo Journal of GraphicsTools vol 2 no 4 pp 1ndash13 1997

[6] G Sanchez and J-C Latombe ldquoOn delaying collision checkingin PRM planning Application to multi-robot coordinationrdquoInternational Journal of Robotics Research vol 21 no 1 pp 5ndash262002

[7] S Dalibard and J-P Laumond ldquoLinear dimensionality reduc-tion in random motion planningrdquo International Journal ofRobotics Research vol 30 no 12 pp 1461ndash1476 2011

[8] DHsu T Jiang J Reif andZ Sun ldquoThe bridge test for samplingnarrow passages with probabilistic roadmap plannersrdquo in Pro-ceedings of the 2003 IEEE International Conference on Roboticsand Automation pp 4420ndash4426 Taiwan September 2003

[9] L Zhang and D Manocha ldquoAn efficient retraction-basedRRT plannerrdquo in Proceedings of the 2008 IEEE InternationalConference on Robotics and Automation ICRA 2008 pp 3743ndash3750 USA May 2008

[10] M Saha J-C Latombe Y-C Chang and F Prinz ldquoFindingnarrow passages with probabilistic roadmaps The small-stepretraction methodrdquo Autonomous Robots vol 19 no 3 pp 301ndash319 2005

[11] B Burns and O Brock ldquoInformation theoretic construction ofprobabilistic roadmapsrdquo in Proceedings of the Intelligent Robotsand Systems 2003(IROS 2003) 2003 IEEERSJ InternationalConference vol 1 pp 650ndash655 2003

[12] O Arslan and P Tsiotras ldquoMachine learning guided explorationfor sampling-based motion planning algorithmsrdquo in Proceed-ings of the IEEERSJ International Conference on IntelligentRobots and Systems IROS 2015 pp 2646ndash2652 GermanyOctober 2015

[13] O Arslan and P Tsiotras e role of vertex consistency insampling-based algorithms for optimal motion planning 2012arXiv preprint arXiv12046453

[14] J Bialkowski M Otte and E Frazzoli ldquoFree-configurationbiased sampling for motion planningrdquo in Proceedings of the 201326th IEEERSJ International Conference on Intelligent Robotsand Systems New Horizon IROS 2013 pp 1272ndash1279 JapanNovember 2013

[15] B Burns and O Brock Model-based motion planning vol51 Computer Science Department Faculty Publication Series2004

[16] C Yang X Wang L Cheng and H Ma ldquoNeural-Learning-Based Telerobot Control with Guaranteed Performancerdquo IEEETransactions on Cybernetics vol 47 no 10 pp 3148ndash3159 2017

[17] J Pan and D Manocha ldquoFast and robust motion planning withnoisy data using machine learningrdquo in Proceedings of the 30thInternational Conference on Machine Learning 2013

[18] K Yang S Keat Gan and S Sukkarieh ldquoA Gaussian process-based RRT planner for the exploration of an unknown andcluttered environment with a UAVrdquo Advanced Robotics vol 27no 6 pp 431ndash443 2013

[19] J Huh and D D Lee ldquoLearning high-dimensional MixtureModels for fast collision detection in Rapidly-Exploring Ran-dom Treesrdquo in Proceedings of the Robotics and Automation(ICRA) 2016 IEEE International Conference pp 63ndash69 2016

[20] B Ichter J Harrison and M Pavone Learning samplingdistributions for robot motion planning 2017 arXiv preprintarXiv170905448

[21] C Yang G Peng Y Li R Cui L Cheng and Z Li ldquoNeuralnetworks enhanced adaptive admittance control of optimizedrobot-environment interactionrdquo IEEE Transactions on Cyber-netics vol 99 p 12 2018

[22] M Phillips B J Cohen S Chitta andM Likhachev ldquoE-GraphsBootstrapping Planning with Experience Graphsrdquo RoboticsScience and Systems vol 5 no 1 2012

[23] C Yang C Chen W He R Cui and Z Li ldquoRobot Learn-ing System Based on Adaptive Neural Control and DynamicMovement Primitivesrdquo in Proceedings of the EEE transactionson neural networks and learning systems vol 99 pp 1ndash11 2018

[24] N Vlassis and A Likas ldquoA greedy EM algorithm for Gaussianmixture learningrdquo Neural Processing Letters vol 15 no 1 pp77ndash87 2002

[25] J Q Li andA R Barron ldquoMixture density estimationrdquoAdvancesin neural information processing systems pp 279ndash285 2000

[26] J Pan S Chitta and D Manocha ldquoFCL A general purposelibrary for collision and proximity queriesrdquo pp 3859ndash3866

[27] I A Sucan M Moll and L Kavraki ldquoThe open motionplanning libraryrdquo IEEE Robotics and Automation Magazine vol19 no 4 pp 72ndash82 2012

[28] J J Kuffner Jr and S M la Valle ldquoRRT-connect an efficientapproach to single-query path planningrdquo in Proceedings of theIEEE International Conference on Robotics and Automation pp995ndash1001 April 2000

[29] D Hsu J-C Latombe and R Motwani ldquoPath planning inexpansive configuration spacesrdquo in Proceedings of the 1997 IEEEInternational Conference on Robotics and Automation ICRAPart 3 (of 4) pp 2719ndash2726 April 1997

[30] I A Sucan and L E Kavraki ldquoKinodynamic motion planningby interior-exterior cell explorationrdquo inAlgorithmic Foundationof Robotics VIII pp 449ndash464 Springer Berlin Heidelberg2009

[31] A Hornung K M Wurm M Bennewitz C Stachniss andW Burgard ldquoOctoMap An efficient probabilistic 3D mappingframework based on octreesrdquo Autonomous Robots vol 34 no3 pp 189ndash206 2013

Hindawiwwwhindawicom Volume 2018

MathematicsJournal of


Mathematical Problems in Engineering

Applied MathematicsJournal of


Probability and StatisticsHindawiwwwhindawicom Volume 2018

Journal of


Mathematical PhysicsAdvances in

Complex AnalysisJournal of


OptimizationJournal of



Engineering Mathematics

International Journal of


Operations ResearchAdvances in

Journal of


Function SpacesAbstract and Applied AnalysisHindawiwwwhindawicom Volume 2018

International Journal of Mathematics and Mathematical Sciences


Hindawi Publishing Corporation httpwwwhindawicom Volume 2013Hindawiwwwhindawicom

The Scientific World Journal

Volume 2018

Hindawiwwwhindawicom Volume 2018Volume 2018

Numerical AnalysisNumerical AnalysisNumerical AnalysisNumerical AnalysisNumerical AnalysisNumerical AnalysisNumerical AnalysisNumerical AnalysisNumerical AnalysisNumerical AnalysisNumerical AnalysisNumerical AnalysisAdvances inAdvances in Discrete Dynamics in

Nature and SocietyHindawiwwwhindawicom Volume 2018

Hindawiwwwhindawicom

Dierential EquationsInternational Journal of

Volume 2018


Decision SciencesAdvances in


AnalysisInternational Journal of


Stochastic AnalysisInternational Journal of

Submit your manuscripts atwwwhindawicom

2 Complexity

A large amount of research work is devoted to performingadaptive sampling and guiding the planning algorithm toexplore certain areas in the configuration space with machinelearning methods Dalibard et al [7] use the principalcomponent analysis (PCA) method for online analysis ofsamples to estimate the direction and position of the localnarrow passage in collision-free space and increase thesampling density in the direction of the passage axis Similarmethods include bridge test [8] and retraction strategy [9 10]These methods can significantly improve the efficiency ofthe planning algorithm at the local narrow passage Brockand Burns [11] propose an exploration strategy based onentropy guidance which can guide the planning algorithm tosample the areas that maximize information gain Howeverbecause of the high computational cost of this method itis more suitable for the off-line path graph construction ofmultiqueue query algorithms like PRM Arslan and Tsiotras[12] use the kernel function to learn the feasibility andheuristics of samples generated in the previous planninginstances and increase the sampling density of the areasthat may make current path cost lower This method canimprove the convergence rate of asymptotically optimummotion planning algorithms RRT [13] Bialkowski et al [14]propose to store empirically observed estimates of collision-free space in a point proximity data structure and then usekd-tree algorithm to generate future valid samples

In addition by learning the past experience of motionplanning the prediction of the new samplesrsquo feasibility canbe achieved Burns et al [15] present a model-based motionplanning method This method utilizes the local weightedregression to incrementally learn the approximate model ofthe configuration space and the classification of new samplesis implemented Compared with the traditional collisionquery module the method has a smaller computationalcomplexity Yang et al [16] propose a neural networksframework to achieve robot automatic collision avoidance Byexploiting the joint space redundancy the human operatorwould be able to only concentrate on motion of robots endeffector without concern over possible collision Pan et al [17]report that the collision query results produced in previousmotion planning tasks are stored in a data set When the newsample is generated the KNN algorithm is used to searchthe 119896 nearest neighbors of the sample in the data set andthe probability of its feasibility is estimated according to theneighborsrsquo collision information Yang et al [18] combine theflexibility of the Gauss process (GP) with the efficiency ofthe RRT algorithm and establish a motion model to predictthe movement of obstacles so that the robot can find a safepath in the dynamic environment constraints Huh et al[19] propose that the Gaussian mixture model can be usedto fit the probability distribution of the high-dimensionalspace obstacle region so as to quickly predict the feasibilityof the new samples However the influence of the numberof Gaussian components on the model prediction accuracyhas not been analyzed and considered Besides some otherlearning 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 motionplanning to some extent but there are the following prob-lems

(1) Some lazy learning methods such as kd-tree andKNN require data sets with large sample size In the motionplanning problem of high-dimensional space this will lead togreat spatial complexity which is hard to realize

(2) Models like PCA CVAE E-Graphs and localweighted regression have poor flexibility Even if the environ-ment or the base of robot changes slightly the model needsto be retrained which greatly increases the computationaloverhead

Therefore in this article we propose to use Gaussianmixture model (GMM) as a prior model of robot config-uration space to improve the efficiency of robot motionplanning GMM can represent the unknown model by thelinear combination of Gaussian probability density functionsIt has the ability to fit the continuous probability densitydistribution in any dimensional space with small spatialcomplexity On the other hand the Greedy EM algorithmutilized in this paper can realize incremental training ofGMM so that the model can be updated according to theenvironment changes without retraining

In general the main contributions of this paper includethe following (1) The influence of number of componentson the prediction accuracy is analyzed and a method foradaptively training GMM of collision region in robot high-dimensional configuration space based on the convergenceof log-likelihood function is proposed (2) The Greedy EMalgorithm is used to update the GMM online with real-time 3D map to adapt to environmental changes (3) Theabove method is applied to a variety of sampling-basedmotion planning algorithms and the planning efficiency issignificantly improved

Thepaper is organized as follows Section 2 introduces themain motivation of the research work Section 3 introducesthe incremental training process of GMM using GreedyEM clustering algorithm and the integration with motionplanning algorithms Section 4 shows the simulations andthe real world experimentsrsquo results Section 5 summarizes theresults and provides directions for future work

2 Motivations and Problem Statement

The Gaussian mixture model [24] is a mixture probabilisticmodel that can fit the probability density distribution ofarbitrary dimensions and arbitrary shapes by weighting theprobability density functions of multiple Gaussian distribu-tions

119875 (119909 | 120579) =119870

sum119896=1

120587119896120601 (119909 | 120579119896) (1)

where120587119896 is the component weights and satisfied 1205871+1205872+sdot sdot sdot+120587119870 = 1 120587119896 ge 0 120601(119909 | 120579119896) is the probability density function ofthe 119896th Gaussian distribution and 120579119896 is the parameter vectorof the distribution

Complexity 3

(a) Map (b) 119870 = 7 (c) 119870 = 10

(d) 119870 = 15 (e) 119870 = 25

55

50

45

40

35

30

25

20

15

10

05

Col

lisio

n Pr

obab

ility

times10minus4

(f) 119870 = 40


120601 (119909 | 120579119896) = (2120587)minus1198892 10038161003816100381610038161198781198961003816100381610038161003816minus12






119875 (119896 | 119909119895) =120587119896120601 (119909119895 | 120579119896)

sum119870119896=1 120587119896120601 (119909119895 | 120579119896) 119895 = 1 2 119873 (3)


120587119896 = 1119873119873

sum119895=1

119875 (119896 | 119909119895)

119898119896 =sum119873119895=1 119875 (119896 | 119909119895) 119909119895sum119873119895=1 119875 (119896 | 119909119895)

119878119896 =sum119873119895=1 119875 (119896 | 119909119895) (119909119895 minus 119898119896) (119909119895 minus 119898119896)

T

sum119873119895=1 119875 (119896 | 119909119895)

(4)


L119870 =119873

sum119895=1

log119875 (119909119895 | 120579) (5)


4 Complexity


00501

01502

02503

03504

Erro

r pro

babi

lity

GMMsFitting curve


times 10


0

02

04

06

08

1

Aver

age c

alcu

latio

n tim

e (s)

GMMsFitting curve











3 Algorithms




Complexity 5


Random expansion




Forward kinematic






model

Newsamples

Add new component


Binary EM cluster

Global EM cluster

New GMM model

Perception

Newmap

RGBDCamera


Update the octomap



119891119896+1 (119909) = (1 minus 119886119899119890119908) 119891119896 (119909) + 119886119899119890119908120601 (119909 | 120579) 119886119899119890119908 isin (0 1)

(6)


119886119899119890119908 =

05 if 119896 = 12119896 + 1 if 119896 ge 2 (7)


120590 = 120573 [ 4119889 + 2119873]

1(119889+4)

(8)



L119896+1 =119873

sum119895=1

log119891119896+1 (119909119895)

=119873

sum119895=1

log [(1 minus 119886119899119890119908) 119891119896 (119909119895) + 119886119899119890119908120601 (119909 | 120579)](9)




119875 (119896 + 1 | 119909119895) =119886119899119890119908120601 (119909119895 | 120579)

(1 minus 119886119899119890119908) 119891119896 (119909119895) + 119886119899119890119908120601 (119909119895 | 120579)(10)

6 Complexity





119886119899119890119908 = 1119873119873

sum119895=1

119875 (119896 + 1 | 119909119895)

119898119899119890119908 =sum119873119895=1 119875 (119896 + 1 | 119909119895) 119909119895sum119873119895=1 119875 (119896 + 1 | 119909119895)

119878119899119890119908 =sum119873119895=1 119875 (119896 + 1 | 119909119895) (119909119895 minus 119898) (119909119895 minus 119898)

T

sum119873119895=1 119875 (119896 + 1 | 119909119895)

(11)









Complexity 7

Previous GMMs

Current GMMs

Olds samples

New samples

Add new component












8 Complexity


03

004

30102030405060708090

100

Erro

r pro

babi

lity

()

times10-3


Tolerance＝ＩＦ




minus22

minus21

minus2

minus19

minus18

minus17

Log-

likel

ihoo

d

times104










Complexity 9


25

20

15

10

05

0

minus05

minus10

minus15

25201510050minus05 30 35

3th

join

t (ra

d)

1st joint (rad)



25

20

15

10

05

0

minus05

minus10

minus15

25201510050minus05 30 35

3th

join

t (ra

d)

1st joint (rad)



05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)


Figure 8 Continued

10 Complexity


05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)



GMM-RRT Basic-RRT0

10

20

30

40

50

60

Tim

e (s)

RRT


1

2

3

4

5

6

7

8

9

10

11

Tim

e (s)

RRT-Connect

(b) RRT-Connect


5

10

15

20

25

30

35

40

Tim

e (s)

BiEST


0

10

20

30

40

50

60

Tim

e (s)

KPIECE

(d) KPIECE


Complexity 11












12 Complexity

Table2Planning

perfo

rmance

ofthem

otionplanning

algorithm

sinfour

scenes

AverageP

lann

ingtim

e(s)SuccessfulR

ate()

RRT

RRT-Con

nect

BiES

TKP

IECE

Basic

GMM

Basic

GMM

Basic

GMM

Basic

GMM

Scene1

3227312

2747720

09711

000530100

3425860

172410

04680823

145810

0Scene2

1755902

9759987

0310100

0178100

192910

00779100

1446

100

0632100

Scene3

1632

903

4176100

060

7100

0372100

6792700

2056100

4128100

160510

0Scene4

3320686

741610

00510100

02711

003737100

0734100

3348100

182610

0

Complexity 13



RRT-Connect

2

4

6

8

10

Tim

e (s)




5 Conclusion




Data Availability




14 Complexity

Acknowledgments


References







































Journal of












Journal of







Volume 2018






Volume 2018








Complexity 3

(a) Map (b) 119870 = 7 (c) 119870 = 10

(d) 119870 = 15 (e) 119870 = 25

55

50

45

40

35

30

25

20

15

10

05

Col

lisio

n Pr

obab

ility

times10minus4

(f) 119870 = 40


120601 (119909 | 120579119896) = (2120587)minus1198892 10038161003816100381610038161198781198961003816100381610038161003816minus12






119875 (119896 | 119909119895) =120587119896120601 (119909119895 | 120579119896)

sum119870119896=1 120587119896120601 (119909119895 | 120579119896) 119895 = 1 2 119873 (3)


120587119896 = 1119873119873

sum119895=1

119875 (119896 | 119909119895)

119898119896 =sum119873119895=1 119875 (119896 | 119909119895) 119909119895sum119873119895=1 119875 (119896 | 119909119895)

119878119896 =sum119873119895=1 119875 (119896 | 119909119895) (119909119895 minus 119898119896) (119909119895 minus 119898119896)

T

sum119873119895=1 119875 (119896 | 119909119895)

(4)


L119870 =119873

sum119895=1

log119875 (119909119895 | 120579) (5)


4 Complexity


00501

01502

02503

03504

Erro

r pro

babi

lity

GMMsFitting curve


times 10


0

02

04

06

08

1

Aver

age c

alcu

latio

n tim

e (s)

GMMsFitting curve











3 Algorithms




Complexity 5


Random expansion




Forward kinematic






model

Newsamples

Add new component


Binary EM cluster

Global EM cluster

New GMM model

Perception

Newmap

RGBDCamera


Update the octomap



119891119896+1 (119909) = (1 minus 119886119899119890119908) 119891119896 (119909) + 119886119899119890119908120601 (119909 | 120579) 119886119899119890119908 isin (0 1)

(6)


119886119899119890119908 =

05 if 119896 = 12119896 + 1 if 119896 ge 2 (7)


120590 = 120573 [ 4119889 + 2119873]

1(119889+4)

(8)



L119896+1 =119873

sum119895=1

log119891119896+1 (119909119895)

=119873

sum119895=1

log [(1 minus 119886119899119890119908) 119891119896 (119909119895) + 119886119899119890119908120601 (119909 | 120579)](9)




119875 (119896 + 1 | 119909119895) =119886119899119890119908120601 (119909119895 | 120579)

(1 minus 119886119899119890119908) 119891119896 (119909119895) + 119886119899119890119908120601 (119909119895 | 120579)(10)

6 Complexity





119886119899119890119908 = 1119873119873

sum119895=1

119875 (119896 + 1 | 119909119895)

119898119899119890119908 =sum119873119895=1 119875 (119896 + 1 | 119909119895) 119909119895sum119873119895=1 119875 (119896 + 1 | 119909119895)

119878119899119890119908 =sum119873119895=1 119875 (119896 + 1 | 119909119895) (119909119895 minus 119898) (119909119895 minus 119898)

T

sum119873119895=1 119875 (119896 + 1 | 119909119895)

(11)









Complexity 7

Previous GMMs

Current GMMs

Olds samples

New samples

Add new component












8 Complexity


03

004

30102030405060708090

100

Erro

r pro

babi

lity

()

times10-3


Tolerance＝ＩＦ




minus22

minus21

minus2

minus19

minus18

minus17

Log-

likel

ihoo

d

times104










Complexity 9


25

20

15

10

05

0

minus05

minus10

minus15

25201510050minus05 30 35

3th

join

t (ra

d)

1st joint (rad)



25

20

15

10

05

0

minus05

minus10

minus15

25201510050minus05 30 35

3th

join

t (ra

d)

1st joint (rad)



05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)


Figure 8 Continued

10 Complexity


05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)



GMM-RRT Basic-RRT0

10

20

30

40

50

60

Tim

e (s)

RRT


1

2

3

4

5

6

7

8

9

10

11

Tim

e (s)

RRT-Connect

(b) RRT-Connect


5

10

15

20

25

30

35

40

Tim

e (s)

BiEST


0

10

20

30

40

50

60

Tim

e (s)

KPIECE

(d) KPIECE


Complexity 11












12 Complexity

Table2Planning

perfo

rmance

ofthem

otionplanning

algorithm

sinfour

scenes

AverageP

lann

ingtim

e(s)SuccessfulR

ate()

RRT

RRT-Con

nect

BiES

TKP

IECE

Basic

GMM

Basic

GMM

Basic

GMM

Basic

GMM

Scene1

3227312

2747720

09711

000530100

3425860

172410

04680823

145810

0Scene2

1755902

9759987

0310100

0178100

192910

00779100

1446

100

0632100

Scene3

1632

903

4176100

060

7100

0372100

6792700

2056100

4128100

160510

0Scene4

3320686

741610

00510100

02711

003737100

0734100

3348100

182610

0

Complexity 13



RRT-Connect

2

4

6

8

10

Tim

e (s)




5 Conclusion




Data Availability




14 Complexity

Acknowledgments


References







































Journal of












Journal of







Volume 2018






Volume 2018








4 Complexity


00501

01502

02503

03504

Erro

r pro

babi

lity

GMMsFitting curve


times 10


0

02

04

06

08

1

Aver

age c

alcu

latio

n tim

e (s)

GMMsFitting curve











3 Algorithms




Complexity 5


Random expansion




Forward kinematic






model

Newsamples

Add new component


Binary EM cluster

Global EM cluster

New GMM model

Perception

Newmap

RGBDCamera


Update the octomap



119891119896+1 (119909) = (1 minus 119886119899119890119908) 119891119896 (119909) + 119886119899119890119908120601 (119909 | 120579) 119886119899119890119908 isin (0 1)

(6)


119886119899119890119908 =

05 if 119896 = 12119896 + 1 if 119896 ge 2 (7)


120590 = 120573 [ 4119889 + 2119873]

1(119889+4)

(8)



L119896+1 =119873

sum119895=1

log119891119896+1 (119909119895)

=119873

sum119895=1

log [(1 minus 119886119899119890119908) 119891119896 (119909119895) + 119886119899119890119908120601 (119909 | 120579)](9)




119875 (119896 + 1 | 119909119895) =119886119899119890119908120601 (119909119895 | 120579)

(1 minus 119886119899119890119908) 119891119896 (119909119895) + 119886119899119890119908120601 (119909119895 | 120579)(10)

6 Complexity





119886119899119890119908 = 1119873119873

sum119895=1

119875 (119896 + 1 | 119909119895)

119898119899119890119908 =sum119873119895=1 119875 (119896 + 1 | 119909119895) 119909119895sum119873119895=1 119875 (119896 + 1 | 119909119895)

119878119899119890119908 =sum119873119895=1 119875 (119896 + 1 | 119909119895) (119909119895 minus 119898) (119909119895 minus 119898)

T

sum119873119895=1 119875 (119896 + 1 | 119909119895)

(11)









Complexity 7

Previous GMMs

Current GMMs

Olds samples

New samples

Add new component












8 Complexity


03

004

30102030405060708090

100

Erro

r pro

babi

lity

()

times10-3


Tolerance＝ＩＦ




minus22

minus21

minus2

minus19

minus18

minus17

Log-

likel

ihoo

d

times104










Complexity 9


25

20

15

10

05

0

minus05

minus10

minus15

25201510050minus05 30 35

3th

join

t (ra

d)

1st joint (rad)



25

20

15

10

05

0

minus05

minus10

minus15

25201510050minus05 30 35

3th

join

t (ra

d)

1st joint (rad)



05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)


Figure 8 Continued

10 Complexity


05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)



GMM-RRT Basic-RRT0

10

20

30

40

50

60

Tim

e (s)

RRT


1

2

3

4

5

6

7

8

9

10

11

Tim

e (s)

RRT-Connect

(b) RRT-Connect


5

10

15

20

25

30

35

40

Tim

e (s)

BiEST


0

10

20

30

40

50

60

Tim

e (s)

KPIECE

(d) KPIECE


Complexity 11












12 Complexity

Table2Planning

perfo

rmance

ofthem

otionplanning

algorithm

sinfour

scenes

AverageP

lann

ingtim

e(s)SuccessfulR

ate()

RRT

RRT-Con

nect

BiES

TKP

IECE

Basic

GMM

Basic

GMM

Basic

GMM

Basic

GMM

Scene1

3227312

2747720

09711

000530100

3425860

172410

04680823

145810

0Scene2

1755902

9759987

0310100

0178100

192910

00779100

1446

100

0632100

Scene3

1632

903

4176100

060

7100

0372100

6792700

2056100

4128100

160510

0Scene4

3320686

741610

00510100

02711

003737100

0734100

3348100

182610

0

Complexity 13



RRT-Connect

2

4

6

8

10

Tim

e (s)




5 Conclusion




Data Availability




14 Complexity

Acknowledgments


References







































Journal of












Journal of







Volume 2018






Volume 2018








Complexity 5


Random expansion




Forward kinematic






model

Newsamples

Add new component


Binary EM cluster

Global EM cluster

New GMM model

Perception

Newmap

RGBDCamera


Update the octomap



119891119896+1 (119909) = (1 minus 119886119899119890119908) 119891119896 (119909) + 119886119899119890119908120601 (119909 | 120579) 119886119899119890119908 isin (0 1)

(6)


119886119899119890119908 =

05 if 119896 = 12119896 + 1 if 119896 ge 2 (7)


120590 = 120573 [ 4119889 + 2119873]

1(119889+4)

(8)



L119896+1 =119873

sum119895=1

log119891119896+1 (119909119895)

=119873

sum119895=1

log [(1 minus 119886119899119890119908) 119891119896 (119909119895) + 119886119899119890119908120601 (119909 | 120579)](9)




119875 (119896 + 1 | 119909119895) =119886119899119890119908120601 (119909119895 | 120579)

(1 minus 119886119899119890119908) 119891119896 (119909119895) + 119886119899119890119908120601 (119909119895 | 120579)(10)

6 Complexity





119886119899119890119908 = 1119873119873

sum119895=1

119875 (119896 + 1 | 119909119895)

119898119899119890119908 =sum119873119895=1 119875 (119896 + 1 | 119909119895) 119909119895sum119873119895=1 119875 (119896 + 1 | 119909119895)

119878119899119890119908 =sum119873119895=1 119875 (119896 + 1 | 119909119895) (119909119895 minus 119898) (119909119895 minus 119898)

T

sum119873119895=1 119875 (119896 + 1 | 119909119895)

(11)









Complexity 7

Previous GMMs

Current GMMs

Olds samples

New samples

Add new component












8 Complexity


03

004

30102030405060708090

100

Erro

r pro

babi

lity

()

times10-3


Tolerance＝ＩＦ




minus22

minus21

minus2

minus19

minus18

minus17

Log-

likel

ihoo

d

times104










Complexity 9


25

20

15

10

05

0

minus05

minus10

minus15

25201510050minus05 30 35

3th

join

t (ra

d)

1st joint (rad)



25

20

15

10

05

0

minus05

minus10

minus15

25201510050minus05 30 35

3th

join

t (ra

d)

1st joint (rad)



05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)


Figure 8 Continued

10 Complexity


05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)



GMM-RRT Basic-RRT0

10

20

30

40

50

60

Tim

e (s)

RRT


1

2

3

4

5

6

7

8

9

10

11

Tim

e (s)

RRT-Connect

(b) RRT-Connect


5

10

15

20

25

30

35

40

Tim

e (s)

BiEST


0

10

20

30

40

50

60

Tim

e (s)

KPIECE

(d) KPIECE


Complexity 11












12 Complexity

Table2Planning

perfo

rmance

ofthem

otionplanning

algorithm

sinfour

scenes

AverageP

lann

ingtim

e(s)SuccessfulR

ate()

RRT

RRT-Con

nect

BiES

TKP

IECE

Basic

GMM

Basic

GMM

Basic

GMM

Basic

GMM

Scene1

3227312

2747720

09711

000530100

3425860

172410

04680823

145810

0Scene2

1755902

9759987

0310100

0178100

192910

00779100

1446

100

0632100

Scene3

1632

903

4176100

060

7100

0372100

6792700

2056100

4128100

160510

0Scene4

3320686

741610

00510100

02711

003737100

0734100

3348100

182610

0

Complexity 13



RRT-Connect

2

4

6

8

10

Tim

e (s)




5 Conclusion




Data Availability




14 Complexity

Acknowledgments


References







































Journal of












Journal of







Volume 2018






Volume 2018








6 Complexity





119886119899119890119908 = 1119873119873

sum119895=1

119875 (119896 + 1 | 119909119895)

119898119899119890119908 =sum119873119895=1 119875 (119896 + 1 | 119909119895) 119909119895sum119873119895=1 119875 (119896 + 1 | 119909119895)

119878119899119890119908 =sum119873119895=1 119875 (119896 + 1 | 119909119895) (119909119895 minus 119898) (119909119895 minus 119898)

T

sum119873119895=1 119875 (119896 + 1 | 119909119895)

(11)









Complexity 7

Previous GMMs

Current GMMs

Olds samples

New samples

Add new component












8 Complexity


03

004

30102030405060708090

100

Erro

r pro

babi

lity

()

times10-3


Tolerance＝ＩＦ




minus22

minus21

minus2

minus19

minus18

minus17

Log-

likel

ihoo

d

times104










Complexity 9


25

20

15

10

05

0

minus05

minus10

minus15

25201510050minus05 30 35

3th

join

t (ra

d)

1st joint (rad)



25

20

15

10

05

0

minus05

minus10

minus15

25201510050minus05 30 35

3th

join

t (ra

d)

1st joint (rad)



05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)


Figure 8 Continued

10 Complexity


05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)



GMM-RRT Basic-RRT0

10

20

30

40

50

60

Tim

e (s)

RRT


1

2

3

4

5

6

7

8

9

10

11

Tim

e (s)

RRT-Connect

(b) RRT-Connect


5

10

15

20

25

30

35

40

Tim

e (s)

BiEST


0

10

20

30

40

50

60

Tim

e (s)

KPIECE

(d) KPIECE


Complexity 11












12 Complexity

Table2Planning

perfo

rmance

ofthem

otionplanning

algorithm

sinfour

scenes

AverageP

lann

ingtim

e(s)SuccessfulR

ate()

RRT

RRT-Con

nect

BiES

TKP

IECE

Basic

GMM

Basic

GMM

Basic

GMM

Basic

GMM

Scene1

3227312

2747720

09711

000530100

3425860

172410

04680823

145810

0Scene2

1755902

9759987

0310100

0178100

192910

00779100

1446

100

0632100

Scene3

1632

903

4176100

060

7100

0372100

6792700

2056100

4128100

160510

0Scene4

3320686

741610

00510100

02711

003737100

0734100

3348100

182610

0

Complexity 13



RRT-Connect

2

4

6

8

10

Tim

e (s)




5 Conclusion




Data Availability




14 Complexity

Acknowledgments


References







































Journal of












Journal of







Volume 2018






Volume 2018








Complexity 7

Previous GMMs

Current GMMs

Olds samples

New samples

Add new component












8 Complexity


03

004

30102030405060708090

100

Erro

r pro

babi

lity

()

times10-3


Tolerance＝ＩＦ




minus22

minus21

minus2

minus19

minus18

minus17

Log-

likel

ihoo

d

times104










Complexity 9


25

20

15

10

05

0

minus05

minus10

minus15

25201510050minus05 30 35

3th

join

t (ra

d)

1st joint (rad)



25

20

15

10

05

0

minus05

minus10

minus15

25201510050minus05 30 35

3th

join

t (ra

d)

1st joint (rad)



05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)


Figure 8 Continued

10 Complexity


05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)



GMM-RRT Basic-RRT0

10

20

30

40

50

60

Tim

e (s)

RRT


1

2

3

4

5

6

7

8

9

10

11

Tim

e (s)

RRT-Connect

(b) RRT-Connect


5

10

15

20

25

30

35

40

Tim

e (s)

BiEST


0

10

20

30

40

50

60

Tim

e (s)

KPIECE

(d) KPIECE


Complexity 11












12 Complexity

Table2Planning

perfo

rmance

ofthem

otionplanning

algorithm

sinfour

scenes

AverageP

lann

ingtim

e(s)SuccessfulR

ate()

RRT

RRT-Con

nect

BiES

TKP

IECE

Basic

GMM

Basic

GMM

Basic

GMM

Basic

GMM

Scene1

3227312

2747720

09711

000530100

3425860

172410

04680823

145810

0Scene2

1755902

9759987

0310100

0178100

192910

00779100

1446

100

0632100

Scene3

1632

903

4176100

060

7100

0372100

6792700

2056100

4128100

160510

0Scene4

3320686

741610

00510100

02711

003737100

0734100

3348100

182610

0

Complexity 13



RRT-Connect

2

4

6

8

10

Tim

e (s)




5 Conclusion




Data Availability




14 Complexity

Acknowledgments


References







































Journal of












Journal of







Volume 2018






Volume 2018








8 Complexity


03

004

30102030405060708090

100

Erro

r pro

babi

lity

()

times10-3


Tolerance＝ＩＦ




minus22

minus21

minus2

minus19

minus18

minus17

Log-

likel

ihoo

d

times104










Complexity 9


25

20

15

10

05

0

minus05

minus10

minus15

25201510050minus05 30 35

3th

join

t (ra

d)

1st joint (rad)



25

20

15

10

05

0

minus05

minus10

minus15

25201510050minus05 30 35

3th

join

t (ra

d)

1st joint (rad)



05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)


Figure 8 Continued

10 Complexity


05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)



GMM-RRT Basic-RRT0

10

20

30

40

50

60

Tim

e (s)

RRT


1

2

3

4

5

6

7

8

9

10

11

Tim

e (s)

RRT-Connect

(b) RRT-Connect


5

10

15

20

25

30

35

40

Tim

e (s)

BiEST


0

10

20

30

40

50

60

Tim

e (s)

KPIECE

(d) KPIECE


Complexity 11












12 Complexity

Table2Planning

perfo

rmance

ofthem

otionplanning

algorithm

sinfour

scenes

AverageP

lann

ingtim

e(s)SuccessfulR

ate()

RRT

RRT-Con

nect

BiES

TKP

IECE

Basic

GMM

Basic

GMM

Basic

GMM

Basic

GMM

Scene1

3227312

2747720

09711

000530100

3425860

172410

04680823

145810

0Scene2

1755902

9759987

0310100

0178100

192910

00779100

1446

100

0632100

Scene3

1632

903

4176100

060

7100

0372100

6792700

2056100

4128100

160510

0Scene4

3320686

741610

00510100

02711

003737100

0734100

3348100

182610

0

Complexity 13



RRT-Connect

2

4

6

8

10

Tim

e (s)




5 Conclusion




Data Availability




14 Complexity

Acknowledgments


References







































Journal of












Journal of







Volume 2018






Volume 2018








Complexity 9


25

20

15

10

05

0

minus05

minus10

minus15

25201510050minus05 30 35

3th

join

t (ra

d)

1st joint (rad)



25

20

15

10

05

0

minus05

minus10

minus15

25201510050minus05 30 35

3th

join

t (ra

d)

1st joint (rad)



05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)


Figure 8 Continued

10 Complexity


05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)



GMM-RRT Basic-RRT0

10

20

30

40

50

60

Tim

e (s)

RRT


1

2

3

4

5

6

7

8

9

10

11

Tim

e (s)

RRT-Connect

(b) RRT-Connect


5

10

15

20

25

30

35

40

Tim

e (s)

BiEST


0

10

20

30

40

50

60

Tim

e (s)

KPIECE

(d) KPIECE


Complexity 11












12 Complexity

Table2Planning

perfo

rmance

ofthem

otionplanning

algorithm

sinfour

scenes

AverageP

lann

ingtim

e(s)SuccessfulR

ate()

RRT

RRT-Con

nect

BiES

TKP

IECE

Basic

GMM

Basic

GMM

Basic

GMM

Basic

GMM

Scene1

3227312

2747720

09711

000530100

3425860

172410

04680823

145810

0Scene2

1755902

9759987

0310100

0178100

192910

00779100

1446

100

0632100

Scene3

1632

903

4176100

060

7100

0372100

6792700

2056100

4128100

160510

0Scene4

3320686

741610

00510100

02711

003737100

0734100

3348100

182610

0

Complexity 13



RRT-Connect

2

4

6

8

10

Tim

e (s)




5 Conclusion




Data Availability




14 Complexity

Acknowledgments


References







































Journal of












Journal of







Volume 2018






Volume 2018








10 Complexity


05

0

minus05

minus10

minus10

minus15

minus15

minus20

minus20

minus25

minus30

1510050minus05

3th

join

t (ra

d)

1st joint (rad)



GMM-RRT Basic-RRT0

10

20

30

40

50

60

Tim

e (s)

RRT


1

2

3

4

5

6

7

8

9

10

11

Tim

e (s)

RRT-Connect

(b) RRT-Connect


5

10

15

20

25

30

35

40

Tim

e (s)

BiEST


0

10

20

30

40

50

60

Tim

e (s)

KPIECE

(d) KPIECE


Complexity 11












12 Complexity

Table2Planning

perfo

rmance

ofthem

otionplanning

algorithm

sinfour

scenes

AverageP

lann

ingtim

e(s)SuccessfulR

ate()

RRT

RRT-Con

nect

BiES

TKP

IECE

Basic

GMM

Basic

GMM

Basic

GMM

Basic

GMM

Scene1

3227312

2747720

09711

000530100

3425860

172410

04680823

145810

0Scene2

1755902

9759987

0310100

0178100

192910

00779100

1446

100

0632100

Scene3

1632

903

4176100

060

7100

0372100

6792700

2056100

4128100

160510

0Scene4

3320686

741610

00510100

02711

003737100

0734100

3348100

182610

0

Complexity 13



RRT-Connect

2

4

6

8

10

Tim

e (s)




5 Conclusion




Data Availability




14 Complexity

Acknowledgments


References







































Journal of












Journal of







Volume 2018






Volume 2018








Complexity 11












12 Complexity

Table2Planning

perfo

rmance

ofthem

otionplanning

algorithm

sinfour

scenes

AverageP

lann

ingtim

e(s)SuccessfulR

ate()

RRT

RRT-Con

nect

BiES

TKP

IECE

Basic

GMM

Basic

GMM

Basic

GMM

Basic

GMM

Scene1

3227312

2747720

09711

000530100

3425860

172410

04680823

145810

0Scene2

1755902

9759987

0310100

0178100

192910

00779100

1446

100

0632100

Scene3

1632

903

4176100

060

7100

0372100

6792700

2056100

4128100

160510

0Scene4

3320686

741610

00510100

02711

003737100

0734100

3348100

182610

0

Complexity 13



RRT-Connect

2

4

6

8

10

Tim

e (s)




5 Conclusion




Data Availability




14 Complexity

Acknowledgments


References







































Journal of












Journal of







Volume 2018






Volume 2018








12 Complexity

Table2Planning

perfo

rmance

ofthem

otionplanning

algorithm

sinfour

scenes

AverageP

lann

ingtim

e(s)SuccessfulR

ate()

RRT

RRT-Con

nect

BiES

TKP

IECE

Basic

GMM

Basic

GMM

Basic

GMM

Basic

GMM

Scene1

3227312

2747720

09711

000530100

3425860

172410

04680823

145810

0Scene2

1755902

9759987

0310100

0178100

192910

00779100

1446

100

0632100

Scene3

1632

903

4176100

060

7100

0372100

6792700

2056100

4128100

160510

0Scene4

3320686

741610

00510100

02711

003737100

0734100

3348100

182610

0

Complexity 13



RRT-Connect

2

4

6

8

10

Tim

e (s)




5 Conclusion




Data Availability




14 Complexity

Acknowledgments


References







































Journal of












Journal of







Volume 2018






Volume 2018








Complexity 13



RRT-Connect

2

4

6

8

10

Tim

e (s)




5 Conclusion




Data Availability




14 Complexity

Acknowledgments


References







































Journal of












Journal of







Volume 2018






Volume 2018








14 Complexity

Acknowledgments


References







































Journal of












Journal of







Volume 2018






Volume 2018















Journal of












Journal of







Volume 2018






Volume 2018








robot motion planning method based on incremental high...

Documents