Magnetic sensing positioning method and system based on confidence domain optimization and adaptive residual correction

Abstract

The application relates to the technical field of magnetic positioning, in particular to a magnetic sensing positioning method and system based on confidence domain optimization and adaptive residual correction, which decomposes a spatial magnetic field generated by double-orthogonal rotating permanent magnets at a transmitting end into vector components under a coordinate system at a receiving end magnetic sensor, and establishes a mathematical mapping between the magnetic field vector and a relative pose vector; the target position coordinates to be solved are taken as hidden states, a state equation is used to describe the target motion law to be solved, an observation equation is used to describe the mathematical mapping between the magnetic field vector and the relative pose, prior information is combined, and lossless Kalman filtering iteration is carried out to obtain optimal estimation of the target position to be solved, wherein, in the lossless Kalman filtering iteration, a theoretical initial value is obtained through confidence domain optimization, and process noise and state noise are dynamically adjusted according to the current target motion state to adaptively correct the relative pose estimation. The application can realize accurate identification of the target motion state and high-precision magnetic positioning tracking.

Description

Technical Field

[0001] This invention relates to the field of magnetic positioning technology, and in particular to a magnetic sensing positioning method and system based on confidence domain optimization and adaptive residual correction, to achieve dynamic tracking of the six degrees of freedom attitude and position of dynamic targets in GNSS signal-denied environments (indoor, underwater, underground, industrial workshops, etc.). Background Technology

[0002] High-precision attitude and position dynamic tracking is a core technology in the fields of artificial intelligence, robotics, and the Internet of Things (IoT), and is widely used in VR / AR, gesture recognition, medical interventional surgical navigation, humanoid robot control, and environmental assistance. Achieving line-of-sight-free and interference-resistant wireless positioning in environments where GNSS signals are unavailable, such as indoors, underwater, and underground, is a current research hotspot in the IoT field. Existing mainstream positioning technologies mainly include radio positioning, optical positioning, and inertial positioning: radio positioning accuracy is limited by base station density and indoor multipath effects; optical positioning is easily affected by lighting and physical obstructions; and inertial positioning suffers from inherent defects such as difficulty in initial position calibration and accelerated error accumulation over time. Magnetic positioning technology, based on the principle of electromagnetic induction, achieves non-contact six-degree-of-freedom parameter estimation. With its unique advantages of being line-of-sight-free, highly resistant to electromagnetic interference, and capable of real-time three-dimensional positioning, it has become the preferred solution in the field of spatial perception. Its technical routes are mainly divided into two categories: coil excitation and permanent magnet excitation. Coil excitation positioning technology constructs a three-axis orthogonal magnetic field architecture, but it has problems such as large receiving coil volume, failure of the near-distance magnetic dipole model, and limited effective range. Permanent magnet excitation positioning technology has been commercially applied in fields such as magnetically driven capsule endoscopy, medical surgical navigation, and finger tracking in consumer electronics due to its low power consumption and low system complexity, but it is still subject to physical limitations such as rapid decay of magnetic field gradient and distortion of environmental magnetic field.

[0003] Due to the highly nonlinear characteristics of magnetic positioning systems, the Extended Kalman Filter (EKF) suffers from inherent linearization errors, resulting in significantly lower positioning accuracy than the Unscented Kalman Filter (UKF) in complex electromagnetic environments. Traditional UKF algorithms achieve nonlinear probability density approximation through unscented transformations, performing well in simple uniform trajectory estimation. However, they lack motion state perception capabilities and cannot adjust filter parameters in real-time for varying conditions such as target maneuvering or stability, making them unsuitable for the high-precision tracking requirements of complex trajectory changes.

[0004] When current dual-axis rotating permanent magnet magnetic positioning systems are coupled with traditional filtering algorithms, the following problems exist due to limitations in system modeling, noise processing, and filtering mechanisms: 1. Fixed filtering parameters and poor motion adaptability: Traditional unscented Kalman filtering (UKF) uses fixed-state noise covariance and estimation error matrix, without establishing a target motion state perception mechanism. In complex trajectory-changing scenarios, the convergence weights of predicted and observed values ​​become unbalanced, and the rapid accumulation of positioning errors leads to divergence in the filtering results. 2. Insufficient anti-interference capability: The system is susceptible to magnetic field distortion caused by uneven Earth magnetic field and metal interference. Traditional algorithms are insufficient in suppressing Gaussian and non-Gaussian mixed noise and outliers in observations / news. In practical environments, the positioning robustness is difficult to meet the high-precision requirements of medical and industrial control. 3. Sensitivity to initial values ​​in the attitude angle π fuzzy domain: Under the dual-axis orthogonal rotating permanent magnet framework, traditional filtering algorithms suffer from attitude angle fuzziness, poor adaptability to complex motion trajectories, and strong dependence on initial values, further limiting positioning accuracy and scene adaptability. 4. Lack of adaptive adjustment mechanism: The adaptive adjustment mechanism driven by residual probability distribution has not been constructed, which makes it impossible to balance the real-time tracking of target maneuvering motion with the high positioning accuracy of stable motion. The mismatch between system modeling and actual physical environment is prominent. Summary of the Invention

[0005] To address the problems of poor motion adaptability, unsatisfactory anti-interference ability, strong dependence on initial values, and modeling mismatch with the physical environment in existing magnetic positioning technologies, this invention provides a magnetic sensing positioning method and system based on confidence region optimization and adaptive residual correction. By constructing residual correction, it achieves accurate identification of the target's motion state and can be applied to scenarios such as medical surgery navigation, VR / AR interaction, industrial robot tracking, and IoT complex environment positioning. It has the advantages of high precision, strong robustness, and adaptive motion state tracking capability.

[0006] According to the design scheme provided by the present invention, on the one hand, a magnetic sensing localization method based on confidence region optimization and adaptive residual correction is provided, comprising:

[0007] The spatial magnetic field generated by the biorthogonal rotating permanent magnet at the transmitting end is decomposed into vector components in the coordinate system at the magnetic sensor at the receiving end, and a mathematical mapping between the magnetic field vector and the relative pose vector is established.

[0008] The target position coordinates are taken as the hidden state, and the magnetic field components obtained based on the space vector model are taken as the observations. The motion law of the target is described by the state equation, and the mathematical mapping between the magnetic field vector and the relative pose is described by the observation equation. Combined with prior information and through lossless Kalman filtering iteration, the optimal estimate of the target position is obtained. In the lossless Kalman filtering iteration, the initial theoretical value is obtained by confidence region optimization, and the process noise and state noise are dynamically adjusted according to the current target motion state to adaptively correct the relative pose estimate.

[0009] As a further step in the present invention, the magnetic sensing localization method based on confidence region optimization and adaptive residual correction utilizes state equations to describe the motion law of the target to be solved, including:

[0010] The target motion path acquired by the magnetic sensor is discretized according to the time series and divided into several time step units;

[0011] In the target motion trajectory modeling, the trajectory segments between adjacent time nodes are transitioned into circular arcs so that the centripetal acceleration can be obtained by the cross product of the angular velocity vector and the linear velocity vector.

[0012] As a magnetic sensing localization method based on confidence region optimization and adaptive residual correction of the present invention, the observation equation is further expressed as: ,in, is the magnetic moment constant. For attitude angle , , The magnetic sensor attitude matrix is ​​represented. This represents the magnetization axis vector of the permanent magnet. This represents the position vector between the magnetic sensor and the permanent magnet, where i represents the permanent magnet number. , , This is environmental magnetic field noise.

[0013] As part of the confidence region optimization and adaptive residual correction magnetic sensing localization method of this invention, the optimal estimate of the target position is obtained through lossless Kalman filtering iteration, including:

[0014] The theoretical initial value is obtained through confidence region optimization, and the theoretical initial value includes the initial state mean vector and the initial state covariance matrix.

[0015] Generate Sigma points based on the mean and covariance of the current state, and calculate the mean weight and covariance weight corresponding to each Sigma point;

[0016] Substitute all generated Sigma points into the state transition function to generate new Sigma points after prediction, and obtain the predicted state and the predicted state covariance matrix based on the new Sigma points;

[0017] The new Sigma point is passed through the observation equation to obtain the predicted observation value and the predicted observation value covariance matrix. If the vector dot product between the transpose of the current predicted observation value and the previous predicted observation value is less than 0, the current predicted observation value is flipped and updated.

[0018] The Kalman gain is obtained based on the predicted state, the predicted observations, and their covariance matrix. The state and covariance are then updated based on the Kalman gain to obtain the optimal estimate for the current time step based on the updated state and covariance.

[0019] As part of the magnetic sensing localization method based on confidence region optimization and adaptive residual correction of this invention, the theoretical initial value is obtained through confidence region optimization, including:

[0020] The initial state of magnetic sensing positioning is obtained through iterative search with confidence region constraints, where the confidence region is the confidence area around the current iteration point.

[0021] As a magnetic sensing localization method based on confidence region optimization and adaptive residual correction of the present invention, further comprising obtaining the Kalman gain based on the predicted state, the predicted observations, and the covariance matrix of the two, the method also includes:

[0022] The residual at the current time step is obtained based on the predicted state and predicted observations, and the residual at the current time step is stored in the sliding window buffer.

[0023] During the filtering process, when the sliding window buffer is full, the probability value of each residual component in the current sliding window buffer being greater than 0 is determined, and the offset of the probability value is obtained using a statistical function. When the offset of all residual components in the sliding window buffer does not exceed the threshold, the current target position is determined to be in steady-state motion, and velocity and angular velocity noise is reduced based on the constraint matrix. If the offset of the residual components in the sliding window buffer exceeds the threshold, the current target position is determined to be in unsteady-state motion, and the noise covariance matrix is ​​linearly incremented based on the noise increment matrix to increase the noise of the velocity and angular velocity process, and the state equation is adjusted based on the current target motion state.

[0024] As part of the confidence region optimization and adaptive residual correction magnetic sensing localization method of this invention, the generation of Sigma points further includes:

[0025] The state covariance is scaled using scaling parameters, and singular value decomposition is performed on the scaled state covariance.

