![[PukiWiki]](image/pukiwiki.png "[PukiWiki]")
近年自動走行技術が我々の生活に身近になり,その技術への需要はより一層高まりつつある. しかし,自動走行技術への需要は自動車だけでなくロボットにおいても高まりつつあり, 最近では飲食店にて自動で動く配膳ロボットを見ることもめずらしくない. 当研究室では,そのような自動で動くロボット"自律走行ロボット-mercury"を 株式会社revastより提供されており,このmercuryを使い, 自立走行ロボットの実験走行会である,"つくばチャレンジ"に参加している.
つくばチャレンジとは,ロボットを自立走行させ,指定コースを完走させる走行チャレンジである. ロボット開発の中で得た,技術や知見を共有することで,自立走行技術の向上を目指している. 当研究室では,自立走行に必要な技術をソフトウェア（モジュール）ごとに分け, プロジェクトメンバーで分担し,つくばチャレンジに参加している. ロボットが自律的に走行するためには多くのモジュールが必要となるが, 中でも必ず必要となるモジュールに"自己位置推定"がある.
当研究室ではロボットの自己位置推定に"ピラミッドマッチング"という手法を採用してる. この手法は,予め作成した全体の点群地図に対し,現在のロボット位置から観測される点群を 二次元に落とし込み作成した,局所点群地図を重ね合わせることで, 全体の点群地図上での位置座標を算出するものである. 本手法では事前にデータを取得し,そのデータから全体点群地図を生成しておかなければ, 自己位置の推定を行うことができない. したがって自己位置推定には全体の点群地図を事前に作成する必要がある. また,自己位置推定の精度は全体の点群地図の生成精度にも起因するため, 良い精度で自己位置推定を行う為には,高精度の点群地図を生成する必要がある. 本研究は,自己位置推定で使用する全体の点群地図を生成するものである. 点群地図生成の際に,二種類のマッチング手法"ピラミッドマッチング","ICPマッチング"の両方を 使用することで,生成される点群地図の精度を向上させることを目的とし, それぞれのマッチング手法を単体で使用し生成した点群地図と, 両方の手法を用いて生成した点群地図を比較検討する.
当研究室では株式会社revastから提供されているロボット"mercury"を使用し研究を行っている. ロボットにはいくつかのセンサーとパソコン,モニターが搭載されており, 当研究室のプロジェクトメンバーがハードウェアのパーツを作成することなく, 搭載するソフトウェアの作成のみで,自律走行ロボットの走行実験会である, "つくばチャレンジ"へ参加している.
mercuryには外界を観測するために,LiDARセンサー,単眼カメラ,ソナーセンサー の三つのセンサーが搭載されている.
LiDARは,ロボットの現在位置を推定する目的や,障害物の検知に使用し, 単眼カメラは,色情報で物体を識別する処理に使用している. また,ソナーセンサーは至近距離障害物の認識に使用する. 上記のセンサーの内,本研究で使用するLiDARについての説明を以下で述べる.
ロボットが人と同様に周囲環境を認識するためには,その為のセンサーが必要となる. 例えば,ソナーセンサーでは至近距離物体の有無を認識し, おおよその距離を測ることができる. 単眼カメラでは,正面の物体の色情報を用いた認識ができることから, 信号機の状態を識別することができるなど, それぞれのセンサーで異なる情報を得ることができる.
LiDARセンサーとは,「light detection and ranging」の頭文字をとった言葉で, レーザー光を照射しその反射によって認識を行うセンサーである. LiDARには2D-LiDAR,3D-LiDARが存在し,mercuryにはその両方が搭載されいる. 本研究ではロボット上部に搭載された3D-LiDARのみを使用する. 3D-LiDARはレーザー光が反射した部分を点として取得することができ, これを上下約32°,左右360°取得することで, 周囲環境を3次元の点の集まり"点群"として取得することができる. この時,距離情報や点ごとの反射強度も同時に取得することができる.
上の図3は実際にロボットに搭載された3D-LiDARで取得した点群の様子である. ロボットに搭載されているLiDARの位置が,地上から約1mであり, LiDARの取得できる点群の範囲が上下約16°づつであることから, ロボットを中心に半径数mの地面の点群は取得出来ず, 円の形に点が抜けているが,それ以外の部分では正確に物理的特徴をとらえることが出来ている. 取得した点群はロボットを中心に三次元座標で(x,y,z)で表され,単位はmで表される.
当研究室で使用しているロボットのソフトウェア構成を以下の図に示す
| 点群 | Point_Cloud |
| 目標地点 | Check_Point,Way_Point |
| カメラ画像 | Image |
| 速度命令 | Twist |
| 推定ロボット現在位置 | Pose |
| 走行距離計ロボット現在位置 | Odometry |
自律走行の基本の流れとしては,"認識","演算","走行"であり,センサーを用いた認識から始まる. 認識には主にLiDARとカメラ画像を用いる.次に認識の結果を用いて演算を行う. LiDARによって認識された点群は,ロボットの現在位置を推定するモジュールや看板を検出するモジュール, 障害物を認識するモジュールに使用され,カメラ画像は色を用いて信号機の状態を検出するために使用される. この時,自己位置推定モジュールによって算出された推定現在位置と,あらかじめ設定していた通行予定地点(ウェイポイント) から次に向かうべき方向を決定し,向かうための速度を算出. 最終的に算出した速度をロボットへ送ることで走行を実現している.
図4中で,緑色の枠線で囲われている以下のモジュールは 自律走行に必ず必要となるモジュールである.
ロボットの自律走行において,はじめに必要となるモジュールとしては"ロボット制御"があげられる. このモジュールはロボットへ速度を送信するもので,この速度命令をロボットが受け取ることで, タイヤが回転し移動を実現している. また,速度を計算する際にロボットの進行方向を決定する必要があるが, 進路決定は"経路計画"で行う.
"経路計画"ではロボットが進むウェイポイント群から,次に進むべき方向を算出する.
しかし,ロボットが次に進むべき方向を正確に決定するためには,
ロボット自身が現在位置を正確に認識しておく必要がある.
ここで,現在位置を認識するために"自己位置推定"モジュールが必要となる.
"自己位置推定"では,自律走行を行う予定の場所で地図を作成しておき,
現在位置の周囲環境の地図を照合することで行う.
また,進行経路上に障害物がある場合,その障害物の有無も進行経路選択の考慮に入れる必要がある.
したがって,上記の処理と同時に"障害物検出"を常に行うことで,自律走行を実現する.
本研究は"自己位置推定"で使用する地図を生成するものである.
それぞれについて以下で述べる.
ロボットの自律走行には図4の通り,多くのモジュールが必要となる. 当研究室では, それぞれのモジュールをプロジェクトメンバーで分担し作成することとした. 点群地図は,図4上の自己位置推定モジュールに 大きく関係している.本節では自己位置推定と点群地図それぞれについて, またそれらの関係について詳しく述べる.
前に示した通り,当研究室のロボットに搭載されたLiDARを使用することで, 三次元の点群を取得することができる.この時取得された点の位置情報は (x,y,z)で表されている.これら点群を鳥瞰で見たものをここでは点群地図と呼ぶ.
三次元の点群を二次元に落とし込む際に,zの成分が高さ1.0m以上の点のみを採用 しているため図5の点群には存在した地面の点が点群地図では消えている. またこの時,三次元の点群の座標系はロボット座標系であるのに対し,点群地図はOpenCV座標系となっている. 点群を画像へ変換する際に座標系を変換する必要があるが,座標系の変換は以下の式で行っている.
図5は局所的な点群地図であり,自律走行に使用する点群地図は 局所的な点群地図をつなぎ合わせ作成した,大きな範囲の全体点群地図である.
人が地図上での現在地を把握するには,現在位置から観測することができる建物や景色を, 地図上のものと照合することで行うことが多い.当研究室で採用しているロボットの 自己位置推定手法は,人が行っている位置の推定に近い手法である.
本研究で作成し用意しておいた全体点群地図に対し,ロボットの現在位置から観測される 点群を,局所点群地図へ変換したものを照合することで,ロボットの全体点群地図上での 位置座標を算出する. この時の全体点群地図と局所点群地図を照合する手法には,画像のテンプレートマッチングを用いている. マッチング手法については後で述べる.
画像のテンプレートマッチングを用いて位置推定を行うこの手法は, 2019年のつくばチャレンジでも使用しており位置推定の精度は保証されている. しかし,全体点群地図の完成度によってはマッチングの精度が落ちてしまうことも確認されており, 自己位置推定には高精度な点群地図が必要であることが分かっている.
全体点群地図を生成する処理は以下の通りである.
地図生成を始める準備として,ログデータの取得がある.ここでのログデータとは, コントローラーを使用してロボットを手動走行させたときに取得した, 点群データ,ロボットの走行距離計から算出した現在位置情報(以降はオドメトリ情報),カメラ画像 などをファイルに出力したものを指す.またこのログデータは取得を始めた位置から0.5m進む ごとに取得するように設定してある. 走行中にリアルタイムで地図生成を行う手法も存在するが, リアルタイム地図生成にはかなりの計算速度が求められる. 本研究では,このような計算速度の問題を考えないために,事前にログデータを取得し, 取得したログデータを使用しオフラインでの地図生成を行う図10のような構成を考えた. Boostライブラリのfile_system を使用しログデータファイルから点群とオドメトリ情報を抽出, はじめの位置のログデータと次の位置のログデータでマッチングを行い,2地点間の差を算出. 算出した差を用いてはじめの位置へ重ね合わせる. 同様の処理で重ね合わせを数回行った後にノイズ除去を行う. これら一連の処理を一つ目のログデータから最後のログデータまで繰り返すことで, 手動走行させた場所の全体点群地図を生成する.
以下ではそれぞれの処理について詳しく述べる.
地図を生成するためには,事前にログデータを取得するプログラムを作成する必要があった. 今回は実際にロボットに搭載するために,"ROS2"を使用したモジュールとして, ログデータを取得するプログラム(以降はロガー)を開発した. 開発したロガーには,自動取得モードと手動取得モードが存在する. 自動取得モードは,ロボットが0.5m進むごとにその場のログデータをファイルに保存するものであり, 手動取得モードはコントローラーのボタンが押された時にその場のログデータをファイルに保存するものである. 地図を作成するために取得するログデータは,自動取得モードで0.5mごとに取得したものを使用する. 手動取得モードは,実際のつくばチャレンジで歩行者信号の画像を取得するために使用する.
本研究では,つくばチャレンジ指定コース,群馬大学, それぞれのログデータを自動取得モードで取得し地図生成に使用した.
ファイルからログデータを取得した後,それらデータでマッチングを行う. ログデータは0.5mごとに取得しており,ロボットのオドメトリ情報から ロボットのおおよその現在位置を算出することができる.しかしロボットのオドメトリ情報は 走行する路面状況によっては多くの誤差を含んでしまう.したがって,オドメトリ情報を使用して 点群の重ね合わせを行うことはできない.そこで必要となる処理がマッチングである. 現在のロボット位置から観測される点群と,ひとつ前のロボットの位置から観測された点群で マッチングを行う.マッチングの結果,現在のロボット位置は一つ前のロボット位置から どの程度移動したのかという数値を"差"として取得することができる.また,この時取得した"差" はマッチングを用いて算出した値であることから,信頼することができる. したがって重ね合わせにはこの信頼することができる"差"を使用して行う. 本研究では,点群マッチング手法として代表的な"ICPマッチング"と, 当研究室のマッチング手法である,"ピラミッドマッチング"の両方を使用することとした.
三次元点群の位置合わせに一般的に使用される技術として, "ICP(iteretive closest point)マッチング"が存在する. 本研究では,二次元点群の位置合わせとして,ICPマッチングを使用するために, マッチングモジュールを作成した.
ICPマッチングとは,異なる位置で取得された点群p_ref,p_comp, が与えられたとき,p_compをp_refに位置合わせする回転行列Rと 並進ベクトルtを算出することを目的とし,以下の手順で推定を行うものである.
太田研究室独自の点群マッチング手法であり,2019年度のつくばチャレンジでも 自己位置推定の手法として用いられていたものである.マッチングには点群ではなく,画像を用いる. 以下の手順でまず画像の生成を行う.
このようにして生成した画像群のうち,サイズの小さいものから順に 相似変換で回転しながら,マッチングを行う手法である.
点群地図を作り出すスタートの位置におけるロボットの位置座標(x,y,θ)は, (0,0,0)であり,この数値は誤差の無い数値といえる. 取得したログデータは,このスタート位置から0.5\mごとに取得するように 設定されているため,マッチングを用いて徐々に重ね合わせていくことで大きい範囲の 点群地図を生成することができる.
重ね合わせの具体的な流れは以下の図に示し,下に説明を述べる.
pos1 はスタート地点のロボット位置であることから, 真の位置である. 点群地図 はこのスタート位置を中心として, 徐々に拡大していくため, スタート位置から観 測される点群を, 最終的に全体地図となる”global map”へ格納する. これが重ね合 わせのはじめの処理である. pos2 は約0.5m 進んだ位置座標を表しているが, これ は, ロボットのオドメトリ座標の数値であり, 誤差が含まれている. したがって,pos1 の位置で観測される点群と, pos2 の位置で観測される点群でマッチングを行う必 要がある. マッチングによって算出された差は,pos2 がpos1 からどの程度離れてい るかを表しており, pos1 に差を適用することで真のpos2(pos2") を算出することが できる. pos1 のオドメトリ位置座標を(x1, y1, θ1) とし, pos1 の真の位置座標(x1", y1", θ1") = (x1, y1, θ1)のとき,pos1,pos2 のマッチングで得られた差(dx, dy, dθ) を用いると, pos2 の 真の位置座標(x2", y2", θ2") は,以下の式で表される.
pos2 の真の位置座標を用いて,global map への重ね合わせを行う. このとき,pos2 の位置から観測される点群はpos2 を中心とした相対座標である. そして,pos2 の真 の位置座標pos2” はglobal map の中心, つまりpos1 を中心とした, 絶対座標系の 数値である. したがって,pos2” で観測される点群をpos1 中心の絶対座標へ変換す ることで重ね合わせることができる. 相対座標系の点群座標(ロボットから観測された点群) を(p_rx, p_ry) とし, 相対座 標系の原点(絶対座標系のロボット位置座標) を(c_rx, c_ry), 角度をθr, 絶対座標系の 点群座標を(p_ax, p_ay) とすると, 変換式は以下の通りである.
次にpos2 とpos3 のマッチングだが, この時pos2,pos3 は両方ともオドメトリ座 標である. つまりpos2,pos3 をマッチングし算出した差はオドメトリ座標の差であ り, 誤差を含んでいると考えられる. したがって,pos2,pos3 の差を重ね合わせに使 うことはできない. しかし,pos2,pos3 の差を使うことで, ロボットが大まかにどの 程度進んだかを判定でき, 一つ前の真の位置座標pos2” に適用することで,pos3 の推 定位置座標pos3’ を算出することができる. 最後に,pos3’ とglobal map 上のpos2” を中心とする点群を切り取り, 切り出した点群でマッチングをすることで, 真の位 置座標pos3” を算出することができ, global map へ重ね合わせることができる. このようにしてglobal map へ重ね合わせる中で, 同じ座標の点群として閾値以 上の点群数が存在するとき, その点を登録し, それ以外の点をノイズとして除去し ている. 以上の処理でログデータのはじめから最後までを重ね合わせ, 点群地図を 生成する.
本研究では, 三次元点群の位置合わせに一般的に用いられる, ICP マッチング, 太 田研究室のマッチング手法である, ピラミッドマッチングの両方を使用することで, マッチング精度を向上させ, 高精度の点群地図を生成することを考えた. 点群地図生成の, 初期段階として作成したモジュールは以下の図のような, ICP マッチングのみを使用する構成であった.
pos1,pos2 をICP マッチングし算出した差を用いて,pos2 の真の位置座標を算出. 真の位置座標から観測される点群の座標系を変換することでglobal map への重ね 合わせを行う. このとき,ICP マッチングの初期位置合わせには,pos1 点群の座標系 と, pos2点群の座標系を合わせることで,行っている. p_ref 座標系の点群を(p_rx, p_ry), 原点(ロボット位置座標) を(c_rx, c_ry) とし, p_comp 座標系の点群を(p_cx, p_cy), 原点(ロボット位置座標) を(c_cx, c_cy) とする. それぞれロボットの向いている角度をθr, θc としたとき変換式は以下の通りである.
