An active vibration isolation method based on Stewart platform

Abstract

The present invention discloses an active vibration isolation method based on a Stewart platform, comprising: based on the kinematic and dynamic models pre-established for the Stewart parallel platform, collecting the posture signal of the upper platform at the current moment and the displacement and velocity signals of each leg; calculating the required displacement and velocity of each leg through an inverse kinematics algorithm according to the posture signal of the upper platform and the expected posture quaternion; calculating the displacement and velocity errors of each leg and performing nonlinear processing; dynamically correcting the control gain of the S-NPD motion controller to obtain the corrected proportional gain and differential gain of each leg at the current moment; for each leg, the nonlinear processing results of the displacement error and the nonlinear processing results of the velocity error and the corrected proportional gain and differential gain of the leg at the current moment are input into the S-NPD motion controller to obtain the control force; repeating until the preset number of moments is accumulated and then stopping. The present invention can ensure that the upper platform remains relatively stable when the lower platform is subjected to different disturbances, and achieve good control.

Description

Technical Field

[0001] The invention belongs to the field of vibration isolation, and in particular relates to an active vibration isolation method based on a Stewart platform. Background Art

[0002] The Stewart platform, also known as the Stewart parallel platform, has a high response speed and a large range of motion. Using it for vibration isolation is one of the most important applications at present. The Stewart platform consists of two platforms, the upper and lower platforms, connected by six legs. The lower platform is connected to the external structure and receives disturbances, and the upper platform bears the required load and provides a relatively stable posture. The legs are driven by a controller, which calculates the required extension of the legs through inverse kinematics, and controls the telescopic movement of the legs through algorithms to regulate the posture and angle of the upper platform, thereby achieving the purpose of vibration isolation and vibration suppression.

[0003] The traditional PID (proportional-integral-derivative control) algorithm is generally used to drive and control the Stewart platform. However, with the increasing requirements for vibration isolation accuracy in space missions, the system structure is becoming more and more complex, and the strong nonlinearity, strong coupling and uncertainty it contains are also gradually increasing. In the face of these problems, the performance of PID is severely restricted. In some high-precision working conditions, the PID algorithm can no longer meet the performance requirements.

[0004] In existing research and applications, modern control theory is usually used to replace traditional PID control, which improves the control efficiency of the system to a certain extent, but at the same time has the following disadvantages:

[0005] 1. Modern control theory usually has extremely high requirements for model accuracy, and strong nonlinearity and uncertainty conditions will greatly increase the difficulty of obtaining this information; 2. When the accuracy of the known model decreases, the control effect of modern control theory will be greatly reduced, and even normal driving cannot be completed; 3. Modern control theory excludes the excellent characteristics that are easy to achieve in PID control, increases the complexity of the structure, and to a certain extent makes the application occasions narrower. Summary of the invention

[0006] In order to solve the above problems existing in the prior art, the present invention provides an active vibration isolation method based on the Stewart platform. The technical problem to be solved by the present invention is achieved through the following technical solutions:

[0007] An active vibration isolation method based on a Stewart platform is applied to a Stewart platform system, wherein the Stewart platform system includes a Stewart parallel platform and an S-NPD motion controller, and the method includes:

[0008] Step 1, based on the kinematic model and dynamic model pre-established for the Stewart parallel platform, collecting the posture signal of the upper platform and the displacement and velocity signals of each leg in the Stewart parallel platform at the current moment;

[0009] Step 2, according to the posture signal of the upper platform and the expected posture quaternion, the required displacement and speed of each leg are calculated by an inverse kinematics algorithm;

[0010] Step 3, calculating the displacement error and velocity error of each leg according to the collected displacement and velocity signals of each leg and the calculated displacement and velocity required by each leg;

[0011] Step 4, performing nonlinear processing on the displacement error and the speed error of each leg to obtain the nonlinear processing result of the displacement error and the speed error of each leg;

[0012] Step 5, dynamically correcting the control gain of the S-NPD motion controller according to the calculated displacement error and velocity error of each leg, to obtain the corrected proportional gain and differential gain of each leg at the current moment;

[0013] Step 6: for each leg, the nonlinear processing result of the displacement error and the speed error of the leg, as well as the corrected proportional gain and differential gain of the leg at the current moment are input into the S-NPD motion controller to obtain the control force of the leg, and the control force of each leg is used to realize the motion control of the corresponding leg at the current moment;

[0014] Step 7, repeat steps 1 to 6 until the preset number of moments is reached.

[0015] In one embodiment of the present invention, the process of pre-establishing a kinematic model for the Stewart parallel platform includes:

[0016] Step a1: Define the global coordinate system and determine the position vectors at the hinge points of the upper and lower platforms:

[0017] r pi =r p +p i ,r bi =r b +b i ;

