(57)【要約】【課題】  衛星測位が利用できず周囲に固定物がない環境でも、低負荷処理かつ高精度な自己位置推定を実施する。【解決手段】  移動用の駆動装置と、下向きに搭載した距離画像センサと、自己位置を推定する自己位置推定装置と、を具備する移動体であって、前記自己位置推定装置は、１フレーム前に検出した画像と現在検出した画像の重複領域を抽出する画像重複領域抽出部と、前記重複領域内の検出点について、前記距離画像センサから前記検出点までの距離と入射角を基に、前記距離画像センサから前記検出点までの距離誤差を算出し、算出した前記距離誤差を基に、前記検出点を用いた場合の前記距離画像センサの現在の位置姿勢の誤差を算出する位置誤差算出部と、を有する移動体。【選択図】  図１
2019027983.tif 000002
【特許請求の範囲】
【請求項１】
移動用の駆動装置と、下向きに搭載した距離画像センサと、自己位置を推定する自己位置推定装置と、を具備する移動体であって、
前記自己位置推定装置は、
１フレーム前に検出した画像と現在検出した画像の重複領域を抽出する画像重複領域抽出部と、
前記重複領域内の検出点について、前記距離画像センサから前記検出点までの距離と入射角を基に、前記距離画像センサから前記検出点までの距離誤差を算出し、算出した前記距離誤差を基に、前記検出点を用いた場合の前記距離画像センサの現在の位置姿勢の誤差を算出する位置誤差算出部と、
を有することを特徴とする移動体。
【請求項２】
請求項１に記載の移動体において、
前記自己位置推定装置は、さらに、
前記重複領域内のそれぞれの検出点に対応した前記距離画像センサの現在の位置からの距離誤差を基に、それぞれの検出点の重み付け量を決定する重み付け量決定部と、
１フレーム前と現在の両方で検出した前記検出点の前記重み付け量と、前記検出点までの距離と、１フレーム前の移動体の位置姿勢を基に、現在の移動体の位置姿勢を算出するオドメトリ部と、
を有することを特徴とする移動体。
【請求項３】
請求項２に記載の移動体において、
前記距離画像センサの姿勢を制御する姿勢制御機構を具備しており、
前記自己位置推定装置は、さらに、
１フレーム前および現在の前記移動体と、前記距離画像センサの姿勢と、前記距離画像センサの画角と、前記距離画像センサの設置高さと、前記距離画像センサのフレームレートを基に、現在および１フレーム後に検出する２枚の画像の前記重複領域を推定し、推定した前記重複領域の面積がゼロよりも大きくなるように前記姿勢制御機構を制御して前記距離画像センサの姿勢を調整するセンサ姿勢調整部を有することを特徴とする移動体。
【請求項４】
請求項２に記載の移動体において、
前記距離画像センサの姿勢を制御する姿勢制御機構を具備しており、
前記自己位置推定装置は、前記距離画像センサで取得する、連続する２フレーム分の画像中に常に重複領域が含まれるように前記姿勢制御機構を制御して前記距離画像センサの姿勢を調整するセンサ姿勢調整部を有することを特徴とする移動体。
【請求項５】
移動用の駆動装置と、下向きに搭載した距離画像センサと、自己位置を推定する自己位置推定装置と、を具備する移動体の自己位置推定方法であって、
前記距離画像センサが１フレーム前に検出した画像と現在検出した画像の重複領域を抽出し、
前記重複領域内の検出点について、前記距離画像センサから前記検出点までの距離と入射角を基に、前記距離画像センサから前記検出点までの距離誤差を算出し、
算出した前記距離誤差を基に、前記検出点を用いた場合の前記距離画像センサの現在の位置姿勢の誤差を算出することを特徴とする移動体の自己位置推定方法。
【請求項６】
請求項５に記載の移動体の自己位置推定方法において、
前記重複領域内のそれぞれの検出点に対応した前記距離画像センサの現在の位置からの距離誤差を基に、それぞれの検出点の重み付け量を決定し、
１フレーム前と現在の両方で検出した前記検出点の前記重み付け量と、前記検出点までの距離と、１フレーム前の移動体の位置姿勢を基に、現在の移動体の位置姿勢を算出することを特徴とする移動体の自己位置推定方法。
【請求項７】
請求項６に記載の移動体の自己位置推定方法において、
前記移動体は前記距離画像センサの姿勢を制御する姿勢制御機構を具備しており、
１フレーム前および現在の前記移動体と、前記距離画像センサの姿勢と、前記距離画像センサの画角と、前記距離画像センサの設置高さと、前記距離画像センサのフレームレートを基に、現在および１フレーム後に検出する２枚の画像の前記重複領域を推定し、
推定した前記重複領域の面積がゼロよりも大きくなるように前記姿勢制御機構を制御して前記距離画像センサの姿勢を調整することを特徴とする移動体の自己位置推定方法。
【請求項８】
請求項６に記載の移動体の自己位置推定方法において、
前記距離画像センサの姿勢を制御する姿勢制御機構を具備しており、
前記距離画像センサで取得する、連続する２フレーム分の画像中に常に重複領域が含まれるように前記姿勢制御機構を制御して前記距離画像センサの姿勢を調整することを特徴とする移動体の自己位置推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、路面に向けた距離画像センサを用いて自己位置を推定する自己位置推定装置を備えた移動体に関する。
【背景技術】
【０００２】
自動車、ロボット、ＵＧＶ（Unmanned Ground Vehicle）などの移動体の自己位置推定手法としては、衛星信号を利用し移動体の絶対位置を推定する衛星測位による手法や、移動体の移動量を検出し加算することで移動体の相対位置を推定するオドメトリによる手法や、ＬｉＤＡＲ（Light Detection and Ranging）や距離画像センサ等で走行環境周辺の特徴的な形状や点を検出し、検出した特徴的な形状等を既知の地図と比較することにより移動体の絶対位置を推定するマップマッチングによる手法が知られている。
【０００３】
しかしながら、悪天候等の影響で衛星測位が利用できず、また、測位基準に利用できる固定物が周囲に存在しない視界が開けた状況では、絶対位置を推定する各手法を利用できないため、その代替技術として、カメラ等の光学センサで検出した路面の画像変化や路面までの距離を基に自己位置推定する手法も提案されている。
【０００４】
例えば、特許文献１の請求項１には、「車両周囲の路面にパターン光を投光する投光器と、前記車両に搭載され、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像部と、前記撮像部で取得した画像から、前記路面のペイント領域を検出する路面ペイント検出部と、少なくとも前記ペイント領域上にある前記パターン光の位置に基づいて前記路面に対する車両の姿勢角を算出する姿勢角算出部と、前記撮像部で取得した画像から、前記路面上の複数の特徴点を検出する特徴点検出部と、前記特徴点検出部により検出された路面上の複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出部と、前記路面に対する車両の初期位置及び姿勢角に前記姿勢変化量を加算してゆくことによって前記車両の現在位置及び姿勢角を算出する自己位置算出部を備えたことを特徴とする自己位置算出装置」が記載されている。
【０００５】
そして、その効果を、同文献の段落０００８では、「路面上のペイント領域に投光されたパターン光の位置から路面に対する車両の距離及び姿勢角を算出する。路面のペイント領域は表面が多孔質ではなく、アスファルト部分に比べ、凹凸の少ない、滑らかな表面であるので、ペイント領域に投光されたパターン光は、多孔質材料であるアスファルト路面に投光されたパターン光より同一平面上に存在している可能性が高い。したがって、本発明によれば、路面上にパターン光を投光する場合、路面に対する車両の距離及び姿勢角の誤差を抑制し、ひいては移動量推定の誤差を抑制することができる。」としている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２０１６−２２４５９０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
上述したように、特許文献１では、カメラで撮像した路面の画像から凹凸の少ないペイント領域を検出し、ペイント領域に投稿したパターン光の位置関係を自己位置算出に利用することで、自己位置推定の誤差を抑制している。
【０００８】
しかし、特許文献１の自己位置推定方法を、ペイント領域が存在しない舗装路面や未舗装路の走行中に使用しても、自己位置の誤差を抑制するような路面上の点を検出できないため、自己位置の推定精度の悪化を招いてしまう。
【０００９】
この様な状況に対処するため、路面の画像と距離情報のみで自己位置推定を行う手法として、路面の画像の変化量を検出し、路面までの距離をもとに画像の変化量を３次元空間での移動量に変換し、変換した移動量を積算することで自己位置を算出する手法（ビジュアルオドメトリ）がある。また、画像全体の変化量ではなく画像の一部の変化量をランダムに使って自己位置推定する処理を複数回繰り返すことによって、ビジュアルオドメトリによる自己位置の精度を向上する手法もある。
【００１０】
しかし、ビジュアルオドメトリは、繰り返し演算処理が必要であるため高負荷処理であり、また、自己位置の誤差が尤もらしくなるような（再尤度の）画素を選択する手法のため、検出した路面までの距離の大部分の精度が悪化するような環境では、自己位置の精度向上は期待できない。
【００１１】
本発明は、このような課題を解決するものであり、衛星測位が利用できず、周囲に固定物がない環境でも、演算処理の負荷を低減しつつ、高精度な自己位置推定を実現する移動体を提供することを目的とする。
【課題を解決するための手段】
【００１２】
上記課題を解決するために、代表的な本発明の移動体は、移動用の駆動装置と、下向きに搭載した距離画像センサと、自己位置を推定する自己位置推定装置と、を具備する移動体であって、前記自己位置推定装置は、１フレーム前に検出した画像と現在検出した画像の重複領域を抽出する画像重複領域抽出部と、前記重複領域内の検出点について、前記距離画像センサから前記検出点までの距離と入射角を基に、前記距離画像センサから前記検出点までの距離誤差を算出し、算出した前記距離誤差を基に、前記検出点を用いた場合の前記距離画像センサの現在の位置姿勢の誤差を算出する位置誤差算出部と、を有するものとした。
【発明の効果】
【００１３】
本発明の移動体によれば、衛星測位が利用できず、周囲に固定物がない環境でも、演算処理の負荷を低減しつつ、高精度な自己位置推定を実現することができる。
【図面の簡単な説明】
【００１４】
【図１】一実施例の移動体の構成を示すブロック図である。
【図２Ａ】画像重複領域抽出部での処理の詳細を説明する側面図である。
【図２Ｂ】画像重複領域抽出部での処理の詳細を説明する平面図である。
【図３Ａ】位置誤差算出部での処理の詳細を説明する側面図である。
【図３Ｂ】位置誤差算出部での処理の詳細を説明する斜視図である。
【発明を実施するための形態】
【００１５】
本実施例の移動体１は、距離画像センサを搭載した自動車やロボット等であり、後述の自己位置推定方法を実行する、自己位置推定装置を具備するものである。以下、図面を用いて本実施例の詳細を説明する。
【００１６】
図１のブロック図は、本実施例の移動体１の概略を示している。ここに示すように、移動体１の筐体の下部には車輪やモータなど移動用の駆動装置を備える。また、移動体１の筐体の前側には距離画像センサ２を下向きに搭載し、走行中の路面４を映した画像を取得できるようになっている。
【００１７】
距離画像センサ２は、LiDARや距離画像カメラ等である。LiDARを用いる場合は、高精度であるが方向によって検出時刻が異なるため、検出した全点群の時刻補正が必要であり、移動体１の移動速度変化（加速度）が大きいと検出点群が歪むという問題がある。これに対し、距離画像カメラを用いる場合は、LiDARよりも距離の測定精度が劣るが、全方向の検出時刻が同時であるため、移動体１の加速度による検出点群の歪みはない。
【００１８】
また、距離画像センサ２には、距離画像センサ２の３軸姿勢（ロール、ピッチ、およびヨー）の姿勢制御機構２１が取り付けられている。姿勢制御機構２１の一態様としては、例えば、３軸のモータと３軸のジンバルで構成されるが、３軸の姿勢を制御できる機構であれば、どの機構を採用してもよい。本実施例の移動体１では、姿勢制御機構２１の３軸の回転中心を距離画像センサ２の受光素子の中央位置にとっており、距離画像センサ２の姿勢が姿勢制御機構２１による制御で変化しても距離画像センサ２から路面４までの距離は変化しないようになっている。
【００１９】
移動体１の内部には計算処理部３が搭載されている。この計算処理部３は、本実施例の自己位置推定装置を実現するものであり、画像重複領域抽出部３１、位置誤差算出部３２、重み付け量決定部３３、オドメトリ部３４、センサ姿勢調整部３５で構成されている。なお、これらの一部または全部は、必ずしも専用のハードウェアである必要はなく、半導体メモリ等の主記憶装置に記憶されたプログラムやハードディスク等の補助記憶装置に記憶されたデータを、ＣＰＵ等の演算装置で処理することで実現されるものであっても良い。以下では、図面を用いながら、画像重複領域抽出部３１、位置誤差算出部３２、重み付け量決定部３３、オドメトリ部３４、センサ姿勢調整部３５の各々で実行される処理内容の詳細を説明する。
＜画像重複領域抽出部＞
最初に、図２Ａ、図２Ｂを用いて、距離画像センサ２が取得した連続する２フレーム分の画像の重複領域を抽出する画像重複領域抽出部３１の処理内容の詳細を説明する。図２Ａの側面図において、２（−１）は現在から１フレーム前の距離画像センサ２の自己位置と姿勢であり、２（０）は現在の距離画像センサ２の自己位置と姿勢である。距離画像センサ２のフレームレートがたとえば３０Ｈｚであるとき、１フレーム前の位置姿勢２（−１）と現在の位置姿勢２（０）には約３３ミリ秒（１フレーム）の時間差がある。
【００２０】
図２Ａに示すＯＬ領域は、位置姿勢２（−１）で検出した路面４と、位置姿勢２（０）で検出した路面４が重なるオーバーラップ領域である。このＯＬ領域上の点Ａは、位置姿勢２（−１）からは視線方向ＥＡ（−１）上に見え、位置姿勢２（０）からは視線方向ＥＡ（０）上に見える。
【００２１】
また、図２Ｂの平面図において、Ｐ（−１）は位置姿勢２（−１）で検出した路面４の輝度、すなわち路面４を撮像した画像である。Ｐ（０）は位置姿勢２（０）で検出した路面４の画像である。図２Ａに示す１フレーム前と現在の路面検出領域が重なるＯＬ領域は、図２Ｂでは、画像上の重複領域ＰＯＬに相当する。
【００２２】
ここで、画像重複領域抽出部３１は、画像Ｐ（−１）と画像Ｐ（０）の重複領域ＰＯＬを抽出するものである。この重複領域ＰＯＬは次のように抽出する。例えば、距離画像センサ２の自己位置の移動により画像の各画素が同じような方向と距離に移動することを利用し、１フレーム前に検出した画像Ｐ（−１）と現在検出した画像Ｐ（０）の全画素にオプティカルフロー推定を適用して全画素の対応付けを実施する。オプティカルフロー推定は、Lucas-Kanade法やHorn-Schunk法などがあるが、どの手法を用いてもよい。何れかの手法で画像Ｐ（−１）と画像Ｐ（０）の画素を対応づけることにより、両画像の重複領域ＰＯＬを抽出することができる。
【００２３】
重複領域ＰＯＬの各画素は、１フレーム前に検出した路面４上の点と現在検出した路面４上の点が同一となる画素であるため、例えば、重複領域ＰＯＬ内に存在する点Ａは、路面４上の点Ａとして、１フレーム前の画像Ｐ（−１）と現在の画像Ｐ（０）の両フレームで検出することができる。
＜位置誤差算出部＞
次に、図３Ａ、図３Ｂを用いて、距離画像センサ２の位置の誤差を算出する位置誤差算出部３２での処理の詳細を説明する。
【００２４】
図３Ａの側面図では、画像重複領域抽出部３１で抽出した重複領域ＰＯＬ内の点Ａに注目している。距離画像センサ２は、位置姿勢２（−１）から点Ａまでの距離ＬＡ（−１）を検出誤差ΔＬＡ（−１）の範囲内で検出し、位置姿勢２（０）から点Ａまでの距離ＬＡ（０）を検出誤差ΔＬＡ（０）の範囲内で検出する。
【００２５】
この場合、点Ａは、検出誤差ΔＬＡ（−１）と検出誤差ΔＬＡ（０）を対角線とする平行四辺形（誤差領域ＳΔＡ）のどこかに存在することになる。そして、点Ａの位置のバラツキを許容すると、点Ａまでの距離と方向を基に算出される現在の位置姿勢２（０）の誤差の大きさは、基準となる１フレーム前の位置姿勢２（−１）に対し、検出誤差ΔＬＡ（−１）と検出誤差ΔＬＡ（０）を辺とする平行四辺形（誤差領域ＳΔ２）で表すことができる。このように、点Ａの検出誤差ΔＬＡ（−１）、検出誤差ΔＬＡ（０）の大きさが、点Ａを自己位置推定に利用したときの距離画像センサ２の位置推定の誤差の大きさに影響する。
【００２６】
ここで、距離画像センサ２で検出した対象物までの距離の誤差は、対象物までの距離の長さと、距離画像センサ２で発光した光の対象物への入射角によって変動する。この理由は次の通りである。すなわち、距離画像センサ２で発光した光は、対象物に到達するまでに距離の長さに応じて減衰し、対象物に到達後、対象物を含む路面４の法線方向からみた方向に応じて光量が小さくなるような拡散反射をし、さらに距離に応じて減衰しながら距離画像センサ２に戻ってくる。そのため、対象物までの距離が小さく、対象物を含む路面４での入射角が小さい（路面の法線方向に近い）ほど、対象物までの検出距離の誤差は小さくなる。例えば、図３Ａの例では、位置姿勢２（−１）と位置姿勢２（０）において、点Ａへの入射角は位置姿勢２（０）の方が小さいが、距離画像センサ２と点Ａとの距離が  ＬＡ（−１）  ＜  ＬＡ（０）である影響がより強いため、距離の影響と入射角の影響を合成した検出誤差は、ΔＬＡ（−１）  ＜  ΔＬＡ（０）の関係となっていることが示されている。
【００２７】
そこで、位置誤差算出部３２では、前述の検出距離と入射角と誤差の関係性を利用し、画像重複領域抽出部３１で抽出した重複領域ＰＯＬにあるそれぞれの検出点の誤差を、それぞれの検出点までの距離と入射角を基に算出し、算出したそれぞれの検出点まで距離の誤差を基に、その検出点を自己位置推定に利用した場合の現在の自己位置２（０）の誤差を算出する。検出点までの距離の誤差は次のように算出する。
【００２８】
例えば、位置姿勢２（−１）での検出誤差ΔＬＡ（−１）は、図３Ｂに示す、点Ａを囲む任意の三点（点Ｂ、点Ｃ、点Ｄ）を利用して次のように検出する。先ず、点Ａの近傍で検出した点Ｂ、点Ｃ、点Ｄの位置姿勢２（−１）を原点とした相対位置を算出する。そして、点Ｂ、点Ｃ、点Ｄを頂点とする三角形の法線Ｎを算出し、位置姿勢２（−１）から点Ａへの視線方向ＥＡ（−１）と法線Ｎがなす角を点Ａへの入射角θＡ（−１）として算出する。そして、この入射角θＡ（−１）と距離ＬＡ（−１）を基に位置姿勢２（−１）での検出誤差ΔＬＡ（−１）を算出する。また、現在の位置姿勢２（０）における入射角θＡ（０）、検出誤差ΔＬＡ（０）についても同様に算出できる。
【００２９】
そして、それぞれで検出した点Ａまでの検出誤差ΔＬＡ（−１）、ΔＬＡ（０）を基に、現在の自己位置２（０）の誤差ＳΔ２を算出する。具体的には、２辺の大きさをそれぞれ検出誤差ΔＬＡ（−１）、ΔＬＡ（０）とし、位置姿勢２（−１）と位置姿勢２（０）のそれぞれからみた点Ａへの２本の視線方向ＥＡ（−１）、ＥＡ（０）がなす角を２頂点の内角とする平行四辺形の面積がＳΔ２である。図３Ａに示すように、ＳΔ２の面積は、点Ａの存在範囲ＳΔＡの面積の２倍でもある。なお、図３Ａでは、ＳΔＡとＳΔ２の関係を２次元で表現しているが、３次元でも同じである。
【００３０】
検出点までの距離と入射角から距離誤差への変換は、事前に同じ環境でキャリブレーションをすることで可能であるが、キャリブレーションを実施した環境と異なる環境での移動は望ましくないため、画像重複領域抽出部３１で抽出した重複領域ＰＯＬ内のそれぞれの検出点について距離と入射角の大きさを算出し、例えば距離と入射角の積（つまり、距離を半径とし入射角を中心角とする弧の長さ）を計算し、前記弧の長さを誤差の大きさとするのが良い。このように重複領域ＰＯＬ内の全検出点に対する相対的な誤差の大きさを評価することで、キャリブレーションをしなくても、かつ、どのような環境でも対応することができる。もちろんこの場合、現在の自己位置の誤差ＳΔ２は、それぞれの検出点の誤差をもとに計算した現在の自己位置の全誤差に対する相対的な誤差の大きさである。
【００３１】
ところで、図２Ａ、図３Ａに例示したような、平坦な路面環境では、距離画像センサ２の近くに慣性計測装置などの姿勢センサを搭載して距離画像センサ２の姿勢を逐次検出することで、前記姿勢と距離画像センサ２の設置高さをもとに、重複領域ＰＯＬ内のそれぞれの検出点の距離と入射角を幾何学的に推定することができる。この場合、距離画像センサ２で検出した誤差を含む距離を扱わずに検出点までの検出誤差ΔＬＡ（−１）、ΔＬＡ（０）を計算することができるため、より高精度に現在の自己位置の誤差ＳΔ２を計算できる。
【００３２】
以上で説明した位置誤差算出部３２を用いることで、移動体１が現在移動している路面４の状態を連続的に監視でき、路面状況を考慮した移動速度の決定や、走行時の要注意箇所（路面の凹凸等）を特定できるなどの、移動体１の走行品質の向上に資する情報を取得することが可能となる。
＜重み付け量決定部＞
重み付け量決定部３３は、位置誤差算出部３２で算出した、重複領域ＰＯＬ内のそれぞれの検出点を用いた場合の現在の自己位置の誤差ＳΔ２を基に、重複領域ＰＯＬ内のそれぞれの検出点の重み付け量を決定する。例えば、前記誤差の逆数の総和に対するそれぞれの検出点に対応する現在の自己位置の誤差の逆数の比率を重み付け量とする。または、重複領域ＰＯＬ内の検出点のうち、対応する現在の自己位置の誤差が最も小さい上位数点を選択し、選択した前記検出点の前記誤差の逆数の総和に対する選択したそれぞれの検出点に対応する現在の自己位置の誤差の逆数の比率を重み付け量とする。
【００３３】
このように対応する現在の自己位置の誤差が小さい検出点を選択することで、現在の自己位置をより高精度に算出できる。上述の手法は一例であり、それぞれの検出点に対応する現在の自己位置の誤差が大きいほど重み付け量を小さくする手法であれば、どの手法を用いてもよい。
＜オドメトリ部＞
オドメトリ部３４は、１フレーム前の位置姿勢２（−１）と、距離画像センサ２で検出した重複領域ＰＯＬ内の任意の検出点までの距離および方向と、重み付け量決定部３３で決定したそれぞれの検出点の重み付け量を基に、移動体１の現在の位置姿勢を算出するものである。
【００３４】
本実施例のオドメトリ部３４では、従来のビジュアルオドメトリに比べ演算に必要な検出点数が少なく、演算負荷が小さいため、より短時間でオドメトリ処理を完了させることができる。これは、移動体１の移動速度を速めた場合でもオドメトリ処理を完了できることを意味しているため、本実施例のオドメトリ部３４を採用することで、従来のビジュアルオドメトリを採用した場合に比べ、移動体１の移動速度をより速めても、オドメトリ処理を継続することができる。
＜センサ姿勢調整部＞
センサ姿勢調整部３５は、距離画像センサ２で取得する、連続する２フレーム分の画像中に常に重複領域ＰＯＬが含まれるように距離画像センサ２の姿勢を調整するものである。
【００３５】
具体的には、センサ姿勢調整部３５は、１フレーム前および現在の位置姿勢２（０）と、距離画像センサ２の姿勢と、距離画像センサ２の画角と、距離画像センサ２の設置高さと、距離画像センサ２のフレームレートを基に、現在および１フレーム後に検出する２枚の画像の重複領域ＰＯＬを推定し、重複領域ＰＯＬの面積がゼロと推定される場合には、重複領域ＰＯＬの面積がゼロよりも大きくなるように、すなわち、現在および１フレーム後に検出する２枚の画像の一部が重複するように、姿勢制御機構２１に制御信号を送信して、距離画像センサ２の姿勢を適切に調整するものである。
【００３６】
このように距離画像センサ２の位置姿勢を調整することで、連続する２フレームの画像中に常に重複領域ＰＯＬを検出できるため、１フレーム後に検出した画像からも自己位置推定を実施することができる。そして、この処理を繰り返すことで、起動から停止までの期間を通して、移動体１の位置姿勢の推定を継続することができる。
【００３７】
以上で説明した本実施例の移動体１によれば、ＧＰＳが利用できず、周囲に固定物がない環境でも、演算処理の負荷を低減しつつ、高精度な自己位置推定を実現することができる。
【符号の説明】
【００３８】
１  移動体
２  距離画像センサ
２１  姿勢制御機構
３  計算処理部
３１  画像重複領域抽出部
３２  位置誤差算出部
３３  重み付け量決定部
３４  オドメトリ部
３５  センサ姿勢調整部
４  路面
【図１】
2019027983.tif 000003
【図２Ａ】
2019027983.tif 000004
【図２Ｂ】
2019027983.tif 000005
【図３Ａ】
2019027983.tif 000006
【図３Ｂ】
2019027983.tif 000007