ˇ ´ vysoke ´ uc ˇen´ı technicke ´ v Praze Cesk e ´ Fakulta elektrotechnicka
´ RSK ˇ ´ PRACE ´ BAKALA A Pl´ anov´ an´ı hladk´ e trajektorie pro mobiln´ı roboty
Praha, 1.6.2009
Autor: Petr Beneˇ s
Prohl´ aˇ sen´ı Prohlaˇsuji, ˇze jsem svou bakal´aˇrskou pr´aci vypracoval samostatnˇe a pouˇzil jsem pouze podklady (literaturu, projekty, SW atd.) uveden´e v pˇriloˇzen´em seznamu.
V Praze dne podpis
i
Podˇ ekov´ an´ı Dˇekuji vedouc´ımu pr´ace Michalu Sojkovi a cel´emu t´ ymu CTU Dragons za moˇznost pod´ılet se na zaj´ımav´em projektu.
ii
Abstrakt C´ılem t´eto pr´ace je analyzovat moˇznosti pl´anov´an´ı pohybu autonomn´ıho mobiln´ıho robota po spojit´ ych kˇrivk´ach, vybrat jednu z moˇznost´ı, kter´a nejv´ıce odpov´ıd´a poˇzadavk˚ um a omezen´ım spojen´ ym s fyzick´ ymi parametry robota a ˇcasovou n´aroˇcnost´ı pouˇzit´ ych v´ ypoˇcetn´ıch algoritm˚ u. Tyto algoritmy jsou n´aslednˇe modelov´any v prostˇred´ı Matlab a nakonec implementov´any jako knihovna v jazyce C++. Integrac´ı t´eto knihovny do cel´eho projektu jsme dos´ahnuli hezˇc´ıho, plynulejˇs´ıho pohybu, a tak´e moˇznosti sn´ıˇzen´ı doby pr˚ ujezdu trajektori´ı, coˇz pˇrispˇeje k vˇetˇs´ımu poˇctu man´evr˚ u, kter´e robot stihne bˇehem hrac´ı doby.
iii
Abstract The aim of this work is to analyze possibilities of mobile autonomous robot trajectory planning using smooth curves, choose one of the options which fits the most to requests and restrictions related to the physical parameters of robot and time complexity of utilised algorithms. Consequently, these algorithms are simulated in Matlab and finally implemented as a C++ library. Thanks to the library integration we gained a more fluent motion and a possibility to lower the pass period. This contributes to increased number of movements during the game period.
iv
Obsah Seznam obr´ azk˚ u
vii
Seznam tabulek
ix
´ 1 Uvod
1
2 Parametry robota a hˇ riˇ stˇ e
3
2.1
Hriˇstˇe pro soutˇeˇz Eurobot . . . . . . . . . . . . . . . . . . . . . . . . . .
3
2.2
Model Robota . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3
3 P˚ uvodn´ı funkce pl´ anov´ an´ı pohybu 3.1
7
Mapa hˇriˇstˇe . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7
3.1.1
Struktura mapy . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7
3.1.2
Informace v buˇ nk´ach . . . . . . . . . . . . . . . . . . . . . . . . .
8
3.1.3
Vkl´ad´an´ı objekt˚ u do mapy . . . . . . . . . . . . . . . . . . . . . .
10
3.2
Hled´an´ı cesty . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
11
3.3
Pl´anov´an´ı trajektorie . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
11
3.4
Vyh´ yb´an´ı se pˇrek´aˇzk´am . . . . . . . . . . . . . . . . . . . . . . . . . . .
12
4 Spliny
13
4.1
Symbolika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
13
4.2
Motivace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
14
4.3
Druhy spojit´ ych kˇrivek . . . . . . . . . . . . . . . . . . . . . . . . . . . .
16
4.3.1
B´ezierovy kˇrivky tˇret´ıho ˇra´du . . . . . . . . . . . . . . . . . . . .
16
4.3.2
Aproximaˇcn´ı polynomi´aln´ı kˇrivka . . . . . . . . . . . . . . . . . .
18
4.3.3
Klotoida . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
20
V´ ybˇer konkr´etn´ı kˇrivky . . . . . . . . . . . . . . . . . . . . . . . . . . . .
23
4.4
v
5 Generov´ an´ı trajektorie
25
5.1
Popis kˇrivky . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
25
5.2
Tvarov´an´ı kˇrivky jako klotoidy
. . . . . . . . . . . . . . . . . . . . . . .
28
5.3
Napojen´ı jednotliv´ ych segment˚ u . . . . . . . . . . . . . . . . . . . . . . .
32
6 Kinematika robota
35
6.1
Pr˚ ubˇeh rychlosti na pˇr´ımoˇcar´em u ´seku . . . . . . . . . . . . . . . . . . .
36
6.2
Pr˚ ubˇeh rychlosti na splinu . . . . . . . . . . . . . . . . . . . . . . . . . .
36
6.3
Maxim´aln´ı rychlosti segment˚ u . . . . . . . . . . . . . . . . . . . . . . . .
37
6.4
Zajiˇstˇen´ı spojitosti rychlost´ı . . . . . . . . . . . . . . . . . . . . . . . . .
38
7 Algoritmus vyh´ yb´ an´ı se pˇ rek´ aˇ zk´ am
41
8 Z´ avˇ er
43
Literatura
45
A Obsah pˇ riloˇ zen´ eho CD
I
vi
Seznam obr´ azk˚ u 2.1
Hˇriˇstˇe pro Eurobot 2009 – Chr´amy atlantidy . . . . . . . . . . . . . . . .
4
2.2
P˚ udorys robota . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5
2.3
DragonBot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6
3.1
Reprezentace mapy v prostˇred´ı Robomon . . . . . . . . . . . . . . . . . .
8
3.2
Pˇr´ıklad konkr´etn´ı situace v mapˇe . . . . . . . . . . . . . . . . . . . . . .
10
3.3
Pouˇzit´ı kruˇznicov´ ych oblouk˚ u na proloˇzen´ı zat´aˇcek . . . . . . . . . . . . .
12
4.1
Nespojit´a rychlost koleˇcek na kruˇznicov´em oblouku . . . . . . . . . . . .
15
4.2
Odchylka od pˇr´ımoˇcar´e trajektorie . . . . . . . . . . . . . . . . . . . . . .
16
4.3
Bezierova kˇrivka tˇret´ıho ˇr´adu . . . . . . . . . . . . . . . . . . . . . . . .
17
4.4
Moˇzn´e vzhledy bezierovy kˇrivky . . . . . . . . . . . . . . . . . . . . . . .
18
4.5
Interpolace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
19
4.6
Aproximace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
19
4.7
D´alniˇcn´ı kˇriˇzovatka sloˇzen´a z u ´sek˚ u klotoid . . . . . . . . . . . . . . . . .
21
4.8
Roller Coaster . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
21
4.9
Klotoida . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
22
4.10 Zat´aˇcka sloˇzen´a ze dvou ˇca´st´ı klotoidy . . . . . . . . . . . . . . . . . . .
22
5.1
V´ yznam symbol˚ u v zat´aˇcce . . . . . . . . . . . . . . . . . . . . . . . . . .
27
5.2
Pr˚ ubˇeh kˇrivosti klotoidy a n´ami generovan´e kˇrivky . . . . . . . . . . . . .
29
5.3
Optimalizovan´ y parametr m v z´avislosti na u ´hlu . . . . . . . . . . . . . .
30
5.4
Optimalizovan´ y parametr m v z´avislosti na mal´ ych u ´hlech . . . . . . . .
31
5.5
Trajektorie se spliny, nebere v u ´vahu omezen´ı emax
. . . . . . . . . . . .
33
5.6
Z´avislost vzd´alenosti e na u ´hlu γ pˇri konstantn´ım d . . . . . . . . . . . .
33
5.7
V´ ysledn´a trajektorie . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
34
6.1
Pr˚ ubˇeh rychlosti po pˇrid´an´ı zrychlen´ı mezi segmenty
. . . . . . . . . . .
39
6.2
Pr˚ ubˇeh rychlosti po pˇrid´an´ı zpomalen´ı mezi segmenty . . . . . . . . . . .
39
vii
7.1
Pˇrepl´anov´an´ı trajektorie za j´ızdy . . . . . . . . . . . . . . . . . . . . . .
viii
42
Seznam tabulek 3.1
Tabulka pˇr´ıznak˚ u pro buˇ nky v mapˇe a jejich barevn´a reprezentace v prostˇred´ı Robomon . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9
4.1
Obecn´e podm´ınky pro navazov´an´ı segment˚ u trajektorie . . . . . . . . . .
20
6.1
Kinematick´e parametry pˇr´ımoˇcar´eho u ´seku . . . . . . . . . . . . . . . . .
36
6.2
Kinematick´e parametry splinu . . . . . . . . . . . . . . . . . . . . . . . .
37
ix
x
Kapitola 1 ´ Uvod Na Katedˇre ˇr´ıdic´ı techniky FEL je vyv´ yjen autonomn´ı mobiln´ı robot, na kter´em pracuj´ı pˇredevˇs´ım studenti za u ´ˇcelem u ´ˇcasti na soutˇeˇz´ıch. Tento robot se ˇcasem zdokonaluje a pˇrib´ yvaj´ı mu nov´e souˇca´sti. Jeden ze z´akladn´ıch prvk˚ u je pl´anov´an´ı trajektorie, kter´e bylo potˇreba vylepˇsit o j´ızdu po spojit´ ych kˇrivk´ach. V kapitole 2 pˇribl´ıˇz´ım zm´ınˇenou soutˇeˇz a pˇredstav´ım robota. V kapitole 3 pop´ıˇsi nˇekter´e souvisej´ıc´ı souˇca´sti, kter´e jiˇz v robotu funguj´ı a budu s nimi spolupracovat nebo je mˇenit. Kapitola 4 uk´aˇze, jak´e spojit´e kˇrivky je moˇzn´e pouˇz´ıt a proˇc. Od kapitoly 5 se jedn´a o mou vlastn´ı pr´aci, v t´eto kapitole (5) se zab´ yv´am t´ım, jak vygenerovat n´ami poˇzadovanou kˇrivku a pak i celou trajektorii, dalˇs´ı kapitola 6 je o zp˚ usobu pr˚ ujezdu robota trajektori´ı. V posledn´ı kapitole 7 je algoritmus vyh´ yb´an´ı se pˇrek´aˇzk´am s pouˇzit´ım algoritm˚ u pˇredchoz´ıch. Pr´aci konˇc´ım z´avˇerem v kapitole 8
1
2
´ KAPITOLA 1. UVOD
Kapitola 2 Parametry robota a hˇ riˇ stˇ e Pˇrestoˇze je robot navrˇzen tak, aby byl variabiln´ı a pouˇziteln´ y pro r˚ uzn´e druhy ˇcinnost´ı a obsahuje spoustu fukcionalit, kter´e drobn´ ymi upravami mohou vytvoˇrit nov´ y funkˇcn´ı celek, jeho hlavn´ı u ´lohou je v souˇcasn´e dobˇe u ´ˇcast na soutˇeˇz´ıch Eurobot. Na u ´vod uk´aˇzi jak vypad´a hˇriˇstˇe, jeho parametry, robota a jeho fyzik´aln´ı model, aby bylo z ˇceho vych´azet a na co se odkazovat v n´asleduj´ıc´ıch kapitol´ach.
2.1
Hriˇ stˇ e pro soutˇ eˇ z Eurobot
ˇ eho kola Eurobot [online], 2009) se poˇr´ad´a jedSoutˇeˇz Eurobot (Webov´e str´anky Cesk´ nou roˇcnˇe a pokaˇzd´e m´a jin´e zad´an´ı, tud´ıˇz je potˇreba vytvoˇrit nov´e manipulaˇcn´ı schopnosti. Pˇrestoˇze hˇriˇstˇe pokaˇzd´e vypad´a trochu jinak a tak´e rozloˇzen´ı hern´ıch prvk˚ u je r˚ uzn´e, co se t´ yˇce rozmˇer˚ u je vˇzdy obd´eln´ıkov´eho p˚ udorysu o rozmˇerech 300 × 210cm. Algoritmy robota mohou tedy poˇc´ıtat s relativnˇe mal´ ym, omezen´ ym hˇriˇstˇem, na kter´em je rozm´ıstˇeno nˇekolik hern´ıch prvk˚ u a soupeˇr, kter´ y m˚ uˇze br´anit v pohybu nebo zapˇr´ıˇcinit kolizi.
2.2
Model Robota
Samotn´ y robot je obd´eln´ıkov´eho p˚ udorysu, m´a dvˇe hnan´a kola a dvˇe mal´a vleˇcen´a kuliˇckov´a koleˇcka. Pro nˇekter´e kinematick´e u ´lohy, kdy je potˇreba br´at v u ´vahu rozmˇery, m´a tˇri stupnˇe volnosti. Pro jin´e u ´lohy, kdy je potˇreba robota vidˇet pouze jako hmotn´ y 3
4
ˇ ST ˇ E ˇ KAPITOLA 2. PARAMETRY ROBOTA A HRI
Obr´ azek 2.1: Hˇriˇstˇe pro Eurobot 2009 – Chr´amy atlantidy
bod, uvaˇzujeme jen dva stupnˇe volnosti (souˇradnice x a y). Tento hmotn´ y bod je stˇredem robota Srob , kter´ y je um´ıstˇen ve stˇredu osy hnan´ ych kol. Pokud oznaˇc´ıme a jako ˇs´ıˇrku a b jako d´elku robota (viz obr. 2.2), d´ale q jako ˇs´ıˇrku zapuˇstˇen´ ych kol, pak polomˇer ot´aˇcen´ı robota je rrot =
a−q 2
(2.1)
2.2. MODEL ROBOTA
5
b
a
S_rob
r_rot
Obr´azek 2.2: P˚ udorys robota
q
6
ˇ ST ˇ E ˇ KAPITOLA 2. PARAMETRY ROBOTA A HRI
Obr´ azek 2.3: DragonBot
Kapitola 3 P˚ uvodn´ı funkce pl´ anov´ an´ı pohybu Nyn´ı pˇredstav´ım algoritmy a funkcionality, o kter´e doposud op´ıralo pl´anov´an´ı pohybu (Webov´e str´anky CTU Dragons [online], 2009) a kter´e jsem jen nepatrnˇe poupravil, aby vyhovovaly poˇzadavk˚ um na integraci s d´ale zm´ınˇen´ ymi ˇca´stmi.
3.1
Mapa hˇ riˇ stˇ e
Jak jiˇz jsem zm´ınil, na hˇriˇsti se nach´az´ı mnoho hern´ıch prvk˚ u, se kter´ ymi je tˇreba poˇc´ıtat, pokud chceme napl´anovat trajektorii. Pokud polohy tˇechto prvk˚ u zjist´ıme, nebo je zn´ame jiˇz pˇred soutˇeˇz´ı, zaznamen´ame je do mapy. Z t´eto mapy potom ˇcerp´ame informace o poloze jednotliv´ ych objekt˚ u. Pomoc´ı programu Robomon (obr. 3.1) s grafick´ ym uˇzivatelsk´ ym rozhran´ım m˚ uˇzeme sledovat bˇehem testov´an´ı polohu robota i objekt˚ u v mapˇe.
3.1.1
Struktura mapy
Mapa je reprezentov´ana 2D mˇr´ıˇzkou, kter´a dˇel´ı hˇriˇstˇe na jednotliv´e buˇ nky. Souˇcasn´a velikost buˇ nek je 10 × 10cm, coˇz pˇri rozmˇerech hˇriˇstˇe 300 × 210cm tvoˇr´ı pole o rozmˇeru 30 × 21 bunˇek. Kaˇzd´a z bunˇek nese soubor informac´ı o tom, jestli je do n´ı m˚ uˇze robot vjet (myˇsleno stˇredem Srob ). Nˇekter´e z tˇechto informac´ı m˚ uˇzeme nastavovat ruˇcnˇe, nˇekter´e se dynamicky mˇen´ı podle informac´ı ze senzor˚ u bˇehem hry. 7
˚ ´I FUNKCE PLANOV ´ ´ ´I POHYBU KAPITOLA 3. PUVODN AN
8
Obr´ azek 3.1: Reprezentace mapy v prostˇred´ı Robomon
3.1.2
Informace v buˇ nk´ ach
Kaˇzd´a buˇ nka je reprezentov´ana strukturou MapCell typedef struct map_cell { map_cell_flag_t flags; map_cell_detobst_t detected_obstacle; /**< 0 = no obstacle, 255 = new obstacle */ } MapCell; a cel´e hˇriˇstˇe tedy struct map { MapCell cells[MAP_HEIGHT][MAP_WIDTH]; }; Jednotliv´e bity v promˇenn´e flags ukazuj´ı status buˇ nky. V prostˇred´ı Robomon vid´ıme jednotliv´e statusy jako odliˇsn´e barvy bunˇek na hˇriˇsti. V souˇcasn´e dobˇe je k dispozici tato mnoˇzina pˇr´ıznak˚ u: Jejich definice je pak:
ˇ ST ˇ E ˇ 3.1. MAPA HRI
9
Tabulka 3.1: Tabulka pˇr´ıznak˚ u pro buˇ nky v mapˇe a jejich barevn´a reprezentace v prostˇred´ı Robomon
N´azev pˇr´ıznaku
V´ yznam pˇr´ıznaku
Barva v prostˇred´ı Robomon
MAP_FLAG_WALL
Zed’
tmavˇe ˇzlut´a
MAP_FLAG_PATH
Trajektorie
hnˇed´a
MAP_FLAG_START
Zaˇc´atek trajektorie
ˇcerven´a
MAP_FLAG_GOAL
Konec trajektorie
zelen´a
MAP_FLAG_DET_OBST
Pˇrek´aˇzka ve hˇre
azurov´a
MAP_FLAG_SIMULATED_WALL
Simulovan´a zed’ v Robomonu
ˇzlut´a
MAP_FLAG_IGNORE_OBST
Ignorov´an´ı detekovan´e pˇrek´aˇzky
tmavˇe zelen´a
MAP_FLAG_PLAN_MARGIN
Bezpeˇcnostn´ı vzd´alenost kolem pˇrek´aˇzky –
MAP_FLAG_INVALIDATE_WALL Zruˇsen´ı existuj´ıc´ı zdi
#define MAP_FLAG_WALL
1
#define MAP_FLAG_PATH
2
#define MAP_FLAG_START
4
#define MAP_FLAG_GOAL
8
#define MAP_FLAG_DET_OBST
16
#define MAP_FLAG_SIMULATED_WALL
32
#define MAP_FLAG_IGNORE_OBST
64
#define MAP_FLAG_PLAN_MARGIN
128
#define MAP_FLAG_INVALIDATE_WALL
256
pr˚ uhledn´a
Promˇenn´a detected_obstacle obsahuje ˇc´ıslo od 0 do 255, kter´e signalizuje, pˇred jakou dobou byla na buˇ nce detekov´ana pˇrek´aˇzka. Je nutn´e si pˇrek´aˇzku pamatovat nˇejakou dobu, neˇz ji prohl´as´ıme za zmizelou, abychom odfiltrovali rychl´e zmˇeny dat ze senzor˚ ua zajistili jakousi setrvaˇcnost, kter´a souvis´ı s fyzik´aln´ı podstatou pˇrek´aˇzky. V naˇsem pˇr´ıpadˇe se jedn´a o robota soupeˇre. Po detekci je nastavena hodnota 255 a ˇcasem kles´a aˇz na 0, pot´e je z mapy pˇrek´aˇzka odstranˇena. Flag MAP_FLAG_PLAN_MARGIN jsem jiˇz pˇridal. Z praktick´ ych pokus˚ u se uk´azalo, ˇze pokud se napl´anuje cesta kolem robota v jeho nejtˇesnˇejˇs´ı bl´ızkosti, mohlo by se st´at, ˇze do m´ı v nejbliˇzˇs´ı dobˇe soupeˇr vjede, a to bud’ opravdu, nebo tuto situaci m˚ uˇze zp˚ usobit zaˇsumˇel´a informace ze senzor˚ u. Doch´az´ı tak k oscilac´ım a zbyteˇcn´emu pˇrepl´anov´av´an´ı
˚ ´I FUNKCE PLANOV ´ ´ ´I POHYBU KAPITOLA 3. PUVODN AN
10
Obr´ azek 3.2: Pˇr´ıklad konkr´etn´ı situace v mapˇe
trajektorie. Tento pˇr´ıznak vytvoˇr´ı jakousi ochrannou z´onu v okol´ı robota, kter´a se bere v u ´vahu pouze pˇri pl´anov´an´ı trajektorie, ale nikoli pˇri detekci kolize se soupeˇrem. Flag MAP_FLAG_INVALIDATE_WALL byl pˇrid´an tak, abychom se mohli pˇribl´ıˇzit a dotknout zdi, pokud to bˇehem hry potˇrebujeme.
3.1.3
Vkl´ ad´ an´ı objekt˚ u do mapy
Abychom samozˇrejmˇe nemuseli vˇzdy plnit flagy na hˇriˇsti po jednotliv´ ych buˇ nk´ach, byly pˇrid´any funkce, kter´e zjednoduˇs´ı pr´aci a vedou k intuitivnˇejˇs´ımu ovl´ad´an´ı pozice detekovan´ ych objekt˚ u na hˇriˇsti. Pˇredpokl´adejme, ˇze objekty, kter´e chceme do mapy zaznaˇcit, jsou obd´eln´ıkov´eho nebo kruhov´eho p˚ udorysu, pˇr´ıpadnˇe je lze z tˇechto u ´tvar˚ u poskl´adat. M´ame tedy k dispozici dvˇe funkce, kter´ ymi m˚ uˇzeme na urˇcitou plochu zapnout ˇci vypnout urˇcit´ y pr´ıznak. Tato plocha se mus´ı vhodnˇe pˇrepoˇc´ıtat na mnoˇzinu bunˇek. Nastaven´ı pˇr´ıznak˚ u na obd´eln´ıkov´em objektu zaˇr´ıd´ı funkce int ShmapSetRectangleFlag(double x1, double y1, double x2, double y2, map_cell_flag_t set_flags, map_cell_flag_t clear_flags);
´ ´I CESTY 3.2. HLEDAN
11
a nastaven´ı pˇr´ıznak˚ u na kruhov´em objektu
int ShmapSetCircleFlag(double xs, double ys, double r, map_cell_flag_t set_flags, map_cell_flag_t clear_flags);
3.2
Hled´ an´ı cesty
M´ame-li vˇsechny potˇrebn´e informace v mapˇe, m˚ uˇzeme pˇristoupit k hled´an´ı cesty. Algoritmus A-star, po zad´an´ı dvou bod˚ u (start, c´ıl), nal´ezne nejkratˇs´ı cestu. K tomu n´am pom˚ uˇze opˇet mˇr´ıˇzkou rozdˇelen´a mapa (obr. 3.1). Nalezen´a cesta uspoˇr´adan´ y seznam bunˇek, kter´e pˇrevedeme na body, stˇredy tˇechto bunˇek. Nyn´ı m´ame cestu poskl´adanou z mnoha bod˚ u, kter´e jsou od sebe navz´ajem vzd´aleny maxim´alnˇe d´elku u ´hlopˇr´ıˇcky buˇ nky. S t´ım bychom mohli b´ yt relativnˇe spokojeni, ale pro zjednoduˇsen´ı jeˇstˇe nˇekter´e redundandn´ı body odebereme. A to tak, ˇze pokud zjist´ıme, ˇze v´ıce bod˚ u za sebou tvoˇr´ı u ´seˇcku, nebo je lze proloˇzit u ´seˇckou, od n´ıˇz jsou tyto body odch´ yleny jen do urˇcit´e mal´e vzd´alenosti, odebereme vˇsechny vnitˇrn´ı body u ´seˇcky a ponech´ame pouze body krajn´ı. T´ım se sn´ıˇz´ı poˇcet pamatovan´ ych bod˚ u, aniˇz by se zmˇenila trajektorie, kterou bychom vytvoˇrili z pˇr´ımoˇcar´ ych segment˚ u. Pokud se ale budeme d´ale snaˇzit o vytvoˇren´ı hladk´e trajektorie, bude se hodit menˇs´ı poˇcet bod˚ u pr˚ ujezdu.
3.3
Pl´ anov´ an´ı trajektorie
Dosud pouˇz´ıvan´a metoda tvoˇren´ı trajektori´ı byla skl´ad´an´ı pˇr´ımoˇcar´ ych segment˚ u a kruˇznicov´ ych oblouk˚ u, kter´ ymi byly prokl´ad´any ostr´e rohy, aby robot nemusel zastavovat v zat´aˇck´ach. Co se t´ yˇce stanoven´ı pr˚ ubˇehu rychlost´ı pod´el trajektorie, algoritmus pop´ıˇsi v kapitole 5, jelikoˇz se pˇr´ıliˇs nemˇenil.
12
˚ ´I FUNKCE PLANOV ´ ´ ´I POHYBU KAPITOLA 3. PUVODN AN
Obr´ azek 3.3: Pouˇzit´ı kruˇznicov´ ych oblouk˚ u na proloˇzen´ı zat´aˇcek
3.4
Vyh´ yb´ an´ı se pˇ rek´ aˇ zk´ am
Pokud robot objev´ı v libovoln´em bodˇe trajektorie pˇrek´aˇzku, je potˇreba se j´ı vyhnout ˇ sen´ı bylo dosud takov´e, ˇze se v tomto pˇr´ıpadˇe robot zastavil, a trajektorii pˇrepl´anovat. Reˇ a pot´e zaˇcal poˇc´ıtat novou trajektorii. Tento pˇr´ıstup je dosti benevolentn´ı vzhledem k ˇcasov´ ym omezen´ım a bude tud´ıˇz v dalˇs´ıch kapitol´ach tak´e pˇredmˇetem u ´prav.
Kapitola 4 Spliny Tato kapitola se zab´ yv´a r˚ uzn´ ymi druhy kˇrivek, kter´e se daj´ı pouˇz´ıt jako trajektorie pro robota. Spline je spojit´a kˇrivka, m´a tedy sloˇzitˇejˇs´ı matematick´ y popis neˇz napˇr´ıklad pˇr´ımka nebo kruˇznice. Na druhou stranu existuje velk´e mnoˇzstv´ı tvar˚ u, kter´ ych m˚ uˇzeme ˇka, J., dos´ahnout. O spojit´e kˇrivky maj´ı z´ajem i jin´e podobn´e robotick´e aplikace (Trdlic 2005). Nejprve bych r´ad zm´ınil, ˇze existuje v´ıce pˇr´ıstup˚ u k ˇr´ızen´ı pohybu robota. Jednou moˇznost´ı je kvalitn´ı regul´ator s velmi omezen´ ym akˇcn´ım z´asahem, kter´ y bude pouze sledovat c´ılov´ y bod jako referenci. Tuto moˇznost zavrhneme, protoˇze pˇri pouˇzit´ı t´eto metody je sloˇzit´e predikovat polohu robota v budoucnosti. Dalˇs´ı moˇznost´ı je souˇcasn´e pl´anov´an´ı trajektorie a pr˚ ubˇehu rychlosti viz (Choset et al., 2005). Asi je zˇrejm´e, ˇze nejjednoduˇsˇs´ı bude postupovat smˇerem dˇelen´eho pl´anov´an´ı trajektorie a rozdˇelov´an´ı rychlost´ı, jelikoˇz tato filozofie je jiˇz v robotu implementov´ana, bude tedy nejjednoduˇsˇs´ı ji integrovat s ostatn´ımi souˇca´stmi.
4.1
Symbolika
V t´eto kapitole budeme pouˇz´ıvat mnoˇzstv´ı geometrick´ ych symbol˚ u, kter´e bych nejprve r´ad ujasnil. • P N T je mnoˇzina bod˚ u pr˚ ujezdu, kter´e jsou vstupem do algoritmu pl´anov´an´ı trajektorie. Skl´ad´a se ze z x-ov´e a y-ov´e sloˇzky P N Tx a P N Ty . • nejvˇetˇs´ı mnoˇzinou je mnoˇzina bod˚ u trajektorie T RAJ 13
14
KAPITOLA 4. SPLINY • trajektorie T RAJ se skl´ad´a ze segment˚ u SEG, pˇriˇcemˇz plat´ı, ˇze SEGi je segment, kter´ y n´asleduje po segmentu SEGi−1 . Skl´ad´a se ze z x-ov´e a y-ov´e sloˇzky SEGx a SEGy . Po sv´e d´elce je parametrizov´an parametrem t ∈ h0; 1i • symbol SEG0 znamen´a derivaci x-ov´e a y-ov´e sloˇzky SEGx a SEGy podle parametru • notace |P N Ti P N Ti+1 | znamen´a vzd´alenost dvou bod˚ u
4.2
Motivace
D˚ uvod, proˇc je v˚ ubec potˇreba nasadit spojit´e kˇrivky m´ısto kruˇznicov´ ych oblouk˚ u, je ten, ˇze mus´ıme splnit urˇcit´a krit´eria, jak mohou segmenty na sebe navazovat. Mus´ı tedy pro vˇsechny segmenty SEGi , kde i ∈ 0, 1, 2, · · · , n platit: • SEGi (1) = SEGi+1 (0) • SEG0i (1) = SEG0i+1 (0) • SEG00i (1) = SEG00i+1 (0) Nyn´ı je tˇreba, abych zm´ınil pojem kˇrivosti kˇrivky (Wikipedia [online], 2009), jenˇz n´as bude ˇcasto prov´azet touto kapitolou. Kˇrivost kˇrivky v bodˇe je pˇrevr´acen´a hodnota polomˇeru kˇrivosti v urˇcit´em bodˇe. Pro parametricky zadan´e kˇrivky lze vyj´adˇrit vztahem 1 |x0 (t)y 00 (t) − y 0 (t)x00 (t)| κ= = , R (x02 (t) + y 02 (t))3/2
(4.1)
kde R je polomˇer kˇrivosti kˇrivky, x(t) pr˚ ubˇeh x-ov´e a y(t) y-ov´e souˇradnice v z´avislosti na parametru t. Pˇri pouˇzit´ı kruˇznic sice segmenty trajektorie splˇ nuj´ı prvn´ı dva body, navazuj´ı tedy v teˇcn´ach (tj. prvn´ı derivace), ale ne uˇz v kˇrivosti (z´avis´ı na prvn´ı i druh´e derivaci). To znamen´a, ˇze pokud se pohybujeme po takov´e trajektorii, pˇr´ımoˇcar´ y u ´sek m´a nulovou kˇrivost a kruˇznicov´ y oblouk m´a kˇrivost konstantn´ı nenulovou (obr. 3.3). Pokud se v tomto bodˇe robot octne, mus´ı jedno z jeho koleˇcek nekoneˇcnˇe zrychlit a druh´e nekoneˇcn´e zpomalit. Pro pˇredstavu uvedu pˇr´ıklad (obr. 4.1), kdy robot pˇrej´ıˇzd´ı z pˇr´ımoˇcar´eho segmentu SEG0 na kruˇznicov´ y oblouk SEG1 o polomˇeru r. Stˇred robota S se pohybuje konstantn´ı dopˇrednou rychlost´ı v. Bˇehem segmentu SEG0 se obˇe kola pohybuj´ı dopˇrednou rychlost´ı
4.2. MOTIVACE
15
v_1R
v_1L
v_0
v
v_0
v_1R v_0
v_1L
0
s
Obr´ azek 4.1: Nespojit´a rychlost koleˇcek na kruˇznicov´em oblouku
v0L = v0P = v. Bˇehem segmentu SEG1 se lev´e kolo pohybuje rychlost´ı v1L = v(r − rrot ) a prav´e kolo v1R = v(r +rrot ). V bodˇe SEG0 (1) = SEG1 (0) mus´ı doj´ıt tedy k nekoneˇcn´emu zrychlen´ı/zpomalen´ı koleˇcka robota. To m˚ uˇze zp˚ usobit nepˇekn´e cuk´an´ı robota, a pokud se snaˇz´ıme urˇcovat svou polohu pomoc´ı odometrie (mˇeˇren´ı ot´aˇcek kol pomoc´ı inkrement´aln´ıch sn´ımaˇc˚ u), koleˇcka mohou podkluzovat a lokalizace bude dosti nepˇresn´a. Jedno z dalˇs´ıch omezen´ı, na kter´a si mus´ıme d´at pozor je, ˇze pokud chceme iterpolovat body trajektorie nˇejakou automaticky vygenerovanou kˇrivkou, odchyluje se urˇcit´ ym zp˚ usobem od trajektorie pˇr´ımoˇcar´e. Tuto vzd´alenost mus´ıme umˇet kontrolovat, jinak bychom nevˇedˇeli, jestli robot nekoliduje s nˇejakou pˇrek´aˇzkou. Na obr´azku je uk´azka moˇzn´eho pr˚ ujezdu zadan´ ymi body a maxim´aln´ı odchylka od pˇr´ımoˇcar´e trajektorie mezi dvˇema body.
16
KAPITOLA 4. SPLINY
Kubicky spline z Matlabu
3.5
3
2.5 y
dev
2
1.5
1 0.5
1
1.5
2
2.5
3
x
Obr´ azek 4.2: Odchylka od pˇr´ımoˇcar´e trajektorie
4.3
Druhy spojit´ ych kˇ rivek
Nyn´ı pˇredstav´ım moˇznosti, kter´ ymi jsem se zab´ yval bˇehem reˇserˇse. V r˚ uzn´ ych praktick´ ych oborech se pouˇz´ıvaj´ı r˚ uzn´e popisy 2D kˇrivek, kter´e pro n´as maj´ı urˇcit´e v´ yhody a nev´ yhody.
4.3.1
B´ ezierovy kˇ rivky tˇ ret´ıho ˇ r´ adu
B´ezierovy kˇrivky jsou nejˇcastˇeji vyuˇz´ıvan´e hladn´e kˇrivky v poˇc´ıtaˇcov´e grafice. Jsou vyj´adˇreny parametricky pomoc´ı polynom˚ u tˇret´ıho ˇra´du. Px (t) = Ax t3 + Bx t2 + Cx t + Dx
(4.2)
Py (t) = Ay t3 + By t2 + Cy t + Dy kde t ∈ h0; 1i. Pokud si uvˇedom´ıme praktick´ y v´ yznam nˇekter´ ych parametr˚ u, m˚ uˇzeme jednoduˇse kˇrivku pˇresouvat a tvarovat. T´ım, ˇze je polynom pouze tˇret´ıho ˇra´du, nelze s kˇrivkou pˇr´ıliˇs divokce tvarovat. Dos´ahnout m˚ uˇzeme pouze tvaru bez inflexe nebo s pr´avˇe jednou
´ ˇ 4.3. DRUHY SPOJITYCH KRIVEK
17
Obr´ azek 4.3: Bezierova kˇrivka tˇret´ıho ˇr´adu
inflex´ı, coˇz by se n´am celkem hodilo. Na obr´azku obr. 4.4 m˚ uˇzeme vidˇet nˇekolik moˇznost´ı, jak lze tuto kˇrivku natvarovat. Existuj´ı jednoduch´e, veˇrejnˇe zn´am´e algoritmy viz napˇr. (Karol´ık, A., 2006), kde na vstup zad´ame dva body a teˇcn´e vektory v tˇechto bodech a dostaneme kˇrivku. Pokud potˇrebujeme vygenerovat kˇrivku delˇs´ı, zadanou v´ıce body, lze vyuˇz´ıt algoritmu, kter´ ym se zab´ yval jiˇz kolega pˇrede mnou (Karol´ık, A., 2006). Tento algoritmus spojuje jednotliv´e polynomi´aln´ı kˇrivky tak, ˇze zachov´av´a spojitost, spojitost smˇeru teˇcny i spojitost kˇrivosti. Probl´em je v tom, ˇze pˇri pouˇzit´ı tohoto algoritmu nev´ıme dopˇredu, jak daleko se bude trajektorie odchylovat od pˇr´ımoˇcar´e. Samozˇrejmˇe, ˇze z parametr˚ u kˇrivky pr˚ ubˇeh odchylky vypoˇc´ıt´ame, ale nelze uˇz analyticky vyj´adˇrit z´avislost tˇechto parametr˚ u na maxim´aln´ı odchylce 1
devmax = max(|SEG(t) SEGLIN E |) t=0
(4.3)
Vlastn´ım ideologick´ ym probl´emem je snaha body trajektorie interpolovat (obr. 4.5), to vede vˇzdy k nˇejak´e odchylce, kterou nebudeme schopni zadat jako vstup do algoritmu. Jin´ y pˇr´ıstup je pak tyto body aproximovat (obr. 4.6) podobn´ ym zp˚ usobem jako v pˇr´ıpadˇe kruˇznicov´ ych oblouk˚ u. Na prvn´ı pohled by se zd´alo, ˇze se nijak zm´ınˇen´eho probl´emu nezbav´ıme, ale realita je jin´a. V´ yhoda je takov´a, ˇze sice m´ame nˇejak´e odchylky od pˇr´ımoˇcar´e trajektorie, ale tyto odchylky jsou vˇzdy pouze uvnitˇr konvexn´ıch u ´hl˚ u. Podle n´asleduj´ıc´ı vˇety se tedy nemus´ıme zab´ yvat kolizemi s obvodov´ ymi zdmi. Pokud jsou vˇsechny body pr˚ ujezdu P N T uvnitˇr hrac´ı plochy, kter´a je vymezena konvexn´ımi u ´hly (v naˇsem pˇr´ıpadˇe prav´ ymi), pak vˇsechny body vnitˇrn´ı aproximaˇcn´ı kˇrivky leˇz´ı uvnitˇr tohoto hˇriˇstˇe. Pro vnitˇrn´ı aproximaˇcn´ı kˇrivku plat´ı, ˇze vˇsechny jej´ı body leˇz´ı uvnitˇr u ´hlu γ, kter´ y je tvoˇren tˇremi n´asleduj´ıc´ımi body pr˚ ujezdu. Z toho plyne, ˇze je potˇreba jiˇz pouze oˇsetˇrit situaci, kdy m´ame pˇrek´aˇzku (hern´ı prvek) uvnitˇr tohoto u ´hlu γ. V kapitole 3.2 jsem uvedl zp˚ usob generov´an´ı bod˚ u pr˚ ujezdu. Pokud zaruˇc´ıme, ˇze tyto body vˇzdy dodrˇz´ı urˇcitou minim´aln´ı vzd´alenost od pˇrek´aˇzky, m˚ uˇzeme
18
KAPITOLA 4. SPLINY
Obr´ azek 4.4: Moˇzn´e vzhledy bezierovy kˇrivky
poˇc´ıtat s touto vzd´alenost´ı jako konstantn´ım poˇzadavkem pro kaˇzd´e generov´an´ı aproximaˇcn´ı kˇrivky, ˇc´ımˇz si v´ yraznˇe zjednoduˇs´ıme pr´aci.
4.3.2
Aproximaˇ cn´ı polynomi´ aln´ı kˇ rivka
Dalˇs´ı b´ad´an´ı vede k ot´azce, jak natvarovat polynoi´aln´ı kˇrivku, aby aproximovala body ˇ pr˚ ujezdu trajektorie. Reknˇ eme, ˇze vzhledem ke snaze rychl´eho pr˚ ujezdu nebude v˚ ubec vadit, ponech´ame-li si moˇznost tvoˇrit pˇr´ımoˇcar´e u ´seky a polynom pouˇzijeme jen pro zmˇeny smˇeru. Pokud budeme cht´ıt navazovat pˇr´ımoˇcar´ y segment na spline, z poˇzadavku na spojitou rychlost koleˇcek je jasn´e, ˇze na zaˇca´tku i na konci kˇrivky je potˇreba zajistit nulovou kˇrivost. D˚ usledkem bude tak´e to, ˇze nen´ı tˇreba rozliˇsovat pˇr´ıpad, kdy za splinem navazuje pˇr´ımoˇcar´ y segment nebo dalˇs´ı spline. Nyn´ı si shrneme matematick´e podm´ınky pro vytvoˇren´ı takov´e polynomi´aln´ı kˇrivky, jakoˇzto segmentu trajektorie. Je tˇreba zajistit moˇznost definov´an´ı parametr˚ u uveden´ ych v tabulce (tabulka 4.1). To znamen´a celkem 10 stupˇ n˚ u volnosti, jimiˇz opl´ yv´a 2D polynomi´aln´ı kˇrivka minim´alnˇe
´ ˇ 4.3. DRUHY SPOJITYCH KRIVEK
19
Kubicky spline z Matlabu 3.8 3.6 3.4 3.2
y
3 2.8 2.6 2.4 2.2 2 1.5
2
2.5
3
3.5
x
Obr´azek 4.5: Interpolace
lin2spline
3.5
3
y
2.5
2
1.5
3.5
4
4.5
5 x
Obr´azek 4.6: Aproximace
5.5
6
20
KAPITOLA 4. SPLINY
Tabulka 4.1: Obecn´e podm´ınky pro navazov´an´ı segment˚ u trajektorie
Podm´ınka
Poˇcet ubran´ ych stupˇ n˚ u volnosti
poloha poˇca´teˇcn´ıho bodu SEG(0) 2 stupnˇe volnosti poloha koncov´eho bodu SEG(1)
2 stupnˇe volnosti
smˇer teˇcny v bodˇe SEG(0)
1 stupeˇ n volnosti
smˇer teˇcny v bodˇe SEG(1)
1 stupeˇ n volnosti
kˇrivost κ = 0 v bodˇe SEG(0)
2 stupnˇe volnosti
kˇrivost κ = 0 v bodˇe SEG(1)
2 stupnˇe volnosti
ˇra´du 4. Jelikoˇz se n´am bude hodit jeˇstˇe kˇrivku d´ale tvarovat pro lepˇs´ı kinematick´e vlastnosti, pouˇzijme polynom ˇra´du 5.
4.3.3
Klotoida
Kˇrivka, kter´a je hojnˇe vyuˇz´ıv´ana v dopravn´ım stavebnictv´ı, se naz´ yv´a klotoida (Seng, R. a Severdia, M., n.d.) (obr. 4.9). Vyuˇz´ıv´a se napˇr´ıklad pˇri navrhov´an´ı d´alniˇcn´ıch kˇriˇzovatek, ale i v jin´ ych praktick´ ych fyzik´aln´ıch aplikac´ıch (obr. 4.7 a obr. 4.8). Z´akladn´ı vlastnost´ı klotoidy, pro kterou je vyuˇz´ıv´ana, je, ˇze s rostouc´ı vzd´alenost´ı se line´arnˇe mˇen´ı jej´ı kˇrivost (Seng, R. a Severdia, M., n.d.). k ∈ R\{0}
κ(s) = ks + κi ;
(4.4)
Probl´emem ovˇsem je, ˇze tuto kˇrivku nelze analyticky popsat jako z´avislost polohy na parametru. Jej´ı vyj´adˇren´ı pomoc´ı tzv. Fresnelov´ ych integr´al˚ u je:
t
x2 )dx 2 0 Z t x2 g(t) = cos( )dx 2 0 Z
f (t) =
sin(
(4.5) (4.6)
Z v´ yˇse zm´ınˇen´e z´avislosti (4.4) plyne, ˇze m˚ uˇzeme jednoduˇse klotoidu napojovat na pˇr´ımoˇcar´e u ´seky. Pokud bychom sloˇzili zat´aˇcku ze dvou klotoid, dos´ahneme kr´asn´eho tvaru zat´aˇcky (viz obr. 4.10), kter´ y splˇ nuje vˇsechna omezen´ı uveden´a v tabulce (tabulka 4.1).
´ ˇ 4.3. DRUHY SPOJITYCH KRIVEK
Obr´ azek 4.7: D´ alniˇcn´ı kˇriˇzovatka sloˇzen´a z u ´sek˚ u klotoid
Obr´azek 4.8: Roller Coaster
21
22
KAPITOLA 4. SPLINY
Obr´ azek 4.9: Klotoida
lin2spline 8 7.8 7.6 7.4
y
7.2 7 6.8 6.6 6.4 3
3.5
4
4.5
5
x
Obr´ azek 4.10: Zat´ aˇcka sloˇzen´a ze dvou ˇc´ast´ı klotoidy
´ ER ˇ KONKRETN ´ ´I KRIVKY ˇ 4.4. VYB
4.4
23
V´ ybˇ er konkr´ etn´ı kˇ rivky
Po shl´ednut´ı vˇsech moˇznost´ı jsem se rozhodl, ˇze nejlepˇs´ı varianta bude n´asleduj´ıc´ı. Polynom p´at´eho ˇr´adu n´am zajist´ı analytick´e vyj´adˇren´ı kˇrivky, kter´a m´a dostateˇcn´ y poˇcet stupˇ n˚ u volnosti k tomu, abychom ji dok´azali natvarovat tak, abychom splnili vˇsechny podm´ınky. Ale protoˇze jsme nevyuˇzili vˇsech stupˇ n˚ u volnosti, je tˇreba jeˇstˇe doplnit dalˇs´ı poˇzadavky. Vzhledem k tomu, ˇze se budeme d´ale snaˇzit optimalizovat dobu pr˚ ujezdu trajektori´ı, nebylo by ˇspatn´e, kdyby se tvar naˇseho polynomu alespoˇ n trochu bl´ıˇzil tvaru klotoidy. Popis 2D polynomu p´at´eho ˇra´du: Px (t) = Ax t5 + Bx t4 + Cx t3 + Dx t2 + Ex t + Fx Py (t) = Ay t5 + By t4 + Cy t3 + Dy t2 + Ey t + Fy kde t ∈ h0; 1i.
(4.7)
24
KAPITOLA 4. SPLINY
Kapitola 5 Generov´ an´ı trajektorie
5.1
Popis kˇ rivky
Vych´azejme prozat´ım ze situace, kdy m´ame zad´an bod pr˚ ujezdu Q, kter´ y se budeme snaˇzit aproximovat, d´ale smˇer pˇr´ıjezdu a smˇer odjezdu z tohoto bodu. Tˇemito dvˇema smˇery vymez´ıme u ´hel γ ∈ h0◦ ; 180◦ i, kter´ y se budeme snaˇzit proloˇzit zat´aˇckou. Vych´azejme ze situace, kdy zn´ame vzd´alenost d od bodu Q, kde bude zat´aˇcka zaˇc´ınat. Jak tuto vzd´alenost vybrat, se dozv´ıme pozdˇeji. V zat´aˇcce d´ale plat´ı vztahy
SEG(t) = P (t)
d = |X0 Q| = |Q X1|,
P (0) = X0,
(5.1)
P (1) = X1
(5.2)
kde P (t) je polynomi´aln´ı kˇrivka dle (4.7) s parametrem t ∈ h0; 1i. Z v´ yrazu (5.2) plyne, ˇze zaˇc´atek polynomu bude stejnˇe daleko od vrcholu zat´aˇcky jako jeho konec. 25
´ ´I TRAJEKTORIE KAPITOLA 5. GENEROVAN
26
Nyn´ı sestavme rovnice (5.3), kter´e chceme, aby platily pro n´aˇs spline (viz obr. 5.1). Px (0) = X0x
(5.3)
Py (0) = X0y Px (1) = X1x Py (1) = X1y Px0 (0) = X00x = m(Qx − X0x ) Py0 (0) = X00y = m(Qy − X0y ) Px0 (1) = X10x = m(X1x − Qx ) Py0 (1) = X00y = m(X1y − Qy ) |Px0 (0)Py00 (0) − Py0 (0)Px00 (0)| κ(0) = =0 (Px02 (0) + Py02 (0))3/2 |Px0 (1)Py00 (1) − Py0 (1)Px00 (1)| κ(1) = =0 (Px02 (1) + Py02 (1))3/2 pˇriˇcemˇz parametr m urˇcuje d´elku obou teˇcn´ ych vektor˚ u, ale jeho hodnotu zat´ım nezn´ame. Pro oba smˇery, smˇer pˇr´ıjezdu i smˇer odjezdu, je shodn´ y kv˚ uli zachov´an´ı symetrie kˇrivky. Tot´eˇz plat´ı i pro hodnotu d. Pro u ´plnost je tˇreba uv´est hodnoty derivac´ı polynomu (4.7)
Px0 (t) = 5Ax t4 + 4Bx t3 + 3Cx t2 + 2Dx t + Ex
(5.4)
Py0 (t) = 5Ay t4 + 4By t3 + 3Cy t2 + 2Dy t + Ey Px00 (t) = 20Ax t3 + 12Bx t2 + 6Cx t + 2Dx Py00 (t) = 20Ay t3 + 12By t2 + 6Cy t + 2Dy Po u ´pravˇe soustavy (5.3) dostaneme vyj´adˇren´ı jednotliv´ ych parametr˚ u splinu: Ax = −3X10x − 3X00x − 6X0x + 6X1x Bx = 7X10x + 8X00x + 15X0x − 15X1x Cx = −4X10x − 6X00x − 10X0x + 10X1x Dx = 0 Ex = X00x Fx = X0x
(5.5)
ˇ 5.1. POPIS KRIVKY
27
Q m
d
X0
d
X1
m
Obr´ azek 5.1: V´ yznam symbol˚ u v zat´aˇcce
´ ´I TRAJEKTORIE KAPITOLA 5. GENEROVAN
28
Ay = −3X10y − 3X00y − 6X0y + 6X1y
(5.6)
By = 7X10y + 8X00y + 15X0y − 15X1y Cy = −4X10y − 6X00y − 10X0y + 10X1y Dy = 0 Ey = X00y Fy = X0y V´ıme tedy nyn´ı, jak vypoˇc´ıtat jednotliv´e parametry splinu, za pˇredpokladu, ˇze zn´ame polohu bod˚ u X0 a X1 a parametr m.
5.2
Tvarov´ an´ı kˇ rivky jako klotoidy
Parametr m se pokus´ıme nastavit tak, aby se kˇrivka co nejv´ıce podobala klotoidˇe. To se n´am podaˇr´ı ovˇsem pouze numerick´ ymi optimalizaˇcn´ımi metodami. Vzhledem k tomu, ˇze tento v´ ypoˇcet je ˇcasovˇe n´aroˇcn´ y, nem˚ uˇzeme jej prov´adˇet za j´ızdy, ale je tˇreba jej pˇredpoˇc´ıtat off-line. Vyuˇzijme vlastnosti klotoidy, ˇze m´a line´arnˇe mˇen´ıc´ı se kˇrivost. Pokud budeme tvarovat polynomi´aln´ı kˇrivku, vypoˇcteme si pr˚ ubˇeh jej´ı kˇrivosti pod´el parametru. Tento pr˚ ubˇeh srovn´ame s pr˚ ubˇehem klotoidy a jejich rozd´ıl se budeme snaˇzit minimalizovat. Ide´aln´ı kˇrivka tedy splˇ nuje podm´ınku: Z
1
|κ(t, m) − κclothoid (t, m)|∂t
m = min(m)m
(5.7)
0
V naˇsem pˇr´ıpadˇe pro numerick´ y v´ ypoˇcet a numerickou integraci bude vhodn´e pouˇz´ıt diskr´etn´ı ekvivalent pˇredeˇsl´eho vztahu. " m = min(m)m
1 X
# ((s(t) − s(t − 1)) |κ(t, m) − κ(t, m)clothoid |)
(5.8)
t=1
pˇriˇcemˇz s(t) je vzd´alenost od zaˇca´tku X0 splinu v z´avislosti na parametru t. Konkr´etn´ı v´ yrazy nebudu uv´adˇet z d˚ uvodu jejich pˇr´ıliˇsn´eho rozsahu, pˇredevˇs´ım co se t´ yˇce vzorc˚ us kˇrivost´ı (v´ yraz (4.1) je tˇreba derivovat). K optimalizaci n´am poslouˇz´ı v Matlabu funkce fminsearch. V´ ysledkem bude kˇrivka, ktr´a se nejv´ıce podob´a klotoidˇe, ovˇsem je tˇreba si uvˇedomit, ˇze pr˚ ubˇeh kˇrivosti polynomi´aln´ı kˇrivky je st´ale spojit´a funkce, ˇcili nem˚ uˇzeme dos´ahnout pr˚ ubˇehu ostr´eho jako u klotoidy. Nicm´enˇe, z praktick´eho hlediska n´am to
´ ´I KRIVKY ˇ 5.2. TVAROVAN JAKO KLOTOIDY
29
Curvature 0.7
0.6
0.5
0.4
0.3
0.2
0.1
0 0
0.1
0.2
0.3
0.4
0.5 t
0.6
0.7
0.8
0.9
1
Obr´ azek 5.2: Pr˚ ubˇeh kˇrivosti klotoidy a n´ami generovan´e kˇrivky
´ ´I TRAJEKTORIE KAPITOLA 5. GENEROVAN
30
Optimal value of m 2.4 2.2 2 1.8
m
1.6 1.4 1.2 1 0.8 0.6 0.4
0
20
40
60
80 100 Angle [deg]
120
140
160
180
Obr´ azek 5.3: Optimalizovan´ y parametr m v z´avislosti na u ´hlu
v˚ ubec v niˇcem nepˇrek´aˇz´ı. Na obr´azku (obr. 5.2) je zn´azornˇen jeden z fin´aln´ıch stav˚ u optimalizace. V´ ysledkem optimalizace tedy je uspoˇr´adan´a dvojice [γ; m], kde γ je vnitˇrn´ı u ´hel zat´aˇcky a m je hledan´ y parametr polynomi´aln´ı kˇrivky. Tˇechto uspoˇra´dan´ ych dvojic bychom mˇeli naj´ıt v ide´aln´ım pˇr´ıpadˇe nekoneˇcnˇe mnoho pro γ ∈ (0◦ ; 180◦ ). Zjednoduˇsme si situaci t´ım, ˇze budeme hledat parametr m pouze pro velikosti u ´hl˚ u, kter´e jsou n´asobky 10◦ . Tyto z´ıskan´e body proloˇzme vhodnˇe zvolenou kˇrivkou. Po testov´an´ı r˚ uzn´ ych polynom˚ u se nejvhodnˇejˇs´ı zd´ala elipsa, pro kterou je tˇreba natvarovat dvˇema parametry A a B. Tyto se podaˇrilo ruˇcnˇe nastavit na hodnoty A = 6860 a B = 4, 4. Proloˇzen´ı bod˚ u elipsou (5.9) je zn´azornˇeno na obr´azku (obr. 5.3). r m(γ) =
B−
(γ − 180)2 A
(5.9)
Z pozdˇejˇs´ıch poznatk˚ u vyplynulo, ˇze se tato kˇrivka ned´a pouˇz´ıt pro mal´e u ´hly zhruba menˇs´ı neˇz 10◦ , protoˇze v t´eto oblasti nedostaˇcuje pˇresnost proloˇzen´ı. Proto je tˇreba ´ e stejnou optimalse na tuto oblast mal´ ych u ´hl˚ u zamˇeˇrit zvl´aˇst’ a podrobnˇeji. Uplnˇ izaˇcn´ı metodou dostaneme body v intervalu γ ∈ (0◦ ; 10◦ i a proloˇz´ıme kˇrivkou (5.10),
´ ´I KRIVKY ˇ 5.2. TVAROVAN JAKO KLOTOIDY
31
Optimal value of m 0.45 0.4 0.35 0.3
m
0.25 0.2 0.15 0.1 0.05 0
1
2
3
4
5 6 Angle [deg]
7
8
9
10
Obr´ azek 5.4: Optimalizovan´ y parametr m v z´avislosti na mal´ ych u ´hlech
zn´azornˇenou na obr´azku (obr. 5.4). m(γ) = C · γ + D
C = 0, 0423
D = 0, 008
(5.10)
V´ ysledek cel´e optimalizace uˇz m˚ uˇze pouˇz´ıvat robot bˇehem pl´anov´an´ı pohybu za j´ızdy jen se znalost´ı tˇechto parametr˚ u A, B, C, D. Cel´a pˇredchoz´ı ˇca´st vych´azela z pˇredpokladu, ˇze vliv vzd´alenosti d neuvaˇzujeme, respektive d je zn´am´ ym vstupem, z ˇcehoˇz vypl´ yv´a i poloha bod˚ u X0 a X1. Jak se tedy vypoˇr´adat se situac´ı, kdy potˇrebujeme zat´aˇcku jinak velkou (myˇsleno d 6= 1, ale u ´hel γ je stejn´ y)? Vypoˇc´ıtan´ y polynom lze jednoduˇse transformovat pˇren´asoben´ım vˇsech jeho parametr˚ u konstantou, kter´a ud´av´a zvˇetˇsen´ı kˇrivky ve smˇeru osy x i y. Pˇritom se zachovaj´ı poˇzadovan´e vlastnosti. Chceme-li tedy posunout body X0 a X1 do jin´e vzd´alenosti, definujeme pomˇern´e zvˇetˇsen´ı =
dnew d
. Pro jednotliv´e parametry polynomu bude tedy platit:
Axnew = · Ax
(5.11)
... Fynew = · Fy Nyn´ı m´ame k dispozici postup, pomoc´ı nˇehoˇz um´ıme vytvoˇrit libovolnˇe velkou zat´aˇcku
´ ´I TRAJEKTORIE KAPITOLA 5. GENEROVAN
32
v libovoln´em u ´hlu γ ∈ (0◦ ; 180◦ ), kter´a navazuje na pˇr´ımoˇcar´e u ´seky. Pokud bychom mˇeli splnit omezen´ı, ˇze se trajektorie nesm´ı odchylovat od vrcholu zat´aˇcky Q v´ıce neˇz urˇcitou zadanou hodnotu emax , je potˇreba spline transformovat pomoc´ı konstanty (5.12). =
5.3
e emax
(5.12)
Napojen´ı jednotliv´ ych segment˚ u
S jiˇz pˇredstaven´ ymi n´astroji se pokus´ıme kompletnˇe sestavit trajektorii ze zadan´ ych bod˚ u. Nejprve je tˇreba oˇsetˇrit nˇekter´e z´aleˇzitosti: • Dva n´asleduj´ıc´ı body p˚ ujezdu nesm´ı b´ yt stejn´e • Mezi tˇremi n´asleduj´ıc´ımi body pr˚ ujezdu nesm´ı b´ yt u ´hel γ = 0 V tˇechto pˇr´ıpadech bychom nebyli schopni trajektorii vygenerovat, protoˇze v prvn´ım pˇr´ıpadˇe nelze vypoˇc´ıtat u ´hel mezi u ´seˇckami, z nichˇz m´a jedna nulovou velikost. S t´ım si porad´ıme jednoduˇse tak, ˇze vˇsechny duplicitn´ı body smaˇzeme. Ve druh´em pˇr´ıpadˇe je v´ıce moˇzn´ ych z´asah˚ u, j´a jsem vybral moˇznost pˇridat bod, kter´ y je nepatrnˇe vzd´alen od vrcholu zat´aˇcky Q. Algoritmus, kter´ ym vybudujeme trajektorii, skl´adaj´ıc´ı se z pˇr´ımoˇcar´ ych a splinov´ ych segment˚ u, funguje tak, ˇze nejdˇr´ıve pospojujeme sekvenci bod˚ u pr˚ ujezdu u ´seˇckami, a n´aslednˇe vyhled´ame body, kde budou zaˇc´ınat a konˇcit spliny. Tyto body najdeme tak, ˇze vezmeme dvˇe pˇrilehl´e u ´seˇcky bodu Qi , a v polovinˇe kratˇs´ı z nich bude leˇzet jeden z bod˚ u X0i , X1i . Na druh´e u ´seˇcce, ve stejn´e vzd´alenosti d, pak druh´ y z tˇechto bod˚ u. Uk´azka takov´e trajektorie je na obr´azku (obr. 5.5). Nyn´ı je potˇreba vz´ıt v u ´vahu omezen´ı emax . Bod Qi je od stˇredu splinu P (0, 5) ve vzd´alenosti ei . Pokud plat´ı, ˇze ei > emax , je tˇreba spline zmenˇsit a zajistit rovnost ei = emax . Vzhledem k tomu, ˇze vzd´alenost e je pˇr´ımo u ´mˇern´a vzd´alenosti d, posuneme body X0 a X1 tak, ˇze plat´ı: emax (5.13) ei Jak´ ym zp˚ usobem ale vypoˇc´ıt´ame e? Nejjednuduˇsˇs´ı moˇznost je spline vygenerovat a i =
potom se pod´ıvat na polohu bodu P (0, 5). To je ale zbyteˇcnˇe ˇcasovˇe n´aroˇcn´e. Lepˇs´ı by bylo zn´at tuto hodnotu pˇredem. Vytvoˇr´ıme si tedy off-line tabulku z´avislosti e na vnitˇrn´ım u ´hlu γ. Tuto tabulku opˇet proloˇz´ıme nˇejakou analyticky vyj´adˇritelnou kˇrivkou:
´ ˚ 5.3. NAPOJEN´I JEDNOTLIVYCH SEGMENTU
33
lin2spline 8
7 6
y
5
4
3 2
1
0 −2
−1
0
1
2
3 x
4
5
6
7
8
Obr´ azek 5.5: Trajektorie se spliny, nebere v u ´vahu omezen´ı emax
Distance from corner to spline − approximation parameter 1 0.9 0.8 0.7
e
0.6 0.5 0.4 0.3 0.2 0.1 0
0
20
40
60
80 100 Angle [deg]
120
140
160
180
Obr´ azek 5.6: Z´ avislost vzd´alenosti e na u ´hlu γ pˇri konstantn´ım d
´ ´I TRAJEKTORIE KAPITOLA 5. GENEROVAN
34
lin2spline 8 7 6
y
5 4 3 2
1
0 −2
−1
0
1
2
3 x
4
5
6
7
8
Obr´ azek 5.7: V´ ysledn´a trajektorie
e(γ)|d=1 = 1, 849 · 10−5 γ 2 − 8, 169 · 10−3 γ + 0.892
[m, m, ◦ ]
(5.14)
Nyn´ı, kdyˇz uˇz v´ıme, jak budou vypadat vˇsechny kˇrivky, je potˇreba zkr´atit pˇr´ımoˇcar´e ˇ kaˇzd´a u u ´seky. Cili ´seˇcka bude vymezena krajn´ımi body X1i−1 a X0i , pokud nastane pˇr´ıpad, ˇze X1i−1 = X0i , bude tento pˇr´ımoˇcar´ y u ´sek odstranˇen. Nyn´ı jsme u konce s veˇskerou geometri´ı trajektorie a m´ame pˇripravenou sekvenci navazuj´ıc´ıch pˇr´ımoˇcar´ ych a splinov´ ych u ´sek˚ u.
Kapitola 6 Kinematika robota M´ame-li hotovou trajektorii, m˚ uˇzeme se zab´ yvat t´ım, jak rozdˇel´ıme rychlosti, kter´ ymi tuto trajektorii budeme proj´ıˇzdˇet. Aby se n´am to povedlo, je potˇreba vz´ıt v u ´vahu omezen´ı zp˚ usoben´a fyzik´aln´ı povahou robota. S tˇemito omezen´ımi budeme muset d´ale poˇc´ıtat. Mus´ı platit, ˇze v kaˇzd´em okamˇziku pohybu m´a robot: • aktu´aln´ı rychlost niˇzˇs´ı neˇz vmax • aktu´aln´ı teˇcn´e zrychlen´ı niˇzˇs´ı neˇz amax • aktu´aln´ı dostˇrediv´e zrychlen´ı niˇzˇs´ı neˇz cenaccmax • aktu´aln´ı u ´hlovou rychlost niˇzˇs´ı neˇz ωmax • aktu´aln´ı u ´hlov´e zrychlen´ı niˇzˇs´ı neˇz αmax Tyto hodnoty jsou konstantn´ı a jsou nastavov´any experiment´alnˇe. V programu jsou reprezentov´any strukturou TrajectoryConstraints: struct TrajectoryConstraints { double maxv; double maxomega; double maxangacc; double maxacc; double maxcenacc; double maxe; }; 35
36
KAPITOLA 6. KINEMATIKA ROBOTA
Tabulka 6.1: Kinematick´e parametry pˇr´ımoˇcar´eho u ´seku
Parametr V´ yznam v1
rychlost na zaˇca´tku u ´seku
v2
rychlost na konci u ´seku
acc zrychlen´ı bˇehem u ´seku
6.1
Vztah
acc =
(v2 −v1 )(v2 +v1 ) l
Pr˚ ubˇ eh rychlosti na pˇ r´ımoˇ car´ em u ´ seku
Tato ˇca´st jiˇz byla implementov´ana jiˇz dˇr´ıve, pr˚ ubˇeh rychlosti na u ´seˇcce je jednoduˇse vyj´adˇren tˇremi parametry v tabulce (tabulka 6.1). Po u ´seˇcce se tedy m˚ uˇzeme pohybovat tˇremi moˇzn´ ymi zp˚ usoby: • pohyb rovnomˇern´ y • pohyb rovnomˇernˇe zrychlen´ y • pohyb rovnomˇernˇe zpomalen´ y
6.2
Pr˚ ubˇ eh rychlosti na splinu
Kdybychom chtˇeli dokonale optimalizovat pr˚ ubˇeh rychlosti bˇehem splinu, museli bychom popsat funkci (6.1), kter´a je minimem vˇsech pr˚ ubˇeh˚ u rychlost´ı z´avisl´ ych na r˚ uzn´ ych omezen´ı. v(s) = min(vmax , v(amax , s), v(cenaccmax , s), v(ωmax , s), v(αmax , s)),
(6.1)
coˇz by bylo dosti n´aroˇcn´e. Pro jednoduchost vych´azejme z toho, ˇze kˇrivost splinu bˇehem ˇ dr´ahy line´arnˇe roste a potom zase line´arnˇe kles´a. Reknˇ eme, ˇze je potˇreba do poloviny zpomalovat a ve druh´e polovinˇe zrychlovat. Pˇrijmˇeme tedy parametrizaci v tabulce (tabulka 6.2). ˇ z´avislost rychlosti na ˇcase se skl´ad´a ze dvou u Cili ´seˇcek, kter´e obvykle maj´ı po ˇradˇe z´apornou a kladnou derivaci. Tento pr˚ ubˇeh vypad´a jako p´ısmeno V“. Zm´ınˇen´ y obvykl´ y“ ” ” pˇr´ıpad vych´az´ı z pˇredpokladu, ˇze nejsme ovlivnˇeni okoln´ımi u ´seky natolik, aby musela b´ yt rychlost v1 ˇci v2 niˇzˇs´ı neˇz vc , coˇz se ovˇsem prakticky st´at m˚ uˇze.
´ ´I RYCHLOSTI SEGMENTU ˚ 6.3. MAXIMALN
37
Tabulka 6.2: Kinematick´e parametry splinu
Parametr V´ yznam
Vlastnost
v1
rychlost v bodˇe P (0)
vc
rychlost v bodˇe P (0, 5)
v2
rychlost v bodˇe P (1)
acc1 zrychlen´ı mezi body P (0) a P (0, 5) obvykle acc1 < 0 acc2 zrychlen´ı mezi body P (0, 5) a P (1) obvykle acc2 > 0
6.3
Maxim´ aln´ı rychlosti segment˚ u
Nejdˇr´ıve nastav´ıme rychlosti v segmentech tak, jako kdyby nebyly ovlivˇ nov´any podm´ınkami segment˚ u sousedn´ıch. Vˇsechny u ´seˇcky tedy lze projet nejvyˇsˇs´ı rychlost´ı maxv: v1 = v2 = vmax . U splin˚ u uˇz nen´ı situace tak jednoduch´a, protoˇze mus´ıme br´at v u ´vahu i ostatn´ı omezen´ı (35). Nejprve nastav´ıme maxim´aln´ı hodnotu rychlosti uprostˇred segmentu. K tomu potˇrebujeme polomˇer kˇrivosti v bodˇe SEG(0.5) rmin =
1 κ(t = 0.5)
(6.2)
Pokud vezmeme v u ´vahu vˇsechna kinematick´e omezen´ı (Kotl´ık, B. et al., 2003), rychlost vc bude minim´aln´ı hodnota ze vˇsech maxim´aln´ıch rychlost´ı pro dan´e omezen´ı. ! r √ lrmin vc = min vmax , ωmax rmin , cenaccmax rmin , αmax 2
(6.3)
kde l je d´elka cel´eho splinu. Posledn´ı ˇclen je zaloˇzen na n´asleduj´ıc´ım odvozen´ı: α = dωdt =
v2 drds
(6.4)
v 2 = αdrds √ v = αdrds 1 drds = dκ = konst. ds
protoˇze
dκ ds
= konst. je vlastnost klotoidy. Tato konstanta lze jednoduˇse vypoˇc´ıtat z
pr˚ ubˇehu kˇrivosti klotoidy viz obr. 5.2. κ(t = 0.5) − 0 dκ = ds l/2
(6.5)
38
KAPITOLA 6. KINEMATIKA ROBOTA M´ame tedy nastavenou maxim´aln´ı rychlost uprostˇred segmentu. D´ale je potˇreba vyv´aˇzit
rychlosti v1 , v2 a vC tak, aby nikde bˇehem segmentu nedoˇslo k pˇrekroˇcen´ı maxim´aln´ıch omezen´ı. Za t´ımto u ´ˇcelem byl implementov´an iterativn´ı optimalizaˇcn´ı algoritmus zaˇc´ınaj´ıc´ı na hodnot´ach v1 = v2 = vmax /2, kter´ y nebudu bl´ıˇze komentovat, je uveden v pˇriloˇzen´em zdrojov´em k´odu (A). Tento algoritmus je sice prakticky docela funkˇcn´ı, ale pˇr´ıliˇs intuitivn´ı. Pˇredevˇs´ım je probl´emem to, ˇze se prov´ad´ı zbyteˇcn´a iterace on-line. Tato ˇc´ast by se hodila v budoucnu jeˇstˇe upravit.
6.4
Zajiˇ stˇ en´ı spojitosti rychlost´ı
Nyn´ı, kdyˇz m´ame nastaveny maxim´aln´ı rychlosti u jednotliv´ ych segment˚ u, jsme v situaci, kdy potˇrebujeme v nˇekter´ ych segmentech rychlost sn´ıˇzit tak, aby navazovala se segmentem sousedn´ım. Algoritmus byl jen nepatrnˇe zmˇenˇen oproti p˚ uvodn´ı verzi s kruˇznicov´ ymi oblouky a pracuje n´asledovnˇe: 1. V prvn´ı ˇca´sti proch´az´ıme segmenty od zaˇc´atku do konce a rychlost v1 nastav´ıme na hodnotu v2 segmentu pˇredchoz´ıho n´asleduj´ıc´ım zp˚ usobem: • pokud se jedn´a o pˇr´ımoˇcar´ y segment, rozdˇel´ıme jej na dva tak, ˇze prvn´ı ˇc´ast bude m´ıt acc = accmax a druh´a ˇc´ast bude m´ıt rychlosti v1 i v2 p˚ uvodn´ı. M˚ uˇze se samozˇrejmˇe st´at, ˇze druh´a ˇca´st v˚ ubec nebude existovat. • pokud se jedn´a o spline, pouze nastav´ıme hodnotu v1 na hodnotu v2 segmentu pˇredchoz´ıho. N´aslednˇe pˇrepoˇcteme hodnotu acc1. 2. Ve druh´e ˇca´sti proch´az´ıme sekvenci segment˚ u zpˇet od konce na zaˇc´atek a nastavujeme rychlost v2 na hodnotu v1 segmentu pˇredchoz´ıho (myˇsleno ve smˇeru od konce): • jedn´a-li se o pˇr´ımoˇcar´ y u ´sek, rozdˇel´ıme jej na dva tak, ˇze jeden bude m´ıt hodnotu acc = −accmax a druh´ y bude m´ıt pr˚ ubˇeh rychlosti nezmˇenˇen. • jedn´a-li se o spline, nastav´ıme hodnotu v2 na hodnotu v1 segmentu pˇredchoz´ıho (myˇsleno ve smˇeru od konce). N´aslednˇe pˇrepoˇcteme hodnotu acc2. T´ım jsme dostali spojit´ y pr˚ ubˇeh rychlosti pod´el cel´e trajektorie. Na obr´azku (obr. 6.1) je vidˇet, ˇze ˇca´sti ve tvaru V“, kter´e reprezentuj´ı pr˚ ubˇeh rychlosti na splinu, navazuj´ı na ”
ˇ EN ˇ ´I SPOJITOSTI RYCHLOST´I 6.4. ZAJIST
39
1.8 1.6
Speed [m/s], dotted [rad/s]
1.4 1.2 1 0.8 0.6 0.4 0.2 0
0
2
4
6
8
10
12
14
Time [s]
Obr´ azek 6.1: Pr˚ ubˇeh rychlosti po pˇrid´an´ı zrychlen´ı mezi segmenty
1.8 1.6
Speed [m/s], dotted [rad/s]
1.4 1.2 1 0.8 0.6 0.4 0.2 0
0
2
4
6
8 Time [s]
10
12
14
16
Obr´ azek 6.2: Pr˚ ubˇeh rychlosti po pˇrid´an´ı zpomalen´ı mezi segmenty
40
KAPITOLA 6. KINEMATIKA ROBOTA
okoln´ı ˇca´sti jen z jedn´e strany, kdeˇzto na obr´azku (obr. 6.2) uˇz z obou stran a cel´ y pr˚ ubˇeh je spojit´ y (modr´a barva).
Kapitola 7 Algoritmus vyh´ yb´ an´ı se pˇ rek´ aˇ zk´ am V kapitole 3 jsem uvedl p˚ uvodn´ı metodu vyh´ yb´an´ı se pˇrek´aˇzk´am, nyn´ı, kdyˇz uˇz jsme vˇenovali u ´sil´ı zrychlov´an´ı pr˚ ujezdu pomoc´ı splin˚ u, bude se hodit vylepˇsit i tuto metodu. Bˇehem pohybu robota je pravidelnˇe kontrolov´ana trajektorie, zda nekoliduje s nˇejakou pˇrek´aˇzkou. Typicky jde o soupeˇre, protoˇze hern´ı prvky, kter´e by n´am vadily, jsou vˇetˇsinou statick´e. Pokud tedy zjist´ıme moˇznou kolizi, je tˇreba pˇrepl´anovat na trajektorii novou. Naˇs´ım c´ılem je, aby toto robot prov´adˇel za j´ızdy. Ovˇsem cel´ y algoritmus pl´anov´an´ı nov´e trajektorie trv´a delˇs´ı dobu neˇz jeden cyklus vzorkov´an´ı trajektorie, coˇz znamen´a, ˇze budeme jeˇstˇe nˇejakou chv´ıli potˇrebovat jet po trajektorii star´e. Pl´anovat trajektorii novou nebudeme z bodu, kde se pr´avˇe robot nach´az´ı, ale z urˇcit´eho bodu v budoucnosti T RAJ(tswitch );
tswitch = tnow + ttrgen
(7.1)
Je tˇreba si uvˇedomit, ˇze existuje rozd´ıl mezi v´ yˇse zm´ınˇen´ ym pl´anov´an´ım a pˇrepl´anov´an´ım za j´ızdy. Pokud pl´anujeme trajektorii za j´ızdy, v bodˇe T RAJ(0) m´ame nenulovou rychlost. To znamen´a, ˇze nejenˇze potˇrebujeme zajistit spojitost trajektorie a jej´ı teˇcny, ale mus´ıme d´at pozor na to, ˇze nem˚ uˇzeme libovolnˇe generovat prvn´ı bod pr˚ ujezdu, abychom splnili kinematick´a omezen´ı (souvis´ı se setrvaˇcnost´ı robota). Tento poˇzadavek jednoduˇse oˇsetˇr´ıme t´ım, ˇze prvn´ı bod vygenerujeme n´asilnˇe ve smˇeru rychlosti ve vzd´alenosti lbrake takov´e, bˇehem n´ıˇz je schopen u ´plnˇe zastavit. lbrake = 1/2
vP2 N T 0 (0) amax
v0 x |v0 | v0 y P N T (1)y = P N T (0)y + lbrake |v0 |
P N T (1)x = P N T (0)x + lbrake
41
(7.2) (7.3) (7.4)
´ AN ´ ´I SE PREK ˇ ´ ZK ˇ AM ´ KAPITOLA 7. ALGORITMUS VYHYB A
42
lin2spline 8
7 6
y
5 SWITCH
4 3 2
CIL
1 START 0
−1
0
1
2
3
4
5
6
7
8
x
Obr´ azek 7.1: Pˇrepl´anov´an´ı trajektorie za j´ızdy
Bod P N T (1) zajist´ı, ˇze po vygenerov´an´ı trajektorie je algoritmus pˇriˇrazov´an´ı rychlost´ı schopen zajistit spojit´ y pr˚ ubˇeh rychlosti, jelikoˇz robot je schopen do tohoto bodu zpomalit na nulovou rychlost. Jin´ y rozd´ıl oproti generov´an´ı trajektorie z klidu nen´ı. Kdyˇz m´ame novou trajektorii hotovou, je tˇreba ji nav´azat na starou v bodˇe S = T RAJi (tswitch ) = T RAJi+1 (0) a udˇelat z nich trajektorii jedinou. Rozdˇel´ıme tedy starou trajektorii na dvˇe poloviny v bodˇe C a druhou polovinu smaˇzeme. Nepatrn´ y probl´em nast´av´a, pokud k rozdˇelen´ı dojde uvnitˇr spline segmentu, protoˇze trajektorie konˇc´ı nenulovou kˇrivost´ı a my nejsme schopni zajistit jinou neˇz nulovou kˇrivost v bodˇe T RAJ(0) pro trajektorii n´asleduj´ıc´ı. Tento probl´em bohuˇzel nelze s pouˇzit´ım st´avaj´ıc´ıch algoritm˚ u ˇza´dn´ ymi modifikacemi odstranit a budeme se s n´ım tud´ıˇz muset sm´ıˇrit.
Kapitola 8 Z´ avˇ er Podaˇrilo se vyj´adˇrit popis spojit´ ych kˇrivek a implementovat funkˇcn´ı knihovnu v C++. Knihovna m´a urˇcit´e nedokonalosti, pˇredevˇs´ım nen´ı doˇreˇsena situace, kdy je tˇreba pˇrepl´anovat trajektorii za j´ızdy, a tak´e pr´ace s kinematick´ ymi omezen´ımi pro pr˚ ujezd splinem nen´ı u ´plnˇe dokonal´a. Dalˇs´ım vylepˇsen´ım robota pro lepˇs´ı lokalizaci pomoc´ı odometrie by mohla b´ yt v´ ymˇena pˇr´ıliˇs ˇsirok´ ych koleˇcek za uˇzˇs´ı.
43
44
´ ER ˇ KAPITOLA 8. ZAV
Literatura Choset, H., Lynch, K. M., Hutchinson, S., Kantor, G. A., Burgard, W., Kavraki, L. E. a Thrun, S. (2005), Principles of Robot Motion: Theory, Algorithms, and Implementations, MIT Press, Cambridge, MA. Karol´ık, A. (2006), ‘Kubick´e spliny pro v´ıcerozmˇern´e prostory’. Semestr´aln´ı pr´ace, ˇ FEL CVUT. ˇkova ´ , K., Vondra, M. a Voˇ ´ , Z. (2003), MatemKotl´ık, B., Lank, V., R˚ uˇ zic sicky atick´e, fyzik´aln´ı a chemick´e tabulky, Fragment. ISBN 80-7200-521-9. Seng, R. a Severdia, M. (n.d.), ‘The Clothoid’, [online] http://online.redwoods.cc.ca.us/instruct/darnold/CalcProj/Fall07/ MollyRyan/Paper.pdf. ˇ ızen´ı pro robotick´ ˇ ˇka, J. (2005), ‘R´ Trdlic y fotbal’. Diplomov´a pr´ace, FEL CVUT. Webov´e str´anky CTU Dragons [online] (2009). [cit. 2009], hhttp://rtime.felk.cvut.cz/robot/i. ˇ eho kola Eurobot [online] (2009). [cit. 2009], Webov´e str´anky Cesk´ hhttp://www.eurobot.cz/i. hhttp://en.wikipedia.org/i.
Wikipedia [online] (2009). [cit. 2009],
45
46
LITERATURA
Pˇ r´ıloha A Obsah pˇ riloˇ zen´ eho CD K t´eto pr´aci je pˇriloˇzeno CD, na kter´em jsou uloˇzeny zdrojov´e k´ody a video. • Adres´aˇr
pdf: Elektronick´a podoba bak´al´aˇrsk´e pr´ace
• Adres´aˇr
src: Zdrojov´e k´ody knihovny v C++
• Adres´aˇr
app: Pˇriloˇzen´a nepublikovan´a literatura (.pdf)
• Adres´aˇr matlab: Skripty v Matlabu k simulac´ım • Adres´aˇr video: Z´aznam j´ızdy robota na cviˇcn´em hˇriˇsti (.avi)
I
