PROBABILITY-BASED OPTIMAL PATH PLANNING FOR TWO-WHEELED MOBILE ROBOTS

Poceedings of the ASME 215 Dynamic Systems and Contol Confeence DSCC215 Octobe 28-3, 215, Columbus, Ohio, USA DSCC215-999 PROBABILITY-BASED OPTIMAL PATH PLANNING FOR TWO-WHEELED MOBILE ROBOTS Jaeyeon Lee Depatment of Electical Engineeing Univesity of Texas at Dallas Richadson, Texas 758 Email: jaeyeon.lee@utdallas.edu Wooam Pak Depatment of Mechanical Engineeing Univesity of Texas at Dallas Richadson, Texas 758 Email: wooam.pak@utdallas.edu ABSTRACT Most dynamic systems show uncetainty in thei behavio. Theefoe, a deteministic model is not sufficient to pedict the stochastic behavio of such systems. Altenatively, a stochastic model can be used fo bette analysis and simulation. By numeically integating the stochastic diffeential equation o solving the Fokke-Planck equation, we can obtain a pobability density function of the motion of the system. Based on this pobability density function, the path-of-pobability(pop) method fo path planning has been developed and veified in simulation. Howeve, thee ae ooms fo moe impovements and its pactical implementation has not been pefomed yet. This pape concens fomulation, simulation and pactical implementation of the path-of-pobability fo two-wheeled mobile obots. In this famewok, we define a new cost function which measues the aveaged tageting eo using oot-mean-squae (RMS), and iteatively minimize it to find an optimal path with the lowest tageting eo. The poposed algoithm is implemented and tested with a two-wheeled mobile obot fo pefomance veification. tual system is inevitable. One easy way to tackle this poblem is to apply the feedback contol, and a vaiety of contol methods ae used to compensate the associated eos. Nevetheless, it has been shown that we can include the stochastic effects in analysis and simulation of dynamic systems. This pape focuses on applying the stochastic effects into a path planning poblem. Along this line, the system uncetainty and stochasticity have been studies in many ways. The stochasticity was included in the theoetical model fo bette analysis as well as contol in [1 3]. Latombe et al. [4] developed a motion planning in an envionment with eos existing in contol and sensing. Alteovitz et al. [5] poposed a new motion planning algoithm fo a flexible medical needle with the consideation of system uncetainty. Thee exist a numbe of studies on path planning methods based on the stochastic model [6 11]. These methods geneate paths fo the system by seaching the highest pobability fo the system to each a taget. In othe wods, they find a path to maximize the pobability fo the system to each a taget. In authos pevious wok [12], we fistly used a tageting eo instead of pobability to find the best configuation of obot manipulato. Inspied by the wok in [12], this pape poposes a method to find an optimal path that minimizes the expected eo of the mobile obot position to the taget. Although the optimality in path planning can be defined in vaious ways, we define it using minimum of tageting eos in this pape. 1 Intoduction In modeling dynamic systems, it is common to ignoe the system uncetainty and use the deteministic model fo motion analysis, contol and simulation. The stochasticity of systems is easily ignoed in many cases when employing the deteministic model even if thee exists the stochasticity such as model eos and distubances in actual wold. Thee is no doubt that some amount of discepancy between a theoetical model and an ac- Even though the path-of-pobability(pop) method fo path geneation has been developed, impoved and veified with compute simulation [6 11], thee has not been implementation with actual hadwae. In this pape, we solve the path planning pob- 1 Copyight 215 by ASME

lem fo two-wheeled mobile obot with the tageting eo estimation. Additionally, we apply two impovement ideas fo the classical POP method and veify the algoithm with both compute simulation and actual expeiments with irobot Ceate. This pape is oganized as follows. In Section 2, we eview the stochasticity-based modeling fo the two-wheeled mobile obot. Two impovements of the classical POP fo the path geneation ae poposed in Section 3. Multiple simulations based on the poposed method and implementation with irobot Ceate ae povided in Sections 4 and 5, espectively. Finally, conclusion is given in Section 6. 2 Stochastic Modeling As a pio step to contol a mobile obot system, we need to have a efeence path that the obot should follow in path planning. Thee have been many eseach activities to obtain bette paths accoding to a couple of diffeent puposes. One example is the maximum pobability appoach [6]. Since the pobability of the system motion stems fom the stochasticity of the system, a mathematical model fo the system with the consideation of the stochasticity should be fistly obtained. In this section, we model a two-wheeled mobile obot with the stochasticity. 1 2 ρ(x,t) t n i, j=1 2 + n i=1 x i x j ( m k=1 (h i (x,t)ρ(x,t)) x i H ik H T k j ρ(x,t) )= (2) Fokke and Planck have found this theoy fistly and the advanced vesions wee intoduced in [13 16]. The analytic solution fo the pobability density function can be obtained by solving this Fokke-Planck equation [17] in pinciple. Howeve, getting a closed fom solution is challenging and sometimes impossible. Anothe way to obtain the pobability density function ρ(x,t) fom the SDE is to numeically integate the SDE (1) multiple times and build a histogam. This histogam epesents the pobability density function. The numeical integation of the SDE can be achieved by the Eule-Mauyama method [18]. The numeical integation fo the geneal SDE (1) can be given as [6, 12] 2.1 Stochastic diffeential equation As afoementioned, actual systems always contain a cetain amount of eos such as uncetainty, noise and distubance. We can include this stochastic effect into the theoetical model equation to expess motion of the system moe accuately by intoducing a stochastic diffeential equation (SDE). The SDE is able to contain the stochastic effects when it expesses actual system s behavio. The stochastic diffeential equation inr n is witten as whee x i+1 = x i + h(x i,t i ) t i + H(x i,t i ) W i, (3) x i = x(t i ), t i = t i+1 t i, W i = W(t i+1 ) W(t i ). dx(t) = h(x(t), t)dt + H(x(t), t)dw(t) (1) whee x R n and W R m. We can obtain this expession by petubing the deteministic system dx/dt = h(x, t) by noise o andom foce at evey time. The andom foce is denoted by W(t) that is a vecto of uncoelated Wiene pocesses with unit stength fo each dimension and a matix H R n m scales and couples these noises. The system vaiable x(t) can be obtained by integating the equation (1). Howeve, the integation esults will not be consistent evey time because of the stochastic tem. We can handle this issue by employing a pobability density function ρ(x, t) of x(t). One of the methods to obtain the pobability density function fom the SDE is to use the Fokke-Planck equation. This special patial diffeential equation govens the time evolution of the pobability density function. Fo the SDE in (1), the Fokke- Planck equation is witten as The simple update pocedue epesented by these equations is the Eule-Mauyama method. Since W(t) is the Wiene pocess, we can conside that W i is sampled fom a Gaussian distibution with zeo mean and vaiance t i, which means W i t i N(,1). 2.2 Stochastic model fo two-wheeled mobile obot Figue 1(a) shows schematic epesentation of a twowheeled mobile obot. The vaiables (x, y) and θ denote the position and the oientation of the obot, espectively. The distance between two wheels measued along the axis is L, the adius of the wheel is, the otation angle of the ight wheel otates is φ 1, and the otation angle of the left wheel otates is φ 2. Both angles ae measued counteclockwise viewed along the axis fom outside to the inside of the mobile obot. This model can be applied to most two-wheeled mobile obots including irobot Ceate. Accoding to the wok in [7], we can assume no-lateal-slip 2 Copyight 215 by ASME

