Instrukcja laboratoryjna

(1)

W O J S K O W A A K A D E M I A T E C H N I C Z N A im. Jarosława Dąbrowskiego

Zintegrowane Systemy Nawigacyjne

Ćwiczenie laboratoryjne:

Analiza działania systemu nawigacji zliczeniowej

Warszawa, 2020

(2)

1. Cel ćwiczenia

Celem ćwiczenia jest analiza systemu nawigacji zliczeniowej wykorzystującego odometrię różnicową.

W ramach ćwiczenia należy przeprowadzić symulację ruchu robota oraz przeanalizować wpływ poszczególnych źródeł błędów na wyniki działania systemu nawigacyjnego.

2. Opis platformy pomiarowej

Symulowaną platformą pomiarową, wykorzystywaną w ćwiczeniu, jest dwukołowy robot, którego przykładową, fizyczną realizację opracowaną w oparciu o zestaw LEGO Mindstorm EV3 przedstawiono na rys. 1. Posiada on jedną oś napędową, której każde z kół wyposażone jest silnik DC z enkoderem o rozdzielczości definiującej liczbę impulów przypadających na jeden obrót wałka silnika. Zliczając te impulsy możliwe jest określenie położenia i orientacji kątowej (kursu) robota. W tym celu koniczna jest także znajomość parametrów geometrycznych robota, takich jak średnica kół oraz odległość między nimi (baza kół).

Rys. 1. Przykładowa platforma kołowa wyposażona w dwa koła z enkoderami

Z platformą pomiarową związany jest układ b-frame, w którym oś OX

^b

skierowana jest w kierunku przedniej części pojadu, oś OY

^b

skierowana jest w jego prawą stronę, natomiast oś OZ

^b

dopełnia układ prawoskrętny, co przedstawiono na rys. 2. Na poniższym rysunku przez b oznaczono odległość pomiędzy kołami pojazdu, mierzoną wzdłuż osi OY

^b

, natomiast D stanowi średnicę koła.

Rys. 2. Schemat platformy kołowej

(3)

3 3. Odometria różnicowa

Położenie i kurs ruchu robota wyznaczane są w lokalnym układzie odniesienia oznaczonym jako n- frame. W chwili początkowej osie układu n-frame (OX

ⁿ

, OY

ⁿ

, OZ

ⁿ

) pokrywają się z osiami układu b-frame związanego z robotem. Podczas ruchu robota układ nawigacyjny zachowuje jednak stałe położenie i orientację względem Ziemi, natomiast zmianie ulega położenie i orientacja układu b-frame, co przedstawiono na rys. 3

Rys. 3. Ruch robota w układzie n-frame

Zmiana położenia prawego koła w jednym okresie pomiarowym wynosi:

1

D

,

  



 



 −

=



⁻

m n

d

_R

n

^R^k ^R^k (1)

gdzie

d_R

to zmiana położenia prawego koła pomiędzy pomiarem k-1 oraz k,

nR

to liczba impulsów z enkodera prawego koła, natomiast to m rozdzielczość enkodera. Zależność opisująca zmianę położenia lewego koła jest analogiczna:

1

D

,

  



 



 −

=



⁻

m n

d

_L

n

^L^k ^L^k ₍₂₎

Zmiana położenia punktu O dana jest wzorem:

^b

2

R

L

d

d  d + 

=



(3)

(4)

4 Dzięki zastosowaniu odometrów na obu kołach robota możliwe jest wyznaczenie kursu jego ruchu względem układu n-frame. W jednym okresie pomiarowym przyrost kursu  wyznaczany jest za pomocą równania:

b

R

L

d

d − 

= 



(4)

Położenie robota oraz jego kurs w chwili k wyznaczane są poprzez sumowanie przyrostów tych wielkości z chwil poprzednich, zgodnie z zależnościami:

(  +  )

 +

=

_k₋₁

cos

_k₋₁

k

x d

x

(5)

(  +  )

 +

=

_k₋₁

sin

_k₋₁

k

y d

y

(6)



+



=



_k _k₋₁ (7)

4. Symulacja ruchu robota

Celem symulacji ruchu robota jest wygenerowanie danych pomiarowych – sygnałów z enkoderów koła lewego i prawego. Są to idealne dane pochodzące z systemu pozbawionego błędów wynikających z geometrii pojazdu (błędy pomiaru średnicy koła i bazy kół) oraz jego ruchu (poślizgi kół).

Za generowanie danych pomiarowych odpowada skrypt encoder_generator.m.

W części początkowej skryptu (Workspace preparations) definiowana jest nazwa pliku (output_filename), do którego trafią wygenerowane sygnały z enkodera lewego koła (n_left) i enkodera koła prawego (n_right) oraz parametry geometryczne robota. Sygnały z enkodera zapisywane są w wektorach kolumnowych, w których w poszczególnych wierszach znajdują się chwilowe wartości stanu licznika enkodera danego koła.

W sekcji Measurements parametes Użytkownik ma możliwość wprowadzenia okresu pomiarów wykonywanych przez enkodery (T) oraz rozdzielczości ekodera (m, liczby impulsów przypadających na jeden obrót koła).

W sekcji Robot parameters wprowadzane są wymiary geometryczne robota: średnica kół (dim_left, dim_right) oraz baza, czyli odległość między nimi (b). Wszystkie jednoski muszą być wyrażone w układzie SI.

W sekcji Motion wykonywane są obliczenia symulujące ruch obiektu. Po wykonaniu inicjalizacji wektorów n_left i n_right Użytkownik ma możliwość zbudowana trajektorii ruchu obiektu z wykorzystaniem dwóch funkcji (narzędzi): move_forward i oraz turn. Funkja move_forward pozwala na wygenerowanie sygnałów z enkoderów podczas symulowanego ruchu robota do przodu, zaś funkcja turn generuje dane związane w manewrem zakrętu. Zakręt wykonywany jest po zakończeniu ruchu postępowego pojazdu. Podczas tego maneweru jedno z kół jest nireuchome (na nim obraca się robot, jest ono środkiem obrotu), zaś drugie koło wykonuje ruch. Przykładowo, w przypadku skrętu w lewo: lewe koło jest nieruchome, prawe koło obraca się do przodu zmieniając orientację robota. Składnia funkcji move_forward jest następująca:

[nL, nR] = move_forward(dist, t, T, m, dimL, dimR, nL_in, nR_in)

(5)

5 gdzie:

nL – wektor kolumnowy wygenerowanych sygnałów z enkodera lewego koła, nR – wektor kolumnowy wygenerowanych sygnałów z enkodera prawego koła, dist – dystans do przejechania [m],

t – czas wykonywania ruchu [s],

T – okres wykonywania pomiarów enkoderem [s], dimL – średnica lewego koła [m],

dimR – średnica prawego koła [m],

nL_in – wektor sygnałów enkodera z lewego koła z czasu przed uruchomieniem tej funkcji, aktualne wyniki zostaną dołożone do tego wektora, w efekcie powstanie wektor nL

nR_in – wektor sygnałów enkodera z prawego koła z czasu przed uruchomieniem tej funkcji, aktualne wynik zostaną dołożone do tego wektora, w efekcie powstanie wektor nR.

Składnia funkcji turn:

[nL, nR] = turn(angle,t_motion,T,m,dimL,dimR,b,nL,nR) gdzie:

nL – wektor kolumnowy wygenerowanych sygnałów z enkodera lewego koła, nR – wektor kolumnowy wygenerowanych sygnałów z enkodera prawego koła, angle – kąt obrotu, dodatni – w prawo, ujemny – w lewo,

t – czas wykonywania ruchu [s],

T – okres wykonywania pomiarów enkoderem [s], m – rozdzielczość enkodera (liczba ipulsów na obrót), dimL – średnica lewego koła [m],

dimR – średnica prawego koła [m], b – baza kół robota [m],

nL_in – wektor sygnałów enkodera z lewego koła z czasu przed uruchomieniem tej funkcji, aktualne wyniki zostaną dołożone do tego wektora, w efekcie powstanie wektor nL

nR_in – wektor sygnałów enkodera z prawego koła z czasu przed uruchomieniem tej funkcji, aktualne wynik zostaną dołożone do tego wektora, w efekcie powstanie wektor nR.

Przykład: wygeneruj dane z ruchu składającego się z 3 sekwencji:

• przejazdu do przodu na dystans 5 metrów, w czasie 5 sekund,

(6)

6 • obrotu w prawo o 45 stopni, wykonanego w czasie 6 sekund,

• przejazdu do przodu na dystans 2 metrów, w czasie 2 sekund.

Fragment skryptu endocer_generator.m:

%% Measurement parameters:

T = 0.01; % Measurement period [s]

m = 360; % tick per revolution

%% Robot parameters:

dim_left = 0.05; % Wheel diameter [m]

dim_right = 0.05; % Wheel diameter [m]

b = 0.1; % Wheel base[m]

%% Motion

% initialization:

n_left = 0;

n_right = 0;

% Move forward:

s = 5; % [m]

t = 5; % [s]

[n_left, n_right] =

move_forward(s,t,T,m,dim_left,dim_right,n_left,n_right);

% Right turn:

angle = 45 * pi/180; % Turn angle (clockwise positive) t = 6; % motion duration [s]

[n_left, n_right] =

turn(angle,t,T,m,dim_left,dim_right,b,n_left,n_right);

% Move forward:

s = 2; % [m]

t = 2; % [s]

[n_left, n_right] =

move_forward(s,t,T,m,dim_left,dim_right,n_left,n_right);

% …

(7)

7 Dla tak zbudowanego ruchu trajektoria przejazdu wygląda tak, jak przedstawiono na rys. 4.

Rys. 4. Trajektoria ruchu robota wygenerowane na podstawie przykładowego scenariusza

5. Obliczenia nawigacyjne

Obliczenia nawiagcji zliczeniowej realizowne są w skrypcie DR.m. UWAGA – skrypt ten wymaga uzupełnienia przez Użytkownika.

W sekcji Workspace preparations wczytywany jest plik z danumi wygenerowanymi przez skrypt ecnoder_generator.m.

W sekcji Robot parameters Użytkownik ma możliwość wprowadzenia błędów do systamu:

• dim_left_error – błąd pomiaru średnicy lewego koła [m],

• dim_right_error – błąd pomiaru średnicy prawego koła [m],

• b_error – błąd pomiaru bazy kół robota [m],

• sliprate_left – poślizgi lewego koła, [0..1], 0 – brak poślizgów, 1 – każdemu impulsowi towarzyszy drugi wynikający z poślizgu,

• sliprate_right – poślizgi prawego koła, [0..1], 0 – brak poślizgów, 1 – każdemu impulsowi towarzyszy drugi wynikający z poślizgu.

W sekcji DR initialization Użytkownik powinien uzupełnić czynności związane z inicjalizacją obliczeń nawigacji zliczeniowej.

W sekcji DR main loop Użytkownik powinien uzupełnić obliczenia nawigacji zliczeniowej.

W sekcji Presentation Użytkownik powinien wprowadzić kod związany z prezentacją wyników (tworzenie

wykresów itp.).

(8)

8 6. Instrukcja ćwiczenia laboratoryjnego 6.1.

• Wygeneruj dane pomiarowe dla przejazdu wzdłóż linii prostej (odległość 5 metrów, w czasie 5 sekund). Wyniki zapisz w pliku trajectory_straight.mat.

• Zaimplementuj równania nawigacji zliczeniowej w skrypcie DR.m.

• Wykonaj obliczenia nawigacji inercjalnej dla przypadków, w których wartości błędów są zgodne z tab. 1. Każdy przypadek rozpatruj osobno. Dla każdego z nich wyrysuj wykresy: trajektoria bez błędów (przypadek nr 1) + trajektoria z błędami (na wspólnym wykresie), błąd wyznaczania położenia w osi OX, błąd wyznaczania położenia w osi OY, błąd wyznaczania kursu.

• Zinterpetuj wyniki i okreś wpływ poszczególnych błędów na otrzymane wyniki.

Tab.1. Przypadki pomiarowe

Żródło błędów Przypadek pomiarowy

1 2 3 4 5

dim_left_error 0 0.001 0 0 0

dim_right_error 0 0 -0.001 0 0

b_error 0 0 0 0.01 0

sliprate_left 0 0 0 0 0.01

sliprate_right 0 0 0 0 0

6.2. • wygeneruj dane pomiarowe dla ruchu składającego się z 4 pełnych obrotów w kierunku zgodnym do ruchu wskazówek zegara. Wyniki zapisz w pliku trajectory_turn_right.mat.

• W równaniach nawigacji zliczeniowej zadbaj o to, aby kurs przyjmował wartości z przedziału <0, 2π).

• Ustaw zerowe wartości błędów.

• Wykonaj obliczenia nawigacyjne, wyrysuj wykres zmiany kursu w czasie oraz zinterpretuj wyniki.

6.3. • wygeneruj dane pomiarowe dla ruchu składającego się z 4 pełnych obrotów w kierunku przeciwnym do ruchu wskazówek zegara. Wyniki zapisz w pliku trajectory_turn_left.mat.

• W równaniach nawigacji zliczeniowej zadbaj o to, aby kurs przyjmował wartości z przedziału <0, 2π).

• Ustaw zerowe wartości błędów.

• Wykonaj obliczenia nawigacyjne, wyrysuj wykres zmiany kursu w czasie oraz zinterpretuj wyniki.

6.4. • wygeneruj dane pomiarowe dla ruchu składającego się z 4 odcinków prostych (AB, BC, CD, DA)

oraz 3 zakrętów (B, C, D) – patrz rys. Parametry ruchu zawarto w tab. 2 i tab. 3. Wyniki zapisz w

pliku trajectory_complex.mat.

(9)

9 • Wykonaj obliczenia nawigacji inercjalnej dla przypadków, w których wartości błędów są zgodne z tab. 1. Każdy przypadek rozpatruj osobno. Dla każdego z nich wyrysuj wykresy: trajektoria bez błędów (przypadek nr 1) + trajektoria z błędami (na wspólnym wykresie), błąd wyznaczania położenia w osi OX, błąd wyznaczania położenia w osi OY, błąd wyznaczania kursu.