A low-frequency magnetic induction posture joint estimation method, device and system based on three-way anchor coil

Abstract

The present invention discloses a low-frequency magnetic induction posture joint estimation method based on a three-way anchor coil, comprising: calculating the receiving voltage of multiple three-way anchor coils, and obtaining the measured voltage of multiple three-way anchor coils after loading an alternating current on a unidirectional target coil; constructing an unlimited nonlinear least squares posture estimation problem based on the receiving voltage and the measured voltage, and using a gradient descent algorithm to solve the unlimited nonlinear least squares posture estimation problem to obtain the posture of the unidirectional target coil. The present invention designs an expression and a measurement model for the receiving voltage of the three-way anchor coil with respect to the position and posture of the unidirectional target coil, which can effectively improve the target positioning accuracy.

Description

Technical Field

[0001] The present invention relates to the technical field of low-frequency magnetic induction joint estimation of posture, and more specifically, to a low-frequency magnetic induction joint estimation method, device and system based on a three-way anchor coil. Background Art

[0002] With the expansion of human production space and the development of industry, the global satellite positioning system is increasingly unable to meet human needs for positioning in complex environments such as indoors, underground, and underwater. With the rise of indoor positioning, medical endoscopes, pipeline fault detection and other fields, it is urgent to develop accurate local pose estimation technology to achieve accurate perception of target position and posture in complex environments. Compared with traditional electromagnetic wave positioning technology, low-frequency magnetic field-based pose perception technology benefits from the advantages of high penetration, strong anti-interference, and no need for line of sight of magnetic field signals, making it easier to achieve high-precision target position and posture perception in complex environments such as indoors, underground, and underwater.

[0003] Existing positioning technologies based on low-frequency magnetic fields usually use unidirectional coils as anchor nodes and assume that the target's posture is known. However, in real environments, the target's posture is usually time-varying, and the angle mismatch between the unidirectional coil anchor node and the target coil will cause changes in the received voltage, resulting in incorrect estimation of the target distance and reduced target positioning accuracy. Summary of the invention

[0004] In response to at least one defect or improvement need in the prior art, the present invention provides a low-frequency magnetic induction posture joint estimation method, device and system based on a three-directional anchor coil, and designs an expression and measurement model for the three-directional anchor coil receiving voltage with respect to the position and posture of a unidirectional target coil, which can effectively improve the target positioning accuracy.

[0005] To achieve the above-mentioned object, according to a first aspect of the present invention, a low-frequency magnetic induction pose joint estimation method based on a three-way anchor coil is provided, which is applied to a low-frequency magnetic induction pose joint estimation system having a plurality of three-way anchor coils and a unidirectional target coil, wherein the three-way anchor coil is a coil composed of three identical and mutually perpendicular unidirectional coils of x-, y- and z-. The method comprises:

[0006] Calculating the received voltages of the multiple three-way anchor coils and obtaining the measured voltages of the multiple three-way anchor coils after the unidirectional target coil is loaded with an alternating current; the received voltages are expressed as a function of the position and attitude of the unidirectional target coil;

[0007] An unrestricted nonlinear least squares pose estimation problem is constructed according to the received voltage and the measured voltage, and a gradient descent algorithm is used to solve the unrestricted nonlinear least squares pose estimation problem to obtain the pose of the unidirectional target coil.

[0008] Furthermore, the above-mentioned low-frequency magnetic induction posture joint estimation method based on three-way anchor coil also includes:

[0009] The unreduced nonlinear least squares pose estimation problem is expressed as:

[0010] ;

[0011] in, is the receiving voltage, is the measured voltage, is the estimated value of the center coordinate of the unidirectional target coil, is the estimated pointing value of the unidirectional target coil, is the number of three-way anchor coils.

[0012] Furthermore, the above-mentioned low-frequency magnetic induction posture joint estimation method based on three-way anchor coil also includes:

[0013] The receiving voltage of the three-way anchor coil It is expressed as:

[0014] ;

