JP2016057236A - 自己位置推定システムおよび自己位置推定方法 - Google Patents

Abstract

【課題】移動体の自己位置の推定の精度を高めることが可能な自己位置推定システムを提供する。
【解決手段】この自己位置推定システム１００は、エンコーダ１２により検出された回転角と、ジャイロセンサ１３により検出されたカート１０の角速度とに基づいて、カート１０の所定区画２００内における位置情報を推定する制御部１５を備え、制御部１５は、ジャイロセンサ１３により検出された角速度より求められたカート１０の角度情報の変化量が微小角度以下の場合で、かつ、カート１０の位置情報に基づいて、カート１０が略直進していると判断される場合、ジャイロセンサ１３により検出された角速度より求められたカート１０の角度情報を、直進に対応する角度情報に補正する第１の補正を行うように構成されている。
【選択図】図３

Description

この発明は、自己位置推定システムおよび自己位置推定方法に関し、特に、エンコーダとジャイロセンサとを備える自己位置推定システムおよび自己位置推定方法に関する。

従来、エンコーダとジャイロセンサとを備える移動体の位置計測装置が知られている（たとえば、特許文献１参照）。上記特許文献１では、エンコーダは、操舵輪の移動速度を計測するように構成されている。また、ジャイロセンサは、移動体の向きと旋回角速度とを計測するように構成されている。そして、エンコーダによる計測結果と、ジャイロセンサによる計測結果と、移動体の車体寸法とに基づいて、演算手段により、移動体の位置が演算される。

特許第３３３４０７４号公報

しかしながら、上記特許文献１に記載の移動体の位置計測装置では、エンコーダおよびジャイロセンサによる計測結果と移動体の車体寸法とに基づいて移動体の位置が演算される一方、エンコーダおよびジャイロセンサの測定誤差に起因して、演算された移動体の位置に誤差が生じるという問題点がある。

この発明は、上記のような課題を解決するためになされたものであり、この発明の１つの目的は、移動体の自己位置の推定の精度を高めることが可能な自己位置推定システムおよび自己位置推定方法を提供することである。

上記目的を達成するために、第１の局面による自己位置推定システムは、所定区画内を走行する移動体の自己位置を推定する自己位置推定システムであって、移動体の車輪の回転角を検出する回転角検出器と、移動体の角速度を検出するジャイロセンサと、回転角検出器により検出された回転角と、ジャイロセンサにより検出された移動体の角速度とに基づいて、移動体の所定区画内における位置情報を推定する制御部とを備え、制御部は、ジャイロセンサにより検出された角速度より求められた移動体の角度情報の変化量が所定の値以下の場合で、かつ、移動体の位置情報に基づいて、移動体が略直進していると判断される場合、ジャイロセンサにより検出された角速度より求められた移動体の角度情報を、直進に対応する角度情報に補正する第１の補正を行うように構成されている。

この第１の局面による自己位置推定システムでは、上記のように、制御部を、ジャイロセンサにより検出された角速度より求められた移動体の角度情報の変化量が所定の値以下の場合で、かつ、移動体の推定位置に基づいて、移動体が略直進していると判断される場合、ジャイロセンサにより検出された角速度より求められた移動体の角度情報を、直進に対応する角度情報に補正する第１の補正を行うように構成する。ここで、移動体が略直進している場合でも、回転角検出器の検出誤差に起因して（誤差が積算されて）、移動体の角度が直進以外の角度であると判断される場合がある。そこで、上記のように構成することにより、移動体が略直進している場合に移動体が直進していないと判断されるのを抑制することができるので、移動体の自己位置の推定の精度を高めることができる。

第２の局面による自己位置推定方法は、所定区画内を走行する移動体の自己位置を推定する自己位置推定方法であって、回転角検出器により検出された車輪の回転角と、ジャイロセンサにより検出された移動体の角速度とに基づいて、移動体の所定区画内における位置情報を推定することと、ジャイロセンサにより検出された角速度より求められた移動体の角度情報の変化量が所定の値以下の場合で、かつ、移動体の位置情報に基づいて、移動体が略直進していると判断される場合、ジャイロセンサにより検出された角速度より求められた移動体の角度情報を、直進に対応する角度情報に補正することとを備える。

この第２の局面による自己位置推定方法では、上記のように、ジャイロセンサにより検出された角速度より求められた移動体の角度情報の変化量が所定の値以下の場合で、かつ、移動体の位置情報に基づいて、移動体が略直進していると判断される場合、ジャイロセンサにより検出された角速度より求められた移動体の角度情報を、直進に対応する角度情報に補正することを備える。ここで、移動体が略直進している場合でも、回転角検出器の検出誤差に起因して（誤差が積算されて）、移動体の角度が直進以外の角度であると判断される場合がある。そこで、上記のように構成することにより、移動体が略直進している場合に移動体が直進していないと判断されるのを抑制することができるので、移動体の自己位置の推定の精度を高めることが可能な自己位置推定方法を提供することができる。

上記のように構成することによって、移動体の自己位置の推定の精度を高めることができる。

