CN112985392A - Pedestrian inertial navigation method and device based on graph optimization framework - Google Patents
Pedestrian inertial navigation method and device based on graph optimization framework Download PDFInfo
- Publication number
- CN112985392A CN112985392A CN202110416612.9A CN202110416612A CN112985392A CN 112985392 A CN112985392 A CN 112985392A CN 202110416612 A CN202110416612 A CN 202110416612A CN 112985392 A CN112985392 A CN 112985392A
- Authority
- CN
- China
- Prior art keywords
- navigation
- data
- inertial navigation
- global
- mimu
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/15—Correlation function computation including computation of convolution operations
Abstract
The application relates to a pedestrian inertial navigation method and device based on a graph optimization framework. The method comprises the following steps: carrying out data processing on the acquired MIMU navigation data and the auxiliary navigation data to obtain position information corresponding to the MIMU navigation data in the variable node position and a global factor corresponding to the auxiliary navigation data; navigation resolving is carried out through Kalman filtering according to the position information, zero-speed detection is carried out according to gait characteristics of MIMU navigation data, and virtual observed quantity of Kalman filtering is obtained; correcting errors of navigation calculation of Kalman filtering according to the virtual observed quantity to obtain an inertial navigation calculation result; constructing a factor graph by taking motion constraint and scene constraint as factors according to the global factors; performing global pose optimization according to the factor graph and the inertial navigation resolving result to obtain a global optimization result; and optimizing inertial navigation parameters according to the global optimization result and the inertial navigation resolving result. By adopting the method, inertial navigation parameter optimization can be carried out.
Description
Technical Field
The application relates to the technical field of pedestrian navigation, in particular to a pedestrian inertial navigation method and device based on a graph optimization framework.
Background
The pedestrian navigation refers to a technology for navigating and positioning pedestrians. In satellite rejection environments such as cities, indoor environments and underground environments, pedestrian navigation becomes an active branch in the field of navigation and positioning, and with the development of technologies, various sensors such as Wi-Fi fingerprints, ultra wide bands, inertia, vision, radio frequency identification and Bluetooth are used for navigation and positioning of pedestrians. Compared with other navigation and positioning means, the pedestrian navigation technology based on the MEMS inertial device can complete the navigation and positioning of pedestrians only by sensors such as a gyroscope, an accelerometer and a magnetometer carried by the pedestrian navigation technology without arranging a base station in advance, can realize the autonomy and the continuity of the navigation and positioning, and can well meet the application requirements of a pedestrian navigation system in the aspects of cost, size, weight, power consumption and the like. At present, the common pedestrian autonomous positioning technology is based on an inertial sensor and integrates other information sources such as Wi-Fi fingerprints, ultra wide bands, inertia, vision, radio frequency identification and Bluetooth for positioning.
A pedestrian strapdown inertial navigation solution is a typical pedestrian inertial navigation algorithm, and on the basis of a traditional SINS algorithm, the navigation error is periodically corrected by combining the special gait characteristics of foot movement and utilizing a zero-speed correction method. Zero velocity correction plays a crucial role in inertial navigation systems, and is an inexpensive and effective means for error control. In each gait cycle of walking of the pedestrian, when the foot is in contact with the ground, the sole and the ground are kept relatively static, the speed of the sensor to the ground is zero, and the step information can be fully utilized by a zero-speed correction method to estimate and correct navigation errors. Therefore, accurate zero-speed interval judgment is crucial to improving the adaptability and robustness of the strapdown inertial navigation resolving algorithm. The existing zero-speed detection method based on the inertia technology has the problem that the detection result is easily influenced by the fluctuation of the measured value and the setting of the detection parameter. On one hand, no matter the detection is carried out by adopting a single detection threshold value or adding a fixed time threshold value, the zero-speed interval cannot be accurately detected; on the other hand, the detection parameter setting has the defects of manual modification and independent adjustment, the subjectivity is strong, once the parameter setting is completed, the parameter setting can be kept constant in the whole navigation process, and the flexibility is poor.
In order to realize accurate zero-speed interval detection, in recent years, domestic and foreign research on the algorithm mainly focuses on three aspects: one is threshold adjustment based on velocity or motion pattern classification, and the detector selects an optimized threshold for that particular velocity or motion level based on inertial data. M, M ä kel ä designed a zero velocity probe that uses a Hidden Markov Model (HMM) to represent the different phases of the gait cycle; r and Zhang add chest accelerometer on the basis of foot-carried inertial sensor, synthesize the information of the two and upgrade the corresponding threshold value of zero-speed detection; B. the method comprises the steps that a Wagstaff training SVM classification model classifies motion types, and then a threshold value is updated; K. pan allows the threshold to vary with time of the accelerometer measurements. Secondly, without the help of a threshold value, the neural network is trained to directly judge whether the speed is zero or not. J. The Wahlstrom uses LSTM network to replace traditional zero-speed detection algorithm, and provides a new zero-speed correction algorithm. And thirdly, starting from analyzing the data characteristics, performing zero-speed detection by adopting other methods. S.k. Park proposes a method for zero velocity detection and correction based on the peak value and the lowest acceleration value between the two peak values without any threshold evaluation. J, Zhang proposes a Bayes self-adaptive threshold detection model based on a still ratio test, and performs zero-speed detection by quantizing the prior probability of zero-speed detection and the cost of error detection. And a pedestrian navigation zero-speed detection threshold value self-adaptive algorithm based on optimal interval estimation is proposed.
And (3) expressing all observation information as a nonlinear error energy function by multi-sensor fusion navigation based on a factor graph, and solving a minimum value of the function by an optimization method to obtain state estimation. Different from the filtering process which recursively estimates the current state distribution, the optimization-based method estimates the state variables in batches, and the precision and the robustness are higher. However, the method is only based on inertial navigation, optimizes the pedestrian pose by fusing other sensors, and does not optimize and update the parameters of the inertial navigation of the pedestrian.
Disclosure of Invention
In view of the above, it is necessary to provide a pedestrian inertial navigation method and apparatus based on a graph optimization framework, which can perform parameter optimization on inertial navigation.
A pedestrian inertial navigation method based on a graph optimization framework, the method comprising:
performing data processing on the acquired MIMU navigation data and the auxiliary navigation data, determining variable nodes, and acquiring position information corresponding to the MIMU navigation data in the positions of the variable nodes and global factors corresponding to the auxiliary navigation data;
navigation resolving is carried out through Kalman filtering according to the position information, zero-speed detection is carried out according to gait characteristics of the MIMU navigation data, and virtual observed quantity of the Kalman filtering is obtained;
correcting errors of navigation calculation of Kalman filtering according to the virtual observed quantity to obtain an inertial navigation calculation result;
constructing a factor graph by taking motion constraint and scene constraint as factors according to the global factors;
performing global pose optimization according to the factor graph and the inertial navigation resolving result to obtain a global optimization result;
and optimizing inertial navigation parameters according to the global optimization result and the inertial navigation resolving result.
In one embodiment, the method further comprises the following steps: performing data processing on the acquired MIMU navigation data and the auxiliary navigation data based on the acceleration data to obtain position information and attitude updating frequency;
aligning the MIMU navigation data and the auxiliary navigation data according to the timestamp information of the MIMU navigation data and the auxiliary navigation data;
and determining variable nodes according to the position information and the attitude updating frequency, and determining a global factor corresponding to the auxiliary navigation data of each variable node.
In one embodiment, the location information includes: position, velocity, and quaternion direction; further comprising: determining the state of a filter of Kalman filtering according to the position, the speed and the direction of the quaternion;
and performing zero-speed detection according to the gait characteristics of the MIMU navigation data to obtain the virtual observed quantity of Kalman filtering.
In one embodiment, the method further comprises the following steps: adjusting inertial navigation parameters according to gait characteristics of the MIMU navigation data; and carrying out zero-speed detection according to the inertial navigation parameters to obtain the virtual observed quantity of Kalman filtering.
In one embodiment, the method further comprises the following steps: and optimizing the inertial navigation parameters according to the global optimization result and the inertial navigation resolving result.
In one embodiment, the method further comprises the following steps: the auxiliary navigation data comprises: magnetometer data, GPS data, WiFi fingerprint data, and bluetooth data.
A pedestrian inertial navigation device based on a graph optimization framework, the device comprising:
the preprocessing module is used for carrying out data processing on the acquired MIMU navigation data and the auxiliary navigation data, determining variable nodes and obtaining position information corresponding to the MIMU navigation data in the positions of the variable nodes and global factors corresponding to the auxiliary navigation data;
the inertial navigation resolving module is used for performing navigation resolving through Kalman filtering according to the position information and performing zero-speed detection according to gait characteristics of the MIMU navigation data to obtain virtual observed quantity of the Kalman filtering; correcting errors of navigation calculation of Kalman filtering according to the virtual observed quantity to obtain an inertial navigation calculation result;
the global optimization module is used for constructing a factor graph by taking motion constraint and scene constraint as factors according to the global factors; performing global pose optimization according to the factor graph and the inertial navigation resolving result to obtain a global optimization result;
and the inertial navigation optimization module is used for optimizing inertial navigation parameters according to the global optimization result and the inertial navigation resolving result.
According to the pedestrian inertial navigation method and device based on the graph optimization framework, firstly, zero-speed detection is carried out through gait features of MIMU navigation data to obtain virtual observed quantity of Kalman filtering, error correction is carried out on navigation calculation of the Kalman filtering according to the virtual observed quantity to obtain an inertial calculation result, then a factor graph is constructed according to global factors by taking motion constraint and scene constraint as factors, global pose optimization is carried out by utilizing the factor graph and the inertial calculation result to obtain a global optimization result, and inertial parameter optimization is carried out according to the global optimization result and the inertial calculation result. By the mode, the inertial navigation parameters are optimized by combining the result of global optimization.
Drawings
FIG. 1 is a flow chart of a pedestrian inertial navigation method based on a graph optimization framework in one embodiment;
FIG. 2 is a flow diagram illustrating a process of Kalman filtering in one embodiment;
FIG. 3 is a block flow diagram of pedestrian inertial navigation based on a graph optimization framework in one embodiment;
FIG. 4 is a diagram of a factor graph for global pose optimization in one embodiment;
FIG. 5 is a schematic structural diagram of a pedestrian inertial navigation device based on a graph optimization framework in one embodiment.
Detailed Description
In order to make the objects, technical solutions and advantages of the present application more apparent, the present application is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the present application and are not intended to limit the present application.
In one embodiment, as shown in fig. 1, there is provided a pedestrian inertial navigation method based on a graph optimization framework, comprising the following steps:
and 102, performing data processing on the acquired MIMU navigation data and the auxiliary navigation data, determining variable nodes, and obtaining position information corresponding to the MIMU navigation data in the positions of the variable nodes and global factors corresponding to the auxiliary navigation data.
And 104, performing navigation calculation through Kalman filtering according to the position information, and performing zero-speed detection according to gait characteristics of the MIMU navigation data to obtain virtual observed quantity of the Kalman filtering.
And 106, correcting errors of navigation calculation of Kalman filtering according to the virtual observed quantity to obtain an inertial navigation calculation result.
And step 108, constructing a factor graph by taking the motion constraint and the scene constraint as factors according to the global factors.
And 110, carrying out global pose optimization according to the factor graph and the inertial navigation resolving result to obtain a global optimization result.
And 112, optimizing inertial navigation parameters according to the global optimization result and the inertial navigation resolving result.
According to the pedestrian inertial navigation method based on the graph optimization framework, firstly, zero-speed detection is carried out through gait features of MIMU navigation data to obtain virtual observed quantity of Kalman filtering, error correction is carried out on navigation calculation of the Kalman filtering according to the virtual observed quantity to obtain inertial navigation calculation results, then a factor graph is constructed according to global factors by taking motion constraint and scene constraint as factors, global pose optimization is carried out by utilizing the factor graph and the inertial navigation calculation results to obtain global optimization results, and inertial navigation parameter optimization is carried out according to the global optimization results and the inertial navigation calculation results. By the mode, the inertial navigation parameters are optimized by combining the result of global optimization.
In one embodiment, the acquired MIMU navigation data and the auxiliary navigation data are subjected to data processing based on acceleration data to obtain position information and attitude updating frequency; aligning the MIMU navigation data and the auxiliary navigation data according to the time stamp information of the MIMU navigation data and the auxiliary navigation data; and determining variable nodes according to the position information and the attitude updating frequency, and determining a global factor corresponding to the auxiliary navigation data of each variable node.
In this embodiment, the number and quality of the navigation information sources are analyzed, and gait segmentation and time alignment are completed based on the inertial data. And carrying out gait segmentation of pedestrian movement based on the acceleration data, and determining the update frequency of the positions and postures of the pedestrians. On this basis, the multi-sensor data is aligned with the gait of the pedestrian according to the time stamp of the information source.
Specifically, in different motion modes, the acceleration signals show different variation trends due to different degrees and characteristics of motion. Will be provided withkIntegrating the three-axis acceleration signals of the moment to obtain the acceleration vector sum of the k moment
。
In the formula (I), the compound is shown in the specification,
is composed ofkAt a time of dayiAcceleration data in the axial direction.
To the sum of acceleration vectors
Obtaining acceleration filter value by low-pass filtering through moving average
。
The negative peak of the acceleration filter value is just off the ground. One step is between two negative peaks.
Selection of data based on quantity and quality of information sourcesAnd time synchronization and alignment are completed. Taking visual and inertial alignment as an example, the state of the pedestrian at the k-th time
By location of IMU
Speed of
And direction in quaternion form
Consists of the following components:
in the formula
Representing the pose and position of the pedestrian's motion. During pedestrian movement, the foot periodically contacts and leaves the ground. The attitude is updated by estimating the frequency of gait using the outputs of the gyroscope and accelerometer, taking into account the characteristics of the pedestrian's motion. Therefore, synchronization is corrected by time aligning the visual key frames with the IMU measurements. The keyframes closest to the current IMU measurement are considered to have the same pose.
In the formula
Representing the time difference between the left/right keyframe and the landing time.
Representing the pose of the solution of the visual odometer.
It should be noted that, in the present embodiment, only the visual navigation is taken as an example for description, and the alignment performed by using other navigation data may also be calculated in the above manner.
In one embodiment, the location information includes: determining the filter state of Kalman filtering according to the position, the speed and the quaternion direction; and performing zero-speed detection according to the gait characteristics of the MIMU navigation data to obtain the virtual observed quantity of Kalman filtering.
In the embodiment, on the basis of a traditional strapdown inertial navigation resolving algorithm, the detection of the zero-speed state is performed by combining the special gait characteristics of the foot motion, the zero-speed state is used as the virtual observation of Kalman filtering, and the error compensation is performed on the inertial navigation resolving result in a recursion manner.
The specific Kalman filtering process is shown in FIG. 2, a data resolving process and a gait feature detection process are arranged in a dotted line frame, zero-speed Kalman filtering correction is carried out according to a course angle and gait features, and then a Kalman filtering result is applied to data resolved by the traditional strapdown inertial navigation system to carry out error correction.
In one embodiment, the inertial navigation parameters are adjusted according to gait characteristics of the MIMU navigation data; and carrying out zero-speed detection according to the inertial navigation parameters to obtain the virtual observed quantity of Kalman filtering.
In one embodiment, as shown in fig. 3, a flow chart of pedestrian inertial navigation based on a graph optimization framework is provided, and inertial navigation parameters are optimized according to a global optimization result and an inertial navigation solution result. In fig. 3, the data preprocessing stage includes auxiliary information preprocessing and MIMU navigation data preprocessing, then the MIMU navigation data is used for zero-speed kalman filtering to correct errors, so as to obtain an inertial navigation solution result, the auxiliary information is extracted to obtain a global factor, then the global factor and the inertial navigation solution result are used for global pose optimization, and the global optimization result and the inertial navigation solution result are combined to perform inertial navigation solution parameter optimization, so that the parameter optimization result is used for parameter adjustment, thereby optimizing the inertial navigation parameters.
Specifically, the motion process of the pedestrian is segmented, variable nodes are determined, a factor graph is constructed by taking motion constraint and scene constraint as factors, an error energy function is established, and then the minimum value of the error energy function is solved through an optimization method, so that the optimal estimation under the least square meaning of the pose is obtained.
The factor graph structure of global pose optimization is shown in fig. 4. The factor graph nodes are the poses of six degrees of freedom (including the position and direction relative to the initial point) of the pedestrian, and the node frequency is determined by the gait update frequency. Pose constraints between adjacent nodes are provided by the MIMU. The visual information can be used as pose constraint information between adjacent nodes, and global constraint can be provided through image feature extraction. The magnetometer can provide course information and constrain the course under certain conditions. The WIFI fingerprint and GPS factor also provide global constraints.
Finally, the specific steps in parameter optimization are as follows:
the estimation precision of the pose after optimization is higher than the pose of the pedestrian solved by inertial navigation, so that the pose result after optimization is taken as the observed quantity, and the error of observation is
In the formula, z is pose estimation after optimization, and h is an observation equation:
x is pose estimation of the previous state, u is inertial measurement input, and p is an inertial navigation resolving parameter. The observed quantities at other moments are also taken into account, and each error is given an index, so that the overall cost function is
Solving this least squares allows optimization of the parameters of the solution.
In one embodiment, the auxiliary navigation data includes: magnetometer data, GPS data, WiFi fingerprint data, and bluetooth data.
It should be understood that, although the steps in the flowchart of fig. 1 are shown in order as indicated by the arrows, the steps are not necessarily performed in order as indicated by the arrows. The steps are not performed in the exact order shown and described, and may be performed in other orders, unless explicitly stated otherwise. Moreover, at least a portion of the steps in fig. 1 may include multiple sub-steps or multiple stages that are not necessarily performed at the same time, but may be performed at different times, and the order of performance of the sub-steps or stages is not necessarily sequential, but may be performed in turn or alternately with other steps or at least a portion of the sub-steps or stages of other steps.
In one embodiment, as shown in fig. 5, there is provided a pedestrian inertial navigation device based on a graph optimization framework, comprising: a preprocessing module 502, an inertial navigation solution module 504, a global optimization module 506, and an inertial navigation optimization module 508, wherein:
the preprocessing module 502 is configured to perform data processing on the obtained MIMU navigation data and the auxiliary navigation data, determine a variable node, and obtain position information corresponding to the MIMU navigation data in a position of the variable node and a global factor corresponding to the auxiliary navigation data;
the inertial navigation resolving module 504 is used for performing navigation resolving through Kalman filtering according to the position information and performing zero-speed detection according to gait characteristics of the MIMU navigation data to obtain virtual observed quantity of the Kalman filtering; correcting errors of navigation calculation of Kalman filtering according to the virtual observed quantity to obtain an inertial navigation calculation result;
a global optimization module 506, configured to construct a factor graph with motion constraints and scene constraints as factors according to the global factor; performing global pose optimization according to the factor graph and the inertial navigation resolving result to obtain a global optimization result;
and the inertial navigation optimization module 508 is configured to perform inertial navigation parameter optimization according to the global optimization result and the inertial navigation solution result.
In one embodiment, the preprocessing module 502 is further configured to perform data processing on the acquired MIMU navigation data and the auxiliary navigation data based on the acceleration data to obtain position information and an attitude update frequency; aligning the MIMU navigation data and the auxiliary navigation data according to the timestamp information of the MIMU navigation data and the auxiliary navigation data; and determining variable nodes according to the position information and the attitude updating frequency, and determining a global factor corresponding to the auxiliary navigation data of each variable node.
In one embodiment, the location information includes: position, velocity, and quaternion direction; the inertial navigation solution module 504 is further configured to determine a filter state of kalman filtering according to the position, the velocity, and the direction of the quaternion; and performing zero-speed detection according to the gait characteristics of the MIMU navigation data to obtain the virtual observed quantity of Kalman filtering.
In one embodiment, the inertial navigation solution module 504 is further configured to adjust inertial navigation parameters according to gait characteristics of the MIMU navigation data; and carrying out zero-speed detection according to the inertial navigation parameters to obtain the virtual observed quantity of Kalman filtering.
In one embodiment, the inertial navigation optimization module 508 is further configured to optimize the inertial navigation parameters according to the global optimization result and the inertial navigation solution result.
In one embodiment, the auxiliary navigation data includes: magnetometer data, GPS data, WiFi fingerprint data, and bluetooth data.
For specific definition of the pedestrian inertial navigation device based on the graph optimization framework, reference may be made to the above definition of the pedestrian inertial navigation method based on the graph optimization framework, and details are not repeated here. The modules in the pedestrian inertial navigation device based on the graph optimization framework can be wholly or partially realized by software, hardware and a combination thereof. The modules can be embedded in a hardware form or independent from a processor in the computer device, and can also be stored in a memory in the computer device in a software form, so that the processor can call and execute operations corresponding to the modules.
It will be understood by those skilled in the art that all or part of the processes of the methods of the embodiments described above can be implemented by hardware instructions of a computer program, which can be stored in a non-volatile computer-readable storage medium, and when executed, can include the processes of the embodiments of the methods described above. Any reference to memory, storage, database, or other medium used in the embodiments provided herein may include non-volatile and/or volatile memory, among others. Non-volatile memory can include read-only memory (ROM), Programmable ROM (PROM), Electrically Programmable ROM (EPROM), Electrically Erasable Programmable ROM (EEPROM), or flash memory. Volatile memory can include Random Access Memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms such as Static RAM (SRAM), Dynamic RAM (DRAM), Synchronous DRAM (SDRAM), Double Data Rate SDRAM (DDRSDRAM), Enhanced SDRAM (ESDRAM), Synchronous Link DRAM (SLDRAM), Rambus Direct RAM (RDRAM), direct bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).
The technical features of the above embodiments can be arbitrarily combined, and for the sake of brevity, all possible combinations of the technical features in the above embodiments are not described, but should be considered as the scope of the present specification as long as there is no contradiction between the combinations of the technical features.
The above-mentioned embodiments only express several embodiments of the present application, and the description thereof is more specific and detailed, but not construed as limiting the scope of the invention. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the concept of the present application, which falls within the scope of protection of the present application. Therefore, the protection scope of the present patent shall be subject to the appended claims.
Claims (7)
1. A pedestrian inertial navigation method based on a graph optimization framework is characterized by comprising the following steps:
performing data processing on the acquired MIMU navigation data and the auxiliary navigation data, determining variable nodes, and acquiring position information corresponding to the MIMU navigation data in the positions of the variable nodes and global factors corresponding to the auxiliary navigation data;
navigation resolving is carried out through Kalman filtering according to the position information, zero-speed detection is carried out according to gait characteristics of the MIMU navigation data, and virtual observed quantity of the Kalman filtering is obtained;
correcting errors of navigation calculation of Kalman filtering according to the virtual observed quantity to obtain an inertial navigation calculation result;
constructing a factor graph by taking motion constraint and scene constraint as factors according to the global factors;
performing global pose optimization according to the factor graph and the inertial navigation resolving result to obtain a global optimization result;
and optimizing inertial navigation parameters according to the global optimization result and the inertial navigation resolving result.
2. The method according to claim 1, wherein the data processing the obtained MIMU navigation data and the auxiliary navigation data, determining the variable node, and obtaining the position information corresponding to the MIMU navigation data and the global factor corresponding to the auxiliary navigation data in the position of the variable node comprises:
performing data processing on the acquired MIMU navigation data and the auxiliary navigation data based on the acceleration data to obtain position information and attitude updating frequency;
aligning the MIMU navigation data and the auxiliary navigation data according to the timestamp information of the MIMU navigation data and the auxiliary navigation data;
and determining variable nodes according to the position information and the attitude updating frequency, and determining a global factor corresponding to the auxiliary navigation data of each variable node.
3. The method of claim 1, wherein the location information comprises: position, velocity, and quaternion direction;
the navigation resolving is carried out through Kalman filtering according to the position information, zero-speed detection is carried out according to gait characteristics of the MIMU navigation data, and virtual observed quantity of the Kalman filtering is obtained, and the method comprises the following steps:
determining the state of a filter of Kalman filtering according to the position, the speed and the direction of the quaternion;
and performing zero-speed detection according to the gait characteristics of the MIMU navigation data to obtain the virtual observed quantity of Kalman filtering.
4. The method of claim 3, wherein performing a zero-speed detection based on gait characteristics of the MIMU navigation data to obtain a virtual observer of Kalman filtering comprises:
adjusting inertial navigation parameters according to gait characteristics of the MIMU navigation data;
and carrying out zero-speed detection according to the inertial navigation parameters to obtain the virtual observed quantity of Kalman filtering.
5. The method of claim 4, wherein performing inertial navigation parameter optimization according to the global optimization result and the inertial navigation solution result comprises:
and optimizing the inertial navigation parameters according to the global optimization result and the inertial navigation resolving result.
6. The method according to any of claims 1 to 5, wherein the auxiliary navigation data comprises: magnetometer data, GPS data, WiFi fingerprint data, and bluetooth data.
7. A pedestrian inertial navigation device based on a graph optimization framework, the device comprising:
the preprocessing module is used for carrying out data processing on the acquired MIMU navigation data and the auxiliary navigation data, determining variable nodes and obtaining position information corresponding to the MIMU navigation data in the positions of the variable nodes and global factors corresponding to the auxiliary navigation data;
the inertial navigation resolving module is used for performing navigation resolving through Kalman filtering according to the position information and performing zero-speed detection according to gait characteristics of the MIMU navigation data to obtain virtual observed quantity of the Kalman filtering; correcting errors of navigation calculation of Kalman filtering according to the virtual observed quantity to obtain an inertial navigation calculation result;
the global optimization module is used for constructing a factor graph by taking motion constraint and scene constraint as factors according to the global factors; performing global pose optimization according to the factor graph and the inertial navigation resolving result to obtain a global optimization result;
and the inertial navigation optimization module is used for optimizing inertial navigation parameters according to the global optimization result and the inertial navigation resolving result.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110416612.9A CN112985392B (en) | 2021-04-19 | 2021-04-19 | Pedestrian inertial navigation method and device based on graph optimization framework |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110416612.9A CN112985392B (en) | 2021-04-19 | 2021-04-19 | Pedestrian inertial navigation method and device based on graph optimization framework |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112985392A true CN112985392A (en) | 2021-06-18 |
| CN112985392B CN112985392B (en) | 2021-07-30 |
Family
ID=76341051
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110416612.9A Active CN112985392B (en) | 2021-04-19 | 2021-04-19 | Pedestrian inertial navigation method and device based on graph optimization framework |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112985392B (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114545472A (en) * | 2022-01-26 | 2022-05-27 | 中国人民解放军国防科技大学 | Navigation method and device of GNSS/INS combined system |
| CN115235454A (en) * | 2022-09-15 | 2022-10-25 | 中国人民解放军国防科技大学 | Pedestrian motion constraint visual inertial fusion positioning and mapping method and device |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106197408A (en) * | 2016-06-23 | 2016-12-07 | 南京航空航天大学 | A kind of multi-source navigation data fusion method based on factor graph |
| CN107635204A (en) * | 2017-09-27 | 2018-01-26 | 深圳大学 | A kind of indoor fusion and positioning method and device of motor behavior auxiliary, storage medium |
| US20180031387A1 (en) * | 2016-07-29 | 2018-02-01 | Carnegie Mellon University | State estimation for aerial vehicles using multi-sensor fusion |
| CN109889974A (en) * | 2019-02-01 | 2019-06-14 | 湖南格纳微信息科技有限公司 | A kind of building and update method of indoor positioning multi-source information fingerprint base |
| CN110207692A (en) * | 2019-05-13 | 2019-09-06 | 南京航空航天大学 | A kind of inertia pre-integration pedestrian navigation method of map auxiliary |
| CN111735478A (en) * | 2020-08-19 | 2020-10-02 | 中国人民解放军国防科技大学 | LSTM-based pedestrian real-time navigation zero-speed detection method |
| CN112525197A (en) * | 2020-11-23 | 2021-03-19 | 中国科学院空天信息创新研究院 | Ultra-wideband inertial navigation fusion pose estimation method based on graph optimization algorithm |
-
2021
- 2021-04-19 CN CN202110416612.9A patent/CN112985392B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106197408A (en) * | 2016-06-23 | 2016-12-07 | 南京航空航天大学 | A kind of multi-source navigation data fusion method based on factor graph |
| US20180031387A1 (en) * | 2016-07-29 | 2018-02-01 | Carnegie Mellon University | State estimation for aerial vehicles using multi-sensor fusion |
| CN107635204A (en) * | 2017-09-27 | 2018-01-26 | 深圳大学 | A kind of indoor fusion and positioning method and device of motor behavior auxiliary, storage medium |
| CN109889974A (en) * | 2019-02-01 | 2019-06-14 | 湖南格纳微信息科技有限公司 | A kind of building and update method of indoor positioning multi-source information fingerprint base |
| CN110207692A (en) * | 2019-05-13 | 2019-09-06 | 南京航空航天大学 | A kind of inertia pre-integration pedestrian navigation method of map auxiliary |
| CN111735478A (en) * | 2020-08-19 | 2020-10-02 | 中国人民解放军国防科技大学 | LSTM-based pedestrian real-time navigation zero-speed detection method |
| CN112525197A (en) * | 2020-11-23 | 2021-03-19 | 中国科学院空天信息创新研究院 | Ultra-wideband inertial navigation fusion pose estimation method based on graph optimization algorithm |
Non-Patent Citations (3)
| Title |
|---|
| CHENG CHEN.ET AL: "Multi-Sensor Fusion Technology in Inertial Navigation System Using Factor Graph", 《PROCEEDINGS OF THE 37TH CHINESE CONTROL CONFERENCE》 * |
| MICHAL NOWICKI .ET AL: "Indoor Navigation with a Smartphone Fusing Inertial and WiFi Data via Factor Graph Optimization", 《MOBI CASE2015:MOBILE COMPUTING, APPLICATIONS, AND SERVICES》 * |
| 张靖等: "一种基于因子图的异步信息融合定位算法", 《导弹与航天运载技术》 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114545472A (en) * | 2022-01-26 | 2022-05-27 | 中国人民解放军国防科技大学 | Navigation method and device of GNSS/INS combined system |
| CN115235454A (en) * | 2022-09-15 | 2022-10-25 | 中国人民解放军国防科技大学 | Pedestrian motion constraint visual inertial fusion positioning and mapping method and device |
| CN115235454B (en) * | 2022-09-15 | 2022-12-30 | 中国人民解放军国防科技大学 | Pedestrian motion constraint visual inertial fusion positioning and mapping method and device |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112985392B (en) | 2021-07-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110146909B (en) | Positioning data processing method | |
| CN111156987B (en) | Inertia/astronomy combined navigation method based on residual compensation multi-rate CKF | |
| CN112985392B (en) | Pedestrian inertial navigation method and device based on graph optimization framework | |
| CN109059907B (en) | Trajectory data processing method and device, computer equipment and storage medium | |
| US10267646B2 (en) | Method and system for varying step length estimation using nonlinear system identification | |
| CN111145251B (en) | Robot and synchronous positioning and mapping method thereof and computer storage device | |
| US11035915B2 (en) | Method and system for magnetic fingerprinting | |
| CN111136660A (en) | Robot pose positioning method and system | |
| KR20210026795A (en) | System for Positioning Hybrid Indoor Localization Using Inertia Measurement Unit Sensor and Camera | |
| CN114485643A (en) | Environment sensing and high-precision positioning method for coal mine underground mobile robot | |
| CN114166221A (en) | Auxiliary transportation robot positioning method and system in dynamic complex mine environment | |
| CN114383605B (en) | Indoor positioning and optimizing method based on MEMS sensor and sparse landmark point | |
| CN114396943A (en) | Fusion positioning method and terminal | |
| CN113899364B (en) | Positioning method and device, equipment and storage medium | |
| CN115235455A (en) | Pedestrian positioning method based on smart phone PDR and vision correction | |
| CN115164887A (en) | Pedestrian navigation positioning method and device based on laser radar and inertia combination | |
| Hnatiuc et al. | Path recognition using mobile phone | |
| CN114459469A (en) | Multi-motion-state navigation method and device and intelligent wearable equipment | |
| CN114608560B (en) | Passive combined indoor positioning system and method based on intelligent terminal sensor | |
| An et al. | A multi-sensor fusion algorithm for pedestrian navigation using factor graphs | |
| Moafipoor et al. | Multi-sensor personal navigator supported by adaptive knowledge based system: Performance assessment | |
| CN114353787B (en) | Multisource fusion positioning method | |
| KR101847113B1 (en) | Estimation method and apparatus for information corresponding camera orientation by using image | |
| Deng et al. | Data-Driven Based Cascading Orientation and Translation Estimation for Inertial Navigation | |
| CN116337048B (en) | Polarization visual inertia synchronous mapping and positioning method and device for unmanned platform |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |