CN116126003A - Wave compensation system modeling and pose control method based on Stewart platform - Google Patents

Wave compensation system modeling and pose control method based on Stewart platform Download PDF

Info

Publication number
CN116126003A
CN116126003A CN202211504719.XA CN202211504719A CN116126003A CN 116126003 A CN116126003 A CN 116126003A CN 202211504719 A CN202211504719 A CN 202211504719A CN 116126003 A CN116126003 A CN 116126003A
Authority
CN
China
Prior art keywords
platform
stewart
coordinate system
pose
matrix
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211504719.XA
Other languages
Chinese (zh)
Inventor
张卫东
尹荣基
陈卫星
熊明磊
谢威
耿雄飞
何伟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hainan University
Original Assignee
Hainan University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hainan University filed Critical Hainan University
Priority to CN202211504719.XA priority Critical patent/CN116126003A/en
Publication of CN116126003A publication Critical patent/CN116126003A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G05D1/0875—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted to water vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Feedback Control In General (AREA)

Abstract

The invention relates to a wave compensation system modeling and pose control method based on a Stewart platform, which comprises the following steps: step 1: carrying out dynamics analysis on the shipborne Stewart platform and obtaining a force balance equation; step 2: calculating the action of the step bridge mechanism on the Stewart upper platform; step 3: expanding a force balance equation based on the action of a step bridge mechanism on a Stewart upper platform, and further establishing a ship-borne Stewart platform dynamics model considering a step bridge; step 4: and designing a Stewart platform pose controller based on a backstepping method to control the pose of the Stewart platform. Compared with the prior art, the invention has the advantages of improving the wave compensation precision, improving the pose control precision and the like.

Description

Wave compensation system modeling and pose control method based on Stewart platform
Technical Field
The invention relates to the technical field of automatic control of a nonlinear full-driving system, in particular to a wave compensation system modeling and pose control method based on a Stewart platform.
Background
The operation behaviors such as fan maintenance, cargo transportation and the like performed at sea need a stable shipborne operation platform, and the seaborne wind waves can cause shaking, heave and the like of the shipborne platform, so that the motion compensation of the environmental factors such as the wind waves and the like is performed through a wave compensation system, and the stability of the shipborne operation platform is ensured. The wave compensation system can be divided into single-degree-of-freedom compensation and multi-degree-of-freedom compensation according to the number of degrees of freedom of compensation, wherein the former is low in control difficulty and poor in compensation effect. The scheme commonly used in the multi-degree-of-freedom compensation is to use a Stewart platform with high precision and good rigidity to perform motion compensation on six degrees of freedom, and simultaneously load a step bridge mechanism on the Stewart upper platform so as to cope with different operation tasks.
As a main mechanism for actively compensating wave interference, the Stewart platform is complex in model, and besides the step bridge pitching, telescoping and gyrating can cause asymmetric platform load; and then, the problems of difficult modeling, difficult precise control and the like of the Stewart platform and the offshore boarding step bridge coupling system are caused by the ship movement caused by the offshore wind wave. The existing control method simply treats the control method as interference, and the control effect is limited. For this reason, how to determine the effects of vessel motions and the working conditions of the step bridge in the dynamics model is a core problem faced by the application of the Stewart platform in a wave compensation system.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provide a wave compensation system modeling and pose control method based on a Stewart platform.
The aim of the invention can be achieved by the following technical scheme:
the invention provides a wave compensation system modeling and pose control method based on a Stewart platform, which comprises the following steps:
step 1: carrying out dynamics analysis on the shipborne Stewart platform and obtaining a force balance equation;
step 2: calculating the action of the step bridge mechanism on the Stewart upper platform;
step 3: expanding a force balance equation based on the action of a step bridge mechanism on a Stewart upper platform, and further establishing a ship-borne Stewart platform dynamics model considering a step bridge;
step 4: and designing a Stewart platform pose controller based on a backstepping method to control the pose of the Stewart platform.
Preferably, in the step 1, the process of dynamically analyzing the shipboard Stewart platform and obtaining the force balance equation specifically includes the following steps:
step 101: three coordinate systems are set up, namely an inertial coordinate system { O } w Is fixed on the ship body coordinate system { O }, of the ship b Platform coordinate System { O } fixed to Stewart upper platform t And in inertial coordinate system { O } w Calculating the speed and angular velocity of the mass center of the platform on Stewart in an inertial coordinate system { O }, and w the expressions for the speed and angular velocity of the platform centroid at Stewart are:
Figure BDA0003967761290000021
/>
Figure BDA0003967761290000022
wherein ,
Figure BDA0003967761290000023
and />
Figure BDA0003967761290000024
Respectively the angular velocity and the linear velocity of the upper platform in the inertial coordinate system,
Figure BDA0003967761290000025
and Jp ＝[J pl J pr ]The angular velocity and the linear velocity of the mass center of the upper platform are respectively in an inertial coordinate system { O w Matrix of } and pose->
Figure BDA0003967761290000026
Conversion matrix between>
Figure BDA0003967761290000027
and />
Figure BDA0003967761290000028
Respectively represent the platform coordinate system { O } t And hull coordinate system { O } b Each } is associated with generalized angular velocity +.>
Figure BDA0003967761290000029
Jacobian matrix, J pl and Jpr Respectively represent the platform coordinate system { O } t And hull coordinate system { O } b Each of } is associated with generalized speed +.>
Figure BDA00039677612900000210
Jacobian matrix between the two, S (·) represents the rotation, R wb Representing the hull coordinate system { O } b And inertial coordinate system { O } w Rotation matrix between }, ∈>
Figure BDA00039677612900000211
Representing the platform coordinate system { O t { O } relative to the hull coordinate system b Pose }, ->
Figure BDA00039677612900000212
Representing the hull coordinate system { O } b Relative to inertial coordinate system { O } w Pose, ψ, θ and +.>
Figure BDA00039677612900000213
All are Euler angles, x, y and z are respectively horizontal, vertical and vertical coordinates, represent positions,
Figure BDA00039677612900000214
platform coordinate system { O t { O } relative to the hull coordinate system b Location, J I ＝[0 3×3 ,I 3×3 ]Is a constant matrix>
Figure BDA00039677612900000215
and />
Figure BDA00039677612900000216
Respectively represent the platform coordinate system { O } t { O } relative to the hull coordinate system b Pose q of } t And hull coordinate system { O b Relative to inertial coordinate system { O } w Pose q of } b A conversion matrix between the respective Euler angular velocity and the own angular velocity;
conversion matrix
Figure BDA00039677612900000217
and />
Figure BDA00039677612900000218
The expressions of (2) are respectively:
Figure BDA00039677612900000219
Figure BDA0003967761290000031
step 102: the speed and the angular speed of the mass center of the platform on Stewart are calculated by deriving, and the expressions of the obtained acceleration and the obtained angular acceleration are respectively as follows:
Figure BDA0003967761290000032
Figure BDA0003967761290000033
wherein ,
Figure BDA0003967761290000034
and />
Figure BDA0003967761290000035
Respectively representing the acceleration and the angular acceleration of the upper platform in an inertial coordinate system;
step 103: and acquiring a force balance equation by utilizing a virtual work principle based on a kinematic calculation result.
Preferably, in the step 103, the process of obtaining the force balance equation according to the kinematic calculation result by using the virtual work principle is specifically:
the inertial force of the upper platform centroid is calculated according to the acceleration of the upper platform centroid, and is converted into a platform coordinate system { O } by combining with the virtual work principle t { O } relative to the hull coordinate system b Pose q of } t The expression of the inertial force after conversion is as follows:
Figure BDA0003967761290000036
wherein ,FI Is the inertia force, m, of the Stewart upper platform p For Stewart upper platform quality, I t The moment of inertia matrix of the Stewart upper platform under the inertia system;
the mass center of the upper platform receives forces including inertial force, equivalent acting force of six driving rods and gravity, and all the forces are in a platform coordinate system { O t { O } relative to the hull coordinate system b Pose q of } t The lower equilibrium, and hence the expression of the force balance equation, is:
Figure BDA0003967761290000037
wherein ,FI Is the inertia force, m, of the Stewart upper platform p G is gravity acceleration, f is the mass of a Stewart upper platform a ＝(f a1 f a2 f a3 f a4 f a5 f a6 ) T F is the input of the system a1 、f a2 、f a3 、f a4 、f a5 and fa6 Indicating the driving force required for the six driving levers.
Preferably, in the step 2, in the inertial coordinate system, the action of the step bridge mechanism on the Stewart upper platform includes an acting force-f 1 w And forceMoment (V)
Figure BDA0003967761290000038
The process for calculating the action of the step bridge mechanism on the Stewart upper platform specifically comprises the following steps:
step 201: corresponding coordinate systems { O } are respectively established at the rotation, pitching and telescoping joints of the step bridge mechanism according to the Denavit-Hartenberg method 1 }、{O 2} and {O3 }；
Step 202: solving the acceleration and the angular acceleration of each connecting rod centroid through forward iteration by using an iterative Newton-Euler method;
step 203: solving acting force f of connecting rod on Stewart upper platform through reverse iteration 1 1 And action moment
Figure BDA0003967761290000041
Step 204: the connecting rod is acted by the acting force f of the upper platform 1 1 And action moment
Figure BDA0003967761290000042
Conversion to inertial coordinate System { O by rotation matrix w In the { O } inertial coordinate system w Force f of Stewart upper platform on connecting rod 1 w And action moment->
Figure BDA0003967761290000043
Step 205: obtain { O in inertial coordinate system w Force-f of step bridge mechanism on Stewart upper platform 1 w And action moment
Figure BDA0003967761290000045
Preferably, in the step 204, the inertial coordinate system { O } w Force f of Stewart upper platform on connecting rod 1 w And action moment
Figure BDA0003967761290000046
The expressions of (2) are respectively:
Figure BDA0003967761290000047
Figure BDA0003967761290000048
wherein ,mbrige-f ∈R 3×3 and mbrige-n ∈R 3×3 Respectively representing the impact matrix of the action force and the action moment of the walking bridge on the acceleration, M brige-f ∈R 3×3 and Mbrige-n ∈R 3×3 The influence matrix of the acting force and the acting moment of the walking bridge on the angular acceleration is respectively C brige-f ∈R 3×3 and Cbrige-n ∈R 3×3 Matrix of influence, M, representing the angular velocity of the action and moment of action of the bridge, respectively θ-f ∈R 3×3 and Mθ-n ∈R 3×3 Respectively representing additional term matrixes introduced by acting force and acting moment of the walking bridge;
preferably, in the step 3, the dynamics model of the shipboard Stewart platform considering the step bridge is:
Figure BDA0003967761290000049
wherein ,
Figure BDA00039677612900000410
representing the platform coordinate system { O t { O } relative to the hull coordinate system b Pose }, ->
Figure BDA00039677612900000411
Representing the hull coordinate system { O } b Relative to inertial coordinate system { O } w Pose, ψ, θ and +.>
Figure BDA00039677612900000412
Are Euler angles, J epsilon R 6×6 Representation of SteMotion jacobian matrix of wart platform, f a ＝(f a1 f a2 f a3 f a4 f a5 f a6 ) T F is the input of the system a1 、f a2 、f a3 、f a4 、f a5 and fa6 Representing the driving force required by six driving rods, M t Representing a task space equivalent inertia matrix, C t Representing a first order matrix of task space, G t Represents the gravity term, M b Representing a second order influence matrix of ship motion, C b Representing a first order influence matrix of ship motion, M θ Representing the additional matrix introduced by the step bridge effect.
Preferably, in the step 4, the process of designing the Stewart platform pose control algorithm based on the backstepping method specifically includes:
step 401: pose q of Stewart upper platform relative to lower platform t For the controlled quantity, a platform coordinate system { O } t { O } relative to the hull coordinate system b The expected pose of } is q td And calculate the first derivative thereof
Figure BDA00039677612900000413
And second derivative->
Figure BDA00039677612900000414
Step 402: and (5) designing a control algorithm through a back-stepping method to obtain the pose controller.
Preferably, in the step 401, the first derivative
Figure BDA0003967761290000051
And second derivative->
Figure BDA0003967761290000052
The calculated expressions of (a) are respectively:
Figure BDA0003967761290000053
Figure BDA0003967761290000054
wherein ,
Figure BDA0003967761290000055
and />
Figure BDA0003967761290000056
For rotating matrix R wb First and second derivatives of (a);
preferably, the rotation matrix R wb The expressions of the first derivative and the second derivative of (c) are:
Figure BDA0003967761290000057
Figure BDA0003967761290000058
preferably, in the step 402, the expression of the pose controller is:
Figure BDA0003967761290000059
wherein ,k1 and k2 Are all positive parameters of the controller and are set,
Figure BDA00039677612900000510
compared with the prior art, the invention has the following beneficial effects:
1. according to the invention, the kinetic influence of ship motion on the Stewart platform is considered, the motion of the ship is reflected in a dynamic model of the system, the relative pose between the Stewart upper platform and the Stewart lower platform is taken as a controlled quantity, an algorithm for calculating the expected value of the relative pose between the Stewart upper platform and the Stewart lower platform according to the ship motion is provided, the shaking, the heave and the like of the Stewart upper platform caused by sea stormy waves are effectively compensated, and the wave compensation precision is further effectively improved.
2. The invention brings the action of the step bridge mechanism into the Stewart platform dynamics model, solves the error caused by asymmetric load due to the change of the step bridge working condition, and improves the pose control precision.
Drawings
FIG. 1 is a flow chart of the method of the present invention.
Fig. 2 is a schematic diagram of the dynamic analysis coordinate system of the shipboard Stewart platform of the present invention.
Fig. 3 is a schematic diagram of the spatial coordinate system of the joints of the walking bridge mechanism of the present invention.
Detailed Description
The invention will now be described in detail with reference to the drawings and specific examples. The present embodiment is implemented on the premise of the technical scheme of the present invention, and a detailed implementation manner and a specific operation process are given, but the protection scope of the present invention is not limited to the following examples.
As shown in fig. 1, the invention provides a wave compensation system modeling and pose control method based on a Stewart platform, which comprises the following steps:
step 1: carrying out dynamics analysis on the shipborne Stewart platform and obtaining a force balance equation;
step 2: calculating the action of the step bridge mechanism on the Stewart upper platform, wherein the action of the step bridge mechanism on the Stewart upper platform in an inertial coordinate system comprises acting force-f 1 w And action moment
Figure BDA0003967761290000061
Step 3: introducing a step bridge mechanism to act on a Stewart upper platform to expand a Stewart platform force balance equation, and establishing a ship-borne Stewart platform dynamics model considering the step bridge;
step 4: and designing a Stewart platform pose control algorithm based on a backstepping method to control the pose of the Stewart platform.
In step 1, the process of dynamically analyzing the shipborne Stewart platform and obtaining the force balance equation specifically includes the following steps:
as shown in fig. 2, three coordinate systems are first defined: inertial coordinate System { O w -hull coordinate system fixed to the vessel (i.e. Stewart lower platform) { O } b Platform coordinate System { O } fixed to Stewart upper platform t }；
In inertial coordinate system { O w The angular and linear velocities obtained from the velocity and angular velocity analysis of the centroid of the platform on Stewart
Figure BDA0003967761290000062
Figure BDA0003967761290000063
wherein ,
Figure BDA0003967761290000064
and />
Figure BDA0003967761290000065
Respectively the angular velocity and the linear velocity of the upper platform in the inertial coordinate system,
Figure BDA0003967761290000066
and Jp ＝[J pl J pr ]The angular velocity and the linear velocity of the mass center of the upper platform are respectively in an inertial coordinate system { O w And } and->
Figure BDA0003967761290000067
Conversion matrix between>
Figure BDA0003967761290000068
and />
Figure BDA0003967761290000069
Respectively represent the platform coordinate system { O } t And hull coordinate system { O } b Each } is associated with generalized angular velocity +.>
Figure BDA00039677612900000610
Jacobian matrix, J pl and Jpr Respectively represent the platform coordinate system { O } t And hull coordinate system { O } b Each of } is associated with generalized speed +.>
Figure BDA00039677612900000611
Jacobian matrix between the two, S (·) represents the rotation, R wb Representing the hull coordinate system { O } b And inertial coordinate system { O } w Rotation matrix between R bt Representing the platform coordinate system { O t { O } and hull coordinate system b A rotation matrix between the two,
Figure BDA00039677612900000612
representing the platform coordinate system { O t { O } relative to the hull coordinate system b The pose of the three-dimensional model is,
Figure BDA00039677612900000613
representing the hull coordinate system { O } b Relative to inertial coordinate system { O } w Pose, ψ, θ and
Figure BDA00039677612900000614
all are Euler angles->
Figure BDA00039677612900000615
Platform coordinate system { O t { O } relative to the hull coordinate system b Location, J I ＝[0 3×3 ,I 3×3 ]Is a constant matrix>
Figure BDA00039677612900000616
and />
Figure BDA00039677612900000617
Respectively represent the platform coordinate system { O } t { O } relative to the hull coordinate system b Pose q of } t And hull coordinate system { O b Relative to inertial coordinate system { O } w Pose q of } b A conversion matrix between the respective Euler angular velocity and the own angular velocity, a conversion matrix +.>
Figure BDA00039677612900000618
and />
Figure BDA00039677612900000619
The expressions of (2) are respectively: />
Figure BDA0003967761290000071
Figure BDA0003967761290000072
Deriving the angular velocity and the velocity of the upper platform centroid from time to obtain the angular acceleration of the upper platform centroid
Figure BDA0003967761290000073
And acceleration->
Figure BDA0003967761290000074
The expression of (2) is:
Figure BDA0003967761290000075
Figure BDA0003967761290000076
the inertial force borne by the mass center of the upper platform is calculated according to the acceleration of the mass center of the upper platform, and the inertial force is converted into a platform coordinate system { O } by combining with the virtual work principle t { O } relative to the hull coordinate system b Pose q of } t The expression of the inertial force after conversion is as follows:
Figure BDA0003967761290000077
wherein ,FI Is the inertia force, m, of the Stewart upper platform p The quality of the platform on Stewart is the quality of the platform on Stewart;
the mass center of the upper platform receives forces including inertial force, equivalent acting force of six driving rods and gravity, and all the forces are in a platform coordinate system { O t { O } relative to the hull coordinate system b Pose q of } t The following balance, from which the expression of the force balance equation is available:
Figure BDA0003967761290000078
wherein ,FI Is the inertia force, m, of the Stewart upper platform p G is gravity acceleration, f is the mass of a Stewart upper platform a ＝(f a1 f a2 f a3 f a4 f a5 f a6 ) T The system input represents the driving force required by the six driving levers.
In step 2, the process of calculating the action of the step bridge mechanism on the Stewart upper platform specifically includes:
as shown in FIG. 3, coordinate systems { O } are established in the joint space of the step-bridge mechanism according to the Denavit-Hartenberg method 1 }、{O 2} and {O3 }，{O 1 }、{O 2} and {O3 -the coordinate systems of the rotation, pitch and telescopic joints of the bridge are represented respectively;
calculation of acceleration of each connecting rod centroid in three joint spaces by forward iteration using iterative Newton-Euler method
Figure BDA0003967761290000079
And angular acceleration->
Figure BDA00039677612900000710
Expression in joint space, regarding Stewart upper platform as joint-0 in the first step of the forward iteration, can be set:
Figure BDA0003967761290000081
wherein ,
Figure BDA0003967761290000082
to be the inertial system acceleration of the Stewart upper platform, +.>
Figure BDA0003967761290000083
Inertial system angular acceleration of Stewart upper platform, +.>
Figure BDA0003967761290000084
The inertial system angular velocity of the upper platform of Stewart;
the acting force f of the connecting rod on the upper platform is calculated through reverse iteration 1 1 And action moment
Figure BDA0003967761290000085
Considering that the wave compensation mechanism is subjected to a force under certain specific working tasks, such as maintenance of an offshore wind turbine, the end of the step bridge is subjected to a small force within a safety range in order to prevent the mechanical mechanism from being damaged, and thus, the force is set in the first step of reverse iteration: />
Figure BDA0003967761290000086
wherein ,
Figure BDA0003967761290000087
for forces to which the end of the bridge is subjected, +.>
Figure BDA0003967761290000088
Moment applied to the tail end of the walking bridge;
the connecting rod is acted by the acting force f of the upper platform 1 1 And action moment
Figure BDA0003967761290000089
Conversion to inertial coordinate System { O by rotation matrix w In the { O } inertial coordinate system w Force f of Stewart upper platform on connecting rod 1 w And action moment->
Figure BDA00039677612900000810
The expression of (2) is:
Figure BDA00039677612900000811
Figure BDA00039677612900000812
wherein ,mbrige-f ∈R 3×3 and mbrige-n ∈R 3×3 Respectively representing the impact matrix of the action force and the action moment of the walking bridge on the acceleration, M brige-f ∈R 3×3 and Mbrige-n ∈R 3×3 The influence matrix of the acting force and the acting moment of the walking bridge on the angular acceleration is respectively C brige-f ∈R 3×3 and Cbrige-n ∈R 3×3 Matrix of influence, M, representing the angular velocity of the action and moment of action of the bridge, respectively θ-f ∈R 3×3 and Mθ-n ∈R 3×3 Additional term matrices respectively representing the introduction of the action force and the action moment of the step bridge, which matrices can be determined by an iterative Newton-Euler method;
in inertial coordinate system { O w In the }, the action of the step bridge mechanism on the Stewart upper platform includes an action force-f 1 w And action moment
Figure BDA00039677612900000813
In step 3, the process of establishing a dynamic model of the shipboard Stewart platform taking into account the step bridge is specifically:
expanding the force balance equation in the step 1, wherein the force borne by the mass center of the upper platform comprises inertia force, equivalent acting force of six driving rods and gravity, taking the action of a step bridge mechanism into consideration, and according to the pose q of each force on the Stewart upper platform relative to the lower platform t The lower equilibrium can be obtained:
Figure BDA00039677612900000814
taking into consideration the various physical quantities, the expression of the dynamic model of the shipborne Stewart platform of the walking bridge can be obtained as follows:
Figure BDA00039677612900000815
wherein ,J∈R6×6 Motion jacobian matrix representing Stewart platform, f a ＝(f a1 f a2 f a3 f a4 f a5 f a6 ) T F is the input of the system a1 、f a2 、f a3 、f a4 、f a5 and fa6 Representing the driving force required by six driving rods, M t Representing a task space equivalent inertia matrix, C t Representing a first order matrix of task space, G t Represents the gravity term, M b Representing a second order influence matrix of ship motion, C b Representing a first order influence matrix of ship motion, M θ Representing the additional matrix introduced by the step bridge effect.
In step 4, the process of designing the Stewart platform pose control algorithm (Stewart platform pose controller) based on the backstepping method specifically includes:
selecting the pose q of the Stewart upper platform relative to the lower platform t For the controlled quantity, the design of the Stewart platform pose controller is divided into two parts of pose expected value calculation and control algorithm design:
the pose expected value calculation process specifically comprises the following steps:
the dynamic model of the wave compensation system deduced in the step 3 is based on the platform coordinate system { O } t { O } relative to the hull coordinate system b Pose q of } t As controlled quantity, the wave compensation system directly controls the target to be a platform coordinate system { O } t Relative to inertial coordinate system { O } w Pose of } maintains a given constant value
Figure BDA0003967761290000091
Therefore, the ship is required to be observed in real timeVolume coordinate system { O b Relative to inertial coordinate system { O } w Pose q of } b And its first derivative>
Figure BDA0003967761290000092
And second derivative->
Figure BDA0003967761290000093
Computing platform coordinate System { O t { O } relative to the hull coordinate system b Desired pose q td And its first derivative +.>
Figure BDA0003967761290000094
And second derivative->
Figure BDA0003967761290000095
/>
The expected pose can be obtained according to the rotation of the space coordinate system and the vector relation
Figure BDA0003967761290000096
The expressions of the elements in (a) are respectively:
θ td ＝arcsin(-R wb (3,1))
Figure BDA0003967761290000097
Figure BDA0003967761290000098
Figure BDA0003967761290000099
wherein ,Rwb (i, j) represents the hull coordinate system { O } b And inertial coordinate system { O } w Elements of row i and column j of the rotation matrix between;
desired pose q td It can also be expressed as:
q td ＝T(q b )
wherein T (·) represents the coordinate system { O by the hull b Relative to inertial coordinate system { O } w Pose q of } b Solving the platform coordinate system { O t { O } relative to the hull coordinate system b Desired pose q td Is a function of (2).
The ship body coordinate system { O }, is achieved through the dynamic positioning system b Relative inertial coordinate system { O } w Position coordinates [ x ] b y b z b ]Remaining unchanged, the first derivative
Figure BDA00039677612900000910
And second derivative->
Figure BDA00039677612900000911
The calculated expression of (2) is:
Figure BDA0003967761290000101
Figure BDA0003967761290000102
the rotation matrix R can be solved according to the property of the rotation matrix and the relationship between the Euler angle and the rotation speed wb First and second derivatives of (a):
Figure BDA0003967761290000103
Figure BDA0003967761290000104
wherein ,
Figure BDA0003967761290000105
and />
Figure BDA0003967761290000106
Respectively a rotation matrix R wb First and second derivatives of (a);
the control algorithm design process specifically comprises the following steps:
the control algorithm is designed through a back-stepping method:
defining errors, wherein the expressions of the errors are respectively:
z 1 ＝q t -q td
Figure BDA0003967761290000107
wherein ,z1 Z is the pose error 2 Is the first derivative error, k of the pose 1 Is a positive controller parameter;
obtaining a Lyapunov function corresponding to the error, wherein the expression of the Lyapunov function is as follows:
Figure BDA0003967761290000108
wherein V is Lyapunov function;
a control algorithm (pose controller) is obtained, and the expression of the control algorithm is as follows:
Figure BDA0003967761290000109
wherein ,k1 and k2 Are all positive parameters of the controller and are set,
Figure BDA00039677612900001010
the foregoing describes in detail preferred embodiments of the present invention. It should be understood that numerous modifications and variations can be made in accordance with the concepts of the invention by one of ordinary skill in the art without undue burden. Therefore, all technical solutions which can be obtained by logic analysis, reasoning or limited experiments based on the prior art by the person skilled in the art according to the inventive concept shall be within the scope of protection defined by the claims.

Claims (10)

1. A wave compensation system modeling and pose control method based on a Stewart platform is characterized by comprising the following steps:
step 1: carrying out dynamics analysis on the shipborne Stewart platform and obtaining a force balance equation;
step 2: calculating the action of the step bridge mechanism on the Stewart upper platform;
step 3: expanding a force balance equation based on the action of a step bridge mechanism on a Stewart upper platform, and further establishing a ship-borne Stewart platform dynamics model considering a step bridge;
step 4: and designing a Stewart platform pose controller based on a backstepping method to control the pose of the Stewart platform.
2. The method for modeling and controlling the pose of the wave compensation system based on the Stewart platform according to claim 1, wherein in the step 1, the process of dynamically analyzing the shipborne Stewart platform and obtaining the force balance equation specifically comprises the following steps:
step 101: three coordinate systems are set up, namely an inertial coordinate system { O } w Is fixed on the ship body coordinate system { O }, of the ship b Platform coordinate System { O } fixed to Stewart upper platform t And in inertial coordinate system { O } w Calculating the speed and angular velocity of the mass center of the platform on Stewart in an inertial coordinate system { O }, and w the expressions for the speed and angular velocity of the platform centroid at Stewart are:
Figure QLYQS_1
Figure QLYQS_2
wherein ,
Figure QLYQS_5
and />
Figure QLYQS_9
Respectively representing the angular and linear speeds of the upper platform in the inertial coordinate system, < >>
Figure QLYQS_13
and Jp ＝[J pl J pr ]The angular velocity and the linear velocity of the mass center of the upper platform are respectively in an inertial coordinate system { O w Matrix of } and pose->
Figure QLYQS_4
Conversion matrix between>
Figure QLYQS_14
and />
Figure QLYQS_15
Respectively represent the platform coordinate system { O } t And hull coordinate system { O } b Each and generalized angular velocity
Figure QLYQS_16
Jacobian matrix, J pl and Jpr Respectively represent the platform coordinate system { O } t And hull coordinate system { O } b Each and generalized velocity
Figure QLYQS_3
Jacobian matrix between the two, S (·) represents the rotation, R wb Representing the hull coordinate system { O } b And inertial coordinate system { O } w Rotation matrix between }, ∈>
Figure QLYQS_7
Representing the platform coordinate system { O t { O } relative to the hull coordinate system b The pose of the three-dimensional model is,
Figure QLYQS_8
representing the hull coordinate system { O } b Relative to inertial coordinate system { O } w Pose, ψTheta and
Figure QLYQS_12
all are Euler angles, x, y and z are respectively the horizontal, vertical and vertical coordinates, representing the position,/->
Figure QLYQS_6
Platform coordinate system { O t { O } relative to the hull coordinate system b Location, J I ＝[0 3×3 ,I 3×3 ]Is a constant matrix>
Figure QLYQS_10
and />
Figure QLYQS_11
Respectively represent the platform coordinate system { O } t { O } relative to the hull coordinate system b Pose q of } t And hull coordinate system { O b Relative to inertial coordinate system { O } w Pose q of } b A conversion matrix between the respective Euler angular velocity and the own angular velocity;
conversion matrix
Figure QLYQS_17
and />
Figure QLYQS_18
The expressions of (2) are respectively:
Figure QLYQS_19
/>
Figure QLYQS_20
step 102: the speed and the angular speed of the mass center of the platform on Stewart are calculated by deriving, and the expressions of the obtained acceleration and the obtained angular acceleration are respectively as follows:
Figure QLYQS_21
Figure QLYQS_22
wherein ,
Figure QLYQS_23
and />
Figure QLYQS_24
Respectively representing the acceleration and the angular acceleration of the upper platform in an inertial coordinate system;
step 103: and acquiring a force balance equation by utilizing a virtual work principle based on a kinematic calculation result.
3. The method for modeling and controlling the pose of a wave compensation system based on a Stewart platform according to claim 2, wherein in step 103, the process of obtaining the force balance equation by using the virtual work principle according to the kinematic calculation result is specifically as follows:
the inertial force of the upper platform centroid is calculated according to the acceleration of the upper platform centroid, and is converted into a platform coordinate system { O } by combining with the virtual work principle t { O } relative to the hull coordinate system b Pose q of } t The expression of the inertial force after conversion is as follows:
Figure QLYQS_25
wherein ,FI Is the inertia force, m, of the Stewart upper platform p For Stewart upper platform quality, I t The moment of inertia matrix of the Stewart upper platform under the inertia system;
the mass center of the upper platform receives forces including inertial force, equivalent acting force of six driving rods and gravity, and all the forces are in a platform coordinate system { O t { O } relative to the hull coordinate system b Pose q of } t Lower equilibrium, and thus the expression of the force balance equationThe method comprises the following steps:
Figure QLYQS_26
wherein ,FI Is the inertia force, m, of the Stewart upper platform p G is gravity acceleration, f is the mass of a Stewart upper platform a ＝(f a1 f a2 f a3 f a4 f a5 f a6 ) T F is the input of the system a1 、f a2 、f a3 、f a4 、f a5 and fa6 Indicating the driving force required for the six driving levers.
4. A method for modeling and controlling the pose of a wave compensation system based on a Stewart platform according to claim 3, wherein in said step 2, in the inertial coordinate system, the action of the step bridge mechanism on the Stewart upper platform includes an acting force-f 1 w And action moment
Figure QLYQS_27
The process for calculating the action of the step bridge mechanism on the Stewart upper platform specifically comprises the following steps:
step 201: corresponding coordinate systems { O } are respectively established at the rotation, pitching and telescoping joints of the step bridge mechanism according to the Denavit-Hartenberg method 1 }、{O 2} and {O3 }；
Step 202: solving the acceleration and the angular acceleration of each connecting rod centroid through forward iteration by using an iterative Newton-Euler method;
step 203: solving acting force f of connecting rod on Stewart upper platform through reverse iteration 1 1 And action moment
Figure QLYQS_28
Step 204: the connecting rod is acted by the acting force f of the upper platform 1 1 And action moment
Figure QLYQS_29
Conversion to inertial coordinate System { O by rotation matrix w In the { O } inertial coordinate system w Force f of Stewart upper platform on connecting rod 1 w And action moment->
Figure QLYQS_30
Step 205: obtain { O in inertial coordinate system w Force-f of step bridge mechanism on Stewart upper platform 1 w And action moment
Figure QLYQS_31
5. The method of modeling and pose control of a Stewart platform based wave compensation system of claim 4, wherein in said step 204, an inertial coordinate system { O } w Force f of Stewart upper platform on connecting rod 1 w And action moment
Figure QLYQS_32
The expressions of (2) are respectively:
Figure QLYQS_33
Figure QLYQS_34
wherein ,mbrige-f ∈R 3×3 and mbrige-n ∈R 3×3 Respectively representing the impact matrix of the action force and the action moment of the walking bridge on the acceleration, M brige-f ∈R 3×3 and Mbrige-n ∈R 3×3 The influence matrix of the acting force and the acting moment of the walking bridge on the angular acceleration is respectively C brige-f ∈R 3×3 and Cbrige-n ∈R 3×3 Matrix of influence, M, representing the angular velocity of the action and moment of action of the bridge, respectively θ-f ∈R 3×3 and Mθ-n ∈R 3×3 And respectively representing additional term matrixes introduced by the acting force and the acting moment of the walking bridge.
6. The method for modeling and controlling the pose of a wave compensation system based on a Stewart platform according to claim 1, wherein in the step 3, the dynamics model of the shipboard Stewart platform considering the step bridge is as follows:
Figure QLYQS_35
wherein ,
Figure QLYQS_36
representing the platform coordinate system { O t { O } relative to the hull coordinate system b Pose }, ->
Figure QLYQS_37
Representing the hull coordinate system { O } b Relative to inertial coordinate system { O } w Pose, ψ, θ and +.>
Figure QLYQS_38
Are Euler angles, J epsilon R 6×6 Motion jacobian matrix representing Stewart platform, f a ＝(f a1 f a2 f a3 f a4 f a5 f a6 ) T F is the input of the system a1 、f a2 、f a3 、f a4 、f a5 and fa6 Representing the driving force required by six driving rods, M t Representing a task space equivalent inertia matrix, C t Representing a first order matrix of task space, G t Represents the gravity term, M b Representing a second order influence matrix of ship motion, C b Representing a first order influence matrix of ship motion, M θ Representing the additional matrix introduced by the step bridge effect.
7. The method for modeling and controlling the pose of the wave compensation system based on the Stewart platform according to claim 6, wherein in the step 4, the process for designing the pose control algorithm of the Stewart platform based on the back-stepping method is specifically as follows:
step 401: pose q of Stewart upper platform relative to lower platform t For the controlled quantity, a platform coordinate system { O } t { O } relative to the hull coordinate system b The expected pose of } is q td And calculate the first derivative thereof
Figure QLYQS_39
And second derivative->
Figure QLYQS_40
Step 402: and (5) designing a control algorithm through a back-stepping method to obtain the pose controller.
8. The method for modeling and controlling the pose of a wave compensation system based on a Stewart platform as claimed in claim 7, wherein in said step 401, the first derivative is
Figure QLYQS_41
And second derivative->
Figure QLYQS_42
The calculated expressions of (a) are respectively:
Figure QLYQS_43
Figure QLYQS_44
wherein ,
Figure QLYQS_45
and />
Figure QLYQS_46
For rotation ofMatrix R wb First and second derivatives of (a).
9. The method for modeling and controlling the pose of a wave compensation system based on a Stewart platform according to claim 8, wherein said rotation matrix R wb The expressions of the first derivative and the second derivative of (c) are:
Figure QLYQS_47
Figure QLYQS_48
/>
10. the method for modeling and controlling the pose of a wave compensation system based on a Stewart platform according to claim 7, wherein in said step 402, the expression of the pose controller is:
Figure QLYQS_49
wherein ,k1 and k2 Are all positive parameters of the controller and are set,
Figure QLYQS_50
/>
CN202211504719.XA 2022-11-28 2022-11-28 Wave compensation system modeling and pose control method based on Stewart platform Pending CN116126003A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211504719.XA CN116126003A (en) 2022-11-28 2022-11-28 Wave compensation system modeling and pose control method based on Stewart platform

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211504719.XA CN116126003A (en) 2022-11-28 2022-11-28 Wave compensation system modeling and pose control method based on Stewart platform

Publications (1)

Publication Number Publication Date
CN116126003A true CN116126003A (en) 2023-05-16

Family

ID=86308931

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211504719.XA Pending CN116126003A (en) 2022-11-28 2022-11-28 Wave compensation system modeling and pose control method based on Stewart platform

Country Status (1)

Country Link
CN (1) CN116126003A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117533473A (en) * 2024-01-09 2024-02-09 上海新纪元机器人有限公司 Ship with parallel robot device and self-balancing method

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117533473A (en) * 2024-01-09 2024-02-09 上海新纪元机器人有限公司 Ship with parallel robot device and self-balancing method
CN117533473B (en) * 2024-01-09 2024-04-12 上海新纪元机器人有限公司 Ship with parallel robot device and self-balancing method

Similar Documents

Publication Publication Date Title
Encarnacao et al. 3D path following for autonomous underwater vehicle
Ihle et al. Formation control of marine surface craft: A Lagrangian approach
CN108628171B (en) Improved thrust distribution method for ship dynamic positioning system
JP2018176922A (en) Motion control device and motion control method for vessel
CN110561425B (en) Rope-driven flexible robot force and position hybrid control method and system
CN112091976B (en) Task space control method for underwater mechanical arm
CN107656530B (en) Variable-parameter open-frame type ocean underwater robot trajectory tracking control method, device and system
CN116126003A (en) Wave compensation system modeling and pose control method based on Stewart platform
Lv et al. Sliding mode control of cable-driven redundancy parallel robot with 6 DOF based on cable-length sensor feedback
Hai et al. Investigation on the mechanical design and manipulation hydrodynamics for a small sized, single body and streamlined I-AUV
CN111055282A (en) Self-stabilization auxiliary structure for repairing and manufacturing parts in marine bumpy environment
CN111857165B (en) Trajectory tracking control method of underwater vehicle
CN111007877A (en) Global robust self-adaptive trajectory tracking control method of four-rotor aircraft
CN115826597A (en) Adaptive neural network-based anti-interference control method and device for rotorcraft
CN113741433B (en) Distributed formation method of unmanned ship on water surface
CN113336110B (en) Time optimal trajectory control method and system for offshore crane
CN117250971B (en) Control method of microminiature AUV
CN113031655B (en) Flying and lifting system accompanied with load lifting and control method thereof
From et al. On the influence of ship motion prediction accuracy on motion planning and control of robotic manipulators on seaborne platforms
CN117163219A (en) Shipborne trestle feedforward fuzzy control method considering constraint between long rods
CN108227723A (en) A kind of underwater robot and its application process of stability analysis and structure optimization
CN113608440B (en) Marine suspended boat system pendulum reduction control method considering rope length change
CN110647161A (en) Under-actuated UUV horizontal plane trajectory tracking control method based on state prediction compensation
Silva et al. Models for simulation and control of underwater vehicles
CN115576334B (en) Under-actuated underwater vehicle formation control method and system

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination