Sara Bouraine
email: s_bouraine@yahoo.fr

Abdelhak Bougouffa
email: abdelhak.bougouffa@universite-paris-saclay.fr

Ouahiba Azouaoui
email: ouahibaazouaouioazouaoui@cdta.dz

Particle swarm optimization for solving a scan-matching problem based on the normal distributions transform

Keywords: Simultaneous localization and mapping, Scan matching, Particle swarm optimization, Normal distributions transform, Mobile robots, Bio-inspired behaviour, Pose estimation, Robot operating system

In this paper, an evolutionary scan-matching approach is proposed to solve an optimization issue in simultaneous localization and mapping (SLAM). A rich literature has been invested in this direction, however, most of the proposed approaches lack fast convergence and simplicity regarding the optimization process, which should directly affect the accuracy of the environment's map and the estimated pose. It is a line of research that is always active, offering various solutions to this issue. Among many SLAM methods, the normal distributions transform approach (NDT) has shown high performances, where numerous works have been published up to date and many studies demonstrate its efficiency wrt other methods. Nevertheless, few works have been interested to solve the optimization issue. The proposed solution is based on NDT scanmatching using particle swarm optimization (PSO) and it is dubbed NDT-PSO. The main contribution is to solve the pose estimation problem based on PSO and iterative NDT maps. The performances of the NDT-PSO approach have been proven in real experiments performed on a car-like mobile robot in both static and dynamic environments. NDT-PSO is tested for different swarm sizes, and the results show that 70 particles are more than enough to find the best particle while avoiding local minima even in loop closing. The algorithm is also suitable for real time applications, with an average runnnig time of 145ms for 70 particles and 70 iterations of the optimization process. This value can be further reduced using fewer particles and iterations. The accuracy of the proposed approach is also evaluated wrt other SLAM methods widely used among the robot operating system community and it has been shown that NDT-PSO outperforms these algorithms.

Introduction

Today, mobile robots are present in our daily life and they become increasingly autonomous. Indeed, thanks to the ability to locate and map its environment, the robot can plan trajectories and navigate in the real world in order to perform various tasks without human intervention. This ability is known as the simultaneous localization and mapping (SLAM) problem [START_REF] Dissanayake | A computationally efficient solution to the simultaneous localisation and map building (slam) problem[END_REF][START_REF] Hahnel | An efficient fastslam algorithm for generating maps of large-scale cyclic environments from raw laser range measurements[END_REF][START_REF] Murphy | Bayesian map learning in dynamic environments[END_REF][START_REF] Khairuddin | Review on simultaneous localization and mapping (slam)[END_REF][START_REF] Singandhupe | A review of slam techniques and security in autonomous driving[END_REF][START_REF] Cadena | Past, present, and future of simultaneous localization and mapping: toward the robust-perception age[END_REF]. It consists of estimating the robot position while building a map of the environment. Most developed SLAM approaches are under the Filtering [START_REF] Sung-Hyeon | A robust slam algorithm using hybrid map approach[END_REF][START_REF] Choi | Hybrid map-based slam with raoblackwellized particle filters[END_REF][START_REF] Zhang | Convergence and consistency analysis for a 3-dinvariant-ekf slam[END_REF][START_REF] Lee | Efficient ekf-slam algorithm based on measurement clustering and real data simulations[END_REF] or scan-matching [START_REF] Li | Feature-based laser scan matching and its application for indoor mapping[END_REF][START_REF] Wang | Accurate mix-norm-based scan matching[END_REF][START_REF] Wang | Combination of the icp and the pso for 3d-slam[END_REF][14] paradigms but share the same main steps, namely; mapping and pose estimation. The improvement of the earliest approaches in the two categories is still ongoing despite their existence for more than two decades. In this paper, the optimization problem is addressed in particular to solve a SLAM problem. There is a rich literature on optimization algorithms [START_REF] Dokeroglu | A survey on new generation metaheuristic algorithms[END_REF][START_REF] Namadchian | A new metaheuristic algorithm for optimization based on variance reduction of guassian distribution[END_REF][START_REF] Ahmadianfar | Gradient-based optimizer: a new metaheuristic optimization algorithm[END_REF], however, evolutionary algorithms [START_REF] Razmjooy | A comprehensive survey of new meta-heuristic algorithms[END_REF] present better performances to solve this issue, such as artificial bee colony (ABC) algorithm [START_REF] Nedjah | Simultaneous localization and mapping using Swarm intelligence based methods[END_REF], sparrow search algorithm (SSA) [START_REF] Xue | A novel swarm intelligence optimization approach: sparrow search algorithm[END_REF], firefly algorithm (FA) [START_REF] Wu | An improved firefly algorithm for global continuous optimization problems[END_REF], invasive weed optimization (IWO) [START_REF] Razmjooy | An improved quantum evolutionary algorithm based on invasive weed optimization[END_REF], world cup optimization (WCO) algorithm [START_REF] Razmjooy | A new metaheuristic optimization algorithm inspired by FIFA world cup competitions: theory and its application in PID designing for AVR system[END_REF], and particle swarm optimization (PSO) [START_REF] Wang | Particle swarm optimization algorithm: an overview[END_REF]. Among these methods, PSO is one of the most effective algorithms, due to its fast convergence and its ability to find a global optimum [START_REF] Bouraine | Safe motion planning based on a new encoding technique for tree expansion using particle swarm optimization[END_REF].

Indeed, in recent years, particle swarm optimization is increasingly exploited in SLAM-based methods to eliminate the problems of insufficiency and inaccuracy of prior information for Kalman filter methods [START_REF] Zhu | Research and application on fractional-order darwinian pso based adaptive extended kalman filtering algorithm[END_REF] and impoverishment of particles for particle filter methods [START_REF] Lee | PSO-FastSlam: An improved FastSlam framework using particle swarm optimization[END_REF][28][START_REF] Wu | FastSlam method based on gaussian particle swarm optimization[END_REF]. In [START_REF] Zhu | Research and application on fractional-order darwinian pso based adaptive extended kalman filtering algorithm[END_REF], the fuzzy adaptive extended Kalman filtering method has been improved by introducing the fractional-order Darwinian particle swarm optimization (PSO) to compute an accurate prior noise model. Lee et al. [START_REF] Lee | PSO-FastSlam: An improved FastSlam framework using particle swarm optimization[END_REF] proposed a FastSLAM framework where the robot position is estimated using a Rao-Blackwellized particle filter. The accuracy of the Fast-SLAM deteriorates over time due to particles depletion in the re-sampling phase. To prevent degeneration, a particle swarm optimization is employed to solve the problem by means of particle cooperation. Another improvement of the Fast-SLAM is the work of Liu et al. [28] where PSO and unscented particle filters are introduced to reduce drastically the number of particles thanks to PSO for pose estimation and to improve map estimation accuracy thanks to the unscented particle filter. In Wu et al. [START_REF] Wu | FastSlam method based on gaussian particle swarm optimization[END_REF], to solve the degeneration of particles and the positioning inaccuracy (due to the need for a large number of particles) problems, the Gaussian particle swarm optimization algorithm is used in the particle filter process.

