特開 2022-144935 姿勢制御装置及び姿勢制御方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2022144935

(43)【公開日】2022-10-03

(54)【発明の名称】姿勢制御装置及び姿勢制御方法

(51)【国際特許分類】

   G03B 5/00 20210101AFI20220926BHJP

   G03B 15/00 20210101ALI20220926BHJP

   G03B 17/56 20210101ALI20220926BHJP

   H04N 5/222 20060101ALI20220926BHJP

   H04N 5/232 20060101ALI20220926BHJP

【ＦＩ】

G03B5/00 J

G03B15/00 P

G03B17/56 B

G03B17/56 Z

H04N5/222 100

H04N5/232 480

【審査請求】未請求

【請求項の数】14

【出願形態】ＯＬ

(21)【出願番号】P 2021046140

(22)【出願日】2021-03-19

(71)【出願人】

【識別番号】000104630

【氏名又は名称】キヤノンプレシジョン株式会社

(74)【代理人】

【識別番号】100110412

【弁理士】

【氏名又は名称】藤元 亮輔

(74)【代理人】

【識別番号】100104628

【弁理士】

【氏名又は名称】水本 敦也

(72)【発明者】

【氏名】會津 誠世

(72)【発明者】

【氏名】小林 大毅

(72)【発明者】

【氏名】山本 陽平

【テーマコード（参考）】

2H105

2K005

5C122

【Ｆターム（参考）】

2H105AA06

2H105AA14

2H105AA25

2H105AA53

2H105AA55

2K005AA20

2K005BA42

2K005BA52

2K005BA54

2K005CA04

2K005CA13

2K005CA14

2K005CA22

2K005CA38

2K005CA52

5C122EA41

5C122GD09

5C122HA77

5C122HA78

5C122HA82

5C122HA88

5C122HB01

5C122HB10

(57)【要約】

【課題】制御対応軸が変化しても安定した姿勢制御を行うことが可能であり、補正後の姿勢情報の精度を向上させることが可能であり、少ない通信情報や時間で補正を行うことが可能な制御装置を提供すること。
【解決手段】制御装置は、制御対象を保持する保持部の第１角速度及び第１加速度を用いて保持部の第１姿勢角を推定する第１推定部と、保持部を第１軸周りに回転させる第１駆動部と、保持部を第１軸に直交する第２軸周りに回転させる第２駆動部と、第１駆動部と保持部との第１相対角度を用いて第１加速度を補正することで算出される第１補正加速度、第１相対角度を用いて第１角速度を補正することで算出される第１補正角速度、及び第１駆動部の角速度を用いて、保持部の第２姿勢角を算出する第２推定部とを有し、第１駆動部は、第１姿勢角を用いて姿勢制御を行い、第２駆動部は、第２姿勢角を用いて姿勢制御を行う。
【選択図】図１Ａ

【特許請求の範囲】

【請求項1】

制御対象の姿勢制御を行う姿勢制御装置であって、
前記制御対象を保持する保持部と、
前記保持部の第１角速度及び第１加速度を検出する第１検出部と、
前記第１角速度及び前記第１加速度を用いて前記保持部の第１姿勢角を推定する第１姿勢推定部と、
第１モータを用いて前記保持部を第１軸周りに回転させる第１駆動部と、
第２モータを用いて前記保持部を前記第１軸に直交する第２軸周りに回転させる第２駆動部と、
前記第１駆動部と前記保持部との第１相対角度を算出する第１相対角度算出部と、
前記第１相対角度を用いて前記第１加速度を補正することで第１補正加速度を算出する第１加速度補正部と、
前記第１相対角度を用いて前記第１角速度を補正することで第１補正角速度を算出する第１角速度補正部と、
前記第１駆動部の角速度を算出する角速度算出部と、
前記第１補正加速度、前記第１補正角速度、及び前記第１駆動部の角速度を用いて前記保持部の第２姿勢角を算出する第２姿勢推定部とを有し、
前記第１駆動部は、前記第１姿勢角を用いて姿勢制御を行い、
前記第２駆動部は、前記第２姿勢角を用いて姿勢制御を行うことを特徴とする姿勢制御装置。

【請求項2】

前記第１モータの回転角を検知する第１回転角検知部を更に有し、
前記第１相対角度算出部は、前記第１姿勢角と前記第１モータの回転角とを用いて前記第１相対角度を算出することを特徴とする請求項１に記載の姿勢制御装置。

【請求項3】

前記第１相対角度算出部は、前記第１姿勢角、前記第１モータの回転角、及び前記第１駆動部に対する前記第２駆動部の相対角を用いて前記第１相対角度を算出することを特徴とする請求項２に記載の姿勢制御装置。

【請求項4】

前記保持部、前記第１駆動部、及び前記第２駆動部を支持する支持部と、
前記支持部の第２角速度及び第２加速度を検出する第２検出部と、
前記第２角速度及び前記第２加速度を用いて前記支持部の姿勢角を推定する支持部姿勢推定部とを更に有し、
前記第１相対角度算出部は、前記第１姿勢角と前記支持部の姿勢角とを用いて、前記第１相対角度を算出することを特徴とする請求項１乃至３の何れか一項に記載の姿勢制御装置。

【請求項5】

前記第１相対角度算出部は、前記第１姿勢角、前記支持部の姿勢角、及び前記第１駆動部に対する前記第２駆動部の相対角を用いて前記第１相対角度を算出することを特徴とする請求項４に記載の姿勢制御装置。

【請求項6】

前記第１モータの回転角を検知する第１回転角検知部を更に有し、
前記角速度算出部は、前記第１モータの回転角を単位時間で微分することで、前記第１駆動部の角速度を算出することを特徴とする請求項１乃至５の何れか一項に記載の姿勢制御装置。

【請求項7】

前記保持部、前記第１駆動部、及び前記第２駆動部を支持する支持部と、
前記支持部の第２角速度及び第２加速度を検出する第２検出部とを更に有し、
前記角速度算出部は、前記第２角速度を用いて前記第１駆動部の角速度を算出することを特徴とする請求項１乃至６の何れか一項に記載の姿勢制御装置。

【請求項8】

第３モータを用いて前記保持部を前記第１軸と前記第２軸とに直交する第３軸周りに回転させる第３駆動部を更に有し、
前記第３駆動部は、前記第２姿勢角を用いて姿勢制御を行うことを特徴とする請求項1乃至７の何れか一項に記載の姿勢制御装置。

【請求項9】

第３モータを用いて前記保持部を前記第１軸と前記第２軸とに直交する第３軸周りに回転させる第３駆動部と、
前記第１駆動部と前記保持部との第２相対角度を算出する第２相対角度算出部と、
前記第２相対角度を用いて前記第１加速度を補正することで第２補正加速度を算出する第２加速度補正部と、
前記第２相対角度を用いて前記第１角速度を補正することで第２補正角速度を算出する第２角速度補正部と、
前記第２補正加速度、前記第２補正角速度、及び前記第１駆動部の角速度を用いて、前記保持部の第３姿勢角を算出する第３姿勢推定部とを更に有し、
前記第３駆動部は、前記第３姿勢角を用いて姿勢制御を行うことを特徴とする請求項1乃至７の何れか一項に記載の姿勢制御装置。

【請求項10】

前記第１モータの回転角を検知する第１回転角検知部を更に有し、
前記第２相対角度算出部は、前記第１姿勢角、前記第１モータの回転角、及び記第１駆動部に対する前記第３駆動部の相対角を用いて前記第２相対角度を算出することを特徴とする請求項９に記載の姿勢制御装置。

【請求項11】

前記保持部、前記第１駆動部、前記第２駆動部、及び前記第３駆動部を支持する支持部と、
前記支持部の第２角速度及び第２加速度を検出する第２検出部と、
前記第２角速度及び前記第２加速度を用いて前記支持部の姿勢角を推定する支持部姿勢推定部とを更に有し、
前記第２相対角度算出部は、前記第１姿勢角、前記支持部の姿勢角、及び前記第１駆動部に対する前記第３駆動部の相対角とを用いて、前記第２相対角度を算出することを特徴とする請求項９又は１０に記載の姿勢制御装置。

