End position error compensation control method for laparoscopic surgery robot

Abstract

The invention discloses a laparoscopic surgery robot tail end position error compensation control method which comprises the steps that S1, a laparoscopic holding arm and an instrument arm of a laparoscopic surgery robot are dragged to a plurality of random positions in a working space, and errors of the tail end positions of all mechanical arms at all the random positions are obtained; s2, according to the error obtained in S1, obtaining the error of each connecting rod parameter; s3, calculating the tail end position of the endoscope holding arm and the tail end position of the instrument arm after the error is considered according to the error obtained in the S2; acquiring a target end position of the instrument arm, thereby calculating a target value of a relative motion joint of the instrument arm; and S4, calculating the total error of the relative motion joint of the instrument arm, calculating the execution value of the relative motion joint of the instrument arm after the error is considered in combination with the target value obtained in the S3, and inversely solving the execution value and issuing the execution value to a driver for execution. According to the method, the total error can be compensated under the condition of not iteratively solving the error joint angle.

Description

Technical Field

[0001] The present invention relates to the technical field of robot control, and particularly to a method for compensating and controlling the end position error of a laparoscopic surgical robot. Background Art

[0002] When existing laparoscopic surgical robots perform minimally invasive surgeries, they will encounter spaces with relatively small surgical lesion ranges. At the same time, the end of the surgical robot arm needs to perform delicate operations such as cutting, stretching, and suturing on organs and their blood vessels, etc., and it is necessary to accurately control the position of the tool at the end of the arm. Also, when the end instrument of the robot arm is not within the field of view, the doctor needs to be reminded of the position of the instrument relative to the endoscope to facilitate finding the position of the instrument for the next movement. Additionally, when the two arms of the laparoscope are operating simultaneously, it is necessary to distinguish between the left and right arms and add visual icon markings to facilitate operating which arm; all these functions require accurate absolute positioning of the robotic arm. However, most of the end position errors of the robot are caused by inaccuracies between the kinematic parameters of each link and the theoretical values. Therefore, it is necessary to compensate for the end position error to improve the position accuracy of the end of the robotic arm.

[0003] Since the kinematic modeling of the robotic arm is calculated according to the theoretical model, however, the actual values deviate from the theoretical values due to manufacturing errors, assembly errors, zero-position deviations, etc. of the links. These parameters mainly include link length error, link twist angle error, link offset error, and link rotation angle error. By sampling the actual position and the theoretical position of the end of the robotic arm, the errors of these main parameters are identified using the least squares method. Traditional robotic arms generally have six motion joints and no relatively fixed joints, and the number of error identification and compensation joints is the same. For laparoscopic surgical robots, as Figure 1 shown, it generally has three robotic arms, and corresponding end tools or instruments are installed on each robotic arm according to requirements. Therefore, each robotic arm of the laparoscopic surgical robot plus the end tools or instruments on its end generally has 11 joints. Among them, the first 5 joints (J1 - J5) are used for preoperative positioning and need to be locked during the operation, and the last 6 joints (J6 - J11) are relative motion joints and are in motion during the operation. How to consider the error identification and compensation of relatively fixed joints and motion joints is a problem. In addition, when performing differential inverse solution compensation for the last 6 joints of the robotic arm of a laparoscopic surgical robot compared with the local error compensation of the joints of a traditional robotic arm, iterative solutions are required, and the number of iterations will also affect the control accuracy. Summary of the Invention

[0004] Objective of the Invention: Aiming at the above deficiencies, the present invention proposes a method for compensating the end position error of a laparoscopic surgical robot. By identifying the error and considering all joint errors, and separately considering the moving joints in the front and rear sections during position compensation, the overall error can be compensated without iteratively solving the error joint angles.

[0005] Technical Solution:

[0006] The present invention provides a method for compensating the end position error of a laparoscopic surgical robot, including the steps of:

[0007] S1. Drag the lens-holding arm and the instrument arm of the laparoscopic surgical robot to multiple random positions in the working space respectively, obtain the theoretical values and actual values of the end positions of each robotic arm at each random position, and thereby calculate the errors of the end positions of each robotic arm at each random position.

[0008] S2. According to the errors obtained in S1, obtain the Jacobian matrix of the end position of each robotic arm at each random position with respect to its respective link parameters, thereby construct a parameter identification matrix equation and solve it to obtain the errors of each link parameter.

