A three-dimensional motion estimation method for capsule robot based on visual sensing

Abstract

The present invention discloses a three-dimensional motion estimation method for a capsule robot based on visual sensing, which relates to the technical field of robot motion estimation. The method comprises the following steps: performing internal parameter calibration on a camera carried by the capsule robot, and using the camera after the internal parameter calibration to collect multiple gastrointestinal tract images; extracting descriptor vectors of the multiple gastrointestinal tract images, and performing feature matching on the descriptor vectors of the gastrointestinal tract images of adjacent frames at different scales; obtaining the position and posture information of the capsule robot and a sparse point cloud of the gastrointestinal tract surface according to the result of the feature matching; constructing a multi-constraint position and posture graph model of the capsule robot according to the co-viewing relationship of the gastrointestinal tract images of adjacent frames and the sparse point cloud of the gastrointestinal tract surface; and optimizing the position and posture information of the capsule robot according to the multi-constraint position and posture graph model.

Description

Technical Field

[0001] The present invention relates to the technical field of robot motion estimation, and in particular to a three-dimensional motion estimation method of a capsule robot based on visual sensing. Background Art

[0002] Capsule robots are a type of micro-electromechanical system equipped with optical sensing units, computing units, and communication units. By increasing the types of sensing devices and enriching the functions of operating devices / mechanisms, capsule robots can be used for clinical inspection and auxiliary treatment of the entire digestive tract system, including biopsy sampling, lesion identification, and targeted drug administration, etc., which has important clinical significance and commercial value. However, due to the lack of low-cost, flexible and simple motion tracking methods, capsule robots can currently only be used as a means of digestive tract image acquisition for medical staff to visually inspect and analyze the internal state of the digestive tract, and cannot achieve autonomous and intelligent medical assistance tasks. How to achieve accurate estimation of the three-dimensional motion information of the capsule robot in the digestive tract is of great significance to expanding the clinical application scope of the capsule robot and improving painless, non-invasive, efficient and accurate digestive tract-related examinations and treatments.

[0003] At present, the positioning method of capsule robots mainly relies on magnetic positioning. This method requires the capsule robot to be equipped with permanent magnets or other sensing devices, and use external magnetic sensor array devices to track the permanent magnets to obtain the position and posture information of the capsule robot. The embedding of permanent magnets and other sensing devices is not conducive to the further miniaturization of capsule robots, and the external magnetic sensor array device also limits the flexibility and convenience of capsule robot applications, and increases additional hardware costs.

[0004] In terms of three-dimensional motion estimation based on visual sensing, most existing methods are aimed at technical fields such as mobile robots, drones, and autonomous driving, providing pose information for autonomous navigation of various unmanned intelligent systems. In terms of specific technology, continuous frame image matching based on visual feature points such as SIFT and ORB is often used to obtain the corresponding relationship between pixel coordinates, so as to construct and solve the multi-view mapping equations of continuous image frames and solve the pose. However, in the digestive tract scene, the image sequence sent back by the capsule robot has problems such as single texture and weak feature resolution, making it difficult to achieve accurate matching based on visual feature points, and the body fluids in the digestive tract will block and reflect the active light source of the capsule robot, further affecting the difficulty of matching visual feature points. Therefore, it is difficult for existing visual solutions to be directly deployed in capsule robots to achieve three-dimensional motion information acquisition in the digestive tract scene. Summary of the invention

[0005] In order to solve the problems in the existing capsule robot positioning methods that the robot needs to be additionally modified and the posture information needs to be obtained with the help of in vitro measurement equipment, the present invention provides a capsule robot three-dimensional motion estimation method based on visual sensing. This method uses weak texture image matching as a breakthrough. Without introducing additional hardware, the method can achieve continuous estimation of the camera position and posture only by relying on the optical sensing unit carried by the capsule robot itself, thereby providing low-cost, flexible and convenient posture information for the capsule robot's clinical applications.

[0006] To achieve the above object, the present invention provides the following technical solutions:

[0007] A method for estimating three-dimensional motion of a capsule robot based on visual sensing comprises the following steps:

[0008] Performing internal parameter calibration on a camera carried by the capsule robot, and using the camera after the internal parameter calibration to capture multiple gastrointestinal tract images;

[0009] extracting descriptor vectors of the plurality of gastrointestinal tract images, and performing feature matching on the descriptor vectors of gastrointestinal tract images of adjacent frames at different scales;

