A lightweight visual and inertial superposition pedestrian localization and composition method in closed environments

Abstract

The present invention discloses a lightweight visual and inertial superposition pedestrian positioning and composition method in a closed environment, and uses the Monte Carlo method to calculate the pedestrian position s of N particles at time t. t Perform prediction sampling #imgabs0#Use the VLAD algorithm to perform prediction sampling on the current image I i Feature Extraction VLAD (I i ) and match it with the existing image features f(I) to obtain a sequence of matching particle pose pairs #imgabs1# and construct a grid map Θ for each particle based on the pose #imgabs2# [m] A map observation model is established and the particle weights of the sequence #imgabs3# are updated in combination with the matched particle poses to correct positioning and composition errors. The present invention can realize real-time positioning and composition based on the vision and inertia of pedestrians in a low-cost processor, which reduces the cost of the visual and inertial pedestrian positioning system and is suitable for engineering applications.

Description

Technical Field

[0001] The present invention belongs to the technical field of vision and inertial pedestrian navigation systems, and in particular relates to a lightweight vision and inertial superposition pedestrian positioning and composing method in a closed environment. Background Art

[0002] In special and complex scenarios such as subways, underground pipelines, and mines, special operations personnel are usually in a closed environment. Because navigation satellite signals are easily interfered with, blocked, and the signals are discontinuous, satellite navigation is difficult to meet the positioning needs of special operations personnel. Secondly, the environment is unknown and complex, and operators need to grasp the current environmental map information in real time, which poses new challenges to the environmental perception capabilities of the positioning and navigation system.

[0003] Navigation and positioning technology based on inertial sensors has the characteristics of small size, low cost, low power consumption and strong anti-interference, but due to factors such as sensor noise, its positioning error accumulates over time. Therefore, how to eliminate the accumulated error of the inertial system during pedestrian movement and achieve long-term stable and reliable pedestrian positioning in a closed environment is of great significance to improving the practicality of the inertial pedestrian positioning system.

[0004] The Simultaneous Localization and Mapping (SLAM) algorithm is a method that constructs an environmental map while positioning and uses the constructed map to correct positioning errors. It can effectively eliminate the cumulative errors of the positioning system in a closed environment and make up for the shortcomings of the inertial navigation system that the errors accumulate over time. However, the current SLAM system with vision, lidar and other sensor devices as the core is large in size and high in cost. It has high requirements on the computing power of the information processing system and is difficult to be widely used in pedestrian positioning. Therefore, it is of outstanding application value to construct a lightweight vision and inertial superposition pedestrian SLAM framework, which can achieve a long-endurance, high-reliability, low-cost lightweight pedestrian real-time positioning and mapping system while ensuring the high-precision positioning performance of the vision and inertial fusion navigation system. Summary of the invention

[0005] Purpose of the invention: The present invention provides a lightweight visual and inertial superposition pedestrian positioning and mapping method in a closed environment, which can meet the needs of low-cost, high-precision, and high-reliability real-time positioning and navigation based on pedestrians in closed spaces such as subways, underground pipelines, and mines.

[0006] Technical solution: The lightweight visual and inertial superposition pedestrian positioning and composition method in a closed environment described in the present invention includes the following steps:

[0007] (1) Use N particles to sample the pedestrian’s position s at time t through the Monte Carlo sampling method. t Perform predictive sampling

[0008] (2) Based on the VLAD algorithm, the current pose s t The corresponding image I i Perform feature extraction VLAD (I i ) and the image feature sequence A I If the image feature matches successfully, then for each particle, Screen the matching point pairs to obtain the matching particle pose pair sequence

[0009] (3) According to the position of the mth particle at time t Update the particle's grid map Θ [m] , according to the grid map Θ [m] The map observation model is established based on the grid occupancy times in;

[0010] (4) Sequence according to particle position Calculate particle image matching probability

[0011] (5) The map observation model obtained in step (3) is combined to update the particle weights to correct the positioning and composition errors.

[0012] Furthermore, the step (1) comprises the following steps:

[0013] (11) If time t is the initial time t0, the initial postures of N particles are Assignment:

[0014]

[0015] In the formula are the x and y coordinates of the pedestrian at the initial moment, is the pedestrian heading angle at the initial moment, the superscript [m] represents the mth particle, and the superscript T represents the transpose of the matrix;

[0016] (12) If time t is not the initial time t0, then the pedestrian posture distribution s at time t is t Perform Monte Carlo sampling; control the step-heading change vector u at time t t Perform Monte Carlo sampling

[0017] u t =[d t Δψ t ] T

[0018]

[0019] Where d is the step size, Δψ is the heading angle change, and subscript t is time. σ d is the step measurement noise standard deviation, σ Δψ is the standard deviation of heading change measurement noise, N represents normal distribution;

[0020] (13) Establish a posture estimation model for pedestrian motion based on step length-heading changes, and calculate the posture at time t-1 and the control vector sample obtained in step (12) For the pedestrian pose s at time t t Perform predictive sampling

[0021] s t =[x t y t ψ t ] T

[0022]

[0023]

[0024] In the formula, x is the direction coordinate, y is the direction coordinate, ψ is the heading angle, and the subscript t represents time. is the pedestrian pose prediction sampling of the mth particle at time t, Δs [m] is the position change vector of the mth particle.

[0025] Furthermore, the step (2) comprises the following steps:

[0026] (21) Extract image I i The M D-dimensional ORB features q 1:M ;

[0027] (22) Use the K-means algorithm to cluster the images in the image library and obtain K cluster centers c 1:k ;

[0028] (23) Image I i The M D-dimensional ORB features q 1:M Convert to global feature f VLAD (I i ) and added to the image feature sequence A I ;

[0029]

[0030]

[0031] Among them, q irepresents the i-th ORB feature, c k represents the kth cluster center, q i With c k They are all D-dimensional vectors; Expressing when ||q i -c k ||The value of k at the minimum, f VLAD (I i ) represents image I i VLAD global features:

[0032] A I =[A I f VLAD (I i )]

[0033] Among them, A I Represents a sequence of image features;

[0034] (24) Image I i The global features and the first i-1 image features A in the image feature sequence I (1:i-1) calculates the Euclidean distance d(I i ,I 1:i-1 ) and take the minimum value d min , the smaller the distance, the higher the image similarity;

[0035] d min =min{d(I i ,I j )},j=1,2,…,i-1

[0036] d(I i ,I j )=||f VLAD (I i )-f VLAD (I j )||

[0037] Among them, f VLAD (I i ) is image I i The global features obtained in step (2.2);

[0038] (25) If the minimum Euclidean distance d min Less than the threshold Tur pic , and for a single particle, image I i With image I j The corresponding position distance is less than the threshold Tur pos , then for this particle, image I i With image I j Match and map the particle poses corresponding to the pair of images Add to the matched particle pose pair sequence middle:

[0039]

[0040]

[0041] in, It means that for the mth particle, image I i With image I j The corresponding particle pose and distance.

[0042] Furthermore, the implementation process of step (3) is as follows:

[0043] (31) The environment is divided into square grids, and the mth particle map set is expressed as: in represents the i-th grid of the m-th particle, is the total number of grids in the mth particle map; where each map grid Contains raster index Occupancy times The positions of m particles at time t Update grid map Θ [m] The process is as follows:

[0044] If there is a raster index in the raster map Particle pose The index sequence The same, then the map grid occupies the number of Add 1, and the formula is as follows:

[0045]

[0046] If the grid map does not contain the particle pose The index sequence If the grid index is the same, a new grid is created for the grid map. And perform raster indexing and occupancy times Assignment, the formula is as follows:

[0047]

[0048]

[0049] In the formula, and They represent the x-direction coordinate and y-direction coordinate of the m-th particle at time t, <·> represents rounding operation, and l is the side length of the square grid;

[0050] (32) The probability of map accessibility for the grid where the mth particle is located at time t is The observation model is as follows:

[0051]

[0052]

[0053]

[0054] Where e is the natural logarithm, n [m] is the number of occupancy of the grid where the mth particle is located, p(θ i |z t ,x t ) is the counter-observation model of inertial information to the environment.

[0055] Furthermore, the implementation process of step (4) is as follows:

[0056] If the particle's current position Not in sequence In If the particle's current position In sequence In the figure, according to the posture And the sequence The matching pose in Calculate image matching probability The formula is as follows:

[0057]

[0058]

[0059]

[0060] Furthermore, the implementation process of step (5) is as follows:

[0061] The map traversability probability calculated according to step (3) and particle image matching probability Update particle weights to correct positioning and composition errors; for each particle m, m = 1, 2, ..., N, the weight at time t As shown below:

[0062]

[0063]

[0064] According to the updated weights, the expected pose of all particles can be obtained, that is, the corrected expected pose s′ t :

[0065]

[0066] Beneficial effects: Compared with the prior art, the beneficial effects of the present invention are: the present invention subverts the current visual and inertial navigation algorithm structure with visual odometer as the core, and greatly reduces the algorithm calculation amount while ensuring high positioning accuracy and strong map perception ability. The low computing cost enables the technology to be carried on portable mobile devices, and can achieve lightweight, high-precision, low-computing-cost real-time positioning and composition; the present invention can meet the needs of low-cost, high-precision, and high-reliability real-time positioning and navigation based on pedestrians in closed spaces such as subways, underground pipelines, and mines. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] Figure 1 This is the flow chart of this article;

[0068] Figure 2 This is a comparison chart between the positioning results and the original inertial odometer results. DETAILED DESCRIPTION

[0069] The present invention is further described in detail below with reference to the accompanying drawings.

[0070] The present invention proposes a lightweight visual and inertial superposition pedestrian positioning and composition method in a closed environment. Figure 1 As shown, the specific steps include:

[0071] Step 1: Using particle filtering as a framework, use N particles to perform Monte Carlo sampling on the pedestrian pose s at time t. t Perform predictive sampling

[0072] If time t is the initial time t0, the initial poses of N particles are Assignment.

[0073]

[0074] In the formula are the x and y coordinates of the pedestrian at the initial moment, is the pedestrian heading angle at the initial moment, the superscript [m] represents the mth particle, and the superscript T represents the transpose of the matrix.

[0075] If time t is not the initial time t0, then the pedestrian posture distribution s at time t t Perform Monte Carlo sampling.

[0076] First, the step-heading change control vector u at time t t Perform Monte Carlo sampling Step length - heading change control vector u t The specific formula is as follows:

[0077] u t =[d t Δψ t ] T

[0078]

[0079] Where d is the step size, Δψ is the heading angle change, and the subscript t represents time. σ d is the step measurement noise standard deviation, σ Δψ is the standard deviation of the heading change measurement noise, and N represents the normal distribution.

[0080] Then, a pose estimation model based on step length and heading change is established for pedestrian motion. and control vector sampling For the pedestrian pose s at time t t Perform predictive sampling

[0081] The specific formula is as follows:

[0082] s t =[x t y t ψ t ] T

[0083] In the formula, s t is the pedestrian posture, x is the direction coordinate, y is the direction coordinate, ψ is the heading angle, and the subscript t represents time.

[0084] The pose estimation model based on step length-heading change is shown in the following formula:

[0085] s t =s t-1 +Δs

[0086] Δs=[d t cosψ t-1 d t sinψ t-1 Δψ t ] T

[0087] Where Δs is the pose change vector. The sampling result of the pedestrian pose at time t-1 is And the step length at time t-the sampling result of the heading change control vector Pedestrian pose prediction sampling of the mth particle at time t The formula is as follows:

[0088]

[0089]

