JP5228641B2 - Attitude control device and position control device - Google Patents

Description

  The present invention relates to an attitude control device for an artificial satellite that performs attitude control using a control moment gyro (hereinafter, CMG) mounted on the artificial satellite, and a position for controlling the hand position of a manipulator having a plurality of joints. The present invention relates to a control device.

  An attitude control device that performs three-axis attitude control of an artificial satellite using a plurality of CMGs is known (see, for example, Patent Document 1). In such a system, there exists a state of a singular point where torque around a certain axis cannot be output by a combination of gimbal angles of the CMGs. Therefore, in order to avoid unstable behavior at a singular point, the target value of the gimbal angular velocity is calculated and controlled from the target value of the attitude control torque using an approximate expression of the pseudo inverse matrix of the Jacobian matrix.

  As an example of a similar system, a control device that controls the position and speed of the tip of a manipulator by driving each joint in a manipulator having a plurality of joints is known (for example, see Patent Document 2). In such a system, the target value of the manipulator tip speed is calculated by combining feedback control and feedforward control. At this time, in order to avoid unstable behavior at a singular point, the target value of the joint angular velocity is calculated and controlled from the target value of the manipulator tip speed using an approximate inverse matrix of the Jacobian matrix.

Japanese translation of PCT publication No. 2002-506773 Japanese Patent Laid-Open No. 7-314363

  However, if an approximate expression of a pseudo inverse matrix of a Jacobian matrix is used, an approximation error occurs. For this reason, a desired posture motion cannot be realized from the gimbal angular velocity calculated in this way, and there is a problem that a posture control error becomes large in the vicinity of a singular point.

  The present invention has been made to solve the above-described problems, and an object of the present invention is to obtain a posture control device that can realize a desired posture motion even at a singular point and its vicinity.

  Another object of the present invention is to provide a position control device capable of realizing a desired hand movement even at a singular point and in the vicinity thereof.

  The attitude control device according to the present invention calculates a feedforward gimbal angular velocity and a target attitude angle of the artificial satellite which are obtained according to a desired attitude movement of the artificial satellite, and rotates the attitude of the artificial satellite and a plurality of control moment gyros. The attitude controller has a feedback control system for calculating attitude control torque based on an attitude angle error between the target attitude angle and the observed attitude angle of the satellite, and the control moment From the attitude control torque calculated by the feedback control system, the control moment gyro is adjusted so that the gimbal angular velocity component of the control moment gyro is smaller than the feed forward gimbal angular velocity component near the singular point in the gyro. Seeking components Nbaru angular velocity, based on a combination of the components of gimbal angular velocity calculated and the feedforward gimbal angular velocity, to generate attitude control torque of the satellite to the control moment gyros, those.

  The position control device according to the present invention is a position control device for controlling the hand position of a manipulator having a plurality of joints, and calculates a joint angular velocity for outputting a feedback control speed based on the hand position error of the manipulator. Calculated by the joint distribution law unit, the manipulator motion planning unit that calculates the feedforward joint angular velocity based on the desired hand movement of the manipulator, the joint angular velocity calculated by the joint distribution law unit, and the manipulator motion planning unit Combined with the feedforward joint angular velocity, the magnitude of the feedback control speed is reduced as the singular point is approached so that the magnitude of the feedback control speed becomes zero at the singular point of the manipulator. Cut continuously It is obtained and wherein the obtaining.

  According to the attitude control device according to the present invention, the three-axis attitude control of the artificial satellite is performed by combining the attitude feedback control of the artificial satellite and the feedforward control of the CMG gimbal axis. In addition, the attitude control error of the artificial satellite can be reduced.

  Further, according to the position control device according to the present invention, by combining the feedback control of the hand position of the manipulator and the feedforward control of the joint angular velocity of the manipulator, the position control of the hand position of the manipulator is performed, whereby the singular point of the manipulator Even in the vicinity thereof, the hand position control error of the manipulator can be reduced.

