JP5161498B2 - 姿勢信号演算装置 - Google Patents

Description

本発明は、ロボットなどの運動体の姿勢を表す姿勢信号の演算を行う姿勢信号演算装置に関する。

ロボット、産業機器、及び自動車などの運動体の姿勢制御において、運動体の姿勢は姿勢角、姿勢行列、４元数などで表現される。例えば、姿勢角は、直交する３軸（ｘ軸、ｙ軸、及びｚ軸）の各軸の周りの回転角によって表される。運動体の姿勢角は、姿勢センサ及び画像センサなどのセンサによって検出される。例えば、角速度センサでｘ軸、ｙ軸、及びｚ軸それぞれの軸の周りの角速度を検出し、検出した角速度センサの出力を時間積分することで、各軸の周りの角度すなわち姿勢角が得られる。また例えば、加速度センサの出力信号、地磁気センサの出力信号、及びＧＰＳ（Global Positioning System）信号などを用いて姿勢角を得ることもある。また例えば、画像センサによって撮像した運動体の画像から得られる位置情報に基づいて運動体の姿勢角を求めることもある。

一般に、姿勢を検出するためのセンサからの出力信号には、制御や表現を乱す周波数成分が含まれていることが多い。例えば、角速度センサの出力信号は、低周波成分にドリフト成分を含むことが多いため、角速度センサの出力を積分することで姿勢角を求めると、低周波成分のドリフト分が蓄積されて、姿勢の誤差が制御の安定性を乱す程度に大きくなることが多い。また例えば、加速度センサの出力信号は、並進加速度及び振動加速度などの外乱によって比較的高い周波数成分に誤差を含む。

姿勢センサの検出誤差を低減する従来技術として、例えば特許文献１には、角速度センサの出力から得られる姿勢を、角速度センサと比較して低周波成分に誤差の少ない加速度センサの出力から得られる姿勢を用いて補正する技術が開示されている。特許文献１に記載の技術では、角速度センサの出力に基づく姿勢と、加速度センサの出力に基づく姿勢と、をそれぞれ姿勢行列の形式で表し、それぞれの姿勢行列をローパスフィルタに通して低域成分を抽出し、両者の低域成分の差分を演算することで、角速度センサの出力に基づく姿勢行列のドリフト分を抽出する。その後、抽出されたドリフト分を表す行列を角速度センサの出力に基づく姿勢行列から減算することで、角速度センサの出力に基づく姿勢のドリフト分を除去する。また、特許文献１には、姿勢を姿勢行列で表す代わりに４元数表記で表して前述と同様の演算を行う技術も開示されている。

特開２００７−４１７３３号公報

運動体の姿勢を高精度で検出するために、一般的な姿勢センサと比較して検出誤差の小さい周波数特性を有する姿勢センサを用いて姿勢検出システムを構成することが考えられる。しかしながら、このような高精度の姿勢センサは、通常、一般的な姿勢センサよりも高価で大型であるため、高精度の姿勢センサを用いると、システム全体の高価格化及び大型化を招く。

また、誤差の多い周波数成分を除去するため姿勢センサの出力をデジタルフィルタ処理する場合、姿勢の演算に必要な周波数成分まで除去されたり、デジタルフィルタ処理による位相遅れなどの誤差が生じたりする場合がある。

また、特許文献１に記載の技術では、誤差成分の除去のための演算は、姿勢を表す姿勢行列又は４元数の加減算により行われる。しかしながら、特に姿勢を４元数で表す場合、４元数の加減算によっては運動体の並進が表現されるため、運動体の姿勢の回転に関する誤差成分を適切に除去するのが困難である場合がある。

本発明に係る姿勢信号演算装置は、運動体の姿勢を表す信号の演算を行う姿勢信号演算装置であって、前記運動体の姿勢を表す４元数である入力姿勢４元数に基づいて得られる第１姿勢４元数と、前記運動体の姿勢を表す４元数であって前記第１姿勢４元数と異なる第２姿勢４元数と、を取得し、前記第１姿勢４元数を前記第２姿勢４元数で除算した結果に基づいて得られる４元数を誤差４元数とする誤差演算部と、前記入力姿勢４元数を取得し、前記入力姿勢４元数を前記誤差４元数で除算した結果の４元数を出力姿勢４元数とする出力姿勢４元数演算部と、を備えることを特徴とする。

本発明の１つの態様の姿勢信号演算装置において、前記誤差演算部は、前記第１姿勢４元数を前記第２姿勢４元数で除算した結果の４元数を距離４元数とする距離演算部と、前記距離４元数が表す回転軸ベクトルＶ_p及び回転角η_pを求め、この回転軸ベクトルＶ_pと、この回転角η_pを所定の定数Ｋ_p倍した回転角Ｋ_pη_pと、によって規定される４元数を倍率回転４元数とする倍率回転演算部と、所定の期間内に得られる複数の前記距離４元数を乗算し、この乗算結果の４元数が表す回転軸ベクトルＶ_i及び回転角η_iを求め、この回転軸ベクトルＶ_iと、この回転角η_iを所定の定数Ｋ_i倍した回転角Ｋ_iη_iと、によって規定される４元数を積算回転４元数とする積算回転演算部と、ある時刻に得られた前記距離４元数を当該時刻よりも前の時刻に得られた前記距離４元数で除算し、この除算結果の４元数が表す回転軸ベクトルＶ_d及び回転角η_dを求め、この回転軸ベクトルＶ_dと、この回転角η_dを所定の定数Ｋ_d倍した回転角Ｋ_dη_dと、によって規定される４元数を差分回転４元数とする差分回転演算部と、を備え、前記倍率回転４元数、前記積算回転４元数、及び前記差分回転４元数を乗算した結果の４元数を前記誤差４元数とすることが好ましい。

本発明の他の１つの態様の姿勢信号演算装置において、前記誤差演算部は、前記第１姿勢４元数を前記第２姿勢４元数で除算した結果の４元数を距離４元数とする距離演算部と、前記距離４元数が表す回転軸ベクトルＶ_p及び回転角η_pを求め、この回転軸ベクトルＶ_pと、この回転角η_pを所定の定数Ｋ_p倍した回転角Ｋ_pη_pと、によって規定される４元数を倍率回転４元数とする倍率回転演算部と、所定の期間内に得られる複数の前記距離４元数を乗算し、この乗算結果の４元数が表す回転軸ベクトルＶ_i及び回転角η_iを求め、この回転軸ベクトルＶ_iと、この回転角η_iを所定の定数Ｋ_i倍した回転角Ｋ_iη_iと、によって規定される４元数を積算回転４元数とする積算回転演算部と、ある時刻に得られた前記距離４元数を当該時刻よりも前の時刻に得られた前記距離４元数で除算し、この除算結果の４元数が表す回転軸ベクトルＶ_d及び回転角η_dを求め、この回転軸ベクトルＶ_dと、この回転角η_dを所定の定数Ｋ_d倍した回転角Ｋ_dη_dと、によって規定される４元数を差分回転４元数とする差分回転演算部と、のいずれか１つを備え、前記倍率回転４元数、前記積算回転４元数、及び前記差分回転４元数のいずれか１つを前記誤差４元数としてもよい。

本発明の他の１つの態様の姿勢信号演算装置において、前記誤差演算部は、前記第１姿勢４元数を前記第２姿勢４元数で除算した結果の４元数を距離４元数とする距離演算部と、前記距離４元数が表す回転軸ベクトルＶ_p及び回転角η_pを求め、この回転軸ベクトルＶ_pと、この回転角η_pを所定の定数Ｋ_p倍した回転角Ｋ_pη_pと、によって規定される４元数を倍率回転４元数とする倍率回転演算部と、所定の期間内に得られる複数の前記距離４元数を乗算し、この乗算結果の４元数が表す回転軸ベクトルＶ_i及び回転角η_iを求め、この回転軸ベクトルＶ_iと、この回転角η_iを所定の定数Ｋ_i倍した回転角Ｋ_iη_iと、によって規定される４元数を積算回転４元数とする積算回転演算部と、ある時刻に得られた前記距離４元数を当該時刻よりも前の時刻に得られた前記距離４元数で除算し、この除算結果の４元数が表す回転軸ベクトルＶ_d及び回転角η_dを求め、この回転軸ベクトルＶ_dと、この回転角η_dを所定の定数Ｋ_d倍した回転角Ｋ_dη_dと、によって規定される４元数を差分回転４元数とする差分回転演算部と、のいずれか２つを備え、前記倍率回転４元数と前記積算回転４元数とを乗算した結果の４元数、前記倍率回転４元数と前記差分回転４元数とを乗算した結果の４元数、又は、前記積算回転４元数と前記差分回転４元数とを乗算した結果の４元数、を前記誤差４元数としてもよい。

本発明の他の１つの態様の姿勢信号演算装置において、前記誤差演算部は、前記第１姿勢４元数を前記第２姿勢４元数で除算した結果の４元数を距離４元数とする距離演算部と、前記距離４元数が表す回転軸ベクトルＶ_p及び回転角η_pを求め、前記回転角η_pを所定の定数Ｋ_p倍した回転角Ｋ_pη_pが所定の値の範囲内である場合は、前記回転軸ベクトルＶ_pと前記回転角Ｋ_pη_pとによって規定される４元数を倍率回転４元数とし、前記回転角Ｋ_pη_pが前記所定の値の範囲内でない場合は、前記回転軸ベクトルＶ_pと前記所定の値の範囲内の回転角とによって規定される４元数を倍率回転４元数とする倍率回転演算部と、所定の期間内に得られる複数の前記距離４元数を乗算し、この乗算結果の４元数が表す回転軸ベクトルＶ_i及び回転角η_iを求め、前記回転角η_iを所定の定数Ｋ_i倍した回転角Ｋ_iη_iが前記所定の値の範囲内である場合は、前記回転軸ベクトルＶ_iと前記回転角Ｋ_iη_iとによって規定される４元数を積算回転４元数とし、前記回転角Ｋ_iη_iが前記所定の値の範囲内でない場合は、前記回転軸ベクトルＶ_iと前記所定の値の範囲内の回転角とによって規定される４元数を積算回転４元数とする積算回転演算部と、ある時刻に得られた前記距離４元数を当該時刻よりも前の時刻に得られた前記距離４元数で除算し、この除算結果の４元数が表す回転軸ベクトルＶ_d及び回転角η_dを求め、前記回転角η_dを所定の定数Ｋ_d倍した回転角Ｋ_dη_dが前記所定の値の範囲内である場合は、前記回転軸ベクトルＶ_dと前記回転角Ｋ_dη_dとによって規定される４元数を差分回転４元数とし、前記回転角Ｋ_dη_dが前記所定の値の範囲ではない場合は、前記回転軸ベクトルＶ_dと前記所定の値の範囲内の回転角とによって規定される４元数を差分回転４元数とする差分回転演算部と、を備え、前記倍率回転４元数、前記積算回転４元数、及び前記差分回転４元数を乗算した結果の４元数を前記誤差４元数とすることが好ましい。

本発明の他の１つの態様の姿勢信号演算装置において、前記誤差演算部は、前記第１姿勢４元数を前記第２姿勢４元数で除算した結果の４元数を距離４元数とする距離演算部と、前記距離４元数が表す回転軸ベクトルＶ_p及び回転角η_pを求め、前記回転角η_pを所定の定数Ｋ_p倍した回転角Ｋ_pη_pが所定の値の範囲内である場合は、前記回転軸ベクトルＶ_pと前記回転角Ｋ_pη_pとによって規定される４元数を倍率回転４元数とし、前記回転角Ｋ_pη_pが前記所定の値の範囲内でない場合は、前記回転軸ベクトルＶ_pと前記所定の値の範囲内の回転角とによって規定される４元数を倍率回転４元数とする倍率回転演算部と、所定の期間内に得られる複数の前記距離４元数を乗算し、この乗算結果の４元数が表す回転軸ベクトルＶ_i及び回転角η_iを求め、前記回転角η_iを所定の定数Ｋ_i倍した回転角Ｋ_iη_iが前記所定の値の範囲内である場合は、前記回転軸ベクトルＶ_iと前記回転角Ｋ_iη_iとによって規定される４元数を積算回転４元数とし、前記回転角Ｋ_iη_iが前記所定の値の範囲内でない場合は、前記回転軸ベクトルＶ_iと前記所定の値の範囲内の回転角とによって規定される４元数を積算回転４元数とする積算回転演算部と、ある時刻に得られた前記距離４元数を当該時刻よりも前の時刻に得られた前記距離４元数で除算し、この除算結果の４元数が表す回転軸ベクトルＶ_d及び回転角η_dを求め、前記回転角η_dを所定の定数Ｋ_d倍した回転角Ｋ_dη_dが前記所定の値の範囲内である場合は、前記回転軸ベクトルＶ_dと前記回転角Ｋ_dη_dとによって規定される４元数を差分回転４元数とし、前記回転角Ｋ_dη_dが前記所定の値の範囲内でない場合は、前記回転軸ベクトルＶ_dと前記所定の値の範囲内の回転角とによって規定される４元数を差分回転４元数とする差分回転演算部と、のいずれか１つを備え、前記倍率回転４元数、前記積算回転４元数、及び前記差分回転４元数のいずれか１つを前記誤差４元数としてもよい。

本発明の他の１つの態様の姿勢信号演算装置において、前記誤差演算部は、前記第１姿勢４元数を前記第２姿勢４元数で除算した結果の４元数を距離４元数とする距離演算部と、前記距離４元数が表す回転軸ベクトルＶ_p及び回転角η_pを求め、前記回転角η_pを所定の定数Ｋ_p倍した回転角Ｋ_pη_pが所定の値の範囲内である場合は、前記回転軸ベクトルＶ_pと前記回転角Ｋ_pη_pとによって規定される４元数を倍率回転４元数とし、前記回転角Ｋ_pη_pが前記所定の値の範囲内でない場合は、前記回転軸ベクトルＶ_pと前記所定の値の範囲内の回転角とによって規定される４元数を倍率回転４元数とする倍率回転演算部と、所定の期間内に得られる複数の前記距離４元数を乗算し、この乗算結果の４元数が表す回転軸ベクトルＶ_i及び回転角η_iを求め、前記回転角η_iを所定の定数Ｋ_i倍した回転角Ｋ_iη_iが前記所定の値の範囲内である場合は、前記回転軸ベクトルＶ_iと前記回転角Ｋ_iη_iとによって規定される４元数を積算回転４元数とし、前記回転角Ｋ_iη_iが前記所定の値の範囲内でない場合は、前記回転軸ベクトルＶ_iと前記所定の値の範囲内の回転角とによって規定される４元数を積算回転４元数とする積算回転演算部と、ある時刻に得られた前記距離４元数を当該時刻よりも前の時刻に得られた前記距離４元数で除算し、この除算結果の４元数が表す回転軸ベクトルＶ_d及び回転角η_dを求め、前記回転角η_dを所定の定数Ｋ_d倍した回転角Ｋ_dη_dが前記所定の値の範囲内である場合は、前記回転軸ベクトルＶ_dと前記回転角Ｋ_dη_dとによって規定される４元数を差分回転４元数とし、前記回転角Ｋ_dη_dが前記所定の値の範囲内でない場合は、前記回転軸ベクトルＶ_dと前記所定の値の範囲内の回転角とによって規定される４元数を差分回転４元数とする差分回転演算部と、のいずれか２つを備え、前記倍率回転４元数と前記積算回転４元数とを乗算した結果の４元数、前記倍率回転４元数と前記差分回転４元数とを乗算した結果の４元数、又は、前記積算回転４元数と前記差分回転４元数とを乗算した結果の４元数、を前記誤差４元数としてもよい。

また、本発明に係る姿勢信号演算装置において、前記距離演算部は、前記第１姿勢４元数を前記第２姿勢４元数で除算した結果の４元数を前記距離４元数とする代わりに、当該除算した結果の４元数を正規化し、この正規化した４元数を前記距離４元数とすることが好ましい。

本発明の他の１つの態様の姿勢信号演算装置において、前記運動体の姿勢を検出する姿勢検出センサの出力を用いて前記入力姿勢４元数を求める入力姿勢４元数演算部と、前記入力姿勢４元数から所定の周波数成分を抽出するフィルタと、をさらに備え、前記誤差演算部は、前記入力姿勢４元数を前記第１姿勢４元数として取得し、前記入力姿勢４元数から前記フィルタが抽出した周波数成分の４元数を前記第２姿勢４元数として取得することが好ましい。

本発明の他の１つの態様の姿勢信号演算装置において、前記運動体の姿勢の変化である微小回転角を表す微小回転信号に基づいて求めた４元数を微小回転４元数とする微小回転４元数演算部と、前記微小回転４元数と前記出力姿勢４元数とを用いて求めた４元数を前記入力姿勢４元数とする入力姿勢４元数演算部と、前記入力姿勢４元数から所定の周波数成分を抽出するフィルタと、をさらに備え、前記誤差演算部は、前記入力姿勢４元数を前記第１姿勢４元数として取得し、前記入力姿勢４元数から前記フィルタが抽出した周波数成分の４元数を前記第２姿勢４元数として取得することが好ましい。

本発明の他の１つの態様の姿勢信号演算装置において、前記運動体の姿勢を検出する第１姿勢検出センサの出力を用いて前記入力４元数を求める入力姿勢４元数演算部と、前記運動体の姿勢を検出するセンサであって前記第１姿勢検出センサと異なる特性を有する第２姿勢検出センサの出力を用いて求めた４元数を参照姿勢４元数とする参照姿勢４元数演算部と、をさらに備え、前記誤差演算部は、前記入力姿勢４元数を前記第１姿勢４元数として取得し、前記参照姿勢４元数を前記第２姿勢４元数として取得することが好ましい。

本発明の他の１つの態様の姿勢信号演算装置において、前記運動体の姿勢の変化である微小回転角を表す微小回転信号に基づいて求めた４元数を微小回転４元数とする微小回転４元数演算部と、前記微小回転４元数と前記出力姿勢４元数とを用いて求めた４元数を前記入力姿勢４元数とする入力姿勢４元数演算部と、前記運動体の姿勢を検出する姿勢検出センサの出力を用いて求めた４元数を参照姿勢４元数とする参照姿勢４元数演算部と、をさらに備え、前記誤差演算部は、前記入力姿勢４元数を前記第１姿勢４元数として取得し、前記参照姿勢４元数を前記第２姿勢４元数として取得することが好ましい。

また、前記参照姿勢４元数演算部を備える本発明の態様において、前記入力姿勢４元数から所定の周波数成分を抽出する第１フィルタをさらに備え、前記誤差演算部は、前記入力姿勢４元数から前記第１フィルタが抽出した周波数成分の４元数を前記第１姿勢４元数として取得することが好ましい。

また、前記参照姿勢４元数演算部を備える本発明の態様において、前記参照姿勢４元数から所定の周波数成分を抽出する第２フィルタをさらに備え、前記誤差演算部は、前記参照姿勢４元数から前記第２フィルタが抽出した周波数成分の４元数を前記第２姿勢４元数として取得することが好ましい。

また、前記微小回転４元数演算部を備える本発明の態様において、前記入力姿勢４元数から所定の低域周波数成分を抽出する第１フィルタと、前記参照姿勢４元数から所定の低域周波数成分を抽出する第２フィルタと、をさらに備え、前記微小回転信号は、前記運動体の角速度を検出する角速度センサの出力から得られる信号であり、前記姿勢検出センサは、前記運動体の加速度を検出する加速度センサであり、前記誤差演算部は、前記入力姿勢４元数から前記第１フィルタが抽出した周波数成分の４元数を前記第１姿勢４元数として取得し、前記参照姿勢４元数から前記第２フィルタが抽出した周波数成分の４元数を前記第２姿勢４元数として取得することが好ましい。

本発明によると、運動体の姿勢を表す姿勢信号を高精度に演算する装置を提供できる。

＜第１実施形態＞
図１は、本発明の第１実施形態の姿勢信号演算装置の構成の例を示すブロック図である。図１に示す姿勢信号演算装置は、入力器１０、姿勢４元数変換器２０、４元数フィルタ器３０、４元数誤差演算器４０、４元数フィードバック演算器５０、４元数姿勢変換器６０、及び出力器７０を備える。

入力器１０は、姿勢センサなどから受け取った入力姿勢信号Ｓｉｎを、姿勢信号演算装置における処理に適した形式に変換するインターフェイス部である。入力器１０は、センサなどの特性に応じてアナログデータをデジタルデータへ変換したり、必要に応じてデジタルデータのフォーマットやプロトコルの変換を行ったりする。入力姿勢信号Ｓｉｎは、基準座標系ＸＹＺから見たセンサ座標系ｘｙｚを表す信号である。入力姿勢信号Ｓｉｎの種類及び表現形式などの詳細については後述する。

姿勢４元数変換器２０は、入力器１０が処理した入力姿勢信号Ｓｉｎを受け取り、受け取った入力姿勢信号Ｓｉｎを４元数表記に変換して出力する。図１では、姿勢４元数変換器２０は、入力姿勢信号Ｓｉｎを表す４元数（以下「入力姿勢４元数」と呼ぶ）を、４元数フィルタ器３０、４元数誤差演算器４０、及び４元数フィードバック演算器５０へ出力する。

４元数フィルタ器３０は、予め設定された周波数特性を有するフィルタを備え、４元数表記の時系列信号を処理する。図１では、４元数フィルタ器３０は、姿勢４元数変換器２０が出力した入力姿勢４元数を入力として受け取り、受け取った入力姿勢４元数から所定の周波数成分を抽出した４元数を出力する。本実施形態においては、４元数フィルタ器３０が備えるフィルタの周波数特性は、入力姿勢４元数において誤差成分を多く含む周波数成分を除去するように設定される。

４元数誤差演算器４０は、運動体の姿勢を表す２つの４元数Ｑ１，Ｑ２を入力として受け取り、４元数Ｑ１に含まれる誤差成分を表す４元数を演算して出力する。図１に示す本実施形態では、４元数誤差演算器４０は、姿勢４元数変換器２０が出力した入力姿勢４元数（誤差成分を含む）をＱ１として、この入力姿勢４元数から４元数フィルタ器３０が抽出した周波数成分の４元数（誤差成分を含まない）をＱ２として受け取って、入力姿勢４元数の誤差成分を表す４元数（以下「誤差４元数」と呼ぶ）を演算して出力する。４元数誤差演算器４０で行われる演算の詳細については後述する。

４元数フィードバック演算器５０は、姿勢４元数変換器２０が出力した入力姿勢４元数と、４元数誤差演算器４０が出力した誤差４元数と、を受け取って、入力姿勢４元数を誤差４元数で除算した結果の４元数を出力姿勢４元数として出力する。出力姿勢４元数は、入力姿勢４元数が表す姿勢から誤差成分を取り除いた姿勢を表す４元数である。

４元数姿勢変換器６０は、４元数フィードバック演算器５０から受け取った出力姿勢４元数をユーザの設定に従った形式に変換し、変換後の姿勢信号を出力する。例えば、４元数表記の出力姿勢４元数を、ロール角・ピッチ角・ヨー角による姿勢角（ψ，θ，φ）表記、オイラー角（Ｏｘ，Ｏｙ，Ｏｚ）表記、又は姿勢行列表記などの形式に変換する。姿勢信号の各種表現形式及び４元数姿勢変換器６０で行われる演算の詳細については後述する。

出力器７０は、４元数姿勢変換器６０が出力した姿勢信号を受け取って、外部制御装置などに出力するインターフェイス部である。出力器７０は、必要に応じて、デジタルデータをアナログデータへ変換したり、デジタルデータのフォーマットやプロトコルの変換を行ったりする。

入力姿勢信号Ｓｉｎとして姿勢信号演算装置に入力される姿勢データは、角速度センサからの積分、加速度センサからの傾斜角、磁気センサからの方位角（仰角を含む）、２つ以上のＧＰＳ(Global Positioning System)受信機から得られる姿勢、電波灯台から得られる姿勢、ＩＣタグ信号からの姿勢、カメラなどの画像から得られる姿勢、及びコンピュータ演算で生成された姿勢などのデータである。カメラ画像から姿勢データを求める場合は、ステレオカメラ画像を用いるとよい。

このような姿勢信号は、一般的に、姿勢行列、ロール角・ピッチ角・ヨー角による姿勢角、オイラー角による姿勢角、及び姿勢行列などの表現形式によって表される。

例えば、基準座標系ＸＹＺにおけるセンサ座標系ｘｙｚを表す姿勢行列Ａは、次の式（１）で表すことができる。

姿勢行列Ａにおいて、第１列（ａ，ｂ，ｃ）、第２列（ｄ，ｅ，ｆ）、第３列（ｇ，ｈ，ｉ）は、それぞれ基準座標系からみたセンサ座標系のｘ軸、ｙ軸、ｚ軸の方向ベクトルを表す。第４列は、基準座標系におけるセンサ座標系の原点位置を表す（一般に並進がある場合はこの第４列に並進量が表される）。原点の移動がない場合は、位置の変換を表す第４列の１〜３行目の要素を０とおく。運動体の姿勢のみを取り扱い、移動を取り扱わない場合は、原点移動がないとみなして取り扱う。

また例えば、ロール角・ピッチ角・ヨー角による姿勢角は、図２に示すように、基準座標系のＸ軸，Ｙ軸，Ｚ軸のそれぞれの周りの回転角ψ（ロール角）、θ（ピッチ角）、φ（ヨー角）によって表される。

以上のような様々な形式で表された入力姿勢信号Ｓｉｎは、姿勢４元数変換器２０において４元数表記に変換される。

ここで、４元数について概説する。４元数は、２次元の自由度を有する複素数を４次元の自由度に拡張したものである。４元数Ｑの表記方法には、様々な形態がある。以下、４元数Ｑの表記方法の代表的なものとして、「虚数単位系表記」、「成分表記」、及び「回転表記」について説明する。

・虚数単位系表記
４元数Ｑは、虚数単位ｉ，ｊ，ｋを用いて、

又は、

と表される。虚数単位ｉ，ｊ，ｋは、それぞれ、ｘ軸、ｙ軸、及びｚ軸に対応する。ε₀，ε₁，ε₂，ε₃はスカラー量である。式（２）及び式（３）の虚部をベクトルｖとして、式（４）のように表記してもよい。

ここで、ｖは、

である。

・成分表記
式（２）及び式（３）におけるスカラー量ε₀，ε₁，ε₂，ε₃を４元数Ｑの４つの要素として、式（６）又は式（７）のように表記してもよい。

以下、式（６）又は式（７）のような４元数Ｑの成分表記を「ε表記」とも呼ぶ。

・回転表記
４元数Ｑを用いて、３次元空間上の回転を表すことができる。基準座標系からセンサ座標系への回転変換を表す４元数Ｑは、その回転軸ベクトルを（α，β，γ）、回転角をηとすると、

と表される。式（８）の４元数Ｑは、回転軸ベクトル（α，β，γ）の周りに角度ηだけ回転させる回転変換を表す。

運動体の姿勢を取り扱う場合、並進、拡大縮小は除外し、回転のみを取り扱う。そこで、回転角ηの変域を、０≦η≦πとすると、０≦η／２≦π／２であり、回転軸ベクトル（α，β，γ）の各要素の変域は、−１≦α，β，γ≦１である。また、α²＋β²＋γ²＝１という関係がある。以下、４元数の回転表記を「η表記」とも呼ぶ。

次に、４元数の基本的な演算について説明する。

・加減算
２つの４元数Ｑ１＝（ε_0Q1；ε_1Q1，ε_2Q1，ε_3Q1），Ｑ２＝（ε_0Q2；ε_1Q2，ε_2Q2，ε_3Q2）の加算及び減算は、それぞれ、次の式（９−１）及び式（９−２）で表される。

・乗算
２つの４元数Ｑ１＝（ε_0Q1；ε_1Q1，ε_2Q1，ε_3Q1），Ｑ２＝（ε_0Q2；ε_1Q2，ε_2Q2，ε_3Q2）の乗算は、次の式（１０）で表される。

・除算
４元数の除算は、４元数Ｑの逆４元数Ｑ^-1を右から乗算することとして定義できる。４元数Ｑと逆４元数Ｑ^-1との間には、

の関係がある。また、４元数Ｑ＝（ε₀；ε₁，ε₂，ε₃）について、Ｑ^*＝（ε₀；−ε₁，−ε₂，−ε₃）で定義される共役４元数Ｑ^*を用いると、逆４元数Ｑ^-1は、

と表される。式（１２）で、｜Ｑ｜は、４元数Ｑのノルム（大きさ）を表す。

・４元数の正規化
本実施形態の姿勢信号演算装置においては、回転変換を表す４元数Ｑを用いた演算が行われる。４元数Ｑを用いて、運動体の並進を考慮せずに回転のみを取り扱う場合、４元数Ｑのノルム｜Ｑ｜は、常に、｜Ｑ｜＝１とする。４元数Ｑのノルムは、

で表されるため、４元数Ｑを正規化した４元数Ｑ´は、次の式（１４）によって求められる。

以下、図１の姿勢４元数変換器２０、４元数フィルタ器３０、４元数誤差演算器４０、４元数フィードバック演算器５０、及び４元数姿勢変換器６０のそれぞれにおいて行われる演算の詳細について説明する。

姿勢４元数変換器２０は、一般的な姿勢表現を４元数表記に変換する演算を行う。一例として、姿勢行列表記された姿勢を４元数表記で表す演算について説明する。前述の式（１）に示す姿勢行列Ａを表す４元数Ｑは、姿勢行列Ａの各要素を用いて、次の式（１５）で表すことができる。

入力姿勢信号Ｓｉｎが４元数表現されている場合は、姿勢４元数変換器２０は、特に演算を行わずに入力された４元数を出力する。

４元数フィルタ器３０は、ε表記された４元数Ｑ（ｎ）＝（ε₀（ｎ）；ε₁（ｎ），ε₂（ｎ），ε₃（ｎ））の各成分に対して、予め設定された周波数特性のフィルタを作用させる処理を行う。例えば、４元数フィルタ器３０は、時系列に変化する４元数の各要素ε（ｎ）に対して、低域通過フィルタ（Low Pass Filter, LPF）、高域通過フィルタ（High Pass Filter, HPF）、帯域通過フィルタ（Band Pass Filter, BPF）、又は帯域除去フィルタ（Band Elimination Filter, BEF）などのフィルタを作用させ、フィルタ処理後の４元数Ｑ_F（ｎ）＝（ε_0F（ｎ）；ε_1F（ｎ），ε_2F（ｎ），ε_3F（ｎ））を得る。次に、４元数フィルタ器３０は、Ｑ_F（ｎ）を式（１４）に従って正規化し、正規化された４元数Ｑ_FN（ｎ）を出力する。

４元数誤差演算器４０は、入力された２つの姿勢４元数Ｑ１，Ｑ２を用いて、第１姿勢４元数Ｑ１が表す姿勢と第２姿勢４元数Ｑ２が表す姿勢との間の距離を表す４元数を求め、この距離を表す４元数を誤差４元数として出力する。第１姿勢４元数Ｑ１と第２姿勢４元数Ｑ２との間の距離を表す距離４元数Ｅｑは、次の式（１６）で定義できる。

式（１６）では、第１姿勢４元数Ｑ１に第２姿勢４元数Ｑ２の逆４元数Ｑ２^-1を右から乗算している。したがって、距離４元数Ｅｑは、第１姿勢４元数Ｑ１を第２姿勢４元数Ｑ２で「除算」した結果の４元数であると言える。ここで、第２姿勢４元数Ｑ２は、基準座標系からセンサ座標系への回転変換を表すため、そのノルム｜Ｑ２｜は｜Ｑ２｜＝１である。よって、式（１２）より、第２姿勢４元数Ｑ２の共役４元数Ｑ２^*は、姿勢４元数Ｑ２の逆４元数Ｑ２^-1と等しい。よって、４元数を用いて運動体の回転を取り扱う本実施形態の姿勢信号装置においては、４元数の除算とは、共役４元数Ｑ^*を右から乗算することであるとも言える。第１姿勢４元数Ｑ１、第２姿勢４元数Ｑ２、及び距離４元数Ｅｑを成分表記で表すと、

となる。

４元数の除算は、物理的には、回転軸ベクトル（α，β，γ）の周りに角度−η回転する回転変換を意味する。回転表記の４元数（式（８））の乗算は、回転軸ベクトル（α，β，γ）の周りに角度η回転する回転変換を表すため、４元数の除算は、４元数の乗算の逆回りの回転変換を表すことになる。４元数の除算を表す回転変換をη表記の４元数で表すと、

となる。式（１８）より、回転軸ベクトル（α，β，γ）の周りに角度−η回転する回転変換は、回転軸ベクトル（−α，−β，−γ）の周りに角度η回転する回転変換と同義であることがわかる。

回転角η＝０の場合、その回転変換を表す４元数は、（１；０，０，０）である。したがって、距離４元数Ｅｑ＝Ｑ１・Ｑ２^-1＝（１；０，０，０）となる場合、第１姿勢４元数Ｑ１と第２姿勢４元数Ｑ２とが同じ姿勢を表していると解釈できる。距離４元数Ｅｑ≠（１；０，０，０）の場合、第１姿勢４元数Ｑ１と第２姿勢４元数Ｑ２とは異なる姿勢を表しており、距離４元数Ｅｑは、第１姿勢４元数Ｑ１と第２姿勢４元数Ｑ２との間の回転に関する差、つまりＱ１とＱ２との間の姿勢としての「距離」を表していると解釈できる。

図１を参照し、第１実施形態における４元数誤差演算器４０は、姿勢４元数変換器２０から出力された姿勢４元数を第１姿勢４元数Ｑ１とし、４元数フィルタ器３０から出力された４元数を第２姿勢４元数Ｑ２として、式（１６）に従って求めた距離４元数Ｅｑを誤差４元数Ｕｑとして出力する。

４元数フィードバック演算器５０は、４元数誤差演算器４０から出力された誤差４元数Ｕｑと、姿勢４元数変換器２０から得られた入力姿勢４元数Ｙｑ（ｎ）と、を用いて、次の式（１９）に従って出力姿勢４元数Ｙｕｑ（ｎ）を求める。

式（１９）によると、４元数フィードバック演算器５０は、入力姿勢４元数Ｙｑ（ｎ）を誤差４元数Ｕｑ（ｎ）で除算することで出力姿勢４元数Ｙｕｑ（ｎ）を得る。よって、出力姿勢４元数Ｙｕｑ（ｎ）は、入力姿勢４元数Ｙｑ（ｎ）が表す姿勢を、誤差４元数Ｕｑ（ｎ）が表す回転の分だけ逆回転させた姿勢を表す。

次に、４元数姿勢変換器６０が行う演算について説明する。一例として、ユーザが姿勢行列の形式で出力姿勢信号を出力するように設定している場合の演算について説明する。４元数Ｑ＝（ε₀；ε₁，ε₂，ε₃）を、姿勢行列Ａ（式（１））の各要素は、次の式（２０）で表すことができる。

４元数姿勢変換器６０は、４元数フィードバック演算器５０が出力した出力姿勢４元数を、式（２０）に従って演算して出力する。また、ユーザが出力形式として４元数表記を設定している場合は、４元数姿勢変換器６０は、形式変換の演算を行わずに、４元数フィードバック演算器５０が出力した出力姿勢４元数を出力する。

図１に示す第１実施形態において、４元数フィルタ器３０が備えるフィルタの周波数特性は、入力姿勢４元数において誤差成分を多く含む周波数成分を除去するように設定される。このように４元数フィルタ器３０の周波数特性を設定すると、４元数誤差演算器４０において、姿勢４元数変換器２０が出力した入力姿勢４元数（誤差成分を含む）を第１姿勢４元数Ｑ１とし、４元数フィルタ器３０が出力した４元数を第２姿勢４元数Ｑ２として上述の演算を行うことで、入力姿勢４元数に含まれる誤差成分を表す４元数が誤差４元数Ｕｑとして求められる。

例えば、図１に示す本実施形態において、ビームなどの先端部に取り付けられた画像センサからの出力を入力姿勢信号Ｓｉｎとして処理する場合について考える。例えば、この画像センサを取り付けたビームが１Ｈｚから１０Ｈｚ程度の固有振動数で揺れたり、画像センサからの入力姿勢信号Ｓｉｎにノイズ（例えば、５０Ｈｚ又は６０Ｈｚの電源周波ノイズ）が混入していたりする場合を想定する。この場合、画像センサから得られる入力姿勢信号Ｓｉｎ及び姿勢４元数変換器２０が出力する入力姿勢４元数は、ある特定の周波数成分の揺らぎ及びノイズなどの誤差成分を含む。この場合、誤差成分を多く含む特定の周波数成分を通過させない帯域除去フィルタ、又は誤差成分の比較的少ない周波数成分を通過させる帯域通過フィルタを用いて４元数フィルタ器３０を構成すると、４元数フィルタ器３０は、入力姿勢４元数から誤差成分を除去した姿勢４元数を出力する。４元数誤差演算器４０において、姿勢４元数変換器２０から出力された入力姿勢４元数（誤差成分を含む）と、４元数フィルタ器３０から出力された誤差成分除去後の姿勢４元数と、の間の距離を求めると、この距離は、入力姿勢４元数に含まれる誤差成分を表すものとなる。この距離を誤差４元数としてフィードバック演算器５０に入力すると、姿勢４元数変換器２０が出力した入力姿勢４元数から、揺らぎ及びノイズなどの誤差成分による姿勢成分を取り除いた出力姿勢４元数を得ることができる。

本実施形態の姿勢信号演算装置によって得られる姿勢信号は、帯域通過フィルタなどのフィルタ処理で揺らぎ及びノイズなどの誤差成分を除去して得られる姿勢データと比較して、フィルタ処理されない信号を基準として出力信号を得るため応答性が良く、補正効果を調整できるため滑らかな姿勢変化データが得られる。

入力姿勢信号Ｓｉｎの他の例として、角速度センサからの姿勢や、電波灯台からの姿勢などのように、ＤＣ付近にドリフト成分がある信号を処理する場合は、高域通過フィルタを用いて４元数フィルタ器３０を構成する。そうすると、４元数フィルタ器３０は、入力姿勢４元数の低周波成分に含まれるドリフト成分を除去した姿勢４元数を出力するため、４元数誤差演算器４０は、入力姿勢４元数と、４元数フィルタ器３０によってドリフト成分が除去された姿勢４元数と、に基づいて、入力姿勢４元数に含まれるドリフト成分を抽出できる。また、入力姿勢信号中の姿勢変化成分、求める帯域以上の揺らぎ、ノイズによる姿勢成分を誤差成分として求める場合は、低域通過フィルタを用いて４元数フィルタ器３０を構成すればよい。

入力姿勢信号Ｓｉｎの他の例として、コンピュータで生成した姿勢データを用いる場合について考える。コンピュータによる姿勢データの生成は離散時間で行われるため、単位時間当たりの姿勢データ量を少なくするほど低コストに演算を実現できる。しかしながら、単位時間当たりの姿勢データ量を少なくすると、姿勢変化が滑らかでなく階段状になる。この階段状の変化成分は高周波成分に含まれる。この場合、図１に示す本実施形態によると、４元数フィルタ器３０を低域通過フィルタで構成する。そうすると、４元数フィルタ器３０は、この階段状の変化成分を入力姿勢４元数から除去した姿勢４元数を出力する。よって、４元数誤差演算器４０において、この階段状の変化成分を含む元の入力姿勢４元数から、この階段状の変化成分を誤差４元数として取り出すことができる。そして、４元数フィードバック演算器５０により、この階段状の変化成分を表す誤差４元数を用いて入力姿勢４元数を補正することで、滑らかな姿勢変化データを得ることができる。

＜第２実施形態＞
図３は、本発明の第２実施形態の姿勢信号演算装置において用いられる４元数誤差演算器４０の構成の例を示すブロック図である。第２実施形態の姿勢信号演算装置は、図１に示す構成の姿勢信号演算装置において、第１姿勢４元数Ｑ１と第２姿勢４元数Ｑ２との間の距離Ｅｑ（式（１６））を誤差４元数Ｕｑとして出力する上述の４元数誤差演算器４０の代わりに、図３に示す構成の４元数誤差演算器４０を用いる。

図３を参照すると、４元数誤差演算器４０は、４元数距離演算器４０２、４元数倍率回転器４０４、４元数積算回転器４０６、４元数差分回転器４０８、及び４元数乗算器４１０を備える。

４元数距離演算器４０２は、４元数誤差演算器４０が受け取った第１姿勢４元数Ｑ１及び第２姿勢４元数Ｑ２について、上述の式（１６）に従って距離４元数Ｅｑを求める。

４元数倍率回転器４０４は、ある離散時間ｎに４元数距離演算器４０２から出力された距離４元数Ｅｑ（ｎ）＝（ε_0Ep（ｎ）；ε_1Ep（ｎ），ε_2Ep（ｎ），ε_3Ep（ｎ））の各要素から、この距離４元数Ｅｑ（ｎ）が表す回転軸ベクトルＶ_p＝（α_p，β_p，γ_p）及び回転角η_pを求める。Ｅｑ（ｎ）は、

と表すことができるので、

である。式（２２）を解くことで、回転軸ベクトルＶ_p及び回転角η_pを求めることができる。回転軸ベクトルＶ_p及び回転角η_pを求めた後、４元数倍率回転器４０４は、予め設定された比例定数Ｋｐを用い、次の式（２３）に従って倍率回転４元数Ｕｑ_pを求める。

４元数積算回転器４０６は、予め定められた離散時間の期間ｉからｎまでの間に４元数距離演算器４０２から出力された距離４元数Ｅｑ（ｉ），Ｅｑ（ｉ＋１），・・・，Ｅｑ（ｎ）について、これらを乗算して積算４元数Ｉｑ（ｎ）を求める（式（２４））。

次に、４元数積算回転器４０６は、積算４元数Ｉｑ（ｎ）＝（ε_0Ei（ｎ）；ε_1Ei（ｎ），ε_2Ei（ｎ），ε_3Ei（ｎ））の各要素から、この積算４元数Ｉｑ（ｎ）が表す回転軸ベクトルＶ_i＝（α_i，β_i，γ_i）及び回転角η_iを求める。Ｉｑ（ｎ）は、

と表すことができるので、

である。式（２６）を解くことで、回転軸ベクトルＶ_i及び回転角η_iを求める。回転軸ベクトルＶ_i及び回転角η_iを求めた後、４元数積算回転器４０６は、予め設定された積算定数Ｋｉを用いて、次の式（２７）に従って、積算回転４元数Ｕｑ_iを求める。

４元数差分回転器４０８は、離散時間ｎに４元数距離演算器４０２が出力した距離４元数Ｅｑ（ｎ）を、離散時間ｎの一時刻前の離散時間ｎ−１に４元数距離演算器４０２が出力した距離４元数Ｅｑ（ｎ−１）で除算することで、Ｅｑ（ｎ）とＥｑ（ｎ−１）との間の距離、すなわち回転の差分を表す差分４元数ΔＥｑ（ｎ）を求める（式（２８））。

次に、４元数差分回転器４０８は、差分４元数ΔＥｑ（ｎ）＝（ε_0Ed（ｎ）；ε_1Ed（ｎ），ε_2Ed（ｎ），ε_3Ed（ｎ））の各要素から、この差分４元数ΔＥｑ（ｎ）が表す回転軸ベクトルＶ_d＝（α_d，β_d，γ_d）及び回転角η_dを求める。ΔＥｑ（ｎ）は、

と表すことができるので、

である。式（３０）を解くことで、回転軸ベクトルＶ_d及び回転角η_dを求める。回転軸ベクトルＶ_d及び回転角η_dを求めた後、４元数積算回転器４０６は、予め設定された差分定数Ｋ_dを用いて、次の式（３１）に従って、差分回転４元数Ｕｑ_dを求める。

４元数乗算器４１０は、４元数倍率回転器４０４、４元数積算回転器４０６、及び４元数差分回転器４０８がそれぞれ求めた倍率回転４元数Ｕｑ_p、積算回転４元数Ｕｑ_i、及び差分回転４元数Ｕｑ_dを乗算し（式（３２））、その乗算結果の４元数Ｕｑ（ｎ）を誤差４元数として出力する。

図３に示す構成の４元数誤差演算器４０を用いる姿勢信号演算装置によると、第１姿勢４元数Ｑ１及び第２姿勢４元数Ｑ２について、現在の距離、過去から積算された距離、及び瞬時変化の距離を求め、それらの距離を反映させた誤差４元数を用いて、４元数フィードバック演算器５０の演算を行うことができる。したがって、図３に示す４元数誤差演算器４０を用いると、時間の経過及び短時間の変化に適応した処理が可能となる。

４元数倍率回転器４０４によると、比例定数Ｋ_pの値を調整することで、現在の姿勢のずれに従った補正効果を容易に調整できる。４元数積算回転器４０６によると、一定期間内の姿勢のずれ量を積算により求めるので、長時間にわたって徐々にずれてゆく姿勢変化を修正できる。４元数差分回転器４０８によると、直前の姿勢と現在の姿勢の変化に対応した補正が可能となるので、最終的に得られる姿勢データの応答性を向上することができる。

なお、第２実施形態の変形例として、４元数誤差演算器４０は、４元数倍率回転器４０４、４元数積算回転器４０６、及び４元数差分回転器４０８のいずれか１つを備えるものであってよい。この変形例の場合、４元数誤差演算器４０は、４元数乗算器４１０を備えず、４元数倍率回転器４０４が求めた倍率回転４元数Ｕｑ_p、４元数積算回転器４０６が求めた積算回転４元数Ｕｑ_i、又は４元数差分回転器４０８が求めた差分回転４元数Ｕｑ_dを誤差４元数Ｕｑとして出力する。

第２実施形態のさらに他の変形例では、４元数誤差演算器４０は、４元数倍率回転器４０４、４元数積算回転器４０６、及び４元数差分回転器４０８のいずれか２つを備えるものであってよい。この変形例の場合、４元数乗算器４１０は、４元数倍率回転器４０４が求めた倍率回転４元数Ｕｑ_pと４元数積算回転器４０６が求めた積算回転４元数Ｕｑ_iとを乗算した結果の４元数Ｕｑ_p・Ｕｑ_i、４元数倍率回転器４０４が求めた倍率回転４元数Ｕｑ_pと４元数差分回転器４０８が求めた差分回転４元数Ｕｑ_dとを乗算した結果の４元数Ｕｑ_p・Ｕｑ_d、又は、４元数積算回転器４０６が求めた積算回転４元数Ｕｑ_iと４元数差分回転器４０８が求めた差分回転４元数Ｕｑ_dとを乗算した結果の４元数Ｕｑ_i・Ｕｑ_d、を誤差４元数Ｕｑとして出力する。

