CN108981702A - A kind of vehicle positioning method of multiposition joint particle filter - Google Patents
A kind of vehicle positioning method of multiposition joint particle filter Download PDFInfo
- Publication number
- CN108981702A CN108981702A CN201810716968.2A CN201810716968A CN108981702A CN 108981702 A CN108981702 A CN 108981702A CN 201810716968 A CN201810716968 A CN 201810716968A CN 108981702 A CN108981702 A CN 108981702A
- Authority
- CN
- China
- Prior art keywords
- anchor point
- particle
- vehicle
- anchor
- point
- 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
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/20—Instruments for performing navigational calculations
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
The invention discloses a kind of vehicle positioning methods of multiposition joint particle filter.The road network map in vehicle driving region is established, vehicle location is obtained by dead reckoning method in real time and detects anchor point, forms initial vehicle subsection driving trace；For the anchor point detected every time, anchor point corresponding position in road network map is indicated using a pile particle, is calculated in real time by joint particle filter and is updated every a pile particle weights；For every a pile particle, anchor point using the maximum particle of weight as anchor point in road network map is sequentially connected anchor point as revised vehicle subsection driving trace.The present invention can effectively correct the accumulated error of the dead reckonings method such as visual odometry generation, good positioning accuracy can be obtained in the driving process of vehicle long range, calculating cost is small, strong real-time.
Description
Technical field
The present invention relates to the navigator fix technical methods of intelligent vehicle, more particularly to a kind of joint particle filter of multiposition
The vehicle positioning method of wave.
Background technique
Positioning refers to the ability that vehicle determines oneself position, and reliable and accurate vehicle location is the elder generation of autonomous vehicle navigation
Certainly condition.In order to solve this problem, traditional navigation system such as GPS, wheeled odometer are all widely used.So
And due to well-known " urban canyons " effect, GPS is easy to be blocked and can not obtain good positioning in urban environment
Precision.In this case, it often requires expensive GPS/INS system provides vehicle location.And wheeled odometer it is long away from
From when also will receive wheel-slip etc. influence and generate error.Currently, the two and three dimensions localization method quilt of many view-based access control models
Constantly propose.Although the three-dimension sensors such as laser radar can accurately obtain the position of terrestrial reference, its high cost limits it
It is widely used.On the other hand, visual odometry is a kind of image using only single or multiple video cameras to estimate vehicle movement
Method.Due to its high performance-price ratio, visual odometry slowly becomes a valuable potential positioning component in navigation system.
However, due to visual odometry step by step calculation vehicle location, the slight error that each frame can all introduce, and over time
And it accumulates.Reducing most common method of drifting about is bundle adjustment (BA) and simultaneous localization and mapping (SLAM).BA is by most
The cost function of smallization entirety weight delivery error provides optimal solution.SLAM executes closed loop to reduce drift accumulation, by constraining rail
Mark closure error reduces positioning cumulative errors.
In recent years, all kinds of Map Services (such as OpenStreetMap) are constantly universal, and all kinds of city road network maps become very
It is easy to get, as prior information, can be used to help to constrain positioning mistake.The present invention utilizes road network map, passes through anchor
The representation method and flexible multiposition of point combine particle filter method to realize the amendment to cumulative errors, to make vehicle
It positions more reliable.
Summary of the invention
In order to solve the problems, such as background technique, the purpose of the present invention is to provide a kind of joint particle filters of multiposition
The vehicle positioning method of wave, suitable for towards the intelligent vehicle positioning under urban environment.
The present invention can obtain the current Primary Location of vehicle under the support of visual odometry or inertial navigation system.It will
The vehicle movement track of acquisition and known road network map combine, by each anchor after motion profile is segmented by anchor point position
Point position is filtered, and corrects the position of each anchor point, to eliminate the accumulated error of dead reckoning method generation, makes positioning more
Add accurate.
The step of the technical solution adopted by the present invention, is as follows:
(1) road network map in vehicle driving region is established, road network map is made of the topological graph structure comprising node and side,
Known to position of the vehicle driving starting point in road network map；
(2) vehicle location is obtained by dead reckoning method in real time and detects anchor point, form initial vehicle subsection traveling
Track；
(3) for the anchor point detected every time, anchor point corresponding position in road network map is indicated using a pile particle,
It is calculated in real time by joint particle filter and updates every a pile particle weights；
(4) for every a pile particle, anchor point using the maximum particle of weight as anchor point in road network map successively connects
Anchor point is connect as revised vehicle subsection driving trace.
The dead reckoning method is using visual odometry etc..
In the step (1), road network map is established with the topological graph structure comprising node and side: node n=(lat,
Lon position) is indicated that lat, lon respectively indicate latitude and longitude, side e=(n by longitude and latitude1,n2) indicate node n1And n2It
Between connection relationship.
The step (2) specifically: obtain the vehicle location at each moment by dead reckoning method in real time, formed original
Vehicle driving trace；Anchor point in real-time detection original vehicle driving trace marks each anchor point and by successively line before and after anchor point
Section connection forms vehicle subsection driving trace.
The step (2) is specifically detected using following two mode simultaneously:
(2.1) current vehicle location is obtained by dead reckoning method in real time, by current vehicle location and nearest point
It is connected with straight line, calculates linear equation；If current vehicle location before detect anchor point not yet, using vehicle driving starting point as
Nearest point；If having detected anchor point before current vehicle location, made with the anchor point nearest from the current vehicle location time
For nearest point；
The vehicle location at each moment, will to the vertical range of straight line between calculating current vehicle location and nearest point
Vertical range and preset distance threshold compare: if each vertical range is respectively less than distance threshold, then it is assumed that work as front truck
New anchor point is not detected between position and nearest point；If there is vehicle location the hanging down to straight line at an at least moment
Straight distance is not less than distance threshold, then finds the vehicle location where the vertical range maximum moment as new anchor point；
The vehicle location of each frame is the current preliminary position of the vehicle obtained in real time by dead reckoning method.
(2.2) the too long situation for not detecting anchor point yet but of operating range in order to prevent, at the same when current vehicle location away from
When travelling longer than preset distance threshold value from nearest point, directly using current vehicle location as new anchor point；
For the new anchor point often detected, it is connected with nearest point with straight line, thus successively connects each anchor point
It connects and constitutes vehicle subsection driving trace, the line of adjacent anchor point forms line segment, and driving trace of the vehicle between two anchor points is used should
Line segment approximate representation.
In the step (3), after the driving trace of vehicle is segmented, each anchor point is indicated with a pile particle in road network
On figure then possible corresponding position carries out Federated filter process, step specific as follows:
For each current anchor, it is less than within the scope of error threshold finding the distance between current anchor in road network map
Node as node corresponding with current anchor, then according to following three kinds of different situations differentiations handle:
(1) node corresponding with current anchor is found, a pile particle is generated in the near nodal, is embodied with this
Node is that center Gaussian Profile generates particle, and directly calculates the weight of each particle；
(2) more than two nodes corresponding with current anchor are found, generate a pile particle, grain in these each near nodals
Son will retain with after current anchor anchor point generate particle carry out Federated filter (first do not calculate directly, but with later
The particle of anchor point is calculated together), obtain each particle weights；
(3) node corresponding with current anchor is not found, vertical range is less than error directly between current anchor
Particle is generated on side in threshold range, particle will retain the particle generated with the anchor point after current anchor and carry out joint filter
Wave (does not calculate directly first, but and the particle of anchor point is calculated together later), obtains each particle weights.
The particle weights calculate in the following ways:
Firstly, particle is attached to combine to form path candidate with all each particles under adjacent anchor point, adjacent anchor
Point is the anchor point adjacent with before and after particle corresponding anchor, calculates length and direction that each combines the path candidate to be formed, with
Line segment in vehicle subsection driving trace is compared, and passes through following formula (1) and (2) computational length and angular error:
eh=| hc-hs|
In formula, lcFor the length of path candidate, lsFor path candidate with corresponding line segment in vehicle subsection driving trace
Length, elFor error in length；hcFor the direction of path candidate, hsIt is path candidate corresponding with vehicle subsection driving trace
The direction of line segment, ehFor towards angular error；
Being calculated using the following equation each again, to combine the path candidate to be formed corresponding with vehicle subsection driving trace
Similarity s between line segments:
ss=α * sl+(1-α)*sh
Wherein, ssFor the similarity between corresponding line segment in path candidate and vehicle subsection driving trace, slFor length
Similarity, shFor towards the similarity at angle, α is obtained as weight parameter, size by experiment；
Then, the path candidate to be formed is combined for each, path candidate is the line segment connection shape that is connected by N anchor point
At being calculated using the following equation similarity of the average value as path candidate of N line segment similarity:
Wherein, spFor the similarity of path candidate, ss[i] is the i-th line section that path candidate includes, and N indicates path candidate
The line segment sum for being included；
Then, the same particle can include the grain of Q item difference path candidate for one in different path candidates
Son occurs in Q item difference path candidate that is, for a particle, is calculated using the following equation the weight of particle:
Wherein, w is the weight of particle, sp[j] is the j-th strip path candidate where particle, and Q indicates the time that particle occurs
The total number of routing diameter；
Finally, all particle weights under one anchor point are normalized using following formula:
Wherein, M indicates that the sum of the particle under anchor point, w [k] indicate the weight of k-th of particle under anchor point,It indicates
The weight of k-th of particle after normalization under anchor point.
In the step (4), anchor point of the maximum particle of weight as anchor point in road network map using under anchor point, successively
After anchor point is connected as revised vehicle subsection driving trace, the final vehicle location between two neighboring anchor point is adopted
It determines with the following methods:
For a certain moment f between anchor point AP1 and anchor point AP2, by calculating the vehicle location of moment f to anchor point
The distance and anchor point AP1 of AP1 is calculated to the ratio of the distance between anchor point AP2 obtains the revised position of moment f positioning；
In formula,WithRespectively indicate the position of two anchor points, pAP1And pAP2Respectively indicate two anchor pointsWithCorresponding anchor point,For the vehicle location of moment f, pfRevised vehicle location is positioned for moment f.
The present invention uses graph model to establish the road network map in vehicle driving region first.It is pushed away by the boat such as visual odometry position
Calculation method obtains original vehicle driving trace.In order to effectively match various serpentine tracks and road, the invention proposes one kind
Based on the representation method of anchor point (AP), anchor point characterizes main bending point on track.Track of vehicle is divided according to anchor point
Duan Hou corrects corresponding position of each anchor point in road network map by implementing the method for multiposition joint particle filter, from
And the accumulated error of dead reckoning method generation is eliminated, improve the precision of positioning.
Compared with the background art, the invention has the advantages that:
(1) present invention can rely on merely the dead reckonings method such as vision or inertial navigation and can be realized in conjunction with road network map
Reliable location；
(2) present invention can effectively correct the accumulated error of dead reckoning algorithm generation, improve the precision of positioning over long distances；
(3) present invention can successfully manage the various roads situation such as linear section, curve section；
(4) present invention uses the method frame of multiposition Federated filter, carries out grain with traditional every frame data arrival moment
The method of son filtering is different, and particle filter is only carried out at the anchor point of track, substantially reduces operand, and with traditional every frame
Filter result difference is obtained, implements multipoint Federated filter, in conjunction with front and back information, improves locating effect.
For synthesis, the present invention can effectively correct the accumulated error of the dead reckonings method such as visual odometry generation, straight
The sections such as line, curve can be applicable in, and good positioning accuracy can be obtained in the driving process of vehicle long range.Simultaneously originally
Inventing has the anchor point obtained in track in real time, and adaptively estimates a series of ability of anchor point positions.In addition, with traditional
Particle filter method is compared, and anchor point position of this method only in track is filtered, and calculating cost is small, strong real-time.
Detailed description of the invention
Fig. 1 is the road network map schematic diagram of embodiment；
Fig. 2 is the segmentation of vehicle initial trace and anchor point schematic diagram；
Fig. 3 is multiposition joint particle filter implementation steps schematic diagram；
Fig. 4 is KITTY00 serial visual odometer result figure；
Fig. 5 is KITTY00 sequence this method result figure；
Fig. 6 is KITTY00 sequence true value trajectory diagram；
Fig. 7 is that KITTY00 serial visual odometer and this method position error compare figure.
Specific embodiment
The present invention will be further described with reference to the accompanying drawings and examples.
The embodiment implemented according to summary of the invention complete method of the present invention is as follows:
(1) road network map in vehicle driving region is established, as shown in Figure 1, road network map is by the topology comprising node and side
Graph structure is constituted, known to position of the vehicle driving starting point in road network map；
(2) vehicle location for obtaining each moment by dead reckoning method in real time, forms original vehicle driving trace；It is real
When detection original vehicle driving trace in anchor point, mark each anchor point and by successively line segment connects to form vehicle point before and after anchor point
Section driving trace, as shown in Figure 2.
(2.1) current vehicle location is obtained by dead reckoning method in real time, by current vehicle location and nearest point
It is connected with straight line；If anchor point is detected not yet before current vehicle location, using vehicle driving starting point as nearest point；If
Anchor point is detected before current vehicle location, using the anchor point nearest from the current vehicle location time as nearest point；
The vehicle location at each moment between current vehicle location and nearest point is calculated to the vertical range of straight line, by vertical range
Compare with preset distance threshold: if each vertical range is respectively less than distance threshold, then it is assumed that current vehicle location with
New anchor point is not detected between nearest point；If there is an at least moment vehicle location to straight line vertical range not
Less than distance threshold, then the vehicle location where the vertical range maximum moment is found as new anchor point；
(2.2) simultaneously when current vehicle location travels longer than preset distance threshold value apart from nearest point, directly
It connects using current vehicle location as new anchor point；For the new anchor point often detected, it is connected with nearest point with straight line, by
Each anchor point is sequentially connected by this constitutes vehicle subsection driving trace, and the line of adjacent anchor point forms line segment.
(3) for the anchor point detected every time, anchor point corresponding position in road network map is indicated using a pile particle,
It is calculated in real time by joint particle filter and updates every a pile particle weights；
The thought and specific reality of multiposition joint particle filter are stated with a simply example (as shown in Figure 3) below
Apply step.Fig. 3 (a) is after dead reckoning method obtains vehicle initial trace and implements vehicle subsection obtained from detecting anchor point
Track, there are five anchor point (terminals also as anchor point processing) in track, therefore we indicate this five anchors with five heap particles
Point corresponding position in map.The step of the following are specific filtering:
Step 1: assuming that start node (node 0) known to position in road network, when we are detecting first anchor point
(AP1), two nodes (gray area) are considered as the possible corresponding node of current anchor in road network map, we are at the two
Near nodal generates particle, and in this case, without starting filtering immediately, each particle is retained, and the position of AP1 will be
It is determined in later the step of, as shown in Fig. 3 (b).
Step 2: in Fig. 3 (c), a new anchor point (AP2) has been detected.We have found that the corresponding node of AP2 can be with
It uniquely determines, we generate the new particle of a pile near corresponding node.Since corresponding node determines AP2 on map, AP1
Corresponding node can also determine simultaneously.Implement multiposition Federated filter algorithm, two groups of particles corresponding with two anchor points at this time
Weight updated.Anchor point of the maximum particle of weight as anchor point in road network map in every group of particle.Then will
Starting point is sequentially connected with the two anchor points, obtains amendment corresponding with filter result path, the dashed trace as shown in Fig. 3 (c).
Step 3: when detecting new anchor point (AP3), since track of vehicle that may be present and road network map miss
Difference, AP3 can not be found in threshold range may corresponding node in road network.At this point, we are directly in coincidence loss threshold value
Particle is generated on the side of the road network map of range, particle will retain the particle generated with subsequent anchor point and carry out Federated filter, such as
Shown in Fig. 3 (d).In this case, the position of AP3 will determine in the steps afterwards.
Step 4: when detecting new anchor point (AP4), the corresponding node in road network can well really
It is fixed, as shown in Fig. 3 (e).It is similar with the situation in step 2, implement multiposition Federated filter, obtains AP3 and AP4 in road network
Anchor point on figure.
Step 5: when detecting new anchor point (AP5), its corresponding node can be directly determined, implements multidigit
It sets Federated filter and determines its final position.Finally, according to the dotted line in Fig. 3 (f), the amendment road of available vehicle initial trace
Diameter.
(4) for every a pile particle, anchor point using the maximum particle of weight as anchor point in road network map successively connects
Anchor point is connect as revised vehicle subsection driving trace.
Tested K ITTY data set assesses the effect of this method, is made with binocular image sequence and corresponding road network map
The mean error and worst error of visual odometry and this method and true value are compared according to the true value that KITTY is provided for input.
Experiment mainly tests KITTY image sequence 00.The result of 00 visual odometry of sequence is as shown in figure 4, this method
Result as shown in figure 5, true value is as shown in Figure 6, it can be seen that visual odometry has accumulated biggish angular error in corner,
The accumulated error of positioning is caused to greatly increase, and this method can effectively eliminate accumulated error, maintain position error always
In a smaller range, and vehicle driving also has always preferable positioning result in longer situation.Error pair
Than curve as shown in fig. 7, average localization error is reduced to 3.5m from 55.8m, maximum positioning error is reduced to 9.2m from 181.1m,
It is detailed in the following table 1.
1 visual odometry of table is compared with this method position error
| Sequence 00 | Mean error/m | Worst error/m |
| Visual odometry | 55.8 | 181.1 |
| This method | 3.5 | 9.2 |
Experiment uses an outfit dominant frequency for the computer of the intel i7 processor of 3.4GHz and 8G memory, in experiment every time
Iteration is averaged time-consuming 115.21ms.Since this method filtering only occurs at the anchor point of track, carried out without every frame, so phase
For original visual odometer, increased calculating cost very little.
As can be seen that using the method for the present invention can effectively reduce visual odometry generation accumulated error, even if compared with
In long driving path, position error is also constantly in a smaller range, and it is smaller to calculate cost.
Claims (7)
1. a kind of vehicle positioning method of multiposition joint particle filter, which comprises the steps of:
(1) road network map in vehicle driving region is established, road network map is made of the topological graph structure comprising node and side, vehicle
It travels known to position of the starting point in road network map；
(2) vehicle location is obtained by dead reckoning method in real time and detects anchor point, form initial vehicle subsection driving trace；
(3) for the anchor point detected every time, anchor point corresponding position in road network map is indicated using a pile particle, is passed through
Joint particle filter calculates in real time updates every a pile particle weights；
(4) for every a pile particle, anchor point using the maximum particle of weight as anchor point in road network map, it is fixed to be sequentially connected
Site is as revised vehicle subsection driving trace.
2. a kind of vehicle positioning method of multiposition joint particle filter according to claim 1, it is characterised in that: described
In step (1), establish road network map with the topological graph structure comprising node and side: the position of node n=(lat, lon) is by passing through
Latitude indicates that lat, lon respectively indicate latitude and longitude, side e=(n1,n2) indicate node n1And n2Between connection relationship.
3. a kind of vehicle positioning method of multiposition joint particle filter according to claim 1, it is characterised in that: described
Step (2) specifically: obtain the vehicle location at each moment by dead reckoning method in real time, form original vehicle and travel rail
Mark；Anchor point in real-time detection original vehicle driving trace marks each anchor point and by successively line segment is connected and to be formed before and after anchor point
Vehicle subsection driving trace.
4. a kind of vehicle positioning method of multiposition joint particle filter according to claim 1 or 3, it is characterised in that:
The step (2) is specifically detected using following two mode simultaneously:
(2.1) current vehicle location is obtained by dead reckoning method in real time, by current vehicle location and nearest point with directly
Line is connected；If anchor point is detected not yet before current vehicle location, using vehicle driving starting point as nearest point；If current
Anchor point is detected before vehicle location, using the anchor point nearest from the current vehicle location time as nearest point；
The vehicle location at each moment, will be vertical to the vertical range of straight line between calculating current vehicle location and nearest point
Distance and preset distance threshold compare: if each vertical range is respectively less than distance threshold, then it is assumed that current vehicle position
It sets and does not detect new anchor point between nearest point；If there is an at least moment vehicle location to straight line it is vertical away from
From distance threshold is not less than, then the vehicle location where the vertical range maximum moment is found as new anchor point；
(2.2) simultaneously when current vehicle location travels longer than preset distance threshold value apart from nearest point, directly will
Current vehicle location is as new anchor point；
For the new anchor point often detected, it is connected with nearest point with straight line, each anchor point is thus sequentially connected structure
At vehicle subsection driving trace, the line of adjacent anchor point forms line segment.
5. a kind of vehicle positioning method of multiposition joint particle filter according to claim 1, it is characterised in that: described
In step (3), after the driving trace of vehicle is segmented, indicate that each anchor point may be corresponding in road network map with a pile particle
Position then carry out Federated filter process, step specific as follows:
For each current anchor, it is less than the section within the scope of error threshold finding the distance between current anchor in road network map
Then point is handled as node corresponding with current anchor according to following three kinds of different situations differentiations:
(1) node corresponding with current anchor is found, generates a pile particle in the near nodal, and directly calculate each grain
The weight of son；
(2) more than two nodes corresponding with current anchor are found, generate a pile particle in these each near nodals, particle is equal
The particle generated with the anchor point after current anchor will be retained and carry out Federated filter, obtain each particle weights；
(3) node corresponding with current anchor is not found, vertical range is less than error threshold directly between current anchor
Particle is generated on side in range, particle will retain the particle generated with the anchor point after current anchor and carry out Federated filter,
Obtain each particle weights.
6. a kind of vehicle positioning method of multiposition joint particle filter according to claim 1, it is characterised in that: described
Particle weights calculate in the following ways:
Firstly, particle is attached to combine to form path candidate with all each particles under adjacent anchor point, adjacent anchor point is
The adjacent anchor point with before and after particle corresponding anchor calculates length and direction that each combines the path candidate to be formed, with vehicle
Line segment in segmentation driving trace is compared, and passes through following formula (1) and (2) computational length and angular error:
eh=| hc-hs|
In formula, lcFor the length of path candidate, lsFor path candidate with the length of corresponding line segment in vehicle subsection driving trace
Degree, elFor error in length；hcFor the direction of path candidate, hsFor path candidate with corresponding line in vehicle subsection driving trace
The direction of section, ehFor towards angular error；
It is calculated using the following equation each again and combines the path candidate to be formed and corresponding line segment in vehicle subsection driving trace
Between similarity ss:
ss=α * sl+(1-α)*sh
Wherein, ssFor the similarity between corresponding line segment in path candidate and vehicle subsection driving trace, slFor the similar of length
Degree, shFor towards the similarity at angle, α is as weight parameter；
Then, the path candidate to be formed is combined for each, path candidate is to be connected to be formed by the connected line segment of N anchor point, is adopted
The similarity of path candidate is calculated with following formula:
Wherein, spFor the similarity of path candidate, ss[i] is the i-th line section that path candidate includes, and N indicates that path candidate is wrapped
The line segment sum contained；
Then, for a particle, occur in Q item difference path candidate, be calculated using the following equation the weight of particle:
Wherein, w is the weight of particle, sp[j] is the j-th strip path candidate where particle, and Q indicates the candidate road that particle occurs
The total number of diameter；
Finally, all particle weights under one anchor point are normalized using following formula:
Wherein, M indicates that the sum of the particle under anchor point, w [k] indicate the weight of k-th of particle under anchor point,Indicate normalizing
The weight of k-th of particle after change under anchor point.
7. a kind of vehicle positioning method of multiposition joint particle filter according to claim 1, it is characterised in that:
In the step (4), anchor point of the maximum particle of weight as anchor point in road network map, is sequentially connected using under anchor point
After anchor point is as revised vehicle subsection driving trace, the final vehicle location between two neighboring anchor point use with
Under type determines:
For a certain moment f between anchor point AP1 and anchor point AP2, by calculating the vehicle location of moment f to anchor point AP1's
Distance and anchor point AP1 are calculated to the ratio of the distance between anchor point AP2 obtains the revised position of moment f positioning；
In formula,WithRespectively indicate the position of two anchor points, pAP1And pAP2Respectively indicate two anchor pointsWithIt is right
The anchor point answered,For the vehicle location of moment f, pfRevised vehicle location is positioned for moment f.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810716968.2A CN108981702A (en) | 2018-07-03 | 2018-07-03 | A kind of vehicle positioning method of multiposition joint particle filter |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810716968.2A CN108981702A (en) | 2018-07-03 | 2018-07-03 | A kind of vehicle positioning method of multiposition joint particle filter |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN108981702A true CN108981702A (en) | 2018-12-11 |
Family
ID=64536503
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810716968.2A Pending CN108981702A (en) | 2018-07-03 | 2018-07-03 | A kind of vehicle positioning method of multiposition joint particle filter |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108981702A (en) |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109815979A (en) * | 2018-12-18 | 2019-05-28 | 通号通信信息集团有限公司 | A kind of weak label semantic segmentation nominal data generation method and system |
| CN110243368A (en) * | 2019-04-29 | 2019-09-17 | 丰疆智能科技研究院(常州)有限公司 | The driving trace of intelligent agricultural machinery establishes system and its application |
| CN112118535A (en) * | 2020-08-12 | 2020-12-22 | 深圳技术大学 | Vehicle roaming area prediction method and system |
| CN112180940A (en) * | 2020-10-16 | 2021-01-05 | 三一机器人科技有限公司 | Mapping method and device for reverse positioning and vehicle operation method and device |
| CN112747744A (en) * | 2020-12-22 | 2021-05-04 | 浙江大学 | Vehicle positioning method combining dead reckoning and multi-lane road network map |
| CN112771352A (en) * | 2020-03-19 | 2021-05-07 | 华为技术有限公司 | Vehicle positioning method and device |
| CN112947495A (en) * | 2021-04-25 | 2021-06-11 | 北京三快在线科技有限公司 | Model training method, unmanned equipment control method and device |
| GB2597335A (en) * | 2020-07-20 | 2022-01-26 | Navenio Ltd | Map matching trajectories |
| CN115660390A (en) * | 2022-12-29 | 2023-01-31 | 北京易控智驾科技有限公司 | Mine work area control method, control device, electronic device, and storage medium |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104236575A (en) * | 2014-09-16 | 2014-12-24 | 百度在线网络技术（北京）有限公司 | Travel path recording method and device as well as navigation method and device |
| CN104819716A (en) * | 2015-04-21 | 2015-08-05 | 北京工业大学 | Indoor and outdoor personal navigation algorithm based on INS/GPS (inertial navigation system/global position system) integration of MEMS (micro-electromechanical system) |
| CN106017486A (en) * | 2016-05-16 | 2016-10-12 | 浙江大学 | Trajectory inflection point filter-based map location method for unmanned vehicle navigation |
| CN106370193A (en) * | 2016-08-30 | 2017-02-01 | 上海交通大学 | Vehicle integrated positioning system and method based on map matching |
| JP2017142168A (en) * | 2016-02-10 | 2017-08-17 | 富士通株式会社 | Information processing device, track information correction method, and track information correction program |
| CN107462260A (en) * | 2017-08-22 | 2017-12-12 | 上海斐讯数据通信技术有限公司 | A kind of trace generator method, apparatus and wearable device |
| DE112016002947T5 (en) * | 2015-08-03 | 2018-03-15 | Scania Cv Ab | Method and system for controlling the driving of a vehicle along a road |
| CN107830862A (en) * | 2017-10-13 | 2018-03-23 | 桂林电子科技大学 | A kind of method of the indoor positioning pedestrian tracking based on smart mobile phone |
-
2018
- 2018-07-03 CN CN201810716968.2A patent/CN108981702A/en active Pending
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104236575A (en) * | 2014-09-16 | 2014-12-24 | 百度在线网络技术（北京）有限公司 | Travel path recording method and device as well as navigation method and device |
| CN104819716A (en) * | 2015-04-21 | 2015-08-05 | 北京工业大学 | Indoor and outdoor personal navigation algorithm based on INS/GPS (inertial navigation system/global position system) integration of MEMS (micro-electromechanical system) |
| DE112016002947T5 (en) * | 2015-08-03 | 2018-03-15 | Scania Cv Ab | Method and system for controlling the driving of a vehicle along a road |
| JP2017142168A (en) * | 2016-02-10 | 2017-08-17 | 富士通株式会社 | Information processing device, track information correction method, and track information correction program |
| CN106017486A (en) * | 2016-05-16 | 2016-10-12 | 浙江大学 | Trajectory inflection point filter-based map location method for unmanned vehicle navigation |
| CN106370193A (en) * | 2016-08-30 | 2017-02-01 | 上海交通大学 | Vehicle integrated positioning system and method based on map matching |
| CN107462260A (en) * | 2017-08-22 | 2017-12-12 | 上海斐讯数据通信技术有限公司 | A kind of trace generator method, apparatus and wearable device |
| CN107830862A (en) * | 2017-10-13 | 2018-03-23 | 桂林电子科技大学 | A kind of method of the indoor positioning pedestrian tracking based on smart mobile phone |
Non-Patent Citations (2)
| Title |
|---|
| 杨卫军等: "区域生长辅助的地图配准在室内定位中的应用", 《传感器与微系统》 * |
| 金亦东: "《结合路网地图的视觉定位优化方法研究》", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109815979A (en) * | 2018-12-18 | 2019-05-28 | 通号通信信息集团有限公司 | A kind of weak label semantic segmentation nominal data generation method and system |
| CN110243368A (en) * | 2019-04-29 | 2019-09-17 | 丰疆智能科技研究院(常州)有限公司 | The driving trace of intelligent agricultural machinery establishes system and its application |
| CN112771352A (en) * | 2020-03-19 | 2021-05-07 | 华为技术有限公司 | Vehicle positioning method and device |
| GB2597335A (en) * | 2020-07-20 | 2022-01-26 | Navenio Ltd | Map matching trajectories |
| CN112118535A (en) * | 2020-08-12 | 2020-12-22 | 深圳技术大学 | Vehicle roaming area prediction method and system |
| CN112180940A (en) * | 2020-10-16 | 2021-01-05 | 三一机器人科技有限公司 | Mapping method and device for reverse positioning and vehicle operation method and device |
| CN112747744A (en) * | 2020-12-22 | 2021-05-04 | 浙江大学 | Vehicle positioning method combining dead reckoning and multi-lane road network map |
| CN112747744B (en) * | 2020-12-22 | 2022-11-18 | 浙江大学 | Vehicle positioning method combining dead reckoning and multi-lane road network map |
| CN112947495A (en) * | 2021-04-25 | 2021-06-11 | 北京三快在线科技有限公司 | Model training method, unmanned equipment control method and device |
| CN115660390A (en) * | 2022-12-29 | 2023-01-31 | 北京易控智驾科技有限公司 | Mine work area control method, control device, electronic device, and storage medium |
| CN115660390B (en) * | 2022-12-29 | 2023-09-08 | 北京易控智驾科技有限公司 | Control method and control device for mine working area, electronic equipment and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108981702A (en) | A kind of vehicle positioning method of multiposition joint particle filter | |
| CN106017486B (en) | A kind of Orientation on map method based on the filtering of track inflection point towards unmanned vehicle navigation | |
| US11530924B2 (en) | Apparatus and method for updating high definition map for autonomous driving | |
| CN106574975B (en) | Trace matching using peripheral signals | |
| CN108759833A (en) | A kind of intelligent vehicle localization method based on priori map | |
| JP6427908B2 (en) | Map information generation system, method and program | |
| CN102538781B (en) | Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method | |
| CN105606102B (en) | Grid model based PDR indoor positioning method and system | |
| JP2022113746A (en) | Determination device | |
| JP6492469B2 (en) | Own vehicle travel lane estimation device and program | |
| WO2020224305A1 (en) | Method and apparatus for device positioning, and device | |
| CN107782321A (en) | A kind of view-based access control model and the Combinated navigation method of high-precision map lane line constraint | |
| KR20170088228A (en) | Map building system and its method based on multi-robot localization | |
| CN112747744B (en) | Vehicle positioning method combining dead reckoning and multi-lane road network map | |
| Blazquez et al. | Simple map-matching algorithm applied to intelligent winter maintenance vehicle data | |
| US20230071794A1 (en) | Method and system for building lane-level map by using 3D point cloud map | |
| CN111397599A (en) | Improved ICCP (Integrated Circuit chip) underwater geomagnetic matching method based on triangular matching algorithm | |
| CN110441760A (en) | A kind of large-scale seabed topographic map expansion patterning process based on priori topographic map | |
| CN111123953B (en) | Particle-based mobile robot group under artificial intelligence big data and control method thereof | |
| CN114879660A (en) | Robot environment sensing method based on target driving | |
| CN109387198A (en) | A kind of inertia based on sequential detection/visual odometry Combinated navigation method | |
| JP2012215442A (en) | Own position determination system, own position determination program, own position determination method | |
| US11131552B2 (en) | Map generation system | |
| Wong et al. | Single camera vehicle localization using SURF scale and dynamic time warping | |
| JP6680319B2 (en) | Map information generation system, method and program |
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 | ||
| WD01 | Invention patent application deemed withdrawn after publication | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20181211 |