Pose estimation optimization method based on information fusion of robot arm and monocular vision slam

Abstract

The application discloses a pose estimation optimization method based on information fusion of a mechanical arm and monocular vision SLAM, and comprises the following steps: S1, a coordinate transformation relationship between a mechanical arm end and a camera is acquired; S2, the mechanical arm is controlled to move an endoscope to collect a data set, monocular vision SLAM is used to extract pose information of a key frame, and a camera pose corresponding to the key frame and a sparse point cloud are obtained; S3, according to the coordinate transformation relationship, a pose of the mechanical arm end in the process of collecting the data set is converted into a camera pose; and S4, the camera pose obtained in the step S2 and the camera pose converted in the step S3 are fused based on an extended Kalman filtering method, and a SLAM pose estimation result is optimized.

Description

Technical Field

[0001] This invention relates to a pose estimation optimization method based on the fusion of robotic arm and monocular visual SLAM information. Background Technology

[0002] With the development of robotics technology, robots have gradually become a hot topic and focus in the medical field, especially in minimally invasive surgery. Traditional minimally invasive surgery mainly relies on endoscopes, which are inserted into the body through small incisions on the patient's surface to obtain a surgical view. On the one hand, the position of the endoscope needs to be manually adjusted; on the other hand, due to the complex internal structure of the human body, the confined space, and the inability of the endoscope to obtain depth information, it is difficult for doctors to accurately determine the location of lesions, which may affect the accuracy of the surgery. Summary of the Invention

[0003] The purpose of this invention is to provide a pose estimation optimization method based on the fusion of robotic arm and monocular vision SLAM information, in order to solve the problem that doctors have great difficulty in accurately judging the location of lesions because the endoscope cannot obtain depth information.

[0004] To address the aforementioned technical problems, this invention provides a pose estimation optimization method based on the fusion of robotic arm and monocular vision SLAM information, comprising the following steps:

[0005] S1: Obtain the coordinate transformation relationship between the robotic arm end effector and the camera;

[0006] S2: Control the robotic arm to move the endoscope to collect the dataset, and use monocular visual SLAM to extract the pose information of key frames to obtain the camera pose and sparse point cloud corresponding to the key frames.

[0007] S3: Based on the coordinate transformation relationship, convert the pose of the robotic arm end effector into the camera pose during the data acquisition process;

[0008] S4: Based on the extended Kalman filter method, fuse the camera pose obtained in step S2 and the camera pose obtained in step S3 to optimize the SLAM pose estimation results.

[0009] Further, step S1 includes:

[0010] S11: Drive the robotic arm to take images of the calibration plate from different angles using the endoscope, and simultaneously record the position of the robotic arm's end effector. B E T;

[0011] S12: Solve the coordinate transformation relationship between the robotic arm end effector and the camera using a hand-eye calibration algorithm. E C T.

[0012] Further, step S2 includes:

[0013] S21: Control the robotic arm to move the endoscope, collect data of bone joints from various angles, and save the data as PNG image frames in time stamp order;

[0014] S22: Use monocular visual SLAM based on feature points to extract key frames from the dataset and output the camera pose and sparse point cloud corresponding to the key frames.

[0015] Furthermore, step S2 also includes:

[0016] Local bundle adjustment is used on the extracted keyframes to optimize the camera pose; at the same time, loop closure detection is performed based on the bag-of-words model, similarity transformation is calculated, and the pose of the previous keyframes and map points is optimized based on the pose after projection matching of similarity transformation; then global BA optimization is used to obtain the camera pose and sparse point cloud of the keyframes.

[0017] Further, step S4 includes:

[0018] S41: Initialize the camera pose and use it as the initial prediction value for EKF. Use the camera pose obtained by the robotic arm as the observation value, perform EKF initialization, and correct the camera pose obtained by the robotic arm pose conversion.

[0019] S42: The camera pose at time k-1 obtained after EKF initialization correction. As a predicted value of the state at time k The camera pose x obtained by monocular visual SLAM a As observations, the pose estimate after EKF update and optimization is obtained.

[0020] Furthermore, the observation equation used in step S42 is:

[0021]

[0022] Calculate the Kalman gain K based on the state prediction:

[0023]

[0024] Based on this, the posterior distribution at time k is obtained:

[0025]

[0026]

[0027] Where, x a The camera pose, i.e., the observation value, is obtained from monocular visual SLAM; Let H be the observed value predicted at time k; H is the partial derivative of the observed value predicted at time k with respect to the state prediction value at time k. x k Let k be the state variable at time k, and its prior distribution satisfies The predicted state value at time k; Let n be the covariance matrix of the prior distribution, representing the uncertainty of the predicted value; k The noise generated during the camera recording process follows a Gaussian distribution. The posterior distribution of the state estimate at time k satisfies The state estimate is updated by EKF; Let be the covariance matrix of the posterior distribution.

[0028] The beneficial effects of this invention are as follows: By combining a robotic arm with a vision camera, the robotic arm can move the endoscope to acquire image information, thereby improving stability; simultaneously, based on visual SLAM technology, a three-dimensional environment of the patient's soft tissues is established, reconstructing the surgical scene, helping doctors determine the location of lesions, and improving surgical accuracy. Furthermore, by employing extended Kalman filtering to fuse the pose output from the robotic arm's end-effector with the camera pose output from the monocular visual SLAM system, the SLAM pose estimation results are optimized, providing more accurate pose estimation for 3D reconstruction. Attached Figure Description

[0029] The accompanying drawings, which are included to provide a further understanding of this application and form part of this application, use the same reference numerals to denote the same or similar parts. The illustrative embodiments of this application and their descriptions are used to explain this application and do not constitute an undue limitation of this application. In the drawings:

[0030] Figure 1 This is a flowchart of the EKF fusion process according to an embodiment of the present invention; Detailed Implementation

[0031] like Figure 1 The pose estimation optimization method shown includes the following steps:

[0032] S1: Obtain the coordinate transformation relationship between the robotic arm end effector and the camera;

[0033] S2: Control the robotic arm to move the endoscope to collect the dataset, and use monocular visual SLAM to extract the pose information of key frames to obtain the camera pose and sparse point cloud corresponding to the key frames.

[0034] S3: Based on the coordinate transformation relationship, convert the pose of the robotic arm end effector into the camera pose during the data acquisition process;

[0035] S4: Based on the extended Kalman filter method, fuse the camera pose obtained in step S2 and the camera pose obtained in step S3 to optimize the SLAM pose estimation results.

[0036] This invention combines a robotic arm with a vision camera. The robotic arm moves the endoscope to acquire image information, improving stability. Simultaneously, it uses visual SLAM technology to create a 3D environment of the patient's soft tissues, reconstructing the surgical scene and helping doctors pinpoint lesion locations, thus improving surgical accuracy. By employing extended Kalman filtering to fuse the pose output from the robotic arm's end-effector with the camera pose output from the monocular visual SLAM system, the SLAM pose estimation results are optimized, providing more accurate pose estimation for 3D reconstruction.

[0037] The above step S1 specifically includes:

[0038] S11: Drive the robotic arm to take images of the calibration plate from different angles using the endoscope, and simultaneously record the position of the robotic arm's end effector.

[0039] S12: Solve the coordinate transformation relationship between the robotic arm end effector and the camera using a hand-eye calibration algorithm.

[0040] Step S2 above includes:

[0041] S21: Control the robotic arm to slowly move the endoscope, collect data sets of bones and joints from various angles, save the data sets as PNG image frames in time stamp order, and generate a txt document indexed by timestamps;

[0042] S22: Keyframes in the dataset are extracted using monocular visual SLAM based on feature points, and the corresponding camera poses and sparse point clouds are output. SLAM stands for Simultaneous Localization and Mapping.

[0043] In step S2, local bundle adjustment (local BA) is used to optimize the camera pose of the extracted keyframes; at the same time, loop closure detection is performed according to the bag-of-words model (DBoW), and the similarity transformation (Sim3) is calculated. The poses of the previous keyframes and map points are optimized based on the poses after projection matching of the similarity transformation (Sim3); then global BA is used for optimization to obtain the camera poses and sparse point clouds of the keyframes.

[0044] Step S4 above includes:

[0045] S41: Initialize the camera pose and use it as the initial prediction value for EKF (Extended Kalman Filter). Use the camera pose obtained by the robotic arm as the observation value to initialize EKF, correct the camera pose obtained by the robotic arm pose, and provide a more accurate prediction value for EKF.

[0046] S42: The camera pose at time k-1 obtained after EKF initialization correction. As a predicted value of the state at time k The camera pose x obtained by monocular visual SLAM a As observations, the pose estimate after EKF update and optimization is obtained.

[0047] The observation equation used in step S42 above is:

[0048]

[0049] Calculate the Kalman gain K based on the state prediction:

[0050]

