Real-time Spatial Precise Magnetic Positioning Device, Ray Imaging System and Magnetic Positioning Method

Abstract

The present invention discloses a real-time spatial precise magnetic positioning device, a ray imaging system and a magnetic positioning method. The ray imaging system includes a ray source, a beam limiter, a flat panel detector and a real-time spatial precise magnetic positioning device. Among them, the magnetic positioning device includes a processor, a magnetic field generating device and a magnetic sensor array. The magnetic field generating device is coaxially arranged with the beam limiter, and a plurality of magnetic sensors of the magnetic sensor array are distributed on the flat panel detector; the magnetic field generating device at least includes an alternating magnetic field generator for generating an alternating magnetic field. The plurality of magnetic sensors of the magnetic sensor array can independently detect the magnetic induction intensity and send the real-time detected magnetic induction intensity data to the processor. The processor determines the positional relationship between the beam limiter and the flat panel detector according to the magnitudes of the magnetic induction intensities detected by each magnetic sensor in real time. The present invention realizes centimeter-level positioning accuracy in an indoor environment, is applicable to medical devices and industrial control, and has strong anti-interference ability.

Description

[0001] Priority Claim

[0002] This application claims the priority of a Chinese patent application with the application number 2020105471548 filed with the Chinese Patent Office on June 16, 2020, the entire content of which is incorporated herein by reference. Technical Field

[0003] The present invention relates to the technical field of medical device positioning, and particularly relates to a real-time spatial precise magnetic positioning device, a ray imaging system, and a magnetic positioning method. Background Art

[0004] Currently, the technologies used for positioning mainly include satellite positioning technology, wireless positioning technology (such as Wi-Fi positioning and Bluetooth positioning), environmental feature magnetic positioning technology, accelerometer positioning technology, etc.

[0005] Among them, satellite positioning technology determines the position of a positioning point by measuring the time difference of electromagnetic wave propagation from the positioning point to different satellites, and then converting it into the distance from the positioning point to the satellite; wireless positioning technology determines the position of a positioning point by measuring the wireless signal strength (such as the strength of Wi-Fi and Bluetooth signals) at the positioning point; environmental feature magnetic positioning technology determines the position of a positioning point by testing the environmental magnetic field at the positioning point and comparing it with a database; accelerometer positioning technology determines the movement trajectory of a positioning point by integrating the acceleration of an object measured in real time twice over time. Currently, according to the application fields, these positioning technologies are combined and supplemented with each other to form various application solutions.

[0006] However, the above positioning technologies each have their own defects: Satellite positioning technology requires receiving signals from more than three satellites at the positioning point to determine the position because it measures the relative position from the positioning point to the satellites, so it can only be applied outdoors and has a high cost; in addition, due to the very fast propagation speed of electromagnetic waves, even with atomic clock timekeeping, the positioning error is at the meter level.

[0007] Wireless positioning technology and feature magnetic positioning technology can be applied indoors and outdoors, but since the test data of the positioning point needs to be compared with a database, environmental mapping and database establishment are required before positioning, so they can only be applied in known environments, and due to large interference, the positioning accuracy is not high, generally also at the meter level.

[0008] Accelerometer positioning technology can only determine the relative movement of the test point, and other technologies are required for position calibration before starting positioning, such as starting to move at a certain designated position, etc.; in addition, since this technology only measures the change of acceleration over time, its error accumulates over time, and generally the positioning accuracy exceeds one meter after several minutes.

[0009] However, the positioning accuracy requirement for medical devices is at the centimeter level, and there is a lack of an accurate positioning technology that can be applied to medical devices in an indoor environment in the existing technology. Summary of the Invention

[0010] In order to overcome the deficiencies of the existing technology, the present invention provides a real-time spatial accurate magnetic positioning device, a ray imaging system, and a magnetic positioning method, which can be used indoors and the positioning accuracy reaches the centimeter level or even less than one centimeter. The technical solutions are as follows:

[0011] On the one hand, the present invention provides a real-time spatial accurate magnetic positioning device for aligning a first object with a reference area on a second object. The magnetic positioning device includes a processor, a magnetic field generating device coaxially arranged with the first object, and a magnetic sensor array arranged on the second object. The magnetic field generating device at least includes an alternating magnetic field generator for generating an alternating magnetic field. The magnetic sensor array is configured such that the distance from each magnetic sensor of the magnetic sensor array to the center point of the reference area is equal;

[0012] Multiple magnetic sensors of the magnetic sensor array can independently and real-time detect the magnetic induction intensity and send the real-time detected magnetic induction intensity data to the processor. The processor compares the magnitudes of the magnetic induction intensities real-time detected by each magnetic sensor;

[0013] Adjust the position of the first object and / or the second object until the processor compares and obtains that the magnitudes of the magnetic induction intensities real-time detected by more than half of the magnetic sensors are the same or the difference in magnetic induction intensity is less than a preset threshold or proportional threshold, then it is determined that the first object at the current position is aligned with the reference area on the second object.

[0014] As another technical solution, the present invention also provides a second real-time spatial accurate magnetic positioning device for determining the positional relationship between a first object and a reference area on a second object. The magnetic positioning device includes a processor, a magnetic field generating device coaxially arranged with the first object, and a magnetic sensor array arranged on the second object. The magnetic field generating device at least includes an alternating magnetic field generator for generating an alternating magnetic field. The magnetic sensor array is configured such that the arrangement positions of its multiple magnetic sensors have a determined positional relationship with the center point of the reference area;

[0015] Multiple magnetic sensors of the magnetic sensor array can independently and real-time detect the magnetic induction intensity and send the real-time detected magnetic induction intensity data to the processor. The processor calculates the position coordinates of each magnetic sensor relative to the magnetic field generating device according to the magnetic induction intensity data;

[0016] The processor obtains the position coordinates of the center point of the reference area relative to the first object based on the position coordinates of more than half of the magnetic sensors relative to the magnetic field generating device and the positional relationship between the magnetic sensor and the center point of the reference area.

[0017] For the technical solution of the second real-time spatial precise magnetic positioning device, the position coordinates of the center point of the reference area obtained by the processor relative to the first object are three-dimensional coordinates of the x / y / z axes. The processor analyzes the three-dimensional coordinates. If the coordinates of the two coordinate axes parallel to the plane where the first object is located are 0 or within a preset range close to 0, it is determined that the first object at the current position is aligned with the reference area on the second object.

