CN109620108A - Vision navigation method in a kind of double dome-type capsule robots bending enteron aisle - Google Patents
Vision navigation method in a kind of double dome-type capsule robots bending enteron aisle Download PDFInfo
- Publication number
- CN109620108A CN109620108A CN201910056648.3A CN201910056648A CN109620108A CN 109620108 A CN109620108 A CN 109620108A CN 201910056648 A CN201910056648 A CN 201910056648A CN 109620108 A CN109620108 A CN 109620108A
- Authority
- CN
- China
- Prior art keywords
- axis
- coordinate system
- type capsule
- double dome
- angle
- 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.)
- Granted
Links
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
Vision navigation method in a kind of double dome-type capsule robots bending enteron aisle, belongs to automation engineering technical field.This method is on the basis of the side-sway pitch angle using double trackability of the dome-type capsule robot in universal rotary magnetic field to determine its axis, vertical angular transducer is installed in double dome-type capsule robots, determine that camera around the angle of rotation of double dome-type capsule robot axis, and then determines camera posture；Recycle the uniformity of universal rotary magnetic field, it is determined in conjunction with posture information by coordinate conversion and is bent direction vector of the enteron aisle dark picture areas mass center relative to double dome-type capsule robots in fixed coordinate system, realize navigation of the universal rotary magnetic field to double dome-type capsule robots in bending enteron aisle.The present invention is avoided using multi-lens camera structure and complicated enteron aisle three-dimensional reconstruction, and need not calculate double dome-type capsule robot more specific location informations, and monocular vision can be realized and assist double navigation operations of the dome-type capsule robot in enteron aisle.
Description
Technical field
The invention belongs to automation engineering technical fields, are related to a kind of double dome-type capsules of space universal rotary magnetic field driving
Vision navigation method of the robot in bending enteron aisle.
Background technique
Human gastrointestinal tract easily causes various fatal diseases, and wherein colorectal cancer is most common illness.However, most of
If enterogastric diseases can will be significantly improved by early detection and diagnosis, cure rate.Therefore, gastrointestinal examination diagnosis is in medical field
Seem particularly significant.Being used to check the most common instrument of enterogastric diseases is conventional endoscope, since conduit can be inserted into length
Limitation, can not check entire enteron aisle.And endoscopic catheters are during insertion, and gastrointestinal tract soft tissue is be easy to cause to damage
Wound, can cause patient pain.
First capsule endoscope M2A is successfully developed from Given Image company, Israel in 2000 and is led in next year
It crosses U.S. FDA and is applied to clinical, just started the revolution of endoscopy.It, can be real by swallowing into after gastrointestinal tract
The now inspection of complete small intestine avoids the direct insertion endoscope of tradition to patient's bring pain and discomfort.After M2A capsule endoscope
Later, the Intelligent Miro-system Center of Japanese Olympus and RF Co.Ltd, South Korea also release one after another
Oneself commercialization series of products EndoCapsule, Norika and MiroCam, China, Kingsoft, Chongqing science and technology (group) have
Limit company also successfully releases the OMOM capsule series endoscope with entirely autonomous intellectual property, and enters clinical application.
Above-mentioned capsule endoscope technology is although relative maturity, and from the point of view of clinical application feedback, there are still problems:
(1) the double dome-type capsule robot initiative controls of capsule: since existing clinical capsule endoscope does not have actively row mostly
Mechanism is walked, gastrointestinal motility is relied on and passively advances, thus its movement in gastrointestinal tract has randomness, and capsule misses disease
It can not be returned after becoming area, doctor can not also carry out careful observation in detail to area-of-interest, therefore omission factor is higher and checks effect
Rate is low.(2) the double dome-type capsule robot navigation problems of capsule: due to real-time position of the existing capsule endoscope in gastrointestinal tract
Setting not can determine that with posture (pose), can not navigate to it, to just cannot achieve to the double dome-type capsule machines of capsule
People effectively controls.These problems are related to the active homing motion control of the double dome-type capsule robots of capsule, therefore want
The efficient diagnosis and treatment function of realizing capsule endoscope, not requiring nothing more than it has reliable movement walking mechanism, carries out with greater need for it
Active homing.
To realize the double dome-type capsule robots of capsule in the active movement walking and non-structure environment in gastrointestinal tract
Turning walking problem, this seminar designs and develops the double dome-type capsule robots of capsule for providing double semiglobes, and obtains
A kind of national inventing patent " master passively double hemispherical capsule robots and its pose adjustment and turning drive control method " (patent
Number: CN201510262778.4) and " a kind of space universal rotary magnetic field man-machine interaction control method " (patent No.:
ZL201610009285.4)。
In patent " a kind of space universal rotary magnetic field man-machine interaction control method " (patent No.: ZL201610009285.4)
The specific method for realizing space universal rotary magnetic field human-computer interactive control is used in longitude-latitude coordinates to rotate magnetic vector side-sway
Angle θ and two attitude angles of pitch angle δ are the space universal rotary magnetic field Superposition Formula of the current forms of input variable, make orientation
Variable is controlled by the α in cartesian coordinate system, three variables of beta, gamma are converted to θ and δ two in longitude-latitude coordinates by orthogonal transformation
A variable to convert the superposition problem of the bidimensional in plane for the three-dimensional overlay problem of space universal rotary magnetic field, and passes through
Side-sway is separated control with pitch angle respectively by two control sticks, is folded in three axis Helmholtz coil device field homogeneity regions
The rotation magnetic vector added can independently be controlled along side-sway and pitching angular direction, realize low dimensional separable geometries interaction rotary magnetic
Vector controlled.Longitude-latitude coordinates are fixed world coordinate system, therefore will unitedly call longitude-latitude coordinates herein is fixed coordinate system.
A kind of patent " master passively double hemispherical capsule robots and its pose adjustment and turning drive control method " (patent
Number: a kind of overall structure of the double dome-type capsule robots proposed in CN201510262778.4) are as follows: double dome-type capsule machines
Device people's structure includes active hemisphere and passive hemisphere two parts, by diametrical magnetization neodymium iron boron annulus inner driver 7 and camera shooting
Head will finally be taken the photograph with 8 elastic conjunction of image transmission by multi-diameter shaft 6 also with camera and 8 elastic conjunction of image transmission
Active hemisphere is constituted as head and 8 component of image transmission are interference fitted with active hemispherical Shell 1 again；Bearing abutment sleeve 3 with
The passive interference fit of hemispherical Shell 2 constitutes passive hemisphere, active hemisphere and passive hemisphere by the process of the suspension connection of bearing 4
It is as follows: bearing 4 to be mounted on the multi-diameter shaft 6 of active hemisphere component, then by the bearing on active hemisphere component multi-diameter shaft 6
4 are fitted into together in bearing abutment sleeve 3, have a step to realize 4 outer ring axially position of bearing, circle spiral shell inside bearing abutment sleeve 3
Mother 5 is packed on multi-diameter shaft 6 so that by 4 inner ring axially position of bearing, round nut 5 cannot be projected into other than spherical surface, to prevent actively partly
Driving round nut 5 to contact in ball rotation process with enteron aisle influences pose adjustment.The working principle of double dome-type capsule robots are as follows:
It includes camera and image transmission 8 that the coupling magnetic moment of rotating excitation field and diametrical magnetization neodymium iron boron annulus inner driver 7, which drives,
Active hemisphere dally around passive hemisphere is opposite, active hemisphere is in driving condition, and passive hemisphere is in counterweight effect
Under be below always, be in drive lacking state, the passive hemisphere under restrictions of the frictional force contact with gastrointestinal tract is in static
State can prevent from rolling when double dome-type capsule robot posture adjustments, active hemisphere be in top do not contacted with gastrointestinal tract or
Person and gastrointestinal tract contact area are smaller, and active hemisphere is located relatively at following static passive hemisphere idle running, camera and figure
As the camera optical axis in transmitting device 8 is consistent with double dome-type capsule robot axis, contacted when adjusting posture in gastrointestinal tract
Apply rotation magnetic vector, the relatively passive hemisphere idle running of active hemisphere above face, servo-actuated effect makes double dome-type capsule machines
The axis that people's axis follows always respective party parallactic angle rotating excitation field realizes that the posture in gastrointestinal tract arbitrarily adjusts.
A kind of patent " master passively double hemispherical capsule robots and its pose adjustment and turning drive control method " (patent
Number: the CN201510262778.4) navigation procedure of the double dome-type capsule robots proposed are as follows: three groups of coils are mutually orthogonal embedding
Set is mounted to the orthogonal nested Helmholtz coil magnetic field superposition device c of three axis, and patient a is allowed to swallow double dome-type capsule robot d,
And lie on hospital bed b, the position of adjustment hospital bed b makes patient d be in the orthogonal nested Helmholtz coil magnetic field superposition device c of three axis
Central area.When double dome-type capsule robots are in position A, according to double dome-type capsule robot axis and magnetic vector
Direction servo-actuated effect principle consistent always, adjustment magnetic vector direction make radio transmitting image be directed at enteron aisle bending direction, at this time
Magnetic vector direction is consistent with enteron aisle bending direction, and applies the rotation magnetic vector vertical with enteron aisle bending direction in the horizontal plane,
To drive double dome-type capsule robots to be rolled to position B, repeats above procedure and move to position C, and so on.
Although two patents of invention give the bending enteron aisle control method of double dome-type capsule robots, this two
Invention does not provide the method for how determining that magnetic vector direction makes radio transmitting image alignment enteron aisle bending direction, i.e., does not give
Specific navigation direction determines method out.
To determine navigation direction, it is necessary to first acquire the posture information of current double dome-type capsule robots.It is general at present
Time vision guided navigation all use multi-vision visual system, however double dome-type capsule robots inner space is narrow, installs multiple takes the photograph
As the method for head can be limited by double dome-type capsule robots space, need additional circuit transmission information and to more ocular heads
Installation accuracy have higher requirement, to cause double dome-type capsule robot internal structures excessively complicated, and complicated circuit
It will cause system reliability reduction.Although three-dimensional reconstruction can calculate current double dome-type capsule robot positions, calculate
Method is complicated, more demanding to image parameter.
Therefore, this seminar is special in conjunction with double dome-type capsule robot structures on the basis of two above patent of invention
It seeks peace man-machine interaction control method, proposes vision navigation method in a kind of double dome-type capsule robot bending enteron aisles.
Utilize servo-actuated effect of double dome-type capsule robots in rotating excitation field, it is known that double dome-type capsule robot axis
Line is consistent with rotary magnetic direction vector always, i.e., the optical axis direction of camera is also consistent with rotary magnetic direction vector, therefore, posture
After adjustment static double dome-type capsule robot cameras optical axis direction it is known that can with lateral swinging angle θ in fixed coordinate system with
Pitch angle δ two attitude angles description, but camera is unknown around the rotation angle of double dome-type capsule robot axis, and have with
Machine, therefore, it is impossible to determine camera posture, it is also necessary to determine camera around the angle of rotation of double dome-type capsule robot axis
Information just can determine that camera posture.This patent is proposed using a kind of vertical angle being installed in double dome-type capsule robots
Sensor under the effect of gravity can in the vertical plane of double dome-type capsule robot axis by vertical angular transducer
Camera is read around the rotation angle of double dome-type capsule robot axis, is denoted as α.Obtain camera optical axis axis direction and
For camera after the rotation angle information of double dome-type capsule robot axis, posture of the camera in fixed coordinate system can be true
It is fixed.The dark space mass center location of pixels for extracting emitted radio transmitting image when camera shooting bending enteron aisle again, in this, as intestines
Turning navigation spots in road can determine the bending enteron aisle image in fixed coordinate system by coordinate conversion in conjunction with the posture information of camera
Direction vector of the upper dark space mass center relative to double dome-type capsule robots realizes the universal magnetic field in space to double dome-type capsule machines
The turning of device people is navigated.
Currently, not yet it has been proposed that using the uniformity and double dome-type capsule robots of universal rotary magnetic field in magnetic field
Trackability, the relatively fixed coordinate system of double dome-type capsule robot cameras is determined by simple vertical angular transducer
Posture information and method that navigation direction is determined by coordinate transform, the remarkable advantage of this method is only to need to install additional a letter
Single vertical angular transducer also reconstructs skill without the use of complicated enteron aisle 3-dimensional image without using multi-vision visual system
Art, by the monocular cam vision of double dome-type capsule robots and uniformity, the double dome-types of combination universal rotary magnetic field
Trackability of the capsule robot in universal rotary magnetic field, is converted by coordinate, obtains the final of double dome-type capsule robots
Navigation information.
Summary of the invention
The present invention is provided a kind of driven with space universal rotary magnetic field and realizes double dome-type capsule robots in bending enteron aisle
Interior vision navigation method is determining double hemisphere with follow-up characteristic of double dome-type capsule robots in universal rotary magnetic field
On the basis of the side-sway of type capsule robot axis and pitch attitude angle, by vertical angular transducer, determine camera around
The rotation angle of double dome-type capsule robot axis, and then determine posture of the camera relative to fixed coordinate system, it is final true
It is scheduled in fixed coordinate system direction vector of the dark space mass center relative to double dome-type capsule robots on bending enteron aisle image, is realized
Navigation in bending enteron aisle to double dome-type capsule robots.
The technical solution of the present invention is as follows:
Vision navigation method in a kind of double dome-type capsule robots bending enteron aisle, steps are as follows:
Step 1: being passed when double dome-type capsule robots are navigated on current location by vertical angle
Sensor 10 determines camera frame around the rotation angle of double dome-type capsule robot axis.
Camera frame described in the technical solution first step is true around the rotation angle of double dome-type capsule robot axis
Determine method are as follows: observe along axis direction as plane 11, by vertical angle in the axis vertical plane of double dome-type capsule robots 9
Sensor 10 can be obtained as the angle α between plane vertical line and plumb line, i.e., camera is around double dome-types due to gravity
The rotation angle of capsule robot axis.
Step 2: in conjunction with the pitch angle of double dome-type capsule robot axis at this time and side-sway angle information and camera around
The rotation angle of double dome-type capsule robot axis determines posture of the camera relative to fixed coordinate system with this.
Camera described in technical solution second step specifically determines method relative to the posture of fixed coordinate system are as follows: utilizes
Servo-actuated effect of double dome-type capsule robots in rotating excitation field, it is known that the axis direction of double dome-type capsule robots is always
Consistent with rotary magnetic direction vector, therefore, the optical axis direction of double dome-type capsule robot cameras is in fixed coordinate system
Lateral swinging angle θ and two attitude angles of pitch angle δ；The camera frame determined in the combination technology scheme first step is around double dome-type capsules
The rotation angle of robot axis is to get having arrived posture information of double dome-type capsule robots relative to fixed coordinate system.
Step 3: extracting the dark space mass center pixel coordinate of corner with image procossing, turns and navigate in this, as enteron aisle
Point.It, will by coordinate transform since double dome-type capsule robots are in three axis Megnetic Equal Region Inside Helmholtz Coil domains always
The pixel coordinate of dark picture areas mass center is converted into direction of the mass center relative to double dome-type capsule robots in fixed coordinate system
Vector, then the application as required by double dome-type capsule robot motion principles roll magnetic field magnetic direction vector and need to put down with horizontal plane
Row needs the magnetic vector direction that applies to calculate and move to target position from current location, realizes double dome-types with this
The navigation of capsule robot.
Double dome-type capsule robot navigation coordinate method for transformation described in technical solution third step and calculation formula tool
Body are as follows: when double dome-type capsule robots enter working environment, need to know that its position and posture in vivo just can be carried out and lead
Boat.Since the rotating excitation field that three axis Helmholtz coils generate has area uniformity and the same tropism of each point, so equal in magnetic field
In even area, no matter double dome-type capsule robots are in any position, and axis direction is consistent with magnetic vector direction always, therefore
The origin O of fixed coordinate system O-XYZ can be moved to the optical center O of cameraC, change in coordinate axis direction is constant, constitutes moving coordinate system OC-
XwYwZw, camera is equivalent to posture of the camera relative to moving coordinate system, and required solution relative to the posture of fixed coordinate system
The dark space mass center in fixed coordinate system be equivalent to dark space mass center dynamic relative to the direction vector of double dome-type capsule robots
Coordinate system OC-XwYwZwInterior direction vector.Therefore only need to calculate direction vector of the target point in moving coordinate system, this to
Amount is final navigation direction.Therefore, the posture information that we only need to obtain double dome-type capsule robots (can be by technical solution
Second step acquires), without knowing double more specific location informations of the dome-type capsule robot in fixed coordinate system.If P point
It is dark space mass center in moving coordinate system OC-XwYwZwIn position, p be P point as the imaging point in plane, it would be desirable to dynamic seat
Mark is interior vector OCThe directional information and vector O of-PCThe direction of-p is consistent, and institute is in the hope of vector O in moving coordinate systemCBelieve in the direction of-P
Breath, which can be converted into, seeks vector O in moving coordinate systemCThe directional information of-p.
In order to carry out the coordinate conversion of next step, coordinate system need to be established.Firstly, established moving coordinate system above
OC-XWYWZWOn the basis of, establish double dome-type capsule robot axis coordinate system OC-X1Y1Z1, OC-X1Axis in the horizontal direction,
OC-Z1Axis is overlapped with double dome-type capsule robot axis directions；Establish camera axis coordinate system OC-XCYCZC, coordinate system OC-
XCYCZCBy coordinate system OC-X1Y1Z1Around OC-Z1Axis rotates clockwise α angle and obtains, OC-ZCAxis and OC-Z1Overlapping of axles；Establish image
Physical coordinates system O1- xy, O1OCLength be camera focal length, O1- xy is perpendicular to OC-ZCAxis, and O1X-axis and OC-XCAxis is parallel,
O1Y-axis and OC-YCAxis is parallel；Establish computer picture coordinate system O0- uv, computer picture pixel coordinate system and physical coordinates system
Between relationship are as follows:
It is available by converting:
The relationship between image physical coordinates system and computer picture pixel coordinate system is just obtained after inverting:
Wherein, dx, dy, u0, v0It is camera inner parameter, the physical size size of each pixel is dx*dy (mm),
u0For image physical coordinates system O1- xy origin O1Relative to computer picture coordinate system O0- uv origin O0In O1In x-axis direction
Offset, v0For image physical coordinates system O1- xy origin O1Relative to computer picture coordinate system O0- uv origin O0In O1Y-axis side
Upward offset, f are camera focal length.
In camera axis coordinate system OC-XCYCZCIn, as the Z of p point in planeCCoordinate value is the focal length f of camera, therefore
P point is in camera axis coordinate system Oc-XcYcZcIn coordinate be (x, y, f).
Double dome-type capsule robot axis coordinate system OC-X1Y1Z1Turn between camera axis coordinate system Oc-XcYcZc
It changes: camera coordinate system OC-XCYCZCIt is by double dome-type capsule robot axis coordinate system OC-X1Y1Z1Around OC-Z1Axis is clockwise
Rotation alpha angle obtains, then can obtain formula
Inverse operation obtains:
Moving coordinate system OC-XWYWZWWith double dome-type capsule robot axis coordinate system OC-X1Y1Z1Conversion process are as follows: such as
Fig. 7 is initially at XWOCZWThe magnetic vector B of plane internal rotation, spin-plane method vector n and OC-YWOverlapping of axles, in order to guarantee to convert
In the process X-axis always in the horizontal plane, first around OC-ZWAxis rotates clockwise the angle θ, further around OC-X1Axis rotates the angle δ counterclockwise, i.e.,
Coordinate system O can be obtainedC-X1Y1Z1, wherein the angle θ is lateral swinging angle, and the angle δ is pitch angle.After rotating, magnetic vector plane normal vector by
OC-YWAxis becomes n=(sin θ cos δ cos θ cos δ sin δ), and orthogonal transform matrix is
Then
It obtains
Wherein
So in conjunction with the above coordinate transform, when double dome-type capsule robots are under rotating excitation field effect, magnetic vector at this time
Direction lateral swinging angle is θ, pitch angle δ, rotation angle [alpha] of the camera around double dome-type capsule robot axis, dark space on image
The computer picture pixel coordinate of mass center is (u, v), can calculate dark picture areas mass center in moving coordinate system O by the above parameterC-
XWYWZWInterior direction nS:
Wherein
Again by patent " a kind of space universal rotary magnetic field man-machine interaction control method " (patent No.:
ZL201610009285.4) the orthogonal transformation method proposed, by (XW,YW,ZW) be transformed to two variables of θ and δ indicate to get
Final navigation direction is arrived.
The orthogonal transformation method are as follows: first by vector nSProject to XWOCYWIn face, this projection and OC-XWThe angle of axis is
Lateral swinging angle θ；Again by vector nSProject to XWOCZWIn face, this projection and OC-ZWThe angle of axis is pitch angle δ.
It need to apply in the horizontal plane due to rolling magnetic field, acquire nSAnd it is transformed to be indicated with two variables of θ and δ
Afterwards, make its δ value 0 to get the rolling magnetic field that should apply to navigation, thus complete double dome-type capsule machines described in third step
The navigation of people.
Beneficial effects of the present invention: it under limited steric requirements inside double dome-type capsule robots, makes full use of outstanding
The follow-up characteristic for stopping double dome-type capsule robot axis and universal rotary magnetic field magnetic vector when posture adjustment mode, in double dome-type glue
It only installs a vertical angular transducer before capsule robot additional, and does not have to calculate double dome-type capsule robots in fixed coordinate system
More specific location information, can determine the navigation direction of double dome-type capsule robots, avoid using complicated more mesh camera knots
Structure and complicated enteron aisle 3-dimensional image reconfiguration technique, and complicated three-dimension sensor and complicated transmission circuit, realize and pass through
Monocular vision assists navigation operation of double dome-type capsule robots in bending enteron aisle, simple and reliable for structure, dark space mass center side
Position solves simplicity, and operating process is simple and fast, and man-machine interactive operation is convenient, improves the double dome-type capsule robots of capsule curved
Operability in bent enteron aisle.
Detailed description of the invention
Fig. 1 is the technical solution schematic diagram of double dome-type capsule robot human-computer interaction Navigation Controls.
Fig. 2 (a) is double hemispherical capsule robot external structure partial enlarged views.
Fig. 2 (b) is double dome-type capsule robot internal structure partial enlarged views.
Fig. 3 is double dome-type capsule robot navigation procedure schematic diagrames.
Fig. 4 is relationship total figure between coordinate system.
Fig. 5 (a) is vertical angular transducer scheme of installation.
Fig. 5 (b) is the schematic diagram determined by vertical angular transducer as the angle α between plane vertical line and plumb line.
Fig. 6 is computer picture coordinate system and pixel coordinate system relational graph.
Fig. 7 is moving coordinate system OC-XWYWZWWith double dome-type capsule robot axis coordinate system OC-X1Y1Z1Transition diagram.
Fig. 8 is dark picture areas mass center pixel coordinate exemplary diagram.
In figure: a patient；B hospital bed；The orthogonal nested Helmholtz coil magnetic field superposition device of tri- axis of c；The bis- dome-type capsules of d
Robot；E1 side-sway control stick；E2 pitch control bar；F signal processor；G extracorporeal magnetic driving device；H human-computer interaction interface；N tune
Appearance magnetic field rotating axis.Location A A；B location B；Location of C C；1 active hemispherical Shell；2 passive hemispherical Shells；3 bearing abutment sleeves；4 axis
It holds；5 round nuts；6 multi-diameter shafts；7 diametrical magnetization neodymium iron boron annulus inner drivers；8 cameras and image transmission；9 pairs of hemisphere
Type capsule robot；10 vertical angular transducers；11 as plane.
Specific embodiment
Implementation steps and specific embodiment of the invention are described in detail below in conjunction with technical solution and attached drawing 3,8.
Implementation steps:
Step 1: reading camera around the rotation angle of double dome-type capsule robot axis by vertical angular transducer
α, value is at -180 ° to 180 °.
Step 2: the axis direction for reading double dome-type capsule robots at this time rotates direction θ, δ of magnetic vector at this time.
Step 3: coordinate value value arrives (640,480) in (0,0) by enteron aisle image zooming-out dark space mass center pixel coordinate
Between.By known parameter substitution formula (12)-(14), calculate dark space mass center relative to the double dome-type capsule machines of double dome-types
The direction vector n of peopleS, then calculate the rolling magnetic vector direction that should apply in next step.
Specific embodiment:
(1) when double dome-type capsule robots are in position A, as shown in figure 3, by vertical angular transducer, camera around
Rotation angle [alpha]=65 ° of double dome-type capsule robot axis.
(2) magnetic vector direction θ=50 ° at this time and δ=75 ° are read from field controller, in conjunction with camera around double dome-types
Rotation angle [alpha]=65 ° of capsule robot axis can determine current double dome-type capsule robots relative to fixed coordinate system
Posture information.
(3) image shot according to camera, extracts its dark space mass center with image processing method, and calculate dark space mass center
Pixel coordinate, in this, as enteron aisle turn navigation spots, dark space mass center as shown in figure 8, dark space mass center pixel coordinate be (520,
135).Known α=65 °, θ=50 °, δ=75 ° and dark space mass center pixel coordinate (520,135) are substituted into formula (12)-(14)
In, wherein dx, dy, u0, v0 are camera inner parameter, calculate direction vector n of the dark space mass center in moving coordinate systemS=
(1.6,1,1.6), by rotating excitation field orthogonal transformation formula by nSIt is indicated with two variables of θ and δ, calculates to obtain θS=45 °, δS=
30 °, double dome-type capsule robot navigation directions have thus been determined.It need to apply in the horizontal plane due to rolling magnetic field,
Rolling magnetic vector direction θ=45 ° that should then apply in next step, δ=0.
Claims (7)
1. vision navigation method in a kind of double dome-type capsule robot bending enteron aisles, which is characterized in that steps are as follows:
Step 1: when double dome-type capsule robots are navigated on current location, by vertical angular transducer,
Determine camera frame around the rotation angle of double dome-type capsule robot axis；
Step 2: combining pair pitch angle of dome-type capsule robot axis and side-sway angle information and camera frame around double
The rotation angle of dome-type capsule robot axis, determines posture of the camera relative to fixed coordinate system；
Step 3: the dark space mass center pixel coordinate of corner is extracted with image procossing, in this, as enteron aisle turning navigation spots；
Double dome-type capsule robots are in three axis Megnetic Equal Region Inside Helmholtz Coil domains always, by coordinate transform, image is dark
The pixel coordinate of area's mass center is converted into direction vector of the mass center relative to double dome-type capsule robots in fixed coordinate system, then
The application as required by double dome-type capsule robot motion principles rolls magnetic field magnetic direction vector and need to be parallel to the horizontal plane, and calculates
The rolling magnetic field magnetic direction vector that target position needs to apply is moved to from current location out, realizes double dome-type capsule robots
Navigation.
2. vision navigation method in a kind of double dome-type capsule robot bending enteron aisles according to claim 1, feature
It is, in the first step, rotation angle determination method of the camera frame around double dome-type capsule robot axis are as follows:
The axis vertical plane of double dome-type capsule robots is observed along axis direction as plane, by vertical angular transducer, due to weight
Power effect, obtains as the angle α between plane vertical line and plumb line, i.e. rotation of the camera around double dome-type capsule robot axis
Angle.
3. vision navigation method in a kind of double dome-type capsule robot bending enteron aisles according to claim 1 or 2, special
Sign is, in second step, the camera specifically determines method relative to the posture of fixed coordinate system are as follows: utilizes double dome-types
Servo-actuated effect of the capsule robot in rotating excitation field, can judge the axis directions of double dome-type capsule robots always with rotation
It is consistent to turn magnetic vector direction, therefore, the optical axis direction of double dome-type capsule robot cameras is side-sway in fixed coordinate system
Angle θ and two attitude angles of pitch angle δ；In conjunction with the camera frame determined in the first step around double dome-type capsule robot axis
Rotation angle is to get having arrived posture information of double dome-type capsule robots relative to fixed coordinate system.
4. vision navigation method in a kind of double dome-type capsule robot bending enteron aisles according to claim 1 or 2, special
Sign is, the process of third step specifically: when double dome-type capsule robots enter working environment, need to know it in vivo
Position and posture just can be carried out navigation；In field homogeneity area, the axis direction of double dome-type capsule robots is sweared with magnetic always
Amount direction is consistent, therefore the origin O of fixed coordinate system O-XYZ is moved to the optical center O of cameraC, change in coordinate axis direction is constant, structure
At moving coordinate system OC-XwYwZw, camera is equivalent to appearance of the camera relative to moving coordinate system relative to the posture of fixed coordinate system
State, and the dark space mass center in fixed coordinate system of required solution is equivalent to relative to the direction vector of double dome-type capsule robots
Dark space mass center is in moving coordinate system OC-XwYwZwInterior direction vector；Therefore only need to calculate side of the target point in moving coordinate system
To vector, this vector, that is, final navigation direction；In turn, it is only necessary to the posture information of double dome-type capsule robots is obtained, without
Need to know double more specific location informations of the dome-type capsule robot in fixed coordinate system；If P point is that dark space mass center is sat dynamic
Mark system OC-XwYwZwIn position, p is P point as the imaging point in plane, vector O in moving coordinate systemCThe directional information of-P with to
Measure OCThe direction of-p is consistent, and institute is in the hope of vector O in moving coordinate systemCThe directional information of-P, which can be converted into, seeks vector O in moving coordinate systemC-
The directional information of p；
Establish coordinate system: firstly, in established moving coordinate system OC-XWYWZWOn the basis of, establish double dome-type capsule robots
Axis coordinate system OC-X1Y1Z1, OC-X1Axis in the horizontal direction, OC-Z1Axis is overlapped with double dome-type capsule robot axis directions；
Establish camera axis coordinate system OC-XCYCZC, coordinate system OC-XCYCZCBy coordinate system OC-X1Y1Z1Around OC-Z1Axis rotates clockwise α
Angle obtains, OC-ZCAxis and OC-Z1Overlapping of axles；Establish image physical coordinates system O1- xy, O1OCLength be camera focal length, O1-
Xy is perpendicular to OC-ZCAxis, and O1X-axis and OC-XCAxis is parallel, O1Y-axis and OC-YCAxis is parallel；Establish computer picture coordinate system
O0- uv, relationship between computer picture pixel coordinate system and physical coordinates system are as follows:
By being converted to:
The relationship between image physical coordinates system and computer picture pixel coordinate system is obtained after inverting:
Wherein, dx, dy, u0, v0It is camera inner parameter, the physical size size of each pixel is dx*dy (mm), u0For
Image physical coordinates system O1- xy origin O1Relative to computer picture coordinate system O0- uv origin O0In O1Offset in x-axis direction
Amount, v0For image physical coordinates system O1- xy origin O1Relative to computer picture coordinate system O0- uv origin O0In O1On y-axis direction
Offset, f be camera focal length；
In camera axis coordinate system OC-XCYCZCIn, as the Z of p point in planeCCoordinate value is the focal length f of camera, therefore p point
In camera axis coordinate system Oc-XcYcZcIn coordinate be (x, y, f)；
Double dome-type capsule robot axis coordinate system OC-X1Y1Z1It is converted between camera axis coordinate system Oc-XcYcZc: camera shooting
Head coordinate system OC-XCYCZCIt is by double dome-type capsule robot axis coordinate system OC-X1Y1Z1Around OC-Z1Axis rotates clockwise the angle α
Degree obtains, then obtains formula (5):
Inverse operation obtains:
Moving coordinate system OC-XWYWZWWith double dome-type capsule robot axis coordinate system OC-X1Y1Z1Conversion process are as follows: be initially at
XWOCZWThe magnetic vector B of plane internal rotation, spin-plane method vector n and OC-YWOverlapping of axles, in order to guarantee X-axis in conversion process
Always in the horizontal plane, first around OC-ZWAxis rotates clockwise the angle θ, further around OC-X1Axis rotates the angle δ counterclockwise to get coordinate is arrived
It is OC-X1Y1Z1, wherein the angle θ is lateral swinging angle, and the angle δ is pitch angle；After rotating, magnetic vector plane normal vector is by OC-YWAxis becomes n
=(sin θ cos δ cos θ cos δ sin δ), orthogonal transform matrix is
Then
It obtains
Wherein
So in conjunction with the above coordinate transform, when double dome-type capsule robots are under rotating excitation field effect, magnetic vector direction side-sway
Angle is θ, and pitch angle δ, camera is α around the rotation angle of double dome-type capsule robot axis, dark space mass center on image
Computer picture pixel coordinate is (u, v), calculates dark picture areas mass center in moving coordinate system O by the above parameterC-XWYWZWInterior side
To nS:
Wherein
Again by orthogonal transformation method, by nS(XW,YW,ZW) be transformed to two variables of lateral swinging angle θ and pitch angle δ indicate to get
Final navigation direction is arrived.
5. vision navigation method in a kind of double dome-type capsule robot bending enteron aisles according to claim 3, feature
It is, the process of third step specifically: when double dome-type capsule robots enter working environment, need to know its position in vivo
It sets and posture just can be carried out navigation；In field homogeneity area, the axis direction of double dome-type capsule robots always with magnetic vector
Direction is consistent, therefore the origin O of fixed coordinate system O-XYZ is moved to the optical center O of cameraC, change in coordinate axis direction is constant, constitutes
Moving coordinate system OC-XwYwZw, camera is equivalent to appearance of the camera relative to moving coordinate system relative to the posture of fixed coordinate system
State, and the dark space mass center in fixed coordinate system of required solution is equivalent to relative to the direction vector of double dome-type capsule robots
Dark space mass center is in moving coordinate system OC-XwYwZwInterior direction vector；Therefore only need to calculate side of the target point in moving coordinate system
To vector, this vector, that is, final navigation direction；In turn, it is only necessary to the posture information of double dome-type capsule robots is obtained, without
Need to know double more specific location informations of the dome-type capsule robot in fixed coordinate system；If P point is that dark space mass center is sat dynamic
Mark system OC-XwYwZwIn position, p is P point as the imaging point in plane, vector O in moving coordinate systemCThe directional information of-P with to
Measure OCThe direction of-p is consistent, and institute is in the hope of vector O in moving coordinate systemCThe directional information of-P, which can be converted into, seeks vector O in moving coordinate systemC-
The directional information of p；Establish coordinate system: firstly, in established moving coordinate system OC-XWYWZWOn the basis of, establish double dome-type glue
Capsule robot axis coordinate system OC-X1Y1Z1, OC-X1Axis in the horizontal direction, OC-Z1Axis and double dome-type capsule robot axis
Direction is overlapped；Establish camera axis coordinate system OC-XCYCZC, coordinate system OC-XCYCZCBy coordinate system OC-X1Y1Z1Around OC-Z1Axis is suitable
Hour hands rotation alpha angle obtains, OC-ZCAxis and OC-Z1Overlapping of axles；Establish image physical coordinates system O1- xy, O1OCLength be camera shooting
Head focal length, O1- xy is perpendicular to OC-ZCAxis, and O1X-axis and OC-XCAxis is parallel, O1Y-axis and OC-YCAxis is parallel；Establish computer graphic
As coordinate system O0- uv, relationship between computer picture pixel coordinate system and physical coordinates system are as follows:
By being converted to:
The relationship between image physical coordinates system and computer picture pixel coordinate system is obtained after inverting:
Wherein, dx, dy, u0, v0It is camera inner parameter, the physical size size of each pixel is dx*dy (mm), u0For
Image physical coordinates system O1- xy origin O1Relative to computer picture coordinate system O0- uv origin O0In O1Offset in x-axis direction
Amount, v0For image physical coordinates system O1- xy origin O1Relative to computer picture coordinate system O0- uv origin O0In O1On y-axis direction
Offset, f be camera focal length；
In camera axis coordinate system OC-XCYCZCIn, as the Z of p point in planeCCoordinate value is the focal length f of camera, therefore p point
In camera axis coordinate system Oc-XcYcZcIn coordinate be (x, y, f)；
Double dome-type capsule robot axis coordinate system OC-X1Y1Z1It is converted between camera axis coordinate system Oc-XcYcZc: camera shooting
Head coordinate system OC-XCYCZCIt is by double dome-type capsule robot axis coordinate system OC-X1Y1Z1Around OC-Z1Axis rotates clockwise the angle α
Degree obtains, then obtains formula (5):
Inverse operation obtains:
Moving coordinate system OC-XWYWZWWith double dome-type capsule robot axis coordinate system OC-X1Y1Z1Conversion process are as follows: be initially at
XWOCZWThe magnetic vector B of plane internal rotation, spin-plane method vector n and OC-YWOverlapping of axles, in order to guarantee X-axis in conversion process
Always in the horizontal plane, first around OC-ZWAxis rotates clockwise the angle θ, further around OC-X1Axis rotates the angle δ counterclockwise to get coordinate is arrived
It is OC-X1Y1Z1, wherein the angle θ is lateral swinging angle, and the angle δ is pitch angle；After rotating, magnetic vector plane normal vector is by OC-YWAxis becomes n
=(sin θ cos δ cos θ cos δ sin δ), orthogonal transform matrix is
Then
It obtains
Wherein
So in conjunction with the above coordinate transform, when double dome-type capsule robots are under rotating excitation field effect, magnetic vector direction side-sway
Angle is θ, and pitch angle δ, camera is α around the rotation angle of double dome-type capsule robot axis, dark space mass center on image
Computer picture pixel coordinate is (u, v), calculates dark picture areas mass center in moving coordinate system O by the above parameterC-XWYWZWInterior side
To nS:
Wherein
Again by orthogonal transformation method, by nS(XW,YW,ZW) be transformed to two variables of lateral swinging angle θ and pitch angle δ indicate to get
Final navigation direction is arrived.
6. vision navigation method in a kind of double dome-type capsule robot bending enteron aisles according to claim 4, feature
It is, the orthogonal transformation method are as follows: first by vector nSProject to XWOCYWIn face, this projection and OC-XWThe angle of axis is side
Pivot angle θ；Again by vector nSProject to XWOCZWIn face, this projection and OC-ZWThe angle of axis is pitch angle δ.
7. vision navigation method in a kind of double dome-type capsule robot bending enteron aisles according to claim 5, feature
It is, the orthogonal transformation method are as follows: first by vector nSProject to XWOCYWIn face, this projection and OC-XWThe angle of axis is side
Pivot angle θ；Again by vector nSProject to XWOCZWIn face, this projection and OC-ZWThe angle of axis is pitch angle δ.
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 true CN109620108A (en) | 2019-04-16 |
| CN109620108B 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) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109998457A (en) * | 2019-04-29 | 2019-07-12 | 大连理工大学 | It is a kind of can radio transmitting image the passive highly integrated capsule robot of double hemisphere of master |
| CN114066781A (en) * | 2022-01-18 | 2022-02-18 | 浙江鸿禾医疗科技有限责任公司 | Capsule endoscope intestinal tract image identification and positioning method, storage medium and equipment |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20050054901A1 (en) * | 2003-09-05 | 2005-03-10 | Koichiro Yoshino | Capsule endoscope |
| CN1944003A (en) * | 2006-10-17 | 2007-04-11 | 华南理工大学 | System for controlling micro robot movement and pose by outer magnetic field and its control method and use |
| WO2011076497A1 (en) * | 2009-12-23 | 2011-06-30 | Siemens Aktiengesellschaft | Coil system and method for contactless magnetic navigation of a magnetic body in a workspace |
| CN102162738A (en) * | 2010-12-08 | 2011-08-24 | 中国科学院自动化研究所 | Calibration method of camera and inertial sensor integrated positioning and attitude determining system |
| CN108685560A (en) * | 2017-04-12 | 2018-10-23 | 香港生物医学工程有限公司 | Automation steering and method for robotic endoscope |
-
2019
- 2019-01-22 CN CN201910056648.3A patent/CN109620108B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20050054901A1 (en) * | 2003-09-05 | 2005-03-10 | Koichiro Yoshino | Capsule endoscope |
| CN1944003A (en) * | 2006-10-17 | 2007-04-11 | 华南理工大学 | System for controlling micro robot movement and pose by outer magnetic field and its control method and use |
| WO2011076497A1 (en) * | 2009-12-23 | 2011-06-30 | Siemens Aktiengesellschaft | Coil system and method for contactless magnetic navigation of a magnetic body in a workspace |
| CN102162738A (en) * | 2010-12-08 | 2011-08-24 | 中国科学院自动化研究所 | Calibration method of camera and inertial sensor integrated positioning and attitude determining system |
| CN108685560A (en) * | 2017-04-12 | 2018-10-23 | 香港生物医学工程有限公司 | Automation steering and method for robotic endoscope |
Non-Patent Citations (1)
| Title |
|---|
| 张永顺，杨慧远: ""磁场与视觉共融的多模态胶囊机器人人机交互控制"", 《机器人》 * |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109998457A (en) * | 2019-04-29 | 2019-07-12 | 大连理工大学 | It is a kind of can radio transmitting image the passive highly integrated capsule robot of double hemisphere of master |
| CN109998457B (en) * | 2019-04-29 | 2021-07-16 | 大连理工大学 | Active and passive double-hemisphere high-integration capsule robot capable of wirelessly transmitting images |
| CN114066781A (en) * | 2022-01-18 | 2022-02-18 | 浙江鸿禾医疗科技有限责任公司 | Capsule endoscope intestinal tract image identification and positioning method, storage medium and equipment |
| CN114066781B (en) * | 2022-01-18 | 2022-05-10 | 浙江鸿禾医疗科技有限责任公司 | Capsule endoscope intestinal image identification and positioning method, storage medium and equipment |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109620108B (en) | 2021-04-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104983385B (en) | A kind of main passive double hemispherical capsule robots and pose adjustment thereof drive control method with turning | |
| Carpi et al. | Magnetically controllable gastrointestinal steering of video capsules | |
| Slawinski et al. | Emerging issues and future developments in capsule endoscopy | |
| Song et al. | 6-D magnetic localization and orientation method for an annular magnet based on a closed-form analytical model | |
| CN106102549B (en) | System and method for controlling imaging instrument orientation | |
| EP2347699B1 (en) | Capsule type endoscope including magnetic drive | |
| Xu et al. | A novel system for closed-loop simultaneous magnetic actuation and localization of WCE based on external sensors and rotating actuation | |
| CN105662318B (en) | A kind of space universal rotary magnetic field man-machine interaction control method | |
| CN110327011B (en) | Double-hemisphere capsule robot attitude detection method based on magnetic vector error calibration image | |
| CN109998457A (en) | It is a kind of can radio transmitting image the passive highly integrated capsule robot of double hemisphere of master | |
| Munoz et al. | Analysis of magnetic interaction in remotely controlled magnetic devices and its application to a capsule robot for drug delivery | |
| CN107348931B (en) | Capsule endoscope space attitude determination system | |
| CN101297756A (en) | Combined method of magnetic field and vision for locating gesture of medical micro type robot in vivo | |
| CN105916462A (en) | A medical optical tracking system | |
| WO2017107116A1 (en) | Navigation system for minimally invasive operation | |
| JP2008216089A (en) | Instrument for measuring three-dimensional position and direction of specimen | |
| CN109620108A (en) | Vision navigation method in a kind of double dome-type capsule robots bending enteron aisle | |
| Zabat et al. | IMU-based sensor-to-segment multiple calibration for upper limb joint angle measurement—A proof of concept | |
| JP2014095953A (en) | Operation system for operation object device and operation input device | |
| Taddese et al. | Closed loop control of a tethered magnetic capsule endoscope | |
| JP7270284B2 (en) | Operation method of magnetic capsule endoscope full-posture measurement system | |
| Kim et al. | Magnetic navigation system composed of dual permanent magnets for accurate position and posture control of a capsule endoscope | |
| CN102139137A (en) | External magnetic control drug release capsule system based on digital image navigation | |
| KR101600985B1 (en) | Medical imaging system using wireless capsule endoscope and medical image reconstruction method for the same | |
| CN105286762A (en) | External-use controller for positioning, steering and displacement of in-vivo microminiature device |
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 |