特許 5909486 自己位置姿勢推定システム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】5909486

(24)【登録日】2016年4月1日

(45)【発行日】2016年4月26日

(54)【発明の名称】自己位置姿勢推定システム

(51)【国際特許分類】

   G05D 1/02 20060101AFI20160412BHJP

【ＦＩ】

   G05D1/02 J

【請求項の数】4

【全頁数】15

(21)【出願番号】特願2013-522770(P2013-522770)

(86)(22)【出願日】2012年6月19日

(86)【国際出願番号】JP2012065626

(87)【国際公開番号】WO2013002067

(87)【国際公開日】20130103

【審査請求日】2014年3月31日

(31)【優先権主張番号】特願2011-143921(P2011-143921)

(32)【優先日】2011年6月29日

(33)【優先権主張国】JP

(73)【特許権者】

【識別番号】502129933

【氏名又は名称】株式会社日立産機システム

(74)【代理人】

【識別番号】110000350

【氏名又は名称】ポレール特許業務法人

(72)【発明者】

【氏名】槙 修一

(72)【発明者】

【氏名】松本 高斉

(72)【発明者】

【氏名】正木 良三

【審査官】 牧 初

(56)【参考文献】

【文献】 特開２００９−２１７３３３（ＪＰ，Ａ）

【文献】 特開２０１０−１７６２８３（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０５Ｄ １／００−１／１２

(57)【特許請求の範囲】

【請求項1】

移動体に搭載され、移動体の位置姿勢推定を行う自己位置姿勢推定システムであって、
移動体に設けられた距離センサからの計測データと地図データを照合することで前記移動体の位置と姿勢を推定する位置姿勢推定部と、
前記位置姿勢推定部の位置姿勢推定結果の信頼度の評価値を算出する位置姿勢推定評価値算出部と、を備え、
前記位置姿勢推定評価値算出部は、前記距離センサからの計測データと地図データの一致度及び位置姿勢推定結果の分散を算出することにより前記位置姿勢推定部の位置姿勢推定結果の信頼度の評価値を算出することを特徴とする位置姿勢推定システム。

【請求項2】

前記移動体において取得する前記距離センサからの計測データは地図データの作成に使用されることを特徴とする請求項１に記載の位置姿勢推定システム。

【請求項3】

前記移動体の移動量を計測するエンコーダからの計測データによって移動量を推定する移動量推定部を備えることを特徴とする請求項１に記載の位置姿勢推定システム。

【請求項4】

前記移動体は、前記評価値に応じて、減速または停止を行うことを特徴とする請求項１に記載の位置姿勢推定システム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、移動体に搭載され、自己位置姿勢を推定する機能を有する自己位置姿勢
推定システムに関する。

【背景技術】

【0002】

所定の領域内で自律移動ロボットを物品の搬送などの作業を行う作業システムを組み込んだ自律移動ロボットシステムがある。
このようなシステムにおいて、自律移動ロボットは、自律移動に不可欠な位置姿勢の推定が正常に行われているか否かの判定機能の実現が非常に重要になっている。位置姿勢推定が正常に動作していないと直ちにこれを検出し、動作モードを通常走行から速度制限や緊急停止などのモードに切り換え、トラブルに対処することができる。
また、不安全状況を引き起こす危険を未然に防止し、危険な場所を回避しながら目的地に移動する自律移動ロボットに関することが特許文献１（特開２００５−２４２４０９号公報）に開示されている。

【0003】

また、特許文献２（特開２０１１−４３４０５号公報）には、自己位置推定を高精度に行うために、検出領域内の物体との距離情報を検出する距離センサを備え、距離センサにより検出された物体との距離情報に基づいて、自己位置を推定することが開示されている。また、特許文献３（特開２０１１−６５３０８号公報）には、測定装置の測定データに基づいて自己位置を推定しながら自律移動する装置について開示されている。

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特開２００５−２４２４０９号公報

【特許文献2】特開２０１１−４３４０５号公報

【特許文献3】特開２０１１−６５３０８号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

特許文献１には、移動環境に設けられた標識、例えば、マークやパターン、超音波発生器、レーザ発光装置などを用い、検出手段として、それぞれの標識に対応したＣＣＤ又はＣＭＯＳセンサなどの画像センサ、超音波受信器、レーザ光受光素子があり、これらの検出結果により位置方向を示し、自己位置推定の誤りを検知でき、危険回避を行うことができると記載してある。しかし、この方式は、標識の設置や登録等の作業コストが大きくなるなどの課題がある。

【0006】

また、特許文献２は、位置姿勢推定評価値として、一致度を用いる手法が提案されているが、長い廊下など同じ風景が連続する場合に正しく評価することができない。さらに、特許文献３は、誤差分散地図を用いて、位置姿勢推定の信頼度を評価しながら運用する手法が提案されているが、ランドマークに対するセンサの特性が既知であることを前提としており、未知の環境で安全なシステムを構築することが難しい。

【0007】

本発明は、上記課題を解消するため、移動ロボットにおいて、パーティクルフィルタを用いて位置姿勢推定を行い、その位置姿勢推定の状態を判定する量としてパーティクルの分散により位置姿勢推定の閾値を求め、移動ロボットの減速、停止を行うことを目的とする。
また、本発明は、上記課題を解消するため、移動ロボットなどの移動体に搭載される自己位置姿勢推定システムにおいて、パーティクルフィルタを用いて位置姿勢推定を行い、その位置姿勢推定の状態を判定する量としてパーティクルの分散により位置姿勢推定の閾値を求め、位置姿勢推定が正常に動作していない場合に、位置姿勢推定が正常に動作していないことを示す情報、信号を出力可能とすることを目的とする。

【課題を解決するための手段】

【0008】

本発明は、上記課題を解決するため、移動体に搭載され、移動体の位置姿勢推定を行う
自己位置姿勢推定システムを、移動体に設けられた距離センサからの計測データと地図デ
ータを照合することで移動体の位置と姿勢を推定する位置姿勢推定部と、位置姿勢推定部
の位置姿勢推定結果の信頼度の評価値を算出する位置姿勢推定評価値算出部と、を備え、
位置姿勢推定評価値算出部は、距離センサからの計測データと地図データの一致度及び位
置姿勢推定結果の分散を算出することにより位置姿勢推定部の位置姿勢推定結果の信頼度の評価値を算出するようにした。

【発明の効果】

【0014】

本発明によれば、移動ロボットの位置姿勢推定が正常に行われているか否かを判定し、正常でない場合はロボットを減速させたりあるいは緊急停止させたりできるため、衝突事故を未然に防ぐことができる。
また、本発明によれば、移動体の位置姿勢推定が正常に行なわれているか否かを判定し、正常でない場合は、正常に行なわれていないことを示す情報、または信号を移動体に出力することを可能とする。

【図面の簡単な説明】

【0015】

【図1】本発明の自律移動ロボットのシステムブロック図を示す。

【図2】障害物のある地図と移動ロボットの経路及び計測状態を示す。

【図3】移動ロボットの走行時の計測状態を示す。

【図4】移動ロボットの走行時の計測状態を示す。

【図5】パーティクルフィルタを説明する図を示す。

【図6】移動ロボットの動作モード判定部の内容を説明する図を示す。

【図7】移動ロボットの動作モード判定部の内容を説明する図を示す。

【図8】障害物が動いた場合の移動ロボットの動作モード判定部の内容を説明する図を示す。

【図9】位置姿勢推定評価値に基づく減速又は停止の処理フローチャートを示す。

【図10】環境変化があった場合の地図変更を行うフローチャートを示す。

【図11】移動ロボットの走行経路を示す図である。

【図12】移動ロボットの試行時に位置姿勢推定評価値の閾値を決めるフローチャートを示す。

【図13】本発明の自律移動ロボットにおけるオドメトリなしの場合のシステムブロック図を示す。

【発明を実施するための形態】

【0016】

以下、本発明の実施の形態を図面を用いて説明する。
以下の明細書では、移動ロボット、及び移動体に搭載される自己位置姿勢推定システムについて説明している。ここで、前記「自己位置姿勢推定」を示す名称について、以下に確認的に説明をする。
「自己位置姿勢推定」は、移動ロボットもしくは自己位置姿勢推定システムが搭載されている移動体の自己の位置（ｘ、ｙ）と姿勢θを推定することを意味し、位置のみ、姿勢のみしか推定を行わない場合も含む。
学会等では、多くの場合、「姿勢」を省略し、単に「自己位置推定」と表記されており、この名称も、以下の明細書にて説明している「自己位置姿勢推定」と同義である。
なお、英語での名称についても、以下に説明する。
英語の名称としては、”position and orientation estimation”と訳される。
”position”は位置(x、y)、”orientation”は姿勢θに相当する。
学会等では、”orientation”を省略し、単に”position estimation”と表記されることがあり、”position and orientation estimation”と同義であり、以下の明細書にて説明されている。
同様に”pose estimation”、”position and posture estimation”、”configuration estimation”、との名称としても同義であり、以下の明細書にて説明がされているものである。
なお、以下の実施の形態の説明では、移動体として、移動ロボットを例にして説明する。
また、移動体に搭載される自己位置姿勢推定システムは、前記移動ロボットに搭載されている位置姿勢推定部を含み構成されるシステムにて説明するものとする。
（実施例１）

【0017】

図１は、本発明の実施例の自律移動ロボットのシステムブロック図である。
図１において、１は自律移動ロボット、９は位置姿勢推定部、２は位置姿勢推定部の結果を評価するための位置姿勢推定評価値計算部、３は動作モードを判定する動作モード判定部、５は距離を計測するレーザ距離センサ、６は、障害物の特徴を記載した地図を記憶した地図記憶部、７は移動ロボットの位置姿勢を制御する位置姿勢制御部で、８ａ、８ｂは車輪の回転数を計測するエンコーダである。また、５０は地図と地図上におけるロボットの位置姿勢とレーザ距離センサの計測データなどを表示する画像表示器である。

【0018】

自律移動ロボット１は、障害物との距離ｄを計測するためのレーザ距離センサ５と、地図記憶部６に記憶した障害物の特徴を記載した地図と、車輪の回転数を計算するエンコーダ８ａ、８ｂを備え、距離ｄ２２と地図６と回転数をもとにロボット１の位置姿勢を推定する位置姿勢推定部９と、位置姿勢推定結果を評価するための評価値計算部２と計算された評価値をもとに動作モードを判定する動作モード判定部３と動作モードと位置姿勢推定結果をもとに移動ロボット１の位置姿勢を制御する位置姿勢制御部７から構成される。
ここで、基準となる座標系（世界座標系）に対する移動ロボット１の位置を（ｘ，ｙ）、ｘ軸を基準とした移動ロボット１の姿勢をθとし、まとめて（ｘ，ｙ，θ）で表すことにする。

【0019】

次に、図３を用いて本実施例で使用されているレーザ距離センサ５について説明する。
図３において、１は移動ロボット、６１は壁、１４は障害物である作業台、２２は移動ロボットのレーザ距離センサ５から障害物までの距離データ、２３は移動後のロボットで、２４はその移動量ｕを表している。
移動ロボット１に搭載したレーザ距離センサ５は、±Ｗ（ｄｅｇ）の範囲を分解能Δθで、移動ロボット１の周囲にある物体との距離を計測する（例えばＷ＝９０、θ＝０．５）。この計測データを距離データｄ２２と呼ぶ。このデータは、移動ロボット１の周囲にある計測物の形状を計測していることになる。そして、この距離データｄ２２と地図の情報を照合することで、移動ロボット１の位置姿勢を推定することができる。

【0020】

次に、図２乃至図５を用いて位置姿勢推定部９に実装されているパーティクルフィルタの説明を行う。
図２において、１０は移動ロボットが走行する領域を表す地図で、１１，１２，１３，１４及び１６は障害物を表している。１７，１８は、搬送用荷物の積み下ろし場所で、３２，３３は作業員を示している。
また、図２に示す地図で、積み下ろし場所１７、１８間の経路は、下側の通路と右側の通路の経路１９、障害物１１，１３と障害物１２，１４の間の通路の経路２０、及び左側の通路から上側の通路の通る経路２１を示している。

【0021】

図４において、２５は移動前の移動ロボットの位置を示している。２７は移動量ｕｅ２６だけ移動した状態を表している。
さらに図５において、２８は移動ロボットが移動する前のパーティクルで、３０は移動量ｕｅ２９だけ移動し、この領域でのパーティクルを表し、３１は分散したパーティクルの重み付けが大きいパーティクルを表したものである。
パーティクルフィルタは、複数のセンサからの位置姿勢推定結果を確率的に融合し、推定誤差を軽減することが可能である。移動ロボット１の位置姿勢候補はパーティクルと呼ばれる複数の候補（ｘｉ，ｙｉ，θｉ）；ｉ＝１，・・・，Ｍによって表現される。レーザ距離センサによる位置姿勢推定結果として複数の解を持つ場合にも有効な手段である。

【0022】

パーティクルフィルタは、予測ステップ、更新ステップの２ステップで実行される。予測ステップでは、エンコーダ８ａ，８ｂから算出されるオドメトリを用いて移動ロボット１の移動後の位置を推定する。更新ステップでは、レーザ距離センサと地図による位置推定結果に基づいて予測されたロボットの位置姿勢を更新する。

【0023】

以下、予測ステップと更新ステップについて詳しく述べる。
１）予測ステップ
予測ステップについて図３乃至図５を用いて説明する。ここでは説明の簡単のためＭ＝１とする。
図３は移動ロボット１が初期位置姿勢（ｘ０，ｙ０，θ０）から移動量ｕだけ移動し、２３の位置姿勢（ｘ，ｙ，θ）でレーザ距離センサ５によって計測物６１、６２との距離を計測した状態を表している。ここで、移動ロボット１の初めの位置姿勢（ｘ０，ｙ０，θ０）は既知であるとする。図４は初期位置姿勢（ｘ０，ｙ０，θ０）２５からエンコーダ８ａ，８ｂの値を用いて移動量ｕｅ＝（ｕｅ＿ｘ，ｕｅ＿ｙ，ｕｅ＿θ）２６を計算し、この値から移動後の位置姿勢を予測した状態を表す。このとき移動後の位置姿勢（ｘ，ｙ，θ）に対して予測値２７は（ｘ０＋ｕｅ＿ｘ，ｙ０＋ｕｅ＿ｙ，θ０＋ｕｅ＿θ）となる。この移動量（ｕｅ＿ｘ，ｕｅ＿ｙ，ｕｅ＿θ）の値には車輪のスリップ等誤差が含まれているため、予測ステップではエンコーダ８ａ，８ｂの誤差分布に従う位置姿勢候補を用いて予測値を複数個生成する。この複数個の位置姿勢候補を、予測パーティクル（ｘｐ，ｙｐ，θｐ）；ｐ＝１，・・・，Ｎで表し、擬似乱数などを利用して作成する。
図５は初期パーティクル２８（Ｍ＝５）から移動量ｕｅに基づいて生成された予測パーティクル３０（ｘｐ，ｙｐ，θｐ）；ｐ＝１，・・・，ＭＮを示している。予測パーティクル３０はエンコーダ８ａ、８ｂの誤差の分だけ初期パーティクル２８より広い面積に分布していることが分かる。

【0024】

２）更新ステップ
次に、更新ステップについて図３乃至図５で説明する。
更新ステップでは、まず予測パーティクル（ｘｐ，ｙｐ，θｐ）；ｐ＝１，・・・，Ｎに対して、距離データｄ２２と地図１０、１４との一致具合を計算する。具体的には、位置姿勢（ｘｐ，ｙｐ，θｐ）で距離データｄ２２を計測したと仮定し、地図１０、１４と距離データｄ２２の重なっている点数を計算する。この値を一致度Ｒと呼ぶ。一致度Ｒは、真の位置姿勢（ｘ，ｙ，θ）と予測パーティクル（ｘｐ，ｙｐ，θｐ）が近い距離にあれば高い値となり、大幅にずれていると低い値となる。図４はパーティクル２７に対して、図３のレーザ距離センサ５による距離データｄ２２を当てはめ、一致度を計算した例である。移動量ｕｅ２６に誤差が含まれるため、そのままあてはめると地図１０、１４とずれていることがわかる。

【0025】

次に、すべての予測パーティクル（ｘｐ，ｙｐ，θｐ）；ｐ＝１，・・・，Ｎに対してこの一致度Ｒを計算することで、もっとも一致の高い位置姿勢（ｘｅ，ｙｅ，θｅ）を見つける。この位置姿勢（ｘｅ，ｙｅ，θｅ）がレーザ距離センサ５で観測した位置姿勢（ｘ，ｙ，θ）に近いであろうから、この点をロボットの位置姿勢推定結果とし、次のステップの（ｘ０，ｙ０，θ０）とする。

【0026】

図５に示すように、初期位置が正確にわからない場合は、ロボットの位置姿勢は複数のパーティクルによって表現される。このときはＭ点の初期パーティクル（ｘ０ｉ，ｙ０ｉ，θ０ｉ）それぞれに対して、予測ステップを適用し、生成された予測パーティクル（ｘｐ，ｙｐ，θｐ）；ｐ＝１，・・・，ＭＮ、に対して一致度を計算する。そのなかで一致度がもっとも高いものをロボットの推定位置姿勢として採用する。その後、一致度に従ってＭＮ点の中からＭ点を選定することで、初期パーティクル（ｘ０ｉ，ｙ０ｉ，θ０ｉ）を更新する。

【0027】

以上がパーティクルフィルタのアルゴリズムである。上記更新ステップでは（ｘｐ，ｙｐ，θｐ）に対して計算された一致度のもっとも高いものを推定結果として採用した。しかしながら、ここで注意しなければならないのは、最大値となる一致度を持つパーティクルが２点存在する場合があることである。この状況において、一致度最大である２点間の距離が近い場合はどちらを選んでも本質的な問題とはならないが、問題となるのは２点間の距離が離れている場合である。この場合、どちらのパーティクルを選ぶべきか、一致度で区別することができない。また、２点ではなく複数点でも一致度が同じ値で最大値となってしまう場合がある。このような状況では、推定結果のばらつきが大きくなるため、移動ロボットがこの値をそのまま制御に用いるのは性能面、安全面において好ましくない。よって、推定結果の信頼性の評価を行うため、位置姿勢推定評価値Ｅを定義する必要がある。この値Ｅを用いれば、移動ロボット１が位置姿勢推定値を取捨選択する、もしくは、減速・停止することが可能であり、移動ロボット１はより安全な自律移動を行うことができる。

【0028】

ここで、複数個のパーティクルの一致度が最大となり、この最大一致度であるパーティクル間の距離が広くなると状態は、パーティクル３１の分散値が大きい状態であると考えられるため、本実施例では、このような位置姿勢推定評価値Ｅとしてパーティクルの平均値周りの分散σを適用する。
分散が大きくなってしまう例としては、移動ロボット１周辺に環境変化（地図に記載された計測物の移動、作業員等）がある場合に距離データｄ２２と地図１０，１４が一致せず、パーティクルが広範囲に分布することがある。また、距離データｄ２２と地図１０，１４が良く一致する場合でも、長い廊下などでは廊下の長手方向にパーティクルが広範囲に分布してしまう。このような状況において、パーティクルの分散を用いることで位置姿勢推定結果の信頼性を評価することが可能となる。

【0029】

位置姿勢推定評価値計算部２では、パーティクルの分散を計算するが、本実施例では次のような評価式を用いる。

【0030】

Ｅ×Ｅ＝σｘ×σｘ＋σｙ×σｙ＋α×σθ×σθ （数１）
ｘ，ｙ，θの標準偏差σｘ，σｙ，σθに対して、それぞれの和をとったものである。αは正規化定数であり、位置の分散と姿勢の分散の平均値をそろえる値に設定する。
このように、Ｅの値として分散の和をとれば１次元化することができ、評価が簡単になる効果がある。この他に共分散行列の固有値や、ｘ、ｙ、θの分散を個別に評価する方法があり、評価方法が複雑化するがより細やかな評価が可能となる。

【0031】

次に、動作モード判定部３について、図６、図７、図８を用いて詳しく説明する。
図６は、移動ロボットの前に作業員３２，３３が重なった状態で立っている場合のパーティクル３１を示している。また、図７は、移動ロボットの前に作業員３２，３３が並んだ状態で立っている場合のパーティクル３１を示している。
図８は、障害者が地震や人がぶつかって動いた場合を示している。図８（ａ）は移動ロボットの距離を計測している状態を示し、図８（ｂ）はその時のパーティクル３１を示す。

【0032】

図８は、最大一致度が複数点表れる状態を示している。図８（ａ）では地図作成後に、作業員３２，３３の接触等により作業台１４が意図せずに移動してしまった状況で移動ロボット１がレーザ距離センサによる計測を行った状況を示している。地図は移動前の作業台６２の配置が記載されているため、距離データｄ２２と地図１０、１４が完全に一致する位置姿勢（ｘ，ｙ，θ）は存在しない。この状態でパーティクルフィルタによって位置姿勢推定を行った結果を図８（ｂ）に示す。一致度が高い場所としては本来の位置と、この位置とは別に移動ロボット進行方向に対して左側にもできている。これは作業台１４が移動してしまったために、通路幅が狭くなり、通路の両側の壁に対して距離データｄ２２が一致するものが出現してしまうためである。この状況では大きく分けて、４０、４１が一致しているものと、４０，４２が一致しているものの２パターンが更新ステップ後にも残り、パーティクルが広く分布するため、位置姿勢推定評価値Ｅの値が増加し、位置姿勢推定状態として不安定になりつつあるということを評価することが可能となる。

【0033】

図６、７は、移動中の移動ロボット１の前方に現場の作業員３２、３３が存在する場合を表している。図６は作業員３２、３３によって移動ロボット１前方の距離データが半分ほど隠されている。このとき、図５の状態に比べて前後方向の計測点数が減少したことにより、移動ロボット１の前後方向に対して相対的に一致度の高いパーティクルが増加する。よって、更新ステップによって選ばれるパーティクルが移動ロボット１の前後方向に通常より広く分布するため位置姿勢推定評価値Ｅの値は大きくなるため、移動ロボット１は減速の制御を行い、より安全な移動が可能となる。

【0034】

図７は、移動ロボット１の前方を作業員３２、３３が完全に隠しており、移動ロボット１の前後方向の座標を推定することがほぼできない状態にある。このとき位置姿勢推定評価値Ｅの値は図６の状態からさらに悪化するため、この値に基づいて動作モード判定部は移動ロボット１を安全に停止させる。

【0035】

