JP5530771B2 - 移動体の旋回角度・方位・位置を検出するシステムおよび方法 - Google Patents

Description

本発明は、移動体に搭載した距離計の測定データを用いてフーリエ変換処理をすることにより、移動体の旋回角度・方位・位置を検出するシステムおよび方法に関する。

特許文献１には、移動体の位置および方位を自動検出する装置が開示されている。この装置はＧＰＳ受信機と速度センサとジャイロを備えている。通常はＧＰＳデータから移動体の位置と方位を検出することができる。また、ＧＰＳデータを受信できない状況でも、移動体の走行中、速度センサからのデータを積分処理することにより移動体の移動距離を演算し、ジャイロからの角速度を積分処理することにより移動体の方位からの旋回角度を演算し、これら移動距離と旋回角度に基づき、移動体の現在の位置と方位を検出するようになっている。

先行技術文献として参考のために特許文献２を挙げる。この特許文献２では、対象物を撮影し、そのビデオ画像データを空間周波数でフーリエ変換し、このフーリエ変換データに基づき対象物の動きを検出する。

特開２００４−２６４１８２号公報 特開２００４−３１０２８１号公報

特許文献１の位置・方位検出システムでは、高価なジャイロを必要とする。また、積分処理の際の誤差の蓄積により旋回角度の検出精度が低下する可能性もある。
また、移動体の旋回指令量から旋回角度を推測し、移動指令量により移動距離を推測して位置・方位情報を得る技術もあるが、これは移動体の滑りにより大きな誤差が生じる可能性がある。

なお、特許文献２では空間周波数でフーリエ変換したデータを用いる点で本願発明と共通であるが、対象物の動きを検出することを目的とする点および原データとしてビデオ画像データを用いる点が本願発明と異なり、さらにフーリエ変換データの利用の仕方も本願発明と異なるものである。

本発明は、上記課題を解決するため、移動体の旋回角度を検出するシステムにおいて、
（ア）移動体に搭載され、所定の走査角度範囲にわたって周囲の対象物までの距離データを生成する距離計と、
（イ）上記距離データ又はこの距離データに基づき演算された上記周囲対象物の二次元マップデータを、空間周波数領域においてフーリエ変換するフーリエ変換手段と、
（ウ）移動体が第１の方位から第２の方位へと旋回する際に、上記第１の方位での上記フーリエ変換データの所定次数の成分と、上記第２の方位での上記フーリエ変換データの上記所定次数の成分の位相差に基づき、移動体の旋回角度を演算する旋回角度演算手段と、
を備えたことを特徴とする。

上記システムによれば、比較的安価な距離計からの測定データを用いて移動体の旋回角度を検出することができる。
また、この旋回角度検出に際して、積分処理をしないので誤差が蓄積されることがなく、旋回指令量を用いないので移動体のすべりに伴う誤差も生じず、比較的高い検出精度が得られる。

好ましくは、上記フーリエ変換データの上記所定次数の成分が、三次成分である。この三次成分は旋回角度との相関性が高いので、旋回角度の検出精度をより高めることができる。

本発明の移動体の方位検出システムは、上記旋回角度検出システムを含み、さらに、この旋回角度検出システムで検出された旋回角度と既知の第１の方位に基づき、移動体の第２の方位を演算する方位演算手段を備えている。
これにより、移動体の第２方位すなわち現在方位を検出することができる。

本発明の移動体の位置・方位検出システムは、上記移動体の方位検出システムを含み、上記走査角度範囲が上記距離計を通る基準軸線を中心として広がっており、さらに、移動体が第１の位置から第２の位置へ上記基準軸線に沿って直線的に移動する際に、上記フーリエ変換手段で演算された第１の位置でのフーリエ変換データの一次成分と、上記第２の位置でのフーリエ変換データの一次成分の振幅差に基づき、移動体の移動距離を演算する移動距離演算手段と、上記演算された移動距離と、移動体の既知の第１の位置とこの第１位置での既知の方位に基づき、移動体の第２の位置を演算する位置演算手段とを備えている。
これによれば、移動体の第２方位すなわち現在方位のみならず、第２位置すなわち現在位置をも距離データを用いて検出することができる。

移動体の旋回角度検出方法は、移動体が第１の方位から第２の方位へと旋回する際の旋回角度を検出するものであって、
（ア）移動体に搭載された距離計により、移動体が上記第１の方位にある時に所定の走査角度範囲にわたって周囲の対象物までの距離データを生成し、移動体が上記第２の方位にある時に上記走査角度範囲にわたって周囲対象物までの距離データを生成し、
（イ）上記第１、第２の方位での距離データ又はこの距離データに基づき演算された上記周囲対象物の二次元マップデータを、それぞれ空間周波数領域においてフーリエ変換し、
（ウ）上記第１方位でのフーリエ変換データの所定次数の成分と上記第２方位でのフーリエ変換データの上記所定次数の成分の位相差に基づき、移動体の旋回角度を演算する
ことを特徴とする。

好ましくは、上記フーリエ変換データの上記所定次数の成分が、三次成分である。

本発明の移動体の方位検出方法は、上記移動体の旋回角度検出方法を用いて検出した移動体の旋回角度と、既知の上記第１方位とに基づき、上記第２方位を検出する。

本発明の移動体の位置・方位検出方法は、上記移動体の方位検出方法を用いて移動体の方位を検出するとともに、移動体の位置を検出する方法であって、
上記走査角度範囲が、上記距離計を通る基準軸線を中心として広がっており、
移動体が第１の位置から第２の位置へ上記基準軸線に沿って直線的に移動する際に、上記距離計により、移動体が上記第１の位置にある時に上記走査角度範囲にわたって周囲の対象物までの距離データを生成し、移動体が上記第２の位置にある時に上記走査角度範囲にわたって周囲対象物までの距離データを生成し、
上記第１、第２の位置での距離データ又はこの距離データに基づき演算された上記周囲対象物の二次元マップデータを、それぞれ空間周波数領域においてフーリエ変換し、
上記第１の位置でのフーリエ変換データの一次成分と、上記第２の位置でのフーリエ変換データの一次成分の振幅差に基づき、移動体の移動距離を演算し、
上記の演算された移動距離と、既知の第１の位置とこの第１位置での既知の方位に基づき、移動体の第２の位置を演算する。

本発明によれば、安価な距離計を用いて、負担の軽いフーリエ変換処理により、移動体の旋回角度、方位、位置を比較的高い精度で検出することができる。

本発明の一実施形態をなす位置・方位検出システムが適用されるクローラロボットの概略平面図である。 上記位置・検出システムで実行されるルーチンを作業別に示すブロック図である。 上記クローラロボットが家屋床下において布基礎の隅部から遠ざかるように直線的に後退する状態を示す概略平面図である。 （Ａ）は上記クローラロボットが後退する前後の２時点での距離データに基づいて演算された布基礎壁面のマップを示す図であり、（Ｂ）は上記２時点でのマップデータを空間周波数領域でフーリエ変換したデータの一次成分をそれぞれ示す波形図であり、（Ｃ）は同三次成分の波形図である。 一次成分の振幅差と直線移動距離とのリニアな関係を示すグラフである。 所定距離だけ直線移動した時の、一次〜四次成分の振幅差を示すグラフである。 上記クローラロボットが家屋床下において超信地旋回をしている状態を示す概略平面図である。 （Ａ）は上記クローラロボットが超信地旋回する前後の２時点での距離データに基づいて演算された布基礎壁面のマップを示す図であり、（Ｂ）は上記２時点でのマップデータを空間周波数領域でフーリエ変換したデータの一次成分を示す波形図であり、（Ｃ）は同三次成分の波形図である。 三次成分の位相差と旋回角度のリニアな関係を示すグラフである。 所定角度だけ超信地旋回した時の、一次〜四次成分の位相差を示すグラフである。

