A laparoscopic surgery robot end position error compensation control method

Abstract

The application discloses a laparoscopic surgery robot end position error compensation control method, comprising the following steps: S1, respectively dragging the mirror holding arm and the instrument arm of the laparoscopic surgery robot to a plurality of random positions in the working space, and acquiring the end position error of each mechanical arm at each random position; S2, acquiring the error of each connecting rod parameter according to the error obtained in S1; S3, calculating the end position of the mirror holding arm after considering the error and the end position of the instrument arm after positioning according to the error obtained in S2; acquiring the target end position of the instrument arm, and thus calculating the target value of the relative motion joint of the instrument arm; S4, calculating the overall error of the relative motion joint of the instrument arm, combining the target value obtained in S3, and calculating the execution value of the relative motion joint of the instrument arm after considering the error, inversely solving and issuing the execution value to the driver for execution. The application can compensate the overall error without iteratively solving the error joint angle.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and in particular to a method for compensating for end-effector position error in a laparoscopic surgical robot. Background Technology

[0002] Existing laparoscopic surgical robots, when performing minimally invasive surgery, often encounter situations where the surgical lesion area is relatively small. Simultaneously, the robot's end effector needs to perform delicate movements such as cutting, stretching, and suturing organs and blood vessels, requiring precise control of the end effector's tool position. Furthermore, when the end effector is not within the field of view, it needs to alert the surgeon to the instrument's position relative to the endoscope, facilitating instrument repositioning and subsequent movement. Additionally, when both laparoscopic arms are operating simultaneously, visual field indicators are needed to distinguish between the left and right arms, making it easier to identify which arm to operate on. All these functions require absolutely accurate robotic arm positioning. However, most end effector position errors are caused by inaccuracies between the kinematic parameters of various links and their theoretical values. Therefore, compensation for end effector position errors is necessary to improve the positional accuracy of the robotic arm's end effector.

[0003] Since the kinematic modeling of robotic arms is based on theoretical calculations, actual values ​​deviate from theoretical values ​​due to manufacturing errors, assembly errors, and zero-position deviations. These deviations primarily include link length error, link torsion angle error, link offset error, and link rotation angle error. By sampling the actual and theoretical positions of the robotic arm's end effector, the least squares method is used to identify the errors in these key parameters. Traditional robotic arms typically have six moving joints and no relatively fixed joints; the number of joints for error identification and compensation is the same. However, for laparoscopic surgical robots, such as... Figure 1 As shown, a laparoscopic surgical robot typically has three robotic arms, each equipped with a corresponding end effector or instrument as needed. Therefore, the robotic arms plus their end effectors or instruments generally have 11 joints. The first five joints (J1-J5) are used for preoperative positioning and need to be locked during surgery, while the last six joints (J6-J11) are relatively movable joints and move during surgery. How to identify and compensate for the errors between relatively fixed and movable joints is a problem. Furthermore, the local joint error compensation of traditional robotic arms requires iterative solutions when performing differential inverse kinematics compensation for the last six joints of the laparoscopic surgical robot's arms, and the number of iterations also affects the control accuracy. Summary of the Invention

[0004] The application aims to provide a laparoscopic surgery robot end position error compensation control method.

[0005] Technical scheme

[0006] The application provides a laparoscopic surgery robot end position error compensation control method, comprising the following steps.

[0007] S1, respectively dragging a mirror holding arm and an instrument arm of the laparoscopic surgery robot to a plurality of random positions in a working space to obtain theoretical values and actual values of end positions of the mechanical arms at the random positions, and calculating errors of the end positions of the mechanical arms at the random positions;

[0008] S2, obtaining Jacobian matrices of the end positions of the mechanical arms at the random positions with respect to link parameters of the mechanical arms according to the errors obtained in S1, constructing a parameter identification matrix equation and solving the equation to obtain errors of the link parameters;

[0009] S3, calculating the end position of the mirror holding arm and the end position of the instrument arm after positioning according to the errors obtained in S2, obtaining a target end position of the instrument arm, calculating a relative position relationship between the end position of the mirror holding arm and the target end position of the instrument arm, and calculating target values of relative motion joints of the instrument arm according to the relative position relationship;

[0010] S4, calculating total errors of the relative motion joints of the instrument arm, combining the target values obtained in S3, calculating execution values of the relative motion joints of the instrument arm after considering the errors, inversely solving the execution values and executing the execution values.

[0011] Specifically, in S1, actual joint values of each joint of each mechanical arm at each random position are obtained, theoretical values of the end positions of the mechanical arms at the random positions are calculated through forward kinematics of the mechanical arms, and actual values of the end positions of the mechanical arms at the random positions are measured.

[0012] More specifically, in S1, the end positions of the mechanical arms at the random positions are expressed as functions of the link parameters according to the actual joint values of each joint of each mechanical arm at each random position through a classical DH modeling method, the functions are differentiated, and position errors of the end positions of the mechanical arms at the random positions are obtained.

[0013] Further, in S2, the parameter identification matrix equation is constructed and solved to obtain errors of the link parameter values, and the errors are specifically as follows:

[0014] The theoretical values of the end positions of the mechanical arms at the random positions expressed as functions of the link parameters are:

[0015] P = F (a, a, 0, d)

[0016] Wherein, P represents the theoretical value of the end position of a certain position of a certain mechanical arm, a represents the torsion angle of the adjacent connecting rod, a represents the distance of the adjacent connecting rod common perpendicular line, 0 represents the deflection angle of the adjacent connecting rod, d represents the offset distance of the adjacent connecting rod, and each connecting rod is connected through the joint into a movable mechanical arm;

[0017] Differential of the above formula, the error Δp of the end position of the mechanical arm at this position and the error Δa, Δa, Δ0, Δd of each connecting rod parameter are as follows:

[0018]

[0019] The error Δp of the end position of each mechanical arm at each random position is transformed into the component in each direction of the base coordinate system, as follows:

[0020]

[0021] Wherein, Δp x , Δp y , Δp z Respectively, the error Δp of each joint value in the x, y, z direction of the base coordinate system, n represents the number of joints of a certain mechanical arm, and i represents the i th position of the mechanical arm;

[0022] Then the parameter identification matrix equation is constructed:

[0023] A ΔX = B

[0024] Wherein, A represents the Jacobian matrix of the end position of each mechanical arm at each random position about its connecting rod parameters, as follows:

[0025]

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

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

[0028] ΔX = (Δa1 … Δa n Δa1 … Δa n Δa1 … Δa n Δa1 … Δa n ) T

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

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

[0031] Solve the parameter identification matrix equation constructed as mentioned above, that is, the error of each link parameter of the mechanical arm is obtained.

[0032] Specifically, the S3 comprises:

[0033] S31, after the laparoscopic surgery robot completes the positioning, the joint values of the mirror holding arm are obtained, the joint values of the fixed joints of the mirror holding arm considering the error are calculated according to the errors of each link parameter of the mirror holding arm obtained by S2, and then the end position of the mirror holding arm considering the error is calculated;

[0034] S32, the joint values of the fixed joints considering the error are obtained according to the errors of each link parameter of the two instrument arms obtained by S2, and then the end position of the instrument arm considering the error of the fixed joints is calculated;

[0035] S33, the target pose of the end of the instrument arm is obtained, the target end position of the instrument arm is obtained, the relative position relationship between the end position of the mirror holding arm and the target end position of the instrument arm is calculated, and the target value of the relative motion joint of the instrument arm is calculated in combination with S31 and S32.

[0036] Specifically, in the S4, the total error of the relative motion joint of the instrument arm is specifically:

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

[0038]

[0039] Wherein, α qv represents the torsion angle of the adjacent link of the relative motion joint, a qv represents the distance of the common perpendicular line of the adjacent link of the relative motion joint, θ qv represents the deflection angle of the adjacent link of the relative motion joint;

[0040] The position dT qv of the relative motion joint is:

[0041]

[0042] Wherein, d qv represents the offset distance of the adjacent link of the relative motion joint;

[0043]

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

[0045] Beneficial effects: The application aims at the position error and compensation accuracy of the multi-joint multi-mechanical arm structure of the laparoscopic surgical robot, proposes to identify the error to consider all joint errors, and considers the fixed joint and the relative motion joint respectively in the position error compensation, so that the error compensation is more accurate. In addition, in view of the problem that the traditional joint local error compensation iteration is performed many times, the total error of the error joint angle can be compensated without iteration, and the running efficiency of the program is improved. BRIEF DESCRIPTION OF DRAWINGS

[0046] In order to more clearly illustrate the technical solutions in the application, the drawings needed to be used in the embodiments will be briefly introduced as follows. Obviously, the drawings in the following description are only embodiments of the application, and other drawings can be obtained by those skilled in the art without creative labor on the basis of these drawings.

[0047] Figure 1 The flowchart of the laparoscopic surgical robot end position error compensation control method of the application.

[0048] Figure 2 The structure example diagram of the laparoscopic robot and a certain mechanical arm thereon. DETAILED DESCRIPTION

[0049] In order to make the purpose, technical scheme and advantages of the application more clear and obvious, the application will be further described in detail below 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 application should be understood as the usual meanings understood by those skilled in the art to which the application belongs. The words such as "include" or "contain" mean that the elements or objects appearing before the words cover the elements or objects listed after the words and their equivalents, and do not exclude other elements or objects. The words such as "connect" or "connected" are not limited to physical or mechanical connection, but can include electrical connection, whether direct or indirect.

[0051] The structure of the laparoscopic surgical robot and a certain mechanical arm thereon is as shown in Figure 2As shown, generally, the laparoscopic surgery robot is provided with four mechanical arms, which are all installed on a hanging tray, and the ends of three of the mechanical arms are installed with corresponding endoscopes or instruments according to requirements, as mirror holding arms or instrument arms; the remaining mechanical arm is used as a redundant mechanical arm to prevent the replacement of the aforementioned three mechanical arms as alternative mechanical arms in case of problems.

[0052] The flow of the laparoscopic surgery robot end position error compensation control method of the present application is as shown in the figure Figure 1

[0053] S1, respectively drag the mirror holding arm and the instrument arm of the laparoscopic surgery robot to a plurality of random positions in the working space, obtain the actual joint values of each joint of each mechanical arm at each random position, calculate the theoretical values of the end positions of each mechanical arm at each random position through forward kinematics of the mechanical arm, and simultaneously measure the actual values of the end positions of each mechanical arm at each random position, thereby calculating the errors of the end positions of each mechanical arm at each random position.

[0054] In the present application, the actual joint values of each joint of a mechanical arm are specifically the rotation angles thereof, which can be obtained through the encoder installed thereon.

[0055] In the present application, the end positions of each mechanical arm at each random position are defined with the hanging tray of the laparoscopic surgery robot as the reference; in the present application, a base coordinate system can be established with the hanging tray as the reference, and thereby the coordinates of the end positions of each mechanical arm at each random position in the base coordinate system can be obtained.

[0056] Specifically, the mirror holding arm and the instrument arm of the laparoscopic surgery robot are respectively dragged to a plurality of random positions in the working space, the actual joint values of each joint of each mechanical arm at each random position are obtained, and thereby the theoretical coordinates of the end positions of each mechanical arm at each random position in the base coordinate system, i.e. the theoretical values of the end positions of each mechanical arm at each random position, can be calculated through forward kinematics of the mechanical arm; thereafter, the actual coordinates of the end positions of each mechanical arm at each random position in the base coordinate system, i.e. the actual values of the end positions of each mechanical arm at each random position, are obtained through measurement, and thereby the errors of the end positions of each mechanical arm at each random position can be calculated.

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

[0058] P=F(alpha,a,theta,d)

[0059] ​Wherein, P represents a theoretical value of a position of an end of a certain mechanical arm, a represents a torsion angle of an adjacent connecting rod, a represents a distance of a perpendicular line of the adjacent connecting rod, θ represents a deflection angle of the adjacent connecting rod, and d represents a deflection distance of the adjacent connecting rod.

[0060] Specifically, in the specific embodiment of the present application, among the aforementioned three mechanical arms, the end of the mechanical arm at the middle position is generally provided with an endoscope, and this mechanical arm is a mirror holding arm; the ends of the other two mechanical arms are provided with instruments, and the two mechanical arms are instrument arms. Figure 2 The mirror holding arm is installed on the hanging plate through eight joints J1-J8, wherein the joints J1-J8 are a turntable rotating joint J1, a horizontal arm telescopic joint J2, a vertical arm lifting joint J3, a vertical arm rotating joint J4, a pre-inclination joint J5, an RCM arm yaw joint J6, an RCM arm inclination joint J7, and an up-down moving joint J8. Since the endoscope only has movement in the axial direction, the mirror holding arm further includes an endoscope axial movement joint J9, that is, the mirror holding arm has a total of nine joints. During the operation process, the joints on the mirror holding arm do not need to move and are all fixed joints. Therefore, the present application can take the mirror holding arm as a reference to calculate the positions and errors of the two instrument arms; the instrument arms have eleven joints, the first eight joints J1-J8 of which are consistent with the first eight joints J1-J8 of the mirror holding arm, and the other three joints are an instrument axial movement joint J9, an instrument inclination joint J10, and an instrument yaw joint J11.

