JP2006155349A - 追尾方法、追尾装置、追尾システム及び追尾プログラム - Google Patents

Abstract

【課題】 追尾対象物と追尾装置との間が遮られても良好な追尾を実現する。
【解決手段】 姿勢センサ装置１００には姿勢センサ１０１が設けられ、この姿勢情報が姿勢情報処理装置１０２に供給されて絶対位置情報の検出が行われる。また、時刻タイマ１０５及び個人認証ＩＤコード記憶部１０６からの時刻情報及びＩＤ情報が姿勢情報処理装置１０２に供給される。そしてこれらの絶対位置情報、時刻情報及びＩＤ情報が無線送受信機１０７を通じて追尾装置２００に送信される。さらに追尾装置２００には姿勢センサ２０１が設けられると共に、制御装置２０２が設けられる。そして無線送受信機２０５で姿勢センサ装置１００からの各情報が受信される。また、追尾装置２００には車輪を駆動するためのモータ２０６、２０７が設けられ、制御装置２０２からの制御信号がモータドライバ２０８、２０９に供給されてモータ２０６、２０７の回転が制御される。
【選択図】 図１

Description

本発明は、特定の追尾対象物の後を離間して追尾する際に使用して好適な追尾方法、追尾装置、追尾システム及び追尾プログラムに関する。詳しくは、位置検出センサによる位置検出情報を時刻情報と共に記憶して、追尾対象物が過去に通過した経路を再現して追尾を行うようにしたものである。

従来から、例えば先行車両との車間距離を測定して、自車両が追従するように走行制御を行う車両追従走行制御装置は提案されている（例えば、特許文献１参照。）。

また、車車間通信方式及び装置において、相互の位置関係を認識して指向性を有する無線通信を円滑に行うようにしているものもある（例えば、特許文献２参照。）。

すなわち、一般的に先行車両に追従する装置や、通信を用いて車両の相互の位置関係を認識する装置はすでに知られているものである。
特開２０００−３８０４８号公報 特開２００２−７４５７７号公報

特許文献１に示された装置では、レーダーや赤外線センサを用いて先行車両を直接確認しながら追従を行っている。従って、例えば先行車両が壁のある角を曲がったような場合には、先行車両の確認ができなくなり、追従が不可能になってしまう恐れがある。また、類似の車両が複数存在した場合には、その中から特定の先行車両を識別して追尾することはできないものである。

これに対して、特許文献２に示された方式及び装置では、絶対位置を検出する手段を設け、検出された絶対位置を相互に無線通信することによって相互の位置関係を認識している。しかしながらこの場合においても、常に現時点の絶対位置の情報を交換しているものであって、これらの通信が遮断された場合には、追尾は不可能になってしまう。

この出願はこのような点に鑑みて成されたものであって、解決しようとする問題点は、従来の装置では、常に現時点での相互の位置を確認しているために、追尾対象物と追尾装置との間が遮られると、追尾を継続することができなくなってしまう恐れがあったというものである。

このため本発明においては、追尾対象物の走行経路を記憶し、追尾対象物が過去に通過した経路を再現して追尾を行うようにしたものであって、これによれば、記憶された過去の経路に基づいて追尾を行うので、追尾対象物と追尾装置との間が遮られても追尾を継続することができ、良好な追尾を実現することができる。

請求項１に記載の本発明の追尾方法によれば、追尾対象物に第１の位置検出センサと第１の情報通信手段を設け、第１の位置検出センサによる位置検出情報を、時計手段からの時刻情報と共に記憶する記憶手段を有すると共に、追尾装置にも第２の位置検出センサと第２の情報通信手段を設け、追尾装置は記憶手段に記憶された情報に基づき追尾対象物が過去に通過した経路を再現して追尾を行うことによって、追尾対象物と追尾装置との間が遮られても追尾を継続することができ、良好な追尾を実現することができる。

また、請求項２に記載の本発明の追尾方法によれば、請求項１において、第１及び第２の位置検出センサは、ジャイロと加速度センサと方位センサを少なくとも有し、ジャイロと加速度センサと方位センサの出力から位置検出情報を算出することによって、簡単な構成で良好な位置検出情報を得ることができ、良好な追尾を実現することができる。

請求項３に記載の本発明の追尾方法によれば、請求項１において、追尾装置は記憶手段に記憶された所定時間前の追尾対象物の位置を目標位置として追尾を行うことによって、追尾対象物が角を曲がったような場合にも、良好な追尾を実現することができる。

請求項４に記載の本発明の追尾方法によれば、請求項１において、追尾装置に障害物検知センサを設け、障害物検知センサが障害物を検知したときには追尾を一時停止させ、障害物検知センサが障害物を検知しなくなった後に追尾を再開することによって、経路が一時的に遮られたような場合にも、良好に追尾を行うことができる。

請求項５に記載の本発明の追尾方法によれば、請求項４において、再開された追尾は、記憶手段に記憶された追尾対象物の位置の経路に従って行うことによって、良好に追尾を継続することができる。

請求項６に記載の本発明の追尾方法によれば、請求項４において、一時停止の期間が所定時間以上になったときは回避行動を行うことによって、良好に追尾を再開することができる。

請求項７に記載の本発明の追尾方法によれば、請求項６において、回避行動は、記憶手段に記憶された情報に基づく追尾対象物が過去に通過した経路を、追尾装置の進行方向の左右に所定距離ずつシフトして行うことによって、良好に障害物の回避を行うことができる。

請求項８に記載の本発明の追尾方法によれば、請求項１において、追尾対象物の情報通信手段には固有の識別コードを設け、追跡装置は複数の追尾対象物の中の一つを識別コードで識別して追尾を行うことによって、複数の追尾対象物がある場合にも、良好な追尾を行うことができる。

請求項９に記載の本発明の追尾方法によれば、請求項１において、記憶手段を追尾対象物と追尾装置の双方に設け、追尾装置側の記憶手段に欠落が生じたときには追尾対象物側の記憶手段に情報の再送信を要求することによって、通信状態が悪化したような状況においても、良好な追尾を行うことができる。

請求項１０に記載の本発明の追尾方法によれば、請求項１において、現時点の追尾対象物の位置検出情報を判別し、現時点の位置検出情報と追尾装置の位置検出情報との距離が所定の距離以内となったときに衝突回避行動を行うことによって、追尾対象物が同じ経路を逆進してきた場合に、追尾対象物と追尾装置との衝突を回避することができる。

請求項１１に記載の本発明の追尾装置によれば、第１の位置検出センサと第１の情報通信手段の設けられた追尾対象物に対して、位置検出センサによる位置検出情報を逐次記憶する記憶手段を有すると共に、第２の位置検出センサと第２の情報通信手段が設けられ、記憶手段に記憶された情報に基づき追尾対象物が過去に通過した経路を再現して追尾を行うことによって、追尾対象物と追尾装置との間が遮られても追尾を継続することができ、良好な追尾を実現することができる。

また、請求項１２に記載の本発明の追尾装置によれば、請求項１１において、第１及び第２の位置検出センサは、ジャイロと加速度センサと方位センサを少なくとも有し、ジャイロと加速度センサと方位センサの出力から位置検出情報を算出することによって、簡単な構成で良好な位置検出情報を得ることができ、良好な追尾を実現することができる。

請求項１３に記載の本発明の追尾装置によれば、請求項１１において、記憶手段に記憶された所定時間前の追尾対象物の位置を目標位置として追尾を行うことによって、追尾対象物が角を曲がったような場合にも、良好な追尾を実現することができる。

請求項１４に記載の本発明の追尾装置によれば、請求項１１において、障害物検知センサを設け、障害物検知センサが障害物を検知したときには追尾を一時停止させ、障害物検知センサが障害物を検知しなくなった後に追尾を再開することによって、経路が一時的に遮られたような場合にも、良好に追尾を行うことができる。

請求項１５に記載の本発明の追尾装置によれば、請求項１４において、再開された追尾は、記憶手段に記憶された追尾対象物の位置の経路に従って行うことによって、良好に追尾を継続することができる。

請求項１６に記載の本発明の追尾装置によれば、請求項１４において、一時停止の期間が所定時間以上になったときは回避行動を行うことによって、良好に追尾を再開することができる。

請求項１７に記載の本発明の追尾装置によれば、請求項１６において、回避行動は、記憶手段に記憶された情報に基づく追尾対象物が過去に通過した経路を進行方向の左右に所定距離ずつシフトして行うことによって、良好に障害物の回避を行うことができる。

請求項１８に記載の本発明の追尾装置によれば、請求項１１において、追尾対象物の情報通信手段には固有の識別コードを設け、複数の追尾対象物の中の一つを識別コードで識別して追尾を行うことによって、複数の追尾対象物がある場合にも、良好な追尾を行うことができる。

請求項１９に記載の本発明の追尾装置によれば、請求項１１において、記憶手段を追尾対象物と自己側の双方に設け、自己側の記憶手段に欠落が生じたときには追尾対象物側の記憶手段に情報の再送信を要求することによって、通信状態が悪化したような状況においても、良好な追尾を行うことができる。

請求項２０に記載の本発明の追尾装置によれば、請求項１１において、現時点の追尾対象物の位置検出情報を判別し、現時点の位置検出情報と自己の位置検出情報との距離が所定の距離以内となったときに衝突回避行動を行うことによって、追尾対象物が同じ経路を逆進してきた場合に、追尾対象物と追尾装置との衝突を回避することができる。

請求項２１に記載の本発明の追尾システムによれば、追尾対象物に第１の位置検出センサと第１の情報通信手段を設け、第１の位置検出センサによる位置検出情報を、時計手段からの時刻情報と共に記憶する記憶手段を有すると共に、追尾装置にも第２の位置検出センサと第２の情報通信手段を設け、追尾装置は記憶手段に記憶された情報に基づき追尾対象物が過去に通過した経路を再現して追尾を行う制御手段を有することによって、追尾対象物と追尾装置との間が遮られても追尾を継続することができ、良好な追尾を実現することができる。

また、請求項２２に記載の本発明の追尾システムによれば、請求項２１において、第１及び第２の位置検出センサは、ジャイロと加速度センサと方位センサを少なくとも有し、ジャイロと加速度センサと方位センサの出力から位置検出情報を算出する算出手段を有することによって、簡単な構成で良好な位置検出情報を得ることができ、良好な追尾を実現することができる。

請求項２３に記載の本発明の追尾システムによれば、請求項２１において、追尾装置は記憶手段に記憶された所定時間前の追尾対象物の位置を目標位置として追尾を行う制御手段を有することによって、追尾対象物が角を曲がったような場合にも、良好な追尾を実現することができる。

請求項２４に記載の本発明の追尾システムによれば、請求項２１において、追尾装置に障害物検知センサを設け、障害物検知センサが障害物を検知したときには追尾を一時停止させ、障害物検知センサが障害物を検知しなくなった後に追尾を再開する制御手段を有することによって、経路が一時的に遮られたような場合にも、良好に追尾を行うことができる。

請求項２５に記載の本発明の追尾システムによれば、請求項２４において、再開された追尾は、記憶手段に記憶された追尾対象物の位置の経路に従って行う制御手段を有することによって、良好に追尾を継続することができる。

請求項２６に記載の本発明の追尾システムによれば、請求項２４において、一時停止の期間が所定時間以上になったときは回避行動を行う制御手段を有することによって、良好に追尾を再開することができる。

請求項２７に記載の本発明の追尾システムによれば、請求項２６において、回避行動は、記憶手段に記憶された情報に基づく追尾対象物が過去に通過した経路を、追尾装置の進行方向の左右に所定距離ずつシフトして行う制御手段を有することによって、良好に障害物の回避を行うことができる。

請求項２８に記載の本発明の追尾システムによれば、請求項２１において、追尾対象物の情報通信手段には固有の識別コードを設け、追跡装置は複数の追尾対象物の中の一つを識別コードで識別して追尾を行う制御手段を有することによって、複数の追尾対象物がある場合にも、良好な追尾を行うことができる。

請求項２９に記載の本発明の追尾システムによれば、請求項２１において、記憶手段を追尾対象物と追尾装置の双方に設け、追尾装置側の記憶手段に欠落が生じたときには追尾対象物側の記憶手段に情報の再送信を要求する制御手段を有することによって、通信状態が悪化したような状況においても、良好な追尾を行うことができる。

請求項３０に記載の本発明の追尾システムによれば、請求項２１において、現時点の追尾対象物の位置検出情報を判別し、現時点の位置検出情報と追尾装置の位置検出情報との距離が所定の距離以内となったときに衝突回避行動を行う制御手段を有することによって、追尾対象物が同じ経路を逆進してきた場合に、追尾対象物と追尾装置との衝突を回避することができる。

請求項３１に記載の本発明の追尾プログラムによれば、第１の位置検出センサと第１の情報通信手段の設けられた追尾対象物に対して、位置検出センサによる位置検出情報を逐次記憶する記憶手段を有すると共に、第２の位置検出センサと第２の情報通信手段が設けられ、記憶手段に記憶された情報に基づき追尾対象物が過去に通過した経路を再現して追尾を行う制御手段を有することによって、追尾対象物と追尾装置との間が遮られても追尾を継続することができ、良好な追尾を実現することができる。

また、請求項３２に記載の本発明の追尾プログラムによれば、請求項３１において、第１及び第２の位置検出センサは、ジャイロと加速度センサと方位センサを少なくとも有し、ジャイロと加速度センサと方位センサの出力から位置検出情報を算出する算出手段を有することによって、簡単な構成で良好な位置検出情報を得ることができ、良好な追尾を実現することができる。

請求項３３に記載の本発明の追尾プログラムによれば、請求項３１において、記憶手段に記憶された所定時間前の追尾対象物の位置を目標位置として追尾を行う制御手段を有することによって、追尾対象物が角を曲がったような場合にも、良好な追尾を実現することができる。

請求項３４に記載の本発明の追尾プログラムによれば、請求項３１において、障害物検知センサを設け、障害物検知センサが障害物を検知したときには追尾を一時停止させ、障害物検知センサが障害物を検知しなくなった後に追尾を再開する制御手段を有することによって、経路が一時的に遮られたような場合にも、良好に追尾を行うことができる。

請求項３５に記載の本発明の追尾プログラムによれば、請求項３４において、再開された追尾は、記憶手段に記憶された追尾対象物の位置の経路に従って行う制御手段を有することによって、良好に追尾を継続することができる。

請求項３６に記載の本発明の追尾プログラムによれば、請求項３４において、一時停止の期間が所定時間以上になったときは回避行動を行う制御手段を有することによって、良好に追尾を再開することができる。

請求項３７に記載の本発明の追尾プログラムによれば、請求項３６において、回避行動は、記憶手段に記憶された情報に基づく追尾対象物が過去に通過した経路を進行方向の左右に所定距離ずつシフトして行う制御手段を有することによって、良好に障害物の回避を行うことができる。

請求項３８に記載の本発明の追尾プログラムによれば、請求項３１において、追尾対象物の情報通信手段には固有の識別コードを設け、複数の追尾対象物の中の一つを識別コードで識別して追尾を行う制御手段を有することによって、複数の追尾対象物がある場合にも、良好な追尾を行うことができる。

請求項３９に記載の本発明の追尾プログラムによれば、請求項３１において、記記憶手段を追尾対象物と追尾装置の双方に設け、追尾装置側の記憶手段に欠落が生じたときには追尾対象物側の記憶手段に情報の再送信を要求する制御手段を有することによって、通信状態が悪化したような状況においても、良好な追尾を行うことができる。

請求項４０に記載の本発明の追尾プログラムによれば、請求項３１において、現時点の追尾対象物の位置検出情報を判別し、現時点の位置検出情報と自己の位置検出情報との距離が所定の距離以内となったときに衝突回避行動を行う制御手段を有することによって、追尾対象物が同じ経路を逆進してきた場合に、追尾対象物と追尾装置との衝突を回避することができる。

これによって、従来の装置では、常に現時点での相互の位置を確認しているために、追尾対象物と追尾装置との間が遮られると、追尾を継続することができなくなってしまう恐れがあったものを、本発明によればこれらの問題点を容易に解消することができるものである。

以下、図面を参照して本発明を説明するに、図１は本発明による追尾方法、追尾装置、追尾システム及び追尾プログラムを適用した追尾対象物、及び追尾装置の一実施形態の構成を示すブロック図である。

図１において、図面の上部には追尾対象物に設けられる姿勢センサ装置１００の構成を示す。この姿勢センサ装置１００には、加速度センサ、ジャイロセンサ、方位センサからなる姿勢センサ１０１と姿勢情報処理装置１０２が設けられ、バッテリ１０３からの電源が、姿勢情報処理装置１０２に供給されると共に、ＤＣ／ＤＣ電源回路１０４を通じて姿勢センサ１０１に供給されている。

これにより、姿勢センサ１０１からの姿勢情報が姿勢情報処理装置１０２に供給されて絶対位置情報の検出が行われる。また、時刻タイマ１０５及び個人認証ＩＤコード記憶部１０６が設けられ、これらからの時刻情報及びＩＤ情報が姿勢情報処理装置１０２に供給される。そしてこれらの絶対位置情報、時刻情報及びＩＤ情報が無線送受信機１０７を通じて、図面の下部に示す追尾装置２００に送信される。

さらに追尾装置２００には、上述の姿勢センサ装置１００と同等の加速度センサ、ジャイロセンサ、方位センサからなる姿勢センサ２０１が設けられると共に、制御装置２０２が設けられ、バッテリ２０３からの電源が、制御装置２０２に供給されると共に、ＤＣ／ＤＣ電源回路２０４を通じて姿勢センサ２０１に供給される。そして制御装置２０２には無線送受信機２０５が設けられて、姿勢センサ装置１００からの各情報が受信される。

また、追尾装置２００には、車輪（図示せず）を駆動するためのモータ２０６、２０７が設けられ、制御装置２０２からの制御信号がモータドライバ２０８、２０９に供給されてモータ２０６、２０７の回転が制御される。さらに接近センサ２１０、２１１が設けられ、追尾装置２００に対する障害物等の接近が検出される。また制御装置２０２には、各種データを記憶するための記憶装置２１２が設けられている。

そして上述の姿勢センサ装置１００及び追尾装置２００において、それぞれの姿勢情報処理装置１０２及び制御装置２０２では、姿勢センサ１０１及び２０１からのデータに従って、例えば図２に示すような構成を用いて絶対位置座標の算出が行われる。なお、図２には、追尾対象物に設けられる姿勢センサ装置１００における姿勢センサ１０１及び姿勢情報処理装置１０２等の具体的な構成を示している。

すなわち、図２において、時刻タイマ１０５としての世界時計１２１で発生される日・時・分・秒の時刻データ（ＤＤ：ＨＨ：ＭＭ：ＳＳ）と、姿勢センサ１０１としての加速度センサ１２２で発生される加速度データ（Ａｓｘ，Ａｓｙ，Ａｓｚ）、ジャイロセンサ１２３で発生されるジャイロデータ（ωｓｒ，ωｓｐ，ωｓｙ）、方位センサ１２４で発生される方位データ（θｒ，θｐ，θｙ）が、それぞれサンプルホールド回路１３１、１３２、１３３、１３４に供給されて、例えば５０ｍＳＥＣ周期でデータが取り出される。

なお、上述の各センサ信号の定義は、移動対象物（Ａ）と追尾車両（Ｂ）のそれぞれにおいて、図３のＡとＢに示すようになっている。ここで、加速度データはＸ、Ｙ、Ｚ軸の加速度信号であり、１Ｖ／（９．６ｍ／ｓｅｃ^２）で表され、単位は〔Ｖ／（ｍ／ｓｅｃ^２）〕である。また、ジャイロデータはＸ，Ｙ，Ｚ軸の角速度信号であり、１Ｖ／（２πｒａｄ／ｓｅｃ）で表され、単位は〔Ｖ／（ｒａｄ／ｓｅｃ）〕である。さらに方位データはＸ、Ｙ、Ｚ軸の方位信号であり、１Ｖ／（πｒａｄ）で表され、単位は〔Ｖ／ｒａｄ〕である。

さらに加速度データ、ジャイロデータ、方位データに関しては、それぞれＡＤ変換回路１４２、１４３、１４４に供給されて、データごとにそれぞれ１２ビット×３軸のデジタルデータに変換される。そして変換されたこれらのデータが、サンプルホールド回路１３１からの時刻データと共に、マイクロコンピュータ１５１に供給される。また、個人認証ＩＤコード記憶部１０６からのＩＤ情報がマイクロコンピュータ１５１に供給される。

従って、時刻データは、各時刻の変換値を添え字のｔで表して、時刻データ（ＤＤｔ：ＨＨｔ：ＭＭｔ：ＳＳｔ）と示し、アスキーコードで３２バイトのデータである。また、３軸加速度データは同様に各時刻ｔの３軸加速度ＡＤ変換データ（Ａｓｘｔ，Ａｓｙｔ，Ａｓｚｔ）と示し、バイナリーコードで６バイトのデータである。さらに３軸角速度データも同様に各時刻ｔの３軸角速度ＡＤ変換データ（ωｓｘｔ，ωｓｙｔ，ωｓｚｔ）と示し、バイナリーコードで６バイトのデータである。また３次元方位データも同様に各時刻ｔの３次元方位角度ＡＤ変換データ（θｒｔ，θｐｔ，θｙｔ）と示し、バイナリーコードで６バイトのデータである。

これにより、マイクロコンピュータ１５１では、加速度データ、ジャイロデータ、方位データに時刻データを加えたものを、まずハードディスク装置１５２に書き込み、このハードディスク装置１５２に書き込まれた各データから、３次元絶対位置座標データ（Ｘｔ，Ｙｔ，Ｚｔ）及び３次元速度データ（Ｖｘｔ，Ｖｙｔ，Ｖｚｔ）の算出が行われる。そして上述の時刻データと共に、これらの３次元絶対位置座標データ及び３次元速度データが、ランダムアクセスメモリ１５３に書き込まれる。

ここで、追尾対象物に設けられる姿勢センサ１０１で検出される姿勢センサ信号から、図４に示すグローバル座標の軌道（Ｘｔ，Ｙｔ）は、下式１〜７に示す演算により求められる。ここでは説明を簡単にするため、二次元平面の運動の座標により説明する。また、追尾装置２００においても同様にして、姿勢サンサ２０１で検出される姿勢センサ信号から追尾装置２００のグローバル座標の現在位置が求められる。

すなわち、センサが時刻ｔ［ｓｅｃ］時間のとき、Ｚ軸の回転角であるヨー軸角度が値θｙｔ［ｒａｄ］であるときのセンサＸｓ、Ｙｓ軸の加速度信号が値Ａｓｘｔ、Ａｓｙｔであるとき、絶対座標軸のＸ、Ｙ軸方向の加速度Ａｘｔ、Ａｙｔは、下式１、２となる。ここでは、他の軸の回転角であるピッチ軸角θｐｔ及びロール軸角θｒｔはゼロとしたときの関係式を示す。

Ａｘｔ＝Ａｓｘｔ×ｃｏｓ（θｙｔ）＋Ａｓｙｔ×ｓｉｎ（−θｙｔ）
［ｍ／ｓｅｃ^２］ 式１
Ａｙｔ＝Ａｓｘｔ×ｓｉｎ（−θｙｔ）＋Ａｓｙｔ×ｃｏｓ（θｙｔ）
［ｍ／ｓｅｃ^２］ 式２

また、回転角θｙｔ［ｒａｄ］はジャイロセンサ信号ωｙｔから下式３となる。
θｙｔ＝∫ωｙｔｄｔ ［ｒａｄ］ 式３
ここで値θｙｔの初期値θｙ０は、方位センサ値θｐｘであることから定められる。こうしてグローバル座標軸の加速度成分が確定される。

このときセンサ位置（Ｘｔ，Ｙｔ）の時間変化である速度を値（Ｖｘｔ，Ｖｙｔ）［ｍ／ｓｅｃ］とすると下式４、５となる。
Ｖｘｔ＝∫Ａｘｔｄｔ ［ｍ／ｓｅｃ］ 式４
Ｖｙｔ＝∫Ａｙｔｄｔ ［ｍ／ｓｅｃ］ 式５

同様に絶対座標軸上のセンサ軌跡位置（Ｘｔ，Ｙｔ）は下式６、７となる。
Ｘｔ＝∫Ｖｘｔｄｔ ［ｍ］ 式６
Ｙｔ＝∫Ｖｙｔｄｔ ［ｍ］ 式７

このようして、追尾対象物に設けられる姿勢センサの動きを、図４に示すように時系列の位置情報（Ｘｔ，Ｙｔ）及び速度情報（Ｖｘｔ，Ｖｙｔ）として、その履歴を装置の記憶装置に保持することができる。なお図５には、グローバル座標とローカル座標のセンサ加速度信号の関係を示し、図６には、グローバル座標のセンサ速度とセンサ加速度の関係を、それぞれＸ軸及びＹ軸について示す。

さらに、ここまでは２次元の平面での動きについて説明したが、実際には３次元の動きが伴うので３次元での姿勢センサからグローバル座標の位置を求めることが必要となる。以下には３次元の姿勢センサ信号からグローバル座標を算出する方法について説明する。すなわちこの場合に、センサ位置(Ｘｔ，Ｙｔ，Ｚｔ)と、センサローカル座標（Ｘｓ，Ｙｓ，Ｚｓ）及びグローバル座標(Ｘ，Ｙ，Ｚ)の関係は、移動対象物（Ａ）と追尾車両（Ｂ）のそれぞれにおいて、図７のＡとＢに示すようになる。

ここで、例えば人が歩行する場合に、身体の任意の位置に取り付けた姿勢センサは人の歩行揺れによるセンサ軸方向が時々刻々と変化するため、センサのローカル座標信号からグローバル座標の信号に変換する必要が生じる。以下にその変換する演算式を示す。こうして変換することにより人のグローバル座標での軌道変化を求めることができる。同様に追尾する車両に実装された姿勢センサ信号から車両のグローバル座標位置をリアルタイムに算出される。

３次元の姿勢センサから検出される３軸の加速度と３軸の角速度と３軸の方位センサからグローバル座標の位置に変換する式を説明する。すなわち、姿勢センサの方位センサのピッチ、ロール、ヨー軸方位信号（θｐ，θｒ，θｙ）とＸｓ、Ｙｓ、Ｚｓ軸の加速度信号（Ａｓｘ，Ａｓｙ，Ａｓｚ）であるとき、グローバル座標のＸ、Ｙ、Ｚ軸の加速度ＡＸ、ＡＹは下式８〜１７により求められる。

ＡＸｔ＝Ａｓｘ×ＣＯＳ（θy）×ＣＯＳ（θｐ）
＋Ａｓｙ×ＳＩＮ（−θｙ）×ＣＯＳ（θｒ）
＋Ａｓｚ×ＳＩＮ（−θｐ）×ＣＯＳ（θｒ） 式８
ＡＹｔ＝Ａｓｘ×ＳＩＮ（−θｙ）×ＣＯＳ（θｐ）
＋Ａｓｙ×ＣＯＳ（θｙ）×ＣＯＳ（θｒ）
＋Ａｓｚ×ＳＩＮ（−θｒ）×ＣＯＳ（θｐ） 式９

上記で求めた（ＡＸ，ＡＹ）から下式１０、１１の積分演算をして、速度信号（ＶＸ，ＶＹ）を算出する。
ＶＸｔ＝∫ＡＸｔｄｔ 式１０
ＶＹｔ＝∫ＡＹｔｄｔ 式１１

さらに、式１０、１１はデジタル信号として実際には下式１２、１３により算出する。ここで現在サンプリングを（ｋ）とし、（ｋ−１）は１過去のサンプリングとしたときのＡＣＸ、ＡＣＹ、ＶＸ、ＶＹ値をＡＣＸ（ｋ）、ＡＣＹ（ｋ）、ＶＸ（ｋ）、ＶＸ（ｋ−１）、ＶＹ（ｋ）、Ｖ（ｋ−１）とすると、値ＫＩは１＞ＫＩ＞０の少数値として、サンプリング時間は設定することができるようにする。
ＶＸ（ｋ）＝ＫＩ×ＡＸ（ｋ）＋ＶＸ（ｋ−１） 式１２
ＶＹ（ｋ）＝ＫＩ×ＡＹ（ｋ）＋ＶＹ（ｋ−１） 式１３

上記で求めた（ＶＸ，ＶＹ）から下式１４、１５の積分演算をして位置信号（Ｘ，Ｖ）を算出する。
Ｘｔ＝∫ＶＸｔｄｔ 式１４
Ｙｔ＝∫ＶＹｔｄｔ 式１５

さらに、式１４、１５はデジタル信号として実際には下式１６、１７により算出する。ここで上記と同様にｋはサンプリング数を示す。値ＫＩ_０は、１＞ＫＩ_０＞０の少数値として、サンプリング時間は設定することができるようにする。
Ｘ（ｋ）＝ＫＩ_０×ＶＸ（ｋ）＋Ｘ（ｋ−１） 式１６
Ｙ（ｋ）＝ＫＩ_０×ＶＹ（ｋ）＋Ｙ（ｋ−１） 式１７

以上のようにして、追尾対象物に設けられた姿勢センサ１０１で求められた各データから、３次元絶対位置座標データ及び３次元速度データが求められる。そして図２においては、これらデータに、上述の時刻データとＩＤ情報を加えたデータが無線送受信機１０７に供給されて、追尾装置２００の無線送受信機２０５に送信される。

なお、上述の送信では、例えば時刻データがアスキーコードで３２バイト、３次元絶対位置座標データがアスキーコードで４８バイト、３次元速度データがアスキーコードで３６バイト、ＩＤ情報がバイナリーコードで１６バイトとされ、これらの計１３２バイトの姿勢データに、ヘッダバイト及びチェックサムバイトを含めたデータを１フレームとしたパケット通信が行われる。

また上述の送信は、例えば５０ｍＳＥＣ周期で行われる。そして、通信障害などによりデータに欠落が生じた場合には、その旨の情報が追尾装置２００の無線送受信機２０５から無線送受信機１０７に送信されることによって、データの再送信が行われる仕組みとなっている。

すなわち、図８において、姿勢センサ装置１００の無線送受信機１０７からは、例えば５０ｍＳＥＣ周期で各フレームのデータが送信される。このデータが追尾装置２００の無線送受信機２０５で受信されると、このデータに対してチェックサムバイトなどを用いた判別が行われ、ＯＫのときはデータが記憶される。

これに対して判別結果がＮＧの場合には、無線送受信機２０５から無線送受信機１０７にデータの再要求が行われる。ここで再要求は、３２バイトの時刻データと１６バイトのＩＤ情報、それに８バイトの要求コマンドによって形成される。そして無線送受信機１０７でこの要求コマンドが受信されると、時刻データで識別される該当フレームのデータが再送信される。さらに無線送受信機２０５でデータが受信されると、受信データが判別され、ＯＫのときはデータが記憶され、ＮＧのときデータにＮＧタグが記憶される。

以上のようにして、追尾対象物に設けられる姿勢センサ装置１００から追尾装置２００に、追尾対象物の３次元絶対位置座標データ及び３次元速度データが時刻データと共に送信される。それと共に、送信されたデータに欠落が生じた場合には、自動的にデータの再送信が行われるものである。

なお、上述の構成によれば、再要求のコマンドに時刻データが付加されているので、データの再送信は欠落が生じた直後だけでなく、必要なときに行うことができる。すなわち図９に示すように、記憶されたデータの中にＮＧタグがある場合には、必要なときにこのＮＧタグの前後のデータから類推される時刻データを基に再送信を要求して、欠如データの補充を行うことも可能である。

そして、追尾装置２００では、受信された信号に対して揺らぎ信号除去の処理を行う。すなわち、センサ（加速度、角速度、方位）信号Ｓｎｔは、ノイズ信号と人の揺らぎの信号が重複された信号となる。ここでノイズは比較的高い周波数成分の信号と定義し、低い周波数信号を揺らぎと定義し、図１０のような関係となる。なお、本システムでは人の動きを扱うものとして１Ｈｚ以下の信号を扱うこととして抽出していく。従って人の軌道は１Ｈｚ以上の信号を揺らぎとして扱い、１Ｈｚ以下の信号を軌道位置として抽出する。

ここで、センサ信号で時刻ｔ_０でのノイズ＆揺らぎ除去方法として、あらかじめ記憶されたセンサ信号に対し時刻ｔ_０の前後の信号からその平均値により求める。すなわち、図１１に示すように真値をＳ０ｔ_０とするとき、ノイズを含むセンサ信号Ｓｎｔ_０、同様に時刻ｔ_１の信号をＳｎｔ_１であるとき、下式１８、１９の評価関数Ｅが最小となる条件からＳ０ｔ_０を求める。

こうして算出された信号Ｓ０ｔは高周波成分が除去されるが、人の動きで低い周波数成分の動き（揺らぎ成分）が含まれている。揺らぎと人の中心軌跡の動きは近い周波数領域になるため正確に判別はできないのが現状である。そこで、人の揺らぎ周波数を約１Ｈｚ（これは人が平均速度で歩行したときの身体の揺れを１Ｈｚとした）と仮定して、１Ｈｚ以上の変化を揺らぎとして扱い、１Ｈｚ未満の周波数を軌跡信号とした軌跡を求めるアルゴリズムを考える。

原理は、サンプリング周期を１秒でサンプルした信号Ｓ０ｋ_１を、図１２に示すように二次のローパスフィルタにより経路を再計算して、１Ｈｚ以上の変化を減衰した経路とする。なお、このときのサンプル周期は１秒以外でもよく、歩行周期と同一でもよい。このようして得た長い周期のサンプルデータから、小さい周期のサンプルデータにより連続的な軌道を得るために、下式２０、２１の演算により軌道を補正して、車両に滑らかな動きを実現させる。

Ｓ１（ｋ）＝（１−Ａｆ０）×Ｓ０（ｋ）＋Ａｆ０×Ｓ１（ｋ−１）
但し、Ａｆ０は０から１までの小数値。 式２０
Ｓ２（ｋ）＝（１−Ａｆ１）×Ｓ１（ｋ）＋Ａｆ１×Ｓ２（ｋ−１）
但し、Ａｆ１は０から１までの小数値。 式２１
ここでサンプリングｋは、ｋ１のサンプリング時間の１秒に比べ短い時間とする。

さらに、具体的な揺らぎ補正演算は次のようになる。すなわち平地路面で人が歩行するとき、人の動きには前後左右に揺れが含むため図１３のように時間軸に対して軌道に揺れが含まれる。これを車両の軌道とするには人の揺れの成分を除去した軌道を生成する必要がある。ここでは、下式２２、２３の簡単なフィルタ演算をすることにより、図１３のように揺れの成分を除去することができる。

さらに、人の軌道の正確さは補正軌道により選択する。ここで正確な追従をする場合は下式２２の一次補正軌道を用い、滑らかな動きや高速な動きの場合は下式２３の二次補正軌道を選択して、車両の目標軌道とする（図１３のＸｐ１（ｋ））。なお、ここでは平地路面としてＺ軸の高さの挙動については略すが、同様な式によりＺ軸の高さ方向の揺らぎも除去することができる。従って人の軌跡のＸ軸方向の位置軌道をＸｐ（ｋ）とすると、補正した軌道Ｘｐ０（ｋ）、Ｘｐ１（ｋ）は下式２２、２３となる。

一次補正軌道
Ｘｐ０（ｋ）＝（１−Ａ０)×Ｘｐ（ｋ）＋Ａ０×Ｘｐ０（ｋ−１），Ａ０＝０．５
式２２
二次補正軌道
Ｘｐ１（ｋ）＝（１−Ａ１)×Ｘｐ０（ｋ）＋Ａ１×Ｘｐ１（ｋ−１） ，Ａ１=０．８５
式２３
ここでサンプリング周期は０．２Ｓｅｃとして演算し、その計算例を図１３に示す。

同様に、人の軌跡のＹ軸方向の位置軌道をＹｐ（ｋ）とすると、補正した軌道Ｙｐ０（ｋ）、Ｙｐ１（ｋ）は下式２４、２５となる。そこで上述Ｘ軸方向と同様に、Ｙ軸方向の位置軌道も求めることにより平面の軌道が定まる。

一次補正軌道
Ｙｐ０（ｋ）＝（１−Ａ０)×Ｙｐ（ｋ）＋Ａ０×Ｙｐ０（ｋ−１） 式２４
二次補正軌道
Ｙｐ１（ｋ）＝（１−Ａ１)×Ｙｐ０（ｋ）＋Ａ１×Ｙｐ１（ｋ−１） 式２５
こうして得られた軌跡位置（Ｘｐ１，Ｙｐ１）を目標値とした車両の走行軌跡とすることにより車両には揺らぎの無い軌道により走行することができるようになる。

さらに以下の説明では、人の軌跡の位置は、位置（Ｘｐ（ｔ），Ｙｐ（ｔ））、または（Ｘｐ１（ｔ），Ｙｐ１（ｔ））として表すが、目標軌道はどちらでも用いても良いものとする。従って、以下の説明では、人の軌跡位置を（Ｘｐ（ｔ），Ｙｐ（ｔ））と記述するが、これを（Ｘｐ０（ｔ），Ｙｐ０（ｔ））または（Ｘｐ１（ｔ），Ｙｐ１（ｔ））として、軌道位置としてもよい。

以上の手段により、追尾対象物の軌跡位置（Ｘｐ１，Ｙｐ１）が求められる。そして、求められた軌跡位置（Ｘｐ１，Ｙｐ１）を目標軌道とした追尾軌道制御が、以下に述べるように行われる。すなわち、上記のように算出された補正経路を目標値として、走行車に取り付けた現在位置との差をゼロとするようトラッキング制御を行う。追尾する経路は指定されたＩＤコードと一致した経路のみを選択して、下記の原理により追尾走行する。

ここでトラッキング方法には、下記の２通りがある。
（１）リアルタイムに一定の距離間隔で人の軌道に追従し走行する。
（２）既に記憶された人の軌道を再現し走行する。
これら２通りの方法の選択は、装置の走行モードスイッチによりモードを決める動作手順とする。

また、追尾する車両は１輪以上の駆動車輪と旋回駆動する軸を備えた車両構造、または２輪で各車輪が独立して駆動する構造、または二歩歩行ロボットの構造であってよい。追尾する車両は制御装置により車輪を制御して路面を自在の方向に進むことができる構造、例として図１４にあるような構造の車両となる。図１５の車両構造の例では左右のタイヤを駆動するモータの回転角、回転速度を制御することにより旋回、前進、後進をすることができる。

さらに、図１６にあるように人（または追尾される対象物）の位置をＸｐ（ｔ）、Ｙｐ（ｔ）、Ｚｐ（ｔ）、追尾する車両の位置をＸｍ（ｔ）、Ｙｍ（ｔ）、Ｚｍ（ｔ）とグローバル座標の位置と定義する。被追尾側、追尾側の方向は実装された姿勢センサのローカル座標に一致するものとする。

そこで、上記の（１）リアルタイムに一定の距離間隔で人の軌道に追従し走行する制御においては、無線で受信した人の軌跡情報をメモリに蓄えて、現在の時点の位置から指定した距離で人の軌跡位置を目標値として常に走行車の現在位置を制御する。

すなわち、制御装置に記憶された過去の人（または追尾される対象物）の軌跡位置（Ｘｐ（ｔｘ），Ｙｐ（ｔｘ），Ｚｐ（ｔｘ））と、現在の人（または追尾される対象物）の位置（Ｘｐ（ｔ），Ｙｐ（ｔ），Ｚｐ（ｔ））から、現在位置（Ｘｐ（ｔ），Ｙｐ（ｔ），Ｚｐ（ｔ））と過去の軌跡位置（Ｘｐ（ｔｘ），Ｙｐ（ｔｘ），Ｚｐ（Ｔｘ））の距離Ｌｄ〔ｍ〕が下式２６により算出される。
Ｌｄ＝√［｛Ｘｐ（ｔｘ）−Ｘｐ（ｔ）｝²＋｛Ｙｐ（ｔｘ）−Ｙｐ（ｔ）｝²
＋｛Ｚｐ（ｔｘ）−Ｚｐ（ｔ）｝²］ 式２６

上式２６で算出された距離Ｌｄが指定された距離と一致した軌跡位置（Ｘｍ（ｔｘ），Ｙｍ（ｔｘ），Ｚｍ（ｔｘ））を車両の目標軌道とする。こうして得られた目標軌道を目標値とした車両の制御により、図１７、図１８に示すような追尾走行を実現することができる。さらに図１９にその制御装置、式２７〜３５に追尾する走行車両の制御式を示す。

すなわち、リアルタイムに人に追尾する走行制御方法については、まず上図１９の追尾車両制御システムで、走行指令ＥＸｒｅｆ、ＥＹｒｅｆは下式２７、２８、２９により与えられる。
ＥＸｒｅｆ＝Ｘｐ（ｔｘ）−Ｘｍ（ｔ） 式２７
ＥＹｒｅｆ＝Ｙｐ（ｔｘ）−Ｙｍ（ｔ） 式２８
Θｍｙ＝ｔａｎ^-1（ＥＸｒｅｆ／ＥＹｒｅｆ） 式２９

すると、ＥＹｒｅｆ＝０ であるとき車両をＸｍ方向に下式３０、３１により、モータ指令Ｍｒｅｆ１、Ｍｒｅｆ２を与えてフィードバック制御すると、Ｘｍ（ｔ）が変化し、誤差ＥＸｒｅｆはゼロに収束していく。
Ｍｒｅｆ１＝ＥＸｒｅｆ×Ｋｓ０：Ｋｓ０は正の定数。 式３０
Ｍｒｅｆ２＝−Ｍｒｅｆ１ 式３１

