JPH06174487A - 姿勢検出装置 - Google Patents

姿勢検出装置

Info

Publication number
JPH06174487A
JPH06174487A JP4359700A JP35970092A JPH06174487A JP H06174487 A JPH06174487 A JP H06174487A JP 4359700 A JP4359700 A JP 4359700A JP 35970092 A JP35970092 A JP 35970092A JP H06174487 A JPH06174487 A JP H06174487A
Authority
JP
Japan
Prior art keywords
acceleration
angular velocity
angle
rotation angular
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.)
Pending
Application number
JP4359700A
Other languages
English (en)
Inventor
Haruo Nonin
治夫 能任
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Individual
Original Assignee
Individual
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Individual filed Critical Individual
Priority to JP4359700A priority Critical patent/JPH06174487A/ja
Publication of JPH06174487A publication Critical patent/JPH06174487A/ja
Pending legal-status Critical Current

Links

Abstract

(57)【要約】 【目的】 回転運動及び並進運動する物体の回転角速
度、姿勢角、位置標定用加速度情報を加速度センサーの
出力のみから推定する軽量・安価な装置を提供する。 【構成】 物体の内部または表面に特定の位置関係及び
加速度検出方向をもって配置した複数の加速度センサー
によって、運動時に生じる各部の微小な加速度差を検出
し、それより回転角加速度及び位置計算用加速度情報を
推定する。つぎに回転角加速度の推定値を積分すること
により回転角速度を推定し、それを微小な時間区間で再
び積分することにより、その時間区間における回転角の
増分を推定する。つぎに、コータニオンパラメータを用
いて運動物体に固定された座標から慣性座標への座標変
換行列を求める。最後に座標変換行列より姿勢角を求め
出力する。

Description

