JPH055044B2 - - Google Patents

Description

【発明の詳細な説明】 発明の技術分野 この発明は、ストラツプダウン慣性航法並びに
慣性装置に係り、特に角速度センサおよび加速度
センサの情報をサンプリングして計算機に入力す
る時刻を夫々異ならしめるものである。

従来技術 ストラツプダウン慣性航法或は慣性装置は、主
として角速度センサ、加速度センサおよび計算機
の三要素から構成され、次のように動作する。

即ち、角速度センサおよび加速度センサの入力
軸の方向は、機体に固定される。ここで、機体に
固定された直交座標系（X_B、Y_B、Z_B）とすると、
角速度センサはX_B軸、Y_B軸、Z_B軸まわりの角速
度ω₁、ω₂、ω₃を計測し、加速度センサはX_B軸、
Y_B軸、Z_B軸方向の角速度α_B1、α_B2、α_B3を計測す
る。

計算機では、これらの角速度および加速度情報
から機体の慣性空間、又は、地球に対する速度お
よび位置を求める。

そのために、まず、機体座標系の加速度 ベクトルα_B1 α_B2 α_B3を慣性空間に固定された 慣性座標系の加速度ベクトルα_I1 α_I2 α_I3に 変換するための方向余弦行列信号を計算する。

方向余弦行列信号を求める方法は種々の方法が
あるが、一例として、四元数を用いる方法を以下
に示す。

機体の姿勢角をあらわす四元数g₁、g₂、g₃、g₄
は、上記の角速度ω₁、ω₂、ω₃を用いて、次の微
分方程式を計算機で積分することにより、時間の
関数として求められる。

ｄ／dt＝g₁ g₂ g₃ g₄＝１／２０ −ω₃ ω₂ −ω₁ ω₃ ０ −ω₁ −ω₂ ω₂ ω₁ ０ −ω₃ ω₁ ω₂ ω₃ ０g₁ g₂ g₃ g₄ ……(1) 次に、この四元数から、次の関係式により、方
向余弦行列信号C_BIが時間の関数として得られる。

C_BI＝q² ₁−q² ₂−q² ₄＋q² ₄ ２（q₁q₂＋q₃q₄） ２（q₁q₃−q₂q₄） ２（q₁q₂−q₃q₄） −q² ₁＋q² ₂−q² ₃＋q² ₄ ２（q₂q₃＋q₁q₄） ２（q₁q₃＋q₂q₄） ２（q₂q₃−q₁q₄） −q² ₁−q² ₂＋q² ₃＋q² ₄ ……(2) この方向余弦行列信号を用いて、次式により慣
性座標系の加速度ベクトルが得られる。

α_I2 α_I2 α_I3＝C_BI・α_B1 α_B2 α_B3 ……(3) 慣性座標系における加速度ベクトルは、次の微
分方程式を積分することによつて得えられる。

ｄ／dtv_I1 v_I2 v_I3＝α_I1 α_I2 α_I3＋g₁ g₂ g₃ ……(4) (4)式の右辺第２項は、加速度センサが地球重力
の影響を受けるため、それを補正するための項
で、位置ベクトルの関数である。

さらに、慣性座標系における位置ベクトルは、
次の微分方程式を積分することによつて得られ
る。

ｄ／dtr_I1 r_I2 r_I3＝v_I1 v_I2 v_I3 ……(5) ここでは、慣性座標系を用いた場合を述べた
が、地球座標系を用いる場合は、上記の式に若干
の補正項をつけくわえることによつて位置ベクト
ルが求められる。

以上がストラツプダウン慣性航法又は慣性装置
の動作原理であるが、これらは公知の事実であ
る。

しかし、これらの計算を精度よく行うために
は、デイジタル計算機を用いなければならず、デ
イジタル計算機では、連続的な情報をそのまま取
扱うことができないため、従来の技術における装
置では次のように行なつている。