[0009] S3. Calculate the end position of the lens-holding arm considering the error and the end position after the instrument arm is positioned according to the errors obtained in S2; obtain the target end position of the instrument arm, thereby calculate the relative position relationship between the end position of the lens-holding arm and the target end position of the instrument arm, and calculate the target value of the relative moving joint of the instrument arm based on this.

[0010] S4. Calculate the overall error of the relative moving joint of the instrument arm, combine with the target value obtained in S3, calculate the execution value of the relative moving joint of the instrument arm considering the error, and inverse-solve and execute it.

[0011] Specifically, in S1, obtain the actual joint values of each joint of each robotic arm at each random position, calculate the theoretical values of the end positions of each robotic arm at each random position through the forward kinematics of the robotic arm, and at the same time measure the actual values of the end positions of each robotic arm at each random position.

[0012] More specifically, in S1, according to the actual joint values of each joint of each robotic arm at each random position, express the end position of each robotic arm at each random position as a function of each link parameter through the classical DH modeling method, and differentiate this function to obtain the position error of the end position of each robotic arm at each random position.

[0013] Furthermore, in S2, the construction of the parameter identification matrix equation and the solution to obtain the errors of each link parameter value are specifically as follows:

[0014] The theoretical value of the end position of each robotic arm at each random position is expressed as a function of each link parameter as:

[0015] P = F(α, a, θ, d)

[0016] Wherein, P represents the theoretical value of the end position of a robotic arm at a certain position, α represents the twist angle of adjacent connecting rods, a represents the distance of the common perpendicular of adjacent connecting rods, θ represents the deflection angle of adjacent connecting rods, d represents the offset distance of adjacent connecting rods, and each connecting rod is connected by joints to form an operable robotic arm;

[0017] Differentiating the above formula, the relationship between the error Δp of the end position of the robotic arm at this position and the errors Δα, Δa, Δθ, Δd of each connecting rod parameter is obtained as follows:

[0018]

[0019] Transform the error Δp of the end position of each robotic arm at each random position into components in each direction of the base coordinate system as follows:

[0020]

[0021] Wherein, Δp x , Δp y , Δp z are respectively the components of the error Δp of each joint value in the x, y, and z directions of the base coordinate system, n represents the number of joints of a certain robotic arm, and i represents the i-th position of the robotic arm;

[0022] Then construct the parameter identification matrix equation:

[0023] AΔX = B

[0024] Wherein, A represents the Jacobian matrix of the end position of each robotic arm at each random position with respect to its respective connecting rod parameters, as follows:

[0025]

[0026] Wherein, m represents the number of randomly adjusted positions of S1;

[0027] ΔX represents the unknown variable to be solved, as follows:

[0028] ΔX = (Δα1 … Δα n Δa1 … Δa n Δθ1 … Δθ n Δd1 … Δd n ) T

[0029] B represents the transpose of the error between the theoretical value and the actual value of the end position of the robotic arm at each random position, as follows:

[0030] B = (Δp1x Δp 1y Δp 1z … Δp mx Δp my Δp mz ) T

[0031] By solving the parameter identification matrix equation constructed above, the errors of the link parameters of the robotic arm can be obtained.

[0032] Specifically, S3 includes:

[0033] S31. After the laparoscopic surgical robot completes the positioning, obtain the joint values of the endoscope-holding arm. According to the errors of the link parameters of the endoscope-holding arm obtained in S2, calculate the joint values of its fixed joints considering the errors, and then calculate the end position of the endoscope-holding arm considering the errors.

[0034] S32. According to the errors of the link parameters of the two instrument arms obtained in S2, obtain the joint values of their fixed joints considering the errors, and then calculate the end positions of the instrument arms considering the errors of their fixed joints.

[0035] S33. Obtain the target pose at the end of the instrument arm to get the target end position of the instrument arm. Based on this, calculate the relative position relationship between the end position of the endoscope-holding arm and the target end position of the instrument arm, and combine S31 and S32 to calculate the target values of the relative motion joints of the instrument arm.

[0036] Specifically, in S4, calculating the overall error of the relative motion joints of the instrument arm is specifically as follows:

[0037] The joint value of a certain relative motion joint is:

[0038]

