JP3425689B2 - 慣性装置 - Google Patents
慣性装置Info
- Publication number
- JP3425689B2 JP3425689B2 JP33620799A JP33620799A JP3425689B2 JP 3425689 B2 JP3425689 B2 JP 3425689B2 JP 33620799 A JP33620799 A JP 33620799A JP 33620799 A JP33620799 A JP 33620799A JP 3425689 B2 JP3425689 B2 JP 3425689B2
- Authority
- JP
- Japan
- Prior art keywords
- calculation unit
- reference matrix
- attitude
- initial alignment
- azimuth angle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Lifetime
Links
Landscapes
- Gyroscopes (AREA)
- Navigation (AREA)
Description
度計の信号を用いて角速度、加速度、姿勢角、方位角、
速度、位置などを計算し、その際に、姿勢角、方位角の
初期値を設定するための初期アライメント計算部を備え
た慣性装置に関する。 【０００２】 【従来の技術】図２に従来のストラップダウン型慣性装
置の基本的機能構成を示す。この種の装置は例えば特開
平８−２１７４０号公報に詳細に述べられているので以
下に簡単に述べる。ジャイロ１から慣性装置内の直交座
標軸（航空機などに慣性装置が搭載された場合機体の進
行方向がｘ軸、右方向がｙ軸）回りの角速度（機軸角速
度）ωx ，ωy ，ωz と加速度計２からの機軸加速度ａ
x ，ａy ，ａz が演算処理部３１に入力され、演算処理
部３１内の姿勢基準行列計算部３において機軸角速度ω
x ，ωy ，ωz を積分して姿勢基準行列が計算され、そ
の姿勢基準行列を用いて機軸加速度ａx ，ａy ，ａz が
加速度座標変換部４で基準座標軸加速度ＡX ，ＡY に座
標変換され、更に速度計算部６で積分されて速度ＶX ，
ＶY に変換される。この速度ＶX ，ＶY が位置行列計算
部７で積分され、その位置行列を用いて位置計算部８で
位置が計算される。 【０００３】また姿勢角、方位角計算部５において姿勢
角（ロール角：φ、ピッチ角：θ）、方位角（Ψ）が姿
勢基準行列Ｃから通常下式により計算される。 φ＝ｔａｎ-1（Ｃ3,2 ／Ｃ3,3 ） θ＝ｓｉｎ-1（−Ｃ3,1 ） ΨC ＝ｔａｎ-1（Ｃ2,1 ／Ｃ1,1 ） Ψ＝ΨC ＋α Ｃ：姿勢基準行列（３×３行列） Ｃi,j ：Ｃのｉ，ｊ要素 α：航法計算座標軸の方位であって位置行列から座標軸
方位計算部９で計算される。 【０００４】位置行列と速度ＶX ，ＶY とから座標軸回
転レートを計算部１０で計算し、この計算結果により姿
勢基準行列計算部３に対する補正を行う。またこの座標
軸回転レートとＶX ，ＶY とから座標軸回転により生じ
るコリオリ加速度を計算部１１で計算し、これにより速
度計算部６に対する補正を行う。その際、速度、位置行
列、姿勢基準行列演算には、それぞれの初期値を設定す
る必要があり、位置データは何らかの方法により外部か
ら入力され設定されるが、姿勢角、方位角計算の基とな
る姿勢基準行列、速度はジャイロ、加速度計が計測する
地球自転角速度、地球重力データに基づいて計算された
結果が設定される。この計算を通常初期アライメント計
算と呼んでいる。 【０００５】初期アライメント計算は初期アライメント
計算部３２で行うがその従来の方法を図３に示す。この
方法は、慣性装置に組込まれ航空機等の機体に取付けら
れた加速度計が計測した地球重力データを正しく設定さ
れた姿勢基準行列を用いて基準座標に座標変換すれば鉛
直方向成分のみの加速度データになるべきものが、計算
開始時に予め設定した姿勢基準行列の値に誤差がある場
合、加速度計信号を座標変換した後の基準座標における
加速度データは、鉛直成分の他に水平成分ＡX ，ＡY も
算出される。そのため、座標変換後の加速度データを積
分して速度を計算すると姿勢基準行列に誤差がある場合
水平方向に速度が発生することになる。この現象は、計
算開始時に設定された姿勢基準行列の値に誤差がない場
合でも、ジャイロによって計測される地球自転角速度デ
ータにより姿勢基準行列が更新され、姿勢基準行列の誤
差となり同様に水平方向速度が発生する。 【０００６】この水平方向速度ＶX ，ＶY を図３に示す
ように計算部２１で帰還利得を掛けて速度フィードバッ
クデータＦＡX ，ＦＡY を作り、これをＡX ，ＡY に帰
還減算し、また計算部２２でＶX ，ＶY に帰還利得を掛
けて、姿勢角フィードバックデータＦωX ，ＦωY を作
る。更にＶX ，ＶY を計算部２３で帰還利得を掛けて積
分（１／Ｓは積分記号を表わす）することにより推定角
速度ＥωX ，ＥωY を求め、このＥωX ，ＥωY を姿勢
角フィードバックデータＦωX ，ＦωY と加算し、これ
らの加算結果ΩX ，ΩY にて姿勢基準行列計算部３内の
姿勢基準行列を補正して、ＡX ，ＡY が小さくなるよう
にする。地球自転角速度鉛直軸成分計算部１０ａにその
慣性装置の位置における緯度が入力されて、地球自転角
速度鉛直軸成分ΩZ （＝ωＥZ ）が計算され、これも姿
勢基準行列計算部３に入力される。 【０００７】このように慣性装置に組込まれた航空機な
どの機体が静止した状態で、基準座標系の水平方向の加
速度成分ＡX ，ＡY を処理して姿勢基準行列計算部３、
速度計算部６にフィードバックすることを繰返すことに
より姿勢基準行列の水平に対する誤差を減少させて正し
い姿勢基準行列が計算されることとなる。その結果を初
期値として、機体移動中において慣性装置は姿勢基準行
列を求め、正しい姿勢角が計算されることになる。 【０００８】一方、図３中に示すフィードバック処理の
計算部２３でＫ3 の掛かるフィードバック項ＥωX ，Ｅ
ωY は定常的には地球自転角速度の基準座標系における
水平成分ωX ，ωY の推定値を示し、この２つの値を使
用して基準座標系Ｘ軸の北からの方位αを次式により算
出することができる。（図４参照） α＝ｔａｎ-1（−ＥωY ／ＥωX ） …（１） ちなみに、北基準局地水平座標軸Ｎ，Ｅ，Ｄ（北、東、
鉛直下方向）と、速度計算などを行う基準座標系でＸ軸
が任意の方位にある局地水平座標軸Ｘ，Ｙ，Ｚ（Ｘ，Ｙ
は局地水平面内、Ｚ軸は鉛直下方向）との関係は例えば
図４Ａに示すようになる。計算部２３で計算されたフィ
ードバック項ＥωX ，ＥωY は先に延べたように定常的
には地球自転角速度の基準座標系における水平成分
ωX ，ωY の推定値を示し、これらは図４Ｂに示すよう
に、地球自転角速度北方向成分ωＥNに対し、ωX ＝ω
ＥN ×ｃｏｓα、ωY ＝−ωＥN ×ｓｉｎαなる関係に
あり、ＥωX ＝ωX ，ＥωY ＝ωY となった状態ではα
は式（１）により求まる。 【０００９】こうして求まった基準座標系Ｘ軸の方位α
と、姿勢基準行列から計算される基準座標系Ｘ軸に対す
る慣性装置ｘ軸の方位ΨC を加算することによりｘ軸の
北からの方位角Ψが次式により計算される。 Ψ＝ΨC ＋α …（２） 基準座標軸Ｘ，Ｙ，Ｚと、慣性装置内の直交座標軸（一
般にジャイロ、加速度計が取付けられる軸であり、慣性
装置が搭載された機体の進行方向、右方向、下方向と一
致する）ｘ，ｙ，ｚとは図４Ｃに示す関係とする。姿勢
角、方位角計算部５では、ｘ軸のＸ，Ｙ面間におけるＸ
軸に対する方向ΨC が計算される。よって慣性装置のｘ
軸の北に対する方位角は式（２）により求まる。 【００１０】 【発明が解決しようとする課題】従来の慣性装置は、加
速度計出力を基準座標系に座標変換して求めた水平方向
速度データを用いて姿勢角誤差の修正、地球自転角速度
の推定を行い、推定された地球自転角速度の水平方向成
分ＥωX ，ＥωY を用いて基準座標系Ｘ軸の北の方から
の方位角αを計算し、更に基準座標系に対する慣性装置
の方位角ΨC を加えて北に対する慣性装置の方位角Ψを
算出している。 【００１１】この方法では、鉛直軸に対しては初期アラ
イメント計算実施地点の緯度データを外部から入力して
地球自転角速度の鉛直軸成分の補正を行っているため、
機体が静止していれば、慣性装置の方位角ΨC は一定の
値（通常はゼロ）にとどまっているが、ジャイロに誤差
があると誤った角速度を計測し機体が回転しているのと
等価な状態となり、慣性装置の方位角ΨC が変化してし
まう。このΨC の変化に対し、地球自転角速度の推定計
算においてαがΨC の変化分を補うように変化するが完
全に補いきれず方位角の誤差が発生することになる。 【００１２】所で計算部２１〜２３における帰還利得Ｋ
1 〜Ｋ3 の各値を小さくして外乱の影響を受け難くして
いるため、初期アライメント計算に、航空機の場合は３
分程度、ロケットの場合は１時間程度も時間を掛けてい
る。このため、ジャイロに誤差があると、姿勢基準行列
計算部３での積分動作のため、誤差が初期アライメント
計算中積分されて増加する、このため、図５Ａに示すよ
うに初期アライメント計算中に方位角誤差が時間と共に
増加し、特に初期アライメント計算が比較的長い場合は
方位角誤差が大きなものとなり慣性装置としての誤差も
大きなものとなった。 【００１３】この発明の目的は、初期アライメント計算
中に発生する方位角誤差を補正し、精度良く方位角を計
算することができる慣性装置を提供することにある。 【００１４】 【課題を解決するための手段】この発明によれば姿勢基
準行列に基づき計算した方位角ΨC 、この値が小さくな
るように姿勢基準行列計算部に帰還する方位角補正演算
部を、初期アライメント計算部に設ける。つまりこの発
明は、慣性装置の初期アライメント計算が、慣性装置が
搭載された機体が移動しない（機体の動揺等はあっても
オフセット的に方位角が変化しない）という条件下で行
われることを利用し、初期アライメント計算で計算され
た方位角ΨC は振動状に変動しても時間とともに増加
（または減少）するものではないので、このΨC の値が
ゼロになるように補正量を計算し、地球自転角速度の鉛
直軸成分と合わせて補正することにより、ジャイロ誤差
によりΨC に誤差が発生することをふせぎ、機体の正し
い方位角Ψを計算することを可能とする。 【００１５】 【発明の実施の形態】この発明の要部は慣性装置中の初
期アライメント計算部３２にあり、その実施例を図３と
対する部分に同一符号を付けて示す。この実施例によれ
ば、初期アライメント計算部３２に方位角補正演算部２
４を設け、初期アライメント計算中に、姿勢角、方位角
計算部５で計算された方位角ΨC を方位角補正演算部２
４に入力し、その方位角補正演算部２４の出力を姿勢基
準行列計算部３に帰還して方位角ΨC の値が小さくなる
ように、つまりΨC がゼロに収斂するようにする。 【００１６】方位角補正演算部２４では例えば図１中に
示すように入力されたΨC に対し、比例演算部２４ａで
帰還利得Ｋ4 を乗算しその出力を姿勢基準行列計算部３
に帰還入力するだけでもよく、更に積分演算部２４ｂで
ΨC に帰還利得Ｋ5 を掛けて積分した結果をも姿勢基準
行列計算部３に帰還してもよい。この方位角補正演算部
２４の出力ＦωZ の帰還は姿勢基準行列計算部３に直接
行ってもよく、図１中に示すように、地球自転角速度鉛
直軸成分計算部１０ａの出力ωＥZ と加算して姿勢基準
行列計算部３に帰還してもよい。 【００１７】この構成により、Ｋ4 ΨC の帰還作用によ
り、ジャイロ誤差に基づく増大していく方位角誤差は微
小レベルに抑えられ、例えば図５Ｂに示すように、方位
角誤差は初期アライメント計算中増加しないようにする
ことができる。また帰還利得Ｋ4 を単なる利得とするこ
となく、適当な時定数をもたせることにより、機体の動
揺により方位角ΨC が振動状に変動しても、その平均な
量が抑圧され、かつジャイロ誤差により増大していく方
位誤差が微小レベルに抑えられるように、フィルタ効果
を持たすこともできる。 【００１８】更に積分演算部２４ｂも設ける場合は、図
５Ｂ中における消去できない微小誤差を積分して、この
誤差がゼロになるように動作させることもできる。上述
においては初期アライメント計算部３２に基準座標速度
ＶX ，ＶY を入力として初期アライメント計算を行った
が、ＶX ，ＶY を積分した位置データを初期アライメン
ト計算部３２に入力して、初期アライメント計算を行う
こともできる。要は基準座標軸加速度ＡX ，ＡY にもと
づく信号により初期アライメント計算を行えばよい。 【００１９】 【発明の効果】以上述べたようにこの発明によれば、慣
性装置の初期アライメント計算中に、加速度計などの基
準軸に対する方位角計算値ΨC がゼロになるように帰還
制御が、方位角補正演算部２４により姿勢基準行列計算
部３に対して行われるため、ジャイロ誤差による時間と
ともに増大する初期アライメント計算の方位角誤差を微
小レベルに抑えることができる。
ト計算部を示すブロック図。 【図２】ストラップダウン型慣性装置の基本的構成を示
すブロック図。 【図３】従来の慣性装置の初期アライメント計算部を示
すブロック図。 【図４】Ａは基準座標軸の定義を示す図、Ｂは基準座標
軸における地球自転角速度成分を示す図、Ｃは基準座標
に対する慣性装置姿勢角、方位角の定義を示す図であ
る。 【図５】Ａは従来の慣性装置における初期アライメント
計算時の方位角誤差の変化状態を示す図、Ｂはこの発明
による慣性装置における初期アライメント計算時の方位
角誤差の変化状態を示す図である。
Claims (1)
- (57)【特許請求の範囲】 【請求項１】 ジャイロの出力から姿勢基準行列計算部
で姿勢基準行列を計算し、その姿勢基準行列により方位
角、姿勢角を計算し、また加速度計の出力を姿勢基準行
列により基準座標軸加速度に変換し、その基準座標軸加
速度から得られた信号を初期アライメント計算部に入力
し、その初期アライメント計算部の出力により上記姿勢
基準行列計算部の姿勢基準行列の値を帰還補正して、姿
勢基準行列の初期値を決定する慣性装置において、 上記姿勢基準行列に基づき計算した方位角を、この値が
小さくなるように上記姿勢基準行列計算部に帰還する方
位角補正演算部が上記初期アライメント計算部に設けら
れていることを特徴とする慣性装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP33620799A JP3425689B2 (ja) | 1999-11-26 | 1999-11-26 | 慣性装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP33620799A JP3425689B2 (ja) | 1999-11-26 | 1999-11-26 | 慣性装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2001153680A JP2001153680A (ja) | 2001-06-08 |
| JP3425689B2 true JP3425689B2 (ja) | 2003-07-14 |
Family
ID=18296750
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP33620799A Expired - Lifetime JP3425689B2 (ja) | 1999-11-26 | 1999-11-26 | 慣性装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3425689B2 (ja) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE102012011632A1 (de) * | 2011-07-28 | 2013-01-31 | Memsic Inc. | System und Verfahren zum Erfassen einer Raumstellung und Winkelrate unter Verwendung eines Magnetfeldsensors und Akzelerometers für tragbare elektronische Vorrichtungen |
| JP5724855B2 (ja) | 2011-12-02 | 2015-05-27 | トヨタ自動車株式会社 | 倒立移動体及び角速度センサの出力値補正方法 |
| CN104482942B (zh) * | 2014-12-11 | 2017-06-20 | 哈尔滨工程大学 | 一种基于惯性系的最优两位置对准方法 |
| US10816337B2 (en) | 2017-01-27 | 2020-10-27 | Fa Drone Inc. | Inertial measurement method, inertial measurement apparatus, and inertial measurement program |
| WO2020245902A1 (ja) * | 2019-06-04 | 2020-12-10 | 三菱電機株式会社 | 方位角推定装置、ジャイロシステム、方位角推定方法及びプログラム |
| US11959775B2 (en) * | 2020-02-19 | 2024-04-16 | Sony Group Corporation | Information processing device and information processing method that enable simple calibration |
| WO2024048264A1 (ja) * | 2022-08-29 | 2024-03-07 | ソニーグループ株式会社 | 情報処理装置、および情報処理方法、並びにプログラム |
-
1999
- 1999-11-26 JP JP33620799A patent/JP3425689B2/ja not_active Expired - Lifetime
Also Published As
| Publication number | Publication date |
|---|---|
| JP2001153680A (ja) | 2001-06-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP4199553B2 (ja) | ハイブリッド航法装置 | |
| JP5328252B2 (ja) | ナビゲーションシステムの位置検出装置および位置検出方法 | |
| US4038527A (en) | Simplified strapped down inertial navigation utilizing bang-bang gyro torquing | |
| US20040133346A1 (en) | Attitude change kalman filter measurement apparatus and method | |
| US7970501B2 (en) | Methods and systems utilizing true airspeed to improve vertical velocity accuracy | |
| EP2472225A2 (en) | Method and system for initial quaternion and attitude estimation | |
| JP2000055678A (ja) | 車両用現在位置検出装置 | |
| JP2012173190A (ja) | 測位システム、測位方法 | |
| JP4447791B2 (ja) | ジャイロメータと加速度計を具備する航空機の姿勢決定装置 | |
| JP3425689B2 (ja) | 慣性装置 | |
| US7933717B2 (en) | Method for elaborating navigation parameters and vertical of a place | |
| CN108871323A (zh) | 一种低成本惯性传感器在机动环境下的高精度导航方法 | |
| US11268812B1 (en) | Bias corrected inertial navigation system | |
| JP2913122B2 (ja) | ストラップダウン型ジャイロ装置 | |
| JP3375720B2 (ja) | 船舶の慣性航法装置 | |
| JP3919911B2 (ja) | 方位姿勢基準装置 | |
| JPS62106385A (ja) | 乗物の運行角度を計算するためのドップラ慣性ループ | |
| JP2756554B2 (ja) | 慣性装置 | |
| JP3038452B2 (ja) | ストラップダウン方式の姿勢検出装置 | |
| JPH11118507A (ja) | 船舶の慣性航法装置 | |
| JPH06201863A (ja) | ストラップダウン方式の姿勢検出装置 | |
| JPH06265367A (ja) | 船舶の慣性航法装置 | |
| RU2741564C2 (ru) | Способ коррекции ошибок интегральных навигационных систем | |
| JP3884742B2 (ja) | 方位計 | |
| JP2003014460A (ja) | 姿勢角検出装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20030401 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 3425689 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20080509 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20090509 Year of fee payment: 6 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100509 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110509 Year of fee payment: 8 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110509 Year of fee payment: 8 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120509 Year of fee payment: 9 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120509 Year of fee payment: 9 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130509 Year of fee payment: 10 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130509 Year of fee payment: 10 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20140509 Year of fee payment: 11 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| EXPY | Cancellation because of completion of term |