Real-time positioning method and device for permanent magnet under rapid motion and readable medium

Abstract

The application discloses a kind of real-time positioning method, device and readable medium for permanent magnet under fast motion, it is related to permanent magnet positioning field using random walk model as the motion state model of permanent magnet, and the pose of permanent magnet and the covariance matrix related to state vector are predicted in combination with EKF algorithm, and the predicted pose and covariance matrix are updated by Kalman gain, the final pose of permanent magnet is obtained after iteration operation, can effectively improve the precision and accuracy of positioning, the relationship between the moving speed of permanent magnet and positioning precision is established, so as to know that the tracking performance of the application in fast moving target has been greatly improved, so as to greatly expand the application scene of permanent magnet positioning, solve the problem that permanent magnet is difficult to be positioned under fast motion, and positioning precision is low.

Description

Technical Field

[0001] This invention relates to the field of permanent magnet positioning, and more specifically to a method, apparatus, and readable medium for real-time positioning of permanent magnets under rapid motion. Background Technology

[0002] Permanent magnet dynamic positioning technology is the most important technology for positioning in medical surgeries, such as dynamic tracking of microsurgical robots, tracking of lung tumor metastases, and scalpel positioning. Permanent magnet dynamic positioning technology can accurately track targets even under rapid movement. For example, when using permanent magnets to track lung tumors, if the patient is nervous, experiencing significant emotional fluctuations, and breathing rapidly, the lung tumor will also move rapidly in sync with the increased breathing. If this situation occurs during treatment, and the real-time positioning of the tumor is inaccurate, radiation therapy can inevitably cause damage to the patient's normal tissues and organs.

[0003] Regarding research on permanent magnet positioning, Su Shijian et al. used a PSO-LM algorithm to locate permanent magnets and studied the optimal tracking distance, achieving a positioning error of approximately 0.7 mm. However, this data was obtained when the permanent magnet was stationary. Hu Chao et al. proposed a tracking algorithm for six-dimensional attitude positioning of permanent magnets, but the permanent magnets were not moving rapidly in their experiments. Song Shuang et al. proposed a positioning system for multiple permanent magnet targets, and Yang Wan'an et al. proposed a positioning algorithm combining PSO and cloning algorithms, also locating multiple targets with a multi-target accuracy of 3.7 mm. Again, none of these experiments involved rapid movement. Therefore, while research on permanent magnet positioning has made significant progress, none of these studies have addressed the scenario of rapid permanent magnet movement. Summary of the Invention

[0004] In view of the aforementioned problems, the purpose of the embodiments of this application is to provide a method, apparatus, and readable medium for real-time positioning of permanent magnets under rapid movement, thereby solving the technical problems mentioned in the background section.

[0005] In a first aspect, the present invention provides a method for real-time positioning of a permanent magnet under rapid motion, comprising the following steps:

[0006] S1, acquire the magnetic field strength data of the permanent magnet collected by the magnetic sensor array and the initial pose of the permanent magnet, and select the initial covariance matrix related to the state vector of the permanent magnet;

[0007] S2, based on the magnetic field strength data of the permanent magnet, a state equation is constructed by combining a random walk model with the EKF algorithm. Based on the initial pose and initial covariance matrix of the permanent magnet, the predicted pose and predicted covariance matrix are predicted by the state equation.

[0008] S3. Obtain the measurement model based on the magnetic field strength data and determine the Jacobian matrix of the measurement model. Establish the pose update equation and covariance matrix update equation based on the Jacobian matrix of the measurement model. Input the predicted pose and predicted covariance matrix into the pose update equation and covariance matrix update equation to obtain the updated pose and updated covariance matrix.

[0009] S4. Using the updated pose and updated covariance matrix as the initial pose and initial covariance matrix, repeat steps S1-S3 for iterative updates to obtain the final pose of the permanent magnet.

[0010] Preferably, the magnetic sensor array consists of several triaxial magnetic sensors, and the magnetic field strength data Z of the permanent magnet collected by the magnetic sensor array is determined according to the magnetic dipole model. k .

[0011] Preferably, the random walk model in step S2 can be expressed by the following equation:

[0012] x (k) =x (k-1) ±randn;

[0013] Where, x (k) This represents the five-dimensional pose of the permanent magnet at this moment. x (k-1) This represents the pose of the permanent magnet at the previous moment, and randn represents an arbitrary value in five dimensions.

[0014] Preferably, the state equation in step S2 is expressed by the following equation:

[0015] x k =f(x) k-1 u k )+w k ;

[0016] z k =h(x k )+v k ;

[0017] w k = sqrt(Q)*randn;

[0018] V k = sqrt(R) * randn;

[0019] Where, f(x) k-1 u k ) represents the location function, u k It is the control vector, h(x) k ) represents the measurement model, v k This is represented as the measurement noise of the magnetic sensor array; wk This represents the process noise during the motion of the permanent magnet, specifically white noise with a Gaussian distribution related to Q. Q is the covariance matrix related to the motion of the permanent magnet, calculated using the following formula: Q = diag(100, 100, 100, 1, 1) × 10 A Where A is a parameter related to the velocity of the permanent magnet, calculated as the sum of the changes in the triaxial output values ​​of all triaxial magnetic sensors in the measuring device. Before calculation, sliding window filtering and function fitting are used to obtain the formula for calculating A: in, Let f represent the output value of the l-th axis of the k-th time-th triaxial magnetic sensor, where f represents the X, Y, and Z axes; R is the measurement noise matrix of the triaxial magnetic sensor, and Z is the magnetic field strength data of the acquired permanent magnet. k As the observation matrix.

[0020] Preferably, step S2 involves predicting the predicted pose and predicted covariance matrix based on the initial pose and initial covariance matrix of the permanent magnet using state equations. Specifically, this includes:

[0021] Let x (k-1|k-1) For the initial pose, P (k-1|k-1) Given the initial covariance matrix, the predicted pose x is obtained using the following formula. (k|k-1) and the predicted covariance matrix P (k|k-1) :

[0022] x (k|k-1) =x (k-1|k-1) +w k ;

[0023] P (k|k-1) =P (k-1|k-1) +Q.

[0024] Preferably, step S3 establishes the pose update equation and covariance matrix update equation based on the Jacobian matrix of the measurement model, specifically including:

[0025] The Kalman gain K is calculated using the following formula. k :

[0026]

[0027] Among them, H k The Jacobian matrix of the measurement model;

[0028] The pose update equation and the covariance matrix update equation are respectively represented as follows:

[0029] x (k|k) =x (k|k-1) +K k [z k -h(xk )];

[0030] P (k|k） =[IK k H k ]P (k|k-1) ;

[0031] Where I represents the identity matrix, x (k|k) To update pose, P (k|k) To update the covariance matrix.

[0032] Preferably, the magnetic sensor array contains 16 triaxial magnetic sensors, and the magnetic sensor array collects magnetic field strength data of the permanent magnet.

[0033] z k = (B 1x B 1y B 1z B 2x B 16y B 16z ) T For any point M in the space surrounding the permanent magnet, the magnetic flux density at point M is calculated using the magnetic dipole model formula:

[0034]

[0035]

[0036] Among them, B r Let B be the magnetic field strength at the location of the r-th three-dimensional magnetic sensor, where r = 1, ..., 16; i, j, and k represent unit vectors along the x, y, and z axes, respectively. rx B ry and B rz M represents the magnetic flux density in the corresponding direction; r R represents the distance vector from point M in space to the center of the permanent magnet. r Let M be the distance from point M to the center of the permanent magnet; where (a, b, c) is the position of the center of the permanent magnet, and (x... r y r , z r () represents the three-dimensional position of point M in space;

[0037] H0 represents the unit vector of the permanent magnet in the direction of the magnetic dipole moment, denoted as H0 = (m, n, p). T (m, n, p) is expressed by the following formula, where θ and These represent the two angles between the center of the permanent magnet and the Cartesian coordinate system:

[0038]

[0039] For a single permanent magnet target, the magnetic flux density components along the three axes at the location of the r-th triaxial magnetic sensor are as follows:

[0040]

[0041] Secondly, the present invention provides a real-time positioning device for a permanent magnet under rapid movement, comprising:

[0042] The initial state module is configured to acquire the magnetic field strength data of the permanent magnet collected by the magnetic sensor array and the initial pose of the permanent magnet, and select the initial covariance matrix related to the state vector of the permanent magnet.

[0043] The prediction module is configured to construct a state equation based on the magnetic field strength data of the permanent magnet using a random walk model combined with the EKF algorithm, and to predict the predicted pose and predicted covariance matrix based on the initial pose and initial covariance matrix of the permanent magnet using the state equation.

[0044] The update module is configured to obtain the measurement model based on the magnetic field strength data, determine the Jacobian matrix of the measurement model, establish the pose update equation and covariance matrix update equation based on the Jacobian matrix of the measurement model, input the predicted pose and predicted covariance matrix into the pose update equation and covariance matrix update equation to obtain the updated pose and updated covariance matrix.

[0045] The iteration module is configured to use the updated pose and updated covariance matrix as the initial pose and initial covariance matrix, and repeatedly execute the initial state module to the update module for iterative updates to obtain the final pose of the permanent magnet.

[0046] Thirdly, the present invention provides an electronic device including one or more processors; and a storage device for storing one or more programs, wherein when the one or more programs are executed by the one or more processors, the one or more processors implement the method as described in any implementation of the first aspect.

[0047] Fourthly, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method as described in any of the implementations of the first aspect.

[0048] Compared with the prior art, the present invention has the following beneficial effects:

[0049] (1) The real-time positioning method for permanent magnets under rapid motion proposed in this invention adopts a random walk model as the motion state model of the permanent magnet, and combines the EKF algorithm to predict the pose of the permanent magnet and the covariance matrix related to the state vector. The predicted pose and covariance matrix are updated by Kalman gain. The final pose of the permanent magnet is obtained through iterative calculation, which can effectively improve the positioning accuracy and precision.

[0050] (2) The real-time positioning method for permanent magnets under rapid motion proposed in this invention can be applied when the object moves at a relatively high speed and can guarantee a certain positioning accuracy.

[0051] (3) The real-time positioning method for permanent magnets under rapid movement proposed in this invention establishes the relationship between the moving speed of the permanent magnet and the positioning accuracy. It is known that the tracking performance of the present invention for rapidly moving targets has been greatly improved, thereby greatly expanding the application scenarios of permanent magnet positioning. Attached Figure Description

[0052] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0053] Figure 1 This is an exemplary device architecture diagram in which an embodiment of this application can be applied;

[0054] Figure 2 This is a flowchart illustrating a real-time positioning method for a permanent magnet under rapid motion, as an embodiment of this application.

[0055] Figure 3 This is a schematic diagram of a magnetic dipole model for a real-time positioning method for a permanent magnet under rapid motion, as an embodiment of this application.

[0056] Figure 4 This is a schematic diagram of a motion model for a real-time positioning method for permanent magnets under rapid motion, as an embodiment of this application.

[0057] Figure 5 This is a schematic diagram of a real-time positioning device for a permanent magnet under rapid movement, according to an embodiment of this application.

[0058] Figure 6 This is a schematic diagram of the structure of a computer device suitable for implementing the electronic device of the present application. Detailed Implementation

[0059] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this invention, and not all of them. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this invention.

[0060] Figure 1 An exemplary device architecture 100 is shown, which can be applied to the real-time positioning method or device for permanent magnets under rapid motion according to the embodiments of this application.

[0061] like Figure 1 As shown, the device architecture 100 may include terminal devices 101, 102, and 103, a network 104, and a server 105. The network 104 serves as a medium for providing communication links between the terminal devices 101, 102, and 103 and the server 105. The network 104 may include various connection types, such as wired or wireless communication links, or fiber optic cables, etc.

[0062] Users can use terminal devices 101, 102, and 103 to interact with server 105 via network 104 to receive or send messages, etc. Various applications, such as data processing applications and file processing applications, can be installed on terminal devices 101, 102, and 103.

[0063] Terminal devices 101, 102, and 103 can be either hardware or software. When terminal devices 101, 102, and 103 are hardware, they can be various electronic devices, including but not limited to smartphones, tablets, laptops, and desktop computers. When terminal devices 101, 102, and 103 are software, they can be installed in the electronic devices listed above. They can be implemented as multiple software programs or software modules (e.g., software programs or software modules used to provide distributed services) or as a single software program or software module. No specific limitations are imposed here.

[0064] Server 105 can be a server that provides various services, such as a background data processing server that processes files or data uploaded by terminal devices 101, 102, and 103. The background data processing server can process the acquired files or data and generate processing results.

[0065] It should be noted that the real-time positioning method for permanent magnets under rapid movement provided in this application embodiment can be executed by server 105 or by terminal devices 101, 102, and 103. Correspondingly, the real-time positioning device for permanent magnets under rapid movement can be set in server 105 or in terminal devices 101, 102, and 103.

[0066] It should be understood that Figure 1 The number of terminal devices, networks, and servers shown is merely illustrative. Any number of terminal devices, networks, and servers can be included depending on implementation needs. If the data being processed does not need to be retrieved remotely, the above architecture may not include a network, requiring only servers or terminal devices.

[0067] Figure 2 An embodiment of this application provides a method for real-time positioning of a permanent magnet under rapid motion, comprising the following steps:

[0068] S1, acquire the magnetic field strength data of the permanent magnet collected by the magnetic sensor array and the initial pose of the permanent magnet, and select the initial covariance matrix related to the state vector of the permanent magnet.

[0069] In a specific embodiment, the permanent magnet generates a specific magnetic field around its surrounding space. The magnetic field strength at any point in space can be obtained using the magnetic dipole model formula. Embodiments of this application employ a magnetic sensor array to acquire the magnetic field strength at a fixed location in space. The magnetic dipole model is typically used to describe the distribution of magnetic field strength in a magnetic field. For any point M in space, if the distance from point M to the permanent magnet is relatively large compared to the size of the permanent magnet, then this permanent magnet can be considered a magnetic dipole. Figure 3 The diagram shows a magnetic dipole model. The magnetic sensor array consists of several triaxial magnetic sensors. Based on the magnetic dipole model, the magnetic field strength data Z collected by the magnetic sensor array from the permanent magnet is determined. k Specifically, the magnetic sensor array contains 16 triaxial magnetic sensors, and the magnetic sensor array collects magnetic field strength data from the permanent magnet.

[0070] z k = (B 1x B 1y B 1z B 2x B 16y B 16z ) T .

[0071] Specifically, for any point M in the space surrounding the permanent magnet, the magnetic flux density at point M is calculated using the magnetic dipole model formula:

[0072]

[0073]

[0074] Among them, B rLet B be the magnetic field strength at the location of the r-th three-dimensional magnetic sensor, where r = 1, ..., 16; i, j, and k represent unit vectors along the x, y, and z axes, respectively. rx B ry and B rz M represents the magnetic flux density in the corresponding direction; r R represents the distance vector from point M in space to the center of the permanent magnet. r Let M be the distance from point M to the center of the permanent magnet; where (a, b, c) is the location of the center of the permanent magnet, which is also the location to be located, and (x... r y r , z r ) represents the three-dimensional position of point M in space, which is also the position of the triaxial magnetic sensor relative to the coordinate system established for the magnetic sensor array in the actual experiment.

[0075] H0 represents the unit vector of the permanent magnet in the direction of the magnetic dipole moment, denoted as H0 = (m, n, p). T (m, n, p) is expressed by the following formula, where θ and These represent the two angles between the center of the permanent magnet and the Cartesian coordinate system:

[0076]

[0077] Given the magnitude of the magnetic field strength, the location of the measurement point, and the value B related to the inherent properties of the permanent magnet. T Then, the position of the permanent magnet can be deduced and located in reverse. Here, due to the inherent properties of the permanent magnet, the B on the three axial components can be determined. T They are considered equal. For a single permanent magnet target, the components of the magnetic flux density at the location of the r-th triaxial magnetic sensor along the three axes are as follows:

[0078]

[0079] S2, based on the magnetic field strength data of the permanent magnet, a state equation is constructed by combining a random walk model with the EKF algorithm. Based on the initial pose and initial covariance matrix of the permanent magnet, the predicted pose and predicted covariance matrix are predicted by the state equation.

[0080] Specifically, the random walk model is a mathematical statistical model that represents a series of trajectories where the position changes randomly each time, indicating an irregular motion pattern. Examples of random walk models are numerous in everyday life, such as the random movement of gas molecules in the air or the random diffusion of a drop of water on a tissue. The random walk model can effectively describe the motion trajectory of objects whose changing trends cannot be predicted in advance. The embodiments of this application employ the random walk model, eliminating the need to predict the object's motion trend beforehand, thus significantly expanding the application scope of permanent magnet positioning.

[0081] In a specific embodiment, the random walk model in step S2 can be represented by the following formula:

[0082] x (k) =x (k-1) ±randn;

[0083] Where, x (k) This represents the five-dimensional pose of the permanent magnet at this moment. x (k-1) This represents the pose of the permanent magnet at the previous moment, and randn represents an arbitrary value in five dimensions; it generates a five-dimensional random number in each calculation to reflect the randomness of the random walk model.

[0084] Specifically, the EKF algorithm solves some nonlinear problems through a locally linear approach, mainly by predicting and updating the state based on the state equation. First, the state equation needs to be determined, and the magnetic field strength data z of the permanent magnet is collected using a magnetic sensor array. k = (B 1x B 1y B 1z B 2x ,…,B 16y B 16z ) T As an observation matrix, it is used for state prediction.

[0085] Specifically, the state equation in step S2 is expressed by the following formula:

[0086] x k =f(x) k-1 u k )+w k ;

[0087] z k =h(x k )+vk;

[0088] w k = sqrt(Q)*randn;

[0089] V k = sqrt(R) * randn;

[0090] Where, f(x) k-1 u k ) represents the location function, u k It is the control vector, h(x) k ) represents the measurement model, v k The measurement noise of the magnetic sensor array is represented by w. kThe noise represents the process noise during the motion of the permanent magnet, and is Gaussian-distributed white noise related to Q, where Q is the covariance matrix related to the motion of the permanent magnet, calculated using the following formula: Q = diag(100,100,100,1,1) × 10 A Where A is a parameter related to the speed of the permanent magnet; the greater the speed of the permanent magnet, the larger A is. A can be calculated by the sum of the changes in the triaxial output values ​​of all triaxial magnetic sensors in the measuring device. Before calculation, sliding window filtering and function fitting are used to obtain the formula for calculating A:

[0091] in Let represent the output value of the f-th axis of the l-th triaxial magnetic sensor at time k, where f represents the X, Y, and Z axes. R is the measurement noise matrix of the triaxial magnetic sensor, which is generally only related to the performance of the selected sensor and is a fixed parameter matrix. To measure the noise parameters of the magnetic sensor array, i.e., R for each axis of each triaxial magnetic sensor, statistical analysis of the triaxial magnetic sensor noise is required. First, the triaxial magnetic sensor data is collected in an experimental environment, requiring the collection of a large amount of magnetic sensor data. Second, a least-squares fitting method is used to fit these magnetic sensor data to obtain the noise matrix of the magnetic sensor array. The collected magnetic field strength data z of the permanent magnet is then used... k As the observation matrix, the measurement model h(x) can be calculated. k Further calculations yielded the measurement model h(x). k The Jacobian matrix H) k .

