Near-Time-Optimal Trajectory Planning for Wheeled Mobile Robots with Translational and Rotational Sections

Transcription

1 IEEE RANSACIONS ON ROBOICS AND AUOMAION, OL. 17, NO. 1, FEBRUARY Near-ime-Optimal rajectory Planning for Wheeled Mobile Robots with ranslational and Rotational Sections Jong-Sk Choi and Byng Kook Kim Abstract We derive a near-time-optimal trajectory for wheeled mobile robots (WMRs) satisfying the following: 1) initial and final postres/velocities as well as 2) battery voltage and armatre crrent constraints, nder assmptions of simplified dynamics and constant translational/rotational velocity sections. We se a simplified dynamic model for WMRs neglecting indctances of motor armatres and divide or trajectory generation algorithm for cornering motion into three sections. We specify a path-deviation reqirement for obstacle avoidance. ransforming dynamics into ncorrelated form with regard to translational and rotational velocities, we make extreme control possible. By splining rotational section with translational sections and determining the velocity scale factor, a near-time-optimal trajectory can be obtained. Simlation reslts along with inverse control of path-following are given to validate the generated trajectory. Index erms Crrent and voltage constraints, mobile robot, time-optimal trajectory. I. INRODUCION Control of WMRs is generally divided into the following three categories: 1) path planning (PP); 2) trajectory generation (G); and 3) trajectory tracking/following (/F). PP problem of characterizing the shortest path for a particle with a constant linear velocity was set by Dbins [1] and Reeds and Shepp [2]. A shortest path synthesis of Dbins car was determined according to Pontryagin s maximm principle by Soeres et al. [3]. Bicchi et al. [4] extended the Reeds and Shepp reslts to the case obstacles are present. However, in those researches based on the Dbins model, the crvatre along the path does not vary continosly. As for continos crvatre, clothoid was sed as splines in compter-aided design [5] and introdced in robotics by Kanayama et al. [6]. For, Kanayama et al. [7] proposed critically damped controller. In Soeres research [8], obstacle avoidance is also inclded dring transition phase sing sliding mode techniqe. ime-optimal direct G (inclding PP) has been stdied by several researchers bt remains an open problem yet. It was initially addressed by Jacobs et al. [9] in which minimm-time trajectories based on Hilare-like model are necessarily made p with bang-bang pieces. Reister [10] made a nmerical stdy of bang-bang trajectories containing only five elementary pieces. However, it was invalidated by Renad [11], who showed that certain configrations cold not be reached by extreme trajectories containing only five elementary pieces. Yamamoto et al. [12] investigated qasi-time-optimal motion planning problem dividing the problem into two sbproblems as follows: 1) time-optimization of trajectory along specified path and 2) search for optimal path. he above researches based on Hilare-like model inclde only linear and anglar acceleration bonds. Most researches considering dynamic model dealt with dynamic constraints of inpt torqes only or jst with limitations of velocities. However, since there are limits on motor s performance and battery s power, WMR systems have motor armatre crrent constraint as Manscript received April 4, 2000; revised October 30, his paper was recommended for pblication by Associate Editor H. Arai and Editor A. De Lca pon evalation of the reviewers comments. he athors are with the Department of Electrical Engineering, Korea Advanced Institte of Science and echnology, aejon , Korea ( jschoi@rtcl.kaist.ac.kr; bkkim@ee.kaist.ac.kr). Pblisher Item Identifier S X(01) Fig. 1. Strctre of WMR. well as battery voltage constraint in practice. In previos researches, control inpts are velocities or accelerations with or withot bonds. In practice, since final control inpts are voltages (PWM dty ratios) generated by those servo modles, there may exist bad cases those modles cannot track the desired velocity/acceleration commands de to voltage and crrent constraints. In this paper, we propose a near-time-optimal trajectory planning algorithm (inclding both PP and G) nder assmption of simplified dynamics, which satisfies initial and final postres and velocities as well as voltage and crrent constraints. o validate the proposed algorithm, simlation reslts are presented. II. PROBLEM SAEMEN A. Dynamic Model for WMRs Assme that WMR has symmetrical strctre driven by two identical DC motors as shown in Fig. 1. Assme both motors of WMR have the same armatre resistance R a, back-emf constant K b, torqe constant K t, and gear ratio. For simpler dynamics, we can neglect indctance of armatre circits since electrical response is mch faster than mechanical response in general. Letting s be the battery voltage spplied, armatre circits of both motors are described by R ai = s 0 K b w (1) i = [i 1 i 2 ] is armatre crrent vector, w = [w 1 w 2 ] is anglar velocity vector of wheels, and = [ 1 2 ] is normalized voltage vector (eqivalent to PWM dty ratios). Sperscripts 1 and 2 correspond to right and left motors, respectively. In addition, dynamic relation between anglar velocity and motor crrent considering inertia and viscos friction becomes J dw dt + Fvw = Kti (2) F v is viscos friction coefficient and eqivalent inertia matrix of motors J is J = J1 J2 J 2 J 1 J 1 = mc 2 b 2 + Ic 2 + I w and J 2 = mc 2 b 2 0 Ic 2. For details, see [13]. Define change of variables as i + 1 = (i 1 + i 2 )=2 + 1 =( )=2 i 0 1 = (i 1 0 i 2 )=2 0 1 =( )=2: (3) X/01$ IEEE

2 86 IEEE RANSACIONS ON ROBOICS AND AUOMAION, OL. 17, NO. 1, FEBRUARY 2001 ABLE I P SRAEGY hen, the translational velocity v = r(w 1 +w 2 )=2 and anglar velocity w = r(w 1 0 w 2 )=2b are ncorrelated with regard to fi + ; + g and fi 0 ; 0 g as rr a i + = r s + 0 K b v (4) dv dt + Fvv = rkti+ (5) rr a = rs 0 K b w b b (6) dw J w dt + F vw = rk t i 0 b (7) = J 1 + J 2, J w = J 1 0 J 2. B. oltage and Crrent Constraints Since there are limits on motor s performance and battery s power, WMR systems have voltage constraint on battery as well as crrent constraints on motor armatres or j j j max ji j ji max; j =1; 2 (8) j + j + j 0 j max ji + j + ji 0 ji max: (9) C. Configrations and Path Deviation We consider only primary configration (PC) it is nnecessary to change the sign of rotational velocity for path-planning. Since the time-optimal paths for PC is expected to be made p with one rotational section and two translational sections srronding the rotational one, we divide or control algorithm for cornering motion into three sections: SB (translational section before rotational section), RS (rotational section), and SA (translational section after rotational section). RS is focsed on the rotational motion for the reqired trning angle, and both SB and SA are the secondary procedres focsed on translational motions. Note that obstacle avoidance is considered implicitly: we consider the bond of path-deviation D [or deviation from corner D 0 = D= cos(( f 0 s )=2)] as shown in Fig. 2, which limits deviations from the given configration, and hence obstacles can be avoided. D. Problem Statement Define a state z composed of postres and velocities as z(t) = [x(t) y(t) (t) v(t) w(t)]. Assme that WMR is in translational motion allowing nonzero velocity at both initial and final states which are given as z s =[x s y s s v s 0] z f =[x f y f f v f 0] : (10) Fig. 2. Sections in path and reqirement of path-deviation. In addition, assme that translational velocity is fixed as a constant vale in RS, and so is rotational velocity as zero in both SB and SA. Problem: Given z s and z f, find the reference velocity trajectory f[v r (t); w r (t)], 0 t t f g minimizing final time t f s.t.: 1) z(t f )=z f ; 2) satisfying voltage and crrent constraints; 3) satisfying path-deviation reqirement D. III. NEAR-IME-OPIMAL RAJECORY PLANNING In this section, a near-time-optimal trajectory is generated by ECA (extreme control algorithm) for WMRs to satisfy given initial and final states as well as voltage and crrent constraints. Or P strategy can be smmarized as able I (symbols will be explained later). First, RS is planned for WMR to trn to the reqired trning angle f 0 s with a proper vale of constant translational velocity satisfying path-deviation reqirement D. o satisfy position condition, SB and SA are planned to cover remaining distances d B (along s) and d A (along f ) as shown in Fig. 3. A. Rotational Section Since the translational velocity which is denoted by is assmed to be constant in RS, from (5), i + is fixed as i + = Fv rk : t (11a) Also, from (4) and (11a) + = Ra F v + KtK 2 b rk t s R a : (11b) Also, bonds of i 0 and 0 are determined as i 0 min i0 i 0 max 0 min 0 max 0 (12)

3 IEEE RANSACIONS ON ROBOICS AND AUOMAION, OL. 17, NO. 1, FEBRUARY Fig. 4. For types of extreme control for anglar velocity. he nknowns t R U, t R M, w M, t R L, and t R f are related as Fig. 3. Calclation of d ; d ; and d from given S. i 0 max = 0i 0 min = i max 0 ji + j 0 max = 0 0 min = max 0j + j: In RS, fi 0 min ;i0 max; 0 min ;0 maxg are all constants from the assmption that is fixed. Hence, control sing those extreme vales is possible. From (12), we can see there is a sitation that crrent constraint is forced rather than voltage constraint corresponding to conditions of 0 and w as i 0 = i 0 max; if 0 > 0 and s r w w U = bk b 0 R ar max 0 bk b i0 max i 0 = i 0 min; if 0 < 0 and w w sr L = bk b 0 R ar min 0 bk b i0 min: (13) Considering the crrent/voltage constraints, (6) and (7) are rearranged into dw dt = R ai w + bi R i 0 ; if 0 > 0; w w U or 0 < 0; w w L a R w + b R 0 ; otherwise a R F v i = 0 b R K tr i = J w J w b a R F v + K b K t 2 =R a = 0 J w b R = K t s r J w br a (14) and the sperscript R means rotational section. With regard to whether crrent constraint is forced or voltage constraint is forced, for types of extreme control for anglar velocity which are shown in Fig. 4 the reference profile of anglar velocity is determined as w r (t)= w +1;i 0 w +1;ie 0t= ; 0 tt R U w +1; +(w U 0 w +1;)e 0(t0t )= ; t R U <tt R M w01;i +(w M 0 w01;i)e 0(t0t )= ; t R M <tt R L w01; +(w L 0 w01;)e 0(t0t )= ; tl R <tt R f (15) R i = 0 1 a R i w01;i = 0 br i a R i w +1;i = 0 br i a R i i 0 min R = 0 1 a R i 0 max w +1; = 0 br a R 0 max w01; = 0 br a R 0 min w U = w r (tu R ) w M = w r (tm R ) w L = w r (tl R ): t R U = R i ln 0w +1;i w U 0 w+1;i 0(t 0t )= w M = w +1; +(w U 0 w +1;)e t R L = t R M + R i ln w M 0 w01;i w L 0 w01;i tf R = tl R + R w L 0 w01; ln : (16) 0w01; Since the resltant trning angle is the area of the anglar velocity profile, we can establish an eqation with respect to only one variable tm R as f 0 s = 0 t w r(t) dt = w +1;it R U 0 R i w U + w +1;(t R M 0 t R U )+ R (w U 0 wm ) + R i w01;i ln wm 0 w01;i w L 0 w01;i + w M 0 wl + R w L 0 w01; w01; ln + w L : (17) 0w01; In (17), we can find tm R sing mathematical tools or nmerical methods. hen, w M and tf R are determined. B. ranslational Section We can apply the same process as in RS by analogy. Since the rotational velocity which is denoted by W is assmed to be zero in translational section, sing (6) and (7), i 0 and 0 are also fixed as i 0 = bf v rk t W =0 (18a) 0 = bra F KtK b 2 v + rk t s R a W =0: (18b) Also, bonds of i + and + are determined as i + max = 0i + min = i max 0ji 0 j max + = 0 + min = 0j0 max j: (19) hen, v U and v L are s r v ar U = K b + max 0 K b i+ max s r v R ar L = K b + min 0 K b i+ min: (20) Considering the crrent/voltage constraints, (4) and (5) are rearranged into dv dt = v + b i i + ; if + > 0; v v U a v + b + ; or + < 0; otherwise v v L (21)

4 88 IEEE RANSACIONS ON ROBOICS AND AUOMAION, OL. 17, NO. 1, FEBRUARY 2001 = 0 Fv b i = Ktr a = 0 Fv + K bk t 2 =Ra b = Ktsr Ra and the sperscript means translational section. If we define v s (v f ) as the initial (final) translational velocity in translational section, the reference profile of translational velocity is v +1;i +(v s 0 v +1;i)e 0t= ; 0 t t U v r (t) = v +1; +(v U 0 v +1;)e 0(t0t )= ; t U <t t M v01;i +(v M 0 v01;i)e 0(t0t )= ; t M <t t L v01; +(v L 0 v01;)e 0(t0t )= ; tl <ttf (22) Fig. 5. Incremental distance occrred from 1. i = 0 1 v01;i = 0 b i v +1; = 0 b a max + v +1;i = 0 b i i + min = 0 1 a i + max v01; = 0 b a + min v U = vr(t U ) v M = vr(t M ) v L = vr(t L ): he nknowns t U, t M, v M, t L, and t f are related as t U = i ln v s 0 v +1;i v U 0 v +1;i 0(t 0t )= v M = v +1; +(v U 0 v +1;)e t L = t M + i ln v M 0 v01;i v L 0 v01;i tf = tl + ln v L 0 v01; vf 0 v : (23) 01; Since the resltant distance of movement d is the area of the translational velocity profile, we can establish an eqation with respect to only one variable t M as d = v +1;itU + i (vs 0 v U ) + v +1;(tM 0 tu )+ (v U 0 v M ) + i v01;iln v M 0 v01;i + v M 0 v L v L 0 v01;i + vl 0 v01; v01;ln vf 0 v + v L 0 vf : (24) 01; In translational section, we shold consider minimm reqired distance d min between two points having different translational velocities vs and vf, since d B and d A which are acqired after planning of RS shold not be smaller than d B min and dmin, A respectively. At first, assme vs <vf. We have the following three cases. 1) If v U <v s <vf se max + ) d min = v v s 0 v +1; +1;ln vf 0 v + vs 0 vf : +1; 3) If v s <v U <v f se first i + max and then + max ) d min = i v v s 0 v +1;i +1;iln + vs 0 v U v U 0 v +1;i + v +1;ln v U 0 v +1; v f 0 v+1; + v U 0 v f : Similarly, d min is can be derived when v s >v f. C. Extreme Control Algorithm We decide the optimal vale of constant translational velocity in RS sing binary search for the scale factor S which is defined as = Sv max ; 0 <S<1 (25) v max = minfv +1;i;v +1;g is maximm translational velocity of WMR in steady state when 1 = 2. Note there is a basic proposition abot S before going to the algorithm. Proposition 1: he largest possible scale factor S leads to the least t f. Proof: Assme that [x s y s s ] = [0 0 0], w s = w f = 0, and f > 0. Define 1 as the incremental change of cased from 1S. On the other hand, if we define P t =[x t y t t ] as the temporal postre after planning RS as shown in Fig. 5 and define l as the angle from P s to P t, then x t = 1x t = 1 1y t = 1 0 t cos (t) dt y t = xt + 1tR f cos f = l1 yt + 1tR f sin f = l1 l = x 2 t + y2 t l =tan 01 y t x t : From (27) and Fig. 5, we can say that 0 t sin (t) dt (26) cos l + 1t R f cos f sin l + 1t R f sin f (27) 2) If v s <v f <v U se i + max ) d min = i v v s 0 v +1;i +1;iln vf 0 v + vs 0 vf : +1;i 1l B = l1 1l A = l1 sin( f 0 l ) = lb sin f 1 sin l + 1tf R = l A sin f 1 + 1tR f : (28)

