Magnet pose estimation method and system based on magnetic dipole constraint

Abstract

This invention discloses a method and system for magnet pose estimation based on magnetic dipole constraints. The method includes estimating the initial orientation and position of the magnet based on the triaxial magnetic field vectors measured by each magnetic sensor in a magnetic sensor array to determine the initial estimated orientation unit vector and initial estimated position of the magnet; iteratively solving the initial pose state parameters of the magnet using a nonlinear optimization algorithm based on a magnetic dipole model to obtain the final estimated orientation unit vector; obtaining the position residual and orientation residual using a pre-trained residual neural network model; and compensating the initial estimated position and the final estimated orientation unit vector based on the position residual and orientation residual to obtain the final magnet pose estimation result composed of the magnet orientation. This invention aims to overcome the influence of initial value deviation, sensor error, and environmental interference factors, improve robustness and generalization ability in complex environments, and meet the high-precision pose detection requirements of magnetic positioning.

Description

Technical Field

[0001] This invention relates to the field of magnetic positioning technology, specifically to a method and system for estimating the pose of a magnet based on magnetic dipole constraints. Background Technology

[0002] In magnetic positioning applications, multiple magnetic sensors are typically used to collect magnetic field data generated by a target magnet in space, and the spatial position and orientation of the target magnet are deduced from the magnetic field distribution. Since a magnet can be approximated as a magnetic dipole when it is far from the sensors, a common approach is to use the magnet's position and magnetic moment parameters as unknowns, construct a nonlinear least-squares problem based on the triaxial magnetic field data measured by multiple sensors, and then use an iterative method to solve for the magnet's position and orientation. However, in the magnetic dipole magnetic field model, there is a strong nonlinear relationship between the magnetic field and the position and magnetic moment. Especially under multi-sensor conditions, the objective function is usually quite complex. If the initial value is not chosen appropriately, iterative solutions can easily converge to local extrema or even diverge, easily getting trapped in local optima. Moreover, in practical applications, nonlinear optimization algorithms such as LM are highly sensitive to initial values. Existing technologies often use fixed empirical values, manually specified initial values, or random initialization, resulting in insufficient algorithm stability and difficulty in adapting to large-scale automated processing scenarios. However, in real-world systems, magnets are often not ideal dipoles, and sensors may suffer from installation deviations, sensitivity differences, zero-bias errors, proportional coefficient errors, and environmental magnetic interference. Even if a relatively good coarse solution is obtained through physical models and nonlinear optimization, system residuals may still exist, which are difficult to eliminate further. If end-to-end neural networks are used to directly regress position and orientation from magnetic field data, although complex nonlinear relationships can be learned from the data, problems such as strong dependence on training data, poor interpretability, and sensitivity to data distribution drift are often encountered, and it is difficult to fully utilize the physical priors of the magnetic dipole model. Summary of the Invention

[0003] The technical problem to be solved by the present invention is to provide a magnetic pose estimation method and system based on magnetic dipole constraint, which addresses the above-mentioned problems in the prior art. The present invention aims to overcome the influence of initial value deviation, sensor error and environmental interference factors, improve the robustness and generalization ability in complex environments, and meet the high-precision pose detection requirements of magnetic positioning.

[0004] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is as follows: A method for estimating the pose of a magnet based on magnetic dipole constraints includes the following steps: S101, based on the triaxial magnetic field vectors measured by each magnetic sensor in the magnetic sensor array, perform initial orientation estimation of the magnet to determine the initial estimated orientation unit vector of the magnet, and perform estimated position estimation of the magnet to determine the initial estimated position of the magnet; S102, Based on the initial estimated direction unit vector and the initial estimated position of the magnet, the initial pose state parameters of the magnet are constructed. The initial pose state parameters are iteratively solved using a nonlinear optimization algorithm based on the magnetic dipole model to obtain the final estimated direction unit vector. S103, the final estimated direction unit vector, the initial estimated position, and the three-axis magnetic field vector measured by each magnetic sensor in the magnetic sensor array are input into the pre-trained residual neural network model to obtain the position residual and the direction residual. The residual neural network model is pre-trained and learns to establish the mapping relationship between the final estimated direction unit vector, the initial estimated position of the magnet, the three-axis magnetic field vector, and the output position residual and direction residual. S104. The initial estimated position is compensated based on the position residual to obtain the final magnet position, and the final estimated direction unit vector is compensated based on the direction residual to obtain the final magnet direction, thus obtaining the magnet pose estimation result composed of the final magnet position and the final magnet direction.

[0005] Optionally, in step S101, the initial orientation estimation of the magnet to determine the initial estimated orientation unit vector of the magnet includes: S201, Obtain the three-axis magnetic field vector detected by the central magnetic sensor in the magnetic sensor array; S202, determine whether the norm of the three-axis magnetic field vector detected by the central magnetic sensor is greater than the set threshold. If it is true, proceed to step S203; otherwise, proceed to step S204. S203, determine the initial estimated direction unit vector of the magnet based on the triaxial magnetic field vector detected by the central magnetic sensor: ; in, Let be the initial estimated direction unit vector of the magnet. The three-axis magnetic field vector detected by the central magnetic sensor. for Norm; jump to step S204; S204, calculate the weighted average magnetic field vector based on the triaxial magnetic field vectors measured by each magnetic sensor in the magnetic sensor array: ; ; ; in, This is the weighted average magnetic field vector. Let be the weight of the i-th magnetic sensor. Let be the triaxial magnetic field vector measured by the i-th magnetic sensor. Let be the magnetic field strength of the i-th magnetic sensor. For the weighting index parameter, for The norm; the initial estimated direction unit vector of the magnet is determined based on the weighted average magnetic field vector: ; in, for The norm of .

