JP5747752B2

JP5747752B2 - Posture estimation device, posture estimation method, posture estimation program

Info

Publication number: JP5747752B2
Application number: JP2011194246A
Authority: JP
Inventors: 晋及川
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2011-09-06
Filing date: 2011-09-06
Publication date: 2015-07-15
Anticipated expiration: 2031-09-06
Also published as: JP2013054009A

Description

本発明は、姿勢推定装置、姿勢推定方法、及び、姿勢推定プログラムに関する。 The present invention relates to a posture estimation device, a posture estimation method, and a posture estimation program.

この種の技術として、特許文献１は、チルト検出手段と角速度検出手段を有した傾動可能な物体の姿勢を推定する方法を開示している。 As this type of technology, Patent Document 1 discloses a method for estimating the attitude of a tiltable object having a tilt detection unit and an angular velocity detection unit.

特表２００３−５２１６９７号公報Special table 2003-521697 gazette

ところで、本願出願人は、移動ロボットの姿勢を推定する技術を開発しており、具体的には、移動ロボットに、姿勢角度を計測する傾斜センサ（例えば３軸加速度センサ）と、姿勢角度の変化である姿勢角速度を計測するジャイロセンサとを設けている。そして、傾斜センサの計測結果と、ジャイロセンサの計測結果とをフィルタ処理により合成することで、移動ロボットの姿勢を精度よく推定することとしている。即ち、移動ロボットの姿勢の推定は、以下の式により行われる。以下の式（１）〜（３）は、オイラー角表現による１つの軸についてのフィルタ処理を示している。 By the way, the applicant of the present application has developed a technique for estimating the posture of a mobile robot. Specifically, the mobile robot includes a tilt sensor (for example, a three-axis acceleration sensor) that measures a posture angle, and a change in posture angle. And a gyro sensor for measuring the posture angular velocity. Then, the posture of the mobile robot is accurately estimated by combining the measurement result of the tilt sensor and the measurement result of the gyro sensor by filter processing. That is, the posture of the mobile robot is estimated by the following equation. The following formulas (1) to (3) show the filter processing for one axis by Euler angle expression.

α^ドット _measuredは、ジャイロセンサによる姿勢角速度の計測結果である。
α_measuredは、傾斜センサによる姿勢角度の計測結果である。
α_predictは、ジャイロセンサによる姿勢角速度の計測結果を用いて予測した、移動ロボットの姿勢角度の予測値である。
α_nは、移動ロボットの姿勢角度の予測値に、傾斜センサによる姿勢角度の計測結果を加味した、移動ロボットの姿勢角度の推定値である。
b_nは、ジャイロセンサの姿勢角速度バイアスの推定値である。
b_n-1は、前回のジャイロセンサの姿勢角速度バイアスの推定値である。
dtは、サンプリング周期である。
K₀は、カルマンフィルタ係数である。
K₁は、カルマンフィルタ係数である。 The α ^dot _measured is a measurement result of the posture angular velocity by the gyro sensor.
α _measured is a measurement result of the posture angle by the tilt sensor.
α _predict is a predicted value of the posture angle of the mobile robot predicted using the measurement result of the posture angular velocity by the gyro sensor.
α _n is an estimated value of the posture angle of the mobile robot obtained by adding the measurement result of the posture angle by the tilt sensor to the predicted value of the posture angle of the mobile robot.
b _n is an estimated value of the attitude angular velocity bias of the gyro sensor.
b _n−1 is an estimated value of the attitude angular velocity bias of the previous gyro sensor.
dt is a sampling period.
K ₀ is a Kalman filter coefficient.
K ₁ is a Kalman filter coefficient.

上記式（２）は１つの軸についてのフィルタ処理を示しているので、３つの軸については、上記フィルタ処理を３回繰り返すことになり、計算コストが高い。これに対し、本願出願人は、四元数の概念を導入して、上記式（２）の計算コストを抑えることに成功している。詳しくは以下の通りである。 Since the above equation (2) shows the filtering process for one axis, the filtering process is repeated three times for three axes, and the calculation cost is high. On the other hand, the present applicant has succeeded in reducing the calculation cost of the above formula (2) by introducing the concept of quaternions. Details are as follows.

上記式（２）を変形すると、下記式（４）となる。下記式（４）においてα_nは、α_predictとα_measuredという２つの角度を(1-K₀):K₀の比率で補間する角度に相当している。αをオイラー角による１つの軸の角度としてではなく四元数であるとすると、下記式（４）の演算は球面線形補間（slerp：Spherical linear interpolation）と同一の形として捉えることができる。 When the above equation (2) is modified, the following equation (4) is obtained. In the following formula (4), α _n corresponds to an angle that interpolates two angles α _predict and α _{measured at} a ratio of (1-K ₀ ): K ₀ . If α is a quaternion rather than an angle of one axis by Euler angles, the calculation of the following equation (4) can be regarded as the same form as spherical linear interpolation (slerp).

ここで、q₀とq₁の２つの四元数の間を変数tで補間する関数slerp(q₀,q₁,t)は理論上、下記式（５）によって定義される。 Here, the function slerp (q ₀ , q ₁ , t) for interpolating between the two quaternions of q ₀ and q ₁ with the variable t is theoretically defined by the following equation (5).

上記式（５）は、q₀とq₁の間の弧の長さであるωを利用して下記式（６）で代替することができる。 The above equation (5) can be replaced by the following equation (6) using ω which is the length of the arc between q ₀ and q ₁ .

そして、２つの四元数の間の三角関数は下記式（７）で簡単に求められる。 The trigonometric function between the two quaternions can be easily obtained by the following equation (7).

従って、下記式（８）のようになる。 Therefore, the following equation (8) is obtained.

以上のように、αをオイラー角による１つの軸の姿勢角度ではなく四元数であるとして四元数特有の球面線形補間を利用することにより、上記式（２）を低い計算コストで実行することが可能となる。このような上記式（２）についての効率的な計算手法については、本願出願人が既に特許出願している。 As described above, the above equation (2) is executed at low calculation cost by using spherical linear interpolation peculiar to a quaternion, assuming that α is not a posture angle of one axis by Euler angles but a quaternion. It becomes possible. The applicant of the present application has already filed a patent for an efficient calculation method for the above equation (2).

