Gravity unloading device and method for space rotating mechanism based on Kalman filter

Abstract

The present invention provides a space rotating mechanism gravity unloading device and method based on Kalman filtering, belonging to the field of gravity unloading or servo control. The position encoder located on the yaw axis of the turntable of the present invention measures the yaw attitude angle, and the yaw attitude angle is subjected to Kalman filtering by an industrial computer to eliminate the interference of delay and noise; the suspension wire tension value calculated by the gravity of the turntable load planned to be unloaded is used as the control instruction of the Z-axis motor, and the suspension wire tension is always kept the same as the Z-axis motor control instruction through the tension sensor on the suspension wire; when the gravity of the turntable load is unloaded, the tension of the suspension wire is locked and controlled by sliding mode control. The space rotating mechanism gravity unloading device and method based on Kalman filtering proposed by the present invention solves the problem of difficulty in setting up a gravity unloading environment for a two-dimensional rotating mechanism. At the same time, the device and method can bear a heavy turntable load, have the advantages of rapid tracking, high unloading accuracy, etc.

Description

Technical Field

[0001] The invention relates to a gravity unloading device and method for a space rotating mechanism based on Kalman filtering, and belongs to the field of gravity unloading or servo control. Background Art

[0002] The patent "An Active Gravity Unloading Device" (invention patent, Hou Peng, 20180504) proposes an active gravity unloading device that can realize ground gravity compensation for large-scale space deployable mechanisms and simulate the state of weightlessness in space. Although it can meet the ground gravity unloading of large-scale multi-joint space deployable mechanisms and has the advantages of small size, light weight and large motion range, it reduces the operability and maintainability of the system and increases the cost of the system due to the complex control system and cumbersome mechanical structure.

[0003] In the paper “Design of Active Gravity Compensation System for Zero-Stiffness Hanging Wire Suspension” (The 9th China Multi-Agent System and Control Conference, Jia Jiao, 20140726), a zero-stiffness hanging wire suspension active gravity compensation system was designed to fully compensate for the gravity exerted on the experimental spacecraft. Although the horizontal delay, zero stiffness in the vertical direction, disturbance of the wire rope and the influence of the suspension device on the experimental spacecraft were taken into account, the delay of the two-dimensional inclination sensor output deviation from the equilibrium position angle information and the interference of noise were not taken into account, so high tracking accuracy and compensation precision were not achieved.

[0004] The paper "Analysis of Gravity Unloading Accuracy of Air Flotation Test Bench" (Journal of Mechanical Engineering, Vol. 55, No. 5, Yang Guoyong, 20190520) designed an air flotation test bench that can achieve graded and synchronous gravity unloading, and ensured that the two axes can rotate simultaneously in a microgravity environment. Although the gravity of the two joints of the drive mechanism is unloaded separately, the method used is the air flotation method, which limits the weight of the load and cannot meet the needs of large loads.

[0005] Based on this, this patent proposes a gravity unloading device and method for a space rotating mechanism based on Kalman filtering. First, a gravity unloading device for a space rotating mechanism is proposed, which can not only perform high-precision posture tracking but also unload the turntable load with constant gravity. Compared with traditional methods, it has the characteristics of simple structure, high operability and maintainability. After that, a gravity unloading method for a space rotating mechanism based on Kalman filtering is proposed. This method enables the posture measurement information to have the advantages of strong anti-interference ability, strong robustness, and elimination of noise influence after passing through Kalman filtering. Finally, in view of the nonlinearity caused by the servo motor model and the slight torsion of the suspension line, a sliding mode control algorithm is used to control the system. This method has the advantages of high posture tracking accuracy, strong load-bearing capacity, simple control system and mechanical structure, and can meet the requirements of gravity unloading of space rotating mechanisms. Summary of the invention

[0006] The purpose of the present invention is to solve the problems existing in the above-mentioned prior art and further provide a gravity unloading device and method for a space rotating mechanism based on Kalman filtering.

[0007] The objective of the present invention is achieved through the following technical solutions:

[0008] A Kalman filter-based gravity unloading device for a space rotating mechanism, comprising a servo motor, a Z-axis motor, a rotating platform, a tension sensor, a suspension line, a turntable load, a turntable and a truss, wherein the turntable load is fixed on both sides of the turntable, the servo motor is fixed under the truss, the shaft of the servo motor is vertically fixed to the rotating platform, the Z-axis motor is fixed at a position on the rotating platform corresponding to the turntable load, and the Z-axis motor and the turntable load are connected via the suspension line and the tension sensor.

[0009] The present invention discloses a gravity unloading device for a space rotating mechanism based on Kalman filtering, wherein the Z-axis motors fixed on the rotating platform are arranged back to back.

[0010] The invention discloses a gravity unloading device for a space rotating mechanism based on Kalman filtering. The turntable adopts a T-type turntable, and the T-type turntable drives the turntable load to perform two-degree-of-freedom motion of pitch and yaw.

[0011] The present invention discloses a gravity unloading device for a space rotating mechanism based on Kalman filtering, wherein both the servo motor and the Z-axis motor are permanent magnet synchronous motors.

[0012] A gravity unloading method for a space rotating mechanism based on Kalman filtering, the specific steps of the gravity unloading method for a space rotating mechanism based on Kalman filtering are:

[0013] Step 1: When the gravity unloading device of the space rotating mechanism based on Kalman filtering described in claims 1-4 is working, the position encoder located on the yaw axis of the turntable measures the yaw attitude angle, and the yaw attitude angle is subjected to Kalman filtering by an industrial computer to eliminate the interference of delay and noise;

[0014] Step 2: The tension value of the suspension wire calculated from the gravity of the turntable load to be unloaded is used as the control instruction of the Z-axis motor, and the tension sensor on the suspension wire is used to ensure that the tension of the suspension wire is always the same as the control instruction of the Z-axis motor;

[0015] Step 3: When the gravity of the turntable load is unloaded, the sliding mode control is used to lock the tension of the suspension wire.

[0016] The present invention discloses a gravity unloading method for a space rotating mechanism based on Kalman filtering, which is characterized in that the Kalman filtering specifically comprises: operating the optimal value obtained previously with the current measured value to calculate the Kalman gain, then taking out the last estimated value and the sensor value according to the corresponding proportion, obtaining the output value this time, and updating the Kalman error proportional coefficient.

[0017] The present invention provides a gravity unloading method for a space rotating mechanism based on Kalman filtering, wherein the specific process of step three is as follows:

[0018] First, the model of the permanent magnet synchronous motor is established. Formula (1) is the voltage equation:

[0019]

[0020] in

[0021]

[0022] Formula (2) is the magnetic flux equation

[0023]

[0024] Formula (3) is the electromagnetic torque equation

[0025] T e =1.5n p (ψ α i β -ψ β i α ) (3)

[0026] Formula (4) is the square equation of the magnetic flux amplitude

[0027]

[0028] in,

[0029] θ e ——the angle between the d-axis and the α-axis;

[0030] R s ——stator resistance;

[0031] ψ f ——permanent magnet flux;

[0032] ω e ——rotor electrical angular velocity;

[0033] n p ——Pole pair number

[0034] D——differential operator;

[0035] L d , Lq ——d-axis and q-axis inductance;

[0036] i d ,i q ——d-axis and q-axis current;

[0037] ψ α , ψ β ——α and β components of stator flux;

[0038] u α ,u β ——α and β components of stator voltage;

[0039] i α ,i β ——α and β components of stator current;

[0040] The sliding mode variable can be expressed as (5):

[0041]

[0042] Where: e T =T * -T K1, K2 are coefficients, T * is the torque given value, T is the torque estimated value, ψ * is the given value of magnetic flux, ψ is the estimated value of magnetic flux;

[0043] Differentiating equation (5) yields equation (6):

[0044]

[0045] To design the sliding mode control algorithm, equation (7) can be assumed to be valid.

[0046]

[0047] From equation (1) to equation (4), we can know Satisfying formula (8)

[0048]

[0049] in

[0050]

[0051] (Does R need to be changed to R s )

[0052] A1=-1.5n p ψ α / L d [ω e (L d -Lq )i α -Ri β -E β ]-1.5n p ψ β / L d [ω e (L d -L q )i β +Ri α +E α ]+K1e T

[0053]

[0054]

[0055] By solving equation (8), we can get the sliding mode controller equation (9):

[0056]

[0057] The present invention discloses a gravity unloading device and method for a space rotating mechanism based on Kalman filtering:

[0058] The proposed gravity unloading device for space rotating mechanism can perform gravity unloading on space two-dimensional rotating mechanism. Compared with the traditional gravity unloading device for space unfolding mechanism, it has the advantages of wide application range, simple mechanical structure and strong maintainability.

[0059] The proposed gravity unloading method for spatial rotating mechanism based on Kalman filtering can perform high-precision follow-up tracking of yaw posture. Compared with the traditional two-dimensional tracking method, it can eliminate the delay of sensor measurement information and the influence of mixed noise, so that the tracking accuracy is higher and gravity unloading can be achieved with higher precision.

[0060] The proposed Kalman filter-based gravity unloading device and method for a spatial rotating mechanism solves the problem of difficulty in setting up a gravity unloading environment for a two-dimensional rotating mechanism. At the same time, the device and method can carry a heavy turntable load and have the advantages of rapid tracking and high unloading accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 It is a structural schematic diagram of the gravity unloading device of the space rotating mechanism based on Kalman filtering of the present invention.

[0062] Figure 2 The present invention is a Kalman filter algorithm flow chart of the gravity unloading method of the space rotating mechanism based on Kalman filter.

[0063] Figure 3This is a position relationship diagram of the sliding surface and three types of motion points in the gravity unloading method of the space rotating mechanism based on Kalman filtering of the present invention.

[0064] The reference numerals in the figure are: 1 is a servo motor; 2 is a Z-axis motor; 3 is a rotating platform; 4 is a tension sensor; 5 is a suspension wire; 6 is a turntable load; 7 is a turntable; and 8 is a truss. DETAILED DESCRIPTION

[0065] The present invention will be further described in detail below in conjunction with the accompanying drawings: This embodiment is implemented on the premise of the technical solution of the present invention, and a detailed implementation method is given, but the protection scope of the present invention is not limited to the following embodiments.

[0066] Embodiment 1: Figure 1-2 As shown, the present embodiment involves a gravity unloading device and method for a space rotating mechanism based on Kalman filtering. The gravity unloading device for a space rotating mechanism based on Kalman filtering is composed of a servo motor, a Z-axis motor, a rotating platform, a tension sensor, a suspension wire, a turntable load, a turntable, a truss, etc. The T-type turntable can drive the load to move in two degrees of freedom, pitch and yaw. The servo motor performs real-time angle tracking of the yaw motion of the turntable through a position encoder located on the yaw axis of the turntable, so that the unloading suspension wire of the turntable load always remains in a vertical state; at the same time, the suspension wire tension value calculated from the planned unloading load gravity is used as the control instruction of the Z-axis motor, and the tension sensor on the suspension wire is used to ensure that the suspension wire tension is always kept the same as the Z-axis motor control instruction. The turntable load can be unloaded while the two-dimensional rotation of the turntable is realized, and its device is as follows Figure 1 The actuator for tracking the yaw motion of the turntable is a servo motor, and the actuator for unloading the gravity of the turntable is a Z-axis motor located on the rotating platform. Both motors are permanent magnet synchronous motors.

[0067] First, the yaw attitude tracking solution is given:

[0068] The position encoder located on the yaw axis of the turntable measures the yaw attitude angle. Due to delay and noise interference, it cannot be directly used as a control instruction to control the servo motor to track the yaw attitude of the turntable. The yaw attitude angle needs to be passed through an industrial computer for Kalman filtering to eliminate delay and noise interference.

[0069] In the problem of posture tracking, in order to solve the delay problem of the values ​​collected by the sensor, Kalman filtering is generally used to perform filtering and prediction. The noise of the measured value is filtered out through its output value, and the motion state of the controlled object is predicted to a certain extent to ensure the accuracy of tracking control.

[0070] Next, the process of the Kalman filter algorithm is given:

[0071] Kalman filtering is to obtain the optimal value of the current posture of the system in real time by combining the collected sensor measurements with the linear state equation. Kalman filtering calculates the Kalman gain by calculating the previously obtained optimal value and the current measurement value, and then takes the last estimated value and the sensor value according to the corresponding proportion to obtain the output value this time, and updates the Kalman error proportional coefficient. The operation process of the Kalman filter algorithm is as follows: Figure 2 shown.

[0072] Embodiment 2: Figure 2 As shown, the present embodiment involves a gravity unloading method for a space rotating mechanism based on Kalman filtering. In order to describe the Kalman filtering algorithm, it is assumed that there is a linear system described by the following differential equation.

[0073] x(k)=Ax(k-1)+Bu(k)+w(k) (1)

[0074] z(k)=Hx(k)+v(k) (2)

[0075] x(k) and x(k-1) represent the state variables at the kth moment and the k-1th moment, u(k) represents the control quantity for the input of the whole system at the kth moment, z(k) represents the value obtained by the system from the sensor at the kth moment, and the three coefficients A, B, and H represent the state matrix, control matrix, and measurement matrix respectively. w(k) and v(k) in equations (1) and (2) are generally used as Gaussian white noise as interference of the whole system, and their covariance is generally represented by Q and R in equations (4) and (5).

[0076] With the state equation of the linear system, we can apply Kalman filtering to it. After derivation, this method mainly consists of five formulas. The first two formulas are used to predict the state of the system, and the last three formulas are used to calculate the current output and update some variables in the formula based on the prediction and the previous error matrix.

[0077] Predict the state at moment k from moment k-1:

[0078] x(k|k-1)=Ax(k-1|k-1)+Bu(k) (3)

[0079] Estimate the kth moment from the error estimate covariance matrix at the k-1th moment:

[0080] P(k|k-1)=A*P(k-1|k-1)*A T +Q (4)

[0081] Calculate the Kalman gain matrix:

[0082]

[0083] Estimate the optimal value at the current moment based on the previous two predicted values:

[0084] x(k|k)=x(k|k-1)+Kg(k)*(z(k)-Hx(k|k-1)) (6)

[0085] Update the error covariance matrix to the kth moment:

[0086] P(k|k)=(I-Kg(k)*H)*P(k|k-1) (7)

[0087] The basic Kalman filter algorithm described above is based on the assumption that the system being processed is a linear system, that is, the state transfer matrix and the measurement matrix in formula (2) are both constants, not time-dependent variables.

[0088] Embodiment 3: Figure 1 and 3 As shown, the present embodiment involves a method for gravity unloading of a space rotating mechanism based on Kalman filtering and a method for controlling the constant tension of a suspension line:

[0089] In order to solve the problems of elastic deformation of the suspension wire, tiny torsion and model nonlinearity caused by the servo motor, unknown model parameters, etc., and because sliding mode control has strong robustness, is extremely insensitive to parameter perturbations and external interference, and the control law design process is simple, sliding mode control is proposed to lock the tension of the suspension wire.

[0090] First we need to give a mathematical definition of sliding mode variable structure control. Consider an arbitrary nonlinear system, expressed as:

[0091]

[0092] For the state space of the system shown in equation (8), there exists a sliding surface

[0093] s(x)=s(x1,x2,…,x n )=0 (9)

[0094] And three types of motion points: usual point A, starting point B and end point C. Figure 3 shown.

[0095] For the system given by equation (8), to design the sliding mode control law, we first need to design the sliding surface

[0096] s(x) s∈R n (10)

[0097] Then design the control law

[0098]

[0099] where u + (x)≠u - (x) and meets the following conditions:

[0100] (1) There is a sliding surface;

[0101] (2) Any initial state point in the state space can slide to the sliding surface s = 0 in a finite time;

[0102] (3) The system meets the stability requirement.

[0103] Permanent magnet synchronous motors have the advantages of good torque response and high reliability. Therefore, in situations where unloading gravity requires high transient torque response, permanent magnet synchronous motors are used as actuators to control the motor torque to control the unloading gravity.

[0104] The above are only preferred specific embodiments of the present invention, which are all different implementations based on the overall concept of the present invention, and the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed by the present invention should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A gravity unloading device for a space rotating mechanism based on Kalman filtering, characterized in that: The Kalman filter-based spatial rotating mechanism gravity unloading device comprises a servo motor (1), a Z-axis motor (2), a rotating platform (3), a tension sensor (4), a suspension wire (5), a turntable load (6), a turntable (7) and a truss (8); the turntable load (6) is fixed on both sides of the turntable (7); the servo motor (1) is fixed under the truss (8); the shaft of the servo motor (1) is vertically fixed to the rotating platform (3); the Z-axis motor (2) is fixed at a position on the rotating platform (3) corresponding to the turntable load (6); the Z-axis motor (2) and the turntable load (6) are connected via the suspension wire (5) and the tension sensor (4); the Z-axis motor (2) fixed on the rotating platform (3) is arranged back to back; the turntable (7) adopts a T-type turntable, which drives the turntable load (6) to move in two degrees of freedom, namely pitch and yaw; and the servo motor (1) and the Z-axis motor (2) both adopt permanent magnet synchronous motors.

2. A gravity unloading method for a space rotating mechanism based on Kalman filtering, characterized in that: The specific steps of the gravity unloading method for a space rotating mechanism based on Kalman filtering are as follows: Step 1: When the gravity unloading device of the space rotating mechanism based on Kalman filtering as claimed in claim 1 is working, the position encoder located on the yaw axis of the turntable (7) measures the yaw attitude angle, and the yaw attitude angle is subjected to Kalman filtering by an industrial computer to eliminate the interference of delay and noise; Kalman filtering is specifically as follows: the previously obtained optimal value is operated with the current measured value to calculate the Kalman gain, and then the last estimated value and the sensor value are taken out according to the corresponding proportion to obtain the output value this time, and the Kalman error proportional coefficient is updated; Step 2: Using the tension value of the suspension wire calculated from the gravity of the turntable load (6) to be unloaded as the control instruction of the Z-axis motor (2), and using the tension sensor (4) on the suspension wire to ensure that the tension of the suspension wire is always the same as the control instruction of the Z-axis motor (2); Step 3: When the gravity of the turntable load (6) is unloaded, the sliding mode control is used to lock the tension of the suspension wire, specifically: First, the model of the permanent magnet synchronous motor is established. Formula (1) is the voltage equation: in Formula (2) is the magnetic flux equation Formula (3) is the electromagnetic torque equation T e =1.5n p (ψ α I β -ψ β I α ) (3) Formula (4) is the square equation of the magnetic flux amplitude in, θ e ——the angle between the d-axis and the α-axis; R s ——stator resistance; ψ f ——permanent magnet flux; ω e ——rotor electrical angular velocity; n p ——Pole pair number D——differential operator; L d ,L q ——d-axis and q-axis inductance; i d ,i q ——d-axis and q-axis current; ψ α ,ψ β ——α and β components of stator flux; u α ,u β ——α and β components of stator voltage; i α ,i β ——α and β components of stator current; The sliding mode variable can be expressed as (5): Where: e T =T * -T, K1, K2 are coefficients, T * is the torque given value, T is the torque estimated value, ψ * is the given value of magnetic flux, ψ is the estimated value of magnetic flux; Differentiating equation (5) yields equation (6): To design the sliding mode control algorithm, we can make (7) valid. From equation (1) to equation (4), we can know Satisfying formula (8) in A1=-1.5n p ψ α / L d [oh e (L d -L q )i α -Ri β -E β ]-1.5n p ψ β / L d [oh e (L d -L q )i β +Ri α +E α ]+K1e T By solving equation (8), we can get the sliding mode controller equation (9):

Images