[0018] For the technical solutions of the above two real-time spatial precise magnetic positioning devices, further, the magnetic field generating device further includes a bias magnet for biasing the magnetic sensor to a preset working magnetic field range, and the bias magnet is a permanent magnet or an electromagnet.

[0019] For the technical solutions of the above two real-time spatial precise magnetic positioning devices, further, the alternating magnetic field generator can perform magnetic field encoding, and the magnetic field generating device and the magnetic sensor array can communicate bidirectionally.

[0020] Further, the alternating magnetic field generator adjusts the magnetic field strength it generates to weaken according to the relative position of the first object and the second object.

[0021] Optionally, the alternating magnetic field generator is any one of the following three methods:

[0022] The alternating magnetic field generator includes three orthogonal modulation coils. By modulating the coil current, the magnetic field generated by the magnetic field generating device can change in terms of magnetic field magnitude and / or direction in three-dimensional space; or,

[0023] The alternating magnetic field generator includes two modulation coils at a preset angle, and the two modulation coils work alternately. When the sensor is at a position where the magnetic field gradient of one of the coils is less than 0.01 mT / m, due to the preset angle between the two coils, the magnetic field gradient at the position where the sensor is located in the magnetic field generated by the other coil must be greater than 0.01 mT / m. At this time, the magnetic field sensor relies on the magnetic field generated by the second coil to obtain the relative position between the first object and the second object; or,

[0024] The alternating magnetic field generator includes a permanent magnet and a mechanical transmission device for driving the permanent magnet to move. The permanent magnet generates an alternating magnetic field in space under the drive of the mechanical transmission device, and the magnetic field can be modulated through the mechanical transmission device.

[0025] As a further first solution, the alternating magnetic field generator and the sensor array are each provided with an angle sensor, which is used to calculate the azimuth angles of the first object and the second object respectively while the magnetic sensor measures the magnetic field, so as to determine the attitude between the two objects.

[0026] As a further second solution, the alternating magnetic field generator and the sensor array are each provided with an acceleration sensor, which is used to calculate the three-dimensional acceleration data of the first object and the second object respectively while the magnetic sensor measures the magnetic field, so as to determine the accelerations, velocities and relative positions of the first object and the second object.

[0027] On the other hand, the present invention provides a ray imaging system with a magnetic positioning function, including a ray source, a beam limiter, a flat panel detector and a real-time spatial precise magnetic positioning device. Among them, the magnetic positioning device includes a processor, a magnetic field generating device and a magnetic sensor array. The magnetic field generating device is coaxially arranged with the beam limiter, and a plurality of magnetic sensors of the magnetic sensor array are distributed on the flat panel detector;

[0028] The magnetic field generating device at least includes an alternating magnetic field generator for generating an alternating magnetic field. A plurality of magnetic sensors of the magnetic sensor array can independently detect the magnetic induction intensity and send the real-time detected magnetic induction intensity data to the processor. The processor determines the positional relationship between the beam limiter and the flat panel detector according to the magnitudes of the magnetic induction intensities detected by each magnetic sensor in real time.

[0029] On yet another aspect, the present invention provides a first real-time spatial precise magnetic positioning method, including the following steps:

[0030] S11. Coaxially arrange the magnetic field generating device with the first object, set a reference area to be aligned on the second object, and determine the center point of the reference area; and arrange a plurality of magnetic sensors on the second object so that the distances from the magnetic sensors to the center point are the same;

[0031] S12. The magnetic field generating device generates an alternating magnetic field, and the plurality of magnetic sensors independently detect the magnetic induction intensity in real time;

[0032] S13. Compare the magnitudes of the magnetic induction intensities detected by the plurality of magnetic sensors in real time;

[0033] S14. If the magnitudes of the magnetic induction intensities detected by more than half of the magnetic sensors are the same or the difference in magnetic induction intensities is less than a preset threshold or proportional threshold, the first object at the current position is aligned with the reference area on the second object, and the positioning ends; otherwise, execute S15;

[0034] S15. Adjust the positions of the first object and / or the second object, and then repeat steps S12 - S14.

[0035] In another aspect, the present invention provides a second real - time spatial precise magnetic positioning method, including the following steps:

[0036] S21. Coaxially arrange the magnetic field generating device with the first object, set a reference area to be aligned on the second object, determine the center point of the reference area; and arrange a plurality of magnetic sensors on the second object to obtain the positional relationships between each magnetic sensor and the center point.

[0037] S22. The magnetic field generating device generates an alternating magnetic field, and the plurality of magnetic sensors independently and real - time detect the magnetic induction intensity.

[0038] S23. According to the detection data of the magnetic induction intensity, calculate the position coordinates of each magnetic sensor relative to the magnetic field generating device respectively.

[0039] S24. If the position coordinates of one or less than half of the magnetic sensors deviate from the plane determined by the position coordinates of the remaining magnetic sensors, then the one or less than half of the magnetic sensors are excluded as the disturbed magnetic sensors, and only based on the position coordinates of the remaining magnetic sensors relative to the magnetic field generating device and the positional relationships between the remaining magnetic sensors and the center point, obtain the position coordinates of the center point relative to the first object.

[0040] S25. If, among the three - dimensional coordinates of the x / y / z axes of the center point relative to the first object, the coordinates of the two coordinate axes parallel to the plane where the first object is located are 0 or within a preset range close to 0, then the first object at the current position is aligned with the reference area on the second object, and the positioning ends; otherwise, execute S26.

[0041] S26. Adjust the positions of the first object and / or the second object, and then repeat steps S22 - S25.

[0042] In another aspect, the present invention provides a third real - time spatial precise magnetic positioning method, including the following steps:

[0043] S31. Arrange two coils of the magnetic field generator on the first object at a preset angle, where the preset angle is not equal to 90°, set a reference area to be aligned on the second object, determine the center point of the reference area; and arrange a plurality of magnetic sensors on the second object to obtain the positional relationships between each magnetic sensor and the center point.

[0044] S32. The two coils of the magnetic field generator work alternately to generate an alternating magnetic field, and the plurality of magnetic sensors independently and real - time detect the magnetic induction intensity.

[0045] S33. One cycle of alternating operation of the two coils is recorded as a group, and the detection data of the magnetic induction intensity is divided into several groups; according to the detection data of the magnetic induction intensity, the position coordinates of each magnetic sensor relative to the magnetic field generator are calculated respectively.

[0046] S34. If the difference between the two position coordinates in a group exceeds the preset threshold, by comparing with the detection data of other magnetic sensors, the position coordinate data with a larger deviation is discarded.

[0047] S35. According to the remaining position coordinate data after the processing in S34 and the position relationship between the other magnetic sensors and the center point, the position coordinates of the center point relative to the first object are obtained.

[0048] S36. If, among the three-dimensional coordinates of the x / y / z axes of the center point relative to the first object, the coordinates of the two coordinate axes parallel to the plane where the first object is located are 0 or within a preset range close to 0, the first object at the current position is aligned with the reference area on the second object, and the positioning ends; otherwise, S37 is executed.

[0049] S37. Adjust the positions of the first object and / or the second object, and then repeat steps S32 - S36.

[0050] In addition, the present invention also provides a real-time spatial precise magnetic positioning correction method, including the following steps:

[0051] S41. Using the magnetic positioning method as described above, calculate the relative position between the first object and the second object at time t0 using the three-dimensional magnetic field data of the magnetic field sensor.

[0052] S42. At time t1, use the magnetic sensor, accelerometer, and angle sensor to obtain the three-dimensional magnetic field data, three-dimensional acceleration data, and three-dimensional attitude data at time t1 respectively.

[0053] S43. According to the three-dimensional magnetic field data and three-dimensional attitude data at time t1, calculate the relative position pt1 between the first object and the second object at time t1; according to the three-dimensional acceleration data at time t1, calculate the relative velocity v1 between the first object and the second object at time t1.

[0054] S44. At time t2, use the magnetic sensor, accelerometer, and angle sensor to obtain the three-dimensional magnetic field data, three-dimensional acceleration data, and three-dimensional attitude data at time t2 respectively.

[0055] S45. According to the three-dimensional magnetic field data and three-dimensional attitude data at time t2, calculate the relative position pt2 between the first object and the second object at time t2; according to the three-dimensional acceleration data at time t2 and the relative velocity v1 at time t1, calculate the relative position pt2' between the first object and the second object at time t2.

[0056] S46. Compare pt2 and pt2'. If the error between the two is within 1 cm, then the relative position of the first object and the second object at time t2 is the average value of pt2 and pt2'; otherwise, execute S47 - S48;

[0057] S47. Compare the three - dimensional acceleration value at time t2 and the velocity v1 at time t1. If both the velocity v1 and the three - dimensional acceleration are approximately 0, then the relative position of the first object and the second object at time t2 is pt2; otherwise, it is pt2';

[0058] S48. Adjust the position of the first object and / or the second object, and then repeat steps S41 - S46.

[0059] The beneficial effects brought by the technical solution provided by the present invention are as follows:

[0060] a. A positioning accuracy of less than 1 cm within a space range of several meters;

[0061] b. Strong anti - interference ability;

[0062] c. Controllable cost, suitable for widely used in the positioning and alignment of devices in the fields of industrial control and medical devices. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following will briefly introduce the drawings required for the description of the embodiments. Obviously, the following drawings are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0064] Figure 1 It is a schematic diagram of the installation and application of the real - time spatial precise magnetic positioning device provided by the embodiment of the present invention in a ray imaging system;

[0065] Figure 2 It is a schematic diagram of the structure of the alternating magnetic field generator in the magnetic positioning device provided by the embodiment of the present invention, which includes three orthogonal modulation coils;

[0066] Figure 3 It is a schematic diagram of the structure of the alternating magnetic field generator in the magnetic positioning device provided by the embodiment of the present invention, which includes a mechanical transmission device and a permanent magnet;

[0067] Figure 4 It is a flowchart of the first precise magnetic positioning method provided by the embodiment of the present invention;

[0068] Figure 5 It is a flowchart of the second precise magnetic positioning method provided by the embodiment of the present invention.

[0069] Among them, the reference signs include: 1 - the first object, 2 - the second object, 31 - the alternating magnetic field generator, 32 - the bias magnet, 4 - the magnetic sensor. Specific embodiments

[0070] In order to enable those skilled in the art to better understand the solution of the present invention, and to more clearly understand the purpose, technical solution and its advantages of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with specific embodiments and with reference to the accompanying drawings. It should be noted that the implementation manners not illustrated or described in the drawings are forms known to those of ordinary skill in the art. Additionally, although this document may provide examples containing parameters with specific values, it should be understood that the parameters need not exactly equal the corresponding values, but may approximate the corresponding values within an acceptable tolerance or design constraint. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the scope of protection of the present invention. In addition, the terms "comprising" and "having" and any variations thereof in the specification and claims of the present invention are intended to cover non-exclusive inclusion. For example, a process, method, device, product or equipment comprising a series of steps or units need not be limited to those steps or units clearly listed, but may include other steps or units not clearly listed or inherent to these processes, methods, products or equipment.

[0071] The present invention proposes a high-precision positioning solution using a single magnet or a magnet array in cooperation with a magnetic sensor array. The magnet / magnet array is composed of one or several permanent magnets, electromagnetic magnets, or a combination of both, and the magnetic sensor is composed of several magnetic field sensors, and the two can communicate wirelessly. This technology can provide a positioning accuracy of less than one centimeter within a range of several meters, filling the gap in centimeter-level precision positioning, and has wide applications in industrial control and medical devices.

[0072] In an embodiment of the present invention, a real-time spatial precise magnetic positioning device is provided for aligning the first object 1 with a reference area on the second object 2, as Figure 1 shown. The magnetic positioning device includes a processor, a magnetic field generating device coaxially arranged with the first object 1, and a magnetic sensor array disposed on the second object 2. The magnetic field generating device at least includes an alternating magnetic field generator 31 for generating an alternating magnetic field. The magnetic sensor array is configured such that the distance from each magnetic sensor 4 of the magnetic sensor array to the center point of the reference area is equal. Specifically, the alternating magnetic field generator 31 can have various forms:

[0073] In one embodiment of the present invention, the alternating magnetic field generator 31 includes a permanent magnet and a mechanical transmission device for driving the permanent magnet to move, and the magnetic field can be modulated by the mechanical transmission device. For example, the permanent magnet is fixed on an electric motor, as Figure 3 shown. When the electric motor drives the permanent magnet to rotate at a certain speed, the permanent magnet can generate an alternating magnetic field in space. By controlling the speed of the electric motor, the spatial magnetic field can be modulated. More preferably, the permanent magnet can be combined with a coil, so that the advantages of the strong magnetic field and no power consumption of the permanent magnet are complemented by the advantages of convenient magnetic field control of the coil, reducing costs while improving the positioning accuracy.

[0074] In another embodiment of the present invention, the alternating magnetic field generator 31 includes three orthogonal modulation coils, as Figure 2 shown. By modulating the coil current, the magnetic field generated by the magnetic field generating device can be changed in terms of arbitrary magnetic field magnitude and / or magnetic field direction in three-dimensional space. In this case, the following positioning method can be used:

[0075] One possible positioning method is the scanning type. The magnetic field generated by the magnetic field generating device scans in three-dimensional space at a certain speed. When the axis of a certain magnetic sensor in the magnetic sensor array is exactly parallel to it, its value reaches the maximum. Since the magnetic sensor array and the magnetic field generating device can communicate with each other, after the magnetic field completes a full-space scan, the magnetic sensor array can calculate its position relative to the magnetic field generating device.

[0076] Another possible positioning method is the guiding type. First, the magnetic sensor completes a search of the entire space to find the position of the magnetic sensor. Then the magnetic field generating device only scans in a small range to ensure that its direction (not necessarily the magnetic field direction, only a conceptual direction, such as the opposite direction of the magnetic field, the perpendicular direction of the magnetic field, etc.) always points to the magnetic sensor array. Since it is not necessary to scan the magnetic field in the entire space each time, this mode can greatly improve the positioning efficiency.

[0077] In addition to the alternating magnetic field generator, in a preferred embodiment of the present invention, the magnetic field generating device further includes a bias magnet 32 for biasing the magnetic sensor 4 to a preset operating magnetic field range, and the bias magnet 32 is a permanent magnet or an electromagnet. Specifically, the magnetic field generating device generates a non-uniform magnetic field in space, which includes two parts: one part is the bias magnet, and the magnetic field of this part is generated by a permanent magnet or an electromagnet and is used to bias the magnetic sensor to a suitable operating magnetic field range; the other part is the alternating magnetic field, which is generated by an alternating current coil or a moving magnet, and the frequency of the alternating magnetic field can be controlled by controlling the alternating current or the movement frequency of the mechanical structure that controls the movement of the magnet to perform magnetic field encoding. It should be noted that the bias magnetic field can be integrated in the alternating magnetic field generator, that is, a constant current is superimposed on the alternating current; if the magnetic sensor can perform high-precision measurement without a bias magnetic field, the bias magnet 32 can also be omitted.

[0078] Multiple magnetic sensors 4 of the magnetic sensor array can independently and real-time detect the magnetic induction intensity and send the real-time detected magnetic induction intensity data to the processor, and the processor compares the magnitudes of the magnetic induction intensities detected by each magnetic sensor 4 in real time;

[0079] Adjust the positions of the first object 1 and / or the second object 2 until the processor compares and obtains that the magnetic induction intensities detected by more than half of the magnetic sensors 4 in real time are the same or the difference in magnetic induction intensity is less than a preset threshold or ratio threshold. Specifically, for example, the difference in magnetic induction intensity between the maximum magnetic induction intensity and the minimum magnetic induction intensity is less than 1 mGs, or the ratio of the difference in magnetic induction intensity to the current average magnetic induction intensity is less than 1%, then it is determined that the first object 1 at the current position is aligned with the reference area on the second object 2.

[0080] Specifically, the second object 2 preferably has a flat surface, and a reference area to be aligned with the first object 1 is provided on the flat surface, and the center point of the reference area is determined. Taking the application of the magnetic positioning device in a radiographic imaging system as an example, the radiographic imaging system includes a radiation source, a collimator, and a flat panel detector. The magnetic positioning device includes a processor, a magnetic field generating device, and a magnetic sensor array. The magnetic field generating device of the magnetic positioning device is coaxially arranged with the collimator (that is, the collimator is the first object 1), and an optional bias coil and an alternating magnetic field coil are coaxial. In such a configuration, the magnet will generate a centrally symmetric spatial magnetic field in space with the X-ray beam as the axis; multiple magnetic sensors 4 of the magnetic sensor array are distributed on the flat panel detector (that is, the flat panel detector is the second object 2), and it is not limited to the form of the number and position of the magnetic sensors 4 shown in Figure 1 the four magnetic sensors 4 located at the four corners of the flat panel.

[0081] AsFigure 1 Taking the setting form of the four magnetic sensors as an example, when the positioning starts, the four magnetic sensors work simultaneously, and the magnetic field amplitude generated by the magnetic field generator is demodulated through the phase-locked filtering and amplification technology. The position of each magnetic sensor relative to the beam limiter can be calculated based on the amplitude. Since 4 magnetic sensors are used, if one of them is interfered and the magnetic field amplitude measurement is inaccurate, the detector can still determine the position of the flat panel through the remaining three. To further improve the anti-interference ability of the system, an array composed of more magnetic sensors can be used. For example, an array composed of eight magnetic sensors is used, and accurate positioning can still be achieved after at most three sensors are interfered, as described below.

[0082] When the flat panel detector takes a picture, it needs to be closely attached to the area to be photographed of the person to be photographed, and it is required that the center line of the X-ray beam is aligned and perpendicular to the central area of the flat panel detector (the area range near the center of the flat panel). This requires the doctor to accurately determine the relative placement position of the flat panel detector and the X-ray source (beam limiter). The magnetic positioning technical solution provided by the embodiments of the present invention can provide positioning guidance for the doctor. The main inventive concept is as follows: The magnetic field generating device at least includes an alternating magnetic field generator for generating an alternating magnetic field. The multiple magnetic sensors of the magnetic sensor array can independently detect the magnetic induction intensity and send the real-time detected magnetic induction intensity data to the processor. The processor determines the positional relationship between the beam limiter and the flat panel detector according to the magnitudes of the magnetic induction intensities detected by each magnetic sensor in real time.

[0083] To further improve the reliability of positioning, the alternating magnetic field generator 31 can perform magnetic field encoding. The magnetic field generating device and the magnetic sensor array can communicate bidirectionally. The alternating magnetic field generator 31 can adjust the magnetic field intensity it generates according to the relative positions of the first object and the second object determined several times. The method for determining the relative positions of the two objects is described in detail below. For example, the magnetic field intensity generated by the alternating magnetic field generator 31 is adjusted to become weaker. When the magnetic sensor array senses that the magnetic field of each axis of each magnetic sensor is weak, the magnetic sensor array can notify the magnetic field generating device to try to increase the magnetic field intensity through wireless communication; conversely, when the magnetic sensor array senses that the magnetic field is too strong and exceeds its range, it notifies the magnetic field generating device to reduce the magnetic field.

