JP6282570B2 - Zero point deviation correction system, zero point deviation correction apparatus, and zero point deviation correction method for correcting zero point deviation of inertial sensor - Google Patents

Zero point deviation correction system, zero point deviation correction apparatus, and zero point deviation correction method for correcting zero point deviation of inertial sensor Download PDF

Info

Publication number
JP6282570B2
JP6282570B2 JP2014217593A JP2014217593A JP6282570B2 JP 6282570 B2 JP6282570 B2 JP 6282570B2 JP 2014217593 A JP2014217593 A JP 2014217593A JP 2014217593 A JP2014217593 A JP 2014217593A JP 6282570 B2 JP6282570 B2 JP 6282570B2
Authority
JP
Japan
Prior art keywords
zero point
point deviation
sensor
inertial sensor
kinetic energy
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2014217593A
Other languages
Japanese (ja)
Other versions
JP2016085104A (en
Inventor
雅人 因藤
雅人 因藤
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.)
Sumitomo Heavy Industries Ltd
Original Assignee
Sumitomo Heavy Industries 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 Sumitomo Heavy Industries Ltd filed Critical Sumitomo Heavy Industries Ltd
Priority to JP2014217593A priority Critical patent/JP6282570B2/en
Publication of JP2016085104A publication Critical patent/JP2016085104A/en
Application granted granted Critical
Publication of JP6282570B2 publication Critical patent/JP6282570B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Gyroscopes (AREA)

Description

本発明は、慣性センサの零点ずれを補正する零点ずれ補正システム、零点ずれ補正装置、及び零点ずれ補正方法に関する。   The present invention relates to a zero point deviation correction system, a zero point deviation correction device, and a zero point deviation correction method for correcting a zero point deviation of an inertial sensor.

加速度センサがある監視時間に亘って出力するデータの特徴量と閾値との比較に基づきその加速度センサが取り付けられる移動体が停止したかを判定する装置が知られている（特許文献１参照。）。   An apparatus is known that determines whether a moving body to which an acceleration sensor is attached has stopped based on a comparison between a feature amount of data output over a certain monitoring time and a threshold value (see Patent Document 1). .

特開２００２−１３１０７７号公報JP 2002-131077 A

しかしながら、上述の装置は、その監視時間内におけるセンサ出力の変動幅又は標準偏差を特徴量として導き出すことを開示するのみであり、停止したかを判定する際のセンサ出力の活用方法に改善の余地がある。   However, the above-described apparatus only discloses that the fluctuation range or standard deviation of the sensor output within the monitoring time is derived as a feature amount, and there is room for improvement in the method of utilizing the sensor output when determining whether or not it has stopped. There is.

上述に鑑み、慣性センサの出力をより有効に活用して慣性センサの零点ずれを補正する零点ずれ補正システム、零点ずれ補正装置、及び零点ずれ補正方法を提供することが望ましい。   In view of the above, it is desirable to provide a zero-point deviation correction system, a zero-point deviation correction device, and a zero-point deviation correction method that more effectively utilize the output of the inertia sensor to correct the zero deviation of the inertial sensor.

本発明の実施例に係る零点ずれ補正システムは、慣性センサと、記憶装置と、前記記憶装置に記憶された前記慣性センサの零点ずれを取得する零点ずれ取得部と、前記零点ずれ取得部が取得する前記慣性センサの零点ずれを用いて前記慣性センサが出力するセンサ値を補正して補正後センサ値を出力するセンサ出力補正部と、前記記憶装置に記憶された所定の閾値を取得する閾値取得部と、前記センサ出力補正部が出力する補正後センサ値から前記慣性センサの運動エネルギを算出する運動エネルギ計算部と、前記閾値取得部が取得する閾値と前記運動エネルギ計算部が算出する前記慣性センサの運動エネルギとを比較し、前記記憶装置に記憶された前記慣性センサの零点ずれを更新する零点ずれ更新部と、を有する。   A zero point deviation correction system according to an embodiment of the present invention includes an inertial sensor, a storage device, a zero point deviation acquisition unit that acquires a zero point deviation of the inertial sensor stored in the storage device, and the zero point deviation acquisition unit. A sensor output correction unit that corrects a sensor value output from the inertial sensor using a zero point deviation of the inertial sensor and outputs a corrected sensor value; and a threshold value acquisition that acquires a predetermined threshold value stored in the storage device , A kinetic energy calculation unit that calculates kinetic energy of the inertial sensor from a corrected sensor value output from the sensor output correction unit, a threshold value acquired by the threshold value acquisition unit, and the inertia calculated by the kinetic energy calculation unit A zero point deviation updating unit that compares the kinetic energy of the sensor and updates the zero point deviation of the inertial sensor stored in the storage device.

本発明の実施例に係る零点ずれ補正装置は、記憶装置に記憶された慣性センサの零点ずれを取得する零点ずれ取得部と、前記零点ずれ取得部が取得する前記慣性センサの零点ずれを用いて前記慣性センサが出力するセンサ値を補正して補正後センサ値を出力するセンサ出力補正部と、前記記憶装置に記憶された所定の閾値を取得する閾値取得部と、前記センサ出力補正部が出力する補正後センサ値から前記慣性センサの運動エネルギを算出する運動エネルギ計算部と、前記閾値取得部が取得する閾値と前記運動エネルギ計算部が算出する前記慣性センサの運動エネルギとを比較し、前記記憶装置に記憶された前記慣性センサの零点ずれを更新する零点ずれ更新部と、を有する。   A zero point deviation correction apparatus according to an embodiment of the present invention uses a zero point deviation acquisition unit that acquires a zero point deviation of an inertial sensor stored in a storage device, and a zero point deviation of the inertial sensor that the zero point deviation acquisition unit acquires. A sensor output correction unit that corrects a sensor value output from the inertial sensor and outputs a corrected sensor value, a threshold acquisition unit that acquires a predetermined threshold stored in the storage device, and an output from the sensor output correction unit Comparing the kinetic energy of the inertial sensor calculated by the kinetic energy of the inertial sensor, the threshold acquired by the threshold acquisition unit and the kinetic energy of the inertial sensor calculated by the kinetic energy calculator, And a zero point deviation updating unit for updating the zero point deviation of the inertial sensor stored in the storage device.

本発明の実施例に係る零点ずれ補正方法は、記憶装置に記憶された慣性センサの零点ずれを取得する零点ずれ取得ステップと、前記零点ずれ取得ステップで取得される前記慣性センサの零点ずれを用いて前記慣性センサが出力するセンサ値を補正して補正後センサ値を出力するセンサ出力補正ステップと、前記記憶装置に記憶された所定の閾値を取得する閾値取得ステップと、前記センサ出力補正ステップで出力される補正後センサ値から前記慣性センサの運動エネルギを算出する運動エネルギ計算ステップと、前記閾値取得ステップで取得される閾値と前記運動エネルギ計算ステップで算出される前記慣性センサの運動エネルギとを比較し、前記記憶装置に記憶された前記慣性センサの零点ずれを更新する零点ずれ更新ステップと、を有する。   A zero-point deviation correction method according to an embodiment of the present invention uses a zero-point deviation acquisition step for acquiring the zero-point deviation of the inertial sensor stored in the storage device, and the zero-point deviation of the inertial sensor acquired in the zero-point deviation acquisition step. A sensor output correction step of correcting the sensor value output from the inertial sensor and outputting a corrected sensor value, a threshold acquisition step of acquiring a predetermined threshold stored in the storage device, and the sensor output correction step. A kinetic energy calculation step of calculating kinetic energy of the inertial sensor from the output corrected sensor value, a threshold acquired in the threshold acquisition step, and a kinetic energy of the inertial sensor calculated in the kinetic energy calculation step. A zero point deviation update step for comparing and updating a zero point deviation of the inertial sensor stored in the storage device. .

上述の手段により、慣性センサの出力をより有効に活用して慣性センサの零点ずれを補正する零点ずれ補正システム、零点ずれ補正装置、及び零点ずれ補正方法を提供できる。   With the above-described means, it is possible to provide a zero-point deviation correction system, a zero-point deviation correction apparatus, and a zero-point deviation correction method that correct the zero point deviation of the inertial sensor by effectively using the output of the inertia sensor.

本発明の実施例に係る零点ずれ補正システムの構成例を示す機能ブロック図である。It is a functional block diagram which shows the structural example of the zero point shift correction system which concerns on the Example of this invention. 静止中の移動体に取り付けられた慣性センサの出力の温度特性を示す図である。It is a figure which shows the temperature characteristic of the output of the inertial sensor attached to the moving body at rest. 図２の領域Ｒ１の拡大図である。It is an enlarged view of area | region R1 of FIG. 慣性センサの３つの出力と判定値の時間的推移を示す図である。It is a figure which shows the time transition of three outputs of an inertial sensor, and a determination value. 図３に示す慣性センサのセンサ値の推移に対応する補正後センサ値の積算値の推移を示す図である。It is a figure which shows transition of the integrated value of the sensor value after correction | amendment corresponding to transition of the sensor value of the inertial sensor shown in FIG. 零点ずれ補正処理の流れを示すフローチャートである。It is a flowchart which shows the flow of a zero point shift correction process.

図１は、本発明の実施例に係る零点ずれ補正システムの構成例を示す機能ブロック図である。零点ずれ補正システム１００は、移動体に取り付けられる慣性センサ２０の零点ずれを補正するシステムであり、主に、零点ずれ補正装置１０、慣性センサ２０、記憶装置２１、及び出力先装置２２を含む。なお、零点ずれ補正装置１０及び記憶装置２１は移動体に取り付けられてもよく移動体に取り付けられることなく移動体の外部に設置されてもよい。同様に、出力先装置２２は移動体に取り付けられてもよく移動体に取り付けられることなく移動体の外部に設置されてもよい。また、各構成要素は無線接続されてもよく有線接続されてもよい。   FIG. 1 is a functional block diagram showing a configuration example of a zero point deviation correction system according to an embodiment of the present invention. The zero point deviation correction system 100 is a system that corrects the zero point deviation of the inertial sensor 20 attached to the moving body, and mainly includes a zero point deviation correction device 10, an inertial sensor 20, a storage device 21, and an output destination device 22. The zero point deviation correction device 10 and the storage device 21 may be attached to the moving body or may be installed outside the moving body without being attached to the moving body. Similarly, the output destination device 22 may be attached to the moving body or may be installed outside the moving body without being attached to the moving body. Each component may be wirelessly connected or wired.

零点ずれ補正装置１０は慣性センサ２０の出力の零点に対するずれを補正して補正後センサ値を出力する装置である。本実施例では、零点ずれ補正装置１０はＣＰＵ、内部メモリ等を備えるコンピュータで構成される。また、零点ずれ補正装置１０は、零点ずれ取得部１１、センサ出力補正部１２、運動エネルギ計算部１３、閾値取得部１４、零点ずれ更新部１５、及び動き推定部１６を機能要素として有する。そして、零点ずれ補正装置１０は、各機能要素に対応するプログラムを内部メモリにロードしてＣＰＵに実行させることで各機能要素を実現する。   The zero point deviation correction device 10 is a device that corrects a deviation of the output of the inertial sensor 20 with respect to the zero point and outputs a corrected sensor value. In this embodiment, the zero point deviation correction apparatus 10 is constituted by a computer having a CPU, an internal memory, and the like. The zero point deviation correction device 10 includes a zero point deviation acquisition unit 11, a sensor output correction unit 12, a kinetic energy calculation unit 13, a threshold acquisition unit 14, a zero point deviation update unit 15, and a motion estimation unit 16 as functional elements. Then, the zero point deviation correction apparatus 10 implements each functional element by loading a program corresponding to each functional element into the internal memory and causing the CPU to execute it.

慣性センサ２０は、慣性を利用して測定を行うデバイスであり、例えば、加速度センサ、ジャイロスコープ等を含む。本実施例では、慣性センサ２０は、３軸加速度センサと３軸ジャイロスコープの組み合わせで構成される６軸ＭＥＭＳ（Micro Electro Mechanical System（微小電気機械システム））センサである。具体的には、零点ずれ補正装置１０は慣性センサ２０の６つの出力のそれぞれの零点に対するずれを補正する。慣性センサ２０の６つの出力は、Ｘ軸方向の加速度、Ｙ軸方向の加速度、Ｚ軸方向の加速度、Ｘ軸回りの角速度、Ｙ軸回りの角速度、及びＺ軸回りの角速度である。なお、慣性センサ２０は、１軸センサであってもよく、６軸未満の多軸センサであってもよく、７軸以上の多軸センサであってもよい。   The inertial sensor 20 is a device that performs measurement using inertia, and includes, for example, an acceleration sensor, a gyroscope, and the like. In this embodiment, the inertial sensor 20 is a 6-axis MEMS (Micro Electro Mechanical System) sensor configured by a combination of a 3-axis acceleration sensor and a 3-axis gyroscope. Specifically, the zero point deviation correction device 10 corrects deviations of the six outputs of the inertial sensor 20 with respect to the respective zero points. The six outputs of the inertial sensor 20 are acceleration in the X-axis direction, acceleration in the Y-axis direction, acceleration in the Z-axis direction, angular velocity around the X axis, angular velocity around the Y axis, and angular velocity around the Z axis. The inertial sensor 20 may be a single-axis sensor, a multi-axis sensor with less than six axes, or a multi-axis sensor with seven or more axes.

記憶装置２１は各種情報を記憶する不揮発性記憶媒体である。本実施例では、記憶装置２１として半導体メモリが採用される。   The storage device 21 is a nonvolatile storage medium that stores various types of information. In this embodiment, a semiconductor memory is employed as the storage device 21.

出力先装置２２は慣性センサの出力先としての装置である。本実施例では、出力先装置２２は補正後センサ値を出力する零点ずれ補正装置１０の出力先装置であり、例えば、ロボットコントローラ、車両コントローラ等である。なお、出力先装置２２は、零点ずれ補正装置１０が出力する情報を表示するディスプレイ、及び、零点ずれ補正装置１０が出力する情報を音声出力するスピーカであってもよい。   The output destination device 22 is a device as an output destination of the inertial sensor. In this embodiment, the output destination device 22 is an output destination device of the zero point deviation correction device 10 that outputs the corrected sensor value, and is, for example, a robot controller, a vehicle controller, or the like. The output destination device 22 may be a display that displays information output from the zero point deviation correction device 10 and a speaker that outputs information output from the zero point deviation correction device 10 as audio.

次に、零点ずれ補正装置１０が有する機能要素について説明する。   Next, functional elements of the zero point deviation correction apparatus 10 will be described.

零点ずれ取得部１１は、慣性センサ２０の零点ずれを取得する機能要素である。本実施例では、零点ずれ取得部１１は記憶装置２１に記憶された慣性センサ２０の６つの出力のそれぞれの零点ずれを取得する。零点ずれは、慣性センサ２０の出力が零であるべきときに慣性センサ２０が実際に出力する出力値である。例えば、Ｘ軸方向の加速度が零であるべきときに慣性センサ２０が実際に出力するＸ軸方向の加速度の値である。零点ずれ補正装置１０は、例えば、慣性センサ２０が実際に出力するＸ軸方向の加速度の値から零点ずれを差し引くことでＸ軸方向の加速度の正味の値を得る。なお、零点ずれは、例えば、慣性センサ２０の出荷時に行われるキャリブレーションによって決まる値であり、零点ずれ補正システム１００の出荷時に記憶装置２１に記憶される。以下では、零点ずれ補正システム１００の出荷時に記憶装置２１に記憶される零点ずれを「基準零点ずれ」とする。また、記憶装置２１に記憶された零点ずれは零点ずれ更新部１５によって更新されるが、零点ずれ補正装置１０が所定時間以上停止していた場合等、所定の条件が満たされた場合には基準零点ずれの値にリセットされてもよい。   The zero point deviation acquisition unit 11 is a functional element that acquires the zero point deviation of the inertial sensor 20. In this embodiment, the zero point deviation acquisition unit 11 acquires zero point deviations of the six outputs of the inertial sensor 20 stored in the storage device 21. The zero point deviation is an output value that the inertial sensor 20 actually outputs when the output of the inertial sensor 20 should be zero. For example, it is the value of the acceleration in the X-axis direction that the inertial sensor 20 actually outputs when the acceleration in the X-axis direction should be zero. For example, the zero point deviation correction apparatus 10 obtains a net value of acceleration in the X axis direction by subtracting the zero point deviation from the acceleration value in the X axis direction actually output by the inertial sensor 20. The zero point deviation is a value determined by, for example, calibration performed at the time of shipment of the inertial sensor 20, and is stored in the storage device 21 at the time of shipment of the zero point deviation correction system 100. Hereinafter, the zero point deviation stored in the storage device 21 at the time of shipment of the zero point deviation correction system 100 is referred to as “reference zero point deviation”. Further, the zero point deviation stored in the storage device 21 is updated by the zero point deviation update unit 15. However, when a predetermined condition is satisfied, such as when the zero point deviation correction device 10 has been stopped for a predetermined time or more, a reference is made. It may be reset to a zero point deviation value.

センサ出力補正部１２は、慣性センサ２０のセンサ値を補正する機能要素である。本実施例では、センサ出力補正部１２は、零点ずれ取得部１１が記憶装置２１から読み出した零点ずれを用いて慣性センサ２０が出力するセンサ値を補正して補正後センサ値を出力する。例えば、センサ出力補正部１２は、慣性センサ２０が出力するセンサ値から、零点ずれ取得部１１が読み出した零点ずれを差し引いた正味のセンサ値を補正後センサ値として出力する。なお、センサ出力補正部１２は、補正後センサ値（電圧）に所定の電圧物理量変換係数を乗じて物理量（加速度、角速度等）を導き出し、それら物理量を出力先装置２２に対して出力してもよい。   The sensor output correction unit 12 is a functional element that corrects the sensor value of the inertial sensor 20. In the present embodiment, the sensor output correction unit 12 corrects the sensor value output from the inertial sensor 20 using the zero point deviation read from the storage device 21 by the zero point deviation acquisition unit 11 and outputs the corrected sensor value. For example, the sensor output correction unit 12 outputs a net sensor value obtained by subtracting the zero point deviation read by the zero point deviation acquisition unit 11 from the sensor value output from the inertial sensor 20 as a corrected sensor value. The sensor output correction unit 12 derives a physical quantity (acceleration, angular velocity, etc.) by multiplying the corrected sensor value (voltage) by a predetermined voltage physical quantity conversion coefficient, and outputs the physical quantity to the output destination device 22. Good.

運動エネルギ計算部１３は、慣性センサ２０の運動エネルギを算出する機能要素である。本実施例では、運動エネルギ計算部１３はセンサ出力補正部１２が出力する補正後センサ値から慣性センサ２０の運動エネルギを算出する。具体的には、運動エネルギ計算部１３は慣性センサ２０の６つの出力のそれぞれに関して運動エネルギを算出する。すなわち、６つの運動エネルギを算出する。その上で、運動エネルギ計算部１３は６つの運動エネルギを合計して１つの合計運動エネルギを算出する。なお、Ｘ軸方向の加速度に関する運動エネルギとＹ軸方向の加速度に関する運動エネルギとＺ軸方向の加速度に関する運動エネルギとを合計して加速度に関する運動エネルギを算出してもよい。角速度に関する運動エネルギについても同様である。また、Ｘ軸方向の加速度に関する運動エネルギとＸ軸回りの角速度に関する運動エネルギとを合計してＸ軸に関する運動エネルギを算出してもよい。Ｙ軸に関する運動エネルギ、及び、Ｚ軸に関する運動エネルギについても同様である。   The kinetic energy calculation unit 13 is a functional element that calculates the kinetic energy of the inertial sensor 20. In this embodiment, the kinetic energy calculation unit 13 calculates the kinetic energy of the inertial sensor 20 from the corrected sensor value output from the sensor output correction unit 12. Specifically, the kinetic energy calculation unit 13 calculates kinetic energy for each of the six outputs of the inertial sensor 20. That is, six kinetic energies are calculated. After that, the kinetic energy calculation unit 13 calculates the total kinetic energy by adding the six kinetic energies. The kinetic energy related to acceleration may be calculated by adding the kinetic energy related to acceleration in the X-axis direction, the kinetic energy related to acceleration in the Y-axis direction, and the kinetic energy related to acceleration in the Z-axis direction. The same applies to the kinetic energy related to the angular velocity. Alternatively, the kinetic energy related to the X axis may be calculated by adding the kinetic energy related to the acceleration in the X axis direction and the kinetic energy related to the angular velocity around the X axis. The same applies to the kinetic energy related to the Y axis and the kinetic energy related to the Z axis.

具体的には、運動エネルギ計算部１３は、以下の（１）式を用いてＸ軸方向の加速度に関する並進運動の運動エネルギKx1を所定周期毎に算出する。 Specifically, the kinetic energy calculation unit 13 calculates the kinetic energy K x1 of the translational motion related to the acceleration in the X-axis direction at predetermined intervals using the following equation (1).

Figure 0006282570
なお、mは慣性センサ２０の質量（定数）を表し、vxは慣性センサ２０のＸ軸方向の速度を表す。また、速度vxは慣性センサ２０の出力の１つであるＸ軸方向の加速度を時間積分することで導き出される。
Figure 0006282570
 Incidentally, m represents the mass (constant) of the inertial sensor 20, v x denotes the speed of the X-axis direction of the inertial sensor 20. The velocity v x is derived by time-integrating the acceleration in the X-axis direction, which is one of the outputs of the inertial sensor 20.

同様にして、運動エネルギ計算部１３は、Ｙ軸方向の加速度に関する運動エネルギKy1、及び、Ｚ軸方向の加速度に関する運動エネルギKz1を算出する。 Similarly, the kinetic energy calculation unit 13 calculates kinetic energy K y1 related to acceleration in the Y-axis direction and kinetic energy K z1 related to acceleration in the Z-axis direction.

また、運動エネルギ計算部１３は、以下の（２）式を用いてＸ軸回りの角速度に関する回転運動の運動エネルギKx2を所定周期毎に算出する。 Further, the kinetic energy calculation unit 13 calculates the kinetic energy K x2 of the rotational motion related to the angular velocity around the X axis for each predetermined period using the following equation (2).

Figure 0006282570
なお、Iは慣性センサ２０の慣性モーメント（定数）を表し、ωxは慣性センサ２０のＸ軸回りの角速度を表す。
Figure 0006282570
 Incidentally, I represents represents the moment of inertia (a constant) of the inertial sensor 20, omega x represents the X-axis of the angular velocity of the inertial sensor 20.

同様にして、運動エネルギ計算部１３は、Ｙ軸回りの角速度に関する運動エネルギKy2、及び、Ｚ軸回りの角速度に関する運動エネルギKz2を算出する。 Similarly, the kinetic energy calculation unit 13 calculates kinetic energy K y2 related to the angular velocity around the Y axis and kinetic energy K z2 related to the angular velocity around the Z axis.

また、運動エネルギ計算部１３は、以下の（３）式に示すように、６つの運動エネルギKx1、Kx2、Ky1、Ky2、Kz1、Kz2を合計して合計運動エネルギKを算出する。 The kinetic energy calculation unit 13 adds up the six kinetic energies K x1 , K x2 , K y1 , K y2 , K z1 , K z2 as shown in the following equation (3) to obtain the total kinetic energy K: calculate.

Figure 0006282570
このようにして合計運動エネルギKを所定周期毎に算出することで、零点ずれ補正装置１０は慣性センサ２０の６つの出力の全てを有効に利用できる。そして、慣性センサ２０の６つの出力のうちの１つにしか影響が出ないような移動体の動きをより確実に検出することができる。その結果、移動体が停止しているか否かをより正確に判断できる。
Figure 0006282570
 Thus, by calculating the total kinetic energy K for each predetermined period, the zero point deviation correction apparatus 10 can effectively use all six outputs of the inertial sensor 20. Then, it is possible to more reliably detect the movement of the moving body that affects only one of the six outputs of the inertial sensor 20. As a result, it can be more accurately determined whether or not the moving body is stopped.

また、零点ずれ補正装置１０は、運動エネルギを算出することで、慣性センサ２０の直接の出力である加速度と角速度のようにそのままでは同列に扱うことのできない単位の異なる物理量を同列に扱うことができる。   Further, the zero-point deviation correction apparatus 10 can handle physical quantities of different units that cannot be handled in the same row as they are, such as acceleration and angular velocity, which are direct outputs of the inertial sensor 20, by calculating kinetic energy. it can.

閾値取得部１４は、運動エネルギ計算部１３が算出する運動エネルギとの比較に用いられる閾値を取得する機能要素である。本実施例では、閾値取得部１４は記憶装置２１に記憶された閾値を取得する。具体的には、閾値取得部１４は、慣性センサ２０の６つの出力のそれぞれに関して算出される運動エネルギのそれぞれに対応する閾値を取得する。すなわち、６つの閾値を取得する。そして、閾値取得部１４は記憶装置２１から読み出した６つの閾値を合計して１つの閾値を算出する。このように、閾値取得部１４は、運動エネルギ計算部１３が算出する運動エネルギの数及び内容に応じ、取得する閾値の数及び内容を変更する。例えば、運動エネルギ計算部１３が６つの運動エネルギを合計して１つの合計運動エネルギを算出する場合、閾値取得部１４は６つの閾値を合計した１つの閾値を取得する。なお、閾値取得部１４は合計運動エネルギとの比較のために予め記憶された１つの閾値を記憶装置２１から読み出してもよい。   The threshold acquisition unit 14 is a functional element that acquires a threshold used for comparison with the kinetic energy calculated by the kinetic energy calculation unit 13. In the present embodiment, the threshold acquisition unit 14 acquires the threshold stored in the storage device 21. Specifically, the threshold acquisition unit 14 acquires a threshold corresponding to each of the kinetic energy calculated for each of the six outputs of the inertial sensor 20. That is, six threshold values are acquired. Then, the threshold acquisition unit 14 adds up the six thresholds read from the storage device 21 to calculate one threshold. Thus, the threshold value acquisition unit 14 changes the number and content of the threshold value to be acquired according to the number and content of the kinetic energy calculated by the kinetic energy calculation unit 13. For example, when the kinetic energy calculation unit 13 adds up six kinetic energies to calculate one total kinetic energy, the threshold acquisition unit 14 acquires one threshold obtained by adding up the six thresholds. The threshold acquisition unit 14 may read one threshold stored in advance for comparison with the total kinetic energy from the storage device 21.

閾値は、主に、慣性センサ２０のセンサ値におけるノイズの大きさ、慣性センサ２０の使用環境、及び、慣性センサ２０の温度特性における所要の線形性に基づいて決定される。本実施例では、センサ値としての電圧値に関する電気的ノイズの大きさ（以下、「静的ノイズレベル」とする。）が大きいほど、或いは、使用環境温度の範囲が広いほど、或いは、要求される線形性の範囲が広いほど、閾値は大きめに設定される。   The threshold value is mainly determined based on the magnitude of noise in the sensor value of the inertial sensor 20, the usage environment of the inertial sensor 20, and the required linearity in the temperature characteristics of the inertial sensor 20. In the present embodiment, the larger the electrical noise level (hereinafter referred to as “static noise level”) related to the voltage value as the sensor value, or the wider the range of the operating environment temperature, or the more demanded. The threshold is set to be larger as the linearity range is wider.

図２は、静止中の移動体に取り付けられた慣性センサ２０の３つの出力の温度特性を示す図である。具体的には、実線はＸ軸回りの角速度に関する出力の推移を表し、破線はＹ軸回りの角速度に関する出力の推移を表し、点線はＺ軸回りの角速度に関する出力の推移を表す。なお、縦軸は角速度［度／秒］に対応し、横軸は温度［℃］に対応する。また、温度Ｔｍｉｎは温度特性の線形性が維持される範囲の下限温度を表し、温度Ｔｍａｘは温度特性の線形性が維持される範囲の上限温度を表す。   FIG. 2 is a diagram showing temperature characteristics of three outputs of the inertial sensor 20 attached to a stationary moving body. Specifically, the solid line represents the transition of the output related to the angular velocity around the X axis, the broken line represents the transition of the output related to the angular velocity around the Y axis, and the dotted line represents the transition of the output related to the angular velocity around the Z axis. The vertical axis corresponds to the angular velocity [degrees / second], and the horizontal axis corresponds to the temperature [° C.]. Further, the temperature Tmin represents the lower limit temperature of the range in which the linearity of the temperature characteristic is maintained, and the temperature Tmax represents the upper limit temperature of the range in which the linearity of the temperature characteristic is maintained.

また、図２は、慣性センサの出力の温度特性がセンサ毎に異なるように、同じ１つの慣性センサであっても各出力の温度特性が異なることを示す。具体的には、図２は、少なくとも温度が同じであれば３軸回りの角速度に関する各出力が何れも値ゼロを示すべきところ、温度Ｔ１においてＸ軸回りの角速度ωｘ１がＹ軸回りの角速度ωｙ１より大きく、また、Ｙ軸回りの角速度ωｙ１がＺ軸回りの角速度ωｚ１より大きい状態を示す。   FIG. 2 shows that the temperature characteristics of the outputs are different even for the same inertial sensor, so that the temperature characteristics of the outputs of the inertial sensors are different for each sensor. Specifically, FIG. 2 shows that each of the outputs related to the angular velocities around the three axes should show a value of zero if at least the temperatures are the same, but the angular velocity ωx1 around the X axis at the temperature T1 is the angular velocity ωy1 around the Y axis. The angular velocity ωy1 about the Y axis is larger than the angular velocity ωz1 about the Z axis.

図３は、図２の領域Ｒ１の拡大図であり、角速度の幅ＮＬは静的ノイズレベルを表す。静的ノイズレベルは各出力でほぼ同じであり温度変化に応じて増減することもない。   FIG. 3 is an enlarged view of the region R1 of FIG. 2, and the angular velocity width NL represents a static noise level. The static noise level is almost the same for each output, and does not increase or decrease according to the temperature change.

なお、本実施例では零点ずれ及び閾値は同じ記憶装置２１に記憶されるが別の実施例では別々の記憶装置に記憶されていてもよい。また、零点ずれは複数の記憶装置に重複して或いは分散して記憶されてもよい。閾値についても同様である。   In this embodiment, the zero point deviation and the threshold value are stored in the same storage device 21, but in another embodiment, they may be stored in separate storage devices. Further, the zero point deviation may be stored in a plurality of storage devices in an overlapping manner or in a distributed manner. The same applies to the threshold value.

