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

Abstract

The application 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. The head-mounted pedestrian positioning method of the nine-axis sensor array comprises the following steps: installing a data processing unit and a plurality of sensors on a head-mounted device, wherein the sensors comprise inertial measurement units and magnetometers, and the plurality of sensors form a sensor array to perform data acquisition; an inertial sensor array formed by the plurality of inertial measurement units is solved through a strapdown inertial navigation; an environment magnetic field distribution model is obtained through observation and fitting of a magnetometer array, absolute speed of a carrier is estimated in combination with magnetic field model changes observed continuously by the magnetometer array, and then position, speed and attitude calculation of an inertial navigation system based on the inertial measurement unit array is assisted, so that the shortcoming that error divergence of a traditional inertial navigation system is fast is made up, positioning precision is significantly improved, and indoor pedestrian positioning is realized without dependence on pedestrian motion assumptions and a pre-established magnetic field fingerprint library.

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 using 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 are not available or are interfered with, achieving accurate and reliable positioning remains an urgent problem.

[0003] Current indoor positioning solutions can be roughly divided into two categories: infrastructure-reliant positioning solutions (such as pseudolites, WiFi, Bluetooth Low Energy (BLE), and 5G positioning) and infrastructure-independent positioning solutions (such as visual positioning, magnetic field matching, and dead reckoning). Infrastructure-reliant solutions require the deployment of specialized signal transceivers in the positioning environment, which not only increases positioning costs but also makes the signal propagation process susceptible to interference from various factors, limiting its usability. Among the infrastructure-independent methods, visual positioning is significantly affected by ambient lighting, occlusion, and dynamic changes, and the device consumes high power, limiting its widespread use in practical applications. Magnetic field matching relies on a pre-established magnetic field fingerprint database, resulting in high system construction and maintenance costs.

[0004] Shortcomings of existing technology

[0005] In indoor pedestrian positioning, the commonly used Pedestrian Dead Reckoning (PDR) method offers the advantages of independent calculations and immunity to interference from external information sources. Therefore, it is often the preferred solution for indoor pedestrian positioning. However, traditional inertial measurement units (IMUs) are typically low-cost and low-precision, resulting in rapid error divergence in strapdown inertial navigation algorithms. To improve positioning accuracy, researchers have sought alternative positioning information sources to assist in the inertial navigation system (INS) in performing dead reckoning. For example, vision-based assistance solutions are highly sensitive to lighting, occlusion, and dynamic environments, and are easily affected by environmental changes and missing features, which reduces positioning accuracy and stability. LiDAR-based positioning solutions, however, are limited in mass pedestrian positioning scenarios due to line-of-sight limitations, high cost, and high computational requirements.

[0006] In addition, currently widely used auxiliary solutions include the Foot-Mounted Inertial Navigation System (Foot-INS) and the Step-and-Heading System (SHS). The Foot-INS uses the periodic contact of the pedestrian's foot with the ground to obtain a virtual zero velocity and uses Zero Velocity Update Technology (ZUPT) to control the divergence of the inertial velocity error. However, this sensor installation method is not pedestrian-friendly and has low convenience.

[0007] SHS primarily involves gait detection, stride length estimation, heading estimation, and position update. Stride length estimation and heading estimation are the primary sources of error. The stride length model used in stride length estimation is poorly adaptable to different users and can easily introduce positioning scale errors. Heading estimation, however, can cause directional drift due to sensor noise accumulation and environmental interference (such as geomagnetic distortion and dynamic magnetic field disturbances), leading to trajectory rotation and offset errors. Furthermore, the inability to accurately estimate the sensor heading and pedestrian's direction of motion significantly reduces the usability of SHS.

[0008] In recent years, magnetic field odometry (MFO) has been proposed as a lightweight method for measuring pedestrian velocity. It uses magnetometer arrays to provide odometry information such as position or velocity. For example, in 2007, Vissière et al. first proposed using indoor magnetic field perturbations to improve IMU-based position and velocity estimation. Subsequently, Dorveaux (2011) proposed the magnetic inertial navigation (MINAV) technique, and Chesneau (2016) integrated magnetometers with inertial sensors for positioning using an extended Kalman filter. Recent research has further improved positioning accuracy by describing the local magnetic field using a polynomial model and combining this magnetic field model with INS errors for Kalman filtering (Huang et al.). Current MFO methods are severely affected by magnetic field gradient noise, and filtering methods that combine magnetic field models with INS errors rely heavily on accurate modeling of the magnetic field, resulting in poor positioning stability. Summary of the Invention

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

[0010] To achieve the above objectives, the invention adopts the following technical solution: a head-mounted pedestrian positioning method using 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 acquisition;

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

[0013] The observation 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 to output the current position, velocity and posture.

[0015] By adopting the above technical solution, 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 positioning accuracy; the multi-sensor distribution design enhances the robustness of the system and reduces the impact of 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 solution, 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, further improving positioning accuracy.

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