以下、本発明の一実施形態に係わるクローラロボット１０（移動体）の位置・方位検出システムについて、図面を参照しながら説明する。
説明の便宜上、図１に示すようにクローラロボット１０を基準として、その前後に延びる座標軸をＹ軸とし、左右に延びる座標軸をＸ軸とする。クローラロボット１０のＹ軸に沿って延びる中心軸線（基準軸線）を符号１０ａで示す。

本実施形態のクローラロボット１０は、軽量小型であり、前後方向に細長いボデイ１１と、このボデイ１１の左右に設けられた一対のクローラ１２とを備えている。
上記ボデイ１１には、上記中心軸線１０ａ上に位置するビデオカメラ１５とレーザー距離計１６が搭載されるとともに、送受信機１７が搭載されている。

他方、基地局２０はパソコンを主要構成としており、送受信機２１と、制御・演算部２２（図２参照）を備えるとともに、ディスプレイとリモートコントローラ（いずれも図示せず）を備えている。送受信機１７，２１間では、無線または有線による信号伝送が行われる。これら構成は周知であるので詳細な説明を省略する。

ビデオカメラ１５で撮影された映像は基地局２０のディスプレイで表示される。操作者は、ディスプレイの映像を見ながらリモートコントローラを操作してクローラロボット１２を移動させる。

上記レーザー距離計１６は、クローラロボット１０の前方および左右にわたって、より正確には上記中心軸線１０ａを中心に角度範囲Θにわたって、水平面上を走査して距離測定を行う。この角度Θは好ましくは１８０〜２４０°であり、本実施形態では２００°である。なお、距離計測は、角度Θを均等に角度分割した１２８のサンプリングポイントで行うようになっている。

図２に示すように、上記基地局２０の制御・演算部２２は後述するルーチンを実行し、実質的に、マップ作成手段２４と、ＦＦＴ２５（高速フーリエ変換手段：フーリエ変換手段）と、移動距離演算手段２６と、位置演算手段２７と、旋回角度演算手段２８と、方位演算手段２９とを備えている。

上記構成をなすクローラロボット１０を家屋の床下点検に用いた場合の位置・方位検出システムの作用について説明する。床下には布基礎１００（周囲対象物）が配置されている。操作者は、クローラロボット１０を移動させ、ディスプレイに表示された映像をみながら床下の状態を点検する。

本実施形態では、クローラロボット１０の移動制御は、中心軸線１０ａに沿った前後の直線的移動と、超信地旋回（位置を変えずにその場での旋回）に限定する。
クローラロボット１０が図３に示す第１の位置・方位にある場合を例にとって説明する。図３の状態では、クローラロボット１０は、布基礎１００において２つの壁面１０１が直角に交差した隅部１０２の近傍に配置されている。クローラロボット１０の中心軸線１０ａが上記隅部１０２を通り、２つの壁面１０１と同角度をなして交差している。

上記クローラロボット１０が直線的移動または超信地旋回をした場合、クローラロボット１０は上記第１の位置・方位から第２の位置・方位に至る。上記クローラロボット１０が図３に示すように直線移動する際には、第１の方位と第２の方位が等しく、第１の位置から第２の位置へと変化する。図７に示すように超信地旋回する際には、第１の位置と第２の位置が等しく、第１の方位から第２の方位へと変化する。

クローラロボット１０が上記第１の位置・方位にある時、レーザー距離計１６により、角度Θにわたりレーザー距離計１６から壁面１０１までの距離データを得る。上記制御・演算部２２ではこの距離データに基づき、図４（Ａ）、図８（Ａ）に実線で示すような壁面１０１のＸ，Ｙ座標における二次元マップデータを作成する。