[0039] Among them, α qv represents the twist angle of the adjacent links of this relative motion joint, a qv represents the distance of the common perpendicular of the adjacent links of this relative motion joint, θ qv represents the deflection angle of the adjacent links of this relative motion joint;

[0040] Then the position dT qv error of this relative motion joint is:

[0041]

[0042] Among them, d qv represents the offset distance of the adjacent links of this relative motion joint;

[0043]

[0044] The actual value of the relative motion joint of the instrument arm is ∏T qtv , then the overall error ΔZ of the relative motion joint of the instrument arm = d(∏T qtv ).

[0045] Beneficial effects: In view of the problems of position error and compensation accuracy of the multi-joint and multi-manipulator structure of the laparoscopic surgical robot, the present invention proposes to identify errors to consider all joint errors; in position error compensation, fixed joints and relative motion joints are considered separately, and the error compensation is more accurate. In addition, in view of the problem of the large number of iterations in the traditional local error compensation of joints, the overall error can be compensated without iteratively solving the error joint angle, improving the operation efficiency of the program. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] In order to more clearly illustrate the technical solutions in the present invention, the following will briefly introduce the drawings required for the description of the embodiments. Obviously, the drawings in the following description are only embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0047] Figure 1 It is a flowchart of the end position error compensation control method for the laparoscopic surgical robot of the present invention.

[0048] Figure 2 It is a structural example diagram of a laparoscopic robot and a certain manipulator on it. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0049] To make the objectives, technical solutions, and advantages of the present invention clearer and more understandable, the following further elaborates on the present application in conjunction with specific embodiments and with reference to the drawings.

[0050] It should be noted that unless otherwise defined, the technical terms or scientific terms used in the embodiments of the present invention should have the ordinary meaning understood by those of ordinary skill in the art to which the present invention belongs. Words such as "including" or "comprising" mean that the elements or objects appearing before the word cover the elements or objects listed after the word and their equivalents, without excluding other elements or objects. Words such as "connected" or "linked" are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect.

[0051] The structure of the laparoscopic surgical robot and a certain manipulator on it is as Figure 2As shown, generally, a laparoscopic surgical robot is provided with four robotic arms, all of which are mounted on a suspension plate. Endoscopes or instruments are installed on the ends of three of the robotic arms as required to serve as the lens-holding arm or the instrument arm; the remaining robotic arm serves as a redundant robotic arm to be used as an alternative in case one of the aforementioned three robotic arms has a problem.

[0052] The flow of the end position error compensation control method for the laparoscopic surgical robot of the present invention is as Figure 1 shown, and includes:

[0053] S1. Drag the lens-holding arm and the instrument arm of the laparoscopic surgical robot to multiple random positions in the working space respectively, obtain the actual joint values of each joint of each robotic arm at each random position, calculate the theoretical values of the end positions of each robotic arm at each random position through forward kinematics of the robotic arm, and at the same time measure the actual values of the end positions of each robotic arm at each random position, thereby calculating the errors of the end positions of each robotic arm at each random position.

[0054] In the present invention, the actual joint value of each joint of a certain robotic arm is specifically its rotation angle, which can be obtained through an encoder installed thereon.

[0055] In the present invention, the end positions of each robotic arm at each random position are defined based on the suspension plate of the laparoscopic surgical robot; in the present invention, a base coordinate system can be established with the suspension plate as the reference, and thus the coordinates of the end positions of each robotic arm at each random position in the base coordinate system can be obtained.

[0056] Specifically, drag the lens-holding arm and the instrument arm of the laparoscopic surgical robot to multiple random positions in the working space respectively, obtain the actual joint values of each joint of each robotic arm at each random position, thereby calculating the theoretical coordinates of the end positions of each robotic arm at each random position in the base coordinate system through forward kinematics of the robotic arm, that is, the theoretical values of the end positions of each robotic arm at each random position; then, measure the actual coordinates of the end positions of each robotic arm at each random position in the base coordinate system through measurement, that is, the actual values of the end positions of each robotic arm at each random position, thereby calculating the errors of the end positions of each robotic arm at each random position.

[0057] In the present invention, according to the actual joint values of each joint of each robotic arm at each random position obtained above, the theoretical values of the end positions of each robotic arm at each random position are expressed as a function F of each link parameter through the classical DH modeling method, as follows:

[0058] P = F(α, a, θ, d)

[0059] Wherein, P represents the theoretical value of the end position of a robotic arm at a certain position, α represents the twist angle of adjacent connecting rods, a represents the distance of the common perpendicular of adjacent connecting rods, θ represents the deflection angle of adjacent connecting rods, d represents the offset distance of adjacent connecting rods, and each connecting rod is connected by joints to form an operable robotic arm.

[0060] Specifically, in a specific embodiment of the present invention, among the aforementioned three robotic arms, generally an endoscope is installed at the end of the robotic arm in the middle position, and this robotic arm is the endoscope-holding arm. Instruments are installed at the ends of the other two robotic arms, and these two robotic arms are the instrument arms. Referring to Figure 2 , the endoscope-holding arm is installed on the suspension plate through 8 joints J1 to J8. Among them, the joints J1 to J8 are respectively the turntable rotation joint J1, the cross-arm telescopic joint J2, the vertical-arm lifting joint J3, the vertical-arm rotation joint J4, the pre-pitching joint J5, the RCM-arm yaw joint J6, the RCM-arm pitching joint J7, and the up-and-down movement joint J8. Since the endoscope only has axial movement, that is, the endoscope-holding arm also includes the endoscope axial movement joint J9. That is to say, the endoscope-holding arm has a total of 9 joints. During the operation, the joints on the endoscope-holding arm do not need to move and are all fixed joints. Therefore, the present invention can use the endoscope-holding arm as a reference to calculate the positions and errors of the two instrument arms respectively; the instrument arm has 11 joints, and its first 8 joints J1 to J8 are the same as the first 8 joints J1 to J8 of the endoscope-holding arm. In addition, the other 3 joints are respectively the instrument axial movement joint J9, the instrument pitching joint J10, and the instrument yaw joint J11.

[0061] S2. According to the errors of the end positions of each robotic arm at each random position obtained in S1, obtain the Jacobian matrix of the end positions of each robotic arm at each random position with respect to its respective connecting rod parameters, thereby constructing a parameter identification matrix equation and solving it to obtain the errors of each connecting rod parameter;

[0062] Specifically, given the error Δp of the end position of each robotic arm at each random position obtained in S1, and at the same time differentiating the theoretical value P of the end position of the robotic arm, the relationship between the position error Δp of the end position of the robotic arm at this position and the errors Δα, Δa, Δθ, Δd of each connecting rod parameter can be obtained as follows:

[0063]

[0064] Thus, the errors of the end positions of each robotic arm at each random position obtained in S1 are transformed into components in each direction of the base coordinate system as follows:

[0065]

[0066] Wherein, Δp x , Δp y , Δp zThey are the components of the position error Δp at the end position in the x, y, and z directions of the base coordinate system. n represents the number of joints of a certain robotic arm, and i represents the i-th position of the robotic arm.

[0067] Then, the parameter identification matrix equation can be constructed:

[0068] AΔX = B

[0069] Among them, A represents the Jacobian matrix of the end position of each robotic arm at each random position with respect to its respective link parameters, as follows:

[0070]

[0071] Among them, m represents the number of random positions adjusted by S1;

[0072] ΔX represents the unknown variable to be solved, as follows:

[0073] ΔX = (Δα1 … Δα n Δa1 … Δa n Δθ1 … Δθ n Δd1 … Δd n ) T

[0074] B represents the transpose of the error between the theoretical value and the actual value of the end position of the robotic arm at each random position, as follows:

[0075] B = (Δp 1x Δp 1y Δp 1z … Δp mx Δp my Δp mz ) T

[0076] Thus, the parameter identification matrix equation constructed above can be solved, and then the errors of the respective link parameters of the robotic arm can be calculated;

[0077] In the present invention, the least squares method or the pseudo-inverse method can be used to solve the parameter identification matrix equation constructed above. Further, before solving the parameter identification matrix equation constructed above, it is also necessary to determine whether the rank of the Jacobian matrix of the end position of each robotic arm at each random position with respect to its respective link parameters is full rank to facilitate the solution of the foregoing system of equations.

[0078] Repeating this step, the errors of the respective link parameters of the lens-holding arm and the two instrument arms can be obtained.

[0079] S3. Calculate the end position of the endoscope - holding arm considering errors and the end position of the instrument arm after positioning based on the errors of the link parameters of each manipulator obtained in S2; obtain the target end position of the instrument arm, thereby calculating the relative position relationship between the end position of the endoscope - holding arm and the target end position of the instrument arm, and calculating the target values of the relative - motion joints of the instrument arm accordingly.

[0080] In the instrument arm of the laparoscopic surgical robot in the specific embodiment of the present invention, the first several joints are used for preoperative positioning and need to be locked during the operation, that is, fixed joints; the last several joints are relative - motion joints and are in motion during the operation, that is, relative - motion joints. Therefore, the endoscope - holding arm is relatively fixed after preoperative positioning, and the first several joints of the instrument arm are also relatively fixed after preoperative positioning. The last several joints of the instrument arm are relative - motion joints. Therefore, different from the traditional robot compensation method, separate compensation needs to be considered. Thus, the present invention uniformly processes the fixed joints and then processes the relative - motion joints.

[0081] Specifically, it includes:

[0082] S31. After the laparoscopic surgical robot completes positioning, obtain the joint values of the endoscope - holding arm, calculate the joint values of its fixed joints considering the errors of the link parameters of the endoscope - holding arm obtained in S2, and then calculate the end position of the endoscope - holding arm considering the errors.

[0083] In the present invention, the pose of the j - th joint of the endoscope - holding arm considering the error is T cj , then the end position of the endoscope - holding arm considering the error is Z c =∏T cj ; where c represents the endoscope - holding arm, and all joints of the endoscope - holding arm are fixed joints.

[0084] In the specific embodiment of the present invention, j ∈ [1,9], Z c =T c1 T c2 T c3 T c4 T c5 T c6 T c7 T c8 T c9 .

[0085] S32. According to the errors of the link parameters of the two instrument arms obtained in S2, the joint values of their fixed joints considering the errors can be obtained, and then the end position after their positioning can be calculated, that is, the end position of the instrument arm considering the errors of its fixed joints.

[0086] In the present invention, the first several joints of the instrument arm are fixed joints, and the joint values of these joints considering the errors are defined as Tqug , and the remaining joints are relative motion joints, and their joint values are T qv , then the end position Z of the instrument arm q = ∏T qug ∏T qv , thus, after obtaining the joint values of the fixed joints of the instrument arm considering the error, the end position of the instrument arm considering the error of the fixed joints of the instrument arm can be calculated, that is, the end position after the instrument arm is positioned; where q represents the instrument arm, u represents one of the fixed joints of the instrument arm, and v represents one of the relative motion joints of the instrument arm

[0087] In a specific embodiment of the present invention, the instrument arm of the laparoscopic surgical robot has a total of 11 joints. The first 5 joints are used for preoperative positioning and need to be locked during the operation, that is, fixed joints, and the corresponding connecting rods are also fixed; the last 6 joints are relative motion joints and are moving during the operation, that is, relative motion joints, that is, u ∈ [1, 5], v ∈ [6, 11], then the end position Z of the instrument arm q = T q1g T q2g T q3g T q4g T q5g T q6 T q7 T q8 T q9 T q10 T q11 .

[0088] S33. Obtain the target pose at the end of the instrument arm, obtain the target end position of the instrument arm, calculate the relative position relationship between the end position of the endoscope holding arm and the target end position of the instrument arm accordingly, and calculate the target value of the relative motion joint of the instrument arm in combination with S31 and S32

[0089] Specifically, in a specific embodiment of the present invention, as follows

[0090] Obtain the target pose at the end of the instrument arm, and the target end position Z of the instrument arm can be obtained qt , so as to calculate the relative position relationship T between the end position of the endoscope holding arm and the target end position of the instrument arm r ;

[0091] Among them, the joint value of the fixed joint of the instrument arm considering the error is T qug , which is locked during the operation, and its relative motion joint needs to be compensated and controlled. Define the joint value of the relative motion joint at this time as T qtv , then T r = Z qt Z c -1 = ∏Tqug ∏T qtv Z c -1 , and then obtain ∏T qtv =(∏T qug ) -1 T r Z c , that is, obtain the target value of the relative motion joint of the robotic arm, which is obtained by multiplying the expected values of the relative motion joints without considering errors;

[0092] In this embodiment, T r =(T q1g T q2g T q3g T q4g T q5g T qt6 T qt7 T qt8 T qt9 T qt10 T qt11 )Z c -1 , and ∏T qtv =T qt6 T qt7 T qt8 T qt9 T qt10 T qt11 =(T q1g T q2g T q3g T q4g T q5g ) -1 T r Z c .

[0093] S4. Calculate the overall error of the relative motion joints of the robotic arm. Combining with the target value of the relative motion joints of the robotic arm obtained in S3, the execution value of the relative motion joints of the robotic arm considering errors can be calculated, and its inverse solution is sent to the driver for execution;

[0094] In the present invention, when the target end position Z qt of the robotic arm is sent, after inverse solution to the joints, it is certain that the target end position Z qt cannot be reached, and there is always an overall error ΔZ. The actual reached position is Z qt +ΔZ. If the specific error size is known in advance, then the position Z qt -ΔZ considering the error of the target position, that is, the execution value of the relative motion joints of the robotic arm considering errors, can be used as the command value for inverse solution, and then sent to the driver, so that it just reaches the target position Z qt =Z q-ΔZ + ΔZ。

[0095] Specifically, the overall error ΔZ is calculated as follows:

[0096] The joint value of a certain relative motion joint is:

[0097]

[0098] Among them, α qv represents the twist angle of the adjacent link of this relative motion joint, a qv represents the distance of the common perpendicular of the adjacent links of this relative motion joint, θ qv represents the deflection angle of the adjacent links of this relative motion joint;

[0099] Then the position dT qv error of this relative motion joint is:

[0100]

[0101] Among them, d qv represents the offset distance of the adjacent links of this relative motion joint;

[0102]

[0103] The actual value of the relative motion joint of the instrument arm is ∏T qtv , then the overall error ΔZ = d(∏T qtv ).

[0104] In the specific embodiment of the present invention, ∏T qtv = T qt6 T qt7 T qt8 T qt9 T qt10 T qt11 , then

[0105] In the above process, the overall error ΔT is obtained by differentiating the matrix. The error is considered in the Cartesian space and not compensated at the joint end, thus avoiding the iterative process of solving equations by the traditional method and improving the efficiency.

[0106] The present invention aims at the problems of position error and compensation accuracy of the multi-joint and multi-manipulator structure of the laparoscopic surgical robot, proposes to identify the error to consider all joint errors; in the position error compensation, the fixed joints and relative motion joints are considered separately, and the error compensation is more accurate. In addition, aiming at the problem of the large number of iterations of the traditional joint local error compensation, the overall error can be compensated without iteratively solving the error joint angle, improving the running efficiency of the program.

[0107] Those of ordinary skill in the art should understand that the discussion of any of the above embodiments is only exemplary and is not intended to imply that the scope of the present invention (including the claims) is limited to these examples; under the concept of the present invention, the technical features in the above embodiments or different embodiments can also be combined, the steps can be implemented in any order, and there are many other variations in different aspects of the embodiments of the present invention as described above, and they are not provided in detail for the sake of brevity.

[0108] Embodiments of the present invention are intended to cover all such substitutions, modifications, and variations that fall within the broad scope of the appended claims. Therefore, any omissions, modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the embodiments of the present invention shall be included within the protection scope of the present invention.

Claims

1. A laparoscopic surgical robot end position error compensation control method, characterized in that: Includes steps: S1. Drag the scope holding arm and the instrument arm of the laparoscopic surgical robot to multiple random positions in the workspace, obtain the theoretical value and actual value of the end position of each robotic arm at each random position, and calculate the error of the end position of each robotic arm at each random position; S2. According to the error obtained in S1, the Jacobian matrix of the end position of each robot arm at each random position with respect to its connecting rod parameters is obtained, thereby constructing a parameter identification matrix equation and solving it to obtain the error of each connecting rod parameter; S3, calculating the end position of the scope holding arm after the error is taken into account and the end position of the instrument arm after the position is swung according to the error obtained in S2; obtaining the target end position of the instrument arm, thereby calculating the relative position relationship between the end position of the scope holding arm and the target end position of the instrument arm, and calculating the target value of the relative motion joint of the instrument arm accordingly; S4. Calculate the overall error of the relative motion joint of the instrument arm, combine it with the target value obtained in S3, calculate the execution value of the relative motion joint of the instrument arm after considering the error, inversely solve it and execute it.

