ROS2 自作 Turtlebot3 による 草刈りロボット開発。#15 Robot Localization
---- 機械学習 Object Detection を使った Robot Localization 案 ----
なんとなく PC のブラウザーを見て暇を潰していたら、浮かんできたので、書いてみた。
ロボット Localization に、ロボットに取り付けた RGB カメラの前方画像を 使ってランドマークになる物を検出する
Object Detection をつかてみてはどうか?
検出する Object は、ランドマークになるものだけにする。
道路、家、塔、川、アンテナ、窓、ドア、橋、欄干...
家は、平屋、2階屋、ビルなどに分類する。
おんちゃんの結論は、Object Detection では、ロボットの正確な位置を算出する Robot Localization には、現状不向きか、
やはり、障害物の判定とそれからの退避利用が合っている。
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 は、普通は両方で必要になるが、....
ROS2 機械学習の最近のブログ記事
Opp(Obstacl Path Planner) TensorFlow 2.16.2 Lite C++ library build.
TensorFlow 2.16.2 Lite C++ library build. の続き。
前回に続いて、今回は、本題の ROS2 自作 Turtlebot3 による 草刈りロボット開発。#9 LSTM で経路計画をする。
で作成した学習モデルを、C++ 及び ROS2 C++ で簡単に predict できるライブラリーを作成する。
環境
PC and Orange Pi 5 Armibian Jummy
Ubuntu 22.04
Tensorflow 12.6.2
ROS2 Humble
最新は、
Ubuntu 24.04
Tensorflow 2.18.0
ROS2 Jazzy
/home/nishi --> /home/your-id に変えとうせ。
1. ROS2 のプログラムから使える、opp_tflie.cpp を作成する。
ROS2 global costamap の一部を、cv::Mat に取り込んで、class Opp_Tflite に渡せば、
opp_with_lstm または、opp_with_transformer のモデルで、predict して、その結果を、
std::vector<u_int8_t> y_pred に返してくれるもので、これをライブラリとして使えるようにする。
1.1 ライブラリ libopp_tflite.a のビルド。
$ make -fMakefile-Archive-opp_tflite
$ make -fMakefile-Archive-opp_tflite install
$ make -fMakefile-Archive-opp_tflite clean
1.2 CMakeLists.txt で、
find_package(Tflite REQUIRED)
find_package(Opp_tflite REQUIRED)
をすれば、同じく簡単にリンケージができるようにします。
cmake/FindOpp_tflite.cmake
ROS2 自作 Turtlebot3 による 草刈りロボット開発。#9 LSTM で経路計画をする。
副題: ROS2 C++ 自立走行ロボットによる草刈りロボット開発
--- ROS2 ロボット自律走行の経路計画の補助に、機械学習(Transformer、LSTM)を使う。 ----
ROS2 自作 Turtlebot3 による 草刈りロボット開発。#8 Transformer で経路計画をする。 の続きです。
Transformer で、学習データ 1000個作って、学習させてみたが、1000個では、まるっきり足りないようで、 train loss = 0.7 を切らない。
業を煮やして、それならと、 Speech to text の LSTM を同じ様に学習させてみました。
こちらは。1000件ほどのデータで、うまく学習できました。
これは、凄い!!
こんなにうまく行くとは、予想できませんでした。
本当に、うまく予測できているのか、今も、半信半疑です。
余りの感激に、涙がちょちょでるぞね!!
github に公開しました。
tosa-no-onchan/opp_with_lstm
1. 開発環境
PC: Ubuntu Mate 22.04
Python 3.10.12
virtual_env
tensorflow 2.16.2
keras 3.6.0
ROS2 自作 Turtlebot3 による 草刈りロボット開発。#8 Transformer で経路計画をする。
副題: ROS2 C++ 自立走行ロボットによる草刈りロボット開発
--- ROS2 ロボット自律走行の経路計画の補助に、機械学習(Transformer、LSTM)を使う。 ----
ROS2 自作 Turtlebot3 による 草刈りロボット開発。#6 Nav2 Planner を替えてみる で、ThetaStarPlanner を使って、Auto Mower 時の走行の直進性の改善がみられた。
しかし、まだおんちゃんが望む直進性能には、足りない。
そこで、ThetaStarPlanner の経路プランの一部補正として、機械学習が使えないか、無い知恵を絞って、考えてみた。
1. 概要
結論を先に言ってしまえば、経路プランに、Transformer、Lstm による、Speech To Text モデルのロジックを使えないだろうか? と言う話じゃ。
下の画像だと、赤線の経路を、青線の様にしたい。
障害物の手前まで真っ直ぐ進んで、障害物をなぞるように進んで、再度、スタートとストップの直線上に戻る、走行が欲しい。
ROS2 自作 Turtlebot3 による 草刈りロボット開発。#7 障害物からの脱出に機械学習を使う。
副題: ROS2 C++ 自立走行ロボットによる草刈りロボット開発
---- Auto Mower で、障害物に囲まれた際の脱出方向の算出に、機械学習の画像分類 (Image Classify) を使ってみる。-----
Auto Mower で、ロボットを動かして、目的地に到達させた場合に、障害物に近づき過ぎる場合がある。
そうすると、navigation2 で、その場所から抜け出せずに、エラー終了してしまう。
これを、防ぐ為に、static map 上で、障害物に囲まれた際に、脱出方向を計算して、cmd_vel モードで、ロボットを少し遠ざけている。
現状は、ProControl::obstacle_escape() で処理を行っています。
処理概要は、
Static Map から、ロボットの周りを、OpenCV の cv::Mat に取りこんで、8方向のMaskを用意して、Mask と AND を取って、障害物の Dot を取得した後、
それぞれの方向で、一番障害物が少ない(Dot が少ない)方向に、cmd_vel を使って動かす。
注) 今は、Local Cost Map(static_layer + obstacle_layer) を、resolution: 0.01 の解像度で使っています。
これって、画像(2値画像か、グレースケール画像)を取りこんで、その画像パーターン(黒い部分が障害物、白が空きスペース)から、8クラス(クラス=方向) に分類する、
Image Classify ではないじゃろか?
Image Classify だったら、Orange Pi 5 の NPUでも使えそうじゃ!!。
少し、試してみないといかん。
ROS2 OAK-D-Lite で、Stereo depth と Mobilenet object detection を同時に動かす。
Sub Title: Does an OAK-D run Stereo depth and Mobilenet object detection at the same time?
ROS2 自作 Turtlebot3 による 草刈りロボット開発。#4 草むらの判定 で、Stereo Camera Depth と、
Object Detection を同時に実行できるハードを検討しないと行けないので、
とりあえず、OAK-D Lite で試してみた。
なんか、同時に動くみたいぞね。
google で検索したけど、該当記事がないので、できないかと思ったが、
なぜか動くみたいぞね。
後で、github に上げちょきます。
tosa-no-onchan / depthai_ros_my
rgb_stereo_mobilenet_node_my.cpp
ただし、今は、Mobilenet で試しただけで、Tiny YOLOv4 では、どうなのか?
|
新品価格 |
(広告)
ROS2 自作 Turtlebot3 による 草刈りロボット開発。#4
副題: ROS2 C++ 自立走行ロボットによる草刈りロボット開発
----- 草むらの判定 -----
一連の、ROS2 草刈りロボット の開発で、 GPS を使って畑を走行させる事は実現できそうだと、判った。
最後に残った懸案は、
1. 草むらを障害物とせずに、どうやってロボットを進めるか?
しかし、これも、
Keras V3 Object detection with Vision Transformers を試す。 で、最近の、Object Detection を体感する事で、解決策が浮かんできた。
今は、単なる思いつきじゃが、
YOLOx、Transformer Object detection で草むらを判定させて、それを Local CostMap に取り込めば良い。
Local CostMap での障害物の判定は、
obstacle_layer の plugins で行っている。
実際のプログラムは、plugin: "nav2_costmap_2d::ObstacleLayer" で、
こいつで、入力を、/scan:"LaserScan" か、 /cloudXYZ:"PointCloud2" のどちらかを取り込んで、障害物と判定している。