JP5419268B2 - 三次元姿勢推定装置、三次元姿勢推定方法及び三次元姿勢推定プログラム - Google Patents
三次元姿勢推定装置、三次元姿勢推定方法及び三次元姿勢推定プログラム Download PDFInfo
- Publication number
- JP5419268B2 JP5419268B2 JP2009210985A JP2009210985A JP5419268B2 JP 5419268 B2 JP5419268 B2 JP 5419268B2 JP 2009210985 A JP2009210985 A JP 2009210985A JP 2009210985 A JP2009210985 A JP 2009210985A JP 5419268 B2 JP5419268 B2 JP 5419268B2
- Authority
- JP
- Japan
- Prior art keywords
- acceleration
- angle
- dimensional posture
- posture angle
- determined
- 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.)
- Active
Links
Images
Landscapes
- Gyroscopes (AREA)
Description
本発明はかかる従来の課題に鑑みて成されたもので、軽量な演算処理によって、三次元姿勢角度を推定することを目的とする。
この発明によれば、軽量な演算処理によって、三次元姿勢角度を推定することができ、加減速運動状態であるか否かの判定を高い精度で行うことができる。
この発明によれば、軽量な演算処理によって、三次元姿勢角度を推定することができ、加減速運動状態であるか否かの判定を高い精度で行うことができる。
この発明によれば、軽量な演算処理によって、三次元姿勢角度を推定することができ、加減速運動状態であるか否かの判定を高い精度で行うことができる。
本発明の第1実施形態である三次元姿勢角度推定装置1は、例えば、上下・左右・前後に加減速可能なロボット(「躯体」に相当)に装着され、三次元姿勢角度推定装置1が推定する三次元姿勢角度に基づいて、そのロボットの姿勢を制御することができる。
θ1=[θ1x θ1y θ1z]T:3次元姿勢角度ベクトル推定値
θ2=[θ2x θ2y θ2z]T:3次元姿勢角度ベクトル暫定値
am=[amx amy amz]T:3軸加速度センサーにより取得される加速度ベクトル
nm=[nmx nmy nmz]T:3軸方位センサーにより取得される方位ベクトル
ωm=[ωmx ωmy ωmz]T:3軸ジャイロセンサーにより取得される角速度ベクトル
Δθ=[Δθx Δθy Δθz]T:3次元姿勢角度ベクトルにおける推定値と暫定値の差分
Δωc=[Δωcx Δωcy Δωcz]T:3軸角速度ベクトルに対する補正値ベクトル
aglobal d=[aglobal dx aglobal dy Δaglobal dz]T:3軸加速度ベクトルにおける重量加速度以外の成分
gglobal=[0 0 −1]T:慣性座標系での重力加速度ベクトル
ここで、
である。
を推定できる。
したがって、式(7)により求められる外乱加速度の推定値の絶対値e2が閾値以上であるか閾値未満であるかによって加減速運動状態であるか否かを判定できる。
そして、それを次式(9)により、
θ1を慣性座標系の角速度に座標変換する(S122)。なお、f(θ1)は次式(10)に示す通りである。
以上、本発明の実施形態について説明したが、本発明は、かかる実施形態に限定されるものではなく、その要旨を逸脱しない範囲で以下に示すような変形が可能である。
そして、この値が重力加速度の1/10以上であれば加速度状態にあると判定でき、重力加速度の1/10未満であれば加速度状態にないと判定できる。
2 情報処理装置
10 CPU
11 メモリー
12 記憶手段
13 通信インターフェイス
21 方位センサー
22 速度センサー
23 ジャイロセンサー
31 暫定角度算出部
32 判定部
33 補正値算出部
34 姿勢角度推定部
Claims (6)
- 加減速運動可能な躯体の三次元姿勢角度を推定する三次元姿勢推定装置であって、
前記躯体の加速度を検出する加速度センサーと、
前記躯体が向いている方位を検出する方位センサーと、
前記躯体の角速度を検出するジャイロセンサーと、
前記加速度センサーが検出した加速度と前記方位センサーが検出した方位とに基づいて、前記躯体の三次元姿勢角度の暫定値である暫定角度を求める暫定角度算出部と、
前記躯体の前記三次元姿勢角度を推定する姿勢角度推定部と、
前記加速度センサーが検出した加速度に基づいて、前記躯体が加減速運動状態にあるかないかを判定する判定部と、
前記判定部が加減速運動状態にないと判定した場合には、前記姿勢角度推定部が直近に推定した三次元姿勢角度と前記暫定角度算出部が直近に求めた暫定角度とに基づいて補正値を求め、前記判定部が加減速運動状態にあると判定した場合には、前記判定部が加減速運動状態にないと判定した時点であって直近の時点において求めた前記補正値を保持する補正値算出部と、
を備え、
前記姿勢角度推定部は、前記ジャイロセンサーが検出した角速度と前記補正値算出部が求めた補正値とに基づいて、前記三次元姿勢角度を推定し、
前記判定部は、該角速度と該補正値に基づいて前記姿勢角度推定部が推定した前記三次元姿勢角度と、前記加速度センサーが検出する加速度と、に基づいて、重力加速度成分を除去した加速度を求め、
前記重力加速度成分を除去した加速度が所定の大きさ未満であれば加減速運動状態でないと判定し、
前記重力加速度成分を除去した加速度が所定の大きさ以上であれば加減速運動状態であると判定することを特徴とする三次元姿勢推定装置。 - 請求項1に記載の三次元姿勢推定装置であって、
前記補正値算出部は、前記判定部が加減速運動状態にないと判定した場合には、前記姿勢角度推定部が推定する三次元姿勢角度が、前記暫定角度算出部が直近に求めた暫定角度と等しくなるような補正値を求めることを特徴とする三次元姿勢推定装置。 - 請求項1又は2に記載の三次元姿勢推定装置であって、
前記補正値算出部は、前記姿勢角度推定部が推定する三次元姿勢角度と、前記暫定角度算出部が求めた暫定角度との差分の累積値に基づいて、補正値を求めることを特徴とする三次元姿勢推定装置。 - 請求項1〜3の何れかに記載の三次元姿勢推定装置であって、
前記姿勢角度推定部は、前記ジャイロセンサーが検出した角速度から前記補正値算出部が求めた補正値を差し引いて三次元姿勢角度を推定することを特徴とする三次元姿勢推定装置。 - 加減速運動可能な躯体の三次元姿勢角度を推定する三次元姿勢推定方法であって、
前記躯体の加速度を加速度センサーで検出することと、
前記躯体が向いている方位を方位センサーで検出することと、
前記躯体の角速度をジャイロセンサーで検出することと、
前記加速度センサーが検出した加速度と前記方位センサーが検出した方位とに基づいて、前記躯体の三次元姿勢角度の暫定値である暫定角度を求めることと、
前記躯体の前記三次元姿勢角度を推定することと、
前記加速度センサーが検出した加速度に基づいて、前記躯体が加減速運動状態にあるかないかを判定することと、
加減速運動状態にないと判定した場合には、直近に推定した前記三次元姿勢角度と直近に求めた前記暫定角度とに基づいて補正値を求め、加減速運動状態にあると判定した場合には、加減速運動状態にないと判定した時点であって直近の時点において求めた前記補正値を保持することと、を有し、
前記三次元姿勢角度を推定する際に、加減速運動状態にないと判定した場合には、前記ジャイロセンサーが検出した角速度と求めた前記補正値とに基づいて、前記三次元姿勢角度を推定し、加減速運動状態にあると判定した場合には、前記ジャイロセンサーが検出した角速度と保持した前記補正値とに基づいて、前記三次元姿勢角度を推定し、
前記躯体が加減速運動状態にあるかないかを判定する際には、該角速度と該補正値に基づいて推定された前記三次元姿勢角度と、前記加速度センサーが検出する加速度と、に基づいて、重力加速度成分を除去した加速度を求め、
前記重力加速度成分を除去した加速度が所定の大きさ未満であれば加減速運動状態でないと判定し、
前記重力加速度成分を除去した加速度が所定の大きさ以上であれば加減速運動状態であると判定することを特徴とする三次元姿勢推定方法。 - 加減速運動可能な躯体の三次元姿勢角度を推定する三次元姿勢推定プログラムであって、
情報処理装置に、
加速度センサーで検出した前記躯体の加速度を取得することと、
方位センサーで検出した前記躯体が向いている方位を取得することと、
ジャイロセンサーで検出した前記躯体の角速度を取得することと、
前記加速度と前記方位とに基づいて、前記躯体の三次元姿勢角度の暫定値である暫定角度を求めることと、
前記躯体の前記三次元姿勢角度を推定することと、
前記加速度に基づいて、前記躯体が加減速運動状態にあるかないかを判定することと、
加減速運動状態にないと判定した場合には、直近に推定した前記三次元姿勢角度と直近に求めた前記暫定角度とに基づいて補正値を求め、加減速運動状態にあると判定した場合には、加減速運動状態にないと判定した時点であって直近の時点において求めた前記補正値を保持することと、
を実行させ、
前記躯体が加減速運動状態にあるかないかを判定する際には、該角速度と該補正値に基づいて推定された前記三次元姿勢角度と、前記加速度センサーが検出する加速度と、に基づいて、重力加速度成分を除去した加速度を求め、
前記重力加速度成分を除去した加速度が所定の大きさ未満であれば加減速運動状態でないと判定し、
前記重力加速度成分を除去した加速度が所定の大きさ以上であれば加減速運動状態であると判定することを特徴とする三次元姿勢推定プログラム。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2009210985A JP5419268B2 (ja) | 2009-09-11 | 2009-09-11 | 三次元姿勢推定装置、三次元姿勢推定方法及び三次元姿勢推定プログラム |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2009210985A JP5419268B2 (ja) | 2009-09-11 | 2009-09-11 | 三次元姿勢推定装置、三次元姿勢推定方法及び三次元姿勢推定プログラム |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2011059020A JP2011059020A (ja) | 2011-03-24 |
JP5419268B2 true JP5419268B2 (ja) | 2014-02-19 |
Family
ID=43946812
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2009210985A Active JP5419268B2 (ja) | 2009-09-11 | 2009-09-11 | 三次元姿勢推定装置、三次元姿勢推定方法及び三次元姿勢推定プログラム |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5419268B2 (ja) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR101910741B1 (ko) * | 2017-05-30 | 2018-10-22 | 경북대학교 산학협력단 | 드론의 고도 측정 장치 및 방법 |
Families Citing this family (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103228195B (zh) * | 2011-08-01 | 2016-01-20 | 奥林巴斯株式会社 | 插入部形状估计装置 |
JP6074897B2 (ja) * | 2012-03-16 | 2017-02-08 | セイコーエプソン株式会社 | 運動解析装置及び運動解析方法 |
KR101678292B1 (ko) * | 2015-02-03 | 2016-11-21 | 아주대학교산학협력단 | 웨어러블 디바이스를 이용한 사물 인터넷 기기의 제어 방법, 시스템 및 컴퓨터 판독 가능한 기록 매체 |
WO2016194398A1 (ja) | 2015-05-29 | 2016-12-08 | 住友電工ハードメタル株式会社 | 焼結体および切削工具 |
JP6476156B2 (ja) * | 2016-10-04 | 2019-02-27 | 日本電信電話株式会社 | 姿勢推定装置、方法およびプログラム |
JP7391616B2 (ja) * | 2019-11-05 | 2023-12-05 | 本田技研工業株式会社 | 移動体の姿勢推定装置 |
JPWO2021251117A1 (ja) * | 2020-06-08 | 2021-12-16 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH08178687A (ja) * | 1994-12-26 | 1996-07-12 | Pioneer Electron Corp | 姿勢検出方法及び装置 |
JPH08285621A (ja) * | 1995-04-14 | 1996-11-01 | Omron Corp | ナビゲーション装置 |
JP2904118B2 (ja) * | 1996-05-27 | 1999-06-14 | 日本電気株式会社 | 姿勢角検出装置及び方法 |
JP3763435B2 (ja) * | 1998-01-20 | 2006-04-05 | Necトーキン株式会社 | 姿勢角検出装置 |
JPH11211458A (ja) * | 1998-01-22 | 1999-08-06 | Tokin Corp | 運動角検出装置、運動角検出方法、および、姿勢角検出装置 |
JP2007163205A (ja) * | 2005-12-12 | 2007-06-28 | Matsushita Electric Ind Co Ltd | 移動体のセンサ補正装置、停止判定装置およびその方法 |
-
2009
- 2009-09-11 JP JP2009210985A patent/JP5419268B2/ja active Active
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR101910741B1 (ko) * | 2017-05-30 | 2018-10-22 | 경북대학교 산학협력단 | 드론의 고도 측정 장치 및 방법 |
Also Published As
Publication number | Publication date |
---|---|
JP2011059020A (ja) | 2011-03-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP5419268B2 (ja) | 三次元姿勢推定装置、三次元姿勢推定方法及び三次元姿勢推定プログラム | |
JP5328252B2 (ja) | ナビゲーションシステムの位置検出装置および位置検出方法 | |
Du et al. | Online serial manipulator calibration based on multisensory process via extended Kalman and particle filters | |
JP5061264B1 (ja) | 小型姿勢センサ | |
CN110887481B (zh) | 基于mems惯性传感器的载体动态姿态估计方法 | |
JP6922641B2 (ja) | 角速度導出装置および角速度導出方法 | |
CN110567461B (zh) | 一种考虑无陀螺仪的非合作航天器姿态和参数估计方法 | |
KR100898169B1 (ko) | 관성항법시스템의 초기정렬 방법 | |
CN108592917B (zh) | 一种基于失准角的卡尔曼滤波姿态估计方法 | |
CN108871323B (zh) | 一种低成本惯性传感器在机动环境下的高精度导航方法 | |
CN104819717B (zh) | 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法 | |
JP2013096724A (ja) | 状態推定装置 | |
CN106197376B (zh) | 基于单轴mems惯性传感器的车身倾角测量方法 | |
Guan et al. | Sensor fusion of gyroscope and accelerometer for low-cost attitude determination system | |
CN109506674B (zh) | 一种加速度的校正方法及装置 | |
CN110861081A (zh) | 一种欠约束索并联机器人末端执行器的自主定位方法 | |
CN113063416A (zh) | 一种基于自适应参数互补滤波的机器人姿态融合方法 | |
CN109916399B (zh) | 一种阴影下的载体姿态估计方法 | |
CN109674480B (zh) | 一种基于改进互补滤波的人体运动姿态解算方法 | |
WO2014171227A1 (ja) | 姿勢角推定装置およびそれを備える移動状態検出装置 | |
Ko et al. | Moving vehicle attitude tracking algorithm based on mems inertial navigation system | |
JP6859917B2 (ja) | 角速度導出装置および角速度導出方法 | |
JP6454857B2 (ja) | 姿勢検出装置及び姿勢検出方法 | |
Magnussen et al. | Calibration procedure for an inertial measurement unit using a 6-degree-of-freedom hexapod | |
JP6409625B2 (ja) | 車両位置算出装置 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20120911 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20130509 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20130521 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20130717 |
|
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: 20131118 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 5419268 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 |
|
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 |