[0015] in, is the voltage measurement constant of the three-way anchor coil, is the near-field component of the magnetic field of the unidirectional target coil in the The induced voltage component vector generated in the three-way anchor coil is: is the far-field component of the magnetic field of the unidirectional target coil in the The induced voltage component vector generated in the three-way anchor coil is: is the direction matrix of the three-way anchor coil, , and are the direction vectors of the x-, y- and z-direction coils of the three-way anchor coil, respectively, For the one-way target coil and the The distance between the center points of the three-way anchor coils, is the unidirectional target coil position vector, is the attitude vector of the unidirectional target coil, For the The position vector of the three-way anchor coil; For the unidirectional target coil The near-field component matrix generated at the center point of the three-way anchor coil is: For the unidirectional target coil The far-field component matrix generated at the center point of the three-way anchor coil is: For the one-way target coil and the The direction vector of the line connecting the center points of the three-way anchor coils, is the wave number of the unidirectional target coil.

[0016] Furthermore, the above-mentioned low-frequency magnetic induction posture joint estimation method based on three-way anchor coil also includes:

[0017] The voltage measurement constant of the three-way anchor coil is expressed as:

[0018] ;

[0019] in is the magnetic permeability of the medium, is the area of ​​the unidirectional target coil, is the number of turns of the unidirectional target coil, Load current to the unidirectional target coil, is the surface area of ​​the three-way anchor coil, is the number of turns of the three-way anchor coil, is the load matching coefficient of the three-way anchor coil, is the unidirectional target coil system frequency.

[0020] Furthermore, the above-mentioned low-frequency magnetic induction posture joint estimation method based on three-way anchor coil also includes:

[0021] When solving the unrestricted nonlinear least squares pose estimation problem, the Cramer-Laur lower bound of the joint pose estimation error is calculated and expressed as:

[0022] ;

[0023] FIM is the Fisher information matrix, which is expressed as follows:

[0024] ;

[0025] in, for The measured voltage error matrix of the three-way anchor coil is: for The Jacobian matrix of the joint estimation of the three-way anchor coil pose is expressed as:

[0026] ;

[0027] is the pose vector of the unidirectional target coil, and:

[0028] ;

[0029] ;

[0030] ;

[0031] ;

[0032] ;

[0033] ;

[0034] ;

[0035] .

[0036] According to a second aspect of the present invention, a low-frequency magnetic induction pose joint estimation device based on a three-way anchor coil is provided, which is applied to a low-frequency magnetic induction pose joint estimation system having a plurality of three-way anchor coils and a unidirectional target coil, wherein the three-way anchor coil is a coil composed of three identical and mutually perpendicular unidirectional coils of x-, y- and z-, and the device comprises:

[0037] A measuring module, used for measuring the measuring voltage of a plurality of three-way anchor coils;

[0038] The calculation module is used to obtain the measurement result of the measurement module, and obtain the position and posture of the unidirectional target coil through the steps of any of the above methods.

[0039] According to a third aspect of the present invention, a low-frequency magnetic induction posture joint estimation system based on a three-way anchor coil is provided, comprising:

[0040] A number of three-way anchor coils and a one-way target coil, as well as measurement equipment and computer equipment; including:

[0041] The three-way anchor coil is a coil consisting of three identical and mutually perpendicular unidirectional coils of x-, y- and z-;

[0042] The measuring device is used to measure the measurement voltage of a plurality of three-way anchor coils;

[0043] The computer device is connected to the measuring device, and comprises a memory, a processor and a computer program stored in the memory, and the processor executes the computer program to implement the steps of any one of the above methods.

[0044] In general, the above technical solutions conceived by the present invention can achieve the following beneficial effects compared with the prior art:

[0045] (1) The present invention provides a low-frequency magnetic induction posture joint estimation method based on a three-directional anchor coil. An expression and measurement model for the three-directional anchor coil receiving voltage with respect to the position and posture of a unidirectional target coil are designed, which can effectively improve the target positioning accuracy.

[0046] (2) The present invention provides a low-frequency magnetic induction joint pose estimation method based on a three-way anchor coil, and analyzes the Cramer-Laur lower bound of the pose estimation method, which is used to quantitatively measure the system accuracy of the low-frequency magnetic induction joint pose estimation scheme based on a three-way anchor coil. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0048] Figure 1 A schematic diagram of a low-frequency magnetic induction posture joint estimation system provided by an embodiment of the present invention;

[0049] Figure 2 The present invention provides a method for joint estimation of low-frequency magnetic induction posture based on a three-way anchor coil at different spatial scales, which is a performance verification result. DETAILED DESCRIPTION

[0050] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0051] The terms "first", "second", "third", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish different objects, rather than to describe a specific order. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device that includes a series of steps or units is not limited to the listed steps or units, but optionally includes steps or units that are not listed, or optionally includes other steps or units inherent to these processes, methods, products or devices.

[0052] As a first embodiment of the present invention, a low-frequency magnetic induction posture joint estimation method based on a three-way anchor coil is provided. The low-frequency magnetic induction posture joint estimation method based on a three-way anchor coil of this embodiment is applied to a low-frequency magnetic induction posture joint estimation system, and the structure diagram of the system is shown in FIG. Figure 1 As shown, it includes a plurality of three-way anchor coils and a unidirectional target coil. The three-way anchor coil in this embodiment refers to a coil composed of three identical and mutually perpendicular unidirectional coils of x-, y- and z-directions.

[0053] Assume that Figure 1 The radius of the one-way target coil in is , the area is , the number of turns is , the loading current is , the system frequency is , the wave number is , the location is , the posture is ;No. The radius of each unidirectional coil in the three-way anchor coil is , the area is , the number of turns is , the location is , the attitude matrix of the three-way anchor coil is ,in:

[0054] ;

[0055] ;

[0056] .

[0057] One-way target coil and The distance between the center points of the three-way anchor coils is , one-way target coil and the first The direction vector of the line connecting the center points of the anchor coils is , the unidirectional target coil is The near-field component matrix generated at the center point of the anchor coil is: , the unidirectional target coil is The far-field component matrix generated at the center point of the anchor coil is: , the magnetic permeability of the ambient medium is , then: The unidirectional target coil of the low-frequency magnetic induction pose joint estimation system based on the three-way anchor coil is in the first The magnetic field generated at the center point of the three-way anchor coil is:

[0058] ;

[0059] No. The relationship between the receiving voltage of the three-way anchor coil and the position and attitude of the unidirectional target coil can be expressed as:

[0060] ;

[0061] in, , is the anchor coil load matching coefficient, is the near-field component of the magnetic field of the target coil in the The induced voltage component vector generated in the anchor coil is is the far-field component of the magnetic field of the target coil The induced voltage component vector generated in the anchor coil.

[0062] Based on the least squares criterion, an unconstrained nonlinear least squares target coil pose estimation problem is designed: ,The steepest descent algorithm is used to solve the target coil pose estimation problem.

[0063] Give the pose vector of the unidirectional target coil , The Jacobian matrix of the pose estimation of a three-way anchor coil is:

[0064] ;

[0065] ;

[0066] , where j represents the imaginary number symbol;

[0067] ;

[0068] ;

[0069] ;

[0070] ;

[0071] ;

[0072] ;

[0073] Given The measured voltage error matrix of a three-way anchor coil , the Fisher information matrix of the unidirectional coil pose estimation is The Cramer-Laur lower bound of the joint estimation error of the unidirectional coil pose in the low-frequency magnetic induction pose estimation system based on the three-directional anchor coil is .

[0074] In this embodiment, a unidirectional target coil and two three-way anchor coils for joint pose estimation are included, and the parameters of the two three-way anchor coils are exactly the same. Among them, the radius of the unidirectional target coil is , the coil area is , the number of coil turns is , the loading current is , the system frequency is , the wave number is , the relative magnetic permeability is ;No. A three-way anchor coil consists of three identical unidirectional coils with a coil radius of , the coil area is The number of coil turns is The two three-way anchor coils are located at The two vertices of the cube, the coordinates of the center point of the first three-way anchor coil are , the coil pointing matrix is ; The coordinates of the center point of the second three-way anchor coil are , the coil pointing matrix is ,,; the center point coordinates of the unidirectional target coil are .

[0075] when When , the coordinates of the center point of the first three-way anchor coil are , the coil pointing matrix is ; The coordinates of the center point of the second three-way anchor coil are , the coil pointing matrix is The coordinates of the center point of the unidirectional target coil are , the coil points to ,in , The distance between the first three-way anchor coil and the one-way target coil is , the distance between the second three-way anchor coil and the one-way coil is ; The direction vectors of the lines connecting the center points of the two three-way anchor coils and the one-way target coil are expressed as:

[0076] ;

[0077] ;

[0078] The near-field component matrix generated by the unidirectional target coil at the center point of the first three-directional anchor coil is:

[0079] ;

[0080] The far-field component matrix generated by the unidirectional target coil at the center point of the first three-directional anchor coil is:

[0081] ;

[0082] The magnetic field vector generated by the unidirectional target coil at the center point of the first three-directional anchor coil is:

[0083] ;

[0084] The near-field component matrix generated by the unidirectional target coil at the center point of the second three-directional anchor coil is:

[0085] ;

[0086] The far-field component matrix generated by the unidirectional target coil at the center point of the second three-directional anchor coil is:

[0087] ;

[0088] The magnetic field vector generated by the unidirectional target coil at the center point of the second three-directional anchor coil is:

[0089] ;

[0090] The receiving voltage vector of the first three-way anchor coil is:

[0091] ;

[0092] The receiving voltage vector of the second three-way anchor coil is:

[0093] ;

[0094] Assume that the measured voltage error follows a Gaussian distribution and has a variance of , then the measured voltage matrices are , The anchor coil load matching coefficient is , the voltage measurement constant related only to the coil parameters is expressed as:

[0095] .

[0096] After calibrating the parameters, the unconstrained nonlinear least squares target coil pose estimation problem is constructed:

[0097] .

[0098] The problem can be quickly solved using the gradient descent algorithm to obtain the position and attitude estimation vectors. Figure 2 The position and attitude estimation error results of the unidirectional target coil under different spatial sizes are given, verifying the effectiveness of the magnetic induction attitude estimation method based on the three-directional coil.

[0099] As a second embodiment of the present invention, a low-frequency magnetic induction pose joint estimation device based on a three-way anchor coil is provided, which is applied to a low-frequency magnetic induction pose joint estimation system having multiple three-way anchor coils and one unidirectional target coil, wherein the three-way anchor coil is a coil composed of three identical and mutually perpendicular unidirectional coils of x-, y- and z-, and the device comprises:

[0100] A measuring module, used for measuring the measuring voltage of a plurality of three-way anchor coils;

[0101] The calculation module is used to obtain the measurement result of the measurement module, and obtain the position and posture of the unidirectional target coil through the steps of the method described in the above embodiment.

[0102] As a third embodiment of the present invention, a low-frequency magnetic induction posture joint estimation system based on a three-way anchor coil is provided, comprising:

[0103] A number of three-way anchor coils and a one-way target coil, as well as measurement equipment and computer equipment; including:

[0104] The three-way anchor coil is a coil consisting of three identical and mutually perpendicular unidirectional coils of x-, y- and z-;

[0105] The measuring device is used to measure the measurement voltage of a plurality of three-way anchor coils;

[0106] The computer device is connected to the measuring device, and includes a memory, a processor, and a computer program stored in the memory. The processor executes the computer program to implement the steps of the method described in the above embodiment.

[0107] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0108] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A low-frequency magnetic induction pose joint estimation method based on a three-way anchor coil, applied to a low-frequency magnetic induction pose joint estimation system having a plurality of three-way anchor coils and a unidirectional target coil, wherein the three-way anchor coil is a coil composed of three identical and mutually perpendicular unidirectional coils of x-, y- and z-, the method comprising: calculating the received voltages of the multiple three-way anchor coils and obtaining the measured voltages of the multiple three-way anchor coils after the unidirectional target coil is loaded with an alternating current; The received voltage is expressed as a function of the position and attitude of the unidirectional target coil; According to the received voltage and the measured voltage, an unlimited nonlinear least squares pose estimation problem is constructed, and a gradient descent algorithm is used to solve the unlimited nonlinear least squares pose estimation problem to obtain the pose of the unidirectional target coil; The unreduced nonlinear least squares pose estimation problem is expressed as: Among them, v n is the receiving voltage, is the measured voltage, is the estimated value of the center coordinate of the unidirectional target coil, is the estimated value of the orientation of the unidirectional target coil, and n is the number of the three-directional anchor coil.