[0090] Step 2: Based on the VLAD algorithm, the current pose s t The corresponding image I i Perform feature extraction fVLAD(I i And with the image feature sequence A I If the image feature matches successfully, then for each particle, Screen the matching point pairs to obtain the matching particle pose pair sequence The specific steps are as follows:

[0091] (2.1) Extract image I i The M D-dimensional ORB features q 1:M .

[0092] Select pixel p in the image, the brightness of the pixel is b p , taking pixel p as the center, select 16 pixels on a circle with a radius of 3; if the selected circle consists of a continuous Num p The brightness of the point is greater than b p +Tur p or less than b p -Tur p , then pixel p is the feature point; where Num p To determine whether it is a feature point, take Num p =12,Tur p is the brightness judgment threshold, take Tur p =1.2b p ; For the i-th feature point, take the pixel corresponding to the feature point as the center, randomly select a pair of pixels j and k in a given neighborhood. If the brightness b of pixel j is j Greater than the brightness b of pixel k k , then the eigenvalue corresponding to the pair of pixels is 1, otherwise it is 0.

[0093] q i (n)=1,b j >b k

[0094] q i (n)=0,b j ≤ b k,n=1,2,…,D

[0095] Among them, D is the number of randomly selected pixel points, which is also the dimension of the feature vector corresponding to the ORB feature point, q i Represents the feature vector of the i-th ORB feature point.

[0096] (2.2) Cluster the images in the image library using the K-means algorithm to obtain K cluster centers c 1:k .

[0097] 1) Extract ORB features from all images in the image library, assuming that the total number of ORB features is N;

[0098] 2) Randomly select K center points c1,…,c k ,The data type and data dimension of the center point are the same as the feature vector corresponding to the ORB feature point;

[0099] 3) For each ORB feature, calculate the distance between it and each center point, and take the smallest one as its classification;

[0100] 4) Take the average of the ORB features belonging to the same class to obtain the new center point of the class;

[0101] 5) If the center point of each class changes very little, the algorithm converges; otherwise, return to step 3).

[0102] (2.3) Image I i The M D-dimensional ORB features q 1:M Convert to global feature f VLAD (I i ) and added to the image feature sequence A I ;

[0103]

[0104] In the formula, q i represents the i-th ORB feature, c k represents the kth cluster center, q i With c k are all D-dimensional vectors. k (x i ) is a symbolic function, if q i Does not belong to cluster center c k , a k (x i )=0; if q i Belongs to cluster center c k , a k (x i )=1,f VLAD (I i) represents image I i VLAD global features.

[0105] A I =[A I f VLAD (I i )].

[0106] (2.4) Image I i The global features and the first i-1 image features A in the image feature sequence I (1:i-1) calculates the Euclidean distance d(I i ,I 1:i-1 ) and take the minimum value d min , the smaller the distance, the higher the image similarity.

[0107] d min =min{d(I i ,I j )},j=1,2,…,i-1

[0108] d(I i ,I j )=||f VLAD (I i )-f VLAD (I j )||.

[0109] (2.5) If the minimum Euclidean distance d min Less than the threshold Tur pic , and for a single particle, image I i With image I j The corresponding position distance is less than the threshold Tur pos , then for this particle, image I i With image I j Match and map the particle poses corresponding to the pair of images Add to the matched particle pose pair sequence The specific formula is as follows:

[0110]

[0111]

[0112] in, It means that for the mth particle, image I i With image I j The corresponding particle pose and distance.

[0113] Step 3: According to the pose sequence Construct a grid map Θ for each particle [m] , according to the constructed grid map Θ [m] The map observation model is established based on the occupancy frequency of each grid in .

[0114] The environment is divided into square grids, and the mth particle map set is expressed as: in represents the i-th grid of the m-th particle, is the total number of grids in the mth particle map; where each map grid Contains raster index Occupancy times The positions of m particles at time t Update grid map Θ [m] The process is as follows:

[0115] If there is a raster index in the raster map Particle pose The index sequence The same, then the map grid occupies the number of Add 1, and the formula is as follows:

[0116]

[0117] If the grid map does not contain the particle pose The index sequence If the grid index is the same, a new grid is created for the grid map. And perform raster indexing and occupancy times Assignment, the formula is as follows:

[0118]

[0119]

[0120] In the formula, and They represent the x-direction coordinate and y-direction coordinate of the m-th particle at time t, <·> represents rounding operation, and l is the side length of the square grid;

[0121] The probability of map accessibility for the grid where the mth particle is located at time t The observation model is as follows:

[0122]

[0123]

[0124]

[0125] Where e is the natural logarithm, n [m] is the number of occupancy of the grid where the mth particle is located, p(θ i |z t ,x t ) is the counter-observation model of inertial information to the environment.

[0126] Step 4: According to the matched particle pose sequence obtained in step 2 Calculate particle image matching probability

[0127] If the particle's current position Not in sequence In If the particle's current position In sequence In the figure, according to the posture And the sequence The matching pose in Calculate image matching probability The formula is as follows:

[0128]

[0129]

[0130]

[0131] Step 5: Combine the map observation model obtained in step 3 to update the particle weights to correct the positioning and composition errors.

[0132] The probability of map accessibility calculated according to step 3 and particle image matching probability Update particle weights to correct positioning and composition errors; for each particle m, m = 1, 2, ..., N, the weight at time t As shown below:

[0133]

[0134]

[0135] According to the updated weights, the expected pose of all particles can be obtained, that is, the corrected expected pose:

[0136]

[0137] like Figure 2As shown, the experimental scene is the corridor and room in the experimental building. The experimental building covers an area of ​​about 2500 square meters. The total length of the route is 889.2 meters. The tester walks steadily along the corridor. The route is approximately rectangular. After a period of movement, he returns to the starting point. The positioning result using the original inertial odometer is shown as the dotted line in the figure, and the positioning result using the lightweight visual and inertial superposition pedestrian SLAM method in a closed environment of the present invention is shown as the solid line in the figure; Figure 2 It can be seen that due to the noise of the inertial odometer, the positioning error of the pedestrian positioning system using only the inertial odometer increases with the increase of the movement distance; in comparison, the positioning system using the present invention can effectively estimate the error of the positioning result because it forms a direct observation of the position and posture by constructing a map while positioning. Figure 2 It can be seen that the positioning trajectory of the present invention has high repeatability and the positioning error does not accumulate over time. Finally, the positioning error of the original inertial odometer positioning system is about 30.70 meters; the positioning error of the positioning system using the present invention, i.e. the lightweight visual and inertial superposition pedestrian SLAM method in a closed environment, is about 2.95 meters, which significantly improves the positioning accuracy.

[0138] The above are only preferred embodiments of the present invention, and the protection scope of the present invention is not limited to the above embodiments. All technical solutions under the concept of the present invention belong to the protection scope of the present invention. It should be pointed out that for ordinary technicians in this technical field, some improvements and modifications without departing from the principle of the present invention should be regarded as the protection scope of the present invention.

Claims

1. A lightweight visual and inertial superposition pedestrian positioning and composition method in a closed environment, characterized in that: The following steps are involved: (1) Use N particles to sample the pedestrian’s position s at time t using the Monte Carlo sampling method. t Perform predictive sampling (2) Based on the VLAD algorithm, the current pose s t The corresponding image I i Perform feature extraction VLAD (I i ) and the image feature sequence A I If the image feature matches successfully, then for each particle, Screen the matching point pairs to obtain the matching particle pose pair sequence (3) According to the position of the mth particle at time t Update the particle's grid map Θ [m] , according to the grid map Θ [m] The map observation model is established based on the grid occupancy times in; (4) Sequence according to particle position Calculate particle image matching probability (5) updating the particle weights based on the map observation model obtained in step (3) to correct the positioning and composition errors; The implementation process of step (3) is as follows: (31) The environment is divided into square grids, and the mth particle map set is expressed as: in represents the i-th grid of the m-th particle, is the total number of grids in the mth particle map; where each map grid Contains raster index Occupancy times The positions of m particles at time t Update grid map Θ [m] The process is as follows: If there is a raster index in the raster map Particle pose The index sequence The same, then the map grid occupies the number of Add 1, and the formula is as follows: If the grid map does not contain the particle pose The index sequence If the grid index is the same, a new grid is created for the grid map. And perform raster indexing and occupancy times Assignment, the formula is as follows: In the formula, and They represent the x-direction coordinate and y-direction coordinate of the m-th particle at time t, <·> represents rounding operation, and l is the side length of the square grid; (32) The probability of map accessibility for the grid where the mth particle is located at time t is The observation model is as follows: Where e is the natural logarithm, n [m] is the number of occupancy of the grid where the mth particle is located, p(θ i |z t ,x t ) is the counter-observation model of inertial information to the environment.