2. The laparoscopic surgical robot end position error compensation control method according to claim 1, characterized in that: In S1, the actual joint value of each joint of each robotic arm at each random position is obtained, the theoretical value of the end position of each robotic arm at each random position is obtained by forward kinematics calculation of the robotic arm, and the actual value of the end position of each robotic arm at each random position is obtained by measurement.

3. The laparoscopic surgical robot end position error compensation control method according to claim 2, characterized in that: In S1, according to the actual joint values ​​of each joint of each robotic arm at each random position, the end position of each robotic arm at each random position is expressed as a function of each connecting rod parameter through the classic DH modeling method. By differentiating the function, the position error of the end position of each robotic arm at each random position can be obtained.

4. The laparoscopic surgical robot end position error compensation control method according to claim 3, characterized in that: In S2, the parameter identification matrix equation is constructed and solved to obtain the error of each connecting rod parameter value, which is as follows: The theoretical value of the end position of each robot arm at each random position is expressed as a function of each link parameter: P=F(α,a,θ,d) Among them, P represents the theoretical value of the end position of a certain robot arm at a certain position, α represents the torsion angle of adjacent links, a represents the distance between the common perpendicular lines of adjacent links, θ represents the deflection angle of adjacent links, d represents the offset distance of adjacent links, and each link is connected to form a movable robot arm through a joint; By differentiating the above formula, we can obtain the relationship between the error Δp ​​of the end position of the manipulator and the errors Δα, Δa, Δθ, and Δd of each link parameter, as follows: The error Δp ​​of the end position of each manipulator at each random position is transformed into components in each direction of the base coordinate system as follows: Among them, Δp x , Δp y , Δp z are the components of the error Δp ​​of each joint value in the x, y, and z directions of the base coordinate system, n represents the number of joints of a certain robot arm, and i represents the i-th position of the robot arm; Then construct the parameter identification matrix equation: AΔX=B Among them, A represents the Jacobian matrix of the end position of each robot arm at each random position with respect to its link parameters, as follows: Where m represents the number of random positions adjusted by S1; ΔX represents the unknown variable that needs to be solved, as follows: ΔX=(Δα1 … Δα) n Δa1 … Δa n Δθ1 … Δθ n Δd1 … Δd n ) T B represents the transposition of the error between the theoretical value and the actual value of the end position of the robot arm at each random position, as follows: B=(Δp 1x Δp 1y Δp 1z … Δp mx Δp my Δp mz ) T By solving the parameter identification matrix equation constructed above, the error of each link parameter of the robot arm is obtained.

5. The laparoscopic surgical robot end position error compensation control method according to claim 1, characterized in that: The S3 includes: S31, after the laparoscopic surgical robot has completed positioning, the joint values ​​of the scope holding arm are obtained, and according to the errors of the connecting rod parameters of the scope holding arm obtained in S2, the joint values ​​of the fixed joints after the errors are taken into account are calculated, and then the end position of the scope holding arm after the errors are calculated; S32, according to the errors of the connecting rod parameters of the two instrument arms obtained in S2, obtain the joint values ​​of the fixed joints after the errors are taken into account, and then calculate the end positions of the instrument arms after the errors are taken into account; S33, obtaining the target posture of the end of the instrument arm, obtaining the target end position of the instrument arm, and calculating the relative position relationship between the end position of the mirror holding arm and the target end position of the instrument arm, and combining S31 and S32 to calculate the target value of the relative motion joint of the instrument arm.

6. The laparoscopic surgical robot end position error compensation control method according to claim 1, characterized in that: In S4, the overall error of the relative motion joint of the instrument arm is calculated, specifically: The joint value of a relative motion joint is: Among them, α qv represents the torsion angle of the adjacent links of the relative motion joint, a qv Represents the distance between the common perpendicular lines of the adjacent links of the relative motion joint, θ qv It represents the deflection angle of the adjacent link of the relative motion joint; Then the position dT of the relative motion joint qv The error is: Among them, d qv Represents the offset distance of the adjacent links of the relative motion joint; The actual value of the relative motion joint of the instrument arm is ∏T qtv , then the overall error of the relative motion joint of the instrument arm is ΔZ = d(∏T qtv ).

Images