Pedestrian inertial / visual / satellite integrated navigation method based on handheld terminal PDR

Abstract

The present invention discloses a pedestrian inertial / visual / satellite combined navigation method based on a handheld terminal PDR, comprising: obtaining inertial navigation pose information based on strapdown inertial navigation solution; constructing observation constraints for zero-speed motion for correction when the pedestrian is stationary, and updating the inertial navigation position information based on step length information when taking a new step; obtaining the pedestrian's visual pose information based on an inertial / visual fusion odometer, obtaining the pedestrian's satellite position information based on satellite positioning, performing coordinate system alignment and step length coefficient correction factor estimation; constructing a pedestrian multi-source factor graph optimization model with the handheld terminal PDR as the core, using inertial navigation pose information and step length information as local state constraints, using visual pose information and satellite position information as global state constraints, and constructing a target optimization function to obtain the pedestrian's final pose. The present invention is applied to the field of pedestrian navigation, has full-scene robust and continuous positioning capabilities, and can effectively achieve high-precision navigation positioning under long flight time.

Description

Technical Field

[0001] The present invention relates to the technical field of pedestrian navigation technology, and in particular to a pedestrian inertial / visual / satellite combined navigation method based on a handheld terminal PDR. Background Art

[0002] Pedestrian navigation technology, a key component of navigation research, aims to provide pedestrians with high-precision, highly reliable real-time positioning and navigation services, playing a vital role in people's daily lives and work. With the rapid development of urbanization, complex indoor and outdoor environments have placed higher demands on the accuracy and reliability of pedestrian navigation systems. This demand is particularly prominent in indoor scenarios where GNSS (Global Navigation Satellite System) is limited.

[0003] To address this problem, researchers have proposed numerous indoor positioning solutions. Typical indoor pedestrian positioning technologies fall into three main categories. The first is infrastructure-based indoor navigation solutions, such as Wi-Fi fingerprint matching, Bluetooth positioning, and ultra-wideband positioning. These methods require the upfront deployment and calibration of navigation infrastructure, resulting in high costs and limited navigation coverage. The second is pedestrian inertial navigation technology, including Zero Velocity Update (ZUPT) and Pedestrian Dead Reckoning (PDR) algorithms. While the ZUPT method offers high positioning accuracy, it requires inertial sensors to be mounted on pedestrians' feet, resulting in a poor user experience. It is primarily used in special operations such as firefighting and individual soldiers. The PDR algorithm, on the other hand, offers the advantage of wearability and can be used at various locations, such as the waist, shoulder, and chest. Furthermore, most portable mobile devices now have built-in micro-inertial sensors, making PDR algorithms widely used in pedestrian navigation. The third is integrated navigation technology. While pedestrian inertial navigation offers the advantages of full autonomy and plug-and-play functionality, the limited accuracy of micro-inertial sensors leads to cumulative errors over time. This often necessitates the use of multi-sensor fusion technology, leveraging the complementary nature of different navigation methods to build a high-performance, highly reliable navigation system. Currently, handheld devices (such as smartphones) have a rich set of built-in sensors, including accelerometers, gyroscopes, cameras, GNSS, and magnetometers, ensuring a robust source of positioning information for multi-sensor integrated pedestrian navigation.

[0004] In recent years, integrated navigation methods based on inertial / visual fusion, such as VINS-mono and ORB-SLAM3, have been widely used in pedestrian navigation due to their low cost and high accuracy. These inertial / visual integrated navigation methods leverage the complementary properties of inertial measurement units (IMUs) and visual sensors. While inertial navigation errors accumulate over time, they can effectively track rapid vehicle motion in a short period of time, ensuring short-term navigation accuracy. Furthermore, visual navigation offers high estimation accuracy in low-dynamic conditions, and the introduction of visual closed-loop correction significantly suppresses integrated navigation errors. The combination of these two methods allows for better estimation of navigation parameters. Both inertial and visual sensors require no external support, resulting in a fully autonomous navigation method. However, in scenes with severe visual degradation, such as darkness, weak textures, and occlusion, the system may fail to track visual features, making it impractical for full-scene pedestrian navigation applications. Furthermore, pedestrian inertial / visual integrated navigation is a local navigation method based on relative dead reckoning. Due to the lack of global observation information, navigation positioning errors accumulate over time during long-term navigation. In order to improve long-term navigation performance, some research works have integrated navigation technologies with global positioning information, such as satellite navigation and ultra-wideband (UWB), into inertial / visual navigation systems, and used intermittent global position observations to suppress error drift, thereby achieving high-precision positioning results under long flight time.

[0005] However, these current integrated navigation methods all rely on visual navigation solutions as their primary processing mechanism. This makes it difficult to provide continuous and reliable pedestrian navigation results in scenarios with severe visual degradation, such as sparse features, visual occlusion, and low light conditions. They also lack robust and continuous positioning capabilities across all scenarios. Furthermore, during the fusion of multi-source navigation information, current integrated navigation models fail to fully exploit the unique kinematic characteristics of pedestrians as observation constraints, such as zero speed and step length models, resulting in limited positioning performance. Summary of the Invention

[0006] In response to the above-mentioned deficiencies in the existing technology, the present invention provides a pedestrian inertial / visual / satellite combined navigation method based on a handheld terminal PDR, which has full-scene robust and continuous positioning capabilities and can effectively achieve high-precision navigation and positioning under long flight time.

[0007] To achieve the above object, the present invention provides a pedestrian inertial / visual / satellite combined navigation method based on a handheld terminal PDR, comprising the following steps:

[0008] Step 1: Perform strapdown inertial navigation solution based on the inertial data of the handheld terminal PDR to obtain the pedestrian's inertial navigation posture information, wherein the inertial navigation posture information includes inertial navigation heading information and inertial navigation position information;

[0009] Step 2: When the pedestrian is stationary, zero-speed motion observation constraint information is constructed to perform zero-speed correction to improve the estimation accuracy of the inertial navigation heading information, and when the pedestrian takes a new step, the inertial navigation position information is updated based on the step length information and the inertial navigation heading information;

[0010] Step 3: Obtain visual pose information of the pedestrian based on the inertial / visual fusion odometry, obtain satellite position information of the pedestrian based on satellite positioning, and perform coordinate system alignment and step coefficient correction factor estimation on the visual pose information, the satellite position information, and the inertial navigation pose information;

[0011] Step 4: Construct a pedestrian multi-source factor graph optimization model with the handheld terminal PDR as the core, use the inertial navigation pose information and the step size information as local state constraints, and use the visual pose information and the satellite position information as global state constraints, construct a target optimization function, and perform nonlinear factor graph optimization to obtain the final pose of the pedestrian.

[0012] Compared with the prior art, the present invention has the following beneficial technical effects:

[0013] 1. This invention adopts a processing mechanism with inertial navigation solution as the main process, fully leveraging the strong autonomy and high reliability of pedestrian inertial navigation. It combines the different characteristics of filtering and factor graph optimization fusion algorithms, and on this basis, explores the kinematic characteristics of pedestrian zero speed and step length, introducing them as dedicated motion constraints into the integrated navigation model.

[0014] 2. This invention integrates visual positioning and satellite global positioning information to achieve long-term, high-precision pedestrian navigation. It constructs a factor graph optimization model based on inertial PDR calculation, supports the adaptive fusion of heterogeneous multi-source information such as inertial, visual, and satellite information, and designs a mechanism for eliminating abnormal observations based on the residual chi-square test. This enables continuous and reliable positioning in complex environments such as those with degraded satellite signal quality and visual degradation, improving the environmental adaptability of the pedestrian autonomous navigation system.

[0015] 3. The present invention addresses the problem of adaptive pedestrian motion step length parameters, fully utilizes visual or satellite navigation observation information decoupled from pedestrian motion parameters, and realizes adaptive initialization correction of pedestrian step length coefficients, thereby effectively improving the human applicability of the pedestrian autonomous navigation system. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the structures shown in these drawings without paying any creative work.

[0017] Figure 1 Flowchart of a pedestrian inertial / visual / satellite combined navigation method based on a handheld terminal PDR in an embodiment of the present invention;

[0018] Figure 2 This is a flowchart of inertial PDR navigation with zero-speed state constraint in an embodiment of the present invention;

[0019] Figure 3 Schematic diagram of a pedestrian multi-source factor graph optimization model based on a handheld terminal PDR in an embodiment of the present invention.

[0020] The purpose, features and advantages of the present invention will be further described with reference to the accompanying drawings and in conjunction with the embodiments. DETAILED DESCRIPTION

[0021] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0022] In addition, the technical solutions between the various embodiments of the present invention can be combined with each other, but it must be based on the fact that ordinary technicians in this field can implement it. When the combination of technical solutions is mutually contradictory or cannot be implemented, it should be deemed that such a combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0023] like Figure 1 The present embodiment discloses a pedestrian inertial / visual / satellite combined navigation method based on a handheld terminal PDR, which mainly includes the following steps:

[0024] Step 1: Perform strapdown inertial navigation calculation based on the inertial data of the handheld terminal PDR to obtain the pedestrian's inertial navigation posture information, wherein the inertial navigation posture information includes inertial navigation heading information and inertial navigation position information;

[0025] Step 2: When the pedestrian is stationary, zero-speed motion observation constraint information is constructed to perform zero-speed correction to improve the estimation accuracy of the inertial navigation heading information. When the pedestrian takes a new step, the inertial navigation position information is updated based on the step length information and the inertial navigation heading information.

[0026] Step 3: Obtain the pedestrian's visual pose information based on the inertial / visual fusion odometry, obtain the pedestrian's satellite position information based on satellite positioning, and align the visual pose information, satellite position information, and inertial navigation pose information in the coordinate system to complete system initialization;

[0027] Step 4: Construct a pedestrian multi-source factor graph optimization model with the handheld terminal PDR as the core, use the inertial navigation pose information and step length information as local state constraints, and the visual pose information and satellite position information as global state constraints, construct the target optimization function, and perform nonlinear factor graph optimization to obtain the final pose of the pedestrian.

[0028] Since the zero bias and noise of the low-cost handheld terminal IMU are relatively large, this embodiment adopts a simplified strapdown inertial navigation algorithm that does not consider the influence of small error terms such as the earth's rotation and conic effect. The calculation formula is as follows:

[0029] ;

[0030] in, For the Time carrier coordinate system To the navigation coordinate system The direction cosine matrix, and Respectively Pedestrians are in the navigation coordinate system at all times The position and velocity of Navigation coordinate system The gravity vector under Represented from Lie algebra To Lie Group The exponential mapping of and Respectively The specific force and angular velocity vectors measured by the accelerometer and gyroscope at all times, and Respectively The zero bias of the accelerometer and gyroscope at all times, For the and The time interval between IMU data.

[0031] refer to Figure 2 To improve the navigation accuracy of the inertial PDR, this embodiment uses the information that pedestrians have zero velocity when they are stationary to correct the inertial navigation error. Therefore, the pedestrian's motion state must be determined first. Then, based on this motion state, the error of the strapdown inertial navigation system is analyzed. The error correction in the inertial navigation system is estimated by designing a Kalman filter based on the error model and the zero-velocity constraint.

[0032] Pedestrians walk in two phases: motion and stillness. During the stillness phase, pedestrians' speed is zero. Based on this fact, accurately detecting the stillness phase and using the zero-speed constraint as observation information in the navigation filter equation helps correct pedestrian velocity and posture errors, improving the navigation accuracy of the inertial PDR system. To this end, this embodiment uses a static hypothesis optimal detection method based on the generalized likelihood ratio to determine pedestrian stillness, specifically including:

[0033] First calculate the statistical results for:

[0034]

[0035] in, is the window size for static detection, and are the measurement noise variances of the accelerometer and gyroscope, respectively, It represents the average value of the accelerometer specific force within the static detection window. and are the raw measurement outputs of the accelerometer and gyroscope respectively;

[0036] Then judge Is it true? If so, it is determined that the pedestrian is in a stationary state. Otherwise, it is determined that the pedestrian has taken a new step. To set the threshold.

[0037] When constructing a Kalman filter based on an error model and zero-speed state constraints, a 15-dimensional error state vector is used, which is:

[0038]

[0039] in, 、 、 are misalignment angle error, velocity error and position error respectively, and Represent the gyroscope bias error and accelerometer bias error respectively;

[0040] The discretized system error model in Kalman filtering is:

[0041] ;

[0042] in, represents the estimated value of the error state vector, represents the predicted value of the error state vector, represents the state transition matrix, represents system noise and measurement noise;

[0043] When a pedestrian is detected to be stationary, a filtered observation with zero velocity is constructed, namely zero velocity correction, and the observation equation in the Kalman filter is:

[0044] ;

[0045] in, Strapdown Inertial Navigation The speed of pedestrians at all times, is the zero-speed observation in the static state, for The speed error at the moment, for The speed measurement noise at the moment.

[0046] Since the low-cost handheld terminal IMU has large sensor noise, the pedestrian position information estimated directly by the strapdown inertial navigation algorithm will diverge rapidly. Therefore, this embodiment uses a PDR algorithm based on a step length model to update the pedestrian position. During the pedestrian's walking process, the Z-axis data waveform of the handheld terminal accelerometer will show a periodicity of bimodal oscillation. According to this regularity, setting appropriate discrimination conditions can realize the detection of pedestrian stepping status. To this end, this embodiment adopts a peak detection method based on multi-threshold constraints to identify gait information. Due to the influence of sensor noise and the body shaking of pedestrians during walking, the original acceleration data often has noise interference. In order to improve the accuracy of gait detection, this embodiment uses an online real-time second-order low-pass filter to smooth the acceleration data, which is:

[0047] ;

[0048] in, 、 、 、 and is the filter coefficient of the second-order low-pass filter, is the acceleration amplitude after low-pass filtering, is the amplitude of the measured acceleration;

[0049] After low-pass filtering, the filtered acceleration data is subjected to peak detection to determine whether the acceleration peak represents a new step. The judgment conditions are:

[0050] ;

[0051] in, represents the peak acceleration, represents the time interval between adjacent peaks, Indicates that in the neighborhood window The non-maximum suppression function within and are the minimum and maximum acceleration thresholds, is the time interval threshold. The first condition in the above formula is used to determine the minimum and maximum acceleration amplitudes of the peak to avoid the influence of sensor noise and abnormal shaking of the individual soldier's body; the second condition is used to ensure the minimum time interval between adjacent gaits; and the third condition is used to filter out the influence of false peaks.

[0052] After determining that a new step has occurred, it is necessary to estimate the step length of this step to update the position of the individual soldier. For this purpose, this embodiment uses a nonlinear Weiberg model to estimate the step length, which is:

[0053] ;

[0054] in, is the step size coefficient, and Respectively represent the maximum and minimum values ​​of acceleration in this step;

[0055] Finally, the inertial navigation position information is updated according to the inertial navigation direction information, which is:

[0056] ;

[0057] in, and Respectively represent the north and east coordinates of the pedestrian in the navigation coordinate system, The direction cosine matrix is The converted heading information.

[0058] Although inertial PDR navigation has the advantages of autonomy and continuity, due to the interference of inertial noise and heading angle estimation errors, its positioning error accumulates over time, making it difficult to provide long-term high-precision positioning results. Therefore, other positioning methods are often needed to suppress error divergence. To address this issue, this embodiment uses an inertial / visual fusion odometry to provide effective auxiliary positioning information for the inertial PDR, thereby further improving positioning accuracy.

[0059] The inertial / visual fusion odometry VIO adopts an optimization-based combined navigation method. It constructs an optimization objective function by fusing IMU pre-integration and visual feature tracking information into a factor graph model, and performs nonlinear optimization to estimate the pose, velocity and IMU zero bias information of the current frame. The visual tracking part of the inertial / visual fusion odometry mainly extracts and matches the image sequence and uses the established feature matching relationship to complete the camera pose estimation. The IMU pre-integration part mainly calculates the relative pose transformation information between the two image frames by accumulating and integrating the IMU data between two adjacent image frames, which is used to assist inter-frame feature tracking, local pose constraints, and camera pose scale recovery. In order to achieve tight coupling of visual and inertial information, it is necessary to consider the IMU pre-integration and visual reprojection error constraint information at the same time, and jointly construct them into an objective function, and then perform nonlinear optimization to estimate the state variables. Therefore, the process of obtaining the visual pose information of the pedestrian based on the inertial / visual fusion odometry in this embodiment is specifically as follows:

[0060] First, based on the camera pose ,speed , gyroscope and accelerometer bias and The state variables to be optimized are:

[0061] ;

[0062] Combining the IMU pre-integration residual term and the visual reprojection residual term, the visual / inertial tightly coupled joint optimization estimation problem is transformed into an objective function:

[0063] ;

[0064] in, represents the IMU pre-integration residual term, represents the IMU bias residual term, represents the visual reprojection residual term, Indicates the The set of 3D map points observed in the image frame at the moment, represents the pre-integrated covariance matrix, represents the covariance matrix of the zero-biased random walk, is the reprojection covariance matrix associated with the key scale;

[0065] Solve the above objective function , the inertial / visual fusion odometry provides 6-DoF pose observation information, namely visual pose information, for the inertial PDR.

[0066] Since inertial PDR navigation uses the pedestrian's initial position and heading as the reference world coordinate system, and the inertial / visual fusion odometry uses the IMU coordinate system initialized in the first frame as the reference origin and the z-axis is aligned with the direction of gravity, the reference world coordinate systems of the visual pose information and the inertial navigation pose information are inconsistent, making it impossible to directly use the visual pose information for fusion optimization. In order to obtain the alignment transformation matrix between the two reference coordinate systems of the visual pose information and the inertial navigation pose information, this embodiment designs a least squares problem based on trajectory alignment, and its specific implementation process is as follows:

[0067] When a pedestrian takes a new step, the inertial navigation pose information consistent with the timestamp of this step is synchronized online and visual pose information , forming a synchronous data pair ;

[0068] When the number of synchronized data pairs obtained is greater than the set threshold Finally, construct a least squares problem to solve the alignment rotation matrix between the two world coordinate systems , translation vector , and the step size coefficient correction factor , the least squares problem is:

[0069] ;

[0070] in, is the PDR posture The translation part, is the VIO pose The translation part, is the number of data synchronization pairs;

[0071] Finally, use the solved alignment rotation matrix , translation vector The world coordinate system of the visual pose information can be aligned to the reference coordinate system of the inertial navigation pose information. Similarly, the satellite position information and the inertial navigation pose information can also be aligned.

[0072] It is worth noting that due to differences in height, gender, walking habits, etc., a fixed step length coefficient is used in inertial PDR navigation. It is difficult to apply to different users, resulting in large navigation errors. Therefore, this embodiment requires the use of an estimated step length coefficient correction factor , the step length coefficient for the inertial PDR step length model Perform online correction to improve the adaptability of inertial PDR navigation personnel. The correction process is as follows:

[0073] ;

[0074] in, is the modified step coefficient. In addition, During the correction process, it is preferred to use the satellite position information and inertial navigation posture information to align the coordinate system to estimate the step coefficient correction factor .

[0075] After the coordinate systems of the visual pose information, satellite position information, and inertial navigation pose information are aligned, whenever a new step of a pedestrian is detected and the visual pose information and satellite position information are obtained synchronously, a pedestrian multi-source factor graph optimization model with the handheld terminal PDR as the core can be constructed. The inertial navigation pose information and step length information are used as local state constraints, and the visual pose information and satellite position information are used as global state constraints. The target optimization function is constructed and nonlinear factor graph optimization is performed to obtain the final pose of the pedestrian. The specific implementation process is as follows:

[0076] In the pedestrian multi-source factor graph optimization model, the state variable of each node is defined as ,in , ;

[0077] Since the positioning results of PDR navigation are stable and relatively accurate in a short time, its pose estimation results can be used as constraints for pose changes between adjacent states to ensure pose consistency within a local range, thereby considering two consecutive gaits. and Calculate the residual of the inertia factor (ie, PDR factor) as:

[0078]

[0079] in, represents the residual of the inertia factor, Indicates the inertial PDR solution from the Step to The relative posture change and position increment of each step, represents subtraction in a general sense, Represents the Lie group To Lie algebra To the logarithmic mapping;

[0080] When the inertial / visual fusion odometry estimates the visual pose information of a frame of image and aligns it with the time of the current step of PDR, its pose After being transformed into the PDR reference coordinate system, it is added to the factor graph model to provide position and attitude constraints, thereby correcting the accumulated positioning error and heading drift of the PDR. Therefore, the residual of the visual factor (i.e., VIO factor) is:

[0081] ;

[0082] in, represents the residual of the visual factor, Indicates the The visual pose information estimated by the inertial / visual fusion odometry in the PDR reference coordinate system, including attitude and location ;

[0083] Since GNSS navigation results are easily affected by factors such as building obstruction and signal interference, which may cause GNSS trajectory jitter or jump. In order to avoid the influence of abnormal GNSS observations on system state estimation, before adding GNSS observation constraints to the factor graph model, the horizontal dilution of precision (HDOP) and the number of satellites are first used to calculate the GNSS trajectory. Two indicators are used to screen GNSS gross errors. GNSS observations are considered reliable if they meet the following thresholds:

[0084] .

[0085] Since GNSS navigation can only provide two-dimensional plane positioning results, the residual of the satellite factor (i.e., GNSS factor) is:

[0086] ;

[0087] in, represents the residual of the satellite factor, Indicates the Step 1 is the horizontal positioning result of GNSS in the PDR reference coordinate system. represents the horizontal position observation function;

[0088] refer to Figure 3 , define the set of optimized state variables of the pedestrian multi-source factor graph optimization model Contains the starting point and the steps from the first step to the All gait nodes between steps are:

[0089] .

[0090] After constructing the pedestrian multi-source factor graph optimization model, the final pose of the pedestrian is jointly optimized by minimizing the following objective function:

[0091] ;

[0092] in, represents the set of all inertia factors, represents the set of all visual factors, represents the set of all satellite factors; represents a robust kernel function, which is used to reduce the impact of abnormal observations; The covariance matrix of the inertia factor is represented by, represents the covariance matrix of the visual factors, represents the covariance matrix of satellite factors;

[0093] Finally, the Levenberg-Marquardt method (LM) is used to solve the above nonlinear optimization problem to obtain the final pose of the pedestrian.

[0094] In complex navigation scenarios such as sparse textures and indoor-outdoor switching, some abnormal visual factors and satellite factors may be added to the factor graph model. Although the robust kernel function can reduce the influence of the error constraint edge, it will still cause the accuracy of the state estimation value to decrease. Therefore, to address this problem, this embodiment designs an outlier detection method based on the error term chi-square statistic after the initial nonlinear factor graph optimization, thereby further improving the stability and robustness of the system state estimation. The chi-square value of each error term is Essentially, it is the square of its Mahalanobis distance, which can help measure the correctness of the constraint edge. Its calculation formula is as follows:

[0095] ;

[0096] in, It is The residuals of the constraint factors, is the information matrix, i.e. the inverse of the covariance matrix, which is used to weight and normalize the errors.

[0097] Assume that each error edge follows a Gaussian distribution with zero mean , then its chi-square value follows the chi-square distribution, and the degree of freedom is the dimension of the error vector. In the factor graph optimization model of this embodiment, the visual factor residual corresponds to 6 degrees of freedom, and the satellite factor residual corresponds to 2 degrees of freedom. When the chi-square value of a single constraint edge When the threshold is exceeded, it indicates that it is an incorrect constraint or an outlier, and outliers need to be removed to improve the stability and accuracy of the state estimation. This embodiment sets a chi-square test threshold at a 5% significance level to determine whether the chi-square statistic of each constraint edge deviates from the expected value. If it exceeds the corresponding threshold, it is marked as an outlier and removed from the factor graph model and does not participate in the factor graph optimization. The chi-square test thresholds for the visual factor and satellite factor are set as follows:

[0098] ;

[0099] in, and They represent the chi-squared statistics of the visual factor and satellite factor after the initial factor graph optimization. After removing abnormal observation constraints through chi-square detection, the global pose optimization is re-executed to jointly optimize the pose estimate by minimizing the objective function.

[0100] The above description is only a preferred embodiment of the present invention and does not limit the scope of protection of the present invention. All equivalent structural transformations made by using the contents of the present invention description and drawings under the inventive concept of the present invention, or direct / indirect application in other related technical fields are included in the scope of protection of the present invention.

Claims

