ERK'2022, Portorož, 233-236 233 Generiranje ˇ casovno minimalnih trajektorij v avtomatiziranem skladiˇ sˇ cu z omejitvami hitrosti, pospeˇ ska in trzaja Martina Benko Loknar 1 , Gregor Klanˇ car 1 , Saˇ so Blaˇ ziˇ c 1 1 Univerza v Ljubljani, Fakulteta za elektrotehniko E-poˇ sta: martina.benkoloknar@fe.uni-lj.si Generation of minimum-time trajectories in warehouse environment considering limitations on velocity, acceleration and jerk This paper considers a problem of minimum-time smooth trajectory planning for wheeled mobile robots with con- straints on velocity, acceleration and jerk. The smooth path is defined by several B´ ezier curves and the calcu- lated velocity profiles on individual segments are minimum- time with continuous velocity and acceleration in the joints. We describe a novel solution for the construction of a 5th order B´ ezier curve that enables simple and intuitive pa- rameterization. The trajectory planning algorithm opera- tion is demonstrated in a warehouse environment. There- fore, we have shown that the proposed path construction and trajectory generation algorithm can be applied to a constrained environment and can also be used in real-life time-minimization. 1 Uvod Mobilni roboti so zaradi vse ˇ sirˇ se uporabe postali sestavni del raznolikih okolij; najdemo jih v proizvodnji, medicini in v ˇ stevilnih drugih storitvah, ki temeljijo na robotih, vkljuˇ cno z avtomatiziranimi skladiˇ sˇ ci. V delovnih oko- ljih, kjer lahka in delovno intenzivna opravila delavcev nadomestijo mobilni roboti, se zmanjˇ sa ˇ cas obdelave iz- delkov, poveˇ cajo se delovna uˇ cinkovitost, nadgradljivost, prilagodljivost in preglednost skladiˇ sˇ ca. Naˇ crtovanje poti in trajektorij sta temeljna problema v avtonomni mobilni robotiki in celo ˇ sirˇ se v okviru av- tomatizacije [1]. Algoritmi naˇ crtovanja poti izraˇ cunajo pot skozi vnaprej doloˇ cene toˇ cke, pri ˇ cemer je cilj najti neprekinjeno pot s ˇ cim krajˇ so razdaljo med zaˇ cetno in konˇ cno toˇ cko tako, da ne pride do trkov z ovirami [2]. Medtem ko je planiranje poti geometrijski problem, pa se pri naˇ crtovanju trajektorije nastalo pot dodatno para- metrira s ˇ casom. Pomemben del metod naˇ crtovanja poti je izbira geometrijskih krivulj, ki so lahko polinomi, B´ e- zierjeve krivulje, kubiˇ cni zlepki, B- zlepki, Dubinove kri- vulje, klotoide, hipocikloide ipd. [10, 11]. Posledica doloˇ canja ˇ casovnih trenutkov vzdolˇ z poti pa vpliva na kinematiˇ cne in dinamiˇ cne lastnosti gibanja mobilnega sistema: sile in navori so odvisni od pospeˇ ska vzdolˇ z trajektorije, medtem ko so nihanja njegove me- hanske strukture veˇ cinoma doloˇ cena z vrednostmi trzaja, ˇ casovnega odvoda pospeˇ ska [3]. Da bi zadostili kine- matiˇ cnim omejitvam vozila in uspeˇ sno prevaˇ zali nevarna, lomljiva ali dragocena bremena, mora biti konˇ cna pot glad- ka, izvedljiva pri visokih hitrostih in hkrati neˇ skodljiva za mehanski sistem v smislu izogibanja trzajem in ˇ cezmer- nim pospeˇ skom aktuatorjev [9]. Problem optimizacije hitrostnega profila je doloˇ citi ˇ casovno optimalni zakon hitrosti, ki upoˇ steva doloˇ cene kinematiˇ cne ali dinamiˇ cne omejitve in je bil obravnavan ˇ stevilnih ˇ clankih [6]. Trzaj se uporablja kot dejavnik obli- kovanja za zagotavljanje udobja med voˇ znjo (npr. pri voˇ znjah v zabaviˇ sˇ cnih parkih, v vodnih plovilih, dviga- lih in avtonomnih avtobusih) in nastopa tudi v ISO stan- dardih. Pomen trzaja se utrjuje v ˇ stevilnih znanstvenih in inˇ zenirskih aplikacijah, veliko raziskav pa je posveˇ cenih omejevanju trzaja za zmanjˇ sanje vibracij, pogreˇ skov po- loˇ zaja ali za izboljˇ sanje uˇ cinkovitosti dela obdelovalnih strojev, robotskih manipulatorjev in avtonomnih mobil- nih robotov [7]. ˇ Clanek obravnava problem generiranja ˇ casovno mini- malnih trajektorij za kolesni mobilni sistem, ki se giba v omejenem prostoru brez ovir. Uporabljamo novo meto- dologijo konstrukcije in parametrizacije B´ ezierjevih kri- vulj petega reda, ki sestavljajo nastalo geometrijsko pot. Naˇ sa predstavljena metoda za generiranje ˇ casovno mini- malnih trajektorij je ˇ se posebej primerna za avtonomne mobile sisteme v avtomatiziranih skadiˇ sˇ cih. 2 Definicija problema Naj bo gibanje delca vzdolˇ z trikrat zvezno odvedljive pla- narne krivuljeC opisano kot funkcija ˇ casa t∈ [0,t f ] s pozicijskim vektorjemr(t) z zaˇ cetkom v danem fiksnem izhodiˇ sˇ cu. Vektorje hitrostiv(t), pospeˇ skaa(t) in trzaja j(t) lahko izrazimo v tangentno-normalni obliki kot: v(t) =v(t)· ˆT (1a) a(t) =a T (t)· ˆT+a R (t)· ˆN (1b) j(t) =j T (t)· ˆT+j R (t)· ˆN, (1c) pri ˇ cemer sta ˆT in ˆN enotski tangentni in enotski nor- malni vektor. Naˇ s cilj je razviti in implementirati algoritem za ge- neriranje ˇ casovno minimalnih trajektorij za kolesne mo- bile sisteme v ravninskem prostoru brez ovir, pri ˇ cemer 234 so upoˇ stevane ˇ se naslednje omejitve hitrosti, pospeˇ ska in trzaja: 0 ≤ ∥ v(t)∥ ≤ v MAX ; ∀ t∈ [0,t f ] (2a) a 2 T (t) a 2 TMAX + a 2 R (t) a 2 RMAX ≤ 1; ∀ t∈ [0,t f ] (2b) j 2 T (t) j 2 TMAX + j 2 R (t) j 2 RMAX ≤ 1; ∀ t∈ [0,t f ] (2c) Reˇ sitev iˇ sˇ cemo z optimizacijo potovalnega ˇ casa vzdolˇ z izvedljive poti, ki je sestavljena iz veˇ cih zlepkov. Al- goritem, ki vzdolˇ z danega segmenta poti generira ˇ caso- vno minimalen hitrostni profil z omejitvami hitrosti, po- speˇ ska in trzaja, je sestavljen iz dveh korakov in je po- drobneje predstavljen v [4]: v prvem koraku algoritma se upoˇ stevajo omejitve hitrosti in pospeˇ ska, v drugem ko- raku pa algoritem modificira prvotni hitrostni profil tako, da so upoˇ stevane tudi omejitve trzaja, pri ˇ cemer se posto- pek razlikuje glede na tip krˇ sitve (toˇ ckaste ali intervalne krˇ sitve tangentnega trzaja). 3 Krivuljni primitivi in opis okolja Pri naˇ crtovanju trajektorij smo uporabili B´ ezierove krivu- lje, ki se zaradi ugodnih lastnosti (prilagodljivost, raˇ cunska nezahtevnost) in numeriˇ cne stabilnosti pogosto uporabljajo pri naˇ crtovanju poti mobilnih sistemov. V posamezni iteraciji optimizacije z algoritmom, ki generira ˇ casovno minimalni hitrostni profil, izraˇ cunamo potovalni ˇ cas na dani poti, sestavljeni iz ˇ stirih B´ ezierovih krivulj. Postopek naˇ crtovanja ˇ casovno minimalnih trajek- torij smo prikazali na primeru avtomatiziranega skladiˇ sˇ ca. 3.1 Konstrukcija krivuljnih zlepkov B´ ezierove krivulje doloˇ cajo Bernsteinovi polinomi. N- dimenzionalen Bernsteinov polinomn-tega reda,r n (λ ) : [0,1]→ R N , je definiran kot: r n (λ ) = n X i=0 P i,n B i,n (λ ), λ ∈ [0,1] (3) pri ˇ cemer jeλ normaliziran ˇ cas (0≤ λ ≤ 1),P i,n ∈ R N jei-ta kontrolna toˇ cka inB i,n (λ ) je baza Bernsteinovih polinomov, definirana kot: B i,n (λ ) = n i λ i (1− λ ) n− i , (4) za vsei∈{ 0,...,n } , pri ˇ cemer je n i binomski koefici- ent. Naj boP n = [P 0,n ,..., P n,n ]∈ R N× (n+1) vektor kontrolnih toˇ ck polinomar n (λ ). Bernsteinov polinom v enaˇ cbi(3) lahko tedaj zapiˇ semo v matriˇ cni obliki: r n (λ ) =P n     B 0,n (λ ) B 1,n (λ ) ... B n,n (λ )     (5) B´ ezierove krivulje, ki jih doloˇ ca zelo veliko ˇ stevilo kontrolnih toˇ ck, so numeriˇ cno nestabilne, zato je pri pla- niranju poti zaˇ zeleno konstruirati gladko pot, ki jo sesta- vlja veˇ c B´ ezierovih krivulj nizkega reda. Na spojih po- sameznih krivulj mora biti izpolnjen pogoj zvezne ukriv- ljenosti. Najmanjˇ si red B´ ezierovih krivulj, ki zadosti tej zahtevi jen = 5. B´ ezierova krivulja petega redar 5 (λ ) = [x(λ ),y(λ )] T je definirana s ˇ sestimi kontrolnimi toˇ ckami P i, 5 = [x i ,y i ],i∈{ 0,1,... 5} : r 5 (λ ) = (1− λ ) 5 P 0, 5 +5λ (1− λ ) 4 P 1, 5 +10λ 2 (1− λ ) 3 P 2, 5 +10λ 3 (1− λ ) 2 P 3, 5 +5λ 4 (1− λ )P 4, 5 +λ 5 P 5, 5 (6) Ker ˇ clanek obravnava zgolj B´ ezierove krivulje petega reda, se v nadaljevanju ˇ clanka zaradi preglednosti pri skliceva- nju na kontrolne toˇ cke izpuˇ sˇ ca drugi indeks, ki oznaˇ cuje red krivulj. Za uˇ cinkovito iskanje optimalne trajektorije je zelo pomemben naˇ cin konstrukcije B´ ezierovih krivulj in izbor ustreznih optimizacijskih parametrov. Odvod ukrivljeno- sti doloˇ ca vrednost radialnega trzaja (drugi ˇ clen v spo- dnjem izrazu): j(t) = ¨v− v 3 κ 2 · ˆT+ 1 v d dt v 3 κ · ˆN (7) ki jo vP 0 postavimo na 0 kot smiselno dodatno ome- jitev trzaja na spojih zlepkov. Koordinate posameznih kontrolnih toˇ ckP i oznaˇ cimo z(x i ,y i ) zai∈{ 0,..., 5} . Za i ∈ { 0,..., 4} pa oznaˇ cimo razdalje med zapore- dnimi kontrolnimi toˇ ckami d(P i ,P i+1 ) z d i+1 in kote ∢ (d i ,y = 0) s φ i . Konstrukcija posamezne B´ ezierove krivulje poteka v naslednjih korakih (slika 1): Slika 1: Konstrukcija B´ ezierove krivulje. 1. Zaˇ crtamo prvo kontrolno toˇ cko in jo oznaˇ cimo z P 0 . V smeriφ 1 odmerimo dolˇ zinod 1 in oznaˇ cimo drugo toˇ cko zP 1 . 2. V smeriφ 1 nato odmerimo dolˇ zinod || 2 : d || 2 =d 2 cos(φ 2 − φ 1 ) = 1 2 d 1 (8) 3. V pravokotni smeri nato odmerimo razdaljod ⊥ 2 : d ⊥ 2 = 5 4 d 2 1 κ 0 (9) in tretjo kontrolno toˇ cko oznaˇ cimo sP 2 . 235 4. Vse toˇ cke, ki so odP 2 v isti smeri (pravokotno na daljicoP 0 P 1 ) oddaljene zad ⊥ 3 , oznaˇ cimo s ˇ crtkano ˇ crto: d ⊥ 3 = 5 12 d 2 1 κ ′ 0 (10) 5. Zaˇ crtamo zadnjo kontrolno toˇ cko in jo oznaˇ cimo s P 5 . V smeri kotaφ 5 odmerimo dolˇ zinod 5 in peto kontrolno toˇ cko oznaˇ cimo sP 4 . 6. Vse toˇ cke, ki so odP 4 oddaljene zad ⊥ 4 pravokotno glede na daljicoP 4 P 5 , oznaˇ cimo s ˇ crtkano ˇ crto: d ⊥ 4 = 5 4 d 2 5 κ 5 (11) 7. ˇ Cetrta kontrolna toˇ ckaP 3 leˇ zi na preseˇ ciˇ sˇ cu obeh ˇ crtkanih ˇ crt. B´ ezierova krivulja je sedaj popolnoma definirana. 3.2 Opis avtomatiziranega skladiˇ sˇ ca Zaradi ˇ sirokih tehnoloˇ skih zmogljivosti avtomatsko vo- denih vozil in drugih avtonomnih mobilnih sistemov je moˇ zen razvoj v smeri popolnoma avtomatiziranih skla- diˇ sˇ c. Med obiˇ cajne naloge, ki jih v skladiˇ sˇ cih opravljajo mobilni roboti, so: natovarjanje in raztovarjanje blaga, zlaganje in prevzemanje artiklov, priprava naroˇ cil, sorti- ranje, vodenje popisov in vzdrˇ zevanje skladiˇ sˇ ca. Algoritem za naˇ crtovanje poti smo preizkusili v simu- laciji avtomatiziranega skladiˇ sˇ ca, pri ˇ cemer smo obrav- navali nalogo prevzemanja in zlaganja artiklov. Obiˇ cajno je v skladiˇ sˇ cih prostor zelo omejen in v naˇ sem primeru se mobilni robot lahko premika v okolici treh ravnih vrst skladiˇ sˇ cnih regalov, kjer pobira in odlaga tovor z vnaprej doloˇ cenih mest. Predpostavili smo, da je gibanje med re- gali omejeno na raven tir, medtem ko se oblika poti med prehodi doloˇ ci naknadno z optimizacijo in jo sestavljata dve B´ ezierovi krivulji z nekaterimi prostimi parametri. Na mestih prevzema/vraˇ cila tovora je hitrost mobilnega robota postavljena na niˇ c. Za analizo primera smo izbrali 0 1 2 3 4 5 6 7 8 9 10 11 0 1 2 3 4 5 6 7 8 9 10 pick-up point drop-off point A B A ’ C Slika 2: Tloris skladiˇ sˇ ca z oznaˇ ceno toˇ cko prevzema in toˇ ckami vraˇ cila tovora. tri pare prevzemno-vraˇ cilnih mest (slika 2). Predpisano gibanje mobilnega robota je v smeri urinega kazalca po kroˇ zni poti od mesta prevzema (ang. PUP: pick-up point) do mesta vraˇ cila (ang. DOP: drop-off point) in nato zopet nazaj do izhodiˇ sˇ ca. Mobilni robot je torej natovorjen med gibanjem od toˇ cke prevzema do mesta vraˇ cila, preostali del poti pa je nenatovorjen. Predpostavimo, da mobilni sistem prevaˇ za obˇ cutljiv in dragocen tovor, zato se ome- jitve hitrosti, pospeˇ ska in trzaja (izrazi 2a-2c) (znatno) razlikujejo glede na poloˇ zaj na kroˇ zni poti (tabela 1). tovor vMAX [m/s ] aR MAX [m/s 2 ] aT MAX [m/s 2 ] jR MAX [m/s 3 ] jT MAX [m/s 3 ] × 1.5 4 2 10 8 ✓ 1.5 1 0.5 2.5 2 Tabela 1: Omejitve hitrosti, pospeˇ skov in trzajev za natovorjen in nenatovorjen mobilni sistem. 4 Rezultati simulacij S posamezno optimizacijo, katere reˇ sitev je bila ˇ casovno minimalna trajektorija, smo doloˇ cili hitrostni profil in obli- ko poti za polovico kroˇ zne poti. V toˇ ckah prevzema in vraˇ cila je namreˇ c hitrost vozila postavljena na niˇ c, opti- mizacija pa se tako tudi izvaja na manjˇ sem ˇ stevilu zlep- kov (ne na osmih segmentih, ampak na ˇ stirih). Zaradi si- metriˇ cno (A ′ glede naA) oziroma komplementarno (B in C glede naA) postavljenih toˇ ck prevzema in vraˇ cila to- vora je enostavno doloˇ citi ˇ cas potovanja na celotni kroˇ zni poti. Najprej smo izraˇ cunali ˇ casovno minimalne trajekto- rije za simetriˇ cno postavljeni toˇ cki prevzema in vraˇ cila tovora. Dobljene poti na prvem delu kroˇ zne poti (dva ravna odseka in dve B´ ezierovi krivulji) so za oba primera obremenitve vrisane na tloris skladiˇ sˇ ca (slika 3). 0 1 2 3 4 5 6 7 8 9 10 11 0 1 2 3 4 5 6 7 8 9 10 pick-up point drop-off point A C A ’ B Slika 3: Vrisane poti, ki so rezultat optimizacije ˇ casa potova- nja. Krajˇ sa pot je reˇ sitev v primeru neobremenjenega vozila. Zanimivo bi bilo izvedeti, kakˇ sne - ˇ ce sploh - so raz- like v ˇ casih potovanja, ˇ ce se natovorjeno/nenatovorjeno vozilo pelje po poti, ki ni bila optimizirana za isto obre- menitev. Pot med toˇ cko prevzema in vraˇ cila tovora (in obratno), ki je reˇ sitev ˇ casovno minimalnega naˇ crtovanja trajektorije za simetriˇ cno postavljeni toˇ cki prevzema in vraˇ cila v primeru polne obremenitve, imenujemo N pot (iz ang. no load), pot, ki je reˇ sitev optimizacije v primeru nenatovorjenega vozila, pa F pot (iz ang. full load). Ana- logno oznaˇ cujemo tudi natovorjen in nenatovorjen mo- 236 primer kroˇ zne poti koordinate prevzema koordinate vraˇ cila ˇ cas potovanja PUP→ DOP DOP→ PUP (PUP) (DOP) [s] F agv , N path N agv , N path A(5.5, 3.5) A ′ (5.5, 6.5) 21.998 A(5.5, 3.5) B(2.5, 6.5) 21.477 A(5.5, 3.5) C(8.5, 6.5) 21.985 F agv , F path N agv , F path A(5.5, 3.5) A ′ (5.5, 6.5) 21.523 A(5.5, 3.5) B(2.5, 6.5) 21.342 A(5.5, 3.5) C(8.5, 6.5) 21.519 F agv , F path N agv , N path A(5.5, 3.5) A ′ (5.5, 6.5) 21.052 A(5.5, 3.5) B(2.5, 6.5) 20.859 A(5.5, 3.5) C(8.5, 6.5) 21.035 Tabela 2: Potovalni ˇ casi natovorjenega/raztovorjenega mobilnega sistema (Fagv /Nagv ) na poti, ki je rezultat optimizacije za nato- vorjeno/raztovorjeno vozilo (F path /N path ). bilni sistem: F agv in F agv (iz ang. autonomous guided vehicle). Tabela 2 prikazuje ˇ case potovanja mobilnega sistema vzdolˇ z poti N pot in F pot glede na oba primera obremenitve. Zaradi razliˇ cno postavljenih toˇ ck prevzema in vraˇ cila tovora se primeri razlikujejo po razmerju ˇ casov oziroma dolˇ zine poti, na kateri je mobilni sistem obreme- njen oziroma razbremenjen. Rezultati kaˇ zejo, da so potovalni ˇ casi res najkrajˇ si v primeru, ko se na prvem delu kroˇ zne poti natovorjen mo- bilni sistem vozi po poti, ki je bila optimizirana za nato- vorjeno vozilo, v drugem delu poti pa nenatovorjeno vo- zilo potuje po poti, ki je bila optimizirana za nenatovor- jen mobilni sistem. Demonstracijo delovanja algoritma v skladiˇ sˇ cu bi lahko ˇ se razˇ sirili in jo postavili v zahtevnejˇ se okolje, opravili analize veˇ c setov dinamiˇ cnih omejitev in z veˇ cimi pari prevzemno-vraˇ cilnih mest. 5 Zakljuˇ cek V ˇ clanku je predstavljen algoritem za planiranje ˇ casovno minimalnih trajektorij v okolju brez ovir. Temelji na upo- rabi B´ ezierovih krivulj in upoˇ steva omejitve hitrosti, pos- peˇ ska in trzaja. Naˇ crtovanje trajektorij smo demonstrirali v simulaciji gibanja avtonomnih mobilih sistemov v av- tomatiziranem skladiˇ sˇ cu. Kljub nekaterim omejitvam predstavljene metode ver- jamemo, da bodo rezultati te ˇ studije pripomogli k razi- skavam o avtomatizaciji skladiˇ sˇ c. V prihodnje se bomo ukvarjali z izboljˇ sevanjem robustnosti algoritma ter s pre- uˇ cevanjem njegove raˇ cunske uˇ cinkovitosti. Morda bi se kot ugoden naˇ cin reˇ sevanja izkazala tudi uporaba B´ eziero- vih krivulj viˇ sjega reda, predvsem zaradi vpliva lastnosti krivulj na velikost radialnega trzaja. Zahvala ˇ Clanek je nastal s finanˇ cno podporo Agencije Republike Slovenije za raziskovalno dejavnost (ˇ st. P2-0219). Literatura [1] A. Gasparetto, P. Boscariol, A. Lanzutti, and R. Vi- doni, Motion and Operation Planning of Robotic Systems, vol. 29 of Mechanisms and Machine Science. Cham: Springer International Publishing, 2015. [2] H. Zhang and S. Yang, “Smooth path and velocity plan- ning under 3D path constraints for car-like vehicles,” Ro- botics and Autonomous Systems, vol. 107, pp. 87–99, sep 2018. [3] M. Tsirlin, “Jerk by axes in motion along a space curve,” Journal of Theoretical and Applied Mechanics (Poland), vol. 55, no. 4, pp. 1437–1441, 2017. [4] M. Benko Loknar, S. Blaˇ ziˇ c, and G. Klanˇ car, “Minimum- time velocity profile planning for planar motion conside- ring velocity, acceleration and jerk constraints,” Internati- onal Journal of Control, pp. 1–15, oct 2021. [5] A. Zdeˇ sar and I. ˇ Skrjanc, “Optimum Velocity Profile of Multiple Bernstein-B´ ezier Curves Subject to Constraints for Mobile Robots,” ACM Transactions on Intelligent Sy- stems and Technology, vol. 9, no. 5, pp. 1–23, 2018. [6] P. F. Lima, M. Trincavelli, J. Martensson, and B. Wahl- berg, “Clothoid-Based Speed Profiler and Control for Au- tonomous Driving,” IEEE Conference on Intelligent Tran- sportation Systems, Proceedings, ITSC, vol. 2015-Octob, pp. 2194–2199, 2015. [7] R. Sato and K. Shirase, “Analytical time constant de- sign for jerk-limited acceleration profiles to minimize re- sidual vibration after positioning operation in NC machine tools,” Precision Engineering, vol. 71, no. March, pp. 47– 56, 2021. [8] H. Y . Zhang, W. M. Lin, and A. X. Chen, “Path planning for the mobile robot: A review,” Symmetry, vol. 10, no. 10, 2018. [9] A. Ravankar, A. Ravankar, Y . Kobayashi, Y . Hoshino, and C.-C. Peng, “Path Smoothing Techniques in Robot Navi- gation: State-of-the-Art, Current and Future Challenges,” Sensors, vol. 18, p. 3170, sep 2018. [10] M. Ghazaei, A. Robertsson, and R. Johansson, “Online Minimum-Jerk Trajectory Generation,” Proc. IMA Conf. Mathematics of Robotics, 2015. [11] C. M. Kang, S.-H. Lee, and C. C. Chung, “On-Road Path Generation and Control for Waypoints Tracking,” IEEE Intelligent Transportation Systems Magazine, vol. 9, no. 3, pp. 36–45, 2017.