Pedestrian indoor and outdoor passive intelligent navigation positioning system and method based on multi-sensor fusion

Abstract

The invention discloses a multi-sensor fusion pedestrian indoor and outdoor passive intelligent navigation positioning system and method. The system comprises a data acquisition module and a handheld terminal, the data acquisition module comprises a radar odometer, a visual odometer, a chest IMU and a foot IMU, and the handheld terminal comprises a multi-sensor communication module, a data processing module, an intelligent interaction module and a fusion algorithm module. The method comprises the following steps: acquiring pedestrian motion data in real time, and fusing laser radar feature points with IMU data by using an iterative extended Kalman filter; track reconstruction and path planning are carried out according to the visual odometer data, and navigation positioning errors are corrected according to the foot IMU data; taking the chest IMU as a navigation positioning space reference, and selecting an optimal sensor at the current moment to carry out fusion navigation; and the handheld terminal application software displays the positioning information. The precision and stability of indoor and outdoor passive navigation positioning of pedestrians are improved, the operation is simple, the cost is low, and the application range is wide.

Description

Technical Field

[0001] The present invention relates to the technical field of indoor and outdoor pedestrian positioning, and in particular to a multi-sensor fusion indoor and outdoor passive intelligent navigation and positioning system and method for pedestrians. Background Art

[0002] Against the backdrop of the booming development of smart cities and the Internet of Things, the importance of pedestrian navigation and positioning systems in daily life has become increasingly prominent, especially indoors and in complex environments. With the continuous advancement of intelligent construction in public places such as shopping malls, hospitals, office buildings, airports, and subway stations, the demand for pedestrian navigation and positioning has become extremely urgent. Traditional satellite navigation systems (GNSS) can provide relatively accurate positioning services in outdoor environments. However, once entering shielded environments such as indoors and underground, the GNSS signal will suffer severe attenuation or even complete signal loss, making effective positioning impossible. Therefore, the development of low-cost, high-precision, and highly adaptable indoor pedestrian positioning technology has become a key task in the construction of the Internet of Things and smart cities.

[0003] Currently, most pedestrian positioning technologies rely on active positioning methods, such as radio frequency identification (RFID), Bluetooth, ultra-wideband, and wireless local area networks. These technologies do have certain advantages in relatively simple and small-scale positioning scenarios, but their limitations become apparent once applied to large and complex environments.

[0004] Regarding radio frequency identification (RFID) technology, invention patent CN201811564408.6 discloses an indoor positioning method and system based on RFID, which uses multiple readers and tags deployed indoors to achieve positioning. Its operating principle is that a fixed reader reads the characteristic information of the target RFID tag to determine the location. In practical applications, although this technology is widely used in low-cost positioning, its positioning accuracy is relatively low. Especially in environments with many obstructions, the signal is extremely susceptible to interference, resulting in a significant decrease in positioning accuracy, which is completely unable to meet the requirements of high-precision and wide-coverage positioning. Similarly, invention patent CN201210528602.5 discloses an indoor positioning and tracking system and method based on passive RFID technology. The system consists of a central controller, a router, a wireless router, an all-in-one electronic tag reader / writer, an antenna, and passive tags. This technology achieves positioning and tracking of dynamic targets such as people and objects through non-line-of-sight, contactless, and high-speed recognition of passive tags carried by the target to be located and tracked. However, in complex environments, passive tags are affected by obstruction and interference, and signal transmission is unstable, resulting in limited positioning accuracy and poor adaptability to large-scale, high-precision positioning scenarios. There is also invention patent CN202410103701.1 that discloses a passive positioning system and method for complex indoor environments based on ultra-high frequency RFID technology. The system includes several passive RFID tags, an RFID reader, several antennas, a router, and a host computer. It uses a specific algorithm to achieve low-cost, low-power, and high-precision RFID positioning in complex indoor environments. However, in actual complex environments, ultra-high frequency signals are easily interfered with by objects such as metal and liquid, resulting in signal attenuation and distortion, affecting the stability of positioning accuracy.

[0005] In terms of Bluetooth positioning technology, taking the invention patent CN202311120275.4 as an example, a method and system for indoor positioning bionic interaction based on Bluetooth technology is disclosed. By installing a Bluetooth LAN access point indoors, positioning is performed based on signal strength. However, Bluetooth positioning technology itself is heavily dependent on signal strength and is greatly affected by multipath effects and occlusion. In large and complex scenarios, the surrounding environment seriously interferes with the signal, and the signal fluctuates frequently, making the positioning accuracy extremely unstable and difficult to provide continuous, stable, and high-precision positioning.

[0006] Ultra-wideband (UWB) positioning systems have high accuracy and anti-interference capabilities, and are widely used in indoor positioning, especially in scenarios with high precision requirements. For example, the indoor positioning method and system based on ultra-wideband UWB disclosed in invention patent CN202410743516.9 uses pre-arranged anchor nodes and bridge nodes to communicate with blind nodes to achieve positioning. However, UWB technology has obvious defects. Invention patent CN202410103701.1 discloses a passive positioning system and method for complex indoor environments based on ultra-high frequency RFID technology, which points out that the ultra-wideband method provides high-precision positioning, but the equipment cost and power consumption are high. Its deployment cost is extremely high, requiring the deployment of multiple base stations, and the installation and maintenance of the base stations are extremely complicated. When used in large areas such as large shopping malls or airports, it will bring a heavy economic burden to users.

[0007] Wireless local area network (Wi-Fi) positioning technology, in the invention patent CN201811654253.5, provides an indoor positioning method based on a mixture of WIFI and ultrasound and its system, which achieves positioning with the help of a wireless local area network composed of wireless access points. In the invention patent CN201810020828.1, a passive indoor positioning method based on a network visual map is disclosed. It is mentioned that with the development of wireless local area networks (WLANs), although existing wireless devices can be used to reduce system deployment costs, the current passive indoor positioning method based on WLAN only uses the amplitude information of CSI, and the positioning effect is not good. Wi-Fi positioning is extremely susceptible to interference in the presence of multiple obstacles and complex environments. In actual application scenarios, various obstacles in indoor environments, such as walls, furniture, etc., will reflect, refract and block Wi-Fi signals, resulting in large fluctuations in positioning accuracy, which is difficult to meet application scenarios with strict requirements on accuracy and stability.

[0008] Given the numerous shortcomings of active positioning technology, passive positioning has gradually become a research focus in the field of indoor and outdoor pedestrian navigation. Common passive positioning methods include inertial navigation, radar odometry, and visual odometry. These offer advantages such as low power consumption, high concealment, and strong anti-interference capabilities, enabling relatively reliable positioning services in complex and dynamic environments. Leveraging multi-source fusion technology and the complementary nature of various sensors and inertial navigation, combinations such as satellite / inertial navigation, radar / inertial navigation, and visual / inertial navigation have shown promising application prospects. However, these combinations also have drawbacks.

[0009] In a passive combination indoor positioning system and method based on intelligent terminal sensors disclosed in invention patent CN202210285267.4, the satellite / inertial navigation combination has a serious drift phenomenon in indoor environments due to the limitation of satellite signals. For example, in an urban canyon environment with tall buildings or in an indoor environment, satellite signals are easily blocked and interfered with, resulting in large deviations in the positioning results and failure to meet the demand for accurate positioning. In an indoor positioning method and system based on machine vision and WSN disclosed in invention patent CN202311456097.2, in an environment with insufficient light, the image acquisition quality of visual positioning decreases and the difficulty of feature extraction increases, resulting in significantly insufficient positioning accuracy; when the environment changes rapidly, such as when objects in the scene move quickly or when light changes suddenly, the visual algorithm is difficult to adapt quickly, has poor robustness, and the positioning results are unstable. In the invention patent CN202311132642.2, a multi-legged robot and its navigation method based on an inertial navigation device are disclosed. While dead reckoning using only foot-based inertial navigation provides high accuracy in the short term, over time, the heading gradually shifts and the error rapidly diverges. This is because the inertial navigation system is affected by factors such as sensor drift and accumulated errors during long-term operation, resulting in an increasing deviation between the positioning result and the actual position.

[0010] In summary, existing indoor and outdoor passive navigation and positioning technologies have significant environmental limitations. A single combination of approaches not only results in low navigation and positioning accuracy but also suffers from poor stability, making them difficult to adapt to complex and changing real-world scenarios. Furthermore, sensor failure can have a devastating impact on the positioning system. Therefore, there is an urgent need to develop a highly robust navigation system that can operate stably in complex and changing environments and maintain a certain level of positioning accuracy over long periods of time to meet the growing demand for real-world applications. Summary of the Invention

[0011] The purpose of this invention is to provide a pedestrian indoor and outdoor passive intelligent navigation and positioning system and method with high positioning accuracy, good robustness, strong stability, and multi-sensor fusion that can cope with changing indoor and outdoor environments.

[0012] The technical solution for achieving the purpose of the present invention is: a multi-sensor fusion indoor and outdoor passive intelligent navigation and positioning system for pedestrians, including a data acquisition module and a handheld terminal, wherein the handheld terminal includes a multi-sensor communication module, a data processing module, an intelligent interaction module and a fusion algorithm module;

[0013] The data acquisition module includes a radar odometer, a visual odometer, a chest IMU, and a foot IMU, which are used to collect raw data from each sensor;

[0014] The multi-sensor communication module is used to transmit the raw data collected by the data collection module to the handheld terminal in real time;

[0015] The data processing module is used to process the multi-sensor data into navigation and positioning information;

[0016] The intelligent interaction module is used to exchange information between the user and the navigation sensor and control the start and end of navigation;

[0017] The fusion algorithm module is used to preferentially combine the information from various sensors and output the optimal navigation and positioning solution.

[0018] A multi-sensor fusion pedestrian indoor and outdoor passive intelligent navigation and positioning method includes the following steps:

[0019] Step 1: The data acquisition module collects pedestrian movement data in real time and transmits it to the handheld terminal;

[0020] Step 2: The data processing module uses a tightly coupled iterative extended Kalman filter to fuse the lidar feature points with the IMU data based on the radar odometry scan point cloud data.

[0021] Step 3: Based on the visual odometry scanned mapping data, the trajectory reconstruction and path planning are completed through the semantic VSLAM-assisted matching algorithm, and the navigation information is output;

[0022] Step 4: Based on the foot IMU positioning and navigation, the navigation positioning error is corrected by zero-speed correction using the characteristics of the pedestrian's feet;

[0023] Step 5: Use the chest IMU as the navigation and positioning spatial reference, and synchronize other sensor data with it in time and space;

[0024] Step 6: Detect whether each sensor is working properly and whether the data output is normal according to the intelligent algorithm;

[0025] Step 7: Based on the adaptive federated filtering intelligent fusion algorithm, the optimal sensor at the current moment is selected for fusion navigation by judging the confidence level and working status of each sensor;

[0026] Step 8: The handheld terminal application software performs human-computer interaction and displays positioning information.

[0027] Compared with the existing technology, the present invention has the following significant advantages: (1) The present invention relies on a high-precision radar odometer for positioning, with a positioning accuracy of 0.5%, and has good responsiveness when switching between indoor and outdoor scenes, and can perform seamless switching and seamless positioning, and significantly improves long-term positioning accuracy; (2) It has better environmental adaptability. Whether in urban high-rise buildings, tunnels, forests and other areas where GPS signals are easily interfered with, or in shopping malls, hospitals, underground garages, mines and other scenes where there are a large number of moving body interference, equipment interference, and magnetic field interference, the intelligent navigation algorithm can get rid of dependence on the environment, output the optimal navigation solution based on wearing multiple passive sensors, and provide guarantee for navigation in various complex environments; (3) Using multiple sensors , the algorithm will detect and isolate the sensor faults. When a sensor is abnormal, the use of the sensor will be abandoned and other sensors will be switched to for navigation and positioning. Passive devices do not rely on any external device information and are more stable. (4) By developing handheld software, the working status of the sensor and navigation information can be displayed in real time. The operation is simple. You only need to click the "communication connection" button to complete the data connection and transmission of the sensor. Click to start navigation to store data and display the location of the motion trajectory. It has strong human-computer interaction, greatly simplifies the difficulty of operation, and is conducive to promotion and application. (5) The present invention only requires wearing a set of integrated equipment, has a high cost performance, good economic benefits, and has good application prospects. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 This is a structural block diagram of a multi-sensor fusion pedestrian indoor and outdoor passive intelligent navigation and positioning system of the present invention.

[0029] Figure 2 The present invention is a flowchart of a multi-sensor fusion pedestrian indoor and outdoor passive intelligent navigation and positioning method.

[0030] Figure 3 It is a flow chart of the fusion of lidar feature points and IMU data in the present invention.

[0031] Figure 4 This is a flow chart of correcting navigation positioning errors using foot IMU zero-speed correction in the present invention.

[0032] Figure 5 It is a flow chart of the adaptive federated filtering intelligent fusion algorithm in the present invention.

[0033] Figure 6 Graph showing the comparison of experimental trajectories and positioning results in an embodiment of the present invention.

[0034] Figure 7 This is an error curve diagram of experimental positioning in an embodiment of the present invention.

[0035] Figure 8 This is a diagram showing the positioning results of a simulated sensor failure in an embodiment of the present invention.

[0036] Figure 9 This is a diagram of the sensor data collection interface of the handheld terminal application software in an embodiment of the present invention.

[0037] Figure 10 This is a navigation track interface diagram of the handheld terminal application software in an embodiment of the present invention. DETAILED DESCRIPTION

[0038] The following combination Figures 1 to 10 The present invention is further described in detail with reference to the accompanying drawings and specific embodiments.

[0039] like Figure 1 As shown, the present invention provides a multi-sensor fusion pedestrian indoor and outdoor passive intelligent navigation and positioning system, comprising a data acquisition module 1 and a handheld terminal 2, wherein the handheld terminal comprises a multi-sensor communication module 21, a data processing module 22, an intelligent interaction module 23 and a fusion algorithm module 24;

[0040] The data acquisition module 1 includes a radar odometer 11, a visual odometer 12, a chest IMU 13, and a foot IMU 14, which are used to collect raw data from each sensor;

[0041] The multi-sensor communication module 21 is used to transmit the raw data collected by the data acquisition module to the handheld terminal in real time;

[0042] The data processing module 22 is used to process the multi-sensor data into navigation and positioning information;

[0043] The intelligent interaction module 23 is used to exchange information between the user and the navigation sensor and control the start and end of navigation;

[0044] The fusion algorithm module 24 is used to preferentially combine and intelligently match the information from various sensors to output the optimal navigation and positioning solution.

[0045] like Figure 2 As shown, the present invention provides a multi-sensor fusion pedestrian indoor and outdoor passive intelligent navigation and positioning method, comprising the following steps:

[0046] Step 1: The data acquisition module 1 collects pedestrian movement data in real time and transmits it to the handheld terminal 2;

[0047] Step 2: The data processing module 22 uses a tightly coupled iterative extended Kalman filter to fuse the lidar feature points with the IMU data based on the point cloud data scanned by the radar odometer 11;

[0048] Step 3: Based on the visual odometry 12 scanning mapping data, the trajectory reconstruction and path planning are completed through the semantic VSLAM assisted matching algorithm, and the navigation information is output;

[0049] Step 4: Position and navigate based on the foot IMU 14, and use the characteristics of the pedestrian's foot to correct the navigation positioning error through zero-speed correction;

[0050] Step 5: Use the chest IMU 13 as the navigation and positioning spatial reference, and synchronize other sensor data with it in time and space;

[0051] Step 6: Detect whether each sensor is working properly and whether the data output is normal according to the intelligent algorithm;

[0052] Step 7: Based on the adaptive federated filtering intelligent fusion algorithm, the optimal sensor at the current moment is selected for fusion navigation by judging the confidence level and working status of each sensor;

[0053] Step 8: The handheld terminal 2 application software performs human-computer interaction and displays the positioning information.

[0054] As a specific example, in step 1, the data acquisition module 1 collects pedestrian motion data in real time and transmits it to the handheld terminal 2, as follows:

[0055] Pedestrian motion data is collected through the radar odometer 11, visual odometer 12, chest IMU 13 and foot IMU 14, and the radar odometer 11 data, visual odometer 12 data, chest IMU 13 data and foot IMU 14 data are transmitted to the handheld terminal 2 via USB, RS232, RS422 and Bluetooth.

[0056] As a specific example, in step 2, the data processing module 22 uses the tightly coupled iterative extended Kalman filter to fuse the lidar feature points with the IMU data based on the scanned point cloud data of the radar odometer 11, such as Figure 3 As shown, the details are as follows:

[0057] Step 2.1: Pre-integrate the IMU data to infer position, velocity, attitude, and other information from the IMU's acceleration and angular velocity information in a relatively short period of time, thereby reducing the real-time computational burden of the IMU's high-frequency data and synchronizing it with the radar data. The pre-integration formula is as follows:

[0058]

[0059] v(t+1)=v(t)+a(t)Δt

[0060]

[0061] Where p(t) and p(t+1) are the positions at the current moment and the next moment, a(t) is the current acceleration, Δt is the time interval, v(t) and v(t+1) are the velocities at the current moment and the next moment; q(t) and q(t+1) are the quaternions at the current moment and the next moment; ω(t) is the angular acceleration at the current moment;

[0062] Step 2.2, the Fast-LIO algorithm uses the lidar to perform point cloud registration based on the least squares method. For the two frame point clouds P1 and P2, their relationship can be described by rigid transformation including rotation and translation. The transformed point cloud is expressed by the following formula:

[0063] P2=RP1+t

[0064] By minimizing the error between point clouds, the optimal R and t can be obtained:

[0065]

[0066] Among them, p 1i and p 2i is the i-th corresponding point in the two point clouds; R is the rotation matrix, t is the translation vector; E(R,t) is p 1i and p 2i The error function between them is used to measure the difference between the transformed point cloud and the target point cloud; the optimal R and t are solved by minimizing E(R,t);

[0067] Step 2.3: During the fusion process, the extended Kalman filter (EKF) is used to estimate the system state. The state vector includes position, velocity, acceleration, and rotation angle. The update formula of the Kalman filter is:

[0068] x k+1 =Ax k +Bu k +w k

[0069] y k =Cx k +v k

[0070] Where A is the state transfer matrix and B is the control matrix u k At time k, input w k is the noise at time k; y k is the observation vector at time k, C is the observation matrix, v k is the observation noise;

[0071] Step 2.4: Use an optimization method to jointly optimize the IMU and LiDAR data to obtain the optimal positioning result of the fusion of the two. The optimization problem is to minimize the following error function:

[0072]

[0073] Among them, h i (x) is the observation model, z i is the observation value, x is the state vector; Φ(x) is the error function, which is used to measure the difference between the observation model and the observation value.

[0074] As a specific example, in step 3, based on the visual odometry 12 scanning mapping data, the trajectory reconstruction and path planning are completed through the semantic VSLAM assisted matching algorithm, and the navigation information is output as follows:

[0075] Step 3.1: Output the category of each pixel through the convolutional neural network, and use the object detection algorithm to identify and track dynamic and static objects. Static objects are used for mapping, and dynamic objects are ignored or processed separately to avoid interference with positioning accuracy.

[0076] Semantic-assisted V-SLAM uses semantic information to optimize motion estimation. The formula is as follows:

[0077]

[0078] Where T is the pose transformation matrix, and are the positions of the jth feature point in the image at the i-th frame and the i+1-th frame respectively;

[0079] Step 3.2, the map update formula is as follows:

[0080] Μ(t)=Μ(t-1)∪{p i ,c i}

[0081] Among them, Μ(t) is the semantic map at the tth moment, p i is the object position, c i is the object category;

[0082] Step 3.3: Use graph optimization to optimize the backend. The goal of the optimization problem is to minimize the error function, where the error includes the error in camera pose and the error in object position. The optimization objective function is as follows:

[0083]

[0084] Among them, T i and is the camera pose before and after optimization, p j and is the object position before and after optimization.

[0085] As a specific example, in step 4, the foot IMU 14 is used for positioning and navigation, and the navigation positioning error is corrected by zero-speed correction using the characteristics of the pedestrian's foot, such as Figure 4 As shown, the details are as follows:

[0086] Step 4.1: The inertial pedestrian navigation system collects pedestrian acceleration and angular velocity information through the IMU and uses the strapdown inertial navigation algorithm to calculate the pedestrian's posture information;

[0087] The error model of the strapdown inertial navigation system is:

[0088]

[0089] Among them, δv n ,δp n and They are velocity error, position error and attitude angle error respectively; is the projection of the angular velocity of the navigation system relative to the inertial system in the navigation system; and are the projections of the Earth's rotation angular velocity and position rate in the navigation system respectively; and are the projections of the Earth's rotation angular velocity and position rate error in the navigation system; v n is the velocity of the carrier relative to the earth; To convert the carrier coordinate system into the attitude matrix of the navigation system; g n , δg n are the local gravity acceleration vector and the local gravity acceleration vector error respectively; f b 、 are the measurements of the accelerometer and gyroscope respectively; δf b 、 are the measurement errors of the accelerometer and gyroscope respectively;

[0090] According to the strapdown inertial navigation system error model, position, velocity and attitude errors are selected to establish the system state error vector:

[0091]

[0092] The system state equation is:

[0093] δX k =F k / k-1 δX k-1 +Γ k / k-1 W k-1

[0094] Among them, δX k is the system error state vector at time k; W k-1 is the system noise vector; Γ k / k-1is the system noise driving matrix; F k / k-1 is the state transfer matrix, and its expression is as shown in the formula:

[0095]

[0096] The observation equation of the system is:

[0097] Z k =H k X k +V k

[0098] Among them, Z k is the system observation vector; V k is the system observation noise vector; H k is the system measurement matrix;

[0099] Step 4.2: Based on the characteristics of pedestrian movement, the foot moves regularly during the movement process, and the foot speed is zero from the moment of contact to the moment of lift-off. The zero-speed intervals in different movement states are identified and recorded using the adaptive threshold method. The statistics are based on the maximum generalized likelihood method, and the formula is as follows:

[0100]

[0101] Where T(Z n ) is the test statistic; are the observation noise variances of acceleration and angular velocity respectively; and are the output values ​​of acceleration and angular velocity at time k respectively; n is the number of sampling points; N is the window size; is the mean of the continuous acceleration within the window; g is the acceleration due to gravity;

[0102] The formula for calculating the adaptive threshold is:

[0103]

[0104] Among them, γ a (n) the minimum value of the test statistic; q is the conditional parameter; is the mean of the test statistic within the window, if T(Z n ) is less than γ a (n), then it is the zero speed interval;

[0105] Step 4.3: Based on the characteristics of the zero-speed interval, a filtering algorithm is designed to eliminate the speed error by using the zero-speed pseudo-observation. When the zero-speed interval is detected, the EKF is used for filtering correction.

[0106] System observation matrix:

[0107]

[0108] in is the velocity error, is the heading error;

