JP6705972B2 - 姿勢推定装置、姿勢推定方法、制御プログラム、および記録媒体 - Google Patents
姿勢推定装置、姿勢推定方法、制御プログラム、および記録媒体 Download PDFInfo
- Publication number
- JP6705972B2 JP6705972B2 JP2016101925A JP2016101925A JP6705972B2 JP 6705972 B2 JP6705972 B2 JP 6705972B2 JP 2016101925 A JP2016101925 A JP 2016101925A JP 2016101925 A JP2016101925 A JP 2016101925A JP 6705972 B2 JP6705972 B2 JP 6705972B2
- Authority
- JP
- Japan
- Prior art keywords
- vector
- posture
- horizontal direction
- sensor
- estimation unit
- 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
- 238000000034 method Methods 0.000 title claims description 23
- 239000013598 vector Substances 0.000 claims description 211
- 230000005484 gravity Effects 0.000 claims description 88
- 230000001133 acceleration Effects 0.000 claims description 35
- 238000007689 inspection Methods 0.000 claims description 17
- 230000006870 function Effects 0.000 claims description 6
- 239000011159 matrix material Substances 0.000 description 51
- 238000004364 calculation method Methods 0.000 description 17
- 238000012545 processing Methods 0.000 description 13
- 230000008569 process Effects 0.000 description 12
- 230000014509 gene expression Effects 0.000 description 8
- 238000005259 measurement Methods 0.000 description 6
- 238000004891 communication Methods 0.000 description 4
- 238000007796 conventional method Methods 0.000 description 3
- 238000001514 detection method Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000005389 magnetism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 210000003371 toe Anatomy 0.000 description 1
- 230000001960 triggered effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
- G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/14—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by recording the course traversed by the object
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1654—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with electromagnetic compass
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- General Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Life Sciences & Earth Sciences (AREA)
- Environmental & Geological Engineering (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Geology (AREA)
- Navigation (AREA)
Description
以下、本発明の一実施の形態について図1から図3に基づいて説明する。図1は、本実施形態の姿勢推定装置1の要部構成の一例を示すブロック図である。姿勢推定装置1は、姿勢の推定対象となる移動体に保持され、該移動体の姿勢をトラッキングする装置である。図示のように、姿勢推定装置1は、制御部10、記憶部11、出力部12、加速度センサ13、タイミングデバイス14、角速度センサ15、および磁気センサ16を備えている。
次に、重力方向推定部100が実行する処理(姿勢推定方法)について図2に基づいて説明する。図2は、重力方向推定部100が実行する処理の一例を示すフローチャートである。
次に、水平方向推定部101が実行する処理(姿勢推定方法)について図3に基づいて説明する。図3は、水平方向推定部101が実行する処理の一例を示すフローチャートである。なお、本処理では、上述した図2の処理の計算結果を利用するので、本処理は図2の処理の終了後に行われる。
(A)磁気ベクトルの大きさの検査
(B)磁気ベクトルの伏角の検査
上記(A)の検査では、水平方向推定部101は、S304で得られた磁気ベクトルの大きさが、この検査が実施されている周辺地域(移動体が存在する地域)の地磁気ベクトルの大きさに対して、所定の誤差範囲に収まっているか否かを判定する。
以上説明したように、重力方向推定部100は、重力方向ベクトルを含む状態データを逐次更新し、水平方向推定部101は、水平方向ベクトルを含む状態データを随時更新する。よって、姿勢推定部102は、任意のタイミングにおいて、重力方向推定部100から重力方向ベクトルを取得すると共に、水平方向推定部101から水平方向ベクトルを取得して、上述の数式(1)により移動体の姿勢を示す姿勢情報を生成する(姿勢推定ステップ)。そして、姿勢の推定を逐次行うことにより、移動体の姿勢のトラッキングを行うことができる。
姿勢推定装置1で更新する重力方向ベクトルと水平方向ベクトルは何れも3×1行列であるから、これらの更新の際に発生する乗算は何れも3×3行列で済む。これに対し、非特許文献1のような従来技術では、9つの成分を持つ9×1行列で表される状態ベクトルを更新し、この更新の際には9×9行列の乗算が発生する。
上記実施形態では、加速度センサ13、角速度センサ15、および磁気センサ16を備える姿勢推定装置1について説明したが、本発明の姿勢推定装置1は、これらのセンサの計測結果を取得できればよく、これらのセンサを必ずしも備えている必要はない。例えば、本発明の姿勢推定装置1は、移動体に装着された上述の各センサから無線通信にて計測結果を取得してもよい。また、リアルタイムで姿勢を推定する必要がない場合には、上述の各センサの計測結果を記録媒体に記録しておき、この記録媒体から計測結果を取得してもよい。同様に、本発明の姿勢推定装置1は、タイミングデバイス14を必ずしも備えている必要はなく、姿勢推定装置1の外部のタイミングデバイス14からタイミングデータを取得してもよい。
上記実施形態では、姿勢情報を出力する姿勢推定装置1について説明したが、生成した姿勢情報を用いて姿勢情報以外のパラメータを算出・出力する装置も本発明の範疇に含まれる。例えば、上記実施形態の手法にて生成した姿勢情報と、移動体の速度(加速度センサ13の計測結果から算出可能)とを用いて、その移動体の現在位置を算出する移動体の測位装置も本発明の範疇に含まれる。また、上記姿勢情報から移動体(例えば歩行者)がどのような動作を行っているかを検出する動作検出装置や、上記姿勢情報から移動体の移動速度を算出する移動速度算出装置も本発明の範疇に含まれる。
上記各実施形態では、従来技術(例えば非特許文献1)におけるカルマンフィルタの状態ベクトルの9つの成分のうち、角速度成分は状態ベクトルとして使用していないが、角速度成分も状態ベクトルに含めてもよい。
姿勢推定装置1の制御ブロック(特に制御部10)は、集積回路(ICチップ)等に形成された論理回路(ハードウェア)によって実現してもよいし、CPU(Central Processing Unit)を用いてソフトウェアによって実現してもよい。
13 加速度センサ
15 角速度センサ
16 磁気センサ
100 重力方向推定部
101 水平方向推定部
102 姿勢推定部
Claims (7)
- 加速度センサ、角速度センサ、および磁気センサの出力を用いてセンサ座標系における移動体の状態ベクトルを逐次更新し、該状態ベクトルを用いて上記移動体の姿勢をトラッキングする姿勢推定装置であって、
上記角速度センサの出力を用いて上記状態ベクトルの成分である重力方向ベクトルの予測値を算出し、該予測値を上記加速度センサの出力を観測として用いて更新する重力方向推定部と、
上記角速度センサの出力を用いて上記状態ベクトルの成分である水平方向ベクトルの予測値を算出し、該予測値を上記磁気センサの出力を観測として用いて更新する水平方向推定部と、
更新後の上記重力方向ベクトルと更新後の上記水平方向ベクトルとを用いて上記移動体の姿勢情報を生成する姿勢推定部と、を備えていることを特徴とする姿勢推定装置。 - 上記水平方向推定部は、上記磁気センサの出力が上記水平方向ベクトルの更新に用いるのに適切な値となっているか否かを検査し、適切な値となっていない場合には該磁気センサの出力を観測として用いた更新を行わないことを特徴とする請求項1に記載の姿勢推定装置。
- 上記水平方向推定部は、上記重力方向推定部が更新した上記重力方向ベクトルを用いて上記検査を行うことを特徴とする請求項2に記載の姿勢推定装置。
- 上記水平方向推定部は、更新後の上記水平方向ベクトルを、更新後の上記重力方向ベクトルと直交するように補正することを特徴とする請求項1から3の何れか1項に記載の姿勢推定装置。
- 加速度センサ、角速度センサ、および磁気センサの出力を用いてセンサ座標系における移動体の状態ベクトルを逐次更新し、該状態ベクトルを用いて上記移動体の姿勢をトラッキングする姿勢推定装置による姿勢推定方法であって、
上記角速度センサの出力を用いて上記状態ベクトルの成分である重力方向ベクトルの予測値を算出し、該予測値を上記加速度センサの出力を観測として用いて更新する重力方向推定ステップと、
上記角速度センサの出力を用いて上記状態ベクトルの成分である水平方向ベクトルの予測値を算出し、該予測値を上記磁気センサの出力を観測として用いて更新する水平方向推定ステップと、
更新後の上記重力方向ベクトルと更新後の上記水平方向ベクトルとを用いて上記移動体の姿勢情報を生成する姿勢推定ステップと、を含むことを特徴とする姿勢推定方法。 - 請求項1から4の何れか1項に記載の姿勢推定装置としてコンピュータを機能させるための制御プログラムであって、上記重力方向推定部、上記水平方向推定部、および上記姿勢推定部としてコンピュータを機能させるための制御プログラム。
- 請求項6に記載の制御プログラムを記録したコンピュータ読み取り可能な記録媒体。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2016101925A JP6705972B2 (ja) | 2016-05-20 | 2016-05-20 | 姿勢推定装置、姿勢推定方法、制御プログラム、および記録媒体 |
US15/169,875 US9939273B2 (en) | 2016-05-20 | 2016-06-01 | Attitude estimating device, attitude estimating method, and storage medium |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2016101925A JP6705972B2 (ja) | 2016-05-20 | 2016-05-20 | 姿勢推定装置、姿勢推定方法、制御プログラム、および記録媒体 |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2017207456A JP2017207456A (ja) | 2017-11-24 |
JP6705972B2 true JP6705972B2 (ja) | 2020-06-03 |
Family
ID=60330015
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2016101925A Active JP6705972B2 (ja) | 2016-05-20 | 2016-05-20 | 姿勢推定装置、姿勢推定方法、制御プログラム、および記録媒体 |
Country Status (2)
Country | Link |
---|---|
US (1) | US9939273B2 (ja) |
JP (1) | JP6705972B2 (ja) |
Families Citing this family (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10197396B2 (en) * | 2016-01-27 | 2019-02-05 | Apple Inc. | Always on compass calibration system and methods |
FR3082611B1 (fr) * | 2018-06-13 | 2020-10-16 | Sysnav | Procede de calibration de magnetometres equipant un objet |
JP6581276B1 (ja) * | 2018-10-18 | 2019-09-25 | 株式会社ショーワ | 状態量推定装置、制御装置、および状態量推定方法 |
CN109631939B (zh) * | 2018-11-08 | 2020-07-28 | 湖北三江航天红峰控制有限公司 | 一种基于磁强计和加速度计的快速对准方法 |
CN109740418B (zh) * | 2018-11-21 | 2022-10-14 | 中山大学 | 一种基于多加速度传感器的瑜伽动作识别方法 |
CN109724597B (zh) * | 2018-12-19 | 2021-04-02 | 上海交通大学 | 一种基于函数迭代积分的惯性导航解算方法及系统 |
CN109596124A (zh) * | 2019-01-31 | 2019-04-09 | 北京永安信通科技股份有限公司 | 基于步行数据的单人定位方法、装置和电子设备 |
CN113728394A (zh) * | 2019-05-21 | 2021-11-30 | 史密夫和内修有限公司 | 身体活动执行和训练的评分度量 |
CN110579620A (zh) * | 2019-10-25 | 2019-12-17 | 深圳航天东方红海特卫星有限公司 | 一种通过磁强计对角速度进行测量的方法及系统 |
CN111649745B (zh) * | 2020-05-18 | 2022-04-05 | 北京三快在线科技有限公司 | 电子设备的姿态估计方法、装置、电子设备和存储介质 |
CN113091769A (zh) * | 2021-03-30 | 2021-07-09 | Oppo广东移动通信有限公司 | 姿态校准方法、装置、存储介质及电子设备 |
CN113267182B (zh) * | 2021-04-28 | 2024-04-30 | 广东国天时空科技有限公司 | 基于地磁测量值的修正方法及地磁检测装置 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8751151B2 (en) * | 2012-06-12 | 2014-06-10 | Trx Systems, Inc. | System and method for localizing a trackee at a location and mapping the location using inertial sensor information |
WO2010001970A1 (ja) | 2008-07-02 | 2010-01-07 | 独立行政法人産業技術総合研究所 | 移動体の姿勢角処理装置 |
US8515707B2 (en) * | 2009-01-07 | 2013-08-20 | Sensor Platforms, Inc. | System and method for determining an attitude of a device undergoing dynamic acceleration using a Kalman filter |
WO2011068184A1 (ja) | 2009-12-03 | 2011-06-09 | 独立行政法人産業技術総合研究所 | 移動体の測位装置 |
US9524268B2 (en) * | 2011-10-31 | 2016-12-20 | University of Floria Research Foundation, Inc. | Vestibular dynamic inclinometer |
US9417091B2 (en) * | 2013-05-13 | 2016-08-16 | The Johns Hopkins University | System and method for determining and correcting field sensors errors |
-
2016
- 2016-05-20 JP JP2016101925A patent/JP6705972B2/ja active Active
- 2016-06-01 US US15/169,875 patent/US9939273B2/en not_active Expired - Fee Related
Also Published As
Publication number | Publication date |
---|---|
US9939273B2 (en) | 2018-04-10 |
JP2017207456A (ja) | 2017-11-24 |
US20170336208A1 (en) | 2017-11-23 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP6705972B2 (ja) | 姿勢推定装置、姿勢推定方法、制御プログラム、および記録媒体 | |
US8758275B2 (en) | Moving body posture angle processing device | |
JP6322960B2 (ja) | 慣性装置、方法及びプログラム | |
CN108731664B (zh) | 机器人状态估计方法、装置、计算机设备和存储介质 | |
US10627237B2 (en) | Offset correction apparatus for gyro sensor, recording medium storing offset correction program, and pedestrian dead-reckoning apparatus | |
US20140222369A1 (en) | Simplified method for estimating the orientation of an object, and attitude sensor implementing such a method | |
Renaudin et al. | Quaternion based heading estimation with handheld MEMS in indoor environments | |
Benzerrouk et al. | Robust IMU/UWB integration for indoor pedestrian navigation | |
Li et al. | Research on multi-sensor pedestrian dead reckoning method with UKF algorithm | |
US20190212146A1 (en) | Positioning system and positioning method | |
US11709056B2 (en) | Method and device for magnetic field measurement by magnetometers | |
Hostettler et al. | IMU and magnetometer modeling for smartphone-based PDR | |
Hasan et al. | Smart phone based sensor fusion by using Madgwick filter for 3D indoor navigation | |
JP6548305B2 (ja) | 進行方向推定装置及び進行方向推定方法 | |
Pan et al. | Real-time accurate odometer velocity estimation aided by accelerometers | |
Schindhelm et al. | Usability of apple iPhones for inertial navigation systems | |
JP2013061309A (ja) | カルマンフィルタ、状態推定装置、カルマンフィルタの制御方法、及びカルマンフィルタの制御プログラム | |
JP2013122384A (ja) | カルマンフィルタ、及び、状態推定装置 | |
JP2006038650A (ja) | 姿勢計測方法、姿勢制御装置、方位計及びコンピュータプログラム | |
Wang et al. | Human motion tracking based on complementary Kalman filter | |
JP2020529016A (ja) | 磁気センサにより測定される磁場からの方位の決定 | |
Zhu et al. | A hybrid step model and new azimuth estimation method for pedestrian dead reckoning | |
RU2629539C1 (ru) | Способ измерения магнитного курса подвижного объекта | |
CN107289935B (zh) | 一种适用于可穿戴设备的室内导航算法 | |
EP3765869A1 (en) | Motion capture system and method |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20160808 |
|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20190515 |
|
A711 | Notification of change in applicant |
Free format text: JAPANESE INTERMEDIATE CODE: A711 Effective date: 20190816 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A821 Effective date: 20190816 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20200312 |
|
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: 20200317 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20200414 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 6705972 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 |