2. The low-frequency magnetic induction pose joint estimation method based on a three-way anchor coil as claimed in claim 1, characterized in that: The receiving voltage v of the three-way anchor coil n It is expressed as: Among them, α n is the voltage measurement constant of the three-way anchor coil, is the induced voltage component vector generated by the near-field component of the magnetic field of the unidirectional target coil in the nth three-directional anchor coil, is the induced voltage component vector generated by the far-field component of the magnetic field of the unidirectional target coil in the nth three-directional anchor coil, n =[o n,x ,o n,y ,o n,z ] is the direction matrix of the three-way anchor coil, o n,x , o n,y and n,z are the direction vectors of the x-, y- and z-direction coils of the three-way anchor coil, r n =||p T -p n || is the distance between the one-way target coil and the center point of the nth three-way anchor coil, p T is the unidirectional target coil position vector, o T is the attitude vector of the unidirectional target coil, p n is the position vector of the nth three-way anchor coil; is the near-field component matrix generated by the unidirectional target coil at the center point of the nth three-directional anchor coil, is the far-field component matrix generated by the unidirectional target coil at the center point of the nth three-directional anchor coil, is the direction vector of the line connecting the unidirectional target coil and the center point of the nth three-way anchor coil, k is the wave number of the unidirectional target coil; j represents the imaginary number sign.

3. The low-frequency magnetic induction pose joint estimation method based on a three-way anchor coil as claimed in claim 2, characterized in that: The voltage measurement constant of the three-way anchor coil is expressed as: a n =μ0A T N T A n N n I T fk 3 ·x n ; Where μ0 is the magnetic permeability of the medium, A T is the area of ​​the unidirectional target coil, N T is the number of turns of the unidirectional target coil, i T Load the unidirectional target coil with current, A n is the surface area of ​​the three-way anchor coil, N n is the number of turns of the three-way anchor coil, ξ n is the load matching coefficient of the three-way anchor coil, and f is the system frequency of the unidirectional target coil.

4. The low-frequency magnetic induction pose joint estimation method based on a three-way anchor coil as claimed in claim 3, characterized in that: When solving the unrestricted nonlinear least squares pose estimation problem, the Cramer-Laur lower bound of the joint pose estimation error is calculated and expressed as: CRLB=END -1 ; FIM is the Fisher information matrix, which is expressed as follows: FIM=J H ∑ -1 J; in, is the measured voltage error matrix of N three-way anchor coils, and J is the Jacobian matrix of the joint estimation of the pose of N three-way anchor coils, which is expressed as: is the pose vector of the unidirectional target coil, and:

5. A low-frequency magnetic induction pose joint estimation device based on a three-way anchor coil, applied to a low-frequency magnetic induction pose joint estimation system having a plurality of three-way anchor coils and a unidirectional target coil, wherein the three-way anchor coil is a coil composed of three identical and mutually perpendicular unidirectional coils of x-, y- and z-, and the device comprises: A measuring module, used for measuring the measuring voltage of a plurality of three-way anchor coils; A calculation module is used to obtain the measurement result of the measurement module, and obtain the position and posture of the unidirectional target coil through the steps of the method according to any one of claims 1 to 4.

6. A low-frequency magnetic induction posture joint estimation system based on a three-way anchor coil, comprising: A number of three-way anchor coils and a one-way target coil, as well as measurement equipment and computer equipment; including: The three-way anchor coil is a coil consisting of three identical and mutually perpendicular unidirectional coils of x-, y- and z-; The measuring device is used to measure the measurement voltage of a plurality of three-way anchor coils; The computer device is connected to the measuring device, and comprises a memory, a processor and a computer program stored in the memory, and the processor executes the computer program to implement the steps of the method according to any one of claims 1 to 4.

Images