Master-slave type double-arm force-position hybrid control method for deboning tail wishbone

Abstract

The invention provides a master-slave type double-arm force-position hybrid control method for deboning a tail wishbone, which comprises double mechanical arms consisting of a master arm and a slave arm. The method comprises the following steps of: establishing pose constraints and motion constraints between the tail wishbone and the master arm as well as between the master arm and the slave arm; therefore, a closed kinematic chain is formed between the double mechanical arms and the tail wishbone, and the slave arm moves along with the master arm; according to the tail wishbone deboning segmentation plane and the closed kinematic chain, a position control equation of a mechanical arm operation space is obtained; the force control rate of the tail wishbone is obtained according to the stress of the tail wishbone, and the position control equation and the force control rate form a force-position mixed control law; carrying out constraint analysis on a tail wishbone meat segmentation plane, and determining a deboning path and deboning directions of all points of the path; in the task of removing the tail wishbone, the two arms are subjected to master-slave force position mixed control, the master arm tightly holds the tail wishbone, and the slave arm carries out accurate deboning operation.

Description

Technical Field

[0001] The invention relates to the field of mechanical arm control, and in particular to a master-slave double-arm force-position hybrid control method for coccyx wishbone deboning. Background Art

[0002] Pork is the main source of meat consumption in the Chinese market, accounting for about 60% to 65% of total meat consumption. Fine segmentation in pork processing is an important link in ensuring the quality and safety of meat products. In the pork segmentation process, the removal of the forkbone is a complex and delicate operation. At present, the removal of the forkbone mainly relies on manual operation, which faces many challenges and shortcomings. The existing manual forkbone removal method is usually operated by workers using a boning straight knife. This process requires the coordination of both hands at the same time, which is labor-intensive and inefficient. It is easy to have an unstable meat rate, which affects the quality and sales of pork. In addition, manual operation is highly dangerous, and once the operation is wrong, it is easy to cause harm to the workers. Faced with the problem of rising labor costs and labor shortages, there is an urgent need for an automated solution that can replace manual deboning.

[0003] The existing use of a single robotic arm for deboning often results in uneven force on the forkbone and positional displacement. The original detection deboning path cannot be used, and continued cutting on this basis will cause scratches that do not meet the deboning requirements. The deboning path must be re-detected and the same problem will occur again when cutting again. Therefore, other equipment is needed to fix the forkbone and accurately adjust its position. The equipment needs to meet the requirements of not damaging the forkbone and not affecting the visual sensor's detection of the deboning path of the forkbone. Summary of the invention

[0004] In response to the needs of the prior art, the present invention provides a master-slave double-arm force-position hybrid control method for coccyx forkbone deboning, the purpose of which is to accurately debone the coccyx forkbone according to the original detection path without damaging the coccyx forkbone meat through master-slave force-position hybrid control.

[0005] A master-slave dual-arm force-position hybrid control method for coccyx wishbone deboning comprises a dual mechanical arm consisting of a master arm and a slave arm and the following steps:

[0006] 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;

[0007] 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;

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

[0009] 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.

[0010] Further, step 1 comprises the following steps:

[0011] Step 1.1: Constrain the position and posture of the robot arm;

[0012] 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};

[0013] 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:

[0014]

[0015] Similarly, the posture constraint relationship between the master arm and the slave arm is expressed as:

[0016]

[0017] in, and are the joint variables of the master arm and the slave arm respectively;

[0018] Step 1.2: Set velocity constraints on the dual manipulators;

[0019] use The absolute velocity vector of the center of mass of the fork bone is represented by 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:

[0020] in, ; refers to the position Jacobian matrix of the main arm; refers to the attitude Jacobian matrix of the main arm;

[0021] Define the Jacobian matrix After the main arm joint speed is determined, the time derivative of both ends of equation (1-3) is:

[0022]

[0023] in, Represents the position vector of the end of the slave arm in the coordinate system of the end of the master arm, is the Jacobian matrix of the slave arm position;

[0024] 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. 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:

[0025] Step 1.3: Add acceleration constraints;

[0026] 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 , the absolute acceleration of the coccyx is obtained by differentiating it with respect to time ;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:

[0027] Similarly, in the speed constraint equation (1-8) of the master arm and the slave arm, 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 :

[0028]

[0029] 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:

[0030] 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.

[0031] Further: In step 2, in the deboning task, position control is adopted in the directions where the master arm and the slave arm have no force control; to debone the coccyx, it is necessary to comprehensively consider the control of the dual robotic arms, form a closed loop with the dual robotic arms and the coccyx, and adopt a dual-arm closed-chain kinematic method for modeling; 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 coccyx deboning segmentation surface, and 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 including speed and acceleration constraints is generated, and the expected speed and posture of the end corresponding to each path point on the expected motion trajectory are determined; then, the expected joint displacement is determined by the kinematic constraint equation ,speed and acceleration ;

[0032] Use q to represent the current actual joint displacement, and use The function representing the pose mapping from the joint space to the operation space is used to obtain the derivative of the time t on both sides of the kinematic equation of the manipulator, that is, to obtain the differential relationship between the operation space x and the joint space q.

[0033]

[0034]

[0035]

[0036] In the formula, refers to the generalized velocity of the end effector in the operating space, refers to the joint velocity in joint space, refers to the Jacobian matrix of the robot arm and is The partial derivative matrix of ; use Represents the joint torque vector, the position control equation of the manipulator joint space Expressed as

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

[0038] 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 , the result is put into equation (2-5) to get the position control expression of the robot operation space ,

[0039]

[0040]

[0041] After obtaining the position control expression of the robot arm operation space, The movement of the robotic arm can be controlled by sending signals to the joint controller through the control system.

[0042] Further: In step 3, use represents the main force acting on the coccygeal furcula, 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 , m represents the mass of the coccygeal wishbone, The height of the wishbone on the boning table. It represents the actual height of the surface of the wishbone after being pressed by the tool. The dynamic equation of the single degree of freedom system is obtained through force analysis:

[0043]

[0044]

[0045] in, represents the stiffness coefficient of the coccygeal deformation;

[0046] The height of the surface at rest, as measured by the force sensor mounted on the end of the slave arm 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:

[0047]

[0048]

[0049]

[0050] The actual force of the forkbone on the boning table and the system's desired output force The relationship is expressed as a proportionality factor 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:

[0051]

[0052]

[0053] 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 , ;

[0054] 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:

[0055] The force control law is obtained After that, the position control expression obtained previously is Sum them and obtain the force-position mixed control law.

[0056] Further: Step 4 includes the following steps:

[0057] 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;

[0058] For a given three-dimensional segmentation surface, a two-dimensional curve is determined by projecting it into the two-dimensional space. The bone removal direction at each point x on the two-dimensional segmentation line is calculated by taking the point For example, the x-axis is the tangent direction of the point and the dividing plane in the horizontal direction, and the z-axis is the tangent direction of the point and the dividing plane in the vertical direction, thereby determining the y-axis direction; in the double-arm deboning process, deboning is performed in 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, and the tool is adjusted along the x-axis and z-axis directions of each deboning point with the tool center of mass as the center point. , ,use Indicates the moving speed of the tool along the x-axis direction. Indicates the deflection angle of the tool along the x-axis. Similarly, Indicates the tool moving speed along the z-axis direction, represents the deflection angle of the tool along the z-axis, so the artificial constraint is obtained , , , ;

[0059] use and They 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 is , ;

[0060] The constraint coordinate system transforms the natural constraints and artificial constraints in the deboning task into input parameters for the robot control. , , , Substitute the kinematic constraint equations (1-5) and (1-8) to calculate the desired joint velocity of the slave arm: ; Solve the expected joint acceleration through the acceleration constraint equation (1-11) (, used to plan the motion trajectory of the end of the robot arm); at the same time, the natural constraints , Bring in the force control law (3-11) and combine it with the actual measured force and expectation ;Finally, the position control expression (2-8) converts these joint accelerations and external forces into the driving torque of the robot joint;

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

[0062] 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;

[0063] The selection matrix S is a The diagonal elements in S are either 0 or 1, and their selection determines the forces and moments at each point on the bone-meat separation trajectory of the coccygeal furcula. The matrix I represents the identity matrix, and the matrix , if an element of the matrix S is selected to be 1, then The corresponding element in is 0, so 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;

[0064] 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.

[0065] 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. , 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 , thus the position control space control expression and force control space control law The obtained driving torques are added together to obtain the total driving torque , and sent to the joint drivers of the robot arm to achieve force-position hybrid control of the robot arm;

[0066] 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 , damping coefficient and stiffness coefficient 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:

[0067] Where X is the actual Cartesian position of the robot workspace, is the desired position of the robot arm; is the desired contact force of the robot, is the actual contact force between the manipulator and the coccygeal wishbone, is the actual contact force Expected contact force Deviation;

