JP2017009456A - 多軸ジャイロセンサ特性評価装置及び方法 - Google Patents

Abstract

【課題】 各センサ実軸の方向及び個別センサ感度を同定できる多軸ジャイロセンサ特性評価装置及び方法を提供する。【解決手段】 入力角速度ω１を与えるための回転軸Ａ１を有するターンテーブル１に、回転軸Ａ１と平行でない回転軸Ａ２を有するターンテーブル２が接合され、シリアルリンク機構を構成する。ターンテーブル２は３軸ジャイロセンサパッケージ５を第１の姿勢状態で固定して回転する。ターンテーブル２は３軸ジャイロセンサパッケージ５を第２の姿勢状態でも固定して回転する。３軸ジャイロセンサパッケージ５の角速度電圧vout_1、vout_２及びvout_３はターンテーブル２の回転軸Ａ２の回転角θ２を検出するロータリエンコーダ７の角度信号に同期して制御ユニットＣＯＮＴに供給され、角速度電圧vout_1、vout_２及びvout_３から３軸ジャイロセンサパッケージ５の各センサ実軸の方向及び個別センサ感度を演算する。【選択図】 図１

Description

本発明は多軸ジャイロセンサ特性評価装置及び方法、たとえば、センサパッケージにおいて多軸ジャイロセンサの各センサ実軸の方向及び個別センサ感度を同定する特性評価装置及び方法に関する。

通常の多軸ジャイロセンサパッケージでは、製造時及びパッケージ取付け時に生じる誤差によりセンサ実軸はセンサパッケージ方向に完全には一致せず、しかもセンサ実軸同士間でも直交していない。この結果、センサパッケージのｘ軸方向の角速度を検出すべきｘ軸センサがｙ軸方向またはｚ軸方向の角速度を検出してしまうという現象を引き起こす。また、各センサ実軸の個別センサ感度についてもセンサ実軸毎に差があり、１つの多軸ジャイロセンサ内でも各センサ実軸間で個別センサ感度がばらつく。現在広く普及しているマイクロエレクトロメカニカルシステム（MEMS）デバイスの多軸ジャイロセンサにおいても、センサパッケージ方向にセンサ実軸を完全に一致させ、また、各センサ実軸での個別センサ感度のばらつきを完全に取除くことはできない。

また、３軸加速度センサパッケージ及び３軸ジャイロセンサパッケージを組合わせた６軸センサにおいて、６軸センサの値を積分して測定対象の位置及び姿勢を推定するデッドレコニングの精度向上のためにも、センサパッケージにおける３軸ジャイロセンサの各センサ実軸の方向及び個別センサ感度を同定することは重要である。

他方、センサパッケージにおいて多軸加速度センサの各センサ実軸の方向及び個別センサ感度を同定する多軸加速度センサ特性評価装置として平行リンクを利用した加振によるものは公知である（参照：特許文献１、２、非特許文献１、２）。

特許第５６５２７６０号公報（特開２０１２−９３２７１号公報） 特許第５６９７１４９号公報（特開２０１２−２３３８４２号公報）

木村仁、中村成志、伊能教夫、松平昌之、吉田稔、"多軸加速度センサの主軸方向と感度の同定手法（主軸方向テンソルと感度テンソルの実測）"、日本機械学会論文集Ｃ編、Vol. 78 (2012), No. 786, (2012), pp.499-507. Hitoshi Kimura, Masashi Nakamura, Kodai Katou, Norio Inou, Minoru Yoshida, "Identification Method of Sensor Directions and Sensitivities in Multi-Axis Accelerometer -Actual Measurement of Direction Tensor and Sensitivity Tensor-", Proceeding of 38th Annual Conference on IEEE Industrial Electronics Society (IECON) 2012, (2012), MF-026085

上述の従来の多軸ジャイロセンサにおいては、センサパッケージにおいて多軸ジャイロセンサの各センサ実軸の方向及び個別センサ感度を同定することができないので、多軸ジャイロセンサ入力の角速度を正確な３次元ベクトルとして算出できないという課題がある。また、３軸加速度センサパッケージ及び３軸ジャイロセンサパッケージを組合わせた６軸センサにおけるデッドレコニングの精度向上が不可能であるいう課題もある。

上述の課題を解決するために、本発明に係る多軸ジャイロセンサ特性評価装置は、入力角速度が与えられ、第１の回転軸を有する第１のターンテーブルと、第１のターンテーブルとシリアルリンク接合され、第１の回転軸に平行でない第２の回転軸を有し、多軸ジャイロセンサパッケージを固定するための第２のターンテーブルと、第１、第２のターンテーブルを同期回転駆動させるモータと、モータを駆動して多軸ジャイロセンサパッケージの出力電圧を入力する制御ユニットとを具備し、制御ユニットは、多軸ジャイロセンサパッケージが第２のターンテーブルに対して第１の姿勢状態のときに多軸ジャイロセンサパッケージの最小または最大の出力電圧に応じて多軸ジャイロセンサのセンサ軸と第２のターンテーブルの第２の回転軸とを含む第１の平面を同定して第１の平面の第１の法線単位ベクトルを同定し、多軸ジャイロセンサパッケージが第２のターンテーブルに対して第２の姿勢状態のときに多軸ジャイロセンサパッケージの最大または最小の出力電圧に応じて多軸ジャイロセンサのセンサ軸と第２のターンテーブルの第２の回転軸とを含む第２の平面を同定して第２の平面の第２の法線単位ベクトルを同定し、各第１、第２の姿勢状態を表す第１、第２の回転行列及び第１、第２の法線単位ベクトルから多軸ジャイロセンサのセンサ実軸の方向単位ベクトルを同定するものである。

