Summary of the invention
The technical matters that (one) will solve
The technical matters that the present invention will solve is: how to write down the continuous path that automobile is passed by exactly; The zone bad at gps signal simultaneously, that road is intensive obtains the better map matching result, so that map-matching algorithm can be applied in the automobile charge system better.
(2) technical scheme
For solving the problems of the technologies described above, the invention provides a kind of many hypothesis map-matching method that shifts based on vehicle condition, may further comprise the steps:
S1, obtain global position system GPS data and inertial navigation system INS data;
S2, according to gps data and INS data definition hypothesis tree more than, said many hypothesis trees are used to write down all paths that automobile is passed by, it has a root node, said root node is represented the starting point of automobile;
S3, when gps data of the every acquisition of automobile, in many hypothesis trees, generate new hypothesis node;
S4, according to the weight of said gps data and the said hypothesis node of INS data computation, said weight is represented the information that this hypothesis node comprises and the similarity of real information;
S5, according to the weight deletion a part of hypothesis node of hypothesis node;
S6, the maximum hypothesis node of selection weight are supposed node as the best.
Repeating step S1~S5 then, each is taken turns and can obtain a best hypothesis node, and the branch at this best hypothesis node place has write down the path locus that automobile is passed by.
Wherein, when in step S2, defining many hypothesis trees, suppose that each said hypothesis node comprises following information at least:
Suppose the time that node produces;
Automobile current state, this state have four kinds: initial, stop, quickening and move, the automobile current state is passed through the acquisition of INS data computation;
The current path link that possibly belong to of automobile;
The direction that automobile moves on link;
Automobile is at Δ
tThe angle that turns in time, it obtains through the INS data;
Automobile is at Δ
tThe mileage of passing by in the time, it obtains through the INS data;
The direction that automobile is current, it obtains through gps data and INS data;
The mileage that automobile is passed by on link;
Suppose the weight of node;
Suppose the activity of node, represent the hypothesis node of automobile current state to be called live-vertex, represent the hypothesis node of automobile historic state to be called non-live-vertex.
Wherein, in step S2, be father node with the root node, to the path that each bar automobile possibly belong to, generate two the path is identical but hypothesis node that direction is different, step specifically comprises:
S21, the validity of judgement gps data if gps data is effectively then carried out following step, otherwise is not carried out initialization;
S22 calculates the automobile current state;
S23 is the center of circle with the gps coordinate, is that radius is drawn a circle with the error range R of GPS, finds all and this circle that the path of common factor is arranged, and these paths all are the current path link that possibly belong to of automobile;
S24 to the possible path link of each bar, is father node with the root node; Generate two hypothesis nodes of representing this path, suppose the in the opposite direction of nodes for two, represent the both direction in this path; These two all possibly be the automobile current state, and the out of Memory of hypothesis node is set then;
S25, said father node is set to non-live-vertex, and the hypothesis node that is generated is set to live-vertex.
Wherein, When obtaining gps data for the first time; Judge according to effective marker position, precision and the number of satellite of said gps data whether these data are effective, when obtaining gps data once more, effective marker position, precision, number of satellite and gps data and the Δ of said gps data
tThe range difference of preceding gps data is apart from judging whether these data are effective.
Wherein, if to suppose that node j is that father node generates new hypothesis node, the step that then generates new hypothesis node among the step S3 is specially: state and automobile current state according to said hypothesis node j generate new hypothesis node.
Wherein, the step of the said weight of calculating is following among the step S4:
Suppose G
n, A
n, O
nThe mileage number that gps coordinate when being illustrated respectively in t=n, automobile deflects angle and automobile are passed by, the current path h that possibly belong to of automobile during to any t=n uses X
kThe position and the automobile steering information of hypothesis node when being illustrated in t=k, then the weight calculation formula of path h is following:
P (* | * *) is illustrated in the conditional probability of calculating " * " under " * * " condition.
Wherein, step S5 is specially:
The hypothesis node that all are active sorts by weight from big to small;
Suppose only to allow to occur MaxActiveHypos active hypothesis node, then weighting weighs maximum MaxActiveHypos active hypothesis node, and deletes other nodes;
Suppose that many hypothesis trees at most only preserve the result in MaxHypoLength step, then begin back to travel through, fall surpassing the hypothesis knot removal that MaxHypoLength goes on foot from each live-vertex.
(3) beneficial effect
All paths that method of the present invention possibly passed by automobile all are recorded on the tree data structure; With tree root is the starting point of automobile; Gps data of the every acquisition of automobile all upgrades once many hypothesis trees, in tree, adds new leaf node (promptly supposing node).Each branch is all being represented one section road sequence that automobile possibly passed by in the tree, thereby has solved the problem that short path is omitted through the continuity that guarantees the path; Through preserving all possibilities simultaneously, even and guaranteed to occur once in a while than mistake at GPS, still can correct the mistake that produces than mistake because of this afterwards; In addition,, the INS data in method, have been introduced: when GPS is accurate, use gps data, when gps data is inaccurate, use the INS data that automobile position is estimated in order to overcome the bad problem of gps signal.
Embodiment
Below in conjunction with accompanying drawing and embodiment, specific embodiments of the invention is done further explain.Following examples are used to explain the present invention, but are not used for limiting scope of the present invention.
As shown in Figure 2, method of the present invention may further comprise the steps:
S1, obtain global position system GPS data and inertial navigation system INS data;
S2, according to gps data and INS data (below be abbreviated as the GPS/INS data) definition hypothesis tree more than, the definition of said many hypothesis trees is following:
Many hypothesis trees are one tree (as shown in Figure 1, among Fig. 1, r1~r7 represent the path, and d1~d7 representes the travel direction of automobile on this path);
This tree has only a root node (root node, the i.e. node represented of r1 and d1), represents the starting point of automobile;
Except root node, all the other all nodes all are the hypothesis nodes that produces because of the GPS/INS data.The information that each hypothesis node comprises at least is following:
Parent: the pointer that points to father node.
BeginTime: suppose the time that node produces.
Type: the running status that automobile is current.The possible state of automobile has four: initial (initially); Stopping (stopping); Accelerating (acceleration); Running (operation).The automobile current states can obtain through the INS data computation.This automobile current state, automobile possibly belong to path, the direction of automobile on this path, have formed the state of this hypothesis node jointly.
Link: the current path that possibly belong to of automobile.
Direction: the direction that automobile moves on current link.
TurnAngle: automobile is at Δ
tThe angle that turns in time.Obtain through the INS data.
RunDis: automobile is at Δ
tThe mileage of passing by in the time.Obtain through the INS data.
FinalBear: the direction that automobile is current.Obtain through the GPS/INS data.
DisOnLink: the mileage that automobile is passed by on this link.
Weight: the weight of supposing node.Said weight is represented the information that this hypothesis node comprises and the similarity of real information.
Active: the activity of supposing node.Represent the hypothesis node of automobile current state to be called live-vertex, the hypothesis node of representing the automobile historic state is non-live-vertex.When automobile receives the GPS/INS data, have only live-vertex can generate new hypothesis node.
Any one hypothesis node and all child nodes thereof constitute hypothesis tree more than again.
Before obtaining first effective GPS data, be to obtain the current absolute position of automobile.So the initialization of many hypothesis trees is a prerequisite with effective GPS data.Before obtaining first gps data, many hypothesis trees are in no initializtion state (initial has only the root node) always.
In step S2, be father node with the root node, the steps that generate two hypothesis nodes specifically comprise:
S21, the validity of judgement gps data if gps data is effectively then carried out following step, otherwise is not carried out initialization;
S22 calculates the automobile current state;
S23 is the center of circle with the gps coordinate, is that radius is drawn a circle with the error range R of GPS, finds all and this circle that the path of common factor is arranged, and these paths all are the current path link that possibly belong to of automobile;
S24 to the possible path link of each bar, is father node with the root node; Generate two hypothesis nodes of representing this path, suppose the in the opposite direction of nodes for two, represent the both direction in this path; These two all possibly be the automobile current state, and the out of Memory of hypothesis node is set then;
S25, said father node is set to non-live-vertex, and the hypothesis node that is generated is set to live-vertex.
The factor that influences gps data validity has a lot.In gps data, have three parameters directly to influence the reliability of gps data, they are: the precision (PDOP) of the effective marker position of gps data, gps data, number of satellite.When the effective marker position of gps data this gps data of mark is invalid, can think the current no signal of GPS; When these data of effective marker mark of gps data were effective, the PDOP value in the gps data had provided the precision of this gps data; When number of satellite less than 4 the time, we can think that this gps data is unreliable.So, have only when the gps data zone bit effectively, and the gps data precision is greater than certain threshold value, and number of satellite is not less than at 4 o'clock, we think that just this gps data is effective.When obtaining gps data for the first time, we can only judge the validity of gps data according to the standard of S211
In addition, in order to guarantee the reliability of algorithm, when obtaining gps data for the second time and later on, also introduced the factor of another one judgement gps data validity.Because the speed of automobilism is limited, automobile is at Δ
tThe mileage of passing by in the time also is limited.So, if thought Δ
tGps data before time is effectively, and current GPS data and Δ
tBefore the range difference of gps data apart from greater than certain threshold value the time, can think that then the current GPS data are invalid.Otherwise, can think that this gps data is effective.
Fig. 3 explains running status how to judge automobile now:
The possible running status of automobile is following:
Initial: original state.When automobile just starts, think that automobile is in this state.
Stopping: halted state.Automobile is at Δ
tSpeed in time and deflection angle think that automobile is in the stopping state during all less than threshold value necessarily.
Running: running status.Automobile is at Δ
tSpeed in time is greater than certain threshold value, and deflection angle thinks that automobile is in the running state during less than certain threshold value.
Accelerating: acceleration mode., automobile during three kinds of states, thinks that automobile is in acceleration mode above not being in.
The mode of judging motoring condition is following:
Judge the validity of gps data.If gps data is invalid, and many hypothesis trees no initializtion also, then explanation need not be calculated vehicle condition.
According to the INS data of automobile, calculate speed, yawing moment and the mileage of automobile.
If the speed of automobile is less than stopping threshold speed, and the automobile deflects angle is during less than the stop angle threshold value, and it is the stopping state that vehicle condition is set.
If the degree of automobile is greater than operational threshold speed, and the automobile deflects angle is during less than the operation angle threshold value, and it is the running state that vehicle condition is set.
Under other situation, it is the accelerating state that vehicle condition is set.
S3, when gps data of the every acquisition of automobile, in many hypothesis trees, generate new hypothesis node; If with hypothesis node j is that father node generates new hypothesis node, the step that then generates new hypothesis node among the step S3 is specially: state and automobile current state according to said hypothesis node j generate new hypothesis node (being the child node of selected hypothesis node).
Among Fig. 4, (a) previous state of expression automobile; (b) the expression automobile is stopping from the stopping state exchange; (c) expression is accelerating from the stopping state exchange; (d) expression converts stopping into from accelerating; (e) expression converts accelerating into from accelerating; (f) expression converts stopping into from running; (g) expression converts accelerating into from running; (h) expression converts running into from stopping; (i) expression converts running into from accelerating; (j) expression converts running into from runing.
As shown in Figure 4, the mode that generates said new hypothesis node is following:
Non-live-vertex does not produce new hypothesis node.
Like Fig. 4 (b), if the vehicle condition in the selected hypothesis node is stopping, and the automobile current state is stopping, then need not produce new node.
Like Fig. 4 (c); If the vehicle condition in the selected hypothesis node is stopping, and the automobile current state is accelerating, then generates two node for selected node; One child node is identical with the direction of father node, another child node and father node in the opposite direction.
Like Fig. 4 (d), if the vehicle condition in the selected hypothesis node is accelerating, and the automobile current state is stopping, then is that selected node generates a child node identical with the father node direction.It is non-live-vertex that father node is set.
Like Fig. 4 (e), if the vehicle condition in the selected hypothesis node is accelerating, and the automobile current state is accelerating, and the automobile distance of on Ben Tiaolu, passing by does not then produce new node less than the length of this road.Otherwise, find and terminal all roads that link to each other of this road, to each bar road, generate a new son hypothesis node.
Like Fig. 4 (f), if the vehicle condition in the selected hypothesis node is running, and the automobile current state is stopping, then is that selected hypothesis node generates a child node identical with the father node direction.It is non-live-vertex that father node is set.
Like Fig. 4 (g), if the vehicle condition in the selected hypothesis node is running, and the automobile current state is accelerating, then is that selected hypothesis node generates a child node identical with the father node direction.It is non-live-vertex that father node is set.
Like Fig. 4 (h); If the vehicle condition in the selected hypothesis node is stopping, and the automobile current state is running, then generates two node for selected hypothesis node; One child node is identical with the direction of father node, another child node and father node in the opposite direction.It is non-live-vertex that father node is set.
Like Fig. 4 (i); If the vehicle condition in the selected hypothesis node is accelerating, and the automobile current state is running, then generates two node for selected hypothesis node; One child node is identical with the direction of father node, another child node and father node in the opposite direction.It is non-live-vertex that father node is set.
Like Fig. 4 (j), if the vehicle condition in the selected hypothesis node is running, and the automobile current state is running, and the automobile distance of on this road, passing by does not then produce new node less than the length of this road.Otherwise, find with this road terminal link to each other and with this road angle all roads less than certain threshold value, to each bar road, generate a new son and suppose node.
S4, according to the weight of said gps data and the said hypothesis node of INS data computation, said weight is represented the information that this hypothesis node comprises and the similarity of real information:
Use the GPS/INS data to calculate the weight of hypothesis node.Suppose G
n, A
n, O
nThe mileage number that gps coordinate when being illustrated respectively in t=n, automobile deflects angle and automobile are passed by, the current path h that possibly belong to of automobile during to any t=n uses X
kThe position and the automobile steering information of hypothesis node when being illustrated in t=k, then the weight calculation formula of path h is following:
P (* | * *) is illustrated in the conditional probability of calculating " * " under " * * " condition.
Top computing formula is merely the most basic computing formula.In addition, we also need consider more situation:
A. represent automobile on an one-way road during reverse going when some hypothesis nodes, we will carry out very big punishment to this weight of supposing node.This punishment is specially weight is deducted a very big fixed value OneWayPunishment.
B. when the direction in automobile steering and the path of a certain hypothesis through the INS data estimation differs greatly, carry out bigger punishment to this weight of supposing node.This punishment is specially weight is deducted a number, this number equal two angles difference square multiply by a COEFFICIENT K 1.
C. think that automobile has run to new path when a certain hypothesis, but through INS add up obtain in the distance of running on the original path during much smaller than the total length in original path, we will carry out less punishment to this weight of supposing node.This punishment is specially weight is deducted a number, and the path of not covering that this number equals to obtain according to the INS data is with respect to the COEFFICIENT K 2 that square multiply by of the ratio of total length.
S5, according to the weight deletion a part of hypothesis node of hypothesis node:
The hypothesis node that all are active sorts by weight from big to small.
Suppose only to allow to occur MaxActiveHypos active hypothesis node, then weighting weighs maximum MaxActiveHypos active hypothesis node, and deletes other nodes;
Suppose that many hypothesis trees at most only preserve the result in MaxHypoLength step, then begin back to travel through, fall surpassing the hypothesis knot removal that MaxHypoLength goes on foot from each live-vertex.MaxActiveHypos and MaxHypoLength are positive integer, and each the hypothesis node in the said branch is represented a step.
S6, select the maximum hypothesis node of weight to suppose node as the best, repeating step S1~S5 then, each is taken turns and can obtain a best hypothesis node, and the branch at this best hypothesis node place has write down the path locus that automobile is passed by.We can use this best hypothesis node; Come calculating optimum to suppose the degree of confidence of node with second largest hypothesis node of weight; Computing formula is following: belief=1-exp (secondweight-biggestweight); Wherein, secondweight representes the weight of the second largest hypothesis node of weight in all hypothesis nodes, and biggestweight representes the weight of the hypothesis node that weight is maximum.
Illustrate method of the present invention below.
As shown in Figure 5, suppose to have two four roads (being path above-mentioned) to form a crossroad, these four roads are respectively: OA, OB, OC, OD, intersection point are the O point.Fig. 5 a) in engrave drawn some during the t1-t5 of coordinate and represent automobile in the t1-t5 position that automobile belongs to constantly the time.To describe below and how begin, constantly upgrade many hypothesis trees according to gps data from t1.
The node of each many hypothesis tree all can comprise the numbering of road, and the direction on this road, walked of automobile.Suppose: (AO) the expression automobile is walked on this road of AO, and direction is by the direction running of A to O.If automobile is to go to A from O, then use (OA) to represent this node.
Step is following:
Suppose that automobile is walked on this road of AO at t1 constantly, direction is driven towards O from A, and the state of automobile is operation.
During the moment, the state of automobile has become acceleration at t2.According to the vehicle condition conversion table of Fig. 4, when automobile when running status is transformed into acceleration mode, automobile only generates a node, and direction is constant.
During the moment, the state of automobile has become and has stopped (waiting red light such as automobile) at t3.According to the vehicle condition conversion table, when automobile when acceleration mode is transformed into halted state, automobile only generates a node, and direction is constant.
During the moment, the state of automobile has become acceleration at t4.Learn according to the vehicle condition conversion table, when automobile when halted state becomes acceleration mode, will on all directions, generate corresponding hypothesis node.
During the moment, the state of automobile has become operation at t5.Learn according to the vehicle condition conversion table, when automobile when acceleration mode becomes running status, will on all directions, generate corresponding hypothesis node.But because in the present embodiment, the automobile crossroad of having passed by, so, only on original road of automobile and direction, generated the hypothesis node of a running status.
As shown in Figure 6; In experiment; On real electronic chart, carry out the resulting coupling path profile of method of the present invention; Map among the figure is depicted as Beijing international trade bridge, and near the beam the coupling path is a gps data, and method as can be seen from the figure of the present invention can guarantee the accuracy and the continuity of road matching well.
Above embodiment only is used to explain the present invention; And be not limitation of the present invention; The those of ordinary skill in relevant technologies field under the situation that does not break away from the spirit and scope of the present invention, can also be made various variations and modification; Therefore all technical schemes that are equal to also belong to category of the present invention, and scope of patent protection of the present invention should be defined by the claims.