JP3576728B2 - Attitude detection device - Google Patents

Description

【００２９】
で表される。
また、逆に、（ｘ，ｙ，ｚ）座標系から（ｘｈ，ｙｈ，ｚｈ）座標系への変換は、
【００３０】
【数２】

【００３１】
で表される。
実際の状態として、（ｘｈ，ｙｈ，ｚｈ）座標系を地表面の座標系（重力が「−ｚ」方向）と考えると、（ｘ，ｙ，ｚ）座標系は例えば船と考えられる。このとき、ベクトルＨ（→）を地磁気と考えると、
ＨＺＨ≒０ ・・・（３）
と近似できる。従って、ベクトルＨ（→）の大きさ、即ち、フラックスゲートセンサ２Ａの水平時の振幅，φ，θが分かると、（１）式よりＨＺを計算できる。
【００３２】
ＨＺ＝−ＨＸＨｓｉｎθ＋ＨＹＨ・ｃｏｓθ・ｓｉｎφ ・・・（４）
マトリックスの式（２）を展開すると、
ＨＸＨ＝ＨＸｃｏｓθ＋ＨＹｓｉｎθ・ｓｉｎφ−ＨＺｓｉｎθ・ｃｏｓφ・・・（５）
ＨＹＨ＝ＨＹ・ｃｏｓφ＋ＨＺ・ｓｉｎφ ・・・（６）
ＨＺＨ＝ＨＸｓｉｎθ−ＨＹｃｏｓθ・ｓｉｎφ＋ＨＺｃｏｓθ・ｃｏｓφ・・・（７）
となる。
【００３３】
ｃｏｓφ＝Ｃ１，ｃｏｓθ＝Ｃ２，ｓｉｎφ＝Ｓ１，ｓｉｎθ＝Ｓ２として、式（４）を式（５）に代入すると、

式（８）をＨＸＨについて整理すると、
【００３４】
【数３】

【００３５】
式（９）をＨＸＨについて整理すると、
【００３６】
【数４】

【００３７】
式（１０），式（１１）を結合して、ＨＹＨについて整理すると、
【００３８】
【数５】

【００３９】
式（８）をＨＹＨについて整理すると、
【００４０】
【数６】

【００４１】
式（９）をＨＹＨについて整理すると、
【００４２】
【数７】

【００４３】
式（１３），式（１４）を結合して、ＨＸＨについて整理すると、
【００４４】
【数８】

【００４５】
式（７）と式（３）より再計算すると、
【００４６】
【数９】

【００４７】
式（５）に式（１６）を代入して整理すると、
【００４８】
【数１０】

【００４９】
ここで、Ｃ２^２ ＋Ｓ２^２ ＝ｃｏｓ^２ θ ＋ｓｉｎ^２ ＝１より、
ＨＸＨ＝ＨＸ／Ｃ２ ・・・（１７）
式（６）に式（１６）を代入して整理すると、
【００５０】
【数１１】

【００５１】
次に、逆変換式の立て直しを行なう。
ＨＸＨ＝ＨＸ・Ｃ２−ＨＺ・Ｓ２・・・（１９）
ＨＹＨ＝ＨＸ・Ｓ１・Ｓ２−ＨＹ・Ｃ１＋ＨＺ・Ｃ１・Ｃ２・・・（２０）
ＨＺＨ＝ＨＸ・Ｃ１・Ｓ２−ＨＹ・Ｓ１＋ＨＺ・Ｃ１・Ｃ２・・・（２１）
式（２１）でＨＺＨ＝０として、ＨＺを求めると、
【００５２】
【数１２】

【００５３】
式（１９）に式（２２）を代入して整理すると、
【００５４】
【数１３】

【００５５】
式（２０）に式（２２）を代入して整理すると、
【００５６】
【数１４】

【００５７】
（Ｂ）基本定義
図９に示すような基準となる座標系（ｘｈ，ｙｈ，ｚｈ）を基準座標系とする。この基準座標系を例えば図１０（ａ），図１０（ｂ）に示すようにｘｈ軸を中心にφだけ回転させた座標系を（ｘ′，ｙ′，ｚ′）とし座標系１とする。このとき、ベクトルｐ（→）を基準座標系ではｐ（→）＝（ｘｈ，ｙｈ，ｚｈ）、座標系１ではｐ（→）＝（ｘ′，ｙ′，ｚ′）とすると、この間の関係はマトリックス表現で、
【００５８】
【数１５】

【００５９】
と表される。
次に、基準座標系を例えば図１１（ａ），図１１（ｂ）に示すようにｙ′軸についてθだけ回転させた座標系（ｘ，ｙ，ｚ）を座標系２とする。このとき、ベクトルｐ（→）を座標系２ではｐ（→）＝（ｘ，ｙ，ｚ）と表現すると、座標系１と座標系２との関係のマトリックス表現は、
【００６０】
【数１６】

【００６１】
と表される。
式（２５），式（２６）より、基準座標系と座標系２との関係は、
【００６２】
【数１７】

【００６３】
となる。これは、式（１）とφとθの符号が逆の式である。以後この式（２７）を基準に考える。
次に、ベクトルｐ（→）について、座標系２から座標系１への逆変換を考えると、
【００６４】
【数１８】

【００６５】
となり、座標系１から基準座標系への逆変換は、
【００６６】
【数１９】

【００６７】
となる。従って、座標系２から基準座標系への変換は、式（２８），式（２９）より、
【００６８】
【数２０】

【００６９】
となる。
（Ｃ）傾斜センサの角度表現
次に、以下では、図１２（ｂ）に示すように、上述の傾斜センサ２Ｂが座標系２の原点に取り付けられている場合を考える。なお、図１２（ａ）に示すように基準座標系においてｚ（−）方向が重力方向であると考える。このとき、傾斜センサ２Ｂの出力を、
ｘ軸の回転角度：ｕ
ｙ軸の回転角度：ｖ
と表現すると、これらの角度ｕ，ｖは、下記に示すように、基準座標系の（ｘｈ，ｙｈ）平面が座標系２の（ｘ，ｚ）平面，（ｙ，ｚ）平面を横切るときの交線とｘ，ｙ軸のなす角度と考えられる。
【００７０】
ｕ：（ｘｈ，ｙｈ）平面と（ｙ，ｚ）平面との交線とｙ軸のなす角度
ｖ：（ｘｈ，ｙｈ）平面と（ｘ，ｚ）平面との交線とｘ軸のなす角度
これらのｕ，ｖと前記の座標変換で用いたφ，θとの関係を考える。まず、φ＝０，θ＝０では、座標系２は基準座標系と一致するので、ｕ＝０，ｖ＝０となることは明らかである。
【００７１】
次に、ｘ軸についてφだけ回転させた座標系１の中心に傾斜センサ２Ｂを取り付けた場合を考える。この場合は、直感的に、ｕ＝−φといえる。ここで、（ｘｈ，ｙｈ）平面に垂直な単位ベクトル（長さ１）を考えて、座標変換後にｚ′軸との角度を求めることで、ｕ，ｖを求めることができると思われる。式（２５）より、
【００７２】
【数２１】

【００７３】
∴ ｘ′＝０，ｙ′＝ｓｉｎφ，ｚ′＝ｃｏｓθ ・・・（３１）
このとき、図１３に示すように、ｚ′軸とベクトルのｙ′，ｚ′成分のなす角がｕであるので、

続いて、ｙ′軸についてθだけ回転させた座標系２の中心に傾斜センサ２Ｂが取り付けられている場合を考える。この場合は、直感的にｖ＝０といえる。また、上記と同様に、単位ベクトルを考えて、ｕ，ｖを求めると、式（２７）より、
【００７４】
【数２２】

【００７５】
∴ ｘ＝−ｃｏｓθ・ｓｉｎθ，ｙ＝ｓｉｎθ，ｚ＝ｃｏｓφ・ｃｏｓθ・・・（３３）
図１４に示すようにｚ軸よりｙ，ｚ成分の傾きを求めると、

同じく、図１５に示すように、ｚ軸よりｘ，ｚ成分の傾きを求めると、

となる。
【００７６】
（Ｄ）地磁気ベクトルの座標系２から基準座標系への変換
地磁気ベクトルを（ｘｈ，ｙｈ，０）とすると、式（２５）より、
ｘ＝ｘｈ・ｃｏｓθ＋ｙｈ・ｓｉｎφ・ｓｉｎθ ・・・（３６）
ｙ＝ｙｈ・ｃｏｓφ ・・・（３７）
ｚ＝ｘｈ・ｓｉｎθ−ｙｈ・ｓｉｎφ・ｃｏｓθ ・・・（３８）
式（３４）より、
ｙｈ＝ｙ／ｃｏｓφ ・・・（３９）
また、式（３６），式（３９）より、
ｘ＝ｘｈ・ｃｏｓθ＋ｙ・ｓｉｎφ・ｓｉｎθ／ｃｏｓφ
∴ｘｈ・ｃｏｓθ＝ｘ−ｙ・ｓｉｎφ・ｓｉｎθ／（ｃｏｓφ・ｃｏｓθ）
∴ｘｈ＝（ｘ／ｃｏｓθ）−ｙ・ｔａｎφ・ｔａｎθ ・・・（４０）
となる。
【００７７】
別のアプローチとして、式（３０）より、
ｘｈ＝ｘ・ｃｏｓθ＋ｚ・ｓｉｎθ ・・・（４１）
ｙｈ＝ｘ・ｓｉｎφ・ｓｉｎθ＋ｙ・ｃｏｓφ−ｚ・ｓｉｎφ・ｃｏｓθ・・・（４２）
ｚｈ＝−ｘ・ｃｏｓφ・ｓｉｎθ＋ｙ・ｓｉｎφ＋ｚ・ｃｏｓφ・ｓｉｎθ・・・（４３）
今、ｚｈ＝０であるから、
０＝−ｘ・ｃｏｓφ・ｓｉｎθ＋ｙ・ｓｉｎφ＋ｚ・ｃｏｓφ・ｓｉｎθより、
【００７８】
【数２３】

【００７９】
式（４１），式（４４）より、
【００８０】
【数２４】

【００８１】
これは、式（４０）同一結果となる。また、式（４２），式（４４）より、
【００８２】
【数２５】

【００８３】
これも、式（３９）と同一結果となる。
φ，θをｕ，ｖに置き換えると、傾斜角センサ２Ｂの検出値（ｕ，ｖ）で地磁気ベクトル〔フラックスゲートセンサ２Ａの検出値（ｘ，ｙ）〕を基準座標系（地表水平面）へ座標変換できる。式（３４）より、
ｔａｎ（ｕ）＝−ｔａｎφ／ｃｏｓθ
∴ｔａｎφ＝−ｃｏｓθ・ｔａｎ（ｕ） ・・・（４７）
これを式（４６）に代入し、式（３５）より、
ｘｈ＝ｘ／ｃｏｓ（ｖ）＋ｃｏｓ（ｖ）・ｔａｎ（ｕ）・ｔａｎ（ｖ）・ｙ・・・（４８）
また、式（４７）を簡略化して、
φ＝ｕ・ｃｏｓθ ・・・（４９）
として、これを式（３９）に代入すると、
ｙｈ＝ｙ／ｃｏｓ（ｕ・ｃｏｓｖ） ・・・（５０）
ＣＰＵ３の回転角演算部３１は、以上のような座標変換により補正された地磁気の磁界強度情報を基にヨウ角を算出する。
【００８４】
なお、上記の座標変換は以下のような演算により行なってもよい。すなわち、図１６に示すように、傾斜センサ２Ｂが取り付けられている座標系を（ｘ，ｙ，ｚ）とする。この場合、実際の水平面がｙｚ平面を横切るときの交線とｙ軸との角度ｕが傾斜センサの出力データと考えられる。同様に、水平面がｘｚ平面を横切るときの交線とｘ軸との角度ｖが傾斜センサの出力データと考えられる。なお、この場合、ｘ，ｙ軸からｚ軸（＋）方向へ正とし、ｘ，ｙ軸と交線が一致する所を０とする。
【００８５】
ここで、地表に固定された基準座標系（ｘｈ，ｙｈ，ｚｈ）を考えると、ｘｈ，ｙｈ平面は水平面に一致する。このとき、ｚ軸とｚｈ軸の関係に注目する。図１７（ａ）に示すように、ｚ軸上空から見下げたときにｚ−ｚｈを結んだ線とｘ軸との角度をαとする。ただし、ｘ軸と一致したときに０、ｘ軸（＋）からｙ軸（＋）へ＋とする。また、図１７（ｂ）に示すように、ｚ軸とｚｈ軸のなす角度をβとする。すると、図１８（ａ），図１８（ｂ）に示すように、αが０のときｖに一致しαが９０度のときｕにそれぞれ一致することになる。
【００８６】
ここで、ｕ，ｖとα，βの関係を求める。ｚｈ軸と一致する長さ１の単位ベクトル（０，０，１）を考える。このベクトルのｘ，ｚ平面に写る影とｚ軸のなす角度がｖで、ｙｚ平面に写る影とｚ軸のなす角度がｕである。
（ｘ，ｙ，ｚ）座標で、このベクトルを表現すると、図１９に示すように、ｚ軸成分とベクトル頂点との間の長さはｓｉｎβ、ｚ軸成分の長さはｃｏｓβとなる。よって、
ｘ＝ｓｉｎβ・ｃｏｓα
ｙ＝ｓｉｎβ・ｓｉｎα ・・・（５１）
ｚ＝ｃｏｓβ
となる。これより、図２０，図２１（ａ），図２１（ｂ）から分かるように、
ｔａｎ（−ｖ）＝ｓｉｎβ・ｃｏｓα／ｃｏｓβ ・・・（５２）
ｔａｎ（−ｕ）＝ｓｉｎβ・ｓｉｎα／ｃｏｓβ ・・・（５３）
式（５２），式（５３）を変形して、
ｃｏｓα＝ｔａｎ（−ｖ）／ｔａｎβ ・・・（５４）
ｓｉｎα＝ｔａｎ（−ｕ）／ｔａｎβ ・・・（５５）
式（５５）を式（５４）で割ると、
ｔａｎα＝ｔａｎ（−ｕ）／ｔａｎ（−ｖ）
∴α＝ｔａｎ^−１〔ｔａｎ（−ｕ）／ｔａｎ（−ｖ）〕
式（５４），式（５５）を２乗して加算すると、
ｃｏｓ^２ α＋ｓｉｎ^２ α＝ｔａｎ^２ （−ｖ）／ｔａｎ^２ β＋ｔａｎ^２ （−ｕ）／ｔａｎ^２ β
∴ １＝〔ｔａｎ^２ （−ｖ）＋ｔａｎ^２ （−ｕ）〕／ｔａｎ^２ β
ｔａｎ^２ β＝ｔａｎ^２ （−ｕ）＋ｔａｎ^２ （−ｖ）
ｔａｎβ＝（ｔａｎ^２ （−ｕ）＋ｔａｎ^２ （−ｖ））^１／２
∴ β＝ｔａｎ^−１〔（ｔａｎ^２ （−ｕ）＋ｔａｎ^２ （−ｖ））^１／２ 〕・・・（５６）
となる。
【００８７】
次に、ｚ軸をｚｈ軸に一致させる。ｚ軸をαだけ回転させた座標系（ｘ′，ｙ′，ｚ′）を考える。このとき、ｚｈ軸はｘ′ｚ′平面上にある。（ｘ，ｙ，ｚ）上の任意のベクトルｎ（→）を考える。ｎ（→）は（ｘ，ｙ，ｚ）座標系で（ｘ，ｙ，ｚ）の値をとるとすると、（ｘ′，ｙ′，ｚ′）座標系では
ｘ′＝ｘ・ｃｏｓα＋ｙ・ｓｉｎα
ｙ′＝−ｘ・ｓｉｎα＋ｙ・ｃｏｓα ・・・（５７）
ｚ′＝ｚ
マトリックス表現で、
【００８８】
【数２６】

【００８９】
となる。
次に、図２２，図２３に示すように、ｙ′軸についてβ回転させた座標を（ｘ″，ｙ″，ｚ″）とすると、
ｘ″＝ｘ′・ｃｏｓβ−ｚ′・ｓｉｎβ
ｙ″＝ｙ′ ・・・（５９）
ｚ″＝ｘ′・ｓｉｎβ＋ｚ′・ｃｏｓβ
マトリックス表現では、
【００９０】
【数２７】

【００９１】
となる。さらに、ｚ″軸について−αだけ回転させると基準座標系（ｘｈ，ｙｈ，ｚｈ）となる。
ｘｈ＝ｘ″・ｃｏｓα−ｙ″・ｓｉｎα
ｙｈ＝ｘ″・ｓｉｎα＋ｙ″・ｃｏｓα ・・・（６１）
ｚｈ＝０
マトリックス表現で
【００９２】
【数２８】

【００９３】
となる。
以上より、（ｘ，ｙ，ｚ）座標から（ｘｈ，ｙｈ，ｚｈ）を求めると、
【００９４】
【数２９】

【００９５】
展開すると、
ｘｈ＝ｘ・（ｃｏｓ^２ α・ｃｏｓβ＋ｓｉｎ^２ α）＋ｙ・（ｃｏｓα・ｃｏｓβ・ｓｉｎα−ｓｉｎα・ｃｏｓα）−ｚ・ｃｏｓα・ｓｉｎβ
ｙｈ＝ｘ・（ｓｉｎα・ｃｏｓβ・ｃｏｓα−ｃｏｓα・ｓｉｎα）＋ｙ・（ｓｉｎ^２ α・ｃｏｓβ＋ｃｏｓ^２ α）−ｚ・ｓｉｎα・ｓｉｎβ
ｚｈ＝ｘ・ｓｉｎβ・ｃｏｓα＋ｙ・ｓｉｎα・ｓｉｎβ＋ｚ・ｃｏｓβ・・・（６３）
となり、整理すると、

式（６４），式（６５）より、ｘの補正項Δｘ，ｙの補正項Δｙは、それぞれ、
Δｘ＝ｃｏｓα〔ｘ・ｃｏｓα（ｃｏｓβ−１）＋ｙ・ｓｉｎα（ｃｏｓβ−１）−ｚ・ｓｉｎβ〕 ・・・（６６）
Δｙ＝ｓｉｎα〔ｘ・ｃｏｓα（ｃｏｓβ−１）＋ｙ・ｓｉｎ（ｃｏｓβ−１）−ｚ・ｓｉｎβ〕・・・（６７）
と表現される。ここで、ΔＴを
ΔＴ＝ｘ・ｃｏｓα（ｃｏｓβ−１）＋ｙ・ｓｉｎ（ｃｏｓβ−１）−ｚ・ｓｉｎβ ・・・（６８）
と定義すると、式（６４），式（６５）は、
ｘｈ＝ｘ＋Δｘ＝ｘ＋ｃｏｓα・ΔＴ ・・・（６９）
ｙｈ＝ｙ＋Δｙ＝ｙ＋ｓｉｎα・ΔＴ ・・・（７０）
と表現される。ここで、ｚｈ≠０とすると、式（６３）より、
ｚｈ＝ｘ・ｓｉｎβ・ｃｏｓαｙ・ｓｉｎα・ｓｉｎβ＋ｚ・ｃｏｓβ
∴ −ｚ＝ｘ（ｃｏｓα・ｓｉｎβ／ｃｏｓβ）＋ｙ（ｓｉｎα・ｓｉｎβ／ｃｏｓβ）−ｚｈ／ｃｏｓβ
−ｚ・ｓｉｎβ＝ｘ（ｃｏｓα・ｓｉｎ^２ β／ｃｏｓβ） ＋ｙ（ｓｉｎα・ｓｉｎ^２ β／ｃｏｓβ）−ｚｈ・ｓｉｎβ／ｃｏｓβ
【００９６】
【数３０】

【００９７】
以上より、
【００９８】
【数３１】

【００９９】
となる。
以上のように、本実施形態の３軸モーションセンサ１によれば、ＣＰＵ３の補正部３３において、フラックスゲートセンサ２Ａで得られた回転角情報に対して地磁気を考慮した補正を施すことにより被検出対象物の回転角（ヨウ角）情報を補正することができるので、地磁気の偏差，傾差，水平力に関わらず、つまり、地表のどの地域においても、常に被検出対象物の姿勢（角度情報）を正しく高精度に検出することができ、本センサ１装置の信頼性の向上に大いに寄与する。
【０１００】
具体的には、被検出対象物のピッチ，ロール（傾斜センサ２Ｂの検出値）に基づいて、フラックスゲートセンサ２Ａで検出した磁界強度情報を地表水平面上での磁界強度情報に変換することにより、地磁気による磁界強度情報を基に得られる被検出対象物のヨウ角情報を補正するので、地磁気の偏差，傾差，水平力などによるヨウ角の逆転現象を確実に防止して、ピッチ，ロールとともに常に正確な被検出対象物のヨウ角を得ることができ、さらに本センサ１の信頼性の向上に寄与する。
【０１０１】
また、本実施形態の３軸モーションセンサ１は、フラックスゲートセンサ２Ａを上述のごとく極めて薄く形成するとともに、傾斜センサ２ＢをＩＣ化して小型化しているので、全体の大きさが非常に小型化されている。
なお、本実施形態の３軸モーションセンサ１は、図４に示すように姿勢情報検出部２としてフラックスゲートセンサ２Ａと傾斜センサ２Ｂとをそなえているが、傾斜センサ２Ｂのみをそなえて２軸モーションセンサとして構成してもよい。また、フラックスゲートセンサ２Ａ，傾斜センサ２Ｂには、必ずしも上述のような特別なものを用いる必要はなく、通常のものを用いてもよい。
【０１０２】
【発明の効果】
以上詳述したように、本発明の姿勢検出装置によれば、演算処理部（ＣＰＵ）の補正部において、姿勢情報検出部で得られた姿勢情報に対して地磁気を考慮した補正を施すことにより被検出対象物の姿勢に応じて得られる角度情報を補正することができるので、地磁気の偏差，傾差，水平力に関わらず、つまり、地表のどの地域においても、常に被検出対象物の姿勢（角度情報）を正しく高精度に検出することができ、本装置の信頼性の向上に大いに寄与する。
【０１０３】
また、本発明の姿勢検出装置によれば、被検出対象物の地表水平面に対する傾斜角情報に基づいて、磁気検出部で検出した磁界強度情報を地表水平面上での磁界強度情報に変換することにより、地磁気による磁界強度情報を基に得られる被検出対象物の回転角情報を補正するので、地磁気の偏差，傾差，水平力などによる回転角情報の逆転現象を確実に防止して、傾斜角情報とともに常に正確な被検出対象物の回転角情報を得ることができ、さらに本装置の信頼性の向上に寄与する。
【０１０４】
さらに、本発明の姿勢検出装置によれば、上記の磁気検出部が、励磁コイルとして機能する環状コイルパターンを有するシート状コイル部材と、直線状のコイルパターンを有するシート状コイル部材とをそなえた基板型フラックスゲートセンサとして構成されるので、磁気検出性能を維持しながら極めてその構成が薄くなり、姿勢検出装置全体の小型化に大いに寄与することとなる。
加えて、本発明の姿勢検出装置によれば、上記の傾斜角情報を得る傾斜センサ（傾斜検出部）がＩＣ化されているので、装置全体の大きさを大幅に小型化することができる。
【図面の簡単な説明】
【図１】本発明の一実施形態にかかる姿勢検出装置としての３軸モーションセンサの外観を模式的に示す図である。
【図２】図１におけるＡ矢視図である。
【図３】図１におけるＢ矢視図である。
【図４】本実施形態の３軸モーションセンサの機能ブロック図である。
【図５】本実施形態の３軸モーションセンサに用いられるフラックスゲートセンサの構成を模式的に示す図である。
【図６】本実施形態の３軸モーションセンサに用いられる傾斜センサの要部の構成を示すブロック図である。
【図７】本実施形態の傾斜センサの動作を説明するための図である。
【図８】本実施形態の３軸モーションセンサにおける座標変換処理を説明するための図である。
【図９】本実施形態の３軸モーションセンサにおける座標変換処理を説明するための図である。
【図１０】（ａ），（ｂ）はいずれも本実施形態の３軸モーションセンサにおける座標変換処理を説明するための図である。
【図１１】（ａ），（ｂ）はいずれも本実施形態の３軸モーションセンサにおける座標変換処理を説明するための図である。
【図１２】（ａ），（ｂ）はいずれも本実施形態の３軸モーションセンサにおける座標変換処理を説明するための図である。
【図１３】本実施形態の３軸モーションセンサにおける座標変換処理を説明するための図である。
【図１４】本実施形態の３軸モーションセンサにおける座標変換処理を説明するための図である。
【図１５】本実施形態の３軸モーションセンサにおける座標変換処理を説明するための図である。
【図１６】本実施形態の３軸モーションセンサにおける座標変換処理を説明するための図である。
【図１７】（ａ），（ｂ）はいずれも本実施形態の３軸モーションセンサにおける座標変換処理を説明するための図である。
【図１８】（ａ），（ｂ）はいずれも本実施形態の３軸モーションセンサにおける座標変換処理を説明するための図である。
【図１９】本実施形態の３軸モーションセンサにおける座標変換処理を説明するための図である。
【図２０】本実施形態の３軸モーションセンサにおける座標変換処理を説明するための図である。
【図２１】（ａ），（ｂ）はいずれも本実施形態の３軸モーションセンサにおける座標変換処理を説明するための図である。
【図２２】本実施形態の３軸モーションセンサにおける座標変換処理を説明するための図である。
【図２３】本実施形態の３軸モーションセンサにおける座標変換処理を説明するための図である。
【図２４】本実施形態の３軸モーションセンサの動作を説明するためのフローチャートである。
【図２５】従来の姿勢検出装置としての３軸モーションセンサの一例を示す模式的斜視図である。
【図２６】（ａ），（ｂ）はいずれもピッチ，ロール，ヨウ角を説明するための図である。
【図２７】（ａ）〜（ｃ）はそれぞれピッチ，ロール，ヨウ角を説明するための図である。
【符号の説明】
１ ３軸モーションセンサ（姿勢検出装置）
２ 姿勢情報検出部
２Ａ フラックスゲートセンサ（磁気検出部）
２Ｂ 傾斜センサ（傾斜検出部）
３ ＣＰＵ（演算処理部）
４ コントロール基板
５ 外部接続端子部
６ 支柱
６′，７，８ エポキシ基板
９ アモルファスコア
１０〜１２ 直線状コイルパターン
２１，２２ 固定極板
２３ メイン・ビーム
２４ センター極板
３１ 回転角演算部
３２ 傾斜角演算部
３３ 補正部[0001]
TECHNICAL FIELD OF THE INVENTION
The present invention relates to a posture detection device that detects the posture (inclination angle, rotation angle, etc.) of an object or a person, for example, the position or inclination angle of a display object on a screen (display) according to the operation (posture) of a person. The present invention relates to a posture detection device suitable for use in a head-mounted display experience game for controlling a rotation angle and the like, a posture control of a robot, a fully automatic traveling system, a navigation system, and the like.
[0002]
[Prior art]
FIG. 25 is a schematic perspective view showing an example of a three-axis motion sensor as a conventional posture detecting device. The three-axis motion sensor 100 shown in FIG. 25 includes an inclination sensor 103, a magnetic sensor 104, and a CPU 105. It is configured. Here, the tilt angle sensor 103 is disposed on the substrate 101, and the magnetic sensor 104 and the CPU 105 are disposed on the substrate 102. The overall dimensions are about 50 mm in length, 40 mm in width, and 50 mm in height.
[0003]
Here, the tilt angle sensor 103 incorporates a liquid such as a silicon liquid or a pendulum, and changes the resistance value of the liquid when a detection target such as an attached object or a person is tilted with respect to the horizontal surface. The magnetic sensor 104 detects the geomagnetism (geomagnetic vector).
[0004]
Further, the CPU 105 calculates the inclination angle of the detection target based on the change in the resistance value of the liquid and the change in the position of the pendulum obtained by the inclination angle sensor 103, and performs detection based on the geomagnetism detected by the geomagnetic sensor 104. The rotation angle of the object is calculated.
Thus, the above-described three-axis motion sensor sets the surface horizontal plane to the XY plane (here, the X-axis direction is left and right, the Y-axis direction is front and rear), and the ground surface horizontal plane as shown in FIG. Assuming that the vertical direction (up and down) is the Z axis and the coordinate system, the rotation angle (tilt) angle of the object to be detected around the X axis by the inclination angle sensor 103 [pitch: FIG. )], A rotation (tilt) angle about the Y axis (roll: see FIGS. 26B and 27C) is detected, and the rotation angle about the Z axis [Yaw angle: FIG. (B) and FIG. 27 (a)] can be detected, and three-axis information of the pitch, roll, and yaw angle (posture information of the detection target) can be obtained. However, in this case, it is defined that the yaw angle is detected with magnetic north as absolute 0 degrees.
[0005]
[Problems to be solved by the invention]
However, such a conventional three-axis motion sensor 100 has the following problems.
{Circle around (1)} The geomagnetism detected by the magnetic sensor 104 has a deviation and a tilt for each region (latitude and longitude of the earth) and a different horizontal force. Occurs. Therefore, if the yaw angle detected using the geomagnetism is not corrected in consideration of the geomagnetic deviation, inclination, and horizontal force in each area, the yaw angle reversal phenomenon occurs.
[0006]
{Circle around (2)} Since the liquid sensor and the mechanical (pendulum) sensor are used for the tilt sensor 103, it takes time until the change of the liquid or the pendulum is stabilized, and the data is not stable. Unreliable data may be output for a certain period of time. Accordingly, when the three-axis motion sensor 100 is used for a screen pointing device, a seasickness phenomenon or the like occurs on a display screen, and when the three-axis motion sensor 100 is used for a positioning device, a wrong direction indication or the like occurs.
[0007]
(3) Since the liquid sensor and the mechanical (pendulum) sensor are used for the tilt sensor 103, the size of the entire sensor 100 is increased.
The present invention has been devised in view of such a problem, and provides a posture detecting device that is small and can always accurately detect posture information of a detection target in any region on the earth. With the goal.
[0008]
[Means for Solving the Problems]
For this reason, the posture detecting device of the present invention described in claim 1It has a control board, a first sheet-shaped coil member provided on the control board via a support and having an annular coil pattern functioning as an exciting coil, and a linear coil pattern for detecting a saturation state of a magnetic field. And a second sheet-like coil member disposed on both sides of the first sheet-like coil member so that the linear coil pattern crosses the annular coil pattern of the first sheet-like coil member. A linear coil pattern for detection is arranged on one surface side of the second sheet coil member so that the linear coil pattern intersects the linear coil pattern of the second sheet coil member. A substrate-type flux gate sensor having a third sheet-shaped coil member, and a ground-type flux-gate sensor disposed on the control substrate. A tilt sensor integrated into an IC for detecting a tilt with respect to a horizontal plane, and a tilt sensor provided on the control board, based on posture information of the detection target object detected by the board type flux gate sensor and the tilt sensor. An arithmetic processing unit for calculating angle information according to the attitude of the detection target object, and an arithmetic processing unit provided in the arithmetic processing unit, for the attitude information obtained by the substrate type fluxgate sensor and the tilt sensor. A CPU having a correction unit that can correct the above angle information by performing correction in consideration of geomagnetism;It is characterized by comprising.
[0009]
Further, according to a second aspect of the present invention, in the configuration of the first aspect,The tilt sensor is provided on the control substrate below the substrate-type fluxgate sensor.It is characterized by:
According to a third aspect of the present invention, in the configuration of the second aspect, the CPU is provided on an opposite surface of the control board on which the tilt sensor is provided. I have.
[0010]
Claims4The described posture detection device of the present invention,In order to detect the magnetic field strength due to geomagnetism,A first sheet-shaped coil member having an annular coil pattern functioning as an exciting coil; and a linear coil pattern for detecting a saturated state of a magnetic field, the linear coil pattern being an annular coil pattern of the first sheet-shaped coil member. A second sheet-shaped coil member disposed on one surface side of the first sheet-shaped coil member so as to cross the first sheet-shaped coil member, and a linear coil pattern for detecting a saturation state of the magnetic field. A third sheet-shaped coil member disposed on one surface of the second sheet-shaped coil member so as to intersect with the linear coil pattern of the two sheet-shaped coil members;A magnetic detection unit, an inclination detection unit that detects an inclination of the detection target object with respect to the surface horizontal plane, a posture information detection unit that detects posture information of the detection target object, and a posture detection unit that detects the posture information. Based on the posture information, the angle information corresponding to the posture of the detection target object is calculated by calculation, and the posture information is corrected in consideration of geomagnetism to correct the angle information. And a control board, the control board is provided with the magnetic detection section via a support, and the tilt detection section and the calculation processing section are provided on the control board.It is characterized by being.
According to a fifth aspect of the present invention, in the configuration of the fourth aspect of the present invention, the arithmetic processing unit is arranged on the surface of the ground surface of the detection target object based on a detection result of the magnetic detection unit. A rotation angle calculation unit that calculates the rotation angle information by calculation, a tilt angle calculation unit that calculates the tilt angle information of the detected object with respect to the surface horizontal plane based on the detection result by the tilt detection unit, By converting the magnetic field intensity information detected by the magnetic detection unit based on the tilt angle information obtained by the tilt angle calculation unit into magnetic field intensity information on the surface horizontal plane, the rotation angle calculation unit obtains the information. And a correction unit for correcting the rotation angle information.
[0011]
BEST MODE FOR CARRYING OUT THE INVENTION
Hereinafter, embodiments of the present invention will be described with reference to the drawings.
FIG. 1 is a diagram schematically showing the appearance of a three-axis motion sensor as a posture detecting device according to an embodiment of the present invention, FIG. 2 is a view as seen from an arrow A in FIG. 1, and FIG. 1 to 3, reference numeral 1 denotes a three-axis motion sensor, 2A denotes a substrate-type fluxgate sensor, 2B denotes a tilt sensor, 3 denotes a CPU, and 4 denotes the tilt sensors 2B and CPU3. A control board 5 and an external connection terminal portion 6 are disposed, and a support post 6 is provided for connecting the flux gate sensor 2A and the control board 4 so as to keep them in parallel.
[0012]
Focusing on its functions, the three-axis motion sensor 1 of the present embodiment is configured as shown in FIG. 4. In the present embodiment, the pitch, roll, A posture information detecting unit 2 for detecting posture information such as a yaw angle (see FIGS. 27A to 27C) includes a fluxgate sensor 2A and an inclination sensor 2B.
[0013]
Here, the flux gate sensor (magnetic detection unit) 2A detects the magnetic field intensity due to the geomagnetism, and the inclination sensor (inclination detection unit) 2B detects the inclination of the detected object with respect to the surface horizontal plane. The CPU (arithmetic processing unit) 3 calculates angle information (pitch, roll, yaw angle) corresponding to the posture of the detection target based on the posture information detected by the sensors 2A and 2B. Here, the above-mentioned yaw angle can be corrected by performing correction in consideration of terrestrial magnetism on the information obtained by each of the sensors 2A and 2B.
[0014]
For this reason, as shown in FIG. 4, the CPU 3 includes an inclination angle calculation unit 31, a rotation angle calculation unit 32, and a correction unit 33, and the rotation angle calculation unit 31 is provided with the flux gate sensor 2A. The yaw angle (rotation angle information) of the object to be detected on the surface horizontal plane is calculated based on the detection result, and the inclination angle calculator 32 calculates the yaw angle based on the result of detection by the inclination sensor 2B. The pitch and roll (tilt angle information) with respect to the surface horizontal plane are calculated by calculation.
[0015]
Further, the correction unit 33 converts the magnetic field intensity information detected by the flux gate sensor 2A based on the pitch and roll obtained by the above-described inclination angle calculation unit 32 into a magnetic field intensity on a surface horizontal surface by coordinate conversion processing described later. By converting the information into information, the yaw angle obtained in the rotation angle calculator 31 is corrected.
Next, FIG. 5 is a diagram schematically showing the configuration of the above-mentioned flux gate sensor 2A. As shown in FIG. 5, the flux gate sensor 2A of this embodiment has two epoxy substrates 6 each. , 7, 8 and an amorphous core 9 formed in a ring shape and functioning as an exciting coil by being supplied with a current. Here, it is assumed that the epoxy substrates 6 ', 7, 8 are made of glass.
[0016]
As shown in FIG. 5, a linear coil pattern 10 for detecting the saturation state of the magnetic field is formed on the epoxy substrate (third sheet coil member) 6 'by etching. On the substrate (second sheet-shaped coil member) 7, a similar linear coil pattern 11 is formed by etching in a direction intersecting (perpendicular to) the above-described coil pattern 10.
[0017]
A plurality of linear coil patterns 12 are formed in a ring shape by etching on the epoxy substrate 8, and an etching pattern such as a toroidal coil is formed on the amorphous core (first sheet coil member) 9. Thus, it functions as an exciting coil. Here, each of the above-mentioned etching (coil) patterns is formed such that its etching width is on the order of several microns.
[0018]
That is, the flux gate sensor 2A of the present embodiment has the amorphous core 9 and the linear coil pattern 11, and the linear coil pattern 11 is disposed on both sides of the amorphous core 9 so as to cross the coil pattern of the amorphous core 9. Epoxy substrate 7 having a linear coil pattern 10 and an epoxy substrate disposed on one side of epoxy substrate 7 such that linear coil pattern 10 intersects linear coil pattern 11 of epoxy substrate 7. 6 '.
[0019]
As a result, the fluxgate sensor 2A can obtain the directionality of the geomagnetism by measuring the information on the magnetic field intensity generated when the amorphous core 9 is excited by the linear coil patterns 10 and 11 intersecting each other. As described above, the flux gate sensor 2A according to the present embodiment does not require the conventional winding technology for forming a coil by forming a coil pattern required as a magnetic sensor by using the etching technology, and therefore the winding is performed. There is no variation in sensitivity due to wire unevenness or winding pressure, and the accuracy can be controlled on the order of microns. As a result, the flux gate sensor 2A is formed extremely thin while maintaining the magnetic detection performance, and the size of the entire three-axis motion sensor 1 is greatly reduced.
[0020]
The above-described amorphous core 9 does not necessarily need to be formed in a ring shape as shown in FIG. 5, but may be formed in another shape such as a flat plate shape. The epoxy substrates 6 ', 7, and 8 may be made of plastic instead of glass as long as they can be etched.
Next, FIG. 6 is a block diagram showing a configuration of a main part of the above-described tilt sensor 2B. As shown in FIG. 6, the tilt sensor 2B according to the present embodiment includes independent fixed pole plates 21 and 22. A center electrode 24 mounted on a main beam 23 that operates in response to the applied acceleration, and the fixed electrodes 21, 22 and the center electrode 24 form two capacitances (CS1, CS2). It is configured to:
[0021]
As a result, in the tilt sensor 2B, as shown in FIG. 7, for example, the main beam 23 operates when acceleration is applied to the tilt sensor 2B due to the detection target tilting with respect to the surface horizontal plane. Then, a mismatch occurs between the two capacitance values CS1 and CS2, and as a result, a current corresponding to the inclination with respect to the surface horizontal plane flows to the center electrode plate 24. If this current value is supplied to the CPU 3 as inclination detection information, Necessary tilt angle information (pitch, roll) is detected in the above-described tilt angle calculating section 32.
[0022]
In other words, the present tilt sensor 2B realizes an IC by making it possible to detect tilt information by utilizing the change in capacitance as described above, thereby further increasing the scale of the entire three-axis motion sensor 1. It is downsized. The dimensions of the three-axis motion sensor 1 are about 45 mm in length, 30 mm in width, and about 8.5 mm in height, as shown in FIG.
[0023]
Hereinafter, the operation of the three-axis motion sensor 1 according to the present embodiment configured as described above will be described with reference to the flowchart (steps S1 to S5) shown in FIG.
First, the flux gate sensor 2A detects the magnetic field intensity due to the terrestrial magnetism (Step S1). Then, the detected magnetic field strength information is provided to the CPU 3, and the CPU 3 calculates the yaw angle by the rotation angle calculation unit 31 based on the magnetic field strength information. There is a difference, a deviation, a horizontal force, and the like, and geomagnetism cannot be detected depending on the inclination angle of the fluxgate sensor 2A.
[0024]
This correction process is performed based on the tilt detection information obtained by the tilt sensor 2B. In the tilt sensor 2B, when the detection target to which the three-axis motion sensor 1 is attached is tilted with respect to the horizontal surface, tilt detection information such as a current value corresponding to the tilt is obtained (step S2). The obtained information is supplied to the CPU 3, and the CPU 3 calculates a pitch and a roll by a predetermined calculation based on the tilt detection information and outputs the pitch and roll (step S3).
[0025]
Then, the correction unit 33 converts the magnetic field strength information detected by the fluxgate sensor 2A into the magnetic field strength information on the surface horizontal plane by performing the following coordinate conversion processing based on these pitches and rolls (coordinates). (Conversion) to make the state free of the influence of these inclination, deviation and horizontal force (step S4). As a result, the rotation angle calculator 31 performs a calculation process based on the corrected magnetic field strength information, and always calculates a correct and accurate yaw angle (step S5).
[0026]
Here, the above-described coordinate conversion processing will be described in detail.
(A) General operation method of coordinate transformation
For example, as shown in FIG. 8, a fixed coordinate system set in the space is (xh, yh, zh), and coordinates when this coordinate system is rotated by φ around the xh axis and by θ around the yh axis. Assume the system is (x, y, z). However, the sign of each rotation angle is set as shown in FIG.
[0027]
First, consider a magnetic flux vector H (→) = (HXH, HYH, HZH) in the (xh, yh, zh) coordinate system. In the (x, y, z) coordinate system, this vector H (→) is expressed as H (→) = (HX, HY, HZ). Therefore, the transformation from the (xh, yh, zh) coordinate system to the (x, y, z) coordinate system is
[0028]
(Equation 1)

[0029]
Is represented by
Conversely, the conversion from the (x, y, z) coordinate system to the (xh, yh, zh) coordinate system is as follows:
[0030]
(Equation 2)

[0031]
Is represented by
As an actual state, when the (xh, yh, zh) coordinate system is considered as the coordinate system of the ground surface (gravity is in the “−z” direction), the (x, y, z) coordinate system is considered to be, for example, a ship. At this time, if the vector H (→) is considered to be geomagnetism,
HZH ≒ 0 (3)
Can be approximated. Therefore, when the magnitude of the vector H (→), that is, the horizontal amplitude, φ, θ of the flux gate sensor 2A is known, HZ can be calculated from the equation (1).
[0032]
HZ = −HXH sin θ + HYH · cos θ · sin φ (4)
Expanding equation (2) of the matrix,
HXH = HXcosθ + HYsinθ · sinφ−HZsinθ · cosφ (5)
HYH = HY · cosφ + HZ · sinφ (6)
HZH = HX sin θ−HY cos θ · sin φ + HZ cos θ · cos φ (7)
It becomes.
[0033]
Assuming that cos φ = C1, cos θ = C2, sin φ = S1, and sin θ = S2, substituting equation (4) into equation (5),

When rearranging equation (8) for HXH,
[0034]
(Equation 3)

[0035]
Rearranging equation (9) for HXH,
[0036]
(Equation 4)

[0037]
By combining Equations (10) and (11) and rearranging for HYH,
[0038]
(Equation 5)

[0039]
When rearranging equation (8) for HYH,
[0040]
(Equation 6)

[0041]
When rearranging equation (9) for HYH,
[0042]
(Equation 7)

[0043]
Combining equations (13) and (14) and rearranging for HXH,
[0044]
(Equation 8)

[0045]
Recalculating from equations (7) and (3) gives:
[0046]
(Equation 9)

[0047]
Substituting equation (16) into equation (5) and rearranging,
[0048]
(Equation 10)

[0049]
Here, C2²+ S2²= Cos²θ + sin²= 1
HXH = HX / C2 (17)
Substituting equation (16) into equation (6) and rearranging,
[0050]
(Equation 11)

[0051]
Next, the reverse conversion formula is reestablished.
HXH = HX · C2-HZ · S2 (19)
HYH = HX.S1.S2-HY.C1 + HZ.C1.C2 (20)
HZH = HX · C1 · S2-HY · S1 + HZ · C1 · C2 (21)
When HZH = 0 in the equation (21), HZ is obtained.
[0052]
(Equation 12)

[0053]
Substituting equation (22) into equation (19) and rearranging,
[0054]
(Equation 13)

[0055]
Substituting equation (22) into equation (20) and rearranging,
[0056]
[Equation 14]

[0057]
(B) Basic definition
A reference coordinate system (xh, yh, zh) as shown in FIG. 9 is defined as a reference coordinate system. The coordinate system obtained by rotating this reference coordinate system by φ around the xh axis as shown in FIGS. 10A and 10B, for example, is defined as (x ′, y ′, z ′) and the coordinate system 1. . At this time, if the vector p (→) is p (→) = (xh, yh, zh) in the reference coordinate system and p (→) = (x ′, y ′, z ′) in the coordinate system 1, then The relationship is a matrix expression,
[0058]
(Equation 15)

[0059]
It is expressed as
Next, a coordinate system (x, y, z) obtained by rotating the reference coordinate system by θ with respect to the y ′ axis as shown in FIGS. 11A and 11B is referred to as a coordinate system 2. At this time, if the vector p (→) is expressed as p (→) = (x, y, z) in the coordinate system 2, the matrix expression of the relationship between the coordinate system 1 and the coordinate system 2 is as follows.
[0060]
(Equation 16)

[0061]
It is expressed as
From the equations (25) and (26), the relationship between the reference coordinate system and the coordinate system 2 is
[0062]
[Equation 17]

[0063]
It becomes. This is an equation in which the signs of φ and θ are opposite to those in equation (1). Hereinafter, consideration will be given based on this equation (27).
Next, considering the inverse transformation from the coordinate system 2 to the coordinate system 1 for the vector p (→),
[0064]
(Equation 18)

[0065]
And the inverse transformation from coordinate system 1 to the reference coordinate system is
[0066]
[Equation 19]

[0067]
It becomes. Therefore, the conversion from the coordinate system 2 to the reference coordinate system is obtained from the equations (28) and (29).
[0068]
(Equation 20)

[0069]
It becomes.
(C) Angle expression of tilt sensor
Next, a case where the above-described tilt sensor 2B is attached to the origin of the coordinate system 2 as shown in FIG. It is assumed that the z (-) direction is the direction of gravity in the reference coordinate system as shown in FIG. At this time, the output of the tilt sensor 2B is
x-axis rotation angle: u
Rotation angle of y axis: v
When expressed as follows, these angles u and v are defined as follows when the (xh, yh) plane of the reference coordinate system crosses the (x, z) plane and the (y, z) plane of the coordinate system 2 as shown below. It can be considered as the angle between the intersection line and the x and y axes.
[0070]
u: the angle between the intersection of the (xh, yh) plane and the (y, z) plane with the y-axis
v: the angle between the intersection of the (xh, yh) plane and the (x, z) plane and the x-axis
Consider the relationship between u and v and φ and θ used in the coordinate conversion. First, when φ = 0 and θ = 0, since the coordinate system 2 matches the reference coordinate system, it is clear that u = 0 and v = 0.
[0071]
Next, consider a case where the inclination sensor 2B is attached to the center of the coordinate system 1 rotated by φ with respect to the x axis. In this case, it can be intuitively said that u = −φ. Here, considering a unit vector (length 1) perpendicular to the (xh, yh) plane, it is considered that u and v can be obtained by obtaining an angle with the z ′ axis after coordinate conversion. From equation (25),
[0072]
(Equation 21)

[0073]
∴ x ′ = 0, y ′ = sin φ, z ′ = cos θ (31)
At this time, as shown in FIG. 13, since the angle between the z 'axis and the y' and z 'components of the vector is u,

Next, consider a case where the inclination sensor 2B is attached to the center of the coordinate system 2 rotated by θ with respect to the y ′ axis. In this case, v = 0 can be said intuitively. As described above, when u and v are obtained in consideration of the unit vector, from the equation (27),
[0074]
(Equation 22)

[0075]
∴ x = −cos θ · sin θ, y = sin θ, z = cos φ · cos θ (33)
As shown in FIG. 14, when the inclination of the y and z components is obtained from the z axis,

Similarly, as shown in FIG. 15, when the slopes of the x and z components are obtained from the z axis,

It becomes.
[0076]
(D) Conversion of geomagnetic vector from coordinate system 2 to reference coordinate system
Assuming that the geomagnetic vector is (xh, yh, 0), from equation (25),
x = xh · cos θ + yh · sin φ · sin θ (36)
y = yh · cosφ (37)
z = xh · sin θ−yh · sin φ · cos θ (38)
From equation (34),
yh = y / cosφ (39)
From equations (36) and (39),
x = xh · cos θ + y · sin φ · sin θ / cos φ
∴xh · cos θ = x−y · sin φ · sin θ / (cos φ · cos θ)
∴xh = (x / cos θ) −y · tan φ · tan θ (40)
It becomes.
[0077]
As another approach, from equation (30),
xh = x · cos θ + z · sin θ (41)
yh = x · sinφ · sinθ + y · cosφ−z · sinφ · cosθ (42)
zh = −x · cosφ · sinθ + y · sinφ + z · cosφ · sinθ (43)
Now, since zh = 0,
0 = −x · cosφ · sinθ + y · sinφ + z · cosφ · sinθ,
[0078]
(Equation 23)

