JPH095104A - 移動物体の三次元姿勢角測定法および三次元姿勢角計測装置 - Google Patents
移動物体の三次元姿勢角測定法および三次元姿勢角計測装置Info
- Publication number
- JPH095104A JPH095104A JP15774695A JP15774695A JPH095104A JP H095104 A JPH095104 A JP H095104A JP 15774695 A JP15774695 A JP 15774695A JP 15774695 A JP15774695 A JP 15774695A JP H095104 A JPH095104 A JP H095104A
- Authority
- JP
- Japan
- Prior art keywords
- moving object
- vector
- dimensional
- angular velocity
- processing means
- 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.)
- Pending
Links
Abstract
(57)【要約】
【目的】空中、地上、地下、水中など任意の場所でも三
次元姿勢角の測定・計測を高精度で行うことができる移
動物体の三次元姿勢角測定法および三次元姿勢角計測装
置を提供する。 【構成】３個の角速度センサ１、３個の加速度センサ
２、及び演算装置３からなる。演算装置３は、角速度セ
ンサ１により得られた角速度の計測値を時間的に積分し
て移動物体の三次元姿勢角の計測値を算出するととも
に、加速度センサ２から得られた重力加速度ベクトルを
観測ベクトルとする拡張カルマンフィルタの適用によ
り、移動物体の三次元姿勢角の計測値を補正する機能を
有する。
次元姿勢角の測定・計測を高精度で行うことができる移
動物体の三次元姿勢角測定法および三次元姿勢角計測装
置を提供する。 【構成】３個の角速度センサ１、３個の加速度センサ
２、及び演算装置３からなる。演算装置３は、角速度セ
ンサ１により得られた角速度の計測値を時間的に積分し
て移動物体の三次元姿勢角の計測値を算出するととも
に、加速度センサ２から得られた重力加速度ベクトルを
観測ベクトルとする拡張カルマンフィルタの適用によ
り、移動物体の三次元姿勢角の計測値を補正する機能を
有する。
Description
【０００１】
【産業上の利用分野】本発明は、高精度に移動物体の三
次元の姿勢角を測定する方法および姿勢角を計測する装
置に係り、特に無人搬送車、移動式建設機械、移動式土
木機械、トンネル堀削用ロボット、海中作業用ロボッ
ト、又は飛翔体の三次元姿勢角（一般にはロール・ピッ
チ・ヨー角などで表される）の計測に好適な移動物体の
三次元姿勢角測定法および三次元姿勢角計測装置に関す
る。
次元の姿勢角を測定する方法および姿勢角を計測する装
置に係り、特に無人搬送車、移動式建設機械、移動式土
木機械、トンネル堀削用ロボット、海中作業用ロボッ
ト、又は飛翔体の三次元姿勢角（一般にはロール・ピッ
チ・ヨー角などで表される）の計測に好適な移動物体の
三次元姿勢角測定法および三次元姿勢角計測装置に関す
る。
【０００２】
【従来の技術】飛翔体やトンネル堀削用ロボットでは、
その姿勢状態を高精度に割出すに当たり、三次元姿勢角
計測装置を用いている。図３は従来の三次元姿勢角計測
装置の構成例を示している。従来の三次元姿勢角計測装
置βは、図３に示すように、３個のジャイロセンサ１
０、及び演算装置１１からなる。
その姿勢状態を高精度に割出すに当たり、三次元姿勢角
計測装置を用いている。図３は従来の三次元姿勢角計測
装置の構成例を示している。従来の三次元姿勢角計測装
置βは、図３に示すように、３個のジャイロセンサ１
０、及び演算装置１１からなる。
【０００３】ジャイロセンサ１０は、図を省略した移動
物体の独立な３軸上の角速度を計測できるように基底盤
１２の上に配置されハウジング１３内に収蔵されてい
る。演算装置１１は、ジャイロセンサ１０により得られ
た角速度の計測値を時間的に積分して移動物体の三次元
の姿勢角を算出する。なお、移動物体の三次元姿勢角を
表現する当たり、ロール・ピッチ・ヨー角法、又はオイ
ラー角法等が用いられている。
物体の独立な３軸上の角速度を計測できるように基底盤
１２の上に配置されハウジング１３内に収蔵されてい
る。演算装置１１は、ジャイロセンサ１０により得られ
た角速度の計測値を時間的に積分して移動物体の三次元
の姿勢角を算出する。なお、移動物体の三次元姿勢角を
表現する当たり、ロール・ピッチ・ヨー角法、又はオイ
ラー角法等が用いられている。
【０００４】ところで、ジャイロセンサ１０は、雑音や
ドリフトによって角速度の計測値に誤差が生じるととも
に、誤差の値は時間とともに累積するため、単独では長
時間の計測に適さないという問題がある。
ドリフトによって角速度の計測値に誤差が生じるととも
に、誤差の値は時間とともに累積するため、単独では長
時間の計測に適さないという問題がある。
【０００５】そこで、移動物体の姿勢角を長時間に亙っ
て精密に計測するには、以下の例に挙げるような、種々
の誤差補正方法が用いられている。 （１）人工衛星や宇宙船に用いられる三次元姿勢計測装
置では、太陽等の恒星にて方向基準を得ることにより、
移動物体の三次元姿勢角の誤差を補正する方法、（２）
航空機に用いられるシステムでは、双曲線航法システム
のロラン（loran）ＣやＧＰＳ（global positioning sy
stem ）から位置情報を得ることにより、移動物体の三
次元姿勢角の誤差を補正する方法、がある。
て精密に計測するには、以下の例に挙げるような、種々
の誤差補正方法が用いられている。 （１）人工衛星や宇宙船に用いられる三次元姿勢計測装
置では、太陽等の恒星にて方向基準を得ることにより、
移動物体の三次元姿勢角の誤差を補正する方法、（２）
航空機に用いられるシステムでは、双曲線航法システム
のロラン（loran）ＣやＧＰＳ（global positioning sy
stem ）から位置情報を得ることにより、移動物体の三
次元姿勢角の誤差を補正する方法、がある。
【０００６】
【発明が解決しようとする課題】しかしながら、上述し
た移動物体の三次元姿勢角計測装置による計測値の誤差
補正方法では、以下の問題がある。 （１）の誤差補正方法は、天候的な制限を受ける、即ち
地球上で用いる場合、地球の自転・公転に関する情報が
必要となるので晴天時しか利用できない。 （２）の誤差補正方法は、地理的な制限を受ける、即ち
遮蔽物の多い地表面、地下、海中、水中では電波がキャ
ッチできないので、その利用が困難となる。
た移動物体の三次元姿勢角計測装置による計測値の誤差
補正方法では、以下の問題がある。 （１）の誤差補正方法は、天候的な制限を受ける、即ち
地球上で用いる場合、地球の自転・公転に関する情報が
必要となるので晴天時しか利用できない。 （２）の誤差補正方法は、地理的な制限を受ける、即ち
遮蔽物の多い地表面、地下、海中、水中では電波がキャ
ッチできないので、その利用が困難となる。
【０００７】したがって、地下等で作業を行うトンネル
堀削用ロボット等の三次元姿勢角を高精度で計測するこ
とは困難であり、最適な誤差補正方法が無いというのが
実情である。
堀削用ロボット等の三次元姿勢角を高精度で計測するこ
とは困難であり、最適な誤差補正方法が無いというのが
実情である。
【０００８】ここにおいて本発明の解決すべき主要な目
的は、次の通りである。本発明の第１の目的は、空中、
地上、地下、水中など任意の場所でも三次元姿勢角の測
定・計測を高精度で行うことができる移動物体の三次元
姿勢角測定法および三次元姿勢角計測装置を提供するも
のである。
的は、次の通りである。本発明の第１の目的は、空中、
地上、地下、水中など任意の場所でも三次元姿勢角の測
定・計測を高精度で行うことができる移動物体の三次元
姿勢角測定法および三次元姿勢角計測装置を提供するも
のである。
【０００９】本発明の第２の目的は、長時間使用して
も、誤差が累積せず、三次元姿勢角の測定・計測を高精
度に、しかも簡便に行うことができる移動物体の三次元
姿勢角測定法および三次元姿勢角計測装置を提供するも
のである。
も、誤差が累積せず、三次元姿勢角の測定・計測を高精
度に、しかも簡便に行うことができる移動物体の三次元
姿勢角測定法および三次元姿勢角計測装置を提供するも
のである。
【００１０】本発明の第３の目的は、移動物体の三次元
姿勢角の計測値を高精度に算出するに当たり、その演算
処理を効率的に行うことができる移動物体の三次元姿勢
角測定法および三次元姿勢角計測装置を提供するもので
ある。
姿勢角の計測値を高精度に算出するに当たり、その演算
処理を効率的に行うことができる移動物体の三次元姿勢
角測定法および三次元姿勢角計測装置を提供するもので
ある。
【００１１】本発明の第４の目的は、移動物体の重力加
速度ベクトルを絶対的基準として当該移動物体の三次元
姿勢情報の補正を行う移動物体の三次元姿勢角測定方法
及び三次元姿勢角計測装置を提供するものである。
速度ベクトルを絶対的基準として当該移動物体の三次元
姿勢情報の補正を行う移動物体の三次元姿勢角測定方法
及び三次元姿勢角計測装置を提供するものである。
【００１２】本発明のその他の目的は、明細書、図面、
特に特許請求の範囲の記載から自ずと明らかとなろう。
特に特許請求の範囲の記載から自ずと明らかとなろう。
【００１３】
【課題を解決するための手段】前記した課題は、本発明
が次に列挙する新規な特徴的構成手法及び手段を採用す
ることにより解決され、本発明の目的を達成する。
が次に列挙する新規な特徴的構成手法及び手段を採用す
ることにより解決され、本発明の目的を達成する。
【００１４】すなわち、本発明方法の第１の特徴は、移
動物体の独立な３軸方向の加速度をそれぞれ計測して当
該移動物体の三次元姿勢角を計測するに当り、三次元姿
勢角の計測値を、当該移動物体の重力加速度ベクトルを
絶対基準として補正してなる移動物体の三次元姿勢角測
定法にある。
動物体の独立な３軸方向の加速度をそれぞれ計測して当
該移動物体の三次元姿勢角を計測するに当り、三次元姿
勢角の計測値を、当該移動物体の重力加速度ベクトルを
絶対基準として補正してなる移動物体の三次元姿勢角測
定法にある。
【００１５】本発明方法の第２の特徴は、前記本発明方
法の第１の特徴における前記移動物体の重力加速度ベク
トルは、前記移動物体の独立な３軸方向の加速度のそれ
ぞれの計測値に基づいて重力方向のそれぞれのベクトル
又は合成ベクトルとして求められてなる移動物体の三次
元姿勢角測定法にある。
法の第１の特徴における前記移動物体の重力加速度ベク
トルは、前記移動物体の独立な３軸方向の加速度のそれ
ぞれの計測値に基づいて重力方向のそれぞれのベクトル
又は合成ベクトルとして求められてなる移動物体の三次
元姿勢角測定法にある。
【００１６】本発明方法の第３の特徴は、移動物体に搭
載され、当該移動物体の独立な３軸方向の角速度を計測
する角速度センサと、前記移動物体に搭載され、当該移
動物体の独立な３軸方向の加速度を計測する加速度セン
サと、演算装置とを備えた移動物体の三次元姿勢角計測
装置において、前記演算装置にてクォータニオンをもと
に移動物体の三次元姿勢角を測定するに当たり、前記角
速度センサで測定される角速度に基づいて状態行列を作
成するステップI と、前記移動物体の初期状態からの姿
勢変化の回転を３次元単位ベクトル軸まわりの回転角に
よって表現するクォータニオンを以って状態ベクトルを
作成するステップIIと、ステップI の状態行列とステッ
プIIの状態ベクトルに基づいて状態方程式を作成するス
テップIII と、前記加速度センサで測定された重力方向
を示す重力加速度ベクトルを観測ベクトルとして、前記
クォータニオンを用いた座標回転を示す回転行列と前記
重力加速度ベクトルに基づいて観測方程式を作成するス
テップIVと、ステップIIIの状態方程式とステップIVの
観測方程式からカルマンゲインを求めるステップVと、
前記回転行列と前記移動物体の初期位置における重力加
速度ベクトルとの積にて得られる算出値と前記移動物体
の移動後の重力加速度ベクトルとの差値にステップV の
カルマンゲインを掛けて補正ベクトルを算出するステッ
プVIと、ステップI の状態行列に基づいて前記角速度セ
ンサで測定される角速度を積分して得られる状態ベクト
ル（クォータニオン）にステップVIの補正ベクトルを加
えて、補正後の姿勢角を表すクォータニオンを算出する
ステップVII と、を順次経由してなる移動物体の三次元
姿勢角測定法にある。
載され、当該移動物体の独立な３軸方向の角速度を計測
する角速度センサと、前記移動物体に搭載され、当該移
動物体の独立な３軸方向の加速度を計測する加速度セン
サと、演算装置とを備えた移動物体の三次元姿勢角計測
装置において、前記演算装置にてクォータニオンをもと
に移動物体の三次元姿勢角を測定するに当たり、前記角
速度センサで測定される角速度に基づいて状態行列を作
成するステップI と、前記移動物体の初期状態からの姿
勢変化の回転を３次元単位ベクトル軸まわりの回転角に
よって表現するクォータニオンを以って状態ベクトルを
作成するステップIIと、ステップI の状態行列とステッ
プIIの状態ベクトルに基づいて状態方程式を作成するス
テップIII と、前記加速度センサで測定された重力方向
を示す重力加速度ベクトルを観測ベクトルとして、前記
クォータニオンを用いた座標回転を示す回転行列と前記
重力加速度ベクトルに基づいて観測方程式を作成するス
テップIVと、ステップIIIの状態方程式とステップIVの
観測方程式からカルマンゲインを求めるステップVと、
前記回転行列と前記移動物体の初期位置における重力加
速度ベクトルとの積にて得られる算出値と前記移動物体
の移動後の重力加速度ベクトルとの差値にステップV の
カルマンゲインを掛けて補正ベクトルを算出するステッ
プVIと、ステップI の状態行列に基づいて前記角速度セ
ンサで測定される角速度を積分して得られる状態ベクト
ル（クォータニオン）にステップVIの補正ベクトルを加
えて、補正後の姿勢角を表すクォータニオンを算出する
ステップVII と、を順次経由してなる移動物体の三次元
姿勢角測定法にある。
【００１７】本発明方法の第４の特徴は、前記本発明方
法の第３の特徴における演算装置が、前記角速度センサ
及び前記加速度センサとともに基底盤に配置され、前記
移動物体に搭載されてなる移動物体の三次元姿勢角測定
法にある。
法の第３の特徴における演算装置が、前記角速度センサ
及び前記加速度センサとともに基底盤に配置され、前記
移動物体に搭載されてなる移動物体の三次元姿勢角測定
法にある。
【００１８】本発明方法の第５の特徴は、前記本発明方
法の第３又は第４の特徴における移動物体が、別個に設
けられたコントロール装置にて遠隔操作可能な構成と
し、前記演算装置の結果を送信手段にて当該コントロー
ル装置に送信してなる移動物体の三次元姿勢角測定法に
ある。
法の第３又は第４の特徴における移動物体が、別個に設
けられたコントロール装置にて遠隔操作可能な構成と
し、前記演算装置の結果を送信手段にて当該コントロー
ル装置に送信してなる移動物体の三次元姿勢角測定法に
ある。
【００１９】本発明方法の第６の特徴は、前記本発明方
法の第３、第４又は第５の特徴における移動物体が、無
人搬送車、移動式建設機械、移動式土木機械、トンネル
堀削用ロボット、海中作業用ロボット又は飛翔体からな
る移動物体の三次元姿勢角測定法にある。
法の第３、第４又は第５の特徴における移動物体が、無
人搬送車、移動式建設機械、移動式土木機械、トンネル
堀削用ロボット、海中作業用ロボット又は飛翔体からな
る移動物体の三次元姿勢角測定法にある。
【００２０】本発明装置の第１の特徴は、移動物体に搭
載され、当該移動物体の独立な３軸方向の角速度を計測
する角速度センサと、前記移動物体に搭載され、当該移
動物体の独立な３軸方向の加速度を計測する加速度セン
サと、前記角速度センサにより得られた角速度の計測値
を時間的に積分して移動物体の三次元姿勢角の計測値を
算出するとともに、前記加速度センサから得られた重力
加速度ベクトルを観測ベクトルとする拡張カルマンフィ
ルタの適用により当該三次元姿勢角の計測値を補正する
機能を有する演算装置と、を備えてなる移動物体の三次
元姿勢角計測装置にある。
載され、当該移動物体の独立な３軸方向の角速度を計測
する角速度センサと、前記移動物体に搭載され、当該移
動物体の独立な３軸方向の加速度を計測する加速度セン
サと、前記角速度センサにより得られた角速度の計測値
を時間的に積分して移動物体の三次元姿勢角の計測値を
算出するとともに、前記加速度センサから得られた重力
加速度ベクトルを観測ベクトルとする拡張カルマンフィ
ルタの適用により当該三次元姿勢角の計測値を補正する
機能を有する演算装置と、を備えてなる移動物体の三次
元姿勢角計測装置にある。
【００２１】本発明装置の第２の特徴は、前記本発明装
置の第１の特徴における演算装置が、クォータニオンを
移動物体の三次元姿勢角の計測値とするために、前記角
速度センサで測定される角速度に基づいて状態行列を作
成する第１処理手段と、前記移動体の初期状態からの姿
勢変化の回転を３次元単位ベクトル軸まわりの回転角に
よって表現するクォータニオンを以って状態ベクトルを
作成する第２処理手段と、第１処理手段の状態行列と第
２処理手段の状態ベクトルに基づいて状態方程式を作成
する第３処理手段と、前記加速度センサで測定された重
力方向を示す重力加速度ベクトルを観測ベクトルとし
て、前記クォータニオンを用いた座標回転を示す回転行
列と前記重力加速度ベクトルに基づいて観測方程式を作
成する第４処理手段と、第３処理手段の状態方程式と第
４処理手段の観測方程式からカルマンゲインを求める第
５処理手段と、前記回転行列と前記移動体の初期位置に
おける重力加速度ベクトルとの積により得られる算出値
と、前記移動体の移動後の重力加速度ベクトルとの差に
第５処理手段のカルマンゲインを掛けて補正ベクトルを
得る第６処理手段と、第１処理手段の状態行列に基づい
て前記角速度センサで測定される角速度を積分して得ら
れる状態ベクトル（クォータニオン）に第６処理手段の
補正ベクトルを加えて、補正後の姿勢角を表すクォータ
ニオンを得る第７処理手段と、を備えてなる移動物体の
三次元姿勢角計測装置にある。
置の第１の特徴における演算装置が、クォータニオンを
移動物体の三次元姿勢角の計測値とするために、前記角
速度センサで測定される角速度に基づいて状態行列を作
成する第１処理手段と、前記移動体の初期状態からの姿
勢変化の回転を３次元単位ベクトル軸まわりの回転角に
よって表現するクォータニオンを以って状態ベクトルを
作成する第２処理手段と、第１処理手段の状態行列と第
２処理手段の状態ベクトルに基づいて状態方程式を作成
する第３処理手段と、前記加速度センサで測定された重
力方向を示す重力加速度ベクトルを観測ベクトルとし
て、前記クォータニオンを用いた座標回転を示す回転行
列と前記重力加速度ベクトルに基づいて観測方程式を作
成する第４処理手段と、第３処理手段の状態方程式と第
４処理手段の観測方程式からカルマンゲインを求める第
５処理手段と、前記回転行列と前記移動体の初期位置に
おける重力加速度ベクトルとの積により得られる算出値
と、前記移動体の移動後の重力加速度ベクトルとの差に
第５処理手段のカルマンゲインを掛けて補正ベクトルを
得る第６処理手段と、第１処理手段の状態行列に基づい
て前記角速度センサで測定される角速度を積分して得ら
れる状態ベクトル（クォータニオン）に第６処理手段の
補正ベクトルを加えて、補正後の姿勢角を表すクォータ
ニオンを得る第７処理手段と、を備えてなる移動物体の
三次元姿勢角計測装置にある。
【００２２】本発明装置の第３の特徴は、前記本発明装
置の第１又は２の特徴における演算装置が、前記角速度
センサ及び前記加速度センサとともに基底盤に一体配置
され、前記移動物体に搭載されてなる移動物体の三次元
姿勢角計測装置にある。
置の第１又は２の特徴における演算装置が、前記角速度
センサ及び前記加速度センサとともに基底盤に一体配置
され、前記移動物体に搭載されてなる移動物体の三次元
姿勢角計測装置にある。
【００２３】本発明装置の第４の特徴は、前記本発明装
置の第１、２又は３の特徴における移動装置が、別個に
設けられたコントロール装置にて遠隔操作可能な構成と
し、前記演算装置の結果を当該コントロール装置に送信
する送信手段を備えてなる移動物体の三次元姿勢角計測
装置にある。
置の第１、２又は３の特徴における移動装置が、別個に
設けられたコントロール装置にて遠隔操作可能な構成と
し、前記演算装置の結果を当該コントロール装置に送信
する送信手段を備えてなる移動物体の三次元姿勢角計測
装置にある。
【００２４】本発明装置の第５の特徴は、前記本発明装
置の第１、２、３又は４の特徴における移動物体が、無
人搬送車、移動式建設機械、移動式土木機械、トンネル
堀削用ロボット、海中作業用ロボット、又は飛翔体から
なる移動物体の三次元姿勢角計測装置にある。
置の第１、２、３又は４の特徴における移動物体が、無
人搬送車、移動式建設機械、移動式土木機械、トンネル
堀削用ロボット、海中作業用ロボット、又は飛翔体から
なる移動物体の三次元姿勢角計測装置にある。
【００２５】
【作用】本発明は、前記のような新規な手法及び手段を
講じているので、以下のような作用をなす。
講じているので、以下のような作用をなす。
【００２６】本発明では、雑音やドリフトによって角速
度センサの計測値に誤差が生じても、加速度センサから
重力方向を示す重力加速度ベクトルを得て、当該重力加
速度ベクトルを絶対基準として姿勢情報の補正を行う。
その結果、誤差が累積せず高精度に移動物体の三次元姿
勢角の測定及び計測することができる。
度センサの計測値に誤差が生じても、加速度センサから
重力方向を示す重力加速度ベクトルを得て、当該重力加
速度ベクトルを絶対基準として姿勢情報の補正を行う。
その結果、誤差が累積せず高精度に移動物体の三次元姿
勢角の測定及び計測することができる。
【００２７】また、自立式で姿勢情報の補正が適宜行わ
れるので、長時間使用しても、誤差が累積せず、三次元
姿勢角の測定・計測を高精度に、しかも簡便に行うこと
ができる。
れるので、長時間使用しても、誤差が累積せず、三次元
姿勢角の測定・計測を高精度に、しかも簡便に行うこと
ができる。
【００２８】さらに、移動物体の三次元姿勢角の計測値
を高精度に算出するに当たり、クォータニオンを使用し
ているので、補正ベクトルを算出する計算量を少なくで
き、その演算処理を効率的に行うことができる。
を高精度に算出するに当たり、クォータニオンを使用し
ているので、補正ベクトルを算出する計算量を少なくで
き、その演算処理を効率的に行うことができる。
【００２９】
【実施例】以下、添付図面を参照し、本発明をその装置
例及び方法例に基づいて、より詳細に説明する。なお、
本装置例及び方法例を適用する移動物体として、無人搬
送車、移動式建設機械、移動式土木機械、トンネル堀削
用ロボット、海中作業用ロボット、又は飛翔体（飛行
機、通信衛星、宇宙船等）等を挙げることができるが、
これらに限定するものでは無い。
例及び方法例に基づいて、より詳細に説明する。なお、
本装置例及び方法例を適用する移動物体として、無人搬
送車、移動式建設機械、移動式土木機械、トンネル堀削
用ロボット、海中作業用ロボット、又は飛翔体（飛行
機、通信衛星、宇宙船等）等を挙げることができるが、
これらに限定するものでは無い。
【００３０】（装置例）図１は本装置例の三次元姿勢計
測装置の構成を示している。三次元姿勢計測装置αは、
図を省略した移動物体に搭載され、図を省略した移動物
体の姿勢角を高精度に計測する。
測装置の構成を示している。三次元姿勢計測装置αは、
図を省略した移動物体に搭載され、図を省略した移動物
体の姿勢角を高精度に計測する。
【００３１】三次元姿勢計測装置αは、３個の角速度セ
ンサ１、３個の加速度センサ２、及び演算装置３からな
る。これら三次元姿勢計測装置αの各ユニットは、基底
盤４に配置されハウジング５内に収蔵されて、移動物体
に搭載される。
ンサ１、３個の加速度センサ２、及び演算装置３からな
る。これら三次元姿勢計測装置αの各ユニットは、基底
盤４に配置されハウジング５内に収蔵されて、移動物体
に搭載される。
【００３２】ここで角速度センサ１には、例えば、ピエ
ゾジャイロ等のジャイロセンサを使用し、加速度センサ
２には、例えば、圧電式加速度センサや半導体式加速度
センサを使用する。
ゾジャイロ等のジャイロセンサを使用し、加速度センサ
２には、例えば、圧電式加速度センサや半導体式加速度
センサを使用する。
【００３３】角速度センサ１と加速度センサ２は一対と
して独立な３軸（直交３軸）にそれぞれ配置される。演
算装置３は、角速度センサ１により得られた角速度の計
測値を時間的に積分して移動物体の三次元姿勢角の計測
値を算出するとともに、加速度センサ２から得られた重
力方向のそれぞれのベクトル又は合成ベクトルとしての
重力加速度ベクトルを観測ベクトルとする拡張カルマン
フィルタの適用により、移動物体の三次元姿勢角の計測
値を補正する機能を有する。
して独立な３軸（直交３軸）にそれぞれ配置される。演
算装置３は、角速度センサ１により得られた角速度の計
測値を時間的に積分して移動物体の三次元姿勢角の計測
値を算出するとともに、加速度センサ２から得られた重
力方向のそれぞれのベクトル又は合成ベクトルとしての
重力加速度ベクトルを観測ベクトルとする拡張カルマン
フィルタの適用により、移動物体の三次元姿勢角の計測
値を補正する機能を有する。
【００３４】（方法例）次に、上述した装置例の三次元
姿勢計測装置αを用いた移動物体の三次元姿勢角測定法
を、具体的に説明する。本方法例の三次元姿勢角測定推
定法は、ロール・ピッチ・ヨー角などによる表現に比
べ、微分方程式に三角関数を含まず、数値的特異点が無
い、数学者Hamiltonにより定義されたクォータニオン表
現（四元数とも呼ばれる）を用いる。
姿勢計測装置αを用いた移動物体の三次元姿勢角測定法
を、具体的に説明する。本方法例の三次元姿勢角測定推
定法は、ロール・ピッチ・ヨー角などによる表現に比
べ、微分方程式に三角関数を含まず、数値的特異点が無
い、数学者Hamiltonにより定義されたクォータニオン表
現（四元数とも呼ばれる）を用いる。
【００３５】
【外１】
【００３６】
【数１】
【００３７】
【外２】 単位クォータニオンと呼ばれ、三次元の姿勢を表現する
ことが可能となる。
ことが可能となる。
【００３８】続いて、三次元の姿勢を三次元空間内の単
位ベクトルｅを軸とするθ(rad) の回転で表すとき、ｅ
のベクトル成分をｅ＝（ｅx ｅy ｅz ）T として、単位
クォータニオンによる三次元姿勢角は次のように表され
る。
位ベクトルｅを軸とするθ(rad) の回転で表すとき、ｅ
のベクトル成分をｅ＝（ｅx ｅy ｅz ）T として、単位
クォータニオンによる三次元姿勢角は次のように表され
る。
【００３９】
【数２】
【００４０】かかるクォータニオンによる姿勢角表現を
用いれば、移動物体の姿勢角が時間変化するとき移動物
体に付随した空間座標における独立な３軸の角速度をω
＝（ω1 ω2 ω3 ）、時間をｔとして姿勢角の変化を微
分方程式で表すと次式となる。
用いれば、移動物体の姿勢角が時間変化するとき移動物
体に付随した空間座標における独立な３軸の角速度をω
＝（ω1 ω2 ω3 ）、時間をｔとして姿勢角の変化を微
分方程式で表すと次式となる。
【００４１】
【数３】
【００４２】すなわち、時間と共に姿勢が変化する移動
物体の時間経過後の姿勢角は、前式（３）を時間積分す
ることにより求められる。ここでωとしては、移動物体
内の角速度センサ１が検出する角速度を用いる。
物体の時間経過後の姿勢角は、前式（３）を時間積分す
ることにより求められる。ここでωとしては、移動物体
内の角速度センサ１が検出する角速度を用いる。
【００４３】クォータニオンによる姿勢角表現と、座標
変換を表す回転行列Ｒには、次のような関係が成り立
つ。
変換を表す回転行列Ｒには、次のような関係が成り立
つ。
【００４４】
【数４】 この式（４）をもとに、初期姿勢において移動物体に搭
載した加速度センサ２で測定される重力加速度ベクトル
を（ａx0 ａy0 ａz0）T とおき、
載した加速度センサ２で測定される重力加速度ベクトル
を（ａx0 ａy0 ａz0）T とおき、
【外３】 （ａx ａy ａz ）T とおくと、両者の間には次のよ
うな関係が成立する。
うな関係が成立する。
【００４５】
【数５】
【００４６】
【外４】 とするシステム式を次のように定義する。
【００４７】
【数６】
【００４８】
【数７】
【００４９】
【数８】
【００５０】ここで前式（６）のｗ1 、ｗ2 、ｗ3 は、
角速度センサ１で角速度を測定するときの計測誤差ベク
トルｗ（ｔ）＝（ｗ1 ｗ2 ｗ3 ）Ｔの各成分であり、
また、該式（７）のｖ（ｔ）は加速度センサ２で重力加
速度を測定するときの計測誤差ベクトルｖ（ｔ）＝（ｖ
1 ｖ2 ｖ3 ）Ｔであり、それぞれ前式（３）と前式
（５）にセンサの計測誤差を含んだ式となる。
角速度センサ１で角速度を測定するときの計測誤差ベク
トルｗ（ｔ）＝（ｗ1 ｗ2 ｗ3 ）Ｔの各成分であり、
また、該式（７）のｖ（ｔ）は加速度センサ２で重力加
速度を測定するときの計測誤差ベクトルｖ（ｔ）＝（ｖ
1 ｖ2 ｖ3 ）Ｔであり、それぞれ前式（３）と前式
（５）にセンサの計測誤差を含んだ式となる。
【００５１】前式（６）と前式（７）はそれぞれ拡張カ
ルマンを適用する際のシステム記述である状態方程式と
観測方程式になる。
ルマンを適用する際のシステム記述である状態方程式と
観測方程式になる。
【００５２】
【外５】
【００５３】ここで、（ａx ａy ａz ）T をある時
点ｔに加速度センサ２で計測される重力加速度ベクト
ル、Ｋ（ｔ）を拡張カルマンフィルタアルゴリズムによ
って得られるカルマンゲインとすると、
点ｔに加速度センサ２で計測される重力加速度ベクト
ル、Ｋ（ｔ）を拡張カルマンフィルタアルゴリズムによ
って得られるカルマンゲインとすると、
【００５４】
【外６】 を得ることができる。
【００５５】
【数９】
【００５６】
【外７】 一連の処理を繰り返すことで累積誤差の無い高精度な三
次元姿勢角の計測値が得られる。
次元姿勢角の計測値が得られる。
【００５７】次に、前記装置例の演算装置３にて計測値
の補正処理をする手順を図面を参照して具体的に説明す
る。図２は本方法例の処理手順を示したフローチャート
である。
の補正処理をする手順を図面を参照して具体的に説明す
る。図２は本方法例の処理手順を示したフローチャート
である。
【００５８】まず、角速度センサ１で測定される角速度
に基づいて状態行列〈式（３）〉を作成する（ステップ
I ）とともに、移動物体の初期状態からの姿勢変化の回
転を３次元単位ベクトル軸まわりの回転角によって表現
するクォータニオンを以って状態ベクトル〈式（４）〉
を作成する（ステップII）。
に基づいて状態行列〈式（３）〉を作成する（ステップ
I ）とともに、移動物体の初期状態からの姿勢変化の回
転を３次元単位ベクトル軸まわりの回転角によって表現
するクォータニオンを以って状態ベクトル〈式（４）〉
を作成する（ステップII）。
【００５９】続いて、ステップI の状態行列〈式
（３）〉とステップIIの状態ベクトル〈式（４）〉に基
づいて状態方程式〈式（６）〉を作成し（ステップIII
）、加速度センサ２で測定された重力方向を示す重力
加速度ベクトルを観測ベクトルとして、クォータニオン
を用いた座標回転を示す回転行列と重力加速度ベクトル
に基づいて観測方程式〈式（７）〉を作成する（ステッ
プIV）。
（３）〉とステップIIの状態ベクトル〈式（４）〉に基
づいて状態方程式〈式（６）〉を作成し（ステップIII
）、加速度センサ２で測定された重力方向を示す重力
加速度ベクトルを観測ベクトルとして、クォータニオン
を用いた座標回転を示す回転行列と重力加速度ベクトル
に基づいて観測方程式〈式（７）〉を作成する（ステッ
プIV）。
【００６０】次に、ステップIII の状態方程式〈式
（６）〉とステップIVの観測方程式〈式（７）〉からカ
ルマンゲインＫ（ｔ）を算出する（ステップV ）。
（６）〉とステップIVの観測方程式〈式（７）〉からカ
ルマンゲインＫ（ｔ）を算出する（ステップV ）。
【００６１】そして、回転行列と移動物体の初期位置に
おける重力加速度ベクトルとの積にて得られる算出値と
移動物体の移動後の重力加速度ベクトルとの差値にステ
ップV のカルマンゲインＫ（ｔ）を掛けて補正ベクトル
を算出する（ステップVI）。
おける重力加速度ベクトルとの積にて得られる算出値と
移動物体の移動後の重力加速度ベクトルとの差値にステ
ップV のカルマンゲインＫ（ｔ）を掛けて補正ベクトル
を算出する（ステップVI）。
【００６２】最後に、ステップI の状態行列に基づいて
角速度センサ１で測定される角速度を積分して得られる
状態ベクトル（クォータニオン）にステップVIの補正ベ
クトルを加えて、姿勢角の計測値を補正したクォータニ
オン〈式（８）〉を算出する（ステップVII ）。
角速度センサ１で測定される角速度を積分して得られる
状態ベクトル（クォータニオン）にステップVIの補正ベ
クトルを加えて、姿勢角の計測値を補正したクォータニ
オン〈式（８）〉を算出する（ステップVII ）。
【００６３】かかる手順を簡単に説明すると、すなわ
ち、姿勢変化する移動物体の内部におてい、三つの独立
な軸に配置された角速度センサ１の計測量ω＝（ω1 ω
2 ω3）と、それと対をなすように配置された加速度セ
ンサ２の計測量（ａx ａy ａz）T によって、角速度ω
＝（ω1 ω2 ω3 ）を積分することにより三次元姿勢角
の計測値を得る。そして、重力加速度ベクトル（ａx ａ
y ａz ）T を観測ベクトルとする拡張カルマンフィルタ
の適用により三次元姿勢角計測値を補正して高精度の姿
勢角計測値を得る。
ち、姿勢変化する移動物体の内部におてい、三つの独立
な軸に配置された角速度センサ１の計測量ω＝（ω1 ω
2 ω3）と、それと対をなすように配置された加速度セ
ンサ２の計測量（ａx ａy ａz）T によって、角速度ω
＝（ω1 ω2 ω3 ）を積分することにより三次元姿勢角
の計測値を得る。そして、重力加速度ベクトル（ａx ａ
y ａz ）T を観測ベクトルとする拡張カルマンフィルタ
の適用により三次元姿勢角計測値を補正して高精度の姿
勢角計測値を得る。
【００６４】以上本発明の代表的な装置例及び方法例に
ついて説明したが、本発明は必ずしもこれらの装置例の
手段及び方法例の手法だけに限定されるものではなく、
本発明にいう目的を達成し、後述する効果を有する範囲
内において適宜変更して実施することができるものであ
る。
ついて説明したが、本発明は必ずしもこれらの装置例の
手段及び方法例の手法だけに限定されるものではなく、
本発明にいう目的を達成し、後述する効果を有する範囲
内において適宜変更して実施することができるものであ
る。
【００６５】特に、移動物体は、別個に設けたコントロ
ール装置にて遠隔操作可能な構成とし、演算装置３の結
果を送信手段にてコントロール装置に送信するシステム
にしても良い。
ール装置にて遠隔操作可能な構成とし、演算装置３の結
果を送信手段にてコントロール装置に送信するシステム
にしても良い。
【００６６】
【発明の効果】以上説明したように本発明によれば、加
速度センサにて得られる重力加速度ベクトルを絶対基準
として姿勢情報の補正を行うので、空中、地上、地下、
水中など任意の場所でも三次元姿勢角の測定・計測を高
精度で行うことができるという効果を奏する。
速度センサにて得られる重力加速度ベクトルを絶対基準
として姿勢情報の補正を行うので、空中、地上、地下、
水中など任意の場所でも三次元姿勢角の測定・計測を高
精度で行うことができるという効果を奏する。
【図１】本発明に係る装置例の三次元姿勢角計測装置の
構成例を示した斜視図である。
構成例を示した斜視図である。
【図２】同上の装置例を用いた本方法例の手順を示した
フローチャートである。
フローチャートである。
【図３】従来の三次元姿勢角計測装置の構成例を示した
斜視図である。
斜視図である。
１、１０…角速度センサ ２…加速度センサ ３、１１…演算装置 ４、１２…基底盤 α…三次元姿勢角計測装置（本発明の装置例） β…三次元姿勢角計測装置（従来例）
Claims (11)
- 【請求項１】移動物体の独立な３軸方向の角速度をそれ
ぞれ計測して当該移動物体の三次元姿勢角を計測するに
当り、 当該三次元姿勢角の計測値を、当該移動物体の重力加速
度ベクトルを絶対基準として補正する、 ことを特徴とする移動物体の三次元姿勢角測定法。 - 【請求項２】前記移動物体の重力加速度ベクトルは、 前記移動物体の独立な３軸方向の加速度のそれぞれの計
測値に基づいて重力方向のそれぞれのベクトル又は合成
ベクトルとして求められる、 ことを特徴とする請求項１に記載の移動物体の三次元姿
勢角測定法。 - 【請求項３】移動物体に搭載され、当該移動物体の独立
な３軸方向の角速度を計測する角速度センサと、当該移
動物体に搭載され、当該移動物体の独立な３軸方向の加
速度を計測する加速度センサと、演算装置とを備えた移
動物体の三次元姿勢角計測装置において、 前記演算装置にてクォータニオンをもとに移動物体の三
次元姿勢角を測定するに当たり、 前記角速度センサで測定される角速度に基づいて状態行
列を作成するステップI と、 前記移動物体の初期状態からの姿勢変化の回転を３次元
単位ベクトル軸まわりの回転角によって表現するクォー
タニオンを以って状態ベクトルを作成するステップII
と、 ステップI の状態行列とステップIIの状態ベクトルに基
づいて状態方程式を作成するステップIII と、 前記加速度センサで測定された重力方向を示す重力加速
度ベクトルを観測ベクトルとして、前記クォータニオン
を用いた座標回転を示す回転行列と前記重力加速度ベク
トルに基づいて観測方程式を作成するステップIVと、 ステップIII の状態方程式とステップIVの観測方程式か
らカルマンゲインを求めるステップV と、 前記回転行列と前記移動物体の初期位置における重力加
速度ベクトルとの積にて得られる算出値と前記移動物体
の移動後の重力加速度ベクトルとの差値にステップV の
カルマンゲインを掛けて補正ベクトルを算出するステッ
プVIと、 ステップI の状態行列に基づいて前記角速度センサで測
定される角速度を積分して得られる状態ベクトル（クォ
ータニオン）にステップVIの補正ベクトルを加えて、補
正後の姿勢角を表すクォータニオンを算出するステップ
VII と、 を順次経由する、 ことを特徴とする移動物体の三次元姿勢角測定法。 - 【請求項４】前記演算装置は、 前記角速度センサ及び前記加速度センサとともに基底盤
に配置され、前記移動物体に搭載された、 ことを特徴とする請求項３に記載の移動物体の三次元姿
勢角測定法。 - 【請求項５】前記移動物体は、 別個に設けられたコントロール装置にて遠隔操作可能な
構成とし、前記演算装置の結果を送信手段にて当該コン
トロール装置に送信する、 ことを特徴とする請求項３又は４に記載の移動物体の三
次元姿勢角測定法。 - 【請求項６】前記移動物体は、 無人搬送車、移動式建設機械、移動式土木機械、トンネ
ル堀削用ロボット、海中作業用ロボット、又は飛翔体か
らなる、 ことを特徴とする請求項３、４又は５に記載の移動物体
の三次元姿勢角測定法。 - 【請求項７】移動体に搭載され、当該移動物体の独立な
３軸方向の角速度を計測する角速度センサと、 前記移動体に搭載され、当該移動物体の独立な３軸方向
の加速度を計測する加速度センサと、 前記角速度センサにより得られた角速度の計測値を時間
的に積分して移動物体の三次元姿勢角の計測値を算出す
るとともに、前記加速度センサから得られた重力加速度
ベクトルを観測ベクトルとする拡張カルマンフィルタの
適用により当該三次元姿勢角の計測値を補正する機能を
有する演算装置と、を備えた、 ことを特徴とする移動物体の三次元姿勢角計測装置。 - 【請求項８】前記演算装置は、 クォータニオンを前記移動物体の三次元姿勢角の計測値
とするために、 前記角速度センサで測定される角速度に基づいて状態行
列を作成する第１処理手段と、 前記移動体の初期状態からの姿勢変化の回転を３次元単
位ベクトル軸まわりの回転角によって表現するクォータ
ニオンを以って状態ベクトルを作成する第２処理手段
と、 第１処理手段の状態行列と第２処理手段の状態ベクトル
に基づいて状態方程式を作成する第３処理手段と、 前記加速度センサで測定された重力方向を示す重力加速
度ベクトルを観測ベクトルとして、前記クォータニオン
を用いた座標回転を示す回転行列と前記重力加速度ベク
トルに基づいて観測方程式を作成する第４処理手段と、 第３処理手段の状態方程式と第４処理手段の観測方程式
からカルマンゲインを求める第５処理手段と、 前記回転行列と前記移動体の初期位置における重力加速
度ベクトルとの積により得られる算出値と、前記移動体
の移動後の重力加速度ベクトルとの差に第５処理手段の
カルマンゲインを掛けて補正ベクトルを得る第６処理手
段と、 第１処理手段の状態行列に基づいて前記角速度センサで
測定される角速度を積分して得られる状態ベクトル（ク
ォータニオン）に第６処理手段の補正ベクトルを加え
て、補正後の姿勢角を表すクォータニオンを得る第７処
理手段と、を備えた、 ことを特徴とする移動物体の三次元姿勢角計測装置。 - 【請求項９】前記演算装置は、 前記角速度センサ及び前記加速度センサとともに基底盤
に一体配置され、前記移動物体に搭載された、 ことを特徴とする請求項７又は８に記載の移動物体の三
次元姿勢角計測装置。 - 【請求項１０】前記移動物体は、 別個に設けられたコントロール装置にて遠隔操作可能な
構成とし、前記演算装置の結果を当該コントロール装置
に送信する送信手段を備えた、 ことを特徴とする請求項７、８又は９に記載の移動物体
の三次元姿勢角計測装置。 - 【請求項１１】前記移動物体は、 無人搬送車、移動式建設機械、移動式土木機械、トンネ
ル堀削用ロボット、海中作業用ロボット、又は飛翔体か
らなる、 ことを特徴とする請求項７、８、９又は１０に記載の移
動物体の三次元姿勢角計測装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP15774695A JPH095104A (ja) | 1995-06-23 | 1995-06-23 | 移動物体の三次元姿勢角測定法および三次元姿勢角計測装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP15774695A JPH095104A (ja) | 1995-06-23 | 1995-06-23 | 移動物体の三次元姿勢角測定法および三次元姿勢角計測装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JPH095104A true JPH095104A (ja) | 1997-01-10 |
Family
ID=15656450
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP15774695A Pending JPH095104A (ja) | 1995-06-23 | 1995-06-23 | 移動物体の三次元姿勢角測定法および三次元姿勢角計測装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPH095104A (ja) |
Cited By (18)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2006514294A (ja) * | 2003-03-17 | 2006-04-27 | ノースロップ グルマン コーポレーション | 力に依存するジャイロスコープの感度を測定する方法 |
| JP2006194822A (ja) * | 2005-01-17 | 2006-07-27 | Oyo Corp | 加速度センサを用いる地盤等の変位モニタリング方法 |
| JP2007183138A (ja) * | 2006-01-05 | 2007-07-19 | Kenzo Nonami | 小型姿勢センサ |
| JP2007322392A (ja) * | 2006-06-05 | 2007-12-13 | Nippon Telegr & Teleph Corp <Ntt> | 姿勢平滑化方法およびそのプログラム |
| JP2008310767A (ja) * | 2007-06-18 | 2008-12-25 | Toyota Central R&D Labs Inc | 姿勢信号演算装置 |
| JP2011203257A (ja) * | 2005-08-01 | 2011-10-13 | Toyota Motor Corp | 運動体の姿勢角検出装置 |
| CN102313548A (zh) * | 2011-09-29 | 2012-01-11 | 王皓冰 | 一种基于三维立体封装技术的微型姿态航向参考系统 |
| US8200452B2 (en) | 2006-06-23 | 2012-06-12 | Toyota Jidosha Kabushiki Kaisha | Attitude-angle detecting apparatus and attitude-angle detecting method |
| JP2013054009A (ja) * | 2011-09-06 | 2013-03-21 | Toyota Motor Corp | 姿勢推定装置、姿勢推定方法、姿勢推定プログラム |
| KR20140054053A (ko) * | 2011-08-16 | 2014-05-08 | 로베르트 보쉬 게엠베하 | 요 레이트 센서 유닛의 출력 신호 분석 방법 및 요 레이트 센서 유닛 |
| US8771504B2 (en) | 2009-12-22 | 2014-07-08 | Zodiac Pool Care Europe | Apparatus for cleaning an immersed surface provided with an accelerometer device which detects gravitational acceleration |
| CN104034332A (zh) * | 2014-06-20 | 2014-09-10 | 东南大学 | 一种基于卡尔曼滤波的救援清障车姿态角估计方法 |
| US9043177B2 (en) | 2010-03-05 | 2015-05-26 | Seiko Epson Corporation | Posture information calculation device, posture information calculation system, posture information calculation method, and information storage medium |
| CN106325276A (zh) * | 2016-09-21 | 2017-01-11 | 苏州瑞得恩自动化设备科技有限公司 | 机器人在斜坡平面上直线行驶的判定方法及控制方法 |
| US9630672B2 (en) | 2009-12-21 | 2017-04-25 | Yamaha Hatsudoki Kabushiki Kaisha | Roll angle estimation device and transport equipment |
| JP2017538919A (ja) * | 2014-11-11 | 2017-12-28 | インテル コーポレイション | 拡張カルマンフィルタベースの自律的磁力計校正 |
| CN108981694A (zh) * | 2018-07-18 | 2018-12-11 | 兰州交通大学 | 基于小波神经网络与ekf的姿态解算方法及系统 |
| CN113959430A (zh) * | 2021-10-13 | 2022-01-21 | 广东汇天航空航天科技有限公司 | 飞行汽车的航姿确定方法、装置、车载终端及存储介质 |
-
1995
- 1995-06-23 JP JP15774695A patent/JPH095104A/ja active Pending
Cited By (27)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2006514294A (ja) * | 2003-03-17 | 2006-04-27 | ノースロップ グルマン コーポレーション | 力に依存するジャイロスコープの感度を測定する方法 |
| JP4929445B2 (ja) * | 2003-03-17 | 2012-05-09 | ノースロップ グラマン システムズ コーポレーション | 力に依存するジャイロスコープの感度を測定する方法 |
| JP2006194822A (ja) * | 2005-01-17 | 2006-07-27 | Oyo Corp | 加速度センサを用いる地盤等の変位モニタリング方法 |
| JP2011203257A (ja) * | 2005-08-01 | 2011-10-13 | Toyota Motor Corp | 運動体の姿勢角検出装置 |
| JP2007183138A (ja) * | 2006-01-05 | 2007-07-19 | Kenzo Nonami | 小型姿勢センサ |
| JP2007322392A (ja) * | 2006-06-05 | 2007-12-13 | Nippon Telegr & Teleph Corp <Ntt> | 姿勢平滑化方法およびそのプログラム |
| JP4759446B2 (ja) * | 2006-06-05 | 2011-08-31 | 日本電信電話株式会社 | 姿勢平滑化方法およびそのプログラム |
| US8200452B2 (en) | 2006-06-23 | 2012-06-12 | Toyota Jidosha Kabushiki Kaisha | Attitude-angle detecting apparatus and attitude-angle detecting method |
| JP2008310767A (ja) * | 2007-06-18 | 2008-12-25 | Toyota Central R&D Labs Inc | 姿勢信号演算装置 |
| US9630672B2 (en) | 2009-12-21 | 2017-04-25 | Yamaha Hatsudoki Kabushiki Kaisha | Roll angle estimation device and transport equipment |
| US8771504B2 (en) | 2009-12-22 | 2014-07-08 | Zodiac Pool Care Europe | Apparatus for cleaning an immersed surface provided with an accelerometer device which detects gravitational acceleration |
| EP2516774B1 (fr) | 2009-12-22 | 2016-03-30 | Zodiac Pool Care Europe | Appareil nettoyeur de surface immergée muni d'un dispositif accélérométrique détectant l'accélération gravitationnelle |
| US9631389B2 (en) | 2009-12-22 | 2017-04-25 | Zodiac Pool Care Europe | Apparatus for cleaning an immersed surface provided with an accelerometer device which detects gravitational acceleration |
| US9043177B2 (en) | 2010-03-05 | 2015-05-26 | Seiko Epson Corporation | Posture information calculation device, posture information calculation system, posture information calculation method, and information storage medium |
| KR20140054053A (ko) * | 2011-08-16 | 2014-05-08 | 로베르트 보쉬 게엠베하 | 요 레이트 센서 유닛의 출력 신호 분석 방법 및 요 레이트 센서 유닛 |
| US9671227B2 (en) | 2011-08-16 | 2017-06-06 | Robert Bosch Gmbh | Method for evaluating output signals of a rotational rate sensor unit and rotational rate sensor unit |
| JP2014524573A (ja) * | 2011-08-16 | 2014-09-22 | ローベルト ボツシユ ゲゼルシヤフト ミツト ベシユレンクテル ハフツング | ヨーレートセンサユニットの出力信号の評価方法、及び、ヨーレートセンサユニット |
| JP2013054009A (ja) * | 2011-09-06 | 2013-03-21 | Toyota Motor Corp | 姿勢推定装置、姿勢推定方法、姿勢推定プログラム |
| CN102313548A (zh) * | 2011-09-29 | 2012-01-11 | 王皓冰 | 一种基于三维立体封装技术的微型姿态航向参考系统 |
| CN104034332A (zh) * | 2014-06-20 | 2014-09-10 | 东南大学 | 一种基于卡尔曼滤波的救援清障车姿态角估计方法 |
| JP2017538919A (ja) * | 2014-11-11 | 2017-12-28 | インテル コーポレイション | 拡張カルマンフィルタベースの自律的磁力計校正 |
| CN106325276A (zh) * | 2016-09-21 | 2017-01-11 | 苏州瑞得恩自动化设备科技有限公司 | 机器人在斜坡平面上直线行驶的判定方法及控制方法 |
| WO2018053983A1 (zh) * | 2016-09-21 | 2018-03-29 | 苏州瑞得恩光能科技有限公司 | 机器人在斜坡平面上直线行驶的判定方法及控制方法 |
| US10802500B2 (en) | 2016-09-21 | 2020-10-13 | Suzhou Radiant Photovoltaic Technology Co., Ltd | Determining method and control method for straight running of robot on slope plane |
| CN108981694A (zh) * | 2018-07-18 | 2018-12-11 | 兰州交通大学 | 基于小波神经网络与ekf的姿态解算方法及系统 |
| CN113959430A (zh) * | 2021-10-13 | 2022-01-21 | 广东汇天航空航天科技有限公司 | 飞行汽车的航姿确定方法、装置、车载终端及存储介质 |
| CN113959430B (zh) * | 2021-10-13 | 2023-12-22 | 广东汇天航空航天科技有限公司 | 飞行汽车的航姿确定方法、装置、车载终端及存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107588769B (zh) | 一种车载捷联惯导、里程计及高程计组合导航方法 | |
| US6409687B1 (en) | Motion tracking system | |
| JP5383801B2 (ja) | 位置及び経路地図表示用の位置及び経路地図データを生成する装置及び当該データ提供方法 | |
| JPH095104A (ja) | 移動物体の三次元姿勢角測定法および三次元姿勢角計測装置 | |
| Aghili et al. | Driftless 3-D attitude determination and positioning of mobile robots by integration of IMU with two RTK GPSs | |
| US9026263B2 (en) | Automotive navigation system and method to utilize internal geometry of sensor position with respect to rear wheel axis | |
| JP4726134B2 (ja) | 移動体制御装置及び移動体制御方法 | |
| US11428823B2 (en) | Methods, apparatuses, and computer programs for estimating the heading of an axis of a rigid body | |
| JP2010032398A (ja) | ナビゲーションシステムの位置検出装置および位置検出方法 | |
| JP6170983B2 (ja) | 慣性航法システム及び慣性航法システムにおける磁気異常検出支援を提供する方法 | |
| US11408735B2 (en) | Positioning system and positioning method | |
| US4347573A (en) | Land-vehicle navigation system | |
| CN111189474A (zh) | 基于mems的marg传感器的自主校准方法 | |
| Goppert et al. | Invariant Kalman filter application to optical flow based visual odometry for UAVs | |
| CN111189442A (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
| US9217639B1 (en) | North-finding using inertial navigation system | |
| RU2487318C1 (ru) | Бесплатформенная инерциальная курсовертикаль на чувствительных элементах средней точности | |
| Baumgartner et al. | State estimation and vehicle localization for the FIDO rover | |
| JP2008241079A (ja) | 航法システム | |
| Jun et al. | State estimation via sensor modeling for helicopter control using an indirect kalman filter | |
| RU2345326C1 (ru) | Способ коррекции инерциальной навигационной системы | |
| KR101916908B1 (ko) | 위치 추정 시스템 및 위치 추정 시스템을 포함하는 무인 이동 장치 | |
| Vepa | Ambulatory position tracking of prosthetic limbs using multiple satellite aided inertial sensors and adaptive mixing | |
| JPH11325951A (ja) | 宇宙航行体の姿勢センサによる軌道決定方法および軌道決定装置 | |
| Doust et al. | Reducing the navigational error of a flying object in space by using an integration of GPS and INS data |