JP7698441B2

JP7698441B2 - Attitude control device and attitude control method

Info

Publication number: JP7698441B2
Application number: JP2021046140A
Authority: JP
Inventors: 誠世會津; 大毅小林; 陽平山本
Original assignee: Canon Precision Inc
Current assignee: Canon Precision Inc
Priority date: 2021-03-19
Filing date: 2021-03-19
Publication date: 2025-06-25
Anticipated expiration: 2041-03-19
Also published as: JP2022144935A

Description

本発明は、制御対象の姿勢を安定させるための姿勢制御装置及び姿勢制御方法に関する。 The present invention relates to an attitude control device and attitude control method for stabilizing the attitude of a controlled object.

複数の駆動軸を有する電動ジンバルの姿勢制御を行っている状態でＴＩＬＴ方向へ持ち手部の姿勢を変化させると、ＲＯＬＬの制御軸がＲＯＬＬ軸からＰＡＮ軸に変化し、ＰＡＮの制御軸がＰＡＮ軸からＲＯＬＬ軸に変化するため、姿勢情報を補正する必要がある。特許文献１には、各駆動部の磁気回転エンコーダからの回転情報を用いて姿勢情報を補正する構成が開示されている。特許文献２には、オフセットするべき角度を考慮した各駆動軸に対する制御コマンドを発行することで、姿勢情報を補正する構成が開示されている。 When the attitude of the handle is changed in the TILT direction while performing attitude control on an electric gimbal having multiple drive axes, the control axis for ROLL changes from the ROLL axis to the PAN axis, and the control axis for PAN changes from the PAN axis to the ROLL axis, so it is necessary to correct the attitude information. Patent Document 1 discloses a configuration in which the attitude information is corrected using rotation information from a magnetic rotary encoder of each drive unit. Patent Document 2 discloses a configuration in which the attitude information is corrected by issuing a control command for each drive axis that takes into account the angle to be offset.

特開２０１５－１７７５３９号公報JP 2015-177539 A 米国特許出願公開第２０１９／００６３６６８号明細書US Patent Application Publication No. 2019/0063668

しかしながら、特許文献１の構成では、各駆動部の磁気回転エンコーダからの回転情報を用いるため、各磁気回転エンコーダが有するリニアリティ誤差が、補正値の誤差に積みあがり、補正後の姿勢情報の精度に悪影響を及ぼしてしまう。また、コントローラが各磁気回転エンコーダの情報を必要とするため、通信情報が多くなったり時間がかかったりしてしまう。 However, the configuration of Patent Document 1 uses rotation information from the magnetic rotary encoders of each drive unit, so the linearity error of each magnetic rotary encoder accumulates in the error of the correction value, adversely affecting the accuracy of the attitude information after correction. In addition, because the controller requires information from each magnetic rotary encoder, communication information increases and takes time.

また、特許文献２の構成では、オフセット分を常に考慮して制御コマンドを発行しなくてはならないので、通信情報が多くなったり時間がかかったりしてしまう。 In addition, in the configuration of Patent Document 2, control commands must always be issued while taking the offset into consideration, which results in a large amount of communication information and takes a long time.

本発明は、制御対応軸が変化しても安定した姿勢制御を行うことが可能であり、補正後の姿勢情報の精度を向上させることが可能であり、少ない通信情報や時間で補正を行うことが可能な姿勢制御装置を提供することを目的とする。 The present invention aims to provide an attitude control device that can perform stable attitude control even if the control corresponding axis changes, can improve the accuracy of attitude information after correction, and can perform corrections with less communication information and time.

本発明の一側面としての姿勢制御装置は、制御対象の姿勢制御を行う姿勢制御装置であって、制御対象を保持する保持部と、保持部の第１角速度及び第１加速度を検出する第１検出部と、第１角速度及び第１加速度を用いて保持部の第１姿勢角を推定する第１姿勢推定部と、第１モータを用いて保持部を第１軸周りに回転させる第１駆動部と、第２モータを用いて保持部を第１軸に直交する第２軸周りに回転させる第２駆動部と、第１駆動部と保持部との第１相対角度を算出する第１相対角度算出部と、第１相対角度を用いて第１加速度を補正することで第１補正加速度を算出する第１加速度補正部と、第１相対角度を用いて第１角速度を補正することで第１補正角速度を算出する第１角速度補正部と、第１駆動部の角速度を算出する角速度算出部と、第１補正加速度、第１補正角速度、及び第１駆動部の角速度を用いて保持部の第２姿勢角を算出する第２姿勢推定部とを有し、第１駆動部は、第１姿勢角を用いて姿勢制御を行い、第２駆動部は、第１駆動部に接続され第２姿勢角を用いて姿勢制御を行うことを特徴とする。 An attitude control device according to one aspect of the present invention is an attitude control device that performs attitude control of a control object, and includes a holder that holds the control object, a first detection unit that detects a first angular velocity and a first acceleration of the holder, a first attitude estimation unit that estimates a first attitude angle of the holder using the first angular velocity and the first acceleration, a first drive unit that rotates the holder about a first axis using a first motor, a second drive unit that rotates the holder about a second axis orthogonal to the first axis using a second motor, a first relative angle calculation unit that calculates a first relative angle between the first drive unit and the holder, and a first relative The actuator control device includes a first acceleration correction unit that calculates a first corrected acceleration by correcting the first acceleration using an angle, a first angular velocity correction unit that calculates a first corrected angular velocity by correcting the first angular velocity using the first relative angle, an angular velocity calculation unit that calculates an angular velocity of the first drive unit, and a second attitude estimation unit that calculates a second attitude angle of the holding unit using the first corrected acceleration, the first corrected angular velocity, and the angular velocity of the first drive unit, wherein the first drive unit performs attitude control using the first attitude angle, and the second drive unit is connected to the first drive unit and performs attitude control using the second attitude angle.

また、本発明の他の側面としての姿勢制御方法は、制御対象の姿勢制御を行う姿勢制御方法であって、制御対象を保持する保持部の第１角速度及び第１加速度を検出するステップと、第１角速度及び第１加速度を用いて保持部の第１姿勢角を推定するステップと、第１モータを用いて保持部を第１軸周りに回転させる第１駆動部と保持部との第１相対角度を算出するステップと、第１相対角度を用いて第１加速度を補正することで第１補正加速度を算出するステップと、第１相対角度を用いて第１角速度を補正することで第１補正角速度を算出するステップと、第１駆動部の角速度を算出するステップと、第１補正加速度、第１補正角速度、及び第１駆動部の角速度を用いて、保持部の第２姿勢角を算出するステップと、第１姿勢角を用いて第１駆動部による姿勢制御を行うステップと、第１駆動部に接続され第２姿勢角を用いて第２モータを用いて保持部を第１軸に直交する第２軸周りに回転させる第２駆動部による姿勢制御を行うステップとを有することを特徴とする。 Moreover, an attitude control method as another aspect of the present invention is an attitude control method for performing attitude control of a control object, comprising the steps of: detecting a first angular velocity and a first acceleration of a holding unit that holds the control object; estimating a first attitude angle of the holding unit using the first angular velocity and the first acceleration; calculating a first relative angle between the holding unit and a first drive unit that rotates the holding unit around a first axis using a first motor; calculating a first corrected acceleration by correcting the first acceleration using the first relative angle; calculating a first corrected angular velocity by correcting the first angular velocity using the first relative angle; calculating the angular velocity of the first drive unit; calculating a second attitude angle of the holding unit using the first corrected acceleration, the first corrected angular velocity, and the angular velocity of the first drive unit; performing attitude control by the first drive unit using the first attitude angle; and performing attitude control by a second drive unit that is connected to the first drive unit and rotates the holding unit around a second axis perpendicular to the first axis using the second attitude angle.

本発明によれば、制御対応軸が変化しても安定した姿勢制御を行うことが可能であり、補正後の姿勢情報の精度を向上させることが可能であり、少ない通信情報や時間で補正を行うことが可能な姿勢制御装置を提供することができる。 The present invention provides an attitude control device that can perform stable attitude control even if the control corresponding axis changes, can improve the accuracy of attitude information after correction, and can perform corrections with less communication information and time.

実施例１の姿勢制御装置のブロック図である。FIG. 1 is a block diagram of an attitude control device according to a first embodiment. 実施例１の姿勢制御装置の変形例のブロック図である。FIG. 4 is a block diagram of a modified example of the attitude control device of the first embodiment. ＴＩＬＴ軸とＲＯＬＬ軸とで構成される２軸スタビライザの駆動部の配置関係が０°である場合を示す図である。FIG. 13 is a diagram showing a case where the arrangement relationship of the drive parts of the two-axis stabilizer composed of a tilt shaft and a roll shaft is 0°. ＴＩＬＴ軸とＰＡＮ軸とで構成される２軸スタビライザの駆動部の配置関係が０°である場合を示す図である。FIG. 13 is a diagram showing a case where the arrangement relationship of the drive parts of the two-axis stabilizer composed of a tilt axis and a pan axis is 0°. ＴＩＬＴ軸とＲＯＬＬ軸とで構成される２軸スタビライザの駆動部の配置関係が０°でない場合を示す図である。13 is a diagram showing a case where the arrangement relationship of the drive parts of a two-axis stabilizer composed of a tilt shaft and a roll shaft is not 0°. FIG. 第１相対角度算出部のブロック図である。FIG. 4 is a block diagram of a first relative angle calculation unit. 加速度補正部による加速度補正処理を示すフローチャートである。13 is a flowchart showing an acceleration correction process performed by an acceleration correction unit. 角速度補正部による角速度補正処理を示すフローチャートである。13 is a flowchart showing an angular velocity correction process performed by an angular velocity correction unit. 実施例２の姿勢制御装置のブロック図である。FIG. 11 is a block diagram of an attitude control device according to a second embodiment. 実施例２の姿勢制御装置の変形例のブロック図である。FIG. 11 is a block diagram of a modified example of the attitude control device according to the second embodiment. 各駆動部と対応軸との関係を示す図である。FIG. 4 is a diagram showing the relationship between each drive unit and a corresponding shaft. ＴＩＬＴ軸とＲＯＬＬ軸とＰＡＮ軸とで構成される３軸スタビライザの駆動部の配置関係が０°である場合を示す図である。FIG. 13 is a diagram showing a case where the arrangement relationship of the drive parts of a three-axis stabilizer composed of a tilt axis, a roll axis, and a pan axis is 0°. ＴＩＬＴ軸とＲＯＬＬ軸とＰＡＮ軸とで構成される３軸スタビライザの駆動部の配置関係が０°でない場合を示す図である。13 is a diagram showing a case where the arrangement relationship of the drive parts of a three-axis stabilizer composed of a tilt axis, a roll axis, and a pan axis is not 0°. FIG. 実施例３の姿勢制御装置のブロック図である。FIG. 11 is a block diagram of an attitude control device according to a third embodiment. 実施例３の姿勢制御装置の変形例のブロック図である。FIG. 13 is a block diagram of a modified example of the attitude control device of the third embodiment. 第２相対角度算出部のブロック図である。FIG. 4 is a block diagram of a second relative angle calculation unit. 実施例４の姿勢制御装置のブロック図である。FIG. 13 is a block diagram of an attitude control device according to a fourth embodiment. 実施例４の姿勢制御装置の変形例のブロック図である。FIG. 13 is a block diagram of a modified example of the attitude control device according to the fourth embodiment. 実施例５の姿勢制御装置のブロック図である。FIG. 13 is a block diagram of an attitude control device according to a fifth embodiment. 実施例５の姿勢制御装置の変形例のブロック図である。FIG. 13 is a block diagram of a modified example of the attitude control device according to the fifth embodiment. 実施例６の姿勢制御装置のブロック図である。FIG. 13 is a block diagram of an attitude control device according to a sixth embodiment. 実施例６の姿勢制御装置の変形例のブロック図である。FIG. 23 is a block diagram of a modified example of the attitude control device of the sixth embodiment.

以下、本発明の実施形態について、図面を参照しながら詳細に説明する。各図において、同一の部材については同一の参照番号を付し、重複する説明は省略する。 The following describes in detail an embodiment of the present invention with reference to the drawings. In each drawing, the same reference numbers are used for the same components, and duplicate descriptions are omitted.

各実施例の姿勢制御機器は、制御対象の姿勢制御を行う装置であって、例えば電動スタビライザであってもよいし、撮像装置に組み込まれる装置であってもよい。 The attitude control device in each embodiment is a device that controls the attitude of a control target, and may be, for example, an electric stabilizer or a device incorporated in an imaging device.

図１Ａは、本実施例の２軸の姿勢制御を行う姿勢制御機器（姿勢制御装置）のブロック図である。本実施例の姿勢制御機器は、第１駆動部１００、第２駆動部１４０、及び保持部３００を有する。保持部３００は、制御対象を保持すると共に、第１駆動部１００及び第２駆動部１４０により２軸方向へ回転可能に保持される。第１駆動部１００は、第１モータ１０２を用いて保持部３００を第１軸周りに回転させ、第２駆動部１４０は第２モータ１４２を用いて第１軸に直交する第２軸周りに回転させる。制御対象は例えば、撮像光学系により形成された被写体像を撮像する撮像手段である。第２駆動部１４０は、第１駆動部１００に対して、例えば図２乃至図４に示されるように配置されている。 Figure 1A is a block diagram of an attitude control device (attitude control device) that performs two-axis attitude control in this embodiment. The attitude control device in this embodiment has a first drive unit 100, a second drive unit 140, and a holding unit 300. The holding unit 300 holds the control target and is held rotatably in two axial directions by the first drive unit 100 and the second drive unit 140. The first drive unit 100 rotates the holding unit 300 around the first axis using the first motor 102, and the second drive unit 140 rotates the holding unit 300 around the second axis perpendicular to the first axis using the second motor 142. The control target is, for example, an imaging means that captures a subject image formed by an imaging optical system. The second drive unit 140 is arranged relative to the first drive unit 100, for example, as shown in Figures 2 to 4.

保持部３００は、保持部３００の角速度（第１角速度）及び加速度（第１加速度）を検出する第１ＩＭＵセンサ（第１検出部）３０１を有する。第１駆動部１００は、第１駆動回路１０１、第１モータ１０２、第１回転角センサ（回転角検知部）１０３、第１ＭＰＵ２００、及び第１メモリ２１５を有する。第２駆動部１４０は、第２駆動回路１４１、第２モータ１４２、及び第２回転角センサ１４３を有する。 The holding unit 300 has a first IMU sensor (first detection unit) 301 that detects the angular velocity (first angular velocity) and acceleration (first acceleration) of the holding unit 300. The first driving unit 100 has a first driving circuit 101, a first motor 102, a first rotation angle sensor (rotation angle detection unit) 103, a first MPU 200, and a first memory 215. The second driving unit 140 has a second driving circuit 141, a second motor 142, and a second rotation angle sensor 143.

第１ＭＰＵ２００は、第１駆動信号処理部２０１、第１姿勢推定部２０３、第１相対角度算出部２０４、第１加速度補正部２０５、第１角速度補正部２０６、角速度算出部２０７、第２姿勢推定部２０８、及び第２駆動信号処理部２４１を有する。第１ＭＰＵ２００は、第１メモリ２１５に記憶されているプログラムを実行することにより、姿勢制御機器の各部を制御する。 The first MPU 200 has a first drive signal processing unit 201, a first attitude estimation unit 203, a first relative angle calculation unit 204, a first acceleration correction unit 205, a first angular velocity correction unit 206, an angular velocity calculation unit 207, a second attitude estimation unit 208, and a second drive signal processing unit 241. The first MPU 200 controls each unit of the attitude control device by executing a program stored in the first memory 215.

第１姿勢推定部２０３は、第１ＩＭＵセンサ３０１からの角速度及び加速度を用いて保持部３００の姿勢を推定する。具体的には、第１姿勢推定部２０３は、第１ＩＭＵセンサ３０１からのＸＹＺ軸の各軸の角速度及び加速度を用いて、保持部３００の第１姿勢角である、第１ＴＩＬＴ角、第１ＲＯＬＬ角、及び第１ＰＡＮ角を算出する。 The first attitude estimation unit 203 estimates the attitude of the holding unit 300 using the angular velocity and acceleration from the first IMU sensor 301. Specifically, the first attitude estimation unit 203 calculates the first attitude angles of the holding unit 300, that is, the first tilt angle, the first roll angle, and the first pan angle, using the angular velocity and acceleration of each of the X, Y, and Z axes from the first IMU sensor 301.

第１駆動信号処理部２０１は、第１姿勢推定部２０３からの第１ＴＩＬＴ角とＴＩＬＴ軸目標角度との角度偏差、及び第１ＩＭＵセンサ３０１からのＸ軸角速度ωｘ１とＴＩＬＴ軸目標角速度との角速度偏差を取得する。第１駆動信号処理部２０１は、取得した角度偏差及び角速度偏差を用いたＰＩＤ制御等により取得される操作量と第１回転角センサ１０３により検知された第１モータ１０２の回転角に基づく第１モータ１０２への通電パターンとを第１駆動回路１０１に送信する。 The first drive signal processing unit 201 acquires the angle deviation between the first tilt angle and the tilt axis target angle from the first attitude estimation unit 203, and the angular velocity deviation between the X-axis angular velocity ωx1 and the tilt axis target angular velocity from the first IMU sensor 301. The first drive signal processing unit 201 transmits to the first drive circuit 101 the operation amount acquired by PID control or the like using the acquired angle deviation and angular velocity deviation, and the current supply pattern to the first motor 102 based on the rotation angle of the first motor 102 detected by the first rotation angle sensor 103.

第１駆動回路１０１は、第１駆動信号処理部２０１からの信号に応じて、第１モータ１０２を駆動する。 The first drive circuit 101 drives the first motor 102 in response to a signal from the first drive signal processing unit 201.

図５は、第１相対角度算出部２０４のブロック図である。第１相対角度算出部２０４は、第１姿勢角と第１回転角センサ１０３により検知された第１モータ１０２の回転角とを用いて第１駆動部１００と保持部３００との相対角度（第１相対角度）を算出する。具体的には、第１相対角度算出部２０４は、第１ＴＩＬＴ角と、第１回転角センサ１０３により検知された第１モータ１０２の回転角及び第１メモリ２１５に記録されている第１駆動部配置角２１６との差である第１角度偏差θｔｄｉｆ１を算出する。なお、第１駆動部配置角２１６は、第１駆動部１００に対する第２駆動部１４０の相対角である。 Figure 5 is a block diagram of the first relative angle calculation unit 204. The first relative angle calculation unit 204 calculates the relative angle (first relative angle) between the first drive unit 100 and the holding unit 300 using the first posture angle and the rotation angle of the first motor 102 detected by the first rotation angle sensor 103. Specifically, the first relative angle calculation unit 204 calculates a first angle deviation θtdif1, which is the difference between the first tilt angle, the rotation angle of the first motor 102 detected by the first rotation angle sensor 103, and the first drive unit arrangement angle 216 recorded in the first memory 215. Note that the first drive unit arrangement angle 216 is the relative angle of the second drive unit 140 with respect to the first drive unit 100.

第１加速度補正部２０５は、第１相対角度算出部２０４からの相対角度に基づいて保持部３００の加速度を補正することで第１補正加速度を算出する。図６は、第１加速度補正部２０５による加速度補正処理を示すフローチャートである。 The first acceleration correction unit 205 calculates the first corrected acceleration by correcting the acceleration of the holding unit 300 based on the relative angle from the first relative angle calculation unit 204. FIG. 6 is a flowchart showing the acceleration correction process performed by the first acceleration correction unit 205.

ステップＳ１０１では、第１加速度補正部２０５は、第１相対角度算出部２０４から第１角度偏差θｔｄｉｆ１を取得する。 In step S101, the first acceleration correction unit 205 acquires the first angle deviation θtdif1 from the first relative angle calculation unit 204.

ステップＳ１０２では、第１加速度補正部２０５は、第１姿勢推定部２０３から保持部３００の第１姿勢角として第１ＲＯＬＬ角及び第１ＰＡＮ角を取得する。 In step S102, the first acceleration correction unit 205 acquires the first ROLL angle and the first PAN angle as the first attitude angle of the holding unit 300 from the first attitude estimation unit 203.

なお、ステップＳ１０１とステップＳ１０２の処理の順番は入れ替えてもよい。 The order of steps S101 and S102 may be reversed.

ステップＳ１０３では、第１加速度補正部２０５は、第１角度偏差θｔｄｉｆ１と第１ＲＯＬＬ角及び第１ＰＡＮ角とを用いて、以下の式（１）で表されるＺ軸方向にのみ重力加速度が発生している基準加速度ベクトルαｂａｓｅをセットする。 In step S103, the first acceleration correction unit 205 uses the first angle deviation θtdif1, the first ROLL angle, and the first PAN angle to set a reference acceleration vector αbase in which gravitational acceleration occurs only in the Z-axis direction, as expressed by the following equation (1):

ステップＳ１０４では、第１加速度補正部２０５は、基準加速度ベクトルαｂａｓｅを、式（２）を用いて第１角度偏差だけ３次元回転させることで第１変形値α’ｂａｓｅを算出する。 In step S104, the first acceleration correction unit 205 calculates the first transformed value α'base by three-dimensionally rotating the reference acceleration vector αbase by the first angle deviation using equation (2).

ステップＳ１０５では、第１加速度補正部２０５は、第１変形値α’ｂａｓｅを、式（３）を用いて第１ＲＯＬＬ角だけ３次元回転させることで第２変形値α”ｂａｓｅを算出する。 In step S105, the first acceleration correction unit 205 calculates the second transformed value α”base by three-dimensionally rotating the first transformed value α’base by the first ROLL angle using equation (3).

ステップＳ１０６では、第１加速度補正部２０５は、第２変形値α”ｂａｓｅを、式（４）を用いてＰＡＮ角だけ３次元回転させることで第１加速度補正値（第１補正加速度）を算出する。 In step S106, the first acceleration correction unit 205 calculates the first acceleration correction value (first corrected acceleration) by three-dimensionally rotating the second transformed value α″base by the PAN angle using equation (4).

第１角速度補正部２０６は、第１相対角度算出部２０４からの相対角度に基づいて保持部３００の角速度を補正することで第１補正角速度を算出する。図７は、第１角速度補正部２０６による角速度補正処理を示すフローチャートである。 The first angular velocity correction unit 206 calculates the first corrected angular velocity by correcting the angular velocity of the holding unit 300 based on the relative angle from the first relative angle calculation unit 204. FIG. 7 is a flowchart showing the angular velocity correction process performed by the first angular velocity correction unit 206.

ステップＳ２０１では、第１角速度補正部２０６は、第１相対角度算出部２０４から第１角度偏差θｔｄｉｆ１を取得する。 In step S201, the first angular velocity correction unit 206 acquires the first angle deviation θtdif1 from the first relative angle calculation unit 204.

ステップＳ２０２では、第１角速度補正部２０６は、第１ＩＭＵセンサ３０１から保持部３００のＹ軸角速度ωｙ１及びＺ軸角速度ωｚ１を取得する。 In step S202, the first angular velocity correction unit 206 acquires the Y-axis angular velocity ωy1 and the Z-axis angular velocity ωz1 of the holding unit 300 from the first IMU sensor 301.

ステップＳ２０３では、第１角速度補正部２０６は、以下の式（５ａ）及び式（５ｂ）を用いて、第１角度偏差θｔｄｉｆ１及びＹ軸角速度ωｙ１から第１Ｙ軸角速度補正値ωｙｃｒｒ１及び第１Ｚ軸角速度補正値ωｚｃｃｒ１を算出する。 In step S203, the first angular velocity correction unit 206 calculates the first Y-axis angular velocity correction value ωycrr1 and the first Z-axis angular velocity correction value ωzccr1 from the first angle deviation θtdif1 and the Y-axis angular velocity ωy1 using the following equations (5a) and (5b).

ステップＳ２０４では、第１角速度補正部２０６は、以下の式（６ａ）及び式（６ｂ）を用いて、第１角度偏差θｔｄｉｆ１及びＺ軸角速度ωｚ１から第２Ｙ軸角速度補正値ωｙｃｒｒ２及び第２Ｚ軸角速度補正値ωｚｃｃｒ２を算出する。 In step S204, the first angular velocity correction unit 206 calculates the second Y-axis angular velocity correction value ωycrr2 and the second Z-axis angular velocity correction value ωzccr2 from the first angle deviation θtdif1 and the Z-axis angular velocity ωz1 using the following equations (6a) and (6b).

ステップＳ２０５では、第１角速度補正部２０６は、第１Ｙ軸角速度補正値ωｙｃｒｒ１と第２Ｙ軸角速度補正値ωｙｃｒｒ２との差分値であるＹ軸角速度補正値（第１補正角速度）を算出する。 In step S205, the first angular velocity correction unit 206 calculates a Y-axis angular velocity correction value (first corrected angular velocity) which is the difference between the first Y-axis angular velocity correction value ωycrr1 and the second Y-axis angular velocity correction value ωycrr2.

ステップＳ２０６では、第１角速度補正部２０６は、第１Ｚ軸角速度補正値ωｚｃｒｒ１と第２Ｚ軸角速度補正値ωｚｃｒｒ２との差分値であるＺ軸角速度補正値（第１補正角速度）を算出する。 In step S206, the first angular velocity correction unit 206 calculates a Z-axis angular velocity correction value (first correction angular velocity) which is the difference between the first Z-axis angular velocity correction value ωzcrr1 and the second Z-axis angular velocity correction value ωzcrr2.

角速度算出部２０７は、第１回転角センサ１０３により検知された第１モータ１０２の回転角と、第１ＭＰＵ２００のサンプリング周期又は内部クロックとを用いて第１駆動部１００の角速度を算出する。具体的には、角速度算出部２０７は、第１モータ１０２の回転角を単位時間で微分することで、第１駆動部１００の角速度を算出する。 The angular velocity calculation unit 207 calculates the angular velocity of the first drive unit 100 using the rotation angle of the first motor 102 detected by the first rotation angle sensor 103 and the sampling period or the internal clock of the first MPU 200. Specifically, the angular velocity calculation unit 207 calculates the angular velocity of the first drive unit 100 by differentiating the rotation angle of the first motor 102 by unit time.

第２姿勢推定部２０８は、第１加速度補正部２０５からの第１加速度補正値と、第１角速度補正部２０６からのＹ軸及びＺ軸角速度補正値と、角速度算出部２０７からの第１駆動部１００の角速度とを用いて、保持部３００の姿勢を推定する。具体的には、第２姿勢推定部２０８は、保持部３００の第２姿勢角である、第２ＴＩＬＴ角、第２ＲＯＬＬ角及び第２ＰＡＮ角を算出する。 The second attitude estimation unit 208 estimates the attitude of the holding unit 300 using the first acceleration correction value from the first acceleration correction unit 205, the Y-axis and Z-axis angular velocity correction values from the first angular velocity correction unit 206, and the angular velocity of the first driving unit 100 from the angular velocity calculation unit 207. Specifically, the second attitude estimation unit 208 calculates the second attitude angles of the holding unit 300, that is, the second tilt angle, the second roll angle, and the second pan angle.

第２駆動信号処理部２４１は、第２駆動部１４０がＲＯＬＬ軸に配置されている場合、第２ＲＯＬＬ角とＲＯＬＬ軸目標角度との角度偏差、及びＹ軸角速度補正値とＲＯＬＬ軸目標角速度との角速度偏差を算出する。また、第２駆動信号処理部２４１は、第２駆動部１４０がＰＡＮ軸に配置されている場合、第２ＰＡＮ角とＰＡＮ軸目標角度との角度偏差、及びＺ軸角速度補正値とＰＡＮ軸目標角速度との角速度偏差を算出する。第２駆動信号処理部２４１は、取得した角度偏差及び角速度偏差を用いたＰＩＤ制御等により取得される操作量と、第２回転角センサ１４３により検知された第２モータ１４２の回転角に基づく第２モータ１４２への通電パターンとを第２駆動回路１４１に送信する。 When the second drive unit 140 is arranged on the ROLL axis, the second drive signal processing unit 241 calculates the angle deviation between the second ROLL angle and the ROLL axis target angle, and the angular velocity deviation between the Y-axis angular velocity correction value and the ROLL axis target angular velocity. When the second drive unit 140 is arranged on the PAN axis, the second drive signal processing unit 241 calculates the angle deviation between the second PAN angle and the PAN axis target angle, and the angular velocity deviation between the Z-axis angular velocity correction value and the PAN axis target angular velocity. The second drive signal processing unit 241 transmits to the second drive circuit 141 the operation amount acquired by PID control or the like using the acquired angle deviation and angular velocity deviation, and the current supply pattern to the second motor 142 based on the rotation angle of the second motor 142 detected by the second rotation angle sensor 143.

第２駆動回路１４１は、第２駆動信号処理部２４１からの信号に応じて、第２モータ１４２を駆動する。 The second drive circuit 141 drives the second motor 142 in response to a signal from the second drive signal processing unit 241.

以上説明したように、本実施例では、第１駆動部１００は第１姿勢推定部２０３からの保持部３００の第１姿勢角で姿勢制御を行い、第２駆動部１４０は第２姿勢推定部２０８からの保持部３００の第２姿勢角で姿勢制御を行う。 As described above, in this embodiment, the first drive unit 100 performs attitude control at the first attitude angle of the holding unit 300 from the first attitude estimation unit 203, and the second drive unit 140 performs attitude control at the second attitude angle of the holding unit 300 from the second attitude estimation unit 208.

本実施例の構成によれば、加速度及び角速度を補正することで得られる加速度補正値及び角速度補正値を用いて姿勢推定を行うことで得られる姿勢角と、角速度補正値とをＴＩＬＴ軸以外のフィードバック制御に用いる。これにより、持ち手の姿勢に対応可能となり、制御対応軸が変化しても安定した姿勢制御を行うことができる。また、補正後の姿勢情報の精度を向上させることが可能であると共に、少ない通信情報や時間で補正を行うことが可能である。 According to the configuration of this embodiment, the attitude angle and angular velocity correction value obtained by performing attitude estimation using the acceleration correction value and angular velocity correction value obtained by correcting the acceleration and angular velocity are used for feedback control other than the tilt axis. This makes it possible to respond to the attitude of the holder, and stable attitude control can be performed even if the control corresponding axis changes. In addition, it is possible to improve the accuracy of the attitude information after correction, and correction can be performed with less communication information and time.

以下、図１Ｂを参照して、本実施例の姿勢制御装置の変形例について説明する。図１Ｂは、本実施例の姿勢制御装置の変形例のブロック図である。 Below, a modified example of the attitude control device of this embodiment will be described with reference to FIG. 1B. FIG. 1B is a block diagram of a modified example of the attitude control device of this embodiment.

変形例の姿勢制御機器は、本実施例の構成に加え、第１駆動部１００、第２駆動部１４０、及び保持部３００を支持する支持部４００を有する。支持部４００は、支持部４００の角速度（第２角速度）及び加速度（第２加速度）を検出する第２ＩＭＵセンサ（第２検出部）４０１を有する。 In addition to the configuration of this embodiment, the attitude control device of the modified example has a support unit 400 that supports the first drive unit 100, the second drive unit 140, and the holding unit 300. The support unit 400 has a second IMU sensor (second detection unit) 401 that detects the angular velocity (second angular velocity) and acceleration (second acceleration) of the support unit 400.

第１駆動部１００は、本実施例の構成に加え、第４姿勢推定部（支持部姿勢推定部）２１４を有する。第４姿勢推定部２１４は、第２ＩＭＵセンサ４０１からの角速度及び加速度を用いて支持部４００の姿勢を推定する。具体的には、第４姿勢推定部２１４は、第２ＩＭＵセンサ４０１からのＸＹＺ軸の各軸の角速度及び加速度を用いて、支持部４００の姿勢角である、第４ＴＩＬＴ角、第４ＲＯＬＬ角、及び第４ＰＡＮ角を算出する。第１相対角度算出部２０４は、第１角度偏差θｔｄｉｆ１として、第１姿勢推定部２０３からの第１ＴＩＬＴ角と第４姿勢推定部２１４からの第４ＴＩＬＴ角との角度偏差を算出してもよい。角速度算出部２０７は、第２ＩＭＵセンサ４０１のデータを用いて第１駆動部１００の角速度を検知してもよい。 The first driving unit 100 has a fourth attitude estimation unit (support unit attitude estimation unit) 214 in addition to the configuration of this embodiment. The fourth attitude estimation unit 214 estimates the attitude of the support unit 400 using the angular velocity and acceleration from the second IMU sensor 401. Specifically, the fourth attitude estimation unit 214 calculates the fourth tilt angle, fourth roll angle, and fourth pan angle, which are attitude angles of the support unit 400, using the angular velocity and acceleration of each axis of the X, Y, and Z axes from the second IMU sensor 401. The first relative angle calculation unit 204 may calculate the angle deviation between the first tilt angle from the first attitude estimation unit 203 and the fourth tilt angle from the fourth attitude estimation unit 214 as the first angle deviation θtdif1. The angular velocity calculation unit 207 may detect the angular velocity of the first driving unit 100 using the data of the second IMU sensor 401.

図８Ａは、本実施例の３軸の姿勢制御を行う姿勢制御装置のブロック図である。図９は、各駆動部と対応軸との関係を示す図である。本実施例では、実施例１と異なる構成について説明し、実施例１と同様の構成については同一の参照番号を付し、詳細な説明は省略する。 Figure 8A is a block diagram of the attitude control device that performs three-axis attitude control in this embodiment. Figure 9 is a diagram showing the relationship between each drive unit and the corresponding axis. In this embodiment, configurations different from those in embodiment 1 are described, and configurations similar to those in embodiment 1 are given the same reference numbers and detailed descriptions are omitted.

本実施例の姿勢制御機器は、実施例１の構成に加え、第３駆動部１７０を有する。保持部３００は、第１駆動部１００、第２駆動部１４０、及び第３駆動部１７０により３軸方向へ回転可能に保持される。第３駆動部１７０は、第１軸と第２軸とに直交する第３軸周りに保持部３００を回転させる。第２駆動部１４０及び第３駆動部１７０は、第１駆動部１００に対して、例えば図１０や図１１に示されるように配置されている。第３駆動部１７０は、第３駆動回路１７１、第３モータ１７２、及び第３回転角センサ１７３を有する。 The attitude control device of this embodiment has a third drive unit 170 in addition to the configuration of the first embodiment. The holding unit 300 is held rotatably in three axial directions by the first drive unit 100, the second drive unit 140, and the third drive unit 170. The third drive unit 170 rotates the holding unit 300 around a third axis perpendicular to the first axis and the second axis. The second drive unit 140 and the third drive unit 170 are arranged relative to the first drive unit 100, for example, as shown in Figures 10 and 11. The third drive unit 170 has a third drive circuit 171, a third motor 172, and a third rotation angle sensor 173.

第１ＭＰＵ２００は、実施例１の構成に加え、第３駆動信号処理部２７１を有する。第３駆動信号処理部２７１は、第２ＰＡＮ角とＰＡＮ軸目標角度との角度偏差、及びＺ軸角速度補正値とＰＡＮ軸目標角速度との角速度偏差を算出する。第３駆動信号処理部２７１は、取得した角度偏差及び角速度偏差を用いたＰＩＤ制御等により操作量と、第３回転角センサ１７３により検知された第３モータ１７２の回転角に基づく第３モータ１７２への通電パターンとを第３駆動回路１７１に送信する。 The first MPU 200 has a third drive signal processor 271 in addition to the configuration of the first embodiment. The third drive signal processor 271 calculates the angle deviation between the second PAN angle and the PAN axis target angle, and the angular velocity deviation between the Z-axis angular velocity correction value and the PAN axis target angular velocity. The third drive signal processor 271 transmits to the third drive circuit 171 the operation amount by PID control or the like using the acquired angle deviation and angular velocity deviation, and the current supply pattern to the third motor 172 based on the rotation angle of the third motor 172 detected by the third rotation angle sensor 173.

第３駆動回路１７１は、第３駆動信号処理部２７１からの信号に応じて、第３モータ１７２を駆動する。 The third drive circuit 171 drives the third motor 172 in response to a signal from the third drive signal processing unit 271.

以上説明したように、本実施例では、第３駆動部１７０は、第２姿勢推定部２０８からの保持部３００の第２姿勢角で姿勢制御を行う。 As described above, in this embodiment, the third drive unit 170 performs attitude control at the second attitude angle of the holding unit 300 from the second attitude estimation unit 208.

以上説明したように、本実施例の構成によれば、加速度及び角速度を補正することで得られる加速度補正値及び角速度補正値を用いて姿勢推定を行うことで得られる姿勢角と、角速度補正値とをＴＩＬＴ軸以外のフィードバック制御に用いる。これにより、持ち手の姿勢に対応可能となり、制御対応軸が変化しても安定した姿勢制御を行うことができる。また、補正後の姿勢情報の精度を向上させることが可能であると共に、少ない通信情報や時間で補正を行うことが可能である。 As described above, according to the configuration of this embodiment, the attitude angle and angular velocity correction value obtained by performing attitude estimation using the acceleration correction value and angular velocity correction value obtained by correcting the acceleration and angular velocity are used for feedback control other than the tilt axis. This makes it possible to respond to the attitude of the holder, and stable attitude control can be performed even if the control corresponding axis changes. In addition, it is possible to improve the accuracy of the attitude information after correction, and correction can be performed with less communication information and time.

なお、第２駆動部１４０がＰＡＮ軸に、第３駆動部１７０がＲＯＬＬ軸に構成される場合であっても、第２駆動信号処理部２４１と第３駆動信号処理部２７１に入力する軸のデータを変更することで、本実施例の構成を適用できるのは言うまでもない。 It goes without saying that even if the second drive unit 140 is configured as the PAN axis and the third drive unit 170 is configured as the ROLL axis, the configuration of this embodiment can be applied by changing the axis data input to the second drive signal processing unit 241 and the third drive signal processing unit 271.

以下、図８Ｂを参照して、本実施例の姿勢制御装置の変形例について説明する。図８Ｂは、本実施例の姿勢制御装置の変形例のブロック図である。 Below, a modified example of the attitude control device of this embodiment will be described with reference to FIG. 8B. FIG. 8B is a block diagram of a modified example of the attitude control device of this embodiment.

変形例の姿勢制御機器は、本実施例の構成に加え、第１駆動部１００、第２駆動部１４０、及び保持部３００を支持する支持部４００を有する。支持部４００は、支持部４００の角速度及び加速度を検出する第２ＩＭＵセンサ４０１を有する。 The attitude control device of the modified example has, in addition to the configuration of this embodiment, a support unit 400 that supports the first drive unit 100, the second drive unit 140, and the holding unit 300. The support unit 400 has a second IMU sensor 401 that detects the angular velocity and acceleration of the support unit 400.

第１駆動部１００は、本実施例の構成に加え、第４姿勢推定部２１４を有する。第４姿勢推定部２１４は、第２ＩＭＵセンサ４０１からの角速度及び加速度を用いて支持部４００の姿勢を推定する。具体的には、第４姿勢推定部２１４は、第２ＩＭＵセンサ４０１からのＸＹＺ軸の各軸の角速度及び加速度を用いて、支持部４００の姿勢角である、第４ＴＩＬＴ角、第４ＲＯＬＬ角、及び第４ＰＡＮ角を算出する。第１相対角度算出部２０４は、第１角度偏差θｔｄｉｆ１として、第１姿勢推定部２０３からの第１ＴＩＬＴ角と第４姿勢推定部２１４からの第４ＴＩＬＴ角との角度偏差を算出してもよい。角速度算出部２０７は、第２ＩＭＵセンサ４０１のデータを用いて第１駆動部１００の角速度を検知してもよい。 The first driving unit 100 has a fourth attitude estimation unit 214 in addition to the configuration of this embodiment. The fourth attitude estimation unit 214 estimates the attitude of the support unit 400 using the angular velocity and acceleration from the second IMU sensor 401. Specifically, the fourth attitude estimation unit 214 calculates the fourth tilt angle, fourth roll angle, and fourth pan angle, which are attitude angles of the support unit 400, using the angular velocity and acceleration of each axis of the X, Y, and Z axes from the second IMU sensor 401. The first relative angle calculation unit 204 may calculate the angle deviation between the first tilt angle from the first attitude estimation unit 203 and the fourth tilt angle from the fourth attitude estimation unit 214 as the first angle deviation θtdif1. The angular velocity calculation unit 207 may detect the angular velocity of the first driving unit 100 using the data of the second IMU sensor 401.

図１２Ａは、本実施例の３軸の姿勢制御を行う姿勢制御装置のブロック図である。本実施例では、実施例１と異なる構成について説明し、実施例１と同様の構成については同一の参照番号を付し、詳細な説明は省略する。 Figure 12A is a block diagram of the attitude control device that performs three-axis attitude control in this embodiment. In this embodiment, configurations different from those in embodiment 1 will be described, and configurations similar to those in embodiment 1 will be given the same reference numbers and detailed descriptions will be omitted.

第１ＭＰＵ２００は、実施例１の構成に加え、第２相対角度算出部（第１相対角度算出部）２０９、第２加速度補正部２１０、第２角速度補正部２１１、第３姿勢推定部２１２、及び第３駆動信号処理部２７１を有する。 In addition to the configuration of the first embodiment, the first MPU 200 has a second relative angle calculation unit (first relative angle calculation unit) 209, a second acceleration correction unit 210, a second angular velocity correction unit 211, a third attitude estimation unit 212, and a third drive signal processing unit 271.

図１３は、第２相対角度算出部２０９のブロック図である。第２相対角度算出部２０９は、第１姿勢角と第１モータ１０２の回転角とを用いて第１駆動部１００と保持部３００との相対角度（第２相対角度（第１相対角度））を算出する。具体的には、第２相対角度算出部２０９は、第１ＴＩＬＴ角と、第１モータ１０２の回転角及び第１メモリ２１５に記録されている第２駆動部配置角２１７との差分である第２角度偏差θｔｄｉｆ２を算出する。第２駆動部配置角２１７は、第１駆動部１００に対する第３駆動部１７０の相対角である。 13 is a block diagram of the second relative angle calculation unit 209. The second relative angle calculation unit 209 calculates the relative angle (second relative angle (first relative angle) ) between the first driving unit 100 and the holding unit 300 using the first attitude angle and the rotation angle of the first motor 102. Specifically, the second relative angle calculation unit 209 calculates a second angle deviation θtdif2 which is the difference between the first tilt angle, the rotation angle of the first motor 102, and the second driving unit arrangement angle 217 recorded in the first memory 215. The second driving unit arrangement angle 217 is the relative angle of the third driving unit 170 with respect to the first driving unit 100.

第２加速度補正部２１０は、第２相対角度算出部２０９からの相対角度に基づいて保持部３００の加速度を補正することで第２補正加速度を算出する。具体的には、第２加速度補正部２１０は、まず、第２相対角度算出部２０９からの第２角度偏差θｔｄｉｆ２と第１ＲＯＬＬ角及び第１ＰＡＮ角とを用いて、式（１）で表されるＺ軸方向にのみ重力加速度が発生している基準加速度ベクトルαｂａｓｅをセットする。次に、第２加速度補正部２１０は、基準加速度ベクトルαｂａｓｅを、式（２）を用いて第２角度偏差θｔｄｉｆ２だけ３次元回転させることで第１変形値α'ｂａｓｅ２を算出する。次に、第２加速度補正部２０５は、第１変形値α'ｂａｓｅ２を、式（３）を用いて第１ＲＯＬＬ角だけ３次元回転させることで第２変形値α”ｂａｓｅ２を算出する。最後に、第２加速度補正部２０５は、第２変形値α”ｂａｓｅ２を、式（４）を用いてＰＡＮ角だけ３次元回転させることで第２加速度補正値を算出する。 The second acceleration correction unit 210 calculates the second corrected acceleration by correcting the acceleration of the holding unit 300 based on the relative angle from the second relative angle calculation unit 209. Specifically, the second acceleration correction unit 210 first sets a reference acceleration vector αbase in which gravitational acceleration occurs only in the Z-axis direction represented by equation (1) using the second angle deviation θtdif2 from the second relative angle calculation unit 209, the first ROLL angle, and the first PAN angle. Next, the second acceleration correction unit 210 calculates the first deformation value α'base2 by three-dimensionally rotating the reference acceleration vector αbase by the second angle deviation θtdif2 using equation (2). Next, the second acceleration correction unit 205 calculates the second transformed value α″base2 by three-dimensionally rotating the first transformed value α'base2 by the first ROLL angle using equation (3). Finally, the second acceleration correction unit 205 calculates the second acceleration correction value by three-dimensionally rotating the second transformed value α″base2 by the PAN angle using equation (4).

第２角速度補正部２１１は、第２相対角度算出部２０９からの相対角度に基づいて保持部３００の角速度を補正することで第２補正角速度を算出する。具体的には、第２角速度補正部２１１は、まず、以下の式（７ａ）及び式（７ｂ）を用いて、第２角度偏差θｔｄｉｆ２及びＹ軸角速度ωｙ１から第１Ｙ軸角速度補正値ωｙｃｒｒ３及び第１Ｚ軸角速度補正値ωｚｃｒｒ３を算出する。 The second angular velocity correction unit 211 calculates the second corrected angular velocity by correcting the angular velocity of the holding unit 300 based on the relative angle from the second relative angle calculation unit 209. Specifically, the second angular velocity correction unit 211 first calculates the first Y-axis angular velocity correction value ωycrr3 and the first Z-axis angular velocity correction value ωzcrr3 from the second angle deviation θtdif2 and the Y-axis angular velocity ωy1 using the following equations (7a) and (7b).

