Strapdown inertial navigation pedestrian three-dimensional position correction method

Abstract

The application claims a kind of strapdown inertial navigation pedestrian three-dimensional position correction method, belongs to the field of inertial pedestrian navigation, it includes the following steps: pedestrian height correction, the height of fusion by barometer and accelerometer independently solved height, the initial correction of pedestrian height is carried out;Pedestrian pitch angle correction, the relationship between step height and pitch angle of pedestrian step length is used to calculate pitch angle, and the pitch angle is fused with attitude solution pitch angle, and the pitch angle is corrected;Pedestrian three-dimensional position correction, the relationship between pedestrian pitch angle, step length and heading angle is used to correct the three-dimensional position of pedestrian.The position information of pedestrian in three-dimensional space in strapdown inertial navigation system is corrected gradually by the above three sub-algorithms according to the application.The method disclosed by the application can realize the accurate positioning of pedestrian position in three-dimensional space, which can not only be applied to the existing city high-rise building such a more regular experimental environment pedestrian inertial navigation, especially suitable for more extensive irregular environment such as ramp, mountainous environment.

Description

Technical Field

[0001] This invention relates to the field of inertial pedestrian navigation, and more particularly to a method for correcting the three-dimensional position of pedestrians using strapdown inertial navigation. Background Technology

[0002] Strap-down Inertial Navigation System (SINS) is an autonomous navigation system with good stealth and anti-interference capabilities. It relies on an Inertial Measurement Unit (IMU) to measure the motion information of the carrier. The IMU mainly consists of a gyroscope, an accelerometer, and a microcontroller unit. The gyroscope is responsible for measuring the instantaneous angular velocity of the carrier, the accelerometer is responsible for measuring the instantaneous acceleration of the carrier, which includes gravitational acceleration, and the microcontroller unit is responsible for acquiring the sensor data and performing corresponding algorithm processing.

[0003] In SINS, the IMU is directly fixed to the carrier. The MCU uses a mathematical platform to transfer the motion information from the carrier coordinate system to the navigation coordinate system for navigation calculation. Based on different technical principles, navigation algorithms can be divided into two types: one uses motion information integration for navigation, i.e., Inertial Navigation System (INS); the other uses motion information deduction for navigation, i.e., Pedestrian Dead Reckoning (PDR). INS can be applied to aircraft, ships, vehicles, and pedestrians, while PDR, which mainly utilizes the physiological characteristics of pedestrian movement, is generally only used for pedestrian navigation.

[0004] The core idea of ​​the PDR algorithm is to use motion information measured by inertial sensors to calculate the direction (heading angle) and displacement (step length) of each pedestrian's movement. The position of each step is the position of the previous step plus the displacement of the next step. In a three-dimensional plane, the step length calculated by PDR is the length in three-dimensional space. It is necessary to project the step length onto the XOY plane, and then combine it with the heading angle to calculate the pedestrian's displacement in the X and Y directions for each step. The current horizontal position of the pedestrian can be obtained by accumulating the displacements of each step.

[0005] Existing pedestrian inertial navigation algorithms are mostly tested in regular scenarios such as indoor buildings or outdoor sports fields. For outdoor sports fields, it's not necessary to calculate the pedestrian's position along the Z-axis. For indoor buildings, the height change of each step for a pedestrian going up or down stairs is almost the same, equal to the step height. Therefore, once the direction of movement is determined, the height information only needs to be added to or subtracted from the fixed step height. However, in irregular scenarios such as slopes or mountainous environments, the height of each step for a pedestrian is not the same. In such environments, simply adding or subtracting the step height as in existing algorithms will produce significant errors.

[0006] In addition, it is well known that inertial navigation suffers from severe cumulative error. To reduce this cumulative error, the results of inertial navigation can be corrected using measurement results from different methods. Different results for the same parameter obtained from different methods require data fusion, and the Kalman filter algorithm is widely used in this data fusion process.

[0007] The Kalman filter algorithm is an algorithm that uses the state equation of a linear system and the system observation matrix to make an optimal estimate of the system state. Since the observation data contains the influence of noise and interference in the system, the optimal estimation can also be regarded as a filtering process.

[0008] To obtain pedestrian 3D position information more accurately in strapdown inertial navigation systems, this invention proposes a pedestrian 3D position correction algorithm based on strapdown inertial navigation.

[0009] The patent "Inertial Pedestrian Navigation Algorithm Based on Zero-Speed ​​Correction and Attitude Self-Observation (CN112362057A)" proposes an inertial pedestrian navigation algorithm based on zero-speed correction and attitude self-observation. By detecting zero speed in the collected data, the algorithm identifies the zero-speed interval during walking. A Kalman filter is designed, and within the zero-speed interval, the zero-speed correction and attitude self-observation algorithm is used to estimate the navigation attitude, position, and velocity errors. Error correction is then applied to the inertial navigation solution, thereby achieving precise positioning and navigation of pedestrians. This patent corrects the pedestrian's position through the zero-speed characteristics of the zero-speed interval; its navigation algorithm is essentially the SINS algorithm. This invention utilizes the characteristics of pedestrian walking, first correcting the step height, then the pitch angle, and finally the pedestrian's three-dimensional position; its navigation algorithm is essentially the PDR algorithm. The two pedestrian position correction methods are different in principle, but the correction method proposed in this invention is also applicable to the above-mentioned patent. That is, in the zero speed range of the above-mentioned patent, the position of the pedestrian is calculated by the method proposed in this invention, and then combined with the position calculated by its own algorithm. The two position information are filtered and fused, thereby combining the two methods.

[0010] Patent "An Autonomous Personnel Height Positioning Algorithm (CN109579832B)" essentially uses accelerometers and gyroscopes to determine whether a pedestrian has reached a floor turn, and then corrects the accelerometer integral using integer multiples of half the floor height. This method is only applicable inside buildings and is different from the method proposed in this invention. Patent "A Barometer-less Height Measurement Method Based on Android Inertial Platform (CN111197974B)" essentially uses a fixed model of pedestrians going up and down stairs, combined with pitch angle information calculated from sensor data and the pedestrian's height to calculate height, which is different from the method proposed in this invention. Patent "Height Filtering Method Based on GPS and Barometer Fusion Data (CN108416387B)" essentially uses GPS to correct barometer height, which is different from the method proposed in this invention. The patent "A 3D Spatial Positioning Method for Pedestrian Navigation System Based on Human / Environmental Constraints (CN106017461B)" essentially uses the SINS algorithm and Kalman filter to correct the position, which is different from the method proposed in this invention. The patent "An Indoor 3D Positioning Method Based on MIMU (CN115451969A)" is essentially a height correction method that directly uses a barometer and altitude sensor, without incorporating pedestrian attitude angle information, which is also different from the method proposed in this invention.

[0011] Based on the aforementioned invention patents, analysis reveals that existing technologies are either only applicable to indoor building scenarios (CN109579832B, CN111197974B, CN106017461B) or require additional sensors such as barometers or GPS integration for outdoor use (CN108416387B, CN115451969A). While indoor buildings can use stair height or building features to correct the 3D position of pedestrians, this method is unsuitable for mountainous or sloping environments. Although using external barometer sensors or GPS can achieve 3D positioning in mountainous or sloping environments, the added equipment increases the complexity of the entire system. Therefore, the problem with existing technologies is: how to use a simpler method to accurately locate the 3D position of people. Summary of the Invention

[0012] This invention aims to solve the problems of the prior art. It proposes a method for correcting the three-dimensional position of pedestrians using strapdown inertial navigation systems. The technical solution of this invention is as follows:

[0013] A method for correcting the three-dimensional position of a pedestrian using strapdown inertial navigation, comprising the following steps:

[0014] Pedestrian height correction steps: Obtain pedestrian height information H using a barometer. b Simultaneously, pedestrian height information H is obtained by pre-integration using accelerometers.a The pedestrian height information obtained by the two methods above is fused using a Kalman filter to obtain the height H, thereby achieving the initial correction of the pedestrian height information.

[0015] Pedestrian pitch angle correction steps: Calculate the pitch angle using the relationship between pedestrian stride length, stride height, and pitch angle, integrate it with the attitude calculation pitch angle, and correct the pitch angle;

[0016] Pedestrian 3D position correction steps: Correct the pedestrian's 3D position by utilizing the relationship between the pedestrian's pitch angle, stride length, and heading angle.

[0017] Furthermore, the pedestrian height correction step specifically includes:

[0018] Step 1: Using the Laplace pressure formula

[0019]

[0020] Obtain the height H corresponding to each step b H b H0; P, P0 represent the height and pressure of the measuring point and the reference point, respectively; T m The average temperature of the measurement point and the reference point is taken as equal to the temperature of the reference point when the pedestrian's walking range is small.

[0021] T m =T 测量点

[0022] The height H calculated by this formula b The height of the measurement point is relative to the reference point, so the height at each step is equal to the current height minus the previous height;

[0023] Step 2: Obtain acceleration information in the navigation coordinate system using the rotation matrix from the carrier coordinate system to the navigation coordinate system and the acceleration information measured by the accelerometer. a n Acceleration information in the navigation coordinate system Let a be the rotation matrix from the vehicle coordinate system to the navigation coordinate system. b The acceleration information is obtained in the carrier coordinate system; after subtracting the influence of gravitational acceleration, the pedestrian's acceleration information along the Z-axis in the navigation coordinate system is obtained. The pedestrian's integral height is obtained by integrating the acceleration along the Z-axis twice.

[0024]

[0025] Step 3: Use the Kalman filter fusion algorithm to calculate the barometer height H obtained in Step 1. b The accelerometer integral height H obtained in Step 2a The height H obtained after the first correction is obtained through fusion. The integral height H is obtained by integrating using accelerometers during the Kalman fusion process. a As a state transition quantity, the altitude information H measured by the barometer b As an observation.

[0026] Furthermore, the pedestrian pitch angle correction step involves: calculating the pitch angle using the relationship between pedestrian stride length, stride height, and pitch angle; integrating this calculation with the attitude-derived pitch angle; and correcting the pitch angle. Specifically, this includes:

[0027] The nonlinear step size model is used to calculate the step length L of a pedestrian.

[0028] The pitch angle θ′ of a pedestrian is calculated by the relationship between the pedestrian's stride length, stride height, and pitch angle.

[0029] The attitude information of the pedestrian at each step is calculated by the strapdown inertial navigation attitude calculation algorithm, where the pitch angle is θ.

[0030] The optimal pitch angle is estimated by fusing the two pitch angle information using the Kalman filter algorithm.

[0031] Furthermore, the calculation of the pedestrian's step length L using a nonlinear step-length model specifically includes:

[0032] Using a nonlinear step size model

[0033]

[0034] To calculate the step length L of a pedestrian's each step, we use an empirical formula, where K is an empirical value. This represents the maximum value of the square of the acceleration magnitude measured by the triaxial accelerometer in this step. This represents the minimum square of the acceleration modulus. When a pedestrian walks on flat ground, the step length calculated by this formula is the pedestrian's true horizontal displacement. When a pedestrian walks on a slope or going up or down stairs, this step length should be multiplied by the cosine of the pitch angle to obtain the pedestrian's true horizontal displacement. In the formula, K is the unit conversion factor. First, let the pedestrian walk a certain distance with a fixed step length, and at the same time collect the information from the accelerometer on the pedestrian's body to calculate K for each step. i Find the value and calculate the average.

[0035] Furthermore, the relationship between the pedestrian's stride length, stride height, and pitch angle is specifically as follows:

[0036] The sum of the square of the step height and the square of the horizontal displacement of each step is equal to the square of the step length. The angle between the step length and the horizontal displacement is the pitch angle, that is, the pitch angle of each step is equal to the arcsine of the ratio of step height to step length.

[0037] The step height of a pedestrian is equal to the product of the step length and the sine of the pitch angle: H = Lsinθ; the horizontal displacement of a pedestrian is equal to the product of the step length and the cosine of the pitch angle: S = Lcosθ. H is the step height of a pedestrian, L is the step length of a pedestrian, and θ is the pitch angle of a pedestrian.

[0038] Furthermore, the pedestrian's three-dimensional position correction specifically includes the following steps:

[0039] The optimal pitch angle estimate obtained using the pitch angle correction algorithm is used to recalculate the pedestrian step height H′.

[0040] The horizontal displacement S′ of the pedestrian is corrected using the corrected pitch angle information;

[0041] The Kalman filter algorithm is used to fuse the height information from the height correction algorithm and this sub-algorithm to obtain the height. To further refine pedestrian height information;

[0042] The horizontal position information X and Y of the pedestrian is corrected by using the heading information calculated from the corrected pedestrian horizontal displacement and attitude.

[0043] Furthermore, the recalculation of the pedestrian step height H′ using the optimal pitch angle estimate obtained by the pitch angle correction algorithm specifically includes:

[0044] The optimal pitch angle obtained by the Kalman algorithm As the true pitch angle, the current step height is recalculated using the trigonometric relationship between the pitch angle and the pedestrian's stride length and stride height.

[0045]

[0046] The step of correcting the pedestrian's horizontal displacement S′ using the corrected pitch angle information specifically includes:

[0047] The optimal pitch angle obtained by the Kalman algorithm As the true pitch angle, the horizontal displacement of the pedestrian in one step at the current moment is calculated through the trigonometric relationship between the pitch angle, the pedestrian's stride length, and the pedestrian's horizontal displacement.

[0048]

[0049] Furthermore, the Kalman filter algorithm is used to fuse the height information from the height correction algorithm and this sub-algorithm to obtain the height. Further refinement of pedestrian height information includes:

[0050] The Kalman filter fusion algorithm is used to fuse the obtained step height H with the calculated step height H′ to estimate the accurate step height for each step. In the Kalman fusion process, the obtained step height H is used as the state transition quantity, and the calculated step height H′ is used as the observation; at this time, the optimal step height estimate is obtained. To obtain accurate height information for each step of the pedestrian, the corrected height of the pedestrian relative to the starting point at the current moment can be obtained by combining the height information from all previous moments.

[0051] Furthermore, the step of correcting the pedestrian's horizontal position information X and Y using the heading information calculated from the corrected pedestrian horizontal displacement and attitude specifically includes:

[0052] Multiplying the corrected horizontal displacement S′ by the sine and cosine values ​​of the heading angle calculated at each step yields the displacement of the pedestrian in the X and Y axes at each step in the navigation coordinate system, i.e., X = S′sinψ, Y = S′cosψ. Combining this with the horizontal coordinate information from the previous step, the current horizontal coordinates of the pedestrian can be obtained.

[0053] The advantages and beneficial effects of this invention are as follows:

[0054] The method of this invention is easy to understand and easy to implement. It can be applied not only to pedestrian inertial navigation in relatively regular experimental environments such as existing urban high-rise buildings, but also to a wider range of irregular environments such as slopes and mountains, effectively expanding the application scenarios and scope of pedestrian navigation algorithms.

[0055] The core of this invention, and often overlooked, is the "pedestrian 3D position correction step: correcting the pedestrian's 3D position using the relationship between the pedestrian's pitch angle, stride length, and heading angle." This step utilizes the pedestrian's pitch angle information in conjunction with stride length and heading information to correct the pedestrian's 3D position. This method is advantageous for achieving accurate 3D positioning because it leverages the characteristics of pedestrians walking in mountainous or sloping environments. Specifically, the pedestrian's pitch angle changes with the environment, and there is a specific triangular relationship between the pedestrian's stride length, stride height, and pitch angle. Therefore, this relationship can be used to correct the pedestrian's 3D position in such environments, which is the ingenious aspect of this method. Based on this, the method avoids the use of other sensors, maximizing the use of information obtained by the system, making the entire system simpler than existing technologies. Attached Figure Description

[0056] Figure 1 This is a flowchart of a pedestrian three-dimensional position correction algorithm according to a preferred embodiment of the present invention.

[0057] Figure 2 This is a schematic diagram illustrating the correction process of a pedestrian three-dimensional position correction algorithm provided in this application.

[0058] Figure 3 The flowchart of a pedestrian three-dimensional position correction algorithm provided in this application is shown. Detailed Implementation

[0059] The technical solutions of the embodiments of the present invention will be clearly and thoroughly described below with reference to the accompanying drawings. The described embodiments are merely some embodiments of the present invention.

[0060] The technical solution of the present invention to solve the above-mentioned technical problems is:

[0061] This invention discloses a strapdown inertial navigation system (SINS) algorithm for correcting the three-dimensional position of pedestrians, enabling pedestrian navigation in common environments such as urban buildings and mountainous jungles. Since pedestrian height information obtained solely from barometers and accelerometers is not entirely accurate, a Kalman filter algorithm is used to fuse the results from these two different methods for pedestrian height correction.

[0062] The method of this invention includes three sub-algorithms: a pedestrian height correction algorithm, a pedestrian pitch angle correction algorithm, and a pedestrian 3D position correction algorithm. The pedestrian height correction algorithm works as follows: first, it calculates the pedestrian's step height using data obtained from barometer and accelerometer measurements; then, it fuses the two height information using a Kalman filter algorithm to perform preliminary correction of the pedestrian's height. The pedestrian pitch angle correction algorithm works as follows: first, it obtains the pitch angle using the relationship between pedestrian step length and height, and two methods for calculating the attitude of the strapdown inertial navigation system (SINS); then, it fuses the two pitch angle information using a Kalman filter algorithm to correct the pedestrian's pitch angle. The pedestrian 3D position correction algorithm works as follows: it corrects the pedestrian's 3D position using the step length information, heading angle information, and the corrected pitch angle information.

[0063] The pedestrian height correction algorithm includes: Step 1: Measure the height distance between the pedestrian's step and the starting point using a barometer; Step 2: Integrate the acceleration measured by the accelerometer twice to obtain the height of the pedestrian's step; Step 3: Use the Kalman filter algorithm to fuse the two height information to estimate the optimal height. The Kalman filter algorithm is an algorithm that uses observations to make an optimal estimate of the system state.

[0064] The pedestrian pitch angle correction algorithm includes: Step 4: Calculate the pedestrian's step length for each step using a nonlinear step length model; Step 5: Calculate the pedestrian's pitch angle during walking using the trigonometric relationship between the pedestrian's step length, step height, and pitch angle; Step 6: Calculate the pedestrian's attitude information for each step using a strapdown inertial navigation attitude calculation algorithm; Step 7: Use a Kalman filter algorithm to fuse the two pitch angle information to estimate the optimal pitch angle.

[0065] The pedestrian 3D position correction algorithm includes: Step 8: Recalculate the pedestrian's step height using the optimal pitch angle estimate obtained from the pitch angle correction algorithm; Step 9: Correct the pedestrian's horizontal displacement using the corrected pitch angle information; Step 10: Use the Kalman filter algorithm to fuse the pedestrian height information recalculated in the height correction algorithm and this sub-algorithm to further correct the pedestrian's height information; Step 11: Correct the pedestrian's horizontal position information using the corrected pedestrian horizontal displacement and the heading information calculated from the attitude solution.

[0066] Please see Figure 1 , Figure 1 This is a flowchart illustrating a strapdown inertial navigation (SINS) 3D position correction algorithm for pedestrians, as disclosed in an embodiment of the present invention. This 3D position correction algorithm for pedestrian navigation and positioning using SINS can be applied to common environments such as urban buildings, slopes, and mountainous jungles; the present invention does not limit the application scenario. Figure 1 As shown, the three-dimensional position correction algorithm for strapdown inertial navigation and positioning of pedestrians may include the following operations:

[0067] 101. Pedestrian height correction: The height calculated independently by the barometer and accelerometer is integrated to make an initial correction to the pedestrian height.

[0068] 102. Pedestrian pitch angle correction: The pitch angle is calculated by using the relationship between pedestrian stride length, stride height and pitch angle, and then integrated with the pitch angle calculated by attitude solution to correct the pitch angle.

[0069] 103. Pedestrian 3D position correction: The pedestrian's 3D position is corrected by utilizing the relationship between the pedestrian's pitch angle, stride length, and heading angle.

[0070] The main design concept of this application is to use the relationship between the step length, step height, pitch angle and heading angle of each step of a pedestrian to correct the pedestrian's step height, pitch angle and horizontal displacement, and thus correct the pedestrian's three-dimensional position.

[0071] This invention provides a pedestrian 3D position correction algorithm for strapdown inertial navigation. This algorithm aims to correct the 3D position information of pedestrians in common urban application scenarios such as shopping malls and buildings. More importantly, it can also correct the 3D position information of pedestrians in a wider range of environments such as slopes and mountainous environments.

[0072] The algorithm consists of three sub-algorithm parts. The first sub-algorithm part obtains the pedestrian's step height by fusing the height calculated from the barometer with the pre-integrated height from the accelerometer. This includes three steps: calculating the height from the barometer, obtaining the height from the accelerometer integration, and fusing the two height information. The second sub-algorithm part obtains the corrected pitch angle by fusing the pitch angle obtained from the relationship between the pedestrian's step length and step height with the pitch angle calculated from the attitude. This includes four steps: calculating the step length, calculating the pitch angle from the step length and step height, calculating the pitch angle from the attitude, and fusing the two pitch angle information. The third sub-algorithm part corrects the pedestrian's three-dimensional position by using the corrected pitch angle, the pedestrian's step length, and the heading angle calculated from the attitude. This includes four steps: calculating the corrected step height, calculating the corrected horizontal displacement, secondary fusion correction of the step height, and correction of the horizontal position information.

[0073] This algorithm first uses a barometer to obtain the relative air pressure height H of a pedestrian at each step (or multiple steps). b Simultaneously, the height H is obtained by pre-integrating the acceleration measured by the accelerometer. a Using the Kalman algorithm to fuse barometer altitude H b and accelerometer pre-integration height H a The height H after the first correction is obtained.

[0074] Then, a nonlinear step size model is used to calculate the step size L for one (or more) steps; then, the pitch angle θ′ of the pedestrian for that step (or more) is calculated using the trigonometric function relationship between the height and step size of each step (or more) steps; simultaneously, a pitch angle θ is calculated through attitude analysis; then, the Kalman algorithm is used to fuse the previously calculated pitch angle θ′ and the pitch angle θ calculated by the inertial navigation algorithm attitude analysis, resulting in a fused pitch angle. It approximates the true pitch angle.

[0075] Next, the pitch angle is estimated by multiplying the step size L. The corrected height H′ and displacement S′ of the pedestrian at each step (or multiple steps) can be obtained by using the sine and cosine values. Finally, the Kalman algorithm is used again to fuse the corrected height H′ with the height H obtained in the first step to obtain more accurate height information. Simultaneously, the corrected pedestrian horizontal coordinate information is obtained by multiplying the corrected displacement S′ by the sine and cosine of the heading angle ψ calculated from the attitude.

