H NDT Kisokos

Autoware NDT térkép készítés

Autoware.ai 1.13 verzióhoz. Az angolnyelvű leírás itt érhető el:

https://autowarefoundation.gitlab.io/autoware.auto/AutowareAuto/ndt-review.html

A térképet rosbag fájlból lehet készíteni a runtime manager segítségével. A térkép készítésének menete:

A simulation fülön töltsük be a rosbaget, indítsuk el majd nyomjunk rá a Pause-gombra mikor a playing 0-1%-on áll.
A setup fülön nyomjunk rá a TF gombra és a Vehicle Model-re ( csak a robot_description paramétert adja meg)
Az ndt_mapping bemenetként node csak a /points_raw topicot fogadja el. Ha egyesített pontfelhőt használunk a points_concat node kimenete legyen ez. Egyébként használjuk a topic_tools relay -t a topic átnevezéséhez.
A paraméterek beállítása után pipáljuk ki az ndt_mapping checkboxot a computing fülön.
Indítsuk el újra a simulation fülön a rosbaget, majd nyomjunk rá a Pause-ra mielőtt a playing elérné a 100%-ot
Várjuk meg amíg a térkép elkészül. A térképet .pcd fájlként menteni tudjuk. A mentést az ndt_mapping pararméterei alatt található PCD OUTPUT gombal tudjuk elvégezni.

A térkép betöltése:

A runtime manager map fülén, a ref gomb segítségével válaszuk ki a .pcd fájlt, majd nyomjuk meg a Point Cloud gombot.
A betöltés után a térkép a /points_map topicon lesz elérhető.

NDT_mapping paraméterek:

Resulution: A térkép felbontása [m], alap érték 1 méter
Step Size: Az iterációk közötti érték növekedés [m], alap érték 0.1 méter
Transformation Epsilon: Konvergencia határérték [m], alap érték 0.01 méter
Maximum Iterations: Iterációk számának maximuma [db], alap érték 30
Leaf Size: Voxel szűrő mérete [m^3], alap érték 1 m^3
Minimum Scan Range
Maximum Scan Range

megjegyzés: A térképezéshez a pcl_generic módszer volt a leghatékonyabb

Autoware NDT lokalizáció (Nissan Leaf)

Az NDT egy illesztési eljárás, ami egy referencia pontfelhőhöz igazítja az aktuálisan mérhető pontfelhőt, aminek eredménye egy homogén transzformáció.

Esetünkben Nissan Leaf járművön a következő lépésekre van szükség a lokalizáció előállításához:

Pontfelhő egyesítése
Meglévő pontfelhő alapján referencia pontfelhő előállítása (ndt_mapping). Ez előállhat akár egy ROSBAG fájl alapján is.
Referencia pontfelhő felhasználása transzformáció illesztésére futásidőben.
Szűrés Kálmán szűrő segítségével.

Megjegyzendő, hogy a transzformációs fa, másképp fog alakulni ebben az esetben:

A szenzorok statikus transzformációi maradnak a korábban megszokott módon.
A map koordinátapont jelöli az UTM koordinátarendszerben az elhelyezkedést.
A map->base_link transzformációt a választott lokalizációs eljárás, esetünkben az NDT matching vagy a Kálmán-szűrő fogja előállítani.
A GPS direkt szenzoros forrás, tehát ebből nem célszerű a base_link transzformációt levezetni (bármennyire is pontos, mint például Novatel). A GPS szenzor egy külön transzformációt a map->gps transzformációt fogja előállítani.
A current_pose topik vagy az NDT, vagy a Kálmán-szűrő kimenete. A GNSS kimenete a gnss_pose.

Pontfelhők egyesítése

Cél a két Ouster LIDAR pontfelhőjének egyesítése, ami a következő topikokat takarja:

/left_os1/os1_cloud_node/points
/right_os1/os1_cloud_node/points

A választott localizer_frame: base_link

A kimeneti topic:

/points_concat

A launch fájl, ami elindítja a konkatenálást runtime manager használata nélkül:

roslaunch nissan_autoware_launch points_concat_all.launch

Voxel szűrés

A két LIDAR hatalmas adatmennyiségben állít elő adatot, ami nem feltétlenül szükséges egy transzformáció előállításához. Esetünkben a következő paraméterezése a szűrésnek megfelelő az NDT működtetéséhez:

bemenet: Az egyesített pontfelhő topicja, alapesetben /points_concat
Voxel levél méret [méterben]:
- valós online teszten a megfelelő érték 2.5 volt
- 3.15 és 4.15 is tesztelve lett, ezek kicsit pontatlanabb eredményt adnak.
- 5m felett nem működőképes.
Measurement Range (mérési hatótáv) : 200 m

A szűrt pontfelhő a filtered_points topikon elérhető.

A launch fájl, ami elindítja a voxel szűrést runtime manager használata nélkül:

roslaunch nissan_autoware_launch nissan_voxel_filter.launch

NDT lokalizáció (ndt_matching)

Runtime manager segítségével indítsuk el az ndt_matching programot:

Kezdeti érték: Ha a kezdeti érték nem megfelelő, kicsi az esély a megfelelő a lokalizációra! A kezdeti érték 2 módon adható meg:
- A map frame-ben való koordináták megadásával
- Megfelelő GNSS forrás esetén beállítható a GNSS alapján történő póz beállítás,
  - A gnss csak akkor működik megfelelően ha a .pcd térkép UTM koordináta rendszerben van. A térkép középppontját eltolni ezzel a node-al tudjuk, ahol az értékek a gnss_pose kezdeti értéke:

 $ pcl_transform_point_cloud input.pcd output.pcd -trans dx,dy,dz -quat w,x,y,z

Iterációk max száma: 50
- 30-40 mellett referencia ROSBAG-en működött.
Odometria segítségével stabilizálható a becslés (Use odometry checkbox). Ez olyankor probléma, ha olyan helyen vezetünk, ahol a LIDAR alacsony intenzitású mértékeket mér (pl. üvegfalak). A bemeneti sebesség előállítható saját kinematikai modell, vagy a can2odom Autoware node segítségével.
Error Threshold: Tűréshatár, minél nagyobb az érték annál pontatlanabb a lokalizáció, de a túl kicsi érték is megátolhatja a lokalizáció létrejöttét. alap érték: 1

Resulution: A térkép felbontása [m], alap érték 1 méter
Step Size: Az iterációk közötti érték növekedés [m], alap érték 0.1 méter
Transformation Epsilon: Konvergencia határérték [m], alap érték 0.01 méter

A Lokalizációhoz a pcl_anh vagy a pcl_anh_gpu módszer ajánlott

Bemenetek

Az ndt_matching a következő topikokon vár üzeneteket:

filtered_points: szűrt pontok (pl.: voxel_grid_filter), pontfelhő
points_map: referencia pontfelhő (ndt_mapping eredménye).

Opcionális:

gnss_pose: GNSS szenzor által előállított referencia póz.
vehicle/odom: jármű kinematikai modell alapján számolt odometria.
imu_raw: állítható, IMU bemenet (elég sok IMU forrás rendelkezésre áll)

Kimeneti topikok (a lista nem teljes):

ndt_pose: az NDT eljárás által megbecsült póz.

Ellenőrizzük az ndt_pose topik kimenetét! Ha közel a GPS-hez hasonló értéket kapunk, rendben vagyunk!

Ellenőrzés a pose_eval.py script segítségével (NDT és Kálmán-szűrő által előállított póz is ellenőrizhető).

GPS vs NDT

A rosbagből utólag előállított NDT (gnss kezdeti pozíció használatával):
A 3 görbe egymáshoz közel van viszont az ndt nincs tökéletesen paraméterezve
Az online mérésből készült NDT: Megjegyzés:: Az NDT alpvetően a map .pcd koordináta rendszerbe lokalizál az origo [0,0,0,0,0,0] vektor. A gps el való összevetéshez az origót az /gnss_pose kezdeti értékébe toltuk. Az ndt elcsúszását eltérését ez okozza.