CN114577210A - Cross-region detection algorithm based on map information matrix - Google Patents

Cross-region detection algorithm based on map information matrix Download PDF

Info

Publication number
CN114577210A
CN114577210A CN202210170655.8A CN202210170655A CN114577210A CN 114577210 A CN114577210 A CN 114577210A CN 202210170655 A CN202210170655 A CN 202210170655A CN 114577210 A CN114577210 A CN 114577210A
Authority
CN
China
Prior art keywords
particle
gate
included angle
cross
map information
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.)
Pending
Application number
CN202210170655.8A
Other languages
Chinese (zh)
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.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202210170655.8A priority Critical patent/CN114577210A/en
Publication of CN114577210A publication Critical patent/CN114577210A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • 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
    • G01C21/383—Indoor data

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Traffic Control Systems (AREA)

Abstract

The invention provides a trans-regional detection algorithm based on a map information matrix, which improves the operation speed of the algorithm on the basis of ensuring that particles which are mistakenly trans-regional are detected. A cross-region detection algorithm is provided aiming at the phenomenon of error cross-region in the particle updating process of a particle filter-based map matching algorithm. Aiming at the problem of large calculation amount of the algorithm, the map modeling process is improved, and a map information matrix is obtained by utilizing a rasterized map. The invention utilizes MATLAB simulation software to carry out simulation analysis and actual measurement verification. The accuracy and the rapidity of the door-through detection algorithm based on the map information matrix are proved by comparing the quantitative analysis with the traditional door-through detection algorithm based on the straddle experiment method; the effectiveness of the cross-region detection algorithm provided by the invention is proved by comparing the qualitative analysis with a map matching algorithm without adding cross-region detection.

Description

