A Two-Stage Feature Point Detection and Marking Approach Based on the Labeled Multi-Bernoulli Filter

Abstract

In recent years, various algorithms using random finite sets (RFS) to solve the issue of simultaneous localization and mapping (SLAM) have been proposed. Compared with the traditional method, the advantage of the RFS method is that it can avoid data association, landmark appearance and disappearance, missed detections, and false alarms in Bayesian recursion. There are many problems in the existing robot SLAM methods, such as low estimation accuracy, poor back-end optimization, etc. On the basis of previous studies, this paper presents a labeled random finite set (L-RFS) SLAM method. We describe a scene where the sensor moves along a given path and avoids obstacles based on the L-RFS framework. Then, we use the labeled multi-Bernoulli filter (LMB) to estimate the state of the sensor and feature points. At the same time, the B-spline curve is used to smooth the obstacle avoidance path of the sensor. The effectiveness of the algorithm is verified in the final simulation.

1. Introduction

Simultaneous localization and mapping (SLAM) is a process of gradually constructing an overall environmental map (feature point location, number, etc.) based on the detection data of the mobile sensor. Feature points are targets that need to be detected by the sensor in a given scene. The sensor also needs to estimate its location simultaneously. Nowadays, SLAM plays an extremely important role in many fields. It is considered to be a major process required by many mobile robot systems. Perception, positioning, and mapping are the key problems in unmanned driving [1], autonomous navigation [2], mining [3], agriculture [4], and many other fields. In the SLAM process, positioning is the estimation of the sensor position; the primary premise of mapping is to estimate the state of feature points in the given scene.

The SLAM issue was first put forward by Hugh Durrant-Whyte and John J. Leonard in 1986. The development of SLAM technology has gone through the early era (1986–2004), the algorithm analysis era (2004–2015), and the robustness era (2015-). There has been some review literature about SLAM frameworks and methods, including more classic ones such as articles written by Cadena et al. [5]. The SLAM problem can be described as follows: A robot moves in an unknown environment. Its position will be determined through the observation of the surrounding environment, and an environmental map will be constructed at the same time. In general, a SLAM system includes the following three processes: perception, positioning, and mapping. Perception is a prerequisite. It is mainly used to determine positioning and navigation in an unknown environment. Robot positioning and map construction are two mutually dependent and complementary processes.

In the field of autonomous navigation and unmanned driving, it is very important to estimate the location of feature points in the surrounding environment. The main theories are based on the Bayes theory. They can be classified into three classes: the traditional Bayes methods, the Bayesian nonparametric estimation methods, and the random finite sets (RFS)-based methods.

In traditional Bayesian estimation problems, there are two types of filtering algorithms commonly used. One is suitable for nonlinear problems. Various parametric filtering algorithms were developed on the basis of the Kalman filtering algorithm, such as extended Kalman filtering, Linear Kalman filtering, etc.; the other is a nonparametric filtering algorithm based on Monte Carlo simulation technology, such as particle filtering. Drovandi, C. et al. [6] presented a sequential Monte Carlo (SMC) algorithm that can be used for anyone-at-a-time Bayesian sequential design problem in the presence of model uncertainty where discrete data are encountered. A SMC algorithm is run in parallel for each model and the algorithm relies on a convenient estimator of the evidence of each model, which is essentially a function of importance sampling weights. Martino, L. et al. [7] designed a sequential Monte Carlo scheme for the dual purpose of Bayesian inference and model selection. They use interacting parallel particle filters, with each one addressing a different model. Urteaga, I. et al. [8] proposed a Sequential Monte Carlo (SMC) method for the filtering and prediction of time-varying signals under model uncertainty. Instead of resorting to model selection, they fuse the information from the considered models within the proposed SMC method. The particle filter algorithm will theoretically give better estimation results, but at an expensive computational cost. This method also has many drawbacks: it relies heavily on the estimation of the initial state and may converge or diverge very quickly. In addition, there is a particle degradation problem. Due to weight sampling, there are problems such as the lack of particles and loss of some data.

In recent years, many Bayesian nonparametric estimation methods have emerged to solve the MOT problem. Moraffah, B. et al. [9] proposed a new method in which they use information from previously tracked objects to track the dynamically changing number of objects. The method is based on nonparametric Bayesian modeling using diffusion processes and random trees. In the same year, Moraffah, B. also proposed robust nonparametric methods to model state priors for MOT problems [10]. These models are shown to be more flexible and robust than existing methods. In particular, the holistic approach estimates time-dependent object cardinality, provides object labels, and identifies object-dependent measurements. Furthermore, their framework dynamically competes with object birth/death and survival through correlated nonparametric processes. Yi, S. et al. [11] considered the robust state space filtering problem in the case where the transition probability density is unknown and possibly degenerate. The resulting robust filter has a Kalman-like structure and solves a minimax game; the least popular model is then naturally selected in the prescribed ambiguity set, which also contains non-Gaussian probability densities, while the other participants design the best filters for the least popular models. Hjalmarsson, H. et al. [12] suggested a simple and explicit method for estimating the model uncertainty, which is also applicable to severe under-modelling. A generalized Kalman-Bucy model under model uncertainty and a corresponding robust problem are studied in the literature [13]. Ji, S. et al. found that this robust problem is equivalent to an estimate problem under a sublinear operator. By using a Girsanov transformation and the minimax theorem, they prove that this problem can be reformulated as a classical Kalman-Bucy filtering problem under a new probability measure.

In recent years, many researchers have introduced the RFS formula in SLAM systems. Kalyan [14] described SLAM topics by using the Finite Set Statistics (FISST) framework. He used RFS to describe the state of feature points. Furthermore, the problem of mutual fusion between local maps can also be represented by RFS. Leung, K. [15] proposed RFS to represent the problem of feature point state estimation in positioning, mapping, and tracking. Under poor detection conditions, he provided a more robust solution. Inostroza, F. [16] proposed RFS to solve the data association problem in maximum likelihood SLAM. At the same time, he also proved to readers that maximum likelihood SLAM can be used without the need for external data association algorithms. Hampton, B. et al. [17] described the implementation of a tiny open-source and cost-effective SLAM robot. The robot utilized a novel occupancy grid SLAM algorithm based on the concept of RFS. Moratuwage, D. et al. [18] extended the SLAM filter framework based on RFS using a multi-sensor information fusion method and proposed a new solution to the multi-vehicle SLAM (MVSLAM) problem. They modelled measurements and landmark maps as RFS and factorized the MVSLAM posterior into the product of the joint vehicle trajectory posterior and the landmark map posterior conditioned on the vehicle trajectory. Du Hang yuan [19] described the status information, observation information, and environmental map in SLAM as RFS form. Subsequently, he proposed probability hypothesis density (PHD) to estimate the state of feature points under the framework of Bayes estimation. When the number of feature points in the environment is unknown, Mullane [20] proposed the SLAM Bayesian framework. The key to this framework modeling is to represent the map as a set of limited feature values. Using RFS theory, the SLAM problem will be transformed into a posterior estimation problem based on Bayesian filtering.