本実施形態による自己位置推定システムのカートとカートが移動する所定区画とを示す図である。 本実施形態による自己位置推定システムのカートを示す図である。 本実施形態による自己位置推定システムのブロック図である。 本実施形態による自己位置推定システムのグリッドマップを説明するための図である。 本実施形態による自己位置推定システムのグリッドマップ（テキストデータ）を説明するための図である。 本実施形態による自己位置推定システムのマップマッチングを説明するための図である。 本実施形態によるマップマッチングにおいて通行可能なブロックの探索を説明するための図である。 本実施形態によるマップマッチングにおいて通行可能なブロックの探索（１階層目）を説明するための図である。 本実施形態によるマップマッチングにおいて通行可能なブロックの探索（２階層目）を説明するための図である。 本実施形態によるマップマッチングの結果を説明するための図である。 本実施形態によるカートの軌跡に基づく角度情報の補正（Ｙ軸方向）を説明するための図である。 本実施形態によるカートの軌跡に基づく角度情報の補正（Ｘ軸方向）を説明するための図である。 本実施形態によるカートの軌跡に基づく角度情報の補正（カーブ部分）を説明するための図である。 本実施形態による自己位置推定システムの自己位置推定方法を説明するためのフロー図である。 本実施形態による自己位置推定システムの自己位置推定の実験結果（デッドデコニングのみ）を説明するためのフロー図である。 本実施形態による自己位置推定システムの自己位置推定の実験結果（ＲＦＩＤタグに基づく補正後）を説明するためのフロー図である。 本実施形態による自己位置推定システムの自己位置推定の実験結果（マップマッチングによる補正後）を説明するためのフロー図である。 本実施形態による自己位置推定システムの自己位置推定の実験の誤差の累積分布を示す図である。

以下、本実施形態を図面に基づいて説明する。

まず、図１〜図１３を参照して、本実施形態の自己位置推定システム１００の構成について説明する。自己位置推定システム１００は、所定区画２００（物品倉庫、建物の廊下など）において走行するカート１０の自己位置を推定するように構成されている。なお、カート１０は、「移動体」の一例である。

図１に示すように、所定区画２００（建物、屋内）には、複数の棚２０１が配置されている。そして、カート１０は、所定区画２００の出入口２０２の近傍を初期位置として、複数の棚２０１の間の移動経路２０３を走行するように構成されている。なお、移動経路２０３は、所定区画２００において、棚２０１が配置されていないカート１０の通行が可能な領域を意味する。移動経路２０３は、略直線状の直線部分２０３ａと、直線部分２０３ａ同士を接続する略直角に屈折するカーブ部分２０３ｂとにより構成されている。

ここで、本実施形態では、カート１０の移動経路２０３近傍には、固有のシリアル番号をそれぞれ有する複数のＲＦＩＤタグ２０が配置されている。また、ＲＦＩＤタグ２０は、移動経路２０３のうちのカーブ部分２０３ｂの曲がり角の前後（近傍）に設けられている。なお、「近傍」とは、移動経路２０３を走行するカート１０が、ＲＦＩＤタグ２０の固有のシリアル番号を読み出すことが可能な程度の位置を意味する。また、ＲＦＩＤタグ２０は、「座標識別部」の一例である。また、シリアル番号は、「座標識別情報」の一例である。

また、カート１０は、図２に示すように、ユーザ３００により、移動経路２０３を移動されるように構成されている。

また、図３に示すように、カート１０には、車輪１１が設けられている。また、カート１０には、エンコーダ１２が設けられている。エンコーダ１２は、車輪１１の回転角を検出するように構成されている。なお、エンコーダ１２は、「回転角検出器」の一例である。また、カート１０には、ジャイロセンサ１３が設けられている。ジャイロセンサ１３は、カート１０の角速度を検出するように構成されている。

また、カート１０には、ＲＦＩＤリーダ１４が設けられている。ＲＦＩＤリーダ１４は、ＲＦＩＤタグ２０からの信号を受信するアンテナを有する。また、ＲＦＩＤリーダ１４は、複数（本実施形態では２つ）設けられている。２つのＲＦＩＤリーダ１４は、カート１０の進行方向に直交する方向（カート１０の両側部）にそれぞれ設けられている。そして、ＲＦＩＤリーダ１４は、カート１０の移動経路２０３近傍に配置されたＲＦＩＤタグ２０のシリアル番号を読み出す（受信する）とともに、読み込んだシリアル信号を出力するように構成されている。なお、ＲＦＩＤリーダ１４は、カート１０がＲＦＩＤタグ２０の近傍に位置した場合に、ＲＦＩＤリーダ１４のシリアル番号を読み出すように構成されている。たとえば、ＲＦＩＤリーダ１４は、ＲＦＩＤタグ２０からの受信信号の強度が予め設定された所定のしきい値以上になった場合に、ＲＦＩＤタグ２０を認識したと判断して、ＲＦＩＤタグ２０からＲＦＩＤリーダ１４のシリアル番号を読み出すように構成されている。すなわち、ＲＦＩＤリーダ１４は、ＲＦＩＤタグ２０が近傍に存在するか、または、存在しないかの２値（０または１）を判断するように構成されている。

また、カート１０には、制御部１５が設けられている。制御部１５は、エンコーダ１２により検出された回転角についての信号、ジャイロセンサ１３により検出された角速度についての信号、および、ＲＦＩＤリーダ１４に読み出されたＲＦＩＤタグ２０のシリアル番号についての信号が、入力されるように構成されている。

また、カート１０には、記憶部１６が設けられている。図４に示すように、記憶部１６には、カート１０が走行する所定区画２００に対応して座標を設定したグリッドマップ３０が記憶されている。グリッドマップ３０は、所定の大きさを有するブロック３１により分割した平面により構成されている。グリッドマップ３０は、所定区画２００の走行面２０４（棚２０１が配置されている面および移動経路２０３、図１参照）に対応するＸＹ座標と、所定区画２００の各階に対応するＺ座標とを含むように構成されている。また、グリッドマップ３０では、グリッドマップ３０の左上の角のブロック３１（ブロック３１ａ）の中心をＸＹ座標（ＸＹ平面）の原点としている。また、所定区画２００の１階部分を、Ｚ座標の原点としている。すなわち、座標（０，０，０）は、所定区画２００の１階部分の左上の角のブロック３１ａの座標を意味する。また、記憶部１６には、後述するデッドデコニングにより推定されたカート１０の第１の位置情報、マップマッチングにより推定された第３の位置情報、その他、以前に位置していたブロック３１において取得されたカート１０の角度情報が記憶されている。

