JP7307736B2 - 慣性計測装置を使用した相対角度推定 - Google Patents
慣性計測装置を使用した相対角度推定 Download PDFInfo
- Publication number
- JP7307736B2 JP7307736B2 JP2020542111A JP2020542111A JP7307736B2 JP 7307736 B2 JP7307736 B2 JP 7307736B2 JP 2020542111 A JP2020542111 A JP 2020542111A JP 2020542111 A JP2020542111 A JP 2020542111A JP 7307736 B2 JP7307736 B2 JP 7307736B2
- Authority
- JP
- Japan
- Prior art keywords
- rigid body
- relative angle
- previous
- gravitational acceleration
- trigonometric function
- 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
Classifications
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F9/00—Component parts of dredgers or soil-shifting machines, not restricted to one of the kinds covered by groups E02F3/00 - E02F7/00
- E02F9/26—Indicating devices
- E02F9/264—Sensors and their calibration for indicating the position of the work tool
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F9/00—Component parts of dredgers or soil-shifting machines, not restricted to one of the kinds covered by groups E02F3/00 - E02F7/00
- E02F9/26—Indicating devices
- E02F9/261—Surveying the work-site to be treated
- E02F9/262—Surveying the work-site to be treated with follow-up actions to control the work tool, e.g. controller
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
- G01B5/00—Measuring arrangements characterised by the use of mechanical techniques
- G01B5/24—Measuring arrangements characterised by the use of mechanical techniques for measuring angles or tapers; for testing the alignment of axes
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C9/00—Measuring inclination, e.g. by clinometers, by levels
- G01C9/02—Details
- G01C9/08—Means for compensating acceleration forces due to movement of instrument
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
- B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
- B25J13/088—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices with position, velocity or acceleration sensors
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/25—Fusion techniques
- G06F18/251—Fusion techniques of input or preprocessed data
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- General Engineering & Computer Science (AREA)
- Mathematical Physics (AREA)
- Mining & Mineral Resources (AREA)
- Civil Engineering (AREA)
- Structural Engineering (AREA)
- Mathematical Optimization (AREA)
- Data Mining & Analysis (AREA)
- Theoretical Computer Science (AREA)
- Mathematical Analysis (AREA)
- Pure & Applied Mathematics (AREA)
- Computational Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- Computing Systems (AREA)
- Gyroscopes (AREA)
- Navigation (AREA)
- Component Parts Of Construction Machinery (AREA)
- Length Measuring Devices With Unspecified Measuring Means (AREA)
Description
本出願は、2018年2月2日に出願された「慣性計測装置を使用した相対角度推定」という名称の米国非仮特許出願第15/887717号の利益と優先権を主張し、その全内容はあらゆる目的でその全体が参照により本明細書に組み込まれる。
を、プロセスモデル:
観測モデル:
ならびに、カルマン・フィルタ・パラメータに基づくプロセスノイズ共分散行列、観測ノイズ共分散行列、および初期推定誤差共分散行列としてのQ、R、およびPのそれぞれと共に使用して計算される。
および観測ベクトル:
を定義する。
カルマンフィルタの時間更新は、
であり、カルマンフィルタの測定の更新は、
であり、式中、上付き文字「+」および上付き文字「-」は、それぞれ事後推定値および事前推定値を表す。K(t)はカルマンゲインであり、上付き文字「T」は行列転置演算子である。各エポックタイムにおいて、X+は、
gn-1は、前の剛体の角速度ベクトルである。
AccGn-1は、前の剛体の重力加速度ベクトルである。
gnは、現在の剛体の角速度ベクトルである。
AccGnは、現在の剛体の重力加速度ベクトルである。
gnは、現在の剛体の角速度ベクトルである。
AccGnは、現在の剛体の重力加速度ベクトルである。
Claims (16)
- 掘削機のバケットとスティックとの間の相対角度の三角関数を使用して前記掘削機のプラットフォームに対する前記バケットの位置を決定するための方法であって、前記方法が、
前記スティックに取り付けられた第1の慣性計測装置(IMU)を使用して前記スティックの角速度および重力加速度ベクトルを測定することと、
前記バケットに取り付けられたIMUを使用して前記バケットの角速度および重力加速度ベクトルを測定することと、
前記スティックと前記バケットとの間の前記相対角度の正弦および前記スティックと前記バケットとの間の前記相対角度の余弦を推定することであって、前記正弦および前記余弦が、前記スティックの前記重力加速度ベクトルおよび前記角速度と、前記バケットの前記重力加速度ベクトルおよび前記角速度と、前記スティックから前記バケットへの回転行列と、少なくとも1つの共分散行列とに基づいて推定されることと、
少なくとも前記正弦および前記余弦を使用して前記掘削機の前記プラットフォームに対する前記バケットの前記位置を決定することと
を含み、
前記少なくとも1つの共分散行列が、初期推定誤差共分散行列、プロセスノイズ共分散行列、または観測ノイズ共分散行列のうちの少なくとも1つを含む、方法。 - 前記スティックと前記バケットとの間の前記相対角度の前記正弦および前記余弦が、前記スティックの絶対角度または前記バケットの絶対角度を使用せずに推定される、請求項1に記載の方法。
- 前記スティックと前記バケットとの間の前記相対角度の前記正弦および前記余弦を推定することが、
前記スティックと前記バケットとの間の前記相対角度の観測された正弦を決定することと、
前記スティックと前記バケットとの間の前記相対角度の観測された余弦を決定することと、
前記スティックから前記バケットへの前記回転行列を決定することと、
前記少なくとも1つの共分散行列を決定することと
を含む、請求項1に記載の方法。 - 前記相対角度の前記観測された正弦および前記相対角度の前記観測された余弦が、前記スティックの前記重力加速度ベクトルおよび前記バケットの前記重力加速度ベクトルを使用して決定される、請求項3に記載の方法。
- 慣性計測装置(IMU)が各々配置されている結合された剛体間の相対角度の三角関数を推定するための方法であって、前記方法が、
前の剛体に結合された第1のIMUを使用して前記前の剛体の角速度および重力加速度ベクトルを測定することと、
剛体に結合された第2のIMUを使用して前記剛体の角速度および重力加速度ベクトルを測定することであって、前記剛体が前記前の剛体に旋回可能に結合されていることと、
前記前の剛体と前記剛体との間の第1の相対角度の第1の観測された三角関数および第2の観測された三角関数を決定することと、
前記前の剛体の前記重力加速度ベクトルおよび前記角速度と、前記剛体の前記重力加速度ベクトルおよび前記角速度と、前記第1の観測された三角関数および前記第2の観測された三角関数とに基づいて、前記前の剛体と前記剛体との間の前記第1の相対角度の第1の三角関数および第2の三角関数を推定することと、
次の剛体に結合された第3のIMUを使用して前記次の剛体の角速度および重力加速度ベクトルを測定することであって、前記次の剛体が前記剛体に旋回可能に結合されていることと、
前記剛体の前記重力加速度ベクトルおよび前記角速度と、前記次の剛体の前記重力加速度ベクトルおよび前記角速度とに基づいて、前記剛体と前記次の剛体との間の第2の相対角度の第1の三角関数および第2の三角関数を推定することと
を含む、方法。 - 前記前の剛体と前記剛体との間の前記第1の相対角度の前記第1の三角関数および前記第2の三角関数が、前記前の剛体の絶対角度または前記剛体の絶対角度を使用せずに推定される、請求項5に記載の方法。
- 前記第1の相対角度の前記第1の三角関数が、前記前の剛体と前記剛体との間の前記第1の相対角度の正弦であり、前記第1の相対角度の前記第2の三角関数が、前記前の剛体と前記剛体との間の前記第1の相対角度の余弦である、請求項5に記載の方法。
- 前記正弦および前記余弦を使用して前記前の剛体と前記剛体との間の前記第1の相対角度を決定することをさらに含む、請求項7に記載の方法。
- 前記剛体と前記次の剛体との間の前記第2の相対角度の前記第1の三角関数および前記第2の三角関数が、前記剛体の絶対角度または前記次の剛体の絶対角度を使用せずに推定される、請求項5に記載の方法。
- 前記剛体と前記次の剛体との間の前記第2の相対角度の前記第1の三角関数および前記第2の三角関数が、前記前の剛体の前記重力加速度ベクトルを使用せずに決定される、請求項5に記載の方法。
- 前記前の剛体と前記剛体との間の前記第1の相対角度の前記第1の三角関数および前記第2の三角関数を推定することが、
前記前の剛体から前記剛体への回転行列を決定することと、
少なくとも1つの共分散行列を決定することと
を含み、
前記少なくとも1つの共分散行列を決定することが、初期推定誤差共分散行列、プロセスノイズ共分散行列、または観測ノイズ共分散行列のうちの少なくとも1つを決定することを含む、請求項5に記載の方法。 - 前記前の剛体と前記剛体との間の前記第1の相対角度の前記第1の観測された三角関数および前記第2の観測された三角関数が、前記前の剛体の前記重力加速度ベクトルおよび前記剛体の前記重力加速度ベクトルを使用して決定される、請求項11に記載の方法。
- 前の剛体および剛体を含む、一連の分離しているが結合されている剛体間の相対角度の三角関数を決定するための方法であって、前記方法が、
前記前の剛体に結合された第1のIMUを使用して前記前の剛体の角速度および重力加速度ベクトルを測定することと、
前記剛体に結合された第2のIMUを使用して前記剛体の角速度および重力加速度ベクトルを測定することと、
前記前の剛体と前記剛体との間の相対角度の第1の観測された三角関数および第2の観測された三角関数を推定することと、
前記前の剛体から前記剛体への回転行列および少なくとも1つの共分散行列を決定することと、
前記第1の観測された三角関数および前記第2の観測された三角関数と、前記前の剛体の前記角速度と、前記剛体の前記角速度と、前記前の剛体から前記剛体への前記回転行列と、前記少なくとも1つの共分散行列とに基づいて、前記前の剛体と前記剛体との間の前記相対角度の第1の三角関数および第2の三角関数を推定することと
を含み、
前記少なくとも1つの共分散行列を決定することが、初期推定誤差共分散行列、プロセスノイズ共分散行列、または観測ノイズ共分散行列のうちの少なくとも1つを決定することを含む、方法。 - 前記第1の三角関数および前記第2の三角関数が、前記前の剛体の絶対角度または前記剛体の絶対角度を使用せずに計算される、請求項13に記載の方法。
- 前記剛体と次の剛体との間の相対角度を、前記前の剛体と前記剛体との間の前記第1の三角関数および前記第2の三角関数を使用せずに決定することをさらに含む、請求項13に記載の方法。
- 連続して配置された旋回可能に結合された剛体間の相対角度を決定するための方法であって、前記方法が、
前の剛体に結合された第1の慣性計測装置(IMU)を使用して前記前の剛体の角速度および重力加速度ベクトルを測定することと、
剛体に結合された第2のIMUを使用して前記剛体の角速度および重力加速度ベクトルを測定することであって、前記剛体が前記前の剛体に旋回可能に結合されていることと、
前記前の剛体の前記重力加速度ベクトルおよび前記角速度と、前記剛体の前記重力加速度ベクトルおよび前記角速度と、前記前の剛体と前記剛体との間の前記相対角度の第1の観測された三角関数および第2の観測された三角関数と、前記前の剛体から前記剛体への回転行列と、少なくとも1つの共分散行列とに基づいて、前記前の剛体と前記剛体との間の相対角度の第1の三角関数および第2の三角関数を推定することと、
前記第1の三角関数および前記第2の三角関数を使用して前記前の剛体と前記剛体との間の前記相対角度を推定することであって、前記前の剛体と前記剛体との間の前記相対角度が、前記前の剛体の絶対角度または前記剛体の絶対角度を使用せずに決定されることと
を含み、
前記少なくとも1つの共分散行列が、初期推定誤差共分散行列、プロセスノイズ共分散行列、または観測ノイズ共分散行列のうちの少なくとも1つを含む、方法。
Applications Claiming Priority (3)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
US15/887,717 | 2018-02-02 | ||
US15/887,717 US10724842B2 (en) | 2018-02-02 | 2018-02-02 | Relative angle estimation using inertial measurement units |
PCT/US2019/015817 WO2019152494A1 (en) | 2018-02-02 | 2019-01-30 | Relative angle estimation using inertial measurement units |
Publications (3)
Publication Number | Publication Date |
---|---|
JP2021512325A JP2021512325A (ja) | 2021-05-13 |
JP2021512325A5 JP2021512325A5 (ja) | 2021-08-19 |
JP7307736B2 true JP7307736B2 (ja) | 2023-07-12 |
Family
ID=65409637
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2020542111A Active JP7307736B2 (ja) | 2018-02-02 | 2019-01-30 | 慣性計測装置を使用した相対角度推定 |
Country Status (4)
Country | Link |
---|---|
US (1) | US10724842B2 (ja) |
EP (1) | EP3746604A1 (ja) |
JP (1) | JP7307736B2 (ja) |
WO (1) | WO2019152494A1 (ja) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE102019107969B3 (de) * | 2019-03-28 | 2020-08-06 | Franka Emika Gmbh | Lagewinkelanzeige beim manuellen Führen eines Robotermanipulators |
EP3913326B1 (en) | 2020-05-20 | 2023-12-27 | Roche Diagnostics GmbH | Sample container spring check apparatus |
US11624169B2 (en) | 2020-06-18 | 2023-04-11 | Deere & Company | Excavator with improved movement sensing |
US11873621B2 (en) | 2020-11-30 | 2024-01-16 | Deere & Company | System and method for tracking motion of linkages for self-propelled work vehicles in independent coordinate frames |
US20230129397A1 (en) * | 2021-10-25 | 2023-04-27 | Deere & Company | Work vehicle implement joint orientation system and method |
EP4286609A1 (de) | 2022-06-01 | 2023-12-06 | Contelec AG | System aus mehreren imus |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20160160472A1 (en) | 2014-12-08 | 2016-06-09 | Caterpillar Global Mining Llc | System for Determining a Position of a Component |
WO2016164975A1 (en) | 2015-04-13 | 2016-10-20 | Leica Geosystems Ag | Dynamic motion compensation |
Family Cites Families (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102227616B (zh) * | 2009-05-27 | 2015-04-15 | 松下电器产业株式会社 | 行为识别装置 |
US8738242B2 (en) | 2011-03-16 | 2014-05-27 | Topcon Positioning Systems, Inc. | Automatic blade slope control system |
US9347205B2 (en) | 2012-02-10 | 2016-05-24 | Topcon Positioning Systems, Inc. | Estimation of the relative attitude and position between a vehicle body and an implement operably coupled to the vehicle body |
EP2696167A1 (en) | 2012-08-08 | 2014-02-12 | Hexagon Technology Center GmbH | Inclination sensor |
US9618338B2 (en) | 2014-03-18 | 2017-04-11 | Caterpillar Inc. | Compensating for acceleration induced inclination errors |
WO2015186845A1 (ja) | 2015-06-29 | 2015-12-10 | 株式会社小松製作所 | 作業機械の制御システム及び作業機械の制御方法 |
WO2018092944A1 (ko) * | 2016-11-18 | 2018-05-24 | 재단법인대구경북과학기술원 | 경직 평가 장치, 방법 및 시스템 |
-
2018
- 2018-02-02 US US15/887,717 patent/US10724842B2/en active Active
-
2019
- 2019-01-30 WO PCT/US2019/015817 patent/WO2019152494A1/en unknown
- 2019-01-30 JP JP2020542111A patent/JP7307736B2/ja active Active
- 2019-01-30 EP EP19705065.1A patent/EP3746604A1/en not_active Ceased
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20160160472A1 (en) | 2014-12-08 | 2016-06-09 | Caterpillar Global Mining Llc | System for Determining a Position of a Component |
WO2016164975A1 (en) | 2015-04-13 | 2016-10-20 | Leica Geosystems Ag | Dynamic motion compensation |
Also Published As
Publication number | Publication date |
---|---|
US20190242687A1 (en) | 2019-08-08 |
EP3746604A1 (en) | 2020-12-09 |
US10724842B2 (en) | 2020-07-28 |
JP2021512325A (ja) | 2021-05-13 |
WO2019152494A1 (en) | 2019-08-08 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP7307736B2 (ja) | 慣性計測装置を使用した相対角度推定 | |
CN107532897B (zh) | 提供对运动学加速度的补偿的方法和补偿测量结果的系统 | |
AU2012209015B2 (en) | Inclination angle compensation systems and methods | |
US11530920B2 (en) | Controlling movement of a machine using sensor fusion | |
US9234758B2 (en) | Machine positioning system utilizing position error checking | |
US8571762B2 (en) | Real time method for determining the spatial pose of electronic mining shovels | |
JP6976412B2 (ja) | 単一の位置センサを用いる回転プラットフォームのヨーおよび回転中心の決定方法 | |
US20160376772A1 (en) | Construction machine control system and construction machine control method | |
AU2017366811B2 (en) | Machine limb length and angle offset determination using a laser distance meter | |
US20140122016A1 (en) | Machine Positioning System Having Angular Rate Correction | |
US9355462B2 (en) | Motion estimation system utilizing point cloud registration | |
US9145144B2 (en) | Inclination detection systems and methods | |
US10371522B2 (en) | Iterative estimation of centripetal accelerations of inertial measurement units in kinematic chains | |
US9052391B2 (en) | Backup velocity estimation utilizing traction device speed | |
RU2566153C1 (ru) | Устройство для определения положения рабочего органа машины | |
JP7065002B2 (ja) | 作業機械 | |
CN113944207A (zh) | 电子围墙设置方法、装置及作业机械 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20200930 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20210706 |
|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20220118 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20230116 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20230417 |
|
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: 20230602 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20230630 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 7307736 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |