ROS2 自作 Turtlebot3 による 草刈りロボット開発。#15 Robot Localization
---- 機械学習 Object Detection を使った Robot Localization 案 ----
なんとなく PC のブラウザーを見て暇を潰していたら、浮かんできたので、書いてみた。
ロボット Localization に、ロボットに取り付けた RGB カメラの前方画像を 使ってランドマークになる物を検出する
Object Detection をつかてみてはどうか?
検出する Object は、ランドマークになるものだけにする。
道路、家、塔、川、アンテナ、窓、ドア、橋、欄干...
家は、平屋、2階屋、ビルなどに分類する。
1. 案 I
検出されたObject部分の画像だけを使って、それ以外の部分は、消去する。
ランドマークのみが抽出された画像を、rtabmap_ros に通す。
2. 案 II
SLAM の時は、rtabmap_ros で Mapping しながら、自分でも Localization 情報を構築する。
Navigation モードの時は、自分で Localization した情報を使う。
処理概要は、検出されたランドマークの class id と向きか、depth camera の画像も使って距離情報と、ロボットの位置、向き情報を保存する。
注1) 検出されたランドマークは、ロボットの位置、向きから、Map上の位置に変換して、単純に、保存するだけ。Map(x0,y0)にプロットするイメージ。
実際は、Roboto の 現在の位置(Map)とランドマークの位置(Map) を加えて、SQLite 等で情報として蓄積するか。
注2) Map上の位置に変換するには、距離が必要になる。距離が測定できる範囲のランドマークに限られる。
注3) ランドマークも見る向きによって、形状が変わるので難しいかも知れない。
家であれば、距離は、ロボットの方に向いている壁や屋根までの距離となる。ロボットの位置で、同じランドマークでも形状が異なる。
注4) Map 上の情報も、実際参照されるのは、ロボットの現在位置と向きによってカメラの画角に入る周辺の情報に限定される。
注5)
a. Navigationモード(非SLAM モード) の時は、もしかしたら、その時の各ランドマークとロボットを頂点とする多角形の形状のマッチングになるのかも知れない。
あるいは、多角形の領域の重なり具合を求めるのかも知れない。
しかし、ランドマークが多いと、多角形化が困難になるので、これは、無理か?
SLAM モードの時は、頻繁に ロボットを 360度回転させて、周囲の情報を収集する必要がある。
b. やはり、距離情報を必要としない、Object Detction によるランドマークのマッチングが出来ないものか?
この点だが、単純にObject Detction でランドマークを計測した時毎に、その時のロボットの位置と向き及び、
見つかったランドマークの Class id と RGB 画面上のboxの情報を SQLite で保存するのはどうだろうか?
これなら、マッチング処理の時、ロボットの位置 Map(Xc,Yc,Zc) が、近いレコードを SQL で 抽出して使える気がする?
これを、Map近辺レコード と呼ぶことにする。
Map近辺レコード が select できれば、後は、
『SLAM入門 ロボットの自己位置推定と地図構築の技術 友納正裕(著)』(オーム社)
でも参考に自己位置の補正ができそうじゃ。
注) ただこちらは、基本 2Dのレーザースキャンを使った入門書じゃ。
スキャンマッチング、センサ融合による退化への対処、ループ閉じ込み の３つが主要処理として出ている。
ランドマークとの距離が必須だが、初心者のおんちゃんには、大いに勉強にはなる。
3D LiDar and 3D SLAM は、LOAM があるらしい。
後は、『インターフェイス 2020 3』第3部 自律走行ロボ制御プログラミング実験室 あたりが参考になるか?
以前ぱっと読んで、 あまり理解せずにしていた。もう一度読んでみようと思う。
もし、Depth Camera で、距離が測定できるランドマークであれば、同時に Map 上の位置を計算していっしょに、SQLite で保存しておけば良いかも知れない。
注6) ロボットの位置と向きの参照情報についての整理。
ROS2 においてロボットの位置は、TF システムによって管理される。
ロボットの現在位置と向きは、下記関連で示される、
tf-map -> tf-odom -> tf-basefoot_print
tf-odom -> tf-basefoot_print は通常、ロボットのMoter の単位時間内の直進値や向きの回転値を積分した値で示される。当然、タイアの回転なので誤差が蓄積されていく。
ただし、ロボットの向きは、IMUのBNO086 の 6軸、9軸 DMP のQuatnion を使えば誤差は少ない。
上記誤差を補正するために、正しい tf-map-> tf-basefoot_print をタイア以外の情報も参考に求めて、
tf-map-> tf-odom を補正するのが、localization の目的ぞね。
注7) ロボットの走行と Map について。
ROS2 で、ロボットを走行をさせるには、予め作成した Static Map が必要ぞね。
通常 SLAM で、Static Map を作成するタスクを先に実施する。
Static Map が作成されたら、それをもとに ロボットを走行させることになる。
なので、Localization は、普通は両方で必要になるが、....
3. 上記、情報を手がかりにマッチング処理を行って、ロボットの今の位置と向きを算出する。
ロボットの位置、向きは基本、tf-map -> tf-basefoot_print で得られているので、それの補正をするだけだと思うが。
結論としては、注5) b. を使えば、なんとなくできそうな気がしてきた。
Object Detection with Vision Transformers 辺りでランドマークを検出すれば、できそうな気がするが。
Localization のときだけ使うので、1[Hz] 程度の Rate でOK だと思う。
Model は、Keras の Object Detection with Vision Transformers を転移学習させれば良い。あとは、学習データの収集じゃ。
その前に、Keras Object Detection with Vision Transformers は、Tensorflow Lite に変換できたのか?
忘れてしまったので、過去のノートをチェックじゃ。
Transformers Object detection - detr の転移学習とONNX変換と実行。
ONNX で出来たみたい。
学習データは、やっぱり実機ロボットのRGB カメラで走行させながら収集するしか無いかも。
作るとなると、
マニーがあれば、トライしてみたいが、そのまえに、生活費を稼がねばならん。
悔しいな。
最終的に完成しなくても、実際に試してみると、技術的に何か得られるものがあると思う。
実際に試してみないと、わからんのが現実の世界じゃ。
4. 実際の走行シーン。
実際の走行シーンを想像してみた。
ロボットカーで実際に走行させるフィールドとなると庭、畑、果樹園や太陽光発電所、ゴルフ場などだろうか?
そうした場合、周りにランドマークとなる良い物は、無い気がする。
それなら、自分で杭の先に何かものをつけて、それを要所要所に設置して使うのが簡単かも知れない。
形状は同じで、OK なので、それを検出させるか。
しかし、屋外であれば、GNSS RTK で、Localization は、十分なのでは?
ならば、屋内か? 倉庫、工場、工場の敷地内。
ビルの中、地下駐車場。
街なか、コンビニの店内、コンビニからの配達。
街なかだと、建物は巨大に映る。街なかだと、GNSS RTK は、性能が落ちるか?
街なかだと、人が教師付きでモデル学習させるより、モデル 自らが対象を選んで学習する自律学習でなければいかない気がする。
これは、おんちゃんには、beyond my skill level じゃ。
念の為、ChatGTP にお伺いしてみた。
"object detection の自律学習は、どうすればできる"
ふむふむ...
5. 参照。
1) Yolo5 と、 Stereo Vision による距離測定の例があるみたい。
An Object Detection and Localization Method Based on Improved YOLOv5 for the Teleoperated Robot
昔は、苦労していたんだなーと痛感させられる。
2) YOLO の昔を思い出す資料があったので、リンクしておくぞね。
Object Classification, Detection,
Localization and Segmentation
3) 画像認識向けTransformerを振り返
4) DETReg: Unsupervised Pretraining with
Region Priors for Object Detection
5)
A-LOAM
6) Introduction to LiDAR SLAM: LOAM and LeGO-LOAM Paper and Code Explanation with ROS 2 Implementation
"Setup and Run LeGO-LOAM with ROS 2" だと試せるかも?
@fishros/LeGO-LOAM-ROS2
@Rotvie/LeGO-LOAM
なぜ、複数あるのか?
Ubuntu 24.04 PC
ROS2 jazzy
$ cd ~/colcon_ws-jazzy/src
$ git clone git clone https://github.com/fishros/LeGO-LOAM-ROS2.git
$ cd ..
$ rosdep update --rosdistro=${ROS_DISTRO} && rosdep install --from-path src/LeGO-LOAM-ROS2 --ignore-src -r -s
#$ colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --parallel-workers 1 --packages-select LeGO-LOAM-ROS2
$ colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --parallel-workers 2
sample bags を試せるみたい。
下記から、ダウンロードできる。
@TixiaoShan/Stevens-VLP16-Dataset
rosbags-convert が必要みたい。
virtual_env で試す。
$ source ~/kivy_env-3.12/bin/activate
(kivy_env-3.12)$ python -m pip install rosbags
rosbags 0.10.11
(kivy_env-3.12)$ rosbags-convert --src <ROS1_bag_file>.bag --dst <ros2_bag_folder>
テストを実行。
# terminal 1
$ ros2 launch lego_loam_sr run.launch.py
# terminal 2
$ ros2 bag play <ros2_bag_folder>
ROS2 Gazebo で、VLP-16 が使えるのか?
How to add vlp-16 model to a custom robot model in ros noetic and gazebo ?
Gazebo velodyne_simulatorの使い方
Gazebo harmonic で、3D Lidar は、Tugbot in Warehose に入っているみたい。
$ gz sim
で起動して、メニューのなかから、 Tugbot in Warehose を選ぶ。
あるいは、下記 tugbot_depot.sdf をダウンロードして、
tugbot_depot.sdf
Terminal #1
$ sudo ufw disable
$ gz sim tugbot_depot.sdf -v 4
Terminal #2
$ gz topic -l
表示された topic を ROS2 の topic へ、関連つければ使えるみたい。
Use ROS 2 to interact with Gazebo
と言ってもすぐにはわからないの、サンプル真似たほうが良い。
デモ が下記にあるので、参考になる。
@gazebosim/ionic_demo