また、図５に示すように、グリッドマップ３０は、具体的には、テキストデータ３２として記憶部１６に記憶されている。テキストデータ３２には、グリッドの間隔（ｍ）、Ｘ軸の最大値、Ｙ軸の最大値、および、Ｚ軸の最大値が含まれている。

また、テキストデータ３２（記憶部１６）には、各ブロック３１の座標（Ｘ，Ｙ，Ｚ）に対応させて、スタート位置フラグ、通行可否フラグおよびＲＦＩＤタグのシリアル番号が含まれている。スタート位置フラグは、ブロック３１が、スタート位置（出入口２０２の近傍、図４参照）に対応する場合には「１」にされ、スタート位置に対応しない場合には「０」にされる。ここで、本実施形態では、通行可否フラグは、グリッドマップ３０の各々のブロック３１上をカート１０が通行可能か否かを表すよう構成されている。具体的には、通行可否フラグは、ブロック３１に棚２０１などの障害物が配置されることにより、カート１０の通行ができない場合には、「１」にされる。また、ブロック３１に棚２０１などの障害物が配置されておらず、カート１０が通行できる場合には、「０」にされる。図４では、カート１０が通行できないブロック３１がハッチングされている。なお、通行可否フラグは、「通行可否情報」の一例である。

また、図５に示すように、記憶部１６（テキストデータ３２）には、ＲＦＩＤタグ２０のシリアル番号が、グリッドマップ３０上の座標に対応させて記憶されている。具体的には、ブロック３１内にＲＦＩＤタグ２０が設けられている場合に、グリッドマップ３０上の座標（Ｘ，Ｙ，Ｚ）に対応させて、ＲＦＩＤタグ２０のシリアル番号が記憶されている。また、ブロック３１内にＲＦＩＤタグ２０が設けられていない場合には、ＲＦＩＤタグ２０が設けられていない旨のフラグが記憶されている。

また、図３に示すように、カート１０には、表示部１７が設けられている。表示部１７には、グリッドマップ３０、および、グリッドマップ３０上のカート１０の位置情報などが表示されるように構成されている。

ここで、本実施形態では、制御部１５は、エンコーダ１２により検出された回転角（回転角についての信号）と、ジャイロセンサ１３により検出されたカート１０の角速度（角速度についての信号）とに基づいて、カート１０の所定区画２００における第１の位置情報を推定するように構成されている。さらに、ＲＦＩＤリーダ１４により読み出されたＲＦＩＤタグ２０のシリアル番号に基づいて、推定された第１の位置情報をＲＦＩＤタグ２０の実際の位置に対応した第２の位置情報に補正するように構成されている。具体的には、制御部１５は、ＲＦＩＤタグ２０のシリアル番号が対応付けられたグリッドマップ３０上の座標に基づいて、第１の位置情報を第２の位置情報に補正するように構成されている。なお、ＲＦＩＤタグ２０のシリアル番号が対応付けられたグリッドマップ３０上の座標に基づいて、第１の位置情報を第２の位置情報にする補正は、「第３の補正」の一例である。

また、移動経路２０３のうちのカーブ部分２０３ｂの曲がり角の前後にＲＦＩＤタグ２０を設けて、制御部１５により、移動経路２０３のうちのカーブ部分２０３ｂの曲がり角の前後における推定された第１の位置情報を第２の位置情報に補正するように構成されている。また、制御部１５は、ＲＦＩＤリーダ１４がシリアル番号を読み出した時点で、第１の位置情報を第２の位置情報に補正することを開始するように構成されている。

以下、ＲＦＩＤタグ２０のシリアル番号に基づいた補正について、具体的に説明する。なお、この補正は、制御部１５により行われる。

（ＲＦＩＤタグのシリアル番号に基づいた補正）
まず、ジャイロセンサ１３により検出されたカート１０の角速度（ｄｅｇ／ｓ）が積分されることにより、カート１０の進行方向に対する角度情報θが算出される。また、エンコーダ１２により検出されたカート１０の車輪１１の回転角に基づいて、カート１０の走行面２０４上の移動距離が算出される。具体的には、前回算出された地点からの移動距離（差分距離ΔＤ（ｍｍ））が算出される。そして、算出された角度情報θおよび差分距離ΔＤを用いて、下記の式（１）および（２）により、カート１０の第１の位置情報（ＸＹ座標）が推定（デッドデコニング）される。たとえば、カート１０の第１の位置情報が、（ｘ１，ｙ１，ｚ１）と推定される。なお、ｚ１は、所定区画２００（建物）の各階に対応する座標である。
Ｘ座標＝前回のＸ座標＋ΔＤ×ｃｏｓθ ・・・（１）
Ｙ座標＝前回のＹ座標＋ΔＤ×ｓｉｎθ ・・・（２）
なお、前回算出された地点の位置情報は、記憶部１６に記憶されている。また、位置情報は、後述するように１００ｍｓ毎に算出されており、前回の位置情報は、１００ｍｓ前の位置情報である。

ここで、ＲＦＩＤリーダ１４により、ＲＦＩＤタグ２０のシリアル番号が読み出された場合（すなわち、カート１０がＲＦＩＤタグ２０の近傍に位置している場合）、テキストデータ３２（図５参照）を用いて、読み出されたＲＦＩＤタグ２０のシリアル番号が検索される。そして、読み出されたＲＦＩＤタグ２０のシリアル番号に対応する座標（たとえば、（ｘ２，ｙ２，ｚ１））が取得される。そして、カート１０の自己位置（座標）が、上記の第１の位置情報（ｘ１，ｙ１，ｚ１）から、カート１０の近傍に配置されるＲＦＩＤタグ２０が配置されているブロック３１の座標（ｘ２，ｙ２，ｚ１）（第２の位置情報）に補正される。すなわち、エンコーダ１２により検出された回転角とジャイロセンサ１３により検出された角速度とに基づいて推定された第１の位置情報が、実際のカート１０の座標とずれていた場合でも、ＲＦＩＤタグ２０のシリアル番号に基づいて、カート１０の自己位置（座標）が補正される。

また、制御部１５は、記憶部１６に記憶されたグリッドマップ３０に基づいて、第２の位置情報をマップマッチングにより推定（補正）するように構成されている。なお、マップマッチングによる推定は、「第２の補正」の一例である。以下、図６〜図１０を参照して、マップマッチングについて、具体的に説明する。

図６は、グリッドマップ３０において、通行可能なブロック３１（ハッチングなし）と、通行可能でないブロック３１（ハッチングあり）を表している。また、図７は、現在カート１０が位置するブロック３１の外側に位置する２４個のブロック３１（図６において太い点線で囲まれた領域のブロック３１）を表している。また、図８は、現在カート１０が位置するブロック３１の外側に隣接する１階層目のブロック３１を表している。また、図９は、１階層目のブロック３１の外側に隣接する２階層目のブロック３１を表している。また、図１０は、現在カート１０が位置するブロック３１に近接する通行可能なブロック３１を表している。

（マップマッチングによる補正）
まず、第２の位置情報に対応するブロック３１が、記憶部１６（グリッドマップ３０）に記憶された通行可否フラグに基づいて、通行可能か否かが判断される。本実施形態では、通行可否フラグに基づいて第２の位置情報に対応するブロック３１が通行可能であると判断される場合、第２の位置情報をカート１０の第３の位置情報として推定する。たとえば、図６に示すように、カート１０が位置Ａ１またはＡ２に位置する場合には、位置Ａ１またはＡ２に対応するブロック３１が通行可能であるので、第２の位置情報がカート１０の第３の位置情報として推定される。

ここで、本実施形態では、記憶部１６に記憶された通行可否フラグに基づいて、第２の位置情報に対応するブロック３１が通行可能でないと判断される場合、第２の位置情報から最も近接する通行可能なブロック３１に対応する座標を、カート１０の第３の位置情報として推定される。たとえば、図６に示すように、カート１０の位置情報が位置Ａ３であると推定されている場合には、位置Ａ３に対応するブロック３１は、通行可能でない。このとき、図７に示すように、グリッドマップ３０に基づいて、現在の位置Ａ３を中心として、１階層目、２階層目・・・と探索する階層を広げてゆき、通行可能なブロック３１が存在する階層が探索される。なお、１階層目とは、位置Ａ３のブロック３１の外側に隣接する８個のブロック３１（ブロック番号１〜８）である。また、２階層目とは、１階層目のブロック番号１〜８の外側に隣接する１６個のブロック番号９〜２４である。

図８に示すように、１階層目のブロック番号１〜８に通行可能なブロック３１が存在しない場合には、図９に示すように、２階層目のブロック番号９〜２４に通行可能なブロック３１が存在するか否かが探索される。２階層目では、ブロック番号９〜１６および２４が、通行可能である。そこで、第２の位置情報（位置Ａ３）から最も近接する通行可能なブロック３１に対応する座標が、カート１０の第３の位置情報として推定される。ここで、第２の位置情報（位置Ａ３）から最も近接する通行可能なブロック３１は、ブロック番号１０および１４の２個である。このとき、本実施形態では、図１０に示すように、第２の位置情報（位置Ａ３）から最も近接する通行可能なブロック３１が複数ある場合、通行可能な複数のブロック３１のうち、前回の推定されたカート１０の位置に最も近接するブロック３１に対応する座標を、カート１０の第３の位置情報として推定する。具体的には、前回の位置Ａ２に最も近接するブロック番号１４に対応する座標を、カート１０の第３の位置情報（位置Ａ４）として推定する。

また、上記のマップマッチングでは、ＲＦＩＤタグのシリアル番号に基づいて補正された第１の位置情報（すなわち第２の位置情報）が、マップマッチングにより第３の位置情報に推定される説明をしたが、ＲＦＩＤタグのシリアル番号を読み出されない場合には、第１の位置情報が、マップマッチングによりそのまま第３の位置情報とされる。

ここで、本実施形態では、制御部１５は、推定されたカート１０の位置が、隣接する２つのブロック３１の一方から他方に移動する前後において、ジャイロセンサ１３により検出された角速度より求められたカート１０の角度情報の変化量が所定の微小角度の場合で、かつ、第３の位置情報（位置Ａ４）に基づいて、カート１０が複数のブロック３１に渡って略直進していると判断される場合、ジャイロセンサ１３により検出された角速度より求められたカート１０の角度情報を、直進に対応する角度情報に補正するように構成されている。なお、このジャイロセンサ１３により検出された角速度より求められたカート１０の角度情報を、直進に対応する角度情報にする補正は、「第１の補正」の一例である。以下、具体的に説明する。

