A five-axis hydraulic mechanical arm control method based on inequality constraints

Abstract

The present application relates to the field of hydraulic mechanical arm, especially to a five-axis hydraulic mechanical arm control method based on inequality constraint, comprising the following steps: defining the generalized coordinates of the mechanical arm according to the improved D-H parameter method, establishing a dynamics model satisfying Newton mechanics or Lagrange mechanics, optimizing the dynamics model according to the inequality constraint condition to obtain the dynamics model of the mechanical constraint system, and adopting the space mapping mode to convert the master arm remote lever Cartesian space to the slave hydraulic mechanical arm Cartesian space to expand the operation space of the master robot on the slave robot. The present application adopts the DH parameter method to establish the base coordinate system with the base center point of the mechanical arm as the origin, and positions the position of the whole mechanical arm according to the joint angle, and then combines the inequality constraint mode to establish the mechanical arm controller, so as to reduce the error between the control parameter output by the controller and the actual operation state and the expected operation state.

Description

Technical Field

[0001] This invention relates to the field of hydraulic robotic arms, and more particularly to a control method for a five-axis hydraulic robotic arm based on inequality constraints. Background Technology

[0002] With the rapid development of robotics technology, the production efficiency of various industries has increased. The emergence of robots has made it possible to operate in dangerous environments or environments with unknown factors that are inaccessible to humans, such as nuclear waste disposal in the nuclear industry, space station maintenance in the aerospace industry, deep-sea exploration and sampling, and disaster relief. However, due to the error factors of existing robot systems, there is a large gap between the expected results of the controller and the actual operation. Furthermore, the operating space for remote operation of robots in harsh environments is relatively narrow. Summary of the Invention

[0003] To address the shortcomings of existing technologies, this invention provides the following technical solution:

[0004] A control method for a five-axis hydraulic robotic arm based on inequality constraints includes the following steps:

[0005] S1: Define the generalized coordinates of the robotic arm according to the improved DH parameter method, and establish a dynamic model that satisfies Newtonian mechanics or Lagrange mechanics.

[0006] S2: Optimize the dynamic model based on the inequality constraints to obtain the dynamic model of the mechanically constrained system.

[0007] S3: The master end arm telescopic lever Cartesian space is transformed into the slave end hydraulic manipulator Cartesian space by using spatial mapping to expand the master robot's operating space on the slave robot.

[0008] As an improvement to the above technical solution, step S1 includes the following steps:

[0009] S11: Establish several coordinate systems with the positions of each node of the robotic arm base as the origin, establish an improved DH parameter table based on the several coordinate systems, and determine the total homogeneous transformation matrix of the hydraulic robotic arm end joint relative to the base coordinate system and the homogeneous transformation matrix of the end effector relative to the fixed coordinate system based on the improved DH parameter table.

[0010] S12: The total homogeneous transformation matrix of the end joint of the hydraulic robotic arm relative to the base coordinate system and the homogeneous transformation matrix of the end effector relative to the fixed coordinate system determine the joint rotation angle between several axes.

[0011] S13: Determine the positions of several axes based on the joint rotation angles to determine the generalized coordinates of the five-axis mechanical system.

[0012] S14: Establish a dynamic model that satisfies Newtonian or Lagrange mechanics based on generalized coordinates.

[0013] As an improvement to the above technical solution, step S2 includes the following steps:

[0014] S21: The system introduces several external constraints and establishes the actual explicit motion equations of the constrained system.

[0015] S22: Based on the Gaussian minimum constraint principle, a dynamic model of a hydraulic five-axis robotic arm is established using the Udwadia-Kalaba equation.

[0016] S23: Transform the inequality constraints using a state transformation function, and input the transformation result into the dynamic model in step S22 to obtain the optimized dynamic model.

[0017] As an improvement to the above technical solution, the dynamic model includes the following equation:

[0018]

[0019]

[0020] Where M(q,t) is the system inertia matrix, and M(q,t) = M T (q, t) ∈ R n×n , For generalized speed, For generalized acceleration, Let q be a known force applied to the system. These are the initial conditions at time t0.

[0021] The optimized dynamic model includes the following equation:

[0022]

