A vision-assisted pedestrian navigation method constrained by gait features

Abstract

The present invention discloses a visually assisted gait feature constrained pedestrian navigation method, which comprises the following steps: firstly, using the data collected by the inertial measurement unit to analyze the gait characteristics of the pedestrian and calculate the zero-speed interval; then, calibrating the inertial measurement unit and the camera to obtain corresponding internal and external parameters, solving the position and attitude data of the six-degree-of-freedom of the visual inertial odometer, and obtaining the relative posture information of the pedestrian's movement; then adopting the strapdown inertial navigation calculation method to perform initial alignment and inertial navigation solution to obtain the pedestrian's instantaneous attitude, speed and position information; finally, establishing a Kalman filter model, using the speed difference and angular velocity difference in the zero-speed interval, the attitude error and position error when the visual signal is valid as observation quantities for measurement and update; using the current corrected state quantity as the initial quantity for the next inertial navigation solution, and repeating the navigation correction process. The present invention can constrain the divergence of navigation errors in an environment where GNSS signals fail, and improve the accuracy of pedestrian navigation positioning.

Description

Technical Field

[0001] The present invention relates to the technical field of pedestrian positioning and combined navigation, and in particular to a vision-assisted gait feature-constrained pedestrian navigation method. Background Art

[0002] With the rapid development of urban modernization and the increasing number of large buildings, the demand for indoor positioning services is becoming increasingly significant. These services are needed for the management of people and belongings, firefighter navigation at fire scenes, intelligent transportation, airport personnel guidance, elderly health assistance systems, and virtual reality gaming. Therefore, providing users with accurate, reliable, and real-time indoor positioning services has significant scientific significance and social application value.

[0003] Combined inertial navigation / satellite navigation and positioning technology has been widely used in numerous fields, including public services and daily transportation. However, in environments such as indoors, underground, and in jungles, satellite signals can be blocked, lost, or susceptible to interference. Positioning data derived solely from inertial navigation offers good short-term accuracy, but suffers from cumulative errors that can spread over time, leading to poor or even incomplete positioning and poor long-term performance. Visual odometry uses a camera to collect image data and estimate pose based on image features, avoiding cumulative errors. However, it has high environmental requirements, and when the visual sensor data sampling rate is low compared to pedestrian motion speed, the number of feature points decreases, leading to reduced accuracy. To meet the demand for indoor positioning, recent advancements in technologies such as Wi-Fi, ultra-wideband (UWB), Bluetooth, and RFID have filled this gap. However, these indoor positioning technologies require the pre-deployment of base stations and are costly.

[0004] To address indoor navigation and positioning issues such as satellite signal loss, researchers have conducted extensive work to improve the performance of pedestrian navigation systems. The most representative approach is zero-speed correction, which corrects inertial navigation results by detecting zero-speed periodicity. However, zero-speed correction technology can only mitigate divergent speeds to a certain extent and lacks the ability to independently perform long-term, large-scale calculations. Nistér et al. designed a real-time visual odometry system and proposed two implementation approaches and processes for visual odometry: monocular vision and stereo vision. This laid a new foundation for subsequent research in visual odometry. However, real-time visual odometry systems suffer from large sample sizes, large computational complexity, and poor robustness. Summary of the Invention

[0005] The object of the present invention is to provide a pedestrian indoor navigation method that can suppress the divergence of integral accumulation error over time, reduce positioning error, and improve positioning accuracy.

[0006] The technical solution to achieve the purpose of the present invention is: a vision-assisted gait feature-constrained pedestrian navigation method, comprising the following steps:

[0007] Step 1: Analyze the pedestrian's gait characteristics based on the data collected by the inertial measurement unit fixed to the foot, and use the three-condition method, sliding filtering technology, and median filtering technology to obtain the zero-speed interval;

[0008] Step 2: Calibrate the connected inertial measurement unit and camera to obtain the corresponding intrinsic and extrinsic parameters;

[0009] Step 3: Run the VINS-Mono / VINS-Fusion algorithm to calculate the six-degree-of-freedom position and attitude data of the visual inertial odometry and obtain the relative position and attitude information of the pedestrian.

[0010] Step 4: Using the strapdown inertial navigation calculation method, the foot inertial measurement unit data is used for initial alignment and inertial navigation solution to obtain the pedestrian's instantaneous posture, velocity and position information;

[0011] Step 5: Establish a Kalman filter model, using the speed difference and angular velocity difference in the zero-speed range, and the attitude error and position error when the visual signal is valid as observation quantities, triggering the measurement update of the Kalman filter to achieve optimal estimation of the navigation state;

[0012] Step 6: Use the current corrected state quantity as the initial quantity to continue the inertial navigation solution and repeat the navigation correction process.

[0013] Compared with the existing technology, the present invention has the following significant advantages: (1) the high accuracy of the inertial measurement unit in the short term is utilized to drive the process model, thereby improving the positioning and navigation accuracy; (2) the zero-speed characteristics of the pedestrian gait are used as constraints to construct the observation quantity, and the accumulated error of the inertial navigation is corrected without adding hardware, thereby ensuring that higher navigation accuracy is obtained in a shorter period of time; (3) the posture information estimated by the visual inertial odometry is used as a constraint to construct the observation quantity, and the accumulated error of the inertial measurement unit is further corrected, thereby ensuring that higher navigation accuracy is obtained over a longer period of time; (4) the dual auxiliary technology of visual inertial odometry and zero-speed correction is used to improve the accuracy of indoor pedestrian navigation positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 Schematic diagram of the flow of the visually assisted gait feature constrained pedestrian navigation method of the present invention.

[0015] Figure 2 Graphs showing the characteristics of linear and angular motion of the foot in an embodiment of the present invention.

[0016] Figure 3 4 is a flowchart of the three-condition method for gait detection in an embodiment of the present invention. DETAILED DESCRIPTION

[0017] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0018] Combine Figure 1 The present invention provides a visually assisted gait feature constrained pedestrian navigation method, comprising the following steps:

[0019] Step 1: Analyze the pedestrian's gait characteristics based on the data collected by the inertial measurement unit fixed to the foot, and use the three-condition method, sliding filtering technology, and median filtering technology to obtain the zero-speed interval;

[0020] Step 2: Calibrate the connected inertial measurement unit and camera to obtain the corresponding intrinsic and extrinsic parameters;

[0021] Step 3: Run the VINS-Mono / VINS-Fusion algorithm to calculate the six-degree-of-freedom position and attitude data of the visual inertial odometry and obtain the relative position and attitude information of the pedestrian.

[0022] Step 4: Using the strapdown inertial navigation calculation method, the foot inertial measurement unit data is used for initial alignment and inertial navigation solution to obtain the pedestrian's instantaneous posture, velocity and position information;

[0023] Step 5: Establish a Kalman filter model, using the speed difference and angular velocity difference in the zero-speed range, and the attitude error and position error when the visual signal is valid as observation quantities, triggering the measurement update of the Kalman filter to achieve optimal estimation of the navigation state;

[0024] Step 6: Use the current corrected state quantity as the initial quantity to continue the inertial navigation solution and repeat the navigation correction process.

[0025] As a specific example, in step 1, the pedestrian's gait characteristics are analyzed based on the data collected by the inertial measurement unit fixed to the foot, and the zero-speed interval is obtained using the three-condition method, sliding filtering technology, and median filtering technology, as follows:

[0026] Step 1.1, obtain the acceleration and angular velocity data sampled by the sensor;

[0027] Step 1.2: Calculate the three judgment conditions of the three-condition method based on the acceleration and angular velocity data. When the judgment results of the three conditions are all 1, the pedestrian is in a stationary state;

[0028] Step 1.3: Use the median filter algorithm to select a sampling center and a certain window space, and take the median of the data within the window as the data for this time;

[0029] Step 1.4: Remove noise points and finally obtain gait detection data.

[0030] As a specific example, the acceleration and angular velocity data sampled by the sensor in step 1.1 are obtained as follows:

[0031] Fix the inertial sensor to the pedestrian's foot surface, sample the foot movement characteristics during walking, and extract the acceleration and angular velocity data obtained by the sensor sampling;

[0032] As a specific example, in step 1.2, the three judgment conditions of the three-condition method are calculated based on the acceleration and angular velocity data. When the judgment results of the three conditions are all "1", the pedestrian is in a stationary state, as follows:

[0033] The inertial measurement unit consists of a three-axis gyroscope and a three-axis accelerometer, which directly outputs the three-axis angular velocity and specific force measurement values ​​in the inertial coordinate system. The inertial sensor is fixed to the pedestrian's foot to sample the foot movement characteristics during walking. By extracting the acceleration and angular velocity data sampled by the sensor, it can be found that the acceleration and angular velocity output during pedestrian movement are periodic and have zero transient characteristics, such as Figure 2 As shown, the zero transient is outlined by a black rectangle.

[0034] The three-condition method completes the identification of the zero-speed interval of gait based on the physical information related to acceleration and angular velocity measured by the inertial sensor. The identified static gait is used as a trigger condition for zero-speed correction to correct the long-term cumulative error caused by IMU positioning, thereby improving positioning accuracy. Adding local method difference sliding filtering technology and median filtering technology to the three-condition method can effectively suppress system noise, eliminate wild points, and make the zero-speed detection results more reliable. The specific process of obtaining the zero-speed interval is as follows: Figure 3 shown.

[0035] Based on the acceleration and angular velocity data, the three judgment conditions of the three-condition method are calculated. When the judgment results of the three conditions are all "1", the pedestrian is in a stationary state, as follows:

[0036] The three-condition method uses three conditions to judge the human body's gait, with state "0" indicating movement and state "1" indicating stillness.

[0037] Judgment condition 1: Acceleration vector and threshold method: When the composite amplitude output by the accelerometer is between the given upper and lower thresholds, the pedestrian is judged to be stationary;

[0038] Defines the composite amplitude of the accelerometer output |a k As shown in formula (1):

[0039]

[0040] Among them, |a k| represents the modulus of acceleration at time k, and They represent the acceleration of the carrier along the x, y, and z axes collected by the k-th accelerometer respectively;

[0041] The judgment formula of definition condition 1 is shown in formula (2):

[0042]

[0043] Among them, C1 is the judgment result of condition 1, C1 = 1 means that the pedestrian is in a stationary state, and C1 = 0 means that the pedestrian is in a moving state; amin =9m / s 2 and th amax =11m / s 2 , represent the acceleration vector and threshold respectively;

[0044] Judgment condition 2: Angular velocity vector and threshold method: When the composite amplitude output by the gyroscope is between the given upper and lower thresholds, the pedestrian is judged to be stationary;

[0045] Defines the composite amplitude of the gyroscope output |ω k As shown in formula (3):

[0046]

[0047] Among them, |ω k | is the modulus of the angular velocity at time k; and They represent the x, y and z angular velocities of the carrier collected by the k-th gyroscope respectively;

[0048] The judgment formula of definition condition 2 is shown in formula (4):

[0049]

[0050] Among them, C2 is the judgment result of condition 2, C2 = 1 means the pedestrian is in a stationary state, C2 = 0 means the pedestrian is in a stationary state; ωmax =2deg / s, representing the angular velocity vector and threshold;

[0051] Judgment condition 3: Acceleration local variance method: When the local variance of the accelerometer output is lower than a given threshold, the pedestrian is judged to be stationary;

[0052] The local variance sliding filtering technique is used to determine the stability of the 2s+1 sampling points in this interval by calculating the local variance within the window. When the local variance within the window is less than the threshold, close to 0, the center point of the window is the point where the gait is still, and the corresponding value of the rectangular wave is 1. On the contrary, when the local variance within the window is greater than the threshold, the center point of the window cannot be considered as an absolute still point, so it is removed and the corresponding rectangular wave value is 0. The center point slides through each sampling point in sequence with a step size of 1. The first s sampling points and the last s sampling points cannot be used as center points, but remain stationary before and after sampling and are set to stationary state 1. The final stable stage after filtering is taken as the absolute gait still moment.

[0053] The local variance of the accelerometer output is defined as shown in formula (5):

[0054]

[0055] in, is the local variance of the accelerometer composite amplitude in this window; is the accelerometer composite amplitude at the qth sampling point; q is the subscript of each sampling point within this window; k is the center value of the sliding window; s is the number of half-window samples, which is set to 15; The output average value of the accelerometer synthetic amplitude in this window interval is shown in formula (6):

[0056]

[0057] The judgment formula of definition condition 3 is shown in formula (7):

[0058]

[0059] Among them, C3 is the judgment result of condition 3, C3 = 1 means that the pedestrian is in a stationary state, and C3 = 0 means that the pedestrian is in a stationary state; The local standard deviation of the accelerometer composite amplitude in the window; Indicates the local standard deviation threshold of acceleration.

[0060] As a specific example, in step 2, the inertial measurement unit and camera that are fixed together are calibrated to obtain the corresponding intrinsic and extrinsic parameters, as follows:

[0061] Calibrate the intrinsic and extrinsic parameters of the sensor, including IMU intrinsic calibration, camera intrinsic calibration, and IMU and camera extrinsic calibration.

[0062] Furthermore, the IMU intrinsic parameter calibration uses the imu_utils open source tool produced by the Hong Kong University of Science and Technology; the IMU and camera extrinsic parameter calibration uses the kalibr toolbox.

[0063] As a specific example, run the VINS-Mono / VINS-Fusion algorithm in step 3 to calculate the six-degree-of-freedom position and attitude data of the visual inertial odometry and obtain the relative position and attitude information of the pedestrian's movement, as follows:

[0064] The camera provides images at the corresponding moment, providing raw data for subsequent pose estimation. Before acquiring image data, the sensor's intrinsic and extrinsic parameters must be calibrated. This involves three steps: IMU intrinsic calibration, camera intrinsic calibration, and IMU and camera extrinsic calibration. The quality of the calibration results is crucial to the accuracy of pose estimation. We recommend using the imu_utils tool from HKUST to calibrate the IMU intrinsic parameters and the kalibr toolbox to calibrate the camera intrinsic parameters and the camera and IMU extrinsic parameters.

[0065] Visual-inertial odometry (VIO) fuses camera and inertial measurement unit data for simultaneous positioning and mapping. After calibration, the relevant configuration files are modified. Then, using the VINS-Mono / VINS-Fusion visual-inertial fusion positioning algorithm, the camera and inertial measurement unit data are combined to calculate the six-degree-of-freedom position and attitude data of the visual-inertial odometry, thereby obtaining the relative pose information of pedestrians.

[0066] As a specific example, step 4 uses a strapdown inertial navigation calculation method to perform initial alignment and inertial navigation solution using foot inertial measurement unit data to obtain the pedestrian's instantaneous posture, speed, and position information, as follows:

[0067] Step 4.1: When describing the position, velocity, attitude, and other navigation information of the carrier, it is necessary to first determine the corresponding reference coordinate system so that the current motion state and positioning position information of the carrier are meaningful. The method described in this article mainly uses the carrier coordinate system (B system) and the navigation coordinate system (N system). The definitions of these two coordinate systems are as follows:

[0068] Carrier coordinate system: use o b x b y b z b Indicates that the origin is defined as the center of gravity or center position of the carrier, o b x b The axis moves forward along the longitudinal axis of the carrier, o b y b The axis is along the carrier's horizontal axis to the right, o b z b The axis is downward along the vertical axis of the carrier, and the carrier coordinate system is fixedly connected to the carrier;

[0069] Navigation coordinate system: use o n x n y n zn Indicates that the origin is defined as the center of gravity or center position of the carrier at the start, o n x n The axis points to geographic north, o n y n The axis points to geographic east, o n z n The axis is perpendicular to the local ellipsoid and points to the earth, that is, "northeast earth" is selected as the navigation reference coordinate system;

[0070] Step 4.2: Determine the initial values ​​of the vehicle's attitude, velocity, and position, and use the foot inertial measurement unit data for initial alignment, including horizontal alignment and azimuth alignment, as follows:

[0071] The first stage is horizontal alignment, using the accelerometer to calculate the pitch and roll angles as follows:

[0072] Assume that the measurement values ​​of the accelerometer along the three axes in the carrier coordinate system are f x b 、f y b 、f z b , the projection values ​​corresponding to the navigation coordinate system are f x n 、f y n 、f z n , the conversion relationship between them satisfies formula (8):

[0073]

[0074] in, is the coordinate transformation matrix from the n system to the b system, also known as the direction cosine matrix;

[0075] Set the navigation coordinate system to "northeast ground" and f under static base conditions n =[0 0g] T , then formula (8) can be further expressed as formula (9):

[0076]

[0077] Among them, g is the acceleration due to gravity, which is a known quantity; f x b 、f y b 、f z b is the accelerometer measurement value;

[0078] According to formula (9), the pitch angle θ and roll angle γ are calculated to complete the horizontal alignment. The calculation formula is shown in formula (10):

[0079]

[0080] The second stage is azimuth alignment, which uses the geomagnetic component output by the magnetic sensor and the roll and pitch angle information obtained from horizontal alignment to calculate the heading angle, as follows:

[0081] Assume that the measurement values ​​of the geomagnetic sensor along the three axes in the carrier coordinate system are The projection values ​​corresponding to the navigation coordinate system are The conversion relationship between them satisfies formula (11):

[0082]

[0083] Where, ψ is the yaw angle of the carrier relative to the magnetic north direction;

[0084] Calculate ψ according to formula (11), obtain the value of local magnetic declination D according to the table, and finally calculate the true heading angle according to formula (12) After completing the azimuth alignment, the calculation formula is shown in formula (12):

[0085]

[0086] Step 4.3: Use the strapdown inertial navigation algorithm to calculate the positioning parameters, as follows:

[0087] Taking the current position as the zero point, the pedestrian's running posture is updated using the quaternion and the gyroscope's angle increment, as shown in formula (13):

[0088]

[0089] Among them, q 1|k+1 ,q 2|k+1 ,q 3|k+1 ,q 4|k+1 are the q1, q2, q3, and q4 values ​​of the quaternion at time k+1, respectively. 1|k ,q 2|k ,q 3|k ,q 4|k is the q1, q2, q3, q4 value of the quaternion at time k; ω x 、ω y 、ω z Respectively represent the x-axis, y-axis, and z-axis angular velocity scalar values ​​output by the gyroscope; T m is the sampling time; Δ is the sampling time T m The carrier motion angle increment within is calculated as follows:

[0090]

[0091] Step 4.4: Calculate the pedestrian speed and location information as follows:

[0092] Step 4.4.1: Normalize the quaternion as shown in formula (14):

[0093]

[0094] Among them, q1' |k+1 ,q' 2|k+1 ,q' 3|k+1 ,q' 4|k+1 q 1|k+1 ,q 2|k+1 ,q 3|k+1 ,q 4|k+1 Normalized value; normalization makes the quaternion become a unit quaternion, and the sum of the squares of the four values ​​is 1;

[0095] Step 4.4.2: Get the direction cosine matrix based on the unit quaternion as follows:

[0096]

[0097] Step 4.4.3. Based on the direction cosine matrix, use trigonometric functions to calculate the Euler angles:

[0098]

[0099] Step 4.4.4: Convert the specific force measured by the accelerometer to the navigation coordinate system, then perform gravity acceleration compensation to obtain the geometric motion acceleration in the navigation coordinate system. Perform Newtonian mechanics integration on the motion acceleration to obtain the velocity and position, as shown in Equation (17):

[0100]

[0101] in, is the output value of the motion accelerometer after compensating for the gravity component, t k Direction cosine matrix from system b to system n at time instant; t k The specific force value output by the accelerometer at the moment; Δt = t k -t k-1 Represents a sampling period; v k-1|k-1 、v k|k-1 t k-1 , t k Speed ​​information at the moment; r k-1|k-1 、r k|k-1 t k-1 , t k Location information at the moment.

[0102] As a specific example, the Kalman filter model is established in step 5, and the speed difference and angular velocity difference in the zero-speed range, the attitude error and position error when the visual signal is valid are used as observation quantities to trigger the measurement update of the Kalman filter to achieve the optimal estimation of the navigation state, as follows:

[0103] Step 5.1: Inertial navigation technology can achieve good positioning accuracy in the short term, but over time, it can accumulate errors that become increasingly larger, leading to poor positioning accuracy. Kalman filtering is a widely used method for eliminating these errors, primarily by implementing a recursive linear minimum square error estimate using a discrete recursive expression.

[0104] The state space model of the Kalman filter discrete system is:

[0105]

[0106] Where, X k 、X k-1 are the state vectors of the system at time k and k-1, i.e. the required estimated state quantities; Φ k,k-1 is the state transition matrix from time k-1 to time k; Γ k-1 is the system noise distribution matrix, which indicates the degree to which each system noise from time k-1 to time k affects each state at time k; W k-1 represents the system noise vector at time k-1; Z k It represents the measurement vector of the system at time k, H k is the system measurement matrix at time k, V k represents the measurement noise vector at time k;

[0107] According to the requirements of Kalman filtering, W k and V k are mutually independent zero-mean Gaussian white noise vector sequences that satisfy:

[0108]

[0109] The above formula is the condition that noise needs to meet in the Kalman filtering process, where E[W k ] indicates that W k Find the expectation; δ kj It is 1 only when k=j, and 0 at other times. k is the noise variance matrix of the system, and a certain state quantity of the system may not have noise, so Q k is a non-negative definite matrix; R k is the measurement noise variance matrix, and each measurement contains noise, so R kis a positive definite matrix;

[0110] Step 5.2: The Kalman filter process is divided into two parts: time update and measurement update, which are represented by the following equations:

[0111] (1) One-step state prediction

[0112]

[0113] in, is the one-step prediction of the state at time k, i.e. the current time; Φ k,k-1 It is the one-step transfer matrix from the k-1 moment to the k moment; is the optimal estimate of the state at the k-1 moment, i.e. the previous moment;

[0114] (2) State one-step prediction mean square error matrix

[0115]

[0116] Among them, P k|k-1 is the state one-step prediction mean square error matrix; P k-1 is the mean square error matrix of the optimal estimate of the state at time k-1;

[0117] (3) Filter gain

[0118]

[0119] Among them, the coefficient matrix K k is the filter gain; is the transpose of the system measurement matrix at time k; P k is the mean square error matrix of the optimal estimate of the state at time k;

[0120] (4) State estimation

[0121]

[0122] in, is the optimal estimate of the state at time k; Z k is the measurement at time k; H k is the system measurement matrix at time k;

[0123] (5) State estimation mean square error matrix

[0124] P k =(IK k H k )P k|k-1 (twenty four)

[0125] Step 5.3: The selection of observations directly affects the filtering effect. Using the dual-assisted technology of visual inertial odometry and zero-speed correction, four observations, namely velocity, angular velocity, attitude, and position, are selected to correct the accumulated error of the inertial navigation.

[0126] First, a 21-dimensional state space equation, i.e., a filter model, is established, which includes three position errors, three velocity errors, three attitude errors, three-axis gyroscope bias error, three-axis accelerometer bias error, three-axis gyroscope scale factor, and three-axis accelerometer scale factor. The three attitude errors are the pitch angle θ, the roll angle γ, and the yaw angle.

[0127] The state space equation of the system is shown in Equation (25):

[0128]

[0129] in, δr k is the position error, δv k is the velocity error, δψ k is the attitude angle error, is the gyroscope bias error, is the accelerometer bias error, is the accelerometer scale factor, is the gyroscope scale factor; W(t) is the system process noise matrix: G(t) is the noise matrix coefficient matched with it; F(t) is the system state matrix; Z(t) is the observation quantity; H(t) is the observation matrix; V(t) is the observation noise matrix;

[0130] Step 5.4: Velocity and angular velocity correction, as follows:

[0131] When a pedestrian walks, the moment their foot contacts the ground is theoretically considered to be at rest, and the output velocity is zero at this moment. However, due to the influence of accelerometer and gyroscope noise, measurement errors and algorithm errors occur, resulting in non-zero calculated velocity values. Similarly, the angular velocity is non-zero.

[0132] The auxiliary correction of speed and angular velocity is: if t k If the pedestrian is detected to be stationary at zero speed at all times, t k The velocity v calculated by the accelerometer output at all times k The speed error Δv is obtained by subtracting the theoretical speed 0 k ; t k The angular velocity w output by the gyroscope at this moment k The angular velocity error Δw is obtained by subtracting the theoretical angular velocity 0. k; At the same time, the velocity error and angular velocity error are used as observation quantities for filtering. The corresponding observation quantities and observation matrices are shown in formula (26):

[0133]

[0134] Among them, Z k is the observed quantity; H k is the observation matrix; O 3×3 Represents a 3*3 zero matrix; I 3×3 represents the 3rd-order identity matrix;

[0135] Step 5.5: Position and attitude correction, as follows:

[0136] VIO uses the data collected by the inertial measurement unit carried by the pedestrian and the continuous images taken by the camera as input to restore the position and attitude of the camera. This method will cause visual failure when there are insufficient feature points in the surrounding environment. Depending on whether the feature points in the surrounding environment are sufficient, it is decided whether to connect the visual signal. When the visual signal is valid, the pose information output by VIO is accurate. The Euler angle ψ' output by VIO at this time is used as the k A set of Euler angles ψ output by the inertial navigation system after attitude update k Subtract to get the attitude angle error Δψ k , the position information r' output by VIO at this time k A set of position information r output by the inertial navigation system k Subtract to get the position error Δr k , and position error and attitude error are used as observation quantities for filtering. The corresponding observation quantities and observation matrices are shown in formula (27):

[0137]

[0138] Step 6: Use the current corrected state as the initial value to continue the inertial navigation solution and repeat the navigation correction process, as follows:

[0139] Use the current corrected state quantity as the initial quantity and return to step 3 to perform the next inertial navigation solution; repeat the navigation correction process until the navigation is completed.

[0140] In summary, the present invention utilizes the high short-term accuracy of the inertial measurement unit to drive the process model, thereby improving the positioning and navigation accuracy; utilizes the zero-speed characteristics of pedestrian gait as a constraint condition to construct the observation quantity, and corrects the accumulated error of the inertial navigation without increasing the hardware basis, thereby ensuring that higher navigation accuracy is obtained in a relatively short period of time; utilizes the posture information estimated by the visual inertial odometry as a constraint condition to construct the observation quantity, further corrects the accumulated error of the inertial measurement unit, and ensures that higher navigation accuracy is still obtained over a longer period of time; utilizes the dual auxiliary technology of visual inertial odometry and zero-speed correction to improve the accuracy of indoor pedestrian navigation positioning.

Claims

1. A vision-assisted gait feature-constrained pedestrian navigation method, characterized in that: The following steps are involved: Step 1: Analyze the pedestrian's gait characteristics based on the data collected by the inertial measurement unit fixed to the foot, and use the three-condition method, sliding filtering technology, and median filtering technology to obtain the zero-speed interval; Step 2: Calibrate the connected inertial measurement unit and camera to obtain the corresponding intrinsic and extrinsic parameters; Step 3: Run the VINS-Mono / VINS-Fusion algorithm to calculate the six-degree-of-freedom position and attitude data of the visual inertial odometry and obtain the relative position and attitude information of the pedestrian. Step 4: Using the strapdown inertial navigation calculation method, the foot inertial measurement unit data is used for initial alignment and inertial navigation solution to obtain the pedestrian's instantaneous posture, velocity and position information; Step 5: Establish a Kalman filter model, using the speed difference and angular velocity difference in the zero-speed range, and the attitude error and position error when the visual signal is valid as observation quantities, triggering the measurement update of the Kalman filter to achieve optimal estimation of the navigation state; Step 6: Use the current corrected state quantity as the initial quantity to continue the inertial navigation solution and repeat the navigation correction process.