（カートの軌跡に基づく角度情報の補正）
図１１に示すように、５つのブロック３１（ブロックＢ１〜Ｂ５）がＹ方向に沿って配置されている場合について説明する。ここで、隣接するブロックＢ４およびＢ５のうち、ブロックＢ４において最初に推定された求められたカート１０の角度情報θ１と、ブロックＢ５において最初に推定されたカート１０の角度情報θ２との変化量（差分値θ１−θ２の絶対値）が、微小角度以下（条件１）であったとする。なお、角度情報θ１は、ブロックＢ４において最初に角度情報が推定された際に記憶部１６に記憶されたものである。また、上記マップマッチングによる補正後のカート１０の位置（軌跡）が、４つのブロックＢ１〜Ｂ４に渡ってＹ方向に沿って略直進している（Ｙ座標が略変化しない）（条件２）とする。そして、条件１および条件２の両方が満たされている場合、ジャイロセンサ１３により検出された角速度より求められたカート１０の角度情報（角速度の積分値）が、直進に対応する角度情報（たとえば、ゼロ）に補正される。

また、図１２に示すように、５つのブロック３１（ブロックＣ１〜Ｃ５）がＸ方向に沿って配置されている場合も、５つのブロック３１（ブロックＢ１〜Ｂ５、図１１参照）がＹ方向に沿って配置されている場合と同様に、ジャイロセンサ１３により検出された角速度より求められたカート１０の角度情報（角速度の積分値）が補正される。

また、図１３に示すように、１つのブロック３１内でカート１０が曲がる場合（たとえば、Ｙ軸に沿った方向から侵入してＸ軸に沿った方向に退出する場合）は、カート１０の角度情報の変化量が、比較的大きくなる（微小角度より大きくなる）か、または、４つのブロック３１に渡って直進していると判断されないので、上記の「カートの軌跡に基づく角度情報の補正」は行われない。

次に、図１４を参照して、自己位置推定システム１００の制御部１５の動作（自己位置推定方法）について説明する。

まず、ステップＳ１に示すように、記憶部１６に記憶されているグリッドマップ３０が読み出される（オープンされる）。そして、カート１０の初期位置が設定される。ここで、予め記憶部１６に記憶されたスタート位置（図４参照）の座標が、第１の位置情報として設定される。また、グリッドマップ３０（図４参照）の座標が第３の位置として設定される。また、角度情報は、所定区画２００のスタート位置（図４参照）にカート１０が配置された方向（進行方向）に設定される。なお、実際のカート１０は、ユーザにより、所定区画２００のスタート位置（図４参照）に配置される。

次に、ステップＳ２において、ジャイロセンサ１３のオフセット補正（ドリフト補正）が行われる。次に、ステップＳ３において、カート１０の走行が開始される。

次に、ステップＳ４において、ジャイロセンサ１３により検出された角速度（ｄｅｇ／ｓ）が取得される。具体的には、カート１０のヨー角（ｙａｗ ａｎｇｌｅ、進行方向に対する角度）が取得される。次に、ステップＳ５において、取得された角速度が積分され、カート１０の進行方向に対する角度情報（ｄｅｇ）が算出される。なお、このとき、ジャイロセンサ１３に固有の既知の誤差に基づいて、算出された角度情報が補正される。なお、この補正は、制御部１５が行ってもよいし、ジャイロセンサ１３自身によって、取得した角速度が補正されてもよい。

次に、ステップＳ６において、エンコーダ１２により検出された車輪１１の回転角が取得される。そして、前回算出された地点からの移動距離（差分距離ΔＤ（ｍｍ））が算出される。なお、前回算出された地点（位置情報）は、１００ｍｓ前の位置情報であり、記憶部１６に記憶されている。また、新たな位置情報が算出されることにより、前回の位置情報が新たな位置情報に更新される。

次に、ステップＳ７において、上記の式（１）および（２）により、カート１０の第１の位置情報（ＸＹ座標）が推定（デッドデコニング）される。

次に、ステップＳ８において、ＲＦＩＤリーダ１４により、ＲＦＩＤタグ２０のシリアル番号が読み出されたか否かが判断される。ＲＦＩＤタグ２０のシリアル番号が読み出されたと判断された場合、ステップＳ９に進んで、上記の「ＲＦＩＤタグのシリアル番号に基づいた補正」が行われる。すなわち、デッドデコニングにより推定されたカート１０の座標（第１の位置情報）が、ＲＦＩＤタグ２０のシリアル番号に対応する座標（第２の位置情報）に補正される。そして、ステップＳ１０に進む。ステップＳ８において、ＲＦＩＤタグ２０のシリアル番号が読み出されていないと判断された場合、ステップＳ１０に進む。

次に、ステップＳ１０において、上記の「マップマッチングによる補正」が行われる。すなわち、カート１０の位置（座標）が、通行可能でないブロック３１に位置すると判断された場合、カート１０の位置（座標）が、通行可能であるブロック３１の座標に推定される。

次に、ステップＳ１１において、上記の「カートの軌跡に基づく角度情報の補正」が行われる。すなわち、カート１０が略直進しているとともに、角度情報の変化量が微小角度以下の場合、ジャイロセンサ１３により検出された角速度より求められたカート１０の角度情報（角速度の積分値）が、直進に対応する角度情報に補正される。なお、ここでは、カート１０の実際の角度が補正されるのではなく、制御部１５により算出された角度情報が補正される。

また、ステップＳ１２において、エンコーダ１２により検出された車輪１１の回転角に基づいて、カート１０が一定の時間（たとえば、１０秒間）静止していると判断される場合、ステップＳ２に戻る。また、ステップＳ１２において、カート１０が一定の時間静止していないと判断される場合、ステップＳ４に戻る。

なお、ステップＳ４〜Ｓ１２は、所定の時間毎（たとえば、１００ｍｓ毎）に行われる。

次に、図１５〜図１８を参照して、本実施形態の自己位置推定システム１００を用いて行ったカート１０の自己位置の推定の実験について説明する。なお、図１５〜図１８において、推定されたカート１０の位置（カート１０の移動の軌跡）が細い実線で表され、棚２０１が太い実線で表されている。

まず、この実験では、３つの棚２０１（棚２０１ａ、棚２０１ｂ、棚２０１ｃ）が配置される部屋２０５の中において、カート１０を、３つの棚２０１の周囲を回るように走行させた場合におけるカート１０の自己位置を推定した。なお、この実験は、実際のカート１０を、３つの棚２０１の周囲の同一の経路上（図１５の太い点線参照）を、複数回（６回）走行させたものである。

図１５に示すように、デッドデコニングのみによるカート１０の位置の推定では、カート１０を走行させるにしたがって、推定されたカート１０の位置（軌跡）が、実際のカート１０の走行経路（太い点線）から徐々にずれていくことが判明した。この原因として、車輪１１のスリップなどに起因してエンコーダ１２の検出した回転角に誤差が生じることによるものと考えられる。また、推定されたカート１０の位置（軌跡）と棚２０１とが交差する場合が多く見られることが判明した。実際には、カート１０と棚２０１とが交差することはありえない。すなわち、デッドデコニングのみによるカート１０の位置の推定では、自己位置の推定の精度が悪いことが判明した。

次に、図１６に示すように、カート１０の移動経路２０６（カーブ部分の曲がり角の前後）にＲＦＩＤタグ２０を配置して、上記の「ＲＦＩＤタグのシリアル番号に基づいた補正」を行った。これにより、実際のカート１０の走行経路（太い点線、図１５参照）からのずれが抑制されることが判明した。また、推定されたカート１０の位置と棚２０１とが交差する場合も抑制されることが判明した。

次に、図１７に示すように、「ＲＦＩＤタグのシリアル番号に基づいた補正」に加えて、上記の「マップマッチングによる補正」および「カートの軌跡に基づく角度情報の補正」をさらに行った場合には、推定されたカート１０の位置と棚２０１とが交差する場合も無くなり、実際のカート１０の走行経路（太い点線、図１５参照）に略近似するように、カート１０の位置が推定されることが判明した。

図１８は、「ＲＦＩＤタグのシリアル番号に基づいた補正」、「マップマッチングによる補正」および「カートの軌跡に基づく角度情報の補正」を行った場合における、推定されたカート１０の位置の実際の位置からのずれ（誤差の累積分布）を表している。横軸は、カート１０の実際の位置からのずれ（誤差）である。また、縦軸は、各誤差の頻度（推定された回数）を表している。図１８に示すように、カート１０の位置は、誤差が１．０（ｍ）以下の範囲で略推定されている。これにより、「ＲＦＩＤタグのシリアル番号に基づいた補正」、「マップマッチングによる補正」および「カートの軌跡に基づく角度情報の補正」による補正が、カート１０の位置の推定の精度を高めることに対して、大変有効であることが確認された。

本実施形態では、以下のような効果を得ることができる。

本実施形態では、上記のように、グリッドマップ３０は隣接するブロック３１を含み、制御部１５を、推定された第１の位置情報が、１つのブロック３１から１つのブロック３１に隣接するブロック３１に移動する前後において、ジャイロセンサ１３により検出された角速度より求められたカート１０の角度情報の変化量が微小角度以下の場合で、かつ、第３の位置情報に基づいて、カート１０が複数のブロック３１に渡って略直進していると判断される場合、ジャイロセンサ１３により検出された角速度より求められたカート１０の角度情報を、直進に対応する角度情報に補正する第１の補正を行うように構成する。ここで、カート１０が略直進している場合でも、ジャイロセンサ１３の検出誤差に起因して（誤差が積算されて）、カート１０の角度が直進以外の角度であると判断される場合がある。そこで、上記のように構成することにより、カート１０が略直進している場合にカート１０が直進していないと判断されるのを抑制することができるので、カート１０の自己位置の推定の精度を高めることができる。

また、本実施形態では、上記のように、制御部１５を、隣接するブロック３１の一方のブロック３１において最初に求められたカート１０の角度情報θ１と、他方のブロック３１において最初に求められたカート１０の角度情報θ２との変化量が微小角度以下か否かを判断するように構成する。ここで、カート１０が略直進している場合には、各ブロック３１における角度情報の変化は比較的小さい。そこで、上記のように構成することによって、容易に、カート１０が略直進しているか否かを判断することができる。

また、制御部１５を、カート１０がグリッドマップ３０の複数のブロック３１に渡って、移動方向に沿った方向の座標（Ｘ座標、または、Ｙ座標）が略変化しない場合、カート１０がグリッドマップ３０の複数のブロック３０に渡って略直進していると判断する。ここで、カート１０がＸ方向（Ｙ方向）に略直進している場合には、カート１０のＸ座標（Ｙ座標）は、略変化しない。そこで、上記のように構成することによって、容易に、カート１０が略直進しているか否かを判断することができる。また、上記のように、角度情報の変化量と座標とを組み合わせてカート１０が略直進しているか否かを判断することによって、カート１０が略直進していることをより正確に判断することができる。

また、本実施形態では、上記のように、記憶部１６に、グリッドマップ３０の各々のブロック３１上をカート１０が通行可能か否かを表す通行可否フラグを記憶して、制御部１５を、通行可否フラグに基づいて第２の位置情報に対応するブロック３１が通行可能でないと判断される場合、第２の位置情報から最も近接する通行可能なブロック３１に対応する座標を、カート１０の第３の位置情報とする第２の補正を行うように構成する。これにより、第２の位置情報が実際には通行可能でないブロック３１に位置する場合（つまり、自己位置の推定が大きくずれている場合）、カート１０の位置が第３の位置情報に推定されるので、カート１０の自己位置の推定の精度をより高めることができる。

