Inertial navigation error correction method based on indoor walking constrained terrain matching

Abstract

The inertial navigation error correction method based on indoor walking constraint terrain matching includes the following steps: Step 1. Constructing a standard path database; Step 2. Reconstructing the vehicle's trajectory based on planar two-dimensional coordinates and attitude angles; dividing the trajectory into multiple feature path segments to be calibrated based on its inflection points; Step 3. Replacing the coordinates of the planar inflection points in the trajectory with standard coordinates, where the standard coordinates are the inflection point coordinates in the standard path; Step 4. Performing two-dimensional correction on the trajectory. This invention corrects the cumulative error generated by the inertial navigation system during pedestrian path reconstruction by matching feature points in special terrain with indoor walking constraint paths, eliminating the error in inertial navigation trajectory estimation caused by human gait, and effectively improving the accuracy of inertial navigation positioning. This invention requires no additional hardware equipment, enabling continuous positioning in complex indoor scenarios and reducing the cost of indoor positioning.

Description

Technical Field

[0001] This invention belongs to the field of electronic information technology, and relates to positioning technology, specifically to an inertial navigation error correction method based on indoor walking constraint terrain matching. Background Technology

[0002] Currently, the mainstream indoor positioning methods include WiFi, Bluetooth, inertial navigation, and geomagnetic positioning. Among them, WiFi and Bluetooth positioning mainly use triangulation, and the more signals deployed, the higher the positioning accuracy. Inertial navigation positioning does not rely on external information and only requires a starting point to achieve autonomous and continuous positioning. Geomagnetic positioning achieves positioning through the regular characteristics of the indoor geomagnetic field, but geomagnetic signals are easily affected by the surrounding environment. Therefore, inertial navigation positioning shows great advantages: low cost, simple operation, easy deployment, unaffected by external signal interference, and can provide continuous positioning.

[0003] However, the accuracy of inertial navigation positioning is greatly limited by the precision of the devices, and it inevitably accumulates errors over time, making it difficult to achieve independent positioning and navigation over extended periods. Therefore, it is usually necessary to continuously calibrate the position calculation using external positioning information sources.

[0004] The invention patent No. 93110021 of Taiwan, entitled "Integrated Positioning System and Method for Vehicle", discloses an integrated positioning system and method for a vehicle. It uses the Global Positioning System (GPS) to provide the initial position, speed, and direction of the vehicle for the positioning output of the Inertial Navigation System (INS) at the next moment. After the GPS repositions the vehicle, the GPS positioning data is used to correct the positioning output of the INS, thus correcting the cumulative error of the INS.

[0005] Although this invention patent solves the problem of inertial navigation error accumulation, the GPS system is easily affected by terrain. Once the satellite signal is blocked by buildings, it cannot provide positioning information, and the inertial navigation system will also be unable to make comparisons to correct the error, causing the positioning system to malfunction.

[0006] Therefore, how to solve the problem of the accumulation of inertial navigation positioning error over time in complex indoor terrain scenarios is the technical problem that this invention aims to solve. Summary of the Invention

[0007] To overcome the shortcomings of existing technologies, this invention discloses an inertial navigation error correction method based on indoor walking constraint terrain matching.

[0008] The inertial navigation error correction method based on indoor walking constraint terrain matching described in this invention is characterized by comprising the following steps:

[0009] Step 1. Construct a standard path database, dividing the standard path into multiple standard feature path segments based on its own inflection points. Each standard feature path segment has one and only one inflection point as the standard feature point of that standard feature path segment.

[0010] Step 2. The mobile carrier equipped with an inertial navigation system completes the journey on a standard path and collects path information, which includes the two-dimensional coordinates, attitude angles and acceleration of the carrier at all time sampling points during the time period of the journey.

[0011] The trajectory of the carrier can be reconstructed based on two-dimensional plane coordinates and attitude angles;

[0012] The activity trajectory is divided into multiple feature path segments to be calibrated based on its own inflection points. Each feature path segment to be calibrated has one and only one inflection point as the feature point to be calibrated for that feature path segment.

[0013] Step 3. Replace the coordinates of the plane inflection points in the activity trajectory with standard coordinates, where the standard coordinates are the coordinates of the inflection points in the standard path;

[0014] Step 4. Perform two-dimensional correction on the activity trajectory, specifically as follows:

[0015] Step 41. Calculate and compare the linear correlation between each feature path segment to be calibrated and all standard feature path segments one by one, and select one or more standard feature path segments that match the feature path segment to be calibrated; if there is only one match, proceed to step 43 as the matching result, and if there is more than one match, proceed to step 42.

[0016] Step 42. Calculate the Euclidean distance between the feature point to be calibrated of the feature path segment to be calibrated and the standard feature points of multiple matching standard feature path segments. The closest distance is taken as the matching result, and proceed to step 43.

[0017] Step 43. Correct the coordinates of the feature points to be calibrated in the feature path segment to be calibrated to the coordinates of the standard feature points in the matching standard feature path segment, and correct the heading angle of the feature points to be calibrated in the feature path segment to be calibrated to the heading angle of the matching standard feature path segment.

[0018] Preferably, the inertial navigation system is an MPU6050 sensor.

[0019] Preferably, in step 2, the trajectory of the carrier is reconstructed based on the two-dimensional coordinates and attitude angles as follows;

[0020] Step 21. Estimate the carrier step size SL.

[0021]

[0022] In the formula, Let α be the step size. max and α min These are the maximum and minimum values ​​of acceleration collected in step 2, respectively, and k is an empirical parameter of the model;

[0023] Step 22. Calculate the direction of the carrier at each step.

[0024] Treating the heading angle φ collected in step 2 as an angle value in polar coordinates, the collected heading angle φ is transformed into coordinate points (cosφ, sinφ) in rectangular coordinates. Subtraction clustering is performed on the transformed rectangular coordinate points (cosφ, sinφ) to obtain cluster centers and cluster numbers. Then, k-means clustering is performed using the cluster centers and cluster numbers to classify all coordinate points, obtaining the rectangular coordinate points corresponding to the heading angle φ in each time period. This is then converted into angle values, which represent the heading angle corresponding to each time period. k ;

[0025] Step 23. Perform trough detection on the z-axis acceleration collected in Step 2 to obtain the time t at the trough point. i , will t i Within the corresponding time period in step 22, determine t. i The heading angle α at time k That is, the heading angle at the start of each step is α. k Using the heading angle α at the start of each step k Replaces the heading angle for each step;

[0026] Step 24. Based on the following calculation formula:

[0027] In the formula, (x i-1 ,y i-1 ) represents t i-1 The location of the time carrier, SL i Represents time t i The estimated step size of the carrier; x0, y0 are the coordinates of the starting point of the path;

[0028] The current two-dimensional coordinates (x, y) are obtained by updating the formula. i ,y i This allows us to reconstruct the two-dimensional activity trajectory.

[0029] Preferably, the method for determining the coordinates of the plane inflection point in step 3 is as follows: cluster the heading angles in the attitude angles at each time point in the activity trajectory, and find the time when the heading angle changes in the clustered heading angles, which corresponds to the plane inflection point in the activity trajectory.

[0030] Preferably, the specific method for calculating the linear correlation and performing screening in step 41 is as follows:

[0031] Define the x-coordinate of a point in a standard feature path segment as variable PX, and the y-coordinate as variable PY.

[0032] The x-coordinates of points in the feature path segment to be calibrated constitute the variable QX, and the y-coordinates constitute the variable QY.

[0033] Calculate the linear correlation coefficient ρ PXQX and ρ PYQY The formula calculation method is as follows:

[0034] ρ PXQX = Cov(PX,QX) / ( σ PX *σ QX )

[0035] ρ PYQY = Cov(PY,QY) / ( σ PY *σ QY )

[0036] Cov(PX,QX) is the covariance between variables PX and QX, σ PX , σ PY , σ QX , σ QY Let PX, PY, QX, and QY be the standard deviations of the variables;

[0037] When ρ PXQX and ρ PYQY If both values ​​are greater than the set threshold, then the two are determined to meet the matching conditions.

[0038] Preferably, the method further includes step 5. Performing three-dimensional correction on the activity trajectory, specifically as follows:

[0039] Step 51. Based on the acceleration and attitude angle information of all sampling points obtained in Step 2, extract the vertical acceleration component α. zn With respect to the vertical acceleration α zn Perform valley value detection;

[0040] Step 52. The vertical acceleration α detected in step 51 zn Valley values ​​are subjected to subtractive clustering to obtain cluster centers and the number of clusters N;

[0041] If N=1, it is determined to be a flat ground walking state, and no three-dimensional correction is performed;

[0042] If N=2, then it is determined that there is an upstairs or downstairs state, and the subsequent steps are continued;

[0043] Step 53. Calculate the vertical acceleration component α of adjacent sampling points.zn The difference is integrated twice to obtain the change in vertical displacement over time, i.e., the change in height Δh. The sum of the height changes Δh, h, is calculated. If h > 0, it is determined to be going upstairs; if h > 0, it is determined to be going downstairs.

[0044] Step 54. For the vertical acceleration α zn The valley values ​​are subjected to k-means clustering, and the number of clusters N obtained in step 52 is input as the number of clusters for k-means clustering to obtain the vertical acceleration α with two cluster centers. zn Valley values ​​A1, A2, ..., A i A j ,……,A n The corresponding times are t1, t2, ..., t i ,t j ,……,t n n is the total number of sampling points;

[0045] Step 55. For A1, A2, ..., A i A j ,……,A n The difference between two adjacent valley values ​​is calculated sequentially to determine the start and end times of the carrier going up and down stairs. The determination method is as follows:

[0046] When A j -A i If the value is less than 0, then time j is determined to be the start time of going up or down the stairs;

[0047] When A j -A i If the value is greater than 0, then time j is determined to be the end time of going up or down the stairs;

[0048] When A j -A i If the value is 0, then it is determined that at time j, the person is walking on flat ground or going up or down stairs.

[0049] The start and end times of two consecutive up and down stairs are labeled as m and n, respectively.

[0050] Step 56. For each pair of adjacent start and end times of going up and down stairs, detect the vertical acceleration α during the time interval from m to n. zn The number of valleys is L, where L represents the number of steps a pedestrian goes up or down during the time interval from m to n. This can be calculated using L*h. stair Obtain the actual height H, h that a pedestrian rises or falls during the process of going up or down the stairs. stair The height of each step;

[0051] Step 57. Based on the two-dimensional coordinates of the stairwell, and using the actual height H obtained in Step 56, correct the vertical coordinates of the carrier at the stairwell to construct the three-dimensional trajectory of the carrier.

[0052] Preferably, step 56 includes a correction to the actual height H, specifically:

[0053] Determine the number of floors M that the carrier climbs based on the actual height, using the following method:

[0054] when Let M=1;

[0055] when Let M=2;

[0056] when Let M=3;

[0057] ...

[0058] And so on; where h total For the building's floor height,

[0059] Using M*h total Obtain the terrain height H' as the pedestrian goes up or down the stairs, and update the actual height H using the terrain height H'.

[0060] The inertial navigation error correction method based on indoor walking constraint terrain matching described in this invention corrects the cumulative error generated by the inertial navigation system during pedestrian path reconstruction by matching feature points in special terrain through indoor walking constraint path matching and using the two-dimensional coordinates of indoor feature points obtained from engineering drawings or on-site surveys. This eliminates the error of human gait in inertial navigation trajectory estimation and effectively improves the accuracy of inertial navigation positioning. This invention does not require additional hardware equipment and can achieve continuous positioning in complex indoor scenes, reducing the cost of indoor positioning. Attached Figure Description

[0061] Figure 1 This is a flowchart illustrating a specific implementation of the inertial navigation error correction method of the present invention;

[0062] Figure 2 This is a flowchart illustrating a specific implementation of the matching method for standard feature path segments and feature path segments to be calibrated according to the present invention.

[0063] Figure 3 This is a schematic diagram of a two-dimensional correction process in a specific implementation;

[0064] Figure 4 This is a schematic diagram of the heading angle before and after terrain matching correction of turning feature points in a specific implementation;

[0065] Figure 5 This is a schematic diagram of the two-dimensional walking path before and after terrain matching and correction of indoor turning feature points in a specific implementation.

[0066] Figure 6 This is a flowchart illustrating the three-dimensional correction process in a specific implementation.

[0067] Figure 7 This is a schematic diagram comparing the paths before and after three-dimensional correction in a specific implementation method.

[0068] Figure 8 This is a schematic diagram illustrating the acquisition of acceleration valley values ​​in a specific implementation method;

[0069] Figure 9 This is a schematic diagram obtained after clustering acceleration valley values ​​in a specific implementation. Detailed Implementation

[0070] The specific embodiments of the present invention will be described in further detail below.

[0071] To achieve the above objectives, the present invention adopts the following technical solution:

[0072] The inertial navigation error correction method based on indoor walking constraint terrain matching includes the following steps:

[0073] Step 1. Construct a standard path database, dividing the standard path into multiple standard feature path segments based on its own inflection points. Each standard feature path segment has one and only one inflection point as the standard feature point of that standard feature path segment.

[0074] Standard route databases are typically based on original building design drawings or actual measurement drawings. They obtain the two-dimensional coordinates of the building's interior using engineering drawings or on-site surveying, and determine and mark the two-dimensional coordinates of characteristic locations such as the start point, end point, stairwell, and turning point in the standard route.

[0075] Step 2. The mobile carrier equipped with an inertial navigation system completes the journey on a standard path and collects path information, which includes the two-dimensional coordinates, attitude angles and acceleration of the carrier at all time sampling points during the time period of the journey.

[0076] The trajectory of the carrier can be reconstructed based on two-dimensional plane coordinates and attitude angles;

[0077] A typical application of this invention is path correction when a person carries a portable inertial navigation system to walk inside a building, so as to facilitate the positioning and navigation of the person. However, it can also be applied to applications such as indoor search and rescue by police dogs and indoor trajectory collection by robots.

[0078] The activity trajectory is divided into multiple feature path segments to be calibrated based on its own inflection points. Each feature path segment to be calibrated has one and only one inflection point as the feature point to be calibrated for that feature path segment.

[0079] A specific implementation method for reconstructing the carrier's trajectory based on two-dimensional coordinates and attitude angles in step 2 is as follows:

[0080] Step 21. Estimate the carrier step size SL.

[0081]

[0082] In the formula, Let α be the step size. max and α min These are the maximum and minimum values ​​of acceleration collected in step 2, respectively, and k is an empirical parameter of the model;

[0083] Step 22. Calculate the direction of the carrier at each step.

[0084] Treating the heading angle φ collected in step 2 as an angle value in polar coordinates, the collected heading angle φ is transformed into coordinate points (cosφ, sinφ) in rectangular coordinates. Subtraction clustering is performed on the transformed rectangular coordinate points (cosφ, sinφ) to obtain cluster centers and cluster numbers. Then, k-means clustering is performed using the cluster centers and cluster numbers to classify all coordinate points, obtaining the rectangular coordinate points corresponding to the heading angle φ in each time period. This is then converted into angle values, which represent the heading angle corresponding to each time period. k ;

[0085] Step 23. Perform trough detection on the z-axis acceleration collected in Step 2 to obtain the time t at the trough point. i Since the acceleration reaches its trough at the trough point, it is considered to be the moment when both feet are on the ground, which is the starting point of the next step.

[0086] t i Within the corresponding time period in step 22, determine t. i The heading angle α at time k That is, the heading angle at the start of each step is α. k Using the heading angle α at the start of each step k Replaces the heading angle for each step;

[0087] Step 24. Based on the following calculation formula:

[0088]

[0089] In the formula, (x i-1 ,y i-1 ) represents t i-1 The location of the time carrier, SL i Represents time t i The estimated step size of the carrier; x0, y0 are the coordinates of the starting point of the path;

[0090] The subscripts i and i-1 represent two adjacent different times, and the set of all values ​​of i is defined as A1;

[0091] The current two-dimensional coordinates (x, y) are obtained by updating the formula. i ,y i This allows us to reconstruct the two-dimensional activity trajectory.

[0092] Step 3. Replace the coordinates of the plane inflection points in the activity trajectory with standard coordinates, where the standard coordinates are the coordinates of the inflection points in the standard path;

[0093] Based on the heading angles after clustering in step 2, find the time t when the heading angle changes. j The moment when the pedestrian makes a turning motion is t. j , corresponding to the turning point in the indoor terrain; j∈A1;

[0094] Based on the two-dimensional coordinates (x, y) of the turning point in the engineering drawings or on-site surveying. j ,y j ), replacing t j Coordinates of the corresponding points in the two-dimensional track map at any given time ;

[0095] The two-dimensional walking path to be calibrated is a series of two-dimensional coordinates (x, y, z). i ,y i A set of ), containing a subset of two-dimensional coordinates (x, y). j ,y j The set of ) is the turning point.

[0096] Step 4. Perform two-dimensional correction on the activity trajectory, specifically as follows:

[0097] Step 41. Calculate and compare the linear correlation between each feature path segment to be calibrated and all standard feature path segments one by one, and select one or more standard feature path segments that match the feature path segment to be calibrated; if there is only one match, proceed to step 43 as the matching result, and if there is more than one match, proceed to step 42.

[0098] The specific method for calculating the linear correlation and performing screening in step 41 can be as follows:

[0099] Define the x-coordinate of a point in a standard feature path segment as variable PX, and the y-coordinate as variable PY.

[0100] The x-coordinates of points in the feature path segment to be calibrated constitute the variable QX, and the y-coordinates constitute the variable QY.

[0101] Calculate the linear correlation coefficient ρ PXQX and ρ PYQY The formula calculation method is as follows:

[0102] ρ PXQX = Cov(PX,QX) / ( σ PX *σ QX )

