JP5776505B2 - 姿勢角度演算装置、姿勢角度演算方法、及びプログラム - Google Patents
姿勢角度演算装置、姿勢角度演算方法、及びプログラム Download PDFInfo
- Publication number
- JP5776505B2 JP5776505B2 JP2011250783A JP2011250783A JP5776505B2 JP 5776505 B2 JP5776505 B2 JP 5776505B2 JP 2011250783 A JP2011250783 A JP 2011250783A JP 2011250783 A JP2011250783 A JP 2011250783A JP 5776505 B2 JP5776505 B2 JP 5776505B2
- Authority
- JP
- Japan
- Prior art keywords
- posture angle
- angle calculation
- acceleration
- angular velocity
- sensors
- 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.)
- Expired - Fee Related
Links
- 238000004364 calculation method Methods 0.000 title claims description 123
- 230000001133 acceleration Effects 0.000 claims description 138
- 230000015572 biosynthetic process Effects 0.000 claims description 65
- 238000003786 synthesis reaction Methods 0.000 claims description 65
- 238000006243 chemical reaction Methods 0.000 claims description 51
- 238000005259 measurement Methods 0.000 claims description 43
- 230000002194 synthesizing effect Effects 0.000 claims description 11
- 239000002131 composite material Substances 0.000 claims description 9
- 238000012545 processing Methods 0.000 description 78
- 238000000034 method Methods 0.000 description 17
- 238000012821 model calculation Methods 0.000 description 16
- 230000008569 process Effects 0.000 description 15
- 230000006870 function Effects 0.000 description 12
- 230000010354 integration Effects 0.000 description 11
- 238000010586 diagram Methods 0.000 description 5
- 239000011159 matrix material Substances 0.000 description 5
- 239000000284 extract Substances 0.000 description 4
- 239000013307 optical fiber Substances 0.000 description 4
- 239000000203 mixture Substances 0.000 description 3
- 238000013139 quantization Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000012937 correction Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 230000002411 adverse Effects 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Images
Landscapes
- Manipulator (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Description
この一態様において、前記計測域の異なる複数のセンサから夫々出力される信号を合成する合成手段を更に備え、前記姿勢角演算手段は、前記合成手段により合成された信号に基づいて、前記姿勢角度を算出してもよい。
この一態様において、前記計測域の異なる複数のセンサから夫々出力される信号を夫々利用する複数の姿勢角演算手段を備えていてもよい。
この一態様において、前記複数のセンサから出力される信号を、アナログ信号からデジタル信号に夫々変換する複数の変換手段を更に備えていてもよい。
この一態様において、前記合成手段は、前記計測域の異なる複数のセンサから夫々出力される信号を前記計測域の切替え閾値付近で連続するように合成してもよい。
この一態様において、複数の前記姿勢角演算手段により夫々算出された複数の姿勢角度に、確率モデル又は合成比率関数モデルに基づいた所定係数を夫々乗算する複数の演算手段と、前記演算手段により夫々算出された複数の演算値を加算する加算手段と、
を更に備えていてもよい。
この一態様において、前記異なる計測域は低速域、中速域又は高速域であってもよい。
この一態様において、前記複数のジャイロセンサは、低速域の角速度を検出する第1ジャイロセンサと、高速域の角速度を検出する第2ジャイロセンサと、を含み、前記複数の加速度センサは、低速域の加速度を検出する第1加速度センサと、高速域の加速度を検出する第2加速度センサと、を含んでいても良い。
この一態様において、前記複数の変換手段は、前記第1ジャイロセンサにより検出された低速域の角速度をアナログ信号からデジタル信号に変換する第1変換手段と、前記第2ジャイロセンサにより検出された高速域の角速度をアナログ信号からデジタル信号に変換する第2変換手段と、前記第1加速度センサにより検出された低速域の加速度をアナログ信号からデジタル信号に変換する第3変換手段と、前記第2加速度センサにより検出された高速域の加速度をアナログ信号からデジタル信号に変換する第4変換手段と、を含んでいても良い。
この一態様において、前記複数の合成手段は、前記第1変換手段により変換された角速度信号と、前記第2変換手段により変換された角速度信号を前記計測域の切替え閾値付近で連続するように合成する第1合成手段と、前記第3変換手段により変換された加速度信号と、前記第4変換手段により変換された加速度信号を前記計測域の切替え閾値付近で連続するように合成する第2合成手段と、を含んでいても良い。
この一態様において、前記複数の姿勢角演算手段は、前記第1合成手段により合成された角速度信号と、前記第2合成手段により合成された加速度信号と、に基づいて姿勢角度を算出する第1姿勢角演算手段と、前記第1合成手段により合成された角速度信号と、前記第2合成手段により合成された加速度信号と、に基づいて姿勢角度を算出する第2姿勢角演算手段と、を含み、前記複数の演算手段は、前記第1姿勢角演算手段により算出された姿勢角度に、確率モデル又は合成比率関数モデルに基づいた第1所定係数を乗算する第1演算手段と、前記第2姿勢角演算手段により算出された姿勢角度に、確率モデル又は合成比率関数モデルに基づいた第2所定係数を乗算する第2演算手段と、を含み、前記加算手段は、前記第1演算手段により算出された演算結果と前記第2演算手段により算出された演算結果と、を加算して最終的な姿勢角度を算出してもよい。
この一態様において、前記姿勢角度演算装置により算出された姿勢角度に基づいて、歩行の制御を行う制御装置と、を備える、ことを特徴とする二足歩行ロボットであってもよい。
他方、上記目的を達成するための本発明の一態様は、角速度を検出するジャイロセンサ及び加速度を検出する加速度センサを備え、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて姿勢角度を算出する姿勢角度演算方法であって、前記ジャイロセンサ及び加速度センサのうち少なくとも一方は、同一箇所及び同一方向を計測する、計測域の異なる複数のセンサで構成され、前記計測域の異なる複数のセンサから夫々出力される信号に基づいて、姿勢角度を算出するステップを含む、ことを特徴とする姿勢角度演算方法であってもよい。
この一態様において、前記計測域の異なる複数のセンサから夫々出力される信号を合成するステップを更に含み、前記合成された信号に基づいて、前記姿勢角度を算出してもよい。
この一態様において、前記複数のセンサから出力される信号を、アナログ信号からデジタル信号に夫々変換するステップを更に含んでいても良い。
この一態様において、前記計測域の異なる複数のセンサから夫々出力される信号を前記計測域の切替え閾値付近で連続するように夫々合成してもよい。
この一態様において、前記算出された複数の姿勢角度に、確率モデル又は合成比率関数モデルに基づいた所定係数を夫々乗算するステップと、前記算出された複数の演算値を加算するステップと、を更に含んでいても良い。
また、上記目的を達成するための本発明の一態様は、角速度を検出するジャイロセンサ及び加速度を検出する加速度センサを備え、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて姿勢角度を算出するプログラムであって、前記ジャイロセンサ及び加速度センサのうち少なくとも一方は、同一箇所及び同一方向を計測する、計測域の異なる複数のセンサで構成され、前記計測域の異なる複数のセンサから夫々出力される信号に基づいて、姿勢角度を算出する処理と、をコンピュータに実行させる、ことを特徴とするプログラムであってもよい。
(a)ωin_low≦ωpの場合
ωout=ωin_low
(b)ωp≦ωin_low≦ωsの場合 (1)式
ωout=(ωin_low-ωp)/(ωs-ωp)×ωin_low+(1-(ωin_low-ωp)/(ωs-ωp))×ωin_high
(c)ωs≦ωin_lowの場合
ωout=ωin_high
ωout=ωin_low×r1+ωin_high×r2 (2)式
また、例えば、ローパスフィルタ(LPF)とハイパスフィルタ(HPF)を用いた下記(2−1)式を用いて、角速度の合成処理を行っても良い。
ωout=ωin_low×LPF+ωin_high×HPF (2−1)式
ここで、LPF=ωc/(s+ωc)、HPF=s/(s+ωc)が一例として考えられるが、2次或いはそれ以上の次数のフィルタでも良く、全周波数帯域に渡って足して1になるLPF及びHPFを用いればよい。
xk=Axk−1+wk−1
zk=Hxk+vk
下記式により状態の予測(姿勢角度の推定)を行う。
xk=Axk−1+Bdk−1
xk=xk+K(zk−Hxk)
下記式により共分散方程式とカルマンゲインの算出を行う。 (3)式
Pk=APk−1AT+Q
Pk=(I−KH)Pk
K=PkHT(HPkHT+R)−1
なお、以上の説明において時間を表わす添え字として、kを用いている。これ以降の説明においては、第1及び第2姿勢角演算部12、13のいずれかを表わす添え字としてiを用いている。また、第1及び第2確率モデル演算部14、15のいずれであるかを表わす添え字としてもiを用いている。
2 第1ジャイロセンサ
3 第2ジャイロセンサ
4 第1AD変換部
5 第2AD変換部
6 第1加速度センサ
7 第2加速度センサ
8 第3AD変換部
9 第4AD変換部
10 第1合成処理部
11 第2合成処理部
12 第1姿勢角演算部
13 第2姿勢角演算部
14 第1確率モデル演算部
15 第2確率モデル演算部
16 加算処理部
Claims (17)
- 角速度を検出するジャイロセンサ及び加速度を検出する加速度センサを備え、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて姿勢角度を算出する姿勢角度演算装置であって、
前記ジャイロセンサ及び加速度センサのうち少なくとも一方は、同一箇所及び同一方向を計測する、計測域の異なる複数のセンサで構成され、
前記計測域の異なる複数のセンサから夫々出力される信号に基づいて、姿勢角度を算出する姿勢角演算手段を備え、
前記異なる計測域は低速域まで、中速域まで又は高速域まである、ことを特徴とする姿勢角度演算装置。 - 請求項1記載の姿勢角度演算装置であって、
前記計測域の異なる複数のセンサから夫々出力される信号を合成する合成手段を更に備え、
前記姿勢角演算手段は、前記合成手段により合成された信号に基づいて、前記姿勢角度を算出する、ことを特徴とする姿勢角度演算装置。 - 請求項2記載の姿勢角度演算装置であって、
前記計測域の異なる複数のセンサから夫々出力される信号を夫々利用する複数の姿勢角演算手段を備える、ことを特徴とする姿勢角度演算装置。 - 請求項3記載の姿勢角度演算装置であって、
前記複数のセンサから出力される信号を、アナログ信号からデジタル信号に夫々変換する複数の変換手段を更に備える、ことを特徴とする姿勢角度演算装置。 - 請求項4記載の姿勢角度演算装置であって、
前記合成手段は、前記計測域の異なる複数のセンサから夫々出力される信号を前記計測域の切替え閾値付近で連続するように合成する、ことを特徴とする姿勢角度演算装置。 - 請求項5記載の姿勢角度演算装置であって、
複数の前記姿勢角演算手段により夫々算出された複数の姿勢角度に、確率モデル又は合成比率関数モデルに基づいた所定係数を夫々乗算する複数の演算手段と、
前記演算手段により夫々算出された複数の演算値を加算する加算手段と、
を更に備える、ことを特徴とする姿勢角度演算装置。 - 請求項6記載の姿勢角度演算装置であって、
前記複数のジャイロセンサは、低速域の角速度を検出する第1ジャイロセンサと、高速域の角速度を検出する第2ジャイロセンサと、を含み、
前記複数の加速度センサは、低速域の加速度を検出する第1加速度センサと、高速域の加速度を検出する第2加速度センサと、を含む、ことを特徴とする姿勢角度演算装置。 - 請求項7記載の姿勢角度演算装置であって、
前記複数の変換手段は、
前記第1ジャイロセンサにより検出された低速域の角速度をアナログ信号からデジタル信号に変換する第1変換手段と、
前記第2ジャイロセンサにより検出された高速域の角速度をアナログ信号からデジタル信号に変換する第2変換手段と、
前記第1加速度センサにより検出された低速域の加速度をアナログ信号からデジタル信号に変換する第3変換手段と、
前記第2加速度センサにより検出された高速域の加速度をアナログ信号からデジタル信号に変換する第4変換手段と、
を含む、ことを特徴とする姿勢角度演算装置。 - 請求項8記載の姿勢角度演算装置であって、
前記複数の合成手段は、
前記第1変換手段により変換された角速度信号と、前記第2変換手段により変換された角速度信号を前記計測域の切替え閾値付近で連続するように合成する第1合成手段と、
前記第3変換手段により変換された加速度信号と、前記第4変換手段により変換された加速度信号を前記計測域の切替え閾値付近で連続するように合成する第2合成手段と、
を含む、ことを特徴とする姿勢角度演算装置。 - 請求項9記載の姿勢角度演算装置であって、
前記複数の姿勢角演算手段は、
前記第1合成手段により合成された角速度信号と、前記第2合成手段により合成された加速度信号と、に基づいて姿勢角度を算出する第1姿勢角演算手段と、
前記第1合成手段により合成された角速度信号と、前記第2合成手段により合成された加速度信号と、に基づいて姿勢角度を算出する第2姿勢角演算手段と、を含み、
前記複数の演算手段は、
前記第1姿勢角演算手段により算出された姿勢角度に、確率モデル又は合成比率関数モデルに基づいた第1所定係数を乗算する第1演算手段と、
前記第2姿勢角演算手段により算出された姿勢角度に、確率モデル又は合成比率関数モデルに基づいた第2所定係数を乗算する第2演算手段と、を含み、
前記加算手段は、
前記第1演算手段により算出された演算結果と前記第2演算手段により算出された演算結果と、を加算して最終的な姿勢角度を算出する、ことを特徴とする姿勢角度演算装置。 - 請求項1乃至10のうちいずれか1項記載の姿勢角度演算装置と、
前記姿勢角度演算装置により算出された姿勢角度に基づいて、歩行の制御を行う制御装置と、
を備える、ことを特徴とする二足歩行ロボット。 - 角速度を検出するジャイロセンサ及び加速度を検出する加速度センサを備え、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて姿勢角度を算出する姿勢角度演算方法であって、
前記ジャイロセンサ及び加速度センサのうち少なくとも一方は、同一箇所及び同一方向を計測する、計測域の異なる複数のセンサで構成され、
前記計測域の異なる複数のセンサから夫々出力される信号に基づいて、姿勢角度を算出するステップを含み、
前記異なる計測域は低速域まで、中速域まで又は高速域までである、ことを特徴とする姿勢角度演算方法。 - 請求項12記載の姿勢角度演算方法であって、
前記計測域の異なる複数のセンサから夫々出力される信号を合成するステップを更に含み、
前記合成された信号に基づいて、前記姿勢角度を算出する、ことを特徴とする姿勢角度演算方法。 - 請求項13記載の姿勢角度演算方法であって、
前記複数のセンサから出力される信号を、アナログ信号からデジタル信号に夫々変換するステップを更に含む、ことを特徴とする姿勢角度演算方法。 - 請求項14記載の姿勢角度演算方法であって、
前記計測域の異なる複数のセンサから夫々出力される信号を前記計測域の切替え閾値付近で連続するように夫々合成する、ことを特徴とする姿勢角度演算方法。 - 請求項15記載の姿勢角度演算方法であって、
前記算出された複数の姿勢角度に、確率モデル又は合成比率関数モデルに基づいた所定係数を夫々乗算するステップと、
前記算出された複数の演算値を加算するステップと、
を更に含む、ことを特徴とする姿勢角度演算方法。 - 角速度を検出するジャイロセンサ及び加速度を検出する加速度センサを備え、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて姿勢角度を算出するプログラムであって、
前記ジャイロセンサ及び加速度センサのうち少なくとも一方は、同一箇所及び同一方向を計測する、計測域の異なる複数のセンサで構成され、
前記計測域の異なる複数のセンサから夫々出力される信号に基づいて、姿勢角度を算出する処理と、をコンピュータに実行させ、
前記異なる計測域は低速域まで、中速域まで又は高速域までである、ことを特徴とするプログラム。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011250783A JP5776505B2 (ja) | 2011-11-16 | 2011-11-16 | 姿勢角度演算装置、姿勢角度演算方法、及びプログラム |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011250783A JP5776505B2 (ja) | 2011-11-16 | 2011-11-16 | 姿勢角度演算装置、姿勢角度演算方法、及びプログラム |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2013103319A JP2013103319A (ja) | 2013-05-30 |
JP5776505B2 true JP5776505B2 (ja) | 2015-09-09 |
Family
ID=48623279
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2011250783A Expired - Fee Related JP5776505B2 (ja) | 2011-11-16 | 2011-11-16 | 姿勢角度演算装置、姿勢角度演算方法、及びプログラム |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5776505B2 (ja) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6616679B2 (ja) * | 2015-12-16 | 2019-12-04 | 日立建機株式会社 | 建設機械 |
JP7547865B2 (ja) | 2020-05-28 | 2024-09-10 | セイコーエプソン株式会社 | 姿勢推定装置、センサーモジュール、計測システム、移動体、及び姿勢推定方法 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH0238974A (ja) * | 1988-07-29 | 1990-02-08 | Toyota Motor Corp | 車両用加速度検出装置 |
JPH08136569A (ja) * | 1994-11-09 | 1996-05-31 | Toyota Motor Corp | 検出装置 |
DE19810213A1 (de) * | 1998-03-10 | 1999-09-23 | Bosch Gmbh Robert | Verfahren und Vorrichtung zur Erzeugung eines Fahrzustandssignals bei einem Kraftfahrzeug |
JP4410051B2 (ja) * | 2004-07-22 | 2010-02-03 | 本田技研工業株式会社 | 角速度計測装置および脚式移動ロボット |
-
2011
- 2011-11-16 JP JP2011250783A patent/JP5776505B2/ja not_active Expired - Fee Related
Also Published As
Publication number | Publication date |
---|---|
JP2013103319A (ja) | 2013-05-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN105717947B (zh) | 控制空中装置的方法和有实现此方法的控制器的空中装置 | |
US7822550B2 (en) | Environmental characteristic determination | |
US20120016623A1 (en) | Use of Multiple Internal Sensors for Measurement Validation | |
KR100898169B1 (ko) | 관성항법시스템의 초기정렬 방법 | |
WO2013084987A1 (ja) | 衛星航法システムを用いた位置出力装置 | |
CN103983278B (zh) | 一种测量影响卫星姿态确定系统精度的方法 | |
US10838408B2 (en) | Vibration control device, vibration control method, vibration control system, program, and recording medium | |
US20190308316A1 (en) | Robot system for performing learning control by using motor encoder and sensor | |
US7248948B2 (en) | Apparatus and method for estimating attitude using inertial measurement equipment | |
JP5164645B2 (ja) | カルマンフィルタ処理における繰り返し演算制御方法及び装置 | |
JP2015094630A (ja) | センサー用ic、センサーデバイス、電子機器及び移動体 | |
JP5776505B2 (ja) | 姿勢角度演算装置、姿勢角度演算方法、及びプログラム | |
KR101564020B1 (ko) | 이동체의 전자세 예측 방법 및 이를 이용한 전자세 예측 장치 | |
KR20060081509A (ko) | 입력 장치의 자세 측정 방법 및 그 장치 | |
JP7401404B2 (ja) | 計測装置 | |
JP7406390B2 (ja) | 較正装置およびその制御方法 | |
KR100585499B1 (ko) | 2축 지자기 센서 및 2축 가속도 센서를 이용한 방위각측정장치 및 측정방법 | |
US10345329B2 (en) | Inertial force sensor | |
JP2015094723A (ja) | 温度特性補正装置および温度特性補正方法並びに電子機器および制御回路 | |
TW201416815A (zh) | 用於六軸運動姿態感測之方法及其感測系統 | |
KR101505866B1 (ko) | 자장 센서 바이어스 보정 장치 및 이를 이용한 바이어스 보정 방법 | |
CN102135431B (zh) | 一种惯性测量单元精度补偿的方法 | |
RU2406645C1 (ru) | Устройство для автоматического вывода судна на заданную прямолинейную траекторию прохождения узкого участка фарватера и удержания его на этой траектории | |
RU106971U1 (ru) | Система автоматического управления беспилотным летательным аппаратом | |
KR101665375B1 (ko) | 이동체 항법 시스템 및 방법 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20140117 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20141006 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20141014 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20141209 |
|
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: 20150609 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20150622 |
|
R151 | Written notification of patent or utility model registration |
Ref document number: 5776505 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R151 |
|
LAPS | Cancellation because of no payment of annual fees |