Smooth Trajectory Planning Along Bezier Curve for Mobile Robots with Velocity Constraints

Transcription

1 Smooth Trajetory Planning Along Bezier Curve for Mobile Robots with Veloity Constraints Gil Jin Yang and Byoung Wook Choi Department of Eletrial and Information Engineering Seoul National University of Siene and Tehnology, Seoul, South Korea Abstrat This paper presents a smooth path planning method onsidering physial limits for twowheeled mobile robots (TMRs). A Bezier urve is utilized to make an S-urve path. A onvolution operator is used to generate the enter veloity trajetory to travel the distane of the planned path while onsidering the physial limits. The trajetory gained through onvolution does not onsider the diretion angle of the TMR, so a transformational method for a enter veloity trajetory following the planned path as a funtion of time of parameter for the Bezier urve is presented. Finally, the joint spae veloity is omputed to drive the TMR from the enter veloity. The effetiveness of the proposed method was performed through numerial simulations. This algorithm an be used for path planning to optimize travel time and energy onsumption. Keywords: TMR, Bezier urve, Smooth path planning, Convolution, Physial Limits 1 Introdution Two-wheeled mobile robots (TMRs) are reently beoming widely used as leaning robots and intelligent servie robots; thus, extensive researh is underway on trajetory planning to minimize energy and optimize traveling time as well as to resolve issues regarding smooth traveling toward the desired destinations in workspaes [1-4]. A navigation system for a TMR largely onsists of a path planner, a trajetory generator and a traking ontroller. Path planning is about generating smooth paths while maintaining the desired position in workspaes. The trajetory generator aims to generate a veloity profile for the planned paths as a funtion of time. The traking ontroller and driving ontroller are ontrol systems that allow a TMR to travel along the predefined trajetory at a desired time while staying within its physial limits. If the physial limits of a TMR during path planning and trajetory generation are onsidered, potential damage to a TMR an be redued; trajetory traking auray and traking veloity an be improved []. To this end, veloity trajetory planning methods using a onvolution operator have been suggested that onsider the physial limits of a TMR in workspaes [2, 5]. However, the suggested method did not onsider the diretion angle, whih is part of a TMR s kinematis. The method only onsidered the translational veloity and path as opposed to the enter of a TMR in Cartesian oordinates. A smooth path planning method that onsiders the initial and final diretion angles is the basi goal in path planning for a TMR. Appliation to an atual TMR is also diffiult sine only the translational veloity limits at the enter point in Cartesian oordinates is onsidered, and not the physial limitations of Corresponding author 225

2 atuators for driving the two wheels, whih are dependent on the variations in angular veloity. When generating a trajetory for a TMR, modifying the diretions of a TMR after it stops during operation has been used beause a disontinuity point may ause a slip or path deviation. In order to overome this issue, researh on path planning with ontinuous urvature for a TMR with kinemati limits has been onduted. Path planning methods have been studied for a TMR arriving in a desired position based on a starting position and diretion angle using a Bezier urve [6]. In this study, a path based on a Bezier urve was generated in order to build a smooth path while onsidering the diretion angle. A onvolution operator was used to generate the entral veloity to travel the planned path. In this proess, the veloity trajetory an be generated while onsidering the maximum veloity and aeleration aording to the physial limits of a TMR. The veloity trajetory gained through onvolution is a trajetory whih a robot travels suh that the given distane does not onsider the diretion angle of the TMR. In order to onsider the diretion angle of the TMR, a transformation method for the trajetory is presented that onsists of segmented paths along the designed Bezier urve with the entral veloity generated through onvolution. The trajetory obtained through the transformation proess an be used for the TMR to smoothly follow the planned path while staying within the physial limits. Finally, a trajetory generation method in joint spae that an be used as an atuator ommand for the TMR driving is proposed. The joint spae trajetory limits the driving veloity profile along the two wheels in order to onsider the atuator s physial limitations that depend on the diretion angle of the entral veloity. In order to determine the effetiveness of the proposed method, numerial simulations were performed. The appliation of the planned trajetory to a simulator showed that the robot arried out desired tasks well while staying within its physial limits. This trajetory an be used for path planning to optimize time and energy onsumption. 2. Bezier Curve based Path Planning As shown in Figure 1, a TMR is represented in the oordinate system using the robot s entral position and diretion angle. The position onsists of a world frame oordinate system and robot frame oordinate system. A TMR s position P is defined on the oordinate system as follows: T P x,y, θ, where x, y, θ denote a robot s position and diretion angle respetively. TMR s kinemati model an be obtained as follows: r r osθ osθ 2 2 r r ωr P sinθ sinθ, (2) 2 2 ωl r r D D where r denotes the radius of a robot s wheels, D denotes the distane between its two wheels, ω r denotes the right wheel s angular veloity, and ω l denotes the left wheel s angular veloity. (1) 226

3 y v l v ω θ y v r 0 Figure 1. Kinematis of TMR When planning a path for TMR s, the position and diretion angle at its starting point and destination should be onsidered and a urved trajetory is ommonly generated using Bezier urves [6]. As shown in Figure 2, a trajetory is generated using a Bezier urve onsisting of an initial point P i (A 0, B 0 ), end point P f (A, B ), and ontrol points C 1 (A 1, B 1 ) and C 2 (A 2, B 2 ). An equation for the Bezier urve is alulated using C 1 and C 2. The equation of Bezier urve is given below in equation (). x x y P f d 2 θ f C 2 C 1 d 1 P i θ i x ( u) y ( u) Figure 2. Bezier Curve-based path planning A J ( u) i n, i i0 2 2 A0 1u) Au 1 ( 1u) A2u ( 1u) ( A u B J ( u) i n, i i0 2 2 B0 1u) B1u ( 1u) B2u ( 1u) ( B u x,, (-a) (-b) In equation (), u is an arbitrary value where 0 u 1 and an be used to generate a smooth urve from a starting point to a target point: a more preise Bezier urve with a smaller inrease. The path given by equation () does not onsider veloity and is only parameterized by u. 227

4 . Convolution based Trajetory Planning Following Bezier Curve There has been researh that the path generation method may use a onvolution operator to reate a entral veloity trajetory of a TMR for smooth path generation while satisfying physial limits [2, ]. In order to use onvolution, a square-wave funtion y 0 (t) is defined as follows: v0, 0 t t0 y0 ( t), (4) 0, otherwise where the nth-applying onvolution funtion h n (t) is defined as a square-wave funtion with the unit area in 0 t t n as follows: 1 tn, 0 t tn hn( t ). (5) 0, otherwise If funtion y n (t) is a resulting funtion to whih the nth onvolution is applied, the result of onvolution y 0 (t) and h 1 (t) an be represented as y 1 (t) and y 2 (t) denotes the result of y 1 (t) and h 2 (t) onvolution. The veloity funtion v (t) generates the veloity ommand of the differentiable S-urve that onsiders the maximum veloity v max for the robot to travel the distane S, as shown in Figure. v (t) v max v f S v i t 0 t0 +t 1 +t 2 Figure. Convolution-based veloity ommand trajetory Let a Bezier-urve-based path as shown in Figure 2 that onsiders the diretion angle using a onstant value u be ρ(u). The distane traveled is alulated using formula (6) to generate the entral veloity trajetory for the robot to travel along the distane S, as shown in Figure. The urved distane B d along the path ρ(u) from P i to P f as in Figure 2, is alulated as follows: 1 B Δρ( u) ( x( u Δu) x( u)) 2 ( y( u Δu) y( u)) 2 (6) d u 0 1 u 0 The alulated distane B d is the atual distane traveled along the path designed with Bezier urve whih has a smooth urve. To generate the enter veloity trajetory of a TMR using onvolution, the distane S is thus used as an input value. Therefore, if the enter veloity trajetory v (t) is generated to have the traveling distane as S = B d, then the trajetory using the advantages of onvolution while onsidering veloity limits an make a smooth path. Here, v i, v f, v max and the sampling time an be arbitrarily set aording to the speifiations of the TMR [2-]. The generated entral veloity trajetory of v (t), as shown in Figure, travels along the 228

5 distane S. However, the entral veloity trajetory of TMR does not onsider the diretion of the robot. In other words, for any position (x(u i ), y(u i )), the robot travels with veloity v (t i ), as shown in Figure 2. In equation (2), the subsequent position an be moved to an entirely different position depending on the angle θ i. In order to onsider the positions in task spae that depend on veloities in paths with diretion angles, the parameter u(t) of Bezier urve for the distane during the sampling time should be determined and alulated using equation (7). The trajetory ρ(u(t)) with the diretion angle an be obtained by inputting the determined u(t) into equation (). In ρ(u(t)), if the sampling time is shorter, the path an more aurately follow ρ(u) as generated by onstant parameter value u. u( t) t0t1 t 2 v t0 ( t) B d (7) Here, u(t) is defined as 0 u(t) 1 and represents the parameter of the Bezier urve that depends on the entral veloity. The trajetory generated by using u(t) satisfies the maximum veloity allowed by the physial limits of a TMR while following the urved path with respet to the diretion angles. 4. Simulation Results Figure 4 illustrates the entral veloity trajetory that satisfies the physial limits from the starting point P i,, ) to the target point P f,, and a Bezier urve trajetory traking it. In this figure, the distane between positions of the trajetory is the distane driven by the entral veloity funtion during sampling time. The results show that the synthesized Bezier urved trajetory was generated depending on the entral veloity funtion s trajetory. Diretion angles θ at eah setion and the angular veloity ω at the enter were alulated as follows and are illustrated in Figure 4. 1 y( Δu( t)) Δθ tan, (8) x( Δu( t)) Δ (9) Δt Figure 4. Smooth trajetory, diretion angle and angular veloity onsidering maximum veloity limits 229

6 The trajetory for the TMR was generated as shown in Figure 4. The generated trajetory satisfies the physial limits desribed above and allows the TMR to travel along the urved path using its entral veloity. The atual ommand for atuating the TMR is the angular veloity for both wheels. It an generate wheel veloity ommands in joint spae using equations (10) and (11): 1 ωr ( v D/ 2ω ) r, 1 ωl ( v D/ 2ω ) r vr rωr. v rω l l The veloity ommand trajetory for two wheels obtained by formula (11) beomes the atual veloity ommand for the TMR, and the robot s angular veloity is represented by the differene between the two wheels translational veloities as shown in equation (12): (10) (11) v - v ω r l D. (12) Figure 5 shows that when physial limits are v max =0.5m/s, a max =0.2m/s 2 and j max =0.2m/s, then the entral veloity trajetory satisfies the physial limits moving from the starting position,, to the target position,,. When the veloity ommands for the two wheels are generated, angular veloity shown in Figure 4 is used. The joint veloity ommands for two wheels is used to drive two wheels to follow the Bezier urve based trajetory Figure 5. Veloity ommands of two-wheels following smooth trajetory Figure 6 shows the TMR s trajetory in task spae obtained through the drive by the veloity ommands for two wheels as shown in Figure 5. The results show that the robot suessfully followed the Bezier urve along the planned path. Figure 7 shows the simulation results driven by atuator ommands on the anykode, Marilou Robotis Studio [10]. 20

7 Figure 6. S-Curve and Trajetory of TMR Figure 7. Trae of TMR driven by Atuator Veloity Commands Figure 8 shows the traking error between the Bezier urve and the trajetory generated aording to sampling time of 1ms, 50ms, 10ms in equation (7). The error inreases as angular veloity and sampling time inreases. The effet of sampling time should be onsidered to ontrol mobile robot. Figure 8. Error Gap While Travelling Along an S-Curve 21

8 The previous urve is S-urve so the traking error ould be ompensated in whole path. We applied the proposed algorithm to another path whih has C-urve. Another path is shown as a C-urve in Figure 9. The figure shows that when physial limits are v max =0.5m/s, a max =0.2m/s 2 and j max =0.2m/s, the entral veloity trajetory satisfies the physial limits moving from the starting position,, to the target position,,. Figure 10 shows the veloity ommands of the two wheels following the path shown in Figure 9. In C-urve path, heading angle of TMR is hanging monotonially. In this ase, Figure 11 shows trajetory of TMR driven by the joint veloity ommands desribed in Figure 10, whih follows C-urve. Compared to the S-urve shown in Figure 6, the error is greater in the C-urve. Figure 9. Smooth trajetory, diretion angle and angular veloity onsidering maximum veloity limits Figure 10. Veloity ommands of two-wheels following smooth trajetory 22

