Spatial stabilization device

Abstract

The present invention provides a spatial stabilization device capable of arresting deterioration in the precision of control over the attitude state of a spatially stabilizing platform due to the influence of a drift component and a noise component included in a detected value of angular velocity. In this spatial stabilization device, a first sensor for detecting the state of a parallel mechanism has an angular velocity sensor and an acceleration sensor for detecting acceleration, including gravitational acceleration, is secured to the platform, computes an estimated value of the attitude angle of the platform on the basis of a detected value of angular velocity detected by the angular velocity sensor, and makes an acceleration correction for correcting the estimated value of the attitude angle on the basis of a detected value of acceleration detected by the acceleration sensor.

Description

spatial stabilization device

[0001] The present disclosure relates to a spatial stabilization device.

[0002] The technology of Patent Document 1 uses one inertial sensor fixed to the moving body, and angle detectors provided on a first object (outer gimbal) rotatably connected to the moving body, and a second object (inner gimbal) rotatably connected to the first object, to stabilize a payload mounted on the inner gimbal in space. The mechanism of this device employs a two-axis gimbal mechanism.

[0003] International Publication No. 2014 / 129168

[0004] However, in the technology of Patent Document 1, the detected values ​​of the angular velocity of the three axes (roll axis, pitch axis, and yaw axis) of the inertial sensor typically drift over time, and the detected values ​​of the angular velocity are subject to noise with a wide frequency range. Therefore, an error occurs in the angle obtained by integrating such detected angular velocity values ​​over time, and the longer the integration time, the greater the angle drift. As a result, the angle (or the time integral value of the angular velocity) and the gimbal angle do not consistently match. Therefore, even if the physical dimension of the spatial stabilization command is the angle and the angle command is given as zero, the object mounted on the gimbal mechanism cannot follow swings with a wide frequency range, making it impossible to achieve broadband spatial stabilization control, or even causing the spatial stabilization control to fail.

[0005] Therefore, the present disclosure aims to provide a spatial stabilization device that can suppress deterioration in the control accuracy of the attitude state of a spatially stabilized platform due to the influence of drift components and noise components contained in the detected value of angular velocity.

[0006] A first spatial stabilization device according to the present disclosure comprises: a parallel mechanism having a plurality of struts each extended and contracted by an actuator, a base to which one ends of the plurality of struts are rotatably connected and fixed to an object, and a platform to which the other ends of the plurality of struts are rotatably connected; a first sensor that detects a state of the parallel mechanism; an attitude estimation unit that calculates an estimate of an attitude state of the platform based on the state detected by the first sensor; an attitude command calculation unit that calculates a command value for an attitude state of the platform relative to the base, which brings the estimated attitude state value closer to a target attitude state value; an expansion / contraction command calculation unit that calculates a command value for an expansion / contraction displacement amount of the actuator of each of the struts based on the attitude state command value; and an actuator control unit that drive controls each of the actuators based on the command value for the expansion / contraction displacement amount of each of the actuators, wherein the first sensor has an angular velocity sensor and an acceleration sensor that detects acceleration including gravitational acceleration, and is fixed to the platform, The attitude estimation unit calculates an estimate of the attitude angle of the platform based on the detected value of the angular velocity detected by the angular velocity sensor, and performs acceleration correction to correct the estimated value of the attitude angle based on the detected value of the acceleration detected by the acceleration sensor.

[0007] A second spatial stabilization device according to the present disclosure includes a parallel mechanism having a plurality of struts each extended and contracted by an actuator, a base fixed to an object and to which one ends of the plurality of struts are rotatably connected, and a platform to which the other ends of the plurality of struts are rotatably connected; a first sensor having an angular velocity sensor and fixed to the platform; an attitude command calculation unit that calculates a command value for an attitude state of the platform with respect to the base, which brings a detected value of the angular velocity of an attitude angle of the platform detected by the angular velocity sensor closer to a target value of the angular velocity; an expansion / contraction command calculation unit that calculates a command value for an expansion / contraction displacement amount of the actuator of each of the struts based on the attitude state command value; and an actuator control unit that drive controls each of the actuators based on the command value for the expansion / contraction displacement amount of each of the actuators.

[0008] According to the first spatial stabilization device of the present disclosure, even if an error occurs in the estimated value of the attitude angle calculated based on the detected value of the angular velocity due to the influence of drift components and noise components contained in the detected value of the angular velocity, the estimated value of the attitude angle can be corrected based on the direction of gravitational acceleration using an acceleration sensor that detects acceleration including gravitational acceleration, thereby preventing a deterioration in the control accuracy of the platform's attitude state.

[0009] According to the second spatial stabilization device of the present disclosure, a command value for the attitude state is calculated so that the detected value of the angular velocity of the attitude angle approaches the target value of the angular velocity without integrating the detected value of the angular velocity of the attitude angle. Therefore, the drift component and noise component included in the detected value of the angular velocity of the attitude angle are not integrated, which causes a drift in the estimated value of the angle. Therefore, the angular velocity of the attitude angle of the platform can be accurately controlled to the target value, and the angular velocity of the attitude angle of the platform can be stabilized even if the base oscillates.

[0010] 1 is a schematic configuration diagram of a parallel mechanism according to embodiment 1. FIG. 2 is a schematic block diagram of a control device according to embodiment 1. FIG. 3 is a schematic hardware configuration diagram of a control device according to embodiment 1. FIG. 4 is a block diagram of an attitude estimation unit according to embodiment 1. FIG. 5 is a block diagram of an actuator control unit according to embodiment 1. FIG. 6 is a block diagram of an attitude estimation unit according to embodiment 2. FIG. 7 is a block diagram of an actuator control unit according to embodiment 3. FIG. 8 is a block diagram of an actuator control unit according to embodiment 5. FIG. 9 is a block diagram of an actuator control unit according to embodiment 6. FIG. 10 is a schematic block diagram of a control device according to embodiment 9.

[0011] The spatial stabilization device 1 will be described below with reference to the drawings according to various embodiments. In each embodiment, the same or corresponding parts are designated by the same reference numerals, and duplicated explanations will be omitted.

[0012] 1. First Embodiment A spatial stabilization device 1 according to this embodiment includes a parallel mechanism 2, a first sensor 20, and a control device 30. Fig. 1 shows a schematic configuration diagram of the parallel mechanism 2. Fig. 2 shows a schematic block diagram of the control device 30.

[0013] 1, the parallel mechanism 2 includes a plurality of struts 52 that are each extended and contracted by an actuator 51, a base 53 to which one ends of the struts 52 are rotatably connected and which is fixed to an object, and a platform 54 to which the other ends of the struts 52 are rotatably connected.

[0014] The struts 52 are support columns that connect the base 53 and the platform 54. Each strut 52 has an actuator 51 that expands or contracts the length of the strut 52. Each actuator 51 is driven and controlled by a control device 30, which will be described later. The base 53 is fixed to a stationary object or a moving object. Stationary objects include the ground, a building, a stationary structure, etc. Moving objects include vehicles, ships, etc. Any object is mounted on the platform 54. Hereinafter, the mounted object will be referred to as a payload or an object. The attitude of the mounted object is stabilized by stabilizing the attitude of the platform 54. Note that an object does not necessarily have to be mounted on the platform 54. The platform 54 has a stabilization surface S on which the attitude is stabilized. P The stabilizing surface S P may be a physical plane or a virtual plane.

[0015] The base 53 and the platform 54 are arranged opposite to each other, and the platform 54 is supported relative to the base 53 by a plurality of struts 52 arranged in a dispersed manner.

[0016] In this embodiment, three struts 521, 522, and 523 are provided. Base-side rotary joints B1, B2, and B3 rotatably connect one end of each strut to the base 53 and are rotary joints that can rotate around one axis. The base-side rotary joints B1, B2, and B3 are distributed in the circumferential direction around a representative position B0 of the base. The rotation axes are arranged along the circumferential direction. Platform-side rotary joints P1, P2, and P3 rotatably connect the other end of each strut to the platform 54 and are rotary joints (spherical joints in this example) that can rotate around three axes. The platform-side rotary joints P1, P2, and P3 are distributed in the circumferential direction around a representative position P0 of the platform. Here, each circumferential direction is also the circumferential direction around a reference line that connects the representative position B0 of the base and the representative position P0 of the platform.

[0017] Note that a so-called Stewart platform (or gaff platform) type structure including six struts and rotation joints at both ends of each strut that can rotate around three axes may also be used as the parallel mechanism 2. In other words, parallel mechanisms of various configurations that allow the attitude state of the platform to be changed may also be used as the parallel mechanism 2.

[0018] As shown in Figure 1, the platform Cartesian coordinate system Σ P is the stabilizing surface S of the platform P Roll axis X parallel to P and pitch axis Y P , and the stabilization surface S P The yaw axis Z is perpendicular to P and is a Cartesian coordinate system fixed to the platform 54. P faces the base 53. The origin of the Cartesian coordinate system is the stabilization surface S P The stabilizing surface S of the platform is fixed at a representative position P0 of the platform. P is the roll axis X P and pitch axis Y P The representative position P0 of the platform is located near the geometric center of the platform 54.

[0019] Base Cartesian coordinate system Σ B is the reference plane S of the base B Roll axis X parallel to B and pitch axis Y B , and the base reference surface S B The yaw axis Z is perpendicular to B and is a Cartesian coordinate system fixed to the base 53. B faces away from the platform 54. The origin of the Cartesian coordinate system is the reference plane S B The reference plane S of the base is fixed to the representative position B0 of the base provided on the base. B is the roll axis X B and pitch axis Y B The representative position B0 of the base is disposed near the geometric center of the base 53.

[0020] As shown in FIG. 1, in this embodiment, in the reference state of the relative position of the platform 54 with respect to the base 53, the stabilization surface S P and the base reference surface S B and become parallel, and the yaw axis Z on the platform side P and the yaw axis Z on the base side B and are aligned in a straight line, and the roll axis X on the platform side P and pitch axis Y P are the roll axis X of the base side B and pitch axis Y B Also, the yaw axis Z on the platform side P and the yaw axis Z on the base side B The base 53 is fixed to a stationary or moving object so that the yaw axis Z of the platform and the yaw axis Z of the platform face downward in the vertical direction when there is no swinging motion. P and the yaw axis Z on the base side B The upper side may be offset from the lower side in the vertical direction.

[0021] Cartesian coordinate system Σ of the rotational joint on each base side i (i = 1, 2, 3) is a three-axis Cartesian coordinate system fixed to the rotation joints B1, B2, and B3 on each base side. The Yi axis coincides with the rotation axis, the Zi axis coincides with the direction of expansion and contraction of the strut, and the Xi axis is set in the direction of a right-hand screw that is perpendicular to the Yi and Zi axes.