Cross-region detection algorithm based on map information matrix
Technical Field
The invention relates to a cross-region detection algorithm based on a map information matrix, belongs to the technical field of map matching, and particularly relates to the field of a map matching algorithm based on particle filtering.
Background
In recent years, with the development of the internet of things industry, location-based services are becoming an important part of people's social life. As an economical and simple positioning method, the indoor map data not only can intuitively display the position of the pedestrian on the map, but also can have natural constraint force on the positioning result without any other auxiliary facilities or instruments. The map matching technology effectively fuses the positioning data and the map data, obtains an initial positioning result by using other indoor positioning technologies, and corrects the positioning result by using the map data, so that more accurate positioning coordinates are obtained, and the map matching technology is an auxiliary positioning algorithm. Conventional map matching techniques are generally classified into three categories, including geometry-based map matching techniques, topology-based map matching techniques, and advanced matching techniques based on particle filtering. The existing indoor map matching technology mostly uses the map matching technology based on particle filtering, the technology fully utilizes the updating process of the state and the wall information of the indoor map, and the technology has higher positioning precision. Therefore, the particle filter based map matching technology has been one of important research directions in the field of map matching.
In the map matching technique based on the particle filter, since a connection area such as an indoor gate is accessible, a part of particles always enter other areas through the gate in error during the update of the particle state, and the subsequent update error increases. Therefore, the accurate cross-region detection algorithm is researched so as to detect and eliminate the particles which should not pass through the gate, and the positioning precision of the map matching algorithm can be improved. According to the function realized by the algorithm, the cross-region detection algorithm can be mainly divided into two steps of door-through detection and cross-region judgment. The cross-region detection is to judge whether the particle updating process passes through a gate by judging whether the particle track intersects with a gate line segment, and is the most important step of cross-region detection. The classic algorithm for judging the intersection of the two line segments is a cross-over experimental method, the method is clear in thought and convenient to understand, but when the method is applied to a map matching technology based on particle filtering, the calculated amount is large, and the real-time performance is low. In addition, in the map matching technology, map data is a prerequisite for implementing a matching algorithm, and the accuracy of the map data seriously affects the accuracy of the ground map matching technology. It is necessary to design a reasonable map information acquisition method to obtain indoor map data.
Disclosure of Invention
The invention aims to provide a trans-regional detection algorithm based on a map information matrix, which improves the operation speed of the algorithm on the basis of ensuring that particles across regions are detected incorrectly.
The purpose of the invention is realized as follows: the method comprises the following steps:
(1) establishing a map information matrix according to an indoor map;
(2) updating the state of the particle filter to obtain the k-1 time position of the ith particle
Figure BDA0003518005190000011
And the position at time k
Figure BDA0003518005190000021
(3) Circularly detecting element values of the particle tracks in the map information matrix, and judging whether the particle tracks pass through the gate or not;
(4) calculating an included angle between the particle track and the door;
(5) comparing with a preset included angle threshold value, and correctly judging whether the particles cross the region or not;
(6) and adding the cross-region detection algorithm into a weight updating part in the particle filtering algorithm to finish filtering.
Optionally, the specific process of step (1) is as follows:
assuming that the total length of the indoor map is L, the width is W, and the selected grid width is L, the map can be divided into m × n grids in total, that is, the map information matrix has m rows and n columns in total, where m, n can be obtained by the following formula:
Figure BDA0003518005190000022
wherein [. is a rounded up symbol. This is because the map starts from 0 and the matrix index position starts from 1. Assuming that a certain position is located in the ith row and the jth column of the map information matrix by solving the above equation, the position thereof can be represented as (i, j), i ═ 1, 2. From the map information represented by the grid, the map data may be converted into a map information matrix Indoor of m rows and n columns. When the position (i, j) is a wall, Indoor (i, j) is 0; when the position (i, j) is Indoor layout such as a table and a chair, the inoor (i, j) is 1; when the position (i, j) is a daily activity area of people, inoor (i, j) ═ 2; when the location (i, j) is a connection area such as a gate, inoor (i, j) ═ 3, the map data can be stored in the map information matrix in a digital form.
Optionally, the specific process of step (3) is:
let the i-th particle at time k-1 have coordinates of
Figure BDA0003518005190000023
Then the row and column of the map information matrix Indoor is:
Figure BDA0003518005190000024
after the state of the particle is updated, the coordinate of the ith particle at the time k is
Figure BDA0003518005190000025
The row and column of the map information matrix Indoor are as follows:
Figure BDA0003518005190000026
according to the relative position relation between the ith particle at the k-1 moment and the ith particle at the k moment, the whole region can be divided into six parts including 2 coordinate axes and 4 regions, and each part is circularly detected as follows:
(1) if it is
Figure BDA0003518005190000031
That is, the connection line of the k-1 and the k time coordinate is the x axis, let
Figure BDA0003518005190000032
If present, is
Figure BDA0003518005190000033
The ith particle passes through a gate in the updating process from the moment k-1 to the moment k, and the included angle between the particle track and the gate is required;
(2) if it is
Figure BDA0003518005190000034
I.e. the connection line of the k-1 and the k time coordinate is the y axis, order
Figure BDA0003518005190000035
If present, is
Figure BDA0003518005190000036
The ith particle passes through a gate in the updating process from the moment k-1 to the moment k, and the included angle between the particle track and the gate is required;
(3) let the slope
Figure BDA0003518005190000037
If | a | < 1 and
Figure BDA0003518005190000038
at this point a column loop traversal is performed. If the start and end position grid is 3, that is
Figure BDA0003518005190000039
Or
Figure BDA00035180051900000310
The ith particle passes through the gate, requiring the angle between the particle trajectory and the gate. If not 3, let
Figure BDA00035180051900000311
Then
Figure BDA00035180051900000312
Figure BDA00035180051900000313
When the y direction changes, the correspondingChange in x direction Δ x ═ Δ y/a, then
Figure BDA00035180051900000314
Let p be1＝[x1/l]，q1＝[y1/l]，p2＝[x2/l]，q2＝[y2/l]If Inoor (p) is present1,q1) 3 or Indoor (p)2,q2) If the number of particles is 3, the ith particle passes through a gate in the updating process from the moment k-1 to the moment k, and the included angle between the particle track and the gate is required;
(4) let the slope
Figure BDA00035180051900000315
If | a | < 1 and
Figure BDA00035180051900000316
at this point a column loop traversal is performed. If the start and end position grid is 3, that is
Figure BDA00035180051900000317
Or
Figure BDA00035180051900000318
The ith particle passes through the gate, requiring the angle between the particle trajectory and the gate. If not 3, let
Figure BDA00035180051900000319
Then
Figure BDA00035180051900000320
Figure BDA00035180051900000321
When the y direction changes, the corresponding x direction change Δ x is Δ y/a, then
Figure BDA00035180051900000322
Let p be1＝[x1/l]，q1＝[y1/l]，p2＝[x2/l]，q2＝[y2/l]If Inoor (p) is present1,q1)＝3 or Indenor (p)2,q2) If the number of particles is 3, the ith particle passes through a gate in the updating process from the moment k-1 to the moment k, and the included angle between the particle track and the gate is required;
(5) let the slope
Figure BDA00035180051900000323
If | a | ≧ 1 and
Figure BDA00035180051900000324
at this point a row loop traversal is performed. If the start and end position grid is 3, that is
Figure BDA00035180051900000325
Or
Figure BDA00035180051900000326
The ith particle passes through the gate, requiring the angle between the particle trajectory and the gate. If not 3, let
Figure BDA00035180051900000327
Then
Figure BDA00035180051900000328
Figure BDA00035180051900000329
When the x direction changes, the corresponding y direction change Δ y equals Δ x · a, then
Figure BDA0003518005190000041
Let p be1＝[x1/l]，q1＝[y1/l]，p2＝[x2/l]，q2＝[y2/l]If Inoor (p) is present1,q1) 3 or Indoor (p)2,q2) If the number of particles is 3, the ith particle passes through a gate in the updating process from the moment k-1 to the moment k, and the included angle between the particle track and the gate is required;
(6) let the slope
Figure BDA0003518005190000042
If | a | ≧ 1 and
Figure BDA0003518005190000043
at this point a row loop traversal is performed. If the start and end position grid is 3, that is
Figure BDA0003518005190000044
Or
Figure BDA0003518005190000045
The ith particle passes through the gate, requiring the angle between the particle trajectory and the gate. If not 3, let
Figure BDA0003518005190000046
Then
Figure BDA0003518005190000047
Figure BDA0003518005190000048
When the x direction changes, the corresponding y direction change Δ y equals Δ x · a, then
Figure BDA0003518005190000049
Let p be1＝[x1/l]，q1＝[y1/l]，p2＝[x2/l]，q2＝[y2/l]If Indenor (p) is present1,q1) 3 or Indoor (p)2,q2) If the number of particles is 3, the ith particle passes through a gate in the updating process from the moment k-1 to the moment k, and the included angle between the particle track and the gate is required;
optionally, the specific process of step (4) is as follows:
after the particles are detected to pass through the gate, whether the particles cross the region needs to be correctly judged. If the track does cross the region, the particle is valid; if the track does not cross a region, the particle needs to be rejected. The specific idea is to set a proper included angle threshold value to judge whether the particles cross the region according to the size of an included angle between the particle track passing through the gate and the gate. And the cross-region judgment is converted into the calculation of the included angle between the coordinate connecting line of the particles at the k-1 moment and the k moment and the door, and the calculated included angle is compared with a preset included angle threshold value. The angle between the particle trajectory and the gate can be abstracted as the angle between two line segments. The included angle between two line segments can be divided into the following three cases:
(1) when the gate represents a line segment parallel to the X-axis, then:
a. when the slope of the trajectory segment is not present, i.e. yk＝yk-1Then, the included angle θ is 90 °;
b. when the slope k of the track line segment1＝(yk-1-yk)/(xk-1-xk) When the angle is more than or equal to 0, the included angle theta is arctan (k)1)；
c. When the slope k of the track line segment1＝(yk-1-yk)/(xk-1-xk) When < 0, the angle θ is arctan | k1|。
(2) When the gate represents a line segment perpendicular to the X-axis, then:
a. when the slope of the trajectory segment is not present, i.e. yk＝yk-1When the included angle theta is 0 degree;
b. when the slope k of the track line segment1＝(yk-1-yk)/(xk-1-xk) When the angle is more than or equal to 0, the included angle theta is 90-arctan (k)1)；
c. When the slope k of the track line segment1＝(yk-1-yk)/(xk-1-xk) When the angle is less than 0, the included angle theta is 90-arctan | k1|。
(3) When the slope of the line segment represented by the gate exists and is k2Then, at this time:
a. when the slope of the trajectory segment is not present, i.e. yk＝yk-1At this time, the included angle θ is 90-arctan | k2|；
b. When the slope of the track line segment is k1When the angle θ is arctan | (k)1-k2)/(1+k1·k2)|。
Optionally, the specific process of step (6) is as follows:
it is reasonable that there is no erroneous particle entering other regions through the gate after the status update of the particle is completed. If a wrong cross-region occurs, the particle is considered to be unreasonable in positioning result, and the weight of the particle needs to be set to 0. Otherwise the weight is set to 1. The cross-region weight can be expressed as follows:
Figure BDA0003518005190000051
the final weight of the particle can be represented by the product of other weights of the particle and the cross-region weight, and after the particle with the weight value of 0 is removed, normalization operation needs to be performed on the weight to obtain a new weight:
Figure BDA0003518005190000052
after the normalized particle weights are obtained, the weights can be used for weighting and summing to obtain an estimated value of the state, so that a final positioning result is obtained. The estimation equation for the state is as follows:
Figure BDA0003518005190000053
compared with the prior art, the invention has the beneficial effects that: (1) the invention has simple design and clear algorithm thought; (2) the invention reduces the calculated amount of the algorithm on the basis of ensuring the correct detection of the through particles; (3) the invention can inhibit the error trans-regional phenomenon of the particle updating process of the particle-filter-based map matching algorithm, so that the particle distribution is more reasonable.
Drawings
FIG. 1 is a flow chart of a cross-region detection algorithm based on a map information matrix according to the present invention;
fig. 2(a) - (b) show the process of establishing the map information matrix, fig. 2(a) is a simplified diagram of an indoor room, and fig. 2(b) is a diagram after the gridding process;
FIG. 3 is a diagram showing the relative positions of the particle trajectories and the gate;
FIGS. 4(a) - (b) are schematic diagrams of loop traversal, FIG. 4(a) is a schematic diagram of | k | ≦ 1 traversal, and FIG. 4(b) is a schematic diagram of | k | > 1 traversal;
FIG. 5 is a graph showing the angle relationship between the particle trajectory and the gate;
fig. 6(a) - (c) are schematic diagrams of the range of particles that can pass when θ is 30 °, θ is 45 °, and θ is 60 °, respectively;
FIG. 7 is a simulation scenario setup;
FIG. 8 is a comparison (simulation) of the accuracy of the detection of a door through in the detection algorithm of the invention with that of the conventional detection method;
FIG. 9 is a comparison graph (simulation) of the operation time of the cross-region detection algorithm of the present invention for the detection of a door crossing and the conventional detection method;
FIG. 10 is a comparison of results of different included angle thresholds in the cross-region detection algorithm of the present invention;
FIG. 11 shows measured scene settings;
FIG. 12 is a UWB measured trace;
FIG. 13 is a particle distribution map (measured) of a particle filter based map matching algorithm;
FIG. 14 is a graph of a particle distribution (measured) after the cross-region detection algorithm of the present invention is added.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings and specific embodiments.
Fig. 1 is a flow chart of the present invention, which comprises the following specific steps:
(1) establishing a map information matrix according to an indoor map;
(2) updating the state of the particle filter to obtain the k-1 time position of the ith particle
Figure BDA0003518005190000061
And the position at time k
Figure BDA0003518005190000062
And converted into the row and column values of the map information matrix
Figure BDA0003518005190000063
(3) Circularly detecting element values of the particle tracks in the map information matrix to judge whether the particle tracks pass through the gate or not;
(4) if the particle track is detected to pass through the gate, calculating an included angle theta between the particle track and the gate;
(5) comparing with a preset included angle threshold, if theta is larger than 45 degrees, crossing the region by the particles, otherwise, the particles are invalid;
(6) and adding the cross-region detection algorithm into a weight updating part in the particle filtering algorithm to finish filtering.
The invention is a trans-regional detection algorithm based on a map information matrix, and improves the operation speed of the algorithm on the basis of ensuring that the particles which are mistakenly trans-regional are detected. A cross-region detection algorithm is provided aiming at the phenomenon of error cross-region in the particle updating process of a particle filter-based map matching algorithm. Aiming at the problem of large calculation amount of the algorithm, the map modeling process is improved, and a map information matrix is obtained by utilizing a rasterized map. The invention utilizes MATLAB simulation software to carry out simulation analysis and actual measurement verification. The accuracy and the rapidity of the door-through detection algorithm based on the map information matrix are proved by comparing the quantitative analysis with the traditional door-through detection algorithm based on the straddle experiment method; the effectiveness of the cross-region detection algorithm provided by the invention is proved by comparing the qualitative analysis with a map matching algorithm without adding cross-region detection.
The examples are intended to illustrate the invention, but not to limit the invention, and any modifications and variations of the invention within the spirit of the invention and the scope of the claims are intended to fall within the scope of the invention.
Firstly, simulation data verification is carried out on the invention:
to verify the performance of the cross-region detection algorithm of the present invention, verification was performed using simulation data. Fig. 7 is a simulation scenario setup, where it is assumed that the room size is 10m x 10m and the grid length is 0.1m, comprising 24 wall segments, 4 doors. The number of particles in the simulation process is 500, and assuming that the position coordinates at the time k are (7.05 ), as shown by red dots in fig. 7, the particles at the time k-1 are uniformly distributed on the map, as shown by blue dots in fig. 7.
FIG. 8 is a comparison graph of the accuracy of the cross-region detection algorithm of the present invention compared with the conventional detection method, wherein a flag is set to indicate whether the cross-region detection algorithm detects a particle passing gate. If the flag is 0, the ith particle does not pass through a gate in the transition process from the time k-1 to the time k, and the particle does not cross the region; if flag is equal to 1, it is described that the ith particle passes through a gate in the updating process from the time k-1 to the time k, and the included angle between the particle track and the gate needs to be obtained. Simulation results show that the two methods have consistent door-through detection results, and the accuracy of the door-through detection algorithm based on the map information matrix is proved.
Fig. 9 is a comparison graph of the operation time of the cross-region detection algorithm of the present invention compared with the conventional detection method, and the number of particles in the simulation process is changed under the condition that the simulation environment is not changed, so as to obtain the time consumed by the two algorithms for performing the cross-region detection under different numbers of particles. Simulation results show that the running time of the door-through detection algorithm based on the map information matrix is shorter than that of the door-through detection performed by a traditional straddle experiment method, and the advantages are greater when the number of doors is large. Comprehensive comparison shows that the door-through detection algorithm based on the map information matrix is superior to the traditional straddle experiment door-through detection algorithm.
Fig. 10 is a comparison of results of different included angle thresholds in the cross-region detection algorithm of the present invention, in the cross-region detection algorithm, a reasonable included angle threshold may improve the accuracy of the algorithm, and the selection of the included angle threshold needs to comprehensively consider two situations, namely, a track cross-region situation and a non-cross-region situation, that is, a cross-region situation near 4 doors in the simulation environment shown in fig. 7. In the simulation process, the included angle threshold is respectively set to be 30 degrees, 45 degrees and 60 degrees, and a plurality of tests are carried out. The result shows that when the particle track does not cross the region, the accuracy of the cross-region detection algorithm is increased along with the increase of the included angle threshold. When the particle track crosses the region, the accuracy of the cross-region detection algorithm is reduced along with the increase of the included angle threshold value. After comprehensive consideration, 45 degrees are selected as an angle threshold of the cross-region detection algorithm.
Secondly, the invention is verified by actual measurement data:
fig. 11 is actual measurement scene setting, and the actual measurement experiment site is in room 4003 of # 61 building of harbourne engineering university. Obtaining an initial positioning result by using UWB technology, selecting the southeast corner of the 4003 room as a coordinate origin, and setting the X axis to be positiveThe direction is north and the positive Y-axis direction is west, as indicated by the dark red arrow in the figure. Four base stations are deployed in a room, and the position coordinates of the four base stations are respectively as follows: s1＝[0.2 0.4]，S2＝[7.74 0.17]，S3＝[7.6 11.75]，S4＝[0.5 11.75]. A room sketch is built in a 4003 open area of a room, 8 walls and 4 doors are arranged in total, and wall information is shown by black lines in the drawing. The walking trajectory is shown by the red arrow in the figure. The measured trace of UWB is shown in fig. 12.
Fig. 13 is a particle distribution diagram of a conventional particle filter-based map matching algorithm, and since a connection area such as an indoor gate is considered to be accessible during a particle update process, some particles may pass through the gate and enter other areas during the particle update process. FIG. 14 is a particle distribution diagram after the cross-region detection algorithm of the present invention is added, and the distribution of particles is more reasonable after the gate is introduced to constrain the update of particles and the particles across the region are removed by mistake. The actual measurement verification is qualitative analysis, and only intuitively shows that the cross-region detection algorithm can eliminate the particles in the wrong cross-region, so as to obtain more reasonable particle distribution.
In summary, in order to solve the problem of particle error cross-region in the particle updating process of the existing particle-filter-based map matching algorithm, the invention provides a cross-region detection algorithm based on a map information matrix, and the operation speed of the algorithm is improved on the basis of ensuring that the particle error cross-region is detected. A cross-region detection algorithm is provided aiming at the phenomenon of error cross-region in the particle updating process of a particle filter-based map matching algorithm. Aiming at the problem of large algorithm calculation amount, the map modeling process is improved, and a map information matrix is obtained by utilizing a grid map. The invention utilizes MATLAB simulation software to carry out simulation analysis and actual measurement verification. The accuracy and the rapidity of the door-through detection algorithm based on the map information matrix are proved by comparing the quantitative analysis with the traditional door-through detection algorithm based on the straddle experiment method; the effectiveness of the cross-region detection algorithm provided by the invention is proved by comparing the qualitative analysis with a map matching algorithm without adding cross-region detection. The simulation result shows that compared with the existing map matching algorithm, the method can further improve the reasonability of particle distribution, and has certain reference significance for further improving the positioning accuracy of the map matching algorithm based on particle filtering.

Claims (5)

1. The cross-region detection algorithm based on the map information matrix is characterized by comprising the following steps of:
the method comprises the following steps: establishing a map information matrix according to an indoor map;
step two: updating the state of the particle filter to obtain the k-1 time position of the ith particle
Figure FDA0003518005180000011
And the position at time k
Figure FDA0003518005180000012
Step three: circularly detecting element values of the particle tracks in the map information matrix, and judging whether the particle tracks pass through the gate or not;
step four: calculating an included angle between the particle track and the door;
step five: comparing with a preset included angle threshold value, and correctly judging whether the particles cross the region or not;
step six: and adding the cross-region detection algorithm into a weight updating part in the particle filtering algorithm to finish filtering.
2. The map information matrix-based trans-regional detection algorithm of claim 1, wherein step one specifically comprises: assuming that the total length of the indoor map is L, the width is W, and the selected grid width is L, the map can be divided into m × n grids in total, that is, the map information matrix has m rows and n columns in total, where m, n can be obtained by the following formula:
Figure FDA0003518005180000013
in the formula, [ · ] is an upward rounding symbol, and a certain position is located in the ith row and jth column of the map information matrix through solving the above formula, so that the position can be represented as (i, j), i ═ 1,2,.. the right, m, j ═ 1,2,. the right, n in the matrix; according to the map information represented by the grid, converting the map data into a map information matrix Inwood with m rows and n columns; when the position (i, j) is a wall, Indoor (i, j) is 0; when the position (i, j) is Indoor layout such as a table and a chair, the inoor (i, j) is 1; when the position (i, j) is a daily activity area of people, inoor (i, j) ═ 2; when the located position (i, j) is a connection area such as a gate, the inoar (i, j) is 3, and the map data is stored in the map information matrix in a digital form.
3. The map information matrix-based transregional detection algorithm of claim 1, wherein step three specifically comprises: let the i-th particle at time k-1 have coordinates of
Figure FDA0003518005180000014
The row and column values of the map information matrix Indoor are as follows:
Figure FDA0003518005180000015
after the state of the particle is updated, the coordinate of the ith particle at the time k is
Figure FDA0003518005180000016
The row and column values of the map information matrix Indor are as follows:
Figure FDA0003518005180000021
according to the relative position relation between the ith particle at the k-1 moment and the ith particle at the k moment, the whole region can be divided into six parts including 2 coordinate axes and 4 regions, and each part is circularly detected as follows:
(1) if it is
Figure FDA0003518005180000022
That is, the connection line of the k-1 and the k time coordinate is the x axis, let
Figure FDA0003518005180000023
If present, is
Figure FDA0003518005180000024
The ith particle passes through a gate in the updating process from the moment k-1 to the moment k, and the included angle between the particle track and the gate needs to be solved;
(2) if it is
Figure FDA0003518005180000025
I.e. the connection line of the k-1 and the k time coordinate is the y axis, order
Figure FDA0003518005180000026
If present, is
Figure FDA0003518005180000027
The ith particle passes through a gate in the updating process from the moment k-1 to the moment k, and the included angle between the particle track and the gate needs to be solved;
(3) let the slope
Figure FDA0003518005180000028
If | a | < 1 and
Figure FDA0003518005180000029
performing row-column cycle traversal at the moment; if the start and end position grid is 3, that is
Figure FDA00035180051800000210
Or
Figure FDA00035180051800000211
The ith particle passes through a gate, and the included angle between the particle track and the gate needs to be solved; if not 3, let
Figure FDA00035180051800000212
Then
Figure FDA00035180051800000213
Figure FDA00035180051800000214
When the y direction changes, the corresponding x direction change Δ x is Δ y/a, then
Figure FDA00035180051800000215
Let p be1＝[x1/l]，q1＝[y1/l]，p2＝[x2/l]，q2＝[y2/l]If Inoor (p) is present1,q1) 3 or Indoor (p)2,q2) If the particle trajectory is 3, the ith particle passes through a gate in the updating process from the time k-1 to the time k, and the included angle between the particle trajectory and the gate needs to be obtained;
(4) let the slope
Figure FDA00035180051800000216
If | a | < 1 and
Figure FDA00035180051800000217
then, performing row-column cycle traversal; if the start and end position grid is 3, that is
Figure FDA00035180051800000218
Or
Figure FDA00035180051800000219
The ith particle passes through the gate, and the included angle between the particle track and the gate needs to be calculated; if not 3, let
Figure FDA00035180051800000220
Then
Figure FDA00035180051800000221
Figure FDA00035180051800000222
When the y direction changes, the corresponding x direction change Δ x is Δ y/a, then
Figure FDA00035180051800000223
Let p be1＝[x1/l]，q1＝[y1/l]，p2＝[x2/l]，q2＝[y2/l]If Inoor (p) is present1,q1) 3 or Indoor (p)2,q2) If the particle trajectory is 3, the ith particle passes through a gate in the updating process from the time k-1 to the time k, and the included angle between the particle trajectory and the gate needs to be obtained;
(5) let the slope
Figure FDA00035180051800000224
If | a | is more than or equal to 1 and
Figure FDA00035180051800000225
performing cycle traversal at the moment; if the start and end position grid is 3, that is
Figure FDA00035180051800000226
Or
Figure FDA00035180051800000227
The ith particle passes through the gate, and the included angle between the particle track and the gate needs to be calculated; if not 3, let
Figure FDA0003518005180000031
Then
Figure FDA0003518005180000032
Figure FDA0003518005180000033
When the x direction changes, the corresponding y direction change Δ y equals Δ x · a, then
Figure FDA0003518005180000034
Let p be1＝[x1/l]，q1＝[y1/l]，p2＝[x2/l]，q2＝[y2/l]If Inoor (p) is present1,q1) 3 or Indoor (p)2,q2) If 3, the ith particle passes through a gate in the updating process from the time k-1 to the time k, and the included angle between the particle track and the gate needs to be solved;
(6) let the slope
Figure FDA0003518005180000035
If | a | ≧ 1 and
Figure FDA0003518005180000036
performing cycle traversal at the moment; if the start and end position grid is 3, that is
Figure FDA0003518005180000037
Or
Figure FDA0003518005180000038
The ith particle passes through a gate, and the included angle between the particle track and the gate needs to be solved; if not 3, let
Figure FDA0003518005180000039
Then the
Figure FDA00035180051800000310
Figure FDA00035180051800000311
When the x direction changes, the corresponding y direction change Δ y equals Δ x · a, then
Figure FDA00035180051800000312
Let p be1＝[x1/l]，q1＝[y1/l]，p2＝[x2/l]，q2＝[y2/l]If Indenor (p) is present1,q1) 3 or Indoor (p)2,q2) If it is 3, the ith particle passes through the gate in the updating process from the time k-1 to the time k, and the included angle between the particle trajectory and the gate needs to be obtained.
4. The map information matrix-based transregional detection algorithm of claim 1, wherein the fourth step specifically comprises: after the particles are detected to pass through the gate, whether the particles cross the region or not needs to be judged correctly; if the track does cross the region, the particle is valid; if the track does not cross the region, the particle needs to be removed; the cross-region judgment is converted into the calculation of the included angle between the coordinate connecting line of the particles at the k-1 moment and the k moment and the door, and the calculated included angle is compared with a preset included angle threshold value; the included angle between the particle track and the gate can be abstracted into the included angle between two line segments; the included angle between two line segments can be divided into the following three cases:
(1) when the gate represents a line segment parallel to the X-axis, then:
a. when the slope of the trajectory segment is not present, i.e. yk＝yk-1Then, the included angle θ is 90 °;
b. when the slope k of the track line segment1＝(yk-1-yk)/(xk-1-xk) When the angle is more than or equal to 0, the included angle theta is arctan (k)1)；
c. When the slope k of the track line segment1＝(yk-1-yk)/(xk-1-xk) When < 0, the angle θ is arctan | k1|；
(2) When the gate represents a line segment perpendicular to the X-axis, then:
a. when the slope of the trajectory segment is not present, i.e. yk＝yk-1When the included angle theta is 0 degree;
b. when the slope k of the track line segment1＝(yk-1-yk)/(xk-1-xk) When the angle is more than or equal to 0, the included angle theta is 90-arctan (k)1)；
c. When the slope k of the track line segment1＝(yk-1-yk)/(xk-1-xk) When the angle is less than 0, the included angle theta is 90-arctan | k1|；
(3) When the slope of the line segment represented by the gate exists and is k2Then, at this time:
a. when the slope of the trajectory segment is not present, i.e. yk＝yk-1At this time, the included angle θ is 90-arctan | k2|；
b. When the slope of the track line segment is k1When the angle θ is arctan | (k)1-k2)/(1+k1·k2)|。
5. The map information matrix-based trans-regional detection algorithm of claim 1, wherein step six specifically comprises: after the status update of the particles is completed, it is reasonable that there is no erroneous particle entering other areas through the gate; if a wrong cross-region occurs, the positioning result of the particle is considered to be unreasonable, the weight of the particle needs to be set to 0 '', otherwise, the weight is set to 1; the cross-region weights are expressed as follows:
Figure FDA0003518005180000041
the final weight of the particle can be represented by the product of other weights of the particle and the cross-region weight, and after the particle with the weight value of 0 is removed, normalization operation needs to be performed on the weight to obtain a new weight:
Figure FDA0003518005180000042
after the weight of the normalized particles is obtained, the weight is used for weighting and summing to obtain an estimated value of the state, and a final positioning result is obtained, wherein an estimation equation of the state is as follows:
Figure FDA0003518005180000043
CN202210170655.8A 2022-02-24 2022-02-24 Cross-region detection algorithm based on map information matrix Pending CN114577210A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210170655.8A CN114577210A (en) 2022-02-24 2022-02-24 Cross-region detection algorithm based on map information matrix

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210170655.8A CN114577210A (en) 2022-02-24 2022-02-24 Cross-region detection algorithm based on map information matrix

Publications (1)

Publication Number Publication Date
CN114577210A true CN114577210A (en) 2022-06-03

Family

ID=81775140

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210170655.8A Pending CN114577210A (en) 2022-02-24 2022-02-24 Cross-region detection algorithm based on map information matrix

Country Status (1)

Country Link
CN (1) CN114577210A (en)

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106767821A (en) * 2016-12-09 2017-05-31 北京羲和科技有限公司 A kind of map match localization method and system based on particle filter
US20170176191A1 (en) * 2015-12-21 2017-06-22 InvenSense, Incorporated Method and system for using offline map information aided enhanced portable navigation
US20180112983A1 (en) * 2016-10-24 2018-04-26 Invensense Inc. Method and system for global shape matching a trajectory
CN110763236A (en) * 2019-10-28 2020-02-07 杭州电子科技大学 Map matching positioning optimization method based on least square
CN111595344A (en) * 2020-06-01 2020-08-28 中国矿业大学 Multi-posture downlink pedestrian dead reckoning method based on map information assistance
CN111829516A (en) * 2020-07-24 2020-10-27 大连理工大学 Autonomous pedestrian positioning method based on smart phone
CN112562077A (en) * 2020-11-25 2021-03-26 西北工业大学 Pedestrian indoor positioning method integrating PDR and prior map
CN113008223A (en) * 2021-03-03 2021-06-22 哈尔滨工程大学 Seabed terrain synchronous positioning and mapping method based on particle filtering and mapping optimization
WO2022018399A1 (en) * 2020-07-20 2022-01-27 Navenio Ltd. Map matching trajectories

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170176191A1 (en) * 2015-12-21 2017-06-22 InvenSense, Incorporated Method and system for using offline map information aided enhanced portable navigation
US20180112983A1 (en) * 2016-10-24 2018-04-26 Invensense Inc. Method and system for global shape matching a trajectory
CN106767821A (en) * 2016-12-09 2017-05-31 北京羲和科技有限公司 A kind of map match localization method and system based on particle filter
CN110763236A (en) * 2019-10-28 2020-02-07 杭州电子科技大学 Map matching positioning optimization method based on least square
CN111595344A (en) * 2020-06-01 2020-08-28 中国矿业大学 Multi-posture downlink pedestrian dead reckoning method based on map information assistance
WO2022018399A1 (en) * 2020-07-20 2022-01-27 Navenio Ltd. Map matching trajectories
CN111829516A (en) * 2020-07-24 2020-10-27 大连理工大学 Autonomous pedestrian positioning method based on smart phone
CN112562077A (en) * 2020-11-25 2021-03-26 西北工业大学 Pedestrian indoor positioning method integrating PDR and prior map
CN113008223A (en) * 2021-03-03 2021-06-22 哈尔滨工程大学 Seabed terrain synchronous positioning and mapping method based on particle filtering and mapping optimization

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
LIU WEILIAN ET AL.: "Slam algorithm for multi-robot communication in unknown environment based on particle filter", 《JOURNAL OF AMBIENT INTELLIGENCE AND HUMANIZED COMPUTING》, pages 1 - 9 *
余彦培;: "一种融合地图与传感器信息的室内地图匹配新算法", 电讯技术, no. 12, pages 68 - 74 *
周瑞;鲁翔;卢帅;李志强;桑楠;: "基于粒子滤波和地图匹配的融合室内定位", 电子科技大学学报, no. 03, 30 May 2018 (2018-05-30), pages 97 - 102 *
周瑞等: "基于粒子滤波和地图匹配的融合室内定位", 《电子科技大学学报》, vol. 47, no. 3, pages 415 - 420 *

Similar Documents

Publication Publication Date Title
CN112417573B (en) GA-LSSVM and NSGA-II shield tunneling multi-objective optimization method based on existing tunnel construction
CN109285348A (en) A kind of vehicle behavior recognition methods and system based on two-way length memory network in short-term
CN101281648A (en) Method for tracking dimension self-adaption video target with low complex degree
CN101499168B (en) Structured light strip center extraction method based on ridge line tracing and Hessian matrix
CN103473540B (en) The modeling of intelligent transportation system track of vehicle increment type and online method for detecting abnormality
US20160195401A1 (en) Method and system for locating an object
CN104330721A (en) Integrated circuit hardware Trojan horse detection method and integrated circuit hardware Trojan horse detection system
CN107833463A (en) Traffic signals Time segments division method and system based on two dimension cluster
CN109858526B (en) Multi-target track fusion method based on sensor in target tracking
CN110458174A (en) A kind of unordered accurate extracting method of cloud key feature points
CN112601173B (en) 5G positioning truth value detection and attack tracing method, system, equipment and application
CN105005878B (en) A kind of comprehensive estimation method of strong intelligent grid
CN103885867B (en) Online evaluation method of performance of analog circuit
CN103247057A (en) Road target multi-hypothesis tracking algorithm under target-echo-road network data association
CN102750522B (en) A kind of method of target following
CN103413131A (en) Tower crane recognition method based on spectral and geometric characteristics
CN106447662A (en) Combined distance based FCM image segmentation algorithm
CN102506812B (en) VT checking method for stability judgment of reference points in deformation monitoring
CN110716998B (en) Fine scale population data spatialization method
CN110059292B (en) Space target posture recognition method
CN103778342A (en) Emitter recognition algorithm based on interval number and evidence theory
CN114577210A (en) Cross-region detection algorithm based on map information matrix
CN111985691B (en) Site selection method for wind power plant booster station
CN104750998B (en) Target tracking method of passive multi-sensor based on density filter
CN116630989A (en) Visual fault detection method and system for intelligent ammeter, electronic equipment and storage medium

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