dr inż. st. of. Paweł Zalewsi Filtracja pomiarów z głowic laserowych słowa luczowe: filtracja pomiaru odległości, PNDS Założenia filtracji pomiaru odległości. Problem wyznaczenia odległości i parametrów ruchu mierzonego puntu adłuba dotyczy oszacowania chwilowego stanu uładu dynamicznego na podstawie pomiarów tego stanu, przy założeniu, że zarówno pomiar, ja i sam proces przetwarzania wewnątrz uładu są obarczone błędem. Rozwiązanie tego problemu możliwe jest poprzez zastosowanie filtru Kalmana lub inaczej reurencyjnego algorytmu dysretnej liniowej filtracji [1,,3]. Załada się, że załócenia i błędy pomiarów są białym szumem typu gaussowsiego. W taim wypadu problem pomiarów odległości za pomocą czujnia laserowego dotyczy oszacowania stanu x n R dysretnego procesu, tóry jest opisywany przez następujące stochastyczne równanie liniowe w chwili (model dynamii stanu): x = A 1 x 1 w (1), natomiast model pomiaru z m R jest opisany następująco: z = H x v (), gdzie: A 1 - macierz przejścia ( n) H - macierz ( n) n wiążąca stan uładu w chwili -1 ze stanem w chwili, m wiążąca stan x z pomiarem z w chwili, w, - zmienne losowe, reprezentujące błąd (szum) przetwarzania (procesu) i pomiaru, v tórych funcje rozładu prawdopodobieństwa są typu gaussowsiego (zmienne o rozładzie normalnym): ( w ) N( 0, Q ), p( v ) N( 0 R ) p,, a funcje gęstości prawdopodobieństwa są równe: 1 ( ) ( w ) µ w 1 f w = exp, f ( v ) = exp σ w π σ w σ v π σ v gdzie: ( v µ ) Q - miara zmienności białego szumu definiowanego zmienną w, dla rozładu normalnego równa wariancji σ w, R - miara zmienności białego szumu definiowanego zmienną v, dla rozładu normalnego równa wariancji σ v, µ, - parametry wartości średnich błędu przetwarzania i pomiaru, w µ v σ w, σ v - parametry odchyleń standardowych błędu przetwarzania i pomiaru, v (3), 47
Niech n xˆ R oznacza estymatę a priori stanu uładu x w chwili, czyli wiedzę o procesie przed tym momentem, oraz n xˆ R oznacza estymatę a posteriori stanu w chwili, czyli wiedzę na podstawie pomiaru z : x ˆ A x (4) = ˆ 1 Na tej podstawie można zdefiniować estymaty błędu e a priori i a posteriori oraz odpowiadające im macierze owariancji P : T - estymaty a priori: e x x, P E[ e e ] T - estymaty a posteriori: e x x, P E[ e e ] ˆ, (5) ˆ. (6) Zasada reurencji dla filtru Kalmana polega na tym, że w danej chwili -tej, doonywany jest pomiar stanu z, na podstawie tórego oraz estymaty a priori w chwili -1 wyznaczana jest estymata a posteriori x ˆ 1. Służy ona następnie do predycji estymaty stanu xˆ w następnym, -tym momencie. A zatem równania opisujące filtr Kalmana dzielą się na dwie ategorie: równania atualizujące w chwili -1 oraz równania predycyjne dla chwili. xˆ 1 Predycja 1 Opóźnienie cylu Atualizacja Rys. 1. Schemat modelu predycyjno-atualizacyjnego filtru Kalmana. xˆ W związu z powyższym otrzymujemy pięć równań Kalmana: 1. równanie estrapolacji stanu (4): x ˆ A xˆ (7) = 1. równanie estrapolacji owariancji błędu procesu: T P = A P 1 A Q (8) 3. równanie wzmocnienia: T [ H P H R ] 1 K (9) T = P H 4. równanie atualizacji stanu: xˆ xˆ K z H xˆ (10) [ ] = 5. równanie atualizacji owariancji błędu procesu: P I K H P (11) gdzie: I - macierz jednostowa o wymiarze [ ] = P, a indes T oznacza transponowanie macierzy 48
Symulacja filtracji pomiaru odległości. Przyładowy algorytm symulacji filtracji pomiaru odległości sensorem laserowym zaimplementowany w środowisu MATLAB na podstawie [] przedstawiono poniżej: function alman_dist(duration, dt) % function alman_dist(duration, dt) - symulacja filtru Kalmana % duration = czas trwania symulacji [s] % dt = odstęp pomiarowy [s] % dists = początowa odległość [m] measnoise = 1; % odchylenie st. - błąd pomiaru odległości [m] accelnoise = -0.01; % zmienność stanu reprezentowana przyśpieszeniem procesu [m/sec^] dists = 10; A = [1 dt; 0 1]; % macierz przejścia H = [1 0]; % macierz wiążąca stan x z pomiarem x = [dists; 0]; % początowy wetor stanu xest = x; % początowa estymata stanu Q = accelnoise^ * [dt^4/4 dt^3/; dt^3/ dt^]; % m. owariancji błędu oszacowania procesu zmiany stanu (odległości) w wyniu ruchu adłuba P = Q; % m. owariancji początowej estymacji R = measnoise^; % m. owariancji błędu pomiaru % ustalenie rozmiaru wetora innowacji Inn = zeros(size(r)); pos = []; % macierz rzeczywistych odległości posest = []; % macierz estymowanych odległości posmeas = []; % macierz pomiarów Counter = 0; for t = 0 : dt: duration, Counter = Counter 1; % Symulacja procesu ProcessNoise = accelnoise * randn * [(dt^/); dt]; x = A * x ProcessNoise; % Symulacja pomiaru MeasNoise = measnoise * randn; z = H * x MeasNoise; % Innowacja Inn = z - H * xest; % Kowariancja innowacji s = H * P * H' R; % Macierz wzmocnienia K = A * P * H' * inv(s); % Estymata stanu xest = A * xest K * Inn; % Kowariancja błędu predycji P = A * P * A' Q - K * H * P * A'; % Zapis parametrów odległości, ich estymat i pomiarów pos = [pos; x(1)]; posest = [posest; xest(1)]; posmeas = [posmeas; z]; end 49
% Wyres symulacji t = 0 : dt : duration; t = t'; plot(t,pos,'r',t,posest,'g',t,posmeas,'b'); grid; xlabel('czas [s]'); ylabel('odleglosc [m]'); title('symulacja filtru Kalmana'); W powyższym algorytmie ruch mierzonego puntu adłuba zasymulowano poprzez dobór parametrów macierzy A i wetora ProcessNoise. Macierz A jest macierzą przejścia uładu ze stanu poprzedniego do bieżącego w oresie dt, wetor ProcessNoise jest wetorem chwilowego przyśpieszenia uzmiennionego według rozładu normalnego funcją randn Problem wyznaczenia odległości i parametrów ruchu mierzonego puntu adłuba dotyczy oszacowania chwilowego stanu uładu dynamicznego na podstawie pomiarów tego stanu, przy założeniu, że zarówno pomiar, ja i sam proces przetwarzania wewnątrz uładu są obarczone błędem. Rozwiązanie tego problemu możliwe jest poprzez zastosowanie filtru Kalmana lub inaczej reurencyjnego algorytmu dysretnej liniowej filtracji [1,,3]. Załada się, że załócenia i błędy pomiarów są białym szumem typu gaussowsiego. W taim wypadu problem pomiarów odległości za pomocą czujnia laserowego dotyczy oszacowania stanu x n R dysretnego procesu, tóry jest opisywany przez następujące stochastyczne równanie liniowe w chwili (model dynamii stanu): 1. czas symulacji równy 60s;. dt = 0,5s; 3. początowa odległość równa 10m; 4. odchylenie standardowe pomiaru odległości równe 1m; 5. odchylenie standardowe przyśpieszenia (w procesie ruchu) równe 0,01m/s ; 6. zmienne odległości i przyśpieszenia generowane według rozładu normalnego. Kolorem czerwonym oznaczono rzeczywiste odległości, olorem niebiesim pomierzone odległości, olorem zielonym odległości estymowane filtrem Kalmana. 50
13 Symulacja filtru Kalmana 1.5 1 11.5 Odleglosc [m] 11 10.5 10 9.5 9 8.5 8 0 10 0 30 40 50 60 Czas [s] Rys.. Symulacja filtracji pomiaru odległości sensorem laserowym. Filtracja pomiarów rzeczywistych odległości. W algorytmie filtracji rzeczywistych pomiarów symulację procesu i pomiaru w algorytmie z rozdz. zastąpią rzeczywiste dane pomiarowe. Rysune 3 przedstawia pracę zbudowanego filtra w środowisu MATLAB dla parametrów: 1. czas symulacji równy 60s;. dt = 1s; 3. początowa odległość równa 10m; 4. odchylenie standardowe pomiaru odległości równe 1m; 5. odchylenie standardowe przyśpieszenia (w procesie ruchu) równe 0,01m/s ; 6. pomiary odległości uzysane z głowicy powięszone 10-rotnie dla przyjętego odch. standardowego pomiarów. Na rys. 3 olorem niebiesim oznaczono pomierzone odległości, olorem zielonym odległości estymowane filtrem Kalmana. 51
14 Measurement filtration ver. 1 13 1 11 x [m] 10 9 8 7 0 10 0 30 40 50 60 Time [s] Rys. 3. Filtracja pomiaru odległości sensorem laserowym wersja pierwsza. Rzeczywiste pomiary z głowicy laserowej obejmują taże pochodną odległości po czasie - prędość zmiany odległości (v). Modyfiując macierz przejścia A do postaci: A = [1 dt v(t1)/dt; 0 1 dt; 0 0 1] oraz macierz owariancji błędu oszacowania procesu zmiany stanu (odległości) w wyniu ruchu adłuba Q do postaci: Q = accelnoise^ * [dt^4/4 dt^3/ dt^; dt^3/ dt^ 1; 0 0 1] algorytm filtracji będzie wierniejszy rzeczywistości. Rysune 4 przedstawia pracę zbudowanego filtra w środowisu MATLAB dla parametrów: 1. czas symulacji równy 60s;. dt = 1s; 3. początowa odległość równa 10m; 4. odchylenie standardowe pomiaru odległości równe 1m; 5. odchylenie standardowe przyśpieszenia (w procesie ruchu) równe 0,01m/s ; 6. pomiary odległości uzysane z głowicy powięszone 10-rotnie dla przyjętego odch. standardowego pomiarów, 7. pomiary prędości uzysane z wartości aprosymowanych przez głowicę laserową jej algorytmem filtracyjnym. 5
Na Rys. 4 olorem niebiesim oznaczono pomierzone odległości, olorem zielonym odległości estymowane filtrem Kalmana. 14 Measurement filtration ver. 13 1 11 x [m] 10 9 8 7 0 10 0 30 40 50 60 Time [s] Rys. 4. Filtracja pomiaru odległości sensorem laserowym wersja druga Podsumowanie Przedstawione wersje algorytmu filtracji pomiarów odległości głowicami laserowymi umożliwiają oretę chwilowych błędów pomiarów w różnych wariantach systemów pomiarowych. Wersja pierwsza opiera się wyłącznie na mierzonych odległościach. Wersja druga bierze pod uwagę dodatowo rejestrowane chwilowe prędości. Możliwość fuzji pomiarów różnych parametrów prowadzi do poprawy jaości stosowanego filtru, a tym samym doładności estymowanych pomiarów. LITERATURA [1] Kalman R., A New Approach to Linear Filtering and Prediction Problems, Transactions of the ASME, Journal of Basing Engineering, vol. 8, March 1960, pp. 34-35. [] Simons D., Kalman Filtering with State Constraints: A Survey of Linear and Nonlinear Algorithms, Cleveland State University Department of Electrical and Computer Engineering, IET Control Theory & Applications, 009. [3] Welch G., Bishop G., An Introduction to the Kalman Filter, Transactions 95-041, Department of Computer Science, University of North Carolina, Chapel Hill, NC 7599-3175, 00. 53
54