(57)【要約】【課題】地面の凹凸や車輪のスリップなどによる移動体の位置推定の誤差を回避することにより，移動体の位置推定の精度を高める技術を提供する。【解決手段】ロータリ装置１０１は，ロボット１０の移動に応じて，地面との接触により回転する，駆動力のない回転部１０２と，回転部１０２の回転から，駆動輪１６の回転に依らない，ロボット１０の地面に対する相対移動速度を検知するエンコーダ１０３とを備える。ロータリ装置１０１は，バネ１０４を介してロボット１０に取り付けられ，バネ１０４のプリロードにより，ロータリ装置１０１の回転部１０２は，常に地面と接触した状態となる。センシング処理部１８は，エンコーダ１０３により検知されたロボット１０の地面に対する相対移動速度から，ロボット１０自身の絶対的な移動速度を求める。【選択図】図２
2011075369.tif 000002
【特許請求の範囲】
【請求項１】
移動体の位置推定を支援する移動体位置推定支援装置であって，
前記移動体の移動に応じて，地面との接触により回転する，駆動力のない回転体を有するロータリ部と，
プリロードにより前記ロータリ部の前記回転体を地面に接触させ，前記回転体が接触する地面の状況に応じて伸縮する弾性体と，
前記回転体の回転から，前記移動体の地面に対する相対移動速度を検知するエンコーダと，
前記エンコーダにより検知された前記移動体の地面に対する相対移動速度から，前記移動体自身の移動速度を求めるセンシング処理部とを備える
ことを特徴とする移動体位置推定支援装置。
【請求項２】
前記移動体に対する前記ロータリ部の相対位置を検知するポテンショメータとをさらに備え，
前記センシング処理部は，前記ポテンショメータにより検知された前記移動体に対する前記ロータリ部の相対位置の変化に基づいて，前記エンコーダにより検知された前記移動体の地面に対する相対移動速度を補正し，補正された前記移動体の地面に対する相対移動速度から，前記移動体自身の移動速度を求める
ことを特徴とする請求項１に記載された移動体位置推定支援装置。
【請求項３】
少なくとも前記回転体を有する前記ロータリ部と，前記弾性体と，前記エンコーダとを含むユニットを複数備え，
前記センシング処理部は，前記ユニットごとの前記エンコーダにより検知された前記移動体の地面に対する相対移動速度を統合して，前記移動体自身の移動速度を求める
ことを特徴とする請求項１または請求項２に記載された移動体位置推定支援装置。
【請求項４】
前記センシング処理部は，前記ユニットごとの前記エンコーダにより検知された前記移動体の地面に対する相対移動速度に対してそれぞれ重み付けを行い，重み付けされた前記ユニットごとの前記移動体の地面に対する相対移動速度を統合して，前記移動体自身の移動速度を求める
ことを特徴とする請求項３に記載された移動体位置推定支援装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は，移動体の位置推定を支援する位置推定支援装置に関する。
【背景技術】
【０００２】
移動ロボットなどの移動体が通常業務を行うときには，常に自己位置の推定を行う必要がある。
【０００３】
移動体の位置推定を行う手法としては，オドメトリ（Odometry）と呼ばれる手法がある。オドメトリは，移動体の左右の車輪の回転速度から，移動体の併進速度と移動体本体の角速度とを求め，それらを積分して移動体の位置と姿勢とを求める手法である。
【０００４】
オドメトリとして，エンコーダ（Encoder ）により，車輪の回転速度を求める手法がある。エンコーダは，機械的変位量を電気信号に変換し，この信号を処理して位置・速度などを検出するソフトウェアや装置である。特に，回転の機械的変位量から位置・速度などを検出するエンコーダは，ロータリエンコーダ（Rotary Encoder）とも呼ばれる。エンコーダは，車輪を回転させるモータなどに取り付けられて，使用される。
【０００５】
なお，切削する路面の凹凸を自動的に検出し，検出された路面の凹凸に応じて切削部を制御し，切削部により形成される溝の深さを自動的に調整する技術が知られている。また，レールまたは路面を移動する移動体において，移動体の振動やレール上の汚れの影響を受けずに，レールまたは路面の凹凸や接続部遊間を測定する技術が知られている。また，計測輪を用いることにより，高価なジャイロを用いずに，また駆動輪の滑りの影響を受けずに，往復移動を繰り返しながら自動的に作業を行う作業ロボットを所望の方向に直進できるようにする技術が知られている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２００６−１８３２６１号公報
【特許文献２】特開平１０−３３９６２９号公報
【特許文献３】特開平７−１７５５１８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
上記のエンコーダによるオドメトリは，実装が簡単であるという利点がある。しかし，このようなオドメトリによって推定される位置情報には，車輪のスリップや地面の凹凸によって誤差が累積するという問題がある。
【０００８】
例えば，車輪が溝などに落ちてスリップしたような場合に，移動体自身は移動していなくても，エンコーダは車輪の回転速度を検知する。このとき，移動体は，実際に自身が移動していなくても，エンコーダによって検知された車輪の回転速度から自身が移動したものと推定してしまう。
【０００９】
また，例えば，移動体が同じ直線距離を移動する場合でも，途中に凹凸がない地面を移動するときと，途中に凹凸がある地面を移動するときとでは，途中に凹凸がある地面を移動するときの方が凹凸を超える分だけ実際に移動する距離が長くなる。移動体が，エンコーダによって検知された車輪の回転速度から自身の移動距離を算出し，二次元のマップ上で自身の位置を推定する場合について考える。このとき，移動体の移動経路上に凹凸があると，車輪の回転速度から推定された二次元マップ上での自身の水平移動量と，地面の凹凸を超えて移動した実際の水平移動量との間に誤差が発生してしまう。
【００１０】
このようなエンコーダによるオドメトリの問題を解決する技術として，ビジュアルオドメトリ（Visual Odometry ）と呼ばれる技術がある。ビジュアルオドメトリは，カメラにより撮影された画像を分析し，移動体の位置と姿勢とを求める技術である。
【００１１】
しかし，ビジュアルオドメトリでは，画像入力，前処理，特徴抽出などが必要となるため，コストが高くなるという問題がある。また，ビジュアルオドメトリには，装置の搭載位置や周囲の環境にも依存するという問題もある。
【００１２】
ロボットなどの移動体の自己位置推定の精度を高めるためには，運用環境への依存性を少なくし，車輪のスリップや地面の凹凸による誤差などの，移動環境によって発生する誤差を補正できるようにすることが望ましい。また，ロボットなどの移動体の自己位置推定のための装置は，ローコストかつ簡単に導入できることが望ましい。
【００１３】
本発明は，上記の問題の解決を図り，移動環境によって発生する可能性のある移動体の位置推定の誤差を回避することにより，移動体の位置推定の精度を高める技術を提供することを目的とする。
【課題を解決するための手段】
【００１４】
移動体の位置推定を支援する移動体位置推定支援装置は，移動体の移動に応じて，地面との接触により回転する，駆動力のない回転体を有するロータリ部と，プリロードによりロータリ部の回転体を地面に接触させ，回転体が接触する地面の状況に応じて伸縮する弾性体と，回転体の回転から，移動体の地面に対する相対移動速度を検知するエンコーダと，エンコーダにより検知された移動体の地面に対する相対移動速度から，移動体自身の移動速度を求めるセンシング処理部とを備える。
【発明の効果】
【００１５】
上記の技術によって，移動環境によって発生する可能性のある位置推定の誤差を回避し，移動体の位置推定の精度を高めることが可能となる。
【図面の簡単な説明】
【００１６】
【図１】本実施の形態によるロボットの運動モデルの例を示す。
【図２】本実施の形態によるロボットの構成例を示す図である。
【図３】本実施の形態による移動体位置推定支援部の構造を示す図である。
【図４】本実施の形態によるロータリ装置の内部構造を示す図である。
【図５】本実施の形態によるユニットの配置の例を示す図である。
【図６】本実施の形態において，駆動輪が溝に落ちてスリップした場合の例を示す図である。
【図７】本実施の形態において，地面の凹凸により発生する誤差を補正する例を説明する図である。
【図８】本実施の形態において，ロータリ装置の回転部が溝に落ちた例を説明する図である。
【図９】本実施の形態のセンシング処理部による移動速度算出処理の流れの例を示す図である。
【発明を実施するための形態】
【００１７】
以下，本実施の形態について，図を用いて説明する。
【００１８】
図１は，本実施の形態によるロボットの運動モデルの例を示す。
【００１９】
本実施の形態では，移動体の例として，差分駆動型のロボット１０を用いて説明を行う。図１には，上方から地面方向を見たものと想定し，差動駆動型のロボット１０の運動モデルが示されている。ロボット１０は，２つの駆動輪１６を持ち，その駆動輪１６を回転させることにより，自身の移動・回転を行う。
【００２０】
図１において，ＸG 軸，ＹG 軸の座標系は，ロボット１０の移動環境における二次元の絶対座標系である。ロボット１０は，自らが移動する環境の二次元のマップデータを記憶している。
【００２１】
図１において，ＸR 軸，ＹR 軸の座標系は，ロボット１０のローカル座標系である。ロボット１０は，ＸR 軸の方向に前進し，２つの駆動輪１６の中点を回転中心とした回転を行う。
【００２２】
ロボット１０は，ＸG 軸，ＹG 軸の絶対座標系のマップにおける回転中心の座標（ｘG ，ｙG ）を自身の位置座標とする。また，ロボット１０は，絶対座標系のＸG 軸，ＹG 軸に対する自身のローカル座標系のＸR 軸，ＹR 軸の角度θG を，自身が向いている方向とする。すなわち，ロボット１０は，二次元マップ上の移動環境における自身の位置と姿勢を，（ｘG ，ｙG ，θG ）で把握している。
【００２３】
駆動輪１６の回転を検知する駆動輪用エンコーダ（図示省略）を用いたオドメトリによるロボット１０の位置推定では，駆動輪１６の回転速度が駆動輪用エンコーダにより検出され，駆動輪１６の半径，駆動輪１６間の距離がパラメータとして利用されて，ロボット１０自身の前進および回転の速度が計算・推定される。
【００２４】
図１において，ｒi は各駆動輪１６の半径を示し，φi は各駆動輪１６の回転速度を示す。ｌは，２つの駆動輪１６間の距離を示す。なお，添え字のｉは，ｉ＝１が左の駆動輪１６ａを示し，ｉ＝２が右の駆動輪１６ｂを示す。このとき，ロボット１０の前進速度ｖは，
ｖ＝（φ1 ｒ1 ＋φ2 ｒ2 ）／２
となり，ロボット１０の回転速度ωは，
ω＝（φ1 ｒ1 −φ2 ｒ2 ）／ｌ
となる。
【００２５】
このように，駆動輪用エンコーダによって各駆動輪１６の回転速度φ1 ，φ2 を検出することにより，ロボット１０の前進速度ｖや，回転速度ωを算出することが可能となり，それらの結果を用いてロボット１０の位置の推定が可能となる。
【００２６】
しかし，上述したように，これだけでは，地面や床などの環境の状態によって，駆動輪１６のスリップなどによる位置推定の誤差が発生する。また，地面や床などに凹凸があるような場合には，駆動輪１６の移動量と実際の水平移動量との間に差分があり，その差分がロボット１０による自己位置の推定における誤差の原因となる。また，ビジュアルオドメトリを用いると，周囲の環境への依存性やコストの高さが問題となる。
【００２７】
以下では，周囲の環境への依存性が小さく，地面の凹凸などの移動環境による位置推定の誤差の補正を可能とすることにより，ロボット１０の位置推定の精度を高めることが可能となる本実施の形態による技術について説明する。
【００２８】
図２は，本実施の形態によるロボットの構成例を示す図である。
【００２９】
ロボット１０は，ＣＰＵ１１，メモリ１２等のハードウェアを備えたコンピュータを有する。また，ロボット１０は，自己位置推定経路生成部１３，マップデータ記憶部１４，運動制御部１５，駆動輪１６，エンコーダ（駆動輪用）１７，センシング処理部１８を備える。自己位置推定経路生成部１３，マップデータ記憶部１４，運動制御部１５，センシング処理部１８等の各機能部は，ロボット１０のコンピュータが備えるＣＰＵ１１，メモリ１２等のハードウェアとソフトウェアプログラムとにより実現される。
【００３０】
自己位置推定経路生成部１３は，ロボット１０の移動速度の情報に基づいて，二次元マップ上のロボット１０の位置を推定し，目標経路を定めて，運動制御部１５に対して目標軌道の指示を送る。なお，ロボット１０の移動にはロボット１０の回転も含まれ，ロボット１０の移動速度にはロボット１０の回転速度も含まれる。
【００３１】
マップデータ記憶部１４は，ロボット１０が移動を行う空間の二次元のマップデータを記憶する，コンピュータがアクセス可能な記憶装置である。二次元マップは，ロボット１０が移動を行う空間における，地面と水平な平面のマップである。なお，地面という表現には，屋外の移動環境における地面だけではなく，屋内の移動環境における床面なども当然に含まれる。
【００３２】
運動制御部１５は，自己位置推定経路生成部１３からの目標軌道の指示に従ってモータ（図示省略）を制御し，駆動輪１６を回転させる。図１に示す左右の駆動輪１６ａ，１６ｂのそれぞれの回転動作によって，ロボット１０は，前進・回転等の移動動作を行う。
【００３３】
エンコーダ（駆動輪用）１７は，駆動輪１６の回転を検知する。エンコーダ（駆動輪用）１７は，駆動輪１６の回転速度から得られるロボット１０の移動速度の情報をセンシング処理部１８に通知する。
【００３４】
ロボット１０には，バネ１０４を介してロータリ装置１０１が取り付けられている。また，ロボット１０には，ポテンショメータ１０５が取り付けられている。
【００３５】
ロータリ装置１０１は，バネ１０４を介してロボット１０に固定された，ロボット１０の移動を計測する装置である。ロータリ装置１０１は，回転部１０２，エンコーダ１０３を備える。ロータリ装置１０１において，エンコーダ１０３は，回転部１０２の回転を検知し，それから得られるロボット１０の地面に対する相対的な移動速度の情報をセンシング処理部１８に送る。ロータリ装置１０１の仕組みについての詳細は，後述する。
【００３６】
バネ１０４は，地面と垂直な方向に伸び縮みする弾性体である。バネ１０４には，プリロード（Preload ）がかけられており，そのプリロードによって，ロータリ装置１０１は地面に接触した状態となっている。プリロードは，バネ１０４に初めからかけられた荷重（Load）のことであり，サスペンションや接触状態の設定として使われる。バネ１０４は，ロータリ装置１０１が接触する地面の凹凸によって，地面と垂直な方向に伸縮する。なお，ロボット１０とロータリ装置１０１とを中継する弾性体として，バネ１０４以外の弾性体を用いてもよい。
【００３７】
ポテンショメータ１０５は，ロータリ装置１０１とロボット１０との相対位置を検知し，その相対位置の情報をセンシング処理部１８に送る。ポテンショメータ１０５により検知されるロータリ装置１０１とロボット１０との相対位置の変化は，ロータリ装置１０１が接触する地面の凹凸を示す情報となる。ポテンショメータ１０５は，回転角や移動量などを電圧に変換する機器・素子である。ポテンショメータ１０５には，回転角度を検出するロータリポテンショメータと，直線上の位置を検出するリニアポテンショメータとがある。
【００３８】
センシング処理部１８は，ロータリ装置１０１のエンコーダ１０３からの移動速度の情報に基づいて，ロボット１０自身の絶対的な移動速度を求める。このとき，センシング処理部１８は，ポテンショメータ１０５からの相対位置の情報とに基づいて，地面の凹凸などの状況により発生する誤差の補正を行う。センシング処理部１８は，得られたロボット１０自身の移動速度の情報を，ロボット１０の自己位置を推定するための情報として，自己位置推定経路生成部１３に送る。
【００３９】
本実施の形態では，回転部１０２とエンコーダ１０３とを含むロータリ装置１０１，バネ１０４，ポテンショメータ１０５，センシング処理部１８が，移動体位置推定支援部１００として機能し，自己位置推定経路生成部１３によるロボット１０の位置推定を支援する。
【００４０】
なお，本実施の形態では，センシング処理部１８は，ロータリ装置１０１のエンコーダ１０３からの移動速度の情報と，ポテンショメータ１０５からの相対位置の情報とに基づいて，ロボット１０自身の移動速度を求めている。センシング処理部１８が，さらに，エンコーダ（駆動輪用）１７からの移動情報や，視覚センサ（図示省略），加速度センサ（図示省略）などの複数のセンサからの情報を統合して，ロボット１０自身の移動速度を求めるようにしてもよい。
【００４１】
図３は，本実施の形態による移動体位置推定支援部の構造を示す図である。
【００４２】
図３に示す例は，ロボット１０の側面方向から見た本実施の形態の移動体位置推定支援部１００の構造をイメージした例である。図３に示す例では，回転部１０２を有するロータリ装置１０１，バネ１０４，ポテンショメータ１０５を組み合わせたユニットが，ロボットに２組取り付けられている。各ユニットにおいて，バネ１０４のロータリ装置１０１と反対側の端は，ロボット１０に固定される。
【００４３】
ロボット１０は，駆動輪１６の回転により推進力を得て移動を行う。回転部１０２は，自らが駆動力を持たず，ロボット１０の移動に応じて，地面との接触から摩擦力を得て回転する。すなわち，ロボット１０の移動に応じて回転部１０２を回転させるためには，回転部１０２と地面とが接触した状態を保つようにする必要がある。
【００４４】
図３に示すように，バネ１０４ａのプリロードによって，ロータリ装置１０１ａの回転部１０２ａは，地面に接触した状態となる。また，バネ１０４ｂのプリロードによって，ロータリ装置１０１ｂの回転部１０２ｂは，地面に接触した状態となる。地面に凹凸がある場合でも，バネ１０４が地面と垂直方向に伸縮し，ロータリ装置１０１の回転部１０２は，地面に接触した状態となり，ロボット１０の移動に合わせて回転する。
【００４５】
ポテンショメータ１０５ａは，ロボット１０とロータリ装置１０１ａとの相対位置関係を検知する。また，ポテンショメータ１０５ｂは，ロボット１０とロータリ装置１０１ｂとの相対位置関係を検知する。バネ１０４のプリロードとポテンショメータ１０５との作用によって，地面の段差・凹凸・溝などによるロータリ装置１０１の上下方向の位置変化を検知することができる。
【００４６】
図４は，本実施の形態によるロータリ装置の内部構造を示す図である。
【００４７】
図４（Ａ）は，ロータリ装置１０１の内部を上方向から地面方向に見たものである。また，図４（Ｂ）は，ロータリ装置１０１の内部を，図４（Ａ）のブロック矢印の方向から横に見たものである。図４において，Ｘm 軸，Ｙm 軸の座標系は，ロータリ装置１０１のローカル座標系である。
【００４８】
ロータリ装置１０１は，回転部１０２と，２つの受動輪１０６と，２つのエンコーダ１０３とを備える。
【００４９】
図４において，回転部１０２は，駆動力がなく，地面と接触し，ロボット１０の移動に合わせて回転する球体である。
【００５０】
受動輪（Ｘm 軸方向）１０６ｘは，回転部１０２と接触し，回転部１０２のＸm 軸方向の回転に合わせて回転する受動輪１０６である。エンコーダ（Ｘm 軸方向）１０３ｘは，受動輪（Ｘm 軸方向）１０６ｘの回転速度から，ロボット１０の地面に対するＸm 軸方向の相対移動速度ｖmxを検知する。受動輪（Ｙm 軸方向）１０６ｙは，回転部１０２と接触し，回転部１０２のＹm 軸方向の回転に合わせて回転する受動輪１０６である。エンコーダ（Ｙm 軸方向）１０３ｙは，受動輪（Ｙm 軸方向）１０６ｙの回転から，回転部１０２の地面に対するＹm 軸方向の相対移動速度ｖmyを検知する。
【００５１】
２つのエンコーダ１０３により得られるロボット１０の地面に対する相対移動速度（ｖmx，ｖmy）は，駆動力を持たず，ロボット１０の移動によって回転する回転部１０２の回転から得られる，純粋なロボット１０の移動速度の情報である。すなわち，２つのエンコーダ１０３により検知されるＸm 軸方向の相対移動速度ｖmxとＹm 軸方向の相対移動速度ｖmyとを成分とする相対移動速度ｖm とその移動方向は，駆動輪１６の回転とは関係なく，まさにその時点でロボット１０が移動している移動速度と移動方向とを示している。
【００５２】
このように，ロータリ装置１０１では，２つのエンコーダ１０３により，ロボット１０の地面との相対移動量（Ｘm 軸，Ｙm 軸両方向）を検知することができる。
【００５３】
図５は，本実施の形態によるユニットの配置の例を示す図である。
【００５４】
図５（Ａ）は，ロボット１０に対してロータリ装置１０１を含む１組のユニットが配置された例を示す。ここでは，ロータリ装置１０１は，図１に示すロボット１０の回転中心を通るＸR 軸上の，駆動輪１６よりもロボット１０の進行方向前の，回転中心から距離ｌm だけ離れた位置に，設置されるものとする。図５（Ａ）には，ロボット１０に設置されたロータリ装置１０１が備える回転部１０２が図示されている。
【００５５】
本実施の形態では，センシング処理部１８は，エンコーダ１０３により検知されるロボット１０の地面に対する相対移動速度（ｖmx，ｖmy）から，ロボット１０自身の絶対移動速度を求める。本実施の形態のロボット１０は，差動駆動型のロボット１０であるので，求めるロボット１０自身の絶対移動速度は，前進速度ｖと回転速度ωとなる。
【００５６】
例えば，図５（Ａ）に示す例において，ロボット１０の前進速度ｖは，
ｖ＝ｖmx
となる。また，ロボット１０の回転速度ωは，
ω＝ｖmy／ｌm 
となる。
【００５７】
図５（Ｂ）は，ロボット１０に対してロータリ装置１０１を含む２組のユニットが配置された例を示す。ここでは，２つのロータリ装置１０１は，図３の例に示すように，駆動輪１６の前後の位置に配置されるものとする。すなわち，ロータリ装置１０１ａは，図１に示すロボット１０の回転中心を通るＸR 軸上の，駆動輪１６よりもロボット１０の進行方向前の，回転中心から距離ｌ1 だけ離れた位置に，設置されるものとする。また，ロータリ装置１０１ｂは，図１に示すロボット１０の回転中心を通るＸR 軸上の，駆動輪１６よりもロボット１０の進行方向後ろの，回転中心から距離ｌ2 だけ離れた位置に，設置されるものとする。図５（Ｂ）には，ロボット１０に設置された，ロータリ装置１０１ａが備える回転部１０２ａと，ロータリ装置１０１ｂが備える回転部１０２ｂとが図示されている。
【００５８】
本実施の形態では，センシング処理部１８は，ロータリ装置１０１ａで得られる相対移動速度（ｖmx1 ，ｖmy1 ）と，ロータリ装置１０１ｂで得られる相対移動速度（ｖmx2 ，ｖmy2 ）とを統合し，ロボット１０自身の絶対移動速度を求める。
【００５９】
本実施の形態のロボット１０は，差動駆動型のロボット１０であるので，求めるロボット自身の絶対移動速度は，前進速度ｖと回転速度ωとなる。ロボット１０の前進速度ｖは，２つのロータリ装置１０１の出力を統合することにより推定される。ロボット１０の回転速度ωは，２つのロータリ装置１０１の出力の差分により推定される。なお，ここでのロータリ装置１０１の出力とは，エンコーダ１０３により得られたロボット１０の地面に対する相対移動速度である。
【００６０】
例えば，図５（Ｂ）に示す例において，ロボット１０の前進速度ｖは，
ｖ＝（ｖmx1 ＋ｖmx2 ）／２
となる。また，ロボット１０の回転速度ωは，
ω＝ｖmy1 ／ｌ1 ＋ｖmy2 ／ｌ2 
となる。
【００６１】
センシング処理部１８が，ロータリ装置１０１の出力ごとに重み付けによる調整を行い，それらの各重み付きのロータリ装置１０１の出力を統合して，ロボット１０自身の移動速度を求めるようにしてもよい。図５（Ｂ）に示す例では，重み係数を用いた統合の式は，例えば，次の式（１）のようになる。
【００６２】
ｖ＝（α1 ・ｖmx1 ＋α2 ・ｖmx2 ）／２，
ω＝α1 ・ｖmy1 ／ｌ1 ＋α2 ・ｖmy2 ／ｌ2         ・・・（１）
式（１）において，α1 ，α2 は，それぞれロータリ装置１０１ａの出力に対する重み係数，ロータリ装置１０１ｂの出力に対する重み係数である。重み係数α1 ，α2 は，動的な調整が可能である。
【００６３】
例えば，重み係数α1 ，α2 のデフォルト値を１とする。図５（Ｂ）において，ある時点で，ロータリ装置１０１ａの出力が明らかに異常であったものとする。このとき，ロータリ装置１０１ａの出力とロータリ装置１０１ｂの出力とを単純に統合してロボット１０自身の移動速度を求めると，ロボット１０による自己位置推定の精度が落ちてしまう。このような場合に，例えば，明らかに異常なロータリ装置１０１ａの出力に対する重み係数α1 を０にするなどして調整を行うことにより，ロボット１０による自己位置推定の精度の低下を防止することができる。
【００６４】
なお，図５（Ｂ）ではロボット１０に２つのロータリ装置１０１を取り付けた例を示したが，ロボット１０に３つ以上のロータリ装置１０１を取り付け，それらのロータリ装置１０１の出力を統合して，ロボット１０自身の移動速度を求めるようにしてもよい。複数のロータリ装置１０１の出力を統合する式は，各ロータリ装置１０１の配置位置によって異なる。複数のロータリ装置１０１の出力を統合する式は，各ロータリ装置１０１の配置位置に応じて，幾何学的に求められる。
【００６５】
センシング処理部１８は，得られたロボット１０自身の移動速度の情報を自己位置推定経路生成部１３に送る。
【００６６】
自己位置推定経路生成部１３は，センシング処理部１８からの移動速度の情報に基づいて，図１に示すロボット１０の位置と姿勢（ｘG ，ｙG ，θG ）を更新することにより，自己位置の推定を行う。
【００６７】
なお，ロボット１０にロータリ装置１０１を含むユニットを複数取り付け，その複数のロータリ装置１０１の出力を統合して，ロボット１０自身の移動速度を求めることにより，より精度が高いロボット１０の自己位置の推定が可能となる。
【００６８】
図６は，本実施の形態において，駆動輪が溝に落ちてスリップした場合の例を示す図である。
【００６９】
図６に示すように，ある時点でロボット１０の駆動輪１６が溝に落ち，駆動輪１６は回転しているもののスリップして空回りしており，ロボット１０自身の移動がストップしてしまっているものとする。
【００７０】
まず，本実施の形態のロータリ装置１０１がなく，センシング処理部１８が，エンコーダ（駆動輪用）１７からの移動速度の情報に基づいて，ロボット１０の移動速度を求める場合を考える。
【００７１】
エンコーダ（駆動輪用）１７は，スリップしている駆動輪１６の回転を検知してしまい，駆動輪１６の回転から検知される移動速度の情報がセンシング処理部１８に送られる。センシング処理部１８は，エンコーダ（駆動輪用）１７により駆動輪１６の回転から得られた移動速度の情報に基づいて，ロボット１０自身の移動速度を推定する。そのため，自己位置推定経路生成部１３は，実際にはロボット１０が移動していないのに，ロボット１０が移動しているものとして自己位置の推定を行ってしまう。すなわち，駆動輪１６のスリップによって，ロボット１０の自己位置推定に誤差が生じる。
【００７２】
次に，本実施の形態のロータリ装置１０１がロボット１０に取り付けられており，センシング処理部１８が，ロータリ装置１０１の出力に基づいて，ロボット１０の移動速度を求める場合を考える。
【００７３】
図６に示すように，駆動輪１６が回転していても，ロボット１０が移動していなければ，ロータリ装置１０１の回転部１０２は回転しないため，ロータリ装置１０１のエンコーダ１０３は，回転部１０２の回転を検知しない。センシング処理部１８は，ロータリ装置１０１の出力に基づいて，ロボット１０自身の移動速度を０と推定し，自己位置推定経路生成部１３は，ロボット１０が移動していないと推定する。
【００７４】
このように，本実施の形態のロータリ装置１０１の出力からロボット１０自身の移動速度を求めて，ロボット１０の位置推定を行うと，駆動輪１６がスリップしても，ロボット１０の位置推定に誤差が生じない。
【００７５】
図７は，本実施の形態において，地面の凹凸により発生する誤差を補正する例を説明する図である。
【００７６】
駆動輪１６の回転速度からロボット１０の移動速度を推定し，その移動速度を使用してロボット１０の自己位置の推定を行う場合には，地面に凹凸があるようなときに，駆動輪１６の回転量から２次元のマップ上で定義された水平移動量への補正ができない。そのため，ロボット１０の自己位置の推定に誤差が生じてしまう。
【００７７】
本実施の形態では，ロータリ装置１０１とロボット１０との間にバネ１０４が取り付けられており，バネ１０４は，ロータリ装置１０１が接触する地面の凹凸に応じて伸縮する。ポテンショメータ１０５は，バネ１０４の伸縮によって変化するロボット１０とロータリ装置１０１との相対位置を計測し，その情報をセンシング処理部１８に出力する。地面に凹凸があれば，ロボット１０とロータリ装置１０１との相対位置関係が変化する。センシング処理部１８は，ポテンショメータ１０５の出力を監視し，地面に凹凸があると判断された場合には，回転部１０２の移動量から水平移動量への補正を行う。以下，図７を用いて，具体的な例を説明する。
【００７８】
図７（Ａ）は，ロータリ装置１０１が地面に凹凸がある部分を移動する例を示す。なお，図７（Ａ）では，図面を見やすくするために，ロータリ装置１０１を省略して，ロータリ装置１０１内部の回転部１０２とバネ１０４とで，ロータリ装置１０１が地面に凹凸がある部分を移動する例を表している。
【００７９】
図７（Ａ）に示すように，ロボット１０の移動に応じて，地面に接触する回転部１０２が回転し，その回転からロボット１０の地面に対する相対移動速度ｖm が検知されているものとする。図７（Ａ）に示すように，この速度ｖm で，ロータリ装置１０１が，地面の凹凸部分を移動するものとする。
【００８０】
図７（Ａ）において，ｈ0 は，ポテンショメータ１０５の定常出力である。すなわち，ｈ0 は，地面に凹凸がない場合のロボット１０とロータリ装置１０１との相対位置を表している。
【００８１】
センシング処理部１８は，ポテンショメータ１０５の出力を監視し，所定の閾値εを超える出力の変化が検知され，出力が変化し続ける場合には，ロータリ装置１０１の回転部１０２が接触する地面に凹凸があると判断する。このとき，センシング処理部１８は，ポテンショメータ１０５の出力の変化が所定の閾値εを超えた時刻ｔ0 を記録する。
【００８２】
センシング処理部１８は，二段直線で地面の凹凸を近似モデルにし，ポテンショメータ１０５の出力を利用して，次の式（２）と式（３）とを用いて，ロータリ装置１０１により検出された移動量から水平移動量への補正を行う。
【００８３】
【数１】
2011075369.tif 000003
【００８４】
図７（Ｂ）は，式（２）におけるΔｈとΔＳとθとの関係を示す。図７（Ｂ）に示すように，Δｈは，図７（Ａ）におけるポテンショメータ１０５の出力ｈ0 とｈとから求められる。また，図７（Ｂ）に示すように，ΔＳは，ロータリ装置１０１により検知された地面に対する相対移動速度ｖm と，時刻ｔ0 ，時刻ｔとから求められる。
【００８５】
式（３）において，ｖmx’は，地面の凹凸による誤差が補正されたロボット１０の地面に対する相対移動速度ｖmxである。
【００８６】
このように，本実施の形態のロータリ装置１０１にバネ１０４とポテンショメータ１０５とを用いた仕組みによって，地面の凹凸による誤差を補正することができる。
【００８７】
図８は，本実施の形態において，ロータリ装置の回転部が溝に落ちた例を説明する図である。
【００８８】
ここでは，ロボット１０に，ロータリ装置１０１，バネ１０４，ポテンショメータ１０５を含むユニットが，複数取り付けられているものとする。
【００８９】
なお，図８では，図面を見やすくするために，ロータリ装置１０１を省略して，ロータリ装置１０１内部の回転部１０２とバネ１０４とで，ロータリ装置１０１が地面に溝がある部分を移動する例を表している。
【００９０】
図８に示すように，ロータリ装置１０１の回転部１０２が溝に落ちた場合には，その前後のロータリ装置１０１の出力が不安定なものになってしまう。このような不安定なロータリ装置１０１の出力をそのまま用いてロボット１０自身の移動速度を推定すると，ロボット１０の自己位置の推定に誤差が生じてしまう。
【００９１】
本実施の形態では，センシング処理部１８は，ポテンショメータ１０５の出力を監視し，所定の閾値εを超える出力の変化が検知され，その出力の変化が続かない場合には，ロータリ装置１０１の回転部１０２が地面の溝に落ちたと判断する。このとき，センシング処理部１８は，溝に落ちたと判断されたロータリ装置１０１の出力に対する重み係数を調整し，不安定なロータリ装置１０１の出力による誤差を回避する。
【００９２】
例えば，上記の式（１）において，重み係数α1 ，α2 のデフォルト値を１とする。図５（Ｂ）において，ポテンショメータ１０５の出力から，ロータリ装置１０１ａが溝に落ちたと判断されたものとする。このとき，溝に落ちたと判断されたロータリ装置１０１ａの出力に対する重み係数α1 を０に調整することにより，ロボット１０による自己位置推定の精度の低下を防止することができる。
【００９３】
このように，本実施の形態のロータリ装置１０１にバネ１０４とポテンショメータ１０５とを用いた仕組みと，センシング処理部１８によるロータリ装置１０１の出力に対する重み付けとによって，回転部１０２が溝に落ちたときの誤差を回避することができる。
【００９４】
図９は，本実施の形態のセンシング処理部による移動速度算出処理の流れの例を示す図である。
【００９５】
ここでは，図５（Ｂ）に示す，ロータリ装置１０１を含む２組のユニットが取り付けられたロボット１０におけるセンシング処理部１８の処理例を説明する。
【００９６】
まず，本実施の形態の移動体位置推定支援部１００によるロボット１０の移動速度の計測を開始する前に，ロータリ装置１０１ａ，１０１ｂの配置決めや，その座標系の設定，閾値εや定常値ｈ0 等のパラメータの設定などを済ませておく。
【００９７】
センシング処理部１８は，各ロータリ装置１０１の出力，すなわちエンコーダ１０３により検知されたロボット１０の地面に対する相対移動速度の情報を取得する（ステップＳ１０）。
【００９８】
センシング処理部１８は，各ポテンショメータ１０５の出力，すなわちロボット１０と各ロータリ装置１０１との相対位置の情報を取得する（ステップＳ１１）。
【００９９】
センシング処理部１８は，それぞれのポテンショメータ１０５について，その出力の変化が所定の閾値εを超えているかを判定する（ステップＳ１２）。
【０１００】
各ポテンショメータ１０５の出力の変化が所定の閾値εを超えていなければ（ステップＳ１２のＮＯ），センシング処理部１８は，地面に凹凸や溝などがないと判断する。センシング処理部１８は，各ロータリ装置１０１の出力をそのまま統合して，ロボット１０の前進速度ｖと回転速度ωを求める（ステップＳ１３）。センシング処理部１８は，結果として得られたロボット１０の前進速度ｖと回転速度ωとの情報を，自己位置推定経路生成部１３に出力する（ステップＳ１４）。
【０１０１】
いずれかのポテンショメータ１０５の出力の変化が所定の閾値εを超えていれば（ステップＳ１２のＹＥＳ），センシング処理部１８は，そのポテンショメータ１０５の出力が変化し続けているかを判定する（ステップＳ１５）。
【０１０２】
ポテンショメータ１０５の出力が変化し続けていれば（ステップＳ１５のＹＥＳ），センシング処理部１８は，該当ロータリ装置１０１の回転部１０２が接触する地面に凹凸があると判断する。センシング処理部１８は，上記の式（２），式（３）を用いて該当ロータリ装置１０１の出力を補正する（ステップＳ１６）。センシング処理部１８は，補正されたロータリ装置１０１の出力を用いて，各ロータリ装置１０１の出力を統合して，ロボット１０の前進速度ｖと回転速度ωを求める（ステップＳ１７）。センシング処理部１８は，結果として得られたロボット１０の前進速度ｖと回転速度ωとの情報を，自己位置推定経路生成部１３に出力する（ステップＳ１４）。
【０１０３】
ポテンショメータ１０５の出力が変化し続けていなければ（ステップＳ１５のＮＯ），センシング処理部１８は，該当ロータリ装置１０１の回転部１０２が溝に落ちたと判断する。センシング処理部１８は，該当ロータリ装置１０１の出力に対する重み係数αを調整する（ステップＳ１８）。センシング処理部１８は，上記の式（１）を用いて，各ロータリ装置１０１の出力に重み付けした統合を行い，ロボット１０の前進速度ｖと回転速度ωを求める（ステップＳ１９）。センシング処理部１８は，結果として得られたロボット１０の前進速度ｖと回転速度ωとの情報を，自己位置推定経路生成部１３に出力する（ステップＳ１４）。
【０１０４】
以上説明したように，本実施の形態の移動体位置推定支援部１００により，駆動輪１６のスリップや地面の凹凸による誤差を回避することができ，ロボット１０の位置推定の精度を高めることができる。また，ロータリ装置１０１，バネ１０４，ポテンショメータ１０５のユニットは，ビジュアルオドメトリと比較してもローコストであり，周囲の環境への依存性も少ない。
【０１０５】
以上，本実施の形態について説明したが，本発明はその主旨の範囲において種々の変形が可能であることは当然である。
【０１０６】
例えば，本実施の形態では，移動体として差動駆動型のロボット１０を例に説明を行っている。移動体が全方位移動型のロボットであっても，同様に，本実施の形態による移動体位置推定支援部１００の技術の適用が可能である。
【符号の説明】
【０１０７】
１０  ロボット
１１  ＣＰＵ
１２  メモリ
１３  自己位置推定経路生成部
１４  マップデータ記憶部
１５  運動制御部
１６  駆動輪
１７  エンコーダ（駆動輪用）
１８  センシング処理部
１００  移動体位置推定支援部
１０１  ロータリ装置
１０２  回転部
１０３  エンコーダ
１０４  バネ
１０５  ポテンショメータ
１０６  受動輪
【図１】
2011075369.tif 000004
【図２】
2011075369.tif 000005
【図３】
2011075369.tif 000006
【図４】
2011075369.tif 000007
【図５】
2011075369.tif 000008
【図６】
2011075369.tif 000009
【図７】
2011075369.tif 000010
【図８】
2011075369.tif 000011
【図９】
2011075369.tif 000012