零点ずれ更新部１５は、所定の条件が満たされた場合に零点ずれを更新する機能要素である。本実施例では、零点ずれ更新部１５は、運動エネルギ計算部１３が算出する慣性センサ２０の運動エネルギと閾値取得部１４が取得する閾値とを比較する。そして、慣性センサ２０の運動エネルギが閾値以下であると判定した場合に記憶装置２１に記憶された慣性センサ２０の零点ずれを更新する。慣性センサ２０が取り付けられた移動体が停止していると判断できるためである。具体的には、零点ずれ更新部１５は、合計運動エネルギが閾値以下であると判定した場合に記憶装置２１に記憶された零点ずれの値を現在のセンサ値で書き換える。一方で、零点ずれ更新部１５は、合計運動エネルギが閾値より大きいと判定した場合には零点ずれの値を更新しない。慣性センサ２０が取り付けられた移動体が停止していないと判断できるためである。   The zero point deviation update unit 15 is a functional element that updates the zero point deviation when a predetermined condition is satisfied. In the present embodiment, the zero point deviation updating unit 15 compares the kinetic energy of the inertial sensor 20 calculated by the kinetic energy calculation unit 13 and the threshold acquired by the threshold acquisition unit 14. And when it determines with the kinetic energy of the inertial sensor 20 being below a threshold value, the zero point shift | offset | difference of the inertial sensor 20 memorize | stored in the memory | storage device 21 is updated. This is because it can be determined that the moving body to which the inertial sensor 20 is attached is stopped. Specifically, the zero point deviation update unit 15 rewrites the zero point deviation value stored in the storage device 21 with the current sensor value when it is determined that the total kinetic energy is equal to or less than the threshold value. On the other hand, the zero point deviation update unit 15 does not update the value of the zero point deviation when it is determined that the total kinetic energy is larger than the threshold value. This is because it can be determined that the moving body to which the inertial sensor 20 is attached has not stopped.

或いは、零点ずれ更新部１５は、６つの運動エネルギのそれぞれが対応する閾値以下であると判定した場合に記憶装置２１に記憶された６つの零点ずれの値を現在の６つのセンサ値で書き換えてもよい。この場合、零点ずれ更新部１５は、６つの運動エネルギのうちの何れか１つが閾値より大きいと判定した場合には零点ずれを更新しない。慣性センサ２０が取り付けられた移動体が停止していないと判断できるためである。運動エネルギ計算部１３が６つ未満或いは７つ以上の運動エネルギを算出する場合についても同様である。   Alternatively, the zero point deviation updating unit 15 rewrites the six zero point deviation values stored in the storage device 21 with the current six sensor values when it is determined that each of the six kinetic energies is equal to or less than the corresponding threshold value. Also good. In this case, the zero point deviation update unit 15 does not update the zero point deviation when it is determined that any one of the six kinetic energies is larger than the threshold value. This is because it can be determined that the moving body to which the inertial sensor 20 is attached has not stopped. The same applies to the case where the kinetic energy calculation unit 13 calculates less than six or seven or more kinetic energies.

また、複数の慣性センサ２０が移動体に取り付けられている場合、零点ずれ更新部１５は、複数の慣性センサ２０のそれぞれに関する合計運動エネルギの全てが閾値以下であると判定した場合に記憶装置２１に記憶された零点ずれの値を現在のセンサ値で書き換えてもよい。すなわち、零点ずれ更新部１５は、複数の慣性センサ２０のそれぞれに関する合計運動エネルギが１つでも閾値より大きければ零点ずれの値を更新しないようにしてもよい。   Further, when a plurality of inertial sensors 20 are attached to the moving body, the zero point deviation update unit 15 stores the storage device 21 when it is determined that all of the total kinetic energy relating to each of the plurality of inertial sensors 20 is equal to or less than the threshold value. May be rewritten with the current sensor value. That is, the zero point deviation updating unit 15 may not update the value of the zero point deviation if the total kinetic energy relating to each of the plurality of inertial sensors 20 is greater than the threshold value.

図４は、慣性センサ２０の３つの出力と判定値の時間的推移を示す図である。慣性センサ２０の３つの出力はＸ軸回りの角速度ωｘ、Ｙ軸回りの角速度ωｙ、Ｚ軸回りの角速度ωｚである。また、判定値は零点ずれ更新部１５の判定結果を表す。具体的には、値「１」は慣性センサ２０の合計運動エネルギが閾値以下であると零点ずれ更新部１５が判定したことを表し、値「０」は慣性センサ２０の合計運動エネルギが閾値より大きいと零点ずれ更新部１５が判定したことを表す。なお、図４の４つのグラフの時間軸は共通である。   FIG. 4 is a diagram showing temporal transitions of the three outputs of the inertial sensor 20 and the determination value. The three outputs of the inertial sensor 20 are an angular velocity ωx around the X axis, an angular velocity ωy around the Y axis, and an angular velocity ωz around the Z axis. The determination value represents the determination result of the zero point deviation update unit 15. Specifically, the value “1” indicates that the zero point deviation update unit 15 determines that the total kinetic energy of the inertial sensor 20 is equal to or less than the threshold value, and the value “0” indicates that the total kinetic energy of the inertial sensor 20 is less than the threshold value. If it is larger, it means that the zero point deviation update unit 15 has made the determination. Note that the time axes of the four graphs in FIG. 4 are common.

図４に示すように、零点ずれ更新部１５は、判定値が値「１」の場合に慣性センサ２０の現在の出力（センサ値）で零点ずれの値を書き換え、判定値が値「０」の場合には書き換えを行わない。   As shown in FIG. 4, when the determination value is the value “1”, the zero point deviation update unit 15 rewrites the zero point deviation value with the current output (sensor value) of the inertial sensor 20, and the determination value is the value “0”. In the case of, rewriting is not performed.

動き推定部１６は、慣性センサ２０が取り付けられた移動体の動きを推定する機能要素である。本実施例では、動き推定部１６は、センサ出力補正部１２が出力する補正後センサ値を積算して移動体の並進及び回転（姿勢）の状態を推定する。そして、動き推定部１６は推定結果を出力先装置２２に対して出力する。そのため、出力先装置２２としてのロボットコントローラ又は車両コントローラはその推定結果に基づいてロボット又は車両の動きを制御できる。或いは、出力先装置２２としてのディスプレイはその推定結果を表示でき、出力先装置２２としてのスピーカはその推定結果を音声出力できる。   The motion estimation unit 16 is a functional element that estimates the motion of the moving body to which the inertial sensor 20 is attached. In the present embodiment, the motion estimation unit 16 estimates the state of translation and rotation (posture) of the moving body by integrating the corrected sensor values output by the sensor output correction unit 12. Then, the motion estimation unit 16 outputs the estimation result to the output destination device 22. Therefore, the robot controller or the vehicle controller as the output destination device 22 can control the movement of the robot or the vehicle based on the estimation result. Alternatively, the display as the output destination device 22 can display the estimation result, and the speaker as the output destination device 22 can output the estimation result by voice.

図５は、図３に示す角速度の時間的推移から推定される回転角度の時間的推移を示す図である。具体的には、図５は静止中の移動体に取り付けられた慣性センサ２０の出力のうちのＸ軸回りの角速度ωｘに関する補正後センサ値を積算することで導き出されるＸ軸回りの回転角度の時間的推移を示す。なお、縦軸は回転角度に対応し、横軸は時間に対応する。また、実線は零点ずれ更新部１５によって零点ずれが更新される場合の推移を表し、破線は図３に示す初期零点ずれＳＰで各センサ値を補正した場合の推移を表し、点線は補正無しの場合の推移を表す。   FIG. 5 is a diagram showing the temporal transition of the rotation angle estimated from the temporal transition of the angular velocity shown in FIG. Specifically, FIG. 5 shows the rotation angle around the X axis derived by integrating the corrected sensor values related to the angular velocity ωx around the X axis in the output of the inertial sensor 20 attached to the stationary moving body. Shows the time transition. The vertical axis corresponds to the rotation angle, and the horizontal axis corresponds to time. The solid line represents the transition when the zero point deviation is updated by the zero point deviation updating unit 15, the broken line represents the transition when each sensor value is corrected with the initial zero deviation SP shown in FIG. 3, and the dotted line represents no correction. This represents the transition of the case.

