Application of Multi-sensor Information Fusion Based on Improved Particle Swarm Optimization in Unmanned System Path Planning

— Intelligent vehicle driving performance is safe and stable, which can significantly improve the efficiency of road traffic and reduce energy consumption, and intelligent vehicle is also the development direction of modern transport. Its core technology is intelligent environment perception module, by using a variety of sensors on the car in which the surrounding environment for data collection, processing module to provide effective control for the basis. In this paper, a new SINS / CNS / GPS integrated navigation observation equation is proposed, and a new federated data fusion structure is designed for the integrated navigation system. The particle filter is used to fuse the multi-source data of the federated filter subsystem, thus eliminating the limitations of the classical Kalman filter. The traditional Kalman filter structure and the federal particle filter mechanism are designed. The comparison shows that the proposed algorithm is effective in the information fusion of the integrated navigation system, and the filtering effect is superior to the traditional filtering method.


Introduction
With the progress of society, the car has become an essential travel means of transport, vehicle congestion, traffic accidents and other issues are increasingly apparent. The rapid growth in the number of cars is caused by the low efficiency of public transport which leads to frequent traffic accidents. The establishment of a modern intelligent transportation system will be mentioned on the agenda. Intelligent Vehicles, as an important part of Intelligent Transportation Systems, is also the main body of the system, which not only improves driving safety and road traffic efficiency, but also reduces energy consumption [1][2][3]. Due to many advantages, the technology research has been increasingly concerned about the relevant institutions at home and abroad. Intelligent transportation system can effectively relieve traffic pressure, rational allocation of public transport resources and road resources. Based on machine sensing technology and control technology, the driving system uses information transmission technology and computer vision technology helps to monitor the road model in any environment. In this paper, a new type of fault-tolerant information fusion algorithm is named federated particle filter, which can deal with non-Gaussian and nonlinear systems is adopted [12]. The algorithm uses particle filtering to deal with the multi-source data of the federated filtering subsystem.

2
Material and methods

Establishment of Integrated Navigation Model
In order to accurately plan the navigation path of the unmanned system, the state of the system needs to be accurately described [13]. Here, the state variable is selected as the attitude error, the velocity error and the position error of the platform. The state equation is as follow.
And ( ) is used as state variables, E, N, U represent geographic coordinate system in the east, north, day three direc- " represents position error. The equations (2) to (4) are the attitude error equations of the platform. The equations (5) ~ (7) for the system speed error equation is shown as follow.
In the equations (5) to (7), the components the carrier axis drifts randomly [14,15]. The acceleration ax ay az ! ! ! is the acceleration on the carrier x, y and z White noise.
The equations (8) to (10) are the system position error equations.
In the formulas (8) to (10), the latitude error is !L , !! is a longitude error and !h is used as a height error [16].
iJOE -Vol. 13, No. 8, 2017 The equation (1) is discredited to the given formula (11) between time k-1to time k, 1 k ! W is the system noise (r dimension) at time k-1, 1 k ! " " is the system noise matrix ( N r ! order).

Integrated Navigation Model Measurement Equation
! means the quaternion product operation, n is the navigation coordinate system, ' n , '' n indicates the actual geographic coordinate system.
The attitude quaternary of the carrier of the star sensor % & is used for the star sensor observation error corresponding to the attitude quaternion.
Based on the above parameters can be obtained measurement equation. And 2) In the INS / GPS integrated navigation algorithm, we select the speed and position given by the inertial navigation system, and the velocity and position difference given by the GPS are measured. ( ) And ( )

