Operation instrument pose determination method and device, readable storage medium and terminal

Abstract

A pose determination method and device of an operating instrument, a readable storage medium and a terminal, the method comprising: determining an image collected for an operating instrument, the operating instrument having a plurality of positioning markers thereon; extracting a positioning marker contour from the image and determining a number of the positioning marker contours; if the number of the positioning marker contours is greater than or equal to a first preset number and less than a number of the positioning markers on the operating instrument, determining a position of a standard centroid of the operating instrument in a target coordinate system according to a standard pose of the operating instrument and corresponding positions of the centroids of the respective positioning marker contours in the target coordinate system; and determining a pose of the operating instrument based on the standard pose of the operating instrument and the position of the standard centroid of the operating instrument in the target coordinate system. The above scheme helps to improve the accuracy of the pose positioning of the operating instrument.

Description

Technical Field

[0001] This invention relates to the field of positioning technology for operating instruments, and in particular to a method and apparatus for determining the position and orientation of an operating instrument, a readable storage medium, and a terminal. Background Technology

[0002] In stereotactic surgery, a surgical navigation system is often required to guide the instruments to the target location. Precisely determining the position of the instruments (e.g., for implantable devices, this position mainly includes the orientation and location of the tip / probe) helps reduce surgical damage and ensures accurate arrival at the target location, thus improving surgical outcomes.

[0003] In the prior art, a stereoscopic positioning surgical navigation system is provided, which uses the light-emitting module of a camera to emit light and acquire images of a surgical instrument (which has several reflective marker balls). Then, the centroid position of each reflective marker ball is determined by image analysis. Next, the centroid of each reflective marker ball is fitted, and the orientation of the surgical instrument is determined based on the fitted plane. The centroid of the surgical instrument is also determined by calculating the average value of the centroid coordinates of each reflective marker ball. Finally, the position and orientation of the end effector are derived based on the known parameters of the end effector.

[0004] In the above scheme, if one or more reflective marker balls on the surgical instrument are obscured or partially obscured during image acquisition (a common occurrence in actual surgery), the posture and centroid position of the surgical instrument determined by the aforementioned planar fitting and coordinate mean calculation methods will deviate from the actual situation. This leads to a decrease in the accuracy and reliability of the surgical instrument's end-effector pose positioning. Summary of the Invention

[0005] The technical problem solved by the embodiments of the present invention is how to improve the accuracy of the position and orientation of the operating instrument.

[0006] To address the aforementioned technical problems, embodiments of the present invention provide a method for determining the pose of an operating device, comprising the following steps: determining an image acquired for the operating device, wherein the operating device has a plurality of positioning marks; extracting the contours of the positioning marks from the image and determining the number of the positioning mark contours; if the number of the positioning mark contours is greater than or equal to a first preset number and less than the number of positioning marks on the operating device, then determining the position of the standard centroid of the operating device in the target coordinate system based on the standard pose of the operating device and the corresponding positions of the centroids of each positioning mark contour in the target coordinate system; and determining the pose of the operating device based on the standard pose of the operating device and the position of the standard centroid in the target coordinate system.

[0007] Optionally, determining the position of the standard centroid of the operating device in the target coordinate system based on the standard posture of the operating device and the corresponding positions of the centroids of each positioning mark contour in the target coordinate system includes: selecting multiple target positioning mark contours from the various positioning mark contours; for each target positioning mark contour, determining the distance between the positioning mark corresponding to the target positioning mark contour and the original centroid of the operating device in the local coordinate system where the operating device is located, denoted as the first offset distance; determining the product of the first offset distance and the rotation matrix, denoted as the first offset vector, wherein the rotation matrix is ​​determined based on the standard posture of the operating device and is used to represent the rotational relationship between the local coordinate system and the target coordinate system; using the first offset vector, offsetting the corresponding position of the centroid of the target positioning mark contour in the target coordinate system to determine the position of the target centroid of the operating device in the target coordinate system; performing a weighted calculation on the coordinates of the multiple target centroids of the operating device in the target coordinate system to obtain the position of the standard centroid of the operating device in the target coordinate system.

[0008] Optionally, selecting multiple target positioning mark contours from the various positioning mark contours includes: taking every two positioning mark contours as a group, determining the distance between the corresponding positions of the centroids of the group of positioning mark contours in the target coordinate system, and determining the distance between the corresponding positioning marks in the local coordinate system where the operating instrument is located, denoted as the first distance and the second distance respectively; determining the difference between the first distance and the second distance, and determining the absolute value of the ratio between the difference and the second distance, and then determining the confidence level of the group of positioning mark contours based on the absolute value, wherein the smaller the absolute value, the greater the confidence level of the group of positioning mark contours; selecting a group of positioning mark contours with a confidence level greater than or equal to a preset confidence threshold, denoted as a candidate positioning mark contour group; and determining the multiple target positioning mark contours from the candidate positioning mark contour group.

[0009] Optionally, determining the plurality of target positioning mark contours from the candidate positioning mark contour groups includes: using all positioning mark contours in the candidate positioning mark contour groups as the target positioning mark contours; or, if the number of candidate positioning mark contour groups is greater than 1, then using the two positioning mark contours contained in the candidate positioning mark contour group with the highest confidence as the target positioning mark contours.