[0026] The square roots of singular values ​​are extracted from the singular value decomposition results, and the offsets of Sigma points are generated based on the square roots of singular values, so that each Sigma point can be obtained according to the offsets.

[0027] Furthermore, this invention also provides a magnetic sensing positioning system based on confidence region optimization and adaptive residual correction, comprising: a modeling module and a solution module, wherein,

[0028] The modeling module is used to decompose the spatial magnetic field generated by the biorthogonal rotating permanent magnet at the transmitter into vector components in the coordinate system at the magnetic sensor at the receiver, and to establish a mathematical mapping between the magnetic field vector and the relative pose vector.

[0029] The solution module uses the target's position coordinates as the hidden state and the magnetic field components obtained from the space vector model as the observations. It uses the state equation to describe the target's motion law and the observation equation to describe the mathematical mapping between the magnetic field vector and the relative pose. Combining prior information and through lossless Kalman filtering iteration, it obtains the optimal estimate of the target's position. In the lossless Kalman filtering iteration, the initial theoretical value is obtained through confidence region optimization, and the process noise and state noise are dynamically adjusted according to the current target motion state to adaptively correct the relative pose estimate.

[0030] The beneficial effects of this invention are:

[0031] 1. This invention constructs a residual sequence and sets a threshold mechanism to achieve accurate identification of the target motion state and online estimation of noise covariance. This enables the algorithm to maintain high estimation consistency when facing sudden motion states, effectively suppresses estimation deviations caused by model mismatch or external interference, and achieves high-precision magnetic positioning and tracking.

[0032] 2. This invention employs an embedded confidence region optimization strategy, which reduces the algorithm's sensitivity to initial parameter settings and enhances its generalization ability in unmodeled dynamic or unexpected noise environments. Joint optimization of system parameters such as sampling frequency, rotor speed, and residual discrimination threshold further improves the algorithm's adaptability under different signal-to-noise ratio conditions, enabling it to maintain expected positioning performance even in various real-world noise backgrounds. This allows the filtering results to adapt to complex motion states such as track changes and maneuvering speed changes. Attached Figure Description

[0033] Figure 1 This is a schematic diagram of the magnetic sensing localization method based on confidence region optimization and adaptive residual correction in the embodiment.

[0034] Figure 2 This is a schematic diagram illustrating the working principle of the magnetic signal transceiver device in the embodiment;

[0035] Figure 3 This is a schematic diagram of the spatial geometry of magnetic positioning in the embodiment;

[0036] Figure 4 This example illustrates the comparison of filtering effects before and after confidence domain optimization, as well as the divergence statistics.

[0037] Figure 5 This is a schematic diagram comparing the filtering effects before and after introducing adaptive residuals in the embodiment;

[0038] Figure 6 The simulation results of the filtering estimation are shown in the example. Detailed Implementation

[0039] To make the objectives, technical solutions, and advantages of this invention clearer and more understandable, the invention will be further described in detail below with reference to the accompanying drawings and technical solutions.

[0040] To address the problems of poor motion adaptability, unsatisfactory anti-interference ability, strong dependence on initial values, and modeling mismatch with the physical environment in existing magnetic positioning technologies, embodiments of the present invention, such as... Figure 1 As shown, a magnetic sensing localization method based on confidence region optimization and adaptive residual correction is provided, comprising:

[0041] S101. The spatial magnetic field generated by the dual orthogonal rotating permanent magnets at the transmitting end is decomposed into vector components in the coordinate system at the magnetic sensor at the receiving end, and a mathematical mapping between the magnetic field vector and the relative pose vector is established.

[0042] like Figure 2 The illustrated magnetic signal transceiver operates on the principle that, in this cooperative positioning platform, the magnetic field generator is located at the origin and its position is known, while the spatial coordinates of the magnetic sensor are unknown. In the spatial geometric model of the magnetic positioning system constructed based on a three-dimensional Cartesian coordinate system, the spatial geometric center of the magnetic sensor is the unknown coordinate point to be solved, and its three-dimensional position is represented by a spatial vector. The image shows a physical model of the magnetic sensor for visualization.

[0043] The electromagnetic signal generated by the rotating permanent magnet at the position of the magnetic sensor and its spatial vector characterization are as follows: Figure 3 As shown, the angle between the magnetic moment square of the permanent magnet and the x-axis is... .like Figure 3 As shown, the near-field magnetic field H generated by the rotating permanent magnet can be composed of a radial component. and tangential components The descriptions and specific expressions are listed in Table 1. In Table 1, M is the magnetic moment, and φ is the angle between the coil axis and the direction pointing towards the sensor.

[0044]

[0045] To describe the total excitation field H at any location within the near-field region, such as Figure 3 As shown, the definition is introduced. The angles ψτ and θτ of the field direction are given in Table 1. Place and Simplified formulas for the components, these formulas are obtained through Figure 3The geometric construction and basic trigonometric transformations are used to derive the formulas. The formulas in Tables 1 and 2 can also be used to write the total excitation field components at any sensor location. , , The explicit equation.

[0046]

[0047] S102. The position coordinates of the target to be solved are taken as the hidden state, and the magnetic field components obtained based on the space vector model are taken as the observation. The motion law of the target to be solved is described by the state equation, and the mathematical mapping between the magnetic field vector and the relative pose is described by the observation equation. Combined with prior information and through lossless Kalman filtering iteration, the optimal estimate of the position of the target to be solved is obtained. In the lossless Kalman filtering iteration, the theoretical initial value is obtained by confidence region optimization, and the process noise and state noise are dynamically adjusted according to the current target motion state to adaptively correct the relative pose estimate.

[0048] Specifically, the target motion path collected by the magnetic sensor can be discretized according to the time sequence and divided into several time step units; in the target motion trajectory modeling, the trajectory segments between adjacent time nodes are transitioned into circular arc curves so as to obtain the centripetal acceleration by the cross product of the angular velocity vector and the linear velocity vector.

[0049] For any time k, Corresponding sensor spatial coordinates , , , Indicates attitude angle , , , Represents the velocity components in the three axes. , , , Corresponding angular velocity , , , Different accelerations are used for different motion states. This represents process noise. To achieve high-precision reconstruction of dynamic target trajectories, in this embodiment, the complex motion trajectory of the positioning target is differentiated, and the motion path collected by the sensor is discretized according to the time series, divided into several time step units.

[0050]

[0051]

[0052] In trajectory modeling, the trajectory segment between adjacent time nodes k-1 and k is approximated as a circular arc. Based on the dynamics of circular motion, centripetal acceleration can be obtained by the cross product of the angular velocity vector and the linear velocity vector, i.e.:

[0053]

[0054] In the observation equation, This represents the magnetic field data observed by the sensor at time k. is the magnetic moment constant. This is the sensor attitude matrix. This represents the magnetization axis vectors of magnet 1 and magnet 2. The vector represents the position vector between the sensor and the two magnets, and the specific expressions are as follows: , , where i represents the magnet number. , , , , , is a constant representing the spatial position of the two rotating permanent magnets in the coordinate system. , , The ambient magnetic field noise is Gaussian white noise with a mean of 0, and they are independent of each other.

[0055] Among them, the optimal estimate of the target position is obtained through lossless Kalman filtering iteration, which can be designed to include:

[0056] The theoretical initial value is obtained through confidence region optimization, and the theoretical initial value includes the initial state mean vector and the initial state covariance matrix.

[0057] Generate Sigma points based on the mean and covariance of the current state, and calculate the mean weight and covariance weight corresponding to each Sigma point;

[0058] Substitute all generated Sigma points into the state transition function to generate new Sigma points after prediction, and obtain the predicted state and the predicted state covariance matrix based on the new Sigma points;

[0059] The new Sigma point is passed through the observation equation to obtain the predicted observation value and the predicted observation value covariance matrix. If the vector dot product between the transpose of the current predicted observation value and the previous predicted observation value is less than 0, the current predicted observation value is flipped and updated.

[0060] The Kalman gain is obtained based on the predicted state, the predicted observations, and their covariance matrix. The state and covariance are then updated based on the Kalman gain to obtain the optimal estimate for the current time step based on the updated state and covariance.

[0061] Calculate the state vector and its covariance matrix. , Then, based on the basic principle of UT transformation, a set of Sigma points is generated to approximate the probability distribution of the nonlinear system, and the weights of the mean and covariance are calculated.

[0062] After generating Sigma points through unscented transformation, they are predicted and propagated using a state transition function to obtain new Sigma points. Then, the predicted state and predicted covariance tensor are calculated based on the newly generated Sigma points.

[0063]

[0064]

[0065]

[0066] In the formula, , These are the weights of the mean and covariance, respectively. Let be the process noise covariance matrix.

[0067] First, the predicted Sigma points are transformed into the observation space using the observation model to obtain the predicted observation values:

[0068]

[0069] Next, the predicted observations are calculated. And calculate the covariance matrix based on the results. Where, It is the observed covariance. R is the cross-covariance, and R is the observation state covariance matrix.

[0070]

[0071]

[0072]

[0073] Calculate Kalman gain and innovation sequence The state and covariance are updated based on the Kalman gain calculation results.

[0074]

[0075]

[0076]

[0077]

[0078] in, It is the actual magnetic field measurement value with noise, that is State estimation covariance matrix The uncertainty of the state estimate is characterized, and its value directly determines the Kalman gain. The calculation results affect the state update process. It should be noted that in the initial stage of filtering, the estimation results are significantly affected by the initial values, and the contribution of noise to the error is relatively small; as the filtering iteration progresses, the influence of noise gradually becomes prominent, the value of the noise covariance matrix Q increases accordingly, and the positioning error exhibits a cumulative effect. At this time, the covariance matrix... This increase in the weight of the predicted value decreases, while the confidence level of the observed value increases, leading to a greater dependence of the filtering result on the observed value. In this situation, the correction magnitude of the state estimate may exceed the dynamic range of the actual system. Even if the estimated value converges to the neighborhood of the true value through the unscented Kalman filter algorithm, the estimation curve may still fluctuate significantly, resulting in a large estimation error. Especially in scenarios with strong system nonlinearity, this phenomenon may cause the covariance update mechanism to fail during the iteration process, leading to an unbounded increase in the positioning error and ultimately causing the filter to diverge.

