WO2005050246A1

WO2005050246A1 - 測位装置

Info

Publication number: WO2005050246A1
Application number: PCT/JP2003/014660
Authority: WO
Inventors: Nobuhiro Suzuki; Atsushi Okamura
Original assignee: Mitsubishi Denki Kabushiki Kaisha
Priority date: 2003-11-18
Filing date: 2003-11-18
Publication date: 2005-06-02
Also published as: JPWO2005050246A1; JP4396636B2

Abstract

複数の固定発信器が放射した電波を受信するセンサ５と、センサ５が受信した電波に基づいて、その電波の前記センサへの到来方向を算出する測角器１３と、複数の固定発信器の位置を記憶する発信器位置記憶器１４と、発信器位置記憶器１４が記憶する固定発信器の位置と電波の到来方向とに基づいて自位置を算出する測位器１５と、を備えた測位装置において、測角器１３は、電波の一次元到来方向を算出し、測位器１５は、センサ５の位置と姿勢との座標成分のうち未知なる成分の数と同数の固定発信器の位置と、それらの固定発信器が放射した電波の一次元到来方向と、を用いて自位置を算出するようにした。

Description

測位装置

腿分野

この発明は、電波を用いて位置を測 ¾Tる装置に係るものであり、特に固定発信器が放射する電波に基づいて位置を測るに関する。

明

背景擺田

m 書

電波を検出して、移動体や所定の場所（以後、「自位置」と称する）などの位置を測 ¾ るシステムとしては、 GP S (Global Positioning System)が知られてレる。これは、人工衛星に搭載した電波の発信機からの電波の伝播時間を計測するものである。

このような電波の伝播時間を用いて位置を測 ¾τる方法においては、時間の計測精度が接的に位置の測定精度に影響する。したがって、高精度な位置の測定を^するためには、例えばセシウム原子時計など、極めて高精度な時計を電波の発信機に搭載する必要がある。しかし、このような時計は高価であるから、例えば、 GP S信号の届かない室内において、この方法による位置測定システムを構築しょうとする場合、高価な発信機を室内に設ける必要が生じる。通常、室内には、壁面や支柱など、電波の遮蔽物がるので、遮睡によらずに位置を測定しょうとすると、高価な発信機を何台も設けることになり、その設備は^:なものとなる。

また、この他の方法として、特公昭 5 1 - 4 1 4 7 7ゃ特開 2 0 0 1— 2 6 4 4 0 5には、複数の固定発信機から到来する電波の到来方向（角度）を測定し、この角度と各固定発信機の位置に基づいて自位置を算出する方法が紹介されている。

これらの ¾による测妨法では、仰角と方位角など 2以上の角度を算出する 2次元の測角センサを、測位を行う文像である測位装置に搭載する必要があり、その結果システム構成がになるという問題が、あった。また、同時に多数の固定発信機からの電波を検出しなければならず、室内など遮蔽物の多レ觀で用いることが困難な ^もあった。

この発明は、高精度の時計を発信機に搭載する必要がなぐかつ、ごく少数の発信器が放射する電波を検出できれば、所定の媳の位置の測定を行うこと力きる方法を提供することを目的とするものである。発明の開示

この発明に係る測位装置は、

複数の固定発信器が放射した電波をするセンサと、

lift己センサが糊した電波に基づいて、その電波の觸己センサへの到来方向を算出する測角器と、

嫌己複数の固定発信器の位置を記憶する発信器位置記憶器と、

Ιϋΐ己発信器位置記憶器が記憶する前記固定発信器の位置と ttit己電波の到来方向とに基づいて自位置を算出する測位器と、を備えた測位装置において、

前言 ¾則角器は、嫌 H電波の一^到来方向を算出し、

ttii y位器は、前記センサの位置及 Ό姿勢め座標成分のうち未知である成分の数と同数の嫌己固定発信器の位置と、それらの固定発信器が放射した電波の一^到来方向と、に基づいて自位置を算出するものである。

このように、この発明に係る測位装置によれば、構成が単純である一^ ¾測角器を用いて自位置を測^ ることができる。また、発信器側に電波の発信時間を測^ るための時計を ί荅載する必要もない。そのため、安価に位置測定システムを施設すること力河能となるのである。

この発明に係る別の爾立装置は、

複数の固定発信器が放射した電波を受信するセンサと、

鎌己固定発信器から編己センサが受信した電波に基づいて、その電波の嫌 3センサへの到来方向を算出する測角器と、嫌己複数の固信器の位置を記憶する発信器位置記憶器と、嫌 3発信器位置記憶器が己憶する前記固^ §信器の位置と前記電波の到来方向とに基づいて自位置を算出する測位器と、を備えた測位装置において、

iiit 目』角器は、嫌己各電波の到来方向の仰角と前記各電波の到来方向間の平行相対角とを算出し、

前記測位器は、嫌己センサの位置と姿勢との座標成分のうち未知なる成分の数より 1少ない数の編 B固定発信器の位置と、嫌則角器が算出した仰角と ¥ff相対角と、を用レ、て自位置を算出するものである。

このように、この発明に係る測位装置によれば、電波の到来方向の仰角成分と ¥1亍相対角成分とを活用することで、自位置のうちの未知成分を特るのに必要となる発信器の個数を少なく抑えることができる。そのため、ごく少数の発信器からの電波を受信することで自位置を測定できるようになるのである。図面の簡単な説明

図 1は、この発明の難の形態 1による位置測定システムの構成を示す概念図、図 2は、この発明の難の形態 1による移動体の背後からの鋭見図、

図 3は、この発明の難の形態 1によるセンサの一部を切り欠いた W見図、図 4は、この発明の実施の形態 1による測位装置の構成を示すブロック図、図 5は、この発明の霞の形態 1による測角方法の説明図、

図 6は、この発明の実施の形態 1による位置の算出原理の説明図、

図 7は、この発明の実施の形態 1における逐次近似法のフローチヤ一ト、図 8は、この発明の難の形態 2による位置測定システムの構成を示す概念図、図 9は、この発明の鍾の形態 2によるセンサの 1見図、

図 1 0は、この発明の実施の形態 2による移動体の鋭見図、

図 1 1は、この発明の実施の形態 2による測位装置の構成を示すブロック図、図 1 2は、この発明の実施の形態 2による空間平均法の原理を説明するための概念図、

図 1 3は、この発明の実施の形態 2による測位装置の方索処理のフローチヤ一卜、

図 1 4は、この発明の実施の形態 2による位置の算出^！を説明するための概念図、図 1 5は、この発明の難の形態 3による位置の算出醒を説明するための概念図、図 1 6は、この発明の実施の形態 4による測位装置の構成を示すブロック図、図 1 7は、この発明の実施の形態 4による位置測定処理のフローチャート、図 1 8は、 'この発明の実施の形態 4による位置の算出原理を説明するための概念図、である。発明を実施するための最良の形態

実施の形態 1.

図 1は、この発明の雄の形態 1による位置測定システムの構成を示す概念図である。図において、天井 1はこの位置測定システムが言躍される施設の天井である。このような施設の例としては、倉庫の他、展^^スポーツの試合、コンサートなどが開催される屋内施設を挙げることができる。隨こ示されるように、天井 1の表面には、固定発信器 2カ复数個装着されている。固定発信器 2のそれぞ、れは、この室内の床面 3に向けて電波を放射するようになっている。ただし、固定発信器 2のそれぞれから放射された電波をするために、発信に異なる周波数が予め割り当てられており、固定発信器 2はその周波数による電波を放射するのである。

移動体 4は、例えば荷物を運搬するために倉庫内を移動するフォークリフトなど、床面 3上を走行又は移動する物体である。屋内里姆場であれば、 «投手を腿するリリ一フカ一なども、このような移動体に該当する。センサ 5は、移動体 4に搭載されたセンサであって、天井 1に装着された固定発信器 2が放射した電波を受信するものである。

