Wyliczanie trajektorii ruchu robota

[draqo]

Chcialbym zeby moj robot przejechal labirynt. Zamontowalem czujnik ultradzwiekowy na serwie, ktory skanuje otoczenie. Jako wynik dostaje odleglosci.

Istnieja jakies algorytmy dzieki ktorym bede mogl nakierowac robota na punkt gdzie ogleglosc jest najwieksza? Nie posiadam enkoderow ani zyroskopu/akcelerometru.

Bede wdzieczny za wszelkie materialy/wskazowki/linki bo jestem w tym temacie zielony.

Zalaczam rysunek labiryntu.

[Bobby]

A robot zna mapę labiryntu?

[matural]

Czujnik ultradźwiękowy mierzy czas od wysłania sygnału, do jego powrotu. Robisz więc np trzy pomiary: z lewej, z prawej i prosto, a następnie kierujesz robota tam gdzie czas jest największy.

[spartakus]

Czujnik ultradźwiękowy mierzy czas od wysłania sygnału, do jego powrotu. Robisz więc np trzy pomiary: z lewej, z prawej i prosto, a następnie kierujesz robota tam gdzie czas jest największy.

Dodałbym jeszcze skanowanie skosów (czyli na róży wiatrów kierunki: N, NE, E, SE, S, SW, W, NW), ze względu na układ labiryntu pod kątami 45 stopni.

Proponuję także dodać ostatnio odwiedzone pole do tego algorytmu, albo brak skanowania z tyłu, aby robot się nie cofał.

[Polecacz 101]

Produkcja i montaż PCB - wybierz sprawdzone PCBWay!
   • Darmowe płytki dla studentów i projektów non-profit
   • Tylko 5$ za 10 prototypów PCB w 24 godziny
   • Usługa projektowania PCB na zlecenie
   • Montaż PCB od 30$ + bezpłatna dostawa i szablony
   • Darmowe narzędzie do podglądu plików Gerber
Zobacz również » Film z fabryki PCBWay

[draqo]

Robot nie zna labiryntu.

Własnie problem pojawia się na kontach 45 stopni - wtedy zawodzi czujnik. Dlatego chcę teraz skanować obszar od 0 do 180 stopni i aby robot podążał w kierunku gdzie czujnik odczytał największą wartość.

Chciałbym aby płynnie dostosował prędkości kół.

__________

Komentarz dodany przez: Sabre

A w jakim banku są te konta 45° ? 😉

[Demooon]

Mógłbyś zrobić tak:

po przeskanowaniu obszaru czujnikiem na serwie, przypisać do jakiejś zmiennej 1 lub 0 w zależności od tego po której stronie osi robota pomiar odleglości był największy.

Nastepnie, skrecac w miejscu robotem z wycentrowanym serwem w odpowiednią stronę do czasu uzyskania wartości największej odległości z poprzedniego skanowania.

[Hudyvolt]

Serwem obracasz tak aby szukać maksimum

1. robisz pomiar 1

2. obracasz w prawo

3. robisz pomiar 2

4. jeśli pomiar 2 jest większy od pomiary 1, obracaj serwo dalej w prawo

5. jeśli nie, obracaj w lewo

A robotem poruszasz tak aby obrócił się w tę stronę którą wskazuje serwo.

[draqo]

Mam dwa pomysły:

1. Czujnik na serwie i robić kilka pomiarów w zakresie od 0 do 180 stopni. Wybrać największą odległość i obliczyć kąt pomiędzy prostą przechodząca przez ten punkt i punkt robota i środek robota. Lepiej wyjasnione na rysunku.

I myślalem, zeby zastosować PID i wykorzystać tą wartość kąta, aby największa odległość była zawsze przed robotem.

2. Dwa czujniki - jeden z lewej strony, drugi z prawej. Zastosować PID tak, aby robota trzymał równą odległość od ścian.

Wydaje mi się, że 2 sposob bedzie lepszy. Jakie jest Wasze zdanie?

[draqo]

Postanowiłem zrealizować pomysł 2. Lecz mam problem z regulatorem PID. Poniżej kod.