[0010] Obtaining the position information of the capsule robot and the sparse point cloud of the gastrointestinal tract surface according to the result of the feature matching;

[0011] Constructing a multi-constraint pose graph model of the capsule robot according to the co-viewing relationship of gastrointestinal tract images of adjacent frames and the sparse point cloud of the gastrointestinal tract surface;

[0012] The pose information of the capsule robot is obtained by optimizing the multi-constraint pose graph model.

[0013] The above-mentioned three-dimensional motion estimation method of the capsule robot based on visual sensing further performs internal parameter calibration on the camera carried by the capsule robot, specifically including:

[0014] Place a checkerboard calibration plate of known size in different postures and positions in the field of view of the capsule robot camera;

[0015] Under the premise of ensuring that the checkerboard calibration plate is complete and occupies the maximum image space, multiple calibration images are captured;

[0016] OpenCV is used to extract and match sub-pixel corner points of the captured image, and the extracted corner points and their matching results are calibrated according to Zhang Zhengyou's method to construct the observation equation and complete the internal parameter calculation, and obtain the internal parameter matrix K and distortion parameters k1, k2, k3, p1, p2. The internal parameter matrix expression is as follows:

[0017]

[0018] In the formula, f x and f y are the two focal length values ​​of the camera on the horizontal and vertical axes, c x and c y is the offset of the two optical centers on the horizontal and vertical axes;

[0019] Using the obtained distortion parameters, nonlinear coordinate transformation is performed on the pixel points of each frame of gastrointestinal tract image collected by the capsule robot to complete the distortion correction.

[0020] The method for estimating three-dimensional motion of a capsule robot based on visual sensing as described above further comprises extracting descriptor vectors of the plurality of gastrointestinal tract images, specifically comprising:

[0021] Build a neural network consisting of an encoder and a decoder;

[0022] Call the neural network to extract the descriptor vector of the gastrointestinal image, where for each pixel point x of the gastrointestinal image input to the neural network i , the neural network corresponds to the output and Three descriptor vectors, where the subscript i represents the pixel index number, and the superscripts h, m, and l represent the descriptor vectors extracted from images at high, medium, and low scales, respectively.

[0023] The above-mentioned method for estimating three-dimensional motion of a capsule robot based on visual sensing further includes performing feature matching on descriptor vectors of gastrointestinal tract images of adjacent frames at different scales, specifically comprising:

[0024] Assume that the gastrointestinal tract images I are from adjacent frames k and I k+1 Two pixels x k，i and x k+1，j , similarity is calculated based on the three scales of high, medium and low:

[0025]

[0026] Gastrointestinal tract image I k and I k+1 All pixels are arranged row by row and expanded into a one-dimensional vector V k and V k+1 , that is, vector V k and V k+1 Dimension D v Equivalent to image I k or I k+1 The number of pixels;

[0027] With vector V k is a row vector, V k+1is a column vector, traverse all the elements in the two vectors, and according to d in formula (2) h d m and d l The calculation method is to construct three dimensions D v ×D v The cost matrix C h , C m and C l ;

[0028] Use the Hungarian algorithm to calculate the cost matrix C h , C m and C l Calculate and obtain three 0 / 1 matrices M of the same dimension h 、M m and M l , then M h 、M m and M l Any element in represents the matching result of the ordinate and abscissa of the element if and only if M h 、M m and M l When the elements with the same coordinates in are all 1, the image I k and I k+1 The two pixels in are considered to be matched successfully;

[0029] For successfully matched pixels, the random sample consensus algorithm is used under the epipolar geometry constraint model to realize the identification of inside / outside points of the matching results.

[0030] The method for estimating three-dimensional motion of a capsule robot based on visual sensing as described above, further, obtaining the position and posture information of the capsule robot according to the result of the feature matching, specifically includes:

[0031] Set the set of pixels that match successfully x k and χ k+1 , with gastrointestinal images I k Pixel X in k，i ∈χ k Gastrointestinal images k+1 Pixel X in k+1，j ∈χ k+1 Correspondingly, the following formula is satisfied:

[0032]

[0033] In the formula, and Yes X k，i and X k+1，j The homogeneous coordinate representation of yes The transposed vector, F k，k+1 is a 3×3 basic matrix, which is composed of the camera's intrinsic parameter matrix K, the gastrointestinal image I k Gastrointestinal images k+1 The relative translation vector and the relative rotation matrix R k，k+1 ∈SO(3), and F k，k+1 =K -T ·t k，k+1 ×R k，k+1 ·K -1 ;

[0034] Traverse the pixel set x k and χ k+1 , construct multiple equations (3) based on the corresponding relationship between pixels in the pixel set, and solve the basic matrix F k，k+1 of;

[0035] For the required basic matrix F k，k+1 Decompose and verify to obtain gastrointestinal image I k Gastrointestinal images k+1 The relative translation vector t k，k+1 and the relative rotation matrix R k，k+1 ;

[0036] Taking the initial position of the first frame as the world coordinate system, the relative motion of the subsequent adjacent frames can be obtained through the transformation matrix of the sequence to obtain the gastrointestinal tract image I k The pose T relative to the world coordinate system k , T k The expression is as follows:

[0037]

[0038] Based on gastrointestinal image I k The pose T relative to the world coordinate system k Get the position information of the capsule robot.

[0039] The method for estimating three-dimensional motion of a capsule robot based on visual sensing as described above, further, obtaining a sparse point cloud of the surface of the gastrointestinal tract according to the result of the feature matching, specifically includes:

[0040] According to the internal reference matrix K, gastrointestinal image I k The pose T relative to the world coordinate system k For the pixel set χ k and χ k+1 The pixel points in the camera imaging projection equation are established;

[0041] According to the pixel set χ k and χ k+1The pixel matching result is combined with the camera imaging projection equation of the corresponding pixel point to solve the three-dimensional coordinates of the space point corresponding to the pixel point pair, and obtain a sparse point cloud on the gastrointestinal tract surface.

[0042] The method for estimating three-dimensional motion of a capsule robot based on visual sensing as described above further constructs a multi-constraint pose graph model of the capsule robot according to the co-viewing relationship of gastrointestinal tract images of adjacent frames and the sparse point cloud of the gastrointestinal tract surface, specifically comprising:

[0043] Each vertex of the multi-constrained pose graph model is connected to the vertex of the next image frame and the vertex of the next next image frame through an edge. n ∈SE(3) represents the image frame I n The position of the edge i，j ∈SE(3) is the connection vertex v i 、v j The relative pose transformation between the vertex v n According to each frame image I collected by the capsule robot k Perform key frame identification, and the key frame identification satisfies the following criteria at the same time:

[0044] 1) Gastrointestinal tract images I k There is a certain gap between the index number k and the index number n of the previous key frame, that is, kn>th t ;

[0045] 2) Gastrointestinal tract images I k The number of successfully matched pixels N χk Greater than the threshold, that is, N χk >th n , where th t and n are threshold values.

[0046] The method for estimating three-dimensional motion of a capsule robot based on visual sensing as described above, further, the pose information of the capsule robot obtained by optimizing the multi-constraint pose graph model specifically includes:

[0047] Since the vertex v i 、v j Cannot fully meet the edge e i，j Represents the transformation constraints, namely:

[0048]

[0049] This produces the residual r(v i , v j , e i，j), for all vertices in the multi-constrained pose graph model, the optimal pose parameters are obtained according to the optimization criterion of minimizing the sum of residuals of the multi-constrained pose graph model The objective function is:

[0050]

[0051] Where n is the total number of keyframes to be optimized, i and j are the index numbers of the vertices, and the ∧ sign at the top of the variable indicates that the quantity is an estimate;

[0052] For this objective function, the specific residual r(v i , v j , e i，j ) is as follows:

[0053]

[0054] In the formula, the superscript V indicates that the quantity is the transformation expression from Lie group SE(3) to Lie algebra se(3) space;

[0055] Solve the objective function to obtain the optimized position and posture information of the capsule robot.

[0056] Compared with the prior art, the present invention has the following beneficial effects:

[0057] In terms of three-dimensional motion estimation of capsule robots, existing methods mostly require the installation of permanent magnets in capsule robots and rely on external magnetic sensor array devices to track the posture and position of capsule robots, which has the problems of large size, high cost, and poor convenience. The three-dimensional motion estimation method of capsule robots based on visual sensing proposed in the present invention does not require the introduction of additional hardware. It can continuously estimate the position and posture of the camera only by relying on the optical sensor unit carried by the capsule robot itself, providing low-cost, flexible and convenient posture information for capsule robots in clinical applications. This method is universal to all existing capsule robots on the market and has good commercial promotion value.

[0058] In terms of three-dimensional motion estimation based on visual sensing, existing methods mostly complete inter-frame image matching based on visual feature points such as SIFT and ORB to achieve the solution of camera motion pose between adjacent frames, which is difficult to cope with the challenges of single texture, similar features, and unstable light source in the digestive tract scene. The image matching method of the end-to-end neural network integrating visual feature extraction-description-matching proposed in the present invention can effectively deal with the problems of difficult and inaccurate image matching in the digestive tract scene, and can effectively improve the digestive tract positioning accuracy of the capsule robot based on visual perception.

[0059] In summary, the proposed method does not require modification of the capsule robot and is applicable to all existing capsule robots on the market. It does not require the use of in vitro measurement equipment to obtain posture information and is flexible and convenient. It can effectively cope with image matching challenges in gastrointestinal scenarios and achieve high-precision positioning. It is expected to be closely integrated with technologies such as lesion recognition in the future, and has good scalability and promotion value. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0061] Figure 1 is a flow chart of a method for estimating three-dimensional motion of a capsule robot based on visual sensing in one embodiment of the present invention;

[0062] Figure 2 is a flow chart of a method for estimating three-dimensional motion of a capsule robot based on visual sensing in another embodiment of the present invention;

[0063] Figure 3 It is a schematic diagram of calling the neural network to extract descriptor vectors from gastrointestinal tract images in an embodiment of the present invention;

[0064] Figure 4 This is a graph of pixel matching results based on a neural network in an embodiment of the present invention;

[0065] Figure 5 The pose graph of the capsule robot is constructed according to the co-viewing relationship of gastrointestinal tract images of adjacent frames in an embodiment of the present invention. DETAILED DESCRIPTION

[0066] 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 application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0067] Example:

[0068] It should be noted that the terms "including" and "having" and any variations of the embodiments of the present invention are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0069] See also Figures 1 to 2 In order to solve the problem that the existing capsule robot positioning method requires additional modification of the robot and requires the use of in vitro measurement equipment to obtain posture information, the embodiment of the present invention provides a capsule robot three-dimensional motion estimation method based on visual sensing, which may specifically include the following process:

[0070] Step 100: perform internal parameter calibration on the camera carried by the capsule robot, and use the camera after the internal parameter calibration to collect multiple gastrointestinal images. Then, step 200: extract descriptor vectors of the multiple gastrointestinal images, and perform feature matching on the descriptor vectors of gastrointestinal images of adjacent frames at different scales.

[0071] In the above steps, the gastrointestinal image feature extraction and matching are mainly realized. Specifically, in clinical use, the capsule robot enters the gastrointestinal tract by swallowing, and uses the camera carried by it to collect, transmit and store gastrointestinal images. The present invention, without the help of other sensing hardware, only performs spatial motion posture inference based on the image sequence collected by the camera carried by the capsule robot to realize the three-dimensional motion estimation of the capsule robot in the gastrointestinal tract. Therefore, it is necessary to first realize the optical geometric parameter calibration of the camera to realize the mathematical model construction of the two-dimensional image to the three-dimensional environment. Then, the image sequence returned by the camera with calibrated parameters is subjected to neural network-based visual feature extraction and multi-scale visual feature matching frame by frame to deal with the problem of single texture and weak feature discrimination in the gastrointestinal scene.

[0072] The above steps can be further divided into sub-steps 101 to 103.

[0073] Sub-step 101, camera calibration and image acquisition: First, the camera carried by the capsule robot is calibrated with internal parameters. To this end, a checkerboard calibration plate of known size is placed in the field of view of the capsule robot camera in different postures and positions. 15 calibration images are taken while ensuring that the calibration plate is complete and occupies the maximum image length. OpenCV is used to extract and match sub-pixel corner points of the captured images, and the Zhang Zhengyou calibration method is used to construct the observation equation and complete the internal parameter calculation for the extracted corner points and their matching results, and obtain the internal parameter matrix K and distortion parameters k1, k2, k3, p1, p2. The internal parameter matrix expression is as follows:

[0074]

[0075] Where f x and f y are the two focal length values ​​of the camera on the horizontal and vertical axes, c x and c y It is the offset between the two optical centers on the horizontal and vertical axes.

[0076] The calibrated parameters are only related to the lens optical parameters and lens installation errors of the capsule robot's camera, that is, the calibration parameter results are only related to the hardware and only need to be calibrated once.

[0077] Using the obtained distortion parameters, nonlinear coordinate transformation is performed on the pixels of each frame of gastrointestinal tract image collected by the capsule robot to complete the distortion correction. The obtained distortion-corrected gastrointestinal tract image is used for subsequent feature extraction and matching.

[0078] Sub-step 102, visual feature extraction based on neural network: see Figure 3 In order to deal with the problem of single texture and insignificant features of the gastrointestinal tract, a visual feature extraction method based on a neural network is proposed in the present invention to achieve reliable and stable feature matching. In the present invention, the extraction of visual features relies on a neural network based on an encoder-decoder structure to guide the image area towards the matching task, and extract and fuse image features at different image scales. The proposed network structure is shown in the figure, where the left half is the encoder structure, which uses a deep convolutional neural network as the backbone network and KPCOnvp as the basic unit to perform multi-step convolution operations on each pixel of the input image. The feature maps between KPCOnvps with different convolution steps are transmitted step by step, and the feature maps generated by different step sizes are recorded and saved; the right half is the decoder structure, which fuses the coded features transmitted by the encoder and the feature maps of the previous different step sizes to generate pixel descriptors at three different scales: high, medium, and low.

[0079] Sub-step 103, multi-scale visual feature matching: After the multi-scale feature descriptor is extracted in sub-step 102, the visual feature points of the two images are matched and the scale consistency is checked at different scales.

[0080] Specifically, for two adjacent images I k and I k+1 Two pixels x k，i and x k+1，j , similarity is calculated based on the three scales of high, medium and low, that is,

[0081]

[0082] Image I k and Ik+1 All pixels are arranged row by row and expanded into a one-dimensional vector V k and V k+1 , that is, vector V k and V k+1 Dimension D V Equivalent to image I k or I k+1 Next, we use vector V k is a row vector, V k+1 is a column vector, traverse all the elements in the two vectors, and follow the d in formula (2) h d m and d l The calculation method is to construct three dimensions D v ×D v The cost matrix C h , C m and C l The three cost matrices are calculated using the Hungarian algorithm to obtain three 0 / 1 matrices M of the same dimension. h 、M m and M l , then M h 、M m and M l Any element in represents the matching result of the ordinate and abscissa of the element, that is, the vector V k and V k+1 The matching results of the elements of M at three scales. h 、M m and M l When the elements with the same coordinates in are all 1, the image I k and I k+1 The two pixels in the image have similar multi-scale features and are considered to be matched successfully. Finally, for the successfully matched pixels, RANSAC is used to identify the inside / outside points of the matching results under the epipolar geometry constraint model. The matching effect achieved is shown in Figure 2. Figure 4 As shown, it can be seen that under the condition of weak texture of the gastrointestinal tract, the proposed method can achieve consistent and uniform pixel matching, laying a good foundation for subsequent motion estimation.

[0083] Step 200: Obtaining the position information of the capsule robot and the sparse point cloud of the gastrointestinal tract surface according to the result of the feature matching.

[0084] In this step, the three-dimensional motion estimation of the capsule robot is mainly realized. Specifically, the inter-frame feature matching relationship obtained in the previous step is used to combine multiple matching point pairs to construct a multi-view observation equation. The relative motion information of adjacent image frames is obtained by solving the equation, and the three-dimensional coordinates of the matching feature point pairs are further estimated to obtain a sparse point cloud of the gastrointestinal tract surface.

[0085] The above steps can be further divided into sub-step 201 and sub-step 202.

[0086] Sub-step 201, adjacent frame motion estimation: for the matching pixel pair set x of sub-step 103 k and χ k+1 , there is I k The pixel x in k，i ∈χ k with I k+1 Pixel X in k+1，j ∈χ k+1 Correspondingly, and satisfying the following formula

[0087]

[0088] In the formula and Yes X k，i and X k+1，j The homogeneous coordinate representation of yes The transposed vector, F k，k+1 is a 3×3 basic matrix, which is composed of the camera internal parameter matrix K, I k with I k+1 The relative translation vector and the relative rotation matrix R k，k+1 ∈SO(3), there is F k，k+1 =K -T ·t k，k+1 ×R k，k+1 ·K -1 .

[0089] Traverse the set x k and χ k+1 , construct multiple equations (3) based on the corresponding relationship of pixels in the set, and complete the basic matrix F k，k+1 The solution of the required basic matrix F k，k+1 Decompose and verify to obtain I k with I k+1 The relative translation vector t k，k+1 and the relative rotation matrix R k，k+1 , complete the estimation of the relative motion of adjacent image frames. Taking the initial position of the first frame as the world coordinate system, the relative motion of subsequent adjacent frames can be obtained through the transformation matrix of the sequence,k The pose T relative to the world coordinate system k , the expression is as follows:

[0090]

[0091] Sub-step 202, feature point triangulation: Based on the camera intrinsic parameter matrix K obtained by the calibration in sub-step 101, the image frame pose information obtained in sub-step 201 is used to triangulate the set x k and χ k+1 The camera imaging projection equation is established using the set x obtained in sub-step 103. k and χ k+1 The corresponding situation is solved by combining the camera imaging projection equation of the corresponding pixel point to obtain the three-dimensional coordinates of the space point corresponding to the point pair, and complete the triangulation construction of the feature point.

[0092] Traverse the set x k and χ k+1 All pixels are constructed k with I k+1 A sparse 3D point cloud of the gastrointestinal tract surface within the common view range, each point is endowed with: a) 3D coordinate information; b) observation association with the image frame; c) feature descriptor on the associated image frame.

[0093] Step 300: construct a multi-constraint pose graph model of the capsule robot based on the co-viewing relationship of gastrointestinal tract images of adjacent frames and the sparse point cloud of the gastrointestinal tract surface, and then optimize the pose information of the capsule robot based on the multi-constraint pose graph model.

[0094] In the above steps, the optimization of the capsule robot pose estimation is mainly achieved. Specifically, considering that the motion estimation error of consecutive frames will accumulate as the number of observed images increases, the common view relationship of the image frames is used to construct a multi-constrained pose graph and perform nonlinear optimization to suppress the growth rate of the pose estimation error and obtain more accurate motion information.

[0095] The above steps can be further divided into sub-step 301 and sub-step 302.

[0096] Step 301, constructing a multi-constrained pose graph: In order to reduce the accumulation of relative pose estimation errors of consecutive frames, the present invention constructs a multi-constrained pose graph model for the estimated pose sequence. The constructed pose graph is as follows: Figure 5 As shown, where the vertex v n ∈SE(3) represents the image frame I n The position of the edge i，j ∈SE(3) is the connection vertex v i 、v j The relative pose transformation between them.

[0097] The selection of vertices comes from the key frame sampling method. In order to control the vertex scale and improve the computational efficiency, each frame of image I collected by the capsule robot is k To identify key frames, the following conditions must be met: 1) Image I k There is a certain gap between the index number k and the index number n of the previous key frame, that is, kn>th t ; 2) Image I k The number of successfully matched pixels N χk Greater than the threshold, that is, N χk >th n , where th t and n These are all artificially set thresholds.

[0098] In the pose graph, the edge connecting the vertices has a physical meaning, indicating that the image frames referred to by the two vertices have a co-viewing relationship. The formation of the edge comes from two aspects: 1) from the pose transformation sequence of sub-step 201, to achieve the estimation and association of the sequential pose transformation of the key frame nodes; 2) from the projection association of the sparse point cloud obtained from sub-step 202 on the image frame, to achieve the estimation and association of the cross-frame pose transformation of the key frame nodes. Therefore, the edge is essentially the relative pose transformation between two frames of images.

[0099] Due to the error, the vertex v i 、v j Cannot fully meet the edge e i，j Represents the transformation constraints, namely:

[0100]

[0101] This produces the residual r(v i , v j , e i，j ). The construction of the pose graph is to optimize the parameter values ​​of the vertices so that the residual of the entire graph is minimized and the pose sequence with the most transformation consistency is obtained.

[0102] Sub-step 302, pose graph optimization: After the pose graph is constructed, all vertices in the pose graph, i.e., the image poses, are optimized. The optimal pose parameters are obtained according to the optimization criterion of minimizing the sum of pose graph residuals. The objective function is:

[0103]

[0104] Where n is the total number of keyframes to be optimized, i and j are the index numbers of the vertices, and the ∧ mark on the top of the variable indicates that the quantity is an estimate. For this objective function, the specific residual r(v i , v j , ei，j ) is expressed as follows

[0105]

