1.概要

MIRS2302 TENQ projectでは、ROSを用いて機体の自動走行を実現した。その詳細な方法や仕組みについて記述する。

2.予備知識 において前提となる知識を示したり直感的な理解を促進した後、 3.TENQにおける実装例 において詳細な方法を記述する。

開発を経て自律走行を行っている様子を次に示す。

2.予備知識

2-1.ROSについて

TENQには、鄭先生よりお借りしたJetson Nanoを搭載し、これにUbuntu 18およびROS melodicをインストールした。

マイコンへのROSインストールについては、検索すれば大量に事例がヒットするため割愛する。

ROSは平たく言えば「ロボットを制御するための、プログラム用掲示板」である。
ROSには、「ノード」と「メッセージ」なるものが存在し、Twitterで例えるとこれらは「ユーザ」と「ツイート」である。
（本来は、ノードはプログラム、メッセージはTCP/IPプロトコルによってやり取りされる情報群である。）
種類ごとにまとめられたメッセージ群を「トピック」と呼ぶ。（ハッシュタグのようなものである。） ノードは、トピックをSubscribeもしくはPublishすることで他のノードと情報交換する。(Twitterで例えると「フォロー」「ツイート」に該当する)

たとえば、Navigation Stackにおいては大まかに次のようなノード構成となっている。

ただし、実際には下図である。このグラフは"rqt_graph"という

つぎに、自律走行を行うにあたり必要なノードやトピックについて述べる。

2-2.トピック

navigation stackにおいて用いるトピックのうち、主要なものを次に示す。

他にも大量のメッセージが存在するが、実装にあたって我々がPublish/Subscribeする必要があるメッセージはこれくらいである。

2-3.ノード

実装すべきノードを、処理の順番にしたがって示す。

(1).【自作必須】odom publisher

Subscribe : なし
Publish : /odomトピック

ロボットのタイヤの偏角(エンコーダ値)を取得して、ロボットの座標へと変換する。
基本的にはArduinoとのシリアル通信でエンコーダ値を取得するが、TENQにおいてはエンコーダ信号をGPIOに入力し、C言語でこれを計算した。
詳細は "差動二輪 odometry" 等で検索するとヒットする。

(2). scan publisher

Subscribe : なし
Publish : /scanトピック

LiDARの測距データを配信する。基本的には、LiDARのメーカや有志によりROS用パッケージが配布されているため、それを使用する。
TENQで使用したRPLiDAR用ノードは、次のgithubリポジトリからcloneした。 https://github.com/Slamtec/rplidar_ros

(3). amcl

Subscribe : /odom /map /scan トピック
Publish : map座標系からodom座標系へのtf

odomトピックによる自己位置には、外乱やスリップによる誤差が生じる。また、この誤差は走行を続けるに連れて誇大化していく。
これを修正するため、LiDARの測距データとマップとを照らし合わせて、より正確な自己位置を推測し、提供するのがAMCLである。

このノードはnavigation stackに標準搭載されている（たぶん）ので、検索して使い方やパラメータについて知ることができる。

(4). move_base

Subscribe : 位置情報に関する諸々
Publish : /cmd_velトピック

AMCLによって提供された現在位置から、目的地までのルートを決定する。また、障害物がある場合にはこれを避けるルートを提供する。
ルートに基づきロボットの速度を決定し、これをpublishする。

このノードはnavigation stackに標準搭載されている（たぶん）ので、検索して使い方やパラメータについて知ることができる。

(5).【自作必須】 cmd_vel subscriber

Subscribe : /cmd_velトピック
Publish : なし

cmd_velトピックは、ロボットの並進方向速度および回転方向速度を司令する。
これを左右モータの速度に変換し、信号をArduinoないしモータドライバに送信する。
基本的には、(1)で行った変換の逆を行えばよい。

2-4."/odom"と"odom"

これまで、"odom"というワードが何度か登場しているが、これらには2つの種類があり、非常に紛らわしい。
ここでは、その違いについて述べる。

結論から言うと、"/odom"トピックは、"odom"座標系原点から"base_link"座標系原点へのTFを指す。

次に、座標系という概念について説明する。

(1).座標系（フレーム）

ROSには、座標系という概念がある。（フレームという呼称が一般的に用いられているが、座標系のほうがわかりやすいため座標系と述べる）

"map"という名前の付いたxyz座標空間を考える。
このとき、その座標系でいう(1,2,0)にA点がいたとする。
また、(2,3,0)にB点がいたとする。

このとき、AからみたBの位置は(1,1,0)であることは明らかである。

上記のような状況で、新たな座標系Aを考える。
これは、Aの位置を原点とする座標系である。
すると、A座標系におけるBの位置は(1,1,0)であるといえる。

(2). TF(座標変換)

座標系からみた座標系の位置をあらわすのがTF(Transform)である。

TFはノードによって任意にpublishすることができる。
また、各TFには親フレーム／子フレームなるパラメータがある。

上述の例でいうと、map座標系からA座標系へのTFは
親フレーム : map
小フレーム : A
座標 : (1,2,0)
とあらわされる。

同様に、A座標系からB座標系へのTFは
親フレーム : A
小フレーム : B
座標 : (1,1,0)
とあらわされる。

これらを踏まえた上で、再度odomの定義を確認する。

これを図にすると次のようになる。

では、これらの座標系が何を表しているのかについて説明する。

(3). odometry

odometryデータとは、センサの値によって得られるロボットの座標である。

MIRSにおいては、エンコーダ値を積分したりsinしたりcosしたりして計算された、ロボットの位置のことをいう。

前述したように、ロボットを走行させながら、エンコーダの値を逐次計算してロボットの位置を概算できる。
その結果をAMCLに伝えるため、"/odom"トピックとしてpublishする必要がある。

たとえば、エンコーダの値が左右それぞれ変化したことから、ロボットが前に5m、右に1m移動したことがわかったとする。
このとき、ノードはodom座標系においてbase_link座標系が(5,1,0)に存在することを知らせる必要がある。
これがodom → base_linkへのTFであり、/odomトピックという名前をつけられているのだ。

次に、map座標系に存在するodom座標系がなぜ必要なのかを説明する。

(4). map → odom 座標系

odometryデータによる座標がなぜmap座標系に存在しないのか。それは、odometryデータが不正確で、それを修正しないとmapにおける機体の座標をロクに特定できないからである。

だから、odometry座標系の位置をズラして修正することで、正確な自己位置推定をしている。これがmap → odomのTFであり、AMCLによって提供される。
(具体的には、LiDARの測距データとマップを照合して、この辺かな～という確率の尤度を云々しているらしい)

前述の例では、エンコーダの値によってロボットが前に5m、右に1m移動したことがわかったとした。
しかし、このときタイヤがめちゃくちゃスリップしており、本当は前に6m、右に4m進んでいたとする。
このとき、odom → base_linkの座標（/odomトピック）には(+1,+3,0)の誤差が含まれていたということである。

この誤差は実際に測定したわけではなく、前述したようにAMCLがLiDARの測距データにより推定したものである。

この場合、タイヤがスリップしたことによる誤差を修正するのがmap → odomのTFである。

（スリップによる誤差は侮れず、しばらく走行するだけでmap → odomのTFが数百メートルオーダーで変化してしまう。すなわち、AMCL+LiDARによる自己位置推定は自律走行において欠かせないものであることがわかる。）

2-5. costmapとplanner

自己位置が推定できたら、次にされる処理は経路決定である。AMCLによって推定された現在地から、目的地までの経路を計算する必要がある。

最初に知るべき概念が、costmapというものである。

簡単な実例を示しながら説明する。
xy座標系において、現在位置(0,0)から(5,5)に移動したいとする。
ただし、(1,2)から(4,2)にかけて障害物があるとする。

このとき、ただ赤点から青点までを結ぶだけでは障害物に衝突してしまう。

そこで、障害物を避けつつ、短くなるルートを決定したい。

マップにおける壁面および障害物に対して、コスト関数なるものを適用すると、下図のようになる。

障害物に近ければ近いほど緑色が濃い(=コスト関数が高い)ことをあらわす。

このとき、経路上のコスト関数の値を積分したときに、コストが少ない経路を採択したい。
すなわち、できるだけ白いところを通りつつ、短いルートを探索する必要がある。

上図の例では、赤いルートが高コスト、橙のルートが低コストなのは明白であろう。したがって、橙のルートを採択する。

このような処理を行うのがplannerと呼ばれるノードであり、このマップのことをcostmapという。

詳細はhttps://robo-marc.github.io/navigation_documents/costmap_2d.htmlを参照すると、パラメータに関する情報を得ることができる。

また、costmapおよびplannerにはlocal／globalの2つの種類があり、それぞれについて次に説明を示す。

・global costmap / global planner

現在地から目的地までの経路を決定する。ただし、障害物の検知は予め読み込んだ地図から取得するため、マッピングを行った後に増えた障害物や、人間などを検知することができない。
したがって、大まかな経路の決定に使われるものである。

・local costmap / local planner

現在地から数m先までのロボットの正確な動きを決定する。
local costmapは、globalとは異なりLiDARによる測距データで壁面や障害物を検知する。したがって、動的な障害物も検知し、回避することができる。
ただし、計算量が膨大であることから、local costmapの大きさには限界がある。
また、local costmapの大きさはLiDARの測距範囲より小さくある必要がある。このような理由から、costmapは2つの用途に分けて作成される。

基本的には、DWA PlannerとよばれるPlannerを使用する。LiDARによって検知した動的な障害物を検知し、避けることができる。
ただし、計算量が膨大であることから、現在地からせいぜい数十メートルの範囲内での詳細な経路を決定する。

リンク先 : https://github.com/amslabtech/dwa_planner/blob/master/doc/demo_dwa.gif?raw=true

また、DWA Plannerには大量のパラメータがあり、走行環境や機体の条件によって細かく調整する必要がある。
実装方法やパラメータについてはhttps://robo-marc.github.io/navigation_documents/dwa_local_planner.html を参照。

2-6. mapping

これまで幾度となくmapという単語があらわれてきたが、ROSにおけるmapとは次のようなものである。

8bitのグレースケール画像であらわされるものであり、白は障害物のない領域、黒は障害物のある領域、灰色は未知の領域をあらわす。

このmapはLiDARを用いて作成(=SLAM)することもできるが、自分で壁面の寸法を計測することで作成することができる。(photoshopやGIMP等を使用し、.pgm形式で作成する)

MIRS2302にて使用したマップは、殆どを後者の方法で作成した。直線的な部分はすべて自作である。

その理由として、直線的な環境でのマッピング精度の悪さが挙げられる。
LiDARの測距データを集め、マップを更新しながら、自己位置推定を行うことをSLAMという。
しかし、廊下など直線的な環境ではスキャンマッチングの精度が急激に低下する。
特徴的な壁面が存在せず、ロボットが壁面に沿って動いてもそれを検知することが難しいためと考えられる。

そのため、廊下の長さが実際よりも短くなってしまい、実際に走行する上でAMCLが不具合を起こす原因となる。

したがって、廊下等単純な形状の環境でのmappingは、SLAMを避け、手動で寸法を計測することが推奨される。

これに対して、屋外など複雑な形状をしている環境でのmappingは、SLAMが適しているといえる。

有名なSLAMパッケージとして、

・gmapping
・Hector SLAM
・Google cartographer

が挙げられる。また、MIRS2302ではGoogle cartographerを使用した。

というより、gmappingもhector slamもまともに動作させることができなかった。

gmappingは非常に処理が重く、rosbagを使用した非リアルタイム処理によるSLAMを行っても、処理に数時間かかった(6C12T CPUによる)挙げ句ぐちゃぐちゃなマップが生成されてしまった。

hector slamではロボットの回転時にマップがぐちゃぐちゃになってしまう問題を解決できなかった。

インターネット上での評判でいうとgmappingやhector slamのほうが優れているようなので、上記の問題を解決できれば廊下などでもこれらのSLAMを使用できるかもしれない。

3. TENQにおける実装

実装の手順を次に示す。

自作のコードに限り、ソースを下部にまとめて示した。

また、rosのworkspaceをhttps://github.com/KaiManabe/mirs2302/tree/main/jetsonに掲載した。

3-1. jetson nanoによるエンコーダ信号の取得

MIRS2302-ELEC-0002に示す基板を実装した。

Jetson nanoにおいて、GPIOの監視はsysfs経由で行った。
当初はpythonで実装していたが応答速度がネックであったためC言語によってライブラリを作成した。

このライブラリをノード内で使用し、低遅延でエンコーダ値の取得を実現した。

(一般的には、Arduinoとシリアル通信をしてodometry情報を取得する。Arduinoによる方法は、インターネット上にたくさん実例がある。)

3-2. odom publisherの実装

前述のライブラリを使用して、エンコーダの値を取得する。

これをもとにロボットの座標などを計算し、親:odom→子:base_linkのTFをpublishする。

3-3. scan publisherの実装

TENQにはSLAMTEC社のRPLiDAR S2Mを搭載している。

https://github.com/Slamtec/rplidar_rosより専用のパッケージをダウンロードした。

このパッケージ内のノード"rplidarNode"をlaunchファイルから呼び出すことで、LiDARによる測距データを/scanトピックとしてpublishする。

3-4. cmd_vel subscriberの実装

cmd_velトピックから左右モータの速度指令値を計算し、モータを回転する。

TENQにおいては、ArduinoはRaspiと接続されているため、Jetson nano内で扱われるモータ指令値はRaspiを経由してArduinoに送信した。

モータの指令値はソケット通信でJetson → Raspiに送信され、これをシリアル通信でArduinoに転送するプログラムをPythonで実装した。

3-5. cartographerによるmapping

次に示すlaunchファイルにより、マッピングを行う。

rviz上にマップが生成されることを確認しながら、ロボットを走行させることでマッピングを行った。
このとき、ロボットをRaspiに接続した無線キーボードで操作した。(一般的にはteleopパッケージを用いてcmd_velをpublishする。)

（このlaunchファイルを実行する前にあらかじめ、cartographerをインストールしておく必要がある。）

3-6.navigationの実装

上述のノードとAMCL, move_baseを組み合わせてひとつのlaunchファイルにまとめた。

これを起動し、rviz上で目的地を決定すると自動走行がはじまる。

ただし、plannerおよびcostmap, AMCLのパラメータはTENQの開発完了時におけるものであり、機体や環境にあわせて微調整を行う必要がある。

3-7.目的地の自動設定

TENQにおいては、注文に応じてmove_baseの目的地を自動設定した。
これは/move_base_simple/goalトピックで設定することができる。

また、/move_base/statusトピックによって走行のステータスを取得することができる。

上記の2つをソケット通信でraspi←→jetson間でやり取りすることで、自動運用を実現した。

以下、自作したソースコード