KINEMATIC CALIBRATION USING LOW-COST LiDAR SYSTEM FOR MAPPING AND AUTONOMOUS DRIVING APPLICATIONS

More recently, mapping sensors for land-based Mobile Mapping Systems (MMSs) have combined cameras and laser scanning measurements defined as Light Detection and Ranging (LiDAR), or laser scanner together. These mobile laser scanning systems (MLS) can be used in dynamic environments and are able of being adopted in traffic-related applications, such as the collection of road network databases, inventory of traffic sign and surface conditions, etc. However, most LiDAR systems are expensive and not easy to access. Moreover, due to the increasing demand of the autonomous driving system, the low-cost LiDAR systems, such as Velodyne or SICK, have become more and more popular these days. These kinds of systems do not provide the total solution. Users need to integrate with Inertial Navigation System/ Global Navigation Satellite System (INS/GNSS) or camera by themselves to meet their requirement. The transformation between LiDAR and INS frames must be carefully computed ahead of conducting direct geo-referencing. To solve these issues, this research proposes the kinematic calibration model for a land-based INS/GNSS/LiDAR system. The calibration model is derived from the direct geo-referencing model and based on the conditioning of target points where lie on planar surfaces. The calibration parameters include the boresight and lever arm as well as the plane coefficients. The proposed calibration model takes into account the plane coefficients, laser and INS/GNSS observations, and boresight and lever arm. The fundamental idea is the constraint where geo-referenced point clouds should lie on the same plane through different directions during the calibration. After the calibration process, there are two evaluations using the calibration parameters to enhance the performance of proposed applications. The first evaluation focuses on the direct geo-referencing. We compared the target planes composed of georeferenced points before and after the calibration. The second evaluation concentrates on positioning improvement after taking aiding measurements from LiDARSimultaneously Localization and Mapping (SLAM) into INS/GNSS. It is worth mentioning that only one or two planes need to be adopted during the calibration process and there is no extra arrangement to set up the calibration field. The only requirement for calibration is the open sky area with the clear plane construction, such as wall or building. Not only has the contribution in MMSs or mapping, this research also considers the self-driving applications which improves the positioning ability and stability.


INTRODUCTION
Light Detection And Ranging (LiDAR) has become more and more popular these days.With the advanced optical technology and hardware design, it plays an important role not only in surveying but also any field which is related to geospatial information.LiDAR is a cost-effective system to collect the geospatial information, allowing the 3D spatial information of objects to be calculated and measured.However, the information acquired from LiDAR is only located in LiDAR coordinate system.Most applications in the really world should combine with other integrated Position and Orientation System (POS) for their products.The common integrated system is Inertial Navigation System/ Global Navigation Satellite System.GNSS provides the position and velocity in global coordinate by using the satellite signal.INS is another navigation system which can achieve the autonomous navigation without any external signal (Titterton et al., 2004).Both navigation systems each has their own advantages and disadvantages.Therefore, INS/GNSS integration has become one of the most popular positioning system now.Researchers introduced the INS/GNSS into the mobile laser scanning systems (MLS) to acquire the continuous EOPs even in GNSS outage situation.The integrated POS overcomes the flaws by only using an integrated system and continuously provides stable navigation information in the GNSS-denied environment.This kind of systems combining with the other mapping sensors can meet the need for rapidly collecting geospatial data by using the direct geo-referencing mathematical model.MLS is contaminated by several error sources, such as GNSS time error, time synchronization between GNSS, INS, and a laser scanner, interpolation of INS/GNSS measurements, system components mounting error, laser range and encoder angle error, etc. (Baltsavias, 1999;Katzenbeisser, 2003;Schenk, 2001).Parts of those errors consist of systematic errors and mounting parameters which influence the expected accuracy and exhibit discrepancies in overlapping areas (Bang, 2010).In order to enhance the overall quality and accuracy of MLS, it is important to calibrate or calculate the relationship between each sensor (boresight and lever arm).
MLS has widely been used in the airborne system which leads to a great increase of strip adjustment method (Kilian et al., 1996;Pfeifer et al., 2005).Habib et al. (2010) introduced the two method to deal with calibration issue (Habib et al., 2010).The first method is simplified method, using the identified discrepancies between parallel overlapping strips to estimate the systematic biases.The other method is quasi-rigorous method to address the non-parallel strips.However, it is not easy to identify the distinct points and lines to conduct calibration like photogrammetric systems, especially for low-cost LiDAR system.As a result, the feature-based calibration is proposed to calculate the boresight and lever arm, minimizing of normal distance between conjugate features (Ravi et al., 2018;Renaudin et al., 2011;Skaloud and Lichti, 2006).These feature-based calibration methods are also applied in different platforms, such as land vehicle, parachute, backpack and balloon, even the ground robot (Glennie, 2012;Glennie et al., 2013;Jung et al., 2015).
Over the past decade, boresight and lever arm calibration have been discussed.However, low-cost LiDAR systems do not provide the total solution for calibration.This research proposes the kinematic calibration model for a land-based INS/GNSS/LiDAR system.The further discussion of the improvement and application will also be shown in this research in terms of mapping and navigation.

METHODOLOGY
In order to integrate the navigation (position and attitude) and point cloud information together, all of the data should be synchronized perfectly.The synchronization process is the most important part in direct geo-referencing.As can be seen from Figure 1, there are four remarkable parts in this research, preprocessing, direct geo-referencing model, kinematic calibration model and evaluation.The first processing is to synchronize the navigation and point cloud information in the same time domain, GPS time.Because the sampling rate of LiDAR measurement is not exactly matched with POS, it is necessary to interpolate the navigation information.Once each LiDAR measurement gets the corresponding position and attitude, the second processing is to conduct the direct geo-referencing.However, we can only get initial results from this processing due to the lack of boresight and lever arm parameters.The third part is the main component in research to carry out the calibration.After calculating these mounting parameters, the final part evaluates the improvement in terms of mapping and navigation.The following sections will describe direct geo-referencing model, kinematic calibration model as well as Simultaneously Localization and Mapping (SLAM) aiding method.
Figure 1.The flow chart of data processing and applications

Land-Based Mapping System
The platform we used is a land vehicle.Although the land-based MLS is popular, the total solution of MLS is quite expensive.In research, we assembled the individual sensor on the top of vehicle and the direct geo-referencing model is shown in Figure 2.
To integrate POS with LiDAR, this research uses the direct georeferencing model illustrated in Figure 2. In direct georeferencing, the laser points in mapping frame (m-frame) are calculated from LiDAR frame to body frame (l-frame and bframe).The geo-referencing formula can be written as: where    is the coordinate vector of i-th laser point in the mframe;    () is the position vector at time  of the INS in the m-frame;    () is the rotation matrix between the navigation system b-frame and the m-frame;    is the differential rotation matrix between the l-frame and the b-frame, determined by calibration;    is the coordinate vector of i-th object point in the l-frame;    is the vector between INS centre and LiDAR, determined by calibration.
In l-frame, the object point    is written as follows: (2) where    ,    , and    are the point coordinates in the -frame;  is the distance between the object and LiDAR centre;  is the vertical angle as indicated by the laser channel, and  is the horizontal angle between the   -axis and object.Also, plane parameters have to satisfy the unit length constraint written as: After the linearization of the calibration model, the final form of normal equation can be derived and minimize the sum of weighted squares of the residuals.The linearized equations of ( 4) and ( 5) are formed as: where  1 and  2 are the partial derivative of equation ( 3) with respect to mounting parameters and plane parameters respectively;  is also the partial derivative of equation ( 3) with respect to observations; ̂1 and ̂2 represent the corrections to adjust the corresponding parameters, ̂ is the residuals from observations; the misclosure vector  is given by ( ⃗⃗⃗⃗⃗⃗⃗ ,  1 which gives the estimated t-th value of the parameters and observations. is also the partial derivative of equation ( 5) with respect to plane parameters;   is the misclosure vector from (5) with estimated values; ̂ is the constraint residuals.
Considering the influences from different error budget, and the correlations within various observations, the weight matrices can be represented as: where  is the weight matrix for observations;   is the weighted constrain for unit length.
In the proposed calibration model, the observations and unknown parameters cannot be separated, the combined (Gauss-Helmert) adjustment model is used.After the iteration of least squares process, the calibration parameters are computed and can be further applied in mapping and navigation applications.

SLAM Aiding Navigation System
In outdoor environment, INS/GNSS system can perform well and give the reliable navigation information for mapping applications.Without GNSS aiding, the error from INS mechanism is degraded with time and the performance heavily relies on the grade of the IMU itself.This research uses the grid-based SLAM (Kohlbrecher et al., 2011) to derive the measurement in EKF integration system to prevent error accumulation.Figure 3 shows the integration flow chart of proposed algorithm.There are two individual systems, INS/GNSS, grid-based SLAM, in proposed algorithm following the LC scheme.Most SLAM algorithms are designed for mobile robot; however, we implement our system on the high speed vehicle to acquire the environment information as soon as possible.The main structure of proposed algorithm is based on reciprocity and mutual benefit.To control the drift from INS mechanism, the SLAM velocity is used as a measurement update in EKF, while INS initial navigation information is also took into the SLAM to improve the robustness and increases the speed of convergence.Therefore, the derived measurements are highly related to the direct geo-referencing, and it is important to acquire the calibration parameters in the integration process.

EXPERIMENT
Land-based mobile mapping systems vehicle is adopted in our case for calibration tasks.For reference, high-grade SPAN-LCI is used as the reference POS.We implement the integrated system by using lower grade INS (C-MIGITS) with low-cost LiDAR (VLP-16) to observe the environment data.Figure 4 shows the platform we used in this research, Table 1 and Table 2 give the specification of two POSs as well as LiDAR sensor.Figure 4 also illustrates the initial relationship between each device.These initial parameters are took into account in the first iteration in least square to make the whole adjustment more efficient and faster.In order to reduce the error sources from navigation, the calibration testing field is determined in the open sky area as shown in Figure 5.The red line is the trajectory during calibration experiment.The blue frame represents the main calibration field which can also be seen in Figure 6.The cyan frame shows the main feature (flat wall) that we extracted.Figure 7 shows the trajectory for navigation test.The initial position is in the open sky area, and we drove into the underground parking lot where the blue frame indicates.After few circlings in parking lot, we went back to the initial point.

RESULTS AND DISCUSSIONS
In this section, the results include two parts, the first part shows the calibration results as well as the final mapping misclosure compared with the reference extracted plane.The second result presents the improvement after adopting SLAM-aiding measurement and calibration parameters.

Calibration and Mapping Results
In this research, we adopt the kinematic calibration model which uses the feature information (surface normal) to implement the calibration.Table 3 to 5 show the calibration results before and after calibration.Table 3 gives the mounting parameters including boresight and lever arm.It is clear that the boresight angles differ from the initial value.If this error source cannot be get rid of, the performance of MLS is not stable and is heavily influenced when we register point cloud from different strips.In terms of mapping results, we evaluate the final point cloud (after calibration) to calculate the misclosure from point to plane.Those points should be located on the plane and correspond to equation (4).It is clear that the standard deviation and average error is quite larger than the after calibration.This result indicates that our calibration model can precisely estimate those unknown values and reduce the misclosure.Figure 8 also illustrates the improvement before and after calibration.

SLAM-aiding After Calibration
As describe in Section 2.3, this research uses the velocity and heading measurement derived from the grid-based SLAM in EKF integration algorithm.Without the measurement update from GNSS solution, the drift error accumulates rapidly over time according to the grade of IMU itself.Even though with the motion constrains such as ZUPT and NHC, the accuracy cannot last for a long time.We proposed the SLAM aiding method that providing the reliable measurements to integration algorithm.Result from Figure 9 illustrates different trajectories which only compared in the underground parking lt.All of results use the same GNSS solution.Red line is the reference, the blue line shows the raw lower grade IMU result.It is clear that without the aiding from GNSS, the blue line deviates the red line.The overall error (RMSE) achieves over 4 meters (Table 6).However, even we take SLAM information into account, the performance gets worse as shown in orange line.Since we do not provide the accurate mounting parameters, the direct geo-referencing point cloud not located in the correct position which leads to the wrong heading and velocity measurements.The RMSE increase to 6.5 meter in east direction.On the contrary, green line shows the final result that uses the estimated mounting parameters.It is worth mentioning that there is a great amount of improvement after we combine the mounting parameters and SLAM together.The overall RMSE is only around 2.5 meters in both east and north direction.

CONCLUSIONS
Recently, MLS has a great potential to be the game changer in the future for autonomous driving.There are more and more researches working on this issue.To improvement the MLS performance with low cost LiDAR sensors, this research proposes the kinematic calibration model to estimate mounting parameters using land-based MLS.
Not only presenting the calibration results, this research also presents the application in both mapping and navigation application.Results show that those calibration parameters can really help the MLS performance.The misclosure declines after adding the mounting parameters in direct geo-reference model.On the other hand, we also propose the SLAM aiding integration in navigation.It is clear that positioning accuracy is enhanced and make our navigation more stable compared with INS-only solution.In the future, this kind of application can be based on the accurate calibration model and apply in the autonomous vehicle industry.

Figure 2 .⃗⃗⃗
Figure 2. Direct geo-referencing model2.2Kinematic Calibration ModelThe kinematic calibration model is proposed based on the direct geo-referencing model and adopts the feature-based method according to the surface normal.The calibration model is derived from the direct geo-referencing model and based on the conditioning of target points where lie on planar surfaces.We modify the calibration model for airborne LiDAR system from(Skaloud and Lichti, 2006) to fit the land-based MLS and make it be able to deal with the low-cost multi-sensor structure.The calibration parameters include the boresight and lever arm as well as the plane coefficients.The proposed calibration model takes into account the plane coefficients, laser and INS/GNSS observations as well as boresight and lever arm.The fundamental idea is the constraint where geo-referenced point clouds should lie on the same plane through different directions during the calibration.The parameters of a plane k is   ⃗⃗⃗⃗ = [ 1 ,  2 ,  3 ,  4 ]  , the  1 ,  2 and  3 are the cosines function of the plane's normal vector as well as  4 is the negative orthogonal distance between the plane and the coordinate system origin.The relationship for plane k and i-th laser point in the m-frame is defined as:( ⃗⃗⃗⃗⃗⃗⃗ ,  1 ⃗⃗⃗ ,  2 ⃗⃗⃗ ) = 0

Figure 4 .
Figure 4. Configuration of the land-based mapping system

Figure 5 .
Figure 5. Calibration testing field Figure 8(b) shows the smaller error than Figure 8(a).The maximum error in Figure 8(a) is over 0.5 meters while the maximum error after calibration in Figure 8(b) is only 0.06 meters.
Misclosure line chart; before (a) and after (b) calibration

Figure 9 .
Figure 9.The trajectories from different integration results

Table 2 .
Performance characteristics of Velodyne LiDAR

Table 4 .
Calibration result, plane parameters

Table 6 .
Evaluation of different integration results