A method and system for positioning and orientation of a three-axis magnetic sensor based on magnetic beacon

Abstract

The application belongs to the technical field of magnetic field navigation, and particularly relates to a three-axis magnetic sensor positioning and pose determination method and system based on a magnetic beacon. Step 1: the distribution of the magnetic field generated by the magnetic beacon at any position in space is expressed by a spherical harmonic function; Step 2: based on the position of the magnetic beacon, the relative position of the magnetic beacon and the magnetic sensor is confirmed; Step 3: based on the relative position of Step 2, a positioning and pose determination equation of the magnetic sensor is established; Step 4: the equation of Step 3 is solved by using a particle swarm algorithm to obtain spherical coordinates; Step 5: the spherical coordinates solved in Step 4 are converted into Cartesian coordinates, and the positioning and pose determination of the magnetic sensor are completed. The spatial coordinates 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 measured, thereby supporting the technical development in the related field.

Description

Technical Field

[0001] This invention belongs to the field of magnetic field navigation technology, specifically relating to a three-axis magnetic sensor positioning and attitude determination method and system based on magnetic beacons. Background Technology

[0002] Research in fields such as spacecraft magnetic measurement and modeling, geomagnetic navigation, UAV aerial surveying, and resource exploration all rely 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, these fields have stringent requirements for the measurement and calibration of the position and attitude of magnetic sensors. Furthermore, with the application of technologies such as intelligent unmanned systems and the Internet of Things in indoor environments, higher demands are being placed on indoor navigation and positioning technologies. Low-cost, high-precision, and easy-to-maintain indoor navigation and positioning solutions are one of the key research directions in the field of indoor navigation technology. In indoor environments where satellite navigation is unavailable, methods such as ultra-wideband (UWB), vision, lidar, and inertial navigation cannot provide high-precision and reliable navigation and positioning services for extended periods due to environmental limitations and their inherent factors. Summary of the Invention

[0003] This invention provides a method for positioning and attitude determination of a triaxial magnetic sensor based on a magnetic beacon. The method accurately calculates the spatial coordinates and attitude of the magnetic sensor by using the magnetic field generated by the magnetic beacon at the magnetic sensor under test, thereby supporting the technological development in related fields.

[0004] The present invention also provides a triaxial magnetic sensor positioning and attitude determination system based on magnetic beacons, which is used to realize a triaxial magnetic sensor positioning and attitude determination method based on magnetic beacons.

[0005] This invention is achieved through the following technical solution:

[0006] A method for positioning and attitude determination using a triaxial magnetic sensor based on a magnetic beacon, the method comprising the following steps:

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

[0008] Step 2: Based on the location 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 the positioning and attitude determination equations for the magnetic sensor;

[0010] Step 4: Solve the equations from Step 3 using the particle swarm optimization algorithm to obtain the spherical coordinates;

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

[0012] Furthermore, step 1 specifically involves expressing the magnetic field distribution generated by the magnetic beacon using spherical harmonic functions. The three-component expression of the magnetic beacon in spherical coordinates is shown below:

[0013]

[0014] 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 These are the spherical harmonic coefficients, and r, θ, and φ are the three coordinates of the sensor in spherical coordinates. For the Schmitt quasi-normalized associated Legendre function;

[0015] The magnetic beacon uses three-axis orthogonal coils; the three orthogonal coils correspond exactly to the three-axis orthogonal magnetic moment vector M. x M y and M z ;

[0016] In equations (1)-(3), if the spherical harmonic order j is set to 1, then the magnetic field generated by the magnetic dipole in space is expressed as follows:

[0017]

[0018] From the multi-level expansion law of spherical harmonic functions, we can know that:

[0019]

[0020] In the formula, I x I y and I z It is the current in the triaxial coil, S x S y and S z It is the cross-sectional area of ​​the triaxial coil, N x N y and N z It refers to the number of turns of the triaxial coil.

[0021] Furthermore, step 2 specifically involves using the center of the magnetic beacon as the origin of the global coordinate system; the spherical coordinates at 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:

[0022]

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

[0024]

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

[0026] Assuming 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, the final reading of the sensor will be expressed as follows:

[0027]

[0028] Substituting equations (8)-(11) into (12), we obtain the three-component expression for the magnetic sensor:

[0029]

[0030] 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.

[0031] Furthermore, step 3 specifically involves placing the magnetic beacon on a non-magnetic turntable, setting the origin of the reference coordinate system at the center of the magnetic beacon, and aligning the three axes of the coordinate system with the axes of the three-axis coils in the magnetic beacon.

[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 has rotated 360 degrees around the z-axis, at this time, B in equations (13)-(15) r B θ and B φ Write it in the following form:

[0033]

[0034] In the formula, As the rotation gradually increases from 0 to 2π, one cycle of rotation is completed.

[0035] The zeroth and first Fourier coefficients of the triaxial readings of the magnetic sensor were calculated based on the magnetic measurement data, as shown below:

[0036]

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

[0038]

[0039] Furthermore, substituting equations (13)-(18) into (19) and (20), we obtain the following simplified expression:

[0040]

[0041] The positioning and orientation determination of the magnetic sensor requires solving for 6 unknowns, namely r, θ, φ, α, β and γ; the positioning and orientation determination of the magnetic sensor can be completed by the least squares method using equations (21)-(29).

[0042] Furthermore, step 4 specifically involves solving the above equations using a particle swarm optimization algorithm. The ranges for the objective function and the six unknowns are set as follows:

[0043] Objective function:

[0044]

[0045] In the formula, A x0 实测 A represents the Fourier coefficients calculated from the actual readings of the magnetic sensor. x0 The right-hand side of expression (21);

[0046] The range of values ​​for the variable is:

[0047]

[0048] With the above settings, using the PSO algorithm, the six unknowns of the magnetic sensor can be calculated, and the spherical coordinates of the magnetic sensor can be obtained.

[0049] Furthermore, step 5 specifically involves converting the spherical coordinates of the magnetic sensor into Cartesian coordinates:

[0050]

[0051] This completes the positioning and attitude determination of the magnetic sensor.

[0052] A triaxial magnetic sensor positioning and attitude determination system based on magnetic beacons, the system employing the aforementioned triaxial magnetic sensor positioning and attitude determination method based on magnetic beacons, the system comprising:

[0053] Magnetic beacon magnetic field representation module: The distribution of the magnetic field generated by the magnetic beacon at any location in space is expressed using spherical harmonic functions;

[0054] Magnetic beacon and magnetic sensor relative position confirmation module: Based on the position of the magnetic beacon, confirm the relative position of the magnetic beacon and the magnetic sensor;

[0055] Equation Establishment Module: Based on the relative positions of the magnetic beacon and the magnetic sensor, establish the positioning and attitude determination equations for the magnetic sensor;

[0056] Spherical coordinate solution module: The spherical coordinates are obtained by solving the equation in step 3 using the particle swarm optimization algorithm;

[0057] Coordinate transformation module: Converts the solved spherical coordinates into Cartesian coordinates to achieve positioning and orientation determination of the magnetic sensor.

[0058] A computer device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the computer program, implements the method described above.

[0059] A computer-readable storage medium storing a computer program that, when executed by a processor, implements the method described above.

[0060] The beneficial effects of this invention are:

[0061] This invention enables 6D magnetic sensor positioning and attitude determination with more accurate results.

[0062] This invention can simultaneously locate the positions of multiple magnetic sensors by using magnetic measurement data from one rotation, with no limit on the number.

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

[0064] Figure 1 This is a schematic diagram showing the relative position of the magnetic sensor and the magnetic beacon of the present invention.

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

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

[0067] In the following description, specific details such as particular system architectures and techniques are set forth for illustrative purposes and not for limitation, in order to provide a thorough understanding of the embodiments of this application. However, those skilled in the art will understand that this application may also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, apparatuses, circuits, and methods are omitted so as not to obscure the description of this application with unnecessary detail.

[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, integrals, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or collections thereof.

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

[0070] The following is in conjunction with the appendix to this application specification. Figure 1-3 The technical solutions in the embodiments of this application are clearly and completely described. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of this application.

[0071] Many specific details are set forth in the following description in order to provide a full understanding of this application. However, 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 spirit of this application. Therefore, this application is not limited to the specific embodiments disclosed below.

[0072] Implementation Method 1

[0073] The present invention provides a method for positioning and attitude determination of a triaxial magnetic sensor based on a magnetic beacon, the method comprising the following steps:

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

[0075] Step 2: Based on the location 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 the positioning and attitude determination equations for the magnetic sensor;

[0077] Step 4: Solve the equations from Step 3 using the Particle Swarm Optimization (PSO) algorithm to obtain the spherical coordinates;

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

[0079] Furthermore, step 1 specifically involves first expressing the magnetic field distribution generated by the magnetic beacon using spherical harmonic functions. The three-component expression of the magnetic beacon in spherical coordinates is shown below:

[0080]

[0081]

[0082] 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 These are the spherical harmonic coefficients, and r, θ, and φ are the three coordinates of the sensor in spherical coordinates. For the Schmitt quasi-normalized associated Legendre function;

[0083] Here, the magnetic beacon uses three-axis orthogonal coils. According to the principle of magnetic dipoles, when the distance between the magnetic sensor and the magnetic beacon is greater than three times the size of the magnetic beacon, the magnetic beacon can be used as a magnetic dipole. Therefore, the three orthogonal coils correspond exactly to the three-axis orthogonal magnetic moment vector M. x M y and M z ;

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

[0085]

[0086] According to the multi-stage expansion law of spherical harmonic functions, the first-order spherical harmonic coefficients and the triaxial magnetic moments of the magnetic beacon have the following relationship:

[0087]

[0088] In the formula, I x I y and I z It is the current in the triaxial coil, S x S y and S z It is the cross-sectional area of ​​the triaxial coil, N x N y and N z It is the number of turns of the triaxial coil; therefore, as long as the electromagnetic parameters and input current of the magnetic beacon are known, its magnetic field at any position in space can be calculated by equations (4)-(7).

[0089] Furthermore, step 2 specifically involves determining the relative positions of the magnetic beacon and the magnetic sensor as follows: Figure 1As shown; with the center of the magnetic beacon as the origin of the global coordinate system; the spherical coordinates of the location of the magnetic sensor 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] Equation (8) only considers the change in spatial position, and the attitude information of the three axes needs to be added in; the rotation of the vector can be represented by the three-axis rotation matrix, as shown below:

[0092]

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

[0094] Assuming 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, the final reading of the sensor can be expressed as:

[0095]

[0096] Substituting equations (8)-(11) into (12), we can obtain the three-component expression for the magnetic sensor:

[0097] 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.

[0098] Furthermore, step 3 specifically involves, as follows: Figure 1 As shown, a magnetic beacon is placed on a non-magnetic turntable, and the origin of the reference coordinate system is set at the center of the magnetic beacon. The three axes of the coordinate system are aligned with the axes of the three-axis coils in the magnetic beacon. Only one sensor is shown in the figure, but this method does not limit the number of sensors or their specific placement. It can simultaneously complete the spatial positioning and orientation of any number of sensors.

[0099] Rotate the non-magnetic turntable 360 ​​degrees around the z-axis, and record the reading of the magnetic sensor every 10 degrees (the step angle can be adjusted as needed); since the magnetic beacon has rotated 360 degrees around the z-axis, at this time, B in equations (13)-(15) r B θ and B φ It can be written in the following form:

[0100]

[0101] In the formula, As the rotation gradually increases from 0 to 2π, one cycle of rotation is completed.

[0102] The zeroth and first Fourier coefficients of the triaxial readings of the magnetic sensor were calculated based on the magnetic measurement data, as shown below:

[0103]

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

[0105]

[0106] Furthermore, substituting equations (13)-(18) into (19) and (20), we can simplify to obtain the following expression:

[0107]

[0108] The positioning and attitude determination of a magnetic sensor requires solving for six unknowns: r, θ, φ, α, β, and γ. There are nine boundary value equations as shown above. Therefore, the positioning and attitude determination of the magnetic sensor can be achieved using equations (21)-(29) via the least squares method. The Fourier coefficients on the left side of equations (21)-(29) can be calculated using measured data combined with equations (19) and (20), and the magnetic moment a on the right side... 11 b 11 and a 10 It can be calculated by combining the current and formula (7).

[0109] Furthermore, step 4 specifically involves solving the above equations using the Particle Swarm Optimization (PSO) algorithm, with the objective function and the ranges for the six unknowns set as follows:

[0110] Objective function:

[0111]

[0112] In the formula, A x0 实测 A represents the Fourier coefficients calculated from the actual readings of the magnetic sensor. x0 The right-hand side of expression (21); the definitions of other Fourier coefficients are also in the same manner;

[0113] The range of values ​​for the variable is:

[0114]

[0115] With the above settings, using the PSO algorithm, the six unknowns of the magnetic sensor can be calculated, and the spherical coordinates of the magnetic sensor can be obtained.

[0116] Furthermore, step 5 specifically involves converting the spherical coordinates of the magnetic sensor into Cartesian coordinates:

[0117]

[0118] This completes the positioning and attitude determination of the magnetic sensor.

[0119] The inversion verification is performed using manually calculated data, and the specific method is as follows:

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

[0121] When the magnetic beacon rotates one revolution, the calculated magnetic field value at target point 1, coordinates (0.5, 0, 0.5), is as follows: Figure 2 As shown in (a). Then, first rotate the sensor 10 degrees around the x-axis, then 20 degrees around the y-axis, and finally...

[0122] Rotating it 30 degrees around the z-axis, its reading becomes as follows: Figure 2 As shown in (b).

[0123] Will Figure 2 Substituting the data from (b) into (19) and (20) respectively, we can obtain the zeroth and first order Fourier transforms.

[0124] Riemann coefficient.

[0125]

[0126] Substituting (33) into (30), the spatial coordinates of the magnetic sensor can be obtained by calculating using the PSO algorithm.

[0127] And the posture is as follows:

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

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

[0130] Following the same method, the coordinates of the other seven 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 ​​and predicted values ​​are basically consistent, which verifies the results of this study.

[0131] The accuracy and effectiveness of the positioning and orientation method proposed in the patent.

[0132] Table 1 Localization results of different target points

[0133]

[0134] Implementation Method 2

[0135] This invention provides a triaxial magnetic sensor positioning and attitude determination system based on magnetic beacons.

[0136] The system employs the aforementioned triaxial magnetic sensor positioning and attitude determination method based on magnetic beacons. The system includes:

[0137] Magnetic beacon magnetic field expression module: Expresses the magnetic field generated by the magnetic beacon at any position in space using spherical harmonic functions.

[0138] Distribution of placement locations;

[0139] Magnetic beacon and magnetic sensor relative position confirmation module: Based on the position of the magnetic beacon, confirm the relative position of the magnetic beacon and the magnetic sensor;

[0140] Equation Establishment Module: Based on the relative positions of the magnetic beacon and the magnetic sensor, establish the positioning and attitude determination equations for the magnetic sensor;

[0141] Spherical coordinate solution module: The spherical coordinates are obtained by solving the equation in step 3 using the particle swarm optimization algorithm;

[0142] Coordinate transformation module: Converts the solved spherical coordinates into Cartesian coordinates to achieve positioning and orientation determination of the magnetic sensor.

[0143] As can be seen from the above, the embodiments of the present invention establish positioning and attitude determination equations for magnetic sensors using magnetic beacons and solve them. The triaxial magnetic sensor converts the solved spherical coordinates into Cartesian coordinates to achieve positioning and attitude determination of the magnetic sensors. Experimental results show that the system can simultaneously locate the positions of multiple magnetic sensors using magnetic measurement data from one rotation, without any limit on the number, thus verifying its effectiveness and generalization.

[0144] Implementation Method 3

[0145] This invention provides an electronic device including a memory, a processor, and a computer program stored in the memory and executable on the processor. The memory stores software programs and modules, and the processor executes various functional applications and data processing by running the software programs and modules stored in the memory. The memory and processor are connected via a bus. Specifically, the processor implements any step in Embodiment 1 by running the computer program stored in the memory.

[0146] It should be understood that, in the embodiments of the present invention, the processor may be a central processing unit (CPU), or it may 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. A general-purpose processor may be a microprocessor or any conventional processor.

[0147] Memory may include read-only memory, flash memory, and random access memory, and provides instructions and data to the processor. Some or all of the memory may also include non-volatile random access memory.

[0148] As can be seen from the above, the electronic device provided by the embodiments of the present invention can implement the three-axis magnetic sensor positioning and attitude determination method based on magnetic beacons as described in Embodiment 1 by running a computer program. By using the magnetic measurement data of one rotation, the positions of multiple magnetic sensors can be located simultaneously without any limit on the number.

[0149] It should be understood that if the integrated modules / units described above are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods described above can also be implemented by a computer program instructing related hardware. This computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content contained in the 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 make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

[0151] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is merely an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, 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. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit. Furthermore, the specific names of the functional units and modules are only for easy differentiation and are not intended to limit the scope of protection of this invention. The specific working process of the units and modules in the above system can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0152] It should be noted that the methods and detailed examples provided in the above embodiments can be incorporated into the apparatus and devices provided in the embodiments for mutual reference, and will not be repeated here.

[0153] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0154] In the embodiments provided by this invention, it should be understood that the disclosed apparatus / terminal devices and methods can be implemented in other ways. For example, the apparatus / device embodiments described above are merely illustrative. For instance, the division of modules or units described above is merely a logical functional division, and in actual implementation, it can be divided in other ways. 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 skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be included within the protection scope of the present invention.

Claims

1. A method for positioning and attitude determination using a triaxial magnetic sensor based on a magnetic beacon, characterized in that, The method includes the following steps: Step 1: Express the distribution of the magnetic field generated by the magnetic beacon at any location in space using spherical harmonic functions; Step 2: Based on the location of the magnetic beacon, confirm the relative position between the magnetic beacon and the magnetic sensor; Step 2 specifically involves using the center of the magnetic beacon 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: (2) The rotation of a vector is represented by a three-axis rotation matrix, as shown below: (3) (4) (5) In the formula, Rot ( x , α ), Rot ( y , β )and Rot ( z , γ ) respectively represent the revolving x Axis rotation α horn ， Around y Axis rotation β Angle and wrap z Axis rotation γ The rotation matrix of the angle; Step 3: Based on the relative position in Step 2, establish the positioning and attitude determination equations for the magnetic sensor; Step 3 specifically involves placing the magnetic beacon on a non-magnetic turntable, setting the origin of the reference coordinate system at the center of the magnetic beacon, and aligning the three axes of the coordinate system with the axes of the three-axis coils in the magnetic beacon. by z The axis rotates a non-magnetic turntable 360 ​​degrees, and the readings of the magnetic sensor are recorded every 10 degrees. Based on the magnetic measurement data, the zero-order and first-order Fourier coefficients of the triaxial readings of the magnetic sensor are calculated. The magnetic sensor's positioning and attitude determination requires solving for six unknowns, namely... r , θ , , α , β and γ The magnetic sensor is positioned and its orientation determined using the least squares method. Step 4: Solve the equations from Step 3 using the particle swarm optimization algorithm to obtain the spherical coordinates; Step 5: Convert the spherical coordinates obtained in Step 4 into Cartesian coordinates to complete the positioning and orientation determination of the magnetic sensor.

2. The method according to claim 1, characterized in that, Step 1 specifically involves expressing the magnetic field distribution generated by the magnetic beacon using spherical harmonic functions. The three-component expression of the magnetic beacon in spherical coordinates is shown below: (6) (7) (8) In the formula, j It is the order of the spherical harmonic function. m It is the degree of the spherical harmonic function. a jm and b jm It is the spherical harmonic coefficient. r , θ and These are the three coordinates of the sensor in spherical coordinates. For the Schmitt quasi-normalized associated Legendre function; The magnetic beacon uses three-axis orthogonal coils; the three orthogonal coils correspond exactly to the three-axis orthogonal magnetic moment vectors. M x , M y and M z ; In equations (1)-(3), the spherical harmonic order is... j If we set it to 1, we get the following expression for the magnetic field produced by the magnetic dipole in space: (9) (10) (11) From the multi-level expansion law of spherical harmonic functions, we can know that: (12) In the formula, I x , I y and I z It is the current of the triaxial coil. S x , S y and S z It is the cross-sectional area of ​​the triaxial coil. N x , N y and N z It refers to the number of turns of the triaxial coil.

3. The method according to claim 2, characterized in that, Step 2 specifically involves assuming that the magnetic sensor first uses... x Axis rotation α Angle, then around y Axis rotation β Corner, finally around z Axis rotation γ If the angle is given, the final reading of the sensor is expressed as: (13) Substituting equations (8)-(11) into (12), we obtain the three-component expression for the magnetic sensor: (14) (15) (16) In the formula, B sx , B sy and B sz These are magnetic sensors. x axis, y shaft and z Axis readings.

4. The method according to claim 3, characterized in that, Specifically, step 3 involves, due to the magnetic beacon orbiting... z The axis rotates 360 degrees. At this time, in equations (13)-(15), B r , B θ and B Write it in the following form: (17) (18) (19) In the formula, φ As the rotation gradually increases from 0 to 2 π This completes one cycle of rotation; The zeroth and first Fourier coefficients of the triaxial readings of the magnetic sensor were calculated based on the magnetic measurement data, as shown below: (20) In the formula, A x0 , A y0 and A z0 These are magnetic sensors x axis, y shaft and z The zeroth-order Fourier coefficients of the axis readings; (21)。 5. The method according to claim 4, characterized in that, Substituting equations (13)-(18) into (19) and (20), we obtain the following simplified expression: (22) (23) (24) (25) (26) (27) (28) (29) (30)。 6. The method according to claim 1, characterized in that, Step 4 specifically involves solving the above equations using a particle swarm optimization algorithm. The ranges for the objective function and the six unknowns are set as follows: Objective function: (31) In the formula, A x0 实测 This represents the Fourier coefficients calculated from the actual readings of the magnetic sensor; A x0 The right-hand side of expression (21); The range of values ​​for the variable is: (32) With the above settings, using the PSO algorithm, the six unknowns of the magnetic sensor can be calculated, and the spherical coordinates of the magnetic sensor can be obtained.

7. The method according to claim 6, characterized in that, Step 5 specifically involves converting the spherical coordinates of the magnetic sensor into Cartesian coordinates: (32) This completes the positioning and attitude determination of the magnetic sensor.

8. A triaxial magnetic sensor positioning and attitude determination system based on magnetic beacons, characterized in that, The system employs the triaxial magnetic sensor positioning and attitude determination method based on magnetic beacons as described in any one of claims 1-7, and the system comprises: Magnetic beacon magnetic field representation module: The distribution of the magnetic field generated by the magnetic beacon at any location in space is expressed using spherical harmonic functions; Magnetic beacon and magnetic sensor relative position confirmation module: Based on the position of the magnetic beacon, confirm the relative position of the magnetic beacon and the magnetic sensor; Equation Establishment Module: Based on the relative positions of the magnetic beacon and the magnetic sensor, establish the positioning and attitude determination equations for the magnetic sensor; Spherical coordinate solution module: The spherical coordinates are obtained by solving the equation in step 3 using the particle swarm optimization algorithm; Coordinate transformation module: Converts the solved spherical coordinates into Cartesian coordinates to achieve positioning and orientation determination of the magnetic sensor.

9. A computer device, characterized in that, It includes 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, it implements the method as described in any one of claims 1-7.

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

Images