JP6670712B2 - 自己位置推定装置、移動体及び自己位置推定方法 - Google Patents

Description

本発明は、自己位置推定装置、移動体及び自己位置推定方法に係り、特に、３次元距離センサを搭載した場合に好適な自己位置推定装置、移動体及び自己位置推定方法に関する。

従来から、自己位置を高精度に推定する技術として、衛星信号を利用し移動体の位置を推定する衛星測位利用の位置推定技術や、走行環境周辺の特徴的な形状や点を検出し地図と比較することにより移動体の絶対位置を推定するマップマッチング位置測定技術が種々提案されている。しかしながら、衛星信号を利用した位置推定では、衛星信号が遮断される時間帯は利用できなくなり、また、地図と比較することで位置推定するマップマッチングでは、周囲の視界が開けていたり、周囲の固定物の形状が災害などにより変化していたりすると利用できなくなる。

そのため、自己位置推定手法として、移動体の移動量を検出し加算することで移動体の相対位置を推定するオドメトリ技術が多く用いられる。この技術は、画像センサで輝度勾配が特徴的な路面上の特徴点を検出し、画像センサから特徴点までの距離を計測し、特徴点の移動方向と移動量を算出することで、移動体の３次元の移動方向と移動量を算出する技術である。

例えば、特開２０１５−１５８４１８号公報に記載されているように、車両の挙動量を検出し、カメラにより取得された画像におけるパターン光の位置から、路面に対する車両の姿勢角を算出し、カメラにより取得された画像の路面上の複数の特徴点の時間変化に基づいて、車両の姿勢変化量を算出し、更に、車両の初期位置及び姿勢角に、姿勢変化量を加算することで、車両の現在位置及び姿勢角を算出するものである。

特開２０１５−１５８４１８号公報

上記従来技術では、投光器とカメラの間の基線長と、各スポット光の画像上の座標とから、三角測量の原理を用いて、路面に対するカメラの距離及び姿勢角を算出し、画像センサで検出した特徴点の画像上位置の時間変化と、前記距離及び姿勢角をもとに、移動体の３次元の移動方向と移動量を算出しており、これにより、振動など前後左右の加速度によって姿勢変動が生じても、移動体の自己位置を精度よく算出することができものの、しかしながら、振動などによって、移動体の姿勢が短時間で大きく変動する場合は、カメラで検出した画像にブレが生じ、特徴点を検出できなかったり、特徴点を検出できたとしても、画像の変動量が大きいため、特徴点の画像上位置の時間変化を正しく検出できなかったりするため、自己位置推定できない。このような場合は、画像情報を使わず、例えば、距離センサで検出した路面までの距離情報のみを使って、路面上の特徴的な形状を追跡することで自己位置推定するが、路面上に反射率の異なる計測対象が混在すると、計測距離の精度が悪化するため、路面上の同一形状を追跡することが困難となり、自己位置推定できない。

本発明は、このような課題を解決するものであり、振動などにより短時間で姿勢が大きく変化する場合や、路面上に反射率の異なるものが混在している場合でも、自己位置推定する自己位置推定装置、移動体及び自己位置推定方法を提供することを目的とす
る。

上記課題を解決するために、本発明では、距離センサからの信号に基づいて地面の各スポットからの距離を所定の軸に投影した座標を算出する座標算出部と、前記座標算出部で算出した前記各スポットに対応する座標に基づいて統計的な処理を施して所定の仮想面を抽出する面抽出部と、前記面抽出部で抽出した面から座標が相対的に離れているスポットを抽出し、以前に検出したスポットとその後に検出したスポットを対応付け、並進量と回転量を算出するマッチング部と、前記マッチング部で算出した並進量と回転量を加算することで、位置と姿勢を算出する位置姿勢推定部とを有するように構成した。

具体的には、代表的な本発明の自律移動装置等の１つは、３次元距離センサを下向きに
備える自己位置推定装置および移動体であって、移動体に備える３次元距離センサで検出
した路面上の各スポットの光軸座標を算出する光軸座標算出部と、前記光軸座標算出部で
算出した各スポットの光軸座標をもとに、光軸座標の平面を抽出する平面抽出部と、前記
平面抽出部で抽出した平面から光軸座標が離れているスポットを抽出し、前記抽出したス
ポットの光軸座標と前記平面との距離を算出する垂直成分算出部と、前記垂直成分算出部
で算出した前記距離、または、前記垂直成分算出部で抽出した平面から光軸座標が離れて
いるスポット位置をもとに、１フレーム前に検出したスポットと現在検出したスポットを
対応付け、１フレーム前から現在までの移動体の並進量と回転量を算するマッチング部と
、前記マッチング部で算出した前記移動体の並進量と回転量を、１フレーム前に算出した
前記移動体の位置と姿勢に加算することで、現在の前記移動体の位置と姿勢を算出する位
置姿勢推定部と、を有することで、振動などにより短時間で姿勢が大きく変化する場合や
、路面上に反射率の異なるものが混在している場合に測距精度が悪化しても位置が不変な
路面凹凸のピーク位置を追跡することで、低処理負荷に自己位置推定するものである。

上記態様の自己位置推定装置によれば、振動などにより短時間で姿勢が大きく変化する
場合や、路面上に反射率の異なるものが混在している場合でも、自己位置推定することができる。

実施例１の形態による移動体の構成を示すブロック図である。 実施例１の形態による３次元距離センサの詳細を表している。 実施例１の形態による光軸座標算出部と平面抽出部の詳細を表している。 実施例１の形態による垂直成分算出部の詳細を表している。 実施例１の形態によるマッチング部の詳細を表している。

以下、図面を用いて実施例を説明する。

本実施例では、３次元距離センサを搭載した自動車やロボットなどの移動体において、本発明の自己位置推定装置により、移動体の自己位置と姿勢を算出する。
図１は、本実施例による移動体１００の構成を示している。
移動体１００は、３次元距離センサ２００と、光軸座標算出部３００と、平面抽出部３０１と、垂直成分算出部３０２と、マッチング部３０３と、位置姿勢推定部３０４と、位置情報４００で構成される。

３次元距離センサ２００は、自己位置推定を実施するためのセンサであり、複数方向の計測対象までの距離を検出するセンサである。例えば、レーザスキャナや距離画像カメラやミリ波レーダがあるが、他に画像ブレによらない３次元の測距方式のセンサであれば、その限りではない。前記３次元距離センサ２００は、姿勢が大きく変動しても、検出範囲内に路面５００が含まれるように、下向きに搭載する。

光軸座標算出部３００は、前記３次元距離センサ２００で検出した各スポットの距離と方向をもとに、各スポットの光軸座標を算出する。

平面抽出部３０１は、前記光軸座標算出部３００で算出した各スポットの光軸座標をもとに、光軸座標の代表的な平面を抽出する。

垂直成分算出部３０２は、前記平面抽出部３０１で抽出した前記平面から各スポットの光軸座標までの距離を、垂直成分として算出する。

マッチング部３０３は、前記垂直成分算出部３０２で算出した各スポットの前記垂直成分を参考に、前記３次元距離センサ２００で１フレーム前に検出した各スポットのなかから、現在検出した各スポットと同一のスポットを探索し、対応付ける。

位置姿勢推定部３０４は、前記マッチング部３０３で対応付けた１フレーム前に検出したスポットと現在検出したスポットの検出距離と方向をもとに、１フレーム前の移動体１００から現在の移動体１００への移動量と姿勢変化量を推定し、推定した前記移動量と姿勢変化量と、後述の位置姿勢情報４００に格納された移動体１００の位置と姿勢の時系列情報をもとに、現在の移動体１００の位置と姿勢を推定する。推定した前記位置と姿勢を位置姿勢情報４００に格納する。

位置姿勢情報４００は、１フレーム以上前に位置姿勢推定部３０４で推定した移動体１００の位置と姿勢情報が時系列情報として格納されている。

図２は、３次元距離センサ２００の詳細を表している。

３次元距離センサ２００は、光軸Ｓ１００を中心としてＸ軸方向３０度Ｙ軸方向３０度の範囲でスポット光Ｓ１０４を照射する。ここで光軸は、３次元距離センサ２００を光学的に見立てたときに、光学中心を結んだ光の軸線を意味する。

３次元距離センサ２００の距離計測において、地面５００はＸ軸方向に１５０の領域に区分けされＹ軸方向に２００の領域に区分けされている。スポット光Ｓ１０４は座標（０，０）から座標（１４９，０）の方向に、まず、スポットＳ１０５が座標（０，０）に照射され、スポット光Ｓ１０４の照射角度と、スポット光Ｓ１０４が照射されてから３次元距離センサ２００に戻る時間に基づいて、スポットＳ１０５の３次元座標を計測する。次に、
スポット光Ｓ１０４は座標（１，０）で照射されて同様に３次元座標の計測を継続する。座標（１４９，０）まで照射されると、走査の列を次に移して、座標（０，１）から座標（１４９、１）に走査する。このように、座標（１４９，１９９）までの各スポットＳ１０５の３次元座標を得る。

座標（０，０）から座標（１４９，１９９）までの走査は約0.1秒で行われる。この走査が終了すると各座標の３次元座標は記憶領域に記憶される。この座標（０，０）から座標（１４９，１９９）まで取得した３次元座標を１フレームと称する。所定の１フレームの３次元座標の取得が終了すると、同様な動作を繰り返し、次の0.1秒にて次のフレームの３次元座標を取得する
図３は、光軸座標算出部３００と平面抽出部３０１の詳細を表している。

光軸座標算出部３００は、３次元距離センサ２００で検出した路面５００上の各スポットＳ１０５の距離と方向をもとに、各スポットＳ１０５から光軸Ｓ１００に対して垂線を下した位置について３次元距離センサ２００からの距離を、各スポットの光軸Ｓ１００上の座標（光軸座標）Ｓ１０２として算出する。算出された光軸座標は、座標（０，０）〜から座標（１４９、１９９）に対応づけて記憶される。

光軸Ｓ１００は、３次元距離センサ２００が検出する範囲の中心となる軸である。

Ｓ１０２は、光軸座標算出部３００で算出した各スポットの光軸１００上の座標Ｓ１０１を、各スポットＳ１０４に対して、３次元空間にプロットしなおした光軸座標である。

光軸座標算出部３００で算出した各スポットの光軸座標Ｓ１０１（Ｓ１０２）は、平面抽出部３０１に出力される。

平面抽出部３０１では、３次元空間にプロットしなおした前記光軸座標Ｓ１０２（光軸座標算出部３００で算出され座標（０，０）〜から座標（１４９、１９９）に対応づけて記憶されている。）から、平面Ｓ１０３を抽出する。平面Ｓ１０３は、例えば、３次元空間に、法線ベクトルと原点からの平面までの距離をパラメータとする平面モデルを作成し、平面モデルと各スポットの光軸座標Ｓ１０２の距離の２乗和が最小となるように、平面モデルの前記パラメータを最適化することで、抽出する。平面Ｓ１０３は図３において簡略化のために線状的に記載されているが実際は面として構成される。前記最適化は、全スポットの光軸座標が平面モデル上にあると仮定し最適化する最小２乗法があり、処理負荷が小さく、移動体１００の高速移動時など、高速処理が必要な場合に有効であるが、スポットのなかに、平面モデル上に乗らない光軸座標（外れ値）がある場合は、誤差を含む平面が抽出される。ここで、ＲＡＮＳＡＣ（ＲＡｎｄｏｍ ＳＡｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ）やＬＭｅｄＳ（Ｌｅａｓｔ Ｍｅｄｉａｎ Ｓｑｕａｒｅｓ）などのロバスト推定法を用いることで、処理負荷は増大するが、外れ値を除去しながら、誤差を含まない平面を抽出することができる。その他、スミルノフ・グラブス検定や、トンプソン検定、マハラノビス距離を用いた検定など、統計検定による様々な外れ値除去手法があるが、要求される処理負荷と精度を満たす手法であれば、どの手法を用いてもよい。