加速度センサおよび角速度センサの情報はセン
サ自身または、インターフエイス回路で、一定時
間積分され、速度増分信号△v_B1、△v_B2、△v_B3お
よび角度増分信号△θ₁、△θ₂、△θ₃の形でサンプ
ルされ、計算機に入力される。

このサンプル周期をT_S、サンプル時刻をt₀、t₁
……t_k……であらわすと次の関係式であらわされ
る。

t_k−t_k-1＝T_S（ｋ＝１、２……） ……(6) △θi＝∫^th _th-1ωidt（ｉ＝１，２，３） ……(7) △v_Bi＝∫^tk _t-1α_Bidt（ｉ＝１、２、３） ……(8) 時刻t_kにおける機体の姿勢角をあらわす四元数
は次の差分方程式で計算される。

g₁ g₂ g₃ g_4k＝Ｃ −△θ₃S △θ₂・Ｓ −△θ₁S △θ₃・Ｓ Ｃ −△θ₁・Ｓ −△θ₂・Ｓ −△θ₂・Ｓ △θ・Ｓ Ｃ −△θ₃・Ｓ △θ₁S △θ₂S △θ₃・Ｓ Ｃg₁ g₂ g₃ g_4k-1 ……(9) 但し、Ｓ＝sinθ₀／２／θ₀ Ｃ＝cosθ₀／２ θ₀＝√（△₁）²＋（△₂）²＋（△₃）² 時刻t_kにおける四元数から(2)式により、時刻t_h
における方向余弦行列信号C_BI(h)が計算される。

この方向余弦行列信号を用いて、慣性座標系に
おける速度増分信号△v_I1、△v_I2、△v_I3は、次の
近似式から計算される。

△v_I1 △v_I2 △v_I3＝C_BI(h)△v_B1 △v_B2 △v_B3 ……(10) 慣性座標系における速度増分信号が求められる
と、(4)式、および(5)式と等価な差分方程式によ
り、速度および位置が求められる。

これらの計算式の中で、精度に大きい影響を与
えるのは、(10)式の近似式である。

慣性座標系における正確な速度増分信号を与え
る式は、(3)式の両辺を時刻t_k-1からt_kまでの間、
積分すれば得られるが、方向余弦行列信号C_BIの
各時刻での値がC_BI(h)に等しい場合のみ(10)式と一
致する。

したがつて、精度を上げるためにはサンプル周
期T_Sをサンプル時刻の間に、方向余弦行列信号
の値がほとんど変化しないようにサンプル周期
T_Sを短かくしなければならない。

しかし、各サンプル毎に上記の計算をしなけれ
ばならないから、サンプル周期T_Sを短かくする
と、単位時間の計算量が増大し、高速の計算機を
必要とする。

発明の概要 この発明は、これ等の点を考慮して高速の計算
機を必要としないで高精度が得られるストラツプ
ダウン慣性航法並びに慣性装置を提供することで
ある。

この発明のストラツプダウン慣性航法ならびに
慣性装置では速度増分信号△v_B1、△v_B2、△v_B3の
サンプル時刻t₀、t₁……、t_k……に対して、角度
増分信号△θ₁、△θ₂、△θ₃のサンプル時刻t₀′、
t₁′……t_k′……を次の関係が成立するように選ぶ。

h_k−t_k′＝T_S／２ ……(11) 但し、ｋ＝０、１、２…… したがつて、時刻t_k′すなわち（t_k−T_S／２）にお ける角度増分信号△θ₁′、△θ₂′、△θ₃′は次の式
で
あらわされる。

△θ_i′＝∫_thk′_tk-1ωidt＝∫^tk-1/2TS _tk-3/2TS
ωidt……(12) 但しｉ＝１、２、３ これら角度増分信号から(9)式、(2)式と同様の式
を用いて、時刻（t_k−T_S／２）における方向余弦行 列信号C_BI（ｋ−１／２）を計算する。

