JP2010146202A - 移動体と移動体の位置推定方法 - Google Patents

Abstract

【課題】 より確実に検知可能なランドマークを用いることで、移動体の位置推定の補正を確実に行うことができる技術を提供する。
【解決手段】可動領域内を走行する移動体は、制御ユニット１０と加速度センサ６４等を備えている。加速度センサ６４は、移動体の上下方向の加速度を検知する。制御ユニット１０は、メモリ１２とＣＰＵ１１を備えている。メモリ１２は、可動領域内の段差の位置を記憶している。ＣＰＵ１１は、加速度センサ６４の出力から移動体が段差を通過したことを検知する段差特定部１６と、段差特定部１６で移動体が段差を通過したことを検知した場合において、段差通過時の移動体の推定位置にメモリ１２に記憶されている段差が存在するときに、メモリに記憶されている当該段差の位置から移動体の推定位置を補正する位置補正部２０を備える。
【選択図】 図４

Description

本発明は、可動領域内を走行する移動体の位置を推定する技術に関する。

移動体の位置を推定する技術が開発されている。移動体の位置は、例えば、ＧＰＳ等のセンサによって推定することができ、あるいは、車輪走行型の移動体の場合には車輪の回転数を積算することによって推定（いわゆるオドメトリによる位置推定）することができる。このような手法によって推定された移動体の推定位置には、誤差が含まれている。そこで、移動体の推定位置を補正する技術が開発されている（例えば、特許文献１）。

特許文献１の移動体は、移動体が移動できる可動領域の地図を記憶している。地図には、予めランドマーク（即ち、ランドマークの絶対位置）が記憶されている。移動体は、移動体に設置された画像センサやレーザレンジファインダ等によってランドマークを検知可能となっている。移動体がランドマークを検知すると、推定される自己の位置（移動体の推定位置）と移動体に対するランドマークの相対位置とから、ランドマークの絶対位置を推定する。推定されたランドマークの絶対位置と、地図上のランドマークの絶対位置とを照合することによって、推定された自己の位置を補正する。

ランドマークとしては、例えば、部屋の壁等の板状の物体や、反射板、物体の角等が利用される。移動体は、画像センサやレーザレンジファインダを用いて、上記したランドマークを非接触で検知している。

特開２００８−１６５２７５号公報

画像センサやレーザレンジファインダを用いてランドマークを非接触で検知しようとすると、ランドマークを検知することが難しい場合がある。例えば、レーザレンジファインダでランドマークを検知する場合、レーザレンジファインダとランドマークとの間に人間や移動する物体が存在すると、レーザレンジファインダから発射されたレーザが遮られて、ランドマークを正確に検知することができなくなる。また、画像センサによってランドマークを検知する場合、照明等の光の変化によって、ランドマークが検知できなくなることがある。ランドマークが検知できなければ、移動体の位置を補正することができない。

本発明は上述した事情を鑑みてなされたものであり、より確実に検知可能なランドマークを用いることで、移動体の推定位置の補正を確実に行うことができる技術を提供することを目的とする。

本発明は、可動領域内を走行する移動体に関する。本発明の移動体は、記憶手段と段差検知手段と位置補正手段を備える。記憶手段は、可動領域内の段差の位置を記憶している。記憶手段は、例えば、段差の中心位置と伸延方向と長さを記憶していてもよい。或いは、段差の伸延方向に沿って断続的に段差の位置を記憶していてもよい。段差検知手段は、移動体が段差を通過したことを検知する。位置補正手段は、段差検知手段で移動体が段差を通過したことを検知した場合において、段差通過時の移動体の推定位置から所定範囲内に記憶手段に記憶されている段差が存在するときに、記憶手段に記憶されている当該段差の位置から移動体の推定位置を補正する。なお、所定範囲は、移動体の推定位置の精度や段差検知手段の検知性能に基づいて、設計者や使用者が予め決定することができる。また、移動体の推定位置は、例えば、オドメトリによる推定位置であってもよく、ＧＰＳ等のセンサを用いて推定した推定位置であってもよい。

例えば、人工的に構成された空間（事務所や工場等）には、各部屋間の仕切りやじゅうたん等の敷物の境界等に段差が存在する。本発明の移動体は、これらの段差をランドマークとして利用する。即ち、本発明の移動体は、実際に接触して通過した段差をランドマークとして利用する。この構成によれば、壁や廊下の角等をランドマークとして利用する場合と比較して、移動体がランドマークをより確実に検知することができる。これにより、ランドマークを利用した推定位置の補正をより確実に行うことができる。

人工的に構成された空間に存在する段差は、直線であることが多い。発明者は、通過した段差の伸延方向と、当該段差に対する移動体の進行方向（段差通過方向）の関係から、移動体の推定進行方向を補正することができることを見出した。この移動体は、移動体の進行方向に沿って見たときに重ならない位置に配置されている少なくとも２つの車輪を備えていることが好ましい。段差検知手段は、車輪毎に段差を通過したことを検知可能であることが好ましい。記憶手段は、可動領域内の段差の伸延方向をさらに記憶していることが好ましい。位置補正手段は、段差検知手段で移動体が段差を通過したことを検知した場合において、段差通過時の移動体の推定位置から所定範囲内に記憶手段に記憶されている段差が存在するときに、（１）記憶手段に記憶されている当該段差の伸延方向と、（２）段差検知手段の検知結果とから移動体の推定進行方向をさらに補正することが好ましい。
この移動体によれば、移動体の推定位置だけでなく、推定進行方向も補正することができる。