一方で、上記式（３）の演算については改善の余地が残されていた。詳しくは、上記式（３）については、上記式（２）と違って、四元数特有の球面線形補間を利用するかたちには持っていくことができない。従って、α_measured等が既に四元数であった場合には、一度オイラー角表現に戻して演算を実行し、再度四元数に戻すといった計算が必要となる。そして、四元数とオイラー角の間の変換には、三角関数や逆関数といった時間がかかる関数を多用するため、結果として上記式（３）の演算には多くの時間を費やしていた。 On the other hand, there was room for improvement in the calculation of the above formula (3). Specifically, unlike the above equation (2), the above equation (3) cannot be brought into the form of using the spherical linear interpolation peculiar to the quaternion. Therefore, when α _measured or the like is already a quaternion, it is necessary to perform calculation by returning to the Euler angle representation once and executing the calculation again. And, since a function that takes time such as a trigonometric function or an inverse function is frequently used for conversion between the quaternion and the Euler angle, as a result, much time is spent on the calculation of the above formula (3).

本願発明の目的は、姿勢角速度バイアスを推定する計算の効率を向上する技術を提供することにある。 An object of the present invention is to provide a technique for improving the efficiency of calculation for estimating a posture angular velocity bias.

本願発明の第１の観点によれば、傾斜角センサとジャイロセンサを備えた機器本体の姿勢を前記傾斜角センサの計測結果と前記ジャイロセンサの計測結果に基づいて推定する姿勢推定装置であって、前記傾斜角センサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第１姿勢角度四元数α_measuredを生成する第１姿勢角度四元数生成手段と、前記ジャイロセンサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第２姿勢角度四元数α_predictを予測して生成する第２姿勢角度四元数生成手段と、フィルタ係数K₁を生成するフィルタ係数生成手段と、下記式（９）により姿勢角速度バイアス四元数b_nを推定する姿勢角速度バイアス四元数推定手段と、を備えた姿勢推定装置が提供される。ただし、b_n-1は、前回の姿勢角速度バイアス四元数である。以上の構成によれば、四元数表現をオイラー角表現に変換したり、オイラー角表現を四元数表現に変換する必要がなく、四元数表現のまま前記姿勢角速度バイアス四元数b_nを推定することができる。従って、前記角速度バイアス四元数b_nを高い計算効率で推定することができる。 According to a first aspect of the present invention, there is provided an attitude estimation device for estimating an attitude of a device body including an inclination angle sensor and a gyro sensor based on a measurement result of the inclination angle sensor and a measurement result of the gyro sensor. A first attitude angle quaternion generating means for generating a first attitude angle quaternion α _measured which is a quaternion representation of the attitude angle of the device main body based on a measurement result of the tilt angle sensor; and the gyro sensor a second attitude angle quaternion generating means for generating and predicting a second orientation angle quaternion alpha _predict on the basis of the measurement result is quaternion number representation of the attitude angle of the device body, the filter coefficient K ₁ There is provided an attitude estimation device including filter coefficient generation means for generating and attitude angular velocity bias quaternion estimation means for estimating an attitude angular velocity bias quaternion b _n by the following equation (9). However, b _n−1 is the previous attitude angular velocity bias quaternion. According to the above configuration, there is no need to convert the quaternion representation into the Euler angle representation or the Euler angle representation into the quaternion representation, and the posture angular velocity bias quaternion b _n remains in the quaternion representation. Can be estimated. Therefore, the angular velocity bias quaternion b _n can be estimated with high calculation efficiency.

好ましくは、前記姿勢角速度バイアス推定手段は、前記フィルタ係数K₁が負である場合は、前記フィルタ係数K₁の正負を反転させた上で、前記式（９）により姿勢角速度バイアス四元数b_nを推定する。以上の構成によれば、四元数の累乗を問題なく演算することができる。 Preferably, when the filter coefficient K ₁ is negative, the attitude angular velocity bias estimation unit inverts the positive / negative of the filter coefficient K ₁ and then calculates the attitude angular velocity bias quaternion b according to the equation (9). Estimate _n . According to the above configuration, the power of the quaternion can be calculated without any problem.

好ましくは、前記姿勢角速度バイアス推定手段は、前記フィルタ係数K₁の絶対値が１を超える場合は、前記フィルタ係数K₁の絶対値を１にした上で、前記式（９）により姿勢角速度バイアス四元数b_nを推定する。以上の構成によれば、四元数の累乗を問題なく演算することができる。 Preferably, the posture angular velocity estimated bias means, said if the absolute value of the filter coefficient K ₁ is greater than 1, the absolute value of the filter coefficient K ₁ in terms of the 1, posture angular velocity bias by the formula (9) Estimate the quaternion b _n . According to the above configuration, the power of the quaternion can be calculated without any problem.

本願発明の第２の観点によれば、傾斜角センサとジャイロセンサを備えた機器本体の姿勢を前記傾斜角センサの計測結果と前記ジャイロセンサの計測結果に基づいて推定する姿勢推定方法であって、前記傾斜角センサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第１姿勢角度四元数α_measuredを生成する第１姿勢角度四元数生成ステップと、前記ジャイロセンサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第２姿勢角度四元数α_predictを予測して生成する第２姿勢角度四元数生成ステップと、フィルタ係数K₁を生成するフィルタ係数生成ステップと、上記式（９）により姿勢角速度バイアス四元数b_nを推定する姿勢角速度バイアス四元数推定ステップと、を備えた姿勢推定方法が提供される。ただし、b_n-1は、前回の姿勢角速度バイアス四元数である。 According to a second aspect of the present invention, there is provided an attitude estimation method for estimating an attitude of a device main body including an inclination angle sensor and a gyro sensor based on a measurement result of the inclination angle sensor and a measurement result of the gyro sensor. A first posture angle quaternion generating step for generating a first posture angle quaternion α _measured which is a quaternion representation of the posture angle of the device main body based on a measurement result of the tilt angle sensor, and the gyro sensor A second attitude angle quaternion generation step that predicts and generates a second attitude angle quaternion α _predict , which is a quaternion representation of the attitude angle of the device body, based on the measurement result, and a filter coefficient K ₁ There is provided a posture estimation method including a filter coefficient generation step to be generated and a posture angular velocity bias quaternion estimation step for estimating the posture angular velocity bias quaternion b _n by the above equation (9). However, b _n−1 is the previous attitude angular velocity bias quaternion.

また、コンピュータに、上記の姿勢推定方法を実行させるための姿勢推定プログラムが提供される。 Also provided is a posture estimation program for causing a computer to execute the posture estimation method described above.