5 IEEE RANSACIONS ON ROBOICS AND AUOMAION, OL. 17, NO. 1, FEBRUARY Fig. 6. z =(0:5; 0:5; 90 ; 0; 0), D =0:05! (S; t )=(0:339; 2:06). We can derive incremental time in each section from the 1 as 1t R f = 2tR M 0 t R f bwm 1t B f = 0 1lB vm B 1t A f = 0 1lA vm A 1 0 (v B M 0 ) v B M (v +1; + ) 1 0 (va M 0 ) vm A 1 (29) (v+1;0 ) sperscripts B and A means SB and SA, respectively. We can infer the overall time difference 1tf as 1tf =1tf R +1tf B +1tf A (vm < 0 B 0 ) vm B (v+1; + ) + A (vm 0 ) vm A (v+1;0 ) v max1s: (30) For 1S >0, we get 1tf < 0. Hence, the total time will be smaller if we make the scale factor larger. If we define d as the maximm deviation from the corner as shown in Fig. 3, the path-deviation reqirement is eqivalent to d D 0. hen, ECA is established as follows. Algorithm 1 (ECA): Set S =0:5, SU =1, SL =0, and search for the optimal S with its tolerance Stol by doing following steps. Step 1) Solve the Problem RS (able I) with pdated S and calclate both d B and d A defined as in Fig. 3 and calclate both d B min from [v s ;v f ] = [vs; Sv max ] and d A min from [v s ;v f ]=[Sv max ;vf ]. Step 2) Calclate S and pdate SL(or SU )as S =0:5(SU 0S); SL S; if d B d B min & d A d A min d D= cos f 0 s 2 S =0:5(SL0S); SU S; otherwise. Step 3) If jsj >Stol, pdate S as S S + S and goto Step 1, otherwise goto Step 4. Step 4) Solve the Problem SB (able I) with d B. Step 5) Solve the Problem SA (able I) with d A. ABLE II PARAMEERS OF WMR Finally, the overall reference velocity trajectory is [v r ;w r ]=[(v B r v R r v A r ); ( w B r w R r w A r )]: (31) I. SIMULAION RESULS Simlations are performed to validate the trajectory generated by algorithm ECA sing parameters in able II. A control for is reqired for WMRs to follow the reference trajectory acqired from the proposed ECA. Inverse control is sed in this research, with which the exact following is possible if the model of plant is exact. z s =[000 00] and z f =[0:5 0: ] are sed. Also D = 0:05 is sed in Fig. 6 while D = 0:10 in Fig. 7, position plot (x; y), velocity plot (v; w), crrent plot (i 1 ;i 2 ), and voltage plot ( 1 ; 2 ) are inclded. We can see that crrent and voltage constraints are satisfied as well as path-deviation reqirement from each figre. In Fig. 6, we get t f = 2:06 s. SB is planned with three extreme controls fi + max; max; + i + ming, RS is with three extreme controls f 0 max; i 0 min ;0 ming, while SA is with two extreme controls f max; + i + min g since > vb U (= 0:12). In Fig. 7, larger scale factor S (larger translational velocity ) can be selected from binary search since the bond of path-deviation D is larger. RS is with only two extreme controls fmax; 0 0 ming since crrent constraint has no effect in RS when larger translational velocity is involved. Also, note that total time t f =1:77 s is smaller than in Fig. 6 de to larger scale factor S. here is a small increase in t R f for s. However, there are larger

6 90 IEEE RANSACIONS ON ROBOICS AND AUOMAION, OL. 17, NO. 1, FEBRUARY 2001 Fig. 7. z =(0:5; 0:5; 90 ; 0; 0), D =0:10! (S; t )=(0:540; 1:77). decreases in t B f for s and in t A f for s, and total time t f is redced as a reslt.. CONCLUSION We have considered real constraints on armatre crrent and battery voltage in WMRs. he near-time-optimal P for WMRs separated with translational and rotational sections is performed by extreme controls based on those constraints satisfying initial and final states. We divide or control algorithm for trning motion into three sections as follows. he first is RS, which is focsed on the rotational motion with the reqired trning angle, and the others are SB and SA, which are secondary procedres focsed on translational motion. No obstacles bt the bond of path-deviation D is considered. We tilized the following two assmptions. 1) Dynamic model of WMRs disregarding the electrical response of armatre circits of motors, and transformed it into ncorrelated form with regard to translational and rotational velocities. 2) ranslational velocity is fixed in RS. hen, we can get a near-time-optimal P, translational velocity is selected for time-optimality by binary search for velocity scale factor S. Simlation reslts reveal validity of or P strategy. REFERENCES [6] Y. Kanayama and B. Hartman, Smooth local path planning for atonomos vehicles, Int. J. Robot. Res., vol. 16, no. 3, pp , [7] Y. Kanayama, Y. Kimra, F. Miyazaki, and. Nogchi, A stable tracking control method for an atonomos mobile robot, in Proc. IEEE Int. Conf. Robotics and Atomation, Cincinnati, OH, May 1990, pp [8] P. Soères,. Hamel, and. Cadenat, A path following controller for wheeled robots which allows to avoid obstacles dring transition phase, in Proc. IEEE Int. Conf. Robotics and Atomation, Leven, Belgim, May 1998, pp [9] P. E. Jacobs, A. Rege, and J. P. Lamond, Non-holonomic motion planning for Hilare-like mobie robots, in Proc. Int. Symp. Intelligent Robotics, Bangalore, India, Jan. 1991, pp [10] D. B. Reister and F. G. Pin, ime-optimal trajectories for mobile robots with two independently driven wheels, Int. J. Robot. Res., vol. 13, no. 1, pp , [11] M. Renad and J. Y. Forqet, Minimm-time motion of a mobile robot with two independent acceleration-driven wheels, in Proc. IEEE Int. Conf. Robotics and Atomation, Albqerqe, NM, Apr. 1997, pp [12] M. Yamamoto, M. Iwamra, and A. Mohri, Qasi-time-optimal motion planning of mobile platforms in the presence of obstacles, in Proc. IEEE Int. Conf. Robotics and Atomation, Detroit, MI, May 1999, pp [13] X. Yn and Y. Yamamoto, Internal dynamics of a wheeled mobile robot, in Proc. IEEE Int. Conf. Intelligent Robots and Systems, Yokohama, Japan, Jly 1993, pp [1] L. E. Dbins, On crves of minimal length with a constraint on average crvatre and with prescribed initial and terminal positions and tangents, Amer. J. Math., vol. 79, pp , [2] J. A. Reeds and L. A. Shepp, Optimal paths for a car that goes both forward and backward, J. Pacific Math., vol. 145, no. 2, pp , [3] P. Soères and J. P. Lamond, Shortest paths synthesis for a car-like robot, IEEE rans. Atomat. Contr., vol. 41, pp , May [4] A. Bicchi, G. Casalino, and C. Santilli, Planning shortest bonded-crvatre paths for a class of nonholonomic vehicles among obstacles, J. Intell. Robot. Syst., vol. 16, pp , [5] D. S. Meek and D. J. Walton, Clothoidal spline transition spirals, Math. Comptat., vol. 59, no. 199, pp , 1992.