Combing VFH with bezier for motion planning of an autonomous vehicle

Vector Field Histogram (VFH) is a method for mobile robot obstacle avoidance. However, due to the nonholonomic constraints of the vehicle, the algorithm is seldom applied to autonomous vehicles. Especially when we expect the vehicle to reach target location in a certain direction, the algorithm is often unsatisfactory. Fortunately, the Bezier Curve is defined by the states of the starting point and the target point. We can use this feature to make the vehicle in the expected direction. Therefore, we propose an algorithm to combine the Bezier Curve with the VFH algorithm, to search for the collision-free states with the VFH search method, and to select the optimal trajectory point with the Bezier Curve as the reference line. This means that we will improve the cost function in the VFH algorithm by comparing the distance between candidate directions and reference line. Finally, select the closest direction to the reference line to be the optimal motion direction.


Introduction
Motion planning is an important part of autonomous vehicle research. It is problem is to generate continuous motion that connects start state and target state, while avoiding collision with known obstacles [1]. The research in robot motion planning can be traced back to the late 60's.Over so many years, many motion planning algorithms were proposed, such as A*,D* [2] and RRT [3]. However, it is quite difficult to take the vehicle's non-holonomic constraints into considerations.
In contrast, the parameter curve is simpler and it can satisfy the dynamic constraints well. Bezier Curve is a typical example. Cornell team proposed the two-dimensional cubic Bezier curve to represent trajectories. Long Han present a trajectory planning algorithm based on Bezier curve [4]. For intelligent vehicles, the ability of avoiding obstacles becomes very important, so more and more researcher focus on the study of obstacle avoidance algorithm. For example, the MIT's intelligent vehicle uses RRT as trajectory planning to avoid obstacle. However, it is still difficult for intelligent vehicles to reach an expected state in complex environment.
The Bezier Curve earliest applied to automotive body design by Pierre Bezier in 1962.It is a typical method of generating trajectories by analytic method. The advantage of Bezier Curve is that it can be generated fast and it is smooth enough which is very important for intelligent vehicle when there is no obstacle or the obstacles are sparse. However, when the curve is generated, only the pose and curvature constrains of the endpoints are taken into account, the obstacles information are not taken into account. Therefore, the method itself does not have the obstacle avoidance function. What's more, , [0,

)
, We firs r algorithm i clusters are g curve as the Thirdly, we s whether the v oint.
zier Curve h ontrol. To su ) , ,  (4) is reduced to the following form: In (5), (6): 3( ), The third-order Bezier Curve is shown in Fig.2. Where respectively. If the tangent of the Bezier Curve and the heading of the motion trajectory are linked, then the Bezier Curve can satisfy the heading of the motion trajectory at the starting state 0 P and target state 3 P . Third, the third-order Bezier Curves are not only self-continuous, but also their first derivative and second derivative are continuous, which fully meet the requirements of trajectory continuity in restricted vehicle systems. Fourth, it is convenient to convert the kinematic constraints into curve constraints, thus adding the vehicle kinematic constraints to the motion planning process. From the Fig.2, we can see that control point 1 P must be tangent at point 0 P , control point 2 P must be tangent at point 3 P .But the distance between 0 P and 1 P can be changed, and the distance between 2 P and 3 P can also be changed. So by changing the location of the control point 1 P and 2 P , we can get the curves cluster connecting 0 P and 3 P .
After obtaining the cluster of curves, we need to further select the smoothest curve .The smoothest curves can be abstracted into optimization problems: Next we derive the curvature calculation formula. According to the definition of curvature: x t y t x t y t t x t y t      (1) (1) ( Next, we divide the active region into several sectors and count the obstacle density for each sector, we can get the initial polar histogram. As shown in Fig. 4, selecting an angular resolution  divides the vehicle's active region into  360  n sectors. In order to ensure that n is a positive integer, angular resolution  is often taken as a divisor of 360.Corresponding to each sector, there is an index k and an obstacle density k h , where k is related to the discrete direction     k of the sector and defined as (17): INT stands for rounding. The obstacle density k h of the sector k is defined as (18): By calculating k h , we can get the initial polar histogram. However, due to the discrete nature of the obstacle grid, the initial histogram is uneven, which may affect the determination of the final motion direction. So we introduce a low-pass filter to smooth the initial polar histogram. Finally, we use a threshold to determine candidate directions based on the smoothed polar histogram [11]. If the obstacle density of sector lowers than the threshold, we regard the sector as candidate directions. The sector is also the collision-free region.
Third, we use the cost function to find the optimal motion direction. Through the threshold we can see from the polar histogram which direction is collision-free? This section we will describe how to use the reference line (Bezier Curve) to select the best direction.
When we select the best direction, we should take the width of vehicle into account [9].Assuming that the search step of the vehicle is 8 meters and the angle of the active region is 60 degrees. If the active region is divided equally into 20 sectors, the arc length corresponding to each sector is about 0.42 meters. The width of vehicle is about 2 meters, so the arc length of five sectors to meet the vehicle's width. This require that the two sectors of the selected sector, which are adjacent to the left, and two sectors(five sectors in total) that are adjacent to the right are also collision-free. We use n f r e e C C  to represents that the n-th sector is collision-free. If the n-th sector satisfies the requirement of vehicle's width, we can express it as free n n n n n And the k-th sector is our optimal motion direction.