ここで、ＥＹｒｅｆ≠０であるとき、図２５に示すように、車両を角度Θｍｙに旋回する下式３２、３３により、モータ指令Ｍｒｅｆ１、Ｍｒｅｆ２を与えてフィードバック制御すると、車両は旋回変化し誤差Θｍｙはゼロに収束していく。
Ｍｒｅｆ１＝Θｍｙ×Ｋｓ１：Ｋｓ１は正の定数。 式３２
Ｍｒｅｆ２＝Ｍｒｅｆ１ 式３３

さらに、旋回してΘｍｙ＝０となった時点で、車両Ｘｍ方向に下式３４、３５により、モータ指令Ｍｒｅｆ１、Ｍｒｅｆ２を与えてフィードバック制御すると、Ｘｍ（ｔ）が変化し誤差ＥＸｒｅｆ、ＥＹｒｅｆはゼロに収束していく。
Ｍｒｅｆ１＝√（ＥＸｒｅｆ²＋ＥＹｒｅｆ²）×Ｋｓ０ 式３４
Ｍｒｅｆ２＝−Ｍｒｅｆ１ 式３５

こうして、車両は、人（または追尾される対象物）の軌跡を一定距離で追従することが出来るようになる。なお、上式では追尾誤差を小さくするため比例ゲインＫｓ０を用いているが、これはＰＩＤ制御により追尾誤差を小さくしてもよい。

なお、座標において、Ｚ軸（高さ）方向の変化が定めた一定値以上の大きさが生じた場合、車両は変化せずにその状態を維持し、Ｚ軸の変化が定めた値以下になると再び上記演算を繰返し実行する。このとき接近センサにより測定された障害物との距離Ｌｘが指定された距離よりも車両が接近するとき走行指令ＥＸｒｅｆ、ＥＹｒｅｆをゼロにして走行を停止する。その後、障害物が除去されて距離Ｌｘが大きくなると、時刻ｔｘから記憶された目標値を読込み、再び上記演算により追尾走行を再開する。

また、目標値Ｘｐ（ｔｘ）、Ｙｐ（ｔｘ）、Ｚｐ（ｔｘ）に一定のオフセット値（Ｘｏｐ，Ｙｏｐ，Ｚｏｐ）を加算した（Ｘｐ（ｔｘ）＋Ｘｏｐ，Ｙｐ（ｔｘ）＋Ｙｏｐ，Ｚｐ（ｔｘ）＋Ｚｏｐ）を目標値とすると、図２１に示すように、人の軌跡に対し一定のオフセット量の位置で追尾するようにできる。こうして人の後方の追尾だけでなく横側の位置を保ちながら追尾できるようになる。

さらに、上記の（２）既に記憶された人の軌道を再現し走行する制御においては、車両は、図２２に示すように、上記の（１）で用いた追尾制御システムとほぼ同等の構成により走行制御する。この場合に、記憶された人（または追尾される対象物）の軌跡を時系列の目標値として走行車の目標値として自律走行する。

従って、軌道走行指令を車両の制御装置に無線または装置のスイッチ信号により与えられると、車両は記憶装置に蓄えている人（または追尾される対象物）の軌跡情報から開始位置、すなわち時間ｔ＝０の位置（Ｘｐ（０），Ｙｐ（０），Ｚｐ（０））に車両を上記の（１）と同じ原理により移動する。

その後、一定の時間間隔の時刻ｔｘの人の軌跡位置（Ｘｐ（ｔｘ），Ｙｐ（ｔｘ），Ｚｐ（ｔｘ））を目標値として、上記の（１）と同じ原理で車両を走行する。こうした目標値による走行を実行すると、人が止まった時間と同じく追尾車両も同じ時間で止まり、経路に時間変化も加わり走行することができる。

また走行目標値を時刻ｔｘの位置でなく一定の距離の変化が生じた位置の時点の時間ｔｘでの位置を目標値（Ｘｐ（ｔｘ），Ｙｐ（ｔｘ），Ｚｐ（ｔｘ））とすれば、人の軌跡を走行時間にかかわらず連続して走行することができる。なお、Ｍｒｅｆ１＝−Ｍｒｅｆ２でＭｒｅｆ１が正で与えられるとき、車両は前進（Ｘｍ軸のプラス方向）する。また、Ｍｒｅｆ１＝Ｍｒｅｆ２でＭｒｅｆ１が正であるとき、車両はその場で時計方向に旋回（ヨー軸θｍｙプラス方向）する。

そこで、記憶装置に記憶された人の軌跡に追尾する走行制御の方法については、上図２２の追尾車両制御システムは、図１９と同じ原理により走行する。記憶装置に記憶された人の軌跡を追尾するには、時刻ｔｘ時間の目標軌道に車両が移動して誤差ＥＸｒｅｆ、ＥＹｒｅｆがゼロに近づいたとき、次の時刻（ｔｘ＋Δｔ）の軌跡位置を呼出して、新たな目標値（Ｘｐ（ｔｘ＋Δｔ），Ｙｐ（ｔｘ＋Δｔ），Ｚｐ（ｔｘ＋Δｔ））として車両を同様に走行制御する。

このときに接近センサにより測定された障害物との距離Ｌｘが、指定された距離よりも車両が接近すると、走行指令ＥＸｒｅｆ、ＥＹｒｅｆをゼロにして走行を停止する。そして次の時刻（ｔｘ＋Δｔ）の軌跡位置の呼び出しを保留する。その後、障害物が除去されて距離Ｌｘが大きくなると、次の時刻（ｔｘ＋Δｔ）の軌跡位置を呼び出して、上記の演算により追尾走行を再開する。

さらに、障害物回避の走行制御は次のようにして行われる。すなわち図２３のように追尾走行車が記憶された位置の軌跡上で追尾走行中に接近センサにより障害物の距離Ｌｘが検出されて目標経路（Ｘｐ（ｔｘ），Ｙｐ（ｔｘ））に到達できない状態にあった場合、上述の方法により障害物が除去されるまでその位置で停止としている。しかし除去されない状態が一定期間ある場合、経路を変更して障害物が無い経路を生成して追尾走行を継続するように図２３の新しい目標値を生成することにより実現する。

ここで、図２３のように障害物があった場合、図２３のように目標値（Ｘｐ（ｔｘ），Ｙｐ（ｔｘ））に対し、オフセット(Ｘｏｐ，Ｙｏｐ)を加算した新しい目標値（Ｘｐｏ（ｔｘ），Ｙｐｏ（ｔｘ））を算出する。
（Ｘｐｏ（ｔｘ），Ｙｐｏ（ｔｘ））＝（Ｘｐ（ｔｘ），Ｙｐ（ｔｘ））
＋（Ｘｏｐ，Ｙｏｐ） 式３６

こうして障害物が無い場合、これを新しい目標値として走行する。なお、新しい目標値によっても障害物が検出された場合には、さらに上式３６に従いオフセットを加算して障害物が無い位置まで目標値を変更する。このようにして、障害物を回避した新しい目標値（Ｘｐｏ（ｔｘ），Ｙｐｏ（ｔｘ））に達すると、記憶された次の時間の軌道位置（Ｘｐ（ｔｘ＋Δｔ），Ｙｐ（ｔｘ＋Δｔ））を読込み、元の軌道に戻り追尾走行を続ける。

ここで、障害物を回避するために加算されるオフセット量（Ｘｏｐ，Ｙｏｐ）は、下式３７、３８、３９により算出される。すなわち、車両に実装された姿勢センサから進行方向（Ｘｓ軸）の方位θｙ（ｔｘ−Δｔ）が検出される。これを反時計方向に回転する方向となるよう目標値を変更して新しい目標値となるオフセットとする。ここで障害物を回避するための回転方向は時計方向でもよい。予めプログラムで回避する回転方向を定めることにより決定される。

Ｌｏ＝√［｛Ｘｐ（ｔｘ）−Ｘｐ（ｔｘ−Δｔ）｝²
＋｛Ｙｐ（ｔｘ）−Ｙｐ（ｔｘ−Δｔ）｝²］ 式３７
Ｘｏｐ＝Ｌｏ×［ＣＯＳ｛Θｙ（ｔｘ−Δｔ）｝
−ＣＯＳ｛Θｙ（ｔｘ−Δｔ）＋ΔΘｙ｝］ 式３８
Ｙｏｐ＝Ｌｏ×［ＳＩＮ｛Θｙ（ｔｘ−Δｔ）｝
−ＳＩＮ｛Θｙ（ｔｘ−Δｔ）＋ΔΘｙ）］ 式３９
ここでΔΘｙは予めプログラムで定められた一定値である。

以上の動作を繰返すことにより、図２３に示すように、障害物から回避した新しい経路を生成し、その後記憶装置に蓄えられた次の目標値を読込んで、元の追尾する目標経路に従い走行する。さらに、図２４は障害物回避する制御装置の構成である。

ところで、図２５のような追尾車両が非追尾側の追尾対象（人または可動物）を追尾するときに、追尾車両は追尾側との距離がセンサにより算出され図２６のように一定距離Ｌｐで追尾する。しかし図２７のように追尾対象が移動して一定距離Ｌｐ以内に追尾車両方向へ接近する場合、追尾車両と追突する問題が生じる。これを回避する方法は、下記の（３）または（４）により追突問題を避けることができる。

（３）追尾車両はＬｐ以内を算出すると、追尾車両は追尾を停止してその場で止まる。
（４）追尾対象の現在位置と位置の時間変化は図２８から算出されることを利用して、追尾車両と追尾対象との現在距離Ｌｄと、その変化方向が算出される。

ここで、図２６、図２７で、現在時刻ｔの距離Ｌｄ（ｔ）は、追尾車両現在位置（Ｘｍ（ｔ），Ｙｍ（ｔ））、追尾対象の現在位置（Ｘｐ（ｔ），Ｙｐ（ｔ））とすると、下式４０により計算される。
Ｌｄ（ｔ）＝√［｛Ｘｐ（ｔ）−Ｘｍ（ｔ）｝^２＋｛Ｙｐ（ｔ）−Ｙｍ（ｔ）｝^２］
式４０

また、図２７で、追尾対象が追尾車両に接近するときの接近方向（ΔＸ（ｔ），ΔＹ（ｔ））は、下式４１、４２により算出される。
ΔＸ（ｔ）＝Ｘｐ（ｔ）−Ｘｐ（ｔｐ） 式４１
ΔＹ（ｔ）＝Ｙｐ（ｔ）−Ｙｐ（ｔｐ） 式４２