[0109] System measurement matrix H k for:

[0110]

[0111] According to the integral of the speed change, the corresponding position information is obtained.

[0112] As a specific example, in step 5, the chest IMU 13 is used as the navigation and positioning spatial reference, and other sensor data are synchronized with it in time and space, as follows:

[0113] Step 5.1: Perform initial alignment using the optimized alignment method (OBA). OBA can complete initial alignment on both a static base and a moving base, and has strong anti-interference capabilities. This method decomposes the attitude matrix into a constant matrix and two time-varying matrices:

[0114] The carrier motion term in the observation vector is compensated by the external auxiliary information BDS, and the influence of the accelerometer random noise is suppressed by the integration process, so that fast alignment can be achieved under moving conditions;

[0115] In the absence of external auxiliary information, the moving base OBA method is used. First, the high-frequency interference and motion interference of the gravity apparent motion vector are filtered out by online wavelet denoising, and then the initial alignment is completed by the OBA method.

[0116] Step 5.2: Align the initial posture matrices of other sensors to a unified coordinate system.

[0117] As a specific example, in step 6, whether each sensor is working properly and whether the data output is normal is detected according to the intelligent algorithm, as follows:

[0118] Step 6.1: Using the time window method, if no sensor data is received within the set maximum time window, the sensor is considered to have a hard fault and will not be used in the fusion algorithm;

[0119] Step 6.2: According to the algorithm settings, detect whether the sensor output data is abnormal, calculate the mean and standard deviation of the data in a sliding window, and check whether the current data point exceeds the threshold. If it exceeds the threshold, it is considered that the sensor output data has a hard fault, and the sensor output data will not be used in the fusion algorithm.

[0120] As a specific example, in step 7, based on the adaptive federated filtering intelligent fusion algorithm, the optimal sensor at the current moment is selected for fusion navigation by judging the confidence level and working status of each sensor, such as Figure 5 As shown, the details are as follows:

[0121] Step 7.1. The federated filtering algorithm consists of two filtering levels: a local sub-filter and a main filter. Each local sub-filter performs Kalman filtering operations independently and in parallel. The sub-filter only obtains the best local estimate of the system's common state, thereby obtaining a global estimate of the system's common state. The initial information includes the system's global estimation error variance matrix P and the system's process noise variance matrix Q at the initial time. Information allocation is the process of allocating information between each subsystem filter and the main filter.

[0122] Through information allocation, the estimation of the system error variance matrix P, the estimation of the process noise variance matrix Q, and the filtering state X can be effectively allocated to different subsystem filters, thereby achieving effective data processing; the information allocation formula is:

[0123]

[0124] Among them, Q i (k) is the process noise variance matrix of the filter of the i-th subsystem at time k; Q g (k) is the system error variance matrix of the i-th subsystem filter at time k, P i (k) is the global system error variance matrix, P g (k) is the global system error variance matrix; is the state estimate of the filter of the ith subsystem at time k, is the global state estimate; β i is the information allocation factor of each filter, and the global process noise variance matrix satisfies information conservation:

[0125]

[0126] Step 7.2, information update, includes information time update and information measurement update. The time update process is to ensure that the calculation of the sub-filter and the main filter is not disturbed in the case of time changes, and they can be executed independently. The specific filtering algorithm is:

[0127]

[0128] in, is the prior estimate of the state of the i-th sub-filter at time k based on time k-1; ψ(k,k-1) is the state transfer matrix, describing the state transition relationship from time k-1 to time k; P i (k|k-1) is the prior estimate of the error covariance of the i-th sub-filter at time k based on time k-1, P i (k-1) is the error covariance matrix of the i-th sub-filter at the k-th moment; ψ T is the transposed matrix; Q i (k-1) is the process noise variance matrix of the i-th sub-filter at time k-1;

[0129] The measurement update process is that the main filter does not have measurement updates, only the sub-filters have measurement updates. The algorithm is as follows:

[0130]

[0131] Among them, P i -1 (k) is the inverse of the error covariance matrix of the i-th sub-filter at time k, P i -1 (k|k-1) is the inverse of the error covariance matrix of the i-th sub-filter at time k based on time k-1, H i T (k) is the observation matrix H of the i-th sub-filter at time k i The transpose of (k), R i (k) is the observation noise variance matrix of the i-th sub-filter at time k, K i (k) is the Kalman gain of the i-th sub-filter at time k;

[0132] Step 7.3: A more complete global optimal estimate is obtained by fusing the estimated information of each sub-filter. The fusion algorithm is:

[0133]

[0134] Among them, P g is the error covariance matrix after global fusion, [P1 -1 +P2 -1 +........P m -1 +P n -1 ] is the inverse of the error covariance matrix of each sub-filter, X g is the state estimate after global fusion, X1.......X n is the state estimate of each sub-filter;

[0135] Step 7.4: According to the working status of all sensors at the current moment, select the optimal sensor positioning result for fusion positioning.

[0136] As a specific example, in step 8, the handheld terminal 2 application software performs human-computer interaction and displays positioning information, as follows:

[0137] Click the "Communication Connection" button to connect to the LAN or Bluetooth, and wait a few seconds for all sensor data to be displayed in real time on the interface.

[0138] According to the status color of the signal light, if it is green, it means the connection is normal, if it is red, it means the connection is abnormal and the operation is abnormal;

[0139] By selecting indoor or outdoor initialization, you can initialize whether there is a satellite signal or not;

[0140] By clicking "Start Navigation", the track will be displayed on the interface. Indoor navigation is based on the initial forward direction as the north direction, while outdoor navigation is based on satellite signals.

[0141] By "storing data", the navigation information of each device can be stored on the handheld terminal.

[0142] Example

[0143] To verify the proposed method's accuracy for indoor and outdoor navigation and positioning, experiments were conducted on the Nanjing University of Science and Technology campus on September 17, 2024, in real-world environments, including road, indoor, and forested environments. The participants wore a MEMS-IMU on their feet, a helmet equipped with a Livox-Mid360 lidar and a T265 vision camera, and an IMU on their chest belt. Reference data came from differential GNSS, and indoor reference trajectories were provided by surveying and mapping.

[0144] The experimental trajectory is fixed, and the experiment involves switching between various scenes, both indoors and outdoors. The total experimental distance is approximately 1,500 meters. The initial section is an open concrete road, followed by a forested area, a low-light indoor area, a water room, and then back to the starting point.

[0145] In order to verify the positioning accuracy and stability of the passive navigation algorithm of the present invention in indoor and outdoor environments, this embodiment conducts multiple repeated experiments and uses the following method for analysis and demonstration:

[0146] 1. Analyze positioning error;

[0147] 2. Analyze the impact of equipment failure on navigation;

[0148] 3. Take a large petrochemical plant as an example to conduct an economic analysis;

[0149] Table 1 shows the positioning errors from repeated experiments. Ten experiments were conducted along the same path, with each run length of approximately 1460 meters and a duration of approximately 25 minutes. It can be seen that, in the absence of satellite signals throughout the entire route, the maximum positioning error at the start and end points of the proposed system was 6.73 meters, the minimum positioning error was 3.23 meters, and the maximum positioning error accounted for 0.46% of the total route. Compared to other indoor and outdoor positioning methods in the absence of satellite signals, the proposed system achieves higher positioning accuracy in a variety of environments. Figure 6 This is a comparison diagram of the experimental path map and the satellite reference trajectory. Figure 7 This is a diagram of the positioning error change. The other protrusions are caused by the deterioration and failure of satellite signals caused by entering indoors. Figure 9 This is the sensor data acquisition interface diagram of the handheld terminal application software. Figure 10 This is the navigation track interface diagram of the handheld terminal application software.

[0150] Table 1 Positioning errors of multiple experiments

[0151]

[0152] In order to simulate the positioning results of sensor failure, the design simulates sensor failure by powering off. The radar is disconnected and then reconnected, the foot IMU is disconnected and then reconnected, and the camera is powered off and then reconnected. The superiority of the algorithm is demonstrated by analyzing whether the positioning error suddenly diverges at the moment of sensor failure. Figure 8 This is the result of the experiment. It can be seen that the trajectory does not diverge at the moment of power failure. The algorithm performs fusion positioning based on the suboptimal sensor, showing the stability of this system, with low dependence on sensors and the environment, and can adapt to various scenarios.

[0153] The above description is only a preferred specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by any technician familiar with this technical field within the technical scope disclosed in the present invention should be covered by the scope of protection of the present invention.

[0154] It should be understood that in order to simplify the present invention and help those skilled in the art understand the various aspects of the present invention, in the above description of the exemplary embodiments of the present invention, various features of the present invention are sometimes described in a single embodiment or described with reference to a single figure. However, the present invention should not be interpreted as if all the features included in the exemplary embodiments are essential technical features of the claims of this patent.

Claims

1. A multi-sensor fusion pedestrian indoor and outdoor passive intelligent navigation and positioning system, characterized by: The invention comprises a data acquisition module (1) and a handheld terminal (2), wherein the handheld terminal comprises a multi-sensor communication module (21), a data processing module (22), an intelligent interaction module (23) and a fusion algorithm module (24); The data acquisition module (1) includes a radar odometer (11), a visual odometer (12), a chest IMU (13) and a foot IMU (14), and is used to collect raw data from each sensor; The multi-sensor communication module (21) is used to transmit the raw data collected by the data collection module to the handheld terminal in real time; The data processing module (22) is used to process the multi-sensor data into navigation and positioning information; The intelligent interaction module (23) is used to exchange information between the user and the navigation sensor and control the start and end of navigation; The fusion algorithm module (24) is used to preferentially combine the information of each sensor and output the optimal navigation and positioning solution.

2. A multi-sensor fusion pedestrian indoor and outdoor passive intelligent navigation and positioning method, characterized in that: The following steps are involved: Step 1: The data acquisition module (1) collects pedestrian movement data in real time and transmits it to the handheld terminal (2); Step 2: The data processing module (22) uses a tightly coupled iterative extended Kalman filter to fuse the lidar feature points with the IMU data based on the point cloud data scanned by the radar odometer (11); Step 3: Scan the mapping data using the visual odometry (12), complete trajectory reconstruction and path planning through the semantic VSLAM-assisted matching algorithm, and output navigation information; Step 4: Based on the foot IMU (14) positioning and navigation, the navigation positioning error is corrected by zero-speed correction using the characteristics of the pedestrian's foot; Step 5: Use the chest IMU (13) as the navigation and positioning spatial reference, and synchronize other sensor data with it in time and space; Step 6: Detect whether each sensor is working properly and whether the data output is normal according to the intelligent algorithm; Step 7: Based on the adaptive federated filtering intelligent fusion algorithm, the optimal sensor at the current moment is selected for fusion navigation by judging the confidence level and working status of each sensor; Step 8: The handheld terminal (2) uses application software to perform human-computer interaction and display positioning information.

3. The multi-sensor fusion pedestrian indoor and outdoor passive intelligent navigation and positioning method according to claim 2 is characterized in that: The data acquisition module (1) described in step 1 collects pedestrian movement data in real time and transmits it to the handheld terminal (2), as follows: Pedestrian motion data is collected through a radar odometer (11), a visual odometer (12), a chest IMU (13) and a foot IMU (14), and the radar odometer (11), visual odometer (12), chest IMU (13) and foot IMU (14) data are transmitted to a handheld terminal (2) via USB, RS232, RS422 and Bluetooth.

