Czujnik ściany, którego zamontowanie i uruchomienie opisywałem poprzednio, daje pomiary w woltach wyrażonych w jednostkach ADC (zakres 0-4095 odpowiada 0 – 3.3 V). Taka wartość nie jest szczególnie przydatna, dopiero po konwersji na odległość w milimetrach może być wykorzystana do nawigacji w labiryncie.

Prawie jak laborka

Zadanie to przypomina trochę popularne na studiach laborki. Należy zebrać pomiary, wyprowadzić wzór opisujący zależność napięcia od odległości, a następnie sprawdzić, czy odległość obliczana przy jego pomocy zgadza się z rzeczywistością. Zasadnicza różnica jest jednak taka, że efekty tych pomiarów będę później wykorzystywał w praktyce. Ogólnie podczas prac nad robotem robiłem więcej laborkowych czynności np. identyfikacja silników i dobór regulatorów. Gdyby tylko na uczelniach zadania z laborek miały takie realne zastosowania.

Stanowisko laboratoryjne

Pierwszym krokiem było zmierzenie charakterystyki napięciowej czujnika. W tym celu potrzebowałem wykonać pomiary napięcia dla kilku znanych wartości odległości. Przygotowałem do tego zadania prowizoryczne stanowisko laboratoryjne. Z kartki A4 zrobiłem miarkę z zaznaczonymi odległościami co 10 mm. Za jej pomocą mierzyłem odległość od ściany labiryntu do czujnika umieszczonego na robocie. Całość wyglądała tak:

Realizacja softwareowa pomiaru

Soft wykonujący pomiar ADC musiał usuwać wpływ światła otoczenia (ambient light). Inaczej świecące słońce, albo żarówka może wpływać na wyniki. Procedura usuwająca jego wpływ jest dosyć prosta i bazuje na fakcie, że natężenie takiego światła jest stałe w krótkim przedziale czasu. Wystarczy więc zmierzyć je przy wyłączonych nadajnikach IR, a następnie odjąć tą wartość od pomiaru przy włączonym.

Skoro jesteśmy przy diodach IR to tutaj pojawia się drugi kruczek. Po włączeniu diody musi upłynąć pewien czas (czas ustalania) zanim dioda zacznie świecić wiązką o docelowej sile. Tak samo sygnał na odbiorniku nie zmienia się skokowo, tylko narasta w sposób ciągły. Jeśli dzieje się to bardzo szybko, mamy wrażenie, że występuje skok. Oba te czasy można odczytać z not katalogowych elementów, są one rzędu kilku mikrosekund. Są one dla nas ważne dlatego, że pomiaru musimy dokonać dla napięcia w stanie ustalonym. Aby to osiągnąć, po włączeniu diody czekam 1 ms zanim odczytam wartość z ADC.

Procedura pomiaru wygląda tak:

        front_r_off = adc_val_get(ADC_PHOTO_FRONT_R);

        led_on(LED_IR_FRONT_R);
        rtos_delay(1);
        front_r_on = adc_val_get(ADC_PHOTO_FRONT_R);
        data = front_r_on - front_r_off;
        led_off(LED_IR_FRONT_R);

Zbieranie charakterystyki

Pomiary z ADC były wysyłane przez Bluetooth na terminal, następnie odczytywałem je z ekranu komputera i zapisywałem do tabelki. Otrzymałem następujące wyniki:

Distance[mm] Voltage[ADC]
20				3782
30				3477
40				2403
50				1693
60				1298
70				1019
80				847
90				717
100				640
110				570
120				518
130				473
140				440
150				410
160				385
170				370
180				360
190				355
300				130

Napięcie mierzyłem dla zakresu 20 – 180 mm. Przy większych odległościach zmiana robi się zbyt mała aby podawać dokładny dystans. Zrobiłem za to dodatkowy pomiar dla dużo większej odległości (300 mm). Dowiedziałem się z niego, że chociaż nie mam dokładnej odległości, to różnica jest wystarczająca aby określić czy wykryto ścianę. Na razie nie planuję wykorzystywać tej informacji.

Wyznaczanie wzoru

Jako punkt wyjściowy do wyznaczania wzoru posłużył mi artykuł z micromouseonline. Przedstawiona tam formuła ma postać:

ADC = e^{\frac{a}{l + b}}

Gdzie ADC to zmierzona wartość, l to odległość, a a i b to współczynniki zależne od użytych elementów i współczynnika odbicia ściany. Pobawiłem się trochę parametrami a i b, nawet udało mi się w miarę dopasować funkcję do wykresu w pewnym przedziale, ale w innych miejscach się rozjeżdżała:

Uporałem się z tym problemem dzięki obserwacji, że mój wykres w stosunku do funkcji eksponencjalnej jest przesunięty. Dla dużych l wartości ADC nie dążą do 0, tylko do wartości około 250. Postanowiłem więc zmodyfikować wzór:

ADC = e^{\frac{a}{l + b}} + c

Po dodaniu trzeciego parametru udało się znaleźć wartości idealnie pasujące do moich danych:

Do znalezienia wartości współczynników użyłem skryptu matlabowego wykorzystującego funkcję lsqcurvefit robiącego optymalizację numeryczną.

data = importdata('../data/wall_sensor_measurements.txt', '\t', 2);
l = data.data(:,1);
v = data.data(:,5);

F = @(a, data) a(3) + exp(a(1)./(data + a(2)));
x0 = [1954 222 220];

fit = lsqcurvefit(F, x0, l(2:end-1), v(2:end-1))

plot(l, F(fit, l), l, v, 'ro')

Wartości początkowe x0 wybrałem po kilku ręcznych próbach z parametrami. Chodziło o to, aby zacząć od wartości dosyć bliskich optymalnym i nie wpaść w jakieś minimum lokalne. Ostateczne wartości:

a = 1462
b = 151
c = 285

Aby z pomiaru ADC uzyskać odległość w milimetrach muszę tylko odwrócić wyznaczony wzór:

l = \frac{a}{ln(ADC - c)} - b

Implementacja

Mam więc wzór na obliczanie odległości z pomiaru ADC. Jednak nie nadaje się on zbytnio na mikrokontroler. mamy tam dzielenie i przede wszystkim logarytm. Są to operacje kosztowne obliczeniowo i może być problem z wykonywaniem ich w czasie rzeczywistym. Dlatego postanowiłem wygenerować lookup table. Idea jest bardzo prosta – przyspieszam obliczenia w zamian za zajęcie dodatkowej pamięci. Zamiast obliczać wzór na bieżąco, zapisuje we FLASHu obliczone wartości dla każdego możliwego pomiaru ADC. Daje to 4096 elementów po 4 bajty.

Do wygenerowania tablicy posłużył mi kolejny skrypt:

wall_sensor_ident

% Equation: ADC = exp (a / (l + b)) + c
% Parameter values obtained from measurement data.
a = fit(1);
b = fit(2);
c = fit(3);

adc = 0:4095;
distance = a ./ log(adc - c) - b;

file = fopen('adc2dist.c','w');
fprintf(file, 'static const int32_t adc2dist_lookup_table[4096] = {\n\t');

for i = 1:max(size(distance))
    
    if i < c + 50
        fprintf(file,'WALL_NOT_FOUND, ');
    elseif round(distance(i)) > 180
        fprintf(file,'WALL_NOT_FOUND, ');
    elseif round(distance(i)) < 30
        fprintf(file,'WALL_TOO_CLOSE, ');
    else
        fprintf(file,'%d, ',round(distance(i)));
    end

    if i == max(size(distance))
        fprintf(file,'\n};\n');
    elseif (mod(i,8) == 0)
        fprintf(file,'\n\t');
    end
end

fclose(file);

Podczas generowania niektóre wartości zamieniam na stałe WALL_TOO_CLOSE i WALL_NOT_FOUND. Jeżeli ściana jest za blisko, wyniki są bardzo duże i nie da się poprawnie zmierzyć odległości rzędu 10 mm. Ustawiłem więc próg na 30 mm. Jeżeli ściana jest bliżej nie dostanę konkretnej wartości. Z kolei jeśli ściana jest daleko, pomiar staje się niedokładny i nie ma sensu podawać wartości co do milimetra. Lepiej założyć, że nie wykryliśmy ściany i poczekać aż podjedziemy bliżej.

Implementację lookup table można zobaczyć na GitHubie.

Wyniki

Wyniki osiągnięte w testach implementacji przeszły moje najśmielsze oczekiwania. Praktycznie w całym mierzonym zakresie dokładność do 1 mm. Tylko dla 170 – 180 mm rośnie do około 3 mm.

Podsumowanie

Pomiar odległości dla pojedynczego czujnika w warunkach takich samych jak podczas zbierania charakterystyki jest bardzo dokładny. Jednak w rzeczywistości mogą zdarzyć się ściany o innym współczynniku odbicia i odległości się rozjadą. Rozwiązaniem jest dodatkowa procedura kalibracji podobna jak tutaj. Kolejnym problemem jest odbicie nie od całej ściany, tylko od końcówki, albo od samego słupka. Wtedy nie całe światło z diody się odbije dając mniejsze napięcie na fototranzystorze. Jest o tym mowa tutaj. Jednak to są problemy na dalszą przyszłość. Na razie muszę polutować pozostałe czujniki, sprawdzić czy działają tak samo dobrze i napisać ostateczną wersję tasku obsługującego wszystkie sensory.