Estimating Vehicle-to-Vehicle Relative Position and Attitude using Multiple UWB Ranges Ehab Ghanem [1,3], Kyle O’Keefe [2], and Richard Klukas[1] 1 University of British Columbia – Okanagan, BC, V1V 1V7, Canada 2 University of Calgary, Calgary, AB, T2N 1N4, Canada 3 Arab Academy for Science and Technology, Cairo, Egypt Abstract. UWB signals can be used to accurately measure ranges over short to medium distances (10 cm to 300 meters). These signals may be used for many positioning applications such as robot automation. Hence, many companies seek to exploit UWB signals. Many manufacturers are already offering chipsets that calculate the Time of Flight (TOF) for Two-Way-Ranging (TWR) with UWB signals. UWB ranges typically have an accuracy of 10 cm making them useful for not only estimating position, but potentially also for estimating the heading of a moving vehicle. This paper proposes a method using multiple UWB ranges to estimate 2-D relative position and heading of vehicle with respect to another. The method is described, a covariance simulation is presented and initial experi- mental results are discussed. Keywords: UWB, Kalman filter, Decawave, Attitude, Multi-Ranging. 1 Introduction The demand for autonomous vehicles is expected to grow in coming decades. These vehicles are required to be easy to control in order to help seniors and people with disabilities to move around with just a few commands. In order for an autonomous vehicle to be safe, relative positioning of surrounding vehicles and objects is important. Thus, a robust and accurate positioning technique is required. A vehicle to vehicle com- munication system is also proposed for these types of vehicles in order to control them in different manners and increase the efficiency of traffic flow [1]. Consequently, sig- nals that can be used for positioning estimation as well as communication are highly desirable. There are multiple sensors and systems that are used for positioning. The most prev- alent is GNSS (Global Satellite Navigation System), which has the advantages of global availability. However, consumer grade GNSS accuracy is generally on the order of 4 to 10 meters, and degrades significantly in urban and indoor areas. There have been many proposed solutions to improve the accuracy of GNSS. For example, one may integrate GNSS with low cost INS (Inertial Navigation System) sensors for use in urban areas. However, the accelerometers and gyroscopes in low cost INS tend to drift over time and require frequent GNSS updates which are generally not available in urban and 2 indoor environments. Other systems such as LIDAR can also be used to enhance GNSS position estimation indoors but require excessive computational resources. UWB communication signals, through the IEEE 802.15.4-2011 standard, can be used to measure ranges while enabling two-way communication [2]. UWB sensors are being promoted as additional sensors that can easily enable relative positioning and thus mitigate the drawbacks of GNSS in urban and indoor areas [3]. In this paper, the relative positioning and attitude capabilities of UWB ranges are tested and characterized. This will lead to the understanding of the performance of UWB in the absence of any other positioning system. The results of this study will point out the strengths and weaknesses of UWB ranging in order to appropriately weight an UWB solution versus others in a sensor fusion scheme. 2 Methodology The principle idea here is to collect range measurements from UWB radio modules placed on each side of each vehicle. The proposed scheme measures 4 different ranges between two UWB modules placed on each of two different vehicles. The ranges cor- respond to the distance between each side of a vehicle to that of the other as shown in Fig. 1(a). These ranges are then used to estimate the x and y coordinates and attitude (ΞΈ) of Vehicle 2 in the body frame of Vehicle 1, as shown in Fig. 1(b). Fig. 1. Illustration of the Two-Vehicle Scenario; (a) Measurements and (b) Unknowns. 𝑑 𝑑1 𝑑 2 𝑅1 = √(0 βˆ’ (π‘₯ βˆ’ 2 cos πœƒ))2 + ( βˆ’ (𝑦 + 2 π‘ π‘–π‘›πœƒ)) (1) 2 2 2 Equation (1) shows the relationship between the unknowns and R1. Similarly, one can form the relationship between the unknowns (x, y and ΞΈ) and all the ranges (R1, R2 R3, R4) and use them with an estimation algorithm such as least squares. In order to investigate the strengths and weaknesses of least squares, a covariance analysis was performed to calculate the Dilution of Precision (DOP) around the reference vehicle 3 (Vehicle 1). The DOP results, for the case of two vehicles with the same orientation (facing right) are shown in Fig. 2 and clearly show that there is large uncertainty in estimating x, y and ΞΈ along the y-axis, as well as in estimating ΞΈ along the x-axis. This is expected since along the y-axis, the four UWB modules are collinear and hence they do not yield a unique least squares solution. Furthermore, due to the symmetry of the observation geometry, there are also multiple valid solutions, and non-linear least- squares will only converge to the correct solution if the point of expansion is near- enough to the correct solution. Fig. 2. Calculated DOP around Vehicle 1; (a) x-DOP, (b) y-DOP and (c) ΞΈ-DOP. These areas of uncertainty can be overcome using observations from multiple epochs and a Kalman filter with a dynamics model. The Kalman filter will need to introduce other state parameters (π‘₯Μ‡ 𝑦̇ π‘Žπ‘›π‘‘ πœƒΜ‡) in order to predict the position and attitude through the dynamics model, represented in discrete time by the transition matrix Ξ¦. The state of the vehicle, π‘₯Μ‚ at any given time can be written as in equation (2). The predicted state of the vehicle can be calculated by: X Μ‚ = Ξ¦ π‘₯Μ‚, where Ξ¦ is defined as in equation 3. π‘₯Μ‚ = [π‘₯ 𝑦 πœƒ π‘₯Μ‡ 𝑦̇ πœƒΜ‡]𝑇 (2) 1 0 0 Δ𝑑 0 0 0 1 0 0 Δ𝑑 0 0 0 1 0 0 Δ𝑑 Ξ¦= (3) 0 0 0 1 0 0 0 0 0 0 1 0 𝑑 [ 0 0 0 0 0 𝑒 βˆ’πœ ] Note that the x coordinate and y coordinate are predicted with the constant velocity model equations π‘₯Μ‚ = π‘₯ + βˆ†π‘‘ π‘₯Μ‡ and 𝑦̂ = 𝑦 + βˆ†π‘‘ 𝑦̇ , while ΞΈ is predicted with the first 𝑑 order Gauss-Markov model equation πœƒΜ‚ = πœƒ + βˆ†π‘‘ 𝑒 βˆ’πœ πœƒΜ‡ , where Ο„ is can be set to an expected time that the vehicle takes to complete a turn and return to a constant heading. 4 3 Field Test In order to determine the capabilities of the proposed system, 2 Decawave TREK 1000 UWB modules [4] were placed on a moving vehicle, as shown in Fig. 3 alongside a GPS/INS integrated system. The GPS/INS integrated system is used to provide a refer- ence trajectory for the moving vehicle (in an outdoor environment) that includes posi- tion, velocity and attitude angles. Moreover, the GPS/INS integrated system has a sub- decimeter positioning accuracy, which is adequate for this scenario. To mimic the ref- erence vehicle, two other UWB modules are placed on a stationary cart, as shown in Fig. 4(a). The moving vehicle then moved around the reference vehicle in a rectangular trajectory, as shown in Fig. 4(b), with a semi-major axis of approximately 50 m. This trajectory is used for this test because of its feasibility. In other words, for the initial test it is much easier to place the stationary cart in the middle of a parking lot than to place it in the middle of a busy road for the initial test. The limited range of the UWB modules necessitated that the moving car stay within range of the UWB modules by moving in a near circular motion, but it is also limited by the layout of the parking lot which is rectangular. It should be noted that the next step will involve two moving vehicles driving on city streets that will occur in the near future. The ranges are meas- ured between the UWB modules with a rate of 3.57 Hz and saved on the computer on the stationary vehicle, while the reference trajectory of the moving vehicle was meas- ured with the tactical grade carrier-phase GPS/INS integrated system. Fig. 3. Installation of two UWB modules, GPS and INS on a moving vehicle. Fig. 4. : Field test scenario showing (a) the vehicles used and (b) the trajectory of the moving vehicle. 5 4 Preliminary Results The UWB range measurements exhibit various error sources, such as bias errors, and synchronization issues. Consequently, to test the positioning scheme and Kalman filter, simulated UWB ranges were calculated from the GPS/INS trajectory and used as input measurements to the Kalman filter. The simulated UWB modules positions are calcu- lating using the GPS position, the fixed known distance between the UWB modules antennas and the heading angle acquired by the INS system. These UWB positions are then used to calculate the ranges between the UWB modules. These simulated ranges are used to test the Kalman filter and verify the proposed algorithm. The Kalman filter solution (red crosses) and the GPS/INS trajectory (black stars) are shown in Fig.5. These results show small errors in the Kalman filter solution with respect to the GPS/INS reference trajectory. These errors are mainly due to increasing distance from the reference cart, poor geometry, and a relatively slow update frequency (1 Hz) of the GPS/INS system. Fine tuning of the Kalman filter parameters will enhance the position estimation and is, therefore, highly recommended. For example, the covariance matrix of the measurements, R, could be made a function of the distance between the two vehicles in order to mitigate errors due to the increasing distance between the two ve- hicles. In Fig.6 the differences between the x, y and ΞΈ estimates from the Kalman filter solution and the GPS/INS trajectory solution are shown. Note that the x and y errors of the Kalman filter solution, with respect to the GPS/INS solution, are small – the x and coordinates of the Kalman filter solution closely track those of the GPS/INS solution. However, Fig.6 (c) shows that the ΞΈ estimate of the Kalman filter solution does not match that of the GPS/INS solution as closely. Note that the ΞΈ estimate from the Kalman filter solution is underdamped after the moving vehicle has made a U-turn (180˚ change in ΞΈ). The underdamped effect could be mitigated by adjusting the value of Ο„ in equation (3) or by increasing the update rate of the measurements. The UWB modules used have a range measurement update rate of at least 4 Hz. Consequently, when actual UWB ranges are used in the Kalman filter, the underdamped effect seen in Fig.6 (c) should be reduced. Fig. 5. Kalman filter solution for the simulated UWB ranges. 6 Fig. 6. Kalman filter solution error (with respect to the reference trajectory) in (a) x, (b) y and (c) ΞΈ. The results in Fig.6 also verify the expected effect of geometry on position esti- mation. Fig.2 shows large DOP values (hence poor geometry) when crossing the x and y- axes. This is manifested in Fig.6 (b) and (c) by the difference between the Kalman solution y and ΞΈ estimates and the GPS/INS trajectory y and ΞΈ, while the moving vehicle is making a U-turn. The standard deviation of the Kalman filter solution errors, with respect to the GPS/INS solution are given in Table 1. Table 1 Standard deviation of the Kalman filter solution errors with respect to GPS/INS solu- tion. x y ΞΈ π‘₯Μ‡ 𝑦̇ ΞΈΜ‡ (m) (m) (˚) (m/sec) (m/sec) (˚/sec) 0.86 0.53 17 0.7 0.2 11.86 Note that the standard deviation of the x errors is greater than that of the y errors. This is not obvious in Fig.6 due to the different vertical axis scales used in Fig.6 (a) and (b). However, due to the poorer geometry in the y-direction (as opposed to the x- direction), one would expect larger y-direction errors for a trajectory that is rotated 90 degrees from that shown in Fig.4 (b). The error in ΞΈ is an average of the ΞΈ errors while moving in straight line and those while making the U-turns. These errors are expected to decrease with the higher measurement update rate available when using actual UWB ranges. The errors can be further decreased by changing the kinematic model in the Kalman filter. 7 5 Conclusion and Future work The Kalman filter, with simulated UWB ranges, yields a 2-D trajectory and heading solution with the accuracy quantified in Table 1. The errors in the Kalman filter solution can be decreased by fine tuning the Kalman filter parameters as discussed above. The next step is to use the actual field measured UWB ranges rather than the ranges simul1ated from the GPS/INS trajectory. UWB range measurements have higher data rates compared to GPS/INS but are noisier. The higher noise level in UWB ranges is expected to increase the errors in the estimated position and attitude. This will require a better modelling of the measurement covariance matrix, R, in the Kalman filter. Fur- thermore, each pair of UWB modules also has a bias in its range measurement that should be removed either by prior knowledge of the value (preferable solution) or by an adaptive recalibration technique. Finally, UWB measurement are subject to frequent outages. This requires the current Kalman filter to be modified to accept one UWB range measurement at a time. Such a modification will also solve the UWB measure- ment synchronization issue. Once this is completed, tests with two moving vehicles will be conducted and analyzed. 6 References 1. L. Qiang, W. Heng, L. Huican, and Z. Ying, β€œDesign and implementation of multi robot research platform based on UWB,” in 2017 29th Chinese Control And Decision Conference (CCDC), 2017, pp. 7246–7251. 2. P. CorbalΓ‘n and G. P. Picco, β€œConcurrent Ranging in Ultra-wideband Radios: Experimental Evidence, Challenges, and Opportunities,” in EWSN, 2018. 3. T. Tourrette, M. Deremetz, O. Naud, R. Lenain, J. Laneurit, and V. D. Rudnicki, β€œClose Coordination of Mobile Robots Using Radio Beacons: A New Concept Aimed at Smart Spraying in Agriculture,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2018, pp. 7727–7734. 4. β€œDWM 1000 Datasheet.” Decawave.