Six degrees of freedom magnetic positioning system

Abstract

The application discloses a six-degree-of-freedom magnetic positioning system, and relates to the technical field of magnetic positioning. A Y-shaped three-axis rotating structure is adopted in a magnetic field generator unit, which comprises a bearing frame and a servo driving system. The bearing frame defines three rotating axes which are symmetrically distributed at a certain angle (for example, 120 DEG) through a special mechanical structure, and is integrated with a permanent magnet with a mass center and a non-magnetic counterweight. The servo driving system adopts a brushless servo motor matched with a precision angular contact bearing set to drive the permanent magnet-counterweight combination to revolve around a first main shaft, while the permanent magnet and the counterweight respectively revolve around a second secondary shaft and a third secondary shaft. Dynamic coupling of the three kinds of rotating motions generates an asymmetric magnetic field with obvious space-time characteristics, so that high-precision and non-fuzzy positioning or orientation is realized.

Description

Technical Field

[0001] This invention relates to the field of magnetic positioning technology, and in particular to a six-degree-of-freedom magnetic positioning system based on a Y-shaped rotating permanent magnet. Background Technology

[0002] Magnetic positioning technology, a spatial pose measurement method based on electromagnetic field sensors, achieves non-contact position and attitude calculation by detecting the magnetic field distribution characteristics around a target object. This technology exhibits unique advantages in fields such as industrial automation, medical navigation, and virtual reality, primarily due to its non-ionizing radiation characteristics, high system deployment flexibility, and insensitivity to line-of-sight obstruction. Compared to optical and ultrasonic positioning methods, magnetic positioning systems demonstrate superior adaptability in complex environments, particularly suitable for applications with visual impairments or requiring penetration of non-ferromagnetic media. By establishing a mapping relationship between magnetic field strength and spatial position, combined with advanced signal processing algorithms, this technology can achieve sub-millimeter-level positioning accuracy, providing crucial technical support for modern precision measurement.

[0003] However, existing technologies still have the following key problems:

[0004] (1) Limitations of static magnetic field positioning.

[0005] The magnetic field generated by a static permanent magnet or fixed electromagnetic coil exhibits rapid gradient decay and poor directionality in its spatial distribution, leading to decreased positioning accuracy at long distances. For example, positioning systems based on a single magnetic dipole experience a rapid increase in error to the centimeter level at distances exceeding 0.5m. Furthermore, static magnetic fields are susceptible to interference from environmental ferromagnetic materials (such as metal equipment and building structures), causing positioning drift.

[0006] (2) Shortcomings of the dynamic magnetic field scheme.

[0007] To enhance anti-interference capabilities, some studies employ rotating or alternating magnetic fields to strengthen signal characteristics, such as the cross-shaped rotating permanent magnet scheme. However, the cross-shaped structure has high magnetic field symmetry, resulting in a detection blind zone in certain directions (such as the 45° diagonal), and the magnetic field modulation depth is insufficient during rotation, making it difficult to maintain stable accuracy in noisy environments.

[0008] Therefore, how to develop a novel magnetic positioning system that can enhance the directionality of the magnetic field while effectively suppressing environmental interference and achieving high-precision, low-latency position calculation is a technical problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0009] In view of the above problems, this invention provides a six-degree-of-freedom magnetic positioning system to overcome or at least partially solve the above problems. Employing a Y-shaped rotating permanent magnet structure, and through asymmetric magnetic field distribution and rotation modulation technology, it overcomes the aforementioned technical bottlenecks, providing an innovative solution for magnetic positioning in complex environments. It is particularly suitable for scenarios requiring high positioning accuracy and anti-interference capabilities, such as indoor robot navigation, minimally invasive surgical instrument tracking, and industrial equipment positioning.

[0010] This invention provides the following solution:

[0011] A six-degree-of-freedom magnetic positioning system, comprising:

[0012] A magnetic field generator unit includes a drive assembly, a permanent magnet, and a non-magnetic counterweight. The drive assembly's drive support frame revolves around a first main axis, the permanent magnet rotates around a second secondary axis, and the non-magnetic counterweight rotates around a third secondary axis. The first main axis, the second secondary axis, and the third secondary axis intersect at a spatial intersection point and are distributed in a target symmetrical spatial angle.

[0013] A magnetic field sensor unit, comprising at least one triaxial magnetic sensor with X, Y, and Z triaxial magnetic induction intensity and magnetic flux measurement functions, wherein the sampling frequency of the triaxial magnetic sensor is not less than 10 times the rotational fundamental frequency;

[0014] The signal processing unit is used to run a motor drive control program and a position calculation algorithm. The motor drive control program is used to adjust the speed and synchronize the phase of the drive component. The position calculation algorithm is used to calculate the six-degree-of-freedom pose information in real time based on a pre-set magnetic field position mathematical model and magnetic field measurement data collected by the magnetic field sensor unit. The improved particle filtering algorithm includes a trust domain strategy and Jacobian matrix regularization processing.

[0015] Preferably, the magnetic field generator unit further includes a first bevel gear, a second bevel gear, and a third bevel gear; the first bevel gear, the second bevel gear, and the third bevel gear are respectively connected to the first main shaft, the second secondary shaft, and the third secondary shaft in a one-to-one correspondence, and the second bevel gear and the third bevel gear are both meshed with the first bevel gear, so that the second bevel gear and the third bevel gear revolve around the first main shaft during transmission;

[0016] The permanent magnet is connected to the second bevel gear, and the non-magnetic counterweight is connected to the third bevel gear; the center of mass of the permanent magnet is collinear with the rotation line of the second secondary shaft, and the center of mass of the non-magnetic counterweight is collinear with the rotation line of the third secondary shaft.

[0017] Preferably, the ratio of the revolution angular velocity to the rotation angular velocity of the second bevel gear and the third bevel gear is an integer or the reciprocal of an integer.

[0018] Preferably, the permanent magnet has a rotationally symmetric shape, and the centroid offset of the permanent magnet is no more than 0.1 mm; the magnetic field distribution of the permanent magnet is mathematically described by one or more magnetic dipole field models, and the model fitting error is ≤5%; the magnetization performance of the permanent magnet is within the rated operating range, and the signal-to-noise ratio of the magnetic field signal to the environmental noise is ≥20 dB.

[0019] Preferably, the weight of the non-magnetic counterweight is the same as the weight of a single permanent magnet, and the material of the non-magnetic counterweight includes aluminum alloy or titanium alloy.

[0020] Preferably, the triaxial magnetic sensor includes an anisotropic magnetoresistive sensor; the anisotropic magnetoresistive sensor is connected to the signal processing unit via a 120Ω twisted-pair shielded cable to achieve a low noise performance of 0.4mGauss / √Hz at a 10Hz bandwidth, providing accurate raw magnetic field data for sub-millimeter positioning.

[0021] Preferably, the signal processing unit includes:

[0022] The data acquisition and processing module is configured to receive magnetic field measurement data acquired by the magnetic field sensor unit, sample the data using a 24-bit high-precision ADC, and preprocess the data using digital filtering and adaptive noise reduction algorithms.

[0023] The motor drive control module is configured to use a stepper motor driver to implement PWM speed control, with a speed adjustment range of 100-3000 rpm. It forms a closed-loop control through optical encoder feedback to ensure speed stability of ±0.1%.

[0024] The pose calculation module is configured to use an improved magnetic field position calculation model based on a pre-established model. Optimize the algorithm to perform real-time position calculation. Parallel computation is implemented to ensure real-time output of 30-60fps;

[0025] The output interface module is configured to output six-DOF pose information via Gigabit Ethernet or serial interface, with latency controlled within 15±5ms, and supports OpenGL 3D visualization display and CSV format data storage.

[0026] Preferably, the objective function of the mathematical model for the magnetic field position is expressed by the following formula:

[0027]

[0028] In the formula: This indicates that the improved magnetic dipole model is used to calculate the spatial magnetic field distribution. Represents the sensor attitude rotation matrix. For the first The measurement values ​​of each sensor, This is the geomagnetic field vector.

[0029] Preferably: the improved magnetic dipole model It can be expressed by the following formula:

[0030]

[0031] In the formula: Represents the permeability of free space. This represents the displacement vector of the magnetic sensor relative to the permanent magnet. This represents the magnetic moment vector.

[0032] Preferably, the improved particle filter algorithm includes the following steps:

[0033] Based on the current parameter estimates Calculate the objective function and its 6×6 Jacobian matrix. ;

[0034] Constructing the damping normal equation ;

[0035] Solving the system of linear equations to obtain parameter increments and update the estimate. ;

[0036] The optimal pose solution is output after the iteration termination condition is met.

[0037] According to specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0038] This application provides a six-degree-of-freedom magnetic positioning system. The magnetic field generator unit adopts a Y-shaped three-axis rotating structure design, including a support frame and a servo drive system. The support frame defines three symmetrically distributed rotation axes at a certain angle (e.g., 120°) through a special mechanical structure, and integrates a permanent magnet with a center of mass and a non-magnetic counterweight. The servo drive system uses a brushless servo motor in conjunction with a precision angular contact bearing assembly to drive the permanent magnet-counterweight combination to revolve around the first main axis. Simultaneously, the permanent magnet and counterweight rotate around the second and third secondary axes, respectively. The dynamic coupling of these three rotational motions generates an asymmetric magnetic field with significant spatiotemporal characteristics, thereby achieving high-precision, ambiguity-free positioning or orientation. The magnetic field sensor unit is responsible for collecting magnetic induction intensity or magnetic flux data. The signal processing unit achieves high-speed data acquisition via I2C or SPI bus, and combines it with a pre-calibrated magnetic field-position calculation model, using an improved particle filter algorithm to invert the six-degree-of-freedom target pose in real time. This system effectively suppresses environmental magnetic interference through Y-shaped rotating magnetic field modulation technology, improving positioning accuracy, and is suitable for scenarios such as indoor robot navigation and medical device tracking.

[0039] Of course, any product implementing this invention does not necessarily need to achieve all of the advantages described above at the same time. Attached Figure Description

[0040] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly described below. Obviously, the drawings described below are merely some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without any creative effort.

[0041] Figure 1 This is a connection block diagram of the six-degree-of-freedom magnetic positioning system provided in an embodiment of the present invention;

[0042] Figure 2 This is a schematic diagram of the structure of the magnetic field generator unit provided in an embodiment of the present invention;

[0043] Figure 3 This is a schematic diagram of the pose calculation model provided in an embodiment of the present invention;

[0044] Figure 4 This is a schematic diagram of the installation and use of the magnetic field generator unit provided in an embodiment of the present invention.

[0045] In the diagram: magnetic field generator unit 1, permanent magnet 11, non-magnetic counterweight 12, first main shaft 13, rotation line of the first main shaft 131, second secondary shaft 14, rotation line of the second secondary shaft 141, third secondary shaft 15, rotation line of the third secondary shaft 151, spatial intersection 16, first bevel gear 17, second bevel gear 18, third bevel gear 19, magnetic field sensor unit 2, signal processing unit 3. Detailed Implementation

[0046] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention are within the scope of protection of the present invention.

[0047] See Figure 1 , Figure 2 This invention provides a six-degree-of-freedom magnetic positioning system, such as... Figure 1 , Figure 2 As shown, the system may include:

[0048] The magnetic field generator unit 1 includes a drive assembly, a permanent magnet 11, and a non-magnetic counterweight 12. The drive support frame of the drive assembly revolves around the rotation line 131 of the first main axis, the permanent magnet 11 rotates around the rotation line 141 of the second secondary axis, and the non-magnetic counterweight 12 rotates around the rotation line 151 of the third secondary axis. The first main axis 13, the second secondary axis 14, and the third secondary axis 15 intersect at the spatial intersection point 16 and are distributed in a target symmetrical spatial angle, which includes, but is not limited to, 120°.

[0049] The magnetic field generator unit 1 adopts a precision-machined support frame, whose mechanical structure defines the rotation line 141 of the second secondary shaft and the rotation line 151 of the third secondary shaft; the dynamically balanced permanent magnet 11 and the non-magnetic counterweight 12 are combined, and their center of mass is strictly located at the design position; the high-precision drive system adopts a closed-loop controlled brushless servo motor with a P4-level precision angular contact bearing assembly, and the bearing assembly is rigidly connected to the support frame.

[0050] The drive system drives the load-bearing frame to revolve around the first main axis 13, while the permanent magnet 11 rotates around the rotation line 141 of the second secondary axis, and the non-magnetic counterweight 12 rotates around the rotation line 151 of the third secondary axis. The first main axis 13, the second secondary axis 14, and the third secondary axis 15 intersect at the spatial intersection point 16 and are distributed in a 120° symmetrical spatial angle (120° is just a typical example).

[0051] In a specific implementation, the magnetic field generator unit 1 may further include a first bevel gear 17, a second bevel gear 18, and a third bevel gear 19. The first bevel gear 17, the second bevel gear 18, and the third bevel gear 19 are respectively connected to the first main shaft 13, the second secondary shaft 14, and the third secondary shaft 15 in a one-to-one correspondence. The second bevel gear 18 and the third bevel gear 19 are both meshed with the first bevel gear, so that the second bevel gear 18 and the third bevel gear 19 revolve around the first main shaft 13 during transmission.

[0052] The permanent magnet 11 is connected to the second bevel gear 18, and the non-magnetic counterweight 12 is connected to the third bevel gear; the center of mass of the permanent magnet 11 is collinear with the rotation line 141 of the second secondary shaft, and the center of mass of the non-magnetic counterweight 12 is collinear with the rotation line 151 of the third secondary shaft.

[0053] The ratio of the revolution angular velocity to the rotation angular velocity of the second bevel gear 18 and the third bevel gear 19 is an integer or the reciprocal of an integer.

[0054] The permanent magnet 11 has a rotationally symmetric shape, and the centroid offset of the permanent magnet 11 is no more than 0.1 mm; the magnetic field distribution of the permanent magnet 11 is mathematically described by one or more magnetic dipole field models, and the model fitting error is ≤5%; the magnetization performance of the permanent magnet 11 is within the rated operating range, and the signal-to-noise ratio of the magnetic field signal to the environmental noise is ≥20 dB.

[0055] The weight of the non-magnetic counterweight 12 is the same as the weight of a single permanent magnet 11, and the material of the non-magnetic counterweight 12 includes aluminum alloy or titanium alloy.

[0056] In practical applications, the permanent magnet 11 is made of high-performance permanent magnet materials such as neodymium iron boron, forming a ring structure with rotational symmetry. The permanent magnet 11 (or permanent magnet assembly) must have a defined center of mass, which is fixed to the preset mounting position of the second bevel gear 18 by mechanical snap-fit. Its center of mass is strictly collinear with the rotation axis of the second bevel gear 18 (defined as the rotation line 141 of the second auxiliary shaft), with a coaxiality error ≤ 0.02 mm. During transmission, the planetary gears (the second bevel gear 18 and the third bevel gear 19) always revolve around the fixed rotation axis of the first bevel gear 17 (defined as the first main shaft 13). The spatial angle between the rotation line of the first main shaft 13 and the rotation line of the second auxiliary shaft 14 must be strictly controlled to 120°, with an angle error ≤ 0.5°, to ensure the periodicity and predictability of the magnetic field distribution during the rotation of the permanent magnet 11. The first bevel gear 17 has the following number of teeth: The second bevel gear has 18 teeth. By the gear ratio ( ) Optimize, so that the revolution angular velocity of the second bevel gear 18 and the third bevel gear 19 ( ) and rotational angular velocity ( The ratio of () The value can be an integer (such as 1, 2, 3) or the reciprocal of an integer (such as 1 / 2, 1 / 3) to ensure that the frequency of magnetic field change is proportional to the frequency of gear movement, which facilitates subsequent position calculation and modeling.

[0057] The permanent magnet 11 provided in this application embodiment must meet the following characteristics:

[0058] 1. Shape characteristics: It has rotational symmetry (such as a ring), and the position of the center of mass can be accurately determined by geometric calculation or physical measurement, with a center of mass offset ≤0.1mm;

[0059] 2. Magnetic field distribution characteristics: Its spatial magnetic field distribution can be mathematically described using one or more magnetic dipole field models, with a model fitting error ≤5%;

[0060] 3. Magnetization performance requirements: By setting the magnetization intensity (e.g., ≥1.2T), ensure that the signal-to-noise ratio of the magnetic field signal to the ambient noise is ≥20dB within the rated operating range of the device (e.g., a space with a radius of 1m);

[0061] 4. Calibration requirements: The actual magnetization intensity and magnetization direction of the permanent magnet 11 need to be calibrated using standard measuring equipment (such as a Helmholtz coil magnetometer and a magnetic dipole field calibration device). The calibration data (including the specific value of the magnetization intensity and the magnetization direction angle parameter) need to be stored as an internal parameter in the signal processing unit 3 for the accuracy correction of the subsequent position calculation algorithm.

[0062] The non-magnetic counterweight 12 has the same weight as the single permanent magnet 11 (made of aluminum alloy or titanium alloy, etc.), and is installed on the symmetrical third bevel gear 19 of the permanent magnet 11 arm. Other structures are consistent with the permanent magnet 11 arm. It is used to balance the weight of the module and reduce vibration during high-speed rotation.

[0063] The supporting frame achieves motion control through a high-precision drive system, specifically including: using a DC brushless servo motor to drive the first bevel gear 17 to rotate synchronously, with the speed being steplessly adjustable within a certain range, thereby generating a periodic dynamic spatial magnetic field with specific spectral characteristics; the drive shaft of the motor of the drive component and the rotation line 131 of the first main shaft adopt an H7 / k6 grade interference fit to ensure coaxiality (≤0.02mm), and integrate a photoelectric encoder to monitor the angular position of the first rotating shaft in real time, and the data is transmitted to the signal processing unit 3 for closed-loop compensation; finally, mechanical locking is achieved through M4 grade anti-loosening screws (pre-tightening torque 2.5N•m±5%) to ensure the structural stability of the system under high-speed rotation.

[0064] Magnetic field sensor unit 2, the magnetic field sensor unit 2 includes at least one triaxial magnetic sensor with X, Y, Z triaxial magnetic induction intensity and magnetic flux measurement functions, the sampling frequency of the triaxial magnetic sensor is not less than 10 times the rotation fundamental frequency;

[0065] The magnetic field sensor unit 2 includes: one or more high-precision triaxial magnetic sensors (signal processing unit 3 supports multi-channel parallel acquisition); each sensor has X, Y, Z triaxial magnetic induction intensity and magnetic flux measurement functions; at the same time, it ensures that the sampling frequency of each sensor is not less than 10 times the rotation fundamental frequency.

[0066] In a specific implementation, the embodiments of this application may provide that the triaxial magnetic sensor includes an anisotropic magnetoresistive sensor; the anisotropic magnetoresistive sensor is connected to the signal processing unit 3 through a 120Ω twisted-pair shielded cable, so as to achieve a low noise performance of 0.4mGauss / √Hz in a 10Hz bandwidth, providing raw magnetic field data for sub-millimeter positioning.

[0067] Employing a high-performance anisotropic magnetoresistive sensor with a range of ±8 Gauss, high sensitivity of 0.0625 mGauss / LSB, and sampling frequency above 500 Hz, it can accurately capture dynamic magnetic field changes generated by the rotating permanent magnet 11. The sensor is connected to the signal processing unit 3 via a 120Ω twisted-pair shielded cable, achieving low noise performance of 0.4 mGauss / √Hz at a bandwidth of 10 Hz, providing high-quality raw magnetic field data for sub-millimeter-level positioning.

[0068] The signal processing unit 3 is used to run the motor drive control program and the position calculation algorithm; the motor drive control program is used to adjust the speed and synchronize the phase of the drive component; the position calculation algorithm is used to calculate the six-degree-of-freedom pose information for positioning in real time based on the preset magnetic field position mathematical model and the magnetic field measurement data collected by the magnetic field sensor unit 2, using an improved particle filter algorithm; the improved particle filter algorithm includes a trust domain strategy and Jacobian matrix regularization processing.

[0069] The signal processing unit 3 can be used to run the motor drive control program to achieve precise speed adjustment and phase synchronization; it can also be used to run the position calculation algorithm, which calculates the six-degree-of-freedom pose information in real time based on a pre-set magnetic field-position mathematical model and an improved particle filter algorithm; finally, it provides a data output interface to support the transmission of pose data via Ethernet / USB 3.0 protocol.

[0070] In a specific implementation, the signal processing unit 3 of this application embodiment may include:

[0071] The data acquisition and processing module is configured to receive magnetic field measurement data acquired by the magnetic field sensor unit 2, sample the data using a 24-bit high-precision ADC, and preprocess the data using digital filtering and adaptive noise reduction algorithms.

[0072] The motor drive control module is configured to use a stepper motor driver to implement PWM speed control, with a speed adjustment range of 100-3000 rpm. It forms a closed-loop control through optical encoder feedback to ensure speed stability of ±0.1%.

[0073] The pose calculation module is configured to use an improved magnetic field position calculation model based on a pre-established model. Optimize the algorithm to perform real-time position calculation. Parallel computation is implemented to ensure real-time output of 30-60fps;

[0074] The output interface module is configured to output six-DOF pose information via Gigabit Ethernet or serial interface, with latency controlled within 15±5ms, and supports OpenGL 3D visualization display and CSV format data storage.

[0075] The objective function of the mathematical model for the magnetic field location is expressed by the following equation:

[0076]

[0077] In the formula: This indicates that the improved magnetic dipole model is used to calculate the spatial magnetic field distribution. Represents the sensor attitude rotation matrix. For the first The measurement values ​​of each sensor, This is the geomagnetic field vector.

[0078] The improved magnetic dipole model It can be expressed by the following formula:

[0079]

[0080] In the formula: Represents the permeability of free space. This represents the displacement vector of the magnetic sensor relative to the permanent magnet 11. This represents the magnetic moment vector.

[0081] The improved particle filter algorithm includes the following steps:

[0082] Based on the current parameter estimates Calculate the objective function and its 6×6 Jacobian matrix. ;

[0083] Constructing the damping normal equation ;

[0084] Solving the system of linear equations to obtain parameter increments and update the estimate. ;

[0085] The optimal pose solution is output after the iteration termination condition is met.

[0086] The following provides a detailed description of the specific implementation methods of the various modules and algorithms included in the signal processing unit 3 provided in the embodiments of this application.

[0087] Signal processing unit 3 may include:

[0088] Data acquisition and processing module: Receives magnetic field measurement data acquired by the magnetic sensor array, samples it through a 24-bit high-precision ADC (sampling rate ≥ 10kHz), and performs data preprocessing using digital filtering (bandpass filter center frequency corresponds to the rotational speed fundamental frequency) and adaptive noise reduction algorithm;

[0089] Motor drive control module: PWM speed control is achieved using a stepper motor driver, with a speed adjustment range of 100-3000 rpm (step resolution 1 rpm). Closed-loop control is formed through feedback from an optical encoder, achieving speed stability of ±0.1%.

[0090] Pose calculation module: Based on a pre-established magnetic field-position calculation model, an improved method is used. Optimize the algorithm to perform real-time position calculation. Parallel computation is implemented to ensure real-time output of 30-60fps;

[0091] Output interface module: Outputs six-DOF pose information via Gigabit Ethernet or serial port interface, with latency controlled within 15±5ms, and supports OpenGL 3D visualization display and CSV format data storage.

[0092] This unit integrates a dynamic tracking algorithm based on Kalman filtering, which can process the real-time pose information of up to 6 targets simultaneously.

[0093] Mathematical modeling methods for magnetic field location include:

[0094] Establish a right-handed rectangular coordinate system with the revolution center 34 as the origin, as the global coordinate system; such as Figure 3 As shown.

[0095] definition The magnetic moment vector of permanent magnet 11 at time t in the global coordinate system can be expressed as:

[0096]

[0097] in, Represents the initial magnetic moment vector. Indicates circling The rotation matrix of the axis. This represents the rotation matrix about the X-axis. This represents the angular velocity of the planetary carrier's revolution. This indicates the angular velocity of the permanent magnet 11's rotation. ( The magnetization intensity, (for volume)

[0098] At a certain moment in the global coordinate system The position and orientation of the magnetic sensor are denoted as ( The spatial magnetic field distribution is calculated using an improved magnetic dipole model:

[0099]

[0100] in, Indicates the permeability of free space ( ), The displacement vector of the magnetic sensor relative to the permanent magnet 11 ( The orbital radius is known and denoted as . Its complete column vector representation is:

[0101]

[0102] The sensor signal conversion model is as follows:

[0103]

[0104] in, Sensor attitude rotation matrix , The sensor measurement value. This is the geomagnetic field vector.

[0105] In the preprocessing stage, a 5th-order Butterworth bandpass filter is used for signal filtering, with a cutoff frequency of [missing value]. Geomagnetic field compensation is ;

[0106] Based on the above formula, establish the objective function:

[0107]

[0108]

[0109] The optimized solution process employs an improved method. Algorithm implementation of pose parameters The iterative calculation specifically includes the following key steps:

[0110] First, based on the current parameter estimates Calculate the objective function and its 6×6 Jacobian matrix (Solved numerically using the central difference method, with a step size set to 1% of the parameter value).

[0111] Subsequently, the damping normal equation was constructed. The adaptive damping factor Initially set to 0.1 and dynamically adjusted based on iteration results (if the residual decreases, then...). ,otherwise );

[0112] Next, the parameter increments are obtained by solving the system of linear equations. and update the estimate. The iteration termination condition is set to the relative residual rate of change being less than 1%. It can reach a maximum of 100 iterations and finally output the optimal pose solution.

[0113] This improved algorithm introduces a trust region strategy (limiting the step size). ) and Jacobian matrix regularization (add) (The identity matrix prevents singularities) Compared with the standard LM algorithm, the convergence speed is improved by 30% and it is less likely to get trapped in local extrema. In actual tests, it can still guarantee a 95% convergence success rate when the initial guess value deviates by 20%.

[0114] Figure 4 This is one example of the installation and use of the magnetic field generator. The rotating permanent magnet magnetic positioning device is driven by a motor, which has a permanent magnet 11 stator or rotor inside, inevitably interfering with the magnetic field generated by the device. The low-Y-shaped magnetic field generator provided in this application embodiment has the function of asymmetric magnetic field distribution, avoiding symmetric solutions generated during calculation. In use, the magnetic field generator can be placed upside down on a support above the operating table, utilizing the space below the generator to avoid motor interference, resulting in higher magnetic positioning accuracy.

[0115] In summary, the six-degree-of-freedom magnetic positioning system provided in this application adopts a Y-shaped three-axis rotating structure design for its magnetic field generator unit. This unit includes a support frame and a servo drive system. The support frame defines three symmetrically distributed rotation axes at 120° using a special mechanical structure and integrates a permanent magnet with a center of mass and a non-magnetic counterweight. The servo drive system uses a brushless servo motor paired with precision angular contact bearings to drive the permanent magnet-counterweight combination to revolve around the first main axis. Simultaneously, the permanent magnet and counterweight rotate around the second and third secondary axes, respectively. The dynamic coupling of these three rotational motions generates an asymmetric magnetic field with significant spatiotemporal characteristics, thereby achieving high-precision, ambiguity-free positioning or orientation. The magnetic field sensor unit is responsible for collecting magnetic induction intensity or magnetic flux data. The signal processing unit achieves high-speed data acquisition via I2C or SPI bus. Combined with a pre-calibrated magnetic field-position calculation model, an improved particle filter algorithm is used to invert the six-degree-of-freedom target pose in real time. This system effectively suppresses environmental magnetic interference through Y-shaped rotating magnetic field modulation technology, improving positioning accuracy and making it suitable for scenarios such as indoor robot navigation and medical device tracking.

[0116] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0117] As can be seen from the above description of the embodiments, those skilled in the art can clearly understand that this application can be implemented by means of software plus necessary general-purpose hardware platforms. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in various embodiments or some parts of the embodiments of this application.

[0118] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to mutually. Each embodiment focuses on describing the differences from other embodiments. In particular, for system or system embodiments, since they are basically similar to method embodiments, the description is relatively simple, and relevant parts can be referred to the descriptions in the method embodiments. The systems and system embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without creative effort.

[0119] The above description is merely a preferred embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention are included within the scope of protection of the present invention.

Claims

1. A six-degree-of-freedom magnetic positioning system, characterized in that, include: A magnetic field generator unit includes a drive assembly, a permanent magnet, and a non-magnetic counterweight. The drive frame of the drive assembly revolves around a first main shaft, the permanent magnet rotates around a second secondary shaft, and the non-magnetic counterweight rotates around a third secondary shaft. The first main shaft, the second secondary shaft, and the third secondary shaft intersect at a spatial intersection point. The spatial angle between the second secondary shaft and the first main shaft is 120°, and the spatial angle between the third secondary shaft and the first main shaft is also 120°. The magnetic field generator unit further includes a first bevel gear, a second bevel gear, and a third bevel gear. The first bevel gear, the second bevel gear, and the third bevel gear are respectively connected to the first main shaft, the second secondary shaft, and the third secondary shaft, and the second bevel gear and the third bevel gear are both meshed with the first bevel gear, so that the second bevel gear and the third bevel gear revolve around the first main shaft during transmission. The permanent magnet is connected to the second bevel gear, and the non-magnetic counterweight is connected to the third bevel gear; the center of mass of the permanent magnet is collinear with the rotation line of the second secondary shaft, and the center of mass of the non-magnetic counterweight is collinear with the rotation line of the third secondary shaft; the permanent magnet and the non-magnetic counterweight have equal masses and are symmetrically distributed relative to the first main shaft, so that the magnetic field generator unit achieves dynamic balance during rotation; A magnetic field sensor unit, comprising at least one triaxial magnetic sensor with X, Y, and Z triaxial magnetic induction intensity and magnetic flux measurement functions, wherein the sampling frequency of the triaxial magnetic sensor is not less than 10 times the rotational fundamental frequency; The signal processing unit runs a motor drive control program and a position calculation algorithm. The motor drive control program adjusts the speed and synchronizes the phase of the drive component. The position calculation algorithm calculates six-degree-of-freedom pose information in real time using an improved particle filter algorithm based on a pre-defined magnetic field position mathematical model and magnetic field measurement data collected by the magnetic field sensor unit. The improved particle filter algorithm includes a trust domain strategy and Jacobian matrix regularization. The trust domain strategy limits the step size of each iteration to no more than 0.1 times the revolution radius, and the Jacobian matrix regularization is achieved by adding [a certain parameter] to the damping normal equation. The identity matrix prevents singularities; The objective function of the mathematical model for the magnetic field location is expressed by the following equation: In the formula: This indicates that the improved magnetic dipole model is used to calculate the spatial magnetic field distribution. Represents the sensor attitude rotation matrix. For the first The measurement values ​​of each sensor, It is the geomagnetic field vector; The improved magnetic dipole model It can be expressed by the following formula: In the formula: Represents the permeability of free space. This represents the displacement vector of the magnetic sensor relative to the permanent magnet. Represents the magnetic moment vector; The improved particle filter algorithm includes the following steps: Based on the current parameter estimates Calculate the objective function and its 6×6 Jacobian matrix. ; Constructing the damping normal equation ; Solving the system of linear equations to obtain parameter increments and update the estimate. ; The optimal pose solution is output after the iteration termination condition is met.

2. The six-degree-of-freedom magnetic positioning system according to claim 1, characterized in that, The ratio of the revolution angular velocity to the rotation angular velocity of the second bevel gear and the third bevel gear is an integer or the reciprocal of an integer.

3. The six-degree-of-freedom magnetic positioning system according to claim 1, characterized in that, The permanent magnet has a rotationally symmetric shape, and the centroid offset of the permanent magnet is no more than 0.1 mm; the magnetic field distribution of the permanent magnet is mathematically described by one or more magnetic dipole field models, and the model fitting error is ≤5%; the magnetization performance of the permanent magnet is within the rated operating range, and the signal-to-noise ratio of the magnetic field signal to the environmental noise is ≥20 dB.

4. The six-degree-of-freedom magnetic positioning system according to claim 1, characterized in that, The non-magnetic counterweight is made of aluminum alloy or titanium alloy.

5. The six-degree-of-freedom magnetic positioning system according to claim 1, characterized in that, The triaxial magnetic sensor includes an anisotropic magnetoresistive sensor; the anisotropic magnetoresistive sensor is connected to the signal processing unit via a 120Ω twisted-pair shielded cable to achieve a low noise performance of 0.4mGauss / √Hz at a 10Hz bandwidth, providing accurate raw magnetic field data for sub-millimeter positioning.

6. The six-degree-of-freedom magnetic positioning system according to claim 1, characterized in that, The signal processing unit includes: The data acquisition and processing module is configured to receive magnetic field measurement data acquired by the magnetic field sensor unit, sample the data using a 24-bit high-precision ADC, and preprocess the data using digital filtering and adaptive noise reduction algorithms. The motor drive control module is configured to use a stepper motor driver to implement PWM speed control, with a speed adjustment range of 100-3000 rpm. It forms a closed-loop control through optical encoder feedback to ensure speed stability of ±0.1%. The pose calculation module is configured to use an improved magnetic field position calculation model based on a pre-established model. Optimize the algorithm to perform real-time position calculation. Parallel computation is implemented to ensure real-time output of 30-60fps; The output interface module is configured to output six-DOF pose information via Gigabit Ethernet or serial interface, with latency controlled within 15±5ms, and supports OpenGL 3D visualization display and CSV format data storage.

Images