Magnetic field generator and spatial pose measurement system

Abstract

The invention discloses a magnetic field generator and a spatial pose measurement system, and relates to the technical field of electromagnetic localization, the generator can generate a time-varying magnetic field with directivity and asymmetry under a single magnetic source architecture so as to enhance the dimension of a magnetic field signal, and meanwhile, environment low-frequency interference is effectively inhibited through a high-frequency rotation modulation technology, so that the spatial pose measurement accuracy is improved. Obtaining a precise unique solution of the spatial range; and excellent static and dynamic balance can be maintained on a machine, small vibration and low noise in the rotating process are ensured, so that the magnet is allowed to operate at a higher rotating speed, a higher pose update rate and mechanical reliability are obtained, and finally, high-precision and low-delay six-degree-of-freedom position calculation is realized.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of electromagnetic positioning, in particular to a magnetic field generator based on a wave rotating magnetic source and a spatial pose measurement system. BACKGROUND

[0002] The spatial electromagnetic positioning technology is a non-contact measurement method for realizing the position and direction sensing of an object by analyzing the magnetic field distribution characteristics of a target area. This technology has irreplaceable advantages in closed environment navigation, human body cavity intervention diagnosis and treatment, etc. due to its characteristics of not being limited by visual obstruction, not needing direct line of sight, and strong penetration for non-magnetic conductive medium. Compared with optical and acoustic positioning schemes, the electromagnetic positioning system performs outstandingly in terms of adaptability in complex environments, avoiding the visual obstruction problem of optical systems and overcoming the dependence on air medium of acoustic systems. By constructing a nonlinear mapping model of magnetic field strength and spatial position and combining modern signal processing and machine learning algorithms, the current technology can achieve sub-millimeter level three-dimensional positioning accuracy, providing key technical support for high-end equipment manufacturing and precision medicine. However, the existing technology still has the following key problems: (1) The lack of magnetic field signal dimension leads to position solution ambiguity and performance degradation. Traditional rotating permanent magnet positioning systems mostly use simple concentric rotation or one-dimensional reciprocating motion, and the time-varying magnetic field generated has high symmetry and insufficient features in spatial distribution. This leads to weak magnetic field changes at certain positions and attitudes (such as the region collinear with the magnetic body rotation axis), and the system cannot uniquely and stably solve all six degrees of freedom (6DOF) of the sensor.

[0003] (2) The system has poor robustness to environmental noise and precision degradation in uncalibrated complex environments. Since the magnetic field generated by the traditional scheme has less change in time and space, when the useful signal and the interference signal overlap in a new environment, the noise cannot be effectively estimated and eliminated. The output signal-to-noise ratio decreases, resulting in positioning system precision degradation in uncalibrated complex environments. SUMMARY

[0004] In view of the above problems, the present application provides a magnetic field generator and a spatial pose measurement system for overcoming the above problems or at least partially solving the above problems. The problem of high-precision and high-robustness six-degree-of-freedom positioning in complex environments is solved, and typical applications include but are not limited to industrial automation inspection, minimally invasive surgery navigation, virtual reality interaction, etc.

[0005] The present application provides the following solutions: A magnetic field generator, comprising: a bearing frame, the bearing frame having a circular ring structure, the circular ring structure being provided with a circle of surface guide rails on the inner side, the surface guide rails having an up-and-down wavy curve structure; A rotating assembly comprising a first secondary shaft, a second secondary shaft, a permanent magnet, a non-magnetic counterweight, and a connecting assembly, the connecting assembly comprising a main frame, a first connecting shaft and a second connecting shaft connected to the main frame through a rotating shaft; the first secondary shaft passing through both sides of the permanent magnet and being fixedly connected to the permanent magnet, the second secondary shaft passing through both sides of the non-magnetic counterweight and being fixedly connected to the non-magnetic counterweight, the first end of each of the first secondary shaft and the second secondary shaft being fixedly connected to a first guide pulley and a second guide pulley arranged in the surface guide rail respectively; the second end of each of the first secondary shaft and the second secondary shaft being hingedly connected to the first connecting shaft and the second connecting shaft respectively; A main shaft, one end of the main shaft being fixedly connected to the main frame, the other end of the main shaft being connected to an output shaft of a driving mechanism; Wherein, the third rotating line of the main shaft intersects with the first rotating line of the first secondary shaft and the second rotating line of the second secondary shaft at a point; the driving mechanism is used to drive the main shaft to rotate, the main shaft is used to drive the rotating assembly to rotate, so that the permanent magnet and the non-magnetic counterweight revolve around the main shaft, in the process of revolution, the first guide pulley and the second guide pulley move along the surface guide rail, so that the permanent magnet and the non-magnetic counterweight move up and down, and under the cooperation of the first guide pulley and the second guide pulley with the surface guide rail, the permanent magnet and the non-magnetic counterweight respectively revolve around the first rotating line and the second rotating line.

[0006] Preferably: the center of mass of the permanent magnet and the center of mass of the non-magnetic counterweight are highly coaxial with the first rotating line and the second rotating line respectively, and the coaxiality deviation is not greater than 0.02 mm.

[0007] Preferably: the geometric characteristics of the permanent magnet and the non-magnetic counterweight each have a rotationally symmetric structure with the same structure and shape, and the maximum offset of the center of mass is not greater than 0.1 mm.

[0008] Preferably: the magnetization intensity of the permanent magnet is not less than 1 Tesla; the magnetization direction of the permanent magnet is not parallel to the first rotating line, and the included angle is not less than 30°.

[0009] Preferably: the first guide pulley and the second guide pulley are both gears, the surface guide rail is provided with a gear rack arranged along the bottom surface, and the gears are connected to the gear rack.

[0010] Preferably: the first guide pulley and the second guide pulley are both rotating wheels made of high-friction coefficient materials.

[0011] Preferably, the driving mechanism comprises a direct-current brushless servo motor, an output shaft of the direct-current brushless servo motor is coaxially connected with the main shaft through H7 / k6 interference fit to ensure coaxiality, and an optical encoder for collecting angle information of the main shaft is arranged in the direct-current brushless servo motor.

[0012] A spatial pose measurement system comprises an FPGA control unit, a magnetic sensor, the magnetic field generator and a host computer. The host computer is configured to send a control instruction to the FPGA control unit through an RJ45 network port, and after successful connection, the FPGA control unit is configured to control the magnetic field generator to generate a changing magnetic field through an RS485 bus; the magnetic sensor is connected with the FPGA control unit through a bus and is configured to detect and synchronously collect three-dimensional magnetic field data in real time. The FPGA control unit is configured to return data to the host computer based on a UDP protocol through the RJ45 network port after real-time calculation.

[0013] Preferably, the FPGA control unit comprises the following sub-modules: A data acquisition and preprocessing module is configured to receive original magnetic field signals from the magnetic sensor, digitize the original magnetic field signals through a 24-bit high-precision analog-to-digital converter, and apply digital band-pass filtering and adaptive noise reduction processing to the original magnetic field signals; A motor driving control module is configured to adopt a servo motor driver based on PWM speed regulation, and integrate an optical encoder to realize speed closed-loop feedback control; A pose calculation module is configured to use an improved Levenberg-Marquardt optimization algorithm to calculate a pose in real time based on a trained magnetic field-position mapping model, and run the algorithm in parallel on a Cyclone V FPGA hardware platform to realize real-time pose output of 30-60 frames per second; An output interface module is configured to output six-degree-of-freedom pose information through a gigabit Ethernet or high-speed serial port, and use OpenGL three-dimensional real-time visualization and CSV format data for recording.

[0014] Preferably, the magnetic field sensor comprises a high-performance sensor based on anisotropic magnetoresistance technology or three orthogonally arranged single-axis sensors; the magnetic sensor is connected with the FPGA control unit through a twisted shielded wire with a characteristic impedance of 120Ω.

[0015] According to the embodiments of the present application, the following technical effects are achieved: The application provides a magnetic field generator and a spatial pose measurement system.

[0016] In addition, in the preferred embodiment, the driving part of the system adopts a plurality of brushless servo motors for cooperative control, and cooperates with a high-precision angular contact bearing array to drive the permanent magnet and the counterweight to rotate around the main shaft and to spin around the auxiliary shaft. In the process of rotation of the main shaft, the rotating shaft with longitudinal axis activity is used to make the permanent magnet produce axial harmonic oscillation, so as to form a composite spiral motion track. This multi-degree-of-freedom motion coupling mechanism can generate a spatiotemporal orthogonal modulation magnetic field, the phase and amplitude of which contain rich spatial encoding information, effectively improving the spatial positioning accuracy. The signal acquisition unit adopts a three-axis magnetoresistance sensor magnetic field vector data, the data processing center synchronously acquires data through a high-speed serial bus, and relies on a pre-established positioning solution model and an optimized Levenberg-Marquardt algorithm to reconstruct the three-dimensional spatial coordinates and Euler angle attitude of the measured target in real time. The application significantly improves the anti-interference and positioning resolution of the system through the multi-dimensional motion magnetic field modulation mechanism, and can be widely applied to intelligent equipment navigation, surgical robot positioning and other high-precision measurement scenes. Of course, implementing any product of the application does not necessarily require all the advantages described above. BRIEF DESCRIPTION OF DRAWINGS

[0017] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments. Obviously, the drawings described below are only some embodiments of the present application, and other drawings can also be obtained by those skilled in the art without creative labor on the basis of these drawings.

[0018] Figure 1 is a structural schematic diagram of a magnetic field generator provided by the embodiments of the present application; Figure 2 is a connection block diagram of a spatial pose measurement system provided by the embodiments of the present application; Figure 3 is a spatial pose solution mathematical model provided by the embodiments of the present application.

[0019] In the figure: FPGA control unit 1, magnetic sensor 2, magnetic field generator 3, bearing frame 31, surface guide rail 311, first auxiliary shaft 32, first rotating line 321, second auxiliary shaft 33, second rotating line 331, permanent magnet 34, non-magnetic counterweight 35, connecting assembly 36, first guide wheel 37, main shaft 38, third rotating line 381, driving mechanism 39, upper computer 4, intersection O. DETAILED DESCRIPTION

[0020] The technical solutions in the embodiments of the present application will be apparently and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments in the present application belong to the scope of protection of the present application.

[0021] Referring to Figure 1 A magnetic field generator provided by the embodiments of the present application is shown in the figure, which can include: Figure 1 The sensor can include: The bearing frame 31 has a circular ring structure, and a circle of surface guide rails 311 is arranged on the inner side of the circular ring structure, and the surface guide rails 311 have a curve structure that undulates up and down. The rotating assembly includes a first auxiliary shaft 32, a second auxiliary shaft 33, a permanent magnet 34, a non-magnetic counterweight 35, and a connecting assembly 36. The connecting assembly 36 includes a main frame, a first connecting shaft, and a second connecting shaft connected to the main frame through a rotating shaft. The first auxiliary shaft 32 passes through the two side surfaces of the permanent magnet 34 and is fixedly connected to the permanent magnet 34. The second auxiliary shaft 33 passes through the two side surfaces of the non-magnetic counterweight 35 and is fixedly connected to the non-magnetic counterweight 35. The first ends of the first auxiliary shaft 32 and the second auxiliary shaft 33 are respectively fixedly connected to a first guide wheel 37 and a second guide wheel (not shown in the figure) arranged in the surface guide rail 311. The second ends of the first auxiliary shaft 32 and the second auxiliary shaft 33 are respectively hingedly connected to the first connecting shaft and the second connecting shaft. The main shaft 38 has one end fixedly connected to the main frame and the other end connected to the output shaft of the driving mechanism 39. The third rotation line 381 of the main shaft 38 intersects with the first rotation line 321 of the first secondary shaft 32 and the second rotation line 331 of the second secondary shaft 33 at a point (intersection O); the driving mechanism 39 is used to drive the rotation of the main shaft 38, and the main shaft 38 is used to drive the rotation of the rotating assembly, so that the permanent magnet 34 and the non-magnetic counterweight 35 revolve around the main shaft 38, so that the first guide wheel 37 and the second guide wheel move along the surface guide rail 311 in the revolution process, so that the permanent magnet 34 and the non-magnetic counterweight 35 make up and down floating movement, and the permanent magnet 34 and the non-magnetic counterweight 35 revolve around the first rotation line 321 of the first secondary shaft 32 and the second rotation line 331 of the second secondary shaft 33, respectively.

[0022] The magnetic field generator 3 provided by the embodiment of the application generates a three-dimensional encoding magnetic field with significant space-time characteristics through the unique T-shaped double-axis rotation structure and Z-axis reciprocating motion. The design not only creates an asymmetric magnetic field distribution, which completely solves the ambiguity problem, but also realizes excellent suppression of environmental interference through axial reciprocating rotation modulation technology. The precise mechanical and counterweight design ensures extremely low vibration at high speed, laying a solid foundation for high-precision and high-refresh-rate pose solution, and providing an innovative solution for high-reliability magnetic positioning in complex industrial and medical environments.

[0023] In a specific implementation, in order to further improve the accuracy of the generated magnetic field, the embodiment of the application can also provide that the center of mass of the permanent magnet 34 and the first rotation line 321 of the first secondary shaft 32 and the center of mass of the non-magnetic counterweight 35 and the second rotation line 331 of the second secondary shaft 33 are highly coaxial, and the coaxiality deviation is not greater than 0.02 millimeters.

[0024] The geometric characteristics of the permanent magnet 34 and the non-magnetic counterweight 35 each have a rotationally symmetric structure with the same structure and shape, and the maximum offset of the center of mass position is not greater than 0.1 millimeters.

[0025] The magnetization intensity of the permanent magnet 34 is not less than 1 Tesla; the magnetization direction of the permanent magnet 34 is not parallel to the first rotation line 321 of the first secondary shaft 32, and the included angle is not less than 30°.

[0026] It can be understood that the first guide wheel 37 and the second guide wheel provided by the embodiment of the application can also drive the respective permanent magnets 34 and non-magnetic counterweights 35 to rotate during the running along the surface guide rail 311. The driving rotation can be achieved in various ways. For example, in one implementation, the first guide wheel 37 and the second guide wheel are both gears, and the surface guide rail 311 is provided with a gear rack along the bottom surface, and the gears are connected with the gear rack. Through the meshing of the gears and the gear rack, it can be ensured that the first guide wheel 37 and the second guide wheel can respectively drive the permanent magnets 34 and the non-magnetic counterweights 35 to rotate during the revolution.

[0027] In another implementation, the first guide wheel 37 and the second guide wheel are both rotating wheels made of high-friction coefficient materials. The first guide wheel 37 and the second guide wheel can be made of high-friction coefficient materials such as rubber, so that the permanent magnets 34 and the non-magnetic counterweights 35 are driven to rotate under the action of friction when the first guide wheel 37 and the second guide wheel move along the surface guide rail 311.

[0028] The power source of the generator provided by the embodiment of the application is mainly the driving mechanism 39. In a specific implementation, the driving mechanism 39 can include a DC brushless servo motor. The output shaft of the DC brushless servo motor is coaxially connected with the main shaft 38 through H7 / k6 interference fit to ensure coaxiality, and an optical encoder for collecting angle information of the main shaft 38 is built in.

[0029] The magnetic field generator 3 provided by the application will be described in detail below. A bearing frame 31 with a circular ring structure is adopted, and the surface guide rail 311 with a curve structure with up and down fluctuations is arranged in the bearing frame 31. The combination of the permanent magnets 34 and the corresponding non-magnetic counterweights 35 is dynamically balanced and optimized, and the overall center of mass is strictly controlled at the theoretical design position. The high-precision driving mechanism 39 adopts a closed-loop controlled brushless servo motor and is equipped with a P4-grade precision angular contact bearing set, which is rigidly connected with the bearing frame 31. The driving mechanism 39 drives the bearing frame 31 to perform revolution around the main shaft 38, while the permanent magnets 34 and the non-magnetic counterweights 35 respectively revolve around the first rotation line 321 of the first auxiliary shaft 32 and the second rotation line 331 of the second auxiliary shaft 33. The main shaft 38 and the auxiliary shafts intersect at a point in space. The included angle between the main shaft 38 and each auxiliary shaft is adjustable and ranges between 90° and 120°.

[0030] Wherein, the permanent magnet 34 adopts high magnetic energy product material such as neodymium iron boron, and is processed into a rotating symmetrical ring configuration; the permanent magnet 34 (or permanent magnet assembly) must have a determined centroid position, and the first guide wheel 37 connected therewith is fixed in a designated installation position in the surface guide rail 311 through a mechanical clamping groove, and the centroid must be coaxial with the rotation axis (i.e. the first secondary shaft 32 rotation line keeps high coaxiality, and the coaxiality deviation is not more than 0.02 mm; the first guide wheel 37 revolves around the main shaft 38 in transmission, and the revolution radius is ; the surface guide rail 311 plane in which the permanent magnet 34 is embedded is periodically changed in height, so that the actual space angle of the main shaft 38 and the first secondary shaft 32 is strictly controlled within the range of 90°-120°, and the deviation is not more than 0.5°; the fixed revolution is carried out at a constant angular velocity , so as to ensure the periodicity and predictability of the magnetic field distribution in the rotation process of the permanent magnet 34, and to be beneficial to the mathematical modeling of the pose solution.

[0031] The permanent magnet 34 should meet the following technical requirements: 1. Geometric characteristics: having a rotating symmetrical structure (such as a ring), the centroid position can be accurately measured by geometric or physical means, and the maximum deviation is not more than 0.1 mm; 2. Magnetic field modeling characteristics: the spatial magnetic field distribution can be mathematically characterized by using a single or multiple magnetic dipole model, and the model fitting error is not more than 5%; 3. Magnetic performance requirements: the magnetization intensity is not less than 1 T, so as to ensure that the signal-to-noise ratio of the magnetic field signal relative to the environmental noise is not less than 20 dB in the rated action range (such as a space with a radius of 50 cm); the magnetization direction is not parallel to the second rotation axis, and the included angle is not less than 30°, so as to avoid the periodic overlap of the magnetic field in rotation; 4. Calibration requirements: the actual magnetization intensity and magnetization direction of the permanent magnet 34 must be calibrated by using a standard magnetic measuring device (such as a Helmholtz coil magnetometer or a magnetic dipole calibration device), and the related calibration parameters (including the specific value of the magnetization intensity and the magnetization direction angle) must be stored in the FPGA control unit 1 as system parameters, so as to improve the calculation accuracy of the pose algorithm.

[0032] The non-magnetic counterweight 35 is made of non-magnetic or weak magnetic material, such as titanium alloy or non-magnetic stainless steel material, and has the same mass as a single permanent magnet 34, and is symmetrically arranged and connected to the second guide wheel (symmetric to the first guide wheel 37), and the rest of the mechanical structure is the same as that of the permanent magnet 34 installation arm, which is used to realize mass balance and suppress vibration in high-speed operation.

[0033] The supporting frame 31 achieves motion control through a high-precision drive system, specifically including: using a DC brushless servo motor to drive the connecting shaft to achieve synchronous rotation, the speed of which can be continuously adjusted within a set range to generate a periodic spatially changing magnetic field with specific spectral characteristics; the connecting shaft has vertical joint mobility, allowing it to cooperate with the surface guide rail 311 to achieve the z-axis movement of the permanent magnet 34; the motor output shaft and the third rotation line 381 of the main shaft 38 are fitted with an H7 / k6 grade interference fit to ensure coaxiality (error ≤ 0.02 mm), and a built-in 23-bit absolute photoelectric encoder is used to collect the angle information of the main shaft 38 in real time (resolution up to 0.15 arcseconds), and this data is uploaded to the FPGA control unit 1 for closed-loop correction; finally, mechanical fastening is achieved through M4 grade anti-loosening screws (pre-tightening torque controlled at 2.5 N•m ± 5%) to ensure the stability of the overall structure under high-speed operation.

[0034] In summary, the magnetic field generator provided in this application can generate a time-varying magnetic field with directionality and asymmetry under a single magnetic source architecture to enhance the dimensionality of the magnetic field signal. At the same time, it can effectively suppress low-frequency interference in the environment through high-frequency rotation modulation technology to obtain a precise and unique solution in the spatial range. It can also maintain excellent static and dynamic balance mechanically to ensure low vibration and low noise during rotation, thereby allowing the magnet to run at a higher speed, obtain a higher pose update rate and mechanical reliability, and ultimately achieve high-precision, low-latency six-degree-of-freedom position calculation.

[0035] like Figure 2 As shown, this application embodiment can also provide a spatial pose measurement system, including an FPGA control unit 1, a magnetic sensor 2, the above-mentioned magnetic field generator 3, and a host computer 4; The host computer 4 is used to send control commands to the FPGA control unit 1 via the RJ45 network port. After successful connection, the FPGA control unit 1 is used to control the magnetic field generator 3 to generate a changing magnetic field via the RS485 bus; the magnetic sensor 2... The bus communicates with the FPGA control unit 1 to detect and synchronously acquire three-dimensional magnetic field data in real time; after the FPGA control unit 1 completes the real-time calculation, it transmits the data back to the host computer 4 through the RJ45 network port based on the UDP protocol.

[0036] Furthermore, the FPGA control unit 1 includes the following sub-modules: The data acquisition and preprocessing module is configured to receive the raw magnetic field signal from the magnetic sensor 2, digitize it through a 24-bit high-precision analog-to-digital converter, and apply digital bandpass filtering and adaptive noise reduction processing. The motor drive control module is configured to use a servo motor driver based on PWM speed regulation; and integrates an optical encoder to realize closed-loop feedback control of speed. A pose solving module configured to train the completed magnetic field-position mapping model, adopt an improved Levenberg-Marquardt optimization algorithm to solve the pose in real time, and run the algorithm in parallel on a Cyclone V FPGA hardware platform to realize real-time pose output of 30-60 frames per second; An output interface module configured to output six-degree-of-freedom pose information through a gigabit Ethernet or high-speed serial port, and record using OpenGL three-dimensional real-time visualization and CSV format data.

[0037] The magnetic field sensor includes a high-performance sensor based on an anisotropic magnetoresistance technology or three orthogonally arranged single-axis sensors; the magnetic sensor 2 is connected to the FPGA control unit 1 through a twisted shielded wire with a characteristic impedance of 120Ω.

[0038] The spatial pose measurement system provided by the embodiment of the application is described in detail below.

[0039] The positioning system mainly includes the following four parts: an FPGA control unit 1, a magnetic sensor 2, a magnetic field generator 3, and an upper computer 4. The upper computer 4 issues control instructions to the FPGA control unit 1 through an RJ45 network port, and after successful connection, the FPGA control unit 1 controls the magnetic field generator 3 to drive a motor to generate a changing magnetic field through an RS485 bus. The magnetic sensor 2 communicates with the FPGA control unit 1 through a bus to detect and synchronously collect three-dimensional magnetic field data in real time; after the FPGA completes real-time solving, the data is finally returned to the upper computer 4 based on a UDP protocol through an RJ45 network port.

[0040] The magnetic field generator 3 is described above and will not be described again here.

[0041] A magnetic field sensing module includes: A plurality of high-precision three-axis magnetic sensors 2, which are matched with the FPGA control unit 1 to support multi-channel synchronous collection; each sensor has X, Y, and Z three-axis magnetic induction intensity and magnetic flux measurement capability, and the sampling frequency is not less than 10 times the fundamental frequency of the magnetic source; The FPGA control unit 1 includes: A motor drive control module for realizing accurate speed control and multi-axis phase synchronization; A pose solving algorithm that relies on a magnetic field-space position mapping database obtained through pre-calibration and adopts an adaptive particle filtering algorithm to solve six-degree-of-freedom pose information in real time; A data communication interface supporting pose data transmission based on Ethernet or USB 3.0 protocol.

[0042] The magnetic field sensor module includes: ​The triaxial sensor is used to collect vector magnetic field information, such as a high-performance sensor based on anisotropic magnetoresistance (AMR) technology, with a range of ±8 gauss, a sensitivity of 0.0625 milligauss per least significant bit, and a sampling frequency of not less than 500 Hz, which can capture the dynamic generated by the rotating permanent magnet 34 with high precision, and three orthogonally arranged single-axis sensors can also be used to achieve the same goal. The magnetic sensor 2 is connected to the FPGA control unit 1 through a twisted shielded wire with a characteristic impedance of 120Ω, and the noise density is as low as 0.4 milligauss / √Hz at a bandwidth of 10 Hz, so that the fundamental frequency 50Hz and its harmonic components can be completely collected when the magnetic source rotates at 3000 rpm, providing high signal-to-noise ratio raw magnetic field data for sub-millimeter spatial positioning.

[0043] The FPGA control unit 1 comprises the following sub-modules: a. Data acquisition and preprocessing module: receiving raw magnetic field signals from the magnetic sensor 2, digitizing through a 24-bit high-precision analog-to-digital converter (sampling rate ≥10 kHz), and applying digital band-pass filtering (center frequency consistent with the rotational base frequency of the magnetic source) and adaptive noise reduction processing to improve signal quality; b. Motor drive control module: using a servo motor driver based on PWM speed regulation, the rotational speed can be continuously adjustable between 100 and 3000 rpm, and the minimum servo amount is 1 rpm; integrated optical encoder for rotational speed closed-loop feedback control, with a rotational speed stability better than ±0.1%; c. Pose solving module: relying on the magnetic field-position mapping model (stored in the form of a three-dimensional lookup table) trained by a million-level data points obtained through pre-calibration experiments, using an improved Levenberg-Marquardt optimization algorithm to solve the pose in real time, and running the algorithm in parallel on a Cyclone V FPGA hardware platform, finally achieving real-time pose output of 30-60 frames per second; d. Output interface module: outputting six-degree-of-freedom pose information through gigabit Ethernet or high-speed serial port, with a position accuracy of ±0.15 mm and an attitude accuracy of ±0.3°; the system transmission delay is controlled within 15±5 ms, and supports OpenGL three-dimensional real-time visualization and CSV format data recording.

[0044] The unit also has a static self-checking function, which can automatically calibrate the sensor zero offset and sensitivity coefficient, and integrates a multi-target dynamic tracking algorithm based on Kalman filtering, which can simultaneously process real-time pose information of not more than 6 targets.

[0045] Figure 3 is a schematic diagram of a spatial pose solving mathematical model, and the magnetic field modeling method comprises: A right-handed rectangular coordinate system with the center of revolution (4) as the origin is established as the global coordinate system; Definitions The magnetic moment vector of the permanent magnet in the global coordinate system at time t is represented as:

[0046] wherein, represents the initial magnetic moment vector, represents the rotation matrix around the Z axis, represents the rotation matrix around the first secondary axis, represents the angular velocity of the permanent magnet around the primary axis, represents the angular velocity of the permanent magnet around the secondary axis, is the magnetization, is the volume; In the global coordinate system, the position and attitude of the magnetic sensor at a certain time are recorded as . The improved magnetic dipole model is used to calculate the spatial magnetic field distribution:

[0047] wherein, represents the vacuum permeability, is the displacement vector of the magnetic sensor relative to the permanent magnet, the orbital radius is known and recorded as , and the initial position of the permanent magnet rotation is , is the function of the height of the orbital surface with respect to time, and its complete column vector is represented as:

[0048] The sensor signal conversion model is:

[0049] wherein, is the sensor attitude rotation matrix, , is the sensor measurement value, is the geomagnetic field vector.

[0050] In the preprocessing process, a 5th order Butterworth band-pass filter is used for signal filtering, and the cutoff frequency is , and the geomagnetic field compensation is: ; According to the above formula, the objective function is established:

[0051] The optimization solving process adopts an improved algorithm to realize the pose parameters​​​ iterative computation, specifically including the following key steps: First, based on the current parameter estimate value Calculate the objective function and its 6x6 Jacobian matrix (numerically solved by central difference method, step size set to 1% of parameter value); Then build a damped normal equation , where the adaptive damping factor is initially set to 0.1 and dynamically adjusted according to the iteration situation (if the residual decreases , otherwise ); then solve the linear equation system to obtain the parameter increment , and update the estimate value ; The iteration termination condition is set to the relative residual change rate being less than or reaching the maximum iteration number of 100 times, and the final optimal pose solution is output.

[0052] The improved algorithm introduces a trust region strategy (limits the step size ) and Jacobian matrix regularization (adds identity matrix to prevent singularity), which improves the convergence speed by 30% compared to the standard L-M algorithm and is less likely to fall into local extrema. In the case of an initial guess value deviation of 20%, it still guarantees a 95% convergence success rate.

[0053] In summary, the spatial pose measurement system provided by the present application uses multiple groups of brushless servo motors for collaborative control, and cooperates with a high-precision angular contact bearing array to drive the permanent magnet and counterweight to rotate around the main shaft and spin around the secondary shaft. During the rotation of the main shaft, the shaft with longitudinal axis activity is matched to make the permanent magnet produce axial harmonic oscillation, thereby forming a composite spiral motion trajectory. This multi-degree-of-freedom motion coupling mechanism can generate a spatiotemporal orthogonal modulation magnetic field, the phase and amplitude of which contain rich spatial encoding information, effectively improving the spatial positioning accuracy. The signal acquisition unit uses a three-axis magnetoresistive sensor magnetic field vector data, and the data processing center synchronously acquires data through a high-speed serial bus, relies on a pre-established positioning solution model and an optimized particle filter algorithm, and reconstructs the three-dimensional spatial coordinates and Euler angle attitude of the measured target in real time. The present application significantly improves the anti-interference performance and positioning resolution of the system through the multi-dimensional motion magnetic field modulation mechanism, and can be widely applied to intelligent equipment navigation, surgical robot positioning, and other high-precision measurement scenarios.

[0054] It should be noted that, in the specification, relational terms such as first and second, and the like, can be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Also, the terms "comprises", "comprising", or any other variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but can include other elements not expressly listed or inherent to such process, method, article, or apparatus. An element proceeded by "comprises... a" does not, without more constraints, exclude the existence of additional identical elements in the process, method, article, or apparatus that comprises the element.

[0055] From the description of the above embodiments, those skilled in the art can clearly understand that the present application can be implemented by means of software plus necessary universal hardware platforms. Based on such an understanding, the technical solutions of the present application can be embodied in the form of a software product, which can be stored in a storage medium, such as a ROM / RAM, a magnetic disk, an optical disk, and the like, and includes a number of instructions to make a computer device (which can be a personal computer, a server, or a network device, etc.) execute the methods described in various embodiments or some parts of the embodiments.

[0056] Each of the embodiments in the specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other. Each embodiment focuses on the difference from other embodiments. In particular, for the system or system embodiments, since it is basically similar to the method embodiments, it is described more simply, and the relevant parts can be referred to the part of the method embodiments. The above-described system and system embodiments are merely illustrative, and the units described as separate components can be or can not be physically separated, and the components displayed as units can be or can not be physical units, i.e., they can be located in one place or distributed on multiple network units. Part or all of the modules can be selected to achieve the purpose of the embodiment according to the actual needs. Those skilled in the art can understand and implement it without creative labor.

[0057] The above only describes the preferred embodiments of the present application and is not used to limit the protection scope of the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application is included in the protection scope of the present application.

Claims

1. A magnetic field generator, characterized by include: A load-bearing frame having a circular ring structure, with a surface guide rail arranged on the inner side of the circular ring structure, the surface guide rail having a wavy curved structure; A rotating assembly includes a first secondary shaft, a second secondary shaft, a permanent magnet, a non-magnetic counterweight, and a connecting assembly. The connecting assembly includes a main frame and a first connecting shaft and a second connecting shaft connected to the main frame via a rotating shaft. The first secondary shaft passes through both sides of the permanent magnet and is fixedly connected to the permanent magnet. The second secondary shaft passes through both sides of the non-magnetic counterweight and is fixedly connected to the non-magnetic counterweight. The first ends of the first and second secondary shafts are respectively fixedly connected to a first guide wheel and a second guide wheel disposed in the surface guide rail. The second ends of the first and second secondary shafts are respectively hinged to the first connecting shaft and the second connecting shaft. The main shaft has one end fixedly connected to the main frame and the other end connected to the output shaft of the drive mechanism. The third rotation line of the main shaft intersects at a point with the first rotation line of the first secondary shaft and the second rotation line of the second secondary shaft. The driving mechanism drives the main shaft to rotate, and the main shaft drives the rotating assembly to rotate, so that the permanent magnet and the non-magnetic counterweight revolve around the main shaft. During the revolution, the first guide wheel and the second guide wheel move along the surface guide rail, causing the permanent magnet and the non-magnetic counterweight to float up and down. Under the combined action of the first guide wheel and the second guide wheel and the surface guide rail, the permanent magnet and the non-magnetic counterweight rotate around the first rotation line and the second rotation line, respectively.

2. The magnetic field generator of claim 1, wherein, The center of mass of the permanent magnet and the first rotation line, as well as the center of mass of the non-magnetic counterweight and the second rotation line, are highly coaxial, and the coaxiality deviation is no greater than 0.02 mm.

3. The magnetic field generator of claim 1, wherein, Both the permanent magnet and the non-magnetic counterweight have the same rotationally symmetric structure and shape, and the maximum offset of the center of mass is no more than 0.1 mm.

4. The magnetic field generator of claim 1, wherein, The magnetization intensity of the permanent magnet is not less than 1 Tesla; the magnetization direction of the permanent magnet is not parallel to the first rotation line, and the included angle is not less than 30°.

5. The magnetic field generator of claim 1, wherein, Both the first guide wheel and the second guide wheel are gears, and the surface guide rail is provided with a rack arranged along its bottom surface. The gears are fitted and connected to the rack.

6. The magnetic field generator of claim 1, wherein, Both the first guide wheel and the second guide wheel are made of materials with a high coefficient of friction.

7. The magnetic field generator of claim 1, wherein, The drive mechanism includes a brushless DC servo motor. The output shaft of the brushless DC servo motor and the main shaft are fitted with an H7 / k6 level interference fit to ensure coaxiality. It also has a built-in photoelectric encoder for acquiring the angle information of the main shaft.

8. A spatial pose measurement system, characterized by Includes an FPGA control unit, a magnetic sensor, a magnetic field generator as described in any one of claims 1 to 7, and a host computer; The upper computer is used for issuing control instructions to the FPGA control unit through an RJ45 network port, and after successful connection, the FPGA control unit is used for controlling the magnetic field generator to act to generate a changing magnetic field through an RS485 bus A bus communicates with the FPGA control unit, and is used for real-time detection and synchronous acquisition of three-dimensional magnetic field data; after the FPGA control unit completes real-time calculation, the FPGA control unit transmits data back to the upper computer based on a UDP protocol through an RJ45 network port.

9. The spatial pose measurement system of claim 8, wherein, The FPGA control unit includes the following sub-modules: The data acquisition and preprocessing module is configured to receive original magnetic field signals from the magnetic sensor, digitize the signals through a 24-bit high-precision analog-to-digital converter, and apply digital band-pass filtering and adaptive noise reduction processing. The motor drive control module is configured to adopt a PWM speed-based servo motor driver, and integrate an optical encoder to realize closed-loop feedback control of the rotation speed. The pose solving module is configured to use the trained magnetic field-position mapping model to solve the pose in real time by using an improved Levenberg-Marquardt optimization algorithm, and run the algorithm in parallel on a Cyclone V FPGA hardware platform to realize real-time pose output of 30-60 frames per second. The output interface module is configured to output six-degree-of-freedom pose information through a gigabit Ethernet or high-speed serial port, and use OpenGL three-dimensional real-time visualization and CSV format data for recording.

10. The spatial pose measurement system of claim 9, wherein, The magnetic field sensor includes a high-performance sensor based on an anisotropic magnetoresistance technology or three orthogonally arranged uniaxial sensors; the magnetic sensor and the FPGA control unit are connected through a twisted shielded wire with a characteristic impedance of 120 Ω.

Images