図５で示すように、移動体が静止している場合であっても、慣性センサ２０のセンサ値をそのまま積算して得られる移動体の回転角度は点線で示すように時間の経過と共に大きく増加していく。センサ値が零点ずれを含み、且つ、慣性センサ２０の温度変化の影響を受けるためである。同様に、慣性センサ２０が制御周期毎に出力するセンサ値のそれぞれから初期零点ずれＳＰを差し引いた補正後センサ値のそれぞれを積算して得られる移動体の回転角度は、補正無しの場合に比べて小さいものの、破線で示すように時間の経過と共に増加していく。零点ずれによる影響が取り除かれるものの、温度変化の影響を依然として受けるためである。これに対し、慣性センサ２０が制御周期毎に出力するセンサ値のそれぞれから零点ずれ更新部１５によって継続的に更新される零点ずれを差し引いた補正後センサ値のそれぞれを積算して得られる移動体の回転角度は、増加することなくほぼ一定のまま推移する。零点ずれ及び温度変化による影響の双方が取り除かれるためである。   As shown in FIG. 5, even when the moving body is stationary, the rotation angle of the moving body obtained by accumulating the sensor values of the inertial sensor 20 as they are increases greatly as time passes. I will do it. This is because the sensor value includes a zero point shift and is affected by the temperature change of the inertial sensor 20. Similarly, the rotational angle of the moving body obtained by integrating each of the sensor values after correction obtained by subtracting the initial zero point deviation SP from each of the sensor values output by the inertial sensor 20 for each control cycle is compared to the case of no correction. Although it is small, it increases with time as shown by the broken line. This is because although the influence of the zero point deviation is removed, it is still affected by the temperature change. On the other hand, the movable body obtained by integrating each of the sensor values after correction obtained by subtracting the zero point deviation continuously updated by the zero point deviation updating unit 15 from the sensor values output by the inertial sensor 20 for each control cycle. The rotation angle of remains unchanged substantially without increasing. This is because both the zero point shift and the influence of the temperature change are removed.

次に、図６を参照し、零点ずれ補正装置１０が慣性センサ２０の零点ずれを補正する処理（以下、「零点ずれ補正処理」とする。）について説明する。なお、図６は、零点ずれ補正処理の流れを示すフローチャートである。零点ずれ補正装置１０は、所定の制御周期で繰り返しこの零点ずれ補正処理を実行する。   Next, with reference to FIG. 6, a process in which the zero point deviation correction apparatus 10 corrects the zero point deviation of the inertial sensor 20 (hereinafter referred to as “zero point deviation correction process”) will be described. FIG. 6 is a flowchart showing the flow of the zero point deviation correction process. The zero point deviation correction apparatus 10 repeatedly executes this zero point deviation correction process at a predetermined control cycle.

最初に、零点ずれ補正装置１０は零点ずれを取得する（ステップＳ１）。本実施例では、零点ずれ補正装置１０の零点ずれ取得部１１は記憶装置２１の所定領域に記憶された零点ずれを読み出す。   First, the zero point deviation correction apparatus 10 acquires a zero point deviation (step S1). In this embodiment, the zero point deviation acquisition unit 11 of the zero point deviation correction device 10 reads the zero point deviation stored in the predetermined area of the storage device 21.

その後、零点ずれ補正装置１０は慣性センサ２０が出力するセンサ値を取得する（ステップＳ２）。本実施例では、零点ずれ補正装置１０は慣性センサ２０が出力する６つのセンサ値を取得する。   Thereafter, the zero point deviation correction apparatus 10 acquires the sensor value output from the inertial sensor 20 (step S2). In this embodiment, the zero point deviation correction apparatus 10 acquires six sensor values output from the inertial sensor 20.

その後、零点ずれ補正装置１０の閾値取得部１４は閾値を取得する（ステップＳ３）。本実施例では、閾値取得部１４は記憶装置２１の所定領域に記憶された６つの零点ずれを読み出す。   Thereafter, the threshold value acquisition unit 14 of the zero point deviation correction apparatus 10 acquires a threshold value (step S3). In the present embodiment, the threshold acquisition unit 14 reads out six zero point deviations stored in a predetermined area of the storage device 21.

その後、零点ずれ補正装置１０のセンサ出力補正部１２はセンサ値を補正する（ステップＳ４）。本実施例では、センサ出力補正部１２は慣性センサ２０が出力するセンサ値から零点ずれの値を差し引いて補正後センサ値を出力する。なお、センサ出力補正部１２は、補正後センサ値（電圧）に所定の電圧物理量変換係数を乗じて物理量（加速度、角速度等）を導き出し、それら物理量を出力先装置２２に対して出力してもよい。   Thereafter, the sensor output correction unit 12 of the zero point deviation correction apparatus 10 corrects the sensor value (step S4). In the present embodiment, the sensor output correction unit 12 subtracts the zero point deviation value from the sensor value output from the inertial sensor 20 and outputs the corrected sensor value. The sensor output correction unit 12 derives a physical quantity (acceleration, angular velocity, etc.) by multiplying the corrected sensor value (voltage) by a predetermined voltage physical quantity conversion coefficient, and outputs the physical quantity to the output destination device 22. Good.

その後、零点ずれ補正装置１０の運動エネルギ計算部１３は慣性センサ２０の運動エネルギを算出する（ステップＳ５）。本実施例では、運動エネルギ計算部１３はセンサ出力補正部１２が出力する補正後センサ値に基づいて６つの運動エネルギを算出する。   Thereafter, the kinetic energy calculation unit 13 of the zero point deviation correction apparatus 10 calculates the kinetic energy of the inertial sensor 20 (step S5). In the present embodiment, the kinetic energy calculation unit 13 calculates six kinetic energies based on the corrected sensor values output from the sensor output correction unit 12.

なお、ステップＳ１、Ｓ２、及びＳ３は順不同であり同時に実行されてもよい。また、ステップＳ３はステップＳ４又はＳ５の後に実行されてもよくステップＳ４又はＳ５と同時に実行されてもよい。   Note that steps S1, S2, and S3 are out of order and may be executed simultaneously. Moreover, step S3 may be performed after step S4 or S5, and may be performed simultaneously with step S4 or S5.

その後、零点ずれ補正装置１０の零点ずれ更新部１５は運動エネルギと閾値とを比較する（ステップＳ６）。本実施例では、零点ずれ更新部１５は運動エネルギ計算部１３が算出した合計運動エネルギと閾値取得部１４が取得した閾値とを比較する。なお、零点ずれ更新部１５は運動エネルギ計算部１３が算出した６つの運動エネルギと閾値取得部１４が取得した６つの閾値とを対応付けて比較してもよい。   Thereafter, the zero point deviation update unit 15 of the zero point deviation correction device 10 compares the kinetic energy and the threshold value (step S6). In this embodiment, the zero point deviation update unit 15 compares the total kinetic energy calculated by the kinetic energy calculation unit 13 with the threshold acquired by the threshold acquisition unit 14. The zero point deviation updating unit 15 may compare the six kinetic energies calculated by the kinetic energy calculating unit 13 with the six thresholds acquired by the threshold acquiring unit 14 in association with each other.

