Optimal Path of a Mobile Robot in an Uncertain

Optimal Path of a Mobile Robot in an Uncertain Environment *Hamda Khan

Umair Aziz

Department of Mechatronics Engineering

Department of Mechatronics Engineering

Air University

Air University

E9, Islamabad, Pakistan

E9, Islamabad, Pakistan

[email protected]

[email protected]

Hamda,[email protected]

HaiderSaif Agha

Zafar-ullahKoreshi

Department of Mechatronics Engineering

Department of Mechatronics Engineering

Air University

Air University

E9, Islamabad, Pakistan

E9, Islamabad, Pakistan

[email protected]

[email protected]

Abstract –The determination of optimal path for a mobile robot in the presence of obstacles, in both static and dynamic environments, is classified as a NP-hard problem that becomes computationally intractable as the size of the problem increases. Thus traditional deterministic methods are usually replaced by random search heuristic methods such as Genetic Algorithms (GA), Particle Swarm Optimization (PSO) and Random Particle Optimization (RPO). Such methods have been used in certain environments where ‘noise’ is not considered. However, realistic situations incorporate non-linear and ‘noisy’ signals for process estimation and measurement for which Monte Carlo (MC) methods are ‘natural’.

optimal algorithms in autonomous mobile robots in realistic environments.

This paper uses the MC method for determining the optimal path in a static environment with ‘uncertainty’ in link paths (distances) modeled as random variables, and highlights the Importance Sampling (IS) method for improving computational effort. It also estimates the probability of a ‘rare event’ i.e.an usually long path which is obtainable after excessive computational effort in straight-forward Monte Carlo simulation. An initial analysis considers Crude Monte Carlo (CMC) with random variables drawn from exponential probability distribution functions, to demonstrate the ‘large’ sample size, and hence computational effort, required to obtain a sufficiently accurate estimate. It is then shown that, by IS, similar to a Bayesian update, considerable speed-up is achievable by biasing successive probability distributions, with a ‘small’ sample size, for estimation of the optimal path. This work is of use for embedding

*Corresponding Author

Keywords: optimal path, autonomous mobile robots, Monte Carlo method

I.

Introduction

In robotics, the locomotion for guided and autonomous robots is achieved by sensors (contact, internal, proximity or satellite based) which provide information on the internal state (proprioceptive sensing) as well as its environment (external sensing). For the internal state, data is required for example on the encoders and accelerometers which provide information on the location. External data is also required for path planning in an environment which may contain obstacles in a static or dynamic configuration. Information is collected by the use of sensors such as accelerometers, digital compasses, sonar or laser range finders, infra-red sensors and simple cameras. Some of these sensors are ‘active’ i.e. information is sent and received, such as sonars, while others are passive i.e. information is only received, such as a camera. Similarly, the level of autonomy may also vary from a low level such as a remotely operated robot to a high level for a fully autonomous robot such as the DARPA Grand Challenge Vehicles (Davison, 2013). It is understood that there are essentially two types of uncertainties in measurements viz systematic uncertainties which are identified by appropriate calibration, and random uncertainties which are identified by repeated measurements and are found to follow some statistical probability distribution function (Thrunet al., 2000).

Figure 1. Raw sensor data for Sonar and Laser sensors for a target located 300 cm away from the sensor (Burgard et al. 2013) The path planning problem, for both static and dynamic environments containing obstacles (Laumond, 1998; LaValle, 2006; ElShamli, 2004), is classified mathematically as a NP-hard problem which can be solved by traditional deterministic methods such as dynamic programming or random search methods such as Genetic Algorithms (GA) and Random Particle Optimization (RPO, Mohajeret al., 2013). These have traditionally been carried out with distances assumed to be known precisely even in the presence of sensors in dynamic environments (Khan, 2012) for cases such as the environment shown in Fig. 2 (Nagib and Gharieb, 2004). Such a trajectory can be followed by applying an optimal control (Burns, 2001; Kirk, 2004; Lewis et al., 2012), for example, using the linear quadratic regulator (LQR) as demonstrated for a simple case by Khan and Koreshi (2014).

Thus measurement noise is usually modelled as a normal distribution function expressed (Burgardet al., 2013) as 𝑃(𝑧|𝑥) =

1 √2𝜋𝑏

𝑒

