Spatial conversion relation determination method based on opencv

By using OpenCV and singular value decomposition algorithm to determine the spatial transformation relationship of the surgical robot navigation module, the problem of complex calibration process and error accumulation is solved, and the calibration accuracy and operational precision are improved.

CN119017388APending Publication Date: 2024-11-26ZHUHAI HENGQIN ALL-STAR MEDICAL TECHNOLOGY CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202411242398.X
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-09-05
Publication Date
2024-11-26

AI Technical Summary

Technical Problem

In the prior art, the calibration process of the navigation module of the surgical robot is complex and prone to errors, which leads to the accumulation of calibration errors and affects the accuracy of operation.

Method used

The singular value decomposition method based on OpenCV is adopted. By determining the specific spatial arrangement order of the marker balls at the end of the robotic arm, multiple sets of spatial position and pose data are obtained. The spatial transformation relationship matrix is ​​calculated using the singular value decomposition algorithm. Combined with the OpenCV hand-eye calibration function, the spatial transformation relationship between the navigator and the robotic arm base is determined.

Benefits of technology

This reduces the computational complexity and error accumulation of spatial coordinate system transformation, and improves the accuracy of the spatial transformation relationship between the robotic arm and the optical navigation system.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119017388A_ABST
    Figure CN119017388A_ABST
Patent Text Reader

Abstract

The invention provides a method for determining a space conversion relation based on opencv. The method comprises the steps that the number of small marking balls is determined, the small marking balls are fixed to the side face of the tail end of the mechanical arm according to a specific space sequence, and a first space coordinate system is determined based on the space sequence; determining a plurality of groups of first space conversion relations based on the plurality of groups of first space and second space coordinate systems; based on the third space coordinate system, multiple groups of second space conversion relations are determined; a third spatial conversion relationship is determined based on the plurality of groups of first and second spatial conversion relationships. Wherein the first space coordinate system is a coordinate system established by the small marking ball, the second space coordinate system is a navigator coordinate system, and the third space coordinate system is a mechanical arm base coordinate system. The first space conversion relation is the space relation of the small marking ball relative to the navigator, the second space relation is the space relation of the tail end TCP of the mechanical arm relative to the base, and the third space relation is the space relation of the navigator relative to the base. According to the technical scheme of the embodiment of the invention, the calculation steps in the calibration process can be reduced, and the calibration precision can be improved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the technical field of surgical robot positioning and navigation, and in particular to a method for determining a spatial transformation relationship based on OpenCV. Background Art

[0002] Medical surgical robots are high-tech equipment that integrates computer technology, medical expertise, mechanical engineering and artificial intelligence, and are designed to assist medical personnel in performing precise and efficient surgical operations. These surgical robots are usually composed of a robotic arm, a navigation module, a force sensing module and surgical instruments. The navigation module is equivalent to the "eyes" of the robotic arm, which is crucial to the operating accuracy of the surgical robot. The core of the navigation module lies in the calibration algorithm between the robotic arm and the navigator, that is, determining the spatial conversion relationship between them. In most calibration processes, the calibration process involves a very complex calculation process. In this complex calculation process, not only the calibration error is accumulated but also errors are prone to occur. Therefore, in order to improve the calibration accuracy and reduce the calculation complexity in the calibration process, the present invention proposes a method for determining the spatial conversion relationship based on OpenCV. Summary of the invention

[0003] In response to the challenges mentioned in the background technology section, the present invention provides an innovative method, a method for determining a spatial transformation relationship based on OpenCV, to determine the spatial transformation relationship so as to improve the calibration accuracy and reduce the computational complexity during the calibration process.

[0004] The embodiment of the present invention provides a method for determining a spatial transformation relationship based on singular value decomposition, which adopts the following technical solutions:

[0005] 1) Determine the number of marker balls and fix them on the side of the end of the robotic arm in a specific spatial arrangement order, and establish the first spatial coordinate system based on the spatial arrangement order;

[0006] 2) moving and rotating the robotic arm within the working range of the navigator to obtain multiple sets of spatial position data of the first spatial coordinate system with respect to the second spatial coordinate system and multiple sets of posture data of the third spatial coordinate system;

[0007] 3) determining a plurality of sets of the first spatial conversion relationships based on a plurality of sets of spatial position data in the first spatial coordinate system and the second spatial coordinate system;

[0008] 4) determining multiple sets of second space conversion relationships based on multiple sets of position and posture data in the third space coordinate system;

[0009] 5) Determine the third space conversion relationship based on multiple groups of the first space conversion relationship and the second space conversion relationship.

[0010] 1. Preferably, the number of marking balls in step 1) is a sufficient number of marking balls required to determine the position and direction of the end effector of the robot arm, and the first spatial coordinate system is a world coordinate system that is different from the marking balls for the navigator coordinate system and the robot arm base and the TCP coordinate system of the end of the robot arm.

[0011] 2. Preferably, the multiple sets of data in step 2) are the minimum number of data required to improve the accuracy and robustness of the calibration results, wherein

[0012] The spatial position data of the first spatial coordinate system relative to the second spatial coordinate system is the three-dimensional spatial coordinate matrix of all the marking balls relative to the navigator after each movement and rotation of the robotic arm. The posture data in the third spatial coordinate system is the posture data matrix of the TCP of the robotic arm end relative to the robotic arm base after each movement and rotation of the robotic arm end, which is usually composed of three-dimensional spatial coordinates and quaternions.

[0013] 3. Preferably, the multiple groups of first spatial transformation relationships in step 3) are obtained by using an algorithm for decomposing singular values ​​of the coordinate points of the multiple groups of marked balls in the first spatial coordinate system to obtain the spatial transformation relationship matrices of the multiple groups of marked balls for the navigator. In addition, the first spatial transformation relationship is composed of the first rotation matrix and the first translation matrix.

[0014] 4. Preferably, the multiple groups of second space transformation relationships in step 4) are the multiple groups of third space coordinate systems, that is, the space coordinate systems of the robot end TCP to the robot base. The posture data of the robot end TCP to the robot base can usually be obtained by the robot teach pendant or the ROS interface, and the multiple groups of posture data can be converted into the second space transformation relationship matrix through conversion. In addition, the second space transformation relationship is composed of the second rotation matrix and the second translation matrix.

[0015] 5. Preferably, the multiple groups of third spatial conversion relationships in step 5) are based on the multiple groups of first conversion relationships and the second conversion relationships. The matrix arrays of the first rotation matrix and the multiple groups of first translation matrices in the multiple groups of first spatial conversion relationships and the matrix arrays of the multiple groups of second rotation matrices and the second translation matrices in the second spatial conversion relationships are passed as parameters into the opencv hand-eye calibration function calibrateHandEye to obtain the spatial conversion relationship of the navigator for the robotic arm base, that is, the third spatial conversion relationship. In addition, the third spatial conversion relationship is composed of the third rotation matrix and the third translation matrix.

[0016] The implementation of the technical solution of the present invention reduces the conversion calculations between spatial coordinate systems, thereby indirectly reducing the error accumulation of each step, thereby improving the accuracy of the spatial conversion relationship between the robotic arm and the optical navigator. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0018] Figure 1 is a flow chart of a method for determining a spatial transformation relationship based on an opencv algorithm provided according to an embodiment of the present invention;

[0019] Figure 2 is a structural schematic diagram of a surgical robot provided according to an example of the present invention; DETAILED DESCRIPTION

[0020] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0021] The terms "first", "second" and the like in the specification and claims of the present invention and the above drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence.

[0022] Figure 1 This is a flow chart of a method for determining a spatial transformation relationship based on an OpenCV algorithm according to an embodiment of the present invention. This embodiment can be applied to determining a spatial transformation relationship between a coordinate system to which a navigator belongs and a coordinate system of a robotic arm base. The method can be implemented by hardware and software of the spatial transformation relationship. The hardware of the spatial transformation relationship can use a six-axis robotic arm and an optical navigator, and the software can use an OpenCV algorithm.

[0023] like Figure 1 As shown, the method of this embodiment may specifically include:

[0024] S110 determines the number of marking balls and fixes them on the side of the end of the robot arm according to a specific spatial arrangement order, and establishes a first spatial coordinate system based on the spatial arrangement order.

[0025] It should be noted that this embodiment is composed of a mechanical arm ( Figure 2 1) in the Marking Ball ( Figure 2 2) and the navigator ( Figure 2 3) in the figure. The robot arm is a six-axis robot arm that can replace human hands to perform some high-precision surgical operations. The marker ball is a ball coated with a special coating material that can reflect near-infrared light for optical positioning. The navigator is a binocular optical camera that can emit and receive near-infrared light to detect the position of the marker ball.

[0026] In the embodiment of the present invention, a number of optical balls are placed within the working range of the robot arm and the optical navigator.

[0027] In a specific implementation, the number of the optical balls is determined, and the number of marker balls is the minimum number of marker balls for determining the third transformation relationship. Using four marker balls can provide sufficient information to determine the position and direction of the end effector of the robot arm. Specifically, the point cloud formed by the four marker balls in space can be used to optimize the calculation of spatial position and posture through methods such as the least squares method. Through these four points, a quaternion or matrix model can be established to solve the posture estimation problem, thereby achieving accurate calibration and control. In three-dimensional space, the four marker points can be used to solve the posture through the least squares method. Suppose the coordinates of the marker points are (x k ,y k ,z k )(1≤k≤4) The actual target point is (x,y,z), and the calculation formula can be expressed in the following matrix form:

[0028] Position relationship equation:

[0029]

[0030] Least squares solution:

[0031] Ap=b

[0032] Among them, A is the coordinate matrix of the four points, p is the pose vector to be calculated, and b is the target position deviation:

[0033] p=(A T A) -1 A T b

[0034] Therefore, the number of marker balls is 4, and the position relationship of the target can be accurately estimated. The specific spatial arrangement order is that the four marker balls are fixedly placed in a regular order in which all the balls are not in the same plane, and the positions are relatively regular, so that the first spatial coordinate system of the marker balls can be determined in this specific spatial order. According to the first spatial coordinate system C m ( Figure 2), determine the spatial position matrix A of all balls in the first spatial coordinate system m .

[0035] S120 moves and rotates the manipulator within the working range of the navigator to obtain multiple sets of spatial position data of the first spatial coordinate system in the second spatial coordinate system and multiple sets of posture data in the third spatial coordinate system. The manipulator is controlled to move and rotate within the working range of the navigator for no less than i times (i≥10), and the range of movement covers most of the working range of the navigator as much as possible. The diversity of the acquired data is positively correlated with the accuracy and robustness of the final calibration result. The manipulator moves i times, and records the i-th second spatial coordinate system C o ( Figure 2 ) under spatial location data A oi and the third space coordinate system C r ( Figure 2 )’s pose data T ri After moving i times, i spatial position data matrices {A o0, A o1 …A oi} and the pose data matrix {T r0 ,T r1 …T ri}.

[0036] S130 Based on the multiple groups of first space coordinate systems C m and the spatial position data matrix {A o0, A o1 …A oi}, determine the multiple groups of first space conversion relationships ( Figure 2 ).

[0037] In the specific implementation, for each set of space conversion relationships The first spatial transformation relationship of the i-th group is the spatial position data matrix A under the first spatial transformation relationship m The i-th spatial position matrix under the second spatial transformation relationship and the spatial position data matrix A under the second spatial transformation relationship o The optimal solution of the i-th spatial position matrix under the decomposition singular value algorithm is obtained. and It consists of the following points:

[0038]

[0039]

[0040] in The i-th spatial position matrix under the first spatial transformation relationship, that is, the three-dimensional coordinates of the marker ball in the world coordinate system, is fixed, so It can be expressed as:

[0041] A m =P m

[0042] is the i-th spatial position matrix in the second spatial coordinate system, that is, the three-dimensional coordinate point matrix of the optical ball under the navigator obtained by the manipulator after the position is changed i times. m is the spatial position data in the first spatial coordinate system, is the i-th spatial position data under the second spatial coordinates. In addition, P m and They are composed of the three-dimensional coordinate matrices of four labeled balls:

[0043]

[0044]

[0045] in, is the three-dimensional coordinate of the nth marker ball in the first spatial coordinate system, is the three-dimensional coordinate of the nth marker ball after the i-th robotic arm changes its posture in the second spatial coordinate system:

[0046]

[0047]

[0048] Based on p m and The SVD algorithm is used to calculate the i-th spatial transformation matrix of the first spatial transformation relationship with respect to the second spatial transformation relationship, that is, the i-th first spatial transformation relationship The calculation steps are as follows:

[0049] 1) Calculate the centroid C of the point set m and C o :

[0050]

[0051]

[0052] 2) Centralized point set A′ m and A′ o :

[0053] A′ m =Am -C m 1 T

[0054] A′ o =A o -C o 1 T

[0055] 3) Calculate the covariance matrix H:

[0056] H=A′ m A′ o

[0057] 4) Perform SVD decomposition on the covariance H, where U and V are orthogonal matrices and S is a diagonal matrix:

[0058] H=USV T

[0059] 5) Calculate the rotation matrix R:

[0060] R=VU T

[0061] 6) If det(R)<0, R needs to be adjusted:

[0062] V(:,n)=V(:,n)*-1

[0063] R=VU T

[0064] 7) Calculate the translation vector t:

[0065] t=C o -RC m

[0066] Through the above algorithm calculation, the i first rotation matrices can be determined and the first translation matrix Based on the i-th first space conversion relationship It can be expressed as follows:

[0067]

[0068] S140 Based on the third space coordinate system C r , determine multiple sets of second space transformation relationships ( Figure 2 ). In the above implementation, the i groups of posture data in the third space coordinate system have been obtained through the robot arm teaching device or the ROS system interface {T r0 ,T r1 …T ri}, where for each set of pose data T ri It consists of the following data:

[0069] T r =(x,y,z,q x ,q y ,q z ,q w )

[0070] Where (x, y, z) is the three-dimensional coordinate point, (q x ,q y ,q z ,q w ) is a quaternion, q w is the real part, q x ,q y ,q z is the imaginary part; the formula for converting the quaternion to a rotation matrix is ​​as follows:

[0071]

[0072] According to the above formula, the plurality of sets of pose data in the third space coordinate system can be converted into the plurality of sets of second rotation matrices and second translation matrices. The specific method is as follows:

[0073] 1) Calculate the rotation matrix of the i-th group of posture data in the third space

[0074]

[0075] 2) Calculate the translation matrix of the i-th group of posture data in the third spatial coordinate system

[0076]

[0077] After the above calculation, the i second rotation matrices can be obtained: and the first translation matrix Based on the i-th second space conversion relationship It can be expressed as follows:

[0078]

[0079] S150: Determine the third spatial transformation relationship based on the multiple sets of first and second spatial transformation relationships. is the spatial transformation relationship from the navigator to the robotic arm base, To mark the spatial transformation relationship from the ball to the TCP at the end of the robot arm, is the spatial transformation relationship from the TCP at the end of the robot arm to the base of the robot arm, is the spatial transformation relationship from the navigator to the marker ball, according to Figure 2 You know, to make sure The spatial transformation relationship, that is, the third spatial transformation relationship, can be obtained by the following matrix transformation relationship:

[0080]

[0081] because That is, the spatial transformation relationship between the marker ball and the TCP at the end of the robot arm is a fixed parameter, which can be eliminated by two posture transformations. is the inverse of the spatial transformation relationship between the end of the robot and the base of the robot, is the inverse of the spatial transformation matrix from the navigator to the optical sphere, and the following equation can be obtained:

[0082]

[0083] According to the transformation relationship between the homogeneous matrix and the inverse of the homogeneous matrix, the above equation is equivalent to:

[0084]

[0085] After two consecutive eliminations, the 0th and the 1st You can get:

[0086]

[0087] Where (0) and (1) in the above equations are the spatial transformation matrices obtained by the robot arm's posture transformation for the 0th and 1st times. is relatively stationary, so the above equation is equivalent to:

[0088]

[0089] Multiply both sides by and We can get:

[0090]

[0091] The above equation can be simplified to the problem of AX = XB, where:

[0092]

[0093]

[0094]