【発明の詳細な説明】
【0001】
【産業上の利用分野】回転運動及び並進運動を同時また
は交互に行う物体の姿勢角検出に使用可能。たとえば、
船舶、自動車、航空機、ロボット等。
【0002】
【従来の技術】運動物体の姿勢角検出の分野では例外な
くジャイロが用いられている。ジャイロには剛体ジャイ
ロ、レーザジャイロ、光ファイバジャイロ等がある。位
置の標定にはジャイロと組んで加速度センサーが用いら
れている。加速度センサーは位置の計算のみを目的とし
て使われている。
【0003】
【課題】ジャイロは重量、体積、消費電力が大きく搭載
上不利であるばかりでなく、精密機器であることから寿
命も比較的短く、高価で取扱いに注意がいる。これらの
欠点を解消する姿勢角検出の手段が望まれている。
【0004】
【課題を解決するための手段】慣性空間において不規則
な回転角加速度を持つ回転運動及び不規則な加速度を持
つ並進運動を同時または交互に行う物体の内部または表
面上にとった複数の点は、それぞれ異なった方向および
値の加速度を持つ。本発明は物体の内部または表面に特
定の位置関係及び検出方向をもって配置した複数の加速
度センサーによって、運動時に生じる各部の微少な加速
度差を検出し、それより回転角速度、姿勢角、位置標定
用加速度情報を推定する装置である。図1に本発明の構
成図を示す。大きく分けて、センサー部1、前処理部
2、積分部3、積分部4、コータニオン更新計算部5、
姿勢角計算部6、コータニオン初期値計算部7からな
る。以下、各ブロックごとに動作を箇条書きにする。
【0005】(イ)センサー部1は、図2に示す運動物
体24に取り付けられた複数の加速度センサーからな
る。図2を説明する。運動物体24の重心を原点12と
し物体に固定されたXYZ座標を定義する。Y座標軸2
0上、原点から正方向に一定距離(これを腕の長さLp
と呼ぶ)離れた場所に加速度センサー14を取り付け
る。矢印22は座標軸の正方向を示す。負方向に同一距
離はなれた場所にも加速度センサー13を取り付ける。
両方とも加速度検出方向はXY面に対し垂直とする。矢
印23は加速度検出方向を示す。Z座標軸21上、原点
から正方向及び負方向に腕の長さLq分はなれた場所に
2個の加速度センサー18、17を取り付ける。両方と
も加速度検出方向はYZ面に対し垂直とする。X座標軸
上、原点から正方向及び負方向に腕の長さLr分はなれ
た場所に2個の加速度センサー15、16を取り付け
る。両方とも加速度検出方向はXZ面に対し垂直とす
る。センサー部1は、それぞれの座標軸に取り付けられ
た加速度センサーの検出値を前処理部2へ出力する。X
座標軸19に取り付けられた加速度センサー15、16
の検出値をarL、arRと置く。Y座標軸20に取り
付けらた加速度センサー13、14の検出値をapL、
apRと置く。Z座標軸21に取り付けられた加速度セ
ンサー18、17の検出値をaqL、aqRと置く。
【0006】(ロ)前処理部2はセンサー部1からの入
力を処理してX座標軸19回りの回
【0007】推定の計算式を数式1に示す。数式1は次
の原理による。一定角速度ばかりでなく時間的に不規則
な回転角速度をもつ回転運動をおこなっている棒の両端
に取り付けられた2個の加速度センサーは、それぞれ異
なった加速度を検出する。加速度の検出方向は棒の両端
が描く円の接線方向とする。また回転中心は棒の重心位
置とし、棒の中間にあるとする。このとき、2個の加速
度センサーの出力差は棒の回転角加速度に比例する。ま
た、棒が回転運動しながら並進運動も同時に行っている
とすると、出力和は棒の並進運動加速度を検出する。
【0008】
【数式1】 速度情報として装置の外へ出力する。これは図1の出力
9で示される。
【0009】(ハ)積分部は前処理部において推定した
回転角加速度の推定値を積分し回転角速度の推定値を求
める。この処理を連続系で記述すると数式2となる。数
式2を微小な時間キザミΔTで離散的に表現すると数式
3となり、数式3を用いて数式2を離散化すると数式4
となる。姿勢検出装置の中で行う実際の計算 積分部4及び装置の外へ出力する。これは図1の出力1
0で示される。
【0010】
【数式2】
【0011】
【数式3】
【0012】
【数式4】
【0013】(ニ)積分部は前の積分部において求めた
回転角速度の推定値を、もう一度微小な時間区間[t,
t+ΔT]で積分し、その時間区間における回転角度の
増分 分を表す。
【0014】
【数式5】 これを同様に数式3を用いて離散化すると数式6とな
る。
【0015】
【数式6】 実際の計算処理には離散化した数式6を用いる。
【0016】(ホ)コータニオン更新計算部は積分部で
求めた回転角度の増分を用いて、コータニオンパラメー
タの更新を行う。更新の計算は従来の方法を高速化した
数式7を用いる。これをコータニオンパラメータの高速
更新計算法と言うことにする。数式7では1次から4次
まで4通りの計算式を記述してあるが、必要な精度に応
じて、どれかひとつの式を選択する。次数が大きくなる
ほど精度は高い。
【0017】
【数式7】
【0018】(ヘ)姿勢角計算部は、まずコータニオン
更新計算部で更新されたコータニオンパラメータを用い
て運動物体固定座標から慣性座標への座標変換行列を求
める。 そのあと、座標変換行列から運動物体の姿勢角
(オイラー角)を求める。座標変換行列の計算は数式8
による。関数signを数式9のように定義すると、姿
勢角の計算は数式10による。求まった姿勢角θ、φ、
φは装置の外へ出力される。これは図1の出力11で示
される。
【0019】
【数式8】
【0020】
【数式9】
【0021】
【数式10】
【0022】(ト)コータニオン初期値計算部は初期姿
勢角に対応するコータニオンパラメータを求める。初期
姿勢角は運動物体に固定された座標の慣性座標に対する
姿勢角(オイラー角)である。オイラー角の変換の順序
はθ→φ→φである。求められたコータニオンパラメー
タはコータニオン更新計算部(ホ)に初期値を与える。
計算式は数式10に示す。
【0023】
【数式10】
【0024】
【作用】慣性空間において不規則な回転運動及び並進運
動を同時または交互に行う物体の内部または表面の異な
った点は、それぞれ異なった加速度を持つ。本発明の作
用は次の様に書ける。物体の内部または表面に特定の位
置関係及び加速度検出方向をもって配置した複数の加速
度センサーによって運動時に生じる微小な加速度差を検
出し、それより回転角加速度及び位置計算用加速度情報
を推定する。つぎに回転角加速度(単位rad/sec
)の推定値を積分することにより回転角速度(単位r
ad/sec)を推定し、それを微小な時間区間で再び
積分することにより、その時間区間における回転角の増
分(単位rad)を推定する。つぎに、コータニオンパ
ラメータを用いて運動物体に固定された座標から慣性座
標への座標変換行列を求める。このとき、さきほどの回
転角の増分推定値を使う。最後に座標変換行列より姿勢
角を求め出力する。
【0025】
【実施例】本発明を航空機の姿勢角検出に応用した例を
計算機シミュレーションによって示す。図3に示すごと
く加速度センサーを航空機に取り付ける。腕の長さは5
mである。原点に取り付けた加速度センサーはY軸方向
の位置計算用加速度情報をとりだすためのものである。
加速度センサーのモデルを図4に示す。座標軸は機体の
重心を原点とするXYZ座標とする。機体は6自由度モ
デルを用い、重心を中心として回転運動を行うとする。
適当に操舵して機体が回転運動及び並進運動したとき、
本発明の装置が姿勢角の真値にどれだけ近い値を出力す
るか、加速度センサーの最小検出G25(=最小検出加
速度)をパラメータとして例示する。最大検出G26は
20Gとする。図2と加速度センサーの取付方は異なっ
ているが計算処理の考え方は同じである。結果を図5、
図6、図7に示す。図5は姿勢角θの推定値と真値の
差、図6は姿勢角φの推定値と真値の差、図7は姿勢角
φの推定値と真値の差である。
【0026】
【発明の効果】図5、図6、図7を見ると、本発明は加
速度センサーの最小検出G25が小さければ十分実用に
なることを示している。ちなみに最小検出G25が0.
01mG(ミリG)の製品は販売されている。本発明で
は姿勢角を検出するのに加速度センサーと信号処理用の
マイクロプロセッサのみを用いジャイロを使わない。し
たがって、軽量にしかも安価にできる。耐故障の面から
も有利である。ただし、精度の面でジャイロより劣り、
長時間では誤差が蓄積するので模型飛行機のように短時
間の飛行で軽量化が要求される用途に応用可能である。
【0027】
【図面の簡単な説明】
【図1】本発明の原理を示すブロック図。
【図2】加速度センサー配置の一例。
【図3】本発明を航空機に適用した場合の加速度センサ
ー配置の一例。
【図4】加速度センサーのモデル。
【図5】加速度センサーの最小検出Gをパラメータとし
た姿勢角θの推定値と真値の差。
【図6】加速度センサーの最小検出Gをパラメータとし
た姿勢角φの推定値と真値の差。
【図7】加速度センサーの最小検出Gをパラメータとし
た姿勢角φの推定値と真値の差。
【0028】
【符号の説明】
1 : センサー部 2 : 前処理部 3 : 積分部 4 : 積分部 5 : コータニオン更新計算部 6 : 姿勢角計算部 7 : コータニオン初期値計算部 8 : 初期姿勢角入力 11 : 出力(姿勢角の推定値θ、φ、φを出力す
る) 12 : 原点 13 : 加速度センサー 14 : 加速度センサー 15 : 加速度センサー 16 : 加速度センサー 17 : 加速度センサー 18 : 加速度センサー 19 : X座標軸 20 : Y座標軸 21 : Z座標軸 22 : 矢印(座標軸の正方向を示す) 23 : 矢印(加速度センサーの検出方向を示す) 24 : 運動物体の輪郭 25 : 最小検出G(=最小検出加速度) 26 : 最大検出G(=最大検出加速度)
フロントページの続き (51)Int.Cl.5 識別記号 庁内整理番号 FI 技術表示箇所 G05D 1/10 9323−3H // B63B 39/00 9035−3D

Claims (2)

    【特許請求の範囲】
  1. 【請求項1】 回転運動及び並進運動を同時または交互
    に行う物体、たとえば、船舶、自動車、航空機、ロボッ
    ト等の内部または表面に、特定の位置関係及び検出方向
    をもって配置した複数の加速度センサーの検出値から物
    体の回転角速度、姿勢角、及び位置標定用加速度情報を
    推定する姿勢検出装置。
  2. 【請求項2】 運動物体固定座標から慣性空間座標への
    変換マトリクスを求めるために使用されるコータニオン
    パラメータの高速更新計算法を組み込んだ請求項1の姿
    勢検出装置。
JP4359700A 1992-12-10 1992-12-10 姿勢検出装置 Pending JPH06174487A (ja)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP4359700A JPH06174487A (ja) 1992-12-10 1992-12-10 姿勢検出装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP4359700A JPH06174487A (ja) 1992-12-10 1992-12-10 姿勢検出装置

Publications (1)

Publication Number Publication Date
JPH06174487A true JPH06174487A (ja) 1994-06-24

Family

ID=18465852

Family Applications (1)

Application Number Title Priority Date Filing Date
JP4359700A Pending JPH06174487A (ja) 1992-12-10 1992-12-10 姿勢検出装置

Country Status (1)

Country Link
JP (1) JPH06174487A (ja)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007322392A (ja) * 2006-06-05 2007-12-13 Nippon Telegr & Teleph Corp <Ntt> 姿勢平滑化方法およびそのプログラム
JP2009528547A (ja) * 2006-03-15 2009-08-06 クゥアルコム・インコーポレイテッド センサベースのオリエンテーションシステム
JP4860697B2 (ja) * 2005-08-01 2012-01-25 トヨタ自動車株式会社 加速度センサの補正装置および加速度センサの出力値補正方法
KR101323785B1 (ko) * 2012-10-16 2013-10-31 숭실대학교산학협력단 입체 회전각 측정장치 및 입체 회전각 측정방법
JP5880726B2 (ja) * 2012-10-16 2016-03-09 トヨタ自動車株式会社 倒立型移動体及びその制御方法
CN107194968A (zh) * 2017-05-18 2017-09-22 腾讯科技(上海)有限公司 图像的识别跟踪方法、装置、智能终端和可读存储介质
CN110160553A (zh) * 2019-06-06 2019-08-23 中北大学 被约束体微小姿态动态测试装置以及解算方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS57211509A (en) * 1981-06-24 1982-12-25 Clarion Co Ltd Detector for position of moving body
JPS59171800A (ja) * 1983-03-16 1984-09-28 宇宙開発事業団 人工衛星の姿勢決定方式
JPS617414A (ja) * 1984-06-21 1986-01-14 Gokou Shoji Kk 運輸機械の動態計測装置
JPH0343807A (ja) * 1989-07-11 1991-02-25 Tsubakimoto Chain Co 自在走行搬送車
US5050087A (en) * 1989-11-29 1991-09-17 Westinghouse Electric Corp. System and method for providing accurate attitude measurements at remote locations within an aircraft
JPH04107709A (ja) * 1990-08-29 1992-04-09 Yamaha Motor Co Ltd 移動体の位置検知装置
JPH06175724A (ja) * 1992-12-02 1994-06-24 Tsubakimoto Chain Co 搬送車の走行制御方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS57211509A (en) * 1981-06-24 1982-12-25 Clarion Co Ltd Detector for position of moving body
JPS59171800A (ja) * 1983-03-16 1984-09-28 宇宙開発事業団 人工衛星の姿勢決定方式
JPS617414A (ja) * 1984-06-21 1986-01-14 Gokou Shoji Kk 運輸機械の動態計測装置
JPH0343807A (ja) * 1989-07-11 1991-02-25 Tsubakimoto Chain Co 自在走行搬送車
US5050087A (en) * 1989-11-29 1991-09-17 Westinghouse Electric Corp. System and method for providing accurate attitude measurements at remote locations within an aircraft
JPH04107709A (ja) * 1990-08-29 1992-04-09 Yamaha Motor Co Ltd 移動体の位置検知装置
JPH06175724A (ja) * 1992-12-02 1994-06-24 Tsubakimoto Chain Co 搬送車の走行制御方法

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4860697B2 (ja) * 2005-08-01 2012-01-25 トヨタ自動車株式会社 加速度センサの補正装置および加速度センサの出力値補正方法
US8910522B2 (en) 2006-03-15 2014-12-16 Qualcomm Incorporated Sensor-based orientation system
JP2009528547A (ja) * 2006-03-15 2009-08-06 クゥアルコム・インコーポレイテッド センサベースのオリエンテーションシステム
JP2011141284A (ja) * 2006-03-15 2011-07-21 Qualcomm Inc センサベースのオリエンテーションシステム
US8006556B2 (en) 2006-03-15 2011-08-30 Qualcomm Incorporated Sensor-based orientation system
JP4759446B2 (ja) * 2006-06-05 2011-08-31 日本電信電話株式会社 姿勢平滑化方法およびそのプログラム
JP2007322392A (ja) * 2006-06-05 2007-12-13 Nippon Telegr & Teleph Corp <Ntt> 姿勢平滑化方法およびそのプログラム
KR101323785B1 (ko) * 2012-10-16 2013-10-31 숭실대학교산학협력단 입체 회전각 측정장치 및 입체 회전각 측정방법
JP5880726B2 (ja) * 2012-10-16 2016-03-09 トヨタ自動車株式会社 倒立型移動体及びその制御方法
CN107194968A (zh) * 2017-05-18 2017-09-22 腾讯科技(上海)有限公司 图像的识别跟踪方法、装置、智能终端和可读存储介质
CN107194968B (zh) * 2017-05-18 2024-01-16 腾讯科技(上海)有限公司 图像的识别跟踪方法、装置、智能终端和可读存储介质
CN110160553A (zh) * 2019-06-06 2019-08-23 中北大学 被约束体微小姿态动态测试装置以及解算方法
CN110160553B (zh) * 2019-06-06 2023-03-28 中北大学 被约束体微小姿态动态测试装置以及解算方法