クローラロボット１０が上記第２の位置・方位にある時にも同様にして距離データを得、二次元マップデータを作成する。
図３に矢印で示すように、クローラロボット１０を中心軸線１０ａに沿って直線的に移動させた場合、例えば布基礎１００の隅部１０２から遠ざかるように後退させた場合には、壁面１０１のマップは図４（Ａ）に破線で示すようになる。また、超信地旋回させた場合のマップは図８（Ａ）に破線で示すようになる。

上記制御・演算部２２における二次元マップ作成処理は、図２のマップ作成手段２４に相当する。上記の壁面１０１のマップはクローラロボット１０が第１位置・方位にある時および第２位置・方位にある時、基地局２０のディスプレイに表示される。

上記制御・演算部２２では、上記第１の位置・方位でのマップデータおよび第２の位置・方位でのマップデータをそれぞれ空間周波数においてフーリエ変換し、記憶する。このフーリエ変換処理は、図２のＦＦＴ２５に相当する。このフーリエ変換処理は、クローラロボット１０が各位置・方位にある時に実行してもよいし、クローラロボットの移動終了後に行ってもよい。

上記制御・演算手段２２は、上記リモートコントローラからの操作情報（直線的移動か超信地旋回かの情報）に応答して以下のルーチンを実行する。
クローラロボット１０が直線的移動例えば上述したように後退動作をする際には、制御・演算手段２２は第１の位置・方位でのフーリエ変換データから図４（Ｂ）に実線で示す一次成分（周波数が「１」の成分）を抽出するとともに、第２の位置・方位でのフーリエ変換データから図４（Ｂ）に破線で示す一次成分を抽出する。なお、図４（Ｂ）において横軸は、位相（または周期）を表し、縦軸は振幅を表す。横軸の数字は０〜１２７のサンプリングポイントを示す。

さらに、上記第１の位置・方位での一次成分と第２の位置・方位での一次成分の振幅の差ΔＷを演算し、この振幅差ΔＷから直線移動距離を演算する。上記一次成分の振幅差ΔＷと直線移動距離は、図５に示す検証試験結果に示すように相関係数０．９９８３のリニアな関係を有している。この演算処理は、図２の移動距離演算手段２６に相当する。

参考までに、上記直線移動の際の、第１の位置・方位および第２の位置・方位でのフーリエ変換データの三次成分を図４（Ｃ）に実線および破線で示す。この三次成分でも振幅差ΔＷ’が生じるが、一次成分の振幅差ΔＷより遥かに小さい。
本発明者は、所定距離だけ直線的に移動した時の一次〜四次のスペクトル成分の振幅差を測定し、図６のようなデータを得た。このデータからも、一次成分の振幅差が最大であることが理解できる。
そこで、本実施形態では上述したように、一次成分の振幅差ΔＷを用いて移動距離を演算したのである。

次に、上記第１の位置・方位と上記演算された直線移動距離に基づき、第２の位置すなわち現在の位置を演算する。上記第１の位置・方位は最初にクローラロボット１０を床下に置く時に操作者が入力されるか、制御・演算部２２で演算された位置・方位であり、既知である。この処理は図２の位置演算手段２７に相当する。

上述したようにして制御・演算部２２は第２の位置を検出する。なお、直線的移動の場合、第２の方位は既知の第１の方位と等しいので、制御・演算部２２は現在の位置・方位（第２の位置・方位）を検出したことになる。この位置・方位は、次の移動（直線的移動または超信地旋回）に際して、第１の位置・方位として提供される。
クローラロボット１０を前進する場合も、上記と同様の演算により現在の位置・方位（第２の位置・方位）を検出することができる。

クローラロボット１０が超信地旋回の場合には、制御・演算手段２２は第１の位置・方位でのフーリエ変換データから図８（Ｃ）に実線で示す三次成分（周波数が「３」の成分、換言すれば角度Θで３周期となる空間周波数をもつ成分）を抽出するとともに、第２の位置・方位でのフーリエ変換データから図８（Ｃ）に破線で示す三次成分を抽出する。