[0051] Based on this, the posterior distribution at time k (i.e., the pose estimate after EKF update) is obtained:

[0052]

[0053]

[0054] Where, x a The camera pose, i.e., the observation value, is obtained from monocular visual SLAM; Let H be the observed value predicted at time k; H is the partial derivative of the observed value predicted at time k with respect to the state prediction value at time k. x k Let k be the state variable at time k, and its prior distribution satisfies The predicted state value at time k; Let n be the covariance matrix of the prior distribution, representing the uncertainty of the predicted value; k The noise generated during the camera recording process follows a Gaussian distribution. The posterior distribution of the state estimate at time k satisfies The state estimate is updated by EKF; Let be the covariance matrix of the posterior distribution.

[0055] Since both the camera pose estimation obtained from coordinate transformation of the robotic arm's end effector and the camera pose estimation result obtained from monocular vision SLAM contain uncertainties, the EKF update part mentioned above reconciles the information of the two to obtain the overlapping part of their Gaussian distributions, i.e., the region where the optimal estimate occurs. Kalman gain is used to feedback-weight the observations, with the weights including the covariance matrix of the state prediction values, to assess the uncertainty of the prediction values ​​and adjust the state estimation results, thereby improving the accuracy of the camera pose estimation results.

[0056] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A pose estimation optimization method based on the fusion of robotic arm and monocular vision SLAM information, characterized in that, Includes the following steps: S1: Obtain the coordinate transformation relationship between the robotic arm end effector and the camera; S2: Control the robotic arm to move the endoscope to collect the dataset, use monocular visual SLAM to extract the pose information of key frames, and obtain the camera pose and sparse point cloud corresponding to the key frames. S3: Based on the coordinate transformation relationship, convert the pose of the robotic arm end effector into the camera pose during the data acquisition process; S4: Based on the extended Kalman filter method, fuse the camera pose obtained in step S2 and the camera pose transformed in step S3 to optimize the SLAM pose estimation result; step S4 includes: S41: Initialize the camera pose and use it as the initial prediction value for EKF. Use the camera pose obtained by the robotic arm as the observation value, perform EKF initialization, and correct the camera pose obtained by the robotic arm pose conversion. S42: The result obtained after EKF initialization correction Camera pose at any moment As Predicted value of state at time step The camera pose obtained by monocular visual SLAM As observations, the pose estimate after EKF update and optimization is obtained.

2. The pose estimation optimization method based on the fusion of robotic arm and monocular vision SLAM information according to claim 1, characterized in that, Step S1 includes: S11: Drive the robotic arm to take images of the calibration plate from different angles using the endoscope, and simultaneously record the position of the robotic arm's end effector. ; S12: Solve the coordinate transformation relationship between the robotic arm end effector and the camera using a hand-eye calibration algorithm. .

3. The pose estimation optimization method based on the fusion of robotic arm and monocular vision SLAM information according to claim 1, characterized in that, Step S2 includes: S21: Control the robotic arm to move the endoscope, collect data of bone joints from various angles, and save the data as PNG image frames in time stamp order; S22: Use monocular visual SLAM based on feature points to extract key frames from the dataset and output the camera pose and sparse point cloud corresponding to the key frames.

4. The pose estimation optimization method based on the fusion of robotic arm and monocular vision SLAM information according to claim 3, characterized in that, Step S2 further includes: Local bundle adjustment is used on the extracted keyframes to optimize the camera pose; at the same time, loop closure detection is performed based on the bag-of-words model, similarity transformation is calculated, and the pose of the previous keyframes and map points is optimized based on the pose after projection matching of similarity transformation; then global BA optimization is used to obtain the camera pose and sparse point cloud of the keyframes.

5. The pose estimation optimization method based on the fusion of robotic arm and monocular vision SLAM information according to claim 1, characterized in that, The observation equation used in step S42 is: Calculate the Kalman gain based on state prediction. : Based on this, we obtain Posterior distribution at time: in, The camera pose, i.e., the observation value, is obtained from monocular visual SLAM; for The predicted observation at that time; for The observations predicted at time are about Partial derivative of the predicted state value at time step, ; for The state variable at time t has a prior distribution that satisfies ; for The predicted state value at time; Let be the covariance matrix of the prior distribution, representing the uncertainty of the predicted value; The noise generated during the camera recording process follows a Gaussian distribution. ; The posterior distribution of the time-state estimate satisfies ; The state estimate is the result of EKF update; Let be the covariance matrix of the posterior distribution.

Images