Embodiment 1 FIG.
Embodiment 1 according to the present invention will be described below with reference to the drawings.
FIG. 1 is a diagram showing the configuration of the attitude control device for an artificial satellite according to the first embodiment. Three or more CMGs are required to perform the three-axis attitude control of the artificial satellite, and FIG. 1 shows an example in which four CMGs having a redundant system are arranged.

  Each CMG includes a CMG mechanism (CMG mechanism unit) 1 and a CMG controller (CMG Controller) 2. The CMG mechanism 1 is controlled by a CMG controller 2. Each CMG controller 2 is connected to an attitude controller 3 and controls the CMG mechanism 1 in accordance with a control signal from the attitude controller 3, and information such as the rotor angular speed and the gimbal angle of the CMG mechanism 1 is transmitted to the attitude controller 3. Output. The attitude controller 3 calculates a torque to be output by each CMG mechanism 1 in order to control the attitude of the artificial satellite, and outputs a control signal to each CMG controller 2 based on the calculated torque. The CMG mechanism 1, the CMG controller 2, the attitude controller 3, and the artificial satellite 15 constitute an attitude control system of the artificial satellite attitude control device.

  FIG. 2 is a diagram illustrating a configuration of the CMG mechanism 1 in the attitude control device for artificial satellite according to the first embodiment. The rotor 4 is rotated by a spin motor 6 around the spin shaft 5 and connected to the gimbal 7 via the spin motor 6. The gimbal 7 is rotated by a gimbal motor 9 around a gimbal shaft 8 substantially orthogonal to the spin shaft 5, and is connected to the frame 10 via the gimbal motor 9. The frame 10 is fixed to the artificial satellite. The CMG mechanism 1 outputs torque in a direction perpendicular to the spin axis 5 and the gimbal axis 8 by rotating the rotor 4 that rotates at a substantially constant angular velocity around the spin axis 5 around the gimbal axis 8. The spin motor 6 is controlled by the CMG controller 2 so that the rotation speed becomes a constant magnitude. Further, the rotation of the gimbal motor 9 is controlled by the CMG controller 2, the current value applied to the motor coil is adjusted according to the input gimbal angular velocity target value, and the output torque is adjusted according to the adjusted current value. It is adjusted to a desired size.

FIG. 3 shows the configuration of the attitude control system in the attitude control apparatus for an artificial satellite according to the first embodiment. The posture motion planning unit 11, the regulator (feedback control system) 12, the CMG distribution law unit 13, and the posture determination system 16 are mounted on the posture controller 3 using a CPU, a memory, a logic circuit, or the like. The posture motion planning unit 11 calculates a posture angle target value (α _d ) 22. The attitude determination system 16 observes the attitude angle (α) of the artificial satellite 15 using a star sensor, an earth sensor, an attitude detection gyro, and the like, and obtains an attitude angle detection value (α _s ) 29. A difference between the posture angle target value 22 calculated by the posture motion planning unit 11 and the posture angle detection value 29 observed by the posture determination system 16 is a posture angle error (ε _α ) 23. The regulator 12 calculates a posture control torque (vector τ _c ) 24 that reduces the posture angle error 23 based on the value of the posture angle error 23. The CMG distribution law unit 13 calculates a gimbal angular velocity (vector γ _c ′) 25 of each CMG for outputting the attitude control torque 24. The adder 100 adds the feedforward gimbal angular velocity 21 calculated by the posture motion planning unit 11 to the gimbal angular velocity 25 calculated by the CMG distribution law unit 13 to calculate a gimbal angular velocity target value (γ _g ′) 26. . In the CMG 14, the rotation of the gimbal 7 is controlled according to the gimbal angular velocity target value 26, and the output torque (τ _a ) 27 is applied to the artificial satellite 15. In the artificial satellite 15, the attitude angle (α) 28 changes according to the laws of mechanics by the action of the output torque 27 of the CMG 14. The regulator 12, the CMG distribution law unit 13, the CMG 14, the artificial satellite 15, and the attitude determination system 16 convert the attitude angle error (ε _α ) 23 obtained from the difference between the attitude angle target value 22 and the attitude angle detection value 29 to the regulator 12. To form a closed loop of the feedback control system. Further, the feedforward gimbal angular velocity 21 calculated by the posture motion planning unit 11 is input to the CMG 14 via the adder 100, thereby forming an open loop of the feedforward control system.

  Hereinafter, the operation of the CMG distribution rule unit 13 will be described. When the number of CMGs is N, the relationship between the attitude control torque 24 and the gimbal angular velocity 25 is as follows.

In Expression (1), τ _c is an attitude control torque vector (three dimensions), γ _c ′ is a gimbal angular velocity vector (N dimensions), and J is a Jacobian matrix (3 × N dimensions). The Jacobian matrix J is a function of the gimbal angle, and the value changes depending on the gimbal angle.