stochastic effects ae applied to the angula velocities of the two wheels. (a) 3 Path Geneation with Stochasticity In this section, we define the tageting eo fo a system to each a taget and show its use fo the path planning of stochastic systems. The poposed method is applied to the two-wheeled mobile obot and then, finally, we will discuss potentials of the POP algoithm to be impoved fo bette pefomance. (b) FIGURE 1. (a) The kinematic model of the two-wheeled mobile obot [7] (b) Two-wheeled mobile obot (irobot Ceate) condition fo the infinitesimal motions of the two-wheeled mobile obot. The infinitesimal motions can be witten as dx 2 dy = cosθ 2 cosθ 2 sin θ 2 sinθ dθ L L ( dφ1 dφ 2 ). (4) To include the stochastic effects, we assume that the infinitesimal changes in wheel otation angles can be the fom of dφ i = ω i dt+ σdw i (5) whee dw i is the incement of unit stength Wiene pocesses, σ is the stength of the Wiene pocess. Let us define 2δ as the diffeence between the angula velocities of the two wheels such that ω 1 = ω + δ and ω 2 = ω δ. By adopting the stochastic behavio of the system, the infinitesimal motion in (4) can be ewitten as dx ω cosθ dy = ω sinθ dt dθ γ 2 +σ cosθ 2 cosθ 2 sinθ 2 sinθ L L ) ( dw1 dw 2 whee γ is 2δ/L. This equation (6) is able to expess the motion of the two-wheeled mobile obot with an assumption that the (6) 3.1 Path geneation with a new cost function To geneate a path fo a stochastic system, we can use the POP method. This method uses the pobability density function to decide the locally optimal intemediate paths. In the pevious implementations [6 11], the maximum pobability was pusued duing the path geneation. Theefoe, thei eseach focused on finding an optimal path with the maximum pobability to each a goal. Howeve accoding to [12], the tageting eo instead of tageting pobability is also a meaningful measue to detemine if the planning is successful. The tageting eo of stochastic systems can be evaluated using the pobability density function. We use this new measue as a cost function fo the POP method. Suppose that a andom vaiable X is dawn fom a pobability density function (PDF) ρ(x). The oot-mean-squae (RMS) distance is computed as L= 1 N N i (x i x d ) 2 σ 2 +(µ x d ) 2 whee x i is the i th sampled value, N is the numbe of samples, x d is the taget, and µ and σ 2 ae the mean and the vaiance of the PDF, espectively. We can modify this fomulation to ou poblem. Since the mobile obot exploes on 2D space and we ae focusing on the minimization of the cost, a new cost can be defined as L (µ x, µ y,σ x,σ y )=σ 2 x + σ 2 y +(µ x x d ) 2 +(µ y y d ) 2. Theefoe, the optimal path in this pape can be defined as a path with the minimum tageting RMS eo to each a goal. 3.2 Path geneation fo two-wheeled mobile obot This statistical appoach can be applied to the two-wheeled mobile obot to geneate the optimal path as follows. We fistly assume a simple input angula velocity ω = 1 with δ =. The obot will follow the intemediate path defined by angula velocity ω = 1 and δ = in a deteministic way duing a small 3 Copyight 215 by ASME

FIGURE 2. segments Pobability density functions with diffeent numbe of time peiod t. Then, in the sampling stage, we geneate multiple andom path samples fom the cuent intemediate state to a taget by applying stochasticity to obtain a pobability density function. We can calculate the cost that the futue path eaches the taget using this pobability density function. Similaly, we can test with othe inputs(ω,δ)=(1,±α) and(ω,δ)=(1,±β) to evaluate the cost function. Afte obtaining all five costs using five candidates fo δ, we compae the costs to find which intemediate path is the best one fo the next intemediate path in tems of the minimum tageting eo. One small path with which the futue path has the lowest tageting eo will be selected. The algoithm uns until a pe-defined total numbe of path segments ae obtained. Eventually, we can have an optimal path fom initial position to a goal location by stitching the chosen intemediate paths. In the simulation and implementation of this pape we suggest the following options fo each intemediate path s candidates. δ = α o β o o β o α. (7) The poposed algoithm will choose one optimal δ among candidates ( α, β,,β,α) by seaching the minimum tageting eo to each a goal. 3.3 Impoved POP method Accoding to the POP method, we choose the total numbe of path segments based on the distance between the initial position and the taget position befoe applying the POP method because the algoithm uns until the fixed numbe of intemediate paths ae obtained. Theefoe, we should caefully decide the numbe of intemediate paths to get a meaningful full path eaching a taget accuately. Othewise, the full path will not each the taget when the total length of the intemediate paths is too shot o the full path may pass ove the taget when the total length is too long. To ovecome this poblem, we geneate the andom path samples with shote and longe paths in the sampling stage. Fo example, we geneate the andom path samples with 9 and 11 segments as well as 1 segments in the case that we pe-defined total 1 path segments fo the POP method. Due to these two additional sampling sets (with 9 and 11 segments), the algoithm can estimate whethe the emained path segments is pope, shot, o long. Theefoe, the algoithm can actively adjust the length of segments. Figue 2 shows the pobability density function with thee diffeent numbes of the path segments when a system is moving fom (,) to the ight diection. The left PDF is geneated by total 9 segments, the middle PDF is geneated by total 1 segments, and the ight PDF is geneated by total 11 segments of the intemediate paths. Moeove, thee is anothe oom fo impovement of the POP algoithm. The idea is that we can iteatively update the optimal path. We geneate multiple candidates of the optimal full paths by epeatedly applying the POP algoithm and evaluate them using the RMS tageting eos. To do this, afte obtaining the fist optimal path (afte the 1st iteation is done), we apply the same algoithm again using the path that we obtained in the pevious iteation. We epeat this iteation until the esult conveges to cetain bounday. 4 Numeical Simulation In this section, we veify the poposed algoithm with compute simulation. Compaed to the classical POP algoithm, the simulation with the poposed method shows two impovements as mentioned in the pevious section. Fo each simulation case, we test the algoithm to geneate the optimal path with the lowest tageting eo with and without obstacles. The two-wheeled mobile obot stats to move at (,) fo evey simulation and all simulation paametes we used ae identical to the actual paametes of the irobot Ceate fo the pupose of consistency between simulation and actual implementation that will be given in the next section. 4.1 Simulation I : Active segments Figue 3 shows simulation esults of the poposed algoithm with and without obstacles. Fo the fist simulation without an obstacle in Figue 3(a), the taget point is at (38, 9). The POP algoithm is applied and the optimized path in tems of RMS eos to hit the taget is geneated. A full path is composed of 13 segments even though default numbe of total segments is set by 1. The numbe of segments is actively changed depending on the position of the goal duing the simulation. Fo each step to get the intemediate path, the algoithm evaluates five candidates of δ and selects one that bings the lowest RMS eo fo the obot to hit the taget. The empiical paametes ae selected as σ =.7 and 4 Copyight 215 by ASME

1 1 1st (E:28.39mm) 2nd (E:24.96mm) 3d (E:21.45mm) 4th (E:21.45mm) 5th (E:18.12mm) 6th (E:15.8mm) 7th (E:12.19mm) 8th (E:12.19mm) 1 1 1 2 2 3 3 4 1 (a) 1 1 1 2 2 3 3 4 1 (a) 1st (E:56.48mm) 2nd (E:44.98mm) 3d (E:37.53mm) 4d (E:22.93mm) 1 1 1 2 2 3 3 4 (b) FIGURE 3. Path geneated by the POP method. (a) Path to taget a point (38,9) without obstacles. (b) Path to taget a point (385,- 3) with obstacles. The cicula obstacles ae placed at (12,2) and (285,-35) and thei adii ae 15mm. 1 1 1 2 2 3 3 4 (b) FIGURE 4. Path geneated by the iteation method. (a) Path to taget a point (38,7) without obstacles. (b) Path to taget a point (38,- 25) with obstacles. The cicula obstacles ae placed at (12,2) and (285,-35) and thei adii ae 15mm. δ =[.1,.5,.,.5,.1]. Figue 3(b) shows the second simulation esult with two obstacles. The taget point is at (385, 3) and two obstacles ae at (12, 2) and (285, 35). The stategy to avoid obstacles is to exclude the paths that ae ovelapped with the obstacles when geneating multiple andom path samples. Theefoe, the path geneated by this stategy intinsically avoids the obstacle. 4.2 Simulation II : Iteative method As afoementioned, we can update the path though the iteative application of the POP method. In this iteative appoach, the multiple andom path samples in the sampling stage ae geneated based on the pevious optimal path. We geneate a set of paths though the iteative application of the POP method until one of thee conditions is satisfied. The conditions ae : The cuent path segments ae moe than the past path segments. FIGURE 5. Test envionment. (2mm 4mm) The cuent tageting eo is highe than the past tageting eo. The numbe of iteation exceeds 5. Figue 4 shows the esult of the new path geneation with the iteative appoach. Figue 4(a) shows the optimal path geneation 5 Copyight 215 by ASME

1 1 Y 1 1 1 2 2 3 3 4 FIGURE 6. Implementation of the algoithm with irobot Ceate (no obstacle). The same paametes fo the taget and obstacles as in Fig. 3(a) ae used. 1 1 1 2 2 3 3 4 X FIGURE 8. Implementation of the algoithm with irobot Ceate (no obstacle) with iteation appoach. The same paametes fo the taget and obstacles as in Fig. 4(a) ae used. 1 1 Y 1 1 1 2 2 3 3 4 FIGURE 7. Implementation of the algoithm with irobot Ceate (obstacles). The same paametes fo the taget and obstacles as in Fig. 3(b) ae used. 1 1 1 2 2 3 3 4 X FIGURE 9. Implementation of the algoithm with irobot Ceate (obstacles). The same paametes fo the taget and obstacles as in Fig. 4(b) ae used. without obstacles. The optimal path fom the fist iteation is denoted by the blue staight line and the final iteation esult is denoted by the ed dotted line. The black dotted line denotes the paths obtained duing the iteation. Figue 4(b) shows the optimal path geneation esult with iteation with two cicula obstacles. With this iteation, the tageting eo deceases. 5 Implementation and Expeiments We implement the poposed algoithm with irobot Ceate based on the simulation esults to veify the algoithm with an actual hadwae system. The irobot Ceate is a cicula twowheeled mobile obot with appoximately 169mm adius, 88mm height and 2.9kg weight. Two wheels ae 3mm in adius and the distance between the two wheels is L=262mm. Figue 5 shows the test envionment with the irobot Ceate. The test aea epesented by the blue ectangle is the size of 4mm 2mm. The motion of the obot is captued by a camea. The white cicles epesent the obstacles. The obot moves and otates accoding to the esult of the poposed path planning algoithm fo each intemediate step. Diffeent fom the simulation, each intemediate position and oientation ae updated by the irobot s encode which means the actual values with noise ae employed. Figue 6 shows ou fist implementation test. This esult shows that the obot follows the optimal path with the lowest tageting eo to each the taget without obstacles. Note that this is an implementation of the fist simulation esult shown in Figue 3(a). The final tageting eo is measued as 51mm. Figue 7 shows an implementation test with two obstacles. The obot follows the optimal path with the lowest tageting eo with obstacles. Similaly, this is an implementation of the second simulation esult in Figue 3(b). The tageting eo is measued as 72mm. Additionally, we implement the iteative application. We make the obot follow the optimal path afte the iteation pocess is finished. In this pocess, the multiple andom path samples in the sampling stage ae geneated by the final optimal path that we obtained in the iteation pocess. Even though this implementation stategy doesn t mean that the obot always eaches to the taget with the minimum tageting eo, we can expect the 6 Copyight 215 by ASME

highe possibility to each the goal with the minimum RMS eo to hit the taget than the implementation without the iteative appoach. Figue 8 shows the implementation esult that eflects the simulation esult in Figue 4(a). The actual eo is slightly deceased to 45mm compaed to the implementation without iteation. The last esult shown in Figue 9 is implementation of the esult in Figue 4(b). The final tageting eo is measued as 65mm. Note that the eo in implementation is slightly lage than the simulation esult because we used actual intemediate position and otation values obtained by the intenal encode to calculate next intemediate path. In othe wods, we consideed the actual uncetainty fom the envionment to veify the poposed method in the actual implementation. Accoding to the implementations, we can guaantee the poposed algoithm can geneate the optimal path with espect to the lowest tageting eo in an envionment with and without obstacles. Additionally, we can veify that the impoved method with iteative appoach can possibly decease the tageting eo. Consequently, the poposed method shows the pefomance to makes an actual two wheeled mobile obot follow the optimal path with effective obstacle avoidance. 6 Conclusion In this pape, we impoved a path planning method to geneate optimal paths with the consideation of system stochasticity. Then, we applied it to an actual mobile obot platfom. The impoved algoithm geneates paths that minimize the tageting eo. To do this, we defined a new cost function by applying the oot-mean-squae eo to hit the taget, and adapt it in the path-of-pobability (POP) method. We also impoved the POP method by actively adjusting the numbe of intemediate paths so that the numbe of intemediate paths is automatically adjusted duing the optimal path geneation pocess. In addition, we modified the algoithm to opeate iteatively. This iteative appoach is justified by the fact that the POP algoithm finds only locally optimal answes with the fist iteation. We veified the pefomance of the path planning algoithm with simulation and actual implementation. We finally applied this theoetic appoach to the irobot Ceate and confimed that the algoithm woks successfully in path planning fo the two-wheeled mobile obot. Fo the futue, we will investigate othe types of citeia fo geneating optimal paths such as shotest paths and minimum tavel time. REFERENCES [1] White, P., Zykov, V., Bongad, J. C., and Lipson, H., 25. Thee dimensional stochastic econfiguation of modula obots.. In Robotics: Science and Systems, Cambidge, pp. 161 168. [2] Shah, S. K., Pahlajani, C. D., and Tanne, H. G., 211. Pobability of success in stochastic obot navigation with state feedback. In Intelligent Robots and Systems (IROS), 211 IEEE/RSJ Intenational Confeence on, IEEE, pp. 3911 3916. [3] Åstöm, K. J., 212. Intoduction to stochastic contol theoy. Couie Copoation. [4] Latombe, J.-C., Lazanas, A., and Shekha, S., 1991. Robot motion planning with uncetainty in contol and sensing. Atificial Intelligence, 52(1), pp. 1 47. [5] Alteovitz, R., Banicky, M., and Goldbeg, K., 28. Motion planning unde uncetainty fo image-guided medical needle steeing. The Intenational jounal of obotics eseach, 27(11-12), pp. 1361 1374. [6] Pak, W., Kim, J. S., Zhou, Y., Cowan, N. J., Okamua, A. M., and Chiikjian, G. S., 25. Diffusion-based motion planning fo a nonholonomic flexible needle model. In Robotics and Automation, 25. ICRA 25. Poceedings of the 25 IEEE Intenational Confeence on, IEEE, pp. 46 465. [7] Pak, W., Liu, Y., Zhou, Y., Moses, M., and Chiikjian, G. S., 28. Kinematic state estimation and motion planning fo stochastic nonholonomic systems using the exponential map. Robotica, 26(4), pp. 419 434. [8] Pak, W., Wang, Y., and Chiikjian, G. S., 29. The pathof-pobability algoithm fo steeing and feedback contol of flexible needles. The Intenational jounal of obotics eseach. [9] Lee, J., and Pak, W., 214. A pobability-based path planning method using fuzzy logic. In Intelligent Robots and Systems (IROS 214), 214 IEEE/RSJ Intenational Confeence on, IEEE, pp. 2978 2984. [1] Mason, R., and Budick, J. W., 22. Tajectoy planning using eachable-state density functions. In Robotics and Automation, 22. Poceedings. ICRA 2. IEEE Intenational Confeence on, Vol. 1, IEEE, pp. 273 28. [11] Lee, J., and Pak, W., 213. Design and path planning fo a spheical olling obot. In ASME 213 Intenational Mechanical Engineeing Congess and Exposition, Ameican Society of Mechanical Enginees, pp. V4AT4A28 V4AT4A28. [12] Lee, J., and Pak, W., 214. Motion planning fo a edundant obot manipulato with stochasticity in joints. In ASME 214 Intenational Design Engineeing Technical Confeences and Computes and Infomation in Engineeing Confeence, Ameican Society of Mechanical Enginees, pp. V5BT8A65 V5BT8A65. [13] Gadine, C. W., 1985. Handbook of stochastic methods, Vol. 3. Spinge Belin. [14] McConnell, J. R., and McConnell, J. R., 198. Rotational Bownian motion and dielectic theoy. Academic Pess London. [15] Risken, H., 1985. The Fokke-Planck equation: methods of solution and applications. Spinge Velag. [16] Van Kampen, N. G., 1992. Stochastic pocesses in physics 7 Copyight 215 by ASME

and chemisty, Vol. 1. Noth holland. [17] Chiikjian, G. S., 29. Stochastic diffeential equations. In Stochastic Models, Infomation Theoy, and Lie Goups, Volume 1. Spinge, pp. 11 139. [18] Higham, D. J., 21. An algoithmic intoduction to numeical simulation of stochastic diffeential equations. SIAM eview, 43(3), pp. 525 546. 8 Copyight 215 by ASME