1. A pedestrian inertial / visual / satellite combined navigation method based on a handheld terminal PDR, characterized in that: The steps include: Step 1: Perform strapdown inertial navigation solution based on the inertial data of the handheld terminal PDR to obtain the pedestrian's inertial navigation posture information, wherein the inertial navigation posture information includes inertial navigation heading information and inertial navigation position information; Step 2: When the pedestrian is stationary, zero-speed motion observation constraint information is constructed to perform zero-speed correction to improve the estimation accuracy of the inertial navigation heading information, and when the pedestrian takes a new step, the inertial navigation position information is updated based on the step length information and the inertial navigation heading information; Step 3: Obtain visual pose information of the pedestrian based on the inertial / visual fusion odometry, obtain satellite position information of the pedestrian based on satellite positioning, and perform coordinate system alignment and step coefficient correction factor estimation on the visual pose information, the satellite position information, and the inertial navigation pose information; Step 4: Construct a pedestrian multi-source factor graph optimization model with the handheld terminal PDR as the core, use the inertial navigation pose information and the step size information as local state constraints, and use the visual pose information and the satellite position information as global state constraints, construct a target optimization function, and perform nonlinear factor graph optimization to obtain the final pose of the pedestrian.

2. The pedestrian inertial / visual / satellite combined navigation method based on a handheld terminal PDR according to claim 1 is characterized in that: In step 1, the strapdown inertial navigation solution based on the inertial data of the handheld terminal PDR is specifically as follows: in, For the Time carrier coordinate system To the navigation coordinate system The direction cosine matrix, and Respectively Pedestrians are in the navigation coordinate system at all times The position and velocity of Navigation coordinate system The gravity vector under Represented from Lie algebra To Lie Group The exponential mapping of and Respectively The specific force and angular velocity vectors measured by the accelerometer and gyroscope at all times, and Respectively The zero bias of the accelerometer and gyroscope at all times, For the and The time interval between IMU data.

3. The pedestrian inertial / visual / satellite combined navigation method based on a handheld terminal PDR according to claim 1 is characterized in that: In step 2, the static hypothesis optimal detection method based on generalized likelihood ratio is used to perform pedestrian static judgment, specifically: Calculate statistical results ,for: in, is the window size for static detection, and are the measurement noise variances of the accelerometer and gyroscope, respectively, It represents the average value of the accelerometer specific force within the static detection window. and are the raw measurement outputs of the accelerometer and gyroscope, respectively; judge Is it true? If so, it is determined that the pedestrian is in a stationary state. Otherwise, it is determined that the pedestrian has taken a new step. To set the threshold.

4. The pedestrian inertial / visual / satellite combined navigation method based on a handheld terminal PDR according to claim 1 is characterized in that: In step 2, the construction of observation constraint information of zero-speed motion for zero-speed correction is specifically as follows: Design a Kalman filter based on the error model to estimate the error correction in the inertial navigation solution; In the Kalman filter, the error state vector is: in, 、 、 are misalignment angle error, velocity error and position error respectively, and Represent the gyroscope bias error and accelerometer bias error respectively; In the Kalman filter, the discretized system error model is: in, represents the estimated value of the error state vector, represents the predicted value of the error state vector, represents the state transition matrix, represents system noise and measurement noise; In the Kalman filter, the observation equation is: in, Strapdown Inertial Navigation The speed of pedestrians at all times, is the zero-speed observation in the static state, for The speed error at the moment, for The speed measurement noise at the moment.

5. The pedestrian inertial / visual / satellite integrated navigation method based on a handheld terminal PDR according to claim 1 is characterized in that: In step 2, the process of updating the inertial navigation position information based on the step length information and the inertial navigation heading information when the pedestrian takes a new step is as follows: The acceleration data is smoothed based on the second-order low-pass filter, which is: in, 、 、 、 and is the filter coefficient of the second-order low-pass filter, is the acceleration amplitude after low-pass filtering, is the amplitude of the measured acceleration; Perform peak detection on the filtered acceleration data to determine whether the acceleration peak is a new step. Then estimate the step length of the new step after it occurs, which is: in, is the step size coefficient, and Respectively represent the maximum and minimum values ​​of acceleration in this step; Finally, the inertial navigation position information is updated according to the inertial navigation heading information, as follows: in, and Respectively represent the north and east coordinates of the pedestrian in the navigation coordinate system, The direction cosine matrix is The converted heading information.