[0006] Optionally, in step S101, the process of estimating the magnet's estimated position to determine the initial estimated position of the magnet includes: S301, Determine the initial estimated position of the magnet. Direction coordinates and Direction coordinates: ; ; ; ; in, Indicates the initial estimated position of the magnet Direction coordinates; Indicates the initial estimated position of the magnet Direction coordinates Let be the weight of the i-th magnetic sensor. Let be the magnetic field strength of the i-th magnetic sensor. For the weighting index parameter, for norm, Let be the three-axis magnetic field vector measured by the i-th magnetic sensor; and These are the i-th magnetic sensors. Direction coordinates and Direction coordinates; S302, based on the magnetic sensor closest to the magnet in the magnetic sensor array, and the initial estimated position of the magnet. Direction coordinates and The offset of the orientation coordinates in the xy plane: ; in, This is the offset. and For the magnetic sensor array, the magnetic sensor closest to the magnet Direction coordinates and Direction coordinates; determining the initial estimated position of the magnet based on the offset. Direction coordinates: ; ; in, For the initial estimated position of the magnet Direction coordinates To obtain the maximum value, for The scaling factor of the orientation coordinates. for The lower limit of the direction coordinate. For reference height, For reference magnetic field strength, This represents the triaxial magnetic field vector measured by the magnetic sensor closest to the magnet in the magnetic sensor array; if the initial estimated position of the magnet is... Direction coordinates exceed range Then crop it to the range Inside, among which for The upper limit of the directional coordinates ultimately yields the initial estimated position of the magnet: ; in, This is the initial estimated position of the magnet. Indicates the initial estimated position of the magnet Direction coordinates; Indicates the initial estimated position of the magnet Direction coordinates For the initial estimated position of the magnet Direction coordinates.

[0007] Optionally, step S102 includes: S401, Constructing the initial pose parameters of the magnet: ; ; in, These are the initial pose parameters of the magnet. This is the initial estimated position of the magnet. Let be the magnetic dipole moment vector of the magnet. for norm, This is the initial estimated direction unit vector of the magnet; S402, the initial estimated position in the initial pose state parameters of the magnet. As the position of the magnet in the magnetic dipole model Magnetic dipole moment vector As the magnetic dipole moment vector of the magnet in the magnetic dipole model, the initial pose state parameters of the magnet are thus substituted into the magnetic dipole model: ; ; in, Let be the triaxial magnetic field vector calculated by the i-th magnetic sensor using the magnetic dipole model. Let be the magnetic dipole moment vector of the magnet. Let be the relative position of the magnet to the i-th magnetic sensor. Let i be the position of the i-th magnetic sensor. The position of the magnet. for The norm of the magnetic field is obtained by minimizing the following objective function to obtain the magnetic dipole moment vector of the magnet when the objective function converges: ; ; in, Let be the triaxial magnetic field vector calculated by the i-th magnetic sensor using the magnetic dipole model. Let be the magnetic dipole moment vector of the magnet. Let be the relative position of the magnet to the i-th magnetic sensor. Let i be the position of the i-th magnetic sensor. The position of the magnet. for norm, This is the set of triaxial magnetic field vectors calculated from all magnetic sensors using the magnetic dipole model. ~ The three-axis magnetic field vectors are calculated for the first to Nth magnetic sensors using the magnetic dipole model. It is the set of three-axis magnetic field vectors measured by each magnetic sensor in the magnetic sensor array; S403, calculate the final estimated direction unit vector based on the magnetic dipole moment vector of the magnet when the objective function converges: ; in, To finally estimate the direction unit vector, Let be the magnetic dipole moment vector of the magnet when the objective function converges. for The norm of .

[0008] Optionally, after step S102 and before step S103, the final estimated direction unit vector is subjected to hemispherical unification processing: ; in, This is the final estimated direction unit vector after hemispherical unification processing. To finally estimate the direction unit vector, This is the preset reference direction.

[0009] Optionally, in step S104, the functional expression for compensating the initial estimated position based on the position residual to obtain the final magnet position is: ; in, For the final magnet position, This is the initial estimated position of the magnet. The position residual; the functional expression for compensating the final estimated direction unit vector based on the direction residual to obtain the final magnet direction is: ; in, For the final magnet orientation, This is the final estimated direction unit vector after hemispherical unification processing. For directional residuals, for and The norm of the sum of the two.

[0010] Optionally, during the training phase of the residual neural network model, the label calculation function expressions used for the position residual and orientation residual are as follows: ; ; in, For location residual labels, This represents the actual position of the magnet. This is the initial estimated position of the magnet. For directional residual labels, The true orientation of the magnet. This is the final estimated direction unit vector.

[0011] Optionally, during the training phase, the residual neural network model includes applying a norm pruning operation to the labels used for the location residuals: ; in, To label the positional residuals after applying the norm clipping operation. For the original position residual labels, for norm, This is the preset maximum allowable norm threshold.

[0012] Optionally, the loss function used by the residual neural network model during the training phase is: ; ; ; in, For loss function, and These are the weighting coefficients. For position loss, For directional loss, For location residual labels, For directional residual labels, The position residuals predicted by the residual neural network model. This refers to the directional residual predicted by the residual neural network model.

[0013] Furthermore, the present invention also provides a magnetic pose estimation system based on magnetic dipole constraints, comprising a microprocessor and a memory interconnected thereto, wherein the microprocessor is programmed or configured to execute the magnetic pose estimation method based on magnetic dipole constraints.

[0014] Compared with existing technologies, this invention mainly achieves the following beneficial effects: To address the problems of strong nonlinearity, sensitivity to initial conditions, susceptibility to noise and systematic errors, and unstable orientation estimation in traditional magnetic dipole models during magnet positioning, this invention achieves magnet pose estimation by fusing a physical model with a residual neural network. It uses magnetic field data collected by a magnetic sensor and a coarse physical pose solution as input, and the magnet position and orientation residuals as learning targets. By constructing a residual compensation network based on the physical calculation results, the pose estimation accuracy is effectively improved. Through error analysis of simulation experimental data and actual test data, this invention's method exhibits lower position and orientation errors compared to methods using only LM physical calculations, significantly improving the stability and reliability of the estimation results. This invention can achieve high-precision nonlinear error compensation while ensuring physical interpretability, overcoming the problems of strong initial condition dependence, susceptibility to local optima, and difficulty in eliminating systematic errors in traditional methods. It improves the accuracy of magnet positioning and attitude estimation in complex environments, possessing advantages such as high accuracy, strong robustness, good real-time performance, and strong engineering feasibility. Attached Figure Description

[0015] Figure 1 This is a schematic diagram of the basic process of the method in an embodiment of the present invention.

[0016] Figure 2 This is a schematic diagram of the network structure of the residual neural network model in an embodiment of the present invention.

[0017] Figure 3 This is a convergence curve of the loss function for the simulation dataset in this embodiment of the invention.

[0018] Figure 4 This is a convergence curve of the loss function for the actual dataset in this embodiment of the invention.

[0019] Figure 5The following are test results of the three-dimensional position and pose of a magnet with a height of 78mm in this embodiment of the invention: (a) is a comparison of the three-dimensional position diagrams of the GT and LM methods based on the measured data; (b) is a comparison of the three-dimensional position diagrams of the GT and LM+NN methods based on the measured data; (c) is a three-dimensional position diagram of the GT and LM methods based on the simulated data; and (d) is a three-dimensional position diagram of the GT and LM+NN methods based on the simulated data.

[0020] Figure 6 The magnet pose of a 108mm high magnet in three dimensions was tested in this embodiment of the invention. (a) is a comparison of the three-dimensional position diagrams of the GT and LM methods based on the measured data, (b) is a comparison of the three-dimensional position diagrams of the GT and LM+NN methods based on the measured data, (c) is a three-dimensional position diagram of the GT and LM methods based on the simulated data, and (d) is a three-dimensional position diagram of the GT and LM+NN methods based on the simulated data.

[0021] Figure 7 The magnet pose of a magnet with a height of 138mm in the embodiment of the present invention was tested in three dimensions. Among them, (a) is a comparison of the three-dimensional position diagrams of the GT and LM methods with respect to the measured data, (b) is a comparison of the three-dimensional position diagrams of the GT and LM+NN methods with respect to the measured data, (c) is a three-dimensional position diagram of the GT and LM methods with respect to the simulated data, and (d) is a three-dimensional position diagram of the GT and LM+NN methods with respect to the simulated data. Detailed Implementation

[0022] To enable those skilled in the art to better understand the technical solutions of the present invention, the technical solutions of the present invention will be further described in detail below with reference to the accompanying drawings in the embodiments of the present invention.

[0023] like Figure 1 As shown, the magnet pose estimation method based on magnetic dipole constraints in this embodiment includes the following steps: S101, based on the triaxial magnetic field vectors measured by each magnetic sensor in the magnetic sensor array, perform initial orientation estimation of the magnet to determine the initial estimated orientation unit vector of the magnet, and perform estimated position estimation of the magnet to determine the initial estimated position of the magnet; S102, Based on the initial estimated direction unit vector and the initial estimated position of the magnet, the initial pose state parameters of the magnet are constructed. The initial pose state parameters are iteratively solved using a nonlinear optimization algorithm based on the magnetic dipole model to obtain the final estimated direction unit vector. S103, the final estimated direction unit vector, the initial estimated position, and the three-axis magnetic field vector measured by each magnetic sensor in the magnetic sensor array are input into the pre-trained residual neural network model to obtain the position residual and the direction residual. The residual neural network model is pre-trained and learns to establish the mapping relationship between the final estimated direction unit vector, the initial estimated position of the magnet, the three-axis magnetic field vector, and the output position residual and direction residual. S104. The initial estimated position is compensated based on the position residual to obtain the final magnet position, and the final estimated direction unit vector is compensated based on the direction residual to obtain the final magnet direction, thus obtaining the magnet pose estimation result composed of the final magnet position and the final magnet direction.

[0024] In this embodiment, the magnetic sensor array specifically adopts an array of nine magnetic sensors arranged in three rows and three columns. The set of three-axis magnetic field vectors measured by each magnetic sensor in the magnetic sensor array is as follows: ; in, It is the set of three-axis magnetic field vectors measured by each magnetic sensor in the magnetic sensor array; ~ These are the triaxial magnetic field vectors measured by the first to Nth magnetic sensors, respectively. Represents the number of magnetic sensors; the triaxial magnetic field vector measured by any i-th magnetic sensor. It can be represented as: ; in, Let x, y, and z be the magnetic fields measured by the i-th magnetic sensor, respectively. The set of the three-axis magnetic field vectors measured by each magnetic sensor in the magnetic sensor array. It represents the overall observation vector composed of all magnetic field observations from magnetic sensors, and can be used to describe the characteristics of the magnetic field distribution in the current space.

[0025] In step S101 of this embodiment, the step of estimating the initial orientation of the magnet to determine the initial estimated orientation unit vector of the magnet includes: S201, Obtain the three-axis magnetic field vector detected by the central magnetic sensor in the magnetic sensor array; The central magnetic sensor in the magnetic sensor array can be specified according to the shape of the magnetic sensor array. For example, for an array consisting of nine magnetic sensors in three rows and three columns, the magnetic sensor in the second row and second column is the central magnetic sensor. S202, determine whether the norm of the three-axis magnetic field vector detected by the central magnetic sensor is greater than the set threshold. If it is true, jump to step S203; otherwise, it means that the magnetic field signal of the central sensor is weak or unstable, and jump to step S204. S203, determine the initial estimated direction unit vector of the magnet based on the triaxial magnetic field vector detected by the central magnetic sensor: ; in, Let be the initial estimated direction unit vector of the magnet. The three-axis magnetic field vector detected by the central magnetic sensor. for The norm (modulus, representing the magnitude of the magnetic moment) is obtained by... Normalization can yield a direction and A unit vector of length 1 with the same magnetic field is used to represent the initial direction estimate of the magnetic dipole moment of the magnet and serves as the initial input for subsequent pose optimization or neural network correction algorithms; jump to step S204; S204, calculate the weighted average magnetic field vector based on the triaxial magnetic field vectors measured by each magnetic sensor in the magnetic sensor array: ; ; ; in, This is the weighted average magnetic field vector. The weight of the i-th magnetic sensor (this weight is used to reflect the contribution of different sensors to the magnet position estimation; sensors with stronger magnetic fields are usually closer to the magnet and are therefore given higher weights in position estimation). Let be the triaxial magnetic field vector measured by the i-th magnetic sensor. Let be the magnetic field strength of the i-th magnetic sensor. For the weighting index parameter, for The norm; the initial estimated direction unit vector of the magnet is determined based on the weighted average magnetic field vector: ; in, for The norm, through the Normalization can yield a direction and A unit vector of length 1 with a consistent magnetic field is used to represent the initial direction estimate of the magnetic dipole moment of the magnet and serves as the initial input for subsequent pose optimization or neural network correction algorithms.

[0026] In step S101 of this embodiment, the step of estimating the magnet's position to determine the initial estimated position of the magnet includes: S301, Determine the initial estimated position of the magnet. Direction coordinates and Direction coordinates: ; ; ; ; in, Indicates the initial estimated position of the magnet Direction coordinates; Indicates the initial estimated position of the magnet Direction coordinates The weight of the i-th magnetic sensor (this weight is used to reflect the contribution of different sensors to the magnet position estimation; sensors with stronger magnetic fields are usually closer to the magnet and are therefore given higher weights in position estimation). Let be the magnetic field strength of the i-th magnetic sensor. For the weighting index parameter, for norm, Let be the three-axis magnetic field vector measured by the i-th magnetic sensor; and These are the i-th magnetic sensors. Direction coordinates and Direction coordinates; S302, select the magnetic sensor in the magnetic sensor array that is closest to the magnet (or has the strongest magnetic field response) as the nearest response point, and record its position as... The corresponding maximum magnetic field modulus is Then, based on the magnetic sensor closest to the magnet in the magnetic sensor array, and the initial estimated position of the magnet, it can be determined. Direction coordinates and The offset of the orientation coordinates in the xy plane: ; in, This is the offset. and For the magnetic sensor array, the magnetic sensor closest to the magnet Direction coordinates and Direction coordinates; determining the initial estimated position of the magnet based on the offset. Direction coordinates: ; ; in, For the initial estimated position of the magnet Direction coordinates To obtain the maximum value, for The scale factor of the directional coordinates (this relationship uses the physical law that the magnetic field of a magnetic dipole decays with distance to make a preliminary estimate of the height of the magnet). for The lower limit of the direction coordinate. For reference height, For reference magnetic field strength, This represents the triaxial magnetic field vector measured by the magnetic sensor closest to the magnet in the magnetic sensor array; if the initial estimated position of the magnet is... Direction coordinates exceed range Then crop it to the range Inside, among which for The upper limit of the directional coordinates. For example, in one embodiment, additional constraints can be added based on prior knowledge, such as if the magnet is known to be located above the sensor plane, then the following can be further set: ,in This indicates the preset lower bound of the height (the minimum height allowed by the system), thus obtaining a stable initial height. Finally, the initial estimated position of the magnet is obtained: ; in, This is the initial estimated position of the magnet. Indicates the initial estimated position of the magnet Direction coordinates; Indicates the initial estimated position of the magnet Direction coordinates For the initial estimated position of the magnet Direction coordinates.

[0027] In this embodiment, step S102 includes: S401, Constructing the initial pose parameters of the magnet: ; ; in, These are the initial pose parameters of the magnet. This is the initial estimated position of the magnet. is the magnetic dipole moment vector of the magnet (which can be expressed as the product of the magnitude and direction vector of the magnetic moment, used to describe the strength and direction characteristics of the magnetic field generated by the magnet). for norm, This is the initial estimated direction unit vector of the magnet; S402, the initial estimated position in the initial pose state parameters of the magnet. As the position of the magnet in the magnetic dipole model Magnetic dipole moment vector As the magnetic dipole moment vector of the magnet in the magnetic dipole model, the initial pose state parameters of the magnet are thus substituted into the magnetic dipole model: ; ; in, Let be the triaxial magnetic field vector calculated by the i-th magnetic sensor using the magnetic dipole model. Let be the magnetic dipole moment vector of the magnet. Let be the relative position of the magnet to the i-th magnetic sensor. Let i be the position of the i-th magnetic sensor. The position of the magnet. for The norm of the magnetic field is obtained by minimizing the following objective function to obtain the magnetic dipole moment vector of the magnet when the objective function converges: ; ; in, Let be the triaxial magnetic field vector calculated by the i-th magnetic sensor using the magnetic dipole model. Let be the magnetic dipole moment vector of the magnet. Let be the relative position of the magnet to the i-th magnetic sensor. Let i be the position of the i-th magnetic sensor. The position of the magnet. for norm, This is the set of triaxial magnetic field vectors calculated from all magnetic sensors using the magnetic dipole model. ~ The three-axis magnetic field vectors are calculated for the first to Nth magnetic sensors using the magnetic dipole model. It is the set of three-axis magnetic field vectors measured by each magnetic sensor in the magnetic sensor array; S403, calculate the final estimated direction unit vector based on the magnetic dipole moment vector of the magnet when the objective function converges: ; in, To finally estimate the direction unit vector, Let be the magnetic dipole moment vector of the magnet when the objective function converges. for The norm of .

[0028] The physical calculation takes into account the reversed S / N poles of the magnet (n and For the problem of multiple solutions for oriented directions (where n directions are equivalent), when the dot product of the direction vector and the preset reference direction is less than zero, the direction vector can be inverted so that it is located in the same hemisphere as the reference direction. Therefore, in this embodiment, after step S102 and before step S103, the final estimated direction unit vector is further subjected to hemispherical unification processing. ; in, This is the final estimated direction unit vector after hemispherical unification processing. To finally estimate the direction unit vector, The preset reference direction is used. The dot product of the direction vector and the preset reference direction is the direction error. The direction error is defined using the axial direction equivalence constraint: ,in To predict direction, For true directions, this definition extends the direction space from the unit sphere. Mapped to projective space This eliminates ambiguity in direction signs, improves the consistency between model training and evaluation, and solves the multivalued problem of magnetic dipole orientation through the above-mentioned hemispherical unified processing, ensuring the consistency of model predictions under different spatial orientations.

[0029] In step S103, when the final estimated direction unit vector, the initial estimated position, and the triaxial magnetic field vectors measured by each magnetic sensor in the magnetic sensor array are input into the pre-trained residual neural network model to obtain the position residual and direction residual, the final estimated direction unit vector is obtained. Initial estimated location and the set of triaxial magnetic field vectors measured by each magnetic sensor in the magnetic sensor array. Can constitute input features : ; Input features The position and orientation residuals obtained by inputting a pre-trained residual neural network model can be expressed as: ; in, For positional residuals, For directional residuals, This is a residual neural network model. The residual neural network model is pre-trained and learns to establish the final estimated direction unit vector, the initial estimated position of the magnet, and the three-axis magnetic field vector, as well as the mapping relationship between the output position residual and direction residual. The required neural network model can be selected as needed, for example... Figure 2 As shown, the residual neural network model in this embodiment includes an input layer, a hidden layer, and an output layer. The hidden layer consists of three fully connected layers FC1 to FC3 connected in sequence. The input layer is used to receive input features. The hidden layers are activated using the ReLU activation function, and the output layer is used to output the position residuals. and directional residuals .

[0030] In step S104 of this embodiment, the functional expression for compensating the initial estimated position based on the position residual to obtain the final magnet position is: ; in, For the final magnet position, This is the initial estimated position of the magnet. The position residual; the functional expression for compensating the final estimated direction unit vector based on the direction residual to obtain the final magnet direction is: ; in, For the final magnet orientation, This is the final estimated direction unit vector after hemispherical unification processing. For directional residuals, for and The norm of the sum of the two. Thus, the magnet pose estimation result, consisting of the final magnet position and the final magnet orientation, is obtained. .

[0031] .

[0032] The training steps for the residual neural network model in this embodiment include: S501, Construct a residual neural network model; S502, Construct the training dataset for the residual neural network model. The samples in the training dataset include input features. Location residual label and directional residual labels Among them, input features The construction method is described in steps S101 to S102 above, and will not be repeated here; S503, a residual neural network model is trained using the training dataset, enabling it to learn and establish the final estimated direction unit vector, the initial estimated position of the magnet, the three-axis magnetic field vector, and the mapping relationship between the output position residual and direction residual. In this embodiment, training the residual neural network model using the training dataset includes updating the neural network parameters using the Adam optimizer, with the learning rate set to 1×10⁻⁶. -3 To improve the model's generalization ability, a weight decay term is introduced during the loss function optimization process, with a decay coefficient set to 1×10⁻⁶. -5 To suppress model overfitting, the network was trained for 300 rounds to approximate the optimal solution, thereby achieving effective learning and compensation for magnet pose errors.

[0033] In this embodiment, during the training phase of the residual neural network model, the label calculation function expressions used for the position residual and orientation residual are as follows: ; ; in, For location residual labels, This represents the actual position of the magnet. This is the initial estimated position of the magnet. For directional residual labels, The true orientation of the magnet. This is the final estimated direction unit vector.

[0034] In this embodiment, the residual neural network model, during the training phase, includes applying a norm pruning operation to the labels used for the positional residuals: ; in, To label the positional residuals after applying the norm clipping operation. For the original position residual labels, for norm, This is the preset maximum allowable norm threshold.

[0035] In this embodiment, the loss function used by the residual neural network model during the training phase is: ; ; ; in, For loss function, and These are the weighting coefficients. For position loss, For directional loss, For location residual labels, For directional residual labels, The position residuals predicted by the residual neural network model. This refers to the direction residual predicted by the residual neural network model. Weighting coefficients. and This loss function is used to balance the optimization intensity of position estimation and orientation estimation tasks, enabling the network to adaptively focus on components with larger errors or slower convergence during training, thereby improving the stability and accuracy of joint learning. This loss function effectively alleviates the conflict of multi-objective (position, orientation) optimization in magnetic positioning tasks.

[0036] To verify the feasibility of the magnet pose estimation method based on magnetic dipole constraints in this embodiment, training was conducted using a mixed dataset for comparative training, including 15,854 sets of simulation data collected by nine sensors in a 3x3 grid, and 1,403 sets of measured data. The ground truth values ​​were mapped by filenames; for example, the filename -60.0 -15.0 170.0.csv could be parsed to obtain the true position as (-60.0, -15.0, 170.0) mm. The inference output was in tabular form, including RMSE (Root Mean Square), Mean (mean error), Max (maximum error), and Time (average time). According to the error statistics table, the magnet pose estimation method based on magnetic dipole constraints in this embodiment (physical model + neural network correction, abbreviated as LM+NN) is significantly better than the traditional LM (physical model) algorithm in all core indicators (the position and orientation accuracy of the simulation data is improved by 39.75% and 97.85% respectively, and the position and orientation accuracy of the measured data is improved by 53.67% and 98.41% respectively). Moreover, after introducing neural network processing, the increase in inference time is within an acceptable range (only 0.419ms for simulation data and 0.372ms for measured dataset), which verifies the feasibility of the magnet pose estimation method based on magnetic dipole constraints in this embodiment in real-time applications.

[0037] To verify the performance of the loss function in the magnet pose estimation method based on magnetic dipole constraints in this embodiment, comparative tests were conducted on simulation datasets and measured datasets. The test results are as follows: Figure 3 and Figure 4 As shown, where Figure 3 This is a graph showing the convergence curve of the loss function for the simulation dataset in this embodiment. Figure 4 This is the convergence curve of the loss function for the measured dataset in this embodiment. According to... Figure 3 The convergence curves of the loss function on the simulation dataset show that the residual network with physical priors reaches the convergence threshold within a very short training period (<5 epochs). This demonstrates that LM optimization provides a strong initial pose prior for the neural network, allowing it to focus on correcting small residuals without having to learn complex electromagnetic nonlinear mappings from scratch, thus significantly reducing training difficulty. Furthermore, the loss curves of the training set (TrainLoss) and the validation set (VailLoss) exhibit high consistency and maintain extremely high stability over 300 epochs, indicating that this residual learning model has significant generalization ability and can effectively cope with complex environmental noise in magnetic positioning. Meanwhile, according to... Figure 4 The convergence curve results of the loss function on the actual test dataset show that the model does not have obvious overfitting, but there are slight fluctuations and a few outliers. Both the training loss and the validation loss converge quickly and remain consistent, indicating that the model has good fitting ability and generalization performance. The loss tends to stabilize in the later stage of training, indicating that the model has converged fully.

[0038] To verify the effectiveness of the magnet pose estimation method based on magnetic dipole constraint in this embodiment, the magnet pose of magnets with heights of 78mm, 108mm, and 138mm in three dimensions was tested. The results are as follows. Figures 5 to 7 As shown, LM represents the test results of the traditional method (physical model, abbreviated as LM), LM+NN represents the test results of the magnet pose estimation method based on magnetic dipole constraint in this embodiment (physical model + neural network correction, abbreviated as LM+NN), and GT represents the true value. Figures 5 to 7 In the diagram, (a) shows a comparison of the 3D position plots of the GT and LM methods with respect to the measured data; (b) shows a comparison of the 3D position plots of the GT and LM+NN methods with respect to the measured data; (c) shows the 3D position plots of the GT and LM methods with respect to the simulated data; and (d) shows the 3D position plots of the GT and LM+NN methods with respect to the simulated data. Figure 5 and Figure 6 It can be seen that the deviations in the three-dimensional positions of the measured and simulated data are both around 1mm, with no significant deviations. The maximum error is mainly concentrated at the edge position of the sensing matrix (x, y=75), which may be affected by the drastic changes in the magnetic field gradient at the edge of the sensor array. Furthermore, the error in the measured data is significantly greater than that in the simulated data, consistent with the phenomenon of environmental interference in the measured data. Figure 7 As can be seen, the positional error of the measured data at a height of 138mm increases significantly. This may be because the magnetic field signal weakens with increasing distance, leading to a decrease in the signal-to-noise ratio and increasing the difficulty of achieving accurate magnetic positioning. Therefore, the magnet pose estimation method based on magnetic dipole constraints in this embodiment combines the prior knowledge of the physical model with the nonlinear fitting capability of the neural network. It uses a method of "LM (physical model) optimization + neural network (NN) residual correction" to obtain the magnet pose estimation result composed of the final magnet position and final magnet orientation from the existing magnetic field data. This allows the neural network to learn the residual between the physical algorithm and the actual pose, effectively eliminating possible systematic biases in the physical model algorithm residuals.

[0039] In summary, to achieve high-precision estimation of the six-dimensional pose of a magnet, this embodiment presents a magnet pose estimation method based on magnetic dipole constraints. By combining physical model constraints with data-driven residual compensation, the method improves the robustness and generalization ability of the system to factors such as initial value deviation, sensor error, and environmental interference. This overcomes the problems of traditional magnetic dipole methods, such as sensitivity to initial values, susceptibility to noise, and systematic errors, thereby improving the accuracy of magnet position and orientation measurement in complex environments.

[0040] This embodiment also provides a magnetic pose estimation system based on magnetic dipole constraints, including a microprocessor and a memory interconnected, wherein the microprocessor is programmed or configured to execute the magnetic pose estimation method based on magnetic dipole constraints.

[0041] Those skilled in the art will understand that the technical solutions provided by this invention may take the form of a method, system, or computer program product. Therefore, this invention may take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this invention may take the form of a computer program product embodied on one or more computer-readable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code. This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, produce an implementation of the flowchart... Figure 1 One or more processes and / or boxes Figure 1 The computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to operate in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The functions specified in one or more boxes. These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable apparatus for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0042] The above description is merely a preferred embodiment of the present invention. The scope of protection of the present invention is not limited to the above embodiments. All technical solutions falling within the scope of the present invention's concept are within the scope of protection of the present invention. It should be noted that for those skilled in the art, any improvements and modifications made without departing from the principles of the present invention should also be considered within the scope of protection of the present invention.

