Surgical robot cooperative navigation pose control method and control system

Abstract

The invention discloses a surgical robot cooperative navigation pose control method and system. The method comprises the steps that S1, an optical navigator coordinate system and other coordinate systems are unified to a navigation mechanical arm coordinate system; s2, taking an inflection point on the planned path as a key site, and determining a normal vector of the inflection point on the planned path; s3, an optimization target and constraint conditions used for obtaining the initial observation pose of the navigation mechanical arm are constructed, and the initial observation pose of the navigation mechanical arm is obtained through calculation; s4, when the optical navigator passes through each key point, the observation pose of the navigation mechanical arm is calculated and updated; the control system comprises a surgical robot module, a path information module and a navigation robot module; according to the control method and the control system, active tracking of the surgical mechanical arm can be completed in an autonomous collaborative navigation mode, the requirements of high precision, full view and shielding-free tracking in the surgery are met by continuously optimizing the optimal observation pose, and safety and continuity in the surgery process are effectively guaranteed.

Description

Technical Field

[0001] The present invention relates to the technical field of navigation mechanical arms, and in particular to a surgical robot collaborative navigation posture control method and control system. Background Art

[0002] The navigation system is the command center of the navigation robot system. It can use the optical tracker to track the position of the surgical instruments installed on the surgical robot in real time relative to the surgical position, thereby effectively guiding the safe operation of the surgical robot. Among them, the optical navigation method is widely used in robot-assisted surgery due to its advantages such as high precision, robustness, non-contact and reliability.

[0003] At present, the optical navigator uses two optical sensors to track the state of the positioning ball at the end of the navigation robot arm in real time to calculate the relationship between the surgical robot arm at the end of the navigation robot arm and the surgical position, thereby ensuring the accuracy and safety of the surgical robot arm in executing the surgical action path. However, in the actual execution of the action path, due to the particularity of the surgical position, the planned path is complex, or there are features such as the path bending part being close or the angle being small; and the aforementioned planned path characteristics require the posture of the navigation robot arm to change frequently, which can easily lead to visual occlusion problems for the fixed optical navigator, resulting in the loss of real-time position information of the surgical robot arm. Manual intervention is required to readjust the posture of the optical navigator, which will not only increase the operation time, but also lead to an increase in the risk of the operation. Therefore, it is necessary to improve the current real-time tracking method of surgical instruments and surgical positions based on optical navigators. Summary of the invention

[0004] The purpose of the present invention is to provide a surgical robot collaborative navigation posture control method that solves the above-mentioned technical problems.

[0005] The object of the present invention is to provide a control system for realizing the above-mentioned surgical robot collaborative navigation posture control method.

[0006] To this end, the technical solution of the present invention is as follows:

[0007] A surgical robot collaborative navigation posture control method, the steps are as follows:

[0008] S1, converting a first positioning tool coordinate system for indicating the position of a surgical instrument, a second positioning tool coordinate system for indicating the position of a planned path, and an optical navigator coordinate system to a navigation manipulator coordinate system for indicating the position of an end of a navigation manipulator, so as to realize posture control of the navigation manipulator over the optical navigator;

[0009] S2, taking the inflection points on the planned path as key points, and determining the normal vectors of each key point on the planned path;

[0010] S3. Constructing the optimization objectives and constraints for obtaining the initial observation pose of the navigation manipulator, including:

[0011] S301, define the normal angle between the optical navigator and the positioning tool i as the optimization target, and its expression is: f(1) = min||θ N,i ||, where θ N,i represents the deflection angle between the normal vector of the positioning tool i and the normal vector of the optical navigation device, i = 1, 2, 1 represents the first positioning tool, 2 represents the second positioning tool;

[0012] Define the distance of positioning tool i relative to the center of the field of view of the optical navigator as the optimization target, and its expression is: f(2) = min||N O M i ||, where N O M i is the distance between the positioning tool i and the center of the field of view of the optical navigator;

[0013] S302, setting a virtual sphere for surrounding the positioning tool i, so as to construct a safety distance determined based on the field of view of the optical positioning instrument as a constraint condition;

[0014] S303, constructing a constraint condition for avoiding interference between two positioning tools from the perspective of the optical navigator;

[0015] S304, setting the corresponding maximum rotation angle of each joint on the navigation robot arm as a constraint condition;

[0016] S305, setting the angle β between the normal vector of the positioning tool i and the normal vector of the optical navigation device not to exceed the maximum visible deflection angle of the optical navigation device as a constraint condition;

[0017] S306, substituting the optimization objective function constructed in step S301 and the constraint conditions constructed in steps S302 to S305 into the generalized differential evolution algorithm for solving, and obtaining the initial observation posture of the navigation manipulator;

[0018] S4. When the surgical instrument moves along the planned path, the optical navigator updates the observation posture of the navigation robot arm based on the normal vector of the current key point on the planned path each time it passes a key point.

[0019] Furthermore, the specific implementation steps of step S1 are:

[0020] S101, respectively constructing a navigation manipulator coordinate system, a first positioning tool coordinate system, a second positioning tool coordinate system and an optical navigator coordinate system;

[0021] S102, obtaining a transformation matrix from the navigation manipulator coordinate system to the optical positioning instrument coordinate system, so as to obtain the spatial position of the optical navigator in the navigation manipulator base coordinate system;

[0022] S103, mapping the spatial posture transformation of the optical navigator to the rotation angle of each joint of the navigation robot arm, so as to realize the posture control of the optical navigator by the navigation robot arm.

[0023] Further, in step S301, the deflection angle θ between the normal vector of the positioning tool i and the normal vector of the optical navigation device is N,i The expression is:

[0024]

[0025] In the formula, represents the unit vector of the z-axis of the optical navigator, The unit vector representing the z-axis of the positioning tool i is actually substituted into the normal vector of the key point as the starting point.

[0026] Further, in step S301, the distance between the positioning tool i and the center of the field of view of the optical navigation device is expressed as:

[0027] N O M i =μd 1 +γd 2 =μ·|| N p o - M p 1 ||+β·|| N p o - M p 2 ||

[0028] Where, d 1 Indicates the distance between the first positioning tool and the center of the field of view of the optical navigator, d 2 represents the distance between the second positioning tool and the center of the field of view of the optical navigator; μ and β represent weights respectively; N p o Indicates the spatial position of the center of the field of view of the optical navigator; M p 1 represents the spatial position of the center point of the first positioning tool in the optical navigator coordinate system, M p 2 Indicates the spatial position of the center point of the second positioning tool in the optical navigator coordinate system.

[0029] Furthermore, in step S302, the constraint condition construction step of the safety distance d is:

[0030] Set the radius of the virtual sphere to ri ,

[0031] The expression based on the field of view is:

[0032] A j x+B j y+C j z+D j =0,

[0033] Where A, B, C, D represent plane parameters respectively, j = 1, 2, ..., 10;

[0034] Then, the expression of safety distance d is:

[0035] d=(H 10×4 ·[x i ,y i ,z i ,1] T -r i )>0,

[0036]

[0037] In the formula, H 10×4 is a matrix containing the ten plane parameters of the field of view; x i ,y i ,z i represents the spatial coordinates of positioning tool i in the optical navigator coordinate system {N}; r i Represents the radius of the enclosing sphere of positioning tool i.

[0038] Further, in step S303, the method for constructing the constraint condition for avoiding interference between the two positioning tools in the perspective of the optical navigator is:

[0039] 1) The distance between the marker ball of the first positioning tool and the origin of the positioning tool coordinate system is defined as the minimum enclosing sphere radius r of the first positioning tool 1 The center of the minimum enclosing sphere is the center of the first positioning tool; the distance between the marking ball of the second positioning tool and the origin of the positioning tool coordinate system is defined as the minimum enclosing sphere radius r of the second positioning tool 2 , the center of the minimum enclosing sphere is the center of the first positioning tool; the angle between the left and right cameras of the optical navigator and the boundary of the enclosing sphere is set to be greater than zero;

[0040] 2) Define the angle between the origin of the optical navigation system coordinate system and the tangent lines of the enclosing spheres of the two positioning tools as γ, and construct the constraint conditions as follows:

[0041] γ=-min(α 1,2 -α 1 -α 2 )>0,

[0042]

[0043] In the formula, Represents the vector from the origin of the optical navigation system to the center of the first positioning tool enclosing sphere, Represents the vector from the origin of the optical navigation system to the center point of the second positioning tool's enclosing sphere.

[0044] Furthermore, in step S4, the updating step of the observation posture of the navigation manipulator is:

[0045] S401, define the change of the optical navigator's posture relative to the initial posture as small as possible as the optimization goal, and its expression is:

[0046]

[0047] Where P k is the position coordinate of the end of the robot arm at time point k, k = 1, 2, ..., N, || p k+1 -p k || means p l to p k+1 The Euclidean distance of

[0048] S402, substituting the three optimization objective functions constructed by steps S301 and S401 and the constraints constructed by steps S302 to S305 into the generalized differential evolution algorithm for solving, and obtaining the optimal observation posture of the navigation manipulator at the current key point.

[0049] A control system for implementing the above-mentioned surgical robot collaborative navigation posture control method, comprising:

[0050] A surgical robot module, comprising a surgical robot arm, a surgical instrument and a first positioning tool; the surgical robot arm is a robot arm with N degrees of freedom, N ≥ 1; the surgical instrument is fixed to the end of the surgical robot arm so as to drive the surgical instrument to move through the surgical robot arm; the first positioning tool is arranged at the end of the surgical robot arm so as to determine the spatial position information of the surgical instrument through the first positioning tool;

[0051] The path information module includes a surgical position fixer and a second positioning tool; the surgical position fixer is fixed at the lesion position to avoid deviation or error of the planned path caused by the spatial position movement of the lesion; the second positioning tool is fixed on the surgical position fixer to determine the spatial position information of the planned path at the lesion position through the second positioning tool;

[0052] A navigation robot module includes an optical navigator and a navigation manipulator; the navigation manipulator adopts an N-degree-of-freedom manipulator, N≥1; the optical navigator is fixed to the end of the navigation manipulator to adjust the position of the optical navigator through the navigation manipulator; the optical navigator cooperates with a first positioning tool and a second positioning tool respectively to obtain real-time spatial information of the surgical instrument and the lesion position, so that the surgical instrument performs surgical actions at the lesion position along a planned path.

[0053] Compared with the existing technology, the collaborative navigation posture control method and system of the surgical robot can realize the active tracking of the surgical robot arm in the form of autonomous collaborative navigation, and has the advantages of high initiative and strong flexibility. It can continuously optimize the optimal observation posture during the movement of the surgical instrument, avoid frequent posture adjustments of the navigator, ensure the needs of high-precision, full-field, and unobstructed tracking during the operation, effectively guarantee the safety and continuity of the operation, and provide new ideas for the safe navigation of the navigation robot arm. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 It is a structural schematic diagram of the navigation robot arm collaborative navigation system of the present invention;

[0055] Figure 2 It is a flow chart of the surgical robot collaborative navigation posture control method of the present invention;

[0056] Figure 3 It is a schematic diagram of the conversion relationship between the first positioning tool coordinate system, the second positioning tool coordinate system, the optical navigation device coordinate system and the navigation robot arm coordinate system of the present invention;

[0057] Figure 4 is a schematic diagram of a normal deflection angle between the optical navigator of the present invention and any positioning tool;

[0058] Figure 5 A schematic diagram of the relationship between two positioning tools of the present invention and the center of the field of view of the optical navigator;

[0059] Figure 6 Schematic diagram of visual occlusion of two positioning tools of the present invention

[0060] Figure 7 It is a schematic diagram of adjusting the posture and field of view of the navigation robot in an embodiment of the present invention;

[0061] Figure 8 It is a schematic diagram of the simulation of the navigation manipulator in the embodiment of the present invention;

[0062] Fig. 9 Schematic diagram of different viewing angles of an optical navigator moving with a surgical robot arm in an embodiment of the present invention. DETAILED DESCRIPTION

[0063] The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments, but the following embodiments are by no means intended to limit the present invention in any way.

[0064] See also Figure 1 , the control system for realizing the collaborative navigation posture control method of the surgical robot includes:

[0065] A surgical robot module is used to drive a surgical instrument so that it performs a surgical action while moving along a planned path; specifically, the surgical robot module includes a surgical robot arm 1, a surgical instrument 2 and a first positioning tool 3; wherein the surgical robot arm 1 adopts a robot arm with N degrees of freedom, N ≥ 1; the surgical instrument 2 is fixed to the end of the surgical robot arm 1 so as to drive the surgical instrument 2 to move through the surgical robot arm 1; the first positioning tool 3 is arranged at the end of the surgical robot arm 1 so as to determine the spatial position information of the surgical instrument 2 through the first positioning tool 3; in actual application, the surgical robot module is installed on a mobile trolley so as to be arranged and fixed at a suitable position according to needs;

[0066] A path information module is provided at the lesion position to characterize the surgical action planning path of the surgical instrument 2 at the lesion position; specifically, the path information module includes a surgical position fixer 4 and a second positioning tool 5; wherein the surgical position fixer 4 is fixed at the lesion position to avoid deviation or error of the planned path caused by the spatial position movement of the lesion; the second positioning tool 5 is fixed on the surgical position fixer 4 to determine the spatial position information of the planned path at the lesion position through the second positioning tool 5;

[0067] A navigation robot module is used for real-time tracking of surgical instruments and lesion positions; specifically, the navigation robot module includes an optical navigator 6 and a navigation robotic arm 7; wherein the navigation robotic arm 7 adopts an N-degree-of-freedom robotic arm, N≥1; the optical navigator 6 is fixed at the end of the navigation robotic arm 7 to adjust the posture of the optical navigator 6 through the navigation robotic arm 7; and the optical navigator 6 cooperates with the first positioning tool 3 and the second positioning tool 5 respectively to obtain real-time spatial information of the surgical instrument 2 and the lesion position, so that the surgical instrument 2 performs surgical actions at the lesion position along the planned path; in actual applications, the navigation robot module is installed on a mobile trolley to be arranged and fixed at a suitable position according to needs.

[0068] In this embodiment, the first positioning tool 3 and the second positioning tool 5 have the same structure; specifically, the positioning tool is composed of four reflective balls and a cross-shaped bracket, and the four reflective balls are respectively fixed at the four ends of the cross-shaped bracket; in actual application, the optical navigator 6 obtains the spatial position of the surgical instrument 2 and the spatial position of the planned path at the lesion position by respectively identifying the two positioning tools.

[0069] See also Figure 2 The specific steps of the surgical robot collaborative navigation posture control method are as follows:

[0070] S1, converting a first positioning tool coordinate system for indicating the position of a surgical instrument, a second positioning tool coordinate system for indicating the position of a planned path, and an optical navigator coordinate system to a navigation manipulator coordinate system for indicating the position of an end of a navigation manipulator, so as to realize posture control of the navigation manipulator over the optical navigator;

[0071] See also Figure 3 and Figure 4 The specific implementation steps of step S1 are described as follows.

[0072] S101, constructing a navigation manipulator coordinate system {B}, a first positioning tool coordinate system {M 1}, the second positioning tool coordinate system {M 2} and the optical navigator coordinate system {N}.

[0073] Construct the navigation robot arm coordinate system {B}. The specific construction method is: take the center of the navigation robot arm base as the origin, its Z axis is defined as being collinear with the rotation axis of the first joint of the navigation robot arm and pointing upward, its X axis and Y axis are located on the plane of the base and satisfy the right-hand rule.

[0074] Construct the first positioning tool coordinate system {M 1}, the specific construction method is: take the intersection of the lines of the four marking balls evenly distributed on the end surface of the surgical robot arm along the circumferential direction as the origin, define its Z axis as the normal vector of the fitting plane of the four marking balls, and define its X axis and Y axis as the lines connecting the origin to the two balls above it, and satisfy the right-hand rule; the first positioning tool coordinate system {M 1} is used to represent the coordinate position of the surgical instrument, the surgical instrument is located in the first positioning tool coordinate system {M 1} at the origin.

[0075] Construct the second positioning tool second positioning tool coordinate system {M 2}, the specific construction method is the same as the first positioning tool coordinate system {M 1} is constructed in the same way; the second positioning tool coordinate system {M 2} is used to indicate the coordinate position of the planned path.

[0076] Construct the optical navigator coordinate system {N}. The specific construction method is: take the midpoint of the line connecting the optical centers of the left and right cameras of the optical navigator as the origin, define its X-axis as the vector pointing from the optical center of the left camera to the optical center of the right camera, define its Z-axis as being coplanar with the X-axis and the optical axis of the left camera and perpendicular to the X-axis, and pointing in the direction of the field of view, and define its Y-axis as being perpendicular to the X-axis and the Z-axis respectively, and satisfying the right-hand rule.

[0077] S102, using the hand-eye calibration method, obtain the transformation matrix from the navigation manipulator coordinate system {B} to the optical positioner coordinate system {N}

[0078] Transformation matrix from navigation manipulator coordinate system {B} to optical positioner coordinate system {N} The expression is:

[0079]

[0080] In the formula, is the transformation matrix from the end of the navigation manipulator to the base of the navigation manipulator, which is obtained by the forward kinematics calculation of the robot; is the hand-eye calibration matrix, which is obtained by performing hand-eye calibration between the navigation robot and the optical locator.

[0081] The hand-eye calibration method is as follows: the positioning tool i is placed in a fixed position, the navigation robot adjusts at least four different positions, and the spatial coordinate information of the positioning tool i in the corresponding position is collected by the optical navigator respectively, and finally the hand-eye calibration matrix is ​​obtained by the nonlinear optimization algorithm, that is, the transformation matrix between the optical navigator coordinate system and the navigation robot end coordinate system; as a preferred technical solution of this embodiment, after the above-mentioned hand-eye calibration is completed, the navigation robot robot is made to perform a specific movement, and the position of the reference object is detected by the optical locator, and the error between the prediction and the actual measurement is checked to verify the result; if the verification error is large, data collection and calibration need to be performed again.

[0082] Since the spatial position of the surgical robot arm and the planned path on the lesion can be obtained through the optical navigator coordinate system, it is also necessary to convert the first positioning tool coordinate system {M 1} and the second positioning tool coordinate system {M 2} is converted to the robot base system {B}, and its expression is:

[0083]

[0084] In the formula, represents the position of the positioning tool in the robot base coordinate system {B}, where i represents the positioning tool, i=1,2, i=1 represents the first positioning tool, and i=2 represents the second positioning tool; represents the position of positioning tool i in {N} in the optical navigator coordinates.

[0085] S103, mapping the spatial posture transformation of the optical navigator to the rotation angle of each joint of the navigation manipulator, so as to realize the posture control of the optical navigator by the navigation manipulator;

[0086] After step S102, based on the transformation matrix from the navigation manipulator coordinate system {B} to the optical positioning instrument coordinate system {N} The spatial position of the optical navigator in the base coordinate system of the navigation manipulator can be converted accordingly; then, the spatial posture transformation of the optical navigator is mapped to the rotation angle of each joint of the navigation manipulator, that is, the navigation manipulator realizes the posture control of the optical navigator, and its expression is:

[0087]

[0088] In the formula, represents the spatial position of the optical navigator in the base coordinate system of the navigation manipulator, n represents the degree of freedom of the navigation manipulator, q 1 ,q 2 , …, q n Indicates the rotation angle from the 1st joint to the nth joint of the navigation robot.

[0089] S2. Take the turning points on the planned path as key points and determine the normal vectors of each key point on the planned path.

[0090] The planned path is the planned path of the surgical instrument during the operation, which is pre-planned according to the surgical requirements. There are multiple inflection points in the planned path. In this application, these inflection points are used as key locations to guide the subsequent path.

[0091] In step S2, the normal vector of each key point on the planned path, that is, the normal vector of the surgical instrument at the key point in order to perform the surgical action, can be determined in advance by preliminary analysis of the motion trajectory of the surgical instrument, and used in the posture control of subsequent steps S3 and S4, so as to achieve precise overlap of the motion trajectory of the surgical instrument with the planned path; in the planned path, one of the key points is also the starting point for the surgical instrument to perform the surgical action.

[0092] S3. Construct optimization objectives and constraints for obtaining the initial observation pose of the navigation manipulator.

[0093] The specific implementation steps of step S3 are as follows:

[0094] S301. In order to obtain the initial observation posture of the navigation manipulator, the problem of obtaining the initial observation posture of the optical navigator is transformed into an optimization problem, and two optimization objective functions are constructed, including:

[0095] I. Determine the spatial normal vector of the positioning tool during surgery based on the planned path, and define the normal angle between the optical navigator and the positioning tool i as the optimization target, which is expressed as:

[0096] f(1)=min||θ N,i||,

[0097] In the formula, θ N,i represents the deflection angle between the normal vector of the positioning tool i and the normal vector of the optical navigation device, which is specifically the angle between the z-axis of the positioning tool i and the z-axis of the optical navigation device; i=1,2, 1 represents the first positioning tool, and 2 represents the second positioning tool;

[0098] Among them, θ N,i The expression is:

[0099]

[0100] In the formula, represents the unit vector of the z-axis of the optical navigator, The unit vector representing the z-axis of the positioning tool i is actually substituted into the normal vector of the key point as the starting point (determined by step S2);

[0101] II. Define the distance of positioning tool i relative to the center of the field of view of the optical navigator as the optimization target, and its expression is:

[0102] f(2)=min||N O M i ||,

[0103] Where N O M i is the distance between the center point of positioning tool i and the center of the field of view of the optical navigator; i=1,2, 1 represents the first positioning tool, and 2 represents the second positioning tool.

[0104] In practical applications, if the positioning tool moves out of the field of view of the optical navigator, navigation will fail. The field of view diagram is as follows: Figure 5 As shown, the field of view is a pyramid-shaped area surrounded by ten planes, and all marking balls of the positioning tool must be kept within the field of view. According to the optical positioning performance requirements, the closer the positioning tool is to the center of the field of view, the higher the positioning accuracy tracked by the optical navigator.

[0105] Among them, the distance between the positioning tool i and the center of the field of view of the optical navigator is expressed as:

[0106] N O M i =μd 1 +γd 2 =μ·|| N p o - M p 1 ||+β·|| N p o - M p 2 ||,

[0107] Where, d 1 Indicates the distance between the first positioning tool and the center of the field of view of the optical navigator, d 2 represents the distance between the second positioning tool and the center of the field of view of the optical navigation device, and the two are specifically the distances from the center point of the positioning tool i to the center of the field of view of the optical navigation device; μ and β represent weights, and in this embodiment, the values ​​of the two are 0.5 and 0.5 respectively; N p o represents the spatial position of the center of the field of view of the optical navigation device; in this embodiment, the center of the field of view is located on the z-axis of the optical navigation device, so N p o is (0, 0, z o ); M p 1 represents the spatial position of the center point of the first positioning tool in the optical navigator coordinate system, M p 2 Indicates the spatial position of the center point of the second positioning tool in the optical navigator coordinate system;

[0108] S302, when the surgical instrument moves out of the field of view of the optical locator due to a large movement amplitude, navigation failure will occur. Therefore, in order to prevent any positioning tool from moving out of the field of view, a virtual sphere surrounding the positioning tool is defined to set a dynamic visual safety range. By setting a threshold of the safety distance d, the virtual sphere always keeps the positioning tool within the effective field of view of the optical navigator;

[0109] Specifically, set the radius of the virtual sphere to r i ,

[0110] The expression based on the field of view is:

[0111] A j x+B j y+C j z+D j =0,

[0112] Where A, B, C, D represent plane parameters respectively, j = 1, 2, ..., 10;

[0113] Then, the expression of safety distance d is:

[0114] d=(H 10×4 ·[x i ,y i ,z i ,1] T -r i )>0,

[0115]

[0116] In the formula, H 10×4 is a matrix containing the ten plane parameters of the field of view; x i ,y i ,z i represents the spatial coordinates of positioning tool i in the optical navigator coordinate system {N}; r i represents the radius of the enclosing sphere of positioning tool i;

[0117] S303. In order to ensure visibility and navigation continuity between the two positioning tools, it is necessary to avoid occlusion of the two positioning tools under the perspective of the optical navigator and perform anti-interference: Specifically, the method for constructing anti-interference constraint conditions to avoid mutual occlusion of the two positioning tools under the perspective of the optical navigator is as follows:

[0118] S3031, define the distance between the marking ball of the first positioning tool and the origin of the positioning tool coordinate system as the minimum enclosing sphere radius r of the first positioning tool 1 The center of the minimum enclosing sphere is the center of the first positioning tool; the distance between the marking ball of the second positioning tool and the origin of the positioning tool coordinate system is defined as the minimum enclosing sphere radius r of the second positioning tool 2 , the center of the minimum enclosing sphere is the center of the first positioning tool; the angle between the left and right cameras of the optical navigator and the boundary of the enclosing sphere is set to be greater than zero;

[0119] S3032, define the angle formed by the tangent lines from the origin of the optical navigation system to the enclosing spheres of the two positioning tools as γ, see Figure 6 , the anti-interference constraint condition is constructed as:

[0120] γ=-min(α 1,2 -α 1 -α 2 )>0,

[0121]

[0122] In the formula, Represents the vector from the origin of the optical navigation system to the center of the first positioning tool enclosing sphere, A vector representing the origin of the optical navigation system coordinate system to the center point of the second positioning tool enclosing sphere;

[0123] S304, according to the navigation manipulator workspace reference operation manual, set the corresponding maximum rotation angle of each joint on the navigation manipulator as a constraint condition;

[0124] S305, since the angle β between the normal vector of the positioning tool i and the normal vector of the optical navigation device does not exceed the maximum visible deflection angle of the optical navigation device as a constraint condition; specifically, the maximum visible deflection angle of the optical navigation device is a value between π / 4 and π / 3;

[0125] S306: Substitute the two optimization objective functions constructed by step S301 and the constraints constructed by steps S302 to S305 into the generalized differential evolution algorithm for solving, and obtain the initial observation posture of the navigation robot arm.

[0126] S4. When the surgical robot drives the surgical instrument to move along the planned path, a large normal vector deflection will occur each time it passes a key point. Therefore, the initial posture of the optical navigator obtained by step S3 cannot meet the observation requirements in the planned path. Based on this, in the process of the surgical instrument moving along the planned path, the optical navigator updates the observation posture of the navigation robot based on the normal vector of the current key point on the planned path each time it passes a key point, to ensure that the surgical robot maintains precise positioning and posture control when entering a new path.

[0127] The specific implementation steps of step S4 are:

[0128] S401, define the change of the optical navigator's posture relative to the initial posture as small as possible as the optimization goal, and its expression is:

[0129]

[0130] Where P k is the position coordinate of the end of the robot arm at time point k, k = 1, 2, ..., N, || p ik+1 -p k || means p k to p k+1 The Euclidean distance of

[0131] S402, substituting the three optimization objective functions constructed by steps S301 and S401 and the constraints constructed by steps S302 to S305 into the generalized differential evolution algorithm for solving, and obtaining the optimal observation posture of the navigation manipulator at the current key point.

[0132] like Figure 7 The figure shows the optimal posture adjustment of the navigation robot and the corresponding field of view change. It can be seen from the figure that the proposed navigation robot adjusts the posture so that both positioning tools are within the field of view, and the robot completes the active navigation during the operation through a small displacement, avoiding the navigation loss problem caused by the excessive deflection angle of the positioning tool, and ensuring the safety of the operation.

[0133] like Figure 8 The figure shows the schematic diagram of the navigation robot motion simulation. Figure 8In part (a), in the initial unobstructed navigation state, the optical navigator can accurately track the spatial posture of the two positioning tools. However, when the end of the surgical robot arm moves to the key hole, the positioning tool undergoes a significant posture change, and part of the positioning ball is blocked, resulting in the loss of surgical instrument position information; at this time, according to the relative posture state of the first positioning tool and the second positioning tool, the optimal observation posture of the optical navigator is calculated in advance, and the navigation robot arm is actively adjusted to achieve the optimal posture adjustment of the optical navigator, such as Figure 8 Different surgical paths will result in different deflection angles of surgical instruments at key holes. This example takes into account that when the surgical robot arm needs to adjust a larger angle to adapt to the surgical path, the first positioning tool has a large posture change, and the corresponding navigation robot arm joints need to adjust a larger range to ensure that the optical navigator can re-track the first positioning tool. See Figure 8 As shown in part (c) of .

[0134] like Fig. 9 Shown are schematic diagrams of different viewing angles of an optical navigator moving with a surgical robot arm; Fig. 9 Taking the combined diagram of part (a) as an example, the combined diagram specifically consists of a side view (Side view) of the optical navigator located at the upper left using the observation posture state determined in the present application, a front view (Front view) of the optical navigator located at the upper right using the observation posture state determined in the present application, a top view (Topview) of the optical navigator located at the lower left using the observation posture state determined in the present application, and an initial posture perspective diagram of the optical navigator located at the lower right before posture adjustment.

[0135] like Fig. 9 As shown in parts (a) and (d), the optical navigator can track the positioning tool by maintaining the optimized initial posture. At this time, the first positioning tool is facing the optical navigator. From the three views (side view, front view and top view) of the optical navigator, it can be seen that the positioning balls of the positioning tools are all within the field of view of the optical navigator.

[0136] like Fig. 9 As shown in part (b), the surgical robot arm makes a large angle turn at the next key position. As can be seen from the lower right actual picture of the combined figure of part (b), if the position of the optical navigator is fixed, the positioning ball of the first positioning tool will inevitably be blocked; and since the optimization method of the present application is adopted in the above-mentioned actual operation, the observation position of the optical navigator is adjusted and updated. Therefore, in three different viewing angles of the optical navigator (see the upper left, lower left and upper right views of the combined figure of part (b)), the optical navigator after the position adjustment can continue to capture the positioning balls of the two positioning tools, thereby ensuring continuous intraoperative tracking.

[0137] like Fig. 9 As shown in part (c), when the surgical instrument moves to the next key position again with the surgical robot arm, it can be seen from the initial posture viewing angle diagram of the optical navigator before the posture adjustment that the field of view of the two positioning balls of the first positioning tool is lost. However, after the posture of the optical navigator is adjusted using the method of the present application, the optical navigator is once again adjusted to a state where the positioning balls of the two positioning tools can be effectively observed, thereby effectively acquiring the spatial posture of the two positioning tools.

[0138] In order to further demonstrate the superiority of the control effect of the surgical robot collaborative navigation posture control method of the present application, the HV method was used to evaluate the control effect.

[0139] HV (Hypervolume) is a method for effectively evaluating a multi-objective optimization algorithm by measuring the volume of the area formed by the solution set generated by the algorithm and the reference points. It is widely used in the field of multi-objective optimization. Specifically, the higher the value of the corresponding evaluation index HV, the better the processing performance of the multi-objective optimization algorithm for the multi-objective optimization problem.

[0140] As comparative example 1, the non-dominated sorting genetic algorithm (NSGA-II) is also used in the evaluation process to replace the generalized differential evolution algorithm (GDE3) of the present application and the same surgical robot collaborative navigation posture control process is repeated. As comparative example 2, the decomposition-based constrained multi-objective evolutionary algorithm (CMOEAD) is also used in the evaluation process to replace the generalized differential evolution algorithm (GDE3) of the present application and the same surgical robot collaborative navigation posture control process is repeated. The above evaluation process is carried out through the evolutionary multi-objective optimization open source platform platemo developed by the Institute of Biointelligence and Knowledge Discovery of Anhui University, and the evaluation results are shown in Table 1 below.

[0141] Table 1:

[0142] Test Case Comparative Example 1 Comparative Example 2 This application HV value 0.69 0.68 0.80

[0143] It can be seen from the evaluation results in Table 1 above that the accuracy of the surgical robot collaborative navigation posture control method implemented by the method of the present application far exceeds the posture control effects of Comparative Example 1 and Comparative Example 2; it can be seen that the surgical robot collaborative navigation posture control method has significant advantages.

[0144] The above are only preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A surgical robot collaborative navigation posture control method, characterized in that the steps include: S1, converting a first positioning tool coordinate system for indicating the position of a surgical instrument, a second positioning tool coordinate system for indicating the position of a planned path, and an optical navigator coordinate system to a navigation manipulator coordinate system for indicating the position of an end of a navigation manipulator, so as to realize posture control of the navigation manipulator over the optical navigator; S2, taking the inflection points on the planned path as key points, and determining the normal vectors of each key point on the planned path; S3. Constructing the optimization objectives and constraints for obtaining the initial observation pose of the navigation manipulator, including: S301, define the normal angle between the optical navigator and the positioning tool i as the optimization target, and its expression is: f(1) = min||θ N,i ||, where θ N,i represents the deflection angle between the normal vector of the positioning tool i and the normal vector of the optical navigation device, i = 1, 2, 1 represents the first positioning tool, 2 represents the second positioning tool; Define the distance of positioning tool i relative to the center of the field of view of the optical navigator as the optimization target, and its expression is: f(2) = min||N O M i ||, where N O M i is the distance between the positioning tool i and the center of the field of view of the optical navigator; S302, setting a virtual sphere for surrounding the positioning tool i, so as to construct a safety distance determined based on the field of view of the optical positioning instrument as a constraint condition; S303, constructing a constraint condition for avoiding interference between two positioning tools from the perspective of the optical navigator; S304, setting the corresponding maximum rotation angle of each joint on the navigation robot arm as a constraint condition; S305, setting the angle β between the normal vector of the positioning tool i and the normal vector of the optical navigation device not to exceed the maximum visible deflection angle of the optical navigation device as a constraint condition; S306, substituting the optimization objective function constructed in step S301 and the constraint conditions constructed in steps S302 to S305 into the generalized differential evolution algorithm for solving, and obtaining the initial observation posture of the navigation manipulator; S4. When the surgical instrument moves along the planned path, the optical navigator updates the observation posture of the navigation robot arm based on the normal vector of the current key point on the planned path each time it passes a key point.

2. The surgical robot collaborative navigation posture control method according to claim 1, characterized in that: The specific implementation steps of step S1 are: S101, respectively constructing a navigation manipulator coordinate system, a first positioning tool coordinate system, a second positioning tool coordinate system and an optical navigator coordinate system; S102, obtaining a transformation matrix from the navigation manipulator coordinate system to the optical positioning instrument coordinate system, so as to obtain the spatial position of the optical navigator in the navigation manipulator base coordinate system; S103, mapping the spatial posture transformation of the optical navigator to the rotation angle of each joint of the navigation robot arm, so as to realize the posture control of the optical navigator by the navigation robot arm.

