JPH09273932A - 移動体の空間位置測定装置 - Google Patents

Abstract

(57)【要約】 【課題】 ３元空間を移動する運動体の空間位置（経緯
高度）を測定する装置。 【解決手段】 ３元空間を移動するＸＹＺ座標体の方
位、姿勢に関するＳ、Ｍ、Ｆ３角パラメータを座標体３
軸にもつ重力センサ、磁束センサにより算出し、続い
て、該Ｓ、Ｍ、Ｆを用いて移動体のＶベクトルを地球座
標系の３元Ｖ１、Ｖ２、Ｖ３に分解し、これにより移動
体の空間３元位置、経緯高度を算出し、又、運動行跡を
記録する３元空間位置測定装置。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】 測量機器、観測機器、ナビ
ゲーション機器等。

【０００２】

【従来の技術】 従来の走行体の経緯高度の空間位置測
定装置は、３元加速度積分型の空中飛行体、又は海上海
中航行の大型高速移動体に適応するものにして、本発明
の目標とする陸上を移動する車両、人体等の小型、低速
の移動体の車輪回転、歩程等の実量移動速度の積算によ
る、経緯高度測定を測定する簡易型空間位置測定装置
は、いまだ開発されていない。尚本発明に関連する従来
の技術には次の２件がある。 （１）座標変換装置 特願平６−１１０６０５ （２）傾斜面方位測定装置 特願平７−１２８７２４

【０００３】

【発明が解決しようとする課題】 車両、人体等陸上移
動体が傾斜計、磁束計測計をもち起伏する傾斜面をもつ
陸上走行車両（車輪回転数）、人体（歩数）を積算し
て、地上３元（経緯高度）に分解し３元位置を算出す
る。

【０００４】

【課題を解決するための手段】 移動体（ＸＹＺ座標）
は３軸に傾斜計、磁束計をもち、はじめにこれによりＸ
ＹＺ座標体の空間に対する方位姿勢に関する情報のパラ
メータＳ、Ｍ、Ｆを算出し続いて該Ｓ、Ｍ、Ｆを用いて
ＸＹＺ座標体、Ｘ軸の速度ベクトル（車輪回転数、歩
数）を経緯高度の３軸に分解し、これを積分して、経緯
高度を積算表示する。

【０００５】

【発明の実施の形態】 本発明の移動体の空間位置測定
装置は計測部、コンピュータ部、表示及び記憶格納部よ
り構成されている。

【０００６】

【実施例】

【０００７】（計測部） 計測部は、重力検出センサ及
び磁束検出センサよりなっている。

【０００８】図１は本発明の一実施例による計測部に用
いる重力検出センサの例を示す一部断面図である。図に
おいて、ＯＳＣは高周波電源、Ｃ１及びＣ２はコンデン
サ、Ｄ１及びＤ２はダイオード、Ｌ１及びＬ２はそれぞ
れダストコアを芯に持つコイル、Ｓは円形ダイアフラ
ム、ｍは例えば真鍮製の重り、１はケース、２及び３は
出力端子である。重りｍはダイアフラムＳの中心に設け
られ、ダイアフラムＳの周辺はケース１の円壁に支持固
定される。コイルＬ１及びＬ２は、高周波電源ＯＳＣを
持つブリッジ回路の隣接する一対のインダクタンス・ア
ームを構成する。これら２つのインダクタンスに対向す
るブリッジ・アームに、高周波電源ＯＳＣからみて単方
向性をもつ、２つの整流器Ｄ１及びＤ２が接続される。

【０００９】このような構成において、ダイアフラムＳ
の重りｍが中心線Ｚ方向の力Ｐを受けて変位すると各コ
イルＬ１及びＬ２と重りｍの真鍮金属面との間の間隙に
差を生じ、コイルＬ１及びＬ２のインダクタンスに差を
生じて、出力端子２、３間に出力電流を発生する。すな
わち、本例は変位計型のセンサである。この場合、図２
に示すようにセンサの中心線Ｚが重力Ｇの方向と角度φ
をなすときは、Ｇｃｏｓφが荷重Ｐとして重りｍに加わ
るので、出力電流は荷重Ｇｃｏｓφによる間隙変化に対
応するものとなる。

【００１０】図３は、本例の計測部に用いる地磁気検出
センサの例を示す斜視図である。本例は既知のホール素
子型のセンサである。図において、５は半導体型ホール
素子、６−１、６−２、７は電極を示す。一対の電極６
−１、６−２に沿って定電流Ｉを流しておき、主面と直
角の方向に磁束Ｂを加えると、電流Ｉ及び磁束Ｂの両方
に直角な軸方向に電圧ＶＨを発生する。この電圧ＶＨを
電極７より取り出す。この場合ＫＨをホール常数として
次の［数１］の関係が成り立つ。

【００１１】

【数１】ＶＨ＝ＫＨ・ＩＢ

【００１２】よって図４に示すようにホール素子５の直
角軸線Ｚが局所の地球磁場の磁束Ｆの方向となす角度を
φとすると、Ｆｃｏｓφの磁束（上記磁束Ｂに相当す
る）に比例する電圧が得られる。

【００１３】なお、上述においては、重力検出センサと
して高周波変位検出方式の中央に重錘をもつダイアフラ
ム型重力計を、地磁気検出センサとして、ホール素子型
のものを説明したが、必要とする精度と出力が得られる
ものであれば、他の型の重力計、磁束計を使用すること
が出来る。

【００１４】図５は本例の計測部の斜視図である。本例
においては、上述の如き重力センサＰ１、Ｐ２、Ｐ３、
磁束センサＳ１、Ｓ２、Ｓ３を直交３軸移動体Ｘ、Ｙ、
Ｚに、それぞれ１個づつ、各中心線を３軸の軸線に一致
させて取り付ける。この場合、各重力センサよりの出力
はＷ１、Ｗ２、Ｗ３であり、各磁束センサよりの出力は
Ｎ１、Ｎ２、Ｎ３である。

【００１５】コンピュータ部は、移動体（ＸＹＺ座標）
の空間における方位及び姿勢のパラメータを求めるため
の第一段の座標変換手段と、該手段により求められた
Ｓ、Ｍ、Ｆの３パラメータを用いて、移動体の速度ベク
トルを、地球座標系３元座標軸に変換する第二段の座標
変換手段の双方より構成されている。

【００１６】（始めの座標変換手段） 空間の基準指北
水平面垂直線３軸座標系をＸ_０Ｙ_０Ｚ_０とし、実在座標
軸をＸＹＺとする。Ｘ_０Ｙ_０Ｚ_０座標をＺ軸のまわりに
Ｆ角だけ回転した位置にある 直交軸を０−Ｘ’Ｙ’Ｚ’として、０−Ｘ’Ｙ’Ｚ’を
Ｘ’軸のまわりにＭだけ回転した位置にある直交軸を０
−Ｘ”Ｙ”Ｚ”とする。また０−Ｘ”Ｙ”Ｚ”をＺ”軸
のまわりにＳ角だけ回転した位置にある実在座標軸を０
−ＸＹＺとする。空間、実在両座標（Ｘ_０Ｙ_０Ｚ_０）、
（ＸＹＺ）間の関係を変換式［数２］にて示す。

【００１７】

【数２】

【００１８】［数３］に［数２］の展開式を示す。

【００１９】

【数３】

【００２０】［数３］の展開式の逆配列を［数４］に示
す。

【００２１】

【数４】

【００２２】［数５］に［数４］のマトリクスをΔにて
示す。

【００２３】

【数５】

【００２４】［数５］により［数４］を［数６］に置換
する。

【００２５】

【数６】

【００２６】［数６］における、ＸＹＺ座標系には各軸
に重力センサＷ１、Ｗ２、Ｗ３及び磁束センサＮ１、Ｎ
２、Ｎ３をもつ。［数７］及び［数８］にＸＹＺ軸を重
力系、磁束系それぞれ各軸の内容をセンサにより示して
いる。

【００２７】

【数７】

【００２８】

【数８】

【００２９】又この場合には重力系、磁束系それぞれ空
間座標Ｘ_０Ｙ_０Ｚ_０の内容を重力系は重力を１としてＸ
_０（０）、Ｙ_０ （０）、Ｚ_０（１）にて、又磁束系は
Ｈを水平磁束Ｕを垂直磁束としてＸ_０（Ｈ）、Ｙ
_０（０）、Ｚ_０（Ｕ）にて示している。このＧ（１）及
びＴ、Ｕ、Ｈの値は［数９］［数１０］［数１１］［数
１２］によりてＷ１、Ｗ２、Ｗ３、Ｎ１、Ｎ２、Ｎ３に
より算出される。［数１０］のＴは総磁束を示してい
る。

【００３０】

【数９】（ｗ１）^２＋（Ｗ２）^２＋（Ｗ３）^２＝１

【００３１】

【数１０】 （（Ｎ１）^２＋（Ｎ２）^２＋（Ｎ３）^２）^１／２＝Ｔ

【００３２】

【数１１】Ｎ１×Ｗ１＋Ｎ２×Ｗ２＋Ｎ３×Ｗ３＝Ｕ

【００３３】

【数１２】（Ｔ^２−Ｕ^２）^１／２＝Ｈ

【００３４】［数７］の重力系変換式を分解して［数１
３］［数１４］［数１５］にてＸＹＺ各軸重力センサＷ
１、Ｗ２、Ｗ３の内容を、変換パラメータＳ、Ｍの正余
弦値にて示している。

【００３５】

【数１３】Ｗ１＝ｓｉｎＳｓｉｎＭ

【００３６】

【数１４】Ｗ２＝ｃｏｓＳｓｉｎＭ

【００３７】

【数１５】Ｗ３＝ｃｏｓＭ

【００３８】又、［数８］の磁束系変換式を分解して
［数１６］［数１７］［数１８］に各軸Ｎ１、Ｎ２、Ｎ
３の内容をＨ、Ｕ及び変換パラメータＳ、Ｍ、Ｆの正余
弦値を用いて表すことができる。

【００３９】

【数１６】Ｎ１＝Ｈ（ｃｏｓＦｃｏｓＳ−ｓｉｎＦｓｉ
ｎＳｃｏｓＭ）＋ＵｓｉｎＳｓｉｎＭ

【００４０】

【数１７】Ｎ２＝Ｈ（−ｃｏｓＦｃｏｓＳ−ｓｉｎＦｃ
ｏｓＳｃｏｓＭ）＋ＵｃｏｓＳｓｉｎＭ

【００４１】

【数１８】Ｎ３＝ＨｓｉｎＦｓｉｎＭ＋ＵｃｏｓＭ

【００４２】重力系各センサの内容より［数１９］［数
２０］によりてパラメータＳ、Ｍが求められる。

【００４３】

【数１９】Ｓ＝ｔａｎ^−１（Ｗ１／Ｗ２）

【００４４】［数２０］［数２１］により、Ｓ１及びＳ
２が決定される。

【００４５】

【数２０】Ｓ１＝ｓｉｎＳ

【００４６】

【数２１】Ｓ２＝ｃｏｓＳ

【００４７】

【数２２】 Ｍ＝ｔａｎ^−１（（Ｗ１^２＋Ｗ２^２）^１／２／Ｗ３）

【００４８】［数２３］［数２４］によりＭ１、Ｍ２が
決定される。

【００４９】

【数２３】Ｍ１＝ｓｉｎＭ

【００５０】

【数２４】Ｍ２＝ｃｏｓＭ

【００５１】重力センサよりのＳ１、Ｓ２、Ｍ１、Ｍ２
を磁束センサＮ１、Ｎ２、Ｎ３の内容に加減乗除の算法
を行う。即ち［数２５］によりＮ１の内容にＳ１を乗算
し、該乗算値にＮ２の内容とＳ２の乗算値を加算して、
これをＮ４とする。

【００５２】

【数２５】Ｎ４＝Ｎ１×Ｓ１＋Ｎ２×Ｓ２

【００５３】続いて［数２６］によりＮ３の内容にＭ１
を乗算し該乗算値よりＮ４の内容とＭ２の乗算値を減算
し該減算値をＫ１値とする。

【００５４】

【数２６】Ｋ１＝Ｎ３×Ｍ１−Ｎ４×Ｍ２ Ｋ１の内容は［数２７］に示す如く、水平磁束ＨとＦの
正弦値の乗算値となっている。

【００５５】

【数２７】Ｋ１＝ＨｓｉｎＦ

【００５６】一方［数２８］によりＮ１の内容にＳ２値
を乗算し、該値より、Ｎ２の内容にＳ１を乗じた乗算値
を減算して、この出力値をＫ２とする。

【００５７】

【数２８】Ｋ２＝Ｎ１×Ｓ２−Ｎ２×Ｓ１

【００５８】このＫ２の内容は［数２９］に示す如く、
水平磁束ＨとＦの余弦値との乗算値となる。

【００５９】

【数２９】Ｋ２＝ＨｃｏｓＦ

【００６０】従って［数３０］によりＫ１値を分子、Ｋ
２値を分母とする除算値の逆正接函数がパラメータＦ値
となる。

【００６１】

【数３０】Ｆ＝ｔａｎ^−１（Ｋ１／Ｋ２）

【００６２】これより［数３１］［数３２］によりＦの
正余弦値Ｆ１及びＦ２が決定される。

【００６３】

【数３１】Ｆ１＝ｓｉｎＦ

【００６４】

【数３２】Ｆ２＝ｃｏｓＦ

【００６５】上述の如く本発明の第一段座標変換装置に
て、入力重力系Ｗ１、Ｗ２、Ｗ３、磁束系Ｎ１、Ｎ２、
Ｎ３をもつＸＹＺ３軸座標体の方位姿勢のパラメータ
Ｓ、Ｍ、Ｆの３角が算出される。

【００６６】続けて、第２段の座標変換手段として、第
一の変換手段にて求めたＳ、Ｍ、Ｆ値を用いて、新たに
実在直交３軸ＸＹＺ座標をＺ軸のまわりに最大傾斜方向
に直交する位置までの角度Ｓにて時計方向に回転し位置
にある座標をＸ’Ｙ’Ｚ’とする。次に座標Ｘ’Ｙ’
Ｚ’をＸ’軸のまわりにＭ角だけ回転した位置にある直
交３軸座標をＸ”Ｙ”Ｚ”とする。続いてＸ”Ｙ”Ｚ”
座標をＺ”軸のまわりに反時計方向にＳ角だけ回転した
位置にある座標がコンピュータ水平面垂直線空間３軸座
標ξηζとなる。

【００６７】

【数３３】

【００６８】これを展開して、［数３４］が求められ
る。

【００６９】

【数３４】

【００７０】または、［数３５］が求められる。

【００７１】

【数３５】

【００７２】更に、該ξηζ座標の水平面ξηζ面と、
第１段変換手段にて求められる指北水平面Ｘ_００Ｙ_０面
の両面はそれぞれの垂直線０ζと０Ｚ_０は合致している
ので、両面をＦ角を介して［図７］に示す如く重ねるこ
とにより、本体ＸＹＺ座標体を経緯高度の地球座標系に
変換される。

【００７３】従って、ＸＹＺ座標体の速度ベクトルをＶ
_ｄとして、これを［数３６］にて表し

【００７４】

【数３６】Ｘ＝Ｖ_ｄ、 Ｙ＝０、 Ｚ＝０

【００７５】Ｄ面上のＶ_ｄをξηζ３軸に投影し、それ
をＶ_ε、Ｖ_η、Ｖ_ζとして、それぞれを［数３７］［数
３８］［数３９］にて示す。

【００７６】

【数３７】Ｖ_ζ＝Ｖ_ｄ（Ｓ２^２＋Ｓ１^２・Ｍ２）

【００７７】

【数３８】 Ｖ_η＝Ｖ_ｄ（Ｓ２・Ｓ１−Ｓ１・Ｓ２・Ｍ２）

【００７８】

【数３９】Ｖ_ζ＝Ｖ_ｄ・Ｓ１・Ｍ１

【００７９】更に、ξ線上のＶ_ζとη線上のＶ_ηは、合
成されてＨ０面上の投影分速度Ｖ_ｈとなり［数４０］に
て示される。

【００８０】

【数４０】Ｖ_ｈ＝（Ｖ_ζ ^２＋Ｖ_η ^２）^１／２

【００８１】Ｖ_ｈのＨ_０面上の投影位置は、ξ線との偏
角をΔＨとすれば、ΔＨは［数４１］にて示される。

【００８２】

【数４１】ΔＨ＝Ｔａｎ^−１（Ｖ_η／Ｖ_ζ）

