JP2006153662A - Detection method and detector for inclination angle and translational acceleration - Google Patents

Detection method and detector for inclination angle and translational acceleration Download PDF

Info

Publication number
JP2006153662A
JP2006153662A JP2004344703A JP2004344703A JP2006153662A JP 2006153662 A JP2006153662 A JP 2006153662A JP 2004344703 A JP2004344703 A JP 2004344703A JP 2004344703 A JP2004344703 A JP 2004344703A JP 2006153662 A JP2006153662 A JP 2006153662A
Authority
JP
Japan
Prior art keywords
acceleration
inclination angle
detector
axis
values
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
JP2004344703A
Other languages
Japanese (ja)
Other versions
JP3783061B1 (en
Inventor
Yukio Hashimoto
幸男 橋本
Mitsuhisa Yamashita
光久 山下
Naohiko Hanashima
直彦 花島
Hiromitsu Hikita
弘光 疋田
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.)
Muroran Institute of Technology NUC
Original Assignee
Muroran Institute of Technology NUC
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 Muroran Institute of Technology NUC filed Critical Muroran Institute of Technology NUC
Priority to JP2004344703A priority Critical patent/JP3783061B1/en
Application granted granted Critical
Publication of JP3783061B1 publication Critical patent/JP3783061B1/en
Publication of JP2006153662A publication Critical patent/JP2006153662A/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Gyroscopes (AREA)

Abstract

<P>PROBLEM TO BE SOLVED: To simultaneously detect an inclination angle of an object and translational acceleration acting on the object without errors, with satisfactory responsivity. <P>SOLUTION: An inclination angle is known by performing two-stage time differentiation of an inclination angle from acceleration components in four directions in a vertical plane and geometrical relations between them, and digitally integrating two-stage time differential values of an inclination angle found from measurement values. A checking acceleration detector for checking is placed in the vertical plane, in order to avoid initial error or cumulative error due to approximation. The values of the inclination angle found by calculation, translational acceleration, and angular velocity, are used to composit an output value. The values of inclination angles at respective sample points are adjusted so that the output value agrees with the output value of the acceleration detector. Even in the process of acceleration motion, can not only the inclination angles be known with required accuracy, but also the values of vertical and horizontal direction acceleration acting on the acceleration detector can be severally found in a separated manner from each other. <P>COPYRIGHT: (C)2006,JPO&NCIPI

Description

本発明は、動揺や振動を伴う物体、構造物、車輌や船舶などの移動体に設置または搭載され、その傾斜角と加速度を分離して計測する方法に関する。具体的には、複数の加速度検出器を使用して運動中に変化する傾斜角と、その物体に働く鉛直面内の並進の加速度とを同時に検出する方法および検出装置に関する。   The present invention relates to a method of measuring by separately installing an inclination angle and an acceleration of an object, a structure, a moving body such as a vehicle or a ship, which are accompanied by shaking and vibration, and mounting them. Specifically, the present invention relates to a method and a detection device that simultaneously detect an inclination angle that changes during movement and translational acceleration in a vertical plane that acts on the object using a plurality of acceleration detectors.

重力加速度のように定常的に働く加速度も検出できる加速度検出装置は、静的な状況下では重力加速度の方向を検出する傾斜角検出装置として利用できるが、検出装置が移動体に設置されて加速度運動をする場合には、被検出物の運動に伴う検出装置自体の並進および回転の加速度も同時に検出してしまう。したがって、運動中も含めて正しい傾斜角を求めるには、加速度検出器の出力から重力加速度成分とそれ以外の加速度成分とを分離して取り出す必要がある。   Acceleration detectors that can detect steady-state accelerations such as gravitational acceleration can be used as tilt angle detectors that detect the direction of gravitational acceleration under static conditions. In the case of movement, translational and rotational accelerations of the detection apparatus itself accompanying the movement of the object to be detected are also detected at the same time. Therefore, in order to obtain a correct tilt angle including during exercise, it is necessary to separate and extract the gravitational acceleration component and other acceleration components from the output of the acceleration detector.

特許文献1には、機械的な振子を使用し、本体に作用する2方向の加速度と、振子と本体の相対角度を検出し、検出された加速度を振子の運動方程式に適用して振子の角度を求め、この振子の角度から相対角度を引くことにより、運動中の本体の傾斜角を算出する方法が示されているが、信頼性と小型化という観点からは振子のような機械的な可動部をもたない構造の装置である方が望ましい。   In Patent Document 1, a mechanical pendulum is used, two-direction acceleration acting on the main body and a relative angle between the pendulum and the main body are detected, and the detected acceleration is applied to the motion equation of the pendulum to detect the pendulum angle. The method of calculating the tilt angle of the moving body by subtracting the relative angle from the pendulum angle is shown, but from the viewpoint of reliability and miniaturization, it is mechanically movable like a pendulum. It is desirable that the device has a structure having no part.

このような観点から近年は、半導体加速度センサと加速度運動中の回転角度を推定するためのジャイロセンサとを組み合わせた装置が考案された。しかし、レートジャイロセンサや圧電振動ジャイロセンサには角速度が零のときでも出力値が緩やかに変化し続けるゼロ点ドリフトの問題があるので、これに対処しなければ実用的なシステムとはならない。   From such a viewpoint, in recent years, an apparatus that combines a semiconductor acceleration sensor and a gyro sensor for estimating a rotation angle during acceleration motion has been devised. However, since the rate gyro sensor and the piezoelectric vibration gyro sensor have a problem of the zero point drift in which the output value continues to change gradually even when the angular velocity is zero, a practical system cannot be obtained unless this is addressed.