2. The method for positioning and mapping pedestrians by superimposing light-weight vision and inertia in a closed environment according to claim 1, characterized in that: The step (1) comprises the following steps: (11) If time t is the initial time t0, the initial postures of N particles are Assignment: In the formula are the x and y coordinates of the pedestrian at the initial moment, is the pedestrian heading angle at the initial moment, the superscript [m] represents the mth particle, and the superscript T represents the transpose of the matrix; (12) If time t is not the initial time t0, then the pedestrian posture distribution s at time t is t Perform Monte Carlo sampling; control vector u of step length-heading change at time t t Perform Monte Carlo sampling u t =[d t Δψ t ] T Where d is the step size, Δψ is the heading angle change, and subscript t is time. σ d is the step measurement noise standard deviation, σ Δψ is the standard deviation of heading change measurement noise, N represents normal distribution; (13) Establish a posture estimation model for pedestrian motion based on step length-heading changes, and calculate the posture at time t-1 and the control vector sample obtained in step (12) For the pedestrian pose s at time t t Perform predictive sampling s t =[x t y t ψ t ] T In the formula, x is the direction coordinate, y is the direction coordinate, ψ is the heading angle, and the subscript t represents time. is the pedestrian pose prediction sampling of the mth particle at time t, Δs [m] is the position change vector of the mth particle.

3. The method for positioning and mapping pedestrians by lightweight visual and inertial superposition in a closed environment according to claim 1, characterized in that: The step (2) comprises the following steps: (21) Extract image I i The M D-dimensional ORB features q 1:M ; (22) Use the K-means algorithm to cluster the images in the image library and obtain K cluster centers c 1:k ; (23) Image I i The M D-dimensional ORB features q 1:M Convert to global feature f VLAD (I i ) and added to the image feature sequence A I ; Among them, q i represents the i-th ORB feature, c k represents the kth cluster center, q i With c k They are all D-dimensional vectors; Expressing when ||q i -c k ||The value of k when it is the smallest, f VLAD (I i ) represents image I i VLAD global features: A I =[A I f VLAD (I i )] Among them, A I Represents a sequence of image features; (24) Image I i The global features and the first i-1 image features A in the image feature sequence I (1:i-1) calculates the Euclidean distance d(I i ,I 1:i-1 ) and take the minimum value d min , the smaller the distance, the higher the image similarity; d min =min{d(I i ,I j )},j=1,2,…,i-1 d(I i ,I j )=||f VLAD (I i )-f VLAD (I j )|| Among them, f VLAD (I i ) is image I i The global features obtained in step (2.2); (25) If the minimum Euclidean distance d min Less than the threshold Tur pic , and for a single particle, image I i With image I j The corresponding position distance is less than the threshold Tur pos , then for this particle, image I i With image I j Match and map the particle poses corresponding to the pair of images Add to the matched particle pose pair sequence middle: in, It means that for the mth particle, image I i With image I j The corresponding particle pose and distance.

4. The method for positioning and mapping pedestrians by lightweight visual and inertial superposition in a closed environment according to claim 1, characterized in that: The implementation process of step (4) is as follows: If the particle's current position Not in sequence In If the particle's current position In sequence In the figure, according to the posture And the sequence The matching pose in Calculate image matching probability The formula is as follows:

5. The method for positioning and mapping pedestrians by lightweight visual and inertial superposition in a closed environment according to claim 1, characterized in that: The implementation process of step (5) is as follows: The map traversability probability calculated according to step (3) and particle image matching probability Update particle weights to correct positioning and composition errors; for each particle m, m = 1, 2, ..., N, the weight at time t As shown below: According to the updated weights, the expected pose of all particles can be obtained, that is, the corrected expected pose s' t :

Images