In order to calculate the gimbal angular velocity 25 from the attitude control torque 24, a conventional method for obtaining the gimbal angular velocity vector γ _c ′ by multiplying the attitude control torque vector by the pseudo inverse matrix J ^# of the Jacobian matrix as shown in the following equation. is there.

In equation (3), the superscript T on the right shoulder of the matrix means transposition. When this conventional method is used, the determinant det (JJ ^T ) has a value of 0 in the combination (singular point) of the gimbal angles of each CMG in which the rank of the Jacobian matrix is 2 or less. Cannot calculate. In addition, since the value of det (JJ ^T ) is close to 0 in the vicinity of the singular point, the gimbal angular velocity calculated by the equation (2) becomes very large, exceeding the performance of CMG or unstable operation. There is a possibility. In that case, since the attitude movement of the satellite becomes abnormal, it is not desirable to use the pseudo inverse matrix of the Jacobian matrix in the vicinity of the singular point, and various approximation formulas have been devised (for example, patent documents) 1, see Patent Document 2).

  On the other hand, in the first embodiment, the CMG distribution law unit 13 uses the following equations (4) and (5) to calculate the gimbal angular velocity 25 from the attitude control torque 24.

Here, in the calculation formula (5) for obtaining the distribution matrix J ⁺ , max () represents the maximum value, and adj () represents the cofactor matrix. Since det (JJ ^T ) = 0 at the singular point, α is a threshold value set so that the gimbal angular velocity 25 does not become excessive in the vicinity of the singular point.

When the combination of gimbal angles of each CMG is not close to a singular point, det (JJ ^T ) is sufficiently large and is not smaller than α, and Equation (5) becomes as follows.

Since the product of the inverse of the determinant and the cofactor matrix is equal to the inverse matrix, Equation (6) is the same as Equation (3), and the distribution matrix J ⁺ matches the pseudo inverse matrix of the Jacobian matrix. At this time, the CMG distribution law unit 13 calculates a gimbal angular velocity 25 that outputs the attitude control torque 24.

When the combination of gimbal angles of each CMG approaches a singular point and det (JJ ^T ) becomes smaller than α, equation (5) becomes the following equation.

At this time, since the distribution matrix J ⁺ is det (JJ ^T ) / α times the pseudo inverse matrix of the Jacobian matrix, the CMG distribution law unit 13 generates a torque det (JJ ^T ) / α times the attitude control torque 24. An output gimbal angular velocity 25 is calculated. Therefore, the magnitude of the torque output from the CMG 14 by the gimbal angular velocity 25 decreases as it approaches the singular point and becomes zero at the singular point. In this way, the CMG distribution law unit 13 is configured to prevent the gimbal angular velocity 25 from becoming excessive near the singular point.

When the number of CMGs does not have a redundant system, the Jacobian matrix is a square matrix. Therefore, instead of using Expression (5) as the distribution matrix J ⁺ , the following expression can be used as another aspect.

  In Expression (8), sgn () is a sign function. If Expression (8) is used, the gimbal angular velocity 25 is not necessarily 0 at a singular point, so it is desirable to use Expression (5) regardless of the presence or absence of a redundant system. Further, the use of Expression (5) has an advantage that the same calculation method can be applied even when the number of CMGs changes.

  As shown in FIG. 3, the gimbal angular velocity target value 26 input to the CMG 14 is a value obtained by adding the feedforward gimbal angular velocity 21 to the gimbal angular velocity 25. As the combination of the gimbal angles of the CMGs approaches the singular point, the output torque 27 of the CMG 14 is a component output by the feedforward gimbal angular velocity 21 as the component (feedback control torque) output by the gimbal angular velocity 25 decreases. (Feedforward control torque) becomes dominant. At the singular point, only the component due to the feedforward gimbal angular velocity 21 is output. Therefore, by calculating the feedforward gimbal angular velocity 21 that dynamically matches the desired posture motion in the posture motion planning unit 11, the closed loop control and the open loop control are automatically and continuously switched at and around the singular point. A desired posture movement is realized.

  Here, the operation of the feedforward gimbal angular velocity of the posture motion planning unit 11 will be described. FIG. 4 shows the trajectory of the gimbal angle in one CMG, and shows its time history.

  First, the posture motion planning unit 11 determines the initial value 115 and final value 116 of the gimbal angle in the CMG, and divides the gimbal angle into a plurality of sections, and the gimbal angle 117 at the end points of each section excluding the initial value 115 and the final value 116. , 118 are set as parameters. FIG. 4 shows a case where it is divided into three sections.

Next, the posture motion planning unit 11 expresses the time histories 119, 120, and 121 of the gimbal angle in each section by a time function (for example, a polynomial having time as a variable), and sets parameters for determining the time function (for example, Set the polynomial coefficient. Here, the variable of the time function is not limited to time, but may be a variable substantially equivalent to time (for example, a variable obtained by dividing time by a certain numerical value).
The time function of each section is a function that satisfies the gimbal angles 115, 116, 117, and 118 set at the section end points.

  In this way, the parameters are set for all the CMGs to be used, and the differential value of the gimbal angle of the CMG 14 is obtained as the feed forward gimbal angular velocity along the time history of the gimbal angle determined thereby, and the obtained feed forward gimbal is obtained. When the CMG controller 2 of the CMG 14 drives each motor 9 based on the angular velocity and rotates each gimbal, the attitude angle of the artificial satellite 15 moves along a time history that is dynamically matched with the attitude angle.

  Therefore, by controlling the parameters so that the final value of the attitude angle of the artificial satellite matches the target value with the same or small difference as the target value, the attitude angle can be controlled to converge to the target value. As a method for determining parameters, for example, there is a nonlinear programming method, but a detailed description thereof is omitted here.

  As described above, according to the first embodiment, by combining the CMG distribution law unit 13 and the feedforward gimbal angular velocity 21, a desired posture motion is realized even in the singular point and the vicinity thereof, and the posture control error is large. It is possible to pass near the singular point without becoming.

Further, in the CMG distribution law unit 13, instead of using the expression (5) as the distribution matrix J ⁺ , another approximate expression of the pseudo inverse matrix of the Jacobian matrix may be used as another aspect. For example, there is an approximate expression such as the following expression.

In Expression (9), I is a unit matrix, and k is a minute value set to prevent (JJ ^T ) ^{−1 from} being unable to be calculated at a singular point.

When an approximate expression different from Expression (5) is used as the distribution matrix J ⁺ , unnecessary attitude driving torque may be output near the singular point and the attitude control error may increase. Due to the effect of the gimbal angular velocity 21, it is possible to pass near the singular point.

  As described above, the attitude control device according to the first embodiment calculates the feedforward gimbal angular velocity and the target attitude angle of the artificial satellite, which are obtained according to the desired attitude movement of the artificial satellite, and the attitude of the artificial satellite. Feedback control for calculating attitude control torque based on an attitude angle error between the target attitude angle and the observed attitude angle of the artificial satellite. Attitude control calculated by the feedback control system so that the gimbal angular velocity component of the control moment gyro is smaller than the feed forward gimbal angular velocity component near the singular point in the control moment gyro. From above torque We obtain the components of gimbal angular velocity of the roll moment gyros, based on a combination of the components of gimbal angular velocity calculated and the feedforward gimbal angular velocity, characterized in that to generate attitude control torque of the satellite to the control moment gyro.

  In addition, it has a plurality of control moment gyros, a feed-forward gimbal angular velocity determined according to a desired attitude motion of the satellite, and a attitude motion planning unit for calculating a target attitude angle of the satellite, A feedback control system for calculating attitude control torque based on an attitude angle error between the target attitude angle and the observed attitude angle of the artificial satellite; and an attitude controller that controls the rotation of the control moment gyro. A distribution matrix based on a comparison between a determinant composed of a Jacobian matrix with the gimbal angle of the control moment gyro as a parameter and a threshold at which the gimbal angular velocity of the control moment gyro is 0 at a singular point of the control moment gyro. Is calculated Based on the distribution matrix and the attitude control torque calculated by the feedback control system, the distribution law part that calculates the gimbal angular speed of the control moment gyro, and the gimbal angular speed and feedforward gimbal angular speed calculated by the distribution law part are added. The control moment gyro may generate an attitude control torque of the artificial satellite based on the addition result of the adder.

  At this time, the distribution law unit calculates a matrix by the product of the Jacobian matrix that converts the output vector into the input vector and the transposed matrix of the Jacobian matrix, and the input vector has the maximum value of the inverse matrix of the matrix and the threshold value. An output vector is calculated by multiplying the inverse, the cofactor matrix of the matrix, and the transposed matrix of the Jacobian matrix.

  Accordingly, in the satellite attitude control device using a plurality of control moment gyros, the distribution law unit for calculating the gimbal angular velocity of the control moment gyro for outputting a feedback control torque based on the attitude angle error of the satellite. The attitude motion planning unit that calculates the feedforward gimbal angular velocity of the control moment gyro based on the desired attitude motion of the satellite, the gimbal angular velocity calculated by the distribution law unit, and the attitude motion planning unit Combined with the feedforward gimbal angular velocity, the feedback control torque is reduced as the singular point is approached so that the feedback control torque becomes zero at the singular point of the control moment gyro. Tsu attitude control system is configured to a click and feedforward control can continuously switch over.

  Thus, according to Embodiment 1, the singular point and its vicinity are obtained by performing the three-axis attitude control of the artificial satellite by combining the feedback control of the artificial satellite attitude and the feedforward control of the CMG gimbal axis. The desired posture movement can be obtained with higher accuracy.

Embodiment 2. FIG.
FIG. 5 shows the configuration of the attitude control system according to the second embodiment. In the figure, the same reference numerals as those in FIG. The difference between the posture angle target value 22 calculated by the posture motion planning unit 11 and the posture angle detection value 29 observed by the posture determination system 16 is the posture angle error 23. The regulator (feedback control system) 12 calculates an attitude control torque 24 that reduces the attitude angle error 23 based on the value of the attitude angle error 23. The CMG distribution law unit 13 calculates a gimbal angular velocity 25 of each CMG for outputting the attitude control torque 24. The adder 101 calculates the gimbal angle target value 33 by adding the feedforward gimbal angle 31 calculated by the posture motion planning unit 11 to the gimbal angle 32 obtained by time-integrating the gimbal angular velocity 25 by the integrator 17. The CMG 14 controls the rotation of the gimbal according to the gimbal angle target value 33 and causes the output torque 27 to act on the artificial satellite 15. In the artificial satellite 15, the attitude angle 28 changes according to the laws of mechanics by the action of the output torque 27 of the CMG 14. The feedforward gimbal angle 31 performs a feedforward operation on the gimbal angle in the same manner as the feedforward gimbal angular velocity 21 described in FIG.

  According to the second embodiment, by combining the CMG distribution law unit 13 and the feedforward gimbal angle 31, a desired posture motion can be realized even in the singular point and the vicinity thereof, and the singular point is not increased without increasing the posture control error. It is possible to pass through the vicinity.

Embodiment 3 FIG.
FIG. 6 shows the configuration of the attitude control system according to the third embodiment. In the figure, the same reference numerals as those in FIG. The difference between the posture angle target value 22 calculated by the posture motion planning unit 11 and the posture angle detection value 29 observed by the posture determination system 16 is the posture angle error 23. The regulator (feedback control system) 12 calculates an attitude control torque 24 that reduces the attitude angle error 23 based on the value of the attitude angle error 23. The CMG distribution law unit 13 calculates a gimbal angular velocity 25 of each CMG for outputting the attitude control torque 24. The adder 101 calculates the gimbal angle target value 33 by adding the feedforward gimbal angle 31 calculated by the posture motion planning unit 11 to the gimbal angle 32 obtained by time-integrating the gimbal angular velocity 25 by the integrator 17. The adder 102 calculates the gimbal angular velocity target value 26 by adding the gimbal feedforward gimbal angular velocity 21 calculated by the posture motion planning unit 11 to the gimbal angular velocity 25 calculated by the CMG distribution law unit 13. The feedforward gimbal angular acceleration 34 is calculated by the posture motion planning unit 11. The CMG 14 controls the rotation of the gimbal according to the gimbal angle target value 33, the gimbal angular velocity target value 26, and the feedforward gimbal angular acceleration 34, and causes the output torque 27 to act on the artificial satellite 15. In the artificial satellite 15, the attitude angle 28 changes according to the laws of mechanics by the action of the output torque 27 of the CMG 14. The feedforward gimbal angular acceleration 34 performs a feedforward operation on the gimbal angular acceleration in the same manner as the feedforward gimbal angular velocity 21 described in FIG.

  According to the third embodiment, by combining the CMG distribution law unit 13, the feedforward gimbal angle 31, the feedforward gimbal angular velocity 21, and the feedforward gimbal angular acceleration 34, a desired posture motion can be achieved even in the singular point and the vicinity thereof. Is realized, and it is possible to pass through the vicinity of the singular point without increasing the posture control error.

  According to the third embodiment, even when the rotation of the gimbal is greatly accelerated and decelerated, it is possible to improve the control accuracy by inputting the feedforward gimbal angular acceleration 34 to the CMG 14.

  In FIG. 5, the same effect can be obtained by using a feed forward gimbal torque instead of the feed forward gimbal angular acceleration 34.

Embodiment 4 FIG.
FIG. 7 shows a configuration of a hand position control system of a manipulator having a plurality of joints according to the fourth embodiment.
In the figure, the difference between the target value 52 of the manipulator hand position calculated in the manipulator motion planning unit 41 and the manipulator hand position 59 calculated by observing the joint angle 58 in the forward kinematics 46 is the hand position error 53. Become. The regulator (feedback control system) 42 calculates a hand speed 54 that reduces the hand position error 53 based on the value of the hand position error 53. The joint distribution law unit 43 calculates a joint angular velocity 55 of each joint for outputting the hand speed 54.

  The adder 103 calculates the joint angular velocity target value 56 by adding the feedforward joint angular velocity 51 calculated by the manipulator motion planning unit 41 to the joint angular velocity 55 calculated by the joint distribution law unit 43. The joint controller 44 controls the rotation of the joint according to the joint angular velocity target value 56 and causes the joint driving torque 57 to act on the manipulator 45. In the manipulator 45, the joint drive torque 57 acts to perform movement according to the laws of mechanics, and the joint angle 58 changes.

  The regulator 42, the joint distribution law unit 43, the joint controller 44, the manipulator 45, and the forward kinematics 46 input a hand position error 53 obtained from the difference between the manipulator hand position target value 52 and the manipulator hand position 59 to the regulator 42. By doing so, a closed loop of the feedback control system is configured. Further, the feedforward joint angular velocity 51 calculated by the manipulator motion planning unit 41 is input to the joint controller 44 via the adder 103, thereby forming an open loop of the feedforward control system.

  Hereinafter, the function of the joint distribution rule unit 43 will be described. When the number of joints of the manipulator is Nm, the relationship between the hand speed 54 and the joint angular speed 55 is as follows.

In Expression (10), η _c ′ is a hand velocity vector (three dimensions), θ _c ′ is a joint angular velocity vector (N _m dimensions), and J _m is a Jacobian matrix (3 × N _m dimensions). When controlling the position and orientation of the hand, the velocity vector is 6-dimensional and the Jacobian matrix is 6 × Nm-dimensional. The Jacobian matrix is a function of the joint angle, and the value changes depending on the joint angle.

  In the joint distribution law unit 43, the following equations (11) and (12) are used to calculate the joint angular velocity 55 from the hand velocity 54.

Since det (J _m J _m ^T ) = 0 at the singular point, α _m is a threshold value set so that the joint angular velocity 55 does not become excessive in the vicinity of the singular point.

  The magnitude of the hand speed output by the joint angular velocity 55 calculated using Expression (11) decreases as the singular point is approached, and becomes zero at the singular point. As shown in FIG. 7, the joint angular velocity target value 56 input to the joint controller 44 is a value obtained by adding the feedforward joint angular velocity 51 to the joint angular velocity 55. As the combination of the joint angles approaches the singular point, the hand speed is reduced by the component (feedback control speed) output by the joint angular speed 55, and thereby the component output by the feedforward joint angular speed 51 (feedforward control speed). ) Becomes dominant. At the singular point, only the component due to the feedforward joint angular velocity 51 is output. Therefore, the manipulator motion planning unit 41 calculates the feedforward joint angular velocity 51 that matches the desired hand motion, so that the closed loop control and the open loop control are continuously switched between the singular point and the vicinity thereof, thereby realizing the desired motion. Is done.

  According to the fourth embodiment, by combining the joint distribution law unit 43 and the feedforward joint angular velocity 51, desired manipulator movement can be realized even in the singular point and the vicinity thereof, and the control error of the hand position increases. It is possible to pass near the singular point.

In the joint distribution law unit 43, instead of using the expression (12) as the distribution matrix J _m ⁺ , different approximate expressions of the pseudo inverse matrix of the Jacobian matrix may be used. In that case, an unnecessary hand speed is output in the vicinity of the singular point, and the control error of the hand position may increase, but even in that case, it passes through the vicinity of the singular point due to the effect of the feedforward joint angular velocity 51. Is possible.

  In addition, according to the fourth embodiment, by combining the feedback control of the hand position of the manipulator and the feedforward control of the joint angular velocity of the manipulator, the position control of the hand position of the manipulator is performed, and the singular point of the manipulator and its Even in the vicinity, a desired posture motion can be obtained with higher accuracy.

It is a figure for demonstrating the structure of the attitude | position control apparatus for artificial satellites by Embodiment 1 of this invention. It is a figure which shows the structure of the CMG mechanism of the attitude | position control apparatus for artificial satellites by Embodiment 1 of this invention. It is a figure which shows the structure of the attitude | position control system of the attitude | position control apparatus for artificial satellites by Embodiment 1 of this invention. It is a figure which shows the locus | trajectory of the gimbal angle of the attitude | position control apparatus for artificial satellites by Embodiment 1 of this invention. It is a figure which shows the structure of the attitude | position control system of the attitude | position control apparatus for artificial satellites by Embodiment 2 of this invention. 4 shows a configuration of an attitude control system of an attitude control apparatus for an artificial satellite according to Embodiment 3 of the present invention. It is a figure which shows the structure of the front-end | tip position control system of the manipulator control apparatus by Embodiment 4 of this invention.