また、本発明に係る多軸ジャイロセンサ特性評価方法は、入力角速度が与えられ、第１の回転軸を有する第１のターンテーブルと、第１のターンテーブルとシリアルリンク接合され、第１の回転軸に平行でない第２の回転軸を有し、多軸ジャイロセンサパッケージを固定するための第２のターンテーブルと、第１、第２のターンテーブルを同期回転駆動させるモータとを具備する多軸ジャイロセンサ特性評価装置において、モータを駆動して第１、第２のターンテーブルを同期回転駆動させつつ多軸ジャイロセンサパッケージが第２のターンテーブルに対して第１の姿勢状態のときに多軸ジャイロセンサパッケージの最小または最大の出力電圧に応じて多軸ジャイロセンサのセンサ軸と第２のターンテーブルの第２の回転軸とを含む第１の平面を同定して第１の平面の第１の法線単位ベクトルを同定する第１の法線単位ベクトル同定段階と、モータを駆動して第１、第２のターンテーブルを同期回転駆動させつつ多軸ジャイロセンサパッケージが第２のターンテーブルに対して第２の姿勢状態のときに多軸ジャイロセンサパッケージの最大または最小の出力電圧に応じて多軸ジャイロセンサのセンサ軸と第２のターンテーブルの第２の回転軸とを含む第２の平面を同定して第２の平面の第２の法線単位ベクトルを同定する第２の法線単位ベクトル同定段階と、各第１、第２の姿勢状態を表す第１、第２の回転行列及び第１、第２の法線単位ベクトルから多軸ジャイロセンサのセンサ実軸の方向単位ベクトルを同定するセンサ実軸方向単位ベクトル同定段階とを具備するものである。

本発明によれば、センサパッケージにおいて多軸ジャイロセンサの各センサ実軸の方向を同定することができ、この結果、多軸ジャイロセンサの角速度を正確な３次元ベクトルとして算出できる。これにより、３軸加速度センサパッケージ及び３軸ジャイロセンサパッケージとを組合わせた６軸センサの値を積分して測定対象の位置及び姿勢を推定するデッドレクニングの精度向上に貢献できる。

本発明に係る多軸ジャイロセンサ特性評価装置の第１の実施の形態を示す斜視図である。 本発明に係る多軸ジャイロセンサ特性評価装置の第１の実施の形態を示す斜視図である。 図１、図２の多軸ジャイロセンサ特性評価装置の座標系を示す図である。 図１、図２のシリアルリンク機構を説明するための図であり、（Ａ）は斜視図、（Ｂ）は座標系を示す図である。 図１、図２の多軸ジャイロセンサ特性評価装置のリンクのＤＨパラメータを示す表である。 本発明に係る多軸ジャイロセンサ特性評価装置の第２の実施の形態を示す斜視図である。 本発明に係る多軸ジャイロセンサ特性評価装置の第２の実施の形態を示す斜視図である。 図６、図７の多軸ジャイロセンサ特性評価装置のリンクのＤＨパラメータを示す表である。 図６、図７の第１の姿勢状態（ｑ＝１）の３軸ジャイロセンサパッケージを示し、（Ａ）は斜視図、（Ｂ）は右側面図である。 図６、図７の第２の姿勢状態（ｑ＝２）の３軸ジャイロセンサパッケージを示す斜視図である。 図１、図２、図６、図７の制御ユニットの動作を説明するためのフローチャートである。 図１１のフローチャートを補足説明する図である。 図１１のフローチャートを補足説明する図である。

図１、図２は本発明に係る多軸ジャイロセンサ特性評価装置の第１の実施の形態を示す斜視図であり、図１は３軸ジャイロセンサパッケージが第１の姿勢状態（ｑ＝１）にある場合、図２は３軸ジャイロセンサパッケージが第２の姿勢状態（ｑ＝２）にある場合を示し、第１、第２の姿勢状態の切替は手動にて行われる。

図１、図２においては、入力角速度ω_１を与えるための回転軸Ａ１を有するターンテーブル１（リンクＬ１とする）に、回転軸Ａ１と平行でない回転軸Ａ２を有するターンテーブル２（リンクＬ２とする）が接合され、ターンテーブル１とターンテーブル２とはシリアルリンク機構を構成している。図１においては、ターンテーブル２は３軸ジャイロセンサパッケージ５を第１の姿勢状態（ｑ＝１）で固定して回転し、他方、図２においては、ターンテーブル２は３軸ジャイロセンサパッケージ５を第２の姿勢状態（ｑ＝２）で固定して回転する。この場合、３軸ジャイロセンサパッケージ５の第１、第２の姿勢状態の固定は三角柱状治具をねじを用いて固定することにより行われ、３軸ジャイロセンサパッケージ５の回転位置はたとえば後述の式（９）の回転行列Ｒ_ｍ１、Ｒ_ｍ２で表される。また、３軸ジャイロセンサパッケージ５は機構的にはターンテーブル２と同一のリンクＬ２である。さらに、ターンテーブル２はターンテーブル１上の取付部材１ａに固定されているが、この取付部材１ａは機構的にはターンテーブル１と同一のリンクＬ１である。さらにまた、回転軸Ａ１と回転軸Ａ２との角度は直角とする。但し、既知であれば直角でなくともよい。

制御ユニットＣＯＮＴはターンテーブル１、２をモータ６によって同期回転駆動する。この場合、モータ６によるターンテーブル１の回転駆動は非接触型マグネットギア（図示せず）を介して行われ、また、ターンテーブル１とターンテーブル２との間も別の非接触型マグネットギア（図示せず）によって接合され、従って、ω_２＝ｒω_１（但し、ｒは定数）なる関係が成立する。つまり、ターンテーブル１は一定角速度ω_１＝ｄθ_１／ｄｔで回転し、ターンテーブル２も一定角速度ω_２＝ｄθ_２／ｄｔ＝ｒω_１で回転する。このように、ターンテーブル１、２は１つのモータ６によって同期回転駆動されるので、製造コストの点で有利である。しかも、非接触型マグネットギアによりモータ６の振動のノイズはターンテーブル１、２に伝達しにくくなり、ターンテーブル１、１の回転は安定する。

３軸ジャイロセンサパッケージ５の第１軸角速度電圧v_{out_1}、第２軸角速度電圧v_{out_２}及び第３軸角速度電圧v_{out_３}はターンテーブル２の回転軸Ａ２の回転角θ_２を検出するロータリエンコーダ７の角度信号に同期して制御ユニットＣＯＮＴに供給され、この結果、制御ユニットＣＯＮＴは第１軸角速度電圧v_{out_1}、第２軸角速度電圧v_{out_２}及び第３軸角速度電圧v_{out_３}から３軸ジャイロセンサパッケージ５の各センサ実軸ｊの方向単位ベクトルｓ_ｊ及び個別センサ感度ｋ_ｊを演算する。

図３は図１、図２の多軸ジャイロセンサ特性評価装置の座標系を説明する図である。

図３に示すように、ターンテーブル１（リンクＬ１）には、原点Ｏ_１を有するｘ_１ｙ_１ｚ_１座標系を定義する。このｘ_１ｙ_１ｚ_１座標系のｚ_１軸（回転軸Ａ１）は原点Ｏ_０を有する静止ｘ_０ｙ_０ｚ_０座標系のｚ_０軸と一致し、ｘ_１ｙ_１ｚ_１座標系のｘ_１軸はｘ_０ｙ_０ｚ_０座標系のｘ_０軸と角度θ_１をなす。また、ターンテーブル２（リンクＬ２）には、原点Ｏ_２を有するｘ_２ｙ_２ｚ_２座標系を定義する。このｘ_２ｙ_２ｚ_２座標系のｘ_２軸はｘ_１ｙ_１ｚ_１座標系のｘ_１軸と角度θ_２をなす。尚、ｘ_２ｙ_２ｚ_２座標系のｚ_２軸は回転軸Ａ２である。

図４は図１、図２のシリアルリンク機構を説明するための図であって、（Ａ）は斜視図、（Ｂ）は座標系を示す図である。

図４に示すように、原点Ｏ_１を有するｘ_１ｙ_１ｚ_１座標系リンクＬ１にｚ_１軸＝ｄ_１の距離で原点Ｏ_２を有するｘ_２ｙ_２ｚ_２座標系リンクＬ２が距離ｄ_２で回転可能に接合されている。すなわち、リンクＬ１とリンクＬ２とは接合されてシリアルリンク機構をなしている。但し、本発明においては、各座標系の原点Ｏ_０、Ｏ_１、Ｏ_２は同一点として扱ってもよい。従って、
ｄ_１＝ｄ_２＝０
とすることができる。この場合、ＤＨ（Ｄｅｎａｕｉｔ、Ｈａｒｔｅｎｂｅｒｇ）パラメータは図５に示すごとくなる。但し、ＤＨパラメータｄ_ｉ、ａ_ｉ、θ_ｉ、α_ｉは次のごとく定義される。
ｄ_ｉ：リンクＬｉ−１のｘ_ｉ−１軸とリンクＬｉのｘ_ｉ軸との距離
ａ_ｉ：リンクＬｉ−１のｚ_ｉ−１軸とリンクＬｉのｚ_ｉ軸との距離
θ_ｉ：リンクＬｉ−１のｘ_ｉ−１軸に対するリンクＬｉのｘ_ｉ軸の角度
α_ｉ：リンクＬｉ−１のｚ_ｉ−１軸に対するリンクＬｉのｚ_ｉ軸の角度

一般に、隣接するリンクＬｉ−１、Ｌｉ同士の同次変換行列Ｔ_{（ｉ−１）ｉ}はＤＨパラメータを利用すると式（１）で表される。

具体的には、図１、図２、図３における原点Ｏ_ｉ（ｉ＝０、１、２）のｘ_ｉｙ_ｉｚ_ｉ座標系（リンクＬｉ）間の同次変換行列Ｔ_０１、Ｔ_１２は式（２）で表される。尚、表示ベクトルは、本明細書中の式及び図面においては太字とする。

但し、行列内のｃ_ｉ、ｓ_ｉはｃｏｓθ_ｉ、ｓｉｎθ_ｉを表すものとする。

一般に、剛体上のｘｙｚ座標系から見た位置ベクトルｘ_０の点を基準座標系から見た場合、その位置ベクトルｘは両者の座標変換を表す同次変換行列をＧとした場合、式（３）となる。

このときの基準座標系から見た剛体上の位置ベクトルｘ_０の速度行列Ｖは式（４）で示される。

つまり、剛体上の位置ベクトルｘ_０の点は基準座標系からその点を示す位置ベクトルｘに速度行列Ｖを乗算したもので表される。これを図１、図２、図３に適用すると、図１、図２、図３における原点Ｏ_０の静止ｘ_０ｙ_０ｚ_０座標系に対するターンテーブル２（リンクＬ２）上の点の速度行列Ｖ_０２は式（５）で表される。

反対に、ターンテーブル２（リンクＬ２）上から見た原点Ｏ_０の静止ｘ_０ｙ_０ｚ_０座標系に対する速度行列Ｖ_２０は式（６）となる。

式（６）における角速度ベクトルωはターンテーブル２（リンクＬ２）から見た原点Ｏ_０の静止ｘ_０ｙ_０ｚ_０座標系の角速度であるので、静止ｘ_０ｙ_０ｚ_０座標系から３軸ジャイロセンサパッケージ５に入力される角速度ベクトルωは符号を入れ替えて式（７）で表される。

式（７）から３軸ジャイロセンサパッケージ５が受ける角速度成分ω_ｘ、ω_ｙはθ_２に応じて正弦波状に変化する。従って、３軸ジャイロセンサパッケージ５の各センサ実軸ｊと入力角速度ベクトルωの方向とが重なったときに出力電圧ｖ_{ｏｕｔ＿１}、ｖ_{ｏｕｔ＿２}は最大または最小となり、この結果、センサ実軸ｊの方向単位ベクトルｓ_ｊを検出できる。他方、ω_ｚ＝ｄθ_２／ｄｔは一定であり、出力電圧ｖ_{ｏｕｔ＿３}は一定のオフセット値となる。さらに、式（７）において、時間と共に変化するパラメータはθ_１、θ_２だけであり、ギア比ｒの非接触型マグネットギアによってω_２＝ｒω_１となる拘束を加えることにより図１、図２、図３の多軸ジャイロセンサ特性評価装置は１自由度で、つまりモータ６だけで動作できる。このとき、モータ６の振動のノイズも非接触型マグネットギアによって大幅に低減できる。

シリアルリンク接合されたターンテーブル１（リンクＬ１）及びターンテーブル２（リンクＬ２）が等速円運動を行うとすれば、つまり、ω_１＝ｄθ_１／ｄｔ及びω_ｚ＝ｄθ_２／ｄｔが一定であるとすれば、式（７）はターンテーブル２（リンクＬ２）に対し、スカラーが一定でｚ_２軸上を回転する角速度ベクトルωが与えられることを示している。３軸ジャイロセンサパッケージ５の各センサ実軸ｊがｚ_２軸と平行に配置されていない場合、各センサ実軸ｊはθ_２の正弦波状電圧ｖ_{ｏｕｔ＿１}、ｖ_{ｏｕｔ＿２}を出力し、これらの電圧は特定の角度（位相）θ_２で最大となる。この位相はセンサ実軸ｊの方向単位ベクトルｓ_ｊ（ｊ＝１、２、３）とｚ_２軸とを含む平面Ψ_ｊｑ（ｊ＝１、２、３；ｑ＝１、２）と等価である。たとえば、１回目測定（ｑ＝１）で方向単位ベクトルｓ_ｊとｚ_２軸とを含む３つの平面Ψ_ｊ１を得、２回目測定（ｑ＝２）で方向単位ベクトルｓ_ｊとｚ_２軸とを含むさらに３つの平面Ψ_ｊ２を得、平面Ψ_ｊ１、Ψ_ｊ２（ｊ＝１、２、３）の各交線がセンサ実軸ｊの方向単位ベクトルｓ_ｊを示すことになる。各センサ実軸ｊを同定できると、各センサ実軸ｊの正弦波出力電圧ｖ_{ｏｕｔ＿１}、ｖ_{ｏｕｔ＿２}から各センサ実軸ｊの個別センサ感度ｋ_ｊを同定できる。その詳細は後述する。

図６、図７は本発明に係る多軸ジャイロセンサ特性評価装置の第２の実施の形態を示す斜視図であり、図６は多軸ジャイロセンサパッケージが第１の姿勢状態（ｑ＝１）にある場合、図７は多軸ジャイロセンサパッケージが第２の姿勢状態（ｑ＝２）にある場合を示し、第１、第２の姿勢状態の切替は制御ユニットＣＯＮＴによって自動的に行われる。

図６、図７においては、図１、図２のターンテーブル２（リンクＬ２）と３軸ジャイロセンサパッケージ５との間に、自動姿勢機能として、パッケージ姿勢変更ユニット３（リンクＬ３とする）及びセンサパッケージ台４（リンクＬ４とする）を付加してある。これに伴い、３軸ジャイロセンサパッケージ５をリンクＬ５とする。パッケージ姿勢変更ユニット３にはセンサパッケージ台４のシャフト４ａが嵌込まれており、このシャフト４ａを回転させることによってセンサパッケージ台４を図６に示す縦状態または図７に示す横状態とする。センサパッケージ台４の縦状態と横状態との角度変化は９０°である。

また、パッケージ姿勢変更ユニット３の姿勢制御はセンサパッケージ台４のシャフト４ａを回転させるサーボモータ（図示せず）に対する姿勢制御信号Ｓによって行われる。この姿勢制御信号Ｓは制御ユニットＣＯＮＴによって無線によって送信される。この無線はたとえば近距離無線通信規格ＺｉｇＢｅｅを用いる。この場合、無線通信規格ＺｉｇＢｅｅの送信ユニット（図示せず）を制御ユニットＣＯＮＴに設け、無線通信規格ＺｉｇＢｅｅの受信ユニット（図示せず）をターンテーブル２上に設ける。

図６、図７においては、３軸ジャイロセンサの各センサ実軸ｊの方向単位ベクトルｓ_ｊを同定すると共に個別センサ感度ｋ_ｊを同定するためには、３軸ジャイロセンサが感知しない３軸ジャイロセンサパッケージ５の並進運動成分及び並進速度成分には意味がない。パッケージ姿勢変更ユニット３（リンクＬ３）を原点Ｏ_３を有するｘ_３ｙ_３ｚ_３座標系とし、センサパッケージ台４（リンクＬ４）を原点Ｏ_４を有するｘ_４ｙ_４ｚ_４座標系とし、３軸ジャイロセンサパッケージ５（リンクＬ５）を原点Ｏ_５を有するｘ_５ｙ_５ｚ_５座標系としたとき、ｘ_２ｙ_２ｚ_２座標系からｘ_３ｙ_３ｚ_３座標系、ｘ_４ｙ_４ｚ_４座標系、ｘ_５ｙ_５ｚ_５座標系までの同次変換行列は、３軸ジャイロセンサパッケージ５が姿勢変更を受ける回転行列をＲ_ｍｑ（ｑ＝１、２）とすれば、式（８）で表される。

回転行列Ｒ_ｍ１、Ｒ_ｍ２の組合せは無限であるが、その例を式（９）で表す。

測定精度を考慮した場合、上述の１回目の測定（ｑ＝１）、２回目の測定（ｑ＝２）の平面Ψ_ｊ１、Ψ_ｊ２の各法線ベクトルｎ_ｉ１、ｎ_ｉ２は直交するのが好ましい。従って、２回目の測定（ｑ＝２）では、１回目の測定姿勢の回転行列Ｒ_ｍ１から９０°回転させる行列であるＲ_ｒをＲ_ｍ１に乗算して２回目の測定姿勢の回転行列Ｒ_ｍ２を得ている。これらを含めたリンクＬ１からリンクＬ２、Ｌ３、Ｌ４、Ｌ５までのＤＨパラメータを図８に示す。図８のＤＨパラメータのｄ_３、ｄ_４、ｄ_５、ａ_３、ａ_４、ａ_５は定数である任意のオフセット値であり、式（６）及び後述の式（１０）に示されるように、３軸ジャイロセンサパッケージ５への入力角速度ωには寄与しない。

図６に示す３軸ジャイロセンサ３の原点Ｏ_３を有するｘ_３ｙ_３ｚ_３座標系が回転行列Ｒ_ｍ１（ｑ＝１）によって回転された３軸ジャイロセンサパッケージ５の上面図、右側面図を図９に示し、回転行列Ｒ_ｍ２（ｑ＝２）によって回転された３軸ジャイロセンサパッケージ５の上面図を図１０に示す。この場合、図８のリンクＬ４のＤＨパラメータθ_４はその分９０°回転したものとなり、ｑを用いて表現すれば、（ｑ−１）π／２となる。他方、図６に示す姿勢（ｑ＝１）を図７に示す姿勢（ｑ＝２）にするために、制御ユニットＣＯＮＴが姿勢制御信号Ｓを姿勢変更ユニット３のサーボモータ（図示せず）に送出し、この結果、センサパッケージ台４を縦状態から９０°回転し横状態にする。このように、３軸ジャイロセンサパッケージ５の姿勢制御は自動的に行われる。尚、式（９）、図９の（Ｂ）中の角度βは複数考えられる。たとえば、ｃｏｓβ＝√（１／３）の場合には、１回目の測定（ｑ＝１）のとき３軸ジャイロセンサパッケージ５の軸方向の回転面への投影長さが大きくなる。また、ｃｏｓβ＝√（３／５）の場合には、２回目の測定のときの３軸ジャイロセンサパッケージ５の回転面への投影長さが最大となる。さらに、ｃｏｓβ＝√（７／３）の場合には、法線ベクトル同士の外積が等しくなる。

図８におけるリンクＬ３、Ｌ４、Ｌ５における各ＤＨパラメータは時間に依存せず一定であるので、３軸ジャイロセンサパッケージ５のｘ_５ｙ_５ｚ_５座標系（リンクＬ５）から見た静止ｘ_０ｙ_０ｚ_０座標系（リンクＬ０）に対する速度行列Ｖ_５０は式（１０）となる。

つまり、式（６）のＶ_２０と等しくなる。これは３軸ジャイロセンサパッケージ５は実質的に剛体であるターンテーブル２のリンクＬ２上の点と見ることができるからである。つまり、パッケージ姿勢変更ユニット３（リンクＬ３）、センサパッケージ台４（リンクＬ４）及び３軸ジャイロセンサパッケージ５（リンクＬ５）は機構的にはターンテーブル２（リンクＬ２）と同一のリンクである。

次に、図１、図２、図６、図７の制御ユニットＣＯＮＴの動作を図１１を参照して説明する。尚、制御ユニットＣＯＮＴはマイクロコンピュータ等によって構成される。

始めに、ステップ１１０１にて、
ｑ←１
とする。

次に、ステップ１１０２にて、３軸ジャイロセンサパッケージ５をターンテーブル２上にｑ回目の姿勢状態とする。たとえば、図１、図２の３軸ジャイロセンサパッケージ５であれば、３軸ジャイロセンサパッケージ５を式（９）の回転行列をＲ_ｍ１（ｑ＝１）またはＲ_ｍ２（ｑ＝２）に対応する位置に治具、ねじ等を用いて手動で固定する。他方、図６、図７の３軸ジャイロセンサパッケージ５であれば、制御ユニットＣＯＮＴにより無線で姿勢変更信号Ｓを送出することによりターンテーブル２上のサーボモータ（図示せず）を駆動してセンサパッケージ台４を式（９）の回転行列をＲ_ｍ１（ｑ＝１）に対応する縦状態とし、またはＲ_ｍ２（ｑ＝２）に対応する横状態とする。

次に、ステップ１１０３にて、制御ユニットＣＯＮＴはモータ６を駆動してターンテーブル１、２を同期回転駆動させる。

次に、ステップ１１０４にて、ロータリエンコーダ７の角度θ_２と共に、第１軸角速度電圧ｖ_{ｏｕｔ＿１}、第２軸角速度電圧ｖ_{ｏｕｔ＿２}及び第３軸角速度電圧ｖ_{ｏｕｔ＿３}を取込み、角速度電圧ｖ_{ｏｕｔ＿１}またはｖ_{ｏｕｔ＿２}が最大または最小となったときのターンテーブル２の角度θ_２をθ_２１ ^ｑ、θ_２２ ^ｑ、θ_２３ ^ｑとする。図１２の（Ａ）に示すごとく、この角度θ_２１ ^ｑ、θ_２２ ^ｑ、θ_２３ ^ｑ上にセンサ実軸ｊの方向単位ベクトルｓ_１、ｓ_２、ｓ_３が存在すると推定できる。従って、図１２の（Ｂ）に示すごとく、センサ実軸ｊの方向単位ベクトルｓ_ｊ（ｊ＝１、２、３）とｚ_２軸とを含む３つの平面Ψ_ｊｑ（ｊ＝１、２、３）を同定し、各平面Ψ_ｊｑ（ｊ＝１、２、３）の３つの法線単位ベクトルｎ_ｊｑ（ｊ＝１、２、３）を同定する。

尚、平面Ψ_ｊｑ（ｊ＝１、２、３）の法線ベクトルｎ_ｊｑは、式（１１）に示すごとく、センサ実軸ｊの方向単位ベクトルｓ_ｊが回転行列Ｒ_ｍｑによって回転したベクトルとターンテーブル２のｘ_２ｙ_２ｚ_２座標系のｚ_２軸の単位ベクトルｅ_ｚ２との外積で表される。

従って、上述のステップ１１０４にて、式（１１）の左辺が求まる。

次に、ステップ１１０５を参照すると、ｑ＝１か否かを判別し、この結果、ｑ＝１のときには、ステップ１１０６にてｑ＝２とし、ステップ１１０７にて制御ユニットＣＯＮＴはモータ６をオフにしてターンテーブル１、２を停止し、ステップ１１０２〜ステップ１１０５を繰返す。これにより、センサ実軸ｊの方向単位ベクトルｓ_ｊとｚ_２軸とを含む他の３つの平面Ψ_ｊｑ（ｊ＝１、２、３）を同定し、この結果、各平面Ψ_ｊｑ（ｊ＝１、２、３）の他の３つの法線単位ベクトルｎ_ｊｑ（ｊ＝１、２、３）を同定する。他方、ｑ＝２のときには、ステップ１１０８に進む。

ステップ１１０８においては、図１２の（Ｃ）に示す平面Ψ_ｊ１、Ψ_ｊ２（ｊ＝１、２、３）の交点から３軸ジャイロセンサパッケージ５のｘ_５ｙ_５ｚ_５座標系から見たセンサ実軸ｊの方向単位ベクトルｓ_ｊ（ｊ＝１、２、３）を式（１２）に基づいて算出する。

尚、式（１２）の右辺の符号は法線単位ベクトルｎ_ｊ１、ｎ_ｊ２の位置関係からセンサ実軸ｊの方向単位ベクトルｓ_ｊ（ｊ＝１、２、３）の符号が変化する場合を示している。図１３に示すごとく、φ_ｊをセンサ実軸ｊの方向単位ベクトルｓ_ｊとｚ_２軸単位ベクトルｅ_ｚ２との角度とすれば、ｑ＝１、２の何れの場合にも式（１３）が成立する。

最後に、ステップ１１０９にて、各センサ実軸ｊの個別センサ感度ｋ_ｊを演算する。

センサ実軸ｊのセンサが検出する最大角速度ω_ｊ ^ｍａｘ及び最小角速度ω_ｊ ^ｍｉｎは、センサ実軸ｊの方向単位ベクトルｓ_ｊのｘ_１ｚ_１平面への射影がｚ_１軸方向に最大または最小となるときに観測され、その値は、式（１４）で表される。

このときのセンサ実軸ｊの個別センサ感度ｋ_ｊは式（１５）で表される。

但し、ｖ_ｊ ^ｍａｘは最大角速度ω_ｊ ^ｍａｘのときのセンサ実軸ｊの方向の出力電圧、ｖ_ｊ ^ｍｉｎは最小角速度ω_ｊ ^ｍｉｎのときのセンサ実軸ｊの方向の出力電圧である。センサ実軸方向テンソルＳ及び個別センサ感度テンソルＫを式（１６）で定義したとき、

理想的な３軸ジャイロセンサパッケージであれば、センサ実軸方向テンソルＳは単位行列Ｅに等しく、個別センサ感度テンソルＫは共通センサ感度をｋとしたときｋＥとなるが、実際の３軸ジャイロセンサパッケージでは、センサ実軸方向テンソルＳも個別センサ感度テンソルＫも理想の値より多少ずれる。上述のごとく同定された図１、図２、図６、図７の３軸ジャイロセンサパッケージ５に入力された３次元角速度ベクトルω_ｉｎに対して３軸ジャイロセンサパッケージ５が出力電圧ベクトルｖ_ｏｕｔ＝［ｖ_１、ｖ_２、ｖ_３］^Ｔを出力した場合、式（１７）の関係が成立する。

但し、ベクトルｖ_ｏｆｆは３軸ジャイロセンサのオフセット電圧ベクトルであり、上記の３軸ジャイロセンサのセンサ実軸ｊの方向の同定における正弦波出力電圧ｖ_{ｏｕｔ＿１}、ｖ_{ｏｕｔ＿２}の基線から求めることができる。

以上に述べたように、本発明によれば、３軸ジャイロセンサパッケージ５における３軸ジャイロセンサの各センサ実軸ｊの方向単位ベクトルｓ_ｊ及び個別センサ感度ｋ_ｊを同定でき、３軸ジャイロセンサの各センサ実軸ｊが３軸ジャイロセンサパッケージ５に完全に平行になっていなくとも、また、各センサ実軸ｊ間で個別センサ感度ｋ_ｊが多少ずれていても、式（１７）によりこれらの値を補正して正確な入力３次元角速度ベクトルω_ｉｎを得ることができる。

尚、本発明は３軸ジャイロセンサ以外の多軸ジャイロセンサにも適用し得る。

また、本発明は上述の実施の形態の自明な範囲のいかなる変更にも適用できる。

１：ターンテーブル（リンクＬ１）
１ａ：取付部材
２：ターンテーブル（リンクＬ２）
３：パッケージ姿勢変更ユニット（リンクＬ３）
４：センサパッケージ台（リンクＬ４）
５：３軸ジャイロセンサパッケージ（リンクＬ５）
６：モータ
７：ロータリエンコーダ
８：姿勢変更信号

図４に示すように、原点Ｏ_１を有するｘ_１ｙ_１ｚ_１座標系リンクＬ１にｚ_１軸＝ｄ_１の距離で原点Ｏ_２を有するｘ_２ｙ_２ｚ_２座標系リンクＬ２が距離ｄ_２で回転可能に接合されている。すなわち、リンクＬ１とリンクＬ２とは接合されてシリアルリンク機構をなしている。但し、本発明においては、各座標系の原点Ｏ_０、Ｏ_１、Ｏ_２は同一点として扱ってもよい。従って、
ｄ_１＝ｄ_２＝０
とすることができる。この場合、ＤＨ（Ｄｅｎａｖｉｔ−Ｈａｒｔｅｎｂｅｒｇ）パラメータは図５に示すごとくなる。但し、ＤＨパラメータｄ_ｉ、ａ_ｉ、θ_ｉ、α_ｉは次のごとく定義される。
ｄ_ｉ：リンクＬｉ−１のｘ_ｉ−１軸とリンクＬｉのｘ_ｉ軸との距離
ａ_ｉ：リンクＬｉ−１のｚ_ｉ−１軸とリンクＬｉのｚ_ｉ軸との距離
θ_ｉ：リンクＬｉ−１のｘ_ｉ−１軸に対するリンクＬｉのｘ_ｉ軸の角度
α_ｉ：リンクＬｉ−１のｚ_ｉ−１軸に対するリンクＬｉのｚ_ｉ軸の角度

Claims (10)

1. 入力角速度が与えられ、第１の回転軸を有する第１のターンテーブルと、
   前記第１のターンテーブルとシリアルリンク接合され、前記第１の回転軸に平行でない第２の回転軸を有し、多軸ジャイロセンサパッケージを固定するための第２のターンテーブルと、
   前記第１、第２のターンテーブルを同期回転駆動させるモータと、
   前記モータを駆動して前記多軸ジャイロセンサパッケージの出力電圧を入力する制御ユニットと
   を具備し、
   前記制御ユニットは、
   前記多軸ジャイロセンサパッケージが前記第２のターンテーブルに対して第１の姿勢状態のときに前記多軸ジャイロセンサパッケージの最小または最大の出力電圧に応じて前記多軸ジャイロセンサのセンサ軸と前記第２のターンテーブルの第２の回転軸とを含む第１の平面を同定して該第１の平面の第１の法線単位ベクトルを同定し、
   前記多軸ジャイロセンサパッケージが前記第２のターンテーブルに対して第２の姿勢状態のときに前記多軸ジャイロセンサパッケージの最大または最小の出力電圧に応じて前記多軸ジャイロセンサのセンサ軸と前記第２のターンテーブルの第２の回転軸とを含む第２の平面を同定して該第２の平面の第２の法線単位ベクトルを同定し、
   前記各第１、第２の姿勢状態を表す第１、第２の回転行列及び前記第１、第２の法線単位ベクトルから前記多軸ジャイロセンサのセンサ実軸の方向単位ベクトルを同定する多軸ジャイロセンサ特性評価装置。
