[go: up one dir, main page]

CN109620108B - Double-hemispherical capsule robot bending intestinal visual navigation method - Google Patents

Double-hemispherical capsule robot bending intestinal visual navigation method Download PDF

Info

Publication number
CN109620108B
CN109620108B CN201910056648.3A CN201910056648A CN109620108B CN 109620108 B CN109620108 B CN 109620108B CN 201910056648 A CN201910056648 A CN 201910056648A CN 109620108 B CN109620108 B CN 109620108B
Authority
CN
China
Prior art keywords
double
coordinate system
capsule robot
hemisphere
axis
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910056648.3A
Other languages
Chinese (zh)
Other versions
CN109620108A (en
Inventor
张永顺
田丰
王智博
杨慧远
刘旭
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Dalian University of Technology
Original Assignee
Dalian University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Dalian University of Technology filed Critical Dalian University of Technology
Priority to CN201910056648.3A priority Critical patent/CN109620108B/en
Publication of CN109620108A publication Critical patent/CN109620108A/en
Application granted granted Critical
Publication of CN109620108B publication Critical patent/CN109620108B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B1/00—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor
    • A61B1/04—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor combined with photographic or television appliances
    • A61B1/041—Capsule endoscopes for imaging
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B1/00—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor
    • A61B1/04—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor combined with photographic or television appliances
    • A61B1/05—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor combined with photographic or television appliances characterised by the image sensor, e.g. camera, being in the distal end portion
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B1/00—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor
    • A61B1/31—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor for the rectum, e.g. proctoscopes, sigmoidoscopes, colonoscopes
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B5/00—Measuring for diagnostic purposes; Identification of persons
    • A61B5/07—Endoradiosondes
    • A61B5/073—Intestinal transmitters

Landscapes

  • Health & Medical Sciences (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Surgery (AREA)
  • Engineering & Computer Science (AREA)
  • General Health & Medical Sciences (AREA)
  • Physics & Mathematics (AREA)
  • Biomedical Technology (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Medical Informatics (AREA)
  • Molecular Biology (AREA)
  • Biophysics (AREA)
  • Animal Behavior & Ethology (AREA)
  • Pathology (AREA)
  • Public Health (AREA)
  • Veterinary Medicine (AREA)
  • Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
  • Optics & Photonics (AREA)
  • Radiology & Medical Imaging (AREA)
  • Endoscopes (AREA)
  • Manipulator (AREA)

Abstract

一种双半球型胶囊机器人弯曲肠道内视觉导航方法，属于自动化工程技术领域。该方法在利用双半球型胶囊机器人在万向旋转磁场中的随动性以确定其轴线的侧摆俯仰角的基础上，在双半球型胶囊机器人内安装铅垂角度传感器，确定摄像头绕双半球型胶囊机器人轴线的自转角，进而确定摄像头姿态；再利用万向旋转磁场的均匀性，结合姿态信息通过坐标转换确定在固定坐标系内弯曲肠道图像暗区质心相对于双半球型胶囊机器人的方向向量，实现万向旋转磁场对双半球型胶囊机器人在弯曲肠道内的导航。本发明避免使用多目摄像机结构和复杂肠道三维重建技术，且无须计算双半球型胶囊机器人具体位置信息，即可实现单目视觉辅助双半球型胶囊机器人在肠道内的导航作业。

Figure 201910056648

The invention relates to a visual navigation method in the curved intestine of a double hemisphere capsule robot, which belongs to the technical field of automation engineering. The method uses the follow-up of the double hemisphere capsule robot in the universal rotating magnetic field to determine the pitch angle of its axis, and installs a vertical angle sensor in the double hemisphere capsule robot to determine the camera orbiting the double hemisphere. Then, using the uniformity of the universal rotating magnetic field, combined with the attitude information, the center of mass of the dark area of the curved intestinal image in the fixed coordinate system is determined relative to the double hemisphere capsule robot through coordinate transformation. The direction vector realizes the navigation of the dual-hemisphere capsule robot in the curved intestine by the universal rotating magnetic field. The invention avoids the use of multi-eye camera structure and complex intestinal three-dimensional reconstruction technology, and can realize the monocular vision-assisted dual-hemisphere capsule robot's navigation operation in the intestine without calculating the specific position information of the double-hemisphere capsule robot.

Figure 201910056648

Description

Double-hemispherical capsule robot bending intestinal visual navigation method
Technical Field
The invention belongs to the technical field of automation engineering, and relates to a visual navigation method of a space universal rotating magnetic field driven double-hemisphere type capsule robot in a bent intestinal tract.
Background
The gastrointestinal tract of a human body is very easy to cause various fatal diseases, wherein colorectal cancer is the most common disease. However, if most gastrointestinal diseases can be detected and diagnosed early, the cure rate will be significantly improved. Therefore, gastrointestinal tract examination and diagnosis are important in the medical field. The most common apparatus for examining gastrointestinal diseases is a conventional endoscope, which cannot examine the entire intestine due to the limitation of the insertable length of the catheter. And the endoscope conduit is easy to cause gastrointestinal soft tissue injury in the inserting process, and can cause pain of patients.
The first capsule endoscope M2A was successfully developed by Given Image corporation of Israel in 2000 and started to be clinically used by the US FDA the next year, raising the revolution in endoscopy technology. After entering the gastrointestinal tract by swallowing, the endoscope can realize the examination of the whole small intestine and avoid the pain and discomfort of the patient caused by the traditional direct insertion endoscope. After M2A capsule endoscopes, Olympus and RF co.ltd in japan, Intelligent Miro-system Center in korea have also successively introduced its own commercial series of products, endo capsule, Norika and MiroCam, and the science and technology (group) ltd in chongqing jinshan in china has also successfully introduced the OMOM series of capsule endoscopes with completely proprietary intellectual property rights and entered clinical applications.
Despite the relative maturity of the above capsule endoscopy technology, there are still many problems from the point of clinical application feedback: (1) the active control problem of the capsule double-hemisphere type capsule robot is as follows: most of the existing clinical capsule endoscopes do not have an active walking mechanism and passively advance by depending on gastrointestinal tract peristalsis, so that the motion of the existing clinical capsule endoscopes in the gastrointestinal tract is random, the capsule cannot return after missing a lesion area, and a doctor cannot perform detailed and careful observation on an interested area, so that the missed detection rate is high and the examination efficiency is low. (2) The navigation problem of the capsule double-hemisphere type capsule robot is as follows: because the real-time position and posture (pose) of the existing capsule endoscope in the gastrointestinal tract can not be determined, the capsule endoscope can not be navigated, and the effective control of the capsule double-hemisphere type capsule robot can not be realized. These problems are all related to the active navigation motion control of the capsule double-hemisphere type capsule robot, so that the capsule endoscope is required to have a reliable motion walking mechanism and needs to be actively navigated to realize the efficient diagnosis and treatment function of the capsule endoscope.
In order to solve the problems of active movement walking of the capsule double-hemisphere type capsule robot in the gastrointestinal tract and turning walking in a non-structural environment, the subject group designs and develops the capsule double-hemisphere type capsule robot with a double-hemisphere structure, and obtains the national invention patent of 'an active and passive double-hemisphere type capsule robot and a posture adjustment and turning drive control method thereof' (patent number: CN201510262778.4) and 'a space universal rotating magnetic field man-machine interaction control method' (patent number: ZL 201610009285.4).
The specific method for realizing the space universal rotating magnetic field man-machine interaction control in the patent 'a space universal rotating magnetic field man-machine interaction control method' (patent No. ZL201610009285.4) adopts a space universal rotating magnetic field superposition formula in a current form which takes two attitude angles of a rotating magnetic vector yaw angle theta and a pitch angle delta as input variables in a longitude and latitude coordinate system, so that an azimuth control variable is converted into two variables of the theta and the delta in the longitude and latitude coordinate system through orthogonal transformation from three variables of the alpha, the beta and the gamma in a Cartesian coordinate system, thereby converting the three-dimensional superposition problem of the space universal rotating magnetic field into the two-dimensional superposition problem in a plane, separately controlling the lateral swing angle and the pitching angle through two control rods, the rotating magnetic vectors superposed in the uniform magnetic field region of the triaxial Helmholtz coil device can be independently controlled along the directions of the sidesway and the pitch angle respectively, so that the control of the low-dimensional separable variable interactive rotating magnetic vectors is realized. The longitude and latitude coordinate system is a fixed world coordinate system, so the longitude and latitude coordinate system is referred to as a fixed coordinate system in the text.
The general structure of a double-hemispherical capsule robot proposed in the patent 'an active and passive double-hemispherical capsule robot and attitude adjustment and turning drive control method thereof' (patent number: CN201510262778.4) is as follows: the double-hemisphere type capsule robot structure comprises an active hemisphere and a passive hemisphere, wherein a radial magnetized neodymium iron boron annular ring inner driver 7 and a camera are in interference fit with an image transmission device 8, a stepped shaft 6 is also in interference fit with the camera and the image transmission device 8, and finally the camera and an image transmission device 8 assembly are in interference fit with an active hemisphere shell 1 to form the active hemisphere; the bearing positioning sleeve 3 and the passive hemisphere shell 2 form a passive hemisphere in interference fit, and the process that the active hemisphere and the passive hemisphere are connected through the bearing 4 in a suspension mode is as follows: install bearing 4 on the step shaft 6 of initiative hemisphere subassembly, pack into bearing position sleeve 3 with bearing 4 on the initiative hemisphere subassembly step shaft 6 in the lump again, bearing position sleeve 3 is inside to have a step to realize bearing 4 outer lane axial positioning, and round nut 5 is packed into on the step shaft 6 in order to with bearing 4 inner lane axial positioning, and round nut 5 can not be outstanding beyond the sphere to prevent that initiative hemisphere rotation in-process from driving round nut 5 and intestinal contact influence gesture adjustment. The working principle of the double-hemisphere capsule robot is as follows: the coupling magnetic moment of the rotating magnetic field and the radial magnetized neodymium iron boron annular inner driver 7 drives the driving hemisphere comprising the camera and the image transmission device 8 to relatively idle around the driven hemisphere, the driving hemisphere is in a driving state, the driven hemisphere is always positioned below and in an underactuated state under the action of the balance weight, the driven hemisphere under the restraint of the friction force contacting with the gastrointestinal tract is in a static state, the rolling of the double-hemisphere type capsule robot during posture adjustment can be prevented, the driving hemisphere is positioned above and not contacted with the gastrointestinal tract or the contact area with the gastrointestinal tract is smaller, the driving hemisphere idles relative to the driven hemisphere positioned below and is static, the optical axis of the camera in the camera and the image transmission device 8 is consistent with the axis of the double-hemisphere capsule robot, a rotating magnetic vector is applied above the contact surface of the gastrointestinal tract during posture adjustment, and the driving hemisphere idles relative to the driven hemisphere, the following effect enables the axis of the double-hemisphere capsule robot to always follow the axis of the rotating magnetic field with the corresponding azimuth angle to realize the arbitrary adjustment of the posture in the gastrointestinal tract.
The navigation process of the double-hemisphere capsule robot proposed by the patent 'an active and passive double-hemisphere capsule robot and the attitude adjustment and turning drive control method thereof' (patent number: CN201510262778.4) is as follows: three groups of coils are mutually orthogonally nested and installed into a three-axis orthogonally nested Helmholtz coil magnetic field superposition device c, so that a patient a swallows a double-hemisphere capsule robot d and lies on a sickbed b, and the position of the sickbed b is adjusted to enable the patient d to be positioned in the central area of the three-axis orthogonally nested Helmholtz coil magnetic field superposition device c. When the double-hemispherical capsule robot is located at the position A, according to the following effect principle that the axis of the double-hemispherical capsule robot is always consistent with the magnetic vector direction, the magnetic vector direction is adjusted to enable the wireless transmission image to be aligned to the intestinal tract bending direction, at the moment, the magnetic vector direction is consistent with the intestinal tract bending direction, a rotating magnetic vector perpendicular to the intestinal tract bending direction is applied to the horizontal plane, the double-hemispherical capsule robot is driven to roll to the position B, the process is repeated to move to the position C, and the like.
Although the two patents show the method for controlling the bending intestinal tract of the capsule robot in the shape of a double hemisphere, the two patents do not show how to determine the direction of the magnetic vector to enable the wireless transmission image to be aligned with the bending direction of the intestinal tract, namely, a specific navigation direction determination method is not given.
If the navigation direction is to be determined, the posture information of the current double-hemisphere type capsule robot needs to be obtained. At present, a multi-view vision system is adopted for general vision navigation, however, the inner space of the double-hemisphere capsule robot is narrow, a method for installing a plurality of cameras is limited by the space of the double-hemisphere capsule robot, an extra circuit is needed for transmitting information, and higher requirements are met for the installation precision of the multi-view lens, so that the inner structure of the double-hemisphere capsule robot is too complex, and the reliability of the system is reduced due to the complex circuit. Although the three-dimensional reconstruction technology can calculate the position of the current double-hemisphere capsule robot, the algorithm is complex and has higher requirements on image parameters.
Therefore, on the basis of the two invention patents, the subject group provides a visual navigation method for the double-hemisphere capsule robot in the bent intestinal tract by combining the structural characteristics of the double-hemisphere capsule robot and a man-machine interaction control method.
By utilizing the follow-up effect of the double-hemisphere type capsule robot in the rotating magnetic field, the axis of the double-hemisphere type capsule robot is always consistent with the direction of the rotating magnetic vector, namely the optical axis direction of the camera is also consistent with the direction of the rotating magnetic vector, so that the optical axis direction of the static double-hemisphere type capsule robot camera after posture adjustment is known and can be described by two posture angles of a swing angle theta and a pitch angle delta inside a fixed coordinate system, but the self-rotation angle of the camera around the axis of the double-hemisphere type capsule robot is unknown and has randomness, so that the posture of the camera cannot be determined, and the posture of the camera can be determined only by determining the self-rotation angle information of the camera around the axis of the double-hemisphere type capsule robot. The vertical angle sensor is arranged in the double-hemisphere capsule robot, and under the action of gravity, the self-rotation angle of the camera around the axis of the double-hemisphere capsule robot can be read out and recorded as alpha in the vertical plane of the axis of the double-hemisphere capsule robot through the vertical angle sensor. After the information of the axis direction of the optical axis of the camera and the self-rotation angle of the camera around the axis of the double-hemisphere capsule robot is obtained, the posture of the camera in a fixed coordinate system can be determined. And then extracting the pixel position of the dark-area centroid of the wireless transmission image transmitted when the camera shoots the bent intestinal tract, taking the pixel position as an intestinal tract turning navigation point, and determining the direction vector of the dark-area centroid relative to the double-hemisphere capsule robot on the bent intestinal tract image in a fixed coordinate system through coordinate conversion by combining the posture information of the camera, so as to realize the turning navigation of the space universal magnetic field to the double-hemisphere capsule robot.
At present, no method for determining the posture information of a camera of a double-hemisphere capsule robot relative to a fixed coordinate system by using the uniformity of a universal rotating magnetic field and the followability of the double-hemisphere capsule robot in the magnetic field and determining the navigation direction through coordinate transformation has been proposed, and the method has the obvious advantages that only a simple vertical angle sensor needs to be additionally arranged without using a multi-view vision system and a complex intestinal three-dimensional image reconstruction technology, and the final navigation information of the double-hemisphere capsule robot is obtained by depending on the vision of a monocular camera of the double-hemisphere capsule robot and combining the uniformity of the universal rotating magnetic field and the followability of the double-hemisphere capsule robot in the universal rotating magnetic field through coordinate transformation.
Disclosure of Invention
The invention provides a method for realizing visual navigation of a double-hemisphere capsule robot in a bent intestinal canal by driving of a space universal rotating magnetic field, which is characterized in that on the basis of determining the side-sway and pitching attitude angles of the axis of the double-hemisphere capsule robot by the follow-up characteristic of the double-hemisphere capsule robot in the universal rotating magnetic field, the self-rotation angle of a camera around the axis of the double-hemisphere capsule robot is determined by a plumb angle sensor, so that the attitude of the camera relative to a fixed coordinate system is determined, and finally the direction vector of the mass center of a dark zone on a bent intestinal canal image relative to the double-hemisphere capsule robot in the fixed coordinate system is determined, so that the navigation of the double-hemisphere capsule robot in the bent intestinal canal is realized.
The technical scheme of the invention is as follows:
a visual navigation method in a bent intestinal tract of a double-hemisphere capsule robot comprises the following steps:
the first step is as follows: when the double-hemisphere capsule robot needs to navigate at the current position, the self-rotation angle of the camera frame around the axis of the double-hemisphere capsule robot is determined through the vertical angle sensor 10.
The method for determining the self-rotation angle of the camera frame around the axis of the double-hemisphere capsule robot in the first step of the technical scheme comprises the following steps: an image plane 11 is observed along the axial direction on the axial vertical plane of the double-hemisphere capsule robot 9, and by means of the vertical angle sensor 10, an included angle alpha between the vertical line of the image plane and the vertical line can be obtained under the action of gravity, namely the self-rotation angle of the camera around the axial line of the double-hemisphere capsule robot.
The second step is that: and determining the posture of the camera relative to the fixed coordinate system by combining the pitch angle and the yaw angle information of the axis of the double-hemisphere capsule robot and the self-rotation angle of the camera around the axis of the double-hemisphere capsule robot.
The method for specifically determining the posture of the camera relative to the fixed coordinate system in the second step of the technical scheme comprises the following steps: by utilizing the follow-up effect of the double-hemisphere capsule robot in the rotating magnetic field, the axis direction of the double-hemisphere capsule robot is always consistent with the direction of the rotating magnetic vector, so that the optical axis direction of a camera of the double-hemisphere capsule robot is two attitude angles of a swing angle theta and a pitch angle delta inside a fixed coordinate system; and combining the rotation angle of the camera frame around the axis of the double-hemisphere capsule robot determined in the first step of the technical scheme to obtain the posture information of the double-hemisphere capsule robot relative to a fixed coordinate system.
The third step: and (3) extracting coordinates of the centroid pixels of the dark areas at the corners by using image processing, and taking the coordinates as intestinal turning navigation points. Because the double-hemisphere capsule robot is always positioned in the uniform area of the three-axis Helmholtz coil magnetic field, the pixel coordinate of the centroid of the image dark area is converted into the direction vector of the centroid in a fixed coordinate system relative to the double-hemisphere capsule robot through coordinate transformation, and then the direction of the magnetic vector for applying the rolling magnetic field required by the motion principle of the double-hemisphere capsule robot is parallel to the horizontal plane, so that the direction of the magnetic vector which needs to be applied when the double-hemisphere capsule robot moves from the current position to the target position is calculated, and the navigation of the double-hemisphere capsule robot is realized.
The method for converting the navigation coordinate of the double-hemisphere capsule robot and the calculation formula in the third step of the technical scheme are specifically as follows: when the double-hemisphere capsule robot enters a working environment, the position and the posture of the double-hemisphere capsule robot in the body need to be known for navigation. Because the rotating magnetic field generated by the three-axis Helmholtz coil has regional uniformity and all-point isotropy, in the uniform magnetic field region, no matter the double-hemisphere type capsule robot is in any position, the axis direction is always consistent with the magnetic vector direction, and therefore the origin O of a fixed coordinate system O-XYZ can be translated to the optical center O of the cameraCThe coordinate axis direction is unchanged to form a moving coordinate system OC-XwYwZwThe gesture of the camera relative to the fixed coordinate system is equivalent to the gesture of the camera relative to the moving coordinate system, and the direction vector of the dark area centroid relative to the double-hemisphere type capsule robot in the fixed coordinate system to be solved is equivalent to the dark area centroid in the moving coordinate system OC-XwYwZwThe direction vector of the inner. Therefore, only the direction vector of the target point in the moving coordinate system needs to be calculated, and the vector is the final navigation direction. Therefore, only the posture information of the double-hemisphere capsule robot (which can be obtained by the second step of the technical scheme) needs to be obtained, and the specific position information of the double-hemisphere capsule robot in the fixed coordinate system does not need to be obtained. Let P point be the dark area centroid in the moving coordinate system OC-XwYwZwThe position in (1) is P is the imaging point of the point P on the image plane, and the vector O in the required moving coordinate systemC-direction information and vector O of PCP are aligned, so that the vector O in the moving coordinate system is obtainedCThe orientation information of P can be converted into a vector O in a motion coordinate systemC-direction information of p.
In order to perform the next coordinate conversion, a coordinate system needs to be established. First, it has been constructed in the foregoingVertical moving coordinate system OC-XWYWZWOn the basis of the two semi-spherical capsule robot, an axial coordinate system O of the capsule robot is establishedC-X1Y1Z1，OC-X1The axis being in the horizontal direction, OC-Z1The axis is coincided with the axis direction of the double-hemispherical capsule robot; establishing a camera axis coordinate system OC-XCYCZCCoordinate system OC-XCYCZCFrom a coordinate system OC-X1Y1Z1Around OC-Z1Rotation of the shaft clockwise by an angle alpha, OC-ZCShaft and OC-Z1The axes are overlapped; establishing an image physical coordinate system O1-xy，O1OCIs the focal length of the camera, O1Xy perpendicular to OC-ZCA shaft, and O1The x axis and OC-XCAxis parallel, O1The y-axis and OC-YCThe axes are parallel; establishing a computer image coordinate system O0Uv, the relationship between the computer image pixel coordinate system and the physical coordinate system is:
Figure BDA0001952710730000071
Figure BDA0001952710730000072
after conversion, the following can be obtained:
Figure BDA0001952710730000073
obtaining the relation between the image physical coordinate system and the computer image pixel coordinate system after inversion:
Figure BDA0001952710730000074
wherein, dx, dy, u0，v0Are all internal parameters of the camera, and the physical size of each pixel is dx dy (mm), u0As a physical coordinate system O of the image1-xy origin O1Relative to a computer image coordinate system O0Uv origin O0At O1-offset in the direction of the x-axis, v0As a physical coordinate system O of the image1-xy origin O1Relative to a computer image coordinate system O0Uv origin O0At O1-offset in y-axis direction, f is camera focal length.
In camera axis coordinate system OC-XCYCZCZ of p point on middle, image planeCThe coordinate value is the focal length f of the camera, so that the point p is in the camera axis coordinate system Oc-XcYcZcThe coordinates in (d) are (x, y, f).
Double-hemisphere type capsule robot axis coordinate system OC-X1Y1Z1And the camera axis coordinate system Oc-XcYcZc: camera coordinate system OC-XCYCZCIs composed of a double-hemisphere type capsule robot axis coordinate system OC-X1Y1Z1Around OC-Z1The shaft rotates clockwise by an angle alpha, the formula can be obtained
Figure BDA0001952710730000081
The reverse operation is carried out to obtain:
Figure BDA0001952710730000082
Figure BDA0001952710730000083
moving coordinate system OC-XWYWZWAnd axis coordinate system O of double-hemisphere type capsule robotC-X1Y1Z1The conversion process of (1) is as follows: as in fig. 7, initially at XWOCZWIn-plane rotating magnetic vector B, its normal vectors n and OC-YWThe axes are coincident, firstly, the X axis is wound around O in order to ensure that the X axis is always in the horizontal plane in the transformation processC-ZWThe shaft rotates clockwise by an angle theta and then rotates around the shaft OC-X1The axis rotates anticlockwise by an angle delta to obtain a coordinate system OC-X1Y1Z1And the theta angle is a yaw angle, and the delta angle is a pitch angle. After rotation, the normal vector of the magnetic vector plane is formed by OC-YWThe axis becomes n (sin θ cos δ cos θ cos δ sin δ) and the orthogonal transformation matrix is
Figure BDA0001952710730000084
Then
Figure BDA0001952710730000085
To obtain
Figure BDA0001952710730000086
Wherein
Figure BDA0001952710730000091
Therefore, in combination with the above coordinate transformation, when the double-hemisphere type capsule robot is under the action of the rotating magnetic field, the yaw angle of the magnetic vector direction is theta, the pitch angle is delta, the rotation angle alpha of the camera around the axis of the double-hemisphere type capsule robot is alpha, the computer image pixel coordinates of the centroid of the dark area on the image are (u, v), and the centroid of the image dark area in the moving coordinate system O can be calculated according to the above parametersC-XWYWZWInner direction nS：
Figure BDA0001952710730000092
Wherein
Figure BDA0001952710730000093
Figure BDA0001952710730000094
Then (X) is converted into (X) by an orthogonal transformation method provided by the patent of 'a space universal rotating magnetic field man-machine interaction control method' (patent number: ZL201610009285.4)W,YW,ZW) The transformation is expressed by two variables of theta and delta, and the final navigation direction is obtained.
The orthogonal transformation method comprises the following steps: vector n is first introducedSProjection to XWOCYWIn-plane, the projection and OC-XWThe included angle of the shaft is a side swing angle theta; then, the vector nSProjection to XWOCZWIn-plane, the projection and OC-ZWThe included angle of the shafts is a pitch angle delta.
Since the rolling magnetic field needs to be applied in the horizontal plane, n is obtainedSAnd after the data are converted into a variable representation of theta and delta, the delta value is 0, namely a rolling magnetic field to be applied by navigation is obtained, and therefore the navigation of the double-hemisphere capsule robot in the third step is completed.
The invention has the beneficial effects that: under the condition of limited space inside the double-hemisphere capsule robot, the following characteristic of the axis of the double-hemisphere capsule robot and the magnetic vector of the universal rotating magnetic field in the hovering posture adjustment mode is fully utilized, only one vertical angle sensor is additionally arranged in front of the double-hemisphere capsule robot, and the navigation direction of the double-hemisphere capsule robot can be determined without calculating the specific position information of the double-hemisphere capsule robot in a fixed coordinate system, so that the complex multi-view camera structure and the complex intestinal tract three-dimensional image reconstruction technology are avoided, the navigation operation of the capsule robot in the bent intestinal tract is assisted by monocular vision, the structure is simple and reliable, the solution of the centroid position in a dark area is simple and convenient, the operation process is simple and rapid, the man-machine interaction operation is convenient, and the operability of the capsule robot in the bent intestinal tract is improved.
Drawings
Fig. 1 is a schematic diagram of a technical scheme of a robot-robot interactive navigation control of a double-hemisphere capsule.
Fig. 2(a) is a partially enlarged view of the external structure of the double hemispherical capsule robot.
Fig. 2(b) is a partially enlarged view of the internal structure of the double hemispherical capsule robot.
Fig. 3 is a schematic view of a navigation process of the double-hemisphere type capsule robot.
Fig. 4 is a general view of the relationship between coordinate systems.
Fig. 5(a) is a schematic view of the installation of the vertical angle sensor.
Fig. 5(b) is a schematic diagram of the determination of the angle α between the vertical line of the image plane and the vertical line by the vertical angle sensor.
Fig. 6 is a relationship diagram of a computer image coordinate system and a pixel coordinate system.
FIG. 7 is a moving coordinate system OC-XWYWZWAnd axis coordinate system O of double-hemisphere type capsule robotC-X1Y1Z1And (5) converting the schematic diagram.
FIG. 8 is an illustration of image dark region centroid pixel seating designations.
In the figure: a patient; b, a sickbed; c, orthogonally nesting the Helmholtz coil magnetic field superposition device in three axes; d, a double-hemisphere capsule robot; e1 yaw lever; e2 pitch joystick; f a signal processor; g an external magnetic field driver; h human-computer interaction interface; n pose magnetic field rotation axis. A is position A; b, a position B; c position C; 1, an active hemispherical shell; 2, a passive hemispherical shell; 3 bearing positioning sleeve; 4, a bearing; 5, a round nut; 6, a stepped shaft; 7, radially magnetizing the neodymium iron boron circular ring inner driver; 8, a camera and an image transmission device; 9 double hemispherical capsule robot; 10 plumb angle sensors; 11 image plane.
Detailed Description
The following describes the implementation steps and specific embodiments of the present invention in detail with reference to the technical solutions and the accompanying fig. 3 and 8.
The implementation steps are as follows:
the first step is as follows: the self-rotation angle alpha of the camera around the axis of the double-hemisphere capsule robot is read through the plumb angle sensor, and the value is between-180 degrees and 180 degrees.
The second step is that: and reading the axial direction of the double-hemisphere capsule robot at the moment, namely the directions theta and delta of the rotating magnetic vector at the moment.
The third step: and (3) extracting coordinates of the centroid pixels of the dark regions from the intestinal tract image, wherein the values of the coordinate values are between (0,0) and (640, 480). Substituting known parameters into equations (12) - (14), and calculating direction vector n of the centroid of the dark region relative to the capsule robot in the shape of the double hemispheresSAnd then the direction of the rolling magnetic vector to be applied next is calculated.
The specific embodiment is as follows:
(1) when the double-hemisphere capsule robot is at the position a, as shown in fig. 3, the rotation angle α of the camera around the axis of the double-hemisphere capsule robot is 65 ° by the vertical angle sensor.
(2) And reading the magnetic vector direction theta and delta of 50 DEG and 75 DEG from the magnetic field controller, and determining the posture information of the current double-hemisphere capsule robot relative to a fixed coordinate system by combining the self-rotation angle alpha of the camera around the axis of the double-hemisphere capsule robot of 65 deg.
(3) According to the image shot by the camera, extracting the center of mass of the dark area by using an image processing method, and calculating the pixel coordinate of the center of mass of the dark area to be used as the intestinal turning navigation point, wherein the center of mass of the dark area is shown in figure 8, and the pixel coordinate of the center of mass of the dark area is (520, 135). Known pixel coordinates (520, 135) of alpha, theta, 50 DEG, delta, 75 DEG and the center of mass of the dark area are substituted into equations (12) - (14), wherein dx, dy, u0 and v0 are internal parameters of the camera, and a direction vector n of the center of mass of the dark area in a moving coordinate system is calculatedSN is transformed by the rotating magnetic field orthogonal transformation formula (1.6,1,1.6)SExpressed by two variables of theta and delta, calculated as thetaS＝45°，δSAt 30 °, this is trueThe navigation direction of the double-hemisphere capsule robot is determined. Since the rolling magnetic field is applied in the horizontal plane, the direction θ of the rolling magnetic vector to be applied next becomes 45 ° and δ becomes 0.

Claims (5)

1. A visual navigation method in a bent intestinal tract of a double-hemisphere capsule robot is characterized by comprising the following steps:
the first step is as follows: when the double-hemispherical capsule robot needs to navigate at the current position, determining the self-rotation angle of the camera frame around the axis of the double-hemispherical capsule robot through the vertical angle sensor;
the second step is that: determining the posture of the camera relative to a fixed coordinate system by combining pitch angle and yaw angle information of the axis of the double-hemisphere capsule robot and self-rotation angle of the camera frame around the axis of the double-hemisphere capsule robot;
the third step: extracting coordinates of pixels of mass centers of dark areas at corners by using image processing, and taking the coordinates as intestinal turning navigation points; the double-hemisphere type capsule robot is always located in a uniform area of a three-axis Helmholtz coil magnetic field, the pixel coordinate of the center of mass of an image dark area is converted into a direction vector of the center of mass relative to the double-hemisphere type capsule robot in a fixed coordinate system through coordinate transformation, then the direction of a rolling magnetic field applying magnetic vector required by the motion principle of the double-hemisphere type capsule robot needs to be parallel to the horizontal plane, the direction of the rolling magnetic field applying magnetic vector required by the motion principle of the double-hemisphere type capsule robot is calculated, and the navigation of the double-hemisphere type capsule robot is realized; the process specifically comprises the following steps: when the double-hemispherical capsule robot enters a working environment, the navigation can be carried out only by knowing the position and the posture of the double-hemispherical capsule robot in the body; in the magnetic field uniform area, the axial direction of the double-hemisphere type capsule robot is consistent with the direction of the magnetic vector all the time, so that the origin O of the fixed coordinate system O-XYZ is translated to the optical center O of the cameraCThe coordinate axis direction is unchanged to form a moving coordinate system OC-XwYwZwThe gesture of the camera relative to the fixed coordinate system is equivalent to the gesture of the camera relative to the moving coordinate system, and the mass center of a dark area in the fixed coordinate system to be solved is relative to the double-hemisphere type capsule machineThe direction vector of the robot is equivalent to the center of mass of a dark area in a moving coordinate system OC-XwYwZwAn inner direction vector; therefore, only the direction vector of the target point in the moving coordinate system needs to be calculated, and the vector is the final navigation direction; furthermore, only the posture information of the double-hemisphere capsule robot needs to be obtained, and the specific position information of the double-hemisphere capsule robot in a fixed coordinate system does not need to be obtained; let P point be the dark area centroid in the moving coordinate system OC-XwYwZwThe position in (1) is P is the imaging point of the point P on the image plane, and the vector O in the moving coordinate systemC-direction information and vector O of PCP are aligned, so that the vector O in the moving coordinate system is obtainedCThe direction information of P can be converted into a vector O in a solving moving coordinate systemC-direction information of p;
establishing a coordinate system: firstly, in the established moving coordinate system OC-XWYWZWOn the basis of the two semi-spherical capsule robot, an axial coordinate system O of the capsule robot is establishedC-X1Y1Z1，OC-X1The axis being in the horizontal direction, OC-Z1The axis is coincided with the axis direction of the double-hemispherical capsule robot; establishing a camera axis coordinate system OC-XCYCZCCoordinate system OC-XCYCZCFrom a coordinate system OC-X1Y1Z1Around OC-Z1Rotation of the shaft clockwise by an angle alpha, OC-ZCShaft and OC-Z1The axes are overlapped; establishing an image physical coordinate system O1-xy，O1OCIs the focal length of the camera, O1Xy perpendicular to OC-ZCA shaft, and O1The x axis and OC-XCAxis parallel, O1The y-axis and OC-YCThe axes are parallel; establishing a computer image coordinate system O0Uv, the relationship between the computer image coordinate system and the image physical coordinate system is:
Figure FDA0002877432250000021
Figure FDA0002877432250000022
obtaining the following through conversion:
Figure FDA0002877432250000023
obtaining the relation between the image physical coordinate system and the computer image coordinate system after inversion:
Figure FDA0002877432250000024
wherein, dx, dy, u0，v0Are all internal parameters of the camera, and the physical size of each pixel is dx dy (mm), u0As a physical coordinate system O of the image1-xy origin O1Relative to a computer image coordinate system O0Uv origin O0At O1-offset in the direction of the x-axis, v0As a physical coordinate system O of the image1-xy origin O1Relative to a computer image coordinate system O0Uv origin O0At O1-an offset in the y-axis direction, f being the camera focal length;
in camera axis coordinate system OC-XCYCZCZ of p point on middle, image planeCThe coordinate value is the focal length f of the camera, so that the point p is in the camera axis coordinate system Oc-XcYcZcCoordinates in (d) are (x, y, f);
double-hemisphere type capsule robot axis coordinate system OC-X1Y1Z1And the camera axis coordinate system Oc-XcYcZc: camera coordinate system OC-XCYCZCIs composed of a double-hemisphere type capsule robot axis coordinate system OC-X1Y1Z1Around OC-Z1The clockwise rotation of the shaft by an angle alpha can obtain the formula (5):
Figure FDA0002877432250000031
the reverse operation is carried out to obtain:
Figure FDA0002877432250000032
Figure FDA0002877432250000033
moving coordinate system OC-XWYWZWAnd axis coordinate system O of double-hemisphere type capsule robotC-X1Y1Z1The conversion process of (1) is as follows: initially at XWOCZWIn-plane rotating magnetic vector B, its normal vectors n and OC-YWThe axes are coincident, firstly, the X axis is wound around O in order to ensure that the X axis is always in the horizontal plane in the transformation processC-ZWThe shaft rotates clockwise by an angle theta and then rotates around the shaft OC-X1The shaft rotates anticlockwise by an angle delta to obtain a coordinate system OC-X1Y1Z1Wherein the theta angle is a yaw angle, and the delta angle is a pitch angle; after rotation, the normal vector of the magnetic vector plane is formed by OC-YWThe axis becomes n (sin θ cos δ cos θ cos δ sin δ) and the orthogonal transformation matrix is
Figure FDA0002877432250000034
Then
Figure FDA0002877432250000035
To obtain
Figure FDA0002877432250000041
Wherein
Figure FDA0002877432250000042
Therefore, in combination with the above coordinate transformation, when the double-hemisphere type capsule robot is under the action of the rotating magnetic field, the yaw angle in the direction of the magnetic vector is theta, the pitch angle is delta, the autorotation angle of the camera around the axis of the double-hemisphere type capsule robot is alpha, the computer image coordinates of the centroid in the dark area on the image are (u, v), and the computer image coordinates of the centroid in the dark area on the image are calculated by the above parameters in the moving coordinate system OC-XWYWZWInner direction nS：
Figure FDA0002877432250000043
Wherein
Figure FDA0002877432250000044
Figure FDA0002877432250000045
Then n is transformed by orthogonal transformation methodS(XW,YW,ZW) And the transformation is expressed by two variables of a roll angle theta and a pitch angle delta, and the final navigation direction is obtained.
2. The method for visual navigation in the bent intestinal tract of the double-hemisphere capsule robot as claimed in claim 1, wherein in the first step, the method for determining the rotation angle of the camera frame around the axis of the double-hemisphere capsule robot is as follows: observing an image plane along the axial direction on the axial vertical plane of the double-hemisphere capsule robot, and obtaining an included angle alpha between the vertical line of the image plane and the vertical line of the plumb by virtue of a vertical angle sensor and the action of gravity, namely the self-rotation angle of the camera frame around the axial line of the double-hemisphere capsule robot.
3. The method for visual navigation in the curved intestinal tract of the double-hemisphere capsule robot as claimed in claim 1 or 2, wherein in the second step, the specific determination method of the posture of the camera relative to the fixed coordinate system is as follows: the following effect of the double-hemisphere capsule robot in the rotating magnetic field is utilized to judge that the axial direction of the double-hemisphere capsule robot is consistent with the direction of the rotating magnetic vector all the time, so that the optical axis direction of a camera of the double-hemisphere capsule robot is two attitude angles of a swing angle theta and a pitch angle delta inside a fixed coordinate system; and combining the self-rotation angle of the camera frame around the axis of the double-hemisphere capsule robot determined in the first step to obtain the posture information of the double-hemisphere capsule robot relative to a fixed coordinate system.
4. The method for visual navigation in the curved intestinal tract of the double-hemisphere capsule robot as claimed in claim 1 or 2, wherein the orthogonal transformation method is as follows: vector n is first introducedSProjection to XWOCYWIn-plane, the projection and OC-XWThe included angle of the shaft is a side swing angle theta; then, the vector nSProjection to XWOCZWIn-plane, the projection and OC-ZWThe included angle of the shafts is a pitch angle delta.
5. The method for visual navigation in the curved intestinal tract of the double-hemisphere capsule robot as claimed in claim 3, wherein the orthogonal transformation method is as follows: vector n is first introducedSProjection to XWOCYWIn-plane, the projection and OC-XWThe included angle of the shaft is a side swing angle theta; then, the vector nSProjection to XWOCZWIn-plane, the projection and OC-ZWThe included angle of the shafts is a pitch angle delta.
CN201910056648.3A 2019-01-22 2019-01-22 Double-hemispherical capsule robot bending intestinal visual navigation method Active CN109620108B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910056648.3A CN109620108B (en) 2019-01-22 2019-01-22 Double-hemispherical capsule robot bending intestinal visual navigation method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910056648.3A CN109620108B (en) 2019-01-22 2019-01-22 Double-hemispherical capsule robot bending intestinal visual navigation method

Publications (2)

Publication Number Publication Date
CN109620108A CN109620108A (en) 2019-04-16
CN109620108B true CN109620108B (en) 2021-04-20

Family

ID=66062631

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910056648.3A Active CN109620108B (en) 2019-01-22 2019-01-22 Double-hemispherical capsule robot bending intestinal visual navigation method

Country Status (1)

Country Link
CN (1) CN109620108B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109998457B (en) * 2019-04-29 2021-07-16 大连理工大学 A highly integrated capsule robot with active and passive dual hemispheres that can transmit images wirelessly
CN114066781B (en) * 2022-01-18 2022-05-10 浙江鸿禾医疗科技有限责任公司 Recognition and positioning method, storage medium and equipment for intestinal images of capsule endoscopy

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4128505B2 (en) * 2003-09-05 2008-07-30 オリンパス株式会社 Capsule endoscope
CN100450729C (en) * 2006-10-17 2009-01-14 华南理工大学 Micro-robot motion and pose system controlled by external magnetic field and its control method and application
DE102009060514A1 (en) * 2009-12-23 2011-06-30 Siemens Aktiengesellschaft, 80333 Coil system and method for non-contact magnetic navigation of a magnetic body in a working space
CN102162738B (en) * 2010-12-08 2012-11-21 中国科学院自动化研究所 Calibration method of camera and inertial sensor integrated positioning and attitude determining system
US10646288B2 (en) * 2017-04-12 2020-05-12 Bio-Medical Engineering (HK) Limited Automated steering systems and methods for a robotic endoscope

Also Published As

Publication number Publication date
CN109620108A (en) 2019-04-16

Similar Documents

Publication Publication Date Title
Xu et al. A novel system for closed-loop simultaneous magnetic actuation and localization of WCE based on external sensors and rotating actuation
CN104983385B (en) An active and passive dual hemispherical capsule robot and its attitude adjustment and turning drive control method
Slawinski et al. Emerging issues and future developments in capsule endoscopy
US10102334B2 (en) System and method for automatic navigation of a capsule based on image stream captured in-vivo
CN114587241B (en) Magnetic control device capable of positioning capsule endoscope
Xu et al. Adaptive simultaneous magnetic actuation and localization for WCE in a tubular environment
CN103356150A (en) Capsule endoscope magnetic field driving system under orientation tracking guide
Kim et al. Magnetic navigation system composed of dual permanent magnets for accurate position and posture control of a capsule endoscope
CN109620108B (en) Double-hemispherical capsule robot bending intestinal visual navigation method
CN109998457A (en) It is a kind of can radio transmitting image the passive highly integrated capsule robot of double hemisphere of master
Mura et al. Vision-based haptic feedback for capsule endoscopy navigation: a proof of concept
Prendergast et al. A platform for developing robotic navigation strategies in a deformable, dynamic environment
Sun et al. Magnetic actuation systems and magnetic robots for gastrointestinal examination and treatment
JP2022181146A (en) Operation method of magnetic capsule endoscope full-posture measurement system
KR101600985B1 (en) Medical imaging system using wireless capsule endoscope and medical image reconstruction method for the same
Zhang et al. Design and implementation of a highly integrated dual hemisphere capsule robot
CN105286762A (en) External-use controller for positioning, steering and displacement of in-vivo microminiature device
CN115316918B (en) All-round observation of underactuated capsule robot and its axis flipping magnetic control operation method
Sun et al. Multi-applications of a magnet configuration in actuating capsule endoscope
CN109770836B (en) A dual-image visual navigation method in the curved intestine of a dual-hemisphere capsule robot
CN115067863B (en) Wireless capsule endoscope driving system based on spherical driver
Peker et al. Towards capsule endoscope locomotion in large volumes: design, fuzzy modeling, and testing
CN113475997A (en) Reciprocating rotary driving system, device and medium of wireless capsule endoscope
CN108460820B (en) Micro mobile device control device and method based on image feedback
Li et al. Colon lumen center detection enables autonomous navigation of an electromagnetically actuated soft-tethered colonoscope

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant