Summary of the invention
(1) technical matters that will solve
The technical problem to be solved in the present invention 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, described many hypothesis trees are used to write down all paths that automobile is passed by, it has a root node, described 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 described gps data and the described hypothesis node of INS data computation, described 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;
The hypothesis node of S6, selection weight maximum is 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 described 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 path link at the current possibility of automobile place;
The direction that automobile moves on link;
Automobile is at Δ
tThe angle that turns in time, it obtains by the INS data;
Automobile is at Δ
tThe mileage of passing by in the time, it obtains by the INS data;
The direction that automobile is current, it obtains by 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 each bar automobile may the place the path, generate two the path be 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 path link at the current possible place of automobile;
S24 at the possible path link of each bar, is father node with the root node, generate two hypothesis nodes of representing this path, the direction of two hypothesis nodes is opposite, represents the both direction in this path, these two all may be the automobile current state, and the out of Memory of hypothesis node is set then;
S25, described 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 described 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 described 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 described hypothesis node j generate new hypothesis node.
Wherein, the step of the described weight of calculating is as follows 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 may the place of the 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 as follows:
P (* | * *) is illustrated in the conditional probability of calculating " * " under " * * " condition.
Wherein, step S5 is specially:
The hypothesis node that all are active sorts from big to small by weight;
Suppose only to allow to occur MaxActiveHypos active hypothesis node, then weighting weighs maximum MaxActiveHypos active hypothesis node, and other nodes are deleted;
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 may be 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, adds new leaf node (promptly supposing node) in tree.Each branch is all being represented one section road sequence that automobile may be passed by in the tree, thereby has solved the problem that short path is omitted by the continuity that guarantees the path; By 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 drawings and Examples, the specific embodiment of the present invention is described in further detail.Following examples are used to illustrate the present invention, but are not used for limiting the scope of the 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, described many hypothesis trees are defined as follows:
Many hypothesis trees are one tree (as shown in Figure 1, among Fig. 1, r1~r7 represent the path, and d1~d7 represents 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 as follows:
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 current state of automobile can obtain by the INS data computation.This automobile current state, automobile may the paths, place, the direction of automobile on this path, formed the state of this hypothesis node jointly.
Link: the path at the current possibility of automobile place.
Direction: the direction that automobile moves on current link.
TurnAngle: automobile is at Δ
tThe angle that turns in time.Obtain by the INS data.
RunDis: automobile is at Δ
tThe mileage of passing by in the time.Obtain by the INS data.
FinalBear: the direction that automobile is current.Obtain by the GPS/INS data.
DisOnLink: the mileage that automobile is passed by on this link.
Weight: the weight of supposing node.Described 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.
Be father node with the root node in step S2, 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 path link at the current possible place of automobile;
S24 at the possible path link of each bar, is father node with the root node, generate two hypothesis nodes of representing this path, the direction of two hypothesis nodes is opposite, represents the both direction in this path, these two all may be the automobile current state, and the out of Memory of hypothesis node is set then;
S25, described 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.Have three parameters directly to influence the reliability of gps data in 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 as follows:
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 certain threshold value.
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.Above not being in, automobile during three kinds of states, thinks that automobile is in acceleration mode.
The mode of judging motoring condition is as follows:
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 described 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 is converted to stopping from accelerating; (e) expression is converted to accelerating from accelerating; (f) expression is converted to stopping from running; (g) expression is converted to accelerating from running; (h) expression is converted to running from stopping; (i) expression is converted to running from accelerating; (j) expression is converted to running from runing.
As shown in Figure 4, it is as follows to generate the mode of described new hypothesis node:
Non-live-vertex does not produce new hypothesis node.
As 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.
As Fig. 4 (c), if the vehicle condition in the selected hypothesis node is stopping, and the automobile current state is accelerating, then is that selected node generates two child nodes, one child node is identical with the direction of father node, and another child node is opposite with the direction of father node.
As 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.
As 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 passing by on Ben Tiaolu 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, at each bar road, generate a new son hypothesis node.
As 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.
As 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.
As Fig. 4 (h), if the vehicle condition in the selected hypothesis node is stopping, and the automobile current state is running, then is that selected hypothesis node generates two child nodes, one child node is identical with the direction of father node, and another child node is opposite with the direction of father node.It is non-live-vertex that father node is set.
As Fig. 4 (i), if the vehicle condition in the selected hypothesis node is accelerating, and the automobile current state is running, then is that selected hypothesis node generates two child nodes, one child node is identical with the direction of father node, and another child node is opposite with the direction of father node.It is non-live-vertex that father node is set.
As 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 passing by on this road 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, at each bar road, generate a new son and suppose node.
S4, according to the weight of described gps data and the described hypothesis node of INS data computation, described 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 may the place of the 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 as follows:
P (* | * *) is illustrated in the conditional probability of calculating " * " under " * * " condition.
Top computing formula only is the most basic computing formula.In addition, we also need to consider more situation:
A. represent automobile on an one-way road during reverse travelling 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 automobile steering and the direction in path when differing greatly of a certain hypothesis by the INS data estimation, 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 by 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 this number equals the path of not covering that obtains according to the INS data COEFFICIENT K 2 that square multiply by with respect to 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 from big to small by weight.
Suppose only to allow to occur MaxActiveHypos active hypothesis node, then weighting weighs maximum MaxActiveHypos active hypothesis node, and other nodes are deleted;
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 described branch is represented a step.
S6, select the hypothesis node of weight maximum 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 suppose node with this best, the hypothesis node second largest with weight comes calculating optimum to suppose the degree of confidence of node, computing formula is as follows: belief=1-exp (secondweight-biggestweight), wherein, secondweight represents the weight of the second largest hypothesis node of weight in all hypothesis nodes, and biggestweight represents the weight of the hypothesis node of weight 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 the position of automobile at t1-t5 automobile place during the moment.How will to describe below from t1, constantly upgrade many hypothesis trees according to gps data.
The node of each many hypothesis tree all can comprise the numbering of road, and the direction walked on this road 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 travel to A from O, then use (OA) to represent this node.
Step is as follows:
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 child 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 child 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, near the coupling path thick line section 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 illustrate 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 make 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.