[0010] Optionally, the confidence level of each set of location marker contours can be determined using the following formula:

[0011]

[0012] Among them, P mP represents the coordinates of the centroid of one of the positioning marker contours in the target coordinate system. n This represents the coordinates of the centroid of another positioning mark contour in the target coordinate system, |P n -P n | represents the distance between the centroids of the set of positioning marker contours and their corresponding positions in the target coordinate system, i.e., the first distance. This represents the distance between the positioning marks corresponding to the group of positioning mark contours in the local coordinate system, i.e., the second distance. abs() represents the absolute value operation, and conf represents the confidence level of the group of positioning mark contours.

[0013] Optionally, the pose of the manipulator is the pose of the target end of the manipulator; determining the pose of the manipulator based on its standard pose and the position of its standard centroid in the target coordinate system includes: determining the distance between the target end of the manipulator and its original centroid in the local coordinate system where the manipulator is located, denoted as the second offset distance; determining the product of the second offset distance and the rotation matrix, denoted as the second offset vector, wherein the rotation matrix is ​​determined according to the standard pose of the manipulator and is used to represent the rotational relationship between the local coordinate system and the target coordinate system; using the second offset vector to offset the position of the standard centroid of the manipulator in the target coordinate system to determine the target position of the target end of the manipulator in the target coordinate system; and using the target position of the target end of the manipulator in the target coordinate system and the standard pose of the manipulator as the pose of the target end of the manipulator.

[0014] Optionally, the target position of the target end of the operating instrument in the target coordinate system can be determined using the following formula:

[0015] P end =R×tool end +P0;

[0016] Among them, P end The target position of the target end of the operating instrument in the target coordinate system is represented by R, where R represents the rotation matrix, and tool end P0 represents the second offset distance, and P0 represents the position of the standard centroid of the operating instrument in the target coordinate system.

[0017] Optionally, the rotation matrix is ​​determined using the following formula:

[0018]

[0019]

[0020]

[0021] R = R z *R y *R x ;

[0022] Among them, R x R represents the rotation matrix in the first direction. y R represents the rotation matrix in the second direction. z Let Pitch0 represent the attitude data in the first direction, Roll0 represent the attitude data in the second direction, and Yaw0 represent the attitude data in the third direction. The first direction, the second direction, and the third direction are perpendicular to each other.

[0023] Optionally, the number of positioning marks on the operating instrument is greater than or equal to 3, and the first preset number is greater than or equal to 2.

[0024] Optionally, the method further includes: if the number of positioning mark contours is equal to the number of positioning marks on the operating instrument and is symmetrically distributed relative to the original centroid of the operating instrument, then averaging the coordinates of the corresponding positions of the centroids of each positioning mark contour in the target coordinate system to obtain the position of the standard centroid of the operating instrument in the target coordinate system; and determining the pose of the operating instrument based on the standard posture of the operating instrument and the position of the standard centroid in the target coordinate system.

[0025] Optionally, before determining the position of the standard centroid of the operating instrument in the target coordinate system based on the standard posture of the operating instrument and the corresponding positions of the centroids of each positioning mark contour in the target coordinate system, the method further includes: using an attitude sensor to determine the current posture of the operating instrument, and using the determined current posture as the standard posture of the operating instrument.

[0026] Optionally, the attitude sensor includes an accelerometer, a gyroscope, and a magnetic field sensor, and the attitude sensor is detachably connected to the operating device.

[0027] This invention also provides a pose determination device for an operating instrument, comprising: an image determination module for determining an image acquired for the operating instrument, wherein the operating instrument has a plurality of positioning marks; a positioning mark contour extraction module for extracting positioning mark contours from the image and determining the number of positioning mark contours; a standard centroid determination module for determining the position of the standard centroid of the operating instrument in a target coordinate system based on the standard posture of the operating instrument and the corresponding positions of the centroids of each positioning mark contour in the target coordinate system if the number of positioning mark contours is greater than or equal to a first preset number and less than the number of positioning marks on the operating instrument; and a pose determination module for determining the pose of the operating instrument based on the standard posture of the operating instrument and the position of the standard centroid in the target coordinate system.

[0028] This invention also provides a readable storage medium storing a computer program thereon, which, when executed by a processor, performs the steps of the above-described method for determining the pose of the operating device.

[0029] This invention also provides a terminal, including a memory and a processor. The memory stores a computer program that can run on the processor. When the processor runs the computer program, it executes the steps of the above-described method for determining the pose of the operating device.

[0030] Compared with the prior art, the technical solution of the embodiments of the present invention has the following beneficial effects:

[0031] In actual operation, the positioning marks on the instrument are easily obscured, resulting in the number of mark contours extracted from the images of the instrument being less than the actual number of positioning marks on the instrument. In this case, directly determining the attitude and centroid of the instrument by performing plane fitting and averaging based on the centroids of the extracted positioning mark contours will lead to deviations from the actual situation.

[0032] Based on the above reasons, in this embodiment of the invention, when the number of positioning marker contours is greater than or equal to a first preset number and less than the number of positioning marks on the operating device, the position of the standard centroid of the operating device in the target coordinate system is determined based on the known, high-precision standard posture of the operating device and the corresponding positions of the centroids of each positioning marker contour extracted from the image in the target coordinate system. Then, based on the standard posture of the operating device and the position of the standard centroid in the target coordinate system, the pose of the operating device is determined. This avoids positioning deviation problems caused by occlusion or partial occlusion of positioning marks on the operating device, improving the accuracy and reliability of the pose positioning of the operating device.

[0033] Furthermore, in this embodiment of the invention, every two positioning mark contours are grouped together. The distance between the corresponding positions of the centroids of the group of positioning mark contours in the target coordinate system is determined, as well as the distance between the corresponding positioning marks in the local coordinate system where the operating instrument is located. These distances are denoted as the first distance and the second distance, respectively. Then, the confidence level of the group of positioning mark contours is determined based on the first distance and the second distance. Finally, the target positioning mark contour is determined from the group of positioning mark contours with higher confidence levels. The target positioning mark contour is used to subsequently determine the standard centroid of the operating instrument.

[0034] Compared to existing technologies that directly use the average of the centroids of all extracted positioning marker contours or randomly selected positioning marker contours from an image as the standard centroid of the operating instrument, this embodiment, by employing the aforementioned confidence assessment method, helps to select reliable target positioning marker contours from among the various positioning marker contours. This avoids mistaking noise data in the image caused by noise or other factors for positioning marker contours, thus preventing errors in the subsequent determination of the standard centroid of the operating instrument.

[0035] Furthermore, the method further includes: if the number of positioning mark contours is equal to the number of positioning marks on the operating device and is symmetrically distributed relative to the original centroid of the operating device, then the average coordinates of the corresponding positions of the centroids of each positioning mark contour in the target coordinate system are calculated to obtain the position of the standard centroid of the operating device in the target coordinate system; based on the standard posture of the operating device and the position of the standard centroid in the target coordinate system, the pose of the operating device is determined. In practical applications, the number of positioning mark contours being equal to the number of positioning marks on the operating device means that the positioning marks of the operating device are not occluded. In this case, the average coordinates of the corresponding positions of the centroids of each positioning mark contour extracted from the image in the target coordinate system can be directly used as the coordinates of the position of the standard centroid of the operating device in the target coordinate system. This helps to improve the efficiency of pose localization of the operating device. Attached Figure Description

[0036] Figure 1 This is a flowchart of a method for determining the pose of an operating instrument according to an embodiment of the present invention;

[0037] Figure 2 yes Figure 1 A flowchart of a specific implementation of step S13;

[0038] Figure 3 yes Figure 2 A flowchart of a specific implementation of step S21;

[0039] Figure 4 yes Figure 1A flowchart of a specific implementation of step S14;

[0040] Figure 5 This is a schematic diagram of the structure of a posture determination device for an operating instrument according to an embodiment of the present invention. Detailed Implementation

[0041] To make the above-mentioned objectives, features and beneficial effects of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0042] Reference Figure 1 , Figure 1 This is a flowchart illustrating a method for determining the pose of an operating device according to an embodiment of the present invention. The method can be applied to various terminal devices with data processing capabilities, including but not limited to computers, mobile phones, tablets, smart wearable devices, servers, and cloud platforms.

[0043] The operating instruments described in this embodiment can be surgical instruments commonly used in surgery, including but not limited to electrode implantation instruments, scalpels, hemostats, surgical forceps, etc., or they can be repair instruments used by instrument repair engineers, or various experimental tools used by experimental personnel to perform experimental operations.

[0044] In this embodiment, there are two coordinate systems: a target coordinate system and a local coordinate system (specifically, a coordinate system determined based on the actual position of the operating instrument). The target coordinate system can be a pre-defined three-dimensional spatial coordinate system; for example, the world coordinate system can be used. The local coordinate system can be a pre-defined coordinate system based on the actual position of the operating instrument. For example, the origin of the operating instrument can be the original centroid, and the direction of the vector pointing from the original centroid to the end of the operating instrument (the end typically refers to the end inserted into the object being operated on, such as the tip of an implanted needle) can be used as the z-axis. Two mutually perpendicular vector directions passing through the original centroid on a plane perpendicular to the z-axis can be selected as the x-axis and y-axis, respectively, thus obtaining the local coordinate system.

[0045] The original centroid of the operating instrument can be a predetermined, appropriate position located on the operating instrument.

[0046] The method may include steps S11 to S14:

[0047] Step S11: Determine the image acquired for the operating instrument, which has several positioning marks;

[0048] Step S12: Extract the positioning marker contours from the image and determine the number of the positioning marker contours;

[0049] Step S13: If the number of positioning mark contours is greater than or equal to the first preset number and less than the number of positioning marks on the operating device, then determine the position of the standard centroid of the operating device in the target coordinate system according to the standard posture of the operating device and the corresponding position of the centroid of each positioning mark contour in the target coordinate system.

[0050] Step S14: Determine the pose of the operating instrument based on its standard posture and the position of its standard centroid in the target coordinate system.

[0051] In the specific implementation of step S11, a camera (including a binocular camera and a monocular camera) can be used to acquire images of the operating instrument. The camera can be placed at an appropriate position within a preset distance range of the operating instrument, for example, at an appropriate position directly opposite the operator. The determined images can be real-time acquired images. For example, during the operation such as surgery performed by the operator, the camera can be used to acquire real-time images of the moving operating instrument to obtain the real-time pose of the operating instrument and provide timely feedback; or, historical acquired images can be used to analyze historical parameters of completed surgeries.

[0052] The positioning marks on the operating instrument can be reflective markers (e.g., made of flame-retardant reflective fabric). During image acquisition, a preset light source can illuminate the reflective markers, causing them to reflect light and thus forming easily distinguishable marker outlines in the acquired image. Alternatively, the positioning marks can be made of other materials (e.g., fluorescent materials, brightly colored pigments, etc.), as long as they contribute to forming easily distinguishable marker outlines in the image. In one embodiment, preferably, the positioning marks are symmetrically distributed about the original centroid of the operating instrument.

[0053] The shape of the positioning mark can be selected from a sphere, and the diameter of the sphere can be appropriately set according to the size of the operating instrument. For example, for conventional electrode implantation instruments, the diameter of the sphere can be set within the range of [1mm, 2mm]. Alternatively, the shape can also be set as a cube, cuboid, ellipsoid, or other polygon. This embodiment of the invention does not impose a particular limitation on the shape of the positioning mark, as long as it helps to determine the centroid of the corresponding marker outline in the image later. To facilitate subsequent calculations and improve the accuracy of pose positioning, the operating instrument should have at least three positioning marks, and any three positioning marks should not be collinear. Ideally, all positioning marks should be located on the same plane.

[0054] In a specific implementation of step S12, the positioning marker contours are extracted from the image, and the number of positioning marker contours is determined. Specifically, existing conventional image analysis methods can be used to extract the positioning marker contours.

[0055] In the specific implementation of step S13, on the one hand, if the number of positioning mark contours is less than the number of positioning marks on the operating instrument, it means that some positioning marks on the operating instrument are occluded, so that they do not form corresponding positioning mark contours in the image. In practical applications, the situation of positioning marks on the operating instrument being occluded is quite common. For example, the operator's hand, the movement of other operators' bodies, and the movement of other operating equipment may all cause the positioning marks to be occluded or partially occluded. On the other hand, if the number of positioning mark contours is greater than or equal to a first preset number, the pose determination process in the subsequent steps can be performed based on at least two positioning mark contours. That is to say, the first preset number is the minimum number required by the solution provided in the embodiments of the present invention.

[0056] Reference Figure 2 , Figure 2 yes Figure 1 A flowchart of a specific implementation of step S13. In this embodiment, step S13 determines the position of the standard centroid of the operating instrument in the target coordinate system based on the standard posture of the operating instrument and the corresponding positions of the centroids of each positioning mark contour in the target coordinate system. Specifically, it may include steps S21 to S25.

[0057] In step S21, multiple target positioning mark contours are selected from each positioning mark contour.

[0058] Reference Figure 3 , Figure 3 yes Figure 2 A flowchart illustrating a specific implementation of step S21. Step S21 may specifically include steps S31 to S34.

[0059] In step S31, the distance between the corresponding positions of the centroids of the two positioning mark contours in the target coordinate system is determined, and the distance between the positioning marks corresponding to the two positioning mark contours in the local coordinate system where the operating instrument is located is determined, and these distances are denoted as the first distance and the second distance, respectively.

[0060] In one embodiment, preferably, the positioning marks are symmetrically distributed about the original centroid of the operating instrument.

[0061] The coordinates of the centroid of the positioning mark contour, i.e., the coordinates of the corresponding position of the centroid of the positioning mark contour in the target coordinate system, can be determined based on the mapping relationship between the camera coordinate system or image coordinate system and the target coordinate system. Specific determination methods can refer to conventional methods in the prior art, and will not be elaborated here. It should be noted that for scenarios where images are acquired using a monocular camera, since image analysis methods often only determine the two-dimensional coordinates (e.g., x-axis and y-axis coordinates) of the centroid of each positioning mark contour, the z-axis coordinate can be calculated through the standard posture of the operating device, thereby obtaining the three-dimensional coordinates of the centroid of each positioning mark contour.

[0062] The distance between two positioning markers corresponding to their respective outlines can be the distance between the centroids or geometric centers of the positioning markers. For example, if the positioning marker is a sphere, the distance between the centers of the two spheres can be used as the second distance.

[0063] In step S32, the difference between the first distance and the second distance is determined, and the absolute value of the ratio between the difference and the second distance is determined. Then, the confidence level of the set of positioning mark contours is determined based on the absolute value, wherein the smaller the absolute value, the greater the confidence level of the set of positioning mark contours.

[0064] Furthermore, the confidence level of each set of location marker contours is determined using the following formula:

[0065]

[0066] Among them, P m P represents the coordinates of the centroid of one of the positioning marker contours in the target coordinate system. n This represents the coordinates of the centroid of another positioning mark contour in the target coordinate system, |P m -P n | represents the distance between the centroids of the set of positioning marker contours and their corresponding positions in the target coordinate system, i.e., the first distance, |P mtool -P ntool | represents the distance between the positioning marks corresponding to the group of positioning mark contours in the local coordinate system, i.e., the second distance, abs() represents the absolute value operation, and conf represents the confidence level of the group of positioning mark contours.

[0067] Wherein, the first distance is less than or equal to twice the second distance. The value of is between [0, 1].

[0068] In step S33, a group of location marker contours with a confidence level greater than or equal to a preset confidence threshold is selected and denoted as a candidate location marker contour group.

[0069] In practice, the confidence threshold can be appropriately set according to the actual application scenario. For scenarios with higher requirements for pose positioning accuracy (e.g., surgeries with high risk coefficients and importance), the confidence threshold can be set larger. Without limitation, the confidence threshold can be selected from the range [80%, 95%], for example, it can be set to 90%.

[0070] In step S34, the plurality of target positioning mark contours are determined from the candidate positioning mark contour group.

[0071] In one specific implementation, all the positioning marker contours in the candidate positioning marker contour group can be used as the target positioning marker contour. This maximizes the number of target positioning marker contours participating in the subsequent calculation process to determine the standard centroid, and the richer data helps to obtain a more accurate and reliable standard centroid.

[0072] In another specific implementation, if the number of candidate positioning marker contour groups is greater than one, then the two positioning marker contours contained in the candidate positioning marker contour group with the highest confidence are selected as the target positioning marker contour. Thus, by selecting the target positioning marker contour with the highest confidence, the amount of computational data can be minimized, improving computational efficiency. Furthermore, it also helps to reduce accumulated errors.

[0073] In this embodiment of the invention, compared to the prior art which directly uses the average of the centroids of all extracted positioning mark contours or randomly selected positioning mark contours from an image as the standard centroid of the operating instrument, this embodiment, by employing the aforementioned confidence assessment method, helps to select accurate and reliable target positioning mark contours from each positioning mark contour. Therefore, it avoids mistaking noise data in the image caused by noise or other factors as positioning mark contours, which could lead to errors in the subsequently determined standard centroid of the operating instrument.

[0074] Continue to refer to Figure 2 In step S22, for each target positioning mark contour, in the local coordinate system where the operating instrument is located, the distance between the positioning mark corresponding to the target positioning mark contour and the original centroid of the operating instrument is determined and recorded as the first offset distance.

[0075] In step S23, the product of the first offset distance and the rotation matrix is ​​determined and denoted as the first offset vector. The rotation matrix is ​​determined according to the standard posture of the operating instrument and is used to represent the rotational relationship between the local coordinate system and the target coordinate system.

[0076] In step S24, the first offset vector is used to offset the corresponding position of the centroid of the target positioning mark contour in the target coordinate system, so as to determine the position of the target centroid of the operating instrument in the target coordinate system.

[0077] In step S25, the coordinates of the multiple target centroids of the operating instrument in the target coordinate system are weighted to obtain the position of the standard centroid of the operating instrument in the target coordinate system.

[0078] In one specific embodiment, if the number of the positioning mark contours is equal to the number of positioning marks on the operating instrument and is symmetrically distributed relative to the original centroid of the operating instrument, then the coordinates of the corresponding positions of each target centroid in the target coordinate system can be averaged to obtain the position of the standard centroid of the operating instrument in the target coordinate system.

[0079] In another specific implementation, an appropriate weighting ratio can be used to perform a weighted calculation on the coordinates of the corresponding positions of each target centroid in the target coordinate system to obtain the position of the standard centroid of the operating instrument in the target coordinate system. The weighting ratio can be determined based on the confidence level of the target positioning mark contour corresponding to each target centroid; the higher the confidence level of the target positioning mark contour, the greater the weight of the corresponding target centroid. Specifically, the confidence level of the target positioning mark contour can refer to… Figure 3 The confidence level of the candidate positioning mark contour group to which the target positioning mark contour belongs, as described in the illustrated embodiment.

[0080] Furthermore, prior to step S13, the method further includes: using an attitude sensor to acquire the current attitude of the operating instrument, and using the acquired current attitude as the standard attitude of the operating instrument.

[0081] The attitude sensor includes an accelerometer, a gyroscope, and a magnetic field sensor, and is detachably connected to the operating device.

[0082] In existing technologies, the pose of a plane obtained by plane fitting of the contours of various positioning markers extracted from an image is used as the actual pose of the operating device. Considering the occlusion problem of positioning markers, the fitted plane pose will deviate from the actual pose of the operating device. In contrast, in this embodiment of the invention, the pose of the operating device obtained by the pose sensor containing multiple types of sensors is used as the standard pose (actual pose) of the operating device. This standard pose has higher accuracy, and thus a higher accuracy standard centroid can be determined based on the higher accuracy standard pose, improving the accuracy and reliability of the pose positioning of the operating device.

[0083] In the specific implementation of step S14, the pose of the operating instrument is determined based on the standard posture of the operating instrument and the position of the standard centroid in the target coordinate system.

[0084] Figure 4 yes Figure 1 A flowchart illustrating a specific implementation of step S14. In this embodiment, the pose of the operating instrument is the pose of the target end of the operating instrument. Step S14 may specifically include steps S41 to S44.

[0085] In step S41, in the local coordinate system where the operating instrument is located, the distance between the target end of the operating instrument and the original centroid of the operating instrument is determined and denoted as the second offset distance.

[0086] The target end of the operating instrument can refer to the end that is inserted into the object being operated on, such as the tip of an implantation needle.

[0087] In step S42, the product of the second offset distance and the rotation matrix is ​​determined and denoted as the second offset vector. The rotation matrix is ​​determined according to the standard posture of the operating instrument and is used to represent the rotational relationship between the local coordinate system and the target coordinate system.

[0088] Furthermore, the rotation matrix can be determined using the following formula:

[0089]

[0090]

[0091]

[0092] R = R z *R y *R x ;

[0093] Among them, R x R represents the rotation matrix in the first direction. y R represents the rotation matrix in the second direction. z Let Pitch0 represent the attitude data in the first direction, Roll0 represent the attitude data in the second direction, and Yaw0 represent the attitude data in the third direction. The first direction, the second direction, and the third direction are perpendicular to each other.

[0094] In step S43, the second offset vector is used to offset the position of the standard centroid of the operating instrument in the target coordinate system, so as to determine the target position of the target end of the operating instrument in the target coordinate system.

[0095] Furthermore, the target position of the target end of the operating instrument in the target coordinate system can be determined using the following formula:

[0096] P end =R×tool end +P0;

[0097] Among them, P end The target position of the target end of the operating instrument in the target coordinate system is represented by R, where R represents the rotation matrix, and tool end P0 represents the second offset distance, and P0 represents the position of the standard centroid of the operating instrument in the target coordinate system.

[0098] In step S44, the target position of the target end of the operating instrument in the target coordinate system and the standard posture of the operating instrument are taken as the pose of the target end of the operating instrument.

[0099] In this embodiment of the invention, as mentioned above, since the standard posture has high precision, the rotation matrix determined based on the standard posture is also accurate and reliable, thereby accurately determining the target position of the target end of the operating device in the target coordinate system.

[0100] It should be pointed out that, Figure 1 The specific implementation of step S14 in the illustrated embodiment is not limited to the above. Figure 4 The embodiment shown describes the scheme. In specific implementations, other conventional methods can also be used to determine the pose of the manipulator. For example, based on the known relative position parameters (e.g., distance parameters) between the original centroid of the manipulator and the target end of the manipulator, the target position of the target end in the target coordinate system can be determined by manual estimation or by looking up a pre-determined mapping table, and then the pose of the manipulator can be obtained by combining the standard pose.

[0101] Furthermore, the method further includes: if the number of positioning mark contours is equal to the number of positioning marks on the operating instrument and is symmetrically distributed relative to the original centroid of the operating instrument, then the average coordinates of the corresponding positions of the centroids of each positioning mark contour in the target coordinate system are calculated to obtain the position of the standard centroid of the operating instrument in the target coordinate system; based on the standard posture of the operating instrument and the position of the standard centroid in the target coordinate system, the pose of the operating instrument is determined.

[0102] In this embodiment of the invention, if the number of the positioning mark contours is equal to the number of positioning marks on the operating instrument, it means that the positioning marks on the operating instrument are not obscured. In this case, in order to improve positioning efficiency and reduce computational overhead, the standard centroid of the operating instrument can be obtained directly by the mean calculation method.

[0103] Reference Figure 5 , Figure 5 This is a schematic diagram of the structure of a pose determination device for an operating instrument according to an embodiment of the present invention. The pose determination device for the operating instrument may include:

[0104] Image determination module 51 is used to determine an image acquired for an operating instrument, the operating instrument having several positioning marks;

[0105] The positioning marker contour extraction module 52 is used to extract positioning marker contours from the image and determine the number of positioning marker contours;

[0106] The standard centroid determination module 53 is used to determine the position of the standard centroid of the operating device in the target coordinate system based on the standard posture of the operating device and the corresponding position of the centroid of each positioning mark contour in the target coordinate system if the number of the positioning mark contours is greater than or equal to a first preset number and less than the number of positioning marks on the operating device.

[0107] The pose determination module 54 is used to determine the pose of the operating instrument based on the standard posture of the operating instrument and the position of the standard centroid in the target coordinate system.

[0108] For the principle, specific implementation, and beneficial effects of the pose determination device for this operating instrument, please refer to the previous text and... Figures 1 to 4 The description of the method for determining the position of the operating instrument shown is not repeated here.

[0109] This invention also provides a readable storage medium, such as a computer-readable storage medium, on which a computer program is stored, the computer program being executed by a processor. Figures 1 to 4 The steps of the method for determining the pose of the operating device are shown. The computer-readable storage medium may include non-volatile or non-transitory memory, and may also include optical disks, hard disks, solid-state drives, etc.

