CN103472459A - GPS (Global Positioning System)-pseudo-range-differential-based cooperative positioning method for vehicles - Google Patents

Abstract

The invention aims to provide a GPS (Global Positioning System)-pseudo-range-differential-based cooperative positioning method for vehicles. A standard GPS receiver with low cost is used for precisely estimating relative positions of any automobiles. The dominant motive by adopting pseudo-range differential is that the observed quantity of the GPG receiver close to an automobile is ordinarily affected by common errors (for example, same troposphere delay and ionized layer delay), and the errors can be counteracted through a differential process, so that an obtained relative vector is more precise than that of absolute positioning. As the automobile is not directly connected usually, a GPS measured value needs to be exchanged through V2V (Vehicle to Vehicle) wireless communication equipment. According to the method provided by the invention, by combining a constant turning angular velocity and velocity (CTRV) vehicle motion model and adopting an unscented Kalman filter to fuse the GPS measured value and a kinematical sensor measured value, stable estimation to the system state of a target vehicle is provided. The special sensor errors, such as excursion, can be estimated anytime and reduced through unscented Kalman filtering.

Description

A kind of vehicle cooperative localization method based on the GPS pseudo range difference

Technical field

The invention belongs to car networking technology field, be specifically related to the vehicle cooperative localization method based on the GPS pseudo range difference in the car networking.

Background technology

Along with birth and the development of Internet of Things, this several years motor-doms produce similar new ideas---car networking (IOV, Internet of Vehicle).Car networking is the fine vision of automobile industry for following " interconnected automobile ", just as the eighties in 20th century internet appearance independently desktop computer interknit together.The real world applications of car networking, will change people's life in future, working method.

The car networking refers to car and car ,Che Yu road, car and people, car and sensing equipment etc. alternately, realizes the dynamic mobile telecommunication system that vehicle is communicated by letter with public network.It can interconnect and realize information sharing by car and car, car and people ,Che Yu road, collect the information of vehicle, road and environment, and on the information network platform, the information of multi-source collection is processed, is calculated, shared and the safety issue, according to different functional requirements, vehicle is carried out to effectively guiding and supervision, and professional multimedia and mobile Internet application service is provided.

That in car networking, " gives power " is applied as intelligent transportation system (ITS, Intelligent Transportation System).Intelligent transportation system is effectively integrated by advanced infotech, Electronic transducer technology, data communication transmission technology, control technology and computer technology etc., then it is used in the traffic control system of whole ground, so that comprehensive traffic transportation management service large-scale, omnibearing, real-time precise and high efficiency to be provided.The traffic jam that can make intelligent transport technology reduces approximately 60%, short-distance transport efficiency is improved nearly 70%, makes the traffic capacity of existing road net improve 2-3 doubly.

Car networking is to be loaded in electronic tag on vehicle by a kind of vehicular ad hoc network (VANET of the technique constructions such as wireless identification; Vehicular Ad hoc Network) [Toor Y; Muhlethaler P, Laouiti A.Vehicle ad hoc networks:Applications and related technical issues[J] .Communications Surveys & Tutorials, IEEE, 2008,10 (3): 74-88.].The car networking has node characteristic, mobility and data flow characteristic.Architectural framework [Wang Jianqiang from the car networking, Wu Chenwen, Li Xiaojun. car networking framework and gordian technique research [J]. microcomputer information, 2011,27 (4): 156-158.] in, can find out, car networking RFID, sensor, from the physical world obtaining information, then these information exchanges are crossed to network and pass to user or server end, for the user provides various services.Because automobile moves, all services that provide all are based on the service of position, so location technology is the gordian technique in the car networking, has very large researching value and engineering practice and are worth.