なお、以後の説明において、垂直方向の座標を z座標とし、水平方向の座標を X座標、 y座標によって表すこととする。さらに実施の形態 1では、床面 3を扉水平面 (z座標の値が 0である平面）として扱うこととする。

図 2は、移動体 4の背後からの廳図である。図に示すように、センサ 5は移動体 4の操縦席上方に設置され固定発信器 2からの電波を受信するようになっている。ただし、センサ 5は、固定発信器 2からの電波を受信できる位置に設置されれば十分であり、移動体 4の §週部に言躍されること〖、須ではない。

図 3は、 ^^を切り欠いたセンサ 5カ够動体 4に装着されている状況を示す #疆図である。図において、アンテナ 5 a及び 5 bは、無指向性のダイポールアンテナであつて、且のアレーアンテナを構成している。アンテナ 5 a及び 5 bは、無指向性を高めるためにその形状を円柱状としており、さらに側面を導体としている。また、ァンテナ 5 a及び 5 bは、円環状の絶椽体 5 cによって隔てられている。ここで、アンテナ 5 aの上端と下端との中点から、アンテナ 5 bの上端と下端との中点までの距離を dであるものとする。距離 dは、後にアンテナ 5 a及び 5 bが検出した電波の到来方向を算出する上で必要となる。

さらにアンテナ 5 bは、円環状の終椽体 5 dを隔てて、移動体 4に装着されている。アンテナ 5 aは、フィーダ線 5 eと半田などを用いた接点 5 fによって齢されている。またアンテナ 5 bも、フィーダ線 5 gと半田などを用いた接点 5 hによって接合されている。フィーダ線 5 eと 5 gは、移動体 4の本体表面に開設された孔 6を通過しており、それらの端点は、移動体 4の内部にある図^ ϋ:ぬ信号処理部と漏している。

なお、ここに示したセンサ 5の構成は、に過ぎない。すなわち、センサ 5は、移動体 4の周囲に配置された固定発信器 2が放射する電波を受信するのであれば十分なのである。したがって、例えば円柱形状でないアンテナを用いるものであっても、無指向性アンテナ、あるい〖お旨向性を繩できるアンテナであれば、このような条件を満たす。

またこの例では、アンテナ 5 aと 5 bとをァレ一素子として上下に配置する例を示したが、このような配置に限るものではなく、一定の距離（ここでは d) を隔てて酉己置されるものであればどのような配置であってもよい。また素子数も 2に限るものでないことはいうまでもない。

図 4は、この発明の実施の形態 1による測位装置の構成を示すブロック図である。図において測位装置 7は、センサ 5と信号処理部 8とを備えている。図 3で示したように、センサ 5は、アンテナ 5 aとアンテナ 5 bとをァレ一素子として備えるアレーアンテナである。信号処理部 8において、受信器 1 1 aは、例えば G a A s F ET などの低雑音増幅器を用いて増幅を行い、その結果を受信信号として出力する部位である。その目的は、アンテナ 5 aによって検出された電波信号の最小受信信号レベルをできるだけ低くし、電波の探知能力を高めることにある。

なお、この説明及び以降の説明において、部位という語は、所定の機能を觀する.. ために設計された専用の素子又は回路を意味するが、汎用の D S P (Digital Signal Processor) によって所定の作用を及ぼすように構成してもよい。また汎用の C P U (Central Processing Unit)に所定の動作を行わせるようなコンピュ一タプログラムを用いて構成するようにしても構わない。

AZD変腿 1 2 aは、受信器 1 1 aによって出力されたアナログ受信信号をデジタル信号に変換して出力する部位である。

受信器 1 1 bと AZD変麵 1 2 bは、アンテナ 5 bが検出した電波信号に対して作用する部位であって、受信器 1 l bは受信器 1 1 aと同様の作用を有し、また A/ D変擺 1 2 bは A/D変 β 1 2 aと同様の作用を有するものである。

測角器 1 3は、 A/D変騰 1 2 a及び 1 2 bの出力するディジタル信号を高速フーリェ変換などの手法によって周波斤し、これらのアンテナ素子の受信した電波の位相差を求めて、電波の到来方向を算出する音啦である。発信器 I青報記憶器 1 4は、天井 1に装着された複数の固信器の位置と、その固定発信器が放射する電波の周波数と、を関連づけて記憶する記憶^?または回路、あるいはそのような情報が予め記録された CD— ROMなどの記憶媒体を読み取る記憶装置である。測位器 1 5は、測角器 1 3が出力した電波の到来方向と、発信器 I青報記憶器 1 4力 3憶している固定発 ί言器の位置とに基づいて、自位置を算出する音 Μ立である。

次に、この発明の実施の形態 1による位置測定システムの動作について説明する。複数の固定発信器によって放射された電波は、移動体 4のセンサ 5に到来し、センサ 5のアンテナ 5 aと 5 bとによってそれぞれ検出される。そしてアンテナ 5 a及び 5 bは、これらの電波を受信信号として出力する。続いて受信器 1 1 aと 1 l bは、この受信信号を増幅する。その後、 A/D変換器 1 2 aと 1 2 bは、受信信号をディジタル信号に変換して、測角器 1 4に出力する。

測角器 1 3は、高速フーリエ変換などを用いて、受信信号位相差を求める。ここで図 5に示すように、この受信信号位相差をゅ、到来する電波の波長を λとし、入射する電波とアンテナ 5 a及び 5 bの表面に対する ¾|泉とがなす角（仰角）を φとする。そうすると、電波の波源である固定発信器とセンサ 5との距離が、アンテナ 5 aの中心とアンテナ 5 bの中心との距離 dに比べて十分に長いものとすれば、式（1 ) の関係が成立する。

そこで、測角器 1 3は、周波数麟斤の結果として取得した位相差と予め記憶しておいた波長えとから、式（1 ) に基づいて仰角 φを算出するのである。

なお、測角器 1 3は、一度に一つの固定発信器からの電波に基づいて到来方向を計算して出力するが、複数の固定発信器からの電波を弁別するには、固定発信廳に異なる周波数を割り当てる方法（周波数分割方式)、固定発信器毎に発信タイミングを変える方法（時分割^)、あるいは固定発信器の変調符号を固定発信器毎に変える方法

(符号分割: を用いて、複数の固定発信器からの電波の到来方向を算出すればよこのようにして、則角器 1 3は、少なくとも 2つの固定発信器からの電波について仰角 φを算出する。以後の説明において、この 2つの固定発信器をそれぞれ固定発信器 2— 1、 2— 2とし、各固信器から到来した電波の仰角を Φ 1および ψ 2とする。 '

続いて測位器 1 5は、測角器 1 3によって算出された仰角 φ 1及び 2と、固定発信器 2—1及び 2— 2の座標値とを用いて、センサ 5の位置を計算する。図 6は、位置の算出方法の原理を示す図であり、固定発信器 2— 1と 2— 2、移動体 4及びセンサ 5の位置関係を異なる角度から見た 2つの #1見図から成っている。図において、円錐 1 6は、固定発信器 2— 1を頂点とする円錐である。円錐 1 6の頂点を ® する平面で円錐 1 6を切断すると、その切断面の形状は 2等 32H角形となるが、その 2等辺三角形の底角は仰角 Φ 1に等しい。また円 1 7は、円錐 1 6の底面の円周をなす円である。

同じように、円錐 1 8は、固定発信器 2— 2を頂点とする円錐である。円錐 1 8の頂点を通過する平面で円錐 1 8を切断すると、その切断面の形状は 2等辺三角形となるが、その 2等角形の底角は仰角 φ 2に等しい。また円 1 9は、円錐 1 8の円周をなす円である。ここで、円 1 7と円 1 9は同一平面内に ¾Tるものであり、さらにアンテナ 5の座標は円 1 7と円 1 9との 2つの交点のいずれかに一致する。円 1 7 と円 1 9とがる平面と基準水平面とがなす角（円 1 7と円 1 9と力j^¾fする平面の ¾泉ベクトルと^水平面の ¾ϋベクトルとがなす角、としてもよい）は、センサ 5の傾きを表す角である。仮に、センサ 5の傾きが 0でない; tj^であっても、仰角 Φ 1と Φ 2をその傾きの分だけ補正すればよいので、ここでは説明を簡単にするために、センサ 5の傾きが 0であるものとする。つまり、円 1 7と円 1 9とがする平面と,水平面とは TOである。

また測角器 1 3は、高速フーリエ変換などの周波翻晰を行うことで、固定発信器 2— 1と 2— 2の放射する電波の周波数をも求めている。そこで、測位器 1 5は、この周波数をキーに、発信器 I青報記慮器 1 4が己憶している固定発信器 2— 1と 2— 2 の座標値を^ ^する。このようにして取得した固定発信器 2—1の座標を（Xl， Y 1, Z 1) とし、固信器 2— 2の座標を（X2, Y2, Z2) とする。センサ 5 の座標を (x, y, z) とすれば、 (x, y， z) と（XI, Yl， Ζ 1)、 (Χ2, Υ 2， Ζ2)、 Φ 1、 φ 2との間には式 (2) 及び式 (3) が航する。

tan φΐ fc^ = ( -Z2)² ₊₍₎ -72)

tan φ2

このうち、 zは、移動体 4の大きさと形状、センサ 5の取り付け位置から求めることができるから、結果として式（2) と式（3) は Xと yについての連 It^程式と見なすことができる。この連程式は、 2個の未知数 2個と 2個の式を有しているから、それぞれの未知数について解くことができる。ただし、式（2) と式（3) は 2 程式であるから、角科斤的にこれらの式を Xと yについて解こうとすると、演算負荷が増大してしまう。

そこで、実施の形態 1では、逐次近似法 (successive approximations) によって、この連 ϊ£ ^程式の解を求めることとする。逐次近似法では、初めに Xと yの暫定解を任意に選んでおき、この暫定解を式 (4) によって表される線形近似式に ¾Aして、厶と八とを算出する。続いて、 Xと yの暫と Δχと Ayとから、次の暫定解を求める、という操作を繰り返すものである。これらの操作は、 Δχと厶 yとが、一定の誤差範囲に含まれるようになるまで繰り返される。

式（4) において、避亍列の各成分は式 (2) と式（3) 力算出される。例えば φ 1を Xで微分する成分については、式 (2) の両辺を Xで扮することで

= ÷( -Xl)²+÷(y-71)² (5) dx tan² φΐ dx

両辺に合成関数の微分公式を適用すると、

dx δφΐ I tan φι )

となり、結果として

となる。同様にして、

4( -71) sin³ 1

(8)

(z -Zl)² cos 1 δφ2 _ 4( -Z2)sin³ 2

(9)

dx ( -Z2)²cos 2

である。なお以降、 A, B, C， Dをそれぞれ

とする。そうすると、 A、 B、 C、 D«それぞ、れ式 (7) (10) によって求めることができ、さらに式（4) は、

ΑΔχ + BAy = Αφΐ

(12)

CAx+DAy = Αφ2

という 2 立 1 程式となる。

図 7は、測角器 15における逐次近似法に基づく処理のフローチャートである。図のステツプ S T 101において、 Xと yの暫定解： 0及び y 0に所定の初期値を設定する。この初期値はどのような値であってもよい。続くステップ ST 102において、 2¾¾1次式（12) を吐き出し法によって解き、 Δχと Δγとを求める。 S T103において、 Δχと厶 y力 ψ万定値以下かどうかを^ Τる。例えばこの位置測定システムの測定誤差を 1 0 c mと定めた場合、ステップ S T 1 0 3における所定値は 1 0 c mが翻される。そして Δ χと A yとが定値以下にはならない: は、ステツプ S T 1 0 4に進む（ステップ S T 1 0 3 : N o)。ステップ S T 1 0 4では、暫定解 X 0に△ Xを加えた値 X 0 +厶 Xを新たな觀解 X 0とし、暫定解 y 0に Δ yを加えた値 y 0 + A yを新たな暫定解 y 0とする。そして、ステップ S T 1 0 2に戻り、測角器 1 5は、次の Δ Xと△ yとを算出する。ステツプ S T 1 0 3において、再び算出された Δ χと A yについても、所定値以下かどうかを判^ Tる。このような処理を数回繰り返して Δ Xと Δ yの双方が定値以下となった場合に、逐次近似法を終了する（ステップ S T 1 0 3： Y e s )。

以上より明らかなように、この位置測定システムは、わずか 2つの固信器からの電波の仰角のみを算出し、それらの仰角と電波の波源である固定発信器の位置とに基づいて位置を決^ る。このようにわずかな個数の固定発信器さえあれば、位置を測定できるので、位置測定システムの設置に要する設備投資を低減できる。

さらに、例えば倉庫内の狭い通路の両脇に、貨物をうず高く積み上げているために、天井に施設した"^の固定発信器からしか電波を傲 Πできない齡であっても、わずか 2つの固定発信器からの電波を検出するだけ動体 4の位置を特^ る。したがつて、倉庫内のフォークリフトの位置を特定し、所定の貨物の位置とフォークリフトを適切に誘導するシステムに応用することができる。

なお、この例では固錢信器 2を天井に言耀し、移動体 4の上から電波を放射する構成としたが、このような構成に限られるわけではなぐ例えば固定発信器 2を床 3 の床面や床下に設置してもよいし、さらに通路の側壁に言耀してもよいし、側壁のないエリアでは、支柱を立てて固定発信器を支柱の上に設置するようにしてもよい。したが、つて天井のない施設、すなわち屋外においても、このシステムを用いることがでさる。

またこの例では、ァンテナ軒 5 aと 5 b «¾水平面に対して垂直をなすように配置され、さらに円 1 7と円 1 9とが »水平面に対してであることを仮定して位置を算出した。しかし、円 1 7と円 1 9とが纏水平面に対してでなく、力つアンテナ素子 5 aと 5 bとが »水平面に対して垂直でないように配置されている場合であっても、 φ 1と φ 2とを補正することで、（X, y, z)と（X l， Y l， Z 1 )、 (Χ 2, Υ 2 , Ζ 2) との関係は式 (2) と式（3) に帰着する。したがつてこの実施の形態の位置測法は、基準水平面に ¥ί亍であるにのみ適用が Ρ跪されるものではない。さらに、この例では床面 3を基準水平面としているが、他の面を纏 7j 平面としてもよい。実施の形態 2.

実施の形態 1においては、基準水平面からセンサまでの距離 z、あるいは固定発信器 2とセンサまでの距離の z成分である Z 1— z若しくは Z 2— zが既知である（予め定められている）ことを謝是として位置を測定する方法について説明した。しかし、床面 3が整地されていないために、 z成分が |¾ロとはならない状況も考えられる。この発明の実施の形態 2においては、移動体 4の挪水平面からの高さやセンサとの z 成分が未知であっても、移動体 4の位置を測 ¾Tるシステムについて説明する。図 8は、この発明の実施の形態 2による位置測定システムの構成を示す概念図である。図において、図 1と同一の符号を付した構成要素については、実施の形態 1と同様であるので説明を省略する。図に示すように、実施の形態 2における構成では、床面 3に破線 2 0で囲んだ部分のようななだらかな凹凸が^ &するために、移動体 4の基準水平面からの高さが未知となる：^がある。またセンサ 2 1は、固定発信器 2からの電波を検出するセンサである。

図 9は、センサ 2 1の詳細な構成を説明するための斜視図である。図に示すように、センサ 2 1は、絶椽体による配置面 2 2上に、 {本からなる円形の平面軒アンテナ 2 3 a, 2 3 b、 2 3 c、 2 3 d、 2 3 e、 2 3 f 2 3 gを配置したものである。素子アンテナ 2 3 a〜2 3 gには、図ぬフィーダ線を接続してあり、これによつて素子アンテナ 2 3 a〜2 3 gが受信した電波の信号が外部に出力されるようになつている。ここで、素子アンテナ 23 a〜23dは、且のアレーアンテナ（第 1のァレーアンテナ）を構成するものであって、それぞれの中心カ泉分 24aに沿うように配列されており、また隣接する素子ァンテナの中心同士が等間隔になるようになつてレる。同様に、軒アンテナ 23d〜23gも、第 1のアレーアンテナとは別の一組のアレーアンテナを構财るものであって、それぞれの中心が線分 24bに沿うように配列されており、隣接する素子ァンテナの中心同士が等間隔になるようになっている。さらに糸泉分 24 aと線分 24 bとのなす角は π/2である。なお、以降の説明において、素子ァンテナ 23 a〜 23 gに到来する電波の入射方向と線分 24aとがなす角を Φ Xとし、電波の入射方向と線分 24bとがなす角を φ yとする。

なお、この例では、線分 24 aと 24 bとがなす角が TCZ 2であるものとしているが、線分 24 aと線分 24 bとが TOでさえなければ（異なる傾きを有しているということ)、電波の入射方向の算出に用いる直分を得ることができるので、素子アンテナ 23a〜23eの配置形状は、この例の形状に限られるものではない。

図 10は、移動体 4の#¾|図である。図に示すように、この例ではセンサ 21は移動体 4の操縦席上方付近に装着されている。この位置に装着する理由は、天井に装着されている固定発信器 2からの電波を検出しゃすくするためであるが、そのような目的を果たすことができる限りにおいては、センサ 21を他の部位に装着しても構わない。

続いて図 11は、この発明の実施の形態 2の測位装置の構成を示すブロック図である。図において測位装置 25は、センサ 21と信号処理部 26とを備えている。図 9 で示したように、センサ 21は、素子アンテナ 23 a〜23 gをアレー素子として備えている。また信号処理咅 6は、受信器 11 a〜l 1 gと、 AZD変 il 2 a〜 12g、さらに t目』角器 13— 1、 13— 2、発 ί言器' I青幸隨 3陰器 14、御 W立器 15とを備えている。受信器 11 a、 11 b、 AZD変鶴 12 a、 12b,発ィ言器 I青幸艮記 '慮器 14、測位器 15は、図 4においてそれぞれの構成要素と同一の符号を付して示した構成要素と同様であるので、説明を省略する。また、受信器 1 1 c〜l 1 gは、受信器 1 2 aおよび 1 2 bと同様の機會 gを^ Tる受信器であって、受信器 1 2 aはアンテナ 2 3 a、受信器 1 2 bはアンテナ 2 3 b、受信器 1 2 cはアンテナ 2 3 c、受信器 1 2 dはアンテナ 2 3 d、受信器 1 2 eはァンテナ 2 3 e、受 ί言器 1 2 fはアンテナ 2 3 f、受信器 1 2 gはアンテナ 2 3 gのそれぞれの信号を処理するようになっている。 A/D変鶴 1 2 c〜 1 2 gは、 A/D 変腿 1 2 aおよび 1 2 bと同様の機能を^ Tる A/D変擺である。測角器 1 3— 1及び 1 3— 2は、図 4において測角器 1 3として示した構成要素と同様の機能を有する部位である。測角器 1 3— 1は、 AZD変鶴 1 2 a〜l 2 dが出力する受信デイジタル信号に基づいて、電波の到来方向を算出する部位である。また測角器 1 3 - 2は、 A/D変麟 1 2 d〜 1 2 gが出力する受信ディジタル信号に基づいて、電波の至 |J来方向 Φ yを算出する部位である。

次に、この発明の実施の形態 2による位置測定システムの動作について説明する。複数の固定発信器によって放射された電波は、移動体 4のセンサ 2 1に到来し、センサ 2 1のアンテナ 2 3 a〜2 3 gによつてそれぞれ検出される。そしてアンテナ 2 3 a〜2 3 gによって受信された電波は、アナログ受信信号として出力され受信器 1 1 a〜l 1 gによってそれぞ幅された後に、 AZD変鶴 1 2 a〜l 2 gによつてディジ夕ル信号に変換される。続いて測角器 1 3— 1は、アンテナ 2 3 a〜2 3 d が検出した受信信号に基づいて電波の到来方向のうち、角度を算出する。また測角器 1 3— 2は、アンテナ 2 3 d〜2 3 gが検出した受信信号に基づいて電波の到来方向のうち、角度を算出する。

ここで、移動体 4は倉庫などの狭い通路を移動することもある。その： t船には、固定発信器 2からの電波は、通路の壁面や積み上げられた貨物などに反射するために、電波のマルチパス (干渉波) が発生する。一方、アンテナ 2 3 a〜2 3 d及びアンテナ 2 3 d〜2 3 gはそれぞれ等間隔に並んだアレーアンテナを構成している。そこで、測角器 1 3— 1及び 1 3— 2では、空間平均法 (spatial smoothing method) を用いて、干渉波の抑圧を行うこととする。空間平均法の原理を簡単に説明すれば、次のようになる。すなわち、相関のある波の位相関係（相互相関係数の位相）は受信位置によって異なるので、受信点を平 »動させて相関値を求めれば、その平均効果により相互相関値が低下する。そこで、空間平均法では、アレーアンテナのアレー素子を組み合わせてサブァレーを構成し、サブァレー毎の部分的な相関値の平均を求めることによって、干渉波を抑圧するのである。

図 1 2は、空間平均法の原理を説明するための概念図である。図において、固定発信器 2からの直接波 2 7は、入射角 0 sでアンテナ 2 3 a〜2 3 dのそれぞれに入射している。また干渉波 2 8は、入射角 0 cでアンテナ 2 3 a〜2 3 dのそれぞれに入射している。さらに、アンテナ 2 3 aの中心と 2 3 bの中心との距離及びアンテナ 2 3 bの中心と 2' 3 cの中心との距離、アンテナ 2 3 cの中心と 2 3 dの中心との距離はともに dであるものとする。この場合に、アンテナ 2 3 a、 2 3 b、 2 3 c、 2 3 dの受信べクトルをそれぞれ X _x{ t )、 x ₂( t )、 x ₃( t x ₄( t)とすれば、全アレーの受信べクトルは、

X{t)

( 1 3 )

となる。式（1 3) はアレー素«が 4の齡であるが、ァレ一素 ¾：が Mの場合には、

である。アレー素 4の場合、図に示すサブアレー 2 9 - 1 (アンテナ 2 3 aと 2 3 bとの 2個のァレー素子により構成される) 及びサブアレー 2 9— 2 (アンテナ 2 3 aと 2 3 bとの 2個のァレ一素子により構成される）、さらにサブアレー 2 9— 3 (アンテナ 23 cと 23 dとの 2個のアレー素子により構成される）カ铐えられる。以後、サブアレーを構)^るアレーを K (上記の例では K= 2) とし、 Ν=Μ —K+1とすれば、第 ηサブァレ一（ただし ηは Ν以下の自然数）の受信ベクトル X _n(t)は、

^Xn(^t) = [ぶ" (り， " ₊₁ (り，…， (り]— (15)

となる。したがって、第 nサブアレーの相関行列は、 =^„( ^ ( ] ("=1,2，.."A (16)

式（16) において、 E[*]は、平均操作を示している。

各部分相関行列に対する重み付けを z_nとすると、測角器 13—1及び 13— 2は、 N個の部分相関行列の平均（空間平均）をとることによって、式（17) の相関行列を算出する。

ただし、平均操作を行う上では、電力を平均操作前後で一定とすべきであり、さらに R X Xが正定値ェルミ一ト行列であることから、 z _nは実数であつて、式（18) を満たすように選択する。

空間平均法の効果を具体的に示すと、次のようになる。固定発信器 2からの直接波を s(t)、相関性干渉波を c(t)とし、内部雑音を辦見した、第 m^?の受信べクトル（m=l， 2, ···, M) は、式（19) で与えられる。

= s{t) exp [； (0_S)] + c( xp[j _m (θ )} (19) ここで、 _m ( は角度 Sから到来する電波の第 m軒における受信位相であって、式（20) によって与えられる。

このとき、部分相関行列 Rnxxの第（p， q) 成分 R _q (ただし、 p， q= l, 2, …， K) は、式（21) で与えられる。なお P_sは固定発信器 2からの直接波の受信電力であり、 P_cは干渉波の受信電力である。

(21)

式（21) において、 Z_sc(p, q, n)と Z_cs(p， q， n)、 pは、式（22) (24) によって与えられる。 ^z p,q,ⁿ、

2ττά +l (22)

= exp (n + p -l)sin0_s -(n + ¾ -l)sin0_c一 (sin -sin0_c)

λ 2

Z_sc(p,q,n)

2πά +l (23) exp j J(n + -l)sin0_c -(« + — l)sin - (sin0_c -sin^)

λ 2

式（21) の右辺第 1項は、固定発信器 2からの直接波の自己相関項であり、第 2 項は干渉波の自己相関項である。また、第 3項はこれら 2波の相互相関項である。空間平均によって得られる相関行列 Rxxの第 (P， Q)成分 r_pqは、式（17)と式（2 1) より、式（25) として表される。

N 「

+ 2)^z» [p Z_sc(p,q,n) +

(25)

= „ exp j-—(p-q)sin0 + P_c exp ]'—— p-q)s e_c

+ >/尸 || * P *Z 'sc (p,q,n) + ξρΖ '_cs (p,q,n)]

式（25) において、 Z_sc(p, Q)と Z_cs(p, q)、 ξは、式（26) 〜式 (28) によって与えられる。 (sm6_s -sin0_c) (26)

(sin - sin (27)

(28)

式（21) と式（25) を比^ "ると、空間平均によって 2つの電波の相互相関が ξだけ抑圧されていることが分かる。重み付け ζ_ηをうまく選択することで、 ξ = 0 とすること力、きれば、固定発信器 2からの直接波と干渉波の相互相関を完全に抑圧できる。例えば、

のように z。を選べば、 ξは式（30) のようになって、ごく小さな値にまで抑圧できることが ¾ ^早される。

このようにして、彻 j角器 13—1及び 13— 2では、空間平均法による干渉波の抑圧を行った後、 MUS I C (Multiple Signal Classification)アルゴリズムを用いて、電波の到来方向を推定する。図 13は、 MUS I Cアルゴリズムによる測角器 13— 1及び 13— 2における方ィ索処理のフローチヤ一トである。まず、ステツプ S Τ 201において、測角器 13—1及び 13— 2は、受信信号相関行列 Rを計算する。受信信号相関行列 Rは、ここまで説明してきた空間平均法によって算出された Rxxでめる。

続いて、ステップ ST202において、受信信号相関行列 Rの固有値と、それら各固有値に対 iSTる固有べクトルとを計算する。そしてステップ ST 203において、算出された固有値を降順にソートする。以降の説明では、このようにして得られたソ —ト結果配列が！ぃ d₂, …， d_K

であるとする。また、ステップ ST 202

トルを算出しているが、以降の説明においては、固有値， d₂， ···， d_Kに対 il る固有ベクトルをそれぞれ e_l5 e ₂， ···, e_Kとする。

ステップ ST 204において、入射信 ¾:Lを算出する。そのために、測角器 13 —1及び 13— 2は、ソート結果配列のうち、最も小さい要素である d_Kと等しい要素を除外した残りの要素のうち、最小の要素のソート順位（配列の先頭を 1位としたの順位）をとする。すなわち、入射信" ¾Lは、

di≥d₂≥〜≥d_L>d_L+1=d_L+2=〜=d_K

を満たすものである。また、 d_L+1, d_L+2, ···, d_Kはノイズ固有値と呼ばれる。続いて、ステップ ST205において、ノイズ固有値 d_L+い d_L+2, …， d_Kに対る固有ベクトル e_L+1, d_L+2, ···, e_K (これらはノイズ固有べグトルと呼ばれる）のすべてと直交するステアリングべクトル a (のを持つ方位 0を、信号数である L個だけ探索する。具体的には、式（31) で表される方位諮面関数のピーク（F (のが ¾ となるのを L個探す。

Ρβ)—κ 一 (31) ここで得られた方位関数の L個のピークが固信器 2の放射する電波の到来方向となる。このようにして、 MUS I Cアルゴリズムを用いることで、同時に L個の到来方向が算出できるのである。なお一般に、 Uま（サブアレー素子数 K一 1) となるので、所望の数の電波の到来方向を同時に測 ¾Tるには、センサ 21に配置する軒アンテナの数を増加することがましい。

しかしながら、実施の形態 1でも述べたように、所望の電波数よりも少ない素子ァンテナしかないセンサを用いて構成した場合であっても、周波数分割^や時分割方式、符^^割:^を用いることで、多くの固定発信器からの電波の到来方向を測 ¾Τ ること力河能である。

このようにして、複数の固定発信器からの電波について、測角器 13— 1はアンテナ 23a〜 23cの受信べクトルに基づレ ^て φ xを出力し、測角器 13— 2はアンテナ 23 (〜 23 eの受信べクトルに基づいてを出力する。

続いて、測位器 15は、図 14に示すように 3つの固定発信器 2— 1、 2— 2、 2 -3が放射する電波について、測角器 13-1が出力した φ Xと 13— 2の出力した 4>yから、それぞれの仰角値 Φ1、 Φ2、 φ 3を算出する。ここでアンテナ素子 23 a〜 23 eの配置面 22が水平であるとすれば、電波到来方向の仰角 φは、次のように計算される。

また、固定発信器 2— 1、 2— 2、 2— 3の座標値は、それぞれ（XI, Yl, Ζ 1)、 (Χ2, Υ2, Ζ2)、 (Χ3, Υ3, Ζ 3) であることが発信器情報記憶器 14 に記憶されているものとする。センサ 21の位置を（X, y， z) とすれば、これらの座標値及び仰角値 1、 Φ 2、 Φ3との間には次の関係カ¾¾¾する。 ^^ = ( -Zl)² ₊(> - Fl)² (33)

tan 1

tan φ3 実施の形態 2の説明の冒頭で述べたとおり、この例では（X, y， z) のいずれもが未知数であるが、式（33)、式（34)、式（35) の 3つの式からなる連 3 ^程式を解くことで、 x、 y、 zを決 ¾Tることができる。ただし、式（33)、式（34)、式（35) は程式であるから、解斤的にこれらの式を χ、 γ ζについて解こうとすると、寅算負荷が、 ί曽大してしまう。

そこで、実施の形態 2においても実施の形態 1と同様に、測位器 15tt¾次近似法を用いて、 x、 y、 zを求める。具体的には、

とし、式（36) の逆行列の各成分を微分により求めて、 x、 y、 zに所定の初期値を与えることを皮切りに Δχ、 Ay, Δζが一定範囲内の誤差に含まれるまで繰り返し演算を行う。これらの処理は実施の形態 1において、図 7のフローチャートを用いて説明したものと同様であるので、詳細な説明については省略する。以上から明らかなように、実施の形態 2の位置測定システムによれば、挪水平面からの位置あるいは固信器からの距離が未知であっても、 3つの固信器からの電波を検出することにより位置を測 ¾Tることができる。したがって、床面の整地がなされていない倉庫や床面に段差を有する倉庫などでも安定的に位置を測定すること力河能となるのである。

なお、実施の形態 2においても、センサ 2 1が水平に設定されていることを前提として説明したが、センサ 2 1が »7j平面に対して傾いている場合冰平以外の角度をなしている：^) には、その傾きに応じて式（3 2) のとの値を増減してやればよい。したがってセンサ 2 1が水平でなくても、この発明の特徴が失われることはない。

また、この例では、センサ 2 1のような平面アンテナを用いて到来方向を測 ¾fる構成としているが、位置測定に用いる角度はであるので、実施の形態 1で用いたセンサ 5を代わりに用いること力河能であることはいうまでもない。

実施の形態 1で説明したセンサ 5は、説明を簡単にするために 2個の素子アンテナから構成されることとした。しかし、素子アンテナの数を追加して 3個以上の^?ァンテナから構成されるようにすれば、実施の形態 1によるセンサ 5のようなダイポ一ルアンテナでも空間平均法を用いることができるので、センサ 2 1をセンサ 5に置換してもよい。実施の形態 3.

実施の形態 1と 2では、センサが水平であるか、あるいは傾きが既知であることを前提に位置を測 ¾Tる方法について説明した。これに対して、この発明の実施の形態 3では、センサが水平であることが保証されず、さらに傾きが未知であるに、位置を測定するシステムについて説明する。

この例では、実施の形態 2と同一の構成による位置測定システムを用いることとし、移動体 4の^ p7j平面からの高さにつレてであるものとする。ここでセンサ 2 1の傾きを、配置面 22の海泉ベクトル（u， V, w)で表すこととすれば、 u、 v、妹知数となる。しかし、ここではセンサ 21の傾きさえ表現すれば十分であって、泉べクトル（u， v， w) の大きさは特別な意味を持たないので、 ¾泉べクトルの大きさを 1であるとする。すなわち、

の関係にあるものとする。移動体 4の位置を（x， y, z) とすれば、 ζ«|¾ であるが、 Xと y〖妹知数である。したがって、結果として、 Xと y、 u、 vの 4個の未知数がることになる。この 4個の未知数を決 ¾ "るためには、 4つの式からなる連 3¾程式が必要となるから、この例の条件 ¾動体 4の位置を測るには、 4 つの固定発信器が放射した電波を検出する必要があることが分かる。

図 15は、このような 4つの固定発信器からの電波をセンサ 21が検出して位置を測^る^を説明するための概念図である。図において、移動体 4に搭載されたセンサ 21は、固定発信器 2— 1、 2-2, 2— 3、 2— 4からなる 4つの固定発信器からの電波を検出する。また固定発信器 2— 1、 2— 2、 2— 3、 2— 4の座標はそれぞれ (XI, Yl， Z l)、 (Χ2, Υ2, Ζ2)、（Χ3, Υ3, Ζ 3)、 (Χ4, Υ 4， Ζ4) であることが発信器 I青報記憶器 14により記憶されているものとし、また測角器 13— 1及び 13— 2によって、固定発信器 2— 1、 2-2, 2— 3、 2— 4 のそれぞれの電波の到来方向と配置面 22とのなす角が φ 1、 Φ 2、 φ 3、 φ 4であるものとする。 Φ 1、 φ 2、 φ 3、 φ 4は測角器 13— 1と 13— 2とが接的に取得する角度であり、（x， y, z)、 (u, v, w), (XI, Yl, Zl)、 (X2, Y2， Z2)、 (X3, Y3， Z3)、 (X4, Y4, Z4)、 Φ 1、 Φ2、 φ 3、 φ4との間には次の関係式が^Ϊする。

(Z2 -- Λ + (72 -）) v + (Z 2 - z /ϊ- u² -V

cos 2 = (39)

(x一 Z2)² + ( - Y2)² + (ζ -Ζ2)²

(Ζ3一 Χ)Μ + (73 -_y)v + (Ζ3 - z)Vl一 w ²一 v²

V(x- 3)² + (> -73)² ₊(z-Z3)²

(X4一 )M + (Y4 - )v + (Z4- Ζ)Λ/Ϊ u 2 -v 2

cos 4 = (41)

(x一 Z4)² + 0 - 74)² + (z - Z4Y

式（38)、式（39)、式（40)、式（41)から、 x、 y、 u、 vを解いてもよい。しかしここでは、蕭立器 15力、実施の形態 1または実施の形態 2と同様に逐次近似法によって x、 y、 u、 vを算出する。具体的には、

、

として、式 (42) の^ ίΐ列の各成分を式 (38) 〜式 (41) を x、 y、 u、 vについて稗扮することで求め、 x、 y、 u、 vに所定の初期値を与えることを皮切りに厶 x、 Ay、 Δ ιι、 Δ νが一定範囲内の誤差に含まれるまで繰り返し演算を行う。これらの処理は実施の形態 1において、図 7のフローチャートを用いて説明したものと同様であるので、詳細な説明については省略する。

以上から明らかなように、実施の形態 3の位置測定システムによれば、センサの傾きが、未知であつても、 4つの固定発信器からの電波を検出することにより位置を測定することができる。実施の形態 4.

実施の形態 3では、扉 ζΚ平面からの高さであるが、センサの傾きが未知であるという条件の下に、位置を測るシステムについて説明した。しかし、基準水平面からの高さとセンサの傾きとがともに未知である齢も考えられる。そこで、実施の形態 4による位置測定システムでは、このような条件下であっても位置を測 ¾T ること力河能であり、さらにより良好な条件では実施の形態 1〜 3の測法を選択して位置を測定するシステムについて説明する。

この発明の実施の形態 4の位置測定システムの構成は、実施の形態 2と同様に図 8 によって示される。またセンサの構成は図 9によって示される。図 8及び図 9において、実施の形態 2と同一の符号を付した構成要素については実施の形態 2と同様であるので説明を省略する。

図 1 6は、この発明の実施の形態 4の測位装置の構成を示すプロック図である。実施の形態 2における測位装置の構成を示すプロック図である図 1 0と比して、この図で謝たに電波数算出器 3 1が設けられている点が異なっている。

電波體出器 3 1は、測角器 1 3— 1及び 1 3— 2が算出する角度の個数、すなわち、センサ 2 l eが受信する電波の個数に応じて、位置測法を選択する信号を測位器 1 5に出力する部位である。位置情報記憶器 3 2は、測位器 1 5が算出した前回の位置および傾きを記慮する^?又は回路である。位置情報記憶器 3 2は、測位装置 2 5が電源 ¾λ時に前回の測定値に基づいて円滑に測理を開始できるようにするために、電源断によっても記憶が失われないような不揮発性記憶装置を握することが望ましい。

この発明の実施の形態 4における測角器 1 3— 1と 1 3— 2は、アンテナ 2 3 a〜 2 3 e力の受信べクトルに基づいて電波の到来方向を算出する点において実施の形態 2と同様であるが、電波数算出器 3 1〖こ対しても電波の到来方向を出力するようになっている。そして測位器 1 5は、電波数算出器 3 1が出力する位置測定方法を選択する信号に基づいて、位置と傾きの算出に用いる電波の到来方向と固定発信器の位置とを最適に組み合わせて位置と傾きを算出するになっている。また電波の受 1え態が悪いために、必要な電波を受信できない場合には、位置情報記憶器 3 2が己憶している前回の位置と傾きとを補助的な情報として用いるようにもなつている。その他、図 1 0と同一の符号を付した他の構成要素については実施の形態 2と同様であるので、説明を省略する。

続いて、この発明の実施の形態 4の位置測定システムの動作について説明する。この位置測定システムにおいても、実施の形態 2と同様に、アンテナ 2 3 a〜 2 3 eが固定発信器 2の放射した電波を受信して受信ベクトルを出力する。そして、測角器 1

3— 1は、、その受信べクトルに基づいて固定発信器 2の放射した電波の到来方向のうち、角度 φ χを計算し、測角器 1 3— 2が固定発信器 2の放射した電波の到来方向のうち、角度を計算する。

ただし測角器 1 3 - 1及び 1 3— 2は、受信べクトルの電力や振幅が^ f定値に達しない塲合には角度を算出しない。このようにすることで、反射波の影響を小さくすることができるのである。

測角器 1 3—1及び 1 3— 2が計算した角度 φ χ及び φ γは、測位器 1 5に出力されるとともに、電波露出器 3 1にも出力される。電波数算出器 3 1は、測角器 1 3 — 1と 1 3— 2が計算した角度の個数に基づいて、位置測法を選択する信号を出力する。なお、測角器 1 3— 1と 1 3— 2とが計算した角度の個数が食い違うには、小さい方を採用してもよい。

電波購出器 3 1が出力する信号は、測角器 1 3— 1及び 1 3— 2が出力した角度の個数に対 JiTTるもので、例えば角度の個数そのもの（2， 3 , 4， 5など）を信号の値として出力する。以後、電波難出器 3 1が角度の個数（受信可能な電波の個数) を信号値として出力するものとして説明を行う。

測位器 1 5は、電波難出器 3 1が出力した信号に基づいて位置と傾きとを算出する。図 1 7は、測位器 1 5が行う位置測理のフローチャートである。まずステツプ S T 3 0 1において測位器 1 5は、電波数算出器 3 1から信号値を取得する。信号値はすでに述べたように、受信可能な電波の個数を反映したものであって、以後、測位器 1 5内では電波の個数に基づいて処理を行う。

ステップ S T 3 0 2において、測位器 1 5は電波の個数が 2に等しいかどうか判定する。電波の個数が 2に等しい場合は、ステップ S T 3 0 3に進む（ステップ S T 3 0 2： Y e s )₀等しくない 8·はステップ S T 3 0 6に進むが、この;^については ^することとする。

ステップ S Τ 3 0 3において、測位器 1 5はセンサ 2 1の位置と傾きの成分のうち、 z、 u、 Vが未知かどうかを判定する。まず初期状態（電源を投入した直^) においては、 z、 u、 Vは不定な牝態となつているので、未知であると判断する。またすでに位置測寅算を数回行っているも、碰する逐次近似法の繰り返し処理を所定回数以上行っても、 Δ ζ、 Δ ιι、 Δ νが一定の誤差範囲に含まれない場合には、未知であると判断するようにしてもよい。

z、 u、 vが未知であると判断した齢には、前回の測定値を用いて今回の z、 u、 vを推測する処理を行う（ステップ S T 3 0 3： Y e s )。 z、 u、 vを推測する処理の例としては、 uと Vには前回の値を採用し、 zについては前回測定の zと経過時間 Δ tと図^^ぬ 3igセンサから得た避 Vから算出する。例えば前回簡定時の zを z *とすれば、 z = z (43)

11と vについては前回値を用い、 zについては前回値とは異なる値を式（43) で用いることとしたのは、 uと Vはセンサの傾きであるから時間的変化が比較的緩 I曼であるのに対し、 zは位置であるから移動体 4が移動する以上時々刻々と変化する可能性があるからである。

また、 z、 u、 vが未知でないと判断した場合には、直接ステップ ST 305に進む。

次に、ステップ ST305において、式（4) 〜式（10) を用いて逐次近似法によって Xと yとを算出する。以後、ステップ ST301に戻り、次の信号値を処理する。

一方、電波の個数が 2に等しくない場合は、ステップ ST306に進む（ステップ ST302： No)oそしてステップ ST 306において、電波の個数が 3に等しいかどうかを判 ¾Tる。電波の個数が 3に等しい齢は、ステップ ST307に進む（ステツプ S Τ 306： Y e s )。また 3に等しくない場合は、ステツプ S T 310に進むが、この: については観する。

ステップ ST307において、 u、 Vが未知かどうかを調べる。この処理はステツプ S T 303の処理と同様であるので詳細な説明については省略する。その結果、 u、 Vが未知であると判断した場合は、ステップ S T 308に進み、ステップ S T 304 と同様に uと Vを推測する処理を行い（ステツプ S T 308： Y e s )、その後にステップ S T 309に進む。 uと Vとが未知でないと判断したには、直接ステツプ S T309に進む（ステップ ST 307： No)。

ステップ ST309においては、式（33) 〜式（36) を用いて、 x、 y、 zを算出する。この場合においても、ステップ ST305と同様に逐次近似法によって、 x、 y、 zを算出する。また、電波の個数が 3に等しくない驗は、ステップ ST310において、電波の個数が 4に等しいかどうかを判定し、 4に等しい場合は、ステップ ST311に進む (ステップ ST310： Ye s また 4に等しくない場合はステップ ST 314に進むが、このについては難する。

ステップ ST311において、 zが未知かどうかを判定する。この処理はステップ ST303と同様であるので、詳細な説明については省略する。その結果、 zが未知であると判断した:^には、ステップ ST 312に進み（ステップ ST311 ： Ye s)、例えば式（43)を用いて zを推測する。また zが未知でないと判断した場合（ステツプ ST311： No),あるいはステップ ST312の後、ステップ ST313において、式（38)〜式 (42) を用いて、 x、 y、 u、 vを算出する。このも、逐次近似法を用いて算出することが望ましい。

電波の個数が 4に等しくない場合は、ステツプ S T 314に進み、電波の個数が 5 以上かどうかを判 ¾ "る。電波の個数が 5以上の場合は、ステップ ST315に進んで x、 y、 z、 u、 vを算出する。図 18は、このような場合の位置の算出原理を説明するための概念図である。図において、固定発信器 2— 1、 2— 2、 2— 3、 2— 4、 2— 5は、その放射した電波を移動体 4に搭載したセンサ 21力 ¾出可能な位置にある固定発信器である。図に示すように、固定発信器 2— 1、 2— 2、 2— 3、 2 —4、 2— 5の座標はそれぞれ (XI, Yl, Z 1)、 (X2, Y2, Z2)、 (X3, Y3, Z 3)、 (X4, Υ4， Ζ4)、（Χ5， Υ5， Ζ 5) とし、固定発信器 2— 1、 2— 2、 2— 3、 2— 4、 2— 5が放射する電波の到来方向の仰角をそれぞれ φ 1、 Φ2、 Φ3、 Φ4、 Ψ5とする。そうすると、（Xl， Yl, Ζ1)、 (Χ2, Υ2, Ζ 2)、 (Χ3, Υ3， Ζ 3)、 (Χ4, Υ4, Ζ4)、 (Χ5, Υ5， Ζ 5)、 φ 1、 φ2、 Φ3、 Φ4、 φ 5、 χ、 y、 ζ、 u、 νとの間には、式（44) から式（48) までの関係が敝する。 (XI - x)u + (71- y)v + (Zl -z)Vl -u^l -v² cos 01 = (44)

V( -zi)² + ( -n)² + (z-zi)²

(X2 - x)u +(Y2-y)v + (Z2- Ζ)Λ[Ι -U² - V

(45) {~χ-Χ2)¹ +{y-Y2f + {z-Z2f

(X5- x)u + (75 - )v + (Z5 - -w² -v²

cos = ^ , ^y , (48)

^(x-X5)² ₊ (y-Y5)² +(z-Z5)²

これらの 5つの式を x、 y、 z、 u、 vについて直接的に解いてもよいが、ここでも実施の形態 1と同じように逐次近似法によって、連程式 (44) 〜（48) の解を求めることとする。具体的には、

とおいて、式 (4 4)〜式（4 8) を x、 y、 z、 u、 vで ί救分することにより式（4 9) の逆行列の各成分を求める。以降の処理は、実施の形態 1などと同様であるので詳細な説明については省略することとする。

以上から明らかなように、この発明の実施の形態 5の位置測定システムによれば、センサ 2 1が受信できる電波の数に応じて位置を計算する方法を適切に選択することとしたので、さまざまな局面で位置測定を行うことができる。，また、センサの高さと傾きが未知であっても、 5個の固定発信器が放射した電波を取得して、センサの位置と傾きとを測 ¾Τるので、床面に傾斜がある i t^段差のある環境など、センサの位置や傾きが不定となりやすい状況でも位置の測定を行うことカ坷能である。

なお、 z、 u、 Vが未知であるか否かを判断する例として、初期状態及び次近似法における誤差の «状況に基づいて判断する方法について説明したが、この他にも、 x、 y、 z、 u、 vについて追尾処理（前回の測定結果から今回の測定結果を予測する処理）を行い、予測値と測定値とが一定値以上 »したに、測定値が未知である、と判断するようにしてもよい。

また、この例では、 5個を超える電波が得られる場合であっても、 5個のみの電波を用いて位置測定することとしている。しかし、受信可能な固定発信器の数が 6偭以上のに、式（5 0 )によって平均二乗誤差 MS ECMean Square Error)を算出し、最小二乗法によってこの平均二乗誤差を最小にするように位置を決定するようにしてもよい。これによつて、より精度の高い位置測定結果を求めることができる。

MSE = ( 5 0 )

なお、式（5 0 )において、固定発信器の個数を Nとし、各固定発信器の座標を（X n， Yn, Zn)、各固定発信器から受信した電波の仰角をそれぞれ Φηとしている。さらに、式（5 0 ) を用いてより精度の高い位置測定結果を得る方法は、 3個から 5個の固定発信器からの電波が得られる: ^にあっても適用すること力きる。例えばセンサ 2 1の高さと傾きが既知であり、 2個の固定発信器からの電波が得られれば、位置を測 ¾Τるのに十分な場合に、 3個の固定発信器からの電波が得られたものとする。このに、 3個の固定発信器からの電波の到来方向と、それぞれの固定発信器の位置に基づいて、式（5 0 ) で算出した M S Εを最小にするようにセンサ 2 1の位置を決 ¾Tるのである。こうすれば、 3個の固定発信器から得られた情報を有効に活用し、測定精度の向上に活かすことができる。実施の形態 5 .

実施の形態 2においては、図 9に示したようなセンサ 2 1を用いて、式（3 2 ) を用いて仰角のみを算出し、一次元の角度のみから位置を測定したが、センサ 2 1は複数の方向のァレ一素子列を有しているので、 Φ Xと Φ yという異なる次元の角度を算出することカ坷能である。そこで、このような 2 の角度をそのまま用いて仰角の他に水平相対角を算出するようにし、 τΚ平相対角と仰角とを組み合わせて位置を測定するようにしてもよい。この発明の実施の形態 5による位置測定システムは、このような特徴を有するものである。

なお、実施の形態 5の位置測定システムが位置測定を行う謝是として、センサの傾きは «Iである一方で、センサの基準水平面からの高さ（z成分）知であるものとする。

実施の形態 5の位置測定システムの構成は実施の形態 2と同様に図 8によって示される。そして実施の形態 2と同様に図 9に示したようなセンサ 2 1を備えるものとし、測位装置の構成は、図 1 1によって表されるものとする。

次に、実施の形態 5による位置測定システムの動作について説明する。この位置測定システムにおいても、実施の形態 2と同様に測角器 1 3 - 1と測角器 1 3— 2によつて各固定発信器が放射した電波の到来方向について、 Φ X及び Φ yとを出力する。根 IJ位器 1 5は、式（3 2) によって、各固定発信器が放射した電波の到来方向の仰角を算出する。また式（5 1 ) によって、 2つの固定発信器の水平方向相対角 0 _{1 2}を算出する。

式（ 5 1 ) において、 2つの固定発信器を固定発信器 2一 1と固定発信器 2— 2とする。また固定発信器 2— 1から到来した電波について測角器 1 3— 1が算出した角度を Φ _χい測角器 1 3— 2が算出した角度を φ _{ν 1}とする。さらに固定発信器 2— 2から到来した電波について測角器 1 3— 1が算出した角度を φ_{χ 2}、測角器 1 3— 2が算出した角度を _{y 2}とする。

続いて、測位器 1 5は、式（3 3)、式（3 4) と式（5 2) を用いて、 x、 y、 z を決 ¾Tる。 cose₁₂ = (^ (^2)Hy-YD(y-Y2) _{(5 2)}

- l)² + ( -Yl)²}{(x-X2)² + (y -Y2)²} なお、 x、 y、 zの決定にあたっては、他の実施の形態と同じように逐次近似法を用いるようにしてもよい。

以上から明らかなように、この発明の実施の形態 5の位置測定システムによれば、高さが未知であっても 2つの固定発信器からの電波さえ受信すれば、位置を測 ¾Tることができる。

なお、この例では、 zのみ未知であるとして位置測定の方法を述べたが、他にが ^で u、 V (センサの傾き）が未知である場合、あるいは z及び u、 Vが未知であるについても式（52) を組み合わせれば、必要とされる固定発信器の個数を 1 減ずることができるのである。産業上の利用の可能性

以上のように、この発明に係る位置測定システムは、固定発信器の放射する電波に基づいて位置を測る用途に有用である。

Claims

請求の範囲

1. 複数の固定発信器が放射した電波を受るセンサと、

前記固定発信器から編己センサが受信した電波に基づいて、その電波の嫌 3センサへの到来方向を算出する測角器と、

編己複数の固定発信器の位置を記憶する発信器位置記憶器と、

嫌3発信器位置記憶器が ¾憶する前記固定発信器の位置と前記電波の到来方向とに基づいて自位置を算出する測位器と、を備えた測位装置において、

tots則角器は、嫌己電波の一^到来方向を算出し、

嫌目』位器は、編己センサの位置と姿勢との座標成分のうち未知なる成分の数と同数の嫌己固定発信器の位置と、それらの固定発信器が放射した電波の一到来方向と、を用いて自位置を算出することを特徴とする測位装置。

2. 請求の範囲第 1項に記載の測位装置において、

前言則位器は、既知なる嫌 3センサの高さと姿勢と、 2個の鎌 3固定発信器の位置と、それらの固定発信器が放射した電波の一^到来方向と、を用いて自位置を算出することをとする視立装置。

3. 請求の範囲第 1項に記載の測位装置において、

Ι ΙΒΜ立器は、既知なる鎌 3センサの姿勢と、 3個の嫌 3固定発信器の位置と、それらの固定発信器が放射した電波の一^到来方向と、を用いて自位置を算出することを特徴とする測位装置。

4. 請求の範囲第 1項に記載の測位装置において、

嫌則位器は、賺 [1なる嫌己センサの高さと、 4個の編己固定発信器の位置と、それらの固信器が放射した電波の一到来方向と、を用いて自位置を算出することを特徴とする視 U位装置。

5. 請求の範囲第 1項に記載の測位装置において、

嫌則位器は、 5個の觸己固信器の位置と、それらの固錢信器が放射した電波の一^ ¾到来方向と、を用いて自位置を算出することを特徴とする測位装置。

6. 請求の範囲第 1項に記載の測位装置において、

前記センサが受信する電波を放射した鎌 3固信器の個数を計算し、その個数を MtHM立器に出力する電波数算出器をさらに備え、

鎌 B?則位器は、嫌 3個数が 2個の場合に、嫩 [Iなる前記センサの高さと姿勢と、 2 個の嫌己固定発信器の位置と、それらの固定発信器が放射した電波の一 ^到来方向と、を用いて自位置を算出し、嫌顧数が 3個の:^に、雕 [Iなる嫌 3センサの姿勢と、 3個の前記固定発信器の位置と、それらの固定発信器が放射した電波の一^ ΰ到来方向と、を用いて自位置を算出し、嫌己個数が 4個の場合に、職 Πなる嫌 3センサの高さと、 4個の觸己固定発 ί言器の位置と、それらの固定発信器が放射した電波の一 ^到来方向と、を用いて自位置を算出し、嫌己個数が 5個以上の場合に、 5個の前記固定発信器の位置と、それらの固定発信器が方谢した電波の一次元到来方向と、を用いて自位置を算出することを特徴とする測位装置。

7. 請求の範囲第 6項に記載の測位装置において、

前回の自位置の算出に用いた前記センサの高さ及 ϋ¾勢を記憶する位置情報記憶器を備え、

鎌 1涖器は、嫌 3個数が 2個の: ^に、嫌己センサの高さと姿勢の何れかが未知である場合に、前記位置情報記憶器カ憶する鎌己センサの位置及び姿勢を用いて自位置を算出することをとする視 W立装置。

8. 請求の範囲第 6項に記載の測位装置において、前回の自位置の算出に用いた嫌己センサの高さを記憶する位置情報記憶器を備え、嫌則位器は、嫌 3個数が 3個の場合に、前記センサの高さが未知である場合に、鎌己位置情報記憶器が己憶する嫌己センサの位置を用いて自位置を算出することを特徵とする測位装置。

9. 請求の範囲第 6項に記載の測位装置において、

前回の自位置の算出に用いた前記センサの姿勢を記憶する位置情報記憶器を備え、鎌 ffi則位器は、前記個数が 4個の場合に、嫌己センサの姿勢が未知である場合に、前記位置情報記憶器が憶する前記センサの姿勢を用いて自位置を算出することを特徵とする測位装置。

1 0. 請求の範囲第 1項に記載の測位装置において、

嫌立器は、嫌 3未知なる成分を未知数とし前記固定発信器の位置とそれらの固定発信器が放射した電波の一次元到来方向との間に成立する連立方程式から、逐次近似法を用いて未知数を決定し、自位置を算出することを特徴とする測位装置。

1 1 . 請求の範囲第 1項に記載の測位装置において、

嫌己センサは、直線状に配列された素子アンテナからなる第 1のアレーアンテナと、この第 1のアレーアンテナの配列方向とは異なる方向を有する直線に沿って配列された軒アンテナからなる第 2のアレーアンテナとを備え、

前言則角器は、前記第 1のアレーアンテナが受信した電波からその電波の到来方向を求めるとともに、前記第 2のァレーアンテナが受信した電波からその電波の到来方向を求め、前記第 1のァレーアンテナから求めた電波と前記第 2のァレーアンテナから求めた電波とから嫌己電波の到来方向を算出することを特徴とする測位装置。

1 2. 請求の範囲第 1 1項に記載の測位装置において、嫌 Hi則角器は、嫌 3各ァレーアンテナの各ァレー^?から複数のサブァレーを構成し、それらのサブアレー毎に相関行列を求めるとともに、それらの相関行列の各成分の平均を求めることによって、前記各アレーアンテナの相関行列を算出し、この相関行列から謙己電波の到来方向を算出することを糊敷とする測位装置。

1 3. 複数の固定発信器が放射した電波を受信するセンサと、

前記固定発信器から觸己センサが受信した電波に基づいて、その電波の觸己センサへの到来方向を算出する測角器と、

嫌 3複数の固定発信器の位置を記憶する発信器位置記憶器と、

嫌 3発信器位置記憶器が記憶する前記固定発信器の位置と前記電波の到来方向とに基づいて自位置を算出する測位器と、を備えた測位装置において、

鎌 ffi 角器は、前記各電波の到来方向の仰角と前記各電波の到来方向間の⁵ Ρί亍相対角とを算出し、

ffftffi 位器は、前記センサの位置と姿勢との座標成分のうち未知なる成分の数より 1少ない数の前記固定発信器の位置と、嫌 Βί則角器が算出した仰角と ¥ί亍相対角と、を用いて自位置を算出することを特徴とする彻立装置。

' 1 4. 請求の範囲第 1 3項に記載の測位装置において、

鎌己センサは、直線状に配列された^?アンテナからなる第 1のァレ一アンテナと、この第 1のアレーアンテナの配列方向とは異なる方向を有する直線に沿って配列された素子ァンテナからなる第 2のァレーアンテナとを備え、

ΙίίΙΒί則角器は、 tiifS第 1のァレーアンテナが受信した電波からその電波の到来方向を求めるとともに、嫌己第 2のアレーアンテナが受信した電波からその電波の到来方向を求め、嫌己第 1のアレーアンテナから求めた電波と鎌 3第 2のアレーアンテナから求めた電波とから嫌己電波の到来方向の仰角を算出し、嫌 3各電波の到来方向間の相対 TO角とを算出することを特徴とする測位装置。