移動体の段差通過方向は、移動体の推定進行方向と段差検知手段の検知結果とから、様々な手法を用いて特定することができる。例えば、段差検知手段が段差を通過した車輪の順序及び各車輪の段差を通過したタイミングを検知し、これらから移動体の段差通過方向（即ち、当該段差を移動体がどの方向に通過したか）を特定することができる。この場合、位置補正手段は、（１）記憶手段に記憶されている当該段差の伸延方向と、（２）段差検知手段で検知された「段差通過時の車輪順序及び各車輪の段差通過タイミング」から推定される移動体の段差通過方向とを用いて、移動体の推定進行方向をさらに補正してもよい。
段差を通過した車輪の順序及び各車輪の段差を通過したタイミングが分かると、移動体から見た段差の通過方向を特定することができる。例えば、移動体が、移動体の左右両側（移動体の直進方向に垂直な方向に伸びる１本の軸線（車軸）上）に１対の車輪を備えているとする。移動体が段差の伸延方向に対して垂直に通過した場合、一対の車輪は同時に段差を通過する。一方において、移動体が段差の伸延方向に対して斜めに通過した場合、一方の車輪（例えば左車輪）が先に段差を通過し、他方の車輪（例えば右車輪）が後で段差を通過する。移動体の段差通過方向が変化すると、左車輪が段差を通過してから右車輪が段差を通過するまでの時間が変化する。段差通過時の移動体の速度は測定できるため、両車輪の段差通過のタイミングの差（時間差）が分かれば、その時間差と移動体の速度から移動体から見た段差通過方向を特定することができる。移動体から見た段差通過方向を特定できると、その特定した段差通過方向と移動体の推定進行方向から段差の伸延方向を推定することができる。位置補正手段は、このように推定される段差伸延方向と地図上に記憶された段差伸延方向とを用いて、移動体の推定進行方向を補正する。この移動体によれば、移動体の推定位置だけでなく、推定進行方向も補正することができる。

この移動体では、段差検知手段は、移動体の上下方向の加速度を検知する加速度センサと、移動体の傾きを検知するジャイロセンサを有することが好ましい。
移動体が段差を通過するとき、移動体の上下方向の加速度が急激に変化する。従って、移動体の上下方向の加速度を検知することによって、移動体が段差を通過したことを検知することができる。また、移動体の段差通過方向によって、移動体の傾く方向と傾くタイミング（時点）が異なる。例えば、段差の伸延方向に対する移動体の進行方向が垂直の場合、移動体は、進行方向に垂直な方向には傾かない。また、段差の伸延方向に対する移動体の進行方向が異なれば、移動体がいずれかの方向に傾いてからその傾きが戻るまでのタイミングが異なる。このことから、移動体の傾く方向とそのタイミングを検知することによって、移動体の段差通過方向を特定することができる。この移動体によれば、複雑な検知装置を利用することなく、確実にランドマークを検知することができる。

上記した移動体では、段差通過時の移動体の推定位置から所定範囲内に記憶手段に記憶されている段差が存在しない場合に、段差通過時の移動体の推定位置を当該段差の位置とし、移動体の推定進行方向と段差検知手段の検知結果とから特定される当該段差の推定伸延方向を当該段差の伸延方向として記憶手段に登録する登録手段をさらに備えることが好ましい。
ここで、段差通過時の移動体の推定位置に記憶手段に記憶されている段差が存在しないか否かの判断は、移動体の推定位置の推定精度や段差検知手段の検知性能による誤差を考慮して、段差が存在するか否かを判断することができる。上記の移動体によれば、移動体の記憶手段に記憶されている地図の内容を自動的に更新することができる。

可動領域内の段差の位置を記憶している記憶手段と、段差を通過したことを検知する段差検知手段とを有する移動体が可動領域内を走行する際の移動体の位置を推定する方法も有用である。この位置推定方法は、位置推定工程と位置補正工程を備える。位置推定工程では、段差検知手段で移動体が段差を通過したことを検知したときの移動体の位置を指定する。位置補正工程では、推定された移動体の推定位置から所定範囲内に記憶手段に記憶されている段差が存在するときに、記憶手段に記憶されている当該段差の位置から移動体の推定位置を補正する。
この位置推定方法によれば、移動体は、実際に通過した段差をランドマークとして用いて、移動体の推定位置を補正する。このため、より確実にランドマークが検知され、移動体の推定位置の補正を確実に行うことができる。

本発明によると、自己の推定位置を補正するためのランドマークをより確実に検知することができる。これにより、移動体は、推定位置の補正をより確実に行うことができる。

