Wizualizacja pomiarów skanera LIDAR

Wizualizacja pomiarów skanera LIDAR

Skanery LIDAR to urządzenia wykonujące pomiary odległości poprzez emitowanie i odbieranie impulsów światła lasera. Technologia ta stosowana jest między innymi w algorytmach nawigacji oraz orientacji przestrzennej pojazdów autonomicznych, tworzeniu wirtualnych map terenów o wysokiej rozdzielczości, skanowaniu przedmiotów, a nawet badaniu warunków atmosferycznych.

Jesteśmy członkami Koła Naukowego Elektroniki i Informatyki w Knurowie (knei.pl), które od 2015 roku zrzesza uczniów szkół podstawowych, średnich oraz studentów pomagając im rozwijać się w zakresie elektroniki, informatyki i nauk spokrewnionych. Sklep Kamami.pl wypożyczył nam skaner LIDAR, który zamierzamy zastosować w systemach naszego autonomicznego łazika marsjańskiego „Knurów Rover”.

Fotografia 1. Płytka STM32F746-Discovery z wbudowanym wyświetlaczem

Naszym pierwszym projektem zapoznawczym było opracowanie oprogramowania wizualizującego w czasie rzeczywistym dane odbierane z LIDAR-u. Przygotowaliśmy dwie wersje na różne platformy:

  • na płytkę STM32F746-Discovery (fotografia 1) z wbudowanym wyświetlaczem,
  • na komputery z systemami Windows, Linux i macOS.

Kod źródłowy obu aplikacji udostępniony jest na platformie GitHub.

LIDAR

Skaner, który otrzymaliśmy to RPLIDAR A3M1 firmy Slamtec (fotografia 2), umożliwiający skanowanie płaszczyzny 2D w zakresie 360°. Ma możliwość regulacji prędkości obrotu głowicy oraz wykonuje do 16000 pojedynczych pomiarów na sekundę. W zależności od zastosowań, dopasowany jest zarówno do pracy wewnątrz jak i na zewnątrz pomieszczeń. Maksymalny promień skanowania wynosi 25 metrów, ale jest zależny od warunków oświetleniowych i kolorów skanowanego otoczenia.

Fotografia 2. Skaner LIDAR typu RPLIDAR A3M1 firmy Slamtec

Komunikacja z LIDAR-em przebiega z użyciem interfejsu UART, a producent udostępnia otwarto-źródłowe SDK (Standard Development Kit), które zwalnia nas z obowiązku tworzenia własnych funkcji dotyczących wymiany oraz synchronizacji danych. Pakiet SDK jest dostępny na systemy Windows, Linux i macOS.

Wizualizacja na platformę STM32

Zadania jakie postawiliśmy przed mikrokontrolerem STM32F746 obejmowały:

  • konfigurowanie LIDAR-u do pracy,
  • odbieranie danych ze skanera,
  • generowanie dwuwymiarowej mapy przestrzeni na zintegrowanym wyświetlaczu płytki Discovery.

Do pracy z płytką użyliśmy środowiska STM32CubeIDE z intuicyjnym generatorem kodu pozwalającym na łatwe inicjowanie mikrokontrolera, biblioteki producenta BSP (Board Support Platform) i HAL (Hardware Abstraction Layer).

Po wygenerowaniu kodu inicjującego układ, odrobinę oszukaliśmy IDE zmieniając nazwę pliku main.c na main.cpp, aby kontynuować programowanie w języku C++ (Cube domyślnie generuje kod w C). Następnie opracowaliśmy sposób wysyłania poleceń do LIDARu, kontrolę szybkości obrotu przez PWM oraz odbiór i analizę danych odebranych od niego z użyciem interfejsu UART kontrolowanego przez moduł DMA (Direct Memory Access) mikrokontrolera. DMA umożliwia odczyt i zapis do peryferiów (w tym przypadku UART) i pamięci RAM bez zbędnego obciążania procesora (rysunek 1).

Rysunek 1. Schemat blokowy ilustrujący działanie programu na platformie STM

Odbiór i analiza danych

Moduł DMA z użyciem interfejsu UART odbiera ramkę 132 bajtów z LIDARu, po czym wywołuje przerwanie, w którym najnowsza ramka danych porównywana jest z poprzednią. W tym czasie odbierana jest kolejna ramka danych. W procesie przetwarzania odebranych danych tworzona jest chmura punktów na płaszczyźnie 2D. Opracowanie tego mechanizmu było niełatwym zadaniem ze względu na brak potrzebnych informacji w dokumentacji protokołu wymiany danych z LIDAR-em.

Strukturę ramki danych rozszyfrowaliśmy na podstawie kodu SDK i opracowaliśmy funkcje dekodujące. Producent zaleca korzystanie z SDK, odradzając samodzielną implementację dekodowania danych, jednak ze względu na odmienność platform wspieranych przez SDK i STM32 między innymi pod względem dostępności zasobów i działania UART, zdecydowaliśmy się na pozornie trudniejszą opcję.

Wyświetlanie

Gdy mapa jest kompletna, zapis nowych danych zostaje wstrzymany, a chmura punktów zostaje złożona w obraz i wyrenderowana na ekranie (fotografia 3). Wyświetlacz jest obsługiwany przez sprzętowy kontroler LTDC, który jest konfigurowany z pomocą biblioteki BSP. Aby nie wyświetlać na ekranie aktualnie edytowanego obrazu, bufor obrazu składa się z dwóch warstw. Edytowana w danym momencie warstwa jest zawsze niewidoczna. Później warstwy są podmieniane, a cykl odbioru i wyświetlania zostaje powtórzony.

Fotografia 3. Obraz wygenerowany na ekranie zestawu STM32F746-Discovery

Warto nadmienić, że obie warstwy bufora przechowywane są w zewnętrznej kości SDRAM, ponieważ wewnętrzna pamięć RAM mikrokontrolera nie zmieściłaby nawet jednej. Ze względu na specyfikę pamięci SDRAM, na potrzeby optymalizacji umieściliśmy warstwy w wydzielonych bankach (niezależnych sektorach pamięci o identycznym rozmiarze, tu 4 MB), używanych wyłącznie przez LTDC. Gdyby warstwa obrazu była umieszczona częściowo w jednym, a częściowo w drugim, sąsiadującym banku (co mogłoby np. wynikać z nieprzemyślanego wyboru adresu warstwy), przy zapisie lub odczycie warstwy pojawiłoby się opóźnienie związane z przejściem do kolejnego banku.

Dalsza rozbudowa

Program na STM32 przeznaczony jest do dalszej rozbudowy. Mikrokontrolery z tej rodziny mają szeroki wachlarz możliwości i nie ograniczają nas do wizualizacji danych na ekranie. Chmury punktów można, na przykład zapisać na karcie microSD, strumieniować przez Wi-Fi lub użyć do innych zadań.

Wizualizacja na platformę Desktop

Aplikacja desktopowa, podobnie jak implementacja na STM32, umożliwia wizualizację pomiarów w czasie rzeczywistym, a także daje możliwość większego eksperymentowania z funkcjonalnościami dzięki elastyczności jaką oferuje ta platforma.

Po pobraniu pliku wykonywalnego lub samodzielnej kompilacji program jest gotowy do użycia. Użytkownik określa jego zachowanie za pomocą argumentów wiersza poleceń. Myśląc o przyszłości staraliśmy się nadać programowi nieco modularnej budowy. Pętla główna składa się z trzech niezależnych części: mechanizmu pobierania danych, opcjonalnych operacji na danych oraz wizualizacji danych (rysunek 2).

Rysunek 2. Schemat blokowy aplikacji desktopowej

Używając wspomnianych argumentów wiersza poleceń jesteśmy w stanie skonfigurować program i określić z których implementacji chcemy skorzystać. Szczegółowy i zaktualizowany wykaz ustawień znaleźć można w repozytorium projektu w serwisie GitHub lub po uruchomieniu programu z argumentem --help.

Pobieranie danych

Pierwsza część pętli głównej, czyli pobieranie danych, ma za zadanie przede wszystkim komunikację z LIDAR-em. W tym przypadku skorzystaliśmy z wysokopoziomowego SDK udostępnionego przez producenta LIDAR-u, co znacznie przyspieszyło naszą pracę. Innym źródłem danych mogą być odpowiednio sformatowane pliki tekstowe, które mogą składać się z pojedynczej chmury punktów lub też ich serii (wtedy program odtworzy jedną po drugiej niczym film). Takie rozwiązanie nie wymaga do działania połączenia z LIDAR-em.

Wizualizacja

Wzorcowa implementacja interfejsu graficznego programu została napisana z zastosowaniem, bardzo przyjaznej programiście, biblioteki graficznej SFML. Implementacja w intuicyjny sposób wizualizuje chmurę punktów uzyskanych przez skaner (rysunek 3) oraz oferuje kilka pomocnych skrótów klawiszowych. Skróty te pozwalają na zapis zrzutu ekranu lub wyświetlanej chmury do pliku. W lewej części okna ukazana jest inna reprezentacja danych w postaci histogramu odległości od kąta pod jakim została ona zmierzona (puste miejsca oznaczają odrzucone pomiary).

Rysunek 3. Efekty działania aplikacji

Dodatkowo istnieje jeszcze jedna (bardzo prymitywna) implementacja wizualizacji, której jedynym zadaniem jest wypisywanie wartości poszczególnych par kąta i odległości na standardowe wyjście programu.

Operacje na danych

Środkowa część pętli głównej aplikacji przeznaczona jest na kod opcjonalnych operacji, które mają możliwość modyfikacji wyświetlanej chmury punktów. Mogą one posłużyć programiście do definiowania dodatkowych zachowań, które będą wykonywane przez program. Przykładową implementacją tego interfejsu może być zapis, wspomnianej wcześniej, serii chmur punktów do pliku. W przyszłości planujemy dalszy rozwój oprogramowania poprzez wzbogacanie go o nowe implementacje w zależności od naszych potrzeb.

Podsumowanie

Skanery LIDAR to interesujące urządzenia, które stale znajdują nowe zastosowania i stają się coraz bardziej dostępne. Zaprezentowane przez nas oprogramowanie może stanowić ciekawe wprowadzenie do dalszych prac nad projektami korzystającymi z tej technologii.

Bartłomiej Dudek
Szymon Bednorz
knei.pl

Odnośniki:
GitHub projektu STM32: http://bit.ly/3tzw93P
GitHub projektu Desktop: http://bit.ly/3bUvXWB
Youtube projektu STM32: https://bit.ly/3vvYZUh
Youtube projektu Desktop: https://bit.ly/3cBhgqG

Artykuł ukazał się w
Elektronika Praktyczna
kwiecień 2021

Elektronika Praktyczna Plus lipiec - grudzień 2012

Elektronika Praktyczna Plus

Monograficzne wydania specjalne

Elektronik listopad 2021

Elektronik

Magazyn elektroniki profesjonalnej

Raspberry Pi 2015

Raspberry Pi

Wykorzystaj wszystkie możliwości wyjątkowego minikomputera

Świat Radio listopad - grudzień 2021

Świat Radio

Magazyn krótkofalowców i amatorów CB

Automatyka Podzespoły Aplikacje listopad 2021

Automatyka Podzespoły Aplikacje

Technika i rynek systemów automatyki

Elektronika Praktyczna listopad 2021

Elektronika Praktyczna

Międzynarodowy magazyn elektroników konstruktorów

Elektronika dla Wszystkich listopad 2021

Elektronika dla Wszystkich

Interesująca elektronika dla pasjonatów