JP2019074532A - Ｓｌａｍデータに実寸法を付与する方法とそれを用いた位置測定 - Google Patents

Abstract

【課題】本発明の課題は、SLAMによる自己位置推定と環境地図（三次元点群データ）にIMUを用いて実寸法を付与するものにおいて、時間軸に対してドリフトの発生しない新しい手法を提供すること、また、その手法を用いGNSSに依存しない、かつ狭い場所でも実施できる位置測定システムを短い処理時間で提供することにある。【解決手段】SLAMによる自己位置推定と環境地図に実寸法を付与する方法は、カメラとIMUを相対位置が固定された形態で設置し、移動に伴う該カメラによる動画撮影とIMUによる加速度の計測を行い、撮影した動画はフレーム毎に静止画として書き出し、撮影された画像をSfMで解析してカメラの自己相対位置を求め、該カメラの自己相対位置を２階微分してカメラの加速度を求め、該カメラの加速度と前記IMUから得られた加速度情報との同期をとってスケール係数を得、前記カメラの自己相対位置に該スケール係数をかけるものとした。【選択図】 図３

Description

本発明はSLAM (Simultaneous Localization and Mapping)により推定された自己位置と環境地図（三次元点群データ）情報に、IMUを用いて実寸法を付与する技術及びそれを用いた位置測定に関する。

SLAM (Simultaneous Localization and Mapping)とは画像やセンサー情報等から自己位置の推定と環境地図の作成を同時に行う技術で、自律移動ロボットや自動運転車、拡張現実(Augmented Reality)などの重要な技術となってきている。自律移動するロボットが未知の環境を認識するには、画像やセンサーによって周囲の環境の形状を把握し、その形状データをもとに自己位置も推定する。自分の位置を推定し、修正しながら環境地図を作って動いていくことになる。移動距離を推定する方法の一つにオドメトリ(odmetry)がある。車輪型移動ロボットにおける車輪の回転数や回転角度から移動量を求め、ロボットの位置を推定する手法でデッドレコニング（dead reckoning）とも呼ぶ。しかし、車輪が空回りした場合には検出できず、また移動量には誤差が含まれ、その誤差の上にさらに誤差が足し合わされていくという問題がありオドメトリだけでは、ロボットが長距離の移動をした場合、走行するにつれて位置がずれていくということが起き、周りの障害物などの環境地図も累積誤差の影響を受けてしまう。

上記したSLAMにより推定された自己位置と環境地図（三次元点群データ）情報に実寸法や地球座標上の絶対位置情報を付与する手法として、GNSSすなわち、"Global Navigation Satellite System(s)"「全地球航法衛星システム」から得られる情報を用いることがよく知られている。点群データの複数の特定点の位置情報をGNSSによって得、地球座標位置と関係付けを行うのである。しかし、衛星からの電波の届かない場所ではこの手法は採用できないため、そのような条件下では慣性装置（Inertial Measurement Unit：以下IMUと略称する。）によって実寸法を付与する方法も知られている。この慣性装置とは加速度計とジャイロを備えたものであって、加速度は速度の変化率であり、物が動くところには必ず発生する。この加速度を１回積分すると速度、もう１回積分すると距離になる。そこで、加速度計を水平（Ｘ,Ｙ）・垂直（Ｚ）の３軸に配置し、その出力を２階積分することで各軸の出発点からの移動距離（位置）を知ることができる。移動中の軸の傾きはジャイロにより計測する。ジャイロも加速度計と同様に３軸に配置し、出力される角速度を１回積分することで各軸の傾き（姿勢角）を知ることができる。ところが、このIMU手法には時間累積に伴いドリフト誤差が積算され大きくなるという問題点がある。

カメラから入力する画像内の特徴点の位置と、カメラの位置姿勢を併せて検出するSLAMのマッピング技術が特許文献１に示されている。この文献には、カメラの取得画像に基づいて正規分布に従った存在確率分布を持つカメラ位置姿勢情報を生成し、さらに、カメラの取得画像に基づいて正規分布に従った特徴点追跡誤差分布を生成し、これらの正規分布に従った存在確率分布を持つカメラ位置姿勢情報と、正規分布に従った特徴点追跡誤差分布情報を適用して特徴点の三次元空間における存在確率分布を算出するという手法が開示されている。