6. The pedestrian inertial / visual / satellite combined navigation method based on a handheld terminal PDR according to claim 5 is characterized in that: The conditions for judging whether the acceleration peak is a new step are: in, represents the peak acceleration, represents the time interval between adjacent peaks, Indicates that in the neighborhood window The non-maximum suppression function within and are the minimum and maximum acceleration thresholds, is the time interval threshold.

7. The pedestrian inertial / visual / satellite integrated navigation method based on a handheld terminal PDR according to any one of claims 1 to 6, characterized in that: In step 3, the visual pose information of the pedestrian is obtained based on the inertial / visual fusion odometry as follows: Based on camera pose ,speed , gyroscope and accelerometer bias and The state variables to be optimized are: Combining the IMU pre-integration residual term and the visual reprojection residual term, the visual / inertial tightly coupled joint optimization estimation problem is transformed into an objective function: in, represents the IMU pre-integration residual term, represents the IMU bias residual term, represents the visual reprojection residual term, Indicates the The set of 3D map points observed in the image frame at the moment, represents the pre-integrated covariance matrix, represents the covariance matrix of the zero-biased random walk, is the reprojection covariance matrix associated with the key scale; The objective function is solved to obtain 6-DoF pose observation information provided by the inertial / visual fusion odometry for the inertial PDR, namely the visual pose information.

8. The pedestrian inertial / visual / satellite integrated navigation method based on a handheld terminal PDR according to any one of claims 1 to 6, characterized in that: In step 3, the process of aligning the visual pose information with the inertial navigation pose information is as follows: When a pedestrian takes a new step, the inertial navigation pose information consistent with the timestamp of this step is synchronized online and visual pose information , forming a synchronous data pair ; When the number of synchronized data pairs obtained is greater than the set threshold Finally, construct a least squares problem to solve the alignment rotation matrix between the two world coordinate systems , translation vector , and the step size coefficient correction factor , the least squares problem is: in, is the PDR posture The translation part, is the VIO pose The translation part, is the number of data synchronization pairs; Then based on the step size coefficient correction factor Step size coefficient Perform online corrections as follows: in, is the corrected step size coefficient.

9. The pedestrian inertial / visual / satellite integrated navigation method based on a handheld terminal PDR according to any one of claims 1 to 6, characterized in that: Step 4 specifically includes: In the pedestrian multi-source factor graph optimization model, the state variable of each node is defined as ,in , ; Calculate the residual of the inertia factor as: in, represents the residual of the inertia factor, Indicates the inertial PDR solution from the Step to The relative posture change and position increment of each step, represents subtraction in a general sense, Represents the Lie group To Lie algebra To the logarithmic mapping; Calculate the residual of the visual factor as: in, represents the residual of the visual factor, Indicates the The visual pose information estimated by the inertial / visual fusion odometry in the PDR reference coordinate system, including attitude and location ; Calculate the residual of the satellite factor as: in, represents the residual of the satellite factor, Indicates the Step 1 is the horizontal positioning result of GNSS in the PDR reference coordinate system. represents the horizontal position observation function; Define the optimized state variable set of the pedestrian multi-source factor graph optimization model Contains the starting point and the steps from the first step to the All gait nodes between steps are: After constructing the pedestrian multi-source factor graph optimization model, the final pose of the pedestrian is jointly optimized by minimizing the following objective function: in, represents the set of all inertia factors, represents the set of all visual factors, represents the set of all satellite factors; represents a robust kernel function, which is used to reduce the impact of abnormal observations; The covariance matrix of the inertia factor is represented by, represents the covariance matrix of the visual factors, Represents the covariance matrix of the satellite factors.

10. The pedestrian inertial / visual / satellite integrated navigation method based on a handheld terminal PDR according to claim 9, characterized in that: In the process of joint optimization of the pedestrian's final pose, when there is a single constraint edge chi-square value for the visual factor or satellite factor When the threshold is exceeded, the corresponding visual factor or satellite factor will be eliminated.

Images