[0103] ρ PYQY = Cov(PY,QY) / ( σ PY *σ QY )

[0104] Cov(PX,QX) is the covariance between variables PX and QX, σ PX , σ PY , σ QX , σ QY Let PX, PY, QX, and QY be the standard deviations of the variables;

[0105] When ρ PXQX and ρ PYQY If both values ​​are greater than the set threshold, then the two are determined to meet the matching conditions.

[0106] Step 42. Calculate the Euclidean distance between the feature point to be calibrated of the feature path segment to be calibrated and the standard feature points of multiple matching standard feature path segments. The closest distance is taken as the matching result, and proceed to step 43.

[0107] Step 43. Correct the coordinates of the feature points to be calibrated in the feature path segment to be calibrated to the coordinates of the standard feature points in the matching standard feature path segment, and correct the heading angle of the feature points to be calibrated in the feature path segment to be calibrated to the heading angle of the matching standard feature path segment.

[0108] After the above two-dimensional correction is completed, three-dimensional correction can be performed on the trajectory of the activity that has displacement in the vertical direction.

[0109] It also includes step 5. Performing three-dimensional correction on the activity trajectory, specifically:

[0110] Step 51. Based on the acceleration and attitude angle information of all sampling points obtained in Step 2, extract the vertical acceleration component α. zn With respect to the vertical acceleration α zn Perform valley value detection;

[0111] Step 52. The vertical acceleration α detected in step 51 zn Valley values ​​are subjected to subtractive clustering to obtain cluster centers and the number of clusters N;

[0112] If N=1, it is determined to be a flat ground walking state, and no three-dimensional correction is performed;

[0113] If N=2, then it is determined that there is an upstairs or downstairs state, and the subsequent steps are continued;

[0114] like Figure 8 As shown, Figure 8 The vertical axis represents the acceleration value (m / s). 2 ), it can be seen Figure 8 Two types of acceleration valley values ​​are given in the text. Figure 8 The acceleration valley value less than zero is divided at -0.2. An acceleration valley value greater than -0.2 represents walking on flat ground, and an acceleration valley value less than -0.2 represents walking up stairs. Figure 8 The actual acceleration data collected was from going upstairs, walking briefly on flat ground, and then going upstairs again, which is consistent with the typical setup of a stairwell with a small platform.

[0115] After clustering the acceleration valleys, the resulting clustered acceleration valley curves are shown below. Figure 9 As shown, the clustering is divided into two types of acceleration values. The acceleration valley values ​​of all sampling points are divided into two categories: the larger ones represent walking on flat ground, and the smaller ones represent walking upstairs.

[0116] Step 53. Calculate the vertical acceleration component α of adjacent sampling points. zn The difference is integrated twice to obtain the change in vertical displacement over time, i.e., the change in height Δh. The sum of the height changes Δh, h, is calculated. If h > 0, it is determined to be going upstairs; if h > 0, it is determined to be going downstairs.

[0117] Step 54. For the vertical acceleration α zn The valley values ​​are subjected to k-means clustering, and the number of clusters N obtained in step 52 is input as the number of clusters for k-means clustering to obtain the vertical acceleration α with two cluster centers. zn Valley values ​​A1, A2, ..., A i A j ,……,A n The corresponding times are t1, t2, ..., t i ,t j ,……,t n n is the total number of sampling points;

[0118] Step 55. For A1, A2, ..., A i A j ,……,A n The difference between two adjacent valley values ​​is calculated sequentially to determine the start and end times of the carrier going up and down stairs. The determination method is as follows:

[0119] When A j -A i If the value is less than 0, then time j is determined to be the start time of going up or down the stairs;

[0120] When A j -A iIf the value is greater than 0, then time j is determined to be the end time of going up or down the stairs;

[0121] When A j -A i If the value is 0, then it is determined that at time j, the person is walking on flat ground or going up or down stairs.

[0122] The start and end times of two consecutive up and down stairs are labeled as m and n, respectively.

[0123] Step 56. For each pair of adjacent start and end times of going up and down stairs, detect the vertical acceleration α during the time interval from m to n. zn The number of valleys is L, where L represents the number of steps a pedestrian goes up or down during the time interval from m to n. This can be calculated using L*h. stair Obtain the actual height H, h that a pedestrian rises or falls during the process of going up or down the stairs. stair The height of each step;

[0124] Step 57. Based on the two-dimensional coordinates of the stairwell, and using the actual height H obtained in Step 56, correct the vertical coordinates of the carrier at the stairwell to construct the three-dimensional trajectory of the carrier. Specific implementation examples:

[0126] like Figure 1 As shown, the inertial navigation error correction method of the present invention specifically includes the following steps:

[0127] 1) Determine the indoor walking constraint path. Obtain the two-dimensional coordinate position of the building interior through engineering drawings or on-site surveying. Determine and mark the two-dimensional coordinates of characteristic positions such as the start point, end point, stairwell, and turning point in the pedestrian walking constraint path.

[0128] 2) The inertial navigation system acquires acceleration and angular velocity information during human walking. The inertial navigation system adopts a strap-down inertial navigation system, which does not require a stable platform. The inertial sensing unit integrating accelerometer and gyroscope is directly fixed to the waist of the human body to acquire acceleration and angular velocity information in the current coordinate system, i.e., the carrier coordinate system, and acceleration information in the navigation coordinate system during human walking.

[0129] Step 2) specifically involves:

[0130] 2.1) An inertial sensing unit integrating an accelerometer and gyroscope is attached to the waist of the human body to collect data in the carrier coordinate system ox. b y b z b Acceleration during human movement x, y, z and angular velocity w x , w y , w z ;

[0131] 2.2) The angular velocity w collected x , w y , w z The attitude angles of the current accelerometer-corresponding navigation coordinate system are calculated, and the output data is further processed using the digital motion processor integrated within the inertial sensing unit and the corresponding motion processing database.

[0132] The inertial sensing unit uses the MPU6050 sensor, whose internal structure includes a three-axis accelerometer, a three-axis gyroscope, a temperature sensor, and a scalable digital motion processor (DMP). The MPU6050 reads raw data from the accelerometer and gyroscope. The DMP in the MPU6050 converts the raw angular velocity data into quaternion data, outputting quaternions in q30 format (a 230-fold amplification of floating-point numbers). Preprocessing is then performed on the output data.

[0133] q0 = quat[0] / q30

[0134] q1 = quat[1] / q30

[0135] q2 = quat[2] / q30

[0136] q3 = quat[3] / q30

[0137] We obtain floating-point numbers q0, q1, q2, q3, where quat[0]~quat[3] are quadruple data;

[0138] Substitute the floating-point numbers q0, q1, q2, and q3 into the conversion formula:

[0139] Roll angle γ = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3

[0140] Pitch angle θ = asin(-2*q1*q3 + 2*q0*q2) * 57.3

[0141] The heading angle φ = atan²(2*(q₁*q₂ + q₀*q₃), q₀*q₀ + q₁*q₁ - q₂*q₂ - q₃*q₃) * 57.3,

[0142] atan2 is the arctangent function.

[0143] We obtain the roll angle, pitch angle, and yaw angle, i.e., Euler angles, where all angle information is in degrees.

[0144] The attitude angles can be obtained as follows: pitch angle θ, roll angle γ, and yaw angle φ.

[0145] 2.3) Using the attitude angles θ, γ, and φ obtained in 2.2), the acceleration measurement value a x a y a z Coordinate system transformation is performed using a transformation matrix, that is, the carrier coordinate system ox is transformed. b y b z b The acceleration measurement value a x a y a z Projected onto navigation coordinate system ox n y n z n Below, the navigation coordinate system ox is obtained. n y n z n acceleration xn, yn, zn;

[0146] In step 2.3), the navigation coordinate system ox n y n z n Using the conventional local horizontal "Northeast Sky" geographic coordinate system ox n y n z n Let's define it as an n-system with x-axis. n y n , z n The specific steps for coordinate system transformation are as follows:

[0147] ox n y n z n Rotating around the axis by an angle φ according to the right-hand rule yields ox1y1z1, which is the first rotation, corresponding to the heading angle φ. The direction cosine matrix corresponding to the transformation process is:

[0148]

[0149] ox1y1z1 is rotated about the axis by an angle θ according to the right-hand rule to obtain ox2y2z2, which is the second rotation, corresponding to the pitch angle θ. The direction cosine matrix corresponding to the transformation process is:

[0150]

[0151] ox2y2z2 is rotated about the axis by an angle γ according to the right-hand rule to obtain ox3y3z3, which is the third rotation, corresponding to the roll angle γ. The direction cosine matrix corresponding to the transformation process is:

[0152]

[0153] The coordinate system transformation matrix can then be integrated as follows:

[0154]

[0155] The original acceleration signal α output by the sensor in the carrier coordinate system x , α y , α z Acceleration signal α converted to navigation coordinate system xn , α yn , α zn for:

[0156]

[0157] 3) Based on the acceleration and heading angle information obtained in step 2), the pedestrian dead reckoning (PDR) method is used to calculate the pedestrian's walking trajectory and location information, and a two-dimensional track map that changes continuously over time is obtained to realize two-dimensional path reconstruction;

[0158] Step 3) involves reconstructing the path using the pedestrian dead reckoning method, which mainly includes estimating the pedestrian's stride length and calculating the direction of movement. Specifically, this includes the following steps:

[0159] 3.1) Estimate pedestrian stride length. Stride length estimation uses a model training method, fixing the number of steps and the stride length of each step, and employing the Weinberg model:

[0160]

[0161] In the formula, Let α be the step size. max and α min These are the maximum and minimum values ​​of acceleration data in one step, respectively. k is a model parameter. The parameter k is solved by substituting a large amount of acceleration data into the formula with a fixed step size. The average value of the solved k is taken as the empirical k value of the model.

[0162] 3.2) Calculate the direction of each step taken by the pedestrian. This direction calculation mainly involves calculating the heading angle. The collected heading angle φ is considered as an angle value in polar coordinates. Based on the conversion relationship between rectangular and polar coordinate systems, assuming that the coordinates of any point P in the plane are P(x,y) in the rectangular coordinate system and P(ρ,θ) in the polar coordinate system, then:

[0163]

[0164] The coordinates in the rectangular coordinate system can be calculated from the coordinates in the polar coordinate system using the above formula;

[0165]

[0166] The above formula can be used to calculate the coordinates in the polar coordinate system from the coordinates in the rectangular coordinate system.

[0167] By setting ρ=1, the collected heading angle φ can be converted to coordinate points (cosφ, sinφ) in a rectangular coordinate system. Subtraction clustering is then performed on these converted rectangular coordinate points (cosφ, sinφ) to obtain cluster centers and cluster numbers. Finally, k-means clustering is performed using the cluster centers and cluster numbers to classify all coordinate points, resulting in the rectangular coordinate points corresponding to the heading angle φ for each time period. These coordinates are then converted to angle values, which represent the heading angle for each time period. k ;

[0168] 3.3) Further, in the clustered heading angles, find the moments when the heading angle changes. This corresponds to a turning point in the indoor terrain.

[0169] 3.4) The moment when both feet of the pedestrian simultaneously touch the ground is taken as the starting moment of each step. This corresponds to the moment when the acceleration in the z-axis direction (direction of gravity) is at its minimum. By detecting the troughs in the z-axis acceleration waveform, the time t at the trough point is obtained. i , will t i Within the corresponding time period in 3.2), determine t. i The heading angle α at time k That is, the heading angle at the start of each step is α. k Using the heading angle α at the start of each step k Replaces the heading angle for each step;

[0170] 3.5) Formula for calculating pedestrian tracks:

[0171]

[0172] In the formula, (x i-1 ,y i-1 ) represents t i-1 The location of pedestrians at any given time, SL i Represents time t i The estimated stride length of a pedestrian;

[0173] The above formula is used to update the pedestrian's current two-dimensional coordinates (x, y) during the walking process. i ,y i This allows us to reconstruct the two-dimensional walking path of the traveler.

[0174] 4) Based on the heading angles after clustering in step 3.2), find the times when the heading angles change. That is, the moment when the pedestrian makes a turning motion. This corresponds to a turning point in the indoor terrain.

[0175] Based on the two-dimensional coordinates (x, y) of the turning point in the engineering drawings or on-site surveying. j ,y j ), find t j The coordinates of the corresponding points in the two-dimensional track diagram at any given time ( , );

[0176] 5) In indoor walking-constrained terrain, turning points and stairwells are selected as feature points. The two-dimensional coordinates of the feature points are obtained using engineering drawings or on-site surveying. The walking path reconstructed by the pedestrian dead reckoning (PDR) method based on the inertial navigation system is corrected. Specifically, it is divided into two-dimensional and three-dimensional path correction. Two-dimensional path correction mainly uses turning points, while three-dimensional path correction mainly uses stairwells and turning points.

[0177] A specific implementation of the two-dimensional path correction process is as follows: Figure 2 As shown, the specific steps are as follows:

[0178] 5.1) Construct a feature point matching path library, such as... Figure 2 As shown, this includes all possible standard feature path segments containing each feature point in the standard path obtained from engineering drawings or on-site surveys, denoted as the feature path segment set P. Each standard feature path segment contains one and only one feature point, and the coordinates of the points in each standard feature path segment are labeled as (x1, y1), (x2, y2), ..., (x...). n ,y n Let n be the total number of measurement points in the standard characteristic path segment. Then, the x-coordinate of each point in the standard characteristic path segment constitutes the variable PX, and the y-coordinate constitutes the variable PY, which are expressed as follows:

[0179] PX=(x1,x2,……,x n ),

[0180] PY=(y1,y2,……,y n );

[0181] 5.2) Extract the feature path segment to be calibrated for each feature point in the activity trajectory to be calibrated, such as... Figure 3 As shown, the feature path segment to be calibrated, including the location of each feature point in the activity trajectory, is labeled as the feature path segment set Q. Each feature path segment to be calibrated contains one and only one feature point to be calibrated, and the coordinates of the points in each feature path segment are labeled as follows: , , ..., Then, the x-coordinate of each point in the feature path segment to be calibrated constitutes the variable QX, and the y-coordinate constitutes the variable QY, which are expressed as follows:

[0182] QX=( , ,……, ),

[0183] QY=( , ,……, );

[0184] 5.3) Calculate the linear correlation coefficient between the standard path and the feature point segment to be calibrated in the activity trajectory, that is, calculate the linear correlation coefficient ρ between the variables formed by the x-coordinates of each point in the feature path segment sets P and Q respectively. PXQX The linear correlation coefficient ρ between the variables formed by the ordinates of each point in the feature path segment sets P and Q. PYQY The formula calculation method is as follows:

[0185] ρ PXQX = Cov(PX,QX) / ( σ PX *σ QX )

[0186] ρ PYQY = Cov(PY,QY) / ( σ PY *σ QY )

[0187] Where Cov(PX,QX) is the covariance between variables PX and QX, and σ PX , σ PY , σ QX , σ QY Let PX, PY, QX, and QY be the standard deviations of the variables;

[0188] 5.4) Determine the linear correlation between the feature points in the standard path and the path to be calibrated, and the determination method is as follows:

[0189] For a standard feature path segment containing a feature point in the standard path and a feature path segment to be calibrated containing a feature point in the path to be calibrated,

[0190] When ρ PXQX >0.8 and ρ PYQY If the correlation is greater than 0.8, the two are determined to be extremely strongly positively correlated and meet the matching conditions, and their corresponding feature points are matched.

[0191] Otherwise, the two are determined to be not extremely strongly positively correlated, do not meet the matching conditions, and cannot be matched with their corresponding feature points;

[0192] Using the above method, the linear correlation between all standard feature path segments in the standard path and all feature path segments to be calibrated in the path to be calibrated is determined, and feature points that meet the matching conditions are matched.

[0193] 5.5) If step 5.4) determines that a certain feature path segment in the path to be calibrated matches two or more standard feature path segments in the standard path, then further calculate the feature path segment Q containing the corresponding feature point in the path to be calibrated and the standard path. i and P i The Euclidean distance between them is calculated as follows:

[0194] , i = 1, 2, ..., n;

[0195] In the above formula, (x i ,y i ) represents the coordinates of the feature point corresponding to the feature path segment to be calibrated. , () represents the coordinates of the feature points of the standard feature path segment;

[0196] Using the above formula, find the two feature path segments with the closest Euclidean distance between the feature points, and match the feature points corresponding to the two.

[0197] If step 5.4) determines that a certain characteristic path segment in the path to be calibrated matches the only standard characteristic path segment in the standard path, then no further calculation is needed, and the two can be directly matched.

[0198] Steps 5.4 and 5.5 are used to match each feature path segment to be calibrated in the path to be calibrated with the standard feature path segment in the standard path one by one.

[0199] 5.6) Correct the coordinates of feature points in the path to be calibrated to the coordinates of feature points in the matching standard path, and correct the heading angles at the feature points in the path to be calibrated to the heading angles at the feature points in the matching standard path. The heading angles before and after terrain matching correction using turning feature points are as follows: Figure 4 As shown;

[0200] Based on steps 1) to 5), the indoor two-dimensional walking path was obtained before and after terrain matching correction using turning feature points, as shown below. Figure 5 As shown; Figure 5 As can be seen, at the inflection point, the path before correction was adjusted to the actual path. Figure 5 To demonstrate the correction effect, the path error was increased; the actual error is less than [a certain value]. Figure 5 As shown.

[0201] A specific process of three-dimensional correction is as follows: Figure 6 As shown, the specific steps are as follows:

[0202] 5.7) Based on the acceleration and heading angle information obtained in step 2), extract the vertical acceleration component α. zn With respect to the vertical acceleration α zn Perform valley value detection;