[0076] The flowchart of the pedestrian three-dimensional position correction algorithm proposed in this invention is as follows: Figure 1 As shown in the diagram, the correction process is illustrated below. Figure 2 As shown, the algorithm execution flowchart is as follows: Figure 3 As shown.

[0077] The technical solution of this algorithm will be described in detail below with reference to the accompanying drawings.

[0078] Part 1: Pedestrian Height Correction Algorithm

[0079] Step 1: Using the Laplace pressure formula

[0080]

[0081] Obtain the height H corresponding to each step b H b H0; P, P0 represent the height (in meters) and pressure (in Pa) of the measuring point and the reference point, respectively; T m The average temperature (in °C) of the measuring point and the reference point is given. When the pedestrian's walking range is small, the temperature of the measuring point can be considered equal to the temperature of the reference point.

[0082] T m =T 测量点

[0083] The height H calculated by this formula b The height of the measurement point (at each step) relative to the reference point (starting point) is equal to the height at the current moment minus the height at the previous moment.

[0084] Step 2: Obtain acceleration information in the navigation coordinate system using the rotation matrix from the carrier coordinate system to the navigation coordinate system and the acceleration information measured by the accelerometer. After subtracting the effect of gravitational acceleration, we obtain the pedestrian's acceleration information along the Z-axis in the navigation coordinate system. The pedestrian's integral height is obtained by integrating the acceleration along the Z-axis twice.

[0085]

[0086] Step 3: Use the Kalman filter fusion algorithm to calculate the barometer height H obtained in Step 1. b The accelerometer integral height H obtained in Step 2 a The fusion yields the first corrected height H. The integral height H is obtained by integrating using accelerometer data during the Kalman fusion process. a As a state transition quantity, the altitude information H measured by the barometer b As an observation.

[0087] Part Two: Pedestrian Pitch Angle Correction Algorithm

[0088] Step 4: Using a nonlinear step size model

[0089]

[0090] To calculate the step length L of a pedestrian's each step, we use an empirical formula, where K is an empirical value. This represents the maximum value of the square of the acceleration magnitude measured by the triaxial accelerometer in this step. This represents the minimum square of the acceleration modulus. When a pedestrian walks on flat ground, the step length calculated by this formula is the pedestrian's true horizontal displacement. When the pedestrian walks on a slope or going up or down stairs, this step length should be multiplied by the cosine of the pitch angle to obtain the pedestrian's true horizontal displacement. In the formula, K is the unit conversion factor. When the value of K is accurately selected, the step length L calculated by this formula is also relatively accurate. The value of K can be determined as follows: First, have the pedestrian walk a certain distance with a fixed step length, and simultaneously collect information from the accelerometer on the pedestrian's body to calculate K for each step. i Find the value and calculate the average.

[0091] Step 5: From the trigonometric relationship between the step length L and height H of each step, the pitch angle corresponding to each step can be obtained, i.e.

[0092]

[0093] Step 6: Obtain the pitch angle θ and yaw angle ψ using an inertial navigation attitude calculation algorithm. In the quaternion update algorithm, the pitch angle...

[0094] θ=arcsin(2(q2q3+q0q1))

[0095] Heading angle

[0096]

[0097] Where q0, q1, q2, and q3 are the quaternions at the current time.

[0098] Step 7: Use the Kalman filter fusion algorithm to fuse the pitch angle θ′ calculated in Step 5 based on the relationship between step size and altitude with the pitch angle θ obtained from attitude calculation in Step 6 to estimate the accurate pitch angle for each step. In the Kalman fusion process, the pitch angle θ is calculated using attitude as the state transition quantity, and the pitch angle θ′ is calculated using step size and step height as the observation quantity.

[0099] Part Three: Pedestrian 3D Position Correction Algorithm

[0100] Step 8: Optimal estimated pitch angle obtained using the Kalman algorithm As the true pitch angle, the current step height is recalculated using the trigonometric relationship between the pitch angle and the pedestrian's stride length and stride height.

[0101]

[0102] Step 9: Optimal estimated pitch angle obtained using the Kalman algorithm As the true pitch angle, the horizontal displacement of the pedestrian in one step at the current moment is calculated through the trigonometric relationship between the pitch angle, the pedestrian's stride length, and the pedestrian's horizontal displacement.

[0103]

[0104] Step 10: Use the Kalman filter fusion algorithm to fuse the step height H obtained in Step 3 with the step height H′ calculated in Step 8 to estimate the accurate step height for each step. In the Kalman fusion process, the step height H obtained in Step 3 is used as the state transition variable, and the step height H′ calculated in Step 8 is used as the observation. At this point, the optimal step height estimate is obtained. To obtain accurate height information for each step of the pedestrian, the corrected height of the pedestrian relative to the starting point at the current moment can be obtained by combining the height information from all previous moments.

[0105] Step 11: Multiply the corrected horizontal displacement S′ from Step 9 by the sine and cosine values ​​of the heading angle calculated at each step in Step 6 to obtain the pedestrian's displacement in the X and Y axes of the navigation coordinate system for each step. Combining the horizontal coordinate information from all previous moments, the corrected horizontal displacement of the pedestrian relative to the starting point at the current moment can be obtained.

[0106] The Kalman filter used in this algorithm is a classic linear Kalman filter. In a linear discrete system, the system state transitions and observations satisfy the following equations:

[0107]

[0108] In the formula X k It is an n-dimensional estimated state variable, i.e., a state transition variable, Φ. k|k-1 Γ is the one-step state transition matrix of the system in n×n dimensions. k|k-1 It is an n×p dimensional noise input matrix, W k It is p-dimensional process noise; Z k H is an m-dimensional measurement variable, i.e., an observation. k It is an m×n dimensional measurement matrix, V k It is an m-dimensional measurement noise matrix, and the subscript k indicates time k.

[0109] Assuming the system noise is Gaussian white noise, the steps for solving the optimal state estimate using a linear Kalman filter are as follows:

[0110] Step 1: Prediction of the next state transition quantity:

[0111]

[0112] Step 2: Prediction error covariance matrix:

[0113]

[0114] Q k System process noise W k The variance matrix;

[0115] Step 3: Calculate the Kalman gain matrix:

[0116]

[0117] Step 4: Estimate the error covariance matrix:

[0118]

[0119] Where R k To measure noise V k The variance matrix;

[0120] Step 5: Optimal estimation of state transition quantities:

[0121]

[0122] The systems, devices, modules, or units described in the above embodiments can be implemented by computer chips or entities, or by products with certain functions. A typical implementation device is a computer. Specifically, a computer can be, for example, a personal computer, laptop computer, cellular phone, camera phone, smartphone, personal digital assistant, media player, navigation device, email device, game console, tablet computer, wearable device, or any combination of these devices.

[0123] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0124] The above embodiments should be understood as illustrative only and not as limiting the scope of protection of the present invention. After reading the description of the present invention, those skilled in the art can make various alterations or modifications to the present invention, and these equivalent changes and modifications also fall within the scope defined by the claims of the present invention.

Claims

1. A method for correcting the three-dimensional position of a pedestrian using strapdown inertial navigation, characterized in that, Includes the following steps: Pedestrian height correction steps: Obtain pedestrian height information using a barometer. Simultaneously, pedestrian height information is obtained by pre-integration using accelerometers. The pedestrian height information obtained from the two methods above is fused using a Kalman filter to obtain the height. This allows for the initial correction of pedestrian height information; Pedestrian pitch angle correction steps: Calculate the pitch angle using the relationship between pedestrian stride length, stride height, and pitch angle, integrate it with the attitude calculation pitch angle, and correct the pitch angle; Pedestrian 3D position correction steps: Correct the pedestrian's 3D position using the relationship between the pedestrian's pitch angle, stride length, and heading angle; The pedestrian pitch angle correction step involves: calculating the pitch angle using the relationship between pedestrian stride length, stride height, and pitch angle; integrating this calculation with the attitude-derived pitch angle; and correcting the pitch angle. Specifically, this includes: Calculating the step length of a pedestrian using a nonlinear step-size model ; The pitch angle of a pedestrian during walking is calculated by the relationship between the pedestrian's stride length, stride height, and pitch angle. ; The attitude calculation algorithm of the strapdown inertial navigation system calculates the attitude information of the pedestrian at each step, where the pitch angle is... ; The optimal pitch angle is estimated by fusing the two pitch angle information using the Kalman filter algorithm. ; The method utilizes a nonlinear step-size model to calculate the step length of each pedestrian. Specifically, it includes: Using a nonlinear step size model To calculate the stride length of each step taken by a pedestrian This formula is an empirical formula, where Based on experience points. This represents the maximum value of the square of the acceleration magnitude measured by the triaxial accelerometer in this step. This represents the minimum square of the acceleration modulus. When a pedestrian walks on flat ground, the step length calculated by this formula is the pedestrian's true horizontal displacement. When a pedestrian walks on a slope or going up or down stairs, this step length should be multiplied by the cosine of the pitch angle to obtain the pedestrian's true horizontal displacement. In the formula... Using a unit conversion factor, the pedestrian walks a certain distance with a fixed stride length, while accelerometer data is collected from the pedestrian's body to calculate the value of each step. Find the value and calculate the average; The specific relationship between the pedestrian's stride length, stride height, and pitch angle is as follows: The sum of the square of the step height and the square of the horizontal displacement of each step is equal to the square of the step length. The angle between the step length and the horizontal displacement is the pitch angle, that is, the pitch angle of each step is equal to the arcsine of the ratio of step height to step length. ; The stride height of a pedestrian's step is equal to the product of the stride length and the sine of the pitch angle. The horizontal displacement of a pedestrian's each step is equal to the product of the step length and the cosine of the pitch angle. , The stride height of each step taken by a pedestrian. The length of each step taken by a pedestrian. The pitch angle for each step a pedestrian takes.

2. The method for correcting the three-dimensional position of a pedestrian using strapdown inertial navigation according to claim 1, characterized in that, The pedestrian height correction steps specifically include: Step 1: Using the Laplace pressure formula Get the height corresponding to each step ,in These represent the height and pressure of the measuring point and the reference point, respectively. The average temperature of the measurement point and the reference point is taken as equal to the temperature of the reference point when the pedestrian's walking range is small. The height calculated by this formula The height of the measurement point is relative to the reference point, so the height at each step is equal to the current height minus the previous height; Step 2: Obtain acceleration information in the navigation coordinate system using the rotation matrix from the carrier coordinate system to the navigation coordinate system and the acceleration information measured by the accelerometer. , Acceleration information in the navigation coordinate system Let be the rotation matrix from the vehicle coordinate system to the navigation coordinate system. The acceleration information is obtained in the carrier coordinate system; after subtracting the influence of gravitational acceleration, the pedestrian's acceleration information along the Z-axis in the navigation coordinate system is obtained. The pedestrian's integral height is obtained by integrating the acceleration along the Z-axis twice. Step 3: Use the Kalman filter fusion algorithm to calculate the altitude from the barometer reading obtained in Step 1. and the accelerometer integral height obtained in Step 2 The height obtained after the first correction was obtained through fusion. In the Kalman fusion process, the integral height obtained by integrating with the accelerometer is... Altitude information measured by a barometer is used as a state transition quantity. As an observation.

3. The method for correcting the three-dimensional position of a pedestrian using strapdown inertial navigation according to claim 1, characterized in that, The pedestrian three-dimensional position correction specifically includes the following steps: The pedestrian step height is recalculated using the optimal pitch angle estimate obtained from the pitch angle correction algorithm. ; Correcting the horizontal displacement of pedestrians using the corrected pitch angle information ; The Kalman filter algorithm is used to fuse the height information from the height correction algorithm and this sub-algorithm to obtain the height. This allows for further correction of pedestrian height information; Using the corrected pedestrian horizontal displacement and attitude calculations to determine the pedestrian's horizontal position information Make corrections.

4. The method for correcting the three-dimensional position of a pedestrian using strapdown inertial navigation according to claim 3, characterized in that, The optimal pitch angle estimate obtained using the pitch angle correction algorithm is used to recalculate the pedestrian step height. Specifically, it includes: The optimal pitch angle obtained by the Kalman algorithm As the true pitch angle, the current step height is recalculated using the trigonometric relationship between the pitch angle and the pedestrian's stride length and stride height. ； The correction of the pedestrian's horizontal displacement is achieved by using the corrected pitch angle information. Specifically, it includes: The optimal pitch angle obtained by the Kalman algorithm As the true pitch angle, the horizontal displacement of the pedestrian in one step at the current moment is calculated through the trigonometric relationship between the pitch angle, the pedestrian's stride length, and the pedestrian's horizontal displacement. 。 5. The method for correcting the three-dimensional position of a pedestrian using strapdown inertial navigation according to claim 3, characterized in that, The Kalman filter algorithm is used to fuse the height information from the height correction algorithm and this sub-algorithm to obtain the height. This allows for further correction of pedestrian height information, specifically including: The obtained step height is obtained by using the Kalman filter fusion algorithm. With the calculated step height Fusion estimation to accurately estimate the step height for each step During Kalman fusion, the obtained step height is used as the reference. As a state transition quantity, the calculated step height As an observation; at this point, the optimal step height estimate To obtain accurate height information for each step of the pedestrian, the corrected height of the pedestrian relative to the starting point at the current moment can be obtained by combining the height information from all previous moments.

6. The method for correcting the three-dimensional position of a pedestrian using strapdown inertial navigation according to claim 3, characterized in that, The heading information calculated using the corrected pedestrian horizontal displacement and attitude is used to determine the pedestrian's horizontal position information. The corrections include: Corrected horizontal displacement Multiplying by the sine and cosine values ​​of the heading angle calculated at each attitude step yields the displacement of the pedestrian in the X and Y axes at each step in the navigation coordinate system. By combining the horizontal coordinate information from the previous step, the current horizontal coordinates of the pedestrian can be obtained.

Images