移動ロボット１を安全に停止させる方法として、レーザ距離センサの計測範囲の中で、ロボットの進行方法にロボットの移動速度に比例する大きさの物体検出エリアを設け、そのエリアに計測物が検出された場合に危険とみなして停止するものがある。しかしながら、この手法を用いるとロボット旋回時に不要に壁を検出し、ノイズを検出してしまうなどの問題があり、作業効率を下げる原因となってしまう。本発明は位置推定結果の信頼性に基づいた、本質的に必要な停止処理であり、ノイズを検出して不要に停止するといったことは起こらないことが本発明の特徴である。

【0036】

また、移動ロボット１は一旦停止閾値を越えてしまったため、その原因が作業員の遮りであるか、環境変化であるかなどを特定するまで復帰することはできない。このときには、作業員が移動ロボット１の状態を確認、操作することを想定し、自動的に手動モードに切り替わることも特徴である。

【0037】

図９は、位置姿勢推定評価値Ｅに基づいて、移動ロボット１の減速・停止を行う処理のフローチャートである。
先ず、移動ロボット１は、荷物の積み下ろし場所１７または１８に荷積みを行い、目的地に向かって自律移動をする（ステップ２０１）。そして、移動している間、移動ロボット１は、センシングを行い、位置姿勢推定を行う（ステップ２０２）。次に、位置姿勢推定結果に対して、位置姿勢推定評価値Ｅの計算処理を実施する（ステップ２０３）。
移動ロボット１は、位置姿勢推定結果から目的地に到達したかを判定する（ステップ２０４）。到達していれば動作を終了し、到達していなければ位置姿勢推定評価値Ｅに基づき、動作モードの変更を行い、位置姿勢推定評価値Ｅと閾値Ｅ１との比較を行う（ステップ２０５）。ここで、位置姿勢推定評価値Ｅが閾値Ｅ１より大きければ、移動ロボット１は通常走行モードから減速モードに移行する（ステップ２０６）。
評価値Ｅが閾値Ｅ１より小さければ、自律移動のステップ２０１へ戻る。
減速モード（ステップ２０６）になると、次に位置姿勢推定評価値Ｅと閾値Ｅ２とを比較し、評価値Ｅが大きければ、すなわち停止の閾値を超えてしまった場合、自律移動は事実上不可能の状態であるため、手動モードへ移行する（ステップ２０８）。

【0038】

評価値Ｅが閾値Ｅ２より小さければ、動作モードは減速モードの状態で、自律移動を継続する（ステップ２０１に戻る）。
手動モードに切り替わったら（ステップ２０８）、切り替わったことを作業者に通知し（ステップ２０９）、移動ロボット１は停止する。
本実施例では、減速の閾値は１段階のみ設けているが、多段階に閾値を設けて減速することも可能である。

【0039】

図１０は、停止閾値を越えてしまった場合の移動ロボット１の復帰手順を示すフローチャートを示す。
まず移動ロボット１は位置姿勢推定を行っている（ステップ３０１）。次にステップ３０１の位置姿勢推定結果に対する評価値を計算する（ステップ３０２）。次に移動ロボット１は現在停止中であることを作業員／管理者に通知を行う（ステップ３０３）。通知を受けた作業員は移動ロボット１に取り付けられた画像表示器５０を確認し、位置姿勢推定評価値Ｅが停止閾値Ｅ２より小であることと、距離データｄと地図１０、１４が良く一致していることを確認する（ステップ３０４）。このとき、画像表示器には地図６と移動ロボット１の位置と、距離データｄと評価値Ｅ、一致度Ｒなどが表示されている。距離データは地図と重なっている点と重なっていない点で色分けされており、環境変化がある点について一目瞭然な状態となっていることが特徴である。

【0040】

ステップ３０４で作業員が現状を確認後、閾値を超えた原因が作業員である場合に、手動モードを解除し、移動ロボット１を作業に復帰させる（ステップ３０８）。距離データｄと地図１０、１４が一致しておらず、その原因が作業台など、計測物の移動であると認められた場合、作業者／管理者は移動ロボット１が停止した周辺の地図を更新する（ステップ３０６）。地図の更新には、手動モードに切り替えられた移動ロボット１を操作し、距離データｄを収集しても良いし、地図作成専用の器具を用いても良い。また、このとき地図の更新は移動ロボット１の表示で確認しながらロボット上で行っても良いし、距離データｄのみを持ち帰り、別のＰＣで地図作成を行うことも可能である。その後、移動ロボット１は更新された地図を読み込み、作業に復帰する。

【0041】

図１１は、ロボット１の作業現場環境を示している。作業現場には荷の積み下ろし場所１７、１８が存在し、それを結ぶ移動経路１９、２０、２１が設定されている。ロボット１がこの環境において自律移動を行うためには、まず現場の地図を作成する必要がある。次に地図作成が正しく行われているかの確認のため、ロボット１は現場で試走を行う。
この試走における計測データに基づいて閾値Ｅを設定する。閾値の値はセンサの特性から決定されるべきものであるが、センサの特性は計測物の材質や色に依存するため、すべての組み合わせを把握することは難しい。よって、実際に計測を行うことで、現場に適した閾値を設定することができるため、より安全な自律移動が可能となる。

【0042】

図１２は、この閾値決めの処理のフローチャートを示す。
先ず、移動ロボット１は環境地図の作成を行う（ステップ１０１）。次に、移動ロボット１は、ステップ１０１で作成された地図を用いて、ロボットが実際に運用される経路にそって自律移動を行う（ステップ１０２）。このとき移動ロボット１は実際に位置姿勢推定を行い、この結果を地図記憶部６に保存しながら移動を行う。このとき、移動ロボット１が問題なく移動経路を走破した場合、記憶部６に保存された位置姿勢推定評価値を用いて、閾値決めのためのデータ処理を行う（ステップ１０３）。閾値が決まったら実際の走行で運用する（ステップ１０４）。

【0043】

具体的には、移動ロボット１が問題なく試走を終えた場合において、走行中に計算された位置姿勢推定評価値Ｅの最大値ＥｍａｘなどＥの統計量を利用して閾値の設定を行う。減速の閾値として２Ｅｍａｘ＞Ｅ１＞Ｅｍａｘの値を利用し、停止の閾値としてはＥ２＝２Ｅｍａｘを用いる。閾値としてこのような値を用いるのは、Ｅｍａｘまでの値は地図作成直後の試走で発生した値であり、この範囲であれば通常走行可能であることが保証されているためである。ここまでが移動ロボット１の実運用における準備段階である。

【0044】

本実施例において、パーティクルフィルタの推定結果を一致度最大のパーティクルとしたが、パーティクルの平均値をもって位置姿勢推定結果としても良い。
本実施例において、位置姿勢推定評価値として、パーティクルの分散を用いたが、パーティクルの広がりを表す統計量、たとえば、パーティクルすべてを含む長方形、楕円の面積などそのまま本発明に適用することが可能である。

【0045】

また、評価値として一致度Ｒを用いることもできる。一致度は環境変化の程度をそのまま表すものであり、移動ロボット１の周囲環境の情報を与える重要な値である。
ただし、本実施例で述べたとおり一致度のみでは位置推定結果の信頼性を評価することができないため、分散を含めた形で評価を行う。評価方法としては、一致度Ｒと分散を個別に評価した結果を統合しても良いし、多次元評価がわずらわしい場合には、一次元的に評価できる評価関数を作成しても良い。具体的な評価関数の例としては次式のようなものを用いる。

【0046】

Ｅ＝Ｒ／（１＋σ／２σｍａｘ） （数２）
この式において、σｍａｘは図１１の試走行で決定される標準偏差の最大値を表す。この式は分散値が良い値であるときは、一致度で評価を行い、一致度が良いときは分散値で評価を行うといった形となっている。標準偏差σは０の時が最良であるが、このときはＥ＝Ｒとなる。このときＥ＝０．５は計測物５０％であることを表す。また、Ｒは１が最良であり、このとき、評価値はＥ＝１／（１＋σ／２σｍａｘ）となり、σ＝２σｍａｘのとき、Ｅ＝０．５となる。よって、停止閾値を０．５とすれば一致度を含めた、より良い評価が可能となる。

【0047】

本実施例において、位置姿勢推定方法としてパーティクルフィルタを適用して位置姿勢推定評価値の説明を行ったが、推定方法としてカルマンフィルタを利用し、位置姿勢推定評価値としては、推定誤差の共分散行列を用いてもよい。また、オドメトリがない状態、つまり、予測が全くできない状態でも、移動量ｕをも誤差とみなし、パーティクルフィルタ、カルマンフィルタを適用することもできる。

【0048】

さらにまた、初期位置姿勢（ｘ０，ｙ０，θ０）を中心とした探索領域（ｘ０±Ｌｘ，ｙ±Ｌｙ，θ０±Ｌθ）を設け、その探索領域内を（ｘ０±ｋ１Δｘ、ｙ０±ｋ２Δｙ、θ０±ｋ３Δθ）；（ただし、ｋ１，ｋ２，ｋ３は整数。）とし、とびとびに表現される格子点上で一致度を計算すれば、パーティクルフィルタと同様の位置姿勢推定評価値を定義することができる。具体的には、一致度を重みとして探索領域内で画像処理における主成分分析を行い、その主成分値を用いればよい。以下これを詳しく述べる。

【0049】