【請求項12】

前記姿勢制御装置は、スタビライザであることを特徴とする請求項１乃至１１の何れか一項に記載の姿勢制御装置。

【請求項13】

前記制御対象として、撮像光学系により形成された被写体像を撮像する撮像手段を更に有し、
前記姿勢制御装置は、撮像装置であることを特徴とする請求項１乃至１１の何れか一項に記載の姿勢制御装置。

【請求項14】

制御対象の姿勢制御を行う姿勢制御方法であって、
前記制御対象を保持する保持部の第１角速度及び第１加速度を検出するステップと、
前記第１角速度及び前記第１加速度を用いて前記保持部の第１姿勢角を推定するステップと、
第１モータを用いて前記保持部を第１軸周りに回転させる第１駆動部と前記保持部との第１相対角度を算出するステップと、
前記第１相対角度を用いて前記第１加速度を補正することで第１補正加速度を算出するステップと、
前記第１相対角度を用いて前記第１角速度を補正することで第１補正角速度を算出するステップと、
前記第１駆動部の角速度を算出するステップと、
前記第１補正加速度、前記第１補正角速度、及び前記第１駆動部の角速度を用いて、前記保持部の第２姿勢角を算出するステップと、
前記第１姿勢角を用いて前記第１駆動部による姿勢制御を行うステップと、
前記第２姿勢角を用いて第２モータを用いて前記保持部を前記第１軸に直交する第２軸周りに回転させる第２駆動部による姿勢制御を行うステップとを有することを特徴とする姿勢制御方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、制御対象の姿勢を安定させるための姿勢制御装置及び姿勢制御方法に関する。

【背景技術】

【0002】

複数の駆動軸を有する電動ジンバルの姿勢制御を行っている状態でＴＩＬＴ方向へ持ち手部の姿勢を変化させると、ＲＯＬＬの制御軸がＲＯＬＬ軸からＰＡＮ軸に変化し、ＰＡＮの制御軸がＰＡＮ軸からＲＯＬＬ軸に変化するため、姿勢情報を補正する必要がある。特許文献１には、各駆動部の磁気回転エンコーダからの回転情報を用いて姿勢情報を補正する構成が開示されている。特許文献２には、オフセットするべき角度を考慮した各駆動軸に対する制御コマンドを発行することで、姿勢情報を補正する構成が開示されている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０１５－１７７５３９号公報

【特許文献2】米国特許出願公開第２０１９／００６３６６８号明細書

【発明の概要】

【発明が解決しようとする課題】

【0004】

しかしながら、特許文献１の構成では、各駆動部の磁気回転エンコーダからの回転情報を用いるため、各磁気回転エンコーダが有するリニアリティ誤差が、補正値の誤差に積みあがり、補正後の姿勢情報の精度に悪影響を及ぼしてしまう。また、コントローラが各磁気回転エンコーダの情報を必要とするため、通信情報が多くなったり時間がかかったりしてしまう。

【0005】

また、特許文献２の構成では、オフセット分を常に考慮して制御コマンドを発行しなくてはならないので、通信情報が多くなったり時間がかかったりしてしまう。

【0006】

本発明は、制御対応軸が変化しても安定した姿勢制御を行うことが可能であり、補正後の姿勢情報の精度を向上させることが可能であり、少ない通信情報や時間で補正を行うことが可能な姿勢制御装置を提供することを目的とする。

【課題を解決するための手段】

【0007】

本発明の一側面としての姿勢制御装置は、制御対象の姿勢制御を行う姿勢制御装置であって、制御対象を保持する保持部と、保持部の第１角速度及び第１加速度を検出する第１検出部と、第１角速度及び第１加速度を用いて保持部の第１姿勢角を推定する第１姿勢推定部と、第１モータを用いて保持部を第１軸周りに回転させる第１駆動部と、第２モータを用いて保持部を第１軸に直交する第２軸周りに回転させる第２駆動部と、第１駆動部と保持部との第１相対角度を算出する第１相対角度算出部と、第１相対角度を用いて第１加速度を補正することで第１補正加速度を算出する第１加速度補正部と、第１相対角度を用いて第１角速度を補正することで第１補正角速度を算出する第１角速度補正部と、第１駆動部の角速度を算出する角速度算出部と、第１補正加速度、第１補正角速度、及び第１駆動部の角速度を用いて保持部の第２姿勢角を算出する第２姿勢推定部とを有し、第１駆動部は、第１姿勢角を用いて姿勢制御を行い、第２駆動部は、第２姿勢角を用いて姿勢制御を行うことを特徴とする。

【0008】

また、本発明の他の側面としての姿勢制御方法は、制御対象の姿勢制御を行う姿勢制御方法であって、制御対象を保持する保持部の第１角速度及び第１加速度を検出するステップと、第１角速度及び第１加速度を用いて保持部の第１姿勢角を推定するステップと、第１モータを用いて保持部を第１軸周りに回転させる第１駆動部と保持部との第１相対角度を算出するステップと、第１相対角度を用いて第１加速度を補正することで第１補正加速度を算出するステップと、第１相対角度を用いて第１角速度を補正することで第１補正角速度を算出するステップと、第１駆動部の角速度を算出するステップと、第１補正加速度、第１補正角速度、及び第１駆動部の角速度を用いて、保持部の第２姿勢角を算出するステップと、第１姿勢角を用いて第１駆動部による姿勢制御を行うステップと、第２姿勢角を用いて第２モータを用いて保持部を第１軸に直交する第２軸周りに回転させる第２駆動部による姿勢制御を行うステップとを有することを特徴とする。

【発明の効果】

【0009】

本発明によれば、制御対応軸が変化しても安定した姿勢制御を行うことが可能であり、補正後の姿勢情報の精度を向上させることが可能であり、少ない通信情報や時間で補正を行うことが可能な姿勢制御装置を提供することができる。

【図面の簡単な説明】

【0010】

【図1A】実施例１の姿勢制御装置のブロック図である。

【図1B】実施例１の姿勢制御装置の変形例のブロック図である。

【図2】ＴＩＬＴ軸とＲＯＬＬ軸とで構成される２軸スタビライザの駆動部の配置関係が０°である場合を示す図である。

【図3】ＴＩＬＴ軸とＰＡＮ軸とで構成される２軸スタビライザの駆動部の配置関係が０°である場合を示す図である。

【図4】ＴＩＬＴ軸とＲＯＬＬ軸とで構成される２軸スタビライザの駆動部の配置関係が０°でない場合を示す図である。

【図5】第１相対角度算出部のブロック図である。

【図6】加速度補正部による加速度補正処理を示すフローチャートである。

【図7】角速度補正部による角速度補正処理を示すフローチャートである。

【図8A】実施例２の姿勢制御装置のブロック図である。

【図8B】実施例２の姿勢制御装置の変形例のブロック図である。

【図9】各駆動部と対応軸との関係を示す図である。

【図10】ＴＩＬＴ軸とＲＯＬＬ軸とＰＡＮ軸とで構成される３軸スタビライザの駆動部の配置関係が０°である場合を示す図である。

【図11】ＴＩＬＴ軸とＲＯＬＬ軸とＰＡＮ軸とで構成される３軸スタビライザの駆動部の配置関係が０°でない場合を示す図である。

【図12A】実施例３の姿勢制御装置のブロック図である。

【図12B】実施例３の姿勢制御装置の変形例のブロック図である。

【図13】第２相対角度算出部のブロック図である。

【図14A】実施例４の姿勢制御装置のブロック図である。

【図14B】実施例４の姿勢制御装置の変形例のブロック図である。

【図15A】実施例５の姿勢制御装置のブロック図である。

【図15B】実施例５の姿勢制御装置の変形例のブロック図である。

【図16A】実施例６の姿勢制御装置のブロック図である。

【図16B】実施例６の姿勢制御装置の変形例のブロック図である。

【発明を実施するための形態】

【0011】

以下、本発明の実施形態について、図面を参照しながら詳細に説明する。各図において、同一の部材については同一の参照番号を付し、重複する説明は省略する。

【0012】

各実施例の姿勢制御機器は、制御対象の姿勢制御を行う装置であって、例えば電動スタビライザであってもよいし、撮像装置に組み込まれる装置であってもよい。

【実施例0013】

図１Ａは、本実施例の２軸の姿勢制御を行う姿勢制御機器（姿勢制御装置）のブロック図である。本実施例の姿勢制御機器は、第１駆動部１００、第２駆動部１４０、及び保持部３００を有する。保持部３００は、制御対象を保持すると共に、第１駆動部１００及び第２駆動部１４０により２軸方向へ回転可能に保持される。第１駆動部１００は、第１モータ１０２を用いて保持部３００を第１軸周りに回転させ、第２駆動部１４０は第２モータ１４２を用いて第１軸に直交する第２軸周りに回転させる。制御対象は例えば、撮像光学系により形成された被写体像を撮像する撮像手段である。第２駆動部１４０は、第１駆動部１００に対して、例えば図２乃至図４に示されるように配置されている。

【0014】

保持部３００は、保持部３００の角速度（第１角速度）及び加速度（第１加速度）を検出する第１ＩＭＵセンサ（第１検出部）３０１を有する。第１駆動部１００は、第１駆動回路１０１、第１モータ１０２、第１回転角センサ（回転角検知部）１０３、第１ＭＰＵ２００、及び第１メモリ２１５を有する。第２駆動部１４０は、第２駆動回路１４１、第２モータ１４２、及び第２回転角センサ１４３を有する。

【0015】

第１ＭＰＵ２００は、第１駆動信号処理部２０１、第１姿勢推定部２０３、第１相対角度算出部２０４、第１加速度補正部２０５、第１角速度補正部２０６、角速度算出部２０７、第２姿勢推定部２０８、及び第２駆動信号処理部２４１を有する。第１ＭＰＵ２００は、第１メモリ２１５に記憶されているプログラムを実行することにより、姿勢制御機器の各部を制御する。

【0016】

第１姿勢推定部２０３は、第１ＩＭＵセンサ３０１からの角速度及び加速度を用いて保持部３００の姿勢を推定する。具体的には、第１姿勢推定部２０３は、第１ＩＭＵセンサ３０１からのＸＹＺ軸の各軸の角速度及び加速度を用いて、保持部３００の第１姿勢角である、第１ＴＩＬＴ角、第１ＲＯＬＬ角、及び第１ＰＡＮ角を算出する。

【0017】

第１駆動信号処理部２０１は、第１姿勢推定部２０３からの第１ＴＩＬＴ角とＴＩＬＴ軸目標角度との角度偏差、及び第１ＩＭＵセンサ３０１からのＸ軸角速度ωｘ１とＴＩＬＴ軸目標角速度との角速度偏差を取得する。第１駆動信号処理部２０１は、取得した角度偏差及び角速度偏差を用いたＰＩＤ制御等により取得される操作量と第１回転角センサ１０３により検知された第１モータ１０２の回転角に基づく第１モータ１０２への通電パターンとを第１駆動回路１０１に送信する。

【0018】

第１駆動回路１０１は、第１駆動信号処理部２０１からの信号に応じて、第１モータ１０２を駆動する。

【0019】

図５は、第１相対角度算出部２０４のブロック図である。第１相対角度算出部２０４は、第１姿勢角と第１回転角センサ１０３により検知された第１モータ１０２の回転角とを用いて第１駆動部１００と保持部３００との相対角度（第１相対角度）を算出する。具体的には、第１相対角度算出部２０４は、第１ＴＩＬＴ角と、第１回転角センサ１０３により検知された第１モータ１０２の回転角及び第１メモリ２１５に記録されている第１駆動部配置角２１６との差である第１角度偏差θｔｄｉｆ１を算出する。なお、第１駆動部配置角２１６は、第１駆動部１００に対する第２駆動部１４０の相対角である。

【0020】

第１加速度補正部２０５は、第１相対角度算出部２０４からの相対角度に基づいて保持部３００の加速度を補正することで第１補正加速度を算出する。図６は、第１加速度補正部２０５による加速度補正処理を示すフローチャートである。

【0021】

ステップＳ１０１では、第１加速度補正部２０５は、第１相対角度算出部２０４から第１角度偏差θｔｄｉｆ１を取得する。

【0022】

ステップＳ１０２では、第１加速度補正部２０５は、第１姿勢推定部２０３から保持部３００の第１姿勢角として第１ＲＯＬＬ角及び第１ＰＡＮ角を取得する。

【0023】

なお、ステップＳ１０１とステップＳ１０２の処理の順番は入れ替えてもよい。

【0024】

ステップＳ１０３では、第１加速度補正部２０５は、第１角度偏差θｔｄｉｆ１と第１ＲＯＬＬ角及び第１ＰＡＮ角とを用いて、以下の式（１）で表されるＺ軸方向にのみ重力加速度が発生している基準加速度ベクトルαｂａｓｅをセットする。

【0025】

【数1】

【0026】

ステップＳ１０４では、第１加速度補正部２０５は、基準加速度ベクトルαｂａｓｅを、式（２）を用いて第１角度偏差だけ３次元回転させることで第１変形値α’ｂａｓｅを算出する。

【0027】

【数2】

【0028】

ステップＳ１０５では、第１加速度補正部２０５は、第１変形値α’ｂａｓｅを、式（３）を用いて第１ＲＯＬＬ角だけ３次元回転させることで第２変形値α”ｂａｓｅを算出する。

【0029】

【数3】

【0030】

ステップＳ１０６では、第１加速度補正部２０５は、第２変形値α”ｂａｓｅを、式（４）を用いてＰＡＮ角だけ３次元回転させることで第１加速度補正値（第１補正加速度）を算出する。

【0031】

【数4】

【0032】

第１角速度補正部２０６は、第１相対角度算出部２０４からの相対角度に基づいて保持部３００の角速度を補正することで第１補正角速度を算出する。図７は、第１角速度補正部２０６による角速度補正処理を示すフローチャートである。

【0033】

ステップＳ２０１では、第１角速度補正部２０６は、第１相対角度算出部２０４から第１角度偏差θｔｄｉｆ１を取得する。

【0034】

ステップＳ２０２では、第１角速度補正部２０６は、第１ＩＭＵセンサ３０１から保持部３００のＹ軸角速度ωｙ１及びＺ軸角速度ωｚ１を取得する。

【0035】

なお、ステップＳ１０１とステップＳ１０２の処理の順番は入れ替えてもよい。

【0036】

ステップＳ２０３では、第１角速度補正部２０６は、以下の式（５ａ）及び式（５ｂ）を用いて、第１角度偏差θｔｄｉｆ１及びＹ軸角速度ωｙ１から第１Ｙ軸角速度補正値ωｙｃｒｒ１及び第１Ｚ軸角速度補正値ωｚｃｃｒ１を算出する。

【0037】

【数5】

【0038】

ステップＳ２０４では、第１角速度補正部２０６は、以下の式（６ａ）及び式（６ｂ）を用いて、第１角度偏差θｔｄｉｆ１及びＺ軸角速度ωｚ１から第２Ｙ軸角速度補正値ωｙｃｒｒ２及び第２Ｚ軸角速度補正値ωｚｃｃｒ２を算出する。

【0039】

【数6】

【0040】

ステップＳ２０５では、第１角速度補正部２０６は、第１Ｙ軸角速度補正値ωｙｃｒｒ１と第２Ｙ軸角速度補正値ωｙｃｒｒ２との差分値であるＹ軸角速度補正値（第１補正角速度）を算出する。

【0041】

ステップＳ２０６では、第１角速度補正部２０６は、第１Ｚ軸角速度補正値ωｚｃｒｒ１と第２Ｚ軸角速度補正値ωｚｃｒｒ２との差分値であるＺ軸角速度補正値（第１補正角速度）を算出する。

【0042】

角速度算出部２０７は、第１回転角センサ１０３により検知された第１モータ１０２の回転角と、第１ＭＰＵ２００のサンプリング周期又は内部クロックとを用いて第１駆動部１００の角速度を算出する。具体的には、角速度算出部２０７は、第１モータ１０２の回転角を単位時間で微分することで、第１駆動部１００の角速度を算出する。

