JPH08178687A - Posture detecting method, and device therefor - Google Patents

Abstract

PURPOSE: To provide a posture detecting method and a device therefor, in which the precise azimuth of a moving body can be detected, even when the posture of the moving body is inclined, without using a measuring instrument having an expensive movable part. CONSTITUTION: A geomagnetic sensor 2 is provided on a moving body to detect the temporary azimuth by the change of magnetic flux density of a geomagnetism passing in its own inner part. A vibration gyroscope 14 detects the roll angle of the moving body, and a vibration gyroscope 18 detects the pitch angle of the moving body. A software gimbal 3 determines a first correction quantity by the temporary azimuth in the pitching direction to the temporary azimuth by multiplying the pitch angle by a prescribed first coefficient, determines a second correction quantity by the roll angle to the temporary azimuth by multiplying the roll angle by a prescribed second coefficient, and corrects the temporary azimuth on the basis of the first correction quantity and the second correction quantity to generate an azimuth from which the disturbance of the posture of the moving body is removed.

Description

【発明の詳細な説明】Detailed Description of the Invention

【０００１】[0001]

【産業上の利用分野】本発明は、地磁気センサ、差動ジ
ャイロ、傾斜計等を用いて自動車、航空機等の移動体の
姿勢検出するための姿勢検出装置に関する。BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a posture detecting device for detecting the posture of a moving body such as an automobile or an aircraft by using a geomagnetic sensor, a differential gyro, an inclinometer or the like.

【０００２】[0002]

【従来の技術】一般に、自動車等の移動体の上で衛星放
送を受信するためには、常に自らの車体の向きを検出
し、アンテナの方向を衛星に追従させる必要がある。2. Description of the Related Art Generally, in order to receive a satellite broadcast on a moving body such as an automobile, it is necessary to always detect the direction of the vehicle body of the vehicle and make the direction of the antenna follow the satellite.

【０００３】これら移動体の姿勢状態を特定するのに、
図８に示すように、移動体の中心軸方向をｘ（ローリン
グ）軸、水平方向をｙ（ピッチング）軸、垂直方向をｚ
（ヨー）軸とする座標系と、ヨー軸の回りのヨーイング
する方向への偏位角（ヨー角（yaw ））、ピッチング軸
の回りのピッチングする方向への偏位角（ピッチ角（pi
tch ））、ローリング軸の回りのローリングする方向へ
の偏位角（ロール角（roll））という主要３角度を用い
る。In order to specify the posture state of these moving bodies,
As shown in FIG. 8, the central axis direction of the moving body is x (rolling) axis, the horizontal direction is y (pitching) axis, and the vertical direction is z.
The coordinate system used as the (yaw) axis, the deflection angle around the yaw axis in the yawing direction (yaw angle (yaw)), and the deflection angle around the pitching axis in the pitching direction (pitch angle (pi
tch)), and three main angles of deflection angle (roll angle (roll)) in the rolling direction around the rolling axis are used.

【０００４】絶対座標系として、水平面における東方向
をＸ軸、北方向をＹ軸、鉛直方向をＺ軸としたとき、ヨ
ー角は移動体の中心軸を絶対座標系のＸ−Ｙ面に正射影
したときのＸ軸とローリング軸の水平面への正射影との
なす角であり、ピッチ角はローリング軸と水平面とのな
す角であり、ロール角はピッチング軸と水平面移動体と
のなす角である。As an absolute coordinate system, when the east direction in the horizontal plane is the X axis, the north direction is the Y axis, and the vertical direction is the Z axis, the yaw angle is positive with the central axis of the moving body in the XY plane of the absolute coordinate system. The angle between the X-axis and the orthogonal projection of the rolling axis onto the horizontal plane when projected, the pitch angle is the angle between the rolling axis and the horizontal plane, and the roll angle is the angle between the pitching axis and the horizontal plane moving body. is there.

【０００５】従来、移動体座標系の主要３角度を測定す
るために、地磁気センサ、ジャイロ装置等を用いた姿勢
検出装置が用いられていた。これら従来の姿勢検出装置
では、ヨーイング方向を検出するために地磁気センサの
出力とジャイロ出力を用い、ピッチング方向及びローリ
ング方向の姿勢を検出するためにジャイロ出力等を用い
ていた。Conventionally, an attitude detecting device using a geomagnetic sensor, a gyro device or the like has been used to measure the three main angles of the moving body coordinate system. In these conventional attitude detection devices, the output of the geomagnetic sensor and the gyro output are used to detect the yawing direction, and the gyro output and the like are used to detect the attitudes in the pitching direction and the rolling direction.

【０００６】ここで、地磁気センサは絶対的な方位角を
装置に与えるので装置内で重要な位置を占める。地磁気
センサは水平状態での使用を基準としているので、この
地磁気センサに傾きが生ずると、装置の出力に多大に誤
差が生ずる。Here, the geomagnetic sensor occupies an important position in the device because it gives an absolute azimuth angle to the device. Since the geomagnetic sensor is used in a horizontal state as a reference, if the geomagnetic sensor is tilted, a large error will occur in the output of the device.

【０００７】そこで、地磁気センサの水平状態を保つた
めに、いわゆるジンバル（gimbal）機構を利用した水平
儀が用いられていた。また、上記ジンバル機構を利用し
ない姿勢検出装置として、移動体の主要３角度のそれぞ
れの角度を検出するジャイロと、主要３角度の方向に加
わる加速度の影響を加速度計により検出し、ジャイロの
出力を加速度計の検出した加速度により補正するとい
う、特公平４−５５２４８号公報に記載の発明があっ
た。Therefore, in order to maintain the horizontal state of the geomagnetic sensor, a horizontal mount using a so-called gimbal mechanism has been used. Further, as a posture detection device that does not use the gimbal mechanism, a gyro that detects each of the three main angles of the moving body and the effect of acceleration applied in the directions of the three main angles are detected by an accelerometer and the output of the gyro is detected. There is an invention described in Japanese Examined Patent Publication No. 4-55248, in which correction is performed by the acceleration detected by the accelerometer.

【０００８】当該発明は、地磁気センサの出力とジャイ
ロの出力に基づいて行ったマトリクス演算により得られ
た方位角信号とを比較して、両者の差が少なくなる方向
へマトリクス演算を収束させていくものである。According to the invention, the output of the geomagnetic sensor is compared with the azimuth angle signal obtained by the matrix calculation performed based on the output of the gyro, and the matrix calculation is converged in the direction in which the difference between the two becomes smaller. It is a thing.

【０００９】[0009]

【発明が解決しようとする課題】しかしながら、地磁気
センサの出力を基本として方位角を得る従来技術では、
地磁気センサを水平に保つためにジンバル機構を必要と
した。ジンバル機構は高い精度を必要とするために高価
なものであるため経済性が低く、装置全体のサイズも大
きいものになっていた。However, in the conventional technique for obtaining the azimuth angle based on the output of the geomagnetic sensor,
A gimbal mechanism was needed to keep the geomagnetic sensor horizontal. Since the gimbal mechanism requires high accuracy and is expensive, it is not economical and the size of the entire device is large.

【００１０】また、加速度を測定してマトリクス演算を
行う特許公報記載の発明では、ヨー角の出力を補正する
ことは行われているが、基準となる地磁気センサが傾い
ている場合に地磁気センサの出力がシフトし、正確な磁
気的方位角を示さなくなるという点が考慮されていなか
った。このため、長時間にわたり移動体の姿勢が傾いて
いる場合には、出力される方位角に誤差を生ずる危険が
あった。Further, in the invention described in the patent publication in which the acceleration is measured and the matrix operation is performed, the output of the yaw angle is corrected, but when the reference geomagnetic sensor is tilted, The fact that the output shifts and does not show the correct magnetic azimuth has not been taken into consideration. Therefore, when the posture of the moving body is tilted for a long time, there is a risk that an error may occur in the output azimuth angle.

【００１１】そこで、本発明の目的は、高価な可動部を
有する測定器を用いることなく、移動体の姿勢が傾いて
も移動体の正確な方位角を検出し得る姿勢検出方法及び
装置を提供することにある。Therefore, an object of the present invention is to provide a posture detecting method and apparatus capable of detecting an accurate azimuth angle of a moving body even if the posture of the moving body is tilted, without using a measuring instrument having an expensive movable part. To do.

【００１２】[0012]

【課題を解決するための手段】請求項１に記載の発明
は、移動体（例えば、自動車）に備えられ、所定のタイ
ミング毎（例えば、２５〔msec〕毎）に、自らの内部を
通過する地磁気の磁束密度の変化により方位角を測定す
る地磁気測定手段（例えば、検出コイルにより地磁気の
強さを測定する地磁気センサ）を有する姿勢検出装置の
姿勢検出方法であって、移動体がローリングするときの
ロール角及び移動体がピッチングするときのピッチ角を
検出し、ロール角及びピッチ角に基づいてロール角及び
ピッチ角による移動体の姿勢の乱れ（例えば、水平面か
らの移動体の偏位角）によって生ずる地磁気測定手段の
出力の誤差を補償する。The invention according to claim 1 is provided in a moving body (for example, an automobile) and passes through the inside of itself at a predetermined timing (for example, every 25 [msec]). A posture detection method of a posture detection device having a geomagnetism measuring means (for example, a geomagnetic sensor that measures the intensity of the geomagnetism by a detection coil) that measures an azimuth angle by a change in the magnetic flux density of the geomagnetism, when a moving body rolls. Of the roll angle and the pitch angle when the moving body pitches, and the roll angle and the disturbance of the posture of the moving body due to the pitch angle based on the pitch angle (for example, the deviation angle of the moving body from the horizontal plane) The error in the output of the geomagnetic measuring means caused by

【００１３】請求項２に記載の発明は、移動体（例え
ば、自動車）に備えられ、自らの内部を通過する地磁気
の磁束密度の変化により仮方位角を検出する地磁気検出
手段（例えば、検出コイルにより地磁気の強さを測定す
る地磁気センサ）と、移動体のピッチ角を検出するピッ
チ角検出手段と、移動体のロール角を検出するロール角
検出手段と、ピッチ角と所定の第１係数を乗ずることに
より仮方位角に対するピッチングする方向への仮偏位角
による第１補正量を決定し、ロール角と所定の第２係数
を乗ずることにより仮方位角に対するロール角による第
２補正量を決定し、第１補正量及び第２補正量に基づい
て仮方位角を補正し、方位角を生成する地磁気方位補正
手段（例えば、ソフトウェアによる疑似ジンバル機構）
と、を備えて構成される。According to a second aspect of the present invention, a geomagnetism detecting means (for example, a detection coil) provided in a moving body (for example, an automobile) for detecting a temporary azimuth angle by a change in magnetic flux density of geomagnetism passing through the inside of the vehicle. A geomagnetic sensor for measuring the strength of the geomagnetism), a pitch angle detecting means for detecting the pitch angle of the moving body, a roll angle detecting means for detecting the roll angle of the moving body, a pitch angle and a predetermined first coefficient. The first correction amount based on the temporary deviation angle in the pitching direction with respect to the temporary azimuth angle is determined by multiplying it, and the second correction amount based on the roll angle relative to the temporary azimuth angle is determined by multiplying the roll angle by a predetermined second coefficient. Then, a geomagnetic azimuth correcting means (for example, a pseudo gimbal mechanism by software) that corrects the temporary azimuth angle based on the first correction amount and the second correction amount to generate the azimuth angle.
And are configured.

【００１４】請求項３に記載の発明は、移動体（例え
ば、自動車）に備えられ自らの内部を通過する地磁気の
磁束密度の変化により仮方位角を検出する地磁気検出手
段（例えば、検出コイルにより地磁気の強さを測定する
地磁気センサ）と、移動体のピッチングする方向への角
速度を検出するピッチ角速度検出手段と、移動体のロー
リングする方向への角速度を検出するロール角速度検出
手段と、移動体のピッチング軸に垂直な平面に投映した
移動体に加わる加速度成分と重力加速度との合成ベクト
ルが、移動体のローリング軸となす傾斜角を検出するピ
ッチング傾斜角検出手段（例えば、傾斜計）と、移動体
のローリング軸に垂直な平面に投映した移動体に加わる
加速度成分と重力加速度との合成ベクトルが、移動体の
ピッチング軸となす傾斜角を検出するローリング傾斜角
検出手段（例えば、傾斜計）と、移動体の移動により移
動体に加わる加速度を測定する加速度測定手段と、移動
体の移動により移動体に加わる遠心力を測定する遠心力
測定手段と、ピッチングする方向への角速度の積分値に
基づいてピッチングする方向への仮偏位角を計算し、ロ
ーリングする方向への角速度の積分値に基づいてローリ
ングする方向への仮偏位角を計算する仮偏位角計算手段
と、加速度に対応させてピッチングする方向への仮偏位
角に対するピッチングする方向への傾斜角による補正量
を規定するピッチング重み付け係数を決定し、ピッチン
グ重み付け係数に基づいて加速度の影響を取り除いたピ
ッチ角を出力するピッチング重み付け演算手段と、遠心
力に対応させてローリングする方向への仮偏位角に対す
るローリングする方向への傾斜角による補正量を規定す
るローリング重み付け係数を決定し、ローリング重み付
け係数に基づいて遠心力の影響を取り除いたロール角を
出力するローリング重み付け演算手段と、ピッチ角と所
定の第１係数を乗ずることにより仮方位角に対するピッ
チングする方向への仮偏位角による第１補正量を決定
し、ロール角と所定の第２係数を乗ずることにより仮方
位角に対するロール角による第２補正量を決定し、第１
補正量及び第２補正量に基づいて仮方位角を補正し、方
位角を生成する地磁気方位補正手段（例えば、ソフトウ
ェアによる疑似ジンバル機構）と、を備えて構成され
る。According to a third aspect of the present invention, a geomagnetism detecting means (for example, a detection coil is provided in a moving body (for example, an automobile) to detect a temporary azimuth angle by a change in magnetic flux density of the geomagnetism passing through the inside of itself. A geomagnetic sensor for measuring the strength of the geomagnetism), a pitch angular velocity detecting means for detecting an angular velocity of the moving body in a pitching direction, a roll angular velocity detecting means for detecting an angular velocity of the moving body in a rolling direction, and a moving body. A composite vector of the acceleration component applied to the moving body and the gravitational acceleration projected on a plane perpendicular to the pitching axis of, and a pitching tilt angle detecting means (for example, an inclinometer) for detecting a tilt angle formed with the rolling axis of the moving body, The composite vector of the acceleration component applied to the moving body and the gravitational acceleration projected on the plane perpendicular to the rolling axis of the moving body forms the pitching axis of the moving body. A rolling tilt angle detecting means (for example, an inclinometer) for detecting a tilt angle, an acceleration measuring means for measuring an acceleration applied to the moving body by the movement of the moving body, and a centrifugal force applied to the moving body by the movement of the moving body. The centrifugal force measuring means and the temporary deviation angle in the pitching direction are calculated based on the integrated value of the angular velocity in the pitching direction, and the temporary deviation in the rolling direction is calculated based on the integrated value of the angular velocity in the rolling direction. Temporary deviation angle calculating means for calculating the displacement angle, and a pitching weighting coefficient that determines the correction amount by the inclination angle in the pitching direction with respect to the temporary displacement angle in the pitching direction corresponding to the acceleration, and determines the pitching weighting. Pitching weighting calculation means for outputting a pitch angle from which the influence of acceleration is removed based on a coefficient, and rolling direction corresponding to centrifugal force A rolling weighting calculation means that determines a rolling weighting coefficient that defines a correction amount according to an inclination angle in a rolling direction with respect to the temporary deviation angle, and outputs a roll angle in which the influence of centrifugal force is removed based on the rolling weighting coefficient, and a pitch. By multiplying the angle by a predetermined first coefficient, the first correction amount by the temporary deviation angle in the pitching direction with respect to the temporary azimuth is determined, and by multiplying the roll angle by the predetermined second coefficient, the roll for the temporary azimuth is obtained. The second correction amount depending on the angle is determined, and the first
And a geomagnetic azimuth correcting unit (for example, a pseudo gimbal mechanism by software) that corrects the temporary azimuth angle based on the correction amount and the second correction amount to generate the azimuth angle.

【００１５】請求項４に記載の発明は、請求項２又は請
求項３に記載の姿勢検出装置において、ピッチングする
方向への仮偏位角をθ_anp とし、ピッチングする方向へ
の傾斜角をθ_mnp とし、ピッチング重み付け係数をＪ_p
としたとき、補正されたピッチ角θ_p は、 θ_p ＝θ_anp ＋Ｊ_p ・（θ_mnp −θ_anp ） （０≦
Ｊ_p ≦１） という関係により算出される。According to a fourth aspect of the invention, in the posture detecting apparatus according to the second or third aspect, the provisional deviation angle in the pitching direction is θ _anp, and the inclination angle in the pitching direction is θ. _mnp and the pitching weighting coefficient is J _p
Then, the corrected pitch angle θ _p is θ _p = θ _anp + J _p · (θ _mnp −θ _anp ) (0 ≦
It is calculated by the relationship of J _p ≦ 1).

【００１６】請求項５に記載の発明は、請求項２又は請
求項３に記載の姿勢検出装置において、ローリングする
方向への仮偏位角をθ_anr とし、ローリングする方向へ
の傾斜角をθ_mnr とし、ローリング重み付け係数をＪ_r
としたとき、補正されたロール角θ_r は、 θ_r ＝θ_anr ＋Ｊ_r ・（θ_mnr −θ_anr ） （０≦
Ｊ_r ≦１） という関係により算出される。According to a fifth aspect of the present invention, in the posture detecting apparatus according to the second or third aspect, the provisional deviation angle in the rolling direction is θ _anr, and the inclination angle in the rolling direction is θ. _mnr and the rolling weighting coefficient is J _r
Then, the corrected roll angle θ _r is θ _r = θ _anr + J _r · (θ _mnr −θ _anr ) (0 ≦
It is calculated by the relationship of J _r ≦ 1).

【００１７】請求項６に記載の発明は、請求項２乃至請
求項５に記載の姿勢検出装置において、仮方位角の座標
を（ｘ、ｙ）とし、第１係数又は第２係数をＪとし、ピ
ッチ角をθp とし、ロール角をθ_r とたとき、移動体の
姿勢の乱れを取り除いた方位角の座標（Ｘ，Ｙ）は、 Ｘ＝ｘ＋Ｊ・θ_r Ｙ＝ｙ−Ｊ・θ_p という関係により算出される。According to a sixth aspect of the present invention, in the posture detection apparatus according to the second to fifth aspects, the coordinates of the temporary azimuth are (x, y) and the first coefficient or the second coefficient is J. , The pitch angle is θ _p, and the roll angle is θ _r , the azimuth coordinate (X, Y) without disturbance of the posture of the moving body is X = x + J · θ _r Y = y−J · θ _p It is calculated by the relationship.

【００１８】[0018]

【作用】請求項１に記載の発明によれば、地磁気測定手
段（例えば、検出コイルにより地磁気の強さを測定する
地磁気センサ）は、移動体（例えば、自動車）の所定の
タイミング毎（例えば、２５〔msec〕毎）に、自らの内
部を通過する地磁気の磁束密度の変化により方位角を測
定する。According to the invention described in claim 1, the geomagnetism measuring means (for example, the geomagnetic sensor for measuring the intensity of the geomagnetism by the detection coil) is provided at a predetermined timing of the moving body (for example, an automobile) (for example, Every 25 [msec]), the azimuth angle is measured by the change in the magnetic flux density of the geomagnetism that passes through itself.

【００１９】そのとき、移動体がローリングするときの
ロール角及び移動体がピッチングするときのピッチ角を
検出する。そして、ロール角及びピッチ角に基づいてロ
ール角及びピッチ角による移動体の姿勢の乱れ（例え
ば、水平面からの移動体の偏位角）によって生ずる地磁
気測定手段の出力の誤差を補償する。At this time, the roll angle when the moving body rolls and the pitch angle when the moving body pitches are detected. Then, based on the roll angle and the pitch angle, the error of the output of the geomagnetism measuring means caused by the disturbance of the posture of the moving body due to the roll angle and the pitch angle (for example, the deviation angle of the moving body from the horizontal plane) is compensated.

【００２０】請求項２に記載の発明によれば、地磁気検
出手段（例えば、検出コイルにより地磁気の強さを測定
する地磁気センサ）は移動体（例えば、自動車）に備え
られ、自らの内部を通過する地磁気の磁束密度の変化に
より仮方位角を検出する。According to the second aspect of the present invention, the geomagnetism detecting means (for example, the geomagnetic sensor for measuring the intensity of the geomagnetism by the detection coil) is provided in the moving body (for example, an automobile) and passes through the inside of itself. The temporary azimuth angle is detected by the change in the magnetic flux density of the geomagnetism.

【００２１】ピッチ角検出手段は移動体のピッチ角を検
出し、ロール偏位角検出手段は移動体のロール角を検出
する。地磁気方位補正手段は、ピッチ角と所定の第１係
数を乗ずることにより仮方位角に対するピッチングする
方向への仮偏位角による第１補正量を決定する。ロール
角と所定の第２係数を乗ずることにより仮方位角に対す
るロール角による第２補正量を決定する。そして、第１
補正量及び第２補正量に基づいて仮方位角を補正し、移
動体の姿勢の乱れを取り除いた方位角を生成する。The pitch angle detecting means detects the pitch angle of the moving body, and the roll deviation angle detecting means detects the roll angle of the moving body. The geomagnetic azimuth correction means determines the first correction amount based on the temporary deviation angle in the pitching direction with respect to the temporary azimuth angle by multiplying the pitch angle by a predetermined first coefficient. By multiplying the roll angle by a predetermined second coefficient, the second correction amount based on the roll angle with respect to the temporary azimuth angle is determined. And the first
The temporary azimuth angle is corrected based on the correction amount and the second correction amount, and the azimuth angle in which the disturbance of the posture of the moving body is removed is generated.

【００２２】請求項３に記載の発明によれば、地磁気検
出手段（例えば、検出コイルにより地磁気の強さを測定
する地磁気センサ）は、移動体（例えば、自動車）に備
えられ自らの内部を通過する地磁気の磁束密度の変化に
より仮方位角を検出する。According to the third aspect of the invention, the geomagnetism detecting means (for example, the geomagnetic sensor for measuring the intensity of the geomagnetism by the detection coil) is provided in the moving body (for example, automobile) and passes through the inside of itself. The temporary azimuth angle is detected by the change in the magnetic flux density of the geomagnetism.

【００２３】ピッチ角速度検出手段は移動体のピッチン
グする方向への角速度を検出し、ロール角速度検出手段
は移動体のローリングする方向への角速度を検出する。
また、ピッチング傾斜角検出手段は、例えば、振子と筐
体との相対角を検出する傾斜計である。移動体のピッチ
ング軸に垂直な平面に投映した移動体に加わる加速度成
分と重力加速度との合成ベクトルと、移動体のローリン
グ軸と、の間の傾斜角を検出する。The pitch angular velocity detecting means detects the angular velocity of the moving body in the pitching direction, and the roll angular velocity detecting means detects the angular velocity of the moving body in the rolling direction.
The pitching tilt angle detection means is, for example, an inclinometer that detects the relative angle between the pendulum and the housing. An inclination angle between a rolling vector of the moving body and a combined vector of an acceleration component applied to the moving body and a gravitational acceleration projected on a plane perpendicular to the pitching axis of the moving body is detected.

【００２４】ローリング傾斜角検出手段は、例えば、振
子と筐体との相対角を検出する傾斜計である。移動体の
ローリング軸に垂直な平面に投映した移動体に加わる加
速度成分と重力加速度との合成ベクトルと、移動体のピ
ッチング軸と、の間の傾斜角を検出する。The rolling inclination angle detecting means is, for example, an inclinometer for detecting the relative angle between the pendulum and the housing. An inclination angle between a composite vector of an acceleration component applied to a moving body projected on a plane perpendicular to the rolling axis of the moving body and a gravitational acceleration and a pitching axis of the moving body is detected.

【００２５】一方、加速度測定手段は移動体の移動によ
り移動体に加わる加速度を測定し、遠心力測定手段は移
動体の移動により移動体に加わる遠心力を測定する。そ
して、仮偏位角計算手段は、ピッチングする方向への角
速度の積分値に基づいてピッチングする方向への仮偏位
角を計算し、ローリングする方向への角速度の積分値に
基づいてローリングする方向への仮偏位角を計算する。On the other hand, the acceleration measuring means measures the acceleration applied to the moving body by the movement of the moving body, and the centrifugal force measuring means measures the centrifugal force applied to the moving body by the movement of the moving body. Then, the temporary displacement angle calculating means calculates a temporary displacement angle in the pitching direction based on the integrated value of the angular velocity in the pitching direction, and a rolling direction based on the integrated value of the angular velocity in the rolling direction. Calculate the provisional excursion angle to.

【００２６】さらに、ピッチング重み付け演算手段は、
加速度に対応させてピッチングする方向への仮偏位角に
対するピッチングする方向への傾斜角による補正量を規
定するピッチング重み付け係数を決定し、ピッチング重
み付け係数に基づいて加速度の影響を取り除いたピッチ
角を出力する。Further, the pitching weight calculation means is
Determine the pitching weighting coefficient that defines the amount of correction by the tilt angle in the pitching direction with respect to the temporary deviation angle in the pitching direction corresponding to the acceleration, and remove the effect of acceleration based on the pitching weighting coefficient. Output.

【００２７】ローリング重み付け演算手段は、遠心力に
対応させてローリングする方向への仮偏位角に対するロ
ーリングする方向への傾斜角による補正量を規定するロ
ーリング重み付け係数を決定し、ローリング重み付け係
数に基づいて遠心力の影響を取り除いたロール角を出力
する。The rolling weighting calculation means determines a rolling weighting coefficient that defines a correction amount according to the inclination angle in the rolling direction with respect to the temporary displacement angle in the rolling direction corresponding to the centrifugal force, and based on the rolling weighting coefficient. Output the roll angle without the influence of centrifugal force.

【００２８】最後に、地磁気方位補正手段は、ピッチ角
と第１係数を乗ずることにより仮方位角に対するピッチ
ングする方向への仮偏位角による第１補正量を決定し、
ロール角と第２係数を乗ずることにより仮方位角に対す
るロール角による第２補正量を決定し、第１補正量及び
第２補正量に基づいて仮方位角を補正し、移動体の姿勢
の乱れを取り除いた方位角を生成する。Finally, the geomagnetic azimuth correction means determines the first correction amount by the temporary deviation angle in the pitching direction with respect to the temporary azimuth angle by multiplying the pitch angle by the first coefficient,
The roll angle and the second coefficient are multiplied to determine the second correction amount by the roll angle with respect to the temporary azimuth angle, the temporary azimuth angle is corrected based on the first correction amount and the second correction amount, and the attitude of the moving body is disturbed. To remove the azimuth angle.

【００２９】請求項４に記載の発明によれば、加速度が
所定値以上のときはピッチング重み付け係数をゼロと
し、加速度が加わると誤差を多く含む傾斜角θ_mnp の影
響を排除し短時間の偏位に正確に動作する仮偏位角θ
_anp のみを出力するので、正確な偏位角を算出する。According to the fourth aspect of the present invention, when the acceleration is equal to or higher than a predetermined value, the pitching weighting coefficient is set to zero, and when the acceleration is applied, the influence of the inclination angle θ _mnp , which includes many errors, is eliminated, and the deviation in a short time is eliminated. Position deviation angle θ
_Since only _anp is output, the accurate deviation angle is calculated.

【００３０】請求項５に記載の発明によれば、遠心力が
所定値以上のときはローリング重み付け係数をゼロと
し、遠心力が加わると誤差を多く含む傾斜角θ_mnr の影
響を排除し短時間の偏位に正確に動作する仮偏位角θ
_anr のみを出力するので、正確な偏位角を算出する。According to the fifth aspect of the invention, when the centrifugal force is equal to or greater than a predetermined value, the rolling weighting coefficient is set to zero, and when the centrifugal force is applied, the influence of the inclination angle θ _mnr including many errors is eliminated to _shorten the time. The eccentric deviation angle θ
Only _anr is output, so an accurate deviation angle is calculated.

【００３１】請求項６に記載の発明によれば、ピッチ角
θ_p 又はロール角θ_r がゼロでないとき、所定の関係に
より仮偏位角の座標（ｘ，ｙ）を移動体の姿勢の乱れを
取り除いた方位角の座標（Ｘ，Ｙ）に収束させる。According to the sixth aspect of the invention, when the pitch angle θ _p or the roll angle θ _r is not zero, the coordinates (x, y) of the temporary deviation angle are disturbed due to a predetermined relationship. Is converged to the azimuth angle coordinates (X, Y) from which is removed.

【００３２】[0032]

【実施例】本発明の姿勢検出方法及び装置に係る好適な
実施例を図面を参照して説明する。DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENT A preferred embodiment of the attitude detecting method and apparatus of the present invention will be described with reference to the drawings.

【００３３】本実施例は本発明の姿勢検出装置をＢＳア
ンテナ等のアンテナ追従装置に適用するものである。 （Ｉ）構成の説明 システム構成 図２に、本実施例のアンテナ追従装置の全体図を示す。In this embodiment, the attitude detecting device of the present invention is applied to an antenna tracking device such as a BS antenna. (I) Description of Configuration System Configuration FIG. 2 shows an overall view of the antenna tracking device of this embodiment.

【００３４】図２に示すように、本実施例のアンテナ追
従装置は、自動車等の移動体に搭載されることを想定し
たものであり、移動体（ここでは自動車とする。）の姿
勢を検出する姿勢検出装置１００と、姿勢検出装置１０
０の検出した自動車の姿勢に対応してアンテナを向ける
べき方角を指定し、システムを制御するコントローラ２
００と、アンテナ本体３００_-1を含みコントローラ２０
０の制御によりアンテナ本体３００_-1を任意の方位角、
又は、任意の仰角に向けることが可能なアンテナメカ３
００と、を備えて構成される。 姿勢検出装置の構成 図１に本発明の第１実施例の構成を示す。As shown in FIG. 2, the antenna tracking device of the present embodiment is assumed to be mounted on a moving body such as an automobile, and detects the posture of the moving body (herein, an automobile). Attitude detecting apparatus 100 and attitude detecting apparatus 10
Controller 2 that controls the system by designating the direction in which the antenna should be directed according to the detected vehicle attitude of 0
00 and the antenna main body 300 _-1 including the controller 20
The antenna main body 300 _-1 can be set to an arbitrary azimuth angle by controlling 0.
Or, the antenna mechanism 3 that can be turned to any elevation angle
00 and. Configuration of Attitude Detection Device FIG. 1 shows the configuration of a first embodiment of the present invention.

【００３５】図１に示すように、本実施例の姿勢検出装
置１００は、自動車の姿勢を表示するための主要３角度
（ヨー（yaw ）、ピッチ（pitch ）、ロール（roll））
のそれぞれの角度を検出した後座標演算を行い、出力す
るものである。As shown in FIG. 1, the posture detecting apparatus 100 of this embodiment has three main angles (yaw, pitch, roll) for displaying the posture of the automobile.
After detecting the respective angles of, the coordinate calculation is performed and output.

【００３６】ヨー角を検出する系統は、地磁気センサ２
と振動ジャイロ８がセンサとして働く。地磁気センサ２
は電子的に地磁気方位を検出するもので、互いに直交す
る二つの検出コイルを有している。地磁気センサ２は、
二つの検出コイルの内部を通過する磁束密度を検出し地
磁気の方向を検出するため、通常は水平状態で使用す
る。The system for detecting the yaw angle is the geomagnetic sensor 2
And the vibration gyro 8 works as a sensor. Geomagnetic sensor 2
Is for electronically detecting the geomagnetic direction and has two detection coils that are orthogonal to each other. Geomagnetic sensor 2
It is normally used in a horizontal state in order to detect the direction of the earth's magnetism by detecting the magnetic flux density passing through the inside of the two detection coils.

【００３７】ソフトウェアジンバル３は、地磁気センサ
２の出力を、ピッチ、ロールの角度に基づいて疑似的に
完全な水平状態で出力された地磁気方位データに補正す
るもので、従来の水平儀（機構的ジンバル）に代わるも
のである。このソフトウェアジンバル３は、本発明の主
要部であり、請求項２及び請求項３に記載した地磁気方
位補正手段に相当するものである。動作の詳細は後述す
る。The software gimbal 3 corrects the output of the geomagnetic sensor 2 into the geomagnetic direction data output in a pseudo perfect horizontal state based on the pitch and roll angles. ) Is an alternative. The software gimbal 3 is a main part of the present invention, and corresponds to the geomagnetic direction correcting means described in claims 2 and 3. Details of the operation will be described later.

【００３８】地磁気方位計算回路４は、ソフトウェアジ
ンバル３の出力した補正した地磁気方位角から金属体で
ある自動車の車体への着磁により生ずる磁界の影響を除
去して、実際の地磁気方位を検出する。The geomagnetic azimuth calculation circuit 4 detects the actual geomagnetic azimuth by removing the effect of the magnetic field generated by the magnetization of the metal body of the automobile from the corrected geomagnetic azimuth output from the software gimbal 3. .

【００３９】着磁ベクトル更新回路５は、着磁補正によ
り得られた着磁ベクトルを短周期（例えば、２５〔mse
c〕）で更新する。重み付け加算回路６は、地磁気方位
検出系統に影響する外乱の影響を除去しヨー角θ_mny を
真値θ_y に収束させるための重み付け計算を行う。The magnetization vector update circuit 5 uses the magnetization vector obtained by the magnetization correction for a short period (for example, 25 [mse
Update with c)). The weighting addition circuit 6 removes the influence of disturbance affecting the geomagnetic direction detection system and performs weighting calculation for converging the yaw angle θ _mny to the true value θ _y .

【００４０】着磁補正回路７は、重み付け計算により得
られるヨー角の真値に基づいて地磁気センサ２に影響を
与えている車体の着磁の影響を除去するための着磁ベク
トルを出力する。The magnetization correction circuit 7 outputs a magnetization vector for removing the influence of the magnetization of the vehicle body affecting the geomagnetic sensor 2 based on the true value of the yaw angle obtained by the weighting calculation.

【００４１】振動ジャイロ８は、固有共振周波数で振動
する内部の音叉又は多角柱に働くコリオリの力を圧電素
子で検出し、振動ジャイロが備え付けられている移動体
の角速度を検出する。回転角度はこれを積分回路で積分
することで得られる。The vibrating gyro 8 detects the Coriolis force acting on the internal tuning fork or polygonal prism vibrating at the natural resonance frequency by the piezoelectric element, and detects the angular velocity of the moving body equipped with the vibrating gyro. The rotation angle is obtained by integrating this with an integrating circuit.

【００４２】オフセットリセット回路９は、積分により
角度を検出するために検出値にオフセットを保持し易い
振動ジャイロのオフセットを自動車の直進進行時、停止
時にリセットする。詳しくは後述する。The offset reset circuit 9 resets the offset of the vibration gyro which easily holds the offset in the detected value because the angle is detected by integration when the vehicle goes straight ahead or stops. Details will be described later.

【００４３】積分回路１０は、振動ジャイロ９の計測し
たヨー方向の角速度データを積分し、ヨー角に相当する
電圧値を出力する。ピッチ角を検出する系統は、傾斜計
１１と振動ジャイロ１４がセンサとして働く。The integration circuit 10 integrates the angular velocity data in the yaw direction measured by the vibration gyro 9 and outputs a voltage value corresponding to the yaw angle. In the system for detecting the pitch angle, the inclinometer 11 and the vibration gyro 14 work as sensors.

【００４４】傾斜計１１は自動車のピッチング方向への
傾斜を検出する。但し、この角度データは加速度の影響
を受けるため、振動ジャイロ１４の出力データと併せて
判断しなければならない。傾斜計１１は、振り子の傾き
で姿勢を検出するが、短時間で自動車の姿勢を検出する
必要があるため特殊なアルゴリズムを必要とする。則
ち、通常、傾斜計では、角度が変化すると内部の振り子
が振動しながら真値に収束して行くのに対し、本実施例
の傾斜計は、振り子の振動の極大値をサンプリグし、短
時間に真値を予測するというアルゴリズムを用いる。The inclinometer 11 detects the inclination of the automobile in the pitching direction. However, since this angle data is affected by acceleration, it must be judged together with the output data of the vibration gyro 14. The inclinometer 11 detects the posture by the tilt of the pendulum, but needs a special algorithm because it needs to detect the posture of the vehicle in a short time. In general, in the inclinometer, when the angle changes, the internal pendulum vibrates and converges to the true value, whereas in the inclinometer of the present embodiment, the maximum value of the vibration of the pendulum is sampled and shortened. An algorithm that predicts the true value at time is used.

【００４５】ロール角が０°以外のときにピッチ角には
誤差を含むことになるので、ロール影響補正回路１２
は、別途検出したロール角によりこの誤差を補正する。
重み付け加算回路１３は、請求項３に記載のピッチング
重み付け演算手段に相当し、加速度データ及び傾斜計１
７の出力したデータθ_mnp を利用して、絶対座標角度計
算回路１の出力したピッチ仮角度θ_anp から加速度の影
響を取り除き、ピッチ角の真値θ_p を出力する。詳しく
は後述する。Since the pitch angle includes an error when the roll angle is other than 0 °, the roll effect correction circuit 12
Corrects this error by the roll angle detected separately.
The weighting addition circuit 13 corresponds to the pitching weighting calculation means according to claim 3, and the acceleration data and the inclinometer 1 are included.
Using the data θ _mnp output by 7, the effect of acceleration is removed from the temporary pitch angle θ _anp output by the absolute coordinate angle calculation circuit 1, and the true value θ _p of the pitch angle is output. Details will be described later.

【００４６】振動ジャイロ１４は、ピッチング方向の自
動車の角速度を検出する。オフセットリセット回路１５
は、オフセットリセット回路９の動作と同様に、振動ジ
ャイロ１４の出力に含まれるオフセット成分を除去す
る。The vibration gyro 14 detects the angular velocity of the vehicle in the pitching direction. Offset reset circuit 15
Like the operation of the offset reset circuit 9, the offset component included in the output of the vibration gyro 14 is removed.

【００４７】積分回路１６は、振動ジャイロ１４の検出
した角速度を積分し、自動車のピッチ角に相当する電圧
値を出力する。ロール角を検出する系統は、傾斜計１７
と振動ジャイロ１８がセンサとして働く。The integrating circuit 16 integrates the angular velocity detected by the vibration gyro 14 and outputs a voltage value corresponding to the pitch angle of the automobile. The system that detects the roll angle is the inclinometer 17
The vibrating gyro 18 acts as a sensor.

【００４８】傾斜計１７は、自動車のローリング方向へ
の傾斜角を検出する。動作内容は傾斜計１１と同様に短
期間に傾斜状態を検出可能なものとする。ローリング方
向への傾斜計はピッチ角やヨー角の影響を受けないの
で、ロール角の系統で設けたロール影響補正回路１２の
ように他の角の影響を除去する構成が必要ない。但し、
この検出値は、自動車が曲がるときに働く遠心力の影響
を受けている。The inclinometer 17 detects the inclination angle of the automobile in the rolling direction. As with the inclinometer 11, the operation content is such that the inclination state can be detected in a short period of time. Since the inclinometer in the rolling direction is not affected by the pitch angle and the yaw angle, it is not necessary to remove the influence of other angles unlike the roll effect correction circuit 12 provided in the roll angle system. However,
This detected value is affected by the centrifugal force that acts when the vehicle turns.

【００４９】重み付け回路２１は、請求項３に記載のロ
ーリング重み付け演算手段に相当し、遠心力データ及び
傾斜計１７の出力したデータθ_mnr を用いて、絶対座標
角度計算回路１の出力したロール仮角度θ_anr から遠心
力の影響を取り除き、ロール角の真値θ_r を出力する。
詳しくは後述する。The weighting circuit 21 corresponds to the rolling weighting calculation means described in claim 3, and uses the centrifugal force data and the data θ _mnr output by the inclinometer 17 to calculate the roll temporary output output by the absolute coordinate angle calculation circuit 1. The effect of centrifugal force is removed from the angle θ _anr, and the true value θ _r of the roll angle is output.
Details will be described later.

【００５０】振動ジャイロ１８は、ローリング方向の自
動車の角速度を検出する。オフセットリセット回路１９
は、オフセットリセット回路９の動作と同様に、振動ジ
ャイロ１８の出力に含まれるオフセット成分を除去す
る。The vibration gyro 18 detects the angular velocity of the vehicle in the rolling direction. Offset reset circuit 19
Like the operation of the offset reset circuit 9, the offset component included in the output of the vibration gyro 18 is removed.

【００５１】積分回路２０は、振動ジャイロ１８の検出
した角速度を積分し、自動車のロール角に相当する電圧
値を出力する。絶対座標角度計算回路１は、積分回路１
０の出力したヨーイング方向の角速度の積分電圧値、積
分回路１６の出力したピッチング方向の角速度の積分電
圧値及び積分回路２０の出力したローリング方向の角速
度の積分電圧値を入力し、振動ジャイロから得られた角
度として仮角度（θ_any 、θ_anp 、θ_anr ）を出力す
る。The integrating circuit 20 integrates the angular velocity detected by the vibration gyro 18 and outputs a voltage value corresponding to the roll angle of the automobile. The absolute coordinate angle calculation circuit 1 is the integration circuit 1
The integrated voltage value of the angular velocity in the yawing direction output by 0, the integrated voltage value of the angular velocity in the pitching direction output by integration circuit 16 and the integrated voltage value of the angular velocity in the rolling direction output by integration circuit 20 are input and obtained from the vibration gyro. The provisional angles (θ _any , θ _anp , θ _anr ) are output as the obtained angles.

【００５２】遠心力計算回路２６は、エンジン制御系か
ら伝達される車速パルスＰ_v より当該自動車の進行方向
に対して直角方向（横方向）への遠心力を算出する。加
速度計算回路２７は、車速パルスＰ_v の変化率に基づい
て当該自動車の進行方向への加速度を計算する。なお、
加速度計算回路は車速パルスＰ_v の変化率によらず、直
接加速度による力を検出して加速度を得る加速度計を用
いてもよい。The centrifugal force calculation circuit 26 calculates the centrifugal force in the direction perpendicular to the traveling direction of the vehicle (lateral direction) from the vehicle speed pulse P _v transmitted from the engine control system. The acceleration calculation circuit 27 calculates the acceleration in the traveling direction of the vehicle based on the rate of change of the vehicle speed pulse P _v . In addition,
The acceleration calculation circuit may use an accelerometer that directly detects the force of the acceleration to obtain the acceleration, regardless of the rate of change of the vehicle speed pulse P _v .

【００５３】上記のようにして得られた主要３角度の真
値は、更に座標変換される。通常、放送衛星の位置は絶
対座標系、則ち、大地を基準として放送衛星を観察した
ときの座標で与えられる。自動車からこの放送衛星の方
角を得るためには、姿勢検出回路１００で得た自動車の
主要角度に基づいて、与えられる放送衛星の絶対座標系
での位置を移動体座標系における相対位置に変換する必
要がある。The true values of the three main angles obtained as described above are further coordinate-converted. Usually, the position of the broadcasting satellite is given by an absolute coordinate system, that is, coordinates when the broadcasting satellite is observed with reference to the ground. In order to obtain the direction of this broadcasting satellite from the car, the given position of the broadcasting satellite in the absolute coordinate system is converted into the relative position in the moving body coordinate system based on the main angle of the car obtained by the attitude detection circuit 100. There is a need.

【００５４】そこで、座標変換回路２２は、重み付け回
路６、１３及び２１から供給された補正後の３軸デー
タ、則ち、ヨー角θ_y 、ピッチ角θ_p 、ロール角θ_r を
入力する。そして、コントローラ２００から与えられる
受信目標となる放送衛星に関する方位角、仰角を表す絶
対座標データ（Ｘ，Ｙ，Ｚ）に基づいて、移動体座標系
から放送衛星を見た場合の相対座標（ｘ，ｙ，ｚ）を出
力する。Therefore, the coordinate conversion circuit 22 inputs the corrected three-axis data supplied from the weighting circuits 6, 13 and 21, that is, the yaw angle θ _y , the pitch angle θ _p , and the roll angle θ _r . Then, based on the absolute coordinate data (X, Y, Z) representing the azimuth angle and the elevation angle regarding the reception target broadcast satellite given from the controller 200, the relative coordinate (x , Y, z) is output.

【００５５】オイラー角変換回路２４は、相対座標
（ｘ，ｙ，ｚ）をオイラー角に変換し、車体座標系から
放送衛星を見た相対的な方位角、仰角を出力する。シリ
アル通信回路２５は、相対的な方位角データ、仰角デー
タをコードとしてアンテナメカ３００に通信する。 アンテナメカの構成 アンテナメカ３００は、マイコン３０と、周辺ハードウ
ェア３１と、実際にアンテナ本体３００_-1を駆動するモ
ータ４５と、モータ４６の回転角を検出するエンコーダ
４６と、を備える。The Euler angle conversion circuit 24 converts the relative coordinates (x, y, z) into Euler angles and outputs the relative azimuth angle and elevation angle of the broadcasting satellite viewed from the vehicle body coordinate system. The serial communication circuit 25 communicates with the antenna mechanism 300 using the relative azimuth data and elevation data as codes. Configuration of Antenna Mechanism The antenna mechanism 300 includes a microcomputer 30, peripheral hardware 31, a motor 45 that actually drives the antenna body 300 _-1, and an encoder 46 that detects the rotation angle of the motor 46.

【００５６】マイコン３０は、位置制御器４０を含む。
位置制御器４０は、エンコーダ４６から出力される二相
のパルスと、アンテナ本体３００_-1のメカ基準位置（則
ち、仰角方向ではアンテナ可動範囲の上限と下限の範
囲、方位角方向では正面方向）によって、自動車に対す
るアンテナ本体３００_-1の現在位置を得る。また、姿勢
検出装置１００から主要３軸データを得て、目標となる
放送衛星の方向と現在位置との差に比例する値をＤ／Ａ
コンバータより出力する。The microcomputer 30 includes a position controller 40.
The position controller 40 outputs the two-phase pulse output from the encoder 46 and the mechanical reference position of the antenna body 300 _-1 (that is, the upper and lower limits of the antenna movable range in the elevation direction, and the front direction in the azimuth direction). ), The current position of the antenna body 300 _-1 with respect to the automobile is obtained. Also, main three-axis data is obtained from the attitude detection apparatus 100, and a value proportional to the difference between the target direction of the broadcasting satellite and the current position is set to D / A.
Output from the converter.

【００５７】速度制御器４１は、速度検出部４７を含
み、比例積分制御を行う。則ち、速度制御器４１は、位
置制御器４０からの速度指令と速度検出部４７からのフ
ィードバック信号を入力し、方向信号と振幅信号とに二
分割して出力する。方向信号はアンテナ本体３００_-1を
回転させる方向を指定するもので、電力増幅器４４に極
性指定信号として出力される。振幅信号は駆動速度を定
めるもので電流制御器４３に供給される。The speed controller 41 includes a speed detector 47 and performs proportional-plus-integral control. In other words, the speed controller 41 receives the speed command from the position controller 40 and the feedback signal from the speed detector 47, and outputs the direction signal and the amplitude signal by dividing them into two. The direction signal specifies the direction in which the antenna body 300 _-1 is rotated, and is output to the power amplifier 44 as a polarity specifying signal. The amplitude signal, which determines the driving speed, is supplied to the current controller 43.

【００５８】速度検出部４７は、エンコーダ４６から得
られる二相パルスの移相差を検出しアップダウンパルス
を生成し、これを周波数−電圧（Ｆ／Ｖ）変換すること
により速度と回転方向に比例した電圧を出力する。The speed detector 47 detects the phase shift difference of the two-phase pulse obtained from the encoder 46, generates an up / down pulse, and converts this up to frequency-voltage (F / V) to be proportional to the speed and the rotation direction. Output the voltage.

【００５９】電流制御器４３は、内部のＰＷＭ回路にて
速度制御器４１からの振幅信号をＰＷＭ変調し、モータ
４５を駆動するのに適する制御信号波形にする。この制
御信号はスイッチング音として人間に感知されることの
ない周波数（例えば、２０〔kHz 〕）とする。The current controller 43 PWM-modulates the amplitude signal from the speed controller 41 with an internal PWM circuit to form a control signal waveform suitable for driving the motor 45. This control signal has a frequency (for example, 20 [kHz]) that is not perceived by humans as a switching sound.

【００６０】電力増幅器４４は、速度制御器４１からの
方向信号と電流制御器４３からのＰＷＭ信号とを入力
し、トランジスタをスイッチング動作させて、モータ４
５を方向信号で指定される方向にＰＷＭ信号で定まる電
力で駆動する。The power amplifier 44 inputs the direction signal from the speed controller 41 and the PWM signal from the current controller 43, causes the transistor to perform a switching operation, and drives the motor 4
5 is driven in the direction specified by the direction signal with electric power determined by the PWM signal.

【００６１】モータ４５は直流モータで構成され、エン
コーダ４６により所定量（例えば、１２０パルス／回
転）の２相パルスを出力する。 （ＩＩ）動作の説明 次に動作を説明する。The motor 45 is a DC motor, and the encoder 46 outputs a predetermined amount (for example, 120 pulses / revolution) of two-phase pulses. (II) Description of Operation Next, the operation will be described.

【００６２】図１に示すように、本実施例の姿勢検出装
置１００は、主要３軸の各角度当たり２つのセンサを用
いて自動車の姿勢を検出する。本実施例では、各軸の振
動ジャイロの出力を基本とし、振動ジャイロと異なる特
性を有するセンサを利用することにより、加速度や遠心
力の影響を取り除いた真値の３軸角度データを得る。そ
して、補正した真値のピッチ角、ロール角を用いて、最
終的に傾いた状態で使用される地磁気センサの出力を補
正する。As shown in FIG. 1, the attitude detecting device 100 of the present embodiment detects the attitude of the automobile by using two sensors for each angle of the main three axes. In this embodiment, based on the output of the vibration gyro of each axis, a sensor having a characteristic different from that of the vibration gyro is used to obtain true-valued three-axis angle data in which influences of acceleration and centrifugal force are removed. Then, the corrected true value pitch angle and roll angle are used to correct the output of the geomagnetic sensor used in the finally tilted state.

【００６３】例えば、振動ジャイロは角速度を感知する
ため短時間の変化には反応するが、積分が必要であるた
め、オフセット、誤差が生じ易い。則ち、検出データの
高周波成分が正確である。For example, the vibration gyro is sensitive to a change in a short time because it senses an angular velocity, but since integration is required, an offset and an error are likely to occur. That is, the high frequency component of the detection data is accurate.

【００６４】これに対し、ヨーイング方向に関しては、
地磁気センサは正確に地磁気の方角を検出するが、自動
車の姿勢が変化し検出コイルが水準状態から偏位すると
補正が必要となる。換言すれば、長期間のデータ、則
ち、検出データの低周波成分は正確な値を示すといえ
る。On the other hand, regarding the yawing direction,
The geomagnetic sensor accurately detects the direction of the geomagnetism, but correction is necessary when the attitude of the vehicle changes and the detection coil deviates from the standard state. In other words, it can be said that the long-term data, that is, the low frequency component of the detection data shows an accurate value.

【００６５】また、ピッチング方向、ローリング方向に
関し、傾斜計は自動車全体に加速度や遠心力が加わると
傾斜角が大きく変化するが、地磁気センサと同様に低周
波成分は正確である。Further, regarding the pitching direction and the rolling direction, the tilt angle of the inclinometer changes greatly when acceleration or centrifugal force is applied to the entire vehicle, but the low frequency component is accurate as in the geomagnetic sensor.

【００６６】本実施例では、以上述べたような高周波成
分に対して正確なセンサ（振動ジャイロ）と低周波成分
に対して正確なセンサ（地磁気センサ、傾斜計）との性
質の相違を利用して、正確な真値の角度データを得る。
具体的には、振動ジャイロの出力を主要なデータとし
て、地磁気センサ、傾斜計から得られる値を所定の計算
式で収束演算等を行い、最終的に真値に収束させる。In this embodiment, the difference in properties between the accurate sensor (vibration gyro) for high frequency components and the accurate sensor (geomagnetic sensor, inclinometer) for low frequency components as described above is utilized. To obtain accurate true angle data.
Specifically, using the output of the vibration gyro as the main data, the values obtained from the geomagnetic sensor and the inclinometer are subjected to a convergence calculation by a predetermined calculation formula, and finally converged to the true value.

【００６７】以下、ヨーイング方向、ピッチング方向、
ローリング方向に共通の振動ジャイロの累積誤差及びオ
フセットを排除するための動作を説明し、更に、ピッチ
角、ロール角における加速度、遠心力に応じた重み付け
加算の係数設定動作と、ヨー角における補正（ソフトウ
ェアジンバル）について説明する。 振動ジャイロの一般的な出力補正 振動ジャイロにより検出される３軸データ（以下「仮角
度」とする。）は、ｉ）オフセットリセット、ii）重み
付け加算、という２つの補正がなされる。 ｉ）オフセットリセット 振動ジャイロ８、１４、１８の出力はオフセットリセッ
ト回路９、１５、１９にてオフセットのリセットがなさ
れる。Hereinafter, the yawing direction, the pitching direction,
The operation for eliminating the cumulative error and offset of the vibration gyro common to the rolling direction will be described, and further, the coefficient setting operation of the weighted addition according to the acceleration at the pitch angle and the roll angle, the centrifugal force, and the correction at the yaw angle ( Software gimbal). General output correction of vibration gyro Three-axis data (hereinafter referred to as “temporary angle”) detected by the vibration gyro are subjected to two corrections, i) offset reset, and ii) weighted addition. i) Offset reset The outputs of the vibration gyros 8, 14, 18 are reset by the offset reset circuits 9, 15, 19.

【００６８】上記したように、振動ジャイロは積分して
角度を得るため、検出値に僅かな誤差が含まれている
と、累積誤差となって大きな検出角度のズレを生ずる。
そのため、本実施例では車速パルスにより自動車の速度
を検出し、（ａ）自動車が停止状態の場合、（ｂ）定速
度走行状態の場合、のそれぞれの状態を検出し、オフセ
ットリセットを行う。As described above, since the vibration gyro is integrated to obtain the angle, if the detected value includes a slight error, it becomes a cumulative error and a large deviation of the detected angle occurs.
Therefore, in this embodiment, the vehicle speed is detected by the vehicle speed pulse, and each of the states (a) when the vehicle is stopped and (b) when the vehicle is traveling at a constant speed is detected, and the offset is reset.

【００６９】（ａ）停止状態でのオフセットリセット 各オフセットリセット回路は、車速パルスより一定期間
自動車が停止していることを検出すると、その停止が検
出された期間の振動ジャイロ出力の平均値をオフセット
補正量と判断する。 (A) Offset reset in stop state When each offset reset circuit detects that the vehicle is stopped for a certain period from the vehicle speed pulse, it offsets the average value of the vibration gyro output in the period in which the stop is detected. Judge as the correction amount.

【００７０】但し、自動車の停止直後や発振直前では車
速パルスＰv は「０」を示していても、自動車が車速パ
ルスに現れないようなゆっくりとした速度で移動してい
る場合があるので、上記平均値の算出は停止期間の前後
０．５〔sec 〕を除く。また、振動ジャイロの出力変動
が激しい場合、地磁気センサ２、傾斜計１１、１７の変
動が激しい場合は安定状態でない可能性があるため、オ
フセット補正を行わない。However, even if the vehicle speed pulse Pv is "0" immediately after the vehicle stops or immediately before the oscillation, the vehicle may move at a slow speed that does not appear in the vehicle speed pulse. The calculation of the average value excludes 0.5 [sec] before and after the suspension period. Further, when the output fluctuation of the vibration gyro is large, or when the geomagnetic sensor 2 and the inclinometers 11 and 17 are large, there is a possibility that it is not in a stable state, so offset correction is not performed.

【００７１】また、通常の安定停止状態であるときは、
以下に示すような重み付け加算演算を繰り返して真値の
データに収束させる演算を行う。則ち、オフセットリセ
ット回路は、停止検出期間から前後０．５〔sec 〕を取
り除いた期間中に振動ジャイロから入力された出力の標
準偏差を取り、例えば、以下のような条件を満足する場
合にオフセット補正量の更新を行う。In the normal stable stop state,
The following weighted addition operation is repeated to perform an operation for converging to true value data. In other words, the offset reset circuit takes the standard deviation of the output input from the vibration gyro during the period of removing 0.5 [sec] before and after the stop detection period and, for example, when the following conditions are satisfied: The offset correction amount is updated.

【００７２】・停止状態（車速パルスＰ_v ＝「０」） ・振動ジャイロの出力の標準偏差＜０．１２ ・地磁気方位角、傾斜角の変化量＜０．３５〔deg/sec
〕 オフセット補正量は、オフセット補正量をＷ_off 、振動
ジャイロの出力をＷ_iとすると、例えば式（１）に従っ
て行われる。-Stop state (vehicle speed pulse _Pv = "0")-Standard deviation of output of vibration gyro <0.12-Amount of change in geomagnetic azimuth and tilt angle <0.35 [deg / sec]
When the offset correction amount is W _off and the output of the vibration gyro is W _i , the offset correction amount is _calculated , for example, according to the equation (1).

【００７３】[0073]

【数１】 （ｂ）定速走行状態でのオフセットリセット 地磁気センサ２又は傾斜計１１、１７により、角度変動
が少ない状態で走行していることが検出されると、オフ
セットリセット回路は振動ジャイロの出力の上記停止状
態でのオフセット補正量の計算と同様にして、オフセッ
ト補正量を算出する。[Equation 1] (B) Offset reset in constant speed running state When the geomagnetic sensor 2 or the inclinometers 11 and 17 detect that the vehicle is running in a state where there is little angular variation, the offset reset circuit stops the output of the vibration gyro. The offset correction amount is calculated similarly to the calculation of the offset correction amount in the state.

【００７４】例えば、以下の条件を満足するとき、オフ
セットリセット回路はオフセットリセットをする。 ・ヨーイング方向の角度変動量＜１．０〔deg 〕 ・ピッチング方向の角度変動量＜１．５〔deg 〕 但し、この場合は上記（ａ）とは異なり、あくまで走行
中であって、絶えず自動車の状態は変動していくことを
考えて演算する。例えば、ｎ回目のオフセットの更新演
算において、オフセット補正量をＷ_off(n)’、振動ジャ
イロの出力をＷ _i(n)’とすると、式（２）に示すように
一定の長期間にわたって収束させる演算とする。For example, when the following conditions are satisfied, it is turned off.
The set / reset circuit resets the offset.・ Amount of angular fluctuation in yawing direction <1.0 [deg] ・ Amount of angular fluctuation in pitching direction <1.5 [deg] However, in this case, unlike the above (a)
In the middle, the state of the car is constantly changing
Think and calculate. For example, update performance of the nth offset
In the calculation, the offset correction amount is W_{off (n)}’Vibration ja
The output of Iro is W _{i (n)}’, As shown in equation (2),
The calculation is to converge over a certain long period.

【００７５】[0075]

【数２】 ii）重み付け加算 さて、本発明の特徴の一つである重み付け加算について
説明する。[Equation 2] ii) Weighted addition The weighted addition , which is one of the features of the present invention, will be described.

【００７６】振動ジャイロは角速度を出力するため、角
度を算出するためには、振動ジャイロの出力たる角速度
を積分しなければならない。いま、ｎ回目の角速度の更
新演算において、ジャイロより得られる出力を
Ｗ_{i( n)}’、振動ジャイロのオフセット量をＷ_off(n)、ジ
ャイロゲインをＧ、ジャイロゲインの補正量をＧｋ
_(n-1) とすると、振動ジャイロの相対角速度Ｗ_(n) は、 Ｗ_(n) ＝Ｇｋ_(n-1) ・Ｇ・（Ｗ_(n) ’−Ｗ_off(n)） で表される。ここで、Ｗ_(n) ’＝Ｗ_off(n)であれば、ジ
ャイロの出力を積分しても累積誤差を生じないが、Ａ／
Ｄ変換ですでにＷ_(n) ’に誤差を含んでいるため、Ｗ
_(n) ’＝Ｗ_off(n)とするのは困難である。Since the vibration gyro outputs the angular velocity, the angular velocity output from the vibration gyro must be integrated in order to calculate the angle. Now, in the nth angular velocity update calculation, the output obtained from the gyro is W _{i ( n)} ', the vibration gyro offset amount is W _{off (n)} , the gyro gain is G, and the gyro gain correction amount is Gk.
_(n-1) , the relative angular velocity W _(n) of the vibrating gyro is expressed as W _(n) = Gk _(n-1) · G · (W _(n) '-W _{off (n)} ). . Here, if W _(n) '= W _{off (n)} , the accumulated error does not occur even if the output of the gyro is integrated, but A /
Since W _(n) 'has already included an error in D conversion, W
_It is difficult to make _(n) '= W _{off (n)} .

【００７７】更に、θ_n-1 を前回演算時の絶対角度とす
ると今回の振動ジャイロの絶対角度θ_anは、 θ_an＝θ_n-1 ＋Ｗ_(n) …（３） という関係で累積され算出される。このため、相対角速
度Ｗ_(n) に誤差が含まれていると、短いサイクル（例え
ば、２５〔msec〕毎）で誤差が累積されてしまう。Further, when θ _n-1 is the absolute angle at the time of the previous calculation, the absolute angle θ _an of the vibration gyro this time is accumulated and calculated in the relationship of θ _an = θ _n-1 + W _(n) (3) To be done. Therefore, if the relative angular velocity W _(n) includes an error, the error is accumulated in a short cycle (for example, every 25 [msec]).

【００７８】上記の如く振動ジャイロの出力信号のみを
用いると、絶対角度を得る際に誤差が生じることを避け
ることができない。よって、本実施例では、振動ジャイ
ロの累積誤差を補正するものとして、ヨー角では地磁気
センサの出力、ピッチ角とロール角では傾斜計の出力を
用い、振動ジャイロのおける累積誤差を収束させるよう
な重み付け加算を行う。If only the output signal of the vibration gyro is used as described above, it is inevitable that an error will occur when the absolute angle is obtained. Therefore, in this embodiment, the output of the geomagnetic sensor at the yaw angle and the output of the inclinometer at the pitch angle and the roll angle are used to correct the cumulative error of the vibration gyro so that the cumulative error of the vibration gyro is converged. Weighted addition is performed.

【００７９】式（３）に本発明で使用する重み付け演算
の関数を示す。 θ_n ＝θ_an＋Ｊ・（θ_mn−θ_an） …（４） （０≦Ｊ≦１） θ_n は目標とする角度の真値である。θ_anには、振動ジ
ャイロ８、１４、１８の出力を絶対座標角度計算回路１
で角度に変換したヨーイング方向仮角度θ_any、ピッチ
ング方向仮角度θ_anp 、ローリング方向仮角度θ_anr が
代入される。Ｊは重み付け係数であり、遠心力計算回路
２６により検出される遠心力又は加速度計算回路２７に
より検出される加速度、地磁気の外乱強度等に応じて変
化する値が入力される。θ_mnには、地磁気センサ２で検
出される仮方位データθ_mny 、傾斜計１１で検出される
傾斜角θ_mnp 、傾斜計１７で検出される傾斜角θ_mnr が
代入される。Equation (3) shows a weighting calculation function used in the present invention. θ _n = θ _an + J · (θ _mn −θ _an ) (4) (0 ≦ J ≦ 1) θ _n is the true value of the target angle. The output of the vibrating gyroscopes 8, 14 and 18 is used as θ _an for the absolute coordinate angle calculation circuit 1
The yawing direction provisional angle θ _any , the pitching direction provisional angle θ _anp , and the rolling direction provisional angle θ _{anr, which} have been converted into the angles in _{step 1} , are substituted. J is a weighting coefficient, and a value that changes according to the centrifugal force detected by the centrifugal force calculation circuit 26 or the acceleration detected by the acceleration calculation circuit 27, the disturbance intensity of the geomagnetism, etc. is input. The theta _mn, temporary orientation data theta _MNY detected by the geomagnetic sensor 2, the inclination angle theta _mnp detected by the inclinometer 11, the inclination angle theta _mnr detected by the inclinometer 17 is substituted.

【００８０】上記式（３）、（４）を用いて、式（４）
がθ_mnに収束することを示す。簡単のため、自動車は直
進走行状態であると仮定する。直進状態では、以下の条
件が成立する。Using equations (3) and (4) above, equation (4)
_{Shows that} it converges to θ _mn . For the sake of simplicity, it is assumed that the vehicle is traveling straight ahead. The following conditions are satisfied in the straight traveling state.

【００８１】Ｗ_(n) ＝０ θ_mn ＝θ_m ここで、θ_m は一定値である。W _(n) = 0 θ _mn = θ _m where θ _m is a constant value.

【００８２】式（３）より、 θ_an＝θ_n-1 となり、これを式（４）に代入して式（５）を得る。From the equation (3), θ _an = θ _n−1 , which is substituted into the equation (4) to obtain the equation (5).

【００８３】 θ_m ＝θ_n-1 ＋Ｊ（θ_m −θ_n-1 ） ＝（１−Ｊ）θ_n-1 ＋Ｊθ_m …（５） 式（５）の両辺からθ_m を差し引くと、 θ_n −θ_m ＝（１−Ｊ）θ_n-1 −（１−Ｊ）θ_m ＝（１−Ｊ）（θ_n-1 −θ_m ） ＝（１−Ｊ）² （θ_n-2 −θ_m ） ・ ・ ＝（１−Ｊ）^n-1 （θ₁ −θ_m ）…（６） となる。ここで、０＜（１−Ｊ）＜１となるようなＪを
設定すると、式（６）はｎ→∞で、Θ _m = θ _n-1 + J (θ _m −θ _n-1 ) = (1−J) θ _n-1 + J θ _m (5) When θ _m is subtracted from both sides of the equation (5), θ _n- θ _m = (1-J) θ _n-1- (1-J) θ _m = (1-J) (θ _n-1 -θ _m ) = (1-J) ² (θ _n-2 − θ _m ) ··· = (1-J) ^n-1 (θ ₁ −θ _m ) ... (6) Here, when J is set such that 0 <(1-J) <1, the equation (6) is n → ∞,

【００８４】[0084]

【数３】 となる。この重み付け加算の演算を繰り返すことで、θ
_n はθ_m に収束することがわかる。 ピッチ角の補正 ピッチング方向の角度は、自動車の加速、減速により生
ずる加速度により傾斜計１１の検出角度に変動が生ずる
ため、式（４）による補正を行う前に自動車の加速度を
検出する必要がある。(Equation 3) Becomes By repeating this weighted addition operation,
_It can be seen that _n converges to θ _m . Correction of pitch angle Since the angle in the pitching direction changes in the detection angle of the inclinometer 11 due to the acceleration generated by the acceleration and deceleration of the vehicle, it is necessary to detect the acceleration of the vehicle before the correction by the equation (4). .

【００８５】図４に加速度の検出原理図を示す。図４に
示すように、ジャイロの検出方向に対して加速度が加わ
ると、重力と加速度との合成力が振り子に加わる。この
ときの加速度ａと重力Ｇと振り子の角度θとの関係は ａ＝ｔａｎθ・Ｇ となる。則ち、図４に示すθが誤差角度として傾斜計１
１に余計に加わることになる。FIG. 4 shows the principle of acceleration detection. As shown in FIG. 4, when acceleration is applied in the gyro detection direction, a combined force of gravity and acceleration is applied to the pendulum. At this time, the relationship between the acceleration a, the gravity G, and the pendulum angle θ is a = tan θ · G. That is, θ shown in FIG.
It is an extra addition to 1.

【００８６】そこで、加速度を検出し誤差角度θが一定
以下の場合にだけ計測を行うようにすれば、加速・減速
による加速度の影響を受けずに測定が行える。例えば、
誤差角度を１°とすると、姿勢検出を禁止すべき加速度
ａ_inh は ａ_inh ≧ｔａｎ（１°）・９．８〔ｍ／ｓ² 〕＝０．１
７〔ｍ／ｓ² 〕 というように、検出を禁止すべき加速度を得ることがで
きる。Therefore, if the acceleration is detected and the measurement is performed only when the error angle θ is less than a certain value, the measurement can be performed without being affected by the acceleration due to the acceleration / deceleration. For example,
If the error angle is 1 °, the acceleration a _{inh for} which posture detection should be prohibited is a _inh ≧ tan (1 °) · 9.8 [m / s ² ] = 0.1
It is possible to obtain the acceleration at which detection should be prohibited, such as 7 [m / s ² ].

【００８７】そして、重み付け加算の演算では加速度の
値に応じて式（４）における係数Ｊを変更する。例え
ば、 停止中 ：車速パルスＰ_v ＝０でＪ_p ＝１ 通常運転：加速度ａ＜０．１７でＪ_p ＝０．００１ 加速中 ：加速度ａ≧０．１７でＪ_p ＝０ というように、係数Ｊ_p を設定する。Then, in the weighted addition calculation, the coefficient J in the equation (4) is changed according to the acceleration value. For example, during stop: vehicle speed pulse P _v = 0, J _p = 1 normal operation: acceleration a <0.17, J _p = 0.001 during acceleration: acceleration a ≧ 0.17, J _p = 0. Set the coefficient J _p .

【００８８】従って、重み付け加算回路１３は、車速パ
ルスＰ_v の変化率により加速度を算出する加速度計算回
路２７の出力をパラメータとして、上記の条件に従い係
数Ｊを適宜変更して式（８）に基づいて計算を行う。Therefore, the weighting addition circuit 13 appropriately changes the coefficient J according to the above condition by using the output of the acceleration calculation circuit 27 for calculating the acceleration based on the change rate of the vehicle speed pulse P _v as a parameter, and based on the equation (8). Calculate.

【００８９】 θ_p ＝θ_anp ＋Ｊ_p ・（θ_mnp −θ_anp ） …（８） （０≦Ｊ_p ≦１） これによれば、停止中は、静止状態での精度のよい傾斜
計１１の出力がそのままピッチ角度とされる。通常運転
中は振動ジャイロ１４の出力に基づくピッチ仮角度を傾
斜計１１の出力する正確な長周期の角度に徐々に収束さ
せ、加速度の変化が激しいときは振動ジャイロ１４の出
力のみで短期の角度検出を行う。 ロール角の補正 ローリング方向の角度は、自動車が曲がるときに生ずる
遠心力によって、傾斜計１７の検出角度に変動が生ずる
ため、補正を行う前に自動車の遠心力を検出する必要が
ある。Θ _p = θ _anp + J _p · (θ _mnp −θ _anp ) ... (8) (0 ≦ J _p ≦ 1) According to this, the inclinometer 11 with high accuracy in the stationary state can be used during the stop. The output is directly used as the pitch angle. During normal operation, the temporary pitch angle based on the output of the vibration gyro 14 is gradually converged to an accurate long-cycle angle output by the inclinometer 11, and when the acceleration changes drastically, only the output of the vibration gyro 14 is used for a short-term angle. Detect. Correction of the roll angle The angle in the rolling direction must be detected before the correction because the detection angle of the inclinometer 17 varies due to the centrifugal force generated when the vehicle bends.

【００９０】図５に遠心力の検出原理説明図を示す。図
５に示すように、自動車が所定の回転半径ｒでカーブす
るとき、遠心力Ｆが働く。このときの遠心力Ｆによる加
速度は、Ｆ＝ｍ・ｒ・ｗ₂ により得られる。また、角速
度と接線方向の速度ｖとの関係はｖ＝ｒ・ｗであるか
ら、横方向の加速度ａは、 Ｆ＝ｍ・ａ＝ｍ・ｒ・ｗ₂ ＝ｍ・ｖ・ｗ ∴ａ＝ｖ・ｗ …（９） により、車速パルスＰ_v による速度ｖと、ヨーイング方
向の角速度ｗが判れば算出できる。FIG. 5 is a diagram for explaining the principle of centrifugal force detection. As shown in FIG. 5, when an automobile curves at a predetermined turning radius r, centrifugal force F acts. The acceleration due to the centrifugal force F at this time is obtained by F = m · r · w ₂ . Further, since the relationship between the angular velocity and the velocity v in the tangential direction is v = r · w, the lateral acceleration a is F = m · a = m · r · w ₂ = m · v · w ∴a = v · w ... (9), it can be calculated and the velocity v by the vehicle speed pulse P _v, knowing the yawing direction of the angular velocity w.

【００９１】よって、加速度と傾斜計１７に及ぼす誤差
角度との関係は図４に示した通りなので、遠心力計算回
路２６は式（６）に基づいて自動車の横方向に対する加
速度を計算する。重み付け加算回路２１は、例えば下記
の条件に基づいて係数Ｊ_r を適宜変更し、式（１０）に
基づいてロール角度の真値θ_r を得ることができる。Therefore, since the relationship between the acceleration and the error angle exerted on the inclinometer 17 is as shown in FIG. 4, the centrifugal force calculation circuit 26 calculates the lateral acceleration of the vehicle based on the equation (6). The weighting addition circuit 21 can appropriately change the coefficient J _r based on the following conditions, for example, and can obtain the true value θ _r of the roll angle based on the equation (10).

【００９２】停止中 ：車速パルスＰ_v ＝０でＪ_p ＝１ 通常運転：加速度ａ＜０．１７でＪ_p ＝０．００１ 加速中 ：加速度ａ≧０．１７でＪ_p ＝０ θ_r ＝θ_anr ＋Ｊ_r ・（θ_mnr −θ_anr ） …（１０） （０≦Ｊ_r ≦１） ヨー角の補正 ヨーイング方向の角度を検出のためには、振動ジャイロ
８の出力補正に地磁気センサ２の出力を用いる。しか
し、ピッチ角とロール角が存在している場合、則ち、絶
対座標系のＸ−Ｙ平面と同じ面に移動体座標系のｘ−ｙ
軸がない場合は、地磁気センサの検出コイルを通過する
磁束数が変化するため、地磁気センサに誤差を生じてし
まう。During stop: Vehicle speed pulse P _v = 0, J _p = 1 Normal operation: Acceleration a <0.17, J _p = 0.001 During acceleration: Acceleration a ≧ 0.17, J _p = 0 θ _r = θ _anr + J _r · (θ _mnr −θ _anr ) (10) (0 ≦ J _r ≦ 1) Correction of yaw angle In order to detect the angle in the yawing direction, the output of the vibration gyro 8 is corrected by the geomagnetic sensor 2. Use output. However, when the pitch angle and the roll angle exist, the xy of the moving body coordinate system is on the same plane as the XY plane of the absolute coordinate system.
When there is no axis, the number of magnetic fluxes passing through the detection coil of the geomagnetic sensor changes, which causes an error in the geomagnetic sensor.

【００９３】ソフトウェアジンバル３では、真値として
得られたピッチ角θ_p とロール角θ _r とを用いて地磁気
センサの誤差を補正する。図６にソフトウェアジンバル
で行われる地磁気センサの出力補正の原理説明図を示
す。In software gimbal 3, as the true value
Obtained pitch angle θ_pAnd roll angle θ _rWith and geomagnetism
Correct the sensor error. Software gimbal in Figure 6
An explanatory diagram of the principle of the output correction of the geomagnetic sensor performed in
You.

【００９４】図６において円Ｃ₀ は磁気円を示す。磁気
円というのは、地磁気センサを移動体に搭載して一定時
間走行しながら、地磁気センサのｘ軸の検出コイルから
の電圧とｙ軸の検出コイルからの電圧とをｘ−ｙ座標系
に順次プロットしていくことにより、プロットの集合と
して描かれる一定の半径の円のことをいう。磁北とこの
ｘ−ｙ座標系の特定方向を一致させることにより、プロ
ットのそれぞれは、例えばｙ軸の正方向を「北」とした
場合の方角を示すことになる。則ち、地磁気センサの出
力はヨーイング方向の角度を示している。In FIG. 6, the circle C ₀ represents a magnetic circle. The magnetic circle means that the voltage from the x-axis detection coil and the voltage from the y-axis detection coil of the geomagnetic sensor are sequentially transferred to the xy coordinate system while the geomagnetic sensor is mounted on a moving body and the vehicle travels for a certain period of time. By plotting, it means a circle with a constant radius drawn as a set of plots. By matching the magnetic north and the specific direction of this xy coordinate system, each of the plots indicates the direction when the positive direction of the y-axis is “north”. That is, the output of the geomagnetic sensor indicates the angle in the yawing direction.

【００９５】さて、地磁気は水平面に対して所定の伏角
（例えば、関東周辺では５０°）を有しているため、自
動車に地磁気センサを搭載した際、自動車の姿勢が変化
すると、ｘ軸、ｙ軸のそれぞれの検出コイルを通過する
磁束数が変化する。Since the geomagnetism has a predetermined dip angle with respect to the horizontal plane (for example, 50 ° around the Kanto region), when the geomagnetic sensor is mounted on an automobile, if the posture of the automobile changes, the x axis and y The number of magnetic flux passing through each detection coil of the shaft changes.

【００９６】例えば、自動車に備えられた地磁気センサ
が水平状態でＰ₀ の出力を示しているときに自動車の姿
勢がピッチング方向、ローリング方向に乱れると、図６
の原点Ｐ₀ を中心とする座標軸で示されるような出力の
変化を示す。則ち、自動車が水平状態のとき角度θのヨ
ー角を示している場合、ピッチング方向に−２０°偏位
すると地磁気センサのプロットは点Ｐ₁ に移動し、ヨー
角はθ_p に変化する。また、ローリング方向に＋２０°
偏位すると地磁気センサのプロットは点Ｐ₂ に移動し、
ヨー角はθ_r に変化する。さらに、ピッチング方向に−
２０°、ローリング方向に−２０°偏位したときはプロ
ットは点Ｐ₃ に移動し、ヨー角はθ_prに変化する。For example, if the attitude of the vehicle is disturbed in the pitching direction and the rolling direction when the geomagnetic sensor provided in the vehicle shows the output P _{0 in} the horizontal state, FIG.
The change of the output is shown as indicated by the coordinate axis having the origin P ₀ as the center. In other words, when the yaw angle of the angle θ is shown when the vehicle is in the horizontal state, the plot of the geomagnetic sensor moves to the point P ₁ and the yaw angle changes to θ _p when deviated by −20 ° in the pitching direction. Also, + 20 ° in the rolling direction
When deviated, the plot of the geomagnetic sensor moves to point P ₂ ,
The yaw angle changes to θ _r . Furthermore, in the pitching direction −
When deviated by 20 ° and -20 ° in the rolling direction, the plot moves to the point P ₃ and the yaw angle changes to θ _pr .

【００９７】上記の性質に鑑み、姿勢の乱れた自動車上
で得られる地磁気センサの出力を水平状態での正規の出
力に補正するための演算式を導くことができる。ｘを地
磁気センサのｘ軸検出コイルからの出力、ｙを地磁気セ
ンサのｙ軸検出コイルからの出力、ｒを使用する地磁気
センサ固有の磁気円の半径、φ_p をピッチ角、φ_r をロ
ール角、ピッチ角又はロール角が１°傾く度に磁気円の
半径の何パーセントの割合で図６に示すような地磁気セ
ンサのプロットが移動するかを示す変換定数をＪとする
と、補正後の地磁気センサの出力（Ｘ，Ｙ）は、 Ｘ＝ｘ＋Ｊ・φ_r ・ｒ Ｙ＝ｙ−Ｊ・φ_p ・ｒ …（１１） という関係により導くことができる。変換定数は、例え
ば、伏角５０°の状態であって磁気円の半径が３４〔m
m〕とすると、３０°のピッチ角又はロール角の偏位で
プロットが１７．５〔mm〕移動することが測定されてい
るので、 Ｊ＝（１７．５／３４）×（１／３０）＝１．７〔%/de
g 〕 となる。この定数は伏角により徐々に変化するので、変
換定数のテーブル等を用意し、地磁気センサの使用地域
により定数の値を適宜変更していくことになる。In view of the above-mentioned properties, it is possible to derive an arithmetic expression for correcting the output of the geomagnetic sensor obtained on the automobile with a distorted posture to the normal output in the horizontal state. x is the output from the x-axis detection coil of the geomagnetic sensor, y is the output from the y-axis detection coil of the geomagnetic sensor, r is the radius of the magnetic circle specific to the geomagnetic sensor, φ _p is the pitch angle, and φ _r is the roll angle , J is a conversion constant that indicates the percentage of the radius of the magnetic circle that the plot of the geomagnetic sensor moves as shown in FIG. 6 each time the pitch angle or roll angle is inclined by 1 °, and the corrected geomagnetic sensor The output (X, Y) can be derived by the following relationship: X = x + J · φ _r · r Y = y−J · φ _p · r (11) The conversion constant is, for example, when the dip angle is 50 ° and the radius of the magnetic circle is 34 [m
m], it is measured that the plot moves 17.5 [mm] with a pitch angle or roll angle deviation of 30 °, so J = (17.5 / 34) × (1/30) = 1.7 [% / de
g]. Since this constant gradually changes depending on the dip angle, a table of conversion constants or the like is prepared, and the value of the constant will be appropriately changed depending on the region where the geomagnetic sensor is used.

【００９８】誤差のない真値のピッチ角、ロール角を用
いれば、式（１１）によりジンバル機構等を用いなくて
も、正確なロール角を得ることができる。本実施例で
は、ソフトウェアジンバル回路３が、地磁気センサ２か
らの出力ｘ、ｙを入力し、加速度、遠心力の影響を取り
除いたθ_p 、θ_r を式（１１）に代入する。そして、内
部に保持する変換定数Ｊと磁気円の半径を用いて、ソフ
トウェアによる演算で疑似的に水平状態の地磁気センサ
出力を生成する。従って、ソフトウェアジンバル回路３
の作用により、高価なジンバル機構を用いることなく、
ジンバル機構を備えた地磁気センサとほぼ同等の出力を
得ることができる。 着磁補正 自動車の車体に着磁が発生すると地磁気センサは、正確
な出力を得ることができないため、着磁補正の必要があ
る。着磁補正とは、地磁気センサや自動車の車体が磁化
された場合（着磁という。）に、この着磁による磁界が
あっても、着磁がないときと同一の方位角を示すことが
できるよう、磁気円の中心座標を移動する補正をいう。If the true pitch angle and roll angle without error are used, an accurate roll angle can be obtained according to the equation (11) without using a gimbal mechanism or the like. In the present embodiment, the software gimbal circuit 3 inputs the outputs x and y from the geomagnetic sensor 2 and substitutes θ _p and θ _{r from} which the effects of acceleration and centrifugal force are removed into the equation (11). Then, by using the conversion constant J and the radius of the magnetic circle held inside, a pseudo horizontal magnetic field sensor output is generated by calculation by software. Therefore, the software gimbal circuit 3
By the action of, without using an expensive gimbal mechanism,
It is possible to obtain an output almost equal to that of a geomagnetic sensor equipped with a gimbal mechanism. Magnetization correction When the car body of the automobile is magnetized, the geomagnetic sensor cannot obtain an accurate output, and thus it is necessary to correct the magnetization. The magnetization correction can indicate the same azimuth angle as when there is no magnetization even when there is a magnetic field due to the magnetization when the geomagnetic sensor or the vehicle body of the automobile is magnetized (referred to as magnetization). As described above, it refers to correction for moving the center coordinates of the magnetic circle.

【００９９】図７に着磁補正の概念図を示す。例えば、
図７に示すように、着磁する前の磁気円の中心座標（着
磁ベクトル）が（ｘ_d 、ｙ_d ）で与えられる場合に、ヨ
ー角θを指し示すとき地磁気センサのｘ軸検知コイル及
びｙ軸検知コイルそれぞれからの出力はプロット（Ｘ，
Ｙ）であるとする。ここで、この状態で車体又は地磁気
センサに着磁することにより、所定の向きと大きさの磁
界Ｂが地磁気センサに与えられる場合、地磁気センサか
ら得られる出力によりプロットは、プロット（Ｘ，Ｙ）
に向かうベクトルと磁界Ｂとの合成した位置（Ｘ’，
Ｙ’）に現れる。このとき、検出されるヨー角はθ’と
なり、地磁気センサの指し示す方位角に誤差が生ずる。FIG. 7 shows a conceptual diagram of the magnetization correction. For example,
As shown in FIG. 7, when the center coordinates (magnetization vector) of the magnetic circle before magnetization is given by (x _d , y _d ), the x-axis detection coil of the geomagnetic sensor and the yaw angle θ are indicated. The output from each y-axis sensing coil is plotted (X,
Y). Here, when a magnetic field B of a predetermined direction and magnitude is given to the geomagnetic sensor by magnetizing the vehicle body or the geomagnetic sensor in this state, the plot obtained by the geomagnetic sensor is a plot (X, Y).
Vector (X ',
Y '). At this time, the detected yaw angle becomes θ ′, and an error occurs in the azimuth angle indicated by the geomagnetic sensor.

【０１００】これを補正するには、着磁前の着磁ベクト
ル（ｘ_d 、ｙ_d ）を着磁後の着磁ベクトル（ｘ_d ’、ｙ
_d ’）に変化させれば、磁界Ｂが存在していても、得ら
れるヨー角はθとなり着磁前の値を示すことになる。To correct this, the magnetization vector before magnetization (x _d , y _d ) is changed to the magnetization vector (x _d ', y after magnetization).
If the magnetic field B is present, the obtained yaw angle becomes θ, which is the value before magnetization, if _d ′) is changed.

【０１０１】本実施例では、振動ジャイロ８を中心に式
（４）に準ずる収束演算の式（１１）により算出される
ヨー角θ_y θ_y ＝θ_any ＋Ｊ_y ・（θ_mny −θ_any ） …（１２） （０≦Ｊ_y ≦１） と、ソフトウェアジンバル回路３にて水平状態に変換さ
れた地磁気センサにより得られる仮のヨー角θ_mny と、
の間に相違が生じたとき、着磁ベクトルが変更された
（着磁した）と判断する。重み付け加算回路６は式（１
１）における準ずる重み付け加算の演算として係数Ｊ_y
＝０とし、振動ジャイロの値のみによりヨー角θ_y を算
出する。そして、振動ジャイロ８による累積誤差が蓄積
する前に、着磁補正回路７が着磁ベクトルの微調整を試
みる。地磁気方位計算回路４の出力がヨー角θ_y にほぼ
収束するのを条件として、着磁ベクトル更新回路５が着
磁ベクトルを新たな値に更新する。着磁ベクトルの更新
が終了すると、重み付け加算回路６は、再び、収束係数
を適当な値（例えば、Ｊ_y ＝０．００１）として重み付
け加算演算を行う。 （III ）実施例の効果 上記の如く、本実施例によれば、自動車の急発振や、急
停車、カーブにより、自動車の進行方向や左右方向に加
速度が働いても、遠心力計算回路、加速度計算回路がこ
れら加速度成分を検知し、重み付け加算回路が収束係数
の変更を行うので、安定した走行状態のときのみ地磁気
センサ、傾斜計と振動ジャイロとの併用による精度の高
い姿勢検出動作が行える。従って、出力されるヨー角、
ピッチ角、ロール角は、自動車の走行状態によらず大き
な変動を生じないので、これを座標変換することによ
り、常に衛星の方向にアンテナを向けることができ、良
好な受信状態が維持できる。In this embodiment, the yaw angle θ _y θ _y = θ _any + J _y · (θ _mny −θ _any ) calculated by the equation (11) of the convergence calculation based on the equation (4) with the vibration gyro 8 as the center. (12) (0 ≦ J _y ≦ 1), and a temporary yaw angle θ _mny obtained by the geomagnetic sensor converted into the horizontal state by the software gimbal circuit 3,
When there is a difference between the two, it is determined that the magnetization vector has been changed (magnetized). The weighted addition circuit 6 uses the equation (1
As the calculation of the quasi-weighted addition in 1), the coefficient J _y
= 0, and the yaw angle θ _y is calculated only by the value of the vibration gyro. Then, before the accumulated error due to the vibration gyro 8 is accumulated, the magnetization correction circuit 7 attempts a fine adjustment of the magnetization vector. The magnetizing vector updating circuit 5 updates the magnetizing vector to a new value on condition that the output of the geomagnetic direction calculating circuit 4 substantially converges to the yaw angle θ _y . When the updating of the magnetization vector is completed, the weighting addition circuit 6 again performs the weighting addition operation with the convergence coefficient set to an appropriate value (for example, J _y = 0.001). (III) Effects of the Embodiment As described above, according to the present embodiment, even if acceleration occurs in the traveling direction or the left-right direction of the vehicle due to the sudden oscillation, sudden stop, or curve of the vehicle, the centrifugal force calculation circuit and the acceleration calculation are performed. Since the circuit detects these acceleration components and the weighting addition circuit changes the convergence coefficient, highly accurate posture detection operation can be performed by using the geomagnetic sensor, the inclinometer and the vibration gyro only in a stable traveling state. Therefore, the output yaw angle,
Since the pitch angle and the roll angle do not largely change regardless of the running state of the automobile, by converting the coordinates, the antenna can always be directed in the direction of the satellite, and a good reception state can be maintained.

【０１０２】[0102]

【発明の効果】以上の通り、請求項１乃至請求項６に記
載の本発明によれば、移動体の姿勢に乱れによる地磁気
センサの値を水平状態等の正規の状態での値に補正する
ように構成したので、移動体がどのような動きをしてい
ても真値の移動体の方位角を検出できる。このため、車
載用追尾アンテナの為の姿勢検出の他、ＲＶ車への搭載
も考えられる。As described above, according to the present invention as set forth in claims 1 to 6, the value of the geomagnetic sensor due to the disturbance of the posture of the moving body is corrected to the value in the normal state such as the horizontal state. With this configuration, the true value of the azimuth angle of the moving body can be detected regardless of the movement of the moving body. Therefore, in addition to the attitude detection for the vehicle-mounted tracking antenna, it may be installed in an RV vehicle.

【０１０３】請求項３に記載の発明によれば、上記効果
の他、ピッチング方向、ローリング方向に係る加速、減
速による加速度、カーブ走行による遠心力の影響を取り
除いたピッチ角度、ロール角度を得ることができる。According to the third aspect of the present invention, in addition to the above effects, it is possible to obtain a pitch angle and a roll angle by eliminating the effects of the acceleration in the pitching direction and the rolling direction, the acceleration due to the deceleration, and the centrifugal force due to the curve running. You can

【０１０４】請求項４に記載の発明によれば、上記効果
の他、加速度の量に応じて最適な収束係数を選ぶことが
でき、移動体が安定しているときは速い収束、安定度が
少ないときは遅い収束というように、最適な収束特性を
得ることができる。According to the invention described in claim 4, in addition to the above effects, the optimum convergence coefficient can be selected according to the amount of acceleration, and when the moving body is stable, fast convergence and stability can be achieved. Optimal convergence characteristics such as slow convergence when the number is small can be obtained.

【０１０５】請求項５に記載の発明によれば、上記効果
の他、遠心力の量に応じて最適な収束係数を選ぶことが
でき、移動体が安定しているときは速い収束、安定度が
少ないときは遅い収束というように、最適な収束特性を
得ることができる。According to the invention described in claim 5, in addition to the above effects, an optimum convergence coefficient can be selected according to the amount of centrifugal force, and when the moving body is stable, fast convergence and stability are achieved. Optimal convergence characteristics can be obtained, such as slow convergence when is small.

【０１０６】請求項６に記載の発明によれば、第１係数
及び第２係数を地磁気方位検出手段の特性に応じて適当
に選ぶ、ピッチ角、ロール角を用いて簡単に水平状態で
の正確な地磁気方位を得ることができる。According to the sixth aspect of the present invention, the first coefficient and the second coefficient are appropriately selected according to the characteristics of the geomagnetic azimuth detecting means. Pitch angle and roll angle are used to easily and accurately measure in the horizontal state. It is possible to obtain various geomagnetic directions.

【図面の簡単な説明】[Brief description of drawings]

【図１】実施例の姿勢検出装置を示す構成図である。FIG. 1 is a configuration diagram illustrating a posture detection device according to an embodiment.

【図２】実施例のアンテナ追従装置の全体図である。FIG. 2 is an overall view of an antenna tracking device according to an embodiment.

【図３】実施例のアンテナサーボ系の構成図である。FIG. 3 is a configuration diagram of an antenna servo system according to an embodiment.

【図４】加速度の検出原理図である。FIG. 4 is a principle diagram of acceleration detection.

【図５】遠心力の検出原理図である。FIG. 5 is a principle diagram of centrifugal force detection.

【図６】地磁気センサの出力補正の原理説明図である。FIG. 6 is a diagram illustrating the principle of output correction of the geomagnetic sensor.

【図７】着磁補正の原理説明図である。FIG. 7 is a diagram illustrating the principle of magnetization correction.

【図８】ヨー・ピッチ・ロールの説明図である。FIG. 8 is an explanatory diagram of a yaw pitch roll.

【符号の説明】[Explanation of symbols]

１…絶対座標角度計算回路 ２…地磁気センサ ３…ソフトウェアジンバル ４…地磁気方位計算回路 ５…着磁ベクトル補正回路 ６、１３、２１…重み付け計算回路 ７…着磁補正回路 ８、１４、１８…振動ジャイロ ９、１５、１９…オフセットリセット回路 １０、１６…積分回路 １１、１７…傾斜計 １２…ロール影響補正回路 ２２…座標変換回路 ２３…ベクトル変換回路 ２４…オイラー角変換回路 ２５…シリアル通信回路 ２６…遠心力計算回路 ２７…加速度計算回路 ３０…マイコン ３１…ハードウェア ４０…位置制御器 ４１…速度制御器 ４３…電流制御器 ４４…電力増幅器 ４５…モータ ４６…エンコーダ ４７…速度検出部 １００…姿勢検出回路 ２００…コントローラ ３００…アンテナメカ ３００_-1…アンテナ本体1 ... Absolute coordinate angle calculation circuit 2 ... Geomagnetic sensor 3 ... Software gimbal 4 ... Geomagnetic direction calculation circuit 5 ... Magnetization vector correction circuit 6, 13, 21 ... Weighting calculation circuit 7 ... Magnetization correction circuit 8, 14, 18 ... Vibration Gyro 9, 15, 19 ... Offset reset circuit 10, 16 ... Integration circuit 11, 17 ... Inclinometer 12 ... Roll effect correction circuit 22 ... Coordinate conversion circuit 23 ... Vector conversion circuit 24 ... Euler angle conversion circuit 25 ... Serial communication circuit 26 Centrifugal force calculation circuit 27 ... Acceleration calculation circuit 30 ... Microcomputer 31 ... Hardware 40 ... Position controller 41 ... Speed controller 43 ... Current controller 44 ... Power amplifier 45 ... Motor 46 ... Encoder 47 ... Speed detection unit 100 ... Attitude Detection circuit 200 ... Controller 300 ... Antenna mechanism _300-1 ... Antenna body

───────────────────────────────────────────────────── フロントページの続き (72)発明者 栗原 正樹 埼玉県川越市大字山田字西町25番地１ パ イオニア株式会社川越工場内 ─────────────────────────────────────────────────── ─── Continuation of the front page (72) Inventor Masaki Kurihara 25 Nishimachi, Yamada, Kawagoe City, Saitama Prefecture 1 Pioneer Co., Ltd. Kawagoe Factory

Claims (6)

【特許請求の範囲】[Claims] 【請求項１】 移動体に備えられ自らの内部を通過する
地磁気の磁束密度の変化により方位角を測定する地磁気
測定手段を有する姿勢検出装置の姿勢検出方法であっ
て、 所定のタイミング毎に、当該移動体がローリングすると
きのロール角及び当該移動体がピッチングするときのピ
ッチ角を検出し、当該ロール角及び当該ピッチ角に基づ
いて当該ロール角及び当該ピッチ角による当該移動体の
姿勢の乱れによって生ずる前記地磁気測定手段の出力の
誤差を補償することを特徴とする姿勢検出方法。1. A posture detection method for a posture detection device having a geomagnetic measurement means for measuring an azimuth angle by a change in the magnetic flux density of the geomagnetism which is provided in a moving body and passes through the inside thereof, wherein the posture detection method comprises: The roll angle when the moving body rolls and the pitch angle when the moving body pitches are detected, and the roll angle and the posture angle of the moving body due to the pitch angle are disturbed based on the roll angle and the pitch angle. An attitude detection method, which comprises compensating an error in the output of the geomagnetism measuring means caused by 【請求項２】 移動体に備えられ自らの内部を通過する
地磁気の磁束密度の変化により仮方位角を検出する地磁
気検出手段と、 当該移動体のピッチングするときのピッチ角を検出する
ピッチ角検出手段と、 移動体のローリングするときのロール角を検出するロー
ル角検出手段と、 前記ピッチ角と所定の第１係数を乗ずることにより前記
仮方位角に対する前記ピッチ角による第１補正量を決定
し、前記ロール角と所定の第２係数を乗ずることにより
前記仮方位角に対する前記ロール角による第２補正量を
決定し、当該第１補正量及び当該第２補正量に基づいて
前記仮方位角を補正し、方位角を生成する地磁気方位補
正手段と、 を備えたことを特徴とする姿勢検出装置。2. A geomagnetism detecting means which is provided in a moving body and detects a provisional azimuth angle by a change in the magnetic flux density of the geomagnetism passing through the inside of the moving body, and a pitch angle detection which detects a pitch angle when the moving body is pitched. Means, a roll angle detecting means for detecting a roll angle when the moving body rolls, and a first correction amount by the pitch angle with respect to the temporary azimuth angle is determined by multiplying the pitch angle by a predetermined first coefficient. , A second correction amount by the roll angle with respect to the temporary azimuth angle is determined by multiplying the roll angle by a predetermined second coefficient, and the temporary azimuth angle is calculated based on the first correction amount and the second correction amount. An attitude detection device comprising: a geomagnetic azimuth correction unit that corrects and generates an azimuth angle. 【請求項３】 移動体に備えられ自らの内部を通過する
地磁気の磁束密度の変化により仮方位角を検出する地磁
気検出手段と、 当該移動体のピッチングする方向への角速度を検出する
ピッチ角速度検出手段と、 当該移動体のローリングする方向への角速度を検出する
ロール角速度検出手段と、 当該移動体のピッチング軸に垂直な平面に投映した当該
移動体に加わる加速度成分と重力加速度との合成ベクト
ルが、当該移動体のローリング軸となす傾斜角を検出す
るピッチング傾斜角検出手段と、 当該移動体のローリング軸に垂直な平面に投映した当該
移動体に加わる加速度成分と重力加速度との合成ベクト
ルが、当該移動体のピッチング軸となす傾斜角を検出す
るローリング傾斜角検出手段と、 当該移動体の移動により当該移動体に加わる加速度を測
定する加速度測定手段と、 当該移動体の移動により当該移動体に加わる遠心力を測
定する遠心力測定手段と、 前記ピッチングする方向への角速度の積分値に基づいて
ピッチングする方向への仮偏位角を計算し、前記ローリ
ングする方向への角速度の積分値に基づいてローリング
する方向への仮偏位角を計算する仮偏位角計算手段と、 前記加速度に対応させて前記ピッチングする方向への仮
偏位角に対する前記ピッチングする方向への傾斜角によ
る補正量を規定するピッチング重み付け係数を決定し、
当該ピッチング重み付け係数に基づいて前記加速度の影
響を取り除いたピッチ角を出力するピッチング重み付け
演算手段と、 前記遠心力に対応させて前記ローリングする方向への仮
偏位角に対する前記ローリングする方向への傾斜角によ
る補正量を規定するローリング重み付け係数を決定し、
当該ローリング重み付け係数に基づいて前記遠心力の影
響を取り除いたロール角を出力するローリング重み付け
演算手段と、 前記ピッチ角と所定の第１係数を乗ずることにより前記
仮方位角に対する前記ピッチングする方向への仮偏位角
による第１補正量を決定し、前記ロール角と所定の第２
係数を乗ずることにより前記仮方位角に対する前記ロー
ル角による第２補正量を決定し、当該第１補正量及び当
該第２補正量に基づいて前記仮方位角を補正し、方位角
を生成する地磁気方位補正手段と、 を備えたことを特徴とする姿勢検出装置。3. A geomagnetism detecting means, which is provided in a moving body, detects a temporary azimuth angle by a change in magnetic flux density of the geomagnetism passing through the inside of the moving body, and pitch angular velocity detection for detecting an angular velocity of the moving body in a pitching direction. Means, a roll angular velocity detecting means for detecting the angular velocity of the moving body in the rolling direction, and a composite vector of the acceleration component applied to the moving body and the gravitational acceleration projected on the plane perpendicular to the pitching axis of the moving body. , A pitching inclination angle detecting means for detecting an inclination angle formed with the rolling axis of the moving body, and a composite vector of an acceleration component applied to the moving body and a gravitational acceleration projected on a plane perpendicular to the rolling axis of the moving body, A rolling tilt angle detection means for detecting a tilt angle formed with the pitching axis of the moving body, and an addition to the moving body due to the movement of the moving body. Acceleration measuring means for measuring the degree of rotation, centrifugal force measuring means for measuring the centrifugal force applied to the moving body by the movement of the moving body, and temporary movement in the pitching direction based on the integrated value of the angular velocity in the pitching direction. Temporary deviation angle calculating means for calculating a deviation angle and calculating a temporary deviation angle in a rolling direction based on an integrated value of angular velocities in the rolling direction; and a pitching direction corresponding to the acceleration. Determining a pitching weighting coefficient that defines a correction amount according to the inclination angle in the pitching direction with respect to the temporary deviation angle to
Pitching weighting calculation means for outputting a pitch angle in which the influence of the acceleration is removed based on the pitching weighting coefficient, and inclination in the rolling direction with respect to the temporary deviation angle in the rolling direction corresponding to the centrifugal force. Determine the rolling weighting coefficient that defines the correction amount by the angle,
Rolling weighting calculation means for outputting a roll angle from which the influence of the centrifugal force is removed based on the rolling weighting coefficient, and multiplying the pitch angle by a predetermined first coefficient in the pitching direction with respect to the temporary azimuth angle. The first correction amount based on the temporary deviation angle is determined, and the roll angle and the predetermined second
A geomagnetism that determines a second correction amount by the roll angle with respect to the temporary azimuth angle by multiplying by a coefficient, corrects the temporary azimuth angle based on the first correction amount and the second correction amount, and generates an azimuth angle. A posture detecting device comprising: an azimuth correcting means; 【請求項４】 請求項２又は請求項３に記載の姿勢検出
装置において、 前記ピッチングする方向への仮偏位角をθ_anp とし、前
記ピッチングする方向への傾斜角をθ_mnp とし、前記ピ
ッチング重み付け係数をＪ_p としたとき、前記補正され
たピッチ角θ_p は、 θ_p ＝θ_anp ＋Ｊ_p ・（θ_mnp −θ_anp ） （０≦
Ｊ_p ≦１） という関係により算出されることを特徴とする姿勢検出
装置。4. The posture detecting device according to claim 2 or claim 3, the temporary deflection angle in the direction of the pitching and theta _ANP, the inclination angle to the pitching directions and theta _mnp, the pitching When the weighting coefficient is J _p , the corrected pitch angle θ _p is θ _p = θ _anp + J _p · (θ _mnp −θ _anp ) (0 ≦
A posture detection device characterized by being calculated according to a relationship of J _p ≦ 1). 【請求項５】 請求項２又は請求項３に記載の姿勢検出
装置において、 前記ローリングする方向への仮偏位角をθ_anr とし、前
記ローリングする方向への傾斜角をθ_mnr とし、前記ロ
ーリング重み付け係数をＪ_r としたとき、前記補正され
たロール角θ_r は、 θ_r ＝θ_anr ＋Ｊ_r ・（θ_mnr −θ_anr ） （０≦
Ｊ_r ≦１） という関係により算出されることを特徴とする姿勢検出
装置。5. The posture detecting device according to claim 2, wherein the provisional deviation angle in the rolling direction is θ _anr, and the inclination angle in the rolling direction is θ _mnr. When the weighting coefficient is J _r , the corrected roll angle θ _r is θ _r = θ _anr + J _r · (θ _mnr −θ _anr ) (0 ≦
A posture detecting device characterized by being calculated according to a relationship of J _r ≦ 1). 【請求項６】 請求項２乃至請求項５に記載の姿勢検出
装置において、 前記仮方位角の座標を（ｘ、ｙ）とし、前記第１係数又
は前記第２係数をＪとし、前記ピッチ角をθ_p とし、前
記ロール角をθ_r とたとき、当該移動体の姿勢の乱れを
取り除いた方位角の座標（Ｘ，Ｙ）は、 Ｘ＝ｘ＋Ｊ・θ_r Ｙ＝ｙ−Ｊ・θ_p という関係により算出されることを特徴とする姿勢検出
装置。6. The attitude detecting device according to claim 2, wherein the coordinates of the temporary azimuth are (x, y), the first coefficient or the second coefficient is J, and the pitch angle is Is θ _p and the roll angle is θ _r , the coordinates (X, Y) of the azimuth angle from which the disturbance of the posture of the moving body is removed are: X = x + J · θ _r Y = y−J · θ _p A posture detection device characterized by being calculated according to the following relationship.