[0061] S2, according to the errors of the end positions of the mechanical arms at the random positions obtained in S1, obtaining the Jacobian matrix of the end positions of the mechanical arms at the random positions with respect to the parameters of the connecting rods, thereby constructing a parameter identification matrix equation and solving the equation to obtain the errors of the parameters of the connecting rods;

[0062] Specifically, the errors Δp of the end positions of the mechanical arms at the random positions obtained in S1 are known, and the theoretical value P of the end position of the mechanical arm is differentiated to obtain the relationship between the position error Δp of the end position of the mechanical arm at the position and the errors Δα, Δa, Δθ, Δd of the parameters of the connecting rods, as follows:

[0063]

[0064] Thus, the errors of the end positions of the mechanical arms at the random positions 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 zPosition error Δp in x, y, z direction of base coordinate system respectively, n represents the number of joints of a certain robot arm, i represents the ith position of the robot arm;

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

[0068] AΔX=B

[0069] Wherein, A represents the Jacobian matrix of the end position of each random position of each robot arm about its link parameters, as follows:

[0070]

[0071] Wherein, m represents the number of S1 adjusted random positions;

[0072] ΔX represents unknown variables 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 each random position of the robot arm, as follows:

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

[0076] Therefore, the parameter identification matrix equation constructed as mentioned above can be solved, and the error of each link parameter of the robot arm can be calculated;

[0077] In the application, the parameter identification matrix equation constructed as mentioned above can be solved by least square or pseudo-inverse. Further, before solving the parameter identification matrix equation constructed as mentioned above, it is necessary to determine whether the rank of the Jacobian matrix of the end position of each random position of each robot arm about its link parameters is full rank, so as to facilitate the solution of the equation group.

[0078] Repeat this step, and the error of each link parameter of the mirror holding arm and the two instrument arms can be obtained.

[0079] S3, the end position of the holding mirror arm after considering the error and the end position of the instrument arm after the instrument arm is positioned are calculated according to the error of each link parameter of each mechanical arm obtained in S2; the target end position of the instrument arm is obtained, and the relative position relationship between the end position of the holding mirror arm and the target end position of the instrument arm is calculated, and the target value of the relative motion joint of the instrument arm is calculated according to the relative position relationship;

[0080] The instrument arm of the laparoscopic surgery robot in the embodiment of the application is used for preoperative positioning, and needs to be locked, that is, the joint is fixed; the last several joints are relative motion joints, and are moved in the operation, that is, the relative motion joint. Therefore, the holding mirror arm is relatively fixed after the preoperative positioning is completed, the first several joints of the instrument arm are also relatively fixed after the preoperative positioning is completed, and the last several joints of the instrument arm are relative motion joints. Therefore, unlike the traditional robot compensation mode, separate compensation needs to be considered. Therefore, the fixed joint is uniformly processed, and the relative motion joint is processed.

[0081] Specifically, the method comprises:

[0082] S31, after the laparoscopic surgery robot is positioned, the joint values of the holding mirror arm are obtained, the joint values of the fixed joints of the holding mirror arm after considering the error are calculated according to the error of each link parameter of the holding mirror arm obtained in S2, and the end position of the holding mirror arm after considering the error is calculated.

[0083] In the application, the pose of the jth joint of the holding mirror arm after considering the error is T cj , and the end position of the holding mirror arm after considering the error is Z c =∏T cj ; wherein c represents the holding mirror arm, and the joints of the holding mirror arm are fixed joints.

[0084] In the embodiment of the application, 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, the joint values of the fixed joints of the instrument arm after considering the error can be obtained according to the error of each link parameter of the two instrument arms obtained in S2, and the end position of the instrument arm after positioning is calculated, that is, the end position of the instrument arm after considering the error of the fixed joints of the instrument arm.

[0086] In the application, the first several joints of the instrument arm are fixed joints, and the joint values of the joints after considering the error are defined as Tqug The rest of the joints are relative motion joints, and the joint value is T qv The end position Z of the instrument arm q =∏T qug ∏T qv Therefore, after obtaining the joint value of the fixed joint of the instrument arm considering the error, the end position of the instrument arm after the fixed joint considering the error of the instrument arm can be calculated, that is, the end position of the instrument arm after the positioning; wherein q represents the instrument arm, u represents a joint in the fixed joint of the instrument arm, and v represents a joint in the relative motion joint of the instrument arm.

[0087] In the embodiment of the present application, the instrument arm of the laparoscopic surgery robot has 11 joints, the first 5 joints are used for preoperative positioning and need to be locked during the operation, that is, the fixed joint, and the corresponding connecting rod is also fixed; the last 6 joints are relative motion joints, which are moving during the operation, that is, the relative motion joint, that is, u∈[1,5], v∈[6,11], and 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, obtaining the target pose of the end of the instrument arm, obtaining the target end position of the instrument arm, calculating the relative position relationship between the end position of the holding mirror arm and the target end position of the instrument arm, combining S31 and S32 to calculate the target value of the relative motion joint of the instrument arm;

[0089] Specifically, in the embodiment of the present application, the following is true:

[0090] Obtaining the target pose of the end of the instrument arm, 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 holding mirror arm and the target end position of the instrument arm r ;

[0091] Wherein, the joint value of the fixed joint of the instrument arm after considering the error is T qug , which is locked during the operation, and the relative motion joint needs to be compensated and controlled, and the joint value of the relative motion joint at this time is defined as T qtv , then T r =Z qt Z c -1 =∏Tqug ∏T qtv Z c -1 , and then ∏T qtv = (∏T qug ) -1 T r Z c , so as to obtain the target value of the relative motion joint of the instrument arm, which is the expected value of each relative motion joint without considering errors multiplied together;

[0092] In the 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, the total error of the relative motion joint of the instrument arm is calculated, and the target value of the relative motion joint of the instrument arm obtained in S3 is combined, so as to calculate the execution value of the relative motion joint of the instrument arm considering the error, inverse solution is performed on the execution value, and the execution value is sent to the driver for execution;

[0094] In the application, when the target end position Z qt of the instrument arm is sent, the inverse solution to the joint cannot reach the target end position Z qt , and there is always a total error ΔZ, and the actual position reached is Z qt + ΔZ, if the specific error is known in advance, the position Z qt of the target position considering the error can be obtained, that is, the execution value of the relative motion joint of the instrument arm considering the error, the execution value is taken as a command value for inverse solution, and then sent to the driver, so as to reach the target position Z qt = Z q-dT

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

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

[0097]

[0098] wherein α qv represents the torsion angle of the adjacent connecting rod of the relative motion joint, a qv represents the distance of the common perpendicular line of the adjacent connecting rod of the relative motion joint, θ qv represents the deflection angle of the adjacent connecting rod of the relative motion joint;

[0099] The position dT qv of the relative motion joint is:

[0100]

[0101] wherein d qv represents the offset distance of the adjacent connecting rod of the relative motion joint;

[0102]

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

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

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

[0106] The present application aims at the position error and compensation accuracy of the multi-joint multi-mechanical arm structure of the laparoscope mobile robot, proposes to identify the error to consider all joint errors; in the position error compensation, the fixed joint and the relative motion joint are considered respectively, and the error compensation is more accurate. In addition, in view of the problem of many times of iteration of the traditional joint local error compensation, the total error of the error joint angle can be compensated without iteration, and the running efficiency of the program is improved.

[0107] Those skilled in the art should understand: the discussion of the above any embodiment is only exemplary, and is not intended to imply that the scope of the present application (including claims) is limited to these examples; 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 changes of different aspects of the embodiments of the present application as described above, which are not provided in details for the sake of brevity.

[0108] Embodiments of the present application are intended to cover all such alternatives, modifications, and variations as fall within the broad scope of the appended claims. Accordingly, any one of the above-mentioned alternatives, modifications, equivalents, improvements, etc. made within the spirit and principle of the embodiments of the present application should be included in the scope of the present application.

Claims

