自律移動装置
【課題】マルチパス環境下においても精度良い測位ができ、走行経路に対して精度良く走行することができる自律移動装置を提供することにある、
【解決手段】自律移動装置１の測位部３は、平面内で規定した絶対座標系における座標位置を既知位置に配置された少なくとも３台の電波発信器２１〜２３からの電波の到来方位を用いて検出する。この際測位アルゴリズム処理部３５は、電波発信器２１〜２３の少なくとも一つについて直接波と反射波とを共に設定し、直接波と反射波との別を考慮して電波発信器２１〜２３を３つ選択して組にし、各組毎に、各電波発信器からの電波の到来方位を用いて求めた座標位置から既知の電波発信器２１〜２３を見込む時の各電波発信器２１〜２３の存在方位を求めるとともに、到来方位と存在方位とを照合して到来方位と存在方位ととが整合すると判定された直接波と反射波との別を考慮した電波発信器の組から得られた座標位置を採用する。
【解決手段】自律移動装置１の測位部３は、平面内で規定した絶対座標系における座標位置を既知位置に配置された少なくとも３台の電波発信器２１〜２３からの電波の到来方位を用いて検出する。この際測位アルゴリズム処理部３５は、電波発信器２１〜２３の少なくとも一つについて直接波と反射波とを共に設定し、直接波と反射波との別を考慮して電波発信器２１〜２３を３つ選択して組にし、各組毎に、各電波発信器からの電波の到来方位を用いて求めた座標位置から既知の電波発信器２１〜２３を見込む時の各電波発信器２１〜２３の存在方位を求めるとともに、到来方位と存在方位とを照合して到来方位と存在方位ととが整合すると判定された直接波と反射波との別を考慮した電波発信器の組から得られた座標位置を採用する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自律移動装置に関するものである。
【背景技術】
【０００２】
従来から、位置が既知である複数個の基地局において移動局からの電波を受信し、各基地局において検出した移動局からの電波の到来方向の関係を用いることにより移動局の位置を求める技術が知られている（例えば、特許文献１参照）。
【０００３】
この特許文献１には、ロランや、ＧＰＳ等の既存の測位システムを利用して各移動局において測位を行い、通信手段を通じて測位結果を基地局や他の移動局に伝送する測位方法の開示や、基地局側で移動局の位置を求める場合に、基地局で位置を求める電波と移動局に位置を通知する電波とを共用する測位方法の開示があるが、前者の測位方法では、ロランやＧＰＳ等の受信器を別途備える必要があって、コスト、重量、容積の増大等の問題がある。
【０００４】
一方後者では、前者の課題を解決できるものの、移動局が自分の位置を知ることができないという問題がある。
【０００５】
これらの課題を解決する測位方法として、到来方向推定アルゴリズムであるＭＵＳＩＣ（ＭＵｌｔｉｐｌｅ ＳＩｇｎａｌ Ｃｌａｓｓｉｆｉｃａｔｉｏｎ）法等を用いた電波到来方向推定により自己位置を測位する測位方法も提供されている（例えば、特許文献２）。
【特許文献１】特開平９−１１９９７０号公報
【特許文献２】特開２００６−２３４６８３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
ところで、特許文献２に開示されている到来波数の設定が必要なＭＵＳＩＣ法では、電波の回折、反射等実際に起こりうる現象の影響等によって、直接波と反射波が同時に受信されるというマルチパス環境のように、どれだけの電波が到来するか未定である環境下では正しい到来方向推定結果が得られないという問題があった。
【０００７】
本発明は、上述の点に鑑みて為されたもので、その目的とするところは、マルチパス環境下においても精度良い測位ができ、走行経路に対して精度良く走行することができる自律移動装置を提供することにある。
【課題を解決するための手段】
【０００８】
上述の目的を達成するために、請求項１の自律移動装置の発明では、走行手段と、少なくとも３つの既知位置の電波発信器から受信した電波に基づいて測位する測位部と、測位エリアの地図情報を記憶した記憶部と、前記地図情報に基づいて走行経路を生成する走行生成部と、生成された走行経路に基づいて走行手段を制御する走行制御部と、走行手段の動作に基づいて現在位置を推定するデッドレコニング手段とを備え、デッドレコニング手段は、前記測位部で測位した結果に基づいて現在位置を修正する自律移動装置において、前記測位部は、平面内で規定した絶対座標系における座標位置を既知位置に配置された少なくとも３台の電波発信器からの電波の到来方位を用いて検出するものであって、前記電波発信器の少なくとも一つについて直接波と反射波とを共に設定し、直接波と反射波との別を考慮して前記電波発信器を３つ選択して組にし、各組毎に、各電波発信器からの電波の到来方位を用いて求めた座標位置から既知の電波発信器を見込む時の各電波発信器の存在方位を求めるとともに、到来方位と存在方位とを照合して到来方位と存在方位ととが整合すると判定された直接波と反射波との別を考慮した電波発信器の組から得られた座標位置を採用することを特徴とする。
【０００９】
請求項１の自律移動装置の発明によれば、マルチパス環境下においても測位部が精度良い測位ができ、その結果、走行経路に対して精度良く走行することができる。
【００１０】
請求項２の自律移動装置の発明では、請求項１の発明において、前記測位部は、到来方向推定を方向に対するスペクトルを用いて定期的に行うものであって、前回測位時に推定された自身の方向と、電波発信器の位置とに基づいて方向に対するスペクトルの存在領域を限定することを特徴とする。
【００１１】
請求項２の自律移動装置の発明によれば、測位部が、定期的に測位することで、スペクトルについてフィルタをかけることができ、その結果、測位部での測位時間を短縮できる。
【００１２】
請求項３の自律移動装置の発明では、請求項１の発明において、マルチパスが発生する可能性が高いエリアに自身が存在するかどうかを判断する周囲環境判断部を備え、前記測位部は、マルチパスが発生する可能性の低い領域では、マルチパスを考慮しない到来方向推定を行うことを特徴とする。
【００１３】
請求項３の自律移動装置の発明によれば、マルチパスに対する考慮の有無を周囲環境に応じて判断することで、測位部の測位時間を短縮することができる。
【発明の効果】
【００１４】
本発明は、マルチパス環境下においても測位部が精度良い測位ができ、その結果、走行経路に対して精度良く走行することができるという効果がある。
【発明を実施するための最良の形態】
【００１５】
以下本発明を実施形態により説明する。
【００１６】
図１は本実施形態の自律移動装置１の構成を示しており、自律移動装置１は、図２に示すように走行エリアに配置した電波発信器２（２１〜２３）から発信される電波を、アンテナ３０を通じて受信し、この受信した電波の到来方向に基づいて測位する測位部３と、動作エリアの地図情報Ｍａｐを記憶する記憶部４と、地図情報Ｍａｐに基づいて走行経路を生成する経路生成部５と、生成された走行経路に基づいて走行手段（電動モータと走行輪等）６を制御する走行制御部７と、走行手段６の動作に基づいて現在位置を推定するデッドレコニング部８とを備えている。
【００１７】
測位部３は、図１に示すように電波発信器２（図２参照）からの電波を受信するアンテナ３０と、アンテナ３０の出力から電波の到来方位を検出するための成分を取り出すとともに抽出した成分を以後の処理のためにデジタル信号に変換する機能を有したアンテナ信号処理回路部３１と、アンテナ信号処理回路部３１の出力を用いてアンテナ座標系における電波の到来方位を推定する電波到来方向推定部３２と、電波到来方向推定のためのパラメータを格納しているパラメータ格納部３３とを備える。また、測位部３では、絶対座標系における座標位置を求めるために、電波発信器２の座標位置を知る必要があるから、絶対座標系における電波発信器２の座標をあらかじめ登録した発信器座標記憶部３４が設けられ、電波到来方向推定部３２で推定された到来方位と発信器座標記憶部３４に登録された電波発信器２の座標位置とを用いて測位部３の座標位置を求める測位アルゴリズム処理部３５が設けられる。
【００１８】
測位部３の座標位置は、測位部３の代表点の座標位置であって、以下の説明では測位部３に設けたアンテナ３０を基準として設定したアンテナ座標系の原点の座標位置を、絶対座標系における測位部３の座標位置に用いるものとする。
【００１９】
アンテナ３０は、電波の到来方位を検出することができるように構成されたものを用い、本実施形態では、複数本（図示例では円周上に等間隔に４本）のアンテナ素子３０ａを基台３０ｂに立設した形状のアレイアンテナを用いる。各アンテナ素子３０ａはモノポールであって、図示例では基台２１ｂの一面に円周上に等間隔４本を立設してある。アンテナ座標系の原点は、アンテナ素子３０ａに囲まれた部位の中心、すなわち円の中心とする。
【００２０】
アンテナ３０の出力はアンテナ信号処理回路部３１に入力される。アンテナ信号処理回路部３１は、図２に示すように、各アンテナ素子３０ａ毎のゲインを切り換えるアッテネータと、アンテナ素子３０ａで受信した信号を一定周波数に周波数変換する混合回路からなる高周波回路部ＲＦと、混合回路に局発信号を与える局部発振回路ＳＧとを備え、更に、混合回路の出力をデジタル信号に変換するＡ／Ｄ変換部ＡＤを備える。
【００２１】
高周波回路部ＲＦのアッテネータ及び混合回路はアンテナ素子３０ａと同数個設けられる。混合回路はＩＱ分離（実数成分と虚数成分の分離）の機能も備える。混合回路ではダウンコンバートを行っており、局部発振回路ＳＧから出力する局発信号の周波数（局発周波数）を変化させることによって、混合回路では一定周波数への周波数変換を行う。したがって、混合回路の出力に所定周波数を通過させる帯域フィルタを設けておくことにより、アンテナ素子３０ａで受信した信号のうち局発周波数に対応する成分のみが混合回路から出力されることになる。つまり、局発周波数を変化させることによって、電波発信器２からの電波に対応した成分を混合回路から取り出すことができる。尚高周波回路部ＲＦの出力、つまり混合回路の出力周波数は、Ａ／Ｄ変換部ＡＤにおけるサンプリングに適した周波数（サンプリング周波数の２分の１以下の周波数）に設定される。
【００２２】
Ａ／Ｄ変換部ＡＤは、高周波回路部ＲＦから出力された実数成分と虚数成分とをそれぞれデジタル値に変換する。Ａ／Ｄ変換部ＡＤについて、サンプリング周波数、サンプリング点数、出力ビット数は、例えば１０ＭＨｚ、１０００点、１２ビットとする。電波発信器２はそれぞれ異なる周波数の電波を送信しているから、Ａ／Ｄ変換部ＡＤでは各周波数毎にサンプリング点数分のサンプリングを行う。本実施形態では、３台の電波発信器２からの電波を受信するものとして、受信周波数を３回切り換える。尚、受信周波数は、測位部３の現在位置に応じて、近距離に存在する電波発信器２からの電波を受信するように選択される。
【００２３】
Ａ／Ｄ変換部ＡＤの出力は、電波到来方位推定部３２の機能を実現するＤＳＰ（デジタルシグナルプロセッサ）３００に入力される。ＤＳＰ３００は、アンテナ素子３０ａで受信した電波の実数成分と虚数成分とを用いて電波の到来方位を推定する。一方、絶対座標系Ｏ−ＸＹにおける測位部３の座標位置を登録した発信器座標記憶部３４は、ＤＳＰ３００に付設された内部メモリ（図示せず）により実現される。発信器座標記憶部３４には、電波発信器２の座標位置のほか電波発信器２からの送信信号の周波数も記憶されており、測位部３から近距離に存在する電波発信器２を３台選択し、当該電波発信器２の周波数に応じて局部発振回路ＳＧの局発周波数を選択できる。
【００２４】
ＤＳＰ３００は、測位アルゴリズム処理部３５としての機能も備えており、Ａ／Ｄ変換部ＡＤから出力された実数成分及び虚数成分により求めた電波の到来方位と、内部メモリに登録された電波発信器２の座標位置とを用いて、絶対座標系における測位部３の座標位置を演算により求める。この演算結果をマイクロコンピュータからなる制御部９に通信部１０を介して送出する。
【００２５】
この制御部９は、経路生成部５，走行制御部７，デッドレコニング部８を構成するもので、ＤＳＰ３００からは、座標位置のほか、電波の到来方位と各アンテナ素子３０ａでの受信電力も出力される。
【００２６】
測位部３では、電波発信器２毎に局発周波数を変化させる必要があるが、１台の電波発信器２からの電波の到来方位を検出する間にはアンテナ３０の受信条件を変化させる必要が無いから、１台ずつの電波発信器２については電波の到来方位を実質的に同時刻とみなせる程度の短時間で推定することができる。つまり、測位部３では、電波発信器２とアンテナ３０との相対位置が変動する場合があるが、各電波発信器２からの電波の到来方位を推定するのに必要なデータを収集する時間は短いから、電波の到来方位が変動しない程度の短時間で到来方位を推定することができる。
【００２７】
尚デッドレコニング部８は、自律移動装置１の回転角Δθ（ｔ）を計測するジャイロセンサ８０と、自律移動装置１の移動距離（Δｘ（ｔ）、Δｙ（ｚ））を計測する車速センサ８１と、これらセンサ８０、８１の検出出力から自律移動装置１の移動距離と回転角を計算して現在位置を推定する移動距離・回転角計算手段８２とで構成される。尚走行手段６の左右の車輪の回転数の差によってΔθ（ｔ）を計算する手段を、ジャイロセンサ８０の代わりに設けてもよい。
【００２８】
次に図３の測位システムの概念図を用いて、自律移動装置１の座標位置を求める方法について詳説する。
【００２９】
図３は、測位を行う対象空間（対象平面）について規定した絶対座標系Ｏ−ＸＹの座標位置として自律移動装置１の位置を検出する。具体的には、絶対座標系Ｏ−ＸＹにおける座標位置が既知である３台の電波発信器２（２１〜２３）から測位装置２に電波が到来する方位をそれぞれ検出し、各電波発信器２からの電波の到来方位と各電波発信器２（２１〜２３）の既知の座標位置とに基づいて、絶対座標系Ｏ−ＸＹにおける測位装置２の座標位置を検出する。測位装置２において各電波発信器２１〜２３からの電波を区別するために、各電波発信器２１〜２３から送信する電波はそれぞれ異なる周波数に設定される。
【００３０】
測位部３は、絶対座標系Ｏ−ＸＹにおける電波の到来方位を特定することはできないから、自律移動装置１について設定したアンテナ座標系Ｏ’−Ｘ’Ｙ’において電波の到来方位を検出する。アンテナ座標系Ｏ’−Ｘ’Ｙ’のＸ’Ｙ’平面は絶対座標系Ｏ−ＸＹのＸＹ平面と一致しているか平行であるものとする。
【００３１】
而して今、図３において、３つの電波発信器２１〜２３が既知の地点Ｐ１（ｘ１，ｙ１），Ｐ２（ｘ２，ｙ２），Ｐ３（ｘ３，ｙ３）に置かれているとする。そのとき、地点Ｐ１（ｘ１，ｙ１）からの電波到来方向推定結果がφ１ａ，φ１ｂ，Ｐ２（ｘ２，ｙ２）からの電波到来方向推定結果がφ２，Ｐ３（ｘ３，ｙ３）からの電波到来方向推定結果がφ３で、それぞれの電波発信器２１〜２３の座標値と到来方向推定結果と測位結果が表１のように求められたとする。尚電波発信器２１は直接波と反射波とを共に設定したもので、２１ａ、２１ｂは直接波と反射波とを区別して示す符号である。
【００３２】
【表１】
【００３３】
さて測位部３５の測位アルゴリズム処理部３５は、既知のカッシーニの解法を用いて自己位置の推定を行うのであるが、組み合わせを重複するような３つずつを選ぶ。上述の表１の例であれば、２１ａ，２２，２３ と２１ｂ，２２，２３である。このとき２１ａ，２２，２３ の組と、２１ｂ，２２，２３の組に対して、以下のような判定を行う。
【００３４】
電波発信器２１〜２３がＰ１（ｘ１，ｙ１），Ｐ２（ｘ２，ｙ２），Ｐ３（ｘ３，ｙ３）の３箇所に存在し、到来方向推定角がφ１ａ，φ１ｂ，φ２，φ３のとき，測位結果は以下の２通りとなる。
【００３５】
Ｐ１，Ｐ２，Ｐ３， φ１ａ，φ２，φ３⇒ｘ１ａ２３，ｙ１ａ２３，θ１ａ２３
Ｐ１，Ｐ２，Ｐ３， φ１ｂ，φ２，φ３⇒ｘ１ｂ２３，ｙ１ｂ２３，θ１ｂ２３
電波発信器座標：Ｐｉ（ｘｉ，ｙｉ） （ｉ＝１，２，３）
測位位置Ｐ＿ｔから見た電波発信器Ｐ＿ｉの方位（θｔ，ｉ）は数１で示すようになる。
【００３６】
【数１】
【００３７】
正しい測位結果ｔでは幾何的に
θｔ，ｉ＝θｔ＋θｉ
が成り立つため、ｔに含まれる全てのｉ（ｔ＝１ａ２３，１ｂ２３ならｉ＝１ａ，２，３、ｔ＝１ｂ２３なら ｉ＝１ｂ，２，３）で上式が成立すれば、そのｔの組み合わせは正
しい測位結果とし、少なくとも一つのｉで上式が不成立なら、そのｔは間違った測位結果として除去する。つまり測位アルゴリズム処理部３５の異常測位結果除去の機能が働くことになる。
【００３８】
ここで、上述のアルゴリズムによって表１の測位結果を処理したものを表２に示す。
【００３９】
尚表２では、推定された到来方位に対応する電波発信器２を添え字のみで示す。また以降に示す表においても同様に添え字で示す。またＯＫは採用を示し、ＮＧは不採用の測位結果を示す。
【００４０】
【表２】
【００４１】
よって、判定処理の結果、１ａ，２，３の組み合わせが選択され，正しい測位結果は（ｘ，ｙ，θ）＝（１０，１０，０）であると求められる。もし電波発信器２２からの電波到来方向推定角が２つになったとして、φ２ａ，φ２ｂが得られたとすれば，電波発信器２の組み合わせとしては、（２１ａ，２２ａ，２３），（２１ｂ，２２ａ，２３）（２１ａ，２２ｂ，２３），（２１ｂ，２２ｂ，２３）という４通りになる。このときの４通りの組み合わせを表３に全て列挙する。
【００４２】
【表３】
【００４３】
この４通りの組み合わせに対する判定処理の結果が表４のようになれば、２１ａ，２２ａ、２３の組み合わせのみが採用され、その測位結果（ｘ、ｙ、θ）＝（１０，１０，０）となる。
【００４４】
【表４】
【００４５】
以上のようにして、測位部３の測位アルゴリズム処理部３５は、直接波と、反射波とを分離して到来方向推定角の不適な角度を排除することによって、反射波を用いた測位結果を除去（ＮＧ）し、マルチパス環境下において精度の良い測位を行うことができるのである。
【００４６】
而して自律移動装置１の経路生成部５は記憶部４の地図情報Ｍａｐに基づいて外部から指定された地点へ移動する走行経路を生成する際に、測位部３で得られた精度良い測位結果と、デッドレコニング部８が推定する現在位置とに基づいて走行経路のずれを算出し、このずれを補正する。
【００４７】
これによって本実施形態の自律移動装置１は、走行経路に対して精度良く走行させることができることになる。
【００４８】
尚本実施形態で用いるカッシーニの解法は既知であるので、説明は省略する。
（実施形態２）
本実施形態では、実施形態１と同じ構成を用いるが、測位部３による測位を所定時間ごとに逐次行い、次回測位時の位置と、方向を測位時の位置と方向と、車速センサ８１が検出する速度から推測する。その推測した位置と方向とを用いて地図情報Ｍａｐと照合し、測位に使用する電波発信器２の方向を推定する。
【００４９】
測位部３の測位アルゴリズム処理部３５が次回測位時の電波発信器２の方向を推定する方法を図４に基づいて更に説明すると、自律移動装置１の位置がＲｏｂｏｔ（ｘｔ１，ｙｔ１）のとき、例えば車速センサ８１が検出する速度から次回測位時の位置（ｘｔ２ ，ｙｔ２）を推測し、そのときの各電波発信器２１〜２３の方向（φｄ，φｅ，φｆ）を推定する。 そして測位アルゴリズム処理部３５では、例えば既知のＣａｐｏｎ法を用いて方向に対する電波強度のスペクトルを電波発信器２１〜２３毎に図５（ａ）〜（ｃ）に示すように検出してその測位の演算に用いるが、検出したスペクトルに対して，上述の推定した範囲を設定し，その範囲以外を測位の演算に使用しないようにする。尚図５中φａ〜φｃは現在の位置における移動方向を示す。
【００５０】
つまり本実施形態では、図５（ａ）〜（ｃ）の破線部分にフィルタリングをかけることで、測位による処理時間を短縮するのである。
【００５１】
尚Ｃａｐｏｎ法は既知のものであるので、説明は省略する。またその他の構成は実施形態１と同じであるので、図示は省略する。
（実施形態３）
本実施形態の自律移動装置１は、図１に示す記憶部４に記憶させる地図情報Ｍａｐとして図６に示すようにマルチパスが起こりやすい場所（α）を設定した地図情報を用いる一方、測位アルゴリズム処理部３５ではアルゴリズムとしてマルチパスを考慮したアルゴリズムと、マルチパスを考慮していないが処理時間が短いアルゴリズムとの切り替えで測位を行う機能を備えている点に特徴がある。尚その他の構成は実施形態１と同じであるので、図示は省略する。
【００５２】
つまり、測位アルゴリズム処理部３５は、デッドレコニング部８及び測位部３自体によって推定される現在位置と、地図情報Ｍａｐに設定されたマルチパスが起こりやすい場所（α）とを比較して現在位置がマルチパスを起こりやすい場所（α）にある場合には、マルチパスを考慮するアルゴリズムを用い、マルチパスを起こしにくい場所ではマルチパスを考慮しないアルゴリズムに切り替えるのである。
【００５３】
これにより、本実施形態では、マルチパスが起こりやすい場所（α）ではこれを考慮するアルゴリズムで測位を行い、アルゴリズムを考慮する必要が無い場所では、アルゴリズムを考慮せず、処理時間が短いアルゴリズムに切り換えることで、全体の測位時間を短縮することができるのである。
【図面の簡単な説明】
【００５４】
【図１】実施形態１の全体構成図である。
【図２】実施形態１の具体的回路を示す構成図である。
【図３】実施形態１を用いた測位方法の説明図である。
【図４】実施形態２の次回測位時の電波発信器の方向推定の概念説明図である。
【図５】実施形態２の各電波発信器に対応したスペクトルデータとフィルタリングの説明図である。
【図６】実施形態３に用いる地図情報の説明図である。
【符号の説明】
【００５５】
１ 自律移動装置
２１〜２３ 電波発信器
３ 測位部
３０ アンテナ
３０ａ アンテナ素子
３０ｂ 基台
３１ アンテナ信号処理回路部
３２ 電波到来方向推定部
３３ パラメータ格納部
３４ 発信器座標記憶部
３５ 測位アルゴリズム処理部
４ 記憶部
５ 経路生成部
６ 走行手段
７ 走行制御部
８ デッドレコニング部
８０ ジャイロセンサ
８１ 車速センサ
【技術分野】
【０００１】
本発明は、自律移動装置に関するものである。
【背景技術】
【０００２】
従来から、位置が既知である複数個の基地局において移動局からの電波を受信し、各基地局において検出した移動局からの電波の到来方向の関係を用いることにより移動局の位置を求める技術が知られている（例えば、特許文献１参照）。
【０００３】
この特許文献１には、ロランや、ＧＰＳ等の既存の測位システムを利用して各移動局において測位を行い、通信手段を通じて測位結果を基地局や他の移動局に伝送する測位方法の開示や、基地局側で移動局の位置を求める場合に、基地局で位置を求める電波と移動局に位置を通知する電波とを共用する測位方法の開示があるが、前者の測位方法では、ロランやＧＰＳ等の受信器を別途備える必要があって、コスト、重量、容積の増大等の問題がある。
【０００４】
一方後者では、前者の課題を解決できるものの、移動局が自分の位置を知ることができないという問題がある。
【０００５】
これらの課題を解決する測位方法として、到来方向推定アルゴリズムであるＭＵＳＩＣ（ＭＵｌｔｉｐｌｅ ＳＩｇｎａｌ Ｃｌａｓｓｉｆｉｃａｔｉｏｎ）法等を用いた電波到来方向推定により自己位置を測位する測位方法も提供されている（例えば、特許文献２）。
【特許文献１】特開平９−１１９９７０号公報
【特許文献２】特開２００６−２３４６８３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
ところで、特許文献２に開示されている到来波数の設定が必要なＭＵＳＩＣ法では、電波の回折、反射等実際に起こりうる現象の影響等によって、直接波と反射波が同時に受信されるというマルチパス環境のように、どれだけの電波が到来するか未定である環境下では正しい到来方向推定結果が得られないという問題があった。
【０００７】
本発明は、上述の点に鑑みて為されたもので、その目的とするところは、マルチパス環境下においても精度良い測位ができ、走行経路に対して精度良く走行することができる自律移動装置を提供することにある。
【課題を解決するための手段】
【０００８】
上述の目的を達成するために、請求項１の自律移動装置の発明では、走行手段と、少なくとも３つの既知位置の電波発信器から受信した電波に基づいて測位する測位部と、測位エリアの地図情報を記憶した記憶部と、前記地図情報に基づいて走行経路を生成する走行生成部と、生成された走行経路に基づいて走行手段を制御する走行制御部と、走行手段の動作に基づいて現在位置を推定するデッドレコニング手段とを備え、デッドレコニング手段は、前記測位部で測位した結果に基づいて現在位置を修正する自律移動装置において、前記測位部は、平面内で規定した絶対座標系における座標位置を既知位置に配置された少なくとも３台の電波発信器からの電波の到来方位を用いて検出するものであって、前記電波発信器の少なくとも一つについて直接波と反射波とを共に設定し、直接波と反射波との別を考慮して前記電波発信器を３つ選択して組にし、各組毎に、各電波発信器からの電波の到来方位を用いて求めた座標位置から既知の電波発信器を見込む時の各電波発信器の存在方位を求めるとともに、到来方位と存在方位とを照合して到来方位と存在方位ととが整合すると判定された直接波と反射波との別を考慮した電波発信器の組から得られた座標位置を採用することを特徴とする。
【０００９】
請求項１の自律移動装置の発明によれば、マルチパス環境下においても測位部が精度良い測位ができ、その結果、走行経路に対して精度良く走行することができる。
【００１０】
請求項２の自律移動装置の発明では、請求項１の発明において、前記測位部は、到来方向推定を方向に対するスペクトルを用いて定期的に行うものであって、前回測位時に推定された自身の方向と、電波発信器の位置とに基づいて方向に対するスペクトルの存在領域を限定することを特徴とする。
【００１１】
請求項２の自律移動装置の発明によれば、測位部が、定期的に測位することで、スペクトルについてフィルタをかけることができ、その結果、測位部での測位時間を短縮できる。
【００１２】
請求項３の自律移動装置の発明では、請求項１の発明において、マルチパスが発生する可能性が高いエリアに自身が存在するかどうかを判断する周囲環境判断部を備え、前記測位部は、マルチパスが発生する可能性の低い領域では、マルチパスを考慮しない到来方向推定を行うことを特徴とする。
【００１３】
請求項３の自律移動装置の発明によれば、マルチパスに対する考慮の有無を周囲環境に応じて判断することで、測位部の測位時間を短縮することができる。
【発明の効果】
【００１４】
本発明は、マルチパス環境下においても測位部が精度良い測位ができ、その結果、走行経路に対して精度良く走行することができるという効果がある。
【発明を実施するための最良の形態】
【００１５】
以下本発明を実施形態により説明する。
【００１６】
図１は本実施形態の自律移動装置１の構成を示しており、自律移動装置１は、図２に示すように走行エリアに配置した電波発信器２（２１〜２３）から発信される電波を、アンテナ３０を通じて受信し、この受信した電波の到来方向に基づいて測位する測位部３と、動作エリアの地図情報Ｍａｐを記憶する記憶部４と、地図情報Ｍａｐに基づいて走行経路を生成する経路生成部５と、生成された走行経路に基づいて走行手段（電動モータと走行輪等）６を制御する走行制御部７と、走行手段６の動作に基づいて現在位置を推定するデッドレコニング部８とを備えている。
【００１７】
測位部３は、図１に示すように電波発信器２（図２参照）からの電波を受信するアンテナ３０と、アンテナ３０の出力から電波の到来方位を検出するための成分を取り出すとともに抽出した成分を以後の処理のためにデジタル信号に変換する機能を有したアンテナ信号処理回路部３１と、アンテナ信号処理回路部３１の出力を用いてアンテナ座標系における電波の到来方位を推定する電波到来方向推定部３２と、電波到来方向推定のためのパラメータを格納しているパラメータ格納部３３とを備える。また、測位部３では、絶対座標系における座標位置を求めるために、電波発信器２の座標位置を知る必要があるから、絶対座標系における電波発信器２の座標をあらかじめ登録した発信器座標記憶部３４が設けられ、電波到来方向推定部３２で推定された到来方位と発信器座標記憶部３４に登録された電波発信器２の座標位置とを用いて測位部３の座標位置を求める測位アルゴリズム処理部３５が設けられる。
【００１８】
測位部３の座標位置は、測位部３の代表点の座標位置であって、以下の説明では測位部３に設けたアンテナ３０を基準として設定したアンテナ座標系の原点の座標位置を、絶対座標系における測位部３の座標位置に用いるものとする。
【００１９】
アンテナ３０は、電波の到来方位を検出することができるように構成されたものを用い、本実施形態では、複数本（図示例では円周上に等間隔に４本）のアンテナ素子３０ａを基台３０ｂに立設した形状のアレイアンテナを用いる。各アンテナ素子３０ａはモノポールであって、図示例では基台２１ｂの一面に円周上に等間隔４本を立設してある。アンテナ座標系の原点は、アンテナ素子３０ａに囲まれた部位の中心、すなわち円の中心とする。
【００２０】
アンテナ３０の出力はアンテナ信号処理回路部３１に入力される。アンテナ信号処理回路部３１は、図２に示すように、各アンテナ素子３０ａ毎のゲインを切り換えるアッテネータと、アンテナ素子３０ａで受信した信号を一定周波数に周波数変換する混合回路からなる高周波回路部ＲＦと、混合回路に局発信号を与える局部発振回路ＳＧとを備え、更に、混合回路の出力をデジタル信号に変換するＡ／Ｄ変換部ＡＤを備える。
【００２１】
高周波回路部ＲＦのアッテネータ及び混合回路はアンテナ素子３０ａと同数個設けられる。混合回路はＩＱ分離（実数成分と虚数成分の分離）の機能も備える。混合回路ではダウンコンバートを行っており、局部発振回路ＳＧから出力する局発信号の周波数（局発周波数）を変化させることによって、混合回路では一定周波数への周波数変換を行う。したがって、混合回路の出力に所定周波数を通過させる帯域フィルタを設けておくことにより、アンテナ素子３０ａで受信した信号のうち局発周波数に対応する成分のみが混合回路から出力されることになる。つまり、局発周波数を変化させることによって、電波発信器２からの電波に対応した成分を混合回路から取り出すことができる。尚高周波回路部ＲＦの出力、つまり混合回路の出力周波数は、Ａ／Ｄ変換部ＡＤにおけるサンプリングに適した周波数（サンプリング周波数の２分の１以下の周波数）に設定される。
【００２２】
Ａ／Ｄ変換部ＡＤは、高周波回路部ＲＦから出力された実数成分と虚数成分とをそれぞれデジタル値に変換する。Ａ／Ｄ変換部ＡＤについて、サンプリング周波数、サンプリング点数、出力ビット数は、例えば１０ＭＨｚ、１０００点、１２ビットとする。電波発信器２はそれぞれ異なる周波数の電波を送信しているから、Ａ／Ｄ変換部ＡＤでは各周波数毎にサンプリング点数分のサンプリングを行う。本実施形態では、３台の電波発信器２からの電波を受信するものとして、受信周波数を３回切り換える。尚、受信周波数は、測位部３の現在位置に応じて、近距離に存在する電波発信器２からの電波を受信するように選択される。
【００２３】
Ａ／Ｄ変換部ＡＤの出力は、電波到来方位推定部３２の機能を実現するＤＳＰ（デジタルシグナルプロセッサ）３００に入力される。ＤＳＰ３００は、アンテナ素子３０ａで受信した電波の実数成分と虚数成分とを用いて電波の到来方位を推定する。一方、絶対座標系Ｏ−ＸＹにおける測位部３の座標位置を登録した発信器座標記憶部３４は、ＤＳＰ３００に付設された内部メモリ（図示せず）により実現される。発信器座標記憶部３４には、電波発信器２の座標位置のほか電波発信器２からの送信信号の周波数も記憶されており、測位部３から近距離に存在する電波発信器２を３台選択し、当該電波発信器２の周波数に応じて局部発振回路ＳＧの局発周波数を選択できる。
【００２４】
ＤＳＰ３００は、測位アルゴリズム処理部３５としての機能も備えており、Ａ／Ｄ変換部ＡＤから出力された実数成分及び虚数成分により求めた電波の到来方位と、内部メモリに登録された電波発信器２の座標位置とを用いて、絶対座標系における測位部３の座標位置を演算により求める。この演算結果をマイクロコンピュータからなる制御部９に通信部１０を介して送出する。
【００２５】
この制御部９は、経路生成部５，走行制御部７，デッドレコニング部８を構成するもので、ＤＳＰ３００からは、座標位置のほか、電波の到来方位と各アンテナ素子３０ａでの受信電力も出力される。
【００２６】
測位部３では、電波発信器２毎に局発周波数を変化させる必要があるが、１台の電波発信器２からの電波の到来方位を検出する間にはアンテナ３０の受信条件を変化させる必要が無いから、１台ずつの電波発信器２については電波の到来方位を実質的に同時刻とみなせる程度の短時間で推定することができる。つまり、測位部３では、電波発信器２とアンテナ３０との相対位置が変動する場合があるが、各電波発信器２からの電波の到来方位を推定するのに必要なデータを収集する時間は短いから、電波の到来方位が変動しない程度の短時間で到来方位を推定することができる。
【００２７】
尚デッドレコニング部８は、自律移動装置１の回転角Δθ（ｔ）を計測するジャイロセンサ８０と、自律移動装置１の移動距離（Δｘ（ｔ）、Δｙ（ｚ））を計測する車速センサ８１と、これらセンサ８０、８１の検出出力から自律移動装置１の移動距離と回転角を計算して現在位置を推定する移動距離・回転角計算手段８２とで構成される。尚走行手段６の左右の車輪の回転数の差によってΔθ（ｔ）を計算する手段を、ジャイロセンサ８０の代わりに設けてもよい。
【００２８】
次に図３の測位システムの概念図を用いて、自律移動装置１の座標位置を求める方法について詳説する。
【００２９】
図３は、測位を行う対象空間（対象平面）について規定した絶対座標系Ｏ−ＸＹの座標位置として自律移動装置１の位置を検出する。具体的には、絶対座標系Ｏ−ＸＹにおける座標位置が既知である３台の電波発信器２（２１〜２３）から測位装置２に電波が到来する方位をそれぞれ検出し、各電波発信器２からの電波の到来方位と各電波発信器２（２１〜２３）の既知の座標位置とに基づいて、絶対座標系Ｏ−ＸＹにおける測位装置２の座標位置を検出する。測位装置２において各電波発信器２１〜２３からの電波を区別するために、各電波発信器２１〜２３から送信する電波はそれぞれ異なる周波数に設定される。
【００３０】
測位部３は、絶対座標系Ｏ−ＸＹにおける電波の到来方位を特定することはできないから、自律移動装置１について設定したアンテナ座標系Ｏ’−Ｘ’Ｙ’において電波の到来方位を検出する。アンテナ座標系Ｏ’−Ｘ’Ｙ’のＸ’Ｙ’平面は絶対座標系Ｏ−ＸＹのＸＹ平面と一致しているか平行であるものとする。
【００３１】
而して今、図３において、３つの電波発信器２１〜２３が既知の地点Ｐ１（ｘ１，ｙ１），Ｐ２（ｘ２，ｙ２），Ｐ３（ｘ３，ｙ３）に置かれているとする。そのとき、地点Ｐ１（ｘ１，ｙ１）からの電波到来方向推定結果がφ１ａ，φ１ｂ，Ｐ２（ｘ２，ｙ２）からの電波到来方向推定結果がφ２，Ｐ３（ｘ３，ｙ３）からの電波到来方向推定結果がφ３で、それぞれの電波発信器２１〜２３の座標値と到来方向推定結果と測位結果が表１のように求められたとする。尚電波発信器２１は直接波と反射波とを共に設定したもので、２１ａ、２１ｂは直接波と反射波とを区別して示す符号である。
【００３２】
【表１】
【００３３】
さて測位部３５の測位アルゴリズム処理部３５は、既知のカッシーニの解法を用いて自己位置の推定を行うのであるが、組み合わせを重複するような３つずつを選ぶ。上述の表１の例であれば、２１ａ，２２，２３ と２１ｂ，２２，２３である。このとき２１ａ，２２，２３ の組と、２１ｂ，２２，２３の組に対して、以下のような判定を行う。
【００３４】
電波発信器２１〜２３がＰ１（ｘ１，ｙ１），Ｐ２（ｘ２，ｙ２），Ｐ３（ｘ３，ｙ３）の３箇所に存在し、到来方向推定角がφ１ａ，φ１ｂ，φ２，φ３のとき，測位結果は以下の２通りとなる。
【００３５】
Ｐ１，Ｐ２，Ｐ３， φ１ａ，φ２，φ３⇒ｘ１ａ２３，ｙ１ａ２３，θ１ａ２３
Ｐ１，Ｐ２，Ｐ３， φ１ｂ，φ２，φ３⇒ｘ１ｂ２３，ｙ１ｂ２３，θ１ｂ２３
電波発信器座標：Ｐｉ（ｘｉ，ｙｉ） （ｉ＝１，２，３）
測位位置Ｐ＿ｔから見た電波発信器Ｐ＿ｉの方位（θｔ，ｉ）は数１で示すようになる。
【００３６】
【数１】
【００３７】
正しい測位結果ｔでは幾何的に
θｔ，ｉ＝θｔ＋θｉ
が成り立つため、ｔに含まれる全てのｉ（ｔ＝１ａ２３，１ｂ２３ならｉ＝１ａ，２，３、ｔ＝１ｂ２３なら ｉ＝１ｂ，２，３）で上式が成立すれば、そのｔの組み合わせは正
しい測位結果とし、少なくとも一つのｉで上式が不成立なら、そのｔは間違った測位結果として除去する。つまり測位アルゴリズム処理部３５の異常測位結果除去の機能が働くことになる。
【００３８】
ここで、上述のアルゴリズムによって表１の測位結果を処理したものを表２に示す。
【００３９】
尚表２では、推定された到来方位に対応する電波発信器２を添え字のみで示す。また以降に示す表においても同様に添え字で示す。またＯＫは採用を示し、ＮＧは不採用の測位結果を示す。
【００４０】
【表２】
【００４１】
よって、判定処理の結果、１ａ，２，３の組み合わせが選択され，正しい測位結果は（ｘ，ｙ，θ）＝（１０，１０，０）であると求められる。もし電波発信器２２からの電波到来方向推定角が２つになったとして、φ２ａ，φ２ｂが得られたとすれば，電波発信器２の組み合わせとしては、（２１ａ，２２ａ，２３），（２１ｂ，２２ａ，２３）（２１ａ，２２ｂ，２３），（２１ｂ，２２ｂ，２３）という４通りになる。このときの４通りの組み合わせを表３に全て列挙する。
【００４２】
【表３】
【００４３】
この４通りの組み合わせに対する判定処理の結果が表４のようになれば、２１ａ，２２ａ、２３の組み合わせのみが採用され、その測位結果（ｘ、ｙ、θ）＝（１０，１０，０）となる。
【００４４】
【表４】
【００４５】
以上のようにして、測位部３の測位アルゴリズム処理部３５は、直接波と、反射波とを分離して到来方向推定角の不適な角度を排除することによって、反射波を用いた測位結果を除去（ＮＧ）し、マルチパス環境下において精度の良い測位を行うことができるのである。
【００４６】
而して自律移動装置１の経路生成部５は記憶部４の地図情報Ｍａｐに基づいて外部から指定された地点へ移動する走行経路を生成する際に、測位部３で得られた精度良い測位結果と、デッドレコニング部８が推定する現在位置とに基づいて走行経路のずれを算出し、このずれを補正する。
【００４７】
これによって本実施形態の自律移動装置１は、走行経路に対して精度良く走行させることができることになる。
【００４８】
尚本実施形態で用いるカッシーニの解法は既知であるので、説明は省略する。
（実施形態２）
本実施形態では、実施形態１と同じ構成を用いるが、測位部３による測位を所定時間ごとに逐次行い、次回測位時の位置と、方向を測位時の位置と方向と、車速センサ８１が検出する速度から推測する。その推測した位置と方向とを用いて地図情報Ｍａｐと照合し、測位に使用する電波発信器２の方向を推定する。
【００４９】
測位部３の測位アルゴリズム処理部３５が次回測位時の電波発信器２の方向を推定する方法を図４に基づいて更に説明すると、自律移動装置１の位置がＲｏｂｏｔ（ｘｔ１，ｙｔ１）のとき、例えば車速センサ８１が検出する速度から次回測位時の位置（ｘｔ２ ，ｙｔ２）を推測し、そのときの各電波発信器２１〜２３の方向（φｄ，φｅ，φｆ）を推定する。 そして測位アルゴリズム処理部３５では、例えば既知のＣａｐｏｎ法を用いて方向に対する電波強度のスペクトルを電波発信器２１〜２３毎に図５（ａ）〜（ｃ）に示すように検出してその測位の演算に用いるが、検出したスペクトルに対して，上述の推定した範囲を設定し，その範囲以外を測位の演算に使用しないようにする。尚図５中φａ〜φｃは現在の位置における移動方向を示す。
【００５０】
つまり本実施形態では、図５（ａ）〜（ｃ）の破線部分にフィルタリングをかけることで、測位による処理時間を短縮するのである。
【００５１】
尚Ｃａｐｏｎ法は既知のものであるので、説明は省略する。またその他の構成は実施形態１と同じであるので、図示は省略する。
（実施形態３）
本実施形態の自律移動装置１は、図１に示す記憶部４に記憶させる地図情報Ｍａｐとして図６に示すようにマルチパスが起こりやすい場所（α）を設定した地図情報を用いる一方、測位アルゴリズム処理部３５ではアルゴリズムとしてマルチパスを考慮したアルゴリズムと、マルチパスを考慮していないが処理時間が短いアルゴリズムとの切り替えで測位を行う機能を備えている点に特徴がある。尚その他の構成は実施形態１と同じであるので、図示は省略する。
【００５２】
つまり、測位アルゴリズム処理部３５は、デッドレコニング部８及び測位部３自体によって推定される現在位置と、地図情報Ｍａｐに設定されたマルチパスが起こりやすい場所（α）とを比較して現在位置がマルチパスを起こりやすい場所（α）にある場合には、マルチパスを考慮するアルゴリズムを用い、マルチパスを起こしにくい場所ではマルチパスを考慮しないアルゴリズムに切り替えるのである。
【００５３】
これにより、本実施形態では、マルチパスが起こりやすい場所（α）ではこれを考慮するアルゴリズムで測位を行い、アルゴリズムを考慮する必要が無い場所では、アルゴリズムを考慮せず、処理時間が短いアルゴリズムに切り換えることで、全体の測位時間を短縮することができるのである。
【図面の簡単な説明】
【００５４】
【図１】実施形態１の全体構成図である。
【図２】実施形態１の具体的回路を示す構成図である。
【図３】実施形態１を用いた測位方法の説明図である。
【図４】実施形態２の次回測位時の電波発信器の方向推定の概念説明図である。
【図５】実施形態２の各電波発信器に対応したスペクトルデータとフィルタリングの説明図である。
【図６】実施形態３に用いる地図情報の説明図である。
【符号の説明】
【００５５】
１ 自律移動装置
２１〜２３ 電波発信器
３ 測位部
３０ アンテナ
３０ａ アンテナ素子
３０ｂ 基台
３１ アンテナ信号処理回路部
３２ 電波到来方向推定部
３３ パラメータ格納部
３４ 発信器座標記憶部
３５ 測位アルゴリズム処理部
４ 記憶部
５ 経路生成部
６ 走行手段
７ 走行制御部
８ デッドレコニング部
８０ ジャイロセンサ
８１ 車速センサ
【特許請求の範囲】
【請求項１】
走行手段と、少なくとも３つの既知位置の電波発信器から受信した電波に基づいて測位する測位部と、測位エリアの地図情報を記憶した記憶部と、前記地図情報に基づいて走行経路を生成する走行生成部と、生成された走行経路に基づいて走行手段を制御する走行制御部と、走行手段の動作に基づいて現在位置を推定するデッドレコニング手段とを備え、デッドレコニング手段は、前記測位部で測位した結果に基づいて現在位置を修正する自律移動装置において、
前記測位部は、平面内で規定した絶対座標系における座標位置を既知位置に配置された少なくとも３台の電波発信器からの電波の到来方位を用いて検出するものであって、前記電波発信器の少なくとも一つについて直接波と反射波とを共に設定し、直接波と反射波との別を考慮して前記電波発信器を３つ選択して組にし、各組毎に、各電波発信器からの電波の到来方位を用いて求めた座標位置から既知の電波発信器を見込む時の各電波発信器の存在方位を求めるとともに、到来方位と存在方位とを照合して到来方位と存在方位ととが整合すると判定された直接波と反射波との別を考慮した電波発信器の組から得られた座標位置を採用することを特徴とする自律移動装置。
【請求項２】
前記測位部は、到来方向推定を方向に対するスペクトルを用いて定期的に行うものであって、前回測位時に推定された自身の方向と、電波発信器の位置とに基づいて方向に対するスペクトルの存在領域を限定することを特徴とする請求項１記載の自律移動装置。
【請求項３】
マルチパスが発生する可能性が高いエリアに自身が存在するかどうかを判断する周囲環境判断部を備え、前記測位部は、マルチパスが発生する可能性の低い領域では、マルチパスを考慮しない到来方向推定を行うことを特徴とする請求項１記載の自律移動装置。
【請求項１】
走行手段と、少なくとも３つの既知位置の電波発信器から受信した電波に基づいて測位する測位部と、測位エリアの地図情報を記憶した記憶部と、前記地図情報に基づいて走行経路を生成する走行生成部と、生成された走行経路に基づいて走行手段を制御する走行制御部と、走行手段の動作に基づいて現在位置を推定するデッドレコニング手段とを備え、デッドレコニング手段は、前記測位部で測位した結果に基づいて現在位置を修正する自律移動装置において、
前記測位部は、平面内で規定した絶対座標系における座標位置を既知位置に配置された少なくとも３台の電波発信器からの電波の到来方位を用いて検出するものであって、前記電波発信器の少なくとも一つについて直接波と反射波とを共に設定し、直接波と反射波との別を考慮して前記電波発信器を３つ選択して組にし、各組毎に、各電波発信器からの電波の到来方位を用いて求めた座標位置から既知の電波発信器を見込む時の各電波発信器の存在方位を求めるとともに、到来方位と存在方位とを照合して到来方位と存在方位ととが整合すると判定された直接波と反射波との別を考慮した電波発信器の組から得られた座標位置を採用することを特徴とする自律移動装置。
【請求項２】
前記測位部は、到来方向推定を方向に対するスペクトルを用いて定期的に行うものであって、前回測位時に推定された自身の方向と、電波発信器の位置とに基づいて方向に対するスペクトルの存在領域を限定することを特徴とする請求項１記載の自律移動装置。
【請求項３】
マルチパスが発生する可能性が高いエリアに自身が存在するかどうかを判断する周囲環境判断部を備え、前記測位部は、マルチパスが発生する可能性の低い領域では、マルチパスを考慮しない到来方向推定を行うことを特徴とする請求項１記載の自律移動装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２００８−１５８９０５（Ｐ２００８−１５８９０５Ａ）
【公開日】平成２０年７月１０日（２００８．７．１０）
【国際特許分類】
【出願番号】特願２００６−３４８５４９（Ｐ２００６−３４８５４９）
【出願日】平成１８年１２月２５日（２００６．１２．２５）
【出願人】（０００００５８３２）松下電工株式会社 (17,916)
【Ｆターム（参考）】
【公開日】平成２０年７月１０日（２００８．７．１０）
【国際特許分類】
【出願日】平成１８年１２月２５日（２００６．１２．２５）
【出願人】（０００００５８３２）松下電工株式会社 (17,916)
【Ｆターム（参考）】
[ Back to top ]