合計運動エネルギが閾値以下であると判定した場合（ステップＳ６のＹＥＳ）、零点ずれ更新部１５は零点ずれを更新する（ステップＳ７）。本実施例では、合計運動エネルギが閾値以下であると判定した場合、零点ずれ更新部１５は慣性センサ２０が取り付けられた移動体が停止していると判断する。そして、記憶装置２１に記憶された零点ずれの値を現在の補正後センサ値で書き換える。なお、零点ずれ更新部１５は、６つの運動エネルギのそれぞれが対応する閾値以下であると判定した場合に慣性センサ２０が取り付けられた移動体が停止していると判断してもよい。   When it is determined that the total kinetic energy is equal to or less than the threshold value (YES in step S6), the zero point deviation update unit 15 updates the zero point deviation (step S7). In this embodiment, when it is determined that the total kinetic energy is equal to or less than the threshold value, the zero point deviation update unit 15 determines that the moving body to which the inertial sensor 20 is attached is stopped. Then, the zero shift value stored in the storage device 21 is rewritten with the current sensor value after correction. Note that the zero point deviation updating unit 15 may determine that the moving body to which the inertial sensor 20 is attached is stopped when it is determined that each of the six kinetic energies is equal to or less than the corresponding threshold value.

一方で、合計運動エネルギが閾値より大きいと判定した場合（ステップＳ６のＮＯ）、零点ずれ更新部１５は零点ずれを更新することなく今回の零点ずれ補正処理を終了させる。本実施例では、合計運動エネルギが閾値より大きいと判定した場合、零点ずれ更新部１５は慣性センサ２０が取り付けられた移動体が停止していないと判断する。そして、記憶装置２１に記憶された零点ずれの値を現在の補正後センサ値で書き換えることなく今回の零点ずれ補正処理を終了させる。なお、零点ずれ更新部１５は、６つの運動エネルギの何れかが対応する閾値より大きいと判定した場合に慣性センサ２０が取り付けられた移動体が停止していないと判断してもよい。   On the other hand, when it is determined that the total kinetic energy is larger than the threshold value (NO in step S6), the zero point deviation update unit 15 ends the current zero point deviation correction process without updating the zero point deviation. In this embodiment, when it is determined that the total kinetic energy is larger than the threshold value, the zero point deviation update unit 15 determines that the moving body to which the inertial sensor 20 is attached has not stopped. Then, the current zero shift correction process is terminated without rewriting the zero shift value stored in the storage device 21 with the current corrected sensor value. Note that the zero point deviation updating unit 15 may determine that the moving body to which the inertial sensor 20 is attached has not stopped when it is determined that any of the six kinetic energies is greater than the corresponding threshold value.

以上の構成により、零点ずれ補正システム１００は、慣性センサ２０の出力から算出される１又は複数の運動エネルギの値に基づいて慣性センサ２０が取り付けられた移動体が停止しているか否かを判断する。そして、移動体が停止していると判断した場合、記憶装置２１に記憶された零点ずれを更新する。そのため、移動体が停止している場合に例えば慣性センサ２０の温度が上昇し、慣性センサ２０が出力するセンサ値と基準零点ずれとの差が増大する場合であっても、そのセンサ値を適切に補正できる。その結果、移動体が停止しているにもかかわらず零点ずれが積算されてしまい移動体が並進し或いは回転していると推定してしまうのをより確実に防止できる。   With the above configuration, the zero point deviation correction system 100 determines whether or not the moving body to which the inertial sensor 20 is attached is stopped based on one or more kinetic energy values calculated from the output of the inertial sensor 20. To do. If it is determined that the moving body is stopped, the zero point deviation stored in the storage device 21 is updated. For this reason, when the moving body is stopped, for example, the temperature of the inertial sensor 20 rises, and even if the difference between the sensor value output from the inertial sensor 20 and the reference zero point deviation increases, the sensor value is set appropriately. Can be corrected. As a result, it is possible to more reliably prevent the zero point deviation from being accumulated even though the moving body is stopped and estimating that the moving body is translating or rotating.

また、零点ずれ補正システム１００は、慣性センサ２０の軸数が多いほど、移動体が停止しているか否かの判定結果の信頼性を向上させることができる。特定の軸にしか影響しない移動体の動きをより確実に検出できるためである。また、零点ずれ補正システム１００は、移動体が停止していると判断する毎に零点ずれの値を更新するため、慣性センサ２０の温度を検出するための温度センサを用いなくとも慣性センサ２０の個体毎に異なる温度特性の影響を排除できる。そのため、軸数が多いが温度センサが実装されていない安価なＭＥＭＳセンサの零点ずれを補正する際に特に有効である。   Further, the zero point deviation correction system 100 can improve the reliability of the determination result as to whether or not the moving body is stopped as the number of axes of the inertial sensor 20 increases. This is because the movement of the moving body that affects only a specific axis can be detected more reliably. Further, since the zero point deviation correction system 100 updates the value of the zero point deviation every time it is determined that the moving body is stopped, the zero point deviation correction system 100 does not use the temperature sensor for detecting the temperature of the inertial sensor 20. The influence of different temperature characteristics for each individual can be eliminated. Therefore, it is particularly effective when correcting the zero point shift of an inexpensive MEMS sensor having a large number of axes but not mounted with a temperature sensor.

また、零点ずれ補正システム１００は、ハイパスフィルタを用いないため低周波数の感度を犠牲にすることもない。   Further, since the zero point deviation correction system 100 does not use a high-pass filter, it does not sacrifice low frequency sensitivity.

以上、本発明の好ましい実施例について詳説したが、本発明は、上述した実施例に制限されることはなく、本発明の範囲を逸脱することなしに上述した実施例に種々の変形及び置換を加えることができる。   Although the preferred embodiments of the present invention have been described in detail above, the present invention is not limited to the above-described embodiments, and various modifications and substitutions can be made to the above-described embodiments without departing from the scope of the present invention. Can be added.

１０・・・零点ずれ補正装置 １１・・・零点ずれ取得部 １２・・・センサ出力補正部 １３・・・運動エネルギ計算部 １４・・・閾値取得部 １５・・・零点ずれ更新部 １６・・・動き推定部 ２０・・・慣性センサ ２１・・・記憶装置 ２２・・・出力先装置 １００・・・零点ずれ補正システム   DESCRIPTION OF SYMBOLS 10 ... Zero point deviation correction apparatus 11 ... Zero point deviation acquisition part 12 ... Sensor output correction part 13 ... Kinetic energy calculation part 14 ... Threshold value acquisition part 15 ... Zero point deviation update part 16 ... -Motion estimation unit 20 ... Inertial sensor 21 ... Storage device 22 ... Output destination device 100 ... Zero point deviation correction system

Claims (3)

慣性センサと、
記憶装置と、
前記記憶装置に記憶された前記慣性センサの零点ずれを取得する零点ずれ取得部と、
前記零点ずれ取得部が取得する前記慣性センサの零点ずれを用いて前記慣性センサが出力するセンサ値を補正して補正後センサ値を出力するセンサ出力補正部と、
前記記憶装置に記憶された所定の閾値を取得する閾値取得部と、
前記センサ出力補正部が出力する補正後センサ値から前記慣性センサの運動エネルギを算出する運動エネルギ計算部と、
前記閾値取得部が取得する閾値と前記運動エネルギ計算部が算出する前記慣性センサの運動エネルギとを比較し、前記記憶装置に記憶された前記慣性センサの零点ずれを更新する零点ずれ更新部と、を有する、
零点ずれ補正システム。 An inertial sensor;
A storage device;
A zero point deviation acquisition unit for acquiring a zero point deviation of the inertial sensor stored in the storage device;
A sensor output correction unit that corrects a sensor value output by the inertial sensor using a zero point shift of the inertial sensor acquired by the zero point shift acquisition unit and outputs a corrected sensor value;
A threshold acquisition unit for acquiring a predetermined threshold stored in the storage device;
A kinetic energy calculation unit that calculates kinetic energy of the inertial sensor from a corrected sensor value output by the sensor output correction unit;
A zero point deviation update unit that compares the threshold value acquired by the threshold value acquisition unit with the kinetic energy of the inertial sensor calculated by the kinetic energy calculation unit, and updates the zero point deviation of the inertial sensor stored in the storage device; Having
Zero shift correction system. 記憶装置に記憶された慣性センサの零点ずれを取得する零点ずれ取得部と、
前記零点ずれ取得部が取得する前記慣性センサの零点ずれを用いて前記慣性センサが出力するセンサ値を補正して補正後センサ値を出力するセンサ出力補正部と、
前記記憶装置に記憶された所定の閾値を取得する閾値取得部と、
前記センサ出力補正部が出力する補正後センサ値から前記慣性センサの運動エネルギを算出する運動エネルギ計算部と、
前記閾値取得部が取得する閾値と前記運動エネルギ計算部が算出する前記慣性センサの運動エネルギとを比較し、前記記憶装置に記憶された前記慣性センサの零点ずれを更新する零点ずれ更新部と、を有する、
零点ずれ補正装置。 A zero point deviation acquisition unit for acquiring a zero point deviation of the inertial sensor stored in the storage device;
A sensor output correction unit that corrects a sensor value output by the inertial sensor using a zero point shift of the inertial sensor acquired by the zero point shift acquisition unit and outputs a corrected sensor value;
A threshold acquisition unit for acquiring a predetermined threshold stored in the storage device;
A kinetic energy calculation unit that calculates kinetic energy of the inertial sensor from a corrected sensor value output by the sensor output correction unit;
A zero point deviation update unit that compares the threshold value acquired by the threshold value acquisition unit with the kinetic energy of the inertial sensor calculated by the kinetic energy calculation unit, and updates the zero point deviation of the inertial sensor stored in the storage device; Having
Zero point correction device. 記憶装置に記憶された慣性センサの零点ずれを取得する零点ずれ取得ステップと、
前記零点ずれ取得ステップで取得される前記慣性センサの零点ずれを用いて前記慣性センサが出力するセンサ値を補正して補正後センサ値を出力するセンサ出力補正ステップと、
前記記憶装置に記憶された所定の閾値を取得する閾値取得ステップと、
前記センサ出力補正ステップで出力される補正後センサ値から前記慣性センサの運動エネルギを算出する運動エネルギ計算ステップと、
前記閾値取得ステップで取得される閾値と前記運動エネルギ計算ステップで算出される前記慣性センサの運動エネルギとを比較し、前記記憶装置に記憶された前記慣性センサの零点ずれを更新する零点ずれ更新ステップと、を有する、
零点ずれ補正方法。 A zero point deviation obtaining step for obtaining a zero point deviation of the inertial sensor stored in the storage device;
A sensor output correction step of correcting a sensor value output by the inertial sensor using a zero point shift of the inertial sensor acquired in the zero point shift acquisition step and outputting a corrected sensor value;
A threshold acquisition step of acquiring a predetermined threshold stored in the storage device;
A kinetic energy calculation step of calculating kinetic energy of the inertial sensor from the corrected sensor value output in the sensor output correction step;
A zero point deviation update step of comparing the threshold value acquired in the threshold value acquisition step with the kinetic energy of the inertial sensor calculated in the kinetic energy calculation step and updating the zero point deviation of the inertial sensor stored in the storage device. And having
Zero point correction method.
JP2014217593A 2014-10-24 2014-10-24 Zero point deviation correction system, zero point deviation correction apparatus, and zero point deviation correction method for correcting zero point deviation of inertial sensor Active JP6282570B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2014217593A JP6282570B2 (en) 2014-10-24 2014-10-24 Zero point deviation correction system, zero point deviation correction apparatus, and zero point deviation correction method for correcting zero point deviation of inertial sensor

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2014217593A JP6282570B2 (en) 2014-10-24 2014-10-24 Zero point deviation correction system, zero point deviation correction apparatus, and zero point deviation correction method for correcting zero point deviation of inertial sensor

Publications (2)

Publication Number Publication Date
JP2016085104A JP2016085104A (en) 2016-05-19
JP6282570B2 true JP6282570B2 (en) 2018-02-21

Family

ID=55972834

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2014217593A Active JP6282570B2 (en) 2014-10-24 2014-10-24 Zero point deviation correction system, zero point deviation correction apparatus, and zero point deviation correction method for correcting zero point deviation of inertial sensor

Country Status (1)

Country Link
JP (1) JP6282570B2 (en)

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE4404195C1 (en) * 1994-02-10 1995-08-24 Aeg Postautomation Gmbh Device and method for monitoring the transit time of goods to be transported
US7350394B1 (en) * 2004-12-03 2008-04-01 Maxtor Corporation Zero-g offset identification of an accelerometer employed in a hard disk drive
KR100777107B1 (en) * 2005-12-09 2007-11-19 한국전자통신연구원 apparatus and method for handwriting recognition using acceleration sensor
JP4950729B2 (en) * 2007-03-28 2012-06-13 本田技研工業株式会社 Zero correction device for vehicle angular velocity sensor
JP5625557B2 (en) * 2010-07-08 2014-11-19 セイコーエプソン株式会社 Stop judgment method
JP5935617B2 (en) * 2012-09-14 2016-06-15 オムロン株式会社 Image processing apparatus, moving body state determination method, and moving body state determination program

Also Published As

Publication number Publication date
JP2016085104A (en) 2016-05-19

Similar Documents

Publication Publication Date Title
JP3947531B2 (en) Acceleration error correction method and apparatus, and inertial navigation system using the same
JP5198514B2 (en) Robot controller
US10101174B2 (en) Air data aided inertial measurement unit
JP6922641B2 (en) Angular velocity derivation device and angular velocity derivation method
US20080249732A1 (en) System, method and medium calibrating gyrosensors of mobile robots
KR101106048B1 (en) Method for calibrating sensor errors automatically during operation, and inertial navigation using the same
AU2015305864B2 (en) Earthmoving machine comprising weighted state estimator
US20190025056A1 (en) Electrostatic offset correction
JP2011017688A (en) Angle measuring method and angle measuring gyro system for executing the same
EP2930467A1 (en) A system and method for sensing the inclination of a moving platform with respect to gravity
JP2015087388A (en) Signal error compensation for magnetometer in sensor package
EP3527948A1 (en) Air data aided inertial measurement unit
JP6604175B2 (en) Pitch angular velocity correction value calculation device, attitude angle calculation device, and pitch angular velocity correction value calculation method
JP6383907B2 (en) Vehicle position measuring apparatus and method
CN115244407B (en) Compensating for stress induced errors in accelerometers using MEMS gyroscopes
JP6282570B2 (en) Zero point deviation correction system, zero point deviation correction apparatus, and zero point deviation correction method for correcting zero point deviation of inertial sensor
JP7406390B2 (en) Calibration device and its control method
US20140088906A1 (en) Inertial Sensor Bias Estimation by Flipping
JP6653479B2 (en) Inertial force sensor
CN114964214B (en) Extended Kalman filtering attitude calculation method of attitude heading reference system
KR101140379B1 (en) Method and apparatus for estimating orientation by using lie algebra and kalman filter
CN105829830A (en) Information processing device, information processing method, and computer program product
TWI680382B (en) Electronic device and pose calibration method thereof
JP6859917B2 (en) Angular velocity derivation device and angular velocity derivation method
US20200363208A1 (en) Factory-specific inertial measurement unit error model

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20170417

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20180118

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

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20180124

R150 Certificate of patent or registration of utility model

Ref document number: 6282570

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150