4. The multi-sensor fusion pedestrian indoor and outdoor passive intelligent navigation and positioning method according to claim 2 is characterized in that: The data processing module (22) described in step 2 uses a tightly coupled iterative extended Kalman filter to fuse the lidar feature points with the IMU data based on the scanned point cloud data of the radar odometer (11), as follows: Step 2.1: Pre-integrate the IMU data to calculate the position, velocity, and attitude information based on the IMU's acceleration and angular velocity information, and synchronize them with the radar data. The pre-integration formula is as follows: v(t+1)=v(t)+a(t)Δt Where p(t) and p(t+1) are the positions at the current moment and the next moment, a(t) is the current acceleration, Δt is the time interval, v(t) and v(t+1) are the velocities at the current moment and the next moment; q(t) and q(t+1) are the quaternions at the current moment and the next moment; ω(t) is the angular acceleration at the current moment; Step 2.2: The Fast-LIO algorithm uses the lidar to perform point cloud registration based on the least squares method. For the two frame point clouds P1 and P2, they are described by a rigid transformation including rotation and translation. The transformed point cloud is expressed by the following formula: P2=RP1+t By minimizing the error between point clouds, the optimal R and t are obtained: Among them, p 1i and p 2i is the i-th corresponding point in the two point clouds; R is the rotation matrix, t is the translation vector; E(R,t) is p 1i and p 2i The error function between them is used to measure the difference between the transformed point cloud and the target point cloud; the optimal R and t are solved by minimizing E(R,t); Step 2.3: During the fusion process, the extended Kalman filter (EKF) is used to estimate the system state. The state vector x includes position, velocity, acceleration, and rotation angle. The update formula of the Kalman filter is: x k+1 =Ax k +Bu k +w k y k =Cx k +v k Where A is the state transfer matrix and B is the control matrix u k At time k, input w k is the noise at time k; y k is the observation vector at time k, C is the observation matrix, v k is the observation noise; Step 2.4: Use an optimization method to jointly optimize the IMU and LiDAR data to obtain the optimal positioning result of the fusion of the two. The optimization problem is to minimize the following error function: Among them, h i (x) is the observation model, z i is the observation value, x is the state vector; Φ(x) is the error function, which is used to measure the difference between the observation model and the observation value.

5. The multi-sensor fusion pedestrian indoor and outdoor passive intelligent navigation and positioning method according to claim 2 is characterized in that: In step 3, the visual odometry (12) is used to scan the mapping data, complete the trajectory reconstruction and path planning through the semantic VSLAM assisted matching algorithm, and output the navigation information, as follows: Step 3.1: Output the category of each pixel through the convolutional neural network, and use the object detection algorithm to identify and track dynamic and static objects. Static objects are used for mapping, and dynamic objects are ignored or processed separately. Semantic-assisted V-SLAM uses semantic information to optimize motion estimation. The formula is as follows: Where T is the pose transformation matrix, and are the positions of the jth feature point in the image at the i-th frame and the i+1-th frame respectively; Step 3.2, the map update formula is as follows: M(t)=M(t-1)∪{p i ,c i } Among them, Μ(t) is the semantic map at the tth moment, p i is the object position, c i is the object category; Step 3.3: Use graph optimization to optimize the backend. The goal of the optimization problem is to minimize the error function, where the error includes the error in camera pose and the error in object position. The optimization objective function is as follows: Among them, T i and is the camera pose before and after optimization, p j and It is the position of the object before and after optimization.

6. The multi-sensor fusion pedestrian indoor and outdoor passive intelligent navigation and positioning method according to claim 2 is characterized in that: In step 4, the positioning and navigation according to the foot IMU (14) is performed by using the characteristics of the pedestrian's foot and correcting the navigation positioning error through zero-speed correction, as follows: Step 4.1: The inertial pedestrian navigation system collects pedestrian acceleration and angular velocity information through the IMU and uses the strapdown inertial navigation algorithm to calculate the pedestrian's posture information; The error model of the strapdown inertial navigation system is: Among them, δv n ,δp n and They are velocity error, position error and attitude angle error respectively; is the projection of the angular velocity of the navigation system relative to the inertial system in the navigation system; and are the projections of the Earth's rotation angular velocity and position rate in the navigation system respectively; and are the projections of the Earth's rotation angular velocity and position rate error in the navigation system; v n is the velocity of the carrier relative to the earth; To convert the carrier coordinate system into the attitude matrix of the navigation system; g n , δg n are the local gravity acceleration vector and the local gravity acceleration vector error respectively; f b 、 are the measurements of the accelerometer and gyroscope respectively; δf b 、 are the measurement errors of the accelerometer and gyroscope respectively; According to the strapdown inertial navigation system error model, position, velocity and attitude errors are selected to establish the system state error vector: The system state equation is: δX k =F k / k-1 δX k-1 +Γ k / k-1 W k-1 Among them, δX k is the system error state vector at time k; W k-1 is the system noise vector; Γ k / k-1 is the system noise driving matrix; F k / k-1 is the state transfer matrix, and its expression is as shown in the formula: The observation equation of the system is: Z k =H k X k +V k Among them, Z k is the system observation vector; V k is the system observation noise vector; H k is the system measurement matrix; Step 4.2: Based on the characteristics of pedestrian movement, the foot moves regularly during the movement process, and the foot speed is zero from the moment of contact to the moment of lift-off. The zero-speed intervals in different movement states are identified and recorded using the adaptive threshold method. The statistics are based on the maximum generalized likelihood method, and the formula is as follows: Where T(Z n ) is the test statistic; are the observation noise variances of acceleration and angular velocity respectively; and are the output values ​​of acceleration and angular velocity at time k respectively; n is the number of sampling points; N is the window size; is the mean of the continuous acceleration within the window; g is the acceleration due to gravity; The formula for calculating the adaptive threshold is: Among them, γ a (n) the minimum value of the test statistic; q is the conditional parameter; is the mean of the test statistic within the window, if T(Z n ) is less than γ a (n), then it is the zero speed interval; Step 4.3: Based on the characteristics of the zero-speed interval, a filtering algorithm is designed to eliminate the speed error by using the zero-speed pseudo-observation. When the zero-speed interval is detected, the EKF is used for filtering correction. System observation matrix: in is the velocity error, is the heading error; System measurement matrix H k for: According to the integral of the speed change, the corresponding position information is obtained.

7. The multi-sensor fusion pedestrian indoor and outdoor passive intelligent navigation and positioning method according to claim 2 is characterized in that: In step 5, the chest IMU (13) is used as the navigation and positioning spatial reference, and the other sensor data are synchronized with it in time and space, as follows: Step 5.1: Perform initial alignment using the optimized alignment method (OBA). OBA completes initial alignment on both the static and moving bases, decomposing the attitude matrix into a constant matrix and two time-varying matrices: The carrier motion term in the observation vector is compensated by the external auxiliary information BDS, and the influence of the accelerometer random noise is suppressed by the integration process; In the absence of external auxiliary information, the moving base OBA method is used. First, the high-frequency interference and motion interference of the gravity apparent motion vector are filtered out by online wavelet denoising, and then the initial alignment is completed by the OBA method. Step 5.2: Align the initial posture matrices of other sensors to a unified coordinate system.

8. The multi-sensor fusion pedestrian indoor and outdoor passive intelligent navigation and positioning method according to claim 2 is characterized in that: Step 6, based on the intelligent algorithm, checks whether each sensor is working properly and whether the data output is normal, as follows: Step 6.1: Using the time window method, if no sensor data is received within the set maximum time window, the sensor is considered to have a hard fault and will not be used in the fusion algorithm; Step 6.2: According to the algorithm settings, detect whether the sensor output data is abnormal, calculate the mean and standard deviation of the data in a sliding window, and check whether the current data point exceeds the threshold. If it exceeds the threshold, it is considered that the sensor output data has a hard fault, and the sensor output data will not be used in the fusion algorithm.

9. The multi-sensor fusion pedestrian indoor and outdoor passive intelligent navigation and positioning method according to claim 2, characterized in that: According to the adaptive federated filtering intelligent fusion algorithm described in step 7, the optimal sensor at the current moment is selected for fusion navigation by judging the confidence level and working status of each sensor. The details are as follows: Step 7.

1. The federated filtering algorithm consists of two filtering levels: a local sub-filter and a main filter. Each local sub-filter performs Kalman filtering operations independently and in parallel. The sub-filter obtains only the best local estimate of the system's common state to obtain a global estimate of the system's common state. The initial information includes the global estimation error variance matrix P and the system's process noise variance matrix Q at the system's initial moment. Information allocation is the process of allocating information between each sub-system filter and the main filter. Through information distribution, the estimation of the system error variance matrix P, the estimation of the process noise variance matrix Q and the filtering state X are distributed to different subsystem filters; the information distribution formula is: Among them, Q i (k) is the process noise variance matrix of the filter of the i-th subsystem at time k; Q g (k) is the system error variance matrix of the i-th subsystem filter at time k, P i (k) is the global system error variance matrix, P g (k) is the global system error variance matrix; is the state estimate of the filter of the ith subsystem at time k, is the global state estimate; β i is the information allocation factor of each filter, and the global process noise variance matrix satisfies information conservation: Step 7.2, information update, includes information time update and information measurement update. The time update process is to ensure that the calculation of the sub-filter and the main filter is not disturbed and can be executed independently under the condition of time change. The specific filtering algorithm is: in, is the prior estimate of the state of the i-th sub-filter at time k based on time k-1; ψ(k,k-1) is the state transfer matrix, describing the state transition relationship from time k-1 to time k; P i (k|k-1) is the prior estimate of the error covariance of the i-th sub-filter at time k based on time k-1, P i (k-1) is the error covariance matrix of the i-th sub-filter at the k-th moment; ψ T is the transposed matrix; Q i (k-1) is the process noise variance matrix of the i-th sub-filter at time k-1; The measurement update process is that the main filter does not have measurement updates, only the sub-filters have measurement updates. The algorithm is as follows: Among them, P i -1 (k) is the inverse of the error covariance matrix of the i-th sub-filter at time k, P i -1 (k|k-1) is the inverse of the error covariance matrix of the i-th sub-filter at time k based on time k-1, H i T (k) is the observation matrix H of the i-th sub-filter at time k i The transpose of (k), R i (k) is the observation noise variance matrix of the i-th sub-filter at time k, K i (k) is the Kalman gain of the i-th sub-filter at time k; Step 7.3: A more complete global optimal estimate is obtained by fusing the estimated information of each sub-filter. The fusion algorithm is: Among them, P g is the error covariance matrix after global fusion, [P1 -1 +P2 -1 +........P m -1 +P n -1 ] is the inverse of the error covariance matrix of each sub-filter, X g is the state estimate after global fusion, X1.......X n is the state estimate of each sub-filter; Step 7.4: According to the working status of all sensors at the current moment, select the optimal sensor positioning result for fusion positioning.

10. The multi-sensor fusion pedestrian indoor and outdoor passive intelligent navigation and positioning method according to claim 2, characterized in that: The handheld terminal (2) application software described in step 8 performs human-computer interaction and displays positioning information, as follows: Click the "Communication Connection" button to connect to the LAN or Bluetooth, and wait for the set seconds for all sensor data to be displayed in real time on the interface. According to the status color of the signal light, if it is green, it means the connection is normal, if it is red, it means the connection is abnormal and the operation is abnormal; By selecting indoor or outdoor initialization, initialization can be achieved under the condition of whether there is a satellite signal; By clicking "Start Navigation", the track will be displayed on the interface. Indoor navigation will be based on the initial forward direction as the north direction, while outdoor navigation will be based on satellite signals. By "storing data", the navigation information of each device can be stored on the handheld terminal.

Images