Time-Optimal Cornering Trajectory Planning for Differential-Driven Wheeled Mobile Robots with Motor Current and Voltage Constraints

Transcription

1 Time-Optimal Cornering Trajectory Planning for Differential-Driven Wheeled Mobile Robots ith Motor Current and Voltage Constraints Yunjeong Kim Department of Electrical Engineering KAIST Daejeon, Kaist Byung Kook Kim Department of Electrical Engineering KAIST Daejeon, Kaist Abstract We derive time-optimal cornering trajectory planning algorithm for differential-driven heeled mobile robots(dwmrs) satisfying both battery voltage and armature current constraints. We use mobile robot s dynamics, hich includes motor dynamics. We divide our collision-free cornering trajectory planning algorithm into three sections, in each of hich to subsections are contained. We obtain a time-optimal trajectory by applying bangbang principles in all subsections hich have not ahieved in previous orks. Either current bounded or voltage bounded control input is utilized in each subsection to produce time-optimal trajectory ith continuous velocity. Simulation results are provided to validate the effectiveness of the proposed algorithm and compared ith extreme control algorithm(eca) in [4]. I. INTRODUCTION During the past decades, extensive research efforts have been devoted to plan collision-free paths for DWMRs since DWMRs are increasingly used in industrial and service robotics. One of the key challenges for DWMRs is a trajectory planning hich generates an off-line velocity profile connecting the desired initial and final configurations ith obstacle avoidance. There are numerous researches of trajectory planning for DWMRs considering kinematics only in [1], [7] and [8], hich is simple in calculating but poor performance. Most researches considering dynamic model of DWMRs confined to dynamic constraints of input torques only or just limitations of velocities/accelerations [9],[11]. Since motors are used in actuating DWMRs, e also need to consider motor armature current constraint as ell as battery voltage constraint. Otherise, DWMR cannot track the desired velocity commands due to current and voltage constraints. Furthermore, planning trajectory using direct control input instead of torque is suitable for DWMRs motion planning since e don t need any extra motor-torque or current controller [],[4],[5],[6],[13]. Hence, it is essentially required to plan efficient trajectory algorithm for DWMRs considering not only dynamics but also motor current and voltage constraints ith direct control input. This paper proposes a time-optimal mobile robot velocity trajectory generation algorithm for DWMRs, hich satisfies Fig. 1: Structure of DWMR initial and final postures as ell as motor s armature voltage and current constraints. We consider the bound of pathdeviation D so that obstacle avoidance is considered implicitly ith collision check algorithm. To validate the proposed algorithm, e conducted various simulations, and compared the results ith ECA in [4]. The remainder of this paper is organized as follos. Section represents the problem statement for minimum-time trajectory planning ith DWMR dynamics and voltage and current constraints, and Section 3 describes time-optimal trajectory planning algorithm. In Section 4, simulation results hich demonstrate the effectiveness of our proposed approaches are presented ith comparison to ECA. The paper concludes ith Section 5. II. PROBLEM STATEMENT A. Dynamic Model for DWMRs including DC Motors[4] We consider a DWMR hich has symmetrical structure driven by to identical DC motors as shon in Fig. 1. Dynamic relation beteen angular velocity and motor torque considering inertia and viscous friction becomes J d dt + F v = τ (1)

2 here = [ R L ] T is the angular velocity vector of heels, τ = [τ R τ L ] T is the motor toque of heels, F v is viscous friction coefficient, and J is equivalent inertia matrix of motors hich has J 1 and J as its component. For details, see [1]. Underscript nd L correspond to right and left motors, respectively. In addition, neglecting inductance of armature circuits of motors, e can obtain simple dynamics of both motors such as i = V s u K b ρ () here is armature resistance, i = [i R i L ] T is armature current vector, V s is battery voltage, u = [u R u L ] T is normalized voltage vector (equivalent to PWM duty ratios), K b is backemf constant, and ρ is gear ratio. Also the relation beteen armature current and motor torque is τ = K t ρi (3) here K t is torque constant. In order to transform dynamics into uncorrelated form ith regard to translational and rotational velocities, define change of variables as i + i R+i L, u + u R+u L i i R i L, u u R u L (4), and use kinematics of DWMRs[3] such as ṗ = cosθ sinθ v (5) 1 and v = [ r r r b b r, ] (6) here p = [x y θ] T is DWMR s posture vector, v = [v ] T is velocity vector, v and are translational and rotational velocity, respectively, r is a radius of driving heels, and b is distance beteen the to heels as shon in Fig. 1. After substituting (3) and () into (1) and changing variables using (4) ith (6), e can obtain mobile robot dynamics including motors, in hich v and are uncorrelated ith regard to {i +,u + } and {i,u } as [ ][ Jv dv ] [ ] [ ][ ] dt v rkt ρ i + J d + F v = rk dt t ρ i (7) b [ ] [ ] i + Vs [ ] [ ] i = u + Kb ρ [ ] u rv s v (8) V s bk b ρ rv s here J v = J 1 + J, J = J 1 J. We can define state z(t) [x(t) y(t) θ(t) v(t) (t)] T. B. Voltage and Current Constraints[4] In practice, battery in DWMR has voltage constraint v V s and motors have current constraint i i max. Hoever, since the final control inputs are PWM duty ratio u and v = V s u, the voltage constraint can be expressed as u u max = 1. Hence the voltage and current constraints become u j u max, i j i max, j = R,L (9) or Fig. : Corner configuration ith path deviation u + + u u max, i + + i i max. (1) C. Problem Statement Problem: Consider a DWMR ith dynamics including actuator motors having battery voltage and current constraints, hich should perform cornering motion as shon in Fig.. Find the reference velocity trajectory {[v r (t), r (t)], t t f } minimizing final time t f under constraints: i) Initial and final state: z s = [x s y s θ s ] T (11) z f = [x f y f θ f f ree ] T. (1) ii) Inner path deviation D Note that obstacle avoidance is considered implicitly by the inner path deviation. Assumption 1: Assume that L is long enough for the DWMR to reach the maximum velocity before starting rotational motion. Assumption : Assume that L is long enough for the DWMR to have zero rotational and maximum translational velocity hen it arrives at final destination. III. TIME-OPTIMAL CORNERING TRAJECTORY PLANING ALGORITHM A. Sections and Subsections We divide our control algorithm for cornering motion into three sections. DWMR has translational motion only in the first section S 1. In the second section S, DWMR increases its rotational velocity (RV) hile decreasing its translational velocity (TV) for cornering motion. Finally, for reaching final configuration in section S 3, DWMR decreases its RV and increases its TV. In case that DWMR has a sudden change of input, the current is saturated ith its maximum value before the voltage. Hence each section (S m, m = 1,,3) is divided into to subsections: current constant section(ccs, S m1 ) here current is saturated as a constant value and voltage constant section(vcs, S m ) here DWMR has constant voltage input as shon in Fig. 3. We use bang-bang or at least one

3 Fig. 3: Control input and velocity profile bang-bang control in each section for time-optimal solution. Bang-bang control means that control inputs have loer or upper bound values of their input and at least one bang-bang control means that more than one control input have bangbang input[1][13]. B. Algorithm Time-Optimal Cornering (TOC) TP Algorithm is established as follos. TOC TP Algorithm Set u =.5, u L =, u U = 1, and serch for the optimal u using binary search ith tolerance utol by performing the folloing steps. Step 1. Solve S 1 Planning, S Planning, S 31 Planning, and S 3 Planning ith updated u. Step. Carry out the collision check algorithm. Step 3. Calculate u and update u L or u U as { u =.5(uU u ), u L u, if collision occurs u =.5(u L u ), u U u, otherise. Step 4. If u > u tol, update u u + u and goto Step 1. Step 5. Solve S 11 Planning and S 1 Planning. The overall flo chart of TOC is shon in Fig. 4. C. Reference Profile in CCS and VCS In CCS (S m1 ), motors have constant current as their inputs. Hence e can obtain the folloing velocity profile from (7) such as Fig. 4: Flo chart of TOC Algorithm ẇ + a i = b i i m1, (13) v + a i vv = b i vi + m1, (14) here a i = F v J, b i = rk tρ bj, a i v = F v J v, b i v = rk tρ J v. For t (m 1) < t t m1 (m = 1,,3), (13) and (14) are solved as (t) = ((t (m 1) ) m1, )e ai (t t (m 1) ) + m1,. (15) v(t) = (v(t (m 1) ) v m1, )e ai v(t t (m 1) ) + v m1,. (16) here m1, = bi a i i m1, v m1, = bi v a i i+ v m1. In VCS (S m ), since constant voltage input is applied to each motor, e can obtain an equation using (7),(8) as ẇ + a u = b u u m, (17) v + a u vv = b u vu + m, (18) here a u = F v J + ρ K t K b J, b u = rk tρv s b J, a u v = F v J v + ρ K t K b J v, b u v = rk t ρv s J v. For t m1 < t t m (m = 1,,3), (17) and (18) are solved as (t) = ((t m1 ) m, )e au (t t m1 ) + m,. (19) v(t) = (v(t m1 ) v m, )e au v(t t m1 ) + v m,. () here m, = bu a u u m, v m, = bu v a u v u+ m. D. CCS Planning In DC motor circuits, motor armature current exceeds its maximum value hen there is abrupt change of inputs. In this

4 case, the current should be saturated ith its maximum value until the applied input doesn t cause the current to overflo. Hence, e can plan CCS by determining the unknons i + m1, i m1, T m1 satisfying u m1 (t m1) + u + m1 (t m1) = u max, (1) u m1 (t m1) = u m. () (1) is derived from the condition that voltage constraint should be satisfied in CCS. Meeting the voltage constraint s condition, u + m1 (t) + u m1 (t) u max for t (m 1) < t t m1, the equality holds in the end of each CCS for the time optimal planning. In addition, e can obtain () since the voltage input is a continuous function inside each section. 1) S 11 Planning: Since the angular velocity is zero in S 1n, i R 1n = il 1n. Then u 11 (t 11) = u 1 = is satisfied. Using (8) and (16), (1) and () become i + 11 V + K bρ v(t 11 ) = u max, (3) =. (4) For bang-bang control input, let i R 11 = il 11 = i max. From (3), the unknon T 1 is determined as T 1 = 1 a i ln v K b ρ rv s K b ρ rv s b i v a i v i max b i v a i v i max + V s i max u max. (5) ) S 1 Planning: In S 1, (1) and () are expressed as u 1 (t 1) + u + 1 (t 1) = u max, u 1 (t 1) = u. (6) Since DWMR begins to turn left in S 1, i L 1 is saturated as i L 1 = i max, and from Assumption 1, e obtain v(t 11 ) = v max. The unknons i R 1 and T 1 are related as i 1 V + bk bρ (t 1 ) S rv s + i + 1 V + K bρ v(t 1 ) = u max, i 1 V + bk bρ (t 1 ) = u S rv, (7) s here i + 1 = ir 1 i max, i 1 = ir 1 +i max, (t 1 ) = 1, e ai T 1 + 1,, and v(t 1 ) = (v max v 1, )e ai vt 1 + v 1,. In (7), e can find i R 1 and T 1 using to variable Neton method. 3) S 31 and T Planning: In S 31, since DWMR sitches its motion from rotaional one to translational one and i L 31 = i max, (1) and () are expressed as i 31 V + bk bρ (t 31 ) S rv s + i + 31 V + K bρ v(t 31 ) = u max i 31 V + bk bρ (t 31 ) = u 3 (8) using (8), (15)-() and i L 31 = i max, here i + 31 = ir 31 +i max, i 31 = i R 31 i max and (t 31 ),(t 31 ) can be obtained from (19), (). In contrast to S 1 planning of hich initial translational and rotational velocity in the section are knon as v max and respectly, the initial velocity (t ) and v(t ) in S 31 are correlated ith the parameters in S. Since e kno T 1, i 1 and u because of the algorithm procedure as shon in Fig. 4, the unknons are i R 31, T 31 and T. Since the number of the unknons is three, e need one more condition hich can be obtained from θ f θ s = θ 1 + θ + θ 31 + θ 3 (9) here θ mn, for m =,3, n = 1,, is the resultant turning angle in S mn hich can be obtained by the area of the angular velocity profile in each section. Due to Assumption, θ 3 can be approximated as t3 θ 3 = t 31 (t)dt (t)dt t 31 (3) Hence, (9) becomes θ f θ s t1 t 1 (t)dt + t t 1 (t)dt + = 1, a i (1 e ai T 1 a i T 1 ) + (t 1) a u + (t ) a i + (t ) a u t31 t (t)dt + (1 e au T ), a u (1 e au T a u T ) (1 e ai T 31 ) 31, a i (1 e ai T 31 a i T 31 ) t 31 (t)dt (31) hich is composed of the unknons i R 31, T 31 and T. From (8) and (31), e can find i R 31, T 31 and T by three variable Neton method. E. VCS Planning In CCS Planning, e use at least one bang-bang control to determine the constant current inputs hich fulfill to conditions at the same time: 1) the motor voltage is bounded in CCS and ) voltage input is continuous in each section. Since VCS is folloed by CCS, e don t need to consider the boundedness of armature current in VCS. Hence e can use bang-bang control not at least bang-bang control such that u + m + u m = u max. (3) 1) S 1 Planning: For planning S 1, e should find constant voltage control inputs and time interval in S 1. Since the rotational velocity (t) is assumed to be zero in S 1n for n = 1,, e obtain u 1 = from (7). Using (3), e get u + 1 = u max (33) Since S 1 is planned last, the remaining x distance s 1 after planning S n and S 3n, for n = 1,, as shon in Fig. is the resultant distance of movement x 1n in S 1n for n = 1, such that [13] s 1 = x 11 + x 1 (34)

