JP2010224755A - 移動体及び移動体の位置推定方法 - Google Patents

Abstract

【課題】必要十分なパーティクルフィルタを動的に設定すること。
【解決手段】本発明に係る移動体は、レーザセンサの測定値と複数のパーティクルに基づいて、自己位置を推定する。パーティクルのパーティクル数は、パーティクルの存在分布半径に応じて動的に変更する。エンコーダの出力からの積分による移動量と、過去のパーティクルと、から現在の自己位置を推定し、自己位置推定における複数のパーティクルのうち、レーザセンサ測定値に最もマッチしたパーティクルと、エンコーダによる自己位置と、に基づいて、自己位置の補正量を算出する。そして、自己位置の補正量に基づいて、パーティクルの動作をガウス分布で近似した場合の、パラメータである平均及び標準偏差を算出し、そのパラメータにより規定され、自己位置推定におけるパーティクルを動作させるために使用される運動モデルを算出する。
【選択図】図１

Description

本発明は移動体及び移動体の位置推定方法に関し、特に、パーティクルフィルタを用いて自己位置を推定しながら移動する移動体、及びその位置推定方法に関する。

環境を自律的に移動する自律移動ロボットは、自己の位置を推定する手段を備えている。例えば、特許文献１及び２には、上位センサからの距離情報と下位センサからの自己位置とを統合して、自己の位置を推定する技術が開示されている。

特許文献１には、カルマンフィルタを用いたエンコーダ（下位センサ）からの自己位置と、ステレオカメラ（上位センサ）から測定した距離情報によるパーティクルフィルタを用いた自己位置と、を結合することで、最終的な自己位置を算出する自律移動体が開示されている。パーティクルフィルタでは、取得したセンサ値に基づいてガウス分布の分散・期待値を変動させ、動的にパーティクル存在分布を変化させる。

特許文献２には、ステレオカメラによって環境情報に自己位置を推定する上位制御機構と、エンコーダやジャイロセンサによる下位制御機構とを統合させることにより、自己位置を推定する自律移動体が開示されている。

また、特許文献３には、パーティクルフィルタによる位置推定システムが開示されている。特許文献３に係る自己位置推定システムは、緯度経度情報などの測定データに基づいて座標系上に確率分布を設定し、その設定した確率分布を用いたパーティクルフィルタから自己位置を推定する。

特開２００８−２３４３５０号公報 特開２００８−０３３６９６号公報 特開２００８−２４９６４０号公報

しかしながら、特許文献１に開示された位置推定技術では、パーティクルの個数を変化させないものであるために、分散が大きくなり広範囲にパーティクルを配置していく場合には、真の位置を補足するパーティクルが存在しない確率が上昇してしまう。このため、オドメトリ誤差の大きな移動体である場合や、滑り易い走行路面や個体差によってオドメトリの誤差が大きくなった場合には、真の位置を補足し続けることが困難になるという問題がある。
また、上位センサにステレオカメラを用いていることから、環境の明るさなどによる距離算出の精度への影響が大きく、暗い場所での自己位置の精度が低下するという問題がある。

さらに、特許文献２及び３に開示された位置推定技術によっても、路面の滑り易さや、メカ的なばらつきに応じた、必要十分なパーティクルフィルタを動的に設定することはできないものであった。

従って、本発明は、路面の滑り易さやメカ的なばらつきに応じた、必要十分なパーティクルフィルタを動的に設定可能な、移動体及びその位置推定方法を提供することを目的とする。

本発明に係る移動体は、パーティクルフィルタを用いて自己位置を推定しながら移動する移動体であって、レーザセンサを用いて障害物までの距離を測定するレーザセンサ距離測定部と、前記レーザセンサの測定値と複数のパーティクルに基づいて、自己位置を推定する自己位置推定演算部と、前記複数のパーティクルの存在分布半径に応じて、前記自己位置推定演算部における複数のパーティクルのパーティクル数を算出するパーティクル数演算部と、エンコーダの出力からの積分による移動量と、過去のパーティクルと、から現在の自己位置を推定するエンコーダ積算演算部と、前記自己位置推定演算部における複数のパーティクルのうち、前記レーザセンサの測定値に最もマッチしたパーティクルと、前記エンコーダ積算部による自己位置と、に基づいて、自己位置の補正量を算出する補正値演算部と、前記自己位置の補正量に基づいて、パーティクルの動作をガウス分布で近似した場合の、当該ガウス分布のパラメータである平均及び標準偏差を算出するガウス分布パラメータ演算部と、算出した前記ガウス分布のパラメータにより規定され、前記自己位置推定演算部における複数のパーティクルを動作させるために使用される運動モデルを算出する運動モデル演算部と、を備えるものである。

これにより、路面の滑り易さやメカ的なばらつきに応じた、必要十分なパーティクルフィルタを動的に設定することができる。

本発明に係る位置推定方法は、環境を自律的に移動する移動体の位置推定方法であって、レーザセンサを用いて障害物までの距離を測定するステップと、前記レーザセンサの測定値と複数のパーティクルに基づいて、自己位置を推定するステップと、前記複数のパーティクルの存在分布半径に応じて、前記複数のパーティクルのパーティクル数を算出するステップと、エンコーダの出力からの積分による移動量と、過去のパーティクルと、から現在の自己位置を推定するステップと、前記複数のパーティクルのうち、前記レーザセンサの測定値に最もマッチしたパーティクルと、前記エンコーダ積算による自己位置と、に基づいて、自己位置の補正量を算出するステップと、前記自己位置の補正量に基づいて、パーティクルの動作をガウス分布で近似した場合の、当該ガウス分布のパラメータである平均及び標準偏差を算出するステップと、算出した前記ガウス分布のパラメータにより規定され、前記複数のパーティクルを動作させるために使用される運動モデルを算出するステップと、を有するものである。

本発明によれば、路面の滑り易さやメカ的なばらつきに応じた、必要十分なパーティクルフィルタを動的に設定可能な、移動体及びその位置推定方法を提供することができる。

実施の形態１に係る移動体の演算部の構成図である。 実施の形態１に係る位置推定処理を説明するための図である。

実施の形態１
以下、図面を参照して本発明の実施の形態について説明する。図１は、本発明の実施の形態１に係る移動体が有する、自己位置推定に関する演算部の構成図である。図に示すように、演算部は、レーザセンサ距離測定部１０と、自己位置推定演算部２０と、パーティクル数演算部３０と、エンコーダ積算演算部４０と、補正値演算部５０と、ガウス分布パラメータ演算部６０と、運動モデル演算部７０と、を備えている。

レーザセンサ距離測定部１０は、ＴＯＦの原理に基づいたレーザセンサからの測定値により、環境に存在する障害物までの距離を測定する。

自己位置推定演算部２０は、レーザセンサ距離測定部１０からの測定値と、パーティクル数と、各パーティクルの動作と、に基づいて、移動体の自己位置を推定する。後述するように、自己位置推定演算の結果、自己位置推定演算部２０は、パーティクルの存在分布半径２１と、マップマッチングの結果の良い位置２２と、を出力する。尚、パーティクルは、移動体の自己の位置（及び姿勢）を示す情報であり、ｘ、ｙ、θの値を有している。

パーティクル数演算部３０は、パーティクルの存在分布半径２１に基づいて、パーティクル数３１を算出する。エンコーダ積算演算部４０は、エンコーダの出力値から、積算のみの位置４１を算出する。補正値演算部５０は、マップマッチングの結果の良い位置２２と、積算のみの位置４１と、から、自己位置補正量５１を算出する。

ガウス分布パラメータ演算部６０は、自己位置補正量５１から、運動モデルをガウス分布で近似した場合の、パラメータであるガウス分布の標準偏差と平均６１を算出する。そして、運動モデル演算部７０は、ガウス分布の標準偏差と平均６１から、ｘ、ｙ、θの各方向についての、各パーティクルの動作７１を算出する。

図２は、本発明に係る位置推定処理を説明するための図である。
ステップＳ１０１では、全パーティクルを運動モデルによりそれぞれランダムに移動させ、レーザセンサ測定値と地図のマッチング度合いを計算する。具体的には、例えば、レーザセンサにより壁までの測定値を取得した場合には、地図上に、壁が見えるレーザセンサの測定値となる移動体の存在範囲と、パーティクルにより示される自己の位置とを重ねあわせて、互いのズレ量を評価する。そして、全パーティクルのうちで、最もマッチング結果の良い最良パーティクル（即ち、レーザセンサの測定値と最もマッチするパーティクル）を出力する。また、各パーティクルの存在分布半径を算出して、出力する。

ステップＳ１０２では、パーティクルの存在分布半径に応じて、パーティクル数を算出する。例えば、図に示すように、パーティクルの存在分布半径の増加に伴って、パーティクル数を動的に増加させて変化させる。算出したパーティクル数は、次回のパーティクルの位置更新処理に使用される。また、図に示す存在分布半径とパーティクル数との関係は、図示しない記憶部に予め設定されて記憶される。

ステップＳ２０１では、エンコーダ２からの出力から、積分によるΔｔの移動量を演算する。即ち、Δｔの間に、ｘ、ｙ、θ方向に関して、移動体が移動した移動量を積分することで算出する。

ステップＳ２０２では、ステップＳ１０１で求めた最良パーティクルの前回値に対して、ステップＳ２０１で算出したΔｔの移動量を加算することで、エンコーダ積算による自己位置を計算する。即ち、前回の時点での最良のパーティクルの位置及び姿勢から、エンコーダに基づく、自己位置を推定する。

ステップＳ２０３では、ステップＳ１０１で求めた最良パーティクルと、ステップＳ２０２で求めたエンコーダ積算による自己位置と、に基づいて、自己位置の補正量を演算する。具体的には、図に示すように、最良パーティクルの位置とエンコーダ積算による自己位置から位置の補正量Ａを算出する。また、最良パーティクルの姿勢とエンコーダ積算による姿勢から角度の補正量Ｂを算出する。補正量Ａ及びＢは、ｘ、ｙ、θの各方向について算出する。

ステップＳ２０４では、ステップＳ２０３で算出した位置及び角度の補正量から、（ガウス分布のパラメータにより規定される）パーティクルの運動モデルを演算する。具体的には、まず、ステップＳ２０３で算出した位置及び角度の補正量を、ガウス分布で近似した場合の、パラメータである平均及び標準偏差σを算出する。そして、算出した平均と、所定の基準値とのズレ量をＯｆｆｓｅｔとして算出する。そして、算出したＯｆｆｓｅｔと標準偏差σから、図に示す、パーティクルの運動モデルを演算する。ステップＳ２０４では、ｘ、ｙ、θ方向それぞれの運動モデルを演算する。各方向の運動モデルを、各パーティクルの動作データ（Δｘ、Δｙ、Δθ）として出力する。

尚、図において、横軸はステップＳ２０３で算出した補正量の値に対応する値であり、縦軸はその値の出現頻度である。実線で示す縦軸と破線で示す縦軸との間の量が、Ｏｆｆｓｅｔである。Ｏｆｆｓｅｔは、ステップＳ２０３での出力値をフィルタ１で処理して、その値を積分することで算出する。標準偏差σは、ステップＳ２０３での出力値をフィルタ２で処理して、その値を積分することで算出する。

ステップＳ２０５では、ステップＳ１０２で求めたパーティクル数と、ステップＳ２０４で求めた各パーティクルの動作データと、に基づいて、パーティクル位置を更新する。即ち、ステップＳ２０４で求めた各方向の運動モデルに従って、ステップＳ１０２で求めたパーティクル数のパーティクルを生成する。

このようにして、本発明に係る移動体によれば、動的にパーティクル数を変化させると共に、推定自己位置とエンコーダ積算との差に基づく補正量をパーティクルの動作に反映させることで、路面の滑り易さや、メカ的なばらつきに応じた、必要十分なパーティクルフィルタを動的に設定することができる。

本発明に係る移動体は、環境を自律的に移動する移動ロボットである。例えば、移動体が車両型の移動ロボットである場合には、移動ロボットは、移動手段として１対の対向する左右駆動輪を備えている。さらに、車両本体の内部には、左右駆動輪をそれぞれ駆動する駆動部（モータ）と、駆動輪の回転数や回転速度を検出するためのエンコーダと、駆動輪を駆動するための制御信号を作成し、駆動部にその制御信号を送信する制御部が備えられている。そして、制御部内部に備えられた記憶部としてのメモリなどの記憶領域には、制御信号に基づいて車両の移動速度や移動方向、移動距離などを制御するための制御プログラムが記録されている。前述の移動速度や移動距離などは、エンコーダで検知された左右駆動輪の回転数に基づいて求められている。