[0019] By adopting the above technical solution, a barometer and thermometer are added to the helmet, which expands the function of the sensor and can measure the ambient air pressure and temperature. 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, making it suitable for pedestrian positioning in complex environments.

[0022] Optionally, the strapdown inertial navigation solution method includes using a certain inertial measurement unit in the array for solution, averaging the observation values ​​of the inertial measurement unit array before solution, and using each inertial measurement unit in the array for separate solution and then averaging the navigation results.

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

[0024] Optionally, the relative pose constraint method includes extended Kalman filtering, unscented Kalman filtering, particle filtering, state Lonker Kalman filtering, 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] Where, 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 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 by 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 Error; 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 Coordinates of the system; Indicates the position change of n systems at adjacent moments The projection of the system; Indicates that the coordinates are converted from the navigation coordinate system n to the carrier coordinate system The rotation matrix of the system; Represents the projection of the position change of the navigation coordinate system n at adjacent moments under the 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, making it 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 or 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 combines multiple 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, ensuring 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 using a nine-axis sensor array, the head-mounted device comprising one of a helmet, glasses or a hood, and the sensor being 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 multiple wearing methods, increasing the user's flexibility and comfort. The sensor is installed through a fixed base 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 velocity of the carrier is estimated by combining the changes in the magnetic field model continuously observed by the magnetometer array. This assists the inertial navigation system based on the inertial measurement unit array in calculating the position, velocity, and attitude. This effectively compensates for the shortcomings of the traditional inertial navigation system with rapid error divergence, significantly improves positioning accuracy, and realizes indoor pedestrian positioning that is independent of pedestrian motion assumptions and a pre-established magnetic field fingerprint library. Compared with traditional magnetic field matching methods, this greatly reduces the system construction and maintenance costs, while improving the flexibility and applicability of positioning.

[0042] 2. A head-mounted device (such as a helmet, glasses, or head cover) 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, making it particularly suitable for indoor public or professional pedestrian positioning scenarios.

[0043] 3. This invention utilizes the error transmission relationship between the magnetic field model and relative posture to effectively deal with common magnetic field interference in indoor environments (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 dynamically changing environments. 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 Schematic diagram of the glasses structure of the present invention.

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

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

[0049] In the description of the present invention, it should be noted that the terms "middle", "upper", "lower", "left", "right", "inside", "outside", etc., indicating directions or positional relationships, are based on the directions or positional relationships shown in the accompanying drawings. They are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific direction, be constructed and operated in a specific direction. Therefore, they cannot be understood as limiting the present invention.

[0050] like Figure 1 —3, the specific scheme of the embodiment is as follows: A head-mounted pedestrian positioning method using 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. When the head-mounted device 1 is worn, the sensor array is activated to collect data. The sensor array formed between 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 made of lightweight materials and fixed straps, and the sensor 12 is mounted 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 magnetometer types include vector magnetometer, scalar magnetometer, and gradient magnetometer. The number of sensors 12 is greater than or equal to 3, and the installation positions of multiple sensors 12 are not on the same straight line.

[0053] By installing sensors 12 at multiple locations on the head-mounted device 1, multi-sensor fusion is achieved, which enhances the redundancy and reliability of the system. The sensors 12 are distributed at different locations and can acquire data from multiple angles, ensuring the accuracy of spatial magnetic field model parameter estimation. The sensors 12 are not installed on the same straight line, which effectively reduces mutual interference between the sensors 12 and improves 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 the barometer and the 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, which 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 current position, speed and posture values; 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 before solving, and using each inertial measurement unit in the array to solve separately and then average the navigation results;

[0057] Multiple strapdown inertial navigation solution methods (single-point solution, average solution, multi-point independent solution) and real-time feedback mechanism significantly improve 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] Where, 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] Where, 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] Where, represents the scalar potential function of the magnetic field; represents a coefficient vector constructed from the position vector; are magnetic field model parameters; 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] Where, is the product of a coefficient matrix (related to the position vector r) and the magnetic field model parameters,

[0074] By assuming 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 in 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 simplifying 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, based on 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] Where, is the observation value vector of the magnetometer array;

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

[0091] ,

[0092] Where, 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 the magnetometer position at the last moment is Coordinates of the system;

[0097] ,

[0098] ,

[0099] ,

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

[0101] Because of the last moment Department and The observed vectors of the same magnetic field in the system B differ only in the relative rotation of the system B:

[0102] ,

[0103] Magnetometer observations at the last moment:

[0104] ,

