Własny system precyzyjnego pozycjonowania dla robota — bez GPS i GSM

GPS ma zasięg globalny, ale na własnym podwórku jest bezużyteczny — dokładność ±3–10 metrów to za mało, żeby robot wiedział, czy stoi przy trawniku, czy przy grządce. Co gorsza, sygnał satelitarny bywa blokowany przez dach, drzewa albo budynki. Na szczęście możesz zbudować własny, w pełni autonomiczny system pozycjonowania — działający bez internetu, bez GPS i bez GSM — o dokładności do kilku centymetrów. Właśnie temu poświęcony jest ten poradnik.

Zasada działania — trilateracja w 5 minut

Zanim przejdziesz do lutownicy, jedna kluczowa koncepcja: trilateracja. To matematyczna metoda wyznaczania pozycji punktu w przestrzeni na podstawie znanych odległości od trzech punktów o znanych współrzędnych.

Wyobraź sobie, że znasz swoją odległość od trzech drzew w ogrodzie. Każda odległość to promień okręgu wokół danego drzewa. Punkt, w którym wszystkie trzy okręgi się przecinają — to twoja pozycja. W praktyce wystarczą trzy anchory (punkty odniesienia) ustawione w znanych miejscach oraz jeden tag na robocie, który mierzy odległość do każdego z nich.

Kluczowe pytanie: jak mierzyć odległość? Tu właśnie rozchodzą się drogi poszczególnych wariantów.

Wariant 1: UWB — precyzja do ±5 cm (polecany)

UWB (Ultra-Wideband) to technologia radiowa używana m.in. w iPhone'ach do funkcji AirDrop i kluczyków samochodowych. Wysyła ultraszerokie impulsy radiowe i mierzy czas ich przebiegu w obie strony (Two-Way Ranging — TWR). Wynik to odległość z dokładnością do kilku centymetrów, odporna na odbicia i zakłócenia.

Czego potrzebujesz?

• 4× moduł DW1000 (np. Makerfabs UWB, ~30–50 zł/szt. na AliExpress) — 3 jako anchory, 1 jako tag na robocie
• 3× Arduino Nano lub ESP32 dla anchórów (ESP32 polecany — obsługuje ESP-NOW)
• 1× ESP32 na robocie (mózg systemu)
• Zasilanie 3,3 V / 5 V dla każdego anchora (powerbank lub mały akumulator)
• Przewody, obudowy (wydruk 3D lub plastikowe puszki)
• Szacunkowy koszt: 200–300 zł

Jak to działa w praktyce?

Ustawiasz trzy anchory w znanych pozycjach — np. narożniki ogrodu. Tag na robocie kolejno wysyła sygnał do każdego anchora i odbiera odpowiedź. Na podstawie czasu przejścia sygnału oblicza odległość (prędkość światła × czas / 2). Mając trzy odległości i znane współrzędne anchórów, ESP32 robota liczy swoją pozycję w czasie rzeczywistym.

Kod anchora (ESP32 + DW1000)

Biblioteka: arduino-dw1000 od Thomasa Trotzki (GitHub).

#include <DW1000.h>

const uint8_t PIN_CS  = 5;   // Chip Select (SPI)
const uint8_t PIN_RST = 27;  // Reset
const uint8_t ANCHOR_ID = 1; // Zmień na 2 i 3 dla kolejnych anchórów

void setup() {
    Serial.begin(115200);
    DW1000.begin(PIN_CS, PIN_RST);
    DW1000.newConfiguration();
    DW1000.setDeviceAddress(ANCHOR_ID);
    DW1000.setNetworkId(42);       // ten sam ID sieci dla wszystkich
    DW1000.enableMode(DW1000.MODE_LONGDATA_RANGE_LOWPOWER);
    DW1000.commitConfiguration();
    Serial.printf("Anchor %d gotowy\n", ANCHOR_ID);
    DW1000.startReceive();         // czeka na żądania od taga
}

void loop() {
    // Obsługa TWR — biblioteka zarządza wymianą ramek
    // szczegóły: przykład RangingAnchor z repozytorium
}

Obliczanie pozycji na robocie

// Pozycje anchórów (metry, mierz z taśmą mierniczą!)
float ax[3] = {0.0, 8.5, 4.2};
float ay[3] = {0.0, 0.0, 7.0};

// Zmierzone odległości od anchórów 0, 1, 2
float r[3];

bool obliczPozycje(float &x, float &y) {
    // Uproszczona trilateracja — eliminacja liniowa
    float A = 2*(ax[1]-ax[0]), B = 2*(ay[1]-ay[0]);
    float C = r[0]*r[0] - r[1]*r[1]
            - ax[0]*ax[0] + ax[1]*ax[1]
            - ay[0]*ay[0] + ay[1]*ay[1];

    float D = 2*(ax[2]-ax[0]), E = 2*(ay[2]-ay[0]);
    float F = r[0]*r[0] - r[2]*r[2]
            - ax[0]*ax[0] + ax[2]*ax[2]
            - ay[0]*ay[0] + ay[2]*ay[2];

    float det = A*E - B*D;
    if (fabsf(det) < 0.001f) return false; // anchory w linii prostej!

    x = (C*E - F*B) / det;
    y = (A*F - D*C) / det;
    return true;
}

void loop() {
    // Po odebraniu odległości od wszystkich 3 anchórów:
    float posX, posY;
    if (obliczPozycje(posX, posY)) {
        Serial.printf("Pozycja: X=%.2f m, Y=%.2f m\n", posX, posY);
    }
}

Komunikacja anchory → robot przez ESP-NOW

ESP-NOW to protokół Espressif działający bez punktu dostępowego Wi-Fi — latencja poniżej 1 ms, zasięg do 200 m na otwartym terenie. Idealny do przesyłania odległości z anchórów do robota.

// Na każdym anchorze (nadaje pomiar do robota)
#include <esp_now.h>
#include <WiFi.h>

uint8_t adresRobota[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; // broadcast

typedef struct { uint8_t anchor_id; float odleglosc; } Pomiar;

void wyslijPomiar(float dist) {
    Pomiar p = {ANCHOR_ID, dist};
    esp_now_send(adresRobota, (uint8_t*)&p, sizeof(p));
}

// Na robocie (odbiera pomiary z anchórów)
void onOdebrano(const uint8_t *mac, const uint8_t *data, int len) {
    Pomiar p;
    memcpy(&p, data, sizeof(p));
    r[p.anchor_id] = p.odleglosc; // zapisz odległość
    Serial.printf("Anchor %d: %.2f m\n", p.anchor_id, p.odleglosc);
}

💡 Wskazówka: Ustaw anchory jak najwyżej (min. 1,5 m nad ziemią) i unikaj metalowych powierzchni w pobliżu. Dokładność UWB dramatycznie spada przy odbiciach od blach czy siatek.

Wariant 2: Ultradźwięki + ESP-NOW — budżetowe ±15–30 cm

Jeśli masz już kilka zestawów HC-SR04 w szufladzie, możesz zbudować tańszy system oparty na ultradźwiękach. Nie osiągnie dokładności UWB, ale na płaskim terenie bez przeszkód da radę.

Czego potrzebujesz?

• 3× ESP8266 (Wemos D1 Mini) lub ESP32 jako anchory
• 3× moduł HC-SR04 (po jednym na anchor) — łącznie ~20–30 zł
• 1× ESP32 na robocie
• Szacunkowy koszt: 80–120 zł

Jak to działa?

Każdy anchor ma czujnik HC-SR04 wycelowany poziomo w kierunku obszaru działania robota. Robot wysyła co 100 ms sygnał ESP-NOW (broadcast) do wszystkich anchórów — to sygnał wyzwalający pomiar. Każdy anchor odpala swój HC-SR04, mierzy odległość do najbliższego obiektu (czyli robota) i odsyła wynik do robota. Robot zbiera trzy odległości i liczy trilaterację.

⚠️ Ważne ograniczenia: HC-SR04 ma kąt widzenia ~15°, więc anchory muszą być ustawione tak, żeby robot zawsze znajdował się w ich stożku widzenia. Najlepiej sprawdza się na otwartym, płaskim terenie bez wysokiej trawy czy innych przeszkód między anchorem a robotem.

// Anchor — ESP8266 + HC-SR04
#include <ESP8266WiFi.h>
#include <espnow.h>

#define TRIG 14  // D5 na Wemos D1 Mini
#define ECHO 12  // D6
#define ANCHOR_ID 0

typedef struct { uint8_t id; float cm; } Pomiar;

float zmierzOdleglosc() {
    digitalWrite(TRIG, LOW);  delayMicroseconds(2);
    digitalWrite(TRIG, HIGH); delayMicroseconds(10);
    digitalWrite(TRIG, LOW);
    long t = pulseIn(ECHO, HIGH, 30000); // timeout 30ms
    return t * 0.0343f / 2.0f;
}

void onZadanie(uint8_t *mac, uint8_t *data, uint8_t len) {
    float d = zmierzOdleglosc();
    Pomiar p = {ANCHOR_ID, d};
    esp_now_send(mac, (uint8_t*)&p, sizeof(p));
}

Wariant 3: IR beacony — najprostszy start (±30–50 cm)

Podczerwień to najłatwiejszy i najtańszy punkt startowy. Trzy stałe diody IR (modulowane na 38 kHz) + odbiorniki TSOP38238 na robocie. Każda dioda świeci w określonym rytmie — robot rozpoznaje, który beacon jest który, i na podstawie intensywności sygnału (RSSI) lub kąta odbioru szacuje swoją pozycję.

Czego potrzebujesz?

• 3× dioda IR 940 nm (~1 zł/szt.) + rezystor 100 Ω
• 3× odbiornik TSOP38238 (~3 zł/szt.) na robocie
• 1× Arduino Nano lub ESP32 zarządzający beaconami
• Szacunkowy koszt: 30–50 zł

Jak to działa?

Każdy beacon świeci swoim unikalnym kodem (np. beacon A — 3 błyski, beacon B — 5 błysków, beacon C — 7 błysków). Robot ma trzy odbiorniki TSOP ustawione pod różnymi kątami (np. przód, lewy bok, prawy bok). Na podstawie tego, który odbiornik widzi który beacon i z jaką intensywnością, robot szacuje kierunek i przybliżoną odległość.

Podczerwień nie sprawdza się dobrze w słoneczny dzień na zewnątrz (słońce zaburza sygnał), ale w zacienionym ogrodzie lub wewnątrz działa świetnie jako prosty system strefowy.

Wariant 4: WiFi RSSI z ESP32/ESP8266 — to, co już masz

Jeśli masz kilka ESP32 lub ESP8266 w szufladzie, możesz zbudować system pozycjonowania oparty na sile sygnału Wi-Fi (RSSI). Trzy moduły działają jako punkty dostępu o znanych pozycjach, a robot mierzy moc sygnału z każdego z nich i szacuje odległość.

// Robot — skanuje RSSI z anchórów
#include <WiFi.h>

void skanujRSSI() {
    int n = WiFi.scanNetworks();
    for (int i = 0; i < n; i++) {
        String ssid = WiFi.SSID(i);
        int rssi = WiFi.RSSI(i);
        // Konwersja RSSI → odległość (model uproszczony)
        // d = 10^((TxMoc - RSSI) / (10 * n))
        // TxMoc ≈ -59 dBm (dla ESP32 w odl. 1m), n ≈ 2.7 (w terenie otwartym)
        if (ssid == "ANCHOR_A") Serial.printf("A: %d dBm\n", rssi);
        if (ssid == "ANCHOR_B") Serial.printf("B: %d dBm\n", rssi);
        if (ssid == "ANCHOR_C") Serial.printf("C: %d dBm\n", rssi);
    }
}

⚠️ Uczciwe ostrzeżenie: RSSI jest bardzo niestabilny — wiatr, liście, a nawet twoje ciało między robotem a anchorem potrafią zmienić wynik o 10–15 dBm, co przekłada się na błąd pozycji rzędu 1–3 metrów. Ten wariant sprawdza się jako wskazanie strefy (wiem, że jestem w lewej części ogrodu), nie jako precyzyjne pozycjonowanie.

Odometria — niezbędny dodatek do każdego systemu

Żaden z powyższych systemów nie jest idealny w każdej chwili — UWB może chwilowo stracić łączność z anchorem, ultradźwięki odbijają się od przeszkód. Rozwiązaniem jest fuzja danych: zewnętrzny system pozycjonowania (anchor + tag) + odometria (enkodery kół).

Enkodery zliczają obroty kół, a mały procesor liczy, gdzie robot pojechał od ostatniego poprawnego pomiaru z anchórów. Gdy pojawi się nowy dobry pomiar — pozycja jest korygowana. To samo robi GPS w samochodzie z nawigacją, gdy wjeżdżasz do tunelu.

// Dead reckoning — śledzenie pozycji między pomiarami z anchórów
volatile long impulsyL = 0, impulsyR = 0;
const float SREDNICA   = 0.065f;  // średnica koła w metrach
const int   IMP_OBROT  = 20;      // impulsy enkodera na 1 obrót
const float ROZSTAW    = 0.170f;  // rozstaw kół w metrach

float pozX = 0, pozY = 0, kat = 0;

void IRAM_ATTR enkL() { impulsyL++; }
void IRAM_ATTR enkR() { impulsyR++; }

void aktualizujOdometrie() {
    long iL = impulsyL, iR = impulsyR;
    impulsyL = impulsyR = 0;

    float dL = (iL / (float)IMP_OBROT) * PI * SREDNICA;
    float dR = (iR / (float)IMP_OBROT) * PI * SREDNICA;
    float dS  = (dL + dR) / 2.0f;
    float dKat = (dR - dL) / ROZSTAW;

    kat  += dKat;
    pozX += dS * cosf(kat);
    pozY += dS * sinf(kat);
}

// Wywołaj np. co 20 ms w loop()
// Gdy przyjdzie poprawna pozycja z UWB — nadpisz pozX, pozY

💡 Dodaj magnetometr (np. HMC5883L lub QMC5883L, ~5 zł) jako kompas — znacząco zmniejsza dryfowanie kąta przy odometrii.

Porównanie wariantów

Fizyczne rozmieszczenie anchórów — praktyczne wskazówki

• Minimum 3 anchory dla 2D. Czwarty anchor dramatycznie poprawia dokładność i daje redundancję.
• Anchory nie mogą być w jednej linii — trójkąt jak najbliższy równobocznemu daje najlepszą dokładność w całym obszarze.
• Montuj anchory jak najwyżej, powyżej poziomu przeszkód (min. 1,5–2 m) i skieruj antenę ku dołowi / środkowi pola.
• Zmierz pozycje anchórów dokładnie (taśma miernicza lub dalmierz laserowy) — błąd pomiaru pozycji anchora bezpośrednio przekłada się na błąd pozycji robota.
• Zasilaj anchory z powerbanków lub małych akumulatorów LiFePO4 — unikaj długich kabli zasilających, które działają jak anteny zakłócające.

Pomysły na zastosowanie

• 🤖 Robot kosiarka — precyzyjne koszenie według mapy, omijanie grządek i tarasów
• 💧 Automatyczne podlewanie — robot-wózek z pompką, który wie, gdzie jest każda grządka
• 🐾 Lokalizator zwierząt — tag na obroży, anchory w ogrodzie — wiesz, gdzie jest pies
• 🎮 Gry terenowe — paintball z precyzyjną lokalizacją graczy, cyfrowe zagraj w podchody
• 📦 Autonomiczny wózek magazynowy — w garażu, warsztacie, małym magazynie
• 🌱 Mapa ogrodu — robot kartografuje teren i tworzy mapę przeszkód

Jak rozbudować system?

Filtr Kalmana — zamiast brać surowe odległości z anchórów, użyj filtru Kalmana do fuzji danych z UWB i odometrii. Wygładza skoki pozycji i kompensuje chwilowe błędy. Biblioteka SimpleKalmanFilter na Arduino IDE ma prostą implementację 1D; dla 2D użyj matrycowej wersji lub gotowych implementacji na ESP32.

Pozycjonowanie 3D — dołóż czwarty anchor w innej wysokości. Przy UWB wystarczy zmienić obliczenia z 2D na 3D (dodaj równanie dla osi Z). Przydatne, jeśli robot ma się poruszać po pochyłym terenie.

Mapa przeszkód (occupancy grid) — gdy robot zna swoją pozycję, dodaj czujnik odległości (LIDAR lub ultradźwięk) obracający się 360°. Robot buduje mapę ogrodu w czasie rzeczywistym i zapamiętuje przeszkody.

MQTT + Home Assistant — wyślij pozycję przez Wi-Fi do serwera domowego. Możesz śledzić robota w czasie rzeczywistym na mapie, wysyłać komendy i rejestrować trasy.

RTK-GPS jako alternatywa — jeśli masz otwarty teren i budżet ~500–800 zł, moduły RTK-GPS (np. SparkFun ZED-F9P) dają dokładność ±2 cm z poprawkami od stacji bazowej. To gotowe rozwiązanie, bez potrzeby budowy własnej infrastruktury anchórów.

Podsumowanie — od czego zacząć?

Jeśli stawiasz pierwsze kroki: zacznij od odometrii z enkoderami — najtaniej, najprościej, a dostaniesz poczucie jak działa śledzenie pozycji. Gdy robot jeździ już po swojej mapie, dołóż UWB jako korektę co kilka sekund — i masz gotowy, praktyczny system z dokładnością na poziomie kilku centymetrów.

Wariant ultradźwiękowy polecam jako projekt edukacyjny do zrozumienia trilateracji — jest tani i czytelny, ale na zewnątrz ma swoje ograniczenia. IR to dobry punkt wejścia do świata beaconów, jeśli masz zajęty ogród z dużą ilością cienia.

Niezależnie od wybranego wariantu — dokładność zmierzonej pozycji anchórów jest tak samo ważna jak dobry algorytm. Zmierz, zapisz, sprawdź. Powodzenia z projektem!