[0095] Among them, and is the inverse of the spatial transformation matrix of the robot base to the TCP at the end of the robot arm and the spatial transformation matrix of the robot base to the TCP at the end of the robot arm. and is the spatial transformation matrix of the marking ball for the navigator and the inverse of the spatial transformation matrix of the marking ball for the navigator. These are all known variables. The final result is X, that is The navigator's transformation matrix for the robot arm base coordinate system, that is, the third space transformation relationship, substitutes multiple groups of A and B's space transformation relationships, that is, the multiple groups of first and second rotation matrices and translation matrices, into the opencv hand-eye calibration algorithm calibrateHandEye function. The fifth and sixth parameters in the function are the calculated rotation matrix and translation matrix of the calibration matrix, that is, the third space rotation matrix and translation matrix.

[0096] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope defined in the claims.

Claims

1. A method for determining a spatial transformation relationship based on opencv, characterized in that: The following steps are involved: 1) Determine the number of marker balls and fix them on the side of the end of the robotic arm in a specific spatial arrangement order, and establish the first spatial coordinate system based on the spatial arrangement order; 2) moving and rotating the robotic arm within the working range of the navigator to obtain multiple sets of spatial position data of the first spatial coordinate system with respect to the second spatial coordinate system and multiple sets of posture data of the third spatial coordinate system; 3) determining a plurality of sets of the first spatial conversion relationships based on a plurality of sets of spatial position data in the first spatial coordinate system and the second spatial coordinate system; 4) determining multiple sets of second space conversion relationships based on multiple sets of position and posture data in the third space coordinate system; 5) Determine the third space conversion relationship based on multiple groups of the first space conversion relationship and the second space conversion relationship.

2. The method for determining a spatial transformation relationship based on OpenCV according to claim 1, characterized in that: The number of marking balls in step 1) is a sufficient number of marking balls required to determine the position and direction of the end effector of the robot arm, and the first spatial coordinate system is a world coordinate system that is different from the marking balls for the navigator coordinate system and the robot arm base and the TCP coordinate system of the end of the robot arm.

3. The method for determining a spatial transformation relationship based on OpenCV according to claim 1, characterized in that: The multiple sets of data in step 2) are the minimum number of data required to improve the accuracy and robustness of the calibration results, wherein the spatial position data of the first spatial coordinate system relative to the second spatial coordinate system is the three-dimensional spatial coordinate matrix of all marking balls relative to the navigator after each movement and rotation of the robotic arm, and the posture data in the third spatial coordinate system is the posture data matrix of the TCP of the robotic arm end relative to the robotic arm base after each movement and rotation of the robotic arm end, which is usually composed of three-dimensional spatial coordinates and quaternions.

4. The method for determining a spatial transformation relationship based on OpenCV according to claim 1, characterized in that: The multiple groups of first spatial transformation relationships in step 3) are obtained by using the algorithm of decomposing singular values ​​to obtain the spatial transformation relationship matrices of the multiple groups of marked balls for the navigator by using the coordinate points of the multiple groups of marked balls in the first spatial coordinate system. In addition, the first spatial transformation relationship is composed of the first rotation matrix and the first translation matrix.

5. The method for determining a spatial transformation relationship based on OpenCV according to claim 1, characterized in that: The multiple groups of second space transformation relationships in step 4) are the multiple groups of third space coordinate systems, that is, the space coordinate systems of the robot end TCP to the robot base. Usually, the posture data of the robot end TCP to the robot base can be obtained by the robot teach pendant or the ROS interface, and the multiple groups of posture data can be converted to the second space transformation relationship matrix through conversion. In addition, the second space transformation relationship is composed of the second rotation matrix and the second translation matrix.

6. The method for determining a spatial transformation relationship based on OpenCV according to claim 1, characterized in that: The multiple groups of third spatial conversion relationships in step 5) are based on the multiple groups of first conversion relationships and the second conversion relationships. The matrix arrays of the first rotation matrix and the multiple groups of first translation matrices in the multiple groups of first spatial conversion relationships and the multiple groups of second rotation matrices and the second translation matrices in the second spatial conversion relationships are passed as parameters to the opencv hand-eye calibration function calibrateHandEye to obtain the spatial conversion relationship of the navigator for the robotic arm base, that is, the third spatial conversion relationship. In addition, the third spatial conversion relationship is composed of the third rotation matrix and the third translation matrix.