# A Road Environment Prediction System for Intelligent Vehicle

Authors: Chao Ma; Zhao Sun; Shanshan Pei; Chao Liu; Feng Cui
Journal: Wireless Communications and Mobile Computing
 (2021)

---

## Abstract

The road environment prediction is an essential task for intelligent vehicle. In this study, we provide a flexible system that focuses on freespace detection and road environment prediction to host vehicle. The hardware of this system includes two parts: a binocular camera and a low-power mobile platform, which is flexible and portable for a variety of intelligent vehicle. We put forward a multiscale stereo matching algorithm to reduce the computing cost of the hardware unit. Based on disparity space and points cloud, we propose a weighted probability grid map to detect freespace region and a state model to describe the road environment. The experiments show that the proposed system is accurate and robust, which indicates that this technique is fully competent for road environment prediction for intelligent vehicle.


---

## Body

## 1. Introduction

The road environment prediction is an essential task for intelligent vehicle and robotic applications. As the basis of path planning [1], motion strategy [2], and collision avoidance [3], the road environment prediction focuses on whether the host vehicle or robot will pass through without collision [4].In recent years, light detection and ranging (LiDAR) [5], cameras [6], and multisensor fusion technique are adopted to perceive the road environment. In literatures, LiDAR and cameras are devoted to the odometry method, in which the relative motion is estimated by matched points in continuous frames. The multisensors fusion technique includes visual sensors, inertial sensors, and precision map. The inertial sensors are used to estimate the relative pose based on laws of classical mechanics. Those methods are considered as passive sensing that has only detected road information when a vehicle drives into it. The precision map method relies on a global positioning system (GPS) signal that can predict the road path; however, it is not robust in tunnel or underground parking.In this paper, we propose a flexible and robust road environment perception system. This system consists of a binocular camera and a low-power mobile platform. Because that software functions run in the terminal device rather than transmitting a large amount of data to the cloud [7], the proposed system is suitable for the Internet of Things (IoT) [8]. For the sake of real-time system in the low-power platform, we propose a multiscale stereo matching algorithm, weight probability grid map, and state model to describe road environment.The traditional stereo matching algorithms, such as [9, 10], provide dense disparity map by predefined matching measures and semiglobal optimization, which consumes a lot of computing sources and takes a large of electric power. In this study, we propose a new stereo matching framework to adapt to the field-programmable gate array (FPGA) implement environment. The focus of the improvement is to reduce the computational cost, maintain the matching accuracy, and effectively perceive the target in different scales.The traditional road environment perception methods, such as [11, 12], focus on the detection of freespace, which estimates the boundary of freespace region by geometric constraints in u-disparity or v-disparity space. The traditional position estimate methods, such as [13, 14], put forward motion estimation method based on the Bundle Adjustment (BA) algorithm with feature points. It is necessary to select the points which are motionless relative to world coordinate system from the feature points. In this paper, we propose a weighted probability grid map to freespace detection. It is a robust and flexible strategy because we avoid the motion estimate that is different to match the feature point on static objects. In addition, we define a state model to describe the road environment, which describes the road status in the front of the host vehicle.The main contributions of this work are summarized as follows.(i)
A multiscale stereo matching algorithm is presented to reduce the computing cost and improve the accuracy(ii)
Based on the disparity map, a weight probability grid map is proposed to detection the freespace region(iii)
A state model is proposed to describe the road environment in the front of the host vehicle(iv)
An efficient deployment programme is put forward to process our system at the low-power mobile platform in realtime


## 2. Related Work

### 2.1. Stereo Matching

Humenberger et al. [15] have presented a large spare Census mask for the fast stereo matching algorithm. In this literature, authors compare the large sparse masks and small dense masks, which the experiment shows that the former has better performance.Yang [16] has presented a new matching cost aggregation method to preserve depth edges and reduce the computing complexity. In this literature, the author suggests an adaptive cost aggregation technique based on pixel similarity. A bilateral filter is used to compute the aggregation cost with the spatial similarity and the range (intensity/color) similarity, respectively.Zhang et al. [17] have presented a cross-scale cost aggregation framework to allow multiscale interaction in cost aggregation. In this literature, the authors consider the cost aggregation as a weighted least square optimization problem. Therefore, multiscale cost aggregation methods come from different similarity kernels in the optimization objective.Mao et al. [18] have presented a robust deep learning solution for semidense stereo matching. In this literature, two CNN models are utilized for computing stereo matching cost and performing confidence-based filtering. Due to that the global information is provided, the model is suitable for dealing with the challenging cases, such as lighting changes and lack of textures.However, the above algorithms cost a large number of computing resources, which is a risk to process in realtime. In this paper, we propose a multiscale stereo matching fusion algorithm. This algorithm is designed to reduce the computing cost of FPGA and process stereo matching in realtime.


### 2.2. Road Environment Perception

Qu et al. [19] have presented an improved V-disparity space algorithm to generate the confidence map to estimate the freespace. In this literature, the authors discuss a sub-V-disparity space method to avoid the assumption that the road is locally planar and the variance in the latitudinal slope is small. Based on the v-disparity space, road confidence map and obstacle confidence map are calculated for freespace estimation by dynamic programming.Deilamsalehy and Havens [20] have presented a multisensor fusion method to improve the accuracy of the estimation and to compensate for individual sensor deficiencies. In this literature, the three sensors, inertial measurement unit (IMU), camera, and LiDAR, form a rigid body, where position estimates from the camera and LiDAR are used in the EKF as a measurement to correct the IMU pose estimation.Xiao et al. [21] have presented a Bayesian framework and conditional random field to fuse the multiple features that includes 2D image and 3D point cloud geometric information. Besides, a Gaussian process regression is employed to enhance performance. In this literature, the results are outstanding compared to some relevant LiDAR-based methods when a conditional random field with color and geometry constraints is applied to make the result more robust.Zheng et al. [22] have presented a low-cost GPS-assisted LiDAR state estimation system for autonomous vehicle. In this literature, a LiDAR is employed to obtain highly precise 3D geometry data and an IMU is used to correct point cloud misalignment. A low-cost GPS is used to refine the estimated LiDAR inertial odometry.Cong et al. [23] have presented LiDAR-based simultaneous localization and mapping (SLAM) system embedding dynamic objects removal module to improve the pose estimation. In this literature, the authors remove the point clouds of moving objects to relieve their influence on the odometry, so that the precision relative pose is estimated.In this paper, we propose a weighted probability grid map to freespace detection and a state model to describe the road environment. By describing of road state in front of the host vehicle, we predict the future road environment information and vehicle’s estimation direction.


## 2.1. Stereo Matching

Humenberger et al. [15] have presented a large spare Census mask for the fast stereo matching algorithm. In this literature, authors compare the large sparse masks and small dense masks, which the experiment shows that the former has better performance.Yang [16] has presented a new matching cost aggregation method to preserve depth edges and reduce the computing complexity. In this literature, the author suggests an adaptive cost aggregation technique based on pixel similarity. A bilateral filter is used to compute the aggregation cost with the spatial similarity and the range (intensity/color) similarity, respectively.Zhang et al. [17] have presented a cross-scale cost aggregation framework to allow multiscale interaction in cost aggregation. In this literature, the authors consider the cost aggregation as a weighted least square optimization problem. Therefore, multiscale cost aggregation methods come from different similarity kernels in the optimization objective.Mao et al. [18] have presented a robust deep learning solution for semidense stereo matching. In this literature, two CNN models are utilized for computing stereo matching cost and performing confidence-based filtering. Due to that the global information is provided, the model is suitable for dealing with the challenging cases, such as lighting changes and lack of textures.However, the above algorithms cost a large number of computing resources, which is a risk to process in realtime. In this paper, we propose a multiscale stereo matching fusion algorithm. This algorithm is designed to reduce the computing cost of FPGA and process stereo matching in realtime.


## 2.2. Road Environment Perception

