JP4287975B2 - 姿勢計測装置 - Google Patents
姿勢計測装置 Download PDFInfo
- Publication number
- JP4287975B2 JP4287975B2 JP2000088628A JP2000088628A JP4287975B2 JP 4287975 B2 JP4287975 B2 JP 4287975B2 JP 2000088628 A JP2000088628 A JP 2000088628A JP 2000088628 A JP2000088628 A JP 2000088628A JP 4287975 B2 JP4287975 B2 JP 4287975B2
- Authority
- JP
- Japan
- Prior art keywords
- calculation
- direction cosine
- cosine matrix
- calculation unit
- unit
- 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 - Fee Related
Links
Images
Landscapes
- Gyroscopes (AREA)
- Navigation (AREA)
Description
【発明の属する技術分野】
この発明は移動体の角速度をジャイロで検出し、そのジャイロの出力を用いて姿勢基準計算を行い、その計算結果から移動体の姿勢角を計測する姿勢計測装置に関する。
【0002】
【従来の技術】
図3に従来の姿勢計測装置を示す。この種の装置は例えば特開平8−21740号公報に詳細に説明されているから、以下では簡単に述べる。ジャイロ11から移動体の各軸まわりの角速度がジャイロデータとして出力され、このジャイロデータはジャイロデータ補正計算部12で、ジャイロ11のバイアス誤差、ミスアライメント誤差、スケールファクタの誤差が補正されて、姿勢基準計算、例えば方向余弦行列計算部13へ供給される。
【0003】
一方、移動体の各軸方向の加速度が加速度計14で検出され、加速度計データとして加速度計補正計算部15に入力され、加速度計自体の誤差が補正されて、座標変換計算部16に入力され、方向余弦行列を用いて、機軸加速度が基準座標軸加速度に座標変換される。この基準座標軸加速度から速度計算部17で各軸の速度が計算され、更に位置計算部18で各軸上の位置が計算される。速度計算部17よりの速度と位置計算部18よりの位置とが航法座標軸回転量計算部19に入力され、航法座標軸回転量計算部19で移動体の移動(トランスポートレート)と地球自転(アースレート)に基づくジャイロデータへの影響(航法座標軸回転量)が計算され、これらの影響が方向余弦行列計算部13においてジャイロデータから除去される。また前記航法座標軸回転量と、速度計算部17の速度とがコリオリ加速度計算部21に入力され、座標軸回転により生じるコリオリ加速度が計算され、これにより速度計算部17に対する補正が行われる。
【0004】
初期アライメント計算部22でジャイロ11、加速度計14が計測する地球自転角速度、地球重力データに基づいて方向余弦行列初期値、速度初期値が計算され、それぞれ方向余弦行列計算部13、速度計算部17に設定される。
方向余弦行列計算部13においては、時刻nにおける方向余弦行列Cn が、時刻n−1での方向余弦行列Cn-1 に対し、次式に示す方向余弦行列増分ΔCn を掛算して計算される。
【0005】
Cn =Cn-1 ・ΔCn …(1)
ΔCn =I+((sin Φ)/Φ)[Φm ×]+((1−cos Φ)/Φ2 )[Φm ×]2 …(2)
Iは単位行列、
Φm =[Φx Φy Φz ]T …(3)
Φi はi軸(i=x,y,z)ジャイロデータ、[ ]T は行列の転置、
【0006】
【数1】
【0007】
方向余弦行列Cn の計算結果が得られるごとに、そのCn の行列の要素を用いて姿勢角計算部23で次の計算をして出力する。
ロール角=tan -1(C32/C33)
ピッチ角=tan -1(C31/√(1−C31 2 ))又はsin -1(−C31)
方位角=tan -1(C21/C11)
【0008】
【発明が解決しようとする課題】
方向余弦行列計算部13において方向余弦行列増分ΔCn の計算におけるsin ,cos を含む計算法を、下記に示すように比較的高次のテーラー展開で展開した近似式により行っている。
(sin Φ)/Φ≒1−(Φ/3!)2 +(Φ/5!)4 …(6)
(1−cos Φ)/Φ2 ≒(1/2!)−(Φ2 /4!) …(7)
方向余弦行列は積分系であるため、近似計算に基づく計算誤差の増大をおさえるように、従来においては比較的高次の近似式を用いていた。よって計算量が多くなり、比較的高い精度で姿勢角及び方位角を高速に計算することが困難であった。
【0009】
【課題を解決するための手段】
この発明によれば、第1演算部において比較的次数が大きい近似式を用いて姿勢基準、例えば方向余弦行列が比較的遅い速度で計算され、第2演算部において、第1演算部よりも次数が小さい近似式を用いて姿勢基準が第1演算部より高速に計算され、その姿勢基準にもとづき移動体の姿勢角が計算され、演算誤差補正部により、第1演算部で姿勢基準計算の結果が得られるごとに、その結果が第2演算部での姿勢基準計算ごとの初期値として設定される。その第2演算部で姿勢基準計算結果が得られるごとにそれを用いて移動体の姿勢角を計算する。
【0010】
特に、第1演算部にジャイロ出力が入力されてから、その姿勢基準の計算結果が得られ、初期値設定するまでに、第2演算部での姿勢基準計算結果の各前回の計算結果に対する変動分の総てを求め、この総ての変動分で前回の第1演算部の姿勢基準計算結果に補正演算した値が第2演算部の各姿勢基準の初期値として用いられる。
【0011】
【発明の実施の形態】
図1にこの発明の実施例を示し、図3と対応する部分に同一番号を付けてある。ジャイロデータ補正計算部12、方向余弦行列計算部13、加速度計補正計算部15、座標変換計算部16、速度計算部17、位置計算部18、航法座標軸回転量計算部19、コリオリ加速度計算部21により第1演算部31が構成される。第1演算部31は少なくともジャイロ11よりのジャイロデータが入力され、比較的高次の近似式により高い精度で方向余弦行列、つまり姿勢基準を計算する。
【0012】
この発明では第2演算部32が設けられ、ジャイロ11からジャイロデータが入力され、第1演算部31で行う方向余弦行列計算よりも、次数が小さい近似式により第2方向余弦行列の計算が、第2方向余弦行列計算部33で第1演算部31の方向余弦行列計算の繰返しよりも速く繰返される。
更に演算誤差補正部34が設けられ、第1演算部31の方向余弦行列計算部13にジャイロデータが入力されるごとに、その入力から、その方向余弦行列計算部13による方向余弦行列計算結果が得られるまでに、第2方向余弦行列計算部33で計算された方向余弦行列の変動分の総てに対応した値が変動分検出部35で計算され、方向余弦行列計算部13で計算された方向余弦行列が、変動分検出部35からの変動分の総てに対応した値で補正演算部36において補正演算され、その演算結果が第2方向余弦行列計算部33の第2方向余弦行列計算ごとの初期値として設定される。第2方向余弦行列計算部33で計算された第2方向余弦行列を用いて姿勢角計算部23により姿勢角が計算される。
【0013】
例えば図2に示すように、方向余弦行列計算部13では周期T1 ごとに方向余弦行列の計算を、例えば従来の技術の項で説明したように、式(1)を、式(6)及び式(7)の近似式を用いて計算する。一方第2方向余弦行列計算部33では周期T1 /4ごとに、方向余弦行列の計算を次式により行う。
Bm =Bm-1 ・ΔBm …(8)
ΔBm =I+[Φm ×]+(1/2)・[Φm ×]2 …(9)
Bm は時刻mでの第2方向余弦行列
Bm-1 は時刻m-1 での第2方向余弦行列
ΔBm は第2方向余弦行列の変化量
Φm は式(3)と、[Φm ×]は式(5)と同一である。つまり
(sin Φ)/Φ≒1 …(10)
(1−cos Φ)/Φ2≒1/2! …(11)
なるテーラー展開の各1次の近次式を用いて式(8)を計算する。このように次数が小さい近次式を用いるため、第2方向余弦行列Bm の計算量が少なく、短時間でBm を計算することができる。
【0014】
図2では方向余弦行列計算部13の計算と第2方向余弦行列計算部33の計算とを共通のCPU(中央処理ユニット)で行うようにさせ、同時にジャイロデータが入力された場合は、第2方向余弦行列計算部33の計算を優先させた場合である。図中の各斜線を施している部分が、それぞれ計算を行っている期間を示す。方向余弦行列計算部13と第2方向余弦行列計算部33への各ジャイロデータの入力は同期しているが、前者の入力周期はT1 であり後者の入力周期はT1 /4である。従って例えば時刻nにジャイロデータが両計算部13と33に同時に入力され、第2方向余弦行列計算部33はそのT1 /4前に計算された第2方向余弦行列B0=B4を初期値として増分ΔB1が掛算され、第2方向余弦行列B1=B0・ΔB1が計算される。時刻n=n0 からT1 /4経過したサブ時刻n1 においてもB1を初期値として増分ΔB2が掛算され、第2方向余弦行列B2=B1・ΔB2(=B0・ΔB1・ΔB2)が計算される。この第2方向余弦行列の前回の計算値に対する変動分ΔB2が変動分検出部35で計算される。
【0015】
次のサブ時刻n2 において第2方向余弦行列が計算されるが、サブ時刻n2 の前に、方向余弦行列計算部13で、時刻nに入力されたジャイロデータに対する方向余弦行列の計算結果Cn =Cn-1 ・ΔCn が得られているから、その演算結果Cnが、変動分検出部35で求めた、時刻nからn1 までの変動分の総て、この例ではΔB2で補正演算部36において補正演算され、この演算結果値Cn・ΔB2が第2方向余弦行列計算部33にその計算の初期値として設定される。つまり第2方向余弦行列計算部33で計算された第2方向余弦行列B2はCn・ΔB2に補正される。このようにして時刻nに入力されたデータに基づく精度の高い演算結果Cn が、時刻nに入力されたデータに基づき計算された第2方向余弦行列B1に置きかえられたことになる。
【0016】
よってサブ時刻n2 における第2方向余弦行列計算は補正されたB2を初期値としてB3=B2・ΔB3(=Cn ・ΔB2・ΔB3)が計算される。このようにして第2方向余弦行列計算部33は周期T1 /4ごとに次数の小さい近似式で第2方向余弦行列計算を行うが、その計算の初期値が、周期T1 ごとに方向余弦行列計算部13で次数の高い近似式で計算された精度の高い方向余弦行列によって補正されるため、周期T1 /4ごとに比較的高い精度の姿勢角を得ることができる。ちなみに、400°/sで1軸回転しているときの1/(200Hz)間の回転軸の角度誤差は下記の通りである。
【0017】
上記条件は通常はない大きな値であるが、近似次数と誤差はこのような関係にあり、例えば方向余弦行列Cn の計算に4次の近似式を用いて誤差は、周期が1/(50Hz)であれば、10-8度のオーダという高い精度の値が得られ、第2方向余弦行列Bm の計算に1次の近似式を用いてもサブ時刻周期を1/(200Hz)とした場合誤差が10-4度のオーダの精度となるが、この誤差が積分されるのは、方向余弦行列Cn の計算周期の間だけで、Cn が得られるごとにBm の計算の初期値がCn により補正され、結果として常に比較的高い精度の姿勢角が高速(短かい周期)で得られる。
【0018】
なお姿勢角の計算は第2方向余弦行列
【0019】
【数2】
【0020】
に対し、ロール角=tan -1(B32/B33)
ピッチ角=tan -1(B31/√(1−B31 2 ))又はsin -1(−B31)
方位角=tan -1(B21/B11)
で行うことは従来と同様である。
方向余弦行列Cn の計算周期は例えば1/(50Hz)、第2方向余弦行列Bm の計算周期は例えば1/(200Hz)が考えられるが、これらの値は任意に選ぶことができ、少くとも、方向余弦行列Cn の計算周期に対し、第2方向余弦行列Bm の計算周期は1/2以下であればよい。方向余弦行列Cn の計算周期に対し、第2方向余弦行列Bm の計算周期は整数分の1が好ましい。かつ方向余弦行列計算部13はジャイロデータを取込む際に、第2方向余弦行列計算部33にジャイロデータを取込むような周期関係をもたせると同時にジャイロデータが入力されることが好ましいが、必ずしもその必要はない。方向余弦行列Cn が得られた時は、その計算のためのジャイロデータを取込んだ時刻に近い、サブ時刻での第2方向余弦行列Bm をCn で置きかえたと等価になるように補正すればよい。また方向余弦行列の計算結果Cn が得られると直に第2方向余弦行列計算部33の初期値を設定する必要もない。つまり方向余弦行列Cn を計算するためにジャイロデータを取込んだ時刻から、その計算結果Cn が得られた後の適当な時刻(サブ時刻)までの第2方向余弦行列Bm の変動分の総てを用いてCn に対し補正演算してその演算結果を、次の第2方向余弦行列Bm の計算の初期値(直前に得られた第2方向余弦行列Bm-1 )とすればよい。
【0021】
方向余弦行列Cn の計算、第2方向余弦行列Bm の計算にそれぞれ用いる近似式は前記例に限らず前者に次数が3〜6次の高次のテーラー展開式を用い、後者は次数が1〜2次の低次のテーラー展開式を用いることができるが、前者の次数より後者の次数は必ず低くすればよい。近似式もテーラー展開式に限らず、他の近似式を用いてもよい。
ジャイロデータ補正計算部には、比較的精度が悪いジャイロ11を使用しても、その補正は例えば0.1°/1時間程度である1秒間当りの補正は0.000027°/秒程度の補正となり、第2方向余弦行列計算部33における計算ごとの初期値が、方向余弦行列計算部13で得られた高い精度の計算結果Cn により周期的に補正されるため、第2方向余弦行列計算部33へ供給するジャイロデータに対してはジャイロデータ補正計算を行わなくても十分であるが、必要に応じて行ってもよい。同様に航法座標軸回転量計算部19によるジャイロデータに対する補正は長い周期で行えばよく、従ってこの補正は、第2方向余弦行列計算部33に入力するジャイロデータに対しては行わなくてもよいが、必要に応じて行ってもよい。
【0022】
なお上述では姿勢基準を方向余弦行列により求めたが、コータニオン方法による場合もsin,cosの関数を含む計算であるため、この発明を適用して、高い精度で、かつ短かい周期で姿勢角を測定することができる。
【0023】
【発明の効果】
以上述べたようにこの発明によれば第1演算部により高次の近似式を用いて高い精度の姿勢基準を計算すると共に、第1演算部よりも次数が低い近似式を用いて第2演算部でも姿勢基準を計算し、この計算は用いる近似式の次数が小さいため短時間に計算することができ、従って、短かい周期で姿勢基準を得ることができ、この第2演算部のその計算ごとの初期値を、第1演算部で高い精度の姿勢基準が得られるごとに補正するため、結果として、可成り精度の高い姿勢角を短かい周期で得ることができる。従って演算器(一般にCPU)として比較的低速度の安価なものを使用することができ、全体としても安価に構成することができる。このように高速度に姿勢角、方位角が得られるため、これらを用いるシステムに対し、時間遅れによる誤差などを低減することができる。
【図面の簡単な説明】
【図1】この発明の実施例の機能構成を示すブロック図。
【図2】図1に示した実施例における方向余弦行列の計算タイミングと更新タイミングの例を示すタイムチャート。
【図3】従来の姿勢計測装置を示すブロック図。
Claims (2)
- 移動体の角速度を検出するジャイロと、
上記ジャイロの出力を用いて、方向余弦行列増分ΔC n の計算における正弦と余弦三角関数を含む計算をn次関数の近似演算により、周波数mHzで姿勢基準計算を行う第1演算部と、
上記ジャイロの出力を用いて、上記nより小さい次数のp次の近似演算により、mHzより大きい周波数rHzで姿勢基準計算を行って移動体の姿勢角を計測する第2演算部と、
上記第1演算部で方向余弦行列C n が計算されるごとに、その方向余弦行列C n に第2演算部で計算した方向余弦行列の変動分ΔBmを乗算して補正する演算誤差補正部と
を具備する姿勢計測装置。 - 上記演算誤差補正部は、上記第1演算部での姿勢基準の計算に用いたジャイロ出力が入力されてから、上記第1演算部での上記姿勢基準の計算結果が得られ、上記初期設定するまでの、上記第2演算部における姿勢基準計算結果の前回の計算値に対する変動分ΔBmを求める変動分検出部と、その変動分ΔBmを上記第1演算部の方向余弦行列に補正演算して上記第2演算部の方向余弦行列の初期値として設定する補正演算部とよりなることを特徴とする請求項1記載の姿勢計測装置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2000088628A JP4287975B2 (ja) | 2000-03-28 | 2000-03-28 | 姿勢計測装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2000088628A JP4287975B2 (ja) | 2000-03-28 | 2000-03-28 | 姿勢計測装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2001280970A JP2001280970A (ja) | 2001-10-10 |
JP4287975B2 true JP4287975B2 (ja) | 2009-07-01 |
Family
ID=18604482
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2000088628A Expired - Fee Related JP4287975B2 (ja) | 2000-03-28 | 2000-03-28 | 姿勢計測装置 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP4287975B2 (ja) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR101250257B1 (ko) | 2011-02-28 | 2013-04-04 | 국방과학연구소 | 관성 측정기의 교정 장치 및 그 방법 |
JP6255924B2 (ja) | 2013-11-11 | 2018-01-10 | セイコーエプソン株式会社 | センサー用ic、センサーデバイス、電子機器及び移動体 |
CN113137983B (zh) * | 2021-04-30 | 2023-08-22 | 深圳市恒星物联科技有限公司 | 一种自学习的井盖姿态监测方法及监测系统 |
-
2000
- 2000-03-28 JP JP2000088628A patent/JP4287975B2/ja not_active Expired - Fee Related
Also Published As
Publication number | Publication date |
---|---|
JP2001280970A (ja) | 2001-10-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US6859727B2 (en) | Attitude change kalman filter measurement apparatus and method | |
CN107655493B (zh) | 一种光纤陀螺sins六位置系统级标定方法 | |
JP5243956B2 (ja) | リアルタイムバイアス推定器に基づく慣性機器のための自己較正 | |
EP1970669B1 (en) | Self-calibration of scale factor for dual resonator class II coriolis vibratory gyros | |
US6876926B2 (en) | Method and system for processing pulse signals within an inertial navigation system | |
CN100547352C (zh) | 适合于光纤陀螺捷联惯性导航系统的地速检测方法 | |
CN103429986B (zh) | 惯性导航划桨算法 | |
RU2509980C2 (ru) | Калибровка вибрационного гироскопа | |
CN102224395B (zh) | 带有振动陀螺仪的陀螺仪系统的标定 | |
CN105352527B (zh) | 一种基于双轴转位机构光纤陀螺标定方法 | |
JPS5936208B2 (ja) | 航空機の慣性台を迅速に整列させる方法および装置 | |
CN109489661B (zh) | 一种卫星初始入轨时陀螺组合常值漂移估计方法 | |
CN105606093B (zh) | 基于重力实时补偿的惯性导航方法及装置 | |
US8510079B2 (en) | Systems and methods for an advanced pedometer | |
CN111912427B (zh) | 一种多普勒雷达辅助捷联惯导运动基座对准方法及系统 | |
JP2004264240A (ja) | 慣性装置のミスアライメント計測方法 | |
JP4287975B2 (ja) | 姿勢計測装置 | |
US8725415B2 (en) | Method and device for long-duration navigation | |
CN107228683B (zh) | 一种多星敏感器间慢变误差实时在轨修正方法 | |
RU2502049C1 (ru) | Малогабаритная бесплатформенная инерциальная навигационная система средней точности, корректируемая от системы воздушных сигналов | |
RU2092402C1 (ru) | Способ калибровки гироинерциальных измерителей бесплатформенной инерционной навигационной системы ориентации космического аппарата | |
JPH0949737A (ja) | 航法信号出力方法 | |
CN112327333A (zh) | 一种卫星位置计算方法及装置 | |
RU2049311C1 (ru) | Способ определения коэффициентов модели инструментальных погрешностей навигационной системы | |
Zheng et al. | Influences of gyro biases on ship angular flexure measurement |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20051026 |
|
RD03 | Notification of appointment of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7423 Effective date: 20051026 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20081111 |
|
A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20090109 |
|
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: 20090324 |
|
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20090330 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120403 Year of fee payment: 3 |
|
R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 Ref document number: 4287975 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120403 Year of fee payment: 3 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120403 Year of fee payment: 3 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130403 Year of fee payment: 4 |
|
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: 20130403 Year of fee payment: 4 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130403 Year of fee payment: 4 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20140403 Year of fee payment: 5 |
|
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 |
|
LAPS | Cancellation because of no payment of annual fees |