特許文献2では、観測周期ごとに圧電振動ジャイロセンサの出力値の差分をとることでゼロ点ドリフトの影響から逃れる方法を提案している。装置自体は姿勢角(3次元空間内における傾斜状態)を求めることができるが、圧電振動ジャイロセンサの出力は加速度運動中の姿勢角を求める目的で利用されるのではなく、静的な状態の姿勢角を求める目的で使われている。   Patent Document 2 proposes a method of escaping from the influence of zero point drift by taking the difference between the output values of the piezoelectric vibration gyro sensor for each observation period. The device itself can determine the posture angle (tilt state in the three-dimensional space), but the output of the piezoelectric vibration gyro sensor is not used for the purpose of determining the posture angle during acceleration motion, but in a static state. It is used for the purpose of finding the posture angle.

圧電振動ジャイロセンサの出力を高域通過フィルタに通してドリフト成分を除去する方法も考えられている。例えば、特許文献3には、高域通過フィルタを通した後の信号を積分することを基本にした誤差の少ない回転角度算出法が開示されており、この方法を適用して回転運動中は振動ジャイロの信号から求めた傾斜角を出力し、静止あるいは殆ど静止していると見なせるときには加速度センサの信号から求めた傾斜角を出力する1軸の傾斜角検出器も製品化されている(非特許文献1参照)。
この方法では高域通過フィルタの特性上、低い周波数成分ほど角速度情報を多く捨てることになるので、加速度検出器の信号を用いた出力に切換える前の低域における傾斜角の検出精度は悪いものとなる。また、回転の角速度がある程度大きくなり、圧電振動ジャイロセンサの信号を用いた出力に切換える直前の周波数域での加速度センサ信号による姿勢角の精度も回転の加速度が重畳されている分だけ悪くなる。
A method of removing the drift component by passing the output of the piezoelectric vibration gyro sensor through a high-pass filter is also considered. For example, Patent Document 3 discloses a rotation angle calculation method with less error based on integrating a signal after passing through a high-pass filter. A single-axis tilt angle detector that outputs the tilt angle obtained from the gyro signal and outputs the tilt angle obtained from the acceleration sensor signal when it can be regarded as stationary or almost stationary has also been commercialized (non-patented). Reference 1).
In this method, due to the characteristics of the high-pass filter, the lower the frequency component, the more the angular velocity information is discarded, so the detection accuracy of the tilt angle in the low band before switching to the output using the signal of the acceleration detector is poor. Become. Moreover, the angular velocity of rotation increases to some extent, and the accuracy of the attitude angle by the acceleration sensor signal in the frequency range immediately before switching to the output using the signal of the piezoelectric vibration gyro sensor is also deteriorated by the amount of the rotation acceleration superimposed.

高域通過フィルタを通して取り出した圧電振動ジャイロセンサの出力を使って傾斜角を求める方法は、高域では正しい再現値を与え、加速度検出器の出力を使って傾斜角を求める方法は低域ほど正しい値を示すという相補的な特徴が見られる。非特許文献2では、このような相補的特徴に着目して、液体傾斜計と圧電振動ジャイロセンサに対して、Complementary Filter(非特許文献3)を構成して傾斜角を求めるという方法を提案しており、液体傾斜計を加速度検出器に置換しても、同様の効果が得られる。   The method of obtaining the tilt angle using the output of the piezoelectric vibration gyro sensor taken out through the high-pass filter gives a correct reproduction value in the high region, and the method of obtaining the tilt angle using the output of the acceleration detector is correct in the lower region. There is a complementary feature that shows the value. In Non-Patent Document 2, focusing on such complementary features, a method of obtaining a tilt angle by configuring a Complementary Filter (Non-Patent Document 3) for a liquid inclinometer and a piezoelectric vibration gyro sensor is proposed. Even if the liquid inclinometer is replaced with an acceleration detector, the same effect can be obtained.

Complementary Filterを構成する方法は、ジャイロセンサのゼロ点ドリフトの問題を解決するだけでなく、静止状態から必要とする帯域までの動的な傾斜角を、同一の理論を使って検出できる点で魅力的であるが、液体傾斜計、あるいは、加速度検出器の出力に重畳する重力以外の加速度の影響を定量的に取り扱うことはできないので、並進の加速度が働くような状況下では傾斜角の検出値に誤差が生じる欠点がある。
この欠点は特許文献2の方法でも特許文献3の方法でも、あるいは、Kalmanフィルタを利用することによって3次元の動的な姿勢角を検出可能とした特許文献4の方法でも解決されてはいない。
The Complementary Filter is attractive because it not only solves the zero drift problem of the gyro sensor, but also detects the dynamic tilt angle from the stationary state to the required band using the same theory. However, since the influence of acceleration other than gravity superimposed on the output of a liquid inclinometer or accelerometer cannot be handled quantitatively, the detected value of the tilt angle under circumstances where translational acceleration works. There is a drawback that an error occurs.
This drawback is not solved by the method of Patent Document 2, the method of Patent Document 3, or the method of Patent Document 4 in which a three-dimensional dynamic posture angle can be detected by using a Kalman filter.

