A forward dynamics solution and posture control method for a six-degree-of-freedom motion platform

Abstract

The present invention provides a method for the dynamics forward solution and posture control of a six-degree-of-freedom motion platform, and relates to the technical field of dynamics calculation and control of robot systems. The present application solves the dynamic state of the motion platform based on the dynamic state of the six electric cylinders of the six-degree-of-freedom motion platform. And based on the solved dynamic state of the motion platform, the real-time posture of the six-degree-of-freedom motion platform is obtained; the electric cylinder is adjusted to execute in a predetermined manner according to the real-time posture, and the posture of the six-degree-of-freedom motion platform at a predetermined time in the future is predicted. The method disclosed in the present application plays an important role in predicting the future state of the platform, quickly responding to external changes, maintaining a stable motion trajectory, optimizing the dynamic performance of the robot, improving the response speed and accuracy, verifying the effectiveness of the control algorithm, and optimizing the robot design.

Description

Technical Field

[0001] The present invention relates to the technical field of dynamics calculation and control of a robot system, and in particular to a dynamics forward solution and position and posture control method for a six-degree-of-freedom motion platform. Background Art

[0002] The forward dynamic solution method of the six-degree-of-freedom motion platform is a method for solving the dynamic state of the motion platform based on the dynamic state of the six electric cylinders of the six-degree-of-freedom motion platform.

[0003] The kinematics of traditional six-degree-of-freedom motion platforms typically require a nonlinear system of equations, solved using Newton's iteration method. This method is computationally intensive, and if the platform's position and posture need to be monitored in real time using the elongation of an electric cylinder, it places high demands on computer hardware, potentially causing system lag or even system freezes. Summary of the Invention

[0004] Purpose of the invention: To propose a dynamics forward solution and posture control method for a six-degree-of-freedom motion platform to solve the above-mentioned problems existing in the prior art.

[0005] In a first aspect of the present invention, a method for solving the dynamics of a six-degree-of-freedom motion platform is provided, comprising the following steps:

[0006] S1. Obtain the electric cylinder length L_i of the target six-degree-of-freedom motion platform at a predetermined moment, and calculate the translational motion results △X, △Y, △Z and the attitude angular motion results △α, △β, △γ of the target six-degree-of-freedom motion platform at the current moment;

[0007] S2. Calculate the actual length of the electric cylinder after a time interval Δt based on the extension and retraction speed Speed_i of the electric cylinder (L_i′), where Li_i′ = Li + Speed_i × Δt. Also calculate the translational motion results ΔX′, ΔY′, ΔZ′ and the attitude angular motion results Δα′, Δβ′, Δγ′ of the target six-degree-of-freedom motion platform after the time interval Δt.

[0008] S3. Based on the calculation results of step S1 and step S2, solve the translational velocity V_x, V_y, V_z and angular velocity V_α, V_β, V_γ of the six-degree-of-freedom motion platform;

[0009] S4. Based on the calculation results of step S1, solve the direction vectors and vector components of the six electric cylinders;

[0010] S5. Calculate the coordinates of the center of the upper hinge circle of the target six-degree-of-freedom motion platform;

[0011] S6. Calculate the moment arm of the electric cylinder with respect to the center of the upper hinge circle on three projection planes;

[0012] S7. Based on the calculation result of step S1, solve the load center of gravity coordinates X_loadcm, Y_loadcm, and Z_loadcm of the six-degree-of-freedom motion platform;

[0013] S8. Calculate the angle Angle_xz between the projection of the line connecting the center of gravity of the load and the center of the upper hinge circle on the XZ plane and the Z axis, the angle Angle_yz between the projection on the YZ plane and the Z axis, and the angle Angle_xy between the projection on the XY plane and the X axis;

[0014] S9. Calculate the thrust components of the six electric cylinders in the X, Y, and Z directions based on the known electric cylinder thrust Force_i.

[0015] S10, calculating the three resultant forces and three resultant moments generated by the electric cylinder;

[0016] S11. Based on the calculation results of step S9 and step S10, establish a force and torque balance equation group; solve the equation group to obtain the acceleration parameters of the six-degree-of-freedom motion platform: Acceleration_x, Acceleration_y, Acceleration_z, Acceleration_α, Acceleration_β, Acceleration_γ.

[0017] In a further embodiment of the first aspect, in step S1, when the electric cylinder length L_i is L_i, a nonlinear equation group about unknown quantities ΔX, ΔY, ΔZ, Δα, Δβ, and Δγ is established according to the inverse solution of motion:

[0018]

[0019] Where Xb_i represents the X coordinate of the i-th lower hinge point of the platform; Yb_i represents the Y coordinate of the i-th lower hinge point of the platform; Zb_i represents the Z coordinate of the i-th lower hinge point of the platform; Xnew_i represents the X coordinate of the i-th upper hinge point of the platform after movement; Ynew_i represents the Y coordinate of the i-th upper hinge point of the platform after movement; Znew_i represents the Z coordinate of the i-th upper hinge point of the platform after movement;

[0020] The length of the electric cylinder L_i is known, and the nonlinear equations are solved by Newton iteration to obtain approximate solutions of △X, △Y, △Z, △α, △β, and △γ.

[0021] In a further embodiment of the first aspect, in step S1, the translational velocities V_x, V_y, V_z and the angular velocities V_α, V_β, V_γ of the six-degree-of-freedom motion platform are solved according to the following formulas:

[0022] V_x=(ΔX-ΔX′) / Δt

[0023] V_y=(ΔY-ΔY′) / Δt

[0024] V_z=(ΔZ-ΔZ′) / Δt

[0025] V_α=(Δα-Δα′) / Δt

[0026] V_β=(Δβ-Δβ′) / Δt

[0027] V_γ=(Δγ-Δγ′) / Δt

[0028] Where V_x represents the lateral velocity; V_y represents the longitudinal velocity; V_z represents the vertical velocity; V_α represents the pitch angular velocity; V_β represents the roll angular velocity; and V_γ represents the yaw angular velocity.

[0029] In a further embodiment of the first aspect, the direction vectors a_i of the six electric cylinders are solved according to the calculation result of step S1:

[0030] a_i=[Xnew_i-Xb_i, Ynew_i-Yb_i, Znew_i-Zb_i]

[0031] Where Xnew_i-Xb_i, Ynew_i-Yb_i, and Znew_i-Zb_i represent the vector components of the electric cylinder in the X, Y, and Z directions, respectively.

[0032] In a further embodiment of the first aspect, the coordinates (X_platcm, Y_platcm, Z_platcm) of the center of the upper articulated circle of the target six-degree-of-freedom motion platform are calculated;

[0033] in:

[0034]

[0035] Calculate the moment arm of the electric cylinder with respect to the center of the upper hinge circle on the three projection surfaces:

[0036]

[0037] Where Arm_xy_i represents the i-th electric cylinder arm on the XY plane; Arm_xz_i represents the i-th electric cylinder arm on the XZ plane; and Arm_yz_i represents the i-th electric cylinder arm on the YZ plane.

[0038] In a further embodiment of the first aspect, the angle between the projection of the line connecting the center of gravity of the load and the center of the upper hinge circle on the XZ plane and the Z axis is calculated according to the following formula:

[0039]

[0040] The angle between the projection of the line connecting the load center of gravity and the center of the upper hinge circle on the YZ plane and the Z axis is calculated according to the following formula:

[0041]

[0042] The angle between the projection of the line connecting the center of gravity of the load and the center of the upper hinge circle on the XY plane and the X-axis is calculated according to the following formula:

[0043]

[0044] Where X_loadcm represents the X coordinate of the load's center of gravity; Y_loadcm represents the Y coordinate of the load's center of gravity; Z_loadcm represents the Z coordinate of the load's center of gravity; X_platcm, Y_platcm, and Z_platcm represent the X, Y, and Z coordinates of the center of the hinge circle, respectively.

[0045] In a further embodiment of the first aspect, the thrust component of the electric cylinder along the X direction is calculated according to the following formula:

[0046]

[0047] The thrust component of the electric cylinder along the Y direction is calculated according to the following formula:

[0048]

[0049] The thrust component of the electric cylinder along the Z direction is calculated according to the following formula:

[0050]

[0051] Where Force_i represents the rated thrust of the electric cylinder; a_i[1], a_i[2], and a_i[3] represent the vector components of the electric cylinder in the X, Y, and Z directions, namely Xnew_i-Xb_i, Ynew_i-Yb_i, and Znew_i-Zb_i, respectively.

[0052] The resultant force generated by the electric cylinder on the XY projection plane:

[0053]

[0054] The resultant force generated by the electric cylinder on the XZ projection plane:

[0055]

[0056] The resultant force generated by the electric cylinder on the YZ projection plane:

[0057]

[0058] Therefore, the net yaw moment Rolling moment Pitching moment

[0059] In a further embodiment of the first aspect, a force and torque balance equation system is established based on the thrust components of the electric cylinder along the X, Y, and Z directions, and the resultant force and torque of the electric cylinder on the XY, XZ, and YZ projection surfaces:

[0060]

[0061] Inertial_Mγ=Mγ

[0062] Inertial_Mβ=Mβ

[0063] Inertial_Mα=Mα

[0064] The force and torque balance equations are written in the following matrix form:

[0065]

[0066] Solve the equations using Gaussian elimination or Jacobi iteration to obtain the lateral acceleration Acceleration_x, longitudinal acceleration Acceleration_y, lift acceleration Acceleration_z, pitch angular acceleration Acceleration_α, roll angular acceleration Acceleration_β, and yaw angular acceleration Acceleration_γ of the six-degree-of-freedom motion platform.

[0067] A second aspect of the present invention provides a posture control method for a six-degree-of-freedom motion platform, comprising the following steps:

[0068] Based on the above-disclosed forward solution method for the dynamics of the six-degree-of-freedom motion platform, the translational velocities V_x, V_y, V_z, angular velocities V_α, V_β, V_γ, lateral acceleration Acceleration_x, longitudinal acceleration Acceleration_y, lift acceleration Acceleration_z, pitch angular acceleration Acceleration_α, roll angular acceleration Acceleration_β, and yaw angular acceleration Acceleration_γ of the six-degree-of-freedom motion platform are calculated, thereby obtaining the real-time position and posture of the six-degree-of-freedom motion platform;

[0069] The electric cylinder is adjusted to execute in a predetermined manner according to the real-time posture, and the posture of the six-degree-of-freedom motion platform at a predetermined time in the future is predicted.

[0070] According to a third aspect of the present invention, an electronic device is provided, comprising a processor and a memory storing computer program instructions; when the processor executes the computer program instructions, the posture control method of the six-degree-of-freedom motion platform as described in the second aspect is implemented.

[0071] In a fourth aspect of the present invention, a computer-readable storage medium is proposed, which stores at least one executable instruction. When the executable instruction is run on an electronic device, the electronic device executes the posture control method of the six-degree-of-freedom motion platform as described in the second aspect.

[0072] Beneficial effects: This application discloses a dynamic forward solution and posture control method for a six-degree-of-freedom motion platform, which plays an important role in predicting the future state of the platform, quickly responding to external changes, maintaining a stable motion trajectory, optimizing the dynamic performance of the robot, improving response speed and accuracy, verifying the effectiveness of the control algorithm, and optimizing the robot design. BRIEF DESCRIPTION OF THE DRAWINGS

[0073] Figure 1 It is a flow chart of the forward dynamic solution method of the six-degree-of-freedom motion platform in the embodiment. DETAILED DESCRIPTION

[0074] In the following description, numerous specific details are provided to provide a more thorough understanding of the present invention. However, it will be apparent to those skilled in the art that the present invention may be practiced without one or more of these details. In other instances, certain technical features well known in the art have not been described to avoid confusion with the present invention.

[0075] Example 1:

[0076] This embodiment discloses a method for solving the dynamics of a six-degree-of-freedom motion platform, specifically calculating the velocity, acceleration, angular velocity, and angular acceleration of the motion platform based on the three parameters of the extension and retraction amount, extension and retraction speed, and thrust of the six electric cylinders of the six-degree-of-freedom motion platform.

[0077] Before describing the specific calculation process of this embodiment, the following parameters are first defined:

[0078] Mass: load mass;

[0079] Length: Equivalent length of the load, along the Y-axis;

[0080] Width: load equivalent width, along the X-axis;

[0081] Heigth: equivalent height of load, along the Z axis;

[0082] X_loadcm: X coordinate of the load center of gravity;

[0083] Y_loadcm: Y coordinate of the load center of gravity;

[0084] Z_loadcm: Z coordinate of the load center of gravity;

[0085] X_rotatecm: X coordinate of the platform's attitude angular motion center (X coordinate of the origin of the motion reference coordinate system);

[0086] Y__rotatecm: The Y coordinate of the platform's attitude angular motion center (the Y coordinate of the origin of the motion reference coordinate system);

[0087] Z__rotatecm: Z coordinate of the platform's attitude angular motion center (Z coordinate of the origin of the motion reference coordinate system);

[0088] X_platcm: X coordinate of the center of the hinge circle on the platform;

[0089] Y__platcm: Y coordinate of the center of the hinged circle on the platform;

[0090] Z__platcm: Z coordinate of the center of the hinge circle on the platform;

[0091] L0: initial length of the electric cylinder;

[0092] H0: initial height of the platform;

[0093] (For the following parameters, the value range of i is 1≤i≤6)

[0094] Xb_i: X coordinate of the i-th lower hinge point of the platform;

[0095] Yb_i: Y coordinate of the i-th lower hinge point of the platform;

[0096] Zb_i: Z coordinate of the i-th lower hinge point of the platform;

[0097] Xp_i: X coordinate of the i-th upper hinge point of the platform at zero position;

[0098] Yp_i: Y coordinate of the i-th upper hinge point of the platform at zero position;

[0099] Zp_i: Z coordinate of the i-th upper hinge point of the platform at zero position;

[0100] Xnew_i: X coordinate of the i-th upper hinge point of the platform after movement;

[0101] Ynew_i: Y coordinate of the i-th upper hinge point of the platform;

[0102] Znew_i: Z coordinate of the i-th upper hinge point of the platform;

[0103] Force_i: the thrust of the i-th electric cylinder;

[0104] L_i: length of the i-th electric cylinder;

[0105] Speed_i: extension and retraction speed of the i-th electric cylinder;

[0106] △X, △Y, △Z, △α, △β, △γ: actual translational motion and attitude angular motion of the motion platform;

[0107] △X is the linear motion along the X direction, referred to as lateral movement;

[0108] △Y is the linear motion along the Y direction, referred to as longitudinal movement;

[0109] △Z is the linear motion along the Z direction, referred to as lifting;

[0110] △α is the rotation around the X-axis of the motion reference coordinate system, referred to as pitch,

[0111] △β is the rotation around the Y axis of the motion reference coordinate system, referred to as roll;

[0112] △γ is the rotation around the X-axis of the motion reference coordinate system, referred to as yaw.

[0113] R: coordinate transformation matrix;

[0114] a_i: electric cylinder direction vector;

[0115] a_i[j]: the j-th vector component in vector a_i;

[0116] Arm_xy_i: The i-th electric cylinder arm on the XY plane. Specifically, it is the vertical distance from the reference point (center of the upper hinge circle) to the line connecting the i-th electric cylinder, projected onto the XY plane along the Z axis.

[0117] Arm_xz_i: The i-th electric cylinder arm on the XZ plane. Specifically, it is the vertical distance from the reference point (center of the upper hinge circle) to the line connecting the i-th electric cylinder, projected onto the XZ plane along the Y axis.

[0118] Arm_yz_i: The i-th electric cylinder arm on the YZ plane. Specifically, it is the vertical distance from the reference point (center of the upper hinge circle) to the line connecting the i-th electric cylinder, projected onto the YZ plane along the X axis.

[0119] Angle_xz: The angle between the projection of the line connecting the center of gravity of the load and the center of the upper hinge circle on the XZ plane and the Z axis;

[0120] Angle_yz: The angle between the projection of the line connecting the center of gravity of the load and the center of the upper hinge circle on the YZ plane and the Z axis;

[0121] Angle_xy: Calculates the angle between the projection of the line connecting the center of gravity of the load and the center of the upper hinge circle on the XY plane and the X axis;

[0122] Acceleration_x: lateral acceleration;

[0123] Acceleration_y: longitudinal acceleration;

[0124] Acceleration_z: lifting acceleration;

[0125] Acceleration_α: pitch angle acceleration;

[0126] Acceleration_β: roll angular acceleration;

[0127] Acceleration_γ: yaw acceleration;

[0128] V_x: traverse speed;

[0129] V_y: longitudinal speed;

[0130] V_z: lifting speed;

[0131] V_α: pitch angular velocity;

[0132] V_β: rolling angular velocity;

[0133] V_γ: yaw angular velocity.

[0134] The specific solution process of the dynamics forward solution method of a six-degree-of-freedom motion platform disclosed in this embodiment is as follows:

[0135] At a specific moment, the length L_i, extension speed Speed_i, and thrust Force_i of the six-degree-of-freedom electric cylinder platform are known.

[0136] Step 1: Based on the length of the electric cylinder L_i, solve △X, △Y, △Z, △α, △β, and △γ according to the kinematic forward solution method of the six-degree-of-freedom motion platform.

[0137] According to the inverse solution of motion, a nonlinear equation group about unknown quantities △X, △Y, △Z, △α, △β, △γ is established:

[0138]

[0139] Because Li is known, the approximate solutions of △X, △Y, △Z, △α, △β, and △γ can be obtained by solving the nonlinear equations through Newton iteration.

[0140] Step 2: Calculate the actual length L_i' of the electric cylinder after a very small time interval based on the extension and retraction speed Speed_i of the electric cylinder

[0141] L_i′=L_i+Speed_i×Δt

[0142] Based on the length Li' of the electric cylinder, △X', △Y', △Z', △α', △β', and △γ' are solved according to the kinematic forward solution method of the six-degree-of-freedom motion platform.

[0143] Step 3: Based on △X, △Y, △Z, △α, △β, △γ and △X', △Y', △Z', △α', △β', △γ' in steps 1 and 2, calculate the velocities V_x, V_y, V_z and the angular velocities V_α, V_β, V_γ of the six-degree-of-freedom motion platform.

[0144] V_x=(ΔX-ΔX′) / Δt

[0145] V_y=(ΔY-ΔY′) / Δt

[0146] V_z=(ΔZ-ΔZ′) / Δt

[0147] V_α=(Δα-Δα′) / Δt

[0148] V_β=(Δβ-Δβ′) / Δt

[0149] V_γ=(Δγ-×γ′) / Δt

[0150] Step 4: Based on the results of step 1, calculate Xnew_i, Ynew_i, and Znew_i according to the kinematic inverse solution of the six-degree-of-freedom motion platform; and then calculate the six electric cylinder direction vectors:

[0151] a_i=[Xnew_i-Xb_i, Ynew_i-Yb_i, Znew_i-Zb_i]

[0152] Each vector component:

[0153] a_i[1]=Xnew_i-Xb_i

[0154] a_i[2]=Ynew_i-Yb_i

[0155] a_i[3]=Znew_i-Zb_i

[0156] Step 5: Calculate the coordinates of the center of the upper hinge circle:

[0157]

[0158] Step 6: Calculate the lever arm of the electric cylinder with respect to the reference point (center of the upper hinge circle) on the three projection surfaces:

[0159]

[0160] Step 7: Based on the results of step 1, calculate the load center of gravity coordinates X_loadcm, Y_loadcm, and Z_loadcm according to the kinematic inverse solution of the six-degree-of-freedom motion platform.

[0161] The coordinates of the platform's attitude angular motion center are only related to translational motion. When calculating X_rotatecm, Y__rotatecm, and Z__rotatecm based on the kinematic inverse solution of the six-degree-of-freedom motion platform, the R matrix is:

[0162]

[0163] Calculate the distance between the load center of gravity and the center of attitude angular motion along the three coordinate axes:

[0164]

[0165]

[0166] Step 8: Calculate the angle between the projection of the line connecting the center of gravity of the load and the center of the upper hinge circle on the XZ plane and the Z axis:

[0167]

[0168] Calculate the angle between the projection of the line connecting the center of gravity of the load and the center of the upper hinge circle on the YZ plane and the Z axis:

[0169]

[0170] Calculate the angle between the projection of the line connecting the center of gravity of the load and the center of the upper hinge circle on the XY plane and the X-axis:

[0171]

[0172] Step 9: Calculate the moment of inertia.

[0173] Because the six-degree-of-freedom platform motion has multiple postures, assuming that the load has equivalent length, width, and height respectively, the load in a complex posture (two or more of the α, β, and γ angles exist at the same time), its moment of inertia calculation cannot be calculated according to the existing formula. Here we propose a method for calculating the moment of inertia of a load with equivalent length, width, and height:

[0174] (1) Calculation of correction coefficient

[0175] When the load performs pitch motion and roll motion at the same time, calculate the correction coefficient

[0176]

[0177] When the load performs pitch motion and yaw motion at the same time, calculate the correction coefficient

[0178]

[0179] When the load is rolling and pitching at the same time, calculate the correction factor

[0180]

[0181] When the load is rolling and yawing at the same time, calculate the correction factor

[0182]

[0183] When the load performs yaw motion and pitch motion at the same time, calculate the correction coefficient

[0184] When the load performs yaw motion and roll motion at the same time, calculate the correction coefficient

[0185] The calculation method of pitching motion moment of inertia is:

[0186]

[0187] The calculation method of the moment of inertia of rolling motion is:

[0188]

[0189] The calculation method of the yaw motion moment of inertia is:

[0190]

[0191] Step 10: Calculate the thrust components of the six electric cylinders in the X, Y, and Z directions. Thrust component in the X direction:

[0192]

[0193] Thrust component in the Y direction:

[0194]

[0195] Thrust component in the Z direction:

[0196]

[0197] Step 11: Calculate the three resultant forces and three resultant moments generated by the electric cylinders. (1) Calculate the resultant forces of the six electric cylinders on the XY, XZ, and YZ projection planes respectively.

[0198] The resultant force on the XY projection plane:

[0199]

[0200] The resultant force on the XZ projection plane:

[0201]

[0202] The resultant force on the YZ projection plane:

[0203]

[0204] Resultant yaw moment:

[0205]

[0206] Rolling moment:

[0207]

[0208] Resultant pitching moment:

[0209]

[0210] Step 12: Calculate the inertial force of the load in motion.

[0211] Resultant inertial force in the X direction:

[0212] Inertial_x=mass×Acceleration_x+mass×Acceleration_β×Distance_xz×cos(Angle_xz)+mass×Acceleration_γ×Distance_xy×sin(Angle_xy)+mass×(V_β) 2 ×Distance_xz×sin(Angle_xz)+mass×(V_γ) 2 ×Distance_xy×cos(Angle_xy)

[0213] Where: the first term is the inertial force of the lateral linear velocity, the second term is the component of the inertial force of the roll angular acceleration along the X direction, the third term is the component of the inertial force of the yaw angular acceleration along the X direction, the fourth term is the component of the inertial force of the roll angular velocity along the X direction, and the fifth term is the component of the inertial force of the yaw angular velocity along the X direction. The fourth and fifth terms are known, so let:

[0214] Known_1=mass×(V_β) 2 ×Distance_xz×sin(Angle_xz)+mass×(V_γ) 2 ×Distance_xy×cos(Angle_xy)

[0215] Resultant inertial force in the Y direction:

[0216] Inertial_y=mass×Acceleratio n_y+mass×Acceleratio n_α×Distance_y z×cos(Angle_yz)+mass×Acceleratio n_γ×Distance_x y×cos(Angle_xy)+mass×(V_α) 2 ×Distance_y z×sin(Angle_yz)+mass×(V_γ) 2 ×Distance_x y×sin(Angle_xy)

[0217] Where: the first term is the inertial force of the longitudinal linear velocity, the second term is the component of the inertial force of the pitch angular acceleration along the Y direction, the third term is the component of the inertial force of the yaw angular acceleration along the Y direction, the fourth term is the component of the inertial force of the pitch angular velocity along the Y direction, and the fifth term is the component of the inertial force of the yaw angular velocity along the Y direction. The fourth and fifth terms are known, so:

[0218] Known_2=mass×(V_α) 2 ×Distance_yz×sin(Angle_yz)+mass×(V_γ) 2 ×Distance_xy×sin(Angle_xy)

[0219] Resultant inertial force in Z direction:

[0220] Inertial_z=mass×Acceleration_z+mass×Acceleration_a×Distance_yz×sin(Angle_yz)+mass×Acceleration_β×Distance_xz×sin(Angle_xz)+mass×(V_α) 2 ×Distance_yz×cos(Angle_yz)+mass×(V_β) 2 ×Distance_xz×cos(Angle_xz)+mass×9.8066

[0221] Where: the first term is the inertial force of the lift linear velocity, the second term is the component of the inertial force of the pitch angular acceleration along the Z direction, the third term is the component of the inertial force of the roll angular acceleration along the Z direction, the fourth term is the component of the inertial force of the pitch angular velocity along the Z direction, the fifth term is the component of the inertial force of the roll angular velocity along the Z direction, and the sixth term is gravity. The fourth, fifth, and sixth terms are known, so let:

[0222] Known_3=mass×(V_α) 2 ×Distance_yz×cos(Angle_yz)+mass×(V_β) 2 ×Distance_xz×cos(Angle_xz)+mass×9.8066

[0223] Yaw moment of inertia:

[0224] Inertial_Mγ=J γ ×Acceleration_γ+mass×Acceleration_x×(Y_loadcm-Y_platcm)+mass×Acceleration_β×Distance_xz×cos(Angle_xz)×(Y_loadcm-Y _platcm)+mass×Acceleration_y×(X_loadcm-X_platcm)+mass×Acceleration_α×Distance_yz×cos(Angle_yz)×(X_loadcm-X_platcm)

[0225] Where: the first term is the inertia moment of the yaw angular acceleration, the second term is the moment of the inertia force of the lateral acceleration about the reference point, the third term is the moment of the inertia force of the roll angular acceleration along the X direction about the reference point, the fourth term is the moment of the inertia force of the longitudinal acceleration about the reference point, and the fifth term is the moment of the inertia force of the pitch angular acceleration along the Y direction about the reference point.

[0226] Rolling moment of inertia:

[0227] Inertial_Mβ=J β ×Acceleration_β+mass×Acceleration_x×(Z_loadcm-Z_platcm)+mass×Acceleration_γ×Distance_xy×sin(Angle_xy)×(Z_loadcm-Z_platcm)+mass×Ac celeration_z×(X_loadcm-X_platcm)+mass×Acceleration_α×Distance_yz×sin(Angle_yz)×(X_loadcm-X_platcm)+mass×9.8066×(X_loadcm-X_platcm)

[0228] Where: the first term is the inertial moment of the roll angular acceleration, the second term is the moment of the inertial force of the lateral acceleration about the reference point, the third term is the moment of the inertial force of the yaw angular acceleration along the X direction about the reference point, the fourth term is the moment of the inertial force of the lift acceleration about the reference point, the fifth term is the moment of the inertial force of the pitch angular acceleration along the Z direction about the reference point, and the sixth term is the moment of gravity about the reference point. Since the sixth term is known, let:

[0229] Known_5=mass×9.8066×(X_loadcm-X_platcm)

[0230] Pitching moment of inertia:

[0231] Inertial_Mα=J α ×Acceleration_α+mass×Acceleration_y×(Z_loadcm-Z_platcm)+mass×Acceleration_γ×Distance_xy×cos(Angle_xy)×(Z_loadcm-Z_platcm)+mass×Ac celeration_z×(Y_loadcm-Y_platcm)+mass×Acceleration_β×Distance_xz×sin(Angle_xz)×(Y_loadcm-Y_platcm)+mass×9.8066×(Y_loadcm-Y_platcm)

[0232] Where: the first term is the inertial moment of the pitch angular acceleration, the second term is the moment of the inertial force of the longitudinal acceleration about the reference point, the third term is the moment of the inertial force of the yaw angular acceleration along the Y direction about the reference point, the fourth term is the moment of the inertial force of the lift acceleration about the reference point, the fifth term is the moment of the inertial force of the roll angular acceleration along the Z direction about the reference point, and the sixth term is the moment of gravity about the reference point. Since the sixth term is known, let:

[0233] Known_6=mass×9.8066×(Y_loadcm-Y_platcm)

[0234] Step 13: Based on the calculations in steps 10 and 11, establish the force and moment balance equations.

[0235]

[0236] Inertial_Mγ=Mγ

[0237] Inertial_Mβ=Mβ

[0238] Inertial_M α=M α

[0239] Move the known quantity (constant term) on the left side of the equal sign to the right side of the equal sign, and write the force and torque balance equations in matrix form as follows:

[0240] in:

[0241] A12=mass×Distance_xz×cos(Angle_xz)

[0242] A13=mass×Distance_xy×sin(Angle_xy)

[0243] A21=mass×Distance_yz×cos(Angle_yz)

[0244] A23=mass×Distance_xy×cos(Angle_xy)

[0245] A31=mass×Distance_yz×sin(Angle_yz)

[0246] A32=mass×Distance_xz×sin(Angle_xz)

[0247] A41=mass×Distance_yz×cos(Angle_yz)×(X_loadcm-X_platcm)

[0248] A42=mass×Distance_xz×cos(Angle_xz)×(Y_loadcm-Y_platcm)

[0249] A43=J γ

[0250] A44=mass×(Y_loadcm-Y_platcm)

[0251] A45=mass×(X_loadcm-X_platcm)

[0252] A51=mass×Distance_yz×sin(Angle_yz)×(X_loadcm-X_platcm)

[0253] A52=J β

[0254] A53=mass×Distance_xy×sin(Angle_xy)×(Z_loadcm-Z_platcm)

[0255] A54=mass×(Z_loadcm-Z_platcm)

[0256] A56=mass×(X_loadcm-X_platcm)

[0257] A61=J α

[0258] A62=mass×Distance_xz×sin(Angle_xz)×(Y_loadcm-Y_platcm)

[0259] A63=mass×Distance_xy×cos(Angle_xy)×(Z_loadcm-Z_platcm)

[0260] A65=mass×(Z_loadcm-Z_platcm)

[0261] A66=mass×(Y_loadcm-Y_platcm)

[0262] Solving the above equations by Gaussian elimination or Jacobi iteration method can obtain the (angular) acceleration parameters of the six-degree-of-freedom motion platform: Acceleration_x, Acceleration_y, Acceleration_z, Acceleration_α, Acceleration_β, Acceleration_γ.

[0263] Example 2:

[0264] This embodiment discloses a posture control method for a six-degree-of-freedom motion platform. Based on the dynamic forward solution method of the six-degree-of-freedom motion platform disclosed in Example 1, the translational velocities V_x, V_y, V_z, angular velocities V_α, V_β, V_γ, lateral acceleration Acceleration_x, longitudinal acceleration Acceleration_y, lifting acceleration Acceleration_z, pitch angular acceleration Acceleration_α, roll angular acceleration Acceleration_β, and yaw angular acceleration Acceleration_γ of the six-degree-of-freedom motion platform are calculated, thereby obtaining the real-time posture of the six-degree-of-freedom motion platform; the electric cylinder is adjusted to execute in a predetermined manner according to the real-time posture, and the posture of the six-degree-of-freedom motion platform at a predetermined time in the future is predicted.

[0265] Example 3:

[0266] This embodiment discloses an electronic device comprising a processor, a memory, a communication interface, and a communication bus. The processor, memory, and communication interface communicate with each other via the communication bus. The memory is configured to store at least one executable instruction, which causes the processor to execute all steps of the method for defending against adversarial examples based on synonym embedding disclosed in the above embodiment. A detailed description thereof is omitted here.

[0267] The electronic device may also communicate with one or more external devices (e.g., keyboards, pointing devices, Bluetooth devices, etc.), one or more devices that enable a user to interact with the electronic device, and / or any device that enables the electronic device to communicate with one or more other computing devices (e.g., routers, modems, etc.). Such communication may be performed via an input / output (I / O) interface. Furthermore, the electronic device may also communicate with one or more networks (e.g., local area networks (LANs), wide area networks (WANs), and / or public networks, such as the Internet) via a network adapter. The network adapter communicates with other modules of the electronic device via a bus. It should be understood that, although not shown in the figures, other hardware and / or software modules may be used in conjunction with the electronic device, including but not limited to: microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems, tape drives, and data backup storage systems.

[0268] It should be noted that the above embodiments can be implemented in whole or in part by software, hardware, firmware or any other combination. When implemented using software, the above embodiments can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions or computer programs. When the computer instructions or computer program are loaded or executed on a computer, the process or function described in the embodiment of the present application is generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another computer-readable storage medium. For example, the computer instructions can be transmitted from one website, computer, server or data center to another website, computer, server or data center via a wired (e.g., infrared, wireless, microwave, etc.) method. The computer-readable storage medium can be any available medium that can be accessed by a computer or a data storage device such as a server or data center that contains one or more available media sets. The available medium can be a magnetic medium (e.g., a floppy disk, a hard disk, a tape), an optical medium (e.g., a DVD), or a semiconductor medium. The semiconductor medium can be a solid-state drive.

[0269] It should be understood that in the various embodiments of the present application, the size of the serial numbers of the above-mentioned processes does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present application.

[0270] As described above, although the present invention has been shown and described with reference to specific preferred embodiments, it should not be construed as limiting the present invention itself. Various changes may be made to it in form and detail without departing from the spirit and scope of the present invention as defined in the appended claims.

Claims

1. A method for solving the dynamics of a six-degree-of-freedom motion platform, characterized in that: The steps include: S1. Obtain the electric cylinder length L_i of the target six-degree-of-freedom motion platform at a predetermined moment, and calculate the translational motion results △X, △Y, △Z and the attitude angular motion results △α, △β, △γ of the target six-degree-of-freedom motion platform at the current moment; S2. Calculate the actual length of the electric cylinder after a time interval Δt based on the extension and retraction speed Speed_i of the electric cylinder, Li_i′, where Li_i′ = Li + × Speed_i × Δt. Also calculate the translational motion results ΔX′, ΔY′, ΔZ′ and the attitude angular motion results Δα′, Δβ′, Δγ′ of the target six-degree-of-freedom motion platform after the time interval Δt. S3. Based on the calculation results of step S1 and step S2, solve the translational velocity V_x, V_y, V_z and angular velocity V_α, V_β, V_γ of the six-degree-of-freedom motion platform; S4. Based on the calculation results of step S1, solve the direction vectors and vector components of the six electric cylinders; S5. Calculate the coordinates of the center of the upper hinge circle of the target six-degree-of-freedom motion platform; S6. Calculate the moment arm of the electric cylinder with respect to the center of the upper hinge circle on three projection planes; S7. Based on the calculation result of step S1, solve the load center of gravity coordinates X_loadcm, Y_loadcm, and Z_loadcm of the six-degree-of-freedom motion platform; S8. Calculate the angle Angle_xz between the projection of the line connecting the center of gravity of the load and the center of the upper hinge circle on the XZ plane and the Z axis, the angle Angle_yz between the projection on the YZ plane and the Z axis, and the angle Angle_xy between the projection on the XY plane and the X axis; S9. Calculate the thrust components of the six electric cylinders in the X, Y, and Z directions based on the known electric cylinder thrust Force_i. S10, calculating the three resultant forces and three resultant moments generated by the electric cylinder; S11. Based on the calculation results of step S9 and step S10, establish a force and torque balance equation group; solve the equation group to obtain the acceleration parameters of the six-degree-of-freedom motion platform: Acceleration_x, Acceleration_y, Acceleration_z, Acceleration_α, Acceleration_β, Acceleration_γ.

2. The method for solving the dynamics of a six-degree-of-freedom motion platform according to claim 1, characterized in that: In step S1, when the length of the electric cylinder is L_i, a nonlinear equation group about the unknown quantities △X, △Y, △Z, △α, △β, and △γ is established according to the inverse solution of motion: Where Xb_i represents the X coordinate of the i-th lower hinge point of the platform; Yb_i represents the Y coordinate of the i-th lower hinge point of the platform; Zb_i represents the Z coordinate of the i-th lower hinge point of the platform; Xnew_i represents the X coordinate of the i-th upper hinge point of the platform after movement; Ynew_i represents the Y coordinate of the i-th upper hinge point of the platform after movement; Znew_i represents the Z coordinate of the i-th upper hinge point of the platform after movement; The length of the electric cylinder L_i is known, and the nonlinear equations are solved by Newton iteration to obtain approximate solutions of △X, △Y, △Z, △α, △β, and △γ.

3. The method for solving the dynamics of a six-degree-of-freedom motion platform according to claim 1, characterized in that: In step S1, the translational velocity V_x, V_y, V_z and angular velocity V_α, V_β, V_γ of the six-degree-of-freedom motion platform are solved according to the following formula: V_x=(ΔX-ΔX′) / Δt V_y=(ΔY-ΔY′) / Δt V_z=(ΔZ-ΔZ′) / Δt V_α=(Δα-Δα′) / Δt V_β=(Δγ-Δβ′) / Δt V_γ=(Δγ-Δγ′) / Δt Where V_x represents the lateral velocity; V_y represents the longitudinal velocity; V_z represents the vertical velocity; V_α represents the pitch angular velocity; V_β represents the roll angular velocity; and V_γ represents the yaw angular velocity.

4. The method for solving the dynamics of a six-degree-of-freedom motion platform according to claim 2, characterized in that: According to the calculation results of step S1, the direction vectors a_i of the six electric cylinders are solved: a_i=[Xnew_i-Xb_i, Ynew_i-Yb_i, Znew_i-Zb_i] Where Xnew_i-Xb_i, Ynew_i-Yb_i, and Znew_i-Zb_i represent the vector components of the electric cylinder in the X, Y, and Z directions, respectively; Calculate the coordinates of the center of the upper hinge circle of the target six-degree-of-freedom motion platform (X_platcm, Y_platcm, Z_platcm); in: Calculate the moment arm of the electric cylinder with respect to the center of the upper hinge circle on the three projection surfaces: Where Arm_xy_i represents the i-th electric cylinder arm on the XY plane; Arm_xz_i represents the i-th electric cylinder arm on the XZ plane; and Arm_yz_i represents the i-th electric cylinder arm on the YZ plane.

5. The method for solving the dynamics of a six-degree-of-freedom motion platform according to claim 4, characterized in that: The angle between the projection of the line connecting the center of gravity of the load and the center of the upper hinge circle on the XZ plane and the Z axis is calculated according to the following formula: The angle between the projection of the line connecting the load center of gravity and the center of the upper hinge circle on the YZ plane and the Z axis is calculated according to the following formula: The angle between the projection of the line connecting the center of gravity of the load and the center of the upper hinge circle on the XY plane and the X-axis is calculated according to the following formula: Where X_loadcm represents the X coordinate of the load's center of gravity; Y_loadcm represents the Y coordinate of the load's center of gravity; Z_loadcm represents the Z coordinate of the load's center of gravity; X_platcm, Y_platcm, and Z_platcm represent the X, Y, and Z coordinates of the center of the hinge circle, respectively.

6. The method for solving the dynamics of a six-degree-of-freedom motion platform according to claim 4, characterized in that: The thrust component of the electric cylinder along the X direction is calculated according to the following formula: The thrust component of the electric cylinder along the Y direction is calculated according to the following formula: The thrust component of the electric cylinder along the Z direction is calculated according to the following formula: Where Force_i represents the rated thrust of the electric cylinder; a_i[1], a_i[2], and a_i[3] represent the vector components of the electric cylinder in the X, Y, and Z directions, namely Xnew_i-Xb_i, Ynew_i-Yb_i, and Znew_i-Zb_i, respectively. The resultant force generated by the electric cylinder on the XY projection plane: The resultant force generated by the electric cylinder on the XZ projection plane: The resultant force generated by the electric cylinder on the YZ projection plane: Therefore, the net yaw moment Rolling moment Pitching moment 7. The method for solving the dynamics of a six-degree-of-freedom motion platform according to claim 6, characterized in that: According to the thrust components of the electric cylinder along the X, Y, and Z directions, as well as the resultant force and torque of the electric cylinder on the XY, XZ, and YZ projection surfaces, the force and torque balance equations are established: Inertial_Mγ=Mγ Inertial_Mβ=Mβ Inertial_Mα=Mα The force and torque balance equations are written in the following matrix form: Solve the equations using Gaussian elimination or Jacobi iteration to obtain the lateral acceleration Acceleration_x, longitudinal acceleration Acceleration_y, lift acceleration Acceleration_z, pitch angular acceleration Acceleration_α, roll angular acceleration Acceleration_β, and yaw angular acceleration Acceleration_γ of the six-degree-of-freedom motion platform.

8. A posture control method for a six-degree-of-freedom motion platform, characterized in that: The translational velocities V_x, V_y, and V_z, the angular velocities V_α, V_β, and V_γ, the lateral acceleration Acceleration_x, the longitudinal acceleration Acceleration_y, the lifting acceleration Acceleration_z, the pitch angular acceleration Acceleration_α, the roll angular acceleration Acceleration_β, and the yaw angular acceleration Acceleration_γ of the six-degree-of-freedom motion platform are calculated based on the dynamics forward solution method of any one of claims 1 to 7, thereby obtaining the real-time position and posture of the six-degree-of-freedom motion platform; The electric cylinder is adjusted to execute in a predetermined manner according to the real-time posture, and the posture of the six-degree-of-freedom motion platform at a predetermined time in the future is predicted.

9. An electronic device, characterized in that: The device includes: a processor and a memory storing computer program instructions; when the processor executes the computer program instructions, the posture control method of the six-degree-of-freedom motion platform as claimed in claim 8 is implemented.

10. A computer-readable storage medium, characterized in that The storage medium stores at least one executable instruction, and when the executable instruction is executed on the electronic device, the electronic device executes the posture control method of the six-degree-of-freedom motion platform according to claim 8.

Images