JP2904118B2 - 姿勢角検出装置及び方法 - Google Patents
姿勢角検出装置及び方法Info
- Publication number
- JP2904118B2 JP2904118B2 JP8132249A JP13224996A JP2904118B2 JP 2904118 B2 JP2904118 B2 JP 2904118B2 JP 8132249 A JP8132249 A JP 8132249A JP 13224996 A JP13224996 A JP 13224996A JP 2904118 B2 JP2904118 B2 JP 2904118B2
- Authority
- JP
- Japan
- Prior art keywords
- moving body
- angle
- component
- axis
- attitude 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
- 238000001514 detection method Methods 0.000 title claims description 23
- 238000000034 method Methods 0.000 title claims description 14
- 230000005484 gravity Effects 0.000 claims description 57
- 230000001133 acceleration Effects 0.000 claims description 26
- 230000033001 locomotion Effects 0.000 claims description 19
- 230000003044 adaptive effect Effects 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 241000287463 Phalacrocorax Species 0.000 claims 2
- 230000010354 integration Effects 0.000 description 12
- 238000010586 diagram Methods 0.000 description 7
- 230000008859 change Effects 0.000 description 4
- 238000000605 extraction Methods 0.000 description 3
- 238000005259 measurement Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 239000011159 matrix material Substances 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 230000004069 differentiation Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000002123 temporal effect Effects 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
Landscapes
- Gyroscopes (AREA)
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Description
【０００１】
【発明の属する技術分野】本発明は姿勢角検出装置及び
方法に関し、特に運動体の実時間での姿勢角検出装置及
び方法に関する。
方法に関し、特に運動体の実時間での姿勢角検出装置及
び方法に関する。
【０００２】
【従来の技術】飛行機や自動車等の運動体の姿勢角は、
運動体の姿勢、運動方向、速度等を制御するためのパラ
メータとして重要であり、その姿勢角の高精度検出は重
要である。
運動体の姿勢、運動方向、速度等を制御するためのパラ
メータとして重要であり、その姿勢角の高精度検出は重
要である。
【０００３】従来、姿勢角への高精度検出のためには、
ジャイロ装置が用いられるが、ジャイロ装置は高価であ
るため、安価な姿勢角検出装置として傾斜計を用いて重
力に対する傾斜角を測定し、得られた傾斜角と向きによ
り姿勢を判定する装置が実用化されている。この姿勢角
検出装置では、運動体が水平方向に動き始めるときと、
運動状態から停止状態に至るときに傾斜計が加速度を検
出してしまい誤った姿勢情報を出力してしまう。そのた
め、傾斜計の出力をローパスフィルタに通すことにより
加速度成分を除去して姿勢変化だけを検出している。し
かし、ローパスフィルタは、一次遅れ特性を持つため、
上記装置では姿勢変化を迅速に検出することができない
という問題がある。
ジャイロ装置が用いられるが、ジャイロ装置は高価であ
るため、安価な姿勢角検出装置として傾斜計を用いて重
力に対する傾斜角を測定し、得られた傾斜角と向きによ
り姿勢を判定する装置が実用化されている。この姿勢角
検出装置では、運動体が水平方向に動き始めるときと、
運動状態から停止状態に至るときに傾斜計が加速度を検
出してしまい誤った姿勢情報を出力してしまう。そのた
め、傾斜計の出力をローパスフィルタに通すことにより
加速度成分を除去して姿勢変化だけを検出している。し
かし、ローパスフィルタは、一次遅れ特性を持つため、
上記装置では姿勢変化を迅速に検出することができない
という問題がある。
【０００４】そこで、特開平2ー161504号公報では、移動
体の重心に対して対称に一対の傾斜計を設け、この一対
の傾斜計の出力差を求めて重心を中心とする回転角加速
度を求め、この角加速度信号を第１のローパスフィルタ
に与えて低域成分を取り出している。更に、一対の傾斜
計の何れか一方の出力から第２のローパスフィルタによ
り低域成分を取り出し、角加速度信号の低域成分と、一
方の傾斜計の出力の低域成分を加算することにより一次
遅れが除去された姿勢検出信号を得ている。
体の重心に対して対称に一対の傾斜計を設け、この一対
の傾斜計の出力差を求めて重心を中心とする回転角加速
度を求め、この角加速度信号を第１のローパスフィルタ
に与えて低域成分を取り出している。更に、一対の傾斜
計の何れか一方の出力から第２のローパスフィルタによ
り低域成分を取り出し、角加速度信号の低域成分と、一
方の傾斜計の出力の低域成分を加算することにより一次
遅れが除去された姿勢検出信号を得ている。
【０００５】また、かかる従来の姿勢角検出装置より簡
易に姿勢角の検出を行う装置として、角速度計の出力を
直接積分する姿勢角検出装置も従来から用いられてい
る。
易に姿勢角の検出を行う装置として、角速度計の出力を
直接積分する姿勢角検出装置も従来から用いられてい
る。
【０００６】図５には、角速度計の出力を直接積分する
姿勢角検出装置を用いた姿勢角制御装置／ナビゲーショ
ン装置の制御方式の構成ブロック図が示されている。角
速度計１０で検出された運動体の角速度は、角速度積分
手段２０で角速度積分の初期値を基にして数値的に積分
されて姿勢角が得られる。姿勢角制御装置／ナビゲーシ
ョン装置３０は、得られた姿勢角情報を基にナビゲーシ
ョン等の所定の動作を行う。また得られた姿勢角は角速
度積分手段２０の初期値として入力される。
姿勢角検出装置を用いた姿勢角制御装置／ナビゲーショ
ン装置の制御方式の構成ブロック図が示されている。角
速度計１０で検出された運動体の角速度は、角速度積分
手段２０で角速度積分の初期値を基にして数値的に積分
されて姿勢角が得られる。姿勢角制御装置／ナビゲーシ
ョン装置３０は、得られた姿勢角情報を基にナビゲーシ
ョン等の所定の動作を行う。また得られた姿勢角は角速
度積分手段２０の初期値として入力される。
【０００７】
【発明が解決しようとする課題】以上のように従来の姿
勢角検出装置としては、傾斜計を用いる装置と、角速度
計の出力を積分する装置が実用化されているが、それぞ
れ次のような課題を抱えており、高精度姿勢検出の要望
に応えるものとはなっていない。
勢角検出装置としては、傾斜計を用いる装置と、角速度
計の出力を積分する装置が実用化されているが、それぞ
れ次のような課題を抱えており、高精度姿勢検出の要望
に応えるものとはなっていない。
【０００８】すなわち、傾斜計を用いる装置は、運動体
の重心に対して傾斜計を対称配置しなければならないた
め、ハードウェア構成等に影響を与えてしまう。また、
角速度計を使用せずに加速時計と同じ原理で機能する傾
斜計の出力の差分から姿勢変化情報を得るのは、物理的
に無理があり、構成が複雑化する。
の重心に対して傾斜計を対称配置しなければならないた
め、ハードウェア構成等に影響を与えてしまう。また、
角速度計を使用せずに加速時計と同じ原理で機能する傾
斜計の出力の差分から姿勢変化情報を得るのは、物理的
に無理があり、構成が複雑化する。
【０００９】更に、角速度計の出力を積分する装置で
は、角速度計の出力が周囲環境変動の影響を受けるため
出力誤差が生じる恐れがある。また、角速度の数値積分
による数値誤差が発生する恐れがある。更に、両者の誤
差を含んだ姿勢角が次のステップでの積分にフィードバ
ックされて誤差が累積され、姿勢角誤差が時間とともに
増大してしまう。
は、角速度計の出力が周囲環境変動の影響を受けるため
出力誤差が生じる恐れがある。また、角速度の数値積分
による数値誤差が発生する恐れがある。更に、両者の誤
差を含んだ姿勢角が次のステップでの積分にフィードバ
ックされて誤差が累積され、姿勢角誤差が時間とともに
増大してしまう。
【００１０】すなわち、従来の姿勢角検出装置では、角
速度出力が誤差を含んでいること及び角速度を数値的に
積分することによる演算誤差の両者により姿勢角の誤差
が時間と共に増加し検出精度が劣化するという問題点が
ある。
速度出力が誤差を含んでいること及び角速度を数値的に
積分することによる演算誤差の両者により姿勢角の誤差
が時間と共に増加し検出精度が劣化するという問題点が
ある。
【００１１】そこで、本発明の目的は、上述従来の問題
を解決し、簡単な構成で姿勢角を実時間で高精度に検出
できる姿勢角検出装置及び方法を提供することにある。
を解決し、簡単な構成で姿勢角を実時間で高精度に検出
できる姿勢角検出装置及び方法を提供することにある。
【００１２】
【課題を解決するための手段】前述の課題を解決するた
め、本発明では、他のハードウェアを追加することな
く、運動体に搭載されている加速度計、角速度計から成
る運動測定系をそのまま活用し、姿勢角の誤差を含んだ
重力ベクトルと参照重力ベクトルとを比較することによ
り、姿勢角の誤差を実時間で測定している。すなわち、
本発明では、ストラップダウン型慣性測定系の加速度出
力から姿勢角の誤差だけを陰に含んだ重力成分を抽出し
た見掛けの重力成分と地球上では既知量である重力ベク
トルとの差により姿勢角の誤差補正を行い、姿勢角の検
出精度を向上している。
め、本発明では、他のハードウェアを追加することな
く、運動体に搭載されている加速度計、角速度計から成
る運動測定系をそのまま活用し、姿勢角の誤差を含んだ
重力ベクトルと参照重力ベクトルとを比較することによ
り、姿勢角の誤差を実時間で測定している。すなわち、
本発明では、ストラップダウン型慣性測定系の加速度出
力から姿勢角の誤差だけを陰に含んだ重力成分を抽出し
た見掛けの重力成分と地球上では既知量である重力ベク
トルとの差により姿勢角の誤差補正を行い、姿勢角の検
出精度を向上している。
【００１３】本発明による姿勢角検出装置は、運動体の
角速度を測定して運動体固定軸上の角速度を出力する角
速度測定手段と、該角速度測定手段の出力を積分して現
在の運動体姿勢角を出力する角速度積分手段と、前記運
動体の加速度を測定して運動体固定軸上の加速度を出力
する加速度測定手段と、該加速度測定手段の出力から運
動体固定軸上での重力成分を抽出する重力成分抽出手段
と、運動体固定軸上のベクトル成分を基準軸上のベクト
ル成分に変換する座標変換手段と、基準軸上において、
姿勢角誤差を有する重力成分と参照重力成分とを比較す
ることにより姿勢角誤差を測定する姿勢角誤差算出手段
と、姿勢角誤差を用いて姿勢角の補正を行う姿勢角補正
手段と、を備えて構成される。
角速度を測定して運動体固定軸上の角速度を出力する角
速度測定手段と、該角速度測定手段の出力を積分して現
在の運動体姿勢角を出力する角速度積分手段と、前記運
動体の加速度を測定して運動体固定軸上の加速度を出力
する加速度測定手段と、該加速度測定手段の出力から運
動体固定軸上での重力成分を抽出する重力成分抽出手段
と、運動体固定軸上のベクトル成分を基準軸上のベクト
ル成分に変換する座標変換手段と、基準軸上において、
姿勢角誤差を有する重力成分と参照重力成分とを比較す
ることにより姿勢角誤差を測定する姿勢角誤差算出手段
と、姿勢角誤差を用いて姿勢角の補正を行う姿勢角補正
手段と、を備えて構成される。
【００１４】本発明の他の態様による姿勢角検出方法
は、運動体が静止している状態か運動している状態かの
判断を行うステップと、前記運動体が静止していると判
断した場合、運動体姿勢角の初期値を算出するステップ
と、前記運動体の角速度を積分して前記運動体の姿勢角
を求めるステップと、前記運動体の固定軸のｘ軸、ｙ軸
及びｚ軸方向のそれぞれの加速度成分と重力成分を用い
て重力成分の抽出を行うステップと、前記運動体の姿勢
角を用いて、前記運動体の固定軸上の重力成分を基準軸
上の重力成分へ変換するステップと、前記基準軸上の重
力成分と基準軸上の参照重力成分に基づいて前記運動体
の姿勢角誤差を推定するステップと、前記姿勢角誤差に
基づいて前記運動体姿勢角の補正を行い、補正された運
動体姿勢角を前記運動体姿勢角の初期値とするステップ
と、を備えて構成される。
は、運動体が静止している状態か運動している状態かの
判断を行うステップと、前記運動体が静止していると判
断した場合、運動体姿勢角の初期値を算出するステップ
と、前記運動体の角速度を積分して前記運動体の姿勢角
を求めるステップと、前記運動体の固定軸のｘ軸、ｙ軸
及びｚ軸方向のそれぞれの加速度成分と重力成分を用い
て重力成分の抽出を行うステップと、前記運動体の姿勢
角を用いて、前記運動体の固定軸上の重力成分を基準軸
上の重力成分へ変換するステップと、前記基準軸上の重
力成分と基準軸上の参照重力成分に基づいて前記運動体
の姿勢角誤差を推定するステップと、前記姿勢角誤差に
基づいて前記運動体姿勢角の補正を行い、補正された運
動体姿勢角を前記運動体姿勢角の初期値とするステップ
と、を備えて構成される。
【００１５】ここで、前記運動体姿勢角としてオイラー
角が用いられ、前記運動体姿勢角の初期値としては、傾
斜計として機能する加速度計の出力が用いられ、前記重
力成分推定は、そのカットオフ周波数が前記運動体の姿
勢変動を伴う運動のうち最も周波数の高い運動よりは高
い周波数に設定されているローパスフィルタを用いるこ
とにより抽出される。このローパスフィルタは、カット
オフ周波数を適宜切替えることができるような適応フィ
ルタとすることもできる。
角が用いられ、前記運動体姿勢角の初期値としては、傾
斜計として機能する加速度計の出力が用いられ、前記重
力成分推定は、そのカットオフ周波数が前記運動体の姿
勢変動を伴う運動のうち最も周波数の高い運動よりは高
い周波数に設定されているローパスフィルタを用いるこ
とにより抽出される。このローパスフィルタは、カット
オフ周波数を適宜切替えることができるような適応フィ
ルタとすることもできる。
【００１６】
【発明の実施の形態】次に、本発明の実施形態例を図面
を参照しながら説明する。図１は本発明による姿勢角検
出装置の一実施形態例を示す構成ブロック図である。ま
た、図２には、以下で使用される運動体固定軸及び基準
軸との関係が示されている。運動体固定軸は、運動体に
剛に固定された仮想的な軸であり、基準軸は運動体の姿
勢角制御装置及びナビゲーション装置が基準とする、例
えば、地表面に固定された仮想的な軸であり、運動体姿
勢角はこれら両者の軸の角度関係で定義される。
を参照しながら説明する。図１は本発明による姿勢角検
出装置の一実施形態例を示す構成ブロック図である。ま
た、図２には、以下で使用される運動体固定軸及び基準
軸との関係が示されている。運動体固定軸は、運動体に
剛に固定された仮想的な軸であり、基準軸は運動体の姿
勢角制御装置及びナビゲーション装置が基準とする、例
えば、地表面に固定された仮想的な軸であり、運動体姿
勢角はこれら両者の軸の角度関係で定義される。
【００１７】本実施形態による姿勢角検出装置は、従来
と同様な角速度計１０、角速度積分手段２０及び姿勢角
制御装置／ナビゲーション装置３０の他に、加速度計４
０、重力成分推定手段５０、座標変換手段６０、姿勢角
誤差算出手段７０及び姿勢角補正手段８０を備えてい
る。
と同様な角速度計１０、角速度積分手段２０及び姿勢角
制御装置／ナビゲーション装置３０の他に、加速度計４
０、重力成分推定手段５０、座標変換手段６０、姿勢角
誤差算出手段７０及び姿勢角補正手段８０を備えてい
る。
【００１８】角速度計１０は、運動体の角速度を検出し
て運動体固定軸上角速度を出力する。角速度積分手段２
０は、角速度計１０から出力される運動体固定軸上角速
度を受け、後述する姿勢角補正手段８０から出力される
１ステップ前の運動体姿勢角を初期値として角速度積分
処理を実行して現在の運動体姿勢角を算出する。
て運動体固定軸上角速度を出力する。角速度積分手段２
０は、角速度計１０から出力される運動体固定軸上角速
度を受け、後述する姿勢角補正手段８０から出力される
１ステップ前の運動体姿勢角を初期値として角速度積分
処理を実行して現在の運動体姿勢角を算出する。
【００１９】また、加速度計４０は、運動体の加速度を
検出して運動体固定軸上加速度を出力する。重力推定手
段５０は、加速度計４０から出力される運動体固定軸上
での重力成分の推定を行い、運動体固定軸上推定重力成
分を出力する。すなわち、重力推定手段５０は、例え
ば、ストラップダウン型慣性測定系の加速度出力を姿勢
角の変動を伴う運動体の固有運動のうち最も周波数の高
い固有運動の周波数よりは高いカットオフ周波数を有す
るローパスフィルタで処理することによって運動体固定
軸上の重力成分を推定する。
検出して運動体固定軸上加速度を出力する。重力推定手
段５０は、加速度計４０から出力される運動体固定軸上
での重力成分の推定を行い、運動体固定軸上推定重力成
分を出力する。すなわち、重力推定手段５０は、例え
ば、ストラップダウン型慣性測定系の加速度出力を姿勢
角の変動を伴う運動体の固有運動のうち最も周波数の高
い固有運動の周波数よりは高いカットオフ周波数を有す
るローパスフィルタで処理することによって運動体固定
軸上の重力成分を推定する。
【００２０】座標変換手段６０は、重力成分推定手段５
０から出力される運動体固定軸上推定重力成分と角速度
積分手段２０から出力される運動体姿勢角を用いて基準
軸上疑似重力成分を算出する。座標変換手段６０は、例
えば、姿勢角により記述される運動体固定軸上の重力ベ
クトル量を基準軸上の重力ベクトル量へ変換を行う方向
余弦行列を用いて重力成分推定手段５０より推定された
運動体固定軸上の重力成分を基準軸上への変換を行う。
０から出力される運動体固定軸上推定重力成分と角速度
積分手段２０から出力される運動体姿勢角を用いて基準
軸上疑似重力成分を算出する。座標変換手段６０は、例
えば、姿勢角により記述される運動体固定軸上の重力ベ
クトル量を基準軸上の重力ベクトル量へ変換を行う方向
余弦行列を用いて重力成分推定手段５０より推定された
運動体固定軸上の重力成分を基準軸上への変換を行う。
【００２１】姿勢角誤差算出手段７０は、座標変換手段
６０から出力される基準軸上疑似重力成分と基準軸上参
照重力成分とを比較することにより姿勢角誤差を算出す
る。例えば、既知量である基準軸上の重力成分と見掛け
の重力成分との方向余弦により姿勢角誤差を算出する。
６０から出力される基準軸上疑似重力成分と基準軸上参
照重力成分とを比較することにより姿勢角誤差を算出す
る。例えば、既知量である基準軸上の重力成分と見掛け
の重力成分との方向余弦により姿勢角誤差を算出する。
【００２２】姿勢角補正手段８０は、姿勢角誤差算出手
段７０から出力される姿勢角誤差を用いて、角速度積分
手段２０から出力される運動体姿勢角の補正を行い、運
動体推定姿勢角を算出する。姿勢角制御装置／ナビゲー
ション装置３０は、姿勢角補正手段８０から出力される
運動体推定姿勢角を基にして所定の動作を実行する。
段７０から出力される姿勢角誤差を用いて、角速度積分
手段２０から出力される運動体姿勢角の補正を行い、運
動体推定姿勢角を算出する。姿勢角制御装置／ナビゲー
ション装置３０は、姿勢角補正手段８０から出力される
運動体推定姿勢角を基にして所定の動作を実行する。
【００２３】次に、上述実施形態の具体的処理手順を図
３のフローチャートを参照して説明する。先ず、運動体
が静止している状態か運動している状態かの判断、初期
状態の判断を行う（ステップＳ１）。この判断は、例え
ば、加速度計４０の出力の履歴をメモリに蓄えておき、
仮に３０秒以上変化がなければ静止しているとみなすこ
とにより行うことができる。ステップＳ１において、運
動体が静止していると判断した場合、運動体姿勢角の初
期値を算出する（ステップＳ２）。この運動体姿勢角の
初期値としては、傾斜計として機能している加速度計４
０の出力を用いることができる。次に、ステップＳ４に
おける角速度計１０の出力の数値積分処理を実行するた
めの初期値を更新する（ステップＳ３）。初期値は運動
体姿勢角であり、静止状態であればステップＳ２の処理
により求められた運動体姿勢角を、運動状態であれば後
述するステップＳ９姿勢角の出力処理結果を採用する。
３のフローチャートを参照して説明する。先ず、運動体
が静止している状態か運動している状態かの判断、初期
状態の判断を行う（ステップＳ１）。この判断は、例え
ば、加速度計４０の出力の履歴をメモリに蓄えておき、
仮に３０秒以上変化がなければ静止しているとみなすこ
とにより行うことができる。ステップＳ１において、運
動体が静止していると判断した場合、運動体姿勢角の初
期値を算出する（ステップＳ２）。この運動体姿勢角の
初期値としては、傾斜計として機能している加速度計４
０の出力を用いることができる。次に、ステップＳ４に
おける角速度計１０の出力の数値積分処理を実行するた
めの初期値を更新する（ステップＳ３）。初期値は運動
体姿勢角であり、静止状態であればステップＳ２の処理
により求められた運動体姿勢角を、運動状態であれば後
述するステップＳ９姿勢角の出力処理結果を採用する。
【００２４】続いて、角速度計１０の出力を数値的に積
分する（ステップＳ４）。運動体固定軸のｘ軸、ｙ軸、
ｚ軸まわりのそれぞれの角速度を測定するように運動体
内部に取り付けられている角速度計の現在の出力を
［ｐ，ｑ，ｒ］とし、角速度積分の初期値となる１積分
周期前の運動体姿勢角を［Ψ’，θ’，φ’］、すなわ
ち、一般的にオイラー角の名称で定義されている姿勢角
を採用することにすれば、現在の運動体姿勢角［Ψ，
θ，φ］は次の関係式により記述できる。
分する（ステップＳ４）。運動体固定軸のｘ軸、ｙ軸、
ｚ軸まわりのそれぞれの角速度を測定するように運動体
内部に取り付けられている角速度計の現在の出力を
［ｐ，ｑ，ｒ］とし、角速度積分の初期値となる１積分
周期前の運動体姿勢角を［Ψ’，θ’，φ’］、すなわ
ち、一般的にオイラー角の名称で定義されている姿勢角
を採用することにすれば、現在の運動体姿勢角［Ψ，
θ，φ］は次の関係式により記述できる。
【数１】 ただし、（・）は時間微分を表わす。この微分方程式
は、例えば、数値的解法の一つであるルンゲクッタ法等
により解くことができる。このとき、ステップＳ４の処
理出力である運動体姿勢角には、角速度計の出力誤差に
よる誤差と数値的積分に起因する誤差の両者が陰に含ま
れている。
は、例えば、数値的解法の一つであるルンゲクッタ法等
により解くことができる。このとき、ステップＳ４の処
理出力である運動体姿勢角には、角速度計の出力誤差に
よる誤差と数値的積分に起因する誤差の両者が陰に含ま
れている。
【００２５】次に、運動体固定軸のｘ軸、ｙ軸及びｚ軸
方向のそれぞれの加速度を測定するように運動体内部に
取り付られている加速度計の運動体の加速度成分と重力
成分からなる出力を用いて、重力成分の抽出を行う（ス
テップＳ５）。重力成分の抽出を行うためには、例え
ば、ローパスフィルタを利用することができ、そのカッ
トオフ周波数は、運動体の姿勢変動を伴う運動のうち最
も周波数の高い運動よりは大きく設定すれば良い。ステ
ップＳ５の処理で得られる運動体固定軸上の重力成分
は、真の重力成分と抽出誤差からなる。
方向のそれぞれの加速度を測定するように運動体内部に
取り付られている加速度計の運動体の加速度成分と重力
成分からなる出力を用いて、重力成分の抽出を行う（ス
テップＳ５）。重力成分の抽出を行うためには、例え
ば、ローパスフィルタを利用することができ、そのカッ
トオフ周波数は、運動体の姿勢変動を伴う運動のうち最
も周波数の高い運動よりは大きく設定すれば良い。ステ
ップＳ５の処理で得られる運動体固定軸上の重力成分
は、真の重力成分と抽出誤差からなる。
【００２６】続いて、ステップＳ４の処理の出力である
誤差を有した運動体姿勢角を用いて、ステップＳ５の処
理出力である運動体固定軸上の重力成分［γｘ，γｙ，
γｚ］を基準軸上の重力成分［ηｘ，ηｙ，ηｚ］へ変
換する。
誤差を有した運動体姿勢角を用いて、ステップＳ５の処
理出力である運動体固定軸上の重力成分［γｘ，γｙ，
γｚ］を基準軸上の重力成分［ηｘ，ηｙ，ηｚ］へ変
換する。
【００２７】具体的には、運動体姿勢角により記述され
る運動体固定軸に対する基準軸への方向余弦行列Ｌを用
いて、
る運動体固定軸に対する基準軸への方向余弦行列Ｌを用
いて、
【数２】 により変換できる。運動体姿勢角としてオイラー角を採
用しているならば、
用しているならば、
【数３】 となる。このとき、ステップＳ５の処理の抽出誤差が無
視できるほど小さいとすれば、ステップＳ６の処理の出
力は姿勢角誤差を含んだ基準軸上の重力成分となる。
視できるほど小さいとすれば、ステップＳ６の処理の出
力は姿勢角誤差を含んだ基準軸上の重力成分となる。
【００２８】更に、続いて、ステップＳ６処理の出力で
ある姿勢角誤差を含んだ基準軸上の重力成分と基準軸上
の参照重力成分との比較により、運動体姿勢角の誤差を
推定する。基準軸上の参照重力成分は、例えば、地表面
を基準軸として採用している場合、［０，０，１］（単
位；Ｇ）となる。このとき、運動体姿勢角の誤差［△
ｘ，△ｙ，△ｚ］は、
ある姿勢角誤差を含んだ基準軸上の重力成分と基準軸上
の参照重力成分との比較により、運動体姿勢角の誤差を
推定する。基準軸上の参照重力成分は、例えば、地表面
を基準軸として採用している場合、［０，０，１］（単
位；Ｇ）となる。このとき、運動体姿勢角の誤差［△
ｘ，△ｙ，△ｚ］は、
【数４】 となる。運動体姿勢角としてオイラー角を採用している
場合は、
場合は、
【数５】 となり、△θがθの誤差量、△φがφの誤差量となる。
【００２９】次に、ステップＳ７処理により求められた
姿勢角誤差を用いて、ステップＳ４処理の出力である誤
差を陰に有した運動体姿勢角の補正を行った後（ステッ
プＳ８）、誤差補正が行われた運動体姿勢角を姿勢角制
御装置／ナビゲーション装置３０に出力するとともに、
姿勢角積分初期値の更新データとして出力する（ステッ
プＳ９）。
姿勢角誤差を用いて、ステップＳ４処理の出力である誤
差を陰に有した運動体姿勢角の補正を行った後（ステッ
プＳ８）、誤差補正が行われた運動体姿勢角を姿勢角制
御装置／ナビゲーション装置３０に出力するとともに、
姿勢角積分初期値の更新データとして出力する（ステッ
プＳ９）。
【００３０】図４には、上述本発明を適用した姿勢角検
出装置と、適用しない従来の姿勢角検出装置で生ずる姿
勢角誤差の時間的特性が示されている。本発明を適用し
ない従来の姿勢角検出装置では、前述のとおり、姿勢角
誤差は時間に対して累積されるが、本発明を適用した姿
勢角検出装置によれば、姿勢角誤差は重力成分の抽出精
度程度に抑えることが可能になることが明かである。
出装置と、適用しない従来の姿勢角検出装置で生ずる姿
勢角誤差の時間的特性が示されている。本発明を適用し
ない従来の姿勢角検出装置では、前述のとおり、姿勢角
誤差は時間に対して累積されるが、本発明を適用した姿
勢角検出装置によれば、姿勢角誤差は重力成分の抽出精
度程度に抑えることが可能になることが明かである。
【００３１】以上の実施形態例では、運動体姿勢角とし
てオイラー角を採用しているが、例えば、４元数を利用
した姿勢角表現を用いることにり、オイラー角を用いた
角速度積分の計算特異性を回避することができる。ま
た、図１の重力成分推定手段５０としてはローパスフィ
ルタを用い、そのカットオフ周波数を運動体の姿勢変動
を伴う運動のうち最も周波数の高い運動よりは高い周波
数に設定しているが、運動体の運動がある特定のパター
ンになるような場合については、カットオフ周波数を適
宜切替えることができるような適応フィルタで実現する
こともできる。こうすることにより、運動体姿勢角の検
出精度をさらに向上することができる。
てオイラー角を採用しているが、例えば、４元数を利用
した姿勢角表現を用いることにり、オイラー角を用いた
角速度積分の計算特異性を回避することができる。ま
た、図１の重力成分推定手段５０としてはローパスフィ
ルタを用い、そのカットオフ周波数を運動体の姿勢変動
を伴う運動のうち最も周波数の高い運動よりは高い周波
数に設定しているが、運動体の運動がある特定のパター
ンになるような場合については、カットオフ周波数を適
宜切替えることができるような適応フィルタで実現する
こともできる。こうすることにより、運動体姿勢角の検
出精度をさらに向上することができる。
【００３２】
【発明の効果】以上説明したように、本発明の姿勢角検
出装置及び方法では、重力の方向を利用して運動体姿勢
角の誤差を推定しているので、の重力が既知である場所
を運動する運動体に関して運動体姿勢角の検出精度が向
上し、姿勢角制御、ナビゲーションの性能を改善するこ
とができる。また、本発明は運動体に搭載されている運
動測定系の情報のみしか利用しておらず、既存のソフト
ウェアへの追加で実現できるため、姿勢角制御／ナビゲ
ーションを必要とする運動体に関して姿勢角の検出精度
を向上するためのハードウェアの追加が必要なく既存の
運動体へハードウェアの影響を及ぼさないようにするこ
とができる。
出装置及び方法では、重力の方向を利用して運動体姿勢
角の誤差を推定しているので、の重力が既知である場所
を運動する運動体に関して運動体姿勢角の検出精度が向
上し、姿勢角制御、ナビゲーションの性能を改善するこ
とができる。また、本発明は運動体に搭載されている運
動測定系の情報のみしか利用しておらず、既存のソフト
ウェアへの追加で実現できるため、姿勢角制御／ナビゲ
ーションを必要とする運動体に関して姿勢角の検出精度
を向上するためのハードウェアの追加が必要なく既存の
運動体へハードウェアの影響を及ぼさないようにするこ
とができる。
【図１】本発明の姿勢角検出装置を示す構成図である。
【図２】本発明の姿勢角検出装置における姿勢角の説明
図である。
図である。
【図３】本発明の姿勢角検出装置の一実施例の動作を示
す動作図である。
す動作図である。
【図４】本発明の姿勢角検出装置の効果の説明図であ
る。
る。
【図５】従来の姿勢角検出装置を示す構成図である。
１０ 角速度計 ２０ 角速度積分手段 ３０ 姿勢角制御装置／ナビゲーション装置 ４０ 加速度計 ５０ 重力成分推定手段 ６０ 座標変換手段 ７０ 姿勢角誤差推定手段 ８０ 姿勢角誤差補正手段
Claims (6)
- 【請求項１】運動体の角速度を測定して運動体固定軸上
の角速度を出力する角速度測定手段と、 該角速度測定手段の出力を積分して現在の運動体姿勢角
を出力する角速度積分手段と、 前記運動体の加速度を測定して運動体固定軸上の加速度
を出力する加速度測定手段と、 該加速度測定手段の出力から運動体固定軸上での重力成
分を抽出する重力成分抽出手段と、 運動体固定軸上のベクトル成分を基準軸上のベクトル成
分に変換する座標変換手段と、 基準軸上において、姿勢角誤差を有する重力成分と参照
重力成分とを比較することにより姿勢角誤差を測定する
姿勢角誤差算出手段と、 姿勢角誤差を用いて姿勢角の補正を行う姿勢角補正手段
とを備え、前記運動体姿勢角としてオイラー角とする ことを特徴と
する姿勢角検出装置。 - 【請求項２】運動体が静止している状態か運動している
状態かの判断を行うステップと、 前記運動体が静止していると判断した場合、運動体姿勢
角の初期値を算出するステップと、前記運動体の角速度
を積分して前記運動体の姿勢角を求めるステップと、 前記運動体の固定軸のｘ軸、ｙ軸及びｚ軸方向のそれぞ
れの加速度成分と重力成分を用いて重力成分の抽出を行
うステップと、 前記運動体の姿勢角を用いて、前記運動体の固定軸上の
重力成分を基準軸上の重力成分へ変換するステップと、 前記基準軸上の重力成分と基準軸上の参照重力成分に基
づいて前記運動体の姿勢角誤差を推定するステップと、 前記姿勢角誤差に基づいて前記運動体姿勢角の補正を行
い、補正された運動体姿勢角を前記運動体姿勢角の初期
値とするステップとを備え、前記運動体姿勢角としてオイラー角とする ことを特徴と
する姿勢角検出方法。 - 【請求項３】運動体が静止している状態か運動している
状態かの判断を行うステップと、 前記運動体が静止していると判断した場合、運動体姿勢
角の初期値を算出するステップと、前記運動体の角速度
を積分して前記運動体の姿勢角を求めるステップと、 前記運動体の固定軸のｘ軸、ｙ軸及びｚ軸方向のそれぞ
れの加速度成分と重力成分を用いて重力成分の抽出を行
うステップと、 前記運動体の姿勢角を用いて、前記運動体の固定軸上の
重力成分を基準軸上の重力成分へ変換するステップと、 前記基準軸上の重力成分と基準軸上の参照重力成分に基
づいて前記運動体の姿勢角誤差を推定するステップと、 前記姿勢角誤差に基づいて前記運動体姿勢角の補正を行
い、補正された運動体姿勢角を前記運動体姿勢角の初期
値とするステップとを備え、 前記運動体姿勢角の初期値としては、傾斜計として機能
する加速度計の出力を用いる請求項２に記載の姿勢角検
出方法。 - 【請求項４】運動体の角速度を測定して運動体固定軸上
の角速度を出力する角速度測定手段と、 該角速度測定手段の出力を積分して現在の運動体姿勢角
を出力する角速度積分手段と、 前記運動体の加速度を測定して運動体固定軸上の加速度
を出力する加速度測定手段と、 該加速度測定手段の出力から運動体固定軸上での重力成
分を抽出する重力成分抽出手段と、 運動体固定軸上のベクトル成分を基準軸上のベクトル成
分に変換する座標変換手段と、 基準軸上において、姿勢角誤差を有する重力成分と参照
重力成分とを比較することにより姿勢角誤差を測定する
姿勢角誤差算出手段と、 姿勢角誤差を用いて姿勢角の補正を行う姿勢角補正手段
とを備え、 前記重力成分は、そのカットオフ周波数が前記運動体の
姿勢変動を伴う運動のう ち最も周波数の高い運動よりは
高い周波数に設定されているローパスフィルタを用いて
抽出される請求項１に記載の姿勢角検出装置。 - 【請求項５】運動体が静止している状態か運動している
状態かの判断を行うステップと、 前記運動体が静止していると判断した場合、運動体姿勢
角の初期値を算出するステップと、前記運動体の角速度
を積分して前記運動体の姿勢角を求めるステップと、 前記運動体の固定軸のｘ軸、ｙ軸及びｚ軸方向のそれぞ
れの加速度成分と重力成分を用いて重力成分の抽出を行
うステップと、 前記運動体の姿勢角を用いて、前記運動体の固定軸上の
重力成分を基準軸上の重力成分へ変換するステップと、 前記基準軸上の重力成分と基準軸上の参照重力成分に基
づいて前記運動体の姿勢角誤差を推定するステップと、 前記姿勢角誤差に基づいて前記運動体姿勢角の補正を行
い、補正された運動体姿勢角を前記運動体姿勢角の初期
値とするステップとを備え、 前記重力成分は、そのカットオフ周波数が前記運動体の
姿勢変動を伴う運動のうち最も周波数の高い運動よりは
高い周波数に設定されているローパスフィルタを用いて
抽出される請求項２に記載の姿勢角検出方法。 - 【請求項６】前記ローパスフィルタは、カットオフ周波
数を適宜切替えることができるような適応フィルタであ
る請求項４または５に記載の姿勢角検出装置及び方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP8132249A JP2904118B2 (ja) | 1996-05-27 | 1996-05-27 | 姿勢角検出装置及び方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP8132249A JP2904118B2 (ja) | 1996-05-27 | 1996-05-27 | 姿勢角検出装置及び方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH09318382A JPH09318382A (ja) | 1997-12-12 |
| JP2904118B2 true JP2904118B2 (ja) | 1999-06-14 |
Family
ID=15076863
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP8132249A Expired - Lifetime JP2904118B2 (ja) | 1996-05-27 | 1996-05-27 | 姿勢角検出装置及び方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2904118B2 (ja) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101739390B1 (ko) * | 2015-12-11 | 2017-05-24 | 국방과학연구소 | 중력오차보상을 통한 관성항법장치의 자체정렬 정확도 향상기법 |
Families Citing this family (29)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP3763435B2 (ja) * | 1998-01-20 | 2006-04-05 | Ｎｅｃトーキン株式会社 | 姿勢角検出装置 |
| DE69924649T2 (de) * | 1999-06-30 | 2006-03-09 | Deka Products Ltd. Partnership | Vorrichtung und verfahren zum abschätzen des nickneigungszustandes eines personengebundenen fahrzeuges |
| US6377906B1 (en) * | 2000-02-03 | 2002-04-23 | Independence Technology, L.L.C. | Attitude estimation in tiltable body using modified quaternion data representation |
| JP3559965B2 (ja) * | 2001-06-29 | 2004-09-02 | 日本航空電子工業株式会社 | 姿勢角検出装置 |
| JP2004264060A (ja) | 2003-02-14 | 2004-09-24 | Akebono Brake Ind Co Ltd | 姿勢の検出装置における誤差補正方法及びそれを利用した動作計測装置 |
| US6904377B2 (en) * | 2003-03-17 | 2005-06-07 | Northrop Grumman Corporation | Method for measuring force-dependent gyroscope sensitivity |
| JP3795498B2 (ja) * | 2004-03-12 | 2006-07-12 | 日本航空電子工業株式会社 | 姿勢角検出装置 |
| JP2007041733A (ja) | 2005-08-01 | 2007-02-15 | Toyota Motor Corp | 運動体の姿勢角検出装置 |
| JP4955285B2 (ja) * | 2006-03-03 | 2012-06-20 | マイクロストーン株式会社 | 傾斜角測定センサ |
| JP4547346B2 (ja) * | 2006-03-22 | 2010-09-22 | 任天堂株式会社 | 傾き算出装置および傾き算出プログラムならびにゲーム装置およびゲームプログラム |
| JP4321554B2 (ja) | 2006-06-23 | 2009-08-26 | トヨタ自動車株式会社 | 姿勢角検出装置と姿勢角検出方法 |
| WO2008026357A1 (fr) * | 2006-08-29 | 2008-03-06 | Microstone Corporation | Procédé de capture de mouvements |
| JP5161498B2 (ja) * | 2007-06-18 | 2013-03-13 | 株式会社豊田中央研究所 | 姿勢信号演算装置 |
| JP5541851B2 (ja) * | 2008-06-30 | 2014-07-09 | 任天堂株式会社 | 姿勢算出装置、姿勢算出プログラム、ゲーム装置、およびゲームプログラム |
| JP5128456B2 (ja) * | 2008-12-18 | 2013-01-23 | 株式会社豊田中央研究所 | 車両姿勢角推定装置及びプログラム |
| JP5128457B2 (ja) * | 2008-12-18 | 2013-01-23 | 株式会社豊田中央研究所 | 車両姿勢角推定装置及びプログラム |
| JP5419268B2 (ja) * | 2009-09-11 | 2014-02-19 | 学校法人千葉工業大学 | 三次元姿勢推定装置、三次元姿勢推定方法及び三次元姿勢推定プログラム |
| JP5393432B2 (ja) | 2009-12-21 | 2014-01-22 | ヤマハ発動機株式会社 | ロール角推定装置および輸送機器 |
| CN104820434A (zh) * | 2015-03-24 | 2015-08-05 | 南京航空航天大学 | 一种无人机对地面运动目标的测速方法 |
| KR101676470B1 (ko) * | 2015-06-08 | 2016-11-15 | 세종대학교산학협력단 | 주행 로봇의 진행각 추정 장치 및 그 방법, 그 방법을 수행하기 위한 프로그램이 기록된 컴퓨터 판독 가능한 기록 매체 |
| CN105700550B (zh) * | 2016-01-26 | 2018-06-26 | 深圳市大疆创新科技有限公司 | 无人机及其飞行控制方法与*** |
| CN106681344B (zh) * | 2016-12-26 | 2019-08-27 | 湖南纳雷科技有限公司 | 一种用于飞行器的高度控制方法及控制*** |
| EP3566017A4 (en) * | 2017-01-05 | 2020-08-12 | BAE SYSTEMS Information and Electronic Systems Integration Inc. | GUIDED AMMUNITION TILT ORIENTATION DETERMINATION |
| JP6859917B2 (ja) * | 2017-10-13 | 2021-04-14 | 株式会社Ｊｖｃケンウッド | 角速度導出装置および角速度導出方法 |
| JP6922641B2 (ja) * | 2017-10-13 | 2021-08-18 | 株式会社Ｊｖｃケンウッド | 角速度導出装置および角速度導出方法 |
| JP6922640B2 (ja) * | 2017-10-13 | 2021-08-18 | 株式会社Ｊｖｃケンウッド | 角速度導出装置および角速度導出方法 |
| JP7391616B2 (ja) * | 2019-11-05 | 2023-12-05 | 本田技研工業株式会社 | 移動体の姿勢推定装置 |
| CN113438399B (zh) * | 2021-06-25 | 2022-04-08 | 北京冠林威航科技有限公司 | 用于无人机的目标导引***、方法、无人机和存储介质 |
| CN116519011B (zh) * | 2023-03-11 | 2024-03-01 | 中国人民解放军国防科技大学 | 基于Psi角误差修正模型的长航时双惯导协同标定方法 |
-
1996
- 1996-05-27 JP JP8132249A patent/JP2904118B2/ja not_active Expired - Lifetime
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101739390B1 (ko) * | 2015-12-11 | 2017-05-24 | 국방과학연구소 | 중력오차보상을 통한 관성항법장치의 자체정렬 정확도 향상기법 |
Also Published As
| Publication number | Publication date |
|---|---|
| JPH09318382A (ja) | 1997-12-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP2904118B2 (ja) | 姿勢角検出装置及び方法 | |
| CA2381196C (en) | Vibration compensation for sensors | |
| US8185309B2 (en) | Enhanced inertial system performance | |
| KR100898169B1 (ko) | 관성항법시스템의 초기정렬 방법 | |
| CN109916395B (zh) | 一种姿态自主冗余组合导航算法 | |
| US8768621B2 (en) | Signal processing module, navigation device with the signal processing module, vehicle provided with a navigation device and method of providing navigation data | |
| WO2009063400A2 (en) | Orientation measurement of an object | |
| WO1997008514A1 (fr) | Detecteur de mouvement | |
| EP1941236A1 (en) | Systems and methods for reducing vibration-induced errors in inertial sensors | |
| JP2016033473A (ja) | 位置算出方法及び位置算出装置 | |
| JP4447791B2 (ja) | ジャイロメータと加速度計を具備する航空機の姿勢決定装置 | |
| JP3380404B2 (ja) | 移動検出装置 | |
| WO2021218731A1 (zh) | Imu与刚体的位姿融合方法、装置、设备及存储介质 | |
| CN110567457A (zh) | 一种基于冗余的惯导自检测*** | |
| Carratù et al. | IMU self-alignment in suspensions control system | |
| CN110954103B (zh) | 基于mems传感器的车体动态姿态估计的方法及*** | |
| CN114964214B (zh) | 一种航姿参考***的扩展卡尔曼滤波姿态解算方法 | |
| CN113267183B (zh) | 一种多加速度计惯导***的组合导航方法 | |
| CN113227714B (zh) | 用于表征惯性测量单元的方法 | |
| JP3783061B1 (ja) | 傾斜角と並進加速度の検出方法および検出装置 | |
| JP3169213B2 (ja) | 移動速度検出方法及び装置、車両のすべり角検出装置 | |
| JP2003139536A (ja) | 方位計および方位測定方法 | |
| KR20210000095A (ko) | 자세 추정 방법 및 장치 | |
| CN109631895B (zh) | 一种物体的位姿估计方法和装置 | |
| CN114008410A (zh) | 用于监测惯性测量单元的性能的方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 19990223 |