(𝑧−𝑧𝑒𝑥𝑝 )2 2𝑏

(1)

and the presence of an unexpected obstacle in a dynamic environment is expressed as an exponential distribution 𝜆𝑒 −𝜆𝑧 𝑧 < 𝑧𝑒𝑥𝑝 𝑃𝑢𝑛𝑒𝑥𝑝𝑒𝑐 (𝑧|𝑥) = { (2) 0 𝑜𝑡ℎ𝑒𝑟𝑤𝑖𝑠𝑒 Figure 1 shows raw sensor data from both sonar and laser sensors for a target which is located 300 cm away from the sensor. The data can be expressed mathematically as random with a normal distribution with appropriate mean and variance calculated from the measurements.

Figure 2. Optimal path problem with 16 link points showing the optimal path (red) obtained by Genetic Algorithms when the distances are deterministic quantities This paper presents research work carried out for estimating the optimal path and its probability of not exceeding a specified distance when the link

distances, understood to be sensor estimates in an environment, are known as random variables with a specified distribution. Section II presents the theoretical foundations of Monte Carlo (MC) simulation with the cross-entropy method utilizing importance sampling for enhanced computational efficiency of a ‘rare-event’ formulation of a large environment. In Section III, analog, or crude, MC simulation is carried out initially to obtain the probability of the shortest, or optimal path, being less than a specified upper bound. This is followed by importance sampling to estimate the resulting computational advantage. The conclusions drawn from an improved scheme, especially the hardware implementation advantages in a FPGA based system such as Altera’s BeInMotion (Altera, 2007) are then summarized in Section IV. II.

Theoretical Foundations

We consider the problem given in Rubenstein and Kroese (2004) shown in Fig. 3 with distances specified as random variables.

In the ‘improved’ method, a weighting function W is defined with a ‘better’ pdf g and the ‘estimator’ is appropriately adjusted as follows: 𝑊(𝑥; 𝑢, 𝑣) =

𝑓(𝑥;𝑢) 𝑔(𝑥;𝑣)

(6)

5

5

𝑗=1

𝑗=1

1 1 𝑣 = exp( − ∑ 𝑥𝑗 ( − ) ) ∏ 𝑢𝑗 𝑣𝑗 𝑢𝑗 𝑣̂ 𝑡 ,𝑗 =

∑𝑁 𝑊(𝑋𝑖 ; 𝑢, 𝑉̂ 𝑡−1) 𝑋𝑖𝑗 𝑖=1 𝐼{𝑆(𝑋𝑖 )≥𝛾̂ 𝑡} (7) ̂ ∑𝑁 𝑖=1 𝐼{𝑆(𝑋 )≥𝛾̂} 𝑊(𝑋𝑖 ; 𝑢, 𝑉𝑡−1 ) 𝑖

̂ 𝑙 =

1 𝑁1

𝑡

1 ∑𝑁 ̂𝑇 𝑖=1 𝐼{𝑆(𝑋𝑖 )≥𝛾} 𝑊(𝑋𝑖 ; 𝑢, 𝑣

(8)

The objective is to use a sample as an a priori estimate to obtain an a posteriori estimate of the improved pdf with means vj instead of the initial means ujjust as in a Bayesian formulation Consider such a scenario where thenominal parameter vector u is given by the values (0.25, 0.4, 0.1, 0.3 and 0.2. It is desired to identify the probability that the minimum path is greater than 𝛾 = 2. Rubenstein and Kroese (2004) report that Crude Monte Carlo having 107 samples results in an estimate of 1.65*10-5 with an estimated √𝑉𝑎𝑟 (𝑙̂)

relative error, RE, (that is

𝑙

) of 0.165. With

8

samples increased to 10 the result obtained is 1.30*10-5 and RE 0.03.

Figure 3. Monte Carlo determination of the shortest path for the four-point problem.

Table 2.1 shows results of the CE method, withN= 1000 and ƍ = 0.1. In that simulation, it is reported that less than half a second CPU timegave the results. Table 2. 1. Evolution of the sequence

The probability distribution function (pdf) for the path lengths 𝑥 with mean values 𝑢 is given by 𝑥𝑗

1

𝑢𝑗

𝑢𝑗

𝑓(𝑥; 𝑢) = exp(− ∑5𝑗=1 ) ∏5𝑗=1

(3)

and the distance 𝑙 that the shortest path S(X) is less than some specified value 𝛾 is expressed as 𝑙 = 𝑷(𝑆(𝑋) ≥ 𝛾) = 𝑬𝐼{𝑆(𝑋)≥𝛾}

(4)

It is then possible to estimate l by the sample mean (for sample size N) 𝑁

1 ∑ 𝐼{𝑆(𝑋𝑖 )≥𝛾} 𝑁 𝑖=1

(5)

{( (γt ) ) (vt ) ̂)} t

𝒗̂𝒕

𝜸̂𝒕

0

0.250

0.400

0.100

0.300

0.200

1

0.575

0.513

0.718

0.122

0.474

0.335

2

1.032

0.873

1.057

0.120

0.550

0.436

3

1.502

1.221

1.419

0.121

0.707

0.533

4

1.917

1.681

1.803

0.132

0.638

0.523

5

2.000

1.692

1.901

0.129

0.712

0.564

III.

Monte Carlo Simulation

As a preliminary exercise, a program was written in Matlab® to determine the computational efficiency of a crude MC run. It was found that the probability was 1.4 10-5 for a simulation with 106 points and a CPU time of 2 hours on a 2.70 GHz Intel(R ) Core (TM) i7-2620M CPU @2.70 GHz processor 32-bit Operating System. The importance sampling simulation produced the results shown in Table 3.1. Rubenstein and Kroese (2004) have presented results for 6 iterations since convergence is seen at the 6th iteration when γ_6=2.000 from the 0.9 quantiles varying from a shortest path of 0.5747 in the 2nd iteration to 1.9571 in the 5th iteration and finally 2.000 in the 6th iteration. Our results are for a sample of 2000 values while Rubinstein has carried out a simulation for 1000 particles. The extra effort has been made only to observe convergence of the means v ̂j(j=1,2,3,4,5). Table 3.1. Mean values for random variable distances v ̂j by importance sampling

𝒊

𝜸𝒊

̂𝟏 𝒗

̂𝟐 𝒗

̂𝟑 𝒗

̂𝟒 𝒗

̂𝟓 𝒗

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20

2.0000

0.2500

0.4000

0.1000

0.3000

0.2000

0.5747

0.5234

0.6822

0.1091

0.4704

0.3728

1.0028

0.8529

1.0257

0.1124

0.5480

0.4127

1.4703

1.2479

1.5084

0.1242

0.6610

0.5253

1.9571

1.6827

1.8214

0.1319

0.7912

0.6650

2.0000

1.8179

2.0296

0.0865

0.5277

0.4998

2.0000

1.7607

1.9509

0.1116

0.5970

0.4651

2.0000

1.8027

2.1670

0.1296

0.5633

0.4324

2.0000

1.9330

2.0844

0.0934

0.5069

0.4841

2.0000

1.8441

1.9577

0.1088

0.4803

0.4145

2.0000

1.7675

2.0233

0.1302

0.5855

0.4918

2.0000

1.7686

1.8841

0.1091

0.6413

0.5530

2.0000

1.7182

1.8929

0.1399

0.6833

0.5047

2.0000

1.6890

1.8421

0.1339

0.8325

0.5673

2.0000

1.5109

1.8526

0.1263

0.9481

0.7524

2.0000

1.5426

1.6130

0.1195

0.9063

0.7933

2.0000

1.6896

1.8486

0.1655

0.6919

0.5383

2.0000

1.7769

1.9480

0.1365

0.6943

0.6092

2.0000

1.8082

1.9924

0.0995

0.5601

0.4498

2.0000

1.6992

1.8787

0.1313

0.6985

0.5508

With the last estimates of the converged means 𝑣̂𝑗 we obtain the result that the probability of the shortest path being less than 𝛾 = 2.000, is 1.36 10-5 in only 3.6 seconds of CPU time. Results from the final iteration for N=1000 and 6 iterations are shown in Table 3.2. The sample size in the final iteration is NF, the probability is L, the standard deviation of the estimate (L) is 𝝈, and the relative error REL ERR is 𝜎⁄𝐿, and the CPU time is T in seconds. Table 3.2 Results of the final iteration NFL𝝈REL ERR T (S)

20000 1.4068E-005 8.6972E-007 0.0658 3.4425 40000 1.2738E-005 5.9282E-007 0.0465 16.6592 60000 1.3818E-005 5.9763E-007 0.0433 40.2426 80000 1.3950E-005 5.5863E-007 0.0400 78.3370 100000 1.3787E-005 4.1782E-007 0.0303 134.4225

It is seen from Table 3.2 that with a sample size of 105, the probability has converged to 1.3787 10 -5 with an acceptably small 𝝈of 4.1782 10-7 implying a ‘good’ MC result i.e. accurate and precise in the sense of converged mean and small variance. For this last result, the relative error is 0.0303. The computer time is ~134 s which is far better than the 2 hours for crude MC. Table 3.2 further illustrates the quality of results as sample size increase from 20,000 to 100,000 yielding better relative error but at the cost of increased CPU time. The superiority of importance sampling is clearly evident. The mean optimal path is found to be 1.2715, from the above last simulation (NF =105) in this uncertain environment. IV.

Conclusions

For the case of five uncertain distances, the probability that the optimal (shortest) path would exceed a distance of 2.000 units was found to be 1.4 10-5 when crude MC was performed. This was with a sample size of 106 values for each random variable and the associated CPU time was 2 hours on an Intel 2.70 GHz processor while the importance sampling gave an estimate of 1.4068 10-5 in 3.4425 seconds with standard deviation σ=8.6927 10-7 (relative error 0.0658). This was improved, for a larger sample of 105, yielding a probability of 1.3787 10-5, σ=4.1782 10-7with a relative error 0f 0.0303. The method was demonstrated to be highly efficient and is thus applicable for real-time optimal analysis for large

path-planning problems for mobile robots with uncertainty arising out of sensor measurements. References 1.

Andrew Davison, Sensors in Robotics, Department of Computing, Imperial College, London, http://www.doc.ic.ac.uk/~ajd/Robotics/ 2. S.M.LaValle., Planning Algorithms, Cambridge University Press, 2006. 3. F. Lewis, D. Vrabie, and V. Syrmos, Optimal Control, John Wiley and Sons, Inc., 2012. 4. R.S. Burns, Advanced Control Engineering, Butterworth-Heinemann, 2001. 5. D. E. Kirk, Optimal Control Theory, Dover Publications, 2004. 6. Reuven Y. Rubenstein and Dirk P. Kroese, The Cross-Entropy Method, A Unified Approach to Combinatorial Optimization, Monte Carlo Simulation, and Machine Learning, Springer, 2004. 7. K. Mohajer, E. Kiani, E. Samiei and M. Sharifi, “A new online random particles optimization algorithm for mobile robot path planning in dynamic environments”, Mathematical Problems in Engineering, vol 2013, Article ID 491346, January 2013. 8. J.P.Laumond, Editor, Robot Motion Planning and Control, Springer-Verlag London Limited 1998. 9. Nagib, G. and Gharieb, W., 2004, Path planning for a mobile robot using genetic algorithms, Proceedings of the International Conference on Electrical, Electronic and Computer Engineering (ICEEC’04), Cairo, Egypt, pp.185-189. 10. Altera and Arrow Electronics, Inc., BeMicro SDK – BeInMotion Motor Control Design Lab, August 5, 2011, www.arrow.com/beinmotion (accessed: 21st July 2013) 11. Wolfram Burgard, CyrillStachniss, Maren Bennewitz, Giorgio Grisetti and Kai Arras, University of FreiburgIntroduction to Mobile Robots, Probabilistic Sensor Models, 2013. http://ais.informatik.unifreiburg.de/teaching/ss11/robotics/slides/0 0-intro.ppt.pdf

12. H. Khan, Optimal Path Planning of Mobile Robots using Computational Algorithms, MS Thesis, Air University, Islamabad, Pakistan, 2013. 13. H. Khan and Z. U. Koreshi, Optimal Tracking of a Mobile Robot with Path Determined by Random Particle Optimization and Genetic Algorithms, iCREATE 2014, International Conference, IEEE, National University of Sciences and Technology (NUST), April 2014. 14. ElShamli, Mobile Robots Path Planning Optimization in Static and Dynamic Environments, MSc Thesis, University of Guelph, 2004. 15. Sebastian Thrun, Wolfram Burgard, Dieter Fox, Probabilistic Robots, 1999-2000.