JP2022144935A - Attitude control device and attitude control method - Google Patents

Abstract

To provide a control device that can perform stable attitude control even if a control dealing axis varies, can enhance accuracy of attitude information after being corrected, and can perform a correction with a few of communication information or time.SOLUTION: A control device has: a first estimation unit that estimates a first attitude angle of a holding part, using a first angular velocity of the holding part holding a control object and first acceleration thereof; a first drive unit that rotates the holding part around a first axis; a second drive unit that rotates the holding part around a second axis orthogonal to the first axis; and a second estimation unit that calculates first correction acceleration to be calculated by correcting the first acceleration using a first relative angle between the first drive unit and the holding part, a first correction angular velocity to be calculated by correcting the first angular velocity using the first relative angle and a second attitude angle of the holding part using an angular velocity of the first drive unit, in which the first drive unit is configured to perform attitude control using the first attitude angle, and the second drive unit is configured to perform attitude control using the second attitude angle.SELECTED DRAWING: Figure 1A

Description

The present invention relates to an attitude control device and an attitude control method for stabilizing the attitude of an object to be controlled.

When the posture of the handle is changed in the TILT direction while the posture of an electric gimbal having multiple drive axes is being controlled, the control axis of ROLL changes from the ROLL axis to the PAN axis, and the PAN control axis changes to PAN. Since it changes from the axis to the ROLL axis, it is necessary to correct the attitude information. Patent Literature 1 discloses a configuration for correcting posture information using rotation information from a magnetic rotary encoder of each drive unit. Patent Document 2 discloses a configuration for correcting attitude information by issuing a control command for each drive axis in consideration of the angle to be offset.

JP 2015-177539 A U.S. Patent Application Publication No. 2019/0063668

However, in the configuration of Patent Document 1, since the rotation information from the magnetic rotary encoder of each driving unit is used, the linearity error of each magnetic rotary encoder is added to the error of the correction value, and the accuracy of the posture information after correction is reduced. have a bad effect. In addition, since the controller needs the information of each magnetic rotary encoder, the amount of communication information increases and it takes time.

In addition, in the configuration of Patent Document 2, since the control command must always be issued in consideration of the offset, the amount of communication information increases and it takes time.

The present invention can perform stable attitude control even if the control-corresponding axis changes, can improve the accuracy of attitude information after correction, and can perform correction with less communication information and less time. It is an object of the present invention to provide an attitude control device capable of

An attitude control apparatus according to one aspect of the present invention is an attitude control apparatus that performs attitude control of an object to be controlled, and includes a holding unit that holds the object to be controlled, and a first angular velocity and a first acceleration of the holding unit that detect a first angular velocity and a first acceleration. a detection unit, a first posture estimation unit that estimates a first posture angle of the holding unit using a first angular velocity and a first acceleration, and a first drive that rotates the holding unit around a first axis using a first motor a second drive unit that uses a second motor to rotate the holding unit about a second axis orthogonal to the first axis; and a first relative that calculates a first relative angle between the first drive unit and the holding unit. an angle calculation unit; a first acceleration correction unit that calculates a first corrected acceleration by correcting the first acceleration using the first relative angle; A first angular velocity correction section that calculates one corrected angular velocity, an angular velocity calculation section that calculates the angular velocity of the first drive section, and a holding section using the first corrected acceleration, the first corrected angular velocity, and the angular velocity of the first drive section. a second posture estimating unit that calculates a second posture angle, the first driving unit performs posture control using the first posture angle, and the second driving unit performs posture control using the second posture angle. characterized by performing

Further, an attitude control method as another aspect of the present invention is an attitude control method for performing attitude control of an object to be controlled, the method comprising the step of detecting a first angular velocity and a first acceleration of a holding portion that holds the object to be controlled; estimating a first posture angle of the holding part using the first angular velocity and the first acceleration; calculating a relative angle; calculating a first corrected acceleration by correcting the first acceleration using the first relative angle; correcting the first angular velocity using the first relative angle to obtain the first acceleration; calculating a corrected angular velocity; calculating an angular velocity of the first driving part; and calculating a second posture angle of the holding part using the first corrected acceleration, the first corrected angular velocity, and the angular velocity of the first driving part. performing attitude control by the first driving section using the first attitude angle; and using the second attitude angle to move the holding section around a second axis perpendicular to the first axis using a second motor. and a step of performing attitude control by the rotating second drive unit.

According to the present invention, it is possible to perform stable attitude control even if the control-corresponding axis changes, and it is possible to improve the accuracy of the attitude information after correction, so that the correction can be performed with less communication information and less time. It is possible to provide an attitude control device capable of performing

1 is a block diagram of an attitude control device of Example 1. FIG. 4 is a block diagram of a modified example of the posture control device of Embodiment 1. FIG. FIG. 10 is a diagram showing a case where the positional relationship of the driving portion of the biaxial stabilizer composed of the TILT axis and the ROLL axis is 0°; FIG. 10 is a diagram showing a case where the positional relationship of the drive units of the biaxial stabilizer composed of the TILT axis and the PAN axis is 0°; FIG. 10 is a diagram showing a case where the positional relationship of the driving portion of the biaxial stabilizer composed of the TILT axis and the ROLL axis is not 0°; It is a block diagram of a first relative angle calculator. 4 is a flowchart showing acceleration correction processing by an acceleration correction unit; 6 is a flowchart showing angular velocity correction processing by an angular velocity correction unit; FIG. 11 is a block diagram of the posture control device of Example 2; FIG. 11 is a block diagram of a modified example of the attitude control device of the second embodiment; It is a figure which shows the relationship between each drive part and a corresponding axis|shaft. FIG. 10 is a diagram showing a case where the positional relationship of the drive units of the three-axis stabilizer composed of the TILT axis, the ROLL axis, and the PAN axis is 0°; FIG. 12 is a diagram showing a case where the arrangement relationship of the drive units of the three-axis stabilizer composed of the TILT axis, the ROLL axis, and the PAN axis is not 0°; FIG. 11 is a block diagram of an attitude control device of Example 3; FIG. 11 is a block diagram of a modification of the posture control device of Example 3; It is a block diagram of a second relative angle calculator. FIG. 11 is a block diagram of a posture control device of Example 4; FIG. 11 is a block diagram of a modification of the posture control device of Example 4; FIG. 11 is a block diagram of a posture control device of Example 5; FIG. 12 is a block diagram of a modification of the posture control device of Example 5; FIG. 11 is a block diagram of an attitude control device of Example 6; FIG. 12 is a block diagram of a modification of the posture control device of Example 6;

BEST MODE FOR CARRYING OUT THE INVENTION Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. In each figure, the same reference numerals are given to the same members, and overlapping descriptions are omitted.

The posture control device of each embodiment is a device that performs posture control of a controlled object, and may be, for example, an electric stabilizer or a device incorporated in an imaging device.

FIG. 1A is a block diagram of an attitude control device (attitude control device) that performs two-axis attitude control according to this embodiment. The attitude control device of this embodiment has a first driving section 100 , a second driving section 140 and a holding section 300 . The holding unit 300 holds the object to be controlled and is rotatably held in two axial directions by the first driving unit 100 and the second driving unit 140 . The first driving section 100 uses the first motor 102 to rotate the holding section 300 around the first axis, and the second driving section 140 uses the second motor 142 to rotate around the second axis orthogonal to the first axis. rotate. The controlled object is, for example, imaging means for imaging a subject image formed by the imaging optical system. The second driving section 140 is arranged with respect to the first driving section 100 as shown in FIGS. 2 to 4, for example.

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 drive section 100 has a first drive circuit 101 , a first motor 102 , a first rotation angle sensor (rotation angle detection section) 103 , a first MPU 200 and a first memory 215 . The second drive section 140 has a second drive circuit 141 , a second motor 142 and a second rotation angle sensor 143 .

The first MPU 200 includes 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, and a second attitude estimation unit. It has a section 208 and a second drive signal processing section 241 . The first MPU 200 controls each part of the attitude control equipment by executing programs stored in the first memory 215 .

First attitude estimation section 203 estimates the attitude of holding section 300 using the angular velocity and acceleration from first IMU sensor 301 . Specifically, the first posture estimating unit 203 uses the angular velocity and acceleration of each of the XYZ axes from the first IMU sensor 301 to calculate the first tilt angle and the first roll angle, which are the first posture angles of the holding unit 300 . , and the first PAN angle.

The first drive signal processing unit 201 calculates the angular deviation between the first TILT angle and the TILT axis target angle from the first posture estimating 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. to get The first driving signal processing unit 201 performs a first driving signal based on the operation amount obtained by PID control or the like using the obtained angular deviation and angular velocity deviation and the rotation angle of the first motor 102 detected by the first rotation angle sensor 103 . The energization pattern for the motor 102 is transmitted to the first drive circuit 101 .

The first drive circuit 101 drives the first motor 102 according to the signal from the first drive signal processing section 201 .

FIG. 5 is a block diagram of the first relative angle calculator 204. As shown in FIG. The first relative angle calculator 204 uses the first attitude angle and the rotation angle of the first motor 102 detected by the first rotation angle sensor 103 to calculate the relative angle (the first angle) between the first drive unit 100 and the holding unit 300 . 1 relative angle). Specifically, the first relative angle calculator 204 calculates the first tilt angle, the rotation angle of the first motor 102 detected by the first rotation angle sensor 103, and the first driving unit recorded in the first memory 215. A first angular deviation θtdif1, which is a difference from the arrangement angle 216, is calculated. The first driving unit arrangement angle 216 is the relative angle of the second driving unit 140 with respect to the first driving unit 100 .

A first acceleration correction unit 205 calculates a 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 acceleration correction processing by the first acceleration correction unit 205. As shown in FIG.

In step S<b>101 , the first acceleration corrector 205 acquires the first angular deviation θtdif1 from the first relative angle calculator 204 .

In step S<b>102 , the first acceleration correction unit 205 acquires the first ROLL angle and the first PAN angle as the first posture angles of the holding unit 300 from the first posture estimation unit 203 .

Note that the order of the processes in steps S101 and S102 may be exchanged.

In step S103, the first acceleration correction unit 205 uses the first angular deviation θtdif1, the first ROLL angle, and the first PAN angle to generate gravitational acceleration only in the Z-axis direction represented by the following equation (1). Set the reference acceleration vector αbase.