2. 前記制御ユニットは、さらに、
   前記センサ実軸が最大角速度のときの該センサ実軸の方向の最大出力電圧ｖ_ｊ ^ｍａｘ及び最小角速度のときの該センサ実軸の方向の最小出力電圧ｖ_ｊ ^ｍｉｎに応じて各センサ実軸の個別センサ感度を演算する請求項１に記載の多軸ジャイロセンサ特性評価装置。
3. 前記多軸ジャイロセンサパッケージの前記第２のターンテーブルに対する第１、第２の姿勢状態の固定は手動にて行われる請求項１に記載の多軸ジャイロセンサ特性評価装置。
4. さらに、前記第２のターンテーブルと前記多軸ジャイロセンサパッケージとの間に、
   前記第２のターンテーブル上の固定されたパッケージ姿勢変更ユニットと、
   前記多軸ジャイロセンサパッケージを固定し、回転可能に前記パッケージ姿勢変更ユニットに固定されたセンサパッケージ台と
   を具備し、
   前記制御ユニットは前記パッケージ姿勢変更ユニットを無線で制御して前記センサパッケージ台を回転させて前記多軸ジャイロセンサパッケージを前記第１、第２の姿勢状態とする請求項１に記載の多軸ジャイロセンサ特性評価装置。
5. 前記多軸ジャイロセンサパッケージは、３軸加速度センサパッケージ及び３軸ジャイロセンサパッケージよりなる６軸センサの該３軸ジャイロセンサパッケージである請求項１に記載の多軸ジャイロセンサ特性評価装置。
6. 入力角速度が与えられ、第１の回転軸を有する第１のターンテーブルと、
   前記第１のターンテーブルとシリアルリンク接合され、前記第１の回転軸に平行でない第２の回転軸を有し、多軸ジャイロセンサパッケージを固定するための第２のターンテーブルと、前記第１、第２のターンテーブルを同期回転駆動させるモータとを具備する多軸ジャイロセンサ特性評価装置において、
   前記モータを駆動して前記第１、第２のターンテーブルを同期回転駆動させつつ前記多軸ジャイロセンサパッケージが前記第２のターンテーブルに対して第１の姿勢状態のときに前記多軸ジャイロセンサパッケージの最小または最大の出力電圧に応じて前記多軸ジャイロセンサのセンサ軸と前記第２のターンテーブルの第２の回転軸とを含む第１の平面を同定して該第１の平面の第１の法線単位ベクトルを同定する第１の法線単位ベクトル同定段階と、
   前記モータを駆動して前記第１、第２のターンテーブルを同期回転駆動させつつ前記多軸ジャイロセンサパッケージが前記第２のターンテーブルに対して第２の姿勢状態のときに前記多軸ジャイロセンサパッケージの最大または最小の出力電圧に応じて前記多軸ジャイロセンサのセンサ軸と前記第２のターンテーブルの第２の回転軸とを含む第２の平面を同定して該第２の平面の第２の法線単位ベクトルを同定する第２の法線単位ベクトル同定段階と、
   前記各第１、第２の姿勢状態を表す第１、第２の回転行列及び前記第１、第２の法線単位ベクトルから前記多軸ジャイロセンサのセンサ実軸の方向単位ベクトルを同定するセンサ実軸方向単位ベクトル同定段階と
   を具備する多軸ジャイロセンサ特性評価方法。
7. さらに、
   前記センサ実軸が最大角速度のときの該センサ実軸の方向の最大出力電圧ｖ_ｊ ^ｍａｘ及び最小角速度のときの該センサ実軸の方向の最小出力電圧ｖ_ｊ ^ｍｉｎに応じて各センサ実軸の個別センサ感度を演算する個別センサ感度演算段階を具備する請求項６に記載の多軸ジャイロセンサ特性評価方法。
8. 前記多軸ジャイロセンサパッケージの前記第２のターンテーブルに対する第１、第２の姿勢状態の固定は手動にて行われる請求項６に記載の多軸ジャイロセンサ特性評価方法。
9. 前記多軸ジャイロセンサ特性評価装置は、
   さらに、前記第２のターンテーブルと前記多軸ジャイロセンサパッケージとの間に、
   前記第２のターンテーブル上の固定されたパッケージ姿勢変更ユニットと、
   前記多軸ジャイロセンサパッケージを固定し、回転可能に前記パッケージ姿勢変更ユニットに固定されたセンサパッケージ台と
   を具備し、
   さらに、前記パッケージ姿勢変更ユニットを無線で制御して前記センサパッケージ台を回転させて前記多軸ジャイロセンサパッケージを前記第１、第２の姿勢状態とする多軸ジャイロセンサパッケージ姿勢状態変更段階を具備する請求項６に記載の多軸ジャイロセンサ特性評価方法。
10. 前記多軸ジャイロセンサパッケージは、３軸加速度センサパッケージ及び３軸ジャイロセンサパッケージよりなる６軸センサの該３軸ジャイロセンサパッケージである請求項６に記載の多軸ジャイロセンサ特性評価方法。
    

Images