JP2012047495A - 姿勢角安定化装置及び方法 - Google Patents
姿勢角安定化装置及び方法 Download PDFInfo
- Publication number
- JP2012047495A JP2012047495A JP2010187553A JP2010187553A JP2012047495A JP 2012047495 A JP2012047495 A JP 2012047495A JP 2010187553 A JP2010187553 A JP 2010187553A JP 2010187553 A JP2010187553 A JP 2010187553A JP 2012047495 A JP2012047495 A JP 2012047495A
- Authority
- JP
- Japan
- Prior art keywords
- posture
- posture angle
- matrix
- speed
- kalman filter
- 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.)
- Granted
Links
Images
Abstract
【解決手段】姿勢角安定化装置100は、移動体の角速度とフィードバックした補正値とから姿勢角の変化量を算出し、算出した姿勢角の変化量を積分した姿勢角と、カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し出力する。そして、動作時と判定した場合に生成した姿勢行列のラッチを行い、静止時と判定した場合に静止時であると判定する前の姿勢行列のラッチを保持し、ラッチしている姿勢行列と、生成した姿勢行列とから姿勢角誤差を算出し、算出した姿勢角誤差と生成した姿勢行列とに基づいて姿勢角を補正するための補正制御値を算出し、フィードバックする。
【選択図】図1
Description
101 姿勢角変化量算出部
102 積分処理部
103 カルマンフィルタ姿勢補正処理部
104 速度旋回判定部
105 姿勢角ラッチ部
106 転置処理部
107 姿勢角誤差演算処理部
108 補正制御部
Claims (2)
- GPS受信機及びIMUを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と前記計測速度とを融合して自律航法アルゴリズムによって移動体の速度を算出する装置に含まれる姿勢角安定化装置であって、
前記IMUを使用して計測された移動体の角速度と、フィードバックされた補正値とから姿勢角の変化量を算出する姿勢角変化量算出手段と、
前記姿勢角変化量算出手段によって算出された姿勢角の変化量を積分する積分処理手段と、
前記積分処理手段によって積分された姿勢角と、前記カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し、出力するカルマンフィルタ姿勢補正処理手段と、
前記GPS受信機によって計測された速度が一定の閾値未満であり、かつ、前記IMUを使用して計測された角速度が一定の閾値未満であるか否かを判定する速度旋回判定手段と、
前記速度旋回判定手段によって、速度又は角速度が一定の閾値以上であると判定された場合に、前記カルマンフィルタ姿勢補正処理手段によって生成された姿勢行列のラッチを行い、速度及び角速度が一定の閾値未満であると判定された場合に、前記生成された姿勢行列のラッチを行わずに前記閾値未満であると判定される前の姿勢行列のラッチを保持する姿勢角ラッチ手段と、
前記姿勢角ラッチ手段によってラッチされている姿勢行列の転置行列を算出する転置処理手段と、
前記転置処理手段によって算出された転置行列と、前記カルマンフィルタ姿勢補正処理手段によって生成された姿勢行列との乗算処理を行い、姿勢角誤差を算出する姿勢角誤差演算処理手段と、
前記姿勢角誤差演算処理手段によって算出された姿勢角誤差と、前記カルマンフィルタ姿勢補正処理手段によって生成された姿勢行列とに基づいて姿勢角を補正するための補正制御値を算出し、前記姿勢角変化量算出手段にフィードバックする前記補正値とする補正制御手段と、
を備える姿勢角安定化装置。 - GPS受信機及びIMUを使用して計測した計測速度からカルマンフィルタによって移動体の速度を推定し、推定した速度と前記計測速度とを融合して自律航法アルゴリズムによって移動体の速度を算出する装置に含まれる姿勢角安定化装置が実行する方法であって、
前記IMUを使用して計測された移動体の角速度と、フィードバックされた補正値とから姿勢角の変化量を算出する姿勢角変化量算出ステップと、
前記姿勢角変化量算出ステップによって算出された姿勢角の変化量を積分する積分処理ステップと、
前記積分処理ステップによって積分された姿勢角と、前記カルマンフィルタによって推定された姿勢角の推定値とから姿勢行列を生成し、出力するカルマンフィルタ姿勢補正処理ステップと、
前記GPS受信機によって計測された速度が一定の閾値未満であり、かつ、前記IMUを使用して計測された角速度が一定の閾値未満であるか否かを判定する速度旋回判定ステップと、
前記速度旋回判定ステップによって、速度又は角速度が一定の閾値以上であると判定された場合に、前記カルマンフィルタ姿勢補正処理ステップによって生成された姿勢行列のラッチを行い、速度及び角速度が一定の閾値未満であると判定された場合に、前記生成された姿勢行列のラッチを行わずに前記閾値未満であると判定される前の姿勢行列のラッチを保持する姿勢角ラッチステップと、
前記姿勢角ラッチステップによってラッチされている姿勢行列の転置行列を算出する転置処理ステップと、
前記転置処理ステップによって算出された転置行列と、前記カルマンフィルタ姿勢補正処理ステップによって生成された姿勢行列との乗算処理を行い、姿勢角誤差を算出する姿勢角誤差演算処理ステップと、
前記姿勢角誤差演算処理ステップによって算出された姿勢角誤差と、前記カルマンフィルタ姿勢補正処理ステップによって生成された姿勢行列とに基づいて姿勢角を補正するための補正制御値を算出し、前記姿勢角変化量算出ステップにフィードバックする前記補正値とする補正制御ステップと、
を備える方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2010187553A JP5405417B2 (ja) | 2010-08-24 | 2010-08-24 | 姿勢角安定化装置及び方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2010187553A JP5405417B2 (ja) | 2010-08-24 | 2010-08-24 | 姿勢角安定化装置及び方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2012047495A true JP2012047495A (ja) | 2012-03-08 |
JP5405417B2 JP5405417B2 (ja) | 2014-02-05 |
Family
ID=45902562
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2010187553A Expired - Fee Related JP5405417B2 (ja) | 2010-08-24 | 2010-08-24 | 姿勢角安定化装置及び方法 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5405417B2 (ja) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2012193965A (ja) * | 2011-03-15 | 2012-10-11 | Mitsubishi Electric Corp | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム |
CN104570742A (zh) * | 2015-01-29 | 2015-04-29 | 哈尔滨工业大学 | 基于前馈pid控制的异面交叉快变轨道快速高精度相对指向控制方法 |
WO2017175628A1 (ja) * | 2016-04-06 | 2017-10-12 | 株式会社クボタ | 測位検出装置及び測位検出装置を備えた作業機 |
US20220074967A1 (en) * | 2020-09-04 | 2022-03-10 | Denso Corporation | Apparatus and method for calibrating inertial sensor |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH07239236A (ja) * | 1994-02-28 | 1995-09-12 | Hitachi Ltd | 移動体の状態量計測方法と装置および移動体の姿勢角演算装置 |
JP2007040765A (ja) * | 2005-08-01 | 2007-02-15 | Toyota Motor Corp | 角速度センサの零点補正装置 |
JP2008232869A (ja) * | 2007-03-22 | 2008-10-02 | Furuno Electric Co Ltd | Gps複合航法装置 |
WO2010001970A1 (ja) * | 2008-07-02 | 2010-01-07 | 独立行政法人産業技術総合研究所 | 移動体の姿勢角処理装置 |
-
2010
- 2010-08-24 JP JP2010187553A patent/JP5405417B2/ja not_active Expired - Fee Related
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH07239236A (ja) * | 1994-02-28 | 1995-09-12 | Hitachi Ltd | 移動体の状態量計測方法と装置および移動体の姿勢角演算装置 |
JP2007040765A (ja) * | 2005-08-01 | 2007-02-15 | Toyota Motor Corp | 角速度センサの零点補正装置 |
JP2008232869A (ja) * | 2007-03-22 | 2008-10-02 | Furuno Electric Co Ltd | Gps複合航法装置 |
WO2010001970A1 (ja) * | 2008-07-02 | 2010-01-07 | 独立行政法人産業技術総合研究所 | 移動体の姿勢角処理装置 |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2012193965A (ja) * | 2011-03-15 | 2012-10-11 | Mitsubishi Electric Corp | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム |
CN104570742A (zh) * | 2015-01-29 | 2015-04-29 | 哈尔滨工业大学 | 基于前馈pid控制的异面交叉快变轨道快速高精度相对指向控制方法 |
WO2017175628A1 (ja) * | 2016-04-06 | 2017-10-12 | 株式会社クボタ | 測位検出装置及び測位検出装置を備えた作業機 |
JP2017187394A (ja) * | 2016-04-06 | 2017-10-12 | 株式会社クボタ | 測位検出装置及び測位検出装置を備えた作業機 |
US10884429B2 (en) | 2016-04-06 | 2021-01-05 | Kubota Corporation | Positioning detection device and working machine having positioning detection device |
US11650599B2 (en) | 2016-04-06 | 2023-05-16 | Kubota Corporation | Positioning detection device and working machine having positioning detection device |
US20220074967A1 (en) * | 2020-09-04 | 2022-03-10 | Denso Corporation | Apparatus and method for calibrating inertial sensor |
Also Published As
Publication number | Publication date |
---|---|
JP5405417B2 (ja) | 2014-02-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP5398120B2 (ja) | Gps複合航法装置 | |
CA3003298C (en) | Gnss and inertial navigation system utilizing relative yaw as an observable for an ins filter | |
US8560234B2 (en) | System and method of navigation based on state estimation using a stepped filter | |
JP5046687B2 (ja) | ナビゲーション・システム内での反復拡張カルマン・フィルタを実施する方法及び装置 | |
US8019542B2 (en) | Heading stabilization for aided inertial navigation systems | |
JP5405417B2 (ja) | 姿勢角安定化装置及び方法 | |
EP2681513A1 (en) | Attitude determination method, position calculation method, and attitude determination device | |
JP4412381B2 (ja) | 方位検出装置 | |
JP5074950B2 (ja) | 航法装置 | |
CN107228674B (zh) | 一种针对星敏感器和陀螺联合滤波的改进方法 | |
JP2012173190A (ja) | 測位システム、測位方法 | |
EP2717014A1 (en) | Position calculation method and position calculation device | |
JP2013181985A (ja) | 転送アライメントの間、マスターナビゲーション・システム・リセットを組み込むシステム及び方法 | |
US20140104101A1 (en) | Position calculating method and position calculating device | |
US20140106775A1 (en) | Position calculating method and position calculating device | |
JP6221295B2 (ja) | 位置算出方法及び位置算出装置 | |
KR100870091B1 (ko) | 센서를 이용한 회전상태 판단 방법 및 장치 | |
JP2012194066A (ja) | 端末装置及び地磁気環境判定プログラム | |
JP2019082328A (ja) | 位置推定装置 | |
JP2012042285A (ja) | ヨー角初期化異常判定装置及び方法 | |
JP2015094631A (ja) | 位置算出装置及び位置算出方法 | |
JP2015102330A (ja) | 移動情報算出装置、移動情報算出方法、移動情報算出プログラム、および移動体 | |
JP2007155365A (ja) | 方位センサの補正係数演算装置及び演算プログラム | |
JP5569276B2 (ja) | 車両用現在位置検出装置 | |
JP5183050B2 (ja) | 車載用ナビゲーション装置及びナビゲーション方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20130204 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20130807 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20130813 |
|
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: 20131022 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20131030 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 5405417 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
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 |