Starting from the point process theory, Mahler [21] proposed the theoretical method tool of Finite Set Statistics (FISST), which provided a theoretical basis for engineering calculations for target tracking and detection using RFS. Due to the low accuracy of robot mapping in environments with dense clutter and many map feature points, the literature [22] proposed a RFS SLAM method based on amplitude information. This method used the amplitude information measured by the map feature to obtain the likelihood function of the map feature and clutter. It is used to improve the estimation accuracy of the feature map in the SLAM process. In the PHD prediction stage, the map that has been observed before the last moment is used as the prior information of the prediction stage. Adding the prior information improves the estimation accuracy of the position and number of feature points [23]. In Bayesian filtering, Li, T. et al. [24] considered the rebirth, death, and regeneration of feature points, and gave the birth intensity of feature points to improve the detection accuracy of the sensor.

FISST has laid a scientific and rigorous theoretical foundation for the multi-target tracking problem. Among them, based on the RFS and the probability statistics of sets, the multi-target Bayesian equation for the multi-target tracking problem can be obtained [21,25]. In the multi-target tracking process, the number of targets may change with time and the state of each target is also constantly changing. The number of measurements also changes randomly and each measurement itself can be regarded as a random variable. The statistical model of the problem can be constructed as RFS, and the complex data association process can be effectively solved. In multitarget tracking, a closed form solution to the Bayes multitarget filter, which can also output target tracks, is given in [26,27,28]. The main breakthrough is the first multitarget conjugate prior with respect to the standard multitarget likelihood function. The prior is called the generalized labeled multi-Bernoulli prior. Additionally, this multi-target prior is also closed under the Chapman-Kolmogorov equation for the standard multi-target transition density [27].

In multi-target tracking and labeling, the objective is to jointly estimate the number of trajectories and their states from a sequence of noisy and cluttered observation sets. A multi-target system is fundamentally different from a single-target system in that the number of states changes with time due to births and deaths of targets. In addition, existing targets may or may not be detected and the sensor also receives a set of spurious measurements (clutter) not originating from any target. As a result, at each time step the measurement is a collection of detections, only some of which are generated by targets. Stephan R et al. [29] proposed a generalization of the multi-Bernoulli filter called the labeled multi-Bernoulli filter that outputs target tracks. Moreover, the labeled multi-Bernoulli filter does not exhibit a cardinality bias due to a more accurate update approximation compared to the multi-Bernoulli filter by exploiting the conjugate prior form for labeled Random Finite Sets. The proposed filter can be interpreted as an efficient approximation of the δ-Generalized Labeled Multi-Bernoulli filter. It inherits the advantages of the multi-Bernoulli filter in regard to particle implementation and state estimation. It also inherits the advantages of the δ-Generalized Labeled Multi-Bernoulli filter in that it outputs (labeled) target tracks and achieves better performance. When it comes to the application of the LMB filtering method to the SLAM problems, the LMB filter was introduced as an efficient approximation of the computationally expensive δ-Generalized LMB (δ-GLMB) filter. The LMB filter converts its representation of an LMB distribution to the δ-GLMB form and back during the measurement update step. Hendrik, D. et al. [30] addressed the simultaneous localization and mapping (SLAM) problem and proposed a Rao-Blackwellized implementation of the Labeled Multi-Bernoulli SLAM (LMB-SLAM) filter. Further, they established that the LMB-SLAM does not require the approximations used in Probability Hypothesis Density SLAM (PHD-SLAM). Moratuwage, D. et al. [31] presented a SLAM solution using an efficient variant of the δ-GLMB filter (δ-GLMB-SLAM) based on Gibbs sampling, which is computationally comparable to LMB-SLAM, yet more accurate and robust against sensor noise, measurement clutter, and feature detection uncertainty. Herrmann, M. et al. [32] described a novel method to additionally incorporate multiple hypotheses for fusing the measurements of the object reference points using an extension to the previously presented Labeled Multi-Bernoulli (LMB) filter.

By treating the map as an RFS and updating the first-order moment of its multi-target density, a local SLAM method based on random sets is used on each sensor. The map is regarded as a random finite set and the first-order moment of its multi-object density is updated, which is called PHD. The consensus on the map PHDs is adopted to disseminate map information through the sensor team, while also taking into account the different and time-varying fields of view of the team members [33]. Since low-cost sensors are prone to missed detections and false alarms, Ristic B. et al. [34] proposed an occupied grid algorithm for SLAM systems. The algorithm used the RFS framework to describe sensor observation information. The Rao-Blackwellised particle filter is also used to estimate the sensor state. Based on the existing theory, this paper proposes a two-stage SLAM feature point detection and marking algorithm based on LMB filtering. The first stage is to plan the sensor’s initial path according to the given scene size. We refer to the feature points that appear on the initial path as obstacles. In the second stage of path planning, the sensor avoids obstacles during movement and detects the position of obstacles. Subsequently, LMB filtering is used to estimate the state of sensors and feature points. The B-spline curve is used to smooth the path of obstacle avoidance. Finally, the algorithm was verified.

The main innovation of our paper is to provide a two-stage algorithm based on the labeled RFS theory for the SLAM problem; specifically, we divide the sensor path planning into rough and refined planning stages. Considering the limited detection field for the mobile sensor platform, we use the LMB filtering method to estimate the state of the feature points for its simplicity and relatively better precision.

The structure of this paper is organized as follows: We explain relevant background knowledge and problem descriptions, including L-RFS and SLAM problem descriptions, in Section 2. Subsequently, in Section 3, we describe the two-stage sensor path planning (initial path planning and local obstacle avoidance) and LMB filtering algorithm in detail. Section 4 describes the algorithm simulation and the simulation results are organized into charts to illustrate the effectiveness of the algorithm in this paper. Section 5 is the conclusion of this paper.

2. Background Knowledge and Problem Description

2.1. L-RFS Model for Map Observation

The robot positioning process in the SLAM system is to estimate the position of the robot when the external environment map is known. On the contrary, in the process of mapping, the state of the robot or the sensor is known. During the movement of the sensor, the measurement information will be acquired and saved, and will be used to estimate the state of the feature points. Accurate positioning requires an unbiased map, but such a map also requires accurate location estimation to describe. The SLAM system combines the two processes of positioning and mapping, so the states of robots or sensors and feature points must be estimated together. These two processes are complementary and inseparable.

In the RFS framework, the number of elements in an RFS variable is random, and the elements themselves are random and disordered. ${\mathrm{X}}_{\mathrm{k}}$ and ${\mathrm{Z}}_{\mathrm{k}}$ are defined as a finite set composed of state information and observation information. In the case of extending from a single feature point to multiple feature points, the feature point status information can be described as a set [35,36]:

$${\mathrm{X}}_{\mathrm{k}}=\{{\mathrm{x}}_{\mathrm{k},1},{\mathrm{x}}_{\mathrm{k},2},\dots ,{\mathrm{x}}_{\mathrm{k},\mathrm{i}}\}\in \mathcal{F}(\mathcal{X})$$

(1)

${\mathrm{x}}_{\mathrm{k},\mathrm{i}}$ represents the state of the i-th feature point at time $\mathrm{k}$. The state set ${\mathrm{X}}_{\mathrm{k}}$ contains state information of multiple feature points at time $\mathrm{k}$. The RFS of the feature point state can be described by the following model:

$${\mathrm{X}}_{\mathrm{k}}=[{\cup }_{\mathrm{x}\in {\mathrm{X}}_{\mathrm{k}-1}}{\mathrm{S}}_{\mathrm{k}|\mathrm{k}-1}(\mathrm{x})]\cup [{\cup }_{\mathrm{x}\in {\mathrm{X}}_{\mathrm{k}-1}}{\mathrm{B}}_{\mathrm{k}|\mathrm{k}-1}(\mathrm{x})]\cup {\mathrm{\Gamma }}_{\mathrm{k}}(\mathrm{x})$$

(2)

where ${\mathrm{S}}_{\mathrm{k}}(\mathrm{x})$ represents the survival feature points, ${\mathrm{B}}_{\mathrm{k}}(\mathrm{x})$ represents the newly born feature points, and ${\mathsf{\Gamma }}_{\mathrm{k}}\left(\mathrm{x}\right)$ represents the regenerated feature points at time.

Similarly, the measurement RFS can be modeled as:

$${\mathrm{Z}}_{\mathrm{k}}=\{{\mathrm{z}}_{\mathrm{k},1},{\mathrm{z}}_{\mathrm{k},2},\dots ,{\mathrm{z}}_{\mathrm{k},\mathrm{j}}\}\in \mathcal{F}(\mathcal{Z})$$

(3)

${\mathrm{z}}_{\mathrm{k},\mathrm{i}}$ represents the measured value of the i-th feature point at time $\mathrm{k}$. $\mathrm{i}$ and $\mathrm{j}$ are the true value and the measured value of the number of feature points, respectively. Due to missed detections and false alarms, $\mathrm{i}$ and $\mathrm{j}$ are not necessarily the same. $\mathcal{F}(\mathcal{X})$ and $\mathcal{F}(\mathcal{Z})$ are the collections of the state space $\mathcal{X}$ and the finite subset of the observation space $\mathcal{Z}$, respectively.

Unlike traditional RFS, L-RFS adds a unique label $\mathrm{l}\in \mathcal{L}=\{{\mathrm{\alpha }}_{\mathrm{i}},\mathrm{i}\in \mathrm{N}\}$ to the state space. The following formula represents the static characteristic RFS of the entire environment map,

$${\mathrm{M}}_{\mathrm{k}}=\{{\mathrm{m}}_{\mathrm{k},1}^{},{\mathrm{m}}_{\mathrm{k},2}^{},\dots ,{\mathrm{m}}_{\mathrm{k},\mathrm{i}}^{}\}$$

(4)

where ${\mathrm{m}}_{\mathrm{k},1}^{},{\mathrm{m}}_{\mathrm{k},2}^{},\dots ,{\mathrm{m}}_{\mathrm{k},{\mathrm{n}}_{\mathrm{m}}}^{}$ is the feature vector at time $\mathrm{k}$. ${\mathrm{M}}_{\mathrm{k}-1}$ is a subset of the map that has been traversed, and the currently observed features are represented by $\mathrm{C}\left({\mathrm{x}}_{\mathrm{k}}\right)$.

$${\mathrm{M}}_{\mathrm{k}}={\mathrm{M}}_{\mathrm{k}-1}\cap \mathrm{C}({\mathrm{x}}_{0:\mathrm{k}-1})$$

(5)

where $\mathrm{C}({\mathrm{x}}_{0:\mathrm{k}-1})=\mathrm{C}({\mathrm{x}}_0)\cup \mathrm{C}({\mathrm{x}}_1)\cup \dots \cup \mathrm{C}({\mathrm{x}}_{\mathrm{k}-1})$. Therefore, ${\mathrm{M}}_{\mathrm{k}-1}$ represents the union of each observation feature space before $\mathrm{k}-1$, so the traversal process map or feature update equation is:

$${\mathrm{M}}_{\mathrm{k}}={\mathrm{M}}_{\mathrm{k}-1}\cup (\mathrm{C}({\mathrm{x}}_{\mathrm{k}-1})\cap {\overline{\mathrm{M}}}_{\mathrm{k}-1})$$

(6)

where ${\overline{\mathrm{M}}}_{\mathrm{k}-1}=\mathrm{M}-{\mathrm{M}}_{\mathrm{k}-1}$. $\mathrm{C}({\mathrm{x}}_{\mathrm{k}-1})\cap {\overline{\mathrm{M}}}_{\mathrm{k}-1}$ is the newly observed feature point, which can be expressed by independent RFS, and the transfer density of the RFS map can be expressed as:

$${\mathrm{f}}_{\mathrm{M}}({\mathrm{M}}_{\mathrm{k}}|{\mathrm{M}}_{\mathrm{k}-1},{\mathrm{x}}_{\mathrm{k}})={\displaystyle \sum _{\mathrm{w}\subseteq {\mathrm{M}}_{\mathrm{k}}}{\mathrm{f}}_{\mathrm{M}}(\mathrm{w}|{\mathrm{M}}_{\mathrm{k}-1})}{\mathrm{f}}_{\mathrm{B}}({\mathrm{M}}_{\mathrm{k}}-\mathrm{w}|{\mathrm{x}}_{\mathrm{k}})$$

(7)

where ${\mathrm{f}}_{\mathrm{M}}(\mathrm{w}|{\mathrm{M}}_{\mathrm{k}-1})$ is the transition density of the feature set in the detection range from time $\mathrm{k}-1$ to time $\mathrm{k}$; ${\mathrm{f}}_{\mathrm{B}}({\mathrm{M}}_{\mathrm{k}}-\mathrm{w}|{\mathrm{x}}_{\mathrm{k}})$ is the density of the new feature ${\mathrm{B}}_{\mathrm{k}}({\mathrm{x}}_{\mathrm{k}})$ in the detection range at time.

2.2. Mobile Robot SLAM Problem Description

SLAM technology based on Lidar and visual images has become an indispensable new technology in current SLAM systems. Because it has many advantages, visual SLAM can accurately obtain the state of feature points, without prearranging the scene, and can fuse multiple sensors. Laser SLAM can work in poor light environments and generate an occupation grid map that is easy to navigate [37]. There are two main tasks of laser SLAM: one is to estimate the position of the subject carrying the lidar in motion, and the other is to simultaneously build a map of the surrounding environment. Accurate positioning requires an accurate map. An accurate map comes from accurate positioning. Positioning focuses on self-understanding, while mapping focuses on external understanding. The classic framework of SLAM is mainly composed of the following five parts: sensor, visual odometer, back-end, closed-loop detection, and mapping, as shown in the Figure 1 [38].

