CN112741586B - Position acquisition method based on capsule endoscopy human body internal position acquisition system - Google Patents

Position acquisition method based on capsule endoscopy human body internal position acquisition system Download PDF

Info

Publication number
CN112741586B
CN112741586B CN202011539785.1A CN202011539785A CN112741586B CN 112741586 B CN112741586 B CN 112741586B CN 202011539785 A CN202011539785 A CN 202011539785A CN 112741586 B CN112741586 B CN 112741586B
Authority
CN
China
Prior art keywords
axis
magnetic field
field signal
magnetic
sampling period
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011539785.1A
Other languages
Chinese (zh)
Other versions
CN112741586A (en
Inventor
张鹏
陈锐志
董卫国
田山
董明玥
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuhan University WHU
Original Assignee
Wuhan University WHU
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Wuhan University WHU filed Critical Wuhan University WHU
Priority to CN202011539785.1A priority Critical patent/CN112741586B/en
Publication of CN112741586A publication Critical patent/CN112741586A/en
Application granted granted Critical
Publication of CN112741586B publication Critical patent/CN112741586B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B1/00—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor
    • A61B1/04—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor combined with photographic or television appliances
    • A61B1/041—Capsule endoscopes for imaging
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B1/00—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor
    • A61B1/00002—Operational features of endoscopes
    • A61B1/00004—Operational features of endoscopes characterised by electronic signal processing
    • 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/00002—Operational features of endoscopes
    • A61B1/00011—Operational features of endoscopes characterised by signal transmission
    • 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/00002—Operational features of endoscopes
    • A61B1/00011—Operational features of endoscopes characterised by signal transmission
    • A61B1/00016—Operational features of endoscopes characterised by signal transmission using wireless means
    • 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/00147—Holding or positioning arrangements
    • 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/00147—Holding or positioning arrangements
    • A61B1/00158—Holding or positioning arrangements using magnetic field

Abstract

The invention provides a position acquisition method based on a capsule endoscopy in-human body position acquisition system. The magnet is arranged in the capsule endoscope and emits a magnetic field signal outwards, and the magnetic field signal intensity is collected by the plurality of magnetic sensors of the vest type three-axis magnetic sensor array plate and is transmitted to the second wireless processor for data processing; the accelerometer and the gyroscope respectively acquire acceleration and angular velocity information of the capsule endoscope and transmit the acceleration and the angular velocity information to the first wireless processor, the first wireless processor obtains attitude information through calculation according to the acceleration and the angular velocity, and the first wireless processor wirelessly transmits the attitude information to the second wireless processor, so that attitude parameters are determined, and only position parameters are estimated. The patent provides a nonlinear least square joint adjustment method based on each axial observed value of the magnetometer according to the system, so that the calculation efficiency is improved, and the occurrence of gross errors is reduced, thereby meeting the requirement of real-time calculation of a capsule endoscope positioning system.

Description

Position acquisition method based on capsule endoscopy human body internal position acquisition system
Technical Field
The invention belongs to the field of medical engineering, and particularly relates to a position acquisition method based on a capsule endoscopy in-vivo position acquisition system.
Background
Since the emergence of wireless capsule endoscopes in israel in 2001, the application of the wireless capsule endoscopes in clinical diagnosis greatly makes up the defects that wired endoscopes such as gastroscope enteroscopes are limited in visual field and poor in experience of patients. And the wireless capsule endoscope can diagnose and treat gastrointestinal tract diseases without causing discomfort of patients and influencing normal work and life of the patients. In order to better develop the clinical application of the wireless capsule endoscope and assist doctors in diagnosing and treating gastrointestinal diseases, a problem needs to be solved urgently: and acquiring the position information of the wireless capsule endoscope in the body. So as to help doctors to determine the exact position of the focus in the human body, improve the diagnosis efficiency and reduce the pain of patients. Therefore, a positioning system with high precision, simple and convenient operation and convenient carrying is configured for the wireless capsule endoscope, and becomes an important task in the development process of the wireless capsule endoscope. Therefore, the positioning technology of the wireless capsule endoscope becomes the research focus in the field at present
In the development time of the last two decades from 2000 to the present, a plurality of researchers utilize different sensors and different positioning technologies to carry out deep research on the track and accurate positioning of the wireless capsule endoscope in the human body. The main positioning modes of the existing wireless capsule endoscope are as follows: radio frequency signals (RF), electromagnetic localization (permanent magnets built into the capsule), inertial navigation localization, visual localization, multi-sensor fusion localization, and localization using CT, MRF, ultrasound, and other auxiliary devices. Among many positioning modes, magnetic field has become a research hotspot as a technical means which is harmless to human body and has the advantages that static low-frequency magnetic signals can pass through human tissue without any attenuation.
At present, the research on the magnetic positioning of the capsule endoscope is carried out under the laboratory environment based on the surrounding structural design, and the capsule endoscope can not be positioned completely under the laboratory environment because the stay time of the capsule endoscope in the human body is more than 7 hours. Meanwhile, the positioning efficiency of the current high-precision positioning algorithm is low, and the real-time positioning requirement cannot be met. There is therefore a need for a wearable capsule-endoscope positioning system that determines the position of a capsule-endoscope within the body of a person without affecting the normal activities of the patient.
Disclosure of Invention
The invention provides a position acquisition method based on a capsule endoscope in-vivo position acquisition system, which is characterized in that a vest type magnetic sensor array positioning device is worn on a human body, and meanwhile, the problem of acquiring the position of a capsule endoscope in the human body in real time is solved according to the position acquisition method.
Position acquisition system in capsule scope human body, its characterized in that includes:
the system comprises a capsule endoscope, a magnet, an accelerometer, a gyroscope, a first wireless processor, a plurality of three-axis magnetic sensors, a vest, a second wireless processor, a memory and an application end;
the accelerometer is connected with the first wireless processor in a wired mode; the gyroscope is connected with the first wireless processor in a wired mode; the first wireless processor is wirelessly connected with the second wireless processor; the second wireless processor is connected with the plurality of three-axis magnetic sensors in sequence in a wired mode; the second wireless processor is connected with the memory in a wired mode; the second wireless processor is connected with the application end in a wireless mode;
the plurality of three-axis magnetic sensors, the second wireless processor and the memory form a magnetic sensor array plate and are arranged in the back core;
the vest is worn on a human body;
the accelerometer, the gyroscope and the first wireless processor are all arranged in the capsule endoscope;
the magnet is a permanent magnet or an electromagnet, is arranged in the capsule endoscope and emits a magnetic field outwards;
the accelerometer is used for acquiring acceleration of the capsule endoscope and transmitting the acceleration to the first wireless processor, the gyroscope is used for acquiring angular velocity of the capsule endoscope and transmitting the angular velocity of the capsule endoscope to the first wireless processor, the first wireless processor obtains attitude information according to the acceleration and the angular velocity through a mechanical arrangement algorithm using inertial navigation, and further the attitude information is wirelessly transmitted to the second wireless processor through the first wireless processor;
the plurality of three-axis magnetic sensors sequentially acquire the strength of magnetic field signals and transmit the strength of the magnetic field signals to the second wireless processor;
the second wireless processor is used for carrying out positioning calculation according to the magnetic field signal intensity and the attitude information acquired by the plurality of three-axis magnetic sensors, calculating the position information of the magnet and wirelessly transmitting the position information to the application end;
the memory is used for storing a coordinate database of a plurality of magnetic sensors and magnet receiving position information;
and the application end is used for receiving the magnet position information from the microprocessor.
The database comprises an observation value database, a sensor coordinate database and sensor position data;
the observation value database comprises identification codes of all the three-axis magnetic sensors and the corresponding observed magnetic field signal intensity, and is stored in the second wireless processor.
The sensor coordinate database comprises a capsule endoscope identification code, a position, a posture and time information corresponding to the capsule endoscope position, and the time information is stored in the second wireless processor.
The sensor position database is used for storing the positions of the magnetic sensors of all three axes, storing the positions in the second wireless processor and sending the positions to the sensor position database.
The position acquisition method comprises the following steps:
step 1: the magnet emits magnetic field signals, the magnetic field signal intensities are sequentially collected by the plurality of magnetic sensors and transmitted to the second wireless processor, the magnetic field signal intensities are compensated by combining the environmental magnetic field intensity to obtain compensated magnetic field signal intensities, coarse difference elimination is carried out on the compensated magnetic field signal intensities according to the sampling time to obtain effective magnetic field signal intensities, and the effective magnetic field signal intensities are further subjected to smoothing processing to obtain smoothed magnetic field signal intensities;
step 2: the accelerometer is used for acquiring acceleration of the capsule endoscope and transmitting the acceleration to the first wireless processor, the gyroscope is used for acquiring angular velocity of the capsule endoscope and transmitting the angular velocity of the capsule endoscope to the first wireless processor, the first wireless processor obtains attitude information according to the acceleration and the angular velocity through a mechanical arrangement algorithm using inertial navigation, and the first wireless processor wirelessly transmits the attitude information to the second wireless processor and stores the attitude information to an observed value of the second wireless processor;
and 3, calculating a magnetic moment direction vector by combining the attitude information through the second microprocessor, sequentially constructing a first magnetic dipole model function, a second magnetic dipole model function and a third magnetic dipole model function according to the magnetic moment direction vector, and further calculating the position of the capsule endoscope by utilizing the smoothed magnetic field signal intensity, the linearized first magnetic dipole model function, the linearized second magnetic dipole model function and the linearized third magnetic dipole model function.
Preferably, the magnetic field signal strength in step 1 is:
Figure BDA0002854573000000031
i∈[1,N],t∈[1,M],k∈[1,T]
wherein N is the number of magnetic sensors, M is the number of sampling instants, T is the number of sampling periods,
Figure BDA0002854573000000032
in order to sample the magnetic field signal strength of the ith triaxial magnetic sensor at time t in the kth sampling period,
Figure BDA0002854573000000033
in order to sample the X-axis magnetic field signal strength of the ith three-axis magnetic sensor at time t in the kth sampling period,
Figure BDA0002854573000000034
in order to sample the Y-axis magnetic field signal strength of the ith three-axis magnetic sensor at time t in the kth sampling period,
Figure BDA0002854573000000035
sampling the Z-axis magnetic field signal intensity of the ith triaxial magnetic sensor at the time t in the kth sampling period;
step 1, the magnetic field signal intensity is compensated by combining the environmental magnetic field intensity to obtain the compensated magnetic field signal intensity as follows:
Figure BDA0002854573000000036
i∈[1,N],t∈[1,M],k∈[1,T]
wherein N is the number of magnetic sensors, M is the number of sampling moments, T is the number of sampling periods, and
Figure BDA0002854573000000037
in order to sample the compensated magnetic field signal strength of the ith triaxial magnetic sensor at time t in the kth sampling period,
Figure BDA0002854573000000041
in order to sample the compensated X-axis magnetic field signal strength of the ith three-axis magnetic sensor at time t in the kth sampling period,
Figure BDA0002854573000000042
in order to sample the compensated Y-axis magnetic field signal strength of the ith three-axis magnetic sensor at time t in the kth sampling period,
Figure BDA0002854573000000043
to sample the compensated Z-axis magnetic field signal strength of the ith three-axis magnetic sensor at time t in the kth sampling period, [ B ]e,x Be,y Be,z]Being the intensity of the ambient magnetic field, Be,xIs the X-axis ambient magnetic field strength, Be,yIs the Y-axis ambient magnetic field strength, Be,zIs the Z-axis ambient magnetic field strength;
step 1, performing gross error elimination on the compensated magnetic field signal intensity according to the sampling time to obtain the effective magnetic field signal intensity:
if the magnetic field signal intensity of any axis of the X, Y and Z axes at the time t in the kth sampling period meets the following condition, judging that the data at the time is a gross error:
Figure BDA0002854573000000044
Figure BDA0002854573000000045
Figure BDA0002854573000000046
wherein the content of the first and second substances,
Figure BDA0002854573000000047
the intensity of the X-axis magnetic field signal compensated for the sampling moment before the moment t in the kth sampling period,
Figure BDA0002854573000000048
the compensated intensity of the X-axis magnetic field signal at the sampling moment after the t moment in the kth sampling period,
Figure BDA0002854573000000049
the intensity of the Y-axis magnetic field signal compensated for the sampling moment before the t moment in the kth sampling period,
Figure BDA00028545730000000410
the compensated Y-axis magnetic field signal strength for the sampling time after time t,
Figure BDA00028545730000000411
the intensity of the Z-axis magnetic field signal after compensation at the sampling moment before the t moment,
Figure BDA00028545730000000412
the intensity of the Z-axis magnetic field signal compensated for the sampling time after the time t is the Th is a set threshold, and if the intensity of the magnetic field signal at the time t is judged to be coarse difference, the weight of the intensity can be reduced or the data can be discarded;
step 1, performing data smoothing processing on the effective magnetic field signal intensity according to the sampling time to obtain the smoothed magnetic field signal intensity:
Figure BDA00028545730000000413
i∈[1,N]
w1＜w2...＜wN
wherein, it is made
Figure BDA00028545730000000414
The smoothed magnetic field signal strength of the ith triaxial magnetic sensor in the kth sampling period,
Figure BDA0002854573000000051
for the k sampling periodThe smoothed X-axis magnetic field signal strength of the ith three-axis magnetic sensor during the period,
Figure BDA0002854573000000052
the smoothed Y-axis magnetic field signal strength of the ith three-axis magnetic sensor in the kth sampling period,
Figure BDA0002854573000000053
the intensity of the smoothed Z-axis magnetic field signal of the ith triaxial magnetic sensor in the kth sampling period is obtained;
preferably, the attitude information of step 2 includes: pitch angle, roll angle, course angle;
the roll angle is defined as phikShowing X in the local coordinate system of the capsule endoscope around the capsule endoscope in the kth sampling periodlThe angle of rotation of the shaft;
the pitch angle is defined as thetakRepresents Y in the local coordinate system of the capsule endoscope around the capsule endoscope in the kth sampling periodlThe angle of rotation of the shaft;
the heading angle is defined as psikShowing Z in the local coordinate system of the capsule endoscope around the capsule endoscope in the kth sampling periodlThe angle of rotation of the shaft;
the construction method of the capsule endoscope positioning local coordinate system comprises the following steps:
the first wireless processor selects any one of the three-axis magnetic sensors as a coordinate system origin, and uses a human body sagittal axis as a capsule endoscope to position an X in a local coordinate systemlThe axis and the human coronal axis are Y in a capsule endoscope positioning local coordinate systemlThe axis and the pointing direction are Z in a capsule endoscope positioning local coordinate systemlA shaft;
preferably, the step 3 of calculating the magnetic moment direction vector in combination with the attitude information is:
Figure BDA0002854573000000054
wherein, [ l0 m0 n0]TIs the initial magnetic moment vector of the magnet, l0Is the initial magnetic moment direction vector of the magnet in a local coordinate system XlComponent of the axis, m0Is the initial magnetic moment direction vector of the magnet in a local coordinate system YlComponent of the axis, n0Is the initial magnetic moment direction vector of the magnet in a local coordinate system ZlThe component of the axis is such that,
Figure BDA0002854573000000055
representing X in a local coordinate system of the capsule endoscope around the capsule endoscope for the roll angle of the kth sampling periodlThe angle of rotation of the shaft is such that,
Figure BDA0002854573000000056
representing the pitch angle of the kth sampling period as Y in a local coordinate system of the capsule endoscope around the capsule endoscopelThe angle of rotation of the shaft is such that,
Figure BDA0002854573000000061
representing Z in a local coordinate system of the capsule endoscope around the capsule endoscope for the course of the kth sampling periodlThe angle of rotation of the shaft;
step 3, constructing a first magnetic dipole model function is as follows:
Figure BDA0002854573000000062
step 3, constructing a second magnetic dipole model function as follows:
Figure BDA0002854573000000063
step 3, constructing a third magnetic dipole model function is as follows:
Figure BDA0002854573000000064
wherein, BTConstant is determined by capsule endoscope magnet，
Figure BDA0002854573000000065
For the X of the k-th sampling period capsule endoscopelThe coordinates of the axes are set to be,
Figure BDA0002854573000000066
for the kth sampling period of Y of the capsule endoscopelThe coordinates of the axes are set to be,
Figure BDA0002854573000000067
for the Z of the k-th sampling period of the capsule endoscopelAxial coordinate, xiFor the ith three-axis magnetic sensor SiX of (2)lAxial coordinate, yiFor the ith three-axis magnetic sensor SiY of (A) islAxial coordinate, ziFor the ith three-axis magnetic sensor SiZ of (A)lThe coordinates of the axes are set to be,
Figure BDA0002854573000000068
the X-axis magnetic field signal strength value of the ith three-axis magnetic sensor in the kth sampling period,
Figure BDA0002854573000000069
the Y-axis magnetic field signal intensity value of the ith three-axis magnetic sensor in the kth sampling period, the Z-axis magnetic field signal intensity value of the ith three-axis magnetic sensor in the kth sampling period,
Figure BDA00028545730000000610
for the k sampling period, the magnetic moment direction vector is in the local coordinate system XlThe component of the axis is such that,
Figure BDA00028545730000000611
for the k sampling period, the magnetic moment direction vector is in the local coordinate system YlThe component of the axis is such that,
Figure BDA00028545730000000612
for the k sampling period, the magnetic moment direction vector is in the local coordinate system ZlComponent of the axis, LiFrom capsule endoscope to ith triaxial magnetic transmissionThe euclidean distance at the sensor is,
Figure BDA00028545730000000613
forming a magnetic moment direction vector;
the Euclidean distance from the capsule endoscope to the ith triaxial magnetic sensor is as follows:
Figure BDA00028545730000000614
and 3, linearizing the smoothed magnetic field signal intensity, the first magnetic dipole model function, the second magnetic dipole model function and the third magnetic dipole model function:
the observation model between the magnetic field signal observation value and the capsule endoscope coordinate is as follows:
Figure BDA00028545730000000615
wherein the content of the first and second substances,
Figure BDA0002854573000000071
the smoothed magnetic field signal intensity vector for the kth sampling period,
Figure BDA0002854573000000072
is the coordinate of the capsule endoscope in the kth sampling period, h (#) is the magnetic dipole model function,
Figure BDA0002854573000000073
and observing the error of the smoothed magnetic field signal intensity in the kth sampling period.
And performing Taylor series expansion on an observation model between the magnetic field signal observation value and the capsule endoscope coordinate:
Figure BDA0002854573000000074
wherein the content of the first and second substances,
Figure BDA0002854573000000075
the estimated value of the coordinates of the capsule endoscope in the kth sampling period is calculated as the coordinates of the capsule endoscope in the kth-1 sampling period
Figure BDA0002854573000000076
If the estimated value is calculated for the first time and is the initial value given by the user, the high-order term is omitted, and the formula is taken to be the first-order term to obtain:
Figure BDA0002854573000000077
wherein
Figure BDA0002854573000000078
In order to obtain the coordinate error to be solved,
Figure BDA0002854573000000079
waiting for a coordinate error for the kth sampling period
Figure BDA00028545730000000710
Local coordinate system XlThe axial component of the magnetic flux is,
Figure BDA00028545730000000711
waiting for a coordinate error for the kth sampling period
Figure BDA00028545730000000712
Local coordinate system YlThe axial component of the magnetic flux is,
Figure BDA00028545730000000713
waiting for a coordinate error for the kth sampling period
Figure BDA00028545730000000714
Local coordinate system ZlThe axial component of the magnetic flux is,
Figure BDA00028545730000000715
to design the matrix, the above formula is transformed to obtain an observed value closurePoor linear observation model:
Figure BDA00028545730000000716
Figure BDA00028545730000000717
smoothing the post-magnetic field signal intensity vector for the kth sampling period
Figure BDA00028545730000000718
And the magnetic strength estimation of the k sampling period
Figure BDA00028545730000000719
The difference between the two;
the linear observation model based on the three-axis magnetic sensor array observation value can be designed as follows:
Figure BDA00028545730000000720
wherein N is the total number of the magnetic sensors,
Figure BDA00028545730000000721
the component of the ith three-axis magnetic sensor observation closure difference in the X-axis for the kth sampling period,
Figure BDA00028545730000000722
the component of the ith three-axis magnetic sensor observation closure difference in the Y-axis for the kth sampling period,
Figure BDA00028545730000000723
the component of the ith triaxial magnetic sensor observation value closure difference in the Z axis in the kth sampling period is as follows:
Figure BDA0002854573000000081
i∈[1,N]
wherein
Figure BDA0002854573000000082
The component of the intensity of the magnetic field signal on the X axis after the ith three-axis magnetic sensor is smoothed for the kth sampling period,
Figure BDA0002854573000000083
the component of the intensity of the magnetic field signal on the Y axis after the ith three-axis magnetic sensor is smoothed for the kth sampling period,
Figure BDA0002854573000000084
the component of the intensity of the magnetic field signal on the Z axis after the ith three-axis magnetic sensor is smoothed for the kth sampling period,
Figure BDA0002854573000000085
using a kth sampling period capsule endoscopy coordinate estimate for passing through the first magnetic dipole model function
Figure BDA0002854573000000086
The calculated component of the magnetic field signal strength estimation value of the ith three-axis magnetic sensor on the X axis of the local coordinate system,
Figure BDA0002854573000000087
using a kth sampling period capsule endoscopy coordinate estimate for passing the second magnetic dipole model function
Figure BDA0002854573000000088
The calculated component of the magnetic field signal strength estimation value of the ith three-axis magnetic sensor on the Y axis of the local coordinate system,
Figure BDA0002854573000000089
using a kth sampling period capsule endoscopy coordinate estimate for passing the third magnetic dipole model function
Figure BDA00028545730000000810
ComputingAnd obtaining a component of the magnetic field signal strength estimation value of the ith three-axis magnetic sensor on the Z axis of the local coordinate system.
Design matrix
Figure BDA00028545730000000811
Can be calculated from the following formula:
Figure BDA00028545730000000812
obtaining X-axis observation value from the first magnetic dipole model function, the second magnetic dipole model function and the third magnetic dipole model function
Figure BDA00028545730000000813
The partial derivative of (a) is expressed as:
Figure BDA0002854573000000091
Figure BDA0002854573000000092
Figure BDA0002854573000000093
wherein, the simplified expression PiThe expression is as follows:
Figure BDA0002854573000000094
Figure BDA0002854573000000095
the partial derivative of (d) is expressed as:
Figure BDA0002854573000000096
similarity can be found
Figure BDA0002854573000000097
And
Figure BDA0002854573000000098
partial derivatives of (a) so that a design matrix can be determined
Figure BDA0002854573000000099
And 3, calculating the capsule endoscope position as follows:
the method for calculating the position of the capsule endoscope by adopting a least square method comprises the following steps:
Figure BDA00028545730000000910
wherein R is observation value covariance matrix, and inverse matrix R of R-1The weight of the measurement values of different three-axis magnetic sensors in least square estimation is determined, and an R array meets the following requirements:
Figure BDA00028545730000000911
wherein the content of the first and second substances,
Figure BDA00028545730000000912
as a priori variance, may be provided by a sensor performance parameter, QRIs a co-factor matrix of observed values, Q, due to independence between the measured values of the sensorsRCan be written for a diagonal array:
QR＝diag(Q11,Q22,...,Q3N3N)T
wherein the diagonal element Q11,Q22,...,Q3N3NRepresenting the variance of each sensor observation.
Generally, the covariance matrix of least square estimation is a unit matrix, and in the present invention, since the farther from the magnet, the greater the variance of the magnetometer measurement values, the present invention estimates the variance of the observation values by using the weight of the magnetometer observation values, that is:
Figure BDA00028545730000000913
wherein the content of the first and second substances,
Figure BDA0002854573000000101
the mode of the intensity of the smoothed magnetic field signal of the ith magnetic sensor;
in addition to least squares, other estimation methods may be used, such as kalman filtering or LM methods;
to obtain
Figure BDA0002854573000000102
Can be later passed
Figure BDA0002854573000000103
And (5) obtaining the coordinates of the capsule endoscope.
The invention has the following beneficial effects:
a set of wearable sensor array positioning system using a low-cost magnetometer is designed, and the system is designed according to actual use conditions and has strong practicability.
The traditional LM method for solving the nonlinear equation of the capsule magnet position has the advantage of high precision, but simultaneously faces two problems: one is to estimate 6 parameters of position and attitude. Estimating 6 parameters in the case of a small number of observations falls into local optima. This patent proposes an LM method based on attitude constraints. The basic idea is to determine the attitude parameters and only estimate the position parameters. The method can greatly reduce the probability of the algorithm falling into local optimization.
And compared with a linear method, the traditional LM method has large calculation amount and low efficiency, needs a relatively accurate initial position and is difficult to meet the real-time requirement. In order to solve the problem, the patent provides a nonlinear least square joint adjustment method based on each axial observed value of a magnetometer, so that the calculation efficiency is improved, and the occurrence of gross errors is reduced. Thereby meeting the requirement of real-time calculation of an embedded platform of the capsule endoscope positioning system.
Drawings
FIG. 1: is a schematic diagram of a capsule endoscope human body internal position acquisition system;
FIG. 2: is a flow chart of the method of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention. In addition, the technical features involved in the embodiments of the present invention described below may be combined with each other as long as they do not conflict with each other. For the parameters that need to be analyzed in the actual situation, we have noted the parameter setting method above and will not be described herein.
The embodiment of the invention is particularly applied to acquiring the specific position of a focus area in a human body when a doctor analyzes the disease condition.
Fig. 1 is a schematic diagram of the system for acquiring the position of the capsule endoscope in the human body according to the present invention. Referring to fig. 1, the system for acquiring a position in a human body by using a capsule endoscope comprises:
position acquisition system in capsule scope human body, its characterized in that includes:
the system comprises a capsule endoscope, a magnet, an accelerometer, a gyroscope, a first wireless processor, a plurality of three-axis magnetic sensors, a vest, a second wireless processor, a memory and an application end;
the accelerometer is connected with the first wireless processor in a wired mode; the gyroscope is connected with the first wireless processor in a wired mode; the first wireless processor is wirelessly connected with the second wireless processor; the second wireless processor is connected with the plurality of three-axis magnetic sensors in sequence in a wired mode; the second wireless processor is connected with the memory in a wired mode; the second wireless processor is connected with the application end in a wireless mode;
the plurality of three-axis magnetic sensors, the second wireless processor and the memory form a magnetic sensor array plate and are arranged in the back core;
the vest is worn on a human body, and the vest covers the trunk of the human body in a wearing manner;
the accelerometer, the gyroscope and the first wireless processor are all arranged in the capsule endoscope;
the magnet is a permanent magnet or an electromagnet, is arranged in the capsule endoscope and emits a magnetic field outwards;
the accelerometer is used for acquiring acceleration of the capsule endoscope and transmitting the acceleration to the first wireless processor, the gyroscope is used for acquiring angular velocity of the capsule endoscope and transmitting the angular velocity of the capsule endoscope to the first wireless processor, the first wireless processor obtains attitude information according to the acceleration and the angular velocity through a mechanical arrangement algorithm using inertial navigation, and further the attitude information is wirelessly transmitted to the second wireless processor through the first wireless processor;
the plurality of three-axis magnetic sensors sequentially acquire the strength of magnetic field signals and transmit the strength of the magnetic field signals to the second wireless processor;
the second wireless processor is used for carrying out positioning calculation according to the magnetic field signal intensity and the attitude information acquired by the plurality of three-axis magnetic sensors, calculating the position information of the magnet and wirelessly transmitting the position information to the application end;
the memory is used for storing a coordinate database of a plurality of magnetic sensors and magnet receiving position information;
and the application end is used for receiving the magnet position information from the microprocessor.
The database comprises an observation value database, a sensor coordinate database and sensor position data;
the observation value database comprises identification codes of all the three-axis magnetic sensors and the corresponding observed magnetic field signal intensity, and is stored in the second wireless processor.
The sensor coordinate database comprises a capsule endoscope identification code, a position, a posture and time information corresponding to the capsule endoscope position, and the time information is stored in the second wireless processor.
The sensor position database is used for storing the positions of the magnetic sensors of all three axes, storing the positions in the second wireless processor and sending the positions to the sensor position database.
Fig. 2 is a schematic diagram of a capsule endoscope position acquisition method according to the present invention. Referring to fig. 2, the method for acquiring the position of the capsule endoscope includes the following steps:
step 1: the magnet emits magnetic field signals, the magnetic field signal intensities are sequentially collected by the plurality of magnetic sensors and transmitted to the second wireless processor, the magnetic field signal intensities are compensated by combining the environmental magnetic field intensity to obtain compensated magnetic field signal intensities, coarse difference elimination is carried out on the compensated magnetic field signal intensities according to the sampling time to obtain effective magnetic field signal intensities, and the effective magnetic field signal intensities are further subjected to smoothing processing to obtain smoothed magnetic field signal intensities;
step 1 the magnetic field signal intensity is:
Figure BDA0002854573000000121
i∈[1,N],t∈[1,M],k∈[1,T]
where N-18 is the number of magnetic sensors, M-72000 is the number of sampling times, T-10 is the number of sampling cycles,
Figure BDA0002854573000000122
in order to sample the magnetic field signal strength of the ith triaxial magnetic sensor at time t in the kth sampling period,
Figure BDA0002854573000000123
in order to sample the X-axis magnetic field signal strength of the ith three-axis magnetic sensor at time t in the kth sampling period,
Figure BDA0002854573000000124
in order to sample the Y-axis magnetic field signal strength of the ith three-axis magnetic sensor at time t in the kth sampling period,
Figure BDA0002854573000000125
sampling the Z-axis magnetic field signal intensity of the ith triaxial magnetic sensor at the time t in the kth sampling period;
step 1, the magnetic field signal intensity is compensated by combining the environmental magnetic field intensity to obtain the compensated magnetic field signal intensity as follows:
Figure BDA0002854573000000126
i∈[1,N],t∈[1,M],k∈[1,T]
wherein N is the number of magnetic sensors, M is the number of sampling moments, T is the number of sampling periods, and
Figure BDA0002854573000000127
in order to sample the compensated magnetic field signal strength of the ith triaxial magnetic sensor at time t in the kth sampling period,
Figure BDA0002854573000000128
in order to sample the compensated X-axis magnetic field signal strength of the ith three-axis magnetic sensor at time t in the kth sampling period,
Figure BDA0002854573000000129
in order to sample the compensated Y-axis magnetic field signal strength of the ith three-axis magnetic sensor at time t in the kth sampling period,
Figure BDA00028545730000001210
to sample the compensated Z-axis magnetic field signal strength of the ith three-axis magnetic sensor at time t in the kth sampling period, [ B ]e,x Be,y Be,z]Being the intensity of the ambient magnetic field, Be,xIs the X-axis ambient magnetic field strength, Be,yIs the Y-axis ambient magnetic field strength, Be,zIs the Z-axis ambient magnetic field strength;
step 1, performing gross error elimination on the compensated magnetic field signal intensity according to the sampling time to obtain the effective magnetic field signal intensity:
if the magnetic field signal intensity of any axis of the X, Y and Z axes at the time t in the kth sampling period meets the following condition, judging that the data at the time is a gross error:
Figure BDA0002854573000000131
Figure BDA0002854573000000132
Figure BDA0002854573000000133
wherein the content of the first and second substances,
Figure BDA0002854573000000134
the intensity of the X-axis magnetic field signal compensated for the sampling moment before the moment t in the kth sampling period,
Figure BDA0002854573000000135
the compensated intensity of the X-axis magnetic field signal at the sampling moment after the t moment in the kth sampling period,
Figure BDA0002854573000000136
the intensity of the Y-axis magnetic field signal compensated for the sampling moment before the t moment in the kth sampling period,
Figure BDA0002854573000000137
the compensated Y-axis magnetic field signal strength for the sampling time after time t,
Figure BDA0002854573000000138
the intensity of the Z-axis magnetic field signal after compensation at the sampling moment before the t moment,
Figure BDA0002854573000000139
the intensity of the Z-axis magnetic field signal compensated for the sampling time after the time t, Th is a set threshold, and if the intensity of the magnetic field signal at the time t is determined to be coarse, the weight can be reduced orDiscarding the data;
step 1, performing data smoothing processing on the effective magnetic field signal intensity according to the sampling time to obtain the smoothed magnetic field signal intensity:
Figure BDA00028545730000001310
i∈[1,N]
w1＜w2...＜wN
wherein, it is made
Figure BDA00028545730000001311
The smoothed magnetic field signal strength of the ith triaxial magnetic sensor in the kth sampling period,
Figure BDA00028545730000001312
the smoothed X-axis magnetic field signal strength of the ith three-axis magnetic sensor in the kth sampling period,
Figure BDA00028545730000001313
the smoothed Y-axis magnetic field signal strength of the ith three-axis magnetic sensor in the kth sampling period,
Figure BDA00028545730000001314
the intensity of the smoothed Z-axis magnetic field signal of the ith triaxial magnetic sensor in the kth sampling period is obtained;
step 2: the accelerometer is used for collecting acceleration of the capsule endoscope and transmitting the acceleration to the first wireless processor, the gyroscope is used for collecting angular velocity of the capsule endoscope and transmitting the angular velocity of the capsule endoscope to the first wireless processor, the first wireless processor obtains attitude information according to the acceleration and the angular velocity through a mechanical arrangement algorithm using inertial navigation, and the first wireless processor wirelessly transmits the attitude information to the second wireless processor and stores the attitude information to an observed value of the second wireless processor.
Step 2, the attitude information comprises: pitch angle, roll angle, course angle;
the roll angle is defined as phikShowing X in the local coordinate system of the capsule endoscope around the capsule endoscope in the kth sampling periodlThe angle of rotation of the shaft;
the pitch angle is defined as thetakRepresents Y in the local coordinate system of the capsule endoscope around the capsule endoscope in the kth sampling periodlThe angle of rotation of the shaft;
the heading angle is defined as psikShowing Z in the local coordinate system of the capsule endoscope around the capsule endoscope in the kth sampling periodlThe angle of rotation of the shaft;
the construction method of the capsule endoscope positioning local coordinate system comprises the following steps:
the first wireless processor selects any one of the three-axis magnetic sensors as a coordinate system origin, and uses a human body sagittal axis as a capsule endoscope to position an X in a local coordinate systemlThe axis and the human coronal axis are Y in a capsule endoscope positioning local coordinate systemlThe axis and the pointing direction are Z in a capsule endoscope positioning local coordinate systemlA shaft;
step 3, the second microprocessor calculates a magnetic moment direction vector by combining the attitude information, sequentially constructs a first magnetic dipole model function, a second magnetic dipole model function and a third magnetic dipole model function according to the magnetic moment direction vector, and further calculates the position of the capsule endoscope by utilizing the smoothed magnetic field signal intensity, the linearized first magnetic dipole model function, the linearized second magnetic dipole model function and the linearized third magnetic dipole model function;
step 3, calculating the magnetic moment direction vector by combining the attitude information as follows:
Figure BDA0002854573000000141
wherein, [ l0 m0 n0]TIs the initial magnetic moment vector of the magnet, l0Is the initial magnetic moment direction vector of the magnet in a local coordinate system XlComponent of the axis, m0Is the initial magnetic moment direction vector of the magnet in a local coordinate system YlComponent of the axis, n0Is the initial magnetic moment direction vector of the magnet in a local coordinate system ZlThe component of the axis is such that,
Figure BDA0002854573000000142
representing X in a local coordinate system of the capsule endoscope around the capsule endoscope for the roll angle of the kth sampling periodlThe angle of rotation of the shaft is such that,
Figure BDA0002854573000000143
representing the pitch angle of the kth sampling period as Y in a local coordinate system of the capsule endoscope around the capsule endoscopelThe angle of rotation of the shaft is such that,
Figure BDA0002854573000000151
representing Z in a local coordinate system of the capsule endoscope around the capsule endoscope for the course of the kth sampling periodlThe angle of rotation of the shaft;
step 3, constructing a first magnetic dipole model function is as follows:
Figure BDA0002854573000000152
step 3, constructing a second magnetic dipole model function as follows:
Figure BDA0002854573000000153
step 3, constructing a third magnetic dipole model function is as follows:
Figure BDA0002854573000000154
wherein, BTThe constant value is determined by a magnet of the capsule endoscope,
Figure BDA0002854573000000155
for the X of the k-th sampling period capsule endoscopelThe coordinates of the axes are set to be,
Figure BDA0002854573000000156
for the kth sampling period of Y of the capsule endoscopelThe coordinates of the axes are set to be,
Figure BDA0002854573000000157
for the Z of the k-th sampling period of the capsule endoscopelAxial coordinate, xiFor the ith three-axis magnetic sensor SiX of (2)lAxial coordinate, yiFor the ith three-axis magnetic sensor SiY of (A) islAxial coordinate, ziFor the ith three-axis magnetic sensor SiZ of (A)lThe coordinates of the axes are set to be,
Figure BDA0002854573000000158
the X-axis magnetic field signal strength value of the ith three-axis magnetic sensor in the kth sampling period,
Figure BDA0002854573000000159
the Y-axis magnetic field signal intensity value of the ith three-axis magnetic sensor in the kth sampling period, the Z-axis magnetic field signal intensity value of the ith three-axis magnetic sensor in the kth sampling period,
Figure BDA00028545730000001510
for the k sampling period, the magnetic moment direction vector is in the local coordinate system XlThe component of the axis is such that,
Figure BDA00028545730000001511
for the k sampling period, the magnetic moment direction vector is in the local coordinate system YlThe component of the axis is such that,
Figure BDA00028545730000001512
for the k sampling period, the magnetic moment direction vector is in the local coordinate system ZlThe component of the axis is such that,
Figure BDA00028545730000001513
the Euclidean distance from the capsule endoscope to the ith triaxial magnetic sensor in the kth sampling period,
Figure BDA00028545730000001514
forming a magnetic moment direction vector;
the Euclidean distance from the capsule endoscope to the ith triaxial magnetic sensor is as follows:
Figure BDA00028545730000001515
and 3, linearizing the smoothed magnetic field signal intensity, the first magnetic dipole model function, the second magnetic dipole model function and the third magnetic dipole model function:
the observation model between the magnetic field signal observation value and the capsule endoscope coordinate is as follows:
Figure BDA0002854573000000161
wherein the content of the first and second substances,
Figure BDA0002854573000000162
the smoothed magnetic field signal intensity vector for the kth sampling period,
Figure BDA0002854573000000163
is the coordinate of the capsule endoscope in the kth sampling period, h (#) is the magnetic dipole model function,
Figure BDA0002854573000000164
and observing the error of the smoothed magnetic field signal intensity in the kth sampling period.
And performing Taylor series expansion on an observation model between the magnetic field signal observation value and the capsule endoscope coordinate:
Figure BDA0002854573000000165
wherein the content of the first and second substances,
Figure BDA0002854573000000166
for the kth sampling period capsule endoscopy coordinateEstimated value of the coordinates of the capsule endoscope calculated in the k-1 sampling period
Figure BDA0002854573000000167
If the estimated value is calculated for the first time and is the initial value given by the user, the high-order term is omitted, and the formula is taken to be the first-order term to obtain:
Figure BDA0002854573000000168
wherein
Figure BDA0002854573000000169
In order to obtain the coordinate error to be solved,
Figure BDA00028545730000001610
waiting for a coordinate error for the kth sampling period
Figure BDA00028545730000001611
Local coordinate system XlThe axial component of the magnetic flux is,
Figure BDA00028545730000001612
waiting for a coordinate error for the kth sampling period
Figure BDA00028545730000001613
Local coordinate system YlThe axial component of the magnetic flux is,
Figure BDA00028545730000001614
waiting for a coordinate error for the kth sampling period
Figure BDA00028545730000001615
Local coordinate system ZlThe axial component of the magnetic flux is,
Figure BDA00028545730000001616
in order to design a matrix, the linear observation model of the observed value closure difference can be obtained by transforming the formula:
Figure BDA00028545730000001617
Figure BDA00028545730000001618
smoothing the post-magnetic field signal intensity vector for the kth sampling period
Figure BDA00028545730000001619
And the magnetic strength estimation of the k sampling period
Figure BDA00028545730000001620
The difference between the two;
the linear observation model based on the three-axis magnetic sensor array observation value can be designed as follows:
Figure BDA00028545730000001621
wherein N is the total number of the magnetic sensors,
Figure BDA00028545730000001622
the component of the ith three-axis magnetic sensor observation closure difference in the X-axis for the kth sampling period,
Figure BDA00028545730000001623
the component of the ith three-axis magnetic sensor observation closure difference in the Y-axis for the kth sampling period,
Figure BDA0002854573000000171
the component of the ith triaxial magnetic sensor observation value closure difference in the Z axis in the kth sampling period is as follows:
Figure BDA0002854573000000172
i∈[1,N]
wherein
Figure BDA0002854573000000173
The component of the intensity of the magnetic field signal on the X axis after the ith three-axis magnetic sensor is smoothed for the kth sampling period,
Figure BDA0002854573000000174
the component of the intensity of the magnetic field signal on the Y axis after the ith three-axis magnetic sensor is smoothed for the kth sampling period,
Figure BDA0002854573000000175
the component of the intensity of the magnetic field signal on the Z axis after the ith three-axis magnetic sensor is smoothed for the kth sampling period,
Figure BDA0002854573000000176
using a kth sampling period capsule endoscopy coordinate estimate for passing through the first magnetic dipole model function
Figure BDA0002854573000000177
The calculated component of the magnetic field signal strength estimation value of the ith three-axis magnetic sensor on the X axis of the local coordinate system,
Figure BDA0002854573000000178
using a kth sampling period capsule endoscopy coordinate estimate for passing the second magnetic dipole model function
Figure BDA0002854573000000179
The calculated component of the magnetic field signal strength estimation value of the ith three-axis magnetic sensor on the Y axis of the local coordinate system,
Figure BDA00028545730000001710
using a kth sampling period capsule endoscopy coordinate estimate for passing the third magnetic dipole model function
Figure BDA00028545730000001711
And calculating the component of the magnetic field signal strength estimation value of the ith three-axis magnetic sensor on the Z axis of the local coordinate system.
Design matrix
Figure BDA00028545730000001712
Can be calculated from the following formula:
Figure BDA00028545730000001713
obtaining X-axis observation value from the first magnetic dipole model function, the second magnetic dipole model function and the third magnetic dipole model function
Figure BDA00028545730000001714
The partial derivative of (a) is expressed as:
Figure BDA0002854573000000181
Figure BDA0002854573000000182
Figure BDA0002854573000000183
wherein, the simplified expression PiThe expression is as follows:
Figure BDA0002854573000000184
Figure BDA0002854573000000185
the partial derivative of (d) is expressed as:
Figure BDA0002854573000000186
similarity can be found
Figure BDA0002854573000000187
And
Figure BDA0002854573000000188
partial derivatives of (a) so that a design matrix can be determined
Figure BDA0002854573000000189
And 3, calculating the capsule endoscope position as follows:
the method for calculating the position of the capsule endoscope by adopting a least square method comprises the following steps:
Figure BDA00028545730000001810
wherein R is observation value covariance matrix, and inverse matrix R of R-1The weight of the measurement values of different three-axis magnetic sensors in least square estimation is determined, and an R array meets the following requirements:
Figure BDA00028545730000001811
wherein the content of the first and second substances,
Figure BDA00028545730000001812
as a priori variance, may be provided by a sensor performance parameter, QRIs a co-factor matrix of observed values, Q, due to independence between the measured values of the sensorsRCan be written for a diagonal array:
QR＝diag(Q11,Q22,...,Q3N3N)T
wherein the diagonal element Q11,Q22,...,Q3N3NRepresenting the variance of each sensor observation.
Generally, the covariance matrix of least square estimation is a unit matrix, and in the present invention, since the farther from the magnet, the greater the variance of the magnetometer measurement values, the present invention estimates the variance of the observation values by using the weight of the magnetometer observation values, that is:
Figure BDA00028545730000001813
wherein the content of the first and second substances,
Figure BDA0002854573000000191
the mode of the intensity of the smoothed magnetic field signal of the ith magnetic sensor;
in addition to least squares, other estimation methods may be used, such as kalman filtering or LM methods;
to obtain
Figure BDA0002854573000000192
Can be later passed
Figure BDA0002854573000000193
And (5) obtaining the coordinates of the capsule endoscope.
The wearable device is low in power consumption and easy to wear, and the capsule endoscope human body positioning system can remarkably reduce the number of required magnetic sensors and is convenient for design and use of the system.
Compared with a nonlinear calculation method, the linear least square algorithm provided by the embodiment has higher efficiency under the same precision, and is suitable for an application scene of acquiring the capsule position in real time.
The method of the invention is also applicable in combination with other sensors such as inertial sensors, visual sensors.
It should be understood that parts of the application not described in detail are prior art.
It should be understood that the above description of the preferred embodiments is given for clearness of understanding and no unnecessary limitations should be understood therefrom, and all changes and modifications may be made by those skilled in the art without departing from the scope of the invention as defined by the appended claims.

Claims (3)

1. The utility model provides a position acquisition system in human of capsule scope which characterized in that:
position acquisition system in capsule scope human body, its characterized in that includes:
the system comprises a capsule endoscope, a magnet, an accelerometer, a gyroscope, a first wireless processor, a plurality of three-axis magnetic sensors, a vest, a second wireless processor, a memory and an application end;
the accelerometer is connected with the first wireless processor in a wired mode; the gyroscope is connected with the first wireless processor in a wired mode; the first wireless processor is wirelessly connected with the second wireless processor; the second wireless processor is connected with the plurality of three-axis magnetic sensors in sequence in a wired mode; the second wireless processor is connected with the memory in a wired mode; the second wireless processor is connected with the application end in a wireless mode;
the plurality of three-axis magnetic sensors, the second wireless processor and the memory form a magnetic sensor array plate and are arranged in the back core;
the vest is worn on a human body;
the accelerometer, the gyroscope and the first wireless processor are all arranged in the capsule endoscope;
the magnet is a permanent magnet or an electromagnet, is arranged in the capsule endoscope and emits a magnetic field outwards;
the accelerometer is used for acquiring acceleration of the capsule endoscope and transmitting the acceleration to the first wireless processor, the gyroscope is used for acquiring angular velocity of the capsule endoscope and transmitting the angular velocity of the capsule endoscope to the first wireless processor, the first wireless processor obtains attitude information according to the acceleration and the angular velocity through a mechanical arrangement algorithm using inertial navigation, and further the attitude information is wirelessly transmitted to the second wireless processor through the first wireless processor;
the plurality of three-axis magnetic sensors sequentially acquire the strength of magnetic field signals and transmit the strength of the magnetic field signals to the second wireless processor;
the second wireless processor is used for carrying out positioning calculation according to the magnetic field signal intensity and the attitude information acquired by the plurality of three-axis magnetic sensors, calculating the position information of the magnet and wirelessly transmitting the position information to the application end;
the memory is used for storing a coordinate database of a plurality of three-axis magnetic sensors and magnet receiving position information;
the application end is used for receiving magnet position information from the second wireless processor;
the magnet emits a magnetic field signal, the magnetic field signal strength is sequentially acquired by the plurality of three-axis magnetic sensors and transmitted to the second wireless processor, the magnetic field signal strength is compensated by combining the environmental magnetic field strength to obtain a compensated magnetic field signal strength, coarse difference elimination is carried out on the compensated magnetic field signal strength according to the sampling time to obtain an effective magnetic field signal strength, and the effective magnetic field signal strength is further subjected to smoothing processing to obtain a smoothed magnetic field signal strength;
the accelerometer is used for acquiring acceleration of the capsule endoscope and transmitting the acceleration to the first wireless processor, the gyroscope is used for acquiring angular velocity of the capsule endoscope and transmitting the angular velocity of the capsule endoscope to the first wireless processor, the first wireless processor obtains attitude information according to the acceleration and the angular velocity through a mechanical arrangement algorithm using inertial navigation, and the first wireless processor wirelessly transmits the attitude information to the second wireless processor and stores the attitude information to an observed value of the second wireless processor;
the second wireless processor calculates a magnetic moment direction vector by combining the attitude information, sequentially constructs a first magnetic dipole model function, a second magnetic dipole model function and a third magnetic dipole model function according to the magnetic moment direction vector, and further calculates the position of the capsule endoscope by utilizing the intensity of the smoothed magnetic field signal and the linearized first magnetic dipole model function, second magnetic dipole model function and third magnetic dipole model function;
the calculation of the magnetic moment direction vector in combination with the attitude information is:
Figure FDA0003509646890000021
wherein, [ l0 m0 n0]TIs the initial magnetic moment vector of the magnet, l0Is the initial magnetic moment direction vector of the magnet in a local coordinate system XlComponent of the axis, m0Is the initial magnetic moment direction vector of the magnet in a local coordinate system YlComponent of the axis, n0Is the initial magnetic moment direction vector of the magnet in a local coordinate system ZlComponent of the axis phitkRepresenting X in a local coordinate system of the capsule endoscope around the capsule endoscope for the roll angle of the kth sampling periodlThe angle of rotation of the shaft is such that,
Figure FDA0003509646890000022
representing the pitch angle of the kth sampling period as Y in a local coordinate system of the capsule endoscope around the capsule endoscopelThe angle of rotation of the shaft is such that,
Figure FDA0003509646890000023
representing Z in a local coordinate system of the capsule endoscope around the capsule endoscope for the course of the kth sampling periodlThe angle of rotation of the shaft;
the first magnetic dipole model function is constructed by:
Figure FDA0003509646890000024
the second magnetic dipole model function is constructed by:
Figure FDA0003509646890000025
the third magnetic dipole model function is constructed by:
Figure FDA0003509646890000026
wherein, BTThe constant value is determined by a magnet of the capsule endoscope,
Figure FDA0003509646890000031
for the X of the k-th sampling period capsule endoscopelThe coordinates of the axes are set to be,
Figure FDA0003509646890000032
for the kth sampling period of Y of the capsule endoscopelThe coordinates of the axes are set to be,
Figure FDA0003509646890000033
for the Z of the k-th sampling period of the capsule endoscopelAxial coordinate, xiFor the ith three-axis magnetic sensor SiX of (2)lAxial coordinate, yiFor the ith three-axis magnetic sensor SiY of (A) islAxial coordinate, ziFor the ith three-axis magnetic sensor SiZ of (A)lThe coordinates of the axes are set to be,
Figure FDA0003509646890000034
the X-axis magnetic field signal strength value of the ith three-axis magnetic sensor in the kth sampling period,
Figure FDA0003509646890000035
is the Y-axis magnetic field signal intensity value of the ith three-axis magnetic sensor in the kth sampling period,
Figure FDA0003509646890000036
is the Z-axis magnetic field signal strength value of the ith three-axis magnetic sensor in the kth sampling period,
Figure FDA0003509646890000037
for the k sampling period, the magnetic moment direction vector is in the local coordinate system XlThe component of the axis is such that,
Figure FDA0003509646890000038
for the k sampling period, the magnetic moment direction vector is in the local coordinate system YlThe component of the axis is such that,
Figure FDA0003509646890000039
for the k sampling period, the magnetic moment direction vector is in the local coordinate system ZlThe component of the axis is such that,
Figure FDA00035096468900000310
the Euclidean distance from the capsule endoscope to the ith triaxial magnetic sensor,
Figure FDA00035096468900000311
Figure FDA00035096468900000312
forming a magnetic moment direction vector;
the Euclidean distance from the capsule endoscope to the ith triaxial magnetic sensor is as follows:
Figure FDA00035096468900000313
linearizing the smoothed magnetic field signal intensity, the first magnetic dipole model function, the second magnetic dipole model function and the third magnetic dipole model function into:
the observation model between the magnetic field signal observation value and the capsule endoscope coordinate is as follows:
Figure FDA00035096468900000314
wherein the content of the first and second substances,
Figure FDA00035096468900000315
the smoothed magnetic field signal intensity vector for the kth sampling period,
Figure FDA00035096468900000316
is the coordinate of the capsule endoscope in the kth sampling period, h (#) is the magnetic dipole model function,
Figure FDA00035096468900000317
observing the magnetic field signal intensity error after smoothing in the kth sampling period;
and performing Taylor series expansion on an observation model between the magnetic field signal observation value and the capsule endoscope coordinate:
Figure FDA00035096468900000318
wherein the content of the first and second substances,
Figure FDA00035096468900000319
the estimated value of the coordinates of the capsule endoscope in the kth sampling period is calculated as the coordinates of the capsule endoscope in the kth-1 sampling period
Figure FDA00035096468900000320
If the estimated value is calculated for the first time and is the initial value given by the user, the high-order term is omitted, and the formula is taken to be the first-order term to obtain:
Figure FDA0003509646890000041
wherein
Figure FDA0003509646890000042
In order to obtain the coordinate error to be solved,
Figure FDA0003509646890000043
waiting for a coordinate error for the kth sampling period
Figure FDA0003509646890000044
Local coordinate system XlThe axial component of the magnetic flux is,
Figure FDA0003509646890000045
waiting for a coordinate error for the kth sampling period
Figure FDA0003509646890000046
Local coordinate system YlThe axial component of the magnetic flux is,
Figure FDA0003509646890000047
waiting for a coordinate error for the kth sampling period
Figure FDA0003509646890000048
Local coordinate system ZlThe axial component of the magnetic flux is,
Figure FDA0003509646890000049
in order to design a matrix, the linear observation model of the observed value closure difference can be obtained by transforming the formula:
Figure FDA00035096468900000410
Figure FDA00035096468900000411
Figure FDA00035096468900000412
smoothing the post-magnetic field signal intensity vector for the kth sampling period
Figure FDA00035096468900000413
And the magnetic strength estimation of the k sampling period
Figure FDA00035096468900000414
The difference between the two;
the linear observation model based on the three-axis magnetic sensor array observation value is as follows:
Figure FDA00035096468900000415
wherein N is the total number of the magnetic sensors,
Figure FDA00035096468900000416
the component of the ith three-axis magnetic sensor observation closure difference in the X-axis for the kth sampling period,
Figure FDA00035096468900000417
the component of the ith three-axis magnetic sensor observation closure difference in the Y-axis for the kth sampling period,
Figure FDA00035096468900000418
the component of the ith triaxial magnetic sensor observation value closure difference in the Z axis in the kth sampling period is as follows:
Figure FDA00035096468900000419
wherein
Figure FDA00035096468900000420
The component of the intensity of the magnetic field signal on the X axis after the ith three-axis magnetic sensor is smoothed for the kth sampling period,
Figure FDA00035096468900000421
the component of the intensity of the magnetic field signal on the Y axis after the ith three-axis magnetic sensor is smoothed for the kth sampling period,
Figure FDA00035096468900000422
the component of the intensity of the magnetic field signal on the Z axis after the ith three-axis magnetic sensor is smoothed for the kth sampling period,
Figure FDA00035096468900000423
using a kth sampling period capsule endoscopy coordinate estimate for passing through the first magnetic dipole model function
Figure FDA00035096468900000424
The calculated component of the magnetic field signal strength estimation value of the ith three-axis magnetic sensor on the X axis of the local coordinate system,
Figure FDA0003509646890000051
using a kth sampling period capsule endoscopy coordinate estimate for passing the second magnetic dipole model function
Figure FDA0003509646890000052
The calculated component of the magnetic field signal strength estimation value of the ith three-axis magnetic sensor on the Y axis of the local coordinate system,
Figure FDA0003509646890000053
using a kth sampling period capsule endoscopy coordinate estimate for passing the third magnetic dipole model function
Figure FDA0003509646890000054
Calculating a component of the magnetic field signal intensity estimated value of the ith triaxial magnetic sensor on the Z axis of the local coordinate system;
design matrix
Figure FDA0003509646890000055
Can be calculated from the following formula:
Figure FDA0003509646890000056
obtaining X-axis observation value from the first magnetic dipole model function, the second magnetic dipole model function and the third magnetic dipole model function
Figure FDA0003509646890000057
The partial derivative of (a) is expressed as:
Figure FDA0003509646890000058
Figure FDA0003509646890000059
Figure FDA00035096468900000510
wherein, the simplified expression PiThe expression is as follows:
Figure FDA00035096468900000511
Figure FDA00035096468900000512
the partial derivative of (d) is expressed as:
Figure FDA00035096468900000513
similarity can be found
Figure FDA00035096468900000514
And
Figure FDA00035096468900000515
partial derivatives of (a) so that a design matrix can be determined
Figure FDA00035096468900000516
The calculating of the capsule endoscope position comprises the following steps:
calculating the position of the capsule endoscope by adopting a least square method:
Figure FDA0003509646890000061
wherein R is observation value covariance matrix, and inverse matrix R of R-1The weight of the measurement values of different three-axis magnetic sensors in least square estimation is determined, and an R array meets the following requirements:
Figure FDA0003509646890000062
wherein the content of the first and second substances,
Figure FDA0003509646890000063
as a priori variance, may be provided by a sensor performance parameter, QRIs a co-factor matrix of observed values, Q, due to independence between the measured values of the sensorsRCan be written for a diagonal array:
QR＝diag(Q11,Q22,...,Q3N3N)T
wherein the diagonal element Q11,Q22,...,Q3N3NRepresenting variance of observations of each sensor;
estimating the observation variance using the magnetometer observation weights, i.e.:
Figure FDA0003509646890000064
wherein the content of the first and second substances,
Figure FDA0003509646890000065
the mode of the intensity of the smoothed magnetic field signal of the ith magnetic sensor;
to obtain
Figure FDA0003509646890000066
Can be later passed
Figure FDA0003509646890000067
And (5) obtaining the coordinates of the capsule endoscope.
2. The system for acquiring a position in a human body by capsule endoscopy of claim 1, wherein:
the magnetic field signal strength is:
Figure FDA0003509646890000068
wherein N is the number of the three-axis magnetic sensors, M is the number of sampling moments, T is the number of sampling periods,
Figure FDA0003509646890000069
in order to sample the magnetic field signal strength of the ith triaxial magnetic sensor at time t in the kth sampling period,
Figure FDA00035096468900000610
in order to sample the X-axis magnetic field signal strength of the ith three-axis magnetic sensor at time t in the kth sampling period,
Figure FDA00035096468900000611
in order to sample the Y-axis magnetic field signal strength of the ith three-axis magnetic sensor at time t in the kth sampling period,
Figure FDA00035096468900000612
sampling the Z-axis magnetic field signal intensity of the ith triaxial magnetic sensor at the time t in the kth sampling period;
the magnetic field signal intensity is compensated by combining the environmental magnetic field intensity to obtain the compensated magnetic field signal intensity as follows:
Figure FDA0003509646890000071
wherein N is the number of the three-axis magnetic sensors, M is the number of sampling moments, and T is the number of sampling periods, so that
Figure FDA0003509646890000072
In order to sample the compensated magnetic field signal strength of the ith triaxial magnetic sensor at time t in the kth sampling period,
Figure FDA0003509646890000073
in order to sample the compensated X-axis magnetic field signal strength of the ith three-axis magnetic sensor at time t in the kth sampling period,
Figure FDA0003509646890000074
in order to sample the compensated Y-axis magnetic field signal strength of the ith three-axis magnetic sensor at time t in the kth sampling period,
Figure FDA0003509646890000075
to sample the compensated Z-axis magnetic field signal strength of the ith three-axis magnetic sensor at time t in the kth sampling period, [ B ]e,x Be,y Be,z]Being the intensity of the ambient magnetic field, Be,xIs the X-axis ambient magnetic field strength, Be,yIs the Y-axis ambient magnetic field strength, Be,zIs the Z-axis ambient magnetic field strength;
the effective magnetic field signal intensity obtained by performing gross error rejection on the compensated magnetic field signal intensity according to the sampling time is as follows:
if the magnetic field signal intensity of any axis of the X, Y and Z axes at the time t in the kth sampling period meets the following condition, judging that the data at the time is a gross error:
Figure FDA0003509646890000076
Figure FDA0003509646890000077
Figure FDA0003509646890000078
wherein the content of the first and second substances,
Figure FDA0003509646890000079
the intensity of the X-axis magnetic field signal compensated for the sampling moment before the moment t in the kth sampling period,
Figure FDA00035096468900000710
the compensated intensity of the X-axis magnetic field signal at the sampling moment after the t moment in the kth sampling period,
Figure FDA00035096468900000711
the intensity of the Y-axis magnetic field signal compensated for the sampling moment before the t moment in the kth sampling period,
Figure FDA00035096468900000712
the compensated Y-axis magnetic field signal strength for the sampling time after time t,
Figure FDA00035096468900000713
the intensity of the Z-axis magnetic field signal after compensation at the sampling moment before the t moment,
Figure FDA00035096468900000714
the intensity of the Z-axis magnetic field signal compensated for the sampling time after the time t is the Th is a set threshold, and if the intensity of the magnetic field signal at the time t is judged to be coarse difference, the weight of the intensity can be reduced or the data can be discarded;
the magnetic field signal intensity after smoothing obtained by performing data smoothing on the effective magnetic field signal intensity according to the sampling time is as follows:
Figure FDA0003509646890000081
wherein, it is made
Figure FDA0003509646890000082
Is as followsThe smoothed magnetic field signal intensity of the ith triaxial magnetic sensor in k sampling periods,
Figure FDA0003509646890000083
the smoothed X-axis magnetic field signal strength of the ith three-axis magnetic sensor in the kth sampling period,
Figure FDA0003509646890000084
the smoothed Y-axis magnetic field signal strength of the ith three-axis magnetic sensor in the kth sampling period,
Figure FDA0003509646890000085
and the intensity of the smoothed Z-axis magnetic field signal of the ith three-axis magnetic sensor in the kth sampling period.
3. The system for acquiring a position in a human body by capsule endoscopy of claim 1, wherein:
the attitude information includes: pitch angle, roll angle, course angle;
the roll angle is defined as phikShowing X in the local coordinate system of the capsule endoscope around the capsule endoscope in the kth sampling periodlThe angle of rotation of the shaft;
the pitch angle is defined as thetakRepresents Y in the local coordinate system of the capsule endoscope around the capsule endoscope in the kth sampling periodlThe angle of rotation of the shaft;
the heading angle is defined as psikShowing Z in the local coordinate system of the capsule endoscope around the capsule endoscope in the kth sampling periodlThe angle of rotation of the shaft;
the construction method of the capsule endoscope positioning local coordinate system comprises the following steps:
the first wireless processor selects any one of the three-axis magnetic sensors as a coordinate system origin, and uses a human body sagittal axis as a capsule endoscope to position an X in a local coordinate systemlThe axis and the human coronal axis are Y in a capsule endoscope positioning local coordinate systemlThe axis and the direction of the ground are used for positioning the local part of the capsule endoscopeZ in the coordinate systemlA shaft.
CN202011539785.1A 2020-12-23 2020-12-23 Position acquisition method based on capsule endoscopy human body internal position acquisition system Active CN112741586B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011539785.1A CN112741586B (en) 2020-12-23 2020-12-23 Position acquisition method based on capsule endoscopy human body internal position acquisition system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011539785.1A CN112741586B (en) 2020-12-23 2020-12-23 Position acquisition method based on capsule endoscopy human body internal position acquisition system

Publications (2)

Publication Number Publication Date
CN112741586A CN112741586A (en) 2021-05-04
CN112741586B true CN112741586B (en) 2022-04-01

Family

ID=75646640

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011539785.1A Active CN112741586B (en) 2020-12-23 2020-12-23 Position acquisition method based on capsule endoscopy human body internal position acquisition system

Country Status (1)

Country Link
CN (1) CN112741586B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023221283A1 (en) * 2022-05-20 2023-11-23 深圳硅基智控科技有限公司 Inertial measurement-integrated magnetic control system

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101053517A (en) * 2007-05-18 2007-10-17 深圳先进技术研究院 Method and system for tracking internal mini device
CN104720805A (en) * 2015-03-24 2015-06-24 上海交通大学 Power capsule real-time positioning method based on permanent magnet
CN106937860A (en) * 2017-04-12 2017-07-11 重庆金山医疗器械有限公司 Know the capsule endoscope system and its method of work of real-time pose
CN109620104A (en) * 2019-01-10 2019-04-16 深圳市资福医疗技术有限公司 Capsule endoscope and its localization method and system

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4813190B2 (en) * 2005-05-26 2011-11-09 オリンパスメディカルシステムズ株式会社 Capsule medical device
WO2010004555A1 (en) * 2008-07-10 2010-01-14 Given Imaging Ltd. Localization of capsule with a synthetic source of quadrupoles and dipoles

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101053517A (en) * 2007-05-18 2007-10-17 深圳先进技术研究院 Method and system for tracking internal mini device
CN104720805A (en) * 2015-03-24 2015-06-24 上海交通大学 Power capsule real-time positioning method based on permanent magnet
CN106937860A (en) * 2017-04-12 2017-07-11 重庆金山医疗器械有限公司 Know the capsule endoscope system and its method of work of real-time pose
CN109620104A (en) * 2019-01-10 2019-04-16 深圳市资福医疗技术有限公司 Capsule endoscope and its localization method and system

Also Published As

Publication number Publication date
CN112741586A (en) 2021-05-04

Similar Documents

Publication Publication Date Title
CN104757976B (en) A kind of Human Body Gait Analysis method and system based on Multi-sensor Fusion
Brigante et al. Towards miniaturization of a MEMS-based wearable motion capture system
CN100594840C (en) Method and system for tracking internal mini device
US20080091373A1 (en) Method for calibrating sensor positions in a human movement measurement and analysis system
CN107348931B (en) Capsule endoscope space attitude determination system
CN105850773A (en) Device and method for monitoring of pig attitudes based on micro-inertial sensor
CN102499762B (en) Three-dimensional spatial positioning system for medical ultrasonic probe relative to part to be checked and method
US11712309B2 (en) Magnetic flexible catheter tracking system and method using digital magnetometers
CN112741586B (en) Position acquisition method based on capsule endoscopy human body internal position acquisition system
Wang et al. A new physiological signal acquisition patch designed with advanced respiration monitoring algorithm based on 3-axis accelerator and gyroscope
Vertens et al. Measuring Respiration and Heart Rate using Two Acceleration Sensors on a Fully Embedded Platform.
Wang et al. CanoeSense: Monitoring canoe sprint motion using wearable sensors
CN111895997A (en) Human body action acquisition method based on inertial sensor without standard posture correction
El-Gohary et al. Joint angle tracking with inertial sensors
JP7270284B2 (en) Operation method of magnetic capsule endoscope full-posture measurement system
Johnson et al. Estimation of three-dimensional thoracoabdominal displacements during respiration using inertial measurement units
CN108186017B (en) Detection system and method for determining in-vivo pose of endoscope capsule
Qiu et al. Heterogeneous data fusion for three-dimensional gait analysis using wearable MARG sensors
Sun et al. An artificial neural network framework for lower limb motion signal estimation with foot-mounted inertial sensors
US20230337923A1 (en) Device for mapping a sensor's baseline coordinate reference frames to anatomical landmarks
Li et al. Calibrated analytical model for magnetic localization of wireless capsule endoscope based on onboard sensing
Wang et al. 3-D electromagnetic position estimation system using high-magnetic-permeability metal for continuum medical robots
Chen et al. An inertial-based human motion tracking system with twists and exponential maps
Lee et al. A wearable RFID system to monitor hand use for individuals with upper limb paresis
Francelino et al. Markov system with self-aligning joint constraint to estimate attitude and joint angles between two consecutive segments

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