Detailed Description
The invention will now be described in further detail with reference to the drawings and examples.
With reference to fig. 1, the vision-assisted gait feature-constrained pedestrian navigation method of the invention comprises the following steps:
Step 1, analyzing gait characteristics of pedestrians based on data acquired by an inertial measurement unit fixed on feet, and acquiring a zero-speed interval by using a three-condition method, a sliding filtering technology and a median filtering technology;
Step 2, calibrating the inertial measurement unit and the camera which are fixedly connected together to obtain corresponding internal parameters and external parameters;
step 3, running a VINS-Mono/VINS-Fusion algorithm, and calculating to obtain the position and posture data of six degrees of freedom of the visual inertial odometer, so as to obtain the relative posture information of pedestrian movement;
step 4, adopting a strapdown inertial navigation calculation method, and carrying out initial alignment and inertial navigation calculation by utilizing foot inertial measurement unit data to obtain the instantaneous posture, speed and position information of the pedestrian;
Step 5, establishing a Kalman filtering model, taking the speed difference and the angular speed difference in the zero-speed interval and the attitude error and the position error when the visual signal is effective as observables, triggering the measurement update of the Kalman filter, and realizing the optimal estimation of the navigation state;
and step 6, continuing inertial navigation calculation by taking the current corrected state quantity as an initial quantity, and repeating the navigation correction process.
As a specific example, in step 1, gait characteristics of a pedestrian are analyzed based on data acquired by an inertial measurement unit fixed to a foot, and a zero-speed interval is acquired by using a tri-condition method, a sliding filtering technique and a median filtering technique, which is specifically as follows:
Step 1.1, acquiring acceleration and angular velocity data obtained by sampling a sensor;
step 1.2, calculating three judging conditions of a three-condition method according to acceleration and angular velocity data, and when judging results of the three conditions are all 1, enabling pedestrians to be in a static state;
Step 1.3, selecting a sampling center and a certain window space through a median filtering algorithm, and taking the median of the data in the window as the data of the time;
And 1.4, removing noise points, and finally obtaining gait detection data.
As a specific example, the acceleration and angular velocity data sampled by the sensor in step 1.1 is specifically as follows:
fixing an inertial sensor on the foot surface of a walker, sampling the motion characteristics of the foot in the walking process, and extracting acceleration and angular velocity data obtained by sampling the sensor;
As a specific example, in step 1.2, three judgment conditions of the three-condition method are calculated according to the acceleration and angular velocity data, and when the judgment results of the three conditions are all "1", the pedestrian is in a stationary state, specifically as follows:
the inertial measurement unit consists of a triaxial gyroscope and a triaxial accelerometer and directly outputs triaxial angular velocity and specific force measurement values under an inertial coordinate system. The inertial sensor is fixed on the foot surface of the walker, and the motion characteristics of the foot during walking are sampled. The acceleration and angular velocity data obtained by sampling the sensor are extracted, and it can be found that the acceleration and angular velocity output in the process of moving the pedestrian have periodicity and have zero transient characteristics, as shown in fig. 2, the zero transient is outlined by a black rectangle.
The three-condition method completes the recognition work of the gait zero-speed interval based on the acceleration and angular velocity related physical information measured by the inertial sensor. And the identified static gait is used as a trigger condition for zero speed correction, and long-time accumulated errors caused by IMU positioning are corrected, so that the positioning accuracy is improved. The addition of the local method difference sliding filtering technology and the median filtering technology in the three-condition method can effectively inhibit system noise and remove wild points, so that the zero-speed detection result is more reliable. A specific flow for acquiring the zero-speed interval is shown in fig. 3.
According to the acceleration and angular velocity data, three judging conditions of a three-condition method are calculated, and when judging results of the three conditions are all 1, the pedestrian is in a static state, and the method specifically comprises the following steps:
the three-condition method adopts three conditions to judge the human body movement gait, wherein the state '0' represents movement and the state '1' represents rest;
judging the condition 1, namely judging that the pedestrian is stationary when the synthesized amplitude output by the accelerometer is between a given upper threshold value and a given lower threshold value by an acceleration vector sum threshold value method;
the resulting amplitude of the accelerometer output is defined as ak as shown in equation (1):
Wherein, |ak | represents the modulus of the acceleration at time k,AndThe x, y and z triaxial acceleration values of the carrier acquired by the kth accelerometer are respectively shown;
The judgment formula defining the condition 1 is shown in the formula (2):
Wherein, C1 is the judgment result of condition 1, c1=1 indicates that the pedestrian is in a static state, and c1=0 indicates that the pedestrian is in a moving state, thamin＝9m/s2 and thamax＝11m/s2 respectively indicate an acceleration vector and a threshold value;
Judging the condition 2, namely judging that the pedestrian is stationary when the synthesized amplitude output by the gyroscope is between a given upper threshold value and a given lower threshold value by an angular velocity vector sum threshold value method;
the resulting amplitude of the gyroscope output is defined as # omegak @ shown in equation (3):
wherein, |omegak | is the modulus of the angular velocity at the time k; AndThe x, y and z triaxial angular speeds of the carrier acquired by the kth gyroscope are respectively shown;
the judgment formula defining the condition 2 is shown in the formula (4):
wherein, C2 is the judgment result of condition 2, c2=1 indicates that the pedestrian is in a static state, c2=0 indicates that the pedestrian is in a static state, thωmax =2 deg/s indicates an angular velocity vector and a threshold value;
judging the condition 3, namely judging that the pedestrian is stationary when the local variance output by the accelerometer is lower than a given threshold value;
The method comprises the steps of adopting a local variance sliding filtering technology, judging the stability of 2s+1 sampling points in a region by calculating the local variance in a window, setting a central point of the window as a gait stationary point and a rectangular wave corresponding value as 1 when the local variance in the window is smaller than a threshold value and setting the central point of the window as a stationary state 1 when the local variance in the window is larger than the threshold value, removing the window because the central point of the window cannot be calculated as an absolute stationary point and the rectangular wave value is 0 when the local variance in the window is larger than the threshold value, sliding the central point through each sampling point in sequence according to a step of stepping to be 1, setting the first s sampling points and the last s sampling points as central points, and setting the stationary state 1 in the stationary state in place before and after sampling;
The local variance of the accelerometer output is defined as shown in equation (5):
Wherein, theSynthesizing a local variance of the amplitude for the window accelerometer; Synthesizing amplitude values for the accelerometer with the q sampling points, wherein q is the subscript of each sampling point in the window, k is the central value of the sliding window, s is the sampling number of the half window, and the set value is 15; the accelerometer for this window interval synthesizes an output average of the magnitudes as shown in equation (6):
The judgment formula defining the condition 3 is shown in the formula (7):
Wherein C3 is the judgment result of condition 3, c3=1 indicates that the pedestrian is in a stationary state, and c3=0 indicates that the pedestrian is in a stationary state; Synthesizing local standard deviation of amplitude values for the accelerometer of the window; representing the acceleration local standard deviation threshold.
As a specific example, in step 2, calibrating the inertial measurement unit and the camera that are fixedly connected together to obtain corresponding internal parameters and external parameters, which are specifically as follows:
And calibrating the internal parameters and the external parameters of the sensor, including IMU internal parameter calibration, camera internal parameter calibration, IMU and camera external parameter calibration.
Further, IMU internal parameter calibration adopts an imu_ utils open source tool of Kong university, and IMU and camera external parameter calibration adopts a kalibr tool box.
As a specific example, step 3 runs the VINS-Mono/VINS-Fusion algorithm, and obtains the position and posture data of six degrees of freedom of the visual inertial odometer by calculating, and obtains the relative posture information of the pedestrian motion, which is specifically as follows:
The camera is used for providing images at corresponding moments and providing raw data for subsequent pose estimation. Before image data are acquired, the internal parameters and the external parameters of the sensor are required to be calibrated, and the method mainly comprises three steps of IMU internal parameter calibration, camera internal parameter calibration, IMU and camera external parameter calibration, and the quality of a calibration result is important to pose resolving precision. Imu_ utils, a commercially available product from harbor, is recommended to calibrate IMU references, and kalibr toolbox is recommended to calibrate camera references and camera and IMU references.
And a Visual Inertial Odometer (VIO) is used for fusing the data of the camera and the inertial measurement unit to realize instant positioning and map construction. After calibration, the relevant configuration file is modified, and then the position and posture data of six degrees of freedom of the visual inertial odometer are obtained by means of the VINS-Mono/VINS-Fusion visual inertial Fusion positioning algorithm and Fusion of the camera and the inertial measurement unit data, so that the relative posture information of pedestrian movement is obtained.
As a specific example, the method for calculating strapdown inertial navigation in step 4 uses the data of the foot inertial measurement unit to perform initial alignment and inertial navigation calculation to obtain the instantaneous posture, speed and position information of the pedestrian, and specifically includes the following steps:
in step 4.1, when describing navigation information such as position, speed, gesture and the like of the carrier, a corresponding reference coordinate system needs to be determined first, so that the current motion state and positioning position information of the carrier are meaningful. Mainly used in the method described herein are a carrier coordinate system (b-system) and a navigation coordinate system (n-system), which are defined as follows:
The carrier coordinate system is represented by obxbybzb, the origin is defined as the gravity center or the central position of the carrier, the obxb axis is forwards along the longitudinal axis of the carrier, the obyb axis is rightwards along the transverse axis of the carrier, the obzb axis is downwards along the vertical axis of the carrier, and the carrier coordinate system is fixedly connected with the carrier;
The navigation coordinate system is represented by onxnynzn, the origin is defined as the gravity center or the center position of the carrier at the beginning, the onxn axis points to the geographic north direction, the onyn axis points to the geographic east direction, and the onzn axis is perpendicular to the local rotation elliptical surface and points to the ground direction, namely, the north east ground is selected as the navigation reference coordinate system;
step 4.2, determining initial values of the attitude, the speed and the position of the carrier, and performing initial alignment by utilizing foot inertia measurement unit data, wherein the initial alignment comprises horizontal alignment and azimuth alignment and is specifically as follows:
The first stage is horizontal alignment, and pitch angle and roll angle are calculated by using an accelerometer, specifically as follows:
the measured values of the accelerometer along three axes under a carrier coordinate system are respectively fxb、fyb、fzb, the projected values corresponding to a navigation coordinate system are respectively fxn、fyn、fzn, and the conversion relation between the measured values meets the formula (8):
Wherein, theA coordinate transformation matrix from n-system to b-system, also called a directional cosine matrix;
assuming that the navigation coordinate system is "north east", and fn＝[0 0g]T is under the static base condition, formula (8) may be further expressed as formula (9):
Wherein g is the gravitational acceleration, which is a known quantity, and fxb、fyb、fzb is the accelerometer measurement;
The pitch angle theta and the roll angle gamma are calculated according to the formula (9) to finish horizontal alignment, and the calculation formula is shown as the formula (10):
The second stage is azimuth alignment, which calculates a course angle by using geomagnetic components output by the magnetic sensor and combining roll angle and pitch angle information obtained by horizontal alignment, and the method comprises the following steps:
the measured values of the geomagnetic sensor along three axes under the carrier coordinate system are respectively set to beProjection values corresponding to the navigation coordinate system are respectivelyThe conversion relationship therebetween satisfies the formula (11):
wherein, psi is the yaw angle of the carrier relative to the magnetic north direction;
Calculating psi according to formula (11), obtaining the value of local geomagnetic deflection angle D according to table lookup, and finally calculating the true course angle according to formula (12)And finishing azimuth alignment, wherein a calculation formula is shown in a formula (12):
and 4.3, calculating positioning parameters by using a strapdown inertial navigation algorithm, wherein the positioning parameters are specifically as follows:
Updating the running gesture of the pedestrian by using the current position as a zero point and using the quaternion and the angular increment of the gyroscope, as shown in a formula (13):
Wherein q1|k+1、q2|k+1、q3|k+1、q4|k+1 is the q1, q2, q3, q4 value of the quaternion at k+1 time, q1|k、q2|k、q3|k、q4|k is the q1, q2, q3, q4 value of the quaternion at k time, ωx、ωy、ωz is the x, y, z axis angular velocity scalar value of the gyroscope output, Tm is the sampling time, Δ is the carrier movement angle increment in the sampling time Tm, and the calculation formula is as follows:
and 4.4, calculating pedestrian speed and position information, wherein the method specifically comprises the following steps of:
Step 4.4.1, carrying out normalization processing on the quaternion, wherein the normalization processing is as shown in a formula (14):
Wherein, q1'|k+1、q'2|k+1、q'3|k+1、q'4|k+1 is the value after q1|k+1、q2|k+1、q3|k+1、q4|k+1 normalization treatment, normalization is carried out to make the quaternion become a unit quaternion, and the square sum of the four values is 1;
step 4.4.2, obtaining a direction cosine matrix according to the unit quaternionThe following are provided:
and 4.4.3, calculating an Euler angle by adopting a trigonometric function according to the direction cosine array:
Step 4.4.4, converting the specific force measured by the accelerometer into a navigation coordinate system, then compensating for gravity acceleration to obtain geometrical movement acceleration under the navigation coordinate system, and carrying out Newton mechanical integration on the movement acceleration to obtain speed and position, wherein the speed and position are shown in a formula (17):
Wherein, theTo compensate for the motion accelerometer output after the gravitational component,A direction cosine array from time b of tk to n; The specific force value output by the accelerometer at the time tk is measured, deltat=tk-tk-1 represents one sampling period, vk-1|k-1、vk|k-1 is the speed information at the time tk-1、tk respectively, and rk-1|k-1、rk|k-1 is the position information at the time tk-1、tk respectively.
As a specific example, in the step 5, a kalman filter model is established, and the speed difference and the angular speed difference in the zero speed interval, and the attitude error and the position error when the visual signal is valid are taken as observables, so as to trigger the measurement update of the kalman filter, thereby realizing the optimal estimation of the navigation state, which is specifically as follows:
step 5.1, the inertial navigation technology can achieve better positioning accuracy in a short period, but integration accumulated errors exist for a long time, and the accumulated errors become larger and larger, so that the positioning accuracy is poor. The Kalman filtering technology is a method widely used for eliminating integral accumulated errors, and recursive linear minimum variance estimation is mainly realized through a discrete recursive expression form.
The state space model of the Kalman filtering discrete system is as follows:
Wherein Xk、Xk-1 is the state vector of the system at the time of k and the time of k-1, namely the required estimated state quantity, phik,k-1 is the state transition matrix from the time of k-1 to the time of k, gammak-1 is the system noise distribution matrix and represents the degree that each system noise from the time of k-1 to the time of k affects each state at the time of k respectively, Wk-1 represents the system noise vector at the time of k-1, Zk represents the measurement vector of the system at the time of k, Hk is the system measurement matrix at the time of k, and Vk represents the measurement noise vector at the time of k;
According to the Kalman filtering requirement, Wk and Vk are mutually independent zero-mean Gaussian white noise vector sequences, and the following conditions are satisfied:
The above formula is the condition that noise needs to meet in the Kalman filtering process, wherein E [ Wk ] represents the requirement on Wk, deltakj is 0 only when k=j is 1 and other times are all 0, meanwhile, Qk is a non-negative definite matrix because Qk is a noise variance matrix of the system and a certain state quantity of the system possibly does not have noise, Rk is a measured noise variance matrix, and each quantity measurement contains noise, so Rk is a positive definite matrix;
Step 5.2, the Kalman filtering process is divided into two parts of time updating and measurement updating, which are represented by the following equations:
(1) State one-step prediction
Wherein, thePhik,k-1 is a one-step transition matrix from k-1 time to k time; The optimal estimation of the state at the moment k-1, namely the last moment;
(2) State one-step prediction mean square error array
Wherein, Pk|k-1 is a state one-step prediction mean square error matrix, Pk-1 is a k-1 moment state optimal estimated mean square error matrix;
(3) Filtering gain
Wherein, the coefficient matrix Kk is the filtering gain; Pk is the mean square error matrix of the optimal estimation of the state at the moment k;
(4) State estimation
Wherein, theThe method is used for estimating the state at the moment k optimally, wherein Zk is the measurement at the moment k, and Hk is the system measurement matrix at the moment k;
(5) State estimation mean square error matrix
Pk＝(I-KkHk)Pk|k-1 (24)
Step 5.3, selecting observables to directly influence the filtering effect, and correcting inertial navigation accumulated errors by using a visual inertial odometer and a zero-speed correction dual auxiliary technology, and selecting four observables of speed, angular speed, attitude and position;
firstly, a state space equation of 21 dimensions including 3 position errors, 3 speed errors, 3 attitude errors, 3-axis gyroscope zero offset errors, 3-axis accelerometer zero offset errors, 3-axis gyroscope scale factors and 3-axis accelerometer scale factors is established, namely a filtering model, wherein the 3 attitude errors are respectively a pitch angle theta, a roll angle gamma and a yaw angle
The state space equation of the system is shown in formula (25):
Wherein, theΔrk is a position error, δvk is a velocity error, δψk is an attitude angle error,Is the zero offset error of the gyroscope,For the zero offset error of the accelerometer,For the scale factor of the accelerometer,Is a gyroscope scale factor, and W (t) is a system process noise matrix: G (t) is a noise matrix coefficient matched with the system, F (t) is a system state matrix, Z (t) is an observed quantity, H (t) is an observed matrix, and V (t) is an observed noise matrix;
Step 5.4, speed and angular velocity correction, specifically as follows:
In the walking process of pedestrians, the moment that the sole contacts with the ground is regarded as a theoretical static state, the output speed is 0 at the moment, but measurement errors and algorithm errors occur due to the influence of accelerometer noise and gyroscope noise, so that the calculated speed value at the moment is not zero, and the angular speed is not zero in the same way;
The auxiliary speed and angular speed correction is that if the zero speed stillness of the pedestrian is detected at the moment tk, the speed vk calculated by using the output of the accelerometer at the moment tk is differenced from the theoretical speed 0 to obtain a speed error Deltavk, the angular speed wk output by the gyroscope at the moment tk is differenced from the theoretical angular speed 0 to obtain an angular speed error Deltawk, meanwhile, the speed error and the angular speed error are adopted as observational quantity to carry out filtering, and the corresponding observational quantity and the observation matrix are shown as the formula (26):
Wherein Zk is observed quantity, Hk is observation matrix, O3×3 represents 3*3 zero matrix, and I3×3 represents 3-order identity matrix;
step 5.5, correcting the pose and the posture, wherein the method specifically comprises the following steps:
When the visual signal is valid, the Euler angle psi 'k output by the VIO and a group of Euler angles psik output by the inertial navigation system through gesture updating are subtracted to obtain gesture angle errors delta psik, the position information r'k output by the VIO and a group of position information rk output by the inertial navigation system are subtracted to obtain position errors delta rk, and the position errors and the gesture errors are adopted as observational quantities for filtering, wherein the corresponding observational quantities and observation matrixes are respectively shown as formula (27):
and step 6, continuing inertial navigation calculation by taking the current corrected state quantity as an initial quantity, and repeating the navigation correction process, wherein the method comprises the following specific steps of:
and (3) returning the current corrected state quantity to the step (3) as an initial quantity, performing next inertial navigation calculation, and repeating the navigation correction process until the navigation is finished.
In summary, the invention utilizes the characteristic of higher accuracy in a short period of an inertial measurement unit to drive a process model, improves positioning and navigation accuracy, utilizes the gait zero speed characteristic of a pedestrian as a constraint condition to construct an observed quantity, corrects inertial navigation accumulated errors on the basis of no increase of hardware, ensures higher navigation accuracy in a shorter period, utilizes pose information estimated by a visual inertial odometer as the constraint condition to construct the observed quantity, further corrects the accumulated errors of the inertial measurement unit, ensures higher navigation accuracy in a longer period, and improves the accuracy of indoor pedestrian navigation positioning by utilizing a visual inertial odometer and zero speed correction dual auxiliary technology.