In step S104, the first acceleration correction unit 205 calculates the first deformation value α'base by three-dimensionally rotating the reference acceleration vector αbase by the first angular deviation using Equation (2).

In step S<b>105 , the first acceleration correction unit 205 calculates the second deformation value α″base by three-dimensionally rotating the first deformation value α′base by the first ROLL angle using Equation (3).

In step S106, the first acceleration correction unit 205 calculates a first acceleration correction value (first correction acceleration) by three-dimensionally rotating the second deformation value α″base by the PAN angle using equation (4). do.

A first angular velocity correction unit 206 calculates a 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 angular velocity correction processing by the first angular velocity correction unit 206. As shown in FIG.

In step S<b>201 , the first angular velocity corrector 206 acquires the first angular deviation θtdif1 from the first relative angle calculator 204 .

In step S<b>202 , the first angular velocity correction unit 206 acquires the Y-axis angular velocity ωy<b>1 and the Z-axis angular velocity ωz<b>1 of the holding unit 300 from the first IMU sensor 301 .

Note that the order of the processes in steps S101 and S102 may be exchanged.

In step S203, the first angular velocity correction unit 206 calculates a first Y-axis angular velocity correction value ωycrr1 and a first Z-axis angular velocity from the first angular deviation θtdif1 and Y-axis angular velocity ωy1 using the following equations (5a) and (5b). A correction value ωzccr1 is calculated.

In step S204, the first angular velocity correction unit 206 calculates a second Y-axis angular velocity correction value ωycrr2 and a second Z-axis angular velocity from the first angular deviation θtdif1 and Z-axis angular velocity ωz1 using the following equations (6a) and (6b). A correction value ωzccr2 is calculated.

In step S205, the first angular velocity correction unit 206 calculates a Y-axis angular velocity correction value (first corrected angular velocity) that is a difference value 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 corrected angular velocity), which is a difference value 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 internal clock of the first MPU 200 . Specifically, the angular velocity calculation unit 207 calculates the angular velocity of the first driving unit 100 by differentiating the rotation angle of the first motor 102 per unit time.

The second attitude estimation unit 208 receives 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 first drive value from the angular velocity calculation unit 207 . The attitude of the holding unit 300 is estimated using the angular velocity of the unit 100 and the angular velocity of the unit 100 . Specifically, the second orientation estimation unit 208 calculates the second orientation 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 angular deviation between the second roll angle and the roll axis target angle, and the difference between the Y-axis angular velocity correction value and the roll axis target angular velocity. Calculate the angular velocity deviation. Further, when the second drive unit 140 is arranged on the PAN axis, the second drive signal processing unit 241 calculates the angular deviation between the second PAN angle and the PAN axis target angle, the Z-axis angular velocity correction value, and the PAN axis target angular velocity. Calculate the angular velocity deviation from The second drive signal processing unit 241 is based on the operation amount obtained by PID control or the like using the obtained angular deviation and angular velocity deviation, and the rotation angle of the second motor 142 detected by the second rotation angle sensor 143 . 2, the energization pattern for the motor 142 is transmitted to the second drive circuit 141;

The second drive circuit 141 drives the second motor 142 according to the signal from the second drive signal processing section 241 .

As described above, in this embodiment, the first driving section 100 performs attitude control using the first attitude angle of the holding section 300 from the first attitude estimating section 203, and the second driving section 140 uses the second attitude estimating section. Attitude control is performed at the second attitude angle of the holding unit 300 from 208 .

According to the configuration of the present embodiment, the attitude angle obtained by estimating the attitude using the acceleration correction value and the angular velocity correction value obtained by correcting the acceleration and the angular velocity, and the angular velocity correction value are applied to axes other than the TILT axis. Used for feedback control. As a result, it is possible to respond to the posture of the holding hand, and stable posture control can be performed even if the control correspondence axis changes. In addition, it is possible to improve the accuracy of post-correction posture information, and to perform correction with less communication information and less time.

A modification of the attitude control device of this embodiment will be described below with reference to FIG. 1B. FIG. 1B is a block diagram of a modification of the posture control device of this embodiment.

The posture control device of the modified example has, in addition to the configuration of the present embodiment, a support section 400 that supports the first drive section 100 , the second drive section 140 and the holding section 300 . The support section 400 has a second IMU sensor (second detection section) 401 that detects the angular velocity (second angular velocity) and acceleration (second acceleration) of the support section 400 .

The first driving section 100 has a fourth posture estimating section (supporting section posture estimating section) 214 in addition to the configuration of the present embodiment. A fourth posture estimation unit 214 estimates the posture of the support unit 400 using the angular velocity and acceleration from the second IMU sensor 401 . Specifically, the fourth posture estimation unit 214 uses the angular velocity and acceleration of each of the XYZ axes from the second IMU sensor 401 to calculate the posture angles of the support unit 400, that is, the fourth TILT angle, the fourth ROLL angle, and the A fourth PAN angle is calculated. First relative angle calculation section 204 may calculate an angle deviation between the first TILT angle from first posture estimation section 203 and the fourth TILT angle from fourth posture estimation section 214 as first angle deviation θtdif1. The angular velocity calculator 207 may detect the angular velocity of the first driver 100 using data from the second IMU sensor 401 .