補正演算された逐次位置情報を三次元SLAMアルゴリズムによって全補正して、前記計測車が走行した経路の全体位置情報を特定する手法が特許文献２に開示されている。この文献は管路内を走行する計測車の位置と姿勢を、タイヤの回転数から距離を出すオドメトリとIMU（慣性装置）およびSLAMを用いて算出する方法です。IMUの解析は積分によるものと考えます。IMUの解析は積分によるものであるために、時間と共に誤差が累積し精度が出ません。オドメトリでIMUの誤差を修正してもそれほど精度は向上しません。

また、本出願人は走行体にカメラと慣性装置を取り付け、視角を異にする複数の写真から得られた三次元点群データに実寸法を付与する方法とそれを用いた管路等の位置測定について先に出願（特願２０１６−８０８８２号）した。この発明は、カメラとIMUを相対位置が固定された形態で設置し、移動に伴う該カメラによる動画撮影とIMUによる加速度の計測を行い、撮影した動画はフレーム毎に静止画として書き出し、撮影された画像をSfMで解析してカメラの自己相対位置を求め、該カメラの自己相対位置を２階微分してカメラの加速度を求め、該カメラの加速度と前記IMUから得られた加速度情報との同期をとってスケール係数を得、前記カメラの自己相対位置に該スケール係数をかけるものとしたものである。この発明の方法は、加速度データを２階積分することなく、計測データをそのままカメラの加速度と比較するものであるから、従来のIMUによるものに比較してドリフト誤差は極めて小さいものとなる特有の効果を奏するのであるが、三次元点群データ全体にデータ処理を行うとなると処理時間が長くなるという問題がある。

特開２００８−３０４２６９号公報 「情報処理装置、および情報処理方法、並びにコンピュータ・プログラム」平成２０年１２月１８日公開 特開２０１５−２５７５３号公報 「埋設管路の計測装置、及び、埋設管路の計測方法」 平成２７年２月５日公開

本発明の課題は、SLAMによる自己位置推定と環境地図（三次元点群データ）にIMUを用いて実寸法を付与するものにおいて、時間軸に対してドリフトの発生しない新しい手法を提供すること、また、その手法を用いGNSSに依存しない、かつ狭い場所でも実施できる位置測定システムを提供することにある。

本発明の更なる課題は、自己位置推定情報としてSLAMによる迅速なデータ処理手法を採用することにより、IMUを組み合わせた位置測定に係る時間を短縮することにある。

本発明のSLAMによる自己位置推定と環境地図（三次元点群データ）に実寸法を付与する方法は、カメラとIMUを相対位置が固定された形態で設置し、移動に伴う該カメラによる動画撮影とIMUによる加速度の計測を行い、撮影した動画はフレーム毎に静止画として書き出し、撮影された画像をSfMで解析してカメラの自己相対位置を求め、該カメラの自己相対位置を２階微分してカメラの加速度を求め、該カメラの加速度と前記IMUから得られた加速度情報との同期をとってスケール係数を得、前記カメラの自己相対位置に該スケール係数をかけるものとした。

本発明のSLAMによる自己位置推定と環境地図（三次元点群データ）に実寸法を付与する方法は、前記走行体は加速度の増減を繰り返しながら計測を行うことを特徴とする。

本発明の位置測定方法は、カメラとIMUを相対位置が固定された形態は走行体上の設置であって、移動は走行体によって対象領域内を走行させ、上記の方法によって得たSLAMによる自己位置推定情報から管路位置を把握するものとした。

本発明のSLAMによる自己位置推定と環境地図（三次元点群データ）に実寸法を付与する方法は、加速度データを２階積分することなく、計測データをそのままカメラの加速度と比較するものであるから、従来のIMUによるものに比較してドリフト誤差は極めて小さいものとなる。
本発明のSLAMによる自己位置推定と環境地図（三次元点群データ）に実寸法を付与する方法は、全点群データに対してSfMの手法を適用することなく、特徴点追跡をおこなうことで、演算回数が減少することでデータ処理時間を大幅に短縮できる。
本発明の同期方法により、従来必要であった同期装置を用いることなくカメラとIMUの同期を行うことを可能とした。
本発明の計測方法は適度な加減速を得ていれば、従来の慣性写真測量の様に途中で静止をする必要がないため、実用性が高い。
この手法はドリフト現象が欠点である慣性測量での測位精度向上にも寄与するものである。

特徴点の検出の実例を示すもので、左側に示す図は通常の撮影画像であり、右側の図は検出された特徴点部位がプロットされた画像である。 地下トンネルを撮像し検出された特徴点部位がプロットされた画像である。 本発明の動作フローを示す図である。 図のＡはカメラとIMUの時間差による平面方向の加速度値の変化を示すグラフであり、Ｂは同期位置の算定を示すグラフである。 フィルタ通過前のノイズを含んだ波形例を示すグラフである。 フィルタ通過後のノイズを取り除いた波形例を示すグラフである。 相関によるスケールの決定手法を説明するグラフである。 ワゴンにカメラ、IMUそしてノートパソコンを固定的に設置した計測装置である。 SLAMがサンプリングした特徴点群と推定されたカメラ位置の軌跡を示したSLAMデータである。 SLAMデータに実寸法を付与したカメラ位置の軌跡を平面図、側面図のグラフで示したものである。 SLAMデータに実寸法を付与した結果を地図上に書き込んだものである。 ノートパソコン上にカメラとIMUを固定的に取り付けた計測装置、それを人が抱えて行う計測形態を示したものである。 実験を行った地下通路とSLAMがサンプリングした特徴点を示したものである。 この実験によって得られたカメラ中心位置の水平面軌跡を地球座標系のｘ軸情報とｙ軸情報から得て、建物情報が書き込まれたこの地域の地図上に重ねて表示したものである。

測量分野のマッピング技術による位置測定とロボット位置制御の分野や自動車の自動運転分野で行われてきた移動体の自己位置測定技術は独自の進歩を遂げてきた。しかし両技術はマッピング技術を使った位置測定という意味で極めて近い関係にある。本発明は、測量分野ではあまり使われていなかったSLAMの技術を走行体の移動位置を基礎に周辺環境の位置測定に応用することに出発した。

一口にSLAMと言っても、その手法にはPTAM、LSD-SLAM、ORB-SLAM等様々なアルゴリズムがあり、撮影画像における特徴点の検出のアルゴリズムも何種類かが知られている。今回の我々が提示する発明の実施形態ではORB-SLAMを使用し、ORB-SLAMにおける特徴点の検出はFASTアルゴリズムを使うこととした。
特徴点の検出の実験例を図１に示す。左側に示す図は通常の撮影画像であり、左側に示す図は通常の撮影画像であり、右側の図は検出された特徴点部位がプロットされた画像である。本発明はこの点群に対して実スケールを与えるものとする。また、図２に示すものは、地下水路を撮像したものであり、形状としては平坦な壁状のような面であっても特徴点が検出できている。

本発明の理論は、この点群に実寸法を与える方法の理論であり、図３に本発明における解析の流れを示す。
本発明において、カメラと慣性装置（IMU）は互いに相対位置が固定された形態であることが必須であって、走行車上に載置することは必須ではないが、ここでは走行車等の走行体にカメラと加速度計、ジャイロを備えたIMUを搭載したものを例に説明する。撮影と計測を行うのであるが、先ず、カメラによる動画撮影とIMUによる加速度の計測を行う。等速直線走行時には加速度変化は生じないから、走行体は不等速走行させながら加速度情報を得つつ測定を実施する。撮影した動画はフレーム毎に静止画として書き出す。検出された点群をSfMで解析し相対的な被写体の３次元点群データとカメラの位置と傾きを得る。SfMによるカメラの外部標定要素について説明すると、式(１)は画像に写る２次元な点(左辺)と実際の３次元の点(右辺)の関係を表した透視変換の式である。右辺の焦点距離ｆを含む３×３行列はカメラの内部標定要素(レンズの歪、主点位置など)である。これはカメラキャリブレーションを行うことでパラメータが決定される。右辺のカメラの位置ｔと傾きｒを含んだ３×４行列がカメラの外部標定要素である。
初期条件として基準座標値を与えずに SfM の解析を行った場合、得られるカメラの外部標定要素と３次元点群データはスケール係数ｓが不確定のため、本来のモデルとは相似関係のモデルが生成される。ここで、算出されたカメラの位置を時間で２階微分したものをカメラの加速度と整合させて定義する。スケール係数を求めるためにはカメラとIMUの同期を図る必要がある。

次に、カメラとIMUの同期方法について説明する。本発明では同期装置を用いず、カメラとIMUの加速度により同期を図る。図４はカメラとIMUの加速度を示したグラフであり、Ａにおいて破線がカメラの、実線がIMUの加速度である。２つはスケールの異なる２つのグラフであって、同期に使用するカメラとIMUの加速度は式(２)の平面方向の加速度とする。
ここでは、同期をしていない状態を仮定し、カメラの加速度は15秒遅れて得たものとして図示してある。図４のＢは式(３)で求めた計算結果である。図中のΣ（カメラ×IMU）とは、IMUの平面方向の加速度にカメラの平面方向の加速度を乗じたものである。式(３)により値Ａｔが最も大きい時の時間を求める。時間の求め方は、図４のＢに示すように多項式近似を行い、より詳細に算出した時間を同期位置として決定する。なお、グラフ中の◆印はカメラとIMUの加速度を乗じた元データ、○印は多項式に用いるデータである。
具体的には図４のＢをみると、多項式の凸の頂点より、カメラとIMUの起動開始の時間差は15秒になっている。このことは初めに設定したカメラの起動の遅れは15秒であるので、本手法により求めた時間は正しく、正確な同期位置を求めることが可能であることが分かる。
なお、同期を行うためには、できる限りノイズのないデータを利用するのが好ましい。そこで、ノイズ除去のためにフーリエ変換を行う。図５はノイズを含んだ波形データである。図６は高速フーリエ変換にてノイズ成分を取り除いた波形データである。本手法では、採用するフーリエ・スペクトルを変えては同期処理を行い、最もカメラ×IMUの値が大きい波形を採用する。

次に、絶対寸法化を図るスケール係数の決定について説明すると、同期したカメラとIMUの加速度を用いてスケール係数を求めるには、カメラの加速度がIMUの加速度と等しくなる倍率を求めればよい。そこで、カメラと IMUの加速度を用いて相関を取れば、スケール係数は式(４) の多項式として考えることができる。ｙ＝ｘとなる傾きを求めるためスケール係数(s) はａの項と同義である。よってｓはａを求めることによって求まる。
相関を取るにあたり、ノイズがスケール係数の決定に大きく影響を及ぼす。図５は外れ値を含んだカメラとIMUの加速度を想定した相関図である。小点は外れ値としてランダムに生成した点、○で囲んだ点は正しい観測値である。破線は外れ値を含んだデータで求めた直線回帰である。本来は傾きが１：１となるような近似曲線を求めたい。しかし、外れ値が影響するため正確な傾きが求められない。

そこで、ロバスト性を持たせるため RANSAC(RANdom SAmple Consensus) を用いて外れ値のデータ棄却を行う。RANSAC は、最小二乗法とは異なり、外れ値を含んだデータにおいても使用することのできるロバスト推定である。ランダムに抽出したデータを基に仮の傾きを求め、仮の傾きと抽出した２点の傾きが閾値以下であれば、インライアーとして取り入れ、閾値を超えている場合にはアウトライアーとしてデータを棄却する。これをすべてのパターンで繰り返す。そして、仮の傾きを変更し、上記工程を繰り返す。
図７において実線はRANSAC を用いた直線回帰である。 RANSAC により外れ値のデータが棄却されているため、外れ値を除いた傾きを求めることができる。
求めたスケール係数を、SfM の値に乗算することで、IMUを積分することなくカメラの外部標定要素および、三次元点群を得ることができる。図７のグラフにより、スケール係数(ｓ)は0.9274と特定できる。

上記の実施態様では、カメラとIMUを相対位置が固定された形態で設置し、移動に伴う該カメラによる動画撮影と該IMUによる加速度の計測を行ものであったが、SLAM技術にはカメラではなくレーザ送受信システム（ライダー）を搭載したものも知られている。本発明ではカメラに限らず、レーザビームの放射方向を走査する機能を備えたライダーとIMUを相対位置が固定された形態で設置し、移動に伴う該ライダーによる時系列データと該IMUによる加速度の計測を行い、撮影した時系列撮影データはデータ毎に静止画像として書き出し、該画像をSLAMの手法で解析して前記ライダーの自己相対位置を求め、該ライダーの自己相対位置を２階微分してカメラの加速度を求め、該ライダーの加速度と前記IMUから得られた加速度情報との同期をとってスケール係数を得、前記ライダーの自己相対位置に該スケール係数をかけるものとした三次元点群データに実寸法を付与する方法をとることも可能である。

本発明を長野県飯田市川路竜西土地改良区にある宮ケ洞３号隧道で実施した計測データを以下に示す。この隧道の内径は幅１ｍ、高さ１.６ｍで、人が通ることが可能な構造の水路であった。計測装置は図８に示すようにワゴン前面にライト付きのカメラを、荷台にはIMUと信号処理用のノートパソコンを固定的に設置した。このワゴンを人が押しながら隧道を通り抜けて実験データを取得した。

カメラの撮像画像データからSLAMにより特徴点の注出が行われると共に、隧道内壁のマップとカメラの自己位置の軌跡が順次得られる。また、他方でこのデータと同期しながらIMUのデータが得られデータが蓄積される。このとき得られたSLAMデータを図９に示す。図中の黒点はSLAMにより注出された特徴点群であり、矩形の枠はカメラの結像面を斜線の交点はカメラ中心を示している。したがって、このカメラ中心を結んだ線がSLAMにより推定されたカメラの自己位置の軌跡ということになる。

このSLAMデータとIMUのデータを結合させてSLAMデータに実寸法を付与した結果を図１０に示す。進行方向をZ軸に、鉛直方向をY軸にそして水平方向をX軸にしてデータを示している。SLAMにより得られた隧道内壁のマップ情報から、隧道内壁は直線設計されており、直線水路として形成されていることが確認できた。図中の平面図はカメラ中心の水平方向位置の軌跡を、側面図はカメラ中心の上下方向位置の軌跡を示している。水平方向位置の変化はワゴンを押しながらの計測であるため、ワゴンの位置変化を示している。また、上下方向位置の変化は隧道底部に堆積した土砂等の上をワゴンが移動したことによるものである。このカメラ中心の位置変動は計測対象である隧道内壁の計測には影響はない。隧道の入口から出口までの距離は95.300ｍであり、入口の位置を基準とすると出口の高さ位置は0.819ｍ低く、水平位置のずれは0.378ｍであることが確認できた。

このSLAMデータに実寸法を付与した結果を地図上に書き込んだものを図１１に示す。この結果から分かるように、この宮ケ洞３号隧道は直線状の隧道であり、勾配は低くほぼ水平であることが分かった。

宮ケ洞３号隧道が単純な直線状のトンネルであったため、もっと複雑な経路について本発明による計測を実施しようと、街中の地下道（都営浅草線東日本橋駅地下通路）で計測を実施した。階段を含む公道での実験であるので、計測装置としてはワゴンを用いずに、図１２に示すようにノートパソコン上にカメラ付きのSLAMとIMUを固定的に取り付け、一体構造としたものを人が抱えて持ち、地下道内（図１３参照）を歩行するという形態で本発明の効果確認実験を行った。図におけるプロット点はSLAM装置は抽出したサンプリング特徴点を示している。この実験において取得した具体的数値データを表1に示す。
この表において左４列（Ａ〜Ｄ）のデータは用いたＳＬＡＭ装置から得られた生データであり、右４列（Ｅ〜Ｈ）は本発明によって演算処理したデータである。

Ａ列のUnix_time というのはSLAM装置が示す時刻情報であり、これを標準時時刻に直したものをＥ列に示している。Ｂ列はこの装置のレンズ中心のｘ軸位置情報をSLAM座標系で示した値であり、Ｃ列とＤ列はレンズ中心のｙ軸位置情報とｚ軸位置情報をSLAM座標系で示した値である。この値を基に二階微分を実行してカメラの加速度を算出し、IMUから得られた加速度情報との同期をとってスケール係数を求めて、SLAM座標系の位置情報を国土地理院が定めた東京都の座標系情報に変換処理する。Ｆ列のデータがその地球座標系のｘ軸情報、そしてＧ列のデータがその地球座標系におけるｙ軸情報である。Ｈ列データは高さ位置情報であるが、計測開始位置の値を０として算出してある。数値の単位はメートルである。ループとなっている地下通路を移動して計測し、元の出発位置に戻って実験を完了した。表１のデータは実験開始から途中までの連続データで、途中データを省略し、最下列のデータは出発地点の戻った時のデータである。値が当初データとずれがあるのはドリフト等の計測誤差が含まれているが、この実験は人が計測装置を手に抱えて行ったものであるため、カメラ中心の位置が正確に原点に来ないためである。