In the second category, namely scan-matching methods, most of the methods use a nonlinear least-square optimization to determine the robot pose, but it is subject to the local minima problem which leads to not guaranteeing the algorithm convergence particularly in the presence of dynamic objects or fast movement of the robot. These few last years, some works have been conducted to prevent and improve the scan-matching-based methods. In [14], the ICP and Hector SLAM algorithms are improved using the system model as an initialization step followed by the ICP or Hector as an alignment step. The work in [START_REF] Li | Feature-based laser scan matching and its application for indoor mapping[END_REF] introduces in the ICP algorithm both a 2D laser scan-matching method using point and line features as an initialization phase and the lq-norm ( 0 < q < 1 ) metric as a pose estimation to filter the outliers. In Wang et al. [START_REF] Wang | Accurate mix-norm-based scan matching[END_REF], a mixture of exponential power (MoEP) distributions is used for approximating the residual error distribution. The optimization of the scan-matching method is iteratively achieved via two phases: an on-line parameter learning (OPL) phase to learn residual error distribution for a better representation according to the likelihood field model (LFM), and an iteratively re-weighted least squares (IRLS) phase to attain transformation for accuracy and efficiency. Nevertheless, among many SLAM methods, NDT has shown high performances, where numerous works have been published up to date [START_REF] Biber | The normal distributions transform: a new approach to laser scan matching[END_REF][START_REF] Stoyanov | Fast and accurate scan registration through minimization of the distance between compact 3D NDT representations[END_REF][START_REF] Hong | Probabilistic normal distributions transform representation for accurate 3-d point cloud registration[END_REF][START_REF] Zaganidis | Semantic assisted 3-d normal distributions transform for scan registration in environments with limited structure[END_REF][START_REF] Einhorn | Generic NDT mapping in dynamic environments and its application for lifelong SLAM[END_REF][START_REF] Li | A GMM based uncertainty model for point clouds registration[END_REF][START_REF] Wolcott | Robust LIDAR localization using multiresolution Gaussian mixture maps for autonomous driving[END_REF][START_REF] Schmiedel | Iron: a fast interest point descriptor for robust ndt-map matching and its application to robot localization[END_REF]] also many studies demonstrate its efficiency wrt other methods [START_REF] Magnusson | A comparison of 3d registration algorithms for autonomous underground mining vehicles[END_REF][START_REF] Stoyanov | On the accuracy of the 3d normal distributions transform as a tool for spatial representation[END_REF][START_REF] Saarinen | Normal distribution transform monte-carlo localization (ndt-mcl)[END_REF][START_REF] Pang | 3d scan registration based localization for autonomous vehicles-a comparison of NDT and ICP under realistic conditions[END_REF]. However, to solve the optimization problem, most NDT-based methods use Newton algorithm (like [START_REF] Morita | Point-cloud mapping and merging using mobile laser scanner[END_REF][START_REF] Li | Efficient laserbased 3D SLAM for coal mine rescue robots[END_REF]). Few works have been interested in the problem of optimization; in [START_REF] Schmiedel | Iron: a fast interest point descriptor for robust ndt-map matching and its application to robot localization[END_REF][START_REF] Saarinen | Normal distribution transform monte-carlo localization (ndt-mcl)[END_REF], the pose estimation is performed using Monte Carlo Localization (MCL). In [START_REF] Einhorn | Generic NDT mapping in dynamic environments and its application for lifelong SLAM[END_REF][START_REF] Stoyanov | Point set registration through minimization of the l 2 distance between 3d-ndt models[END_REF], the best fitting alignment between two point samples sets is found through the minimization of the L2 distance between NDT models. Moreover, in other works, the optimal transformation to solve the scan-matching problem is directly formulated as a maximum likelihood estimate of Gaussian mixture maps [START_REF] Wolcott | Robust LIDAR localization using multiresolution Gaussian mixture maps for autonomous driving[END_REF].

The main contribution of this paper is to propose a new NDT-based scan-matching method dubbed NDT-PSO. Among SLAM methods and NDT-based methods in particular, few works have been interested in the problem of optimization in pose estimation. Most approaches use gradient-based approaches such as the least squares or Newton methods, known to be be computationally expensive and sensitive to the choice of the departure position affecting the convergence of the algorithm. However, the proposed approach is based on PSO, where to the authors' knowledge this is the first time that it is used to solve the problem of pose estimation in the SLAM issue. Furthermore, this paper adds a new approach to the NDT's variant family, which overcomes the cited problems.

The principle advantages of NDT-PSO are its efficiency, its fast convergence, and its ability to find a global optimum. Moreover, the problems of insufficiency and inaccuracy of prior information do not arise, as is the case for the filtering methods. Another significant advantage of NDT-PSO is that dynamic objects do not affect the accuracy of the results without the need for additional algorithms. NDT-PSO is also very suitable for real time applications, which is a worry in most evolutionary optimization techniques.

The highlights of the research can be expressed as follows: (1) NDT-PSO is based on a normal distributions transform (NDT) for the environment mapping. This approach overcomes uncertainties and environment constraints. [START_REF] Hahnel | An efficient fastslam algorithm for generating maps of large-scale cyclic environments from raw laser range measurements[END_REF] To determine the estimated position of the robot, the optimization problem in the scan-matching process is solved using a particle swarm optimization approach. It is a key issue in scan-matching-based SLAM methods. The solution is encoded as the best particle in the swarm, representing the best transformation between two successive NDT maps. (3) NDT-PSO is based on a modified version of PSO that incorporates an inertia weight [START_REF] Shi | A modified particle swarm optimizer[END_REF]. (4) It has been tested in real experiment to validate the algorithm in real world conditions on a car-like mobile robot. Experiments have been carried out in both static and dynamic environments. (5) NDT-PSO has been also evaluated wrt other SLAM methods widely used among the ROS community, namely gmapping [START_REF] Grisettiand | Fast and accurate SLAM with Rao-Blackwellized particle filters[END_REF] and Hector-SLAM [START_REF] Kohlbrecher | A flexible and scalable slam system with full 3d motion estimation[END_REF] methods. The obtained results show a high accuracy of NDT-PSO compared to the two other methods.

This paper is organized as follows: Sect. 2 illustrates the proposed approach, where the scan-matching problem is formalized before presenting the NDT-PSO algorithm.

Validations in real experiments are given in Sect. 3. Finally, Sect. 4 provides a summary and a conclusion of the paper.

Proposed approach: NDT-PSO

To solve an optimization problem in simultaneous localization and mapping (SLAM), the proposed solution is based on a bio-inspired approach dubbed normal distributions transform particle swarm optimization (NDT-PSO). Like any SLAM problem, two key issues to answer are environment mapping and pose estimation. The following sections answer these two issues.

Environment representation

In this paper, a normal distributions transform (NDT) based representation is used to deal with uncertainties and environment constraints (presence of objects with arbitrary shapes). Proposed by Biber and Strasser [START_REF] Biber | The normal distributions transform: a new approach to laser scan matching[END_REF], NDT is a refined version of the popular occupancy grid approach [48,[START_REF] Einhorn | Sensor integration for robot navigation: combining sonar and stereo range data in a grid-based representation[END_REF]. As a common point, NDT is based upon a grid discretization of the space; the collected data for a given time step (a 2D laser scan in the case of this paper) are modeled as a set of cells with an appropriate resolution. However, by taking a closer look to a cell, instead of affecting a probability of occupancy to the whole cell, a probability of measurement is associated with each 2D scan point in this cell.

Let M = {q j }, j = 1, … , N M a points set corresponding to a given laser scan, with q j ∈ ℝ 2 . These raw data are trans- formed to NDT-based representation by the following steps:

• As the space is subdivided into a set of cells, each cell with ID "c" will be assigned a sub-set of points m c con- taining N m c points, i.e. m c = {q i }, i = 1, … , N m c , where m c ⊂ M. • For each grid cell (with ID c), the two parameters c and c representing respectively the mean and the covariance should be computed to determine the corresponding normal distribution N( c , c ) . They have the form:

with c ∈ ℝ 2 and c ∈ ℝ 2 × ℝ 2
The NDT map is therefore represented by a set of local normal distributions.

(

) c = 1∕N m c N m c ∑ i=1 q i (2) c = 1∕N m c N m c ∑ i=1 (q i -c )(q i -c ) t 1

Pose estimation

From a SLAM perspective, the position is computed based on a scan-matching approach with the basic principle of comparing two successive scans. By founding the transformation between the two scans, the position is estimated as long as this transformation represents the robot displacement. To this end, an optimization approach should be invested to solve the problem of pose estimation. Standard SLAM approaches, including NDT use gradient-based approaches such as Newton method [START_REF] Ypma | Historical development of the newton-raphson method[END_REF] with major drawbacks to be computationally expensive and sensitive to the choice of the departure position, i.e. if this position is far from the real solution, the method can either diverge or converge to a local minimum. It is a frequent problem in scan-matching [START_REF] Olson | Real-time correlative scan matching[END_REF][START_REF] Walter | Numerical methods and optimization: a consumer guide[END_REF]. In this paper, the proposed approach NDT-PSO is based on the particle swarm optimization approach (PSO), which is commonly used to solve several optimization problems due to its efficiency and simplicity. Furthermore, PSO is more likely to fall on the global minimum without position initialization [53,[START_REF] Venkatarao | Mechanical design optimization using advanced optimization techniques[END_REF]. An important contribution in this work is the exploitation of particle swarm optimization for solving a SLAM problem.

PSO is a stochastic population-based approach developed by Kennedy (a social psychologist) and Eberhart (an electrical engineer) who combined their expertise to model and formalize an animal behaviour based on fish schooling and bird flocking [55]. The swarm movement is a very intelligent behaviour, which can be simply interpreted as follows:

During their motion, each individual of the population has to continuously update its position given its own experience over time and the experience of its neighbours, with the ultimate goal of reaching food.

This behaviour is similar to an optimization problem where each individual is a possible solution dubbed particle.

Let p a particle defined by its position X ∈ ℝ D and velocity V ∈ ℝ D with D the dimension of the search space. Based on a modified version of PSO proposed by Shi and Eberhart [START_REF] Shi | A modified particle swarm optimizer[END_REF], the problem can be formalized thanks to the following equation: From Eq. 3, V +1 (p) is the velocity of p at the iteration + 1 of the optimization process expressed according to the functions F n , n = 1, ..., 3 representing different attractive forces affecting the motion of the particle and the parameters w n , n = 1, ..., 3 depicting weighting factors balancing the importance of each force. The first function represents the momentum behaviour expressed by forces attracting the particle to keep its current motion and has the form:

(3) V +1 (p) = ∑ n F n (p, w n )
with w m the momentum weighting factor (also called the inertia weight) and V particle's current velocity.

The second function F c represents the cognitive behav- iour depicting forces constraining the particle to consider its own experience by biasing its motion toward the personal best position denoted Pbest. It has the form: where rand1 is a random variable uniformly distributed in the range [0, 1], w c is the cognitive weighting factor and X is the current position of the particle.

The third function F s concerns the social behaviour where the particle considers the swarm's experience by adjusting its motion according to the global best position denoted Gbest. It represents the best position found so far in the swarm, such that: with rand2 is a random variable and w s is the social weight- ing factor. w c and w s are also known as acceleration coeffi- cients. The velocity of each particle in the swarm is updated thanks to equation 3, which in turn is used to update the particle's position according to the following equation:

In NDT-PSO, pose estimation problem is solved by encoding the geometric transformation T (translation (T x , T y ) and rotation ) between two scans into a particle:

Let M k-1 and M k two successive scans at iterations k -1 and k. A 2D point q j ∈ M k can be represented in the coordinate frame of the scan M k-1 thanks to:

where T depicts the geometric transformation between two robot Cartesian coordinate frames, such that:

The swarm particles ( p = 1, … , N p ) propose possible solu- tions to the scan-matching problem. In our case, the optimal solution should be selected with respect to the best matching between the two scans M k-1 and M k . Therefore, it is evalu- ated by summing the normal distributions P of all points q j ′ given the transformation expressed by the vector X(p). Therefore, the particles are optimized by maximizing the following objective function:

(4) F m = w m V (p) (5) F c = w c |rand 1 |(Pbest (p) -X (p)) (6) F s = w s |rand 2 |(Gbest -X (p)) (7) X +1 (p) = X (p) + V +1 (p) (8) X(p) = T = (T x , T y , ) (9
) q j � = T(q j , X)

(10) q xj � q yj � =
cossin sin cos

q xj q yj + T x T y
The normal distribution P(q j � ) of each mapped point q j ′ is determined by a simple lookup in the built map MAP k-1 . After attributing each scan point q j ′ to its corresponding cell c, the measurement probability P of q j ′ is computed thanks to:

NDT-PSO algorithm

From the NDT-PSO approach illustrated above, two key aspects can be highlighted: (1) the environment representation is based on the normal distributions transform approach and (2) the scan-matching is based on a bio-inspired stochastic approach. Algorithm 1 depicts the NDT-PSO process for a given iteration k. Based on two successive scans M k-1 and M k (respectively at previous and current iteration) and given a particles swarm with N p population size. NDT-PSO computes the geometric transformation Tran between M k-1 and M k and accordingly updates environment's global map GMAP . These parameters are respectively inputs and outputs of the algorithm. In the first step of the algorithm, the scan M k-1 is mapped given Eqs. 1 and 2 as illustrated in Sect. 2.1 (thanks to the function BUILD_MAP). It results local normal distributions based map denoted MAP k-1 (line # 1, algorithm 1, which on this basis will compute the transformation Tran. The next algorithm step is the swarm initialization corresponding to the first optimization process iteration ( = 0 ). During this step, the whole particles ( p = 1, … , N p ) ) are randomly initialized according to the function RANDOM_INITIALIZATION, where each particle is defined by its position vector X and velocity vector V . Based on X =0 , the personnel best Pbest is initialized and the set of scan points M k is mapped according to the function MAPPING_SCAN. This function proceeds mainly in two steps; (1) Determine the transformation of the set of points q j ∈ M k into the coordinate frame of scan M k-1 according to Eq. 10 (line # 37, algorithm 1. (2) Compute the normal distribution P of each mapped point q j ′ according to Eq. 12 (line # 38, algorithm 1. Based on P(q j � ) of the whole scan, the objective function f for each particle is evaluated and consequently the global best particle Gbest is determined. Given Pbest and Gbest , the velocity and position vectors ( X +1 and V +1 ) are updated (lines # 13 and # 17, algorithm 1. Therefore, the scan points q j ∈ M k are re-mapped according to the updated vector X +1 (line # 18, algorithm 1. Similarly to the first step of the optimization process, the objective function f +1 is computed based on updated parameters. Given the maximization criterion, the [START_REF] Li | Feature-based laser scan matching and its application for indoor mapping[END_REF]

f (p) = N M ∑ j=1 P(q j � ) (12) P(q j � ) = exp(-(q j � -c ) t c -1 (q j � -c )∕2)
personnel best Pbest +1 and the global best Gbest +1 are com- puted. This process is repeated for a maximum number of iterations iteration max . At the end of the process, the best solution corresponding to the global best (the best found so far) is assigned to the transformation Tran , and correspond- ingly, the global map GMAP is updated with

M ′ k , such that M � k = {q j � ∈ M � k ∕q j � = T(q j , Tran) ∧ Tran = Gbest} .
To demonstrate and validate the performances of NDT-PSO algorithm, it has been implemented and tested on an experimental platform in static and dynamic, indoor and outdoor environments. The implementation has been done under the operating system ROS1 (Robot Operating System) using C++ language. the ROS Computation graph of the NDT-PSO algorithm is depicted in figure 1.

Robot model

The experimental platform is a standard car-like vehicle called Robucar, with two fixed rear wheels and two orientable front wheels (see Fig. 2), governed by the following kinematic model: with (x, y) the coordinates of the rear axle midpoint, the orientation of the robot, the steering angle (orientation of the front wheels), v the linear velocity and L denotes the robot's wheelbase.

The Robucar is equipped with a laser range finder LMS511 placed in front of the robot, with an 80M maximum range and 190 • field of view.

NDT-PSO at work

To illustrate how NDT-PSO works, it has been tested in different scenarios; the first one is called CDTA-hallscenario, it shows the performances of NDT-PSO in indoor structured environment. The second scenario is called CDTA-urban-scenario which is more challenging given its unstructured nature. NDT-PSO has been also evaluated with respect to the presence of moving objects. The swarm size is set to 70 particles, the maximum number of optimization process iterations is 70. The acceleration coefficients in equation 5 and 6 are defined by w c = w s = 2 , given [START_REF] Shi | A modified particle swarm optimizer[END_REF]55]. The particles are randomly initialized, however, for more accuracy, they are generated in a limited area around the previously estimated pose such that : x,y,; the dimensions of the initialization area given T x , T y trans- lation and rotation.

X(p) ∈ [-x∕2, x∕2] × [-y∕2, y∕2] × [-∕2, ∕2] = [-1m, 1m] × [-1m, 1m] × [-∕8, ∕8] . With
NDT-PSO is based on a modified version of PSO that incorporates an inertia weight [START_REF] Shi | A modified particle swarm optimizer[END_REF], which has been chosen after implementating and testing several PSO variants. The variants involved are : (1) A modified version of PSO that incorporate a constriction coefficient [START_REF] Clerc | The particle swarm-explosion, stability, and convergence in a multidimensional complex space[END_REF][START_REF] Poli | Particle swarm optimization[END_REF]. [START_REF] Hahnel | An efficient fastslam algorithm for generating maps of large-scale cyclic environments from raw laser range measurements[END_REF] The global-local best PSO (GLBest-PSO), an improved version of PSO algorithm incorporating globallocal best inertia weight (GLBest IW) with global-local best acceleration coefficient (GLBest Ac) [START_REF] Arumugam | A new and improved version of particle swarm optimization algorithm with global-local best parameters[END_REF]. (3) Rotation PSO (RPSO), in which the velocity vector of each particle is multiplied by a random rotation matrix at each iteration [START_REF] Bonyadi | An analysis of the velocity updating rule of the particle swarm optimization algorithm[END_REF]. [START_REF] Khairuddin | Review on simultaneous localization and mapping (slam)[END_REF] The global-local optimal information ratio PSO (GLIR-PSO), which uses the global and local optimal information ratio to enhance appropriately the ability of global search [START_REF] Li | Research on global-local optimal information ratio particle swarm optimization for vehicle scheduling problem[END_REF]. (5) A modified PSO algorithm with a dynamic population size. It adjusts the size of the population for each iteration [START_REF] Teguar | Optimal design of Corona ring on HV composite insulator using PSO Approach with dynamic population size[END_REF]. However, in our case, all these variants made no contribution wrt the modified PSO version used in this paper. ( 13)

⎡ ⎢ ⎢ ⎣ ̇ẋy φ ⎤ ⎥ ⎥ ⎦ = v ⎡ ⎢ ⎢ ⎣ cos 𝜙 sin 𝜙 tan 𝜉∕L ⎤ ⎥ ⎥ ⎦

CDTA hall scenario

This dataset has been recorded in the CDTA hall scenario, an indoor environment with static objects of different natures (see Fig. 3a and Fig. 3b ). The resulting map and the robot's trajectory are depicted in Fig. 3c, where the quality of the built map and the accuracy of the estimated positions are high even in loop closing. The map has been faithfully rebuilt and the robot's trajectory correctly estimated.

CDTA urban scenario

The second scenario has been carried out in an urban environment at CDTA (represented in Fig. 4). It is an unstructured outdoor environment with the presence of arbitrary shaped objects (ex. trees, shrubs, buildings, persons, etc.). Figure 5 illustrates the resulting map and estimated trajectory for different robot paths. These experiments show the performance of NDT-PSO in an outdoor environment.

From an optimization perspective, NDT-based representation is known for its high capacities in such conditions as it deals with data uncertainty due to its probabilistic nature. Furthermore, with only 70 particles (where many tests were successful with less particles), NDT-PSO can find the best particle while avoiding local minima even in a loop closing situation (see Fig. 5 at the bottom).

To more illustrate NDT-PSO performances when closing loops, in the case of Fig. 6, the robot is performing three loops while maintaining a constant steering angle (for more clarity). The resulting robot trajectories seem perfectly superposed.

In figure 7, NDT-PSO has been tested in a more challenging environment conditions, so as to evaluate how it tackles moving objects with arbitrary trajectories. The experiments carry out in CDTA urban scenario with the presence of four pedestrians moving arbitrary in different directions. The environment and the robot's displacement are respectively correctly mapped and estimated. Objects movement appears clearly in the figure by the blue traces. It can be concluded that, the accuracy of the results is not affected by the presence of moving objects.

Algorithm performances

NDT-PSO algorithm is a function of some parameters; mainly the swarm size N p and the number of optimization process iterations iteration max . In the above experiments, these parameters have been set to fixed values, however, to more understand their effects on NDT-PSO, the algorithm has been assessed accordingly. The Tables 1 and2 give respectively running times of NDT-PSO wrt N p and iteration max . From the obtained results, it is clear that the computation time complexity grows linearly with N p and iteration max , mainly due to for loop line # 12 and while loop line # 11 of algorithm 1 The running times presented in these tables are encouraging. After many tests, it has been noted that a swarm of 70 particles and a process of 70 iterations are largely sufficient to have good results. Moreover, most PSO-based approaches seek the best particle among the whole swarm, a new variant is to subdivide the swarm to sub-swarms, and perform the different calculations independently for each sub-swarm. The set of sub-swarms act in parallel using threads. At the end of the process, the final solution is computed using all the subswarms solutions. Therefore, even if the swarm size and the algorithm iterations number grow, the running time of the algorithm can easily decrease.

Comparison with SLAM methods

To evaluate the performances of the NDT-PSO algorithm with respect to other SLAM methods, it has been compared with two widely used methods in the ROS community, namely gmapping [START_REF] Grisettiand | Fast and accurate SLAM with Rao-Blackwellized particle filters[END_REF] and Hector-SLAM [START_REF] Kohlbrecher | A flexible and scalable slam system with full 3d motion estimation[END_REF]. Gmapping method is based on a Rao-Blackwellized particle filter [START_REF] Grisettiand | Fast and accurate SLAM with Rao-Blackwellized particle filters[END_REF]. The robot's trajectory and the environment map are estimated using a particle filter, which incrementally processes the laser data (observations) and the odometric data as they are available. Hector-SLAM is based on a scan-matching approach using map gradients approximations and a multi-resolution grid [START_REF] Kohlbrecher | A flexible and scalable slam system with full 3d motion estimation[END_REF]. The environment is represented using an occupancy grid map [START_REF] Thrun | Probabilistic robotics[END_REF]. The tests have been carried out in the CDTA hall scenario (see Fig. 8a) in the same conditions and constraints for the three methods. The results are depicted in Fig. 8, where, it is clear that gmapping and Hector-SLAM methods (respectively in Fig. 8b,c) are not as accurate as NDT-PSO method (see Fig. 8d). From a map building perspective, both Hector-SLAM and gmapping are based on a grid map representation [48], which is known to be less accurate than NDT-based representation (as stated in a previous work [START_REF] Saarinen | Normal distribution transform monte-carlo localization (ndt-mcl)[END_REF]). Moreover, to solve scan-matching problem, Hector-SLAM uses a gradient-based approach which can be subject to local minima. However, for NDT-PSO, based on PSO, a bio-inspired stochastic optimization approach presents better performances to find a global optimum [START_REF] Jang | Trajectory optimization with particle swarm optimization for manipulator motion planning[END_REF]. Regarding gmapping, the main problem of the Rao-Blackwellized approach is its complexity in terms of number and variety of particles needed to build an accurate map. It should be also noted that the accuracy of gmapping results is invariant through different runs of the package (while maintaining the same conditions), where the presented results are the best. It can be explained by the fact that for the same input, a Rao-Blackwellized particle filter can produce different outputs. For all this reasons NDT-PSO provides better performances. In this paper, NDT-PSO a bio-inspired stochastic approach has been proposed. It is based on particle swarm optimization algorithm to solve the pose estimation problem, which is a key process in scan-matching-based SLAM methods. The solution is encoded as the best particle in the swarm representing the best transformation between two successive NDT maps. The obtained results demonstrate the performances of NDT-PSO in real situations in both indoor and outdoor environments, either static or dynamic. The resulting map and estimated positions remain accurate even in loop closing situations and scenarios crowded with moving objects. Furthermore, it has been demonstrated that the algorithm converges rapidly and it is able to find the best particle while avoiding local minima. NDT-PSO is tested for different swarm sizes and different numbers of optimization process iterations, and the results show that a swarm of 70 particles and a process of 70 iterations are more than enough to have good results. It is also very suitable for real time applications , where the average running time of NDT-PSO for 70 particles and 70 optimization process iterations, is 145ms. The running time varies depending on these two parameters. Moreover, NDT-PSO has been evaluated against other commonly used SLAM methods and has shown better results in term of accuracy.

As future work, this algorithm should be tested in very large scale environments for an intelligent transportation application. It could be also interesting to improve the mapping based on data fusion techniques using onboard sensors.

Fig. 1 Fig. 2

 12 Fig. 1 ROS Computation graph generated by rqt_graph. It highlights ROS nodes and topics. The ndt_pso node communicates with the topics/laser and/odom (to respectively get the laser data and initialize the robot's position). This node is publishing the robot's position (via/ pose topic) and the built map (via/map topic)

Fig. 3

 3 Fig. 3 NDT-PSO at work in the CDTA hall scenario; a CDTA hall from different view angles. b CDTA hall plan (the red trajectory is generated by NDT-PSO). c NDT-PSO generated map and trajectory for 70 particles and 1m map resolution

Fig. 5

 5 Fig. 5 NDT-PSO at work in the CDTA urban scenario for different situations (for 70 particles and 1M map resolution)

Fig. 6 Fig. 7

 67 Fig.6NDT-PSO at work in a loop closing situation. The robot is performing three superposed circular trajectories (trajectories in red)

Fig. 8

 8 Fig. 8 Comparing NDT-PSO with gmapping and Hector-SLAM methods; a test scenario, b gmapping, c Hector-SLAM and d NDT-PSO

Table 1

 1 Average running time of NDT-PSO wrt the number of particles in the swarm ( N p ), for iteration max = 70 (tests carried out in figure7)

	Population	Running time (s)
	size ( N p )
	10	0.0303893232
	20	0.0487240912
	30	0.0690673167
	40	0.0864106667
	50	0.1100520787
	60	0.1250710661
	70	0.1440125
	80	0.1619612011
	90	0.1784725106
	100	0.1990441554

Table 2

 2 Average running time of NDT-PSO wrt the number of iterations of the optimization process ( iteration max ) during one NDT-PSO iteration, (for N p = 70 , tests carried out in Fig.7)

	Number of iterations ( iteration max )	Running time (s)
	10	0.0369800877
	20	0.0579088445
	30	0.0718727725
	40	0.0883877902
	50	0.1078476976
	60	0.1250863987
	70	0.1457493524
	80	0.1607982934
	90	0.1786244514
	100	0.1990917095

www.ros.org


Sara Bouraine

Abdelhak Bougouffa

Ouahiba Azouaoui

NDT-PSO, a New NDT based SLAM Approach using Particle Swarm Optimization

This paper deals with the problem of simultaneous localization and mapping (SLAM). Providing both accurate environment's map and pose estimation is necessary to correctly navigate, which is a key issue for a mobile robot interacting with human beings. It is a line of research that is always active, offering various solutions to this issue. Nevertheless, among many SLAM methods, Normal Distributions Transform (NDT) has shown high performances, where numerous works have been published up to date and many studies demonstrate its efficiency wrt to other methods. In this paper a new NDT based SLAM method using Particle Swarm Optimization called NDT-PSO is proposed. The main contribution is to invest the bioinspired approach PSO to solve pose estimation problem based on iterative NDT maps. Real experiments have been performed on a car-like mobile robot to confirm the performances of NDT-PSO approach and its efficiency in both static and dynamic environments.

I. INTRODUCTION

Today, mobile robots are present in our daily life and they become more and more autonomous. Indeed, thanks to the ability to locate and map its environment, the robot can plan trajectories and navigate in a real world in order to perform various tasks without human intervention. This ability is known as Simultaneous Localization And Mapping (SLAM) problem [START_REF] Khairuddin | Review on simultaneous localization and mapping (slam)[END_REF], [START_REF] Singandhupe | A review of slam techniques and security in autonomous driving[END_REF], [START_REF] Cadena | Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age[END_REF]. It consists of estimating the robot position while building the map of the environment. Most developed SLAM approaches are under the Filtering [START_REF] Choi | Hybrid map-based slam with rao-blackwellized particle filters[END_REF], [START_REF] Zhang | Convergence and consistency analysis for a 3-dinvariant-ekf slam[END_REF], [START_REF] Lee | Efficient ekf-slam algorithm based on measurement clustering and real data simulations[END_REF] or scan matching [START_REF] Li | Feature-based laser scan matching and its application for indoor mapping[END_REF], [START_REF] Wang | Accurate mix-norm-based scan matching[END_REF], [START_REF] Zahran | Enhancement of real-time scan matching for uav indoor navigation using vehicle model[END_REF] paradigms but share the same main steps, namely; mapping and pose estimation. The improvement of the earliest approaches in the two categories is still ongoing despite their existence for more than two decades.

Indeed, in recent years, Particle Swarm Optimization (PSO) is increasingly exploited in SLAM-based methods to eliminate the problems of insufficiency and inaccuracy of priori information for Kalman filter methods [START_REF] Zhu | Research and application on fractionalorder darwinian pso based adaptive extended kalman filtering algorithm[END_REF] and impoverishment of particles for particle filter methods [START_REF] Lee | PSO-FastSlam: An improved FastSlam framework using particle swarm optimization[END_REF], [START_REF] Liu | An improved FastSlam framework based on particle swarm optimization and unscented particle filter[END_REF], [START_REF] Wu | FastSlam method based on gaussian particle swarm optimization[END_REF]. In [START_REF] Zhu | Research and application on fractionalorder darwinian pso based adaptive extended kalman filtering algorithm[END_REF], the fuzzy adaptive extended Kalman filtering method was improved by introducing the fractional-order Darwinian particle swarm optimization (PSO) to compute an accurate priori noise model. Lee et al. [START_REF] Lee | PSO-FastSlam: An improved FastSlam framework using particle swarm optimization[END_REF] proposed 1 Sara Bouraine is with Center for development of advanced technologies (CDTA), cite 20 Aout 1956, Baba Hassen, Algiers, Algeria s bouraine@yahoo.fr 2 Abdelhak Bougouffa was with Center for development of advanced technologies (CDTA), cite 20 Aout 1956, Baba Hassen, Algiers, Algeria abdelhak.bougouffa@universite-paris-saclay.fr 1 Ouahiba Azouaoui is with Center for development of advanced technologies (CDTA), cite 20 Aout 1956, Baba Hassen, Algiers, Algeria oazouaoui@cdta.dz a FastSLAM framework where the robot position is estimated using Rao-Blackwellized particle filter. FastSLAM is known to degenerate over time in terms of accuracy due to the particle depletion in re-sampling phase. To prevent degeneration, a particle swarm optimization is employed to solve the problem by means of particle cooperation. Another improvement of the Fast-SLAM is the work of Liu et al. [START_REF] Liu | An improved FastSlam framework based on particle swarm optimization and unscented particle filter[END_REF] where PSO and unscented particle filter are introduced to reduce drastically the number of particles thanks to PSO for pose estimation and to improve map estimation accuracy thanks to unscented particle filter. In Wu et al. [START_REF] Wu | FastSlam method based on gaussian particle swarm optimization[END_REF], to solve the degeneration of particles and the position inaccuracy (due to the need of a large number of particles) problems, the Gaussian particle swarm optimization algorithm is used in the particle filter process.

In the second category, namely scan-matching methods, most of the methods use a nonlinear least-square optimization to determine the robot pose, but it is subject to the local minima problem which leads to not guaranteeing the algorithm convergence particularly in the presence of dynamic objects or fast movement of the robot. These few last years, some works have been conducted to prevent and improve the scan-matching-based methods. In [START_REF] Zahran | Enhancement of real-time scan matching for uav indoor navigation using vehicle model[END_REF], the ICP and Hector SLAM algorithms are improved using the system model as an initialization step followed by the ICP or Hector as alignment step. The work in [START_REF] Li | Feature-based laser scan matching and its application for indoor mapping[END_REF] introduces in the ICP algorithm both a 2D laser scan matching method based on point and line features as an initialization phase and the lq-norm (0 < q < 1) metric as a pose estimation to filter the outliers. In Wang et al. [START_REF] Wang | Accurate mix-norm-based scan matching[END_REF], a mixture of exponential power (MoEP) distributions is proposed to approximate the residual error distribution. The optimization of the scan matching method is iteratively achieved via two phases: an on-line parameter learning (OPL) phase to learn residual error distribution for better representation according to the likelihood field model (LFM), and an iteratively reweighted least squares (IRLS) phase to attain transformation for accuracy and efficiency. Nevertheless, among many SLAM methods, NDT has shown high performances, where numerous works have been published up to date [START_REF] Biber | The normal distributions transform: A new approach to laser scan matching[END_REF], [START_REF] Stoyanov | Fast and accurate scan registration through minimization of the distance between compact 3-d ndt representations[END_REF], [START_REF] Einhorn | Generic ndt mapping in dynamic environments and its application for lifelong slam[END_REF], [START_REF] Wolcott | Robust lidar localization using multiresolution gaussian mixture maps for autonomous driving[END_REF], [START_REF] Schmiedel | Iron: A fast interest point descriptor for robust ndt-map matching and its application to robot localization[END_REF] also many studies demonstrate its efficiency wrt to other methods [START_REF] Saarinen | Normal distribution transform monte-carlo localization (ndt-mcl)[END_REF], [START_REF] Pang | 3d scan registration based localization for autonomous vehicles -a comparison of ndt and icp under realistic conditions[END_REF]. However, to solve the optimization problem, most NDT-based methods use Newton Algorithm (like [START_REF] Morita | Point-cloud mapping and merging using mobile laser scanner[END_REF], [START_REF] Li | Efficient laser-based 3d slam for coal mine rescue robots[END_REF]). Few works have been interested in the problem of optimization; in [START_REF] Saarinen | Normal distribution transform monte-carlo localization (ndt-mcl)[END_REF], [START_REF] Schmiedel | Iron: A fast interest point descriptor for robust ndt-map matching and its application to robot localization[END_REF], pose estimation is performed using Monte Carlo Localization (MCL). In [START_REF] Einhorn | Generic ndt mapping in dynamic environments and its application for lifelong slam[END_REF], the best fitting alignment between two sets of point samples is found through the minimization of the L2 distance between NDT models. Moreover, in other works, the optimal transformation to solve the scan matching problem is directly formulated as a maximum likelihood estimate of Gaussian mixture maps [START_REF] Wolcott | Robust lidar localization using multiresolution gaussian mixture maps for autonomous driving[END_REF].

In this paper a new NDT-based SLAM method called NDT-PSO is proposed, where the scan matching process is based on PSO method. Experiments performed on a carlike mobile robot confirm the performances of NDT-PSO approach and its efficiency in both static and dynamic environments.

This paper is organized as follows: section 2 presents the proposed approach, where the scan matching problem is formalized before presenting NDT-PSO algorithm. Validations in real experiments are given in section 3. Finally, section 4 provides a summary and a conclusion of the paper.

II. PROPOSED APPROACH: NDT-PSO

To solve a SLAM problem, the proposed solution is based on a bio-inspired approach dubbed Normal Distributions Transform Particle Swarm Optimization (NDT-PSO). Like any SLAM problem, two key issues to answer are environment mapping and pose estimation. The following sections answer these two issues.

A. Environment Representation

In this paper, a NDT-based representation is used so as to deal with uncertainties and environment constraints. Proposed by Biber and Strasser [START_REF] Biber | The normal distributions transform: A new approach to laser scan matching[END_REF], NDT is based upon a grid discretization of the space, where each 2D laser scan of the collected data is modeled as a set of cells, where a probability of measurement is associated with a 2D scan point contained in a cell.

Let M = {q j }, j = 1, ..., N M a points set of a given laser scan, with q j ∈ R 2 a scan point, j its indice and N M the number of scan points. These raw data are transformed to NDT-based representation by the following steps:

• As the space is partitioned into a set of cells, each cell with ID "c" will be assigned a sub-set of points m c containing N m c points, i.e. m c = {q i }, i = 1, ..., N m c , where m c ⊂ M . • For each grid cell with ID c, the mean µ c and the covariance Ω c should be computed to determine the corresponding normal distribution. They have the form:

µ c = 1/N m c N m c i=1 q i ( 1
)

Ω c = 1/N m c N m c i=1 (q i -µ c )(q i -µ c ) t (2)
With

µ c ∈ R 2 and Ω c ∈ R 2 × R 2
The NDT map is therefore represented by a set of local normal distributions.

B. Pose Estimation

To solve the pose estimation problem, scan matching approaches, including NDT use gradient based approaches such as Newton method [START_REF] Ypma | Historical development of the newton-raphson method[END_REF] with major drawbacks to be computationally expensive and sensitive to the choice of departure position, which is a frequent problem [START_REF] Olson | Real-time correlative scan matching[END_REF], [START_REF] Walter | Numerical methods and optimization: A consumer guide[END_REF]. In this paper, the proposed approach NDT-PSO is based on PSO, which is commonly used to solve several optimization problems due to its efficiency and simplicity and it is more likely to fall on the global minimum without position initialization [START_REF] Venkatarao | Mechanical design optimization using advanced optimization techniques[END_REF].

Developed by Kennedy and Eberhart [START_REF] Kennedy | Particle swarm optimization[END_REF], PSO is a stochastic population-based approach inspired from an animal behaviour, namely fish schooling and bird flocking. The swarm movement is a very intelligent behaviour similar to an optimization problem where each individual is a possible solution dubbed particle, and it is formalized as follows:

V τ +1 (p) = n F n (p, w n ) (3)
with p a given particle, X ∈ R D its position vector, V ∈ R D its velocity vector, and D the search space dimension. From equation 3, V τ +1 (p) is the velocity of p at the iteration τ + 1 of the optimization process expressed according to the functions F n , n = 1, ..., 3 representing different attractive forces affecting the particle motion and the parameters w n , n = 1, ..., 3 depicting weighting factors balancing the importance of each force. The first function represents the momentum behaviour expressed by forces attracting the particle to keep its current motion and has the form:

F m = w m V τ (p) (4)
with w m the momentum weighting factor and V τ particle's current velocity.

The second function F c represents the cognitive behaviour depicting forces constraining the particle to consider its own experience by biasing its motion toward the personal best position denoted P best. It has the form:

F c = w c |rand 1 |(P best τ (p) -X τ (p)) ( 5
)
where rand1 is a random variable uniformly distributed in the range [0, 1], w c is the cognitive weighting factor and X τ is the current position of the particle. The third function F s concerns the social behaviour where the particle considers the swarms experience by adjusting its motion according to the global best position denoted Gbest. It represents the best position found so far in the swarm, such that:

F s = w s |rand 2 |(Gbest τ -X τ (p)) (6)
With rand2 is a random variable and w s is the social weighting factor. w c and w s are also known as acceleration coefficients. The velocity of each particle in the swarm is updated thanks to equation 3, which in turn is used to update the particle's position according to the following equation:

X τ +1 (p) = X τ (p) + V τ +1 (p) (7)
In NDT-PSO, pose estimation problem is solved by encoding the geometric transformation T (translation (T x , T y ) and rotation θ) between two scans into a particle:

X(p) = T = (T x , T y , θ) (8)
Let M k-1 and M k two successive scans at iterations k -1 and k. A 2D point q j ∈ M k can be represented in the coordinate frame of the scan M k-1 thanks to:

q j = T (q j , X) (9)
such that:

q xj q yj = cos θ -sin θ sin θ cos θ q xj q yj + T x T y ( 10
)
The particles represent possible solutions to the scan matching problem. The optimal solution should be selected with respect to the best matching between the two scans M k-1 and M k . Therefore, it is evaluated by summing the normal distributions P of all points q j given the transformation expressed by X(p). Therefore, the particles are optimized by maximizing the following objective function:

f (p) = N M j=1 P (q j ) (11)
P (q j ) of each mapped point q j is determined by a simple lookup in the built map MAP k-1 . After attributing each scan point q j to its corresponding cell c, the measurement probability P of q j is computed thanks to:

P (q j ) = exp(-(q j -µ c ) t Ω c -1 (q j -µ c )/2) (12)

C. NDT-PSO Algorithm

The overall process of NDT-PSO approach for a given iteration k is depicted in algorithm 1. Based on two successive scans M k-1 and M k and given a swarm with N p particles, NDT-PSO computes the geometric transformation T ran between M k-1 and M k and accordingly updates environment's global map GMAP. These parameters are respectively inputs and outputs of the algorithm. In the first step of the algorithm, the scan M k-1 is mapped given equations 1 and 2 (thanks to the function BUILD MAP). It results a local normal distributions based map denoted MAP k-1 (line # 1, algorithm 1), which is used to compute the transformation T ran. Next, the swarm is initialized for the first optimization process iteration (τ = 0). During this step, the whole particles are randomly initialized according to the function RANDOM INITIALIZATION, where each particle is defined by its X τ and V τ vectors. Based on X τ =0 , the personnel best P best τ is initialized and the set of the scan points M k is mapped according to the function MAPPING SCAN. This function proceeds mainly in two steps; (1) Determine the transformation of the points set q j ∈ M k into the coordinate frame of scan M k-1 according to equation 10 (line #37, algorithm 1). (2) Compute the normal distribution P of each mapped point q j according to equation 12 (line #38, algorithm 1). Based on P (q j ) of the whole scan, the objective function f τ for each particle is evaluated and consequently the global best particle Gbest τ is determined. Given P best τ and Gbest τ , X τ +1 and V τ +1 are updated (lines #13 and #17, algorithm 1). Therefore, the scan points q j ∈ M k are re-mapped according to X τ +1 (line #18, algorithm 1) and f τ +1 is computed based on updated parameters. Given the maximization criterion, P best τ +1 and Gbest τ +1 are computed. This process is repeated for a given iteration number iteration max . At the end, the best solution corresponding to Gbest is assigned to T ran, and correspondingly, GMAP is updated with M k , such that M k = {q j ∈ M k /q j = T (q j , T ran) ∧ T ran = Gbest}.

III. RESULTS

To demonstrate and validate the performances of NDT-PSO algorithm, it has been implemented and tested on an experimental platform in static and dynamic, indoor and outdoor environments. The implementation has been done under the operating system ROS1 (Robot Operating System) using C++ language. ROS Computation graph of the NDT-PSO algorithm is depicted in Fig. 1. Fig. 1: ROS Computation graph generated by rqt graph. ndt pso node communicates with /laser and /odom topics to get sensors data and with /pose and /map topics to publish robot position and constructed map.

A. Experimental platform

It is a standard car-like vehicle called Robucar, with two fixed rear wheels and two orientable front wheels (see Fig. 2). The Robucar is equipped with a laser range finder LMS511 placed in front of the robot, with an 80m maximum range and 190 • field of view.

B. NDT-PSO at work

NDT-PSO algorithm has been tested in two scenarios: [START_REF] Khairuddin | Review on simultaneous localization and mapping (slam)[END_REF] the CDTA-hall-scenario that shows NDT-PSO performances in indoor structured environment, and (2) the CDTA-urbanscenario, which is an outdoor unstructured environment. The swarm size is set to 70 particles, the maximum number of optimization process iterations is 70. The acceleration coefficients in equation 5 and 6 are defined w c = w s = 2 given refs. [START_REF] Shi | A modified particle swarm optimizer[END_REF], [START_REF] Kennedy | Particle swarm optimization[END_REF]. The particles are randomly initialized for each particle p=1 to N p do 13: Gbest τ +1 = X τ +1 (Gbest index ); //Transform q j into coordinate frame of the scan M k-1 using equation 10 37: q j = T (q j , X);

V τ +1 (p) = V τ (p) + w c |rand 1 |(P best τ (p) - X τ (p)) + w s |rand 2 |(Gbest τ -X τ (p)); 14: if |V τ +1 (p)| > V pmax then 15: V τ +1 (p) = V pmax ; 16: end if 17: X τ +1 (p) = X τ (p) + V τ +1 (p);

38:

Compute P (q j ); //The normal distribution of q j according to equation 12 39: end for 40: EndProcedure in a limited area around the previously estimated pose within a radius of 1m according to the cartesian coordinates and an angle of π/8 according to the orientation.

1) CDTA hall scenario: This dataset has been recorded in the CDTA hall scenario, in the presence of static objects of different natures (see Fig. 3a and Fig. 3b). The resulting map and robot trajectory are depicted in Fig. 3c. The map has been faithfully rebuilt and robot trajectory correctly estimated even in closing loop. 2) CDTA urban scenario: The second scenario has been carried out in an urban environment at CDTA represented in Fig. 4, in the presence of arbitrary shaped objects (ex. trees, shrubs, buildings, persons, etc.). Fig. 5 illustrates the resulting map and estimated trajectory for different robot paths. These experiments show the performance of NDT-PSO in an outdoor environment. Furthermore, with only 70 particles, Fig. 4: CDTA urban scenario. NDT-PSO can find the best particle while avoiding local minima even when closing loop (see Fig. 5 at the bottom). Moreover, in the case of Fig. 6, NDT-PSO has been tested when the robot is performing three loops while maintaining a constant steering angle. The resulting robot trajectories seem perfectly superposed.

In Fig. 7, NDT-PSO has been tested in a more challenging environment conditions to evaluate how it tackles moving objects with arbitrary trajectories. The experiments carry out in CDTA urban scenario in the presence of four pedestrians moving arbitrary in different directions. Objects movement appears clearly by the blue traces. Environment and robot's displacement are respectively correctly mapped and estimated, and moving objects does not affect the accuracy of the results.

The NDT-based representation has already been evaluated against other SLAM methods to demonstrate its performances, like in ref. [START_REF] Magnusson | Evaluation of 3D registration reliability and speed-A comparison of ICP and NDT[END_REF]. Furthermore, from an optimization perspective, PSO method is known for its fast convergence and its ability to find a global optimum [START_REF] Bouraine | Safe motion planning based on a new encoding technique for tree expansion using particle swarm optimization[END_REF], which justifies the above results and the effectiveness of NDT-PSO method.

C. Algorithm Performances

NDT-PSO algorithm is a function of some parameters: mainly the swarm size N p and the number of optimization process iteration iteration max . In the above experiments, these parameters were set to fixed values, however, to more understand their effects on NDT-PSO, the algorithm has been assessed accordingly. The tables I and II give respectively running times of NDT-PSO wrt N p and iteration max . From the obtained results, it can be stated that the computation time complexity grows linearly with N p and iteration max , mainly due to for loop line # 12 and while loop line # 11 of algorithm 1. These running times are encouraging. After many tests, it has been noted that a swarm of 70 particles and a process of 70 iterations are largely sufficient to have good results.

IV. CONCLUSIONS

In this paper, NDT-PSO a bio-inspired stochastic approach has been proposed to solve a scan matching based SLAM problem. Even though this issue has been largely addressed in literature, most proposed approaches lack fast convergence and the simplicity of the optimization algorithm. The primary contribution of this paper has been to propose a new approach to solve the optimization problem using PSO method. The solution is encoded as the best particle in the swarm representing the best transformation between two successive NDT maps. The obtained results demonstrate the performances of NDT-PSO in real situations in both TABLE I: Average running time of NDT-PSO wrt the number of particles in the swarm (N p ), for iteration max = 70 (tests carried out in Fig. 7). The resulting map and estimated positions remain accurate even in closing loop situations and scenarios crowded with moving objects. It has been also demonstrated that the algorithm converges rapidly and it is very suitable for real time applications. This work could be extended by comparing NDT-PSO algorithm with other SLAM methods to better show its performances, which is ongoing. Furthermore, this algorithm should be tested in very large scale environments for an intelligent transportation application.

Algorithm 1

 1 NDT-PSO process. Input: M k-1 (laser scan corresponding to previous iteration), M k (laser scan corresponding to current iteration), number of particles N p . Output: T ran geometric transformation between two scans (at k and k +1), GMAP. 1: MAP k-1 ← BUILD MAP(M k-1 ); //Map construction given equations 1 and 2 2: //Particles initialization (optimization iteration τ = 0) 3: for each particle p=1 to N p do 4: (X τ (p), V τ (p)) ← RANDOM INITIALIZATION(); 5: P best τ (p) = X τ (p);

6 :

 6 MAPPING SCAN(M k ,X τ (p)); 7: compute f τ (p); // the objective function according to equation 11 8: end for 9: Gbest index = argmax p=1,...,Np f τ (p); //index of the particle corresponding to the global best 10: Gbest τ = X τ (Gbest index ); 11: while τ < iteration max do 12:

18 :

 18 MAPPING SCAN(M k ,X τ +1 (p)); 19: compute f τ +1 (p); // the objective function according to equation 11 20: if f τ +1 (p) > f τ (p) then 21: P best τ +1 (p) = X τ +1 (p); //update personal best 22: if max p=1,...,Np f τ +1 (p) > max p=1,...,Np f τ (p) then 26: Gbest index = argmax p=1,...,Np f τ +1 (p);27:

  end while 30: T ran = Gbest τ +1 ; 31: GMAP ← UPDATE GLOBAL MAP(T ran,M k ); 32: return T ran,GMAP 33: 34: Procedure MAPPING SCAN(M k ,X) 35: for each 2D point q j ∈ M k do 36:

Fig. 2 :

 2 Fig. 2: Experimental platform Robucar.

Fig. 3 :

 3 Fig. 3: NDT-PSO at work in the CDTA hall scenario; (a) CDTA hall from different view angles. (b) CDTA hall plan (the red trajectory is generated by NDT-PSO). (c) NDT-PSO generated map and trajectory for 70 particles and 1m map resolution.

Fig. 5 :

 5 Fig. 5: NDT-PSO at work in the CDTA urban scenario for different situations (for 70 particles and 1m map resolution).

Fig. 6 :

 6 Fig. 6: NDT-PSO at work in a closing loop situation.

Fig. 7 :

 7 Fig. 7: NDT-PSO at work in the presence of moving objects in the CDTA urban scenario (see text).

TABLE II :

 II Average running time of NDT-PSO wrt the number of iterations of the optimization process (iteration max ) during one NDT-PSO iteration, (for N p = 70, tests carried out in Fig.7).

	Number of iterations (iterationmax)	Running time (s)
	10	0.0369800877
	20	0.0579088445
	30	0.0718727725
	40	0.0883877902
	50	0.1078476976
	60	0.1250863987
	70	0.1457493524
	80	0.1607982934
	90	0.1786244514
	100	0.1990917095
	indoor and outdoor environments, either static or dynamic.

www.ros.org


A A Lebedev

A M Strel'chuk

S Ortolland

C Raynaud

M.-L Locatelli

D Planson

J.-P Chante

The negative temperature coefficient of the breakdown voltage of SiC p-n structures and deep centers in SiC

In this paper negative temperature coefficient of the 6H-SiC diode breakdown voltage is considered. It is shown that the temperature dependence of the breakdown voltage value can be explained in terms of recharging of deep centers in the space charge region of the diode. Experiments made with boron doped 6H-SiC diodes are in good agreement with calculation.

Introduction

In Ref. 1 it was found that in SiC p-n structures in which the electric field is applied parallel to the crystallographic axis c the breakdown voltage shows a negative temperature coefficient (BVTC). In a number of studies [2,3] this fact was related to the crystal structure of hexagonal SiC polytypes and to the occurrence of a natural superlattice. In some studies [4], alongside with the negative BVTC, a positive BVTC was observed and it was suggested that the negative value of BVTC might be related to recharging of deep centers, as in the case of silicon-based p-n structures [5].

Samples

The present study was concerned with SiC p-n structures prepared by sublimation epitaxy, with boron diffusion carried out prior to forming mesas. The electrical field in diode the p-n junction of the diode was parallel to the c-axis of the crystal. The structures had a negative BVTC of about 2x10 -3 K -1 . The absolute value of BVTC depended on temperature and dropped by about an order of magnitude when the junctions were heated to 600K.

Theory and experiment

To find an explanation for the observed negative values of BVTC we considered, following the authors of refs. 4 and 5, the effect of recharging of deep centers. It is known that boron in SiC introduces two types of deep acceptor levels, the deeper level (D-center) having the following parameters:

E " + 0.58 eV, σ -= 1 -3 × 10 345 cm 8 , σ 9 = 1 -3 × 10 38: cm 8 The model we suggested is based on the recharging of deep acceptor levels in a lightly doped p-type intermediate layer near the metallurgical boundary of the p-n junction. We made two assumptions:

(i)
The region of avalanche multiplication is situated in the low doped p-type region near the metallurgical boundary of the p-n junction. (ii)

The concentration of deep acceptor centers is comparable to that of shallow acceptors.

Such a region can be formed through overcompensation of the n-type region during the diffusion of acceptor impurities (boron, for example). The presence of a region of this kind in our samples has been shown experimentally [START_REF] Lebedev | Strel'[END_REF]. The breakdown voltage of a p-n junction is given by the well-known equation:

U ?@ = ε B E C@ 8 2qN G (1)
where ε B is the statistic permittivity, Ecr is the critical electric field, q is the electron charge, Ni is the impurity concentration in the base region.

Taking into account deep acceptor levels in the low doped p-base, we can write :

U ?@ = ε B E C@ 8 2q(N J + KM) = U ?@M N1 + KM N J O P ( 2
)
where NS is the concentration of shallow acceptor impurities, K is the electron occupancy of the deep acceptor level, and K = (M-m)/M, where M is the total concentration of deep levels; m is the concentration of deep acceptors occupied by the holes, Ubro is the breakdown voltage of p-n junction without deep acceptors.

When U<<Ubr, there is practically no current through the p-n junction and all deep acceptors in the p-type layer are occupied by electrons (K=1). When U~U ?@ , avalanche multiplication occurs in the space charge region and holes are captured by deep acceptors. Because acceptors occupied by holes are neutral, K decreases and, consequently, the electrical field becomes weak. This leads to an increase in the observed value of the Ubr. It should be noted that the level occupancy depends on temperature and decreases with heating. For this reason, Ubr will decrease with temperature, so we can see negative BVTC.

Based on the Shockley-Read statistics we can write the following equation for the discharging rate of deep acceptors in our case :

dm dt = (M -m)Δpα --mα 9 Δn -α -β G m (3)
Here, Δp = Δn is the carriers concentration; α 9 (p) = σ 9 (p)V Y , where σ 9 (p) is the electron (hole) capture cross section of the deep acceptor, Vt is the thermal velocity; and 𝛽 [ = 𝑁 ] 𝑒 3 _à b , where NV is the density of state in the valence band, Ei is the ionisation energy of the deep acceptor, T is the absolute temperature; k is the Boltzmann constant.

For the case of constant current (dm/dt = 0), and taking into account that α -≫ α 9 , we obtain form (3) the following equation for K :

K = β G (Δp + β G ) 34 (4)
Equations ( 4) and (2) give for Ubr : Fig. 1 shows experimental and calculated values of F = Ubr/Ubro-1. The following parameters were used in calculation: Ubro = 800 V, M/NS = 0.65 and NV = 5.10 15 x T 3/2 cm -3 . As one can see in the figure the best agreement between experimental and theoretical results is obtained for Δp of about 10 11 -10 12 cm -3 . However the magnitude of Δp calculated from the value of the breakdown current as Δp =J/VSq (where J is the current density in the p-n junction and VS is the drift velocity) is 10 10 -10 11 cm -3 . This discrepancy can be explained, from our point of view, if we take into account the fact that the breakdown in SiC usually takes place in small areas (microplasma breakdown) whose size is several orders of magnitude less than the total area of the p-n junctions. But when we calculate the current density we take the total area of the p-n junction. In other words, actual current density (and the Δp value) may be higher by one order of magnitude in the breakdown areas which coincides with the value needed by the theory.

U ?@ = U ?@M 1 + Mβ G N J (Δp + β G ) (5)

Summary

The temperature dependence of BVTC calculated in terms of our model and known D-denter parameters shows a good agreement with experiments. Because the boron diffusion is widely used for protecting the periphery of SiC mesas from surface breakdown [1,[START_REF] Lebedev | Strel'[END_REF] and because boron (D-center) is a typical background impurity in SiC fabricated by various techniques [7], the problem of the BVTC sign can be, in our opinion, ultimately solved only by taking into account (or eliminating) the effect of D-centers in studies of breakdown voltage in SiC p-n structures.

Figure 1 :

 1 Figure 1: temperature dependence of calculated (1-3) and experimental (4-5) values of F. Calculations were made for the following 𝛥𝑝 values, cm -3 : 1-10 10 ; 2-10 11 ; 3-10 12 . The experimental values of Ubr correspond to breakdown currents: 4-100 mA; 5-500 mA.

Acknowledgements.

This work was supported in part by US Department of Defence and Researcher Center of Schneider Electric S. A. (France)