A pedestrian autonomous positioning and navigation method assisted by three-axis ultrasound
Through a three-axis ultrasonic assisted six-axis attitude meter, the zero-speed interval is judged and the navigation attitude quaternary is integrated. The inertial navigation system is calibrated by ultrasonic ranging sensors, which solves the problem of inertial navigation error and insufficient accuracy in judgment of zero-speed intervals, and improves positioning Navigation accuracy and stability.
Patent Information
- Application Number
- CN202211386612.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-11-07
- Publication Date
- 2024-12-06
- Estimated Expiration
- 2042-11-07
AI Technical Summary
In the prior art, there is a problem of heading angle offset caused by inertial device accumulation error in personnel positioning and navigation, and the accuracy of zero-speed interval judgment is insufficient, resulting in low positioning and navigation accuracy.
A three-axis ultrasonic assisted six-axis attitude meter is used to determine the zero-speed interval, and the navigation attitude quaternion is fused through adaptive complementary filtering, and an inertial navigation system is calibrated using an ultrasonic ranging sensor to achieve precise positioning.
It improves the accuracy and stability of zero-speed interval judgment, reduces inertial navigation errors, and enhances the accuracy and stability of positioning navigation.
Smart Images

Figure CN115655269B_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the technical field of inertial navigation and positioning, and relates to a pedestrian autonomous positioning and navigation method assisted by three-axis ultrasonic waves. Background Art
[0002] The key issue in personnel positioning and navigation is to obtain accurate personnel position information and posture information in order to accurately issue navigation instructions and realize high-precision personnel positioning and navigation functions. At present, personnel positioning and navigation are divided into satellite positioning navigation and autonomous navigation. The former covers a wide area, but cannot be used in closed environments. The latter is a completely autonomous passive navigation, and there is no need to worry about whether the use environment is blocked, but the accuracy will decrease over time. This is because the cumulative error of the inertial system gyroscope device will increase over time, causing the heading angle to shift.
[0003] In order to solve the above problems, a zero-speed interval calibration method was proposed. The cumulative error of the inertial device is calibrated by using the short zero-speed interval of the foot when people are walking. This can suppress the heading deviation to a certain extent, but the heading deviation will still increase with time.
[0004] Furthermore, a constraint method for the zero-speed interval of two feet was proposed (CN112362057A), which uses the theoretical constraint conditions between the two feet during walking to constrain the navigation posture, speed and position. However, the calibration result is affected by the model accuracy. The more the constraint model accuracy conforms to the current person's two-foot constraint relationship, the higher the accuracy of the calibration result, and vice versa. As time goes by, the calibration result will still shift. This is a method that can suppress heading deviation, but the positioning and navigation accuracy is still affected by gyroscope deviation and model accuracy.
[0005] In the above methods, in addition to the inertial device offset causing errors in personnel positioning and navigation, there is also the problem of accuracy in judging the zero-speed interval. When the zero-speed interval is misjudged, the characteristic data of the non-zero-speed interval will be brought into the calibration, which will introduce errors in personnel positioning and navigation. The traditional zero-speed interval judgment method uses the method of whether the maximum modulus and variance of the accelerometer and gyroscope at rest are less than the minimum threshold at rest to comprehensively judge the zero-speed interval. However, as the running time increases, the gyroscope will produce zero bias, and the originally set minimum threshold of the maximum modulus at rest may be less than the maximum modulus of the gyroscope at rest after the offset, resulting in errors in the zero-speed interval judgment. However, there is also the possibility of misjudgment when using an accelerometer alone to judge the zero-speed interval, so a stable and accurate zero-speed interval judgment method that is not affected by gyroscope offset is needed. Summary of the invention
[0006] The present invention aims to solve the problems of the above traditional technologies. This paper proposes a pedestrian autonomous positioning and navigation method assisted by three-axis ultrasonic waves. On the one hand, this method uses three-axis ultrasonic ranging to assist the accelerometer in the six-axis attitude meter to determine the zero-speed interval, solving the problem of misjudgment of the zero-speed interval in the traditional method. On the other hand, the foot spacing measured by the ultrasonic ranging sensor in the zero-speed interval is used as the observation quantity to calibrate the pure inertial navigation solution, which causes the cumulative error of attitude, speed and position to increase, thereby realizing personnel positioning and navigation. The technical solution of the present invention is as follows:
[0007] A pedestrian autonomous positioning and navigation method assisted by three-axis ultrasound comprises the following steps:
[0008] Step 1: Collect triaxial ultrasonic and six-axis attitude meter data of the pedestrian's forefoot, and use triaxial ultrasonic to assist the six-axis attitude meter to determine the zero-speed interval of the pedestrian's forefoot;
[0009] Step 2: In the zero-speed range, the three-axis accelerometer and three-axis gyroscope data are used to update two sets of navigation attitude quaternions respectively, and the fused navigation attitude quaternion is obtained by using adaptive complementary filtering fusion;
[0010] Step 3: Use the three-axis ultrasonic data in the zero-speed range to correct the navigation position and fused navigation attitude quaternion, so as to achieve accurate positioning and navigation of pedestrians.
[0011] Furthermore, the step 1 collects the triaxial ultrasonic and six-axis attitude meter data of the forefoot of the pedestrian, and uses the triaxial ultrasonic to assist the six-axis attitude meter to determine the zero speed interval of the forefoot of the pedestrian, and the specific steps are as follows:
[0012] Step 1.1: Calculate and judge the zero-speed interval state identifier S and the distance measurement information of the three-axis ultrasonic module in the sliding window of length L: When S = 0, and (j=k-L+1, k-L+2,…, k) is less than or equal to the threshold ε ultr When , let S = 1, otherwise S = 0;
[0013] in, Ultrasound of the right foot The axis sends out the left foot (right foot) ultrasonic wave at time k The measured distance received by the axis, is the left foot ultrasound at time k Axonometric distance from the ground;
[0014] Step 1.2: Calculate the acceleration of the i (i = x, y, z) axis in the sliding window of length L at time k (j=k-L+1,k-L+2,…,k) and the initial time k 0 Acceleration The average slope between The calculation formula is:
[0015]
[0016] Where N is the data length between two adjacent zero rate intervals;
[0017] Step 1.4: Judgment: When S = 1, and the The absolute values of are all less than the thresholds of each axis. When , let S = 2, otherwise let S = 0.
[0018] Furthermore, in step 1, the specific axes of each coordinate system in the zero-speed interval of the pedestrian's forefoot are determined by using three-axis ultrasound to assist the six-axis attitude meter:
[0019] The axis of the left and right foot three-axis gyroscope, x gyro Axis right, y gyro Axis forward, z gyro Axis-wise, the three axes are orthogonal, forming a right-handed coordinate system x gyro y gyro z gyro ;
[0020] The axis of the left and right foot triaxial accelerometers, x acc Axis right, y acc Axis forward, z acc Axis-wise, the three axes are orthogonal, forming a right-handed coordinate system x acc y acc z acc , and x gyro y gyro z gyro The coordinate systems coincide;
[0021] The left and right foot three-axis ultrasonic module axial direction, Axis to the right (left) front, Axis to the right (left) rear, Axis downward, three axes are orthogonal, forming a right-hand coordinate system
[0022] Furthermore, in step 2, two sets of navigation attitude quaternions are updated respectively using the three-axis accelerometer and three-axis gyroscope data in the zero-speed range, and the fused navigation attitude quaternion is obtained by using adaptive complementary filtering fusion. The specific steps are as follows:
[0023] Step 2.1: Use the fourth-order Picard method to update the navigation attitude quaternion and obtain the prior navigation attitude quaternion of the left foot (right foot) at time k
[0024] Step 2.2: Exploitation Solve the coordinate transformation matrix from the n system (northeast celestial coordinate system) to the b system (right front upper coordinate system) Then the zero speed interval n is the theoretical value of acceleration a 0 Through the coordinate transformation matrix Perform coordinate transformation to obtain the theoretical value of acceleration of system b Finally, the b-axis measurement value of the accelerometer at time k is and Substitute the extended Kalman filter measurement update Get the first set of posterior navigation attitude quaternion of the left foot (right foot)
[0025] Step 2.3: Use the theoretical value of the angular velocity ω of the frame b in the zero-speed interval 0 Update the quaternion to obtain the theoretical navigation attitude quaternion of the left foot in the zero speed interval And substitute it as the measurement value into the measurement update of the general Kalman filter Get the second set of posterior navigation attitude quaternion of the left foot (right foot)
[0026] Step 2.4: According to the actual measurement value of the left (right) foot accelerometer and theoretical value Calculate the relationship between Weight w 1 , the formula is:
[0027]
[0028] Among them, μ L(R)1 is the weight w L(R)1 The adjustment factor of
[0029] Step 2.5: Weight w L(R)2 The calculation formula is:
[0030] w L(R)2 =1-w L(R)1
[0031] Step 2.6: Calculate the fused left foot (right foot) fusion posterior navigation attitude quaternion according to the weight The formula is:
[0032]
[0033] Furthermore, the extended Kalman filter measurement update of step 2.2 The specific equation is:
[0034]
[0035]
[0036]
[0037]
[0038]
[0039] in, represents the first set of measurement update matrix for the left foot (right foot), Represents the first set of Kalman gain coefficients for the left foot (right foot), represents the first set of prior estimated covariance of the left foot (right foot), represents the first set of measurement error matrix of the left foot (right foot), It indicates the theoretical acceleration measurement value of the left foot (right foot) b system. It indicates the actual acceleration measurement value of the left foot (right foot) b. Represents the first set of posterior estimated covariance matrix of the left foot (right foot).
[0040] Furthermore, the Kalman filter measurement update in step 2.3 The specific equation is:
[0041]
[0042]
[0043]
[0044]
[0045] in, represents the second set of measurement update matrix for the left foot (right foot), Represents the second set of Kalman gain coefficients for the left foot (right foot), represents the second set of prior estimated covariance of the left foot (right foot), represents the second set of measurement error matrix of the left foot (right foot), Represents the theoretical navigation posture quaternion of the left foot (right foot), Represents the covariance matrix of the second group of posterior estimates of the left foot (right foot).
[0046] Furthermore, the step 3 uses the three-axis ultrasonic data in the zero-speed range to correct the navigation position and the fused navigation attitude quaternion, so as to achieve accurate positioning and navigation of the pedestrian. The specific steps are as follows:
[0047] Step 3.1: Start with your left foot Establish an azimuth coordinate system for the origin whose axis coincides with the n-coordinate system, and determine the coordinate point of the right foot relative to the origin of the azimuth coordinate system at the initial moment During the movement between two adjacent zero-rate intervals, only state updates are performed, and no measurement updates are performed. Depend on Get the coordinate transformation matrix from the b coordinate system to the n coordinate system Then take the b-axis measurement value of the left (right) foot accelerometer at time k-1 Perform coordinate transformation to obtain the components of each axis of the n-axis of the left (right) foot accelerometer at time k-1 After subtracting the gravitational acceleration g, the acceleration of the x-axis, y-axis, and z-axis is integrated to obtain the movement speed of the left foot (right foot) in the x-axis, y-axis, and z-axis directions at time k. and Double integration obtains the moving distance of the left foot (right foot) in the x-axis, y-axis and z-axis at time k and Get the coordinates of the left foot after moving Coordinates of the right foot The distance between the feet can be calculated using the coordinate points of the left and right feet. Specifically:
[0048]
[0049]
[0050]
[0051]
[0052]
[0053]
[0054]
[0055]
[0056]
[0057]
[0058]
[0059]
[0060]
[0061]
[0062] in, represents the acceleration of the left foot (right foot) in the n-system coordinate system at time k-1, They represent the displacement of the left foot (right foot) in the x, y, and z axes in the n coordinate system at time k-1, respectively. They represent the x, y, and z axis coordinates of the left foot (right foot) at time k in the n coordinate system. It represents the distance between the left foot (right foot) and the right foot (left foot) calculated by the inertial navigation system at time k.
[0063] Step 3.2: Fusion of the posterior navigation attitude quaternion of the left foot (right foot) obtained in step 2 The state space vector X is constructed by using the velocity and position coordinates of the left and right feet in step 3.1 k , specifically:
[0064]
[0065] Step 3.3: For the state space vector X k Update the status, specifically:
[0066]
[0067]
[0068]
[0069] F k =I 20×20
[0070]
[0071]
[0072] Among them, U k-1 represents the variable vector, B k-1 and G k-1 Represents the relationship matrix between the variable vector and the state vector, F k represents the state transition matrix at time k, O m×n represents an m×n zero matrix, represents the prior estimated covariance at time k, represents the posterior estimated covariance matrix at time k-1, Q k Represents the state update error matrix at time k.
[0073] Step 3.4: For the state space vector X k Perform measurement updates, specifically:
[0074]
[0075]
[0076]
[0077]
[0078] Among them, H k represents the measurement update matrix at time k, K k represents the Kalman gain coefficient at time k, R k represents the measurement error matrix at time k, Indicates that the ultrasonic sensor The axis sends ultrasonic waves to The axis receives the calculated inter-foot distance, Represents the posterior estimated covariance matrix at time k.
[0079] Step 3.4: Get the navigation attitude quaternion and position coordinates of the left and right feet after calibration:
[0080]
[0081]
[0082]
[0083]
[0084]
[0085]
[0086] Step 3.5: Repeat the state update of step 1, step 2.1 and step 3.3 until S=2, and the attitude indicator is in the zero speed range. At this time, repeat step 1, step 2, step 3.1, step 3.2, step 3.3 and step 3.4 until S≠2, and the attitude indicator is in the non-zero speed range.
[0087] The advantages and beneficial effects of the present invention are as follows:
[0088] (1) In step 1, the present invention proposes a three-axis ultrasonic assisted attitude meter sliding mean slope method to judge the zero speed interval, and uses the advantage of the three-axis ultrasonic ranging sensor and the three-axis accelerometer having no time accumulation error to comprehensively judge whether the data in the zero speed interval meets the zero speed interval characteristics, and performs the next step of calculation for the data that meets the characteristic requirements. Compared with the traditional method of using the three-axis gyroscope and the three-axis accelerometer for comprehensive judgment, the advantage of this method is that the preset threshold condition will not fail as the sensor operation time increases, thereby achieving a more accurate and stable locking of the zero speed interval.
[0089] (2) In step 2, when it is determined that the vehicle is in the zero-speed range, the attitude quaternion updated by the gyroscope and the attitude quaternion updated by the accelerometer are integrated using an adaptive complementary filtering method. The high-frequency information of the gyroscope can be used to compensate for the high-frequency error of the accelerometer, and the low-frequency information of the accelerometer can be used to compensate for the low-frequency error of the gyroscope, thereby obtaining an attitude quaternion with higher accuracy.
[0090] (3) In step 3, the extended Kalman filter method is used to take the inertial calculated foot distance as the theoretical measurement value and the ultrasonic ranging sensor measured foot distance as the observed value to perform measurement updates and calibrate the navigation attitude, velocity and position of the state space vector. Compared with the traditional method of using constraint models to provide observation values, the possibility of model errors introducing errors into the calibration results can be avoided. In addition, the ultrasonic ranging data has no time accumulation error and is a reliable information source over time, which makes the personnel positioning navigation of this method have higher accuracy and higher stability. BRIEF DESCRIPTION OF THE DRAWINGS
[0091] Figure 1 The present invention provides a preferred embodiment of the scheme flow chart of the present invention;
[0092] Figure 2 It is a schematic diagram of autonomous positioning and navigation of pedestrians assisted by three-axis ultrasound in an embodiment of the present invention. DETAILED DESCRIPTION
[0093] The following will describe the technical solutions in the embodiments of the present invention in detail in conjunction with the accompanying drawings in the embodiments of the present invention. The described embodiments are only part of the embodiments of the present invention.
[0094] The technical solution of the present invention to solve the above technical problems is:
[0095] like Figure 1 The figure is a flow chart of a pedestrian autonomous positioning and navigation algorithm assisted by three-axis ultrasound. The pedestrian autonomous positioning and navigation algorithm assisted by three-axis ultrasound specifically includes the following steps:
[0096] Step 1: Collect triaxial ultrasonic and six-axis attitude meter data of the pedestrian's forefoot, and use triaxial ultrasonic to assist the six-axis attitude meter to determine the zero-speed interval of the pedestrian's forefoot;
[0097] Step 2: In the zero-speed range, the three-axis accelerometer and three-axis gyroscope data are used to update two sets of navigation attitude quaternions respectively, and the fused navigation attitude quaternion is obtained by using adaptive complementary filtering fusion;
[0098] Step 3: Use the three-axis ultrasonic data in the zero-speed range to correct the navigation position and fused navigation attitude quaternion, so as to achieve accurate positioning and navigation of pedestrians.
[0099] Furthermore, the step 1 collects the triaxial ultrasonic and six-axis attitude meter data of the forefoot of the pedestrian, and uses the triaxial ultrasonic to assist the six-axis attitude meter to determine the zero speed interval of the forefoot of the pedestrian, and the specific steps are as follows:
[0100] Step 1.1: Calculate and judge the zero-speed interval state identifier S and the distance measurement information of the three-axis ultrasonic module in the sliding window of length L: When S = 0, and (j=k-L+1, k-L+2,…, k) is less than or equal to the threshold ε ultr When , let S = 1, otherwise S = 0;
[0101] in, Ultrasound of the right foot The axis sends out the left foot (right foot) ultrasonic wave at time k The measured distance received by the axis, is the left foot ultrasound at time k Axonometric distance from the ground;
[0102] Step 1.2: Calculate the acceleration of the i (i = x, y, z) axis in the sliding window of length L at time k (j=k-L+1,k-L+2,…,k) and the initial time k 0 Acceleration The average slope between The calculation formula is:
[0103]
[0104] Where N is the data length between two adjacent zero rate intervals;
[0105] Step 1.4: Judgment: When S = 1, and the The absolute values of are all less than the thresholds of each axis. When , let S = 2, otherwise let S = 0.
[0106] Furthermore, in step 1, the specific axes of each coordinate system in the zero-speed interval of the pedestrian's forefoot are determined by using three-axis ultrasound to assist the six-axis attitude meter:
[0107] The axis of the left and right foot three-axis gyroscope, x gyro Axis right, y gyro Axis forward, z gyro Axis-wise, the three axes are orthogonal, forming a right-handed coordinate system x gyro y gyro z gyro ;
[0108] The axis of the left and right foot triaxial accelerometers, x acc Axis right, y acc Axis forward, z acc Axis-wise, the three axes are orthogonal, forming a right-handed coordinate system x acc y acc z acc , and x gyro y gyro z gyro The coordinate systems coincide;
[0109] The left and right foot three-axis ultrasonic module axial direction, Axis to the right (left) front, Axis to the right (left) rear, Axis downward, three axes are orthogonal, forming a right-hand coordinate system
[0110] Furthermore, in step 2, two sets of navigation attitude quaternions are updated respectively using the three-axis accelerometer and three-axis gyroscope data in the zero-speed range, and the fused navigation attitude quaternion is obtained by using adaptive complementary filtering fusion. The specific steps are as follows:
[0111] Step 2.1: Use the fourth-order Picard method to update the navigation attitude quaternion and obtain the prior navigation attitude quaternion of the left foot (right foot) at time k
[0112] Step 2.2: Exploitation Solve the coordinate transformation matrix from the n system (northeast celestial coordinate system) to the b system (right front upper coordinate system) Then the zero speed interval n is the theoretical value of acceleration a 0 Through the coordinate transformation matrix Perform coordinate transformation to obtain the theoretical value of acceleration of system b Finally, the b-axis measurement value of the accelerometer at time k is and Substitute the extended Kalman filter measurement update Get the first set of posterior navigation attitude quaternion of the left foot (right foot)
[0113] Step 2.3: Use the theoretical value of the angular velocity ω of the frame b in the zero-speed interval 0 Update the quaternion to obtain the theoretical navigation attitude quaternion of the left foot in the zero speed interval And substitute it as the measurement value into the measurement update of the general Kalman filter Get the second set of posterior navigation attitude quaternion of the left foot (right foot)
[0114] Step 2.4: According to the actual measurement value of the left (right) foot accelerometer and theoretical value Calculate the relationship between Weight w 1 , the formula is:
[0115]
[0116] Among them, μ L(R)1 is the weight w L(R)1 The adjustment factor of
[0117] Step 2.5: Weight w L(R)2 The calculation formula is:
[0118] w L(R)2 =1-w L(R)1
[0119] Step 2.6: Calculate the fused left foot (right foot) fusion posterior navigation attitude quaternion according to the weight The formula is:
[0120]
[0121] Furthermore, the extended Kalman filter measurement update of step 2.2 The specific equation is:
[0122]
[0123]
[0124]
[0125]
[0126]
[0127] in, represents the first set of measurement update matrix for the left foot (right foot), Represents the first set of Kalman gain coefficients for the left foot (right foot), represents the first set of prior estimated covariance of the left foot (right foot), represents the first set of measurement error matrix of the left foot (right foot), It indicates the theoretical acceleration measurement value of the left foot (right foot) b system. It indicates the actual acceleration measurement value of the left foot (right foot) b. Represents the first set of posterior estimated covariance matrix of the left foot (right foot).
[0128] Furthermore, the Kalman filter measurement update in step 2.3 The specific equation is:
[0129]
[0130]
[0131]
[0132]
[0133] in, represents the second set of measurement update matrix for the left foot (right foot), Represents the second set of Kalman gain coefficients for the left foot (right foot), represents the second set of prior estimated covariance of the left foot (right foot), represents the second set of measurement error matrix of the left foot (right foot), Represents the theoretical navigation posture quaternion of the left foot (right foot), Represents the covariance matrix of the second group of posterior estimates of the left foot (right foot).
[0134] Furthermore, the step 3 uses the three-axis ultrasonic data in the zero-speed range to correct the navigation position and the fused navigation attitude quaternion, so as to achieve accurate positioning and navigation of the pedestrian. The specific steps are as follows:
[0135] Step 3.1: Start with your left foot Establish an azimuth coordinate system for the origin whose axis coincides with the n-coordinate system, and determine the coordinate point of the right foot relative to the origin of the azimuth coordinate system at the initial moment During the movement between two adjacent zero-rate intervals, only state updates are performed, and no measurement updates are performed. Depend on Get the coordinate transformation matrix from the b coordinate system to the n coordinate system Then take the b-axis measurement value of the left (right) foot accelerometer at time k-1 Perform coordinate transformation to obtain the components of each axis of the n-axis of the left (right) foot accelerometer at time k-1 After subtracting the gravitational acceleration g, the acceleration of the x-axis, y-axis, and z-axis is integrated to obtain the movement speed of the left foot (right foot) in the x-axis, y-axis, and z-axis directions at time k. and Double integration obtains the moving distance of the left foot (right foot) in the x-axis, y-axis and z-axis at time k and Get the coordinates of the left foot after moving Coordinates of the right foot The distance between the feet can be calculated using the coordinate points of the left and right feet. Specifically:
[0136]
[0137]
[0138]
[0139]
[0140]
[0141]
[0142]
[0143]
[0144]
[0145]
[0146]
[0147]
[0148]
[0149]
[0150] in, represents the acceleration of the left foot (right foot) in the n-system coordinate system at time k-1, They represent the displacement of the left foot (right foot) in the x, y, and z axes in the n coordinate system at time k-1, respectively. They represent the x, y, and z axis coordinates of the left foot (right foot) at time k in the n coordinate system. It represents the distance between the left foot (right foot) and the right foot (left foot) calculated by the inertial navigation system at time k.
[0151] Step 3.2: Fusion of the posterior navigation attitude quaternion of the left foot (right foot) obtained in step 2 The state space vector X is constructed by using the velocity and position coordinates of the left and right feet in step 3.1 k , specifically:
[0152]
[0153] Step 3.3: For the state space vector X k Update the status, specifically:
[0154]
[0155]
[0156]
[0157] F k =I 20×20
[0158]
[0159]
[0160] Among them, U k-1 represents the variable vector, B k-1 and G k-1 Represents the relationship matrix between the variable vector and the state vector, F k represents the state transition matrix at time k, O m×n represents an m×n zero matrix, represents the prior estimated covariance at time k, represents the posterior estimated covariance matrix at time k-1, Q k Represents the state update error matrix at time k.
[0161] Step 3.4: For the state space vector X k Perform measurement updates, specifically:
[0162]
[0163]
[0164]
[0165]
[0166] Among them, H k represents the measurement update matrix at time k, K k represents the Kalman gain coefficient at time k, Rk represents the measurement error matrix at time k, Indicates that the ultrasonic sensor The axis sends ultrasonic waves to The axis receives the calculated inter-foot distance, Represents the posterior estimated covariance matrix at time k.
[0167] Step 3.4: Get the navigation attitude quaternion and position coordinates of the left and right feet after calibration:
[0168]
[0169]
[0170]
[0171]
[0172]
[0173]
[0174] Step 3.5: Repeat the state update of step 1, step 2.1 and step 3.3 until S=2, and the attitude indicator is in the zero speed range. At this time, repeat step 1, step 2, step 3.1, step 3.2, step 3.3 and step 3.4 until S≠2, and the attitude indicator is in the non-zero speed range.
[0175] like Figure 2 As shown in FIG. 1 , a schematic diagram of pedestrian autonomous positioning and navigation assisted by three-axis ultrasound in an embodiment of the present invention is shown. The present invention uses the left foot starting point as the origin to establish an azimuth coordinate system whose axial direction coincides with the geographic coordinate system. The origin is The initial coordinates of the right foot are In the process of walking, the fourth-order Picard method is used to update the state of the quaternion to obtain the prior navigation attitude quaternion. And the subsequent steps of this method are not executed at this time, so only When the zero speed interval is determined under the assistance of ultrasound, there is a zero speed interval state indicator S = 2, then the theoretical acceleration measurement value is used. and the actual acceleration measurement Perform extended Kalman filter measurement to update the first set of navigation attitude quaternions of the left foot (right foot) Using theoretical navigation attitude quaternion As the measured value, Kalman filter measurement update, correction Get the second set of navigation posture quaternions of the left foot (right foot) Then, adaptive complementary filtering is used to obtain the fused navigation attitude quaternion The distance between feet calculated by inertial navigation As a theoretical measurement value, the distance between feet obtained by the ultrasonic distance sensor The extended Kalman filter is used as the actual measurement value to calibrate the navigation attitude, velocity and position of the personnel, and the optimal navigation attitude quaternion, velocity and position estimation are obtained.
[0176] The systems, devices, modules or units described in the above embodiments may be implemented by computer chips or entities, or by products with certain functions. A typical implementation device is a computer. Specifically, the computer may be, for example, a personal computer, a laptop computer, a cellular phone, a camera phone, a smart phone, a personal digital assistant, a media player, a navigation device, an email device, a game console, a tablet computer, a wearable device, or a combination of any of these devices.
[0177] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.
[0178] The above embodiments should be understood to be only used to illustrate the present invention and not to limit the protection scope of the present invention. After reading the contents of the present invention, technicians can make various changes 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 pedestrian autonomous positioning and navigation method assisted by three-axis ultrasound, characterized in that: The following steps are involved: Step 1: Collect triaxial ultrasonic and six-axis attitude meter data of the pedestrian's forefoot, and use triaxial ultrasonic to assist the six-axis attitude meter to determine the zero-speed interval of the pedestrian's forefoot; Step 2: In the zero-speed range, the three-axis accelerometer and three-axis gyroscope data are used to update two sets of navigation attitude quaternions respectively, and the fused navigation attitude quaternion is obtained by using adaptive complementary filtering fusion; Step 3: In the zero-speed range, the three-axis ultrasonic data is used to correct the navigation position and the fused navigation attitude quaternion, thereby achieving accurate positioning and navigation of the pedestrian; The step 1 collects the triaxial ultrasonic and six-axis attitude meter data of the pedestrian's forefoot, and uses the triaxial ultrasonic to assist the six-axis attitude meter to determine the zero speed interval of the pedestrian's forefoot. The specific steps are: Step 1.1: Calculate and judge the zero-speed interval state identifier S and the distance measurement information of the three-axis ultrasonic module in the sliding window of length L: When S = 0, and Less than or equal to the threshold ε ultr When j=k-L+1, k-L+2,…,k, let S=1, otherwise S=0; in, Ultrasound of the right foot The axis emits the left foot ultrasonic wave at time k The measured distance received by the axis, is the left foot ultrasound at time k Axonometric distance from the ground; Step 1.2: Calculate the acceleration of the i-axis in the sliding window with a length of L at time k With the initial acceleration k0 The average slope between The calculation formula is: Where N is the data length between two adjacent zero rate intervals; Step 1.4: Judgment: When S = 1, and the The absolute values of are all less than the thresholds of each axis. When , let S = 2, otherwise S = 0; Step 1: Use the three-axis ultrasonic wave to assist the six-axis attitude meter to determine the specific axes of each coordinate system in the zero-speed interval of the pedestrian's forefoot: The axis of the left and right foot three-axis gyroscope, x gyro Axis right, y gyro Axis forward, z gyro Axis-wise, the three axes are orthogonal, forming a right-handed coordinate system x gyro y gyro z gyro ; The axis of the left and right foot triaxial accelerometers, x acc Axis right, y acc Axis forward, z acc Axis-wise, the three axes are orthogonal, forming a right-handed coordinate system x acc y acc z acc , and x gyro y gyro z gyro The coordinate systems coincide; The left and right foot three-axis ultrasonic module axial direction, Axis to the right front, Axis right rear, Axis downward, three axes are orthogonal, forming a right-hand coordinate system The step 2 uses the three-axis accelerometer and three-axis gyroscope data to update two sets of navigation attitude quaternions in the zero speed range, and uses adaptive complementary filtering to fuse to obtain a fused navigation attitude quaternion. The specific steps are: Step 2.1: Use the fourth-order Picard method to update the navigation attitude quaternion and obtain the prior navigation attitude quaternion of the left foot at time k Step 2.2: Exploitation Solve the coordinate transformation matrix from the n system, i.e. the northeast celestial coordinate system, to the b system, i.e. the right front upper coordinate system Then the theoretical value of acceleration a0 in the zero-speed interval n is converted into the coordinate transformation matrix Perform coordinate transformation to obtain the theoretical value of acceleration of system b Finally, the b-axis measurement value of the accelerometer at time k is and Substitute the extended Kalman filter measurement update Get the first set of posterior navigation attitude quaternion of the left foot Step 2.3: Update the quaternion using the theoretical value of the angular velocity ω0 of the zero-speed interval b system to obtain the theoretical navigation attitude quaternion of the left foot in the zero-speed interval And substitute it as the measurement value into the measurement update of the general Kalman filter Get the second set of posterior navigation attitude quaternion of the left foot Step 2.4: According to the actual measurement value of the left foot accelerometer and theoretical value Calculate the relationship between Weight w1, the formula is: Among them, μ L(R)1 is the weight w L(R)1 The adjustment factor of Step 2.5: Weight w L(R)2 The calculation formula is: In L(R)2 =1-in L(R)1 Step 2.6: Calculate the fused left foot fusion posterior navigation attitude quaternion according to the weight The formula is: The extended Kalman filter measurement update in step 2.2 The specific equation is: Among them, H kL(R)1 represents the first set of measurement update matrix for the left foot, represents the first set of Kalman gain coefficients for the left foot, represents the first set of prior estimated covariances of the left foot, represents the first set of measurement error matrix for the left foot, It represents the theoretical acceleration measurement value of the left foot b system, It indicates the actual acceleration measurement value of the left foot b system. represents the covariance matrix of the first group of posterior estimates of the left foot; The Kalman filter measurement update in step 2.3 The specific equation is: in, represents the second set of measurement update matrix for the left foot, represents the second set of Kalman gain coefficients for the left foot, represents the second set of prior estimated covariances of the left foot, represents the second set of measurement error matrix for the left foot, Represents the theoretical navigation posture quaternion of the left foot, represents the covariance matrix of the second group of posterior estimates of the left foot; The step 3 uses the three-axis ultrasonic data to correct the navigation position and the fused navigation attitude quaternion in the zero-speed range, so as to achieve accurate positioning and navigation of the pedestrian. The specific steps are as follows: Step 3.1: Start with your left foot Establish an azimuth coordinate system for the origin whose axis coincides with the n-coordinate system, and determine the coordinate point of the right foot relative to the origin of the azimuth coordinate system at the initial moment During the movement between two adjacent zero-rate intervals, only state updates are performed, and no measurement updates are performed. Depend on Get the coordinate transformation matrix from the b coordinate system to the n coordinate system Then take the b-axis measurement value of the left foot accelerometer at time k-1 as Perform coordinate transformation to obtain the components of each axis of the n-axis of the left foot accelerometer at time k-1 After subtracting the gravitational acceleration g, the acceleration of the x-axis, y-axis, and z-axis is integrated to obtain the movement speed of the left foot in the x-axis, y-axis, and z-axis directions at time k. and Double integration obtains the moving distance of the left foot in the x-axis, y-axis and z-axis at time k and Get the coordinates of the left foot after moving Coordinates of the right foot The distance between the feet can be calculated using the coordinate points of the left and right feet. Specifically: in, represents the acceleration of the left foot in the n-coordinate system at time k-1, They represent the x, y, and z axial displacements of the left foot in the n coordinate system at time k-1, They represent the x, y, and z axis coordinates of the left foot in the n coordinate system at time k, represents the distance between the left and right feet calculated by the inertial navigation system at time k; Step 3.2: Fuse the posterior navigation attitude quaternion of the left foot obtained in step 2 The state space vector X is constructed by using the velocity and position coordinates of the left and right feet in step 3.1 k , specifically: Step 3.3: For the state space vector X k Update the status, specifically: F k =I 20×20 Among them, U k-1 represents the variable vector, B k-1 and G k-1 Represents the relationship matrix between the variable vector and the state vector, F k represents the state transition matrix at time k, O m×n represents an m×n zero matrix, represents the prior estimated covariance at time k, represents the posterior estimated covariance matrix at time k-1, Q k represents the state update error matrix at time k; Step 3.4: For the state space vector X k Perform measurement updates, specifically: Among them, H k represents the measurement update matrix at time k, K k represents the Kalman gain coefficient at time k, R k represents the measurement error matrix at time k, Indicates that the ultrasonic sensor The axis sends ultrasonic waves to The axis receives the calculated inter-foot distance, Represents the posterior estimated covariance matrix at time k; Step 3.4: Get the navigation attitude quaternion and position coordinates of the left and right feet after calibration: Step 3.5: Repeat the status updates of step 1, step 2.1 and step 3.3 until S=2, and the attitude indicator is in the zero speed range; at this time, repeat step 1, step 2, step 3.1, step 3.2, step 3.3 and step 3.4 until S≠2, and the attitude indicator is in the non-zero speed range.
Citation Information
Patent Citations
Inertial pedestrian navigation algorithm based on zero-speed correction and attitude self-observation
CN112362057A
Foot binding type pedestrian three-dimensional indoor positioning method assisted by distance measurement information
CN118149823A