さらに、上記第１の位置・方位での三次成分と第２の位置・方位での三次成分の位相差αを演算し、この位相差αから旋回角度を演算する。上記三次成分の位相差αと旋回角度は、図９に示す検証試験結果に示すように相関係数０．９９７９のリニアな関係を有している。この演算処理は、図２の旋回角度演算手段２８に相当する。

参考までに、上記超信地旋回の際の、第１の位置・方位および第２の位置・方位でのフーリエ変換データの一次成分を図８（Ｂ）に実線および破線で示す。この一次成分での位相差α’は三次成分の位相差αより小さい。
本発明者は、所定角度だけ超信地旋回時の一次〜四次のスペクトル成分の位相差を測定し、図１０のようなデータを得た。このデータからも、三次成分の位相差が最大であることが理解できる。
そこで、本実施形態では上述したように、三次成分の振幅差αを用いて旋回角度を演算したのである。

次に、既知の第１の方位と上記演算された旋回角度に基づき、第２の方位すなわち現在の方位を演算する。この処理は、図２の方位演算手段２９に相当する。
上述したようにして制御・演算部２２は第２の方位を検出する。なお、超信地旋回の場合、第２の位置は第１の位置と等しいので、制御・演算部２２は、現在の位置・方位（第２の位置・方位）を検出したことになる。

本発明は上記実施例に制約されず、種々の態様を採用することができる。例えば、上記実施形態では距離データからＸ，Ｙ軸の２次元マップを作成し、このマップデータをフーリエ変換したが、距離データを直接フーリエ変換してもよい。

上記実施形態では旋回角度演算のためにフーリエ変換データの三次成分を用いたが、一次、二次、四次以降の成分を用いてもよいし、２以上の成分に基づいて演算された旋回角度を評価し、平均化処理して最終的な旋回角度を得てもよい。

上記実施形態では直線的移動または超信地旋回の前後における移動距離、旋回角度、位置、方位を演算したが、これら演算を移動の過程で所定時間間隔毎に行なってもよい。

本実施形態では、距離計からの距離データだけに基づいて、移動体の旋回角度、移動距離、方位、位置の演算を行ったが、例えばＧＰＳと併用し、ＧＰＳの電波が届かない場所で本実施形態の検出システムを作動させるようにしてもよい。
また、本実施形態の検出システムと、ジャイロ、速度センサを含む位置・方位検出システムとの併用も可能である。

本発明は、床下点検ロボット等の移動体の位置・方位の検出システムとして用いることができる。

１０ クローラロボット（移動体）
１６ 距離計
２４ マップ作成手段
２５ ＦＦＴ（フーリエ変換手段）
２６ 移動距離演算手段
２７ 位置演算手段
２８ 旋回角度演算手段
２９ 方位演算手段

Claims (4)

1. 移動体の旋回角度を検出するシステムにおいて、
   （ア）移動体に搭載され、所定の走査角度範囲にわたって周囲の対象物までの距離データを生成する距離計と、
   （イ）上記距離データ又はこの距離データに基づき演算された上記周囲対象物の二次元マップデータを、空間周波数領域においてフーリエ変換するフーリエ変換手段と、
   （ウ）移動体が第１の方位から第２の方位へと旋回する際に、上記第１の方位での上記フーリエ変換データの三次成分と、上記第２の方位での上記フーリエ変換データの三次成分の位相差に基づき、移動体の旋回角度を演算する旋回角度演算手段と、
   を備えたことを特徴とする移動体の旋回角度検出システム。