本願発明によれば、四元数表現をオイラー角表現に変換したり、オイラー角表現を四元数表現に変換する必要がなく、四元数表現のまま前記姿勢角速度バイアス四元数b_nを推定することができる。従って、前記角速度バイアス四元数b_nを高い計算効率で推定することができる。 According to the present invention, there is no need to convert the quaternion representation into the Euler angle representation or the Euler angle representation into the quaternion representation, and the posture angular velocity bias quaternion b _n is not changed from the quaternion representation. Can be estimated. Therefore, the angular velocity bias quaternion b _n can be estimated with high calculation efficiency.

図１は、倒立二輪車本体のブロック図である。FIG. 1 is a block diagram of an inverted motorcycle body. 図２は、姿勢推定部のブロック図である。FIG. 2 is a block diagram of the posture estimation unit. 図３は、姿勢角速度バイアス四元数推定部のブロック図である。FIG. 3 is a block diagram of the attitude angular velocity bias quaternion estimation unit. 図４は、倒立二輪車の制御フローである。FIG. 4 is a control flow of the inverted motorcycle.

以下、図１〜４を参照して、本願発明の実施形態を説明する。 Hereinafter, an embodiment of the present invention will be described with reference to FIGS.

倒立二輪車１（移動ロボット、機器）は、倒立二輪車本体２（機器本体）と、図示しない台車、ハンドル、車輪等を備えて構成されている。 The inverted two-wheeled vehicle 1 (mobile robot, device) includes an inverted two-wheeled vehicle main body 2 (device main body), a cart, a handle, wheels, and the like (not shown).

倒立二輪車本体２は、傾斜角センサ３と、ジャイロセンサ４と、CPU５（Central Processing Unit）と、RAM６（Random Access Memory）と、ROM７（Read Only Memory）と、によって構成されている。ROM７には、姿勢推定プログラムと姿勢制御プログラムが記憶されている。姿勢推定プログラムと姿勢制御プログラムは、CPU５に読み込まれ、CPU５上で実行されることで、CPU５等のハードウェアに、姿勢推定部８（姿勢推定装置）及び姿勢制御部９としての機能を発揮させるようになっている。 The inverted two-wheeled vehicle body 2 includes an inclination angle sensor 3, a gyro sensor 4, a CPU 5 (Central Processing Unit), a RAM 6 (Random Access Memory), and a ROM 7 (Read Only Memory). The ROM 7 stores an attitude estimation program and an attitude control program. The posture estimation program and the posture control program are read into the CPU 5 and executed on the CPU 5, thereby causing the hardware such as the CPU 5 to function as the posture estimation unit 8 (posture estimation device) and the posture control unit 9. It is like that.

姿勢推定部８は、倒立二輪車本体２の姿勢を傾斜角センサ３の計測結果とジャイロセンサ４の計測結果に基づいて推定するものである。なお、「倒立二輪車本体２の姿勢」は、「倒立二輪車本体２の姿勢角度」と「倒立二輪車本体２の姿勢角速度」を含む概念である。 The posture estimation unit 8 estimates the posture of the inverted motorcycle body 2 based on the measurement result of the tilt angle sensor 3 and the measurement result of the gyro sensor 4. The “posture of the inverted motorcycle body 2” is a concept including “the attitude angle of the inverted motorcycle body 2” and “the attitude angular velocity of the inverted motorcycle body 2”.

姿勢制御部９は、姿勢推定部８によって推定された倒立二輪車本体２の姿勢に基づいて、倒立二輪車本体２の姿勢を制御するものである。 The attitude control unit 9 controls the attitude of the inverted motorcycle body 2 based on the attitude of the inverted motorcycle body 2 estimated by the attitude estimation unit 8.

図２に示すように、姿勢推定部８は、第１姿勢角度四元数生成部２０（第１姿勢角度四元数生成手段）と、第２姿勢角度四元数生成部２１（第２姿勢角度四元数生成手段）と、フィルタ係数生成部２２（フィルタ係数生成手段）と、姿勢角度四元数合成部２３と、姿勢角度四元数正規化部２４と、オイラー角変換部２５と、姿勢角速度バイアス四元数推定部２６（姿勢角速度バイアス四元数推定手段）と、姿勢角速度バイアス四元数正規化部２７と、によって構成されている。 As shown in FIG. 2, the posture estimation unit 8 includes a first posture angle quaternion generation unit 20 (first posture angle quaternion generation unit) and a second posture angle quaternion generation unit 21 (second posture). Angle quaternion generation unit), filter coefficient generation unit 22 (filter coefficient generation unit), posture angle quaternion synthesis unit 23, posture angle quaternion normalization unit 24, Euler angle conversion unit 25, The posture angular velocity bias quaternion estimation unit 26 (posture angular velocity bias quaternion estimation means) and the posture angular velocity bias quaternion normalization unit 27 are configured.

図３に示すように、姿勢角速度バイアス四元数推定部２６は、差分演算部４０（差分演算手段）と、フィルタ演算部４１（フィルタ演算手段）と、積分演算部４２（積分演算手段）と、によって構成されている。 As shown in FIG. 3, the attitude angular velocity bias quaternion estimating unit 26 includes a difference calculating unit 40 (difference calculating unit), a filter calculating unit 41 (filter calculating unit), an integral calculating unit 42 (integral calculating unit), and , Is composed of.

（傾斜角センサ３）
図１に戻り、傾斜角センサ３は、倒立二輪車本体２の現在の姿勢角度を計測するためのものである。本実施形態において傾斜角センサ３は、３軸加速度センサと地磁気センサの組み合わせによって構成されている。３軸加速度センサは、倒立二輪車本体２の姿勢角度のうちロール角度とピッチ角度を計測可能である。地磁気センサは、倒立二輪車本体２の姿勢角度のうちヨー角度を計測可能である。傾斜角センサ３は、計測により得られた姿勢角度信号を姿勢推定部８の第１姿勢角度四元数生成部２０に出力する。 (Inclination angle sensor 3)
Returning to FIG. 1, the tilt angle sensor 3 is for measuring the current posture angle of the inverted motorcycle body 2. In the present embodiment, the tilt angle sensor 3 is configured by a combination of a triaxial acceleration sensor and a geomagnetic sensor. The triaxial acceleration sensor can measure the roll angle and the pitch angle among the posture angles of the inverted two-wheeled vehicle body 2. The geomagnetic sensor can measure the yaw angle of the posture angle of the inverted motorcycle body 2. The tilt angle sensor 3 outputs the posture angle signal obtained by the measurement to the first posture angle quaternion generation unit 20 of the posture estimation unit 8.