Sensors and visual odometers are called front-end in visual SLAM. Sensors are mainly used to collect environmental information. The purpose of the visual odometer is to estimate its state based on the image data collected by the sensor. The formed pose information at different moments is sorted and optimized to obtain a complete environmental map. The loop detection link is obviously a feedback link. It compares the information of the sensor, the visual odometer, and the back-end to determine whether there is drift or repetition in the position of the robot’s movement. If there is, it will feedback its information to the back-end to reduce the cumulative error [38].

In the SLAM system, we can use the Bayesian framework to describe and define two variables for the sensor: one is the state quantity, the other is the observation measurement. State variables: ${\mathrm{x}}_{1:\mathrm{k}}\cong \left({\mathrm{x}}_1,{\mathrm{x}}_2\dots \dots {\mathrm{x}}_{\mathrm{k}}\right)$, observed variables: ${\mathrm{z}}_{1:\mathrm{k}}\cong \left({\mathrm{z}}_1,{\mathrm{z}}_2\dots \dots {\mathrm{z}}_{\mathrm{k}}\right)$. Among them, the state quantity refers to the posture, position velocity, and three-dimensional point coordinates of the sensor at each moment, and ${\mathrm{x}}_{\mathrm{k}}$ refers specifically to the state at time $\mathrm{k}$. The observation is the position of the feature point detected by the sensor in the current state. Similarly, ${\mathrm{z}}_{\mathrm{k}}$ is the observation at time $\mathrm{k}$.

SLAM can be modeled as the state estimation problem of the robot system:

$$\begin{gathered} {\mathrm{x}}_{\mathrm{k}}=\mathrm{f}({\mathrm{x}}_{\mathrm{k}-1},{\mathrm{u}}_{\mathrm{k}},{\mathrm{w}}_{\mathrm{k}})\hspace{1em}\mathrm{k}=1,\dots ,\mathrm{K} \\ {\mathrm{z}}_{\mathrm{k},\mathrm{j}}=\mathrm{h}({\mathrm{m}}_{\mathrm{j}},{\mathrm{x}}_{\mathrm{k}},{\mathrm{v}}_{\mathrm{k},\mathrm{j}})\hspace{1em}\mathrm{k}=0,\dots ,\mathrm{K} \end{gathered}$$

(8)

where ${\mathrm{x}}_{\mathrm{k}}$ is the pose of the robot at time $\mathrm{k}$; ${\mathrm{u}}_{\mathrm{k}}$ is the control input; ${\mathrm{w}}_{\mathrm{k}}$ is the measurement noise; ${\mathrm{z}}_{\mathrm{k},\mathrm{j}}$ is the observation of the environmental feature point ${\mathrm{m}}_{\mathrm{j}}$ at time $\mathrm{k}$; $\mathrm{j}$ is the number of feature points; and ${\mathrm{v}}_{\mathrm{k},\mathrm{j}}$ is the observation noise. Using the control amount ${\mathrm{u}}_{\mathrm{k}}$ of the driver and the environmental observation data ${\mathrm{z}}_{\mathrm{k},\mathrm{j}}$ eliminates the influence of noises such as ${\mathrm{w}}_{\mathrm{k}}$ and ${\mathrm{v}}_{\mathrm{k},\mathrm{j}}$. The state of the robot system, namely the pose ${\mathrm{x}}_{\mathrm{k}}$ and the feature point ${\mathrm{m}}_{\mathrm{j}}$, is estimated for the uncertainty of the state space, thereby obtaining the estimate of the ${\mathrm{x}}_{\mathrm{k}}$ sequence. This process is called positioning. The process of estimating the ${\mathrm{m}}_{\mathrm{j}}$ sequence is called mapping.

3. Two-Stage Feature Point Detection and Estimation Algorithm Based on the LMB Filter

3.1. Two-Stage Path Planning for Sensor

The algorithm proposed in this paper is the two-stage path planning based on event triggering. The sensor is divided into two modes: driving mode and obstacle avoidance mode. The first stage is the initial path planning, focusing on the overall situation. At this time, the sensor enters the driving mode and drives normally according to the given route. For initial path planning, all information about the scene is known in advance and this information is used as the basis for path planning. In other words, an optimal path is found between the starting point and the ending point according to the research purpose. Thus, in the process of driving, the detection range of the sensor covers the entire scene to the maximum extent. The accuracy of path planning depends on the accuracy of environmental information acquisition. Although the planning result is overall better, it is not detailed enough. If the sensor detects an obstacle during the movement, it triggers the second phase of local path planning. The sensor enters obstacle avoidance mode. In this process, the sensor uses the measurement information to update and adjust the original path locally based on the initial path, avoiding obstacles in real time. After successfully avoiding the obstacle, the sensor returns to the driving mode and continues driving according to the initial path. There is no essential difference between initial path planning and partial path planning. The sensor works in stages and can better plan the walking path from the start point to the end point.

3.2. Mobile Sensor Modeling

According to the finite coverage theorem, it is known that if $\mathrm{H}$ is an (infinite) open coverage of closed interval $\left[\mathrm{a},\mathrm{b}\right]$, then a finite number of open intervals can be selected from $\mathrm{H}$ to cover interval $\left[\mathrm{a},\mathrm{b}\right]$. The limited coverage theorem is a useful and important theorem. It is an important method to deal with mathematical analysis problems, and it has a wide range of applications in various fields of mathematics. The function of the finite coverage theorem is to select a finite number of open intervals from an infinite number of open intervals covering a closed interval, and also cover this closed interval [39].

In this paper, the sensor is modeled as a constant-speed straight line model (CV) and a constant-speed turning model (CT). The sensor follows the following dynamic model during driving:

$$\left\{\begin{array}{l}{\mathrm{x}}_{\mathrm{k}+1}={\mathrm{F}}_{\mathrm{k}}{\mathrm{x}}_{\mathrm{k}}+{\mathrm{G}}_{\mathrm{k}}{\mathrm{u}}_{\mathrm{k}}+{\mathrm{v}}_{\mathrm{k}}\\ {\mathrm{u}}_{\mathrm{k}+1}={\mathrm{L}}_{\mathrm{k}}{\mathrm{u}}_{\mathrm{k}}\\ {\mathrm{y}}_{\mathrm{k}+1}={\mathrm{O}}_{\mathrm{k}+1}{\mathrm{x}}_{\mathrm{k}+1}+{\mathrm{w}}_{\mathrm{y},\mathrm{k}+1}\end{array}\right.$$

(9)

where ${\mathrm{F}}_{\mathrm{k}}$, ${\mathrm{G}}_{\mathrm{k}}$, ${\mathrm{L}}_{\mathrm{k}}$, and ${\mathrm{O}}_{\mathrm{k}}$ are coefficient matrices. ${\mathrm{v}}_{\mathrm{k}}$ and ${\mathrm{w}}_{\mathrm{y},\mathrm{k}+1}$ are system noises. ${\mathrm{u}}_{\mathrm{k}}$ is the control input at time $\mathrm{k}$. ${\mathrm{x}}_{\mathrm{k}}$ is the sensor state at time $\mathrm{k}$. ${\mathrm{y}}_{\mathrm{k}+1}$ is the observation of the sensor at time $\mathrm{k}+1$. When the sensor moves in a straight line,

$${\mathrm{F}}_{\mathrm{k}}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],{\mathrm{G}}_{\mathrm{k}}=\left[\begin{array}{cc}\mathrm{T}& 0\\ 0& \mathrm{T}\end{array}\right],{\mathrm{L}}_{\mathrm{k}}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],{\mathrm{O}}_{\mathrm{k}+1}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$$

When the sensor moves along a circular arc with a turning rate $\mathrm{w}$,

$$\begin{gathered} {\mathrm{F}}_{\mathrm{k}}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],{\mathrm{L}}_{\mathrm{k}}=\left[\begin{array}{cc}\mathrm{c}\mathrm{o}\mathrm{s}\mathrm{w}\mathrm{T}& -\mathrm{s}\mathrm{i}\mathrm{n}\mathrm{w}\mathrm{T}\\ \mathrm{sinwT}& \mathrm{c}\mathrm{o}\mathrm{s}\mathrm{w}\mathrm{T}\end{array}\right], \\ {\mathrm{G}}_{\mathrm{k}}=\left[\begin{array}{cc}\frac{\mathrm{s}\mathrm{i}\mathrm{n}\mathrm{w}\mathrm{T}}{\mathrm{w}}& -\frac{1-\mathrm{c}\mathrm{o}\mathrm{s}\mathrm{w}\mathrm{T}}{\mathrm{w}}\\ \frac{1-\mathrm{c}\mathrm{o}\mathrm{s}\mathrm{w}\mathrm{T}}{\mathrm{w}}& \frac{\mathrm{s}\mathrm{i}\mathrm{n}\mathrm{w}\mathrm{T}}{\mathrm{w}}\end{array}\right],{\mathrm{O}}_{\mathrm{k}+1}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right] \end{gathered}$$

where $\mathrm{T}$ is the period and $\mathrm{w}$ is the angular velocity of the sensor movement.

The observation model is:

$${\mathrm{z}}_{\mathrm{k}+1}=\left\{\begin{array}{l}\mathrm{H}{\mathrm{m}}_{\mathrm{k}}+{\mathrm{w}}_{\mathrm{k}},𝓅({\mathrm{m}}_{\mathrm{k}})\in \mathrm{S}\stackrel{\Delta }{=}\mathrm{R}(𝓅({\mathrm{x}}_{\mathrm{k}}),{\mathrm{r}}_{\mathrm{k}})\\ \varnothing ,𝓅({\mathrm{m}}_{\mathrm{k}})\notin \mathrm{S}\stackrel{\Delta }{=}\mathrm{R}(𝓅({\mathrm{x}}_{\mathrm{k}}),{\mathrm{r}}_{\mathrm{k}})\end{array}\right.$$

(10)

$$\mathrm{H}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]$$

where $𝓅(\cdot )$ is the position function and $\mathrm{S}$ represents the detection area of the sensor. $\mathrm{R}(𝓅({\mathrm{x}}_{\mathrm{k}}),{\mathrm{r}}_{\mathrm{k}})$ represents a circular area where the obstacle position is the center of the circle and ${\mathrm{r}}_{\mathrm{k}}$ is the radius. ${\mathrm{w}}_{\mathrm{k}}$ is noise.

3.3. Feature Points Estimation Method Based on LMB

Both unlabeled RFS and labeled RFS belong to the random finite point process [29,30]. The LMB filter can realize the requirement of feature point detection and marking.

First, add the label in the feature point state set, $\mathcal{M}=\{({\mathrm{m}}_{\mathrm{i}},{\mathrm{l}}_{\mathrm{i}})\}$ $(\mathrm{i}=1,2,\dots ,\mathrm{m}),{\mathrm{m}}_{\mathrm{i}}\in \mathcal{M},{\mathrm{l}}_{\mathrm{i}}\in \mathrm{L}$, $\mathcal{M}$ is the feature point state space, $\mathrm{L}$ is the label space, and $|\cdot |$ represents the potential of the set. LMB RFS can be represented by parameter $\{({\mathrm{r}}^{(\mathrm{l})},{\mathrm{p}}^{(\mathrm{l})}(\cdot ),\mathrm{l}\in \mathrm{L}\}$, where ${\mathrm{r}}^{(\mathrm{l})}\in [0,1]$ is the probability of the l-th feature point label, assuming that the position is the true position. The LMB RFS density is as follows [40,41,42]:

$$\mathrm{\pi }(\mathrm{{\rm X}})=\Delta (\mathrm{{\rm X}})\mathrm{w}(\mathcal{L}(\mathrm{X})){\mathrm{p}}^{\left\{\mathrm{{\rm X}}\right\}}$$

(11)

$$\Delta (\left\{\mathrm{{\rm X}}\right\})={\mathrm{\delta }}_{\left|\left\{\mathrm{{\rm X}}\right\}\right|}(|\mathcal{L}(\left\{\mathrm{{\rm X}}\right\})|)$$

(12)

$$\mathrm{w}(\mathrm{L})={\displaystyle \prod _{\mathrm{i}\in \mathrm{L}}(1-{\mathrm{r}}^{\mathrm{i}})}{\displaystyle \prod _{\mathrm{l}\in \mathrm{L}}\frac{1_{\mathrm{L}}(\mathrm{l}){\mathrm{r}}^{\mathrm{l}}}{1-{\mathrm{r}}^{\mathrm{l}}}}$$

(13)

$${\mathrm{p}}^{\mathrm{{\rm X}}}\stackrel{\Delta }{=}{\displaystyle \prod _{(\mathrm{x},\mathrm{l})\in \mathrm{{\rm X}}}{\mathrm{p}}^{(\mathrm{l})}(\mathrm{x})}$$

(14)

where $\Delta (\mathcal{M})$ is a discrete label indicator; $\mathcal{L}(\mathcal{M})$ is a label set; $\mathrm{w}(\mathcal{L}(\mathcal{M}))$ is a weighting coefficient and depends on the status label $\mathcal{L}(\mathcal{M})$; and $1_{\mathrm{L}}(\mathcal{M})$ is an indicator function, when $\mathcal{M}\subseteq \mathrm{L}$ is equal to 1, and others are equal to 0.

The LMB filter algorithm is used to detect and estimate the feature points. The LMB filter is mainly divided into two steps: the prediction step and the update step [43,44]. The feature point state is $(\mathrm{m},\mathrm{l})$, the probability of a feature point being detected is ${\mathrm{p}}_{\mathrm{D}}(\mathrm{m},\mathrm{l})$, and the probability of not being detected is ${\mathrm{q}}_{\mathrm{D}}(\mathrm{m},\mathrm{l})=1-{\mathrm{p}}_{\mathrm{D}}(\mathrm{m},\mathrm{l})$.

Prediction: Consider the survival and rebirth of feature points here, so the parameters of LMB predicting the density of multiple feature points can be expressed as:

$${\mathrm{\pi }}_{+}={\{({\mathrm{r}}_{+,\mathrm{S}}^{(\mathrm{l})},{\mathrm{p}}_{+,\mathrm{S}}^{(\mathrm{l})})\}}_{\mathrm{l}\in \mathrm{L}}\cup {\{({\mathrm{r}}_{\mathrm{B}}^{(\mathrm{l})},{\mathrm{p}}_{\mathrm{B}}^{(\mathrm{l})})\}}_{\mathrm{l}\in \mathrm{B}}$$

(15)

$${\mathrm{r}}_{+,\mathrm{S}}^{(\mathrm{l})}={\mathsf{\eta }}_{\mathrm{S}}(\mathrm{l}){\mathrm{r}}^{(\mathrm{l})}$$

(16)

$${\mathrm{p}}_{+,\mathrm{S}}^{(\mathrm{l})}=〈{\mathrm{p}}_{\mathrm{S}}(\cdot ,\mathrm{l}),\mathrm{f}(\mathrm{m}|\cdot ,\mathrm{l})〉/{\mathsf{\eta }}_{\mathrm{S}}(\mathrm{l})$$

(17)

$${\mathsf{\eta }}_{\mathrm{S}}(\mathrm{l})=〈{\mathrm{p}}_{\mathrm{S}}(\cdot ,\mathrm{l}),{\mathrm{p}}^{(\mathrm{l})}(\cdot )〉$$

(18)

The LMB parameter for predicting feature points is $\{{\mathrm{r}}_{+}^{(\mathrm{l})},{\{{\mathrm{w}}_{+,\mathrm{j}}^{(\mathrm{l})},{\mathrm{m}}_{+,\mathrm{j}}^{(\mathrm{l})}\}}_{\mathrm{j}=1}^{{\mathrm{J}}_{+}^{(\mathrm{l})}}\}$, where ${\mathrm{L}}_{+}=\mathrm{L}\cup \mathrm{B}$; $〈\cdot ,\cdot 〉$ is the inner product function.

Update: Suppose that the multi-feature point prediction is recorded as the LMB RFS defined in the state space ${\mathcal{M}}_{+}$ and the label space ${\mathrm{L}}_{+}$, the parameter set is then expressed as follows:

$${\mathrm{\pi }}_{+}={\{({\mathrm{r}}_{+}^{(\mathrm{l})},{\mathrm{p}}_{+}^{(\mathrm{l})})\}}_{\mathrm{l}\in {\mathrm{L}}_{+}}$$

(19)

The posterior density of multiple feature points can be expressed as [40]:

$$\mathrm{\pi }(\cdot |\mathrm{Z})={\{({\mathrm{r}}^{(\mathrm{l})},{\mathrm{p}}^{(\mathrm{l})}(\mathrm{m}))\}}_{\mathrm{l}\in {\mathrm{L}}_{+}}$$

(20)

$${\mathrm{r}}^{(\mathrm{l})}={\displaystyle \sum _{({\mathrm{I}}_{+},\mathsf{\theta })\in \mathrm{F}({\mathrm{L}}_{+})\times {\mathrm{\Theta }}_{{\mathrm{I}}_{+}}}{\mathrm{w}}^{({\mathrm{I}}_{+},\mathsf{\theta })}(\mathrm{Z})1_{{\mathrm{I}}_{+}}(\mathrm{l})}$$

(21)

$${\mathrm{p}}^{(\mathrm{l})}(\mathrm{m})=\frac{1}{{\mathrm{r}}^{(\mathrm{l})}}\times {\displaystyle \sum _{({\mathrm{I}}_{+},\mathsf{\theta })\in \mathrm{F}({\mathrm{L}}_{+})\times {\mathrm{\Theta }}_{{\mathrm{I}}_{+}}}{\mathrm{w}}^{({\mathrm{I}}_{+},\mathsf{\theta })}(\mathrm{Z})\times 1_{{\mathrm{I}}_{+}}(\mathrm{l}){\mathrm{p}}^{\left(\mathsf{\theta }\right)}(\mathrm{m},\mathrm{l}|\mathrm{Z})}$$

(22)

$${\mathrm{w}}^{({\mathrm{I}}_{+},\mathsf{\theta })}(\mathrm{Z})\propto {\mathrm{w}}_{+}({\mathrm{I}}_{+}){[{\mathsf{\eta }}_{\mathrm{Z}}^{\left(\mathsf{\theta }\right)}]}^{{\mathrm{I}}_{+}}$$

(23)

$${\mathrm{p}}^{\left(\mathsf{\theta }\right)}(\mathrm{x},\mathrm{l}|\mathrm{Z})=\frac{{\mathrm{p}}_{+}(\mathrm{m},\mathrm{l}){\mathsf{\psi }}_{\mathrm{Z}}(\mathrm{m},\mathrm{l};\mathsf{\theta })}{{\mathsf{\eta }}_{\mathrm{Z}}^{\left(\mathsf{\theta }\right)}(\mathrm{l})}$$

(24)

$${\mathsf{\eta }}_{\mathrm{Z}}^{\left(\mathsf{\theta }\right)}(\mathrm{l})=〈{\mathrm{p}}_{+}(\mathrm{m},\mathrm{l}){\mathsf{\psi }}_{\mathrm{Z}}(\mathrm{m},\mathrm{l};\mathsf{\theta })〉$$

(25)

$${\mathsf{\psi }}_{\mathrm{Z}}(\mathrm{m},\mathrm{l};\mathsf{\theta })=\left\{\begin{array}{l}\frac{{\mathrm{p}}_{\mathrm{D}}(\mathrm{m},\mathrm{l})\mathrm{g}({\mathrm{z}}_{\mathsf{\theta }(\mathrm{l})}|\mathrm{m},\mathrm{l})}{\mathrm{K}({\mathrm{z}}_{\mathsf{\theta }(\mathrm{l})})},\mathsf{\theta }(\mathrm{l})>0\\ {\mathrm{q}}_{\mathrm{D}}(\mathrm{m},\mathrm{l}),\mathsf{\theta }(\mathrm{l})=0\end{array}\right.$$

(26)

where ${\mathrm{p}}_{\mathrm{D}}(\mathrm{m},\mathrm{l})$ is the probability that the feature point labeled $\mathrm{l}$ is detected, and ${\mathrm{q}}_{\mathrm{D}}(\mathrm{m},\mathrm{l})=1-{\mathrm{p}}_{\mathrm{D}}(\mathrm{m},\mathrm{l})$ is the probability that the feature point $\mathrm{m}$ labeled $\mathrm{l}$ is not detected. $\mathrm{g}(\mathrm{z}|\mathrm{m},\mathrm{l})$ is the single feature point likelihood function of the feature point $\mathrm{m}$ labeled $\mathrm{l}$ with respect to the measurement $\mathrm{z}$. $\mathsf{\theta }$ represents the mapping from the label set to the measurement set. $\mathrm{K}(\cdot )$ represents the clutter function.

3.4. Obstacle Avoidance Strategy Based on Event Triggering

In the two-stage path planning mentioned in this article, the sensor is switched between the driving mode and the obstacle avoidance mode by event triggering, and the sensor enters the driving mode in the first stage and travels according to the given path. When formula (29) is satisfied, the sensor switches to the second stage obstacle avoidance mode.

$$\left|\right|𝓅({\mathrm{x}}_{\mathrm{k}})-𝓅({\mathrm{m}}_{\mathrm{k}})|{|}_2\le {\mathsf{\sigma }}_{\mathrm{m}\mathrm{i}\mathrm{n}}$$

(27)

$$\mathrm{K}=\mathrm{m}\mathrm{i}\mathrm{n}\{\mathrm{k}:||𝓅({\mathrm{x}}_{\mathrm{k}})-𝓅({\mathrm{m}}_{\mathrm{k}})|{|}_2\le {\mathsf{\sigma }}_{\mathrm{m}\mathrm{i}\mathrm{n}}\}$$

(28)

$$𝓅({\mathrm{y}}_{\mathrm{k}})=𝓅({\mathrm{x}}_{\mathrm{k}})+{\mathsf{\sigma }}_{\mathrm{m}\mathrm{i}\mathrm{n}}$$

(29)

where ${\mathsf{\sigma }}_{\mathrm{m}\mathrm{i}\mathrm{n}}$ is the safety distance, which is a constant. $\left|\right|𝓅({\mathrm{x}}_{\mathrm{k}})-𝓅({\mathrm{m}}_{\mathrm{k}})|{|}_2$ indicates the distance between the feature point and the sensor. When formula (28) is satisfied, the sensor switches from the driving mode to the mode of obstacle avoidance. Here, time is denoted as $\mathrm{k}$, and the minimum value is the time of event triggering, denoted as $\mathrm{K}$. In the obstacle avoidance phase, the preview point ${\mathrm{y}}_{\mathrm{k}}$ is first given. When the sensor travels to the preview point, it exits the obstacle avoidance mode and enters the driving mode.

Assuming that the radius of all obstacles in the scene is not greater than 5 m, the following figure shows the relationship curve between sensor obstacle avoidance radius $\mathrm{r}$ and driving speed $\mathrm{v}$. As shown in Figure 2, the two are directly proportional, that is, the faster the driving speed, the larger the obstacle avoidance radius, and correspondingly, the larger ${\mathsf{\sigma }}_{\mathrm{m}\mathrm{i}\mathrm{n}}$.

After determining the obstacle avoidance radius, the initial straight path is directly connected with the sensor obstacle avoidance arc path. This will cause the sensor to instantly change 90° in the direction of driving at the junction of the two paths. Sensors on the move cannot do this. Therefore, this paper uses the B-spline curve fitting method to smooth the path. The B-spline curve is widely used in path planning and it is an effective path optimization method. It has the characteristics of geometric invariance, convexity, and reduced deterioration. The B-spline curve is described by the following formula [41]:

$$\mathrm{C}(\mathrm{u})={\displaystyle \sum _{\mathrm{i}=0}^{\mathrm{n}}{\mathrm{N}}_{\mathrm{i},\mathrm{k}}(\mathrm{u})\mathrm{X}(\mathrm{i})}$$

(30)

$${\mathrm{N}}_{\mathrm{i},\mathrm{k}}(\mathrm{u})=\left\{\begin{array}{l}1,{\mathrm{u}}_{\mathrm{i}}\le \mathrm{u}\le {\mathrm{u}}_{\mathrm{i}+1}\\ 0,\mathrm{o}\mathrm{t}\mathrm{h}\mathrm{e}\mathrm{r}\mathrm{w}\mathrm{i}\mathrm{s}\mathrm{e}\end{array}\right.$$

(31)

$${\mathrm{N}}_{\mathrm{i},\mathrm{k}}(\mathrm{u})=\frac{\mathrm{u}-{\mathrm{u}}_{\mathrm{i}}}{{\mathrm{u}}_{\mathrm{i}+\mathrm{k}}-{\mathrm{u}}_{\mathrm{i}}}{\mathrm{N}}_{\mathrm{i},\mathrm{k}-1}(\mathrm{u})+\frac{{\mathrm{u}}_{\mathrm{i}+\mathrm{k}+1}-\mathrm{u}}{{\mathrm{u}}_{\mathrm{i}+\mathrm{k}+1}-{\mathrm{u}}_{\mathrm{i}+1}}{\mathrm{N}}_{\mathrm{i},\mathrm{k}+1}(\mathrm{u})$$

(32)

$$\mathrm{d}\mathrm{e}\mathrm{f}\mathrm{i}\mathrm{n}\mathrm{e}\frac{0}{0}=0$$

(33)

$${\mathrm{u}}_0\le {\mathrm{u}}_1\le \dots \le {\mathrm{u}}_{\mathrm{m}-1}\le {\mathrm{u}}_{\mathrm{m}}$$

(34)

where $\mathrm{C}\left(\mathrm{u}\right)$ indicates the fitted curve. $\mathrm{k}$ is the order of the spline curve, ${\mathrm{u}}_{\mathrm{i}}$ is the node on the curve after fitting, $\mathrm{X}(\mathrm{i})$) is the path point to be fitted, and ${\mathrm{N}}_{\mathrm{i},\mathrm{k}}(\mathrm{u})$ is the basis function.

4. Algorithm Simulation and Verification

The scene given in this paper is a square area with multiple feature points. The locations of the feature points are shown in Figure 3. The algorithm detects and labels feature points based on some parameters such as size and shape of a given scene. The straight-line part is the CV model, and the arc part is the CT model. As shown in Figure 4, the initial position of the path is (−800 m, −800 m) and the end position is (800 m, 800 m). According to the above obstacle avoidance strategy, the schematic diagram of sensor obstacle avoidance is shown in Figure 5.

Set the sensor to detect along the initial path shown in Figure 4. The parameter settings are shown in the table below.

All parameters in

Table 1. Sensor parameter setting.

Sensor	Parameter Settings
Straight-line speed	2 m/s
Arc travel speed	1 m/s
Detection step	10 m
Detection radius	50 m, 100 m, 150 m

can be adjusted at any time according to the number of obstacles in the scene and changes in position distribution.

We use the B-spline curve in the obstacle avoidance phase to smooth the sensor path. It can be observed from Figure 5 that according to the obstacle avoidance strategy mentioned above, the sensor effectively avoids the characteristic points appearing on the initial path.

When the initial path is a serpentine curve,, based on the initial path, we will verify whether the algorithm proposed in this paper can accurately detect the marked feature points. When the feature points in the scene are static, Figure 6 and Figure 7 are the simulation results obtained by using the LMB filtering method and the GLMB filtering method respectively.

When the feature points are changed from static to dynamic, the simulation results are shown in Figure 8 and Figure 9.

Considering the scene where multiple feature points are moving, the number of feature points detected by the sensor changes all the time and the probability that the feature points are detected is ${\mathrm{p}}_{\mathrm{D}}=0.98$. The clutter is uniformly distributed, the clutter intensity is set to 20, the clutter area $\mathrm{S}=\left[-1000,1000\left]\times \right[-1000,1000\right]$, and the process noise ${\mathrm{Q}}_{\mathrm{k}}={0.5}^2\times \mathrm{B}\times {\mathrm{B}}^{\mathrm{T}}$. The dynamic feature points make uniform linear motion in the two-dimensional plane, and the detection time is 100 s.

When the initial path of the sensor is a spiral curve, the feature point marking effect is as shown in the figure below. When the feature points in the scene are static, Figure 10 and Figure 11 are the simulation results obtained by using the LMB filtering method and the GLMB filtering method respectively.

Subsequently, we change the feature points in the scene from static to dynamic. Figure 12 and Figure 13 show the situation where the feature points are detected and marked by the sensor during the movement.

From the pictures shown above, we can see that whether the feature points are static or dynamic, the red markers and blue markers have a high degree of coincidence. This shows that the LMB filtering method works well for feature point labeling. Furthermore, as the sensor travels along the initial path, its detection range covers almost the entire given area. This also shows that the initial path planning effect of the first stage is good.

When there are ten feature points in the scene, Figure 14 shows the number of obstacles detected by the sensor at different times while walking along the path from the start point to the end point. This shows that the local path planning in the second stage has achieved the expected effect. The above simulation results show that the method proposed in this paper has a good effect on the estimation and detection of the position and quantity of feature points.

Consider a scene with ten feature points. The scenario ran for 100 time steps. $\mathrm{T}=1$ is the sampling period. In the process of the sensor following the initial path, it is assumed that the given ten obstacles are independent of each other. The algorithm proposed in this paper can detect and mark multiple obstacles well. The simulation results also correctly estimated the location and number of feature points, and the incidence of false alarms and missed detections is very small.

Then, we compared the performance of the two algorithms, LMB filtering and GLMB filtering. The 64-bit Win10 system PC CPU used to test the algorithm was: Intel(R) Core (TM) i5-10210U CPU @ 1.60 GHz 2.11 GHz. RAM: 12.0 GB. In the two cases where there are 10 feature points in the scene, we have performed ten simulation experiments in total. In each simulation experiment, the program runs for 100 time steps. For the above experimental simulation, the average CPU time is shown in the

Table 2. Comparison of the time spent in the experiments with the LMB and GLMB algorithms.

Algorithm	LMB	GLMB
Average time (100 MCs)	102.25	158.02

.

The results are further verified by the Optimal Sub Pattern Assignment (OSPA) distance. OSPA is a consistency measurement method for the overall performance evaluation of the feature point identification, detection, and tracking system. It defines a measurement distance in the system state space, which can be used to measure the error between the true value and the estimated value.

Define the true state set of the detected feature points as $\mathrm{X}$ and the estimated state set after the algorithm processing as $\mathrm{Y}$. $\mathrm{m},\mathrm{n}\in \left\{0,1,2,\dots \right\}$, $\mathrm{m}\le \mathrm{n}$. $\mathrm{x}\in \mathrm{X}$, $\mathrm{y}\in \mathrm{Y}$ represent the true state vector and estimated state vector of an independent feature point at a certain observation times respectively. The OSPA distance is defined as follows [34]:

$${\overline{\mathrm{d}}}^{(\mathrm{c})}(\mathrm{X},\mathrm{Y})={\left\{\frac{1}{\mathrm{n}}\left[\underset{\mathrm{\pi }\in {\mathrm{\Pi }}_{\mathrm{n}}}{\mathrm{m}\mathrm{i}\mathrm{n}}{\displaystyle \sum _{\mathrm{i}=1}^{\mathrm{m}}{\mathrm{d}}^{(\mathrm{c})}{({\mathrm{x}}_{\mathrm{i}},{\mathrm{y}}_{\mathrm{\pi }(\mathrm{i})})}^{\mathrm{p}}}+{\mathrm{c}}^{\mathrm{p}}(\mathrm{n}-\mathrm{m})\right]\right\}}^{\raisebox{1ex}{$1$}\!\left/ \!\raisebox{-1ex}{$\mathrm{p}$}\right.}$$

(35)

where ${\displaystyle {\prod }_{\mathrm{n}}}$ represents taking all permutations and combinations of $\mathrm{m}$ elements from the sequence set {1,2,...}, and $\underset{\mathrm{\pi }\in {\mathrm{\Pi }}_{\mathrm{n}}}{\mathrm{m}\mathrm{i}\mathrm{n}}$ represents finding the pair with the smallest distance error from all the combination pairs of the estimated state set and the true state set. $\mathrm{p}=1$ is the order, which can adjust the weight of OSPA for the distance error between the two sets. $\mathrm{c}=100$ is a related parameter of the cut-off distance, which can adjust the correlation error weights of different numbers between the two sets. The OSPA distance is shown in Figure 15.

The experimental data in Table 2 show that the GLMB algorithm takes longer and is more computationally expensive than the LMB algorithm. It can be observed from the OSPA distance in Figure 15 that the LMB algorithm is slightly better than the GLMB algorithm in the detection and labeling effect of multiple feature points. The OSPA-loc curve shows that the two filtering algorithms have similar accuracy and can estimate the location of feature points more accurately, but the OSPA-card curve shows that the GLMB algorithm performs poorly in the estimation of the number of feature points. Considering the computational complexity of the algorithm, we believe that the LMB algorithm is more accurate for the detection and labeling of feature points, and the LMB algorithm can better estimate the state of multiple feature detection points under clutter conditions.

5. Conclusions

Aiming at the problem of low estimation accuracy of the location and quantity of map feature points in existing SLAM methods, this paper proposes a two-stage SLAM feature point detection and marking algorithm based on the LMB filter. We carry out path planning in stages and use measurement information to estimate the state of sensors and feature points. The B-spline curve is used to smooth the obstacle avoidance path of the sensor. Experimental simulation results show that the algorithm can effectively detect and mark feature points. However, when group feature points and extended feature points appear in the scene, further research is needed.