以下に、本実施例の主たる形態を列記する。
（形態１）記憶手段に記憶されている段差の位置及び／又はその段差の伸延方向には、誤差情報が含まれていてもよい。誤差情報は、設計者又は使用者が予め記憶手段に記憶させることができる。
（形態２）移動体の推定位置及び／又は推定進行方向には、誤差情報が含まれていてもよい。位置補正手段は、記憶手段に記憶されている段差に関する各情報から、移動体の推定位置及び／又は推定進行方向の誤差情報を補正することができる。
（形態３）登録手段は、段差の位置及び／又はその段差の伸延方向を登録する場合、その誤差情報も登録してもよい。誤差情報は、例えば、移動体の推定位置及び／又は推定進行方向に含まれる誤差情報と、段差検知手段の検知精度によって決められる誤差情報とを加算することによって算出することができる。
（形態４）位置補正手段は、例えば、記憶手段に記憶されている段差の中心位置を用いて、移動体の推定位置を補正することができる。移動体の推定位置や記憶手段に記憶されている段差の位置に誤差情報が含まれている場合、位置補正手段は、例えば、カルマンフィルタ等の手法を用いて移動体の推定位置を補正してもよい。
（形態５）位置補正手段は、例えば、（１）記憶手段に記憶されている段差の伸延方向と、（２）段差検知手段で検知された「段差通過時の車輪順序及び各車輪の段差通過タイミング」と、「段差通過時の移動体の速度」とから推定される移動体の段差通過方向を用いて、移動体の推定進行方向を補正することができる。
（形態６）位置補正手段は、例えば、（１）記憶手段に記憶されている段差の伸延方向と、（２）段差検知手段で検知された各車輪の段差通過時の移動体の推定位置から推定される移動体の段差通過方向とを用いて、移動体の推定進行方向を補正することができる。
（形態７）位置補正手段は、例えば、（１）記憶手段に記憶されている段差の伸延方向と、（２）段差検知手段で検知された各車輪の段差通過時の各車輪の推定位置から推定される移動体の段差通過方向とを用いて、移動体の推定進行方向を補正することができる。
（形態８）位置補正手段は、例えば、記憶手段に記憶されている段差の伸延方向と、移動体の段差通過方向及び移動体の推定進行方向とから推定される段差の推定伸延方向とが一致するように、移動体の推定進行方向を補正することができる。
（形態９）移動体の推定進行方向等に誤差情報が含まれている場合、位置補正手段は、例えば、カルマンフィルタ等の手法を用いて移動体の推定進行方向を補正してもよい。

本発明を具現化した実施例に係る移動体を図面を参照して説明する。図１は移動体５０の平面図である。図２は移動体５０の駆動系統を示す概略構成図である。図３は移動体５０の電気系統を示すブロック図である。移動体５０は、本体５２と、４つの車輪５４ａ、５４ｂ、５６ａ、５６ｂと、車軸５４ｃ、５６ｃと、制御ユニット１０と、モータ５７と、モータ５８を備えている。車軸５４ｃは、本体５２の前方に回転可能に取付けられている。車軸５４ｃは、本体５２の軸方向（移動体５０の直進方向）に対して垂直方向に配置されている。なお、以下の説明では、車軸５４ｃの長手方向の長さをＷとする。車軸５４ｃの左端（図２の左端、以下同様）には、車輪５４ａが取り付けられている。車軸５４ｃの右端（図２の右端、以下同様）には、車輪５４ｂが取り付けられている。車輪５４ａ，５４ｂは、車軸５４ｃに対して傾動可能となっている。

車輪５４ａ、５４ｂは、軸５７ｂを介してモータ５７に接続されている。モータ５７の回転軸５７ａには、ピニオン（図示省略）が形成されている。回転軸５７ａのピニオンは、軸５７ｂに形成されているラック（図示省略）に係合している。モータ５７は回転軸５７ａを回転させることによって、車輪５４ａ、５４ｂの本体５２に対する角度を変えることができる。即ち、モータ５７を駆動することで、移動体５０の進行方向を変えることができる。モータ５７には、エンコーダ６０（図３参照）が取り付けられている。エンコーダ６０は、回転軸５７ａの回転角度を検知して、制御ユニット１０に送信する。

車軸５６ｃは、本体５２の後方に回転可能に取付けられている。車軸５６ｃは、本体５２の軸方向に対して垂直方向に配置されている。車軸５６ｃの左端には、車輪５６ａが固定されている。車軸５６ｃの右端には、車輪５６ｂが固定されている。車軸５６ｃの中央には、モータ５８が取り付けられている。車輪５６ａ、５６ｂは、モータ５８の駆動によって、車軸５６ｃを中心に回転する。モータ５８には、エンコーダ６２（図３参照）が取り付けられている。エンコーダ６２は、モータ５８の回転数を検知して、制御ユニット１０に送信する。

図３に示すように、制御ユニット１０は、加速度センサ６４とジャイロセンサ６６とモータ５７とモータ５８とエンコーダ６０、６２に接続されている。加速度センサ６４とジャイロセンサ６６は、移動体５０に取り付けられている。加速度センサ６４は、移動体５０の上下方向の加速度を検知する。ジャイロセンサ６６は、移動体５０が左右方向に傾くときの角速度を検知する。

図４は、移動体５０の制御系統の機能ブロック図である。制御ユニット１０は、ＣＰＵ１１とメモリ１２と駆動回路２６，２８を備えている。ＣＰＵ１１は、位置推定部１４と段差特定部１６と照合部１８と位置補正部２０と段差登録部２２と制御指令値算出部２４の機能を有している。各部１４，１６，１８，２０，２２，２４の機能については、後述する。メモリ１２は、移動体５０の可動領域の地図を記憶している。可動領域は、移動体５０が移動することを予定されている領域であり、設計者・使用者等によって予め設定されている。図５に示すように、メモリ１２に記憶される地図は、予め決められた地点（例えば、地図上の左下端）を原点とするＸＹ座標で規定されている。地図には、可動領域内に存在する段差Ｓの中央位置（Ｘ，Ｙ）、段差Ｓの伸延方向（段差のＸ軸に対する角度γ）及び段差Ｓの長さＬが記憶されている。また、メモリ１２は、車軸５４ｃの長さＷと、本体５２に対する車輪５４ａ、５４ｂのそれぞれの位置を記憶している。また、メモリ１２は、段差Ｓの中央位置、伸延方向、長さのそれぞれについて、誤差分散を記憶している。メモリ１２に記憶されている内容は、設計者、使用者等が予めメモリ１２に登録しておく。

位置推定部１４には、エンコーダ６０からの出力、即ち、モータ５７の回転軸５７ａの回転角度が入力される。位置推定部１４は、エンコーダ６２からの出力、即ち、モータ５８の回転数が入力される。位置推定部１４は、回転軸５７ａの回転角度とモータ５８の回転数とから、移動体５０の推定位置と推定進行方向を算出する（即ち、オドメトリによる位置推定を行う）。移動体５０の推定位置は、本体５２の中心の位置とされる。本体５２の中心の位置が推定されると、移動体５０の寸法は既知であるため、移動体５０の各部（例えば、車軸５４ｃの中央の位置）を推定できる。位置推定部１４は、推定位置と推定進行方向の誤差分散を算出する。誤差分散は、エンコーダ６０、６２による計測誤差とオドメトリによる位置推定の積算誤差に基づいて算出される。位置推定の積算誤差とは、例えば、位置推定部１４にエンコーダ６０、６２から断続的に計測値が入力される場合、それらの値を積算すると、実際の移動体５０の位置に対して誤差が生じる。即ち、エンコーダ６０、６２の計測値は誤差を有しているため、これらの計測値を積算して算出する移動体５０の推定位置は、実際の移動体５０の位置と相違する。また、積算誤差は、エンコーダ６０、６２から計測値が入力される時間間隔が大きくなるに従って大きくなる。

段差特定部１６には、加速度センサ６４とジャイロセンサ６６からの信号が入力される。段差特定部１６は、（１）加速度センサ６４の出力及びジャイロセンサ６６の出力から、車輪５４ａ，５４ｂ，５６ａ，５６ｂのいずれの車輪が段差を通過したか、また、各車輪５４ａ，５４ｂ，５６ａ，５６ｂが段差を通過したタイミング（時点）を特定し、（２）特定された結果（段差を通過した車輪及び通過タイミング）と、位置推定部１４で推定された移動体の推定位置及び推定進行方向とから、段差の位置と伸延方向を特定する。まず、段差特定部１６の上記（１）の作用について、図６を参照して具体的に説明する。

図６は、移動体５０が図１に示す状態から図７に示す状態に移動する場合（即ち、段差１００を通過する場合）における加速度センサ６４とジャイロセンサ６６の出力の変化を説明するためのグラフである。図１は、移動体５０の左前車輪５４ａが最初に段差１００を通過した状態を示している。図７は、移動体５０が図１の状態から矢印の方向に進行し、右前車輪５４ｂが段差１００を通過した状態を示している。

図６（ａ）、（ｂ）、（ｃ）の横軸は時間を示す。図６では、移動体５０の左前車輪５４ａが段差１００を通過し始めてから、右前輪５４ｂが段差１００を通過し終わるまで期間を示している。図６（ａ）は、移動体５０の左右方向の傾きθ_Ｒの変化を示すグラフである。図６（ａ）の縦軸は、移動体５０の左右方向の傾きθ_Ｒを示している。図６（ａ）では、移動体５０の左への傾きが大きくなるに従って、縦軸の正方向の値が大きくなっている。図６（ｂ）は、移動体５０の左右方向の傾きθ_Ｒの角速度ω_Ｒの変化を示すグラフである。図６（ｂ）の縦軸は、移動体５０の左右方向の傾きθ_Ｒの角速度ω_Ｒを示している。角速度ω_Ｒは、ジャイロセンサ６６の出力に対応している。図６（ｃ）は、移動体５０の上下方向の加速度ａを示すグラフである。図６（ｃ）の縦軸は、移動体５０の上下方向の加速度ａを示している。加速度ａは、加速度センサ６４の出力に対応している。図６のｔ_１は、左前車輪５４ａが段差１００を通過している期間を示す。図６のｔ_２は、右前輪５４ｂが段差１００を通過している期間を示す。図６の各グラフでは、理解を簡単にするため、角度、角速度の変化を折れ線状に記載している。しかしながら、実際の角度、角速度は、なだらかに変化する。また、図６（ｃ）では、加速度の変化を見やすくするために、実際の変化期間より長めに示している。

図６（ａ）〜（ｃ）に示すように、移動体５０が図１に示す状態から図７に示す状態に移動する場合（段差１００を通過する場合）、まず左車輪５４ａが段差１００を通過する。このため、移動体５０の左右方向の傾きθ_Ｒが変化し（図６（ａ）のｔ_１の期間）、これに応じて、ジャイロセンサ６６の出力（移動体５０の左右方向の傾きθ_Ｒの角速度ω_Ｒ）が変化し（図６（ｂ）のｔ_１の期間）、また、加速度センサ６４の出力が変化する（図６（ｃ）のｔ_１の期間）。したがって、段差特定部１６は、加速度センサ６４の出力及びジャイロセンサ６６の出力から、前方の車輪５４ａ，５４ｂのいずれかが段差１００を通過したと判断する。なお、前方の車輪５４ａ，５４ｂが段差１００を通過したか、後方の車輪５６ａ，５６ｂが段差１００を通過したかは、移動体５０の進行方向によって判断することができる。
また、段差特定部１６は、ジャイロセンサ６６の出力から、左車輪５４ａが段差１００を通過したことを検知する。即ち、左車輪５４ａが段差１００を通過したときに移動体５０が傾斜する方向は、右車輪５４ｂが段差１００を通過したときに移動体５０の傾斜する方向とは異なる。このため、左車輪５４ａが段差１００を通過した時と、右車輪５４ｂが段差１００を通過した時とでは、ジャイロセンサ６６の出力が異なることとなる。段差特定部１６は、ジャイロセンサ６６の出力から、左車輪５４ａが段差１００を通過したと判断する。そして、ジャイロセンサ６６の出力と加速度センサ６４の出力が変化した時点を、左車輪５４ａが段差１００を通過した時点であると判断する。

なお、図６（ｂ）の信号出力から明らかなように、ジャイロセンサ６６の出力のみから移動体５０の左車輪５４ａが段差１００を通過したと判断することはできる。ただし、本実施例では、ジャイロセンサ６６の出力と加速度センサ６４の出力の両者を用いて車輪５４ａ，５４ｂ，５６ａ，５６ｂが段差１００を通過したことを判断するため、走行面の傾き等による誤判断を防止している。即ち、走行面が傾いているだけの場合、ジャイロセンサ６６の出力だけで判断すると、移動体５０が段差を通過したと判断してしまうが、ジャイロセンサ６６と加速度センサ６４の出力から判断することで、移動体が段差を通過していないと判断することができる。

左車輪５４ａが段差１００を通過すると、次に、右車輪５４ｂが段差１００を通過する。このため、移動体５０の左右方向の傾きθ_Ｒが変化し（図６（ａ）のｔ_２の期間）、これに応じて、ジャイロセンサ６６の出力（移動体５０の左右方向の傾きθ_Ｒの角速度ω_Ｒ）が変化し（図６（ｂ）のｔ_２の期間）、また、加速度センサ６４の出力が変化する（図６（ｃ）のｔ_２の期間）。したがって、段差特定部１６は、加速度センサ６４の出力及びジャイロセンサ６６の出力から、前方の車輪５４ａ、５４ｂのいずれかが段差１００に乗り上げたと判断でき、ジャイロセンサ６６の出力から右車輪５４ａが段差１００を通過したと判断する。そして、ジャイロセンサ６６の出力と加速度センサ６４の出力が変化した時点を、右車輪５４ａが段差１００を通過した時点であると判断する。

次に、段差特定部１６の上記（２）の作用、ずなわち、加速度センサ６４及びジャイロセンサ６６の出力から特定された段差を通過した車輪及び通過タイミング（時点）と、位置推定部１４で推定された移動体の推定位置及び推定進行方向とから、段差の位置と伸延方向を特定する作用について、図８を参照して説明する。図８は、左前車輪５４ａが段差１００を通過するときの移動体５０の状態と、右前車輪５４ｂが段差１００を通過するときの移動体の状態を併せて示す模式図である。図８では、左前車輪５４ａが段差１００を通過するときの車輪５４ａ、５４ｂ及び車軸５４ｃが二点鎖線で示されており、車輪５４ｂが段差１００を通過するときの車輪５４ａ、５４ｂ及び車軸５４ｃが実線で示されている。

段差特定部１６は、図６（ｂ）、（ｃ）に示すように、加速度センサ６４とジャイロセンサ６６の出力から、移動体５０の上下方向の加速度が急激に変化し、かつ、移動体５０が右側に傾斜したと判定すると、移動体５０の左車輪５４ａが段差１００を通過し始めたと判断する。次に、段差特定部１６は、左車輪５４ａが段差１００を通過するときの位置推定部１４によって推定される移動体５０の推定位置（本体５２の中央の座標）と、メモリ１２に記憶されている移動体５０の各部の寸法（本体５２の中央に対する車輪５４ａの位置関係）とから、左車輪５４ａが段差を通過するときの左車輪５４ａの座標Ｐ_Ｌを算出する。同様に、段差特定部１６は、右車輪５４ｂが段差１００を通過するときの位置推定部１４によって推定される移動体５０の推定位置と、メモリ１２に記憶されている移動体５０の各部の寸法とから、右車輪５４ｂが段差を通過するときの右車輪５４ｂの座標Ｐ_Ｒを算出する。段差特定部１６は、算出した左車輪５４ａの座標Ｐ_Ｌと右車輪５４ｂの座標Ｐ_Ｒの中点を段差１００の中心座標Ｐ_Ｃに特定する。

段差特定部１６は、車軸５４ｃの長さＷと左車輪５４ａが段差１００を通過し始めてから右車輪５４ｂが段差１００を通過し始めるまでの移動体５０の移動距離Ｄから、図８に示す角度α（段差通過方向）を算出する。即ち、α＝ｔａｎ^−１（Ｗ／Ｄ）により算出する。段差特定部１６は、算出した角度αと位置推定部１４によって推定される移動体５０の推定進行方向とから段差１００の伸延方向を特定する。具体的には、移動体５０の推定進行方向から角度αを減算することによって、段差１００の伸延方向を算出する。
なお、移動距離Ｄは、例えば、移動体５０が速度計を備えている場合、車輪５４ａが段差１００を通過するタイミング（時点）と車輪５４ｂが段差１００を通過するタイミング（時点）との差（時間差）に速度を乗算することによって算出することができる。あるいは、移動距離Ｄは、左車輪５４ａが段差１００を通過するときの移動体５０の推定位置と、右車輪５４ｂが段差１００を通過するときの移動体５０の推定位置との差から算出することができる。
また、角度αは、車軸５４ｃの長さＷと、左車輪５４ａが段差１００を通過するときの左車輪５４ａの推定位置（点Ｐ_Ｌ）と、右車輪５４ｂが段差１００を通過するときの右車輪５４ｂの推定位置（点Ｐ_Ｒ）から算出することもできる。即ち、α＝ｓｉｎ^−１（Ｗ／Ｐ_ＬＰ_Ｒ）によって算出することができる。

照合部１８は、段差特定部１６で特定された段差１００の位置及び伸延方向と、メモリ１２に記憶されている段差とを照合する。図９は、照合部１８の段差の照合処理を説明するための図である。図９では、段差特定部１６で特定された段差１００の中心座標を（ｘ，ｙ）とし、段差１００の伸延角度をβとする。
照合部１８は、メモリ１２に記憶されている段差の中から、段差１００に最も近い段差１１０を選択する。段差１１０の中心座標は（Ｘ，Ｙ）であり、伸延角度はγである。照合部１８は、段差１００の中心座標（ｘ，ｙ）と段差１１０の中心座標（Ｘ，Ｙ）の距離Ｃを算出する。照合部１８は、距離Ｃが第１許容誤差以下であり、β−γが第２許容誤差範囲内である場合に、段差１００が段差１１０であると特定する。第１、第２許容誤差は、エンコーダ６０，６２、加速度センサ６４、ジャイロセンサ６６の測定精度等によって予め設定されている。一方において、照合部１８は、距離Ｃが第１許容誤差より大きいか、β−γが第２許容誤差範囲外である場合、照合部１８は、段差１００がメモリ１２に記憶されていないと判断する。

位置補正部２０は、照合部１８で特定された段差１１０の位置と伸延方向から位置推定部１４で推定された推定位置及び推定進行方向を補正する。補正の手法は公知の様々な手法を採用することができる。例えば、位置補正部２０は、カルマンフィルタによる位置推定手法を用いて、移動体５０の推定位置、推定進行方向及びそれらの誤差分散を補正してもよい。この場合、各推定値の誤差分散は分散行列である。また、カルマンフィルタによる位置推定手法に必要な状態方程式等は、移動体５０や各センサの構造によって決定される。
なお、位置補正部２０による位置補正は、移動体５０の位置補正前の推定位置から段差に下ろした垂線と段差の交点を、移動体５０の推定位置とするように補正してもよい。即ち、移動体５０の推定位置から最も近い段差上の点を移動体５０の推定位置として補正する。

段差登録部２２は、照合部１８で段差１００がメモリ１２に記憶されていないと判断された場合に、照合部１８から段差１００をメモリ１２に登録する旨の指示を受ける。段差登録部２２は、位置推定部１４で特定された段差１００の位置（中心座標）と伸延方向をメモリに登録する。また、段差登録部２２は、段差１００の長さを登録する。段差１００の長さは、段差を乗り越え始めの左右の車輪５４ａ、５４ｂそれぞれの座標から算出することができる。あるいは、設計者や使用者が、段差１００の長さを予め入力していてもよい。

制御指令値算出部２４には、メモリ１２に記憶されている移動体５０の目的地点となる座標と目標経路が入力される。また、制御指令値算出部２４には、位置推定部１４、位置補正部２０から移動体５０の推定位置及び推定進行方向が入力される。制御指令値算出部２４は、移動体５０の推定位置及び推定進行方向と目標経路とを比較し、その比較結果に基づいて移動体５０の進行方向を算出する。例えば、移動体５０の推定位置が目標経路からずれている場合、移動体５０が目標経路上を移動するように、移動体５０の進行方向を修正する。修正量は、移動体５０の推定進行方向から決定する。制御指令値算出部２４は、修正した進行方向に基づいて、モータ５７を制御するための指令値を算出する。制御指令値算出部２４は、算出した指令値を駆動回路２６に出力する。駆動回路２６は、入力された指令値に従って、モータ５７を駆動させる。また、制御指令値算出部２４は、上記比較結果に基づいて移動体５０の移動速度を決定する。制御指令値算出部２４は、算出した移動速度に基づいて、モータ５８の指令値を算出する。制御指令値算出部２４は、算出した指令値を駆動回路２８に出力する。駆動回路２８は、入力された指令値に従って、モータ５８を駆動させる。

次に、上記した制御ユニット１０の処理手順を説明する。図１０は、制御ユニット１０が移動体５０を制御する処理手順を示すフローチャートである。制御指令値算出部２４は、メモリ１２に記憶されている目標座標及び目標経路に基づいて、モータ５７及びモータ５８を駆動するための指令値を算出する（ステップＳ１２）。制御指令値算出部２４は、算出した指令値を駆動回路２６，２８に出力する（ステップＳ１４）。モータ５７及びモータ５８は、出力された指令値に基づいて駆動回路２６，２８によって駆動される。

位置推定部１４には、エンコーダ６０，６２からの出力が入力される（ステップＳ１６）。位置推定部１４は、エンコーダ６０，６２の出力から移動体５０の位置及び進行方向を推定する（ステップＳ１８）。制御指令値算出部２４は、位置推定部１４で推定された推定位置が目標座標と等しいか否かを確認する（ステップＳ２０）。即ち、移動体５０が目的地（目標座標）に到達したか否かを判断する。Ｓ２０でＹＥＳの場合、制御指令値算出部２４は、モータ５７及びモータ５８を停止し、（ステップＳ２２）、処理を終了する。

一方において、Ｓ２０でＮＯの場合、段差特定部１６は、移動体５０が段差を通過したか否かを確認する（ステップＳ２４）。Ｓ２４でＮＯの場合、Ｓ１２に進み、Ｓ１２からの処理を繰り返す。一方において、Ｓ２４でＹＥＳの場合、段差特定部１６は、段差の位置及び伸延方向を特定する（ステップＳ２６）。照合部１８は、段差特定部１６で特定された段差とメモリ１２の地図上の段差とを照合する（ステップＳ２８）。特定された段差と地図上の段差が合致した場合（ステップＳ２８でＹＥＳ）、位置補正部２０は、移動体５０の推定位置及び推定進行方向を補正して（ステップＳ３０）、Ｓ１２に戻る。一方において、特定された段差と地図上の段差が合致しなかった場合（ステップＳ２８でＮＯ）、段差登録部２２は、特定された段差の位置、伸延方向及び長さをメモリ１２の地図上に登録して（ステップＳ３２）、Ｓ１２に戻る。

本実施例の移動体５０では、推定位置及び推定進行方向を補正するためのランドマークとして、移動体５０が通過する段差を用いている。また、制御ユニット１０は、移動体５０の上下方向の加速度と、移動体５０が傾く方向と、そのタイミングから移動体５０が段差を通過したことを検知する。このような構成によれば、移動体５０は、接触式でランドマークを検知することから、ランドマークを確実に検知でき、また、移動体５０の上下方向の加速度と傾きを検知するだけでよいため、容易な構成でランドマークを検知することができる。
移動体５０では、移動体５０が通過した段差を利用して、移動体５０の推定位置及び推定進行方向を補正している。このため、移動体５０は、目標経路上を精度良く走行することができる。

また、移動体５０では、加速度センサ６４とジャイロセンサ６６を用いて段差を検知している。このような構成によれば、画像センサやレーザレンジファインダ等を利用することなく、段差を検知することができる。また、加速度センサ６４とジャイロセンサ６６を用いることによって、画像センサ等では検知できないような小さな段差を検知することができる。

以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。
例えば、移動体５０の車輪は、２つ、３つ又は５つ以上であってもよい。この場合、少なくとも２つの車輪が、移動体５０の進行方向に沿って見たときに重ならない位置に配置されている。また、移動体５０の左右の前輪をそれぞれモータで独立駆動し、移動体の後輪をキャスタ輪とすることができる。左右の前輪を独立で駆動することで、移動体の進行方向を変更することができる。このため、より簡易な構成とすることができる。
例えば、上述した実施例の移動体５０では、オドメトリにより位置推定を行っている。しかしながら、移動体５０は、ＧＰＳ等のセンサを用いた位置推定を行ってもよい。
例えば、上述した実施例では、移動体５０の推定位置と推定進行方向の両者を補正したが、推定位置のみを補正して推定進行方向を補正しなくてもよい。この場合、移動体５０は、加速度センサ６４とジャイロセンサ６６の一方を省略することができる。また、ＣＰＵ１１の構成を、移動体５０の推定位置のみを補正することができる構成に置き換えてもよい。例えば、ＣＰＵ１１の段差特定部１６は、段差の伸延方向を特定しなくてもよい。また、照合部１８は、段差特定部１６で特定された段差の位置を用いて、メモリ１２に記憶されている段差と照合してもよい。

例えば、上述した実施例では、メモリ１２に、可動範囲内に存在する段差の中央位置の座標、段差の伸延方向γ、及び段差の長さＬを記憶し、これらによって段差を規定している。しかしながら、メモリ１２には、段差の伸延方向と長さを記憶し、これらによって段差を規定することができる。段差の伸延方向と長さにより段差を規定する方法としては、例えば、段差の伸延方向を示す２次方程式とその範囲（例えばＸの範囲）により規定することができる。
また、本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時の請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は、複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

段差を通過しているときの移動体の平面図を示す。 移動体の駆動系統を示す概略構成図を示す。 移動体の電気系統を示すブロック図を示す。 移動体の制御系統を示すブロック図を示す。 メモリに記憶されている段差の記憶内容を示す。 移動体が段差を通過する場合の加速度センサとジャイロセンサの検知結果を示すグラフを示す。 図１の状態から移動体が進行した状態を示す。 移動体の車輪が段差を通過し始めたときの移動体の進行方向と段差の伸延方向の関係を示す模式図を示す。 照合部の段差の照合処理を説明するための図を示す。 制御ユニットが移動体５０を制御する処理手順を示すフローチャートを示す。

符号の説明

１０：制御ユニット
１１：ＣＰＵ
１２：メモリ
１４：位置推定部
１６：段差特定部
１８：照合部
２０：位置補正部
２２：段差登録部
２４：制御指令値算出部
５０：移動体
５７：アクチュエータ
５８：モータ
６０：エンコーダ
６２：エンコーダ
６４：加速度センサ
６６：ジャイロセンサ

Claims (6)

1. 可動領域内を走行する移動体であって、
   可動領域内の段差の位置を記憶している記憶手段と、
   移動体が段差を通過したことを検知する段差検知手段と、
   段差検知手段で移動体が段差を通過したことを検知した場合において、段差通過時の移動体の推定位置から所定範囲内に記憶手段に記憶されている段差が存在するときに、記憶手段に記憶されている当該段差の位置から移動体の推定位置を補正する位置補正手段と、
   を備える移動体。
2. 移動体は、移動体の進行方向に沿って見たときに重ならない位置に配置されている少なくとも２つの車輪を備えており、
   段差検知手段は、車輪毎に段差を通過したことを検知可能であり、
   記憶手段は、可動領域内の段差の伸延方向をさらに記憶し、
   位置補正手段は、段差検知手段で移動体が段差を通過したことを検知した場合において、段差通過時の移動体の推定位置から所定範囲内に記憶手段に記憶されている段差が存在するときに、（１）記憶手段に記憶されている当該段差の伸延方向と、（２）段差検知手段の検知結果とから移動体の推定進行方向を補正することを特徴とする請求項１に記載の移動体。
3. 段差検知手段は、段差を通過した車輪の順序及び各車輪の段差を通過したタイミングを検知可能であり、
   位置補正手段は、（１）記憶手段に記憶されている当該段差の伸延方向と、（２）段差検知手段で検知された「段差通過時の車輪順序及び各車輪の段差通過タイミング」から推定される移動体の段差通過方向とを用いて、移動体の推定進行方向をさらに補正することを特徴とする請求項２に記載の移動体。
4. 段差検知手段は、移動体の上下方向の加速度を検知する加速度センサと、移動体の傾きを検知するジャイロセンサを有することを特徴とする請求項２又は３に記載の移動体。
5. 段差通過時の移動体の推定位置から所定範囲内に記憶手段に記憶されている段差が存在しない場合に、段差通過時の移動体の推定位置を当該段差の位置とし、移動体の推定進行方向と段差検知手段の検知結果とから特定される当該段差の推定伸延方向を当該段差の伸延方向として記憶手段に登録する登録手段をさらに備えることを特徴とする請求項２〜４のいずれか一項に記載の移動体。
6. 可動領域内の段差の位置を記憶している記憶手段と、段差を通過したことを検知する段差検知手段とを有する移動体が可動領域内を走行する際の移動体の位置を推定する方法であって、
   段差検知手段で移動体が段差を通過したことを検知したときの移動体の位置を推定する位置推定工程と、
   推定された移動体の推定位置から所定範囲内に記憶手段に記憶されている段差が存在するときに、記憶手段に記憶されている当該段差の位置から移動体の推定位置を補正する位置補正工程と、
   を備える位置推定方法。

Images