(57)【要約】      （修正有）【課題】自己位置推定時の環境が地図作成時の環境から変動している場合において自己位置推定精度が低下することを抑制することができる自己位置推定方法を提供する。【解決手段】自己位置推定方法は、環境地図に示される点群と距離センサから得られた点群をマッチングして自己位置推定を行う自己位置推定方法である。この自己位置推定方法では、距離センサの計測方向毎に、距離センサから取得される点群について、予め定められた指標に基づく好適度を算出し、好適度が予め定められた基準を満たすような点群が取得される計測方向において距離センサから取得される点群を環境地図に示される点群とのマッチングに用いる。【選択図】図４
2017146893.tif 000002
【特許請求の範囲】
【請求項１】
環境地図に示される点群と距離センサから得られた点群をマッチングして自己位置推定を行う自己位置推定方法であって、
前記距離センサの計測方向毎に、前記距離センサから取得される点群について、予め定められた指標に基づく好適度を算出し、
前記好適度が予め定められた基準を満たすような点群が取得される前記計測方向において前記距離センサから取得される点群を前記環境地図に示される点群とのマッチングに用いる
自己位置推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は自己位置推定方法に関し、特に点群のマッチングを行って自己位置を推定する自己位置推定方法に関する。
【背景技術】
【０００２】
地図を用いて自律的に移動する自律移動ロボットが知られている。このような自律移動ロボットは、距離センサから得られた情報と地図に示される情報とのマッチングを行って、自己位置を推定する。例えば、特許文献１は、距離センサの測定結果から得られた距離和データに対して、地図データ内におけるこれに近似した距離和データを検索し、その結果に基づいて自律移動ロボットの位置・姿勢を推定するマッチング処理を行うことについて開示している。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１３−０２０３４５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで、例えば、地図中に示されている物体が自己位置推定時には移動していたり、地図に示されていなかった人が自己位置推定時に出現していたりすることによって、地図に示される環境の情報と、自己位置推定時の実際の環境が異なることが起こりうる。これは、自己位置推定精度の低下を招く。
【０００５】
本発明は、このような問題を解決するためになされたものであり、自己位置推定時の環境が地図作成時の環境から変動している場合において自己位置推定精度が低下することを抑制することができる自己位置推定方法を提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明の一態様に係る自己位置推定方法は、環境地図に示される点群と距離センサから得られた点群をマッチングして自己位置推定を行う自己位置推定方法であって、前記距離センサの計測方向毎に、前記距離センサから取得される点群について、予め定められた指標に基づく好適度を算出し、前記好適度が予め定められた基準を満たすような点群が取得される前記計測方向において前記距離センサから取得される点群を前記環境地図に示される点群とのマッチングに用いる方法である。
この方法によれば、環境が、環境地図の作成時から変動している場合に、その変動による影響が小さいデータを選んで自己位置推定を行うことが可能となる。このため、自己位置推定の精度の低下が抑制される。
【発明の効果】
【０００７】
本発明によれば、自己位置推定時の環境が地図作成時の環境から変動している場合において自己位置推定精度が低下することを抑制することができる自己位置推定方法を提供することができる。
【図面の簡単な説明】
【０００８】
【図１】実施の形態にかかる自律移動ロボットを示す模式図である。
【図２】実施の形態にかかる制御装置の機能構成の一例を示すブロック図である。
【図３】好適度の算出に用いられる各変数について示す模式図である。
【図４】実施の形態にかかる自己位置推定方法による自律移動ロボットの動作の一例を示すフローチャートである。
【図５】図４に示されるフローチャートのステップ１００の動作の具体的な一例を示すフローチャートである。
【図６】図４に示されるフローチャートのステップ１００の動作の具体的な他の一例を示すフローチャートである。
【図７】比較例にかかる自己位置推定方法について説明する模式図である。
【発明を実施するための形態】
【０００９】
本発明の実施の形態について説明する前に、まず、特許文献１に記載された自己位置推定方法を基に発明者が検討した比較例について説明する。
【００１０】
この比較例では、まず、環境地図を作成する際に、距離センサにより取得される点群と、この点群の距離の和の値である距離和と、自律移動体の自己位置とを紐づけて一緒に保存する。そして、自己位置推定の際、図７に示されるように、距離センサから取得される点群から算出される距離和に対して、環境地図の中から一番近い距離点群を取り出す。図７に示した例では、距離センサデータから算出される距離和が１０であり、環境地図の中から、この距離和に最も近い距離和である１１を有する距離点群が取り出される。続いて、取り出された距離点群に対応する自己位置を初期値に使い、点群同士のマッチングを行って、自己位置推定を行う。
【００１１】
このような自己位置推定方法の場合、次のような問題があることを発明者は見出した。例えば、環境地図を作成した時に比べて物体の配置が変更されている場合や環境地図を作成した時に存在していなかった物体（人間等）が自己推定時に存在している場合など、環境地図を作成した時と比較して自己位置推定時に計測する環境に変化があった場合において、変化があった部分が、距離センサデータの中で大部分を占めるようなときには、以下の問題がある。すなわち、距離和のみで初期値を取ろうとすると、誤った点群とマッチしてしまい、自己位置推定に失敗する可能性がある。このような問題への対策として、距離センサの向きを変えながら、自己位置推定に成功するまで、処理を続けるという方法も考えられるが、推定結果の自己位置の精度が悪い恐れがある。また、自律移動体の位置の真値が分からず、かつ、環境の変化した部分がどこか分からないため、自己位置推定結果の精度の良し悪しの評価を行うことができない。これは、環境変化部分の割合が大きい距離センサデータが入力されても、その入力に基づく自己位置推定結果を採用せざるを得ないことに起因している。
【００１２】
したがって、自己位置推定を行う際に、自己位置推定の良し悪しの評価を行うことができる基準を導入し、その基準を元に評価を行うことができれば、上記の問題が発生する可能性が低くなることが期待される。
【００１３】
実施の形態１
以下、本発明の実施の形態について図面を参照して詳細に説明する。図１には、実施の形態にかかる自己位置推定装置の一例として自律移動ロボット１が示されている。自律移動ロボット１は、距離センサ１１と、センサ移動制御部１２と、制御装置１３と、車輪１４とを有しており、車輪１４を駆動させることにより移動する。なお、本実施の形態では、自律移動ロボット１は、車輪１４の駆動により移動するが、他の手段により移動を実現してもよい。
【００１４】
距離センサ１１は、自律移動ロボット１の周辺の３次元の検出領域Ｓ内にある環境（物体）と自律移動ロボット１との距離情報を検出する。距離センサ１１として、例えば、レーザレンジファインダが用いられるが、距離センサ１１は、これに限らず、例えば、デプスセンサ、超音波センサ、赤外線センサ、カメラ等の任意の距離センサであってもよい。
【００１５】
センサ移動制御部１２は、距離センサ１１の視点パターン、すなわち計測方向を変更するよう距離センサ１１の向きを変更する。センサ移動制御部１２は、例えば、制御信号に基づいてアクチュエータを駆動することにより、距離センサ１１の向きを移動させる。
【００１６】
制御装置１３は、例えば、制御処理、演算処理等を行うＣＰＵ（Central Processing Unit）、ＣＰＵによって実行される制御プログラム、演算プログラム、処理データ等を記憶する、ＲＯＭ（Read Only Memory）及びＲＡＭ（Random Access Memory）などにより構成される記憶装置等からなるマイクロコンピュータを中心にして、ハードウェア構成されている。
【００１７】
制御装置１３は、自律移動ロボット１の全体を制御する。例えば、制御装置１３は、センサ移動制御部１２の制御、及び車輪１４の駆動制御といった各種制御を行うとともに、図２に示される構成により以下に示す処理を実行する。
【００１８】
図２は、制御装置１３の機能構成の一例を示すブロック図である。なお、図２に示される各構成は、例えば、上記制御プログラム又は演算プログラムがＣＰＵにより実行されることで実現される。
【００１９】
制御装置１３は、環境地図記憶部１３０と、視点選択部１３１と、距離センサデータ取得部１３２と、自己位置推定部１３３とを有している。本実施の形態では、距離センサデータが自己位置推定に好適なものであるかどうかの指標（以下、好適度という）を導入し、自律移動ロボット１が距離センサデータを取得するごとに、この距離センサデータに対応する好適度を計算する。自律移動ロボット１は、センサ移動制御部１２の制御により距離センサ１１を動かしながら、距離センサ１１の姿勢、距離センサ１１から取得される距離センサデータ、及びこの距離センサデータに対する好適度からなる種々の組み合わせを収集し、好適度が最適となる距離センサ１１の姿勢を選択する。そして、選択された距離センサ１１の姿勢から取得される距離センサデータと、環境地図との照合を行い、自己位置の計算を行う。以下、これを実現する各構成について説明する。
【００２０】
環境地図記憶部１３０は、例えば上述の記憶装置により実現され、自律移動ロボット１が動作する環境についての地図情報である環境地図を記憶している。この環境地図は、予め、自律移動ロボット１が移動する範囲について距離センサ１１の検出結果に基づいて作成され、環境地図記憶部１３０に記憶されている。この環境地図には、地図作成の際に距離センサ１１により取得された点群の情報（点群の座標及び各点の距離）が含まれる。
【００２１】
視点選択部１３１は、センサ移動制御部１２に指示することにより距離センサ１１を動かし、距離センサ１１の視点パターン（計測方向）毎に、距離センサ１１から取得される点群について、予め定められた指標に基づく好適度を算出する。そして、視点選択部１３１は、好適度が予め定められた基準を満たす点群が取得される視点を選択する。また、視点選択部１３１は、選択された視点へと距離センサ１１を向ける。
【００２２】
距離センサデータ取得部１３２は、視点選択部１３１により選択された視点において距離センサ１１から取得された距離センサデータを取得する。
【００２３】
自己位置推定部１３３は、距離センサデータ取得部１３２が取得した距離センサデータと、環境地図との照合を行い、自己位置を推定する。
【００２４】
以下、好適度について具体的に説明する。本実施の形態では、自己位置推定の好適度を、距離センサ１１から得られた点群の情報のみから規定する。好適度をＣ、距離センサ１１から得られたｎ点の点群分布を表す行列をＰとすると、好適度は以下の式（１）に示す通り、関数ｆを用いて一意に計算される。なお、行列Ｐは、以下の式（２）のように表される。
【００２５】
【数１】
2017146893.tif 000003
【００２６】
【数２】
2017146893.tif 000004
【００２７】
以下、好適度Ｃの具体的な例について説明する。従来手法における問題は、環境地図に対して変化した物体の割合が大きい場合に生じる。すなわち、変化した物体の割合が小さくなるよう、自己位置推定に用いるデータを選択することにより、自己位置推定精度の低下を抑制することができる。したがって、そのようなデータである場合に好適度Ｃの値が大きくなるようにすればよい。変化する物体の割合が小さくなるようにするには、例えば、距離センサ１１が、なるべく遠くから、なるべく縦横に広い分布をなす点群を見ることが好ましいと考えられる。なぜならば、このようにすることで、分布の中で一部の物体が動いても、全体の分布の中での動いた点の割合は小さくなるからである。なお、縦方向は、例えば鉛直方向であり、横方向は例えば水平方向である。
【００２８】
この考え方を式に表すと、好適度Ｃは例えば以下の式（３）のように表される。なお、以下の式（３）において、図３に示されるように、Ｇは、距離センサ１１を中心とした座標系における距離センサデータの点群位置のなす重心であり、ＬＧは距離センサ１１と重心Ｇとの距離であり、ＶＸ、ＶＹ、ＶＺは、それぞれ重心Ｇを中心とする点群の分布のばらつき具合を表す分散である。ここで、ＶＸは縦方向の分散を示し、ＶＹは横方向の分散を示し、ＶＺは奥行方向の分散を示す。また、ｍ、ｎは、それぞれ調整ゲインである。
【００２９】
【数３】
2017146893.tif 000005
【００３０】
式（３）に示した例では、好適度Ｃは、距離センサ１１からの点群分布の距離を表す指標（以下、距離指標という）と点群分布の縦横への広がりの大きさを表す指標（以下、分散指標という）との和により定義される。式（３）に示される好適度Ｃの式の第一項は、点群分布が距離センサ１１から遠くにあるほど好適であることを表す成分を示している。また、第二項は、分布の縦横への広がり具合が大きいほど好適であることを表す成分を示している。ただし、距離センサ１１からの距離は異なるが縦横の広がり具合は同じ大きさである分布について、距離センサ１１から遠い分布ほど、相対的に分布が小さくなるようにする必要がある。このため、第二項においては、縦方向の分散と横方向の分散の積を、重心への距離ＬＧで割り、距離の影響をキャンセルするようにしている。なお、調整ゲインｍ、ｎの値を調整することにより、好適度Ｃにおける、距離指標の寄与度合い及び分散指標の寄与度合いを調整することができる。
【００３１】
次に、本実施の形態にかかる自己位置推定方法による動作の一例を説明する。図４は、本実施の形態にかかる自己位置推定方法による自律移動ロボット１の動作の一例を示すフローチャートである。なお、環境地図については、予め作成されているものとする。
【００３２】
図４に示される例では、ステップ１００（Ｓ１００）及びステップ１０１（Ｓ１０１）により、最適な視点の選出についての動作がなされ、その後、ステップ１０２（Ｓ１０２）で取得されたデータに基づいて、ステップ１０３（Ｓ１０３）及びステップ１０４（Ｓ１０４）により、自己位置推定処理を行う動作がなされる。以下、詳細に説明する。
【００３３】
ステップ１００において、視点選択部１３１は、自己位置推定にもっとも好適な距離センサ視点を選出する。視点選択部１３１は、距離センサ１１を動かし、視点パターンを変更し、視点パターン毎に好適度を計算し、好適度が一番大きくなる視点を選出する。なお、ステップ１００の詳細については、図５を用いてさらに説明する。
【００３４】
ステップ１０１において、次に、センサ移動制御部１２が、ステップ１００で選出された視点に距離センサ１１を向けるよう距離センサ１１の位置を変更する。
【００３５】
ステップ１０２において、距離センサデータ取得部１３２が、ステップ１０１で設定された視点による距離センサデータを取得し、自己位置推定部１３３に出力する。
【００３６】
ステップ１０３において、自己位置推定部１３３は、環境地図記憶部１３０に記憶された環境地図に示される点群と、ステップ１０２で取得された距離センサデータに示される点群をマッチングして、距離センサ１１に固定された座標系における位置と、移動空間に固定された座標系（世界座標系）における位置との間の変換を行う三次元変換を求める。
【００３７】
ステップ１０４において、自己位置推定部１３３は、三次元変換により、環境地図上における自己位置を推定する。
【００３８】
このように、本実施の形態にかかる自律移動ロボット１（自己位置推定装置）によれば、環境地図に示される点群と距離センサ１１から得られた点群をマッチングして自己位置推定を行う自己位置推定方法であって、次のステップを含む方法が提供される。すなわち、距離センサ１１の計測方向毎に、距離センサ１１から取得される点群について、予め定められた指標に基づく好適度を算出するステップ（ステップ１００）と、好適度が予め定められた基準を満たすような点群が取得される計測方向において距離センサ１１から取得される点群を環境地図に示される点群とのマッチングに用いるステップ（ステップ１０３）とを含む自己位置推定方法が提供される。
【００３９】
次に、上記ステップ１００における動作の詳細について説明する。図５は、図４に示されるフローチャートのステップ１００の動作の具体的な一例を示すフローチャートである。図５に示す例では、視点選択部１３１は、全ての視点パターンのうち、好適度Ｃが最大となる視点パターンを探し出すことにより、視点パターンを決定する。具体的には、以下の動作がなされる。
【００４０】
ステップ１１１（Ｓ１１１）において、視点選択部１３１は、距離センサ１１から距離センサデータ（点群センサデータ）を取得する。
【００４１】
ステップ１１２（Ｓ１１２）において、視点選択部１３１は、ステップ１１１において取得された点群について好適度の値を計算し、計算結果を一時的にメモリ等に蓄積する。
【００４２】
ステップ１１３（Ｓ１１３）において、視点選択部１３１は、全ての視点パターンについて計測が完了したか否かを判定する。すなわち、全ての視点パターンについて好適度の値が計算された場合、処理はステップ１１５に移行し、全ての視点パターンについて好適度の値が計算されていない場合、処理はステップ１１４に移行する。
【００４３】
ステップ１１４（Ｓ１１４）において、視点選択部１３１は、好適度の値が算出されていない視点パターンへと変更するよう、センサ移動制御部１２に指示する。これにより、距離センサ１１の視点が動かされ、再び、処理はステップ１１１に戻る。
【００４４】
ステップ１１５（Ｓ１１５）において、視点選択部１３１は、全ての視点パターンに対する各好適度の値のうち最大値を求める。具体的には、視点選択部１３１は、ステップ１１２において蓄積された各値を比較して最大値を求める。
【００４５】
ステップ１１６（Ｓ１１６）において、視点選択部１３１は、好適度の値が最大となるセンサ視点を出力する。これにより、視点選択部１３１により視点の選択が完了する。
【００４６】
以上、実施の形態１について説明した。実施の形態１にかかる自己位置推定方法によれば、自己位置推定への好適度が一番大きい距離センサデータが選出される。このため、取得される距離センサデータのうち、変化した物体の割合が他よりも小さそうなデータを選んで、自己位置推定を行うことが可能となる。つまり、環境が、環境地図の作成時から変動している場合に、その変動による影響が小さいデータを選んで自己位置推定を行うことが可能となる。その結果、自己位置推定時の環境が地図作成時の環境から変動している場合において、自己位置推定の成功率又は精度が低下することを抑制することができる。なお、予め作成した環境地図と距離センサデータとを用いて、データの適正を判断する指標の計算を行うことも考えられるが、このような手法の場合、距離センサで計測する環境が変化した際に、環境地図とのマッチングがうまくいかない。これに対し、上記実施の形態にかかる自己位置推定方法によれば、距離センサデータのみから好適度の計算を行うため、複数の視点のうち環境の変化の小さそうな視点で得られたデータを選択でき、環境地図とうまくマッチングさせることが可能となる。
【００４７】
また、好適度を自己位置推定の尤度の計算に使用することにより、推定した結果がどのくらい確からしいかという指標を上位処理系が知ることができ、以後の処理に利用することも可能となる。
【００４８】
実施の形態２
次に、実施の形態２について説明する。実施の形態１では、図５で示したように、視点選択部１３１は、好適度の値が最大となる視点パターンを選択する。これに対し、本実施の形態では、視点選択部１３１は、好適度の値が予め定められた閾値を超える視点パターンを選択する。本実施の形態は、この点において異なり、他については実施の形態１と同様であるため、異なる部分についてのみ説明し、重複する点については説明を省略する。
【００４９】
図６は、図４に示されるフローチャートのステップ１００の動作の具体的な他の一例を示すフローチャートである。図６に示す例では、視点選択部１３１は、好適度の値が予め定められた閾値を超える視点パターンを探し出すことにより、視点パターンを決定する。具体的には、以下の動作がなされる。
【００５０】
ステップ１２１（Ｓ１２１）において、図５のステップ１１１と同様、視点選択部１３１は、距離センサ１１から距離センサデータ（点群センサデータ）を取得する。
【００５１】
ステップ１２２（Ｓ１２２）において、視点選択部１３１は、ステップ１２１において取得された点群について好適度の値を計算する。そして、ステップ１２３（Ｓ１２３）において、視点選択部１３１は、算出された値が、予め定められた閾値を超えるか否かを判定する。好適度の値が閾値を超える場合、処理はステップ１２７へ移行する。これに対し、好適度の値が閾値を超えない場合、処理はステップ１２４へ移行する。
【００５２】
ステップ１２４（Ｓ１２４）において、図５のステップ１１３と同様、視点選択部１３１は、全ての視点パターンについて計測が完了したか否かを判定する。全ての視点パターンについて好適度の値が計算された場合、処理はステップ１２５に移行し、全ての視点パターンについて好適度の値が計算されていない場合、処理はステップ１２６に移行する。
【００５３】
ステップ１２５（Ｓ１２５）では、全ての視点パターンにおいて、好適度の値が閾値未満であったとして、視点選択部１３１は、処理の失敗を通知し、処理を終了する。これに対し、ステップ１２６（Ｓ１２６）では、図５のステップ１１４と同様、視点選択部１３１は、好適度の値が算出されていない視点パターンへと変更するよう、センサ移動制御部１２に指示する。これにより、距離センサ１１の視点が動かされ、再び、処理はステップ１２１に戻る。
【００５４】
ステップ１２７（Ｓ１２７）において、ステップ１２３において、好適度の値が閾値を超えると判定された視点を出力する。これにより、視点選択部１３１により視点の選択が完了する。
【００５５】
本実施形態にかかる自己位置推定方法によれば、自己位置推定への好適度が所定の基準を満たす距離センサデータが選出される。このため、取得される距離センサデータのうち、変化した物体の割合が基準以下であるデータを選んで、自己位置推定を行うことが可能となる。その結果、自己位置推定時の環境が地図作成時の環境から変動している場合において、自己位置推定の成功率又は精度が低下することを抑制することができる。
【００５６】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上記実施の形態では、環境地図に対して変化した物体の割合が小さくなるような好適度Ｃを例に挙げているが、その他にも様々な目的に応じて好適度を設定し、上記の実施形態と同様に自己位置推定に適用することができる。例えば、特定の範囲の距離の計測精度が他の範囲の距離の計測精度よりも良いという特性を距離センサ１１が有している場合、距離センサ１１で取得した点群の重心と距離センサ１１との間の距離が、この特定の範囲に近いほど好適度の値が大きくなるように好適度を設定する、などといったことが考えられる。
【符号の説明】
【００５７】
１    自律移動ロボット
１１    距離センサ
１２    センサ移動制御部
１３    制御装置
１４    車輪
１３０    環境地図記憶部
１３１    視点選択部
１３２    距離センサデータ取得部
１３３    自己位置推定部
【図１】
2017146893.tif 000006
【図２】
2017146893.tif 000007
【図３】
2017146893.tif 000008
【図４】
2017146893.tif 000009
【図５】
2017146893.tif 000010
【図６】
2017146893.tif 000011
【図７】
2017146893.tif 000012