【0043】

第２姿勢推定部２０８は、第１加速度補正部２０５からの第１加速度補正値と、第１角速度補正部２０６からのＹ軸及びＺ軸角速度補正値と、角速度算出部２０７からの第１駆動部１００の角速度とを用いて、保持部３００の姿勢を推定する。具体的には、第２姿勢推定部２０８は、保持部３００の第２姿勢角である、第２ＴＩＬＴ角、第２ＲＯＬＬ角及び第２ＰＡＮ角を算出する。

【0044】

第２駆動信号処理部２４１は、第２駆動部１４０がＲＯＬＬ軸に配置されている場合、第２ＲＯＬＬ角とＲＯＬＬ軸目標角度との角度偏差、及びＹ軸角速度補正値とＲＯＬＬ軸目標角速度との角速度偏差を算出する。また、第２駆動信号処理部２４１は、第２駆動部１４０がＰＡＮ軸に配置されている場合、第２ＰＡＮ角とＰＡＮ軸目標角度との角度偏差、及びＺ軸角速度補正値とＰＡＮ軸目標角速度との角速度偏差を算出する。第２駆動信号処理部２４１は、取得した角度偏差及び角速度偏差を用いたＰＩＤ制御等により取得される操作量と、第２回転角センサ１４３により検知された第２モータ１４２の回転角に基づく第２モータ１４２への通電パターンとを第２駆動回路１４１に送信する。

【0045】

第２駆動回路１４１は、第２駆動信号処理部２４１からの信号に応じて、第２モータ１４２を駆動する。

【0046】

以上説明したように、本実施例では、第１駆動部１００は第１姿勢推定部２０３からの保持部３００の第１姿勢角で姿勢制御を行い、第２駆動部１４０は第２姿勢推定部２０８からの保持部３００の第２姿勢角で姿勢制御を行う。

【0047】

本実施例の構成によれば、加速度及び角速度を補正することで得られる加速度補正値及び角速度補正値を用いて姿勢推定を行うことで得られる姿勢角と、角速度補正値とをＴＩＬＴ軸以外のフィードバック制御に用いる。これにより、持ち手の姿勢に対応可能となり、制御対応軸が変化しても安定した姿勢制御を行うことができる。また、補正後の姿勢情報の精度を向上させることが可能であると共に、少ない通信情報や時間で補正を行うことが可能である。

【0048】

以下、図１Ｂを参照して、本実施例の姿勢制御装置の変形例について説明する。図１Ｂは、本実施例の姿勢制御装置の変形例のブロック図である。

【0049】

変形例の姿勢制御機器は、本実施例の構成に加え、第１駆動部１００、第２駆動部１４０、及び保持部３００を支持する支持部４００を有する。支持部４００は、支持部４００の角速度（第２角速度）及び加速度（第２加速度）を検出する第２ＩＭＵセンサ（第２検出部）４０１を有する。

【0050】

第１駆動部１００は、本実施例の構成に加え、第４姿勢推定部（支持部姿勢推定部）２１４を有する。第４姿勢推定部２１４は、第２ＩＭＵセンサ４０１からの角速度及び加速度を用いて支持部４００の姿勢を推定する。具体的には、第４姿勢推定部２１４は、第２ＩＭＵセンサ４０１からのＸＹＺ軸の各軸の角速度及び加速度を用いて、支持部４００の姿勢角である、第４ＴＩＬＴ角、第４ＲＯＬＬ角、及び第４ＰＡＮ角を算出する。第１相対角度算出部２０４は、第１角度偏差θｔｄｉｆ１として、第１姿勢推定部２０３からの第１ＴＩＬＴ角と第４姿勢推定部２１４からの第４ＴＩＬＴ角との角度偏差を算出してもよい。角速度算出部２０７は、第２ＩＭＵセンサ４０１のデータを用いて第１駆動部１００の角速度を検知してもよい。

【実施例0051】

図８Ａは、本実施例の３軸の姿勢制御を行う姿勢制御装置のブロック図である。図９は、各駆動部と対応軸との関係を示す図である。本実施例では、実施例１と異なる構成について説明し、実施例１と同様の構成については同一の参照番号を付し、詳細な説明は省略する。

【0052】

本実施例の姿勢制御機器は、実施例１の構成に加え、第３駆動部１７０を有する。保持部３００は、第１駆動部１００、第２駆動部１４０、及び第３駆動部１７０により３軸方向へ回転可能に保持される。第３駆動部１７０は、第１軸と第２軸とに直交する第３軸周りに保持部３００を回転させる。第２駆動部１４０及び第３駆動部１７０は、第１駆動部１００に対して、例えば図１０や図１１に示されるように配置されている。第３駆動部１７０は、第３駆動回路１７１、第３モータ１７２、及び第３回転角センサ１７３を有する。

【0053】

第１ＭＰＵ２００は、実施例１の構成に加え、第３駆動信号処理部２７１を有する。第３駆動信号処理部２７１は、第２ＰＡＮ角とＰＡＮ軸目標角度との角度偏差、及びＺ軸角速度補正値とＰＡＮ軸目標角速度との角速度偏差を算出する。第３駆動信号処理部２７１は、取得した角度偏差及び角速度偏差を用いたＰＩＤ制御等により操作量と、第３回転角センサ１７３により検知された第３モータ１７２の回転角に基づく第３モータ１７２への通電パターンとを第３駆動回路１７１に送信する。

【0054】

第３駆動回路１７１は、第３駆動信号処理部２７１からの信号に応じて、第３モータ１７２を駆動する。

【0055】

以上説明したように、本実施例では、第３駆動部１７０は、第２姿勢推定部２０８からの保持部３００の第２姿勢角で姿勢制御を行う。

【0056】

以上説明したように、本実施例の構成によれば、加速度及び角速度を補正することで得られる加速度補正値及び角速度補正値を用いて姿勢推定を行うことで得られる姿勢角と、角速度補正値とをＴＩＬＴ軸以外のフィードバック制御に用いる。これにより、持ち手の姿勢に対応可能となり、制御対応軸が変化しても安定した姿勢制御を行うことができる。また、補正後の姿勢情報の精度を向上させることが可能であると共に、少ない通信情報や時間で補正を行うことが可能である。

【0057】

なお、第２駆動部１４０がＰＡＮ軸に、第３駆動部１７０がＲＯＬＬ軸に構成される場合であっても、第２駆動信号処理部２４１と第３駆動信号処理部２７１に入力する軸のデータを変更することで、本実施例の構成を適用できるのは言うまでもない。

【0058】

以下、図８Ｂを参照して、本実施例の姿勢制御装置の変形例について説明する。図８Ｂは、本実施例の姿勢制御装置の変形例のブロック図である。

【0059】

変形例の姿勢制御機器は、本実施例の構成に加え、第１駆動部１００、第２駆動部１４０、及び保持部３００を支持する支持部４００を有する。支持部４００は、支持部４００の角速度及び加速度を検出する第２ＩＭＵセンサ４０１を有する。

【0060】

第１駆動部１００は、本実施例の構成に加え、第４姿勢推定部２１４を有する。第４姿勢推定部２１４は、第２ＩＭＵセンサ４０１からの角速度及び加速度を用いて支持部４００の姿勢を推定する。具体的には、第４姿勢推定部２１４は、第２ＩＭＵセンサ４０１からのＸＹＺ軸の各軸の角速度及び加速度を用いて、支持部４００の姿勢角である、第４ＴＩＬＴ角、第４ＲＯＬＬ角、及び第４ＰＡＮ角を算出する。第１相対角度算出部２０４は、第１角度偏差θｔｄｉｆ１として、第１姿勢推定部２０３からの第１ＴＩＬＴ角と第４姿勢推定部２１４からの第４ＴＩＬＴ角との角度偏差を算出してもよい。角速度算出部２０７は、第２ＩＭＵセンサ４０１のデータを用いて第１駆動部１００の角速度を検知してもよい。