【００８３】一方、Ｈ_０面上にては、０ξ線と磁北、０
Ｘ_０線との挟角は［数３４］に示される如く、Ｓ角とＦ
角との和の負数が方位角θとなるので［数４２］に示す
如く、偏角修正を加えた方位角をＤ角とすれば

【００８４】

【数４２】Ｄ＝−（Ｆ＋Ｓ−ΔＨ）

【００８５】従って、Ｈ_０面上においてＶ_ｈのＤ角余弦
及び正弦値、即ちＶ_ｈの０Ｘ_０線分力及び０Ｙ_０線分力
がそれそれ経度分速度、緯度分速度となる。これをＶ
１、Ｖ２とし［数４３］及び［数４４］に示す。

【００８６】

【数４３】Ｖ１＝Ｖ_ｈＳｉｎ（Ｄ）

【００８７】

【数４４】Ｖ２＝Ｖ_ｈＣｏｓ（Ｄ） 又、高度方向の分速度をＶ_３にて表せば、これは既に求
めたＶ_ζとなる。

【００８８】

【数４５】Ｖ３＝Ｖ_ζ

【００８９】このようにして求められたＶ１、Ｖ２、Ｖ
３を［数４６］［数４７］［数４８］に示される如く、
それぞれ積分演算を施して、経、緯、高度Ｌ１、Ｌ２、
Ｌ３が求められる。

【００９０】

【数４６】Ｌ１＝∫Ｖ１ｄｔ＝ΔＬ１＋Ａ

【００９１】

【数４７】Ｌ２＝∫Ｖ２ｄｔ＝ΔＬ２＋Ｂ

【００９２】

【数４８】Ｌ３＝∫Ｖ３ｄｔ＝ΔＬ３＋Ｃ

【００９３】［数４６］［数４７］［数４８］の各式に
おけるＡ、Ｂ、Ｃ値は積分常数にして、運動経路の各定
点に於いて地図又はＧＰＳ関連機器より該地点の経緯高
度が与えられる。

【００９４】Ｌ１、Ｌ２、Ｌ３は表示器に示される。

【００９５】移動体の運動に伴うＬ１、Ｌ２、Ｌ３の増
分はＬ１、Ｌ２はＸＹ軌道、Ｌ３はＺ高度線の各点列と
して、記憶装置に格納される。

【００９６】記憶装置に貯へられる点群は関連した地図
情報に重ね、運動の速度に対応して表示される。

【００９７】

【発明の効果】 ３元測定が人体歩数又は車両車輪の回
転数による速度実量型であるので、実用上充分なる精度
が期待できる。又、全装置を小型、半導体集積化が可能
であるので車両、人体に装着できる。測地、地形の調
査、登山、旅行等の用途が期待できる。

【００９８】

【図面の簡単な説明】

【図１】 本発明の実施例に適用される、重力センサを
一部断面で示す構成図である。

【図２】 ［図１］の重力センサの動作説明図である。

【図３】 本発明の一実施例に適用される、地磁気セン
サを示す斜視図である。

【図４】 ［図３］の地磁気センサの動作説明図であ
る。

【図５】 一実施例における計測部の構成を示す説明図
である。

【図６】 本発明の第一段座標変換式の座標系を示す説
明図であり、最大傾斜角Ｍを挟む最大傾斜方位線Ｈ_０面
上のＣ_０−０−Ｃ_０’線、Ｄ面上のＣ−０−Ｃ’線を示
している。

【図７】 第二変換手段の原理説明用鳥瞰図である。理
解しやすくするため、Ｈ面、Ｄ面に共通する交切線ＡＢ
をＨ面、Ｄ面に分離している。

【図８】 本発明の座標変換手段に関する演算のフロー
チャートである。

【符号の説明】

Ｘ、Ｙ、Ｚ 実在直交３軸座標体 Ｄ面、
Ｘ０Ｙ面 Ｘ_０Ｙ_０Ｚ_０ コンピュータ基準指北水平面直
交３軸座標体Ｈ_０面、Ｘ_００Ｙ_０ ξ、η、ζ ＸＹＺに対応する鉛直水平座標
系 Ｗ１、Ｗ２、Ｗ３ 重力検出センサ Ｎ１、Ｎ２、Ｎ３ 地球磁束検出センサ Ｆ角 水平面Ｈ_０面内における指北０
Ｘ_０線よりＨ_０面Ｄ面との交切線Ａ−Ｂ線までの角度 Ｓ角 Ｄ面上におけるＨ_０面とＤ面の
交切線Ａ−Ｂ線と０Ｘ線との間に挟む角度 Ｍ角 最大傾斜角 Ｃ角 コンパス方位角 Ａ−Ｂ線 Ｈ_０面、Ｄ面の交切線 Ｃ_０−Ｃ_０’ Ｈ_０面上の傾斜方位線 Ｃ−Ｃ’ Ｄ面上の傾斜方位線

Claims (3)

【特許請求の範囲】
1. 【請求項１】 移動体本体直交３軸に、重力分力Ｗ１、
   Ｗ２、Ｗ３、並びに磁束分力Ｎ１、Ｎ２、Ｎ３をもつ上
   方軸０Ｚ床面Ｘ０Ｙ（Ｄ面）の０−ＸＹＺ座標体の空間
   における姿勢を求めるため、第一の座標変換手段とし
   て、Ｚ軸まわりに０Ｘ線をＤ面上にて最大傾斜方向に直
   交する位置までの角度Ｓにて回転した位置にある、座標
   軸を、０−Ｘ’Ｙ’Ｚ’座標とし、０−Ｘ’Ｙ’Ｚ’座
   標軸を０Ｘ’のまわりに最大傾斜角Ｍにて回転し、Ｄ面
   を水平面に一致させた位置の直交座標軸を０−Ｘ”Ｙ”
   Ｚ”とする。続いて該０−Ｘ”Ｙ”Ｚ”座標軸を０Ｚ”
   （鉛直０Ｚ_０線）を中心にＸ”０ Ｙ”面を水平面上に
   て、水平面の方位基準線にいたる角Ｆだけ回転して空間
   指北基準線０Ｘ_０、水平面Ｘ_Ｏ０ Ｙ_０、鉛直線０Ｚ_０
   のコンピュータ基準空間座標０−Ｘ_０Ｙ_０Ｚ_０に変換さ
   れる。該第一の座標変換手段により該重力系３ケ、磁束
   系３ケの入力値を用いてＸＹＺ座標体の方位姿勢のパラ
   メータ３角Ｆ、Ｍ、Ｓを求め、つづけて該Ｓ、Ｍ、Ｆ３
   角より実在ＸＹＺ座標系を地球座標系に変換する第二の
   手段として新たに本体直交３軸のＸＹＺ座標軸をＺ軸の
   まわりに最大傾斜角方向に直交する位置までの角度Ｓに
   て、時計方向に回転した位置にある直交座標をＸ’Ｙ’
   Ｚ’とし、Ｘ’Ｙ’Ｚ’座標をＸ’軸のまわりに最大傾
   斜角Ｍだけ回転した位置にある直交座標を、Ｘ”Ｙ”
   Ｚ”とする。つづいてＸ”Ｙ”Ｚ”座標をＺ”軸のまわ
   りに反時計方向にＳだけ回転した位置にある空間３軸座
   標ξηζを求め、更に該ξηζ座標の水平面ξ０η面
   を、第一の座標変換手段にて求められた指北水平Ｘ_００
   Ｙ_０面にＦ角を介して重ねることにより、本体ＸＹＺ座
   標体は地球座標系に変換され、該本体ＸＹＺ座標体の０
   Ｘ上の移動体の速度ベクトルＶは、地球座標系の経緯高
   度の３元速度ベクトルＶ１、Ｖ２、Ｖ３に分解され、そ
   れぞれを積分演算により移動体の現在位置の経緯高度を
   算出表示する、移動体の空間位置測定装置。
2. 【請求項２】 積分演算が出力する経緯高度を、走行経
   路上の各定点において地図又はＧＰＳ情報によりて較正
   する装置をもつ、請求項１の移動体の空間位置測定装
   置。
3. 【請求項３】 積分演算が出力する経緯高度を記憶装置
   に格納し、必要に応じて走行経路近傍を含む地図上に重
   ね、又は並列して表示する画面処理装置をもつ、請求項
   １、請求項２の移動体の空間位置測定装置。