＜第３実施形態＞
図４は、本発明の第３実施形態で用いられる４元数誤差演算器４０の構成の例を示すブロック図である。図４において、図３と同様の要素には、図３と同様の符号を付す。第３実施形態の姿勢信号演算装置は、図１に示す構成の姿勢信号演算装置において、図４に示す構成の４元数誤差演算器４０を用いる。図４に示す４元数誤差演算器４０は、４元数倍率回転器４０４、４元数積算回転器４０６、及び４元数差分回転器４０８のそれぞれと、４元数乗算器４１０と、の間に４元数リミット器４１２ａ，４１２ｂ，４１２ｃを備える。

４元数リミット器４１２ａ，４１２ｂ，４１２ｃは、回転変換を表す４元数の回転角ηが所定の値の範囲内にない場合は、予め定められた値に制限（リミット）する。４元数倍率回転器４０４と４元数乗算器４１０との間に設けられた４元数リミット器４１２ａを例に説明すると、４元数リミット器４１２ａは、４元数倍率回転器４０４から倍率回転４元数Ｕｑ_pを受け取ると、倍率回転４元数Ｕｑ_pの表す回転角Ｋ_pη_pが、−π≦Ｋ_pη_p≦πの範囲内であるか否かを判定する。４元数リミット器４１２ａは、回転角Ｋ_pη_pが、−π≦Ｋ_pη_p≦πの範囲内である場合は、４元数倍率回転器４０４から受け取った倍率回転４元数Ｕｑ_pをそのまま出力し、回転角Ｋ_pη_pがπを超えている場合は、式（２３）において、Ｋ_pη_p＝πとした４元数を倍率回転４元数Ｕｑ_pとして出力し、回転角Ｋ_pη_pが−πを下回っている場合は、式（２３）において、Ｋ_pη_p＝−πとした４元数を倍率回転４元数Ｕｑ_pとして出力する。４元数リミット器４１２ｂ及び４元数リミット器４１２ｃは、それぞれ、積算回転４元数Ｕｑ_i及び差分回転４元数Ｕｑ_dについて、４元数リミット器４１２ａを例に説明した処理と同様の処理を行う。

また、第３実施形態の変形例として、４元数誤差演算器４０は、４元数倍率回転器４０４、４元数積算回転器４０６、及び４元数差分回転器４０８のいずれか１つを備えるものであってよい。この変形例の場合、４元数誤差演算器４０は、４元数乗算器４１０を備えず、４元数倍率回転器４０４が求めた倍率回転４元数Ｕｑ_p、４元数積算回転器４０６が求めた積算回転４元数Ｕｑ_i、又は４元数差分回転器４０８が求めた差分回転４元数Ｕｑ_dについて、４元数リミット器４１２による上述の処理を行った結果の４元数を誤差４元数Ｕｑとして出力する。

第３実施形態のさらに他の変形例では、４元数誤差演算器４０は、４元数倍率回転器４０４、４元数積算回転器４０６、及び４元数差分回転器４０８のいずれか２つを備えるものであってよい。この変形例の場合、４元数倍率回転器４０４が求めた倍率回転４元数Ｕｑ_p、４元数積算回転器４０６が求めた積算回転４元数Ｕｑ_i、及び４元数差分回転器４０８が求めた差分回転４元数Ｕｑ_dのいずれか２つについて、それぞれ、４元数リミット器４１２による上述の処理を行った結果の４元数が、４元数乗算器４１０に対して出力される。４元数乗算器４１０は、受け取った２つの４元数を乗算した結果の４元数を誤差４元数Ｕｑとして出力する。

＜第４実施形態＞
図５は、本発明の第４実施形態で用いられる４元数誤差演算器４０の構成の例を示すブロック図である。図５において、図３及び図４と同様の要素には、図３及び図４と同様の符号を付す。第４実施形態の姿勢信号演算装置は、図１に示す構成の姿勢信号演算装置において、図５に示す構成の４元数誤差演算器４０を用いる。図５に示す４元数誤差演算器４０は、図４に示す４元数誤差演算器４０の構成に加えて、４元数正規化器４１４を備える。４元数正規化器４１４は、４元数距離演算器４０２が出力する距離４元数Ｅｑを式（１４）に従って正規化し、正規化後の４元数を距離４元数Ｅｑとして４元数倍率回転器４０４、４元数積算回転器４０６、及び４元数差分回転器４０８に対して出力する。

なお、第４実施形態の変形例として、４元数誤差演算器４０は、４元数倍率回転器４０４、４元数積算回転器４０６、及び４元数差分回転器４０８のいずれか１つを備えるものであってよい。また、第４実施形態のさらに他の変形例として、４元数誤差演算器４０は、４元数倍率回転器４０４、４元数積算回転器４０６、及び４元数差分回転器４０８のいずれか２つを備えるものであってよい。これらの第４実施形態の変形例において、誤差４元数Ｕｑは、対応する第３実施形態の変形例に関して説明した上述の処理と同様の処理によって求められる。

＜第５実施形態＞
図６は、本発明の第５実施形態の姿勢信号演算装置の構成の例を示すブロック図である。図６を参照すると、本実施形態の姿勢信号演算装置は、入力器１０、微小回転４元数変換器８０、４元数回転器９０、４元数フィルタ器３０、４元数誤差演算器４０、４元数フィードバック演算器５０、４元数姿勢変換器６０、及び出力器７０を備える。

入力器１０は、図１を参照して説明した第１実施形態の姿勢信号演算装置が備える入力器１０と同様のインターフェイス部である。図６において、入力器１０は、センサなどから、運動体の姿勢の変化を表す微小回転信号Ｓｒｏｔを受け取って、例えばアナログデジタル変換処理などによって、本実施形態の姿勢信号演算装置で扱うのに適したデータ形式に変換し、変換した信号を微小回転４元数変換器８０へ出力する。微小回転信号Ｓｒｏｔは、例えば角速度センサによって得られる、センサ座標系のｘ軸、ｙ軸、及びｚ軸の周りの角速度を表す信号である。

微小回転４元数変換器８０は、入力器１０から受け取った微小回転信号を４元数表記に変換する。以下、微小回転４元数変換器８０で行われる演算について、入力器１０に対して、センサ座標系のｘ軸、ｙ軸、ｚ軸の正方向に向けて設けられた角速度センサから、それぞれ図７に示す回転方向（軸の正方向に対して右回り）を正とする角速度ω_x，ω_y，ω_zが微小回転信号Ｓｒｏｔとして入力された場合を例に説明する。

センサ座標系のｘ軸、ｙ軸、ｚ軸それぞれの周りの角速度ω_x，ω_y，ω_z及びサンプリング周期ｔ_sを用いると、各軸の周りの回転変換を表す４元数Ｑｘ，Ｑｙ，Ｑｚは、

と表すことができる。ｘ軸、ｙ軸、ｚ軸の周りの回転変換をまとめて表記した回転変換Ｑ_xyzは、次の式（３４）で表される。

微小回転４元数変換器８０は、式（３４）に従って求めたＱ_xyzを、微小回転４元数として４元数回転器９０へ出力する。

４元数回転器９０は、微小回転４元数変換器８０から得られる微小回転４元数Ｑ_xyzを用いて、基準座標系から見たセンサ座標系を表す姿勢４元数Ｑを求める。例えば、４元数回転器９０は、４元数フィードバック演算器５０から出力される出力姿勢４元数Ｙｕｑ（ｎ）に微小回転４元数Ｑ_xyzを右から乗算し、新たな姿勢４元数Ｑ＝Ｙｕｑ（ｎ＋１）を得る（式（３５））。

また、４元数回転器９０は、４元数フィードバック演算器５０から出力姿勢４元数Ｙｕｑ（ｎ）を用いる代わりに、初期姿勢Ｑ（０）が離散時間ｎにおいてＱ（ｎ）となった場合、離散時間０からｎまでの微小回転４元数Ｑ_xyzを順に右からかけて、姿勢４元数Ｑ（ｎ）を求めてもよい（式（３６））。

４元数回転器９０は、求めた姿勢４元数Ｑを、４元数フィルタ器３０、４元数誤差演算器４０、及び４元数フィードバック演算器５０に対してそれぞれ出力する。

４元数フィルタ器３０は、４元数回転器９０が出力した姿勢４元数に対して、図１を参照して説明した第１実施形態の４元数フィルタ器３０と同様のフィルタ処理を行う。

４元数誤差演算器４０は、４元数回転器９０が出力した姿勢４元数を第１姿勢４元数Ｑ１とし、４元数フィルタ器３０が出力した４元数を第２姿勢４元数Ｑ２として、第１実施形態の４元数誤差演算器４０と同様に、第１姿勢４元数Ｑ１と第２姿勢４元数との間の距離４を表す距離４元数をそのまま誤差４元数として出力する。図６に示す第５実施形態における４元数誤差演算器４０としては、第１実施形態の４元数誤差演算器４０と同様のものを用いる代わりに、図３から図５を参照して説明した第２から第４実施形態の４元数誤差演算器４０のいずれかと同様のものを用いてもよい。

４元数フィードバック演算器５０、４元数姿勢変換器６０、及び出力器７０は、それぞれ、図１を参照して説明した第１実施形態の姿勢信号演算装置の対応する各要素と同様の処理を行う。

＜第６実施形態＞
図８は、本発明の第６実施形態の姿勢信号演算装置の構成の例を示すブロック図である。図８を参照すると、本実施形態の姿勢信号演算装置は、入力器１０ａ，１０ｂ、姿勢４元数変換器２０ａ，２０ｂ、４元数誤差演算器４０、４元数フィードバック演算器５０、４元数姿勢変換器６０、及び出力器７０を備える。図８に示す姿勢信号演算装置の各要素で行われる処理は、図１を参照して説明した第１実施形態において対応する各要素で行われる処理と同様である。

図８に示す姿勢信号演算装置は、２つの入力姿勢信号Ｓｉｎ１，Ｓｉｎ２を入力として受け付ける。入力姿勢信号Ｓｉｎ１，Ｓｉｎ２は、それぞれ、入力器１０ａ，１０ｂを介して姿勢４元数変換器２０ａ，２０ｂに入力される。姿勢４元数変換器２０ａ，２０ｂは、それぞれ、受け取った入力姿勢信号Ｓｉｎ１，Ｓｉｎ２を４元数表記に変換する。４元数誤差演算器４０は、姿勢４元数変換器２０ａが出力した姿勢４元数を第１姿勢４元数Ｑ１として、姿勢４元数変換器２０ｂが出力した姿勢４元数を第２姿勢４元数Ｑ２として受け取って、誤差４元数の演算を行う。図８に示す姿勢信号演算装置の４元数誤差演算器４０は、第１姿勢４元数Ｑ１と第２姿勢４元数Ｑ２との間の距離を表す距離４元数をそのまま誤差４元数として出力するものであってよいし、図３から図５を参照して説明した４元数誤差演算器４０のいずれかと同様のものであってもよい。

図８に示す第６実施形態によると、２つの異なるセンサから得られる姿勢信号を用いて、補正された姿勢信号を得ることができる。例えば、図８に示す実施形態において、入力姿勢信号Ｓｉｎ１として、ロボットの頭部に設けられたカメラ（以下「頭部カメラ」と呼ぶ）の出力から得られる姿勢信号を、入力姿勢信号Ｓｉｎ２として、ロボットの胴体に設けられたカメラ（以下「胴体カメラ」と呼ぶ）の出力から得られる姿勢信号を処理する場合について考える。

頭部カメラはロボットの頭部に設けられるため、頭部カメラとしては、軽量、かつ小型のカメラを用いる必要がある。一方、胴体カメラとしては、頭部カメラと比較してより大きなカメラを用いることができる。頭部カメラは、胴体カメラの設置位置よりも高い位置に設けられるため、胴体カメラと比較してより広範囲の画像を撮像することができる。したがって、頭部カメラの出力からは、胴体カメラと比較して、より正確な姿勢信号を得ることができる。しかしながら、頭部カメラはロボットの頭部に設けられるため、その出力信号はある特定の振動数で振動することが多い。また、頭部カメラは、胴体カメラよりも小型であるため、胴体カメラと比較して出力信号にノイズを多く含む。これに対して、胴体カメラからの出力は、振動に強く安定しており、また、頭部カメラからの出力と比較してノイズが少ない。ただし、胴体カメラは、頭部カメラの設置位置よりも低い位置に設けられるため撮像できる画像の範囲が制限され、胴体カメラの出力から得られる姿勢信号の信頼性は、頭部カメラから得られる姿勢信号よりも低い。

図８の４元数誤差演算器４０において、頭部カメラの出力に基づく入力姿勢信号Ｓｉｎ１を表す姿勢４元数を第１姿勢４元数Ｑ１として、胴体カメラの出力に基づく入力姿勢信号Ｓｉｎ２を表す姿勢４元数を第２姿勢４元数Ｑ２として、両者の距離に基づいて誤差４元数Ｕｑを求めると、この誤差４元数Ｕｑは、頭部カメラの出力に含まれる誤差成分（特定の振動数で振動する姿勢成分及びノイズによる姿勢成分）を表すものとなる。４元数フィードバック演算器５０において、姿勢４元数変換器２０ａが出力した頭部カメラの出力に基づく姿勢４元数を、４元数誤差演算器４０が出力した誤差４元数Ｕｑで除算することにより、頭部カメラの出力に基づく精度の良い姿勢信号から振動成分及びノイズ成分などの誤差成分を取り除いた良好な姿勢信号が得られる。

＜第７実施形態＞
図９は、本発明の第７実施形態の姿勢信号演算装置の構成の例を示すブロック図である。図９に示す第７実施形態の姿勢信号演算装置は、図８に示す第６実施形態の姿勢信号演算装置の構成要素に加えて、４元数フィルタ器３０ａ，３０ｂを備える。４元数フィルタ器３０ａ，３０ｂは、それぞれ、姿勢４元数変換器２０ａ，２０ｂが出力した姿勢４元数に対して、予め設定された周波数特性を有するフィルタを作用させ、フィルタ処理後の４元数を４元数誤差演算器４０へ出力する。４元数誤差演算器２０は、４元数フィルタ器３０ａが出力した４元数を第１姿勢４元数Ｑ１として、４元数フィルタ器３０ｂが出力した４元数を第２姿勢４元数Ｑ２として、両者の距離に基づいて誤差４元数Ｕｑを演算する。

図９に示す第７実施形態の姿勢信号演算装置において、第６実施形態について説明した上述の例と同様、入力姿勢信号Ｓｉｎ１として、ロボットの頭部に設けられた頭部カメラの出力から得られる姿勢信号を、入力姿勢信号Ｓｉｎ２として、ロボットの胴体に設けられた胴体カメラの出力から得られる姿勢信号を処理する場合について考える。一般にタイプが異なるカメラからの姿勢信号の応答性は異なっている。そこで、４元数誤差演算器４０に入力する２つの信号の周波数特性をそろえることが望ましい。

この例では、４元数フィルタ器３０ａは、頭部カメラの出力に含まれる振動成分及びノイズ成分などの誤差成分を多く含む周波数成分を抽出する帯域通過フィルタを用いて構成される。４元数フィルタ器３０ａは、頭部カメラの出力に基づく入力姿勢信号Ｓｉｎ１を表す姿勢４元数から、誤差成分を多く含む周波数成分を抽出して出力する。この信号Ｑ１は、誤差成分と帯域通過フィルタを通過した一部の姿勢信号とを含む。

他方、４元数フィルタ器３０ｂも、４元数フィルタ器３０ａと同じ帯域通過フィルタを用いて構成される。４元数フィルタ器３０ｂは、胴体カメラの出力に基づく姿勢４元数から、頭部カメラの出力に含まれる誤差成分（振動成分及びノイズ成分）に対応する周波数帯域に対応した姿勢４元数を出力する。この信号Ｑ２は、胴体カメラからの姿勢信号を用いているので、頭部カメラからの姿勢信号に含まれる誤差成分を含まないが、帯域通過フィルタを通過した一部の姿勢信号を含む。

４元数誤差演算器４０は、頭部カメラの出力に基づく姿勢４元数であって、誤差成分を多く含む周波数成分と姿勢信号の一部とからなる第１姿勢４元数（４元数フィルタ器３０ａからの出力）Ｑ１と、胴体カメラの出力に基づく姿勢４元数であって、頭部カメラの出力に含まれる誤差成分に対応する周波数成分からなり、誤差成分を含まないが姿勢信号の一部を含む第２姿勢４元数（４元数フィルタ器３０ｂからの出力）Ｑ２と、の間の距離を表す４元数に基づいて、誤差４元数Ｕｑを演算する。この誤差４元数Ｕｑは、頭部カメラの出力に含まれる誤差成分に対応する成分からなる姿勢を表す。４元数フィードバック演算器５０において、姿勢４元数変換器２０ａが出力した頭部カメラの出力に基づく姿勢４元数を、４元数誤差演算器４０が出力した誤差４元数Ｕｑで除算することにより、頭部カメラの出力に基づく精度の良い姿勢信号から振動成分及びノイズ成分などの誤差成分を取り除き、姿勢信号成分の一部が欠落しない良好な姿勢信号が得られる。

図９に示す第７実施形態によると、例えば頭部カメラの出力から得られる姿勢信号が、胴体カメラの出力から得られる姿勢信号より高い応答性を有する場合に、より応答性の高い頭部カメラの出力から得られる姿勢４元数に基づいて、誤差成分を除去した４元数を出力とするため、応答性が高く、かつ誤差の少ない出力を得ることができる。

必要に応じて、図１０及び図１１に示すように、姿勢４元数変換器２０ａ，２０ｂが出力する４元数の両方ではなく、いずれか一方のみを４元数フィルタ器を介して４元数誤差演算器４０に入力するようにしてもよい。

＜第８実施形態＞
図１０に示す第８実施形態の姿勢信号演算装置では、姿勢４元数変換器２０ａの出力は４元数フィルタ器３０ａによるフィルタ処理の後、第１姿勢４元数Ｑ１として４元数誤差演算器４０に入力され、姿勢４元数変換器２０ｂの出力はフィルタ処理なしでそのまま第２姿勢４元数Ｑ２として４元数誤差演算器４０に入力される。

＜第９実施形態＞
図１１に示す第９実施形態の姿勢信号演算装置では、姿勢４元数変換器２０ａの出力はフィルタ処理なしでそのまま第１姿勢４元数Ｑ１として４元数誤差演算器４０に入力され、姿勢４元数変換器２０ｂの出力は４元数フィルタ器３０ｂによるフィルタ処理の後、第２姿勢４元数Ｑ２として４元数誤差演算器４０に入力される。

＜第１０実施形態＞
図１２は、本発明の第１０実施形態の姿勢信号演算装置の構成の例を示すブロック図である。図１２に示す姿勢信号演算装置は、図６に示す第５実施形態の姿勢信号演算装置の構成要素に加えて、入力器１０ｂ、姿勢４元数変換器２０ｂ、及び４元数フィルタ器３０ｂを備える。

図１２を参照すると、運動体の姿勢の変化を表す微小回転信号Ｓｒｏｔは、入力器１０ａを介して微小回転４元数変換器８０に入力される。微小回転４元数変換器８０及び４元数回転器９０、及びフィルタ器３０ａは、図６に示す第５実施形態の微小回転４元数変換器８０、４元数回転器９０、及び４元数フィルタ３０と同様の処理を行う。また、図１２において、入力器１０ｂは、運動体の姿勢を表す入力姿勢信号Ｓｉｎ２を受け取って姿勢４元数変換器２０ｂへ渡す。入力姿勢信号Ｓｉｎ２は、第１実施形態に関連して説明した上述の入力姿勢信号と同様の信号である。また、図１２に示す姿勢４元数変換器２０ｂ及び４元数フィルタ器３０ａは、図１を参照して説明した第１実施形態の姿勢４元数変換器２０及び４元数フィルタ器３０と同様の処理を行う。

図１２を参照すると、４元数誤差演算器４０は、４元数回転器９０が出力した姿勢４元数から４元数フィルタ器３０ａが抽出した周波数成分を表す４元数を第１姿勢４元数Ｑ１として受け取る。また、４元数誤差演算器４０は、入力姿勢信号Ｓｉｎ２に基づいて姿勢４元数変換器２０ｂが出力した姿勢４元数から４元数フィルタ器３０ｂが抽出した周波数成分を表す４元数を第２姿勢４元数Ｑ２として受け取る。図１２に示す４元数誤差演算器４０は、第１姿勢４元数Ｑ１と第２姿勢４元数との間の距離４を表す距離４元数をそのまま誤差４元数として出力するものであってよいし、図３から図５を参照して説明した４元数誤差演算器４０のいずれかと同様のものであってもよい。

＜第１１実施形態＞
図１３は、本発明の第１１実施形態の姿勢信号演算装置の構成の例を示すブロック図である。図１３に示す姿勢信号演算装置は、図１２に示す姿勢信号演算装置において、加速度姿勢演算器１２０をさらに備え、入力器１０ａには微小回転信号として角速度センサ１００の出力が入力され、入力器１０ｂには、入力姿勢信号として加速度姿勢演算器１２０の出力が入力される。

角速度センサ１００の出力を微小回転信号として、微小回転４元数変換器８０及び４元数回転器９０によって姿勢４元数を求める処理は、式（３３−１）〜（３３−３）、式（３４）、及び図６を参照して説明した上述の処理と同様である。

加速度姿勢演算器１２０は、運動体に設けられた加速度センサ１１０の出力に基づいて、運動体の姿勢を表す姿勢行列を演算して出力する。加速度センサ１１０は、センサ座標系のｘ軸、ｙ軸、及びｚ軸のそれぞれの軸方向の加速度Ｇｘ、Ｇｙ、及びＧｚを出力する。加速度姿勢演算器１２０は、加速度Ｇｘ、Ｇｙ、及びＧｚを用いて、まず、センサ座標系のｘ軸、ｙ軸、及びｚ軸のそれぞれと、基準座標系のＸ軸、Ｙ軸、及びＺ軸のそれぞれと、の間の角度（傾斜角）を求める。ｘ軸とＸ軸との間の角度をλ_x、ｙ軸とＹ軸との間の角度をλ_y、かつｚ軸とＺ軸との間の角度をλ_zとすると、それぞれの角度は、次の式（３７）に従って求めることができる。

ここで、Ａｃｏｓ（）は逆余弦関数である。また、Ｇ０＝１Ｇ、又はＧ０＝９．８０６６５ｍ／ｓ²とする。傾斜角λ_x，λ_y，λ_zの変域は、
０≦λ_x≦π
０≦λ_y≦π
０≦λ_z≦π
である。

傾斜角λ_x，λ_y，λ_zを求めた後、加速度姿勢演算器１２０は、姿勢行列Ａ（式（１））の各要素を傾斜角λ_x，λ_y，λ_zを用いて求める。傾斜角λ_x，λ_y，λ_zから姿勢行列Ａの各要素は、

と得られる。

角速度センサの出力は、低周波数領域にドリフト成分を含むことが多い。一方、加速度センサの出力は、高周波数領域に誤差成分を含むことがあるが、低周波数領域は角速度センサの出力と比較して安定している。図１３に示す姿勢信号演算装置において、４元数フィルタ器３０ａ，３０ｂは、互いに同じ周波数特性を有する低域通過フィルタを用いて構成される。４元数フィルタ器３０ａは、角速度センサに基づく姿勢４元数から、ドリフト成分（誤差成分）を多く含む低周波数領域を抽出する。４元数フィルタ器３０ｂは、加速度センサに基づく姿勢４元数から、４元数フィルタ器３０ａと同様に低周波数領域の成分を抽出する。すると、４元数誤差演算器４０において、ドリフト成分を含む角速度センサに基づく姿勢４元数の低周波成分と、誤差成分を含まない加速度センサに基づく姿勢４元数の低周波成分と、の間の距離に基づいて誤差４元数Ｕｑが求められるため、この誤差４元数Ｕｑは、角速度センサ出力のドリフト成分を表す４元数となる。４元数フィードバック演算器５０において、４元数回転器９０が出力した姿勢４元数（角速度センサ出力に基づく姿勢４元数）を、ドリフト成分を表す誤差４元数Ｕｑで除算することで、角速度センサ出力による姿勢からドリフト成分を取り除いた姿勢を表す４元数が出力される。

なお、本実施形態においても、図１３に示す４元数誤差演算器４０は、第１姿勢４元数Ｑ１と第２姿勢４元数との間の距離４を表す距離４元数をそのまま誤差４元数として出力するものであってよいし、図３から図５を参照して説明した４元数誤差演算器４０のいずれかと同様のものであってもよい。

＜第１２実施形態＞
以上で説明した第１から第１１実施形態では、１つの入力姿勢信号又は２つの入力姿勢信号を用いて生成した２つの姿勢４元数に基づいて４元数誤差演算器が求めた誤差成分を、４元数フィードバック演算器での演算に用いる。本発明の第１２実施形態では、３つ以上の入力姿勢信号を用いて、４元数フィードバック演算器での演算に用いる誤差を演算する。図１４に、Ｎ（≧３）個の入力姿勢信号を用いる実施形態の姿勢信号演算装置の構成の例を示す。図１４に示す姿勢信号演算装置の各構成要素において行われる演算は、以上で説明した実施形態の姿勢信号演算装置の対応する各構成要素で行われる演算と同様である。

図１４に示す姿勢信号演算装置において、Ｎ個の入力姿勢信号Ｓｉｎ１，Ｓｉｎ２，…，ＳｉｎＮは、それぞれ、入力器１０を介して姿勢４元数演算器２０に入力され、４元数表記に変換される。また、図１４に示す姿勢信号演算装置は、Ｎ−１個の４元数誤差演算器４０を備える。図１４を参照すると、４元数誤差演算器４０はそれぞれ、入力姿勢信号Ｓｉｎ１に基づく姿勢４元数を第１姿勢４元数Ｑ１として受け取り、入力姿勢信号Ｓｉｎｋ（ｋ＝２，３，…，Ｎ）に基づく姿勢４元数を第２姿勢４元数Ｑ２として受け取る。４元数誤差演算器４０は、それぞれ、入力姿勢信号Ｓｉｎ１に基づく姿勢４元数と、入力姿勢信号Ｓｉｎｋ（ｋ＝２，３，…，Ｎ）に基づく姿勢４元数と、の間の距離を求め、求めた距離を誤差４元数Ｕｑ_i（ｉ＝１，２，…，Ｎ−１）として出力する。求めた距離に所定の定数をかけて重み付けした４元数を誤差４元数Ｕｑ_iとして出力してもよい。

４元数乗算器４１０は、４元数誤差演算器４０が出力した誤差４元数Ｕｑ₁，Ｕｑ₂，…，Ｕｑ_N-1を乗算し、その乗算結果の４元数Ｕｑｓ（式（３９））を４元数フィードバック演算器５０に対して出力する。

４元数フィードバック演算器５０は、入力姿勢信号Ｓｉｎ１に基づく姿勢４元数を、４元数乗算器４１０が出力した４元数Ｕｑｓで除算した結果の４元数を出力姿勢４元数として、４元数姿勢変換器６０に渡す。

図１４に示す姿勢信号演算装置は、４元数フィルタ器を備えない。しかしながら、図１４に示す姿勢信号演算装置において、必要に応じて、例えば図９に示す第７実施形態と同様に、姿勢４元数変換器２０と４元数誤差演算器４０との間に４元数フィルタ器を設け、４元数誤差演算器４０は、フィルタ処理後の姿勢４元数を第１姿勢４元数Ｑ１及び第２姿勢４元数Ｑ２として受け取って演算を行ってもよい。

本発明の第１実施形態の姿勢信号演算装置の構成の例を示すブロック図である。 基準座標系におけるロール角・ピッチ角・ヨー角による姿勢角を表す図である。 本発明の第２実施形態の姿勢信号演算装置の４元数誤差演算器４０の構成の例を示すブロック図である。 本発明の第３実施形態の姿勢信号演算装置の４元数誤差演算器４０の構成の他の例を示すブロック図である。 本発明の第４実施形態の姿勢信号演算装置の４元数誤差演算器４０の構成の他の例を示すブロック図である。 本発明の第５実施形態の姿勢信号演算装置の構成の例を示すブロック図である。 センサ座標系における微小回転角を示す図である。 本発明の第６実施形態の姿勢信号演算装置の構成の例を示すブロック図である。 本発明の第７実施形態の姿勢信号演算装置の構成の例を示すブロック図である。 本発明の第８実施形態の姿勢信号演算装置の構成の例を示すブロック図である。 本発明の第９実施形態の姿勢信号演算装置の構成の例を示すブロック図である。 本発明の第１０実施形態の姿勢信号演算装置の構成の例を示すブロック図である。 本発明の第１１実施形態の姿勢信号演算装置の構成の例を示すブロック図である。 本発明の第１２実施形態の姿勢信号演算装置の構成の例を示すブロック図である。

符号の説明

１０ 入力器、２０ 姿勢４元数変換器、３０ ４元数フィルタ器、４０ ４元数誤差演算器、５０ ４元数フィードバック演算器、６０ ４元数姿勢変換器、７０ 出力器、８０ 微小回転４元数変換器、９０ ４元数回転器、１００ 角速度センサ、１１０ 加速度センサ、１２０ 加速度姿勢演算器、４０２ ４元数距離演算器、４０４ ４元数倍率回転器、４０６ ４元数積算回転器、４０８ ４元数差分回転器、４１０ ４元数乗算器、４１２ ４元数リミット器、４１４ ４元数正規化器。

Claims (15)

1. 運動体の姿勢を表す信号の演算を行う姿勢信号演算装置であって、
   前記運動体の姿勢を表す４元数である入力姿勢４元数に基づいて得られる第１姿勢４元数と、前記運動体の姿勢を表す４元数であって前記第１姿勢４元数と異なる第２姿勢４元数と、を取得し、前記第１姿勢４元数を前記第２姿勢４元数で除算した結果に基づいて得られる４元数を誤差４元数とする誤差演算部と、
   前記入力姿勢４元数を取得し、前記入力姿勢４元数を前記誤差４元数で除算した結果の４元数を出力姿勢４元数とする出力姿勢４元数演算部と、
   を備えることを特徴とする姿勢信号演算装置。
2. 請求項１に記載の姿勢信号演算装置において、
   前記誤差演算部は、
   前記第１姿勢４元数を前記第２姿勢４元数で除算した結果の４元数を距離４元数とする距離演算部と、
   前記距離４元数が表す回転軸ベクトルＶ_p及び回転角η_pを求め、この回転軸ベクトルＶ_pと、この回転角η_pを所定の定数Ｋ_p倍した回転角Ｋ_pη_pと、によって規定される４元数を倍率回転４元数とする倍率回転演算部と、
   所定の期間内に得られる複数の前記距離４元数を乗算し、この乗算結果の４元数が表す回転軸ベクトルＶ_i及び回転角η_iを求め、この回転軸ベクトルＶ_iと、この回転角η_iを所定の定数Ｋ_i倍した回転角Ｋ_iη_iと、によって規定される４元数を積算回転４元数とする積算回転演算部と、
   ある時刻に得られた前記距離４元数を当該時刻よりも前の時刻に得られた前記距離４元数で除算し、この除算結果の４元数が表す回転軸ベクトルＶ_d及び回転角η_dを求め、この回転軸ベクトルＶ_dと、この回転角η_dを所定の定数Ｋ_d倍した回転角Ｋ_dη_dと、によって規定される４元数を差分回転４元数とする差分回転演算部と、
   を備え、
   前記倍率回転４元数、前記積算回転４元数、及び前記差分回転４元数を乗算した結果の４元数を前記誤差４元数とすることを特徴とする姿勢信号演算装置。
3. 請求項１に記載の姿勢信号演算装置において、
   前記誤差演算部は、
   前記第１姿勢４元数を前記第２姿勢４元数で除算した結果の４元数を距離４元数とする距離演算部と、
   前記距離４元数が表す回転軸ベクトルＶ_p及び回転角η_pを求め、この回転軸ベクトルＶ_pと、この回転角η_pを所定の定数Ｋ_p倍した回転角Ｋ_pη_pと、によって規定される４元数を倍率回転４元数とする倍率回転演算部と、
   所定の期間内に得られる複数の前記距離４元数を乗算し、この乗算結果の４元数が表す回転軸ベクトルＶ_i及び回転角η_iを求め、この回転軸ベクトルＶ_iと、この回転角η_iを所定の定数Ｋ_i倍した回転角Ｋ_iη_iと、によって規定される４元数を積算回転４元数とする積算回転演算部と、
   ある時刻に得られた前記距離４元数を当該時刻よりも前の時刻に得られた前記距離４元数で除算し、この除算結果の４元数が表す回転軸ベクトルＶ_d及び回転角η_dを求め、この回転軸ベクトルＶ_dと、この回転角η_dを所定の定数Ｋ_d倍した回転角Ｋ_dη_dと、によって規定される４元数を差分回転４元数とする差分回転演算部と、
   のいずれか１つを備え、
   前記倍率回転４元数、前記積算回転４元数、及び前記差分回転４元数のいずれか１つを前記誤差４元数とすることを特徴とする姿勢信号演算装置。
4. 請求項１に記載の姿勢信号演算装置において、
   前記誤差演算部は、
   前記第１姿勢４元数を前記第２姿勢４元数で除算した結果の４元数を距離４元数とする距離演算部と、
   前記距離４元数が表す回転軸ベクトルＶ_p及び回転角η_pを求め、この回転軸ベクトルＶ_pと、この回転角η_pを所定の定数Ｋ_p倍した回転角Ｋ_pη_pと、によって規定される４元数を倍率回転４元数とする倍率回転演算部と、
   所定の期間内に得られる複数の前記距離４元数を乗算し、この乗算結果の４元数が表す回転軸ベクトルＶ_i及び回転角η_iを求め、この回転軸ベクトルＶ_iと、この回転角η_iを所定の定数Ｋ_i倍した回転角Ｋ_iη_iと、によって規定される４元数を積算回転４元数とする積算回転演算部と、
   ある時刻に得られた前記距離４元数を当該時刻よりも前の時刻に得られた前記距離４元数で除算し、この除算結果の４元数が表す回転軸ベクトルＶ_d及び回転角η_dを求め、この回転軸ベクトルＶ_dと、この回転角η_dを所定の定数Ｋ_d倍した回転角Ｋ_dη_dと、によって規定される４元数を差分回転４元数とする差分回転演算部と、
   のいずれか２つを備え、
   前記倍率回転４元数と前記積算回転４元数とを乗算した結果の４元数、前記倍率回転４元数と前記差分回転４元数とを乗算した結果の４元数、又は、前記積算回転４元数と前記差分回転４元数とを乗算した結果の４元数、を前記誤差４元数とすることを特徴とする姿勢信号演算装置。
5. 請求項１に記載の姿勢信号演算装置において、
   前記誤差演算部は、
   前記第１姿勢４元数を前記第２姿勢４元数で除算した結果の４元数を距離４元数とする距離演算部と、
   前記距離４元数が表す回転軸ベクトルＶ_p及び回転角η_pを求め、前記回転角η_pを所定の定数Ｋ_p倍した回転角Ｋ_pη_pが所定の値の範囲内である場合は、前記回転軸ベクトルＶ_pと前記回転角Ｋ_pη_pとによって規定される４元数を倍率回転４元数とし、前記回転角Ｋ_pη_pが前記所定の値の範囲内でない場合は、前記回転軸ベクトルＶ_pと前記所定の値の範囲内の回転角とによって規定される４元数を倍率回転４元数とする倍率回転演算部と、
   所定の期間内に得られる複数の前記距離４元数を乗算し、この乗算結果の４元数が表す回転軸ベクトルＶ_i及び回転角η_iを求め、前記回転角η_iを所定の定数Ｋ_i倍した回転角Ｋ_iη_iが前記所定の値の範囲内である場合は、前記回転軸ベクトルＶ_iと前記回転角Ｋ_iη_iとによって規定される４元数を積算回転４元数とし、前記回転角Ｋ_iη_iが前記所定の値の範囲内でない場合は、前記回転軸ベクトルＶ_iと前記所定の値の範囲内の回転角とによって規定される４元数を積算回転４元数とする積算回転演算部と、
   ある時刻に得られた前記距離４元数を当該時刻よりも前の時刻に得られた前記距離４元数で除算し、この除算結果の４元数が表す回転軸ベクトルＶ_d及び回転角η_dを求め、前記回転角η_dを所定の定数Ｋ_d倍した回転角Ｋ_dη_dが前記所定の値の範囲内である場合は、前記回転軸ベクトルＶ_dと前記回転角Ｋ_dη_dとによって規定される４元数を差分回転４元数とし、前記回転角Ｋ_dη_dが前記所定の値の範囲内でない場合は、前記回転軸ベクトルＶ_dと前記所定の値の範囲内の回転角とによって規定される４元数を差分回転４元数とする差分回転演算部と、
   を備え、
   前記倍率回転４元数、前記積算回転４元数、及び前記差分回転４元数を乗算した結果の４元数を前記誤差４元数とすることを特徴とする姿勢信号演算装置。
6. 請求項１に記載の姿勢信号演算装置において、
   前記誤差演算部は、
   前記第１姿勢４元数を前記第２姿勢４元数で除算した結果の４元数を距離４元数とする距離演算部と、
   前記距離４元数が表す回転軸ベクトルＶ_p及び回転角η_pを求め、前記回転角η_pを所定の定数Ｋ_p倍した回転角Ｋ_pη_pが所定の値の範囲内である場合は、前記回転軸ベクトルＶ_pと前記回転角Ｋ_pη_pとによって規定される４元数を倍率回転４元数とし、前記回転角Ｋ_pη_pが前記所定の値の範囲内でない場合は、前記回転軸ベクトルＶ_pと前記所定の値の範囲内の回転角とによって規定される４元数を倍率回転４元数とする倍率回転演算部と、
   所定の期間内に得られる複数の前記距離４元数を乗算し、この乗算結果の４元数が表す回転軸ベクトルＶ_i及び回転角η_iを求め、前記回転角η_iを所定の定数Ｋ_i倍した回転角Ｋ_iη_iが前記所定の値の範囲内である場合は、前記回転軸ベクトルＶ_iと前記回転角Ｋ_iη_iとによって規定される４元数を積算回転４元数とし、前記回転角Ｋ_iη_iが前記所定の値の範囲内でない場合は、前記回転軸ベクトルＶ_iと前記所定の値の範囲内の回転角とによって規定される４元数を積算回転４元数とする積算回転演算部と、
   ある時刻に得られた前記距離４元数を当該時刻よりも前の時刻に得られた前記距離４元数で除算し、この除算結果の４元数が表す回転軸ベクトルＶ_d及び回転角η_dを求め、前記回転角η_dを所定の定数Ｋ_d倍した回転角Ｋ_dη_dが前記所定の値の範囲内である場合は、前記回転軸ベクトルＶ_dと前記回転角Ｋ_dη_dとによって規定される４元数を差分回転４元数とし、前記回転角Ｋ_dη_dが前記所定の値の範囲内でない場合は、前記回転軸ベクトルＶ_dと前記所定の値の範囲内の回転角とによって規定される４元数を差分回転４元数とする差分回転演算部と、
   のいずれか１つを備え、
   前記倍率回転４元数、前記積算回転４元数、及び前記差分回転４元数のいずれか１つを前記誤差４元数とすることを特徴とする姿勢信号演算装置。
7. 請求項１に記載の姿勢信号演算装置において、
   前記誤差演算部は、
   前記第１姿勢４元数を前記第２姿勢４元数で除算した結果の４元数を距離４元数とする距離演算部と、
   前記距離４元数が表す回転軸ベクトルＶ_p及び回転角η_pを求め、前記回転角η_pを所定の定数Ｋ_p倍した回転角Ｋ_pη_pが所定の値の範囲内である場合は、前記回転軸ベクトルＶ_pと前記回転角Ｋ_pη_pとによって規定される４元数を倍率回転４元数とし、前記回転角Ｋ_pη_pが前記所定の値の範囲内でない場合は、前記回転軸ベクトルＶ_pと前記所定の値の範囲内の回転角とによって規定される４元数を倍率回転４元数とする倍率回転演算部と、
   所定の期間内に得られる複数の前記距離４元数を乗算し、この乗算結果の４元数が表す回転軸ベクトルＶ_i及び回転角η_iを求め、前記回転角η_iを所定の定数Ｋ_i倍した回転角Ｋ_iη_iが前記所定の値の範囲内である場合は、前記回転軸ベクトルＶ_iと前記回転角Ｋ_iη_iとによって規定される４元数を積算回転４元数とし、前記回転角Ｋ_iη_iが前記所定の値の範囲内でない場合は、前記回転軸ベクトルＶ_iと前記所定の値の範囲内の回転角とによって規定される４元数を積算回転４元数とする積算回転演算部と、
   ある時刻に得られた前記距離４元数を当該時刻よりも前の時刻に得られた前記距離４元数で除算し、この除算結果の４元数が表す回転軸ベクトルＶ_d及び回転角η_dを求め、前記回転角η_dを所定の定数Ｋ_d倍した回転角Ｋ_dη_dが前記所定の値の範囲内である場合は、前記回転軸ベクトルＶ_dと前記回転角Ｋ_dη_dとによって規定される４元数を差分回転４元数とし、前記回転角Ｋ_dη_dが前記所定の値の範囲内でない場合は、前記回転軸ベクトルＶ_dと前記所定の値の範囲内の回転角とによって規定される４元数を差分回転４元数とする差分回転演算部と、
   のいずれか２つを備え、
   前記倍率回転４元数と前記積算回転４元数とを乗算した結果の４元数、前記倍率回転４元数と前記差分回転４元数とを乗算した結果の４元数、又は、前記積算回転４元数と前記差分回転４元数とを乗算した結果の４元数、を前記誤差４元数とすることを特徴とする姿勢信号演算装置。
8. 請求項２から７に記載の姿勢信号演算装置において、
   前記距離演算部は、前記第１姿勢４元数を前記第２姿勢４元数で除算した結果の４元数を前記距離４元数とする代わりに、当該除算した結果の４元数を正規化し、この正規化した４元数を前記距離４元数とすることを特徴とする姿勢信号演算装置。
9. 請求項１から８のいずれか１項に記載の姿勢信号演算装置において、
   前記運動体の姿勢を検出する姿勢検出センサの出力を用いて前記入力姿勢４元数を求める入力姿勢４元数演算部と、
   前記入力姿勢４元数から所定の周波数成分を抽出するフィルタと、
   をさらに備え、
   前記誤差演算部は、前記入力姿勢４元数を前記第１姿勢４元数として取得し、前記入力姿勢４元数から前記フィルタが抽出した周波数成分の４元数を前記第２姿勢４元数として取得することを特徴とする姿勢信号演算装置。
10. 請求項１から８のいずれか１項に記載の姿勢信号演算装置において、
    前記運動体の姿勢の変化である微小回転角を表す微小回転信号に基づいて求めた４元数を微小回転４元数とする微小回転４元数演算部と、
    前記微小回転４元数と前記出力姿勢４元数とを用いて求めた４元数を前記入力姿勢４元数とする入力姿勢４元数演算部と、
    前記入力姿勢４元数から所定の周波数成分を抽出するフィルタと、
    をさらに備え、
    前記誤差演算部は、前記入力姿勢４元数を前記第１姿勢４元数として取得し、前記入力姿勢４元数から前記フィルタが抽出した周波数成分の４元数を前記第２姿勢４元数として取得することを特徴とする姿勢信号演算装置。
11. 請求項１から８のいずれか１項に記載の姿勢信号演算装置において、
    前記運動体の姿勢を検出する第１姿勢検出センサの出力を用いて前記入力４元数を求める入力姿勢４元数演算部と、
    前記運動体の姿勢を検出するセンサであって前記第１姿勢検出センサと異なる特性を有する第２姿勢検出センサの出力を用いて求めた４元数を参照姿勢４元数とする参照姿勢４元数演算部と、
    をさらに備え、
    前記誤差演算部は、前記入力姿勢４元数を前記第１姿勢４元数として取得し、前記参照姿勢４元数を前記第２姿勢４元数として取得することを特徴とする姿勢信号演算装置。
12. 請求項１から８のいずれか１項に記載の姿勢信号演算装置において、
    前記運動体の姿勢の変化である微小回転角を表す微小回転信号に基づいて求めた４元数を微小回転４元数とする微小回転４元数演算部と、
    前記微小回転４元数と前記出力姿勢４元数とを用いて求めた４元数を前記入力姿勢４元数とする入力姿勢４元数演算部と、
    前記運動体の姿勢を検出する姿勢検出センサの出力を用いて求めた４元数を参照姿勢４元数とする参照姿勢４元数演算部と、
    をさらに備え、
    前記誤差演算部は、前記入力姿勢４元数を前記第１姿勢４元数として取得し、前記参照姿勢４元数を前記第２姿勢４元数として取得することを特徴とする姿勢信号演算装置。
13. 請求項１１又は１２に記載の姿勢信号演算装置において、
    前記入力姿勢４元数から所定の周波数成分を抽出する第１フィルタをさらに備え、
    前記誤差演算部は、前記入力姿勢４元数から前記第１フィルタが抽出した周波数成分の４元数を前記第１姿勢４元数として取得することを特徴とする姿勢信号演算装置。
14. 請求項１１から１３のいずれか１項に記載の姿勢信号演算装置において、
    前記参照姿勢４元数から所定の周波数成分を抽出する第２フィルタをさらに備え、
    前記誤差演算部は、前記参照姿勢４元数から前記第２フィルタが抽出した周波数成分の４元数を前記第２姿勢４元数として取得することを特徴とする姿勢信号演算装置。
15. 請求項１２に記載の姿勢信号演算装置において、
    前記入力姿勢４元数から所定の低域周波数成分を抽出する第１フィルタと、
    前記参照姿勢４元数から所定の低域周波数成分を抽出する第２フィルタと、
    をさらに備え、
    前記微小回転信号は、前記運動体の角速度を検出する角速度センサの出力から得られる信号であり、
    前記姿勢検出センサは、前記運動体の加速度を検出する加速度センサであり、
    前記誤差演算部は、前記入力姿勢４元数から前記第１フィルタが抽出した周波数成分の４元数を前記第１姿勢４元数として取得し、前記参照姿勢４元数から前記第２フィルタが抽出した周波数成分の４元数を前記第２姿勢４元数として取得することを特徴とする姿勢信号演算装置。

Images