XIV Krajowa Konferencja Automatyki
Zielona Góra, 24-27 czerwca 2002

ALGORYTM ´SLEDZENIA TRAJEKTORII ROBOTA MOBILNEGO MK

†

Krzysztof TCHO ´

N, Marek KABAŁA, Marek WNUK

Politechnika Wrocławska, Instytut Cybernetyki Technicznej

ul. Janiszewskiego 11/17, 50-372, Wrocław,

e-mail:tchon|mkabala|[e-mail]

Streszczenie: Przedmiotem referatu jest algorytm ´sledzenia
trajektorii dwukołowego robota mobilnego MK nap˛edzanego
wzgl˛edem wewn˛etrznego układu odniesienia. Algorytm wyko-
rzystuje metod˛e linearyzacji modelu kinematyki i dynamiki ro-
bota przy pomocy dynamicznego sprz˛e˙zenia zwrotnego. Dzia-
łanie algorytmu zostało zilustrowane wynikami badania´n symu-
lacyjnych.

Słowa kluczowe: Robot mobilny, ´sledzenie trajektorii, lineary-
zacja dynamiczna.

1.

WPROWADZENIE

Problematyka sterowania nieholonomicznych robotów
mobilnych znajduje si˛e od dłu˙zszego czasu w centrum
zainteresowania robotyków [1, 2, 3]. Zarówno na pozio-
mie kinematyki, jak i dynamiki rozwa˙zane s ˛

a dwa zada-

nia, ró˙zni ˛

ace si˛e stopniem trudno´sci i wymagaj¸ace zasad-

niczo odmiennego sposobu podej´scia: zadanie sterowa-
nia punktowego (stabilizacji) i zadanie sterowania ci ˛

a-

głego (´sledzenia trajektorii). Przegl ˛

ad problemów stabi-

lizacji kinematyki układów nieholonomicznych zawiera
praca [4]. Typowym narz˛edziem stosowanym do synte-
zy algorytmów ´sledzenia trajektorii układów nieholono-
micznych jest linearyzacja przez statyczne lub dynamicz-
ne sprz˛e˙zenie zwrotne [1]. Przypomnijmy, ˙ze istot ˛

a me-

tody linearyzacji przez dynamiczne sprz˛e˙zenie zwrotne
jest zdefiniowanie tzw. wyj´s´c linearyzuj ˛

acych układu i

takie rozszerzenie dynamiki układu, ˙zeby było mo˙zliwe
odsprz˛e˙zenie i linearyzacja odwzorowania mi˛edzy wej-
´sciami a wyj´sciami linearyzuj ˛

acymi. Nale˙zy zauwa˙zy´c,

˙ze warunkiem poprawnego działania algorytmów stero-

wania opartego na linearyzacji dynamicznej jest pozo-
stawanie sterowanego obiektu w ruchu; algorytmy te ze
swojej natury nie nadaj ˛

a si˛e do sterowania obiektów nie-

ruchomych, ani do ich zatrzymania lub ruszenia z miej-
sca. Przykładowe zastosowanie dynamicznego sprz˛e˙ze-
nia zwrotnego do sterowania kinematyk ˛

a manipulatora

mobilnego przedstawiono w pracy [5].
W niniejszym referacie zajmujemy sie zadaniem ´sle-
dzenia trajektorii dwukołowego robota mobilnego MK,
skonstruowanego w Zakładzie Podstaw Cybernetyki i
Robotyki Instytutu Cybernetyki Technicznej Politechni-

†

Praca wykonana w ramach grantu statutowego KBN. Sy-

mulacje komputerowe zrealizowano przy wykorzystaniu programu
MATLAB udost˛epnionego przez Wrocławskie Centrum Sieciowo-
Superkomputerowe.

ki Wrocławskiej. Robot MK jest nap˛edzany wzgl˛edem
poruszaj ˛

acego si˛e korpusu robota, co uniemo˙zliwia od-

separownie jego kinematyki od dynamiki i wymaga ste-
rowanie robotem jako integraln ˛