[0105] Where, 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] Where, 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 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 disturbance 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 by 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 Error; 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 Coordinates of the system; represents the projection of the position change of the adjacent time n system in the navigation coordinate system n system under the projection of the navigation coordinate system n system; ; represents the rotation matrix of the coordinate conversion from the navigation coordinate system n system to the carrier coordinate system ; represents the projection of the position change of the adjacent time n system in the navigation coordinate system n system under the projection of the navigation coordinate system n system; : represents the velocity vector of the carrier in the navigation coordinate system n system at the last time; represents a 3x3 unit matrix; is an attitude error vector; represents the error of the velocity vector ; the data processing unit corrects and optimizes the magnetic field model in real time 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 change is obtained by combining the magnetic field model parameters of the adjacent time. The magnetic field model satisfies the constraints of zero divergence and zero divergence, meets the physical characteristics of Maxwell equations, ensures the accuracy and reliability of the model, constructs the error transmission model between the magnetic field model and the relative pose, and can quantify the influence of the magnetic field change on the relative pose, which provides a theoretical basis for subsequent error correction. The establishment of such a model significantly improves the anti-interference ability and positioning accuracy of the system.

[0114] A relative pose constraint algorithm is constructed to fuse the relative pose obtained by integrating the magnetic field model parameter change of the magnetometer array and the inertial navigation, and output the position, velocity and attitude at the current time;

[0115] The relative pose constraint method includes extended Kalman filter, unscented Kalman filter, particle filter, state 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 requirement and the calculation resource, and evaluates and optimizes the constraint result in real time;

[0116] The relative pose constraint method includes n system 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 represented 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, represents the observation vector of the sensor array information for error state Kalman filter;

[0120] Because The covariance matrix of the observations consists of two parts:

[0121] ,

[0122] Where, 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 embodiment, m historical clone state error vectors are added to the 15-dimensional error state vector. Taking m=2 and the 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] Where, 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 gyroscope 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. The attitude change constraint is constructed using the difference in magnetic fields in the n-frame measured by a single magnetometer at adjacent moments. Filtered measurement updates are performed. Assuming that the magnetic field vectors in the n-frame at adjacent moments are the same and the attitude errors at adjacent moments are unchanged, the magnetic field vector observations in the n-frame at adjacent moments 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 in 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] The system provides a variety of relative pose constraint methods (such as extended Kalman filtering, unscented Kalman filtering, and particle filtering). The data processing unit selects the appropriate constraint method based on positioning accuracy requirements and computing resources. This flexibility enables the system to achieve optimal positioning under varying accuracy requirements and resource conditions. It combines the relative pose obtained by changes in magnetic field model parameters and inertial navigation integration to output the current position, velocity, and attitude. Multi-source data fusion significantly improves positioning accuracy and system reliability, reducing the impact of single sensor errors on positioning results. The introduction of n-axis magnetic field vector constraints, building orientation information constraints, or absolute positioning constraints using Wi-Fi and Bluetooth further enriches the system's data sources. The inclusion of this external information significantly improves 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. Instead, it uses pose information at multiple different times to construct geometric constraints and perform EKF updates. This algorithm abandons excessive reliance on the magnetic field model and reduces the degrees of freedom in state estimation, thereby improving estimation accuracy and making positioning results more stable.

[0133] While 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 these embodiments without departing from the principles and spirit of the invention, and that the scope of the 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 acquisition; An inertial sensor array formed by a plurality of the inertial measurement units calculates the current position, velocity 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 observation 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; Constructing a relative posture constraint algorithm, fusing the magnetic field model parameter changes of the magnetometer array and the relative posture obtained by inertial navigation integration, and outputting the current position, velocity and posture; The perturbation equation of the magnetic field model is as follows: ， ， ， Combining the above two formulas: ， Where, 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 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 by position vectors, The constructed matrix; Represented by the position coordinate vector Constructed matrix; represents a matrix related to θ; Position vector Error; 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 Coordinates of the system; Indicates the position change of n systems at adjacent moments The projection of the system; Indicates that the coordinates are converted from the navigation coordinate system n to the carrier coordinate system The rotation matrix of the system; Represents the projection of the position change of the navigation coordinate system n at adjacent moments under the 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.

2. The head-mounted pedestrian positioning method using a nine-axis sensor array according to claim 1, wherein: The magnetometer types include vector magnetometers, scalar magnetometers, and gradient magnetometers.

3. The head-mounted pedestrian positioning method using a nine-axis sensor array according to claim 2, wherein: The helmet is equipped with a barometer and thermometer.

4. The head-mounted pedestrian positioning method using a nine-axis sensor array according to claim 1, wherein: 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 using a nine-axis sensor array according to claim 1, wherein: 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 before solving, and using each inertial measurement unit in the array to solve separately and then average the navigation results; The data processing unit selects the optimal solution method according to the pedestrian's walking speed and the complexity of the environment, and evaluates and corrects the solution results in real time.

6. The head-mounted pedestrian positioning method using a nine-axis sensor array according to claim 1, wherein: The relative pose 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 pose 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 using a nine-axis sensor array according to claim 1, wherein: The relative posture constraint method includes n-axis magnetic field vector constraint, building orientation information constraint or Wi-Fi, Bluetooth absolute positioning method constraint.

8. The head-mounted pedestrian positioning method using a nine-axis sensor array according to claim 1, wherein: The data processing unit includes a hardware processing module and a software processing module.

9. The head-mounted device for the head-mounted pedestrian positioning method using a nine-axis sensor array according to any one of claims 1 to 8, wherein: 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