さらにこの方向余弦行列信号C_BI（ｋ−１／２）を 用いて、慣性座標系における速度増分信号△v_I1、
△v_I2、△v_I3を次の近似式で計算する。

△v_I1 △v_I2 △v_I3＝C_BI（ｋ−１／２）△v_B1 △v_B2 △v_B3 ……(13) (13)式で計算した速度増分信号は、(10)式で計算し
た速度増分信号にくらべて、非常に精度が高い。

なぜなら、第１図ａの時間ｔと機体の姿勢角θ_i
の特性曲線で示すように従来では、時刻t₀、t₁、
t₂……t_kにおける正しい機体の姿勢角θ_iがわかり
その関数としてその時刻での正しい方向余弦行列
信号C_BI(h)が第１図ｂで求められる。尚、この場
合角度増分信号は３個の要素を示し方向余弦行列
信号は９個の要素から成るが第１図では各１個の
要素のみ示す。

一方、この発明では時刻t₀−T_S／２、t₁−T_S／２、… …t_k−T_S／２、……における正しい方向余弦行列信 号C_BI（ｋ−１／２）が求められる。

(10)式および(13)式は、ともに(3)式の両辺をt_k-1か
らt_kまでの間、積分して得られる式の近似である
が、(10)式では方向余弦行列信号C_BIを１番端の時
刻t_kでの値で近似しているのに対して(13)式では、
積分区間の中央の時刻での値で近似している。一
般的に区間の中央の値で近似する方が誤差を小さ
くする。

特に加速度の極性が一定で、C_BIの各要素が単
調に変化する場合は、C_BIをC_BI（ｋ−１／２）で近似 するとt_k−T_s／２の前後で誤差の極性がかわるので、 その積分値は誤差が打ち消しあい、(13)式の誤差は
非常に小さくなる。

実際の慣性航法或は慣性装置では、機体の運動
の時定数にくらべて、十分短かい周期でサンプル
されるから、ほとんどすべての区間において、上
記のことが成りたつ。

したがつて、この発明においては計算誤差は非
常に小さくなり、しかも、必要な計算量は、従来
のものと同じである。

発明の実施例 この発明の実施例のブロツク図を第２図に示
す。

図で１は角速度センサ、２は加速度センサでい
ずれも機体に固定されており、夫々機体の３軸ま
わりの角速度および３軸方向の加速度を計測す
る。３，４は第１、第２のインターフエイス回
路、５は一定間隔T_S毎の時刻t₀t₁……t_kとt₀−１／２ T_S、……t_k-1−１／２T_S、t_k−１／２T_S……の1 2ずれ た時刻にサンプル時刻信号をとりだすインターフ
エイス制御回路であつて、角速度センサ１では３
軸まわりの角速度ω₁ω₂ω₃を検出して第１のイン
ターフエイス回路３にインターフエイス制御回路
５の一定間隔T_S毎のサンプル時刻信号とともに
与えてこのサンプル時刻t₀−１／２T_S、……t_k-1− １／２T_S、t_k−１／２T_Sの間、角速度信号を積分し、 角度増分信号△θ₁′△θ₂′△θ₃′をとりだして計算
機
６の方向余弦計算部７に与える。又、加速度セン
サ２では３軸方向の加速度α_B1α_B2α_B3を検出して
第２のインターフエイス回路４にインターフエイ
ス制御回路５の一定間隔T_S毎の時刻t₀t₁……t_kサ
ンプル時刻信号とともに与えて、このサンプル時
刻の間加速度信号を積分して速度増分信号△v_B1
△v_B2△v_B3をとりだして、計算機６の座標変換計
算部８に方向余弦計算部７で時刻t_k−１／２T_Sでの 角度増分信号から計算したその時刻の方向余弦行
列信号C_BI（ｋ−１／２）とともに与える。かくて座 標変換計算部８ではこの方向余弦行列信号C_BI（ｋ
−１／２）を用いて時刻t_kでの機体座標探知速度増 分信号を慣性座標系の速度増分信号に変換して、
速度、位置計算部９に与える。速度位置計算部９
ではこの速度増分信号と重力補正項計算部１０か
らの重力補正項を用いて機体の慣性空間又は地球
に対する速度ｖおよび位置ｒを計算してとりだす
ことができる。

尚、ここで方向余弦計算部７、座標変換計算部
８、速度、位置計算部９ならびに重力補正項計算
部１０は計算機のソフトウエア６として用いられ
ている。

発明の効果 以上のように、この発明では角速度センサと加
速度センサとの情報をサンプリングして計算機に
入力する時刻がほぼ1/2周期異なるようにするこ
とにより計算誤差は非常に小さくなり、必要な計
算量は従来と全く同じで可である。

【図面の簡単な説明】

第１図ａ，ｂは時間に対する機体の姿勢角、サ
ンプル時間に対する方向余弦行列信号の関係を
夫々示す特性線図、第２図はこの発明の一実施例
を示すブロツク図である。 １は角速度センサ、２は加速度センサ、３，４
は第１、第２のインターフエイス回路、５はイン
ターフエイス制御回路、６は計算機のソフトウエ
ア、７は方向余弦計算部、８は座標変換計算部、
９は速度・位置計算部、１０は重力補正項計算
部。

Claims (1)

1. 【特許請求の範囲】 １ 機体に固定される角速度センサおよび加速度
   センサにより、夫々機体の角速度および加速度を
   計測し、夫々の情報から、デイジタル計算機によ
   り、機体の位置求める慣性航法において、角速度
   センサおよび加速度センサの情報をサンプリング
   して、計算機に入力する時刻を、角速度センサと
   加速度センサで、ほぼ1/2周期異ならしめること
   を特徴とするストラツプダウン慣性航法。 ２ 機体に固定される３軸まわりの角速度および
   ３軸方向の加速度を計測するための角速度センサ
   並びに加速度センサと、一定間隔T_s毎の時刻t₀−
   １／２T_s、t₁−１／２T_s、……、t_k−１／２T_s……並び
   に t₀t₁……t_k……とに夫々サンプル時刻信号を送る
   ためのインタフエース制御回路と、前記一定間隔
   T_s毎のサンプル時刻t₀−１／２T_s、t₁−１／２T_s……、 t_k−１／２T_s……の間、角速度信号を積分し角度増 分信号を送出するための第１のインタフエース回
   路と、前記一定間隔T_s毎のサンプル時刻t₀、t₁…
   …t_k……の間、加速度信号を積分し速度増分信号
   を送出するための第２のインタフエース回路と、
   前記第１と第２のインタフエース回路出力を受け
   入れて機体の速度および位置を計算するための計
   算機とを備えることを特徴とするストラツプダウ
   ン慣性装置。 ３ 前記計算機には時刻t_k−１／２T_sでの角度増分 信号からその時刻の方向余弦行列信号C_BI（ｋ−
   １／２）を計算する方向余弦計算部を含むことを特 徴とする特許請求の範囲第２項記載のストラツプ
   ダウン慣性装置。 ４ 前記計算機には方向余弦行列信号C_BI（ｋ−
   １／２）を用いて時刻t_kでの機体座標系速度増分信 号を慣性座標系の速度増分信号に変換するための
   座標変換計算部を含むことを特徴とする特許請求
   の範囲第２項または第３項記載のストラツプダウ
   ン慣性装置。 ５ 前記計算機には前記速度増分信号と重力補正
   項を用いて機体の速度および位置を計算するため
   の速度位置計算部を含むことを特徴とする特許請
   求の範囲第２項または第４項記載のストラツプダ
   ウン慣性装置。 ６ 前記計算機には、重力補正項を用いて機体の
   慣性空間又は地球に対する速度および位置を前記
   速度増分信号から計算するための重力補正項計算
   部を含むことを特徴とする特許請求の範囲第２項
   または第５項記載のストラツプダウン慣性装置。