また、本実施形態では、上記のように、制御部１５を、第２の位置情報から最も近接する通行可能なブロック３１が複数ある場合に、通行可能な複数のブロック３１のうち、前回の推定されたカート１０の位置に最も近接するブロック３１に対応する座標を、カート１０の第３の位置情報として推定するように構成する。これにより、第２の位置情報から最も近接する通行可能なブロック３１が複数ある場合でも、容易に、カート１０の位置を第３の位置情報に推定することができる。また、一般的に、前回の推定されたカート１０の位置に最も近接するブロック３１が、実際にカート１０が位置するブロック３１である場合が多い。そこで、前回の推定されたカート１０の位置に最も近接するブロック３１に対応する座標を、カート１０の第３の位置情報として推定するように構成することによって、カート１０の自己位置の推定の精度をさらに高めることができる。

また、本実施形態では、上記のように、制御部１５を、通行可否フラグに基づいて第２の位置情報に対応するブロック３１が通行可能であると判断される場合、第２の補正を行わないように構成する。これにより、第２の位置情報に対応するブロック３１が通行可能である場合に、誤って、カート１０が実際に位置しないブロック３１に補正されることを抑制することができる。

また、本実施形態では、上記のように、ＲＦＩＤリーダ１４により読み出されたシリアル番号に基づいて、エンコーダ１２により検出された回転角とジャイロセンサ１３により検出されたカート１０の角速度とに基づいて推定された第１の位置情報を第２の位置情報（ＲＦＩＤタグ２０に対応する座標をカート１０の位置情報）にする第３の補正を行うように制御部１５を構成する。これにより、エンコーダ１２等の検出誤差に起因してカート１０の推定された第１の位置情報に誤差が生じた場合でも、ＲＦＩＤリーダ１４により読み出されたＲＦＩＤタグ２０の固有のシリアル番号に基づいて第２の位置情報に補正されるので、カート１０の自己位置の推定の精度をさらに高めることができる。

なお、今回開示された実施形態は、すべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は、上記した実施形態の説明ではなく特許請求の範囲によって示され、さらに特許請求の範囲と均等の意味および範囲内でのすべての変更（変形例）が含まれる。

たとえば、上記実施形態では、カートの自己位置を推定するシステムを示したが、たとえば、カート以外の移動体の自己位置を推定するようにしてもよい。

また、上記実施形態では、カートの移動経路近傍にＲＦＩＤタグを配置する例を示したが、たとえば、ＲＦＩＤタグ以外のＩＣタグ（電波を受けて働く電子装置）を配置してもよい。

また、上記実施形態では、車輪の回転角を検出するためにエンコーダを用いる例を示したが、たとえば、エンコーダ以外の機器によって車輪の回転角を検出するようにしてもよい。

また、上記実施形態では、ＲＦＩＤタグのシリアル番号が対応付けられたグリッドマップ上の座標に基づいて、第１の位置情報を第２の位置情報に補正する例を示したが、たとえば、シリアル番号以外の識別情報を用いて、第１の位置情報を第２の位置情報に補正してもよい。

また、上記実施形態では、ＲＦＩＤタグが移動経路のカーブ部分の曲がり角の前後に設けられる例を示したが、たとえば、移動経路の直線部分に設けてもよい。具体的には、移動経路の直線部分が長い場合には、移動経路の直線部分に沿ってＲＦＩＤタグを複数配置してもよい。

また、上記実施形態では、ＲＦＩＤリーダがシリアル番号を読み出した時点で、第１の位置情報を第２の位置情報に補正することを開始する例を示したが、たとえば、ＲＦＩＤリーダがシリアル番号を読み出してから所定時間経過後、第１の位置情報を第２の位置情報に補正してもよい。

また、上記実施形態では、通行可否フラグに基づいて第２の位置情報に対応するブロックが通行可能でないと判断される場合、第２の位置情報から最も近接する通行可能なブロックに対応する座標を、カートの第３の位置情報として推定する例を示したが、たとえば、第２の位置情報から最も近接する通行可能なブロック以外の通行可能なブロックに対応する座標をカートの第３の位置情報として推定してもよい。

また、上記実施形態では、カートが４つのブロックに渡って略直進していると判断される場合（でかつ、カートの角度情報の変化量が所定の値以下の場合）、カートの角度情報を直進に対応する角度情報に補正する例を示したが、たとえば、４つのブロック以外の複数のブロックに渡って略直進していると判断される場合、カートの角度情報を直進に対応する角度情報に補正してもよい。

また、上記実施形態では、カートの角度情報の変化量が微小角度以下の場合（でかつ、カートが４つのブロックに渡って略直進していると判断される場合）、カートの角度情報を直進に対応する角度情報に補正する例を示したが、たとえば、カートの角度情報の変化量が、微小角度以外の所定の値以下の場合に、カートの角度情報を直進に対応する角度情報に補正してもよい。

また、上記実施形態では、ＲＦＩＤタグのシリアル番号が対応付けられたグリッドマップ上の座標に基づいて、第１の位置情報を第２の位置情報に補正する第３の補正を行う例を示したが、ＲＦＩＤタグのシリアル番号以外のブロックの座標に対応する座標識別情報に基づいて、第１の位置情報を第２の位置情報に補正する第３の補正を行うようにしてもよい。

また、上記実施形態では、「ＲＦＩＤタグのシリアル番号に基づいた補正」、「マップマッチングによる補正」および「カートの軌跡に基づく角度情報の補正」の全てを行う例を示したが、たとえば、これらの補正のうちのいずれか１つまたは２つを行ってもよい。たとえば、図１６に示すように、「ＲＦＩＤタグのシリアル番号に基づいた補正」のみを行った場合でも、カートの位置の推定の精度をある程度高めることが可能である。

また、上記実施形態では、制御部、記憶部および表示部がカートに設けられる例を示したが、たとえば、制御部、記憶部および表示部をカートとは別個に（たとえば、ＰＣ（パーソナル コンピュータ）などに）設けてもよい。

１０ カート（移動体）
１１ 車輪
１２ エンコーダ（回転角検出器）
１３ ジャイロセンサ
１５ 制御部
１６ 記憶部
２０ ＲＦＩＤタグ（座標識別部）
３０ グリッドマップ
３１ ブロック
１００ 自己位置推定システム
２００ 所定区画
２０３ 移動経路

Claims (8)

1. 所定区画内を走行する移動体の自己位置を推定する自己位置推定システムであって、
   前記移動体の車輪の回転角を検出する回転角検出器と、
   前記移動体の角速度を検出するジャイロセンサと、
   前記回転角検出器により検出された回転角と、前記ジャイロセンサにより検出された前記移動体の角速度とに基づいて、前記移動体の前記所定区画内における位置情報を推定する制御部とを備え、
   前記制御部は、前記ジャイロセンサにより検出された角速度より求められた前記移動体の角度情報の変化量が所定の値以下の場合で、かつ、前記移動体の推定位置に基づいて、前記移動体が略直進していると判断される場合、前記ジャイロセンサにより検出された角速度より求められた前記移動体の角度情報を、直進に対応する角度情報に補正する第１の補正を行うように構成されている、自己位置推定システム。
2. 前記移動体が走行する前記所定区画内に対応して座標を設定したグリッドマップが記憶される記憶部をさらに備え、
   前記グリッドマップは、第１のブロックと、前記第１のブロックに隣接する第２のブロックとを含み、
   前記制御部は、前記推定された位置情報が、前記第１のブロックから前記第２のブロックに移動する前後において、前記移動体の角度情報の変化量が前記所定の値以下の場合で、かつ、前記移動体が前記グリッドマップの複数のブロックに渡って略直進していると判断される場合、前記ジャイロセンサにより検出された角速度より求められた前記移動体の角度情報を、直進に対応する角度情報に補正する前記第１の補正を行うように構成されている、請求項１に記載の自己位置推定システム。
3. 前記制御部は、前記第１のブロックにおいて最初に求められた移動体の角度情報と、前記第２のブロックにおいて最初に求められた移動体の角度情報との変化量が前記所定の値以下の場合で、かつ、前記移動体が前記グリッドマップの複数の前記ブロックに渡って、移動方向に沿った方向の座標が略変化しない場合、前記移動体が前記グリッドマップの複数の前記ブロックに渡って略直進していると判断して、前記ジャイロセンサにより検出された角速度より求められた前記移動体の角度情報を、直進に対応する角度情報に補正する前記第１の補正を行うように構成されている、請求項２に記載の自己位置推定システム。
4. 前記記憶部は、前記グリッドマップの各々のブロック上を前記移動体が通行可能か否かを表す通行可否情報を記憶しており、
   前記制御部は、
   前記通行可否情報に基づいて前記推定された位置情報に対応する前記ブロックが通行可能でないと判断される場合、前記推定された位置情報から最も近接する通行可能な前記ブロックに対応する座標を、前記移動体の位置情報と推定する第２の補正を行い、
   前記第２の補正によって補正された位置情報が、前記第１のブロックから前記第２のブロックに移動する前後において、前記移動体の角度情報の変化量が前記所定の値以下の場合で、かつ、前記第２の補正によって補正された位置情報に基づいて、前記移動体が複数の前記ブロックに渡って略直進していると判断される場合、前記ジャイロセンサにより検出された角速度より求められた前記移動体の角度情報を、直進に対応する角度情報に補正する前記第１の補正を行うように構成されている、請求項２または３に記載の自己位置推定システム。
5. 前記制御部は、前記推定された位置情報から最も近接する通行可能な前記ブロックが複数ある場合に、前記通行可能な複数のブロックのうち、前回の推定された前記移動体の位置情報に最も近接する前記ブロックに対応する座標を、前記移動体の位置情報と推定する前記第２の補正を行うように構成されている、請求項４に記載の自己位置推定システム。
6. 前記制御部は、前記通行可否情報に基づいて前記推定された位置情報に対応する前記ブロックが通行可能であると判断される場合、前記第２の補正を行わないように構成されている、請求項４または５に記載の自己位置推定システム。
7. 前記所定区画内に配置され、前記移動体の移動経路の座標に対応する座標識別情報を有する座標識別部をさらに備え、
   前記制御部は、前記座標識別情報に対応する座標を前記移動体の位置情報と推定する第３の補正を行うように構成されている、請求項１〜６のいずれか１項に記載の自己位置推定システム。
8. 所定区画内を走行する移動体の自己位置を推定する自己位置推定方法であって、
   回転角検出器により検出された車輪の回転角と、ジャイロセンサにより検出された前記移動体の角速度とに基づいて、前記移動体の前記所定区画内における位置情報を推定することと、
   前記ジャイロセンサにより検出された角速度より求められた前記移動体の角度情報の変化量が所定の値以下の場合で、かつ、前記移動体の位置情報に基づいて、前記移動体が略直進していると判断される場合、前記ジャイロセンサにより検出された角速度より求められた前記移動体の角度情報を、直進に対応する角度情報に補正することとを備える、自己位置推定方法。

Images