Stewart wave compensation platform control method and system
Through adaptive gravity estimation and non-singular terminal sliding mode control, the control accuracy and robustness of the six-degree-of-freedom wave compensation platform are solved, and high-precision and anti-interference performance are improved.
Patent Information
- Application Number
- CN202411023000.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-29
- Publication Date
- 2024-11-08
AI Technical Summary
The existing six-degree-of-freedom wave compensation platform control method is difficult to meet the control accuracy requirements, and the traditional method has strong dependence on system parameters, and the robustness and accuracy control need to be improved.
Using a non-singular terminal sliding mode control method based on adaptive gravity estimation, the velocity feedforward term is introduced to realize the Stewart wave compensation platform by obtaining hull motion information, dynamic characteristic analysis, Kalman filter filter and second-order filter calculation. Stick control.
The model-free characteristics are achieved, with strong drift resistance and high control accuracy, and the position compensation rate and rotation compensation rate reach 94.46% and 94.39% respectively, which significantly improves the robustness and control accuracy of the system.
Smart Images

Figure CN118915549A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of parallel robot control methods, and in particular, to a control method and system for a Stewart wave compensation platform, and more particularly, to a robust sliding mode control method based on combining adaptive gravity estimation. Background Art
[0002] The parallel six-degree-of-freedom platform can be used to compensate for the six-dimensional interference motion of ships caused by factors such as sea wind and waves, providing a safe and stable working environment for offshore operations. However, the marine environment is always unpredictable, and the wave compensation system faces multi-source disturbances, large lags, variable parameters, strong coupling, and nonlinear working characteristics.
[0003] At present, there are some control methods for six-degree-of-freedom wave compensation platforms. Traditional controllers are difficult to meet the control accuracy requirements, and most of the existing advanced control methods are highly dependent on system parameters, which are difficult to implement in the actual control process. In the prior art, patent document CN115598966A proposes a visual servo neural network control method for a six-degree-of-freedom parallel swing platform, which uses vision instead of force sensing to respond to external interference, but still relies on the established dynamic model when controlling, and adds cameras, which increases the external disturbance sources; patent document CN111708274A proposes a wave compensation control system based on a feedforward PID control algorithm, which includes four micro-inertial sensors. The sources of external disturbances are complicated, and feedforward PID is used, and the robustness and precision control need to be improved.
[0004] Therefore, a robust controller that is not strongly dependent on the model is needed to achieve high-precision control. In addition, the controller should be simple and easy to implement in engineering. Summary of the invention
[0005] In view of the defects in the prior art, an object of the present invention is to provide a control method and system for a Stewart heave compensation platform.
[0006] A control method for a Stewart heave compensation platform provided by the present invention comprises:
[0007] Step S1: Obtain the hull motion information, perform kinematic inverse solution to obtain the target length of the six branches to be compensated;
[0008] Step S2: Based on the dynamic characteristics of the Stewart wave compensation platform, the time-varying gravity is obtained by using delay estimation;
[0009] Step S3: filtering the time-varying gravity based on the Kalman filter to obtain the filtered time-varying gravity;
[0010] Step S4: Calculate the branch target length change rate through second-order filtering;
[0011] Step S5: using the target length change rate, a velocity feedforward term is introduced into the non-singular terminal sliding mode controller; based on the target length and the filtered time-varying gravity, the drive input is calculated using the non-singular terminal sliding mode controller with the velocity feedforward term introduced to realize the heave compensation control of the Stewart heave compensation platform.
[0012] Preferably, the step S1 adopts:
[0013] The posture sensor is connected to the lower platform of the wave compensation platform and fixed at the center of the plane of the hinge point of the lower platform to continuously obtain the posture information of the hull movement. b ;
[0014] Based on the acquired hull motion posture information, the target lengths of the six branches are obtained by inverse kinematics solution:
[0015]
[0016] Among them: id represents the target length of the i-th branch; l id represents the target vector of the i-th branch from the lower platform hinge point to the upper platform hinge point; Represents the position information x according to the motion of the ship b The displacement vector of the upper platform of wave compensation relative to the lower platform is obtained; R btd Represents the position information x according to the motion of the ship b The obtained rotation matrix of the upper platform of wave compensation relative to the lower platform; t i b represents the coordinates of the spherical joint point of the ith branch chain on the wave compensation platform; i It represents the coordinates of the Hooke's hinge point of the i-th branch chain of the platform under wave compensation; i represents the sequence number of the branch chain, there are 6 branches in total, i=1, 2, 3, 4, 5, 6.
[0017] Preferably, the Stewart wave compensation platform dynamic characteristics in step S2 includes: obtaining the Stewart wave compensation platform dynamic characteristics based on the Stewart wave compensation platform dynamic equation;
[0018] According to the Newton-Euler formula, the dynamic equation of the platform on the wave compensation platform in the inertial system is:
[0019]
[0020] Where: M p represents the generalized mass matrix of the platform on the compensation platform in the inertial frame; represents the generalized acceleration of the compensation platform in the inertial system; Cp represents the generalized Coriolis matrix of the platform on the compensation platform in the inertial frame; G represents the generalized velocity of the compensation platform in the inertial system; p represents the gravity matrix of the platform on the compensation platform in the inertial system; F a Indicates the active external force on the compensation platform in the current state;
[0021] The dynamic equation of the platform on the wave compensation platform in the non-inertial system is:
[0022]
[0023] Where: M l represents the generalized mass matrix of the heave compensation platform in joint space; represents the acceleration of the actual length change of the branch chain; C1 represents the generalized Coriolis matrix of the wave compensation platform in the joint space; Indicates the speed at which the actual length of the branch chain changes; G l represents the generalized gravity matrix of the heave compensation platform in the joint space; M bl represents the generalized mass matrix of the lower platform in the joint space; represents the acceleration of the lower platform; C bl represents the generalized Coriolis matrix of the lower plateau in the joint space; Indicates the speed of the lower platform; f a represents the driving force of the branch chain;
[0024] According to the principle of virtual work, f a With F a Have the following relationship:
[0025] F a =Jf a
[0026] Combining the dynamic equations in the two reference frames, we get the inertial force F ml The expression is:
[0027]
[0028] When full compensation is achieved, the length of the branch chain, the speed of length change and the acceleration all approach the target, which is equivalent to making the end platform and Inertial force F ml Approaching:
[0029]
[0030] At this point, the branch chain output should only be gravity:
[0031] G l =fa ;
[0032] The dynamic characteristic of the heave compensation platform when achieving full compensation is that the output of the six branches is approximately equal to the decomposition of the platform's gravity on the six branches.
[0033] Preferably, the non-singular terminal sliding mode controller comprises: constructing a non-singular terminal sliding mode controller based on the dynamic characteristics of the Stewart heave compensation platform and in combination with a non-singular terminal sliding mode control method;
[0034] The designed non-singular terminal sliding surface is:
[0035]
[0036] Where: e = l d -l represents the following error of the branch length; β=diag(β1,…,β6), γ represents the custom parameter of the designed non-singular terminal sliding surface, and its value range is β i >0,1<γ<2,l d represents the target length of the six branches, and l represents the actual length of the branches;
[0037] The terminal fast sliding mode approximation rate is defined as:
[0038]
[0039] Where k1 = diag(k 11 ,…,k 16 ), k2=diag(k 21 ,…,k 26 ), and ρ represents the custom parameter of the terminal fast sliding mode approximation rate, with a value range of k 1i ,k 2i >0,0<ρ<1;
[0040] Combined with the non-singular terminal sliding mode control algorithm, a non-singular terminal sliding mode controller is constructed:
[0041]
[0042] in: is the generalized gravity matrix of the heave compensation platform in the estimated joint space;
[0043] According to the dynamic characteristics, the non-singular terminal sliding mode controller is simplified;
[0044]
[0045] Preferably, the step S2 adopts:
[0046] According to the dynamic characteristics, the time-varying gravity is estimated according to the output u of the non-singular terminal sliding mode controller:
[0047] G p ≈J T u
[0048] The output u of the non-singular terminal sliding mode controller at the current moment needs to be known under G p Under the condition of obtaining, according to the characteristic that gravity remains unchanged in a short time, the current moment can be estimated by delaying one sampling cycle:
[0049] G p (t)≈J T u(tL)
[0050] Where: G p (t) represents the estimated gravity at the current moment; u(tL) represents the output of the controller in the previous sampling period at the current moment.
[0051] Preferably, the step S3 adopts:
[0052] Based on the Kalman filter, the abstract state transfer equation is designed as:
[0053]
[0054] G rk =γG r(k-1) +(1-λ)G p(k-1)
[0055] Where: G pk Represents the estimated gravity in the world coordinate system at the current moment; G r(k-1) Represents the reference force, which depends on the constant reference gravity; G p(k-1) represents the estimated gravity at the last sampling time; p∈(0,1) represents the weight of the last estimated gravity; G l represents the decomposition of the gravity of the top platform on the six branches, which is the estimated control input u; w k-1 and v k represents noise; λ represents the forgetting factor;
[0056] Combined with the classical Kalman filter equation, the prediction part and the correction part of the gravity Kalman filter are designed; the prediction part of the gravity Kalman filter is
[0057]
[0058] Where: P k represents the prior estimation error covariance; Q represents the process noise covariance;
[0059] The correction part of the gravity Kalman filter is
[0060]
[0061] Where: K k represents the Kalman gain; R represents the measurement noise covariance; represents the control input force of the six branches; represents the estimated gravity of the top platform;
[0062] The gravity term in the inertial system is decomposed into 6 branches, and the gravity matrix of the wave compensation platform in the joint space used by the non-singular terminal sliding mode control law is:
[0063]
[0064] The gravity matrix in the joint space is the time-varying gravity in the non-singular terminal sliding mode controller.
[0065] Preferably, the step S4 adopts:
[0066] The velocity feedforward term is obtained by a second-order filter
[0067]
[0068] Where: z1(0) = l d (0); In process ω represents the natural frequency; ζ represents the damping ratio.
[0069] Preferably, the step S5 adopts:
[0070] Introducing a speed feedforward term in the controller, the control input of the non-singular terminal sliding mode controller to the drive is
[0071]
[0072] Where: K v Represents the feedforward gain.
[0073] A control system of a Stewart heave compensation platform provided by the present invention comprises:
[0074] Module M1: Obtain the hull motion information and perform kinematic inverse solution to obtain the target length of the six branches to be compensated;
[0075] Module M2: Based on the dynamic characteristics of the Stewart wave compensation platform, time-delay estimation is used to obtain time-varying gravity;
[0076] Module M3: filtering the time-varying gravity based on the Kalman filter to obtain the filtered time-varying gravity;
[0077] Module M4: Calculate the change rate of branch target length through second-order filtering;
[0078] Module M5: A velocity feedforward term is introduced into the non-singular terminal sliding mode controller using the target length change rate. Based on the target length and filtered time-varying gravity, the drive input is calculated using the non-singular terminal sliding mode controller with the velocity feedforward term introduced to achieve heave compensation control of the Stewart heave compensation platform.
[0079] Preferably, the module M1 adopts:
[0080] The posture sensor is connected to the lower platform of the wave compensation platform and fixed at the center of the plane of the hinge point of the lower platform to continuously obtain the posture information of the hull movement. b ;
[0081] Based on the acquired hull motion posture information, the target lengths of the six branches are obtained by inverse kinematics solution:
[0082]
[0083] Among them: id represents the target length of the i-th branch; l id represents the target vector of the i-th branch from the lower platform hinge point to the upper platform hinge point; Represents the position information x according to the motion of the ship b The displacement vector of the upper platform of wave compensation relative to the lower platform is obtained; R btd Represents the position information x according to the motion of the ship b The obtained rotation matrix of the upper platform of wave compensation relative to the lower platform; t i b represents the coordinates of the spherical joint point of the ith branch chain on the wave compensation platform; i It represents the coordinates of the Hook hinge point of the i-th branch chain of the platform under wave compensation; i represents the sequence number of the branch chain, there are 6 branches in total, i=1, 2, 3, 4, 5, 6;
[0084] The Stewart wave compensation platform dynamic characteristics in the module M2 include: obtaining the Stewart wave compensation platform dynamic characteristics based on the Stewart wave compensation platform dynamic equation;
[0085] According to the Newton-Euler formula, the dynamic equation of the platform on the wave compensation platform in the inertial system is:
[0086]
[0087] Where: M prepresents the generalized mass matrix of the platform on the compensation platform in the inertial frame; represents the generalized acceleration of the compensation platform in the inertial system; C p represents the generalized Coriolis matrix of the platform on the compensation platform in the inertial frame; G represents the generalized velocity of the compensation platform in the inertial system; p represents the gravity matrix of the platform on the compensation platform in the inertial system; F a Indicates the active external force on the compensation platform in the current state;
[0088] The dynamic equation of the platform on the wave compensation platform in the non-inertial system is:
[0089]
[0090] Where: M l represents the generalized mass matrix of the heave compensation platform in joint space; represents the acceleration of the actual length change of the branch chain; C1 represents the generalized Coriolis matrix of the wave compensation platform in the joint space; Indicates the speed at which the actual length of the branch chain changes; G l represents the generalized gravity matrix of the heave compensation platform in the joint space; M bl represents the generalized mass matrix of the lower platform in the joint space; represents the acceleration of the lower platform; C bl represents the generalized Coriolis matrix of the lower plateau in the joint space; Indicates the speed of the lower platform; f a represents the driving force of the branch chain;
[0091] According to the principle of virtual work, f a With F a Have the following relationship:
[0092] F a =Jf a
[0093] Combining the dynamic equations in the two reference frames, we get the inertial force F ml The expression is:
[0094]
[0095] When full compensation is achieved, the length of the branch chain, the speed of length change and the acceleration all approach the target, which is equivalent to making the end platform and Inertial force F ml Approaching:
[0096]
[0097] At this point, the branch chain output should only be gravity:
[0098] G l =f a ;
[0099] The dynamic characteristics of the heave compensation platform when achieving full compensation are as follows: the output of the six branches is approximately equal to the decomposition of the platform's gravity on the six branches;
[0100] The non-singular terminal sliding mode controller comprises: constructing a non-singular terminal sliding mode controller based on the dynamic characteristics of the Stewart heave compensation platform and in combination with a non-singular terminal sliding mode control method;
[0101] The designed non-singular terminal sliding surface is:
[0102]
[0103] Where: e = l d -l represents the following error of the branch length; β=diag(β1,…,β6), γ represents the custom parameter of the designed non-singular terminal sliding surface, and its value range is β i >0,1<γ<2,l d represents the target length of the six branches, and l represents the actual length of the branches;
[0104] The terminal fast sliding mode approximation rate is defined as:
[0105]
[0106] Where k1 = diag(k 11 ,…,k 16 ), k2=diag(k 21 ,…,k 26 ), and ρ represents the custom parameter of the terminal fast sliding mode approximation rate, with a value range of k 1i ,k 2i >0,0<ρ<1;
[0107] Combined with the non-singular terminal sliding mode control algorithm, a non-singular terminal sliding mode controller is constructed:
[0108]
[0109] in: is the generalized gravity matrix of the heave compensation platform in the estimated joint space;
[0110] According to the dynamic characteristics, the non-singular terminal sliding mode controller is simplified;
[0111]
[0112] The module M2 adopts:
[0113] According to the dynamic characteristics, the time-varying gravity is estimated according to the output u of the non-singular terminal sliding mode controller:
[0114] G p ≈J T u
[0115] The output u of the non-singular terminal sliding mode controller at the current moment needs to be known under G p Under the condition of obtaining, according to the characteristic that gravity remains unchanged in a short time, the current moment can be estimated by delaying one sampling cycle:
[0116] G p (t)≈J T u(tL)
[0117] Where: G p (t) represents the estimated gravity at the current moment; u(tL) represents the output of the controller in the previous sampling period at the current moment;
[0118] The module M3 adopts:
[0119] Based on the Kalman filter, the abstract state transfer equation is designed as:
[0120]
[0121] G rk =γG r(k-1) +(1-λ)G p(k-1)
[0122] Where: G pk Represents the estimated gravity in the world coordinate system at the current moment; G r(k-1) Represents the reference force, which depends on the constant reference gravity; G p(k-1) represents the estimated gravity at the last sampling time; p∈(0,1) represents the weight of the last estimated gravity; G l represents the decomposition of the gravity of the top platform on the six branches, which is the estimated control input u; w k-1 and v k represents noise; λ represents the forgetting factor;
[0123] Combined with the classical Kalman filter equation, the prediction part and the correction part of the gravity Kalman filter are designed; the prediction part of the gravity Kalman filter is
[0124]
[0125] Where: P k represents the prior estimation error covariance; Q represents the process noise covariance;
[0126] The correction part of the gravity Kalman filter is
[0127]
[0128] Where: K k represents the Kalman gain; R represents the measurement noise covariance; represents the control input force of the six branches; represents the estimated gravity of the top platform;
[0129] The gravity term in the inertial system is decomposed into 6 branches, and the gravity matrix of the wave compensation platform in the joint space used by the non-singular terminal sliding mode control law is:
[0130]
[0131] The gravity matrix in the joint space is the time-varying gravity in the non-singular terminal sliding mode controller;
[0132] The module M4 adopts:
[0133] The velocity feedforward term is obtained by a second-order filter
[0134]
[0135] Where: z1(0) = l d (0); In process ω represents the natural frequency; ζ represents the damping ratio;
[0136] The module M5 adopts:
[0137] Introducing a speed feedforward term in the controller, the control input of the non-singular terminal sliding mode controller to the drive is
[0138]
[0139] Where: K v Represents the feedforward gain.
[0140] Compared with the prior art, the present invention has the following beneficial effects:
[0141] 1. The Stewart wave compensation platform control method provided by the present invention has a satisfactory model-free characteristic, and can realize gravity adaptive estimation and robust control without an accurate system dynamics model. This characteristic also makes it easier to be applied in the engineering field;
[0142] 2. Compared with the existing PID controller and the backstepping-based sliding mode controller (RBSMC) controller, the gravity adaptive estimation non-singular terminal sliding mode controller (NTSMC-GE) provided by the present invention has stronger anti-drift capability, excellent robustness, and more prominent model-free advantages;
[0143] 3. The gravity estimation method provided by the present invention has also achieved satisfactory results. Through testing, the estimated value of gravity is compared with the actual value. The time-varying gravity component is accurate and smooth enough to be used as part of the controller control output, which can realize the estimation of time-varying gravity and improve the control accuracy of the system.
[0144] 4. The NTSMC-GE method proposed in the present invention also achieved satisfactory results in terms of anti-interference performance and small residual motion, with a position compensation rate of up to 94.46% and a rotation compensation rate of up to 94.39%. BRIEF DESCRIPTION OF THE DRAWINGS
[0145] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:
[0146] Figure 1 Control block diagram of the non-singular terminal sliding mode control law combined with adaptive gravity estimation.
[0147] Figure 2 Schematic diagram of the control target of the Stewart heave compensation platform.
[0148] Figure 3 Schematic diagram of the simulation model established in MATLAB / Simulink.
[0149] Figure 4a to Figure 4b Schematic diagram of six-degree-of-freedom random wave signal.
[0150] Figure 5a to Figure 5f Schematic diagram of the compensated residual motion of the three controllers under random waves.
[0151] Figures 6a to 6c Schematic diagram of the control output of the three controllers.
[0152] Figure 7a to Figure 7b Schematic diagram of gravity estimation results. DETAILED DESCRIPTION
[0153] The present invention is described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several changes and improvements can also be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.
[0154] Example 1
[0155] The lower platform of the wave compensation platform is fixed on the deck of the ship. The Stewart wave compensation platform control method and system provided by the present invention provide instructions for the driver to drive the moving pairs on the six branch chains, and adjust the lengths of the six branch chains so that the posture of the upper platform relative to the lower platform and the posture of the hull movement compensate each other, so that the upper platform remains stable relative to the world coordinate system.
[0156] According to the Stewart heave compensation platform control method provided by the present invention, Figure 1 As shown, the following steps are included:
[0157] Step S1, obtaining the hull motion information, and solving the target length of compensating six branch chains;
[0158] Step S2, establishing the compensation platform dynamics equation to obtain the platform dynamics characteristics;
[0159] Step S3, deriving a non-singular terminal sliding mode control law and constructing a non-singular terminal sliding mode controller;
[0160] Step S4, simplifying the non-singular terminal sliding mode controller;
[0161] Step S5, using delay estimation to obtain time-varying gravity;
[0162] Step S6, filtering the time-varying gravity based on the Kalman filter algorithm;
[0163] Step S7, calculating the branch chain target length change rate by second-order filtering;
[0164] Step S8: Based on the target length change rate, a velocity feedforward term is introduced into the non-singular terminal sliding mode controller;
[0165] Step S9: Based on the target length and the filtered time-varying gravity, the drive input is calculated by using a non-singular terminal sliding mode controller that introduces a velocity feedforward term to implement heave compensation control of the Stewart platform.
[0166] Specifically, step S1, obtaining the hull motion information, and solving to achieve the target length of compensating six branch chains.
[0167] The posture sensor is fixedly connected to the lower platform of the wave compensation platform and fixed at the center of the plane of the hinge point of the lower platform to continuously obtain the posture information of the hull movement. b .
[0168] Based on the acquired hull motion posture information, in order to achieve motion compensation, the upper platform is kept stationary relative to the world coordinate system, such as Figure 2As shown in the figure, the target length of the six branches is obtained by inverse kinematics solution:
[0169]
[0170] Among them: id represents the target length of the i-th branch; l id represents the target vector of the i-th branch from the lower platform hinge point to the upper platform hinge point; Represents the position information x according to the motion of the ship b The displacement vector of the upper platform of wave compensation relative to the lower platform is obtained; R btd Represents the position information x according to the motion of the ship b The obtained rotation matrix of the upper platform of wave compensation relative to the lower platform; t i b represents the coordinates of the spherical joint point of the ith branch chain on the wave compensation platform; i It represents the coordinates of the Hooke's hinge point of the i-th branch chain of the platform under wave compensation; i represents the sequence number of the branch chain, there are 6 branches in total, i=1, 2, 3, 4, 5, 6.
[0171] Step S2: establishing the compensation platform dynamics equation to obtain the platform dynamics characteristics.
[0172] Since the external disturbance is very small compared to the gravity term, the gravity term is mainly considered when establishing the dynamic model. According to the Newton-Euler formula, the dynamic equation of the platform on the wave compensation platform in the inertial system is:
[0173]
[0174] Where: M p represents the generalized mass matrix of the platform on the compensation platform in the inertial frame; represents the generalized acceleration of the compensation platform in the inertial system; C p represents the generalized Coriolis matrix of the platform on the compensation platform in the inertial frame; G represents the generalized velocity of the compensation platform in the inertial system; p represents the gravity matrix of the platform on the compensation platform in the inertial system; F a Indicates the active external force on the compensation platform in the current state.
[0175] The dynamic equation of the platform on the wave compensation platform in the non-inertial system is:
[0176]
[0177] Where: M l represents the generalized mass matrix of the heave compensation platform in joint space; represents the acceleration of the actual length change of the branch chain; C1 represents the generalized Coriolis matrix of the wave compensation platform in the joint space; Indicates the speed at which the actual length of the branch chain changes; G l represents the generalized gravity matrix of the heave compensation platform in the joint space; M bl represents the generalized mass matrix of the lower platform in the joint space; represents the acceleration of the lower platform; C bl represents the generalized Coriolis matrix of the lower plateau in the joint space; Indicates the speed of the lower platform; f a Represents the driving force of the branch chain.
[0178] According to the principle of virtual work, f a With F a Have the following relationship:
[0179] F a =Jf a
[0180] Combining the dynamic equations in the two reference frames, we can get the inertial force F ml The expression is:
[0181]
[0182] When full compensation is achieved, the length of the branch chain, the speed of length change and the acceleration all approach the target, which is equivalent to making the end platform and Inertial force F ml Approaching:
[0183]
[0184] At this point, the branch chain output should only be gravity:
[0185] G l =f a
[0186] That is, the dynamic characteristic of the wave compensation platform when achieving full compensation is that the output of the six branches is approximately equal to the decomposition of the platform gravity on the six branches. This characteristic provides a basis for the simplification of subsequent controller design and gravity estimation.
[0187] Specifically, step S3, deriving a non-singular terminal sliding mode control law and constructing a non-singular terminal sliding mode controller;
[0188] In this embodiment, the non-singular terminal sliding membrane surface is designed as:
[0189]
[0190] Where: e = l d-l represents the following error of the branch length;
[0191] In this embodiment, a terminal fast sliding mode approximation rate is defined as
[0192]
[0193] Combined with the non-singular terminal sliding mode control algorithm, the non-singular terminal sliding mode controller is:
[0194]
[0195] in: is the generalized gravity matrix of the heave compensation platform in the estimated joint space.
[0196] Specifically, step S4: simplifying the non-singular terminal sliding mode controller;
[0197] According to the dynamic characteristics described in step S2, the non-singular terminal sliding mode controller described in step S3 is simplified:
[0198]
[0199] The simplified controller control input only consists of the estimated gravity term and the approximation term.
[0200] Specifically, step S5: using delay estimation to obtain time-varying gravity.
[0201] According to the dynamic characteristics described in step S2, the time-varying gravity can be estimated based on the output u of the controller:
[0202] G p ≈J T u
[0203] From step S4, we can see that the output u of the controller at the current moment needs to be known under the condition that G p Under the condition of obtaining, according to the characteristic that gravity remains unchanged in a short time, the current moment can be estimated by delaying one sampling cycle:
[0204] G p (t)≈J T u(tL)
[0205] Where: G p (t) represents the estimated gravity at the current moment; u(tL) represents the output of the controller in the previous sampling period at the current moment.
[0206] Specifically, step S6: filtering the time-varying gravity based on a Kalman filter algorithm.
[0207] Due to the existence of control noise and external interference, the estimated gravity G obtained directly in step S5p (t) is inaccurate, and it is necessary to use a filter to estimate the gravity G with time delay p (t) is filtered, based on the Kalman filter, and the abstract state transfer equation is designed as:
[0208]
[0209] G rk =γG r(k-1) +(1-λ)G p(k-1)
[0210] Where: G pk Represents the estimated gravity in the world coordinate system at the current moment; G r(k-1) Represents the reference force, which depends on the constant reference gravity; G p(k-1) represents the estimated gravity at the last sampling time; p∈(0,1) represents the weight of the last estimated gravity; G l represents the decomposition of the gravity of the top platform on the six branches, which is the estimated control input u; w k-1 and v k represents noise; λ represents the forgetting factor;
[0211] Combined with the classical Kalman filter equation, the prediction part and the correction part of the gravity Kalman filter are designed; the prediction part of the gravity Kalman filter is
[0212]
[0213] Where: P k represents the prior estimation error covariance; Q represents the process noise covariance;
[0214] The correction part of the gravity Kalman filter is
[0215]
[0216] Where: K k represents the Kalman gain; R represents the measurement noise covariance; represents the control input force of the six branches; represents the estimated gravity of the top platform;
[0217] The gravity term in the inertial system is decomposed into 6 branches, and the gravity matrix of the wave compensation platform in the joint space used by the non-singular terminal sliding mode control law is:
[0218]
[0219] The gravity matrix in the joint space is the estimated gravity in the non-singular terminal sliding mode control law.
[0220] Specifically, step S7, calculating the branch target length change rate through second-order filtering.
[0221] The velocity feedforward term is obtained by a second-order filter
[0222]
[0223] Where: z1(0) = l d (0); In process ω represents the natural frequency; ζ represents the damping ratio.
[0224] Specifically, step S8, introducing a speed feedforward term into the controller;
[0225] Introducing a speed feedforward term in the controller, the control input of the non-singular terminal sliding mode controller to the drive is
[0226]
[0227] Where: K v Represents the feedforward gain.
[0228] Large speed tracking errors may lead to larger Coriolis force interference. Therefore, high-precision control requires the controller to have good speed tracking performance.
[0229] Introducing the speed feedforward term into the controller can effectively overcome the damping force and improve the speed tracking performance.
[0230] The present invention also provides a control system for a Stewart wave compensation platform. The control system of the Stewart wave compensation platform can be implemented by executing the process steps of the control method of the Stewart wave compensation platform, that is, those skilled in the art can understand the control method of the Stewart wave compensation platform as a preferred implementation of the control system of the Stewart wave compensation platform.
[0231] Build a simulation model for verification, and then build a simulation model in the Simspace module of MATLAB / Simulink. The model is as follows Figure 3 The lower Stewart platform simulates wave motion and the upper Stewart platform compensates for the simulated waves.
[0232] Three controllers are compared: PID controller, sliding mode controller based on backstepping method (RBSMC) and non-singular terminal sliding mode controller with adaptive gravity estimation (NTSMC-GE) proposed in this paper.
[0233] Configure the simulation environment: input the random wave shown in Figure 4 to the lower Stewart to simulate the ship motion; design the top platform center of mass of the upper Stewart platform as a sinusoidal motion to simulate time-varying gravity; the six-degree-of-freedom motion information of the ship relative to the world coordinate system is measured by the digital sensor of Simulink, and its signal output is passed through a first-order low-pass filter to indicate the measurement inaccuracy; add square wave and sine wave external interference at the center of mass of the upper platform to test the anti-interference ability of the system.
[0234] The compensation effects of the three controllers are shown in Figure 5. Figure 5a and Figure 5d It can be seen that the anti-interference ability of PID control is very limited, and the residual motion is easily affected by interference. Figure 5b and Figure 5e It can be seen that RBSMC shows excellent anti-interference and tracking capabilities. Even in the presence of interference, the residual motion is smooth. However, due to measurement errors and parameter uncertainties, the residual motion of RBSMC is still large. A low-pass filter with a cutoff frequency of 5Hz will cause sinusoidal residual motion. Parameter uncertainty and gravity inaccuracy will cause zero drift problems. Due to the uncertainty of gravity, the residual displacement of RBSMC is worse than PID, because PID has an integral term to compensate for gravity. From Figure 5c and Figure 5f It can be seen that the proposed NTSMC-GE method has the ability to resist interference and eliminate drift. On the one hand, NTSMC-GE is robust enough to resist interference and keep the residual motion smooth. On the other hand, although the exact parameters of the platform are unknown, with the help of the gravity estimator, the proposed method can still compensate for gravity and eliminate zero drift. Compared with RBSMC, NTSMC-GE has stronger drift resistance and similar robustness to interference. Since RBSMC requires too much information, including sensors, inertia, velocity, and acceleration, the advantage of model-free NTSMC-GE is obvious.
[0235] Figure 6 shows the control outputs of the three controllers in the random wave test. Figures 6a to 6c Different colors represent 6 branches. Since the ship motion sensor adds white noise, the control target of each leg is not completely smooth, so the control output will inevitably experience jitter. The jitter problem of RBSMC is obvious, but it is stable within a certain range, and the control power consumed is greater than that of the other two controllers. The nonlinear convergence law of NTSMC-GE proposed in this invention weakens the jitter, and the jitter problem is acceptable compared with RBSMC and PID.
[0236] The effect of gravity estimation by combining time delay estimation with Kalman filtering is shown in FIG7 . Figure 7aThe yellow line in the figure represents the actual gravity that needs to be compensated, and the dotted line represents the actual control output. It can be seen that during the compensation process, when the residual motion is well controlled, the input force of each leg in the system is approximately equal to the gravity, which confirms the system dynamic characteristics proposed in the present invention. The method proposed in the present invention uses the control output of the system as a reference value to estimate the actual gravity component of each leg of the system. The estimated value of gravity (solid line) and the actual value (dotted line) are shown in Figure 2. Figure 7b As shown, the time-varying gravity component is accurate and smooth enough to be used as part of the controller control output. The gravity estimation method of the present invention can be used to estimate the time-varying gravity and improve the control accuracy of the system.
[0237] The displacement error and angle error are defined as
[0238]
[0239] Where: e x , e y , e z are real-time compensation displacement absolute error, e Rx , e Ry , e Rz is the absolute error of the rotation angle compensated in real time. Arrange the N absolute error observations from large to small, and get the peak height by averaging the first N / 10 error points, and designate it as the significant residual motion. The calculation formula is:
[0240]
[0241] Among them, e i Represents position error or rotation error. The significant compensation rate is calculated as η c
[0242]
[0243] Among them, a 1 / 10 Indicates significant excitation motion caused by wave excitation.
[0244] Table 1
[0245]
[0246] The compensation performance of the three controllers is shown in Table 1. The proposed NTSMC-GE method achieves satisfactory results in terms of anti-interference performance and small residual motion, with a position compensation rate of up to 94.46% and a rotation compensation rate of up to 94.39%. Methods similar to RBSMC are based on accurate system dynamics models, but the actual system model parameters are constantly changing, and the actual measurement of the ship's position is not accurate, so it is difficult to achieve ideal control performance. The NTSMC-GE method has a satisfactory model-free characteristic, which can achieve gravity adaptive estimation and robust control without an accurate system dynamics model, which also makes it easier to apply in the engineering field.
[0247] Those skilled in the art know that, in addition to realizing the system and its various devices, modules, and units provided by the present invention in a purely computer-readable program code, it is entirely possible to realize the same functions in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system and its various devices, modules, and units provided by the present invention can be considered as a hardware component, and the devices, modules, and units included therein for realizing various functions can also be regarded as structures within the hardware component; the devices, modules, and units for realizing various functions can also be regarded as both software modules for realizing the method and structures within the hardware component.
[0248] The above describes the specific embodiments of the present invention. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the present invention. In the absence of conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.
Claims
1. A control method for a Stewart heave compensation platform, It is characterized in that include: Step S1: Obtain the hull motion information, perform kinematic inverse solution to obtain the target length of the six branches to be compensated; Step S2: Based on the dynamic characteristics of the Stewart wave compensation platform, the time-varying gravity is obtained by using delay estimation; Step S3: filtering the time-varying gravity based on the Kalman filter to obtain the filtered time-varying gravity; Step S4: Calculate the branch target length change rate through second-order filtering; Step S5: using the target length change rate, a velocity feedforward term is introduced into the non-singular terminal sliding mode controller; based on the target length and the filtered time-varying gravity, the drive input is calculated using the non-singular terminal sliding mode controller with the velocity feedforward term introduced to realize the heave compensation control of the Stewart heave compensation platform.
2. The control method of the Stewart heave compensation platform according to claim 1, It is characterized in that The step S1 adopts: The posture sensor is connected to the lower platform of the wave compensation platform and fixed at the center of the plane of the hinge point of the lower platform to continuously obtain the posture information of the hull movement. b ; Based on the acquired hull motion posture information, the target lengths of the six branches are obtained by inverse kinematics solution: Among them: id represents the target length of the i-th branch; l id represents the target vector of the i-th branch from the lower platform hinge point to the upper platform hinge point; Represents the position information x according to the motion of the ship b The displacement vector of the upper platform of wave compensation relative to the lower platform is obtained; R btd Represents the position information x according to the motion of the ship b The obtained rotation matrix of the upper platform of wave compensation relative to the lower platform; t i b represents the coordinates of the spherical joint point of the ith branch chain on the wave compensation platform; i It represents the coordinates of the Hooke's hinge point of the i-th branch chain of the platform under wave compensation; i represents the sequence number of the branch chain, there are 6 branches in total, i=1, 2, 3, 4, 5, 6.
3. The control method of the Stewart heave compensation platform according to claim 1, It is characterized in that The Stewart wave compensation platform dynamic characteristics in step S2 include: obtaining the Stewart wave compensation platform dynamic characteristics based on the Stewart wave compensation platform dynamic equation; According to the Newton-Euler formula, the dynamic equation of the platform on the wave compensation platform in the inertial system is: Where: M p represents the generalized mass matrix of the platform on the compensation platform in the inertial frame; represents the generalized acceleration of the compensation platform in the inertial system; C p represents the generalized Coriolis matrix of the platform on the compensation platform in the inertial frame; G represents the generalized velocity of the compensation platform in the inertial system; p represents the gravity matrix of the platform on the compensation platform in the inertial system; F a Indicates the active external force on the compensation platform in the current state; The dynamic equation of the platform on the wave compensation platform in the non-inertial system is: Where: M l represents the generalized mass matrix of the heave compensation platform in joint space; Indicates the acceleration of the actual length change of the branch chain; C l The generalized Coriolis matrix representing the heave compensation platform in joint space; Indicates the speed at which the actual length of the branch chain changes; G l represents the generalized gravity matrix of the heave compensation platform in the joint space; M bl represents the generalized mass matrix of the lower platform in the joint space; represents the acceleration of the lower platform; C b1 represents the generalized Coriolis matrix of the lower plateau in the joint space; Indicates the speed of the lower platform; f a represents the driving force of the branch chain; According to the principle of virtual work, f a With F a Have the following relationship: F a =See a Combining the dynamic equations in the two reference frames, we get the inertial force F ml The expression is: When full compensation is achieved, the length of the branch chain, the speed of length change and the acceleration all approach the target, which is equivalent to making the end platform and Inertial force F ml Approaching: At this point, the branch chain output should only be gravity: G l =f a ; The dynamic characteristic of the heave compensation platform when achieving full compensation is that the output of the six branches is approximately equal to the decomposition of the platform's gravity on the six branches.
4. The control method of the Stewart heave compensation platform according to claim 1, It is characterized in that The non-singular terminal sliding mode controller comprises: constructing a non-singular terminal sliding mode controller based on the dynamic characteristics of the Stewart heave compensation platform and in combination with a non-singular terminal sliding mode control method; The designed non-singular terminal sliding surface is: Where: e = l d -l represents the following error of the branch length; β = diag (β 1 ,…,β 6 ), γ represents the custom parameter of the designed non-singular terminal sliding surface, and its value range is β i >0,1<γ<2,l d represents the target length of the six branches, and l represents the actual length of the branches; The terminal fast sliding mode approximation rate is defined as: Among them, k 1 =diag(k 11 ,…,k 16 ), k 2 =diag(k 21 ,…,k 26 ), and ρ represents the custom parameter of the terminal fast sliding mode approximation rate, with a value range of k 1i ,k 2i >0,0<ρ<1; Combined with the non-singular terminal sliding mode control algorithm, a non-singular terminal sliding mode controller is constructed: in: is the generalized gravity matrix of the heave compensation platform in the estimated joint space; According to the dynamic characteristics, the non-singular terminal sliding mode controller is simplified; 5. The control method of the Stewart heave compensation platform according to claim 1, It is characterized in that The step S2 adopts: According to the dynamic characteristics, the time-varying gravity is estimated according to the output u of the non-singular terminal sliding mode controller: G p ≈J T u The output u of the non-singular terminal sliding mode controller at the current moment needs to be known under G p Under the condition of obtaining, according to the characteristic that gravity remains unchanged in a short time, the current moment can be estimated by delaying one sampling cycle: G p (t)≈J T u(t-L) Where: G p (t) represents the estimated gravity at the current moment; u(tL) represents the output of the controller in the previous sampling period at the current moment.
6. The control method of the Stewart heave compensation platform according to claim 1, It is characterized in that The step S3 adopts: Based on the Kalman filter, the abstract state transfer equation is designed as: G rk =γG r(k-1) +(1-λ)G p(k-1) Where: G pk Represents the estimated gravity in the world coordinate system at the current moment; G r(k-1) Represents the reference force, which depends on the constant reference gravity; G p(k-1) represents the estimated gravity at the last sampling time; p∈(0,1) represents the weight of the last estimated gravity; G l represents the decomposition of the gravity of the top platform on the six branches, which is the estimated control input u; w k-1 and v k represents noise; λ represents the forgetting factor; Combined with the classical Kalman filter equation, the prediction part and the correction part of the gravity Kalman filter are designed; the prediction part of the gravity Kalman filter is Where: P k represents the prior estimation error covariance; Q represents the process noise covariance; The correction part of the gravity Kalman filter is Where: K k represents the Kalman gain; R represents the measurement noise covariance; represents the control input force of the six branches; represents the estimated gravity of the top platform; The gravity term in the inertial system is decomposed into 6 branches, and the gravity matrix of the wave compensation platform in the joint space used by the non-singular terminal sliding mode control law is: The gravity matrix in the joint space is the time-varying gravity in the non-singular terminal sliding mode controller.
7. The control method of the Stewart heave compensation platform according to claim 1, It is characterized in that The step S4 adopts: The velocity feedforward term is obtained by a second-order filter Where: z 1 (0) = l d (0); In process ω represents the natural frequency; ζ represents the damping ratio.
8. The control method of the Stewart heave compensation platform according to claim 1, It is characterized in that The step S5 adopts: Introducing a speed feedforward term in the controller, the control input of the non-singular terminal sliding mode controller to the drive is Where: K v Represents the feedforward gain.
9. A control system for a Stewart heave compensation platform, It is characterized in that include: Module M1: Obtain the hull motion information and perform kinematic inverse solution to obtain the target length of the six branches to be compensated; Module M2: Based on the dynamic characteristics of the Stewart wave compensation platform, time-delay estimation is used to obtain time-varying gravity; Module M3: filtering the time-varying gravity based on the Kalman filter to obtain the filtered time-varying gravity; Module M4: Calculate the change rate of branch target length through second-order filtering; Module M5: A velocity feedforward term is introduced into the non-singular terminal sliding mode controller using the target length change rate. Based on the target length and filtered time-varying gravity, the drive input is calculated using the non-singular terminal sliding mode controller with the velocity feedforward term introduced to achieve heave compensation control of the Stewart heave compensation platform.
10. The control system of the Stewart heave compensation platform according to claim 9, It is characterized in that The module M1 adopts: The posture sensor is connected to the lower platform of the wave compensation platform and fixed at the center of the plane of the hinge point of the lower platform to continuously obtain the posture information of the hull movement. b ; Based on the acquired hull motion posture information, the target lengths of the six branches are obtained by inverse kinematics solution: Among them: id represents the target length of the i-th branch; l id represents the target vector of the i-th branch from the lower platform hinge point to the upper platform hinge point; Represents the position information x according to the motion of the ship b The displacement vector of the upper platform of wave compensation relative to the lower platform is obtained; R btd Represents the position information x according to the motion of the ship b The obtained rotation matrix of the upper platform of wave compensation relative to the lower platform; t i b represents the coordinates of the spherical joint point of the ith branch chain on the wave compensation platform; i It represents the coordinates of the Hook hinge point of the ith branch chain of the platform under wave compensation; i represents the sequence number of the branch chain, there are 6 branches in total, i=1, 2, 3, 4, 5, 6; The Stewart wave compensation platform dynamic characteristics in the module M2 include: obtaining the Stewart wave compensation platform dynamic characteristics based on the Stewart wave compensation platform dynamic equation; According to the Newton-Euler formula, the dynamic equation of the platform on the wave compensation platform in the inertial system is: Where: M p represents the generalized mass matrix of the platform on the compensation platform in the inertial frame; represents the generalized acceleration of the compensation platform in the inertial system; C p represents the generalized Coriolis matrix of the platform on the compensation platform in the inertial frame; G represents the generalized velocity of the compensation platform in the inertial system; p represents the gravity matrix of the platform on the compensation platform in the inertial system; F a Indicates the active external force on the compensation platform in the current state; The dynamic equation of the platform on the wave compensation platform in the non-inertial system is: Where: M l represents the generalized mass matrix of the heave compensation platform in joint space; Indicates the acceleration of the actual length change of the branch chain; C l The generalized Coriolis matrix representing the heave compensation platform in joint space; Indicates the speed at which the actual length of the branch chain changes; G l represents the generalized gravity matrix of the heave compensation platform in the joint space; M bl represents the generalized mass matrix of the lower platform in the joint space; represents the acceleration of the lower platform; C bl represents the generalized Coriolis matrix of the lower plateau in the joint space; Indicates the speed of the lower platform; f a represents the driving force of the branch chain; According to the principle of virtual work, f a With F a Have the following relationship: F a =See a Combining the dynamic equations in the two reference frames, we get the inertial force F ml The expression is: When full compensation is achieved, the length of the branch chain, the speed of length change and the acceleration all approach the target, which is equivalent to making the end platform and Inertial force F ml Approaching: At this point, the branch chain output should only be gravity: G 1 =f a ; The dynamic characteristics of the heave compensation platform when achieving full compensation are as follows: the output of the six branches is approximately equal to the decomposition of the platform's gravity on the six branches; The non-singular terminal sliding mode controller comprises: constructing a non-singular terminal sliding mode controller based on the dynamic characteristics of the Stewart heave compensation platform and in combination with a non-singular terminal sliding mode control method; The designed non-singular terminal sliding surface is: Where: e = l d -l represents the following error of the branch length; β = diag (β 1 ,…,β 6 ), γ represents the custom parameter of the designed non-singular terminal sliding surface, and its value range is β i >0,1<γ<2,l d represents the target length of the six branches, and l represents the actual length of the branches; The terminal fast sliding mode approximation rate is defined as: Among them, k 1 =diag(k 11 ,…,k 16 ), k 2 =diag(k 21 ,…,k 26 ), and ρ represents the custom parameter of the terminal fast sliding mode approximation rate, with a value range of k 1i ,k 2i >0,0<ρ<1; Combined with the non-singular terminal sliding mode control algorithm, a non-singular terminal sliding mode controller is constructed: in: is the generalized gravity matrix of the heave compensation platform in the estimated joint space; According to the dynamic characteristics, the non-singular terminal sliding mode controller is simplified; The module M2 adopts: According to the dynamic characteristics, the time-varying gravity is estimated according to the output y of the non-singular terminal sliding mode controller: G p ≈J T u The output u of the non-singular terminal sliding mode controller at the current moment needs to be known under G p Under the condition of obtaining, according to the characteristic that gravity remains unchanged in a short time, the current moment can be estimated by delaying one sampling cycle: G p (t)≈J T u(t-L) Where: G p (t) represents the estimated gravity at the current moment; u(tL) represents the output of the controller in the previous sampling period at the current moment; The module M3 adopts: Based on the Kalman filter, the abstract state transfer equation is designed as: G rk =γG r(k-1) +(1-λ)G p(k-1) Where: G pk Represents the estimated gravity in the world coordinate system at the current moment; G r(k-1) Represents the reference force, which depends on the constant reference gravity; G p(k-1) represents the estimated gravity at the last sampling time; p∈(0,1) represents the weight of the last estimated gravity; G l represents the decomposition of the gravity of the top platform on the six branches, which is the estimated control input u; w k-1 and v k represents noise; λ represents the forgetting factor; Combined with the classical Kalman filter equation, the prediction part and the correction part of the gravity Kalman filter are designed; the prediction part of the gravity Kalman filter is Where: P k represents the prior estimation error covariance; Q represents the process noise covariance; The correction part of the gravity Kalman filter is Where: K k represents the Kalman gain; R represents the measurement noise covariance; represents the control input force of the six branches; represents the estimated gravity of the top platform; The gravity term in the inertial system is decomposed into 6 branches, and the gravity matrix of the wave compensation platform in the joint space used by the non-singular terminal sliding mode control law is: The gravity matrix in the joint space is the time-varying gravity in the non-singular terminal sliding mode controller; The module M4 adopts: The velocity feedforward term is obtained by a second-order filter Where: z 1 (0) = l d (0); In process ω represents the natural frequency; ζ represents the damping ratio; The module M5 adopts: Introducing a speed feedforward term in the controller, the control input of the non-singular terminal sliding mode controller to the drive is Where: K v Represents the feedforward gain.
Citation Information
Patent Citations
Wave compensation control system based on feedforward PID control algorithm
CN111708274A
Visual servo neural network control method for six-degree-of-freedom parallel swinging table
CN115598966A
Cited By
Electromagnetic force compensation method and system based on virtual displacement
CN120595876A
Model-free finite time saturation control method for wave compensation platform
CN122308420A