9 Figure 11. Trajetories of TMR and trae of TMR driven by atuator veloity ommands Figure 12. Error Gap While Travelling Along the C-Curve Figure 12 shows the error of the path between the ideal parametri Bezier urve by using equation () and the generated path onsidering physial limits with various sampling time by using equation (7). The error was omputed by finding the differene between the ideal smooth trajetory (as shown by the solid-line graph in Figure 11) and the trajetory of the Bezier urve (as shown by the dotted-line graph in Figure 11). The traking error resulted from sampling time is also shown Cartesian trajetory simulated using the joint veloity ommands. Compared to Figure 8, the errors in x oordinates beome larger as traking C- urve path. The shape of the error gap is different in depending on the target path shape. Therefore, the path s shape should be onsidered. If the sampling time is shorter, error is dereased. 5. Conlusions A veloity ommand trajetory generation method was proposed that enables a TMR to travel smoothly along a urved path while staying within the atuator s physial limits for smooth run and ontrol. 2

10 The proposed veloity trajetory generation method generates a trajetory to satisfy the maximum veloity as opposed to a entral veloity of TMR using the harateristis of onvolution, and the entral veloity trajetory follows the Bezier urve based path to travel smoothly. In the future, this trajetory generation method an be applied to obstale avoidane algorithms that satisfy veloity limits at the any points. Researh on ontinuous path generation at the any points to satisfy physial limits is urrently underway. Traking errors aording to the sampling time during onvolution and transformation proess was examined. For smooth ontrol, the effet of sampling time should be onsidered. If the veloity trajetory performs at a real-time operating system, then traking error an be redued. The performane evaluations of real-time mehanisms an predit the traking error aording to the system, performane evaluations of real-time mehanisms [7]. It an also analyze traking error and apply the results to the ontroller so that traking error an be redued [8-9]. The path planning method proposed in this paper an be utilized for a path planning with optimized travelling time and an energy-effiient path planning that onsiders the limited battery power of a running robot. Aknowledgements This researh was supported by Basi Siene Researh Program through the National Researh Foundation of Korea (NRF) funded by the Ministry of Eduation, Siene and Tehnology (No ). Referenes [1] J. J. Craig, Introdution to Robotis, Prentie-Hall, (2005). [2] G. Lee, B. J. Yi, D. I. Kim and Y. J. Choi, New Roboti Motion Generation using Digital Convolution with Physial System Limitation, IEEE Conferene on Deision and Control and European Control Conferene, (2011), pp [] G. Lee, D. I. Kim and Y. J. Choi, Faster and Smoother Trajetory Generation onsidering Physial System Limits under Disontinuously Assigned Target Angles, IEEE International Conferene on Mehatronis and Automation, (2012), pp [4] M. S. Jang, E. H. Lee and S. B. Choi, A Study on Human Robot Interation Tehnology Using a Cirular Coordinate System for the Remote Control of the Mobile Robot, International Journal of Control and Automation, vol. 5, (2012), pp [5] M. Lepeti, G. Klanar, I. Skrjan, D. Matko and B. Potonik, Time optimal path planning onsidering aeleration limits, Robotis and Automation Systems, vol. 45, (200), pp [6] K. G. Jolly, R. S. Kumara and R. Vijayakumara, A Bezier urve based path planning in a multi-agent robot soer system without violating the aeleration limits, Robotis and Automation Systems, vol. 57, (2009), pp. 2-. [7] J. H. Koh and B. W. Choi, Real-time Performane of Real-time Mehanisms for RTAI and Xenomai in Various Running Conditions, International Journal of Control and Automation, vol. 6, (201), pp [8] S. Sheel and O. Gupta, High Performane Fuzzy Adaptive PID Speed Control of a Converter Driven DC Motor, International Journal of Control and Automation, vol. 5, (2012), pp [9] A. Abdelkrim, C. Ghorbel and M. Benrejeb, LMI-based traking ontrol for Takagi-Sugeno fuzzy model, International Journal of Control and Automation, vol., (2010), pp [10] AnyKode, Marilou Robotis Studio, 24