2. The visually assisted gait feature constrained pedestrian navigation method according to claim 1, characterized in that: In step 1, the pedestrian's gait characteristics are analyzed based on the data collected by the inertial measurement unit fixed to the foot. The zero-speed interval is obtained using the three-condition method, sliding filtering technology and median filtering technology, as follows: Step 1.1, obtain the acceleration and angular velocity data sampled by the sensor; Step 1.2: Calculate the three judgment conditions of the three-condition method based on the acceleration and angular velocity data. When the judgment results of the three conditions are all 1, the pedestrian is in a stationary state; Step 1.3: Use the median filter algorithm to select a sampling center and a certain window space, and take the median of the data within the window as the data for this time; Step 1.4: Remove noise points and finally obtain gait detection data.

3. The visually assisted gait feature constrained pedestrian navigation method according to claim 2, characterized in that: The acceleration and angular velocity data obtained by the sensor sampling in step 1.1 are as follows: The inertial sensor is fixed on the pedestrian's foot, the foot motion characteristics during walking are sampled, and the acceleration and angular velocity data obtained by the sensor sampling are extracted.

4. The visually assisted gait feature constrained pedestrian navigation method according to claim 2, characterized in that: In step 1.2, the three judgment conditions of the three-condition method are calculated based on the acceleration and angular velocity data. When the judgment results of the three conditions are all 1, the pedestrian is in a stationary state, as follows: The three-condition method uses three conditions to judge the human body's gait, with state 0 representing movement and state 1 representing stillness; Judgment condition 1: Acceleration vector and threshold method: When the composite amplitude output by the accelerometer is between the given upper and lower thresholds, the pedestrian is judged to be stationary; Defines the composite amplitude of the accelerometer output |a k As shown in formula (1): Among them, |a k | represents the modulus of acceleration at time k, and They represent the acceleration of the carrier along the x, y, and z axes collected by the k-th accelerometer respectively; The judgment formula of definition condition 1 is shown in formula (2): Among them, C1 is the judgment result of condition 1, C1 = 1 means that the pedestrian is in a stationary state, and C1 = 0 means that the pedestrian is in a moving state; amin =9m / s 2 and th amax =11m / s 2 , represent the acceleration vector and threshold respectively; Judgment condition 2: Angular velocity vector and threshold method: When the composite amplitude output by the gyroscope is between the given upper and lower thresholds, the pedestrian is judged to be stationary; Defines the composite amplitude of the gyroscope output |ω k As shown in formula (3): Among them, |ω k | is the modulus of the angular velocity at time k; and They represent the x, y and z angular velocities of the carrier collected by the k-th gyroscope respectively; The judgment formula of definition condition 2 is shown in formula (4): Among them, C2 is the judgment result of condition 2, C2 = 1 means the pedestrian is in a stationary state, C2 = 0 means the pedestrian is in a stationary state; ωmax =2deg / s, representing the angular velocity vector and threshold; Judgment condition 3: Acceleration local variance method: When the local variance of the accelerometer output is lower than a given threshold, the pedestrian is judged to be stationary; The local variance sliding filtering technique is used to determine the stability of the 2s+1 sampling points in this interval by calculating the local variance within the window. When the local variance within the window is less than the threshold, close to 0, the center point of the window is the point where the gait is still, and the corresponding value of the rectangular wave is 1. On the contrary, when the local variance within the window is greater than the threshold, the center point of the window cannot be considered as an absolute still point, so it is removed and the corresponding rectangular wave value is 0. The center point slides through each sampling point in sequence with a step size of 1. The first s sampling points and the last s sampling points cannot be used as center points, but remain stationary before and after sampling and are set to stationary state 1. The final stable stage after filtering is taken as the absolute gait still moment. The local variance of the accelerometer output is defined as shown in formula (5): in, is the local variance of the accelerometer composite amplitude in this window; is the accelerometer composite amplitude at the qth sampling point; q is the subscript of each sampling point within this window; k is the center value of the sliding window; s is the number of half-window samples, which is set to 15; The output average value of the accelerometer synthetic amplitude in this window interval is shown in formula (6): The judgment formula of definition condition 3 is shown in formula (7): Among them, C3 is the judgment result of condition 3, C3 = 1 means that the pedestrian is in a stationary state, and C3 = 0 means that the pedestrian is in a stationary state; The local standard deviation of the accelerometer composite amplitude in the window; Indicates the local standard deviation threshold of acceleration.

5. The visually assisted gait feature constrained pedestrian navigation method according to claim 1, characterized in that: In step 2, the connected inertial measurement unit and camera are calibrated to obtain the corresponding intrinsic and extrinsic parameters, as follows: Calibrate the intrinsic and extrinsic parameters of the sensor, including IMU intrinsic calibration, camera intrinsic calibration, and IMU and camera extrinsic calibration.

6. The visually assisted gait feature constrained pedestrian navigation method according to claim 5, characterized in that: The IMU intrinsic parameter calibration is performed using the imu_utils open source tool from HKUST; the IMU and camera extrinsic parameter calibration is performed using the kalibr toolbox.

7. The visually assisted gait feature constrained pedestrian navigation method according to claim 1, characterized in that: In step 3, run the VINS-Mono / VINS-Fusion algorithm to calculate the six-degree-of-freedom position and attitude data of the visual inertial odometry and obtain the relative position and attitude information of the pedestrian's movement, as follows: Modify the relevant configuration files and use the VINS-Mono / VINS-Fusion visual-inertial fusion positioning algorithm to fuse camera and inertial measurement unit data, calculate the six-degree-of-freedom position and attitude data of the visual-inertial odometry, and obtain the relative position and attitude information of pedestrian motion.

8. The visually assisted gait feature constrained pedestrian navigation method according to claim 1, characterized in that: In step 4, the strapdown inertial navigation calculation method is used to perform initial alignment and inertial navigation solution using the foot inertial measurement unit data to obtain the pedestrian's instantaneous posture, velocity, and position information, as follows: Step 4.1: Determine the reference coordinate systems used for strapdown inertial navigation as the vehicle coordinate system and the navigation coordinate system. The definitions of these two coordinate systems are as follows: Carrier coordinate system: use o b x b y b z b Indicates that the origin is defined as the center of gravity or center position of the carrier, o b x b The axis moves forward along the longitudinal axis of the carrier, o b y b The axis is along the carrier's horizontal axis to the right, o b z b The axis is downward along the vertical axis of the carrier, and the carrier coordinate system is fixedly connected to the carrier; Navigation coordinate system: use o n x n y n z n Indicates that the origin is defined as the center of gravity or center position of the carrier at the start, o n x n The axis points to geographic north, o n y n The axis points to geographic east, o n z n The axis is perpendicular to the local ellipsoid and points to the earth, that is, "northeast" is selected as the navigation reference coordinate system; Step 4.2: Determine the initial values ​​of the vehicle's attitude, velocity, and position, and use the foot inertial measurement unit data for initial alignment, including horizontal alignment and azimuth alignment, as follows: The first stage is horizontal alignment, using the accelerometer to calculate the pitch and roll angles as follows: Assume that the measurement values ​​of the accelerometer along the three axes in the carrier coordinate system are The projection values ​​corresponding to the navigation coordinate system are The conversion relationship between them satisfies formula (8): in, is the coordinate transformation matrix from the n system to the b system, also known as the direction cosine matrix; Set the navigation coordinate system to "northeast ground" and f under static base conditions n =[0 0 g] T , then formula (8) can be expressed as formula (9): Among them, g is the acceleration due to gravity, which is a known quantity; is the accelerometer measurement value; According to formula (9), the pitch angle θ and roll angle γ are calculated to complete the horizontal alignment. The calculation formula is shown in formula (10): The second stage is azimuth alignment, which uses the geomagnetic component output by the magnetic sensor and the roll and pitch angle information obtained from horizontal alignment to calculate the heading angle, as follows: Assume that the measurement values ​​of the geomagnetic sensor along the three axes in the carrier coordinate system are The projection values ​​corresponding to the navigation coordinate system are The conversion relationship between them satisfies formula (11): Where, ψ is the yaw angle of the carrier relative to the magnetic north direction; Calculate ψ according to formula (11), obtain the value of local magnetic declination D according to the table, and finally calculate the true heading angle according to formula (12) After completing the azimuth alignment, the calculation formula is shown in formula (12): Step 4.3: Use the strapdown inertial navigation algorithm to calculate the positioning parameters, as follows: Taking the current position as the zero point, the pedestrian's running posture is updated using the quaternion and the gyroscope's angle increment, as shown in formula (13): Among them, q 1|k+1 ,q 2|k+1 ,q 3|k+1 ,q 4|k+1 are the q1, q2, q3, and q4 values ​​of the quaternion at time k+1, respectively. 1|k ,q 2|k ,q 3|k ,q 4|k is the q1, q2, q3, q4 value of the quaternion at time k; ω x 、ω y 、ω z Respectively represent the x-axis, y-axis, and z-axis angular velocity scalar values ​​output by the gyroscope; T m is the sampling time; Δ is the sampling time T m The carrier motion angle increment within is calculated as follows: Step 4.4: Calculate the pedestrian speed and location information as follows: Step 4.4.1: Normalize the quaternion as shown in formula (14): Among them, q1' |k+1 ,q' 2|k+1 ,q' 3|k+1 ,q' 4|k+1 q 1|k+1 ,q 2|k+1 ,q 3|k+1 ,q 4|k+1 Normalized value; normalization makes the quaternion become a unit quaternion, and the sum of the squares of the four values ​​is 1; Step 4.4.2: Get the direction cosine matrix based on the unit quaternion as follows: Step 4.4.

3. Based on the direction cosine matrix, use trigonometric functions to calculate the Euler angles: Step 4.4.4: Convert the specific force measured by the accelerometer to the navigation coordinate system, then perform gravity acceleration compensation to obtain the geometric motion acceleration in the navigation coordinate system. Perform Newtonian mechanics integration on the motion acceleration to obtain the velocity and position, as shown in Equation (17): in, is the output value of the motion accelerometer after compensating for the gravity component, t k Direction cosine matrix from system b to system n at time instant; t k The specific force value output by the accelerometer at the moment; Δt = t k -t k-1 Represents a sampling period; v k-1|k-1 、v k|k-1 t k-1 , t k Speed ​​information at the moment; r k-1|k-1 、r k|k-1 t k-1 , t k Location information at the moment.