図１３はオドメトリがない状態の位置姿勢推定システムの構成図である。図１と異なりオドメトリの入力が位置姿勢処理部に入力されていない。このとき、移動後のロボット位置姿勢予測ができないために計算コストが増加するため、高性能のＣＰＵを利用する必要がある。
しかしながら、エンコーダなしの車輪を用いることが可能となり、位置姿勢推定システムを手軽に活用できるメリットもある。
この場合における位置姿勢推定方式としてマッチング処理を例として、位置姿勢推定評価値の算出方法について述べる。
マッチング処理においては、パーティクルフィルタのようにランダムに選んだ位置姿勢に対して一致度を計算するのではなく、初期位置姿勢（ｘ０，ｙ０，θ０）を中心とした限られた領域を、格子点状に選んだ位置姿勢に対して一致度を計算することで最も一致度の高い点を検出し、推定位置姿勢とする方式である。初期位置姿勢（ｘ０，ｙ０，θ０）としては前回の位置姿勢推定結果を用いる。

【0050】

具体的な処理方法としては以下のように行う。
まず、初期位置姿勢（ｘ０，ｙ０，θ０）を中心とした探索領域（ｘ０±Ｌｘ,ｙ±Ｌｙ, θ０±Ｌθ）を設け、その領域内のとびとびの点（Ｘ,Ｙ,Θ）=（ｘ０±ｋ１Δｘ, ｙ０±ｋ２Δｙ, θ０±ｋ３Δθ）で表現される格子点上で一致度を計算する。
ここで、Ｌｘ、Ｌｙ、Ｌθは正の実数であり、ロボットの移動速度を考慮した大きさに設定する。前記のとおり、ｋ１,ｋ２,ｋ３は整数であり、Δｘ、Δｙ、Δθは格子点の幅を表す。このとき、この各点における一致度をR（Ｘ,Ｙ,Θ）で表し、この値が最も高い（Ｘ,Ｙ,Θ）を位置姿勢推定結果とする。以上がマッチング処理の概略である。

【0051】

次に位置姿勢評価値の計算方法について述べる。図7の状態でマッチング処理を行った場合、ロボット前方の作業員がレーザ光を遮っている。このため、ロボット両側にある壁としか一致せず、Ｒ（Ｘ,Ｙ,Θ）のなかで壁に沿った位置姿勢に対する一致度が相対的に高くなる。
逆に、Ｒ（Ｘ,Ｙ,Θ）の計算結果として、一致度の高い値が広く分布する方向には推定結果が不定となることを表しているため、このＲ（Ｘ,Ｙ,Θ)における一致度の分散度を計算することでパーティクルフィルタと同様の位置姿勢評価値を定義することができる。この一致度が強く分散する方向を検出するために、まずRの共分散行列Ｂ（3×3行列）を計算する。

【0052】

Ｂ＝ΣＲ(Ｘ,Ｙ,Θ）(Ｘ−μ_ｘ,Ｙ−μ_ｙ,Θ−μ_θ)^Ｔ(Ｘ−μ_ｘ,Ｙ−μ_ｙ,Θ−μ_θ） （数３）
（上付きのＴはベクトルの転置を表す。μ_ｘ、μ_ｙ、μ_θはＸ,Ｙ,ΘのＲ （Ｘ,Ｙ,Θ）で重み付けした平均値を表す。Σは探索領域内の格子点（Ｘ,Ｙ,Θ）に対する総和を表す。）
次に、Ｂを固有値分解すれば、次式のように表現される。

【0053】

Ｂ ＝ λ_１ｖ_１^Ｔｖ_１ ＋ λ_２ｖ_２^Ｔｖ_２ ＋ λ_３ｖ_３^Ｔｖ_３ （数４）
このｖ_１は３次元の固有ベクトル（行ベクトル）であり一致度が分散する方向を表す。λ₁≧λ_２≧λ₃は固有値であり、それぞれλ₁はｖ_１の方向に一致度がどの程度分散しているのを表している。このような手法は主成分分析とも呼ばれ、λ₁は第一主成分、λ₂は第二主成分などと呼ばれる。
この主成分値λを用いることによって、パーティクルフィルタと同様に、位置姿勢推定ができない状態を検出することが可能となる。
評価を行う際の閾値はパーティクルフィルタと同様に決定する。

【符号の説明】

【0054】

１‥自律移動ロボット
２‥位置姿勢推定評価値計算部 ３‥動作モード判定部
５‥レーザ距離センサ ６‥地図記憶部 ７‥位置姿勢制御部
８ａ、８ｂ‥エンコーダ ９‥位置姿勢推定部 １０‥地図
１４‥作業台 １９，２０，２１‥移動経路
２２‥距離データｄ ２４‥移動量ｕ
３１‥パーティクル ３２、３３‥作業員
５０‥画像表示器

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】