Vision-based map building and path planning method in unmanned air/ground vehicle cooperative systems

: To reduce the casualties and improve the efficiency of robot, unmanned air/ground vehicle (UAV/UGV) cooperative systems are proposed. In this study, mathematical algorithms are applied in a typical-rescue scenario. As a ‘flying eye’, the UAV provided the global information by vision sensor. Then, the images were processed with SURF algorithm and image binarisation to model the environment. Based on the UAV's information, the optimised A* algorithm was proposed. Finally, a feasible path was designed for UGV to rescue. Experiments are performed to evaluate the performances of the proposed method. Results show that SURF algorithm and image binarisation can realise the accuracy and robustness of map building. The optimised A* algorithm can provide a real-time and feasible path.


Introduction
At present, unmanned air/ground vehicle (UAV/UGV) cooperative systems can replace human beings to approach these dangerous missions, such as fire rescuing and military [1,2]. A typical scenario of UAV/UGV cooperative systems is shown in Fig. 1, where the target needs to be rescued. From Fig. 2, we can see that the UGV is easily obscured by obstacles and cannot get the information of the target. By contrast, the UAV can easily recognise the target, but the UAV is limited by flight time. Therefore, the UAV and the UGV should work cooperatively.
Visual sensor is one of the main sensors to solve the problem. Chaimowicz et al. [3,4] had carried out a series of studies based on UAV/UGV cooperative system and studied the visual target detection and localisation algorithm. The verification experiment had been carried out successfully. However, they did not consider information exchange between UAVs and UAVs.
Li et al. [5] studied the ground map building and put forward a path planning in UAV/UGV cooperative systems. They processed image with image denoising, image correction, and obstacle recognition to construct the ground map automatically. Nevertheless, the results did not convert the map in pixels to a metric map. Feng et al. [6] explored the method of using robot to achieve cooperative navigation. The recognition of ground obstacles was realised by using UAV vision sensor. Based on the YCbRc colour space, the moving target was detected and the tracking of the ground robot was realised by using the Windows Tracking. However, this method was verified only in a special environment.
In the study of path planning, Aghaeeyan et al. [7] put forward the system of leader and follower in UAV/UGV cooperative systems and designed a UGV sliding mode controller. However, the path planning of UGV is planned under the local strategy and the accuracy cannot achieve the desired result in the process of target following. Ganeshmurthy et al. proposed a heuristic-based method to search the feasible initial path efficiently to solve the problem of dynamic environments [8]. Zhu et al. proposed a global path planning of robots by multi-objective algorithms [9,10]. Zhao et al. [11] proposed modified artificial potential field in a complex environment and used the adjustment factor to improve the objective function of the gravitational point. Finally, they realised the path planning of UGV. However, the artificial potential field method has different objective functions for different obstacles, resulting in large amount of computations.
In this paper, the main contributions as follows: (i) With a vision sensor, images are acquired from outside environment by UAV. Then, the target and obstacles are found and a traversable map is built. (ii) Three path planning algorithms are compared. Then, the optimised A* algorithm is proposed, which can plan a feasible path. (iii) Two groups of experiments are designed to evaluate the performance of the proposed algorithms.
The remaining sections of the paper are organised as follows. A hierarchical system is given in Section 2. Main steps of map building are discussed in Section 3. Coordinate transformation is discussed in Section 4. Methods of global path planning are discussed in Section 5. Simulation and discussion are shown in Section 6 and Section 7 gives conclusions. Based on the automated highway system [12], a hierarchical system is proposed in Fig. 3. The management layer is mainly responsible for strategy and supervise the UAV and UGV. The communication layer is mainly responsible for information exchange among inter-UAVs, inter-UGVs, intra-UAV/UGV. The function of execution and control layer is to sense the environment and complete the mission by controlling the UAV/UGV.
To more clearly illustrate the proposed structure framework, a simplified-rescuing scenario is shown in Fig. 4: The UAV is mainly equipped with a visual sensor, which acquires environmental information. The UGV is mainly equipped with data transmission and performs a rescue mission. The start point is the location of UGV, the end point is the location of rescuer. Others are treated as obstacles.

Mapping with vision sensor
Map-building method includes target searching and obstacle recognition.

Target searching in a simplified-rescuing scenario
The method of target searching is to find an optimal transformation by comparing the histogram, feature, texture, structure, greyscale etc. [13]. The general process of target searching is shown in Fig. 5.

Image greyscale
The image is acquired, when the UAV hovering on the target, the image is a RGB model. In order to adapt human sensitivity, it is necessary to quantify the colour value of image [14]. Greyscale is a weighted average method. Wr = 0.3, Wg = 0.59, Wb = 0.11. Thus, the result is as follows:

SURF algorithm
As the SURF algorithm has strong robustness and scale invariance in image features extracting [15]. The general idea of SURF algorithm is shown as follows: i. Integral image. The integral image value of i(i′, j′) is shown as follows: where p(i′, j′) is the greyscale value. ii. determinant of Hessian approximation. The size of template is 9 × 9 and σ = 1.2 are used to filter and detect. D xx , D yy , D xy are the convolution results of template and image. Thus, the Hessian matrix H is given as The box filter and integral image are adopted. Indirect method of increasing the size of box filter template is applied to filter. iv. SURF algorithm feature descriptors. Taking the feature point as the centre, the 20s × 20s image is divided into 4 × 4 pixels with a 5s × 5s rectangle region. ∑ d x , ∑ d y , ∑ d x , ∑ d y are the Haar wavelet response values of each rectangle in each region. v. Matching the target. In order to achieve rapid matching, the Laplace response sign of feature points is added. Only the same response point can match successfully.
By the above-mentioned steps, the result of target searching is shown in Fig. 6.

Obstacle recognition by image segmentation
Image segmentation is to divide the image into several regions with unique properties and find interesting regions [16]. i. Spatial transformation. The environmental information collected by UAV is easily affected by light. Therefore, the hue, saturation, value (HSV) spatial model is applied. ii. Image binarisation. In order to show the image information of obstacles, the image binarisation is used to mark obstacle [17].
The image binarisation is shown as follows: where f (x, y) is the image grey value. T is the grey value threshold. The result in HSV space is shown in Fig. 7.
According to the SURF algorithm and image segmentation, the image information has transformed into executable map through a coordinate transformation. The map is shown in Fig. 8.

Coordinate transformation
In Section 3, the pixelated map has to be gained. In order to realise the global path planning, the map in pixels should be converted to a metric map. UAV and UAV coordinate systems are transformed into the same world coordinate system [18]. The main coordinate systems are shown in Fig. 9: O 0 uv is the pixel coordinate system, O 1 X 1 Y 1 Z C is the image coordinate system, O C X C Y C Z C is the camera coordinate system, O G X G Y G Z G is the UGV coordinate system, and O W X W Y W Z W is the world coordinate system.

UAV's coordinate transformation
To avoid errors caused by camera motion, the camera coordinate system has simplified to the UAV coordinate system and the UAV is taken as a rigid motion model. The Homogeneous transformation is given as where K is the Camera Intrinsics. Rotation matrix R 1 and translation vector t 1 are the exterior of camera.

UGV's coordinate system transformation
The Homogeneous transformation of the UGV is shown as follows: where R 2 is the rotation matrix and t 2 is the translation vector.

Global path planning
In Section 4, the traversable map is gained. In this section, the global path planning of UGV is discussed. First, the traditional Bidirectional Rapidly exploring Random Trees and the A* algorithm are introduced and analysed. Then, the optimised A* algorithm is proposed.

Bidirectional rapidly exploring random trees
Bidirectional rapidly exploring random trees (BRRT) is an improved algorithm based on rapidly-exploring random trees (RRT) [19]. It is aim to find a feasible path from the start point q init to the goal q goal . The basic structure of the RRT is shown in Fig. 10.

A* algorithm
A* algorithm is a heuristic algorithm with optimality and high efficiency. A* balances the distance between the start and the goal [20]. The function of the A* algorithm is shown as follows: where f(n) is the total movement cost function. g(n) is the exact cost of the path from the starting point to the node n. h(n) is the heuristic estimated cost from the node n to the goal.

Optimised A* algorithm
For the UGV to track the feasible path, the main step is to ensure the path smoother. The optimised algorithm A* algorithm is to eliminate the influence of the vertices [21]. The basic function of the optimised algorithm A* algorithm is shown as where p i is the coordinate position before optimisation. p i ′ is the coordinate position after optimisation. α and β are constants and Z is the minimum distance. For α, the following iteration is used: For β, the following iteration is used: Considering the maximum curvature limit of the UGV, α = 0.5 and β = 0.2 are chosen to optimised A* algorithm.

Simulation in a simple experiment
In order to verify the superiority of the optimised A* algorithm, a simple experiment is obtained. Simulation results are shown in Fig. 11. In this experiment, the real time and feasibility of the three path-planning algorithms mentioned in Section 5 are tested. Twenty experimental results are compared and analysed with the same starting point (50, 650) and the same ending point (650, 50) in 700 × 700 pixels experiment. Simulation results are shown in Fig. 12 and Table 1. Table 1 shows the average results of 20 groups. From the results, we can see that the average shortest path length is the optimised A* algorithm, the average shortest computing time is A* algorithm. Although the optimised A* algorithm adds a little calculating time compares to the A* algorithm, most important is that the optimised A* algorithm reduces the number of vertices to 0. Therefore, the optimised A* algorithm has a stable performance in shortening length and smoothing path. It is more suitable to design a real time and feasible path for UGV to track in a real environment.

Simulation in a complicated experiment
In order to further verify and apply the proposed algorithms. More complicated conditions are set. In the complicated experiment, the information of environmental is obtained by UAV in real environment. Fig. 8 has showed the result of mapping. Figs. 13 and  14 show the trajectory in a complex environment. Fig. 13 shows the complete trajectory. Fig. 13 shows part of the trajectory, it aims to show the vertices are optimised by the optimise A* algorithm. Table 2 shows the average results of 20 groups in the complex environment.
The results show that the generated path by the optimised A* algorithm is better than the A* algorithm with shorter distance, smoother trajectory. Most important is that the generated path can apply in practice.

Simulation in a physical experiment
All the algorithms are applied in a physical experiment. The result of target recognition is shown in Fig. 15.
Base on the result, the optimised A* algorithm was used to plan a feasible trajectory, the result is shown in Fig. 16.

Conclusion
In this paper, a typical rescue application is considered to study the map building and path planning. Image pre-processing methods were used to improve the recognition accuracy of the target and obstacles. Especially, the SURF algorithm and image segmentation have the strong robustness in map building. In addition, the HSV spatial model is more suited for illumination environment. Furthermore, the coordinate transformed system is to convert the map in pixels to a metric map. Finally, the designed path by the optimised A* algorithm is shorter and smoother for the UGV to track. All experimental results prove that the practicability of the proposed algorithms are useful.