[0018] Among them, in the initial state, the mass center of the lower platform is located at the origin of the global coordinate system; the mass center coordinate system of the upper platform and the mass center coordinate system of the lower platform are fixed to the upper and lower platforms respectively; in the initial state, the directions of the coordinate axes of the mass center coordinate systems of the upper and lower platforms are consistent with the global coordinate system;pi is the position vector of the hinge point where the upper platform is connected to leg i; r p is the position vector from the origin of the coordinate system to the geometric centroid P of the upper platform; p i is the vector from the centroid of the upper platform to the position of the outrigger i and the upper platform constraint; r bi is the position vector of the hinge point where the lower platform is connected to leg i; r b is the position vector from the origin of the coordinate system to the geometric centroid B of the lower platform; b i is the vector from the centroid of the lower platform to the position of the leg i and the constraint of the lower platform;

[0019] Step a2, determine the leg i vector pointing from the lower platform hinge point to the upper platform hinge point:

[0020] l i =r pi -r bi =r p +p i -r b -b i ;

[0021] Step a3, calculate the leg vector change rate to represent the leg speed, expressed as:

[0022]

[0023] Among them, e i is the unit vector of the normalized leg i vector; is the velocity at the hinge point of the upper platform; ω p is the angular velocity of the upper platform; is the velocity at the hinge point of the lower platform; ω b is the rotation angular velocity of the lower platform; J pi is the Jacobian matrix of the upper platform corresponding to leg i, J bi is the Jacobian matrix of the lower platform corresponding to leg i;

[0024] J p and J b are the Jacobian matrices of the upper and lower platforms respectively; For J p The coefficients of different legs in For J b The coefficients of different legs in ;

[0025] Step a4, calculate the second-order differential of the leg vector to represent the acceleration of the leg, expressed as:

[0026]

[0027] Among them, α pis the acceleration of the upper platform; α b is the lower platform acceleration.

[0028] In one embodiment of the present invention, the process of establishing a kinetic model for the Stewart parallel platform in advance includes:

[0029] Step b1, consider the legs as rigid, and ignore the influence of gravity and the friction when the legs move relative to each other on the overall motion of the parallel platform, and construct the rotation balance equation of the upper and lower legs as follows:

[0030] +l ui e i ×F ui -M i -(l U -l ui ) i ×F ci =I ui α i +ω i ×I ui ω i ;

[0031] -l di e i ×F di +M i -(l D -l di ) i ×F ci =I di α i +ω i ×I di ω i ;

[0032] Among them, F ui is the restraint force on the upper leg of leg i; F di is the restraint force on the lower leg of leg i; F ci is the constraint force at the sliding hinge of leg i; l ui is the length from the hinge point of leg i on the upper platform to the center of mass of the upper leg, l di is the length from the hinge point of leg i on the lower platform to the center of mass of the lower leg; l U and l D are the upper leg length and the lower leg length of any leg respectively; M i is the rotational moment at the sliding joint of leg i; I ui and I di are the moments of inertia of the upper and lower legs of leg i about the center of mass; α i is the acceleration of leg i; ω i is the angular velocity of leg i;

[0033] Step b2, express the axial force of leg i as:

[0034]

[0035] Among them, k i is the stiffness of leg i; c i is the damping of leg i; F ai represents the axial force of leg i generated by the actuator; Δl i represents the relative displacement of the upper and lower legs of leg i when they are extended and retracted, represents the relative speed of the upper and lower legs of leg i;

[0036] Step b3, write the axial force of the outrigger into a matrix form, expressed as:

[0037]

[0038] Where k is the matrix of leg stiffness; E is the matrix defined by quaternion; J p and J b are the Jacobian matrices of the upper and lower platforms respectively; q p is the upper platform posture; q b It is the lower platform posture; is the derivative of the upper platform posture; is the derivative of the lower platform posture; F ai is the matrix form of the axial force of leg i generated by the actuator, and c represents the matrix form of the damping;

[0039] Step b4, omit the upper platform, combine the upper platform with the load, and determine the equilibrium equation of the Stewart parallel platform as:

[0040]

[0041] Where m is the effective load mass; r c is the position vector of the payload in the global coordinate system; is the position tensor of the payload in the global coordinate system; is the position tensor of the upper hinge point i; is the acceleration of the upper platform; is the position tensor of the lower hinge point i; is the acceleration of the lower platform; e1…e6 is the unit vector after normalization of the vectors from leg 1 to leg 6; F is the matrix form of the axial force of the leg;

[0042]

[0043]

[0044] m ui and mdi are the masses of the upper and lower legs of leg i respectively; l i is the outrigger length; for e i The tensor form of ; Step b5, determine the rotational equilibrium equation as:

[0045]

[0046] Among them, I m is the effective load inertia;

[0047] Step b6, the kinetic equation of the Stewart parallel platform is expressed as:

[0048]

[0049] in,

[0050] Step b7, the kinetic equation of the Stewart parallel platform based on the workspace is sorted out as a kinetic model, which is expressed as:

[0051]

[0052] in, is the rate of change of the upper platform attitude; is the second-order differential of the upper platform attitude;

[0053]

[0054] In one embodiment of the present invention, in step 3, the error calculation formula used to calculate the displacement error and speed error of each leg is:

[0055]

[0056] Where s is the displacement of the legs obtained; s x is the required displacement of the outrigger; e p is the displacement error of the outrigger; v is the velocity of the outrigger obtained; v x is the speed required for the outrigger; e d is the speed error of the outrigger.

[0057] In one embodiment of the present invention, in step 4, the nonlinear function used for nonlinear processing of the displacement error and the speed error of each leg is:

[0058]

[0059] Among them, when the displacement error of the outrigger is processed nonlinearly, e is the displacement error of the outrigger p, α is α1, δ is δ1, and the nonlinear function outputs the nonlinear processing result of the displacement error of the outrigger; when the nonlinear processing of the velocity error of the outrigger is performed, e is the velocity error e of the outrigger d , α is α2, δ is δ2, and the nonlinear function outputs the nonlinear processing result of the speed error of the outrigger; α1, δ1, α2, and δ2 are all preset values.

[0060] In one embodiment of the present invention, in step 5, the control gain correction function used to dynamically correct the control gain of the S-NPD motion controller is:

[0061]

[0062] Among them, K p K is the proportional gain after outrigger correction; p0 is the initial value of the preset proportional gain; k and n are different constant parameters; K pmax is the maximum value of the preset proportional gain; K d K is the differential gain after leg correction; d0 is the preset initial value of the differential gain; K d1 K is the correction coefficient of the preset differential gain; dmin is the preset minimum value of differential gain; S(e p ) uses the preset function S(), expressed as:

[0063]

[0064] s0 and s1 are constant parameters different from k and n.

[0065] In one embodiment of the present invention, in step 6, for each leg, the nonlinear processing result of the displacement error and the nonlinear processing result of the velocity error of the leg, as well as the corrected proportional gain and differential gain of the leg at the current moment are input into the S-NPD motion controller to obtain the control force of the leg. The input control force formula used is:

[0066] u=K p fal(e p ,α1,δ1)+K d fal(e d ,α2,δ2);

[0067] Where u represents the control force of the current leg; fal(e p ,α1,δ1) is the nonlinear processing result of the displacement error of the current leg; fal(e d ,α2,δ2) is the result of nonlinear processing of the velocity error of the current leg.

[0068] Beneficial effects of the present invention:

[0069] In the solution provided by the embodiment of the present invention, it can be ensured that when the lower platform is subjected to different disturbances, the upper platform remains relatively stable, thereby achieving a good vibration isolation effect. The present invention uses an algorithm that affects the control gain through error feedback to achieve real-time setting of the control gain, further improve the control performance, and reduce parameter sensitivity. Under different working conditions, the present invention can achieve good vibration isolation and control effects. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] Figure 1 A schematic flow chart of an active vibration isolation method based on a Stewart platform provided in an embodiment of the present invention;

[0071] Figure 2 It is a flowchart of the implementation of the entire vibration isolation mechanism and control algorithm of the embodiment of the present invention;

[0072] Figure 3 This is the structural diagram of the Stewart parallel platform;

[0073] Figure 4 This is the mechanical model diagram of the Stewart parallel platform;

[0074] Figure 5 Schematic diagram of the implementation flow of the S-NPD control algorithm of the embodiment of the present invention;

[0075] Figure 6 The vibration isolation effect and comparison of the S-NPD algorithm in the embodiment of the present invention under the condition of a sinusoidal disturbance with an input frequency of 0.5 Hz and an amplitude of 0.2°;

[0076] Figure 7 The vibration isolation effect and comparison of the S-NPD algorithm in the embodiment of the present invention under the condition of a sinusoidal disturbance with an input frequency of 1 Hz and an amplitude of 0.1°;

[0077] Figure 8 The vibration isolation effect and comparison of the S-NPD algorithm in the embodiment of the present invention under the condition of a series of random sine superposition disturbances with an input frequency of 5 to 15 Hz and an amplitude of 0.05 to 0.15°;

[0078] Fig. 9 The vibration isolation effect and comparison of the S-NPD algorithm when random disturbance occurs under a flexible model in an embodiment of the present invention. DETAILED DESCRIPTION

[0079] The present invention is further described in detail below with reference to specific embodiments, but the embodiments of the present invention are not limited thereto.

[0080] The embodiment of the present invention provides an active vibration isolation method based on a Stewart platform, which is an improved S-NPD (Nonlinear Proportional-derivative control) control algorithm, which solves the problems of poor control effect and low adaptability in the face of nonlinearity and uncertainty in traditional control algorithms, and improves the vibration isolation performance of a vibration isolation mechanism based on a Stewart platform.

[0081] The method is applied to a Stewart platform system, which includes a Stewart parallel platform and an S-NPD motion controller. The Stewart parallel platform includes an upper and lower platform connected by six legs. The lower platform is connected to an external structure and receives disturbances, and the upper platform bears the required load and provides a relatively stable posture. The legs are driven by a controller. The method can be used for vibration isolation of a spacecraft's Stewart platform, and the legs are driven by an S-NPD motion controller; the S-NPD motion controller regulates the required elongation of the legs calculated by inverse kinematics, and further improves the control performance through the proposed improved S-NPD control algorithm.

[0082] like Figure 1 As shown, the method may include the following steps 1 to 7, and see Figure 2 Understand the overall algorithmic process. Figure 2 The S-NPD controller is the S-NPD motion controller; the Stewart platform is the Stewart parallel platform. For the structure of the Stewart platform, please refer to Figure 3 and Figure 4 Another flow chart of this method can be found in Figure 5 shown.

[0083] The specific steps of this method are as follows:

[0084] Step 1, based on the kinematic model and dynamic model pre-established for the Stewart parallel platform, collecting the posture signal of the upper platform and the displacement and velocity signals of each leg in the Stewart parallel platform at the current moment;

[0085] The embodiment of the present invention firstly gives an initial value to the control gain of the S-NPD motion controller according to the conventional parameter setting method, including an initial value K of the proportional gain. p0 and the initial value K of the differential gain d0 , and then the proportional gain and differential gain corresponding to each leg are corrected in real time according to the proposed algorithm to achieve the purpose of leg motion control.

[0086] To facilitate understanding, the process of establishing the kinematic model and the dynamic model is first explained.

[0087] (1) The process of establishing a kinematic model for the Stewart parallel platform in advance includes:

[0088] Step a1: Define the global coordinate system and determine the position vectors at the hinge points of the upper and lower platforms:

[0089] r pi =r p +p i ,r bi =r b +b i ;

[0090] Among them, see Figure 3 Understand that in the initial state, the mass center of the lower platform is located at the origin of the global coordinate system; the mass center coordinate system of the upper platform and the mass center coordinate system of the lower platform are fixed to the upper and lower platforms respectively; the coordinate origins of the upper and lower platforms are P and B respectively; in the initial state, the directions of the coordinate axes of the mass center coordinate systems of the upper and lower platforms are consistent with the global coordinate system; Figure 3 , P is the hinge point of the outrigger on the upper platform, and the subscript represents the outrigger number, such as P 5,6 Indicates the hinge points of the legs 5 and 6 on the upper platform. The embodiment of the present invention uses attitude parameters such as Euler angles and quaternions to describe the attitude change of the platform;

[0091] The position vector at the upper and lower platform hinge points is along the outriggers; specifically, r pi is the position vector of the hinge point where the upper platform is connected to leg i; r p is the position vector from the origin of the coordinate system to the geometric centroid P of the upper platform; p i is the vector from the centroid of the upper platform to the position of the outrigger i and the upper platform constraint; r bi is the position vector of the hinge point where the lower platform is connected to leg i; r b is the position vector from the origin of the coordinate system to the geometric centroid B of the lower platform; b i is the vector from the centroid of the lower platform to the position of the leg i and the constraint of the lower platform;

[0092] Step a2, determine the leg i vector pointing from the lower platform hinge point to the upper platform hinge point:

[0093] l i =r pi -r bi =r p +p i -r b -b i ;

[0094] Step a3, calculate the leg vector change rate to represent the leg speed, expressed as:

[0095]

[0096] Among them, e i is the unit vector of the normalized leg i vector; is the velocity at the hinge point of the upper platform; ω p is the angular velocity of the upper platform; is the velocity at the hinge point of the lower platform; ω b is the rotation angular velocity of the lower platform; J pi is the Jacobian matrix of the upper platform corresponding to leg i, J bi is the Jacobian matrix of the lower platform corresponding to leg i; J p and J b are the Jacobian matrices of the upper and lower platforms respectively; For J p The coefficients of different legs in For J b The coefficients of different legs in ;

[0097] Step a4, calculate the second-order differential of the leg vector to represent the acceleration of the leg, expressed as:

[0098]

[0099] Among them, α p is the acceleration of the upper platform; α b is the lower platform acceleration.

[0100] Through the above steps a1 to a4, a kinematic model is constructed.

[0101] (2) The process of establishing a dynamic model for the Stewart parallel platform in advance includes:

[0102] Step b1, consider the legs as rigid, and ignore the influence of gravity and the friction when the legs move relative to each other on the overall motion of the parallel platform, and construct the rotation balance equation of the upper and lower legs as follows:

[0103] +l ui e i ×F ui -M i -(l U -l ui ) i ×F ci =I ui α i +ω i ×I ui ω i ;

[0104] -l di e i ×F di +M i -(l D -l di ) i ×F ci =I di α i +ω i ×I di ω i ;

[0105] Specifically, when describing the dynamic modeling of the Stewart parallel platform in the embodiment of the present invention, the legs are regarded as rigid for leg dynamics analysis, and the six-degree-of-freedom parallel platform is considered to work in a space environment, so the influence of gravity is ignored. In addition, since the friction of the legs during relative motion has little effect on the overall motion of the parallel platform, it is ignored in the modeling process.

[0106] Among them, F ui is the restraint force on the upper leg of leg i; F di is the restraint force on the lower leg of leg i; F ci is the constraint force at the sliding hinge of leg i; l ui is the length from the hinge point of leg i on the upper platform to the center of mass of the upper leg, l di is the length from the hinge point of leg i on the lower platform to the center of mass of the lower leg; l U and l D are the upper leg length and the lower leg length of any leg respectively; M i is the rotational moment at the sliding joint of leg i; I ui and I di are the moments of inertia of the upper and lower legs of leg i about the center of mass; α i is the acceleration of leg i; ω i is the angular velocity of leg i;

[0107] Step b2, express the axial force of leg i as:

[0108]

[0109] Among them, k i is the stiffness of leg i; c i is the damping of leg i; F ai represents the axial force of leg i generated by the actuator; Δl i represents the relative displacement of the upper and lower legs of leg i when they are extended and retracted, represents the relative speed of the upper and lower legs of leg i;

[0110] Step b3, write the axial force of the outrigger into a matrix form, expressed as:

[0111]

[0112] Where k is the matrix of leg stiffness; E is the matrix defined by quaternion; J p and J b are the Jacobian matrices of the upper and lower platforms respectively; q p is the upper platform posture; q b It is the lower platform posture; is the derivative of the upper platform posture; is the derivative of the lower platform posture; F ai is the matrix form of the axial force of leg i generated by the actuator, and c represents the matrix form of the damping;

[0113] Step b4, omit the upper platform, combine the upper platform with the load, and determine the equilibrium equation of the Stewart parallel platform as:

[0114]

[0115] Specifically, in actual space applications, the upper platform model is often omitted, and the legs are directly installed on the load base. Therefore, in the embodiment of the present invention, the upper platform and the load are combined in the process of formula derivation.

[0116] Where m is the effective load mass; r c is the position vector of the payload in the global coordinate system; is the position tensor of the payload in the global coordinate system; is the position tensor of the upper hinge point i; is the acceleration of the upper platform; is the position tensor of the lower hinge point i; is the acceleration of the lower platform; e1…e6 is the unit vector after normalization of the vectors from leg 1 to leg 6; F is the matrix form of the axial force of the leg;

[0117]

[0118]

[0119] m ui and m di are the masses of the upper and lower legs of leg i respectively; l i is the outrigger length; for e i The tensor form of ;

[0120] Step b5, determine the rotational equilibrium equation as:

[0121]

[0122] Among them, I m is the effective load inertia;

[0123] Step b6, the kinetic equation of the Stewart parallel platform is expressed as:

[0124]

[0125] in,

[0126]

[0127]

[0128] Step b7, the kinetic equation of the Stewart parallel platform based on the workspace is sorted out as a kinetic model, which is expressed as:

[0129]

[0130] in, is the rate of change of the upper platform attitude; is the second-order differential of the upper platform attitude;

[0131]

[0132] In the embodiment of the present invention, specific parameters of the Stewart parallel platform may be as shown in Table 1.

[0133] Table 1

[0134]

[0135] Among them, m P , m B , I P , I B are the mass and moment of inertia of the upper and lower platforms respectively; a m is the side length of the load cube; r P is the radius of the upper platform; r B is the radius of the lower platform; l u is the length of each upper leg; l d The length of each lower leg; m u and m d are the upper and lower leg masses of each leg respectively; I u and I d are the moments of inertia of the upper and lower legs of each leg about the center of mass; m is the effective load mass; I m A value indicating the effective load inertia.

[0136] For step 1, based on the kinematic model and dynamic model established in advance for the Stewart parallel platform, the relevant sensors configured for the Stewart parallel platform can be used to collect the posture signal of the upper platform and the displacement and velocity signals of each leg in the Stewart parallel platform at the current moment. For the specific collection process, please refer to the relevant technical understanding and will not be described in detail here.

[0137] Step 2, according to the posture signal of the upper platform and the expected posture quaternion, the required displacement and speed of each leg are calculated by an inverse kinematics algorithm;

[0138] The expected posture quaternion represents the expected posture of the upper platform, which can be preset. In step 2, the expected posture quaternion of the upper platform posture signal collected in step 1 can be used to calculate the required displacement and speed of each leg using the existing inverse kinematics algorithm, indicating the expected displacement and expected speed. Please refer to the relevant technical understanding for the specific calculation process, which will not be explained here.

[0139] Step 3, calculating the displacement error and velocity error of each leg according to the collected displacement and velocity signals of each leg and the calculated displacement and velocity required by each leg;

[0140] In step 3, the error calculation formula used to calculate the displacement error and velocity error of each leg is:

[0141]

[0142] Where s is the displacement of the legs obtained; s x is the required displacement of the outrigger; e p is the displacement error of the outrigger; v is the velocity of the outrigger obtained; v x is the speed required for the outrigger; e d is the speed error of the outrigger.

[0143] It can be understood that the collected displacement and velocity signals of each leg represent the measured displacement and the measured velocity. For each leg, the difference between the measured displacement of the leg and the expected displacement is calculated to obtain the displacement error e of the leg. p ; Take the difference between the measured speed of the leg and the expected speed to get the speed error e of the leg d Therefore, each of the six legs will receive a set of (e p , e d ).

[0144] Step 4, performing nonlinear processing on the displacement error and the speed error of each leg to obtain the nonlinear processing result of the displacement error and the speed error of each leg;

[0145] In step 4, the nonlinear function used for nonlinear processing of the displacement error and speed error of each leg is:

[0146]

[0147] Among them, when the displacement error of the outrigger is processed nonlinearly, e is the displacement error of the outrigger p , α is α1, δ is δ1, and the nonlinear function outputs the nonlinear processing result of the displacement error of the outrigger; when the nonlinear processing of the velocity error of the outrigger is performed, e is the velocity error e of the outrigger d , α is α2, δ is δ2, and the nonlinear function outputs the nonlinear processing result of the speed error of the outrigger; α1, δ1, α2, and δ2 are all preset values. α1 and α2 are different, and δ1 and δ2 are different.

[0148] Specifically, the above nonlinear function represents a general expression for processing displacement error and velocity error. For displacement error, the nonlinear function is specifically:

[0149]

[0150] For the speed error, the nonlinear function is:

[0151]

[0152] For each leg, the displacement error e of the leg is p Substitute the nonlinear function for the displacement error to obtain the nonlinear processing result fal(e p ,α1,δ1); the speed error of the leg is e d Substitute the nonlinear function for the speed error to obtain the nonlinear processing result of the speed error of the leg fal(e d ,α2,δ2); therefore, each leg will get a set of fal(e p ,α1,δ1),fal(e d ,α2,δ2).

[0153] Step 5, dynamically correcting the control gain of the S-NPD motion controller according to the calculated displacement error and velocity error of each leg, to obtain the corrected proportional gain and differential gain of each leg at the current moment;

[0154] In step 5, the control gain correction function used to dynamically correct the control gain of the S-NPD motion controller is:

[0155]

[0156] Among them, K pK is the proportional gain after outrigger correction; p0 is the initial value of the preset proportional gain; k and n are different constant parameters; K pmax is the maximum value of the preset proportional gain; K d K is the differential gain after leg correction; d0 is the preset initial value of the differential gain; K d1 K is the correction coefficient of the preset differential gain; dmin is the preset minimum value of differential gain; S(e p ) uses the preset function S(), expressed as:

[0157]

[0158] s0 and s1 are constant parameters different from k and n.

[0159] For each leg, the displacement error e of the leg is p and speed error e d Substituting into the above control gain correction function, we get K p and K d Therefore, each of the six legs will get a set of (K p , K d ).

[0160] It should be noted that in Figure 2 In the figure, displacement error and velocity error are represented by e1 and e2 respectively, and f and g represent K p and K d ; fal function refers to nonlinear function.

[0161] Step 6: for each leg, the nonlinear processing result of the displacement error and the speed error of the leg, as well as the corrected proportional gain and differential gain of the leg at the current moment are input into the S-NPD motion controller to obtain the control force of the leg, and the control force of each leg is used to realize the motion control of the corresponding leg at the current moment;

[0162] In step 6, for each leg, the nonlinear processing results of the displacement error and the velocity error of the leg, as well as the corrected proportional gain and differential gain of the leg at the current moment are input into the S-NPD motion controller to obtain the control force of the leg. The input control force formula used is:

[0163] u=K p fal(e p ,α1,δ1)+K d fal(e d ,α2,δ2);

[0164] Where u represents the control force of the current leg; fal(e p ,α1,δ1) is the nonlinear processing result of the displacement error of the current leg; fal(e d ,α2,δ2) is the result of nonlinear processing of the velocity error of the current leg.

[0165] It can be understood that for each leg, its K p ,fal(e p ,α1,δ1),K d ,fal(e d ,α2,δ2) into the above input control force formula, we can get the control force u of the leg, that is, the driving force. The control force u can be used to realize the motion control of the leg.

[0166] Specifically, after calculating the control force of the outrigger, the control force can be sent to the joint controller as the control amount for motion control, and the K calculated at the current moment p and K d As the initial value of the proportional gain and the initial value of the differential gain when the control gain is dynamically corrected at the next moment, K p and K d Calculation.

[0167] Step 7, repeat steps 1 to 6 until the preset number of moments is reached.

[0168] The embodiment of the present invention can complete steps 1 to 6 for each moment to realize the motion control of each leg at the current moment. When the cumulative number of moments reaches the preset number of moments, it means that the task is completed and returns to zero, and the algorithm can be terminated at this time.

[0169] For the effects of the embodiments of the present invention, please refer to 6 to Fig. 9 Under the four working conditions, the control performance of the embodiment of the present invention is improved to varying degrees.

[0170] Working condition 1:

[0171] Under the condition of sinusoidal disturbance with input frequency of 0.5Hz and amplitude of 0.2°, compared with the original PD control and NPD control, S-NPD has a relative performance improvement of 17.2% and 16.4% respectively.

[0172] Working condition 2:

[0173] Under the condition of sinusoidal disturbance with input frequency of 1Hz and amplitude of 0.1°, compared with the original PD control and NPD control, S-NPD has relative performance improvements of 64.0% and 31.4% respectively.

[0174] Working condition three:

[0175] Under the condition of a series of random sinusoidal superposition disturbances with input frequency of 5 to 15 Hz and amplitude of 0.05 to 0.15°, compared with the original PD (Proportional-derivative control) and NPD control, S-NPD has a relative performance improvement of 37.9% and 9.6% respectively.

[0176] Working condition 4:

[0177] The flexibility in the legs is taken into account, where the elastic modulus of the flexible legs is 2×10 11 Pa, under the flexible model with the same random disturbance input, compared with the original PD control and NPD control, S-NPD has a relative performance improvement of 70.8% and 8.7% respectively.

[0178] In the solution provided by the embodiment of the present invention, it can be ensured that when the lower platform is subjected to different disturbances, the upper platform remains relatively stable, thereby achieving a good vibration isolation effect. The present invention uses an algorithm that affects the control gain through error feedback to achieve real-time setting of the control gain, further improve the control performance, and reduce parameter sensitivity. Under different working conditions, the present invention can achieve good vibration isolation and control effects.

[0179] The above description is only a preferred embodiment of the present invention and is not intended to limit the protection scope of the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention are included in the protection scope of the present invention.

Claims

1. An active vibration isolation method based on a Stewart platform, characterized in that: Applied to a Stewart platform system, the Stewart platform system includes a Stewart parallel platform and an S-NPD motion controller, the method includes: Step 1, based on the kinematic model and dynamic model pre-established for the Stewart parallel platform, collecting the posture signal of the upper platform and the displacement and velocity signals of each leg in the Stewart parallel platform at the current moment; Step 2, according to the posture signal of the upper platform and the expected posture quaternion, the required displacement and speed of each leg are calculated by an inverse kinematics algorithm; Step 3, calculating the displacement error and velocity error of each leg according to the collected displacement and velocity signals of each leg and the calculated displacement and velocity required by each leg; Step 4, performing nonlinear processing on the displacement error and the speed error of each leg to obtain the nonlinear processing result of the displacement error and the speed error of each leg; Step 5, dynamically correcting the control gain of the S-NPD motion controller according to the calculated displacement error and velocity error of each leg, to obtain the corrected proportional gain and differential gain of each leg at the current moment; Step 6: for each leg, the nonlinear processing result of the displacement error and the speed error of the leg, as well as the corrected proportional gain and differential gain of the leg at the current moment are input into the S-NPD motion controller to obtain the control force of the leg, and the control force of each leg is used to realize the motion control of the corresponding leg at the current moment; Step 7, repeating steps 1 to 6 until the preset number of moments is reached; Wherein, in step 5, the control gain correction function used to dynamically correct the control gain of the S-NPD motion controller is: Among them, K p K is the proportional gain after outrigger correction; p0 is the initial value of the preset proportional gain; k and n are different constant parameters; K pmax is the maximum value of the preset proportional gain; K d K is the differential gain after leg correction; d0 is the preset initial value of the differential gain; K d1 K is the correction coefficient of the preset differential gain; dmin is the preset minimum value of differential gain; S(e p ) uses the preset function S(), expressed as: s0 and s1 are constant parameters different from k and n; for each leg, the displacement error e of the leg is p and speed error e d Substituting into the above control gain correction function, we get K p and K d , so that each leg gets a set of (K p , K d ).

2. The active vibration isolation method based on the Stewart platform according to claim 1, characterized in that: The process of establishing a kinematic model for the Stewart parallel platform in advance includes: Step a1: Define the global coordinate system and determine the position vectors at the hinge points of the upper and lower platforms: r pi =r p +p i ,r bi =r b +b i ; Among them, in the initial state, the mass center of the lower platform is located at the origin of the global coordinate system; the mass center coordinate system of the upper platform and the mass center coordinate system of the lower platform are fixed to the upper and lower platforms respectively; in the initial state, the directions of the coordinate axes of the mass center coordinate systems of the upper and lower platforms are consistent with the global coordinate system; pi is the position vector of the hinge point where the upper platform is connected to leg i; r p is the position vector from the origin of the coordinate system to the geometric centroid P of the upper platform; p i is the vector from the centroid of the upper platform to the position of the outrigger i and the upper platform constraint; r bi is the position vector of the hinge point where the lower platform is connected to leg i; r b is the position vector from the origin of the coordinate system to the geometric centroid B of the lower platform; b i is the vector from the centroid of the lower platform to the position of the leg i and the constraint of the lower platform; Step a2, determine the leg i vector pointing from the lower platform hinge point to the upper platform hinge point: l i =r pi -r bi =r p +p i -r b -b i ; Step a3, calculate the leg vector change rate to represent the leg speed, expressed as: Among them, e i is the unit vector of the normalized leg i vector; is the velocity at the hinge point of the upper platform; ω p is the angular velocity of the upper platform; is the velocity at the hinge point of the lower platform; ω b is the rotation angular velocity of the lower platform; J pi is the Jacobian matrix of the upper platform corresponding to leg i, J bi is the Jacobian matrix of the lower platform corresponding to leg i; J p and J b are the Jacobian matrices of the upper and lower platforms respectively; For J p The coefficients of different legs in For J b The coefficients of different legs in ; Step a4, calculate the second-order differential of the leg vector to represent the acceleration of the leg, expressed as: Among them, α p is the acceleration of the upper platform; α b is the lower platform acceleration.

3. The active vibration isolation method based on the Stewart platform according to claim 2, characterized in that: The process of establishing a kinetic model for the Stewart parallel platform in advance includes: Step b1, consider the legs as rigid, and ignore the influence of gravity and the friction when the legs move relative to each other on the overall motion of the parallel platform, and construct the rotation balance equation of the upper and lower legs as follows: +l ui e i ×F ui -M i -(l U -l ui )e i ×F ci =I ui a i +oh i ×I ui oh i ; -l di e i ×F di +M i -(l D -l di )e i ×F ci =I di a i +oh i ×I di oh i ; Among them, F ui is the restraint force on the upper leg of leg i; F di is the restraint force on the lower leg of leg i; F ci is the constraint force at the sliding hinge of leg i; l ui is the length from the hinge point of leg i on the upper platform to the center of mass of the upper leg, l di is the length from the hinge point of leg i on the lower platform to the center of mass of the lower leg; l U and l D are the upper leg length and the lower leg length of any leg respectively; M i is the rotational moment at the sliding joint of leg i; I ui and I di are the moments of inertia of the upper and lower legs of leg i about the center of mass; α i is the acceleration of leg i; ω i is the angular velocity of leg i; Step b2, express the axial force of leg i as: Among them, k i is the stiffness of leg i; c i is the damping of leg i; F ai represents the axial force of leg i generated by the actuator; Δl i represents the relative displacement of the upper and lower legs of leg i when they are extended and retracted, represents the relative speed of the upper and lower legs of leg i; Step b3, write the axial force of the outrigger into a matrix form, expressed as: Where k is the matrix of leg stiffness; E is the matrix defined by quaternion; J p and J b are the Jacobian matrices of the upper and lower platforms respectively; q p is the upper platform posture; q b It is the lower platform posture; is the derivative of the upper platform posture; is the derivative of the lower platform posture; F ai is the matrix form of the axial force of leg i generated by the actuator, and c represents the matrix form of the damping; Step b4, omit the upper platform, combine the upper platform with the load, and determine the equilibrium equation of the Stewart parallel platform as: Where, m is the effective load mass; r c is the position vector of the payload in the global coordinate system; is the position tensor of the payload in the global coordinate system; is the position tensor of the upper hinge point i; is the acceleration of the upper platform; is the position tensor of the lower hinge point i; is the acceleration of the lower platform; e1…e6 is the unit vector after normalization of the vectors from leg 1 to leg 6; F is the matrix form of the axial force of the leg; m ui and m di are the masses of the upper and lower legs of leg i respectively; l i is the outrigger length; for e i The tensor form of ; Step b5, determine the rotational equilibrium equation as: Among them, I m is the effective load inertia; Step b6, the kinetic equation of the Stewart parallel platform is expressed as: in, Step b7, the kinetic equation of the Stewart parallel platform based on the workspace is sorted out as a kinetic model, which is expressed as: in, is the rate of change of the upper platform attitude; is the second-order differential of the upper platform attitude; 4. The active vibration isolation method based on the Stewart platform according to claim 3, characterized in that: In step 3, the error calculation formula used to calculate the displacement error and velocity error of each leg is: Where s is the displacement of the legs obtained; s x is the required displacement of the outrigger; e p is the displacement error of the outrigger; v is the velocity of the outrigger obtained; v x is the speed required for the outrigger; e d is the speed error of the outrigger.

5. The active vibration isolation method based on the Stewart platform according to claim 4, characterized in that: In step 4, the nonlinear function used for nonlinear processing of the displacement error and speed error of each leg is: Among them, when the displacement error of the outrigger is processed nonlinearly, e is the displacement error of the outrigger p , α is α1, δ is δ1, and the nonlinear function outputs the nonlinear processing result of the displacement error of the outrigger; when the nonlinear processing of the velocity error of the outrigger is performed, e is the velocity error e of the outrigger d , α is α2, δ is δ2, and the nonlinear function outputs the nonlinear processing result of the speed error of the outrigger; α1, δ1, α2, and δ2 are all preset values.

6. The active vibration isolation method based on the Stewart platform according to claim 5, characterized in that: In step 6, for each leg, the nonlinear processing results of the displacement error and the velocity error of the leg, as well as the corrected proportional gain and differential gain of the leg at the current moment are input into the S-NPD motion controller to obtain the control force of the leg. The input control force formula used is: u=K p one(e p ,α1,δ1)+K d one(e d ,α2,δ2)): Where u represents the control force of the current leg; fal(e p ,α1,δ1) is the nonlinear processing result of the displacement error of the current leg; fal(e d ,α2,δ2) is the result of nonlinear processing of the velocity error of the current leg.

Images