[0023] Where θ is the n-dimensional generalized coordinate describing the system. The velocity vector, M(θ, t), is an n×n symmetric positive definite inertia matrix, and A(θ, t) is an m×n constraint matrix. It is an m×1 vector. It is a centrifugal force or Coriolis force matrix. It is the gravity matrix, q m and q M Let τ be the upper and lower bounds of the joint angle, and τ be the generalized control force (torque). + This represents the generalized inverse of a matrix, where t is the time variable.

[0024] As an improvement to the above technical solution, step S3 includes the following steps:

[0025] S31: Determine the master-slave robot position control relationship in the teleoperation system's free mode based on the position of the slave robot's end effector and the master robot's end effector.

[0026] S32: Establish a spatial mapping model A in free mode by treating the three directions in the coordinate system as straight line segments.

[0027] S33: Determine the position control relationship between the master and slave robots in the remote operating system working mode based on the position of the slave hydraulic manipulator and the position of the master robot, and establish a mapping model B based on the control relationship.

[0028] S34: Determine the operating space of the slave robot controlled by the master robot based on mapping model A and mapping model B.

[0029] As an improvement to the above technical solution, the master-slave robot position control relationship in the free mode of the teleoperation system is as follows:

[0030] Q s =λQ m +Q n

[0031] Among them, Q S From the position of the robot's end, Q M Q is the position of the end effector of the main robot, λ is the proportional coefficient, and Q is the position of the end effector of the main robot. N It is a position constant.

[0032] The mapping model A includes the following formula:

[0033]

[0034]

[0035]

[0036] Where, x m y m z m It is the position of the master robot in the Cartesian coordinate system, x mMax The maximum position of the master hand controller in the X direction, x mMin The minimum position of the master hand controller in the X direction, x sMax x represents the maximum position of the hydraulic machinery in the X direction. sMin Let x be the minimum position of the hydraulic machinery at the slave end in the X direction. s y s z sIt is the target spatial position that the slave robot should reach after the master robot's spatial position is mapped through the model.

[0037] The master-slave robot position control relationship in the teleoperation system's working mode:

[0038] P S =λ·P M +C·V+P N

[0039] Among them, P s For the position of the end hydraulic robotic arm, P M Main robot position, P N λ is the position offset, λ is the Cartesian space scaling factor, k is the velocity scaling factor, and V is the linear velocity of the master end effector.

[0040] The mapping model B includes the following formula:

[0041]

[0042] Where, x s y s z s These are the position components of the end hydraulic robotic arm, λ x , λ y , λ z These are the components of the Cartesian scale coefficients in the x, y, and z directions, respectively. M y M z M Main robot position components, k x k y k z These are the components of the velocity proportionality coefficient in the x, y, and z directions, respectively, v x ν y v z These are the linear velocity components of the end effector of the master robot, x N y N z N For positional variable components.

[0043] As an improvement to the above technical solution, when the gripper at the end of the robotic arm grasps an object, the gripping force control of the gripper depends on the following steps:

[0044] S41: Based on Coulomb's friction theorem, determine the relationship between the normal force and the frictional force in a static and stable state, and obtain the conditions for stable gripping by the gripper based on this relationship.

[0045] S42: Set a target static friction coefficient and compensate for errors caused by prior measurement based on the set static friction coefficient.

[0046] S43: Determine the minimum gripping force required by the gripper based on the calculation error and the conditions for stable gripping.

[0047] As an improvement to the above technical solution, the condition for stable gripping by the gripper in step S41 is:

[0048]

[0049] Among them, f n For clamping force, M g Let f be the weight of the object, f be the frictional force acting on the object, and μ0 be the coefficient of static friction.

[0050] As an improvement to the above technical solution, step S42 includes the following steps:

[0051] S421: Preset initial static friction coefficient.

[0052] S422: Increase the weight of the object gripped by the gripper to obtain the current static friction coefficient.

[0053] S423: Determine the static friction coefficient error based on the current static friction coefficient and the initial static friction coefficient, and determine the clamping force deviation based on the static friction coefficient error.

[0054] As an improvement to the above technical solution, the calculation of the current static friction coefficient in step S422 depends on the following formula:

[0055] F′ y / F′ x =μ s

[0056] Among them, F′ y To increase the new tangential force generated after grasping the weight of the object, F′ x This refers to the new clamping force applied by the system to stably grasp the object;

[0057] The ratio between the tangential force after increasing the object's weight and the clamping force before increasing the object's weight is:

[0058] η=F′ y / F x

[0059] The static friction coefficient error in step S423 is:

[0060] err μ =μ s -η

[0061] The deviation of the clamping force is:

[0062]

[0063] Among them, err μ For the static friction coefficient error, F x For clamping force, F y It is a tangential force.

[0064] The beneficial effects of this invention are:

[0065] The DH parameter method is used to establish a base coordinate system with the center point of the robot arm base as the origin. Based on this coordinate system, the joint angles between two adjacent arms of the five-axis robot arm are obtained. The position of the entire robot arm is located based on these joint angles. In addition, the robot arm controller is established by combining inequality constraints, thereby reducing the error between the control parameters output by the controller and the actual operating state and the desired operating state. Attached Figure Description

[0066] Figure 1 This is a structural diagram of the five-axis hydraulic robotic arm involved in this invention.

[0067] Figure 2 This is a schematic diagram of the master-slave space mapping of the present invention.

[0068] Figure 3 This is a schematic diagram of the gripper holding an object in the five-axis hydraulic robotic arm involved in this invention.

[0069] Figure 4 This is a schematic diagram of the force proportional control principle based on two-dimensional force feedback of the present invention. Detailed Implementation

[0070] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0071] Due to the error factors in existing robot systems, there is a large gap between the controller's expected results and the actual operation, and the operating space for remote operation of robots is narrow in harsh environments.

[0072] To address the aforementioned issues, a five-axis hydraulic robotic arm control method based on inequality constraints is provided, comprising the following steps:

[0073] S1: Define the generalized coordinates of the robotic arm according to the improved DH parameter method, and establish a dynamic model that satisfies Newtonian mechanics or Lagrange mechanics.

[0074] Specifically, with Figure 1 Based on the robotic arm shown, step S1 includes the following steps:

[0075] S11: Establish several coordinate systems with the positions of each node of the robotic arm base as the origin, establish an improved DH parameter table based on the several coordinate systems, and determine the total homogeneous transformation matrix of the hydraulic robotic arm end joint relative to the base coordinate system and the homogeneous transformation matrix of the end effector relative to the fixed coordinate system based on the improved DH parameter table.

[0076] Specifically, the improved DH parameter method is used to establish a base coordinate system with the center point of the robot arm base as the origin, establish a link coordinate system at each joint of the robot arm, and establish a tool coordinate system at the end effector of the robot arm.

[0077] The improved DH parameter table is as follows:

[0078]

[0079] Under forward kinematics:

[0080] Based on the DH parameter table, the homogeneous transformation matrix between two adjacent joint coordinate systems is as follows:

[0081]

[0082]

[0083]

[0084]

[0085]

[0086] Among them, S i =sinθ i C i =cosθ i , i = 1, 2, 3, 4, 5.

[0087] Multiplying the above matrices together, we obtain the total homogeneous transformation matrix of the end joint relative to the base coordinate system as follows:

[0088]

[0089] Given the angles of each joint, the position and orientation information of the end effector can be determined.

[0090] Under inverse kinematics:

[0091] The homogeneous transformation matrix of the hydraulic robotic arm's end effector relative to the fixed coordinate system is as follows:

[0092]

[0093] S12: The total homogeneous transformation matrix of the end joint of the hydraulic robotic arm relative to the base coordinate system and the homogeneous transformation matrix of the end effector relative to the fixed coordinate system determine the joint rotation angle between several axes.

[0094] Moving the part containing θ1 in equation (1-1) to the left side of the equation, we can transform the above equation to obtain:

[0095]

[0096] It can be written as:

[0097]

[0098] Making the elements (2,4) on both sides of equation (1-3) equal, we get:

[0099]

[0100] After conversion, we can obtain θ1:

[0101]

[0102] By making the elements (1,4) and (3,4) on both sides of equation (1-3) equal respectively, we get:

[0103] C1P x +S1P y =a1c2+a2c 23 -d5S 234 (1-5)

[0104] P z -L0=-a1S2-a2S 23 -d5c 234 (1-6)

[0105] By making the elements (1,3) and (3,3) on both sides of equation (1-3) equal respectively, we get:

[0106] c1r 13 +S1r 23 =-S 234 (1-7)

[0107] r 33 =--C 234 (1-8)

[0108] Substituting equations (1-7) and (1-8) into equations (1-5) and (1-6) respectively, eliminating the terms S234 and C234, and then eliminating the terms S23 and C23 by squaring, we obtain:

[0109] [C1P x +S1P y -d5C1r13 -d5S1r 23 -a1C2] 2 +[P z -d1-d5r 23 +a1S2] 2 =a2 2 (1-9)

[0110] Let K1 = C1P x +S1P y -d5C1r 13 -d5S1r 23 K2 = P z -d1-d5r 23 Then equation (1-9) can be simplified to:

[0111] -2K1a1C2+2K2a1S2=a22-a12-K1 2 -K2 2 (1-10)

[0112] get:

[0113]

[0114] Using the same method, substitute equations (1-7) and (1-8) into equations (1-5) and (1-6) respectively, eliminate the S234 and C234 terms, and separate the S23 and C23 terms to obtain:

[0115] a2C 23 +a2S 23 =C1P x +S1P y -a1C2-d5(C1r 13 +S1r 23 )+d5r 23 +d1-P z -a1S2(1-11)

[0116] make:

[0117] K3=C1P x +S1P y -a1C2-d5(C1t 13 +S1r 23 )+d5r 23 +d1-P z -a1S2

[0118] get:

[0119]

[0120] therefore:

[0121]

[0122] Then by The transformation yields:

[0123]

[0124] Right now:

[0125]

[0126] By making the elements (3,1) and (3,2) on both sides of equation (1-12) equal respectively, we get:

[0127] -S1r 11 +C1r 21 =-S5(1-13)

[0128] -S1r 12 +C1r 22 =-C5(1-14)

[0129] Therefore, we get:

[0130]

[0131] If we make the elements (1, 3) and (2, 3) on both sides of equation (1-12) equal respectively, we get:

[0132] C 23 C1r 13 +C 23 S1r 23 --S 23 r 33 =-S4 (1-15)

[0133] -S 23 C1r 13 -S 23 S1r 23 -C 23 r 33 =C4 (1-16)

[0134] Find:

[0135]

[0136] S13: Determine the positions of several axes based on the joint rotation angles to determine the generalized coordinates of the five-axis mechanical system.

[0137] After the joint angles are determined, the position of each joint axis (i.e., the position of each robotic arm) can be determined based on the center point of the base. Based on this position, the generalized coordinates of the five-axis mechanical system are determined as follows:

[0138] q = [q1 q2 q3 q4 q5] T

[0139] Where q1……q5 are the coordinate positions of the five joint axes respectively.

[0140] S14: Establish a dynamic model that satisfies Newtonian or Lagrange mechanics based on generalized coordinates.

[0141] The dynamic model includes the following equation:

[0142]

[0143]

[0144] Where M(q,t) is the system inertia matrix, and M(q,t) = M T (q, t) ∈ R n×n , For generalized speed, For generalized acceleration, Let q be a known force applied to the system. The initial conditions at time t0

[0145] S2: Optimize the dynamic model based on the inequality constraints to obtain the dynamic model of the mechanically constrained system.

[0146] Specifically, step S2 includes the following steps:

[0147] S21: The system introduces several external constraints and establishes the actual explicit motion equations of the constrained system.

[0148] First, we introduce the constraints that should be considered in the system. Assume the system is subject to h complete constraints, which take the form:

[0149]

[0150] The form subject to kh nonholonomic constraints is:

[0151]

[0152] Second-order constraints are the most suitable form for further dynamic analysis and control design. The advantage of using second-order constraints is that the acceleration is linear, and the information satisfying the zeroth or first-order initial conditions is still preserved in the initial conditions of the second-order constraint equations. Assuming the constraint equations are sufficiently smooth, differentiating the holonomic constraint equations twice and the nonholonomic constraint equations once yields the second-order form of the constraint equations, i.e.

[0153]

[0154] in For an m×n matrix, It is an m×1 column vector.

[0155] By introducing a series of external constraints into the system, the robotic arm system becomes a "constrained system." Therefore, the actual explicit equations of motion for the constrained system can be expressed as:

[0156]

[0157] in, Additional constraints are the forces created by applying external constraints; these can be either whole or nonholonomic constraints.

[0158] S22: Based on the Gaussian minimum constraint principle, a dynamic model of a hydraulic five-axis robotic arm is established using the Udwadia-Kalaba equation.

[0159] Based on the Gaussian minimum constraint principle, Udwadia-Kalaba proposed an explicit expression for the additional constraint force vector, namely...

[0160]

[0161] The dynamic model of the mechanical constraint system of the hydraulic five-axis robot arm is obtained using the Udwadia-Kalaba equations:

[0162]

[0163] Where q is the n-dimensional generalized coordinate describing the system. The velocity vector, M(q,t), is an n×n symmetric positive definite inertia matrix, and A(q,t) is an m×n constraint matrix. It is an m×1 vector. It is the centrifugal force or Coriolis force matrix, τ is the generalized control force (torque), () + This represents the generalized inverse of a matrix, where t is the time variable.

[0164] To ensure that the output of the control system is within the expected range, considering the inequality constraints that the system needs to satisfy, a suitable state transformation function can be selected to transform from a bounded state to an unbounded state. The specific execution step is S23.

[0165] S23: Transform the inequality constraints using a state transformation function, and input the transformation result into the dynamic model in step S22 to obtain the optimized dynamic model.

[0166] Specifically, the inequality constraints include the following:

[0167] q m <q<qM

[0168] Where, q m and q M Upper and lower bounds of joint angles

[0169] The state transition function includes the following equation:

[0170]

[0171] The generalized coordinates are obtained by transforming them according to the state change function:

[0172]

[0173] After differentiation, we get:

[0174]

[0175] Substituting the above equation into the dynamic model of the mechanically constrained system, we obtain the optimized dynamic model as follows:

[0176]

[0177] Where θ is the n-dimensional generalized coordinate describing the system. The velocity vector, M(θ, t), is an n×n symmetric positive definite inertia matrix, and A(θ, t) is an m×n constraint matrix. It is an m×1 vector. It is a centrifugal force or Coriolis force matrix. It is the gravity matrix, q m and q M Let τ be the upper and lower bounds of the joint angle, and τ be the generalized control force (torque). + This represents the generalized inverse of a matrix, where t is the time variable.

[0178] S3: The master end arm telescopic lever Cartesian space is transformed into the slave end hydraulic manipulator Cartesian space by using spatial mapping to expand the master robot's operating space on the slave robot.

[0179] Specifically, step S3 includes the following steps:

[0180] S31: Determine the master-slave robot position control relationship in the free mode of the teleoperation system based on the position of the slave robot's end effector and the master robot's end effector;

[0181] The master-slave robot position control relationship in the free mode of the teleoperation system is as follows:

[0182] Q s =λQ m +Q n

[0183] Among them, Q S From the position of the robot's end, Q M This refers to the position of the end effector of the main robot, λ is the scaling factor (from the perspective of line segments, it is the magnification factor by which the short line is expanded into the length of the long line), and Q. N It is a position constant (from the perspective of line segments, the distance that the extended shorter line needs to move to be in the same position as the longer line).

[0184] In order for the master robot to control more of the slave robot's operating space and achieve a greater coverage, the specific execution steps are as follows: S32.

[0185] S32: Establish a spatial mapping model A in free mode by treating the three directions in the coordinate system as straight line segments;

[0186] That is, the mapping algorithm is designed by treating the X, Y, and Z directions as line segments respectively. The resulting mapping model A includes the following formula:

[0187]

[0188]

[0189]

[0190] Where, x m y m z m It is the position of the master robot in the Cartesian coordinate system, x mMax The maximum position of the master hand controller in the X direction, x mMin The minimum position of the master hand controller in the X direction, x sMax x represents the maximum position of the hydraulic machinery in the X direction. sMin Let x be the minimum position of the hydraulic machinery at the slave end in the X direction. s y s z s This refers to the target spatial position that the slave robot should achieve after the master robot's spatial position is mapped through the model.

[0191] After determining the mapping function in free mode, step S33 is executed to determine the mapping function in working mode.

[0192] S33: Determine the position control relationship between the master and slave robots in the remote operating system working mode based on the position of the slave hydraulic manipulator and the position of the master robot, and establish a mapping model B based on the control relationship.

[0193] The master-slave robot position control relationship in the teleoperation system's working mode:

[0194] P S =λ·P M +C·V+P N (3)

[0195] Among them, P s For the position of the end hydraulic robotic arm, P M Main robot position, P N λ is the position offset, λ is the Cartesian space scaling factor, k is the velocity scaling factor, and V is the linear velocity of the master end effector.

[0196] The mapping model B includes the following formula:

[0197]

[0198] Where, x s y s z s These are the position components of the end hydraulic robotic arm, λ x , λ y , λ z These are the components of the Cartesian scale coefficients in the x, y, and z directions, respectively. M y M z M Main robot position components, k x k y k z These are the components of the velocity proportionality coefficient in the x, y, and z directions, respectively, v x ν y v z These are the linear velocity components of the end effector of the master robot, x N y N z N For positional variable components.

[0199] As can be seen from equation (3), when the operator acts on the master robot, the master robot will generate a position P. M And the end-effector linear velocity V, while the end-effector position P S By P M Together with V, it is determined that the faster the master robot moves, the greater the displacement increment of the slave robot's end point.

[0200] S34: Determine the operating space of the slave robot controlled by the master robot based on mapping model A and mapping model B.

[0201] Specifically, the impact of mapping model A or mapping model B on the operation of the main robot depends on the following steps:

[0202] S341: Solve for the workspace of the master and slave devices of the remote operating system respectively.

[0203] S342: Select a mapping model and solve it to address the differences in the workspace.

[0204] S343: Obtain joint angle changes from the master robot's motion and perform forward kinematics analysis on the master robot.

[0205] S344: The master-end forward solution result is fed into the solved mapping model to obtain the target pose.

[0206] S345: The target pose is transmitted to the slave system, inverse kinematics is performed to obtain the rotation angles of each joint of the slave device, and then sent to the slave driver to control the joint motors.

[0207] Furthermore, to better control the robotic arm, a further control method is provided for the gripper's grasping action. Specifically, when the gripper at the end of the robotic arm grasps an object, the gripping force control of the gripper depends on the following steps:

[0208] S41: Based on Coulomb's friction theorem, determine the relationship between the normal force and the frictional force in a static and stable state, and obtain the conditions for stable gripping by the gripper based on this relationship;

[0209] According to Coulomb's friction theorem, in a static and stable state, the normal force f n The frictional force f satisfies the following equation:

[0210] f≤μ0f n (4-1)

[0211] Where μ0 is the static friction coefficient, its magnitude is related to the surface roughness and other surface characteristics of the contact surface. The selection of μ0 is related to the physical properties of the material. Different objects to be gripped have different static friction coefficients, but it is not related to the size of the contact area.

[0212] From equation (4-1), we can obtain:

[0213]

[0214] If the grasp satisfies equation (4-2), then by the normal force f n The resulting frictional force f can counteract the object's weight, thus preventing relative sliding.

[0215] Combining equation (4-1), it can be seen that the condition for stable gripping by the gripper in step S41 is:

[0216] 2μ0f n ≥Mg

[0217] Simplifying, we get:

[0218]

[0219] Among them, f n For clamping force, M g Let f be the weight of the object, f be the frictional force acting on the object, and μ0 be the coefficient of static friction.

[0220] In addition, M g μ0 can be detected by an octagonal ring two-dimensional force sensor. According to the force balance of the object, F... y =f,F x =f n M g =2F y Equation (4-3) can be simplified to:

[0221] μ0≥F y / F x

[0222] In order to prevent the object from slipping and to prevent damage to the object, the minimum gripping force that can grip the object must be obtained. The coefficient of friction can be obtained by the ratio of tangential force to normal force. The coefficient of friction is detected by using an octagonal ring two-dimensional force sensor. Based on this, step S42 is executed.

[0223] S42: Preset target static friction coefficient, and compensate for the error caused by the pre-measurement based on the preset static friction coefficient;

[0224] Step S42 includes the following steps:

[0225] S421: Preset initial static friction coefficient.

[0226] The default value can be set to μ. s =0.7μ0, to pre-compensate for measurement errors.

[0227] The tangential force is F y With clamping force F x The following equation applies between them:

[0228] F y / F x =μ s (4-4)

[0229] To detect the impact of the error, step S422 is performed.

[0230] S422: Increase the weight of the object gripped by the gripper to obtain the current static friction coefficient.

[0231] Even after increasing the weight of the object, equation (4-4) still needs to be satisfied. Therefore, the calculation of the current static friction coefficient in step S422 still needs to satisfy the following equation:

[0232] F′ y / F′ x=μ s

[0233] Among them, F′ y To increase the new tangential force generated after grasping the weight of the object, F′ x The new clamping force applied by the system to stably grasp the object.

[0234] The ratio between the tangential force after increasing the object's weight and the clamping force before increasing the object's weight is:

[0235] η = F y / F x

[0236] The static friction coefficient error in step S423 is:

[0237] err μ =μ s -η

[0238] The deviation of the clamping force is:

[0239]

[0240] Among them, err μ For the static friction coefficient error, F x For clamping force, F y It is a tangential force.

[0241] Let K1 = F 2 x / F y Then the above formula can be rewritten as:

[0242] err F =K1err μ

[0243] S423: Determine the static friction coefficient error based on the current static friction coefficient and the initial static friction coefficient, and determine the clamping force deviation based on the static friction coefficient error.

[0244] S43: Determine the minimum gripping force required by the gripper based on the calculation error and the conditions for stable gripping.

[0245] By collecting the force information of the octagonal ring sensor, the current contact point F′ is calculated. y / F x The value η is compared with the maximum static friction coefficient of 0.7 times μ0, and the current force deviation is calculated using the friction coefficient deviation. This force deviation is then eliminated through force-based closed-loop incremental PI force control shifting, ensuring that F... y / F x The value of μ stabilizes and tends to be μ sThis avoids slippage between the gripper and the object, achieving gripping with minimal clamping force.

[0246] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it.

Claims

1. A control method for a five-axis hydraulic robotic arm based on inequality constraints, characterized in that, Includes the following steps: S1: Define the generalized coordinates of the robotic arm according to the improved DH parameter method, and establish a dynamic model that satisfies Newtonian mechanics or Lagrange mechanics; S2: Optimize the dynamic model based on the inequality constraints to obtain the dynamic model of the mechanically constrained system; S3: Use spatial mapping to transform the Cartesian space of the master arm joystick to the Cartesian space of the slave hydraulic manipulator to expand the master robot’s operating space on the slave robot; Step S3 includes the following steps: S31: Determine the master-slave robot position control relationship in the free mode of the teleoperation system based on the position of the slave robot's end effector and the master robot's end effector; S32: Establish a spatial mapping model A in free mode by treating the three directions in the coordinate system as straight line segments; S33: Determine the position control relationship between the master and slave robots in the teleoperation system working mode based on the position of the slave hydraulic manipulator and the position of the master robot, and establish a mapping model B based on the control relationship; S34: Determine the operating space of the slave robot controlled by the master robot based on mapping model A and mapping model B; The master-slave robot position control relationship in the free mode of the teleoperation system is as follows: in, It is from the end of the robot. It is the position of the end effector of the main robot. It is a proportionality coefficient. It is a position constant; The mapping model A includes the following formula: in, , , It is the position of the master robot in the Cartesian coordinate system. The maximum position of the main hand controller in the X direction. The minimum position of the master hand controller in the X direction. This represents the maximum position of the hydraulic mechanism at the X-axis. These represent the minimum positions of the hydraulic machinery at the slave end in the X direction. , , This is the target spatial position that the slave robot should reach after the master robot's spatial position is mapped through the model; The master-slave robot position control relationship in the teleoperation system's working mode: in, Position of the hydraulic robotic arm at the end. Position of the main robot. For positional offset, The scaling factor for Cartesian space. This is the speed proportionality coefficient. The linear velocity of the end effector of the main robot; The mapping model B includes the following formula: in, , , These are the position components of the hydraulic robotic arm. , These are the components of the Cartesian scale coefficients in the x, y, and z directions, respectively. , , Position components of the master robot, , , These are the components of the velocity proportionality coefficient in the x, y, and z directions, respectively. , , These are the linear velocity components at the end effector of the main robot. , , For positional variable components.

2. The five-axis hydraulic robotic arm control method based on inequality constraints according to claim 1, characterized in that: Step S1 includes the following steps: S11: Establish several coordinate systems with the positions of each node of the robotic arm base as the origin, establish an improved DH parameter table based on the several coordinate systems, and determine the total homogeneous transformation matrix of the hydraulic robotic arm end joint relative to the base coordinate system and the homogeneous transformation matrix of the end effector relative to the fixed coordinate system based on the improved DH parameter table. S12: The total homogeneous transformation matrix of the end joint of the hydraulic robotic arm relative to the base coordinate system and the homogeneous transformation matrix of the end effector relative to the fixed coordinate system determine the joint rotation angle between several axes; S13: Determine the positions of several axes based on the joint rotation angles to determine the generalized coordinates of the five-axis mechanical system; S14: Establish a dynamic model that satisfies Newtonian or Lagrange mechanics based on generalized coordinates.

3. The five-axis hydraulic robotic arm control method based on inequality constraints according to claim 2, characterized in that: Step S2 includes the following steps: S21: The system introduces several external constraints and establishes the actual explicit equations of motion for the constrained system; S22: Based on the Gaussian minimum constraint principle, a dynamic model of a hydraulic five-axis robotic arm is established using the Udwadia-Kalaba equation; S23: Transform the inequality constraints using a state transformation function, and input the transformation result into the dynamic model in step S22 to obtain the optimized dynamic model.

4. The five-axis hydraulic robotic arm control method based on inequality constraints according to claim 3, characterized in that: The dynamic model includes the following equation: in, Let be the system inertia matrix, and , For generalized speed, For generalized acceleration, For a known force applied to the system, for Initial conditions at time t; The optimized dynamic model includes the following equation: in, To describe the system's n-dimensional generalized coordinates, velocity vector It is an n×n symmetric positive definite inertia matrix. It is an m×n constraint matrix. It is an m×1 vector. It is a centrifugal force or Coriolis force matrix. It is the gravity matrix. and These are the upper and lower bounds of the joint angle. It is a generalized control force (torque). Represents the generalized inverse of a matrix. It is a time variable.

5. The five-axis hydraulic robotic arm control method based on inequality constraints according to claim 1, characterized in that: When the gripper at the end of the robotic arm grasps an object, the gripping force of the gripper is controlled by the following steps: S41: Based on Coulomb's friction theorem, determine the relationship between the normal force and the frictional force in a static and stable state, and obtain the conditions for stable gripping by the gripper based on this relationship; S42: Preset target static friction coefficient, and compensate for the error caused by the pre-measurement based on the preset static friction coefficient; S43: Determine the minimum gripping force required by the gripper based on the calculation error and the conditions for stable gripping.

6. The five-axis hydraulic robotic arm control method based on inequality constraints according to claim 5, characterized in that: The condition for stable gripping by the gripper in step S41 is: in, For clamping force, Let be the weight of the object, and f be the frictional force acting on the object. It is the static friction coefficient.

7. The five-axis hydraulic robotic arm control method based on inequality constraints according to claim 5, characterized in that: Step S42 includes the following steps: S421: Preset initial static friction coefficient; S422: Increase the weight of the object gripped by the gripper to obtain the current static friction coefficient; S423: Determine the static friction coefficient error based on the current static friction coefficient and the initial static friction coefficient, and determine the clamping force deviation based on the static friction coefficient error.

8. The five-axis hydraulic robotic arm control method based on inequality constraints according to claim 7, characterized in that: The calculation of the current static friction coefficient in step S422 depends on the following formula: in, To increase the new tangential force generated after grasping the weight of the object, This refers to the new clamping force applied by the system to stably grasp the object; The ratio of the tangential force after increasing the object's weight to the clamping force before increasing the object's weight is: The static friction coefficient error in step S423 is: The deviation of the clamping force is: in, This is the error in the static friction coefficient. For clamping force, It is a tangential force.

Images