Combined positioning path planning
In order to compare the filtering effect with the common Kalman filter, this paper designs the Kalman filter structure and the structure of the federal particle filter.
Kalman filter. Kalman filter basic equation gives state one -step prediction equation.
We can get one step prediction of mean square error equation.
Filter gain equation is here.
State estimation equation is expressed as follows. (20) Estimation of Mean Square Error Equation is given here.
The equations (17) to (21) are the basic equations of Kalman filter for stochastic linear discrete systems. Figure 1 describes the above discrete Kalman filter algorithm, we can see from the figure Kalman filter which has two calculation loops [19,20]. Among them, the gain calculation circuit is an independent calculation circuit, and the filter calculation circuit depends on the gain calculation circuit.
INS/CNS/GPS integrated navigation system block diagram is shown in Figure 2. Path planning of federated particle swarm optimization algorithm. Particle Swarm Optimization is a non-parametric Monte Carlo simulation method which helps to implement recursive Bayesian filtering. It can also be applied to any nonlinear system which can be described by the state space model. The precision can be approximated to the optimal estimate. Figure 3 is a schematic diagram of a particle swarm filter algorithm that describes the three steps of time update, observation update, and re-sampling contained in a particle swarm filter algorithm. The prior probability of time is approximated by the particle with the weight value. During the time 1 k ! updating process, the state of each particle N is predicted by the system state transition i k x ! . After observing the value, update the particle weights is ! w k (i ) . The resampling process discards smaller particles and replaces them with larger weights, and the weights of the particles are reset to 1/ N .  (4) Re-sampling. The particle group{ ! x k (i ) , ! w k (i ) } is used to re-sampling after the par- The problem of weight degradation in particle swarm optimization is unavoidable. Although the re-sampling method can alleviate the weight degradation phenomenon to a certain extent, the re-sampling method can also cause some other problems. Resampling needs to be integrated with all the particles to achieve, limiting the parallel computation of particle swarm optimization. In addition, according to the principle of re-sampling, the larger weight of the particles will be more of the selected copy [21]. After a number of iterations, it will inevitably lead to the same particles more and more, the lack of diversity of particles, particle degradation may occur, So that the state estimation produces a large deviation. In order to solve the problem of highdimensional non-Gaussian, nonlinear system's state and parameter estimation, particle filter is introduced into the federated filter, and the federated particle filter algorithm is obtained [22]. Its structure is shown in Figure 4.  Figure 4 in the public reference system, its output on the one hand directly to the main filter, on the other hand output to the sub-filter as a measurement value. The sub-filter is used to calculate the n-group local optimal estimate ( ),

1, 2,
i i X P i n = ! of the system by the particle filter according to the state equation and the measurement equation. Then, the n-group local optimal estimateˆ, g g X P is sent to the main filter for global optimal information fusion. Finally, after each filtering phase is completed, the global filters will feedback the resultsˆ, g g X P to each sub-filter and the main filter according to the principle of "information conservation".

Particle fitness function
In this paper, based on the path length as the fitness function, the safety and smoothness are added, and all the parameters are weighted average to meet the requirements of unmanned system path planning in complex environment.
1) The planned path is as short as possible and the length of the path can be expressed by follows.

Paper-Application of Multi-sensor Information Fusion Based on Improved Particle Swarm Optimiza…
And 1 f represents the sum of the straight-line distances between all adjacent vertices in a particle, ( , ) i i x y represents the current coordinates of the particle, is representing the coordinates of the particle next position.
2) The introduction of the penalty functions helps to improve the safety of the path. When the robot collides with the obstacle, a penalty function is introduced on the path length of the unmanned system. The more the obstacle is, the greater the penalty is imposed, and the smaller the probability of the path is generated. The penalty function is expressed as follows. 2 1 Where N is the sum of the number of obstacles on the straight path from the starting point to the end point, and M represents a given constant term.
3) Introduce path smoothness. Unmanned system in the course of the process of walking diagonal route can shorten the length of the path to save time, but change the direction of motion of unmanned systems which will be time-consuming, in order to make the system to complete the task of the shortest time; this paper introduces the path smoothness Formula. n is expressed as the number of times the robot which is rotated by 90 ° in the right-angled path, and r is the radius of the robot. The comprehensive fitness function is 1 2 3 f f f .
In the formula, the weighting factor for each function is of which any real number greater than or equal to zero. By adjusting , you can adjust is converted to a conventional mathematical model with only the path length as a fitness function [23]. In the formula 2 f ! and 3 f as a penalty judgment function and smoothness correction function, the value is generally less than the order of magnitude.

Discussions
In a complex road environment, a single sensor has its limitations. We just install a single sensor which is difficult to provide a comprehensive description of the road environment, so the design of intelligent vehicles must be equipped with a variety of sensors. Such as the night when the infrared sensor is essential, radar detection of the surrounding obstacles near the machine vision in addition to daily applications and other sensors can be combined with the intelligent vehicle driving safety which can be more reliable. With the development of computer information technology, communication technology, control technology and electronic technology, the application of multi-sensor information fusion technology in intelligent vehicle technology research has made many exciting achievements, such as automotive systems interconnection technology, the European Peugeot system, the US IVHS system. Tsai-Hong Hong and other use of laser sensors to collect images to obtain the distance information in front of the vehicle, in the normal road environment, the use of color cameras and laser sensors to jointly understand the road surface and locate the road boundary. These technologies have been continually improved and are believed to have revolutionized the automotive industry in the near future.
On the two-dimensional plane, the working environment of the unmanned system is modeled by the improved raster method. The effectiveness of environmental modeling is critical to the efficient planning and obstacle avoidance of unmanned systems. The advantages of raster methods are the ease of modeling and the ability to use multiple grids to represent multiple obstacles. Reducing the damage caused by obstacle modeling in the feasible area is necessary. In order to ensure that the unmanned system can move without collision in the environmental model, the model is simplified to a small particle size, and the actual size of the unmanned system is converted into the area of the obstacle. In this paper, the boundary of the obstacle is expanded outward according to the actual size of the unmanned system. If there is an obstacle in a grid, the black raster is defined as a barrier grid, which is expressed as 1; otherwise, the white is a free raster, which is expressed as 0. The use of multiple grids to represent different shapes of obstacles, the free grid is merged into the reach of the unmanned system area.
In this paper, when the space is modeled, the obstacle in the space is treated as follows. The actual obstacle is shown in Figure 5 (a), and the obstruction after treatment is shown in Figure 5  In the established spatial model, the particle swarm optimization algorithm is used to find an optimal path. Each particle in the particle swarm algorithm has a solution, that is, a path, particle swarm algorithm from a large number of solutions is used to find an optimal solution and form the optimal path.
When planning a path, it is necessary to determine the best path based on the effective particle to determine the effectiveness of the particle. The validity of a particle means that the rectangular area between any two adjacent elements in the particle can be freely connected and there is no obstacle in the middle. That is, the particles satisfying the constraints and passing the grids as free grids are valid particles, as long as any two adjacent elements in the particle are not free to connect or the path in the same row (same column) in the grid. The particles are invalid. Figure 6 is an effective path in the path planning process.

Results and analysis
In this paper, the simulation software Matlab is used to simulate the computer. The simulation parameters are as follows, population size is the number of particles M = 100, the maximum particle velocity max v = 20, the maximum number max 500 k = of iterations is the acceleration factor 1 2 4 c c = = of algorithm, the maximum inertia weight is max 0.85 w = , the minimum inertia weight is min 0.3 w = , and the fixed inertia weight is The Kalman filter algorithm and the federated particle swarm algorithm are simulated by using the grid model. The simulation results in the complex obstacle environment model are shown in Figure 7 and Figure 8.
From the simulation results can be seen, the unmanned system from the starting point "S" to the end of the "T", the two algorithms can achieve the ability to find and avoid obstacles. However, Kalman filter algorithm unmanned system path planning is easy to fall into the local optimal, too much in the obstacle avoidance to consider the impact of obstacles, while ignoring the real-time, and the federal particle swarm filter unmanned system path planning can jump out of the local best. In this paper, considering the influence of the edge of the obstacle, the safety and smoothness of the path length are introduced, and the simulation is 80 times. The experimental record is shown in Table 1.  It can be seen from Table 1 that the optimal path of the federated particle swarm algorithm is the shortest. Due to the introduction of the fitness function, the convergence speed of the federated particle swarm algorithm is larger than that of the Kalman filter in the robot path planning application, and the optimal path is shorter and the running time is also shorter. The federated particle swarm algorithm is superior to the Kalman filter algorithm regardless of the optimal path length or running time.

Conclusions
In the intelligent vehicle environment perception module technology research, the sensor is the key of intelligent vehicle control system. How to make the sensor technology better applied to the automotive industry up, the future will become a sensor technology research in the field of development. The integration of various types of sensor technology to provide intelligent vehicles with more reliable and reliable road environment information, the development of intelligent automotive technology is essential. Due to the actual application of the information obtained by the environment is mostly uncertain information, sensor feedback information fusion to restore the real road conditions there are still great difficulties.
Multi-sensor information fusion Federal particle swarm filter as the realization of unmanned system path planning is based on parallel filtering technology and information sharing principle, so it has the advantages of good real-time, fault-tolerant and high precision. In this paper, a highly potential nonlinear filtering algorithm which is a particle swarm filter is introduced into the federated filtering structure, and it extends the federated filtering to solve the system state, parameter estimation and path planning in nonlinear complex environments. In the field, computer simulation proves that this method can not only find the optimal path, but also the algorithm is easy to implement and it can get better results than the traditional Kalman filter algorithm.