Master-slave heterogeneous double-arm teleoperation mapping method based on redundant degree-of-freedom solution

Abstract

The invention belongs to the technical field of robot teleoperation, and relates to a master-slave heterogeneous two-arm teleoperation mapping method based on redundant degree-of-freedom resolving, which comprises the following steps: firstly, acquiring DH parameters of two exoskeleton arms of a main arm, establishing a forward kinematics model, and resolving poses of a fourth joint and a seventh joint of left and right arms of the main arm; then four virtual points of a left elbow, a left tail end, a right elbow and a right tail end of the main arm are constructed, posture alignment and zoom mapping are completed in combination with zero postures of fourth and seventh joints of the left and right arms of the slave arm, and a target posture of the slave arm is obtained; and finally, an inverse solution model is constructed by taking the arm-shaped angle as the redundant degree of freedom and combining the slave arm joint soft limit, the slave arm joint angle is solved, and the slave arm is driven to move. High-precision pose mapping between the master arm and the slave arm can be achieved, it is guaranteed that the slave arm accurately follows the master arm to move, meanwhile, slave arm joint limiting is effectively avoided, the stability and continuity of master-slave movement in the teleoperation process are improved, and the real-time operation requirement in the complex environment can be met.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of robot teleoperation, and relates to a master-slave heterogeneous dual-arm teleoperation mapping method based on redundant degree of freedom solving. BACKGROUND

[0002] A master-slave heterogeneous dual-arm teleoperation system drives a remote slave arm to perform a task by an operator controlling a master arm, and has an irreplaceable role in dangerous and complex environments. However, the structural heterogeneity of the master-slave arms, such as the number of joints, the length of links, and the difference in the range of motion, leads to two core challenges in motion mapping:

[0003] Insufficient pose mapping accuracy: Traditional absolute or incremental mapping methods only rely on the kinematic model for direct scaling and do not consider the influence of the mechanical structure difference between the master-slave arms on the motion space, which easily leads to deviation of the end pose of the slave arm. For example, different sizes of the master-slave arms will cause significant differences in the Cartesian space displacement corresponding to the same joint angle change, and direct mapping will cause operation errors.

[0004] Difficulty in coordinating redundant degrees of freedom: The redundant degrees of freedom of the slave arm, such as the arm shape angle of the SRS configuration, provide flexibility for motion solving, but existing methods such as multi-task hierarchical control and weighted least squares do not fully utilize the redundant degrees of freedom to optimize joint motion and do not effectively avoid joint limits and workspace restrictions. For example, traditional inverse algorithms may output solutions that exceed joint limits or exhibit oscillation near singular configurations.

[0005] In addition, existing technologies do not fully utilize the characteristics of the SRS configuration of the slave arm. The SRS configuration of the slave arm forms a zero-axis with the base, elbow, and end in the zero state, but traditional methods do not establish a reference alignment relationship based on this, resulting in inaccurate matching of the virtual point pose. At the same time, the initial pose difference between the master-slave arms in the zero state is also not systematically calibrated, further affecting the mapping accuracy. SUMMARY

[0006] To solve the above technical problems in the prior art, the application provides a master-slave heterogeneous dual-arm teleoperation mapping method based on redundant degree of freedom solving, which performs high-precision and high-robustness motion mapping through systematic kinematic modeling, virtual point alignment, and redundant degree of freedom optimization to achieve accurate solving of the joint angle of the slave arm. The specific technical scheme is as follows:

[0007] A master-slave heterogeneous dual-arm teleoperation mapping method based on redundant degree of freedom solving, comprising the following steps:

[0008] Step 1: Obtain the DH parameters of the master arm, establish a forward kinematic model of the master arm based on the DH parameters, and calculate the poses of the fourth joint and the seventh joint of the left and right arms of the master arm through the forward kinematic model;

[0009] Step 2: constructing four virtual points of a left elbow, a left end, a right elbow and a right end, aligning poses of the four virtual points with poses of fourth joints and seventh joints of left and right arms of the slave arm, and establishing a scaling mapping model according to a size ratio of the master arm and the slave arm, and calculating target poses of the fourth joints and the seventh joints of the left and right arms of the slave arm through the scaling mapping model;

[0010] Step 3: obtaining current joint angles of the slave arm, combining the target poses of the fourth joints and the seventh joints of the left and right arms of the slave arm obtained in step 2, first calculating an arm shape angle of the slave arm, then taking the arm shape angle as a redundant degree of freedom, constructing a kinematics inverse solution model of the slave arm, and completing calculation of joint angles of the slave arm through the inverse solution model to realize teleoperation mapping of the master-slave heterogeneous dual arms.

[0011] Further, the step 1 comprises the following specific steps:

[0012] Step 1.1: obtaining DH parameters of a left arm and a right arm of the master arm, the left arm of the master arm corresponding to each joint is numbered as left_Link4 to left_Link7, and DH parameters of the corresponding joints of the left arm of the master arm are left arm link length a L,i , left arm link torsion angle a L,i , left arm joint offset d L,i , and left arm joint angle Q L,i .

[0013] The right arm corresponding to each joint is numbered as right_Link4 to right_Link7, and DH parameters of the corresponding joint right_Link i of the right arm of the master arm are right arm link length a R,i , right arm link torsion angle a R,i , right arm joint offset d R,i , and right arm joint angle Q R,i .

[0014] Step 1.2: based on the DH parameters of the left arm and the right arm obtained in step 1.1, constructing a homogeneous transformation matrix between adjacent joints of the left arm of the master arm and a homogeneous transformation matrix between adjacent joints of the right arm.

[0015] Step 1.3: establishing a forward kinematics model of the left arm of the master arm and a forward kinematics model of the right arm by multiplying the homogeneous transformation matrices of adjacent joints, and calculating poses of the fourth joint left_Link4 and the seventh joint left_Link7 of the left arm of the master arm and poses of the fourth joint right_Link4 and the seventh joint right_Link7 of the right arm of the master arm by using the established forward kinematics models of the left and right arms.

[0016] Further, the step 1.2 is specifically:

[0017] The constructed left arm of the master arm kth joint left_Link k The homogeneous transformation matrix between the k+1th joint left_Link k+1 L,k T L,k+1 , as follows:

[0018]

[0019] Wherein, the value of k is 1 to 6; θ L,k+1 The rotation joint variable representing the joint angle of the left arm k+1th joint; a L,k The length of the left arm kth link, along the x-axis direction of the link coordinate system; α L,k The twist angle of the left arm kth link, that is, the rotation angle around the x-axis of the link coordinate system; d L,k+1 The offset of the left arm k+1th joint, along the z-axis direction of the joint coordinate system.

[0020] The constructed right arm of the master arm mth joint right_Link m The homogeneous transformation matrix between the m+1th joint right_Link m+1 R,m T R,m+1 , as follows:

[0021]

[0022] Wherein, the value of m is 1 to 6, θ R,m+1 The rotation joint variable representing the joint angle of the right arm m+1th joint; a R,m The length of the right arm kth link, along the x-axis direction of the link coordinate system; α R,k The twist angle of the right arm mth link, that is, the rotation angle around the x-axis of the link coordinate system; d R,k+1 The offset of the right arm m+1th joint, along the z-axis direction of the joint coordinate system.

[0023] Further, the step 1.3 is specifically:

[0024] The positive kinematics model of the left arm of the master arm includes: the homogeneous transformation matrix of the base coordinate system to the fourth joint left_Link4 of the left arm of the master arm L,0 T L,4 , the homogeneous transformation matrix of the base coordinate system to the seventh joint left_Link7 of the left arm of the master arm L,0 T L,7 , the expressions are respectively:

[0025] L,0 T L,4 = L,0 T L,1 × L,1 T​​L,2 X L,2 T L,3 X L,3 T L,4

[0026] L,0 T L,7 = L,0 T L,1 X L,1 T L,2 X L,2 T L,3 X L,3 T L,4 X L,4 T L,5 X L,5 T L,6 X L,6 T L,7

[0027] The positive kinematics model of the right arm of the master arm includes: a homogeneous transformation matrix of the base coordinate system to the fourth joint of the right arm of the master arm right_Link4 L,0 T R,4 , a homogeneous transformation matrix of the base coordinate system to the seventh joint of the right arm of the master arm right_Link7 R,0 T R,7 , the expressions are respectively:

[0028] R,0 T R,4 = R,0 T R,1 X R,1 T R,2 X R,2 T R,3 X R,3 T R,4

[0029] R,0 T R,7 = R,0 T R,1 X R,1 T R,2 X R,2 T R,3 X R,3 T R,4 X R,4 T R,5 X R,5 T R,6 X R,6 T R,7

[0030] Wherein, L,0 T L,1 is a homogeneous transformation matrix of the base coordinate system of the left arm to the first joint coordinate system of the left arm, R,0 TR,1 is the homogeneous transformation matrix from the right arm base coordinate system to the first joint coordinate system of the right arm, which is calculated according to the corresponding DH parameters;

[0031] The poses of the main arm left_Link4, left_Link7, right_Link4 and right_Link7 are obtained by analyzing the corresponding homogeneous transformation matrix, and are as follows:

[0032] The pose of left_Link4 is as follows: the position information is taken as L,0 T L,4 The fourth column elements in the first three rows are taken to obtain the three-dimensional coordinates (x L4 , y L4 , z L4 ) of left_Link4 in the main arm base coordinate system, which satisfy:

[0033]

[0034] The attitude information is taken as L,0 T L,4 The first three column elements in the first three rows are taken to form the rotation matrix R L4 of left_Link4:

[0035]

[0036] The poses of left_Link7, right_Link4 and right_Link7 are obtained in the same way.

[0037] Further, the step 2 specifically comprises:

[0038] Step 2.1: Taking the zero position state of the master-slave arm as a reference, constructing a left elbow virtual point, a left hand end virtual point, a right elbow virtual point and a right hand end virtual point, adjusting the main arm base to be collinear with the corresponding elbow virtual point and end virtual point, and aligning the poses of the four virtual points with the zero poses of the fourth joint and the seventh joint of the left and right arms of the slave arm, respectively;

[0039] Step 2.2: Measuring the physical dimensions of the corresponding large arms of the master-slave arm, establishing the proportion relationship of the large arms of the master-slave arm and constructing a scaling mapping model, inputting the poses of the fourth joint and the seventh joint of the left and right arms of the master arm into the model, and combining the pose alignment relationship of step 2.1 to calculate the target poses of the fourth joint and the seventh joint of the left and right arms of the slave arm.

[0040] Further, the step 2.1 is specifically:

[0041] When the main arm is in the zero position state, the joint angles of the left and right arms of the main arm are all initial zero values, and the main arm left arm collinear axis L a-left and the right arm collinear axis La-right , the main arm left arm co-linear axis L a-left , the base coordinate system Z axis direction under the zero position state of the main arm left arm, the starting point is the left arm base coordinate system origin O a-left0 , the axis equation is expressed as O a-left0 +t·Z a-left0 , wherein t is a parameter, Z a-left0 is the main arm left arm base coordinate system Z axis unit vector, the right arm co-linear axis L a-right , the same reason; the main arm left arm co-linear axis L a-left The normal plane P a-left4 is the plane passing through the current position of the fourth joint of the main arm left arm and perpendicular to L a-left The normal plane P a-right of the main arm right arm co-linear axis L a-right4 is the plane passing through the current position of the fourth joint of the main arm right arm and perpendicular to L a-right , and the normal plane P a-left7 , P a-left7 ;

[0042] Obtain the current position P left4 of the fourth joint of the main arm left arm in the main arm base coordinate system, which is obtained by the homogeneous transformation of the forward kinematics model of the main arm left arm, and translate the position P left4 to the main arm left arm co-linear axis L a-left along the normal plane P a-left , and the translation vector is ΔP left4 , and ΔP left4 satisfies ΔP left4 ·Z a-left0 =0, the position P v-left4 of the left elbow virtual point is the point on L a-left after translation, and the coordinates satisfy:

[0043] P v-left4 =P left4 +ΔP left4

[0044] Determine the position P v-left4 of the left elbow virtual point in this way, and the positions P v-left7 of the left hand virtual end point, the right elbow virtual point P v-right4 and the right hand virtual end point P v-right7 can be determined in the same way;

[0045] When the slave arm is in the zero position state, all joint angles are 0°, at this time, the origin of the slave arm base coordinate system, the origin of the elbow coordinate system of the fourth joint, and the origin of the end coordinate system of the seventh joint are on the same straight line, which is defined as the zero position axis of the slave arm single arm, and the X axes of the elbow and end coordinate systems are in the same direction as the zero position axis, the Z axes are perpendicular to the zero position axis and point in the same direction, and the Y axes are determined by the right-hand rule.

[0046] Assume that the rotation matrices of the left fourth joint left_Link4, the left seventh joint left_Link7, the right fourth joint right_Link4 and the right seventh joint right_Link7 of the slave arm in the zero position state are R L4,0 , R L7,0 , R R4,0 and R R7,0 , respectively, and that the left elbow virtual point, the left end virtual point, the right elbow virtual point and the right end virtual point of the master arm are aligned with the postures of left_Link4, left_Link7, right_Link4 and right_Link7 of the slave arm in the zero position state in the same reference coordinate system, that is:

[0047] R v-left4 =R L4,0

[0048] R v-left7 =R L7,0

[0049] R v-right4 =R R4,0

[0050] R v-right7 =R R7,0

[0051] wherein R v-left4 , R v-left7 , R v-right4 and R v-right7 represent the postures of the left elbow virtual point, the left end virtual point, the right elbow virtual point and the right end virtual point, respectively.

[0052] Further, the step 2.2 is specifically:

[0053] The zero position state of the master-slave arm determined in step 2.1 is used as the only measurement reference to determine the length L m1 of the large arm of the master arm, the length L m2 of the small arm of the master arm, the length L s1 of the large arm of the slave arm and the length L s2 of the small arm of the slave arm, so as to determine the scaling coefficients of the large arm and the small arm:

[0054]

[0055] The master arm base coordinate system O m -X m Y m Z m and the slave arm base coordinate system O s -X s Y s Zs , the homogeneous transformation matrix between them is T ms , the definition in the master arm motion state is:

[0056] Master arm left arm large arm vector Master arm left arm small arm vector Where P mLO is the master arm left arm base origin coordinate, P mL4 is the master arm left hand elbow virtual point position, P mL7 is the master arm left hand end virtual point position; Master arm right arm large arm vector Master arm right arm small arm vector Where P mRO is the master arm right arm base origin coordinate, P mR4 is the master arm right hand elbow virtual point position, P mR7 is the master arm right hand end virtual point position;

[0057] Convert the master arm left arm large arm vector to the slave arm coordinate system Then the slave arm left_Link4 position is:

[0058]

[0059] Where P sLO is the slave arm left arm base origin coordinate;

[0060] Convert the master arm left arm small arm vector to the slave arm coordinate system Then the slave arm left_Link7 position is:

[0061]

[0062] Similarly, the positions of the slave arm right_Link4 and right_Link7 are P sR4 and P sR7 ;

[0063] According to the alignment relationship of step 2.1, the pose of the master arm virtual point is consistent with the zero pose of the corresponding joint of the slave arm, so the target pose of the slave arm directly inherits the pose of the master arm virtual point, and the definition is:

[0064] The slave arm left_Link4 target pose rotation matrix R sL4 = R mL4 , where R mL4 is the pose rotation matrix of the master arm left hand elbow virtual point;

[0065] The slave arm left_Link7 target pose rotation matrix R sL4 = R mL4 , where R mL7 is the pose rotation matrix of the master arm left hand end virtual point;

[0066] Target pose rotation matrix R of slave arm right_Link4 sR4 = R mR4 , where R mR4 is the pose rotation matrix of the virtual point of the right hand elbow of the master arm;

[0067] Target pose rotation matrix R of slave arm right_Link7 sR7 = R mR7 , where R mR7 is the pose rotation matrix of the virtual point of the right hand end of the master arm.

[0068] Further, the step 3 specifically comprises:

[0069] Step 3.1: Obtain the current joint angles of the left and right arms of the slave arm through the joint detection components of the slave arm, and call the preset mechanical motion limit range of each joint of the slave arm; combine the target poses of the fourth joint and the seventh joint of the left and right arms of the slave arm obtained in step 2.2 to calculate the arm shape angle of each of the left and right arms of the slave arm;

[0070] Step 3.2: Take the calculated arm shape angle as the redundant degree of freedom to build a kinematics inverse solution model of the slave arm; in the model, combine the joint limit requirements and the target pose of the slave arm to solve the target angles that each joint of the left and right arms of the slave arm needs to reach through the model.

[0071] Further, the step 3.1 specifically comprises:

[0072] Define the key structural parameters of the left arm of the slave arm: the fixed length L1 from the left arm base to the shoulder, the total length L2 of the connecting rod from the shoulder to the elbow, the total length L3 of the connecting rod from the elbow to the wrist, and the total length L4 of the connecting rod from the wrist to the end; and obtain the current joint angle of the left arm of the slave arm through the joint encoder, and call the joint limit range r lim = [[r 1min , r 1max ], [r 2min , r 2max ], …, [r 7min , r 7max ]];

[0073] Establish the left arm base coordinate system {B L} of the slave arm, and call the position P s , the pose R s of the slave arm left_Link4 under the slave arm base coordinate system O s -X s Y sL4 Z sL4 , and the position P sL7 , the pose R sL7Transform to the left arm base coordinate system {B L} of the slave arm, i.e. the coordinate system {B L} of the slave arm left_Link4 pose and the position of the slave arm left_Link7 pose

[0074] wrist position in the base coordinate system wherein is the Z-axis unit vector of the slave arm left arm, defining the position of the shoulder in the slave arm left arm base coordinate system then the vector from the shoulder to the wrist whose length is: if nX sw >L2+L3+10 -6 (10 -6 is a numerical error threshold) or nX sw <|L2-L3|-10 -6 , it is determined that the end target position exceeds the slave arm workspace, and the solution is terminated.

[0075] Based on the cosine theorem, the calculation formula of the arm shape angle ψ L of the slave arm left arm is as follows:

[0076]

[0077] The range of the cosine value is clipped to avoid numerical overflow, and the arm shape angle ψ R of the slave arm right arm is obtained in the same way.

[0078] Further, the step 3.2 is specifically:

[0079] The core input parameters of the inverse solution model of the slave arm left arm are designed, including: the slave arm left_Link7 target pose obtained by step 3.1, the arm shape angle ψ L of the slave arm left arm;

[0080] The slave arm structure parameters, including the fixed length L1 from the left arm base to the shoulder, the total length L2 of the connecting rod from the shoulder to the elbow, the total length L3 of the connecting rod from the elbow to the wrist, and the total length L4 of the connecting rod from the wrist to the end;

[0081] The joint reference angle r ref of the slave arm left arm is [r ref1 , r ref2 , r ref3 , r ref4 , r ref5 , r ref6 , r ref7 , which is used to assist in selecting continuous solutions in multiple solutions;

[0082] From-arm joint limit r lim = [[r 1min , r 1max ], [r 2min , r 2max ], …, [r 7min , r 7max ]] represents the minimum / maximum motion angle of each joint;

[0083] The from-arm left-arm arm shape angle ψ L is combined with the joint limit [r 4min , r 4max ] and the reference angle r ref4 to obtain a reasonable r4;

[0084] The position vector a is defined as:

[0085]

[0086] The from-arm left-arm elbow rotation matrix R 03_0 under the reference plane when the 3rd joint angle is 0 is solved, which satisfies:

[0087] R 03_0 ·a = X sw

[0088] The shoulder-wrist vector is calculated as a unit vector and its inverse matrix E Xsw is constructed, which satisfies

[0089]

[0090] The from-arm left-arm elbow rotation matrix R 03_0 is corrected using the from-arm left-arm arm shape angle ψ L to obtain the actual from-arm left-arm elbow rotation matrix R 03 :

[0091]

[0092] The R 03 [2,2] = cos(r2) is combined with the joint limit [r 2min , r 2max ] and the reference angle r ref2 to obtain a reasonable r2;

[0093] If |r2| < 10 -6 :

[0094] r2 = 0, and r1 = arctan2(R 03 [1,0], R 03 [1,1]) - r ref [2], r3 = rref [2], and normalize r1 to [-π, π]: r1 = (r1 + π) % (2π) - π;

[0095] Otherwise, if sin(r2) > 0:

[0096] r1 = arctan2(R 03 [1, 2], R 03 [0, 2]), r3 = arctan2(R 03 [2, 1], -R 03 [2, 0]);

[0097] if sin(r2) < 0:

[0098] r1 = arctan2(-R 03 [1, 2], -R 03 [0, 2]), r3 = arctan2(-R 03 [2, 1], R 03 [2, 0]);

[0099] Construct the rotation matrix R w→ee from the left arm wrist to the end of the arm:

[0100]

[0101] r6 = arctan2(R w→ee [0, 2], combined with joint limits [r 6min , r 6max ] and reference angle r ref6 to get a reasonable r6;

[0102] if |r6| < 10 -6 :

[0103] r6 = 0, and r5 = arctan2(R w→ee [2, 0], R w→ee [2, 1]) - r ref [6], r7 = r ref [6], and normalize r5 to [-π, π]: r5 = (r5 + π) % (2π) - π;

[0104] Otherwise, if sin(r6) > 0:

[0105] r5 = arctan2(R w→ee [2, 2], R w→ee [2, 2]), r7 = arctan2(R w→ee [0, 1], -R w→ee [0, 0]);

[0106] If sin(r6) < 0:

[0107] r5 = arctan2(-R w→ee [0, 1], R w→ee [1, 2]), r7 = arctan2(-R w→ee [0, 1], R w→ee [0, 0]);

[0108] Finally, the required target angles of each joint of the slave left arm r = [r1, r2, r3, r4, r5, r6, r7] are obtained, and the target angles of each joint of the slave right arm are calculated by repeating the above process, thereby completing the teleoperation mapping of the master-slave heterogeneous dual arms.

[0109] Beneficial effects: The application can realize high-precision pose mapping between the master-slave arms, ensure that the slave arm accurately follows the movement of the master arm, effectively avoid joint limits of the slave arm, and improve the stability and continuity of the master-slave movement in the teleoperation process, and is particularly suitable for complex environment operation scenarios such as nuclear radiation area equipment maintenance, deep sea exploration, fire rescue, etc. with significant structural differences between the master-slave arms and high-precision pose mapping requirements. BRIEF DESCRIPTION OF DRAWINGS

[0110] Figure 1 The flowchart of the master-slave heterogeneous dual-arm teleoperation mapping method based on redundant degree of freedom calculation of the application. DETAILED DESCRIPTION

[0111] In order to make the purpose, technical scheme and technical effects of the application clearer, the application is further described in detail below in combination with the drawings and examples of the specification.

[0112] As shown in the drawings, Figure 1 a master-slave heterogeneous dual-arm teleoperation mapping method based on redundant degree of freedom calculation, specifically comprising the following steps:

[0113] Step 1: Obtain the DH parameters of the master arm, establish a forward kinematics model of the master arm based on the DH parameters, and calculate the poses of the fourth and seventh joints of the left and right arms of the master arm through the forward kinematics model. Specifically, the following sub-steps are included:

[0114] Step 1.1: Obtain the DH parameters of the left and right arms of the master arm, specifically as follows:

[0115] The DH parameters of the i-th joint left_Link i of the left arm are the left arm link length a L,i , the left arm link torsion angle α L,i , the left arm joint offset d L,i , and the left arm joint angle θ L,i ;

[0116] The j-th joint of the right arm (right_Link) i The DH parameter is the length a of the right arm link. R,i Right arm connecting rod torsion angle α R,i Right arm joint offset d R,i Right arm joint angle θ R,i ;

[0117] The values ​​of i and j are both from 1 to 7.

[0118] Step 1.2: Construct the homogeneous transformation matrices between adjacent joints in the left and right arms of the main arm, respectively, as follows:

[0119] The k-th joint of the left arm of the main arm, left_Link, is constructed. k With the (k+1)th joint left_Link k+1 Homogeneous transformation matrix between L,k T L,k+1 ,as follows:

[0120]

[0121] Where k takes values ​​from 1 to 6; θ L,k+1 Let a be the rotational joint variable representing the joint angle of the (k+1)th joint of the left arm; L,k Let α be the length of the k-th link in the left arm, along the x-axis of the link coordinate system; L,k Let d be the torsion angle of the k-th link in the left arm, i.e., the rotation angle about the x-axis of the link coordinate system; L,k+1 It represents the offset of the (k+1)th joint of the left arm, along the z-axis of the joint coordinate system.

[0122] The m-th joint of the right arm of the main arm is constructed as right_Link m With the (m+1)th joint right_Link m+1 Homogeneous transformation matrix between R,m T R,m+1 ,as follows:

[0123]

[0124] Where m takes values ​​from 1 to 6, θ R,m+1 Let a be the rotational joint variable representing the joint angle of the (m+1)th joint of the right arm; R,m Let α be the length of the k-th link in the right arm, along the x-axis of the link coordinate system; R,k Let d be the torsion angle of the m-th link in the right arm, i.e., the rotation angle about the x-axis of the link coordinate system; R,k+1 It represents the offset of the (m+1)th joint of the right arm, along the z-axis of the joint coordinate system.

[0125] Step 1.3: Establish the positive kinematics model of the left arm and the positive kinematics model of the right arm of the main arm by multiplying the adjacent joint homogeneous transformation matrix, and calculate the pose of the fourth joint and the seventh joint of the left arm and the right arm of the main arm by using the positive kinematics model, as follows:

[0126] The positive kinematics model of the left arm of the main arm includes: the homogeneous transformation matrix from the base coordinate system to the fourth joint left_Link4 of the left arm of the main arm L,0 T L,4 , the homogeneous transformation matrix from the base coordinate system to the seventh joint left_Link7 of the left arm of the main arm L,0 T L,7 , and the expressions are as follows:

[0127] L,0 T L,4 = L,0 T L,1 × L,1 T L,2 × L,2 T L,3 × L,3 T L,4

[0128] L,0 T L,7 = L,0 T L,1 × L,1 T L,2 × L,2 T L,3 × L,3 T L,4 × L,4 T L,5 × L,5 T L,6 × L,6 T L,7

[0129] The positive kinematics model of the right arm of the main arm includes: the homogeneous transformation matrix from the base coordinate system to the fourth joint right_Link4 of the right arm of the main arm L,0 T R,4 , the homogeneous transformation matrix from the base coordinate system to the seventh joint right_Link7 of the right arm of the main arm R,0 T R,7 , and the expressions are as follows:

[0130] R,0 T R,4 = R,0 T R,1 × R,1 T R,2 × R,2 T R,3 × R,3 T R,4

[0131] R,0 T R,7 = R,0 T R,1 × R,1 T R,2 × R,2 T R,3 × R,3 T R,4 × R,4 T R,5 × R,5 T R,6 × R,6 T R,7

[0132] wherein, L,0 T L,1 is a homogeneous transformation matrix from the left arm base coordinate system to the left arm 1st joint coordinate system, R,0 T R,1 is a homogeneous transformation matrix from the right arm base coordinate system to the right arm 1st joint coordinate system, both of which are calculated by corresponding DH parameters.

[0133] The poses of the main arms left_Link4, left_Link7, right_Link4 and right_Link7 are analytically obtained by corresponding homogeneous transformation matrices, and are specifically as follows:

[0134] The pose of left_Link4: the position information is taken from the 4th column of the first 3 rows of L,0 T L,4 , to obtain the three-dimensional coordinates (x L4 ,y L4 ,z L4 ) of left_Link4 in the main arm base coordinate system, satisfying:

[0135]

[0136] The attitude information is taken from the first 3 columns of the first 3 rows of L,0 T L,4 , to form a rotation matrix R L4 of left_Link4:

[0137]

[0138] Similarly, the poses of left_Link7, right_Link4 and right_Link7 can be obtained.

[0139] Step 2: construct four virtual points of left elbow, left end, right elbow, and right end, align the poses of the four virtual points with the poses of the fourth joint and the seventh joint of the left and right arms of the slave arm, and establish a scaling mapping model according to the size ratio of the master arm and the slave arm, and calculate the target poses of the fourth joint and the seventh joint of the left and right arms of the slave arm through the scaling mapping model. Specifically, the following sub-steps are included:

[0140] Step 2.1: Take the zero position state of the master-slave arm as the reference, construct the virtual points of left elbow, left end, right elbow, and right end, adjust the master arm base to be collinear with the corresponding virtual points of elbow and end, and align the poses of the four virtual points with the zero poses of the fourth joint and the seventh joint of the left and right arms of the slave arm. The specific derivation process is as follows:

[0141] Set the master arm in the zero position state, at this time the joint angles of the left and right arms of the master arm are all initial zero values. Take the master arm base coordinate system as the reference, define the master arm left arm collinear axis L a-left and the right arm collinear axis L a-right , the master arm left arm collinear axis L a-left is along the Z-axis direction of the base coordinate system in the zero position state of the master arm left arm, and the starting point is the origin O a-left0 of the left arm base coordinate system, and the axis equation is represented as O a-left0 +t·Z a-left0 , where t is a parameter, and Z a-left0 is the Z-axis unit vector of the master arm left arm base coordinate system. The right arm collinear axis L a-right is the same; the normal plane P a-left of the master arm left arm collinear axis L a-left4 is the plane passing through the current position of the fourth joint of the master arm left arm and perpendicular to L a-left , and the normal plane P a-right of the master arm right arm collinear axis L a-right4 is the plane passing through the current position of the fourth joint of the master arm right arm and perpendicular to L a-right , and the normal planes P a-left7 and P a-left7 are the same.

[0142] Get the current position P left4 of the fourth joint of the master arm left arm in the master arm base coordinate system, which is obtained by the homogeneous transformation of the forward kinematics model of the master arm left arm. Translate the position P left4 to the master arm left arm collinear axis L a-left along the normal plane P a-left , and the translation vector is ΔP left4 , and ΔP left4 satisfies ΔP left4 ·Z a-left0 =0 to ensure that it is in the normal plane. The position P v-left4 of the left elbow virtual point after translation in La-left whose coordinates satisfy:

[0143] P v-left4 = P left4 + ΔP left4

[0144] The position P v-left4 of the virtual point of the left elbow is determined in this way, and the position P v-left7 of the virtual point of the left hand tip, the position P v-right4 of the virtual point of the right elbow, and the position P v-right7 of the virtual point of the right hand tip can be determined in the same way.

[0145] The slave arm of the embodiment is of SRS configuration, and the core structural features are as follows: the slave arm single arm comprises 7 joints, which are classified according to the motion types as: the base joint of the 1st joint, the shoulder joint of the 2nd-3rd joints, the elbow joint of the 4th joint, and the wrist joint of the 5th-7th joints. The 1st joint is a rotating joint S, rotating around the Z-axis of the base coordinate system; the 2nd-3rd joints constitute a shoulder motion group, realizing the pitching of the arm in the vertical plane through linkage; the 4th joint is a rotating joint R, serving as the core joint of the elbow, controlling the bending and stretching of the arm; and the 5th-7th joints constitute a wrist motion group, being spherical joints S, realizing the posture adjustment of the end effector.

[0146] When the slave arm of SRS configuration is in the zero position state, all joint angles are 0°, at this time, the origin of the slave arm base coordinate system, the origin of the elbow coordinate system of the 4th joint, and the origin of the end coordinate system of the 7th joint are on the same straight line, which is defined as the zero position axis of the slave arm single arm, and the X-axes of the elbow and end coordinate systems are in the same direction as the zero position axis, the Z-axes are perpendicular to the zero position axis and point in the same direction, and the Y-axes are determined by the right-hand rule;

[0147] Suppose that the rotation matrices of the left_Link4, left_Link7, right_Link4, and right_Link7 of the slave arm in the zero position state are R L4,0 , R L7,0 , R R4,0 , and R R7,0 , respectively, in the same reference coordinate system, the left elbow virtual point, the left hand tip virtual point, the right elbow virtual point, and the right hand tip virtual point of the master arm are aligned with the poses of the left_Link4, left_Link7, right_Link4, and right_Link7 of the slave arm in the zero position state, respectively, that is:

[0148] R v-left4 = R L4,0

[0149] R v-left7 = R L7,0

[0150] R v-right4 = R R4,0

[0151] R v-right7 = R R7,0

[0152] wherein R v-left4 , R v-left7 , R v-right4 and R v-right7 represent the poses of the left elbow virtual point, the left hand virtual point, the right elbow virtual point and the right hand virtual point, respectively.

[0153] Step 2.2: Measure the physical dimensions of the master arm and the slave arm, establish the size arm proportion relationship of the master arm and the slave arm and construct a scaling mapping model, input the poses of the fourth joint and the seventh joint of the left arm and the right arm of the master arm into the model, combine the pose alignment relationship of step 2.1, calculate the target poses of the fourth joint and the seventh joint of the left arm and the right arm of the slave arm, and the specific derivation process is as follows:

[0154] Take the master arm and slave arm zero position state determined in step 2.1 as the only measurement reference, and all joint angles are 0°, to determine the following parameters:

[0155] L m1 : the straight line distance from the origin of the base coordinate system to the origin of the fourth joint coordinate system of the master arm, that is, the equivalent total length of the first-fourth joint connecting rod;

[0156] L m2 : the straight line distance from the origin of the fourth joint coordinate system of the master arm to the origin of the seventh joint coordinate system of the master arm, that is, the equivalent total length of the fourth-seventh joint connecting rod;

[0157] L s1 : the straight line distance from the origin of the base coordinate system to the origin of the fourth joint coordinate system of the slave arm, that is, the equivalent total length of the first-fourth joint connecting rod;

[0158] L s2 : the straight line distance from the origin of the fourth joint coordinate system of the slave arm to the origin of the seventh joint coordinate system of the slave arm, that is, the equivalent total length of the fourth-seventh joint connecting rod;

[0159] Thus, the scaling coefficients of the large arm and the small arm are determined:

[0160]

[0161] Define the master arm base coordinate system O m -X m Y m Z m and the slave arm base coordinate system O s -X s Ys Z s , the homogeneous transformation matrix between them is T ms , in the master arm moving state, define:

[0162] Master arm left arm large arm vector Master arm left arm small arm vector Where P mLO is the master arm left arm base origin coordinate, P mL4 is the master arm left hand elbow virtual point position, P mL7 is the master arm left hand end virtual point position; Master arm right arm large arm vector Master arm right arm small arm vector Where P mRO is the master arm right arm base origin coordinate, P mR4 is the master arm right hand elbow virtual point position, P mR7 is the master arm right hand end virtual point position;

[0163] Convert the master arm left arm large arm vector to the slave arm coordinate system Then the slave arm left_Link4 position is:

[0164]

[0165] Where P sLO is the slave arm left arm base origin coordinate;

[0166] Convert the master arm left arm small arm vector to the slave arm coordinate system Then the slave arm left_Link7 position is:

[0167]

[0168] Similarly, the positions of the slave arm right_Link4 and right_Link7 are P sR4 and P sR7 ;

[0169] According to the alignment relationship of step 2.1, the pose of the master arm virtual point is consistent with the zero pose of the corresponding joint of the slave arm, so the target pose of the slave arm directly inherits the pose of the master arm virtual point, define:

[0170] The slave arm left_Link4 target pose rotation matrix R sL4 = R mL4 , where R mL4 is the pose rotation matrix of the master arm left hand elbow virtual point;

[0171] The slave arm left_Link7 target pose rotation matrix R sL4 = R mL4 , where R mL7Rotation matrix of the posture of the virtual point at the left hand end of the master arm;

[0172] Rotation matrix R of the target posture of the arm right_Link4 sR4 = R mR4 , wherein R mR4 is a rotation matrix of the posture of the virtual point at the right hand elbow of the master arm;

[0173] Rotation matrix R of the target posture of the arm right_Link7 sR7 = R mR7 , wherein R mR7 is a rotation matrix of the posture of the virtual point at the right hand end of the master arm.

[0174] Step 3: Obtain the current joint angles of the slave arm and the joint limits, combine the target poses of the fourth joint and the seventh joint of the left arm and the right arm of the slave arm obtained in step 2, first calculate the arm shape angle of the slave arm, and then use the arm shape angle as the redundant degree of freedom to construct the inverse kinematics model of the slave arm, and complete the calculation of the joint angles of the slave arm through the inverse kinematics model of the slave arm, so as to realize the teleoperation mapping of the master-slave heterogeneous dual arms. Specifically, the following sub-steps are included:

[0175] Step 3.1: Obtain the current joint angles of the left arm and the right arm of the slave arm through the joint detection component of the slave arm, and retrieve the preset mechanical motion limit range of each joint of the slave arm; combine the target poses of the fourth joint and the seventh joint of the left arm and the right arm of the slave arm obtained in step 2.2, and calculate the arm shape angles of the left arm and the right arm of the slave arm, and the specific derivation process is as follows:

[0176] Define the key structural parameters of the left arm of the slave arm: the fixed length L1 from the left arm base to the shoulder, the total length L2 of the connecting rod from the shoulder to the elbow, the total length L3 of the connecting rod from the elbow to the wrist, and the total length L4 of the connecting rod from the wrist to the end; and obtain the current joint angle of the left arm of the slave arm through the joint encoder, and retrieve the joint limit range r lim = [[r 1min , r 1max ], [r 2min , r 2max ], …, [r 7min , r 7max ]].

[0177] Establish the left arm base coordinate system {B L} of the slave arm, and the slave arm base coordinate system O s -X s Y s Z s , the position P sL4 and the posture R sL4 of the slave arm left_Link4, and the position P sL7 and the posture R sL7Transform to the left arm base coordinate system {B L}, i.e. the position of the left arm of the slave arm left_Link4 in the coordinate system {B L}. The pose of the left arm of the slave arm left_Link7 The pose of the left arm of the slave arm left_Link7 The pose of the left arm of the slave arm left_Link7

[0178] The wrist position in the base coordinate system Wherein is the Z-axis unit vector of the left arm of the slave arm, defining the position of the shoulder in the left arm base coordinate system of the slave arm The vector from the shoulder to the wrist is The length of the vector, i.e. the straight-line distance from the shoulder to the wrist, is If nX sw > L2+L3+10 -6 (10 -6 is a numerical error threshold) or nX sw < |L2-L3| -10 -6 , it is determined that the end target position exceeds the working space of the slave arm, and the solving is terminated.

[0179] Based on the cosine theorem, the calculation formula of the arm shape angle ψ L of the left arm of the slave arm is as follows:

[0180]

[0181] The cosine value needs to be range-cropped to avoid numerical overflow, and the arm shape angle ψ R of the right arm of the slave arm can be obtained in the same way.

[0182] Step 3.2: Take the calculated arm shape angle as the redundant degree of freedom, and build the inverse kinematics model of the slave arm; in the model, combine the joint limiting requirements of the slave arm with the target pose, and solve the target angles that each joint of the left and right arms of the slave arm needs to reach through the model, to ensure that the solving result meets the pose requirements, and at the same time, all joint angles do not exceed the limiting range. The specific derivation process is as follows:

[0183] Taking the left arm of the slave arm as an example, the core input parameters of the inverse model are: the target pose (position pose ) of the left arm of the slave arm left_Link7, obtained from step 3.1; the arm shape angle ψ L of the left arm of the slave arm, obtained from step 3.1; the structure parameters of the slave arm, including the fixed length L1 from the left arm base to the shoulder, the total length L2 of the connecting rod from the shoulder to the elbow, the total length L3 of the connecting rod from the elbow to the wrist, and the total length L4 of the connecting rod from the wrist to the end; the joint reference angles r ref = [r ref1 , r ref2 , rref3 ,r ref4 ,r ref5 ,r ref6 ,r ref7 ], used to assist in selecting continuous solutions from multiple solutions; from the arm joint limit r lim =[[r 1min ,r 1max ],[r 2min ,r 2max ],...,[r 7min ,r 7max The minimum / maximum range of motion for each joint.

[0184] From the left arm arm shape angle ψ L Combined with joint limitation [r 4min ,r 4max ] and reference angle r ref4 A reasonable solution is obtained: r4;

[0185] Define position vector a:

[0186]

[0187] Solve for the rotation matrix R of the left elbow of the secondary arm in the reference plane when the angle of the third joint is 0. 03_0 ,satisfy:

[0188] R 03_0 ·a=X sw

[0189] Calculate the normalized shoulder-wrist vector And construct its anti-forming matrix satisfy

[0190]

[0191]

[0192] Using the angle ψ of the left arm of the arm L Correcting the rotation matrix R from the left elbow of the arm 03_0 The actual rotation matrix R from the left elbow of the arm is obtained. 03 :

[0193]

[0194] By R 03 [2,2]=cos(r2), combined with joint limitation [r 2min ,r 2max and reference angle r ref2 A reasonable solution r2 is obtained;

[0195] If |r2|<10 -6:

[0196] r2 = 0, and r1 = arctan2(R 03 [1, 0], R 03 [1, 1]) - r ref [2], r3 = r ref [2], and normalize r1 to [-π, π]: r1 = (r1 + π) % (2π) - π.

[0197] Otherwise, if sin(r2) > 0:

[0198] r1 = arctan2(R 03 [1, 2], R 03 [0, 2]), r3 = arctan2(R 03 [2, 1], -R 03 [2, 0]);

[0199] if sin(r2) < 0:

[0200] r1 = arctan2(-R 03 [1, 2], -R 03 [0, 2]), r3 = arctan2(-R 03 [2, 1], R 03 [2, 0]);

[0201] Construct the rotation matrix R w→ee from the left arm wrist to the end of the arm:

[0202]

[0203] From R w→ee [0, 2] = cos(r6), combined with joint limits [r 6min , r 6max ] and reference angle r ref6 , get a reasonable r6;

[0204] if |r6| < 10 -6 :

[0205] r6 = 0, and r5 = arctan2(R w→ee [2, 0], R w→ee [2, 1]) - r ref [6], r7 = r ref [6], and normalize r5 to [-π, π]: r5 = (r5 + π) % (2π) - π.

[0206] Otherwise, if sin(r6) > 0:

[0207] r5 = arctan2(Rw→ee [2,2], R w→ee [2,2], r7 = arctan2(R w→ee [0,1], -R w→ee [0,0]);

[0208] If sin(r6) < 0:

[0209] r5 = arctan2(-R w→ee [2,2], -R w→ee [1,2], r7 = arctan2(-R w→ee [0,1], R w→ee [0,0]);

[0210] Finally, the required target angles of each joint of the slave left arm are obtained, i.e., r = [r1, r2, r3, r4, r5, r6, r7], and the target angles of each joint of the slave right arm are obtained by repeating the above process, thereby completing the teleoperation mapping of the master-slave heterogeneous dual arms.

[0211] In one embodiment, the present application is based on a master-slave heterogeneous dual-arm teleoperation mapping method based on redundant degree of freedom calculation, and the specific implementation object is a master-slave heterogeneous teleoperation system composed of a master arm exoskeleton dual arm and a slave arm Rieman dual arm. In the master-slave heterogeneous teleoperation system, the joint soft limit of the slave arm Rieman dual arm is as follows: joint 1: -180°~+180°; joint 2: -120°~+120°; joint 3: -180°~+180°; joint 4 left arm: -105°~+85°; joint 4 right arm: -85°~+105°; joint 5: -180°~+180°; joint 6: -100°~+100°; joint 7: -180°~+180°; and the maximum angular velocity of each joint is 225° / s. The structure parameters of the slave arm are as follows: the fixed length from the left arm base to the shoulder is 0.1239 m, the total length of the connecting rod from the shoulder to the elbow is 0.305 m, the total length of the connecting rod from the elbow to the wrist is 0.1975 m, and the total length of the connecting rod from the wrist to the end is 0.135 m; the scaling mapping coefficients of the master-slave arms are k1 = 1.12 and k2 = 1.15.

[0212] To sum up, in the implementation process of the present application, first, the DH parameters of the master arm exoskeleton double arms are acquired, the forward kinematics model is established and the fourth joint and the seventh joint of the left and right arms of the master arm are calculated; then the left hand elbow, the left hand end, the right hand elbow and the right hand end of the master arm are constructed, and the pose alignment and scaling mapping are completed combined with the zero pose of the fourth and seventh joints of the slave arm, so as to obtain the target pose of the slave arm; finally, the arm shape angle is taken as the redundant freedom degree, and the inverse solution model is constructed combined with the joint soft limit of the slave arm, so as to calculate the joint angle of the slave arm and drive the slave arm to move. Based on the above settings, the present application can realize high-precision pose mapping between the master and slave arms, ensure that the slave arm accurately follows the movement of the master arm, effectively avoid the joint limit of the slave arm, improve the stability and continuity of the master and slave movement in the teleoperation process, and is especially suitable for complex environment operation scenarios such as nuclear radiation area equipment maintenance, deep sea exploration, fire rescue, etc. with significant structural difference between the master and slave arms and high-precision pose mapping requirement.

[0213] The above is only the preferred embodiment of the present application, and does not limit the present application in any form. Although the implementation process of the present application has been described in detail in the foregoing, those skilled in the art can still modify the technical solutions recorded in the foregoing examples or replace some of the technical features with equivalent ones. Any modification, equivalent replacement, etc. within the spirit and principles of the present application shall be included in the protection scope of the present application.

Claims

1. A master-slave heterogeneous dual-arm teleoperation mapping method based on redundant degrees of freedom calculation, characterized in that, Includes the following steps: Step 1: Obtain the DH parameters of the main arm, establish a forward kinematics model of the main arm based on the DH parameters, and calculate the pose of the fourth and seventh joints of the left and right arms of the main arm through the forward kinematics model; Step 2: Construct four virtual points: left elbow, left hand end, right elbow, and right hand end. Align the poses of these four virtual points with the poses of the fourth and seventh joints of the left and right arms of the slave arm. At the same time, establish a scaling mapping model based on the proportional relationship between the master and slave arms and the upper and lower arms. Calculate the target poses of the fourth and seventh joints of the left and right arms of the slave arm through the scaling mapping model. Step 3: Obtain the current joint angle and joint limit of the slave arm. Combine the target poses of the fourth and seventh joints of the left and right arms obtained in Step 2, first calculate the arm shape angle of the slave arm, and then use the arm shape angle as redundant degrees of freedom to construct the inverse kinematics model of the slave arm. The inverse kinematics model is used to complete the calculation of the joint angle of the slave arm, and realize the teleoperation mapping of the master-slave heterogeneous dual arms.

2. The master-slave heterogeneous dual-arm teleoperation mapping method according to claim 1, characterized in that, Step 1 includes the following specific steps: Step 1.1: Obtain the DH parameters of the left and right arms of the main arm. The links corresponding to each joint of the left arm are numbered from left_Link4 to left_Link7, and the DH parameter of the corresponding joint of the left arm is the length a of the left arm link. L,i Left arm connecting rod torsion angle α L,i Left arm joint offset d L,i Left arm joint angle θ L,i ; The links corresponding to the joints of the right arm are numbered right_Link4 to right_Link7, and the corresponding links of the main arm and right arm are right_Link4 to right_Link7. i The DH parameter is the length a of the right arm link. R,i Right arm connecting rod torsion angle α R,i Right arm joint offset d R,i Right arm joint angle θ R,i ; Step 1.2: Based on the DH parameters of the left and right arms obtained in Step 1.1, construct the homogeneous transformation matrix between adjacent joints of the left arm and the homogeneous transformation matrix between adjacent joints of the right arm, respectively. Step 1.3: By multiplying the homogeneous transformation matrices of adjacent joints, establish the forward kinematics models of the left and right arms of the main arm. Using the established forward kinematics models of the left and right arms, calculate the poses of the fourth joint left_Link4 and the seventh joint left_Link7 of the left arm of the main arm, as well as the poses of the fourth joint right_Link4 and the seventh joint right_Link7 of the right arm of the main arm.

3. The master-slave heterogeneous dual-arm teleoperation mapping method according to claim 2, characterized in that, Step 1.2 specifically includes: The k-th joint of the left arm of the main arm, left_Link, is constructed. k With the (k+1)th joint left_Link k+1 Homogeneous transformation matrix between L,k T L,k+1 ,as follows: Where k takes values ​​from 1 to 6; θ L,k+1 Let a be the rotational joint variable representing the joint angle of the (k+1)th joint of the left arm; L,k Let α be the length of the k-th link in the left arm, along the x-axis of the link coordinate system; L,k Let d be the torsion angle of the k-th link in the left arm, i.e., the rotation angle about the x-axis of the link coordinate system; L,k+1 It represents the offset of the (k+1)th joint of the left arm, along the z-axis of the joint coordinate system. The m-th joint of the right arm of the main arm is constructed. m With the (m+1)th joint right_Link m+1 Homogeneous transformation matrix between R,m T R,m+1 ,as follows: Where m takes values ​​from 1 to 6, θ R,m+1 Let a be the rotational joint variable representing the joint angle of the (m+1)th joint of the right arm; R,m Let α be the length of the k-th link in the right arm, along the x-axis of the link coordinate system; R,k Let d be the torsion angle of the m-th link in the right arm, i.e., the rotation angle about the x-axis of the link coordinate system; R,k+1 It represents the offset of the (m+1)th joint of the right arm, along the z-axis of the joint coordinate system.

4. The master-slave heterogeneous dual-arm teleoperation mapping method according to claim 3, characterized in that, Step 1.3 specifically includes: The forward kinematic model of the main arm's left arm includes: the homogeneous transformation matrix from the base coordinate system to the fourth joint of the main arm's left arm, left_Link4. L,0 T L,4 Homogeneous transformation matrix from the base coordinate system to the seventh joint of the left arm of the main arm (left_Link7) L,0 T L,7 The expressions are as follows: L,0 T L,4 = L,0 T L,1 × L,1 T L,2 × L,2 T L,3 × L,3 T L,4 L,0 T L,7 = L,0 T L,1 × L,1 T L,2 × L,2 T L,3 × L,3 T L,4 × L,4 T L,5 × L,5 T L,6 × L,6 T L,7 The forward kinematics model of the main arm and right arm includes: the homogeneous transformation matrix from the base coordinate system to the fourth joint (right_Link4) of the main arm and right arm. L,0 T R,4 Homogeneous transformation matrix from the base coordinate system to the seventh joint of the right arm of the main arm (right_Link7) R,0 T R,7 The expressions are as follows: R,0 T R,4 = R,0 T R,1 × R,1 T R,2 × R,2 T R,3 × R,3 T R,4 R,0 T R,7 = R,0 T R,1 × R,1 T R,2 × R,2 T R,3 × R,3 T R,4 × R,4 T R,5 × R,5 T R,6 × R,6 T R,7 in, L,0 T L,1 Let be the homogeneous transformation matrix from the left arm base coordinate system to the left arm first joint coordinate system. R,0 T R,1 The homogeneous transformation matrix from the right arm base coordinate system to the right arm first joint coordinate system is calculated from the corresponding DH parameters. The poses of the main arms left_Link4, left_Link7, right_Link4, and right_Link7 are obtained analytically through the corresponding homogeneous transformation matrices, as follows: Pose of left_Link4: position information retrieved L,0 T L,4 The element in the first 3 rows and 4th column gives the 3D coordinates (x, y) of left_Link4 in the main arm base coordinate system. L4 ,y L4 ,z L4 ),satisfy: Attitude information acquisition L,0 T L,4 The first 3 rows and first 3 columns form the rotation matrix R of left_Link4. L4 : Similarly, the poses of left_Link7, right_Link4, and right_Link7 can be obtained.

5. The master-slave heterogeneous dual-arm teleoperation mapping method according to claim 1, characterized in that, Step 2 specifically includes: Step 2.1: Based on the zero position of the master and slave arms, construct the virtual points of the left elbow, left end, right elbow, and right end. Adjust the master arm base to be collinear with the corresponding elbow and end virtual points, and align the postures of the four virtual points with the zero positions of the fourth and seventh joints of the left and right arms of the slave arms, respectively. Step 2.2: Measure the physical dimensions of the master and slave arms corresponding to the large and small arms, establish the proportional relationship between the master and slave arms and the large and small arms, and construct a scaling mapping model. Input the poses of the fourth and seventh joints of the left and right arms of the master arm into the model. Combine the pose alignment relationship in Step 2.1 to calculate the target poses of the fourth and seventh joints of the left and right arms of the slave arm.

6. The master-slave heterogeneous dual-arm teleoperation mapping method according to claim 5, characterized in that, Step 2.1 specifically involves: When the main arm is in the zero position, all joint angles of the left and right arms of the main arm are initially zero. Using the main arm base coordinate system as a reference, the collinear axis L of the left arm of the main arm is defined. a-left Collinear axis L with the right arm a-right The main arm and left arm are collinear axes L a-left Along the Z-axis of the base coordinate system in the zero-position state of the main arm's left arm, starting from the origin O of the left arm's base coordinate system. a-left0 The equation of the axis is expressed as O a-left0 +t·Z a-left0 Where t is a parameter, Z a-left0 The unit vector of the Z-axis in the coordinate system of the main arm's left arm base, and the collinear axis of the right arm L. a-right Similarly; the main arm and left arm are collinear along axis L a-left normal plane P a-left4 The current position of the 4th joint of the left arm, which is perpendicular to L. a-left The plane, the main arm and the right arm are collinear with axis L a-right normal plane P a-right4 The current position of the 4th joint of the right arm, which is perpendicular to L. a-right The plane, similarly, has the normal plane P. a-left7 P a-left7 ; Get the current position P of the 4th joint of the left arm of the main arm in the coordinate system of the main arm base. left4 This position is obtained by a homogeneous transformation of the forward kinematic model of the main arm and the left arm, and the position P is... left4 Along the normal plane P a-left Translate to the collinear axis L of the main arm and left arm a-left The translation vector is ΔP left4 And ΔP left4 Satisfying ΔP left4 ·Z a-left0 =0, the position P of the virtual point at the left elbow v-left4 To translate to L a-left A point on the [plane] whose coordinates satisfy: P v-left4 =P left4 +ΔP left4 This determines the position P of the virtual point on the left elbow. v-left4 Similarly, the position P of the virtual point at the left end can be determined. v-left7 The virtual point P of the right elbow v-right4 and the position of the virtual point P at the right end v-right7 ; When the slave arm is in the zero position, all joint angles are 0°. At this time, the origin of the slave arm base coordinate system, the origin of the elbow coordinate system of the 4th joint, and the origin of the end coordinate system of the 7th joint are on the same straight line. This straight line is defined as the zero axis of the slave arm. The X-axis of the elbow and end coordinate systems are in the same direction as the zero axis, the Z-axis are perpendicular to the zero axis and point in the same direction, and the Y-axis is determined by the right-hand rule. Assume that the rotation matrices of the left arm's fourth joint (left_Link4), left arm's seventh joint (left_Link7), right arm's fourth joint (right_Link4), and right arm's seventh joint (right_Link7) in the zero-position state are R. L4,0 R L7,0 R R4,0 and R R7,0 In the same reference coordinate system, the virtual points of the left elbow, left end, right elbow, and right end of the main arm are respectively aligned with the poses of the slave arm's left_Link4, left_Link7, right_Link4, and right_Link7 in the zero-position state, that is: R v-left4 =R L4,0 R v-left7 =R L7,0 R v-right4 =R R4,0 R v-right7 =R R7,0 Among them, R v-left4 R v-left7 R v-right4 and R v-right7 These represent the postures of the virtual points at the left elbow, left hand tip, right elbow, and right hand tip, respectively.

7. The master-slave heterogeneous dual-arm teleoperation mapping method according to claim 6, characterized in that, Step 2.2 specifically involves: Using the master-slave arm zero-position state determined in step 2.1 as the sole measurement benchmark, determine the master arm's upper arm length L. m1 Length L of main arm and forearm m2 From the length of the upper arm L s1 From the length of the forearm L s2 This allows us to determine the scaling factors for the upper arm and forearm: Define the main arm base coordinate system O m -X m Y m Z m and from the arm base coordinate system O s -X s Y s Z s The homogeneous transformation matrix between the two is T. ms Defined under the main arm's motion state: Main arm, left arm, upper arm vector Main arm, left arm, forearm vector Among them, P mLO The coordinates of the origin point of the main arm and left arm base are P. mL4 Virtual point location of the left elbow of the main arm, P mL7 The virtual point position at the left end of the main arm; the vector of the right upper arm of the main arm. Main arm, right arm, forearm vector Among them, P mRO The coordinates of the origin point of the main arm and right arm base are P. mR4 The virtual point position of the right elbow of the main arm, P mR7 The virtual point position at the right end of the main arm; Transform the vectors of the main arm, left arm, and upper arm to the coordinate system of the slave arm. Then from the left_Link4 position of the arm: Among them, P sLO The coordinates of the origin point of the left arm base are given; Transform the vectors of the main arm, left arm, and forearm to the slave arm coordinate system. Then from the left_Link7 position of the arm: Similarly, the position of right_Link4 and right_Link7 is obtained as P. sR4 and P sR7 ; Based on the alignment relationship in step 2.1, the pose of the master arm virtual point is consistent with the zero-position pose of the corresponding joint of the slave arm. Therefore, the target pose of the slave arm directly inherits the pose of the master arm virtual point, defined as follows: Rotation matrix R from the target pose of arm left_Link4 sL4 =R mL4 , where R mL4 The attitude rotation matrix of the virtual point at the left elbow of the main arm; Rotation matrix R from the target pose of arm left_Link7 sL4 =R mL4 , where R mL7 The attitude rotation matrix of the virtual point at the left end of the main arm; From the target pose rotation matrix R of arm_Link4 sR4 =R mR4 , where R mR4 The attitude rotation matrix of the virtual point at the right elbow of the main arm; From the target attitude rotation matrix R of arm_right_Link7 sR7 =R mR7 , where R mR7 The attitude rotation matrix of the virtual point at the right end of the main arm.

8. The master-slave heterogeneous dual-arm teleoperation mapping method according to claim 7, characterized in that, Step 3 specifically includes: Step 3.1: Obtain the current joint angles of the left and right arms by using the joint detection component of the arm, and simultaneously retrieve the pre-set mechanical movement limit range of each joint of the arm; combine the target poses of the fourth and seventh joints of the left and right arms obtained in Step 2.2, and calculate the arm shape angles of the left and right arms respectively. Step 3.2: Use the calculated arm angle as redundant degrees of freedom to build an inverse kinematics model of the slave arm; combine the joint limit requirements and target pose of the slave arm in the model to calculate the target angles that each joint of the left and right arms of the slave arm needs to achieve.

9. The master-slave heterogeneous dual-arm teleoperation mapping method according to claim 8, characterized in that, Step 3.1 specifically involves: Define the key structural parameters of the left arm: the fixed length L1 from the left arm base to the shoulder, the total length L2 of the link from the shoulder to the elbow, the total length L3 of the link from the elbow to the wrist, and the total length L4 of the link from the wrist to the end effector; and obtain the current joint angle of the left arm through a joint encoder, and retrieve the limit range r of each joint. lim =[[r 1min ,r 1max ],[r 2min ,r 2max ],...,[r 7min ,r 7max ]]; Establish the coordinate system {B} of the left arm base. L }, take the coordinates obtained in step 2.2 from the arm base coordinate system O s -X s Y s Z s From the position P of left_Link4 on the lower arm sL4 Posture R sL4 and from the position P of arm left_Link7 sL7 Posture R sL7 Transform to the coordinate system of the left arm base {B L Under}, that is, coordinate system {B L }From the position of left_Link4 on the lower arm attitude and from the position of left_Link7 on the arm attitude Wrist position in base coordinate system in The Z-axis unit vector of the left arm defines the position of the shoulder in the left arm base coordinate system. The vector from the shoulder to the wrist Its mold length is: If nX sw >L2+L3+10 -6 (10 -6 (for numerical error threshold) or nX sw <|L2-L3|-10 -6 If the end-target position is determined to be outside the workspace of the slave arm, the calculation is terminated. Based on the law of cosines, from the angle ψ of the left arm... L The calculation formula is as follows: To avoid numerical overflow, the range of the cosine value is clipped. Similarly, the arm-shape angle ψ of the right arm is obtained. R .

10. The master-slave heterogeneous dual-arm teleoperation mapping method according to claim 8, characterized in that, Step 3.2 specifically involves: The core input parameters for designing the inverse kinematic model of the left arm include: the target pose of the left arm (left_Link7) obtained from step 3.1, and the arm shape angle ψ of the left arm. L ; The arm structure parameters include the fixed length L1 from the left arm base to the shoulder, the total length L2 of the link from the shoulder to the elbow, the total length L3 of the link from the elbow to the wrist, and the total length L4 of the link from the wrist to the end link. Reference angle of the left arm joint Used to assist in selecting continuous solutions from multiple solutions; From the arm joint limit r lim =[[r 1min ,r 1max ],[r 2min ,r 2max ],...,[r 7min ,r 7max ]] represents the minimum / maximum angle of motion for each joint; From the left arm of the arm, the angle ψ L Combined with joint limitation [r 4min ,r 4max and reference angle r ref4 A reasonable solution is obtained: r4; Define position vector a: Solve for the rotation matrix R of the left elbow of the secondary arm in the reference plane when the angle of the third joint is 0. 03_0 ,satisfy: R 03_0 ·a=X sw Calculate the normalized shoulder-wrist vector And construct its anti-forming matrix satisfy Using the angle ψ of the left arm of the arm L Correcting the rotation matrix R from the left elbow of the arm 03_0 The actual rotation matrix R from the left elbow of the arm is obtained. 03 : By R 03 [2,2]=cos(r2), combined with joint limitation [r 2min ,r 2max and reference angle r ref2 A reasonable solution r2 is obtained; If |r2|<10 -6 : Then r2 = 0, and r1 = arctan2(R 03 [1,0],R 03 [1,1])-r ref [2], r3=r ref [2], and normalize r1 to [-π,π]: r1=(r1+π)%(2π)-π; Otherwise, if sin(r2) > 0: r1=arctan2(R 03 [1,2],R 03 [0,2]),r3=arctan2(R 03 [2,1],-R 03 [2,0]); If sin(r2) < 0: r1=arctan2(-R 03 [1,2],-R 03 [0,2]),r3=arctan2(-R 03 [2,1],R 03 [2,0]); Construct a rotation matrix R from the left arm wrist to the distal end. w→ee : By R w→ee [0,2]=cos(r6), combined with joint limitation [r 6min ,r 6max and reference angle r ref6 A reasonable explanation is obtained: r6; If |r6| < 10 -6 : Then r6 = 0, and r5 = arctan2(R w→ee [2,0],R w→ee [2,1])-r ref [6], r7=r ref [6] and normalize r5 to [-π,π]: r5=(r5+π)%(2π)-π; Otherwise, if sin(r6) > 0: r5=arctan2(R w→ee [2,2],R w→ee [2,2]),r7=arctan2(R w→ee [0,1],-R w→ee [0,0]); If sin(r6) < 0: r5=arctan2(-R w→ee [2,2],-R w→ee [1,2]),r7=arctan2(-R w→ee [0,1],R w→ee [0,0]); Finally, the target angles of each joint in the left arm that meet the requirements are obtained as r = [r1, r2, r3, r4, r5, r6, r7]. Similarly, the above process is repeated to calculate the target angles of each joint in the right arm, thus completing the teleoperation mapping of the master-slave heterogeneous dual arms.

Images