a cało´sci ˛

a. Zało˙zenie o

braku po´slizgu bocznego i wzdłu˙znego kół robota po-
woduje, ˙ze jest on układem nieholonomicznym. Model
kinematyki i dynamiki robota MK, wyprowadzony w
oparciu o Zasad˛e d’Alemberta [3], ma posta´c afinicz-
nego układu sterowania z 8-miowymiarow ˛

a przestrzeni ˛

a

stanu i dwoma wej´sciami steruj¸acymi odpowiadaj ˛

acymi

momentom nap˛edowym kół robota. Dla takiego mode-
lu wybieramy współrz˛edne poło˙zenia charakterystyczne-
go punktu robota jako wyj´scia linearyzuj ˛

ace i dokonu-

jemy odsprz˛e˙zenia i linearyzacji odwzorowania wej´scie-
wyj´scie wykorzystuj ˛

ac dynamiczne sprz˛e˙zenie zwrotne z

1-wymiarowym rozszerzeniem dynamiki układu. W re-
zultacie, odwzorowanie to uzyskuje prost ˛

a struktur˛e zło-

˙zon ˛

a z dwóch równoległych ła´ncuchów zawieraj ˛

acych po

3 integratory poł ˛

aczone szeregowo. Struktura odwzoro-

wania wej´scie-wyj´scie pozwala rozwi ˛

aza´c zadanie ´sle-

dzenia przy u˙zyciu liniowego regulatora rz˛edu 3. Al-
gorytm sterowania robota MK został pokazany na ry-
sunku 2. Badania symulacyjne potwierdziły efektywno´s´c
działania algorytmu i pokazały, ˙ze zapewnia on bardzo
dobr ˛

a jako´s´c sterowania, zale˙zn ˛

a od wzmocnie ´n regula-

tora liniowego.

Układ referatu jest nast˛epuj ˛

acy. W rozdziale 2 opisujemy

model kinematyki i dynamiki robota MK. Rozdział 3 zo-
stał po´swi˛econy syntezie algorytmu sterowania. Wyniki
bada ´n symulacyjnych przedstawia rozdział 4. Rozdział 5
stanowi podsumowanie referatu.

2.

MODEL DYNAMIKI ROBOTA MK

Robot MK jest dwukołowym nieholonomicznym robo-
tem mobilnym nap˛edzanym wzgl˛edem wewn˛etrznego
układu odniesienia [6, 7]. Schemat robota, zawieraj ˛

acy

definicje jego współrz˛ednych kinematycznych oraz pa-
rametrów geometrycznych i dynamicznych, przedstawia
rysunek 1.

We współrz˛ednych x

y

ϕ

1

ϕ

2

α

η

1

η

2

η

3



i przy ozna-

czeniach

η

123



η

1



η

2



η

3

,

θ



R

2d

ϕ

2



ϕ

1



, równania

kinematyki robota, uwzgl˛edniaj ˛

ace brak po´slizgu kół, s ˛

a

Rys. 1. Schematyczny rysunek robota mobilnego MK.

nast˛epuj ˛

ace

˙

x



η

123

cos

θ

˙

y



η

123

sin

θ

˙

ϕ

1



2

R

η

1

˙

ϕ

2



2

R

η

2

˙

α



1

R

η

3



(1)

Interpretacja współrz˛ednych

η

1

η

2

η

3

wynika bezpo-

´srednio ze wzoru (1). Przyjmuj ˛

ac, ˙ze wielko´sciami steru-

j ˛

acymi u

1

u

2

robota s ˛

a momenty nap˛edowe kół wywiera-

ne przez silniki, uzyskujemy równania dynamiki postaci

M

α



˙

η



N

α

η



Bu

(2)

zawieraj ˛

ace macierz bezwładno´sci

M



α



M







m

c



4
3

m

o



8m

k



d

2



m

k

R

2



m

c



2

sin

2

α



1
3



2

c



1
2

r

2

c





m

c



2
3

m

o



d

2



m

k

R

2



m

c





2

sin

2

α



1
3



2

c



1
2

r

2

c





m

c



m

o



4m

k



m

c



R

cos

α



d

2



m

c



2
3

m

o



d

2



m

k

R

2



m

c



2

sin

2

α



1
3



2

c



1
2

r

2

c





m

c



4
3

m

o



8m

k



d

2



m

k

R

2



m

c





2

sin

2

α



1
3



2

c



1
2

r

2

c





m

c



m

o



4m

k



m

c



R

cos

α



d

2



m

c



m

o



4m

k



m

c



R

cos

α



d

2



m

c



m

o



4m

k



m

c



R

cos

α



d

2



m

c



m

o



4m

k



2m

c



R

cos

α



m

c

R

2





2



1
2

r

2

c





d

2











wektor momentów sił Coriolisa, od´srodkowych i tarcia
lepkiego

N

α

η



N































m

c

!

sin

α

2

η

2



η

3



η

2



η

1

"

2m

c

#

2

R

sin

α

cos

αη

3

η

2



η

1



m

c

d

2

#

sin

α

R

2

η

2

3



4d

2

k

1

R

2

η

1

m

c

!

sin

α

2

η

1



η

3



η

2



η

1

$

2m

c

#

2

R

sin

α

cos

αη

3

η

2



η

1



m

c

d

2

#

sin

α

R

2

η

2

3



4d

2

k

2

R

2

η

2



m

c

#

sin

α

R

η

2



η

1



2

R

%!

cos

α

&

m

c

#

d

2

R

2

sin

αη

2

3



m

c

g

#

d

2

R

sin

α

')(

(

(

(

(

(

(

(

(

(

(

(

(

*

i macierz sterowa´n

B



2d

2

R

+,

1

0

0

1

0

0

-.



3.

ALGORYTM STEROWANIA

B˛edziemy

rozwa˙za´c

zadanie

´sledzenia

trajektorii

x

d

t



y

d

t

/

charakterystycznego punktu robota MK,

le˙z ˛

acego na ´srodku osi kół. W tym celu wybieramy

współrz˛edne y

1



x

y

2



y tego punktu jako wyj´scia

linearyzuj ˛

ace i szukamy mo˙zliwo´sci odsprz˛e˙zenia ukła-

du (1)-(2) z tymi wyj´sciami przy pomocy sprz˛e˙zenia
zwrotnego od stanu. Kolejne ró˙zniczkowanie wyj´s´c
linearyzuj ˛

acych daje nast˛epuj ˛

ace wyniki

˙

y

1



˙

x



η

123

cos

θ

˙

y

2



˙

y



η

123

sin

θ

oraz

01

1

1

1

2

1

1

1

1

3

¨

y

1



˙

η

123

cos

θ



η

123

sin

θ

˙

θ



˙

η

123

cos

θ



η

123

sin

θ

1
d

η

2



η

1

4

˙

η

123

cos

θ



˙y

2

1
d

η

2



η

1



¨

y

2



˙

η

123

sin

θ



η

123

cos

θ

˙

θ



˙

η

123

sin

θ



η

123

cos

θ

1
d

η

2



η

1

5

˙

η

123

sin

θ



˙y

1

1
d

η

2



η

1

6

Przy oznaczeniach

η

123



P

η

1

d

η

2



η

1



Q

η

gdzie

P

87

1

1

1

9

Q



1

d

7



1

1

0

9

ostatnie dwie formuły przyjmuj ˛

a posta´c

:

¨

y

1



P ˙

η

cos

θ



˙

y

2

Q

η

¨

y

2



P ˙

η

sin

θ



˙

y

1

Q

η

czyli, bior ˛

ac pod uwag˛e równania (2),

;

¨

y

1

<

˙

y

2

Q

η



PM

=

1

N cos

θ



PM

=

1

B cos

θ

u

¨

y

2



˙

y

1

Q

η



PM

=

1

N sin

θ



PM

=

1

B sin

θ

u



(3)

Nietrudno zauwa˙zy´c, ˙ze macierz

>

P cos

θ

P sin

θ

?

M

=

1

B

statycznego odsprz˛e˙zenia układu (3) jest osobliwa. Po-
szukuj ˛

ac mo˙zliwo´sci odsprz˛e˙zenia dynamicznego zdefi-

niujemy now ˛

a współrz˛edn ˛

a stanu

q



P M

=

1

Bu

i przepiszemy równania (3) w postaci

;

¨

y

1

<

˙

y

2

Q

η



PM

=

1

N cos

θ



q cos

θ

¨

y

2



˙

y

1

Q

η



PM

=

1

N sin

θ



q sin

θ

Rys. 2. Schemat blokowy algorytmu sterowania

dogodnej do dalszego ró˙zniczkowania. Pochodne rz˛edu
trzeciego wyj´s´c linearyzuj ˛

acych mo˙zna wyrazi´c w posta-

ci

y

@

3

A

1



f

1

α

η

θ

q

6

˙

q cos

θ



˙

y

2

QM

=

1

Bu



PM

=

1

∂

N

∂η

M

=

1

B cos

θ

u

y

@

3

A

2



f

2

α

η

θ

q

6

˙

q sin

θ



˙

y

1

QM

=

1

Bu



PM

=

1

∂

N

∂η

M

=

1

B sin

θ

u



(4)

gdzie

f

1

α

η

θ

q

5<

¨

y

2

Q

η



∂

@

PM

B

1

N

A

∂α

η

3

R

cos

θ



PM

=

1

N



q



Q

η

sin

θ



˙

y

2

QM

=

1

N



PM

=

1

∂

N

∂η

M

=

1

N cos

θ

f

2

α

η

θ

q

5<

¨

y

1

Q

η



∂

@

PM

B

1

N

A

∂α

η

3

R

sin

θ



PM

=

1

N



q



Q

η

cos

θ



˙

y

1

QM

=

1

N



PM

=

1

∂

N

∂η

M

=

1

N sin

θ



Wprowadzenie nowych wej´s´c

˙

q



v

1

QM

=

1

Bu



v

2

(5)

takich ˙ze

>

PM

=

1

B

QM

=

1

B

?

u

DC

q

v

2

E

F

u



R

C

q

v

2

E



R

1

q



R

2

v

2

R



R

α



>

PM

=

1

B

QM

=

1

B

?

=

1

przekształca wzór (4) do postaci

G

y

@

3

A

1

y

@

3

A

2

H



g

α

η

θ

q

/

>

cos

θ



˙

y

2



h

α

η



cos

θ

sin

θ

˙

y

1



h

α

η



sin

θ

?

C

v

1

v

2

E



g



Dv

(6)

przy odpowiednio zdefiniowanych funkcjach g i h. Łatwo
zauwa˙zy´c, ˙ze nowa macierz odsprz˛e˙zenia D



D

α

η

θ



jest nieosobliwa, o ile tylko pr˛edko´s´c ruchu post˛epowego
robota

˙

y

1

cos

θ



˙

y

2

sin

θ

I



0



Przy zało˙zeniu, ˙ze robot si˛e porusza, sprz˛e˙zenie zwrotne

w



g



Dv

F

v

<

D

=

1

g



D

=

1

w

pozwala na uzyskanie w układzie (1)-(2) rozszerzonym o
(5) odsprz˛e˙zonego odwzorowania wej´scie-wyj´scie

G

y

@

3

A

1

y

@

3

A

2

H

DC

w

1

w

2

E



(7)

Do ´sledzenia trajektorii x

d

t



y

d

t

J

wystarczy zatem

zastosowa´c regulator liniowy

;

w

1



y

@

3

A

1d



k

12

¨

y

1



¨y

1d

&

k

11

˙

y

1



˙

y

1d

&

k

10

y

1



y

1d



w

2



y

@

3

A

2d



k

22

¨

y

2



¨y

2d

&

k

21

˙

y

2



˙

y

2d

&

k

20

y

2



y

2d



(8)

ze wzmocnieniami dobranymi w taki sposób, by układ
równa ´n bł˛edu regulacji e

1



y

1



y

1d

e

2



y

2



y

2d

;

e

@

3

A

1



k

12

¨

e

1



k

11

˙

e

1



k

10

e

1



0

e

@

3

A

2



k

22

¨

e

2



k

21

˙

e

2



k

20

e

2



0

był asymptotycznie stabilny. Schemat blokowy algoryt-
mu ´sledzenia trajektorii, którego syntezy dokonali´smy w
tym rozdziale został przedstawiony na rysunku 2.

4.

SYMULACJE KOMPUTEROWE

W celu zilustrowania działania zaproponowanego algo-
rytmu sterowania przeprowadzili´smy symulacje w ´srodo-
wisku MATLAB. Przyj˛eli´smy parametry modelu robota
zgodne z obiektem rzeczywistym [6, 7] (m

k



2

m

o



0



5

m

c



7

R



0



254

l



0



12

d



0



225

l

c



0



165

r

c



0



035

k

1



k

2



0



05). Wzmocnienia regu-

latora (k

10



k

20



7

k

11



k

21



20

k

12



k

22



20)

zostały dobrane metod ˛

a Hurwitza.

Dla sinusoidalnej trajektorii zadanej

:

x

d

t

4

1

10

t

y

d

t

4

sin

π

12

t



(9)

Tor ruchu robota

Bł˛edy poło˙zenia

0

0.5

1

1.5

2

2.5

−1

−0.5

0

0.5

1

1.5

x

y

0

5

10

15

20

25

−0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

e

1

, e

2

t

e

2

e

1

Orientacja robota

Wyznacznik macierzy odsprz˛e˙zenia D

0

5

10

15

20

25

−1.5

−1

−0.5

0

0.5

1

1.5

t

θ

0

5

10

15

20

25

0.05

0.1

0.15

0.2

0.25

0.3

t

detD

Sterowania

K ˛

at odchylenia korpusu

0

5

10

15

20

25

−3

−2.5

−2

−1.5

−1

−0.5

0

0.5

1

1.5

2

t

u

1

, u

2

u

1

u

2

0

5

10

15

20

25

−0.5

−0.4

−0.3

−0.2

−0.1

0

0.1

0.2

0.3

0.4

0.5

t

α

Rys. 3. Wyniki symulacji dla trajektorii sinusoidalnej (9)

Tor ruchu robota

Bł˛edy poło˙zenia

−1

−0.5

0

0.5

1

−1

−0.8

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

0.8

1

x

y

0

5

10

15

20

25

−0.1

−0.08

−0.06

−0.04

−0.02

0

0.02

e

1

, e

2

t

e

2

e

1

Orientacja robota

Wyznacznik macierzy odsprz˛e˙zenia D

0

5

10

15

20

25

1

2

3

4

5

6

7

8

9

t

θ

0

5

10

15

20

25

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

t

detD

Sterowania

K ˛

at odchylenia korpusu

0

5

10

15

20

25

−3

−2

−1

0

1

2

3

t

u

1

, u

2

u

1

u

2

0

5

10

15

20

25

−0.4

−0.3

−0.2

−0.1

0

0.1

0.2

0.3

0.4

t

α

Rys. 4. Wyniki symulacji dla trajektorii kołowej (10)

i stanu pocz ˛

atkowego (x



0

y



0



5

ϕ

1



1

ϕ

2





1

α



0

η

1



0



05

η

2



0



05

η

3



0

q



0) uzy-

skali´smy ´sledzenie trajektorii z małymi bł˛edami przy nie-
wielkich amplitudach sygnałów steruj ˛

acych (rysunek 3).

Dla kołowej trajektorii zadanej

:

x

d

t



cos

π

12

t



y

d

t



sin

π

12

t



(10)

i stanu pocz ˛

atkowego (x



0



9

y



0

ϕ

1

K

1

ϕ

2



1

α



0

η

1



0



05

η

2



0



05

η

3



0

q



0) wyniki

symulacji okazały si˛e równie˙z zadowalaj ˛

ace (rysunek 4).

5.

ZAKO ´

NCZENIE

Korzystaj ˛

ac z metody linearyzacji modelu robota przez

dynamiczne sprz˛e˙zenie zwrotne uzyskali´smy algorytm
sterowania dwukołowego robota mobilnego MK, realizu-
j ˛

acy ´sledzenie zadanej trajektorii ruchu robota na płasz-

czy´znie. Przy syntezie algorytmu wykorzystali´smy nie-
holonomiczny model kinematyki oraz model dynamiki
robota MK uwzgl˛edniaj ˛

acy bezwładno´sci, siły Coriolisa

i tarcie lepkie. Opisany algorytm sterowania pracuje po-
prawnie przy zachowaniu warunku, ˙ze macierz odsprz˛e-

˙zenia D



D

α

η

θ



jest nieosobliwa (pr˛edko´s´c ruchu

post˛epowego robota jest ró˙zna od zera).
Wyniki przeprowadzonych symulacji pokazuj ˛

a, ˙ze algo-

rytm ´sledzenia trajektorii zapewnia efektywn ˛

a redukcj˛e

bł˛edu poło˙zenia przy niewielkich wzmocnieniach regu-
latora i niewielkich amplitudach sygnałów steruj ˛

acych

(momentów wywieranych przez silniki). Własno´s´c ta ma
du˙ze znaczenie z punktu widzenia mo˙zliwo´sci implemen-
tacji algorytmu na rzeczywistym robocie. Kolejnym za-
daniem wynikaj ˛

acym z logiki podj˛etych bada ´n b˛edzie za-

implementowanie przedstawionego algorytmu w sterow-
niku prototypu laboratoryjnego robota mobilnego MK.

TRAJECTORY TRACKING ALGORITHM FOR MK

MOBILE ROBOT

Abstract: The paper presents a trajectory tracking algorithm
for a two wheel mobile robot MK driven with respect to an in-
ternal coordinate frame. The algorithm exploits a linearization
procedure of robot kinematics and dynamics equations by a dy-
namic feedback. The performance of the algorithm has been il-
lustrated with results of computer simulations.

Literatura

[1] B. d’Andrea-Novel, G. Campion i G. Bastin, Con-

trol of nonholonomic wheeled mobile robots by sta-
te fedback linearization, Int. J. Robot. Res.,, vol. 14,
no. 6, 1995, ss. 543-559.

[2] C. Canudas de Wit, B. Siciliano i G. Bastin, Theory

of Robot Control, Springer-Verlag, New York, 1996.

[3] K. Tcho ´n, A. Mazur, I. Dul˛eba, R. Hossa i R. Mu-

szy´nski, Manipulatory i roboty mobilne: Modele,

planowanie ruchu i sterowanie, Akademicka Oficy-
na Wydawnicza, Warszawa 2000.

[4] P. Morin i C. Samson, Feedback control of nonholo-

nomic wheeled vehicles. A survey, Archives of Con-
trol Sciences, w druku.

[5] J. Jakubiak i K. Tcho ´n, The continuous inverse ki-

nematic problem for mobile manipulators: a case
study in the dynamic extension, Proc. 2001 IEEE
Int. Conf. Robot. Automat., Seoul, 2001, ss. 2401-
2406.

[6] M. Kabała, K. Tcho ´n i M. Wnuk, Robot mobilny

nap˛edzany w układzie wewn˛etrznym, Materiały VII
Krajowej Konferencji Robotyki, L ˛

adek-Zdrój, 2001,

t. 1, ss. 149-158.

[7] M. Kabała, K. Tcho ´n i M. Wnuk, Dwukołowy nie-

holonomiczny robot mobilny, Materiały Konferen-
cji Automation 2002, Warszawa, 2002, ss. 269-280.