CN114668362B

CN114668362B - Positioning system and device of wireless capsule endoscope and computer equipment

Info

Publication number: CN114668362B
Application number: CN202210269851.0A
Authority: CN
Inventors: 孟李艾俐; 李可喻; 许杨昕
Original assignee: Yuanhua Intelligent Technology Shenzhen Co ltd
Current assignee: Yuanhua Intelligent Technology Shenzhen Co ltd
Priority date: 2022-03-18
Filing date: 2022-03-18
Publication date: 2022-11-11
Anticipated expiration: 2042-03-18
Also published as: CN114668362A

Abstract

The application is applicable to the technical field of medical equipment, and provides a positioning system, a positioning device and computer equipment of a wireless capsule endoscope, wherein the system comprises the wireless capsule endoscope, an extracorporeal driver and the computer equipment; a plurality of in-vitro magnetic sources are arranged in the in-vitro driver; an inertial sensor and a plurality of magnetic sensors are arranged in the wireless capsule endoscope; the computer device is used for executing the following operations: acquiring biaxial rotation data and measuring a magnetic field value; determining position data of the extracorporeal driver, and calculating theoretical magnetic field values of the plurality of extracorporeal magnetic sources at each magnetic sensor according to the biaxial rotation data and the position data; separating the measured magnetic field values measured by each magnetic sensor to obtain the separated magnetic field value of each in-vitro magnetic source at each magnetic sensor; and positioning the wireless capsule endoscope based on the theoretical magnetic field value and the separation magnetic field value. By adopting the system, the real-time six-dimensional pose of the wireless capsule endoscope can be accurately determined.

Description

Positioning system and device of wireless capsule endoscope and computer equipment

Technical Field

The application belongs to the technical field of medical equipment, and particularly relates to a positioning system and device of a wireless capsule endoscope and computer equipment.

Background

The wireless capsule endoscope technology is used for complete digestive tract examination and has the features of no pain, no invasion, etc. The wireless capsule endoscope is actually a capsule-shaped micro-robot, and is equipped with an illumination module, a camera module, an image processing module, a wireless transmission module, and the like. After swallowed by a patient and entering the digestive tract of a human body, the wireless capsule endoscope can shoot images in vivo and transmit the images to the outside of the body in real time. A doctor or a computer can make a disease diagnosis based on the received image.

During the process of using the wireless capsule endoscope to carry out the digestive tract examination, the wireless capsule endoscope is accurately positioned in real time, which is related to the correctness of the final diagnosis result. In general, a six-dimensional pose consisting of a three-dimensional position and a three-dimensional rotation of the wireless capsule endoscope can be used to describe the precise location of the wireless capsule endoscope in the alimentary tract. However, in the prior art, the real-time positioning of the wireless capsule endoscope is usually only capable of solving five-dimensional poses of the wireless capsule endoscope at most. For example, in the positioning technology of the wireless capsule endoscope based on magnetic driving, an external permanent magnet is used for driving the wireless capsule endoscope embedded with a permanent magnet, then the superposed magnetic field of the external permanent magnet and the permanent magnet in the capsule is measured through a magnetic sensor, and then the real-time five-dimensional pose of the wireless capsule endoscope is obtained through calculation. Some researchers estimate the advancing direction of the wireless capsule endoscope by adopting a five-dimensional pose sequence measured within a period of time so as to obtain the six-dimensional pose of the wireless capsule endoscope, but the method cannot intuitively obtain the three-dimensional rotation of the wireless capsule endoscope relative to a world coordinate system at each moment, and because the measured value within a period of time is required to estimate the sixth-dimensional pose, the positioning frequency of the wireless capsule endoscope is greatly reduced, and the adverse effect on the digestive tract examination is easily brought. Therefore, how to accurately estimate the six-dimensional pose of the wireless capsule endoscope in real time is a difficult problem to be solved by those skilled in the art in the course of using the wireless capsule endoscope to perform the digestive tract examination.

Disclosure of Invention

In view of this, the present application provides a positioning system, an apparatus, and a computer device for a wireless capsule endoscope, which can estimate the six-dimensional pose of a capsule in real time by using a composite extracorporeal driver based on a permanent magnet and an electromagnetic coil and fusing a plurality of magnetic sensors in the capsule, and are helpful to realize positioning with high precision and high update rate in a large three-dimensional working space.

A first aspect of the application provides a positioning system for a wireless capsule endoscope, the system comprising a wireless capsule endoscope, an extracorporeal driver, and a computer device communicatively connected to the wireless capsule endoscope and the extracorporeal driver, respectively; the in-vitro driver is internally provided with a plurality of in-vitro magnetic sources, the plurality of in-vitro magnetic sources comprise in-vitro permanent magnets, first electromagnetic coils and second electromagnetic coils, and unit magnetic moments of the in-vitro permanent magnets, the first electromagnetic coils and the second electromagnetic coils are orthogonal pairwise; the wireless capsule endoscope is internally provided with a capsule permanent magnet, an inertial sensor and a plurality of magnetic sensors; the computer device is configured to perform the following operations:

in the process of controlling the extracorporeal driver to drive the wireless capsule endoscope to move, acquiring biaxial rotation data of the wireless capsule endoscope measured by the inertial sensor and a measured magnetic field value measured by each magnetic sensor;

determining position data for the extracorporeal drive and calculating theoretical magnetic field values for a plurality of the extracorporeal magnetic sources at each of the magnetic sensors based on the biaxial rotation data and the position data;

separating the measured magnetic field values measured by each magnetic sensor to obtain a separated magnetic field value of each in-vitro magnetic source at each magnetic sensor;

and positioning the wireless capsule endoscope based on the theoretical magnetic field value and the separation magnetic field value, and outputting the real-time six-dimensional pose of the wireless capsule endoscope.

A second aspect of the present application provides a positioning device of a wireless capsule endoscope, comprising:

the acquisition module is used for acquiring biaxial rotation data of the wireless capsule endoscope measured by an inertial sensor built in the wireless capsule endoscope and a measured magnetic field value measured by each magnetic sensor built in the wireless capsule endoscope in the process of controlling an extracorporeal driver to drive the wireless capsule endoscope to move;

the calculation module is used for determining position data of the in-vitro driver and calculating theoretical magnetic field values of a plurality of in-vitro magnetic sources arranged in the in-vitro driver at each magnetic sensor according to the two-axis rotation data and the position data, wherein the plurality of in-vitro magnetic sources comprise in-vitro permanent magnets, first electromagnetic coils and second electromagnetic coils, and unit magnetic moments of the in-vitro permanent magnets, the first electromagnetic coils and the second electromagnetic coils are orthogonal in pairs;

the separation module is used for separating the measured magnetic field values measured by each magnetic sensor to obtain separated magnetic field values of each in-vitro magnetic source at each magnetic sensor;

and the positioning module is used for positioning the wireless capsule endoscope based on the theoretical magnetic field value and the separation magnetic field value and outputting the real-time six-dimensional pose of the wireless capsule endoscope.

A third aspect of the application provides a computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, the processor when executing the computer program implementing the following operations:

in the process of controlling an external driver to drive a wireless capsule endoscope to move, acquiring biaxial rotation data of the wireless capsule endoscope measured by an inertial sensor built in the wireless capsule endoscope and a measured magnetic field value measured by each magnetic sensor built in the wireless capsule endoscope;

determining position data of the in-vitro driver, and calculating theoretical magnetic field values of a plurality of in-vitro magnetic sources arranged in the in-vitro driver at each magnetic sensor according to the two-axis rotation data and the position data, wherein the plurality of in-vitro magnetic sources comprise an in-vitro permanent magnet, a first electromagnetic coil and a second electromagnetic coil, and unit magnetic moments of the in-vitro permanent magnet, the first electromagnetic coil and the second electromagnetic coil are orthogonal in pairs;

A fourth aspect of the present application provides a computer-readable storage medium storing a computer program which, when executed by a processor, performs the operations of:

determining position data of the in-vitro driver, and calculating theoretical magnetic field values of a plurality of in-vitro magnetic sources arranged in the in-vitro driver at each magnetic sensor according to the two-axis rotation data and the position data, wherein the plurality of in-vitro magnetic sources comprise in-vitro permanent magnets, first electromagnetic coils and second electromagnetic coils, and unit magnetic moments of the in-vitro permanent magnets, the first electromagnetic coils and the second electromagnetic coils are orthogonal in pairs;

A fifth aspect of the present application provides a computer program product which, when run on a computer, causes the computer to perform the following:

Compared with the prior art, the method has the following advantages:

the positioning system provided by the application uses a composite extracorporeal driver based on a permanent magnet-electromagnetic coil and combines a plurality of magnetic sensors arranged in a capsule, and is used for positioning the wireless capsule endoscope under magnetic drive in six degrees of freedom in real time. The utility model provides a positioning system only adopts the inertial sensor of a miniature low-power consumption and the three magnetic sensor who installs with special arrangement mode in that the capsule is inside, need not set up the sensor in vitro and can carry out the real-time six-dimensional location of capsule, and the power consumption is less than general inside positioning mechanism, has reduced the volume and the weight of capsule, has improved battery utilization ratio. The working space can be expanded to the whole working space of the mechanical arm, and the limitation of the installation position of the external sensor is avoided.

Secondly, among the positioning system that this application provided, the installation mode that external drive adopted two orthogonal solenoid to cover a permanent magnet, simple structure and easily installation compare in other schemes that use external magnetic sensor more nimble, help providing bigger working space, and do not require that the person of being examined puts into a fixed sensor array structure, improved the accuracy and the stability of location.

Third, among the positioning system that this application provided, hold a spherical external drive by a certain end of arm, control the motion of capsule in vivo through changing the interior permanent magnet magnetic field of external drive, the device is simple and easily install, compare in the drive arrangement who only relies on solenoid lighter and more exquisite light, can provide bigger workspace, the spherical surface of external drive more be fit for with patient's body surface contact, reduce safe risk.

Fourthly, a mathematical model is established for the magnetic fields of the permanent magnet and the electromagnetic coil in the external driver and the magnetic field of the passive permanent magnet in the capsule, a magnetic field is generated based on an external magnetic source to control the capsule, a magnetic sensor arranged in the capsule is used for sensing the magnetic field, an inertial sensor arranged in the capsule is used for sensing the rotation of the capsule, and a signal processing algorithm and a nonlinear optimization algorithm are used for carrying out real-time six-dimensional pose positioning on the capsule. The positioning algorithm is not influenced by a driving magnetic field, the positioning result is the six-dimensional pose of the capsule relative to a fixed world coordinate system, the capsule positioning method is more visual and convenient to use, and higher positioning frequency can be realized.

Drawings

In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings used in the embodiments or the description of the prior art will be briefly described below. It is obvious that the drawings in the following description are only some embodiments of the application, and that for a person skilled in the art, other drawings can be derived from them without inventive effort.

FIG. 1 is a schematic diagram of a positioning system of a wireless capsule endoscope provided by an embodiment of the present application;

FIG. 2 is a schematic diagram of the internal structure of a wireless capsule endoscope provided by an embodiment of the present application;

fig. 3 is a schematic diagram illustrating an arrangement of a plurality of magnetic sensors built in a wireless capsule endoscope according to an embodiment of the present application;

FIG. 4 is a schematic diagram of an extracorporeal drive provided in an embodiment of the present application;

FIG. 5 is a schematic diagram of a positioning algorithm for a wireless capsule endoscope provided by an embodiment of the present application;

FIG. 6 is a schematic diagram of a magnetic sensor built in a wireless capsule endoscope measuring a magnetic field generated by an external magnetic source according to an embodiment of the present application;

FIG. 7 is a schematic diagram of a measured magnetic field value separation algorithm provided by an embodiment of the present application;

FIG. 8 is a schematic view of a positioning device of a wireless capsule endoscope provided by an embodiment of the present application;

fig. 9 is a schematic diagram of a computer device according to an embodiment of the present application.

Detailed Description

In the following description, for purposes of explanation and not limitation, specific details are set forth, such as particular system structures, techniques, etc. in order to provide a thorough understanding of the embodiments of the present application. However, it will be apparent to one skilled in the art that the present application may be practiced in other embodiments that depart from these specific details. In other instances, detailed descriptions of well-known systems, devices, circuits, and methods are omitted so as not to obscure the description of the present application with unnecessary detail.

The technical solution of the present application will be described below by way of specific examples.

Referring to fig. 1, a schematic diagram of a positioning system of a wireless capsule endoscope provided by an embodiment of the present application is shown, which may include a table, a robotic arm, a wireless capsule endoscope, an extracorporeal driver, and a computer device. Wherein:

(1) Examination bed

The examination bed can be used for the patient to lie on.

(2) Mechanical arm

The robotic arm may be located near the examination couch. The robotic arm includes an end effector that may be used to mount an extracorporeal drive. Rigid connection can be adopted between the end effector of the mechanical arm and the extracorporeal driver. The structures may be rigidly connected, for example, by means of 3D printing. In an embodiment of the present application, a world coordinate system { W } may be defined based on the robot base center, as shown in FIG. 1.

(3) Wireless capsule endoscope

After swallowed by a patient into the body, the wireless capsule endoscope can be driven by an external driver so as to move in the cavity of the alimentary canal and the like and acquire images of the internal environment for diagnosis by computer equipment or doctors. Fig. 2 is a schematic view of an internal structure of a wireless capsule endoscope according to an embodiment of the present application. The wireless capsule endoscope shown in fig. 2 has a capsule permanent magnet, an inertial sensor (IMU), and a plurality of magnetic sensors built therein, in addition to a conventional camera module, an illumination module, a wireless communication module, a microprocessor module, and a button battery. Wherein, the capsule permanent magnet can be a cylindrical permanent magnet, and the capsule permanent magnet can be magnetized along the axial direction of the capsule and is consistent with the z-axis direction of the inertial sensor. The inertial sensor may be used to estimate two-degree-of-freedom rotation of the capsule in the human body relative to a fixed world coordinate system, resulting in biaxial rotation data (α, β). The number of magnetic sensors shown in fig. 2 is three, and the three magnetic sensors can be installed in a wireless capsule endoscope in a specific arrangement for four-degree-of-freedom pose estimation of the capsule (three-dimensional position and orientation angle γ of the capsule) in the human body. A capsule coordinate system C may be constructed based on the wireless capsule endoscope of fig. 2.

As can be seen from FIG. 1, the world coordinate system { W } defined in the embodiments of the present application is the center of the base of the robot arm. Because the pose of the extracorporeal driver relative to the fixed world coordinate system { W } can be easily and accurately measured in real time through the mechanical arm kinematics model, the pose of the wireless capsule endoscope relative to the world coordinate system { W } can be calculated as long as the pose of the wireless capsule endoscope relative to the extracorporeal driver is obtained. Based on the analysis, the embodiment of the application solves the pose of the in-vivo wireless capsule endoscope relative to the in-vitro driver by installing the magnetic sensor in the wireless capsule endoscope and measuring the magnetic field generated by the in-vitro magnetic source at the position of the capsule by the magnetic sensor.

As can be seen from fig. 1 and 2, the wireless capsule endoscope incorporates a capsule permanent magnet for a capsule driving function. During the examination process, the capsule permanent magnet also generates a magnetic field and is sensed by a magnetic sensor in the capsule. In order to accurately measure the magnetic field generated by the in-vitro magnetic field by using the magnetic sensors in the capsule, the embodiment of the application is specifically designed for the arrangement mode of each magnetic sensor in the capsule, so that the magnetic field generated by the capsule permanent magnet cannot influence the measurement of the magnetic sensor on the magnetic field generated by the in-vitro magnetic source. That is, according to the embodiment of the present application, by designing the arrangement of the plurality of magnetic sensors in the capsule, the component of the magnetic field generated by the capsule permanent magnet on the plurality of magnetic sensors installed in a specific arrangement may be zero.

Fig. 3 is a schematic diagram illustrating an arrangement of a plurality of magnetic sensors built in a wireless capsule endoscope according to an embodiment of the present application, and fig. 3 also illustrates an example of a magnetic field generated by a cylindrical capsule permanent magnet. The magnetic sensors installed in the capsule may include a first magnetic sensor, a second magnetic sensor, and a third magnetic sensor. As shown in FIG. 3, at any position on the xy-plane of the capsule coordinate system { C }, the vector of the magnetic field generated by the capsule permanent magnet is perpendicular to the xy-plane. Thus, a point can be determined on the x-axis of the capsule coordinate system { C }, and a magnetic sensor is mounted perpendicular to the x-axis, and the magnetic sensor measures the magnetic field component b of the permanent magnet of the capsule _cx And =0. That is, the first magnetic sensor may be mounted at one point a on the x-axis of the capsule coordinate system { C } shown in fig. 3 and perpendicular to the x-axis direction of the capsule coordinate system. Similarly, a point can be determined on the y-axis of the capsule coordinate system { C }, and a magnetic sensor is arranged in the direction perpendicular to the y-axis and used for measuring the magnetic field component b of the capsule permanent magnet _cy And =0. That is, the second magnetic sensor may be mounted at one point B on the y-axis of the capsule coordinate system { C } shown in fig. 3 and perpendicular to the y-axis direction of the capsule coordinate system. At a location where the slope of the tangent to the induction line is 0, such as at point C in FIG. 3, the magnetic field vector produced by the capsule permanent magnet contains only a component along the xy plane and zero along the z-axis. Thus, canA magnetic sensor is arranged at the point C and perpendicular to the z-axis direction, and the magnetic sensor measures the magnetic field component b of the permanent magnet of the capsule _cz And =0. That is, the third magnetic sensor may be attached to a point C where the tangential gradient of the magneto-inductive line generated by the capsule permanent magnet is 0 and perpendicular to the z-axis direction of the capsule coordinate system { C } as shown in fig. 3.

By installing three magnetic sensors in the specific arrangement mode at A, B, C three points around the capsule permanent magnet built in the wireless capsule endoscope, the components of the magnetic field generated by the external magnetic source in the directions of the x, y and z axes of the capsule coordinate system { C } can be respectively measured without being influenced by the magnetic field generated by the capsule permanent magnet.

In the embodiment of the application, the positions and normal vectors of three magnetic sensors in a capsule coordinate system { C } are respectively assumed to be

The six-dimensional position of the wireless capsule endoscope in the world coordinate system W can be represented by a three-dimensional position

And a three-dimensional rotation matrix R _c And epsilon SO (3). Then, the position and normal vector direction of the ith magnetic sensor in the world coordinate system { W } can be expressed as:

and

(4) Extracorporeal driver

The extracorporeal drive may be mounted to the end effector of the robotic arm and above the examination table. Fig. 4 is a schematic diagram of an extracorporeal driver according to an embodiment of the present disclosure. The extracorporeal driver may include a plurality of extracorporeal magnetic sources, such as an extracorporeal permanent magnet, a first electromagnetic coil, and a second electromagnetic coil shown in fig. 4 (a). Wherein the axes of the first electromagnetic coil and the second electromagnetic coil are orthogonal. After the first electromagnetic coil and the second electromagnetic coil are energized,the first and second electromagnetic coils may each generate a corresponding electromagnetic field. The external permanent magnet can be a cylindrical permanent magnet, and the external permanent magnet can be magnetized along the axial direction of the external permanent magnet and is orthogonal to the rotation axis direction of the tail end joint of the mechanical arm. As shown in fig. 4 (a), the external permanent magnet may be nested inside the first and second electromagnetic coils. Based on the external driver, a driver coordinate system { A } can be formed, and the unit magnetic moments of the external permanent magnet, the first electromagnetic coil and the second electromagnetic coil can be respectively parallel to the axial directions of the driver coordinate system { A }. For example, in (a) in fig. 4, an example is also shown in a view in which the unit magnetic moments of the respective external magnetic sources are in different directions from the respective axis directions of the driver coordinate system { a }. Wherein, the unit magnetic moment of the external permanent magnet

Unit magnetic moment of the first electromagnetic coil parallel to the z-axis of the driver coordinate system { A }

Unit magnetic moment of second electromagnetic coil parallel to y-axis of driver coordinate system { A }

Parallel to the x-axis of the drive coordinate system { a }. Thus, the unit magnetic moments of the external permanent magnet, the first electromagnetic coil and the second electromagnetic coil are orthogonal in pairs. In the embodiment of the present application, the outer casing of the extracorporeal driver may be spherical, and the outer casing may be made of plastic. Therefore, in the process of controlling the extracorporeal driver, the risk of causing additional injury to the patient due to the fact that the extracorporeal driver contacts the body of the patient during examination can be reduced.

Fig. 4 (b) is a schematic diagram of the magnetic field generated by the extracorporeal magnetic source shown in fig. 4 (a). Because the magnetic field generated by the single magnetic source is in cylindrical symmetry around the self magnetic moment direction, multiple solutions can occur when the capsule is positioned by using the measured value of the magnetic field generated by the single magnetic source. For example, assume that the true position of the capsule is as shown in FIG. 4 (b), but the magnetic moment direction of a single magnetic source

When the measured magnetic field value is used to locate the capsule in four degrees of freedom (the three-dimensional position and the orientation angle γ of the capsule) parallel to the z-axis of the world coordinate system { W }, an infinite number of solutions are generated that are cylindrically symmetric about the magnetic moment direction of the magnetic source, and any point on the circle shown in fig. 4 (c) can be considered as the position of the solved capsule. When using two magnetic sources with orthogonal magnetic moment directions (e.g. using a magnetic field of opposite polarity)

) When positioning is performed, since the cylindrical symmetry is broken, two solutions about mirror symmetry can be obtained, such as two dots a and b on a circle shown in (c) in fig. 4, and the positions corresponding to the two dots a and b can be regarded as the positions of the solved capsules. Using three magnetic sources with orthogonal magnetic moments (e.g. magnetic flux-generating devices)

) When positioning is performed, because the three magnetic fields with cylindrical symmetry are not symmetrical after being superposed, a unique solution of the four-degree-of-freedom pose of the capsule can be obtained according to the magnetic field values measured by the three magnetic sources independently, for example, a circular point b on a circle shown in (c) in fig. 4, and the position corresponding to the circular point b can be regarded as the solved position of the capsule. By using the magnetic sources with the three magnetic moments orthogonal in pairs for positioning, a stable, accurate and unique positioning result can be obtained theoretically. Thus, the embodiments of the present application use three sources of external magnetic energy to design the extracorporeal drive.

(5) Computer equipment

The computer device may be a general purpose computer located near the examination table and in which programs and algorithms relating to magnetic actuation and positioning may be stored. During examination, the computer equipment can be operated by a worker or a doctor so as to send a control command to the mechanical arm, collect position data of the mechanical arm and the extracorporeal driver, magnetic field data measured by each magnetic sensor in the wireless capsule endoscope and attitude data measured by the inertial sensor. The computer equipment can operate a positioning algorithm according to the acquired data, and calculate and visualize the real-time six-dimensional pose of the capsule in vivo.