Explanation of symbols

  1 CMG mechanism, 2 CMG controller, 3 attitude controller, 4 rotor, 5 spin axis, 6 spin motor, 7 gimbal, 8 gimbal axis, 9 gimbal motor, 10 frame, 11 attitude motion planning unit, 12 regulator, 13 CMG distribution law , 14 CMG, 15 artificial satellite, 16 attitude determination system, 17 integrator, 21 feed forward gimbal angular velocity, 22 attitude angle target value, 23 attitude angle error, 24 attitude control torque, 25 gimbal angular speed, 26 gimbal angular speed target value, 27 output torque, 28 posture angle, 29 posture angle detection value, 31 feed forward gimbal angle, 32 gimbal angle, 33 gimbal angle target value, 34 feed forward gimbal angular acceleration, 41 manipulator motion planning unit, 42 regulator, 43 Joint distribution law part, 44 joint controller, 45 manipulator, 46 forward kinematics, 51 feedforward joint angular velocity, 52 hand position target value, 53 hand position error, 54 hand speed, 55 joint angular speed, 56 joint angular speed target value, 57 joints Driving torque, 58 joint angle, 59 hand position, 100, 101, 102, 103 Adder.

Claims (3)

Multiple control moment gyros,
An attitude control unit that calculates the feedforward gimbal angular velocity required according to the desired attitude movement of the satellite and the target attitude angle of the satellite, and controls the attitude of the satellite and the rotation of the control moment gyro. With a controller,
The attitude controller
A feedback control system for calculating attitude control torque based on an attitude angle error between the target attitude angle and the observed attitude angle of the satellite,
A determinant composed of a Jacobian matrix with the gimbal angle of the control moment gyro as a parameter, and the determinant near the singular point so that the gimbal angular velocity of the control moment gyro does not become excessive in the vicinity of the singular point The distribution matrix is calculated based on a comparison with a threshold value that is set larger than the value, and the gimbal angular velocity of the control moment gyro is calculated based on the calculated distribution matrix and the attitude control torque calculated by the feedback control system. A distribution law part to be calculated;
An adder for adding the gimbal angular velocity calculated in the distribution law part and the feedforward gimbal angular velocity;
The control moment gyro generates an attitude control torque of the artificial satellite based on the addition result of the adder.
An attitude control device characterized by the above. The distribution law unit, the reciprocal of the maximum value of the Jacobian matrix for transforming the gimbal angular velocity vector in the attitude control torque vector, the product of the transposed matrix of the Jacobian matrix and calculating the determinant, and the calculated determinant and the threshold value The attitude control device according to claim 1 , wherein the distribution matrix is calculated by multiplying the cofactor matrix of the matrix and the transposed matrix of the Jacobian matrix. Combining the gimbal angular velocity calculated by the distribution law part with the feedforward gimbal angular speed calculated by the posture motion planning part, the magnitude of the feedback control torque becomes 0 at the singular point of the control moment gyro, The attitude control device according to claim 1 or 2, wherein the feedback control torque is reduced as approaching the singular point, and the feedback control and the feedforward control are continuously switched.

Images