Wireless capsule robot system and control method
阅读说明：本技术 一种无线胶囊机器人系统及控制方法 (Wireless capsule robot system and control method ) 是由 阳万安 戴厚德 于 2020-07-24 设计创作，主要内容包括：本发明提供了医疗检测设备技术领域的一种无线胶囊机器人系统及控制方法,系统包括：胶囊机器人；永磁环,设于所述胶囊机器人的外侧；计算机；驱动装置,与所述计算机连接,并通过所述永磁环驱动胶囊机器人运动；第一无线通信模块,一端与所述计算机连接,另一端与所述胶囊机器人连接；无线交变电磁跟踪装置,与所述计算机连接,并与所述胶囊机器人电磁耦合。本发明的优点在于：极大的提升了胶囊机器人位置跟踪和位姿控制的精度以及系统稳定性。(The invention provides a wireless capsule robot system and a control method in the technical field of medical detection equipment, wherein the system comprises: a capsule robot; the permanent magnet ring is arranged on the outer side of the capsule robot; a computer; the driving device is connected with the computer and drives the capsule robot to move through the permanent magnet ring; one end of the first wireless communication module is connected with the computer, and the other end of the first wireless communication module is connected with the capsule robot; and the wireless alternating electromagnetic tracking device is connected with the computer and is electromagnetically coupled with the capsule robot. The invention has the advantages that: the precision of capsule robot position tracking and position appearance control and system stability have greatly been promoted.)
1. A wireless capsule robotic system, comprising: the method comprises the following steps:
a capsule robot;
the permanent magnet ring is arranged on the outer side of the capsule robot;
a computer;
the driving device is connected with the computer and drives the capsule robot to move through the permanent magnet ring;
one end of the first wireless communication module is connected with the computer, and the other end of the first wireless communication module is connected with the capsule robot;
and the wireless alternating electromagnetic tracking device is connected with the computer and is electromagnetically coupled with the capsule robot.
2. A wireless capsule robotic system as claimed in claim 1, wherein: the capsule robot includes:
the outer side of the shell is annularly provided with the permanent magnet ring;
the MCU is arranged in the shell;
the power supply module is connected with the MCU and arranged in the shell;
the camera is connected with the MCU and arranged in the shell;
the temperature sensor is connected with the MCU and arranged in the shell;
one end of the second wireless communication module is connected with the MCU, and the other end of the second wireless communication module is connected with the first wireless communication module and arranged in the shell;
the electromagnetic induction coil is connected with the MCU, electromagnetically coupled with the wireless alternating electromagnetic tracking device and arranged in the shell;
and the PH detector is connected with the MCU and arranged on the surface of the shell.
3. A wireless capsule robotic system as claimed in claim 1, wherein: the driving device includes:
a robotic arm connected to the computer;
and the permanent magnet is arranged at the tail end of the mechanical arm.
4. A wireless capsule robotic system as claimed in claim 1, wherein: the first wireless communication module is a 2G communication module, a 3G communication module, a 4G communication module, a 5G communication module, an NB-IOT communication module, an LORA communication module, a WIFI communication module, a Bluetooth communication module or a ZigBee communication module.
5. A wireless capsule robotic system as claimed in claim 2, wherein: the second wireless communication module is a 2G communication module, a 3G communication module, a 4G communication module, a 5G communication module, an NB-IOT communication module, an LORA communication module, a WIFI communication module, a Bluetooth communication module or a ZigBee communication module.
6. A wireless capsule robotic system as claimed in claim 1, wherein: the wireless alternating electromagnetic tracking device comprises:
a multipath alternating electromagnetic emission array electromagnetically coupled with the capsule robot;
and one end of the alternating circuit is connected with the multi-path alternating electromagnetic emission array, and the other end of the alternating circuit is connected with the computer.
7. A wireless capsule robotic system as claimed in claim 1, wherein: the multiplexed alternating electromagnetic transmission array comprises:
and one end of each alternating electromagnetic transmitting coil is connected with the alternating circuit, and the other end of each alternating electromagnetic transmitting coil is electromagnetically coupled with the capsule robot.
8. A control method of a wireless capsule robot system is characterized in that: the method entails using the wireless capsule robotic system of any one of claims 1 to 7, comprising the steps of:
step S10, the computer drives the permanent magnet to move through the mechanical arm, the permanent magnet drives the permanent magnet ring to move through magnetic force, and then the capsule robot is linked to move;
step S20, the computer sends a detection instruction to the MCU through the first wireless communication module and the second wireless communication module in sequence;
step S30, the MCU controls the camera to shoot a picture of the interior of the human body based on the received detection instruction, controls the temperature sensor to collect the temperature of the interior of the human body, controls the PH detector to collect the PH value of the interior of the human body, and sends the picture, the temperature and the PH value to the computer through the second wireless communication module and the first wireless communication module in sequence;
and step S40, the computer tracks the position and the posture of the capsule robot in real time through the wireless alternating electromagnetic tracking device and the electromagnetic induction coil.
9. The method of claim 8, wherein the method further comprises: the step S40 specifically includes:
step S41, the computer controls each alternating electromagnetic transmitting coil to generate the size of
wherein (m, n, p)TRepresents the normalized direction vector of the alternating electromagnetic transmitting coil, and m2+n2+p2＝1；(a,b,c)TRepresenting a center position point of the alternating electromagnetic transmission coil; b isTRepresents a magnetic field constant; l denotes the center position point to space point (x) of the alternating electromagnetic transmission coil1,y1,z1) A distance of, andBx1representing alternating magnetic fieldsAt a point in space (x)1,y1,z1) A component of x-axis direction of (a); b isy1Representing alternating magnetic fieldsAt a point in space (x)1,y1,z1) A component of y-axis direction of (a); b isz1Representing alternating magnetic fieldsAt a point in space (x)1,y1,z1) A component of z-axis direction of (a);
step S42, setting the center position point of the electromagnetic induction coil as (x)2,y2,z2)，(vx,vy,vz)TNormalizing the direction vector for the electromagnetic induction coil, and
when the normalized direction vectors of the alternating electromagnetic transmitting coil and the electromagnetic induction coil are not parallel, calculating the intensity of the magnetic field generated by the alternating electromagnetic transmitting coil
based on the Faraday electromagnetic induction principle, the induced electromotive force of the electromagnetic induction coil is as follows:
wherein N represents a turn of an electromagnetic induction coilCounting;represents the surface area of the electromagnetic induction coil;representing the magnetic flux passing through the electromagnetic induction coil;
when the transmission signal of the alternating electromagnetic transmission coil is a sine wave with a frequency omega,the electromotive force induced by the electromagnetic induction coil is:
whereinRepresenting the maximum amplitude of the alternating electromagnetic transmission coil transmission signal; i is a positive integer; m represents the total number of the alternating electromagnetic transmitting coils and is a positive integer, and m is more than or equal to 6; 'imaxRepresenting an induced electromagnetic force of an ith alternating electromagnetic transmit coil;imaxrepresenting the theoretical electromagnetic force of the ith alternating electromagnetic transmit coil;
obtaining an error equation based on the electromotive force induced by the electromagnetic induction coil:
s43, solving initial values of the error equation by using a particle swarm optimization algorithm, and iterating the initial values by using an LM algorithm to obtain pose parameters (x, y, z, v)x,vy,vz) And tracking the position and the posture of the capsule robot in real time based on the pose parameters.
Technical Field
The invention relates to the technical field of medical detection equipment, in particular to a wireless capsule robot system and a control method.
Background
The wireless capsule robot has the functions of endoscopy, pathological sampling, medicine application and the like, can finish painless and noninvasive inspection and operation of the digestive tract, is a revolutionary breakthrough to the traditional wired gastroscope and surgical operation mode, and has higher efficiency and safety.
The first capsule endoscope named "M2A" was introduced by Given Imaging corporation of israel 5 months 2001 and was able to continuously capture images of the inner wall of the small intestine for 6 to 8 hours or more; the driving mode is a passive motion mode based on natural peristalsis of the alimentary canal, and the positioning of the M2A in the body is realized by an external radio frequency antenna array facing wireless communication, but the driving mode has the defect of low precision. When the capsule robot stays in the body, the actual position of the capsule robot in the body needs to be photographed by X-ray and taken out through operation.
In order to realize the real-time tracking and positioning of the wireless capsule robot in vivo, the wireless capsule robot embedded with permanent magnets is subjected to pose tracking by a magnetic sensor array, such as V.Schlageter, Bongqing tiger/Hu super professor team of Chinese university of hong Kong, Pixitian professor team of Chongqing university, Yangtze university, and the national school team of Shanghai traffic university, but the precision of the wireless capsule robot is influenced by environmental magnetic fields such as a geomagnetic field and is not combined with a driving mode of effective cooperation with the environmental magnetic fields. Some scientific research institutes and companies have begun to develop actively or externally driven capsule robots, such as RF System Lab corporation, which published a prototype "Norika 3" of capsule endoscope model without using a battery. The patient wears a vest embedded with three sets of transmitting and receiving coils, which transmit radio frequency. Three groups of 60-degree-spaced coils are also arranged in the capsule, and after the three groups of coils are received by the magnetic coils and converted by a capacitor, current is induced to form a three-pole motor mode, so that the rotation of the capsule is controlled, and therefore, the focus can be observed in different directions. Anhan science and technology (Wuhan) IncA series of products of a magnetic control capsule gastroscope system drive a magnetic control capsule with an embedded permanent magnet magnetic ring through an external permanent magnet system; because the external driving magnetic field is far larger than the magnetic field of the magnetic ring in the magnetic control capsule, the capsule robot tracking mode based on permanent magnetism cannot work with the magnetic drive at the same time.
In conclusion, the traditional capsule robot has the defects of low position tracking and pose control precision and poor stability, the position tracking is the basis of the pose control, and the intelligent control of the capsule robot based on a closed-loop mode can be realized only by combining the position tracking and the pose control. Therefore, how to provide a wireless capsule robot system and a control method thereof to improve the accuracy of capsule robot position tracking and pose control and the system stability becomes a problem to be solved urgently.
Disclosure of Invention
The invention aims to provide a wireless capsule robot system and a control method thereof, so as to improve the accuracy of capsule robot position tracking and pose control and the system stability.
In a first aspect, the present invention provides a wireless capsule robot system comprising:
a capsule robot;
the permanent magnet ring is arranged on the outer side of the capsule robot;
a computer;
the driving device is connected with the computer and drives the capsule robot to move through the permanent magnet ring;
one end of the first wireless communication module is connected with the computer, and the other end of the first wireless communication module is connected with the capsule robot;
and the wireless alternating electromagnetic tracking device is connected with the computer and is electromagnetically coupled with the capsule robot.
Further, the capsule robot includes:
the outer side of the shell is annularly provided with the permanent magnet ring;
the MCU is arranged in the shell;
the power supply module is connected with the MCU and arranged in the shell;
the camera is connected with the MCU and arranged in the shell;
the temperature sensor is connected with the MCU and arranged in the shell;
one end of the second wireless communication module is connected with the MCU, and the other end of the second wireless communication module is connected with the first wireless communication module and arranged in the shell;
the electromagnetic induction coil is connected with the MCU, electromagnetically coupled with the wireless alternating electromagnetic tracking device and arranged in the shell;
and the PH detector is connected with the MCU and arranged on the surface of the shell.
Further, the driving device includes:
a robotic arm connected to the computer;
and the permanent magnet is arranged at the tail end of the mechanical arm.
Further, the first wireless communication module is a 2G communication module, a 3G communication module, a 4G communication module, a 5G communication module, an NB-IOT communication module, a LORA communication module, a WIFI communication module, a Bluetooth communication module or a ZigBee communication module.
Further, the second wireless communication module is a 2G communication module, a 3G communication module, a 4G communication module, a 5G communication module, an NB-IOT communication module, a LORA communication module, a WIFI communication module, a bluetooth communication module, or a ZigBee communication module.
Further, the wireless alternating electromagnetic tracking device comprises:
a multipath alternating electromagnetic emission array electromagnetically coupled with the capsule robot;
and one end of the alternating circuit is connected with the multi-path alternating electromagnetic emission array, and the other end of the alternating circuit is connected with the computer.
Further, the multiple alternating electromagnetic transmission array comprises:
and one end of each alternating electromagnetic transmitting coil is connected with the alternating circuit, and the other end of each alternating electromagnetic transmitting coil is electromagnetically coupled with the capsule robot.
In a second aspect, the present invention provides a method for controlling a wireless capsule robot system, comprising the steps of:
step S10, the computer drives the permanent magnet to move through the mechanical arm, the permanent magnet drives the permanent magnet ring to move through magnetic force, and then the capsule robot is linked to move;
step S20, the computer sends a detection instruction to the MCU through the first wireless communication module and the second wireless communication module in sequence;
step S30, the MCU controls the camera to shoot a picture of the interior of the human body based on the received detection instruction, controls the temperature sensor to collect the temperature of the interior of the human body, controls the PH detector to collect the PH value of the interior of the human body, and sends the picture, the temperature and the PH value to the computer through the second wireless communication module and the first wireless communication module in sequence;
and step S40, the computer tracks the position and the posture of the capsule robot in real time through the wireless alternating electromagnetic tracking device and the electromagnetic induction coil.
Further, the step S40 specifically includes:
step S41, the computer controls each alternating electromagnetic transmitting coil to generate the size ofThe alternating magnetic field of (a); alternating magnetic field
wherein (m, n, p)TRepresents the normalized direction vector of the alternating electromagnetic transmitting coil, and m2+n2+p2＝1；(a,b,c)TRepresenting a center position point of the alternating electromagnetic transmission coil; b isTRepresents a magnetic field constant; l denotes the center position point to space point (x) of the alternating electromagnetic transmission coil1,y1,z1) A distance of, andBx1representing alternating magnetic fieldsIn spacePoint (x)1,y1,z1) A component of x-axis direction of (a); b isy1Representing alternating magnetic fieldsAt a point in space (x)1,y1,z1) A component of y-axis direction of (a); b isz1Representing alternating magnetic fieldsAt a point in space (x)1,y1,z1) A component of z-axis direction of (a);
step S42, setting the center position point of the electromagnetic induction coil as (x)2,y2,z2)，(vx,vy,vz)TNormalizing the direction vector for the electromagnetic induction coil, and
when the normalized direction vectors of the alternating electromagnetic transmitting coil and the electromagnetic induction coil are not parallel, calculating the intensity of the magnetic field generated by the alternating electromagnetic transmitting coil
based on the Faraday electromagnetic induction principle, the induced electromotive force of the electromagnetic induction coil is as follows:
wherein N represents the number of turns of the electromagnetic induction coil;represents the surface area of the electromagnetic induction coil;representing the magnetic flux passing through the electromagnetic induction coil;
when the transmission signal of the alternating electromagnetic transmission coil is a sine wave with a frequency omega,the electromotive force induced by the electromagnetic induction coil is:
whereinRepresenting the maximum amplitude of the alternating electromagnetic transmission coil transmission signal; i is a positive integer; m represents the total number of the alternating electromagnetic transmitting coils and is a positive integer, and m is more than or equal to 6; 'imaxRepresenting an induced electromagnetic force of an ith alternating electromagnetic transmit coil;imaxrepresenting the theoretical electromagnetic force of the ith alternating electromagnetic transmit coil;
obtaining an error equation based on the electromotive force induced by the electromagnetic induction coil:
step S43, solving the initial value of the error equation by utilizing a particle swarm optimization algorithm, and then utilizing LMThe algorithm iterates the initial values to obtain pose parameters (x, y, z, v)x,vy,vz) And tracking the position and the posture of the capsule robot in real time based on the pose parameters.
The invention has the advantages that:
the capsule robot is provided with the permanent magnet ring on the outer side, the computer grabs the permanent magnet through the control mechanical arm and drives the permanent magnet ring to move through magnetic force, and the capsule robot is further linked to adjust the pose, so that the pose control precision of the capsule robot is greatly improved; the electromagnetic induction coil is arranged in the capsule robot, the signal of the alternating magnetic field is transmitted to the capsule robot through the multi-channel alternating electromagnetic transmitting array, and then the position of the capsule robot is positioned by calculating the corresponding electromotive force, so that the position tracking precision of the capsule robot is greatly improved; and the rotating magnetic field frequency generated by the pose change of the permanent magnet and the signal frequency generated by the multipath alternating electromagnetic emission array are not interfered with each other, so that the stability of the system is greatly improved, and the intelligent control of the capsule robot based on a closed-loop mode is finally realized.
Drawings
The invention will be further described with reference to the following examples with reference to the accompanying drawings.
Fig. 1 is a schematic block circuit diagram of a wireless capsule robotic system of the present invention.
Fig. 2 is a schematic block circuit diagram of the capsule robot of the present invention.
Fig. 3 is a schematic structural diagram of a wireless capsule robot system of the present invention.
Fig. 4 is a flowchart of a control method of a wireless capsule robot system of the present invention.
FIG. 5 is a schematic diagram of the coupling relationship of the alternating electromagnetic transmitter coil and the electromagnetic induction coil of the present invention.
Fig. 6 is a schematic view of the capsule robot motion control of the present invention.
Description of the labeling:
100-a wireless capsule robot system, 1-a capsule robot, 2-a permanent magnet ring, 3-a computer, 4-a driving device, 5-a first wireless communication module, 6-a wireless alternating electromagnetic tracking device, 11-a shell, 12-an MCU, 13-a power supply module, 14-a camera, 15-a temperature sensor, 16-a second wireless communication module, 17-an electromagnetic induction coil, 18-a PH detector, 41-a mechanical arm, 42-a permanent magnet, 61-a multi-path alternating electromagnetic emission array, 62-an alternating circuit and 611-an alternating electromagnetic emission coil.
Detailed Description
Referring to fig. 1 to 5, a preferred embodiment of a wireless capsule robot system 100 according to the present invention comprises:
the capsule robot 1 is used for entering any position of the digestive tract of a human body so as to check and treat the human body, and has the functions of endoscopy, sampling, pesticide application, measurement of the temperature and the pH value of the digestive tract and the like;
the permanent magnet ring 2 is arranged on the outer side of the capsule robot 1 and used for linking the capsule robot 1;
the computer 3 is used for controlling the work of the driving device 4 and the wireless alternating electromagnetic tracking device 6, communicating with the capsule robot 1 through the first wireless communication module 5 and issuing a control instruction;
the driving device 4 is connected with the computer 3, drives the capsule robot 1 to move through the permanent magnet ring 2, is used for generating a magnetic field with adjustable direction and strength to enable the capsule robot 1 to move, and has 6 angles of view such as upward view, overlooking view, rotation and the like;
a first wireless communication module 5, one end of which is connected with the computer 3 and the other end of which is connected with the capsule robot 1, for communicating with the capsule robot 1;
and the wireless alternating electromagnetic tracking device 6 is connected with the computer 3, is electromagnetically coupled with the capsule robot 1, and is used for applying a multi-channel alternating electromagnetic signal with a time sequence synchronization signal to the outside so as to track the real-time pose of the capsule robot 1 with the built-in electromagnetic induction coil 17.
The capsule robot 1 includes:
the outer side of the shell 11 is annularly provided with the permanent magnet ring 2; the shell 11 is made of transparent materials;
the MCU12 is arranged in the shell 11 and is used for controlling the camera 14, the temperature sensor 15, the second wireless communication module 16 and the PH detector 18 to work, amplifying, filtering and sampling the induction signals of the electromagnetic induction coil 17, calculating to obtain real-time pose information, and sending the pose information and the measurement data to the computer 3;
the power module 13 is connected with the MCU12, is arranged in the shell 11, and is used for supplying power to the capsule robot 1;
the camera 14 is connected with the MCU12, arranged in the shell 11 and used for shooting pictures of gastrointestinal passages;
the temperature sensor 15 is connected with the MCU12, is arranged in the shell 11 and is used for collecting the temperature in the intestines and the stomach of the human body;
a second wireless communication module 16, one end of which is connected with the MCU12 and the other end of which is connected with the first wireless communication module 5, and which is disposed in the housing 11 for communicating with the computer 3;
the electromagnetic induction coil 17 is connected with the MCU12, electromagnetically coupled with the wireless alternating electromagnetic tracking device 6 and arranged in the shell 11; the electromagnetic induction coil 17 adopts more than 1 single-axis coil or a combination thereof with a certain angle; 5-dimensional pose information can be obtained based on the positioning of the single-axis coils, and complete 6-dimensional information can be obtained based on the combination of the two single-axis coils;
and the pH detector 18 is connected with the MCU12, arranged on the surface of the shell 11 and used for collecting the pH value in the intestines and stomach of the human body.
The capsule robot 1 is further provided with an LED (not shown) for illumination, a drug spraying chamber (not shown) for spraying drugs, and a biopsy taking chamber (not shown) for taking a pathological examination sample; the biopsy taking bin can extend out of the micro titanium metal needle to take a biopsy.
The drive device 4 includes:
the mechanical arm 41 is connected with the computer 3 and is used for clamping the permanent magnet 42 to generate a rotating magnetic field or a gradient magnetic field or a combination of the rotating magnetic field and the gradient magnetic field so as to control and adjust the posture of the capsule robot 1 sleeved with the permanent magnet ring 2 in the digestive tract;
and the permanent magnet 42 is arranged at the tail end of the mechanical arm 41 and used for driving the permanent magnet ring 2 through magnetic force.
The first wireless communication module 5 is a 2G communication module, a 3G communication module, a 4G communication module, a 5G communication module, an NB-IOT communication module, an LORA communication module, a WIFI communication module, a Bluetooth communication module or a ZigBee communication module.
The second wireless communication module 16 is a 2G communication module, a 3G communication module, a 4G communication module, a 5G communication module, an NB-IOT communication module, an LORA communication module, a WIFI communication module, a bluetooth communication module or a ZigBee communication module.
The wireless alternating electromagnetic tracking device 6 comprises:
a multi-channel alternating electromagnetic emission array 61 electromagnetically coupled with the capsule robot 1; the multi-channel alternating electromagnetic emission array 61 can be effectively coupled by the electromagnetic induction coil 17 after being loaded with alternating electromagnetic signals;
an alternating circuit 62, one end of which is connected with the multi-path alternating electromagnetic emission array 61, and the other end of which is connected with the computer 3; the alternating circuit 62 is used to generate an alternating electromagnetic signal of at least 6 channels.
The multiple alternating electromagnetic emission array 61 comprises:
a plurality of alternating electromagnetic transmission coils 611, one end of which is connected to the alternating circuit 62 and the other end of which is electromagnetically coupled to the capsule robot 1; the alternating electromagnetic transmitting coils 611 are arranged in a manner of a single-axis array or a combination of orthogonal coils.
The invention discloses a control method of a wireless capsule robot system, which comprises the following steps:
step S10, the computer drives the permanent magnet to move through the mechanical arm, the permanent magnet drives the permanent magnet ring to move through magnetic force, and then the capsule robot is linked to move;
step S20, the computer sends a detection instruction to the MCU through the first wireless communication module and the second wireless communication module in sequence;
step S30, the MCU controls the camera to shoot a picture of the interior of the human body based on the received detection instruction, controls the temperature sensor to collect the temperature of the interior of the human body, controls the PH detector to collect the PH value of the interior of the human body, and sends the picture, the temperature and the PH value to the computer through the second wireless communication module and the first wireless communication module in sequence;
and step S40, the computer tracks the position and the posture of the capsule robot in real time through the wireless alternating electromagnetic tracking device and the electromagnetic induction coil.
The step S40 specifically includes:
step S41, the spatial magnetic field distribution of the alternating electromagnetic transmission coil fed with the alternating electromagnetic signal may be equivalent to a magnetic dipole, i.e., each alternating electromagnetic transmission coil may be equivalent to a magnetic dipole; the computer controls the generation of the alternating electromagnetic transmitting coils through the alternating circuit to have the size
wherein (m, n, p)TRepresents the normalized direction vector of the alternating electromagnetic transmitting coil, and m2+n2+p2＝1；(a,b,c)TRepresenting a center position point of the alternating electromagnetic transmission coil; b isTRepresents the magnetic field constant, influenced by the coil and loading current characteristics; l represents the center position point to the space point of the alternating electromagnetic transmitting coil(x1,y1,z1) A distance of, andBx1representing alternating magnetic fields
because each alternating electromagnetic transmitting coil of the multi-channel alternating electromagnetic transmitting array adopts a frequency division excitation mode, in order to realize the time sequence synchronization of transmitting and receiving, a zero signal with a certain time interval is fixedly output while the alternating electromagnetic transmitting coil transmits a signal, and the phase synchronization of the electromagnetic induction coils is realized.
Alternating electromagnetic signals induced by the electromagnetic induction coil end are represented by superposition of multi-channel transmitting alternating signals with different frequencies, amplitude separation of different frequency components is achieved through Fourier transform, and the separated amplitude signals of different channels are used for solving the alignment posture through an optimization algorithm.
Step S42, setting the center position point of the electromagnetic induction coil as (x)2,y2,z2)，(vx,vy,vz)TNormalizing the direction vector for the electromagnetic induction coil, and
when the normalized direction vectors of the alternating electromagnetic transmitting coil and the electromagnetic induction coil are not parallel, calculating the intensity of the magnetic field generated by the alternating electromagnetic transmitting coil
based on the Faraday electromagnetic induction principle, the induced electromotive force of the electromagnetic induction coil is as follows:
wherein N represents the number of turns of the electromagnetic induction coil;represents the surface area of the electromagnetic induction coil;
when the transmission signal of the alternating electromagnetic transmission coil is a sine wave with a frequency omega,then electromagnetic inductionThe electromotive force induced by the coil is:
wherein
obtaining an error equation based on the electromotive force induced by the electromagnetic induction coil:
s43, solving initial values of the error equation by using a Particle Swarm Optimization (PSO) algorithm, and iterating the initial values by using an LM algorithm to obtain pose parameters (x, y, z, v)x,vy,vz) And tracking the position and the posture of the capsule robot in real time based on the pose parameters.
Because the error equation is a nonlinear least square optimization problem, the LM algorithm (Levenberg-Marquardt) is used for accurately solving, but the requirement on the initial value is high, the PSO algorithm which has strong initial value finding capability (fast iteration rate) but poor tracking accuracy is adopted for outputting as the initial value of the LM algorithm in tracking and positioning, and the LM output in the previous iteration process is used as the initial value of the next iteration LM in the following tracking process.
In summary, the invention has the advantages that:
the capsule robot is provided with the permanent magnet ring on the outer side, the computer grabs the permanent magnet through the control mechanical arm and drives the permanent magnet ring to move through magnetic force, and the capsule robot is further linked to adjust the pose, so that the pose control precision of the capsule robot is greatly improved; the electromagnetic induction coil is arranged in the capsule robot, the signal of the alternating magnetic field is transmitted to the capsule robot through the multi-channel alternating electromagnetic transmitting array, and then the position of the capsule robot is positioned by calculating the corresponding electromotive force, so that the position tracking precision of the capsule robot is greatly improved; and the rotating magnetic field frequency generated by the pose change of the permanent magnet and the signal frequency generated by the multipath alternating electromagnetic emission array are not interfered with each other, so that the stability of the system is greatly improved, and the intelligent control of the capsule robot based on a closed-loop mode is finally realized.
Although specific embodiments of the invention have been described above, it will be understood by those skilled in the art that the specific embodiments described are illustrative only and are not limiting upon the scope of the invention, and that equivalent modifications and variations can be made by those skilled in the art without departing from the spirit of the invention, which is to be limited only by the appended claims.
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种单筒型内窥镜快速防雾装置及其使用方法