A master-slave dual-arm force-position hybrid control method for tail fork bone deboning

Abstract

The present invention provides a master-slave dual-arm force-position hybrid control method for tail fork bone boning, including a dual robotic arm composed of a master arm and a slave arm and the following steps: establishing pose constraints and motion constraints between the tail fork bone and the master arm, and between the master arm and the slave arm, so as to form a closed kinematic chain between the dual robotic arm and the tail fork bone, and enabling the slave arm to follow the motion of the master arm; obtaining the position control equation of the robotic arm operation space according to the tail fork bone boning segmentation surface and the closed kinematic chain; obtaining the force control rate of the tail fork bone according to the force on the tail fork bone, and forming a force-position hybrid control law with the position control equation and the force control rate; performing constraint analysis on the tail fork bone meat-bone segmentation surface to determine the boning path and the boning direction at each point of the path; in the tail fork bone removal task, performing master-slave force-position hybrid control on the dual arms, with the master arm tightly holding the tail fork bone and the slave arm performing precise boning operations.

Description

Technical Field

[0001] The present invention relates to the field of robotic arm control, and particularly to a master-slave dual-arm force-position hybrid control method for removing the tail fork bone. Background Art

[0002] Pork is the main source of meat consumption in the Chinese market, accounting for about 60% to 65% of the total meat consumption. Fine segmentation in pork processing is an important link to ensure the quality and safety of meat products. During the pork segmentation process, the removal of the tail fork bone is a complex and delicate operation. Currently, the removal of the tail fork bone mainly relies on manual operation, and this method faces many challenges and deficiencies. The existing manual method for removing the tail fork bone usually involves workers using a boning straight knife. This process requires coordinating the operation of both hands simultaneously, resulting in a high labor intensity and low efficiency. It is prone to unstable meat-carrying rates, affecting the quality and sales of pork. In addition, manual operation is highly dangerous, and once an operation error occurs, it is easy to cause harm to workers. In the face of the continuous increase in labor costs and the shortage of the labor force, there is an urgent need for an automated solution to replace manual boning.

[0003] When using a single robotic arm for boning, it often leads to uneven force on the tail fork bone and position deviation. The original detected boning path cannot be used, and continuing to cut based on this will cause scratches that do not meet the boning requirements. When re-detecting the boning path and cutting again, the same problem will still be encountered. Therefore, it is necessary to use other equipment to fix and precisely adjust the pose of the tail fork bone, and this equipment needs to meet the requirements of not damaging the tail fork bone and not affecting the detection of the boning path of the tail fork bone by the vision sensor. Summary of the Invention

[0004] In response to the needs in the prior art, the present invention provides a master-slave dual-arm force-position hybrid control method for removing the tail fork bone, aiming to perform precise boning along the original detected path without damaging the meat of the tail fork bone through master-slave force-position hybrid control.

[0005] A master-slave dual-arm force-position hybrid control method for removing the tail fork bone includes a dual robotic arm composed of a master arm and a slave arm and the following steps:

[0006] Step 1: Establish pose constraints and motion constraints between the tail fork bone and the master arm, as well as between the master arm and the slave arm, so as to form a closed motion chain between the dual robotic arm and the tail fork bone, and make the slave arm follow the master arm in motion;

[0007] Step 2: Obtain the position control equation of the robotic arm operation space according to the boning segmentation surface of the tail fork bone and the closed motion chain;

[0008] Step 3: Obtain the force control rate of the tail fork bone according to the force on the tail fork bone, and form a force-position hybrid control law with the position control equation and the force control rate;

[0009] Step 4: Conduct a constraint analysis on the bone-separating surface of the tail fork bone to determine the bone-removing path and the bone-removing directions at each point on the path; in the task of removing the tail fork bone, the position control equation is used to control the main arm. The main arm always holds one side of the tail fork bone with a gripper and remains stationary. The force-position hybrid control law is used to control the slave arm, thereby achieving the master-slave type dual-arm force-position hybrid control.

[0010] Furthermore: Step 1 includes the following steps:

[0011] Step 1.1: Conduct pose constraints on the robotic arm.

[0012] In the dual-arm collaborative task, the base coordinate systems of the main arm and the slave arm are {L0} and {F0} respectively; the end coordinate systems of the two arms are {L} and {F} respectively; use {T} to represent the coordinate system of the tail fork bone, and the coordinate origin is the centroid of the tail fork bone. is the homogeneous transformation matrix of the tail fork bone coordinate system {T} relative to the end coordinate system {L} of the slave arm.

[0013] During the movement of the dual-arm coordinated operation, the main arm holds the tail fork bone. The end effector of the main arm is relatively stationary with the clamped tail fork bone. Therefore, the transformation matrix is a constant matrix. Decompose the transformation matrix into a position vector and a rotation matrix; use to represent the position vector of the centroid of the tail fork bone in the base coordinate system of the main arm, use to represent the rotation matrix of the tail fork bone coordinate system relative to the base coordinate system of the main arm, use to represent the position vector of the end coordinate system of the main arm in the base coordinate system of the main arm, use to represent the position vector of the centroid of the tail fork bone in the end coordinate system of the main arm, use to represent the rotation matrix of the end coordinate system of the main arm relative to the base coordinate system of the main arm, use to represent the rotation matrix of the tail fork bone coordinate system relative to the end coordinate system of the main arm; the pose constraints between the main arm and the tail fork bone are expressed as:

[0014]

[0015]

[0016] Similarly, the pose constraint relationship between the main arm and the slave arm is expressed as

[0017]

[0018]

[0019] Among them, and are the joint variables of the main arm and the slave arm respectively;

[0020] Step 1.2: Apply velocity constraints to the dual robotic arms;

[0021] Use to represent the absolute velocity vector of the centroid movement of the tail fork bone, use to represent the absolute angular velocity vector of the tail fork bone rotating around its own inertia axis, and use to represent the main arm joint velocity vector; compensate for the velocity difference between the end of the main arm and the tail fork bone. According to the velocity superposition principle of rigid body motion, the velocity constraint equation between the tail fork bone and the main arm is expressed as follows:

[0022]

[0023] Among them, ; refers to the position Jacobian matrix of the main arm; refers to the attitude Jacobian matrix of the main arm;

[0024] Define the Jacobian matrix . After the main arm joint velocity is determined, take the derivative of both sides of Equation (1-3) with respect to time:

[0025]

[0026]

[0027] Among them, represents the position vector of the end of the slave arm in the coordinate system of the end of the main arm, is the position Jacobian matrix of the slave arm;

[0028] When the dual arms hold the tail fork bone and adjust the position of the tail fork bone, the ends of the main arm and the slave arm are relatively stationary, so the angular velocities of the ends of the main arm and the slave arm are equal. Use to represent the attitude Jacobian matrix of the slave arm, then the velocity constraint equation between the main arm and the slave arm is expressed as:

[0029]

[0030] Step 1.3: Add acceleration constraints;

[0031] In the velocity constraint equation (1-5) between the tail fork bone and the main arm, use to represent the absolute velocity vector of the tail fork bone movement, then , and take the derivative with respect to time to obtain the absolute acceleration of the tail fork bone ; Use to represent the absolute velocity vector of the main arm, and let represent the vector of the tail fork bone and the main arm under velocity constraint. Then the absolute acceleration of the tail fork bone is expressed as , and . Use to represent the variation relationship between the tail fork bone and the main arm under acceleration constraint. Therefore, the acceleration constraint equation of the tail fork bone and the end of the main arm is expressed as:

[0032]

[0033] Similarly, in the velocity constraint equation (1-8) of the main arm and the slave arm, use to represent the Jacobian matrix of the slave arm. The absolute velocity of the end of the slave arm is , and represents the velocity coupling relationship between the end of the main arm and the end of the slave arm. Then the velocity constraint equation of the end of the main arm and the end of the slave arm is abbreviated, and the corresponding acceleration constraint equation is obtained by taking the derivative with respect to time :

[0034]

[0035]

[0036] By combining equations (1-9) and (1-11), the constraint relationship between the acceleration of the end of the main arm and the acceleration of the tail fork bone is obtained, that is:

[0037]

[0038] The kinematic constraint equations for the coordinated operation of the dual-arm robot are equations (1-5), (1-8) and (1-11). When the above kinematic constraint equations are satisfied, the slave arm follows the main arm to move to achieve the coordinated movement of the dual-arm robot.

[0039] Furthermore, in step 2, in the boning task, position control is adopted in the directions where there is no force control for both the main arm and the slave arm; for boning the tail fork bone, the control of the dual-arm robot needs to be comprehensively considered. A closed loop is formed between the dual-arm robot and the tail fork bone, and the dual-arm closed-chain kinematics method is used for modeling. During the coordinated operation of the dual-arm collaborative robot, the desired motion trajectory of the end of the robotic arm is determined by the boning segmentation surface of the tail fork bone. The segmentation surface is used as a three-dimensional geometric model, and the path points along the segmentation surface are obtained through sampling; combined with the path planning method, a desired motion trajectory including velocity and acceleration constraints is generated, and the desired end velocity and attitude corresponding to each path point on the desired motion trajectory are determined; subsequently, the desired joint displacements and velocities and acceleration ;

[0040] Let \(q\) represent the current actual joint displacement, and use to represent the function of the pose mapping from the joint space to the operation space. Take the derivative of both sides of the kinematic equation formula of the robotic arm with respect to time \(t\), that is, to obtain the differential relationship between the operation space \(x\) and the joint space \(q\).

[0041]

[0042]

[0043]

[0044]

[0045] In the formula, refers to the generalized velocity of the end effector in the operation space, refers to the joint velocity in the joint space, refers to the Jacobian matrix of the robotic arm and is the partial derivative matrix of;

[0046] Use to represent the joint torque vector. The position control equation of the robotic arm joint space is expressed as

[0047]

[0048] In the formula, represents the generalized inertia mass matrix, represents the inertial force term, represents the centrifugal force and Coriolis force terms, represents the gravity term, and represent the proportional coefficient and differential coefficient in PD control;

[0049] Take the derivative of both sides of Equation (2-4) with respect to time to obtain the acceleration of the end operation space of the robotic arm , for find the kinematic inverse solution to obtain the joint acceleration of the robotic arm , substitute the result into Equation (2-5) to obtain the position control expression of the operation space of the robotic arm ,

[0050]

[0051]

[0052]

[0053] After obtaining the position control expression of the robotic arm's operating space, send this to the joint controller through the control system to control the movement of the robotic arm.

[0054] Furthermore, in step 3, use to represent the active force acting on the tail fork bone, to represent the force exerted by the boning table on the tail fork bone. Assume that the force exerted by the bottom of the tail fork bone on the boning table is , m represents the mass of the tail fork bone, represents the surface height at which the tail fork bone is placed stationary on the boning table, represents the actual surface height of the tail fork bone after being pressed by the tool. Through force analysis, the dynamic equation of the single-degree-of-freedom system is obtained:

[0055]

[0056]

[0057]

[0058] Among them, represents the stiffness coefficient of the deformation of the tail fork bone;

[0059] According to the measurement of the force sensor installed at the end of the slave arm, the surface height when stationary does not change. After taking the second derivative of both sides of Equation (3-2), an expression about is obtained. Combining it with Formula (3-3) gives:

[0060]

[0061]

[0062]

[0063]

[0064] The actual force and the output force expected by the system is expressed as a relationship including a proportionality coefficient and a differential coefficient for the force control law of the control parameter , thereby realizing the dynamic adjustment of the actual force gradually approaching the expected force; combining this force control law with Equation (3-7) to obtain a second-order system and determining a characteristic equation:

[0065]

[0066]

[0067]

[0068] wherein, represents the force error in the force control system, and , the proportionality coefficient and the differential coefficient are solved according to the given system damping ratio and the natural frequency values, , ;

[0069] there are errors in the measured values of , and after removing the noise, it represents the output force expected by the system , and the force control law is expressed as:

[0070]

[0071] After obtaining the force control law , sum it with the position control expression obtained previously to obtain the force-position hybrid control law.

[0072] Furthermore: Step 4 includes the following steps:

[0073] Step 4.1: In the double-arm tail fork bone removal task, the main arm adopts position control, always holds one side of the tail fork bone with the gripper, keeps this position unchanged, and the slave arm is force-position hybrid control to establish the constraint coordinate system for the tail fork bone removal task;

[0074] wherein, for a given three-dimensional segmentation plane, project it onto a two-dimensional space to determine a two-dimensional curve, and for the bone removal direction at each point x on this two-dimensional segmentation line, at the point For example, the x-axis is the tangent direction of the point along the horizontal direction of the segmentation plane, and the z-axis is the tangent direction of the point along the vertical direction of the segmentation plane, thereby determining the y-axis direction; during the double-arm boning process, boning is performed along the resultant force direction of the x-axis and the negative z-axis of the arm. The boning knife is regarded as a planar tool, with the center of mass of the tool as the center point, and the tool adjusts the angle along the x-axis and z-axis directions of each boning point. Therefore, , , use to represent the moving speed of the tool along the x-axis direction, and use to represent the deflection angle of the tool along the x-axis direction. Similarly, use to represent the moving speed of the tool along the z-axis direction, and use to represent the deflection angle of the tool along the z-axis direction. Therefore, the artificial constraints , , , ;

[0075] Use and to represent the force along the x-axis direction and the force along the y-axis direction at the end of the from-arm during the boning process, respectively. Then, the constraints , ;

[0076] The constraint coordinate system converts the natural constraints and artificial constraints in the boning task into input parameters for the manipulator control. In the constraint coordinate system, the artificial constraints , , , are substituted into the kinematic constraint equations (1-5) and (1-8) to calculate the desired joint velocities of the from-arm; the desired joint accelerations are solved through the acceleration constraint equation (1-11) (, which is used to plan the motion trajectory of the end of the manipulator); meanwhile, the natural constraints , are substituted into the force control law (3-11), combined with the actual measured force and the desired force ; finally, the position control expression (2-8) converts these joint accelerations and external forces into the driving torques of the manipulator joints;

[0077] Step 4.2: Combine the force-position control law to achieve the hybrid control of position, force, and torque;

[0078] Among them, when the end of the from-arm comes into contact with the tail fork bone, it is necessary to control the position, force, and torque of the manipulator simultaneously; the operation space of the manipulator is decomposed into a position control space and a force control space, and the division of the operation space is determined by the selection matrix S;

[0079] The selection matrix S is a diagonal matrix. The elements on the diagonal of S are 0 or 1, and their selection determines the forces and torques at each point on the bone and meat segmentation trajectory of the tail fork bone. The identity matrix is represented by the matrix I, and the matrix . If a certain element of the selection matrix S is 1, then the corresponding element in is 0. In this way, the position control direction and the force control direction are respectively selected in the space coordinate system to ensure that the two do not act in the same direction at the same time, so as to divide the constrained coordinate system space into two mutually orthogonal position control sub-spaces and force control sub-spaces;

[0080] The selection matrix determines the control directions of the force and the position in real time, and then the position and force in the manipulator joint space are transformed to the constrained coordinate system through coordinate transformation. Then, the position control expression and the force control law are designed in the position control space and the force control space respectively, and then through the inverse coordinate transformation, the position and force variables in the operation space are mapped to the joint space variables and input into the joint controller to drive the joint movement, so as to realize the position control of the tool tip and the control of the contact force with the tail fork bone;

[0081] The force-position hybrid control consists of two relatively independent control servo loops: the position control part and the force control part. In the position control part, the expected movement trajectory of the slave-arm tool is solved according to the inverse kinematics of the manipulator to obtain the movement displacement in the joint space and sent to the PD controllers of the positions of each joint, and the position loop control torque is output to drive the manipulator to move. In the force control part, the expected end force and torque are transformed into joint space torques through the force Jacobian and sent to the joint torque PD controller, and the force loop control torque is output. Thus, the driving torques obtained from the position control space control expression and the force control space control law are superimposed together to obtain the total driving torque and sent to the joint drivers of each joint of the manipulator to realize the force-position hybrid control of the manipulator;

[0082] For the compliance control mode of the slave arm, impedance control is adopted, and the force feedback received by the six-dimensional force sensor at the end of the manipulator in contact with the tail fork bone and the response of the manipulator are respectively equivalent to admittance and impedance. By adjusting the mass coefficient , the damping coefficient and the stiffness coefficient to change the relationship between the end pose and the end acting force and torque; The impedance control mathematical model is adopted:

[0083]

[0084] In the formula, X is the actual Cartesian position in the manipulator working space, is the desired position of the robotic arm; is the desired contact force of the robotic arm, is the actual contact force between the robotic arm and the tail fork bone, is the actual contact force and the desired contact force deviation;

[0085] In the constraint coordinate system, from Equations (2-8) and (3-11), by introducing the mapping matrices and , which respectively represent the conversion of position control and force control from the joint space to the operation space, the expression of the force-position hybrid control of the robotic arm in the position control space and the control law in the force control space are expressed as:

[0086]

[0087]

[0088] Let R represent the rotation transformation matrix from the inertial coordinate system to the constraint coordinate system, , using the impedance control mathematical model formula (5-1) and the robot joint acceleration formula (2-6) represented by the Jacobian matrix, the control torque is obtained and substituted into formulas (5-2) and (5-3) to obtain the following equations:

[0089]

[0090] After obtaining the force-position hybrid control law , the force-position hybrid control of the slave arm is realized, and this control law is combined with the master arm control law to obtain the master-slave force-position hybrid control law;

[0091] Step 4.3: Perform master-slave dual-arm force-position hybrid control on the dual robotic arms. The master arm tightly holds the tail fork bone, and the slave arm moves to debone

[0092] The dual-arm robot uses the master-slave control method. The left arm is the master arm and uses position control, while the right arm is the slave arm and uses force-position hybrid control; when the master arm performs position control, the gripper tightly holds the tail fork bone and remains stationary. Removing the selection matrix S from formula (5-2) gives the expression of the master arm end position control law , in each motion cycle, the slave arm always obtains the pose of the master arm at each moment in real time from the control system. According to the acceleration constraint equation (1-11) of the master arm and the slave arm, the following acceleration of the slave arm end is calculated , let n represent a 7x1 order matrix, which is determined through step 1.3 , thus, the expression of the slave arm end position control law is obtained :

[0093]

[0094]

[0095]

[0096] According to formula (5-3), the force control law expression at the end of the slave arm is obtained:

[0097]

[0098] According to the above-obtained control law expression, the control laws of the master arm and the slave arm are superimposed to obtain the dual-arm force-position hybrid control law expression :

[0099]

[0100] By installing six-axis force sensors at the ends of the dual arms, the interaction forces between the dual arms and the tail fork bone are collected in real time, and the state information of each joint is obtained synchronously; the force and position data are input into the dual-arm force-position hybrid control law equation to calculate the dual-arm force-position hybrid control law.

[0101] Furthermore: The dual-arm force-position hybrid control law is transmitted to the joint controller through the control system interface using the EtherCAT communication protocol; after receiving the dual-arm force-position hybrid control law, the joint controller converts it into corresponding joint torque commands to drive each joint to perform precise movements, thereby realizing the master-slave dual-arm tail fork bone boning control based on the force and position feedback information.

[0102] The beneficial effects of the present invention are as follows: First, by imposing kinematic constraints on the dual arms, the postures, speeds, and accelerations during the collaborative operation of the dual arms are controlled; then, position control is performed on the dual arms to ensure stable and non-offset movement of the dual arms; secondly, force control is performed on the slave arm to perform boning by controlling the force; then, constraint control is performed on the collaborative tail fork bone boning of the dual arms to make the slave arm bone along the predetermined trajectory direction; then, force-position hybrid control is performed on the slave arm to determine the position, force, and torque of boning; finally, master-slave force-position hybrid control is performed on the dual arms, the master arm tightly holds the tail fork bone, and the slave arm performs precise boning operations. Brief Description of the Drawings

[0103] Figure 1 is the flowchart of the present invention;

[0104] Figure 2 is the force diagram of a single-degree-of-freedom system;

[0105] Figure 3It is a diagram of double-arm coordinated boning;

[0106] Figure 4 It is the algorithm flowchart in the present invention;

[0107] Figure 5 It is the schematic diagram of the control algorithm in the present invention. Specific implementation manners

[0108] The present invention will be described in detail below with reference to the accompanying drawings. The embodiments of the present invention will be described in detail below. The examples of the embodiments are shown in the accompanying drawings, in which the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below by referring to the accompanying drawings are exemplary and are only used to explain the present invention and should not be construed as limiting the present invention. The terms of orientation such as left, middle, right, up, and down in the embodiments of the present invention are only relative concepts to each other or are referenced based on the normal use state of the product, and should not be considered restrictive.

[0109] A master-slave double-arm force-position hybrid control method for tail fork bone boning, in combination with Figure 1 , Figure 3 , Figure 4 and Figure 5 as shown, includes a double robotic arm composed of a master arm and a slave arm and the following steps:

[0110] Step 1: Establish the pose constraints and motion constraints between the tail fork bone and the master arm, and between the master arm and the slave arm, so as to form a closed motion chain between the double robotic arm and the tail fork bone, and make the slave arm follow the master arm in motion;

[0111] Step 1.1: Perform pose constraints on the robotic arm so that the master arm can stably move to the position of clamping the tail fork bone without deviation;

[0112] In order to make the master arm stably move to the position of clamping the tail fork bone without deviation, it is necessary to perform pose constraints on the robotic arm; in the double-arm cooperation task, the base coordinate systems of the master arm and the slave arm are {L0} and {F0} respectively; the end coordinate systems of the two arms are {L} and {F} respectively; use {T} to represent the tail fork bone coordinate system, and the coordinate origin is the centroid of the tail fork bone, is the homogeneous transformation matrix of the tail fork bone coordinate system {T} relative to the end coordinate system {L} of the slave arm;

[0113] During the motion process of the double-arm coordinated operation, the master arm is used to clamp the tail fork bone, and the end effector of the master arm is relatively stationary with the clamped tail fork bone. Therefore, the transformation matrix is a constant matrix. Decompose the transformation matrix into a position vector and a rotation matrix; use to represent the position vector of the centroid of the tail fork bone in the base coordinate system of the master arm, and use The rotation matrix representing the tail fork bone coordinate system relative to the main arm base coordinate system is denoted by The position vector of the end of the main arm coordinate system in the main arm base coordinate system is denoted by The position vector of the centroid of the tail fork bone in the end coordinate system of the main arm is denoted by The rotation matrix representing the end coordinate system of the main arm relative to the main arm base coordinate system is denoted by The rotation matrix representing the tail fork bone coordinate system relative to the end coordinate system of the main arm; the pose constraint between the main arm and the tail fork bone is expressed as:

[0114]

[0115]

[0116] Similarly, the pose constraint relationship between the main arm and the slave arm is expressed as

[0117]

[0118]

[0119] Wherein, and are the joint variables of the main arm and the slave arm respectively;

[0120] Step 1.2: Apply velocity constraints to the dual robotic arms to avoid damage to the tail fork bone meat caused by vibration or instability due to excessive movement speed of the robotic arms;

[0121] First, analyze the velocity constraint relationship between the tail fork bone and the main arm; during the boning process, there is no relative movement between the end of the main arm and the tail fork bone, so they have the same angular velocity; use to represent the absolute velocity vector of the centroid movement of the tail fork bone, use to represent the absolute angular velocity vector of the tail fork bone rotating around its own inertia axis, use to represent the main arm joint velocity vector; compensate for the velocity difference between the end of the main arm and the tail fork bone, and the velocity constraint equation between the tail fork bone and the main arm is expressed by the following formula

[0122]

[0123] Wherein, ; refers to the position Jacobian matrix of the main arm; refers to the attitude Jacobian matrix of the main arm;

[0124] The movement trajectory of the tail crosspiece determines its speed and attitude changes in the operating space. By using the Jacobian matrix to map the speed in the operating space to the joint space and combining the speed superposition principle, the speed of the main arm joint can uniquely meet the motion requirements of the tail crosspiece; define the Jacobian matrix , after determining the speed of the main arm joint, taking the derivative of both sides of Equation (1-3) with respect to time gives:

[0125]

[0126]

[0127] wherein, represents the position vector of the end of the slave arm in the coordinate system of the end of the main arm, is the position Jacobian matrix of the slave arm;

[0128] When the two arms hold the tail crosspiece and adjust the position of the tail crosspiece, the ends of the main arm and the slave arm are relatively stationary, so the angular velocities of the ends of the main arm and the slave arm are equal. Using to represent the attitude Jacobian matrix of the slave arm, the speed constraint equation of the main arm and the slave arm is expressed as:

[0129]

[0130] Step 1.3: By adding acceleration constraints, avoid the manipulator from generating excessive inertial forces or mechanical shock damage to the meat of the tail crosspiece;

[0131] In the speed constraint equation (1-5) of the tail crosspiece and the main arm, using to represent the absolute speed vector of the movement of the tail crosspiece, then , taking the derivative with respect to time gives the absolute acceleration of the tail crosspiece; using to represent the absolute speed vector of the main arm, and letting represent the vector of the tail crosspiece and the main arm under speed constraints, then the absolute acceleration of the tail crosspiece is expressed as , and , using to represent the change relationship between the tail crosspiece and the main arm under acceleration constraints; the absolute acceleration of the tail crosspiece is the comprehensive result of the absolute acceleration

[0132]

[0133] Similarly, in the speed constraint equation (1-8) of the main arm and the slave arm, using The Jacobian matrix of the slave arm, and the absolute velocity of the end of the slave arm is , and represents the velocity coupling relationship between the end of the master arm and the end of the slave arm. Then, the velocity constraint equation of the end of the master arm and the end of the slave arm is abbreviated, and the corresponding acceleration constraint equation is obtained by taking the derivative with respect to time :

[0134]

[0135]

[0136] By combining equations (1-9) and (1-11), the constraint relationship between the acceleration of the end of the master arm and the acceleration of the tail fork bone is obtained, that is:

[0137]

[0138] The kinematic constraint equations for the coordinated operation of the dual-arm robot are equations (1-5), (1-8) and (1-11). When the above kinematic constraint equations are satisfied, the slave arm follows the master arm to move to achieve the coordinated movement of the dual-arm robot;

[0139] Step 2: Obtain the position control equation of the manipulator operation space according to the deboning segmentation surface of the tail fork bone and the closed kinematic chain;

[0140] Among them, in the deboning task, position control is adopted in the directions where there is no force control for both the master arm and the slave arm; for deboning the tail fork bone, the control of the dual-arm robot needs to be comprehensively considered, and a closed loop is formed between the dual-arm robot and the tail fork bone, and the dual-arm closed-chain kinematics method is used for modeling. During the coordinated operation of the dual-arm collaborative robot, the desired motion trajectory of the end of the manipulator is determined by the deboning segmentation surface of the tail fork bone. The segmentation surface is used as a three-dimensional geometric model, and the path points along the segmentation surface are obtained by sampling; combined with the path planning method, a desired motion trajectory including velocity and acceleration constraints is generated, and the desired end velocity and attitude corresponding to each path point on the desired motion trajectory are determined; subsequently, the desired joint displacements , velocity and acceleration ;

[0141] Let q represent the current actual joint displacement, and use to represent the function of the pose mapping from the joint space to the operation space. Take the derivative of both sides of the kinematic equation formula of the manipulator with respect to time t, that is, the differential relationship between the operation space x and the joint space q is obtained,

[0142]

[0143]

[0144]

[0145]

[0146] wherein, refers to the generalized velocity of the end effector in the operating space, refers to the joint velocity in the joint space, refers to the Jacobian matrix of the robotic arm, and is the partial derivative matrix of;

[0147] Use to represent the joint torque vector, and the position control equation of the robotic arm joint space is expressed as

[0148]

[0149] wherein, represents the generalized inertia mass matrix, represents the inertial force term, represents the centrifugal force and Coriolis force terms, represents the gravity term, and represent the proportional coefficient and differential coefficient in PD control;

[0150] Taking the derivative of both sides of Equation (2-4) with respect to time gives the acceleration of the end operating space of the robotic arm , for solving the kinematic inverse solution to obtain the joint acceleration of the robotic arm , substituting the result into Equation (2-5) to obtain the position control expression of the robotic arm operating space ,

[0151]

[0152]

[0153]

[0154] After obtaining the position control expression of the robotic arm operating space, send this to the joint controller through the control system to control the movement of the robotic arm; in addition It is also used to sum with the force control law subsequently to obtain the force-position hybrid control law;

[0155] Step 3: Obtain the force control rate of the tail fork bone according to the force on the tail fork bone, and form the force-position hybrid control law by combining the position control equation and the force control rate;

[0156] In the force control mode of the slave arm force-position hybrid control, the force system of the tail fork bone is regarded as a single-degree-of-freedom system for research. Combining Figure 2 As shown, use to represent the active force acting on the tail fork bone, to represent the force of the boning table on the tail fork bone. Assume that the force of the bottom of the tail fork bone on the boning table is , m represents the mass of the tail fork bone, represents the surface height where the tail fork bone is placed stationary on the boning table, represents the actual surface height of the tail fork bone after being pressed by the tool. Through force analysis, the dynamic equation of the single-degree-of-freedom system is obtained:

[0157]

[0158]

[0159]

[0160] Among them, represents the stiffness coefficient of the deformation of the tail fork bone;

[0161] According to the measurement of the force sensor installed at the end of the slave arm, the surface height when stationary will not change. After taking the second-order derivative of both sides of Equation (3-2), an equation about is obtained. Combining with Formula (3-3), we have:

[0162]

[0163]

[0164]

[0165]

[0166] Express the relationship between the actual force of the tail fork bone on the boning table and the desired output force of the system as including the proportional coefficient and the differential coefficient Force Control Law of Control Parameters , so as to achieve dynamic adjustment of the actual force gradually approaching the expected force; the force control law Combined with formula (3-7), we get a second-order system and determine a characteristic equation:

[0167]

[0168]

[0169]

[0170] in, represents the force error in the force control system, and , proportionality coefficient and the differential coefficient According to the given system damping ratio and natural frequency To solve the value of , ;

[0171] There is an error in the measured value. After removing the noise, it represents the expected output force of the system , the force control law is expressed as:

[0172]

[0173] The force control law is obtained After that, the position control expression obtained previously is Sum them up to obtain the force-position hybrid control law; send the obtained force-position hybrid control law to the slave arm joint controller to realize the force-position hybrid control;

[0174] Step 4: Constraint analysis is performed on the forkbone bone-meat segmentation surface to determine the deboning path and the deboning direction of each point on the path; in the forkbone removal task, the position control equation is used to control the master arm, the master arm always clamps one side of the forkbone with a clamp and keeps it still, and the force-position hybrid control law is used to control the slave arm, thereby realizing the master-slave double-arm force-position hybrid control; specifically, the following steps are included:

[0175] Step 4.1: In the two-arm forkbone removal task, the main arm adopts position control, and always holds one side of the forkbone with the gripper to keep the position fixed. The slave arm adopts force-position mixed control to establish the constraint coordinate system of the forkbone removal task;

[0176] Among them, for a given three-dimensional cutting plane, projecting it onto a two-dimensional space determines a two-dimensional curve. For the boning direction at each point x on this two-dimensional cutting line, taking the point as an example, the x-axis is the tangent direction of this point and the cutting plane along the horizontal direction, and the z-axis is the tangent direction of this point along the vertical direction and the cutting plane, thereby determining the y-axis direction; during the double-arm boning process, boning is carried out along the resultant force direction of the x-axis and the negative z-axis of the arm. The boning knife is regarded as a planar tool, with the center of mass of the tool as the center point. The tool adjusts the angle along the x-axis and z-axis directions of each boning point. Therefore, , , using to represent the moving speed of the tool along the x-axis direction, and using to represent the deflection angle of the tool along the x-axis direction. Similarly, using to represent the moving speed of the tool along the z-axis direction, and using to represent the deflection angle of the tool along the z-axis direction. Therefore, the artificial constraints , , , ;

[0177] To ensure that the double-arm operation can complete the boning task, a certain force needs to be generated at the end of the slave arm to overcome the resistance of the meat and ensure that the tool moves along the target force direction for precise boning; using and to represent the force along the x-axis direction and the force along the y-axis direction at the end of the slave arm during the boning process respectively, then the constraints , ;

[0178] The constraint coordinate system converts the natural constraints and artificial constraints in the boning task into input parameters for the manipulator control. In the constraint coordinate system, substituting the artificial constraints , , , into the kinematic constraint equations (1-5) and (1-8), the expected joint velocity of the slave arm is calculated; the expected joint acceleration is solved through the acceleration constraint equation (1-11) for planning the motion trajectory of the end of the manipulator; at the same time, substituting the natural constraints , into the force control law (3-11), combining the actual measured force and the expected force , the output force at the end of the slave arm is adjusted in real time; finally, the position control expression (2-8) converts these joint accelerations and external acting forces into the driving torques of the manipulator joints to ensure that the manipulator can apply force accurately along the boning path and maintain stability, thereby meeting the dynamic requirements of the boning task;

[0179] Step 4.2: Combine the force-position control law to achieve the hybrid control of position, force, and torque, and use it for the slave arm to perform precise bone removal along the bone removal path;

[0180] Among them, when the end of the slave arm comes into contact with the tail fork bone, it is necessary to control the position, force, and torque of the robotic arm simultaneously; decompose the operation space of the robotic arm into a position control space and a force control space, and the division of the operation space is determined by the selection matrix S;

[0181] The selection matrix S is a diagonal matrix, and the elements on the diagonal of S are 0 or 1. Its selection determines the force and torque at each point on the bone and meat segmentation trajectory of the tail fork bone; use the matrix I to represent the identity matrix, and the matrix , if an element of the selection matrix S is 1, then the corresponding element in is 0. In this way, the position control direction and the force control direction are selected in the space coordinate system respectively to ensure that the two do not act in the same direction at the same time, so as to divide the constraint coordinate system space into two mutually orthogonal position control subspaces and force control subspaces;

[0182] The selection matrix determines the control directions of force and position in real time, and then transforms the position and force in the joint space of the robotic arm to the constraint coordinate system. Then, design the position control expression and the force control law in the position control space and the force control space respectively, and then map the position and force variables in the operation space to the joint space variables through the inverse coordinate transformation and input them into the joint controller to drive the joint movement, so as to achieve the position control of the tool tip and the control of the contact force with the tail fork bone;

[0183] The force-position hybrid control consists of two relatively independent control servo loops: the position control part and the force control part; in the position control part, solve the motion displacement in the joint space according to the inverse kinematics of the robotic arm for the expected tool motion trajectory of the slave arm and send it to the PD controllers of the positions of each joint, and output the control torque of the position loop , to drive the robotic arm to move; in the force control part, transform the expected end force and torque into the joint space torque through the force Jacobian and send it to the PD controller of the joint torque, and output the control torque of the force loop , thus adding the control torque obtained from the position control space expression and the control law obtained from the force control space together to obtain the total driving torque , and send it to the joint drivers of each joint of the robotic arm to achieve the force-position hybrid control of the robotic arm;

[0184] The bone and meat density of different parts of the tail fork bone is uneven, and there may be a problem of stuck bones that was not detected in advance. The position control cannot cross the stuck bone area, so force-position hybrid control is adopted from the slave arm; the impedance control is used for the compliant control method of the slave arm, and the force feedback received by the six-axis force sensor at the end of the robotic arm in contact with the tail fork bone and the response of the robotic arm are respectively equivalent to admittance and impedance; by adjusting the quality coefficient , damping coefficient and stiffness coefficient to change the relationship between the end pose and the end acting force and moment; the impedance control mathematical model is adopted:

[0185]

[0186] In the formula, X is the actual Cartesian position of the robotic arm's workspace, is the desired position of the robotic arm; is the desired contact force of the robotic arm, is the actual contact force between the robotic arm and the tail fork bone, is the actual contact force and the desired contact force deviation;

[0187] In the constraint coordinate system, from Equation (2-8) and Equation (3-11), by introducing the mapping matrices and , which respectively represent the conversion from the joint space to the operation space for position control and force control, the expression of the robotic arm's force-position hybrid control in the position control space and the control law in the force control space are expressed as:

[0188]

[0189]

[0190] Let R represent the rotation transformation matrix from the inertial coordinate system to the constraint coordinate system, , using the impedance control mathematical model formula (5-1) and the robotic arm joint acceleration formula (2-6) represented by the Jacobian matrix, the control torque is obtained and substituted into formula (5-2) and formula (5-3) to obtain the following equations:

[0191]

[0192] After obtaining the force-position hybrid control law , the force-position hybrid control of the slave arm is realized, and this control law is combined with the master arm control law to obtain the master-slave force-position hybrid control law;

[0193] Step 4.3: Perform master-slave dual-arm force-position hybrid control on the dual robotic arms. The master arm tightly holds the tail fork bone, and the slave arm moves to debone

[0194] The dual-arm robot uses a master-slave control method. The left arm is the master arm and uses position control, while the right arm is the slave arm and uses force-position hybrid control. When the master arm performs position control, the gripper tightly holds the tail fork bone and remains stationary, without the need to control the direction of force and without the need to select a matrix. Remove the selection matrix S from formula (5-2) to obtain the expression of the master arm end position control law In each motion cycle, the slave arm obtains the pose of the master arm at each moment from the control system in real time. According to the acceleration constraint equation (1-11) of the master arm and the slave arm, calculate the following acceleration of the slave arm end Let n represent a 7x1 order matrix, which is determined through Step 1.3 Thus, obtain the expression of the slave arm end position control law :

[0195]

[0196]

[0197]

[0198] According to formula (5-3), obtain the expression of the force control law at the slave arm end:

[0199]

[0200] According to the above-obtained control law expressions, superimpose the control laws of the master arm and the slave arm to obtain the expression of the dual-arm force-position hybrid control law :

[0201]

[0202] By installing six-axis force sensors at the ends of the dual arms, the interaction forces between the dual arms and the tail fork bone are collected in real time, and the state information of each joint is obtained synchronously. Input the force and position data into the dual-arm force-position hybrid control law equation to calculate the dual-arm force-position hybrid control law. The dual-arm force-position hybrid control law is transmitted to the joint controller through the control system interface using the EtherCAT communication protocol. After receiving the dual-arm force-position hybrid control law, the joint controller converts it into the corresponding joint torque command to drive each joint to perform precise motion, thereby realizing the master-slave dual-arm tail fork bone deboning control based on the force and position feedback information.

[0203] The basic principles, main features and advantages of the present invention have been shown and described above. Those skilled in the art should understand that the present invention is not limited by the above embodiments. What is described in the above embodiments and the specification only illustrates the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention will have various changes and improvements, and these changes and improvements all fall within the scope of the present invention claimed. The scope of the present invention claimed is defined by the appended claims and their equivalents.

Claims

1. A master-slave double-arm force-position hybrid control method for coccyx wishbone deboning, characterized in that: It includes a dual robotic arm consisting of a master arm and a slave arm and the following steps: Step 1: Establish the posture constraints and motion constraints between the tail wishbone and the main arm, and between the main arm and the slave arm, so that a closed kinematic chain is formed between the dual manipulators and the tail wishbone, and the slave arm follows the movement of the main arm; Step 2: According to the forkbone deboning segmentation surface and the closed kinematic chain, the position control equation of the robot arm operation space is obtained; In the deboning task, position control is used in the directions where the master arm and the slave arm have no force control. To debony the forkbone, it is necessary to comprehensively consider the control of the dual robotic arms, form a closed loop with the dual robotic arms and the forkbone, and use the dual-arm closed-chain kinematics method to model it. During the coordinated operation of the dual-arm collaborative robot, the expected motion trajectory of the end of the robotic arm is determined by the deboning segmentation surface of the forkbone. The segmentation surface is used as a three-dimensional geometric model, and the path points along the segmentation surface are obtained by sampling. Combined with the path planning method, the expected motion trajectory containing velocity and acceleration constraints is generated, and the expected velocity and posture of the end corresponding to each path point on the expected motion trajectory are determined. Subsequently, the expected joint displacement q is determined by the kinematic constraint equation. d ,speed and acceleration Let q represent the current actual joint displacement, and f(q) represent the function of mapping the pose from the joint space to the operation space. Take the derivative of the time t on both sides of the kinematic equation of the robot arm, that is, get the differential relationship between the operation space x and the joint space q. x=f(q) (2-1) q=f -1 (x) (2-2) In the formula, refers to the generalized velocity of the end effector in the operating space, refers to the joint velocity in the joint space, J ij (q) refers to the Jacobian matrix of the manipulator and is the partial derivative moment of 6×n; Use τ q Represents the joint torque vector, the position control equation of the manipulator joint space τ q Expressed as Where M(q) represents the generalized inertial mass matrix, represents the inertia force term, represents the centrifugal force and Coriolis force, G(q) represents the gravity term, K p and K d Represents the proportional coefficient and differential coefficient in PD control; Taking the derivative of both sides of equation (2-4) with respect to time, we can get the acceleration of the end operating space of the robot arm: right Find the inverse kinematic solution to obtain the joint acceleration of the robotic arm Substituting the result into equation (2-5), we can get the position control expression τ of the robot operation space: d , After obtaining the position control expression of the robot arm operation space, the τ d The movement of the robot arm can be controlled by sending the control system to the joint controller; Step 3: Obtain the force control law of the coccyx according to the force on the coccyx, and form a force-position mixed control law by combining the position control equation and the force control law; use f to represent the main force acting on the coccyx, and f e represents the force exerted by the boning table on the forkbone, assuming that the force exerted by the bottom of the forkbone on the boning table is f t , m represents the mass of the coccygeal wishbone, x e represents the surface height of the coccyx wishbone when it is placed on the boning table, and x represents the actual surface height of the coccyx wishbone after being pressed by the tool. The dynamic equation of the single-degree-of-freedom system is obtained through force analysis: f e =k e (x e -x) (3-1) f t =-f e =k e (x-x e ) (3-2) Among them, k e represents the stiffness coefficient of the coccygeal deformation; f t The height x of the surface at rest is measured by the force sensor mounted at the end of the slave arm. e Will not change, after taking the second-order derivative of both sides of equation (3-2), we can get The formula of , combined with formula (3-3), is: The actual force exerted by the forkbone on the boning table is f t and the system's desired output force f d The relationship is expressed as including the proportionality coefficient k p and the differential coefficient k d The force control law f of the control parameter is used to achieve dynamic adjustment of the actual force gradually approaching the expected force; the force control law f is combined with formula (3-7) to obtain a second-order system and determine a characteristic equation: Where Δf represents the force error in the force control system, and Δf = f d -f t , proportionality coefficient k p and the differential coefficient k d According to the given system damping ratio ξ and natural frequency ω n To solve the value of f t There is an error in the measured value of f t After removing the noise, it represents the expected output force f of the system d , the force control law is expressed as: The force control law is obtained Then, the position control expression τ obtained previously is d Sum them and obtain the force-position mixed control law; Step 4: Perform constraint analysis on the forkbone bone-meat segmentation surface to determine the deboning path and the deboning direction at each point on the path; in the forkbone removal task, the position control equation is used to control the master arm, and the master arm always holds one side of the forkbone with a clamp and keeps it still. The force-position hybrid control law is used to control the slave arm, thereby realizing the master-slave double-arm force-position hybrid control.

2. The master-slave double-arm force-position hybrid control method for coccyx wishbone deboning according to claim 1, characterized in that: Step 1 includes the following steps: Step 1.1: Constrain the position and posture of the robot arm; In the dual-arm collaborative task, the base coordinate systems of the master arm and the slave arm are {L0} and {F0} respectively; the end coordinate systems of the two arms are {L} and {F} respectively; {T} represents the coccygeal coordinate system, and the origin of the coordinate is the center of mass of the coccygeal. is the homogeneous transformation matrix of the coccygeal coordinate system {T} relative to the arm end coordinate system {L}; In the process of coordinated operation of the two arms, the main arm is used to clamp the tail wishbone, and the end effector of the main arm and the clamped tail wishbone are relatively stationary, so the transformation matrix is a constant matrix, decomposing the transformation matrix into position vector and rotation matrix; using It represents the position vector of the center of mass of the fork bone in the main arm base coordinate system, and is expressed by The rotation matrix of the wishbone coordinate system relative to the main arm base coordinate system is expressed by It represents the position vector of the main arm end coordinate system in the main arm base coordinate system. The position vector of the center of mass of the fork bone in the main arm end coordinate system is represented by It represents the rotation matrix of the main arm end coordinate system relative to the main arm base coordinate system. represents the rotation matrix of the wishbone coordinate system relative to the main arm end coordinate system; the posture constraint of the main arm and the wishbone is expressed as: Similarly, the posture constraint relationship between the master arm and the slave arm is expressed as: Among them, q l and q f are the joint variables of the master arm and the slave arm respectively; Step 1.2: Set velocity constraints on the dual manipulators; use The absolute velocity vector of the center of mass of the fork bone is represented by ω o The absolute angular velocity vector of the coccyx around its own inertia axis is represented by Represents the main arm joint velocity vector; the velocity difference between the end of the main arm and the coccyx is compensated, and the velocity constraint equation of the coccyx and the main arm is expressed as follows based on the velocity superposition principle of rigid body motion: in, J ll (q l ) refers to the position Jacobian matrix of the main arm; J la (q l ) refers to the attitude Jacobian matrix of the main arm; Define the Jacobian matrix L(q l ), after the main arm joint speed is determined, the time derivative of both ends of equation (1-3) is: in, represents the position vector of the end of the slave arm in the coordinate system of the end of the master arm, J fl (q f ) is the Jacobian matrix of the slave arm position; When the fork bone is clamped by both arms and the position of the fork bone is adjusted, the end of the main arm and the end of the slave arm are relatively static, so the angular velocity of the end of the main arm and the end of the slave arm is equal. fa (q f ) represents the posture Jacobian matrix of the slave arm, then the velocity constraint equations of the master arm and the slave arm are expressed as: Step 1.3: Add acceleration constraints; In the velocity constraint equation (1-5) between the wishbone and the main arm, use represents the absolute velocity vector of the coccygeal movement, then Taking the derivative with respect to time, we get the absolute acceleration of the coccyx use represents the absolute velocity vector of the main arm, let represents the vector of the coccyx and the main arm under velocity constraint, then the absolute acceleration of the coccyx is expressed as and use It represents the changing relationship between the coccyx and the main arm under acceleration constraint, so the acceleration constraint equation between the coccyx and the end of the main arm is expressed as: Similarly, in the speed constraint equation (1-8) of the master arm and the slave arm, I f represents the Jacobian matrix of the slave arm, and the absolute velocity of the end of the slave arm is and Represents the velocity coupling relationship between the end of the main arm and the end of the slave arm, then the velocity constraint equation of the end of the main arm and the end of the slave arm is simplified, and then the corresponding acceleration constraint equation is derived with respect to time The constraint relationship between the acceleration of the main arm end and the acceleration of the tail wishbone is obtained by combining equations (1-9) and (1-11), namely: The kinematic constraint equations for the coordinated operation of dual robotic arms are equations (1-5), (1-8) and (1-11). When the above kinematic constraint equations are met, the slave arm follows the movement of the master arm to achieve coordinated movement of the dual-arm robot.

3. The master-slave double-arm force-position hybrid control method for coccyx wishbone deboning according to claim 2, characterized in that: Step 4 includes the following steps: Step 4.1: In the two-arm forkbone removal task, the main arm adopts position control, and always holds one side of the forkbone with the gripper to keep the position fixed. The slave arm adopts force-position mixed control to establish the constraint coordinate system of the forkbone removal task; Among them, for a given three-dimensional dividing surface, a two-dimensional curve is determined by projecting it into the two-dimensional space. For the deboning direction at each point x on the two-dimensional dividing line, taking point x0 as an example, the x-axis is the tangent direction of the point and the dividing surface in the horizontal direction, and the z-axis is the tangent direction of the point and the dividing surface in the vertical direction, thereby determining the y-axis direction; in the double-arm deboning process, deboning is performed from the direction of the combined force of the arm along the x-axis direction and the negative z-axis. The deboning knife is regarded as a plane tool. With the tool center of mass as the center point, the tool adjusts the angle along the x-axis and z-axis directions of each deboning point, so v y =0,ω y =0, use v x0 Indicates the moving speed of the tool along the x-axis, ω x0 Indicates the deflection angle of the tool along the x-axis. Similarly, v z0 Indicates the moving speed of the tool along the z-axis direction, ω z0 represents the deflection angle of the tool along the z-axis, so the artificial constraint v is obtained x =v x0 ,v z =v z0 ,ω x =ω x0 ,ω z =ω z0 ; Use F x0 and F z0 represent the force along the x-axis and the force along the y-axis from the end of the arm during the deboning process, respectively, and the constraint F is obtained x =F x0 , F z =F z0 ; The constraint coordinate system transforms the natural and artificial constraints in the deboning task into input parameters for the robot control. In the constraint coordinate system, the artificial constraints v x =v x0 ,v z =v z0 ,ω x =ω x0 ,ω z =ω z0 Substitute the kinematic constraint equations (1-5) and (1-8) to calculate the desired joint velocity of the slave arm: Solve the desired joint acceleration using the acceleration constraint equation (1-11) At the same time, the natural constraint F x =F x0 , F z =F z0 Substitute the force control law (3-11) into the actual measured force f t and the expected force f d ;Finally, the position control expression (2-8) converts these joint accelerations and external forces into the driving torque of the robot joint; Step 4.2: Combine the force-position hybrid control law to realize the hybrid control of position, force and torque; Among them, when the end of the slave arm contacts the coccyx, the position, force and torque of the robot arm need to be controlled simultaneously; the operation space of the robot arm is decomposed into position control space and force control space, and the division of the operation space is determined by selecting the matrix S; The selection matrix S is a 6×6 diagonal matrix, and the diagonal elements in S are either 0 or 1. Its selection determines the force and torque at each point on the bone-meat separation trajectory of the coccygeal furcula. The matrix I represents the unit matrix, and the matrix If an element of the matrix S is selected as 1, the corresponding element in IS is 0. In this way, the position control direction and the force control direction are selected in the spatial coordinate system respectively to ensure that the two do not act in the same direction at the same time, thereby dividing the constraint coordinate system space into two mutually orthogonal position control subspaces and force control subspaces; The matrix is ​​selected to determine the control direction of force and position in real time, and then the position and force in the joint space of the robot arm are transformed into the constraint coordinate system through coordinate transformation. Then, the position control expression and force control law are designed in the position control space and force control space respectively. Then, the position and force variables in the operation space are mapped to the joint space variables through inverse coordinate transformation and input into the joint controller to drive the joint movement, thereby realizing the position control of the tool tip and the control of the contact force acting on the tail wishbone. The force-position hybrid control consists of two relatively independent control servo loops: the position control part and the force control part. In the position control part, the desired motion trajectory of the arm tool is solved according to the inverse kinematics of the robot arm to obtain the motion displacement of the joint space and send it to the PD controller of each joint position, outputting the position loop control torque τ d , drives the robot to move; in the force control part, the desired end force and torque are converted into joint space torque through force Jacobian and sent to the joint torque PD controller, which outputs the force loop control torque Therefore, the position control space control expression τ d and force control space control law The obtained driving torque is added together to obtain the total driving torque τ m , and sent to the joint drivers of the robot arm to achieve force-position hybrid control of the robot arm; The compliant control method of the slave arm adopts impedance control, and the force feedback received by the six-dimensional force sensor at the end of the robot arm in contact with the tail wishbone and the response of the robot arm are equivalent to admittance and impedance respectively; by adjusting the mass coefficient M d , damping coefficient B d and stiffness coefficient K d The size of the end position is used to change the relationship between the end force and torque; the impedance control mathematical model is used: Where X is the actual Cartesian position of the robot workspace, X r is the desired position of the robot arm; F r is the desired contact force of the robot, F e is the actual contact force between the manipulator and the coccyx, E f is the actual contact force F e and the expected contact force F r Deviation; In the constrained coordinate system, according to equations (2-8) and (3-11), by introducing the mapping matrix S and They represent the transformation of position control and force control from joint space to operation space respectively. The expression of the manipulator force-position hybrid control in the position control space and the control law in the force control space are expressed as: Let R represent the rotation transformation matrix from the inertial coordinate system to the constraint coordinate system. Using the impedance control mathematical model formula (5-1) and the robot joint acceleration formula (2-6) expressed by the Jacobian matrix, the control torque is obtained. Substituting it into formula (5-2) and formula (5-3), the following equation is obtained: Obtain the force-position hybrid control law τ m Finally, the force-position hybrid control of the slave arm is realized, and the control law is combined with the control law of the master arm to obtain the master-slave force-position hybrid control law; Step 4.3: Perform force-position hybrid control of the dual robotic arms, with the master arm tightly holding the tail wishbone and the slave arm moving to remove the bone; The dual-arm robot uses a master-slave control method. The left arm is the master arm and uses position control, while the right arm is the slave arm and uses force-position mixed control. When the master arm is in position control, the gripper holds the tail bone tightly to keep it still. Remove the selection matrix S from formula (5-2) to obtain the expression of the master arm end position control law: In each motion cycle, the slave arm obtains the position and posture of the master arm at each moment in real time from the control system. According to the acceleration constraint equations (1-11) of the master arm and the slave arm, the following acceleration of the end of the slave arm is calculated: Let n represent the 7x1 matrix, determined by step 1.3 Thus, the control law expression of the arm end position is obtained: According to formula (5-3), the force control law expression of the slave arm end is obtained: According to the above solution to the required control law expression, the control law of the master arm and the slave arm are superimposed together to obtain the double-arm force-position mixed control law expression τ: By installing six-dimensional force sensors at the ends of both arms, the interaction force between the two arms and the coccyx is collected in real time, and the state information of each joint is obtained synchronously; the force and position data are input into the force-position mixed control law equation of both arms, and the force-position mixed control law of both arms is calculated.

4. The master-slave double-arm force-position hybrid control method for coccyx wishbone deboning according to claim 3, characterized in that: The double-arm force-position hybrid control law is transmitted to the joint controller through the control system interface using the EtherCAT communication protocol; after receiving the double-arm force-position hybrid control law, the joint controller converts it into corresponding joint torque instructions to drive each joint to perform precise movement, thereby realizing the master-slave double-arm tail wishbone deboning control based on force and position feedback information.

Images