Claims

1. A method for estimating the pose of a magnet based on magnetic dipole constraints, characterized in that, Includes the following steps: S101, based on the triaxial magnetic field vectors measured by each magnetic sensor in the magnetic sensor array, perform initial orientation estimation of the magnet to determine the initial estimated orientation unit vector of the magnet, and perform estimated position estimation of the magnet to determine the initial estimated position of the magnet; S102, Based on the initial estimated direction unit vector and the initial estimated position of the magnet, the initial pose state parameters of the magnet are constructed. The initial pose state parameters are iteratively solved using a nonlinear optimization algorithm based on the magnetic dipole model to obtain the final estimated direction unit vector. S103, the final estimated direction unit vector, the initial estimated position, and the three-axis magnetic field vector measured by each magnetic sensor in the magnetic sensor array are input into the pre-trained residual neural network model to obtain the position residual and the direction residual. The residual neural network model is pre-trained and learns to establish the mapping relationship between the final estimated direction unit vector, the initial estimated position of the magnet, the three-axis magnetic field vector, and the output position residual and direction residual. S104. The initial estimated position is compensated based on the position residual to obtain the final magnet position, and the final estimated direction unit vector is compensated based on the direction residual to obtain the final magnet direction, thus obtaining the magnet pose estimation result composed of the final magnet position and the final magnet direction.

2. The magnet pose estimation method based on magnetic dipole constraints according to claim 1, characterized in that, In step S101, the initial orientation estimation of the magnet to determine the initial estimated orientation unit vector of the magnet includes: S201, Obtain the three-axis magnetic field vector detected by the central magnetic sensor in the magnetic sensor array; S202, determine whether the norm of the three-axis magnetic field vector detected by the central magnetic sensor is greater than the set threshold. If it is true, proceed to step S203; otherwise, proceed to step S204. S203, determine the initial estimated direction unit vector of the magnet based on the triaxial magnetic field vector detected by the central magnetic sensor: ； in, Let be the initial estimated direction unit vector of the magnet. The three-axis magnetic field vector detected by the central magnetic sensor. for Norm; jump to step S204; S204, calculate the weighted average magnetic field vector based on the triaxial magnetic field vectors measured by each magnetic sensor in the magnetic sensor array: ； ； ； in, This is the weighted average magnetic field vector. Let be the weight of the i-th magnetic sensor. Let be the triaxial magnetic field vector measured by the i-th magnetic sensor. Let be the magnetic field strength of the i-th magnetic sensor. For the weighting index parameter, for The norm; the initial estimated direction unit vector of the magnet is determined based on the weighted average magnetic field vector: ； in, for The norm of .

3. The magnet pose estimation method based on magnetic dipole constraints according to claim 1, characterized in that, In step S101, the process of estimating the magnet's estimated position to determine the initial estimated position of the magnet includes: S301, Determine the initial estimated position of the magnet. Direction coordinates and Direction coordinates: ； ； ； ； in, Indicates the initial estimated position of the magnet Direction coordinates; Indicates the initial estimated position of the magnet Direction coordinates Let be the weight of the i-th magnetic sensor. Let be the magnetic field strength of the i-th magnetic sensor. For the weighting index parameter, for norm, Let be the three-axis magnetic field vector measured by the i-th magnetic sensor; and These are the i-th magnetic sensors. Direction coordinates and Direction coordinates; S302, based on the magnetic sensor closest to the magnet in the magnetic sensor array, and the initial estimated position of the magnet. Direction coordinates and The offset of the orientation coordinates in the xy plane: ； in, This is the offset. and For the magnetic sensor array, the magnetic sensor closest to the magnet Direction coordinates and Direction coordinates; determining the initial estimated position of the magnet based on the offset. Direction coordinates: ； ； in, For the initial estimated position of the magnet Direction coordinates To obtain the maximum value, for The scaling factor of the orientation coordinates. for The lower limit of the direction coordinate. For reference height, For reference magnetic field strength, This represents the triaxial magnetic field vector measured by the magnetic sensor closest to the magnet in the magnetic sensor array; if the initial estimated position of the magnet is... Direction coordinates exceed range Then crop it to the range Inside, among which for The upper limit of the directional coordinates ultimately yields the initial estimated position of the magnet: ； in, This is the initial estimated position of the magnet. Indicates the initial estimated position of the magnet Direction coordinates; Indicates the initial estimated position of the magnet Direction coordinates For the initial estimated position of the magnet Direction coordinates.

4. The magnet pose estimation method based on magnetic dipole constraints according to claim 1, characterized in that, Step S102 includes: S401, Constructing the initial pose parameters of the magnet: ； ； in, These are the initial pose parameters of the magnet. This is the initial estimated position of the magnet. Let be the magnetic dipole moment vector of the magnet. for norm, This is the initial estimated direction unit vector of the magnet; S402, the initial estimated position in the initial pose state parameters of the magnet. As the position of the magnet in the magnetic dipole model Magnetic dipole moment vector As the magnetic dipole moment vector of the magnet in the magnetic dipole model, the initial pose state parameters of the magnet are thus substituted into the magnetic dipole model: ； ； in, Let be the triaxial magnetic field vector calculated by the i-th magnetic sensor using the magnetic dipole model. Let be the magnetic dipole moment vector of the magnet. Let be the relative position of the magnet to the i-th magnetic sensor. Let i be the position of the i-th magnetic sensor. The position of the magnet. for The norm of the magnetic field is obtained by minimizing the following objective function to obtain the magnetic dipole moment vector of the magnet when the objective function converges: ； ； in, Let be the triaxial magnetic field vector calculated by the i-th magnetic sensor using the magnetic dipole model. Let be the magnetic dipole moment vector of the magnet. Let be the relative position of the magnet to the i-th magnetic sensor. Let i be the position of the i-th magnetic sensor. The position of the magnet. for norm, This is the set of triaxial magnetic field vectors calculated from all magnetic sensors using the magnetic dipole model. ~ The three-axis magnetic field vectors are calculated for the first to Nth magnetic sensors using the magnetic dipole model. It is the set of three-axis magnetic field vectors measured by each magnetic sensor in the magnetic sensor array; S403, calculate the final estimated direction unit vector based on the magnetic dipole moment vector of the magnet when the objective function converges: ； in, To finally estimate the direction unit vector, Let be the magnetic dipole moment vector of the magnet when the objective function converges. for The norm of .

5. The magnet pose estimation method based on magnetic dipole constraints according to claim 1, characterized in that, After step S102 and before step S103, the process also includes performing hemispherical unification processing on the final estimated direction unit vector: ； in, This is the final estimated direction unit vector after hemispherical unification processing. To finally estimate the direction unit vector, This is the preset reference direction.

6. The magnet pose estimation method based on magnetic dipole constraint according to claim 1, characterized in that, In step S104, the functional expression for compensating the initial estimated position based on the position residual to obtain the final magnet position is: ； in, For the final magnet position, This is the initial estimated position of the magnet. The position residual; the functional expression for compensating the final estimated direction unit vector based on the direction residual to obtain the final magnet direction is: ； in, For the final magnet orientation, This is the final estimated direction unit vector after hemispherical unification processing. For directional residuals, for and The norm of the sum of the two.

7. The magnet pose estimation method based on magnetic dipole constraints according to claim 1, characterized in that, During the training phase of the residual neural network model, the label calculation function expressions used for the position residual and orientation residual are as follows: ； ； in, For location residual labels, This represents the actual position of the magnet. This is the initial estimated position of the magnet. For directional residual labels, The true orientation of the magnet. This is the final estimated direction unit vector.

8. The magnet pose estimation method based on magnetic dipole constraints according to claim 7, characterized in that, During the training phase, the residual neural network model includes applying a norm pruning operation to the labels used for the location residuals: ； in, To label the positional residuals after applying the norm clipping operation. For the original position residual labels, for norm, This is the preset maximum allowable norm threshold.

9. The magnet pose estimation method based on magnetic dipole constraints according to claim 7, characterized in that, The loss function used by the residual neural network model during the training phase is: ； ； ； in, For loss function, and These are the weighting coefficients. For position loss, For directional loss, For location residual labels, For directional residual labels, The position residuals predicted by the residual neural network model. This refers to the directional residual predicted by the residual neural network model.

10. A magnetic pose estimation system based on magnetic dipole constraints, comprising a microprocessor and a memory interconnected, characterized in that, The microprocessor is programmed or configured to execute the magnet pose estimation method based on magnetic dipole constraints as described in any one of claims 1 to 9.

Images