Location technology not only can provide various location-based services, as the inquiry of the information such as peripheral shop, refuelling station, restaurant, and can also be for services such as the scheduling of relief in emergency circumstances, vehicle and personal managements.Location in the car networking also has a most important application to provide navigation Service for automobile exactly.GPS (GPS, Global Positioning System) [Kaplan E D, Hegarty C J.Understanding GPS:principles and applications[M] .Artech House Publishers, 2006.] can provide position and velocity information for user vehicle.In recent years, along with the reduction of GPS receiver cost, GPS has become the standard scheme that solves the vehicle location problem, and the positioning navigating device of most motor vehicles for civilian use all is based on the GPS technology.The stand alone type GPS positioning precision that can provide, between 10-20 rice, and easily is subject to the impact of urban environment, and the existence of " blind area " is often arranged, and as satellite-signal is stopped by increasing skyscraper and three-dimensional traffic, the precision of location can't guarantee.Inertial navigation system (INS, Inertial navigation system) is a kind of autonomic navigation system.The navigation information continuity that the INS system produces is good and disturb to external world insensitively, but its error has in time the characteristics of accumulation, so be not suitable for long-time location-independent.The combination of the two---GPS/INS combined positioning and navigating system can solve the problem of " blind area " location effectively, is once becoming the focus of academia's research.

Although GPS/INS combined positioning and navigating system can solve the problem of " blind area " location, the absolute precision of location can't be brought up to the track level, can not meet the reliability requirement of many many aspects of vehicle safety applications.In recent years, this comparatively novel concept of co-positioned has appearred carrying out in vehicular ad hoc network.By periodic broadcast, each automobile can be collected the GPS information of adjacent vehicle, and simultaneously, the distance between vehicle can record by the radio distance-measuring technology.Therefore each automobile can utilize GPS information and the distance measure of adjacent vehicle, adjusts the position of oneself according to the co-positioned algorithm.

Document [Hofmann-Wellenhof B, Lichtenegger H, Collins J.GPS theory and practice[J] .1997.] gps system is studied, gps system is current the most ripe, most popular technical a kind of satellite navigation and location system, and high precision and continuous position, speed, course, attitude and temporal information can be provided easily.It is round-the-clock running, range finding via satellite and position, velocity information and the precise time of continuous three-dimensional are provided while surveying.Civilian GPS positioning precision is generally in the 10m left and right, and military GPS positioning precision is higher, can reach the 1m left and right.

The GPS location technology is ripe, use extensively, but also comes with some shortcomings, and at first, GPS is only in the situation that there are 4 satellites just can position simultaneously in the visual field, and, under High-Density Urban Area and indoor shielding environment, can't collect neat 4 satellites, thereby GPS lost efficacy.Secondly, the GPS primary positioning time is long, in the situation that more nervous possibly can't meeting the demands of time.Again, if do not transmit by correspondence positional information, the GPS receiver has just been realized the location to self, when running into emergency condition, still can't obtain relief.

Document [Rogers R M.Applied mathematics in integrated navigation systems[M] .AIAA, 2003.] the INS system is studied, the INS system be a kind ofly do not rely on external information, also not to the autonomic navigation system of outside emittance.It is comprised of two parts: Inertial Measurement Unit (IMU, Inertial Measurement Unit) and navigation computing unit (NCU, Navigation Computer Unit).IMU partly is comprised of as gyroscope and accelerometer the inertia sensing device.These senser elements are arranged on mobile platform inside orthogonally, are used for measure linear acceleration and angular velocity increment.The NUC part is merged the measured value of senser element, to calculate position, speed and the attitude angle increment of automobile, and these measured values is converted to the local horizon system of automobile from inertial reference system.

The INS system is due to the impact that is not subject to outside electromagnetic interference, therefore can, in the situation that gps signal interrupts, according to the vehicle movement rule, carry out independent positioning.But because inertia device has fixing drift rate, can cause like this positioning error of moving object, this error can constantly be accumulated in time, thereby be not suitable for long-time location-independent.GPS/INS combined positioning and navigating system can solve the problem of " blind area " location, but the absolute precision of location can't be brought up to the track level, can not meet the reliability requirement of many many aspects of vehicle safety applications.

Summary of the invention

Goal of the invention: the present invention proposes a kind of vehicle cooperative localization method based on the GPS pseudo range difference, accurately estimate the relative position of any automobile with standard GPS receiver cheaply.It is the impact (as identical tropospheric delay and ionosphere delay) that the observed quantity of the GPS receiver of adjacent vehicle generally can be subject to common error that the present invention adopts the mainspring of pseudo range difference, pass through differential process, these errors can be offset, and the relative vector therefore drawn can be more more accurate than absolute fix.Because automobile is not direct-connected usually, the GPS measured value need to be by the exchange of V2V Wireless Telecom Equipment.

Technical scheme

Vehicle cooperative localization method based on the GPS pseudo range difference

Usually, GPS position definite is a plurality of original GPS measured value by catching contemporaneity and processes through least-squares estimation.It is the determined observed quantity of flight time of receiving antenna and visible satellite that these original observed quantities---are commonly referred to pseudorange---.Because describing by a three-dimensional coordinate fully tellurian position, therefore need in theory at least three pseudoranges to position.Owing to receiving the asynchronous of clock, the time migration between satellite and receiver user also must be considered.Therefore, the clock skew of position need to be estimated by the 4th Pseudo-range Observations.

According to document [Kaplan E D, Hegarty C J.Understanding GPS:principles and applications[M] .Artech House Publishers, 2006.], the GPS receiver of automobile a and the pseudorange between satellite i can be expressed as:

${\mathrm{\ρ}}_a^i=R_a^i+c(t_a-T_i)---\left(1\right)$

In formula,

represent the GPS receiver of automobile a and the actual range between satellite i, c is the light velocity, t _aleading while representing receiver clock with respect to system, T _ileading while representing satellite clock with respect to system.Skew T when satellite clock and system _ican compensate by the correcting value in broadcast message, no longer think T _iit is unknown number.Therefore, the equation of front can be designated as:

${\mathrm{\ρ}}_a^i=R_a^i+{\mathrm{ct}}_a---\left(2\right)$

Three-dimensional position (x for the GPS receiver of determining automobile a _a, y _a, z _a) and clock offset t _a, four satellites are carried out to pseudo range measurement, produce system of equations

${\mathrm{\ρ}}_a^1=\sqrt{{(x_1-x_a)}^2+{(y_1-y_a)}^2+{(z_1-z_a)}^2+{\mathrm{ct}}_a}---\left(3\right)$

${\mathrm{\ρ}}_a^2=\sqrt{{(x_2-x_a)}^2+{(y_2-y_a)}^2+{(z_2-z_a)}^2+{\mathrm{ct}}_a}---\left(4\right)$

${\mathrm{\ρ}}_a^3=\sqrt{{(x_3-x_a)}^2+{(y_3-y_a)}^2+{(z_3-z_a)}^2+{\mathrm{ct}}_a}---\left(5\right)$

${\mathrm{\ρ}}_a^4=\sqrt{{(x_4-x_a)}^2+{(y_4-y_a)}^2+{(z_4-z_a)}^2+{\mathrm{ct}}_a}---\left(6\right)$

Here (x _i, y _i, z _i) referring to the three-dimensional position of i satellite, the scope of i is 1～4.

These nonlinear equations can solve unknown number by method of linearization or Kalman filtering.In practice, pseudorange also can be subject to the harmful effect such as ephemeris error, atmospheric envelope error equal error.The pseudorange equation can further be rewritten as:

${\mathrm{\ρ}}_a^i=R_a^i+{\mathrm{ct}}_a+{\mathrm{\ϵ}}_a^{i,\mathrm{eph}}+{\mathrm{\ϵ}}_a^{i,\mathrm{trop}}+{\mathrm{\ϵ}}_a^{i,\mathrm{ion}}---\left(7\right)$

In formula,

represent the GPS receiver of automobile a and the actual range between satellite i, c is the light velocity, t _aleading while representing receiver clock with respect to system,

represent ephemeris error,

represent tropospheric error,

represent ionospheric error.Other errors, as receiver noise and multipath error are not temporarily considered.Can solve the overdetermined equation group, and by the least-squares estimation technology, redundant measurements be processed by being measured more than 4 satellites, obtain the estimation that unknown quantity is improved.But the error of not taking into account still can make final absolute position estimate to produce deviation.

If can obtain the absolute position of the GPS receiver of two automobiles, estimate, just can carry out relative positioning, a kind of direct way is directly to two position coordinates difference in same coordinate system.Although this method is simple, has many weak points.At first, must guarantee that two receivers carry out pseudo range measurement to same group of satellite, to guarantee identical common error.Secondly, two receivers must adopt identical absolute position solution technique, otherwise just may produce strange result.In the situation that above-mentioned condition can be satisfied, the relative vector that differential position obtains is really more accurate than absolute position, because two receivers almost are subject to same error, after difference, impact reduces.

With position class seemingly, the pseudo-range measurements of the GPS receiver of automobile also is subject to same error effect in certain area.GPS receiver by the GPS receiver to automobile a and automobile b carries out difference with respect to the pseudo-range measurements of same satellite i, and the common error caused by satellite i can be cancelled:

${\mathrm{\Δ\ρ}}_{\mathrm{ab}}^i={\mathrm{\ρ}}_a^i-{\mathrm{\ρ}}_b^i$

$=R_a^i-R_b^i+c(t_a-t_b)---\left(8\right)$

$={\mathrm{\ΔR}}_{\mathrm{ab}}^i+c\·{\mathrm{\Δt}}_{\mathrm{ab}}$

In formula,

the pseudo range difference value,

what be automobile a and b to the actual range of satellite i is poor, △ t _abit is the clock difference of two receivers.Therefore, have four unknown quantitys (three-dimensional relative vector

clock difference △ t with two receivers _ab).Two automobiles with respect to the pseudo range difference schematic diagram of same satellite as shown in Figure 1.Because satellite i is greater than the distance between automobile a, b to the distance of automobile a, b, so the vector that points to a, b from i can be thought to be parallel to each other.Known according to Fig. 1, the pseudo range difference value

for relative vector

projection.For example, when two automobiles approaches each other very much, even be positioned at same position, the pseudo range difference value will approach 0.

By upper analysis, when at least four pseudo-range measurements are arranged, relative vector (poor with receiver clock) just can be estimated.

The wireless communication system of vehicle cooperative location

The wireless communication system of the vehicle cooperative location that the present invention proposes as shown in Figure 2.

GPS pseudorange information can be exchanged by the V2V Wireless Telecom Equipment, and this Wireless Telecom Equipment is based on the IEEE802.11p standard.For the sake of simplicity, it can be regarded as to the reorganization of well-known family expenses Wi-Fi network IEEE802.11a under vehicle environmental.This reorganization allows to carry out self-organization communication flexibly between node.In automobile enters the communication range at another ITS station, they are without further configuration immediately just can exchange message.

The IEEE802.11p agreement in Europe operates in 5.8GHz, has three independent transmission channels to can be used for different services.The general range of communication reaches 250m, and actual range depends on the environmental parameter such as site coverage and plant.Therefore, vehicle can not carry out direct point-to-point with near every other ITS station and is connected.ETSI (ETSI) is still continuing to be devoted to study a network layer protocol, and it can pass through the multi-hop Routing expansion communication range based on geographical.This so-called geographical network agreement is supported the number of ways propagation data.A kind of main method is geographical broadcast, allows all nodes of broadcast data to the specific geographic position, and this position can be circular, square or oval under a specific coordinate.May be as relaying until message be passed to the node in final position even each ITS station is---directly not interested in this content---.Another kind method is topology broadcast, and data are broadcast to all ITS station in certain jumping figure.The relevant message of safety is generally to send by such network layer.These message are used a predetermined channel---to be called control channel---to guarantee the transmission of fast and reliable.Typical Representative is cooperative sensing message (CAM), and it also has traffic light status information except comprising position and dynamic information, usually with the frequency of 1-10Hz, broadcasts.These initial messages most of by standardization to guarantee compatibility and the interoperability between different suppliers.

Need to exchange the GPS raw data in the present invention, be achieved as supplier's characteristic expansion by the top in the geographical network agreement.In this expansion, defined new GPS original measurement value (GRM) message.Because for co-positioned, near the vehicle only having is most interested to GRM message, so with geographical broadcast addressing mode transmission GRM message.This means do not have the ITS station to forward GRM message to increase communication range.The transmission frequency of GRM message is made as 1Hz at first, and can dynamic-configuration.Owing to lacking current control and definite transmission sequence, the content of each GRM message must be included in (nearly 1500 bytes) in a wireless data packet.In order to meet this restriction, after data are compressed, with effective binary format, store.

Table 1 has been described the main contents that single GRM message comprises.Except the pseudo-range measurements of GPS receiver in car, beyond synchronized timestamp, this message also comprises a unique vapour license number.

The parametric description of the GRM message of transmitting between table 1 automobile

The vehicle movement model

Usually the supposition vehicle is subject to the restriction of power and can not moves arbitrarily.In fact vehicle is owing to being subject to inertia effect, and meeting is more or less constantly moved.Have multiple vehicle movement model in document, difference only be to suppose what constant on.A simple model is constant speed (CV) model.Its assumed vehicle travels with a constant speed, and acceleration and deceleration are motor-driven to be only had by enough process noise parameter v are provided _kcould process proper.Select an appropriate motion model not only to be determined by required application, also will consider available sensor.In the present invention, the estimation of target vehicle state has been selected to constant turning rate and speed (CTRV) model.

The state space of CTRV model comprises 5 parameters:

CTRV _X=(xyθvw) ^T (9)

In formula, x and y mean the horizontal level of automobile in cartesian coordinate system, the course angle that θ is automobile, and v and w are respectively speed and angular velocity

this type of kinematic sensors measured value can obtain by CAN bus in car.

The state transition equation of CTRV is:

$X_{k+1}=X_k+\left[\begin{array}{c}\frac{v}{w}[\sin ({\mathrm{\θ}}_k+{\mathrm{wT}}_k)-\sin \left({\mathrm{\θ}}_k\right)]\\ \frac{v}{w}[\cos {\mathrm{\θ}}_k-\cos ({\mathrm{\θ}}_k+{\mathrm{wT}}_k)]\\ {\mathrm{wT}}_k\\ 0\\ 0\end{array}\right]---\left(10\right)$

For the special circumstances of w=0, this equation can't calculate, and therefore needs the state transition equation of a simplification:

$X_{k+1}=X_k+\left[\begin{array}{c}v\cos \left({\mathrm{\θ}}_k\right)T_k\\ v\sin \left({\mathrm{\θ}}_k\right)T_k\\ 0\\ 0\\ 0\end{array}\right]---\left(11\right)$

If the angular velocity of vehicle current driving is w=0, just do not mean and turn, so the CTRV equation has become more simple CV model.

Because the CTRV state transition equation is nonlinear, therefore adopt the Unscented kalman filtering device that GPS measured value and kinematic sensors measured value are merged, provide the stable estimation to the target vehicle system state.By Unscented kalman filtering, even if kine bias is moved this class, specific sensor error can estimate in time and reduce.

Beneficial effect

1) the present invention proposes a kind of vehicle cooperative localization method based on pseudo range difference, offset the common error caused by satellite by pseudo range difference, therefore the relative vector drawn is more more accurate than absolute fix.Provided the wireless communication system block diagram of vehicle cooperative location, defined new GPS original measurement value (GRM) message, and transmitted between vehicle with the geographical broadcast addressing mode.

2) the present invention, in conjunction with constant turning rate and speed (CTRV) vehicle movement model, adopts the Unscented kalman filtering device that GPS measured value and kinematic sensors measured value are merged, and the stable estimation to the target vehicle system state is provided.By Unscented kalman filtering, even if kine bias is moved this class, specific sensor error can estimate in time and reduce.

The accompanying drawing explanation

Fig. 1 is with respect to the pseudo range difference schematic diagram of the two automobiles of same satellite

The wireless communication system of Fig. 2 vehicle cooperative location

Embodiment

Below by conjunction with Figure of description, further illustrate technical scheme of the present invention.

GPS receiver by the GPS receiver to automobile a and automobile b carries out difference with respect to the pseudo-range measurements of same satellite i, and the common error caused by satellite i can be cancelled:

${\mathrm{\Δ\ρ}}_{\mathrm{ab}}^i={\mathrm{\ρ}}_a^i-{\mathrm{\ρ}}_b^i$

$=R_a^i-R_b^i+c(t_a-t_b)$

$={\mathrm{\ΔR}}_{\mathrm{ab}}^i+c\·{\mathrm{\Δt}}_{\mathrm{ab}}$

In formula,

the pseudo range difference value,

what be automobile a and b to the actual range of satellite i is poor, △ t _abit is the clock difference of two receivers.Therefore, have four unknown quantitys (three-dimensional relative vector clock difference △ t with two receivers _ab).Two automobiles with respect to the pseudo range difference schematic diagram of same satellite as shown in Figure 1.Because satellite i is greater than the distance between automobile a, b to the distance of automobile a, b, so the vector that points to a, b from i can be thought to be parallel to each other.Known according to Fig. 1, the pseudo range difference value