平面抽出部３０１で抽出した平面Ｓ１０３は、垂直成分算出部３０２に出力される。平面抽出手法に最小２乗法を用いた場合は、全スポットの光軸座標の平面からの距離２乗値の平均値も、垂直成分算出部３０２に出力される。平面抽出手法にＲＡＮＳＡＣやＬＭｅｄＳなどのロバスト推定手法、または、統計検定による外れ値除去手法を用いた場合は、外れ値として検出されたスポットと平面との距離の最大値が垂直成分算出部３０２に出力される。

図４は、垂直成分算出部３０２の詳細を表している。

垂直成分算出部３０２は、平面抽出部３０１で抽出した平面Ｓ１０３から閾値Ｓ２００以上離れたスポットの光軸座標Ｓ２０１を抽出し、前記抽出した光軸座標Ｓ２０１の平面からの距離Ｓ２０２を算出する。図４において距離Ｓ２０２は簡略化された光軸座標Ｓ１０２に対して記載されているが実際は二次元的な各光軸座標Ｓ１０２に対して算出される。前記閾値は、平面抽出部３０１で算出した値を用いる。例えば、平面抽出部３０１の平面抽出で最小２乗法を用いた場合は、全スポットの光軸座標の平面からの距離２乗値の平均値を、前記閾値とする。平面抽出でロバスト推定手法または統計検定による外れ値除去手法を用いた場合は、外れ値として検出されたスポットと平面との距離の最大値を、前記閾値とする。垂直成分算出部３０２で算出した閾値Ｓ２００の範囲外にある各スポットの光軸座標、および前記スポットの平面からの距離Ｓ２０２は、マッチング部３０３に出力される。

垂直成分算出部３０２により、路面にある凹凸のピーク近傍を抽出することができる。路面上に反射率の異なる計測対象が混在する場合、３次元距離センサの測定精度は悪化するが、スポット全体で一様に精度が悪化するため、路面にある凹凸のピーク位置は不変である。よって、垂直成分３０２は、測定精度の悪化によらない不変量を抽出する処理である。

図５は、マッチング部３０３の詳細を表している。

マッチング部３０３は、１フレーム前に検出したスポットの光軸座標Ｓ３００のなかで平面から離れた路面凹凸のピーク近傍Ｓ３０２および平面からピーク近傍までの距離Ｓ３０４と、現在検出したスポットの光軸座標Ｓ３０１のなかで平面から離れた路面凹凸のピーク近傍Ｓ３０３および平面からピーク近傍までの距離Ｓ３０５を参考に、１フレーム前と現在に検出した同一スポットの対応Ｓ３０８を同定する（２次元的な座標において同定される）。ここで、１フレーム前に検出したスポットの光軸座標Ｓ３００に対してマッチングをしているが、１フレームよりも前の光軸座標Ｓ３００に対してマッチングをしても良く、また、検出された直近のフレームを用いずに、選択的な過去の２つフレームを互いにマッチングをして用いても良い。

前記対応３０６を同定する手法としては、例えば、平面からピーク近傍までの距離Ｓ３０４、Ｓ３０５の勾配を算出し、勾配の近いスポットを対応付ける手法があり、低処理負荷であるが、前記勾配が類似するスポットが複数存在すると、誤対応が発生することがある。

もう１つの手法としては、１フレーム前に検出したスポットＳ３０２と現在検出したスポットＳ３０３をそれぞれ２次元座標Ｓ３０６、Ｓ３０７に投影し、投影したスポットの２次元座標を参考に、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）を用いて対応Ｓ３０８を得る手法がある。ＩＣＰは、点群同士の位置を回転と並進により近づけていき、点群同士の位置が最小値をとるような回転と並進を同定することで、点群の対応を求める手法であり、点群の座標の次元が低いほど、処理負荷が小さくなる。よって、スポットＳ３０２とスポットＳ３０３の座標を２次元Ｓ３０６、Ｓ３０７に投影することで、低負荷で処理することができる。ただし、この場合は、１軸の回転（ヨー角）と、２次元の並進（Ｘ座標とＹ座標）のみが算出される。そこで、平面抽出部３０１で抽出した１フレーム前の平面パラメータと現在の平面パラメータを用いて、残りの２軸の回転（ロール角とピッチ角）と、１次元の並進（Ｚ座標）を算出する。

