過去作成した自律移動ソフトウェアに関するROS 2パッケージ解説
ROS 2アドベントカレンダーの23日目の記事になります。
色々勉強するぞ、のお気持ちで去年から自律移動ロボットに関するソフトウェアをできる限り１から書いてみる試みをしていました。
作成したパッケージは、以下の通りです。まだまだ作成途中のものも多いですが、勉強中だから仕方ないね。
一応ROS 2ベースで全て実装しています。
今回の記事では、これらのパッケージについてどういうものを実装したか中身を簡単に紹介しつつ、現在実装中のもの、今後追加していきたいものなどを語れれば良いかなと思います。この記事を読んでくれたそこの貴方には、ぜひついでにレポジトリのstarを押していただけると幸いです。
lidar_graph_slam
3D LiDARを用いたSLAMパッケージです。本パッケージはフロントエンド処理としてLiDARを用いたスキャンマッチングによるlidar odometry推定及び最適化候補となるキーフレームポーズ生成、バックエンド処理としてglobal map構築とループ検出及びポーズグラフ最適化による全体軌跡の修正を行っています。大まかなシステムとしては以下になります。
points prefiltering
このノードでは点群の事前処理を実施しています。大まかに、
- crop filtering
- downsampling
- outlier filtering
を実施しています。crop filteringで点群の余分な領域をカットしつつ、downsamplingにより点群を間引き、outlierにて簡単な外れ値除去の処理を行っています。これらはPCLにて実装されているAPIを使用しています。
lidar scan matcher
LiDARの計測値を用いて、LiDAR Odometryの推定を行う部分です。scan matchingによりLiDARの計測フレーム間の局所的な移動量を計算しています。スキャンマッチングにはndt_ompを使用しています(pcl実装のndtをベースにompによる並列計算の実装がされています)。NDTのアルゴリズムについてはこちらが参考になります。
また一定距離ごとにキーフレームとなるポーズと点群のセットを生成していきます。これらは後段のポーズグラフ最適化の部分とGlobal map構築で使用されます。
しかし実際のスキャンマッチングにおいては、マッチングを実施して移動量を積算していくとどんどん誤差が溜まってしまいます。これはLiDARスキャンのノイズや動的物体の影響、そもそものスキャンマッチングの精度などが要因であり、これらは補正をしてあげないと長距離走るほど誤差が蓄積し地図が破綻してしまいます。
特に同じ場所を周回するような環境では、誤差の蓄積により一度通った場所を通過しても地図が閉じず破綻してしまうことが多いです。これを解消するため、ポーズグラフ最適化により全体軌跡の誤差を小さくする処理を入れてあげます。
graph based slam
グラフ最適化による軌跡の誤差修正とglobal map構築を行う部分です。前述の通り、LiDAR Odometryだけでは誤差が蓄積してしまい、綺麗な地図が作れないためグラフ最適化により全体の誤差を小さくするような最適化処理を実施します。SLAMにおけるグラフ最適化の話は、fuRo友納先生のSLAM入門およびこちらの記事が非常に参考になります。SLAM入門は特に第2版がおすすめです(IMUを用いた話や3D SLAM、tightly coupledアプローチ、リー代数の説明などより詳しい話が記載されています)。
グラフ最適化は変数をグラフ構造として表現した上で最適化問題を解いていく方法です。近年のSLAMパッケージはグラフ最適化ベースのものが多く、最適化ソルバもOSSを使用するケースが非常に多く(代表的なものはg2o、gtsam、ceres)、比較的実装のハードルが低めになってきている気がします。LiDAR SLAMだけでなくVisual SLAMにおけるカメラのBundle Adjustmentや経路計画問題にもグラフ最適化は使用されます。
SLAMにおけるポーズグラフ最適化では、scan matchingにより推定した位置を頂点(Node)として、それぞれの相対ポーズを拘束とするように辺(Edge)で繋いでいきながら最適化を実施していきます。それと合わせて、一度通った箇所を通過した時にその箇所を検出し(ループ検出)、現在推定位置とそのループ箇所を同じように拘束をかけて最適化を実施することで全体の誤差が最小となるような最適化処理(loop closure)がなされます。
Scan MatchingによるOdometry推定の誤差でかくなりすぎなければそれなりに大きい環境の地図も綺麗に作れるようになります。ちなみにグラフ最適化のソルバに関しては本パッケージではgtsamを使用していて、基本的な最適化処理は全てgtsamがやってくれます。
今後の改良点
IMUを使っていないので、使えるように拡張をしようと現在実装中です。具体的には、
- LiDARスキャン間に更新されたIMU計測値と最新のOdometry推定結果を用いて、次ステップのスキャンマッチングの初期位置を計算する
- IMUの計測値を用いて点群の歪み補正実施
あたりを実装中です。IMUの生値を積分するだけではバイアスなど色々な要因で誤差が蓄積してしまうため、LiDARの計測値によって求まった位置姿勢を用いて補正しつつ正しい予測位置を求めようというやつです。実際IMUを併用するだけでもOdometry推定性能はかなり上がるので、是非積極的に入れていきたいところです(実際はそのためにもバイアスも補正してあげる必要もありバイアス推定も検討していきたいところ。現状はgtsamを用いて最適化ベースで同時にバイアス推定を行おうとしています)。
将来的にはグラフ最適化ソルバも自作に置き換えればいいな。。。と思いつつまだ数式レベルでの理解が及んでいないので、絶賛勉強中です。自作に置き換えたいモチベとしては理解度を増したいというのもありますが、gtsamのドキュメントに関して実装レベルものがほとんどであり、なんか便利そうなAPIあるなと思ってもコードを読むしか無いことがほとんどなので、じゃあ自分で実装しても良くね？と思った次第です…。
global_pose_initializer
初期位置情報なしの大域的自己位置推定をやりたい、というモチベの元とりあえず実装してみようと思い立って作ったのがこのパッケージです。実装してみようと言いつつ、TEASER++の機能におんぶに抱っこです。TEASER++はロバストな点群Regstration手法が実装されたパッケージであり、初期位置が与えられない状況で点群の位置合わせをしなければならないglobal localizationにおいて結構使えそうと思い試しました。
TEASER++を実行するに当たり、まず事前に入力点群、参照点群に対し、FPFH特徴量というものを計算しておく必要があります(またFPFH特徴量を計算するために事前に法線推定もしておく必要があります)。ある点とその近傍点群のセットを作って特徴を計算しヒストグラムとして扱うことでそれなりにロバストに特徴を扱うことが可能になります。ただこれが実はそこそこ計算時間を取られるため、点群の総量をダウンサンプリングで間引いて軽くしたり、特徴量計算時の近傍探索範囲を狭めるなどして計算負荷を下げてあげる必要がありました(リアルタイムで遅れが許されない自己位置推定と違い、多少処理的に遅れがあっても怒られなさそうな初期位置推定なので、数秒のラグは我慢することにしました)。色々試した結果、downsamplingのvoxel sizeを0.5、近傍探索範囲を4.0としておくことで比較的ストレスのない数秒レベルで推定が可能になりそうでした。
試しに動かしてみると、それっぽく初期位置推定ができていそうです(下の動画では初期位置情報は一切与えていません)。
問題点
あくまで点群のみを使用した特徴量マッチングであることから、類似した特徴量がある環境下では高確率で失敗します。試しにビルなどに囲まれている市街地で取得したデータと地図に対しては、高確率で誤った箇所に収束します。ある程度環境が限定された箇所ではそれなりの確率で位置合わせができていたので、例えばGNSSが使えれば測位結果を用いておおまかな位置を決定、その周辺からTEASER++によるScan Regitrationを実施する、といったようにすればより成功率があがるかもしれません(Autowareの初期位置推定では、GNSSの測位結果を用いてそこからパーティクルフィルタにより網羅的にNDTを回して一番Probabilityの高いものを最尤な初期位置とします。現状これでも一定確率で失敗するので、これをTEASER++に置き換えるとより良いかも？)。
最近では深層学習ベースのものが研究として上がってくることも多く、それらを参考にしてみるのも良いかなと思っています。
今後はとりあえず色々論文をサーベイしつつ、使えそうな手法をトライしていってみようかな〜と思ってます。
navyu
上記２つは自己位置推定、SLAMに関わる部分ですが、本パッケージはPath Planning、Path FollowingまでまたがるNavigation Stackになります。OSSのNavigationパッケージとして有名なのはmove_baseがありますが、ただこれを使うだけじゃ味気ないと思った自分はとりあえずNavigationパッケージを極力自力で書いてみようと思い立ちました。おそらく人間誰しも一度や二度経験したことがあると思います。
作成したシステムは大まかに以下の通りです。
navyu_constmap_2d
センサや事前地図から得られる障害物情報をコストとして反映したmapを生成します。主にPath Planningにおいて障害物に対する減速や回避経路生成に使用します。
主に以下の3つのレイヤーから生成されます。
- static layer
- 事前地図に含まれる静的障害物情報をコストとして反映。大域的経路生成に使用
- dynamic layer
- センサなどから得られる動的障害物情報をコストとして反映
- inflation layer
- static/dynamic layerを融合した上で、各セルのコストをロボットのfoot print(大きさ)分ふくらませる。障害物を回避する際、ロボットの大きさを考慮して上げないと壁にぶつかる経路が生成されてしまうため
navyu_path_planner
ロボットの現在位置と目標値を与えて、障害物を回避しつつ最短となる大域的な経路を出力します。
経路生成のアルゴリズムとしてはAを使用しており、またcostmapを受け取り、一定間隔で動的障害物情報を反映した経路を再計算します。ただ現状の実装ではそのたびに全体を計算し直していて時間がかかってしまいあまり良くないので、costmapが更新された箇所のみ再計算するようなアルゴリズムを実装しようとしています。そのためにDLiteの導入を検討しています。
経路は占有格子地図を用いて計算するのですが、その結果出力される経路がグリッド状でカクカクした経路になってしまいます。実際にロボットに追従させるときにはもう少しなめらかな経路になるようにBezier曲線を用いた平滑化を実施しています。
navyu_path_tracker
path_plannerで計算された経路に対して追従するような速度を計算する部分です。アルゴリズムとしてはpure pursuitによる非常にシンプルなものを実装しています。経路上の一定距離先の点を目標点として、そこに追従するようなステア速度を計算しています。並進速度に関しては目標速度を設定し、そこに近づくようなPID制御を別途実施しています。低速ならちゃんと追従できるのですが、速度が上がったりカーブが多い場所では制御が発散しがちなので、要改善です。
(出典: https://jp.mathworks.com/help/nav/ug/pure-pursuit-controller.html)
近いうちにMPCを実装しよう、と思ってまだやっていないです。近いうちにやりたい。近いうちに。
navyu_safety_limiter
path_trackerで計算された目標速度をロボットに送る前に、急に経路上に出てきた障害物にぶつかりそうな場合に速度制限をかけるためのノードです。アルゴリズムとしては複数のポリゴン(Stop/Slowdown)の内外判定をしていて、ポリゴン内にスキャン点群が存在した場合停止もしくは減速するような制御をしているだけです。ただポリゴンの大きさは固定ではなく、速度に応じて伸縮するようになっており、速度が上がればより遠くの点もみるようになる、という実装になっています。
今は並進方向にしかポリゴンを伸ばしていない非常にシンプルなものですが、今後やろうとしているのは走行しようとしている経路に対し、衝突シミュレーションのようなことをして、何秒後に衝突する可能性があるのか、もしくは衝突はしないけど障害物ギリギリを走行するかもしれないので減速する、みたいなことができるようにしたいと思っています。
navyuの全体動作
ここまでのパッケージを組み合わせて動作しているのが以下になります。
現状は経路計画にまだ若干のバグ(特定のケースで最短経路を出してくれないこと)があったりするので、まだ絶賛デバッグ中だったりします…。
実機でのテストは今の所、PFR社製のカチャカでしかできていません。カチャカはハードウェアの性能が良すぎるのか、インテグ後ほぼ一発で動いてくれました。
カチャカで動かすためのlaunchファイルはこちらに置いてあります。
今後つくばチャレンジ等を題材に、屋外での動作検証などもやっていけたらと思っています。
navyu_simulator
これは本体のnavyuとは別途でシミュレータに関する部分なのですが、Gazeboシミュレータを立ち上げるためのlaunchやconfigファイル、ロボットモデルを置いているパッケージもあります。その他にも、センサや物理シミュレーションはいらないけど、経路計画・追従制御のデバッグのための環境がほしいと思い、簡易シミュレーションを実装しました。これはセンサシミュレーションや自己位置推定などは無しで、速度のみを受け取りただ単純に理想環境でロボットの動作シミュレーションをするというものです。これは経路計画、追従のデバッグにかなり役立ちました。一応バイアスノイズやガウシアンノイズなどの簡易ノイズ計算などはしています。
またGazebo以外にもシミュレータを使えるようにしたいと思い、choreonoidでnavyuが使えるようなプラグインを作成しました。
公開されているchoreonoid_rosプラグインを用いて、diff drive controlの制御とオドメトリを計算するプラグインを追加し、navyuで走行できるようにつなげました。
今後の改良点
まず自己位置推定パッケージを追加したいです。とりあえずamclを踏襲しパーティクルフィルタベースの2D自己位置推定ができる機能を入れようと思っています。
またpath plannerはglobal plannerのみでlocal plannerが入っていないため、DWAやstate lattice plannerなどのアルゴリズムを追加しようと奮闘しています。
最終的にはラズパイレベルで動作できたら嬉しいなと思っているので、ある程度パッケージが出揃ったらコードの高速化もやっていきたいところです。
navyu_slam
lidar_graph_slamでは3D LiDARを用いた3次元SLAMでしたが、こちらは2D版になります。基本思想はlidar_graph_slamと同じですが、グラフ最適化部分が未実装になっています。つまりscan matching odometryのみの実装です。
ただいくつか違う点として、スキャンマッチング部分はNDT、ICPをフルスクラッチで実装しています。これは当初のモチベとしてスキャンマッチングのアルゴリズムについて色々調べていて、まず2Dから自分で書いてみようと思い実装していたが故です。詳しい解説は以前にrobosemiで発表したときの資料があるため、そちらを参照頂けると幸いです。
地図表現に関しては、3次元のときは点群として扱っていたのに対し、本パッケージでは占有格子地図(occupancy grid map)として生成しています。占有格子地図はスキャン点を格子上に分割し、各セルに障害物情報がどの程度含まれている可能性があるか、を確率的に表現したものになります。例えばとあるセルが0.7、とした場合70%の確率でそのセルには障害物があるということになり、これが0に近づくほどそのセルは自由空間(障害物が存在しない)とみなされます。ロボットの経路計画ではこの占有格子地図を使うことも多く、この形式で出すようにしています。
ちなみに私が今回実装しているのは点群ベースのスキャンマッチングなので、自己位置推定の観点では点群のままでも使えるのですが、実際SLAMを行うと動的物体が地図に映り込むケースがあり、とある時刻tでは写っていた障害物が時刻t+1ではスキャンから消えていて、ただ地図には反映されてしまっていてそこに障害物が存在していることになる、という状態になってしまいます。なのでできれば次のスキャン観測結果に障害物がいなくなっていれば地図からは消し去りたいのですが、その場合占有格子地図で確率として扱えれば結構楽に処理できたりします。例えば、スキャンを観測したときに、そこに写ったものが静的な障害物だったのか、移動障害物でその一回のスキャンにたまたま写っただけなのか、を複数回の観測結果を用いて確率的に計算することができます。実際のロジックとしてはバイナリベイズフィルタを用いて時系列的にセルの確率更新をしています。
今後の改良点
自作のスキャンマッチングですが、正直まだPCL実装のものと比べると性能が悪いので(旋回方向に対してPCL実装のものと比べると破綻しやすかったり、ノイズに弱かったり…)、頑張ってPCL実装のものと性能が遜色ないレベルにしていきたいです。最終的には依存関係をEigenを最低限にしつつ、スキャンマッチング専用ライブラリとして切り出すことが目標です。
あとはlidar_graph_slam同様、グラフ最適化処理を追加したいです。
おわりに
車輪の再発明、とはよく言われますが、個人的には勉強のための再開発はどんどんすべきだなと思いました。なかなか社会人になって仕事でそうはいかないケースも多いですが、特に研究室入りたてで、研究何もわからない学生さんとかはどんどん論文を読みつつ再現実装から始めていくのが良いんじゃないのかな、と思ってます(もちろんその分野に付随する数学や座学の話も超重要なのでその知識があることも前提として必要と思いますが)。私みたいな人間は論文や数式眺めても分かった気になって8割理解していないことも多いですが、実装してみて実機を動かすとやりたかった意図を理解できるケースが非常に多いです。実際にコードを書いてみたり、ライブラリを作ってみることで実装力も上がると思いますし、時間が許す限りは勉強のためにどんどん再現実装をしてみるのは良いことだな、と思いました。
Discussion