[0022] 1-2. Actuator 51 Each strut 52 has an actuator 51 that expands or contracts the length of the strut 52. In this embodiment, the actuator 51 includes a motor, a conversion mechanism that converts the rotational motion of the motor into translational motion to expand or contract the strut 52, and a second sensor 63 that detects the rotation angle and supply current of the motor. For example, a ball screw mechanism is used as the conversion mechanism. Each motor is driven and controlled by the control device 30, and the output signal of each second sensor 63 is input to the control device 30.

[0023] Note that a hydraulic or electric hydraulic actuator that uses a hydraulic pump to push and pull a rod in a hydraulic cylinder may be used as the actuator 51. In other words, various types of actuators may be used as the actuator 51 as long as they can extend and retract the strut 52.

[0024] 1-3. First Sensor 20 The first sensor 20 detects the state of the parallel mechanism 2. The first sensor 20 has an angular velocity sensor 201 and an acceleration sensor 202 that detects accelerations including gravitational acceleration g, and is fixed to the platform 54. An output signal of the first sensor 20 is input to the control device 30.

[0025] In this embodiment, the angular velocity sensor 201 detects an angular velocity ω around each coordinate axis in an orthogonal coordinate system having three coordinate axes X, Y, and Z (hereinafter also referred to as a sensor coordinate system). x , ω y , ω z The acceleration sensor 202 detects accelerations a in the directions of the coordinate axes, including the gravitational acceleration g. x , a y , a z An inertial measurement unit (IMU) is provided in which the angular velocity sensor 201 and the acceleration sensor 202 are integrated together. Note that the angular velocity sensor 201 and the acceleration sensor 202 do not necessarily have to be integrated together.

[0026] In this embodiment, the three coordinate axes X, Y, and Z of the first sensor 20 are defined in the platform Cartesian coordinate system Σ P Roll axis X P , pitch axis Y P , and the yaw axis Z P (X = X P , Y=Y P , Z=Z P The angular velocity sensor 201 of the first sensor 20 detects the roll angular velocity ω x , pitch angular velocity ω y , yaw angular velocity ω z The acceleration sensor 202 of the first sensor 20 detects the acceleration a in the roll axis direction. x , acceleration in the pitch axis direction a y, acceleration in the yaw axis direction a z is detected.

[0027] Each coordinate axis is in the platform Cartesian coordinate system Σ P The control device 30 described later converts the detected values ​​of the angular velocity around each coordinate axis and the detected values ​​of the acceleration in each coordinate axis direction into the platform Cartesian coordinate system Σ P Therefore, even when the coordinate axes do not coincide with each other, for the sake of simplicity of explanation, the detected values ​​of the angular velocity around each coordinate axis and the detected values ​​of the acceleration in each coordinate axis direction detected by the first sensor 20 are converted into the detected values ​​of the angular velocity around each coordinate axis and the detected values ​​of the acceleration in each coordinate axis direction of the platform Cartesian coordinate system Σ after the coordinate conversion. P The detected values ​​of angular velocity around each coordinate axis and acceleration in each coordinate axis direction are assumed to be the detected values ​​of angular velocity around each coordinate axis and acceleration in each coordinate axis direction.

[0028] In addition to the angular velocity sensor and acceleration sensor, the first sensor 20 may include an angle detection sensor that detects the rotation angle of the rotary joint on each base side, and a displacement detection sensor that directly detects the expansion / contraction displacement amount of each actuator, but these are not required.

[0029] 2, the control device 30 includes functional units such as an attitude estimation unit 31, an attitude command calculation unit 32, an expansion / contraction command calculation unit 33, and an actuator control unit 34. Each function of the control device 30 is realized by a processing circuit included in the control device 30. Specifically, as shown in FIG. 3, the control device 30 includes an arithmetic processing device 90 such as a CPU (Central Processing Unit), a storage device 91, an input circuit 92 that inputs external signals to the arithmetic processing device 90, and an output circuit 93 that outputs signals from the arithmetic processing device 90 to the outside.

[0030] The arithmetic processing device 90 may be an ASIC (Application Specific Integrated Circuit), an IC (Integrated Circuit), a DSP (Digital Signal Processor), an FPGA (Field Programmable Gate Array), a GPU (Graphics Processing Unit), an AI (Artificial Intelligence) chip, various logic circuits, various signal processing circuits, etc. Furthermore, the arithmetic processing device 90 may be a plurality of the same or different types, and each process may be shared and executed. As the storage device 91, various storage devices such as a RAM (Random Access Memory), a ROM (Read Only Memory), a flash memory, an EEPROM (Electrically Erasable Programmable Read Only Memory), etc. may be used.

[0031] The input circuit 92 is connected to various sensors such as the first sensor 20 and the second sensor 63, and includes an A / D converter and the like that inputs output signals from these sensors to the arithmetic processing device 90. The output circuit 93 is connected to electric loads such as the actuators 51 (in this example, the motors), and includes a drive circuit and the like that outputs control signals from the arithmetic processing device 90 to these electric loads.

[0032] The functions of the functional units 31 to 34 of the control device 30 are realized by the arithmetic processing device 90 executing software (programs) stored in the storage device 91 and cooperating with other hardware of the control device 30, such as the storage device 91, input circuitry 92, and output circuitry 93. Various setting data used by the functional units 31 to 34 is stored in the storage device 91, such as an EEPROM. Each function of the control device 30 will be described in detail below.

[0033] 1-4-1. Attitude Estimation Unit 31 The attitude estimation unit 31 calculates an estimate of the attitude state of the platform 54 based on the detection state of the first sensor 20.

[0034] As described above, the first sensor 20 is fixed to the platform 54, and includes the angular velocity sensor 201 and the acceleration sensor 202 that detects accelerations including the gravitational acceleration g.

[0035] The attitude estimation unit 31 calculates an estimate of the attitude angle of the platform based on the detected value of the angular velocity detected by the angular velocity sensor 201, and performs acceleration correction to correct the estimated value of the attitude angle based on the detected value of the acceleration detected by the acceleration sensor 202.

[0036] According to this configuration, even if an error occurs in the estimated value of the attitude angle calculated based on the detected value of the angular velocity due to the influence of drift components and noise components contained in the detected value of the angular velocity, the estimated value of the attitude angle can be corrected based on the direction of gravitational acceleration using the acceleration sensor that detects acceleration including gravitational acceleration, thereby preventing a deterioration in the control accuracy of the platform's attitude state.

[0037] In this embodiment, the attitude estimation unit 31 integrates the detected values ​​of angular velocity to calculate an estimated value of the attitude angle of the platform. The attitude estimation unit 31 also corrects the estimated value of the attitude angle so that the estimated direction of gravitational acceleration estimated from the estimated value of the attitude angle approaches the detected direction of the detected value of acceleration.

[0038] According to this configuration, the estimated value of the attitude angle is corrected so that the estimated direction of gravitational acceleration estimated from the estimated value of the attitude angle approaches the detected direction of gravitational acceleration included in the detected value of acceleration, so that the estimated value of the attitude angle can be accurately corrected based on the direction of gravitational acceleration.

[0039] The attitude estimation unit 31 calculates an estimated value of the attitude angle of the platform relative to the horizontal plane. In this embodiment, the three coordinate axes are the stabilization plane S of the platform. P Roll axis X parallel to P and pitch axis Y P , and the stabilization surface S P The yaw axis Z is perpendicular to P The attitude estimation unit 31 calculates the estimated values ​​of the attitude angles, φe, θe, and ψe, respectively, of the roll angle relative to the horizontal plane, the pitch angle, and the yaw angle. The roll angle φ is calculated based on the rotation of the roll axis X relative to the horizontal plane.P and pitch axis Y P The roll axis X of the plane passing through P The pitch angle θ is the angle of inclination around the roll axis X relative to the horizontal plane. P and pitch axis Y P The pitch axis Y of the plane passing through P The angle of inclination around the object.

[0040] As shown in FIG. 4 , the posture estimation unit 31 includes an integration calculation unit 311 and an acceleration correction unit 312 .

[0041] <Integration Calculation Unit 311> The integration calculation unit 311 calculates the detected value ω of the angular velocity around each coordinate axis. x , ω y , ω z The differential value of the estimated value of the attitude angle is calculated based on the above equation, and the differential value of the estimated value of the attitude angle is integrated to calculate the estimated value of the attitude angle.

[0042] The angular velocity, angle, acceleration, and velocity of each coordinate axis are expressed as quaternions. The detected value of the angular velocity around each coordinate axis is ω. x , ω y , ω z Quaternion ω of ~ is expressed by the following equation: Quaternion q of the estimated values ​​φe, θe, and ψe of the angles around each coordinate axis ~ is expressed by the following equation: where fq() is a function for converting Euler angles into quaternions. x , a y , a z Quaternion a ~ is expressed by the following equation: A vector may be used instead of a quaternion.

[0043] The integral calculation unit 311 calculates the quaternion ω of the detected value of the angular velocity at the current time t using the following equation: ~ (t), and the quaternion q of the estimated attitude angle at the previous time t-Δt ~ Based on (t-Δt), the quaternion q of the differential value of the estimated attitude angle at the current time t is ~ '(t) is calculated, where Δt is the calculation period.

[0044] Then, the integral calculation unit 311 calculates the quaternion q of the angular velocity of the differential value of the estimated value of the attitude angle at the current time t using the following equation: ~ '(t) and the quaternion q of the estimated attitude angle at the previous time t-Δt ~ (t-Δt), the quaternion q of the estimated attitude angle at the current time t is calculated. bf ~ (t) is calculated. Here, the quaternion q of the attitude angle estimate is calculated. bf ~ (t) is the quaternion of the estimated value of the attitude angle before the acceleration correction by the acceleration correction unit 312 is performed.

[0045] <Acceleration Correction Unit 312> The acceleration correction unit 312 calculates the gravitational acceleration components g in each coordinate axis direction obtained by decomposing the gravitational acceleration g into components in each coordinate axis direction based on the estimated value of the attitude angle. x , g y , g z is the detected acceleration value a in each coordinate axis direction. x , a y , a z The estimated attitude angle is changed so that it approaches

[0046] According to this configuration, the detected acceleration values ​​a in the directions of the coordinate axes including the gravitational acceleration g are x , a y , a z and the gravitational acceleration components g in the coordinate axis directions obtained by decomposing and converting the gravitational acceleration g into components in the coordinate axis directions based on the estimated values ​​of the attitude angles. x , g y , g z The deviation from the above is caused by an error in the estimated value of the attitude angle relative to the horizontal plane. x , g y , g z is the detected acceleration value a in each coordinate axis direction. x , a y , a z By changing the estimated value of the attitude angle so as to approach

[0047] In other words, in the initial state of the spatial stabilization device 1 in which the base 53 and the platform 54 shown in FIG. 1 are parallel to each other, the acceleration correction unit 312 calculates the acceleration in the base Cartesian coordinate system Σ B The acceleration component at the time of acceleration detection, i.e., the component of the gravitational acceleration g, is used as a reference, and the basis of the sensor coordinate system (in this embodiment, the platform Cartesian coordinate system Σ P (note that the basis of the equation is the same as that of the equation of the acceleration in each coordinate axis direction) x , a y , a z More specifically, the correction is performed by converting the base of the initial state including the gravitational acceleration into the sensor coordinate system and the detected value a of the acceleration in each coordinate axis direction detected in the sensor coordinate system. x , a y , a z A correction is made to minimize the difference between the base spanned by

[0048] According to this configuration, the detected values ​​a of acceleration in each coordinate axis direction detected in the sensor coordinate system that changes from moment to moment due to the swing of the base 53 are calculated based on the base defined by the gravitational acceleration g. x , a y , a z Since the base of the reference is changed, the deviation obtained by converting the base of the reference to the sensor coordinate system and subtracting the bases of the reference in the same coordinate system is the detected value a of the acceleration in each coordinate axis direction. x , a y , a z Therefore, the basis set by the gravitational acceleration g is the detected value a of the acceleration in each coordinate axis direction. x , a y , a z By minimizing the estimation error of the attitude angle so as to approach a basis spanned by , the error in the estimated value of the attitude angle can be reduced.

[0049] In this embodiment, the acceleration correction unit 312 calculates the gravitational acceleration components g in each coordinate axis direction obtained by decomposing the gravitational acceleration g (in other words, the reference base including the gravitational acceleration g) into components in each coordinate axis direction based on the estimated value of the attitude angle. x , g y , g zand the detected acceleration values ​​a in each coordinate axis direction x , a y , a z The deviation N from the estimated value of the attitude angle is partially differentiated by the estimated value of the attitude angle to calculate the gradient ∇N of the deviation with respect to the change in the estimated value of the attitude angle, and the estimated value of the attitude angle is changed in the direction in which the deviation N decreases based on the gradient ∇N of the deviation.

[0050] According to this configuration, by changing the estimated attitude angle in a direction that reduces the deviation N, the gravitational acceleration components g in the directions of the coordinate axes after conversion using the estimated values ​​of the attitude angle can be x , g y , g z (In other words, the base of the reference after conversion by the estimated value of the attitude angle) is calculated by the detected value a of the acceleration in each coordinate axis direction. x , a y , a z (In other words, a x , a y , a z This allows the attitude angle to approach the deviation N (a basis spanned by ), thereby reducing the error in the estimated value of the attitude angle. Furthermore, since the estimated value of the attitude angle is changed so as to minimize the deviation N, the minimization problem of minimizing the deviation N is solved, allowing for accurate correction.

[0051] Quaternion g of a vector representing the basis of reference including gravitational acceleration g ~ is the quaternion q of the estimated attitude angle at the previous time t-Δt. ~ The gravitational acceleration component g in each coordinate axis direction on the sensor coordinate system at the current time t, converted based on (t-Δt) x (t), g y (t), g z Quaternion g of (t) xyz ~ (t) is expressed by the following equation: where fgq(A, B) is a function that performs a rotation coordinate transformation on A by B. Here, the quaternion g of the vector that represents the base of reference including the gravitational acceleration g is ~ may be divided by g and normalized to a magnitude of 1. In this case, the detected acceleration values ​​a x , a y , a z Quaternion a ~is the norm of the detected value, that is, a x , a y , a z Note that it is necessary to normalize by dividing by the magnitude of the vector whose elements are . In other words, the physical units used to measure the deviation must match.

[0052] The deviation N(t) at the current time t is the gravitational acceleration component g x (t), g y (t), g z Quaternion g of (t) xyz ~ (t) and the detected acceleration value a in each coordinate axis direction at the current time t x (t), a y (t), a z Quaternion a of (t) ~ (t), the deviation N(t) is expressed by the following equation. The acceleration correction unit 312 also partially differentiates the deviation N(t) with quaternions q1e, q2e, q3e, and q4e that represent the angles around each coordinate axis (i.e., the roll, pitch, and yaw attitude angles), and calculates the gradient ∇N(t) of the deviation with respect to changes in the estimated value of the angle around each coordinate axis. Note that the deviation N(t) is a function of the quaternions q1e, q2e, q3e, and q4e of the angles around each coordinate axis, as shown in the first equation of equations (4) and (5). Various known calculation methods are used for the partial differentiation.

[0053] Then, the acceleration correction unit 312 calculates the quaternion q of the angular velocity for correcting the angle using the following equation: ~ ' acc (t) is calculated and multiplied by the calculation period Δt to obtain the quaternion q ~ acc (t) is found.

[0054] Then, the quaternion q of the estimated attitude angle after correction ~ That is, the quaternion q of the estimated value of the attitude angle before correction at the current time t calculated by the integral calculation unit 311 is bf~ (t) is calculated, and the detected acceleration values ​​a in each coordinate axis direction detected in the sensor coordinate system, which is constantly changing due to the swing of the base 53 and the gravitational acceleration g, are calculated. x , a y , a z The quaternion q of angular velocity that minimizes the deviation from the basis spanned by ~ ' acc (t) and the quaternion q of the estimated attitude angle after correction ~ (t) is calculated, where β is a positive correction gain. Note that Δt may be deleted and included in β. By dividing by the norm of the deviation gradient ||∇N(t)||, the amount of change is prevented from becoming too large or too small. Note that the above description of the minimization calculation is based on the gradient descent method, but the calculation method is not limited to this and various minimization calculations can be used. Note that the estimated value of the attitude angle after correction is expressed as q ~ (t) can be used as it is, but from the viewpoint of clarifying its physical meaning, it is necessary to obtain the estimated value of the platform posture, that is, the platform Cartesian coordinate system Σ P It is more preferable to convert these into the roll angle φe, pitch angle θe, and yaw angle ψe and output them.

[0055] <Whether or not to correct acceleration based on whether the acceleration is stationary or uniformly moving> The acceleration corrector 312 corrects the detected acceleration value a in each coordinate axis direction. x , a y , a z It may be possible to determine whether the base 53 is stationary or moving at a constant speed based on the result of the determination, and to change the correction gain β by which the gradient ∇N of the deviation is multiplied.

[0056] For example, when the base 53 is stationary or moving at a constant speed, the detected acceleration values ​​a x , a y , a zSince no acceleration components of the base other than the gravitational acceleration g are superimposed on the estimated value of the attitude angle, the error in the estimated value of the attitude angle due to drift or the like can be accurately reduced by bringing the reference base of the gravitational acceleration g after conversion based on the estimated value of the attitude angle closer to the base of the sensor coordinate system based on acceleration. On the other hand, when the base 53 is not stationary or moving at a constant speed, the detected value a of the acceleration in each coordinate axis direction x , a y , a z Because base acceleration components other than the gravitational acceleration g are superimposed on the estimated value of the attitude angle, if the correction gain β is set too large to bring the reference base of the gravitational acceleration g after conversion based on the estimated value of the attitude angle closer to the base of the acceleration-based sensor coordinate system, the base acceleration component may be over-reacted, resulting in a correction error. Therefore, the acceleration correction unit 312 may set the correction gain β smaller when it determines that the base 53 is not stationary or moving at a constant speed than when it determines that the base 53 is stationary or moving at a constant speed. Setting the correction gain β sufficiently small to avoid over-reacting to the base acceleration component averages the acceleration components of the base acceleration and deceleration that alternately occur, allowing for accurate correction of the estimated value of the attitude angle. In any case, the correction gain β should be carefully set based on the specifications of the angular velocity sensor 201 and acceleration sensor 202 used, and may basically be a fixed positive value.

[0057] For example, the acceleration correction unit 312 calculates the detected acceleration values ​​a in the directions of the coordinate axes using the following equations: x , a y , a z When the absolute value of the deviation between the norm || a(t)|| of || a(t) || and the gravitational acceleration g, which is the norm of the basis including the gravitational acceleration g, is equal to or less than the judgment value, it is determined that the object is stationary or moving at a uniform velocity, and when the absolute value of the deviation is greater than the judgment value, it is determined that the object is not stationary or moving at a uniform velocity.

[0058] Note that acceleration correction may be performed regardless of whether the base 53 is stationary or moving at a constant speed. Since the average value of the acceleration in each coordinate axis direction other than the gravitational acceleration g over a certain long period of time is approximately 0, the correction accuracy can be maintained by reducing the correction gain β or correcting the estimated value of the attitude angle using a value obtained by performing a smoothing process on the gradient of the deviation.

[0059] 1-4-2. Attitude Command Calculation Unit 32 The attitude command calculation unit 32 calculates a command value for the attitude state of the platform 54 relative to the base 53, which brings the estimated value of the attitude state closer to the target value of the attitude state.

[0060] In this embodiment, the attitude command calculation unit 32 uses the target value of the attitude angle of the platform 54 relative to the horizontal plane as the target value of the attitude state, and calculates the command value of the attitude angle of the platform 54 relative to the base 53 as the command value of the attitude state.

[0061] The attitude command calculation unit 32 uses the target value φo of the roll angle of the platform relative to the horizontal plane and the target value θo of the pitch angle as the target values ​​of the attitude angles. As command values ​​for the attitude state, the attitude command calculation unit 32 calculates a command value φc of the roll angle of the platform relative to the base that brings the estimated value φe of the roll angle closer to the target value φo of the roll angle, and calculates a command value θc of the pitch angle of the platform relative to the base that brings the estimated value θe of the pitch angle closer to the target value θo of the pitch angle.

[0062] For example, as shown in the following equation, the attitude command calculation unit 32 performs proportional-plus-integral control based on the deviation between the target value φo of the roll angle and the estimated value φe of the roll angle to calculate a command value φc of the roll angle, and performs proportional-plus-integral control based on the deviation between the target value θo of the pitch angle and the estimated value θe of the pitch angle to calculate a command value θc of the pitch angle.

[0063] Here, Kp is a proportional gain, Ki is an integral gain, and s is a Laplace operator. Note that various types of feedback control other than proportional-integral control may also be used.

[0064] In this embodiment, the attitude command calculation unit 32 sets the target value φo of the roll angle and the target value θo of the pitch angle to 0, and the stabilization surface S of the platform 54 P The target value φo of the roll angle and the target value θo of the pitch angle may be set to a value other than 0, and may be changed depending on the purpose of control.

[0065] The roll angle command value φc is set to the reference plane S of the base. B The roll axis X of the platform 54 relative to P and pitch axis Y P The roll axis X of the plane passing through P or roll axis X B The command value of the tilt angle around the base is the reference plane S. B The roll axis X of the platform 54 relative to P and pitch axis Y P The pitch axis Y of the plane passing through P or pitch axis Y B This is the command value for the tilt angle of the surroundings.

[0066] The attitude command calculation unit 32 also calculates a command value Zc for the position of the platform in the yaw axis direction relative to the base. The command value Zc for the position in the yaw axis direction is calculated based on the yaw axis Zc relative to the representative position B0 of the base. B or yaw axis Z P This is the command value for the position of the representative position P0 of the platform in the direction.

[0067] In this embodiment, the attitude command calculation unit 32 sets the command value Zc for the position in the yaw axis direction to a predetermined value Zconst. Note that the command value Zc for the position in the yaw axis direction may be changed depending on the control purpose.

[0068] The parallel mechanism 2 of this embodiment cannot control all variables independently. Therefore, the attitude command calculation unit 32 calculates dependent variables, namely, a command value ψc for the yaw angle of the platform relative to the base, a command value Xc for the position of the platform relative to the base in the roll axis direction, and a command value Yc for the position of the platform relative to the base in the pitch axis direction, based on independent variables, namely, a command value φc for the roll angle, a command value θc for the pitch angle, and a command value Zc for the position in the yaw axis direction.

[0069] Here, the yaw angle command value ψc is the yaw axis Z with respect to the representative position B0 of the base. B or the yaw axis Z relative to the representative position P0 of the platform P The command value Xc of the position in the roll axis direction is the angle around the base, and the command value Xc of the position in the roll axis direction is the angle around the base. B or roll axis X P The command value Yc of the position in the pitch axis direction is the command value of the representative position P0 of the platform in the pitch axis direction, and the command value Yc of the position in the pitch axis direction is the command value of the representative position B0 of the base in the pitch axis direction. B or pitch axis Y P This is the command value for the position of the representative position P0 of the platform in the direction.

[0070] The attitude command calculation unit 32 calculates, in order, dependent variables, a yaw angle command value ψc, a roll axis position command value Xc, and a pitch axis position command value Yc, based on independent variables, a roll angle command value φc, a pitch angle command value θc, and a yaw axis position command value Zc, as shown in the following equations: ψ , f x , f y is a function that calculates each dependent variable based on the geometric relationship of the parallel mechanism 2, and a known formula is used. The geometric specifications of the parallel mechanism are included in the function.

[0071] In the case of a Stewart platform type parallel mechanism 2 or the like, all variables can be controlled independently. In this case, the attitude command calculation unit 32 independently sets the yaw angle command value ψc, the roll axis direction position command value Xc, and the pitch axis direction position command value Yc. For example, these are set to ψc = 0, Xc = 0, and Yc = 0. Alternatively, the command values ​​ψc, Xc, and Yc may be set to a value other than 0, or may be changed depending on the control purpose.

[0072] 1-4-3. Expansion / contraction command calculation unit 33 The expansion / contraction command calculation unit 33 calculates a command value for the expansion / contraction displacement amount of the actuator of each strut based on the command value of the posture state.

[0073] In this embodiment, the extension / contraction command calculation unit 33 calculates a command value Lc for the extension / contraction displacement amount of the actuator of each strut based on the roll angle command value φc, the pitch angle command value θc, the yaw axis position command value Zc, the yaw angle command value ψc, the roll axis position command value Xc, and the pitch axis position command value Yc.

[0074] The extension / contraction command calculation unit 33 calculates the extension / contraction displacement command value Lci (i=1, 2, 3) of the actuator 51i of each strut 52i based on the roll angle command value φc, the pitch angle command value θc, the yaw axis position command value Zc, the yaw angle command value ψc, the roll axis position command value Xc, and the pitch axis position command value Yc, using the following equations: Li is a function that converts each command value into a command value Lci for the expansion / contraction displacement amount of each actuator by the inverse kinematics of the parallel mechanism, and a known formula is used. The geometrical specifications of the parallel mechanism are included in the function.

[0075] In addition, in the Stewart platform type parallel mechanism 2, when ψc = 0, Xc = 0, and Yc = 0 are set, the extension / contraction command calculation unit 33 may calculate the extension / contraction displacement command value Lci (i = 1 to 6) of the actuator 51i of each strut 52i based on the roll angle command value φc, the pitch angle command value θc, and the yaw axis position command value Zc.

[0076] 1-4-4 Actuator Control Unit 34 The actuator control unit 34 controls the drive of each actuator 51 based on the command value Lc for the expansion / contraction displacement amount of each actuator 51, thereby changing the expansion / contraction displacement amount of each actuator 51.

[0077] The actuator 51 of each strut is equipped with a second sensor 63 that detects the state of the actuator. Then, for each actuator 51, the actuator control unit 34 detects the detected value Ld of the expansion / contraction displacement amount of the actuator 51 based on the detected state of the second sensor 63. For each actuator 51, the actuator control unit 34 controls the drive of the actuator 51 so that the detected value Ld of the expansion / contraction displacement amount approaches the command value Lc of the expansion / contraction displacement amount.

[0078] In this embodiment, as described above, each actuator 51 includes a motor, a conversion mechanism that converts the rotational motion of the motor into translational motion to expand and contract the strut 52, and a second sensor 63 (a rotation sensor and a current sensor) that detects the rotation angle and supply current of the motor. The motor may be a brushed motor or a brushless motor.

[0079] In this embodiment, as shown in FIG. 5, for each actuator 51, the actuator control section 34 includes a position control section 341, a speed control section 342, and a current control section 343.

[0080] For each actuator 51, the position control unit 341 calculates the detected value Ld of the expansion / contraction displacement amount of the actuator based on the detected value of the motor's rotation angle by the second sensor 63 (rotation sensor), and calculates the command value of the motor's angular velocity so that the detected value Ld of the actuator's expansion / contraction displacement amount approaches the command value Lc of the actuator's expansion / contraction displacement amount.

[0081] For example, the position control unit 341 calculates the detected value Ld of the expansion / contraction displacement amount by multiplying the integrated angle obtained by integrating the detected value θmd of the motor rotation angle by a conversion coefficient. The position control unit 341 performs proportional-plus-integral control based on the deviation between the command value Lc of the expansion / contraction displacement amount and the detected value Ld of the expansion / contraction displacement amount, and calculates the command value ωmc of the angular velocity of the motor. Various types of feedback control other than proportional-plus-integral control may also be used.

[0082] For each actuator 51, the speed control unit 342 calculates the detected value ωmd of the motor's angular velocity based on the detected value θmd of the motor's rotation angle by the second sensor (rotation sensor), and calculates the command value Imc of the motor's supply current so that the detected value of the motor's angular velocity approaches the command value of the motor's angular velocity.

[0083] For example, the speed control unit 342 calculates the detected angular velocity value ωmd by differentiating the detected rotation angle value θmd of the motor. The speed control unit 342 performs proportional-plus-integral control based on the deviation between the angular velocity command value ωmc and the detected angular velocity value ωmd to calculate the supply current command value Imc. Various types of feedback control other than proportional-plus-integral control may also be used.

[0084] For each actuator 51, the current control unit 343 calculates a command value Vc of the motor's applied voltage so that the detected value Imd of the motor's supply current detected by the second sensor (current sensor) approaches the command value Imc of the motor's supply current, and applies voltage to the motor based on the command value Vc of the motor's applied voltage.

[0085] For example, the current control unit 343 performs proportional-integral control based on the deviation between the supply current command value Imc and the supply current detection value Imd to calculate the applied voltage command value Vc. Various types of feedback control other than proportional-integral control may also be used.

[0086] The types and calculation methods of the supply current command value, the supply current detection value, and the applied voltage command value are changed as appropriate depending on the type of motor, such as a brushed motor or a brushless motor. In any type of motor, as the supply current increases, the motor torque increases.

[0087] As described above, a multi-loop feedback control system consisting of an expansion / contraction displacement feedback control system, an angular velocity feedback control system, and a current feedback control system can be configured, and the response characteristics of each feedback control system can be optimized, thereby improving the response and stability of the entire control system. For example, the response characteristics of the current feedback control system can be made the fastest, and the supply current can be changed quickly in response to fluctuations in the external load acting on the motor, thereby maintaining the controllability of the angular velocity.

[0088] Note that a hydraulic or electrohydraulic actuator that pushes and pulls a rod in a hydraulic cylinder using a hydraulic pump may be used as the actuator 51. In this case, the second sensor detects the extension / contraction displacement of the rod and the supply current of the hydraulic pump or the electromagnetic hydraulic valve. For example, the position control unit 341 calculates a command value for the extension / contraction displacement speed of the rod so that the detected value of the extension / contraction displacement amount approaches the command value of the extension / contraction displacement amount, the speed control unit 342 calculates a command value for the supply current of the hydraulic pump or the electromagnetic hydraulic valve so that the detected value of the extension / contraction displacement speed of the rod approaches the command value for the extension / contraction displacement speed, and the current control unit 343 changes the voltage applied to the hydraulic pump or the electromagnetic hydraulic valve so that the detected value of the supply current approaches the command value for the supply current.

[0089] As long as the expansion / contraction displacement amount of each actuator 51 can be changed based on the expansion / contraction displacement amount command value Lc, various control methods may be used for the actuator control unit 34.

[0090] 2. Second Embodiment A spatial stabilization device 1 according to a second embodiment will be described with reference to the drawings. Description of components similar to those of the first embodiment will be omitted. The basic configuration of the spatial stabilization device 1 according to this embodiment is similar to that of the first embodiment, but part of the processing of the posture estimation unit 31 differs from that of the first embodiment.

[0091] As in the first embodiment, for each actuator, the actuator control unit 34 detects the detected value Ld of the expansion / contraction displacement amount of the actuator based on the detection state of the second sensor 63 .

[0092] 6, the posture estimation unit 31 has a detection value correction unit 313. The detection value correction unit 313 uses the forward kinematics of the parallel mechanism to calculate a detection value of the posture state of the platform with respect to the base based on the detection value Ld of the expansion / contraction displacement amount of each actuator 51, and performs detection value correction to correct the estimated value of the posture state based on the detected value of the posture state.

[0093] According to this configuration, the estimated value of the posture state estimated by the angular velocity sensor and the acceleration sensor can be corrected by the detected value of the posture state calculated using the forward kinematics of the parallel mechanism based on the detected values ​​of the expansion / contraction displacement amounts of each actuator 51, thereby improving the accuracy of correction of the estimated value of the posture state.

[0094] Generally, parallel mechanisms differ from serial mechanisms typified by industrial robots in that the posture of the platform relative to the amount of expansion / contraction displacement of the actuator must be determined by repeated calculations.

[0095] The distance L between the platform-side rotary joints P1, P2, and P3 is calculated using the rotation angles α1, α2, and α3 of the base-side rotary joints B1, B2, and B3 and the detected values ​​Ld1, Ld2, and Ld3 of the expansion / contraction displacement of each actuator. 12 , L 23 , L 31 The formula for calculating this can be expressed as follows:

[0096] Here, L 12 is the distance between the revolute joints P1 and P2, and f 12 (α1, α2, Ld1, Ld2) are the distances L based on the rotation angles α1, α2 of the rotary joints B1, B2 and the detected values ​​Ld1, Ld2 of the expansion and contraction displacement amounts of the actuators 511, 512. 12 This is the equation of forward kinematics that calculates L 23 is the distance between the revolute joints P2 and P3, and f 23 (α2, α3, Ld2, Ld3) are the distances L based on the rotation angles α2, α3 of the rotary joints B2, B3 and the detected values ​​Ld2, Ld3 of the expansion and contraction displacement amounts of the actuators 512, 513. 23 This is the equation of forward kinematics that calculates L31 is the distance between the revolute joints P3 and P1, and f 31 (α3, α1, Ld3, Ld1) is the distance L based on the rotation angles α3, α1 of the rotary joints B3, B1 and the detected values ​​Ld3, Ld1 of the expansion / contraction displacement amounts of the actuators 513, 511. 31 The other geometrical parameters of the parallel mechanism are included in each equation.

[0097] The detection value correction unit 313 calculates each distance L 12 , L 23 , L 31 The rotation angles α1, α2, α3 are gradually changed by repeated calculations using the Newton method or the like so that f approaches a geometrically predetermined value, and a solution for the rotation angles α1, α2, α3 is calculated. Then, using the following equations, the detection value corrector 313 calculates the detected roll angle φd, pitch angle θd, and yaw angle ψd of the platform relative to the base based on the calculated rotation angles α1, α2, α3 of each rotation joint on the base side and the detected values ​​Ld1, Ld2, Ld3 of the expansion / contraction displacement amounts of each actuator. Here, f φ , f θ , f ψ is a function that calculates the detected value of the roll angle φd, the detected value of the pitch angle θd, and the detected value of the yaw angle ψd from the detected value of each rotation angle α and the extension / contraction displacement amount Ld by the forward kinematics of the parallel mechanism, and is a known formula. Other geometric parameters of the parallel mechanism are included in the function.

[0098] The detection value corrector 313 corrects the estimated roll angle value φe, the estimated pitch angle value θe, and the estimated yaw angle value ψe based on the detected roll angle value φd, the detected pitch angle value θd, and the detected yaw angle value ψd.

[0099] For example, the detection value correction unit 313 sets the initial values ​​of the estimated roll angle value φe, the estimated pitch angle value θe, and the estimated yaw angle value ψe to the detected roll angle value φd, the detected pitch angle value θd, and the detected yaw angle value ψd.

[0100] This configuration improves the accuracy of setting the initial value of the estimation value, and improves the estimation accuracy after the start of estimation, particularly when the platform 54 is not parallel to the base 53 but is in an arbitrary posture at the start of estimation.

[0101] As described in the first embodiment, estimated values ​​of the roll angle, pitch angle, and yaw angle can be obtained with extremely small drift from the angular velocity sensor 201 and the acceleration sensor 202 using a so-called complementary filter. However, this is possible only if the initial value of the time integration is accurately known. If spatial stabilization is started from an initial state in which the base and platform are parallel, the translational displacement and rotational angular displacement of the platform relative to the base can be solved relatively easily. However, spatial stabilization is not always started from such an initial state. Therefore, the roll angle, pitch angle, and yaw angle of the platform are calculated from time to time based on Equation (14) and Equation (15). In this way, the three-axis rotational displacement, which is usually indefinite, can be identified when spatial stabilization is started. Therefore, by replacing the initial value of the time integration in the integration calculation unit 311 with the appropriate timing during the process of calculating the roll angle, pitch angle, and yaw angle, which are the output of the detection value correction unit 313, the estimated value of the attitude angle, which is the output of the detection value correction unit 313, can be accurately estimated at all times.

[0102] Furthermore, because the angular velocity sensor 201 has a characteristic that its detection value itself drifts slowly, even if the complementary filter described in embodiment 1 obtains estimates of the roll, pitch, and yaw angles with extremely small drift, the drift cannot be completely eliminated. The longer the spatial stabilization operation time, the more the accumulated error in the integrated value becomes apparent as drift in the estimated value. In such a case, a sequence is established in which, after a certain time has elapsed since spatial stabilization, at least the time integration in the integration calculation unit 311 is reset, the initial value is updated using the output of the detection value correction unit 313, which is continuously calculating, and then the time integration is resumed. Naturally, the time integration may be reset for both the integration calculation unit 311 and the acceleration correction unit 312. This further reduces the effect of drift on the estimates due to long-term time integration, enabling wide-band and high-precision spatial stabilization to be achieved regardless of the operation time.

[0103] Here, the timing of resetting the time integral in integral calculation unit 311 and updating the initial value becomes an issue, but for example, the difference between the output of detection value correction unit 313 and the output of integral calculation unit 311 or acceleration correction unit 312, i.e., the difference in attitude angle (roll angle, pitch angle, yaw angle), may be successively compared, and a trigger signal may be output that resets the time integral in integral calculation unit 311 and updates the initial value when the difference exceeds a predetermined threshold. In this way, the timing of resetting the numerical integral and updating the initial value can be automated.

[0104] The estimated roll angle φe and estimated pitch angle θe are the attitude angles of the platform with respect to the horizontal plane, while the detected roll angle φd and detected pitch angle θd are the attitude angles of the platform with respect to the base. If the base is tilted with respect to the horizontal plane, these values ​​will not match. Therefore, the detection value corrector 313 may perform correction when the base is fixed to a stationary object and does not sway. On the other hand, even if the base is fixed to a moving object, the detection value corrector 313 may perform correction when it is determined that the base is not tilted with respect to the horizontal plane. For example, the base may also be provided with an acceleration sensor that detects acceleration including gravitational acceleration or an inclination sensor that detects inclination from the horizontal plane, and it may be determined whether the base is tilted with respect to the horizontal plane based on the direction of gravitational acceleration or the inclination from the horizontal plane. Alternatively, the detection value correction unit 313 may detect the inclination of the roll angle and the pitch angle of the base with respect to the horizontal plane using an acceleration sensor or an inclination sensor provided on the base, and add the inclination of the roll angle and the pitch angle with respect to the horizontal plane to the detected roll angle value φd and the detected pitch angle value θd. In this case, correction by the detection value correction unit 313 may be performed regardless of whether the base is inclined with respect to the horizontal plane.

[0105] 3. Embodiment 3 A spatial stabilization device 1 according to embodiment 3 will be described with reference to the drawings. Description of components similar to those of embodiment 1 or 2 will be omitted. The basic configuration of the spatial stabilization device 1 according to this embodiment is similar to that of embodiment 1 or 2, but part of the processing of the actuator control unit 34 differs from that of embodiment 1 or 2.

[0106] The disturbance torque Td (or disturbance force) acting on the driving force source of the actuator 51 (a motor in this example) is generated by frictional forces of the various moving parts, the axial load of the strut, and the cogging torque of the motor. Frictional forces include frictional components of the linear guide of the ball screw mechanism and frictional components of the rotary joints at both ends of the strut. The frictional force changes suddenly when the direction of extension and contraction of the actuator is reversed. Cogging torque generally exists in a frequency band higher than the current control band.

[0107] On the other hand, in a normal feedback control system, there is a limit to how high the gain of the control system can be, and the feedback control system alone cannot quickly cancel out nonlinear and fast-changing disturbance components such as frictional force and cogging torque.

[0108] Therefore, for each actuator 51, the actuator control unit 34 estimates the disturbance torque, which is the torque required to cancel out the disturbance load acting on the driving force source (in this example, the motor) of the actuator 51, based on the detection value of the second sensor 63, and supplies a current equivalent to the disturbance torque to the driving force source in a feedforward manner.

[0109] According to this configuration, by estimating the disturbance torque and supplying a current equivalent to the disturbance torque to the driving force source in a feedforward manner, it is possible to quickly cancel out disturbance components that cannot be canceled out quickly enough by the feedback control system of the driving force source, thereby improving the control performance of the actuator 51 and the control performance of the attitude state of the platform 54.

[0110] In this embodiment, as shown in FIG. 7, the actuator control section 34 includes a disturbance estimation section 344 for each actuator.

[0111] The disturbance estimation unit 344 detects a detected value of angular acceleration αmd based on a detected value of rotation angle θmd by the second sensor (rotation sensor). The detected value of rotation angle θmd is second-order differentiated to calculate a detected value of angular acceleration αmd. The disturbance estimation unit 344 estimates a disturbance torque Td acting on the motor based on the detected value of angular acceleration αmd and a detected value Imd of the current supplied to the motor by the second sensor (current sensor, rotation sensor) using the following equation. The disturbance estimation unit 344 then estimates a disturbance current Id, which is a current required to cancel out the disturbance torque Td, based on the disturbance torque Td.

[0112] Here, Km is a coefficient for converting the supply current into the output torque of the motor, and Jm is the inertia of a rotating member that rotates integrally with the motor. Instead of the detected value αmd of angular acceleration, an angular velocity obtained by equivalent conversion of a disturbance observer may be used.

[0113] The current control unit 343 adds the disturbance current Id calculated by the disturbance estimation unit 344 to the supply current command value Imc calculated by the speed control unit 342 to calculate a command value for the motor supply current after the addition, calculates a command value Vc for the motor's applied voltage so that the detected value Imd of the motor's supply current by the second sensor (current sensor) approaches the command value Imc+Id for the motor supply current after the addition, and applies a voltage to the motor based on the command value Vc for the motor's applied voltage.

[0114] According to this configuration, it is possible to estimate the disturbance torque Td due to nonlinear and fast-changing disturbances such as frictional force and cogging torque based on the detected value αmd of angular acceleration and the detected value Imd of the supply current, and to estimate the disturbance current Id. By supplying the disturbance current Id to the driving force source in a feedforward manner, it is possible to quickly cancel disturbance components that cannot be canceled sufficiently quickly by a feedback control system for speed control, for example, thereby improving the control performance of the actuator 51 and the control performance of the attitude state of the platform 54.

[0115] 4. Embodiment 4 A spatial stabilization device 1 according to embodiment 4 will be described with reference to the drawings. Description of the same components as those in embodiment 3 will be omitted. The basic configuration of the spatial stabilization device 1 according to this embodiment is the same as that of embodiment 3, but differs from embodiment 3 in that the disturbance estimation unit 344 of the actuator control unit 34 uses a command value Imc of the motor supply current instead of the detected value Imd of the motor supply current.

[0116] In this embodiment, the disturbance estimation unit 344 estimates the disturbance torque Td acting on the motor based on the detected angular acceleration value αmd and the command value Imc of the supply current of the motor calculated by the speed control unit 342. Then, the disturbance estimation unit 344 estimates the disturbance current Id, which is a current required to cancel out the disturbance torque Td, based on the disturbance torque Td. Here, the command value Imc of the supply current is a command value corresponding to the detected value Imd of the supply current.

[0117] Therefore, in equation (16), the motor supply current command value Imc calculated by the speed control unit 342 is used instead of the motor supply current detection value Imd. Because the supply current command value Imc is used, the current phase margin cannot be increased as much as when the supply current detection value Imd is used. However, the responsiveness of the feedback control system for current control is sufficiently faster than the responsiveness of the feedback control system for speed control. Therefore, by supplying the disturbance current Id estimated based on the supply current command value Imc to the driving force source in a feedforward manner, disturbance components that cannot be quickly canceled by the speed control feedback control system can be quickly canceled, as in the third embodiment. This improves the control performance of the actuator 51 and the control performance of the attitude state of the platform 54.

[0118] 5. Fifth Embodiment A spatial stabilizing device 1 according to a fifth embodiment will be described with reference to the drawings. Description of components similar to those of the first, second, third, or fourth embodiment will be omitted. The basic configuration of the spatial stabilizing device 1 according to this embodiment is similar to that of the first, second, third, or fourth embodiment, but differs from the first, second, third, or fourth embodiment in that the actuator control unit 34 uses an estimated value Ime of the motor supply current instead of a detected value Imd of the motor supply current.

[0119] As shown in FIG. 8, the current control unit 343 estimates an estimated value Ime of the motor supply current based on the detected value ωmd of the motor's angular velocity and the command value Vc of the motor's applied voltage.

[0120] In this embodiment, the estimation is performed using the following voltage / current equation for a brushed motor.

[0121] where Lm is the winding inductance, Rm is the winding resistance, Vm is the applied voltage, and Km is a coefficient for converting the supplied current into the motor's output torque.

[0122] The current control unit 343 uses the following equation, which is a modification of equation (17), to estimate the estimated value Ime of the motor supply current based on the detected value ωmd of the motor's angular velocity and the command value Vc of the motor's applied voltage. Here, the command value Vc of the applied voltage calculated in the previous calculation cycle is used as the command value Vc of the applied voltage. s is a Laplace operator.

[0123] Then, the current control unit 343 calculates a command value Vc of the motor's applied voltage so that the estimated value Ime of the motor's supply current approaches the command value Imc of the motor's supply current, and applies voltage to the motor based on the command value Vc of the motor's applied voltage.

[0124] For example, the current control unit 343 performs proportional-integral control based on the deviation between the supply current command value Imc and the supply current estimate value Ime to calculate the applied voltage command value Vc. Various types of feedback control other than proportional-integral control may also be used.

[0125] With this configuration, the current control accuracy deteriorates compared to a configuration that uses the detected value Imd of the supply current due to modeling errors such as changes in winding resistance caused by temperature changes. However, if the control accuracy of the entire control system can be maintained through feedback control of the angular velocity and feedback control of the expansion / contraction displacement, using this method can reduce the cost of the current sensor.

[0126] Alternatively, instead of performing feedback control using the estimated value Ime of the motor supply current, the current control unit 343 may use the following equation, which is a modification of equation (17), to calculate the command value Vc of the motor application voltage through feedforward control based on the command value Imc of the motor supply current and the detected value ωmd of the motor angular velocity. The command value Imc of the supply current after first-order lag processing representing the reference response may be used in equation (19).

[0127] When a brushless motor is used, the supply current is estimated using a known voltage-current equation for the brushless motor, or a feedforward control calculation is performed.

[0128] In the third embodiment, the disturbance estimating unit 344 may estimate the disturbance torque Td acting on the motor using the estimated value Ime of the supply current instead of the detected value Imd of the supply current.

[0129] 6. Sixth Embodiment A spatial stabilization device 1 according to a sixth embodiment will be described with reference to the drawings. A description of the same components as those in the third embodiment will be omitted. The basic configuration of the spatial stabilization device 1 according to this embodiment is the same as that in the third embodiment, but the disturbance estimation unit 344 uses the rotation angle θ of the motor instead of the detected value αmd of the angular acceleration of the motor and the detected value Imd of the supply current. m and angular velocity ω m This embodiment differs from the third embodiment in that the disturbance torque Td acting on the motor is estimated using the following equation:

[0130] As in the third embodiment, for each actuator 51, the actuator control unit 34 estimates the disturbance torque, which is the torque required to cancel out the disturbance load acting on the driving force source (in this example, the motor) of the actuator 51, based on the detection value of the second sensor 63, and supplies a current equivalent to the disturbance torque to the driving force source in a feedforward manner.

[0131] Unlike the third embodiment, as shown in Fig. 9, in this embodiment, the disturbance estimation unit 344 uses the following equation to estimate the disturbance torque Td acting on the motor based on the rotation angle detection value θmd and the angular velocity detection value ωmd detected by the second sensor (rotation sensor). Then, the disturbance estimation unit 344 estimates the disturbance current Id, which is a current required to cancel out the disturbance torque Td, based on the disturbance torque Td. Here, f t is a function that calculates the disturbance torque Td based on the detected value θmd of the rotation angle and the detected value ωmd of the angular velocity. Km is a coefficient that converts the supply current into the output torque of the motor.

[0132] According to this configuration, the cogging torque is proportional to the rotation angle θ m The friction force of a ball screw mechanism, etc., depends on the angular velocity ω mThe disturbance torque Td due to the cogging torque and friction force can be estimated based on the detected value θmd of the rotation angle and the detected value ωmd of the angular velocity. By calculating the disturbance current Id based on the disturbance torque Td and supplying the disturbance current Id to the driving force source in a feedforward manner, it is possible to quickly cancel disturbance components that cannot be canceled quickly by a feedback control system for speed control, for example, thereby improving the control performance of the actuator 51 and the control performance of the attitude state of the platform 54.

[0133] 7. Seventh Embodiment A spatial stabilization device 1 according to a seventh embodiment will be described with reference to the drawings. Description of components similar to those of any of the above-described first to sixth embodiments will be omitted. The basic configuration of the spatial stabilization device 1 according to this embodiment is similar to that of any of the first to sixth embodiments, but differs from any of the first to sixth embodiments in that the actuator control unit 34 performs filtering on the current command value.

[0134] For each actuator 51, the actuator control unit 34 calculates a current command value for the driving force source of the actuator 51 based on the expansion / contraction displacement command value Lc, performs filtering on the current command value to reduce the resonant frequency component of the mechanical system including the actuator, and supplies power to the driving force source based on the filtered current command value. For filtering, a notch filter or a low-pass filter that attenuates a frequency band including the resonant frequency of the mechanical system is used.

[0135] According to this configuration, it is possible to reduce the resonant frequency component of the mechanical system superimposed on the current command value, and it is possible to reduce vibration of the platform 54 .

[0136] In this embodiment, for each actuator 51, the speed control unit 342 performs filtering on the supply current command value Imc calculated based on the angular velocity detection value ωmd and the motor angular velocity command value ωmc to calculate the filtered supply current command value Imc. Then, for each actuator 51, the current control unit 343 calculates the applied voltage command value Vc based on the filtered supply current command value Imc and the supply current detection value Imd.

[0137] 8. Eighth Embodiment A spatial stabilization device 1 according to an eighth embodiment will be described with reference to the drawings. Description of components similar to those of any of the first to seventh embodiments will be omitted. The basic configuration of the spatial stabilization device 1 according to this embodiment is similar to that of any of the first to seventh embodiments, but the method of calculating the command value Zc for the position in the yaw axis direction in the attitude command calculation unit 32 differs from that of any of the first to seventh embodiments.

[0138] In this embodiment, the attitude estimation unit 31 estimates the attitude state by calculating the yaw axis Z P Detected acceleration value in the direction a z Integrate the yaw axis Z P Estimated velocity in the direction ve z Calculate the following.

[0139] Here, the detected acceleration value a z Although the estimated value of the position may be calculated by integrating twice, the detected value of acceleration a z The error included in is integrated twice, causing a significant drift in the estimated position value. Therefore, if control is performed based on the estimated position value, the control accuracy will deteriorate. z The estimated velocity ve obtained by integrating z By keeping the value within this range, the influence of drift is reduced and deterioration of control accuracy is suppressed.

[0140] In this embodiment, the attitude command calculation unit 32 calculates the estimated value ve of the velocity in the yaw axis direction. z is the target value of the velocity in the yaw axis direction, vo z The command value vc of the velocity of the platform in the yaw axis direction relative to the base is z is calculated, and the command value vc of the velocity in the yaw axis direction is zA phase delay process is performed on the yaw axis to calculate a command value Zc for the position in the yaw axis direction.

[0141] In this embodiment, the attitude command calculation unit 32 calculates the target value vo of the velocity in the yaw axis direction as shown in the following equation: z and the estimated value of the velocity in the yaw axis direction, ve z The command value vc of the velocity in the yaw axis direction is obtained by proportional control based on the deviation from z Here, Kp z is the proportional gain. For example, the target value of the velocity in the yaw axis direction, vo z is set to 0. The target value of the velocity in the yaw axis direction, vo z may be varied depending on the control objectives.

[0142] By performing proportional control and not performing integral control, it is possible to suppress the promotion of drift due to integral control.

[0143] The attitude command calculation unit 32 calculates the command value vc of the velocity in the yaw axis direction as shown in the following equation. z A phase lag filter process is performed on Zc to calculate a command value Zc for the position in the yaw axis direction. Here, fdly is a function that performs the phase lag filter process, and Zconst is a predetermined reference position.

[0144] Since the position is delayed by 90 degrees relative to the velocity, the command value of the velocity in the yaw axis direction, vc z By performing phase-lag filtering on the velocity, it is possible to calculate a value equivalent to the position in the yaw axis direction. Here, the phase lag in the phase-lag filtering is preferably set close to 90 degrees (for example, 90 degrees). Taking into account the gain characteristics and phase characteristics, a multi-stage connection of phase-lag compensators and a phase-lead compensator may be used in combination to perform phase-lag filtering, which can roughly express the physical phase relationship between velocity and position. By performing phase-lag filtering, it is possible to suppress the promotion of drift caused by the integral operation performed to calculate position from velocity.

[0145] From the above, the detected acceleration value a in the yaw axis direction is z While suppressing the occurrence of drift by integratingP It is also possible to suppress directional fluctuations.

[0146] 9. Ninth Embodiment A spatial stabilization device 1 according to a ninth embodiment will be described with reference to the drawings. Description of components similar to those of any of the above-described first to eighth embodiments will be omitted. The basic configuration of the spatial stabilization device 1 according to this embodiment is similar to that of any of the first to eighth embodiments, but the attitude estimation unit 31 is not provided, and the calculation processing of the attitude command calculation unit 32 differs from that of any of the first to eighth embodiments. Figure 10 shows a schematic block diagram of a control device 30 according to this embodiment.

[0147] In this embodiment, the attitude command calculation unit 32 calculates a command value for the attitude state of the platform relative to the base, which brings the detected value of the angular velocity of the attitude angle of the platform 54 detected by the angular velocity sensor 201 of the first sensor 20 closer to the target value of the angular velocity.

[0148] According to this configuration, the command value for the attitude state is calculated so that the detected value of the angular velocity of the attitude angle approaches the target value of the angular velocity without integrating the detected value of the angular velocity of the attitude angle. Therefore, there is no problem in that drift components and noise components contained in the detected value of the angular velocity of the attitude angle are integrated, causing drift in the estimated value of the angle. Therefore, the angular velocity of the attitude angle of the platform can be accurately controlled to the target value, and the angular velocity of the attitude angle of the platform can be stabilized even if the base oscillates.

[0149] As in the first embodiment, the angular velocity sensor 201 of the first sensor 20 detects an angular velocity ω around each of the three coordinate axes in an orthogonal coordinate system having three coordinate axes X, Y, and Z. x , ω y , ω z The three coordinate axes X, Y, and Z are the stabilization plane S of the platform. P Roll axis X parallel to P and pitch axis Y P , and the yaw axis Z perpendicular to the stabilization surface P It consists of:

[0150] The attitude command calculation unit 32 receives the roll angular velocity detection value ω x and the detected value of the pitch angular velocity ω yThe attitude command calculation unit 32 detects the roll angular velocity detection value ω x The target value of the roll angular velocity ωo x The roll angle command value φc of the platform relative to the base is calculated to approach the detected value ω of the pitch angular velocity. y is the target value of the pitch angular velocity ωo y A command value θc of the pitch angle of the platform relative to the base is calculated so as to approach the

[0151] For example, the attitude command calculation unit 32 calculates the target value ωo of the roll angular velocity as shown in the following equation. x and the detected value of the roll angular velocity ω x The roll angle command value φc is calculated by performing proportional-integral control based on the deviation from the target value ωo of the pitch angular velocity. y and the detected value of pitch angular velocity ω y A proportional-plus-integral control is performed based on the deviation from the reference value θc to calculate the command value θc of the pitch angle.

[0152] Here, Kp is a proportional gain, Ki is an integral gain, and s is a Laplace operator. Note that various types of feedback control other than proportional-integral control may also be used.

[0153] In this embodiment, the attitude command calculation unit 32 calculates the target value ωo of the roll angular velocity. x and the target value of the pitch angular velocity ωo y and are set to 0. The target value of the roll angular velocity ωo x and the target value of the pitch angular velocity ωo y may be set to a value other than 0 and may be changed depending on the control purpose.

[0154] With this configuration, the stabilizing surface S of the platform P Roll axis X parallel to P and pitch axis Y P Roll angular velocity ω x and pitch angular velocity ω y can be stabilized to a target value, and the stabilization surface S P This can suppress fluctuations in the attitude angular velocity.

[0155] The attitude command calculation unit 32 calculates a command value Zc for the position of the platform in the yaw axis direction relative to the base. In this embodiment, similar to the attitude estimation unit 31 in the eighth embodiment, the attitude command calculation unit 32 calculates a command value Zc for the position of the platform in the yaw axis direction relative to the base as an estimated value of the attitude state. P Detected acceleration value in the direction a z is integrated to obtain the estimated value of the velocity in the yaw axis direction, ve z The configuration is the same as that of the posture estimation unit 31 in the eighth embodiment, so a description thereof will be omitted.

[0156] Similar to the attitude command calculation unit 32 of the eighth embodiment, the attitude command calculation unit 32 calculates the estimated value ve of the velocity in the yaw axis direction. z is the target value of the velocity in the yaw axis direction, vo z The command value vc of the velocity of the platform in the yaw axis direction relative to the base is z is calculated, and the command value vc of the velocity in the yaw axis direction is z The attitude command calculation unit 32 performs a phase delay process on the attitude command value Zc to calculate the command value Zc for the position in the yaw axis direction. This has the same configuration as the attitude command calculation unit 32 in the eighth embodiment, so a description thereof will be omitted.

[0157] Alternatively, similarly to the first embodiment, the attitude command calculation unit 32 may set the command value Zc for the position in the yaw axis direction to a predetermined value Zconst.

[0158] The processing by the attitude command calculation unit 32, the processing by the expansion / contraction command calculation unit 33, and the processing by the actuator control unit 34 after calculating the roll angle command value φc, the pitch angle command value θc, and the position command value Zc in the yaw axis direction is the same as in any of the first to eighth embodiments, and therefore description thereof will be omitted.

[0159] Although various exemplary embodiments and examples are described in this disclosure, the various features, aspects, and functions described in one or more embodiments are not limited to the application of a particular embodiment, but may be applied to the embodiments alone or in various combinations. Therefore, countless variations not illustrated are contemplated within the scope of the technology disclosed in this disclosure specification. For example, this includes cases where at least one component is modified, added, or omitted, or where at least one component is extracted and combined with components of another embodiment.

[0160] 1: spatial stabilization device, 2: parallel mechanism, 20: first sensor, 30: control device, 31: attitude estimation unit, 32: attitude command calculation unit, 33: extension / contraction command calculation unit, 34: actuator control unit, 51: actuator, 52: strut, 53: base, 54: platform, 63: second sensor, 201: angular velocity sensor, 202: acceleration sensor, 341: position control unit, 342: velocity control unit, 343: current control unit, 344: disturbance estimation unit, Lc: extension / contraction displacement amount command value, Ld: extension / contraction displacement amount detection value, SP: stabilization surface, T d : disturbance torque, Vc: command value of applied voltage to the motor, X P : Roll axis, Y P : pitch axis, Z P : yaw axis, Xc: command value for position in the roll axis direction, Yc: command value for position in the pitch axis direction, Zc: command value for position in the yaw axis direction, a x : Detected value of acceleration in the roll axis direction, a y : Detected acceleration value in the pitch axis direction, a z : Detected value of acceleration in the yaw axis direction, g: Gravitational acceleration, vc z : command value of velocity in the yaw axis direction, ve z : Estimated value of velocity in the yaw axis direction, vo z : target value of velocity in the yaw axis direction, ψc: command value of yaw angle, ψe: estimated value of yaw angle, θc: command value of pitch angle, θd: detected value of pitch angle, θe: estimated value of pitch angle, θo: target value of pitch angle, φc: command value of roll angle, φd: detected value of roll angle, φe: estimated value of roll angle, φo: target value of roll angle, ωo x : target value of roll angular velocity, ωoy : target value of pitch angular velocity, ω x : Detected value of roll angular velocity, ω y : Detected value of pitch angular velocity, ω z : Detected value of yaw angular velocity

Claims

1. A parallel mechanism having a plurality of struts each extended and contracted by an actuator, a base to which one ends of the plurality of struts are rotatably connected and fixed to an object, and a platform to which the other ends of the plurality of struts are rotatably connected; a first sensor for detecting the state of the parallel mechanism; an attitude estimation unit for calculating an estimated value of the attitude state of the platform based on the state detected by the first sensor; an attitude command calculation unit for calculating a command value of the attitude state of the platform relative to the base so as to bring the estimated value of the attitude state closer to a target value of the attitude state; an expansion / contraction command calculation unit for calculating a command value of the expansion / contraction displacement amount of the actuator of each of the struts based on the command value of the attitude state; and an actuator control unit for driving and controlling each of the actuators based on the command value of the expansion / contraction displacement amount of each of the actuators, wherein the first sensor has an angular velocity sensor and an acceleration sensor for detecting acceleration including gravitational acceleration, and is fixed to the platform, The attitude estimation unit calculates an estimated value of the attitude angle of the platform based on the detected value of the angular velocity detected by the angular velocity sensor, and performs acceleration correction to correct the estimated value of the attitude angle based on the detected value of the acceleration sensor.

2. A spatial stabilization device according to claim 1, wherein the attitude estimation unit calculates an estimated value of the attitude angle by integrating the detected value of the angular velocity, and corrects the estimated value of the attitude angle so that the estimated direction of gravitational acceleration estimated from the estimated value of the attitude angle approaches the detected direction of the detected value of the acceleration.

3. A spatial stabilization device as described in claim 1 or 2, wherein the angular velocity sensor detects angular velocity around each of three coordinate axes in an orthogonal coordinate system, and the acceleration sensor detects acceleration in the direction of each of the coordinate axes, including gravitational acceleration.

4. The spatial stabilization device according to claim 3, wherein the attitude estimation unit calculates an estimate of the attitude angle of the platform relative to a horizontal plane; the attitude command calculation unit uses a target value of the attitude angle of the platform relative to a horizontal plane as a target value of the attitude state and calculates a command value of the attitude angle of the platform relative to the base as a command value of the attitude state; the attitude estimation unit calculates a differential value of the estimated value of the attitude angle based on the detected value of the angular velocity around each of the coordinate axes and calculates the estimated value of the attitude angle by integrating the differential value of the estimated value of the attitude angle; and changes the estimated value of the attitude angle so that the components of the gravitational acceleration in each of the coordinate axis directions, obtained by decomposing the gravitational acceleration into components in each of the coordinate axis directions based on the estimated value of the attitude angle, approach the detected value of the acceleration in each of the coordinate axis directions.

5. The spatial stabilization device described in claim 4, wherein the attitude estimation unit, in the acceleration correction, partially differentiates the deviation between the gravitational acceleration components in each of the coordinate axis directions, obtained by decomposing the gravitational acceleration into components in each of the coordinate axis directions based on the estimated value of the attitude angle, and the detected value of the acceleration in each of the coordinate axis directions, with respect to the estimated value of the attitude angle, calculates the gradient of the deviation with respect to a change in the estimated value of the attitude angle, and changes the estimated value of the attitude angle in a direction that reduces the deviation based on the gradient of the deviation.

6. A spatial stabilization device as described in claim 4 or 5, wherein the attitude estimation unit determines whether the base is stationary or moving at a constant speed based on the detected acceleration values ​​in each of the coordinate axis directions, and changes a correction gain that increases or decreases the magnitude of the correction amount for the estimated value of the attitude angle based on the determination result.

7. A spatial stabilization device according to any one of claims 1 to 6, wherein the actuator of each strut is equipped with a second sensor that detects the state of the actuator, and for each actuator, the actuator control unit detects a detected value of the expansion / contraction displacement amount of the actuator based on the detected state of the second sensor, and the attitude estimation unit uses forward kinematics of the parallel mechanism to calculate a detected value of the attitude state of the platform relative to the base based on the detected value of the expansion / contraction displacement amount of each actuator, and performs detection value correction to correct the estimated value of the attitude state based on the detected value of the attitude state.

8. A spatial stabilization device according to any one of claims 1 to 7, wherein the angular velocity sensor detects angular velocity around each of the coordinate axes in a Cartesian coordinate system having three coordinate axes, and the acceleration sensor detects acceleration in the direction of each of the coordinate axes, including gravitational acceleration, the three coordinate axes consisting of a roll axis and a pitch axis parallel to a stabilization plane of the platform, and a yaw axis perpendicular to the stabilization plane, the attitude estimation unit calculates, as the estimated values ​​of the attitude angles, estimated values ​​of the roll angle and the pitch angle of the platform relative to a horizontal plane, and the attitude command calculation unit uses, as the target values ​​of the attitude angles, a target value of the roll angle and a target value of the pitch angle of the platform relative to the horizontal plane, and calculates, as the command values ​​of the attitude state, a command value of the roll angle of the platform relative to the base that brings the estimated value of the roll angle closer to the target value of the roll angle, and calculates a command value of the pitch angle of the platform relative to the base that brings the estimated value of the pitch angle closer to the target value of the pitch angle.

9. A spatial stabilization device according to any one of claims 1 to 8, wherein the angular velocity sensor detects angular velocities around each of the coordinate axes in a Cartesian coordinate system having three coordinate axes, and the acceleration sensor detects accelerations in the directions of the coordinate axes, including gravitational acceleration, the three coordinate axes consisting of a roll axis and a pitch axis parallel to a stabilization plane of the platform, and a yaw axis perpendicular to the stabilization plane, the attitude command calculation unit calculates a command value for a roll angle of the platform relative to the base and a command value for a pitch angle of the platform relative to the base, which bring the estimated value of the attitude state closer to a target value of the attitude state, and calculates a command value for a position of the platform in the yaw axis direction relative to the base, and the extension / contraction command calculation unit calculates a command value for the extension / contraction displacement amount of the actuator of each of the struts based on at least the command value for the roll angle, the command value for the pitch angle, and the command value for the position in the yaw axis direction.

10. Three struts are provided, the rotary joint rotatably connecting one end of each strut to the base is a rotary joint rotatable around one axis, and the rotary joint rotatably connecting the other end of each strut to the platform is a rotary joint rotatable around three axes, the angular velocity sensor detects angular velocities around each of the coordinate axes in an orthogonal coordinate system having three coordinate axes, and the acceleration sensor detects acceleration in each of the coordinate axis directions including gravitational acceleration, the three coordinate axes consisting of a roll axis and a pitch axis parallel to a stabilization plane of the platform, and a yaw axis perpendicular to the stabilization plane, the attitude command calculation unit calculates a command value for the roll angle of the platform relative to the base and a command value for the pitch angle of the platform relative to the base that bring the estimated value of the attitude state closer to a target value of the attitude state, and calculates a command value for the position of the platform in the yaw axis direction relative to the base, the expansion / contraction command calculation unit calculates a command value for a yaw angle of the platform relative to the base, a command value for a position in the roll axis direction of the platform relative to the base, and a command value for a position in the pitch axis direction of the platform relative to the base, based on the command value for the roll angle, the command value for the pitch angle, and the command value for a position in the yaw axis direction; and the expansion / contraction command calculation unit calculates a command value for an expansion / contraction displacement amount of the actuator of each of the struts, based on the command value for the roll angle, the command value for the pitch angle, the command value for the position in the yaw axis direction, the command value for the yaw angle, the command value for the position in the roll axis direction, and the command value for the position in the pitch axis direction.

11. A spatial stabilization device as described in claim 9 or 10, wherein the attitude estimation unit calculates an estimated value of the velocity in the yaw axis direction by integrating the detected value of acceleration in the yaw axis direction as an estimated value of the attitude state, and the attitude command calculation unit calculates a command value of the velocity in the yaw axis direction of the platform relative to the base, which brings the estimated value of the velocity in the yaw axis direction closer to a target value of the velocity in the yaw axis direction, and performs phase delay processing on the command value of the velocity in the yaw axis direction to calculate a command value of the position in the yaw axis direction.

12. A spatial stabilization device as described in claim 11, wherein the attitude command calculation unit calculates a command value for the velocity in the yaw axis direction by performing proportional control based on the deviation between the target value for the velocity in the yaw axis direction and the estimated value for the velocity in the yaw axis direction.

13. A spatial stabilization device comprising: a parallel mechanism having a plurality of struts each extended and contracted by an actuator, a base to which one ends of the plurality of struts are rotatably connected and fixed to an object, and a platform to which the other ends of the plurality of struts are rotatably connected; a first sensor having an angular velocity sensor and fixed to the platform; an attitude command calculation unit that calculates a command value for the attitude state of the platform relative to the base so that a detected value of the angular velocity of the attitude angle of the platform detected by the angular velocity sensor approaches a target value of the angular velocity; an expansion / contraction command calculation unit that calculates a command value for the expansion / contraction displacement amount of the actuator of each of the struts based on the attitude command value; and an actuator control unit that drives and controls each of the actuators based on the command value for the expansion / contraction displacement amount of each of the actuators.

14. A spatial stabilization device as described in claim 13, wherein the angular velocity sensor detects angular velocities around each of the coordinate axes in a Cartesian coordinate system having three coordinate axes, the three coordinate axes consisting of a roll axis and a pitch axis parallel to a stabilization plane of the platform, and a yaw axis perpendicular to the stabilization plane, the attitude command calculation unit detects a detected value of roll angular velocity and a detected value of pitch angular velocity using the angular velocity sensor, calculates a command value for a roll angle of the platform relative to the base that brings the detected value of roll angular velocity closer to a target value of roll angular velocity, calculates a command value for a pitch angle of the platform relative to the base that brings the detected value of pitch angular velocity closer to a target value of pitch angular velocity, and calculates a command value for a position of the platform in the yaw axis direction relative to the base, and the extension / contraction command calculation unit calculates a command value for the extension / contraction displacement amount of the actuator of each of the struts based on at least the command value for the roll angle, the command value for the pitch angle, and the command value for the position in the yaw axis direction.

15. A spatial stabilization device as described in claim 14, wherein the first sensor has an acceleration sensor that detects acceleration in each of the coordinate axis directions, and the attitude command calculation unit detects a detected value of acceleration in the yaw axis direction using the acceleration sensor, integrates the detected value of acceleration in the yaw axis direction to calculate an estimated value of velocity in the yaw axis direction, calculates a command value for the velocity in the yaw axis direction of the platform relative to the base, brings the estimated value of velocity in the yaw axis direction closer to a target value of velocity in the yaw axis direction, and performs phase delay processing on the command value for velocity in the yaw axis direction to calculate a command value for position in the yaw axis direction.

16. A spatial stabilization device according to claim 15, wherein the attitude command calculation unit calculates a command value for the velocity in the yaw axis direction by performing proportional control based on the deviation between the target value for the velocity in the yaw axis direction and the estimated value for the velocity in the yaw axis direction.

17. The actuator of each strut comprises a motor, a conversion mechanism that converts the rotational motion of the motor into translational motion to expand and contract the strut, and a second sensor that detects the state of the motor, and for each actuator, the actuator control unit comprises: a position control unit that calculates a detected value of the expansion / contraction displacement of the actuator based on the value of the rotation angle of the motor detected by the second sensor, and calculates a command value for the angular velocity of the motor so that the detected value of the expansion / contraction displacement of the actuator approaches a command value for the expansion / contraction displacement of the actuator; and a speed control unit that calculates a detected value of the angular velocity of the motor based on the value of the rotation angle of the motor detected by the second sensor, and calculates a command value for the supply current of the motor so that the detected value of the angular velocity of the motor approaches a command value for the angular velocity of the motor. a current control unit that calculates a command value for the motor's applied voltage so that an estimated value of the motor's supply current estimated based on a detection value of the motor's supply current by the second sensor or a detection value of the motor's angular velocity and a command value for the motor's applied voltage approaches the command value for the motor's supply current, and applies a voltage to the motor based on the command value for the motor's applied voltage.

18. A spatial stabilization device as described in any one of claims 1 to 17, wherein the actuator of each strut is equipped with a second sensor that detects the state of the actuator, and for each actuator, the actuator control unit estimates a disturbance torque, which is the torque required to cancel out a disturbance load acting on the driving force source of the actuator, based on the detection value of the second sensor, a command value corresponding to the detection value of the second sensor, or an estimated value corresponding to the detection value of the second sensor, and supplies a current equivalent to the disturbance torque to the driving force source in a feedforward manner.

19. A spatial stabilization device as described in any one of claims 1 to 18, wherein for each of the actuators, the actuator control unit calculates a current command value for the driving force source of the actuator based on the command value for the expansion / contraction displacement amount, performs filtering on the current command value to reduce the resonant frequency components of the mechanical system including the actuator, and supplies current to the driving force source based on the current command value after filtering.

Images