Operation stability evaluation method based on dynamic modeling of submersible manipulator arm

Abstract

The present invention relates to the technical field of submersibles, and specifically to an operational stability assessment method based on the dynamic modeling of a submersible manipulator arm. A fluid-mechanical coupling dynamic model of a deep-sea manipulator arm system is established, the influence of seawater on the manipulator arm during deep-sea operations is clarified, and the dynamic behavior of the underwater manipulator arm and its key parameters are revealed. The underwater coupling dynamic model can analyze the requirements of the motion and load parameters of the manipulator arm's end gripper on the driving torque of each joint, thereby providing an important reference for the design and operation of the manipulator arm. Mechanical modeling and submersible stability analysis are carried out for six situations of submersible linear slip, rotational slip, y-axis clockwise overturning, y-axis counterclockwise overturning, x-axis clockwise overturning, and x-axis counterclockwise overturning, revealing the mechanical boundary conditions for the instability of the submersible-manipulator arm system in extreme deep-sea environments. Through the modeling and analysis of the submersible-manipulator arm dynamic operational stability, a reliable guarantee is provided for safe deep-sea operations.

Description

Technical Field

[0001] The present invention relates to the technical field of submersibles, and in particular to an operation stability evaluation method based on dynamic modeling of a submersible manipulator arm. Background Art

[0002] The submersible-robotic arm system can carry out various operations such as exploration, sampling, rescue, and repair in complex deep-sea environments. However, due to the complexity of the terrain, geology, and mission objectives in deep-sea environments, the core technology of the submersible-robotic arm system is to quickly and accurately complete various operations while ensuring the safety and stability of the system. Summary of the Invention

[0003] To address the above issues, this application provides an operational stability assessment method based on the dynamic modeling of a submersible manipulator arm. Using the Newton-Euler equation and the Morison hydrodynamic equation, a mechanical-fluid coupling dynamic model of the manipulator arm system is established, accurately revealing the impact of the manipulator arm joint motion parameters on the manipulator arm's end gripper during deep-sea operations. The technical solution is as follows:

[0004] A method for evaluating the operational stability of a submersible manipulator based on dynamic modeling of the submersible comprises the following steps:

[0005] S1. Establish the submersible's world coordinate system {w}, base coordinate system {0}, and each link coordinate system {n};

[0006] S2. Analysis of the joint driving torque vector τ of the submersible-manipulator a ; Water resistance moment vector τ d and the added mass moment vector τ m ;

[0007] S3. Combine the Newton-Euler equation and the hydrodynamic equation to establish the underwater coupling dynamic model of the deep-sea hydraulic manipulator τ = τ a +τ d +τ m .

[0008] Preferably, in step S1, the deep-sea hydraulic manipulator base coordinate system {0} and each connecting rod coordinate system {n} are established based on the DH method. is the attitude transformation matrix of the manipulator base coordinate system {0} relative to the world coordinate system {w},

[0009]

[0010] Where γ is the rotation angle from the world coordinate system {w} to the base coordinate system {0} around the x-axis, β is the rotation angle from the world coordinate system {w} to the base coordinate system {0} around the y-axis, and α is the rotation angle from the world coordinate system {w} to the base coordinate system {0} around the z-axis. Set: sα =sinα,c α =cosα,s β =sinβ,c β =cosβ,s γ =sinγ,c γ =cosγ.

[0011] Preferably, in step S2, based on the Morison equation, the calculation formula of the infinitesimal water resistance experienced by the infinitesimal element of the nth connecting rod relative to the coordinate system {n} of each connecting rod can be expressed as:

[0012]

[0013] Where:

[0014]

[0015]

[0016] ρ represents the water density, C d represents the water resistance coefficient, represents the y-direction normal linear velocity of the nth link element relative to the coordinate system {n}, represents the normal linear velocity of the nth connecting rod relative to the coordinate system {n} of each connecting rod, D represents the equivalent mass diameter of the connecting rod, dl n represents the nth infinitesimal length of the connecting rod, It represents the component force of the water resistance of the nth link in the y direction relative to the coordinate system {n} of each link, It represents the component of the water resistance of the nth link in the z direction relative to the coordinate system {n} of each link;

[0017] The expression of the water resistance of the nth connecting rod relative to the connecting rod coordinate system {1} is:

[0018]

[0019] Where, Represents the attitude transformation matrix of each link coordinate system {n} relative to the link coordinate system {1};

[0020] The expression of the water resistance torque of the first revolute joint caused by the nth connecting rod infinitesimal element is:

[0021]

[0022] Where, 1 p lnrepresents the position vector of the origin of the nth link element coordinate system {dln} relative to the coordinate system {1}. Preferably, in step S2, based on the Morison equation, the calculation formula of the element added mass force on the nth link element relative to each link coordinate system {n} can be expressed as:

[0023]

[0024] Where:

[0025]

[0026]

[0027] C m represents the added mass force coefficient, represents the y-normal linear acceleration of the nth link element relative to the link coordinate system {n}, represents the z-direction normal linear acceleration of the nth link element relative to the link coordinate system {n}; dl n represents the nth infinitesimal length of the connecting rod; It represents the component of the additional mass force on the nth link relative to the coordinate system {n} of each link in the y direction. It represents the component of the added mass force on the nth link relative to the coordinate system {n} of each link in the z direction;

[0028] The calculation formula of the additional mass force on the nth connecting rod element relative to the connecting rod coordinate system {1} can be expressed as:

[0029]

[0030] Where, Represents the attitude transformation matrix of each link coordinate system {n} relative to the link coordinate system {1};

[0031] The analytical expression of the additional mass moment of the nth connecting rod relative to the connecting rod coordinate system {1} is:

[0032]

[0033] Where, 1 p ln represents the position vector of the origin of the nth link infinitesimal coordinate system {dln} relative to the coordinate system {1}. Preferably, in step S2, in order to balance the gravitational acceleration and buoyancy, the linear acceleration at the origin of the base coordinate system {0} of the submersible is set to:

[0034]