また、車両本体の前面には、移動する方向に現れた障害物等を認識するためのレーザセンサが搭載されている。レーザセンサは、障害物等において反射されたレーザを検知するセンサである。このレーザセンサによる測定値が制御部に入力された結果、制御プログラムに従って車両の移動する方向や速度等が決定される。

また、上述した実施の形態では、移動体が自己位置推定機能を有するものとして説明したが本発明はこれに限定されない。自己位置推定機能は、例えば、移動体に搭載されたコンピュータや移動体とは別に設けられたコンピュータにより実現される。このコンピュータは、例えば、中央処理装置（ＣＰＵ）、ＲＯＭ、ＲＡＭ、ハードディスク等の補助記憶装置、ＣＤ−ＲＯＭ等の可搬型記憶媒体が挿入される記憶媒体駆動装置、入力手段や出力手段を備えている。ＲＯＭ、補助記憶装置、可搬型記憶媒体等の記憶媒体には、オペレーティングシステムと協働してＣＰＵ等に命令を与え、アプリケーションプログラムを記録することができ、ＲＡＭにロードされることによって実行される。このアプリケーションプログラムは、本発明に係る自己位置推定機能を実現する特有の自己位置推定プログラムを含む。自己位置推定プログラムによる自己位置推定は、中央処理装置がアプリケーションプログラムをＲＡＭ上に展開した上で当該アプリケーションプログラムに従った処理を補助記憶装置に格納されたデータを読み出し、また格納を行なうことにより、実行される。

尚、本発明は上述した各実施の形態に限定されず、趣旨を逸脱しない範囲で適宜変更することが可能である。

１ レーザセンサ、 ２ エンコーダ、
１０ レーザセンサ距離測定部、 ２０ 自己位置推定演算部、
２１ パーティクルの存在分布半径、 ２０ パーティクル数演算部、
３１ パーティクル数、 ４０ エンコーダ積算演算部、 ４１ 積算のみの位置、
５０ 補正値演算部、 ５１ 自己位置補正量、
６０ ガウス分布パラメータ演算部、 ６１ ガウス分布の標準偏差と平均、
７０ 運動モデル演算部、 ７１ 各パーティクルの動作

Claims (2)

1. パーティクルフィルタを用いて自己位置を推定しながら移動する移動体であって、
   レーザセンサを用いて障害物までの距離を測定するレーザセンサ距離測定部と、
   前記レーザセンサの測定値と複数のパーティクルに基づいて、自己位置を推定する自己位置推定演算部と、
   前記複数のパーティクルの存在分布半径に応じて、前記自己位置推定演算部における複数のパーティクルのパーティクル数を算出するパーティクル数演算部と、
   エンコーダの出力からの積分による移動量と、過去のパーティクルと、から現在の自己位置を推定するエンコーダ積算演算部と、
   前記自己位置推定演算部における複数のパーティクルのうち、前記レーザセンサの測定値に最もマッチしたパーティクルと、前記エンコーダ積算部による自己位置と、に基づいて、自己位置の補正量を算出する補正値演算部と、
   前記自己位置の補正量に基づいて、パーティクルの動作をガウス分布で近似した場合の、当該ガウス分布のパラメータである平均及び標準偏差を算出するガウス分布パラメータ演算部と、
   算出した前記ガウス分布のパラメータにより規定され、前記自己位置推定演算部における複数のパーティクルを動作させるために使用される運動モデルを算出する運動モデル演算部と、
   を備える移動体。
2. 環境を自律的に移動する移動体の位置推定方法であって、
   レーザセンサを用いて障害物までの距離を測定するステップと、
   前記レーザセンサの測定値と複数のパーティクルに基づいて、自己位置を推定するステップと、
   前記複数のパーティクルの存在分布半径に応じて、前記複数のパーティクルのパーティクル数を算出するステップと、
   エンコーダの出力からの積分による移動量と、過去のパーティクルと、から現在の自己位置を推定するステップと、
   前記複数のパーティクルのうち、前記レーザセンサの測定値に最もマッチしたパーティクルと、前記エンコーダ積算による自己位置と、に基づいて、自己位置の補正量を算出するステップと、
   前記自己位置の補正量に基づいて、パーティクルの動作をガウス分布で近似した場合の、当該ガウス分布のパラメータである平均及び標準偏差を算出するステップと、
   算出した前記ガウス分布のパラメータにより規定され、前記複数のパーティクルを動作させるために使用される運動モデルを算出するステップと、
   を有する位置推定方法。

Images