図１４は、この実験によって得られたカメラ中心位置の水平面軌跡を地球座標系のｘ軸情報とｙ軸情報から得て、建物情報が書き込まれたこの地域の地図上に重ねて表示したものである。この図から東日本橋駅近傍の三角地帯にループ状の地下通路が設置されていることが確認できる。
このデータは通路内を人が移動して得たカメラ中心の位置情報であるが、このカメラ中心の位置情報を基に地下通路の床面位置、両側の壁位置、天井位置情報を算出することも可能である。そもそもSLAM装置は上記したカメラ中心のSLAM座標系位置情報のほか、カメラの方向を示すクウォータニオン（４元数：ｘ，ｙ，ｚ，ｗ）が得られるものであるから、本発明によってSLAM座標系の位置情報に実寸法が与えられることにより、SLAMがサンプリングした特徴点の位置情報を地球座標系に変換できることは原理上自明のことといえる。したがって、通路情報を床面位置、両側の壁位置、天井位置情報の形のほか、通路の中心位置情報として示すことも可能である。

本発明は、場所的制約はないが、衛星電波を用いないカメラ撮影による三次元点群データと加速度計によって実現可能であるから、電波の届かない地下や施設内領域で適用することが有効である。下水道のような地下埋設管、トンネル、鉱山等の坑道(本明細書ではこれらを総称して地中閉路と呼ぶ。)、ばかりではなく、瓦礫状になった災害事故現場での位置測定や検証、更には人体内の検査にも広い分野での応用が見込まれる。
埋設管路の位置測定においては、走行体にビデオカメラとIMUを搭載し、管路内を不等速駆動させながら動画画像と加速度データを順次取得することにより、本発明の手法によってカメラ位置の軌跡を得ることができる。このカメラ位置の軌跡はすなわち配管位置を意味することとなる。埋設管路が空洞である場合には、走行体は走行車を用いることが適し、水路のように流れがある場合にはフロートや小舟に設置し、移動速度を変化させながら用いることができる。いずれにせよ、基準点がない領域、衛星電波の届かない領域でも精度の高い位置測定を行うことができる。

Claims (4)

1. カメラとIMUを相対位置が固定された形態で設置し、移動に伴う該カメラによる動画撮影と該IMUによる加速度の計測を行い、撮影した動画はフレーム毎に静止画として書き出し、撮影された画像をSLAMの手法で解析してカメラの自己相対位置を求め、該カメラの自己相対位置を２階微分してカメラの加速度を求め、該カメラの加速度と前記IMUから得られた加速度情報との同期をとってスケール係数を得、前記カメラの自己相対位置に該スケール係数をかけるものとした三次元点群データに実寸法を付与する方法。
2. 前記移動は加速度の増減を繰り返しながら計測を行うことを特徴とする請求項１に記載の三次元点群データに実寸法を付与する方法。
3. 前記カメラとIMUを相対位置が固定された形態は走行体上の設置であって、移動は走行体によって対象領域内を走行させ、請求項１に記載の方法によって得た前記カメラの位置情報から対象物の位置を測定する方法。
4. 放射方向を走査する機能を備えたライダーとIMUを相対位置が固定された形態で設置し、移動に伴う該ライダーによる時系列データと該IMUによる加速度の計測を行い、撮影した時系列撮影データはデータ毎に静止画像として書き出し、該画像をSLAMの手法で解析して前記ライダーの自己相対位置を求め、該ライダーの自己相対位置を２階微分してカメラの加速度を求め、該ライダーの加速度と前記IMUから得られた加速度情報との同期をとってスケール係数を得、前記ライダーの自己相対位置に該スケール係数をかけるものとした三次元点群データに実寸法を付与する方法。