特開2004−219079号公報JP 2004-219079 A 特開2004−117139号公報JP 2004-117139 A 特開平10−132849号公報JP 10-132849 A 米国特許第6647352号明細書US Pat. No. 6,647,352 「SENSORカタログ VRセンサ/1軸角度センサ/3軸角度センサ」,株式会社データテック,2004年,p2“SENSOR catalog VR sensor / 1-axis angle sensor / 3-axis angle sensor”, Datatech Co., Ltd., 2004, p2 Baerveldt A.J.,R.Klang: A Low−cost and Low−weight Attitude Estimation System for an Autonomous Helicopter,Proc. of the IEEE Int.Conf. on Intelligent Engineering Systems,INES‘97,Budapest,Hungary,September 1997Baerveldt A.M. J. et al. , R. Klang: A Low-cost and Low-weight Attitudinal Estimation System for an Autonomous Helicocopter, Proc. of the IEEE Int. Conf. on Intelligent Engineering Systems, INE'97, Budapest, Hungary, September 1997 M.Zimmermann and W.Sulzer:High Bandwidth Orientation Measurement and Control Based on Complementary Filtering,Proc.of SYROCO‘91,IFAC Symposium on Robot Control,Vienna,Austria,September 1991M.M. Zimmermann and W.M. Sulzer: High Bandwidth Orientation Measurement and Control Based on Complementary Filtering, Proc. of SYROCO '91, IFAC Symposium on Robot Control, Vienna, Austria, September 1991

以上のように、物体に働く並進の加速度を加速度検出器の出力から分離して取り出すことは、傾斜角の測定精度を上げることができるだけでなく、慣性航行の加速度データとして使用することも可能となる。特に、自動車や船舶、自律移動ロボットなどの慣性航行では低周波域の微小な加速度の検出精度が移動位置の計算精度を決めるので、低周波域も含めた並進の加速度を精度良く取り出す要求が大きく、本発明は、複数の加速度検出器を用いて、物体の傾斜角と物体に働く並進の加速度とを応答性よく、かつ、誤差なく同時に検出できる廉価な加速度検出装置を提供するものである。   As described above, separating and extracting the translational acceleration acting on the object from the output of the acceleration detector can not only increase the measurement accuracy of the inclination angle, but can also be used as acceleration data for inertial navigation. Become. In particular, in inertial navigation of automobiles, ships, autonomous mobile robots, etc., the detection accuracy of minute accelerations in the low frequency range determines the calculation accuracy of the moving position, so there is a great demand for accurately extracting translational acceleration including the low frequency range. The present invention provides an inexpensive acceleration detection apparatus that can detect the inclination angle of an object and the translational acceleration acting on the object at the same time with high responsiveness and without error by using a plurality of acceleration detectors.

感受軸を交差配置した加速度検出器を検出器の大きさより十分大きな間隔をあけて配置し、この二つの加速度検出器の出力から傾斜角を求め、2階時間微分値を時間で積分することにより傾斜角と並進加速度とを分離して検出するものであり、応答性よく、かつ、誤差なく傾斜角と加速度を同時に検出することができる。
感受軸を交差配置した加速度検出器を検出器の大きさより十分大きな間隔をあけて配置してあり、この二つの検出器の出力から傾斜角と並進加速度とを分離する演算装置からなる加速度検出装置である。
また、他の加速度感受軸を設けて加速度検出器の検出値の補正ができるようにし、更には、加速度検出器の温度補償装置を設けて環境温度による加速度検出装置の出力変化を補償できるようにした。
By placing an acceleration detector with the sensing axis crossed at a distance sufficiently larger than the size of the detector, obtaining the tilt angle from the output of these two acceleration detectors, and integrating the second-order time differential value over time The tilt angle and the translational acceleration are detected separately, and the tilt angle and the acceleration can be detected at the same time with good responsiveness and without error.
An acceleration detector comprising an arithmetic unit that separates an inclination angle and a translational acceleration from outputs of the two detectors, in which an acceleration detector having a crossing arrangement of sensitive axes is arranged with a sufficiently larger interval than the size of the detector. It is.
In addition, another acceleration sensing axis is provided so that the detection value of the acceleration detector can be corrected, and furthermore, a temperature compensation device for the acceleration detector is provided to compensate for an output change of the acceleration detection device due to the environmental temperature. did.

図1に示すように、加速度検出器は、二つの加速度感受軸1、2または3、4を同一平面内で交差させて配置したものであり、加速度感受軸を含む面内で距離Lだけ平行移動した位置P、Qにおいて剛体に取り付けてある。
あるいは、平行な二平面上に別方向を向けた加速度感受軸を交差させたものでもよい。
As shown in FIG. 1, the acceleration detector has two acceleration sensing axes 1, 2 or 3, 4 arranged so as to intersect in the same plane, and is parallel by a distance L in a plane including the acceleration sensing axes. It is attached to the rigid body at the moved positions P and Q.
Alternatively, an acceleration sensing axis directed in a different direction on two parallel planes may be crossed.