マッチング部３０３により、測定精度の悪化によらない路面凹凸のピーク近傍のスポットを対応づけるため、測定精度の悪化によらないスポットの対応結果が得られる。さらに、対応付けた結果として、対応づいたスポットの相対的な移動量（３軸の回転と３次元の並進）が算出されるが、前記移動量は、移動体１００の移動量の符号が逆転したものなので、マッチング部３０３で算出された前記移動量の符号を逆転することで、１フレーム前から現在までの移動体１００の移動量（３軸の回転と３次元の並進）が算出される。

位置姿勢推定部３０４では、前記マッチング部３０３で算出した前記移動体１００の移動量を、位置姿勢情報４００に格納した１フレーム前の移動体１００の位置と姿勢に加算することで、現在の移動体１００の３次元位置と３軸姿勢を算出する。

以上のような構成により、本発明の自己位置推定は、路面までの距離情報のみを用いて、振動などにより短時間で姿勢が大きく変化する場合や、路面上に反射率の異なるものが混在している場合でも、低処理負荷な自己位置推定を実施することができる。

１００ 移動体
２００ ３次元距離センサ
３００ 光軸座標算出部
３０１ 平面抽出部
３０２ 垂直成分算出部
３０３ マッチング部
３０４ 位置姿勢推定部
４００ 位置姿勢情報
５００ 路面

Claims (8)

1. 距離センサからの信号に基づいて地面の各スポットからの距離を所定の軸に投影した座
   標を算出する座標算出部と、
   前記座標算出部で算出した前記各スポットに対応する座標に基づいて統計的な処理を施
   して所定の仮想面を抽出する面抽出部と、
   前記面抽出部で抽出した面から座標が相対的に離れているスポットを抽出し、以前に検
   出したスポットとその後に検出したスポットを対応付け、並進量と回転量を算出するマッ
   チング部と、
   前記マッチング部で算出した並進量と回転量を加算することで、位置と姿勢を算出する
   位置姿勢推定部とを有することを特徴とする自己位置推定装置。
2. 請求項１において、前記所定の軸は光軸であり、前記投影した座標は光軸座標であるこ
   とを特徴とする自己位置推定装置。
3. 請求項２において、前記所定の仮想面は平面座標であることを特徴とする自己位置推定
   装置。
4. 請求項３において、前記光軸と前記仮想面の距離を算出して前記面抽出部で抽出した面か
   ら座標が相対的に離れているスポットを抽出することを特徴とする自己位置推定装置。
5. 請求項１において、前記距離センサは３次元距離センサであることを特徴とする自己位
   置推定装置。
6. 請求項１乃至５のいずれかにおいて、以前に検出したスポットとその後に検出したスポ
   ットを対応付けをフレーム毎に行うことを特徴とする自己位置推定装置。
7. 自己位置推定装置を有する移動体であって、
   前記自己位置推定装置は、
   距離センサからの信号に基づいて地面の各スポットからの距離を所定の軸に投影した座
   標を算出する座標算出部と、
   前記座標算出部で算出した前記各スポットに対応する座標に基づいて統計的な処理を施
   して所定の仮想面を抽出する面抽出部と、
   前記面抽出部で抽出した面から座標が相対的に離れているスポットを抽出し、以前に検
   出したスポットとその後に検出したスポットを対応付け、並進量と回転量を算出するマッ
   チング部と、
   前記マッチング部で算出した並進量と回転量を加算することで、位置と姿勢を算出する
   位置姿勢推定部とを有することを特徴とする移動体。
8. 距離センサからの信号に基づいて地面の各スポットからの距離を所定の軸に投影した座
   標を座標算出部で算出し、
   前記座標算出部で算出した前記各スポットに対応する座標に基づいて統計的な処理を施
   して所定の仮想面を面抽出部で抽出し、
   前記面抽出部で抽出した面から座標が相対的に離れているスポットを抽出し、以前に検
   出したスポットとその後に検出したスポットを対応付けてマッチング部で並進量と回転量
   を算出し、
   前記マッチング部で算出した並進量と回転量を加算して位置姿勢推定部で位置と姿勢を
   算出する自己位置推定方法。

Images