Similar Documents

Publication Publication Date Title
JP5061264B1 (ja) 小型姿勢センサ
CN107560613B (zh) 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法
CN110125936B (zh) 一种空间机器人的地面实验验证系统
CN107872180A (zh) 一种检测电机转子位置的方法、装置、电子设备及无人飞行器
Antonello et al. Exploring the potential of MEMS gyroscopes: Successfully using sensors in typical industrial motion control applications
JPWO2020158485A1 (ja) 複合センサおよび角速度補正方法
JPH06174487A (ja) 姿勢検出装置
CN111504314A (zh) Imu与刚体的位姿融合方法、装置、设备及存储介质
US20190302804A1 (en) Inertial navigation system using all-accelerometer
CN114291295A (zh) 一种单磁悬浮控制敏感陀螺的卫星双轴姿态测控一体方法
JP4946562B2 (ja) 姿勢検出装置および方法
JPH102754A (ja) ストラップダウン慣性航法装置で使用するためのコーニング補償の方法、ならびに、そのための装置およびデジタルプロセッサ
JP2004359002A (ja) 無人ヘリコプターの自立制御方法及び装置
Haotian et al. Accurate attitude estimation of HB2 standard model based on QNCF in hypersonic wind tunnel test
JPH11105797A (ja) 着陸装置
JP2006177909A (ja) 慣性センサのバイアス測定方法及びその装置
JP6454857B2 (ja) 姿勢検出装置及び姿勢検出方法
JPH05133762A (ja) 慣性航法装置
KR100617602B1 (ko) 비접촉식 3축 시뮬레이터의 자세 측정 시스템
JPH07294240A (ja) 位置センサ
JP2009116751A (ja) 多軸制御システムの設計ツール
JPH0465677A (ja) 角速度検出装置
CN108286989B (zh) 一种修正无人机二维水平速度的方法及一体化模块
Chynoradský et al. Research and development of a new system of the autonomous control of robot trajectory
JPH11142428A (ja) 加速度計用サーボプラットホームおよび飛行制御装置