従って、接近方向（ΔＸ（ｔ），ΔＹ（ｔ））に従い追尾車両は、現在位置（Ｘｍ（ｔ），Ｙｍ（ｔ））から（Ｘｍ（ｔ）＋ΔＸ（ｔ），Ｙｍ（ｔ）＋ΔＹ（ｔ））の位置に、図２７のように移動して追突を回避することができる。こうして接近センサが無くても追尾対象との衝突を回避することが可能となる。

また、上記の（４）で追尾車両が追突回避するとき追尾側の接近方向に移動するとき、障害物が接近センサにより検出された場合、図２３により回避することができる。さらに、障害物との距離も追尾対象の距離も接近する場合のように回避する経路が無いとき追尾車両はその場で停止する。なお、上記の（３）または（４）の選択は中央制御装置のプログラムにより設定される。

こうして上述の装置によれば、センサ信号に時刻情報、固有アドレス情報を付加した情報を無線ＬＡＮまたは無線装置により送信・受信して人と車両、または車両と車両間の追尾走行するとき特定の対象物を認識し、視覚や距離が測定できない環境や状況においても位置関係を認識することができるようになり、対象物の過去の経路を再現して追尾するようにできる。また、プログラムにより経路を忠実に追尾することもでき、もしくは経路が重なった場合、経路を短絡して追尾することもできるようになる。

さらに、追尾中に通信に何らかの障害があって通信が途絶えた場合でも途絶えた時刻情報を記憶装置から再び受信することにより経路を認識し、追尾を維持し継続することができる。従って、電波が届く範囲であれば、例えば無線ＬＡＮを経由することにより地理的距離に依存せず追尾経路情報を送り車両を経路に従い運転することができるようになる。また、多数の車両が追尾走行していても、車両間の距離も計算することにより複数の車両が衝突する問題は、定義された優先車両を先行させて衝突回避することができる。

このように経路の途中に障害物が存在して一時的に停止しても、障害物が排除された後に記憶された経路に従い、再び追尾走行を継続することが可能となる。通信データには個人認証コードが通信データに含められるので追従する対象物（または人）を特定できる。こうして多数の中から対象物を特定し追尾することができる。また経路を記憶しているため、追尾する対象物に対し後部だけでなく側面や指定した位置関係を保ち追尾することも可能となる。例えば斜め後ろで追尾するようなことが可能となる。こうして画像や接近センサで検出できない環境でも追尾することができる。

また画像装置またはレーザによる障害物回避回路を併用することにより、装置の近傍の環境を容易に認識して障害物をリアルタイムに認識し、障害物が取除かれない状況にあって目標の追尾経路に従い走行できない状態になっても障害物を回避する新たな経路を探索し、経路から逸れても再び追尾経路に戻り追尾走行を継続することができるようになる。このとき追尾する車両は１輪の駆動車輪と方向を定める旋回軸の車両、または２輪の駆動装置による走行・旋回する車両、または３輪以上の車両、または二足歩行ロボットでも良い。追尾される対象は人または動く対象物で姿勢センサを実装できる装置であれば良い。

さらに、図１９、２２に示すように、走行する場合に生じる経路誤差が生じても障害物が排除されると目的の経路に復帰することができるようになる。こうして装置自身が障害物を自動で回避し、新たな経路が発生しても目的の経路を失うことなく追尾することができるようになる。さらに、リアルタイムに相互の位置関係が分かるので視覚に入らない位置関係においても事前に衝突の防止をすることが可能となる。

また、図１７、１８、１９に示すように、複数の車両が相互の衝突を回避して一定の間隔を保ちながら同じ対象物を追尾することが可能となる。さらに、データに個人情報を盛込めることにより個人認識が容易となり、障害物による一時停止が生じても追尾走行経路を失わずに走行を継続できるようになり、人の経路を失なわずに確実に追尾をすることができる。多数の追尾車両が存在しても特定の個人を認識判別して追尾することが可能となる。

すなわち本発明においては、図１４に示すように姿勢センサ、接近センサ（またはカメラ）を実装した走行車（または歩行ロボットでもよい）は図１５のような構成により車輪を制御する走行制御される。また図１５の構成の車両には図７で定義された座標により加速度、角速度、方位を検出される姿勢センサが実装され、そのセンサ信号は図３で定義される。また、同様に追尾される人（または対象物）に、図１６のように同じ姿勢センサとそのセンサ信号または位置信号に変換されたデータを無線で送信する装置が実装される。

これにより、図１、２及び１６に示すように、被追尾側から無線により送信されたデータは追尾する車両（または歩行ロボット）に実装された受信機により受信し、図９に示すように車両の制御装置の記憶装置に追尾軌跡情報が記憶される。そして無線により追尾する対象物から姿勢情報（または軌跡情報）と時間情報と固有に定義された認識のＩＤコードを受信して車両の制御装置の記憶装置に記憶される。

こうして、図１８に示すように記憶された追尾軌跡情報を読込み、車両の走行指令として走行制御する。さらに、追尾する経路は図２８により定義されたグローバル座標に変換されて追尾する経路が算出される（式１、２、３、４、５、６、７）。このとき人にセンサを実装すると歩行姿勢によりセンサ座標が３次元的に変化する、これを式８〜１７ によりグローバル座標軸に変換して人の経路が算出される。

このとき人の歩行には、図１０に示すように歩行に伴う揺らぎやノイズ信号が含まれるため、車両の軌道とするには、図１３に示すようにノイズを式１８，１９により、揺らぎを式２０〜２５により除去した信号を軌跡として用いる。このノイズ、揺らぎ除去は被追尾側、または追尾側の装置で行ってもよい。

こうして求めた図１７、１８に示す追尾軌跡を目標値として、図１９に示すように車両に実装された姿勢センサから算出されたグローバル座標位置と比較して車両を追尾走行制御する。なお図１９は実時間で被追尾する対象物に一定距離を保ち追尾する構成で、車両は式２７〜３５によりモータが制御されて軌跡に従い追尾する。また図２２は過去に記憶された軌跡に従い追尾する構成で、車両は同様に式２７〜３５により制御されて軌跡に従い追尾する。

また、このとき軌跡上に障害物が接近センサ（またはカメラ）により検出されると、図２４にある制御装置の構成で式３６〜３９により軌道を新たに生成して回避して、図２３に示すように目標の軌道に復帰して追尾することができる。なお、ここで被追尾する対象物は人であっても車両であっても歩行ロボットでもよい。また追尾する車両は、歩行ロボットのような脚を用いた移動型ロボットでもよい。

さらに以下には、上記の被追尾システム及び追尾走行システムの処理動作についてフローチャートを用いて説明する。すなわち図２９には被追尾システムの制御装置のフローチャートを示す。このフローチャートはいわゆるメインルーチンであって、動作がスタートされるとステップＳ１でイニシャライズが行われ、ステップＳ２でサーボゲイン設定が行われ、ステップＳ３でタイマ割り込み許可が行われる。そしてステップＳ４でインジケータの表示が行われて、以後はこのステップＳ４が繰り返されている。

次に、図３０には被追尾システムの姿勢センサ回路のフローチャート（Ａ）と、姿勢情報処理のフローチャート（Ｂ）を示す。ここでフローチャート（Ａ）はタイマ割り込みルーチンであって、姿勢センサ回路にタイマ割り込みが入ると、ステップＳ１１でジャイロセンサの角速度信号と、重力加速度センサの重力方向信号と、方位センサの方位信号がＡＤ変換される。そしてステップＳ１２で中央制御装置の姿勢情報処理装置へ各データがシリアル送信される。これらの処理が終るとメインルーチンへリターンされる。

一方、ステップＳ１２でのシリアル送信によってフローチャート（Ｂ）に割り込みが入ると、ステップＳ２１で被追尾システムの姿勢センサ回路からのデータがシリアル受信される。この受信されたデータに基づいて、ステップＳ２２で移動対象物の位置が算出される。さらにステップＳ２３でＩＤコードが読み込まれ、ステップＳ２４で時刻タイマが読み込まれる。そしてステップＳ２５で、受信データ及び算出された位置データと、ＩＤコード、時刻タイマの値がメモリに保管される。

さらに、ステップＳ２６で無線ＬＡＮを通じて上記のデータが追尾走行システムへ送信される。そしてステップＳ２７で追尾システムからの欠如データ要求が受信されたか否か判断され、受信があるとき（ＹＥＳ）はステップＳ２８で追尾システムからの欠如データの時刻タイマを読み込み、ステップＳ２９で要求時刻タイマのメモリデータが読み込まれてステップＳ２６に戻される。これによって欠如データが再送信される。また、ステップＳ２７で欠如データ要求の受信がないとき（ＮＯ）はメインルーチンへリターンされる。

また図３１には、追尾走行システムの制御装置のフローチャートを示す。このフローチャートはいわゆるメインルーチンであって、動作がスタートされるとステップＳ３１でイニシャライズが行われ、ステップＳ３２でサーボゲイン設定が行われ、ステップＳ３３でタイマ割り込み許可が行われる。そしてステップＳ３４でインジケータの表示が行われて、以後はこのステップＳ３４が繰り返されている。

さらに図３２には追尾走行システムの接近センサ回路のフローチャートを示す。このフローチャートはタイマ割り込みルーチンであって、接近センサ回路にタイマ割り込みが入ると、ステップＳ４１で車両進行方向の障害物距離Ｌｘが接近センサにより計測される。そしてステップＳ４２で制御装置へ障害物距離Ｌｘのデータが送信される。これらの処理が終るとメインルーチンへリターンされる。

次に、図３３には追尾制御装置の姿勢センサ回路のフローチャート（Ａ）と、制御装置のフローチャート（Ｂ）を示す。ここでフローチャート（Ａ）はタイマ割り込みルーチンであって、姿勢センサ回路にタイマ割り込みが入ると、ステップＳ５１でジャイロセンサの角速度信号と、重力加速度センサの重力方向信号と、方位センサの方位信号がＡＤ変換される。そしてステップＳ２２で制御装置へ各データがシリアル送信される。これらの処理が終るとメインルーチンへリターンされる。

一方、ステップＳ５２でのシリアル送信によってフローチャート（Ｂ）に割り込みが入ると、ステップＳ６１で追尾制御装置の姿勢センサ回路からのデータがシリアル受信される。この受信されたデータに基づいて、ステップＳ６２で移動対象物の位置が算出される。さらにステップＳ６３で受信データ及び算出された位置データがメモリに保管される。

また、ステップＳ６４で上記のデータが追尾走行システムからのデータが無線ＬＡＮを通じて受信される。そしてステップＳ６５で受信データがメモリに保管される。さらにステップＳ６６で受信データに欠如があるか否か判断される。そして欠如があるとき（ＹＥＳ）は、ステップＳ６７で被追尾システムへ欠如データの時刻タイマデータの送信要求をしてステップＳ６４に戻される。

さらに、ステップＳ６６で欠如データないとき（ＮＯ）は、ステップＳ６８でＩＤコードの一致が判断され、一致していないとき（ＮＯ）はステップＳ６９でモータ回転角速度指令をゼロ出力にしてメインルーチンへリターンされる。これによって、追尾走行システムによる被追尾システムの追尾は終了される。すなわち、ＩＤコードが一致しない場合には追尾は終了される。

一方、ステップＳ６８でＩＤコードが一致しているときは、図３４のステップＳ７１に進められる。このステップＳ７１では、揺らぎ補正演算が行われ、追尾対象位置の座標値に対するデジタルフィルタ演算が行われる。さらに、ステップＳ７２で追尾対象の揺らぎ補正位置がメモリに記憶される。またステップＳ７３で間隔距離Ｌｄがメモリに読み込まれる。そしてステップＳ７４で車体の現在位置画が読み込まれ、被追尾対象位置の参照時刻が設定される。

さらに、ステップＳ７５で被追尾対象（人）の時刻ｔｘの位置が読み込まれ、ステップＳ７６で被追尾対象（人）の現在位置と過去位置の距離が比較される。そして距離が不一致のとき（ＮＯ）は、ステップＳ７７で過去参照時刻が更新され、ステップＳ７８で参照時間が正の値であるか否か判断されて、正の値のとき（ＹＥＳ）はステップＳ７５に戻される。そしてステップＳ７６で被追尾対象（人）の現在位置と過去位置の距離が一致のとき（ＹＥＳ）は、ステップＳ７９で目標時刻ｔｘと目標位置が決定される。

また、ステップＳ８０で接近センサから障害物距離Ｌｘのデータが受信され、ステップＳ８１で障害物距離Ｌｘが所定値Ｌｐより小さいか判断される。ここで障害物距離Ｌｘが所定値Ｌｐより大きいとき（ＮＯ）は、障害物無しとして、ステップＳ８２でオフセット値がデフォルト値と異なるか否か判断され、異なるとき（ＹＥＳ）はステップＳ８３でオフセット値をデフォルト値に微小変化させる。

一方、ステップＳ８１で障害物距離Ｌｘが所定値Ｌｐより小さいとき（ＹＥＳ）は、障害物有りとして、ステップＳ８４で回避旋回指令が発生され、所定のオフセット値が設定される。そして、ステップＳ８５において目標位置にオフセット値が加算される。なお、ステップＳ８２でオフセット値がデフォルト値と異ならないとき（ＮＯ）、及びステップＳ８３でオフセット値をデフォルト値に微小変化させた後も、ステップＳ８５において目標位置にオフセット値が加算される。

さらにステップＳ８６で車体現在位置とオフセット加算した目標位置を差分し、差分方向を求める。また、ステップＳ８７で差分値が０か否か判断され、０でないとき（ＮＯ）はステップＳ８８でモータ回転角速度指令を算出し、ステップＳ８９でドライバにより回転角速度指令と一致するようにモータを駆動する。そしてステップＳ９０で回転角速度指令をドライバに出力する。

また、ステップＳ８７で差分値が０のとき（ＹＥＳ）は目標値に到達したものと見做され、ステップＳ９１で回転角速度指定が０にされてステップＳ９０に進められる。なお、ステップＳ７８で参照時間が正の値でないとき（ＮＯ）も、ステップＳ９１で回転角速度指定が０にされてステップＳ９０に進められる。

さらにステップＳ９２で、被追尾対象（人）の現在位置と追尾車両の現在位置の距離が所定値Ｌｐより小さいか否か判断される。そして距離が所定値Ｌｐより小さくないとき（ＮＯ）は、このままメインルーチンへリターンされる。また、距離が所定値Ｌｐより小さいとき（ＹＥＳ）は被追尾側が追尾車両に接近していると判断され、ステップＳ９３で接近回避指令が発生される。

すなわちステップＳ９３では、接近方向に移動する指令を発生し、その結果に接近方向を加算し、この値から回転角速度指令を算出し、算出された回転角速度指令をドライバに出力する。そしてこれらの処理が終るとメインルーチンへリターンされる。

さらに図３５にはモータ・ドライバ回路のフローチャートを示す。このフローチャートもタイマ割り込みルーチンであって、モータ・ドライバ回路にタイマ割り込みが入ると、ステップＳ１０１で制御装置から回転角速度指令を入力し、ステップＳ１０２でモータの位置制御演算をしてＰＩＤ制御を行う。そしてステップＳ１０３でモータに電流出力がされてメインルーチンへリターンされる。

以上のフローチャートを用いて、被追尾システム及び追尾走行システムの処理動作が行われ、これにより追尾走行システムによる被追尾システムの追尾と、障害物の回避、被追尾システムの停止、あるいは被追尾システムが経路を戻ってきたときの衝突の回避が行われる。

こうして、本発明の追尾方法によれば、追尾対象物に第１の位置検出センサと第１の情報通信手段を設け、第１の位置検出センサによる位置検出情報を、時計手段からの時刻情報と共に記憶する記憶手段を有すると共に、追尾装置にも第２の位置検出センサと第２の情報通信手段を設け、追尾装置は記憶手段に記憶された情報に基づき追尾対象物が過去に通過した経路を再現して追尾を行うことによって、追尾対象物と追尾装置との間が遮られても追尾を継続することができ、良好な追尾を実現することができる。

また、本発明の追尾装置によれば、第１の位置検出センサと第１の情報通信手段の設けられた追尾対象物に対して、位置検出センサによる位置検出情報を逐次記憶する記憶手段を有すると共に、第２の位置検出センサと第２の情報通信手段が設けられ、記憶手段に記憶された情報に基づき追尾対象物が過去に通過した経路を再現して追尾を行うことによって、追尾対象物と追尾装置との間が遮られても追尾を継続することができ、良好な追尾を実現することができる。

さらに、本発明の追尾システムによれば、追尾対象物に第１の位置検出センサと第１の情報通信手段を設け、第１の位置検出センサによる位置検出情報を、時計手段からの時刻情報と共に記憶する記憶手段を有すると共に、追尾装置にも第２の位置検出センサと第２の情報通信手段を設け、追尾装置は記憶手段に記憶された情報に基づき追尾対象物が過去に通過した経路を再現して追尾を行う制御手段を有することによって、追尾対象物と追尾装置との間が遮られても追尾を継続することができ、良好な追尾を実現することができる。

また、本発明の追尾プログラムによれば、第１の位置検出センサと第１の情報通信手段の設けられた追尾対象物に対して、位置検出センサによる位置検出情報を逐次記憶する記憶手段を有すると共に、第２の位置検出センサと第２の情報通信手段が設けられ、記憶手段に記憶された情報に基づき追尾対象物が過去に通過した経路を再現して追尾を行う制御手段を有することによって、追尾対象物と追尾装置との間が遮られても追尾を継続することができ、良好な追尾を実現することができる。

なお本発明は、上述の説明した実施形態に限定されるものではなく、本発明の精神を逸脱することなく種々の変形が可能とされるものである。

本発明による追尾方法、追尾装置、追尾システム及び追尾プログラムを適用した追尾対象物、及び追尾装置の一実施形態の構成を示すブロック図である。 その姿勢情報処理装置の説明のための構成図である。 そのセンサ信号の定義の説明のための図である。 その姿勢センサのグローバル座標位置の時間変化の説明のための図である。 そのグローバル座標とローカル座標のセンサ加速度信号の説明のための図である。 そのグローバル座標軸の加速度と速度の時間変化の説明のための図である。 その姿勢センサ座標とグローバル座標の説明のための図である。 そのデータ送受信シーケンスの説明のための図である。 そのデータ送受信のデータ損失復帰手順の説明のための図である。 その信号成分の説明のための図である。 その記録されたセンサ信号とノイズ信号処理の説明のための図である。 その揺らぎ除去経路の説明のための図である。 その揺らぎ補正軌道の説明のための図である。 その追尾走行車の説明のための図である。 その追尾車両制御装置の構造図である。 その人の姿勢センサと追尾車両の姿勢センサの実装図である。 その記録された目標軌道と追従軌道の時系列変化の説明のための図である。 その人の軌跡と追尾する車両の軌跡の説明のための図である。 そのリアルタイムでの人の追尾車両制御システムの説明のための図である。 その走行指令ベクトルの説明のための図である。 その人の軌跡に対しオフセットを有して追尾する車両の軌跡の説明のための図である。 その記録した経路の追尾制御システム説明のための図である。 その障害物検出時の回避経路生成の原理の説明のための図である。 その経路中に障害物があった場合の回避経路生成システムの説明のための図である。 その追尾経路の説明のための図である。 その被追尾の経路と追尾車両の関係の説明のための図である。 その追尾側（人）が車両に接近する場合の説明のための図である。 そのグローバル座標での人の軌跡とセンサローカル座標の説明のための図である。 その処理動作の説明のためのフローチャート図である。 その処理動作の説明のためのフローチャート図である。 その処理動作の説明のためのフローチャート図である。 その処理動作の説明のためのフローチャート図である。 その処理動作の説明のためのフローチャート図である。 その処理動作の説明のためのフローチャート図である。 その処理動作の説明のためのフローチャート図である。

符号の説明

１００…姿勢センサ装置、１０１…姿勢センサ、１０２…姿勢情報処理装置、１０３…バッテリ、１０４…ＤＣ／ＤＣ電源回路、１０５…時刻タイマ、１０６…個人認証ＩＤコード記憶部、１０７…無線送受信機、２００…追尾装置、２０１…姿勢センサ、２０２…制御装置、２０３…バッテリ、２０４…ＤＣ／ＤＣ電源回路、２０５…無線送受信機、２０６，２０７…モータ、２０８，２０９…モータドライバ、２１０，２１１…接近センサ、２１２…記憶装置

Claims (40)

1. 追尾対象物に第１の位置検出センサと第１の情報通信手段を設け、
   前記第１の位置検出センサによる位置検出情報を、時計手段からの時刻情報と共に記憶する記憶手段を有すると共に、
   追尾装置にも第２の位置検出センサと第２の情報通信手段を設け、
   前記追尾装置は前記記憶手段に記憶された情報に基づき前記追尾対象物が過去に通過した経路を再現して追尾を行う
   ことを特徴とする追尾方法。
2. 請求項１記載の追尾方法において、
   前記第１及び第２の位置検出センサは、ジャイロと加速度センサと方位センサを少なくとも有し、
   前記ジャイロと加速度センサと方位センサの出力から前記位置検出情報を算出する
   ことを特徴とする追尾方法。
3. 請求項１記載の追尾方法において、
   前記追尾装置は前記記憶手段に記憶された所定時間前の前記追尾対象物の位置を目標位置として追尾を行う
   ことを特徴とする追尾方法。
4. 請求項１記載の追尾方法において、
   前記追尾装置に障害物検知センサを設け、
   前記障害物検知センサが障害物を検知したときには前記追尾を一時停止させ、
   前記障害物検知センサが障害物を検知しなくなった後に前記追尾を再開する
   ことを特徴とする追尾方法。
5. 請求項４記載の追尾方法において、
   前記再開された追尾は、前記記憶手段に記憶された前記追尾対象物の位置の経路に従って行う
   ことを特徴とする追尾方法。
6. 請求項４記載の追尾方法において、
   前記一時停止の期間が所定時間以上になったときは回避行動を行う
   ことを特徴とする追尾方法。
7. 請求項６記載の追尾方法において、
   前記回避行動は、前記記憶手段に記憶された情報に基づく前記追尾対象物が過去に通過した経路を、前記追尾装置の進行方向の左右に所定距離ずつシフトして行う
   ことを特徴とする追尾方法。
8. 請求項１記載の追尾方法において、
   前記追尾対象物の情報通信手段には固有の識別コードを設け、
   前記追跡装置は複数の前記追尾対象物の中の一つを前記識別コードで識別して追尾を行う
   ことを特徴とする追尾方法。
9. 請求項１記載の追尾方法において、
   前記記憶手段を前記追尾対象物と前記追尾装置の双方に設け、
   前記追尾装置側の記憶手段に欠落が生じたときには前記追尾対象物側の記憶手段に情報の再送信を要求する
   ことを特徴とする追尾方法。
10. 請求項１記載の追尾方法において、
    現時点の前記追尾対象物の位置検出情報を判別し、
    前記現時点の位置検出情報と前記追尾装置の位置検出情報との距離が所定の距離以内となったときに衝突回避行動を行う
    ことを特徴とする追尾方法。
11. 第１の位置検出センサと第１の情報通信手段の設けられた追尾対象物に対して、
    前記位置検出センサによる位置検出情報を逐次記憶する記憶手段を有すると共に、
    第２の位置検出センサと第２の情報通信手段が設けられ、
    前記記憶手段に記憶された情報に基づき前記追尾対象物が過去に通過した経路を再現して追尾を行う
    ことを特徴とする追尾装置。
12. 請求項１１記載の追尾装置において、
    前記第１及び第２の位置検出センサは、ジャイロと加速度センサと方位センサを少なくとも有し、
    前記ジャイロと加速度センサと方位センサの出力から前記位置検出情報を算出する
    ことを特徴とする追尾装置。
13. 請求項１１記載の追尾装置において、
    前記記憶手段に記憶された所定時間前の前記追尾対象物の位置を目標位置として追尾を行う
    ことを特徴とする追尾装置。
14. 請求項１１記載の追尾装置において、
    障害物検知センサを設け、
    前記障害物検知センサが障害物を検知したときには前記追尾を一時停止させ、
    前記障害物検知センサが障害物を検知しなくなった後に前記追尾を再開する
    ことを特徴とする追尾装置。
15. 請求項１４記載の追尾装置において、
    前記再開された追尾は、前記記憶手段に記憶された前記追尾対象物の位置の経路に従って行う
    ことを特徴とする追尾装置。
16. 請求項１４記載の追尾装置において、
    前記一時停止の期間が所定時間以上になったときは回避行動を行う
    ことを特徴とする追尾装置。
17. 請求項１６記載の追尾装置において、
    前記回避行動は、前記記憶手段に記憶された情報に基づく前記追尾対象物が過去に通過した経路を進行方向の左右に所定距離ずつシフトして行う
    ことを特徴とする追尾装置。
18. 請求項１１記載の追尾装置において、
    前記追尾対象物の情報通信手段には固有の識別コードを設け、
    複数の前記追尾対象物の中の一つを前記識別コードで識別して追尾を行う
    ことを特徴とする追尾装置。
19. 請求項１１記載の追尾装置において、
    前記記憶手段を前記追尾対象物と自己側の双方に設け、
    前記自己側の記憶手段に欠落が生じたときには前記追尾対象物側の記憶手段に情報の再送信を要求する
    ことを特徴とする追尾装置。
20. 請求項１１記載の追尾装置において、
    現時点の前記追尾対象物の位置検出情報を判別し、
    前記現時点の位置検出情報と自己の位置検出情報との距離が所定の距離以内となったときに衝突回避行動を行う
    ことを特徴とする追尾装置。
21. 追尾対象物に第１の位置検出センサと第１の情報通信手段を設け、
    前記第１の位置検出センサによる位置検出情報を、時計手段からの時刻情報と共に記憶する記憶手段を有すると共に、
    追尾装置にも第２の位置検出センサと第２の情報通信手段を設け、
    前記追尾装置は前記記憶手段に記憶された情報に基づき前記追尾対象物が過去に通過した経路を再現して追尾を行う制御手段を有する
    ことを特徴とする追尾システム。
22. 請求項２１記載の追尾システムにおいて、
    前記第１及び第２の位置検出センサは、ジャイロと加速度センサと方位センサを少なくとも有し、
    前記ジャイロと加速度センサと方位センサの出力から前記位置検出情報を算出する算出手段を有する
    ことを特徴とする追尾システム。
23. 請求項２１記載の追尾システムにおいて、
    前記追尾装置は前記記憶手段に記憶された所定時間前の前記追尾対象物の位置を目標位置として追尾を行う前記制御手段を有する
    ことを特徴とする追尾システム。
24. 請求項２１記載の追尾システムにおいて、
    前記追尾装置に障害物検知センサを設け、
    前記障害物検知センサが障害物を検知したときには前記追尾を一時停止させ、
    前記障害物検知センサが障害物を検知しなくなった後に前記追尾を再開する前記制御手段を有する
    ことを特徴とする追尾システム。
25. 請求項２４記載の追尾システムにおいて、
    前記再開された追尾は、前記記憶手段に記憶された前記追尾対象物の位置の経路に従って行う前記制御手段を有する
    ことを特徴とする追尾システム。
26. 請求項２４記載の追尾システムにおいて、
    前記一時停止の期間が所定時間以上になったときは回避行動を行う前記制御手段を有する
    ことを特徴とする追尾システム。
27. 請求項２６記載の追尾システムにおいて、
    前記回避行動は、前記記憶手段に記憶された情報に基づく前記追尾対象物が過去に通過した経路を、前記追尾装置の進行方向の左右に所定距離ずつシフトして行う前記制御手段を有する
    ことを特徴とする追尾システム。
28. 請求項２１記載の追尾システムにおいて、
    前記追尾対象物の情報通信手段には固有の識別コードを設け、
    前記追跡装置は複数の前記追尾対象物の中の一つを前記識別コードで識別して追尾を行う前記制御手段を有する
    ことを特徴とする追尾システム。
29. 請求項２１記載の追尾システムにおいて、
    前記記憶手段を前記追尾対象物と前記追尾装置の双方に設け、
    前記追尾装置側の記憶手段に欠落が生じたときには前記追尾対象物側の記憶手段に情報の再送信を要求する前記制御手段を有する
    ことを特徴とする追尾システム。
30. 請求項２１記載の追尾システムにおいて、
    現時点の前記追尾対象物の位置検出情報を判別し、
    前記現時点の位置検出情報と前記追尾装置の位置検出情報との距離が所定の距離以内となったときに衝突回避行動を行う前記制御手段を有する
    ことを特徴とする追尾システム。
31. 第１の位置検出センサと第１の情報通信手段の設けられた追尾対象物に対して、
    前記位置検出センサによる位置検出情報を逐次記憶する記憶手段を有すると共に、
    第２の位置検出センサと第２の情報通信手段が設けられ、
    前記記憶手段に記憶された情報に基づき前記追尾対象物が過去に通過した経路を再現して追尾を行う制御手段を有する
    ことを特徴とする追尾プログラム。
32. 請求項３１記載の追尾プログラムにおいて、
    前記第１及び第２の位置検出センサは、ジャイロと加速度センサと方位センサを少なくとも有し、
    前記ジャイロと加速度センサと方位センサの出力から前記位置検出情報を算出する算出手段を有する
    ことを特徴とする追尾プログラム。
33. 請求項３１記載の追尾プログラムにおいて、
    前記記憶手段に記憶された所定時間前の前記追尾対象物の位置を目標位置として追尾を行う前記制御手段を有する
    ことを特徴とする追尾プログラム。
34. 請求項３１記載の追尾プログラムにおいて、
    障害物検知センサを設け、
    前記障害物検知センサが障害物を検知したときには前記追尾を一時停止させ、
    前記障害物検知センサが障害物を検知しなくなった後に前記追尾を再開する前記制御手段を有する
    ことを特徴とする追尾プログラム。
35. 請求項３４記載の追尾プログラムにおいて、
    前記再開された追尾は、前記記憶手段に記憶された前記追尾対象物の位置の経路に従って行う前記制御手段を有する
    ことを特徴とする追尾プログラム。
36. 請求項３４記載の追尾プログラムにおいて、
    前記一時停止の期間が所定時間以上になったときは回避行動を行う前記制御手段を有する
    ことを特徴とする追尾プログラム。
37. 請求項３６記載の追尾プログラムにおいて、
    前記回避行動は、前記記憶手段に記憶された情報に基づく前記追尾対象物が過去に通過した経路を進行方向の左右に所定距離ずつシフトして行う前記制御手段を有する
    ことを特徴とする追尾プログラム。
38. 請求項３１記載の追尾プログラムにおいて、
    前記追尾対象物の情報通信手段には固有の識別コードを設け、
    複数の前記追尾対象物の中の一つを前記識別コードで識別して追尾を行う前記制御手段を有する
    ことを特徴とする追尾プログラム。
39. 請求項３１記載の追尾プログラムにおいて、
    前記記憶手段を前記追尾対象物と前記追尾装置の双方に設け、
    前記追尾装置側の記憶手段に欠落が生じたときには前記追尾対象物側の記憶手段に情報の再送信を要求する前記制御手段を有する
    ことを特徴とする追尾プログラム。
40. 請求項３１記載の追尾プログラムにおいて、
    現時点の前記追尾対象物の位置検出情報を判別し、
    前記現時点の位置検出情報と自己の位置検出情報との距離が所定の距離以内となったときに衝突回避行動を行う前記制御手段を有する
    ことを特徴とする追尾プログラム。

Images