[0106] The superscript V indicates that the quantity is the transformation expression from the Lie group SE(3) to the Lie algebra se(3) space. In addition, for the objective function (4), if the vertex v i 、v j There is no edge e (i，j) As a constraint, the residual r(v i , v j , e i，j )=0.

[0107] For the optimization equation (4), it is essentially a nonlinear least squares problem, which can be solved by existing graph optimization solving tools such as g20 to obtain the optimized pose sequence, that is, the optimized three-dimensional motion estimation information of the capsule robot.

[0108] It can be seen that the present invention can realize low-cost, flexible and convenient digestive tract positioning of capsule robots, thereby improving the functional scalability of capsule robots in digestive tract inspection, lesion localization, fixed-point drug administration, etc., reducing the intervention time and labor intensity of medical staff, and promoting the clinical application of capsule robots.

[0109] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, without contradiction.

[0110] The above embodiments are only for illustrating the technical concept and features of the present invention, and their purpose is to enable ordinary technicians in the field to understand the content of the present invention and implement it accordingly, and they cannot be used to limit the protection scope of the present invention. Any equivalent changes or modifications made based on the essence of the content of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for estimating three-dimensional motion of a capsule robot based on visual sensing, characterized in that: The following steps are involved: Performing internal parameter calibration on a camera carried by the capsule robot, and using the camera after the internal parameter calibration to capture multiple gastrointestinal tract images; extracting descriptor vectors of the plurality of gastrointestinal tract images, and performing feature matching on the descriptor vectors of gastrointestinal tract images of adjacent frames at different scales; Obtaining the position information of the capsule robot and the sparse point cloud of the gastrointestinal tract surface according to the result of the feature matching; A multi-constraint pose graph model of the capsule robot is constructed according to the co-viewing relationship of the gastrointestinal tract images of adjacent frames and the sparse point cloud of the gastrointestinal tract surface; wherein each vertex of the multi-constraint pose graph model is connected to the vertex of the next image frame and the vertex of the next next image frame through an edge, and the vertex v of the multi-constraint pose graph model n ∈SE(3) represents the image frame I n The position of the edge i,j ∈SE(3) is the connection vertex v i 、v j The relative pose transformation between the vertex v n According to each frame image I collected by the capsule robot k Perform key frame identification, and the key frame identification satisfies the following criteria at the same time: 1) Gastrointestinal tract images I k There is a certain gap between the index number k and the index number g of the previous key frame, that is, kg>th t ; 2) Gastrointestinal tract images I k The number of successfully matched pixels N χk Greater than the threshold, that is, N χk >th g , where th t and g All are threshold values; The pose information of the capsule robot is obtained by optimizing the multi-constraint pose graph model; wherein, due to the vertex v i 、v j Cannot fully meet the edge e i,j Represents the transformation constraints, namely: This produces the residual r(v i ,v j ,e i,j ), for all vertices in the multi-constrained pose graph model, the optimal pose parameters are obtained according to the optimization criterion of minimizing the sum of residuals of the multi-constrained pose graph model The objective function is: Where n is the total number of keyframes to be optimized, i and j are the index numbers of the vertices, and the ∧ mark at the top of the variable indicates the estimated value; For this objective function, the specific residual r(v i ,v j ,e i,j ) is as follows: In the formula, the superscript ∨ indicates the transformation expression from Lie group SE(3) to Lie algebra se(3) space; Solve the objective function to obtain the optimized position and posture information of the capsule robot.

2. The method for estimating three-dimensional motion of a capsule robot based on visual sensing according to claim 1, characterized in that: The internal parameters of the camera carried by the capsule robot are calibrated, including: Place a checkerboard calibration plate of known size in different postures and positions in the field of view of the capsule robot camera; Under the premise of ensuring that the checkerboard calibration plate is complete and occupies the maximum image space, multiple calibration images are captured; OpenCV is used to extract and match sub-pixel corner points of the captured image, and the Zhang Zhengyou calibration method is used to construct the observation equation and complete the internal parameter calculation for the extracted corner points and their matching results, and to obtain the internal parameter matrix K and distortion parameters. k1,k2,k3,p1,p2, where the internal parameter matrix expression is as follows: In the formula, f x and f y are the two focal length values ​​of the camera on the horizontal and vertical axes, c x and c y is the offset of the two optical centers on the horizontal and vertical axes; Using the obtained distortion parameters, nonlinear coordinate transformation is performed on the pixel points of each frame of gastrointestinal tract image collected by the capsule robot to complete the distortion correction.

3. The method for estimating three-dimensional motion of a capsule robot based on visual sensing according to claim 1, characterized in that: Extracting descriptor vectors of the plurality of gastrointestinal tract images specifically includes: Build a neural network consisting of an encoder and a decoder; Call the neural network to extract the descriptor vector of the gastrointestinal image, where for each pixel point x of the gastrointestinal image input to the neural network i , the neural network corresponds to the output and Three descriptor vectors, where the subscript i represents the pixel index number, and the superscripts h, m, and l represent the descriptor vectors extracted from images at high, medium, and low scales, respectively.

4. The method for estimating three-dimensional motion of a capsule robot based on visual sensing according to claim 3, characterized in that: Feature matching is performed on the descriptor vectors of gastrointestinal images of adjacent frames at different scales, specifically including: Assume that the gastrointestinal tract images I are from adjacent frames k and I k+1 Two pixels x k,i and x k+1,j , the similarity calculation of high, medium and low scales is performed as follows: Gastrointestinal tract image I k and I k+1 All pixels are arranged row by row and expanded into a one-dimensional vector V k and V k+1 , that is, vector V k and V k+1 Dimension D V Equivalent to image I k or I k+1 The number of pixels; With vector V k is a row vector, V k+1 is a column vector, traverse all the elements in the two vectors, and according to d in formula (2) h ,d m and d l The calculation method is to construct three dimensions D V ×D V The cost matrix C h , C m and C l ; Use the Hungarian algorithm to calculate the cost matrix C h , C m and C l Calculate and obtain three 0 / 1 matrices M of the same dimension h 、M m and M l , then M h 、M m and M l Any element in represents the matching result of the ordinate and abscissa of the element if and only if M h 、M m and M l When the elements with the same coordinates in are all 1, the image I k and I k+1 The two pixels in are considered to be matched successfully; For successfully matched pixels, the random sample consensus algorithm is used under the epipolar geometry constraint model to realize the identification of inside / outside points of the matching results.

5. The method for estimating three-dimensional motion of a capsule robot based on visual sensing according to claim 1, characterized in that: The position and posture information of the capsule robot is obtained according to the result of the feature matching, which specifically includes: Set the set of pixels that match successfully x k and χ k+1 , with gastrointestinal images I k The pixel x in k,i ∈χ k Gastrointestinal images k+1 The pixel x in k+1,j ∈χ k+1 Correspondingly, the following formula is satisfied: In the formula, and is x k,i and x k+1,j The homogeneous coordinate representation of yes The transposed vector, F k,k+1 is a 3×3 basic matrix, which is composed of the camera's intrinsic parameter matrix K, the gastrointestinal image I k Gastrointestinal images k+1 The relative translation vector and the relative rotation matrix R k,k+1 ∈SO(3), and F k,k+1 =K -T ·t k,k+1 ×R k,k+1 ·K -1 ; Traverse the pixel set x k and χ k+1 , construct multiple equations (3) based on the corresponding relationship between pixels in the pixel set, and solve the basic matrix F k,k+1 ; For the required basic matrix F k,k+1 Decompose and verify to obtain gastrointestinal image I k Gastrointestinal images k+1 The relative translation vector t k,k+1 and the relative rotation matrix R k,k+1 ; The initial position of the first frame is used as the world coordinate system, and the relative motion of the subsequent adjacent frames is obtained through the transformation matrix of the sequence to obtain the gastrointestinal image I k The pose T relative to the world coordinate system k , T k The expression is as follows: Based on gastrointestinal image I k The pose T relative to the world coordinate system k Get the position information of the capsule robot.

6. The method for estimating three-dimensional motion of a capsule robot based on visual sensing according to claim 1, characterized in that: According to the result of the feature matching, a sparse point cloud of the gastrointestinal tract surface is obtained, which specifically includes: According to the internal reference matrix K, gastrointestinal image I k The pose T relative to the world coordinate system k For the pixel set X k and χ k+1 The pixel points in establish the camera imaging projection equation; According to the pixel set χ k and X k+1 The pixel matching result is combined with the camera imaging projection equation of the corresponding pixel point to solve the three-dimensional coordinates of the space point corresponding to the pixel point pair, and obtain a sparse point cloud on the gastrointestinal tract surface.

Images