9. The visually assisted gait feature constrained pedestrian navigation method according to claim 1, characterized in that: In step 5, a Kalman filter model is established. The speed difference and angular velocity difference in the zero-speed range, and the attitude error and position error when the visual signal is valid are used as observation quantities to trigger the measurement update of the Kalman filter and achieve the optimal estimation of the navigation state. The details are as follows: Step 5.1, the state space model of the Kalman filter discrete system is: Where, X k 、X k-1 are the state vectors of the system at time k and k-1, i.e. the required estimated state quantities; Φ k,k-1 is the state transition matrix from time k-1 to time k; Γ k-1 is the system noise distribution matrix, which indicates the degree to which each system noise from time k-1 to time k affects each state at time k; W k-1 represents the system noise vector at time k-1; Z k It represents the measurement vector of the system at time k, H k is the system measurement matrix at time k, V k represents the measurement noise vector at time k; According to the requirements of Kalman filtering, W k and V k are mutually independent zero-mean Gaussian white noise vector sequences that satisfy: The above formula is the condition that noise needs to meet in the Kalman filtering process, where E[W k ] indicates that W k Find the expectation; δ kj It is 1 only when k=j, and 0 at other times. k is the noise variance matrix of the system, and a certain state quantity of the system may not have noise, so Q k is a non-negative definite matrix; R k is the measurement noise variance matrix, and each measurement contains noise, so R k is a positive definite matrix; Step 5.2: The Kalman filter process is divided into two parts: time update and measurement update, which are represented by the following equations: (1) One-step state prediction in, is the one-step prediction of the state at time k, i.e. the current time; Φ k,k-1 It is the one-step transfer matrix from the k-1 moment to the k moment; is the optimal estimate of the state at the k-1 moment, i.e. the previous moment; (2) State one-step prediction mean square error matrix Among them, P k|k-1 is the state one-step prediction mean square error matrix; P k-1 is the mean square error matrix of the optimal estimate of the state at time k-1; (3) Filter gain Among them, the coefficient matrix K k is the filter gain; is the transpose of the system measurement matrix at time k; P k is the mean square error matrix of the optimal estimate of the state at time k; (4) State estimation in, is the optimal estimate of the state at time k; Z k is the measurement at time k; H k is the system measurement matrix at time k; (5) State estimation mean square error matrix P k =(I-K k H k )P k|k-1 (24) Step 5.3: The selection of observations directly affects the filtering effect. Using the dual-assisted technology of visual inertial odometry and zero-speed correction, four observations, namely velocity, angular velocity, attitude, and position, are selected to correct the accumulated error of the inertial navigation. First, a 21-dimensional state space equation, i.e., a filter model, is established, which includes three position errors, three velocity errors, three attitude errors, three-axis gyroscope bias error, three-axis accelerometer bias error, three-axis gyroscope scale factor, and three-axis accelerometer scale factor. The three attitude errors are the pitch angle θ, the roll angle γ, and the yaw angle. The state space equation of the system is shown in Equation (25): in, δr k is the position error, δv k is the velocity error, δψ k is the attitude angle error, is the gyroscope bias error, is the accelerometer bias error, is the accelerometer scale factor, is the gyroscope scale factor; W(t) is the system process noise matrix: G(t) is the noise matrix coefficient matched with it; F(t) is the system state matrix; Z(t) is the observation quantity; H(t) is the observation matrix; V(t) is the observation noise matrix; Step 5.4: Velocity and angular velocity correction, as follows: When a pedestrian walks, the moment their foot contacts the ground is theoretically considered to be at rest, and the output velocity is zero at this moment. However, due to the influence of accelerometer and gyroscope noise, measurement errors and algorithm errors occur, resulting in non-zero calculated velocity values. Similarly, the angular velocity is non-zero. The auxiliary correction of speed and angular velocity is: if t k If the pedestrian is detected to be stationary at zero speed at all times, t k The velocity v calculated by the accelerometer output at all times k The speed error Δv is obtained by subtracting the theoretical speed 0 k ; t k The angular velocity w output by the gyroscope at this moment k The angular velocity error Δw is obtained by subtracting the theoretical angular velocity 0. k ; At the same time, the velocity error and angular velocity error are used as observation quantities for filtering. The corresponding observation quantities and observation matrices are shown in formula (26): Among them, Z k is the observed quantity; H k is the observation matrix; O 3×3 Represents a 3*3 zero matrix; I 3×3 represents the 3rd-order identity matrix; Step 5.5: Position and attitude correction, as follows: VIO uses the data collected by the inertial measurement unit carried by the pedestrian and the continuous images taken by the camera as input to restore the position and attitude of the camera. This method will cause visual failure when there are insufficient feature points in the surrounding environment. Depending on whether the feature points in the surrounding environment are sufficient, it is decided whether to connect the visual signal. When the visual signal is valid, the pose information output by VIO is accurate. The Euler angle ψ' output by VIO at this time is used as the k A set of Euler angles ψ output by the inertial navigation system after attitude update k Subtract to get the attitude angle error Δψ k , the position information r' output by VIO at this time k A set of position information r output by the inertial navigation system k Subtract to get the position error Δr k , and position error and attitude error are used as observation quantities for filtering. The corresponding observation quantities and observation matrices are shown in formula (27):

10. The visually assisted gait feature constrained pedestrian navigation method according to claim 1, characterized in that: In step 6, the current corrected state quantity is used as the initial quantity to continue the inertial navigation solution and repeat the navigation correction process, as follows: Use the current corrected state quantity as the initial quantity and return to step 3 to perform the next inertial navigation solution; repeat the navigation correction process until the navigation is completed.

Images