[0084] In an embodiment of the present invention, a real-time spatial precise magnetic positioning method is provided. As Figure 4 shown, the magnetic positioning method includes the following steps:

[0085] S11. Coaxially arrange the magnetic field generating device with the first object, set a reference area to be aligned on the second object, and determine the center point of the reference area; and arrange a plurality of magnetic sensors on the second object so that the distances from the magnetic sensors to the center point are the same;

[0086] S12. The magnetic field generating device generates an alternating magnetic field, and the multiple magnetic sensors independently and real-time detect the magnetic induction intensity;

[0087] S13. Compare the magnitudes of the magnetic induction intensities detected in real time by the multiple magnetic sensors;

[0088] S14. If the magnitudes of the magnetic induction intensities detected in real time by more than half of the magnetic sensors are the same, or the difference in magnetic induction intensity is less than a preset threshold or proportional threshold, then the first object at the current position is aligned with the reference area on the second object, and the positioning ends; otherwise, execute S15;

[0089] S15. Adjust the positions of the first object and / or the second object, and then repeat steps S12 - S14.

[0090] As can be seen from the above, when the distances from all the arranged magnetic sensors to the center point of the reference area on the second object to be aligned with the first object are the same, it can be known through geometric relationships that three points can determine a plane. Therefore, preferably, the number of magnetic sensors is greater than or equal to three. In the case of being greater than or equal to three, only when the magnetic field generating device is directly opposite the center of the circle where these three or more magnetic sensors are located, is it possible for the magnitudes of the magnetic induction intensities detected in real time by the multiple magnetic sensors to be the same. In actual operation, it is the most ideal situation that the first object is completely directly opposite the center point of the second object; in the case where a certain deviation is allowed, then when the difference in magnetic induction intensity detected in real time by the magnetic sensors is less than a preset threshold or proportional threshold, it should also be possible to be used as the basis for determining alignment. For example, the magnetic induction intensity difference is obtained by subtracting the minimum magnetic induction intensity value from the maximum magnetic induction intensity value among all the magnetic induction intensity values detected by all the magnetic sensors. If this magnetic induction intensity difference is less than 1 mGs, or the ratio of the magnetic induction intensity difference to the current average magnetic induction intensity is less than 1%, it is determined that the first object 1 at the current position (within the allowed error range) is aligned with the reference area on the second object 2.

[0091] Preferably, the number of magnetic sensors is set to more than three, such as four. Then, when one of the magnetic sensors is interfered, it will not affect magnetic positioning; or eight magnetic sensors are set. If three of the magnetic sensors are interfered, then it is still possible to determine whether the first object and the second object are aligned based on the remaining five non-interfered magnetic sensors. Specifically, for example, if six of the eight magnetic sensors detect the same magnitude of magnetic induction intensity in real time, and the magnetic induction intensity values detected by the remaining two are different, then it can be determined that these two magnetic sensors are interfered, and the detection results under their interference can be excluded.

[0092] The technical solutions of the above embodiments limit that the distances between the magnetic sensors and the center point are the same. Under this condition, the distances and coordinate values between the specific magnetic sensors (or the center point) and the magnetic field generating device do not need to be considered. In the following embodiments of the present invention, a real-time spatial precise magnetic positioning device based on another inventive concept is provided for determining the positional relationship between a reference area on a first object 1 and a second object 2. The magnetic positioning device includes a processor, a magnetic field generating device coaxially arranged with the first object 1, and a magnetic sensor array arranged on the second object 2. The magnetic field generating device at least includes an alternating magnetic field generator 31 for generating an alternating magnetic field. The magnetic sensor array is configured such that the installation positions of its multiple magnetic sensors 4 have a determined positional relationship with the center point of the reference area;

[0093] In this embodiment, the positions of the magnetic sensors 4 can be set arbitrarily, and then the positional relationship between the magnetic sensors 4 and the center point can be determined. For example, in a pre-established coordinate system, with the magnetic sensor 4 as the origin, the coordinates of the center point are (x1, y1, z1).

[0094] The multiple magnetic sensors 4 of the magnetic sensor array can independently and real-time detect the magnetic induction intensity and send the real-time detected magnetic induction intensity data to the processor. Since the frequency of the magnetic field change is known, techniques such as lock-in amplification can be used for noise filtering and suppression to obtain high-precision magnetic field measurement values. The processor can obtain the precise position of the magnetic sensor relative to the magnetic field generating device through corresponding operations based on the detection results of each magnetic sensor, that is, the position coordinates of each magnetic sensor 4 relative to the magnetic field generating device. The position coordinates here are based on the same coordinate system (the x / y / z axis directions being the same is called the same coordinate system) as above, with the magnetic field generating device as the origin. If the coordinates of the magnetic sensor are (x2, y2, z2), then the coordinates of the center point relative to the magnetic field generating device are (x1 + x2, y1 + y2, z1 + z2).

[0095] Specifically, the magnetic field assignment can be obtained by real-time detecting the magnetic induction intensity of the magnetic sensor. One of the classical algorithms for calculating the positions of each magnetic sensor relative to the magnetic field generating device according to the amplitude is the Biot-Savart Law: The magnitude of the magnetic induction intensity dB generated by a current element Idl at a point P in space is proportional to the magnitude of the current element Idl, proportional to the sine of the angle between the position vector from the location of the current element Idl to point P and the current element Idl, and inversely proportional to the square of the distance from the current element Idl to point P. The classical formula is as follows:

[0096]

[0097] Where, I is the source current, L is the integration path, dl is the infinitesimal line element of the source current, is the unit vector pointing from the current element to the field point to be calculated, μ0 is the magnetic permeability of vacuum, and its value is 4π×10 -7 N / A 2 , the direction of dB is perpendicular to the plane determined by Idl and , r is the distance of the magnetic sensor relative to the magnetic field generating device, is the vector pointing from the magnetic sensor to the magnetic field generating device.

[0098] The vector calculated by the above formula can be converted into coordinates in the coordinate system.

[0099] If the position of the coil relative to the magnetic sensor is far enough, the following approximation can be used:

[0100] Where, m = NSIn,

[0101] Where, N is the number of turns of the coil, S is the area of the coil, I is the current of the coil, n represents the direction, which is along the axis of the coil and perpendicular to the coil plane.

[0102] In principle, only one magnetic sensor is needed to determine the positional relationship of the center point relative to the magnetic field generating device. However, in order to improve the positioning accuracy and prevent the phenomenon that the positioning result is incorrect due to the interference of the magnetic sensor, the magnetic sensor array is preferably provided with three or more magnetic sensors. If one magnetic sensor is interfered, it will not affect the position judgment of the entire array, and the stability is high.

[0103] The processor obtains the position coordinates of the center point of the reference area relative to the first object 1 according to the position coordinates of more than half of the magnetic sensors 4 relative to the magnetic field generating device and the positional relationship between the magnetic sensor 4 and the center point of the reference area. For example, if the number of magnetic sensors is six, and the center point coordinates obtained from the real-time detection results of four magnetic sensors are all (x’, y’, z’), while the center point coordinates obtained from the real-time detection results of the other two magnetic sensors are different from them, it can be determined that these two magnetic sensors are interfered, and the detection results under their interference can be excluded.

[0104] For the technical solution of the real-time spatial precise magnetic positioning device, the processor obtains the position coordinates of the center point of the reference area relative to the first object 1 as the three-dimensional coordinates of the x / y / z axes. The processor analyzes the three-dimensional coordinates. If the coordinates of the two coordinate axes parallel to the plane where the first object 1 is located are 0, it is determined that the first object 1 at the current position is aligned with the reference area on the second object 2. In actual operation, the most ideal situation is that the first object is exactly facing the center point of the second object. For example, taking the magnetic field generating device as the origin to establish a coordinate system, and taking the center line of the beam expander as the x-axis (it can also be the y-axis or z-axis), then when the coordinate value of the center point is (50, 0, 0), it indicates that the current first object 1 is exactly facing the center point of the second object 2; in the case where a certain deviation is allowed, for example, among the coordinate values of the center point, the coordinate values of the y-axis and the z-axis are within a preset range close to 0, such as the range [-5, +5], for example, the center point coordinates corresponding to different magnetic sensors are (50, 5, -5), (50, -5, 5), (50, 5, 5), (50, -5, -5), etc., it can be determined that the first object 1 at the current position (within the allowed error range) is aligned with the reference area on the second object 2.

[0105] Same as the previous embodiment, except for the alternating magnetic field generator, the magnetic field generating device in this embodiment preferably further includes a bias magnet 32 for biasing the magnetic sensor 4 to a preset working magnetic field range, and the bias magnet 32 is a permanent magnet or an electromagnet.

[0106] In order to further improve the reliability of positioning, the alternating magnetic field generator 31 can perform magnetic field encoding, the magnetic field generating device and the magnetic sensor array can communicate bidirectionally, and the alternating magnetic field generator 31 can adjust the magnetic field strength it generates according to the relative positions of the first object and the second object determined several times, such as weakening the magnetic field strength generated by the alternating magnetic field generator 31. That is, when the magnetic sensor array senses that the magnetic field of each axis of each magnetic sensor is weak, the magnetic sensor array can notify the magnetic field generating device to try to increase the magnetic field strength through wireless communication; conversely, when the magnetic sensor array of the magnetic field senses that the magnetic field is too strong and exceeds its range, it notifies the magnetic field generating device to reduce the magnetic field.

[0107] In an embodiment of the present invention, another real-time spatial precise magnetic positioning method is provided. See Figure 5 , including the following steps:

[0108] S21. Coaxially arrange the magnetic field generating device with the first object, set a reference area to be aligned on the second object, and determine the center point of the reference area; and arrange a plurality of magnetic sensors on the second object to obtain the positional relationship between each magnetic sensor and the center point.

[0109] S22. The magnetic field generating device generates an alternating magnetic field, and the multiple magnetic sensors independently and real-time detect the magnetic induction intensity;

[0110] S23. According to the detection data of the magnetic induction intensity, calculate the position coordinates of each magnetic sensor relative to the magnetic field generating device respectively;

[0111] S24. If the position coordinates of one or less than half of the magnetic sensors deviate from the plane determined by the position coordinates of the remaining magnetic sensors, then the one or less than half of the magnetic sensors are excluded as the disturbed magnetic sensors, and only according to the position coordinates of the remaining magnetic sensors relative to the magnetic field generating device and the position relationship between the remaining magnetic sensors and the center point, obtain the position coordinates of the center point relative to the first object; if there is no deviation, that is, all the magnetic sensors are in the same plane, then according to the position coordinates of all or any part of the magnetic sensors relative to the magnetic field generating device and the position relationship between all or any part of the magnetic sensors and the center point, obtain the position coordinates of the center point relative to the first object.

[0112] S25. If in the x / y / z axis three-dimensional coordinates of the center point relative to the first object, the coordinates of the two coordinate axes parallel to the plane where the first object is located are 0 or within a preset range close to 0, then the first object at the current position is aligned with the reference area on the second object, and the positioning ends. For details, refer to the above; otherwise, execute S26;

[0113] S26. Adjust the positions of the first object and / or the second object, and then repeat steps S22 - S25.

[0114] It should be noted that: the above embodiments of the magnetic positioning method and the magnetic positioning device provided by the above embodiments belong to the same concept. For the specific implementation process, refer to the device embodiments. That is, all the features in the above device embodiments can be introduced into the method embodiments by reference, and will not be elaborated here.

[0115] The third structural form of the alternating magnetic field generator 31 is proposed in the embodiment of the present invention: the alternating magnetic field generator 31 includes two modulation coils at a preset angle, and the two modulation coils work alternately. If the magnetic sensor is in an area where the coil gradient of one coil is less than 0.01 mT / m, then the relative position between the first object 1 and the second object 2 can be obtained according to the other coil.

[0116] Specifically, the alternating magnetic field generator 31 and the sensor array are each provided with an angle sensor, and the angle sensor is used to calculate the azimuth angles of the first object (1) and the second object 2 respectively while the magnetic sensor measures the magnetic field, so as to determine the attitude between the two objects; and / or

[0117] The alternating magnetic field generator 31 and the sensor array are each equipped with an acceleration sensor. The acceleration sensor is used to calculate the three-dimensional acceleration data of the first object 1 and the second object 2 respectively while the magnetic sensor measures the magnetic field, so as to determine the acceleration, velocity and relative position of the first object 1 and the second object 2, and is used to correct the magnetic positioning. The specific correction method will be described in detail below.

[0118] The method for obtaining the relative position between the first object 1 and the second object 2 according to another coil specifically includes the following steps:

[0119] S31. Set two coils of the magnetic field generator on the first object at a preset angle, where the preset angle is not equal to 90°. Set a reference area to be aligned on the second object and determine the center point of the reference area; and arrange a plurality of magnetic sensors on the second object to obtain the position relationship between each magnetic sensor and the center point;

[0120] S32. The two coils of the magnetic field generator work alternately to generate an alternating magnetic field, and the plurality of magnetic sensors independently and real-time detect the magnetic induction intensity;

[0121] S33. One alternating operation of the two coils is recorded as a group, and the detection data of the magnetic induction intensity is divided into several groups; according to the detection data of the magnetic induction intensity, calculate the position coordinates of each magnetic sensor relative to the magnetic field generator respectively;

[0122] S34. If the difference between the two position coordinates in a certain group exceeds the preset threshold, compare with the detection data of other magnetic sensors and discard the position coordinate data with a larger deviation;

[0123] S35. According to the remaining position coordinate data after the processing in S34 and the position relationship between the other magnetic sensors and the center point, obtain the position coordinates of the center point relative to the first object;

[0124] S36. If among the three-dimensional coordinates of the x / y / z axes of the center point relative to the first object, the coordinates of the two coordinate axes parallel to the plane where the first object is located are 0 or within a preset range close to 0, the first object at the current position is aligned with the reference area on the second object, and the positioning ends; otherwise, execute S37;

[0125] S37. Adjust the position of the first object and / or the second object, and then repeat steps S32 - S36.

[0126] The method for correcting the magnetic positioning described above includes the following steps:

[0127] S41. Using the magnetic positioning method described above, calculate the relative position of the first object and the second object at time t0 using the three-dimensional magnetic field data of the magnetic field sensor;

[0128] S42. At time t1, use the magnetic sensor, accelerometer, and angle sensor to obtain the three-dimensional magnetic field data, three-dimensional acceleration data, and three-dimensional attitude data at time t1 respectively;

[0129] S43. According to the three-dimensional magnetic field data and three-dimensional attitude data at time t1, calculate the relative position pt1 of the first object and the second object at time t1; according to the three-dimensional acceleration data at time t1, calculate the relative velocity v1 of the first object and the second object at time t1;

[0130] S44. At time t2, use the magnetic sensor, accelerometer, and angle sensor to obtain the three-dimensional magnetic field data, three-dimensional acceleration data, and three-dimensional attitude data at time t2 respectively;

[0131] S45. According to the three-dimensional magnetic field data and three-dimensional attitude data at time t2, calculate the relative position pt2 of the first object and the second object at time t2; according to the three-dimensional acceleration data at time t2 and the relative velocity v1 at time t1, calculate the relative position pt2' of the first object and the second object at time t2;

[0132] S46. Compare pt2 and pt2'. If the error between the two is within 1 cm, then the relative position of the first object and the second object at time t2 is the average value of pt2 and pt2'; otherwise, execute S47 - S48;

[0133] S47. Compare the three-dimensional acceleration value at time t2 and the velocity v1 at time t1. If both the velocity v1 and the three-dimensional acceleration are approximately 0, then the relative position of the first object and the second object at time t2 is pt2; otherwise, it is pt2';

[0134] S48. Adjust the position of the first object and / or the second object, and then repeat steps S41 - S46.

[0135] The present invention can allow multiple magnetic sensor arrays to be located simultaneously, that is, the positioning of multiple objects is achieved. This is because different magnetic sensor arrays can communicate with the magnetic field generating device independently and determine their positions relative to the magnetic field generating device. In order to better control the spatial magnetic field and improve the positioning accuracy, multiple magnetic field generating devices can also be used and placed at different spatial positions.

[0136] Some magnetically sensitive devices, such as cardiac pacemakers, etc., cannot be applied to magnetic positioning because, although weak magnetic fields are used, the magnetic field may still be greater than the Earth's magnetic field, especially near the magnetic field generating device. For this reason, an additional magnetic sensor array can be installed near these magnetic field sensitive devices. As described above, the present technology can use a magnetic field generating device to locate multiple magnetic sensor arrays. For this purpose, the position of the magnetically sensitive device can be determined in real time during positioning, and the magnetic field near the magnetically sensitive device can be actively reduced to the Earth's magnetic field level to ensure that it is not affected.

[0137] The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention shall be included within the protection scope of the present invention.

Claims

1. A real-time spatial precise magnetic positioning method, characterized in that It includes the following steps: S31. Set two coils of the magnetic field generator on the first object at a preset angle, where the preset angle is not equal to 90°. Set a reference area to be aligned on the second object and determine the center point of the reference area; And arrange a plurality of magnetic sensors on the second object to obtain the positional relationship between each magnetic sensor and the center point; S32. The two coils of the magnetic field generator work alternately to generate an alternating magnetic field, and the plurality of magnetic sensors independently and real-time detect the magnetic induction intensity; S33. One cycle of the two coils working alternately is recorded as a group, and the detection data of the magnetic induction intensity is divided into several groups; according to the detection data of the magnetic induction intensity, calculate the position coordinates of each magnetic sensor relative to the magnetic field generator respectively; S34. If the difference between the two position coordinates in a certain group exceeds the preset threshold, compare with the detection data of other magnetic sensors and discard the position coordinate data with a larger deviation; S35. According to the remaining position coordinate data after the processing of S34 and the positional relationship between the other magnetic sensors and the center point, obtain the position coordinates of the center point relative to the first object; S36. If in the x / y / z axis three-dimensional coordinates of the center point relative to the first object, the coordinates of the two coordinate axes parallel to the plane where the first object is located are 0 or within a preset range close to 0, then the first object at the current position is aligned with the reference area on the second object and the positioning ends; otherwise, execute S37; S37. Adjust the position of the first object and / or the second object, and then repeat steps S32 - S36.

2. A real-time spatial precise magnetic positioning correction method, characterized in that, It includes the following steps: S41. Use the magnetic positioning method as described in claim 1, and calculate the relative position between the first object and the second object at time t0 using the three-dimensional magnetic field data of the magnetic field sensor; S42. At time t1, use the magnetic sensor, accelerometer, and angle sensor to obtain the three-dimensional magnetic field data, three-dimensional acceleration data, and three-dimensional attitude data at time t1 respectively; S43. According to the three-dimensional magnetic field data and three-dimensional attitude data at time t1, calculate the relative position pt1 between the first object and the second object at time t1; according to the three-dimensional acceleration data at time t1, calculate the relative velocity v1 between the first object and the second object at time t1; S44. At time t2, use the magnetic sensor, accelerometer, and angle sensor to obtain the three-dimensional magnetic field data, three-dimensional acceleration data, and three-dimensional attitude data at time t2 respectively; S45. According to the three-dimensional magnetic field data and three-dimensional attitude data at time t2, calculate the relative position pt2 between the first object and the second object at time t2; according to the three-dimensional acceleration data at time t2 and the relative velocity v1 at time t1, calculate the relative position pt2' between the first object and the second object at time t2; S46. Compare pt2 and pt2'. If the error between the two is within 1 cm, then the relative position between the first object and the second object at time t2 is the average value of pt2 and pt2'; otherwise, execute S47 - S48; S47. Compare the three-dimensional acceleration value at time t2 with the velocity v1 at time t1. If both the velocity v1 and the three-dimensional acceleration are approximately 0, the relative position of the first object and the second object at time t2 is pt2; otherwise, it is pt2'. S48. Adjust the positions of the first object and / or the second object, and then repeat steps S41 - S46.

3. A real-time spatial precise magnetic positioning correction method, characterized in that, It includes the following steps: S41. Using the following magnetic positioning method, calculate the relative position of the first object and the second object at time t0 using the three-dimensional magnetic field data of the magnetic field sensor: S11. Coaxially set the magnetic field generating device with the first object, set a reference area to be aligned on the second object, and determine the center point of the reference area; And arrange a plurality of magnetic sensors on the second object so that the distances from the magnetic sensors to the center point are the same; S12. The magnetic field generating device generates an alternating magnetic field, and the plurality of magnetic sensors independently and real-time detect the magnetic induction intensity; S13. Compare the magnitudes of the magnetic induction intensities real-time detected by the plurality of magnetic sensors; S14. If the magnitudes of the magnetic induction intensities real-time detected by more than half of the magnetic sensors are the same or the difference in magnetic induction intensity is less than a preset threshold or proportional threshold, the first object at the current position is aligned with the reference area on the second object, and the positioning ends; otherwise, execute S15; S15. Adjust the positions of the first object and / or the second object, and then repeat steps S12 - S14; S42. At time t1, use the magnetic sensor, accelerometer, and angle sensor to respectively obtain the three-dimensional magnetic field data, three-dimensional acceleration data, and three-dimensional attitude data at time t1. S43. According to the three-dimensional magnetic field data and three-dimensional attitude data at time t1, calculate the relative position pt1 of the first object and the second object at time t1; according to the three-dimensional acceleration data at time t1, calculate the relative velocity v1 of the first object and the second object at time t1. S44. At time t2, use the magnetic sensor, accelerometer, and angle sensor to respectively obtain the three-dimensional magnetic field data, three-dimensional acceleration data, and three-dimensional attitude data at time t2. S45. According to the three-dimensional magnetic field data and three-dimensional attitude data at time t2, calculate the relative position pt2 of the first object and the second object at time t2; according to the three-dimensional acceleration data at time t2 and the relative velocity v1 at time t1, calculate the relative position pt2' of the first object and the second object at time t2. S46. Compare pt2 and pt2'. If the error between the two is within 1 cm, the relative position of the first object and the second object at time t2 is the average value of pt2 and pt2'; otherwise, execute S47 - S48; S47. Compare the three-dimensional acceleration value at time t2 with the velocity v1 at time t1. If both the velocity v1 and the three-dimensional acceleration are approximately 0, the relative position of the first object and the second object at time t2 is pt2; otherwise, it is pt2'. S48. Adjust the positions of the first object and / or the second object, and then repeat steps S41 - S46.

4. A real-time spatial precise magnetic positioning correction method, characterized in that It includes the following steps: S41. Using the following magnetic positioning method, calculate the relative position of the first object and the second object at time t0 using the three-dimensional magnetic field data of the magnetic field sensor: S21. Coaxially arrange the magnetic field generating device with the first object, set a reference area to be aligned on the second object, and determine the center point of the reference area; And arrange multiple magnetic sensors on the second object to obtain the positional relationship between each magnetic sensor and the center point; S22. The magnetic field generating device generates an alternating magnetic field, and the multiple magnetic sensors independently detect the magnetic induction intensity in real time; S23. According to the detection data of the magnetic induction intensity, calculate the position coordinates of each magnetic sensor relative to the magnetic field generating device respectively; S24. If there is one or less than half of the magnetic sensors whose position coordinates deviate from the plane determined by the position coordinates of the remaining magnetic sensors, then the one or less than half of the magnetic sensors are excluded as the disturbed magnetic sensors, and only based on the position coordinates of the remaining magnetic sensors relative to the magnetic field generating device and the positional relationship between the remaining magnetic sensors and the center point, obtain the position coordinates of the center point relative to the first object; S25. If among the three-dimensional coordinates of the center point relative to the first object on the x / y / z axes, the coordinates of the two coordinate axes parallel to the plane where the first object is located are 0 or within a preset range close to 0, then the first object at the current position is aligned with the reference area on the second object, and the positioning ends; otherwise, execute S26; S26. Adjust the position of the first object and / or the second object, and then repeat steps S22 - S25; S42. At time t1, use a magnetic sensor, an accelerometer, and an angle sensor to respectively obtain the three-dimensional magnetic field data, three-dimensional acceleration data, and three-dimensional attitude data at time t1; S43. According to the three-dimensional magnetic field data and three-dimensional attitude data at time t1, calculate the relative position pt1 of the first object and the second object at time t1; according to the three-dimensional acceleration data at time t1, calculate the relative velocity v1 of the first object and the second object at time t1; S44. At time t2, use a magnetic sensor, an accelerometer, and an angle sensor to respectively obtain the three-dimensional magnetic field data, three-dimensional acceleration data, and three-dimensional attitude data at time t2; S45. According to the three-dimensional magnetic field data and three-dimensional attitude data at time t2, calculate the relative position pt2 of the first object and the second object at time t2; according to the three-dimensional acceleration data at time t2 and the relative velocity v1 at time t1, calculate the relative position pt2' of the first object and the second object at time t2; S46. Compare pt2 and pt2'. If the error between the two is within 1 cm, then the relative position of the first object and the second object at time t2 is the average value of pt2 and pt2'; otherwise, execute S47 - S48; S47. Compare the three-dimensional acceleration value at time t2 and the velocity v1 at time t1. If both the velocity v1 and the three-dimensional acceleration are approximately 0, then the relative position of the first object and the second object at time t2 is pt2; otherwise, it is pt2'; S48. Adjust the position of the first object and / or the second object, and then repeat steps S41 - S46.

Images