1. A laparoscopic surgery robot end position error compensation control system, characterized in that, the mirror holding arm and the instrument arm of the laparoscopic surgery robot are respectively dragged to a plurality of random positions within the working space, the laparoscopic surgery robot obtains the theoretical value and the actual value of the end position of each mechanical arm at each random position, thereby calculating the error of the end position of each mechanical arm at each random position, and obtaining the Jacobian matrix of the end position of each mechanical arm at each random position with respect to each link parameter, thereby constructing a parameter identification matrix equation and solving it to obtain the error of each link parameter; after the laparoscopic surgery robot completes the positioning, the joint values of the mirror holding arm are obtained, the joint values of the fixed joints of the mirror holding arm considering the error are calculated according to the error of each link parameter of the mirror holding arm obtained above, and the end position of the mirror holding arm considering the error is calculated, the joint values of the fixed joints of the two instrument arms considering the error are obtained according to the error of each link parameter of the two instrument arms obtained above, and the end position of the instrument arms considering the error of the fixed joints is calculated, and the target pose of the end of the instrument arm is obtained, the target end position of the instrument arm is obtained, and the relative position relationship between the end position of the mirror holding arm and the target end position of the instrument arm is calculated, and the target value of the relative motion joint of the instrument arm is calculated; the laparoscopic surgery robot calculates the overall error of the relative motion joint of the instrument arm, combines the above target value, calculates the execution value of the relative motion joint of the instrument arm considering the error, and inversely solves and executes it.

2. The laparoscopic surgical robotic end-effort position error compensation control system of claim 1, wherein, the laparoscopic surgery robot obtains the actual joint values of each joint of each mechanical arm at each random position, and calculates the theoretical value of the end position of each mechanical arm at each random position through forward kinematics of the mechanical arm, and simultaneously measures the actual value of the end position of each mechanical arm at each random position.

3. The laparoscopic surgical robotic end-effort position error compensation control system of claim 2, wherein, the laparoscopic surgery robot expresses the end position of each mechanical arm at each random position as a function of each link parameter according to the actual joint values of each joint of each mechanical arm at each random position through the classical DH modeling method, differentiates the function, and thereby obtains the position error of the end position of each mechanical arm at each random position.

4. The laparoscopic surgical robotic end-effort position error compensation control system of claim 3, wherein, the parameter identification matrix equation is constructed and solved to obtain the error of each link parameter value, which is as follows: the theoretical value of the end position of each mechanical arm at each random position is expressed as a function of each link parameter as follows: P=F(α,a,θ,d); wherein P represents the theoretical value of the end position of a certain mechanical arm at a certain position, α represents the torsion angle of the adjacent link, a represents the distance of the common perpendicular line of the adjacent link, θ represents the deflection angle of the adjacent link, and d represents the offset distance of the adjacent link, and each link is connected through a joint to form a movable mechanical arm; differentiate the above formula to obtain the relationship between the error Δp of the end position of the mechanical arm at the position and the error Δα, Δa, Δθ, Δd of each link parameter, as follows: ； transform the error Δp of the end position of each mechanical arm at each random position into components in each direction of the base coordinate system, as follows: ； wherein Δp x , Δp y , Δp z are components of the error Δp of each joint value in the x, y, 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 the parameter identification matrix equation is constructed as follows: AΔX=B; wherein A represents the Jacobian matrix of the end position of each mechanical arm at each random position with respect to each link parameter, as follows: ； Wherein, m represents the number of random positions of S1 adjustment; ΔX represents unknown variables to be solved, as follows: ； B represents the transpose 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: ； Solve the parameter identification matrix equation constructed as mentioned above, that is, the error of each link parameter of the robot arm is obtained.

5. The laparoscopic surgical robotic end-effort position error compensation control system of claim 1, wherein, The total error of the relative motion joint of the robot arm, specifically: The joint value of a certain relative motion joint is: ； wherein α qv represents the torsion angle of the adjacent link of the relative motion joint, a qv represents the distance of the common perpendicular of the adjacent link of the relative motion joint, θ qv represents the deflection angle of the adjacent link of the relative motion joint; The position of the relative motion joint The error is: ； where d qv denotes the offset distance of the adjacent link of the relative motion joint; ； ； ； ； The actual value of the relative motion joint of the instrument arm is πT qtv Then the total error ΔZ of the relative motion joint of the instrument arm is d(πT qtv ).

Images