[0079]
From equations (41) and (44),
[0080]
[Equation 24]

[0081]
This has the same result as equation (40). From equations (42) and (44),
[0082]
(Equation 25)

[0083]
This also has the same result as equation (39).
When φ and θ are replaced by u and v, the geomagnetic vector [the detection value (x, y) of the fluxgate sensor 2A] is coordinated to the reference coordinate system (surface horizontal surface) by the detection value (u, v) of the inclination angle sensor 2B. Can be converted. From equation (34),
tan (u) =-tanφ / cosθ
∴tanφ = -cosθ · tan (u) (47)
This is substituted into Expression (46), and from Expression (35),
xh = x / cos (v) + cos (v) tan (u) tan (v) y (48)
Further, by simplifying the equation (47),
φ = u · cos θ (49)
Substituting this into equation (39) gives
yh = y / cos (u · cosv) (50)
The rotation angle calculation unit 31 of the CPU 3 calculates the yaw angle based on the terrestrial magnetism magnetic field strength information corrected by the coordinate conversion as described above.
[0084]
The above coordinate conversion may be performed by the following calculation. That is, as shown in FIG. 16, the coordinate system to which the inclination sensor 2B is attached is (x, y, z). In this case, the angle u between the intersection line when the actual horizontal plane crosses the yz plane and the y-axis is considered as output data of the tilt sensor. Similarly, the angle v between the intersection line when the horizontal plane crosses the xz plane and the x-axis is considered as output data of the tilt sensor. In this case, the direction from the x and y axes is positive in the z axis (+) direction, and the point where the x and y axes coincide with the intersection line is 0.
[0085]
Here, considering a reference coordinate system (xh, yh, zh) fixed to the ground, the xh, yh plane coincides with the horizontal plane. At this time, attention is paid to the relationship between the z axis and the zh axis. As shown in FIG. 17A, the angle between the line connecting z-zh and the x-axis when looking down from above the z-axis is α. However, when the position coincides with the x-axis, it is set to 0, and from the x-axis (+) to the y-axis (+), + is set. Further, as shown in FIG. 17B, the angle between the z axis and the zh axis is represented by β. Then, as shown in FIGS. 18A and 18B, when α is 0, it matches v, and when α is 90 degrees, it matches u.
[0086]
Here, the relationship between u and v and α and β is obtained. Consider a unit vector (0, 0, 1) of length 1 that coincides with the zh axis. The angle formed by the shadow of the vector on the x and z planes and the z axis is v, and the angle formed by the shadow on the yz plane and the z axis is u.
When this vector is expressed by (x, y, z) coordinates, as shown in FIG. 19, the length between the z-axis component and the vector vertex is sin β, and the length of the z-axis component is cos β. Therefore,
x = sinβ · cosα
y = sinβ · sinα (51)
z = cosβ
It becomes. Thus, as can be seen from FIGS. 20, 21 (a) and 21 (b),
tan (−v) = sinβ · cosα / cosβ (52)
tan (−u) = sinβ · sinα / cosβ (53)
By transforming equations (52) and (53),
cosα = tan (−v) / tanβ (54)
sinα = tan (−u) / tanβ (55)
By dividing equation (55) by equation (54),
tan α = tan (−u) / tan (−v)
∴α = tan^-1[Tan (-u) / tan (-v)]
When Equations (54) and (55) are squared and added,
cos²α + sin²α = tan²(-V) / tan²β + tan²(-U) / tan²β
１ 1 = [tan²(-V) + tan²(-U)] / tan²β
tan²β = tan²(-U) + tan²(-V)
tanβ = (tan²(-U) + tan²(-V))^1/2
∴ β = tan^-1[(Tan²(-U) + tan²(-V))^1/2] (56)
It becomes.
[0087]
Next, the z axis is made to coincide with the zh axis. Consider a coordinate system (x ′, y ′, z ′) in which the z-axis is rotated by α. At this time, the zh axis is on the x'z 'plane. Consider an arbitrary vector n (→) on (x, y, z). If n (→) takes the value of (x, y, z) in the (x, y, z) coordinate system, then in the (x ′, y ′, z ′) coordinate system
x ′ = x · cos α + y · sin α
y ′ = − x · sin α + y · cos α (57)
z '= z
In matrix expression,
[0088]
(Equation 26)

[0089]
It becomes.
Next, as shown in FIGS. 22 and 23, if the coordinates rotated β with respect to the y ′ axis are (x ″, y ″, z ″),
x ″ = x ′ · cos β−z ′ · sin β
y ″ = y ′ (59)
z ″ = x ′ · sin β + z ′ · cos β
In matrix representation,
[0090]
[Equation 27]

[0091]
It becomes. Further, when the z ″ axis is rotated by −α, a reference coordinate system (xh, yh, zh) is obtained.
xh = x ″ · cosα−y ″ · sinα
yh = x ″ · sin α + y ″ · cos α (61)
zh = 0
Matrix expression
[0092]
[Equation 28]

[0093]
It becomes.
From the above, when (xh, yh, zh) is obtained from the (x, y, z) coordinates,
[0094]
(Equation 29)

[0095]
When expanded,
xh = x · (cos²α ・ cosβ + sin²α) + y · (cosα · cosβ · sinα-sinα · cosα) -z · cosα · sinβ
yh = x · (sin α · cos β · cos α−cos α · sin α) + y · (sin²α · cosβ + cos²α) -z ・ sinα ・ sinβ
zh = x · sinβ · cosα + y · sinα · sinβ + z · cosβ (63)
And organize it,

From Equations (64) and (65), the correction term Δx of x and the correction term Δy of y are
Δx = cosα [x · cosα (cosβ-1) + y · sinα (cosβ-1) -z · sinβ] (66)
Δy = sin α [x · cos α (cos β-1) + y · sin (cos β-1) −z · sin β] (67)
Is expressed as Where ΔT is
ΔT = x · cosα (cosβ−1) + y · sin (cosβ−1) −z · sinβ (68)
Equations (64) and (65) give
xh = x + Δx = x + cosα · ΔT (69)
yh = y + Δy = y + sinα · ΔT (70)
Is expressed as Here, if zh ≠ 0, then from equation (63),
zh = x · sinβ · cosαy · sinα · sinβ + z · cosβ
∴−z = x (cosα · sinβ / cosβ) + y (sinα · sinβ / cosβ) −zh / cosβ
−z · sin β = x (cos α · sin²β / cosβ) + y (sin α · sin²β / cosβ) -zh · sinβ / cosβ
[0096]
[Equation 30]

[0097]
From the above,
[0098]
[Equation 31]