二組の加速度感受軸1、2および3、4の取付位置である点P、Qを結ぶ線が取付軸6であり、取付軸6と加速度感受軸1、2とのなす角度は、φ1、φ2である。また、2方向の速度検出軸をそれぞれX軸、Y軸とする。X軸とY軸に平行な面でかつ取付軸6を含む平面を取付面と呼ぶ。なお、加速度検出部の寸法は、距離Lに比較して十分小さいものであり、点P、Qにおける加速度感受軸であるX軸方向およびY軸方向の加速度がそれぞれ検出できるように配置したものである。   The line connecting the points P and Q, which are the mounting positions of the two sets of acceleration sensing shafts 1, 2 and 3, 4 is the mounting shaft 6, and the angle between the mounting shaft 6 and the acceleration sensing shafts 1 and 2 is φ1, φ2. In addition, the speed detection axes in two directions are assumed to be an X axis and a Y axis, respectively. A plane parallel to the X-axis and the Y-axis and including the mounting shaft 6 is called a mounting surface. Note that the dimensions of the acceleration detection unit are sufficiently small compared to the distance L, and are arranged so that the accelerations in the X axis direction and the Y axis direction, which are acceleration sensing axes at points P and Q, can be detected. is there.

図2に示すように、鉛直軸7が取付面に含まれるときの鉛直軸7と取付軸6とのなす傾斜角をθ、取付面内のx−y直交座標系8のy軸を鉛直軸7と平行なものとし、点Pの座標を(x,y)で表すと、点Qの座標は(x+Lsinθ,y+Lcosθ)となる。
したがって、この基本ユニットが重力加速度(g)の下で取付面に沿って加速度運動をする場合は、点P、Qに作用する加速度はそれぞれ式(1)、および式(2)で表すことができる(図3参照)。
As shown in FIG. 2, when the vertical axis 7 is included in the mounting surface, the inclination angle between the vertical axis 7 and the mounting shaft 6 is θ, and the y axis of the xy orthogonal coordinate system 8 in the mounting surface is the vertical axis. 7 and the coordinates of the point P are represented by (x, y), the coordinates of the point Q are (x + Lsin θ, y + L cos θ).
Therefore, when this basic unit performs acceleration motion along the mounting surface under the gravitational acceleration (g), the acceleration acting on the points P and Q can be expressed by the equations (1) and (2), respectively. Yes (see FIG. 3).

Figure 2006153662
Figure 2006153662

これらの加速度のX軸射影、Y軸射影が加速度検出器の出力になるので、点PでのX軸方向及びY軸方向の加速度はそれぞれ式(3)、式(4)で表される。

Figure 2006153662
Since the X-axis projection and Y-axis projection of these accelerations become the output of the acceleration detector, the acceleration in the X-axis direction and the Y-axis direction at the point P is expressed by Expression (3) and Expression (4), respectively.
Figure 2006153662

同様に、点QにおけるX軸方向、Y軸方向の加速度は、それぞれ式(5)、式(6)で求められる。

Figure 2006153662
Similarly, the acceleration in the X-axis direction and the Y-axis direction at the point Q can be obtained by Expression (5) and Expression (6), respectively.
Figure 2006153662

式(3)〜式(6)より、傾斜角θに関する式(7)および式(8)の2本の式が導かれる。

Figure 2006153662
From Expressions (3) to (6), two expressions of Expression (7) and Expression (8) regarding the inclination angle θ are derived.
Figure 2006153662

更に、式(3)と式(4)より、水平方向の加速度と鉛直方向の加速度は式(9)および式(10)によってそれぞれ求めることができる。

Figure 2006153662
Furthermore, from the equations (3) and (4), the horizontal acceleration and the vertical acceleration can be obtained by the equations (9) and (10), respectively.
Figure 2006153662

水平方向の加速度と鉛直方向の加速度は、各加速度検出軸の出力値の観測と傾斜角から計算で求めることができる量であり、傾斜角は加速度検出軸の出力値できまる式(7)を時間で積分することによって求めることができるので、傾斜角と並進の加速度が共に得られるのである。   The horizontal acceleration and the vertical acceleration are quantities that can be calculated from the observation of the output value of each acceleration detection axis and the inclination angle, and the inclination angle can be calculated from the output value of the acceleration detection axis (7). Since it can be obtained by integration over time, both the tilt angle and the translational acceleration can be obtained.

以上のように、本発明によれば、並進加速度と傾斜角を分離して求めることができるが、式(7)の時間積分をそのまま実行すると、実世界での初期条件と積分で使用する初期条件が異なった場合、傾斜角に誤差が生じる。更に、積分を離散積分公式で置き換えて実施するような場合には初期値誤差の他に積分の近似誤差も累積することになり、正確な傾斜角を得ることができない。したがって、式(7)を積分して正確な傾斜角を得るには、実世界の観測値と比較して絶えず真の傾斜角を与えるような修正ループが必要になる。   As described above, according to the present invention, the translational acceleration and the tilt angle can be obtained separately. However, if the time integration in Expression (7) is performed as it is, the initial conditions used in the real world and the initial integration are used. If the conditions are different, an error occurs in the tilt angle. Further, when the integration is performed by replacing with a discrete integration formula, an approximation error of the integration is accumulated in addition to the initial value error, and an accurate inclination angle cannot be obtained. Therefore, in order to integrate the equation (7) to obtain an accurate tilt angle, a correction loop that constantly gives a true tilt angle as compared with the observed value in the real world is required.

図3に示すように、PQの中央の位置Rに加速度感受軸が取付軸に垂直でかつ取付面内に含まれる加速度検出器5を固定し、加速度検出器5の出力値を真の傾斜角を求めるための修正ループを形成するのに利用する。
R点に働く加速度から加速度検出器5における出力値を求めると、式(11)を得る。

Figure 2006153662
As shown in FIG. 3, the acceleration detector 5 whose acceleration sensing axis is perpendicular to the mounting axis and included in the mounting surface is fixed at the center position R of the PQ, and the output value of the acceleration detector 5 is changed to the true inclination angle. This is used to form a correction loop for obtaining.
When the output value in the acceleration detector 5 is obtained from the acceleration acting on the R point, the equation (11) is obtained.
Figure 2006153662

式(11)に式(7)〜(10)を代入して整理すると式(12)が導かれるので、両辺の差をとり、2乗したものを式(13)と定義する。

Figure 2006153662
Substituting Equations (7) to (10) into Equation (11) leads to Equation (12). Therefore, the difference between both sides is taken and squared is defined as Equation (13).
Figure 2006153662

積分により求められた傾斜角θが真の値であるときには、式(13)はゼロの値をとり、それ以外では正の値となる。したがって、傾斜角を求める積分を行う場合には、適切な初期傾斜角とその時間微分の初期値を与えて式(7)の積分を行い、求められた傾斜角θを式(13)に代入して要求精度を満たすほどゼロに近いかどうかを判定する。要求精度を満たしていない場合は、積分で求められた傾斜角θを初期値として要求精度を満たすまで式(13)を最小化するθを探索し、最終的に求められたθの値を傾斜角とする。つぎの傾斜角を求める積分の初期値にも式(13)を最小化するこの傾斜角θの値を使い、同様のプロセスを繰り返すことにより、精度の保証された傾斜角を得ることができる。なお、この繰り返しプロセスではθの増分からθの時間微分値(角速度)符号を知ることができるので、θの時間微分値の初期値には式(8)で求めた値を使用する。   When the inclination angle θ obtained by integration is a true value, the expression (13) takes a value of zero, and otherwise becomes a positive value. Therefore, when integration for obtaining the tilt angle is performed, an appropriate initial tilt angle and an initial value of its time derivative are given to perform integration of equation (7), and the obtained tilt angle θ is substituted into equation (13). Then, it is determined whether or not it is close to zero to satisfy the required accuracy. If the required accuracy is not satisfied, an inclination angle θ obtained by integration is used as an initial value to search for θ that minimizes Equation (13) until the required accuracy is satisfied, and the finally obtained θ value is inclined. A corner. By using the value of the inclination angle θ that minimizes the equation (13) as the initial value of the integration for obtaining the next inclination angle, the inclination angle with a guaranteed accuracy can be obtained by repeating the same process. In this iterative process, since the time differential value (angular velocity) sign of θ can be known from the increment of θ, the value obtained by equation (8) is used as the initial value of the time differential value of θ.

実施例
加速度検出器は、アナログデバイセス社ADXL311EBなどの小型で廉価な静電容量型2軸半導体加速度センサを使用した。
図4に示すように、点Pと点Qの位置に加速度検出器を1個ずつ配置し、それぞれの加速度感受軸(1、2および3、4)が取付軸6と軸対称(φ1=π/4、φ2=π/4)になるように向きを揃えて固定してある。また、点PQの中点Rの位置に、検算用の加速度検出器5をその加速度感受軸が取付軸6と直交する向きに固定してある。
更に、半導体加速度センサの出力は環境温度の影響を受けやすいので、環境温度補償モニタ用として温度センサ9を加速度検出器を設置した点P,Qの近くに固定してある。
Example As the acceleration detector, a small and inexpensive electrostatic capacitance type two-axis semiconductor acceleration sensor such as ADXL311EB manufactured by Analog Devices was used.
As shown in FIG. 4, one acceleration detector is arranged at each of the points P and Q, and each acceleration sensing axis (1, 2, and 3, 4) is symmetric with respect to the mounting axis 6 (φ1 = π / 4, φ2 = π / 4). Further, at the position of the middle point R of the point PQ, the acceleration detector 5 for verification is fixed so that its acceleration sensing axis is orthogonal to the mounting axis 6.
Further, since the output of the semiconductor acceleration sensor is easily affected by the environmental temperature, the temperature sensor 9 is fixed near the points P and Q where the acceleration detector is installed for monitoring the environmental temperature compensation.

アナログ信号で出力される加速度検出器のデータと温度センサ9のデータをサンプリングし、傾斜角、水平方向加速度、鉛直方向加速度を演算装置でそれぞれ求めて結果を出力する。
演算手段は、A/D変換器、PWMタイマカウンタモジュール、およびシリアルデータ通信モジュールを内蔵するワンチップマイコン(ルネサスのH8−3664F)を用いる。
The data of the acceleration detector and the data of the temperature sensor 9 output as analog signals are sampled, the inclination angle, the horizontal acceleration, and the vertical acceleration are obtained by an arithmetic device, and the results are output.
The arithmetic means uses a one-chip microcomputer (Renesas H8-3664F) incorporating an A / D converter, a PWM timer counter module, and a serial data communication module.

式(7)に基づいて傾斜角の近似値を求める近似積分計算の一例として、観測周期をΔTにとる式(14)を示す。

Figure 2006153662
ここに、kは時系列のkステージを表す。 As an example of the approximate integration calculation for obtaining the approximate value of the tilt angle based on Expression (7), Expression (14) in which the observation period is ΔT is shown.
Figure 2006153662
Here, k represents a time-series k stage.

P点、Q点に固定した加速度検出器の出力から求めた各軸のkステージ加速度式(15)とkステージ用に作成された初期値式(16)を使い、式(14)の計算を実行して傾斜角とその時間変化式(17)を予測する。初期値式(18)には、k−1ステージで求められた予測値式(19)を探索初期値として使い、式(20)を最小化する値を用いる。最小化する値を見つけるには、例えば2分法などが利用できる。式(20)を最小化する式(19)、すなわち、式(18)はこの時点での傾斜角である。   Using the k-stage acceleration equation (15) of each axis obtained from the output of the acceleration detector fixed at the P point and the Q point, and the initial value equation (16) created for the k stage, the calculation of the equation (14) is performed. This is executed to predict the tilt angle and its time change equation (17). In the initial value formula (18), the predicted value formula (19) obtained in the k-1 stage is used as a search initial value, and a value that minimizes the formula (20) is used. In order to find a value to be minimized, for example, a bisection method can be used. Equation (19) that minimizes Equation (20), that is, Equation (18) is the tilt angle at this point.

Figure 2006153662
Figure 2006153662

式(21)の初期値には、k−1ステージで求めた式(22)による予測値符号とk−2ステージ、k−1ステージでの式(18)の増分の符号を調べ、共に符号が一致しているときには式(23)の符号としてその符号をもつ式(23)の値を採用する。符号に不一致が生じた場合には、θ(k)の時間微分の値が小さいものとして式(21)をゼロとおく。

Figure 2006153662
For the initial value of equation (21), the prediction value sign obtained by equation (22) obtained at the k-1 stage and the sign of the increment of equation (18) at the k-2 stage and k-1 stage are checked, and both are signed. Are equal to each other, the value of Expression (23) having the sign is adopted as the sign of Expression (23). If there is a discrepancy between the signs, Equation (21) is set to zero assuming that the value of time differentiation of θ (k) is small.
Figure 2006153662

この初期値の設定法では、k=0およびk=1での式(21)の値とk=0での式(19)が定まらず、したがって、これらの値をゼロとおいて上に述べた繰り返しアルゴリズムを実行せざるを得ないが、このような条件からスタートしても、式(20)の最小化で正しい傾斜角が求められていくので、実用上は問題が生じない。   In this initial value setting method, the value of the equation (21) at k = 0 and k = 1 and the equation (19) at k = 0 are not determined. Therefore, these values are set to zero and are described above. Although it is necessary to execute an iterative algorithm, even if it starts from such a condition, a correct inclination angle is obtained by minimizing the equation (20), so that there is no problem in practical use.

図5は、この近似積分を使用した傾斜角と並進の加速度の算出アルゴリズムを示したもので、Bと表されているブロックは式(14)の右辺第二項目の行列を指す。右側の矢印の上に描かれている式(18)が傾斜角の値であり、水平方向加速度と鉛直方向加速度はそれぞれ式(24)、式(25)の計算を実行して求める。   FIG. 5 shows an algorithm for calculating the tilt angle and the translational acceleration using this approximate integration, and the block represented by B indicates the matrix of the second item on the right side of Equation (14). Expression (18) drawn on the right arrow is the value of the inclination angle, and the horizontal acceleration and the vertical acceleration are obtained by executing calculations of Expression (24) and Expression (25), respectively.

Figure 2006153662
Figure 2006153662

前述のように、加速度検出器の出力値は、環境温度の影響を受ける。また、加速度検出器の取付誤差は傾斜角や並進加速度に誤った結果を与える。図6はこのこのような誤差の原因を避けるために導入する加速度信号の前置演算部のブロック図である。センサアレイ10から出力される全てのアナログ信号は、高周波ノイズを除去するため、高域遮断フィルタ11を通して取り出し、スケール調整部12でスケール校正用データテーブル13を参照してスケール変換を行う。スケール校正用データテーブル13には単位換算データだけでなく、加速度検出器の取付誤差を補償する校正データもテーブルとして持つ。このようにしてスケール調整されたデータは、温度補正部14で温度補正用データテーブル15のデータを使って温度補正され、傾斜角と並進加速度を求めるブロック16に入る。ブロック16の処理は図5に示した処理である。   As described above, the output value of the acceleration detector is affected by the environmental temperature. In addition, the acceleration detector mounting error gives an incorrect result to the tilt angle and translational acceleration. FIG. 6 is a block diagram of a pre-calculation unit for acceleration signals to be introduced in order to avoid such a cause of error. All analog signals output from the sensor array 10 are extracted through the high-frequency cutoff filter 11 to remove high-frequency noise, and the scale adjustment unit 12 performs scale conversion with reference to the scale calibration data table 13. The scale calibration data table 13 has not only unit conversion data but also calibration data for compensating for an acceleration detector mounting error as a table. The scale-adjusted data is temperature-corrected by the temperature correction unit 14 using the data of the temperature correction data table 15 and enters the block 16 for obtaining the tilt angle and the translational acceleration. The processing of block 16 is the processing shown in FIG.

本発明のハードウェアの構成の一例を図7に示す。高域遮断フィルタ11は、アナログ回路で構成したフィルタで、ノイズの軽減と同時にアンチエリアスフィルタの役目を果たす。各種のアナログ信号は観測周期に同期させたA/D変換器18で読み取り、マイクロプロセッサ19で、スケール調整、温度補正、傾斜角の計算および並進加速度の計算を行う。様々な利用形態を考慮し、傾斜角と並進加速度の計算値はアナログ量で出力する他、RS−232C等のシリアルデータ出力の機能も持たせる。ルネサス社のH8−3664Fマイクロプロセッサは、A/D変換器8チャンネル、PWM出力3チャンネル、シリアルインターフェースモジュール1を内蔵しているので、図7のD/A変換器20による機能をPWMのデューティ比を使ったD/A変換に置き換えると、破線で囲ったブロック17の部分がH8−3664F1個で実現できることになり、小型化が図れる。更に、H8−3664F内蔵のEEPROMは、装置固有の校正データを記録保存するものである。
なお、加速度感受軸を同一平面内で交差させて配置したものに基づいて本発明を具体的に説明したが、それ以外の配列を行っても同様に傾斜角と加速度を分離することが可能であり、二軸、三軸などの複数軸の加速度の測定に適用できることはいうまでもない。
An example of the hardware configuration of the present invention is shown in FIG. The high-frequency cutoff filter 11 is a filter composed of an analog circuit and serves as an anti-alias filter at the same time as reducing noise. Various analog signals are read by the A / D converter 18 synchronized with the observation period, and the microprocessor 19 performs scale adjustment, temperature correction, inclination angle calculation, and translational acceleration calculation. In consideration of various usage forms, the calculated values of the tilt angle and the translational acceleration are output in analog quantities, and a serial data output function such as RS-232C is provided. Since the Renesas H8-3664F microprocessor incorporates 8 channels of A / D converter, 3 channels of PWM output, and serial interface module 1, the function of the D / A converter 20 of FIG. If it is replaced with D / A conversion using, the portion of the block 17 surrounded by a broken line can be realized with one H8-3664F1, and the size can be reduced. Further, the EEPROM built in the H8-3664F records and saves calibration data unique to the apparatus.
Although the present invention has been specifically described based on the acceleration sensing axes arranged so as to intersect in the same plane, the tilt angle and the acceleration can be similarly separated even if other arrangements are performed. Needless to say, the present invention can be applied to measurement of accelerations of a plurality of axes such as two axes and three axes.

本発明は可動部を有しない小型の検出装置であって傾斜角と並進加速度を分離して検出することができるので、建設機械・クレーン等の揺れ検出、または、吊り荷の傾斜角測定、車輌搭載の各種プラットフォームの角度制御、ロボットの姿勢制御など、応答性のよい傾斜角センサが必要とされている分野、並びに自律移動車等の慣性航行制御やGPS信号が受信できない場所でのカーナビゲーション、水中ロボットの慣性航行制御など、傾斜角と加速度の計測が同時に必要とされる技術分野に適用することができる。   The present invention is a small detection device having no moving part, and can detect the inclination angle and the translational acceleration separately. Therefore, the present invention can detect a swing of a construction machine, a crane, etc., or measure the inclination angle of a suspended load, vehicle Car navigation in areas where responsive tilt angle sensors are required, such as angle control of various on-board platforms and robot attitude control, as well as inertial navigation control such as autonomous mobile vehicles and places where GPS signals cannot be received, It can be applied to technical fields that require simultaneous measurement of tilt angle and acceleration, such as inertial navigation control for underwater robots.

本発明の基本概念図。The basic conceptual diagram of this invention. 傾斜角と座標の定義の説明図。Explanatory drawing of the definition of an inclination angle and coordinates. 検算用加速度検出軸の配置の説明図。Explanatory drawing of arrangement | positioning of the acceleration detection axis for a check. 加速度検出器の配置の実施例の説明図。Explanatory drawing of the Example of arrangement | positioning of an acceleration detector. デジタル計算アルゴリズムの説明図。Explanatory drawing of a digital calculation algorithm. 前置処理のブロック図。The block diagram of pre-processing. 実施例のハードウェア構成図。The hardware block diagram of an Example.

符号の説明Explanation of symbols

P 計測用加速度検出器の取付位置
Q 計測用加速度検出器の取付位置
R 検算用加速度検出器の取付位置
1 加速度検出器のX軸方向加速度感受軸
2 加速度検出器のY軸方向加速度感受軸
3 加速度検出器のX軸方向加速度感受軸
4 加速度検出器のY軸方向加速度感受軸
5 検算用加速度検出器
6 加速度検出器の取付軸
7 鉛直軸
8 x−y直交座標系
9 温度センサ
10 センサアレイ
11 高域遮断フィルタ
12 スケール調整ブロック
13 スケール校正用データテーブル
14 温度補正ブロック
15 温度補正用データテーブル
16 傾斜角および並進加速度演算ブロック
17 ワンチップマイクロコンピュータ H8/3664F
18 A/D変換器
19 マイクロコンピュータの演算処理部
20 D/A変換器
21 シリアル通信インターフェース
P Measurement acceleration detector mounting position Q Measurement acceleration detector mounting position R Calculation acceleration detector mounting position 1 Acceleration detector X-axis direction acceleration sensing shaft 2 Acceleration detector Y-axis direction acceleration sensing shaft 3 Acceleration detector X-axis direction acceleration sensing axis 4 Acceleration detector Y-axis direction acceleration sensing axis 5 Acceleration detector 6 Acceleration detector mounting axis 7 Vertical axis 8 xy orthogonal coordinate system 9 Temperature sensor 10 Sensor array 11 High-frequency cutoff filter 12 Scale adjustment block 13 Scale calibration data table 14 Temperature correction block 15 Temperature correction data table 16 Inclination angle and translational acceleration calculation block 17 One-chip microcomputer H8 / 3664F
18 A / D converter 19 Arithmetic processing section 20 of microcomputer 20 D / A converter 21 Serial communication interface

Claims (5)

