Research on obstacle avoidance algorithm for unmanned ground vehicle based on multi-sensor information fusion

: With the wide application of unmanned ground vehicles (UGV) in a complex environment, the research on the obstacle avoidance system has gradually become an important research part in the field of the UGV system. Aiming at the complex working environment, a sensor detection system mounted on UGV is designed and the kinematic estimation model of UGV is studied. In order to meet the obstacle avoidance requirements of UGVs in a complex environment, a fuzzy neural network obstacle avoidance algorithm based on multi-sensor information fusion is designed in this paper. MATLAB is used to simulate the obstacle avoidance algorithm. By comparing and analyzing the simulation path of UGV's obstacle avoidance motion under the navigation control of fuzzy controller and fuzzy neural network algorithm, the superiority of the proposed fuzzy neural network algorithm was verified. Finally, the superiority and reliability of the obstacle avoidance algorithm are verified through the obstacle avoidance experiment on the UGV experimental platform.


Introduction
Unmanned ground vehicles (UGVs) have the characteristics of high movement stability and carrying capacity, simple mechanical structure, fast movement speed, high movement flexibility and high work efficiency [1]. UGVs can perform tasks that difficult to be completed by human beings in a complex and harsh environment, which has attracted attention in more and more fields [2,3]. In the military field, UGVs can replace humans in high-temperature, high-radiation, or other high-risk environments to perform tasks such as reconnaissance, search and rescue, and explosion elimination [4]. In the civilian field, UGVs can replace security personnel to complete frequently repeated patrol tasks [5]; cleaning robots can enter the small gaps in the home environment to complete cleaning tasks [6].
The obstacle avoidance function is one of the important criteria for measuring the intelligence of UGVs [7], and it is also a research hot spot in the field of UGVs [8,9]. During the past few years, the studied of obstacle avoidance algorithm has produced outstanding results. For example, Jesus Savage et al. [10] combined genetic algorithm with periodic neural network to overcome the local minimum problem in the process of robot avoidance. Anish Pandey et al. [11] combined fuzzy reasoning, neural network algorithm and adaptive algorithm to independently design a reasoning system used in a static environment. The input of the reasoning system is the distance detected by the sensor system, and the output is the steering angle of the next movement of the robot, thus realizing the obstacle avoidance function during the autonomous movement of the robot. Kim and Chwa [12] proposed a fuzzy neural network(FNN) control algorithm using interval two fuzzy membership function, which increased the degree of freedom for uncertainty, and proved that the algorithm has a smaller linear velocity and angular velocity, making the obstacle avoidance path smoother. Shidrokh Goudarzi et al. [13] planned the shortest path based on the travel sales problem, aiming at the problem that UAV need smooth path. Bessel curve is used to transform the flyable path to realize the path smoothing. This method can efficiently and effectively collect data with high packet transmission rate and low energy consumption.
The working environment of UGVs is changeable and uncontrollable [14], which makes robots have stricter requirements on the accuracy of obtaining information from the surrounding environment [15]. The multi-sensor detection system contains a variety of different sensors, which can use a variety of different methods to detect the environment from multiple angles [16,17]. In recent years, multi-sensor information fusion technology has been widely studied and applied. For example, Sang Won Yoon et al. [18] used Kalman filter to fuse the information of speed sensor and positioning sensor in order to realize the coordinate calibration of UGV in the case of sliding. Risang Gatot Yudanto et al. [19] designed an extended Kalman filter information fusion algorithm based on velocity sensor and angle sensor, and realized the position tracking of robot with high dynamic motion. Alastise et al. [20] made use of extended Kalman filter to integrate the information of inertial measurement unit and visual sensor, to achieve accurate positioning of UGVs. Baofeng Ji et al. [21] proposed a two-hop cognitive network with the transmitter as a special radio frequency source to collect radio frequency energy from secondary user source nodes and secondary user relay nodes. Tao Tang et al. [22] discussed the important role of UAV in the future 5G Internet of Things, and proposed a UAV-PHD filter. They used KNN and K-means algorithms to improve GM-PHD algorithm, and applied it to target detection and tracking of UAV, and realized the trajectory tracking of multiple UAV targets.
The existing information fusion technology has poor processing capability for the information obtained by the sensor and long calculation time, which cannot meet the accuracy and real-time requirements of the UGV's obstacle avoidance function [23,24]. Therefore, the study of multi-sensor information fusion algorithms with high information fusion accuracy and strong real-time performance is one of the important components to improve the obstacle avoidance level of UGVs, and has very important research value [25,26]. This paper analyzes and designs a FNN algorithm to avoid obstacles for the UGV. We designed the adaptive weighted fusion algorithm to process the information obtained by the sensors, and the result of the multi-sensor information fusion algorithm is the input of the FNN. The rest of the paper is organized as follows: Section 2 introduces the UGV and establishes the kinematics model. Section 3 analysis the multi-sensor information fusion algorithm and the FNN algorithm. Section 4 shows the simulation experiment and discusses the result. Section 5 shows the real experiment and discusses the result. Section 6 concludes the paper.

