CN114440873B - Inertial pedestrian SLAM method for magnetic field superposition in closed environment - Google Patents

Inertial pedestrian SLAM method for magnetic field superposition in closed environment Download PDF

Info

Publication number
CN114440873B
CN114440873B CN202111655225.7A CN202111655225A CN114440873B CN 114440873 B CN114440873 B CN 114440873B CN 202111655225 A CN202111655225 A CN 202111655225A CN 114440873 B CN114440873 B CN 114440873B
Authority
CN
China
Prior art keywords
pedestrian
grid
moment
map
particle
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.)
Active
Application number
CN202111655225.7A
Other languages
Chinese (zh)
Other versions
CN114440873A (en
Inventor
丁一鸣
熊智
崔岩
李婉玲
曹志国
王铮淳
李晓东
陈芷心
孙银收
张苗
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202111655225.7A priority Critical patent/CN114440873B/en
Publication of CN114440873A publication Critical patent/CN114440873A/en
Application granted granted Critical
Publication of CN114440873B publication Critical patent/CN114440873B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/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/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3807—Creation or updating of map data characterised by the type of data
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3859—Differential updating map data

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention discloses an inertial pedestrian SLAM method for magnetic field superposition in a closed environment. Belongs to the technical field of inertial/magnetic field pedestrian navigation systems, and comprises the following specific steps: predicting and sampling the pose s t of the pedestrian at the moment t by using N particles through a Monte Carlo sampling methodPredictive sampling from pedestrian poseConstructing a grid map Θ [m] for each particle, then constructing a consistency criterion of the constructed grid map Θ [m] by using a magnetic field measurement mag t, and judging the consistency criterion of each particleEstablishing a map observation model, and judging the result according to the consistency of the map grid modelUpdating the grid map; based on the consistency discrimination resultAnd the established map observation model realizes the correction of positioning and composition errors through weight updating. The invention solves the problem of accumulation of errors of the inertial pedestrian positioning system in a closed environment along with time, has the advantages of good real-time performance, small operand and strong long-time positioning stability, improves the positioning precision and stability of the inertial pedestrian positioning system, and is suitable for engineering application.

Description

Inertial pedestrian SLAM method for magnetic field superposition in closed environment
Technical Field
The invention belongs to the technical field of inertial/magnetic field pedestrian navigation systems, and relates to an inertial pedestrian SLAM method for magnetic field superposition in a closed environment.
Background
In the prior art, the pedestrian positioning information of the closed environment is an important guarantee for the special operators to safely and efficiently complete the operation tasks, and the special operators are usually in the closed environment under special complex scenes such as geological exploration, fire fighting and disaster relief. Because the navigation satellite signals are easy to interfere and shield and the signals are discontinuous, satellite navigation is difficult to meet the real-time and stable positioning requirements of special operators. Secondly, the environment is unknown and complex, and operators need to master the current environment map information in real time, which provides a new challenge for the environment perception capability of the positioning navigation system.
The navigation positioning technology based on the inertial sensor has the characteristics of small volume, low cost, low power consumption and strong anti-interference performance, but positioning errors are accumulated along with time due to factors such as sensor noise and the like. Aiming at the accumulated errors of the inertial pedestrian positioning system, the speed observation of the system is established by the aid of the periodic static characteristic in the pedestrian walking process, and the positioning errors of the inertial navigation system are corrected. However, since the speed-to-position observation can only inhibit the divergence of the position error, the positioning accumulated error cannot be eliminated, and the positioning error of the system using the method is accumulated over time, so that the long-time stable and reliable positioning cannot be realized. Therefore, how to eliminate the accumulated error of the inertial system in the moving process of the pedestrian realizes the long-time stable and reliable pedestrian positioning in the closed environment, and has great significance for improving the practicability of the inertial pedestrian positioning system.
An instant positioning and composition (SLAM) algorithm is a method for constructing an environment map while positioning and correcting positioning errors by utilizing the constructed map, can effectively eliminate accumulated errors of a positioning system in a closed environment, and can make up the defect that errors of an inertial navigation system are accumulated along with time. However, the current SLAM algorithm mainly uses devices such as inertia and laser radar as sensing sources, and the system has large volume, high power consumption and high requirement on the operation capability of an information processing system, and is difficult to popularize and apply in pedestrian positioning. Therefore, how to construct an SLAM algorithm frame by using simple information such as inertia, magnetic field and the like, and the realization of a long-time stable and reliable light-weight pedestrian autonomous positioning system has outstanding application value.
Disclosure of Invention
The invention aims to: the invention aims to provide an inertial pedestrian SLAM method for magnetic field superposition in a closed environment; the method can eliminate accumulated errors of the pedestrian inertial positioning system in closed environments such as indoor environments, underground environments, roadway environments and the like, and meets the long-time high-precision positioning requirement of the pedestrian positioning system.
The technical scheme is as follows: the invention relates to an inertial pedestrian SLAM method for magnetic field superposition in a closed environment, which comprises the following specific operation steps:
(1) Predicting and sampling the pedestrian pose s t at the moment t by using N particles through a Monte Carlo sampling method
(2) Predictive sampling according to pedestrian pose s t in step (1)Constructing a grid map Θ [m] for each particle, then constructing a consistency criterion of the constructed grid map Θ [m] by using a magnetic field measurement mag t, and judging a consistency criterion result of each particle
(3) Establishing a map observation model according to the occupation times of each grid in the constructed grid map theta [m];
And then according to the consistency discrimination result in the constructed grid map theta [m] Updating the grid map by combining the established map observation model;
(4) Combining the consistency discrimination results of the step (2) And (3) the map observation model established in the step (3) realizes the correction of positioning and composition errors through weight updating.
Further, in the step (1), the pedestrian pose s t at the time t is predicted and sampled by using N particles through a monte carlo sampling methodThe process of (1) is specifically as follows:
(1.1) performing Monte Carlo sampling on the pedestrian pose distribution s t-1 at the time t-1;
(1.2) performing Monte Carlo sampling on the step length-course change control vector u t at the t moment;
(1.3) establishing a dead reckoning probability model of the pedestrian movement process, and predicting the pedestrian pose s t in one step to finally obtain a predicted sample of the pedestrian pose s t at the moment t
Further, the pedestrian pose s t is specifically shown in the following formula:
st＝[xt,yt,ψt]T，
where x is a direction coordinate, y is a direction coordinate, ψ is a heading angle, and T is a transpose of the vector.
Further, in the step (1.1), the monte carlo sampling is performed on the pedestrian pose distribution s t-1 at the time t-1 specifically as shown in the following formula:
Wherein s t-1 is the pose vector of the pedestrian at the time t-1, u t-1 is the control vector at the time t-1, z t-1 is the observation vector at the time t-1, p (s t-1|zt-1,ut-1) is the pose distribution of the pedestrian at the time t-1, The sampling result of the m-th particle pedestrian pose vector at the t-1 moment,The weight of the mth particle, N is the total number of particles;
Further, in step (1.2), the monte carlo sampling is performed on the t moment step-heading change control vector u t according to the following formula:
Wherein,
Superscript [ m ] denotes the mth particle and
In the method, in the process of the invention,For the time t step measurement, σ d is the step measurement noise standard deviation,Measuring noise standard deviation for course change at the moment t by using a course change measurement value sigma Δψ, wherein N represents normal distribution; the step sampling result of the mth particle at the time t is shown, Representing the m-th particle course change sampling result at the time t; the heading of the mth particle at the t-1 moment, And controlling the vector sampling result for the step length-course change of the mth particle at the moment t.
Further, in the step (1.3), the dead reckoning probability model is specifically shown as the following formula:
Wherein s t is a pose vector of a pedestrian at t moment, s t-1 is a pose vector of the pedestrian at t-1 moment, u t is a t moment control vector, x t,yt,ψt is an x-direction coordinate, a y-direction coordinate and a course angle of the pedestrian at t moment respectively, x t-1,yt-1,ψt-1 is an x-direction coordinate, a y-direction coordinate and a course angle of the pedestrian at t-1 moment respectively, d t is a step size of the pedestrian at t moment, and Deltaψ t is a course change angle of the pedestrian at t moment;
in addition, according to the dead reckoning model, combining the sampling result of the pedestrian pose at the t-1 moment And t moment step length-course change control vector sampling resultPedestrian pose prediction sampling of mth particle at t momentThe formula is as follows:
Wherein, And the result is a predicted sampling result of the pose of the mth particle pedestrian at the moment t.
Further, in the step (2), the building grid map Θ [m] specifically is: the environment is partitioned using an equilateral square grid, expressed for the mth set of particle maps as: Wherein, An ith grid representing an mth particle,The total number of grids in the mth particle map;
Wherein, each map grid comprises three kinds of information including grid index, occupation times and magnetic field intensity; index to grid where mth particle is located at time t The method comprises the following steps:
in the method, in the process of the invention, AndRespectively representing the x-direction coordinate and the y-direction coordinate of the mth particle at the time t,
"· > Means rounding operation, l is the side length of a square grid; the initial occupation times of all grids are 0, and the magnetic field intensity is-1;
in addition, the consistence of the grid map model theta [m] constructed by the mth particle The criterion is shown as follows:
Wherein,
In the method, in the process of the invention,Representing the probability of coincidence at the mth particle T moment, T mag E (0, 1) being the coincidence discrimination threshold, mag t representing the magnetic field strength measured at the magnetometer T moment,Representing the grid at which the mth particle t is locatedThe stored magnetic field strengths, max { · } and min { · } represent the set maximum and minimum operations, respectively.
Further, in the step (3), the established map observation model means to establish a variation relationship between the passable probability of the map grid in the step (2) along with the one-step prediction of the pedestrian pose s t in the step (1) and the occupation times of the map grid in the step (2),
The method specifically comprises the following steps: map passability probability for grid where mth particle t moment is locatedThe observation model is shown as follows:
Wherein e is the natural logarithm,
Wherein,For the number of times the mth particle experiences the ith grid, p (θ i|zt,xt) is an inverse observation model of inertial information on the environment, taking p (θ i|zt,xt) =0.8;
In addition, the grid map update comprises a magnetic field update and an occupation frequency update;
The magnetic field update is to update the magnetic field intensity of the current grid in the map, and for the grid where the mth particle t moment is located Is the magnetic field strength of (2)The update rule is as follows:
the occupied times update, namely the number of times of the current grid is updated, and the number of times of the grid where the mth particle t is located is updated The update rule of (2) is as follows:
further, in the step (4), the specific process of updating the weight is as follows:
particle uniformity probability calculated according to step (3) Map passability probabilityUpdating the weight to correct positioning and composition errors; weights at time t for each particle m, m=1, 2, …, NThe following is shown:
Wherein the method comprises the steps of
And (3) according to the updated weights, the pose expectations of all particles, namely corrected pose expectations, can be obtained:
The beneficial effects are that: compared with the prior art, the invention carries out grid consistency judgment by constructing the environment occupation grid map and utilizing the magnetic field information, estimates the distribution of the environment occupation grid map by combining the inertia information, and simultaneously utilizes the estimated map distribution to restrain the positioning error of the inertia system so as to realize the real-time correction of the positioning error of the inertia positioning system. The method can well compensate the accumulated error of the inertial positioning system and improve the reliability and positioning precision of the inertial pedestrian positioning system.
Drawings
FIG. 1 is a schematic diagram of an inertial pedestrian SLAM method of magnetic field superposition in a closed environment of the present invention;
FIG. 2 is a graph comparing the positioning results with the raw inertial odometer results in an embodiment of the invention;
FIG. 3 is a graph comparing the heading estimation results with the heading estimation results of the original inertial odometer in an embodiment of the invention.
Detailed Description
In order to more clearly describe the technical scheme of the utility model, the technical scheme of the utility model is further described in detail below with reference to the accompanying drawings:
as shown in fig. 1, the method for inertially SLAM by magnetic field superposition in the closed environment comprises the following specific operation steps:
(1) Predicting and sampling the pedestrian pose s t at the moment t by using N particles through a Monte Carlo sampling method
(2) Predictive sampling according to pedestrian pose s t in step (1)Constructing a grid map Θ [m] for each particle, then constructing a consistency criterion of the constructed grid map Θ [m] by using a magnetic field measurement mag t, and judging a consistency criterion result of each particle
(3) Establishing a map observation model according to the occupation times of each grid in the constructed grid map theta [m];
And then according to the consistency discrimination result in the constructed grid map theta [m] Updating the grid map by combining the established map observation model;
(4) Combining the consistency discrimination results of the step (2) And (3) the map observation model established in the step (3) realizes the correction of positioning and composition errors through weight updating.
Further, in the step (1), the pedestrian pose s t at the time t is predicted and sampled by using N particles through a monte carlo sampling methodThe process of (1) is specifically as follows:
(1.1) performing Monte Carlo sampling on the pedestrian pose distribution s t-1 at the time t-1;
(1.2) performing Monte Carlo sampling on the step length-course change control vector u t at the t moment;
(1.3) establishing a dead reckoning probability model of the pedestrian movement process, and predicting the pedestrian pose s t in one step to finally obtain a predicted sample of the pedestrian pose s t at the moment t
Further, in the step (1), the pedestrian pose s t is specifically shown as the following formula:
st＝[xt,yt,ψt]T，
where x is a direction coordinate, y is a direction coordinate, ψ is a heading angle, and T is a transpose of the vector.
Further, in the step (1.1), the monte carlo sampling is performed on the pedestrian pose distribution s t-1 at the time t-1 specifically as shown in the following formula:
Wherein s t-1 is the pose vector of the pedestrian at the time t-1, u t-1 is the control vector at the time t-1, z t-1 is the observation vector at the time t-1, p (s t-1|zt-1,ut-1) is the pose distribution of the pedestrian at the time t-1, The sampling result of the m-th particle pedestrian pose vector at the t-1 moment,The weight of the mth particle, N is the total number of particles, p (s t-1|zt-1,ut-1 is the pose distribution of the pedestrian at the moment t-1, in particular,
p(s0|z0,u0)＝p(s0)～N(s0,Σ0)
Wherein s 0,Σ0 is the prior pose expectation and covariance matrix of the pedestrian at the initial moment, and N (s 0,Σ0) represents a Gaussian distribution with a mean value of s 0 and a variance of Σ 0.
Further, in step (1.2), the monte carlo sampling is performed on the t moment step-heading change control vector u t according to the following formula:
Wherein,
Superscript [ m ] denotes the mth particle and
In the method, in the process of the invention,For the time t step measurement, σ d is the step measurement noise standard deviation,Measuring noise standard deviation for course change at the moment t by using a course change measurement value sigma Δψ, wherein N represents normal distribution; the step sampling result of the mth particle at the time t is shown, Representing the m-th particle course change sampling result at the time t; the heading of the mth particle at the t-1 moment, And controlling the vector sampling result for the step length-course change of the mth particle at the moment t.
Further, in the step (1.3), the dead reckoning probability model is specifically shown as the following formula:
Wherein s t is a pose vector of a pedestrian at t moment, s t-1 is a pose vector of the pedestrian at t-1 moment, u t is a t moment control vector, x t,yt,ψt is an x-direction coordinate, a y-direction coordinate and a course angle of the pedestrian at t moment respectively, x t-1,yt-1,ψt-1 is an x-direction coordinate, a y-direction coordinate and a course angle of the pedestrian at t-1 moment respectively, d t is a step size of the pedestrian at t moment, and Deltaψ t is a course change angle of the pedestrian at t moment;
in addition, according to the dead reckoning model, combining the sampling result of the pedestrian pose at the t-1 moment And t moment step length-course change control vector sampling resultPedestrian pose prediction sampling of mth particle at t momentThe formula is as follows:
Wherein, And the result is a predicted sampling result of the pose of the mth particle pedestrian at the moment t.
Further, in the step (2), the building grid map Θ [m] specifically is: the environment is segmented using an equilateral square grid,
The mth particle map set is expressed as: Wherein, theta i [m] represents the ith grid of the mth particle, The total number of grids in the mth particle map;
Wherein, each map grid comprises three kinds of information including grid index, occupation times and magnetic field intensity; index to grid where mth particle is located at time t The method comprises the following steps:
in the method, in the process of the invention, AndRespectively representing the x-direction coordinate and the y-direction coordinate of the mth particle at the time t,
"· > Means rounding operation, l is the side length of a square grid;
probability of trafficability of all grids at initial time The magnetic field strength m is-1, wherein-1 indicates that no effective magnetic field information exists;
In addition, the purpose of the map grid consistency determination in the step (2) is to determine whether the magnetic field information of the grid where the pedestrian pose one-step prediction in the step (1) is consistent with the existing magnetic field information, and reflects the accuracy of the one-step prediction of the pedestrian pose, namely, calculate the consistency probability p (θ t＝θi|st,mt,mi) at the moment of the particle t, which is abbreviated as Wherein, theta t represents the grid where the pedestrian is according to the pose estimation of the moment t of the pedestrian, theta i is the corresponding grid stored in the map, m t represents the magnetic field intensity measured by the magnetometer at the moment t, and m i represents the magnetic field intensity stored in the corresponding grid in the map;
Consistency of grid map model Θ [m] constructed by mth particle The criterion is shown as follows:
in the method, in the process of the invention, T mag E (0, 1) is the consistency probability of the mth particle and is a consistency judgment threshold;
wherein, as a preferable scheme of the invention, the consistency probability is calculated Modeling is as follows:
in the method, in the process of the invention, Representing the probability of coincidence at the mth particle T moment, T mag E (0, 1) being the coincidence discrimination threshold, mag t representing the magnetic field strength measured at the magnetometer T moment,Representing the grid at which the mth particle t is locatedThe stored magnetic field intensity, max { · } and min { · } respectively represent the maximum value taking operation and the minimum value taking operation of the set;
Probability of coincidence for mth particle t moment Then there are:
Wherein, Representing the magnetic field strength stored by the ith grid of the mth particle.
Further, in the step (3), the established map observation model means to establish a variation relationship between the passable probability of the map grid in the step (2) along with the one-step prediction of the pedestrian pose s t in the step (1) and the occupation times of the map grid in the step (2),
The method comprises the following specific steps: map passability probability for grid where mth particle t moment is locatedThe observation model is shown as follows:
Wherein e is the natural logarithm,
Wherein,For the number of times the mth particle experiences the ith grid, p (θ i|zt,xt) is an inverse observation model of inertial information on the environment, taking p (θ i|zt,xt) =0.8;
In addition, the grid map update comprises a magnetic field update and an occupation frequency update;
The magnetic field update is to update the magnetic field intensity of the current grid in the map, and for the grid where the mth particle t moment is located Is the magnetic field strength of (2)The update rule is as follows:
the occupied times update, namely the number of times of the current grid is updated, and the number of times of the grid where the mth particle t is located is updated The update rule of (2) is as follows:
further, in the step (4), the specific process of updating the weight is as follows:
particle uniformity probability calculated according to step (3) Map passability probabilityUpdating the weight to correct positioning and composition errors; weights at time t for each particle m, m=1, 2, …, NThe following is shown:
Wherein the method comprises the steps of
And (3) according to the updated weights, the pose expectations of all particles, namely corrected pose expectations, can be obtained:
as shown in fig. 2, the experimental scene is a corridor and a room in an experimental building, the floor area of the experimental building is about 2500 square meters, the total route length is 1219.2 meters, a tester walks steadily along the corridor and the room, and after a period of movement, the tester returns to the starting point. The positioning result of the original inertial odometer is shown as a dotted line in the figure, and the positioning result of the inertial pedestrian SLAM method by using the magnetic field superposition in the closed environment is shown as a solid line in the figure; it can be seen in fig. 2 that the positioning error of the pedestrian positioning system using only the inertial odometer increases with the increase of the movement path, affected by the noise of the inertial odometer; compared with the prior art, the positioning system can effectively estimate the error of the positioning result due to the fact that the direct observation of the pose is formed by constructing the map while positioning, and the positioning track by the positioning system is high in repeatability and free from accumulation of positioning errors with time.
FIG. 3 is a graph showing the course estimation results of the two methods, wherein the course estimation result of the original inertial odometer is shown as a dashed curve in the graph, and the course estimation result of the inertial pedestrian SLAM method by using the magnetic field superposition in the closed environment is shown as a solid curve in the graph; it can be seen that the heading of a positioning system using only an inertial odometer gradually builds up, deviating from the true value, due to the lack of absolute heading observations. Compared with the method, the positioning system can effectively inhibit the heading error divergence due to the fact that the direct observation of the pose is formed by constructing the map while positioning, and the heading estimation result on the corridor is always near the true value, so that the error is not accumulated with time.
Finally, the initial and final point positioning errors of the positioning system of the original inertial odometer are about 38.52 meters; the start and end point positioning error of the positioning system of the inertial pedestrian SLAM method by utilizing the magnetic field superposition in the closed environment is about 4.10 meters, and the positioning precision is obviously improved.
The above is only a preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above examples, and all technical solutions belonging to the concept of the present invention belong to the protection scope of the present invention. It should be noted that modifications and adaptations to the invention without departing from the principles thereof are intended to be within the scope of the invention as set forth in the following claims.

Claims (7)

1. The inertial pedestrian SLAM method for magnetic field superposition in a closed environment is characterized by comprising the following specific operation steps:
(1) Predicting and sampling the pedestrian pose s t at the moment t by using N particles through a Monte Carlo sampling method
Wherein m represents the number of particles, m=1, 2, …, N is the total number of particles;
(2) Predictive sampling according to pedestrian pose s t in step (1) Constructing a grid map Θ [m] for each particle, then constructing a consistency criterion of the constructed grid map Θ [m] by using a magnetic field measurement mag t, and judging a consistency criterion result of each particle
The built grid map Θ [m] specifically comprises: the environment is partitioned using a square grid, expressed for the mth set of particle maps as: Wherein, An ith grid representing an mth particle,The total number of grids in the mth particle map;
Wherein, each map grid comprises three kinds of information including grid index, occupation times and magnetic field intensity; index to grid where mth particle is located at time t The method comprises the following steps:
in the method, in the process of the invention, AndRespectively representing an x-direction coordinate and a y-direction coordinate of the mth particle at the time t, wherein < & gt represents rounding operation, and l is the side length of a square grid; the initial occupation times of all grids are 0, and the magnetic field intensity is-1;
in addition, the consistence of the grid map model theta [m] constructed by the mth particle The criterion is shown as follows:
Wherein,
In the method, in the process of the invention,Representing the probability of coincidence at the mth particle T moment, T mag E (0, 1) being the coincidence discrimination threshold, mag t representing the magnetic field strength measured at the magnetometer T moment,Representing the grid at which the mth particle t is locatedThe stored magnetic field intensity, max { · } and min { · } respectively represent the maximum value taking operation and the minimum value taking operation of the set;
(3) Establishing a map observation model according to the occupation times of each grid in the constructed grid map theta [m];
And then according to the consistency discrimination result in the constructed grid map theta [m] Updating the grid map by combining the established map observation model;
Wherein the established map observation model is to establish the variation relation between the passable probability of the map grid in the step (2) along with the one-step prediction of the pedestrian pose s t in the step (1) and the occupation times of the map grid in the step (2),
The method specifically comprises the following steps: map passability probability for grid where mth particle t moment is locatedThe observation model is shown as follows:
Wherein e is the natural logarithm,
Wherein,For the number of times the mth particle experiences the ith grid, p (θ i|zt,xt) is an inverse observation model of inertial information on the environment, taking p (θ i|zt,xt) =0.8;
in addition, the updating of the grid map comprises magnetic field updating and occupation frequency updating;
The magnetic field update is to update the magnetic field intensity of the current grid in the map, and for the grid where the mth particle t moment is located Is the magnetic field strength of (2)The update rule is as follows:
the occupied times update, namely the number of times of the current grid is updated, and the number of times of the grid where the mth particle t is located is updated The update rule of (2) is as follows:
(4) Combining the consistency discrimination results of the step (2) And (3) the map observation model established in the step (3) realizes the correction of positioning and composition errors through weight updating.
2. The method of inertial pedestrian SLAM with magnetic field superposition in a closed environment according to claim 1,
In the step (1), the pedestrian pose s t at the time t is predicted and sampled by using N particles through a Monte Carlo sampling methodThe process of (1) is specifically as follows:
(1.1) carrying out Monte Carlo sampling on the pedestrian pose distribution s t-1 at the time t-1;
(1.2) carrying out Monte Carlo sampling on the step length-course change control vector u t at the t moment;
(1.3) establishing a dead reckoning probability model of the pedestrian movement process, and predicting the pedestrian pose s t in one step to finally obtain a predicted sample of the pedestrian pose s t at the moment t
3. The inertial pedestrian SLAM method of magnetic field superposition in a closed environment of claim 2, wherein the pedestrian pose s t is specifically represented by the following formula:
st＝[xt,yt,ψt]T，
where x is a direction coordinate, y is a direction coordinate, ψ is a heading angle, and T is a transpose of the vector.
4. The method of inertial pedestrian SLAM with magnetic field superposition in a closed environment according to claim 2,
In the step (1.1), the monte carlo sampling is performed on the pedestrian pose distribution s t-1 at the time t-1, which is specifically shown as the following formula:
Wherein s t-1 is the pose vector of the pedestrian at the time t-1, u t-1 is the control vector at the time t-1, z t-1 is the observation vector at the time t-1, p (s t-1|zt-1,ut-1) is the pose distribution of the pedestrian at the time t-1, The sampling result of the m-th particle pedestrian pose vector at the t-1 moment,The weight of the mth particle, and N is the total number of particles.
5. The method of inertial pedestrian SLAM with magnetic field superposition in a closed environment according to claim 2,
In step (1.2), the monte carlo sampling is performed on the t moment step-heading change control vector u t, which is specifically shown in the following formula:
Wherein,
Superscript [ m ] denotes the mth particle and
In the method, in the process of the invention,For the time t step measurement, σ d is the step measurement noise standard deviation,For the course change measurement at time tMeasuring a noise standard deviation for course change, wherein N represents normal distribution; the step sampling result of the mth particle at the time t is shown, Representing the m-th particle course change sampling result at the time t; the heading of the mth particle at the t-1 moment, And controlling the vector sampling result for the step length-course change of the mth particle at the moment t.
6. The method of inertial pedestrian SLAM with magnetic field superposition in a closed environment according to claim 2,
In the step (1.3), the dead reckoning probability model is specifically shown as follows:
st＝st-1+ut
Wherein s t is a pose vector of a pedestrian at t moment, s t-1 is a pose vector of the pedestrian at t-1 moment, u t is a t moment control vector, x t,yt,ψt is an x-direction coordinate, a y-direction coordinate and a course angle of the pedestrian at t moment respectively, x t-1,yt-1,ψt-1 is an x-direction coordinate, a y-direction coordinate and a course angle of the pedestrian at t-1 moment respectively, d t is a step size of the pedestrian at t moment, and Deltaψ t is a course change angle of the pedestrian at t moment;
In addition, according to the dead reckoning model, combining the sampling result of the pedestrian pose at the t-1 moment And t moment step length-course change control vector sampling resultPedestrian pose prediction sampling of mth particle at t momentThe formula is as follows:
Wherein, And the result is a predicted sampling result of the pose of the mth particle pedestrian at the moment t.
7. The inertial pedestrian SLAM method of magnetic field superposition in a closed environment according to claim 1, wherein in step (4), the specific process of weight update is:
particle uniformity probability calculated according to step (3) Map passability probabilityUpdating the weight to correct positioning and composition errors; weights at time t for each particle mThe following is shown:
Wherein the method comprises the steps of
And (3) according to the updated weights, the pose expectations of all particles, namely corrected pose expectations, can be obtained:
CN202111655225.7A 2021-12-30 2021-12-30 Inertial pedestrian SLAM method for magnetic field superposition in closed environment Active CN114440873B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111655225.7A CN114440873B (en) 2021-12-30 2021-12-30 Inertial pedestrian SLAM method for magnetic field superposition in closed environment

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111655225.7A CN114440873B (en) 2021-12-30 2021-12-30 Inertial pedestrian SLAM method for magnetic field superposition in closed environment

Publications (2)

Publication Number Publication Date
CN114440873A CN114440873A (en) 2022-05-06
CN114440873B true CN114440873B (en) 2024-07-23

Family

ID=81366271

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111655225.7A Active CN114440873B (en) 2021-12-30 2021-12-30 Inertial pedestrian SLAM method for magnetic field superposition in closed environment

Country Status (1)

Country Link
CN (1) CN114440873B (en)

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105928505A (en) * 2016-04-19 2016-09-07 深圳市神州云海智能科技有限公司 Determination method and apparatus for position and orientation of mobile robot
CN109933056A (en) * 2017-12-18 2019-06-25 九阳股份有限公司 A kind of robot navigation method and robot based on SLAM

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8751151B2 (en) * 2012-06-12 2014-06-10 Trx Systems, Inc. System and method for localizing a trackee at a location and mapping the location using inertial sensor information
WO2018134686A2 (en) * 2017-01-19 2018-07-26 Mindmaze Holding Sa Systems, methods, device and apparatuses for performing simultaneous localization and mapping
WO2018184108A1 (en) * 2017-04-04 2018-10-11 Appropolis Inc. Location-based services system and method therefor
CN111639505B (en) * 2020-05-29 2023-01-13 广东电网有限责任公司电力科学研究院 Hybrid positioning navigation system and method for indoor inspection robot
CN112964257B (en) * 2021-02-05 2022-10-25 南京航空航天大学 Pedestrian inertia SLAM method based on virtual landmarks
CN113237485A (en) * 2021-05-10 2021-08-10 北京庆龙科技有限公司 SLAM method and system based on multi-sensor fusion

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105928505A (en) * 2016-04-19 2016-09-07 深圳市神州云海智能科技有限公司 Determination method and apparatus for position and orientation of mobile robot
CN109933056A (en) * 2017-12-18 2019-06-25 九阳股份有限公司 A kind of robot navigation method and robot based on SLAM

Also Published As

Publication number Publication date
CN114440873A (en) 2022-05-06

Similar Documents

Publication Publication Date Title
CN110849374B (en) Underground environment positioning method, device, equipment and storage medium
Zhang A fusion methodology to bridge GPS outages for INS/GPS integrated navigation system
Davidson et al. Application of particle filters for indoor positioning using floor plans
CN106949895B (en) Inspection robot positioning method suitable for transformer substation environment
CN110244715B (en) Multi-mobile-robot high-precision cooperative tracking method based on ultra wide band technology
CN113074739B (en) UWB/INS fusion positioning method based on dynamic robust volume Kalman
CN110118560B (en) Indoor positioning method based on LSTM and multi-sensor fusion
CN102768361A (en) GPS/INS combined positioning method based on genetic particle filtering and fuzzy neural network
CN103644903B (en) Synchronous superposition method based on the tasteless particle filter of distributed edge
CN105910601B (en) A kind of indoor ground magnetic positioning method based on Hidden Markov Model
CN108983271B (en) Train combined positioning method based on RTK-GPS/INS
CN109164411B (en) Personnel positioning method based on multi-data fusion
US20160195401A1 (en) Method and system for locating an object
CN114216459B (en) ELM-assisted GNSS/INS combined navigation unmanned target vehicle positioning method
Shojaei et al. Experimental study of iterated Kalman filters for simultaneous localization and mapping of autonomous mobile robots
CN108801267B (en) Indoor anchor-free positioning method integrating multiple sensors
CN115112121A (en) Multi-source fusion positioning method, system and terminal based on combination of data and model
CN111711432A (en) Target tracking algorithm based on UKF and PF hybrid filtering
Roy et al. Target tracking and estimated time of arrival (ETA) prediction for arrival aircraft
CN114440873B (en) Inertial pedestrian SLAM method for magnetic field superposition in closed environment
CN105866807A (en) Algorithm capable of improving precision of GNSS real-time monitoring data
Sun et al. Study on safe evacuation routes based on crowd density map of shopping mall
Chao et al. Location-constrained particle filter human positioning and tracking system
Havangi An adaptive particle filter based on PSO and fuzzy inference system for nonlinear state systems
CN113483753A (en) Inertial heading error elimination method based on environmental constraint

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