#!/usr/bin/python
import time
import RPi.GPIO as GPIO
from PID_reg import PID
import math
# -----------------------
# Ustawienia silnikow
# -----------------------
GPIO.setmode(GPIO.BOARD)
# Lewy czujnik
TRIGGER_LEWY = 12
ECHO_LEWY    = 15

# Prawy czujnik
TRIGGER_PRAWY = 5
ECHO_PRAWY    = 3

GPIO.setup(TRIGGER_LEWY,GPIO.OUT)
GPIO.setup(ECHO_LEWY,GPIO.IN)

GPIO.setup(TRIGGER_PRAWY,GPIO.OUT)
GPIO.setup(ECHO_PRAWY,GPIO.IN)

GPIO.output(TRIGGER_LEWY, False)
GPIO.output(TRIGGER_PRAWY, False)

# Prawa strona
M1_PWM = 26
M1_EN = 24

# Lewa strona
M2_PWM = 23
M2_EN = 21

# Prawa strona ustawienia jako output
GPIO.setup(M1_PWM,GPIO.OUT)
GPIO.setup(M1_EN,GPIO.OUT)

# Lewa strona ustawienia jako output
GPIO.setup(M2_PWM,GPIO.OUT)
GPIO.setup(M2_EN,GPIO.OUT)

# Prawa strona ustawienie stanow niskich
GPIO.output(M1_PWM, False)
GPIO.output(M1_EN, True) # True - do przodu, False - do tylu

# Lewa strona ustawienie stanow niskich
GPIO.output(M2_PWM, False)
GPIO.output(M2_EN, False) # False - do przodu, True - do tylu

# Ustawienie PWM dla prawej strony
PRAWA_PWM = GPIO.PWM(26,50)

# Ustawienie PWM dla lewej strony
LEWA_PWM = GPIO.PWM(23,50)

# Poczatkowe ustawienia PWM silnikow
PRAWA_PWM.start(0)
LEWA_PWM.start(0)


def prawy():
 GPIO.output(TRIGGER_PRAWY, True)
 time.sleep(0.00001)
 GPIO.output(TRIGGER_PRAWY, False)
 start = time.time()

 while GPIO.input(ECHO_PRAWY)==0:
   start = time.time()

 while GPIO.input(ECHO_PRAWY)==1:
   stop = time.time()

 elapsed = stop-start
 distance = (elapsed * 34300)/2

 return distance

def lewy():
 GPIO.output(TRIGGER_LEWY, True)
 time.sleep(0.00001)
 GPIO.output(TRIGGER_LEWY, False)
 start = time.time()

 while GPIO.input(ECHO_LEWY)==0:
   start = time.time()

 while GPIO.input(ECHO_LEWY)==1:
   stop = time.time()

 elapsed = stop-start
 distance = (elapsed * 34300)/2

 return distance  

def lewa_average():
 distance1=lewy()
 time.sleep(0.01)
 distance2=lewy()
 time.sleep(0.01)
 distance3=lewy()
 distance = distance1 + distance2 + distance3
 distance = distance / 3
 return distance

def prawa_average():
 distance1=prawy()
 time.sleep(0.01)
 distance2=prawy()
 time.sleep(0.01)
 distance3=prawy()
 distance = distance1 + distance2 + distance3
 distance = distance / 3
 return distance

print "Ultrasonic Measurement"


suma_bledow = 0
poprzedni_blad = 0
blad = 0
Kp = 0.005
Kd = 0.0005
Ki = 0.005
max_predkosc = 8;
blad = lewa_average() - ((lewa_average() + prawa_average())/2)
try:
 p=PID(0.1,0.4,0.2)
 p.setPoint(0)
 while True:
   blad = lewa_average() - ((lewa_average() + prawa_average())/2)
   pid = p.update(blad)
   # poprzedni_blad = blad

   # suma_bledow = suma_bledow + blad

   # if (pid > 15):
     # pid = 10
   # if (pid <-15):
     # pid = -10

   # if(pid<0):
     # LEWA_PWM.ChangeDutyCycle(max_predkosc)
     # PRAWA_PWM.ChangeDutyCycle(max_predkosc  + abs(pid))
   # else :
     # LEWA_PWM.ChangeDutyCycle(max_predkosc + abs(pid))
     # PRAWA_PWM.ChangeDutyCycle(max_predkosc)

   # if(prawy() > 50):
     # LEWA_PWM.ChangeDutyCycle(0)
     # PRAWA_PWM.ChangeDutyCycle(0)

   # korekta = (Kp*blad) + (Kd*(blad-poprzedni_blad)) + (Ki*suma_bledow)

   # if (pid > max_predkosc):
     # pid = max_predkosc
   # if (pid < max_predkosc):
     # pid = -max_predkosc
   # if (pid > 0 and pid <max_predkosc):
     # pid = 0
   # pid = math.ceil(pid)
   # if (pid<0):
     # GPIO.output(M1_EN, True)
     # GPIO.output(M2_EN, False)
     # print "przod"
     # LEWA_PWM.ChangeDutyCycle(abs(pid))
     # PRAWA_PWM.ChangeDutyCycle(abs(pid))
   # else:
     # GPIO.output(M1_EN, False)
     # GPIO.output(M2_EN, True)
     # print "tyl"
     # LEWA_PWM.ChangeDutyCycle(abs(pid))
     # PRAWA_PWM.ChangeDutyCycle(abs(pid))


   print "Lewy: %.f" % lewy()
   print "Prawy: %.f" % prawy()
   print "Blad: %.f" % blad
   print "Korekta: %.f" % pid
   time.sleep(0.5)

except KeyboardInterrupt:
 GPIO.cleanup()

Problem jest taki, że robot nie stabilizuje swojej pozycji na środku labiryntu. Wartość wyliczana z regulatora przy błędzie 0 narasta i powoduje, że robot skręca.

Poniżej przykład:

Korekta to wartość z regulatora. Narasta ona od 0 do 4. A przy błędzie = 0 korekta powinna wynosić 0.

[skmskm]

Narasta ci cały czas #suma_bledow. Za każdym razem dodajesz je do siebie a one mają wpływ na #korektę.

A poza tym twój robot pojedzie wprost do tego rogu co nie chcesz by jechał 😋.

Czujnik tam zobaczy prawdopodobnie nieskończoność, charakterystyka fal ultradźwiękowych.

[draqo]

To akurat jest zakomentowane.

[aw]

Czujnik tam zobaczy prawdopodobnie nieskończoność, charakterystyka fal ultradźwiękowych.

skm o którym miejsu mówisz że czujnik zobaczy nieskończoność? Wydawało się mi że fala dźwiękowa po uderzeniu w przeszkodę rozchodzi się we wszystkie strony, nie mają znaczenia skosy ściany, co najwyżej może pokazać krótszy dystans (a nie dłuższy), ale też nie dużo bo wiązka wychodząca jest kierunkowa (sprawdzone przeze mnie doświadczalnie) dopiero po odbiciu rozchodzi się na wszystkie strony.

[skmskm]

Chodziło mi oto że z mojego doświadczenia czujnik kiepsko widzi skosy, fala dźwiękowa się odbija i nie wraca...

[aw]

Chodziło mi oto że z mojego doświadczenia czujnik kiepsko widzi skosy, fala dźwiękowa się odbija i nie wraca...

Myślę że to jest problem samego czujnika niż ogólnie wszystkich czujników odbiciowych. Problem jest taki że fala odbita od przeszkody pod kątem prostym ma inny "kształt", niz fala odbita po skosie. Fala odbita po skosie powinna być "dłuższa" tzn odbiornik dostaje dłużej trwający sygnał odbicia. Możliwe że filtr sobi z tym nie radzi. No i poza tym jest jeszcze jeden problem że czujnik wysyła 3-5 sygnałów. Pojawia się więc problem echa w takich labiryntach. To też jest związane pośrednio z filtrowaniem.

Podsumowając w przypadku własnego czujnika i pod warunkiem posiadania oscyloskopu można to ładnie poprawić. Podejrzewam że podobnie radzą sobie z tym te lepsze czujniki. Niestety SR-04 faktycznie do takich nie należy.