[0092] In a specific embodiment, step S2, which involves predicting the predicted pose and predicted covariance matrix based on the initial pose and initial covariance matrix of the permanent magnet using state equations, specifically includes:

[0093] Let x (k-1|k-1) For the initial pose, P (k-1|k-1) Given the initial covariance matrix, the predicted pose x is obtained using the following formula. (k|k-1) and the predicted covariance matrix P (k|k-1) :

[0094] x (k|k-1) =x (k-1|k-1) +w k ;

[0095] P (k|k-1) =P (k-1|k-1) +Q;

[0096] in,

[0097] Specifically, an initial pose x is required. (0|0) and the initial covariance matrix P (0|0)For x (0|0) The initial values ​​can be arbitrarily given, or the actual position of the magnet can be directly input as the initial pose. Similarly, the initial covariance matrix P (0|0) The initial covariance matrix P can be chosen arbitrarily, as numerous iterations will be performed later. (0|0) The impact on the final performance is minimal. Then, the predicted pose x is output based on the random walk motion model. (1|0) and the predicted covariance matrix P (1|0) .

[0098] S3. Obtain the measurement model based on the magnetic field strength data and determine the Jacobian matrix of the measurement model. Establish the pose update equation and covariance matrix update equation based on the Jacobian matrix of the measurement model. Input the predicted pose and predicted covariance matrix into the pose update equation and covariance matrix update equation to obtain the updated pose and updated covariance matrix.

[0099] In a specific embodiment, step S3, which establishes the pose update equation and covariance matrix update equation based on the Jacobian matrix of the measurement model, specifically includes:

[0100] The Kalman gain K is calculated using the following formula. k :

[0101]

[0102] Among them, H k The Jacobian matrix of the measurement model;

[0103] The pose update equation and the covariance matrix update equation are respectively represented as follows:

[0104] x (k|k) =x (k|k-1) +K k [z k -h(x k )];

[0105] P (k|k) =[IK k H k ]P (k|k-1) ;

[0106] Where I represents the identity matrix, x (k|k) To update pose, P (k|k) To update the covariance matrix.

[0107] For details, please refer to Figure 4 When the permanent magnet moves, changes in the spatial magnetic field can be acquired from the magnetic sensor array, and the output data of the magnetic sensor array is the observation matrix. Simultaneously, the attitude of the permanent magnet is estimated using its motion state equation. Finally, the Kalman gain K is calculated and applied to the magnetic field.k The permanent magnet attitude is updated using other parameters, including the observation matrix z. k Measurement noise of matrix and magnetic sensor arrays to obtain measurement model h(x) k Further based on the measurement model h(x) k Calculate its Jacobian matrix H k Finally, the Jacobian matrix H of the measured model is... k Predicting the covariance matrix P (k|k-1) The Kalman gain K is calculated from the measurement noise matrix R of the triaxial magnetic sensor. k .

[0108] Specifically, the predicted pose x (1|0) and the predicted covariance matrix P (1|0) The updated pose x is calculated by inputting the pose update equation and the covariance matrix update equation. (1|1) and update the covariance matrix P (1|1) Therefore, the prediction and update in the first iteration are completed.

[0109] S4. Using the updated pose and updated covariance matrix as the initial pose and initial covariance matrix, repeat steps S1-S3 for iterative updates to obtain the final pose of the permanent magnet.

[0110] Specifically, after the first iteration is completed, the second iteration begins. In the second iteration, the pose x will be updated. (1|1) As the initial pose for the next iteration, the covariance matrix P is also updated. (1|1) This also serves as the initial covariance matrix for the second iteration. Subsequent steps are the same as the first iteration: first, calculate the predicted pose x based on the random walk model. (2|0) and the predicted covariance matrix P (2|0) Then, the EKF algorithm is used to calculate and update the pose and the updated covariance matrix P related to the state vector. The above prediction and update steps are repeated to complete the permanent magnet positioning and tracking.

[0111] The positioning accuracy error for each point in the data sample is calculated using the following formula, where (a0, b0, c0, m0, n0, p0) are the true pose coordinates of the permanent magnet, and (a1, b1, c1, m1, n1, p1) are the pose coordinates calculated by the method proposed in the embodiments of this application. Since the quaternion-based orientation error can be converted to Euler angles, the position error E... e and direction error E o The attitude error can be defined as follows:

[0112]

[0113] Among them, E oand E o1 The units are radians and degrees, respectively. The root mean square (RMS) of the attitude error can be described by the following formula, where RMS Ee and RMS Eo These are the RMS values ​​of position error and orientation error, respectively.

[0114]

[0115] In the experiment, four different moving speeds were used to verify the effectiveness of the method proposed in the embodiments of this application. The moving speeds were 1 cm / s, 2 cm / s, 3 cm / s, and 4 cm / s. A polynomial fitting method was used to fit the relationship between speed and accuracy, and the relationship between moving speed and positioning accuracy was obtained, as shown in the following formula:

[0116] f(x) = p1x 4 +p2x 3 +p3x 2 +p4x 1 +p5;

[0117] Where f(x) represents the positioning accuracy in mm, x represents the speed of the magnet in mm / s, and p1 to p5 are the coefficients of the fitting equation: p1 = -4.708e-06, p2 = 0.0004592, p3 = -0.01453, p4 = 0.1901, p5 = 0.

[0118] When the target object's moving speed reaches 40 mm / s, experimental results show that the average positioning error, orientation error, and algorithm delay are 2.85 ± 0.41 mm, 1.302 ± 0.023°, and 0.485 ± 0.064 ms, respectively. The method proposed in this application can perform permanent magnet positioning and tracking even when the object is moving at high speeds, ensuring a certain level of positioning accuracy. Compared with existing methods, the method proposed in this application significantly improves the tracking performance of fast-moving targets, thereby greatly expanding the application scenarios of permanent magnet positioning. The high positioning accuracy in the embodiments of this application stems from the establishment of the EKF algorithm and the random walk model, while the low latency also originates from the EKF algorithm.

[0119] Further reference Figure 5 As an implementation of the methods shown in the above figures, this application provides an embodiment of a real-time positioning device for permanent magnets under rapid movement. This device embodiment is similar to... Figure 2 Corresponding to the method embodiments shown, this device can be specifically applied to various electronic devices.

[0120] This application provides a real-time positioning device for a permanent magnet under rapid movement, comprising:

[0121] Initial state module 1 is configured to acquire the magnetic field strength data of the permanent magnet collected by the magnetic sensor array and the initial pose of the permanent magnet, and select an initial covariance matrix related to the state vector of the permanent magnet;

[0122] Prediction module 2 is configured to construct a state equation based on the magnetic field strength data of the permanent magnet through a random walk model combined with the EKF algorithm, and to predict the predicted pose and predicted covariance matrix based on the initial pose and initial covariance matrix of the permanent magnet through the state equation.

[0123] The update module 3 is configured to obtain the measurement model based on the magnetic field strength data, determine the Jacobian matrix of the measurement model, establish the pose update equation and covariance matrix update equation based on the Jacobian matrix of the measurement model, input the predicted pose and predicted covariance matrix into the pose update equation and covariance matrix update equation to obtain the updated pose and updated covariance matrix.

[0124] Iteration module 4 is configured to use the updated pose and updated covariance matrix as the initial pose and initial covariance matrix, and repeatedly execute the initial state module 1 to update module 3 for iterative updates to obtain the final pose of the permanent magnet.

[0125] The following is for reference. Figure 6 It illustrates an electronic device suitable for implementing embodiments of this application (e.g., Figure 1 The diagram shows the structure of a computer device 600 (a server or terminal device). Figure 6 The electronic device shown is merely an example and should not impose any limitation on the functionality and scope of use of the embodiments of this application.

[0126] like Figure 6 As shown, the computer device 600 includes a central processing unit (CPU) 601 and a graphics processing unit (GPU) 602, which can perform various appropriate actions and processes according to programs stored in read-only memory (ROM) 603 or programs loaded from storage section 609 into random access memory (RAM) 604. The RAM 604 also stores various programs and data required for the operation of the device 600. The CPU 601, GPU 602, ROM 603, and RAM 604 are interconnected via a bus 605. An input / output (I / O) interface 606 is also connected to the bus 605.

[0127] The following components are connected to I / O interface 606: an input section 607 including a keyboard, mouse, etc.; an output section 608 including an LCD, speakers, etc.; a storage section 609 including a hard disk, etc.; and a communication section 610 including a network interface card, such as a LAN card or modem. The communication section 610 performs communication processing via a network such as the Internet. A drive 611 may also be connected to I / O interface 606 as needed. A removable medium 612, such as a hard disk, optical disk, magneto-optical disk, semiconductor memory, etc., is installed on drive 611 as needed so that computer programs read from it can be installed into storage section 609 as needed.

[0128] In particular, according to embodiments of this disclosure, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments of this disclosure include a computer program product comprising a computer program carried on a computer-readable medium, the computer program containing program code for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via communication section 610, and / or installed from removable medium 612. When the computer program is executed by central processing unit (CPU) 601 and graphics processing unit (GPU) 602, the functions defined in the methods of this application are performed.

[0129] It should be noted that the computer-readable medium described in this application can be a computer-readable signal medium, a computer-readable medium, or any combination thereof. A computer-readable medium can be, for example,—but not limited to—an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor device, or any combination thereof. More specific examples of a computer-readable medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In this application, a computer-readable medium can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution device, apparatus, or device. In this application, a computer-readable signal medium can include a data signal propagated in baseband or as part of a carrier wave, carrying computer-readable program code. Such propagated data signals can take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. Computer-readable signal media can also be any computer-readable medium other than a computer-readable medium, which can send, propagate, or transmit a program for use by or in connection with an instruction execution device, apparatus, or apparatus. The program code contained on the computer-readable medium can be transmitted using any suitable medium, including but not limited to: wireless, wire, optical fiber, RF, etc., or any suitable combination thereof.

[0130] Computer program code for performing the operations of this application can be written in one or more programming languages ​​or a combination thereof, including object-oriented programming languages ​​such as Java, Smalltalk, and C++, and conventional procedural programming languages ​​such as the "C" language or similar programming languages. The program code can be executed entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving remote computers, the remote computer can be connected to the user's computer via any type of network—including a local area network (LAN) or a wide area network (WAN)—or it can be connected to an external computer (e.g., via the Internet using an Internet service provider).

[0131] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods, and computer program products according to various embodiments of this application. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, can be implemented using dedicated hardware-based means to perform the specified function or operation, or using a combination of dedicated hardware and computer instructions.

[0132] The modules described in the embodiments of this application can be implemented in software or hardware. These modules can also be located within a processor.

[0133] In another aspect, this application also provides a computer-readable medium, which may be included in the electronic device described in the above embodiments; or it may exist alone and not assembled into the electronic device. The aforementioned computer-readable medium carries one or more programs. When the electronic device executes one or more of these programs, the electronic device causes the following actions: It acquires magnetic field strength data and the initial pose of the permanent magnet collected by the magnetic sensor array, and selects an initial covariance matrix related to the state vector of the permanent magnet; it constructs a state equation based on the magnetic field strength data of the permanent magnet using a random walk model combined with the EKF algorithm, and predicts the predicted pose and predicted covariance matrix based on the initial pose and initial covariance matrix of the permanent magnet using the state equation; it obtains a measurement model based on the magnetic field strength data and determines the Jacobian matrix of the measurement model, establishes a pose update equation and a covariance matrix update equation based on the Jacobian matrix of the measurement model, inputs the predicted pose and predicted covariance matrix into the pose update equation and covariance matrix update equation to obtain the updated pose and updated covariance matrix; it repeats the above steps iteratively to update the permanent magnet using the updated pose and updated covariance matrix as the initial pose and initial covariance matrix, thus obtaining the final pose of the permanent magnet.

[0134] The above description is merely a preferred embodiment of this application and an explanation of the technical principles employed. Those skilled in the art should understand that the scope of the invention involved in this application is not limited to technical solutions formed by specific combinations of the above-described technical features, but should also cover other technical solutions formed by arbitrary combinations of the above-described technical features or their equivalents without departing from the above-described inventive concept. For example, technical solutions formed by substituting the above features with (but not limited to) technical features with similar functions disclosed in this application.

Claims

1. A method for real-time positioning of a permanent magnet under rapid motion, characterized in that, Includes the following steps: S1, acquire the magnetic field strength data of the permanent magnet collected by the magnetic sensor array and the initial pose of the permanent magnet, and select the initial covariance matrix related to the state vector of the permanent magnet; S2, based on the magnetic field strength data of the permanent magnet, a state equation is constructed by combining a random walk model with the EKF algorithm. Based on the initial pose and initial covariance matrix of the permanent magnet, the predicted pose and predicted covariance matrix are predicted by the state equation. S3. Obtain the measurement model based on the magnetic field strength data, and determine the Jacobian matrix of the measurement model. Establish the pose update equation and covariance matrix update equation based on the Jacobian matrix of the measurement model. Input the predicted pose and predicted covariance matrix into the pose update equation and covariance matrix update equation to obtain the updated pose and updated covariance matrix. S4, using the updated pose and updated covariance matrix as the initial pose and initial covariance matrix, repeat steps S1-S3 for iterative updates to obtain the final pose of the permanent magnet.

2. The real-time positioning method for permanent magnets under rapid motion according to claim 1, characterized in that, The magnetic sensor array has several triaxial magnetic sensors, and the magnetic field strength data Z of the permanent magnet collected by the magnetic sensor array is determined according to the magnetic dipole model. k .

3. The real-time positioning method for permanent magnets under rapid motion according to claim 2, characterized in that, The random walk model in step S2 can be represented by the following formula: x (k) =x (k-1) ±randn; Where, x (k) This represents the five-dimensional pose of the permanent magnet at this moment. x (k-1) This represents the pose of the permanent magnet at the previous moment, and randn represents an arbitrary value in five dimensions.

4. The real-time positioning method for permanent magnets under rapid motion according to claim 3, characterized in that, The state equation in step S2 is expressed by the following formula: x k =f(x k-1 ,u k )+w k ; z k =h(x k )+v k ; w k =sqrt(Q)*randn; V k =sqrt(R)*randn; Where, f(x) k-1 u k ) represents the location function, u k It is the control vector, h(x) k ) represents the measurement model, v k The measurement noise of the magnetic sensor array is represented by w. k The noise represents the process noise during the motion of the permanent magnet, and is Gaussian-distributed white noise related to Q, where Q is the covariance matrix related to the motion of the permanent magnet, calculated using the following formula: Q = diag(100, 100, 100, 1, 1) × 10 A Where A is a parameter related to the velocity of the permanent magnet, calculated as the sum of the changes in the triaxial output values ​​of all sensors in the measuring device. Before calculation, sliding window filtering and function fitting are used to derive the formula for calculating A: in, Let f represent the output value of the l-th triaxial magnetic sensor at time k, where f represents the X, Y, and Z axes, R is the measurement noise matrix of the triaxial magnetic sensor, and z is the magnetic field strength data of the permanent magnet collected. k As the observation matrix.

5. The real-time positioning method for permanent magnets under rapid motion according to claim 4, characterized in that, In step S2, the predicted pose and predicted covariance matrix are predicted using the state equation based on the initial pose and initial covariance matrix of the permanent magnet. Specifically, this includes: Let x (k-1|k-1) For the initial pose, P (k-1|k-1) Given the initial covariance matrix, the predicted pose x is obtained using the following formula. (k|k-1) and the predicted covariance matrix P (k|k-1) : x (k|k-1) =x (k-1|k-1) +w k ; P (k|k-1) =P (k-1|k-1 )+Q。 6. The real-time positioning method for permanent magnets under rapid motion according to claim 5, characterized in that, Step S3, which establishes the pose update equation and covariance matrix update equation based on the Jacobian matrix of the measurement model, specifically includes: The Kalman gain K is calculated using the following formula. k : Among them, H k The Jacobian matrix of the measurement model; The pose update equation and the covariance matrix update equation are respectively expressed as follows: x (k|k) =x (k|k-1) +K k [z k -h(x k )]; P (k|k) =[I-K k H k ]P (k|k-1) ; Where I represents the identity matrix, x (k|k) For the updated pose, P (k|k) The updated covariance matrix is ​​then used.

7. The real-time positioning method for permanent magnets under rapid motion according to claim 2, characterized in that, The magnetic sensor array contains 16 triaxial magnetic sensors, and the magnetic sensor array collects magnetic field strength data of the permanent magnet. z k =(B 1x ,B 1y ,B 1z ,B 2x ,…,B 16y ,B 16z ) T ; For any point M in the space surrounding the permanent magnet, the magnetic flux density at point M is calculated using the magnetic dipole model formula: Among them, B r Let B be the magnetic field strength at the location of the r-th three-dimensional magnetic sensor, where r = 1, ..., 16; i, j, and k represent unit vectors along the x, y, and z axes, respectively. rx B ry and B rz M represents the magnetic flux density in the corresponding direction; r R represents the distance vector from point M in space to the center of the permanent magnet. r Let M be the distance from point M to the center of the permanent magnet; where (a, b, c) is the position of the center of the permanent magnet, and (x... r y r , z r () represents the three-dimensional position of point M in space; H0 represents the unit vector of the permanent magnet in the direction of the magnetic dipole moment, denoted as H0 = (m, n, p). T (m, n, p) is expressed by the following formula, where θ and These represent the two angles between the center of the permanent magnet and the Cartesian coordinate system: For a single permanent magnet target, the magnetic flux density components along the three axes at the location of the r-th triaxial magnetic sensor are as follows:

8. A real-time positioning device for a permanent magnet under rapid movement, characterized in that, include: The initial state module is configured to acquire the magnetic field strength data of the permanent magnet collected by the magnetic sensor array and the initial pose of the permanent magnet, and select an initial covariance matrix related to the state vector of the permanent magnet. The prediction module is configured to construct a state equation based on the magnetic field strength data of the permanent magnet using a random walk model combined with the EKF algorithm, and to predict the predicted pose and predicted covariance matrix based on the initial pose and initial covariance matrix of the permanent magnet using the state equation. The update module is configured to obtain a measurement model based on the magnetic field strength data, determine the Jacobian matrix of the measurement model, establish a pose update equation and a covariance matrix update equation based on the Jacobian matrix of the measurement model, and input the predicted pose and predicted covariance matrix into the pose update equation and covariance matrix update equation to obtain the updated pose and updated covariance matrix. The iteration module is configured to repeatedly execute the initial state module to the update module for iterative updates, using the updated pose and the updated covariance matrix as the initial pose and the initial covariance matrix, to obtain the final pose of the permanent magnet.

9. An electronic device, comprising: One or more processors; Storage device for storing one or more programs. When the one or more programs are executed by the one or more processors, the one or more processors implement the method as described in any one of claims 1-7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the method as described in any one of claims 1-7.

Images