3. The surgical robot collaborative navigation posture control method according to claim 1, characterized in that: In step S301, the deflection angle θ between the normal vector of the positioning tool i and the normal vector of the optical navigation device is N,i The expression is: In the formula, represents the unit vector of the z-axis of the optical navigator, The unit vector representing the z-axis of the positioning tool i is actually substituted into the normal vector of the key point as the starting point.

4. The surgical robot collaborative navigation posture control method according to claim 1, characterized in that: In step S301, the distance between the positioning tool i and the center of the field of view of the optical navigation device is expressed as: N O M i =μd1+γd2=μ·|| N p o - M p1||+β·|| N p o - M p2|| Wherein, d1 represents the distance between the first positioning tool and the center of the field of view of the optical navigator, d2 represents the distance between the second positioning tool and the center of the field of view of the optical navigator; μ and β represent weights respectively; N p o Indicates the spatial position of the center of the field of view of the optical navigator; M p1 represents the spatial position of the center point of the first positioning tool in the optical navigator coordinate system. M p2 represents the spatial position of the center point of the second positioning tool in the optical navigator coordinate system.

5. The surgical robot collaborative navigation posture control method according to claim 1, characterized in that: In step S302, the steps for constructing the constraint condition of the safety distance d are: Set the radius of the virtual sphere to r i , The expression based on the field of view is: A j x+B j y+C j z+D j =0, Where A, B, C, D represent plane parameters respectively, j = 1, 2, ..., 10; Then, the expression of safety distance d is: d=(H 10×4 ·[x i ,y I ,z i ,1] T -r i )>0, In the formula, H 10×4 is a matrix containing the ten plane parameters of the field of view; x i ,y i ,z i represents the spatial coordinates of positioning tool i in the optical navigator coordinate system {N}; r i Represents the radius of the enclosing sphere of positioning tool i.

6. The method for collaborative navigation posture control of a surgical robot according to claim 1, characterized in that: In step S303, the method for constructing the constraint condition for avoiding interference between two positioning tools in the perspective of the optical navigator is as follows: 1) The distance between the marker ball of the first positioning tool and the origin of the positioning tool coordinate system is defined as the minimum enclosing sphere radius r1 of the first positioning tool, and the center of the minimum enclosing sphere is the center of the first positioning tool; the distance between the marker ball of the second positioning tool and the origin of the positioning tool coordinate system is defined as the minimum enclosing sphere radius r2 of the second positioning tool, and the center of the minimum enclosing sphere is the center of the first positioning tool; the angle between the left and right cameras of the optical navigator and the boundary of the enclosing sphere is set to be greater than zero; 2) Define the angle between the origin of the optical navigation system coordinate system and the tangent lines of the enclosing spheres of the two positioning tools as γ, and construct the constraint conditions as follows: γ=-min(α 1,2 -α1-α2)>0, In the formula, Represents the vector from the origin of the optical navigation system to the center of the first positioning tool enclosing sphere, Represents the vector from the origin of the optical navigation system to the center point of the second positioning tool's enclosing sphere.

7. The method for collaborative navigation posture control of a surgical robot according to claim 1, characterized in that: In step S4, the update steps of the observation pose of the navigation manipulator are: S401, define the change of the optical navigator's posture relative to the initial posture as small as possible as the optimization goal, and its expression is: Where P k is the position coordinate of the end of the robot arm at time point k, k = 1, 2, ..., N, || p k+1 -p k || means p k to p k+1 The Euclidean distance of S402, substituting the three optimization objective functions constructed by steps S301 and S401 and the constraints constructed by steps S302 to S305 into the generalized differential evolution algorithm for solving, and obtaining the optimal observation posture of the navigation manipulator at the current key point.

8. A control system for implementing the surgical robot collaborative navigation posture control method according to any one of claims 1 to 7, characterized in that: include: A surgical robot module comprises a surgical robot arm (1), a surgical instrument (2) and a first positioning tool (3); the surgical robot arm (1) is a robotic arm with N degrees of freedom, N≥1; the surgical instrument (2) is fixed to the end of the surgical robot arm (1) so that the surgical instrument (2) is driven to move by the surgical robot arm (1); the first positioning tool (3) is arranged at the end of the surgical robot arm (1) so that the spatial position information of the surgical instrument (2) is determined by the first positioning tool (3); The path information module comprises a surgical position fixer (4) and a second positioning tool (5); the surgical position fixer (4) is fixed at the position of the lesion to avoid deviation or error of the planned path caused by the spatial position movement of the lesion; the second positioning tool (5) is fixed on the surgical position fixer (4) to determine the spatial position information of the planned path at the position of the lesion through the second positioning tool (5); A navigation robot module comprises an optical navigator (6) and a navigation mechanical arm (7); the navigation mechanical arm (7) is a mechanical arm with N degrees of freedom, N≥1; the optical navigator (6) is fixed to the end of the navigation mechanical arm (7) so that the position and posture of the optical navigator (6) can be adjusted by the navigation mechanical arm (7); the optical navigator (6) cooperates with a first positioning tool (3) and a second positioning tool (5) respectively, so as to obtain real-time spatial information of a surgical instrument (2) and a lesion position, so that the surgical instrument (2) performs a surgical action at the lesion position along a planned path.

Images