向き計測装置、向き計測システム、向き計測方法、及び向き計測プログラム
【課題】移動装置が、自装置の方向を正確に測定できること。
【解決手段】位置積分部１１６２は、位置の変化量を積分して移動ロボット１の位置を示す推定位置情報を推定する。方向積分部１１６１は、方向の変化量を積分して移動装置の方向を示す推定方向情報を推定する。位置補正部１１６３は、絶対位置測定装置から受信した位置情報であって時々刻々の移動ロボット１の位置を示す絶対位置情報と、時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出する。方向補正部１１６４は、方向補正値を用いて、推定方向情報が示す方向を補正する。
【解決手段】位置積分部１１６２は、位置の変化量を積分して移動ロボット１の位置を示す推定位置情報を推定する。方向積分部１１６１は、方向の変化量を積分して移動装置の方向を示す推定方向情報を推定する。位置補正部１１６３は、絶対位置測定装置から受信した位置情報であって時々刻々の移動ロボット１の位置を示す絶対位置情報と、時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出する。方向補正部１１６４は、方向補正値を用いて、推定方向情報が示す方向を補正する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、向き計測装置、向き計測システム、向き計測方法、及び向き計測プログラムに関する。
【背景技術】
【０００２】
近年、人の活動するエリアで人にサービスを行うロボットに関する技術が知られてきている。特許文献１には、パーティクルフィルタを用いて、人の位置および移動速度を推定し、胴体および両腕を３つの円で組み合わせた人形状モデルを用いて、人の身体の方向と腕の動きとを推定する計測装置が記載されている。
【０００３】
一方、移動ロボットの技術分野において、移動ロボットが自らの位置を検出する技術が知られている。例えば、特許文献２には、移動ロボットの運動によって変化する状態量を感知し、移動ロボットと固定位置との距離をセンシングして移動ロボットの絶対位置を計算し、変化する状態量及び計算された絶対位置をカルマンフィルタの入力として、現在移動ロボットの最適位置を推定する移動ロボットが記載されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００９−１６８５７８号公報
【特許文献２】特開２００７−１４９０８８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、引用文献１記載の技術では、人を真上から撮像できない場合や、人が遠い場所に位置して十分な画質の画像を得ることができない場合に、人形状モデルを適用することができず、人の身体の方向を測定できないという欠点があった。また、引用文献１記載の技術では、上半身の方向を測定するものであり、例えば身体を腰部分で捻っている場合には、下半身、つまり、足の進行方向を測定できないという欠点があった。
また、引用文献２記載の技術では、移動ロボット各々が、距離をセンシングする機能や最適位置を推定する機能を備えなければ、位置を正確に測定できず、また、方向を正確に測定できないという欠点があった。
以上のように、従来技術では、移動装置の向きを正確に測定できない場合があるという欠点があった。
【０００６】
本発明は上記の点に鑑みてなされたものであり、移動装置の向きを正確に測定できる向き計測装置、向き計測システム、向き計測方法、及び向き計測プログラムを提供する。
【課題を解決するための手段】
【０００７】
本発明は上記の課題を解決するためになされたものであり、本発明は、自装置の位置の変化量（例えば、実施形態における相対位置情報）と方向の変化量（例えば、実施形態における相対方向情報）とを検出する移動装置（例えば、実施形態における移動ロボット１）に搭載される向き計測装置（例えば、実施形態における位置方向補正部１１６）において、前記位置の変化量を積分して前記移動装置の位置を示す推定位置情報を推定する位置積分部（例えば、実施形態における位置積分部１１６２）と、前記方向の変化量を積分して前記移動装置の方向を示す推定方向情報（例えば、実施形態における推定正面方向情報）を推定する方向積分部（例えば、実施形態における方向積分部１１６１）と、移動装置外の装置（例えば、実施形態における絶対位置測定装置ｃ１）から受信した位置情報であって時々刻々の移動装置の位置を示す絶対位置情報と、前記時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出する方向補正値算出部（例えば、実施形態における位置補正部１１６３）と、前記方向補正値を用いて、前記推定方向情報が示す方向を補正する方向補正部（例えば、実施形態における方向補正部１１６４）と、を備えることを特徴とする向き計測装置である。
【０００８】
また、本発明は、上記の向き計測装置において、前記方向補正値算出部は、複数の時刻の絶対位置情報と推定位置情報とを用いて方向補正値を算出することを特徴とする。
【０００９】
また、本発明は、上記の向き計測装置において、前記移動装置が移動した距離に基づいて、前記方向補正値を算出する処理を開始するタイミングを決定する補正開始命令部（例えば、実施形態における補正開始命令部１１９）を備え、前記方向補正値算出部は、前記補正開始命令部が決定したタイミングで、方向補正値を算出することを特徴とする。
【００１０】
また、本発明は、自装置の位置の変化量と方向の変化量とを検出する移動装置に搭載される向き計測装置と、時々刻々の前期移動装置の位置を示す絶対位置情報を測定して前記向き計測装置に送信する位置測定装置と、を具備する向き計測システム（例えば、実施形態における向き計測システムＳ１）において、前記向き計測装置は、前記位置の変化量を積分して前記移動装置の位置を示す推定位置情報を推定する位置推定部と、前記方向の変化量を積分して前記移動装置の方向を示す推定方向情報を推定する方向推定部と、前記位置測定装置から受信した絶対位置情報と、前記時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出する方向補正値算出部と、前記方向補正値を用いて、前記推定方向情報が示す方向を補正する方向補正部と、を備えることを特徴とする向き計測システムである。
【００１１】
また、本発明は、自装置の位置の変化量と方向の変化量とを検出する移動装置に搭載される向き計測装置における向き計測方法おいて、位置積分部が、前記位置の変化量を積分して前記移動装置の位置を示す推定位置情報を推定する過程と、方向積分部が、前記方向の変化量を積分して前記移動装置の方向を示す推定方向情報を推定する過程と、方向補正値算出部が、移動装置外の装置から受信した位置情報であって時々刻々の移動装置の位置を示す絶対位置情報と、前記時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出する過程と、方向補正部が、前記方向補正値を用いて、前記推定方向情報が示す方向を補正する過程と、を有することを特徴とする向き計測方法である。
【００１２】
また、本発明は、自装置の位置の変化量と方向の変化量とを検出する移動装置に搭載される向き計測装置のコンピュータを、前記位置の変化量を積分して前記移動装置の位置を示す推定位置情報を推定する位置推定手段、前記方向の変化量を積分して前記移動装置の方向を示す推定方向情報を推定する方向推定手段、移動装置外の装置から受信した位置情報であって時々刻々の移動装置の位置を示す絶対位置情報と、前記時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出する方向補正値算出手段、前記方向補正値を用いて、前記推定方向情報が示す方向を補正する方向補正手段、
として機能させる向き計測プログラムである。
【発明の効果】
【００１３】
請求項１に記載した発明によれば、向き計測装置は、位置の変化量を積分して移動装置の位置を示す推定位置情報を推定し、方向の変化量を積分して移動装置の方向を示す推定方向情報を推定する。また、向き計測装置は移動装置外の装置から受信した位置情報であって時々刻々の移動装置の位置を示す絶対位置情報と、時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出し、方向補正値を用いて推定方向情報が示す方向を補正する。これにより、向き計測装置は、移動装置の向きを正確に測定できる。
【００１４】
また、請求項２に記載した発明によれば、向き計測装置は、複数の時刻の絶対位置情報と推定位置情報とを用いて方向補正値を算出する。これにより、向き計測装置は、１個の絶対位置情報と推定位置情報とを用いる場合と比較して、精度よく移動装置の向きを測定できる。
【００１５】
また、請求項３に記載した発明によれば、向き計測装置は、移動装置が移動した距離に基づいて、方向補正値を算出する処理を開始するタイミングを決定する。これにより、向き計測装置は、移動装置が移動した距離に応じて移動局装置の向きを補正することができる。
【図面の簡単な説明】
【００１６】
【図１】本発明の実施形態に係る向き計測システムＳ１を示す概略図である。
【図２】本実施形態に係る移動ロボットの構成を示す概略ブロック図である。
【図３】本実施形態に係る推定位置、絶対位置、及び推定正面方向の関係を示す概略図である。
【図４】本実施形態に係る補正値算出処理の一例を表わす概略図である。
【図５】本実施形態に係る位置方向補正処理を行うタイミングの一例を示す概略図である。
【発明を実施するための形態】
【００１７】
（第１の実施形態）
以下、図面を参照しながら本発明の実施形態について詳しく説明する。
図１は、本発明の実施形態に係る向き計測システムＳ１を示す概略図である。向き計測システムＳ１は、移動ロボット１（移動装置）及び絶対位置測定装置ｃ１を具備する。
図１は、建物の部屋の内部の図である。この図において、移動ロボット１は、部屋の中を移動する。ここで、符号１１、１２、１３を付した移動ロボット１は、それぞれ、動作時刻Ｔが対象時刻ｔの移動ロボット１、対象時刻ｔの１秒前の移動ロボット１、対象時刻ｔのｎ秒前の移動ロボット１を示す。この図において、矢印を付した軌跡は、移動ロボット１が移動した軌跡を示す。
【００１８】
また、図１の部屋の天井には、絶対位置測定装置ｃ１が設置されている。絶対位置測定装置ｃ１は、撮像機能を有し、また、撮像した画像に基づいて移動ロボット１の位置情報（絶対位置情報という）を測定する機能を有する。ここで、絶対位置測定装置ｃ１は、測定した移動ロボット１の絶対位置情報を、重力方向に垂直（床に水平）な座標（図１のＸ軸、Ｙ軸；グローバル座標という）上の値として測定する。
具体的には、絶対位置測定装置ｃ１は、十分に高い位置に設置され、移動ロボット１をほぼ真上から撮像する。絶対位置測定装置ｃ１は、撮像した画像の移動ロボット１部分に対して円近似を行って、その円の中心を移動ロボット１の絶対位置情報とする。絶対位置測定装置ｃ１は、無線通信等を用いて、単位時間（本実施形態では１秒とする）毎に、測定した絶対位置情報と測定した時刻を示す絶対位置測定時刻とからなる情報を移動ロボット１に送信する。
なお、絶対位置測定装置ｃ１のように、建物等に設置され、物体の位置情報等を測定するセンサをインフラセンサという。また、絶対位置測定装置ｃ１は、物体の位置情報を測定できる装置であればよく、例えば、レーザ光を用いて測定する装置であってもよい。また、天井以外の場所に設置されていてもよく、異なる位置に設置された複数の装置であってもよい。
【００１９】
また、図１の部屋の床には、マークＭ１１〜Ｍ１４が付されている。このマークＭ１１〜Ｍ１４各々は、グローバル座標のＹ軸に平行な２本の線であり、各々の線は色が異なる（例えば、Ｘ軸での値が大きい線は赤色の線、値が小さい線は青色の線）。移動ロボット１は、腰部分の側面に備え付けられたカメラで床を撮像し、画像認識によってマークＭ１１〜Ｍ１４のいずれかを検出する。移動ロボット１は、撮像する画像において正面方向（進行方向）と垂直になる線の画像を予め記憶しており、その線の画像と検出した線の画像との角度差を、グローバル座標上のＸ軸に対する角度（θｚ）として算出する。つまり、この角度は、移動ロボット１は、グローバル座標での正面方向（進行方向）を示す角度である。
なお、グローバル座標は、線に垂直で青色の線から赤色の線に向かう方向がＸ軸の方向であり、Ｘ軸を９０度回転させた軸がＹ軸である。また、マークＭ１１〜Ｍ１４は、移動ロボット１が、絶対位置測定装置ｃ１の用いるグローバル座標での方向を検出できるものであればよく、他の形状のマークであってもよい。また、移動ロボット１は、方位磁針を備え、予め定められた方位とグローバル座標の関係に基づいて正面方向を算出してもよい。
【００２０】
＜移動ロボット１の構成について＞
図２は、本実施形態に係る移動ロボット１の構成を示す概略ブロック図である。この図において、移動ロボット１は、ジャイロスコープ１０１、駆動部１０２、通信部１０３、方向検出部１０４、及び位置方向制御部１１を含んで構成される。また、位置方向制御部１１は、エンコーダ部１１１、相対方向情報記憶部１１２、相対位置情報記憶部１１３、絶対位置情報記憶部１１４、初期方向情報記憶部１１５、位置方向補正部１１６、位置方向情報記憶部１１７、駆動制御部１１８、及び、補正開始命令部１１９を含んで構成される。ここで、位置方向補正部１１６（向き計測装置）は、方向積分部１１６１、位置積分部１１６２、位置補正部１１６３、及び、方向補正部１１６４を含んで構成される。
【００２１】
ジャイロスコープ１０１は、角度や角速度を検出する計測器である。ジャイロスコープ１０１は、単位時間毎に検出した角度及び角速度をエンコーダ部１１１に出力する。
駆動部１０２は、駆動制御部１１８の制御に従い、移動ロボット１の移動手段（二足歩行型ロボットの場合は両足）を動かすことで、移動ロボット１を移動させる。駆動部１０２は、予め定められた時間間隔（駆動検出時間という。例えば、１／２００秒）毎に、動作時刻Ｔと動作結果情報とをエンコーダ部１１１に出力する。
ここで、動作結果情報とは、動作時刻Ｔの駆動検出時間での進行方向と進行距離とであり、例えば、移動ロボット１が二足歩行型ロボットの場合は、動作時刻Ｔに足をどの方向にどの距離踏み出したかを示す情報である。なお、動作時刻Ｔの進行方向（足を踏み出した方向）は、正面方向に対する相対的な方向である。また、例えば、移動ロボット１が車輪型移動ロボットの場合には、動作結果情報は、各車輪についての回転方向及び回転数を示す情報である。
【００２２】
通信部１０３は、絶対位置測定装置ｃ１と無線通信を行う。通信部１０３は、絶対位置情報と絶対位置測定時刻とからなる情報を、絶対位置測定装置ｃ１から受信し、絶対位置情報記憶部１１４に記憶する。
方向検出部１０４は、図１のマークＭ１１〜Ｍ１４に基づいて、絶対位置測定装置ｃ１の用いるグローバル座標で、自装置の正面方向を算出する。方向検出部１０４は、算出した正面方向を示す初期方向情報と、初期方向情報を算出した時刻を示す初期方向算出時刻と、からなる情報を初期方向情報記憶部１１５に記憶する。
【００２３】
エンコーダ部１１１は、駆動部１０２から入力された動作結果情報に基づいて、移動ロボット１について、前後の動作時刻Ｔでの方向の差を示す相対方向情報、及び、前後の動作時刻Ｔでの位置の差を示す相対位置情報を算出する。すなわち、エンコーダ部１１１は、移動ロボット１の位置の変化量と方向の変化量とを検出する。
なお、エンコーダ部１１１は、ジャイロスコープ１０１から入力された角度及び角速度を用いて、相対方向情報の精度を上げてもよい。エンコーダ部１１１は、動作時刻Ｔと算出した相対方向情報とからなる情報を相対方向情報記憶部１１２に記憶し、動作時刻と算出した相対位置情報とからなる情報を相対位置情報記憶部１１３に記憶する。
【００２４】
方向積分部１１６１は、方向補正部１１６４から基準方向情報及び基準方向測定時刻を入力される。この基準方向情報とは、グローバル座標での方向を示す情報であり、基準方向測定時刻での移動ロボット１の正面方向を示す情報である。
方向積分部１１６１は、相対方向情報記憶部１１２から動作時刻Ｔと相対方向情報とからなる情報を読み出す。方向積分部１１６１は、基準方向情報を初期値として、基準方向測定時刻から動作時刻Ｔまでの相対方向情報を加算することで、移動ロボット１のグローバル座標における動作時刻Ｔでの正面方向を推定する。方向積分部１１６１は、駆動検出時間の動作時刻Ｔ毎に、推定した正面方向（推定正面方向という）を示す推定正面方向情報（推定方向情報）と動作時刻Ｔとからなる情報を方向補正部１１６４に出力する。
【００２５】
位置積分部１１６２は、位置補正部１１６３から基準位置情報及び基準位置測定時刻を入力される。この基準位置情報とは、グローバル座標での位置情報であり、基準位置測定時刻での移動ロボット１の位置を示す情報である。
位置積分部１１６２は、相対位置情報記憶部１１３から動作時刻と相対位置情報とからなる情報を読み出す。位置積分部１１６２は、基準位置情報を初期値として、基準位置測定時刻から動作時刻Ｔまでの相対位置情報を加算することで、移動ロボット１のグローバル座標における動作時刻Ｔでの位置を推定する。方向積分部１１６１は、駆動検出時間の動作時刻Ｔ毎に、推定した位置（推定位置という）を示す推定位置情報と動作時刻Ｔとからなる情報を位置補正部１１６３及び補正開始命令部１１９に出力する。
【００２６】
位置補正部１１６３（方向補正値算出部）は、位置積分部１１６２から入力された情報のうち、単位時間毎の推定位置情報であって動作時刻Ｔが対象時刻ｔから時刻ｔ−ｎの期間の推定位置情報を抽出する。また、位置補正部１１６３は、絶対位置情報記憶部１１４が記憶する情報のうち、絶対位置測定時刻が対象時刻ｔから時刻ｔ−ｎの期間の絶対位置情報を選択して読み出す。
位置補正部１１６３は、抽出した推定位置情報と読み出した絶対位置情報とを同定して、対象時刻ｔから時刻ｔ−ｎの期間について、推定位置と絶対位置とのずれを計算する。位置補正部１１６３は、計算したずれに対して、推定位置のずれを補正する補正値（ΔＸ、ΔＹ）と方向のずれを補正する補正値（Δθｚ；方向補正値）とを算出する。なお、補正値算出処理の詳細については、後述する位置方向補正処理と併せて説明をする。位置補正部１１６３は、算出した方向補正値を方向補正部１１６４に出力する。
【００２７】
また、位置補正部１１６３は、絶対位置測定時刻が対象時刻ｔの絶対位置情報及び対象時刻ｔを、それぞれ、基準位置情報及び基準位置測定時刻として、位置積分部１１６２に出力する。ここで、位置補正部１１６３は、位置積分部１１６２が初めて処理を開始する時点では、基準位置情報及び基準位置測定時刻として、それぞれ、絶対位置情報記憶部１１４が記憶する絶対位置情報及び絶対位置測定時刻を位置積分部１１６２に出力する。また、位置補正部１１６３は、絶対位置情報記憶部１１４から読み出した情報を方向補正部１１６４に出力する。
【００２８】
方向補正部１１６４は、方向積分部１１６１から入力された情報のうち、動作時刻Ｔが対象時刻ｔから時刻ｔ−ｎの期間の推定正面方向情報を抽出する。方向補正部１１６４は、抽出した推定正面方向情報が示す推定正面方向に、位置補正部１１６３から入力された方向補正値を加算する。すなわち、方向補正部１１６４は、方向補正値を用いて、推定正面方向情報が示す方向を補正する。以下、加算後の方向を補正方向といい、補正方向を示す情報を補正方向情報という。
方向補正部１１６４は、補正方向情報、動作時刻Ｔ（対象時刻ｔから時刻ｔ−ｎ）、及び、位置補正部１１６３から入力された動作時刻Ｔでの絶対位置情報を、動作時刻Ｔでの位置方向情報として、位置方向情報記憶部１１７に記憶する。つまり、位置方向情報記憶部１１７は、対象時刻ｔから時刻ｔ−ｎの期間の動作時刻Ｔについて、それぞれ、位置と方向が補正された位置方向情報を記憶する。
【００２９】
また、方向補正部１１６４は、動作時刻Ｔが対象時刻ｔの補正方向情報及び対象時刻ｔを、それぞれ、基準方向情報及び基準方向測定時刻として、方向積分部１１６１に出力する。ここで、方向補正部１１６４は、方向積分部１１６１が初めて処理を開始する時点では、基準方向情報及び基準方向測定時刻として、それぞれ、初期方向情報記憶部１１５が記憶する初期方向情報及び初期方向算出時刻を方向積分部１１６１に出力する。
なお、位置方向補正部１１６、つまり、方向積分部１１６１、位置積分部１１６２、位置補正部１１６３、及び方向補正部１１６４が行う上記の処理を、位置方向補正処理という。
【００３０】
駆動制御部１１８は、位置方向情報記憶部１１７が記憶する位置方向情報に基づいて、駆動部の制御を行う。
補正開始命令部１１９は、相対位置情報記憶部１１３から読み出した情報に基づいて、移動ロボット１が、前回位置方向補正処理を行った時点から予め定めた距離を移動したか否かを判定する。補正開始命令部１１９は、予め定めた距離を移動したと判定した場合、位置方向補正処理を開始させる信号を、位置方向補正部１１６の位置補正部１１６３及び方向補正部１１６４に出力する。
すなわち、補正開始命令部１１９は、移動ロボット１が移動した距離に基づいて、方向補正値を算出する処理を開始するタイミングを決定する。ここで、この予め定めた距離は、２〜３ｍ程度が好ましい。この距離が大きい場合には、移動ロボット１の内部オドメトリ軌跡（図３参照）の形状と移動ロボット１のインフラセンサ軌跡の形状との差が大きくなってしまい、この距離が小さい場合には、方向の誤差が大きくなってしまうからである。
【００３１】
＜位置方向補正処理について＞
以下、位置補正部１１６３が行う位置方向補正処理について説明をする。
図３は、本実施形態に係る推定位置、絶対位置、及び推定正面方向の関係を示す概略図である。この図において、座標はグローバル座標であり、横軸がＸ軸、縦軸がＹ軸である。
図３において、符号Ｌ１１を付した軌跡Ｌ１１は、対象時刻ｔから時刻ｔ−ｎの期間の推定位置であって単位時間毎の推定位置（白抜き矢印の始点）を示す軌跡（移動ロボット１の内部オドメトリ軌跡）である。また、各推定位置での白抜きの矢印は、各推定位置での推定正面方向を示す。一方、符号Ｌ１２を付した軌跡Ｌ１２は、対象時刻ｔから時刻ｔ−ｎの期間の絶対位置（黒丸）を示す軌跡（移動ロボット１のインフラセンサ軌跡；図１に示した軌跡）である。また、この図において、点線の矢印で対応付けられた推定位置及び絶対位置は、同じ動作時刻Ｔのものである。
【００３２】
例えば、図３において、符号Ｐ１１１、Ｐ１２１を付した位置Ｐ１１１、Ｐ１２１は、それぞれ、動作時刻Ｔ＝ｔ−ｎでの推定位置、絶対位置を示す。また、符号Ｄ１２１を付した矢印Ｄ１２１は、動作時刻Ｔ＝ｔ−ｎでの推定正面方向を示す。また、例えば、位置Ｐ１１２、Ｐ１２２、及び、符号Ｄ１２２を付した矢印Ｄ１２１は、それぞれ、動作時刻Ｔ＝ｔでの推定位置、絶対位置、及び推定正面方向を示す。この図が示すように、推定位置は、絶対位置からずれる場合があるが、軌跡Ｌ１１及びＬ１２はほとんど同じ形の軌跡となる。
【００３３】
図４は、本実施形態に係る補正値算出処理の一例を表わす概略図である。この図において、座標はグローバル座標であり、横軸がＸ軸、縦軸がＹ軸である。
符号Ｆ４１を付した図Ｆ４１は、図３と同じ図であり、補正値算出処理を行う前の推定位置、絶対位置、及び推定正面方向の関係を示す図である。また、図Ｆ４１は、位置補正部１１６３がアフィン変換量（ΔＸ、ΔＹ、Δθｚ）を算出していることを示す。
【００３４】
具体的には、位置補正部１１６３は、対象時刻ｔから時刻ｔ−ｎの期間について絶対位置の平均値と相対位置の平均値とを算出し、これらの平均値の差（ΔＸ、ΔＹ；位置補正値という）を算出する。位置補正部１１６３は、算出した位置補正値（ΔＸ、ΔＹ）を、対象時刻ｔから時刻ｔ−ｎの期間の相対位置に加算する。
位置補正部１１６３は、対象時刻ｔから時刻ｔ−ｎの期間の絶対位置について、最小二乗法を用いて近似した一次方程式の傾きを計算する。また、位置補正部１１６３は、位置補正値を加算した相対位置について、最小二乗法を用いて近似した一次方程式の傾きを計算する。位置補正部１１６３は、計算した傾きが示す角度（Ｘ軸からの角度を変数θｚとする）の差を、方向補正値Δθｚとして算出する。ただし、本発明はこれに限らず、位置補正部１１６３は、矢印で対応付けられた推定位置及び絶対位置同士の距離の総和が最小となるようなΔＸ、ΔＹ、Δθｚを求めてもよい。
【００３５】
符号４２を付した図Ｆ４２は、絶対位置、位置補正値を加算した推定位置、及び補正方向を示す図である。
例えば、図Ｆ４２おいて、符号Ｄ２２１を付した白抜きの矢印Ｄ２２１は、対象時刻ｔでの補正方向を示す。この矢印Ｄ２２１は、図Ｆ４１の矢印Ｄ１２１が示す方向に、方向補正値Δθｚを加算した方向を示す。
【００３６】
図５は、本実施形態に係る位置方向補正処理を行うタイミングの一例を示す概略図である。この図において、座標はグローバル座標であり、横軸がＸ軸、縦軸がＹ軸である。この図において、位置Ｐ２１１、Ｐ２１２、Ｐ２１３、Ｐ２１４、Ｐ２１５を含む軌跡（破線と実線の両方）は、推定位置を示す軌跡である。また、位置Ｐ２２０、Ｐ２２１、Ｐ２２２、Ｐ２２３、Ｐ２２４、Ｐ２２５を含む軌跡は、絶対位置を示す軌跡である。例えば、位置Ｐ２１１、Ｐ２１２、Ｐ２１１、Ｐ２２２が、それぞれ、図３の位置Ｐ１１１、Ｐ１１２、Ｐ１２１、Ｐ１２２である。
図５は、Ｐ２２２、Ｐ２２３、Ｐ２２５の時点で、位置方向補正処理が行われたことを示す。また、この図において、符号ＩＤ１、ＩＤ２を付した白抜きの矢印ＩＤ１、ＩＤ２は、方向検出部１０４が検出した初期方向情報が示す方向を表わす。
【００３７】
以下、図５及び図２を用いて、位置方向制御部１１の動作の一例について説明をする。
位置補正部１１６３は、位置Ｐ２２０を示す絶対位置情報及び絶対位置測定時刻を、基準位置情報及び基準位置測定時刻として、位置積分部１１６２に出力する。また、方向補正部１１６４は、矢印ＩＤ１が表わす方向を示す初期方向情報及び初期方向算出時刻を、基準方向情報及び基準方向測定時刻として、方向積分部１１６１に出力する。
【００３８】
方向積分部１１６１及び位置積分部１１６２は、それぞれ、入力された初期値に基づいて、推定正面方向及び推定位置を算出する。例えば、位置積分部１１６２は、推定位置として位置Ｐ２１１、２１２を算出する。また、通信部１０３は、絶対位置情報を受信する。この絶対位置情報は、例えば、位置Ｐ２２１、Ｐ２２２である。
補正開始命令部１１９は、位置Ｐ２２０から位置Ｐ２１２までの推定位置の軌跡に沿った距離が、予め定めた距離を超えたか否かを判定する。図５は、推定位置Ｐ２１２の時点（対象時刻ｔ＝ｔ１とする）で予め定めた距離を超えたと判定した場合の図である。補正開始命令部１１９は、対象時刻ｔ１で、位置方向補正処理を開始させる信号を、位置補正部１１６３及び方向補正部１１６４に出力する。
【００３９】
位置補正部１１６３は、時刻ｔ１−ｎから対象時刻ｔ１まで（図５では、位置Ｐ２１１、Ｐ２２１の時点）の期間の推定位置情報及び絶対位置情報に基づいて、方向補正値を算出する。方向補正部１１６４は、推定方向に位置補正部１１６３が算出した方向補正値を加算し、補正方向を算出する。位置補正部１１６３は対象時刻ｔ１での絶対位置Ｐ２２２を示す基準位置情報を位置積分部１１６２に出力する。また、方向補正部１１６４は対象時刻ｔ１での補正方向を示す基準方向情報を方向積分部１１６１に出力する。
【００４０】
方向積分部１１６１は入力された基準方向情報に基づいて推定正面方向を算出し、位置積分部１１６２は入力された基準位置情報に基づいて推定位置を算出する。同様に、補正開始命令部１１９は、推定位置Ｐ２１３の時点で、位置方向補正処理を開始させる信号を出力する。位置方向補正部１１６は、この信号に基づいて、位置方向補正処理を行う。
【００４１】
また、図５は、位置Ｐ２２４の時点（推定Ｐ２１４の時点；時刻ｔ２とする）で、方向検出部１０４が、矢印ＩＤ２が表わす方向を検出したことを示す。この場合、方向補正部１１６４は、この方向を示す初期方向情報及び初期方向算出時刻を、基準方向情報及び基準方向測定時刻として、方向積分部１１６１に出力する。
また、図５は、推定位置Ｐ２１３の時点（対象時刻ｔ＝ｔ３とする）で、位置Ｐ２２３から位置Ｐ２１５までの推定位置の軌跡に沿った距離が、予め定めた距離を超えたと判定した場合の図である。ここで、図５の一例では、時刻ｔ２は、時刻ｔ３−ｎより遅い時刻である。この場合、位置補正部１１６３は、時刻ｔ２から対象時刻ｔ３までの期間の推定位置情報及び絶対位置情報に基づいて、方向補正値を算出する。つまり、位置補正部１１６３は、時刻ｔ−ｎと、方向検出部１０４が方向を検出した時刻と、のいずれか遅い方の時刻から、対象時刻ｔまでの推定位置情報及び絶対位置情報に基づいて、方向補正値を算出する。
【００４２】
このように、本実施形態によれば、移動ロボット１の位置方向制御部１１は、位置の変化量を積分して移動ロボットの位置を示す推定位置情報を推定し、方向の変化量を積分して移動ロボットの正面方向を示す推定正面方向情報を推定する。また、位置方向制御部１１は、絶対位置測定装置ｃ１から受信した位置情報であって時々刻々の移動装置の位置を示す絶対位置情報と、時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出し、方向補正値を用いて推定正面方向情報が示す正面方向を補正する。これにより、位置方向制御部１１は、移動装置の向きを正確に測定できる。
また、本実施形態では、移動ロボット１以外の装置（絶対位置測定装置ｃ１）が移動ロボット１の位置のみを測定できればよいので、絶対位置測定装置ｃ１が方向を検出できない場合でも、位置方向制御部１１は、移動装置の向きを正確に測定できる。また、位置方向制御部１１は、距離をセンシングする機能や最適位置を推定する機能を備えなくても、移動装置の向きを正確に測定できる。
さらに、位置方向制御部１１は、正面方向、つまり、進行方向を正確に測定できるので、進行方向と他の部位（例えば、上半身の正面方向）の方向の相対方向を検出することにより、他の部位の方向も正確に測定することができる。
【００４３】
また、本実施形態によれば、位置方向制御部１１は、複数の時刻の絶対位置情報と推定位置情報とを用いて方向補正値を算出する。これにより、位置方向制御部１１は、１個の絶対位置情報と推定位置情報とを用いる場合と比較して、精度よく移動装置の向きを測定できる。
また、本実施形態によれば、位置方向制御部１１は、移動ロボット１が移動した距離に基づいて、方向補正値を算出する処理を開始するタイミングを決定する。これにより、位置方向制御部１１は、移動ロボット１が移動した距離に応じて移動局ロボット１の向きを補正することができる。
【００４４】
なお、上記実施形態において、位置方向制御部１１を移動ロボット１が備える場合について説明をしたが、本発明はこれに限らず、位置方向制御部１１は、車両（自動二輪車等も含む）や車椅子等の自動で移動するものに備えられてもよい。
また、上記実施形態において、移動ロボット１が部屋の中を移動する場合について説明したが、本発明はこれに限らず、移動ロボット１（又は車両）の絶対位置を測定できる環境であればよい。例えば、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ；全地球測位システム）で移動ロボット１の位置を測定することができる場合には、屋外であってもよい。
【００４５】
また、上記実施形態において、補正開始命令部１１９が、移動ロボット１が移動した距離に基づいて方向補正値を算出する処理を開始するタイミングを決定する場合について説明をしたが、本発明はこれに限らず、補正開始命令部１１９は、時刻に基づいて、タイミングを決定してもよい。例えば、補正開始命令部１１９は、前回位置方向補正処理を行った時刻から予め定めた時間が経過したか否かを判定し、予め定めた時間が経過したと判定した場合、位置方向補正処理を開始させる信号を、位置方向補正部１１６の位置補正部１１６３及び方向補正部１１６４に出力してもよい。
また、上記実施形態において、補正開始命令部１１９は、推定位置の軌跡に沿った距離に基づいて、方向補正値を算出する処理を開始するタイミングを決定する場合について説明をした。しかし、本発明はこれに限らず、推定位置間の距離であってもよい。また、補正開始命令部１１９は、Ｘ座標又はＹ座標での距離のいずれかが予め定めた値を超えたと判定したときを、方向補正値を算出する処理を開始するタイミングとしてもよい。
【００４６】
また、上記実施形態において、位置補正部１１６３が単位時間毎の推定位置情報と絶対位置情報とを同定して方向補正値を算出する場合について説明したが、本発明はこれに限らず、単位時間毎より小さい時間毎の情報でもよいし、大きい時間毎の情報であってもよい。例えば、位置補正部１１６３は、通信部１０３が絶対位置測定装置ｃ１から受信した絶対位置測定時刻の時間間隔（上記実施形態では１秒）を算出し、算出した時間間隔毎の推定位置情報と絶対位置情報とを同定して方向補正値を算出してもよい。
【００４７】
また、上記実施形態において、位置補正部１１６３は、推定位置情報又は絶対位置情報が示す数値が極端に変化する特異点（１つ前の時刻の数値との差が、予め定めた閾値より大きくなる時刻の数値）がある場合、特異点の時刻の情報を除いた推定位置情報と絶対位置情報とを同定して方向補正値を算出してもよい。これにより、位置方向制御部１１は、１個の絶対位置情報と推定位置情報とを用いる場合と比較して、精度よく移動装置の向きを測定できる。
また、上記実施形態において、予め定められた方位とグローバル座標の関係に基づいて正面方向を算出しているが、この処理を省略しても良い。
【００４８】
なお、上述した実施形態における移動ロボット１の一部、例えば、エンコーダ部１１１、方向積分部１１６１、位置積分部１１６２、位置補正部１１６３、方向補正部１１６４、駆動制御部１１８、及び補正開始命令部１１９をコンピュータで実現するようにしても良い。その場合、この制御機能を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することによって実現しても良い。なお、ここでいう「コンピュータシステム」とは、移動ロボット１に内蔵されたコンピュータシステムであって、ＯＳや周辺機器等のハードウェアを含むものとする。また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、ＣＤ−ＲＯＭ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピュータ読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムを送信する場合の通信線のように、短時間、動的にプログラムを保持するもの、その場合のサーバやクライアントとなるコンピュータシステム内部の揮発性メモリのように、一定時間プログラムを保持しているものも含んでも良い。また上記プログラムは、前述した機能の一部を実現するためのものであっても良く、さらに前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるものであっても良い。
【００４９】
以上、図面を参照してこの発明の一実施形態について詳しく説明してきたが、具体的な構成は上述のものに限られることはなく、この発明の要旨を逸脱しない範囲内において様々な設計変更等をすることが可能である。
【符号の説明】
【００５０】
１、１１、１２、１３・・・移動ロボット（移動装置）、ｃ１・・・絶対位置測定装置、１０１・・・ジャイロスコープ、１０２・・・駆動部、１０３・・・通信部、１０４・・・方向検出部、１１・・・位置方向制御部、１１１・・・エンコーダ部、１１２・・・相対方向情報記憶部、１１３・・・相対位置情報記憶部、１１４・・・絶対位置情報記憶部、１１５・・・初期方向情報記憶部、１１６・・・位置方向補正部（向き計測装置）、１１７・・・位置方向情報記憶部、１１８・・・駆動制御部、１１９・・・補正開始命令部、１１６１・・・方向積分部、１１６２・・・位置積分部、１１６３・・・位置補正部（方向補正値算出部）、１１６４・・・方向補正部
【技術分野】
【０００１】
本発明は、向き計測装置、向き計測システム、向き計測方法、及び向き計測プログラムに関する。
【背景技術】
【０００２】
近年、人の活動するエリアで人にサービスを行うロボットに関する技術が知られてきている。特許文献１には、パーティクルフィルタを用いて、人の位置および移動速度を推定し、胴体および両腕を３つの円で組み合わせた人形状モデルを用いて、人の身体の方向と腕の動きとを推定する計測装置が記載されている。
【０００３】
一方、移動ロボットの技術分野において、移動ロボットが自らの位置を検出する技術が知られている。例えば、特許文献２には、移動ロボットの運動によって変化する状態量を感知し、移動ロボットと固定位置との距離をセンシングして移動ロボットの絶対位置を計算し、変化する状態量及び計算された絶対位置をカルマンフィルタの入力として、現在移動ロボットの最適位置を推定する移動ロボットが記載されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００９−１６８５７８号公報
【特許文献２】特開２００７−１４９０８８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、引用文献１記載の技術では、人を真上から撮像できない場合や、人が遠い場所に位置して十分な画質の画像を得ることができない場合に、人形状モデルを適用することができず、人の身体の方向を測定できないという欠点があった。また、引用文献１記載の技術では、上半身の方向を測定するものであり、例えば身体を腰部分で捻っている場合には、下半身、つまり、足の進行方向を測定できないという欠点があった。
また、引用文献２記載の技術では、移動ロボット各々が、距離をセンシングする機能や最適位置を推定する機能を備えなければ、位置を正確に測定できず、また、方向を正確に測定できないという欠点があった。
以上のように、従来技術では、移動装置の向きを正確に測定できない場合があるという欠点があった。
【０００６】
本発明は上記の点に鑑みてなされたものであり、移動装置の向きを正確に測定できる向き計測装置、向き計測システム、向き計測方法、及び向き計測プログラムを提供する。
【課題を解決するための手段】
【０００７】
本発明は上記の課題を解決するためになされたものであり、本発明は、自装置の位置の変化量（例えば、実施形態における相対位置情報）と方向の変化量（例えば、実施形態における相対方向情報）とを検出する移動装置（例えば、実施形態における移動ロボット１）に搭載される向き計測装置（例えば、実施形態における位置方向補正部１１６）において、前記位置の変化量を積分して前記移動装置の位置を示す推定位置情報を推定する位置積分部（例えば、実施形態における位置積分部１１６２）と、前記方向の変化量を積分して前記移動装置の方向を示す推定方向情報（例えば、実施形態における推定正面方向情報）を推定する方向積分部（例えば、実施形態における方向積分部１１６１）と、移動装置外の装置（例えば、実施形態における絶対位置測定装置ｃ１）から受信した位置情報であって時々刻々の移動装置の位置を示す絶対位置情報と、前記時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出する方向補正値算出部（例えば、実施形態における位置補正部１１６３）と、前記方向補正値を用いて、前記推定方向情報が示す方向を補正する方向補正部（例えば、実施形態における方向補正部１１６４）と、を備えることを特徴とする向き計測装置である。
【０００８】
また、本発明は、上記の向き計測装置において、前記方向補正値算出部は、複数の時刻の絶対位置情報と推定位置情報とを用いて方向補正値を算出することを特徴とする。
【０００９】
また、本発明は、上記の向き計測装置において、前記移動装置が移動した距離に基づいて、前記方向補正値を算出する処理を開始するタイミングを決定する補正開始命令部（例えば、実施形態における補正開始命令部１１９）を備え、前記方向補正値算出部は、前記補正開始命令部が決定したタイミングで、方向補正値を算出することを特徴とする。
【００１０】
また、本発明は、自装置の位置の変化量と方向の変化量とを検出する移動装置に搭載される向き計測装置と、時々刻々の前期移動装置の位置を示す絶対位置情報を測定して前記向き計測装置に送信する位置測定装置と、を具備する向き計測システム（例えば、実施形態における向き計測システムＳ１）において、前記向き計測装置は、前記位置の変化量を積分して前記移動装置の位置を示す推定位置情報を推定する位置推定部と、前記方向の変化量を積分して前記移動装置の方向を示す推定方向情報を推定する方向推定部と、前記位置測定装置から受信した絶対位置情報と、前記時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出する方向補正値算出部と、前記方向補正値を用いて、前記推定方向情報が示す方向を補正する方向補正部と、を備えることを特徴とする向き計測システムである。
【００１１】
また、本発明は、自装置の位置の変化量と方向の変化量とを検出する移動装置に搭載される向き計測装置における向き計測方法おいて、位置積分部が、前記位置の変化量を積分して前記移動装置の位置を示す推定位置情報を推定する過程と、方向積分部が、前記方向の変化量を積分して前記移動装置の方向を示す推定方向情報を推定する過程と、方向補正値算出部が、移動装置外の装置から受信した位置情報であって時々刻々の移動装置の位置を示す絶対位置情報と、前記時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出する過程と、方向補正部が、前記方向補正値を用いて、前記推定方向情報が示す方向を補正する過程と、を有することを特徴とする向き計測方法である。
【００１２】
また、本発明は、自装置の位置の変化量と方向の変化量とを検出する移動装置に搭載される向き計測装置のコンピュータを、前記位置の変化量を積分して前記移動装置の位置を示す推定位置情報を推定する位置推定手段、前記方向の変化量を積分して前記移動装置の方向を示す推定方向情報を推定する方向推定手段、移動装置外の装置から受信した位置情報であって時々刻々の移動装置の位置を示す絶対位置情報と、前記時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出する方向補正値算出手段、前記方向補正値を用いて、前記推定方向情報が示す方向を補正する方向補正手段、
として機能させる向き計測プログラムである。
【発明の効果】
【００１３】
請求項１に記載した発明によれば、向き計測装置は、位置の変化量を積分して移動装置の位置を示す推定位置情報を推定し、方向の変化量を積分して移動装置の方向を示す推定方向情報を推定する。また、向き計測装置は移動装置外の装置から受信した位置情報であって時々刻々の移動装置の位置を示す絶対位置情報と、時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出し、方向補正値を用いて推定方向情報が示す方向を補正する。これにより、向き計測装置は、移動装置の向きを正確に測定できる。
【００１４】
また、請求項２に記載した発明によれば、向き計測装置は、複数の時刻の絶対位置情報と推定位置情報とを用いて方向補正値を算出する。これにより、向き計測装置は、１個の絶対位置情報と推定位置情報とを用いる場合と比較して、精度よく移動装置の向きを測定できる。
【００１５】
また、請求項３に記載した発明によれば、向き計測装置は、移動装置が移動した距離に基づいて、方向補正値を算出する処理を開始するタイミングを決定する。これにより、向き計測装置は、移動装置が移動した距離に応じて移動局装置の向きを補正することができる。
【図面の簡単な説明】
【００１６】
【図１】本発明の実施形態に係る向き計測システムＳ１を示す概略図である。
【図２】本実施形態に係る移動ロボットの構成を示す概略ブロック図である。
【図３】本実施形態に係る推定位置、絶対位置、及び推定正面方向の関係を示す概略図である。
【図４】本実施形態に係る補正値算出処理の一例を表わす概略図である。
【図５】本実施形態に係る位置方向補正処理を行うタイミングの一例を示す概略図である。
【発明を実施するための形態】
【００１７】
（第１の実施形態）
以下、図面を参照しながら本発明の実施形態について詳しく説明する。
図１は、本発明の実施形態に係る向き計測システムＳ１を示す概略図である。向き計測システムＳ１は、移動ロボット１（移動装置）及び絶対位置測定装置ｃ１を具備する。
図１は、建物の部屋の内部の図である。この図において、移動ロボット１は、部屋の中を移動する。ここで、符号１１、１２、１３を付した移動ロボット１は、それぞれ、動作時刻Ｔが対象時刻ｔの移動ロボット１、対象時刻ｔの１秒前の移動ロボット１、対象時刻ｔのｎ秒前の移動ロボット１を示す。この図において、矢印を付した軌跡は、移動ロボット１が移動した軌跡を示す。
【００１８】
また、図１の部屋の天井には、絶対位置測定装置ｃ１が設置されている。絶対位置測定装置ｃ１は、撮像機能を有し、また、撮像した画像に基づいて移動ロボット１の位置情報（絶対位置情報という）を測定する機能を有する。ここで、絶対位置測定装置ｃ１は、測定した移動ロボット１の絶対位置情報を、重力方向に垂直（床に水平）な座標（図１のＸ軸、Ｙ軸；グローバル座標という）上の値として測定する。
具体的には、絶対位置測定装置ｃ１は、十分に高い位置に設置され、移動ロボット１をほぼ真上から撮像する。絶対位置測定装置ｃ１は、撮像した画像の移動ロボット１部分に対して円近似を行って、その円の中心を移動ロボット１の絶対位置情報とする。絶対位置測定装置ｃ１は、無線通信等を用いて、単位時間（本実施形態では１秒とする）毎に、測定した絶対位置情報と測定した時刻を示す絶対位置測定時刻とからなる情報を移動ロボット１に送信する。
なお、絶対位置測定装置ｃ１のように、建物等に設置され、物体の位置情報等を測定するセンサをインフラセンサという。また、絶対位置測定装置ｃ１は、物体の位置情報を測定できる装置であればよく、例えば、レーザ光を用いて測定する装置であってもよい。また、天井以外の場所に設置されていてもよく、異なる位置に設置された複数の装置であってもよい。
【００１９】
また、図１の部屋の床には、マークＭ１１〜Ｍ１４が付されている。このマークＭ１１〜Ｍ１４各々は、グローバル座標のＹ軸に平行な２本の線であり、各々の線は色が異なる（例えば、Ｘ軸での値が大きい線は赤色の線、値が小さい線は青色の線）。移動ロボット１は、腰部分の側面に備え付けられたカメラで床を撮像し、画像認識によってマークＭ１１〜Ｍ１４のいずれかを検出する。移動ロボット１は、撮像する画像において正面方向（進行方向）と垂直になる線の画像を予め記憶しており、その線の画像と検出した線の画像との角度差を、グローバル座標上のＸ軸に対する角度（θｚ）として算出する。つまり、この角度は、移動ロボット１は、グローバル座標での正面方向（進行方向）を示す角度である。
なお、グローバル座標は、線に垂直で青色の線から赤色の線に向かう方向がＸ軸の方向であり、Ｘ軸を９０度回転させた軸がＹ軸である。また、マークＭ１１〜Ｍ１４は、移動ロボット１が、絶対位置測定装置ｃ１の用いるグローバル座標での方向を検出できるものであればよく、他の形状のマークであってもよい。また、移動ロボット１は、方位磁針を備え、予め定められた方位とグローバル座標の関係に基づいて正面方向を算出してもよい。
【００２０】
＜移動ロボット１の構成について＞
図２は、本実施形態に係る移動ロボット１の構成を示す概略ブロック図である。この図において、移動ロボット１は、ジャイロスコープ１０１、駆動部１０２、通信部１０３、方向検出部１０４、及び位置方向制御部１１を含んで構成される。また、位置方向制御部１１は、エンコーダ部１１１、相対方向情報記憶部１１２、相対位置情報記憶部１１３、絶対位置情報記憶部１１４、初期方向情報記憶部１１５、位置方向補正部１１６、位置方向情報記憶部１１７、駆動制御部１１８、及び、補正開始命令部１１９を含んで構成される。ここで、位置方向補正部１１６（向き計測装置）は、方向積分部１１６１、位置積分部１１６２、位置補正部１１６３、及び、方向補正部１１６４を含んで構成される。
【００２１】
ジャイロスコープ１０１は、角度や角速度を検出する計測器である。ジャイロスコープ１０１は、単位時間毎に検出した角度及び角速度をエンコーダ部１１１に出力する。
駆動部１０２は、駆動制御部１１８の制御に従い、移動ロボット１の移動手段（二足歩行型ロボットの場合は両足）を動かすことで、移動ロボット１を移動させる。駆動部１０２は、予め定められた時間間隔（駆動検出時間という。例えば、１／２００秒）毎に、動作時刻Ｔと動作結果情報とをエンコーダ部１１１に出力する。
ここで、動作結果情報とは、動作時刻Ｔの駆動検出時間での進行方向と進行距離とであり、例えば、移動ロボット１が二足歩行型ロボットの場合は、動作時刻Ｔに足をどの方向にどの距離踏み出したかを示す情報である。なお、動作時刻Ｔの進行方向（足を踏み出した方向）は、正面方向に対する相対的な方向である。また、例えば、移動ロボット１が車輪型移動ロボットの場合には、動作結果情報は、各車輪についての回転方向及び回転数を示す情報である。
【００２２】
通信部１０３は、絶対位置測定装置ｃ１と無線通信を行う。通信部１０３は、絶対位置情報と絶対位置測定時刻とからなる情報を、絶対位置測定装置ｃ１から受信し、絶対位置情報記憶部１１４に記憶する。
方向検出部１０４は、図１のマークＭ１１〜Ｍ１４に基づいて、絶対位置測定装置ｃ１の用いるグローバル座標で、自装置の正面方向を算出する。方向検出部１０４は、算出した正面方向を示す初期方向情報と、初期方向情報を算出した時刻を示す初期方向算出時刻と、からなる情報を初期方向情報記憶部１１５に記憶する。
【００２３】
エンコーダ部１１１は、駆動部１０２から入力された動作結果情報に基づいて、移動ロボット１について、前後の動作時刻Ｔでの方向の差を示す相対方向情報、及び、前後の動作時刻Ｔでの位置の差を示す相対位置情報を算出する。すなわち、エンコーダ部１１１は、移動ロボット１の位置の変化量と方向の変化量とを検出する。
なお、エンコーダ部１１１は、ジャイロスコープ１０１から入力された角度及び角速度を用いて、相対方向情報の精度を上げてもよい。エンコーダ部１１１は、動作時刻Ｔと算出した相対方向情報とからなる情報を相対方向情報記憶部１１２に記憶し、動作時刻と算出した相対位置情報とからなる情報を相対位置情報記憶部１１３に記憶する。
【００２４】
方向積分部１１６１は、方向補正部１１６４から基準方向情報及び基準方向測定時刻を入力される。この基準方向情報とは、グローバル座標での方向を示す情報であり、基準方向測定時刻での移動ロボット１の正面方向を示す情報である。
方向積分部１１６１は、相対方向情報記憶部１１２から動作時刻Ｔと相対方向情報とからなる情報を読み出す。方向積分部１１６１は、基準方向情報を初期値として、基準方向測定時刻から動作時刻Ｔまでの相対方向情報を加算することで、移動ロボット１のグローバル座標における動作時刻Ｔでの正面方向を推定する。方向積分部１１６１は、駆動検出時間の動作時刻Ｔ毎に、推定した正面方向（推定正面方向という）を示す推定正面方向情報（推定方向情報）と動作時刻Ｔとからなる情報を方向補正部１１６４に出力する。
【００２５】
位置積分部１１６２は、位置補正部１１６３から基準位置情報及び基準位置測定時刻を入力される。この基準位置情報とは、グローバル座標での位置情報であり、基準位置測定時刻での移動ロボット１の位置を示す情報である。
位置積分部１１６２は、相対位置情報記憶部１１３から動作時刻と相対位置情報とからなる情報を読み出す。位置積分部１１６２は、基準位置情報を初期値として、基準位置測定時刻から動作時刻Ｔまでの相対位置情報を加算することで、移動ロボット１のグローバル座標における動作時刻Ｔでの位置を推定する。方向積分部１１６１は、駆動検出時間の動作時刻Ｔ毎に、推定した位置（推定位置という）を示す推定位置情報と動作時刻Ｔとからなる情報を位置補正部１１６３及び補正開始命令部１１９に出力する。
【００２６】
位置補正部１１６３（方向補正値算出部）は、位置積分部１１６２から入力された情報のうち、単位時間毎の推定位置情報であって動作時刻Ｔが対象時刻ｔから時刻ｔ−ｎの期間の推定位置情報を抽出する。また、位置補正部１１６３は、絶対位置情報記憶部１１４が記憶する情報のうち、絶対位置測定時刻が対象時刻ｔから時刻ｔ−ｎの期間の絶対位置情報を選択して読み出す。
位置補正部１１６３は、抽出した推定位置情報と読み出した絶対位置情報とを同定して、対象時刻ｔから時刻ｔ−ｎの期間について、推定位置と絶対位置とのずれを計算する。位置補正部１１６３は、計算したずれに対して、推定位置のずれを補正する補正値（ΔＸ、ΔＹ）と方向のずれを補正する補正値（Δθｚ；方向補正値）とを算出する。なお、補正値算出処理の詳細については、後述する位置方向補正処理と併せて説明をする。位置補正部１１６３は、算出した方向補正値を方向補正部１１６４に出力する。
【００２７】
また、位置補正部１１６３は、絶対位置測定時刻が対象時刻ｔの絶対位置情報及び対象時刻ｔを、それぞれ、基準位置情報及び基準位置測定時刻として、位置積分部１１６２に出力する。ここで、位置補正部１１６３は、位置積分部１１６２が初めて処理を開始する時点では、基準位置情報及び基準位置測定時刻として、それぞれ、絶対位置情報記憶部１１４が記憶する絶対位置情報及び絶対位置測定時刻を位置積分部１１６２に出力する。また、位置補正部１１６３は、絶対位置情報記憶部１１４から読み出した情報を方向補正部１１６４に出力する。
【００２８】
方向補正部１１６４は、方向積分部１１６１から入力された情報のうち、動作時刻Ｔが対象時刻ｔから時刻ｔ−ｎの期間の推定正面方向情報を抽出する。方向補正部１１６４は、抽出した推定正面方向情報が示す推定正面方向に、位置補正部１１６３から入力された方向補正値を加算する。すなわち、方向補正部１１６４は、方向補正値を用いて、推定正面方向情報が示す方向を補正する。以下、加算後の方向を補正方向といい、補正方向を示す情報を補正方向情報という。
方向補正部１１６４は、補正方向情報、動作時刻Ｔ（対象時刻ｔから時刻ｔ−ｎ）、及び、位置補正部１１６３から入力された動作時刻Ｔでの絶対位置情報を、動作時刻Ｔでの位置方向情報として、位置方向情報記憶部１１７に記憶する。つまり、位置方向情報記憶部１１７は、対象時刻ｔから時刻ｔ−ｎの期間の動作時刻Ｔについて、それぞれ、位置と方向が補正された位置方向情報を記憶する。
【００２９】
また、方向補正部１１６４は、動作時刻Ｔが対象時刻ｔの補正方向情報及び対象時刻ｔを、それぞれ、基準方向情報及び基準方向測定時刻として、方向積分部１１６１に出力する。ここで、方向補正部１１６４は、方向積分部１１６１が初めて処理を開始する時点では、基準方向情報及び基準方向測定時刻として、それぞれ、初期方向情報記憶部１１５が記憶する初期方向情報及び初期方向算出時刻を方向積分部１１６１に出力する。
なお、位置方向補正部１１６、つまり、方向積分部１１６１、位置積分部１１６２、位置補正部１１６３、及び方向補正部１１６４が行う上記の処理を、位置方向補正処理という。
【００３０】
駆動制御部１１８は、位置方向情報記憶部１１７が記憶する位置方向情報に基づいて、駆動部の制御を行う。
補正開始命令部１１９は、相対位置情報記憶部１１３から読み出した情報に基づいて、移動ロボット１が、前回位置方向補正処理を行った時点から予め定めた距離を移動したか否かを判定する。補正開始命令部１１９は、予め定めた距離を移動したと判定した場合、位置方向補正処理を開始させる信号を、位置方向補正部１１６の位置補正部１１６３及び方向補正部１１６４に出力する。
すなわち、補正開始命令部１１９は、移動ロボット１が移動した距離に基づいて、方向補正値を算出する処理を開始するタイミングを決定する。ここで、この予め定めた距離は、２〜３ｍ程度が好ましい。この距離が大きい場合には、移動ロボット１の内部オドメトリ軌跡（図３参照）の形状と移動ロボット１のインフラセンサ軌跡の形状との差が大きくなってしまい、この距離が小さい場合には、方向の誤差が大きくなってしまうからである。
【００３１】
＜位置方向補正処理について＞
以下、位置補正部１１６３が行う位置方向補正処理について説明をする。
図３は、本実施形態に係る推定位置、絶対位置、及び推定正面方向の関係を示す概略図である。この図において、座標はグローバル座標であり、横軸がＸ軸、縦軸がＹ軸である。
図３において、符号Ｌ１１を付した軌跡Ｌ１１は、対象時刻ｔから時刻ｔ−ｎの期間の推定位置であって単位時間毎の推定位置（白抜き矢印の始点）を示す軌跡（移動ロボット１の内部オドメトリ軌跡）である。また、各推定位置での白抜きの矢印は、各推定位置での推定正面方向を示す。一方、符号Ｌ１２を付した軌跡Ｌ１２は、対象時刻ｔから時刻ｔ−ｎの期間の絶対位置（黒丸）を示す軌跡（移動ロボット１のインフラセンサ軌跡；図１に示した軌跡）である。また、この図において、点線の矢印で対応付けられた推定位置及び絶対位置は、同じ動作時刻Ｔのものである。
【００３２】
例えば、図３において、符号Ｐ１１１、Ｐ１２１を付した位置Ｐ１１１、Ｐ１２１は、それぞれ、動作時刻Ｔ＝ｔ−ｎでの推定位置、絶対位置を示す。また、符号Ｄ１２１を付した矢印Ｄ１２１は、動作時刻Ｔ＝ｔ−ｎでの推定正面方向を示す。また、例えば、位置Ｐ１１２、Ｐ１２２、及び、符号Ｄ１２２を付した矢印Ｄ１２１は、それぞれ、動作時刻Ｔ＝ｔでの推定位置、絶対位置、及び推定正面方向を示す。この図が示すように、推定位置は、絶対位置からずれる場合があるが、軌跡Ｌ１１及びＬ１２はほとんど同じ形の軌跡となる。
【００３３】
図４は、本実施形態に係る補正値算出処理の一例を表わす概略図である。この図において、座標はグローバル座標であり、横軸がＸ軸、縦軸がＹ軸である。
符号Ｆ４１を付した図Ｆ４１は、図３と同じ図であり、補正値算出処理を行う前の推定位置、絶対位置、及び推定正面方向の関係を示す図である。また、図Ｆ４１は、位置補正部１１６３がアフィン変換量（ΔＸ、ΔＹ、Δθｚ）を算出していることを示す。
【００３４】
具体的には、位置補正部１１６３は、対象時刻ｔから時刻ｔ−ｎの期間について絶対位置の平均値と相対位置の平均値とを算出し、これらの平均値の差（ΔＸ、ΔＹ；位置補正値という）を算出する。位置補正部１１６３は、算出した位置補正値（ΔＸ、ΔＹ）を、対象時刻ｔから時刻ｔ−ｎの期間の相対位置に加算する。
位置補正部１１６３は、対象時刻ｔから時刻ｔ−ｎの期間の絶対位置について、最小二乗法を用いて近似した一次方程式の傾きを計算する。また、位置補正部１１６３は、位置補正値を加算した相対位置について、最小二乗法を用いて近似した一次方程式の傾きを計算する。位置補正部１１６３は、計算した傾きが示す角度（Ｘ軸からの角度を変数θｚとする）の差を、方向補正値Δθｚとして算出する。ただし、本発明はこれに限らず、位置補正部１１６３は、矢印で対応付けられた推定位置及び絶対位置同士の距離の総和が最小となるようなΔＸ、ΔＹ、Δθｚを求めてもよい。
【００３５】
符号４２を付した図Ｆ４２は、絶対位置、位置補正値を加算した推定位置、及び補正方向を示す図である。
例えば、図Ｆ４２おいて、符号Ｄ２２１を付した白抜きの矢印Ｄ２２１は、対象時刻ｔでの補正方向を示す。この矢印Ｄ２２１は、図Ｆ４１の矢印Ｄ１２１が示す方向に、方向補正値Δθｚを加算した方向を示す。
【００３６】
図５は、本実施形態に係る位置方向補正処理を行うタイミングの一例を示す概略図である。この図において、座標はグローバル座標であり、横軸がＸ軸、縦軸がＹ軸である。この図において、位置Ｐ２１１、Ｐ２１２、Ｐ２１３、Ｐ２１４、Ｐ２１５を含む軌跡（破線と実線の両方）は、推定位置を示す軌跡である。また、位置Ｐ２２０、Ｐ２２１、Ｐ２２２、Ｐ２２３、Ｐ２２４、Ｐ２２５を含む軌跡は、絶対位置を示す軌跡である。例えば、位置Ｐ２１１、Ｐ２１２、Ｐ２１１、Ｐ２２２が、それぞれ、図３の位置Ｐ１１１、Ｐ１１２、Ｐ１２１、Ｐ１２２である。
図５は、Ｐ２２２、Ｐ２２３、Ｐ２２５の時点で、位置方向補正処理が行われたことを示す。また、この図において、符号ＩＤ１、ＩＤ２を付した白抜きの矢印ＩＤ１、ＩＤ２は、方向検出部１０４が検出した初期方向情報が示す方向を表わす。
【００３７】
以下、図５及び図２を用いて、位置方向制御部１１の動作の一例について説明をする。
位置補正部１１６３は、位置Ｐ２２０を示す絶対位置情報及び絶対位置測定時刻を、基準位置情報及び基準位置測定時刻として、位置積分部１１６２に出力する。また、方向補正部１１６４は、矢印ＩＤ１が表わす方向を示す初期方向情報及び初期方向算出時刻を、基準方向情報及び基準方向測定時刻として、方向積分部１１６１に出力する。
【００３８】
方向積分部１１６１及び位置積分部１１６２は、それぞれ、入力された初期値に基づいて、推定正面方向及び推定位置を算出する。例えば、位置積分部１１６２は、推定位置として位置Ｐ２１１、２１２を算出する。また、通信部１０３は、絶対位置情報を受信する。この絶対位置情報は、例えば、位置Ｐ２２１、Ｐ２２２である。
補正開始命令部１１９は、位置Ｐ２２０から位置Ｐ２１２までの推定位置の軌跡に沿った距離が、予め定めた距離を超えたか否かを判定する。図５は、推定位置Ｐ２１２の時点（対象時刻ｔ＝ｔ１とする）で予め定めた距離を超えたと判定した場合の図である。補正開始命令部１１９は、対象時刻ｔ１で、位置方向補正処理を開始させる信号を、位置補正部１１６３及び方向補正部１１６４に出力する。
【００３９】
位置補正部１１６３は、時刻ｔ１−ｎから対象時刻ｔ１まで（図５では、位置Ｐ２１１、Ｐ２２１の時点）の期間の推定位置情報及び絶対位置情報に基づいて、方向補正値を算出する。方向補正部１１６４は、推定方向に位置補正部１１６３が算出した方向補正値を加算し、補正方向を算出する。位置補正部１１６３は対象時刻ｔ１での絶対位置Ｐ２２２を示す基準位置情報を位置積分部１１６２に出力する。また、方向補正部１１６４は対象時刻ｔ１での補正方向を示す基準方向情報を方向積分部１１６１に出力する。
【００４０】
方向積分部１１６１は入力された基準方向情報に基づいて推定正面方向を算出し、位置積分部１１６２は入力された基準位置情報に基づいて推定位置を算出する。同様に、補正開始命令部１１９は、推定位置Ｐ２１３の時点で、位置方向補正処理を開始させる信号を出力する。位置方向補正部１１６は、この信号に基づいて、位置方向補正処理を行う。
【００４１】
また、図５は、位置Ｐ２２４の時点（推定Ｐ２１４の時点；時刻ｔ２とする）で、方向検出部１０４が、矢印ＩＤ２が表わす方向を検出したことを示す。この場合、方向補正部１１６４は、この方向を示す初期方向情報及び初期方向算出時刻を、基準方向情報及び基準方向測定時刻として、方向積分部１１６１に出力する。
また、図５は、推定位置Ｐ２１３の時点（対象時刻ｔ＝ｔ３とする）で、位置Ｐ２２３から位置Ｐ２１５までの推定位置の軌跡に沿った距離が、予め定めた距離を超えたと判定した場合の図である。ここで、図５の一例では、時刻ｔ２は、時刻ｔ３−ｎより遅い時刻である。この場合、位置補正部１１６３は、時刻ｔ２から対象時刻ｔ３までの期間の推定位置情報及び絶対位置情報に基づいて、方向補正値を算出する。つまり、位置補正部１１６３は、時刻ｔ−ｎと、方向検出部１０４が方向を検出した時刻と、のいずれか遅い方の時刻から、対象時刻ｔまでの推定位置情報及び絶対位置情報に基づいて、方向補正値を算出する。
【００４２】
このように、本実施形態によれば、移動ロボット１の位置方向制御部１１は、位置の変化量を積分して移動ロボットの位置を示す推定位置情報を推定し、方向の変化量を積分して移動ロボットの正面方向を示す推定正面方向情報を推定する。また、位置方向制御部１１は、絶対位置測定装置ｃ１から受信した位置情報であって時々刻々の移動装置の位置を示す絶対位置情報と、時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出し、方向補正値を用いて推定正面方向情報が示す正面方向を補正する。これにより、位置方向制御部１１は、移動装置の向きを正確に測定できる。
また、本実施形態では、移動ロボット１以外の装置（絶対位置測定装置ｃ１）が移動ロボット１の位置のみを測定できればよいので、絶対位置測定装置ｃ１が方向を検出できない場合でも、位置方向制御部１１は、移動装置の向きを正確に測定できる。また、位置方向制御部１１は、距離をセンシングする機能や最適位置を推定する機能を備えなくても、移動装置の向きを正確に測定できる。
さらに、位置方向制御部１１は、正面方向、つまり、進行方向を正確に測定できるので、進行方向と他の部位（例えば、上半身の正面方向）の方向の相対方向を検出することにより、他の部位の方向も正確に測定することができる。
【００４３】
また、本実施形態によれば、位置方向制御部１１は、複数の時刻の絶対位置情報と推定位置情報とを用いて方向補正値を算出する。これにより、位置方向制御部１１は、１個の絶対位置情報と推定位置情報とを用いる場合と比較して、精度よく移動装置の向きを測定できる。
また、本実施形態によれば、位置方向制御部１１は、移動ロボット１が移動した距離に基づいて、方向補正値を算出する処理を開始するタイミングを決定する。これにより、位置方向制御部１１は、移動ロボット１が移動した距離に応じて移動局ロボット１の向きを補正することができる。
【００４４】
なお、上記実施形態において、位置方向制御部１１を移動ロボット１が備える場合について説明をしたが、本発明はこれに限らず、位置方向制御部１１は、車両（自動二輪車等も含む）や車椅子等の自動で移動するものに備えられてもよい。
また、上記実施形態において、移動ロボット１が部屋の中を移動する場合について説明したが、本発明はこれに限らず、移動ロボット１（又は車両）の絶対位置を測定できる環境であればよい。例えば、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ；全地球測位システム）で移動ロボット１の位置を測定することができる場合には、屋外であってもよい。
【００４５】
また、上記実施形態において、補正開始命令部１１９が、移動ロボット１が移動した距離に基づいて方向補正値を算出する処理を開始するタイミングを決定する場合について説明をしたが、本発明はこれに限らず、補正開始命令部１１９は、時刻に基づいて、タイミングを決定してもよい。例えば、補正開始命令部１１９は、前回位置方向補正処理を行った時刻から予め定めた時間が経過したか否かを判定し、予め定めた時間が経過したと判定した場合、位置方向補正処理を開始させる信号を、位置方向補正部１１６の位置補正部１１６３及び方向補正部１１６４に出力してもよい。
また、上記実施形態において、補正開始命令部１１９は、推定位置の軌跡に沿った距離に基づいて、方向補正値を算出する処理を開始するタイミングを決定する場合について説明をした。しかし、本発明はこれに限らず、推定位置間の距離であってもよい。また、補正開始命令部１１９は、Ｘ座標又はＹ座標での距離のいずれかが予め定めた値を超えたと判定したときを、方向補正値を算出する処理を開始するタイミングとしてもよい。
【００４６】
また、上記実施形態において、位置補正部１１６３が単位時間毎の推定位置情報と絶対位置情報とを同定して方向補正値を算出する場合について説明したが、本発明はこれに限らず、単位時間毎より小さい時間毎の情報でもよいし、大きい時間毎の情報であってもよい。例えば、位置補正部１１６３は、通信部１０３が絶対位置測定装置ｃ１から受信した絶対位置測定時刻の時間間隔（上記実施形態では１秒）を算出し、算出した時間間隔毎の推定位置情報と絶対位置情報とを同定して方向補正値を算出してもよい。
【００４７】
また、上記実施形態において、位置補正部１１６３は、推定位置情報又は絶対位置情報が示す数値が極端に変化する特異点（１つ前の時刻の数値との差が、予め定めた閾値より大きくなる時刻の数値）がある場合、特異点の時刻の情報を除いた推定位置情報と絶対位置情報とを同定して方向補正値を算出してもよい。これにより、位置方向制御部１１は、１個の絶対位置情報と推定位置情報とを用いる場合と比較して、精度よく移動装置の向きを測定できる。
また、上記実施形態において、予め定められた方位とグローバル座標の関係に基づいて正面方向を算出しているが、この処理を省略しても良い。
【００４８】
なお、上述した実施形態における移動ロボット１の一部、例えば、エンコーダ部１１１、方向積分部１１６１、位置積分部１１６２、位置補正部１１６３、方向補正部１１６４、駆動制御部１１８、及び補正開始命令部１１９をコンピュータで実現するようにしても良い。その場合、この制御機能を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することによって実現しても良い。なお、ここでいう「コンピュータシステム」とは、移動ロボット１に内蔵されたコンピュータシステムであって、ＯＳや周辺機器等のハードウェアを含むものとする。また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、ＣＤ−ＲＯＭ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピュータ読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムを送信する場合の通信線のように、短時間、動的にプログラムを保持するもの、その場合のサーバやクライアントとなるコンピュータシステム内部の揮発性メモリのように、一定時間プログラムを保持しているものも含んでも良い。また上記プログラムは、前述した機能の一部を実現するためのものであっても良く、さらに前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるものであっても良い。
【００４９】
以上、図面を参照してこの発明の一実施形態について詳しく説明してきたが、具体的な構成は上述のものに限られることはなく、この発明の要旨を逸脱しない範囲内において様々な設計変更等をすることが可能である。
【符号の説明】
【００５０】
１、１１、１２、１３・・・移動ロボット（移動装置）、ｃ１・・・絶対位置測定装置、１０１・・・ジャイロスコープ、１０２・・・駆動部、１０３・・・通信部、１０４・・・方向検出部、１１・・・位置方向制御部、１１１・・・エンコーダ部、１１２・・・相対方向情報記憶部、１１３・・・相対位置情報記憶部、１１４・・・絶対位置情報記憶部、１１５・・・初期方向情報記憶部、１１６・・・位置方向補正部（向き計測装置）、１１７・・・位置方向情報記憶部、１１８・・・駆動制御部、１１９・・・補正開始命令部、１１６１・・・方向積分部、１１６２・・・位置積分部、１１６３・・・位置補正部（方向補正値算出部）、１１６４・・・方向補正部
【特許請求の範囲】
【請求項１】
自装置の位置の変化量と方向の変化量とを検出する移動装置に搭載される向き計測装置において、
前記位置の変化量を積分して前記移動装置の位置を示す推定位置情報を推定する位置積分部と、
前記方向の変化量を積分して前記移動装置の方向を示す推定方向情報を推定する方向積分部と、
移動装置外の装置から受信した位置情報であって時々刻々の移動装置の位置を示す絶対位置情報と、前記時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出する方向補正値算出部と、
前記方向補正値を用いて、前記推定方向情報が示す方向を補正する方向補正部と、
を備えることを特徴とする向き計測装置。
【請求項２】
前記方向補正値算出部は、複数の時刻の絶対位置情報と推定位置情報とを用いて方向補正値を算出することを特徴とする請求項１に記載の向き計測装置。
【請求項３】
前記移動装置が移動した距離に基づいて、前記方向補正値を算出する処理を開始するタイミングを決定する補正開始命令部を備え、
前記方向補正値算出部は、前記補正開始命令部が決定したタイミングで、方向補正値を算出することを特徴とする請求項２に記載の向き計測装置。
【請求項４】
自装置の位置の変化量と方向の変化量とを検出する移動装置に搭載される向き計測装置と、時々刻々の前期移動装置の位置を示す絶対位置情報を測定して前記向き計測装置に送信する位置測定装置と、を具備する向き計測システムにおいて、
前記向き計測装置は、
前記位置の変化量を積分して前記移動装置の位置を示す推定位置情報を推定する位置推定部と、
前記方向の変化量を積分して前記移動装置の方向を示す推定方向情報を推定する方向推定部と、
前記位置測定装置から受信した絶対位置情報と、前記時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出する方向補正値算出部と、
前記方向補正値を用いて、前記推定方向情報が示す方向を補正する方向補正部と、
を備えることを特徴とする向き計測システム。
【請求項５】
自装置の位置の変化量と方向の変化量とを検出する移動装置に搭載される向き計測装置における向き計測方法おいて、
位置積分部が、前記位置の変化量を積分して前記移動装置の位置を示す推定位置情報を推定する過程と、
方向積分部が、前記方向の変化量を積分して前記移動装置の方向を示す推定方向情報を推定する過程と、
方向補正値算出部が、移動装置外の装置から受信した位置情報であって時々刻々の移動装置の位置を示す絶対位置情報と、前記時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出する過程と、
方向補正部が、前記方向補正値を用いて、前記推定方向情報が示す方向を補正する過程と、
を有することを特徴とする向き計測方法。
【請求項６】
自装置の位置の変化量と方向の変化量とを検出する移動装置に搭載される向き計測装置のコンピュータを、
前記位置の変化量を積分して前記移動装置の位置を示す推定位置情報を推定する位置推定手段、
前記方向の変化量を積分して前記移動装置の方向を示す推定方向情報を推定する方向推定手段、
移動装置外の装置から受信した位置情報であって時々刻々の移動装置の位置を示す絶対位置情報と、前記時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出する方向補正値算出手段、
前記方向補正値を用いて、前記推定方向情報が示す方向を補正する方向補正手段、
として機能させる向き計測プログラム。
【請求項１】
自装置の位置の変化量と方向の変化量とを検出する移動装置に搭載される向き計測装置において、
前記位置の変化量を積分して前記移動装置の位置を示す推定位置情報を推定する位置積分部と、
前記方向の変化量を積分して前記移動装置の方向を示す推定方向情報を推定する方向積分部と、
移動装置外の装置から受信した位置情報であって時々刻々の移動装置の位置を示す絶対位置情報と、前記時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出する方向補正値算出部と、
前記方向補正値を用いて、前記推定方向情報が示す方向を補正する方向補正部と、
を備えることを特徴とする向き計測装置。
【請求項２】
前記方向補正値算出部は、複数の時刻の絶対位置情報と推定位置情報とを用いて方向補正値を算出することを特徴とする請求項１に記載の向き計測装置。
【請求項３】
前記移動装置が移動した距離に基づいて、前記方向補正値を算出する処理を開始するタイミングを決定する補正開始命令部を備え、
前記方向補正値算出部は、前記補正開始命令部が決定したタイミングで、方向補正値を算出することを特徴とする請求項２に記載の向き計測装置。
【請求項４】
自装置の位置の変化量と方向の変化量とを検出する移動装置に搭載される向き計測装置と、時々刻々の前期移動装置の位置を示す絶対位置情報を測定して前記向き計測装置に送信する位置測定装置と、を具備する向き計測システムにおいて、
前記向き計測装置は、
前記位置の変化量を積分して前記移動装置の位置を示す推定位置情報を推定する位置推定部と、
前記方向の変化量を積分して前記移動装置の方向を示す推定方向情報を推定する方向推定部と、
前記位置測定装置から受信した絶対位置情報と、前記時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出する方向補正値算出部と、
前記方向補正値を用いて、前記推定方向情報が示す方向を補正する方向補正部と、
を備えることを特徴とする向き計測システム。
【請求項５】
自装置の位置の変化量と方向の変化量とを検出する移動装置に搭載される向き計測装置における向き計測方法おいて、
位置積分部が、前記位置の変化量を積分して前記移動装置の位置を示す推定位置情報を推定する過程と、
方向積分部が、前記方向の変化量を積分して前記移動装置の方向を示す推定方向情報を推定する過程と、
方向補正値算出部が、移動装置外の装置から受信した位置情報であって時々刻々の移動装置の位置を示す絶対位置情報と、前記時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出する過程と、
方向補正部が、前記方向補正値を用いて、前記推定方向情報が示す方向を補正する過程と、
を有することを特徴とする向き計測方法。
【請求項６】
自装置の位置の変化量と方向の変化量とを検出する移動装置に搭載される向き計測装置のコンピュータを、
前記位置の変化量を積分して前記移動装置の位置を示す推定位置情報を推定する位置推定手段、
前記方向の変化量を積分して前記移動装置の方向を示す推定方向情報を推定する方向推定手段、
移動装置外の装置から受信した位置情報であって時々刻々の移動装置の位置を示す絶対位置情報と、前記時々刻々の推定位置情報と、を同定して、方向のずれを補正する方向補正値を算出する方向補正値算出手段、
前記方向補正値を用いて、前記推定方向情報が示す方向を補正する方向補正手段、
として機能させる向き計測プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図２】
【図３】
【図４】
【図５】
【公開番号】特開２０１１−１３４１９１（Ｐ２０１１−１３４１９１Ａ）
【公開日】平成２３年７月７日（２０１１．７．７）
【国際特許分類】
【出願番号】特願２００９−２９４３４７（Ｐ２００９−２９４３４７）
【出願日】平成２１年１２月２５日（２００９．１２．２５）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【Ｆターム（参考）】
【公開日】平成２３年７月７日（２０１１．７．７）
【国際特許分類】
【出願日】平成２１年１２月２５日（２００９．１２．２５）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【Ｆターム（参考）】
[ Back to top ]