（ジャイロセンサ４）
ジャイロセンサ４は、倒立二輪車本体２の現在の姿勢角速度を計測するためのものである。本実施形態においてジャイロセンサ４は、MEMSジャイロによって構成されている。ジャイロセンサ４は、計測により得られた姿勢角速度信号を姿勢推定部８の第２姿勢角度四元数生成部２１に出力する。 (Gyro sensor 4)
The gyro sensor 4 is for measuring the current posture angular velocity of the inverted motorcycle body 2. In this embodiment, the gyro sensor 4 is configured by a MEMS gyro. The gyro sensor 4 outputs the posture angular velocity signal obtained by the measurement to the second posture angle quaternion generation unit 21 of the posture estimation unit 8.

（第１姿勢角度四元数生成部２０）
図２の第１姿勢角度四元数生成部２０は、傾斜角センサ３の計測結果に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第１姿勢角度四元数α_measuredを生成するものである。具体的には以下の通りである。 (First posture angle quaternion generator 20)
The first posture angle quaternion generation unit 20 in FIG. 2 generates a first posture angle quaternion α _measured that is a quaternion representation of the posture angle of the inverted motorcycle body 2 based on the measurement result of the tilt angle sensor 3. To do. Specifically, it is as follows.

（→フィルタ処理）
先ず、第１姿勢角度四元数生成部２０は、傾斜角センサ３から受信した姿勢角度信号をフィルタ処理して、姿勢角度信号に含まれる望ましくない周波数帯域をカットする。 (→ Filter processing)
First, the first posture angle quaternion generation unit 20 filters the posture angle signal received from the tilt angle sensor 3 and cuts an undesirable frequency band included in the posture angle signal.

（→取り付け角度補正処理）
次に、第１姿勢角度四元数生成部２０は、倒立二輪車本体２に対する３軸加速度センサの取り付け角度に基づいて、姿勢角度信号を補正する。これにより、姿勢角度信号から、３軸加速度センサの取り付け角度による影響がキャンセルされる。 (→ Mounting angle correction process)
Next, the first posture angle quaternion generation unit 20 corrects the posture angle signal based on the attachment angle of the three-axis acceleration sensor with respect to the inverted two-wheeled vehicle body 2. Thereby, the influence by the attachment angle of the triaxial acceleration sensor is canceled from the attitude angle signal.

（→自加速度キャンセル処理）
次に、第１姿勢角度四元数生成部２０は、車輪の回転数を微分して得られる倒立二輪車本体２の加速度に基づいて、姿勢角度信号を補正する。これにより、姿勢角度信号から、自加速度による影響がキャンセルされる。 (→ Self acceleration cancellation process)
Next, the first posture angle quaternion generation unit 20 corrects the posture angle signal based on the acceleration of the inverted two-wheeled vehicle body 2 obtained by differentiating the rotational speed of the wheels. Thereby, the influence by the self acceleration is canceled from the attitude angle signal.

（→姿勢角度行列変換処理）
次に、第１姿勢角度四元数生成部２０は、姿勢角度信号（加速度センサベクトル）にヨー角（方位角）を加えた回転を姿勢角度行列に変換する。 (→ Attitude angle matrix conversion process)
Next, the first posture angle quaternion generation unit 20 converts rotation obtained by adding a yaw angle (azimuth angle) to a posture angle signal (acceleration sensor vector) into a posture angle matrix.

（→四元数変換処理）
次に、第１姿勢角度四元数生成部２０は、姿勢角度行列を、姿勢角度の四元数表現である第１姿勢角度四元数α_measuredに変換する。 (→ Quaternion conversion process)
Next, the first posture angle quaternion generation unit 20 converts the posture angle matrix into a first posture angle quaternion α _measured which is a quaternion representation of the posture angle.

そして、第１姿勢角度四元数生成部２０は、生成した第１姿勢角度四元数α_measuredを、姿勢角度四元数合成部２３と姿勢角速度バイアス四元数推定部２６に出力する。 Then, the first posture angle quaternion generating unit 20 outputs the generated first posture angle quaternion α _measured to the posture angle quaternion synthesizing unit 23 and the posture angular velocity bias quaternion estimating unit 26.

（第２姿勢角度四元数生成部２１）
第２姿勢角度四元数生成部２１は、ジャイロセンサ４の計測結果に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第２姿勢角度四元数α_predictを予測して生成するものである。具体的には以下の通りである。 (Second posture angle quaternion generator 21)
Second posture angle quaternion generator 21 is generated by predicting a second orientation angle quaternion alpha _predict which quaternion number representation of the attitude angle of the inverted two-wheel vehicle body 2 based on the measurement result of the gyro sensor 4 Is. Specifically, it is as follows.

（→フィルタ処理）
先ず、第２姿勢角度四元数生成部２１は、ジャイロセンサ４から受信した姿勢角速度信号をフィルタ処理して、姿勢角速度信号に含まれる望ましくない周波数帯域をカットする。 (→ Filter processing)
First, the second posture angle quaternion generation unit 21 filters the posture angular velocity signal received from the gyro sensor 4 and cuts an undesirable frequency band included in the posture angular velocity signal.

（→予測処理）
次に、第２姿勢角度四元数生成部２１は、RAM６から、前回の姿勢角速度バイアスb_n-1を取得した上で、姿勢角速度信号α^ドット _measuredから前回の姿勢角速度バイアスb_n-1を引き算し、これにサンプリング周期dtを掛け、前回の姿勢角度α_n-1に積算し、結果を四元数に変換する。これにより、第２姿勢角度四元数α_predictが求められる。なお、この予測処理は、始めから四元数で演算してもよいし、とりあえずオイラー角で演算して最後に四元数変換することとしてもよい。オイラー角で演算する場合は、下記式（１）による。 (→ prediction process)
Next, the second posture angle quaternion generator 21 obtains the previous posture angular velocity bias b _n−1 from the RAM ₆ and then obtains the previous posture angular velocity bias b _n−1 from the posture angular velocity signal α ^dot _measured. Subtract, multiply this by the sampling period dt, add to the previous attitude angle α _n−1 , and convert the result to a quaternion. Thereby, the second posture angle quaternion α _predict is obtained. This prediction process may be performed with a quaternion from the beginning, or may be performed with Euler angles for the time being and finally converted into a quaternion. When calculating with the Euler angle, the following equation (1) is used.

そして、第２姿勢角度四元数生成部２１は、生成した第２姿勢角度四元数α_predictを姿勢角度四元数合成部２３と姿勢角速度バイアス四元数推定部２６に出力する。 Then, the second posture angle quaternion generation unit 21 outputs the generated second posture angle quaternion α _predict to the posture angle quaternion synthesis unit 23 and the posture angular velocity bias quaternion estimation unit 26.

（フィルタ係数生成部２２）
フィルタ係数生成部２２は、状態フィードバックを実現する際のオブザーバの一種として、カルマンフィルタ係数K₀、K₁を生成する。カルマンフィルタ係数K₀は姿勢角の計測ノイズをフィルタして姿勢角にフィードバックするための係数であり、カルマンフィルタ係数K₁は姿勢角の計測ノイズをフィルタして姿勢角速度バイアスにフィードバックするための係数である。 (Filter coefficient generator 22)
The filter coefficient generation unit 22 generates Kalman filter coefficients K ₀ and K ₁ as a kind of observer for realizing state feedback. The Kalman filter coefficient K ₀ is a coefficient for filtering posture angle measurement noise and feeding it back to the posture angle. The Kalman filter coefficient K ₁ is a coefficient for filtering posture angle measurement noise and feeding it back to the posture angular velocity bias. .

（姿勢角度四元数合成部２３）
姿勢角度四元数合成部２３は、第１姿勢角度四元数α_measuredと第２姿勢角度四元数α_predictをカルマンフィルタ係数K₀でのフィルタ処理により合成して、倒立二輪車本体２の現在の姿勢角度α_n（推定値）を生成する。この姿勢角度四元数合成部２３による合成処理は、前述したように球面線形補間を利用することで効率良く実行できることを示した。必要であれば、オイラー角で表現した式（２）を参照されたい。姿勢角度四元数合成部２３は、生成した倒立二輪車本体２の現在の姿勢角度α_n（推定値）を姿勢角度四元数正規化部２４に出力する。 (Attitude angle quaternion synthesis unit 23)
The posture angle quaternion combining unit 23 _combines the first posture angle quaternion α _measured and the second posture angle quaternion α _predict by filter processing with the Kalman filter coefficient K ₀ , and A posture angle α _n (estimated value) is generated. It has been shown that the composition processing by the attitude angle quaternion composition unit 23 can be efficiently executed by using spherical linear interpolation as described above. If necessary, see Equation (2) expressed in Euler angles. The posture angle quaternion combining unit 23 outputs the generated current posture angle α _n (estimated value) of the inverted motorcycle body 2 to the posture angle quaternion normalizing unit 24.

（姿勢角度四元数正規化部２４）
姿勢角度四元数正規化部２４は、浮動小数点演算の誤差の蓄積を回避すべく、倒立二輪車本体２の現在の姿勢角度α_nを正規化する。姿勢角度四元数正規化部２４は、正規化した姿勢角度α_nをオイラー角変換部２５に出力する。 (Attitude angle quaternion normalization unit 24)
The posture angle quaternion normalization unit 24 normalizes the current posture angle α _n of the inverted two-wheeled vehicle body 2 so as to avoid accumulation of errors in floating point arithmetic. The posture angle quaternion normalization unit 24 outputs the normalized posture angle α _n to the Euler angle conversion unit 25.

（オイラー角変換部２５）
オイラー角変換部２５は、姿勢角度α_nをオイラー角表現に変換した上で姿勢制御部９に出力する。 (Euler angle converter 25)
The Euler angle conversion unit 25 converts the posture angle α _n into Euler angle expression and outputs the converted value to the posture control unit 9.

（姿勢角速度バイアス四元数推定部２６：図３）
姿勢角速度バイアス四元数推定部２６は、下記式（９）により角速度バイアス四元数b_nを推定する。具体的には以下の通りである。 (Attitude angular velocity bias quaternion estimation unit 26: FIG. 3)
The posture angular velocity bias quaternion estimation unit 26 estimates the angular velocity bias quaternion b _n by the following equation (9). Specifically, it is as follows.

姿勢角速度バイアスb_nは、前述したように下記式（３）により求められる。 The attitude angular velocity bias b _n is obtained by the following equation (3) as described above.

上記式（３）は、前述した式（２）のようには球面線形補間を利用できるかたちに変形することができない。そこで、姿勢角速度バイアス四元数推定部２６は、四元数の累乗（べき乗）という概念を導入して、上記式（３）を効率的に解く。詳しく説明すると、q^tは四元数qのt乗を意味するが、t=0のときq⁰=1、即ち恒等四元数(1,0,0,0)を示す。また、t=1のときq¹=qとなり、実数と同等の振る舞いをする。注意すべきは、t<0の場合とt>1の場合であり、四元数は360度以上の回転を示すことができないので、tを大きくして複数回転を表すようなことはできない。(a^s)^t=a^stのようなスカラーの累乗に関する代数が当てはまらないことがある。しかし、今回のようなフィルタ演算の場合は、ノイズをフィルタリングして小さくするという意味からK₁の絶対値は必ず1より小さい値になる上に、第１姿勢角度四元数α_measuredも第２姿勢角度四元数α_predictもある1つの角度を示すことから累乗の乗数が０〜１の範囲で使用することができる。なお、四元数の累乗は乗数が０以上１以下の範囲で使用することが一般的である。 The above equation (3) cannot be transformed into a form in which spherical linear interpolation can be used like the above-described equation (2). Therefore, the attitude angular velocity bias quaternion estimation unit 26 introduces the concept of a quaternion power (power) and efficiently solves the above equation (3). More specifically, q ^t means the quaternion q to the power of t. When t = 0, q ⁰ = 1, that is, the identity quaternion (1,0,0,0). In addition, when t = 1, q ¹ = q, which behaves like a real number. It should be noted that when t <0 and t> 1, the quaternion cannot indicate a rotation of 360 degrees or more, so it is not possible to increase t to represent multiple rotations. Algebras about scalar powers such as (a ^s ) ^t = a ^st may not apply. However, in the case of this filter operation, the absolute value of K ₁ is always smaller than _{1 in} order to reduce noise by filtering, and the first attitude angle quaternion α _measured is also the second. Since the attitude angle quaternion α _predict also indicates one angle, the power multiplier can be used in the range of 0 to 1. In general, the power of the quaternion is used when the multiplier is in the range of 0 to 1.

さて、オイラー角表現での足し算は四元数表現では外積（演算子は×）であり、オイラー角表現での引き算は四元数表現では共役四元数（演算子は^−１）との外積である。また、回転のスカラーの掛け算は、四元数表現での累乗に相当する。従って、上記式（３）は、上記式（９）に置き換えることができる。上記式（９）の意味としては、姿勢角度四元数αのノイズ成分をカルマンフィルタ係数K₁でフィルタリングし、これを用いて姿勢角速度バイアス四元数b_n-1を回転させて修正する、ということになる。 Now, addition in Euler angle representation is a cross product (operator is x) in quaternion representation, and subtraction in Euler angle representation is a cross product with conjugate quaternion (operator is ^-1 ) in quaternion representation. It is. The multiplication of the rotation scalar corresponds to the power in the quaternion representation. Therefore, the above formula (3) can be replaced with the above formula (9). As the meaning of the above equation (9), the noise component of the posture angle quaternion α is filtered by the Kalman filter coefficient K ₁ and is used to rotate and correct the posture angular velocity bias quaternion b _n−1. It will be.

従って、図３において、差分演算部４０は、第１姿勢角度四元数生成部２０から取得した第１姿勢角度四元数α_measuredと、第２姿勢角度四元数生成部２１から取得した第２姿勢角度四元数α_predictと、の差分を求めて姿勢角度四元数αのノイズ成分を算出する。 Therefore, in FIG. 3, the difference calculation unit 40 includes the first posture angle quaternion α _measured acquired from the first posture angle quaternion generation unit 20 and the second posture angle quaternion generation unit 21 acquired from the second posture angle quaternion generation unit 21. The difference between the two attitude angle quaternions α _predict is obtained, and the noise component of the attitude angle quaternion α is calculated.

次に、フィルタ演算部４１は、差分演算部４０が算出した姿勢角度四元数αのノイズ成分をカルマンフィルタ係数K₁でフィルタ処理する。このとき、カルマンフィルタ係数K₁が負である場合は、フィルタ演算部４１は、カルマンフィルタ係数K₁の正負を反転させた上で、姿勢角度四元数αのノイズ成分をフィルタ処理する。即ち、上記式（９）に代えて下記式（１０）を用いる。 Next, the filter calculation unit 41 filters the noise component of the attitude angle quaternion α calculated by the difference calculation unit 40 with the Kalman filter coefficient K ₁ . At this time, if the Kalman filter coefficient K ₁ is negative, the filter calculation unit 41 inverts the sign of the Kalman filter coefficient K ₁ and filters the noise component of the attitude angle quaternion α. That is, the following formula (10) is used instead of the above formula (9).

また、フィルタの方式としてカルマンフィルタなどの係数が変化するタイプが存在する。例えば逐次カルマンフィルタではK₀及びK₁の絶対値が一時的に１より大きな値になることがある。この場合、フィルタ演算部４１は、カルマンフィルタ係数K₁の絶対値を１にした上で、前記の式（９）又は（１０）により姿勢角速度バイアス四元数b_nを推定すればよい。カルマンフィルタ係数K₀についても同様である。 In addition, as a filter method, there is a type in which coefficients such as a Kalman filter change. For example, in the sequential Kalman filter, the absolute values of K ₀ and K ₁ may temporarily be larger than 1. In this case, the filter calculation unit 41 may set the absolute value of the Kalman filter coefficient K ₁ to 1 and estimate the posture angular velocity bias quaternion b _n by the above equation (9) or (10). The same applies to the Kalman filter coefficient K _0.

次に、積分演算部４２は、フィルタ処理後の値を、前回の姿勢角速度バイアス四元数b_n-1に積算して、現在の姿勢角速度バイアス四元数b_nを求める。積分演算部４２は、求めた姿勢角速度バイアス四元数b_nを姿勢角速度バイアス四元数正規化部２７に出力する。 Then, the integral calculating section 42, the value after filtering, by integrating the previous posture angular velocity bias quaternion b _n-1, obtains the current posture angular velocity bias quaternion b _n. The integral calculation unit 42 outputs the obtained posture angular velocity bias quaternion b _n to the posture angular velocity bias quaternion normalization unit 27.

（姿勢角速度バイアス四元数正規化部２７）
姿勢角速度バイアス四元数正規化部２７は、浮動小数点演算の誤差の蓄積を回避すべく、姿勢角速度バイアス四元数b_nを正規化する。そして、姿勢角速度バイアス四元数正規化部２７は、正規化した姿勢角速度バイアス四元数b_nをRAM６に保存する。 (Attitude angular velocity bias quaternion normalization unit 27)
The attitude angular velocity bias quaternion normalization unit 27 normalizes the attitude angular velocity bias quaternion b _n in order to avoid accumulation of errors in floating point arithmetic. Then, the posture angular velocity bias quaternion normalization unit 27 stores the normalized posture angular velocity bias quaternion b _n in the RAM 6.

次に、図４を参照して、主として姿勢推定部８の作動を説明する。 Next, the operation of the posture estimation unit 8 will be mainly described with reference to FIG.

（傾斜角センサによる計測ステップ）
倒立二輪車１の電源を投入すると（S300）、傾斜角センサ３は、倒立二輪車本体２の現在の姿勢角度を計測して、姿勢角度信号を第１姿勢角度四元数生成部２０に出力する（S310）。次に、ジャイロセンサ４は、倒立二輪車本体２の現在の姿勢角速度を計測して、姿勢角速度信号を第２姿勢角度四元数生成部２１に出力する（S320）。次に、第１姿勢角度四元数生成部２０は、傾斜角センサ３から受信した姿勢角度信号に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第１姿勢角度四元数α_measuredを生成する（S330）。次に、第２姿勢角度四元数生成部２１は、ジャイロセンサ４から受信した姿勢角速度信号に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第２姿勢角度四元数α_predictを予測して生成する（S340）。次に、フィルタ係数生成部２２は、カルマンフィルタ係数K₀、K₁を生成する（S350）。なお、S310とS320の処理は可換であり、S330〜S350の処理は適宜順番を入れ替えても良い。次に、姿勢角速度バイアス四元数推定部２６は、下記式（９）により姿勢角速度バイアス四元数b_nを推定する（S360）。そして、処理をS310に戻して次の周期に備える。 (Measurement step by tilt angle sensor)
When the power of the inverted motorcycle 1 is turned on (S300), the tilt angle sensor 3 measures the current posture angle of the inverted motorcycle body 2 and outputs a posture angle signal to the first posture angle quaternion generator 20 ( S310). Next, the gyro sensor 4 measures the current posture angular velocity of the inverted motorcycle body 2 and outputs a posture angular velocity signal to the second posture angle quaternion generation unit 21 (S320). Next, the first posture angle quaternion generator 20 generates a first posture angle quaternion α that is a quaternion representation of the posture angle of the inverted motorcycle body 2 based on the posture angle signal received from the tilt angle sensor 3. Generate _measured (S330). Next, the second posture angle quaternion generator 21, the second posture angle quaternion which quaternion number representation of the attitude angle of the inverted two-wheel vehicle body 2 based on the attitude angular rate signal received from the gyro sensor 4 alpha _predict Is predicted and generated (S340). Next, the filter coefficient generation unit 22 generates Kalman filter coefficients K ₀ and K ₁ (S350). Note that the processes of S310 and S320 are interchangeable, and the processes of S330 to S350 may be interchanged as appropriate. Next, the posture angular velocity bias quaternion estimation unit 26 estimates the posture angular velocity bias quaternion b _n by the following equation (9) (S360). Then, the process returns to S310 to prepare for the next cycle.

以上に本願発明の好適な実施形態を説明したが、上記実施形態は、要するに、以下の特長を有している。 Although the preferred embodiment of the present invention has been described above, the above embodiment has the following features in short.

姿勢推定部８（姿勢推定装置）は、傾斜角センサ３とジャイロセンサ４を備えた倒立二輪車本体２（機器本体）の姿勢を傾斜角センサ３の計測結果とジャイロセンサ４の計測結果に基づいて推定するものである。姿勢推定部８は、傾斜角センサ３の計測結果に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第１姿勢角度四元数α_measuredを生成する第１姿勢角度四元数生成部２０（第１姿勢角度四元数生成手段）と、ジャイロセンサ４の計測結果に基づいて倒立二輪車本体２の姿勢角度の四元数表現である第２姿勢角度四元数α_predictを予測して生成する第２姿勢角度四元数生成部２１（第２姿勢角度四元数生成手段）と、カルマンフィルタ係数K₁（フィルタ係数K₁）を生成するフィルタ係数生成部２２（フィルタ係数生成手段）と、下記式（９）により姿勢角速度バイアス四元数b_nを推定する姿勢角速度バイアス四元数推定部２６（姿勢角速度バイアス四元数推定手段）と、を備える。ただし、b_n-1は、前回の姿勢角速度バイアス四元数である。 The posture estimation unit 8 (posture estimation device) determines the posture of the inverted two-wheeled vehicle body 2 (equipment main body) including the tilt angle sensor 3 and the gyro sensor 4 based on the measurement result of the tilt angle sensor 3 and the measurement result of the gyro sensor 4. To be estimated. The posture estimation unit 8 generates a first posture angle quaternion that generates a first posture angle quaternion α _measured that is a quaternion representation of the posture angle of the inverted motorcycle body 2 based on the measurement result of the tilt angle sensor 3. and part 20 (first posture angle quaternion generating means), predicts a second orientation angle quaternion alpha _predict which quaternion number representation of the attitude angle of the inverted two-wheel vehicle body 2 based on the measurement result of the gyro sensor 4 A second attitude angle quaternion generator 21 (second attitude angle quaternion generator) and a filter coefficient generator 22 (filter coefficient generator) for generating a Kalman filter coefficient K ₁ (filter coefficient K ₁ ). And a posture angular velocity bias quaternion estimating unit 26 (posture angular velocity bias quaternion estimating means) for estimating a posture angular velocity bias quaternion b _n by the following equation (9). However, b _n−1 is the previous attitude angular velocity bias quaternion.

以上の構成によれば、四元数表現をオイラー角表現に変換したり、オイラー角表現を四元数表現に変換する必要がなく、四元数表現のまま姿勢角速度バイアス四元数b_nを推定することができる。従って、角速度バイアス四元数b_nを高い計算効率で推定することができる。 According to the above configuration, to convert quaternary number representation Euler angles representing the Euler angles representation there is no need to convert the quaternion representation, the left posture angular velocity bias quaternion b _n quaternary number representation Can be estimated. Therefore, the angular velocity bias quaternion b _n can be estimated with high calculation efficiency.

また、姿勢角速度バイアス姿勢推定部８は、カルマンフィルタ係数K₁が負である場合は、カルマンフィルタ係数K₁の正負を反転させた上で、前記式（９）により姿勢角速度バイアス四元数b_nを推定する。以上の構成によれば、四元数の累乗を問題なく演算することができる。 Further, when the Kalman filter coefficient K ₁ is negative, the attitude angular velocity bias attitude estimation unit 8 inverts the sign of the Kalman filter coefficient K ₁ and then calculates the attitude angular velocity bias quaternion b _{n according} to the above equation (9). presume. According to the above configuration, the power of the quaternion can be calculated without any problem.

また、姿勢角速度バイアス姿勢推定部８は、カルマンフィルタ係数K₁の絶対値が１を超える場合は、カルマンフィルタ係数K₁の絶対値を１にした上で、前記式（９）により姿勢角速度バイアス四元数b_nを推定する。以上の構成によれば、四元数の累乗を問題なく演算することができる。 Further, the posture angular velocity bias posture estimation unit 8, if the absolute value of the Kalman filter coefficient K ₁ is greater than 1, after the absolute value of the Kalman filter coefficient K ₁ to 1, the formula (9) by the posture angular velocity bias quaternion Estimate the number b _n . According to the above configuration, the power of the quaternion can be calculated without any problem.

なお、角速度バイアスb_nの推定を始めるに際し、角速度バイアスb_nの初期値は「回転しない」ことを表す恒等四元数（1,0,0,0）とすることが考えれる。この場合、倒立二輪車１の状態（温度や各角速度センサのバイアス固有値）によりいずれある値に収束していくことになる。好ましくは、倒立二輪車１を起動し、姿勢推定のフィルタ演算を開始するときに姿勢角速度バイアスb_nの初期値を恒等四元数から開始することに代えて、前回の起動後に収束した値を初期値として採用すれば、一層安定した姿勢角速度バイアスb_nの推定を行うことができる。 When the estimation of the angular velocity bias b _n is started, it is conceivable that the initial value of the angular velocity bias b _n is the identity quaternion (1,0,0,0) representing “not rotating”. In this case, it will eventually converge to a certain value depending on the state of the inverted motorcycle 1 (temperature and the bias eigenvalue of each angular velocity sensor). Preferably, instead of starting the initial value of the attitude angular velocity bias b _n from the identity quaternion when starting the inverted motorcycle 1 and starting the attitude estimation filter calculation, the value converged after the previous activation is calculated. If adopted as the initial value, the posture angular velocity bias b _n can be estimated more stably.

１倒立二輪車
２倒立二輪車本体
３傾斜角センサ
４ジャイロセンサ
８姿勢推定部
９姿勢制御部
２６姿勢角速度バイアス四元数推定部 DESCRIPTION OF SYMBOLS 1 Inverted motorcycle 2 Inverted motorcycle main body 3 Inclination angle sensor 4 Gyro sensor 8 Attitude estimation part 9 Attitude control part 26 Attitude angular velocity bias quaternion estimation part

Claims

傾斜角センサとジャイロセンサを有する機器本体において、ジャイロセンサの静止状態における出力値の四元数表現である角速度バイアス四元数を推定するバイアス四元数推定装置であって、
前記傾斜角センサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第1姿勢角度四元数αmeasuredを生成する第１姿勢角度四元数生成手段と、
前記ジャイロセンサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成する第２姿勢角度四元数生成手段と、
フィルタ係数K1を生成するフィルタ係数生成手段と、
下記式により角速度バイアス四元数bnを推定する角速度バイアス四元数推定手段と、
を備えたバイアス四元数推定装置。

ただし、bn-1は、前回の角速度バイアス四元数である。 In a device body having an inclination angle sensor and a gyro sensor, a bias quaternion estimation device that estimates an angular velocity bias quaternion that is a quaternion expression of an output value in a stationary state of the gyro sensor,
First posture angle quaternion generation means for generating a first posture angle quaternion αmeasured which is a quaternion representation of the posture angle of the device main body based on the measurement result of the tilt angle sensor;
Second attitude angle quaternion generation means for predicting and generating a second attitude angle quaternion αpredict, which is a quaternion representation of the attitude angle of the device body, based on the measurement result of the gyro sensor;
Filter coefficient generation means for generating the filter coefficient K1,
Angular velocity bias quaternion estimating means for estimating angular velocity bias quaternion bn by the following equation;
Bias quaternion estimation device.

However, bn-1 is the previous angular velocity bias quaternion.

請求項１に記載のバイアス四元数推定装置であって、The bias quaternion estimation device according to claim 1,
前記第２姿勢角度四元数生成手段は、前記ジャイロセンサの計測結果から前回の角速度バイアスを引き算し、引き算した結果にサンプリング周期を掛け算し、前記機器本体の前回の姿勢角度に積算することで、前記第２姿勢角度四元数αpredictを生成する、The second attitude angle quaternion generating means subtracts the previous angular velocity bias from the measurement result of the gyro sensor, multiplies the subtracted result by a sampling period, and adds the result to the previous attitude angle of the device body. Generating the second attitude angle quaternion αpredict,
バイアス四元数推定装置。Bias quaternion estimation device.

請求項１又は２に記載の姿勢推定装置であって、
前記角速度バイアス四元数推定手段は、前記フィルタ係数K1が負である場合は、前記フィルタ係数K1の正負を反転させた上で、請求項１の式により前記角速度バイアス四元数bnを推定する、
バイアス四元数推定装置。 The posture estimation apparatus according to claim 1 or 2 ,
The angular velocity bias quaternion estimating means estimates the angular velocity bias quaternion bn according to the equation of claim 1 after inverting the sign of the filter coefficient K1 when the filter coefficient K1 is negative. ,
Bias quaternion estimation device .

請求項１〜３の何れかに記載の姿勢推定装置であって、
前記角速度バイアス四元数推定手段は、前記フィルタ係数K1の絶対値が１を超える場合は、前記フィルタ係数K1の絶対値を１にした上で、請求項１の式により前記角速度バイアス四元数bnを推定する、
バイアス四元数推定装置。 The posture estimation apparatus according to any one of claims 1 to 3 ,
The angular velocity bias quaternion estimating means sets the absolute value of the filter coefficient K1 to 1 when the absolute value of the filter coefficient K1 exceeds 1, and then calculates the angular velocity bias quaternion according to the equation of claim 1. estimate bn ,
Bias quaternion estimation device .

傾斜角センサとジャイロセンサを有する機器本体において、ジャイロセンサの静止状態における出力値の四元数表現である角速度バイアス四元数を推定するバイアス四元数推定方法であって、
前記傾斜角センサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第1姿勢角度四元数αmeasuredを生成する第１姿勢角度四元数生成ステップと、
前記ジャイロセンサの計測結果に基づいて前記機器本体の姿勢角度の四元数表現である第２姿勢角度四元数αpredictを予測して生成する第２姿勢角度四元数生成ステップと、
フィルタ係数K1を生成するフィルタ係数生成ステップと、
下記式により角速度バイアス四元数bnを推定する角速度バイアス四元数推定ステップと、
を含む、バイアス四元数推定方法。

ただし、bn-1は、前回の角速度バイアス四元数である。 In a device body having an inclination angle sensor and a gyro sensor, a bias quaternion estimation method for estimating an angular velocity bias quaternion that is a quaternion expression of an output value in a stationary state of the gyro sensor,
A first posture angle quaternion generating step for generating a first posture angle quaternion αmeasured which is a quaternion representation of the posture angle of the device main body based on the measurement result of the tilt angle sensor;
A second attitude angle quaternion generation step that predicts and generates a second attitude angle quaternion αpredict that is a quaternion representation of the attitude angle of the device body based on the measurement result of the gyro sensor;
A filter coefficient generation step for generating the filter coefficient K1,
Angular velocity bias quaternion estimation step for estimating the angular velocity bias quaternion bn by the following equation;
Including a bias quaternion estimation method.

However, bn-1 is the previous angular velocity bias quaternion.