[0099]
It becomes.
As described above, according to the three-axis motion sensor 1 of the present embodiment, the correction unit 33 of the CPU 3 performs the correction in consideration of the terrestrial magnetism on the rotation angle information obtained by the flux gate sensor 2A to detect the rotation angle information. Since the rotation angle (yaw angle) information of the object can be corrected, regardless of the geomagnetic deviation, inclination, and horizontal force, that is, the attitude (angle information) ) Can be accurately and accurately detected, which greatly contributes to the improvement of the reliability of the present sensor 1 device.
[0100]
Specifically, by converting the magnetic field strength information detected by the fluxgate sensor 2A into the magnetic field strength information on the horizontal surface based on the pitch and roll (detected value of the tilt sensor 2B) of the detection target, Since the yaw angle information of the object to be detected is corrected based on the magnetic field intensity information by the geomagnetism, the yaw angle reversal phenomenon due to the geomagnetic deviation, inclination, horizontal force, etc. is reliably prevented, and the pitch and roll An accurate yaw angle of the object to be detected can be always obtained, which further contributes to the improvement of the reliability of the sensor 1.
[0101]
Further, in the three-axis motion sensor 1 of the present embodiment, the flux gate sensor 2A is formed to be extremely thin as described above, and the tilt sensor 2B is formed into an IC so as to be miniaturized. ing.
Although the three-axis motion sensor 1 according to the present embodiment includes a flux gate sensor 2A and a tilt sensor 2B as the posture information detection unit 2 as shown in FIG. 4, the two-axis motion sensor includes only the tilt sensor 2B. It may be configured as a sensor. Further, the flux gate sensor 2A and the tilt sensor 2B do not necessarily need to use the special ones described above, but may use normal ones.
[0102]
【The invention's effect】
As detailed above,BookAccording to the posture detection device of the invention, the arithmetic processing unit(CPU)In the correction unit, the angle information obtained according to the posture of the detection target object can be corrected by performing the correction considering the geomagnetism on the posture information obtained by the posture information detection unit, Irrespective of the deviation, inclination, and horizontal force, that is, the posture (angle information) of the object to be detected can always be detected accurately and with high accuracy in any area of the ground surface, and the reliability of this device is improved. Greatly contributes.
[0103]
Also,BookAccording to the attitude detection device of the present invention, based on the inclination angle information of the detection target object with respect to the ground surface horizontal plane, the magnetic field strength information detected by the magnetic detection unit is converted to the magnetic field strength information on the ground surface horizontal plane, and thereby the geomagnetic field is detected. Since the rotation angle information of the detected object obtained based on the magnetic field strength information is corrected, the reversal phenomenon of the rotation angle information due to geomagnetic deviation, tilt difference, horizontal force, etc. is reliably prevented, and the rotation angle information is always included. Accurate rotation angle information of the detected object can be obtained, which further contributes to improvement of the reliability of the present apparatus.
[0104]
further,BookAccording to the posture detecting device of the present invention, the magnetic detection unit includes a sheet-shaped coil member having an annular coil pattern functioning as an exciting coil and a sheet-shaped coil member having a linear coil pattern.Substrate type fluxgate sensorThe configuration makes the configuration extremely thin while maintaining the magnetic detection performance, greatly contributing to the downsizing of the entire attitude detection device.
In addition, according to the attitude detection device of the present invention, since the tilt sensor (tilt detection unit) for obtaining the above-described tilt angle information is formed as an IC, the size of the entire device can be significantly reduced.
[Brief description of the drawings]
FIG. 1 is a diagram schematically illustrating an appearance of a three-axis motion sensor as a posture detection device according to an embodiment of the present invention.
FIG. 2 is a view taken in the direction of arrow A in FIG.
FIG. 3 is a view taken in the direction of arrow B in FIG. 1;
FIG. 4 is a functional block diagram of the three-axis motion sensor of the embodiment.
FIG. 5 is a diagram schematically showing a configuration of a flux gate sensor used in the three-axis motion sensor of the embodiment.
FIG. 6 is a block diagram illustrating a configuration of a main part of a tilt sensor used in the three-axis motion sensor according to the embodiment.
FIG. 7 is a diagram for explaining the operation of the tilt sensor according to the embodiment.
FIG. 8 is a diagram for explaining a coordinate conversion process in the three-axis motion sensor of the embodiment.
FIG. 9 is a diagram illustrating a coordinate conversion process in the three-axis motion sensor according to the embodiment.
FIGS. 10A and 10B are diagrams for explaining a coordinate conversion process in the three-axis motion sensor according to the present embodiment.
FIGS. 11A and 11B are diagrams for explaining a coordinate conversion process in the three-axis motion sensor according to the present embodiment.
FIGS. 12A and 12B are diagrams for explaining a coordinate conversion process in the three-axis motion sensor according to the present embodiment.
FIG. 13 is a diagram illustrating a coordinate conversion process in the three-axis motion sensor according to the embodiment.
FIG. 14 is a diagram illustrating a coordinate conversion process in the three-axis motion sensor according to the embodiment.
FIG. 15 is a diagram illustrating a coordinate conversion process in the three-axis motion sensor according to the embodiment.
FIG. 16 is a diagram illustrating a coordinate conversion process in the three-axis motion sensor according to the embodiment.
FIGS. 17A and 17B are diagrams for explaining coordinate conversion processing in the three-axis motion sensor according to the present embodiment.
FIGS. 18A and 18B are diagrams for explaining a coordinate conversion process in the three-axis motion sensor according to the present embodiment.
FIG. 19 is a diagram illustrating a coordinate conversion process in the three-axis motion sensor according to the embodiment.
FIG. 20 is a diagram illustrating a coordinate conversion process in the three-axis motion sensor according to the embodiment.
FIGS. 21A and 21B are diagrams for explaining a coordinate conversion process in the three-axis motion sensor according to the present embodiment.
FIG. 22 is a diagram illustrating a coordinate conversion process in the three-axis motion sensor according to the embodiment.
FIG. 23 is a diagram illustrating a coordinate conversion process in the three-axis motion sensor according to the embodiment.
FIG. 24 is a flowchart for explaining the operation of the three-axis motion sensor of the embodiment.
FIG. 25 is a schematic perspective view showing an example of a three-axis motion sensor as a conventional posture detecting device.
FIGS. 26A and 26B are diagrams for explaining pitch, roll, and yaw angle.
FIGS. 27A to 27C are diagrams for explaining pitch, roll, and yaw angle, respectively.
[Explanation of symbols]
1 3-axis motion sensor (posture detection device)
2 Attitude information detector
2A Flux gate sensor (magnetic detector)
2B Tilt sensor (tilt detector)
3 CPU (arithmetic processing unit)
4 Control board
5 External connection terminal
6 props
6 ', 7,8 epoxy board
9 Amorphous core
10-12 Linear coil pattern
21,22 fixed electrode plate
23 Main beam
24 center electrode plate
31 Rotation angle calculator
32 Tilt angle calculator
33 Correction unit

Claims (5)

A control board,
  A first sheet-like coil member having an annular coil pattern disposed on the control board via a support and functioning as an exciting coil; and a linear coil pattern for detecting a saturation state of a magnetic field. A second sheet-like coil member disposed on both sides of the first sheet-like coil member so that the coil pattern crosses the annular coil pattern of the first sheet-like coil member; A third sheet having a linear coil pattern and disposed on one surface of the second sheet coil member such that the linear coil pattern intersects the linear coil pattern of the second sheet coil member; A substrate-type flux gate sensor configured with a coiled member,
  A tilt sensor that is provided on the control board and is integrated into an IC to detect a tilt of the detected object with respect to a horizontal surface,
  On the control substrate, based on the posture information of the detected object detected by the substrate type fluxgate sensor and the inclination sensor, angle information corresponding to the posture of the detected object is calculated. An arithmetic processing unit for calculating, and a correction provided in the arithmetic processing unit and taking into account terrestrial magnetism with respect to the attitude information obtained by the substrate-type fluxgate sensor and the tilt sensor, thereby obtaining the angle information. A posture detecting device comprising: a CPU having a correction unit that can correct the position. The attitude detecting device according to claim 1, wherein the tilt sensor is provided on the control board below the board type flux gate sensor. The attitude detecting device according to claim 2, wherein the CPU is provided on an opposite surface of the control board on which the tilt sensor is provided. A first sheet-like coil member having an annular coil pattern functioning as an exciting coil, and a linear coil pattern for detecting a saturation state of a magnetic field, wherein the linear coil pattern has A second sheet-shaped coil member disposed on both sides of the first sheet-shaped coil member so as to cross the annular coil pattern of the first sheet-shaped coil member; and a linear coil pattern for detecting a saturation state of a magnetic field. A third sheet-shaped coil member disposed on one surface side of the second sheet-shaped coil member so that the linear coil pattern intersects the linear coil pattern of the second sheet-shaped coil member; It includes a magnetic detector that is configured to include a, a tilt detection unit that detects an inclination with respect to ground the horizontal plane of the detection object, detects the attitude information of the detection object And attitude information detection unit that,
Based on the posture information detected by the posture information detection unit, the angle information corresponding to the posture of the detection target object is calculated by calculation , and the posture information is corrected in consideration of terrestrial magnetism. , Rutotomoni includes an arithmetic processing unit capable of correcting the angle information,
With control board,
An attitude detection device , wherein the magnetic detection unit is disposed on the control substrate via a support, and the tilt detection unit and the arithmetic processing unit are disposed on the control substrate . The arithmetic processing unit is
A rotation angle calculation unit configured to calculate rotation angle information of the detection target object on the surface horizontal plane based on the detection result by the magnetic detection unit,
A tilt angle calculation unit that calculates, based on the detection result of the tilt detection unit, tilt angle information of the detected object with respect to the surface horizontal plane,
By converting the magnetic field strength information detected by the magnetic detection unit based on the tilt angle information obtained by the tilt angle calculation unit into magnetic field strength information on the surface horizontal plane, the rotation angle calculation unit to the rotation angle information obtained is configured to include a correction unit for correcting and said attitude detecting device according to claim 4, wherein.

Images