[0110] Specifically, in this embodiment of the invention, the processor can be a central processing unit (CPU), or it can be other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor or any conventional processor.

[0111] It should also be understood that the memory in the embodiments of this application can be volatile memory or non-volatile memory, or may include both volatile and non-volatile memory. The non-volatile memory can be read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), or flash memory. The volatile memory can be random access memory (RAM), which is used as an external cache. By way of example, but not limitation, many forms of random access memory (RAM) are available, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced synchronous DRAM (ESDRAM), synchronous linked DRAM (SLDRAM), and direct rambus RAM (DR RAM).

[0112] This invention also provides a terminal, including a memory and a processor. The memory stores a computer program that can run on the processor, and when the processor runs the computer program, it performs the above-described... Figures 1 to 4The steps of the method for determining the pose of the operating device are shown. The terminal may include, but is not limited to, terminal devices such as mobile phones, computers, and tablets, and may also be servers, cloud platforms, etc.

[0113] It should be understood that the term "and / or" in this article is merely a description of the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, or B existing alone. Additionally, the character " / " in this article indicates that the preceding and following related objects have an "or" relationship.

[0114] In the embodiments of this application, "multiple" refers to two or more.

[0115] The descriptions of "first," "second," etc., appearing in the embodiments of this application are for illustrative purposes and to distinguish the objects being described. They have no order and do not indicate any special limitation on the number of devices in the embodiments of this application, nor do they constitute any limitation on the embodiments of this application.

[0116] It should be noted that the sequence number of each step in this embodiment does not represent a limitation on the execution order of each step.

[0117] While the present invention has been disclosed above, it is not limited thereto. Any person skilled in the art can make various modifications and alterations without departing from the spirit and scope of the invention; therefore, the scope of protection of the present invention should be determined by the scope defined in the claims.

Claims

1. A method for determining the pose of an operating instrument, characterized in that, include: The image acquired for the operating instrument is determined, and the operating instrument has several positioning marks. Extract the positioning marker contours from the image and determine the number of the positioning marker contours; If the number of positioning mark contours is greater than or equal to a first preset number and less than the number of positioning marks on the operating device, then the position of the standard centroid of the operating device in the target coordinate system is determined according to the standard posture of the operating device and the corresponding position of the centroid of each positioning mark contour in the target coordinate system. The step of determining the position of the standard centroid of the operating device in the target coordinate system based on the standard posture of the operating device and the corresponding positions of the centroids of each positioning mark contour in the target coordinate system includes: selecting multiple target positioning mark contours from the various positioning mark contours; for each target positioning mark contour, determining the distance between the positioning mark corresponding to the target positioning mark contour and the original centroid of the operating device in the local coordinate system where the operating device is located, denoted as the first offset distance; determining the product of the first offset distance and the rotation matrix, denoted as the first offset vector, wherein the rotation matrix is ​​determined based on the standard posture of the operating device and is used to represent the rotation relationship between the local coordinate system and the target coordinate system; using the first offset vector, offsetting the corresponding position of the centroid of the target positioning mark contour in the target coordinate system to determine the position of the target centroid of the operating device in the target coordinate system; and performing a weighted calculation on the coordinates of the multiple target centroids of the operating device in the target coordinate system to obtain the position of the standard centroid of the operating device in the target coordinate system. The pose of the operating instrument is determined based on its standard posture and the position of its standard centroid in the target coordinate system.

2. The method according to claim 1, characterized in that, The step of selecting multiple target positioning mark contours from various positioning mark contours includes: For every two positioning mark contours as a group, the distance between the corresponding positions of the centroids of the group of positioning mark contours in the target coordinate system is determined, and the distance between the positioning marks corresponding to the group of positioning mark contours in the local coordinate system where the operating instrument is located is determined, which are respectively denoted as the first distance and the second distance. The difference between the first distance and the second distance is determined, and the absolute value of the ratio between the difference and the second distance is determined. Then, the confidence level of the set of positioning mark contours is determined based on the absolute value, wherein the smaller the absolute value, the greater the confidence level of the set of positioning mark contours. Select the group of location marker contours with a confidence level greater than or equal to the preset confidence threshold, and record them as the candidate location marker contour group; The plurality of target positioning mark contours are determined from the candidate positioning mark contour group.

3. The method according to claim 2, characterized in that, Determining the plurality of target location marker contours from the candidate location marker contour group includes: All the positioning mark contours in the candidate positioning mark contour group are used as the target positioning mark contour; or, If the number of candidate positioning marker contour groups is greater than 1, then the two positioning marker contours contained in the candidate positioning marker contour group with the highest confidence level shall be used as the target positioning marker contour.

4. The method according to claim 2, characterized in that, The confidence level of each set of location marker contours is determined using the following formula: in, This represents the coordinates of the centroid of one of the positioning mark contours in the target coordinate system. This indicates the coordinates of the centroid of another positioning mark contour in the target coordinate system. This represents the distance between the centroids of the set of positioning marker contours at their corresponding positions in the target coordinate system, i.e., the first distance. This represents the distance between the positioning marks corresponding to the contours of this set of positioning marks in the local coordinate system, i.e., the second distance. `abs()` indicates the operation of taking the absolute value. conf This indicates the confidence level of the group of location marker outlines.

5. The method according to claim 1, characterized in that, The pose of the operating instrument is the pose of the target end of the operating instrument. Based on the standard posture of the manipulator and the position of its standard centroid in the target coordinate system, the pose of the manipulator is determined, including: In the local coordinate system where the operating instrument is located, the distance between the target end of the operating instrument and the original centroid of the operating instrument is determined and denoted as the second offset distance; The product of the second offset distance and the rotation matrix is ​​determined and denoted as the second offset vector, wherein the rotation matrix is ​​determined according to the standard posture of the operating instrument and is used to represent the rotational relationship between the local coordinate system and the target coordinate system; Using the second offset vector, the position of the standard centroid of the operating instrument in the target coordinate system is offset to determine the target position of the target end of the operating instrument in the target coordinate system; The target position of the target end of the operating instrument in the target coordinate system and the standard posture of the operating instrument are taken as the pose of the target end of the operating instrument.

6. The method according to claim 5, characterized in that, The target position of the target end of the operating instrument in the target coordinate system is determined using the following formula: ; in, This indicates the target position of the target end of the operating instrument in the target coordinate system. Represents the rotation matrix, This indicates the second offset distance. This indicates the position of the standard centroid of the operating instrument in the target coordinate system.

7. The method according to claim 1 or 5, characterized in that, The rotation matrix is ​​determined using the following formula: ； ； ； ； in, Represents the rotation matrix in the first direction. This represents the rotation matrix in the second direction. This represents the rotation matrix in the third direction. This represents the attitude data in the first direction. This represents the attitude data in the second direction. This represents the pose data in a third direction, where the first direction, the second direction, and the third direction are perpendicular to each other.

8. The method according to claim 1, characterized in that, The number of positioning marks on the operating instrument is greater than or equal to 3, and the first preset number is greater than or equal to 2.

9. The method according to claim 1, characterized in that, The method further includes: If the number of the positioning mark contours is equal to the number of positioning marks on the operating instrument and is symmetrically distributed relative to the original centroid of the operating instrument, then the average coordinates of the corresponding positions of the centroids of each positioning mark contour in the target coordinate system are calculated to obtain the position of the standard centroid of the operating instrument in the target coordinate system. The pose of the operating instrument is determined based on its standard posture and the position of its standard centroid in the target coordinate system.

10. The method according to claim 1, characterized in that, Before determining the position of the standard centroid of the operating instrument in the target coordinate system based on the standard posture of the operating instrument and the corresponding positions of the centroids of each positioning mark contour in the target coordinate system, the method further includes: An attitude sensor is used to obtain the current attitude of the operating instrument, and the obtained current attitude is used as the standard attitude of the operating instrument.

11. The method according to claim 10, characterized in that, The attitude sensor includes an accelerometer, a gyroscope, and a magnetic field sensor, and the attitude sensor is detachably connected to the operating instrument.

12. A device for determining the position and orientation of an operating instrument, characterized in that, include: An image determination module is used to determine an image acquired for an operating instrument, the operating instrument having several positioning marks; A positioning marker contour extraction module is used to extract positioning marker contours from the image and determine the number of positioning marker contours; The standard centroid determination module is used to determine the position of the standard centroid of the operating device in the target coordinate system based on the standard posture of the operating device and the corresponding positions of the centroids of each positioning mark contour in the target coordinate system if the number of positioning mark contours is greater than or equal to a first preset number and less than the number of positioning marks on the operating device. The step of determining the position of the standard centroid of the operating device in the target coordinate system based on the standard posture of the operating device and the corresponding positions of the centroids of each positioning mark contour in the target coordinate system includes: selecting multiple target positioning mark contours from the various positioning mark contours; and for each target positioning mark contour, determining the target centroid in the local coordinate system where the operating device is located. The distance between the positioning mark corresponding to the marked contour and the original centroid of the operating device is denoted as the first offset distance; the product of the first offset distance and the rotation matrix is ​​determined and denoted as the first offset vector, wherein the rotation matrix is ​​determined according to the standard posture of the operating device and is used to represent the rotational relationship between the local coordinate system and the target coordinate system; the first offset vector is used to offset the corresponding position of the centroid of the target positioning mark contour in the target coordinate system to determine the position of the target centroid of the operating device in the target coordinate system; the coordinates of the positions of multiple target centroids of the operating device in the target coordinate system are weighted to obtain the position of the standard centroid of the operating device in the target coordinate system; The pose determination module is used to determine the pose of the operating instrument based on its standard posture and the position of its standard centroid in the target coordinate system.

13. A readable storage medium having a computer program stored thereon, characterized in that, When the computer program is run by the processor, it performs the steps of the pose determination method of the operating instrument according to any one of claims 1 to 11.

14. A terminal comprising a memory and a processor, wherein the memory stores a computer program capable of running on the processor, characterized in that, When the processor runs the computer program, it performs the steps of the pose determination method of the operating instrument according to any one of claims 1 to 11.

Images