感受軸を交差配置した加速度検出器を間隔をあけて配置し、この二つの加速度検出器の出力から傾斜角と並進加速度とを分離して検出する加速度検出方法。 An acceleration detection method in which acceleration detectors having crossed sensing axes are arranged at intervals, and an inclination angle and a translational acceleration are separately detected from the outputs of the two acceleration detectors. 感受軸を交差配置した加速度検出器を間隔をあけて配置してあり、この二つの加速度検出器の出力から傾斜角と並進加速度とを分離する演算装置からなる加速度検出装置。 An acceleration detection device comprising an arithmetic unit in which acceleration detectors having crossed sensing axes are arranged at intervals, and the inclination angle and translational acceleration are separated from the outputs of the two acceleration detectors. 請求項2において、二つの加速度検出器の間隔は、加速度検出器の大きさより十分大きい加速度検出装置。 3. The acceleration detection device according to claim 2, wherein a distance between the two acceleration detectors is sufficiently larger than a size of the acceleration detector. 請求項2または3のいずれかにおいて、二つの加速度検出器を結ぶ取付軸に交差する感受軸を有する検算用の加速度検出器を設けた加速度検出装置。 4. The acceleration detection apparatus according to claim 2, further comprising a check acceleration sensor having a sensitive axis that intersects an attachment axis that connects the two acceleration detectors. 請求項2〜4のいずれかにおいて、加速度検出器の温度補償装置が設けてある加速度検出装置。 5. The acceleration detection device according to claim 2, wherein a temperature compensation device for the acceleration detector is provided.
JP2004344703A 2004-11-29 2004-11-29 Method and apparatus for detecting tilt angle and translational acceleration Active JP3783061B1 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2004344703A JP3783061B1 (en) 2004-11-29 2004-11-29 Method and apparatus for detecting tilt angle and translational acceleration

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2004344703A JP3783061B1 (en) 2004-11-29 2004-11-29 Method and apparatus for detecting tilt angle and translational acceleration

Publications (2)

Publication Number Publication Date
JP3783061B1 JP3783061B1 (en) 2006-06-07
JP2006153662A true JP2006153662A (en) 2006-06-15

Family

ID=36632151

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2004344703A Active JP3783061B1 (en) 2004-11-29 2004-11-29 Method and apparatus for detecting tilt angle and translational acceleration

Country Status (1)

Country Link
JP (1) JP3783061B1 (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100777404B1 (en) 2005-12-30 2007-11-19 재단법인서울대학교산학협력재단 Method and Apparatus for Estimation of Angular Velocity Using 2 Linear Acceleration Sensors
JP2010143506A (en) * 2008-12-22 2010-07-01 Koito Mfg Co Ltd Auto-leveling system for vehicular lamp

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103616013B (en) * 2013-12-18 2015-10-28 东南大学 A kind of rescue obstacles removing car pose estimation method
CN106840241B (en) * 2017-01-07 2019-06-28 广州博冠光电技术有限公司 The calibration method and calibration system of a kind of six axle sensor product of built-in MEMS

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100777404B1 (en) 2005-12-30 2007-11-19 재단법인서울대학교산학협력재단 Method and Apparatus for Estimation of Angular Velocity Using 2 Linear Acceleration Sensors
JP2010143506A (en) * 2008-12-22 2010-07-01 Koito Mfg Co Ltd Auto-leveling system for vehicular lamp

Also Published As

Publication number Publication date
JP3783061B1 (en) 2006-06-07

Similar Documents

Publication Publication Date Title
KR100227288B1 (en) Movement detecting device
CN107560613B (en) Robot indoor track tracking system and method based on nine-axis inertial sensor
CN102269592B (en) Sensor-based orientation system
EP1653194B1 (en) Azimuth/attitude detecting sensor
US8249800B2 (en) Method and apparatus to detect platform stationary status using three-axis accelerometer outputs
JP2904118B2 (en) Attitude angle detection apparatus and method
US20060185432A1 (en) Five degree of freedom intertial measurement device
Ariffin et al. Low cost MEMS gyroscope and accelerometer implementation without Kalman Filter for angle estimation
US20220252399A1 (en) Composite sensor and angular velocity correction method
US11125560B2 (en) Robust method for tuning of gyroscope demodulation phase
KR100525517B1 (en) Car navigation system and control method thereof
JP3380404B2 (en) Movement detection device
Lobo et al. Inertial navigation system for mobile land vehicles
Zhu et al. A small low-cost hybrid orientation system and its error analysis
CN108801253A (en) Robot builds figure positioning system and robot
CN117589163A (en) Multi-sensor combination navigation method and device
JP3783061B1 (en) Method and apparatus for detecting tilt angle and translational acceleration
CN109916399B (en) Carrier attitude estimation method under shadow
CN111141283A (en) Method for judging advancing direction through geomagnetic data
US11796318B2 (en) Rotation measurement system using Coriolis and Euler forces
US20230349699A1 (en) Absolute heading estimation with constrained motion
US9791277B2 (en) Apparatus and method for measuring velocity of moving object in a navigation system
KR100777404B1 (en) Method and Apparatus for Estimation of Angular Velocity Using 2 Linear Acceleration Sensors
Mumtaz et al. Development of a low cost wireless IMU using MEMS sensors for pedestrian navigation
CN113267183A (en) Combined navigation method of multi-accelerometer inertial navigation system

Legal Events

Date Code Title Description
TRDD Decision of grant or rejection written
R150 Certificate of patent or registration of utility model

Free format text: JAPANESE INTERMEDIATE CODE: R150