CN105559739A - Capsule endoscope motion control method based on magnetic field intensity change - Google Patents

Abstract

The invention discloses a navigation control method of a medical capsule endoscope and solves the problem of external active control on the capsule endoscope when the permanent magnet capsule endoscope performs internal examinations. The method that a permanent magnet and an electromagnetic coil are combined to control internal stable suspension motion of the capsule endoscope is adopted. The control method comprises links including a sensor signal processing link, a feedforward control law link, a feedback control law link, a state estimator link and the like. In the sensor signal processing algorithm link, position information of the capsule endoscope is acquired from an original signal returned by a sensor; in the feedforward control law link, a theoretical output signal value at the steady state is calculated according to a theoretical model to serve as a basic output signal so as to reduce the system nonlinear effect; in the feedback control law link, closed-loop control is realized according to feedback position information to reduce system errors; the state estimator link is used for eliminating or reducing instability caused by system pure lag. With the adoption of the method, stable constraint of any position of the capsule endoscope in liquids in body cavities by an external device can be realized finally.

Description

A kind of capsule endoscope motion control method based on change of magnetic field strength

Technical field

The present invention relates to field of medical device, particularly relate to a kind of external control method using capsule endoscope to carry out stomach inspection, a specifically medical capsule scope magnetic control navigation control method, can be used for controlling, the motion of guide capsule scope in human body and inspection.

Background technology

Capsule endoscope is that a kind of size shape is similar to capsule pill, is but integrated with the human body alimentary canal checking tool of the photo-optic system of complete set, wireless telecommunication system and electric supply system.Capsule endoscope will move after being swallowed by person under inspection in gastro-intestinal tract, carry out shooting inspection on the way to gastro-intestinal tract inwall, and externally terminal sends image.Doctor by the image received, can position focus and diagnoses.

Capsule endoscope product during current reality uses all adopts passive movement mode, and namely capsule endoscope itself does not have the ability of movement, need move by naturally wriggling of intestinal.The main shortcoming of passive type capsule endoscope is: capsule endoscope in human body can only random motion, shooting, be difficult to according to doctor instruction arrive assigned address, can not stop at specific part, thus exist check blind area, have undetected may.Therefore be mainly used in the detection of disease of intestine at present, stomach and large intestine detection cannot be applied to.Therefore the active control technology of capsule endoscope becomes this area research focus.

The ACTIVE CONTROL of the capsule endoscope in current research is divided into inside and outside driving two kinds of modes.Internal drive is exactly utilize the energy of capsule endoscope self and frame for movement to carry out a kind of type of drive of moving, mainly comprise: marmem driving, micromachine driving, magnetostriction driving, pneumatic actuation, wriggling driving, polypody driving, electrostatic driving and bionical driving etc., most of handling principle conceptual phase.Internal drive has following shortcoming: 1, driving itself needs to consume a large amount of energy, and battery electric quantity may be caused inadequate; 2, the structure of microencapsulation scope becomes complicated, produces difficulty, high expensive; 3, motion structure may produce damage to digestive tract; 4, capsule endoscope motion be difficult to control and speed is slow.

External drive provides the type of drive of power by external equipment for capsule endoscope moves.Conventional available contactless force only has gravity (gravitation) and electromagnetic force, gravity is substantially uncontrollable, human body fluid is conduction, and electric field has impact to human-body biological electrical activity, only have magnetostatic field (or low frequency variations magnetic field) substantially unhinderedly can pass human body, and people is had no adverse effects, comparatively safe.

The mode producing magnetic field mainly can be divided into solenoid and permanent magnet two kinds.From the angle being easy to control, solenoid can control magnetic induction by control coil electric current, more convenient, also has many relevant achievements in research both at home and abroad.But because capsule endoscope is at inside of human body, in vitro, equipment is to the distance of capsule endoscope at about 10-20cm, and magnetic field is decay by the cube of distance to the captivation that small magnet produces substantially for equipment.Produce the magnetic field that operating distance is enough far away, the equivalent current of equipment end magnetic field generator reaches hundreds thousand of ampere, general copper conductor is difficult to support so high electric current density, and uses superconducting coil to relate to the problems such as ultralow temperature cooling, and system is huge and equipment cost is very high.Along with the invention of Nd-Fe-B rare earth permanent magnetic material, superpower permanet magnetic body can be obtained and cheap.Therefore permanent magnet is used to become inevitable principle as main Magnetic Field Source.

Because magnetostatic field is a kind of passive rotational field, there is essence different from gravitational field, electrostatic field etc., scleronomic constraint can not be formed to permanent magnet or ferromagnet separately.This is also identical with life experience: Magnet always accelerates ferrous material to be attracted to oneself surface, and can not be stabilized on certain distance.Only magnetic force size suffered by continuous regulating object under feedback control effect, makes itself and other dynamic balances such as gravity, buoyancy, it could be kept to balance.

But permanent magnet magnetic field intensity is unadjustable, can only be changed the Distribution of Magnetic Field of controlled device position by the mechanical movement of permanent magnet, magnetic force suffered by indirect regulation controlled device, it is very slow to react.

A kind of device using spherical permanent magnet to control capsule endoscope suspension and motion has been invented by Ankon Photoelectric Technology (Wuhan) Co., Ltd. of China, and has applied for patent (patent No. 201310136094.0).Shenzhen graduate school of Harbin Institute of Technology proposes the active capsule endoscope kinetic control system of a kind of magnetic control based on handyman (patent No. 201320151833.9).These two inventions are all based on changing the position of magnetic field generator and attitude, thus change spatial magnetic field, and indirect guide capsule scope moves in vivo.And all do not use feedback control to implement scleronomic constraint to capsule endoscope

Therefore, those skilled in the art is devoted to develop a kind of new capsule endoscope motion control method, by the motion of magnetic capsule scope in the motion guidance body of manipulation body permanent magnet and solenoid, thus solve initiative control when capsule endoscope checks in human body.

Summary of the invention

Because the above-mentioned defect of prior art, technical problem to be solved by this invention is position when how to facilitate that ACTIVE CONTROL capsule endoscope checks in human body compactly and attitude.

For achieving the above object, the invention provides a kind of capsule endoscope motion control method based on change of magnetic field strength, its thinking is: permanent magnet and solenoid are combined, permanent magnet forms stronger basic magnetic field, coil forms the adjustable auxiliary magnetic field of intensity by applying variable current, and both are combined to form the adjustable magnetic field of intensity.According to sensor feedback signal, by certain algorithm controls coil current, realize the scleronomic constraint of capsule endoscope in body fluid.Then by magnetic capsule scope stable motion in the motion guidance body of external hybrid magnetic field generator, thus initiative control when capsule endoscope checks in human body can be solved.

Control method of the present invention, based on the control system of complete set, as shown in Figure 4, adopts the closed loop control algorithm of complete set, realizes the scleronomic constraint of device outside to capsule endoscope optional position in human body intracavity liquid.

1, described complete control system, comprises permanent magnet capsule endoscope, hybrid magnetic field generator, sensor, movement mechanism with multiple degrees of freedom, controller.

Described permanent magnet capsule endoscope, the i.e. capsule endoscope of built-in permanent magnet material, function is identical with general capsule endoscope, but itself is a little permanent magnet.Permanent magnet capsule endoscope is the controlled device guided.

Described hybrid magnetic field generator, can produce the magnetic field of intensity variable within the scope of certain space, comprise a strong permanent magnets and some solenoids, coil is directly over capsule endoscope, permanent magnet is directly over coil, and the distance between coil and permanent magnet is adjustable; Permanent magnet forms stronger basic magnetic field, and coil forms the adjustable auxiliary magnetic field of intensity by applying variable current, and both are combined to form the adjustable magnetic field of intensity.

Described sensor, can detect the change in location of capsule endoscope relative to magnetic field generator, especially the distance change of vertical direction.Can be the medical imaging devices such as X-ray imaging, B ultrasonic, MRI, can be the sensors such as external RFID, ultrasound wave, also can be the sensors such as the accelerometer of capsule endoscope itself inline.

Described movement mechanism with multiple degrees of freedom, is used for driving hybrid magnetic field generator to move in hyperspace, and then drives capsule endoscope to move in human body.

Described controller, comprises motion controller and current controller.The former realizes conventional Multi-axis motion control, the feedback control algorithm control coil electric current that the latter proposes according to the present invention, thus controlling magnetic field, implements scleronomic constraint to capsule endoscope.

2, described complete closed loop control algorithm, comprises sensor signal processing links, feedforward link, feedback control link and state estimations device link.

Described sensor signal processing links, for obtaining the positional information of capsule endoscope in the primary signal that returns from sensor.

Processor carries out digital processing to the primary signal that the sensor returns, and obtains the position of capsule endoscope relative to magnetic field generator, as feedback signal transmission to controller, finally realizes the closed loop control of capsule endoscope position.Different according to kind of sensor, need different signal processing algorithms to resolve.

Described feedforward link, theoretical output signals value when calculating steady statue according to theoretical model, based on output signal to reduce mission nonlinear impact.

First according to Systems Theory model, permanent magnet suffered by capsule endoscope, coil magnetic force and capsule endoscope position is obtained, and the relation of coil current size; Then obtain according to capsule endoscope stress balance relation in a liquid the output current signal that feedovers.Circular is as follows:

1) suffered by described capsule endoscope, the relation of the position coordinates of permanent magnet magnetic force and capsule endoscope is provided by following formula:

f _PM＝F ₁(s)＝(M·▽)B _PM(s)

F in formula _pMpermanent magnet magnetic force suffered by capsule endoscope, s is the position coordinates of capsule endoscope, and M is the intensity of magnetization of the embedded permanent magnet of capsule, and ▽ represents Laplace operator, B _pMs magnetic induction that () is magnetic field of permanent magnet, its spatial distribution changes with s, after permanent magnet configuration and size are determined, B _pMs () is also determined thereupon, now F ₁it is the functional relationship determined;

2) position coordinates of coil magnetic force and capsule endoscope suffered by the capsule endoscope described in, and the alive relation of coil is provided by following formula:

f _coil＝I·F ₂(s)＝I·(M·▽)B _coil(s)

F in formula _coilcoil magnetic force suffered by capsule endoscope, s is the position coordinates of capsule endoscope, and I is coil institute galvanization, and M is the intensity of magnetization of the embedded permanent magnet of capsule, and ▽ represents Laplace operator, B _coils magnetic induction that () is coil magnetic field, its spatial distribution changes with s, after coil configuration and size are determined, B _coils () is also determined thereupon, now F ₂be the functional relationship determined, the magnetic force size that representation unit electric current lower coil produces;

3) the stress balance relation in a liquid of the capsule endoscope described in, comprises the balance of magnetic force, gravity, buoyancy, is provided by following formula:

F ₁(s)+I ₁f ₂(s)=mg-f _floating

In formula, s is the position coordinates of capsule endoscope, I ₁represent feedforward electric current, m represents capsule endoscope quality, and g represents acceleration of gravity, f _floatingrepresent buoyancy during the complete submergence of capsule endoscope.

Current capsule endoscope position is obtained by sensor, F ₁and F ₂for the function determined, mg-f _floatingfor known constant, therefore according to above formula can calculate capsule endoscope in a liquid diverse location static suspension time, the stressed size of current reached needed for balance, this current signal I ₁be feedforward output signal.

Described feedback control link, for realizing closed loop control, reduces concussion and the unstability of system.Using the position deviation of current control period as input, calculate feedback control compensation amount, the error reducing system is regulated to output.

1) position deviation described in, subtracted each other by the positional information of capsule endoscope current control period and desired locations and obtain:

Δs _k＝s _k-s _f

Δ s in formula _kfor capsule endoscope position deviation, s _kfor the position of capsule endoscope current control period, s _ffor capsule endoscope desired locations, namely wish the position that capsule endoscope suspends;

2) the feedback control compensation amount described in, comprises three parts: proportional component, integral element, differentiation element.

Described proportional component, is directly proportional to current control period capsule endoscope position deviation:

I _p＝k _p·Δs _k

Described integral element, is directly proportional to the accumulative capsule endoscope position deviation before current control period:

$I_1=k_1\·\underset{n=1}{\overset{k}{\Σ}}{\mathrm{\Δs}}_n$

Described differentiation element, is directly proportional to the difference of the capsule endoscope position deviation of current control period and a upper control cycle:

I _D＝k _D·(Δs _k-Δs _k-1)

Feedback control compensation amount is the summation of above-mentioned three parts, i.e. feedback control output signal current signal I ₂for:

I ₂＝I _p+I ₁+I _D

Described state estimations device link, for eliminating or reduce the unstability that system purely retarded causes.Because feedback signal needs to carry out digital processing, will inevitably bring larger purely retarded, the simple general feedback control rule that adopts there will be instability, must compensate purely retarded.

In this control cycle, control that above-mentioned feedforward and feedback obtain exports sum, is applied to Systems Theory model, estimates out the position of capsule endoscope after a control cycle; Again based on this positional information, through feedforward and the calculating of Feedback Control Laws, the working control obtaining this control cycle outputs signal.

The described signal being applied to Systems Theory model, the control obtained for above-mentioned feedforward and feedback exports sum:

I＝I ₁+I ₂

Described Systems Theory model, namely above-mentioned capsule endoscope is by the theoretical formula of magnetic force:

f _PM＝F ₁(s)

f _coil＝I·F ₂(s)

The described position estimating out capsule endoscope after a control cycle, its computational process comprises several step below:

1) by this and last time control cycle positional information estimate the velocity amplitude of this control cycle capsule endoscope:

$v_k=\frac{s_k-s_{k-1}}{T}$

V in formula _kthe speed of front control cycle, s _ksecondary control cycle capsule endoscope position, s _k-1cycle capsule endoscope position, T control cycle length.

2) the acceleration a of current control period is calculated by Newton's second law _k

F ₁(s _k)+I ₁f ₂(s _k)-mg+f _floating=ma _k

And think in a control cycle, a _kchange small, can be considered constant.

Finally, by capsule endoscope current location information s _k, above-mentioned estimation speed v _kwith acceleration a _k, the position s' of capsule endoscope after a control cycle can be estimated _k

${s^{\′}}_k=s_k+v_{k}T+\frac{1}{2}{a}_k{T}^2$

Estimate on the basis of position above-mentioned, through feedforward and the calculating of Feedback Control Laws, the working control obtaining this control cycle outputs signal, and its computational process comprises following step:

1) the positional information s' estimated _k, calculate the actual feedforward by following formula and output signal I' ₁:

F ₁(s' _k)+I' ₁f ₂(s' _k)=mg-f _floating

2) the positional information s' estimated _k, calculate actual feedback by following formula and control output signal I' ₂:

Δs' _k＝s' _k-s _f

I' _p＝k _p·Δs' _k

${I^{\′}}_1=k_1\·\underset{n=1}{\overset{k}{\Σ}}{{\mathrm{\Δs}}^{\′}}_n$

I' _D＝k _D·(Δs' _k-Δs' _k-1)

I' ₂＝I' _p+I' ₁+I' _D

3) actual feedforward output signal and actual feedback control output signal addition and obtain real output signal I':

I'＝I' ₁+I' ₂

3, the described scleronomic constraint to capsule endoscope optional position in human stomach internal liquid and control, realized by following steps:

Open motion controller, open sensor;

According to the capsule endoscope positional information that sensor provides, control movement mechanism with multiple degrees of freedom, directly over mobile hybrid magnetic field generator to capsule endoscope;

Move down hybrid magnetic field generator reposefully, to capsule endoscope will and not yet depart from liquid bottom and upwards float;

Open current controller, use above-mentioned closed loop control algorithm, make capsule endoscope under the combination field effect of permanent magnet and coil, stable suspersion in a liquid.

After reaching scleronomic constraint, can slow shifting magnetic field generator, now due to Feedback Constrained, capsule endoscope and magnetic field generator keep relative position constant, can follow magnetic field generator and move.

Because Multi-degree-of-freedom moving mechanism has higher kinematic accuracy, so can control capsule endoscope and arrive target position to be checked exactly, realize accurate, stable scope imaging inspection.

The present invention has following beneficial effect:

1) adopt permanent magnet, solenoid combination control, realize target region internal magnetic field intensity quick, accurately regulate and control, avoid simple permanent magnet produce magnetic field passive, have the static instability revolving character and cause; Realize capsule endoscope optional position fixed point in stomach intracavity liquid to suspend;

2) introduce position signalling feedback, in conjunction with closed loop control, realize capsule endoscope optional position in stomach intracavity liquid and suspend, solve the problems such as scope capsule endoscope positioning precision is poor, position controllability is not good;

3) control mode adopting the feedforward and feedback control to combine, solves the nonlinear problem of magnetic force change;

4) adopt Smith Predictor link, solve control loop latency issue.

Be described further below with reference to the technique effect of accompanying drawing to design of the present invention, concrete structure and generation, to understand object of the present invention, characteristic sum effect fully.

Accompanying drawing explanation

Fig. 1 is the permanent magnet scope capsule endoscope schematic diagram of a preferred embodiment of the present invention;

Fig. 2 is the combination cylinder permanent magnet schematic diagram of a preferred embodiment of the present invention;

Fig. 3 is the solenoid schematic diagram of a preferred embodiment of the present invention;

Fig. 4 is the system composition schematic diagram of a preferred embodiment of the present invention;

Fig. 5 is the closed loop control algorithm schematic diagram of a preferred embodiment of the present invention;

Fig. 6 is the control flow schematic diagram of a preferred embodiment of the present invention.

Detailed description of the invention

Below in conjunction with the drawings and specific embodiments, explaination is in detail done further to the present invention, but the present invention is not limited to following examples.

A kind of capsule endoscope motion control method based on change of magnetic field strength of the present invention, its thinking is: permanent magnet and solenoid are combined, permanent magnet forms stronger basic magnetic field, coil forms the adjustable auxiliary magnetic field of intensity by applying variable current, and both are combined to form the adjustable magnetic field of intensity.According to sensor feedback signal, by certain algorithm controls coil current, realize the scleronomic constraint of capsule endoscope in body fluid.

This invention in use, first person under inspection must empty digestive tract, and take in enough clear body fluid, then magnetic capsule scope endoscope is swallowed, when capsule endoscope arrives stomach, according to the step of Fig. 6, the fixed point using this control method to realize capsule endoscope optional position in gastric liquid suspends.Then by magnetic capsule scope stable motion in the motion guidance body of external hybrid magnetic field generator, thus initiative control when capsule endoscope checks in human body can be solved.

Control method of the present invention, based on the control system of complete set, adopts the closed loop control algorithm of complete set, realizes the scleronomic constraint of device outside to capsule endoscope optional position in human stomach internal liquid.

1, described complete control system, comprises permanent magnet capsule endoscope, hybrid magnetic field generator, sensor, movement mechanism with multiple degrees of freedom, controller.

Described permanent magnet capsule endoscope, as shown in Figure 1, i.e. the capsule endoscope of built-in permanent magnet material, function is identical with general capsule endoscope, but itself comprises a little permanent magnet.Permanent magnet capsule endoscope is the controlled device guided.

Described hybrid magnetic field generator, comprises a strong permanent magnets and a solenoid, and permanent magnet forms stronger basic magnetic field, and coil forms the adjustable auxiliary magnetic field of intensity by applying variable current, and both combinations, form the magnetic field that intensity is adjustable.

In the present embodiment, coil is directly over capsule endoscope, and permanent magnet is directly over coil, and coil lower surface is fixed as 145mm to the distance of permanent magnet lower surface.

Described permanent magnet as shown in Figure 2, adopts five pieces of cylinder permanent magnet splits, is one piece of diameter 120mm, high 40mm respectively; Two pieces of diameter 100mm, high 25mm; Two pieces of diameter 50mm, the permanent magnet of high 15mm; Material is neodymium iron boron N35, remanent magnetism Br=1.21T, magnetization M=963kA/m.

Described solenoid as shown in Figure 3, adopts the copper enamel-covered wire of diameter 1mm to be wound around, coil outer diameter 100mm, internal diameter 20mm, height 30mm, the number of turn about 1200 circle.

Described sensor, adopts the x-ray imaging machine of Nanjing medical equipment Co., Ltd of electronics group of China, model DG3310B1.Equipment comprises X-ray generator and receives imaging device, can carry out X-ray and irradiate imaging, thus can identify capsule endoscope in the vertical direction position in real time and accurately in inspection at stomach.Owing to being local irradiation among a small circle, and the persistent period is short, and suffered by examiner, exposure dose is in safety criterion.

Described movement mechanism with multiple degrees of freedom, is used for driving hybrid magnetic field generator in space motion, adjusts its position and attitude, thus changes the spatial distribution in magnetic field, and then realizes the motor control to capsule endoscope.Described five degree of freedom motion comprises the major part such as movable base, large arm, forearm, vertically travel mechanism, vertical axes rotating mechanism, horizontal rotational shaft mechanism.

Described movable base is the pedestal of whole device.There is wheel can facilitate the mobile transport of whole device below.Operationally wheel departs from ground, is landed, makes pedestal stably be fixed on ground, to obtain higher positioning precision by support foot.Controller can be settled in base interior space, installs five degree of freedom motion above pedestal.

Described large arm is connected by horizontally rotating joint with between pedestal, is called the first joint; Also being connected by horizontally rotating joint between forearm with large arm, being called second joint.The motion in these two joints can realize the motion in the horizontal direction of described magnetic field generator.

Described vertical travel mechanism is the 3rd joint, is a straight line slide unit, is arranged on forearm end, for realizing the motion of described magnetic field generator at vertical direction.

Described vertical axes rotating mechanism is the 4th joint, for realizing the rotary motion of described magnetic field generator around vertical axis.

Described horizontal rotational shaft mechanism is the 5th joint, and for realizing the rotary motion of described magnetic field generator around horizontal axis, horizontal axis of rotation and vertical-rotation-axis intersect vertically.

Described magnetic field generator is installed in horizontal rotational shaft mechanism, and install and ensure that the centre of sphere of its spherical permanent magnet is in horizontal axis of rotation and vertical-rotation-axis intersection point, the magnetic pole direction of magnetization is orthogonal with horizontal axis orthogonal.

The first to the three joint interlock can determine the position (spherical permanent magnet center) of magnetic field generator completely, 4th joint and the interlock of the 5th joint can determine magnetic field generator attitude (sensing of field axis), thus realize the control to magnetic field space distribution.Determine that position and the attitude of an object need 6 degree of freedom completely, but due to described magnetic field generator be rotational symmetric, around its axis of symmetry (field axis) rotate, Distribution of Magnetic Field is constant, so reality at least needs 5DOF to move.From effect, it is not controlled that capsule endoscope rotates around own longitudinal axes, the degree of freedom that other can be controlled just in time 5.

Described controller comprises motion controller and current controller, motion controller realizes conventional Multi-axis motion control, described multi-axis motion control card is connected with industrial computer, be connected with AC servo driver down, accept the motion planning information from industrial computer, complete interpolation operation according to certain control algolithm, generate concrete drive singal and be sent to each AC servo driver, control the motor movement in each joint to complete planning action.

Described AC servo driver is AC servo motor auxiliary products, for the driving of AC servo motor.It is connected with multi-axis motion control card, is connected with AC servo motor down.Accept the control signal from multi-axis motion control card, according to the motion of control signal drive motors.

Described D/A converter and current amplifier in order to control coil size of current, thus regulate the magnetic induction in the magnetic field that magnetic field generator produces.

Hybrid magnetic field generator proposed by the invention, adopts the mode that powerful permanent ferrum superposes with solenoid.Permanent magnet more than surface induction intensity 1.2T, can reach 20cm to the attraction distance of capsule endoscope, can realize the permanent magnet capsule endoscope motion controlled in vitro in human body.Solenoid then can realize the fine adjustments to magnetic induction by the change controlling size of current direction, because the signal of telecommunication is more faster than mechanical movement, therefore than causing changes of magnetic field faster by magnet movement merely, also more accurate.

The feedback control algorithm control coil electric current that current controller proposes according to the present invention, thus controlling magnetic field, implement scleronomic constraint to capsule endoscope, comprise industrial computer, data collecting card, current amplifier.

Described industrial computer is used for receiving sensor initial data, carries out computing, signal is exported to data collecting card according to described algorithm.

Described data collecting card, for carrying out A/D conversion, being converted into analogue signal by industrial computer output signal, adopting the USB6211 type capture card of National Instruments, output area ± 10V, precision 16bits.

Described current amplifier is used for the analogue signal geometric ratio that exported by above-mentioned capture card and amplifies, and to drive solenoid, adopts Quan Zhoudong Electron Technology Co., Ltd's model of speeding to be the product of ET-SZL1-D24-3A-V, output current 0 ~ 3A.

2, described complete closed loop control algorithm, comprises sensor signal processing links, feedforward link, feedback control link and state estimations device link.Its control flow as shown in Figure 5.In the present embodiment, control cycle 50ms.

Described sensor signal processing links, for obtaining the positional information of capsule endoscope in the primary signal that returns from sensor, industrial computer carries out digital processing and calculating to the primary signal that the sensor returns, and obtains the position of capsule endoscope relative to magnetic field generator.

Described feedforward link, theoretical output signals value when calculating steady statue according to theoretical model, based on output signal to reduce mission nonlinear impact.

First according to Systems Theory model, permanent magnet suffered by capsule endoscope, coil magnetic force and capsule endoscope position is obtained, and the relation of coil current size; Then obtain according to capsule endoscope stress balance relation in a liquid the output current signal that feedovers.Concrete steps are as follows:

1) described capsule endoscope suffered permanent magnet magnetic force size immediately below permanent magnet axis, is provided by following formula with the relation of capsule endoscope position coordinates:

$f_{PM}=F_1\left(s\right)=\frac{\mathrm{\π}M(D^2-d^2)}{4}\[B_{PM}(s+\frac{H}{2})-B_{PM}(s-\frac{H}{2})\]$

Wherein, s is small magnet position coordinates in capsule endoscope; M is the polarization intensity of small magnet in capsule endoscope; D, D, H are internal-and external diameter and the height of small magnet in capsule endoscope; B _pMz (), under the vertical state of permanent magnet, on the axis of below, coordinate position is the magnetic induction that z place produces, and its expression formula is:

$B_{PM}\left(z\right)=Br\underset{i=1}{\overset{5}{\Σ}}(\frac{l_{ti}}{\sqrt{4l_{ti}^2+D_i^2}}+\frac{l_{bi}}{\sqrt{4l_{bi}^2+D_i^2}})$

Wherein:

l _ti＝|z-s _pi|+L _i/2

l _bi＝|z-s _pi|-L _i/2

Because described permanent magnet is made up of five pieces of cylinder permanent magnets, therefore permanent magnet generation total magnetic field is formed by stacking by five components.Wherein, Br is the remanent magnetism of above-mentioned permanent magnet; s _pibe the center position coordinates of i-th piece of permanent magnet, owing to interfixing between cylinder permanent magnet, and permanent magnet vertical position fixed, so s in operation _pifor fixing constant; L _iand D _ibe respectively height and the diameter of above-mentioned i-th piece of cylinder permanent magnet; l _tifor the distance of z position and i-th piece of cylinder permanent magnet upper surface; l _bifor the distance of z position and i-th piece of cylinder permanent magnet lower surface.Function F can be determined by above formula ₁s data can be calculated to be look-up table to reduce operand in actual use by the expression formula of () in advance.

2) capsule endoscope described in is suffered coil magnetic force size immediately below solenoid axis, and with capsule endoscope position coordinates, and the alive relation of coil is provided by following formula:

$f_{coil}=I\·F_2\left(s\right)=\frac{\mathrm{\π}M(D^2-d^2)}{4}\[B_{coil}(s+\frac{H}{2})-B_{coil}(s-\frac{H}{2})\]$

Wherein, s is small magnet position coordinates in capsule endoscope; I is coil institute galvanization; M is the polarization intensity of small magnet in capsule endoscope; D, D, H are internal-and external diameter and the height of small magnet in capsule endoscope; B _coilz (), under the vertical state of solenoid, on the axis of below, coordinate position is the magnetic induction that z place produces, and its expression formula is:

$B_{coil}\left(z\right)=\frac{1}{2}{\mathrm{\μ}}_{0}I\underset{i=1}{\overset{L_0/\mathrm{\φ}}{\Σ}}\left(\underset{j=1}{\overset{\frac{(D_0-d_0)}{\left(2\mathrm{\φ}\right)}}{\Σ}}\frac{r_j^2}{{(r_j^2+l_i^2)}^{3/2}}\right)$

Wherein:

r _j＝d ₀/2+φ·j

l _i＝|z-s ₀|-L ₀/2+φ·i

Because coil is entwined by some circle enamel-covered wires, it is that the magnetic field superposition that every circle coil produces forms that coil produces magnetic field.Wherein μ ₀for permeability of vacuum; L ₀, d ₀and D ₀be respectively coil height and internal-and external diameter; φ is enamel-covered wire diameter; s ₀for the position coordinates of solenoid, owing to coil vertical position being fixed, so s in operation ₀for fixing constant; r _jfor the radius of coil jth circle from inside to outside; l _ifor the distance of coil the i-th circle and z position from the bottom up.F can be determined by above formula ₂s data can be calculated to be look-up table to reduce operand in actual use by the expression formula of () in advance.

3) the stress balance relation in a liquid of the capsule endoscope described in, comprises the balance of magnetic force, gravity, buoyancy, is provided by following formula:

F ₁(s)+I ₁f ₂(s)=mg-f _floating

In formula, s is the position coordinates of capsule endoscope, I ₁represent feedforward electric current, m represents capsule endoscope quality, and g represents acceleration of gravity, f _floatingrepresent buoyancy during the complete submergence of capsule endoscope.Current capsule endoscope position is obtained by sensor, F ₁and F ₂for the function determined, mg-f _floatingfor known constant, therefore can calculate capsule endoscope under body fluid during diverse location static suspension according to above formula, the stressed size of current reached needed for balance, this current signal I ₁be feedforward output signal.

Described feedback control link, for realizing closed loop control, reduces concussion and the unstability of system.Using the position deviation of current control period as input, calculate feedback control compensation amount, the error reducing system is regulated to output.

1) position deviation described in, subtracted each other by the positional information of capsule endoscope current control period and desired locations and obtain:

Δs _k＝s _k-s _f

Δ s in formula _kfor capsule endoscope position deviation, s _kfor the position of capsule endoscope current control period, s _ffor capsule endoscope desired locations, namely wish the position that capsule endoscope suspends;

2) the feedback control compensation amount described in, comprises three parts: proportional component, integral element, differentiation element.

Described proportional component, is directly proportional to current control period capsule endoscope position deviation:

I _p＝k _p·Δs _k

Wherein, k _p=25.

Described integral element, is directly proportional to the accumulative capsule endoscope position deviation before current control period:

$I_1=k_1\·\underset{n=1}{\overset{k}{\Σ}}{\mathrm{\Δs}}_n$

Wherein, k ₁=1.

Described differentiation element, is directly proportional to the capsule endoscope position deviation of current control period and a upper control cycle is only poor:

I _D＝k _D·(Δs _k-Δs _k-1)

Wherein, k _d=120.

Feedback control compensation amount is expressed as the summation of above-mentioned three parts, i.e. feedback control output signal current signal I ₂for:

I ₂＝I _p+I ₁+I _D

Described state estimations device link, for eliminating or reduce the unstability that system purely retarded causes.Because feedback signal needs to carry out digital processing, will inevitably bring larger purely retarded, the simple general feedback control rule that adopts there will be instability, must compensate purely retarded.

In this control cycle, control that above-mentioned feedforward and feedback obtain exports sum, is applied to Systems Theory model, estimates out the position of capsule endoscope after a control cycle; Again based on this positional information, through feedforward and the calculating of Feedback Control Laws, the working control obtaining this control cycle outputs signal.

The described signal being applied to Systems Theory model, the control obtained for above-mentioned feedforward and feedback exports sum:

I＝I ₁+I ₂

Described Systems Theory model, namely above-mentioned capsule endoscope is by the theoretical formula of magnetic force:

f _PM＝F ₁(s)

f _coil＝I·F ₂(s)

The described position estimating out capsule endoscope after a control cycle, its computational process comprises several step below:

1) first by this and last time control cycle positional information estimate the velocity amplitude of this control cycle capsule endoscope:

$v_k=\frac{s_k-s_{k-1}}{T}$

V in formula _kfor the speed of current control period, s _kfor this control cycle capsule endoscope position, s _k-1for control cycle capsule endoscope last time position, T is control cycle length.

2) the acceleration a of current control period is then calculated by Newton's second law _k:

F ₁(s _k)+I ₁f ₂(s _k)-mg+f _floating=ma _k

And think in a control cycle, a _kchange small, can be considered constant.

3) last, by capsule endoscope current location information s _k, above-mentioned estimation speed v _kwith acceleration a _k, the position s' of capsule endoscope after a control cycle can be estimated _k:

${s^{\′}}_k=s_k+v_{k}T+\frac{1}{2}{a}_k{T}^2$

Estimate on the basis of position above-mentioned, through feedforward and the calculating of Feedback Control Laws, the working control obtaining this control cycle outputs signal, and its computational process comprises following step:

1) based on the positional information s' estimated _k, calculate the actual feedforward by following formula and output signal I' ₁:

F ₁(s' _k)+I' ₁f ₂(s' _k)=mg-f _floating

2) based on the positional information s' estimated _k, calculate actual feedback by following formula and control output signal I' ₂:

Δs' _k＝s' _k-s _f

I' _p＝k _p·Δs' _k

${I^{\′}}_1=k_1\·\underset{n=1}{\overset{k}{\Σ}}{{\mathrm{\Δs}}^{\′}}_n$

I' _D＝k _D·(Δs' _k-Δs' _k-1)

I' ₂＝I' _p+I' ₁+I' _D

3) actual feedforward output signal and actual feedback are controlled output signal addition and obtain real output signal I':

I'＝I' ₁+I' ₂

3, described to the capsule endoscope scleronomic constraint of optional position and control in liquid in human stomach, realized by following steps:

Open motion controller, open sensor;

According to the capsule endoscope positional information that sensor provides, control movement mechanism with multiple degrees of freedom, directly over mobile hybrid magnetic field generator to capsule endoscope;

Move down hybrid magnetic field generator reposefully, to capsule endoscope will and not yet depart from liquid bottom and upwards float;

Open current controller, use above-mentioned closed loop control algorithm, make capsule endoscope under the combination field effect of permanent magnet and coil, stable suspersion in a liquid.

4, after reaching scleronomic constraint, can slow shifting magnetic field generator, now due to Feedback Constrained, capsule endoscope and magnetic field generator keep relative position constant, can follow magnetic field generator and move.Because Multi-degree-of-freedom moving mechanism has higher kinematic accuracy, so can control capsule endoscope and arrive target position to be checked exactly, realize accurate, stable scope imaging inspection.

More than describe preferred embodiment of the present invention in detail.Should be appreciated that the ordinary skill of this area just design according to the present invention can make many modifications and variations without the need to creative work.Therefore, all technical staff in the art, all should by the determined protection domain of claims under this invention's idea on the basis of existing technology by the available technical scheme of logical analysis, reasoning, or a limited experiment.

Claims (10)

1., based on a capsule endoscope motion control method for change of magnetic field strength, it is characterized in that, comprise the following steps:

Capsule endoscope, permanent magnet, solenoid, sensor are provided;

Described permanent magnet is configured to form stronger basic magnetic field; Described solenoid is configured to apply variable current and forms the adjustable auxiliary magnetic field of intensity; Described basic magnetic field and described auxiliary magnetic field are combined to form the adjustable mixed magnetic field of intensity;

Described solenoid is configured to the signal according to described sensor feedback, utilizes closed loop feedback control algolithm to adjust the size of electric current, realizes the scleronomic constraint of described capsule endoscope in body fluid;

The motion of described permanent magnet and described solenoid is configured to guide the stable motion in body fluid of described capsule endoscope.

2. as claimed in claim 1 based on the capsule endoscope motion control method of change of magnetic field strength, it is characterized in that, described sensor is configured to detect the distance change of capsule endoscope relative to the vertical direction of described permanent magnet and described solenoid.

3. as claimed in claim 1 based on the capsule endoscope motion control method of change of magnetic field strength, it is characterized in that, described sensor is the one in the embedded acceierometer sensor of X-ray imaging device, B ultrasonic imaging device, MRI imaging device, RFID device, ultrasonic sensor or described capsule endoscope.

4., as claimed in claim 1 based on the capsule endoscope motion control method of change of magnetic field strength, it is characterized in that, described closed loop feedback control algolithm comprises sensor signal processing links, feedforward link, feedback control link and state estimations device link.

5. as claimed in claim 4 based on the capsule endoscope motion control method of change of magnetic field strength, it is characterized in that, described sensor signal processing links, for obtaining the positional information of capsule endoscope in the primary signal that returns from described sensor, comprises the following step:

Processor carries out digital processing to the primary signal that described sensor returns;

Obtain the position of capsule endoscope relative to magnetic field generator;

Described position as feedback signal transmission to controller;

Described controller carries out closed loop control to the position of capsule endoscope.

6., as claimed in claim 4 based on the capsule endoscope motion control method of change of magnetic field strength, it is characterized in that, described feedforward link, comprises the following step:

1) suffered by described capsule endoscope, the relation of the position coordinates of permanent magnet magnetic force and capsule endoscope is provided by following formula:

$f_{PM}=F_1\left(s\right)=(M\·\▿)B_{PM}\left(s\right)$

F in formula _pMpermanent magnet magnetic force suffered by capsule endoscope, s is the position coordinates of capsule endoscope, and M is the intensity of magnetization of the embedded permanent magnet of capsule, represent Laplace operator, B _pMs magnetic induction that () is magnetic field of permanent magnet, its spatial distribution changes with s;

2) position coordinates of coil magnetic force and capsule endoscope suffered by the capsule endoscope described in, and the alive relation of coil is provided by following formula:

$f_{coil}=I\·F_2\left(s\right)=I\·(M\·\▿)B_{coil}\left(s\right)$

F in formula _coilcoil magnetic force suffered by capsule endoscope, s is the position coordinates of capsule endoscope, and I is coil institute galvanization, and M is the intensity of magnetization of the embedded permanent magnet of capsule, represent Laplace operator, B _coils magnetic induction that () is coil magnetic field, its spatial distribution changes with s;

3) the stress balance relation in a liquid of the capsule endoscope described in, comprises the balance of magnetic force, gravity, buoyancy, is provided by following formula:

F ₁(s)+I ₁f ₂(s)=mg-f _floating

In formula, s is the position coordinates of capsule endoscope, I ₁represent feedforward electric current, m represents capsule endoscope quality, and g represents acceleration of gravity, f _floatingrepresent buoyancy during the complete submergence of capsule endoscope;

4) calculate capsule endoscope in a liquid diverse location static suspension time, the stressed size of current reached needed for balance, this current signal I ₁be feedforward output signal.

7. as claimed in claim 4 based on the capsule endoscope motion control method of change of magnetic field strength, it is characterized in that, described feedback control link is configured to for realizing closed loop control, reduces concussion and the unstability of system; The position deviation of current control period as input, is calculated feedback control compensation amount by described feedback control link, and regulate to output the error reducing system, concrete steps are as follows:

1) position deviation described in, subtracted each other by the positional information of capsule endoscope current control period and desired locations and obtain:

Δs _k＝s _k-s _f

Δ s in formula _kfor capsule endoscope position deviation, s _kfor the position of capsule endoscope current control period, s _ffor capsule endoscope desired locations;

2) the feedback control compensation amount described in comprises three parts: proportional component, integral element, differentiation element;

Described proportional component, is directly proportional to current control period capsule endoscope position deviation:

I _p＝k _p·Δs _k

Described integral element, is directly proportional to the accumulative capsule endoscope position deviation before current control period:

$I_1=k_1\·\underset{n=1}{\overset{k}{\Σ}}{\mathrm{\Δs}}_n$

Described differentiation element, is directly proportional to the difference of the capsule endoscope position deviation of current control period and a upper control cycle:

I _D＝k _D·(Δs _k-Δs _k-1)

Feedback control compensation amount is the summation of above-mentioned three parts, i.e. feedback control output signal current signal I ₂for:

I ₂＝I _p+I ₁+I _D。

8., as claimed in claim 4 based on the capsule endoscope motion control method of change of magnetic field strength, it is characterized in that, described state estimations device link is configured to the unstability caused for eliminating or reduce system purely retarded;

Described state estimations device link is in this control cycle, and control that above-mentioned feedforward and feedback obtain exports sum, is applied to Systems Theory model, estimates out the position of capsule endoscope after a control cycle; Again based on this positional information, through feedforward and the calculating of Feedback Control Laws, the working control obtaining this control cycle outputs signal;

The described signal being applied to Systems Theory model, the control obtained for above-mentioned feedforward and feedback exports sum:

I＝I ₁+I ₂

Described Systems Theory model, namely above-mentioned capsule endoscope is by the theoretical formula of magnetic force:

f _PM＝F ₁(s)

f _coil＝I·F ₂(s)

The described position estimating out capsule endoscope after a control cycle, its computational process comprises several step below:

1) by this and last time control cycle positional information estimate the velocity amplitude of this control cycle capsule endoscope:

$v_k=\frac{s_k-s_{k-1}}{T}$

V in formula _kfor the speed of current control period, s _kfor this control cycle capsule endoscope position, s _k-1for control cycle capsule endoscope last time position, T is control cycle length;

2) the acceleration a of current control period is calculated by Newton's second law _k

F ₁(s _k)+I ₁f ₂(s _k)-mg+f _floating=ma _k

And think in a control cycle, a _kchange small, can be considered constant;

Finally, by capsule endoscope current location information s _k, above-mentioned estimation speed v _kwith acceleration a _k, the position s' of capsule endoscope after a control cycle can be estimated _k

${s^{\′}}_k=s_k+v_{k}T+\frac{1}{2}{a}_k{T}^2$

Estimate on the basis of position above-mentioned, through feedforward and the calculating of Feedback Control Laws, the working control obtaining this control cycle outputs signal, and its computational process comprises following step:

1) the positional information s' estimated _k, calculate the actual feedforward by following formula and output signal I' ₁:

F ₁(s' _k)+I' ₁f ₂(s' _k)=mg-f _floating

2) the positional information s' estimated _k, calculate actual feedback by following formula and control output signal I' ₂:

Δs' _k＝s' _k-s _f

I' _p＝k _p·Δs' _k

${I^{\′}}_1=k_1\·\underset{n=1}{\overset{k}{\Σ}}{{\mathrm{\Δs}}^{\′}}_n$

I' _D＝k _D·(Δs' _k-Δs' _k-1)

I' ₂＝I' _p+I' ₁+I' _D

3) actual feedforward output signal and actual feedback control output signal addition and obtain real output signal I':

I'＝I' ₁+I' ₂。

9., as claimed in claim 1 based on the capsule endoscope motion control method of change of magnetic field strength, it is characterized in that, the described scleronomic constraint to capsule endoscope optional position in body fluid and control, realized by following steps:

Open motion controller, open sensor;

According to the capsule endoscope positional information that sensor provides, control movement mechanism with multiple degrees of freedom, directly over mobile hybrid magnetic field generator to capsule endoscope;

Move down hybrid magnetic field generator reposefully, to capsule endoscope will and not yet depart from liquid bottom and upwards float;

Open current controller, use above-mentioned closed loop control algorithm, make capsule endoscope under the combination field effect of permanent magnet and coil, stable suspersion in a liquid;

After reaching scleronomic constraint, can slow shifting magnetic field generator, now due to Feedback Constrained, capsule endoscope and magnetic field generator keep relative position constant, can follow magnetic field generator and move.

10., as claimed in claim 1 based on the capsule endoscope motion control method of change of magnetic field strength, it is characterized in that, described permanent magnet surfaces magnetic induction is more than 1.2T.