A Design of Cooperative Overtaking Based on Complex Lane Detection and Collision Risk Estimation

Considering a sensor system including a forward facing monocular camera and an onboard V2V sensor, we assume that the ego-motion estimation model is a rigid body motion model and the monocular camera can be modeled by the pinhole camera model and vehicles travel on structured roads, including highways and city roads. Then, using the Zhang method [29], the intrinsic and extrinsic parameters can be easily calibrated in advance. With the calibration matrix, the relative pose transformation matrix between two sensors can be obtained. Hence, we can use a single coordinate system for both the V2V sensor and the forward facing monocular camera. The coordinate systems are defined in the following (see Figure 1 for illustration).

There are two coordinates used in our system, the global V2V based original global coordinate$\left\{{{C}_{V}}\right\}$ and the forward facing monocular camera coordinates system$\left\{{{C}_{C}}\right\}$, which is defined as follows: (1) we paralleled ${{X}_{V}}$-${{O}_{V}}$-${{Y}_{V}}$ plane of $\left\{{{C}_{V}}\right\}$ to the horizontal plane. The ${{Z}_{V}}$-axis points opposite to gravity. The ${{Y}_{V}}$-axis points forward of the vehicle platform, and the ${{X}_{V}}$-axis is determined by the right-hand rule. (2) forward camera coordinate system$\left\{{{C}_{C}}\right\}$ is set originated at the optical center of the camera system. The ${{X}_{C}}$-axis points to the left, the ${{Y}_{C}}$-axis points downward, and the ${{Z}_{C}}$-axis points forward coinciding with the camera principal axis.

The communication mechanism satisfies the standard of IEEE 802.11p protocol, which was used as the information interaction channel. In this way, the effective communication distance is of 200 meters and the information was packaged by the basic safety message (BSM), including sharing information and behavioral collaboration information. In the context of wireless communication based vehicular network, belief-desire-intention (BDI) framework was adopted to construct a hierarchical and cooperation interaction model, which was shown in Figure 2.

In the framework, the belief module was mainly used to realize the environment perception task, especially for road environment perception using the onboard camera and V2V sensors. With the belief module, the system can monitor the nearby environment and vehicle states in real time, including lane detection and relative distance between nearby vehicles. While, the desire module was designed for environment risk evaluation and risk estimation, such as the collision estimation, which is the core function of the vehicle cooperation system. Then, the intention module is used to achieving the task of behavioral cooperation, such as path planning, cruise control, and overtaking collaboration. The detail is shown in Figure 3.

(topskip=0pt, botskip=0pt, midskip=0pt)[width=380pt]Figure/Figure2.pdf BDI based V2V collaboration framework. \Figure(topskip=0pt, botskip=0pt, midskip=0pt)[width=380pt]Figure/Figure3.pdf BDI based V2V collaboration framework.

In order to improve the robustness, two layers of ROI (region of interests) was set to avoid the noise interference in non-road areas and to improve the algorithm’s real-time performance, which was shown in Figure 4. In the high-level layer, static ROI was set in the original image

Where, ImgW and ImgH are the width and height of the image, and $k$, $l$ are proportional adjustment coefficients, ($vpx+\Delta x$ , $vpy+\Delta y$) is the coordinates of the center of the area of interest, $\Delta x$ , $\Delta y$ Is the deviation adjustment coefficient.

When it comes to the low level, dynamic ROI was set on the bird’s-eye view image according to the current vehicle status and driving intention, as shown in Figure 4. As soon as the lane changing behavior is detected from steering signal, the width of dynamic ROI W will be increased and the deviation coefficient a will be decreased, for the purpose of extending the search area of nearby lanes. On the other hand, the height of dynamic ROI H is determined by the current vehicle speed. When the speed is high, the speed coefficient b and the regional height H will be increased dynamically to enlarge the perception area in front of ego vehicle.

When it comes to the low level, dynamic ROI was set on the bird’s-eye view image according to the current vehicle status and driving intention, as shown in Figure 4. For the purpose of extending the search area of nearby lanes, as soon as the lane changing behavior is detected from steering signal, the width of dynamic ROI W will be increased and the deviation coefficient $a$ will be decreased. Meanwhile, the height of dynamic ROI H is determined by the current vehicle speed. When the speed is high, the speed coefficient $b$ and the regional height H will be increased dynamically to enlarge the perception area in front of ego vehicle.

In the above image transformation step, according to the assumption of the pinhole camera model, the transformation matrix from vehicle to the camera can be calculated by the following equation.

Where, $T$ is the transformation matrix from vehicle coordinate to camera coordinate, the $f{{l}_{x}}$and$f{{l}_{y}}$ are the horizontal and vertical shift of camera focus; $o{{c}_{x}}$ and $o{{c}_{y}}$ are the horizontal and vertical position deviation of camera optical center; and ${{c}_{1}}=\cos({{\theta}_{pitch}})$ ${{c}_{2}}=\cos({{\theta}_{yaw}})$ ${{s}_{1}}=\sin({{\theta}_{pitch}})$ ${{s}_{2}}=\sin({{\theta}_{yaw}})$. ${{\theta}_{pitch}}$ and ${{\theta}_{yaw}}$ are defined in Figure 1. Using transformation matrix $T$, the inverse perspective transformation image can be easily obtained.

Hybrid Gaussian anisotropic filter was also used for improving the robustness of the proposed method in the image preprocessing step. Taking Gaussian function as scale function, a hybrid Gaussian anisotropic filter was constructed by using low-pass smooth Gaussian filter and second-order Mexican hat wavelet high-pass filter, as the following equation:

Where,

In the above equations, equation 4 is a Gaussian Lowpass Filter and equation 5 is a second order Mexican hat wavelet high pass filter. $\theta$ is the angle input of filter direction, $\sigma_{u}^{2}$ depends on the width of the front lane, $\sigma_{v}^{2}$ depends on the length of road on the dynamic ROI.

(topskip=5pt, botskip=0pt, midskip=3pt)[width=242pt]Figure/Figure4.pdf Image preprocessing and the setting of two layer ROI.

Commonly, driving roads can be modeled by different curve types, such as straight line model, quadratic curve model and higher order curve model[30]. While, simple line model was widely used in the highway area, but it is hard to fit complex road. The quadratic curve has a unidirectional boundary curvature resulting in poor model adaptability. Higher order curve model can be used to successfully describe the complex road, but it has an unbearable computational complexity. Therefore, in this paper, the Bezier spline curve was adopted to construct the Uncertain Deformation Template (UDT) for complex lane detection, which can automatically choose the complexity of model types.

The Bezier curve is constructed by Bernstein basis function, and the characteristics of the curve can only be determined by the position of its control points [31]. The definition of the Bezier curve is shown in the following equation:

From the definition, we can easily found that the different degree of Bezier equations can be modeled with different n values. In detail, commonly, Linear Bezier curve $P\left(t\right)=\left(1-t\right){{P}_{0}}+t{{P}_{1}}$ can be used to raise the straight road type for highway, and Quadratic Bezier curve $P\left(t\right)={{\left(1-t\right)}^{2}}{{P}_{0}}+2t\left(1-t\right){{P}_{1}}+{{t}^{2}}{{P}_{2}}$, which is constructed by two control points can be used to build the curve road. Cubic Bezier curve $P\left(t\right)={{\left(1-t\right)}^{3}}{{P}_{0}}+3t{{\left(1-t\right)}^{2}}{{P}_{1}}+3{{t}^{2}}\left(1-t\right){{P}_{2}}+{{t}^{3}}{{P}_{3}}$ can be used to put up S-shape path.

Thereafter, using the different degree of Bezier spline curve, a UDT can be constructed, which was shown in equation 7. Then, the land detection question can be transformed to the question of determining the parameters of UDT.

Where, $n$ is the order of the Bezier curve, ${{P}_{n}}$ is the curve control points determined by order, $c$ is the color of lane marking, $s$ is the credibility evaluation coefficient of the current template, and is normalized to the interval of [0 1].

(topskip=0pt, botskip=0pt, midskip=0pt)[width=242pt]Figure/Figure5.pdf Data space setting of possible pixel belonging to lane marking.

In order to solve the hypothesis and testing problem, some of the previous works consider that every point in the image has the opportunity of belonging to the lane marking, so they make all the image points involved in the calculation of posterior probability density function, which consumes a lot of computing resources. [32]. In order to reduce the calculation, the data set of possible lane marking pixels was built, which was shown in Figure 5 with three steps. First, the dynamic ROI area was segmented from the original image for the further process, including hybrid Gaussian anisotropic filtering. Then, we compressed the 2D pixels in the processed image ROI into the image top and sum up the intensity of pixels in each image columns. Further, in the 1D intensity image, a reasonable threshold was set to predetermine the possible pixels belonging to the lane marking. In this way, the data space of possible lane marking pixels can be obtained, as shown in the following equation.

Where, $\Omega$ is the sample space, $M$ is the number of samples, ${{P}_{i}}$ is the possible lane marking pixel. The possible pixel picking method can effectively avoid the negative impact of many road noises, such as light unevenness, road wears, and tears, etc.

In the hypothesis step, the Random Sample Consensus (RANSAC) algorithm was adopted to obtain the parameter hypothesis of current UDT. Thinking of the driving environment, most of roads are straight types, followed by curves, and the complicated road types are few. So, the template parameters are estimated by increasing the template order parameter $n$ from 2 to 4, which means that the parameter estimation process follows the logic from simple to complex. In details, first, N samples are randomly selected from the sample space to form a sample group, and this operation needs to be repeated Q times to obtain Q sample groups. By fitting the sample points in each sample set, Q fitting curves can be obtained. Then, the reliability of the Q fitting results are verified separately. If anyone of the Q fitting curves passes the consistency test, the search stops, and the curves with the highest credibility would be considered as the road fitting curve. Otherwise, N takes N+1 to repeat the above operation. Where the value of N directly determines the number of deformation template levels. It should be pointed out that when N is 2, the sample is fitted with a line; When N is 3, the parabola is used to fit the sample; The least squares fitting sample is used when N is 4.

During the process, the mathematical expression of the Bezier curve can be written in the matrix form as shown below:

It can be abbreviate represented as:

The solution to this equation is

In the credibility testing step, pixel consistency and curve likelihood were used to determine whether the current fitting parameters of UDT can pass the consistency test. Firstly, the pixel consistency examining module was used to verify the grayscale weight of pixel points within the hypothesized curve. If the pixel consistency coefficient $L(S)$ in the following equation was below the set threshold, the current parameters of UDT would lose the verification process.

Where $c$ is the color compensation factor. When the road marking is white, $c=1$, and if it is yellow, $c=1.5$. $S$ is the number of positive pixels passing through the fitting curve, $val$ is the grayscale weight of pixel points. Meanwhile, the curve likelihood coefficient would evaluate the length and bending degree of the fitting curve for the current UDT, which should not deviate from the normal range. The curve likelihood coefficient $Q(S)$ can be calculated using the following equation:

Where, ${{k}_{1}}$is the length coefficient, $l$ is the distance between the furthest two sample points on the curve, $v$ is the image height, ${{k}_{2}}$ is the angle coefficient, $N$ is the number of sample points, ${{\theta}_{i}}$ is the angle between adjacent sample points.

Then, use both of the pixel consistency and curve likelihood coefficients, we can construct a reliability evaluation index $s$ of UDT, which is shown in the following equation:

(topskip=0pt, botskip=0pt, midskip=0pt)[width=400pt]Figure/Figure6.pdf The brief introduction of overtaking procedure.

Here, ${{k}_{L}}$ and ${{k}_{Q}}$ are the weight proportionality coefficient of pixel consistency coefficient $L(S)$ and $Q(S)$ . In this way, we can reckon the parameters of UDT in real time using the RANSAC based hypothesis and testing method.

During the process of overtaking, the uncertainty of collision risk can be represented using the probability density function, as shown in Figure 7. Conflict potential fields can be established respectively, while, the distributions of potential fields are assumed to satisfy multivariate Gaussian distribution with the direction pointing from the reference center to the opposite.

[h](topskip=0pt, botskip=0pt, midskip=0pt)[width=240pt]Figure/Figure7.jpg Potential fields at overtaking stage.

Taking overtaking vehicle (OV) A as an instance, the potential field along the major principal axis and minor principal axis are independent. The probability density function of the potential field of OV A can be denoted by a multivariate Gaussian distribution. As given by the following formula:

Where, $N({{\mathbf{X}}_{A}}\left|{{\mathbf{\mu}}_{A}},{{\mathbf{\Lambda}}_{A}}\right.)$ is the probability density distribution of the conflict potential field, ${{\mathbf{X}}_{A}}$ is the input of two-dimensional variable, ${{\mathbf{\Lambda}}_{A}}$ is covariance matrix, $\left|{{\mathbf{\Lambda}}_{A}}\right|$ is the determinant of ${{\mathbf{\Lambda}}_{A}}$, $D$ is the dimension value of input variables, in this paper $D=2$, ${{\mathbf{\mu}}_{A}}$ is the mean-variance of two-dimensional Gaussian distribution, ${{\Delta}_{A}}$ is the Mahalanobis distance from ${{\mathbf{\mu}}_{A}}$ to ${{\mathbf{X}}_{A}}$ and the calculation is given by:

The distribution of potential fields in the major principal axis and minor principal axis are independent of each other. Then, the covariance matrix of the potential field can be obtained as:

Taking into account the impact of the relative speed, obviously, it would affect the collision risk and make the potential fields more deformable. So, the standard deviation of the covariance matrix ${{\sigma}_{Ax}}$ is constructed by two parts: basic value ${{\sigma}_{x}}$ and compensate value which is constructed by the relative longitudinal velocity ${{\delta}_{v}}$. The standard deviation is given by:

Where, ${{r}_{a}}$ is the gain coefficient of forwarding direction variance. Considering the impact of ${{\sigma}_{Ax}}$ on the standard deviation of the lateral covariance matrix ${{\sigma}_{Ay}}$, the lateral covariance matrix ${{\sigma}_{Ay}}$ can be reckoned by equation (13), while assuming the linear relationship between ${{\sigma}_{Ax}}$ and ${{\sigma}_{Ay}}$ .

Where, ${{r}_{c}}$ is the gain coefficient of lateral direction variance. ${{\bar{\sigma}}_{y}}$ is the saturation value for limiting ${{\sigma}_{Ay}}$ in a reasonable range. Formula (18) and formula (19) obviously implies that the relatively high velocity will not only increase the possibility of the collision before exceeding the overtaken vehicle but also decrease the possibility of collision after the exceeding. Similarly, the conflict potential field of overtaken vehicles can also be constructed in accordance with the method mentioned above.

Based on the constructed conflict potential fields at the surpassing stage, the estimation of conflict probability can be calculated by integrating the probability density of potential fields over the conflict area, which is shown in Figure 6.

In order to simplify the calculation, the two conflict potential fields of overtaking vehicle A and leading vehicle B can be uniformed to the world coordinate system. The transformation matrix from the vehicle coordinate system to the world coordinate system is given by:

Where, $R$ is the transformation matrix. $\theta$ is the azimuth angle between the vehicle coordinate and world coordinate. Therefore, the covariance matrix can be transformed from the vehicle coordinate to the world coordinate, as given by:

Considering the irrelevant of the distribution of two potential fields, the joint error covariance matrix can be obtained according to the synthesis rules of multivariate Gaussian distribution, as follows:

In order to facilitate the integration of conflict area, a conflict coordinate is established by taking the center of the potential field of leading vehicle B as the new origin, the major principal axis of conflict ellipses as new abscissa and the minor principal axis as new ordinate. Then, the center offset of the potential field of overtaking vehicle A can be given by:

Where,${{x}_{r}}$is the center offset of overtaking vehicle A along the abscissa direction of conflict coordinate, ${{y}_{r}}$ is the center offset in the conflict coordinate. According to the characteristic of the multidimensional normal distribution, a linear combination of normal distribution still meets the normal distribution. Hence, the joint probability density function of conflict can be given by:

Then, the probability of collision at time t can be obtained through the integration of conflict probability density over the conflict area:

Where, ${{\text{S}}_{\text{CP}}}$is the estimation of conflict probability. $f(x,y)$is the conflict probability density function. ${{S}_{c}}$ is the conflict area.

[h](topskip=0pt, botskip=0pt, midskip=0pt)[width=240pt]Figure/Figure9.pdf The recognition process of the road images.

In this testing part, 5932 frames of typical road pictures were tested for the proposed lane detection module and the results were statistically analyzed. The results show that the average recognition error rate of the algorithm is less than 6%, the average detection time of each frame lane line is 50ms meeting the real-time requirements

Figure 9 graphically demonstrated the recognition process of the road images. Where, figure 9(a) is the raw RGB image taking from onboard forward facing camera, and the time is in the evening with a good light condition. Figure 9(b) is the result of the image inverse perspective transformation. Fig.9 (c) is the RANSAC based parameters solving procedure of UDT under hypothesis and testing problem. where the red line is the curve fitted by the currently randomly selected deformation template. At this time, N=2, the current template matching credibility is 0.16, and the blue line is the best historical matching result in this search process, and the template matching degree is 1.14. Figure 9(d) is the final lane fitting model, and it has been transformed from the inverse perspective image to the original image.

Case 1: Straight Road with vehicles moving at different relative distances \Figure[h](topskip=0pt, botskip=0pt, midskip=0pt)[width=350pt]Figure/Figure12.pdf Collision probability estimation with different relative distance. \Figure[h](topskip=0pt, botskip=0pt, midskip=0pt)[width=350pt]Figure/Figure13.pdf Collision probability estimation with different relative speed.

In this case, a test environment of rural straight roads with medium concentration of fog was established. The visibility was only 120 m. The ego vehicle traveled with a slow speed and the maximum speed was just 35 km/h. Three straight lanes and four vehicles were set, including vehicle A (ego vehicle), vehicle B (forward leading vehicle), vehicle D (oncoming vehicle) and vehicle E (same direction vehicle). All of them traveled at constant speeds of 25, 18, 28 and 22 km/h respectively. Meanwhile, some slippery ice was added onto the surface of the road to decrease the adhesion coefficient of pavement. The visual-driver started up the overtaking action with different relative distances corresponding to the leading vehicles, which included 10m, 20m, and 25m. The aim of this test was to verify the capability of the proposed method to get correct and stabilized estimation of collision probability during the overtaking process. The test result was shown in Figure 12.

In the test, the ego vehicle started up the overtaking action at t$\sim$2s (before reminding signal), t$\sim$3s (after reminding signal and before warning signal) and t$\sim$5s (after warning signal) respectively, and returned back to the original lane after surpassing the leading vehicle. The simulation result shows that it would be very safe if the driver started the overtaking action before reminding signal, while the occupation time in the rapid lane would increase. On the other hand, as shown in Figure 12, if the driver started the overtaking action after warning signal, the collision risk would increase rapidly to 0.75, that portended a possible traffic accident would happen. If the driver complied with the instruction of the proposed method and start up the overtaking action at the right time, the collision risk would keep below 0.3 to guarantee the safety of the whole overtaking process.

Case 2: Curve Road with other vehicles moving at variable relative high speed on a sunshine day

In this case, a curved road was established with a minimum curvature radius of 150m. Meantime, the weather was changed to a good sunshine day and without lateral wind. In this weather and road condition, the influence of collision risk under variable relative speeds was tested to demonstrate the performance of the proposed method. According to the traffic rules, we set the maximum speed of the vehicles to 100 km/h and the minimum speed of 60 km/h, with the acceleration/deceleration of 3.8 m/s2. The aim of this simulation is to verify the performance of the proposed method under variable conditions of different relative speeds, which includes 7.2 km/h (low relative speed), 14.4 km/h (medium relative speed) and 21.6 km/h (high relative speed). The testing result was shown in Figure 13.

In the test, the ego vehicle cyclically detected the motion states of nearby vehicles in every 100ms, which was the same as the case 2, except that the communication distance was extended to 200m for the highway use. As shown in Figure 13, the ego vehicle started up the overtaking action at t$\sim$5s with three different relative speeds and it returned back to the original lane safely. The simulation result shows that, if the vehicle start the overtaking action with a low relative speed or a medium relative speed, collision risk would stay below the safety threshold of 0.3 to make sure the safety of the overtaking procedure. However, if the driver start up the overtaking action with a high relative speed of 21.6km/h, the collision risk would increase rapidly to 0.45, which is above the safety threshold. During the whole overtaking procedure, the proposed method can calculate the estimation of collision risk timely and correctly.