[0203] 5.8) The vertical acceleration α detected in step 5.7) zn Valley values ​​are used for subtraction clustering to obtain cluster centers and cluster number N. Based on the obtained cluster number N, it is determined whether the pedestrian's motion state has changed during the walking process. Since the area near the stairwell is an indoor walking constraint terrain, the pedestrian only has two motion states near the stairwell: flat ground to go upstairs or flat ground to go downstairs. The acceleration valley values ​​of the pedestrian are significantly different when walking on flat ground and going up and downstairs, resulting in two different categories after clustering.

[0204] Therefore, the determination method is as follows:

[0205] If N=1, it is determined to be a flat walking state, and the following steps are not performed;

[0206] If N=2, then determine whether the pedestrian is going upstairs or downstairs, and continue with the following steps to further determine the pedestrian's movement status;

[0207] 5.9) Perform a second integration on the vertical acceleration difference to obtain the vertical displacement over time, i.e., the height change Δh. Calculate the cumulative sum h of the height changes Δh. Use the cumulative sum h to determine the pedestrian's status when going up or down stairs. The determination method is as follows:

[0208] If h > 0, then it is determined to be going upstairs;

[0209] If h > 0, then it is determined to go downstairs;

[0210] 5.10) Using the cluster number N=2 obtained in step 5.8), the vertical acceleration α is... zn Valley values ​​are subjected to k-means clustering to obtain the vertical acceleration α with two cluster centers. zn Valley values ​​A1, A2, ..., A i A j ,……,A n The corresponding times are t1, t2, ..., t i ,t j ,……,t n ;;

[0211] 5.11) The vertical acceleration α after clustering zn Valley data A1, A2, ..., A i Aj ,……,A n Perform a difference operation to determine the start and end times of pedestrians going up and down stairs. The determination method is as follows:

[0212] When A j -A i If j < 0, then time j is determined to be the start time of going up or down the stairs, that is, the time when the person passes the stairwell.

[0213] When A j -A i If the value is greater than 0, then time j is determined to be the end time of going up or down the stairs, that is, the time when the person passes the stairwell.

[0214] When A j -A i If the value is 0, then it is determined that at time j, the person is walking on flat ground or going up or down stairs.

[0215] The start and end times for going up and down the stairs are labeled as m and n, respectively.

[0216] 5.12) Map the start and end times m and n obtained in step 5.11) for going up and down the stairs to the vertical acceleration α. zn In the process, the vertical acceleration α during the time interval from m to n was detected. zn If the number of valleys is L, then L represents the number of steps a pedestrian goes up or down the stairs during the time interval from m to n, and the height h of each step is obtained using engineering drawings or on-site surveying. stair and the floor height h of each floor total Using L*h stair Obtain the actual height H that a pedestrian rises or falls during the process of going up or down the stairs.

[0217] The actual height H can be corrected by first determining the floor number M that the pedestrian is walking on. The determination method is as follows:

[0218] when Let M=1;

[0219] when Let M=2;

[0220] when Let M=3;

[0221] ...

[0222] And so on;

[0223] Using M*h total The terrain height H' of a pedestrian going up or down stairs is obtained, and the actual height H is corrected using the terrain height H'.

[0224] 5.13) Based on the two-dimensional coordinates of the stairwells obtained from the engineering drawings or on-site surveys, connect the coordinates of the stairwells on each floor to construct a three-dimensional flight path map of pedestrians, thereby correcting the two-dimensional coordinates of pedestrians passing through the stairwells.

[0225] Based on steps 1) to 5), we obtained schematic diagrams of the 3D activity trajectory before and after terrain matching correction using stairwells and turning feature points for the indoor 3D walking path, as shown below. Figure 7 As shown.

[0226] The above embodiments are merely preferred embodiments and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

[0227] The foregoing descriptions are preferred embodiments of the present invention. Unless there is a clear contradiction between the preferred embodiments or a prerequisite for a particular preferred embodiment, the preferred embodiments can be arbitrarily combined and used. The embodiments and specific parameters described are only for clearly illustrating the inventor's invention verification process and are not intended to limit the scope of patent protection of the present invention. The scope of patent protection of the present invention shall still be determined by its claims. Similarly, any equivalent structural changes made based on the description and drawings of the present invention shall also be included within the scope of protection of the present invention.

Claims

1. An inertial navigation error correction method based on indoor walking constraint terrain matching, characterized in that... It includes the following steps: Step 1. Construct a standard path database, dividing the standard path into multiple standard feature path segments based on its own inflection points. Each standard feature path segment has one and only one inflection point as the standard feature point of that standard feature path segment. Step 2. The mobile carrier equipped with an inertial navigation system completes the journey on a standard path and collects path information, which includes the two-dimensional coordinates, attitude angles and acceleration of the carrier at all time sampling points during the time period of the journey. The trajectory of the carrier can be reconstructed based on two-dimensional plane coordinates and attitude angles; The activity trajectory is divided into multiple feature path segments to be calibrated based on its own inflection points. Each feature path segment to be calibrated has one and only one inflection point as the feature point to be calibrated for that feature path segment. Step 3. Replace the coordinates of the plane inflection points in the activity trajectory with standard coordinates, where the standard coordinates are the coordinates of the inflection points in the standard path; Step 4. Perform two-dimensional correction on the activity trajectory, specifically as follows: Step 41. Calculate and compare the linear correlation between each feature path segment to be calibrated and all standard feature path segments one by one, and select one or more standard feature path segments that match the feature path segment to be calibrated; if there is only one match, proceed to step 43 as the matching result, and if there is more than one match, proceed to step 42. Step 42. Calculate the Euclidean distance between the feature point to be calibrated of the feature path segment to be calibrated and the standard feature points of multiple matching standard feature path segments. The closest distance is taken as the matching result, and proceed to step 43. Step 43. Correct the coordinates of the feature points to be calibrated in the feature path segment to be calibrated to the coordinates of the standard feature points in the matching standard feature path segment, and correct the heading angle of the feature points to be calibrated in the feature path segment to be calibrated to the heading angle of the matching standard feature path segment.

2. The inertial navigation error correction method based on indoor walking constraint terrain matching as described in claim 1, characterized in that, The inertial navigation system is an MPU6050 sensor.

3. The inertial navigation error correction method based on indoor walking constraint terrain matching as described in claim 1, characterized in that, In step 2, the trajectory of the carrier is reconstructed based on the two-dimensional coordinates and attitude angles as follows; Step 21. Estimate the carrier step size SL. In the formula, Let α be the step size. max and α min These are the maximum and minimum values ​​of acceleration collected in step 2, respectively, and k is an empirical parameter of the model; Step 22. Calculate the direction of the carrier at each step. Treating the heading angle φ collected in step 2 as an angle value in polar coordinates, the collected heading angle φ is transformed into coordinate points (cosφ, sinφ) in rectangular coordinates. Subtraction clustering is performed on the transformed rectangular coordinate points (cosφ, sinφ) to obtain cluster centers and cluster numbers. Then, k-means clustering is performed using the cluster centers and cluster numbers to classify all coordinate points, obtaining the rectangular coordinate points corresponding to the heading angle φ in each time period. This is then converted into angle values, which represent the heading angle corresponding to each time period. k ; Step 23. Perform trough detection on the z-axis acceleration collected in Step 2 to obtain the time t at the trough point. i , will t i Within the corresponding time period in step 22, determine t. i The heading angle α at time k That is, the heading angle at the start of each step is α. k Using the heading angle α at the start of each step k Replaces the heading angle for each step; Step 24. Based on the following calculation formula: In the formula, (x i-1 ,y i-1 ) represents t i-1 The location of the time carrier, SL i Represents time t i The estimated step size of the carrier; x0, y0 are the coordinates of the starting point of the path; The current two-dimensional coordinates (x, y) are obtained by updating the formula. i ,y i This allows us to reconstruct the two-dimensional activity trajectory.

4. The inertial navigation error correction method based on indoor walking constraint terrain matching as described in claim 1, characterized in that, The method for determining the coordinates of the plane inflection point in step 3 is as follows: cluster the heading angles in the attitude angles at each time point in the activity trajectory, and find the time when the heading angle changes in the clustered heading angles, which corresponds to the plane inflection point in the activity trajectory.

5. The inertial navigation error correction method based on indoor walking constraint terrain matching as described in claim 1, characterized in that, The specific method for calculating the linear correlation and performing screening in step 41 is as follows: Define the x-coordinate of a point in a standard feature path segment as variable PX, and the y-coordinate as variable PY. The x-coordinates of points in the feature path segment to be calibrated constitute the variable QX, and the y-coordinates constitute the variable QY. Calculate the linear correlation coefficient ρ PXQX and ρ PYQY The formula calculation method is as follows: r PXQX = Cov(PX,QX) / ( σ PX *s QX ) r PYQY = Cov(PY,QY) / ( σ PY *s QY ) Cov(PX,QX) is the covariance between variables PX and QX, σ PX , σ PY , σ QX , σ QY Let PX, PY, QX, and QY be the standard deviations of the variables; When ρ PXQX and ρ PYQY If both values ​​are greater than the set threshold, then the two are determined to meet the matching conditions.

6. The inertial navigation error correction method based on indoor walking constraint terrain matching as described in claim 1, characterized in that, It also includes step 5. Performing three-dimensional correction on the activity trajectory, specifically: Step 51. Based on the acceleration and attitude angle information of all sampling points obtained in Step 2, extract the vertical acceleration component α. zn With respect to the vertical acceleration α zn Perform valley value detection; Step 52. The vertical acceleration α detected in step 51 zn Valley values ​​are subjected to subtractive clustering to obtain cluster centers and the number of clusters N; If N=1, it is determined to be a flat ground walking state, and no three-dimensional correction is performed; If N=2, then it is determined that there is an upstairs or downstairs state, and the subsequent steps are continued; Step 53. Calculate the vertical acceleration component α of adjacent sampling points. zn The difference is integrated twice to obtain the change in vertical displacement over time, i.e., the change in height Δh. The sum of the height changes Δh, h, is calculated. If h > 0, it is determined to be going upstairs; if h > 0, it is determined to be going downstairs. Step 54. For the vertical acceleration α zn The valley values ​​are subjected to k-means clustering, and the number of clusters N obtained in step 52 is input as the number of clusters for k-means clustering to obtain the vertical acceleration α with two cluster centers. zn Valley values ​​A1, A2, ..., A i A j ,……,A n The corresponding times are t1, t2, ..., t i ,t j ,……,t n n is the total number of sampling points; Step 55. For A1, A2, ..., A i A j ,……,A n The difference between two adjacent valley values ​​is calculated sequentially to determine the start and end times of the carrier going up and down stairs. The determination method is as follows: When A j -A i If the value is less than 0, then time j is determined to be the start time of going up or down the stairs; When A j -A i If the value is greater than 0, then time j is determined to be the end time of going up or down the stairs; When A j -A i If the value is 0, then it is determined that at time j, the person is walking on flat ground or going up or down stairs. The start and end times of two consecutive up and down stairs are labeled as m and n, respectively. Step 56. For each pair of adjacent start and end times of going up and down stairs, detect the vertical acceleration α during the time interval from m to n. zn The number of valleys is L, where L represents the number of steps a pedestrian goes up or down during the time interval from m to n. This can be calculated using L*h. stair Obtain the actual height H, h that a pedestrian rises or falls during the process of going up or down the stairs. stair The height of each step; Step 57. Based on the two-dimensional coordinates of the stairwell, and using the actual height H obtained in Step 56, correct the vertical coordinates of the carrier at the stairwell to construct the three-dimensional trajectory of the carrier.

7. The inertial navigation error correction method based on indoor walking constraint terrain matching as described in claim 6, characterized in that, Step 56 includes a correction to the actual height H, specifically: The number of floors M that the carrier climbs is determined based on the actual height, using the following method: when Let M=1; when Let M=2; when Let M=3; …… And so on; where h total For the building's floor height, Using M*h total Obtain the terrain height H' as the pedestrian goes up or down the stairs, and update the actual height H using the terrain height H'.

Images