[0079] Furthermore, due to the inherent construction characteristics of the algorithm, traditional unscented Kalman filtering is highly dependent on the prior probability distribution. This makes the algorithm perform excellently when tracking targets moving in uniform linear motion, but when the target undergoes a trajectory change (simultaneous change in velocity and direction), if the noise covariance matrix Q is too small, the estimated value will not converge accurately to the vicinity of the true value, thus causing filter divergence. Therefore, for complex trajectory motions, such as spatial motions with random changes in velocity and direction, and trajectories containing both straight lines and curves, the target motion state needs to be determined in real time based on the current filtering results during the state estimation process to detect whether there is a maneuvering trajectory change. Specifically, when the target undergoes trajectory maneuvering, the value of matrix Q is increased, while the value of matrix Q is decreased when the target is moving in uniform linear motion.

[0080] After estimating the target position, if the attitude angle is not corrected, a π-ambiguity problem will occur: due to the symmetry of the magnetic field, when the magnet's direction is reversed by 180°, the magnetic field strength measured by the sensor remains unchanged, leading to ambiguity in the attitude estimation. This is because in this algorithm, the magnetic field, as a vector field, has directional characteristics, but the magnetic field strength (magnitude) is a scalar. For a dipole magnetic field, when the magnetic moment vector... Invert, that is At that time, the magnetic field The direction will also be reversed. Due to the magnitude of the magnetic field strength The unchanged position estimation results in a generally correct position estimate, but the estimated attitude angle differs from the actual value by an integer multiple of π. To address the π ambiguity problem, this algorithm corrects the attitude estimation using the following method:

[0081]

[0082]

[0083] In the formula, This is the predicted value of the magnetic field at the current moment. It is the predicted value of the magnetic field at the previous moment, when the discriminant function... When the calculation result is less than 0, the calculation steps in formula (16) are executed. Because... It is a magnetic field prediction based on state estimation, and it is flipped. This is equivalent to applying a π correction to the attitude angle in the state estimation. Before the correction, we first analyze the theoretical magnetic field calculation formula:

[0084]

[0085] In the formula, It is the theoretical value of the magnetic field. These are the theoretical measurements after attitude transformation. Because... It is a magnetic field prediction based on state estimation, and it is flipped. This is equivalent to applying a π correction to the attitude angles in the state estimation, with the implicit relationship being:

[0086]

[0087] in, Let be the rotation matrix for rotating π around the z-axis, which is equivalent to increasing the attitude angle α by π. The above continuity constraint can be solved simply by vector dot product. This method not only achieves π fuzzy correction and avoids 180° discontinuous jumps in the estimated attitude angle, but also ensures the simplicity of the algorithm without the need for complex optimization solutions.

[0088] The UKF filtering result is significantly affected by the initial value, while in real-world scenarios, the target initial value often cannot be directly obtained through measurement. Therefore, to ensure the universality of the algorithm, this embodiment uses a randomly set initial value to accurately simulate the real-world scenario. Experimental statistics show that under random initial value conditions, UKF with only adaptive residuals will diverge on average once every 4-6 runs. To improve the robustness of the algorithm while ensuring universality, the initial value needs to be preprocessed under random initial value settings to reduce the risk of filter divergence. To this end, this embodiment introduces a confidence region optimization algorithm based on AR-UKF. Through iterative search with confidence region constraints, the initial state of magnetic sensing positioning is obtained. The confidence region is the confidence area around the current iteration point. The theoretical initial value is calculated by solving the confidence region optimization problem on the electromagnetic signal generated by the system. This method not only ensures the stability of the initial value, thereby enhancing the stability of the algorithm, but also better meets the verification requirements: regardless of how the initial value is set, the algorithm will call the calculation result of the optimization algorithm. Relevant experimental results are as follows: Figure 4 As shown in the figure. Statistical results show that after introducing confidence region optimization into the original algorithm, the stability of the algorithm is significantly improved, and the divergence statistics are as follows. Figure 4 As shown.

[0089] In the confidence region optimization process, the variable to be optimized is defined as a 9-dimensional vector. ,in The initial spatial coordinates of the sensor. The initial attitude angle, This is the environmental geomagnetic background constant.

[0090] Take the magnetic sensor at the first N sampling points (N is recommended to be 100, corresponding to...). The data window of s (much smaller than the motion time constant) is used to observe the triaxial magnetic field sequence. As the fitted data, construct the residual vector:

[0091]

[0092] in, It is the superposition of the theoretical magnetic fields of a dual rotating permanent magnet, calculated through the magnetic field observation equation.

[0093] A regularization of the geomagnetic term is introduced to suppress overfitting in underdetermined directions, and an objective function is constructed as follows:

[0094]

[0095] Where regularity coefficient The range of values ​​is This is used to balance the data fitting term with the geomagnetic prior term.

[0096] For the current iteration point ,remember For gradient, For the Gauss-Newton approximation of Hessian, where This is a Jacobian matrix. In... Centered on, with radius Constructing a quadratic proxy model within the confidence region:

[0097]

[0098] in Physical boundary constraints for variables:

[0099]

[0100] Subproblem (B1) is solved using the reflective Newton method to obtain candidate step sizes. The ratio of the actual descent to the model-predicted descent is then calculated.

[0101]

[0102] Based on this, the confidence region radius is dynamically adjusted. Whether or not to accept step size :

[0103]

[0104] Among them, the acceptance threshold .

[0105] The iteration terminates if any of the following conditions are met:

[0106]

[0107] Take after termination The UKF initial state mean is constructed from its first 6 components:

[0108]

[0109] That is, the position and attitude are based on the optimized results, while the velocity and angular velocity are initially zero. Initial covariance. Depend on The inverse state component remapping is obtained, and then the velocity / angular velocity sub-blocks are supplemented respectively. As prior variance:

[0110]

[0111] This process significantly reduces the UKF's sensitivity to random initial values, resulting in a significant decrease in the filter divergence rate.

[0112] The process of obtaining the Kalman gain based on the predicted state, the predicted observations, and their covariance matrix may also include:

[0113] The residual at the current time step is obtained based on the predicted state and predicted observations, and the residual at the current time step is stored in the sliding window buffer.

[0114] During the filtering process, when the sliding window buffer is full, the probability value of each residual component in the current sliding window buffer being greater than 0 is determined, and the offset of the probability value is obtained using a statistical function. When the offset of all residual components in the sliding window buffer does not exceed the threshold, the current target position is determined to be in steady-state motion, and velocity and angular velocity noise is reduced based on the constraint matrix. If the offset of the residual components in the sliding window buffer exceeds the threshold, the current target position is determined to be in unsteady-state motion, and the noise covariance matrix is ​​linearly incremented based on the noise increment matrix to increase the noise of the velocity and angular velocity process, and the state equation is adjusted based on the current target motion state.

[0115] A decision threshold function (i.e., a residual discriminant function) is established in the filtering algorithm to accurately determine whether the target has maneuvered. After obtaining the estimation result and actual measurement value at the current time step, the residual is calculated and stored in the sliding window buffer, and then the probability distribution of the residual is calculated:

[0116]

[0117]

[0118]

[0119] In the formula, Let j be the residual vector, and the subscript j indicates the j-th component of the vector. It is a buffer used to store residuals; This represents the sliding window threshold, used to determine whether the residual components exceed a preset range. The indicator function calculates the probability that each residual component is greater than 0. In the filtering algorithm, the residual values ​​generated at each time step are uniformly stored in a buffer, which is called the residual sequence to be detected. When the buffer is full, the mobile detection process is initiated: based on the residual sequence to be detected, the probability value of each component being greater than 0 is first calculated; then, based on this probability value, the degree of deviation from the expected value (usually 0.5) is calculated using a preset statistical function, and the result is defined as the offset. For residuals under a Gaussian distribution, the expected threshold of this statistical function is usually set to 0.5.

[0120]

[0121] If the offsets of all residual components within the buffer do not exceed the threshold ( If the target is considered to be in a non-maneuvering state, the model's estimation results are relatively accurate. To improve accuracy, it is necessary to reduce the noise in velocity and angular velocity to decrease the uncertainty of state prediction and make the filtering results closer to the true values. This requires improving the attenuation factor matrix. It is a 12-dimensional constant diagonal matrix, where c is a constant between 0 and 1:

[0122]

[0123]

[0124] To prevent the adjusted noise covariance matrix from becoming too small to track minute changes when reducing the value of matrix Q, the algorithm sets a constraint matrix Qmin to ensure the boundedness of the value of Q.

[0125]

[0126] If the offset corresponding to any component of any residual vector exceeds the threshold, ( If a singular residual is found, it is considered a singular residual. Once a singular residual appears, it is assumed that the currently tracked target has maneuvered, and trajectory correction needs to be performed. The specific steps are as follows:

[0127]

[0128]

[0129] By setting a noise increment matrix A and adding a linear increment to the original noise covariance matrix Q, the process noise of velocity and angular velocity is increased, thereby improving the UKF's tolerance to state changes and effectively avoiding tracking loss due to model bias. In the formula, d is a constant greater than 1, and its specific value depends on the experimental environment.

[0130] After completing the residual discrimination, the state equation is selected according to the current filtering discrimination result. If the current judgment result is the target steady-state motion (uniform linear motion), then the state equation adopts formula (37); otherwise, formula (38) is selected as the current state transition equation. The filtering effect before and after setting the adaptive residual is compared, for example. Figure 5 As shown.

[0131]

[0132]

[0133] In this proposed solution, the unscented transform of the traditional unscented Kalman filter algorithm is improved to avoid error accumulation caused by the introduction of regularization constants due to Cholskey decomposition failure, thereby further improving the filtering accuracy. Specifically, the state covariance can be scaled using scaling parameters, and singular value decomposition is performed on the scaled state covariance; the square roots of singular values ​​are extracted from the singular value decomposition results, and the offsets of Sigma points are generated based on the square roots of singular values, so that each Sigma point can be obtained according to the offsets.

[0134]

[0135]

[0136] In the formula, This is the scaling parameter, expressed as: The scatter distribution of Sigma points is obtained through parameters. control, It is usually set to a positive number less than 1; It is a scale parameter. In this embodiment, the noise follows a Gaussian distribution, therefore [i] represents the i-th column of the square root of the matrix; These are state distribution parameters; for the Gaussian noise in the current positioning system, This is the optimal value. It's worth noting that traditional unscented Kalman filtering typically uses the Cholesky decomposition algorithm when decomposing the state covariance matrix P. However, Cholesky decomposition fails when P is a non-positive definite matrix. A conventional solution to this problem is to introduce a small regularization parameter to the matrix before decomposition to improve its positive definiteness, but this operation causes the sampled values ​​in the state space to deviate from the true distribution, thus increasing the localization error. Therefore, in this embodiment, a sampling strategy based on singular value decomposition (SVD) is adopted in the Sigma point generation stage to fundamentally solve the problem of non-positive definite matrix decomposition failure. Specifically, the scaling covariance matrix is ​​first set... In the formula, the right singular matrix Equal to left singular matrix ; Calculate the square root of the singular values, And generate the offset vector of the Sigma point. At this point, the formula for generating the Sigma point becomes:

[0137]

[0138] Furthermore, based on the above method, this embodiment of the invention also provides a magnetic sensing positioning system based on confidence region optimization and adaptive residual correction, comprising: a modeling module and a solution module, wherein,

[0139] The modeling module is used to decompose the spatial magnetic field generated by the biorthogonal rotating permanent magnet at the transmitter into vector components in the coordinate system at the magnetic sensor at the receiver, and to establish a mathematical mapping between the magnetic field vector and the relative pose vector.

[0140] The solution module uses the target's position coordinates as the hidden state and the magnetic field components obtained from the space vector model as the observations. It uses the state equation to describe the target's motion law and the observation equation to describe the mathematical mapping between the magnetic field vector and the relative pose. Combining prior information and through lossless Kalman filtering iteration, it obtains the optimal estimate of the target's position. In the lossless Kalman filtering iteration, the initial theoretical value is obtained through confidence region optimization, and the process noise and state noise are dynamically adjusted according to the current target motion state to adaptively correct the relative pose estimate.

[0141] To verify the effectiveness of this solution, the following explanation is based on experimental data:

[0142] In this solution, by constructing a residual sequence and setting a threshold mechanism, the accurate identification of the target motion state and the online estimation of noise covariance are achieved. Even when faced with sudden changes in motion state, the solution can still maintain high estimation consistency and effectively suppress estimation bias caused by model mismatch or external interference.

[0143] Simulation verification, such as Figure 6 As shown in the figure, under the condition of SNR=10dB, 50 Monte Carlo experiments were conducted on traditional filtering methods (AF, EM, EKF, UKF) and the improved Kalman filter (ARUKF) after introducing adaptive residuals and dynamic state equation optimization. The optimal result was taken. As can be seen from the figure, introducing adaptive residuals can effectively reduce filtering error.

[0144] Unless otherwise specifically stated, the relative steps, numerical expressions, and values ​​of the components and steps described in these embodiments do not limit the scope of the invention.

[0145] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the systems disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the descriptions are relatively simple; relevant parts can be referred to the method section.

[0146] The units and method steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the components and steps of each example have been generally described in terms of functionality in the foregoing description. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations are not considered to be beyond the scope of this invention.

[0147] Those skilled in the art will understand that all or part of the steps in the above methods can be implemented by a program instructing related hardware, and the program can be stored in a computer-readable storage medium, such as a read-only memory, a disk, or an optical disk. Optionally, all or part of the steps in the above embodiments can also be implemented using one or more integrated circuits. Accordingly, each module / unit in the above embodiments can be implemented in hardware or as a software functional module. This invention is not limited to any particular combination of hardware and software.

[0148] Finally, it should be noted that the above-described embodiments are merely specific implementations of the present invention, used to illustrate the technical solutions of the present invention, and not to limit it. The scope of protection of the present invention is not limited thereto. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that any person skilled in the art can still modify or easily conceive of changes to the technical solutions described in the foregoing embodiments within the technical scope disclosed in the present invention, or make equivalent substitutions for some of the technical features; and these modifications, changes, or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be covered within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A magnetic sensing localization method based on confidence region optimization and adaptive residual correction, characterized in that, Include: The spatial magnetic field generated by the biorthogonal rotating permanent magnet at the transmitting end is decomposed into vector components in the coordinate system at the magnetic sensor at the receiving end, and a mathematical mapping between the magnetic field vector and the relative pose vector is established. The target position coordinates are taken as the hidden state, and the magnetic field components obtained based on the space vector model are taken as the observations. The motion law of the target is described by the state equation, and the mathematical mapping between the magnetic field vector and the relative pose is described by the observation equation. Combined with prior information and through lossless Kalman filtering iteration, the optimal estimate of the target position is obtained. In the lossless Kalman filtering iteration, the initial theoretical value is obtained by confidence region optimization, and the process noise and state noise are dynamically adjusted according to the current target motion state to adaptively correct the relative pose estimate.

2. The magnetic sensing localization method based on confidence region optimization and adaptive residual correction according to claim 1, characterized in that, Describing the motion law of the target object using state equations includes: The target motion path acquired by the magnetic sensor is discretized according to the time series and divided into several time step units; In the target motion trajectory modeling, the trajectory segments between adjacent time nodes are transitioned into circular arcs so that the centripetal acceleration can be obtained by the cross product of the angular velocity vector and the linear velocity vector.

3. The magnetic sensing localization method based on confidence region optimization and adaptive residual correction according to claim 1, characterized in that, The observation equation is expressed as: ,in, is the magnetic moment constant. Let the attitude angle corresponding to time k be , , The magnetic sensor attitude matrix is ​​represented. This represents the magnetization axis vector of the permanent magnet. This represents the position vector between the magnetic sensor and the permanent magnet, where i represents the permanent magnet number. , , This is environmental magnetic field noise.

4. The magnetic sensing localization method based on confidence region optimization and adaptive residual correction according to claim 1, characterized in that, Through lossless Kalman filtering iteration, the optimal estimate of the target position is obtained, including: The theoretical initial value is obtained through confidence region optimization, and the theoretical initial value includes the initial state mean vector and the initial state covariance matrix. Generate Sigma points based on the mean and covariance of the current state, and calculate the mean weight and covariance weight corresponding to each Sigma point; Substitute all generated Sigma points into the state transition function to generate new Sigma points after prediction, and obtain the predicted state and the predicted state covariance matrix based on the new Sigma points; The new Sigma point is passed through the observation equation to obtain the predicted observation value and the predicted observation value covariance matrix. If the vector dot product between the transpose of the current predicted observation value and the previous predicted observation value is less than 0, the current predicted observation value is flipped and updated. The Kalman gain is obtained based on the predicted state, the predicted observations, and their covariance matrix. The state and covariance are then updated based on the Kalman gain to obtain the optimal estimate for the current time step based on the updated state and covariance.

5. The magnetic sensing localization method based on confidence region optimization and adaptive residual correction according to claim 1 or 4, characterized in that, The theoretical initial values ​​are obtained through confidence region optimization, including: The initial state of magnetic sensing positioning is obtained through iterative search with confidence region constraints, where the confidence region is the confidence area around the current iteration point.

6. The magnetic sensing localization method based on confidence region optimization and adaptive residual correction according to claim 4, characterized in that, The Kalman gain is obtained based on the predicted state, predicted observations, and their covariance matrix, and also includes: The residual at the current time step is obtained based on the predicted state and predicted observations, and the residual at the current time step is stored in the sliding window buffer. During the filtering process, when the sliding window buffer is full, the probability value of each residual component in the current sliding window buffer being greater than 0 is determined, and the offset of the probability value is obtained using a statistical function. When the offset of all residual components in the sliding window buffer does not exceed the threshold, the current target position is determined to be in steady-state motion, and velocity and angular velocity noise is reduced based on the constraint matrix. If the offset of the residual components in the sliding window buffer exceeds the threshold, the current target position is determined to be in unsteady-state motion, and the noise covariance matrix is ​​linearly incremented based on the noise increment matrix to increase the noise of the velocity and angular velocity process, and the state equation is adjusted based on the current target motion state.

7. The magnetic sensing localization method based on confidence region optimization and adaptive residual correction according to claim 4, characterized in that, Generate Sigma points, including: The state covariance is scaled using scaling parameters, and singular value decomposition is performed on the scaled state covariance. The square roots of singular values ​​are extracted from the singular value decomposition results, and the offsets of Sigma points are generated based on the square roots of singular values, so that each Sigma point can be obtained according to the offsets.

8. A magnetic sensing positioning system based on confidence region optimization and adaptive residual correction, characterized in that, It includes: a modeling module and a solution module, wherein, The modeling module is used to decompose the spatial magnetic field generated by the biorthogonal rotating permanent magnet at the transmitter into vector components in the coordinate system at the magnetic sensor at the receiver, and to establish a mathematical mapping between the magnetic field vector and the relative pose vector. The solution module uses the target's position coordinates as the hidden state and the magnetic field components obtained from the space vector model as the observations. It uses the state equation to describe the target's motion law and the observation equation to describe the mathematical mapping between the magnetic field vector and the relative pose. Combining prior information and through lossless Kalman filtering iteration, it obtains the optimal estimate of the target's position. In the lossless Kalman filtering iteration, the initial theoretical value is obtained through confidence region optimization, and the process noise and state noise are dynamically adjusted according to the current target motion state to adaptively correct the relative pose estimate.

9. An electronic device, characterized in that, include: At least one processor, and a memory coupled to said at least one processor; The memory stores a computer program that can be executed by the at least one processor to implement the method as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed, enables the implementation of the method as described in any one of claims 1 to 7.

Images