for relative vector

projection.For example, when two automobiles approaches each other very much, even be positioned at same position, the pseudo range difference value will approach 0.

By upper analysis, when at least four pseudo-range measurements are arranged, relative vector (poor with receiver clock) just can be estimated.

The wireless communication system of the vehicle cooperative location that the present invention proposes as shown in Figure 2.

GPS pseudorange information can be exchanged by the V2V Wireless Telecom Equipment, and this Wireless Telecom Equipment is based on the IEEE802.11p standard.For the sake of simplicity, it can be regarded as to the reorganization of well-known family expenses Wi-Fi network IEEE802.11a under vehicle environmental.This reorganization allows to carry out self-organization communication flexibly between node.In automobile enters the communication range at another ITS station, they are without further configuration immediately just can exchange message.

Need to exchange the GPS raw data in the present invention, be achieved as supplier's characteristic expansion by the top in the geographical network agreement.In this expansion, defined new GPS original measurement value (GRM) message.Because for co-positioned, near the vehicle only having is most interested to GRM message, so with geographical broadcast addressing mode transmission GRM message.This means do not have the ITS station to forward GRM message to increase communication range.The transmission frequency of GRM message is made as 1Hz at first, and can dynamic-configuration.Owing to lacking current control and definite transmission sequence, the content of each GRM message must be included in (nearly 1500 bytes) in a wireless data packet.In order to meet this restriction, after data are compressed, with effective binary format, store.

Table 1 has been described the main contents that single GRM message comprises.Except the pseudo-range measurements of GPS receiver in car, beyond synchronized timestamp, this message also comprises a unique vapour license number.

The parametric description of the GRM message of transmitting between table 1 automobile

Usually the supposition vehicle is subject to the restriction of power and can not moves arbitrarily.In fact vehicle is owing to being subject to inertia effect, and meeting is more or less constantly moved.Have multiple vehicle movement model in document, difference only be to suppose what constant on.A simple model is constant speed (CV) model.Its assumed vehicle travels with a constant speed, and acceleration and deceleration are motor-driven to be only had by enough process noise parameter v are provided _kcould process proper.Select an appropriate motion model not only to be determined by required application, also will consider available sensor.In the present invention, the estimation of target vehicle state has been selected to constant turning rate and speed (CTRV) model.

The state space of CTRV model comprises 5 parameters:

CTRV _X=(xyθvw) ^T (9)

In formula, x and y mean the horizontal level of automobile in cartesian coordinate system, the course angle that θ is automobile, and v and w are respectively speed and angular velocity

this type of kinematic sensors measured value can obtain by CAN bus in car.

The state transition equation of CTRV is:

$X_{k+1}=X_k+\left[\begin{array}{c}\frac{v}{w}[\sin ({\mathrm{\θ}}_k+{\mathrm{wT}}_k)-\sin \left({\mathrm{\θ}}_k\right)]\\ \frac{v}{w}[\cos {\mathrm{\θ}}_k-\cos ({\mathrm{\θ}}_k+{\mathrm{wT}}_k)]\\ {\mathrm{wT}}_k\\ 0\\ 0\end{array}\right]---\left(10\right)$

For the special circumstances of w=0, this equation can't calculate, and therefore needs the state transition equation of a simplification:

$X_{k+1}=X_k+\left[\begin{array}{c}v\cos \left({\mathrm{\θ}}_k\right)T_k\\ v\sin \left({\mathrm{\θ}}_k\right)T_k\\ 0\\ 0\\ 0\end{array}\right]---\left(11\right)$

If the angular velocity of vehicle current driving is w=0, just do not mean and turn, so the CTRV equation has become more simple CV model.

Because the CTRV state transition equation is nonlinear, therefore adopt the Unscented kalman filtering device that GPS measured value and kinematic sensors measured value are merged, provide the stable estimation to the target vehicle system state.By Unscented kalman filtering, even if kine bias is moved this class, specific sensor error can estimate in time and reduce.

Claims (2)

1. the vehicle cooperative localization method based on the GPS pseudo range difference, is characterized in that, comprising:

Accurately estimate the relative position of any automobile with standard GPS receiver cheaply; Offset the common error caused by satellite by pseudo range difference; Provided the wireless communication system block diagram of vehicle cooperative location, defined new GPS original measurement value (GRM) message, and transmitted between vehicle with the geographical broadcast addressing mode.

2. a kind of vehicle cooperative localization method based on the GPS pseudo range difference according to claim 1, is characterized in that, comprising:

(1) the vehicle cooperative localization method based on the GPS pseudo range difference

This method needs four pseudoranges, and position and clock skew need to be estimated by four Pseudo-range Observations, the GPS receiver of automobile a and the pseudorange between satellite i

can be expressed as:

${\mathrm{\ρ}}_a^i=R_a^i+c(t_a-T_i)---\left(1\right)$

In formula,

represent the GPS receiver of automobile a and the actual range between satellite i, c is the light velocity, t _aleading while representing receiver clock with respect to system, T _ileading while representing satellite clock with respect to system; Skew T when satellite clock and system _ican compensate by the correcting value in broadcast message, no longer think T _iit is unknown number; Therefore, the equation of front can be designated as:

${\mathrm{\ρ}}_a^i=R_a^i+{\mathrm{ct}}_a---\left(2\right)$

Three-dimensional position (x for the GPS receiver of determining automobile a _a, y _a, z _a) and clock offset t _a, four satellites are carried out to pseudo range measurement, produce system of equations

${\mathrm{\ρ}}_a^1=\sqrt{{(x_1-x_a)}^2+{(y_1-y_a)}^2+{(z_1-z_a)}^2+{\mathrm{ct}}_a}---\left(3\right)$

${\mathrm{\ρ}}_a^2=\sqrt{{(x_2-x_a)}^2+{(y_2-y_a)}^2+{(z_2-z_a)}^2+{\mathrm{ct}}_a}---\left(4\right)$

${\mathrm{\ρ}}_a^3=\sqrt{{(x_3-x_a)}^2+{(y_3-y_a)}^2+{(z_3-z_a)}^2+{\mathrm{ct}}_a}---\left(5\right)$

${\mathrm{\ρ}}_a^4=\sqrt{{(x_4-x_a)}^2+{(y_4-y_a)}^2+{(z_4-z_a)}^2+{\mathrm{ct}}_a}---\left(6\right)$

Here (x _i, y _i, z _i) referring to the three-dimensional position of i satellite, the scope of i is 1～4;

These nonlinear equations can solve unknown number by method of linearization or Kalman filtering; In practice, pseudorange also can be subject to the harmful effect such as ephemeris error, atmospheric envelope error equal error; The pseudorange equation can further be rewritten as:

${\mathrm{\ρ}}_a^i=R_a^i+{\mathrm{ct}}_a+{\mathrm{\ϵ}}_a^{i,\mathrm{eph}}+{\mathrm{\ϵ}}_a^{i,\mathrm{trop}}+{\mathrm{\ϵ}}_a^{i,\mathrm{ion}}---\left(7\right)$

In formula,

represent the GPS receiver of automobile a and the actual range between satellite i, c is the light velocity, t _aleading while representing receiver clock with respect to system,

represent ephemeris error,

represent tropospheric error,

represent ionospheric error; Other errors, as receiver noise and multipath error are not temporarily considered; Can solve the overdetermined equation group, and by the least-squares estimation technology, redundant measurements be processed by being measured more than 4 satellites, obtain the estimation that unknown quantity is improved; But the error of not taking into account still can make final absolute position estimate to produce deviation;

With position class seemingly, the pseudo-range measurements of the GPS receiver of automobile also is subject to same error effect in certain area; GPS receiver by the GPS receiver to automobile a and automobile b carries out difference with respect to the pseudo-range measurements of same satellite i, and the common error caused by satellite i can be cancelled:

${\mathrm{\Δ\ρ}}_{\mathrm{ab}}^i={\mathrm{\ρ}}_a^i-{\mathrm{\ρ}}_b^i$

$=R_a^i-R_b^i+c(t_a-t_b)---\left(8\right)$

$={\mathrm{\ΔR}}_{\mathrm{ab}}^i+c\·{\mathrm{\Δt}}_{\mathrm{ab}}$

In formula,

the pseudo range difference value,

what be automobile a and b to the actual range of satellite i is poor, △ t _abit is the clock difference of two receivers; Therefore, have four unknown quantitys (three-dimensional relative vector clock difference △ t with two receivers _ab); Because satellite i is greater than the distance between automobile a, b to the distance of automobile a, b, so the vector that points to a, b from i can be thought to be parallel to each other; The pseudo range difference value

for relative vector projection; For example, when two automobiles approaches each other very much, even be positioned at same position, the pseudo range difference value will approach 0;

By upper analysis, when at least four pseudo-range measurements are arranged, relative vector (poor with receiver clock) just can be estimated;

(2) wireless communication system of vehicle cooperative location

GPS pseudorange information can be exchanged by the V2V Wireless Telecom Equipment, and this Wireless Telecom Equipment is based on the IEEE802.11p standard; For the sake of simplicity, it can be regarded as to the reorganization of well-known family expenses Wi-Fi network IEEE802.11a under vehicle environmental; This reorganization allows to carry out self-organization communication flexibly between node; In automobile enters the communication range at another ITS station, they immediately just can exchange message without further configuration;

Exchange GPS raw data, defined new GPS original measurement value (GRM) message; Because for co-positioned, near the vehicle only having is most interested to GRM message, so with geographical broadcast addressing mode transmission GRM message; This means do not have the ITS station to forward GRM message to increase communication range; The transmission frequency of GRM message is made as 1Hz at first, and can dynamic-configuration; Owing to lacking current control and definite transmission sequence, the content of each GRM message must be included in (nearly 1500 bytes) in a wireless data packet; In order to meet this restriction, after data are compressed, with effective binary format, store;

Table 1 has been described the main contents that single GRM message comprises; Except the pseudo-range measurements of GPS receiver in car, beyond synchronized timestamp, this message also comprises a unique vapour license number;

The parametric description of the GRM message of transmitting between table 1 automobile

(3) vehicle movement model

Usually the supposition vehicle is subject to the restriction of power and can not moves arbitrarily; In fact vehicle is owing to being subject to inertia effect, and meeting is more or less constantly moved; Have multiple vehicle movement model in document, difference only be to suppose what constant on; A simple model is constant speed (CV) model; Its assumed vehicle travels with a constant speed, and acceleration and deceleration are motor-driven to be only had by enough process noise parameter v are provided _kcould process proper; Select an appropriate motion model not only to be determined by required application, also will consider available sensor; In the present invention, the estimation of target vehicle state has been selected to constant turning rate and speed (CTRV) model;

The state space of CTRV model comprises 5 parameters:

CTRV _X=(xyθvw) ^T (9)

In formula, x and y mean the horizontal level of automobile in cartesian coordinate system, the course angle that θ is automobile, and v and w are respectively speed and angular velocity

this type of kinematic sensors measured value can obtain by CAN bus in car;

The state transition equation of CTRV is:

$X_{k+1}=X_k+\left[\begin{array}{c}\frac{v}{w}[\sin ({\mathrm{\θ}}_k+{\mathrm{wT}}_k)-\sin \left({\mathrm{\θ}}_k\right)]\\ \frac{v}{w}[\cos {\mathrm{\θ}}_k-\cos ({\mathrm{\θ}}_k+{\mathrm{wT}}_k)]\\ {\mathrm{wT}}_k\\ 0\\ 0\end{array}\right]---\left(10\right)$

For the special circumstances of w=0, this equation can't calculate, and therefore needs the state transition equation of a simplification:

$X_{k+1}=X_k+\left[\begin{array}{c}v\cos \left({\mathrm{\θ}}_k\right)T_k\\ v\sin \left({\mathrm{\θ}}_k\right)T_k\\ 0\\ 0\\ 0\end{array}\right]---\left(11\right)$

If the angular velocity of vehicle current driving is w=0, just do not mean and turn, so the CTRV equation has become more simple CV model;

Because the CTRV state transition equation is nonlinear, therefore adopt the Unscented kalman filtering device that GPS measured value and kinematic sensors measured value are merged, provide the stable estimation to the target vehicle system state; By Unscented kalman filtering, even if kine bias is moved this class, specific sensor error can estimate in time and reduce.

Images