FIG. 8A is a block diagram of an attitude control device that performs three-axis attitude control according to this embodiment. FIG. 9 is a diagram showing the relationship between each drive unit and the corresponding shaft. In the present embodiment, a configuration different from that of the first embodiment will be described, and the same reference numerals will be given to the same configurations as those of the first embodiment, and detailed description thereof will be omitted.

The attitude control device of the present embodiment has a third driving section 170 in addition to the configuration of the first embodiment. The holding part 300 is held by the first driving part 100, the second driving part 140, and the third driving part 170 so as to be rotatable in three axial directions. The third driving section 170 rotates the holding section 300 around a third axis orthogonal to the first axis and the second axis. The second driving section 140 and the third driving section 170 are arranged with respect to the first driving section 100 as shown in FIGS. 10 and 11, for example. The third drive section 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 processing section 271 in addition to the configuration of the first embodiment. The third drive signal processor 271 calculates the angular 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 controls the third motor 172 based on the operation amount and the rotation angle of the third motor 172 detected by the third rotation angle sensor 173 by PID control or the like using the acquired angular deviation and angular velocity deviation. and the energization pattern to the third driving circuit 171 .

The third drive circuit 171 drives the third motor 172 according to the signal from the third drive signal processing section 271 .

As described above, in this embodiment, the third driving section 170 performs posture control using the second posture angle of the holding section 300 from the second posture estimating section 208 .

As described above, according to the configuration of the present embodiment, the posture angle obtained by estimating the posture using the acceleration correction value and the angular speed correction value obtained by correcting the acceleration and the angular speed, and the angular speed correction value and are used for feedback control other than the TILT axis. As a result, it is possible to respond to the posture of the holding hand, and stable posture control can be performed even if the control correspondence axis changes. In addition, it is possible to improve the accuracy of post-correction posture information, and to perform correction with less communication information and less time.

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 axes input to the second drive signal processing unit 241 and the third drive signal processing unit 271 are Needless to say, the configuration of this embodiment can be applied by changing the data.

Hereinafter, a modification of the attitude control device of this embodiment will be described with reference to FIG. 8B. FIG. 8B is a block diagram of a modification of the posture control device of this embodiment.

The posture control device of the modified example has, in addition to the configuration of the present embodiment, a support section 400 that supports the first drive section 100 , the second drive section 140 and the holding section 300 . The support section 400 has a second IMU sensor 401 that detects the angular velocity and acceleration of the support section 400 .

The first driving section 100 has a fourth posture estimating section 214 in addition to the configuration of this embodiment. A fourth posture estimation unit 214 estimates the posture of the support unit 400 using the angular velocity and acceleration from the second IMU sensor 401 . Specifically, the fourth posture estimation unit 214 uses the angular velocity and acceleration of each of the XYZ axes from the second IMU sensor 401 to calculate the posture angles of the support unit 400, that is, the fourth TILT angle, the fourth ROLL angle, and the A fourth PAN angle is calculated. First relative angle calculation section 204 may calculate an angle deviation between the first TILT angle from first posture estimation section 203 and the fourth TILT angle from fourth posture estimation section 214 as first angle deviation θtdif1. The angular velocity calculator 207 may detect the angular velocity of the first driver 100 using data from the second IMU sensor 401 .

FIG. 12A is a block diagram of an attitude control device that performs three-axis attitude control according to this embodiment. In the present embodiment, a configuration different from that of the first embodiment will be described, and the same reference numerals will be given to the same configurations as those of the first embodiment, and detailed description thereof will be omitted.

The attitude control device of the present embodiment has a third driving section 170 in addition to the configuration of the first embodiment. The holding part 300 is held by the first driving part 100, the second driving part 140, and the third driving part 170 so as to be rotatable in three axial directions. The third driving section 170 rotates the holding section 300 around a third axis orthogonal to the first axis and the second axis. The second driving section 140 and the third driving section 170 are arranged with respect to the first driving section 100 as shown in FIGS. 10 and 11, for example. The third drive section 170 has a third drive circuit 171 , a third motor 172 and a third rotation angle sensor 173 .

The first MPU 200 includes a second relative angle calculator 209, a second acceleration corrector 210, a second angular velocity corrector 211, a third posture estimator 212, and a third drive signal processor 271 in addition to the configuration of the first embodiment. have.

FIG. 13 is a block diagram of the second relative angle calculator 209. As shown in FIG. A second relative angle calculator 209 calculates a relative angle (second 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 calculator 209 calculates a second angle, which is the difference between the first TILT angle, the rotation angle of the first motor 102 and the second drive unit arrangement angle 217 recorded in the first memory 215 . Angular deviation θtdif2 is calculated. The second drive unit placement angle 217 is the relative angle of the third drive unit 170 with respect to the first drive unit 100 .

The second acceleration correction section 210 calculates the second corrected acceleration by correcting the acceleration of the holding section 300 based on the relative angle from the second relative angle calculation section 209 . Specifically, the second acceleration correction unit 210 first uses the second angle deviation θtdif2 from the second relative angle calculation unit 209, the first ROLL angle, and the first PAN angle, and is represented by Equation (1). A reference acceleration vector αbase is set in which gravitational acceleration is generated only in the Z-axis direction. 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 angular deviation θtdif2 using Equation (2). Next, the second acceleration correction unit 205 calculates the second deformation value α″base2 by three-dimensionally rotating the first deformation value α′base2 by the first ROLL angle using Equation (3). , the second acceleration correction unit 205 calculates the second acceleration correction value by three-dimensionally rotating the second deformation value α″base2 by the PAN angle using equation (4).

A second angular velocity correction unit 211 calculates a 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 uses the following equations (7a) and (7b) to convert the second angular deviation θtdif2 and the Y-axis angular velocity ωy1 to the first Y-axis angular velocity correction value ωycrr3 and the 1 Z-axis angular velocity correction value ωzcrr3 is calculated.

Next, the second angular velocity correction unit 211 calculates a second Y-axis angular velocity correction value ωycrr4 and a second Z-axis angular velocity correction from the second angular deviation θtdif2 and the Z-axis angular velocity ωz1 using the following equations (8a) and (8b). Calculate the value ωzccr4.

Finally, the second angular velocity correction unit 211 calculates a Y-axis angular velocity correction value (second corrected angular velocity) that is a difference value 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) that is a difference value between the first Z-axis angular velocity correction value ωzcrr3 and the second Z-axis angular velocity correction value ωzcrr4.

The third posture estimation unit 212 receives 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 first drive value from the angular velocity calculation unit 207 . The attitude of the holding unit 300 is estimated using the angular velocity of the unit 100 and the angular velocity of the unit 100 . Specifically, the third attitude estimation section 212 calculates the third attitude angles of the holding section 300, namely, the third TILT angle, the third ROLL angle, and the third PAN angle.

The third drive signal processor 271 calculates the angular 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 controls the third motor 172 based on the operation amount and the rotation angle of the third motor 172 detected by the third rotation angle sensor 173 by PID control or the like using the acquired angular deviation and angular velocity deviation. and the energization pattern to the third driving circuit 171 .

The third drive circuit 171 drives the third motor 172 according to the signal from the third drive signal processing section 271 .

As described above, in this embodiment, the third driving section 170 performs posture control using the third posture angle of the holding section 300 from the third posture estimating section 212 .

According to the configuration of the present embodiment, the attitude angle obtained by estimating the attitude using the acceleration correction value and the angular velocity correction value obtained by correcting the acceleration and the angular velocity, and the angular velocity correction value are applied to axes other than the TILT axis. Used for feedback control. As a result, it is possible to respond to the posture of the holding hand, and stable posture control can be performed even if the control correspondence axis changes. In addition, it is possible to improve the accuracy of post-correction posture information, and to perform correction with less communication information and less time.

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 axes input to the second drive signal processing unit 241 and the third drive signal processing unit 271 are Needless to say, the configuration of this embodiment can be applied by changing the data.

A modification of the posture control device of this embodiment will be described below with reference to FIG. 12B. FIG. 12B is a block diagram of a modification of the posture control device of this embodiment.

The posture control device of the modified example has, in addition to the configuration of the present embodiment, a support section 400 that supports the first drive section 100 , the second drive section 140 and the holding section 300 . The support section 400 has a second IMU sensor 401 that detects the angular velocity and acceleration of the support section 400 .

The first driving section 100 has a fourth posture estimating section 214 in addition to the configuration of this embodiment. A fourth posture estimation unit 214 estimates the posture of the support unit 400 using the angular velocity and acceleration from the second IMU sensor 401 . Specifically, the fourth posture estimation unit 214 uses the angular velocity and acceleration of each of the XYZ axes from the second IMU sensor 401 to calculate the posture angles of the support unit 400, that is, the fourth TILT angle, the fourth ROLL angle, and the A fourth PAN angle is calculated. First relative angle calculation section 204 may calculate an angle deviation between the first TILT angle from first posture estimation section 203 and the fourth TILT angle from fourth posture estimation section 214 as first angle deviation θtdif1. The angular velocity calculator 207 may detect the angular velocity of the first driver 100 using data from the second IMU sensor 401 .

FIG. 14A is a block diagram of an attitude control device that performs two-axis attitude control according to this embodiment. In the present embodiment, a configuration different from that of the first embodiment will be described, and the same reference numerals will be given to the same configurations as those of the first embodiment, and detailed description thereof will be 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 the second drive signal processing section 241 .

The second drive section 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 section 241 and a second interface 243, and executes programs stored in a second memory 245 to control each section of the attitude control device.

The first interface 213 stores the second ROLL angle and the 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, communicate. Note that when the second driving 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, or the second driving unit 140 is arranged on the PAN axis. In this case, only the second PAN angle and the Z-axis angular velocity correction value may be stored in the packet. Further, 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 or the like.

The second interface 243 acquires attitude angle information and angular velocity through communication with the first interface 213 . 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 when the second drive unit 140 is arranged on the roll axis. The second drive signal processor 241 then calculates the angular 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, the second driving signal processing unit 241 acquires the second PAN angle and the Z-axis angular velocity correction value from the first interface 213 when the second driving unit 140 is arranged on the PAN axis. Then, the second drive signal processing unit 241 calculates the angular 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 is based on the operation amount obtained by PID control or the like using the obtained angular deviation and angular velocity deviation, and the rotation angle of the second motor 142 detected by the second rotation angle sensor 143 . 2, the energization pattern for the motor 142 is transmitted to the second drive circuit 141;