Referring to fig. 5, which shows a schematic diagram of a positioning algorithm of a wireless capsule endoscope provided in an embodiment of the present application, the computer device shown in fig. 1 can output an accurate six-dimensional pose of the wireless capsule endoscope in real time by executing the algorithm flow shown in fig. 5. Specifically, in the embodiment of the present application, the computer device may calculate the theoretical magnetic field value of each magnetic sensor of the three external magnetic sources in the external driver by using the magnetic dipole model through the direction angle data measured in real time by the inertial sensor built in the capsule and the position data of the external driver connected to the end fed back by the mechanical arm. Meanwhile, the computer equipment can separate and extract the measured magnetic field values of the three in-vitro magnetic sources in the in-vitro driver through the sampling data of the current on the electromagnetic coil and the magnetic field data (measured magnetic field values) obtained by the magnetic sensor in the capsule within a period of time, and solve unknown parameters in the capsule pose by fitting the theoretical magnetic field value and the measured magnetic field value of each magnetic source through nonlinear optimization, so that the capsule is driven in a human body and meanwhile obtains real-time six-dimensional pose estimation.

The following describes in detail a process of outputting an accurate six-dimensional pose of the wireless capsule endoscope in real time by the computer device executing the algorithm flow with reference to the algorithm flow shown in fig. 5.

As shown in fig. 5, the computer device may first acquire biaxial rotation data of the wireless capsule endoscope measured by the inertial sensor in the process of controlling the extracorporeal driver to drive the wireless capsule endoscope to move. The computer device may then determine positional data for the extracorporeal drive and calculate theoretical magnetic field values for the plurality of extracorporeal magnetic sources at each of the magnetic sensors based on the biaxial rotation data and the positional data. The position data of the extracorporeal drive may be position data of the extracorporeal drive in a preset world coordinate system { W }.

In particular implementations, the computer device may determine that each magnetic sensor is within a wireless-based capsule separatelyPosition and normal vector direction under capsule coordinate system { C } formed by sight glass

And then according to the biaxial rotation data and the position and normal vector direction of each magnetic sensor in the capsule coordinate system C

The position and normal vector direction of each magnetic sensor in the world coordinate system W are determined.

With reference to fig. 1, the six-dimensional pose of the wireless capsule endoscope in the world coordinate system (W) can be represented by three-dimensional positions

And a three-dimensional rotation matrix R _c And epsilon SO (3). Wherein R is _c The angle that can be rotated by the capsule coordinate system { C } sequentially around the x, y, z axes of the world coordinate system { W } is represented as:

R _c ＝Rot(z，γ)Rot(y，β)Rot(x，α)

the angle (alpha, beta) of the capsule coordinate system { C } which rotates around the x axis and the y axis of the world coordinate system { W } in turn is the biaxial rotation data of the wireless capsule endoscope, and the biaxial rotation data can be accurately measured in real time by an inertial sensor arranged in the capsule. Therefore, in the process of solving the six-dimensional pose of the capsule, the unknown quantity is the three-dimensional position p _c And the direction angle gamma, four unknown parameters.

Since the extracorporeal drive of known size is rigidly connected to the end effector of the robot arm and the pose of the end of the robot arm can be estimated in real time by a known kinematic model of the robot arm, the six-dimensional pose (p) of the extracorporeal drive can be considered _a ，R _a ) Can be accurately measured in real time. Assuming that the center positions of the external permanent magnet, the first electromagnetic coil and the second electromagnetic coil built in the external actuator are all located, and the magnetic moment directions of the three external magnetic sources are respectively coincident with the z, y and x axes of the actuator coordinate system { a }, the unit magnetic moment directions of the three external magnetic sources can be expressed as:

according to the foregoing description, the location of the ith magnetic sensor within the capsule in the world coordinate system { W } can be expressed as

The normal vector direction can be expressed as

The computer device may be based on the position of each magnetic sensor in the world coordinate system { W } as described above

Sum normal vector direction

The theoretical magnetic field value of each in vitro magnetic source at each magnetic sensor is calculated.

According to the magnetic dipole model, the theoretical magnetic field value b of three in-vitro magnetic sources in the in-vitro driver on the ith magnetic sensor in the capsule _i，a1 ，b _i，a2 ，b _i，a3 Can be induced by a magnetic field at s _i The projection in the direction is obtained. The theoretical magnetic field values of the three in-vitro magnetic sources on the three magnetic sensors can be respectively expressed as a vector b _a1 (p _c ，γ)，b _a2 (p _c ，γ)，b _a3 (p _c ，γ)：

That is, the computer device may calculate, based on the magnetic dipole model and the position and normal vector direction of each magnetic sensor in the world coordinate system { W }, a projection of the magnetic field generated by each in-vitro magnetic source in the normal vector direction of each magnetic sensor in the world coordinate system { W }, resulting in a theoretical magnetic field value of each in-vitro magnetic source at each magnetic sensor.

On the other hand, in the process of controlling the extracorporeal driver to drive the wireless capsule endoscope to move, the computer device can also acquire the measured magnetic field value measured by each magnetic sensor built in the capsule.

Fig. 6 is a schematic diagram illustrating a magnetic sensor built in a wireless capsule endoscope according to an embodiment of the present application measuring a magnetic field generated by an external magnetic source. Because the extracorporeal driver comprises three extracorporeal magnetic sources including the extracorporeal permanent magnet, the first electromagnetic coil and the second electromagnetic coil, the magnetic field data (measured magnetic field value) measured by each magnetic sensor in the capsule is the superposition result of the magnetic fields generated by the three extracorporeal magnetic sources. For example, the magnitude of the magnetic field generated by the external permanent magnet, the first electromagnetic coil and the second electromagnetic coil at a certain magnetic sensor built in the capsule is b _a1 、b _a2 、b _a3 Then, the magnetic field measured by the magnetic sensor should be the superposition result of the magnetic fields generated by the three in-vitro magnetic sources, namely: b = b _a1 +b _a2 +b _a3 . Therefore, the magnetic field values of the three in-vitro magnetic sources need to be accurately detected and separated from the synthesized magnetic field acquired by the magnetic sensors, that is, the measured magnetic field values measured by each magnetic sensor need to be separated, so as to obtain the separated magnetic field value of each in-vitro magnetic source at each magnetic sensor, so as to be used for a subsequent positioning algorithm.

As shown in fig. 5, the computer device separates the measured magnetic field values measured by each magnetic sensor within the capsule, which may be implemented in conjunction with current measurements on the electromagnetic coils.

In a specific implementation, the computer device may first control the passage of periodic square wave currents of different frequencies to the first and second electromagnetic coils, respectively. For example, the computer device may control the passage of a first frequency f to the first electromagnetic coil ₁ Periodic square wave current I ₁ So that the first electromagnetic coil generates the first frequency f ₁ A periodic time-varying magnetic field of (a); at the same time, the computer device may control the passage of the second frequency f to the second electromagnetic coil ₂ Periodic square wave current I ₂ So that the second electromagnetic coil generates the second frequency f ₂ A periodically time-varying magnetic field.

Fig. 7 is a schematic diagram of a measured magnetic field value separation algorithm according to an embodiment of the present application. According to the algorithm shown in fig. 7, the computer device may sample the currents on the first and second electromagnetic coils and the measured magnetic field values on each magnetic sensor, obtaining current sampling results and measured magnetic field value sampling results. For the current sampling result, after performing fast Fourier transform, the frequency f corresponding to the first frequency can be obtained ₁ And a second frequency f ₂ Current phase estimation of (2).

Since the extracorporeal drive can be considered to be in the disabled state approximately for a very short sampling time and the magnetic field generated by the extracorporeal permanent magnet can be considered to be a constant magnetic field, a time average can be calculated by sampling the measured magnetic field values and taken as an estimated value of the extracorporeal permanent magnet, i.e., a separated magnetic field value of the magnetic field generated by the extracorporeal permanent magnet at the corresponding magnetic sensor, as shown in fig. 7. After the estimated value of the in-vitro permanent magnet is subtracted from the sampling result of the measured magnetic field value of each magnetic sensor, the time sequence of the sum of the measured magnetic field values of the first electromagnetic coil and the second electromagnetic coil can be obtained.

In order to further separate the measured magnetic field values of the first and second electromagnetic coils, the computer device may perform a fast fourier transformation of the time series of the sum of the measured magnetic field values of the first and second electromagnetic coils, which may result in a frequency f corresponding to the first frequency ₁ And a second frequency f ₂ Phase estimation and amplitude estimation. The signs of the measured magnetic field values of the first and second electromagnetic coils may be determined from a current phase estimate obtained from the current sampling result and a phase estimate obtained from a time series of the sum of the measured magnetic field values of the first and second electromagnetic coils. The separated magnetic field values of the first electromagnetic coil and the second electromagnetic coil on each magnetic sensor can be obtained by multiplying the amplitude estimation of the first electromagnetic coil and the second electromagnetic coil with the sign of the measured magnetic field value respectively. To this end, three external magnetic sources built in the external driver measure magnetism on three magnetic sensorsThe field values are separated.

The above-described separation process for measuring magnetic field values is described in detail below based on the algorithm shown in fig. 7.

First, with reference to FIG. 6, the computer device may control the energizing of the first electromagnetic coil at a first frequency f ₁ Periodic square wave current I ₁ Energizing a second electromagnetic coil at a second frequency f ₂ Periodic square wave current I ₂ . Then, the computer device can sample the current on the first electromagnetic coil and the second electromagnetic coil respectively to obtain the current sampling results of the first electromagnetic coil and the second electromagnetic coil; by performing fast Fourier transform on the current sampling results, the first frequency f can be obtained ₁ And a second frequency f ₂ A corresponding current phase estimate comprising a first current phase estimate of the first electromagnetic coil

And second current phase estimation of the second electromagnetic coil

For measuring the magnetic field value, the computer device may measure the magnetic field value b measured by each magnetic sensor within the preset time period T according to the preset sampling frequency f _i And i belongs to the field 1,2,3 for sampling, and respectively obtaining a measured magnetic field value sampling sequence corresponding to each magnetic sensor. The preset sampling frequency f meets the following conditions: f > 5f ₁ ，f＞5f ₂ (ii) a The preset time period T meets the following conditions: t is more than 1/f ₁ ，T＞1/f ₂ 。

The computer device can calculate the time average value of each sample sequence of measured magnetic field values respectively:

the time average value can be used as a separated magnetic field value of the external permanent magnet at the corresponding magnetic sensor.

After the time average value of the corresponding magnetic sensor is subtracted from each sampling sequence of the measured magnetic field values, a time sequence of the sum of the measured magnetic field values of the magnetic fields generated by the first electromagnetic coil and the second electromagnetic coil at each magnetic sensor can be obtained:

time series of computer equipment based on the sum of the measured magnetic field values

And separating the measured magnetic field values generated by the first electromagnetic coil and the second electromagnetic coil to obtain separated magnetic field values of the first electromagnetic coil and the second electromagnetic coil at each magnetic sensor.

In particular, the time series of the above-mentioned sums of measured magnetic field values can be

Performing a fast Fourier transform to obtain a phase estimate and an amplitude estimate, the phase estimate comprising a first phase estimate of the first electromagnetic coil

And second phase estimation of the second electromagnetic coil

The amplitude estimate comprises a first amplitude estimate of the first solenoid coil | | b' _i，a2 | | and second amplitude estimate of the second solenoid coil | | b' _i，a3 L. The computer device may calculate the separated magnetic field values of the first and second electromagnetic coils at each magnetic sensor based on the calculated current phase, phase and amplitude estimates described above.

Wherein the absolute values of the phase differences of the first and second electromagnetic coils may be calculated from the current phase estimation and the phase estimation, respectively. For example, the absolute value of the phase difference of the first electromagnetic coil may be expressed as:

the absolute value of the phase difference of the second electromagnetic coil may be expressed as:

from the absolute value of the phase difference, the signs of the measured magnetic field values of the first and second electromagnetic coils can be determined. Specifically, if the absolute value of the phase difference is 0 to 0.5 _π Or 1.5 pi-2 pi, the measured magnetic field value symbol of the coil can be determined to be positive; if the absolute value of the phase difference is 0.5 _π ～1.5 _π And the measured magnetic field value sign of the coil is negative. Namely: if the absolute value of the phase difference of the first electromagnetic coil is between 0-0.5 pi or 1.5 pi-2 pi, the sign of the measured magnetic field value of the first electromagnetic coil can be determined to be positive; if the absolute value of the phase difference of the first electromagnetic coil is between 0.5 pi and 1.5 pi, the sign of the measured magnetic field value of the first electromagnetic coil can be determined to be negative; similarly, if the absolute value of the phase difference of the second electromagnetic coil is between 0-0.5 pi or 1.5 pi-2 pi, the symbol of the measured magnetic field value of the second electromagnetic coil can be determined to be positive; if the absolute value of the phase difference of the second electromagnetic coil is between 0.5 pi and 1.5 pi, the sign of the measured magnetic field value of the second electromagnetic coil can be determined to be negative.

The separated magnetic field values of the first and second solenoids at each magnetic sensor may be obtained by multiplying the measured magnetic field value symbols by the corresponding amplitude estimates, respectively. Thus, the measured magnetic field values of the three extracorporeal magnetic sources built in the extracorporeal drive on the three magnetic sensors are separated into:

after separation of the measured magnetic field values of the plurality of in-vitro magnetic sources is completed and the separated magnetic field value of each in-vitro magnetic source at each magnetic sensor is obtained, as shown in fig. 5, the wireless capsule endoscope can be positioned based on the theoretical magnetic field value and the separated magnetic field value, unknown parameters of the capsule pose are optimized and solved, and the real-time six-dimensional pose of the wireless capsule endoscope is output.

In the embodiment of the application, the unknown parameters in the six-dimensional pose of the wireless capsule endoscope can be solved by adopting a least square method based on the theoretical magnetic field value and the separated magnetic field value.

As previously mentioned, the real-time six-dimensional pose of a wireless capsule endoscope is defined by three-dimensional position

And a three-dimensional rotation matrix R _c E.g. SO (3), wherein the three-dimensional rotation matrix R _c The two-axis rotation data in the e SO (3) is the angle (alpha, beta) of rotation around the x-axis and the y-axis of the preset world coordinate system { w } respectively based on the capsule coordinate system { C } formed by the wireless capsule endoscope, which can be directly measured by the inertial sensor. Therefore, the unknown parameters that need to be solved optimally include the three-dimensional position of the wireless capsule endoscope

And the capsule coordinate system { C } rotates around the z-axis of the world coordinate system { w }, and the unknown parameters and the biaxial rotation data (alpha, beta) jointly form the real-time six-dimensional pose of the wireless capsule endoscope.

In a specific implementation, the unknown parameter p is _c γ can be solved by non-linear least squares optimization, i.e. minimizing the sum of the squares of the errors between the separated magnetic field values measured by the plurality of magnetic sensors and the theoretical magnetic field values:

in order to solve the above nonlinear least squares estimation problem, the above equation can be solved by using L-M (Levenberg-Marquardt) algorithm to obtain p _c And gamma, so as to obtain the accurate six-dimensional pose of the infinite capsule endoscope in the human body.

In the embodiment of the present application, since the direction angle γ is a function with a period of 2 π, after solving the direction angle γ, it can be transformed into a main period of (- π, π).

The positioning system provided by the embodiment of the application uses a composite extracorporeal driver based on a permanent magnet-electromagnetic coil and is combined with a plurality of magnetic sensors arranged in a capsule, and is used for positioning the wireless capsule endoscope under magnetic drive in six degrees of freedom in real time. The positioning system that this application embodiment provided only adopts the inertial sensor of a miniature low-power consumption and the three magnetic sensor who installs with special arrangement mode in that the capsule is inside, need not set up the sensor in vitro and can carry out the real-time six-dimensional location of capsule, and the power consumption is less than general inside positioning mechanism, has reduced the volume and the weight of capsule, has improved battery utilization ratio. The working space can be expanded to the whole working space of the mechanical arm, and the limitation of the installation position of the external sensor is avoided.

Secondly, in the positioning system that this application embodiment provided, the installation that external drive adopted two orthogonal solenoid to cover a permanent magnet mode, simple structure and easily installation compare in other schemes that use outside magnetic sensor more nimble, help providing bigger workspace, and do not need to put into a fixed sensor array structure by the examinee, improved the accuracy and the stability of location.

Thirdly, in the positioning system provided by the embodiment of the application, a spherical external actuator is held by a certain end of a mechanical arm, the capsule movement in the body is controlled by changing the magnetic field of a permanent magnet in the external actuator, the device is simple and easy to install, and compared with a driving device which only depends on an electromagnetic coil, the device is smaller and lighter, a larger working space can be provided, and the spherical surface of the external actuator is more suitable for being in contact with the body surface of a patient, so that the safety risk is reduced.

Fourthly, a mathematical model is established for the magnetic fields of the permanent magnet and the electromagnetic coil in the external driver and the passive permanent magnet in the capsule, a magnetic field is generated based on an external magnetic source to control the capsule, a magnetic sensor arranged in the capsule is used for sensing the magnetic field, an inertial sensor arranged in the capsule is used for sensing the rotation of the capsule, and a signal processing algorithm and a nonlinear optimization algorithm are used for carrying out real-time six-dimensional pose positioning on the capsule. The positioning algorithm is not influenced by a driving magnetic field, the positioning result is the six-dimensional pose of the capsule relative to a fixed world coordinate system, the capsule positioning method is more visual and convenient to use, and higher positioning frequency can be realized.

Referring to fig. 8, a schematic diagram of a positioning apparatus of a wireless capsule endoscope provided in an embodiment of the present application is shown, and specifically may include an acquisition module 801, a calculation module 802, a separation module 803, and a positioning module 804, where:

an acquiring module 801, configured to acquire biaxial rotation data of the wireless capsule endoscope measured by an inertial sensor built in the wireless capsule endoscope and a measured magnetic field value measured by each magnetic sensor built in the wireless capsule endoscope during a process of controlling an extracorporeal driver to drive the wireless capsule endoscope to move;

a calculating module 802, configured to determine position data of the in-vitro driver, and calculate a theoretical magnetic field value of each magnetic sensor of a plurality of in-vitro magnetic sources built in the in-vitro driver according to the two-axis rotation data and the position data, where the plurality of in-vitro magnetic sources include an in-vitro permanent magnet, a first electromagnetic coil, and a second electromagnetic coil, and unit magnetic moments of the in-vitro permanent magnet, the first electromagnetic coil, and the second electromagnetic coil are orthogonal to each other;

a separating module 803, configured to separate the measured magnetic field values measured by each of the magnetic sensors, so as to obtain a separated magnetic field value of each of the in-vitro magnetic sources at each of the magnetic sensors;

and the positioning module 804 is used for positioning the wireless capsule endoscope based on the theoretical magnetic field value and the separation magnetic field value and outputting the real-time six-dimensional pose of the wireless capsule endoscope.

In the embodiment of the application, the plurality of magnetic sensors are installed in the wireless capsule endoscope in a specific arrangement, and the component of the magnetic field generated by the capsule permanent magnet on the plurality of magnetic sensors installed in the specific arrangement is zero.

In an embodiment of the present application, the plurality of magnetic sensors include a first magnetic sensor, a second magnetic sensor, and a third magnetic sensor, the capsule permanent magnet may be a cylindrical permanent magnet, the capsule permanent magnet is magnetized in an axial direction of the capsule itself, a capsule coordinate system is formed based on the wireless capsule endoscope, the first magnetic sensor is installed at a point on an x-axis of the capsule coordinate system and perpendicular to an x-axis direction of the capsule coordinate system, the second magnetic sensor is installed at a point on a y-axis of the capsule coordinate system and perpendicular to a y-axis direction of the capsule coordinate system, and the third magnetic sensor is installed at a point where a tangential slope of a magnetic induction line generated by the capsule permanent magnet is 0 and perpendicular to a z-axis direction of the capsule coordinate system.

In this embodiment, the axes of the first electromagnetic coil and the second electromagnetic coil are orthogonal, the external permanent magnet may be a cylindrical permanent magnet, and is magnetized along the axial direction of the external permanent magnet, the external permanent magnet is nested in the first electromagnetic coil and the second electromagnetic coil, based on the external driver forms a driver coordinate system, and the unit magnetic moments of the external permanent magnet, the first electromagnetic coil and the second electromagnetic coil are respectively parallel to the axial directions of the driver coordinate system.

In this application embodiment, the shell shape of external drive ware is spherical, the shell material of external drive ware can be the plastics material, external drive ware is installed on the end effector of arm, the direction of magnetizing of external permanent magnet with arm end joint rotation axis direction quadrature.

In this embodiment of the application, the position data of the extracorporeal drive is position data of the extracorporeal drive in a preset world coordinate system, and the calculation module 802 may be specifically configured to: respectively determining the position and normal vector direction of each magnetic sensor in a capsule coordinate system formed based on the wireless capsule endoscope; determining the position and normal vector direction of each magnetic sensor in the world coordinate system according to the two-axis rotation data and the position and normal vector direction of each magnetic sensor in the capsule coordinate system; and calculating a theoretical magnetic field value of each in-vitro magnetic source at each magnetic sensor based on the position and normal vector direction of each magnetic sensor in the world coordinate system.

In this embodiment, the calculating module 802 may be further configured to: and calculating the projection of the magnetic field generated by each in-vitro magnetic source on the normal vector direction of each magnetic sensor in the world coordinate system based on the magnetic dipole model and the position and the normal vector direction of each magnetic sensor in the world coordinate system to obtain the theoretical magnetic field value of each in-vitro magnetic source at each magnetic sensor.

In this embodiment, the separation module 803 may be specifically configured to: controlling a periodic square wave current of a first frequency to the first electromagnetic coil to cause the first electromagnetic coil to generate a periodic time-varying magnetic field of the first frequency; and controlling a periodic square wave current of a second frequency to be supplied to the second electromagnetic coil so as to enable the second electromagnetic coil to generate a periodic time-varying magnetic field of the second frequency; sampling the measured magnetic field values measured by each magnetic sensor in a preset time period according to a preset sampling frequency to respectively obtain a measured magnetic field value sampling sequence corresponding to each magnetic sensor; respectively calculating the time average value of each measured magnetic field value sampling sequence, and taking the time average value as the separated magnetic field value of the in-vitro permanent magnet at the corresponding magnetic sensor; respectively subtracting the time average value of the corresponding magnetic sensor from each measured magnetic field value sampling sequence to obtain a time sequence of the sum of the measured magnetic field values of the magnetic field generated by the first electromagnetic coil and the second electromagnetic coil at each magnetic sensor; separating the measured magnetic field values generated by the first electromagnetic coil and the second electromagnetic coil based on the time sequence of the sum of the measured magnetic field values to obtain separated magnetic field values of the first electromagnetic coil and the second electromagnetic coil at each magnetic sensor.

In this embodiment of the present application, the separation module 803 may further be configured to: respectively sampling currents on the first electromagnetic coil and the second electromagnetic coil to obtain current sampling results of the first electromagnetic coil and the second electromagnetic coil; performing fast Fourier transform on the current sampling results respectively to obtain current phase estimation, wherein the current phase estimation comprises a first current phase estimation of the first electromagnetic coil and a second current phase estimation of the second electromagnetic coil; performing a fast fourier transform on the time series of sums of the measured magnetic field values to obtain a phase estimate and an amplitude estimate, the phase estimate comprising a first phase estimate for the first electromagnetic coil and a second phase estimate for the second electromagnetic coil, the amplitude estimate comprising a first amplitude estimate for the first electromagnetic coil and a second amplitude estimate for the second electromagnetic coil; calculating a separated magnetic field value of the first and second electromagnetic coils at each of the magnetic sensors based on the current phase estimate, the phase estimate, and the amplitude estimate.

In this embodiment of the present application, the separation module 803 may further be configured to: calculating absolute values of phase differences of the first and second electromagnetic coils, respectively, based on the current phase estimation and the phase estimation; determining the signs of the measured magnetic field values of the first electromagnetic coil and the second electromagnetic coil according to the absolute value of the phase difference; and multiplying the measured magnetic field value symbol by the amplitude estimation respectively to obtain a separated magnetic field value of the first electromagnetic coil and the second electromagnetic coil at each magnetic sensor.

In this embodiment of the present application, the separation module 803 may further be configured to: if the absolute value of the phase difference of the first electromagnetic coil is between 0-0.5 pi or 1.5 pi-2 pi, determining that the symbol of the measured magnetic field value of the first electromagnetic coil is positive; if the absolute value of the phase difference of the first electromagnetic coil is between 0.5 pi and 1.5 pi, determining that the sign of the measured magnetic field value of the first electromagnetic coil is negative; if the absolute value of the phase difference of the second electromagnetic coil is between 0-0.5 pi or 1.5 pi-2 pi, determining that the sign of the measured magnetic field value of the second electromagnetic coil is positive; and if the absolute value of the phase difference of the second electromagnetic coil is between 0.5 pi and 1.5 pi, determining that the sign of the measured magnetic field value of the second electromagnetic coil is negative.

In an embodiment of the present application, the two-axis rotation data is angles of rotation around an x-axis and a y-axis of a preset world coordinate system, respectively, based on a capsule coordinate system formed by the wireless capsule endoscope, and the positioning module 804 may be specifically configured to: and solving unknown parameters in the six-dimensional pose of the wireless capsule endoscope by adopting a least square method based on the theoretical magnetic field value and the separated magnetic field value, wherein the unknown parameters comprise the three-dimensional position of the wireless capsule endoscope and the rotation angle of the capsule coordinate system around the z axis of the world coordinate system, and the unknown parameters and the two-axis rotation data jointly form the real-time six-dimensional pose of the wireless capsule endoscope.

The description of the apparatus embodiment is relatively simple because it is substantially similar to the description of the system embodiment for the computer device, and reference may be made to the description of the system embodiment for relevant points.

Referring to fig. 9, a schematic diagram of a computer device provided in an embodiment of the present application is shown. As shown in fig. 9, a computer apparatus 900 in the embodiment of the present application includes: a processor 910, a memory 920, and a computer program 921 stored in the memory 920 and operable on the processor 910. The processor 910 implements the steps implemented by the computer device in the above system embodiment when executing the computer program 921. Alternatively, the processor 910, when executing the computer program 921, implements the functions of each module/unit in each device embodiment described above, for example, the functions of the modules 801 to 804 shown in fig. 8.

Illustratively, the computer program 921 may be partitioned into one or more modules/units, which are stored in the memory 920 and executed by the processor 910 to accomplish the present application. The one or more modules/units may be a series of computer program instruction segments capable of performing specific functions, which may be used to describe the execution of the computer program 921 in the computer device 900. For example, the computer program 921 may be divided into an acquisition module, a calculation module, a separation module, and a positioning module, and the specific functions of each module are as follows:

In the embodiment of the application, the plurality of magnetic sensors are arranged in the wireless capsule endoscope in a specific arrangement, and the component of the magnetic field generated by the capsule permanent magnet on the plurality of magnetic sensors arranged in the specific arrangement is zero.

The computer device 900 may be the computer device in the foregoing embodiments, and the computer device 900 may be a desktop computer, a cloud server, or other computing devices. The computer device 900 may include, but is not limited to, a processor 910, a memory 920. Those skilled in the art will appreciate that fig. 9 is merely an example of a computer device 900 and is not intended to limit the computer device 900 and may include more or fewer components than those shown, or some of the components may be combined, or different components, e.g., the computer device 900 may also include input output devices, network access devices, buses, etc.

The Processor 910 may be a Central Processing Unit (CPU), other general purpose Processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), an off-the-shelf Programmable Gate Array (FPGA) or other Programmable logic device, discrete Gate or transistor logic device, discrete hardware component, etc. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.

The storage 920 may be an internal storage unit of the computer device 900, such as a hard disk or a memory of the computer device 900. The memory 920 may also be an external storage device of the computer device 900, such as a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) Card, a Flash memory Card (Flash Card), etc. provided on the computer device 900. Further, the memory 920 may also include both internal and external storage units of the computer device 900. The memory 920 is used for storing the computer program 921 and other programs and data required by the computer apparatus 900. The memory 920 may also be used to temporarily store data that has been output or is to be output.

The above-mentioned embodiments are only used for illustrating the technical solutions of the present application, and not for limiting the same. Although the present application has been described in detail with reference to the foregoing embodiments, it should be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; such modifications and substitutions do not substantially depart from the spirit and scope of the embodiments of the present application and are intended to be included within the scope of the present application.

Claims

1. A positioning system for a wireless capsule endoscope, the system comprising a wireless capsule endoscope, an extracorporeal driver, and a computer device communicatively coupled to the wireless capsule endoscope and the extracorporeal driver, respectively; the in-vitro driver is internally provided with a plurality of in-vitro magnetic sources, the plurality of in-vitro magnetic sources comprise in-vitro permanent magnets, first electromagnetic coils and second electromagnetic coils, and unit magnetic moments of the in-vitro permanent magnets, the first electromagnetic coils and the second electromagnetic coils are orthogonal pairwise; the wireless capsule endoscope is internally provided with a capsule permanent magnet, an inertial sensor and a plurality of magnetic sensors; the computer device is configured to perform the following operations:

determining position data of the extracorporeal driver in a preset world coordinate system, and calculating theoretical magnetic field values of a plurality of extracorporeal magnetic sources at each magnetic sensor according to the biaxial rotation data and the position data;

2. The system of claim 1, wherein the plurality of magnetic sensors are mounted in a particular arrangement in the wireless capsule endoscope, and wherein the component of the magnetic field generated by the capsule permanent magnet on the plurality of magnetic sensors mounted in the particular arrangement is zero.

3. The system according to claim 2, wherein the plurality of magnetic sensors includes a first magnetic sensor, a second magnetic sensor and a third magnetic sensor, the capsule permanent magnet is a cylindrical permanent magnet, the capsule permanent magnet is magnetized along an axial direction of the capsule, a capsule coordinate system is formed based on the wireless capsule endoscope, the first magnetic sensor is installed at a point on an x-axis of the capsule coordinate system and is perpendicular to an x-axis direction of the capsule coordinate system, the second magnetic sensor is installed at a point on a y-axis of the capsule coordinate system and is perpendicular to a y-axis direction of the capsule coordinate system, and the third magnetic sensor is installed at a point where a tangential slope of a magnetic induction line generated by the capsule permanent magnet is 0 and is perpendicular to a z-axis direction of the capsule coordinate system.

4. The system according to any one of claims 1-3, wherein the external permanent magnet is a cylindrical permanent magnet, the external permanent magnet is magnetized along its own axial direction, the external permanent magnet is nested in the first electromagnetic coil and the second electromagnetic coil, a driver coordinate system is formed based on the external driver, and unit magnetic moments of the external permanent magnet, the first electromagnetic coil and the second electromagnetic coil are respectively parallel to the axial directions of the driver coordinate system.

5. The system of claim 4, wherein the outer casing of the extracorporeal drive is spherical, the outer casing of the extracorporeal drive is made of plastic, the extracorporeal drive is mounted on an end effector of the mechanical arm, and the magnetization direction of the extracorporeal permanent magnet is orthogonal to the rotation axis direction of the end joint of the mechanical arm.

6. The system of any one of claims 1-3 or 5, wherein the position data of the extracorporeal drive is position data of the extracorporeal drive in a predetermined world coordinate system, and wherein the calculating theoretical magnetic field values of the plurality of extracorporeal magnetic sources at each of the magnetic sensors based on the biaxial rotation data and the position data comprises:

respectively determining the position and normal vector direction of each magnetic sensor in a capsule coordinate system formed based on the wireless capsule endoscope;

determining the position and normal vector direction of each magnetic sensor in the world coordinate system according to the two-axis rotation data and the position and normal vector direction of each magnetic sensor in the capsule coordinate system;

and calculating a theoretical magnetic field value of each in-vitro magnetic source at each magnetic sensor based on the position and normal vector direction of each magnetic sensor in the world coordinate system.

7. The system of claim 6, wherein said calculating a theoretical magnetic field value for each of said extracorporeal magnetic sources at each of said magnetic sensors based on a location and a normal vector direction of each of said magnetic sensors in said world coordinate system comprises:

and calculating the projection of the magnetic field generated by each in-vitro magnetic source on the normal vector direction of each magnetic sensor in the world coordinate system based on the magnetic dipole model and the position and the normal vector direction of each magnetic sensor in the world coordinate system to obtain the theoretical magnetic field value of each in-vitro magnetic source at each magnetic sensor.

8. The system according to any one of claims 1-3, 5 or 7, wherein said separating said measured magnetic field values measured by each of said magnetic sensors to obtain separated magnetic field values for each of said extracorporeal magnetic sources at each of said magnetic sensors comprises:

controlling a periodic square wave current of a first frequency to the first electromagnetic coil to cause the first electromagnetic coil to generate a periodic time-varying magnetic field of the first frequency; and the number of the first and second groups,

controlling a periodic square wave current of a second frequency to the second electromagnetic coil to cause the second electromagnetic coil to generate a periodic time-varying magnetic field of the second frequency;

sampling the measured magnetic field values measured by each magnetic sensor in a preset time period according to a preset sampling frequency to respectively obtain a measured magnetic field value sampling sequence corresponding to each magnetic sensor;

respectively calculating the time average value of each measured magnetic field value sampling sequence, and taking the time average value as the separated magnetic field value of the external permanent magnet at the corresponding magnetic sensor;

respectively subtracting the time average value of the corresponding magnetic sensor from each measured magnetic field value sampling sequence to obtain a time sequence of the sum of the measured magnetic field values of the magnetic field generated by the first electromagnetic coil and the second electromagnetic coil at each magnetic sensor;

separating the measured magnetic field values generated by the first electromagnetic coil and the second electromagnetic coil based on the time sequence of the sum of the measured magnetic field values to obtain separated magnetic field values of the first electromagnetic coil and the second electromagnetic coil at each magnetic sensor.

9. The system of claim 8, wherein said separating the measured magnetic field values generated by said first electromagnetic coil and said second electromagnetic coil based on a time series of sums of said measured magnetic field values to obtain separated magnetic field values for said first electromagnetic coil and said second electromagnetic coil at each of said magnetic sensors comprises:

respectively sampling currents on the first electromagnetic coil and the second electromagnetic coil to obtain current sampling results of the first electromagnetic coil and the second electromagnetic coil;

performing fast Fourier transform on the current sampling results respectively to obtain current phase estimation, wherein the current phase estimation comprises a first current phase estimation of the first electromagnetic coil and a second current phase estimation of the second electromagnetic coil;

performing a fast fourier transform on the time series of sums of the measured magnetic field values to obtain a phase estimate and an amplitude estimate, the phase estimate comprising a first phase estimate for the first electromagnetic coil and a second phase estimate for the second electromagnetic coil, the amplitude estimate comprising a first amplitude estimate for the first electromagnetic coil and a second amplitude estimate for the second electromagnetic coil;

calculating a separated magnetic field value of the first and second electromagnetic coils at each of the magnetic sensors based on the current phase estimate, the phase estimate, and the amplitude estimate.

10. The system of claim 9, wherein said calculating a separated magnetic field value of said first and second electromagnetic coils at each of said magnetic sensors from said current phase estimate, said phase estimate, and said magnitude estimate comprises:

calculating absolute values of phase differences of the first and second electromagnetic coils, respectively, based on the current phase estimation and the phase estimation;

determining the signs of the measured magnetic field values of the first and second electromagnetic coils based on the absolute value of the phase difference;

and multiplying the measured magnetic field value symbols with the amplitude estimation respectively to obtain separated magnetic field values of the first electromagnetic coil and the second electromagnetic coil at each magnetic sensor.

11. The system of claim 10, wherein said determining a sign of a measured magnetic field value for said first electromagnetic coil and said second electromagnetic coil based on an absolute value of said phase difference comprises:

if the absolute value of the phase difference of the first electromagnetic coil is between 0-0.5 pi or 1.5 pi-2 pi, determining that the symbol of the measured magnetic field value of the first electromagnetic coil is positive;

if the absolute value of the phase difference of the first electromagnetic coil is between 0.5 pi and 1.5 pi, determining that the sign of the measured magnetic field value of the first electromagnetic coil is negative;

if the absolute value of the phase difference of the second electromagnetic coil is between 0-0.5 pi or 1.5 pi-2 pi, determining that the symbol of the measured magnetic field value of the second electromagnetic coil is positive;

and if the absolute value of the phase difference of the second electromagnetic coil is between 0.5 pi and 1.5 pi, determining that the sign of the measured magnetic field value of the second electromagnetic coil is negative.

12. The system according to any one of claims 1 to 3 or 5 or 7 or 9 to 11, wherein the two-axis rotation data is an angle of rotation around an x-axis and a y-axis of a preset world coordinate system respectively based on a capsule coordinate system formed by the wireless capsule endoscope, the wireless capsule endoscope is positioned based on the theoretical magnetic field value and the separation magnetic field value, and a real-time six-dimensional pose of the wireless capsule endoscope is output, and the system comprises:

and solving unknown parameters in the six-dimensional pose of the wireless capsule endoscope by adopting a least square method based on the theoretical magnetic field value and the separated magnetic field value, wherein the unknown parameters comprise the three-dimensional position of the wireless capsule endoscope and the rotation angle of the capsule coordinate system around the z axis of the world coordinate system, and the unknown parameters and the two-axis rotation data jointly form the real-time six-dimensional pose of the wireless capsule endoscope.

13. A positioning device for a wireless capsule endoscope, comprising:

the calculation module is used for determining position data of the in-vitro driver under a preset world coordinate system and calculating theoretical magnetic field values of a plurality of in-vitro magnetic sources arranged in the in-vitro driver at each magnetic sensor according to the two-axis rotation data and the position data, wherein the plurality of in-vitro magnetic sources comprise in-vitro permanent magnets, first electromagnetic coils and second electromagnetic coils, and unit magnetic moments of the in-vitro permanent magnets, the first electromagnetic coils and the second electromagnetic coils are orthogonal in pairs;

14. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor when executing the computer program performs the operations of:

determining position data of the in-vitro driver under a preset world coordinate system, and calculating theoretical magnetic field values of a plurality of in-vitro magnetic sources arranged in the in-vitro driver at each magnetic sensor according to the two-axis rotation data and the position data, wherein the plurality of in-vitro magnetic sources comprise in-vitro permanent magnets, first electromagnetic coils and second electromagnetic coils, and unit magnetic moments of the in-vitro permanent magnets, the first electromagnetic coils and the second electromagnetic coils are orthogonal in pairs;

15. A computer-readable storage medium storing a computer program, the computer program when executed by a processor performing the operations of:

determining position data of the in-vitro driver under a preset world coordinate system, and calculating theoretical magnetic field values of a plurality of in-vitro magnetic sources arranged in the in-vitro driver at each magnetic sensor according to the two-axis rotation data and the position data, wherein the plurality of in-vitro magnetic sources comprise in-vitro permanent magnets, first electromagnetic coils and second electromagnetic coils, and unit magnetic moments of the in-vitro permanent magnets, the first electromagnetic coils and the second electromagnetic coils are orthogonal pairwise;