JP2007183138A - Compact attitude sensor - Google Patents

Compact attitude sensor Download PDF

Info

Publication number
JP2007183138A
JP2007183138A JP2006000850A JP2006000850A JP2007183138A JP 2007183138 A JP2007183138 A JP 2007183138A JP 2006000850 A JP2006000850 A JP 2006000850A JP 2006000850 A JP2006000850 A JP 2006000850A JP 2007183138 A JP2007183138 A JP 2007183138A
Authority
JP
Japan
Prior art keywords
sensor
acceleration
quaternion
vector
angular velocity
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.)
Granted
Application number
JP2006000850A
Other languages
Japanese (ja)
Other versions
JP4876204B2 (en
Inventor
Kenzo Nonami
健蔵 野波
Shin Shin
振玉 辛
Satoshi Suzuki
智 鈴木
Daisuke Nakazawa
大輔 中澤
Yoshiro Koide
義朗 小出
Keitaro Matsuzaka
敬太郎 松坂
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hirobo Ltd
Original Assignee
Hirobo Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Hirobo Ltd filed Critical Hirobo Ltd
Priority to JP2006000850A priority Critical patent/JP4876204B2/en
Publication of JP2007183138A publication Critical patent/JP2007183138A/en
Application granted granted Critical
Publication of JP4876204B2 publication Critical patent/JP4876204B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Navigation (AREA)

Abstract

<P>PROBLEM TO BE SOLVED: To provide a compact attitude sensor capable of calculating easily a Kalman gain and a quaternion to mount practically an attitude sensor, capable of reducing a size, and allowing real time processing. <P>SOLUTION: This compact attitude sensor is provided with an angular velocity sensor 101 for measuring angular velocities on three independent axes of a moving object, an acceleration sensor 102 for measuring accelerations on the three independent axes of the moving object, a geomagnetic sensor 103 for measuring geomagnetisms on the three independent axes of the moving object, and an arithmetic processing part 104 for calculating an estimated value of the quaternion, based on measured values in the angular velocity sensor 101, the acceleration sensor 102 and the geomagnetic sensor 103, wherein the estimated value of the quaternion is output as a three-dimensional attitude angle expressed by the quaternion. <P>COPYRIGHT: (C)2007,JPO&INPIT

Description

姿勢制御を要する移動物体に搭載する、汎用性のある小型姿勢センサに関する。 The present invention relates to a versatile small posture sensor mounted on a moving object that requires posture control.

姿勢制御を要する移動物体には、その三次元姿勢角を計測するため姿勢センサを備えている。一般的に姿勢センサは移動物体の独立な３軸上の角速度を計測する角速度センサ、及び角速度センサにより得られる角速度の計測値を時間的に積分することにより、移動物体の三次元姿勢角を取得する処理を行う演算処理部を備えている。三次元姿勢角はロール・ピッチ・ヨー角等を用いてオイラー角により表現されるものがあり、姿勢センサにより計測される三次元姿勢角を基に移動物体の姿勢制御が行われる。 A moving object that requires posture control is provided with a posture sensor to measure its three-dimensional posture angle. In general, a posture sensor obtains the three-dimensional posture angle of a moving object by temporally integrating the angular velocity sensor that measures the angular velocity on three independent axes of the moving object and the angular velocity measurement value obtained by the angular velocity sensor. An arithmetic processing unit for performing the processing is provided. Some three-dimensional posture angles are expressed by Euler angles using roll, pitch, yaw angle, etc., and the posture control of the moving object is performed based on the three-dimensional posture angle measured by the posture sensor.

しかしながら、オイラー角により三次元姿勢角を表現する場合、オイラー角特有の特異点の問題がある。具体的に説明すると、ピッチ角が±９０度の特異姿勢に近づくとロール角及びヨー角の出力値が急激に変動してしまうという問題がある。さらに、角速度センサは、誤差が生じやすいという問題がある。このように、姿勢センサにより計測される三次元姿勢角が高精度に計測されなければ、移動物体の姿勢制御もまた高精度に行うことができないという問題がある。 However, when the three-dimensional posture angle is expressed by the Euler angle, there is a problem of a singular point peculiar to the Euler angle. More specifically, there is a problem that the output values of the roll angle and yaw angle fluctuate rapidly when the pitch angle approaches a singular posture of ± 90 degrees. Furthermore, the angular velocity sensor has a problem that an error is likely to occur. Thus, there is a problem that the posture control of the moving object cannot be performed with high accuracy unless the three-dimensional posture angle measured by the posture sensor is measured with high accuracy.

こうした問題を解決するため、３つの虚数と１つの実数からなる４次のベクトル表現を用いたクォータニオンにより三次元姿勢角を表現し、これにより、上記特異姿勢の問題を解消する姿勢センサについて開示した特許文献がある。
特開平９−５１０４号公報 In order to solve such a problem, a posture sensor that represents a three-dimensional posture angle by a quaternion using a fourth-order vector representation composed of three imaginary numbers and one real number, thereby solving the above-mentioned singular posture problem has been disclosed. There is patent literature.
Japanese Patent Laid-Open No. 9-5104

図１０に示すように、特許文献１に開示されている姿勢センサ１０００は、移動物体の独立な３軸上の角速度を計測する角速度センサ１００１、移動物体の独立な３軸上の加速度を計測する加速度センサ１００２、及び角速度センサ１００１及び加速度センサ１００２の計測値を基にクォータニオンを算出する演算処理部１００３を備えている。演算処理部１００３では、加速度の計測値を基にして得られる誤差と拡張カルマンフィルタから得られるカルマンゲインとを用いてクォータニオンにより表現する三次元姿勢角の補正値を算出し、補正値を用いて角速度の計測値を補正し、クォータニオンにより表現する三次元姿勢角を算出する処理を行う。これにより、特許文献１に開示されている姿勢センサは、特異姿勢の問題がなく、移動物体の姿勢制御も高精度に行うことができることについて開示している。 As shown in FIG. 10, an attitude sensor 1000 disclosed in Patent Document 1 measures an angular velocity sensor 1001 that measures an independent three-axis angular velocity of a moving object, and measures an independent three-axis acceleration of the moving object. An arithmetic processing unit 1003 is provided that calculates a quaternion based on measurement values of the acceleration sensor 1002, the angular velocity sensor 1001, and the acceleration sensor 1002. The arithmetic processing unit 1003 calculates a correction value of a three-dimensional posture angle expressed by quaternions using an error obtained based on an acceleration measurement value and a Kalman gain obtained from an extended Kalman filter, and uses the correction value to calculate an angular velocity. The measurement value is corrected, and a process of calculating a three-dimensional attitude angle expressed by quaternions is performed. Thus, the posture sensor disclosed in Patent Document 1 discloses that there is no problem of a specific posture and posture control of a moving object can be performed with high accuracy.

しかしながら、上記特許文献１に示されている「拡張カルマンフィルタ」は非線形システムにおいて用いられるので、現実的な実装には不向きであるという問題がある。すなわち、上記特許文献１に示される姿勢センサは、概念的に説明されてはいるものの、非線形モデル式が大変複雑となることから「カルマンゲイン」や「クォータニオン」の算出も大変複雑となり、現実的な姿勢センサの実装は非常に困難であるという問題がある。なお、仮に実装できたとしても、小型化は実現できず、また、演算処理部で行う補正処理が収束しないおそれもあり、さらに実時間処理に乏しいという問題がある。 However, since the “extended Kalman filter” disclosed in Patent Document 1 is used in a nonlinear system, there is a problem that it is not suitable for practical implementation. That is, although the attitude sensor shown in Patent Document 1 is conceptually explained, the calculation of “Kalman gain” and “Quaternion” is very complicated because the nonlinear model formula is very complicated, and it is realistic. There is a problem that it is very difficult to mount a simple posture sensor. Even if it can be implemented, the size reduction cannot be realized, the correction processing performed by the arithmetic processing unit may not converge, and there is a problem that the real-time processing is scarce.

こうした問題に着目し、本発明は、カルマンゲインやクォータニオンの算出を容易にして現実的な姿勢センサの実装を可能にし、さらに、小型化及び実時間処理を可能とする小型姿勢センサを提供することを目的とする。 Focusing on these problems, the present invention provides a small posture sensor that facilitates the calculation of Kalman gain and quaternion, enables the mounting of a realistic posture sensor, and enables miniaturization and real-time processing. With the goal.

本発明の請求項１に記載の小型姿勢センサは、移動物体の独立な３軸上の角速度を計測する角速度センサと、移動物体の独立な３軸上の加速度を計測する加速度センサと、移動物体の独立な３軸上の地磁気を計測する地磁気センサと、前記角速度センサ、前記加速度センサ、及び前記地磁気センサの計測値を基にクォータニオンの推定値を算出する演算処理部とを備え、前記演算処理部は、予め推定されたクォータニオン推定値を用いて座標変換行列を算出し、当該座標変換行列を用いて地上固定座標系成分の地磁気ベクトル及び重力加速度ベクトルを変換し、現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルを推定する座標変換部と、前記地磁気センサ及び前記加速度センサから現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルを取得する地磁気・加速度センサデータ取得部と、前記座標変換部で推定した現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルと、前記地磁気・加速度センサデータ取得部で取得した現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルとの誤差を算出する誤差算出部と、前記誤差算出部で算出した誤差について、ヤコビ行列を用いてクォータニオンレベルの誤差を算出するクォータニオン誤差算出部と、前記角速度センサから現ステップのセンサ座標系成分の角速度ベクトルを取得する角速度センサデータ取得部と、前記クォータニオン誤差算出部が算出した現ステップのクォータニオンレベルの誤差、前記角速度センサデータ取得部から取得した現ステップの角速度ベクトル、ならびに前記予め推定されたクォータニオン推定値を用いて定常カルマンフィルタを構成し、現ステップのクォータニオン補正を行うクォータニオン補正部と、前記クォータニオン補正部で補正を行ったクォータニオンを積分することにより次ステップのクォータニオン推定値を算出するクォータニオン推定部とを有し、前記クォータニオン推定値をクォータニオンにより表現する三次元姿勢角として出力することを特徴とする。 According to a first aspect of the present invention, there is provided a small-sized attitude sensor that measures an angular velocity on three independent axes of a moving object, an acceleration sensor that measures an independent three-axis acceleration of the moving object, and a moving object. A geomagnetic sensor for measuring geomagnetism on three independent axes, and an angular velocity sensor, an acceleration sensor, and an arithmetic processing unit for calculating an estimated value of a quaternion based on measurement values of the geomagnetic sensor. The unit calculates a coordinate transformation matrix using a pre-estimated quaternion estimated value, converts the geomagnetic vector and the gravitational acceleration vector of the ground fixed coordinate system component using the coordinate transformation matrix, and the sensor coordinate system component of the current step A coordinate conversion unit for estimating a geomagnetic vector and a gravitational acceleration vector, and a sensor coordinate system component of the current step from the geomagnetic sensor and the acceleration sensor. A geomagnetism / acceleration sensor data acquisition unit for acquiring an air vector and a gravitational acceleration vector; a geomagnetic vector and a gravitational acceleration vector of a sensor coordinate system component of the current step estimated by the coordinate conversion unit; and the geomagnetic / acceleration sensor data acquisition unit. An error calculation unit that calculates an error between the acquired geomagnetic vector and gravitational acceleration vector of the sensor coordinate system component of the current step, and a quaternion that calculates a quaternion level error using a Jacobian matrix for the error calculated by the error calculation unit An error calculating unit, an angular velocity sensor data acquiring unit that acquires an angular velocity vector of a sensor coordinate system component of the current step from the angular velocity sensor, an error of the quaternion level of the current step calculated by the quaternion error calculating unit, and acquiring the angular velocity sensor data Current step A stationary Kalman filter is configured using the angular velocity vector of the current value and the quaternion estimation value estimated in advance, and a quaternion correction unit that performs quaternion correction at the current step and a quaternion corrected by the quaternion correction unit are integrated by A quaternion estimation unit that calculates a quaternion estimation value of the step, and outputs the quaternion estimation value as a three-dimensional attitude angle expressed by quaternions.

本発明の請求項２に記載の発明は、請求項１に記載の小型姿勢センサについて、前記算出された次ステップのクォータニオン推定値は、前記座標変換部あるいは前記クォータニオン補正部に入力されることを特徴とする。 According to a second aspect of the present invention, in the small posture sensor according to the first aspect, the calculated quaternion estimated value of the next step is input to the coordinate conversion unit or the quaternion correction unit. Features.

本発明の請求項３に記載の発明は、請求項１に記載の小型姿勢センサについて、前記定常カルマンフィルタは、

Figure 2007183138
上記の式で演算処理することを特徴とする。 The invention according to claim 3 of the present invention is the small attitude sensor according to claim 1, wherein the stationary Kalman filter is:
Figure 2007183138
 The arithmetic processing is performed by the above formula.

本発明の請求項４に記載の発明は、請求項１に記載の小型姿勢センサについて、前記演算処理部は、前記角速度センサを用いて前記加速度センサの取り付け位置によって生じる加速度の誤測を補正し、移動物体の速度データを検知するＧＰＳセンサを用いて当該速度データを微分することにより取得する移動物体の並進加速度データを用いて、移動物体に生じる加速度に伴う誤測を補正することを特徴とする。 According to a fourth aspect of the present invention, in the compact posture sensor according to the first aspect, the arithmetic processing unit corrects an erroneous measurement of acceleration caused by a mounting position of the acceleration sensor using the angular velocity sensor. Using the translational acceleration data of the moving object obtained by differentiating the speed data using a GPS sensor that detects the speed data of the moving object, and correcting mismeasurements associated with the acceleration generated in the moving object To do.

本発明の請求項５に記載の発明は、請求項１に記載の小型姿勢センサについて、前記演算処理部は、角速度センサから取得する角速度ベクトルを用いて前記加速度センサから取得するセンサ座標系成分の重力加速度ベクトルの補正を行うセンサ座標系加速度補正部と、
移動物体の速度データを検知するＧＰＳセンサを用いて当該速度データを微分することにより取得する移動物体の並進加速度データを用いて地上固定座標系成分の重力加速度ベクトルの補正を行う地上固定座標系加速度補正部とを有し、移動物体に生じる加速度に伴う誤測を補正することを特徴とする。 According to a fifth aspect of the present invention, in the compact posture sensor according to the first aspect, the arithmetic processing unit uses a sensor coordinate system component acquired from the acceleration sensor using an angular velocity vector acquired from the angular velocity sensor. A sensor coordinate system acceleration correction unit for correcting the gravitational acceleration vector;
Fixed ground coordinate system acceleration that corrects gravity acceleration vector of ground fixed coordinate system component using translational acceleration data of moving object obtained by differentiating the speed data using GPS sensor that detects speed data of moving object And a correction unit that corrects mismeasurements caused by acceleration generated in the moving object.

本発明の請求項６に記載の発明は、請求項４または５に記載の小型姿勢センサについて、前記角速度センサ及び前記ＧＰＳセンサを用いて行う補正は、

Figure 2007183138
上記の式で演算処理することを特徴とする。 In the invention according to claim 6 of the present invention, the correction performed by using the angular velocity sensor and the GPS sensor for the small posture sensor according to claim 4 or 5,
Figure 2007183138
 The arithmetic processing is performed by the above formula.

上記の通り、本発明の小型姿勢センサは、１入力１出力の線形システムにおいて適用し、定常カルマンフィルタを構成することにより、「カルマンゲイン」や「クォータニオン」の算出を容易にすることができ、現実的な実装を可能にし、さらに、小型化及び実時間処理を可能とする。本発明の姿勢センサにより計測される三次元姿勢角は高精度であり、移動物体の姿勢制御もまた高精度に行うことができる。 As described above, the small attitude sensor of the present invention can be applied to a linear system with one input and one output and constitute a stationary Kalman filter, thereby making it easy to calculate “Kalman gain” and “quaternion”. Realization, and further miniaturization and real-time processing. The three-dimensional posture angle measured by the posture sensor of the present invention is highly accurate, and the posture control of the moving object can also be performed with high accuracy.

また、本発明の小型姿勢センサは、移動物体に加速度が生じたときでも加速度センサデータの誤差補正を行うことができる。この時の誤差補正は、加速度センサの取り付け位置によって生じる加速度の誤測について角速度センサを用いて補正し、また、移動物体に生じる加速度に伴う誤測について、ＧＰＳセンサから取得する速度データを微分することにより取得する並進加速度データを用いて補正することにより、移動物体の任意の運動に対応して正確なクォータニオンの推定値を算出することができる。これにより、移動物体に加速度が生じたときでも、高精度な三次元姿勢角を計測することができ、移動物体の姿勢制御もまた高精度に行うことができる。 In addition, the small posture sensor of the present invention can perform error correction of acceleration sensor data even when acceleration occurs in a moving object. The error correction at this time is to correct an erroneous measurement of the acceleration caused by the mounting position of the acceleration sensor by using an angular velocity sensor, and to differentiate the speed data acquired from the GPS sensor for the erroneous measurement caused by the acceleration generated in the moving object. By correcting using the translational acceleration data acquired in this way, it is possible to calculate an accurate quaternion estimated value corresponding to an arbitrary motion of the moving object. Thereby, even when acceleration occurs in the moving object, it is possible to measure the three-dimensional attitude angle with high accuracy, and to perform the attitude control of the moving object with high accuracy.

以下、本発明を実施するための最良の形態について説明する。 Hereinafter, the best mode for carrying out the present invention will be described.

以下、本発明の第１実施例について説明する。図１は本実施例の小型姿勢センサに備えられる各構成をブロックで示したものである。小型姿勢センサ１００は、移動物体の独立な３軸上の角速度を計測する角速度センサ１０１、移動物体の独立な３軸上の加速度を計測する加速度センサ１０２、移動物体の独立な３軸上の地磁気を計測する地磁気センサ１０３、及び角速度センサ１０１、加速度センサ１０２、及び地磁気センサ１０３の計測値を基にクォータニオンを算出する演算処理部１０４を備えている。本実施例の小型姿勢センサ１００は１入力１出力の線形システムに適用し、定常カルマンフィルタを構成することにより、「カルマンゲイン」や「クォータニオン」の算出を容易にすることができ、現実的な実装を可能にし、さらに、小型化及び実時間処理を可能とする。 The first embodiment of the present invention will be described below. FIG. 1 is a block diagram showing each component provided in the small posture sensor of this embodiment. The small posture sensor 100 includes an angular velocity sensor 101 that measures angular velocities on three independent axes of a moving object, an acceleration sensor 102 that measures acceleration on three independent axes of the moving object, and a geomagnetism on three independent axes of the moving object. And an arithmetic processing unit 104 that calculates a quaternion based on the measured values of the angular velocity sensor 101, the acceleration sensor 102, and the geomagnetic sensor 103. The small posture sensor 100 of the present embodiment is applied to a linear system with one input and one output, and by configuring a stationary Kalman filter, it is possible to easily calculate “Kalman gain” and “quaternion”, which is a practical implementation. In addition, downsizing and real-time processing are possible.

図２は演算処理部１０４で行う処理をブロックで示したものである。演算処理部１０４は、座標変換部２０１、地磁気・加速度センサデータ取得部２０２、誤差算出部２０３、クォータにオン誤差算出部２０４、角速度センサデータ取得部２０５、クォータニオン補正部２０６、クォータニオン推定値算出部２０７を備えている。また、図３は演算処理部１０４で行う処理をフローで示したものである。図４は演算処理部１０４で処理を行う際に用いる数式やデータの流れを示したものである。以下、図２乃至図４を用いて、演算処理部１０４で行う処理について説明する。 FIG. 2 is a block diagram showing processing performed by the arithmetic processing unit 104. The arithmetic processing unit 104 includes a coordinate conversion unit 201, a geomagnetism / acceleration sensor data acquisition unit 202, an error calculation unit 203, a quarter-on error calculation unit 204, an angular velocity sensor data acquisition unit 205, a quaternion correction unit 206, and a quaternion estimated value calculation unit. 207. FIG. 3 is a flowchart showing processing performed by the arithmetic processing unit 104. FIG. 4 shows the flow of mathematical formulas and data used when processing is performed by the arithmetic processing unit 104. Hereinafter, processing performed by the arithmetic processing unit 104 will be described with reference to FIGS. 2 to 4.

クォータニオンの各要素は、Ｓｉｍｐｌｅ Ｒｏｔａｔｉｏｎと呼ばれている姿勢表現方法の（λｘ，λｙ，λｚ，φ）を用いて次の式（１）で定義されている。

Figure 2007183138
Each element of the quaternion is defined by the following equation (1) using (λx, λy, λz, φ) of a posture expression method called “Simple Rotation”.
Figure 2007183138

大まかに説明すると、ｑ0は基準座標系からの姿勢変動量の二分の一の余弦値と関係している。他の３つの値ｑ1，ｑ2，ｑ3は、基準座標系からの姿勢変動量の二分の一の正弦値と、ＸＹＺ軸の各方向への姿勢変動量の割合を掛けたものである。Ｓｉｍｐｌｅ Ｒｏｔａｔｉｏｎは任意軸回りの回転などとも呼ばれ、ある単位軸（λｘ，λｙ，λｚ）まわりに回転角φというように座標を回転させることで姿勢を表現する。ステップ３０１では、３つの虚数と１つの実数からなる４次のベクトル表現を用いた予め設定されるクォータニオンの初期値（式（２））を用いて座標変換行列を算出する（式（３））。座標変換行列は地上固定座標系からセンサ座標系へ変換する行列である。

Figure 2007183138
Figure 2007183138
 In general terms, q 0 is related to a cosine value that is a half of the amount of posture variation from the reference coordinate system. The other three values q 1 , q 2 , and q 3 are obtained by multiplying the sine value of one half of the posture variation amount from the reference coordinate system and the proportion of the posture variation amount in each direction of the XYZ axes. . Simple Rotation is also called rotation around an arbitrary axis, and expresses a posture by rotating coordinates such as a rotation angle φ around a certain unit axis (λx, λy, λz). In step 301, a coordinate transformation matrix is calculated using a preset quaternion initial value (equation (2)) using a fourth-order vector expression composed of three imaginary numbers and one real number (equation (3)). . The coordinate transformation matrix is a matrix for transforming from the ground fixed coordinate system to the sensor coordinate system.
Figure 2007183138
Figure 2007183138

ステップ３０２では、座標変換部２０１は初期姿勢における地磁気、加速度について、ステップ３０１で算出した座標変換行列を用いて座標変換を行う。初期姿勢における地磁気は、地磁気ベクトルの地上固定座標系成分であり、一方、初期姿勢における加速度は、重力加速度ベクトルの地上固定座標系成分である。これらを縦に並べたベクトル式は式（４）のようになる。

Figure 2007183138
In step 302, the coordinate conversion unit 201 performs coordinate conversion on the geomagnetism and acceleration in the initial posture using the coordinate conversion matrix calculated in step 301. The geomagnetism in the initial posture is a ground fixed coordinate system component of the geomagnetic vector, while the acceleration in the initial posture is the ground fixed coordinate system component of the gravitational acceleration vector. A vector expression in which these are arranged vertically is as shown in Expression (4).
Figure 2007183138

そして、これらをステップ３０１で算出した座標変換行列によりセンサ座標系に座標変換すると式（５）のようになる。

Figure 2007183138
Then, when these are coordinate-transformed into the sensor coordinate system by the coordinate transformation matrix calculated in step 301, Equation (5) is obtained.
Figure 2007183138

これにより、ステップ３０３では、予め設定されるクォータニオンの初期値を基に現ステップのセンサ座標系の地磁気、加速度を推定することができ、これらを誤差算出部２０３に入力する。 Thereby, in step 303, the geomagnetism and acceleration of the sensor coordinate system of the current step can be estimated based on the preset initial value of the quaternion, and these are input to the error calculation unit 203.

ステップ３０４では、地磁気・加速度センサデータ取得部２０２は、地磁気センサ１０３及び加速度センサ１０２から現ステップの地磁気ベクトル、重力加速度ベクトルを取得する（式（６））。取得する現ステップの地磁気ベクトル、重力加速度ベクトルは、センサ座標系成分のデータであり、これらを誤差算出部２０３に入力する。

Figure 2007183138
In step 304, the geomagnetism / acceleration sensor data acquisition unit 202 acquires the geomagnetic vector and gravity acceleration vector of the current step from the geomagnetic sensor 103 and the acceleration sensor 102 (formula (6)). The acquired geomagnetic vector and gravitational acceleration vector of the current step are data of sensor coordinate system components, which are input to the error calculation unit 203.
Figure 2007183138

ステップ３０５では、誤差算出部２０３は、ステップ３０３で座標変換部２０１が算出した推定される現ステップの地磁気ベクトル及び重力加速度ベクトルと、ステップ３０４で地磁気・加速度センサデータ取得部２０２から取得した現ステップの地磁気ベクトル及び重力加速度ベクトルとの誤差を算出する（式（７））。

Figure 2007183138
In step 305, the error calculation unit 203 calculates the current step geomagnetism vector and gravitational acceleration vector calculated by the coordinate conversion unit 201 in step 303 and the current step acquired from the geomagnetism / acceleration sensor data acquisition unit 202 in step 304. An error between the geomagnetic vector and the gravitational acceleration vector is calculated (formula (7)).
Figure 2007183138

つづいてステップ３０６では、クォータニオン誤差算出部２０４は、ステップ３０５で算出した誤差について、ヤコビ行列（式（８））を用いてクォータニオンレベルの誤差を算出し（式（９））、これをクォータニオン補正部２０５に入力する。

Figure 2007183138
Figure 2007183138
 Subsequently, in step 306, the quaternion error calculation unit 204 calculates an error of the quaternion level using the Jacobian matrix (equation (8)) for the error calculated in step 305 (equation (9)), and this is quaternion corrected. Input to the unit 205.
Figure 2007183138
Figure 2007183138

ステップ３０７では、角速度センサデータ取得部２０５は、角速度センサ１０１から現ステップの角速度を取得する。取得する現ステップの角速度は、センサ座標系成分のデータであり、これをクォータニオン補正部２０６に入力する。 In step 307, the angular velocity sensor data acquisition unit 205 acquires the angular velocity of the current step from the angular velocity sensor 101. The angular velocity of the current step to be acquired is sensor coordinate system component data, which is input to the quaternion correction unit 206.

ステップ３０８では、クォータニオン補正部２０６は、ステップ３０６でクォータニオン誤差算出部２０４が算出した現ステップのクォータニオンレベルの誤差、ステップ３０７で角速度センサデータ取得部２０５から取得した現ステップの角速度ベクトル、ならびに予め設定されるクォータニオンの初期値を用いて、式（１０）のような定常カルマンフィルタを構成し、現ステップのクォータニオン補正を行う。

Figure 2007183138
In step 308, the quaternion correction unit 206 determines the quaternion level error of the current step calculated by the quaternion error calculation unit 204 in step 306, the angular velocity vector of the current step acquired from the angular velocity sensor data acquisition unit 205 in step 307, and the preset value. Using the initial value of the quaternion to be formed, a stationary Kalman filter as shown in Expression (10) is configured, and the quaternion correction at the current step is performed.
Figure 2007183138

最後に、ステップ３０９では、上記のように補正を行ったクォータニオンを積分することにより、次ステップのクォータニオン推定値を算出し、これをクォータニオンにより表現する三次元姿勢角とし、姿勢センサ１００の出力とする（ＥＮＤ）。また、算出された次ステップのクォータニオン推定値は、ステップ３０１やステップ３０８に戻って、座標変換部２０１やクォータニオン補正部２０６に入力される。すなわち、座標変換部２０１では、算出された次ステップのクォータニオン推定値を用いて次ステップの座標変換行列を算出し、クォータニオン補正部２０６では、算出された次ステップのクォータニオン推定値を用いて次ステップのクォータニオン補正に用いる。上記ステップ３０１乃至ステップ３０９の処理が、小型姿勢センサの演算処理部１０４で行う一ステップの処理であり、これを繰り返し行う。 Finally, in step 309, by integrating the quaternion corrected as described above, a quaternion estimated value in the next step is calculated, and this is set as a three-dimensional posture angle expressed by the quaternion. (END). Further, the calculated quaternion estimated value of the next step returns to step 301 or 308 and is input to the coordinate conversion unit 201 or quaternion correction unit 206. That is, the coordinate conversion unit 201 calculates a coordinate conversion matrix of the next step using the calculated quaternion estimated value of the next step, and the quaternion correction unit 206 uses the calculated quaternion estimated value of the next step to perform the next step. Used for quaternion correction. The processing from step 301 to step 309 is one step performed by the arithmetic processing unit 104 of the small posture sensor, and this is repeated.

本実施例の小型姿勢センサを移動物体に搭載し、クォータニオンにより表現する三次元姿勢角を計測した実験データを図５に示す。このデータは本発明の小型姿勢センサにより計測される三次元姿勢角の有効性を検証するデータとなっている。図５に示すのは本発明の小型姿勢センサを小型無人ヘリコプタに搭載し、小型無人ヘリコプタがホバリング飛行状態にあるときの三次元姿勢角を示しており、ほぼ零を示している。このほか小型無人ヘリコプタがロール方向のアクロバット飛行状態にあるときの三次元姿勢角も計測できる。このように、本発明の小型姿勢センサを小型無人ヘリコプタに搭載し、クォータニオンにより表現する三次元姿勢角を計測することにより、小型無人ヘリコプタの自律飛行制御を実現している。演算処理部で行う補正処理を速やかに行い、高精度な実時間処理を行うことにより、移動物体の姿勢制御もまた高精度に行うことができる。 FIG. 5 shows experimental data obtained by mounting the small posture sensor of this embodiment on a moving object and measuring a three-dimensional posture angle expressed by quaternions. This data is data for verifying the effectiveness of the three-dimensional posture angle measured by the small posture sensor of the present invention. FIG. 5 shows a three-dimensional attitude angle when the small unmanned helicopter of the present invention is mounted on a small unmanned helicopter and the small unmanned helicopter is in a hovering flight state, which is substantially zero. In addition, the three-dimensional attitude angle when the small unmanned helicopter is in the aerobatics state in the roll direction can also be measured. As described above, the small attitude sensor of the present invention is mounted on a small unmanned helicopter, and the autonomous flight control of the small unmanned helicopter is realized by measuring the three-dimensional attitude angle expressed by the quaternion. By quickly performing the correction processing performed by the arithmetic processing unit and performing high-accuracy real-time processing, the posture control of the moving object can also be performed with high accuracy.

以上、本実施例の小型姿勢センサ１００は、１入力１出力の線形システムに適用し、定常カルマンフィルタを構成することにより、「カルマンゲイン」や「クォータニオン」の取得を容易にすることができ、現実的な実装を可能にし、さらに、小型化及び実時間処理を可能とする。 As described above, the small posture sensor 100 according to the present embodiment can be applied to a linear system with one input and one output and constitute a stationary Kalman filter, thereby facilitating acquisition of “Kalman gain” and “quaternion”. Realization, and further miniaturization and real-time processing.

なお、本実施例の小型姿勢センサは、推定される次ステップのクォータニオンを出力するものであるが、計測周期を短く設定することにより、システムへの影響は極めて小さいものと考えられる。 Note that the small posture sensor of this embodiment outputs the estimated quaternion of the next step, but it is considered that the influence on the system is extremely small by setting the measurement cycle short.

以下、本発明の第２実施例について説明する。実施例１で用いられている加速度センサ１０２は、重力加速度ベクトルのセンサ座標系成分のデータを取得するものである。そのため、加速度センサ１０２は重力加速度ベクトルのみを取得することが望ましいが、図６に示すような移動物体に加速度ａが生じた場合には、加速度センサ１０２は実際の重力加速度ベクトルｇと加速度ａを合成した重力加速度ベクトルｇ’を検知してしまう。このような加速度センサ１０２の誤測によって生じる誤差は最終的に小型姿勢センサより出力される三次元姿勢角にも少なからず影響を及ぼすので、補正を行う必要がある。 The second embodiment of the present invention will be described below. The acceleration sensor 102 used in the first embodiment acquires sensor coordinate system component data of a gravitational acceleration vector. For this reason, it is desirable for the acceleration sensor 102 to acquire only the gravitational acceleration vector. However, when the acceleration a occurs in the moving object as shown in FIG. 6, the acceleration sensor 102 calculates the actual gravitational acceleration vector g and the acceleration a. The synthesized gravitational acceleration vector g ′ is detected. An error caused by such mismeasurement of the acceleration sensor 102 has a considerable influence on the three-dimensional posture angle finally output from the small posture sensor, and thus correction is necessary.

図７は小型姿勢センサに備えられる各構成をブロックで示したものである。基本的な構成については実施例１と同様であるが、本実施例では、移動物体に加速度ａが生じることによって起こる加速度センサ１０２の誤測を、ＧＰＳセンサ７０１及び角速度センサ１０１を用いて補正を行う。 FIG. 7 is a block diagram showing each component provided in the small posture sensor. Although the basic configuration is the same as that of the first embodiment, in this embodiment, the erroneous measurement of the acceleration sensor 102 caused by the acceleration a generated in the moving object is corrected using the GPS sensor 701 and the angular velocity sensor 101. Do.

図８は演算処理部１０４で行う処理をブロックで示したものである。演算処理部１０４は、実施例１で示した構成の他、センサ座標系加速度補正部８０１及び地上固定座標系加速度補正部８０２を備えている。センサ座標系加速度補正部８０１は、加速度センサ１０２から取得するセンサ座標系成分の重力加速度ベクトルについて、角速度センサ１０１から取得する角速度ベクトルを用いて補正を行う。また、地上固定座標系加速度補正部８０２は、初期姿勢における重力加速度ベクトル、すなわち、地上固定座標系成分の重力加速度ベクトルについてＧＰＳセンサ７０１から取得する重力加速度ベクトルを用いて補正を行う。図９は演算処理部１０４で処理を行う際に用いる数式やデータの流れを示したものである。以下、図８及び図９を用いて、演算処理部１０４で行う、加速度センサ１０２の誤測をＧＰＳセンサ７０１及び角速度センサ１０１を用いて補正を行う処理について説明する。 FIG. 8 is a block diagram showing processing performed by the arithmetic processing unit 104. The arithmetic processing unit 104 includes a sensor coordinate system acceleration correction unit 801 and a ground fixed coordinate system acceleration correction unit 802 in addition to the configuration shown in the first embodiment. The sensor coordinate system acceleration correction unit 801 corrects the gravitational acceleration vector of the sensor coordinate system component acquired from the acceleration sensor 102 using the angular velocity vector acquired from the angular velocity sensor 101. The ground fixed coordinate system acceleration correction unit 802 corrects the gravity acceleration vector in the initial posture, that is, the gravity acceleration vector acquired from the GPS sensor 701 with respect to the gravity acceleration vector of the ground fixed coordinate system component. FIG. 9 shows the flow of mathematical formulas and data used when processing is performed by the arithmetic processing unit 104. Hereinafter, processing for correcting erroneous measurement of the acceleration sensor 102 using the GPS sensor 701 and the angular velocity sensor 101, which is performed by the arithmetic processing unit 104, will be described with reference to FIGS.

はじめに、ＧＰＳセンサ７０１と小型姿勢センサ７００の取り付け位置の違いによる誤差を補正する。ＧＰＳセンサ７０１から取得するのは移動物体の並進加速度であるが、小型姿勢センサ７００は移動物体の回転中心からある程度ずれた位置に取り付けられているので小型姿勢センサ７００内の加速度センサ１０２は並進加速度にくわえて移動物体の回転運動による向心加速度も取得してしまう。 First, an error due to a difference in the attachment position of the GPS sensor 701 and the small posture sensor 700 is corrected. What is acquired from the GPS sensor 701 is the translational acceleration of the moving object, but since the small posture sensor 700 is attached to a position deviated to some extent from the rotation center of the moving object, the acceleration sensor 102 in the small posture sensor 700 is translated acceleration. In addition, the centripetal acceleration due to the rotational motion of the moving object is also acquired.

そこで、角速度センサ１０１から取得する角速度センサデータ（式（１２））を基にＧＰＳセンサ７０１の速度データを微分することにより取得する並進加速度データと加速度センサ１０２から取得する加速度の差を補正する。そのために、式（１１）に示すような位置、速度、加速度レベルそれぞれでのＧＰＳ加速度と加速度センサ１０２の加速度の関係式を考える。

Figure 2007183138
Figure 2007183138
 Therefore, the difference between the translation acceleration data acquired by differentiating the velocity data of the GPS sensor 701 and the acceleration acquired from the acceleration sensor 102 is corrected based on the angular velocity sensor data (formula (12)) acquired from the angular velocity sensor 101. For this purpose, a relational expression between GPS acceleration and acceleration of the acceleration sensor 102 at each position, velocity, and acceleration level as shown in Expression (11) is considered.
Figure 2007183138
Figure 2007183138

つづいて、ＧＰＳセンサ７０１の速度データを微分することにより取得する並進加速度データを用いて、重力加速度ベクトルの地上固定座標系成分を式１３のように補正することができる。

Figure 2007183138
Subsequently, using the translational acceleration data obtained by differentiating the velocity data of the GPS sensor 701, the ground fixed coordinate system component of the gravitational acceleration vector can be corrected as shown in Equation 13.
Figure 2007183138

この式に、式（１１）の３番目の式を代入し、さらにこの式をクォータニオン推定アルゴリズム中で加速度センサが検知する重力ベクトルと重力ベクトルの地上固定座標成分を座標変換したものの差をとる部分に代入すると以下の式のようになる。結局、式（１４）で表されるような演算処理を加えることによって加速度センサの誤差を補正することができる。その後、座標変換行列を用いて現ステップの加速度の誤差を取得する。以後のステップについては、上記実施例１と同様である。

Figure 2007183138
Substituting the third equation of equation (11) into this equation, and further subtracting the difference between the gravity vector detected by the acceleration sensor in the quaternion estimation algorithm and the coordinate transformation of the ground fixed coordinate component of the gravity vector Substituting into, the following formula is obtained. Eventually, the error of the acceleration sensor can be corrected by adding a calculation process represented by Expression (14). Thereafter, the acceleration error of the current step is obtained using the coordinate transformation matrix. The subsequent steps are the same as in the first embodiment.
Figure 2007183138

以上、本実施例の小型姿勢センサ７００は移動物体に加速度が生じたときでも加速度センサデータの誤差補正を行うことができる。この時の誤差補正は、加速度センサの取り付け位置によって生じる加速度の誤測について角速度センサを用いて補正し、また、移動物体に生じる加速度に伴う誤測についてＧＰＳセンサを用いて補正することにより、移動物体の任意の運動に対応して正確なクォータニオンの推定値を取得することができる。これにより、移動物体に加速度が生じたときでも、高精度な三次元姿勢角を計測することができ、移動物体の姿勢制御もまた高精度に行うことができる。 As described above, the small posture sensor 700 according to the present embodiment can correct the error of the acceleration sensor data even when the acceleration is generated in the moving object. The error correction at this time is corrected by using the angular velocity sensor to correct the erroneous measurement of the acceleration caused by the mounting position of the acceleration sensor, and by correcting the erroneous measurement due to the acceleration generated in the moving object using the GPS sensor. An accurate quaternion estimate can be obtained corresponding to any movement of the object. Thereby, even when acceleration occurs in the moving object, it is possible to measure the three-dimensional attitude angle with high accuracy, and to perform the attitude control of the moving object with high accuracy.

本発明の小型姿勢センサは、小型化を実現したことにより、大小を問わない陸海空の様々な移動物体に搭載することができ、汎用性がある。例えば、自律飛行制御を可能とする小型無人ヘリコプタや人工衛星など産業上の利用可能性は高いものである。 The small-sized attitude sensor of the present invention can be mounted on various moving objects in land, sea, and air, regardless of the size, by realizing downsizing, and is versatile. For example, industrial applicability such as small unmanned helicopters and artificial satellites that enable autonomous flight control is high.

本発明の一実施例で用いる小型姿勢センサに備えられる各構成をブロックで示した図（実施例１）The figure which showed each structure with which the small attitude | position sensor used in one Example of this invention was equipped with the block (Example 1). 本発明の一実施例で用いる小型姿勢センサの演算処理部で行う処理をブロックで示した図（実施例１）The figure which showed the process performed with the arithmetic processing part of the small posture sensor used in one Example of this invention with the block (Example 1). 本発明の一実施例で用いる小型姿勢センサの演算処理部で行う処理をフローで示した図（実施例１）The figure which showed the process performed by the arithmetic processing part of the small posture sensor used in one Example of this invention with the flow (Example 1). 本発明の一実施例で用いる小型姿勢センサの演算処理部で処理を行う際に用いる数式やデータの流れを示した図（実施例１）The figure which showed the flow of the numerical formula and data which are used when processing by the arithmetic processing unit of the small posture sensor used in one embodiment of the present invention (Example 1) 小型姿勢センサを移動物体に搭載し、クォータニオンにより表現する三次元姿勢角を計測した実験データを示した図（実施例１）Fig. 1 shows experimental data of a small posture sensor mounted on a moving object and measuring three-dimensional posture angles expressed by quaternions (Example 1). 移動物体に加速度が生じた場合、加速度センサが取得する重力加速度ベクトルに誤差が生じることについて説明する図（実施例２）FIG. 6 is a diagram for explaining that an error occurs in a gravitational acceleration vector acquired by an acceleration sensor when acceleration occurs in a moving object (Example 2). 本発明の一実施例で用いる小型姿勢センサに備えられる各構成をブロックで示した図（実施例２）The figure which showed each structure with which the small attitude | position sensor used by one Example of this invention was equipped with the block (Example 2). 本発明の一実施例で用いる小型姿勢センサの演算処理部で行う処理をブロックで示した図（実施例２）The figure which showed the process performed with the arithmetic processing part of the small attitude | position sensor used in one Example of this invention with the block (Example 2). 本発明の一実施例で用いる小型姿勢センサの演算処理部で処理を行う際に用いる数式やデータの流れを示した図（実施例２）The figure which showed the flow of the numerical formula and data which are used when processing by the arithmetic processing unit of the small posture sensor used in one embodiment of the present invention (second embodiment) 特許文献１に開示されている姿勢センサに備えられる各構成をブロックで示した図（背景技術）The figure which showed each structure with which the attitude | position sensor currently disclosed by patent document 1 was equipped with the block (background art)

符号の説明Explanation of symbols

１００，７００ 小型姿勢センサ
１０１ 角速度センサ
１０２ 加速度センサ
１０３ 地磁気センサ
１０４ 演算処理部
２０１ 座標変換部
２０２ 地磁気・加速度センサデータ取得部
２０３ 誤差取得部
２０４ クォータニオン誤差取得部
２０５ 角速度センサデータ取得部
２０６ クォータニオン補正部
２０７ クォータニオン推定値取得部
７０１ ＧＰＳセンサ
８０１ センサ座標系加速度補正部
８０２ 地上固定座標系加速度補正部 100, 700 Small posture sensor 101 Angular velocity sensor 102 Acceleration sensor 103 Geomagnetic sensor 104 Arithmetic processing unit 201 Coordinate conversion unit 202 Geomagnetic / acceleration sensor data acquisition unit 203 Error acquisition unit 204 Quaternion error acquisition unit 205 Angular velocity sensor data acquisition unit 206 Quaternion correction unit 207 Quaternion estimated value acquisition unit 701 GPS sensor 801 Sensor coordinate system acceleration correction unit 802 Ground fixed coordinate system acceleration correction unit

Claims (6)

移動物体の独立な３軸上の角速度を計測する角速度センサと、
移動物体の独立な３軸上の加速度を計測する加速度センサと、
移動物体の独立な３軸上の地磁気を計測する地磁気センサと、
前記角速度センサ、前記加速度センサ、及び前記地磁気センサの計測値を基にクォータニオンの推定値を算出する演算処理部とを備え、
前記演算処理部は、予め推定されたクォータニオン推定値を用いて座標変換行列を算出し、当該座標変換行列を用いて地上固定座標系成分の地磁気ベクトル及び重力加速度ベクトルを変換し、現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルを推定する座標変換部と、
前記地磁気センサ及び前記加速度センサから現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルを取得する地磁気・加速度センサデータ取得部と、
前記座標変換部で推定した現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルと、前記地磁気・加速度センサデータ取得部で取得した現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルとの誤差を算出する誤差算出部と、
前記誤差算出部で算出した誤差について、ヤコビ行列を用いてクォータニオンレベルの誤差を算出するクォータニオン誤差算出部と、
前記角速度センサから現ステップのセンサ座標系成分の角速度ベクトルを取得する角速度センサデータ取得部と、
前記クォータニオン誤差算出部が算出した現ステップのクォータニオンレベルの誤差、前記角速度センサデータ取得部から取得した現ステップの角速度ベクトル、ならびに前記予め推定されたクォータニオン推定値を用いて定常カルマンフィルタを構成し、現ステップのクォータニオン補正を行うクォータニオン補正部と、
前記クォータニオン補正部で補正を行ったクォータニオンを積分することにより次ステップのクォータニオン推定値を算出するクォータニオン推定部とを有し、
前記クォータニオン推定値を、クォータニオンにより表現する三次元姿勢角として出力することを特徴とする小型姿勢センサ。 An angular velocity sensor that measures angular velocities on three independent axes of the moving object;
An acceleration sensor that measures acceleration on three independent axes of the moving object;
A geomagnetic sensor for measuring geomagnetism on three independent axes of a moving object;
An arithmetic processing unit that calculates an estimated value of a quaternion based on measurement values of the angular velocity sensor, the acceleration sensor, and the geomagnetic sensor,
The arithmetic processing unit calculates a coordinate transformation matrix using a pre-estimated quaternion estimated value, converts a geomagnetic vector and a gravitational acceleration vector of a ground fixed coordinate system component using the coordinate transformation matrix, A coordinate conversion unit for estimating the geomagnetic vector and the gravitational acceleration vector of the coordinate system component;
A geomagnetism / acceleration sensor data acquisition unit for acquiring a geomagnetic vector and a gravitational acceleration vector of a sensor coordinate system component of the current step from the geomagnetic sensor and the acceleration sensor;
The geomagnetic vector and gravity acceleration vector of the sensor coordinate system component of the current step estimated by the coordinate conversion unit, and the geomagnetic vector and gravity acceleration vector of the sensor coordinate system component of the current step acquired by the geomagnetic / acceleration sensor data acquisition unit. An error calculation unit for calculating an error;
For the error calculated by the error calculation unit, a quaternion error calculation unit that calculates a quaternion level error using a Jacobian matrix;
An angular velocity sensor data acquisition unit that acquires an angular velocity vector of a sensor coordinate system component of the current step from the angular velocity sensor;
A stationary Kalman filter is configured using the quaternion level error of the current step calculated by the quaternion error calculator, the angular velocity vector of the current step acquired from the angular velocity sensor data acquisition unit, and the quaternion estimated value estimated in advance. A quaternion correction unit that performs quaternion correction of the step;
A quaternion estimating unit that calculates a quaternion estimated value of the next step by integrating the quaternion corrected by the quaternion correcting unit,
A small attitude sensor that outputs the estimated quaternion value as a three-dimensional attitude angle expressed by quaternions. 前記算出された次ステップのクォータニオン推定値は、前記座標変換部あるいは前記クォータニオン補正部に入力されることを特徴とする請求項１に記載の小型姿勢センサ。 The small posture sensor according to claim 1, wherein the calculated quaternion estimated value of the next step is input to the coordinate conversion unit or the quaternion correction unit. 前記定常カルマンフィルタは、
Figure 2007183138
 上記の式で演算処理することを特徴とする請求項１に記載の小型姿勢センサ。 The stationary Kalman filter is
Figure 2007183138
 The small posture sensor according to claim 1, wherein the arithmetic processing is performed by the above formula. 前記演算処理部は、前記角速度センサを用いて前記加速度センサの取り付け位置によって生じる加速度の誤測を補正し、移動物体の速度データを検知するＧＰＳセンサを用いて当該速度データを微分することにより取得する移動物体の並進加速度データを用いて、移動物体に生じる加速度に伴う誤測を補正することを特徴とする請求項１に記載の小型姿勢センサ。 The arithmetic processing unit is obtained by correcting an erroneous measurement of acceleration caused by the mounting position of the acceleration sensor using the angular velocity sensor and differentiating the velocity data using a GPS sensor that detects velocity data of a moving object. The small posture sensor according to claim 1, wherein erroneous measurement associated with acceleration generated in the moving object is corrected using translational acceleration data of the moving object. 前記演算処理部は、角速度センサから取得する角速度ベクトルを用いて前記加速度センサから取得するセンサ座標系成分の重力加速度ベクトルの補正を行うセンサ座標系加速度補正部と、移動物体の速度データを検知するＧＰＳセンサを用いて当該速度データを微分することにより取得する移動物体の並進加速度データを用いて地上固定座標系成分の重力加速度ベクトルの補正を行う地上固定座標系加速度補正部とを有し、移動物体に生じる加速度に伴う誤測を補正することを特徴とする請求項１に記載の小型姿勢センサ。 The arithmetic processing unit detects a velocity data of a moving object and a sensor coordinate system acceleration correction unit that corrects a gravitational acceleration vector of a sensor coordinate system component acquired from the acceleration sensor using an angular velocity vector acquired from an angular velocity sensor. A ground fixed coordinate system acceleration correction unit that corrects the gravitational acceleration vector of the ground fixed coordinate system component using the translational acceleration data of the moving object obtained by differentiating the velocity data using a GPS sensor, and moves The small posture sensor according to claim 1, wherein a mismeasurement associated with an acceleration generated in an object is corrected. 前記角速度センサ及び前記ＧＰＳセンサを用いて行う補正は、
Figure 2007183138
 上記の式で演算処理することを特徴とする請求項４または５に記載の小型姿勢センサ。 Correction performed using the angular velocity sensor and the GPS sensor is as follows:
Figure 2007183138
 The small posture sensor according to claim 4 or 5, wherein the arithmetic processing is performed by the above formula.
JP2006000850A 2006-01-05 2006-01-05 Small attitude sensor Active JP4876204B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2006000850A JP4876204B2 (en) 2006-01-05 2006-01-05 Small attitude sensor

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2006000850A JP4876204B2 (en) 2006-01-05 2006-01-05 Small attitude sensor

Publications (2)

Publication Number Publication Date
JP2007183138A true JP2007183138A (en) 2007-07-19
JP4876204B2 JP4876204B2 (en) 2012-02-15

Family

ID=38339370

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2006000850A Active JP4876204B2 (en) 2006-01-05 2006-01-05 Small attitude sensor

Country Status (1)

Country Link
JP (1) JP4876204B2 (en)

Cited By (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007276507A (en) * 2006-04-03 2007-10-25 Univ Of Tokyo Mobile element controller and mobile element control method
JP2009276307A (en) * 2008-05-19 2009-11-26 Shimadzu Corp Magnetic measurement device
KR20110033282A (en) * 2008-07-18 2011-03-30 모베아 에스.아 Method for the improved estimation of an object orientation and attitude control unit implementing said method
JP2011230213A (en) * 2010-04-26 2011-11-17 Honda Motor Co Ltd Robot, control system, and control program
CN102901977A (en) * 2012-10-24 2013-01-30 北京航天自动控制研究所 Method for determining initial attitude angle of aircraft
JP2013054009A (en) * 2011-09-06 2013-03-21 Toyota Motor Corp Posture estimation device, posture estimation method and posture estimation program
CN103090870A (en) * 2013-01-21 2013-05-08 西北工业大学 Spacecraft attitude measurement method based on MEMS (micro-electromechanical systems) sensor
US9043177B2 (en) 2010-03-05 2015-05-26 Seiko Epson Corporation Posture information calculation device, posture information calculation system, posture information calculation method, and information storage medium
JP2015169582A (en) * 2014-03-07 2015-09-28 リコーエレメックス株式会社 Attitude detector
JP2017227591A (en) * 2016-06-24 2017-12-28 セイコーエプソン株式会社 Signal processing circuit, physical quantity detection device, attitude arithmetic unit, electronic apparatus, and movable body
JP2019120587A (en) * 2018-01-05 2019-07-22 ローム株式会社 Positioning system and positioning method
US10852846B2 (en) 2010-01-06 2020-12-01 Cm Hk Limited Electronic device for use in motion detection and method for obtaining resultant deviation thereof
JP2021076901A (en) * 2019-11-05 2021-05-20 本田技研工業株式会社 Posture estimation device for moving body
CN113038821A (en) * 2018-10-30 2021-06-25 东京计器株式会社 Error correction device

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102679976B (en) * 2012-05-20 2014-07-09 西安费斯达自动化工程有限公司 Aircraft quaternion revising model based on accelerated speed
CN104535065B (en) * 2014-12-23 2017-09-01 广州快飞计算机科技有限公司 Course generation method of the unmanned plane during flying device under complicated magnetic field environment
JP6474128B2 (en) * 2015-03-18 2019-02-27 アルプスアルパイン株式会社 Electronic equipment with geomagnetic and acceleration sensors
CN108508905B (en) * 2018-04-08 2020-12-25 上海微小卫星工程中心 Attitude maneuver control and guidance law planning method based on shortest space axis

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0518753A (en) * 1991-07-16 1993-01-26 Tokimec Inc Strap-down type gyro device
JPH07234722A (en) * 1994-02-24 1995-09-05 Yamaha Motor Co Ltd Controller for attitude of flight vehicle
JPH0821740A (en) * 1994-07-07 1996-01-23 Yokogawa Denshi Kiki Kk Inertial navigation apparatus for ship
JPH095104A (en) * 1995-06-23 1997-01-10 Nippon Telegr & Teleph Corp <Ntt> Method and apparatus for measurement of three-dimensional attitude angle of moving body
JPH1048305A (en) * 1996-08-07 1998-02-20 Yuseisho Tsushin Sogo Kenkyusho Tracking apparatus for communication satellite for mobile body communication
JP2002160699A (en) * 2000-09-28 2002-06-04 Space Syst Loral Inc Method and device for velocity integralation supplement for attitude collating quanternion differentiation
JP2003312597A (en) * 2002-04-24 2003-11-06 Daicel Chem Ind Ltd Attitude control simulator of flying object
JP2004093562A (en) * 2002-08-06 2004-03-25 Chan Kook Park Attitude error compensating device and method of terrestrial magnetic field sensor
JP2004256022A (en) * 2003-02-26 2004-09-16 Kenzo Nonami Autonomous control method for small size unmanned helicopter

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0518753A (en) * 1991-07-16 1993-01-26 Tokimec Inc Strap-down type gyro device
JPH07234722A (en) * 1994-02-24 1995-09-05 Yamaha Motor Co Ltd Controller for attitude of flight vehicle
JPH0821740A (en) * 1994-07-07 1996-01-23 Yokogawa Denshi Kiki Kk Inertial navigation apparatus for ship
JPH095104A (en) * 1995-06-23 1997-01-10 Nippon Telegr & Teleph Corp <Ntt> Method and apparatus for measurement of three-dimensional attitude angle of moving body
JPH1048305A (en) * 1996-08-07 1998-02-20 Yuseisho Tsushin Sogo Kenkyusho Tracking apparatus for communication satellite for mobile body communication
JP2002160699A (en) * 2000-09-28 2002-06-04 Space Syst Loral Inc Method and device for velocity integralation supplement for attitude collating quanternion differentiation
JP2003312597A (en) * 2002-04-24 2003-11-06 Daicel Chem Ind Ltd Attitude control simulator of flying object
JP2004093562A (en) * 2002-08-06 2004-03-25 Chan Kook Park Attitude error compensating device and method of terrestrial magnetic field sensor
JP2004256022A (en) * 2003-02-26 2004-09-16 Kenzo Nonami Autonomous control method for small size unmanned helicopter

Cited By (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007276507A (en) * 2006-04-03 2007-10-25 Univ Of Tokyo Mobile element controller and mobile element control method
JP2009276307A (en) * 2008-05-19 2009-11-26 Shimadzu Corp Magnetic measurement device
KR20110033282A (en) * 2008-07-18 2011-03-30 모베아 에스.아 Method for the improved estimation of an object orientation and attitude control unit implementing said method
JP2011528432A (en) * 2008-07-18 2011-11-17 モベア エス．アー Method for estimating the direction of an object more accurately and an attitude control device implementing the method
KR101693898B1 (en) * 2008-07-18 2017-01-09 모베아 Method for the improved estimation of an object orientation and attitude control system implementing said method
US11698687B2 (en) 2010-01-06 2023-07-11 Cm Hk Limited Electronic device for use in motion detection and method for obtaining resultant deviation thereof
US10852846B2 (en) 2010-01-06 2020-12-01 Cm Hk Limited Electronic device for use in motion detection and method for obtaining resultant deviation thereof
US9043177B2 (en) 2010-03-05 2015-05-26 Seiko Epson Corporation Posture information calculation device, posture information calculation system, posture information calculation method, and information storage medium
JP2011230213A (en) * 2010-04-26 2011-11-17 Honda Motor Co Ltd Robot, control system, and control program
JP2013054009A (en) * 2011-09-06 2013-03-21 Toyota Motor Corp Posture estimation device, posture estimation method and posture estimation program
CN102901977A (en) * 2012-10-24 2013-01-30 北京航天自动控制研究所 Method for determining initial attitude angle of aircraft
CN103090870A (en) * 2013-01-21 2013-05-08 西北工业大学 Spacecraft attitude measurement method based on MEMS (micro-electromechanical systems) sensor
JP2015169582A (en) * 2014-03-07 2015-09-28 リコーエレメックス株式会社 Attitude detector
JP2017227591A (en) * 2016-06-24 2017-12-28 セイコーエプソン株式会社 Signal processing circuit, physical quantity detection device, attitude arithmetic unit, electronic apparatus, and movable body
JP2019120587A (en) * 2018-01-05 2019-07-22 ローム株式会社 Positioning system and positioning method
JP7025215B2 (en) 2018-01-05 2022-02-24 ローム株式会社 Positioning system and positioning method
CN113038821A (en) * 2018-10-30 2021-06-25 东京计器株式会社 Error correction device
JP2021076901A (en) * 2019-11-05 2021-05-20 本田技研工業株式会社 Posture estimation device for moving body
JP7391616B2 (en) 2019-11-05 2023-12-05 本田技研工業株式会社 Mobile body pose estimation device

Also Published As

Publication number Publication date
JP4876204B2 (en) 2012-02-15

Similar Documents

Publication Publication Date Title
JP4876204B2 (en) Small attitude sensor
CN108036785A (en) A kind of aircraft position and orientation estimation method based on direct method and inertial navigation fusion
CN109550219B (en) Method and system for determining motion information and mobile device
KR100898169B1 (en) Initial alignment method of inertial navigation system
JP2012173190A (en) Positioning system and positioning method
JP2013200162A (en) Compact attitude sensor
JP2011503571A (en) Object orientation measurement
JP7025215B2 (en) Positioning system and positioning method
CN106709222B (en) IMU drift compensation method based on monocular vision
JP2007232443A (en) Inertia navigation system and its error correction method
WO2021218731A1 (en) Method and apparatus for position-attitude fusion of imu and rigid body, device, and storage medium
WO2018191957A1 (en) Camera mount attitude estimation method and device, and corresponding camera mount
CN108827288A (en) A kind of dimensionality reduction strapdown inertial navigation system Initial Alignment Method and system based on dual quaterion
Hoang et al. Measurement optimization for orientation tracking based on no motion no integration technique
JP2007232444A (en) Inertia navigation system and its error correction method
JP5511088B2 (en) Portable device, program and method for correcting gravity vector used for autonomous positioning
JP2014240266A (en) Sensor drift amount estimation device and program
JP2009186244A (en) Tilt angle estimation system, relative angle estimation system, and angular velocity estimation system
CN109674480B (en) Human motion attitude calculation method based on improved complementary filtering
CN107014386A (en) The disturbing acceleration measuring method that a kind of attitude of flight vehicle is resolved
CN114543786B (en) Wall climbing robot positioning method based on visual inertial odometer
JP2006038650A (en) Posture measuring method, posture controller, azimuth meter and computer program
CN106441282B (en) A kind of star sensor star tracking method
MacArthur et al. Unmanned ground vehicle state estimation using an unmanned air vehicle
JP5424224B2 (en) Relative angle estimation system

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20080715

A711 Notification of change in applicant

Free format text: JAPANESE INTERMEDIATE CODE: A711

Effective date: 20090722

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A821

Effective date: 20090722

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20090909

RD03 Notification of appointment of power of attorney

Free format text: JAPANESE INTERMEDIATE CODE: A7423

Effective date: 20100122

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A821

Effective date: 20100122

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20101215

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: 20101221

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20110120

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20110120

A072 Dismissal of procedure [no reply to invitation to correct request for examination]

Free format text: JAPANESE INTERMEDIATE CODE: A073

Effective date: 20110927

R150 Certificate of patent or registration of utility model

Ref document number: 4876204

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150

Free format text: JAPANESE INTERMEDIATE CODE: R150

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20141209

Year of fee payment: 3

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

S111 Request for change of ownership or part of ownership

Free format text: JAPANESE INTERMEDIATE CODE: R313117

R350 Written notification of registration of transfer

Free format text: JAPANESE INTERMEDIATE CODE: R350

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

S111 Request for change of ownership or part of ownership

Free format text: JAPANESE INTERMEDIATE CODE: R313113

R350 Written notification of registration of transfer

Free format text: JAPANESE INTERMEDIATE CODE: R350

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250