はじめに

今更ながら、ROSを使ってどれほどのロボットが作れるのか試してみたくなり、ROSを使ったロボット制御の実例を紹介するだけの記事です。
実現にあたって検討した内容は個別の記事として末尾にリンクを張っていきます。
まだまだ製作段階のため適宜記事は更新していきます。

システム概要

家中を自律徘徊するロボット（以降、ローバ）を製作したので、ROSを使って動作させます。
ローバの外観図は以下の通りです。ざっくり幅300mm × 奥行450mm × 高さ500mmです。

ローバのH/W構成図は以下の通りです。
主要センサは、YDLIDAR製のG2を使用します。スキャン周波数は5〜12Hzで、360°スキャンが可能です。
駆動部（ロコモーション）にはステアリング及びホイール駆動用にそれぞれ4つずつ2相バイポーラ型ステッピングモータを使用します。
駆動部の制御にはFPGAを用いることで、ステッピングモータを動かすための2相励磁パルスを正確にタイミング制御します。

サーバ側となるローバにはRaspberryPi4B(Raspbian32bitのBuster)を搭載、クライアント側にはWindows10 PCのWSLによるUbuntu20.04環境で遠隔制御もしくはRVIZによるステータス監視をする構成となります。
使用しているPC、及び、ROS環境は以下の通り。

システム構成

今回製作したローバ（サーバ側）の自律走行に係る処理フローは以下の通りです。

機能① センシング

レーザスキャナを用いて取得されるローバ周辺近傍の周辺地形データを出力します。

機能➁ SLAM(Simultaneous Localization and Mapping)

逐一レーザスキャナから取得される周辺地形データを連結し、地図データの生成を行うと同時に自己位置同定を行います。スリップ等によって誤差が蓄積したホイール・オドメトリに対し、自己位置同定結果に基づくオドメトリの補正を行います。

機能③ 地形評価

ローバを地図データ上のある座標(x, y)に対し向きθに置いた時に、ローバ自体のフットプリントや走破能力モデルを基に姿勢予測を行い、侵入可能か不可能かを評価したコストマップを生成します。なお、ROSで表現できるコストマップは2次元(x, y)の扱いなので、次元削減をしてθは今回無視しました。

機能④ 経路計画

コストマップを基に、ゴールまでのグローバルパスを計画します。経路計画は「ポテンシャル法」と「グラフ探索法」に大きく分類されます。

ポテンシャル法と呼ばれる経路計画では、ゴール指定した座標に引力を、ローバが侵入不可能な領域全てに斥力を与えることで、ポテンシャル場と呼ばれる空間を生成します。ローバはポテンシャルが高いところから低いところへ、最急降下法に則ってより勾配が急となる斜面を下ることでゴールに向かいます。ポテンシャル法を用いた経路計画の特徴として、比較的に安全な経路が得られやすいことを挙げます。例えば、廊下などの細い道を通るような経路を探索する場合、ゴールからの引力に誘導されつつ、廊下の両端の壁から斥力を受けた経路が形成されるため、廊下の中央を通るような経路となります。そのため、経路追従の時に多少経路を逸れても障害物に衝突するリスクは少ないです。

グラフ探索法としてダイクストラ法やA*法などが有名であり、これらはコスト関数に応じて安全かつ最短にゴールに迎うアルゴリズムとなります。グラフ探索法では障壁から斥力を受けるといった概念はないので、コスト関数の考え方に依っては、最短経路を狙いすぎるがあまり、障壁すれすれを通るような経路となり得ます。そのため、経路追従の時には軌道が逸れないように正確に制御する、もしくはグラフ探索前に経路追従で生じうる誤差を加味した厳しめのコストマップにする、というような対策をしないと障害物と衝突します。その代わり、ポテンシャル法に比べて経路探索の処理が軽い、といった利点があります。

グラフ探索法が主流かな？と思いつつ、コスト関数をチューニングするのも大変なので、今回はポテンシャル法の1つであるラプラスポテンシャル法を採用しています。ラプラスの微分方程式を満たすようなポテンシャルを与えることにより、ローカルミニマム（局所最適解）の存在しないポテンシャル場を形成できるのが特徴です。

機能⑤ 経路追従

代表的なものとして、pure pursuit（別名、人参追従：Follow-the-Carrot）と呼ばれる経路追従アルゴリズムがあります。ある点Aから一定距離D先の計画した経路上の点Bを目標点とし、点Aと点Bを通るような円弧を軌跡として追従するものです。

前述したグローバルパスに対し、pure pursuitによる経路追従を行えば自律走行はできます。しかし、それだけでは動的な障害物に伴う回避ができません。そこで、動的な障害物に対して回避ができるよう、都度レーザスキャナの出力からローカルコストマップを生成し、ラプラスポテンシャル法によるグローバルパスに追従しつつより安全な経路追従を行うための「Morphin Navigation」を実施します。

「Morphin Navigation」はNASAの火星探査ローバであるMars Exploration Rover（2003年）、及び、その後継機であるMars Science Laboratory（2011年）で採用されたナビゲーションアルゴリズムであり、ローカル経路計画と経路追従を一体化させたものになります。

この走行方式は GESTALT(Grid-based Estimation of Surface Traversability) と呼ばれるナビゲーションシステムの中心技術であり、生成したコストマップを基に、予め用意した数種の円弧の経路の中から最もコストが低く安全である、且つ各円弧経路からゴールまでのコストの算出し最適と判断した経路を選択していく手法です。

機能⑥ ロコモーション制御

4S4W（4輪ステアリング、4輪駆動）に係る計8つのモータに対し、回転速度やステアリング角を制御します。
また、実際に各車輪を駆動しているモータが何deg回転したかを逐一モニタし、それに応じてローバのホイール・オドメトリを逆算して通知します。車輪回転速度の平均からローバの並進量、左右の車輪回転速度の差分からロボットの旋回角速度を推定し、これらを積分してロボットの自己位置・姿勢を求めます。一般的に、路面と車輪のスリップに大きく影響されますが、今回は屋内走行に限定されるのでスリップは大して発生しないと考えます。

ハードウェア紹介

機械設計・製造

筐体は5万円の3Dプリンタを使ってパーツ単位で製造していきました。
フィラメントは銀色のPLA樹脂で、メタリック感を出したつもりです。樹脂だけだと脆いので、市販のステンレスシャシと組み合わせて形作りました。故に、EMC対策でも何でもないのですが、ネジ穴だらけの筐体になりました。

電気設計・製造

既製品であるRaspberryPi4Bをコアとして、FPGAボード等の周辺回路を電気的に接続し、電脳部を製作しました。
配線は結構気を使って束ねていたのですが、配線お化けになりました。

モータ駆動のためのドライバ回路や電源生成回路などは設計から実施、中国に設計データを送付すると安価で基板製造・部品実装できました。

ローバのロコモーション機能（移動機能）の核となる2相バイポーラ型ステッピングモータの制御の様子です。FPGAを用いて2相励磁のパルスで細かく角度制御していきます。ロータリーエンコーダ等を用いたフィードバック制御は無しです。
電力は無負荷であれば1ステアリングあたり約0.3W（0.12A@2.85V）で動きます。4輪なので単純計算で約1.2Wです。
実際にはローバ自体の重みで負荷が加わる上に、スイッチングレギュレータの効率の悪さが掛かってくるので、最終的には電力が大きくなりました。

ソフトウェア紹介

ROSを用いて今回製作したローバのノード構成は以下の通りです。
(slam_gmapping → terrain_analysis宛てのtopicはtfではなくmap, slam_gmapping → locomotion宛てのtopicはmapではなくtfです。rqt_graphのバグなのでしょうか。。)

ノード	概要	備考
spi	他ノードからのSPIアクセス要求を受け、外部FPGAのレジスタ空間にアクセスすることで外部H/Wの制御/ステータス収集する。	オリジナルで今回作成
locomotion	ホイール、及びステアリング駆動用のモータ速度制御・角度制御を実施し、制御量をSPI宛てにパブリッシュする。また、モータの回転角度位相を収集しホイールオドメトリを各ノードへパブリッシュする。	オリジナルで今回作成
terrain_analysis	MAPデータをサブスクライブしグローバル経路計画用のグローバルコストマップをパブリッシュする。また、レーザスキャナからの測距データをサブスクライブし、ローカル経路計画用のローカルコストマップをパブリッシュする。	オリジナルで今回作成
path_planning	グローバルコストマップを基にラプラスポテンシャル法によるGoalまでのグローバル経路計画を実施しグローバルパスをパブリッシュする。	オリジナルで今回作成
rover_main	グローバルパスをサブスクライブし、Morphin Navigationによるローカル経路計画および経路追従するためのロコモーション宛てコマンド（回転方向や速度指示）をパブリッシュする。	オリジナルで今回作成
ydlidar_lidar_piblisher	レーザスキャナを駆動し、測距データをパブリッシュする。	公開パッケージ流用(https://www.ydlidar.com/service_support/download.html?gid=1)
slam_gmapping	マップデータをパブリッシュする。また、測距データを基にSLAMを実施し、オドメトリ補正用のtfをパブリッシュする。	公開パッケージ流用([ROS.org] slam_gmapping)
base_link_to_laser4	base_link座標系からlaser座標系に座標変換する。	オリジナルで今回作成

平日仕事で忙殺される筆者にはSLAMを今から学習+自作する気力がありませんでした。
SLAMに疎い筆者でも簡単にSLAMを導入できたことにはROSの恩恵を感じました。

RVIZを用いた仮想空間表示

クライアント側ではRVIZを用いた仮想空間により地形把握を可能としています。
RVIZ上にはmapとローカルコストマップを投影しており、青+橙色になっている部分が侵入不可領域です。

ハードウェア製造時に設計した3次元データ(.cae)をURDF（Unified Robot Description Format）と呼ばれるロボットのモデルをXML形式で記述した文書内で呼ぶことで投影しています。
OpenGLとかを駆使しQtで3D GUIを作成していたのが10年前の話ですが、そんなことに時間を費やす必要がないことに感動しました。

実機テスト

①地図作成

はじめに、自律走行の前に、まずはマニュアル操作でゆっくりと自宅のリビングと廊下を走らせてみて、SLAMによる地図生成をやってみました。
リビングにあるレザーソファやテレビ台（ガラス）が残念ながらレーザスキャナに映らなかったです。
それでもそれっぽい地図はできました。

➁経路計画と経路追従

作成した地図を基にグローバスコストマップの生成し、GUI（RVIZ）上でオペレータがゴールを指定した際に経路計画を実施した時の様子です。
グローバルパスを追従するにあたり、Morphinによる最適パスの選択もできていましたが、アルゴリズム実装時にバグ有りで、その後の経路追従の失敗しました。

その後に改良し、無事自律走行ができたので一旦動画にまとめました。

③動的障害物の回避

試験中

今後の展望

ハウスローバにハンディ掃除機を装備させて、徘徊ついでに家の掃除をさせることが我が家のお局からの要求（お局的にはメインミッション指示）なので、筆者の掃除への興味は薄いですができればそこまでやります。掃除機能がないなら要らない、ルンバで良いじゃんとまで言われています。

ROSを用いたロボット制御の実例紹介（ハウスローバの製作）