MOBILNE ROBOTY KOA
OWE
WYKA
AD 05  PRECYZYJNA
NAWIGACJA ODOMETRYCZNA
GRUPY ROBOTÓW
dr inż. Tomasz Buratowski
Wydział Inżynierii Mechanicznej i Robotyki
Katedra Robotyki i Mechatroniki
Wtęp
Założeniem jest śledzenie pozycji i orientacji robota
poprzez implementacje zaawansowanych algorytmów
bazujących na triangulacji i trilateracji.
Precyzyjna nawigacja jest zadaniem trudnym ponieważ
sensory odpowiedzialne za odczyt pozycji są podatne na
błędy:
" Systematyczne
" Losowe
Wynikające np. z niedokładności konstrukcyjnych, bądz

oddziaływania obiektu z otoczeniem.
2
Wtęp
Problem staję się łatwiejszy do rozwiązania w przypadku
określenia przyczyn błędów systematycznych i ich
skorygowania. Jednak to problemu nie rozwiązuje.
W przypadku pojedynczego robota całkowitym
rozwiązaniem jest odniesienie szacowanej pozycji w
stosunku to pozycji o znanych współrzędnych.
W przypadku grupy robotów sytuacja wygląda inaczej.
Pomiar względnej pozycji i orientacji grupy powinien, w
założeniu, efektywnie zredukować błędy losowe.
3
Wtęp
Pierwszym problemem do rozwiązania jest eliminacja
błędów systematycznych  kalibracja odometrii podczas
ruchu.
Przykłady rozwiązań:
" UMBmark  dla napędów różnicowych
" Filtr Kalmana
" Rozszerzony filtr Kalmana
4
Filtr Kalmana  definicja i założenia
" FK jest optymalnym estymatorem, gdyż przy
konkretnych założeniach może on spełniać pewne
kryterium
" FK wykorzystuje w procesie estymacji stanu wszystkie
dostępne pomiary bez względu na fakt
z jaką dokładnością i precyzją zostały one wykonane.
" FK jest algorytmem typu rekursywnego. Informacje są
przetwarzane sukcesywnie, bazując na wartościach
obliczonych w poprzednim kroku.
5
Filtr Kalmana  definicja i założenia
Proces estymacji Filtru Kalmana zakłada znajomość:
" równań dynamiki systemu jak i układu pomiarowego
(równania stanu)
" statystycznego opisu szumów systemu, błędów
pomiarowych oraz niepewności występujące w dynamice
modelu
" wszystkich dostępnych informacji odnośnie stanu
początkowego estymowanych wartości
6
Filtr Kalmana  definicja i założenia
" W filtrach Kalmana przyjmuje się, że żaden wcześniejszy
pomiar nie wpływa na nowo uzyskany i vice versa -
założenie Markova.
" System jak i pomiar obarczone są szumami białymi o
rozkładzie Gausa
" Filtr Kalmana oparty jest o wzór Bayesa, co oznacza, że
wymagamy od niego propagację gęstości
prawdopodobieństwa warunkowego interesujących nas
wielkości. Założenie to jest uwarunkowane wiedzą na
temat aktualnych danych z czujników.
7
MODELOWANIE SYSTEMU
" Przyjęto różnicowy model napędu
" Charakterystyczne punkty konstrukcji przedstawia
rysunek:
F
l =AC =ABz1
1 z1
l =AS
xS
2
l =AHxS
3
y3
l =AHzS
4
l =ADxS
5
a�1
a�
2
y0
b�
B C
K L
1 2
x
z0 0 0
8
f�
C
B
,z ,z
2
z ,z
1
C
x
C
2
x
2
H
4
,y
4
x
y
S
S
x
1
x
A
x
x
1
2
S
3
B
x
1
x
3
D
f�
B
4
y
y
2
1
MODELOWANIE SYSTEMU
Pozycję i orientację w układzie dwuwymiarowym można
T
opisać przy użyciu trzech parametrów: Xo =� [�x y q�]�
gdzie: x,y są współrzędnymi kartezjańskimi, a 5�� jest
orientacją wyrażoną jako azymut robota w układzie x,y.
Dla określenia parametrów odpowiedzialnych za błędy
T
X =� [�x y q� d� d� d�d ]�
powyższy opis należy rozszerzyć:
R L
gdzie: d�R, d�L odpowiadają błędom systematycznym
dystansu przemierzonego przez koło prawe i lewe, d�d błąd
długości osi, w stosunku to nominalnej wartości.
9
MODELOWANIE SYSTEMU
Nawigacja zostanie zamodelowana jako proces
dyskretny, gdzie bieżąca pozycja Xi+1 jest obliczana jako
suma poprzedniej Xi i wektora przesunięcia ostatniego
kroku. Wektor jest określany na podstawie odczytów z
enkoderów Ui i jest narażony na losowy błąd Gaussa vi
Xi+�1 =� f (�Xi ,Ui , )�� Xi+�1 =� Xi +� F�i ��(�W�i ��Ui +�i )�
i
10
MODELOWANIE SYSTEMU
Macierze F�i i W�i opisują zależność pomiędzy odczytami
enkoderów, błędami losowymi i przemieszczeniem robota.
Dla napędu różnicowego otrzymujemy:
T
cosq�i sinq�i
1
��
0 0 0ł�
ę� ś�
d�Ri 0
�� ł�
2 2 d ��d�di
W�i =�
ę� ś�
F�i =�
ę�
0 d�Li ś�
i
ę�cosq� sinq�i -� 1 0 0 0ś�
�� ��
ę� ś�
2 2 d ��d�di
�� ��
11
MODELOWANIE SYSTEMU
Dla napędu różnicowego odczyty enkoderów mają
formę:
T
Ui =� [�d�r�ieR d�r�ieL]�
gdzie d�r�eR i d�r�eL odpowiadają przesunięciu koła prawego i
lewego w i-tym kroku.
12
MODELOWANIE SYSTEMU
Dla napędu synchronicznego wektor błędów losowych vi
z rozkładem normalnym i zerową średnia możemy zapisać:
2
r�
��s� r�
=� Kr� �� r�
��
�� N(�0,s� )�
��
T
i r�
q�
=�[� ]� �� ��
ir� i
q�
i
2
�� N(�0,s�q� )�
�� =� Kq� �� r�
�� i
��s�q�
gdzie 5�� - dystans jednego kroku. Parametry Kr� i Kq�
opisują niesystematyczny składnik błędu odometrii.
Parametry te związane są z właściwościami terenu, po
którym robot się porusza.
13
MODELOWANIE SYSTEMU
Dla napędu różnicowe macierz kowariancji:
2
R ��s� R =� Kw �� d�r�ieR
��
�� N(�0,s� )�
��
T
i R
i =�[�iR iL]� �� ��
L
2
�� N(�0,s� )�
�� i L
��
��s� L =� Kw �� d�r�ieL
gdzie 5���5��5�V�5�R�5�?� i 5���5��5�V�5�R�5�E� - wartość absolutna
przemieszczenia kół. Parametr Kw  charakteryzuje losowy
błąd przesunięcia każdego z kół.
2
��d�r� eR 0 ł�
��s� R ł�
0
Q =� =� Kw i ś�
ę�
ę� ś�
2
0 0 d�r�ieL ś�
s�
ę�
�� L ��
�� ��
14
MODELOWANIE SYSTEMU  model grupy
robotów
Modelując grupę n robotów, indywidualne modele ruchu
należy połączyć w jeden ogólny model. Model ruchu dla
T
T T
Xi =� [�X1i K� X ]�
całego stanu systemu , jest superpozycją
ni
macierzy indywidualnych robotów:
F�i =� diag{�F�1i ,K�,F�ni}�
W�i =� diag{�W�1i ,K�,W�ni}�
Odczyt enkoderów i wektor błędów losowych dla grupy
robotów:
T T
T T T T
Ui =� [�U1i K� Uni]� i =�[�v1i K� vni]�
15
MODELOWANIE SYSTEMU  model
obserwacji
Dla pojedynczej pary robotów obserwacje są wyrażone jako:
ć� ��
yt -� yo ��
j�
d
��
j�ot =� a tan�� -�q�o +�w�o
dot =� (xt -� xo )2 +� (yt -� yo )2 +�w�o
xt -� xo ��
Ł� ł�
gdzie:
" dot  odległość pomiędzy obserwatorem a celem,
" j�ot  kąt pomiędzy kierunkiem ruchu obserwatora i
segmentu łączącego obserwatora z celem
" q�o  orientacja obserwującego robota,
" xo, yo  szacowane wspołrzędne obserwującego robota
" xt, yt  szacowane współrzędne pojazdu  referyncyjnego
punktu,
" w�o  błąd obserwacji o normalnym rozkładzie i zerową
średnią.
16
MODELOWANIE SYSTEMU  model
obserwacji
Dla jednego robota pełną obserwację można zapisać
jako:
T
Zk =� [�dk1K�dkk -�1 dkk +�1K�dkn j�k1K�j�kk -�1 j�kk +�1K�j�kn]�
gdzie: n  liczba robotów w grupie.
Dla całej grupy robotów, obserwacja jako pojedynczy
wektor:
T
T T
Z =� [�Z1 K� Zn ]�
17
MODELOWANIE SYSTEMU  rozszerzony
filtr Kalmana
Przedstawiony dotąd model może być
scharakteryzowany dwoma funkcjami:
" f()  ruchu
" h() - obserwacji
X =� f (�X ,Ui , )�
i+�1 i i
Zi =� h(�X ,w�i )�
i
18
MODELOWANIE SYSTEMU  rozszerzony
filtr Kalmana
Model używa filtru Kalmana w celu określenia bieżącej
konfiguracji poruszającego się pojazdu (pozycja, orientacja
wartości parametrów systematycznych)
Filtr używa odczytów enkoderów U i obserwacji Z to
oszacowania bieżących wartości parametrów w dwóch
krokach:
" Krok przewidywania
" Krok korygowania
19
MODELOWANIE SYSTEMU  rozszerzony
filtr Kalmana
Równania przewidywania rzutują stan X i kowariancję P
otrzymane z i-go kroku na krok i+1
-�
Ć Ć
X =� f (�X ,Ui ,0)�
i+�1 i
-�
Pi+�1 =� Ai Pi AiT +�WiQWiT
gdzie W jest macierzą Jakobianową pochodnych
cząstkowych funkcji f względem błędów losowych v, A jest
Jakobianem pochodnych cząstkowych funkcji f względem
stanu X, Q jest kowariancją macierzy błędów losowych.
20
MODELOWANIE SYSTEMU  rozszerzony
filtr Kalmana
W kroku korekcji równania uaktualniają stan i
kowariancje oszacowane na podstawie pomiarów:
-�1
Ki =� Pi-�HiT (�Hi Pi-�HiT +� R)�
-� -�
Ć Ć Ć
X =� X +� Ki(�Zi -� h(X ,0))�
i i i
Pi =� (�I -� Ki Hi )�Pi-�
gdzie  K  macierz wzmocnienia Kalmana, H  macierz
Jakobianowa pochodnych cząstkowych funkcji h względem
X, R  macierz kowariancji 5��
21
STANOWISKO POMIAROWE
Dwukołowy robot do zastosowań wewnątrz pomieszczeń
traktowany jest jako system mechatroniczny
nieholonomiczny. Założono brak poślizgu pomiędzy kołami a
podłożem.
W celu zaprezentowania wyników implementacji
algorytmu przeprowadzono test przy użyciu 1 robota w
ruchu i dwóch robotów pozostających w bezruchu. Robot
nawiguje po kwadratowe trajektorii o boku 0.9m
22
STANOWISKO POMIAROWE
W otoczeniu obecne są dwie przeszkody bierne (robot
oblicza odległość od przeszkód) oraz dwa roboty aktywne
wysyłające i odbierające informacje o otoczeniu (oba roboty
obliczają odległość pomiędzy sobą)
Ruchomy robot oblicza swoją pozycję i orientacja
bazując na fuzji sensorów:
" Dwa czujniki podczerwone
" Dwa czujniki ultradzwiękowe
23
STANOWISKO POMIAROWE
Robot nawiguje po danej trajektorii poruszając się w
sekwencji:
1.
2.
24
STANOWISKO POMIAROWE
3.
4.
5. 6.
25
STANOWISKO POMIAROWE
7.
8.
26
STANOWISKO POMIAROWE - wyniki
Trajectory d�d
0.8
1.02
0.6
1.015
0.4
1.01
0.2
1.005
1
0
0.995
-0.2
0.99
-0.4 0.985
0.98
-0.6
0.975
-0.8
-1 -0.5 0 0.5 1 5 10 15 20 25 30
X
Distance [m]
Trajektoria otrzymana ze stanowiska Błąd osi kół
27
d
Y
d�
STANOWISKO POMIAROWE - wyniki
d�L d�R
1.005
1
1
0.99
0.995
0.99
0.98
0.985
0.97
0.98
0.975
0.96
0.97
0.95
0.965
0.96
0.94
0.955
0.93
5 10 15 20 25 30 5 10 15 20 25 30
Distance [m] Distance [m]
Błąd systematyczny koła prawego
Błąd systematyczny koła lewego
28
L
R
d�
d�
DZI
KUJ
ZA UWAG