CN107274721A - Many vehicle cooperative localization methods in a kind of intelligent transportation system - Google Patents
Many vehicle cooperative localization methods in a kind of intelligent transportation system Download PDFInfo
- Publication number
- CN107274721A CN107274721A CN201710423028.XA CN201710423028A CN107274721A CN 107274721 A CN107274721 A CN 107274721A CN 201710423028 A CN201710423028 A CN 201710423028A CN 107274721 A CN107274721 A CN 107274721A
- Authority
- CN
- China
- Prior art keywords
- mrow
- msub
- target vehicle
- moment
- adjacent vehicles
- 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.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G08—SIGNALLING
- G08G—TRAFFIC CONTROL SYSTEMS
- G08G1/00—Traffic control systems for road vehicles
- G08G1/16—Anti-collision systems
- G08G1/166—Anti-collision systems for active traffic, e.g. moving vehicles, pedestrians, bikes
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
Abstract
The present invention provides many vehicle cooperative localization methods in a kind of intelligent transportation system, it is possible to increase the positioning precision of vehicle.Methods described includes：Position detection value and movement observations according to the target vehicle of acquisition at current time, calculate position measurements of the target vehicle in subsequent time；The number of the Adjacent vehicles is obtained, and combines position detection value and movement observations of the target vehicle obtained at current time, and calculates the obtained target vehicle in the position measurements of subsequent time, the equation of motion of system is constructed；Position detection value according to the target vehicle and Adjacent vehicles of acquisition at current time, calculates the relative position information between the target vehicle and Adjacent vehicles, and according to the number of Adjacent vehicles, construct the observational equation of system；The equation of motion and observational equation of system are updated in EKF, the position estimation value of target vehicle is obtained.The present invention relates to wireless location technology field.
Description
Technical field
The present invention relates to wireless location technology field, many vehicle cooperative positioning sides in a kind of intelligent transportation system are particularly related to
Method.
Background technology
In recent years, as vehicle becomes more intelligence and automation, in intelligent transportation system, various safety-related answers
With, such as real-time estimation of traffic, collision-warning system, lane-departure warning system etc., these are provided to improve and driven
Efficiency and security, so as to reduce vehicle collision accident.And these security applications depend on the local network of communication lines
The vehicle position information that network is provided.Automobile navigation technology includes global positioning system (GPS), GPS
(GLONASS), Galileo and Beidou systems (BDS), they can provide positional information for vehicle user.GPS is that vehicle is determined
One of the most frequently used location equipment in position.It is well known, however, that gps signal by separate sources noise and degenerate and
The temporary transient loss of signal under complex environment, and gps satellite visibility underestimates, and this prevents GPS from all situations
It is lower that accurate positional information is provided.We be used for automobile application Low-cost GPS receiver navigation system suffer from low precision and
Frequently signal interruption problem.Under normal circumstances, precision nominal GPS is about 10m, and this is for active safety systems of vehicles
Error is too big.
It is to use other embedded information sources, navigation number that one of most popular method of accuracy is positioned oneself in raising
According to by data fusion, obtaining more accurately location estimation.The technology of conventional raising GPS performances has based on Kalman at present
The method for filtering (Kalman filtering).Method main thought based on Kalman filtering is to reduce GPS puppets by filtering
Away from error, but this method does not combine the positional information of nearby vehicle, the positioning accurate provided in intelligent transportation system
Degree is not high.
The content of the invention
The technical problem to be solved in the present invention is to provide many vehicle cooperative localization methods in a kind of intelligent transportation system, to solve
The problem of positioning precision certainly present in prior art is low.
In order to solve the above technical problems, the embodiment of the present invention provides many vehicle cooperative positioning sides in a kind of intelligent transportation system
Method, including：
The position detection value of target vehicle and Adjacent vehicles at current time in acquisition system, the Adjacent vehicles for institute
State the adjacent vehicle of target vehicle；
Obtain movement observations of the target vehicle at current time；
Position detection value and movement observations according to the target vehicle of acquisition at current time, calculate the target vehicle
In the position measurements of subsequent time；
The number of the Adjacent vehicles is obtained, and combines position detection value and fortune of the target vehicle at current time of acquisition
In-motion viewing measured value, and the obtained target vehicle is calculated in the position measurements of subsequent time, construct the equation of motion of system；
Position detection value according to the target vehicle and Adjacent vehicles of acquisition at current time, calculate the target vehicle with
Relative position information between Adjacent vehicles, and according to the number of Adjacent vehicles, construct the observational equation of system；
The equation of motion and observational equation of system are updated in EKF, the position for obtaining target vehicle is estimated
Evaluation.
Further, the position detection value of target vehicle and Adjacent vehicles at current time includes in the acquisition system：
Obtain k moment target vehicles X0Position detection value X0k=[x0k y0k θ0k]T；
Wherein, the k moment represents current time, and T represents transposition, x0kRepresent k moment target vehicles X0In x-axis coordinate, y0kTable
Show k moment target vehicles X0In y-axis coordinate, θ0kRepresent k moment target vehicles X0The angle that the direction of motion is formed with x-axis；
Obtain k moment Adjacent vehicles XjPosition detection value Xjk=[xjk yjk θjk]T, (j=1,2 ... N)；
Wherein, N represents the number of Adjacent vehicles, xjkRepresent k moment Adjacent vehicles XjIn x-axis coordinate, yjkRepresent k moment phases
Adjacent vehicle XjIn y-axis coordinate, θjkRepresent k moment Adjacent vehicles XjThe angle that the direction of motion is formed with x-axis.
Further, the movement observations for obtaining the target vehicle at current time include：
Obtain k moment target vehicles X0Movement observations u0k=[V0k a0k φ0k]T；
Wherein, V0kRepresent k moment target vehicles X0Speed, a0kRepresent k moment target vehicles X0Acceleration, φ0kTable
Show k moment target vehicles X0Steering angle.
Further, position detection value and movement observations of the target vehicle according to acquisition at current time, meter
The position measurements that the target vehicle is calculated in subsequent time include：
According to target vehicle X0In the position detection value X at k moment0kWith movement observations u0kAnd target vehicle X0Motion
Model, calculates target vehicle X0In the position measurements X at k+1 moment0(k+1)=f (X0k,u0k), wherein, f (X0k,u0k) represent mesh
Mark vehicle X0Motion model discrete motion equation；
According to f (X0k,u0k), obtain f (X0k,u0k) on position detection value X0kJacobian matrixFor：
According to f (X0k,u0k), obtain f (X0k,u0k) on movement observations u0kJacobian matrix Bu0kFor：
Further, the number for obtaining the Adjacent vehicles, and the target vehicle obtained is combined at current time
Position detection value and movement observations, and the obtained target vehicle is calculated in the position measurements of subsequent time, construction
The equation of motion of system includes：
Obtain the number N of the Adjacent vehicles；
According to the number N of the Adjacent vehicles of acquisition, with reference to acquisition target vehicle the k moment position detection value X0kAnd fortune
In-motion viewing measured value u0k, the system mode of whole system of the construction k moment comprising the target vehicle and Adjacent vehiclesSystem is inputtedThen the equation of motion of system is Xk+1
=f (Xk,uk)；Wherein, T representing matrixs transposition；
According to Jacobian matrixObtain the equation of motion f (X of systemk,uk) on system mode XkJacobian matrix
AkFor：
According to Jacobian matrixObtain the equation of motion f (X of systemk,uk) input u on systemkJacobian matrix
BkFor：
Further, position detection value of the target vehicle and Adjacent vehicles according to acquisition at current time, is calculated
Relative position information between the target vehicle and Adjacent vehicles, and according to the number of Adjacent vehicles, construct the observation of system
Equation includes：
According to the k moment target vehicles X of acquisition0Position detection value X0kWith Adjacent vehicles XjPosition detection value Xjk, meter
Calculate k moment target vehicles X0With Adjacent vehicles XjBetween relative position information Zjk；
The k moment target vehicles X obtained according to calculating0With Adjacent vehicles XjBetween relative position information Zjk, construction system
The observational equation Z of systemk=[Z1k Z2k … Zjk … Z(N-1)k ZNk]T, (j=1,2 ... N), wherein, N represents Adjacent vehicles
Number；Wherein, the ZjkIt is expressed as：
Wherein, hj() represents the calculation formula of relative position information, djkFor target vehicle X0With Adjacent vehicles XjBetween
Relative distance,For target vehicle X0With Adjacent vehicles XjBetween relative bearing；
According to Zjk=hj(X0k,Xjk), target vehicle X0Relative to Zjk=hj(X0k,Xjk) Jacobian matrix HjkFor：
According to Jacobian matrix Hjk, obtain observational equation ZkJacobian matrix HkFor：
Further, it is described that the equation of motion and observational equation of system are updated in EKF, obtain mesh
The position estimation value of mark vehicle includes：
By system mode XkJacobian matrix Ak, system input ukJacobian matrix Bk, observational equation ZkJacobi
Matrix HkAnd the equation of motion X of systemk+1With observational equation ZkIt is updated in EKF, obtains target vehicle X0's
Position estimation value
Further, it is described that the equation of motion and observational equation of system are updated in EKF, obtain mesh
The position estimation value of mark vehicle includes：
S1, according to system mode XkJacobian matrix AkU is inputted with systemkJacobian matrix Bk, calculate the k+1 moment
The predicted value of system mode covarianceWherein, PkFor k moment system modes association side
Difference, Q is position detection value error covariance, and T is matrix transposition, TsFor the sampling period；
S2, according to observational equation ZkJacobian matrix HkWith the prediction for calculating obtained k+1 moment system mode covariances
ValueCalculate k+1 moment Kalman filtering gainsWherein, R is observation side
Journey ZkError covariance；
S3, the k+1 moment Kalman filtering gains K obtained according to calculatingk+1, calculate k+1 moment system mode covariancesWherein, I is unit matrix；
S4, according to system mode XkJacobian matrix AkU is inputted with systemkJacobian matrix Bk, calculate the k+1 moment
System mode predicted valueWith systematic observation prediction equation valueWherein,
It is describedWithIt is expressed as：
Wherein,N is represented
The number of Adjacent vehicles；
Wherein,hj() represents the calculation formula of relative position information；
The systematic observation prediction equation value that S5, basis are obtainedJudge target vehicle X0With Adjacent vehicles XjBetween phase
Azimuthal predicted valueWhether default judgment formula is met, if meeting, it is determined that k+1 moment system state estimation valuesIf it is not satisfied, then determining k+1 moment system state estimation valuesFor：
Wherein,Zk+1For k
The observation of+1 moment systematic observation equation；
S6, according to k+1 moment system state estimation valuesCalculate k+1 moment target vehicle position estimation values
For：
Further, the default judgment formula is expressed as：
Wherein, θthreshFor default threshold value.
Further, it is described
Wherein,For target vehicle X0With Adjacent vehicles XjBetween relative bearing variance.
The above-mentioned technical proposal of the present invention has the beneficial effect that：
In such scheme, position detection value and movement observations by the target vehicle of acquisition at current time are calculated
Position measurements of the target vehicle in subsequent time；The number of the Adjacent vehicles is obtained, and combines the target carriage obtained
Position detection value and movement observations at current time, and the obtained target vehicle is calculated in the position of subsequent time
Measured value is put, the equation of motion of system is constructed；According to the target vehicle and Adjacent vehicles of acquisition current time position detection
Value, calculates the relative position information between the target vehicle and Adjacent vehicles, and according to the number of Adjacent vehicles, constructs system
Observational equation；The equation of motion and observational equation of system are updated in EKF, the position of target vehicle is obtained
Put estimate；So, by the way that the position detection value of target vehicle is combined with the position detection value of Adjacent vehicles, target is calculated
Relative position information between vehicle and Adjacent vehicles, based on the relative position calculated between obtained target vehicle and Adjacent vehicles
Confidence ceases, and after being filtered using EKF, estimates the position of target vehicle, realizes co-positioned, can subtract
Few site error, so as to improve the positioning precision of vehicle.
Brief description of the drawings
Fig. 1 is the schematic flow sheet of many vehicle cooperative localization methods in intelligent transportation system provided in an embodiment of the present invention；
Fig. 2 is many Vehicular system model schematics of intelligent transportation system provided in an embodiment of the present invention；
Fig. 3 is the site error contrast schematic diagram that the site error that the embodiment of the present invention is measured is measured with GPS.
Embodiment
To make the technical problem to be solved in the present invention, technical scheme and advantage clearer, below in conjunction with accompanying drawing and tool
Body embodiment is described in detail.
The present invention is directed to the problem of existing positioning precision is low, and there is provided many vehicle cooperative positioning in a kind of intelligent transportation system
Method.
As shown in figure 1, many vehicle cooperative localization methods in intelligent transportation system provided in an embodiment of the present invention, including：
Target vehicle and Adjacent vehicles are in the position detection value at current time, the Adjacent vehicles in S101, acquisition system
For the vehicle adjacent with the target vehicle；
S102, obtains movement observations of the target vehicle at current time；
S103, position detection value and movement observations according to the target vehicle of acquisition at current time calculate the mesh
Mark position measurements of the vehicle in subsequent time；
S104, obtains the number of the Adjacent vehicles, and combine position detection of the target vehicle at current time of acquisition
Value and movement observations, and the obtained target vehicle is calculated in the position measurements of subsequent time, construct the fortune of system
Dynamic equation；
S105, the position detection value according to the target vehicle and Adjacent vehicles of acquisition at current time calculates the target
Relative position information between vehicle and Adjacent vehicles, and according to the number of Adjacent vehicles, construct the observational equation of system；
S106, the equation of motion and observational equation of system are updated in EKF, target vehicle is obtained
Position estimation value.
Many vehicle cooperative localization methods, pass through the target vehicle of acquisition in intelligent transportation system described in the embodiment of the present invention
In the position detection value and movement observations at current time, position measurements of the target vehicle in subsequent time are calculated；Obtain
The number of the Adjacent vehicles, and position detection value and movement observations with reference to the target vehicle obtained at current time are taken,
And the obtained target vehicle is calculated in the position measurements of subsequent time, construct the equation of motion of system；According to acquisition
Position detection value at current time of target vehicle and Adjacent vehicles, calculate the phase between the target vehicle and Adjacent vehicles
To positional information, and according to the number of Adjacent vehicles, construct the observational equation of system；By the equation of motion and observational equation of system
It is updated in EKF, obtains the position estimation value of target vehicle；So, by by the position detection of target vehicle
Value is combined with the position detection value of Adjacent vehicles, is calculated the relative position information between target vehicle and Adjacent vehicles, is based on
The relative position information between obtained target vehicle and Adjacent vehicles is calculated, after being filtered using EKF,
The position of target vehicle is estimated, co-positioned is realized, site error can be reduced, so as to improve the positioning precision of vehicle.
Many vehicle cooperative localization methods in intelligent transportation system described in the embodiment of the present invention, under low signal-to-noise ratio environment,
Measurement result is more accurate than GPS measurement result, and EKF arithmetic speed is fast, is favorably improved the positioning of vehicle
Precision.And in actual applications, Adjacent vehicles number can be adaptive selected according to actual conditions.In intelligent transportation system
In, the Adjacent vehicles around target vehicle are continually changing, many vehicles in the intelligent transportation system described in the embodiment of the present invention
Cooperative Localization Method can autonomous Adjacent vehicles of selection target vehicle periphery number.
It is further, described in aforementioned intelligent traffic system in the embodiment of many vehicle cooperative localization methods
The position detection value of target vehicle and Adjacent vehicles at current time includes in acquisition system：
Obtain k moment target vehicles X0Position detection value X0k=[x0k y0k θ0k]T；
Wherein, the k moment represents current time, and T represents transposition, x0kRepresent k moment target vehicles X0In x-axis coordinate, y0kTable
Show k moment target vehicles X0In y-axis coordinate, θ0kRepresent k moment target vehicles X0The angle that the direction of motion is formed with x-axis；
Obtain k moment Adjacent vehicles XjPosition detection value Xjk=[xjk yjk θjk]T, (j=1,2 ... N)；
Wherein, N represents the number of Adjacent vehicles, xjkRepresent k moment Adjacent vehicles XjIn x-axis coordinate, yjkRepresent k moment phases
Adjacent vehicle XjIn y-axis coordinate, θjkRepresent k moment Adjacent vehicles XjThe angle that the direction of motion is formed with x-axis.
In the present embodiment, target vehicle X0When k can be obtained by vehicle mounted guidance (for example, GPS device or Big Dipper equipment)
Carve target vehicle X0Position detection value X0k=[x0k y0k θ0k]T；Meanwhile, target vehicle X0Short distance distance communication can be passed through
(Dedicated Short-Range Communication, DSRC) obtains k moment Adjacent vehicles XjPosition detection value Xjk=
[xjk yjk θjk]T, (j=1,2 ... N) pay wages small.
In the present embodiment, θkRepresent k moment target vehicles X0The angle that the direction of motion is formed with x-axis, i.e. k moment targets
Vehicle X0Azimuth；θjkRepresent k moment Adjacent vehicles XjThe angle that the direction of motion is formed with x-axis, i.e. k moment Adjacent vehicles
XjAzimuth.
It is further, described in aforementioned intelligent traffic system in the embodiment of many vehicle cooperative localization methods
The movement observations that the target vehicle is obtained at current time include：
Obtain k moment target vehicles X0Movement observations u0k=[V0k a0k φ0k]T；
Wherein, V0kRepresent k moment target vehicles X0Speed, a0kRepresent k moment target vehicles X0Acceleration, φ0kTable
Show k moment target vehicles X0Steering angle.
In the present embodiment, k moment target vehicles X can be obtained by onboard sensor0Movement observations u0k=[V0k
a0k φ0k]T, wherein, the onboard sensor can include but is not limited to：Acceleration transducer, velocity sensor.
It is further, described in aforementioned intelligent traffic system in the embodiment of many vehicle cooperative localization methods
Position detection value and movement observations according to the target vehicle of acquisition at current time, calculate the target vehicle in lower a period of time
The position measurements at quarter include：
According to target vehicle X0In the position detection value X at k moment0kWith movement observations u0kAnd target vehicle X0Motion
Model, calculates target vehicle X0In the position measurements X at k+1 moment0(k+1)=f (X0k,u0k), wherein, f (X0k,u0k) represent mesh
Mark vehicle X0Motion model discrete motion equation；
According to f (X0k,u0k), obtain f (X0k,u0k) on position detection value X0kJacobian matrixFor：
According to f (X0k,u0k), obtain f (X0k,u0k) on movement observations u0kJacobian matrixFor：
It is further, described in aforementioned intelligent traffic system in the embodiment of many vehicle cooperative localization methods
The number of the Adjacent vehicles is obtained, and combines position detection value and movement observations of the target vehicle at current time of acquisition
Value, and the obtained target vehicle is calculated in the position measurements of subsequent time, constructing the equation of motion of system includes：
Obtain the number N of the Adjacent vehicles；
According to the number N of the Adjacent vehicles of acquisition, with reference to acquisition target vehicle the k moment position detection value X0kAnd fortune
In-motion viewing measured value u0k, the system mode of whole system of the construction k moment comprising the target vehicle and Adjacent vehiclesSystem is inputtedThen the equation of motion of system is Xk+1
=f (Xk,uk)；Wherein, T representing matrixs transposition；
According to Jacobian matrixObtain the equation of motion f (X of systemk,uk) on system mode XkJacobian matrix
AkFor：
According to Jacobian matrixObtain the equation of motion f (X of systemk,uk) input u on systemkJacobian matrix
BkFor：
It is further, described in aforementioned intelligent traffic system in the embodiment of many vehicle cooperative localization methods
Position detection value according to the target vehicle and Adjacent vehicles of acquisition at current time, calculates the target vehicle and Adjacent vehicles
Between relative position information, and according to the number of Adjacent vehicles, the observational equation of construction system includes：
According to the k moment target vehicles X of acquisition0Position detection value X0kWith Adjacent vehicles XjPosition detection value Xjk, meter
Calculate k moment target vehicles X0With Adjacent vehicles XjBetween relative position information Zjk；
The k moment target vehicles X obtained according to calculating0With Adjacent vehicles XjBetween relative position information Zjk, construction system
The observational equation Z of systemk=[Z1k Z2k … Zjk … Z(N-1)k ZNk]T, (j=1,2 ... N), wherein, N represents Adjacent vehicles
Number；Wherein, the ZjkIt is expressed as：
Wherein, hj() represents the calculation formula of relative position information, djkFor target vehicle X0With Adjacent vehicles XjBetween
Relative distance,For target vehicle X0With Adjacent vehicles XjBetween relative bearing；
According to Zjk=hj(X0k,Xjk), target vehicle X0Relative to Zjk=hj(X0k,Xjk) Jacobian matrix HjkFor：
According to Jacobian matrix Hjk, obtain observational equation ZkJacobian matrix HkFor：
It is further, described in aforementioned intelligent traffic system in the embodiment of many vehicle cooperative localization methods
The equation of motion and observational equation of system are updated in EKF, the position estimation value bag of target vehicle is obtained
Include：
By system mode XkJacobian matrix Ak, system input ukJacobian matrix Bk, observational equation ZkJacobi
Matrix HkAnd the equation of motion X of systemk+1With observational equation ZkIt is updated in EKF, obtains target vehicle X0's
Position estimation value
It is further, described in aforementioned intelligent traffic system in the embodiment of many vehicle cooperative localization methods
The equation of motion and observational equation of system are updated in EKF, the position estimation value bag of target vehicle is obtained
Include：
S1, according to system mode XkJacobian matrix AkU is inputted with systemkJacobian matrix Bk, calculate the k+1 moment
The predicted value of system mode covarianceWherein, PkFor k moment system modes association side
Difference, Q is position detection value error covariance, and T is matrix transposition, TsFor the sampling period；
S2, according to observational equation ZkJacobian matrix HkWith the prediction for calculating obtained k+1 moment system mode covariances
ValueCalculate k+1 moment Kalman filtering gainsWherein, R is observation side
Journey ZkError covariance；
S3, the k+1 moment Kalman filtering gains K obtained according to calculatingk+1, calculate k+1 moment system mode covariancesWherein, I is unit matrix；
S4, according to system mode XkJacobian matrix AkU is inputted with systemkJacobian matrix Bk, calculate the k+1 moment
System mode predicted valueWith systematic observation prediction equation valueWherein,
It is describedWithIt is expressed as：
Wherein,N represents phase
The number of adjacent vehicle；
Wherein,hj() represents the calculating of relative position information
Formula；
The systematic observation prediction equation value that S5, basis are obtainedJudge target vehicle X0With Adjacent vehicles XjBetween phase
Azimuthal predicted valueWhether default judgment formula is met, if meeting, it is determined that k+1 moment system state estimation valuesIf it is not satisfied, then determining k+1 moment system state estimation valuesFor：
Wherein,Zk+1For k
The observation of+1 moment systematic observation equation；
S6, according to k+1 moment system state estimation valuesCalculate k+1 moment target vehicle position estimation values
For：
It is further, described in aforementioned intelligent traffic system in the embodiment of many vehicle cooperative localization methods
Default judgment formula is expressed as：
Wherein, θthreshFor default threshold value.
In S5, default judgment formula can be passed throughJudge target vehicle
X0With Adjacent vehicles XjBetween relative bearing predicted valueWhether approachIf closeThen retain system mode predicted valueThat is k+1 moment system state estimation valuesIf keeping off, k+1 moment system state estimation valuesFor：
It is further, described in aforementioned intelligent traffic system in the embodiment of many vehicle cooperative localization methods
Wherein,For target vehicle X0With Adjacent vehicles XjBetween relative bearing variance.
As shown in Fig. 2 with specific example to many vehicle cooperative positioning sides in the intelligent transportation system described in the present embodiment
Method is described in detail, and uses matlab emulation platforms, to many vehicle cooperatives in the intelligent transportation system described in the present embodiment
The performance of localization method carries out simulation analysis：
Step 1, as shown in Figure 2, it is contemplated that a target vehicle X in intelligent transportation system0With four Adjacent vehicles X1,X2,
X3,X4, travel on road, vehicle X0,X1,X2,X3,X4K moment respective position is respectively received by itself GPS device to see
Measured value；
Step 2, target vehicle X0The movement observations u at k moment itself is obtained by onboard sensor0k=[V0k a0k
φ0k]TIf what target vehicle was done is linear motion, and uniformly accelrated rectilinear motion, i.e. a are can be regarded as within the sampling period0kIt is fixed
Value；
Step 3, according to target vehicle X0In the position detection value and movement observations at k moment, draw target vehicle in k+1
The position detection value and movement observations at moment, i.e. target vehicle X0Motion model；Specifically, step 3 can include：
3.1) target vehicle X0The discrete motion equation of motion model be：
Wherein, TsFor the sampling period.
3.2) according to discrete motion Equation f (X0k,u0k), f (X can be obtained0k,u0k) on X0kJacobian matrixFor：
3.3) according to discrete motion Equation f (X0k,u0k), f (X can be obtained0k,u0k) on u0kJacobian matrixFor：
Step 4, X is included by the step 3 construction k moment0kWith Adjacent vehicles X1k,X2k,X3k,X4kWhole system system
State is Xk=[X0k X0k X0k X0k]T, system input is uk=[u0k u0k u0k u0k]T, then the equation of motion of system is Xk+1
=f (Xk,uk)；Specifically, step 4 can include：
4.1) according to the Jacobian matrix of step 3Equation of motion f (the X of system can be obtainedk,uk) on system mode Xk's
Jacobian matrix AkFor：
4.2) according to the Jacobian matrix of step 3Equation of motion f (the X of system can be obtainedk,uk) input u on systemk's
Jacobian matrix BkFor：
Step 5：Calculating obtains the relative position information between target vehicle and Adjacent vehicles, construction observational equation Zk=
[Z1k Z2k Z3k Z4k]T, specifically, step 5 can include：
5.1) k moment target vehicles X0With Adjacent vehicles XjBetween relative position information ZjkFor：
Wherein, djkFor target vehicle X0With Adjacent vehicles XjBetween relative distance,For target vehicle X0With adjacent car
XjBetween relative bearing；
5.2) target vehicle X0Relative to Zjk=hj(X0k,Xjk) Jacobian matrix HjkFor：
5.3) according to Jacobian matrix HjkObservational equation Z can be obtainedkJacobian matrix HkFor：
Step 6, system mode X step 4 obtainedkJacobian matrix Ak, system input ukJacobian matrix BkWith
The observational equation Z that step 5 is obtainedkJacobian matrix HkAnd the equation of motion and observational equation of system are updated to extension karr
In graceful filtering, the position estimation value of target vehicle is obtainedSpecifically, step 6 can include：
6.1) predicted value of computing system state covariance
Wherein, PkFor k moment system mode covariances, Q is position detection value error covariance, TsFor the sampling period；
6.2) according to the predicted value of system mode covarianceCalculate k+1 moment Kalman filtering gains Kk+1：
Wherein, R is observational equation ZkError covariance, HkFor observational equation ZkJacobian matrix.
6.3) according to k+1 moment Kalman filtering gains Kk+1Obtain k+1 moment system mode covariances Pk+1：
Wherein, I is unit matrix, HkFor observational equation ZkJacobian matrix.
6.4) according to system mode XkJacobian matrix AkU is inputted with systemkJacobian matrix BkComputing system state
Predicted value
Wherein,
With systematic observation prediction equation value
Wherein,
6.5) k+1 moment system state estimation values are calculatedAccording to the predicted value of the systematic observation equation 6.4) obtainedJudge target vehicle X0With Adjacent vehicles XjBetween relative bearing predicted valueWhether approachIf closeThen
Retain system mode predicted valueI.e.If keeping off,
Wherein,Kk+1Increase for k+1 moment Kalman filtering
Benefit, Zk+1For the observation of k+1 moment systematic observation equations,For k+1 moment system mode predicted values.
6.6) obtained according to 6.5) calculatingK+1 moment target vehicle position estimation values can be obtainedFor：
In the present embodiment, as shown in figure 3, Fig. 3 describes the position that the site error of measurement of the embodiment of the present invention is measured with GPS
It is the sampling time to put the abscissa in error contrast schematic diagram, Fig. 3, and ordinate is site error size, and the result is in noise
Than repeating what experiment was drawn to carry out the same condition of 1000 times under 3dB.From the figure 3, it may be seen that the method drop that the embodiment of the present invention is proposed
Low site error, and give more accurately location estimation.This illustrates that the Cooperative Localization Method that embodiment of the present invention is proposed is
It is accurate and effective, while comparing result also indicates that the method performance that the present invention is used is better than the performance that GPS is measured.
It should be noted that herein, such as first and second or the like relational terms are used merely to a reality
Body or operation make a distinction with another entity or operation, and not necessarily require or imply these entities or deposited between operating
In any this actual relation or order.
Described above is the preferred embodiment of the present invention, it is noted that for those skilled in the art
For, on the premise of principle of the present invention is not departed from, some improvements and modifications can also be made, these improvements and modifications
It should be regarded as protection scope of the present invention.
Claims (10)
1. many vehicle cooperative localization methods in a kind of intelligent transportation system, it is characterised in that including：
The position detection value of target vehicle and Adjacent vehicles at current time in acquisition system, the Adjacent vehicles be and the mesh
Mark the adjacent vehicle of vehicle；
Obtain movement observations of the target vehicle at current time；
Position detection value and movement observations according to the target vehicle of acquisition at current time, calculate the target vehicle under
The position measurements at one moment；
The number of the Adjacent vehicles is obtained, and combines position detection value and motion view of the target vehicle at current time of acquisition
Measured value, and the obtained target vehicle is calculated in the position measurements of subsequent time, construct the equation of motion of system；
Position detection value according to the target vehicle and Adjacent vehicles of acquisition at current time, calculate the target vehicle with it is adjacent
Relative position information between vehicle, and according to the number of Adjacent vehicles, construct the observational equation of system；
The equation of motion and observational equation of system are updated in EKF, the location estimation of target vehicle is obtained
Value.
2. many vehicle cooperative localization methods in intelligent transportation system according to claim 1, it is characterised in that the acquisition
The position detection value of target vehicle and Adjacent vehicles at current time includes in system：
Obtain k moment target vehicles X0Position detection value X0k=[x0k y0k θ0k]T；
Wherein, the k moment represents current time, and T represents transposition, x0kRepresent k moment target vehicles X0In x-axis coordinate, y0kWhen representing k
Carve target vehicle X0In y-axis coordinate, θ0kRepresent k moment target vehicles X0The angle that the direction of motion is formed with x-axis；
Obtain k moment Adjacent vehicles XjPosition detection value Xjk=[xjk yjk θjk]T, (j=1,2 ... N)；
Wherein, N represents the number of Adjacent vehicles, xjkRepresent k moment Adjacent vehicles XjIn x-axis coordinate, yjkRepresent k moment adjacent car
XjIn y-axis coordinate, θjkRepresent k moment Adjacent vehicles XjThe angle that the direction of motion is formed with x-axis.
3. many vehicle cooperative localization methods in intelligent transportation system according to claim 1, it is characterised in that the acquisition
Movement observations of the target vehicle at current time include：
Obtain k moment target vehicles X0Movement observations u0k=[V0k a0k φ0k]T；
Wherein, V0kRepresent k moment target vehicles X0Speed, a0kRepresent k moment target vehicles X0Acceleration, φ0kWhen representing k
Carve target vehicle X0Steering angle.
4. many vehicle cooperative localization methods in intelligent transportation system according to claim 1, it is characterised in that the basis
Position detection value and movement observations of the target vehicle of acquisition at current time, calculate the target vehicle in subsequent time
Position measurements include：
According to target vehicle X0In the position detection value X at k moment0kWith movement observations u0kAnd target vehicle X0Motion mould
Type, calculates target vehicle X0In the position measurements X at k+1 moment0(k+1)=f (X0k,u0k), wherein, f (X0k,u0k) represent target
Vehicle X0Motion model discrete motion equation；
According to f (X0k,u0k), obtain f (X0k,u0k) on position detection value X0kJacobian matrixFor：
<mrow>
<msub>
<mi>A</mi>
<msub>
<mi>X</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mo>&part;</mo>
<mi>f</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>X</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>u</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mo>&part;</mo>
<msub>
<mi>X</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
</mrow>
</mfrac>
<mo>;</mo>
</mrow>
According to f (X0k,u0k), obtain f (X0k,u0k) on movement observations u0kJacobian matrixFor：
<mrow>
<msub>
<mi>B</mi>
<msub>
<mi>u</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mo>&part;</mo>
<mi>f</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>X</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>u</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mo>&part;</mo>
<msub>
<mi>u</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
</mrow>
</mfrac>
<mo>.</mo>
</mrow>
5. many vehicle cooperative localization methods in intelligent transportation system according to claim 4, it is characterised in that the acquisition
The number of the Adjacent vehicles, and position detection value and movement observations of the target vehicle obtained at current time are combined, with
And the obtained target vehicle is calculated in the position measurements of subsequent time, constructing the equation of motion of system includes：
Obtain the number N of the Adjacent vehicles；
According to the number N of the Adjacent vehicles of acquisition, with reference to acquisition target vehicle the k moment position detection value X0k
With movement observations u0k, the system mode of whole system of the construction k moment comprising the target vehicle and Adjacent vehiclesSystem is inputtedThen the equation of motion of system is Xk+1=
f(Xk,uk)；Wherein, T representing matrixs transposition；
According to Jacobian matrixObtain the equation of motion f (X of systemk,uk) on system mode XkJacobian matrix Ak
For：
According to Jacobian matrixObtain the equation of motion f (X of systemk,uk) input u on systemkJacobian matrix Bk
For：
6. many vehicle cooperative localization methods in intelligent transportation system according to claim 2, it is characterised in that the basis
Position detection value of the target vehicle and Adjacent vehicles of acquisition at current time, is calculated between the target vehicle and Adjacent vehicles
Relative position information, and according to the number of Adjacent vehicles, the observational equation of construction system includes：
According to the k moment target vehicles X of acquisition0Position detection value X0kWith Adjacent vehicles XjPosition detection value Xjk, when calculating k
Carve target vehicle X0With Adjacent vehicles XjBetween relative position information Zjk；
The k moment target vehicles X obtained according to calculating0With Adjacent vehicles XjBetween relative position information Zjk, construct the sight of system
Survey equation Zk=[Z1k Z2k … Zjk … Z(N-1)k ZNk]T, (j=1,2 ... N), wherein, N represents the number of Adjacent vehicles；
Wherein, the ZjkIt is expressed as：
Wherein, hj() represents the calculation formula of relative position information, djkFor target vehicle X0With Adjacent vehicles XjBetween it is relative
Distance,For target vehicle X0With Adjacent vehicles XjBetween relative bearing；
According to Zjk=hj(X0k,Xjk), target vehicle X0Relative to Zjk=hj(X0k,Xjk) Jacobian matrix HjkFor：
<mrow>
<msub>
<mi>H</mi>
<mrow>
<mi>j</mi>
<mi>k</mi>
</mrow>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mfrac>
<mrow>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>x</mi>
<mrow>
<mi>j</mi>
<mi>k</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>x</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<msqrt>
<mrow>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>y</mi>
<mrow>
<mi>j</mi>
<mi>k</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>y</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>x</mi>
<mrow>
<mi>j</mi>
<mi>k</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>x</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
</msqrt>
</mfrac>
</mtd>
<mtd>
<mfrac>
<mrow>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>y</mi>
<mrow>
<mi>j</mi>
<mi>k</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>y</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<msqrt>
<mrow>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>y</mi>
<mrow>
<mi>j</mi>
<mi>k</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>y</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>x</mi>
<mrow>
<mi>j</mi>
<mi>k</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>x</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
</msqrt>
</mfrac>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mfrac>
<mrow>
<mo>(</mo>
<msub>
<mi>y</mi>
<mrow>
<mi>j</mi>
<mi>k</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>y</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mrow>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>y</mi>
<mrow>
<mi>j</mi>
<mi>k</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>y</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>x</mi>
<mrow>
<mi>j</mi>
<mi>k</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>x</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
</mfrac>
</mtd>
<mtd>
<mfrac>
<mrow>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>x</mi>
<mrow>
<mi>j</mi>
<mi>k</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>x</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>y</mi>
<mrow>
<mi>j</mi>
<mi>k</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>y</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>x</mi>
<mrow>
<mi>j</mi>
<mi>k</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>x</mi>
<mrow>
<mn>0</mn>
<mi>k</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
</mfrac>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>=</mo>
<mn>1</mn>
<mo>,</mo>
<mn>2</mn>
<mo>,</mo>
<mn>...</mn>
<mi>N</mi>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
According to Jacobian matrix Hjk, obtain observational equation ZkJacobian matrix HkFor：
7. many vehicle cooperative localization methods in intelligent transportation system according to claim 1, it is characterised in that described to be
The equation of motion and observational equation of system are updated in EKF, are obtained the position estimation value of target vehicle and are included：
By system mode XkJacobian matrix Ak, system input ukJacobian matrix Bk, observational equation ZkJacobian matrix
HkAnd the equation of motion X of systemk+1With observational equation ZkIt is updated in EKF, obtains target vehicle X0Position
Estimate
8. many vehicle cooperative localization methods in the intelligent transportation system according to claim 1 or 7, it is characterised in that described
The equation of motion and observational equation of system are updated in EKF, the position estimation value bag of target vehicle is obtained
Include：
S1, according to system mode XkJacobian matrix AkU is inputted with systemkJacobian matrix Bk, etching system shape when calculating k+1
The predicted value of state covarianceWherein, PkFor k moment system mode covariances, Q is
Position detection value error covariance, T is matrix transposition, TsFor the sampling period；
S2, according to observational equation ZkJacobian matrix HkWith the predicted value for calculating obtained k+1 moment system mode covariancesCalculate k+1 moment Kalman filtering gainsWherein, R is observational equation
ZkError covariance；
S3, the k+1 moment Kalman filtering gains K obtained according to calculatingk+1, calculate k+1 moment system mode covariancesWherein, I is unit matrix；
S4, according to system mode XkJacobian matrix AkU is inputted with systemkJacobian matrix Bk, etching system shape when calculating k+1
State predicted valueWith systematic observation prediction equation valueWherein,
It is describedWithIt is expressed as：
<mrow>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mi>A</mi>
<mi>k</mi>
</msub>
<mo>&CenterDot;</mo>
<msub>
<mi>X</mi>
<mi>k</mi>
</msub>
<mo>+</mo>
<msub>
<mi>T</mi>
<mi>s</mi>
</msub>
<mo>&CenterDot;</mo>
<msub>
<mi>B</mi>
<mi>k</mi>
</msub>
<mo>&CenterDot;</mo>
<msub>
<mi>u</mi>
<mi>k</mi>
</msub>
<mo>;</mo>
</mrow>
Wherein,N represents adjacent car
Number；
<mrow>
<msub>
<mover>
<mi>Z</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mover>
<mi>Z</mi>
<mo>^</mo>
</mover>
<mrow>
<mn>1</mn>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mover>
<mi>Z</mi>
<mo>^</mo>
</mover>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</msub>
</mtd>
<mtd>
<mn>...</mn>
</mtd>
<mtd>
<msub>
<mover>
<mi>Z</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>j</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</msub>
</mtd>
<mtd>
<mn>...</mn>
</mtd>
<mtd>
<msub>
<mover>
<mi>Z</mi>
<mo>^</mo>
</mover>
<mrow>
<mo>(</mo>
<mi>N</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mover>
<mi>Z</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
<mo>,</mo>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>=</mo>
<mn>1</mn>
<mo>,</mo>
<mn>2</mn>
<mo>,</mo>
<mo>...</mo>
<mi>N</mi>
<mo>)</mo>
</mrow>
</mrow>
Wherein,hj() represents the calculation formula of relative position information；
The systematic observation prediction equation value that S5, basis are obtainedJudge target vehicle X0With Adjacent vehicles XjBetween contra
Parallactic angle predicted valueWhether default judgment formula is met, if meeting, it is determined that k+1 moment system state estimation valuesIf it is not satisfied, then determining k+1 moment system state estimation valuesFor：
<mrow>
<msub>
<mover>
<mi>X</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>K</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>&CenterDot;</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>Z</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mover>
<mi>Z</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
Wherein,Zk+1During for k+1
The observation of etching system observational equation；
S6, according to k+1 moment system state estimation valuesCalculate k+1 moment target vehicle position estimation valuesFor：
<mrow>
<msub>
<mover>
<mover>
<mi>X</mi>
<mo>~</mo>
</mover>
<mo>~</mo>
</mover>
<mrow>
<mn>0</mn>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>j</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>N</mi>
</munderover>
<msub>
<mover>
<mi>X</mi>
<mo>~</mo>
</mover>
<mrow>
<mn>0</mn>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mi>j</mi>
</mrow>
</msub>
</mrow>
<mi>N</mi>
</mfrac>
<mo>.</mo>
</mrow>
9. many vehicle cooperative localization methods in intelligent transportation system according to claim 8, it is characterised in that described default
Judgment formula be expressed as：
Wherein, θthreshFor default threshold value.
10. many vehicle cooperative localization methods in intelligent transportation system according to claim 9, it is characterised in that described
Wherein,For target vehicle X0With Adjacent vehicles XjBetween relative bearing variance.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710423028.XA CN107274721B (en) | 2017-06-07 | 2017-06-07 | Multi-vehicle cooperative positioning method in intelligent transportation system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710423028.XA CN107274721B (en) | 2017-06-07 | 2017-06-07 | Multi-vehicle cooperative positioning method in intelligent transportation system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107274721A true CN107274721A (en) | 2017-10-20 |
| CN107274721B CN107274721B (en) | 2020-03-31 |
Family
ID=60066432
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710423028.XA Expired - Fee Related CN107274721B (en) | 2017-06-07 | 2017-06-07 | Multi-vehicle cooperative positioning method in intelligent transportation system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107274721B (en) |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108398704A (en) * | 2018-02-06 | 2018-08-14 | 北京科技大学 | A kind of more vehicle cooperative localization methods of Bayesian filter |
| CN108519738A (en) * | 2018-04-13 | 2018-09-11 | 中国科学院微电子研究所 | A kind of state of motion of vehicle information delay compensation method and device |
| CN111912413A (en) * | 2020-07-23 | 2020-11-10 | 腾讯科技（深圳）有限公司 | Positioning method and device |
| CN112567203A (en) * | 2018-07-23 | 2021-03-26 | 赛峰集团 | Method and apparatus for assisting fleet vehicle navigation using an invariant Kalman filter |
| CN113706854A (en) * | 2021-08-20 | 2021-11-26 | 北京科技大学 | Vehicle cooperative positioning method in intelligent Internet of vehicles |
| CN113870489A (en) * | 2021-09-10 | 2021-12-31 | 摩拜(北京)信息技术有限公司 | Vehicle positioning method and device and vehicle |
| CN113963551A (en) * | 2021-10-18 | 2022-01-21 | 中国电力科学研究院有限公司 | Vehicle positioning method, system, device and medium based on cooperative positioning |
| CN112567203B (en) * | 2018-07-23 | 2024-04-26 | 赛峰集团 | Method and apparatus for assisting in the navigation of a fleet of vehicles using a invariant Kalman filter |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1934513A (en) * | 2004-02-25 | 2007-03-21 | 学校法人立命馆 | Control system of floating mobile body |
| CN102077259A (en) * | 2009-02-27 | 2011-05-25 | 丰田自动车株式会社 | Vehicle relative position estimation apparatus and vehicle relative position estimation method |
| CN102192745A (en) * | 2010-02-24 | 2011-09-21 | 歌乐株式会社 | Position estimation device and position estimation method |
| CN104282020A (en) * | 2014-09-22 | 2015-01-14 | 中海网络科技股份有限公司 | Vehicle speed detection method based on target motion track |
| CN105682222A (en) * | 2016-03-01 | 2016-06-15 | 西安电子科技大学 | Vehicle location positioning information fusion method based on vehicular ad hoc network |
| WO2016170635A1 (en) * | 2015-04-23 | 2016-10-27 | 三菱電機株式会社 | Leading vehicle selection assistance device, travel plan creation device, leading vehicle selection assistance method, and travel plan creation method |
-
2017
- 2017-06-07 CN CN201710423028.XA patent/CN107274721B/en not_active Expired - Fee Related
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1934513A (en) * | 2004-02-25 | 2007-03-21 | 学校法人立命馆 | Control system of floating mobile body |
| CN102077259A (en) * | 2009-02-27 | 2011-05-25 | 丰田自动车株式会社 | Vehicle relative position estimation apparatus and vehicle relative position estimation method |
| CN102192745A (en) * | 2010-02-24 | 2011-09-21 | 歌乐株式会社 | Position estimation device and position estimation method |
| CN104282020A (en) * | 2014-09-22 | 2015-01-14 | 中海网络科技股份有限公司 | Vehicle speed detection method based on target motion track |
| WO2016170635A1 (en) * | 2015-04-23 | 2016-10-27 | 三菱電機株式会社 | Leading vehicle selection assistance device, travel plan creation device, leading vehicle selection assistance method, and travel plan creation method |
| CN105682222A (en) * | 2016-03-01 | 2016-06-15 | 西安电子科技大学 | Vehicle location positioning information fusion method based on vehicular ad hoc network |
Cited By (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108398704B (en) * | 2018-02-06 | 2020-11-06 | 北京科技大学 | Bayesian filtering multi-vehicle cooperative positioning method |
| CN108398704A (en) * | 2018-02-06 | 2018-08-14 | 北京科技大学 | A kind of more vehicle cooperative localization methods of Bayesian filter |
| CN108519738A (en) * | 2018-04-13 | 2018-09-11 | 中国科学院微电子研究所 | A kind of state of motion of vehicle information delay compensation method and device |
| CN112567203A (en) * | 2018-07-23 | 2021-03-26 | 赛峰集团 | Method and apparatus for assisting fleet vehicle navigation using an invariant Kalman filter |
| CN112567203B (en) * | 2018-07-23 | 2024-04-26 | 赛峰集团 | Method and apparatus for assisting in the navigation of a fleet of vehicles using a invariant Kalman filter |
| CN111912413A (en) * | 2020-07-23 | 2020-11-10 | 腾讯科技（深圳）有限公司 | Positioning method and device |
| CN111912413B (en) * | 2020-07-23 | 2022-04-19 | 腾讯科技（深圳）有限公司 | Positioning method and device |
| CN113706854B (en) * | 2021-08-20 | 2023-03-07 | 北京科技大学 | Vehicle cooperative positioning method in intelligent Internet of vehicles |
| CN113706854A (en) * | 2021-08-20 | 2021-11-26 | 北京科技大学 | Vehicle cooperative positioning method in intelligent Internet of vehicles |
| CN113870489A (en) * | 2021-09-10 | 2021-12-31 | 摩拜(北京)信息技术有限公司 | Vehicle positioning method and device and vehicle |
| CN113870489B (en) * | 2021-09-10 | 2023-02-07 | 摩拜(北京)信息技术有限公司 | Vehicle positioning method and device and vehicle |
| CN113963551B (en) * | 2021-10-18 | 2022-09-27 | 中国电力科学研究院有限公司 | Vehicle positioning method, system, device and medium based on cooperative positioning |
| CN113963551A (en) * | 2021-10-18 | 2022-01-21 | 中国电力科学研究院有限公司 | Vehicle positioning method, system, device and medium based on cooperative positioning |
| CN114608590B (en) * | 2022-03-09 | 2024-04-30 | 吉林大学 | Multi-vehicle tracking method based on intelligent reflecting surface in severe environment |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107274721B (en) | 2020-03-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107274721A (en) | Many vehicle cooperative localization methods in a kind of intelligent transportation system | |
| CN109946731B (en) | Vehicle high-reliability fusion positioning method based on fuzzy self-adaptive unscented Kalman filtering | |
| US8315756B2 (en) | Systems and methods of vehicular path prediction for cooperative driving applications through digital map and dynamic vehicle model fusion | |
| Georgy et al. | Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation | |
| Skog et al. | In-car positioning and navigation technologies—A survey | |
| Georgy et al. | Enhanced MEMS-IMU/odometer/GPS integration using mixture particle filter | |
| CN101099076B (en) | Hybrid inertial navigation system based on a kinematic model | |
| CN103777220B (en) | Based on the accurate position and orientation estimation method in real time of optical fibre gyro, speed pickup and GPS | |
| CN104061899B (en) | A kind of vehicle side inclination angle based on Kalman filtering and angle of pitch method of estimation | |
| CN102928816A (en) | High-reliably integrated positioning method for vehicles in tunnel environment | |
| CN104808220B (en) | Vehicle localization integrity monitoring method based on wireless information interaction | |
| WO2018072350A1 (en) | Vehicle trajectory prediction method and device | |
| CN104835353A (en) | Cooperation relative positioning method based on INS and GNSS pseudo-range double difference for VANET | |
| US11117590B2 (en) | Method and system for determining effective wind speeds for motor vehicles | |
| WO2001050088A1 (en) | Global positioning system roadside integrated precision positioning system | |
| Suwandi et al. | Low-cost IMU and GPS fusion strategy for apron vehicle positioning | |
| CN105021198A (en) | Position estimation method based on integrated navigation of multiple sensors | |
| CN112346103A (en) | V2X-based intelligent networking automobile dynamic co-location method and device | |
| CN106093992A (en) | A kind of sub-meter grade combined positioning and navigating system based on CORS and air navigation aid | |
| CN103395435A (en) | Real-time positioning system method of high-precision and high-speed train | |
| Iqbal et al. | Experimental results on an integrated GPS and multisensor system for land vehicle positioning | |
| CN108254775A (en) | Onboard navigation system and its implementation | |
| CN108885269A (en) | Air navigation aid, navigation device and navigation system | |
| Hsu et al. | Intelligent viaduct recognition and driving altitude determination using GPS data | |
| Georgy et al. | Nonlinear filtering for tightly coupled RISS/GPS integration |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CF01 | Termination of patent right due to non-payment of annual fee | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20200331 |