位置推定装置
【課題】自己位置の推定の際に使用する距離計測値の精度を向上した位置推定装置を提供する。
【解決手段】移動装置１に搭載して当該移動装置１周囲の障害物Ｓまでの距離を計測する距離計測装置６Ａと、その距離計測装置６Ａによる計測値に基づき上記移動装置１の位置を推定する位置推定手段６Ｅと、を備える。距離計測装置６Ａによる計測結果のうち、距離測定時における移動装置１の姿勢に対してロバストな計測値を選択して、位置推定手段６Ｅで使用する計測値とする。
【解決手段】移動装置１に搭載して当該移動装置１周囲の障害物Ｓまでの距離を計測する距離計測装置６Ａと、その距離計測装置６Ａによる計測値に基づき上記移動装置１の位置を推定する位置推定手段６Ｅと、を備える。距離計測装置６Ａによる計測結果のうち、距離測定時における移動装置１の姿勢に対してロバストな計測値を選択して、位置推定手段６Ｅで使用する計測値とする。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動装置周囲の障害物との距離に基づき、移動装置の位置を推定する位置推定装置に関する。
【背景技術】
【０００２】
特許文献１には、障害物を回避して目的地まで移動することが可能な自律移動装置の技術が記載してある。この技術では、障害物及び自己位置を認識するための情報を得る環境情報取得手段と、走行を行うための走行手段とを備える。そして、上記環境情報取得手段で得た情報と、予め記憶した地図情報とに基づいて、自己位置認識手段が自己位置を推定する。また、自己位置認識手段により自己位置を推定しつつ、障害物を回避しながら、上記走行手段を制御して、生成した走行経路に沿って走行する。
【０００３】
ここで、上記環境情報取得手段は、距離計測装置により障害物までの距離を計測する。また、撮像装置によって画像を取得する。
そして、距離計測装置により移動装置周囲の障害物までの距離を計測し、上記画像によって計測対象を推定する。続けて、その計測対象の推定結果に基づき行動計画を立てる。
【特許文献１】特許第３８７９８４８号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
移動装置に所定以上のピッチ動作が発生している状態で、障害物までの距離の計測を行った場合には、次のような問題がある。すなわち、ピッチ動作によりノーズダイブすると、前方の障害物までの距離を計測するはずの計測結果の大部分が、走行する床までの距離を計測してしまうおそれがある。そして、床面までの距離を障害物までの距離計測値と誤認識すると、距離計測に基づく自己位置の推定結果に悪影響が発生する。
本発明は、上記のような点に着目したもので、自己位置の推定の際に使用する距離計測値の精度を向上した位置推定装置を提供することを課題としている。
【課題を解決するための手段】
【０００５】
上記課題を解決するために、本発明は、移動装置に搭載して当該移動装置周囲の障害物までの距離を計測する距離計測装置と、その距離計測装置による計測値に基づき上記移動装置の位置を推定する位置推定手段と、を備える位置推定装置である。距離計測装置による複数の計測値のうち、距離測定時における移動装置の姿勢に対してロバストな計測値を選択して、位置推定手段で使用する計測値とする。
【発明の効果】
【０００６】
本発明によれば、移動装置の姿勢変化を考慮することで、例えば移動装置の姿勢に基づき床面までの距離を計測している可能性のある計測結果を使用しないように出来る。これによって、自己位置の推定に使用する距離計測値の精度が向上する。さらには、移動装置の姿勢による位置の誤推定を防ぐことができる。
なお、位置推定に使用する計測値を選択することで、位置推定に使用する計測値の数を制限することが可能となる。
【発明を実施するための最良の形態】
【０００７】
次に、本発明の実施形態について図面を参照しつつ説明する。
図１は、本実施形態の移動装置を示す模式的な斜視図である。図３は、本実施形態に係る移動装置１、及び位置推定装置の構成を示す図である。
（構成）
移動装置１は、図１に示すように、移動装置本体２、車輪３、及びアクチュエータ４を備える。移動装置本体２の左右にそれぞれ左右の車輪３を配置する。アクチュエータ４は、例えばモータから構成する。そして、左右の車輪３を、アクチュエータ４からの駆動トルクで駆動する。
【０００８】
ここで本実施形態の位置推定装置６は、所定の床に沿って移動する、ロボットを含む様々な移動装置１に適用可能である。ただし、本実施形態の移動装置１は、上述のように、２つの車輪３を移動装置本体２の左右に配置し、駆動制御によりバランスをとりながら移動および姿勢変化を行うものを例示して説明する。この２輪形態の移動装置１は、１輪の移動装置に比べて駆動制御が容易である。また、３輪以上の移動装置と比較すると、上記２輪形態の構成は、構成が単純で、且つ行動変化時の重心移動量が少ない。また、容易にその動きをモデル化できるという利点を持つ。
【０００９】
本実施形態の移動装置１を力学的にモデル化すると、図２のようになる。
そして、この関係を示す運動方程式は、下記の式（１）〜式（４）のように表すことが出来る。
Ｊｄ2／ｄｔ2γ ＝ ＶＬｓｉｎγ −ＨＬｃｏｎγ ・・・（１）
ｍｄ2／ｄｔ2（Ｌｃｏｎγ） ＝ Ｖ− ｍｇ ・・・（２）
ｕ ＝ Ｈ ・・・（３）
Ｊ ＝ ｍＬ2／３ ・・・（４）
ここで、
Ｊ：慣性モーメント
θ：移動装置１の移動装置本体２の傾き（ピッチ角γ）
Ｖ：移動装置１の重心に対し垂直方向に掛かる力
Ｈ：移動装置１の重心に対し水平方向に掛かる力
ｍ：移動装置本体２の重さ
Ｌ：移動装置本体２の重心位置から車輪３中心までの距離
ｕ：車輪３で発生する力
を示す。
この（１）式〜（４）式のようなモデルによって、走行、及び、所定のピッチ角γの姿勢とするための、適切な制御出力ｕを求めることが出来る。
【００１０】
また、姿勢計測装置６Ｂ及び姿勢制御装置５を備える。
姿勢計測装置６Ｂは、移動装置本体２の姿勢を計測する。姿勢計測装置６Ｂは、例えばジャイロセンサーやＧセンサなどを用いた姿勢計測装置６Ｂで構成して、移動装置本体２の姿勢（ピッチ角）を検出すればよい。姿勢計測装置６Ｂは、姿勢検出情報を、姿勢制御装置５、及び位置推定装置６の計測結果選択手段６Ｄに出力する。
【００１１】
姿勢制御装置５は、上記移動装置１の姿勢計測結果を入力とし、アクチュエータ４を制御することで、移動装置１の移動、及び移動装置１の姿勢を目的の姿勢状態に保持する。例えば、この姿勢制御装置５によって、移動装置１は、予め設定した地図内の走行路（床）に沿って自律走行する。なお、本実施形態の移動装置１は、図１のように、静的に不安定な機構となっている。したがって、その場での停止時や移動時においても、常に重心が車軸の上に来るように制御する必要がある。
【００１２】
また、位置推定装置６は、図２に示すように、距離計測装置６Ａ、上述の姿勢計測装置６Ｂ、事前位置推定手段６Ｃ、計測結果選択手段６Ｄ、及び位置推定手段６Ｅを備える。
距離計測装置６Ａ及び姿勢計測装置６Ｂは、移動装置本体２に搭載する。
距離計測装置６Ａは、移動装置本体２に搭載し移動装置１周囲の障害物Ｓまでの距離を計測する。すなわち、水平方向に走査するようにスキャンする。距離計測装置６Ａとしては、レーザー、ミリ波、超音波、静電容量を計測する装置、ステレオカメラなどを利用した計測装置を使用することが可能である。ここでは、距離計測装置６Ａとして、レーザーレンジファインダー（ＬＲＦ）を使用する。即ち、レーザーを２次元的に投光することで周囲の物体までの距離を計測する。例えば、レーザーを反射するミラーを回転して水平スキャンを行う。これによって、水平方向に走査することで、例えば１００本以上の距離計測の計測情報を短時間に取得する。そして、距離計測装置６Ａは、取得した複数の計測情報を計測結果選択手段６Ｄに出力する。
【００１３】
事前位置推定手段６Ｃは、前回までの位置情報から現在の移動装置１の確率的な位置推定である事前推定値を求める。前回までの位置情報とは、走行開始時の初期位置情報、車輪３の回転から計算したデッドレコニングの情報などがある。
すなわち、事前位置推定手段６Ｃは、最新の距離計測結果を使用しない位置推定である。例えば、入力として、車輪３の回転から計算するデッドレコニング、過去の位置推定結果、初期位置情報、自己位置に関する事前知識のいずれか、もしくはすべてを用いて位置を推定する。なお、計測開始時は過去に得た距離計測結果が無いため、初期位置情報に基づいて自己位置を推定することとなる。そして、事前位置推定手段６Ｃは、その位置に関する確率的な位置の情報を求める。すなわち位置推定に係る期待値とその分散を求めて出力する。期待値や分散を推定する方法としては、カルマンフィルタをベースとしたパラメトリックなフィルタや、パーティクルフィルタなどのノンパラメトリックなフィルタなどがある。ただし、これらの手法に限定するものではない。
【００１４】
計測結果選択手段６Ｄは、上記距離計測装置６Ａと上記姿勢計測装置６Ｂと上記事前位置推定手段６Ｃとからの入力に基づき、上記距離計測装置６Ａによる計測結果の一部を選択する。その選択した計測情報は、位置推定手段６Ｅに出力する。上記選択の処理については後述する。
位置推定手段６Ｅは、計測結果選択手段６Ｄで選択した計測値に基づき、移動装置１の現在の位置を推定する。すなわち、位置推定装置６は、上記計測結果選択装置の出力から、現在の移動装置１の位置を推定する装置である。例えば、計測結果選択装置で選択したロバストな計測結果に基づき障害物Ｓからの距離を特定し、その特定と、事前位置推定手段６Ｃとからの入力とから、地図上の現在の位置を推定する。位置推定装置６は、出力として、位置推定の期待値を出力する。推定のための手法は、事前位置推定手段６Ｃと同様に、カルマンフィルタをベースとしたパラメトリックなフィルタや、パーティクルフィルタなどのノンパラメトリックなフィルタなどがある。もっとも、他の公知の推定手法を適用しても良い。
【００１５】
次に、上記位置推定装置６の処理を、図３を参照して説明する。
まずステップＳ１００において、事前位置推定手段６Ｃが、上記移動装置１の事前位置推定を実施する。位置を推定する手法としては、前述したようにカルマンフィルタやパーティクルフィルタなど多くの手法がある。ここではパーティクルフィルタを位置検出に適用した手法として公知のモンテカルロローカライゼーションＭＣＬを用いることとする。ただし、別の手法を使用しても良い。
【００１６】
次に、ステップＳ１１０において、上記距離計測装置６Ａで水平スキャンを行う。すなわち、上記移動装置１の周囲の障害物Ｓまでの距離を複数点計測する。これによって、例えば１００本の距離計測結果を得る。ここで、距離計測装置６Ａは、移動装置本体２と床面が平行である場合、つまりピッチ動作が発生していない場合には、水平方向をスキャンした計測結果を得る。ただし、ピッチ動作によって移動装置１の姿勢が変化しノーズダイブした場合、距離計測装置６Ａの照射方向が傾く。このため、同じ環境で距離計測を実施してもピッチ角の発生で異なる場合がある。例えば、ピッチ角がゼロでの計測結果が図５（ａ）とする。同じ環境で所定のピッチ角が発生している状態での距離計測は、例えば図５（ｂ）のようになる。このように距離計測結果が、異なる。なお、図５中、矢印の方向にピッチ動作が発生し、ノーズダイブした場合とする。そして、図５（ｂ）の計測結果を全て使用して自己位置推定を行った場合、移動装置１の前方に壁があるように見える、このことは、自己位置推定を大きく誤らせる原因となる。
【００１７】
次に、ステップＳ１２０では、姿勢計測装置６Ｂによる計測から、距離計測時の姿勢を検出する。本実施形態では、移動装置本体２における、距離計測時の床面に対する角度姿勢（ピッチ角γ）を取得する。ここで、上記ステップＳ１００〜ステップＳ１２０の処理は、同期を取って同時期に実施しても良い。
次に、ステップＳ１３０では、計測結果選択手段６Ｄが、ピッチ方向の姿勢変化にロバストである計測結果を選択する。詳細な処理は、後述する。
ここで、２次元を移動する物体の場合、物体の自由度は、位置と姿勢の「３」である。従って、位置を示す２変数ｘ、ｙと、姿勢を示す１変数θで、移動装置１を表現することが可能である。
【００１８】
そして、計測結果選択手段６Ｄは、上記距離計測装置６Ａから取得した計測結果Ｌk：（ｋ＝｛０〜Ｎ｝）から、図７のような座標系において、ロバストに推定するための計測結果Ｌkを選択する。「ロバストに推定するための計測結果Ｌk」は、距離計測時の姿勢変化であるピッチ角γに対して、移動装置１の場所及び方向（ｘ、ｙ、θ）を特定する際にロバストと推定する計測結果である。ここで、Ｌkを計測するための照射ビームと、ｘ軸との角度を、βと表す。
ここで、上記計測結果の選択を定式化しておく。それぞれの照射ビームによる計測結果Ｌkは、移動物体の周囲地図ｆｍ（）に対する、移動物体の位置Ｐ（ｘ、ｙ、θ）とピッチ方向の傾きγとにより、下記式のように表すことが出来る。
Ｌk ＝ ｆｍ（ｘ、ｙ、θ、γ） ・・・（５）
この（５）式を全微分すると、下記（６）式を得る。
【００１９】
【数１】
【００２０】
ここで、ｘについて位置を推定するのにふさわしい照射ビーム（ロバストなビーム）は、ｘ以外の変数について影響を受けない照射ビームによる計測結果、つまり
【００２１】
【数２】
であることが望ましい。
ｙ、θについても、同様のことがいえる。
したがって、位置推定（ｘ、ｙ、θ）を行うのに適切な計測結果は、それぞれの変数ｘ、ｙ、θに対して、
【００２２】
【数３】
となる照射ビームを選択する。
【００２３】
ここで、（７）式〜（９）式を使用した、照射ビームを選択するための閾値を決定するための基準は、次の通りである。
すなわち、距離計測装置６Ａによる計測値の選択基準は、位置推定が目的とされる精度で行えるか否かにより、これは目標とされる地図作成精度、もしくは位置推定精度により決定される。例えば、ここでは、０．１ｍとする。
ただし、地図作成及び位置推定の誤差は累積するものなので、地図全体を作成したときの誤差でこれを評価する必要がある。１０ｍ四方の地図を作成するときに、１ｍに一回計測を行なう場合、最悪、誤差×10のゆがみが発生することになる。これを加味し要求精度を、上記例の場合には、０．０１ｍとする。このように、閾値は、地図全体を作成したときの誤差で評価すればよい。
【００２４】
次に、ステップＳ１４０では、ステップＳ１３０で選択した計測値の結果を元に、位置推定を実施する。ただし、ステップＳ１３０にて、一部の変数について適切な計測結果が選択出来ない場合には、今回の処理では位置の推定を行わない。この理由は、次のためである。本実施形態では、少ない計測結果で位置推定を行う。このため、一つの変数に対応するロバストな計測値を選択出来ない場合には、精度誤差が大きくなる。従って、そのときは位置推定をやらないで次回に移行する。ここで、位置推定値はステップＳ１３０と同様ＭＣＬを用いて実施する。ただし、これに限定するものではなく、他の様々な手法が利用可能である。
そして、ステップＳ１５０にて、例えば、地図上の所定経路を走行するなど、終了条件を満足したか否かを判定し、終了条件を満足した場合には処理を終了する。一方、終了条件を満足していない場合には、移動装置１を移動して、ステップＳ１００に戻って、上記処理を繰り返す。
【００２５】
次に、ステップＳ１３０の処理である、計測結果の選択について、図６を参照して説明する。
まず、ステップＳ２００にて、距離計測時のピッチ角γに対する、
（∂ｆｍ／∂γ）ｄｒを計算する。
ここで、距離計測装置６Ａは移動装置１の移動装置本体２に水平に取り付けてある。このため、移動装置１にピッチ方向の姿勢変化が発生すると、距離計測装置６Ａの計測方向（走査方向に沿った面）は、走行する床面に対し傾いた状態となる。実際にピッチ方向にノーズダイブが発生した場合は、図５（ｂ）で確認したように、計測結果の一部は、周辺障害物Ｓではなく、床面までの距離を計測するおそれがある。
【００２６】
この特徴は、図８のＡ２，Ａ３のように、進行方向正面方向（ピッチング方向）から角度が離れているところでは発生せず、移動装置１の進行方向正面に近づけば近づくほど（Ａ１の領域等）、影響が大きくなる関数となる。
正面方向から角度が離れているところでは、ピッチ変化にロバスト性があり、下記式、若しくは下記式に近い関係となっている。
（∂ｆｍ／∂γ） ＝０
ここでは、距離計測装置６Ａの取付け位置、及び照射ビームの照射角度βから、（∂ｆ／∂γ）を各照射ビームに対して計算する。そして、計測したｄγとの積を計算する。
続いて、ステップＳ２１０では、（∂ｆｍ／∂θ）ｄθ、（∂ｆｍ／∂ｙ）ｄｙを計算する。
【００２７】
具体的には、まず隣合う照射ビーム同士について計測結果を比較し、各計測結果について、下記式に基づきΔＬを計算する。
ΔＬ＝ Ｌk-l − Ｌk+1
これは計測値Ｌkのθ方向に対する変化であるので、（∂ｆｍ／∂θ）を表しているといえる。
ここで、（∂ｆｍ／∂θ）が小さい領域について注目し、図９（ａ）→（ｂ）のように、時計回りにβ分回転させたｘｙ座標を考えると、ｙ成分による変化量（∂ｆｍ／∂ｙ）も小さいことが確認出来る（図９を参照）。
また、ｄθは、自己位置の推定値におけるθのばらつきに依存する。ここで、本実施形態では、事前位置推定により自己位置推定値の期待値と分散を求めているので、θの自己位置推定の分散をｄθとして利用する。以降ｄｘ、ｄｙについても同様に事前推定値の分散から求める。
【００２８】
これらの結果から、（∂ｆｍ／∂θ）＝（∂ｆｍ／∂ｙ）に対して、事前推定値のばらつきから求めたｄθ、ｄｙを掛け合わせた値から、
｛∂ｆｍ／∂θ）ｄθ｝、｛（∂ｆｍ／∂ｙ）ｄｙ｝を求める。
ステップＳ２３０では、ステップＳ２００及びステップＳ２１０で計算した（∂ｆｍ／∂γ）ｄγ、（∂ｆｍ／∂θ）ｄθ、（∂ｆｍ／∂ｙ）ｄｙが、所定の値よりも小さい計測結果Ｌkf1を、全計測結果から１つ選択する。したがって、この計測結果は、（７）式を満たすロバストな計測結果とする。
【００２９】
次に、Ｌkf1に対して、直角方向に照射され、なおかつ、（∂ｆｍ／∂γ）ｄγ、（∂ｆｍ／∂θ）ｄθが所定の値以下であるＬkf2を選択する。この値は、（∂ｆｍ／∂θ）ｄθが十分小さく、Ｌkf1に対して直角方向であるため、（∂ｆｍ／∂ｘ）ｄｘが十分小さい照射ビームによる計測結果であるといえる。
したがって、この計測結果は、（８）式を満たすロバストな計測結果とする。
ステップＳ２４０では、上記ステップＳ２１０及びステップＳ２２０の処理とは、独立に、ΔＬkが所定のよりも大きく、（∂ｆｍ／∂γ）ｄγが所定の値よりも小さい照射ビームによる計測結果Ｌkf3を選択する。
【００３０】
このステップＳ２４０では、処理量を少なくするために、厳密な意味で、（∂ｆｍ／∂ｘ）ｄｘ、（∂ｆｍ／∂γ）ｄγを評価していない。もし厳密にこれらを評価する場合は、候補となる計測結果のすべてに対して座標変換を行い、確認を行う必要がある。しかし、この手法は計算時間の短縮も視野に入れているため、実施していない。ただし、厳密に評価しても良い。
以上の処理により得たＬkf1、Ｌkf2、Ｌkf3は、それぞれｘ、ｙ、θが他の変数やピッチ角γに対して独立に計測精度が保持出来る、つまりロバストな計測結果である。
もしこれらの結果が得ることが出来ない場合は、正確な推定が行うことができないと考えることができるため、その時間ステップにおける位置推定を反映させない。
ここで、姿勢計測装置６Ｂは、姿勢検出手段を構成する。
【００３１】
（動作・作用）
所定時間毎に、移動装置１の位置推定を行う。
位置推定の処理は、まず、事前位置推定手段６Ｃが、上記移動装置１の事前位置推定を実施する。推定は、公知のモンテカルロローカライゼーションＭＣＬ等によって実施する。また、距離計測装置６Ａで移動装置１周囲の水平スキャンを行う。すなわち、上記移動装置１の周囲の障害物Ｓまでの距離を複数点計測する。また、姿勢計測装置６Ｂによる計測から、距離計測時の姿勢を検出する。
【００３２】
次に、上記推定及び計測結果に基づき、計測結果選択手段６Ｄが、ピッチ方向の姿勢変化にロバストである計測結果を選択する。ここで、２次元を移動する物体、つまり本実施形態の移動装置１の場合には、その自由度は３である。従って、位置を示す２変数ｘ、ｙと、姿勢を示す１変数θで、移動装置１を表現することが可能である。これに基づき、上記距離計測装置６Ａから取得した計測結果Ｌk：（ｋ＝｛０〜Ｎ｝）から、図７のような座標系において、ロバストに推定するための計測結果Ｌkを選択する。本実施形態では、事前位置推定手段６Ｃの事前推定値のバラツキにもロバストな計測値を選択する。
続いて、選択した計測値の結果を元に、位置推定を実施する。
【００３３】
（本実施形態の効果）
（１）移動装置１の姿勢を計測した結果を元に、床面までの距離を計測している計測結果を利用しない。これによって、移動装置１の姿勢次第で床面までの距離を計測する可能性がある計測結果であっても、ロバストな計測値だけを選択して位置推定に使用することで、自己位置の推定に使用する距離計測値の精度が向上する。さらには、移動装置１の姿勢による位置の誤推定を防ぐことができる。
なお、位置推定に使用する計測値を選択することで、位置推定に使用する計測値の数を制限することが可能となる。
（２）移動装置１に発生している実際の姿勢を検出する。これによって、距離計測時の姿勢を検出可能となる。
（３）事前推定値の確からしさを利用する。これによって、事前推定値による誤差要因に応じて、距離計測結果を選択することができる。例えば、事前推定値の確からしさが高い場合、計測結果の誤差要因が低減するため、姿勢変化に対する誤差要因に対する許容度を上げることが可能となる。
【００３４】
（変形例）
（１）上記実施形態では、事前位置推定手段６Ｃによる事前推定値のバラツキによって、選択する計測値を求めている。これに代えて、事前推定値のバラツキを固定にして使用しても良い。この場合には、ｄｘ、ｄｙ、ｄθとして固定値を設定すればよい。例えば、図８におけるＡ１領域からの計測結果を、無条件に選択対象外とすることで、ロバストな計測値を選択する。
この実施例によれば移動装置１の姿勢変化を考慮し、移動装置１の姿勢により床面までの距離を計測している可能性のある計測結果は、最初から利用しない。これによって、移動装置１の姿勢による位置の誤推定を防ぐことができる。
【００３５】
（２）上記実施形態の姿勢検出手段では、移動装置本体２の実際の姿勢をセンサで検出する姿勢計測装置６Ｂで構成した。これに代えて、姿勢検出手段を、次のように構成しても良い。
すなわち、姿勢検出手段は、姿勢制御装置５の出力から、現在の姿勢の推定を行う。本実施形態では、移動装置本体２における、距離計測時の床面に対する角度姿勢（ピッチ角γ）を取得する。
上記姿勢の推定は、上記（１）式〜（４）式から、制御出力ｕが分かれば、Δｔ後の姿勢θを推定することが可能となる。従って、姿勢の履歴を取ることで、距離計測時の姿勢を推定可能である。
【００３６】
この場合には、姿勢計測装置６Ｂによる計測が利用不可能であるか、もしくは利用できても姿勢変化速度が姿勢計測装置６Ｂの応答速度に対して速すぎる場合であっても、距離計測時の姿勢を取得することが可能となる。
なお、姿勢計測装置６Ｂと、この推定による姿勢検出とを併用しても良い。
すなわち、姿勢計測装置６Ｂが利用可能であり、また姿勢計測装置６Ｂの応答速度が姿勢変化速度に対して十分速ければ、姿勢計測装置６Ｂの値をそのまま利用する。これに対して、姿勢計測装置６Ｂによる計測が利用不可能であるか、もしくは利用できても姿勢変化速度が姿勢計測装置６Ｂの応答速度に対して速すぎる場合は、姿勢制御装置５の出力から、現在の姿勢の推定を行う。
なお、姿勢計測装置６Ｂからの計測値の履歴から、距離計測時の姿勢を推定するようにしても良い。
【００３７】
（３）また、距離計測時における移動装置１の姿勢変化（ピッチ角γ）が所定以上の場合にだけ、上記位置推定方法を提供するようにしても良い。すなわち、距離計測時における移動装置１の姿勢変化が所定未満の場合には、距離計測装置６Ａによる計測結果をそのまま使用して位置の推定を行うようにしても良い。
【図面の簡単な説明】
【００３８】
【図１】本発明に基づく実施形態に係る移動装置を示す模式的斜視図である。
【図２】本発明に基づく実施形態に係る移動装置を示す倒立振子モデルを表す図である。
【図３】本発明に基づく実施形態に係る位置推定装置を示すブロック図である。
【図４】本発明に基づく実施形態に係る位置推定装置の処理を示す図である。
【図５】移動装置による計測結果の例を示す上面図である。
【図６】本発明に基づく実施形態に係る計測値の選択に係る表記方法を示す図である。
【図７】計測結果選択手段の処理を説明する図である。
【図８】（∂ｆｍ／∂θ）の特徴を示す平面図である。
【図９】ｘ−ｙ座標の変換を示す図である。
【符号の説明】
【００３９】
１ 移動装置
２ 移動装置本体
３ 車輪
４ アクチュエータ
５ 姿勢制御装置
６ 位置推定装置
６Ａ 距離計測装置
６Ｂ 姿勢計測装置
６Ｃ 事前位置推定手段
６Ｄ 計測結果選択手段
６Ｅ 位置推定手段
ｆｍ（） 周囲地図
Ｌk 計測結果
Ｓ 障害物
ｕ 制御出力
ｘ、ｙ 変数
β 照射角度
γ ピッチ角
θ 変数
【技術分野】
【０００１】
本発明は、移動装置周囲の障害物との距離に基づき、移動装置の位置を推定する位置推定装置に関する。
【背景技術】
【０００２】
特許文献１には、障害物を回避して目的地まで移動することが可能な自律移動装置の技術が記載してある。この技術では、障害物及び自己位置を認識するための情報を得る環境情報取得手段と、走行を行うための走行手段とを備える。そして、上記環境情報取得手段で得た情報と、予め記憶した地図情報とに基づいて、自己位置認識手段が自己位置を推定する。また、自己位置認識手段により自己位置を推定しつつ、障害物を回避しながら、上記走行手段を制御して、生成した走行経路に沿って走行する。
【０００３】
ここで、上記環境情報取得手段は、距離計測装置により障害物までの距離を計測する。また、撮像装置によって画像を取得する。
そして、距離計測装置により移動装置周囲の障害物までの距離を計測し、上記画像によって計測対象を推定する。続けて、その計測対象の推定結果に基づき行動計画を立てる。
【特許文献１】特許第３８７９８４８号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
移動装置に所定以上のピッチ動作が発生している状態で、障害物までの距離の計測を行った場合には、次のような問題がある。すなわち、ピッチ動作によりノーズダイブすると、前方の障害物までの距離を計測するはずの計測結果の大部分が、走行する床までの距離を計測してしまうおそれがある。そして、床面までの距離を障害物までの距離計測値と誤認識すると、距離計測に基づく自己位置の推定結果に悪影響が発生する。
本発明は、上記のような点に着目したもので、自己位置の推定の際に使用する距離計測値の精度を向上した位置推定装置を提供することを課題としている。
【課題を解決するための手段】
【０００５】
上記課題を解決するために、本発明は、移動装置に搭載して当該移動装置周囲の障害物までの距離を計測する距離計測装置と、その距離計測装置による計測値に基づき上記移動装置の位置を推定する位置推定手段と、を備える位置推定装置である。距離計測装置による複数の計測値のうち、距離測定時における移動装置の姿勢に対してロバストな計測値を選択して、位置推定手段で使用する計測値とする。
【発明の効果】
【０００６】
本発明によれば、移動装置の姿勢変化を考慮することで、例えば移動装置の姿勢に基づき床面までの距離を計測している可能性のある計測結果を使用しないように出来る。これによって、自己位置の推定に使用する距離計測値の精度が向上する。さらには、移動装置の姿勢による位置の誤推定を防ぐことができる。
なお、位置推定に使用する計測値を選択することで、位置推定に使用する計測値の数を制限することが可能となる。
【発明を実施するための最良の形態】
【０００７】
次に、本発明の実施形態について図面を参照しつつ説明する。
図１は、本実施形態の移動装置を示す模式的な斜視図である。図３は、本実施形態に係る移動装置１、及び位置推定装置の構成を示す図である。
（構成）
移動装置１は、図１に示すように、移動装置本体２、車輪３、及びアクチュエータ４を備える。移動装置本体２の左右にそれぞれ左右の車輪３を配置する。アクチュエータ４は、例えばモータから構成する。そして、左右の車輪３を、アクチュエータ４からの駆動トルクで駆動する。
【０００８】
ここで本実施形態の位置推定装置６は、所定の床に沿って移動する、ロボットを含む様々な移動装置１に適用可能である。ただし、本実施形態の移動装置１は、上述のように、２つの車輪３を移動装置本体２の左右に配置し、駆動制御によりバランスをとりながら移動および姿勢変化を行うものを例示して説明する。この２輪形態の移動装置１は、１輪の移動装置に比べて駆動制御が容易である。また、３輪以上の移動装置と比較すると、上記２輪形態の構成は、構成が単純で、且つ行動変化時の重心移動量が少ない。また、容易にその動きをモデル化できるという利点を持つ。
【０００９】
本実施形態の移動装置１を力学的にモデル化すると、図２のようになる。
そして、この関係を示す運動方程式は、下記の式（１）〜式（４）のように表すことが出来る。
Ｊｄ2／ｄｔ2γ ＝ ＶＬｓｉｎγ −ＨＬｃｏｎγ ・・・（１）
ｍｄ2／ｄｔ2（Ｌｃｏｎγ） ＝ Ｖ− ｍｇ ・・・（２）
ｕ ＝ Ｈ ・・・（３）
Ｊ ＝ ｍＬ2／３ ・・・（４）
ここで、
Ｊ：慣性モーメント
θ：移動装置１の移動装置本体２の傾き（ピッチ角γ）
Ｖ：移動装置１の重心に対し垂直方向に掛かる力
Ｈ：移動装置１の重心に対し水平方向に掛かる力
ｍ：移動装置本体２の重さ
Ｌ：移動装置本体２の重心位置から車輪３中心までの距離
ｕ：車輪３で発生する力
を示す。
この（１）式〜（４）式のようなモデルによって、走行、及び、所定のピッチ角γの姿勢とするための、適切な制御出力ｕを求めることが出来る。
【００１０】
また、姿勢計測装置６Ｂ及び姿勢制御装置５を備える。
姿勢計測装置６Ｂは、移動装置本体２の姿勢を計測する。姿勢計測装置６Ｂは、例えばジャイロセンサーやＧセンサなどを用いた姿勢計測装置６Ｂで構成して、移動装置本体２の姿勢（ピッチ角）を検出すればよい。姿勢計測装置６Ｂは、姿勢検出情報を、姿勢制御装置５、及び位置推定装置６の計測結果選択手段６Ｄに出力する。
【００１１】
姿勢制御装置５は、上記移動装置１の姿勢計測結果を入力とし、アクチュエータ４を制御することで、移動装置１の移動、及び移動装置１の姿勢を目的の姿勢状態に保持する。例えば、この姿勢制御装置５によって、移動装置１は、予め設定した地図内の走行路（床）に沿って自律走行する。なお、本実施形態の移動装置１は、図１のように、静的に不安定な機構となっている。したがって、その場での停止時や移動時においても、常に重心が車軸の上に来るように制御する必要がある。
【００１２】
また、位置推定装置６は、図２に示すように、距離計測装置６Ａ、上述の姿勢計測装置６Ｂ、事前位置推定手段６Ｃ、計測結果選択手段６Ｄ、及び位置推定手段６Ｅを備える。
距離計測装置６Ａ及び姿勢計測装置６Ｂは、移動装置本体２に搭載する。
距離計測装置６Ａは、移動装置本体２に搭載し移動装置１周囲の障害物Ｓまでの距離を計測する。すなわち、水平方向に走査するようにスキャンする。距離計測装置６Ａとしては、レーザー、ミリ波、超音波、静電容量を計測する装置、ステレオカメラなどを利用した計測装置を使用することが可能である。ここでは、距離計測装置６Ａとして、レーザーレンジファインダー（ＬＲＦ）を使用する。即ち、レーザーを２次元的に投光することで周囲の物体までの距離を計測する。例えば、レーザーを反射するミラーを回転して水平スキャンを行う。これによって、水平方向に走査することで、例えば１００本以上の距離計測の計測情報を短時間に取得する。そして、距離計測装置６Ａは、取得した複数の計測情報を計測結果選択手段６Ｄに出力する。
【００１３】
事前位置推定手段６Ｃは、前回までの位置情報から現在の移動装置１の確率的な位置推定である事前推定値を求める。前回までの位置情報とは、走行開始時の初期位置情報、車輪３の回転から計算したデッドレコニングの情報などがある。
すなわち、事前位置推定手段６Ｃは、最新の距離計測結果を使用しない位置推定である。例えば、入力として、車輪３の回転から計算するデッドレコニング、過去の位置推定結果、初期位置情報、自己位置に関する事前知識のいずれか、もしくはすべてを用いて位置を推定する。なお、計測開始時は過去に得た距離計測結果が無いため、初期位置情報に基づいて自己位置を推定することとなる。そして、事前位置推定手段６Ｃは、その位置に関する確率的な位置の情報を求める。すなわち位置推定に係る期待値とその分散を求めて出力する。期待値や分散を推定する方法としては、カルマンフィルタをベースとしたパラメトリックなフィルタや、パーティクルフィルタなどのノンパラメトリックなフィルタなどがある。ただし、これらの手法に限定するものではない。
【００１４】
計測結果選択手段６Ｄは、上記距離計測装置６Ａと上記姿勢計測装置６Ｂと上記事前位置推定手段６Ｃとからの入力に基づき、上記距離計測装置６Ａによる計測結果の一部を選択する。その選択した計測情報は、位置推定手段６Ｅに出力する。上記選択の処理については後述する。
位置推定手段６Ｅは、計測結果選択手段６Ｄで選択した計測値に基づき、移動装置１の現在の位置を推定する。すなわち、位置推定装置６は、上記計測結果選択装置の出力から、現在の移動装置１の位置を推定する装置である。例えば、計測結果選択装置で選択したロバストな計測結果に基づき障害物Ｓからの距離を特定し、その特定と、事前位置推定手段６Ｃとからの入力とから、地図上の現在の位置を推定する。位置推定装置６は、出力として、位置推定の期待値を出力する。推定のための手法は、事前位置推定手段６Ｃと同様に、カルマンフィルタをベースとしたパラメトリックなフィルタや、パーティクルフィルタなどのノンパラメトリックなフィルタなどがある。もっとも、他の公知の推定手法を適用しても良い。
【００１５】
次に、上記位置推定装置６の処理を、図３を参照して説明する。
まずステップＳ１００において、事前位置推定手段６Ｃが、上記移動装置１の事前位置推定を実施する。位置を推定する手法としては、前述したようにカルマンフィルタやパーティクルフィルタなど多くの手法がある。ここではパーティクルフィルタを位置検出に適用した手法として公知のモンテカルロローカライゼーションＭＣＬを用いることとする。ただし、別の手法を使用しても良い。
【００１６】
次に、ステップＳ１１０において、上記距離計測装置６Ａで水平スキャンを行う。すなわち、上記移動装置１の周囲の障害物Ｓまでの距離を複数点計測する。これによって、例えば１００本の距離計測結果を得る。ここで、距離計測装置６Ａは、移動装置本体２と床面が平行である場合、つまりピッチ動作が発生していない場合には、水平方向をスキャンした計測結果を得る。ただし、ピッチ動作によって移動装置１の姿勢が変化しノーズダイブした場合、距離計測装置６Ａの照射方向が傾く。このため、同じ環境で距離計測を実施してもピッチ角の発生で異なる場合がある。例えば、ピッチ角がゼロでの計測結果が図５（ａ）とする。同じ環境で所定のピッチ角が発生している状態での距離計測は、例えば図５（ｂ）のようになる。このように距離計測結果が、異なる。なお、図５中、矢印の方向にピッチ動作が発生し、ノーズダイブした場合とする。そして、図５（ｂ）の計測結果を全て使用して自己位置推定を行った場合、移動装置１の前方に壁があるように見える、このことは、自己位置推定を大きく誤らせる原因となる。
【００１７】
次に、ステップＳ１２０では、姿勢計測装置６Ｂによる計測から、距離計測時の姿勢を検出する。本実施形態では、移動装置本体２における、距離計測時の床面に対する角度姿勢（ピッチ角γ）を取得する。ここで、上記ステップＳ１００〜ステップＳ１２０の処理は、同期を取って同時期に実施しても良い。
次に、ステップＳ１３０では、計測結果選択手段６Ｄが、ピッチ方向の姿勢変化にロバストである計測結果を選択する。詳細な処理は、後述する。
ここで、２次元を移動する物体の場合、物体の自由度は、位置と姿勢の「３」である。従って、位置を示す２変数ｘ、ｙと、姿勢を示す１変数θで、移動装置１を表現することが可能である。
【００１８】
そして、計測結果選択手段６Ｄは、上記距離計測装置６Ａから取得した計測結果Ｌk：（ｋ＝｛０〜Ｎ｝）から、図７のような座標系において、ロバストに推定するための計測結果Ｌkを選択する。「ロバストに推定するための計測結果Ｌk」は、距離計測時の姿勢変化であるピッチ角γに対して、移動装置１の場所及び方向（ｘ、ｙ、θ）を特定する際にロバストと推定する計測結果である。ここで、Ｌkを計測するための照射ビームと、ｘ軸との角度を、βと表す。
ここで、上記計測結果の選択を定式化しておく。それぞれの照射ビームによる計測結果Ｌkは、移動物体の周囲地図ｆｍ（）に対する、移動物体の位置Ｐ（ｘ、ｙ、θ）とピッチ方向の傾きγとにより、下記式のように表すことが出来る。
Ｌk ＝ ｆｍ（ｘ、ｙ、θ、γ） ・・・（５）
この（５）式を全微分すると、下記（６）式を得る。
【００１９】
【数１】
【００２０】
ここで、ｘについて位置を推定するのにふさわしい照射ビーム（ロバストなビーム）は、ｘ以外の変数について影響を受けない照射ビームによる計測結果、つまり
【００２１】
【数２】
であることが望ましい。
ｙ、θについても、同様のことがいえる。
したがって、位置推定（ｘ、ｙ、θ）を行うのに適切な計測結果は、それぞれの変数ｘ、ｙ、θに対して、
【００２２】
【数３】
となる照射ビームを選択する。
【００２３】
ここで、（７）式〜（９）式を使用した、照射ビームを選択するための閾値を決定するための基準は、次の通りである。
すなわち、距離計測装置６Ａによる計測値の選択基準は、位置推定が目的とされる精度で行えるか否かにより、これは目標とされる地図作成精度、もしくは位置推定精度により決定される。例えば、ここでは、０．１ｍとする。
ただし、地図作成及び位置推定の誤差は累積するものなので、地図全体を作成したときの誤差でこれを評価する必要がある。１０ｍ四方の地図を作成するときに、１ｍに一回計測を行なう場合、最悪、誤差×10のゆがみが発生することになる。これを加味し要求精度を、上記例の場合には、０．０１ｍとする。このように、閾値は、地図全体を作成したときの誤差で評価すればよい。
【００２４】
次に、ステップＳ１４０では、ステップＳ１３０で選択した計測値の結果を元に、位置推定を実施する。ただし、ステップＳ１３０にて、一部の変数について適切な計測結果が選択出来ない場合には、今回の処理では位置の推定を行わない。この理由は、次のためである。本実施形態では、少ない計測結果で位置推定を行う。このため、一つの変数に対応するロバストな計測値を選択出来ない場合には、精度誤差が大きくなる。従って、そのときは位置推定をやらないで次回に移行する。ここで、位置推定値はステップＳ１３０と同様ＭＣＬを用いて実施する。ただし、これに限定するものではなく、他の様々な手法が利用可能である。
そして、ステップＳ１５０にて、例えば、地図上の所定経路を走行するなど、終了条件を満足したか否かを判定し、終了条件を満足した場合には処理を終了する。一方、終了条件を満足していない場合には、移動装置１を移動して、ステップＳ１００に戻って、上記処理を繰り返す。
【００２５】
次に、ステップＳ１３０の処理である、計測結果の選択について、図６を参照して説明する。
まず、ステップＳ２００にて、距離計測時のピッチ角γに対する、
（∂ｆｍ／∂γ）ｄｒを計算する。
ここで、距離計測装置６Ａは移動装置１の移動装置本体２に水平に取り付けてある。このため、移動装置１にピッチ方向の姿勢変化が発生すると、距離計測装置６Ａの計測方向（走査方向に沿った面）は、走行する床面に対し傾いた状態となる。実際にピッチ方向にノーズダイブが発生した場合は、図５（ｂ）で確認したように、計測結果の一部は、周辺障害物Ｓではなく、床面までの距離を計測するおそれがある。
【００２６】
この特徴は、図８のＡ２，Ａ３のように、進行方向正面方向（ピッチング方向）から角度が離れているところでは発生せず、移動装置１の進行方向正面に近づけば近づくほど（Ａ１の領域等）、影響が大きくなる関数となる。
正面方向から角度が離れているところでは、ピッチ変化にロバスト性があり、下記式、若しくは下記式に近い関係となっている。
（∂ｆｍ／∂γ） ＝０
ここでは、距離計測装置６Ａの取付け位置、及び照射ビームの照射角度βから、（∂ｆ／∂γ）を各照射ビームに対して計算する。そして、計測したｄγとの積を計算する。
続いて、ステップＳ２１０では、（∂ｆｍ／∂θ）ｄθ、（∂ｆｍ／∂ｙ）ｄｙを計算する。
【００２７】
具体的には、まず隣合う照射ビーム同士について計測結果を比較し、各計測結果について、下記式に基づきΔＬを計算する。
ΔＬ＝ Ｌk-l − Ｌk+1
これは計測値Ｌkのθ方向に対する変化であるので、（∂ｆｍ／∂θ）を表しているといえる。
ここで、（∂ｆｍ／∂θ）が小さい領域について注目し、図９（ａ）→（ｂ）のように、時計回りにβ分回転させたｘｙ座標を考えると、ｙ成分による変化量（∂ｆｍ／∂ｙ）も小さいことが確認出来る（図９を参照）。
また、ｄθは、自己位置の推定値におけるθのばらつきに依存する。ここで、本実施形態では、事前位置推定により自己位置推定値の期待値と分散を求めているので、θの自己位置推定の分散をｄθとして利用する。以降ｄｘ、ｄｙについても同様に事前推定値の分散から求める。
【００２８】
これらの結果から、（∂ｆｍ／∂θ）＝（∂ｆｍ／∂ｙ）に対して、事前推定値のばらつきから求めたｄθ、ｄｙを掛け合わせた値から、
｛∂ｆｍ／∂θ）ｄθ｝、｛（∂ｆｍ／∂ｙ）ｄｙ｝を求める。
ステップＳ２３０では、ステップＳ２００及びステップＳ２１０で計算した（∂ｆｍ／∂γ）ｄγ、（∂ｆｍ／∂θ）ｄθ、（∂ｆｍ／∂ｙ）ｄｙが、所定の値よりも小さい計測結果Ｌkf1を、全計測結果から１つ選択する。したがって、この計測結果は、（７）式を満たすロバストな計測結果とする。
【００２９】
次に、Ｌkf1に対して、直角方向に照射され、なおかつ、（∂ｆｍ／∂γ）ｄγ、（∂ｆｍ／∂θ）ｄθが所定の値以下であるＬkf2を選択する。この値は、（∂ｆｍ／∂θ）ｄθが十分小さく、Ｌkf1に対して直角方向であるため、（∂ｆｍ／∂ｘ）ｄｘが十分小さい照射ビームによる計測結果であるといえる。
したがって、この計測結果は、（８）式を満たすロバストな計測結果とする。
ステップＳ２４０では、上記ステップＳ２１０及びステップＳ２２０の処理とは、独立に、ΔＬkが所定のよりも大きく、（∂ｆｍ／∂γ）ｄγが所定の値よりも小さい照射ビームによる計測結果Ｌkf3を選択する。
【００３０】
このステップＳ２４０では、処理量を少なくするために、厳密な意味で、（∂ｆｍ／∂ｘ）ｄｘ、（∂ｆｍ／∂γ）ｄγを評価していない。もし厳密にこれらを評価する場合は、候補となる計測結果のすべてに対して座標変換を行い、確認を行う必要がある。しかし、この手法は計算時間の短縮も視野に入れているため、実施していない。ただし、厳密に評価しても良い。
以上の処理により得たＬkf1、Ｌkf2、Ｌkf3は、それぞれｘ、ｙ、θが他の変数やピッチ角γに対して独立に計測精度が保持出来る、つまりロバストな計測結果である。
もしこれらの結果が得ることが出来ない場合は、正確な推定が行うことができないと考えることができるため、その時間ステップにおける位置推定を反映させない。
ここで、姿勢計測装置６Ｂは、姿勢検出手段を構成する。
【００３１】
（動作・作用）
所定時間毎に、移動装置１の位置推定を行う。
位置推定の処理は、まず、事前位置推定手段６Ｃが、上記移動装置１の事前位置推定を実施する。推定は、公知のモンテカルロローカライゼーションＭＣＬ等によって実施する。また、距離計測装置６Ａで移動装置１周囲の水平スキャンを行う。すなわち、上記移動装置１の周囲の障害物Ｓまでの距離を複数点計測する。また、姿勢計測装置６Ｂによる計測から、距離計測時の姿勢を検出する。
【００３２】
次に、上記推定及び計測結果に基づき、計測結果選択手段６Ｄが、ピッチ方向の姿勢変化にロバストである計測結果を選択する。ここで、２次元を移動する物体、つまり本実施形態の移動装置１の場合には、その自由度は３である。従って、位置を示す２変数ｘ、ｙと、姿勢を示す１変数θで、移動装置１を表現することが可能である。これに基づき、上記距離計測装置６Ａから取得した計測結果Ｌk：（ｋ＝｛０〜Ｎ｝）から、図７のような座標系において、ロバストに推定するための計測結果Ｌkを選択する。本実施形態では、事前位置推定手段６Ｃの事前推定値のバラツキにもロバストな計測値を選択する。
続いて、選択した計測値の結果を元に、位置推定を実施する。
【００３３】
（本実施形態の効果）
（１）移動装置１の姿勢を計測した結果を元に、床面までの距離を計測している計測結果を利用しない。これによって、移動装置１の姿勢次第で床面までの距離を計測する可能性がある計測結果であっても、ロバストな計測値だけを選択して位置推定に使用することで、自己位置の推定に使用する距離計測値の精度が向上する。さらには、移動装置１の姿勢による位置の誤推定を防ぐことができる。
なお、位置推定に使用する計測値を選択することで、位置推定に使用する計測値の数を制限することが可能となる。
（２）移動装置１に発生している実際の姿勢を検出する。これによって、距離計測時の姿勢を検出可能となる。
（３）事前推定値の確からしさを利用する。これによって、事前推定値による誤差要因に応じて、距離計測結果を選択することができる。例えば、事前推定値の確からしさが高い場合、計測結果の誤差要因が低減するため、姿勢変化に対する誤差要因に対する許容度を上げることが可能となる。
【００３４】
（変形例）
（１）上記実施形態では、事前位置推定手段６Ｃによる事前推定値のバラツキによって、選択する計測値を求めている。これに代えて、事前推定値のバラツキを固定にして使用しても良い。この場合には、ｄｘ、ｄｙ、ｄθとして固定値を設定すればよい。例えば、図８におけるＡ１領域からの計測結果を、無条件に選択対象外とすることで、ロバストな計測値を選択する。
この実施例によれば移動装置１の姿勢変化を考慮し、移動装置１の姿勢により床面までの距離を計測している可能性のある計測結果は、最初から利用しない。これによって、移動装置１の姿勢による位置の誤推定を防ぐことができる。
【００３５】
（２）上記実施形態の姿勢検出手段では、移動装置本体２の実際の姿勢をセンサで検出する姿勢計測装置６Ｂで構成した。これに代えて、姿勢検出手段を、次のように構成しても良い。
すなわち、姿勢検出手段は、姿勢制御装置５の出力から、現在の姿勢の推定を行う。本実施形態では、移動装置本体２における、距離計測時の床面に対する角度姿勢（ピッチ角γ）を取得する。
上記姿勢の推定は、上記（１）式〜（４）式から、制御出力ｕが分かれば、Δｔ後の姿勢θを推定することが可能となる。従って、姿勢の履歴を取ることで、距離計測時の姿勢を推定可能である。
【００３６】
この場合には、姿勢計測装置６Ｂによる計測が利用不可能であるか、もしくは利用できても姿勢変化速度が姿勢計測装置６Ｂの応答速度に対して速すぎる場合であっても、距離計測時の姿勢を取得することが可能となる。
なお、姿勢計測装置６Ｂと、この推定による姿勢検出とを併用しても良い。
すなわち、姿勢計測装置６Ｂが利用可能であり、また姿勢計測装置６Ｂの応答速度が姿勢変化速度に対して十分速ければ、姿勢計測装置６Ｂの値をそのまま利用する。これに対して、姿勢計測装置６Ｂによる計測が利用不可能であるか、もしくは利用できても姿勢変化速度が姿勢計測装置６Ｂの応答速度に対して速すぎる場合は、姿勢制御装置５の出力から、現在の姿勢の推定を行う。
なお、姿勢計測装置６Ｂからの計測値の履歴から、距離計測時の姿勢を推定するようにしても良い。
【００３７】
（３）また、距離計測時における移動装置１の姿勢変化（ピッチ角γ）が所定以上の場合にだけ、上記位置推定方法を提供するようにしても良い。すなわち、距離計測時における移動装置１の姿勢変化が所定未満の場合には、距離計測装置６Ａによる計測結果をそのまま使用して位置の推定を行うようにしても良い。
【図面の簡単な説明】
【００３８】
【図１】本発明に基づく実施形態に係る移動装置を示す模式的斜視図である。
【図２】本発明に基づく実施形態に係る移動装置を示す倒立振子モデルを表す図である。
【図３】本発明に基づく実施形態に係る位置推定装置を示すブロック図である。
【図４】本発明に基づく実施形態に係る位置推定装置の処理を示す図である。
【図５】移動装置による計測結果の例を示す上面図である。
【図６】本発明に基づく実施形態に係る計測値の選択に係る表記方法を示す図である。
【図７】計測結果選択手段の処理を説明する図である。
【図８】（∂ｆｍ／∂θ）の特徴を示す平面図である。
【図９】ｘ−ｙ座標の変換を示す図である。
【符号の説明】
【００３９】
１ 移動装置
２ 移動装置本体
３ 車輪
４ アクチュエータ
５ 姿勢制御装置
６ 位置推定装置
６Ａ 距離計測装置
６Ｂ 姿勢計測装置
６Ｃ 事前位置推定手段
６Ｄ 計測結果選択手段
６Ｅ 位置推定手段
ｆｍ（） 周囲地図
Ｌk 計測結果
Ｓ 障害物
ｕ 制御出力
ｘ、ｙ 変数
β 照射角度
γ ピッチ角
θ 変数
【特許請求の範囲】
【請求項１】
床面に沿って移動する移動装置に搭載して当該移動装置周囲の障害物までの距離を複数箇所計測する距離計測装置と、その距離計測装置による計測値に基づき上記移動装置の位置を推定する位置推定手段と、を備える位置推定装置であって、
上記移動装置の姿勢を検出する姿勢検出手段と、
その姿勢検出手段で検出した姿勢情報に基づき、距離計測装置による複数の計測値のうち、距離計測時の移動装置の姿勢に対してロバストな計測値を、位置推定手段で上記推定の際に使用する計測値として選択する計測結果選択手段と、を備えることを特徴とする位置推定装置。
【請求項２】
上記計測結果選択手段は、上記姿勢情報に基づき、上記床面までの距離を計測していない可能性の高い計測結果を、距離計測時の移動装置の姿勢に対してロバストな計測値として選択することを特徴とする請求項１に記載した位置推定装置。
【請求項３】
上記姿勢検出手段は、移動装置に発生している実際の姿勢を検出することを特徴とする請求項１又は請求項２に記載した位置推定装置。
【請求項４】
上記姿勢検出手段は、移動装置の駆動制御状態から距離計測時の移動装置の姿勢を推定することで姿勢の検出を行うことを特徴とする請求項２に記載した位置推定装置。
【請求項５】
移動装置について、前回までの位置情報から現在の位置を確率的に推定する事前位置推定手段を備え、
上記計測結果選択手段は、さらに事前位置推定手段の推定結果の確からしさに応じて、距離計測時の移動装置の姿勢及び事前位置推定値手段による推定値のバラツキに対してロバストである計測値を、位置推定手段で上記推定の際に使用する計測値として選択する計測値として選択することを特徴とする請求項１〜請求項４のいずれか１項に記載した位置推定装置。
【請求項１】
床面に沿って移動する移動装置に搭載して当該移動装置周囲の障害物までの距離を複数箇所計測する距離計測装置と、その距離計測装置による計測値に基づき上記移動装置の位置を推定する位置推定手段と、を備える位置推定装置であって、
上記移動装置の姿勢を検出する姿勢検出手段と、
その姿勢検出手段で検出した姿勢情報に基づき、距離計測装置による複数の計測値のうち、距離計測時の移動装置の姿勢に対してロバストな計測値を、位置推定手段で上記推定の際に使用する計測値として選択する計測結果選択手段と、を備えることを特徴とする位置推定装置。
【請求項２】
上記計測結果選択手段は、上記姿勢情報に基づき、上記床面までの距離を計測していない可能性の高い計測結果を、距離計測時の移動装置の姿勢に対してロバストな計測値として選択することを特徴とする請求項１に記載した位置推定装置。
【請求項３】
上記姿勢検出手段は、移動装置に発生している実際の姿勢を検出することを特徴とする請求項１又は請求項２に記載した位置推定装置。
【請求項４】
上記姿勢検出手段は、移動装置の駆動制御状態から距離計測時の移動装置の姿勢を推定することで姿勢の検出を行うことを特徴とする請求項２に記載した位置推定装置。
【請求項５】
移動装置について、前回までの位置情報から現在の位置を確率的に推定する事前位置推定手段を備え、
上記計測結果選択手段は、さらに事前位置推定手段の推定結果の確からしさに応じて、距離計測時の移動装置の姿勢及び事前位置推定値手段による推定値のバラツキに対してロバストである計測値を、位置推定手段で上記推定の際に使用する計測値として選択する計測値として選択することを特徴とする請求項１〜請求項４のいずれか１項に記載した位置推定装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【公開番号】特開２００９−２９４１０４（Ｐ２００９−２９４１０４Ａ）
【公開日】平成２１年１２月１７日（２００９．１２．１７）
【国際特許分類】
【出願番号】特願２００８−１４８２９９（Ｐ２００８−１４８２９９）
【出願日】平成２０年６月５日（２００８．６．５）
【出願人】（０００００３９９７）日産自動車株式会社 (16,386)
【Ｆターム（参考）】
【公開日】平成２１年１２月１７日（２００９．１２．１７）
【国際特許分類】
【出願日】平成２０年６月５日（２００８．６．５）
【出願人】（０００００３９９７）日産自動車株式会社 (16,386)
【Ｆターム（参考）】
[ Back to top ]