Unmanned ground vehicle experiment platform
The kinematics model of UGV is shown in Figure 1 [27], in the coordinate system (x0y), let (x0, y0) represent the center of mass of the UGV, let θe represent azimuth angle of UGV, which is the angle between the forward direction of the robot and the x-axis, θe∈[-π， π)； ωe and ve respectively represent the angular velocity and linear velocity of the UGV. The matrix representation of the kinematics model of UGV is presented in Equation (1) [28]. [ where Δt represents the sampling time during the movement. We can get that when the rotation speed of the left wheel of the UGV is equal to the rotation speed of the right wheel, and the direction is opposite, the turning radius of the UGV is 0, and the UGV rotates in situ around its particle. The kinematics model of the UGV is the basis of the robot motion control strategy. It provides the theoretical basis of kinematics for the MATLAB simulation of the subsequent obstacle avoidance control algorithm, and provides theoretical support for the motion simulation of the robot.

The adaptive weighted multi-sensor information fusion algorithm
According to the basic principles of the adaptive weighting algorithm and the characteristics of each sensor, distinguish the degree of influence of the data obtained by each sensor on the fusion result [29]. Then assign a certain weight to the output data of each sensor in the sensing detection system. The total variance of the weighted fusion result of all data output by the system at one time is used to evaluate the reliability of the algorithm [30]. The model structure of the algorithm is shown in Figure 2. As shown in Figure 2, the actual value of the target feature quantity of the sensor detection system is X. In actual measurement work, the output values of each sensor are X1, X2, … Xn, the measurement variances of each sensor are σ1, σ2, … σn, the weights assigned to each sensor are φ1, φ2, … φn. According to the model of information fusion algorithm, we can get The total variance is According to Equation (3), the fusion variance σ 2 obtained by the system once detected is a quadratic function relationship with the weight φi. Therefore, when the fusion variance is the smallest, the accuracy of the fusion result is the highest, and the sensor has been given the most appropriate weight φ [31]. Using Lagrange product method to solve the conditional extreme value problem to obtain the weight when the total variance is minimum as The φ i obtained by Equation (4) is the most suitable weight that minimizes the data fusion error under the condition that the sum of all the values is 1. Substituting Equation (4) into Equation (3) can get the data distribution after fusion as After obtaining the fusion weight of the sensors in the system from Equation (4), substituting into Equation (2) to obtain the data fusion result after one measurement. However, from Equation (4), it can be known that if the value of each sensor is required for each detection, the variance of each sensor needs to be known [32].
If Xi(r) represents the output result of the i th sensor when the sensing detection system works for the k th time, the average value of the k th output of all n sensors in the system is calculated as The average value of the variance of each output result of the corresponding sensor included in the information fusion range is obtained, and then used as the final variance value of the i th sensor at the k th measurement sampling.
Substitute σ i 2 (r) obtained in Equation (7) into Equation (4) to obtain the weight φi(r) of the i th sensor at the r th measurement sampling. Substitute the weight φi(r) into Equation (2) to obtain the data fusion result X ̅ of the i th sensor at the r th measurement sampling.
The multi-sensor information fusion algorithm can process the environmental information obtained by the sensors installed on the UGV, reduce the impact of environmental noise and sensor measurement errors on the data, and input it into the trained FNN. During the obstacle avoidance movement of the unmanned vehicle, the distance information transmitted to the fuzzy neural network is the information after multiple sampling and fusion. Therefore, this algorithm is suitable for the fusion processing of the data collected by the sensor several times in a certain time.

Design of fuzzy logic controller
Because there are many uncertain factors in the complex working environment of UGVs, it is impossible to establish accurate mathematical models, resulting in many commonly used obstacle avoidance algorithms that are not suitable for UGVs [33]. The fuzzy control algorithm can use the form of fuzzification to transform the information that cannot be accurately described in the environment into the expression form acceptable to human beings, and use professional experience in related fields to reason, and obtain control instructions that meet the system requirements [34].

Determine input and output variables and fuzzification
The distance between the UGV and the obstacle and the azimuth angle of the target point relative to the robot are used as input parameters. The distance d between the UGV and the obstacle is divided into near and far, denoted by N and F respectively, which is {near，far} = {N，F}. The domain is [0,100 mm]. The UGV has five sets of sensors for distance measurement, so d includes {d1, d2, d3, d4, d5}. When the measuring distance is greater than 100mm, it will be treated as 100mm. The membership function with fuzzy parameter N is defined as Equation (8), and the membership function with fuzzy parameter F is defined as Equation (9).
Where cij is the central value and σij is the width value. Set the azimuth angle θ of the target point relative to the UGV as the left side, left front side, front right side, right front side and right side, denoted by L, LF, FR, LR, and R respectively, which is {left side, left front side, front right side, right front side, right side} = {L, LF, FR, RF, R}. The domain is [-80°, 80°]. When the angle is negative, the target point is on the left side of the UGV; when the angle is zero, the target point is in front of the UGV; when the angle is positive, the target point is on the right side of the UGV. The membership function is defined as Equation (10). The membership function graph of the input quantity is shown in the Figure 3.  The output parameter inferred by the fuzzy logic controller is set as the deflection angle of the robot, denoted by TG. The fuzzy logic controller designed in this paper divides the output deflection angle TG into five parts: left side, left front side, front right side, right front side and right side, denoted by TL, TLF, ST, TRF and TR respectively, which is { left side, left front side, front right side, right front side, right side}={TL, TLF, ST, TRF, TR}. The membership function is defined as Equation (10). The membership function graph of the input quantity is shown in the Figure 4. When the angle is less than 0, the robot is controlled to turn left; when the angle is equal to 0, the robot travels in a straight line; when the angle is greater than 0, the robot turns to the right.

Designing Fuzzy Rules
Based on the membership functions of the fuzzy set and inputs and outputs, rules are defined. There are 160 rules for obstacle avoidance of the UGV. We can use the form of 'IF…THEN' to express the rules. For example, the number 6 rule is 'IF (d1 = F, d2 = N, d3 = F, d4 = F, d5 = F, θ = L), THEN (TG = TL)', which means that there is an obstacle in the front left of the UGV and it is closed to the UGV, the target point is on the left side of the forward direction, so the UGV needs to turn left. In addition, Table 1 lists some fuzzy rules.

Defuzzification
The last step of designing the fuzzy logic fusion system is the defuzzification process where outputs are generated based on fuzzy rules, membership values, and a set of inputs. The method used for defuzzification is the Centroid method.

Design of fuzzy neural network
However, the fuzzy control algorithm has the shortcomings of lack of independent learning ability, complex knowledge base construction, and the control rules can't be adjusted effectively according to the changes of the environment, and it cannot fully meet the needs of UGVs to avoid obstacles [35,36]. The neural network algorithm can optimize the parameters in time according to the actual input data through the learning mechanism, and has a strong ability to adapt to the environment [37]. But the neural network also has the problem of high computational complexity when the input data is nonlinear [38,39]. We combined the fuzzy control algorithm with the neural network algorithm, and designed the FNN algorithm to realize the obstacle avoidance function of the UGV.
The structure of FNN is shown as Figure 5. The first layer is the input layer, whose function is to directly transmit the input data to the next layer. The number of neurons in this layer is N1 = 6, that is, there are six input variables. d1, d2, d3, d4, and d5 represent the distance vectors detected by the sensors in the five directions, and θ is the azimuth angle of the target relative to the robot. The second layer is the fuzzification layer, the parameters of each node are fuzzy variables and the corresponding membership results. According to the previous design, one input parameter of distance corresponds to two fuzzy language variables, and one input parameter of angle corresponds to five fuzzy language variables. Therefore, the first five neurons in the first layer correspond to 2 neurons in the second layer, which is m1 = m2 = m3 = m4 = m5 = 2. The sixth neuron in the first layer corresponds to the 5 second-layer neurons. The total number of neurons in this layer is N2 = 15.
The third layer is the reasoning layer, which corresponds to the fuzzy reasoning process, and each fuzzy rule corresponds to a node. N 3 . The function of the third layer is to obtain the matching degree of the corresponding fuzzy rules by using the membership degree of each fuzzy variable in this cycle, which is where are i 1 ∈{1,2}, i 2 ∈{1,2}, i 3 ∈{1,2}, i 4 ∈{1,2}, i 5 ∈{1,2}, i 6 ∈{1,2,3,4,5}, j=1, 2, ⋯, N 3 . The fourth layer is the normalization layer to prepare for the clarity of the fuzzy results, which is where are j = 1, 2, ⋯, N 3 . Number of neurons in this layer is N4 = N3 = 160。 The fifth layer is the defuzzification layer, which realizes the defuzzification calculation, which is The output is the steering angle, and the weight ω j is the central value in the membership function of the linguistic variable corresponding to the inference result of the fuzzy rule.
The complexity of neural network can be divided into space complexity and time complexity. The number of layers and the number of parameters that need to be optimized are used to represent the space complexity. Time complexity is usually represented by multiplication and addition operations in neural networks. According to the structure of fuzzy neural network, we can get that the number of layers with computing power is 5, the number of parameters that need to be optimized is 26.
The time complexity of fuzzy neural network can be calculated layer by layer. If we only have one training sample, the time complexity is 15 + 160 + 160 + 160 + 1 = 496. Assuming that the actual number of training samples is m, the time complexity of the whole algorithm is O(496m) = O(m).

Parameters Optimization Process
Using neural network training to optimize the central value cij and width value σij of the membership function in the second layer of the neural network, as well as the weight wj between each node. There are 15 central values cij, which correspond to the neurons of the second layer one-to-one; a total of 6 width values σij, which are the same as the number of input parameters; and a total of 5 weights wj, which correspond to the number of elements in the fuzzy language set of the output parameters. The training of the neural network adopts the feedforward adjustment method and the gradient descent method of error back propagation. The weight adjustment formula is where η is the learning rate. The flow of FNN algorithm is shown in Figure 6.

Simulation of fuzzy neural network algorithm and discussion
Formulating fuzzy inference rules based on the input parameters, output parameters and their corresponding fuzzy language variables as mentioned above. Selecting the initial value of the weight parameter and establish a fuzzy controller. The initial value of the weight parameter is shown in Table 2.
Using MATLAB software to establish a static multi-obstacle simulation environment. Before the neural network is used to optimize the weight parameters, a fuzzy controller is used to conduct the simulation experiment of obstacle avoidance motion of the UGV. When the rotation speed of the left wheel of the UGV is equal to the rotation speed of the right wheel, and the direction is opposite, the turning radius of the UGV is 0. Therefore, the position of the robot does not change when turning. We have established a robot sports field in MATLAB. There are nine obstacles of different shapes randomly distributed in the field. The robot needs to move from the lower left corner to the upper right corner of the field. Since the UGV designed in this article can turn in a fixe place, the influence of the rectangular structure on the obstacle avoidance function can be ignored in the simulation experiment, and the UGV was assumed to be a circle. In the simulation process, the robot's working sequence is to first measure the distance between the robot and the obstacle, the fuzzy controller calculates the steering angle, and the robot advances one step according to the steering angle. According to the actual distribution of sensors, the distance in five directions needs to be calculated for each advance.
The obstacle avoidance motion path of the UGV and the angle at which the UGV turns each time is shown in Figure 7. The blue circle in the lower left corner of the figure represents the starting position of the UGV, and the red cross in the upper right corner represents the target position of the UGV. The simulation result of the fuzzy control algorithm navigating the obstacle avoidance movement of the UGV is shown in Figure 7. The simulation results show that there is an intersection point between the robot's motion path and the obstacle. Although there is a certain distance between the particle representing the robot and the obstacle, there is an intersection point between the figure representing the contour of the robot and the obstacle. The location of the collision is indicated by the red circle. The simulation results show that the obstacle avoidance motion of the robot under the navigation of the fuzzy control algorithm cannot completely avoid all obstacles and reach the target position safely. Therefore, using fuzzy control algorithm alone cannot meet the obstacle avoidance requirement of UGV.
Using fuzzy controllers to navigate UGVs in different simulation environments, planning optimal robot motion paths artificially in the simulation environments, the distances between the robot and the obstacle in the path and the relative angle between the robot and the target point are collected. The deflection angle of each advance in the ideal trajectory is calculated as the expected output value of the FNN. Using FNN to optimize the weight parameters, setting the learning rate to 0.001, and the allowable error to 0.001. When the error is less than 0.001 or the number of training times is greater than 3000 times, the training stops. The network weights after training adjustment (retaining two decimal places) are shown in Table 3. According to the optimization result, the value of the weight parameter corresponding to the fuzzy controller is adjusted and substituted into the obstacle avoidance algorithm program to obtain the simulation result of the obstacle avoidance motion path as shown in Figure 8. As shown in Figure 8, the robot can maintain a certain distance from the surrounding obstacles and reach the target point smoothly during the movement. Comparing Figure 7 and Figure 8 we can find that the obstacle avoidance movement path of the robot in Figure 8 is smoother, and the distance between the robot and the obstacle is more obvious. Especially at the apex of the rectangular obstacle, the movement path is significantly farther away from the obstacle. The simulation results show that the designed FNN obstacle avoidance algorithm can realize the obstacle avoidance function and has a better obstacle avoidance effect. We collected the distance between the robot and the obstacle calculated every time the robot advanced, and selected the smallest value among the five distances. The distance between the robot and the obstacle in the simulation experiment under the fuzzy controller and FNN algorithm navigation is shown in Figure 9. By comparing the two curves, the overall trend of the blue curve is above the yellow curve, indicating that the distance between the robot and the obstacle under the control of the FNN algorithm is larger than that of the fuzzy controller. The count of different distance ranges is shown in Figure 10, we can get that the distances between the robot and the obstacle under the FNN navigation are all greater than 10cm. These conformed to the boundary setting of the robot in the simulation experiment, indicating that the robot will not collide with obstacles. In addition, the distance controlled by the FNN algorithm is more distributed in a larger interval. Therefore, we proved that the FNN algorithm has certain advantages compared with the fuzzy control algorithm.

Unmanned ground vehicle
The UGV used in this paper is a wheeled unmanned vehicle designed and developed independently. The UGV compactly installs all the electronic components and protects them with mechanical structure to avoid unnecessary damage caused by the exposed electronic components, especially for the protection of sensors and control center. The overall appearance of the UGV (including all parts and components such as tires) is 260mm×280mm×110mm. The 3D model and the physical picture of the UGV is shown in Figure 11. In order to meet the application requirements of UGVs for avoiding obstacles and moving in complex environments, the robot's environment detection system includes multiple sets of sensors to obtain distance information of obstacles. The obstacle avoidance system includes a total 10 sensors, including five ultrasonic sensors and five infrared sensors. The locations where the sensors are installed are depicted in Figure 1. The 10 sensors were divided into five groups and installed around the robot body. Five groups of sensors are distributed within a 180° range in front of the robot. Each group of sensors consists of an ultrasonic sensor and an infrared sensor, providing a hardware foundation for the application of multi-sensor information fusion technology. In addition, there is an electronic compass installed inside the UGV to measure the deflection angle of the UGV.

The experiment and discussion
In order to verify the designed obstacle avoidance technology based on multi-sensor information fusion technology and FNN algorithm, we designed an obstacle avoidance experiment based on the self-designed UGV platform mentioned above. The effect of obstacle avoidance experiment with the robot were analyzed and discussed to verify the correctness and feasibility of the algorithm.
Imitated the motion environment in the simulation experiment to build the motion environment of the robot obstacle avoidance experiment. The initial motion direction of the UGV was set as a constant 90, that is, it moves towards the front. The steering angle obtained by the UGV in subsequent obstacle avoidance calculation needs to be calculated by adding or subtracting with this constant to get a new angle value, which is then stored in the register of the control core. After the first steering of the UGV, each obstacle avoidance calculation took the angle value as the relative direction of the target point as the input of the fuzzy controller. The purpose of this design is to minimize the impact of the obstacle avoidance process on the original driving direction and keep the original driving direction. We repeated the experiment under the same conditions. The result of the obstacle avoidance experiment is shown in Figure 12 and Figure 13. The black solid lines behind the UGV in the figures are the motion track of the UGV. Figure 12 shows the obstacle avoidance experiment results of the UGV under the navigation of the fuzzy controller designed above. There is little difference between the results of repeated experiments, which means that the fuzzy controller has poor adaptability to the environment. As shown in Figure 12(c) and Figure 12(f), the robot collided with the obstacles twice. In the process of robot movement, the distance between the UGV and the obstacle is relatively close. The fuzzy control algorithm cannot be adjusted according to the actual situation of obstacle avoidance, and the weight parameters used are not applicable to the actual environment. This indicates that the fuzzy control algorithm cannot meet the obstacle avoidance requirement of the robot.  Figure 13 shows the obstacle avoidance experiment of the UGV under the control of FNN algorithm. The experimental results show that the UGV can smoothly avoid obstacles and keep a proper distance from the obstacle in this complex environment, which proves that the robot has a good obstacle avoidance function. By comparing the entire path of motion showed in Figure 12(h) with Figure 13(f), the number of movement direction changes of UGV is significantly less in Figure 13(f), which means the UGV reacted more quickly to obstacles under the navigation control of FNN algorithm. By analyzing the results of obstacle avoidance experiments, the correctness and feasibility of the designed FNN algorithm are verified.

Conclusions
This article designed and verified an obstacle avoidance method based on multi-sensor information fusion technology and FNN algorithm. The motion model of the UGV was established combined the structure and motion form, and the motion control strategy of the UGV was determined. Ten distance sensors and an angle measuring sensor were used for the obstacle avoidance behavior. Then an adaptive weighted multi-sensor information fusion algorithm and a FNN obstacle avoidance algorithm were designed and applied to the UGV. The fusion model designed is based on the fuzzy logic inference system, which is composed of six inputs, one output and 160 fuzzy rules. Multiple membership function for inputs and outputs are chosen. The parameters of membership functions were optimized by the neural network. By analyzing the simulation experiment results and actual experiment results under the navigation control of fuzzy controller and FNN algorithms, the superiority and reliability of the FNN algorithm have been proved.