2. 移動体の位置および方位を検出するシステムにおいて、
   （ア）移動体に搭載され、所定の走査角度範囲にわたって周囲の対象物までの距離データを生成する距離計と、
   （イ）上記距離データ又はこの距離データに基づき演算された上記周囲対象物の二次元マップデータを、空間周波数領域においてフーリエ変換するフーリエ変換手段と、
   （ウ）移動体が第１の方位から第２の方位へと旋回する際に、上記第１の方位での上記フーリエ変換データの所定次数の成分と、上記第２の方位での上記フーリエ変換データの上記所定次数の成分の位相差に基づき、移動体の旋回角度を演算する旋回角度演算手段と、
   （エ）上記旋回角度検出システムで検出された旋回角度と既知の第１の方位に基づき、移動体の第２の方位を演算する方位演算手段と、
   を備え、上記距離計の所定の走査角度範囲が上記距離計を通る基準軸線を中心として広がっており、さらに、
   （オ）移動体が第１の位置から第２の位置へ上記基準軸線に沿って直線的に移動する際に、上記フーリエ変換手段で演算された第１の位置でのフーリエ変換データの一次成分と、上記第２の位置でのフーリエ変換データの一次成分の振幅差に基づき、移動体の移動距離を演算する移動距離演算手段と、
   （カ）上記演算された移動距離と、移動体の既知の第１の位置とこの第１位置での既知の方位に基づき、移動体の第２の位置を演算する位置演算手段と、
   を備えたことを特徴とする移動体の位置・方位検出システム。
3. 移動体が第１の方位から第２の方位へと旋回する際の旋回角度を検出する方法において、
   （ア）移動体に搭載された距離計により、移動体が上記第１の方位にある時に所定の走査角度範囲にわたって周囲の対象物までの距離データを生成し、移動体が上記第２の方位にある時に上記走査角度範囲にわたって周囲対象物までの距離データを生成し、
   （イ）制御・演算手段により、上記第１、第２の方位での距離データ又はこの距離データに基づき演算された上記周囲対象物の二次元マップデータを、それぞれ空間周波数領域においてフーリエ変換し、さらに、上記第１方位でのフーリエ変換データの三次成分と上記第２方位でのフーリエ変換データの三次成分の位相差に基づき、移動体の旋回角度を演算する
   ことを特徴とする移動体の旋回角度検出方法。
4. 移動体の位置および方位を検出する方法において、
   移動体に搭載された距離計により、移動体が上記第１の方位にある時に、上記距離計を通る基準軸線を中心として広がる走査角度範囲にわたって周囲の対象物までの距離データを生成し、移動体が上記第２の方位にある時に上記走査角度範囲にわたって周囲対象物までの距離データを生成し、
   制御・演算手段が、
   （ア）上記第１、第２の方位での距離データ又はこの距離データに基づき演算された上記周囲対象物の二次元マップデータを、それぞれ空間周波数領域においてフーリエ変換し、
   （イ）上記第１方位でのフーリエ変換データの所定次数の成分と上記第２方位でのフーリエ変換データの上記所定次数の成分の位相差に基づき、移動体の旋回角度を演算し、
   （ウ）上記演算された移動体の旋回角度と、既知の上記第１方位とに基づき、上記第２方位を演算し、
   （エ）移動体が第１の位置から第２の位置へ上記基準軸線に沿って直線的に移動する際に、上記距離計により、移動体が上記第１の位置にある時に上記走査角度範囲にわたって周囲の対象物までの距離データを生成し、移動体が上記第２の位置にある時に上記走査角度範囲にわたって周囲対象物までの距離データを生成し、
   （オ）上記第１、第２の位置での距離データ又はこの距離データに基づき演算された上記周囲対象物の二次元マップデータを、それぞれ空間周波数領域においてフーリエ変換し、
   （カ）上記第１の位置でのフーリエ変換データの一次成分と、上記第２の位置でのフーリエ変換データの一次成分の振幅差に基づき、移動体の移動距離を演算し、
   （キ）上記の演算された移動距離と、既知の第１の位置とこの第１位置での既知の方位に基づき、移動体の第２の位置を演算する
   ことを特徴とする移動体の位置・方位検出方法。