[0068] In the constrained coordinate system, according to equations (2-8) and (3-11), by introducing the mapping matrix and , respectively represent the transformation of position control and force control from joint space to operation space. The expression of manipulator force-position hybrid control in position control space and the control law in force control space are expressed as:

[0069]

[0070] 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, and the following equation is obtained by substituting into formula (5-2) and formula (5-3):

[0071] After obtaining the force-position hybrid control law , realize the force-position hybrid control of the slave arm, combine this control law with the master arm control law, and thus obtain the master-slave force-position hybrid control law;

[0072] Step 4.3: Perform force-position hybrid control of the dual robotic arms. The master arm holds the tail wishbone tightly, and the slave arm moves to remove the bone.

[0073] 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, we get the control law expression of the arm end position: :

[0074]

[0075]

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

[0077] According to the above solution to the 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: :

[0078] 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.

[0079] Furthermore: 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.

[0080] The beneficial effects of the present invention are as follows: firstly, by kinematically constraining both arms, the posture, speed and acceleration of the two arms in the collaborative operation process are controlled; then, the positions of both arms are controlled to ensure that the two arms move stably without deviation; secondly, the slave arm is force controlled to perform deboning by controlling the force; then, the two arms are coordinated with the coccyx forkbone deboning constraint control to make the slave arm deboning in a predetermined trajectory direction; then, the slave arm is subjected to force-position hybrid control to determine the position, force and torque of deboning; finally, the two arms are subjected to master-slave force-position hybrid control, the master arm tightly holds the coccyx forkbone, and the slave arm performs precise deboning operation. BRIEF DESCRIPTION OF THE DRAWINGS

[0081] Figure 1 is a flow chart of the present invention; Figure 2 It is the force diagram of single degree of freedom system; Figure 3 Coordinated boning diagrams for both arms; Figure 4 It is the algorithm flow chart of the present invention; Figure 5 This is a schematic diagram of the control algorithm in the present invention. DETAILED DESCRIPTION

[0082] The present invention is described in detail below in conjunction with the accompanying drawings. The embodiments of the present invention are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements with the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and cannot be interpreted as limiting the present invention. The directional terms such as left, middle, right, top, and bottom in the embodiments of the present invention are only relative concepts or are based on the normal use state of the product, and should not be considered as restrictive.

[0083] A master-slave double-arm force-position hybrid control method for coccyx forkbone deboning, combined with Figure 1 , Figure 3 , Figure 4 and Figure 5 As shown, it includes a dual robotic arm consisting of a master arm and a slave arm and the following steps:

[0084] 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;

[0085] Step 1.1: Constrain the position of the robot arm so that the main arm can move stably to the position of clamping the tail wishbone without deviation;

[0086] In order to ensure that the main arm can stably move to the position of clamping the coccyx without deviation, the robot arm needs to be constrained in position and posture. 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; {T} represents the coccyx coordinate system, and the origin of the coordinate system is the center of mass of the coccyx. is the homogeneous transformation matrix of the coccygeal coordinate system {T} relative to the arm end coordinate system {L};

[0087] 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:

[0088]

[0089] Similarly, the posture constraint relationship between the master arm and the slave arm is expressed as:

[0090]

[0091] in, and are the joint variables of the master arm and the slave arm respectively;

[0092] Step 1.2: Constrain the speed of the dual robotic arms to prevent vibration or instability caused by excessively fast movement of the robotic arms, which may damage the flesh of the forkbone;

[0093] First, the velocity constraint relationship between the fork bone and the main arm is analyzed; during the deboning process, there is no relative motion between the end of the main arm and the fork bone, so they have a common angular velocity; The absolute velocity vector of the center of mass of the fork bone is represented by 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:

[0094] in, ; refers to the position Jacobian matrix of the main arm; refers to the attitude Jacobian matrix of the main arm;

[0095] The movement trajectory of the coccyx determines its speed and posture changes in the operation space. The speed of the operation space is mapped to the joint space through the Jacobian matrix. Combined with the principle of velocity superposition, the speed of the main arm joint can uniquely meet the movement requirements of the coccyx; define the Jacobian matrix After the main arm joint speed is determined, the time derivative of both ends of equation (1-3) is:

[0096]

[0097] in, Represents the position vector of the end of the slave arm in the coordinate system of the end of the master arm, is the Jacobian matrix of the slave arm position;

[0098] 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. 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:

[0099] Step 1.3: Add acceleration constraints to prevent the robot arm from generating excessive inertial force or mechanical impact that may damage the flesh of the forkbone;

[0100] 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 , the absolute acceleration of the coccyx is obtained by differentiating it with respect to time ;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 represents the changing relationship between the coccyx and the main arm under acceleration constraint; the absolute acceleration of the coccyx is the absolute acceleration at the end of the main arm The combined result of the change in the velocity constraint is that the acceleration constraint equation between the tail wishbone and the end of the main arm is expressed as:

[0101] Similarly, in the speed constraint equation (1-8) of the master arm and the slave arm, 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 :

[0102]

[0103] 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:

[0104] The kinematic constraint equations for the coordinated operation of the dual robotic arms 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 achieve coordinated motion of the dual-arm robot.

[0105] 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;

[0106] Among them, in the deboning task, position control is adopted in the directions where the master arm and the slave arm have no force control; for deboning the coccyx, it is necessary to comprehensively consider the control of the dual robotic arms, form a closed loop with the dual robotic arms and the coccyx, and adopt the dual-arm closed-chain kinematic method for modeling. 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 coccyx deboning segmentation surface. 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 speed and acceleration constraints is generated, and the expected speed and posture of the end corresponding to each path point on the expected motion trajectory are determined; then, the expected joint displacement is determined by the kinematic constraint equation. ,speed and acceleration ;

[0107] Use q to represent the current actual joint displacement, and use The function representing the pose mapping from the joint space to the operation space is used to obtain the derivative of the time t on both sides of the kinematic equation of the manipulator, that is, to obtain the differential relationship between the operation space x and the joint space q.

[0108]

[0109]

[0110]

[0111] In the formula, refers to the generalized velocity of the end effector in the operating space, refers to the joint velocity in joint space, refers to the Jacobian matrix of the robot arm and is The partial derivative matrix of ;

[0112] use Represents the joint torque vector, the position control equation of the manipulator joint space Expressed as

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

[0114] 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 , the result is put into equation (2-5) to get the position control expression of the robot operation space ,

[0115]

[0116]

[0117] After obtaining the position control expression of the robot arm operation space, The movement of the robot arm can be controlled by sending the control system to the joint controller; It is also used for subsequent summation with the force control law to obtain the force-position mixed control law;

[0118] Step 3: Obtain the force control rate of the coccyx according to the force on the coccyx, and form a force-position mixed control law with the position control equation and the force control rate;

[0119] In the force control mode of the arm force position hybrid control, the wishbone force system is studied as a single degree of freedom system. Figure 2 As shown, use represents the main force acting on the coccygeal furcula, 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 , m represents the mass of the coccygeal wishbone, The height of the wishbone on the boning table. It represents the actual height of the surface of the wishbone after being pressed by the tool. The dynamic equation of the single degree of freedom system is obtained through force analysis:

[0120]

[0121]

[0122] in, represents the stiffness coefficient of the coccygeal deformation;

[0123] The height of the surface at rest, as measured by the force sensor mounted on the end of the slave arm 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:

[0124]

[0125]

[0126]

[0127] The actual force of the forkbone on the boning table and the system's desired output force The relationship is expressed as a proportionality factor 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:

[0128]

[0129]

[0130] 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 , ;

[0131] 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:

[0132] 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;

[0133] 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:

[0134] 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;

[0135] For a given three-dimensional segmentation surface, a two-dimensional curve is determined by projecting it into the two-dimensional space. The bone removal direction at each point x on the two-dimensional segmentation line is calculated by taking the point For example, the x-axis is the tangent direction of the point and the dividing plane in the horizontal direction, and the z-axis is the tangent direction of the point and the dividing plane in the vertical direction, thereby determining the y-axis direction; in the double-arm deboning process, deboning is performed in 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, and the tool is adjusted along the x-axis and z-axis directions of each deboning point with the tool center of mass as the center point. , ,use Indicates the moving speed of the tool along the x-axis direction. Indicates the deflection angle of the tool along the x-axis. Similarly, Indicates the tool moving speed along the z-axis direction, represents the deflection angle of the tool along the z-axis, so the artificial constraint is obtained , , , ;

[0136] In order to ensure that the double-arm operation can complete the deboning task, a certain force must be generated from the end of the arm to overcome the resistance of the meat to ensure that the tool moves in the direction of the target force for accurate deboning. and They 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 is , ;

[0137] The constraint coordinate system transforms the natural constraints and artificial constraints in the deboning task into input parameters for the robot control. , , , Substitute the kinematic constraint equations (1-5) and (1-8) to calculate the desired joint velocity of the slave arm: ; Solve the expected joint acceleration through the acceleration constraint equation (1-11) , used to plan the motion trajectory of the end of the robot arm; at the same time, the natural constraints , Bring in the force control law (3-11) and combine it with the actual measured force and expectation , adjust the output force from the end of the arm in real time; finally, the position control expression (2-8) converts these joint accelerations and external forces into the driving torque of the robot arm joints, ensuring that the robot arm can accurately apply force along the deboning path and remain stable, thereby meeting the dynamic requirements of the deboning task;

[0138] Step 4.2: Combine the force-position control law to realize the mixed control of position, force and torque, so as to accurately debone the slave arm on the debone path;

[0139] 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;

[0140] The selection matrix S is a The diagonal elements in S are either 0 or 1, and their selection determines the forces and moments at each point on the bone-meat separation trajectory of the coccygeal furcula. The matrix I represents the identity matrix, and the matrix , if an element of the matrix S is selected to be 1, then The corresponding element in is 0, so 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;

[0141] 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.

[0142] 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. , 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 , thus the position control space control expression and force control space control law The obtained driving torques are added together to obtain the total driving torque , and sent to the joint drivers of the robot arm to achieve force-position hybrid control of the robot arm;

[0143] The bone density of different parts of the coccygeal fork is uneven, and there may be a bone jam problem that has not been detected in advance. Position control cannot cross the bone jam area, so the slave arm adopts force-position hybrid control; impedance control is used for the compliant control of the slave arm, and the force feedback received by the six-dimensional force sensor at the end of the robotic arm in contact with the coccygeal fork and the response of the robotic arm are equivalent to admittance and impedance respectively; by adjusting the mass coefficient , damping coefficient and stiffness coefficient 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:

[0144] Where X is the actual Cartesian position of the robot workspace, is the desired position of the robot arm; is the desired contact force of the robot, is the actual contact force between the manipulator and the coccygeal wishbone, is the actual contact force Expected contact force Deviation;

[0145] In the constrained coordinate system, according to equations (2-8) and (3-11), by introducing the mapping matrix and , respectively represent the transformation of position control and force control from joint space to operation space. The expression of manipulator force-position hybrid control in position control space and the control law in force control space are expressed as:

[0146]

[0147] 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, and the following equation is obtained by substituting into formula (5-2) and formula (5-3):

[0148] After obtaining the force-position hybrid control law , realize the force-position hybrid control of the slave arm, combine this control law with the master arm control law, and thus obtain the master-slave force-position hybrid control law;

[0149] Step 4.3: Perform force-position hybrid control of the dual robotic arms. The master arm holds the tail wishbone tightly, and the slave arm moves to remove the bone.

[0150] The dual-arm robot uses the master-slave control mode. 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 wishbone tightly to keep it still. There is no need to control the direction of the force, and there is no need to select the matrix. The expression of the main arm end position control law is obtained by removing the selection matrix S from formula (5-2): 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, we get the control law expression of the arm end position: :

[0151]

[0152]

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

[0154] According to the above solution to the 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: :

[0155] 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 status information of each joint is obtained synchronously; the force and position data are input into the force-position hybrid control law equation of the two arms, and the force-position hybrid control law of the two arms is calculated; the force-position hybrid control law of the two arms is transmitted to the joint controller through the control system interface using the EtherCAT communication protocol; after receiving the force-position hybrid control law of the two arms, 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 coccyx deboning control based on force and position feedback information.

[0156] The above shows and describes the basic principles, main features and advantages of the present invention. It should be understood by those skilled in the art that the present invention is not limited to the above embodiments. The above embodiments and descriptions are only for explaining the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, which fall within the scope of the present invention. The scope of protection of the present invention is defined by the attached 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; Step 3: Obtain the force control rate of the coccyx according to the force on the coccyx, and form a force-position mixed control law with the position control equation and the force control rate; 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: in, and 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 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, ; refers to the position Jacobian matrix of the main arm; refers to the attitude Jacobian matrix of the main arm; Define the Jacobian matrix 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, 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. 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 , the absolute acceleration of the coccyx is obtained by differentiating it with respect to time ;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, 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: In step 2, in the deboning task, position control is used in the directions where the master arm and the slave arm have no force control; to debon the coccyx, it is necessary to comprehensively consider the control of the dual robotic arms, form a closed loop with the dual robotic arms and the coccyx, and use a dual-arm closed-chain kinematic method to model the dual-arm collaborative robot. 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 coccyx deboning segmentation surface. The segmentation surface is used as a three-dimensional geometric model, and 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; then, the expected joint displacement is determined by the kinematic constraint equation ,speed and acceleration ; Use q to represent the current actual joint displacement, and use The function representing the pose mapping from the joint space to the operation space is used to obtain the derivative of the time t on both sides of the kinematic equation of the manipulator, that is, to obtain the differential relationship between the operation space x and the joint space q. In the formula, refers to the generalized velocity of the end effector in the operating space, refers to the joint velocity in joint space, refers to the Jacobian matrix of the robot arm and is The partial derivative moment of ; use Represents the joint torque vector, the position control equation of the manipulator joint space Expressed as In the formula, represents the generalized inertial mass matrix, represents the inertia force term, represents the centrifugal force and the Coriolis force term, represents the gravity term, and 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 , the result is put into equation (2-5) to get the position control expression of the robot operation space , After obtaining the position control expression of the robot arm operation space, The movement of the robotic arm can be controlled by sending signals to the joint controller through the control system.

4. The master-slave double-arm force-position hybrid control method for coccyx wishbone deboning according to claim 3, characterized in that: In step 3, use represents the main force acting on the coccygeal furcula, 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 , m represents the mass of the coccygeal wishbone, The height of the wishbone on the boning table. It represents the actual height of the surface of the wishbone after being pressed by the tool. The dynamic equation of the single degree of freedom system is obtained through force analysis: in, represents the stiffness coefficient of the coccygeal deformation; The height of the surface at rest, as measured by the force sensor mounted on the end of the slave arm 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 of the forkbone on the boning table and the system's desired output force The relationship is expressed as a proportionality factor 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: 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 , ; 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: The force control law is obtained After that, the position control expression obtained previously is Sum them and obtain the force-position mixed control law.

5. The master-slave double-arm force-position hybrid control method for coccyx wishbone deboning according to claim 4, 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; For a given three-dimensional segmentation surface, a two-dimensional curve is determined by projecting it into the two-dimensional space. The bone removal direction at each point x on the two-dimensional segmentation line is calculated by taking the point For example, the x-axis is the tangent direction of the point and the dividing plane in the horizontal direction, and the z-axis is the tangent direction of the point and the dividing plane in the vertical direction, thereby determining the y-axis direction; in the double-arm deboning process, deboning is performed in 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, and the tool is adjusted along the x-axis and z-axis directions of each deboning point with the tool center of mass as the center point. , ,use Indicates the moving speed of the tool along the x-axis direction, Indicates the deflection angle of the tool along the x-axis. Similarly, Indicates the moving speed of the tool along the z-axis direction, represents the deflection angle of the tool along the z-axis, so the artificial constraint is obtained , , , ; use and They 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 is , ; The constraint coordinate system transforms the natural constraints and artificial constraints in the deboning task into input parameters for the robot control. , , , Substitute the kinematic constraint equations (1-5) and (1-8) to calculate the desired joint velocity of the slave arm: ; Solve the expected joint acceleration through the acceleration constraint equation (1-11) At the same time, natural constraints , Bring in the force control law (3-11) and combine it with the actual measured force and expectation ;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 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 The diagonal elements in S are either 0 or 1, and their selection determines the forces and moments at each point on the bone-meat separation trajectory of the coccygeal furcula. The matrix I represents the identity matrix, and the matrix , if an element of the matrix S is selected to be 1, then The corresponding element in is 0, so 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. , 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 , thus the position control space control expression and force control space control law The obtained driving torques are added together to obtain the total driving torque , 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 and the tail wishbone and the response of the robot arm are equivalent to admittance and impedance respectively; by adjusting the mass coefficient , damping coefficient and stiffness coefficient 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, is the desired position of the robot arm; is the desired contact force of the robot, is the actual contact force between the manipulator and the coccygeal wishbone, is the actual contact force Expected contact force Deviation; In the constrained coordinate system, according to equations (2-8) and (3-11), by introducing the mapping matrix and , respectively represent the transformation of position control and force control from joint space to operation space. The expression of manipulator force-position hybrid control in position control space and the control law in 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, and the following equation is obtained by substituting into formula (5-2) and formula (5-3): After obtaining the force-position hybrid control law , realize the force-position hybrid control of the slave arm, combine this control law with the master arm control law, and thus 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, we get the control law expression of the arm end position: : According to formula (5-3), the force control law expression of the slave arm end is obtained: According to the above solution to the 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.

6. The master-slave double-arm force-position hybrid control method for coccyx wishbone deboning according to claim 5, 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