[0035] Where g represents the acceleration due to gravity, ρm represents the water density, ρ w Indicates the density of the robot material.

[0036] Preferably, the equivalent load of the base load corresponding to each of the two deep-sea hydraulic manipulators at the center point of the submersible support surface is analyzed according to the deep-sea hydraulic manipulator coupling dynamics model. Equivalent moment load of manipulator base

[0037] F mx =F x1 +F x2

[0038] F my =F y1 +F y2

[0039] F mz =F z1 +F z2

[0040] τ mx =τ x1 +τ x2 -(F y1 +F y2 )k z +(F z1 -F z2 )k y

[0041] τ my =τ y1 +τ y2 +(F x1 +F x2 )k z -(F z1 +F z2 )k x

[0042] τ mz =τ z1 +τ z2 +(F x1 -F x2 )k y +(F y1 +F y2 )k x

[0043] Where k x k represents the x-distance between the base of the manipulator and the center point of the submersible support bottom surface, y represents the y-direction distance between the base of the manipulator and the center point of the submersible support bottom surface, k zF represents the z-direction distance between the base of the manipulator and the center point of the submersible support bottom surface, mx is the x-direction equivalent load of the base loads corresponding to the two deep-sea hydraulic manipulators at the center point of the submersible support surface, F my is the y-direction equivalent load of the base load corresponding to each of the two deep-sea hydraulic manipulators at the center point of the submersible support surface, F mz is the equivalent load in the z direction of the base load corresponding to each of the two deep-sea hydraulic manipulators at the center point of the submersible support surface, τ mx is the x-direction equivalent moment load of the base load corresponding to each of the two deep-sea hydraulic manipulator arms at the center point of the submersible support surface, τ my is the y-direction equivalent moment load of the base load corresponding to each of the two deep-sea hydraulic manipulator arms at the center point of the submersible support surface, τ mz is the equivalent moment load in the z direction of the base load corresponding to each of the two deep-sea hydraulic manipulators at the center point of the submersible support surface, F x1 is the x-axis load of the Q-side robot arm, F x2 is the x-axis load of the E-side robot arm, F y1 is the y-axis load of the Q-side robot base, F y2 is the y-axis base load of the E-side robot arm, F z1 is the z-axis base load of the Q-side robot arm, F z2 is the z-axis base load of the E-side robot arm, τ x1 is the X-direction moment load of the Q-side manipulator, τ x2 is the moment load of the E-side robot arm in the x direction, τ y1 is the y-direction moment load of the Q-side manipulator, τ y2 is the y-direction moment load of the E-side manipulator, τ z1 is the z-direction base moment load of the Q-side robot arm, τ z2 is the base moment load of the E-side robot arm in z direction.

[0044] The equivalent load at the center point of the supporting bottom surface of the submersible gravity and the manipulator base load is:

[0045]

[0046]

[0047] Where G x G represents the x-direction distance between the submersible's center of gravity and the center of the support bottom surface. y G represents the y-distance between the submersible's center of gravity and the center of the support bottom surface, z F represents the z-direction distance between the submersible's center of gravity and the center of the support bottom surface, and G represents the submersible's gravity. x is the equivalent load in the x direction at the center of the supporting bottom surface of the submersible gravity and the manipulator base load, Fy is the equivalent load in y direction at the center point of the supporting bottom surface of the submersible gravity and the manipulator base load, F z is the equivalent load in z direction at the center point of the support bottom surface of the submersible gravity and the manipulator base load, τ x is the equivalent moment load in the x direction at the center of the supporting bottom surface of the submersible gravity and the manipulator base load, τ y is the equivalent moment load in the y direction at the center of the supporting bottom surface of the submersible gravity and the manipulator base load, τ z is the equivalent moment load in the z direction at the center point of the supporting bottom surface of the submersible gravity and the manipulator base load.

[0048] Preferably, critical linear slip analysis:

[0049] F fx1 、F fx2 、F fx3 、F fx4 is the component of the friction force of the four brackets in the opposite direction of the x-axis; F fy1 、F ff2 、F yy3 、F fy4 is the component of the friction force of the four brackets in the opposite direction of the y-axis, F n1 、F n2 、F n3 、F n4 is the support force of the four brackets in the z-axis direction, L is the distance between the bracket center and the center of the submersible bottom surface in the y-axis direction, H is the distance between the bracket center and the center of the submersible bottom surface in the x-axis direction, W is the distance between the bracket center and the bracket edge of the submersible bottom surface in the x-axis direction, μ is the friction coefficient, F xy F x and F y The resultant force, F f1 is the total friction force of bracket 1, F f2 is the total friction force of bracket 2, F f3 is the total friction force of bracket 3, F f4 is the total friction force of the bracket 4;

[0050] Assume that the submersible is loaded at the center point of the support bottom surface x and F y Under the action, the xy direction is in a critical linear slip state, the submersible support surface linear sliding load, the total friction force of each bracket and F x and F y The direction of the resultant force is opposite, then the critical force system equilibrium equation is:

[0051]

[0052] Critical rotational slip:

[0053] Assume that the submersible is loaded at the center point of the support bottom surface x and F y Under the action, the submersible is in a critical rotational slip state in the z direction, the support surface of the submersible rotates and slides under the load, and the total friction force of each bracket is perpendicular to the line connecting the center of the bottom surface and the support point of the support bracket. The critical force system equilibrium equation is:

[0054]

[0055] Preferably, the overturning analysis includes critical y-axis clockwise overturning, critical y-axis counterclockwise overturning, critical x-axis clockwise overturning, and critical x-axis counterclockwise overturning:

[0056] 1) Critical y-axis clockwise overturning

[0057] Assume that the submersible is loaded at the center point of the support bottom surface y Under the action of , it is in a critical overturning state clockwise around the y axis. The equilibrium equation of the critical force system in this state is:

[0058] τ y =W(F n3 +F n4 )=-WF z

[0059] 2) Critical y-axis counterclockwise tipping

[0060] Assume that the submersible is loaded at the center point of the support bottom surface y Under the action of , it is in a critical overturning state counterclockwise around the y-axis. The critical force system equilibrium equation in this state is:

[0061] τ y =-W(F n1 +F n2 )=WF z

[0062] 3) Critical x-axis clockwise overturning

[0063] Assume that the submersible is loaded at the center point of the support bottom surface x Under the action of , it is in a critical overturning state clockwise around the x-axis. The critical force system equilibrium equation in this state is:

[0064] τ x =L(F n1 +F n3 )=-LF z

[0065] 4) Critical x-axis counterclockwise overturning

[0066] Assume that the submersible is loaded at the center point of the support bottom surface x Under the action of , it is in a critical overturning state counterclockwise around the x-axis, and the critical force system equilibrium equation is:

[0067] τ x =-L(F n2 +F n4 )=LF z .

[0068] Compared with the prior art, this application has the following beneficial effects:

[0069] This patent applies the Newton-Euler equations and the Morison hydrodynamic equations to establish an underwater coupled dynamic model for the robotic arm. This underwater coupled dynamic model provides an important reference for the design and operation of the robotic arm. Based on this underwater coupled dynamic model, an equation for critical hazardous loads for deep-sea submersible operations is further established, providing reliable assurance for the safety of deep-sea submersible operations. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] Figure 1 This is a schematic diagram of the movement of the deep-sea hydraulic manipulator;

[0071] Figure 2 It is a schematic diagram of the connecting rod 2 infinitesimal coordinate system;

[0072] Figure 3 This is a schematic diagram of the submersible load;

[0073] Figure 4 This is a schematic diagram of the linear sliding load of the submersible;

[0074] Figure 5 This is a schematic diagram of the submersible's rotational slip load;

[0075] Figure 6 This is a schematic diagram of the submersible's clockwise overturning load on the y-axis;

[0076] Figure 7 Schematic diagram of the submersible's counterclockwise overturning load on the y-axis;

[0077] Figure 8 This is a schematic diagram of the submersible's clockwise overturning load along the x-axis;

[0078] Figure 9 Schematic diagram of the submersible's counterclockwise overturning load on the x-axis. DETAILED DESCRIPTION

[0079] To facilitate understanding of the present application, the present application will be described more fully below with reference to the accompanying drawings. The accompanying drawings provide embodiments of the present application. However, the present application may be implemented in many different forms and is not limited to the embodiments described herein. Rather, these embodiments are provided to make the disclosure of the present application more thorough and comprehensive.

[0080] The invention comprises a method for modeling the coupling dynamics of a submersible's mechanical arm and a method for modeling the sliding and overturning loads of a submersible.

[0081] Among them, a deep-sea submersible-robotic arm coupling dynamic modeling method includes a robotic arm dynamic model based on the Newton-Euler equation and a robotic arm hydrodynamic equation based on the Morison equation.

[0082] In order to consider the influence of the submersible's bottom-sitting posture on the underwater operation of the deep-sea hydraulic manipulator, Figure 2 As shown in the figure, the world coordinate system {w}, the base coordinate system {0} and the link coordinate system {i} (i = 1, 2, 3, 4, 5, 6) are established. The origin of the world coordinate system {w} is set to coincide with the base coordinate system {0}, and the z of the world coordinate system {w} is set to coincide with the z of the base coordinate system {0}. w The axis coincides with the direction opposite to gravity.

[0083] Based on the DH method, the base coordinate system {0} of the deep-sea hydraulic manipulator and the coordinate systems {i} (i=1, 2, 3, 4, 5, 6) of each connecting rod are established. The origin of the base coordinate system {0} is fixed to the base of the manipulator, the z0 axis of the base coordinate system {0} coincides with the rotation axis of joint one, the x0 axis of the base coordinate system {0} is parallel to the common normal between the rotation axes of joint one and joint two, and the y0 axis of the base coordinate system {0} is determined according to the right-hand rule; the origin of the link coordinate system {1} coincides with the origin of the base coordinate system {0}, the z1 axis of the link coordinate system {1} is parallel to the rotation axis of joint one, and the x1 axis of the link coordinate system {1} is parallel to the common normal between the rotation axes of joint one and joint two; the origin of the link coordinate system {2} is fixed to joint two, the z2 axis of the link coordinate system {2} is parallel to the rotation axis of joint two, and the x2 axis of the link coordinate system {2} is parallel to the common normal between the rotation axes of joint two and joint three; the origin of the link coordinate system {3} is fixed to joint three, and the link coordinate system {4} is fixed to joint three. The z3 axis of the coordinate system {3} is parallel to the rotation axis of joint three, and the x3 axis of the link coordinate system {3} is parallel to the common normal between the rotation axes of joints three and four; the origin of the link coordinate system {4} is fixed to joint four, the z4 axis of the link coordinate system {4} is parallel to the rotation axis of joint four, and the x4 axis of the link coordinate system {4} is parallel to the direction of the vector product between the z5 axis and the z4 axis; the origin of the link coordinate system {5} is fixed to joint five, the z5 axis of the link coordinate system {5} is parallel to the rotation axis of joint five, and the x5 axis of the link coordinate system {5} is parallel to the direction of the vector product between the z6 axis and the z5 axis; the origin of the link coordinate system {6} is fixed to the end gripper, the z6 axis of the link coordinate system {6} is parallel to the rotation axis of joint six, and the x6 axis of the link coordinate system {6} is parallel to the x5 axis of the link coordinate system {5}.

[0084] In addition, the RPY (roll-pitch-yaw) angle method is used to describe the position and attitude relationship between the world coordinate system {w} and the base coordinate system {0}. That is, considering the influence of the uncertain bottom posture of the submersible on the deep-sea operation of the manipulator, the position and attitude transformation matrix of the manipulator base coordinate system {0} relative to the world coordinate system {w} can be obtained as follows:

[0085]

[0086] Among them, r ij represents the matrix element in row i and column j, T(x w ,γ) is the x-axis around the world coordinate system {w} w The position and attitude transformation matrix of the axis rotation, γ is the rotation angle from the world coordinate system {w} to the base coordinate system {0}, T(y w ,β) is the y-axis around the world coordinate system {w} w The position and attitude transformation matrix of the axis rotation, β is the rotation angle from the world coordinate system {w} to the base coordinate system {0}, T(z w ,α) is the z-axis around the world coordinate system {w} w The position and attitude transformation matrix of the axis rotation, α is the rotation angle from the world coordinate system {w} to the base coordinate system {0}, set: s α =sinα,c α =cosα,s β =sinβ,c β =cosβ,s γ =sinγ,c γ =cosγ.

[0087] is the posture transformation matrix of the robot base coordinate system {0} relative to the world coordinate system {w}.

[0088]

[0089] The dynamic recursion algorithm of the Newton-Euler method includes an outward recursion formula and an inward recursion formula.

[0090] 1) Outward recursion formula

[0091] Assuming that the base coordinate system {0} of the deep-sea hydraulic manipulator base is an absolute coordinate system, based on the kinematic transfer equation of the manipulator link, the kinematic recursion formula is recursively deduced from the first link of the manipulator to the nth link from the inside out.

[0092] The motion transfer equation between the angular velocity of the i+1th link of the robot and the angular velocity of the ith link is:

[0093]

[0094] i+1 ω i+1 is the angular velocity of the i+1th link relative to the coordinate system {i+1}, i ω i is the angular velocity of the i-th link relative to the coordinate system {i}, is the angular velocity of the i+1th link rotating around the i+1th joint, i+1 z i+1 is the z-axis unit vector of the coordinate system {i+1}, is the rotation matrix from coordinate system {i} to coordinate system {i+1}.

[0095] The motion transfer equation between the angular acceleration of the i+1th link of the robot arm and the angular acceleration of the ith link is:

[0096]

[0097] is the angular acceleration of the i+1th link relative to the coordinate system {i+1}, is the angular acceleration of the i-th link relative to the coordinate system {i}, is the angular acceleration of the i+1th link around joint i+1.

[0098] The motion transfer equation between the linear acceleration of the origin of coordinate system {i+1} and the linear acceleration of the origin of coordinate system {i} is:

[0099]

[0100] is the linear acceleration of the i+1th link relative to the coordinate system {i+1}, is the linear acceleration of the origin of coordinate system {i} relative to coordinate system {i}, i p i+1 It refers to the position vector of the origin of coordinate system {i+1} in coordinate system {i}.

[0101] The linear acceleration of the center of mass of the i+1th link of the robotic arm is:

[0102]

[0103] i v ci is the velocity of the center of mass of the i-th connecting rod, is the mass center acceleration of the i+1th connecting rod, i p ci is the position vector of the center of mass of the i-th link in the coordinate system {i}, i+1 r ci+1 is the coordinate representation of the center of mass of the i+1th link in its joint coordinate system {i+1}.

[0104] The center of mass inertia force caused by the motion parameters of the i+1th link of the robotic arm is:

[0105]

[0106] m i represents the mass of the i-th connecting rod.

[0107] The equation of the inertia moment of the center of mass of the i+1th link of the manipulator relative to the coordinate system {i+1} is:

[0108]

[0109] ci+1 I i+1 is the inertia tensor of the i+1th connecting rod relative to the center of mass.

[0110] 2) Inward recursion formula

[0111] Based on the static equations of the robot arm links and the dynamic outward recursion formula, the nth link of the robot arm is recursively deduced from the outside to the inside to the 1st link. The force on the i-th link of the robot arm is determined by the force on the i+1th link and the center of mass inertia force of the i-th link. The torque on the i-th link of the robot arm is determined by the force on the i+1th link, the torque on the i+1th link, the center of mass inertia force of the i-th link, and the center of mass inertia moment of the i-th link. The recursive formula of the joint load from the outside to the inside is as follows:

[0112] The force transmission formula between the i-th link of the manipulator in coordinate system {i} and the i+1-th link of the manipulator in coordinate system {i+1} is:

[0113]

[0114] i f i represents the force on the i-th link of the robotic arm (in coordinate system {i}), i f i+1 represents the force on the i+1th connecting rod (in coordinate system {i}), i+1 f i+1 Represents the force acting on the i+1th link of the robot arm (in the coordinate system {i+1}).

[0115] The torque transfer formula between the i-th link of the manipulator in coordinate system {i} and the i+1-th link of the manipulator in coordinate system {i+1} is:

[0116]

[0117] Where, i r ci is the coordinate representation of the center of mass of the i-th link in its joint coordinate system {i}.

[0118] For the rotary joints of the robotic arm, the joint drive device only needs to drive the z-component of the torque balance equation. The remaining forces and torque loads are balanced by the mechanical connection parts. The driving torque of the i-th joint drive device can be obtained as:

[0119]

[0120] Where, τ i is the driving torque of the i-th joint drive device, i z i is the z-axis unit vector of coordinate system {i}.

[0121] Because the deep-sea hydraulic manipulator works in a complex underwater environment, the submersible's bottom posture, gravity, and buoyancy in the water environment need to be considered during the manipulator dynamics modeling process. At this time, the linear acceleration at the origin of the base coordinate system {0} needs to be set as:

[0122]

[0123] Where g represents the acceleration due to gravity, ρ m represents the water density, ρ w Indicates the density of the robot material.

[0124] Based on the inward recursive algorithm and outward recursive algorithm of the manipulator dynamics, the manipulator dynamics equation established according to the Newton-Euler method can be expressed as:

[0125]

[0126] Where D(q) is the n×n order acceleration inertia matrix, is the centripetal force and Coriolis force matrix of order n×1, G(q) is the gravity matrix, τ a is the joint driving torque vector, f is the end load vector, q is the joint angle, is the joint angular acceleration, and F(q) is the end load matrix.

[0127] According to the Morison equation, the calculation formulas for the water resistance term and the additional mass force term are as follows:

[0128]

[0129] Where, dF d represents the infinitesimal water resistance term, dF m represents the infinitesimal additional mass force term, dF represents the sum of the infinitesimal water resistance term and the infinitesimal additional mass force term, ρ represents the water density, C d Indicates the water resistance coefficient, V n(x) represents the normal linear velocity of the connecting rod, D represents the equivalent mass diameter of the connecting rod, dl represents the length of the connecting rod, C m represents the additional mass force coefficient, and A represents the projected area of the connecting rod in the direction perpendicular to the linear velocity of the water flow.

[0130] The Morison equation is a differential equation. By taking the length of the deep-sea hydraulic manipulator link and modeling the motion and dynamic parameters based on the link element, the resistance torque caused by the water resistance and additional mass force on the entire manipulator link can be obtained by mathematical integration. First, the influence of the additional mass force and water resistance on the second link on the first rotary joint is analyzed as a case study to study the water resistance and additional mass force on each joint and link of the deep-sea hydraulic manipulator. Figure 2 As shown, the microelement coordinate system {dl2} of the second link of the robotic arm is established. The origin of the microelement coordinate system {dl2} is fixed to the second link. The distance between the origin of the microelement coordinate system {dl2} and the origin of the coordinate system {2} is l2. The directions of the x-axis, y-axis, and z-axis of the microelement coordinate system {dl2} are completely consistent with the directions of the three axes of the coordinate system {2}.

[0131] The transformation matrix of the connecting rod 2 infinitesimal coordinate system {dl2} relative to the coordinate system {2} is:

[0132]

[0133] The method for solving the velocity and acceleration of the connecting rod 2 infinitesimal coordinate system {dl2} is consistent with the method for solving the velocity and acceleration of the connecting rod center of mass. Therefore, the solution formula for the linear velocity and linear acceleration of the second connecting rod infinitesimal coordinate system {dl2} in the coordinate system {2} can be expressed as:

[0134]

[0135]

[0136] Where, 2 v l2 represents the linear velocity of the second link infinitesimal coordinate system {dl2} in the coordinate system {2}, represents the linear acceleration of the second link's infinitesimal coordinate system {dl2} in the coordinate system {2}, 2 p l2 represents the position vector of the origin of the second link infinitesimal coordinate system {dl2} relative to the coordinate system {2}, represents the x-axis linear velocity of the second link in the coordinate system {2}, represents the y-axis linear velocity of the second link in the coordinate system {2}, represents the linear velocity in the z direction of the second link in the coordinate system {2}, represents the x-axis linear acceleration of the second link in the coordinate system {2}, represents the y-axis linear acceleration of the second link in the coordinate system {2}, It represents the linear acceleration in the z direction of the second link in the coordinate system {2}.

[0137] The normal linear velocity and linear acceleration of the second link infinitesimal coordinate system {dl2} are:

[0138]

[0139]

[0140] The calculation formula of the infinitesimal water resistance of the second connecting rod relative to the coordinate system {2} can be expressed as:

[0141]

[0142] Where:

[0143] 2 dF d2_y =0.5ρC d 2 v l2_y || 2 v l2_y ||Dd2

[0144] 2 dF d2_z =0.5ρC d 2 v l2_z ‖ 2 v l2_z ‖Dd2

[0145] The expression of the water resistance of the second connecting rod relative to the coordinate system {1} is:

[0146]

[0147] The expression of the water resistance torque of the first revolute joint caused by the second link infinitesimal element is:

[0148]

[0149] Where, 1 p l2 Represents the position vector of the origin of the second link infinitesimal coordinate system {dl2} relative to the coordinate system {1}.

[0150] The additional mass force on the second link element in the coordinate system {2} can be expressed as:

[0151]

[0152] Where:

[0153] 2 dF m2_y =0.25ρC m π 2 a l2_y D 2 dl2

[0154] 2 dF m2_z =0.25ρC m π 2 a l2_z D 2 dl2

[0155] The calculation formula of the additional mass force on the second link element relative to the coordinate system {1} can be expressed as:

[0156]

[0157] The analytical expression of the additional mass moment of the second connecting rod relative to the coordinate system {1} is:

[0158]

[0159] Take the second connecting rod infinitesimal water resistance moment 1 dτ d2 and infinitesimal added mass moment 1 dτ m2The z-component of the axis and the integration of it can be used to obtain the influence of the water resistance and additional mass force on the first revolute joint of the second link. According to the above analysis of the second link, it can be derived to any link, where the water resistance torque and additional mass torque on the first revolute joint are based on the coordinate system {1} to analyze the hydrodynamic loads of the first link, the second link, the third link, the fourth link, the fifth link, and the sixth link, and the water resistance torque and additional mass torque on the second revolute joint need to be based on the coordinate system {2} to analyze the hydrodynamic loads of the second link, the third link, the fourth link, the fifth link, and the sixth link, and the water resistance torque and additional mass torque on the third revolute joint are based on the coordinate system {3}. The hydrodynamic loads on the 3rd, 4th, 5th, and 6th links are analyzed in coordinate system {3}. The water resistance torque and additional mass torque on the 4th revolute joint are analyzed in coordinate system {4}. The water resistance torque and additional mass torque on the 5th revolute joint are analyzed in coordinate system {5}. The water resistance torque and additional mass torque on the 6th revolute joint are analyzed in coordinate system {6}. Therefore, the hydrodynamic equation established according to Morison's formula is:

[0160]

[0161]

[0162] Where: τ d is the water resistance moment vector, is the water resistance velocity matrix, τ m is the added mass moment vector, M m (q) is the additional mass force acceleration matrix, is the additional mass force velocity matrix.

[0163] Combining the above Newton-Euler dynamics and hydrodynamic equations, the coupled dynamics model of the deep-sea hydraulic manipulator is:

[0164]

[0165] Among them, a method for modeling the sliding and overturning loads of a submersible includes the equivalent load at the center point of the supporting bottom surface of the submersible gravity and the robotic arm base load, sliding analysis, and overturning analysis.

[0166] The equivalent load at the center point of the supporting bottom surface of the submersible gravity and the robotic arm base load is:

[0167] like Figure 3 As shown, first, the equivalent load of the base load corresponding to each of the two deep-sea hydraulic manipulators at the center point of the submersible support surface is analyzed. The equivalent load equation of the manipulator base is shown in the following formula:

[0168] F mx =F x1 +F x2

[0169] F my =F y1 +F y2

[0170] F mz =F z1 +F z2

[0171] τ mx =τ x1 +τ x2 -(F y1 +F y2 )k z +(F z1 -F z2 )k y

[0172] τ my =τ y1 +τ y2 +(F x1 +F x2 )k z -(F z1 +F z2 )k x

[0173] τ mz =τ z1 +τ z2 +(F x1 -F x2 )k y +(F y1 +F y2 )k x

[0174] Where k x k represents the x-distance between the base of the manipulator and the center point of the submersible support bottom surface, y represents the y-direction distance between the base of the manipulator and the center point of the submersible support bottom surface, k z F represents the z-direction distance between the base of the manipulator and the center point of the submersible support bottom surface, mx is the x-direction equivalent load of the base loads corresponding to the two deep-sea hydraulic manipulators at the center point of the submersible support surface, F my is the y-direction equivalent load of the base load corresponding to each of the two deep-sea hydraulic manipulators at the center point of the submersible support surface, F mz is the equivalent load in the z direction of the base load corresponding to each of the two deep-sea hydraulic manipulators at the center point of the submersible support surface, τ mxis the x-direction equivalent moment load of the base load corresponding to each of the two deep-sea hydraulic manipulator arms at the center point of the submersible support surface, τ my is the y-direction equivalent moment load of the base load corresponding to each of the two deep-sea hydraulic manipulator arms at the center point of the submersible support surface, τ mz is the equivalent moment load in the z direction of the base load corresponding to each of the two deep-sea hydraulic manipulators at the center point of the submersible support surface, F x1 is the x-axis load of the Q-side robot arm, F x2 is the x-axis load of the E-side robot arm, F y1 is the y-axis load of the Q-side robot base, F y2 is the y-axis base load of the E-side robot arm, F z1 is the z-axis base load of the Q-side robot arm, F z2 is the z-axis base load of the E-side robot arm, τ x1 is the X-direction moment load of the Q-side manipulator, τ x2 is the moment load of the E-side robot arm in the x direction, τ y1 is the y-direction moment load of the Q-side manipulator, τ y2 is the y-direction moment load of the E-side manipulator, τ z1 is the z-direction base moment load of the Q-side robot arm, τ z2 is the base moment load of the E-side robot arm in z direction.

[0175] Secondly, the equivalent load at the center of the support bottom surface, which includes the gravity of the submersible and the load of the robotic arm base, is analyzed. The equivalent load equation is:

[0176]

[0177]

[0178] Where G x G represents the x-direction distance between the submersible's center of gravity and the center of the support bottom surface. y G represents the y-distance between the submersible's center of gravity and the center of the support bottom surface, z F represents the z-direction distance between the submersible's center of gravity and the center of the support bottom surface, and G represents the submersible's gravity. x is the equivalent load in the x direction at the center of the supporting bottom surface of the submersible gravity and the manipulator base load, F y is the equivalent load in y direction at the center point of the supporting bottom surface of the submersible gravity and the manipulator base load, F z is the equivalent load in z direction at the center point of the support bottom surface of the submersible gravity and the manipulator base load, τ x is the equivalent moment load in the x direction at the center of the supporting bottom surface of the submersible gravity and the manipulator base load, τ y is the equivalent moment load in the y direction at the center of the supporting bottom surface of the submersible gravity and the manipulator base load, τz is the equivalent moment load in the z direction at the center point of the supporting bottom surface of the submersible gravity and the manipulator base load.

[0179] Slip analysis includes critical linear slip analysis and critical rotational slip analysis.

[0180] 1) Critical linear slip

[0181] F fx1 、F fx2 、F fx3 、F fx4 is the component of the friction force of the four brackets in the opposite direction of the x-axis; F fy1 、F fy2 、F fy3 、F fy4 is the component of the friction force of the four brackets in the opposite direction of the y-axis, F n1 、F n2 、F n3 、F n4 is the support force of the four brackets in the z-axis direction, L is the distance between the bracket center and the center of the submersible bottom surface in the y-axis direction, H is the distance between the bracket center and the center of the submersible bottom surface in the x-axis direction, W is the distance between the bracket center and the bracket edge of the submersible bottom surface in the x-axis direction, μ is the friction coefficient, F xy F x and F y The resultant force, F f1 is the total friction force of bracket 1, F f2 is the total friction force of bracket 2, F f3 is the total friction force of bracket 3, F f4 is the total friction force of the bracket 4;

[0182] Assume that the submersible is loaded at the center point of the support bottom surface x and F y Under the action, the submersible is in a critical linear slip state in the xy direction, and the linear sliding load on the support surface of the submersible is as follows: Figure 4 As shown, the total friction force of each bracket is related to F x and F y The direction of the resultant force is opposite, then the critical force system equilibrium equation is:

[0183]

[0184] 2) Critical rotational slip

[0185] Assume that the submersible is loaded at the center point of the support bottom surface x and F y Under the action, the submersible is in a critical rotational slip state in the z direction, and the submersible support surface is subjected to a rotational slip load, such as Figure 5As shown, the total friction force of each bracket is perpendicular to the line connecting the center of the bottom surface and the support point of the support bracket, so the critical force system equilibrium equation is:

[0186]

[0187] The overturning analysis includes critical y-axis clockwise overturning, critical y-axis counterclockwise overturning, critical x-axis clockwise overturning, and critical x-axis counterclockwise overturning.

[0188] 1) Critical y-axis clockwise overturning

[0189] Assume that the submersible is loaded at the center point of the support bottom surface y Under the action, the submersible is in a critical overturning state clockwise around the y-axis, and the submersible support surface is overturned clockwise along the y-axis, as shown in Figure 6 As shown, the critical force system equilibrium equation in this state is:

[0190] τ y =W(F n3 +F n4 )=-WF z

[0191] 2) Critical y-axis counterclockwise tipping

[0192] Assume that the submersible is loaded at the center point of the support bottom surface y Under the action, the submersible is in a critical overturning state counterclockwise around the y-axis, and the submersible support surface is overturned counterclockwise around the y-axis, as shown in Figure 7 As shown, the critical force system equilibrium equation in this state is:

[0193] τ y =-W(F n1 +F n2 )=WF z

[0194] 3) Critical x-axis clockwise overturning

[0195] Assume that the submersible is loaded at the center point of the support bottom surface x Under the action, the submersible is in a critical overturning state clockwise around the x-axis, and the submersible support surface is overturned clockwise along the x-axis, as shown in Figure 8 As shown, the critical force system equilibrium equation in this state is:

[0196] τ x =L(F n1 +F n3 )=-LF z

[0197] 4) Critical x-axis counterclockwise overturning

[0198] Assume that the submersible is loaded at the center point of the support bottom surface xUnder the action, the vehicle is in a critical overturning state counterclockwise around the x-axis, and the submersible support surface is overturned counterclockwise around the x-axis, as shown in Figure 9 As shown, the critical force system equilibrium equation is:

[0199] τ x =-L(F n2 +F n4 )=LF z .

[0200] The above-described embodiments merely illustrate several implementations of the present invention, and while their descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the patent. It should be noted that a person skilled in the art would be able to make numerous variations and improvements without departing from the spirit of the present invention, all of which fall within the scope of protection of the present invention. Therefore, the scope of protection of the patent for this invention shall be determined by the appended claims.

Claims

1. A method for evaluating operational stability based on dynamic modeling of a submersible manipulator arm, characterized in that: The following steps are involved: S1. Establish the submersible's world coordinate system {w}, base coordinate system {0}, and each link coordinate system {n}; S2. Analysis of the joint driving torque vector τ of the submersible-manipulator a ; Water resistance moment vector τ d and the added mass moment vector τ m ; Based on the Morison equation, the calculation formula of the infinitesimal water resistance of the nth connecting rod relative to the coordinate system {n} of each connecting rod can be expressed as: Where: ρ represents the water density, C d represents the water resistance coefficient, n V ln_y represents the y-direction normal linear velocity of the nth link element relative to the coordinate system {n}, n v ln_z represents the normal linear velocity of the nth connecting rod relative to the coordinate system {n} of each connecting rod, D represents the equivalent mass diameter of the connecting rod, dl n represents the nth infinitesimal length of the connecting rod, It represents the component force of the water resistance of the nth link in the y direction relative to the coordinate system {n} of each link, It represents the component of the water resistance of the nth link in the z direction relative to the coordinate system {n} of each link; The expression of the water resistance of the nth connecting rod relative to the connecting rod coordinate system {1} is: Where, Represents the attitude transformation matrix of each link coordinate system {n} relative to the link coordinate system {1}; The expression of the water resistance torque of the first revolute joint caused by the nth connecting rod infinitesimal element is: Where, 1 p ln represents the position vector of the origin of the nth link element coordinate system {dln} relative to the coordinate system {1}. Based on the Morison equation, the calculation formula of the element-added mass force on the nth link element relative to each link coordinate system {n} can be expressed as: Where: C m represents the added mass force coefficient, n a ln_y represents the y-direction normal linear acceleration of the nth link element relative to the link coordinate system {n}, n a ln_z represents the z-direction normal linear acceleration of the nth link element relative to the link coordinate system {n}; dl n represents the nth infinitesimal length of the connecting rod; It represents the component of the additional mass force on the nth link relative to the coordinate system {n} of each link in the y direction. It represents the component of the added mass force on the nth link relative to the coordinate system {n} of each link in the z direction; The calculation formula of the additional mass force on the nth connecting rod element relative to the connecting rod coordinate system {1} can be expressed as: Where, Represents the attitude transformation matrix of each link coordinate system {n} relative to the link coordinate system {1}; The analytical expression of the additional mass moment of the nth connecting rod relative to the connecting rod coordinate system {1} is: Where, 1 p ln represents the position vector of the origin of the nth link infinitesimal coordinate system {dln} relative to the coordinate system {1}; S3. Combining the Newton-Euler equation and the hydrodynamic equation, establish the underwater coupled dynamic model of the deep-sea hydraulic manipulator τ = τ a +τ d +τ m .

2. The method for evaluating the operational stability of a submersible manipulator based on dynamic modeling according to claim 1, wherein: In step S1, the deep-sea hydraulic manipulator base coordinate system {0} and each link coordinate system {n} are established based on the DH method. is the attitude transformation matrix of the manipulator base coordinate system {0} relative to the world coordinate system {w}, Where γ is the rotation angle from the world coordinate system {w} to the base coordinate system {0} around the x-axis, β is the rotation angle from the world coordinate system {w} to the base coordinate system {0} around the y-axis, and α is the rotation angle from the world coordinate system {w} to the base coordinate system {0} around the z-axis. Set: s α =sinα,c α =cosα,s β =sinβ,c β =cosβ,s γ =sinγ,c γ =cosγ.

3. The method for evaluating the operational stability of a submersible manipulator based on dynamic modeling according to claim 2, wherein: In step S2, to balance the gravitational acceleration and buoyancy, the linear acceleration at the origin of the submersible's base coordinate system {0} is set to: Where g represents the acceleration due to gravity, ρ m represents the water density, ρ w Indicates the density of the robot material.

4. The method for evaluating the operational stability of a submersible manipulator based on dynamic modeling according to claim 2, wherein: The equivalent load of the base loads corresponding to the two deep-sea hydraulic manipulators at the center point of the submersible support surface is analyzed based on the coupled dynamics model of the deep-sea hydraulic manipulator Equivalent moment load of robot arm base F mx =F x1 +F x2 F my =F y1 +F y2 F mz =F z1 +F z2 t mx =t x1 +t x2 -(F y1 +F y2 )k z +(F z1 -F z2 )k y t my =t y1 +t y2 +(F x1 +F x2 )k z -(F z1 +F z2 )k x t mz =t z1 +t z2 +(F x1 -F x2 )k y +(F y1 +F y2 )k x Where k x k represents the x-distance between the base of the manipulator and the center point of the submersible support bottom surface, y k represents the y-distance between the base of the manipulator and the center point of the submersible support bottom surface, z F represents the z-direction distance between the base of the manipulator and the center point of the submersible support bottom surface, mx is the x-direction equivalent load of the base loads corresponding to the two deep-sea hydraulic manipulators at the center point of the submersible support surface, F my is the y-direction equivalent load of the base load corresponding to each of the two deep-sea hydraulic manipulators at the center point of the submersible support surface, F mz is the equivalent load in the z direction of the base load corresponding to each of the two deep-sea hydraulic manipulators at the center point of the submersible support surface, τ mx is the x-direction equivalent moment load of the base load corresponding to each of the two deep-sea hydraulic manipulator arms at the center point of the submersible support surface, τ my is the y-direction equivalent moment load of the base load corresponding to each of the two deep-sea hydraulic manipulator arms at the center point of the submersible support surface, τ mz is the equivalent moment load in the z direction of the base load corresponding to each of the two deep-sea hydraulic manipulators at the center point of the submersible support surface, F x1 is the x-axis load of the Q-side robot arm, F x2 is the x-axis load of the E-side robot arm, F y1 is the y-axis load of the Q-side robot arm, F y2 is the y-axis base load of the E-side robot arm, F z1 is the z-axis base load of the Q-side robot arm, F z2 is the z-axis base load of the E-side robot arm, τ x1 is the X-direction moment load of the Q-side manipulator, τ x2 is the x-direction moment load of the E-side manipulator, τ y1 is the y-direction moment load of the Q-side manipulator, τ y2 is the y-direction moment load of the E-side manipulator, τ z1 is the z-direction base moment load of the Q-side robot arm, τ z2 is the z-direction base moment load of the E-side robot arm; The equivalent load at the center point of the supporting bottom surface of the submersible gravity and the manipulator base load is: Where G x G represents the x-direction distance between the submersible's center of gravity and the center of the support bottom surface. y G represents the y-distance between the submersible's center of gravity and the center of the support bottom surface, z represents the z-direction distance between the submersible's center of gravity and the center point of the supporting bottom surface, G represents the submersible's gravity; F x is the equivalent load in the x direction at the center of the supporting bottom surface of the submersible gravity and the manipulator base load, F y is the equivalent load in y direction at the center point of the supporting bottom surface of the submersible gravity and the manipulator base load, F z is the equivalent load in z direction at the center point of the support bottom surface of the submersible gravity and the manipulator base load, τ x is the equivalent moment load in the x direction at the center of the supporting bottom surface of the submersible gravity and the manipulator base load, τ y is the equivalent moment load in the y direction at the center of the supporting bottom surface of the submersible gravity and the manipulator base load, τ z is the equivalent moment load in the z direction at the center point of the supporting bottom surface of the submersible gravity and the manipulator base load.

5. The method for evaluating the operational stability of a submersible manipulator based on dynamic modeling according to claim 4, wherein: Critical Linear Slip Analysis: F fx1 、F fx2 、F fx3 、F fx4 is the component of the friction force of the four brackets in the opposite direction of the x-axis; F fy1 、F fy2 、F fy3 、F fy4 is the component of the friction force of the four brackets in the opposite direction of the y-axis, F n1 、F n2 、F n3 、F n4 is the support force of the four brackets in the z-axis direction, L is the distance between the bracket center and the center of the submersible bottom surface in the y-axis direction, H is the distance between the bracket center and the center of the submersible bottom surface in the x-axis direction, W is the distance between the bracket center and the bracket edge of the submersible bottom surface in the x-axis direction, μ is the friction coefficient, F xy F x and F y The resultant force, F f1 is the total friction force of bracket 1, F f2 is the total friction force of bracket 2, F f3 is the total friction force of bracket 3, F f4 is the total friction force of the bracket 4; Assume that the submersible is loaded at the center point of the support bottom surface x and F y Under the action, the xy direction is in a critical linear slip state, the submersible support surface linear sliding load, the total friction force of each bracket and F x and F y The direction of the resultant force is opposite, then the critical force system equilibrium equation is: Critical rotational slip: Assume that the submersible is loaded at the center point of the support bottom surface x and F y Under the action, the submersible is in a critical rotational slip state in the z direction, the support surface of the submersible rotates and slides under the load, and the total friction force of each bracket is perpendicular to the line connecting the center of the bottom surface and the support point of the support bracket. The critical force system equilibrium equation is:

6. The method for evaluating the operational stability of a submersible manipulator based on dynamic modeling according to claim 5, characterized in that: The capsize analysis includes the critical y-axis clockwise capsize, the critical y-axis counterclockwise capsize, the critical x-axis clockwise capsize, and the critical x-axis counterclockwise capsize: 1) Critical y-axis clockwise overturning Assume that the submersible is loaded at the center point of the support bottom surface y Under the action of , it is in a critical overturning state clockwise around the y axis. The equilibrium equation of the critical force system in this state is: τ y =W(F n3 +F n4 )=-WF z 2) Critical y-axis counterclockwise tipping Assume that the submersible is loaded at the center point of the support bottom surface y Under the action of , it is in a critical overturning state counterclockwise around the y-axis. The critical force system equilibrium equation in this state is: τ y =-W(F n1 +F n2 )=WF z 3) Critical x-axis clockwise overturning Assume that the submersible is loaded at the center point of the support bottom surface x Under the action of , it is in a critical overturning state clockwise around the x-axis. The critical force system equilibrium equation in this state is: τ x =L(F n1 +F n3 )=-LF z 4) Critical x-axis counterclockwise overturning Assume that the submersible is loaded at the center point of the support bottom surface x Under the action of , it is in a critical overturning state counterclockwise around the x-axis, and the critical force system equilibrium equation is: τ x =-L(F n2 +F n4 )=LF z 。

Images