The second drive circuit 141 drives the second motor 142 according to the signal from the second drive signal processing section 241 .

As described above, according to the configuration of the present embodiment, the posture angle obtained by estimating the posture using the acceleration correction value and the angular speed correction value obtained by correcting the acceleration and the angular speed, and the angular speed correction value and are used for feedback control other than the TILT axis. As a result, it is possible to respond to the posture of the holding hand, and stable posture control can be performed even if the control correspondence axis changes. In addition, it is possible to improve the accuracy of post-correction posture information, and to perform correction with less communication information and less time.

A modification of the attitude control device of this embodiment will be described below with reference to FIG. 14B. FIG. 14B is a block diagram of a modification of the attitude control device of this embodiment.

The posture control device of the modified example has, in addition to the configuration of the present embodiment, a support section 400 that supports the first drive section 100 , the second drive section 140 and the holding section 300 . The support section 400 has a second IMU sensor 401 that detects the angular velocity and acceleration of the support section 400 .

The first driving section 100 has a fourth posture estimating section 214 in addition to the configuration of this embodiment. A fourth posture estimation unit 214 estimates the posture of the support unit 400 using the angular velocity and acceleration from the second IMU sensor 401 . Specifically, the fourth posture estimation unit 214 uses the angular velocity and acceleration of each of the XYZ axes from the second IMU sensor 401 to calculate the posture angles of the support unit 400, namely, the fourth TILT angle, the fourth ROLL angle, and the A fourth PAN angle is calculated. First relative angle calculation section 204 may calculate an angle deviation between the first TILT angle from first posture estimation section 203 and the fourth TILT angle from fourth posture estimation section 214 as first angle deviation θtdif1. The angular velocity calculator 207 may detect the angular velocity of the first driver 100 using data from the second IMU sensor 401 .

FIG. 15A is a block diagram of an attitude control device that performs three-axis attitude control according to this embodiment. In this embodiment, a configuration different from that of the second embodiment will be described, and the same reference numerals will be given to the same configurations as those of the second embodiment, and detailed description thereof will be omitted.

The first MPU 200 has a first interface 213 in addition to the configuration of the second embodiment. Also, the first MPU 200 does not have the second drive signal processing section 241 and the third drive signal processing section 271 unlike the configuration of the second embodiment.

The second drive section 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 section 241 and a second interface 243, and executes programs stored in a second memory 245 to control each section of the attitude control device.

The third drive section 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 section 271 and a third interface 273, and controls each section of the attitude control device by executing a program stored in the third memory 275. FIG.

The first interface 213 stores the second ROLL angle and the 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, communicate. 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 or the like.

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 . 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 or the like.

The second drive signal processor 241 acquires the second ROLL angle and the Y-axis angular velocity correction value from the second interface 243 . The second drive signal processor 241 calculates the angular 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 is based on the operation amount obtained by PID control or the like using the obtained angular deviation and angular velocity deviation, and the rotation angle of the second motor 142 detected by the second rotation angle sensor 143 . 2, the energization pattern for the motor 142 is transmitted to the second drive circuit 141;

The second drive circuit 141 drives the second motor 142 according to the signal from the second drive signal processing section 241 .

The third interface 273 acquires the attitude angle and angular velocity through communication with the second interface 243 . Note that the third interface 273 may acquire the attitude angle and angular velocity through communication with the first interface 213 .

The third drive signal processor 271 acquires the second PAN angle and the Z-axis angular velocity correction value from the third interface 273 . The third drive signal processor 271 calculates an angular deviation between the second PAN angle and the PAN axis target angle, and an angular velocity deviation between the Z-axis angular velocity correction value and the PAN axis target angular velocity. The third driving signal processing unit 271 performs a third driving signal based on the operation amount obtained by PID control or the like using the obtained angular deviation and angular velocity deviation and the rotation angle of the third motor 172 detected by the third rotation angle sensor 173 . 3, the energization pattern for the motor 172 is transmitted to the third drive circuit 171 .

The third drive circuit 171 drives the third motor 172 according to the signal from the third drive signal processing section 271 .

As described above, according to the configuration of the present embodiment, the posture angle obtained by estimating the posture using the acceleration correction value and the angular speed correction value obtained by correcting the acceleration and the angular speed, and the angular speed correction value and are used for feedback control other than the TILT axis. As a result, it is possible to respond to the posture of the holding hand, and stable posture control can be performed even if the control correspondence axis changes. In addition, it is possible to improve the accuracy of post-correction posture information, and to perform correction with less communication information and less time.

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 axes input to the second drive signal processing unit 241 and the third drive signal processing unit 271 are Needless to say, the configuration of this embodiment can be applied by changing the data.

A modification of the posture control device of this embodiment will be described below with reference to FIG. 15B. FIG. 15B is a block diagram of a modification of the attitude control device of this embodiment.

The posture control device of the modified example has, in addition to the configuration of the present embodiment, a support section 400 that supports the first drive section 100 , the second drive section 140 and the holding section 300 . The support section 400 has a second IMU sensor 401 that detects the angular velocity and acceleration of the support section 400 .

The first driving section 100 has a fourth posture estimating section 214 in addition to the configuration of this embodiment. A fourth posture estimation unit 214 estimates the posture of the support unit 400 using the angular velocity and acceleration from the second IMU sensor 401 . Specifically, the fourth posture estimation unit 214 uses the angular velocity and acceleration of each of the XYZ axes from the second IMU sensor 401 to calculate the posture angles of the support unit 400, that is, the fourth TILT angle, the fourth ROLL angle, and the A fourth PAN angle is calculated. First relative angle calculation section 204 may calculate an angle deviation between the first TILT angle from first posture estimation section 203 and the fourth TILT angle from fourth posture estimation section 214 as first angle deviation θtdif1. The angular velocity calculator 207 may detect the angular velocity of the first driver 100 using data from the second IMU sensor 401 .

FIG. 16A is a block diagram of an attitude control device that performs three-axis attitude control according to this embodiment. In the present embodiment, the configuration different from that of the first embodiment will be described, and the same reference numerals will be given to the same configurations as those of the third embodiment, and detailed description thereof will be omitted.

The first MPU 200 has an interface 213 in addition to the configuration of the third embodiment. Also, the first MPU 200 does not have the second drive signal processing section 241 and the third drive signal processing section 271 unlike the configuration of the first embodiment.

The second drive section 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 section 241 and a second interface 243, and controls each section of the attitude control device by executing a program stored in the second memory 245. FIG.

The third drive section 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 section 271 and a third interface 273, and executes programs stored in a third memory 275 to control each section of the attitude control device.

The first interface 213 receives the second ROLL angle from the second posture estimation unit 208, the third PAN angle from the third posture estimation unit 212, the Y axis from the first angular velocity correction unit 206, and the Y axis from the second angular velocity correction unit 211. A Z-axis angular velocity correction value is stored in a packet. Also, the first interface 213 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 or the like.

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 . 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 or the like.

The second drive signal processor 241 acquires the second ROLL angle and the Y-axis angular velocity correction value from the second interface 243 . The second drive signal processor 241 calculates the angular 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 is based on the operation amount obtained by PID control or the like using the obtained angular deviation and angular velocity deviation, and the rotation angle of the second motor 142 detected by the second rotation angle sensor 143 . 2, the energization pattern for the motor 142 is transmitted to the second drive circuit 141;

The second drive circuit 141 drives the second motor 142 according to the signal from the second drive signal processing section 241 .

The third interface 273 acquires the attitude angle and angular velocity through communication with the second interface 243 . Note that the third interface 273 may acquire the attitude angle and angular velocity through communication with the first interface 213 .

The third drive signal processor 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 an angular deviation between the third PAN angle and the PAN axis target angle, and an angular velocity deviation between the Z axis angular velocity correction value and the PAN axis target angular velocity. The third driving signal processing unit 271 performs a third driving signal based on the operation amount obtained by PID control or the like using the obtained angular deviation and angular velocity deviation and the rotation angle of the third motor 172 detected by the third rotation angle sensor 173 . 3, the energization pattern for the motor 172 is transmitted to the third drive circuit 171 .

The third drive circuit 171 drives the third motor 172 according to the signal from the third drive signal processing section 271 .

As described above, according to the configuration of the present embodiment, the posture angle obtained by estimating the posture using the acceleration correction value and the angular speed correction value obtained by correcting the acceleration and the angular speed, and the angular speed correction value and are used for feedback control other than the TILT axis. As a result, it is possible to respond to the posture of the holding hand, and stable posture control can be performed even if the control correspondence axis changes. In addition, it is possible to improve the accuracy of post-correction posture information, and to perform correction with less communication information and less time.

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 axes input to the second drive signal processing unit 241 and the third drive signal processing unit 271 are Needless to say, the configuration of this embodiment can be applied by changing the data.

A modification of the attitude control device of this embodiment will be described below with reference to FIG. 16B. FIG. 16B is a block diagram of a modification of the posture control device of this embodiment.

The posture control device of the modified example has, in addition to the configuration of the present embodiment, a support section 400 that supports the first drive section 100 , the second drive section 140 and the holding section 300 . The support section 400 has a second IMU sensor 401 that detects the angular velocity and acceleration of the support section 400 .

The first driving section 100 has a fourth posture estimating section 214 in addition to the configuration of this embodiment. A fourth posture estimation unit 214 estimates the posture of the support unit 400 using the angular velocity and acceleration from the second IMU sensor 401 . Specifically, the fourth posture estimation unit 214 uses the angular velocity and acceleration of each of the XYZ axes from the second IMU sensor 401 to calculate the posture angles of the support unit 400, namely, the fourth TILT angle, the fourth ROLL angle, and the A fourth PAN angle is calculated. First relative angle calculation section 204 may calculate an angle deviation between the first TILT angle from first posture estimation section 203 and the fourth TILT angle from fourth posture estimation section 214 as first angle deviation θtdif1. The angular velocity calculator 207 may detect the angular velocity of the first driver 100 using data from the second IMU sensor 401 .

Although preferred embodiments of the present invention have been described above, the present invention is not limited to these embodiments, and various modifications and changes are possible within the scope of the gist thereof.

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 each embodiment, the target angle is determined by an arbitrary angle plan from the command value input by the signal from the outside, and the target angular velocity is determined by an arbitrary angular velocity plan from the command value input by the signal from the outside. be.

Also, in each embodiment, the IMU sensor may be composed of an angular velocity sensor and an acceleration sensor.

100 First drive unit 140 Second drive unit 203 First posture estimation unit 208 Second posture 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 1IMU sensor (first detection unit)

Claims (14)

An attitude control device that performs attitude control of a controlled object,
a holding unit that holds the controlled object;
a first detection unit that detects a first angular velocity and a first acceleration of the holding unit;
a first posture estimating unit that estimates a first posture angle of the holding unit using the first angular velocity and the first acceleration;
a first driving section that rotates the holding section around a first axis using a first motor;
a second driving section that rotates the holding section around a second axis orthogonal to the first axis using a second motor;
a first relative angle calculator that calculates a first relative angle between the first driving unit and the holding unit;
a first acceleration correction 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 the angular velocity of the first driving unit;
a second posture estimating unit that calculates a second posture angle of the holding unit using the first corrected acceleration, the first corrected angular velocity, and the angular velocity of the first driving unit;
The first driving unit performs attitude control using the first attitude angle,
The attitude control device, wherein the second driving section performs attitude control using the second attitude angle. further comprising a first rotation angle detection unit that detects the rotation angle of the first motor;
2. The posture control apparatus according to claim 1, wherein the first relative angle calculator calculates the first relative angle using the first posture angle and the rotation angle of the first motor. The first relative angle calculator calculates the first relative angle using the first posture angle, the rotation angle of the first motor, and the relative angle of the second drive unit with respect to the first drive unit. The attitude control device according to claim 2, characterized by: a supporting portion that supports the holding portion, the first driving portion, and the second driving portion;
a second detector that detects a second angular velocity and a second acceleration of the support;
a support posture estimator that estimates the posture angle of the support by using the second angular velocity and the second acceleration;
4. The apparatus according to any one of claims 1 to 3, wherein the first relative angle calculator calculates the first relative angle using the first posture angle and the posture angle of the support portion. Attitude control device as described. The first relative angle calculator calculates the first relative angle using the first attitude angle, the attitude angle of the support part, and the relative angle of the second driving part with respect to the first driving part. 5. The attitude control device according to claim 4. further comprising a first rotation angle detection unit that detects the rotation angle of the first motor;
The angular velocity calculator according to any one of claims 1 to 5, wherein the angular velocity calculator calculates the angular velocity of the first driving part by differentiating the rotation angle of the first motor by unit time. Attitude control device. a supporting portion that supports the holding portion, the first driving portion, and the second driving portion;
a second detector that detects a second angular velocity and a second acceleration of the support;
The attitude control apparatus according to any one of claims 1 to 6, wherein the angular velocity calculation section calculates the angular velocity of the first driving section using the second angular velocity. further comprising a third driving section that rotates the holding section around a third axis orthogonal to the first axis and the second axis using a third motor;
8. The attitude control device according to claim 1, wherein the third driving section performs attitude control using the second attitude angle. a third drive unit that uses a third motor to rotate the holding unit about a third axis orthogonal to the first axis and the second axis;
a second relative angle calculation unit that calculates a second relative angle between the first driving unit and the holding unit;
a second acceleration correction 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 posture estimating unit that calculates a third posture angle of the holding unit using the second corrected acceleration, the second corrected angular velocity, and the angular velocity of the first driving unit;
8. The attitude control device according to claim 1, wherein the third driving section performs attitude control using the third attitude angle. further comprising a first rotation angle detection unit that detects the rotation angle of the first motor;
The second relative angle calculator calculates the second relative angle using the first posture angle, the rotation angle of the first motor, and the relative angle of the third drive unit with respect to the first drive unit. The attitude control device according to claim 9, characterized by: a supporting portion that supports the holding portion, the first driving portion, the second driving portion, and the third driving portion;
a second detector that detects a second angular velocity and a second acceleration of the support;
a support posture estimator that estimates the posture angle of the support by using the second angular velocity and the second acceleration;
The second relative angle calculator calculates the second relative angle using the first posture angle, the posture angle of the support portion, and the relative angle of the third drive portion with respect to the first drive portion. 11. The attitude control device according to claim 9 or 10, characterized in that: The attitude control device according to any one of claims 1 to 11, wherein the attitude control device is a stabilizer. further comprising imaging means for imaging a subject image formed by the imaging optical system as the object to be controlled;
The posture control device according to any one of claims 1 to 11, wherein the posture control device is an imaging device. An attitude control method for controlling the attitude of a controlled object,
a step of detecting a first angular velocity and a first acceleration of a holding portion holding the controlled object;
estimating a first posture angle of the holding unit using the first angular velocity and the first acceleration;
calculating a first relative angle between the holding portion and a first driving portion that rotates the holding portion 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;
calculating a second posture angle of the holding unit using the first corrected acceleration, the first corrected angular velocity, and the angular velocity of the first driving unit;
a step of performing attitude control by the first driving unit using the first attitude angle;
using the second attitude angle to perform attitude control by a second driving section that rotates the holding section around a second axis orthogonal to the first axis using a second motor. Attitude control method.

Images