Head-mounted pedestrian positioning method of nine-axis sensor array and head-mounted device

Abstract

The invention discloses a head-mounted pedestrian positioning method of a nine-axis sensor array and a head-mounted device, and belongs to the technical field of pedestrian positioning. A head-mounted pedestrian positioning method based on a nine-axis sensor array comprises the following steps that a data processing unit and a plurality of sensors are installed on a head-mounted device, the sensors comprise inertial measurement units and magnetometers, and the sensors form a sensor array for data acquisition; the inertial sensor array formed by the plurality of inertial measurement units is calculated through strapdown inertial navigation; an environment magnetic field distribution model is obtained through magnetometer array observation fitting, the absolute speed of a carrier is estimated by combining magnetic field model changes continuously observed by the magnetometer array, then position, speed and attitude calculation of an inertial navigation system based on the inertial measurement unit array is assisted, and the defect that a traditional inertial navigation system is fast in error divergence is overcome. The positioning precision is obviously improved, and indoor pedestrian positioning independent of pedestrian motion hypothesis and a pre-established magnetic field fingerprint database is realized.

Description

Technical Field

[0001] The present invention belongs to the technical field of pedestrian positioning, and in particular relates to a head-mounted pedestrian positioning method and a head-mounted device of a nine-axis sensor array. Background Art

[0002] With the widespread application of location-based services (LBS), the demand for high-precision positioning is increasing. In outdoor environments, the Global Navigation Satellite System (GNSS) can provide relatively accurate positioning services. However, in indoor environments where GNSS signals cannot be covered or are interfered with, achieving accurate and reliable positioning is still a problem that needs to be solved urgently.

[0003] Current indoor positioning solutions can be roughly divided into two categories: infrastructure-dependent positioning solutions (such as pseudo-satellites, WiFi, Bluetooth low energy positioning [BLE], 5G positioning, etc.) and infrastructure-independent positioning solutions (such as visual positioning, magnetic field matching, and dead reckoning). Infrastructure-dependent solutions require the deployment of professional signal transceivers in the positioning environment, which not only increases the positioning cost, but also the signal propagation process is easily interfered by various factors, limiting its availability. Among the methods that do not rely on infrastructure, visual positioning is greatly affected by ambient lighting, occlusion, and dynamic changes, and the device has high power consumption, which limits its wide application in practical applications; magnetic field matching relies on a pre-established magnetic field fingerprint database, and the system construction and maintenance costs are high.

[0004] Disadvantages of existing technology

[0005] In indoor pedestrian positioning, the commonly used Pedestrian Dead Reckoning (PDR) method has the advantages of independent calculation and no interference from external information sources. Therefore, it is often used as the preferred solution for indoor pedestrian positioning. However, the traditional inertial measurement unit (IMU) is usually low-cost and low-precision, which leads to the rapid divergence of the error of the strapdown inertial navigation algorithm. In order to improve the positioning accuracy, researchers try to find other positioning information sources to assist the inertial navigation system (INS) in calculation. For example, the vision-based auxiliary solution is highly sensitive to lighting, occlusion and dynamic environment, and is easily affected by environmental changes and feature loss, thereby reducing the accuracy and stability of positioning; the positioning solution based on lidar is limited in the mass pedestrian positioning scenario due to line-of-sight limitations, high costs and high computing requirements.

[0006] In addition, currently widely used auxiliary solutions include foot-mounted inertial navigation system (Foot-INS) and step-and-heading system (SHS). Foot-INS uses the characteristics of pedestrians' foot periodically contacting the ground to obtain virtual zero speed, and controls the divergence of inertial navigation speed error through zero velocity update technology (ZUPT). However, this sensor installation method is not friendly to pedestrians and has low convenience.

[0007] SHS mainly includes gait detection, step length estimation, heading estimation and position update, among which step length estimation and heading estimation are the main sources of error. The step length model in step length estimation has poor applicability to different users and is prone to introduce positioning scale errors; heading estimation causes direction drift due to sensor noise accumulation and environmental interference (such as geomagnetic distortion and dynamic magnetic field disturbance), which in turn causes rotation and offset errors of the trajectory. Moreover, the sensor heading and pedestrian movement direction cannot be accurately estimated, resulting in a significant decrease in the availability of SHS.

[0008] In recent years, magnetic field odometry has been proposed as a lightweight pedestrian speed measurement method, using magnetometer arrays to provide odometer information such as position or speed. For example, Vissière et al. first proposed using indoor magnetic field disturbances to improve IMU-based position and speed estimation in 2007. In subsequent studies, Dorveaux (2011) proposed magnetic inertial navigation (MINAV) technology, and Chesneau (2016) fused magnetometers with inertial sensors and used extended Kalman filters for positioning. In recent years, related studies have described the local magnetic field through polynomial models, and combined the magnetic field model with INS errors for Kalman filtering, further improving the positioning accuracy (Huang et al.). The current magnetic odometry-related methods are seriously affected by magnetic field gradient noise, and the method of combining the magnetic field model with INS errors for filtering is highly dependent on the accurate modeling of the magnetic field model, and the positioning results are less stable. Summary of the invention

[0009] The purpose of the present invention is to address the problems existing in the prior art and to provide a head-mounted pedestrian positioning method and a head-mounted device with a nine-axis sensor array.

[0010] To achieve the above object, the invention adopts the following technical solution: a head-mounted pedestrian positioning method of a nine-axis sensor array, comprising the following steps:

[0011] A data processing unit and a plurality of sensors are installed on the head mounted device, wherein the sensors include an inertial measurement unit and a magnetometer, and the plurality of sensors form a sensor array for data collection;

[0012] The inertial sensor array formed by the multiple inertial measurement units calculates the current position, speed and attitude through strapdown inertial navigation solution; the magnetometer array formed by the multiple magnetometers measures the magnetic field and obtains the observation value;

[0013] The observed values ​​estimate the magnetic field model parameters at the current moment, and combine the magnetic field model parameters at adjacent moments to obtain the magnetic field model parameter changes; construct an error transfer model between the magnetic field model and the relative posture, and obtain the conversion relationship between the magnetic field model parameter changes and the relative posture changes;

[0014] A relative posture constraint algorithm is constructed to fuse the magnetic field model parameter changes of the magnetometer array and the relative posture obtained by inertial navigation integration, and output the current position, speed and posture.

[0015] By adopting the above technical solutions, high-precision positioning is achieved through multi-sensor fusion, and the magnetometer array is used to observe the changes in the magnetic field model to assist in inertial navigation, which effectively suppresses the error accumulation problem of traditional inertial navigation calculation and significantly improves the positioning accuracy; the multi-sensor distribution design enhances the robustness of the system and reduces the impact of a single sensor failure on the positioning results; the real-time data processing unit ensures the real-time and reliability of the positioning results, and is suitable for dynamic pedestrian positioning scenarios.

[0016] Optionally, the magnetometer types include vector magnetometer, scalar magnetometer and gradient magnetometer.

[0017] By adopting the above technical solutions, the combination of multiple magnetometers can adapt to different magnetic field environments and improve the accuracy and reliability of magnetic field measurement. At the same time, the diversity of magnetometers enhances the system's perception of complex magnetic field changes and further improves positioning accuracy.

[0018] Optionally, the helmet is provided with a barometer and a thermometer.

[0019] By adopting the above technical solution, a barometer and a thermometer are added to the helmet, which expands the function of the sensor and is capable of measuring the air pressure and temperature of the environment. The barometer can assist in locating mid- / high-level floor information.

[0020] Optionally, the number of the sensors is greater than or equal to 3, and the installation positions of the multiple sensors are not on the same straight line.

[0021] By adopting the above technical solutions, the rationality of the spatial distribution of the sensor array is ensured, the measurement accuracy is improved, and the optimized sensor layout enhances the system's anti-interference ability and positioning accuracy, which is suitable for pedestrian positioning in complex environments.

[0022] Optionally, the strapdown inertial navigation solution method includes using one inertial measurement unit in the array for solution, averaging the observation values ​​of the inertial measurement unit array and then solving, and using each inertial measurement unit in the array to solve separately and then averaging the navigation results.

[0023] By adopting the above technical solutions, a variety of strapdown inertial navigation solution methods are provided (single-point solution, average solution, multi-point independent solution), which increases the flexibility and adaptability of the system.

[0024] Optionally, the relative pose constraint method includes extended Kalman filter, unscented Kalman filter, particle filter, state Lonker Kalman filter, least squares, graph optimization and factor graph; the data processing unit selects a suitable relative pose constraint method according to the positioning accuracy requirements and computing resources, and performs real-time evaluation and optimization of the constraint results.

[0025] By adopting the above technical solutions, a variety of relative posture constraint methods (extended Kalman filter, unscented Kalman filter, particle filter, etc.) are provided, which are suitable for different positioning accuracy requirements and computing resource conditions.

[0026] The perturbation equation of the magnetic field model is as follows:

[0027] M ^ j b j = C ^ i j F ^ ( l j b i ) i = ( C i j − [ d b g dt × ])( F ( l j b i ) + d F ( l j b i )) i = ( C i j − [ d b g dt × ])( F ( l j b i ) i + d F ( l j b i ) i ) = ( C i j − [ d b g dt × ])( F ( l j b i ) i + B ( i ) d l j b i ) ≈ C i j F ( l j b i ) i + C i j B ( i ) d l j b i − [ d b g dt × ] F ( l j b i ) i = M j b j + C i j B ( i ) d l j b i − [ d b g dt × ] M j b i = M j b j + C i j B ( i ) d l j b i + [ M j b i × ] d b g dt ,

[0028] B ( i ) = [ 2 i 4 i 6 i 7 i 6 2 i 5 i 8 i 7 i 8 − 2 ( i 4 + i 5 ) ] ,

[0029] l ^ j b i = C ^ j i l j b j +D r ^ b i = C ^ j i l j b j + C ^ n b i D r ^ n = C ^ j i l j b j + C ^ n b i v ^ j n dt = ( C j i + [ d b g dt × ]) l j b j + [ I + ϕ × ] C n b i ( v j n + d v j n ) dt ≈ C j i l j b j − [ l j b j × ] d b g dt + C n b i v j n dt + C n b i d v j n dt + [ ϕ × ] C n b i v j n dt = l j b i + C n b i d v j n dt − [ l j b j × ] d b g dt − [ D r b i × ] ϕ d l j b i = C n b i d v j n dt − [ l j b j × ] d b g dt − [ D r b i × ] ϕ ,

[0030] Combining the above two formulas:

[0031] M ^ j b j = M j b j + C i j B ( i ) d l j b i + [ M j b i × ] d b g dt = M j b j + C i j B ( i )( C n b i d v j n dt − [ l j b j × ] d b g dt − [ D r b i × ] ϕ ) + [ M j b i × ] d b g dt = M j b j + C i j B ( i ) C n b i d v j n dt + [[ M j b i × ] − C i j B ( i )[ l j b j × ]] dt d b g − C i j B ( i )[ D r b i × ] ϕ ,

[0032] In the formula, Represents the projection of the measurement value of the magnetometer array at the last moment in the bj system; represents the relative rotation between the b systems at adjacent moments; represents the position vector matrix; Indicates the magnetometer at the last moment The coordinates of the system; represents the magnetic field model parameters; represents the relative rotation between the two carrier coordinate systems at adjacent moments bi and bj; is the gyro bias error vector; Indicates the time difference between two adjacent moments, specifically and The time difference between two adjacent carrier coordinate systems; Represents a matrix constructed from position vectors, r= [ x y z ] T The constructed matrix; Represented by the position coordinate vector Constructed matrix; represents a matrix related to θ; Position vector The error of Represents the relative rotation between the two carrier coordinate systems bj and bi at adjacent moments; Indicates that the magnetometer was at the last moment The coordinates of the system; Indicates the position change of n systems at adjacent moments The projection under the system; Indicates that the coordinates are converted from the navigation coordinate system to the carrier coordinate system The rotation matrix of the system; Represents the projection of the position change of navigation coordinate system n at adjacent moments in navigation coordinate system n; : represents the velocity vector of the carrier in the navigation coordinate system n at the last moment; Represents a 3×3 unit matrix; is the attitude error vector; Represents the velocity vector the data processing unit performs real-time correction and optimization on the magnetic field model according to the perturbation equation of the magnetic field model.

[0033] By adopting the above technical solution, a perturbation equation of the magnetic field model is provided to describe the relationship between the change of the magnetic field model and the change of relative posture. By introducing the mathematical model, the theoretical basis and algorithm accuracy of the system are enhanced, which is suitable for pedestrian positioning in complex dynamic environments.

[0034] Optionally, the relative posture constraint method includes n-axis magnetic field vector constraint, building orientation information constraint or Wi-Fi, Bluetooth absolute positioning method constraint.

[0035] By adopting the above technical solution, a variety of relative attitude constraints are provided (magnetic field vector constraints, building orientation information constraints, Wi-Fi / Bluetooth constraints), the external information source of the system is increased, and the processing unit is combined with a variety of constraints to further optimize the positioning accuracy and system reliability.

[0036] Optionally, the data processing unit includes a hardware processing module and a software processing module.

[0037] By adopting the above technical solution, the data processing unit is divided into a hardware processing module and a software processing module, which ensures the efficiency and reliability of the system. The hardware module is responsible for data preprocessing and storage, and the software module is responsible for algorithm implementation and result output, thereby improving the overall performance of the system.

[0038] Optionally, a head-mounted device for a head-mounted pedestrian positioning method with a nine-axis sensor array, the head-mounted device includes one of a helmet, glasses or a hood, and the sensor is mounted on the head-mounted device via a fixing seat.

[0039] By adopting the above technical solutions, the design of the head-mounted device (helmet, glasses, hood) provides a variety of wearing methods, which increases the user's flexibility and comfort. The sensor is installed through a fixed seat to ensure the stability and reliability of the sensor, while facilitating maintenance and replacement.

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

[0041] 1. The environmental magnetic field distribution model is obtained by fitting the magnetometer array observations, and the absolute speed of the carrier is estimated by combining the changes in the magnetic field model continuously observed by the magnetometer array, thereby assisting the position, speed and attitude estimation of the inertial navigation system based on the inertial measurement unit array. This effectively makes up for the shortcomings of the traditional inertial navigation system with fast error divergence, significantly improves the positioning accuracy, and realizes indoor pedestrian positioning that is independent of pedestrian motion assumptions and pre-established magnetic field fingerprint libraries. Compared with the traditional magnetic field matching method, this greatly reduces the system construction and maintenance costs, while improving the flexibility and applicability of positioning.

[0042] 2. The use of head-mounted devices (such as helmets, glasses or hoods) designed with lightweight materials, combined with a low-cost nine-axis sensor array, makes the cost of the entire positioning device significantly lower than solutions that rely on infrastructure (such as BLE, 5G positioning) or high-precision lidar; at the same time, the head-mounted design is easy to wear and use, and is particularly suitable for indoor public or professional pedestrian positioning scenarios.

[0043] 3. The present invention utilizes the error transmission relationship between the magnetic field model and the relative posture, which can effectively deal with the common magnetic field interference in the indoor environment (such as electrical equipment, metal objects, etc.). Through multi-sensor fusion and advanced filtering algorithms (such as state cloning Kalman filter, extended Kalman filter, unscented Kalman filter, etc.), the robustness of the positioning system is further enhanced, and a high positioning accuracy can be maintained even in a dynamically changing environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 This is a flow chart of the positioning method of the present invention;

[0045] Figure 2 This is a schematic diagram of the helmet and sensor installation structure of the present invention;

[0046] Figure 3 It is a schematic diagram of the structure of glasses of the present invention.

[0047] In the figure: 1. head-mounted device; 11. helmet; 12. sensor; 13. glasses. DETAILED DESCRIPTION

[0048] The technical solution of the present invention will be described clearly and completely below in conjunction with the accompanying drawings of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0049] In the description of the present invention, it should be noted that the directions or positional relationships indicated by terms such as "middle", "upper", "lower", "left", "right", "inside" and "outside" are based on the directions or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific direction, be constructed and operated in a specific direction, and therefore cannot be understood as a limitation on the present invention.

[0050] like Figure 1 —3, the specific scheme of the embodiment is as follows: A head-mounted pedestrian positioning method of a nine-axis sensor array comprises the following steps:

[0051] A data processing unit and a plurality of sensors 12 are installed on the head-mounted device 1. The sensors 12 include an inertial measurement unit and a magnetometer. The plurality of sensors 12 form a sensor array. The head-mounted device 1 is worn, and the sensor array is started to collect data. The sensor array formed by the plurality of sensors 12 collects data, and the data processing unit processes and stores the collected data in real time.

[0052] The head-mounted device 1 includes a helmet 11, glasses 13 and a head cover. The head-mounted device 1 is composed of lightweight materials and fixing straps, and the sensor 12 is installed on the head-mounted device 1 through a fixing seat. The sensor 12 is a nine-axis sensor. The sensor 12 includes an inertial measurement unit and a magnetometer. The types of the magnetometer include a vector magnetometer, a scalar magnetometer and a gradient magnetometer. The number of the sensors 12 is greater than or equal to 3, and the installation positions of the multiple sensors 12 are not on the same straight line;

[0053] By installing sensors 12 at multiple positions of the head-mounted device 1, multi-sensor fusion is achieved, and the redundancy and reliability of the system are enhanced. The sensors 12 are distributed at different positions, and data can be obtained from multiple angles to ensure the accuracy of spatial magnetic field model parameter estimation. The installation positions of the sensors 12 are not on the same straight line, which effectively reduces the mutual interference between the sensors 12 and improves the measurement accuracy.

[0054] The helmet 11 is provided with a barometer and a thermometer, and the data processing unit includes a hardware processing module and a software processing module. The addition of a barometer and a thermometer to the helmet 11 can sense the changes in the air pressure and temperature of the environment in real time. The barometer can assist in locating the mid- / high-altitude floor information. The data processing unit includes a hardware processing module and a software processing module, and can process the collected data in real time to ensure the real-time and high efficiency of the positioning results, and is suitable for dynamic pedestrian positioning scenarios.

[0055] During walking, the inertial sensor array formed by the multiple inertial measurement units performs strapdown inertial navigation solution to calculate the values ​​of the current position, speed and posture; at the same time, the magnetometer array formed by the multiple magnetometers measures the surrounding magnetic field and fits the parameters of the environmental magnetic field distribution model;

[0056] The strapdown inertial navigation solution method includes using a certain inertial measurement unit in the array to solve, averaging the observation values ​​of the inertial measurement unit array and then solving, using each inertial measurement unit in the array to solve separately and then averaging the navigation results;

[0057] Multiple strapdown inertial navigation solution modes (single-point solution, average solution, multi-point independent solution), this real-time feedback mechanism significantly improves the stability and reliability of the system, especially in complex dynamic environments.

[0058] The magnetic field model parameters at the current moment are estimated using the observation values ​​of the magnetometer array, and the magnetic field model parameters at adjacent moments are combined to obtain the magnetic field model parameter changes; at the same time, an error transfer model between the magnetic field model and the relative posture is constructed to obtain the conversion relationship between the magnetic field model parameter changes and the relative posture changes;

[0059] The magnetic field is a three-dimensional vector field whose properties are described by Maxwell's equations. represents the magnetic field model, where r= [ r x r y r z ] T or r= [ x y z ] T , represents the position vector from any point in space to the origin of the local magnetic field model, are the parameters of the magnetic field polynomial model, and their number is determined by the polynomial order. If there is no free charge in space and the external magnetic field interference source is greater than 1m, the magnetic field model should satisfy the constraints of zero curl and divergence:

[0060] ,

[0061] ,

[0062] In the formula, It is the Nabla operator. The point product of the vector function represents the divergence of the vector function; the cross product of the vector function represents the curl of the vector function.

[0063] Since the magnetic field is an irrotational vector field, there must be a scalar potential function for this vector field: satisfy:

[0064] ,

[0065] In the formula, is the magnetic field strength M, is with The related (L+1)-order polynomial, L is a natural number, with L=1, 2, 3, 4, the scalar potential function expression can be expressed as:

[0066] ,

[0067] h ( r ) T = [ x y z xyz xz yz x 2 y 2 z 2 ] ,

[0068] m = [ m 1 … m 9 ] T ,

[0069] In the formula, scalar potential function representing the magnetic field; represents a coefficient vector constructed from the position vector; is the magnetic field model parameter; is a constant.

[0070] make , you can export:

[0071] ,

[0072] C ( r ) = [ 1 0 0 y z 0 2 x 0 0 0 1 0 x 0 z 0 2 y 0 0 0 1 0 x y 0 0 2 z ] ,

[0073] In the formula, is the product of a coefficient matrix (related to the position vector r) and the magnetic field model parameters,

[0074] From the assumption that the magnetic field divergence is zero:

[0075] ,

[0076] ∇ r ⋅ [ m 1 + m 4 y + m 5 z + 2 m 7 x m 2 + m 4 x + m 6 z + 2 m 8 y m 3 + m 5 x + m 6 y + 2 m 9 z ] = 0 ,

[0077] Right now:

[0078] ,

[0079] make , the equation can be rewritten as:

[0080] ,

[0081] F ( r ) = [ 1 0 0 y z 0 2 x 0 0 1 0 x 0 z 0 2 y 0 0 1 0 x y − 2 z − 2 z ] ,

[0082] i = [ i 1 … i 8 ] T

[0083] Since the difference of the magnetic field vector at the same position in two adjacent b-systems is only caused by the relative rotation of the two b-systems, after abbreviating the local magnetic field model, the magnetic field vectors in two adjacent b-systems have the following relationship:

[0084] ,

[0085] ,

[0086] Will The system is regarded as the carrier coordinate system at the current moment. The system is the vector coordinate system at the previous moment / cloning moment.

[0087] At this moment, according to the local magnetic field model formula and combined with the observations of the magnetometer array, the magnetic field model parameters can be estimated by least squares:

[0088] ,

[0089] In the formula, is the observation value vector of the magnetometer array;

[0090] Magnetic field model parameters The estimated covariance of is:

[0091] ,

[0092] In the formula, Represents the covariance matrix after estimating the magnetic field model parameters at the current moment;

[0093] It is estimated by calculating the sum of squares of the model residuals.

[0094] Next, we estimate the magnetic field model parameters at the current moment To predict the magnetic field observation value at the last cloning moment , that is, it is assumed that the magnetic field model parameters do not change significantly between two adjacent moments;

[0095] ,

[0096] in The meaning is that at the last moment the magnetometer position was The coordinates of the system;

[0097] ,

[0098] ,

[0099] ,

[0100] In the formula, represents the relative rotation between the b systems at two adjacent moments, Indicates the position change of the last clone time relative to the current time n system The projection under the system; Indicates that the coordinates are from the navigation coordinate system b j The rotation matrix that transforms the coordinate system to the carrier coordinate system n.

[0101] Because of the last moment Department and The observed vector of the same magnetic field in the system a differs only in the relative rotation of the system b:

[0102] ,

[0103] Magnetometer observations at the last moment:

[0104] ,

[0105] In the formula, is the observation noise vector of the magnetometer;

[0106] The perturbation equation of the magnetic field model is as follows:

[0107] M ^ j b j = C ^ i j F ^ ( l j b i ) i = ( C i j − [ d b g dt × ])( F ( l j b i ) + d F ( l j b i )) i = ( C i j − [ d b g dt × ])( F ( l j b i ) i + d F ( l j b i ) i ) = ( C i j − [ d b g dt × ])( F ( l j b i ) i + B ( i ) d l j b i ) ≈ C i j F ( l j b i ) i + C i j B ( i ) d l j b i − [ d b g dt × ] F ( l j b i ) i = M j b j + C i j B ( i ) d l j b i − [ d b g dt × ] M j b i = M j b j + C i j B ( i ) d l j b i + [ M j b i × ] d b g dt ,

[0108] B ( i ) = [ 2 i 4 i 6 i 7 i 6 2 i 5 i 8 i 7 i 8 − 2 ( i 4 + i 5 ) ] ,

[0109] l ^ j b i = C ^ j i l j b j +D r ^ b i = C ^ j i l j b j + C ^ n b i D r ^ n = C ^ j i l j b j + C ^ n b i v ^ j n dt = ( C j i + [ d b g dt × ]) l j b j + [ I + ϕ × ] C n b i ( v j n + d v j n ) dt ≈ C j i l j b j − [ l j b j × ] d b g dt + C n b i v j n dt + C n b i d v j n dt + [ ϕ × ] C n b i v j n dt = l j b i + C n b i d v j n dt − [ l j b j × ] d b g dt − [ D r b i × ] ϕ d l j b i = C n b i d v j n dt − [ l j b j × ] d b g dt − [ D r b i × ] ϕ ,

[0110] Combining the above two formulas:

[0111] M ^ j b j = M j b j + C i j B ( i ) d l j b i + [ M j b i × ] d b g dt = M j b j + C i j B ( i )( C n b i d v j n dt − [ l j b j × ] d b g dt − [ D r b i × ] ϕ ) + [ M j b i × ] d b g dt = M j b j + C i j B ( i ) C n b i d v j n dt + [[ M j b i × ] − C i j B ( i )[ l j b j × ]] dt d b g − C i j B ( i )[ D r b i × ] ϕ ,

[0112] In the formula, Represents the projection of the measurement value of the magnetometer array at the last moment in the bj system; represents the relative rotation between the b systems at adjacent moments; represents the position vector matrix; Indicates the magnetometer at the last moment The coordinates of the system; represents the magnetic field model parameters; represents the relative rotation between the two carrier coordinate systems at adjacent moments bi and bj; is the gyro bias error vector; Indicates the time difference between two adjacent moments, specifically and The time difference between two adjacent carrier coordinate systems; [ •× ] Indicates that The vector at is converted to an antisymmetric matrix, for example, [ M j b i × ] Indicates that The vector at is converted into an antisymmetric matrix; Express The variables are subjected to perturbation analysis, for example, Express The variables are subjected to disturbance analysis, that is, There is an error in the variable at ; Represents a matrix constructed from position vectors, r= [ x y z ] T The constructed matrix; Represented by the position coordinate vector Constructed matrix; represents a matrix related to θ; Position vector The error of Represents the relative rotation between the two carrier coordinate systems bj and bi at adjacent moments; Indicates that the magnetometer was at the last moment The coordinates of the system; Indicates the position change of n systems at adjacent moments The projection under the system; Indicates that the coordinates are converted from the navigation coordinate system to the carrier coordinate system The rotation matrix of the system; Represents the projection of the position change of navigation coordinate system n at adjacent moments in navigation coordinate system n; : represents the velocity vector of the carrier in the navigation coordinate system n at the last moment; Represents a 3×3 unit matrix; is the attitude error vector; Represents the velocity vector the data processing unit performs real-time correction and optimization on the magnetic field model according to the perturbation equation of the magnetic field model.

[0113] The magnetic field model parameters are fitted through the observation values ​​of the magnetometer array, and the magnetic field changes are obtained by combining the magnetic field model parameters at adjacent moments. The magnetic field model satisfies the constraints of zero curl and divergence, conforms to the physical properties of Maxwell's equations, ensures the accuracy and reliability of the model, and constructs an error transfer model between the magnetic field model and the relative posture, which can quantify the impact of magnetic field changes on the relative posture, and provides a theoretical basis for subsequent error correction. The establishment of this model significantly improves the system's anti-interference ability and positioning accuracy.

[0114] Constructing a relative posture constraint algorithm, integrating the magnetic field model parameter changes of the magnetometer array and the relative posture obtained by inertial navigation integration, and outputting the current position, speed and posture;

[0115] The relative posture constraint method includes extended Kalman filter, unscented Kalman filter, particle filter, state Lonker Kalman filter, least squares, graph optimization and factor graph; the data processing unit selects a suitable relative posture constraint method according to positioning accuracy requirements and computing resources, and performs real-time evaluation and optimization on the constraint results;

[0116] The relative posture constraint method includes n-series magnetic field vector constraint, building orientation information constraint or Wi-Fi, Bluetooth absolute positioning method constraint.

[0117] The measurement equation of the error state Kalman filter is expressed as:

[0118] d z mag = M ^ j b j − M ˜ j b j = C i j B ( i ) C n b i d v j n dt + [[ M j b i × ] − C i j B ( i )[ l j b j × ]] dt d b g − C i j B ( i )[ D r b i × ] ϕ + n v ,

[0119] In the formula, The observation vector representing the sensor array information used for the error state Kalman filter;

[0120] because The measurement noise and the error of the magnetic field model parameter estimation are included, so the observation covariance matrix should consist of two parts:

[0121] ,

[0122] In the formula, Represents the observation vector The corresponding covariance matrix; Indicated by A diagonal matrix consisting of the values ​​in ; Represents the standard deviation of the error of the magnetometer observations.

[0123] In one implementation, m historical clone state error vectors are added to the 15-dimensional error state vector. Taking m=2 and clone state position error as an example, the error state vector is as follows:

[0124] d x = [ d r i n d v n ϕ d b oh d b f d r i − 2 n d r i − 1 n ] T ,

[0125] In the formula, and are the position error vector and velocity error vector in the navigation coordinate system n respectively; and are the bias error vectors of the gyro and accelerometer respectively; and is the position error vector of the historical clone state.

[0126] In one embodiment, a relative attitude change correction is added to the algorithm, and the attitude change constraint is constructed by using the difference of the magnetic field in the n-system measured by a single magnetometer at adjacent times, and the filter measurement update is performed. Assuming that the magnetic field vector in the n-system at adjacent times is the same and the attitude error at adjacent times is unchanged, the magnetic field vector observation value in the n-system at adjacent times can be expressed as:

[0127] M ^ cur n = C ^ b n M cur b = [ I − ϕ × ] C b n M cur b M ^ before n = C ^ b n M before b = [ I − ϕ × ] C b n M before b ,

[0128] The Kalman filter measurement equation for relative attitude change correction is as follows:

[0129] d z = M ^ cur n − M ^ before n = [( M cur n − M before n ) × ] ϕ ,

[0130] in represents the magnetic field vector of system b at the current moment; represents the magnetic field vector of system b at the previous moment; Represents the rotation matrix of the vector from the b system to the n system,

[0131] A variety of relative posture constraint methods (extended Kalman filter, unscented Kalman filter, particle filter, etc.) are provided. The data processing unit selects the appropriate constraint method according to the positioning accuracy requirements and computing resources. This flexibility enables the system to achieve optimal positioning under different accuracy requirements and resource conditions, and outputs the current position, speed and attitude by combining the relative posture obtained by the change of magnetic field model parameters and inertial navigation integration; multi-source data fusion significantly improves the positioning accuracy and system reliability, reduces the impact of single sensor errors on positioning results, and introduces n-series magnetic field vector constraints, building orientation information constraints or Wi-Fi, Bluetooth absolute positioning mode constraints, further enriching the system's data source. The introduction of these external information can significantly improve the system's adaptability and positioning accuracy in complex environments.

[0132] The method proposed in this paper does not use the magnetic field model as a state variable, but uses the pose information at multiple different times to construct geometric constraints and perform EKF updates. The algorithm abandons excessive reliance on the magnetic field model and reduces the degrees of freedom of state estimation, thereby improving the estimation accuracy and making the positioning results more stable.

[0133] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A head-mounted pedestrian positioning method using a nine-axis sensor array, characterized in that: The following steps are involved: A data processing unit and a plurality of sensors are installed on the head mounted device, wherein the sensors include an inertial measurement unit and a magnetometer, and the plurality of sensors form a sensor array for data collection; The inertial sensor array formed by the plurality of inertial measurement units calculates the current position, speed and attitude through strapdown inertial navigation solution; A magnetometer array formed by a plurality of the magnetometers performs magnetic field measurement and obtains observation values; The observed values ​​estimate the magnetic field model parameters at the current moment, and combine the magnetic field model parameters at adjacent moments to obtain the magnetic field model parameter changes; construct an error propagation model between the magnetic field model and the relative posture, and obtain the conversion relationship between the magnetic field model parameter changes and the relative posture changes; A relative posture constraint algorithm is constructed to fuse the magnetic field model parameter changes of the magnetometer array and the relative posture obtained by inertial navigation integration, and output the current position, speed and posture.

2. The head-mounted pedestrian positioning method of the nine-axis sensor array according to claim 1 is characterized by: The magnetometer types include vector magnetometers, scalar magnetometers and gradient magnetometers.

3. The head-mounted pedestrian positioning method of the nine-axis sensor array according to claim 2 is characterized by: The helmet is provided with a barometer and a thermometer.

4. The head-mounted pedestrian positioning method of the nine-axis sensor array according to claim 1 is characterized by: The number of the sensors is greater than or equal to 3, and the installation positions of the multiple sensors are not on the same straight line.

5. The head-mounted pedestrian positioning method of a nine-axis sensor array according to claim 1 is characterized by: The strapdown inertial navigation solution method includes using a certain inertial measurement unit in the array to solve, averaging the observation values ​​of the inertial measurement unit array and then solving, using each inertial measurement unit in the array to solve separately and then averaging the navigation results; The data processing unit selects the optimal solution method according to the walking speed of pedestrians and the complexity of the environment, and evaluates and corrects the solution results in real time.

6. The head-mounted pedestrian positioning method of a nine-axis sensor array according to claim 1 is characterized by: The relative posture constraint methods include extended Kalman filtering, unscented Kalman filtering, particle filtering, state Lonker Kalman filtering, least squares, graph optimization and factor graph; the data processing unit selects the appropriate relative posture constraint method according to the positioning accuracy requirements and computing resources, and performs real-time evaluation and optimization of the constraint results.

7. The head-mounted pedestrian positioning method of the nine-axis sensor array according to claim 1 is characterized by: The perturbation equation of the magnetic field model is as follows: ， ， ， Combining the above two formulas: ， In the formula, Represents the projection of the measurement value of the magnetometer array at the last moment in the bj system; represents the relative rotation between the b systems at adjacent moments; represents the position vector matrix; Indicates the magnetometer at the last moment The coordinates of the system; represents the magnetic field model parameters; represents the relative rotation between the two carrier coordinate systems at adjacent moments bi and bj; is the gyro bias error vector; Indicates the time difference between two adjacent moments, specifically and The time difference between two adjacent carrier coordinate systems; Represents a matrix constructed from position vectors, The constructed matrix; Represented by the position coordinate vector Constructed matrix; represents a matrix related to θ; Position vector The error of Represents the relative rotation between the two carrier coordinate systems bj and bi at adjacent moments; Indicates that the magnetometer was at the last moment The coordinates of the system; Indicates the position change of n systems at adjacent moments The projection under the system; Indicates that the coordinates are converted from the navigation coordinate system to the carrier coordinate system The rotation matrix of the system; Represents the projection of the position change of navigation coordinate system n at adjacent moments in navigation coordinate system n; : represents the velocity vector of the carrier in the navigation coordinate system n at the last moment; represents a 3×3 unit matrix; is the attitude error vector; Represents the velocity vector the data processing unit performs real-time correction and optimization on the magnetic field model according to the perturbation equation of the magnetic field model.

8. The head-mounted pedestrian positioning method of the nine-axis sensor array according to claim 1 is characterized by: The relative posture constraint method includes n-series magnetic field vector constraint, building orientation information constraint or Wi-Fi, Bluetooth absolute positioning method constraint.

9. The head-mounted pedestrian positioning method of the nine-axis sensor array according to claim 1 is characterized by: The data processing unit includes a hardware processing module and a software processing module.

10. A head-mounted device for a head-mounted pedestrian positioning method using a nine-axis sensor array according to any one of claims 1 to 9, characterized in that: The head-mounted device includes one of a helmet, glasses or a head cover, and the sensor is installed on the head-mounted device through a fixing seat.

Images