Analytical time-optimal tajectoies fo an omni-diectional vehicle Weifu Wang and Devin J. Balkcom Abstact We pesent the fist analytical solution method fo finding a time-optimal tajectoy between any given pai of configuations fo a thee-wheeled omni-diectional vehicle in an obstacle-fee plane. The mathematical model of the vehicle bounds the velocities of the wheels independently. The timeoptimal tajectoies can be divided into two categoies: singula and geneic. An analytical solution method has peviously been pesented fo singula tajectoies; this pape completes the wok and pesents the solution fo geneic tajectoies. The speed and pecision of the algoithm allow dense sampling of the configuation space, to detemine how the time and stuctue of time-optimal tajectoies change acoss configuations. Simulation esults show that time-optimal tajectoies tend to be ten to twenty pecent faste than a simple but pactical diving stategy: tun until the fastest tanslation diection faces the goal, dive to the goal, and tun to the cuent angle. I. INTRODUCTION This pape pesents the fist analytical method to find a time-optimal tajectoy between any pai of configuations of a paticula symmetic thee-wheeled model of an omnidiectional vehicle, without obstacles in the wokspace. The vehicle, which is shown in figue??, can move in any diection instantaneously. Its wheels ae omniwheels the wheels ae poweed in the diections that they ae diven (v, v and v 3 in figue??), but can slide feely in the pependicula diections. We assume that the speeds of the wheels can be contolled, and that these wheel speeds ae each bounded to fall in the ange [, ] fo some choice of units of distance. The configuation of the obot is given by q = (x, y, θ), the location and oientation of a fame attached to the cente of the obot. Although the wheel speeds ae diectly contolled by the motos, thee is a one-to-one-mapping between wheel speeds and the genealized velocity of the obot in its own fame; we choose the genealized velocity as the contol. The motion of the obot is given by q(t) = q(0) + t 0 R(θ(τ))u(τ)dτ, () whee u = ( R ẋ, R ẏ, R θ) is the genealized velocity in the obot fame, and R is the matix that tansfoms the velocity into the wold fame (fomed by eplacing the uppe left block of a 3x3 identity matix with a x otation matix). The cuent pape builds heavily on pevious wok. The ecent Ph.D. thesis of Andei Futuna [?] attacked a moegeneal poblem: solving the time-optimal tajectoies fo a igid body in the plane with polyhedal bounds on genealized velocity contols; the omni-diectional obot poblem is a special case. The thesis uses Pontyagin s Maximum Pinciple (PMP) to deive stong necessay conditions on optimal tajectoies, and poves futhe geometic and algebaic esults. Thee is a geometic intepetation of tajectoies satisfying the Maximum Pinciple, which we will call extemal tajectoies. Fo each extemal tajectoy, thee is a diected line in the plane (called the contol line), such that the velocity of a point on the line, pushed by the obot, is maximized at almost evey time. Thee ae two types of tajectoies geneic, fo which the motion of the obot is completely chaacteized by the contol line, and singula, fo which it is not. C v θ (x, y) v v Fig. : The shape and the notation of the model Fig. : A sample time-optimal tajectoy In [?], a complete analytical method is pesented fo solving fo the time-optimal singula tajectoy between any pai of configuations. Howeve, until now, the poblem of detemining the pecise geneic tajectoy that connects any two paticula configuations has evaded solution. This solution is the pimay contibution of the cuent pape. We have studied this model in pevious wok [?], and in that wok, deived a complete geometic desciption of the types of tajectoies that might be optimal. We will use seveal esults fom [?]: ) Although time and contol space ae continuous, most contols do not appea in optimal tajectoies. In fact, tajectoies with piecewise constant contols chosen fom a set of 4 discete contols ae sufficient to achieve optimality fo the bounded wheel speed model. The contol set includes clockwise and counteclockwise spins in place about obot cente C s, fowad o backwad tanslation in thee diections, and clockwise o counte-clockwise otations (tuns) about thee points (C, C, C 3 in figue??) placed in an equilateal tiangle about the obot. ) Geneic tajectoies ae peiodic, in the sense that fo each tajectoy thee exists a duation T p such that any two configuations along a tajectoy sepaated by T p epesent effectively a pue tanslation paallel to a fixed line in the plane, the contol line. In this wok, the basic appoach is to compae all the extemal tajectoies connecting given stat and goal configuations to find a time-optimal one. A. Related Wok Time-optimal tajectoies fo seveal othe kinematic models have been found. A ca that can only dive fowads with bounded fowad velocity and steeing angle was studied by Dubins [?]. Reeds and Shepp [?] found the shotest 3 C3 C
C3 C C H, called the Hamiltonian, given by: contol line P C P Fig. 3: Geomety of a shuffle tajectoy C H H C P3 A contol line C3 Fig. 4: Detailed oll tajectoy. paths fo a steeed ca that can dive both fowads and backwads. Sussmann and Tang [?] futhe efined the esults by developing a geneal methodology fo solving this type of poblem. Souèes, Boissonnat and Laumound [?], [?] mapped the pais of configuations to optimal tajectoies fo the Reeds-Shepp ca. Balkcom and Maon [?] discoveed the time-optimal tajectoies fo a diffeential-dive obot with bounds on the wheel speeds; Chitsaz et al. [?] detemined the tajectoies fo a diffeential-dive that minimizes the sum of the otation of the two wheels. Dynamic models of omni-diectional vehicle with o without slip have been developed by Jung and Kim [?], and Williams et al. [?]. Optimal tajectoies fo omni-diectional vehicle using a dynamic model have been studied by Choi et al. [?] and Kalmá-Nagy et al. [?]. Howeve, because of the complexity of the model, the esulting algoithms have been numeical athe than analytical, and assume cetain estictions on the stuctue of tajectoies consideed. The kinematic model we study is less sophisticated, but does admit complete analytical solution. Optimal motion of dynamic models fo an undewate vehicle wee studied by Chyba and Habekon [?]. A new distance function using nonholonomic constaints wee used to find the minimum-time distance between a Dubins ca and polygonal obstacles [?], [?]. II. NECESSARY CONDITIONS FOR OPTIMALITY We have shown in [?] that a necessay condition fo timeoptimal motion of a igid-body with polyhedal bounds on genealized velocity contols (such as ou omni-diectional obot model), deived by application of Pontyagin s Pinciple [?] (PMP), is that fo any time-optimal tajectoy of the obot, thee must exist constants k, k, and k 3, not all zeo, such that at any time along the tajectoy, the genealized velocity of the obot (ẋ, ẏ, θ) must maximize the quantity H = k ẋ + k ẏ + θ (k y k x + k 3 ). () Futhemoe, H must be constant and positive ove the tajectoy. Without loss of geneality, assume k + k =. (Fo this vehicle, we have shown optimality may be achieved without use of the k = k = 0 case, and non-zeo scaling of the constants does not affect the maximization condition.) Let thee be a line in the plane with diection given by the vecto (k, k ); let the signed distance of the line fom the oigin be k 3. Call this line the contol line. Fo tajectoies containing no tanslations (geneic tajectoies fo the omni-diectional obot model do not), the Hamiltonian equation can also be given by: H = L y c θ, (3) whee L y c is the distance of the cuent otation cente fom the contol line. The choice of constants detemines the placement of the contol line in the plane. Geometically, we can say that fo any optimal tajectoy, we must be able to daw a paticula line in the plane, such that at evey time duing the tajectoy, the cuent otation cente, scaled by the angula velocity, is, among all possible otation centes used to contol the obot, as fa as possible fom the line. Initial conditions of the obot elative the line detemine the evolution of a geneic extemal tajectoy. Thee ae two types, called oll and shuffle tajectoies. Shuffle tajectoies occu when the obot stats vey close to the contol line, with paticula oientations. In this case, the tajectoy peiod contains 4 otations, and one of them has opposite diection compaed to the othes. Roll tajectoies tend to occu when the obot stats fa fom the contol line; in this case, all otations ae in the same diection. In this pape, we choose the units such that the distance between each wheel and the cente of the obot is, and the angula velocity of spin has magnitude. Then, each otation cente has distance 4 to the cente of the obot. We descibe each geneic extemal with some notation indicating otation cente and sign of angula velocity. Fo example, one peiod of a shuffle tajectoy might be of the fom C + s C + C C+ 3 C+ s in figue??. A. Time-optimal tajectoies neve exceed one peiod A switch is a time duing a tajectoy at which the contol changes. A segment of tajectoy is the subsection of the tajectoy between two switches. Theoem : No time-optimal oll o shuffle tajectoy fo the omni-diectional vehicle can be longe than one peiod. (One peiod of shuffle may contain at most fou switches, and one peiod of oll may contain at most six switches.) Poof: The esult fo shuffle tajectoies has been peviously poven in [?]. The poof fo olls can be divided into thee cases. In each case, the method is, given a tajectoy longe than one peiod, to constuct a new tajectoy that takes the same time, but does not satisfy the Pontyagin Pinciple. Since the new tajectoy does not satisfy necessay conditions and cannot be optimal, the oiginal equal-time tajectoy also cannot be optimal. Case : A peiod of a oll that stats and ends with spin, and the tun is longe than 60. Constuct a new tajectoy by eflecting the 60 segments of each tun acoss the line passing P and P. This is tajectoy C s C C C C s in figue??. Denote C A = d, and C A = d. In this case, H = 3 d 3 d. So the new tajectoy violates the necessay condition fo optimality, because in this tajectoy the Hamiltonian is not constant.
C d P P P3 P4 P5 P6 P7 P8 ` ` ` d A C` Contol Line Fig. 5: Figue fo poof that oll tajectoy cannot be longe than one peiod. Case : A peiod of a oll that stats and ends with spin o tuns, in which the maximum tun is shote than 60. Revese all the spins and eplace tun contols with symmetic contols following the oiginal path. Denote the distance fom C s to contol line as d. Then the oiginal H = d = 3 d, while the Hamiltonian fo the evesed spin is d. This new tajectoy violates the necessay condition fo optimality because the Hamiltonian is not constant duing the tajectoy. Case 3: A peiod of oll that stats and ends with a tun longe than 60. The initial point is eithe on the oiginal cuve between P P o it is on P C s. If it is on P P, follow the est of peiod by C C C sc C C C s till the end of the peiod. The ac length of P 3 P 4 and P 4 P 5 ae 60. Because the signed distance fom C s to the contol line is d, the Hamiltonian is not constant duing the tajectoy, violating the necessay conditions fo optimality. If the initial point is on P C s, eflect the 60 pat P i P i+, i = 3, 5, 7 acoss the line passing S i s. This new tajectoy also violates the necessay condition of optimality fo the same eason as in Case. III. ALGORITHM Algoithm : Main algoithm flow Input: t min = t singula fo contol i at initial do fo contol j at goal do if i is spin && j is spin then Compute H (eq.??), time t (eq.??,??) Compute H (eq.??), time t s (eq.??,??) if one of i and j is spin and the othe is tun then Compute H (eq.??), time t (eq.??,??,??) Compute H (eq.??), time t s (eq.??,?? o??,??) ( cases) if i is tun && j is tun then Compute H (eq.??), time t (eq.??,??) Compute H (eq.??,??), time t s (eq.??,?? o??,??,??) ( cases) t min = min(t min, t, t s ) The Hamiltonian is a constant ove a time-optimal tajectoy. We will see that fo oll and shuffle tajectoies, knowing the fist and last contol, as well as the stat and goal configuations of the obot, this value of the Hamiltonian can be computed. Once the Hamiltonian has been computed, the complete evolution and time cost of the tajectoy can be computed exactly. Ou algoithm loops ove possible contol choices fo the fist and last contol of the tajectoy, and fo each choice computes a candidate Hamiltonian value and time cost. Equations fo the computation of the Hamiltonian depend on whethe the tajectoy is a oll o shuffle, and whethe the fist and last contols ae spins o tuns; we will now conside each case. A. Calculating time fo each tajectoy Each tajectoy that takes the obot fom initial configuation to the goal configuation contains n segments. All the segments except the fist and the last segments ae complete, in the sense that the duation between contol switches can be diectly computed fom the necessay conditions and the value of the Hamiltonian. Denote the angula velocity and time fo each segment by ω i and t i, i =,,..., n. Assume initial and goal oientation ae θ s and θ g. We have: n t i ω i + θ s = θ g (4) i= n t total = t i. i= In equation??, the equal sign means the same ac length on the cicle. Since both initial and goal oientation ae in [ π, π], we can constuct a function S θ (θ s, θ g ) that calculates the total change of oientation though the tajectoy; denote n i= t iω i by θ inne, n i= t i as t inne. Then, t ω + θ inne + t n ω n = S θ (θ s, θ g ) t ω + t n ω n = S θ (θ s, θ g ) θ inne (5) t total = t + t inne + t n. (6) B. Algoithm fo each case ) Roll: spin-spin: Assume a tajectoy is a oll, and the fist and last contols ae both spins (take figue?? as an example). In this case, the contol line should be paallel to the line passing all the spin centes C s, in which one is the fist and the othe is the last (ode is not impotant). Denote the distance between fist and last otation centes by x. Denote the segment length by l; the oientation change fo each segment by α. l and α of oll in the contol line can be calculated as follows: l spin = 0 l tun = 4H α spin = π/3 acos(h/) α tun = acos(h/), since the distances fom C and C s to the contol line ae 3H and H, which can easily be deived fom the fact that Pontyagin s Pinciple indicates that H must be constant ove a tajectoy. Recall that thee ae n segments, and let k = n/ be the numbe of tuns between the fist and last contol. Theefoe, x = k l tun = k 4H H = (/) x /4k. (7) The Hamiltonian value H can be calculated by looping ove intege values of k, which is less o equal to 3, since optimal oll tajectoy cannot contain moe than six switches. If H ( / 3, ], we can then calculate the time fo this oll tajectoy by using equation??, with ω = ω in this case. θ inne = k α tun + (k ) α spin (8) t inne = 3k α tun + (k ) α spin. (9)
A C γ D α C d d w contol line C3 β α Fig. 6: A simplified case of a oll tajectoy with diffeent types of fist and last contols ) Roll: tun-tun: Assume the tajectoy is oll, and both fist and last contol ae tuns (take figue?? as an example). Fo this case, we conside otation centes C and C 3. Denote C C 3 = c. Also, ω = ω n, using equation?? to solve fo the time. c = k l tun = k 4H H = (/) x /4k (0) θ inne = k α spin + (k ) α tun t inne = k α spin + 3(k ) α tun () t + t n = (S θ (θ i, θ g ) θ inne )/ω. () 3) Roll: tun-spin: Assume the tajectoy is a oll, and the fist and last contols ae diffeent; one is a tun and the othe is a spin. Take figue?? as an example, with stat at A and goal at the ightmost C s. (The geomety also woks if the positions ae evesed, with A the goal.) Define d = C C s, and define d w = A C s. Fo n total segments, thee ae k = n/ spins between the fist and last contol. In figue??, C C = l tun = C 3 B. d 4H = k l tun = (k + ) 4H H = ((k + ) d )/((k + ) 4) (3) θ inne = k α spin + k α tun = kπ/3 t inne = k α spin + 3k α tun (4) t ω + t n ω n = S θ (θ i, θ g ) θ inne. To calculate the time, since ω ω n, we need to calculate the time fo the tun. Denote the angle AC s C by β, the angle C s C s C by α, the angle AC D by γ. If the pojection of d w onto the contol line is longe than C B, then the fist contol is longe than half of a complete tun and vice vesa. Theefoe, the angle fo the fist contol is α atate / ± γ. Use equation?? to find the total time. α = asin(h/d) β = acos((d w + d )/( d w d)) x = d w cos(β α) (k + /) l tun sign = x/ x γ = acos((h + d w sin(β α))/) t = α tun + sign γ (5) t n = (S θ (θ i, θ g ) θ inne t ω )/ω n. (6) B 4) Shuffle: spin-spin: Calculating the value of Hamiltonian fo a shuffle tajectoy is almost the same as fo the oll, except the segment lengths ae diffeent. Conside an example shuffle tajectoy in figue?? that stats with a spin at C s on the left and ends with spin at C s on the ight. At P, the contol switches diectly fom C to C, so C C = 3R and C A = 6H. Denote the distance between stat and goal by d w. We know fom the stuctue of the shuffle tajectoy that ω = ω n, and the sign of ω 3 (the angula velocity coesponding to the tun aound C ) is diffeent fom all the othe angula velocities. Then, we have: α spin = π/3 acos(h/) α tun = acos(h/) π/6 acos(6h/ 3) α tun = (π/6 acos(6h 3)) d w = 4H 3 36H (7) θ inne = 3 α tun ω + 3 α tun ω 3 t inne = 3 α tun + 3 α tun (8) t + t n = (S θ (θ i, θ g ) θ inne )/ω. (9) 5) Shuffle: tun-tun: Assume the tajectoy is shuffle and both the fist and last contols ae tuns (take figue?? as an example). Thee ae two cases in this type of tajectoy. Case : The fist and last tuns have the same sign of angula velocity. Conside the case whee the fist contol is C and the last contol is C 3 (the geomety also woks with the evese ode, and gives the same time cost). Denote the distance between the fist and last otation cente by c. Then C C 3 = c, ω = ω n, and the sign of ω is diffeent fom all the othe angula velocities. Use equation?? to calculate the total time: c w = 3 36H H = (/6) 3 c /4 (0) θ inne = 3 α tun ω t inne = 3 α tun () t + t n = (S θ (θ i, θ g ))/ω. () Case : The fist and last tuns have diffeent signs of angula velocity. Fist, conside the case whee the contol sequence is fom C to C (the contol sequence fom C to C 3 is symmetic). Denote the distance between the two otation centes by c w. Fo the sequence fom C to C, c = 3. Assume the stat is on the ac C s P, denoted by P, let C 3 P = d, and use equations?? to calculate total time. c = 3 (3) t ω + t ω = S θ (θ i, θ g ) (4) t = acos ( ( + 3 d )/( 3) ). (5) (In this case, the Hamiltonian value is not needed.) Now conside the contol sequence fom C to C 4. Define c = C C 4, d = C C s. Assume the goal is on ac C s S 3 and denoted by B. Define d 3 = C B, use equations?? to calculate total time. c 36H = 4H 3 36H (6) d 6H = 4H 3 36H θ inne = 3α tun ω + α spin ω 3 t inne = 3α tun + α spin (7) t ω + t n ω n = S θ (θ i, θ g ) θ inne t = [acos ( ( + c d )/( c) ) acos ( ( + c d 3)/( c) ) ]/ω. (8)
(a) tajectoy types fo π/4 slice (b) isocost cuves Fig. 7: Tajectoies fo θ s = π/4 (c) singula fom (, 4, π/4) to oigin (d) oll fom (,, π/4) to oigin (a) tajectoy types fo 0 slice (b) time fo 0 slice Fig. 8: Tajectoies fo θ s = 0 (c) (3,, 0) (d) (, 3, 0) (a) tajectoy types fo π slice (b) time fo π slice Fig. 9: Tajectoies fo θ s = π (c) (, 4, π) (d) (4, 4, π) 6) Shuffle: tun-spin: Case : Initial and final tun and spin have same sign of angula velocity. Take figue?? as example. Conside the contol sequence fom C to C 3, denote the distance between them by c. Assume initial A is on ac C s P, Define d = A C. Use equations?? to calculate total time. c 4H = 3 36H 4H (9) θ inne = 3 α tun ω + 3 α tun ω 3 t inne = 3 α tun + 3 α tun (30) t ω + t n ω n = S θ (θ i, θ g ) t = (acos ( +3 d 3 ) (π/6))/ω. (3) Case : Initial and final tun and spin have diffeent sign of angula velocity. Take figue?? as example, conside the case that contol sequence fom C to C s. Define c = C C s, assume initial in on ac P P denoted by A, define d = C s A. Use equations?? to calculate total time. c 6H = 4H 3 36H (3) θ inne = 3 α tun ω t inne = 3 α tun (33) d = sin( α/) t = [acos ( ( + c d )/( c) ) acos ( ( + c d )/( c) ) ]/ω. (34) IV. SIMULATION, EXPERIMENTS, AND COMPARISON We implemented this algoithm in C. The algoithm is sufficiently fast (about 70 tajectoies pe second on a standad desktop), to allow dense sampling of the configuation space to exploe how the contol sequences diffe fo diffeent initial configuations of the obot. Figues?? to?? show the esults fo thee diffeent stating oientations of the obot, with the goal at the oigin. Fo each configuation, we geneate the best singula tajectoy if it exists, and the best geneic tajectoy, and compae the two to find the optimal. In each figue, each diffeent colo epesents a diffeent contol sequence (identified by the type: singula, oll o shuffle, and diffeent fist and last contol). The time contou plots shows isocost cuves fo
TABLE I: compaison of time optimal tajectoy and tun - dive - tun distance to oigin bette in pecentage 0-0.697% - 7.9340% - 3.578% 3-4 3.606% 4-5.9535% 5-6.850% 6-7 0.7536% 7-8 9.747% 8-9 8.5558% 9-0 7.047% time costs in incements of 0.5 seconds fo the coesponding slice. The esult shows that the time-optimal tajectoies fo an omni-diectional vehicle ae mostly oll and shuffle fo initial configuations not fa (combine the distance and the angle diffeence) fom the goal, and singulas othewise. Fo initial angle close to the goal angle, thee of six fastest tanslation diections that may be close to stating angle may be used by time-optimal tajectoies depending on stating location (figue??). When the stating angle is vey close to the goal oientation, all six fastest tanslation diections may be used (figue??). And if the angle diffeence between stat and goal ae big enough, the angula cost dominates (figue??). Figue?? and?? also show that thee ae equivalent tajectoies fo some configuations, because the goal oientation can be attained by otating eithe in the positive o negative diection with the same time cost. An example is shown in figue??. Fo configuations that ae fa away fom the goal, we obseve that a simple contol stategy, which is a special case of singula, sometimes tuns out to be time-optimal: tun the fastest tanslation diection to face the goal; dive to the goal; tun to the coect angle. Unde these cicumstances, is it wothwhile to implement the complete algoithm descibed hee? We compaed the time-optimal tajectoy with the simple tun-dive-tun stategy fo some configuations close to the oigin, as shown fo diffeent distances fom the stat to the goal in table??. The time-optimal tajectoies tun out to be faste by 0% to 0%. At a high level, we classify tajectoies as oll, shuffle, o singula, but we might also simply descibe a tajectoy type based on the sequence of contols that occu in the tajectoy. Using this counting method, we found 459 types of tajectoy that ae optimal fo at least some configuations. This numbe compaes to 40 tajectoy types fo the diffeential- dive and 46 fo Reed-Shepp ca. We may explicitly enumeate all 34 diffeent types fo tajectoies with only one o two segments. Fo olls and shuffles with moe segments, because of the stong constaints on the contol sequences, the exact numbe of types can be enumeated; the esults ae listed in table??. The situation is moe complicated fo singulas. We sampled the space of stating configuations in an aea nea the oigin and found 93 diffeent types of singula tajectoies occued; this is only a lowe bound. TABLE II: all types of tajectoy fo thee o moe segments segment length numbe of singula types numbe of oll types numbe of shuffle types 3 4 4 4 33 4 5 47 4 6 5 0 7 0 0 8 8 0 0 shuffles, and used the sampling to find a lowe bound on the singula tajectoy types. We also compaed the timeoptimal tajectoies with othe diving methods, and it tuns out the the time optimal tajectoies ae somewhat faste fo stat configuations that ae not too close o too fa away fom the goal. This wok was suppoted in pat by NSF CAREER Gant IIS-0643476; we also thank Andei Futuna fo his advice and suggestions. V. CONCLUSION We developed an analytical method to efficiently find the time-optimal tajectoy between configuations of an omnidiectional vehicle. We sampled the space of stating configuations and used the algoithm to exploe the distibution of the tajectoy stuctues ove the configuation space. We also explicitly counted the tajectoy types fo olls and