次に、第２角速度補正部２１１は、以下の式（８ａ）及び式（８ｂ）を用いて、第２角度偏差θｔｄｉｆ２及びＺ軸角速度ωｚ１から第２Ｙ軸角速度補正値ωｙｃｒｒ４及び第２Ｚ軸角速度補正値ωｚｃｃｒ４を算出する。 Next, the second angular velocity correction unit 211 calculates the second Y-axis angular velocity correction value ωycrr4 and the second Z-axis angular velocity correction value ωzccr4 from the second angle deviation θtdif2 and the Z-axis angular velocity ωz1 using the following equations (8a) and (8b).

最後に、第２角速度補正部２１１は、第１Ｙ軸角速度補正値ωｙｃｒｒ３と第２Ｙ軸角速度補正値ωｙｃｒｒ４との差分値であるＹ軸角速度補正値（第２補正角速度）を算出する。また、第２角速度補正部２１１は、第１Ｚ軸角速度補正値ωｚｃｒｒ３と第２Ｚ軸角速度補正値ωｚｃｒｒ４との差分値であるＺ軸角速度補正値（第２補正角速度）を算出する。 Finally, the second angular velocity correction unit 211 calculates a Y-axis angular velocity correction value (second corrected angular velocity) which is the difference between the first Y-axis angular velocity correction value ωycrr3 and the second Y-axis angular velocity correction value ωycrr4. The second angular velocity correction unit 211 also calculates a Z-axis angular velocity correction value (second corrected angular velocity) which is the difference between the first Z-axis angular velocity correction value ωzcrr3 and the second Z-axis angular velocity correction value ωzcrr4.

第３姿勢推定部２１２は、第２加速度補正部２１０からの第２加速度補正値と、第２角速度補正部２１１からのＹ軸及びＺ軸角速度補正値と、角速度算出部２０７からの第１駆動部１００の角速度とを用いて、保持部３００の姿勢を推定する。具体的には、第３姿勢推定部２１２は、保持部３００の第３姿勢角である、第３ＴＩＬＴ角、第３ＲＯＬＬ角及び第３ＰＡＮ角を算出する。 The third attitude estimation unit 212 estimates the attitude of the holding unit 300 using the second acceleration correction value from the second acceleration correction unit 210, the Y-axis and Z-axis angular velocity correction values from the second angular velocity correction unit 211, and the angular velocity of the first driving unit 100 from the angular velocity calculation unit 207. Specifically, the third attitude estimation unit 212 calculates the third attitude angles of the holding unit 300, that is, the third tilt angle, the third roll angle, and the third pan angle.

第３駆動信号処理部２７１は、第２ＰＡＮ角とＰＡＮ軸目標角度との角度偏差、及びＺ軸角速度補正値とＰＡＮ軸目標角速度との角速度偏差を算出する。第３駆動信号処理部２７１は、取得した角度偏差及び角速度偏差を用いたＰＩＤ制御等により操作量と、第３回転角センサ１７３により検知された第３モータ１７２の回転角に基づく第３モータ１７２への通電パターンとを第３駆動回路１７１に送信する。 The third drive signal processor 271 calculates the angle deviation between the second PAN angle and the PAN axis target angle, and the angular velocity deviation between the Z-axis angular velocity correction value and the PAN axis target angular velocity. The third drive signal processor 271 transmits to the third drive circuit 171 the operation amount by PID control or the like using the acquired angle deviation and angular velocity deviation, and the current supply pattern to the third motor 172 based on the rotation angle of the third motor 172 detected by the third rotation angle sensor 173.

以上説明したように、本実施例では、第３駆動部１７０は、第３姿勢推定部２１２からの保持部３００の第３姿勢角で姿勢制御を行う。 As described above, in this embodiment, the third drive unit 170 performs attitude control at the third attitude angle of the holding unit 300 from the third attitude estimation unit 212.

以下、図１２Ｂを参照して、本実施例の姿勢制御装置の変形例について説明する。図１２Ｂは、本実施例の姿勢制御装置の変形例のブロック図である。 Below, a modified example of the attitude control device of this embodiment will be described with reference to FIG. 12B. FIG. 12B is a block diagram of a modified example of the attitude control device of this embodiment.

図１４Ａは、本実施例の２軸の姿勢制御を行う姿勢制御機器のブロック図である。本実施例では、実施例１と異なる構成について説明し、実施例１と同様の構成については同一の参照番号を付し、詳細な説明は省略する。 Figure 14A is a block diagram of the attitude control device that performs two-axis attitude control in this embodiment. In this embodiment, configurations different from those in embodiment 1 are described, and configurations similar to those in embodiment 1 are given the same reference numbers and detailed descriptions are omitted.

第１ＭＰＵ２００は、実施例１の構成に加え、第１インターフェース２１３を有する。また、第１ＭＰＵ２００は、実施例１の構成とは異なり、第２駆動信号処理部２４１を有さない。 The first MPU 200 has a first interface 213 in addition to the configuration of the first embodiment. Also, unlike the configuration of the first embodiment, the first MPU 200 does not have a second drive signal processing unit 241.

第２駆動部１４０は、実施例１の構成に加え、第２ＭＰＵ２４０及び第２メモリ２４５を有する。第２ＭＰＵ２４０は、第２駆動信号処理部２４１、及び第２インターフェース２４３を有し、第２メモリ２４５に記憶されているプログラムを実行することにより、姿勢制御機器の各部を制御する。 The second drive unit 140 has a second MPU 240 and a second memory 245 in addition to the configuration of the first embodiment. The second MPU 240 has a second drive signal processing unit 241 and a second interface 243, and controls each part of the attitude control device by executing a program stored in the second memory 245.

第１インターフェース２１３は、第２姿勢推定部２０８からの第２ＲＯＬＬ角及び第２ＰＡＮ角と、第１角速度補正部２０６からのＹ軸及びＺ軸角速度補正値をパケットに格納し、第２インターフェース２４３と通信を行う。なお、第２駆動部１４０がＲＯＬＬ軸に配置されている場合、第２ＲＯＬＬ角及びＹ軸角速度補正値のみをパケットに格納してもよいし、第２駆動部１４０がＰＡＮ軸に配置されている場合、第２ＰＡＮ角及びＺ軸角速度補正値のみをパケットに格納してもよい。また、第１インターフェース２１３と第２インターフェース２４３との通信方式は任意の通信方式でよく、シリアル通信等に限定されない。 The first interface 213 stores the second ROLL angle and second PAN angle from the second attitude estimation unit 208 and the Y-axis and Z-axis angular velocity correction values from the first angular velocity correction unit 206 in a packet, and communicates with the second interface 243. Note that if the second drive unit 140 is arranged on the ROLL axis, only the second ROLL angle and the Y-axis angular velocity correction value may be stored in the packet, and if the second drive unit 140 is arranged on the PAN axis, only the second PAN angle and the Z-axis angular velocity correction value may be stored in the packet. Also, the communication method between the first interface 213 and the second interface 243 may be any communication method and is not limited to serial communication, etc.

第２インターフェース２４３は、第１インターフェース２１３との通信により姿勢角情報と角速度を取得する。第２駆動信号処理部２４１は、第２駆動部１４０がＲＯＬＬ軸に配置されている場合、第１インターフェース２１３から第２ＲＯＬＬ角とＹ軸角速度補正値とを取得する。そして、第２駆動信号処理部２４１は、第２ＲＯＬＬ角とＲＯＬＬ軸目標角度との角度偏差、及びＹ軸角速度補正値とＲＯＬＬ軸目標角速度との角速度偏差を算出する。また、第２駆動信号処理部２４１は、第２駆動部１４０がＰＡＮ軸に配置されている場合、第１インターフェース２１３から第２ＰＡＮ角とＺ軸角速度補正値とを取得する。そして、第２駆動信号処理部２４１は、第２ＰＡＮ角とＰＡＮ軸目標角度との角度偏差、及びＺ軸角速度補正値とＰＡＮ軸目標角速度との角速度偏差を算出する。第２駆動信号処理部２４１は、取得した角度偏差及び角速度偏差を用いたＰＩＤ制御等により取得される操作量と、第２回転角センサ１４３により検知された第２モータ１４２の回転角に基づく第２モータ１４２への通電パターンとを第２駆動回路１４１に送信する。 The second interface 243 acquires posture angle information and angular velocity through communication with the first interface 213. When the second drive unit 140 is arranged on the ROLL axis, the second drive signal processing unit 241 acquires the second ROLL angle and the Y-axis angular velocity correction value from the first interface 213. Then, the second drive signal processing unit 241 calculates the angle deviation between the second ROLL angle and the ROLL axis target angle, and the angular velocity deviation between the Y-axis angular velocity correction value and the ROLL axis target angular velocity. Also, when the second drive unit 140 is arranged on the PAN axis, the second drive signal processing unit 241 acquires the second PAN angle and the Z-axis angular velocity correction value from the first interface 213. Then, the second drive signal processing unit 241 calculates the angle deviation between the second PAN angle and the PAN axis target angle, and the angular velocity deviation between the Z-axis angular velocity correction value and the PAN axis target angular velocity. The second drive signal processing unit 241 transmits to the second drive circuit 141 the operation amount obtained by PID control using the obtained angle deviation and angular velocity deviation, and the current supply pattern to the second motor 142 based on the rotation angle of the second motor 142 detected by the second rotation angle sensor 143.

以下、図１４Ｂを参照して、本実施例の姿勢制御装置の変形例について説明する。図１４Ｂは、本実施例の姿勢制御装置の変形例のブロック図である。 Below, a modified example of the attitude control device of this embodiment will be described with reference to FIG. 14B. FIG. 14B is a block diagram of a modified example of the attitude control device of this embodiment.

図１５Ａは、本実施例の３軸の姿勢制御を行う姿勢制御機器のブロック図である。本実施例では、実施例２と異なる構成について説明し、実施例２と同様の構成については同一の参照番号を付し、詳細な説明は省略する。 Figure 15A is a block diagram of the attitude control device that performs three-axis attitude control in this embodiment. In this embodiment, configurations different from those in embodiment 2 are described, and configurations similar to those in embodiment 2 are given the same reference numbers and detailed descriptions are omitted.

第１ＭＰＵ２００は、実施例２の構成に加え、第１インターフェース２１３を有する。また、第１ＭＰＵ２００は、実施例２の構成とは異なり、第２駆動信号処理部２４１及び第３駆動信号処理部２７１を有さない。 The first MPU 200 has a first interface 213 in addition to the configuration of the second embodiment. Also, unlike the configuration of the second embodiment, the first MPU 200 does not have a second drive signal processing unit 241 and a third drive signal processing unit 271.

第２駆動部１４０は、実施例２の構成に加え、第２ＭＰＵ２４０及び第２メモリ２４５を有する。第２ＭＰＵ２４０は、第２駆動信号処理部２４１、及び第２インターフェース２４３を有し、第２メモリ２４５に記憶されているプログラムを実行することにより、姿勢制御機器の各部を制御する。 The second drive unit 140 has a second MPU 240 and a second memory 245 in addition to the configuration of the second embodiment. The second MPU 240 has a second drive signal processing unit 241 and a second interface 243, and controls each part of the attitude control device by executing a program stored in the second memory 245.

第３駆動部１７０は、実施例２の構成に加え、第３ＭＰＵ２７０及び第３メモリ２７５を有する。第３ＭＰＵ２７０は、第３駆動信号処理部２７１及び第３インターフェース２７３を有し、第３メモリ２７５に記憶されているプログラムを実行することにより、姿勢制御機器の各部を制御する。 The third drive unit 170 has a third MPU 270 and a third memory 275 in addition to the configuration of the second embodiment. The third MPU 270 has a third drive signal processing unit 271 and a third interface 273, and controls each part of the attitude control device by executing a program stored in the third memory 275.

第１インターフェース２１３は、第２姿勢推定部２０８からの第２ＲＯＬＬ角及び第２ＰＡＮ角と、第１角速度補正部２０６からのＹ軸及びＺ軸角速度補正値をパケットに格納し、第２インターフェース２４３と通信を行う。なお、第１インターフェース２１３と第２インターフェース２４３との通信方式は任意の通信方式でよく、シリアル通信等に限定されない。 The first interface 213 stores the second ROLL angle and second PAN angle from the second attitude estimation unit 208 and the Y-axis and Z-axis angular velocity correction values from the first angular velocity correction unit 206 in a packet, and communicates with the second interface 243. Note that the communication method between the first interface 213 and the second interface 243 may be any communication method and is not limited to serial communication, etc.

第２インターフェース２４３は、第１インターフェース２１３との通信により姿勢角と角速度を取得すると共に、第２ＰＡＮ角とＺ軸角速度補正値をパケットに格納し、第３インターフェース２７３と通信を行う。なお、第２インターフェース２４３と第３インターフェース２７３との通信方式は任意の通信方式でよく、シリアル通信等に限定されない。 The second interface 243 acquires the attitude angle and angular velocity through communication with the first interface 213, stores the second PAN angle and Z-axis angular velocity correction value in a packet, and communicates with the third interface 273. Note that the communication method between the second interface 243 and the third interface 273 may be any communication method and is not limited to serial communication, etc.

第２駆動信号処理部２４１は、第２インターフェース２４３から第２ＲＯＬＬ角とＹ軸角速度補正値とを取得する。第２駆動信号処理部２４１は、第２ＲＯＬＬ角とＲＯＬＬ軸目標角度との角度偏差、及びＹ軸角速度補正値とＲＯＬＬ軸目標角速度との角速度偏差を算出する。第２駆動信号処理部２４１は、取得した角度偏差及び角速度偏差を用いたＰＩＤ制御等により取得される操作量と、第２回転角センサ１４３により検知された第２モータ１４２の回転角に基づく第２モータ１４２への通電パターンとを第２駆動回路１４１に送信する。 The second drive signal processing unit 241 acquires the second ROLL angle and the Y-axis angular velocity correction value from the second interface 243. The second drive signal processing unit 241 calculates the angle deviation between the second ROLL angle and the ROLL axis target angle, and the angular velocity deviation between the Y-axis angular velocity correction value and the ROLL axis target angular velocity. The second drive signal processing unit 241 transmits to the second drive circuit 141 the operation amount acquired by PID control or the like using the acquired angle deviation and angular velocity deviation, and the current supply pattern to the second motor 142 based on the rotation angle of the second motor 142 detected by the second rotation angle sensor 143.

第３インターフェース２７３は、第２インターフェース２４３との通信により姿勢角と角速度を取得する。なお、第３インターフェース２７３は、第１インターフェース２１３との通信により姿勢角と角速度を取得してもよい。 The third interface 273 acquires the attitude angle and angular velocity through communication with the second interface 243. The third interface 273 may also acquire the attitude angle and angular velocity through communication with the first interface 213.

第３駆動信号処理部２７１は、第３インターフェース２７３から第２ＰＡＮ角とＺ軸角速度補正値とを取得する。第３駆動信号処理部２７１は、第２ＰＡＮ角とＰＡＮ軸目標角度との角度偏差、及びＺ軸角速度補正値とＰＡＮ軸目標角速度との角速度偏差を算出する。第３駆動信号処理部２７１は、取得した角度偏差及び角速度偏差を用いたＰＩＤ制御等により取得される操作量と、第３回転角センサ１７３により検知された第３モータ１７２の回転角に基づく第３モータ１７２への通電パターンとを第３駆動回路１７１に送信する。 The third drive signal processing unit 271 acquires the second PAN angle and the Z-axis angular velocity correction value from the third interface 273. The third drive signal processing unit 271 calculates the angle deviation between the second PAN angle and the PAN axis target angle, and the angular velocity deviation between the Z-axis angular velocity correction value and the PAN axis target angular velocity. The third drive signal processing unit 271 transmits to the third drive circuit 171 the operation amount acquired by PID control or the like using the acquired angle deviation and angular velocity deviation, and the current supply pattern to the third motor 172 based on the rotation angle of the third motor 172 detected by the third rotation angle sensor 173.

以下、図１５Ｂを参照して、本実施例の姿勢制御装置の変形例について説明する。図１５Ｂは、本実施例の姿勢制御装置の変形例のブロック図である。 Below, a modified example of the attitude control device of this embodiment will be described with reference to FIG. 15B. FIG. 15B is a block diagram of a modified example of the attitude control device of this embodiment.

図１６Ａは、本実施例の３軸の姿勢制御を行う姿勢制御装置のブロック図である。本実施例では、実施例１と異なる構成について説明し、実施例３と同様の構成については同一の参照番号を付し、詳細な説明は省略する。 Figure 16A is a block diagram of the attitude control device that performs three-axis attitude control in this embodiment. In this embodiment, configurations different from those in embodiment 1 will be described, and configurations similar to those in embodiment 3 will be given the same reference numbers and detailed descriptions will be omitted.

第１ＭＰＵ２００は、実施例３の構成に加え、インターフェース２１３を有する。また、第１ＭＰＵ２００は、実施例１の構成とは異なり、第２駆動信号処理部２４１及び第３駆動信号処理部２７１を有さない。 The first MPU 200 has an interface 213 in addition to the configuration of the third embodiment. Also, unlike the configuration of the first MPU 200 of the first embodiment, the first MPU 200 does not have the second drive signal processing unit 241 and the third drive signal processing unit 271.

第２駆動部１４０は、実施例３の構成に加え、第２ＭＰＵ２４０及び第２メモリ２４５を有する。第２ＭＰＵ２４０は、第２駆動信号処理部２４１及び第２インターフェース２４３を有し、第２メモリ２４５に記憶されているプログラムを実行することにより、姿勢制御機器の各部を制御する。 The second drive unit 140 has a second MPU 240 and a second memory 245 in addition to the configuration of the third embodiment. The second MPU 240 has a second drive signal processing unit 241 and a second interface 243, and controls each part of the attitude control device by executing a program stored in the second memory 245.

第３駆動部１７０は、実施例３の構成に加え、第３ＭＰＵ２７０及び第３メモリ２７５を有する。第２ＭＰＵ２７０は、第３駆動信号処理部２７１及び第３インターフェース２７３を有し、第３メモリ２７５に記憶されているプログラムを実行することにより、姿勢制御機器の各部を制御する。 The third drive unit 170 has a third MPU 270 and a third memory 275 in addition to the configuration of the third embodiment. The second MPU 270 has a third drive signal processing unit 271 and a third interface 273, and controls each part of the attitude control device by executing a program stored in the third memory 275.

第１インターフェース２１３は、第２姿勢推定部２０８からの第２ＲＯＬＬ角、第３姿勢推定部２１２からの第３ＰＡＮ角、第１角速度補正部２０６からのＹ軸、及び第２角速度補正部２１１からのＺ軸角速度補正値をパケットに格納する。また、第１インターフェース２１３は、第２インターフェース２４３と通信を行う。なお、第１インターフェース２１３と第２インターフェース２４３との通信方式は任意の通信方式でよく、シリアル通信等に限定されない。 The first interface 213 stores in a packet the second ROLL angle from the second attitude estimation unit 208, the third PAN angle from the third attitude estimation unit 212, the Y axis from the first angular velocity correction unit 206, and the Z axis angular velocity correction value from the second angular velocity correction unit 211. The first interface 213 also communicates with the second interface 243. Note that the communication method between the first interface 213 and the second interface 243 may be any communication method and is not limited to serial communication, etc.

第３駆動信号処理部２７１は、第３インターフェース２７３から第３ＰＡＮ角とＺ軸角速度補正値とを取得する。第３駆動信号処理部２７１は、第３ＰＡＮ角とＰＡＮ軸目標角度との角度偏差、及びＺ軸角速度補正値とＰＡＮ軸目標角速度との角速度偏差を算出する。第３駆動信号処理部２７１は、取得した角度偏差及び角速度偏差を用いたＰＩＤ制御等により取得される操作量と、第３回転角センサ１７３により検知された第３モータ１７２の回転角に基づく第３モータ１７２への通電パターンとを第３駆動回路１７１に送信する。 The third drive signal processing unit 271 acquires the third PAN angle and the Z-axis angular velocity correction value from the third interface 273. The third drive signal processing unit 271 calculates the angle deviation between the third PAN angle and the PAN axis target angle, and the angular velocity deviation between the Z-axis angular velocity correction value and the PAN axis target angular velocity. The third drive signal processing unit 271 transmits to the third drive circuit 171 the operation amount acquired by PID control or the like using the acquired angle deviation and angular velocity deviation, and the current supply pattern to the third motor 172 based on the rotation angle of the third motor 172 detected by the third rotation angle sensor 173.

以下、図１６Ｂを参照して、本実施例の姿勢制御装置の変形例について説明する。図１６Ｂは、本実施例の姿勢制御装置の変形例のブロック図である。 Below, a modified example of the attitude control device of this embodiment will be described with reference to FIG. 16B. FIG. 16B is a block diagram of a modified example of the attitude control device of this embodiment.

以上、本発明の好ましい実施形態について説明したが、本発明は、これらの実施形態に限定されず、その要旨の範囲内で種々の変形及び変更が可能である。 The above describes preferred embodiments of the present invention, but the present invention is not limited to these embodiments, and various modifications and variations are possible within the scope of the gist of the invention.

なお、各実施例では、相補フィルタやカルマンフィルタ等を用いて姿勢推定を行うが、本発明はこれに限定されない。 In each embodiment, posture estimation is performed using a complementary filter, a Kalman filter, or the like, but the present invention is not limited to this.

また、各実施例において、目標角度は外部からの信号によって入力される指令値から任意の角度プランにより決定され、目標角速度は外部からの信号によって入力される指令値から任意の角速度プランにより決定される。 In addition, in each embodiment, the target angle is determined by an arbitrary angle plan from a command value input by an external signal, and the target angular velocity is determined by an arbitrary angular velocity plan from a command value input by an external signal.

また、各実施例において、ＩＭＵセンサを角速度センサと加速度センサで構成してもよい。 In addition, in each embodiment, the IMU sensor may be configured with an angular velocity sensor and an acceleration sensor.

１００第１駆動部
１４０第２駆動部
２０３第１姿勢推定部
２０８第２姿勢推定部
２０４第１相対角度算出部
２０５第１加速度補正部
２０６第１角速度補正部
２０７角速度算出部
３００保持部
３０１第１ＩＭＵセンサ（第１検出部） 100 First driving unit 140 Second driving unit 203 First attitude estimation unit 208 Second attitude estimation unit 204 First relative angle calculation unit 205 First acceleration correction unit 206 First angular velocity correction unit 207 Angular velocity calculation unit 300 Holding unit 301 First IMU sensor (first detection unit)

Claims

An attitude control device that controls the attitude of a control object,
A storage unit that stores the control target;
a first detection unit that detects a first angular velocity and a first acceleration of the holding unit;
a first attitude estimation unit that estimates a first attitude angle of the holding unit using the first angular velocity and the first acceleration;
a first drive unit that rotates the holding unit around a first axis using a first motor;
a second drive unit that rotates the holding unit around a second axis perpendicular to the first axis using a second motor;
a first relative angle calculation unit that calculates a first relative angle between the first driving unit and the holding unit;
a first acceleration correcting unit that calculates a first corrected acceleration by correcting the first acceleration using the first relative angle;
a first angular velocity correction unit that calculates a first corrected angular velocity by correcting the first angular velocity using the first relative angle;
an angular velocity calculation unit that calculates an angular velocity of the first drive unit;
a second attitude estimation unit that calculates a second attitude angle of the holding unit by using the first corrected acceleration, the first corrected angular velocity, and the angular velocity of the first drive unit,
The first drive unit performs attitude control using the first attitude angle,
The second drive unit is connected to the first drive unit and performs attitude control using the second attitude angle.

a first rotation angle detection unit that detects a rotation angle of the first motor,
The attitude control device according to claim 1 , wherein the first relative angle calculation unit calculates the first relative angle by using the first attitude angle and a rotation angle of the first motor.

The attitude control device according to claim 2, characterized in that the first relative angle calculation unit calculates the first relative angle using the first attitude angle, the rotation angle of the first motor, and the relative angle of the second drive unit with respect to the first drive unit.

a support portion that supports the holding portion, the first driving portion, and the second driving portion;
a second detection unit that detects a second angular velocity and a second acceleration of the support unit;
a support portion attitude estimation unit that estimates an attitude angle of the support portion by using the second angular velocity and the second acceleration,
4. The attitude control device according to claim 1, wherein the first relative angle calculation unit calculates the first relative angle by using the first attitude angle and an attitude angle of the support unit.

The attitude control device according to claim 4, characterized in that the first relative angle calculation unit calculates the first relative angle using the first attitude angle, the attitude angle of the support unit, and the relative angle of the second drive unit with respect to the first drive unit.

a first rotation angle detection unit that detects a rotation angle of the first motor,
The posture control device according to claim 1 , wherein the angular velocity calculation unit calculates the angular velocity of the first drive unit by differentiating a rotation angle of the first motor with respect to a unit time.

a support portion that supports the holding portion, the first driving portion, and the second driving portion;
a second detection unit that detects a second angular velocity and a second acceleration of the support unit,
The attitude control device according to claim 1 , wherein the angular velocity calculation unit calculates the angular velocity of the first drive unit by using the second angular velocity.

a third drive unit that uses a third motor to rotate the holding unit about a third axis perpendicular to the first axis and the second axis;
The attitude control device according to claim 1 , wherein the third driving unit is connected to the second driving unit and performs attitude control using the second attitude angle.

a third drive unit that rotates the holding unit around a third axis perpendicular to the first axis and the second axis using a third motor;
a second relative angle calculation unit that calculates a second relative angle between the first driving unit and the holding unit;
a second acceleration correcting unit that calculates a second corrected acceleration by correcting the first acceleration using the second relative angle;
a second angular velocity correction unit that calculates a second corrected angular velocity by correcting the first angular velocity using the second relative angle;
a third attitude estimation unit that calculates a third attitude angle of the holding unit by using the second corrected acceleration, the second corrected angular velocity, and the angular velocity of the first drive unit;
a first rotation angle detection unit that detects a rotation angle of the first motor,
the second relative angle calculation unit calculates the second relative angle by using the first posture angle, a rotation angle of the first motor, and a relative angle of the third drive unit with respect to the first drive unit;
The attitude control device according to claim 3 , wherein the third driving section is connected to the second driving section and performs attitude control using the third attitude angle.

a support portion that supports the holding portion, the first driving portion, the second driving portion, and the third driving portion;
a second detection unit that detects a second angular velocity and a second acceleration of the support unit;
a support portion attitude estimation unit that estimates an attitude angle of the support portion by using the second angular velocity and the second acceleration,
10. The attitude control device according to claim 9, wherein the second relative angle calculation unit calculates the second relative angle by using the first attitude angle, the attitude angle of the support unit, and the relative angle of the third drive unit with respect to the first drive unit.

The attitude control device according to any one of claims 1 to 10, characterized in that the attitude control device is a stabilizer.

The control target further includes an imaging unit that captures an object image formed by an imaging optical system,
11. The attitude control device according to claim 1, wherein the attitude control device is an image pickup device.

A method for controlling the attitude of a control object, comprising:
detecting a first angular velocity and a first acceleration of a holder that holds the control target;
estimating a first attitude angle of the holder using the first angular velocity and the first acceleration;
calculating a first relative angle between a first drive unit that rotates the holding unit around a first axis using a first motor and the holding unit;
calculating a first corrected acceleration by correcting the first acceleration using the first relative angle;
calculating a first corrected angular velocity by correcting the first angular velocity using the first relative angle;
calculating an angular velocity of the first drive unit;
calculating a second attitude angle of the holder using the first corrected acceleration, the first corrected angular velocity, and the angular velocity of the first drive unit;
performing attitude control by the first drive unit using the first attitude angle;
and performing attitude control by a second drive unit, the second drive unit being connected to the first drive unit and using the second attitude angle to rotate the holding unit around a second axis perpendicular to the first axis.