Three-axis magnetic sensor positioning and attitude determining method and system based on magnetic beacons

Abstract

The invention belongs to the technical field of magnetic field navigation, and particularly relates to a three-axis magnetic sensor positioning and attitude determination method and system based on a magnetic beacon. The method comprises the following steps of: 1, expressing distribution of a magnetic field generated by a magnetic beacon at any position in space through a spherical harmonic function; 2, determining the relative position of the magnetic beacon and the magnetic sensor based on the position of the magnetic beacon; 3, based on the relative position in the step 2, establishing a magnetic sensor positioning and attitude determination equation; 4, solving the equation in the step 3 by adopting a particle swarm algorithm to obtain spherical coordinates; and 5, converting the spherical coordinates solved in the step 4 into Cartesian coordinates to complete positioning and attitude determination of the magnetic sensor. The space coordinate and the attitude of the magnetic sensor are accurately calculated through the magnetic field generated by the magnetic beacon at the magnetic sensor to be detected, so that the technical development of related fields is supported.

Description

Technical Field

[0001] The present invention belongs to the technical field of magnetic field navigation, and particularly relates to a method and system for positioning and attitude determination of a three-axis magnetic sensor based on magnetic beacons. Background Art

[0002] The research in fields such as spacecraft magnetic measurement and modeling, geomagnetic navigation, UAV aerial survey, and resource exploration all relies on the spatial position and attitude data of magnetic sensors, and the position and attitude of the magnetic sensors have a significant impact on the test data. Therefore, the above fields have strict requirements for the measurement and calibration of the position and attitude of magnetic sensors. In addition, with the application of technologies such as intelligent unmanned systems and the Internet of Things in indoor environments, higher requirements are put forward for indoor navigation and positioning technologies. A low-cost, high-precision, and easy-to-maintain indoor navigation and positioning solution is one of the key research directions in the field of indoor navigation technology. In indoor environments where satellite navigation is denied, methods such as ultra-wideband (UWB), vision, lidar, and inertial navigation cannot provide high-precision and reliable navigation and positioning services for a long time due to environmental limitations and their own factors. Summary of the Invention

[0003] The present invention provides a method for positioning and attitude determination of a three-axis magnetic sensor based on magnetic beacons, which accurately calculates the spatial coordinates and attitude of the magnetic sensor through the magnetic field generated by the magnetic beacons at the magnetic sensor to be measured, thereby supporting the technological development of related fields.

[0004] The present invention also provides a system for positioning and attitude determination of a three-axis magnetic sensor based on magnetic beacons to implement a method for positioning and attitude determination of a three-axis magnetic sensor based on magnetic beacons.

[0005] The present invention is achieved through the following technical solutions:

[0006] A method for positioning and attitude determination of a three-axis magnetic sensor based on magnetic beacons, the method comprising the following steps:

[0007] Step 1: Express the distribution of the magnetic field generated by the magnetic beacon at any position in space through spherical harmonic functions;

[0008] Step 2: Based on the position of the magnetic beacon, confirm the relative position between the magnetic beacon and the magnetic sensor;

[0009] Step 3: Based on the relative position in Step 2, establish an equation for positioning and attitude determination of the magnetic sensor;

[0010] Step 4: For the equation in Step 3, use the particle swarm algorithm to solve for the spherical coordinates;

[0011] Step 5: Convert the spherical coordinates solved in Step 4 into Cartesian coordinates to complete the positioning and attitude determination of the magnetic sensor.

[0012] Further, the specific content of step 1 is as follows: The magnetic field distribution generated by the magnetic beacon is expressed by spherical harmonic functions. The three-component expressions of the magnetic beacon in spherical coordinates are as follows:

[0013]

[0014] where j is the order of the spherical harmonic function, m is the degree of the spherical harmonic function, a jm and b jm are the spherical harmonic coefficients, r, θ, and φ are the three coordinates of the sensor in spherical coordinates, is the Schmidt semi-normalized associated Legendre function;

[0015] The magnetic beacon adopts a 3-axis orthogonal coil; the three orthogonal coils exactly correspond to the 3-axis orthogonal magnetic moment vectors M x , M y and M z ;

[0016] In equations (1)-(3), when the spherical harmonic order j is set to 1, the following magnetic field expression generated by the magnetic dipole in space is obtained:

[0017]

[0018] According to the multi-level expansion law of spherical harmonic functions, it can be known that:

[0019]

[0020] where I x , I y and I z are the three-axis coil currents, S x , S y and S z are the cross-sectional areas of the three-axis coils, N x , N y and N z are the number of turns of the three-axis coils.

[0021] Further, the specific content of step 2 is as follows: The center of the magnetic beacon is used as the origin of the global coordinate system; the spherical coordinates of the position where the magnetic sensor is located are (r, θ, φ); without considering the three-axis rotation of the magnetic sensor, the three components of the magnetic field at the center of the magnetic sensor can be expressed as follows:

[0022]

[0023] The rotation of the vector is represented by a three-axis rotation matrix, as follows:

[0024]

[0025] In the formula, Rot(x,α), Rot(y,β), and Rot(z,γ) respectively represent the rotation matrices for rotating by an angle α around the x-axis, by an angle β around the y-axis, and by an angle γ around the z-axis;

[0026] Assume that the magnetic sensor first rotates by an angle α around the x-axis, then by an angle β around the y-axis, and finally by an angle γ around the z-axis. Then the final reading of the sensor is expressed as

[0027]

[0028] Substitute equations (8)-(11) into (12) to obtain the three-component expression of the magnetic sensor:

[0029]

[0030] In the formula, B sx , B sy , and B sz are the readings of the x-axis, y-axis, and z-axis of the magnetic sensor respectively.

[0031] Further, step 3 is specifically as follows: Place the magnetic beacon on a non-magnetic turntable, set the origin of the reference coordinate system at the center of the magnetic beacon, and align the three axes of the coordinate system with the axes of the three-axis coil in the magnetic beacon respectively;

[0032] Rotate the non-magnetic turntable 360 degrees around the z-axis, and record the readings of the magnetic sensor every 10 degrees; Since the magnetic beacon rotates 360 degrees around the z-axis, at this time, in equations (13)-(15), B r , B θ , and B φ are written in the following form:

[0033]

[0034] In the formula, Gradually increases from 0 to 2π as the rotation progresses, thus completing one cycle of rotation;

[0035] Calculate the zero-order and first-order Fourier coefficients of the three-axis readings of the magnetic sensor based on the magnetic measurement data as follows:

[0036]

[0037] In the formula, A x0 , A y0 , and A z0 are the zero-order Fourier coefficients of the readings of the x-axis, y-axis, and z-axis of the magnetic sensor respectively;

[0038]

[0039] Further, substituting equations (13)-(18) into (19) and (20), and after simplification, the following expressions are obtained:

[0040]

[0041] Six unknowns need to be solved for the positioning and attitude determination of the magnetic sensor, namely r, θ, φ, α, β, and γ; the positioning and attitude determination of the magnetic sensor can be completed through equations (21)-(29) by the least squares method.

[0042] Further, the specific step 4 is to solve the above equations using the particle swarm optimization algorithm, and the objective function and the value ranges of the six unknowns are set as follows:

[0043] Objective function:

[0044]

[0045] In the formula, A x0 实测 represents the Fourier coefficients calculated from the actual readings of the magnetic sensor; A x0 represents the right end in equation (21);

[0046] The value ranges of the variables are:

[0047]

[0048] According to the above settings, using the PSO algorithm, the six unknowns of the magnetic sensor can be calculated to obtain the spherical coordinates of the magnetic sensor.

[0049] Further, the specific step 5 is to convert the spherical coordinates of the magnetic sensor into Cartesian coordinates:

[0050]

[0051] Thus, the positioning and attitude determination of the magnetic sensor are realized.

[0052] A three-axis magnetic sensor positioning and attitude determination system based on a magnetic beacon, the system adopts the above-mentioned three-axis magnetic sensor positioning and attitude determination method based on a magnetic beacon, and the system includes:

[0053] Magnetic beacon magnetic field expression module: expressing the distribution of the magnetic field generated by the magnetic beacon at any position in space through spherical harmonic functions;

[0054] Relative position confirmation module of the magnetic beacon and the magnetic sensor: confirming the relative position of the magnetic beacon and the magnetic sensor based on the position of the magnetic beacon;

[0055] Equation establishment module: establishing a positioning and attitude determination equation for the magnetic sensor based on the relative position of the magnetic beacon and the magnetic sensor;

[0056] Spherical coordinate solving module: For the equation in step 3, the particle swarm optimization algorithm is used to solve and obtain the spherical coordinates.

[0057] Coordinate conversion module: Convert the solved spherical coordinates into Cartesian coordinates to achieve the positioning and attitude determination of the magnetic sensor.

[0058] A computer device includes a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the computer program, the above-mentioned method is implemented.

[0059] A computer-readable storage medium stores a computer program therein. When the computer program is executed by a processor, the above-mentioned method is implemented.

[0060] The beneficial effects of the present invention are as follows:

[0061] The present invention can achieve 6D positioning and attitude determination of the magnetic sensor, and the result is more accurate.

[0062] With the magnetic measurement data obtained by rotating one week, the present invention can simultaneously locate the positions of multiple magnetic sensors without any quantity limitation.

[0063] The magnetic beacon of the present invention is composed of a three-axis electromagnetic coil. Since the magnetic field signal generated by this magnetic beacon is not affected by the air, seawater, and underground environments, it has strong anti-interference ability and can be used in a satellite-denied environment. Description of the Drawings

[0064] Figure 1 It is a schematic diagram of the relative positions of the magnetic sensor and the magnetic beacon of the present invention.

[0065] Figure 2 It is a schematic diagram of the magnetic field calculation data at target point 1 in the embodiment of the present invention. Among them, (a) is the schematic diagram of the data of the magnetic field before rotation at target point 1, and (b) is the schematic diagram of the calculated data of the magnetic field after rotation at target point 1.

[0066] Figure 3 It is a schematic diagram of the method flow of the present invention. Detailed Embodiments

[0067] In the following description, specific details such as specific system structures and technologies are presented for the purpose of illustration rather than limitation, so as to thoroughly understand the embodiments of the present application. However, those skilled in the art should clearly understand that the present application can also be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to avoid unnecessary details from interfering with the description of the present application.

[0068] It should be understood that when used in this specification and the appended claims, the term "comprising" indicates the presence of the described features, integers, steps, operations, elements, and / or components, but does not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or their groups.

[0069] It should also be understood that the terms used in the specification of this application are only for the purpose of describing specific embodiments and are not intended to limit this application. As used in the specification of this application and the appended claims, unless the context clearly indicates otherwise, the singular forms "a", "an", and "the" are intended to include the plural forms.

[0070] The following combines the appendix of the specification of this application Figures 1-3 , and clearly and completely describes the technical solutions in the embodiments of this application. Obviously, the described embodiments are only a part of the embodiments of this application, rather than all the embodiments. Based on the embodiments in this application, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope protected by this application.

[0071] Many specific details are set forth in the following description to facilitate a full understanding of this application, but this application may also be implemented in other ways different from those described herein. Those skilled in the art can make similar extensions without departing from the connotation of this application. Therefore, this application is not limited by the specific embodiments disclosed below.

[0072] Embodiment 1

[0073] An embodiment of the present invention provides a method for positioning and attitude determination of a three-axis magnetic sensor based on a magnetic beacon. The method includes the following steps:

[0074] Step 1: Express the distribution of the magnetic field generated by the magnetic beacon at any position in space through spherical harmonic functions;

[0075] Step 2: Based on the position of the magnetic beacon, confirm the relative position between the magnetic beacon and the magnetic sensor;

[0076] Step 3: Based on the relative position in Step 2, establish a positioning and attitude determination equation for the magnetic sensor;

[0077] Step 4: For the equation in Step 3, use the particle swarm optimization algorithm (PSO) to solve for the spherical coordinates;

[0078] Step 5: Convert the spherical coordinates solved in Step 4 into Cartesian coordinates to complete the positioning and attitude determination of the magnetic sensor.

[0079] Further, the specific steps of Step 1 are as follows: First, the magnetic field distribution generated by the magnetic beacon is expressed by spherical harmonic functions. The three-component expressions of the magnetic beacon in spherical coordinates are as follows:

[0080]

[0081]

[0082] where j is the order of the spherical harmonic function, m is the degree of the spherical harmonic function, a jm and b jm are the spherical harmonic coefficients, r, θ, and φ are the three coordinates of the sensor in spherical coordinates, is the Schmidt semi-normalized associated Legendre function;

[0083] Here, the magnetic beacon uses a 3-axis orthogonal coil; according to the principle of the magnetic dipole, when the distance between the magnetic sensor and the magnetic beacon is more than 3 times the size of the magnetic beacon, the magnetic beacon can be regarded as a magnetic dipole; therefore, the three orthogonal coils exactly correspond to the 3-axis orthogonal magnetic moment vectors M x , M y and M z ;

[0084] In equations (1)-(3), when the spherical harmonic order j is set to 1, the following magnetic field expression generated by the magnetic dipole in space can be obtained:

[0085]

[0086] According to the multi-level expansion law of spherical harmonic functions, the following relationship exists between the first-order spherical harmonic coefficients and the three-axis magnetic moments of the magnetic beacon:

[0087]

[0088] where I x , I y and I z are the three-axis coil currents, S x , S y and S z are the cross-sectional areas of the three-axis coils, N x , N y and N z are the number of turns of the three-axis coils; therefore, as long as the electromagnetic parameters and input current of the magnetic beacon are known, the magnetic field at any position in space can be calculated through equations (4)-(7).

[0089] Further, the specific steps of Step 2 are as follows: The relative position between the magnetic beacon and the magnetic sensor is as Figure 1As shown in the figure, the center of the magnetic beacon is used as the origin of the global coordinate system; the spherical coordinates of the position where the magnetic sensor is located are (r, θ, φ); therefore, without considering the three-axis rotation of the magnetic sensor, the three components of the magnetic field at the center of the magnetic sensor can be expressed as follows:

[0090]

[0091] In Equation (8), only the change of spatial position is considered, and the attitude information of the three axes needs to be added; the rotation of the vector can be represented by a three-axis rotation matrix, as shown below:

[0092]

[0093] In the formula, Rot(x, α), Rot(y, β), and Rot(z, γ) respectively represent the rotation matrices of rotating by α angle around the x-axis, rotating by β angle around the y-axis, and rotating by γ angle around the z-axis;

[0094] Assume that the magnetic sensor first rotates by α angle around the x-axis, then rotates by β angle around the y-axis, and finally rotates by γ angle around the z-axis. Then the final reading of the sensor can be expressed as

[0095]

[0096] Substituting Equations (8)-(11) into (12), the three-component expression of the magnetic sensor can be obtained:

[0097] In the formula, B sx , B sy and B sz are the readings of the x-axis, y-axis, and z-axis of the magnetic sensor respectively.

[0098] Furthermore, the specific content of step 3 is, as Figure 1 shown in the figure, place the magnetic beacon on a non-magnetic turntable, set the origin of the reference coordinate system at the center of the magnetic beacon, and align the three axes of the coordinate system with the axes of the three coils in the magnetic beacon respectively; only 1 sensor is given in the figure, but the number of sensors and their specific placement positions are not limited in this method, and the spatial positioning and attitude determination of any number of sensors can be completed simultaneously.

[0099] Rotate the non-magnetic turntable 360 degrees around the z-axis, and record the readings of the magnetic sensor every 10 degrees (the step angle can be adjusted according to needs); since the magnetic beacon rotates 360 degrees around the z-axis, at this time, in Equations (13)-(15), B r , B θ and B φ can be written in the following form:

[0100]

[0101] In the formula, gradually increases from 0 to 2π with rotation, thus completing one cycle of rotation;

[0102] Calculate the zero-order and first-order Fourier coefficients of the three-axis readings of the magnetic sensor according to the magnetic measurement data as follows:

[0103]

[0104] In the formula, A x0 , A y0 and A z0 are the zero-order Fourier coefficients of the readings of the x-axis, y-axis, and z-axis of the magnetic sensor respectively;

[0105]

[0106] Furthermore, substituting equations (13)-(18) into (19) and (20), the following expressions can be obtained through simplification:

[0107]

[0108] Six unknowns need to be solved for the positioning and attitude determination of the magnetic sensor, namely r, θ, φ, α, β, and γ; and there are nine equations as above; therefore, the positioning and attitude determination of the magnetic sensor can be completed through equations (21)-(29) by the least squares method; the Fourier coefficients on the left side of equations (21)-(29) can be calculated from the measured data in combination with equations (19) and (20), and the magnetic moments a 11 , b 11 and a 10 can be calculated from the energizing current in combination with equation (7).

[0109] Furthermore, step 4 is specifically as follows: The particle swarm optimization algorithm (PSO) is used to solve the above equations, and the objective function and the value ranges of the six unknowns are set as follows:

[0110] Objective function:

[0111]

[0112] In the formula, A x0 实测 represents the Fourier coefficient calculated from the actual readings of the magnetic sensor; A x0 represents the right side in equation (21); the definitions of other Fourier coefficients are also in the same way;

[0113] The value ranges of the variables are:

[0114]

[0115] According to the above settings, using the PSO algorithm, six unknowns of the magnetic sensor can be calculated to obtain the spherical coordinates of the magnetic sensor.

[0116] Further, the specific step 5 is to convert the spherical coordinates of the magnetic sensor into Cartesian coordinates:

[0117]

[0118] Thus, the positioning and attitude determination of the magnetic sensor are realized.

[0119] The inversion verification is carried out by manually calculating data, and the specific method is as follows:

[0120] Assume that the three-axis magnetic moments of the magnetic beacon are a 11 = 0.646 Am 2 , b 11 = 0.3 Am 2 , a 10 = 0.5 Am 2 .

[0121] When the magnetic beacon rotates one week, the calculated magnetic field values at the target point 1 with coordinates (0.5, 0, 0.5) are as Figure 2 (a) shown. Subsequently, first rotate the sensor 10 degrees around the x-axis, second rotate 20 degrees around the y-axis, and finally

[0122] rotate 30 degrees around the z-axis, and its reading becomes as Figure 2 (b) shown.

[0123] Substitute the Figure 2 data in (b) into (19) and (20) respectively to calculate the zero-order and first-order Fourier

[0124] coefficients.

[0125]

[0126] Substitute (33) into (30), and the spatial coordinates

[0127] and attitude of the magnetic sensor can be calculated through the PSO algorithm as follows:

[0128] (x, y, z, α, β, γ) = (0.5000, 0, 0.5000, 10.0004, 19.9998, 30.0000) (33)

[0129] It can be seen that the calculation results are basically consistent with the preset values.

[0130] In the same way, the coordinates of the other 7 target points were also predicted, and the actual values were compared with the predicted values, as shown in Table 1. It can be seen that the actual values are basically the same as the predicted values, which can verify the accuracy and effectiveness of the positioning and attitude determination method proposed in this

[0131] patent.

[0132] Table 1 Positioning results of different target points

[0133]

[0134] Embodiment 2

[0135] The embodiment of the present invention provides a three-axis magnetic sensor positioning and attitude determination system based on magnetic beacons. The

[0136] system adopts the three-axis magnetic sensor positioning and attitude determination method based on magnetic beacons as described above. The system includes:

[0137] Magnetic beacon magnetic field expression module: expressing the distribution of the magnetic field generated by the magnetic beacon at any position in space through spherical harmonic functions;

[0138] at any position in space;

[0139] Relative position confirmation module of magnetic beacon and magnetic sensor: confirming the relative position of the magnetic beacon and the magnetic sensor based on the position of the magnetic beacon;

[0140] Equation establishment module: establishing a positioning and attitude determination equation for the magnetic sensor based on the relative position of the magnetic beacon and the magnetic sensor;

[0141] Spherical coordinate solving module: solving the equation in step 3 by using the particle swarm optimization algorithm to obtain spherical coordinates;

[0142] Coordinate conversion module: converting the solved spherical coordinates into Cartesian coordinates to realize the positioning and attitude determination of the magnetic sensor.

[0143] As can be seen from the above, the embodiment of the present invention establishes a positioning and attitude determination equation for the magnetic sensor through the magnetic beacon and solves it. The three-axis magnetic sensor converts the solved spherical coordinates into Cartesian coordinates to realize the positioning and attitude determination of the magnetic sensor. The experimental results show that the system can simultaneously position the positions of multiple magnetic sensors through the magnetic measurement data of one rotation, without quantity limitation, which proves its effectiveness and generalization.

[0144] Embodiment 3

[0145] An embodiment of the present invention provides an electronic device, which includes a memory, a processor, and a computer program stored in the memory and executable on the processor. The memory is used to store software programs and modules. The processor executes various functional applications and data processing by running the software programs and modules stored in the memory. The memory and the processor are connected through a bus. Specifically, when the processor runs the computer program stored in the memory, any step in the first embodiment is implemented.

[0146] It should be understood that in the embodiment of the present invention, the so-called processor may be a central processing unit (CPU), and the processor may also be other general-purpose processors, digital signal processors (DSPs), application specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc.

[0147] The memory may include a read-only memory, a flash memory, and a random access memory, and provide instructions and data to the processor. A part or all of the memory may also include a non-volatile random access memory.

[0148] As can be seen from the above, the electronic device provided by the embodiment of the present invention can implement the method for positioning and attitude determination of a three-axis magnetic sensor based on a magnetic beacon as described in the first embodiment by running a computer program. Through the magnetic measurement data obtained by rotating one week, the positions of multiple magnetic sensors can be located simultaneously without any quantity limitation.

[0149] It should be understood that if the above integrated modules / units are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, to implement all or part of the processes in the above-described embodiments of the method of the present invention, it can also be completed by a computer program instructing related hardware. The above computer program can be stored in a computer-readable storage medium. When the computer program is executed by a processor, the steps of the above-described method embodiments can be implemented. Among them, the above computer program includes computer program code, and the above computer program code can be in the form of source code, object code, executable file, or some intermediate form, etc. The above computer-readable medium can include: any entity or device capable of carrying the above computer program code, recording medium, USB flash drive, mobile hard disk, magnetic disk, optical disc, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signal, telecommunication signal, and software distribution medium, etc. It should be noted that the content included in the above computer-readable storage medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction.

[0150] The above description of the disclosed embodiments enables those skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to these embodiments shown herein, but rather to the broadest scope consistent with the principles and novel features disclosed herein.

[0151] Those skilled in the art can clearly understand that for the convenience and brevity of description, only the above division of each functional unit and module is used as an example. In actual applications, the above functions can be allocated to different functional units and modules according to needs, that is, the internal structure of the above device can be divided into different functional units or modules to complete all or part of the functions described above. Each functional unit and module in the embodiments can be integrated into a processing unit, or each unit can exist physically alone, or two or more units can be integrated into one unit. The above integrated unit can be implemented in the form of hardware or in the form of a software functional unit. In addition, the specific names of each functional unit and module are only for the convenience of mutual distinction and do not limit the protection scope of the present invention. The specific working processes of the units and modules in the above system can refer to the corresponding processes in the foregoing method embodiments and will not be elaborated herein.

[0152] It should be noted that the methods and their detailed examples provided in the above embodiments can be combined with the devices and equipment provided in the embodiments and can be referred to each other, and will not be elaborated here.

[0153] Those of ordinary skill in the art can realize that the units and algorithm steps of each example described in combination with the embodiments disclosed herein can be implemented by electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are executed in a hardware or software manner depends on the specific application and design constraints of the technical solution. Professional technicians can use different methods for each specific application to implement the described functions, but such implementation should not be considered to exceed the scope of the present invention.

[0154] In the embodiments provided by the present invention, it should be understood that the disclosed devices / terminal devices and methods can be implemented in other ways. For example, the device / equipment embodiments described above are only illustrative. For example, the above division of modules or units is only a logical function division. In actual implementation, there can be other division methods. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed.

[0155] The above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements for some of the technical features. These modifications or replacements 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 included in the protection scope of the present invention.

Claims

1. A three-axis magnetic sensor positioning and attitude determination method based on magnetic beacons, characterized in that: The method comprises the following steps: Step 1: Use spherical harmonics to express the distribution of the magnetic field generated by the magnetic beacon at any position in space; Step 2: Based on the position of the magnetic beacon, confirm the relative position of the magnetic beacon and the magnetic sensor; Step 3: Based on the relative position in step 2, establish the positioning and attitude equation of the magnetic sensor; Step 4: For the equation in step 3, use the particle swarm algorithm to solve the spherical coordinates; Step 5: Convert the spherical coordinates obtained in step 4 into Cartesian coordinates to complete the positioning and attitude determination of the magnetic sensor.

2. The method according to claim 1, characterized in that: Specifically, step 1 is to express the magnetic field distribution generated by the magnetic beacon through spherical harmonics. The three-component expression of the magnetic beacon in spherical coordinates is as follows: In the formula, j is the order of the spherical harmonic function, m is the degree of the spherical harmonic function, and a jm and b jm are the spherical harmonic coefficients, r, θ and φ are the three coordinates of the sensor in spherical coordinates, is the Schmidt quasi-normalized associated Legendre function; The magnetic beacon uses 3-axis orthogonal coils; the three orthogonal coils correspond exactly to the 3-axis orthogonal magnetic moment vector M x , M y and M z ; In equations (1)-(3), if the spherical harmonic order j is set to 1, the magnetic field expression generated by the magnetic dipole in space is obtained as follows: From the multi-level expansion law of spherical harmonics, we can know that: In the formula, I x ,I y and I z is the triaxial coil current, S x , S y and S z is the cross-sectional area of ​​the triaxial coil, N x 、N y and N z is the number of turns of the triaxial coil.

3. The method according to claim 1, characterized in that: Specifically, step 2 is as follows: the center of the magnetic beacon is used as the origin of the global coordinate system; the spherical coordinates of the location of the magnetic sensor are (r, θ, φ); without considering the three-axis rotation of the magnetic sensor, the three components of the magnetic field at the center of the magnetic sensor can be expressed as follows: The rotation of a vector is represented by a three-axis rotation matrix as follows: Where Rot(x,α), Rot(y,β) and Rot(z,γ) represent the rotation matrices of rotating around the x-axis by an angle of α, around the y-axis by an angle of β and around the z-axis by an angle of γ respectively; Assuming that the magnetic sensor is first rotated by an angle α around the x-axis, then by an angle β around the y-axis, and finally by an angle γ around the z-axis, the final reading of the sensor is expressed as: Substituting equations (8)-(11) into (12), we get the three-component expression of the magnetic sensor: In the formula, B sx , B sy and B sz These are the x-axis, y-axis, and z-axis readings of the magnetic sensor, respectively.

4. The method according to claim 3, characterized in that: Specifically, the step 3 is to place the magnetic beacon on a non-magnetic turntable, set the origin of the reference coordinate system at the center of the magnetic beacon, and align the three axes of the coordinate system with the axes of the three-axis coil in the magnetic beacon; Rotate the non-magnetic turntable 360 ​​degrees around the z-axis and record the reading of the magnetic sensor every 10 degrees. Since the magnetic beacon rotates 360 degrees around the z-axis, at this time, B in equations (13)-(15) r , B θ and B φ Written in the following form: In the formula, As the rotation gradually increases from 0 to 2π, one cycle of rotation is completed; The zero-order and first-order Fourier coefficients of the three-axis readings of the magnetic sensor are calculated based on the magnetic measurement data as follows: In the formula, A x0 , A y0 and A z0 are the zero-order Fourier coefficients of the x-axis, y-axis, and z-axis readings of the magnetic sensor, respectively; 5. The method according to claim 4, characterized in that: Substituting equations (13)-(18) into equations (19) and (20), we can obtain the following expressions after simplification: The positioning and attitude determination of the magnetic sensor requires solving six unknowns, namely r, θ, φ, α, β and γ. The positioning and attitude determination of the magnetic sensor can be completed through equations (21)-(29) using the least squares method.

6. The method according to claim 1, characterized in that: Specifically, step 4 uses a particle swarm algorithm to solve the above equation, and the value ranges of the objective function and the six unknowns are set as follows: Objective function: In the formula, A x0 实测 Represents the Fourier coefficient calculated from the actual reading of the magnetic sensor; A x0 The right side of the expression (21) is: The variable value range is: According to the above settings, the PSO algorithm can be used to calculate the six unknowns of the magnetic sensor and obtain the spherical coordinates of the magnetic sensor.

7. The method according to claim 6, characterized in that: Specifically, step 5 is to convert the spherical coordinates of the magnetic sensor into Cartesian coordinates: At this point, the positioning and posture determination of the magnetic sensor is achieved.

8. A three-axis magnetic sensor positioning and attitude determination system based on magnetic beacons, characterized in that: The system adopts the three-axis magnetic sensor positioning and attitude determination method based on magnetic beacons as claimed in any one of claims 1 to 7, and the system includes: Magnetic beacon magnetic field expression module: expresses the distribution of the magnetic field generated by the magnetic beacon at any position in space through spherical harmonic functions; Relative position confirmation module of magnetic beacon and magnetic sensor: based on the position of magnetic beacon, confirm the relative position of magnetic beacon and magnetic sensor; Equation establishment module: Based on the relative position of the magnetic beacon and the magnetic sensor, the magnetic sensor is used to establish the positioning and attitude determination equation; Spherical coordinates solving module: For the equation in step 3, the particle swarm algorithm is used to solve the spherical coordinates; Coordinate conversion module: converts the solved spherical coordinates into Cartesian coordinates to realize the positioning and attitude determination of the magnetic sensor.

9. A computer device, characterized in that: The method comprises a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the method according to any one of claims 1 to 7 is implemented.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method according to any one of claims 1 to 7 is implemented.

Images