Lane-Level Vehicle Trajectory Reckoning for Cooperative Vehicle-Infrastructure System

This paper presents a lane-level positioning method by trajectory reckoning without Global Positioning System GPS equipment in the environment of Cooperative Vehicle-Infrastructure System CVIS . Firstly, the accuracy requirements of vehicle position in CVIS applications and the applicability of GPS positioning methods were analyzed. Then, a trajectory reckoning method based on speed and steering data from vehicle’s Control Area Network CAN and roadside calibration facilities was proposed, which consists of three critical models, including real-time estimation of steering angle and vehicle direction, vehicle movement reckoning, and wireless calibration. Finally, the proposed method was validated through simulation and field tests under a variety of traffic conditions. Results show that the accuracy of the reckoned vehicle position can reach the lane level and match the requirements of common CVIS applications.


Introduction
The recently developed Cooperative Vehicle-Infrastructure System CVIS enables vehicle with the capability to communicate with other vehicles and infrastructure and to ensure safety and travel efficiency by driving assistance 1 .The capability of providing accurate position estimation is essential for successful and widespread deployment of CVIS applications such as optimal speed advisory and lane change assistant.
The Global Positioning System GPS is one of the most convenient and accurate methods for determining vehicle position in a global coordinate system 2, 3 .Low-cost GPS receivers can provide positioning service with an accuracy of approximately 10-20 meters, and the Differential GPS DGPS receivers have improved the positioning accuracy to approximately 2-3 meters, but require high construction cost for both onboard receiver and base station.To realize lane-level positioning, vehicle movement dynamics was integrated into the position estimate by an Extended Kalman filter utilizing only low-cost GPS equipment, and the GPS errors were estimated from an event-driven vehicle turn and lanealignment optimization model based on an accurate GPS description of the roadway 4 .In addition, a vehicle-lane-determining system was described in 5 , which consists of an onboard DGPS receiver connecting with a wireless communications channel, a unique lanelevel digital roadway database, a developed lane-match algorithm, and a real-time vehicle location display.Deduced Reckoning commonly referred to as "Dead" Reckoning or DR sensors consisting of an odometer and a gyroscope are routinely used to bridge any gaps in GPS positioning 6 .This information is then used with spatial road network data to determine the spatial reference of vehicle location via a process known as map matching 7 .Toledo-Moreo presented a solution that integrated a Global Navigation Satellite System GNSS receiver, an odometer, and a gyroscope, along with the road information stored in enhanced digital maps to analyze the reckoning trajectory 8 .
On the other hand, most of the CVIS applications do not need the vehicle position within a global coordinate system.Positioning methods in relative coordinate systems such as Radio Frequency Identification RFID were developed recently.Accurate indoor position estimation utilizing passive RFID has been realized 9, 10 .In road traffic, Lee proposed a RFID-assisted localization system, which employed the DGPS concept to improve GPS accuracy 11 .Scheme of active RFID positioning of vehicles was developed in 12 , the RFID tags were installed along lanes on road to form RFID matrix and the vehicle equipped with RFID readers locate itself by retrieving position information from tags around it.
This paper explores lane-level positioning by vehicle trajectory reckoning utilizing speed and steering data from vehicle's Control Area Network CAN .In addition, since the method was deployed in CVIS environment, the roadside calibration facility was designed by utilizing Detected Short Range Communication DSRC technology to improve the trajectory reckoning.

The CVIS Environment
In a previous publication 13 , a proof of concept for a prototype of Cooperative Vehicle-Infrastructure System TJ-CVIS developed at the University of Tongji was presented, which provides a vehicle with the capability to communicate with other vehicles and infrastructure.Conceptually, by using sensor technologies and standardized communication protocol, the vehicle probe data such as position, velocity, and preplanned routes can be automatically collected with high frequency and accuracy in CVIS.Therefore, a series of advanced traffic control applications and advanced driving assist applications for safety and efficiency can be deployed relying on the high sensitivity and information interactive abilities of CVIS.Table 1 lists the most common CVIS applications and their accuracy requirements for positioning 14 .

GPS Position Errors
Low-cost GPS equipment has been widely used in the navigation applications.However, as the analysis shown in Section 2.1, the accuracy requirements of navigation and CVIS  applications are markedly different.In order to test the feasibility of low-cost GPS equipment in CVIS, a common GPS receiver was placed at a set of stationary positions which form a grid shown in Figure 1.The positioning result is shown in Figure 2.
In Figure 2, the test result is totally out of shape compared to the actual shape of test sites.The distance between any two points in a line or row was calculated by the GPS data.The result shown in Table 2 indicates that in the situation of stationary positioning, the GPS errors arrive at 2.79 m on average, which is inapplicable for the CVIS applications.
In order to test the dynamic positioning accuracy of GPS, we conducted a test on an urban road, the starting and completing time stamps of lane changing were manually recorded.Figure 3 shows an example of dynamic positioning test in the situation of lane change.Results show that the performance of dynamic positioning is better than static

Input Parameters
The idea of lane-level positioning in this paper is reckoning the vehicle trajectory based on vehicle's driving-related data, then determining the position of vehicles.4 a .According to the data from CAN, the speed, steering wheel angle, and the wheels' steering angle can be recorded with a high frequency of 100 Hz, which enables the possibility to reckon the trajectory of a moving vehicle.Therefore, the vehicle position can be determined by the precise trajectory.Since vehicle's body shape bears on the reckoning input, it is necessary to define the reference point of the vehicle.In this study, we developed the trajectory reckoning algorithm based on the four-wheel vehicle with a wheel track of B and a wheel base of L shown in Figure 5.For the four-wheel vehicle, each front wheel should perform only rolling movement around the rotation center O when steering.As an example of right steering shown in Figure 5, steering angle of the inner wheel β should be greater than the outer wheel α .The relationship between β and α is described as the following: To simplify the algorithm, we use the point of rotation axis of left-front wheel as the reference point shown as P in Figure 5. Finally, the input parameters can be specified as the vehicle body shape parameters, left-front wheel's speed v , and its steering angle α .For the vehicles which can only output the steering wheel angle γ , α can be calculated by α f γ , which is specified by the vehicle type.Figure 6 shows an example of f γ tested by a common four-wheel vehicle.

Trajectory Reckoning
Since GPS is not utilized in our test, which means global coordinate system is not available for the trajectory reckoning method, we choose the dynamic-update coordinate system shown in Figure 7.The calibration method will be introduced in Section 3.3.
At any time, the rotation radius R, in meters-m of the reference point can be calculated as the following: Angular speed μ, in radian/second-rad/s of reference point as the following:

3.3
In 3.3 , v is the linear speed of the reference point in meters/second-m/s .When the trajectory is being reckoning,v and α are sampled with high frequency.Thus, the rotation angle ω, in radian-rad of the vehicle body can be calculated as the following: In 3.4 , μ i denotes the angular speed calculated by 3.3 at time sequencei, and t is the sampling interval.
The movements refer to the calibration point as:  In 3.5 , v i denotes the speed output at time sequence i, and the vehicle position at time sequence i can be reckoned as follows: Δy j .

Calibration Method
For the data output from CAN, the random errors cannot be avoided, and the cumulative errors in reckoning results increased rapidly when the distances get longer.It is necessary to calibrate the trajectory at a certain distance.Once the trajectory is calibrated, the dynamicupdated coordinate system referred above is updated.
The CVIS environment provides a vehicle with the capability to communicate with roadside infrastructures, which can be adopted to calibrate the vehicle trajectory.Figure 8 indicates the design of calibration facilities.The actuated loop, which has been already installed in most signalized intersections, is utilized in this method to detect the passing of vehicle.The directional antenna is set to send the calibration information to the vehicle by DSRC when vehicle is passing through the loop 16 .The road geometry data including road curvature, intersection geometry and lane drop geometry are stored in the onboard unit.In the prototype of TJ-CVIS 13 , the content of calibration information was specified as the following: actuate time; road section ID; loop ID; lane ID of the loop; distance to the downstream calibration points.
With the calibration information, vehicle's position can be determined by comparing the reckoned trajectory with road geometry data.We assume that the vehicle is not steering when passing a loop at intersections.The calibration procedure is described as the following.
Step 1. Calibration message received by the vehicle.
Step 2. Update coordinate system referring to loop ID, lane ID and road section ID, Step 3. x i v i × t latency , y i 0, ω 0. t latency denotes the constant value of wireless communication latency, t latency 0.01 s in the prototype of TJ-CVIS 13 .
Step 4. Keep trajectory reckoning next calibration.

Field Test
In order to validate the trajectory reckoning method, we conducted a field test at the campus of Tongji University.A computer program was developed on Windows platform to collect output data from CAN and reckon the trajectory.A common four-wheel vehicle was utilized as the experiment vehicle.Since the data protocol of CAN has not been disclosured by the manufacturer, we made use of the On-Board Diagnostics OBD protocol to read the vehicle speed.Hence only the algorithm for vehicle heading movement was validated in field test.
The first scenario of field test was deployed at a flattened square at the campus shown in Figure 9.The experiment vehicle was driven to pass through a pre-measured road segment with 37.5 m long.Another scenario of field test was deployed at a 276.6 m-long road shown in Figure 10.When experiment vehicle entering the test segment, the on-board unit started to reckon the trajectory.And when experiment vehicle leaving test segment, the distances of  trajectory between the vehicle's current position and the segment start line were recorded.The sampling frequency is 5 Hz in both scenarios.Test results are shown in Tables 3 and 4.
Results from field test indicate that the trajectory reckoning method can reach the submeter accuracy both in the two scenarios.It can also certify that the data from CAN are applicable for the reckoning method.Since the speed accuracy output by OBD protocol is limited to ±1 km/h 0.28 m/s , the reckoning accuracy will be improved if we use the standard CAN protocol, which has a nominal output speed accuracy of ±0.01 km/h 0.003 m/s .

Driving Simulator Trials
Another validation was taken on driving simulator due to the lack of CAN communication protocol of real vehicles.The driving simulator involved in validation was constructed by Tongji University, including a movement system with eight degrees of freedom shown in Figure 11.A real four-wheel vehicle with real steering system was installed in the movement system to simulate the situation of steering, braking, and acceleration.By using the driving simulator, the steering angle and speed can be sampled with high accuracy.Moreover, the actual trajectory can also be recorded for every sampling interval shown in Figure 12, which  is hardly to record in field test and can make it easier to analyze the trajectory reckoning errors.
In the driving simulator trials, the driver was asked to change lane and turn at different speeds.The speed and steering angle of front-left wheel were recorded every 0.05 s.According to the manufactory, the wheel base L is 2.686 m.In addition, virtual calibration points were set at outbound lanes of each intersection.Examples of lane change trajectory and turning trajectory reckoning are shown in Figures 13 and 14  The two examples indicate that the reckoning results are very close to the actual trajectories.If the geometry of the road and lane drop are known, the position of vehicle i.e., distance to the next stop line and in which lane the vehicle is driving can be determined by the reckoned trajectory.Table 5 shows the statistical result of driving simulator trials, indicating that the proposed trajectory reckoning method can reach the accuracy of submeter both in vehicle heading and lane heading directions U turn is not included in the statistic .

Conclusions and Future Work
This paper introduced a lane-level positioning method by trajectory reckoning without GPS equipment in the environment of CVIS.Vehicle driving data from CAN was adopted to reckon the vehicle movements, and then the position was determined by the reckoned trajectory object to the road geometry.In addition, a calibration method was designed to eliminate the cumulate errors during long-time reckoning.Results from field tests and driving simulator indicate that the proposed method can reach the positioning accuracy of submeter, which meets the requirements of most CVIS applications.Hence, the proposed method can be further deployed in the real traffic.Although the method outperforms GPS, based on the field test and driving simulator trials, it has certain limitations.For instance, the system errors increase as the increase of length of trajectory takes place, and the performance of calibration facility drops when too many vehicles approach, which deserve further discussion.under Grant 60974093.And the authors wish to acknowledge the vast and helpful assistance from Cooperative Vehicle-Infrastructure System Lab and the Driver Behavior and Traffic Safety Simulation Platform Tongji University, China for data collection.

Figure 1 :
Figure 1: Actual shape of test grid.

Figure 3 :
Figure 3: Dynamic positioning test of GPS.
Figure 4 a shows the actual vehicle trajectory of a left lane change event, Figures 4 b and 4 c show the variations of steering wheel angle and the front wheels' steering angle corresponding to the lane change event described in Figure

Figure 4 :
Figure 4: Variations of parameters in lane change event.

Figure 12 :
Figure 12: Actual trajectory and driving scenarios in driving simulator. .

Figure 13 :
Figure 13: Example of trajectory reckoning result of lane change.

Figure 14 :
Figure 14: Example of trajectory reckoning result of turning.

Table 1 :
Typical CVIS applications and positioning accuracy requirements.

Table 2 :
Stationary positioning accuracy of GPS.
13sitioning13.Although map-matching can improve the positioning accuracy if good spatial road network data are available 15 , a poor-quality road map could lead to a large error in map-matched solutions.Hence the lane changing event still cannot be easily recognized in the normal traffic environment.

Table 3 :
Result of field test Scenario 1 5 trials .

Table 4 :
Result of field test Scenario 2 10 trails .