5 here s 1 = (L + Dtan θ f θ s ) x + y sin((θ f θ s ) atan( y, x)) sin(π (θ f θ s )) x 11 = 1 a i v v 11, (1 e ai vt 11 ) + v 11, T 11 x 1 = 1 a u (v(t 11 ) v 1, )(1 e au vt 1 ) + v 1, T 1. v Since (34) has only one unkon variable T 1, e can determine T 1 using one variable Neton method. ) S 3 Planning: DWMR decelarates its rotational velocity hile accerlating translational velocity in S 3. Hence the bangbang control inputs for time-optimal is u + 3 = u max, u 3 = (35) In Fig., x and y are total x and y distance in S n and S 3n for n = 1,. If T 3 is determined as a certain initial value, e can obtain x and y using (5) by numerical methods. Also e have s = (L + Dtan θ f θ s ) x + y sin(atan( y, x)) sin(π (θ f θ s )) from Fig.. If some value of T 3 is same as its real value, s becomes zero. To find T 3, e set Neton function f (T 3 ) as s. Also e can assume that the first derivation of Neton function f (T 3 ) is v max from Assumption. Hence e can obtain T 3 using one variable Neton method. 3) Determination of u : For bang-bang control, u + + u = u max. (36) Hence e need to determine only u since u+ can be obtained from (36). u should be chosen properly so that both timeoptimal condition and collision-avoidance can be achieved. The bigger u enables to turn sharper, but the smaller u enables to turn inside the corner. For time-optimal control, DWMR should make a turn as close to the edge of the corner as possible. In addition, obstacles can be avoided by choosing u ith collision check algorithm considering the bound of path-deviation D as shon in Fig.. IV. SIMULATION RESULTS To illustrate the effectiveness of the proposed optimal solutions, e applied it to the time-optimal trajectory planning problem of the X-bot robot hich has parameters in TABLE I. The simulation results are compared ith the results of ECA[4], here near-optimal arc trajectory section is used. Figs. 5 to Fig. 6 sho the result of TOC TP Algorithm hen z s = [ ] T,z f = [ f ree ] T and D =.3. We get the total time t f =.694 s. Fig. 5 shos planned trajectory. We can see that current and voltage constraints are satisfied and at least one bang-bang control is applied in each subsection for time optimal control in Fig. 6. To compare total time of TOC ith that of ECA, e performed the simulations for various path-deviation D and path-length L Parameter Value Paramete Value i max.5 A V s 17 V u max 1 K t ρ.53 V/(rad/s) F v.49 Nm/(rad/s) K b ρ.486 V/(rad/s) b.1545 m 5.33 Ω r.36 m m 3.1 kg t.1 m I.9 kgm l.175 m I.4 kgm TABLE I: Parameters of X-bot TOC ECA D(m) L(m) TOC(s) ECA(s) TOC 1(%) TABLE II: Comparison of total time for various D and L as shon in Table II. The values in the final column represent percentage difference of the total time ith respect to that of ECA. TOC TP algorithm can save up to 3.91% of the total time compared ith ECA. Since TOC TP algorithm can save time hen DWMR turns the corner, the ratio is increased for a shorter displacement L. In ECA, DWMR needs a longer time for rotation about shorter D because of the assumption that translational velocity in rotational section is constant. Hence, the improved time ratio of TOC compared ith ECA increases for shorter L and D. V. CONCLUSION In this paper, e investigate the time-optimal trajectory planning problem for DWMRs considering motor current and voltage constraints. The time-optimal control input is designed to satisfy initial and final states ith a given bound of path-deviation D. We divide our control algorithm into three sections ith to subsections consisted of CCS and VCS. In CCS, current values of the to heels are determined by at least one bang-bang control, and each subsection duration is also obtained using the continuous velocity and bounded voltage condition in CCS. In VCS, e use bang-bang control and initial and final states condition to determine the voltage control input and the duration. We utilized the folloing to minor assumptions: 1) e neglect inductance of armature circuits, ) the path distance L is long enough for DWMR to reach the maximum velocity before starting rotational motion, and to have zero rotational and maximum translational velocity at final destination. Then e can obtain a time-optimal trajectory planning solution, here u is selected for timeoptimality by binary search ith collision check algorithm. Simulation results sho that proposed algorithm not only satisfies armature current and voltage constraints but also gives time savings, up to 3.9% compared ith ECA. ACKNOWLEDGMENT This ork as supported by Ministry of Knoledge Economy under Human Resources Development Program for Convergence Robot Specialists.

6 R L u =u1=umax 1 R L u =u3=umax y(m).3 u R u =umax u R u L x(m) Fig. 5: Planned trajectory APPENDIX Collision Check Algorithm Collision check algorithm is conducted after S 1,S,S 31,S 3 is planned. Hence e kno x(k) and y(k) for k 1 k k 3 here k mn, m = 1,,3,n = 1,, represents discrete time in the end of section S mn. In Fig., e find k such that θ k becomes θ p using binary search algorithm. We compare D p = D/cos((θ f θ s )/) ith d hich is distance beteent (x p,y p ) and (x c,y c ) so that e can recognize hether collision occurs or not. Hence, collision check algorithm is established by doing folloing steps. Step 1. Set k L = k 1,k U = k 3,k =.5(k L + k U ). Step. Calculate k and update k L or k U as { k =.5(kU k), k L k, if cosθ k < cosθ p k =.5(k L k), k U k, otherise. Step 3. If k 1, update k as k k + k and goto Step. Step 4. Calculate d = (x c x(k)) + (y c y(k)). { Collision occurs, if d Dp Collision doesn t occur, otherise. REFERENCES [1] B. Qin, Y. C. Soh, M. Xie, and D. Wang, Optimal trajectory generation for heeled mobile robot, in Proc. 5th Int. Conf. Comput. Integr. Manuf,, pp ,. [] C. H. Kim and B. K. Kim, Minimum-energy translational trajectory generation for differential-driven heeled mobile robots, Journal of Intelligent and Robotic Systems,, pp , Mar., 7. [3] J. C. Alexander and J. H. Maddocks, On the kinematics of heeled mobile robots, International Journal of Robotics Research, vol. 8, no.5, pp. 15-7, Oct., [4] J. S. Choi and B. K. Kim, Near-time-optimal trajectory planning for heeled mobile robots ith translational and rotational sections, IEEE Trans. Robot. Autom., vol. 17, no.1, pp. 85-9, Feb., 1. [5] J. S. Choi, Trajectory planning and folloing for mobile robots ith current and voltage constraints, Ph.D s thesis, KAIST, Dae-jeon, Korea, 1. [6] J. S. Kim and B. K. Kim, Minimum-time grid coverage trajectory planning algorithm for mobile robots ith battery voltage constraints, Int. Conf. Control and systems,, pp , Oct., 1. [7] L. Labakhua, U. Nunes, R. Rodriguies, and S. F atima Leite, Smooth trajectory planning for fully automated passenger vehicles-spline and clothoid based methods and its simulation, in Proc. Int. Donf. Informat. Control, Autom. Robot, pp , 6. [8] M. Hentschel, D. Lecking, and B. Wagner, Deterministic path planning and navigation for an autonomous fork lift truck in Proc. 6th IFAC Symp. Intell. Auton. Veh. pp.1-6, 8. v(m/s) i(a) t(s) (a) Left and right motor voltage input R L L i =i11=imax i =imax t(s) (b) Left and right motor current t(s) v (c) Translational and rotational velocity profile Fig. 6: Input and velocity profile of TOC TP algorithm: z f = [ f ree ] T,D =.3 [9] M. Haddad, W. Khalil, and H. E. Lehtihet, Trajectory planning of unicycle mobile robots ith a trapezoidal-velocity constraint, IEEE Trans. on Robotics, vol. 6, No. 5, pp , 1. [1] P. E. Jacobs, A. Rege, and J. P. Laumond, Non-holonomic motion planning for Hilare-like mobie robots, in Proc. Int. Symp. Intelligent Robotics, pp , Jan., [11] W. Wu, H. Chen, and P. -Y. Woo, Time optimal path planning for a heeled mobile robot, J. Robot. Syst, vol. 17, no. 11, pp , [1] X. Yun and Y. Yamamoto, Internal dynamics of a heeled mobile robot, in Proc. IEEE Int. Conf. Intelligent Robots and Systems,, pp , July, [13] Y. J. Byeon, Near-minimum-time cornering trajectory planning and control ith the motor actuating voltage constraint for differential-driven heeled mobile robot, Master s thesis, KAIST, Dae-jeon, Korea, 1. i R i L (rad/s)