Qu et al. [19] have presented an improved V-disparity space algorithm to generate the confidence map to estimate the freespace. In this literature, the authors discuss a sub-V-disparity space method to avoid the assumption that the road is locally planar and the variance in the latitudinal slope is small. Based on the v-disparity space, road confidence map and obstacle confidence map are calculated for freespace estimation by dynamic programming.Deilamsalehy and Havens [20] have presented a multisensor fusion method to improve the accuracy of the estimation and to compensate for individual sensor deficiencies. In this literature, the three sensors, inertial measurement unit (IMU), camera, and LiDAR, form a rigid body, where position estimates from the camera and LiDAR are used in the EKF as a measurement to correct the IMU pose estimation.Xiao et al. [21] have presented a Bayesian framework and conditional random field to fuse the multiple features that includes 2D image and 3D point cloud geometric information. Besides, a Gaussian process regression is employed to enhance performance. In this literature, the results are outstanding compared to some relevant LiDAR-based methods when a conditional random field with color and geometry constraints is applied to make the result more robust.Zheng et al. [22] have presented a low-cost GPS-assisted LiDAR state estimation system for autonomous vehicle. In this literature, a LiDAR is employed to obtain highly precise 3D geometry data and an IMU is used to correct point cloud misalignment. A low-cost GPS is used to refine the estimated LiDAR inertial odometry.Cong et al. [23] have presented LiDAR-based simultaneous localization and mapping (SLAM) system embedding dynamic objects removal module to improve the pose estimation. In this literature, the authors remove the point clouds of moving objects to relieve their influence on the odometry, so that the precision relative pose is estimated.In this paper, we propose a weighted probability grid map to freespace detection and a state model to describe the road environment. By describing of road state in front of the host vehicle, we predict the future road environment information and vehicle’s estimation direction.


## 3. Proposed Method

### 3.1. Multiscale Stereo Matching Algorithm

In binocular stereo matching algorithm, feature similarity is an unsupervised matching method, such as census feature [15], structural similarity (SSIM) [10], and convolutional neural network (CNN) model [24]. In this study, our binocular system is set to synchronous exposure and same image signal processing (ISP) parameters. In order to tolerate the small grey difference between the two sensors in the imaging process, the SSIM method is adopted for stereo matching.The SSIM method [25, 26] is proposed to perceive image quality, because this index considers the luminance (lφ,ϕ), contrast (cφ,ϕ), and structure (sφ,ϕ) as shown in Equation. (1), where φ is the index of row and ϕ is the index of column.(1)lφ,ϕ=2μφμϕ+C1μφ2+μϕ2+C1cφ,ϕ=2σφσϕ+C2σφ2σϕ2+C2sφ,ϕ=σφσϕ+C3σφϕ+C3,where μ, σ2, and σ denote the mean, variance, and covariance, respectively. Approximately, μ and σ2 are viewed as the estimated luminance and contrast, respectively, and σ measures the tendency. C1, C2, and C3 are constants. The general form of the SSIM index between u (disparity) and p (pixel coordinate in image) is defined as Equation (2):(2)SSIMp,u=lαφ,ϕcβφ,ϕsγφ,ϕ.In engineering practice,α, β, and γ are set as 1 and C1=0.01, C2=0.03, and C3=C2/2. Therefore, the matching similarity value is simplified as Equation (3):(3)SSIMp,u=2μϕμψ+C12σϕψ+C2μϕ2+μψ2+C1σϕ2+σψ2+C2,which satisfies the following conditions to be a matching cost:(i)
Boundedness.0<SSIM≤1, which indicates that the more similar the patch, the higher the value(ii)
Unique Extremum.SSIM=1 if and only if the two patches are the sameIn order to adapt to the high dynamic matching cost aggregation method, we define the matching cost based on SSIM as Equation (4):(4)costp,u=L1−SSIMp,u,where L=255 is the dynamic range of the pixel values for 8 bits/pixel greyscale images. Therefore, the maximum aggregation cost is less than 512 without penalty item by the semiglobal matching method. The purpose of transforming matching cost into integer type is to be more suitable for FPGA implementation. In this study, we limited the single matching cost to 255, which is represented by 8 bits. Therefore, the maximum matching cost is within a reasonable range in the semiglobal cost aggregation process, in which the maximum cost is limited to 10 binary digits in this system.We set the penalty item toP1=30 and P2=100 in this system, which indicates an assumption that the disparity of adjacent pixels is smooth. In fact, the depth change is discontinuous in automatic driving, which leads to that the disparity fluctuates greatly and the precision decreases in the depth discontinue pixel. In order to improve this problem, we propose to build a guide map to show where depth changes are possible [27]. Compared with the literature [27], we assume that the depth charge always occurs at the edge of objects. In addition, the structure is obvious at the edge region, which makes it easier to get the real and accurate disparity.First, we provide a one-dimensional Gaussian edge feature map (setting toμ=0 and σ=3 in the system) to indicate where the obvious edge is and where is the stereo matching result is more credible. Then, the first derivative in the same direction is calculated to obtain the gradient response. Next, the same process is implemented in the tangent direction. Finally, the sum of the square root of two gradient responses is considered as a feature map, as shown in Figure 1.Figure 1
An example of the feature map in stereo matching. Left: grey image. Right: feature map.
(a)(b)Based on the feature map, the larger the response value is, the stronger the edge characteristics are, and the greater the possibility of depth change. According to the above method, we infer that the maximum aggregation cost between adjacent pixels is 610 (255+100+255) in one path. The sum cost in eight paths is less than 4880 (610×8), which is represented by 13 binary digits. Therefore, the maximum memory cost of a single pixel is limited within 16 binary digits (2 Bytes) by eight aggregation paths. To reduce the computing cost, we propose a stereo matching optimization method, which focuses on reducing the disparity search range. We construct the multiscale images by the image pyramid method. In this study, we propose three layers pyramid method. As shown in Figure 2, the large-scale layer images have the original resolution of 1280×720. The middle-scale layer images are downsampled from the large-scale layer images, and the small-scale layer images are downsampled from the middle-scale layer images.Figure 2
Multiscale images pyramid and disparity map. Top: the large-scale layer. Middle: the middle-scale layer. Bottom: the small-scale layer.The structure features are obvious in the large-scale layer. Correspondingly, the remote details are completely preserved in the small-scale layer. At first, we propose to set the disparity search range as 16 pixels in the small-scale layer, which is equal to the search range of 64 pixels in the large-scale layer. Based on that, we calculate the small-scale disparity map by the SSIM algorithm in the small-scale layer. Then, we initialize the middle-scale disparity map by upsampling from the small-scale disparity map by the linear interpolation method. Because the small-scale disparity map has the same size as the small-scale layer,320×180, the size of initialized middle-scale disparity map is the same as that of the middle-scale layer, 640×360. Next, we refine the middle-scale disparity map. Based on the initialized disparity map, we set the same search range as 16 pixels. However, the beginning of the search depends on the initialized disparity, in which the search range with 16 pixels is a symmetric interval centered on the initialized disparity. Finally, we repeat the above processes to obtain the large-scale disparity map.At the calculation process of the multiscale stereo matching algorithm, the feature map is only adopted in the large-scale layer. The proposed stereo matching method is suitable for the FPGA to process:(i)
There are many reusable operation modules because the rules and parameters are the same(ii)
There is no need to cache all image data in memory at the same time, so the calculation process can be consistent with the data transmission process(iii)
The algorithm is designed to process numerical multiplication and addition by a large number of fixed-point data


### 3.2. Road Prediction

In practice, we focus on two issues: (1) where is the obstacle and (2) what is the trend of road. We propose the grid projection method to predict the trend of road. When obstacles occupy the road, the trend prediction will be hindered, but the freespace region should be correctly described. In this study, our method consists of three stages: grid projection, boundary search, and shape detection.


#### 3.2.1. Grid Projection

Based on the disparity map, we calculate the 3D point cloud coordinate by Equation (5):(5)x=bu−u0disp,y=bv−v0disp,z=bfeqdispwhere x,y,z is the 3D coordinate in the world coordinate system (wcs), u,v is pixel coordinate in image coordinate system (ics), u0,v0 is the camera central point, disp is the disparity, b is the baseline of binocular camera, and feq is equivalent focal length. The disparity map and 3D pints cloud are shown in Figure 3.Figure 3
Disparity and 3D point cloud. (a) The left image. (b) Disparity map. (c) 3D point cloud.
(a)(b)(c)Inspired by pseudolidar data from visual [28], we build a projection space from bird eye view (BEV) in wcs. At BEV, the x-axis, which is parallel to the baseline of the binocular camera, represents the horizontal direction, and the y-axis, which is parallel to the optical axis of the camera and is positive horizontally to the front, represents the depth direction. In the projection space, the fixed size grid is divided as the basic projection unit, which the grid is a rectangle by setting 10cm×50cm in x-direction and z-direction. First, we divide point cloud data by grid mesh and drop out the outliers that points do not belong to the detection space Π=x,y,z∣y∈−0.5,2.5,z∈0,60. This constraint represents a priori assumption that the maximum height of the obstacles on road will not exceed 3 m, and the farthest observation distance is 60 m. Then, we count the number of points in each grid and set this value as the feature value of this grid. In this way, grids with lots of points cloud data have a larger accumulated number. As shown in Figure 4(c), we use color to represent the accumulated number, where the dark blue indicates zero and the brighter the color, the greater the accumulated number.Figure 4
The process of probability grid map. (a) Left grey image. (b) Disparity map. (c) Grid projection. (d) Probability grid map.
(a)(b)(c)(d)


#### 3.2.2. Boundary Search

Inspired by the stochastic occupancy grids method [29], grids with larger feature values represent that there is a greater possibility of an obstacle. On the contrary, grids with smaller feature values are more likely to represent the road region. In addition, grids with zero feature value indicate that it is occluded and cannot be observed.By transferring the ics (disparity map) to vcs (grid projection), a unified physical scale is helpful to build a more robust mathematical model to solve the problem. In detection spaceΠ, we consider the ground as a plane parallel to the xoz and obstacles as a plane vertical to the horizontal plane. As shown in Figure 5, obstacles can be approximated by three plane models in the detection space Π: perpendicular to the optical axis (marked ②), parallel to the optical axis and perpendicular to the horizontal plane (marked ③), and perpendicular to the horizontal plane and intersecting with the optical axis (marked ④). In addition, the road plane is another plane (marked ①) that parallel to the horizontal plane or intersecting at a smaller angle.Figure 5
Road plane and three obstacle plane models in wcs.These plane models describe obstacles in different states. The plane ② represents obstacles perpendicular to the optical axis on the driving route, such as a vehicle in the same lane, which only the vehicle rear is visible. The plane ③ represents obstacles parallel to the optical axis adjacent to the driving route, such as fences and barriers, which only the side is visible. The plane ④ represents obstacles that intersect the optical axis on the driving route, such as guardrails and walls in the curve road. In addition, plane ② and plane ③ are combined to describe adjacent discontinuous obstacles, such as cut-in vehicles, the vehicle side and rear are visible. Based on the above model, obstacles generate a large number of 3D point cloud data on its plane. Therefore, there are many accumulated points in the projection area of the obstacle plane in the grid projection.Suppose that the basic detection unit on planes is a square, which represents that the obstacle is divided into many square units. In practice, the side length of the unit is consistent with the gird projection size inx-direction. At different distances, the number of point clouds generated by the detection unit conforms to the Equation (6):(6)Nunit=a×fz×μ2,where Nunit represents the number of 3D points in unit, a is the side length of unit (a=10cm in this study), f is the focal length of lens, μ is the pixel size of sensor, and z is distance in wcs (0m<z≤60m in this study).In the detection spaceΠ, the height constraint is less than hm. Therefore, the maximum of 3D points is h×Nunit/a in a grid. Furthermore, we construct a probabilistic occupancy grid, where we divide the accumulated number by the maximum. As shown in Figure 4(d), we use the color to indicate the probability. The dark blue represents low probability and red represents high probability. In polar coordinates, we transform the probability grid map (PGM) into a graph model, where the nodes represent elements in PGM with fixed angle resolution and radius resolution. The value of the node is the probability of the grid, the column index is angle, and the row index is radius. Therefore, the road boundary search problem is transformed into a dynamic programming problem to find the path with maximum probability. We consider the graph as a directed acyclic graph (DAG) that the direction is from left to right or opposite. In theory, the two directions have the same path. In this study, we propose a semiglobal optimization method to solve the dynamic programming problem.As shown in Figure6, the node represents the probability of the grid. The horizontal axis is the angle of the field of view, where the resolution is 1 degree. The vertical axis is the radius, where the resolution is the same as that of the grid in z direction. The FOV and grid projection space Π are not completely coincident, so the number of nodes in each column is not consistent. We propose a semiglobal energy function in Equation (7).(7)Ep∣p∈N=Pp−1θ,R+Tp−1,p+Ppθ,R,where N is the best path that the summary of probability is maximum. Ppθ,R represents the probability of pθ,R, and this point belongs to the best path p∈N. Tp−1,p represents the penalty term between adjacent column nodes, in which the p−1 is previous nodes and p is current nodes. The penalty term is defined as Equation (8).(8)Tp−1,p=0Rp−1−Rp=0T1Rp−1−Rp≤1T2otherwise.Figure 6
Directed acyclic graph with dynamic programming method. The black arrow indicates the best path.The penalty term represents an assumption that the path between adjacent columns is smooth in DAG.Rp−1is the radius index of node in the previous column and Rp is in current column. Obviously, the penalty term is constrained to −1≤T2<T1<0. Finally, the optimization function is defined as Equation (9).(9)Np=argmin∑Ep.The purpose of parameter setting is that the row coordinate changes of adjacent column nodes should be smooth. In this study, we set theT1=−0.1 and T2=−0.3. As a result, we obtain an optimal path in DAG, where the sum of probability exists maximum. We convert the node index into the coordinate of PGM, where grids are considered as boundary points of freespace in wcs. In addition, the boundary points in wcs are back projected into ics through Equation (6). As shown in Figure 7, the red line in the probability grad map is the optimal path and the yellow line is the freespace boundary in ics.Figure 7
Freespace boundary. (a) Optimal path by DP. (b) Boundary in ics.
(a)(b)In practice, we employ the multilayer grid projection to overcome that case where the near obstacle is lower than the far obstacles. Three layers are divided by height with−0.5m∼0.5m (subspace Π1), 0.5m∼1.5m (subspace Π2), and 1.5m∼2.5m (subspace Π3). As shown in Figure 8(a), the height of the near barrier is lower than the far greenbelt, so that the probability of barrier is smaller than the probability of greenbelt in Figure 8(c). We propose a weight energy function as Equation (10).(10)Ep∣p∈N=ωp−1Pp−1θ,R+Tp−1,p+ωpPpθ,R,where ωp is the weight of node p that the coordinate of this point is θ,R and 0<ω≤1. The p−1 indicates the previous node. The capital P indicates the penalty item. The ω in Π1 is greater than ω in Π2, and ω in Π2 is greater than ω in Π3. In this study, we set weight as ω=1 in Π1, ω=0.6 in Π2, andω=0.3 in Π3. As a result, the Figure 8(e) shows the weight probability grid map (WPGM) that the best path corresponds to our expectations, where the high brightness represents a large probability.Figure 8
Multilayers grid projection. (a) Left image. (b) Disparity map. (c) PGM inΠ. (d1) PGM in Π1. (d2) PGM in Π2. (d3) PGM in Π3. (d) WPGM in Π.
(a)(b)(c)(d)(e)


#### 3.2.3. Shape Detection

Based on the three obstacle models, we detect geometric constraints on the multilayer grid projection. In practice, we propose to identify the geometry feature by prior. The plane ②, in Figure5, is a segment parallel to the x-axis in the WPGM, the plane ③ is another segment parallel to the z-axis, and the plane ④ is an oblique line or curve. Therefore, we detect these geometry features on the boundary to identify if there any obstacle exists and road environment, for example, vehicle is going to drive into straight road or curved road.To reduce the noise, we discard the grid whose probability is less than 0.1. The result is shown in Figure9. The left column is the grey images, the middle column is the feature points in WPGM, and the right column is the result of shape detection.Figure 9
The process of shape detection. (a) In curved road. (b) In straight road. (c) With obstacles.
(a)(b)(c)In the middle column of Figure9, the red points are feature points that make up the shape feature map (SFM). These feature points are used to progressive probabilistic Hough transform, in which the coordinates use pixel dimensions in the SFM. For example, in the right column of Figure 9(a), the different color line represents different slopes: the slope of line (1) is 0.3862, the slope of line (2) is 0.5062, and the slope of line (3) is 0.0082. We consider the line (3) is modelled as the plane ②, while the line (1) and line (3) are modelled as plane ④. However, the plane ② is usually considered to be an independent obstacle, such as a vehicle. Because line (2) and line (3) are connected, they are regarded as independent obstacle in the wcs. In addition, plane ④ is usually considered continue obstacle, such as guardrails and walls in the curve road. The line (1) is regarded as a guardrails or walls in the curve road.We describe the shape feature by different plane models so that the complex road environment is classified into finite-state models. Table1 describes Figure 9. The (a) scene is described as 1 curve road model and 1 obstacle model, the (b) scene is described as 6 straight road models, and the (c) scene is described as 4 obstacle models. Therefore, the (a) scene is predicted to be going to drive into curve road and there is an obstacle. The (b) scene is predicted to drive on a straight road without obstacle. The (c) scene is predicted to that there are many obstacles in front of the vehicle and other road information is invalid.Table 1
Describe the road environment by state model.
Curve roadStraight roadObstacles(a)101(b)060(c)004


## 3.1. Multiscale Stereo Matching Algorithm

In binocular stereo matching algorithm, feature similarity is an unsupervised matching method, such as census feature [15], structural similarity (SSIM) [10], and convolutional neural network (CNN) model [24]. In this study, our binocular system is set to synchronous exposure and same image signal processing (ISP) parameters. In order to tolerate the small grey difference between the two sensors in the imaging process, the SSIM method is adopted for stereo matching.The SSIM method [25, 26] is proposed to perceive image quality, because this index considers the luminance (lφ,ϕ), contrast (cφ,ϕ), and structure (sφ,ϕ) as shown in Equation. (1), where φ is the index of row and ϕ is the index of column.(1)lφ,ϕ=2μφμϕ+C1μφ2+μϕ2+C1cφ,ϕ=2σφσϕ+C2σφ2σϕ2+C2sφ,ϕ=σφσϕ+C3σφϕ+C3,where μ, σ2, and σ denote the mean, variance, and covariance, respectively. Approximately, μ and σ2 are viewed as the estimated luminance and contrast, respectively, and σ measures the tendency. C1, C2, and C3 are constants. The general form of the SSIM index between u (disparity) and p (pixel coordinate in image) is defined as Equation (2):(2)SSIMp,u=lαφ,ϕcβφ,ϕsγφ,ϕ.In engineering practice,α, β, and γ are set as 1 and C1=0.01, C2=0.03, and C3=C2/2. Therefore, the matching similarity value is simplified as Equation (3):(3)SSIMp,u=2μϕμψ+C12σϕψ+C2μϕ2+μψ2+C1σϕ2+σψ2+C2,which satisfies the following conditions to be a matching cost:(i)
Boundedness.0<SSIM≤1, which indicates that the more similar the patch, the higher the value(ii)
Unique Extremum.SSIM=1 if and only if the two patches are the sameIn order to adapt to the high dynamic matching cost aggregation method, we define the matching cost based on SSIM as Equation (4):(4)costp,u=L1−SSIMp,u,where L=255 is the dynamic range of the pixel values for 8 bits/pixel greyscale images. Therefore, the maximum aggregation cost is less than 512 without penalty item by the semiglobal matching method. The purpose of transforming matching cost into integer type is to be more suitable for FPGA implementation. In this study, we limited the single matching cost to 255, which is represented by 8 bits. Therefore, the maximum matching cost is within a reasonable range in the semiglobal cost aggregation process, in which the maximum cost is limited to 10 binary digits in this system.We set the penalty item toP1=30 and P2=100 in this system, which indicates an assumption that the disparity of adjacent pixels is smooth. In fact, the depth change is discontinuous in automatic driving, which leads to that the disparity fluctuates greatly and the precision decreases in the depth discontinue pixel. In order to improve this problem, we propose to build a guide map to show where depth changes are possible [27]. Compared with the literature [27], we assume that the depth charge always occurs at the edge of objects. In addition, the structure is obvious at the edge region, which makes it easier to get the real and accurate disparity.First, we provide a one-dimensional Gaussian edge feature map (setting toμ=0 and σ=3 in the system) to indicate where the obvious edge is and where is the stereo matching result is more credible. Then, the first derivative in the same direction is calculated to obtain the gradient response. Next, the same process is implemented in the tangent direction. Finally, the sum of the square root of two gradient responses is considered as a feature map, as shown in Figure 1.Figure 1
An example of the feature map in stereo matching. Left: grey image. Right: feature map.
(a)(b)Based on the feature map, the larger the response value is, the stronger the edge characteristics are, and the greater the possibility of depth change. According to the above method, we infer that the maximum aggregation cost between adjacent pixels is 610 (255+100+255) in one path. The sum cost in eight paths is less than 4880 (610×8), which is represented by 13 binary digits. Therefore, the maximum memory cost of a single pixel is limited within 16 binary digits (2 Bytes) by eight aggregation paths. To reduce the computing cost, we propose a stereo matching optimization method, which focuses on reducing the disparity search range. We construct the multiscale images by the image pyramid method. In this study, we propose three layers pyramid method. As shown in Figure 2, the large-scale layer images have the original resolution of 1280×720. The middle-scale layer images are downsampled from the large-scale layer images, and the small-scale layer images are downsampled from the middle-scale layer images.Figure 2
Multiscale images pyramid and disparity map. Top: the large-scale layer. Middle: the middle-scale layer. Bottom: the small-scale layer.The structure features are obvious in the large-scale layer. Correspondingly, the remote details are completely preserved in the small-scale layer. At first, we propose to set the disparity search range as 16 pixels in the small-scale layer, which is equal to the search range of 64 pixels in the large-scale layer. Based on that, we calculate the small-scale disparity map by the SSIM algorithm in the small-scale layer. Then, we initialize the middle-scale disparity map by upsampling from the small-scale disparity map by the linear interpolation method. Because the small-scale disparity map has the same size as the small-scale layer,320×180, the size of initialized middle-scale disparity map is the same as that of the middle-scale layer, 640×360. Next, we refine the middle-scale disparity map. Based on the initialized disparity map, we set the same search range as 16 pixels. However, the beginning of the search depends on the initialized disparity, in which the search range with 16 pixels is a symmetric interval centered on the initialized disparity. Finally, we repeat the above processes to obtain the large-scale disparity map.At the calculation process of the multiscale stereo matching algorithm, the feature map is only adopted in the large-scale layer. The proposed stereo matching method is suitable for the FPGA to process:(i)
There are many reusable operation modules because the rules and parameters are the same(ii)
There is no need to cache all image data in memory at the same time, so the calculation process can be consistent with the data transmission process(iii)
The algorithm is designed to process numerical multiplication and addition by a large number of fixed-point data


## 3.2. Road Prediction

In practice, we focus on two issues: (1) where is the obstacle and (2) what is the trend of road. We propose the grid projection method to predict the trend of road. When obstacles occupy the road, the trend prediction will be hindered, but the freespace region should be correctly described. In this study, our method consists of three stages: grid projection, boundary search, and shape detection.


### 3.2.1. Grid Projection

Based on the disparity map, we calculate the 3D point cloud coordinate by Equation (5):(5)x=bu−u0disp,y=bv−v0disp,z=bfeqdispwhere x,y,z is the 3D coordinate in the world coordinate system (wcs), u,v is pixel coordinate in image coordinate system (ics), u0,v0 is the camera central point, disp is the disparity, b is the baseline of binocular camera, and feq is equivalent focal length. The disparity map and 3D pints cloud are shown in Figure 3.Figure 3
Disparity and 3D point cloud. (a) The left image. (b) Disparity map. (c) 3D point cloud.
(a)(b)(c)Inspired by pseudolidar data from visual [28], we build a projection space from bird eye view (BEV) in wcs. At BEV, the x-axis, which is parallel to the baseline of the binocular camera, represents the horizontal direction, and the y-axis, which is parallel to the optical axis of the camera and is positive horizontally to the front, represents the depth direction. In the projection space, the fixed size grid is divided as the basic projection unit, which the grid is a rectangle by setting 10cm×50cm in x-direction and z-direction. First, we divide point cloud data by grid mesh and drop out the outliers that points do not belong to the detection space Π=x,y,z∣y∈−0.5,2.5,z∈0,60. This constraint represents a priori assumption that the maximum height of the obstacles on road will not exceed 3 m, and the farthest observation distance is 60 m. Then, we count the number of points in each grid and set this value as the feature value of this grid. In this way, grids with lots of points cloud data have a larger accumulated number. As shown in Figure 4(c), we use color to represent the accumulated number, where the dark blue indicates zero and the brighter the color, the greater the accumulated number.Figure 4
The process of probability grid map. (a) Left grey image. (b) Disparity map. (c) Grid projection. (d) Probability grid map.
(a)(b)(c)(d)


### 3.2.2. Boundary Search

Inspired by the stochastic occupancy grids method [29], grids with larger feature values represent that there is a greater possibility of an obstacle. On the contrary, grids with smaller feature values are more likely to represent the road region. In addition, grids with zero feature value indicate that it is occluded and cannot be observed.By transferring the ics (disparity map) to vcs (grid projection), a unified physical scale is helpful to build a more robust mathematical model to solve the problem. In detection spaceΠ, we consider the ground as a plane parallel to the xoz and obstacles as a plane vertical to the horizontal plane. As shown in Figure 5, obstacles can be approximated by three plane models in the detection space Π: perpendicular to the optical axis (marked ②), parallel to the optical axis and perpendicular to the horizontal plane (marked ③), and perpendicular to the horizontal plane and intersecting with the optical axis (marked ④). In addition, the road plane is another plane (marked ①) that parallel to the horizontal plane or intersecting at a smaller angle.Figure 5
Road plane and three obstacle plane models in wcs.These plane models describe obstacles in different states. The plane ② represents obstacles perpendicular to the optical axis on the driving route, such as a vehicle in the same lane, which only the vehicle rear is visible. The plane ③ represents obstacles parallel to the optical axis adjacent to the driving route, such as fences and barriers, which only the side is visible. The plane ④ represents obstacles that intersect the optical axis on the driving route, such as guardrails and walls in the curve road. In addition, plane ② and plane ③ are combined to describe adjacent discontinuous obstacles, such as cut-in vehicles, the vehicle side and rear are visible. Based on the above model, obstacles generate a large number of 3D point cloud data on its plane. Therefore, there are many accumulated points in the projection area of the obstacle plane in the grid projection.Suppose that the basic detection unit on planes is a square, which represents that the obstacle is divided into many square units. In practice, the side length of the unit is consistent with the gird projection size inx-direction. At different distances, the number of point clouds generated by the detection unit conforms to the Equation (6):(6)Nunit=a×fz×μ2,where Nunit represents the number of 3D points in unit, a is the side length of unit (a=10cm in this study), f is the focal length of lens, μ is the pixel size of sensor, and z is distance in wcs (0m<z≤60m in this study).In the detection spaceΠ, the height constraint is less than hm. Therefore, the maximum of 3D points is h×Nunit/a in a grid. Furthermore, we construct a probabilistic occupancy grid, where we divide the accumulated number by the maximum. As shown in Figure 4(d), we use the color to indicate the probability. The dark blue represents low probability and red represents high probability. In polar coordinates, we transform the probability grid map (PGM) into a graph model, where the nodes represent elements in PGM with fixed angle resolution and radius resolution. The value of the node is the probability of the grid, the column index is angle, and the row index is radius. Therefore, the road boundary search problem is transformed into a dynamic programming problem to find the path with maximum probability. We consider the graph as a directed acyclic graph (DAG) that the direction is from left to right or opposite. In theory, the two directions have the same path. In this study, we propose a semiglobal optimization method to solve the dynamic programming problem.As shown in Figure6, the node represents the probability of the grid. The horizontal axis is the angle of the field of view, where the resolution is 1 degree. The vertical axis is the radius, where the resolution is the same as that of the grid in z direction. The FOV and grid projection space Π are not completely coincident, so the number of nodes in each column is not consistent. We propose a semiglobal energy function in Equation (7).(7)Ep∣p∈N=Pp−1θ,R+Tp−1,p+Ppθ,R,where N is the best path that the summary of probability is maximum. Ppθ,R represents the probability of pθ,R, and this point belongs to the best path p∈N. Tp−1,p represents the penalty term between adjacent column nodes, in which the p−1 is previous nodes and p is current nodes. The penalty term is defined as Equation (8).(8)Tp−1,p=0Rp−1−Rp=0T1Rp−1−Rp≤1T2otherwise.Figure 6
Directed acyclic graph with dynamic programming method. The black arrow indicates the best path.The penalty term represents an assumption that the path between adjacent columns is smooth in DAG.Rp−1is the radius index of node in the previous column and Rp is in current column. Obviously, the penalty term is constrained to −1≤T2<T1<0. Finally, the optimization function is defined as Equation (9).(9)Np=argmin∑Ep.The purpose of parameter setting is that the row coordinate changes of adjacent column nodes should be smooth. In this study, we set theT1=−0.1 and T2=−0.3. As a result, we obtain an optimal path in DAG, where the sum of probability exists maximum. We convert the node index into the coordinate of PGM, where grids are considered as boundary points of freespace in wcs. In addition, the boundary points in wcs are back projected into ics through Equation (6). As shown in Figure 7, the red line in the probability grad map is the optimal path and the yellow line is the freespace boundary in ics.Figure 7
Freespace boundary. (a) Optimal path by DP. (b) Boundary in ics.
(a)(b)In practice, we employ the multilayer grid projection to overcome that case where the near obstacle is lower than the far obstacles. Three layers are divided by height with−0.5m∼0.5m (subspace Π1), 0.5m∼1.5m (subspace Π2), and 1.5m∼2.5m (subspace Π3). As shown in Figure 8(a), the height of the near barrier is lower than the far greenbelt, so that the probability of barrier is smaller than the probability of greenbelt in Figure 8(c). We propose a weight energy function as Equation (10).(10)Ep∣p∈N=ωp−1Pp−1θ,R+Tp−1,p+ωpPpθ,R,where ωp is the weight of node p that the coordinate of this point is θ,R and 0<ω≤1. The p−1 indicates the previous node. The capital P indicates the penalty item. The ω in Π1 is greater than ω in Π2, and ω in Π2 is greater than ω in Π3. In this study, we set weight as ω=1 in Π1, ω=0.6 in Π2, andω=0.3 in Π3. As a result, the Figure 8(e) shows the weight probability grid map (WPGM) that the best path corresponds to our expectations, where the high brightness represents a large probability.Figure 8
Multilayers grid projection. (a) Left image. (b) Disparity map. (c) PGM inΠ. (d1) PGM in Π1. (d2) PGM in Π2. (d3) PGM in Π3. (d) WPGM in Π.
(a)(b)(c)(d)(e)


### 3.2.3. Shape Detection

Based on the three obstacle models, we detect geometric constraints on the multilayer grid projection. In practice, we propose to identify the geometry feature by prior. The plane ②, in Figure5, is a segment parallel to the x-axis in the WPGM, the plane ③ is another segment parallel to the z-axis, and the plane ④ is an oblique line or curve. Therefore, we detect these geometry features on the boundary to identify if there any obstacle exists and road environment, for example, vehicle is going to drive into straight road or curved road.To reduce the noise, we discard the grid whose probability is less than 0.1. The result is shown in Figure9. The left column is the grey images, the middle column is the feature points in WPGM, and the right column is the result of shape detection.Figure 9
The process of shape detection. (a) In curved road. (b) In straight road. (c) With obstacles.
(a)(b)(c)In the middle column of Figure9, the red points are feature points that make up the shape feature map (SFM). These feature points are used to progressive probabilistic Hough transform, in which the coordinates use pixel dimensions in the SFM. For example, in the right column of Figure 9(a), the different color line represents different slopes: the slope of line (1) is 0.3862, the slope of line (2) is 0.5062, and the slope of line (3) is 0.0082. We consider the line (3) is modelled as the plane ②, while the line (1) and line (3) are modelled as plane ④. However, the plane ② is usually considered to be an independent obstacle, such as a vehicle. Because line (2) and line (3) are connected, they are regarded as independent obstacle in the wcs. In addition, plane ④ is usually considered continue obstacle, such as guardrails and walls in the curve road. The line (1) is regarded as a guardrails or walls in the curve road.We describe the shape feature by different plane models so that the complex road environment is classified into finite-state models. Table1 describes Figure 9. The (a) scene is described as 1 curve road model and 1 obstacle model, the (b) scene is described as 6 straight road models, and the (c) scene is described as 4 obstacle models. Therefore, the (a) scene is predicted to be going to drive into curve road and there is an obstacle. The (b) scene is predicted to drive on a straight road without obstacle. The (c) scene is predicted to that there are many obstacles in front of the vehicle and other road information is invalid.Table 1
Describe the road environment by state model.
Curve roadStraight roadObstacles(a)101(b)060(c)004


## 3.2.1. Grid Projection

Based on the disparity map, we calculate the 3D point cloud coordinate by Equation (5):(5)x=bu−u0disp,y=bv−v0disp,z=bfeqdispwhere x,y,z is the 3D coordinate in the world coordinate system (wcs), u,v is pixel coordinate in image coordinate system (ics), u0,v0 is the camera central point, disp is the disparity, b is the baseline of binocular camera, and feq is equivalent focal length. The disparity map and 3D pints cloud are shown in Figure 3.Figure 3
Disparity and 3D point cloud. (a) The left image. (b) Disparity map. (c) 3D point cloud.
(a)(b)(c)Inspired by pseudolidar data from visual [28], we build a projection space from bird eye view (BEV) in wcs. At BEV, the x-axis, which is parallel to the baseline of the binocular camera, represents the horizontal direction, and the y-axis, which is parallel to the optical axis of the camera and is positive horizontally to the front, represents the depth direction. In the projection space, the fixed size grid is divided as the basic projection unit, which the grid is a rectangle by setting 10cm×50cm in x-direction and z-direction. First, we divide point cloud data by grid mesh and drop out the outliers that points do not belong to the detection space Π=x,y,z∣y∈−0.5,2.5,z∈0,60. This constraint represents a priori assumption that the maximum height of the obstacles on road will not exceed 3 m, and the farthest observation distance is 60 m. Then, we count the number of points in each grid and set this value as the feature value of this grid. In this way, grids with lots of points cloud data have a larger accumulated number. As shown in Figure 4(c), we use color to represent the accumulated number, where the dark blue indicates zero and the brighter the color, the greater the accumulated number.Figure 4
The process of probability grid map. (a) Left grey image. (b) Disparity map. (c) Grid projection. (d) Probability grid map.
(a)(b)(c)(d)


## 3.2.2. Boundary Search

Inspired by the stochastic occupancy grids method [29], grids with larger feature values represent that there is a greater possibility of an obstacle. On the contrary, grids with smaller feature values are more likely to represent the road region. In addition, grids with zero feature value indicate that it is occluded and cannot be observed.By transferring the ics (disparity map) to vcs (grid projection), a unified physical scale is helpful to build a more robust mathematical model to solve the problem. In detection spaceΠ, we consider the ground as a plane parallel to the xoz and obstacles as a plane vertical to the horizontal plane. As shown in Figure 5, obstacles can be approximated by three plane models in the detection space Π: perpendicular to the optical axis (marked ②), parallel to the optical axis and perpendicular to the horizontal plane (marked ③), and perpendicular to the horizontal plane and intersecting with the optical axis (marked ④). In addition, the road plane is another plane (marked ①) that parallel to the horizontal plane or intersecting at a smaller angle.Figure 5
Road plane and three obstacle plane models in wcs.These plane models describe obstacles in different states. The plane ② represents obstacles perpendicular to the optical axis on the driving route, such as a vehicle in the same lane, which only the vehicle rear is visible. The plane ③ represents obstacles parallel to the optical axis adjacent to the driving route, such as fences and barriers, which only the side is visible. The plane ④ represents obstacles that intersect the optical axis on the driving route, such as guardrails and walls in the curve road. In addition, plane ② and plane ③ are combined to describe adjacent discontinuous obstacles, such as cut-in vehicles, the vehicle side and rear are visible. Based on the above model, obstacles generate a large number of 3D point cloud data on its plane. Therefore, there are many accumulated points in the projection area of the obstacle plane in the grid projection.Suppose that the basic detection unit on planes is a square, which represents that the obstacle is divided into many square units. In practice, the side length of the unit is consistent with the gird projection size inx-direction. At different distances, the number of point clouds generated by the detection unit conforms to the Equation (6):(6)Nunit=a×fz×μ2,where Nunit represents the number of 3D points in unit, a is the side length of unit (a=10cm in this study), f is the focal length of lens, μ is the pixel size of sensor, and z is distance in wcs (0m<z≤60m in this study).In the detection spaceΠ, the height constraint is less than hm. Therefore, the maximum of 3D points is h×Nunit/a in a grid. Furthermore, we construct a probabilistic occupancy grid, where we divide the accumulated number by the maximum. As shown in Figure 4(d), we use the color to indicate the probability. The dark blue represents low probability and red represents high probability. In polar coordinates, we transform the probability grid map (PGM) into a graph model, where the nodes represent elements in PGM with fixed angle resolution and radius resolution. The value of the node is the probability of the grid, the column index is angle, and the row index is radius. Therefore, the road boundary search problem is transformed into a dynamic programming problem to find the path with maximum probability. We consider the graph as a directed acyclic graph (DAG) that the direction is from left to right or opposite. In theory, the two directions have the same path. In this study, we propose a semiglobal optimization method to solve the dynamic programming problem.As shown in Figure6, the node represents the probability of the grid. The horizontal axis is the angle of the field of view, where the resolution is 1 degree. The vertical axis is the radius, where the resolution is the same as that of the grid in z direction. The FOV and grid projection space Π are not completely coincident, so the number of nodes in each column is not consistent. We propose a semiglobal energy function in Equation (7).(7)Ep∣p∈N=Pp−1θ,R+Tp−1,p+Ppθ,R,where N is the best path that the summary of probability is maximum. Ppθ,R represents the probability of pθ,R, and this point belongs to the best path p∈N. Tp−1,p represents the penalty term between adjacent column nodes, in which the p−1 is previous nodes and p is current nodes. The penalty term is defined as Equation (8).(8)Tp−1,p=0Rp−1−Rp=0T1Rp−1−Rp≤1T2otherwise.Figure 6
Directed acyclic graph with dynamic programming method. The black arrow indicates the best path.The penalty term represents an assumption that the path between adjacent columns is smooth in DAG.Rp−1is the radius index of node in the previous column and Rp is in current column. Obviously, the penalty term is constrained to −1≤T2<T1<0. Finally, the optimization function is defined as Equation (9).(9)Np=argmin∑Ep.The purpose of parameter setting is that the row coordinate changes of adjacent column nodes should be smooth. In this study, we set theT1=−0.1 and T2=−0.3. As a result, we obtain an optimal path in DAG, where the sum of probability exists maximum. We convert the node index into the coordinate of PGM, where grids are considered as boundary points of freespace in wcs. In addition, the boundary points in wcs are back projected into ics through Equation (6). As shown in Figure 7, the red line in the probability grad map is the optimal path and the yellow line is the freespace boundary in ics.Figure 7
Freespace boundary. (a) Optimal path by DP. (b) Boundary in ics.
(a)(b)In practice, we employ the multilayer grid projection to overcome that case where the near obstacle is lower than the far obstacles. Three layers are divided by height with−0.5m∼0.5m (subspace Π1), 0.5m∼1.5m (subspace Π2), and 1.5m∼2.5m (subspace Π3). As shown in Figure 8(a), the height of the near barrier is lower than the far greenbelt, so that the probability of barrier is smaller than the probability of greenbelt in Figure 8(c). We propose a weight energy function as Equation (10).(10)Ep∣p∈N=ωp−1Pp−1θ,R+Tp−1,p+ωpPpθ,R,where ωp is the weight of node p that the coordinate of this point is θ,R and 0<ω≤1. The p−1 indicates the previous node. The capital P indicates the penalty item. The ω in Π1 is greater than ω in Π2, and ω in Π2 is greater than ω in Π3. In this study, we set weight as ω=1 in Π1, ω=0.6 in Π2, andω=0.3 in Π3. As a result, the Figure 8(e) shows the weight probability grid map (WPGM) that the best path corresponds to our expectations, where the high brightness represents a large probability.Figure 8
Multilayers grid projection. (a) Left image. (b) Disparity map. (c) PGM inΠ. (d1) PGM in Π1. (d2) PGM in Π2. (d3) PGM in Π3. (d) WPGM in Π.
(a)(b)(c)(d)(e)


## 3.2.3. Shape Detection

Based on the three obstacle models, we detect geometric constraints on the multilayer grid projection. In practice, we propose to identify the geometry feature by prior. The plane ②, in Figure5, is a segment parallel to the x-axis in the WPGM, the plane ③ is another segment parallel to the z-axis, and the plane ④ is an oblique line or curve. Therefore, we detect these geometry features on the boundary to identify if there any obstacle exists and road environment, for example, vehicle is going to drive into straight road or curved road.To reduce the noise, we discard the grid whose probability is less than 0.1. The result is shown in Figure9. The left column is the grey images, the middle column is the feature points in WPGM, and the right column is the result of shape detection.Figure 9
The process of shape detection. (a) In curved road. (b) In straight road. (c) With obstacles.
(a)(b)(c)In the middle column of Figure9, the red points are feature points that make up the shape feature map (SFM). These feature points are used to progressive probabilistic Hough transform, in which the coordinates use pixel dimensions in the SFM. For example, in the right column of Figure 9(a), the different color line represents different slopes: the slope of line (1) is 0.3862, the slope of line (2) is 0.5062, and the slope of line (3) is 0.0082. We consider the line (3) is modelled as the plane ②, while the line (1) and line (3) are modelled as plane ④. However, the plane ② is usually considered to be an independent obstacle, such as a vehicle. Because line (2) and line (3) are connected, they are regarded as independent obstacle in the wcs. In addition, plane ④ is usually considered continue obstacle, such as guardrails and walls in the curve road. The line (1) is regarded as a guardrails or walls in the curve road.We describe the shape feature by different plane models so that the complex road environment is classified into finite-state models. Table1 describes Figure 9. The (a) scene is described as 1 curve road model and 1 obstacle model, the (b) scene is described as 6 straight road models, and the (c) scene is described as 4 obstacle models. Therefore, the (a) scene is predicted to be going to drive into curve road and there is an obstacle. The (b) scene is predicted to drive on a straight road without obstacle. The (c) scene is predicted to that there are many obstacles in front of the vehicle and other road information is invalid.Table 1
Describe the road environment by state model.
Curve roadStraight roadObstacles(a)101(b)060(c)004


## 4. System Design

### 4.1. Hardware Architecture

The proposed system is used for forward sensing of automatic driving or advanced driver assistance system (ADAS). In practice, the system is generally installed in the narrow space between the windshield and the inside rearview mirror. It requires that the space volume of the hardware system must be small. The system hardware design architecture is shown in Figure10.Figure 10
System hardware platform.The lens focal length is8.26mm and the image resolution is 1280×720. The embedded computing core is a Xilinx Z-7020 system on chip (SoC) that includes an Artix-7 field-programmable gate array (FPGA) and two A9 advanced RISC machines (ARM). Based on this hardware platform, the system’s full load power is 6 W without any cooling system. In addition, its volume is only 17cm×8cm×4cm. Other system parameters are shown in Table 2.Table 2
System hardware parameters.
ItemsParametersItemsParametersBaseline120 mmFocal length8.26 mmPixel size4.2×10−3mmResolution1280×720Horizontal FOV30°Vertical FOV22°


### 4.2. Software Architecture

In the algorithm function, we divide three parts: stereo matching, probability grid, and state model, as shown in Figure11. The stereo matching module includes SSIM cost computing, cost aggregation, subpixel disparity estimation, and 3D point cloud computing. The probability grid module includes grid projection, weight probability grid map, and boundary search. The state model module includes shape detection and state module.Figure 11
System software flow framework.The three modules are processed by parallel computing on three processing units. The frequency of data processing depends on the slowest one of the three modules, and the system delay is equal to the sum of the three modules. In the proposed system, the FPGA delay is 66 ms, the ARM1 delay is 16 ms, and the ARM2 delay is 21 ms. Therefore, the frequency of the system is 15 fps and the system delay is 103 ms.


## 4.1. Hardware Architecture

The proposed system is used for forward sensing of automatic driving or advanced driver assistance system (ADAS). In practice, the system is generally installed in the narrow space between the windshield and the inside rearview mirror. It requires that the space volume of the hardware system must be small. The system hardware design architecture is shown in Figure10.Figure 10
System hardware platform.The lens focal length is8.26mm and the image resolution is 1280×720. The embedded computing core is a Xilinx Z-7020 system on chip (SoC) that includes an Artix-7 field-programmable gate array (FPGA) and two A9 advanced RISC machines (ARM). Based on this hardware platform, the system’s full load power is 6 W without any cooling system. In addition, its volume is only 17cm×8cm×4cm. Other system parameters are shown in Table 2.Table 2
System hardware parameters.
ItemsParametersItemsParametersBaseline120 mmFocal length8.26 mmPixel size4.2×10−3mmResolution1280×720Horizontal FOV30°Vertical FOV22°


## 4.2. Software Architecture

In the algorithm function, we divide three parts: stereo matching, probability grid, and state model, as shown in Figure11. The stereo matching module includes SSIM cost computing, cost aggregation, subpixel disparity estimation, and 3D point cloud computing. The probability grid module includes grid projection, weight probability grid map, and boundary search. The state model module includes shape detection and state module.Figure 11
System software flow framework.The three modules are processed by parallel computing on three processing units. The frequency of data processing depends on the slowest one of the three modules, and the system delay is equal to the sum of the three modules. In the proposed system, the FPGA delay is 66 ms, the ARM1 delay is 16 ms, and the ARM2 delay is 21 ms. Therefore, the frequency of the system is 15 fps and the system delay is 103 ms.


## 5. Experiment and Analysis

### 5.1. Stereo Matching

We evaluate our stereo matching method by two comparative experiments: efficiency and accuracy. By compared on KITTI dataset [30] as benchmark data set, our multiscale stereo matching algorithm (93.22%) is lower average error rate than classical unsupervised stereo matching algorithm, such as SGMB [9] (92.36%), ELAS [31] (91.76%), and origin MPV algorithm [10] (94.43%). The accuracy of our method is higher than SGBM and ELAS. Although our method is lower than the origin MPV algorithm in accuracy, its computing efficiency is higher than the origin MPV algorithm. The detail is shown in Table 3. Based on the running times of our proposed algorithm as the benchmark, the efficiency is defined as the running time of the comparison algorithm divided by the benchmark. Therefore, the efficiency of our algorithm is 1.Table 3
Evaluation of stereo matching in KITTI.
MethodAccuracy (%)EfficiencyOur proposed93.221.00Origin MPV94.433.05SGBM92.36—ELAS91.76—In addition, we test the multiscale stereo matching method in the private dataset, where we focus following items: subpixel disparity accuracy, light condition, and point cloud distribution.(i)
Subpixel Disparity Accuracy. When the absolute value of disparity error is less than 1.0 (0.5, 0.3, 0.1) pixel, the matching disparity is correct. The index is defined as the corrected number divided by the total pixels(ii)
Light Condition. The disparity accuracy is evaluated by the different light condition, such as sunny, cloudy, night, and backlight(iii)
Convergence. Assuming the distribution of point clouds being Gaussian model, disparity’s variance represents the convergence degree of the point cloudsThe result is shown in Table4. The subpixel disparity accuracy represents a more precise ability of the stereo matching algorithm, which the large proportion in high precision range indicates the disparity estimation algorithm is close to ground truth (GT). The proposed multiscale stereo matching method has the highest precision in sunny and cloudy, because the imaging quality is best in this weather. The edge will become unclear due to overexposure in backlight, and the texture features will be blurred in dark environment, such as at night. Therefore, the subpixel disparity accuracy decreases slightly in the backlight, but significantly at night. In addition, the variance indicates the aggregation degree of disparity on the same obstacle plane. The small variance represents that the disparity estimation is stable and the point clouds converge in space. The experimental results show that the proposed method is stable and robust under good light conditions.Table 4
Evaluation of stereo matching in private dataset.
Test caseSunnyCloudyNightBacklightAccuracy (1.0)78.94%80.06%58.68%78.49%Accuracy (0.5)51.00%51.33%32.47%50.11%Accuracy (0.3)33.84%32.42%19.54%31.62%Accuracy (0.1)11.97%11.13%6.66%10.84%Convergence (pixel)4.664.155.735.37Figure12 shows middle processing to explain our multiscale stereo matching algorithm and weight probability grid map. The left column is grey images, the middle column is a disparity map corresponding to points cloud space, and the right column is a weighted probability grid map corresponding to points cloud space.Figure 12
The WPGM in different road environment. Grey images, disparity space, and weight probability grid map.


### 5.2. Road Prediction

We evaluate the proposed system by following experiments: freespace detection, obstacle prediction, road environment prediction, and system performance.Based on the private dataset, we label the GT of freespace on images. We evaluate the freespace by recall method, in which the index represents the similarity between the detection result and GT. Recall = 100% indicates that the detection result is completely consistent with the true value, while IoU = 0 indicates completely inconsistent. The result is shown in Table5, where our system is compared by two classic methods to shows that our method is advanced in efficiency and result. Our system is more efficient than Xin et al. [32] method in running time and recall. Compared to Hautiere et al. [33] method, we have halved our running time but lost only 1.5% of recall. The experiment shows that our system is robust and stable in freespace detection.Table 5
Evaluation of freespace detection in private dataset.
MethodRunning time (ms)Recall (%)Hautiere et al. [33]3095.0Xin et al. [32]1836091.7Ours1693.5The obstacle prediction is one of the tasks based on the state model. We evaluate the results of obstacle prediction by the number and location of obstacles based on our private dataset. The GT of obstacles is labelled manually, where we focus on independent objects such as vehicles and pedestrians. We count the number of obstacles in the state model to compare with GT, so the average recall rate of obstacle detection of obstacles is 98.3% in private dataset. As shown in Figure13, the left shows the deep distance and the right shows the horizon distance, where the deep distance is represented by the vertical axis and horizon distance is represented by the horizontal axis on WPGM. We employ the root mean squared error (RMSE) to evaluate the error of relative distance by Equation (11):(11)RMSE=1n∑i=1ndatai−GTi2,where datai is observations and n is the total number of test dataset. The RMSE in the deep distance is 0.42 m and 0.46 m in the horizon distance. The experiment shows that our method is robust and stable in obstacle prediction.Figure 13
Obstacle prediction in deep and horizon distance.
(a)(b)We evaluate the state model by precision and recall as Table6, where the first row represents the GT, and the first column represents the observations. We provide 1000 images including 600 straight road state, 300 curve road state, and 100 obstacle state. Precision and recall index are employed to evaluate state model prediction. The experiment shows that our system is sensitive to road environment prediction.Table 6
Evaluation of state model prediction in private dataset.
Curve roadStraight roadObstaclePrecisionCurve road2937496.38%Straight road1591099.83%Obstacle529693.20%Recall97.67%98.50%96.00%—In addition, we test the system performance by running time and power cost. As shown in Table7, we record the running time of three hardware units. The largest time cost is stereo matching and point cloud in FPGA, where its running time is 66 ms. The frame rate of the system is decided by this module to 15 fps. The system delay time is a summary of three modules, which is 103 ms.Table 7
Running time of modules.
Hardware unitFPGAARM1ARM2ModuleStereo matchingPoint cloudWPGMPath planningShape detectionState modelRunning time55 ms11 ms12 ms4 ms20 ms1 msDelay time66 ms16 ms21 msFinally, we test the power cost and the chip temperature. Our system is installed between the front windshield and the rear-view mirror, so it must meet the requirements of GB/T 28046.4, which stipulates that the maximum operating temperature of the system shall not exceed 90°C. Table8 shows that the maximum temperature in full load power meets the standard requirement.Table 8
Chip temperature test.
Ambient temp (°C)Chip temp (°C)Load power (W)-1540∼5061550∼6064560∼7566575∼906


## 5.1. Stereo Matching

We evaluate our stereo matching method by two comparative experiments: efficiency and accuracy. By compared on KITTI dataset [30] as benchmark data set, our multiscale stereo matching algorithm (93.22%) is lower average error rate than classical unsupervised stereo matching algorithm, such as SGMB [9] (92.36%), ELAS [31] (91.76%), and origin MPV algorithm [10] (94.43%). The accuracy of our method is higher than SGBM and ELAS. Although our method is lower than the origin MPV algorithm in accuracy, its computing efficiency is higher than the origin MPV algorithm. The detail is shown in Table 3. Based on the running times of our proposed algorithm as the benchmark, the efficiency is defined as the running time of the comparison algorithm divided by the benchmark. Therefore, the efficiency of our algorithm is 1.Table 3
Evaluation of stereo matching in KITTI.
MethodAccuracy (%)EfficiencyOur proposed93.221.00Origin MPV94.433.05SGBM92.36—ELAS91.76—In addition, we test the multiscale stereo matching method in the private dataset, where we focus following items: subpixel disparity accuracy, light condition, and point cloud distribution.(i)
Subpixel Disparity Accuracy. When the absolute value of disparity error is less than 1.0 (0.5, 0.3, 0.1) pixel, the matching disparity is correct. The index is defined as the corrected number divided by the total pixels(ii)
Light Condition. The disparity accuracy is evaluated by the different light condition, such as sunny, cloudy, night, and backlight(iii)
Convergence. Assuming the distribution of point clouds being Gaussian model, disparity’s variance represents the convergence degree of the point cloudsThe result is shown in Table4. The subpixel disparity accuracy represents a more precise ability of the stereo matching algorithm, which the large proportion in high precision range indicates the disparity estimation algorithm is close to ground truth (GT). The proposed multiscale stereo matching method has the highest precision in sunny and cloudy, because the imaging quality is best in this weather. The edge will become unclear due to overexposure in backlight, and the texture features will be blurred in dark environment, such as at night. Therefore, the subpixel disparity accuracy decreases slightly in the backlight, but significantly at night. In addition, the variance indicates the aggregation degree of disparity on the same obstacle plane. The small variance represents that the disparity estimation is stable and the point clouds converge in space. The experimental results show that the proposed method is stable and robust under good light conditions.Table 4
Evaluation of stereo matching in private dataset.
Test caseSunnyCloudyNightBacklightAccuracy (1.0)78.94%80.06%58.68%78.49%Accuracy (0.5)51.00%51.33%32.47%50.11%Accuracy (0.3)33.84%32.42%19.54%31.62%Accuracy (0.1)11.97%11.13%6.66%10.84%Convergence (pixel)4.664.155.735.37Figure12 shows middle processing to explain our multiscale stereo matching algorithm and weight probability grid map. The left column is grey images, the middle column is a disparity map corresponding to points cloud space, and the right column is a weighted probability grid map corresponding to points cloud space.Figure 12
The WPGM in different road environment. Grey images, disparity space, and weight probability grid map.


## 5.2. Road Prediction

We evaluate the proposed system by following experiments: freespace detection, obstacle prediction, road environment prediction, and system performance.Based on the private dataset, we label the GT of freespace on images. We evaluate the freespace by recall method, in which the index represents the similarity between the detection result and GT. Recall = 100% indicates that the detection result is completely consistent with the true value, while IoU = 0 indicates completely inconsistent. The result is shown in Table5, where our system is compared by two classic methods to shows that our method is advanced in efficiency and result. Our system is more efficient than Xin et al. [32] method in running time and recall. Compared to Hautiere et al. [33] method, we have halved our running time but lost only 1.5% of recall. The experiment shows that our system is robust and stable in freespace detection.Table 5
Evaluation of freespace detection in private dataset.
MethodRunning time (ms)Recall (%)Hautiere et al. [33]3095.0Xin et al. [32]1836091.7Ours1693.5The obstacle prediction is one of the tasks based on the state model. We evaluate the results of obstacle prediction by the number and location of obstacles based on our private dataset. The GT of obstacles is labelled manually, where we focus on independent objects such as vehicles and pedestrians. We count the number of obstacles in the state model to compare with GT, so the average recall rate of obstacle detection of obstacles is 98.3% in private dataset. As shown in Figure13, the left shows the deep distance and the right shows the horizon distance, where the deep distance is represented by the vertical axis and horizon distance is represented by the horizontal axis on WPGM. We employ the root mean squared error (RMSE) to evaluate the error of relative distance by Equation (11):(11)RMSE=1n∑i=1ndatai−GTi2,where datai is observations and n is the total number of test dataset. The RMSE in the deep distance is 0.42 m and 0.46 m in the horizon distance. The experiment shows that our method is robust and stable in obstacle prediction.Figure 13
Obstacle prediction in deep and horizon distance.
(a)(b)We evaluate the state model by precision and recall as Table6, where the first row represents the GT, and the first column represents the observations. We provide 1000 images including 600 straight road state, 300 curve road state, and 100 obstacle state. Precision and recall index are employed to evaluate state model prediction. The experiment shows that our system is sensitive to road environment prediction.Table 6
Evaluation of state model prediction in private dataset.
Curve roadStraight roadObstaclePrecisionCurve road2937496.38%Straight road1591099.83%Obstacle529693.20%Recall97.67%98.50%96.00%—In addition, we test the system performance by running time and power cost. As shown in Table7, we record the running time of three hardware units. The largest time cost is stereo matching and point cloud in FPGA, where its running time is 66 ms. The frame rate of the system is decided by this module to 15 fps. The system delay time is a summary of three modules, which is 103 ms.Table 7
Running time of modules.
Hardware unitFPGAARM1ARM2ModuleStereo matchingPoint cloudWPGMPath planningShape detectionState modelRunning time55 ms11 ms12 ms4 ms20 ms1 msDelay time66 ms16 ms21 msFinally, we test the power cost and the chip temperature. Our system is installed between the front windshield and the rear-view mirror, so it must meet the requirements of GB/T 28046.4, which stipulates that the maximum operating temperature of the system shall not exceed 90°C. Table8 shows that the maximum temperature in full load power meets the standard requirement.Table 8
Chip temperature test.
Ambient temp (°C)Chip temp (°C)Load power (W)-1540∼5061550∼6064560∼7566575∼906


## 6. Conclusions

In this study, we propose a low-power road environment prediction system, which the proposed system consists of a binocular camera and a low-power computing unit. Our contribution includes three points as follows. Firstly, a multiscale stereo matching algorithm is proposed for hardware computing. Next, we propose a weighted probability grid map-based points cloud. Finally, the plane model and state model are proposed to describe the road environment. Our work proves that the existing technology achieves the function requirement under the low-power constraint. Experiments show that the proposed system is robust and sensitive to road environment prediction and the performance meets the mandatory standards in practice. In future work, our study provides a benchmark for obstacles recognition and path planning.



---

*Source: 5569295-2021-04-30.xml*