Real-Time Motion Tracking for Indoor Moving Sphere Objects with a LiDAR Sensor

Abstract

Object tracking is a crucial research subfield in computer vision and it has wide applications in navigation, robotics and military applications and so on. In this paper, the real-time visualization of 3D point clouds data based on the VLP-16 3D Light Detection and Ranging (LiDAR) sensor is achieved, and on the basis of preprocessing, fast ground segmentation, Euclidean clustering segmentation for outliers, View Feature Histogram (VFH) feature extraction, establishing object models and searching matching a moving spherical target, the Kalman filter and adaptive particle filter are used to estimate in real-time the position of a moving spherical target. The experimental results show that the Kalman filter has the advantages of high efficiency while adaptive particle filter has the advantages of high robustness and high precision when tested and validated on three kinds of scenes under the condition of target partial occlusion and interference, different moving speed and different trajectories. The research can be applied in the natural environment of fruit identification and tracking, robot navigation and control and other fields.

1. Introduction

1.1. Application of LiDAR

Light Detection and Ranging (LiDAR) technology provides realistic 3-dimensional (3D) image information and has been widely utilized in various fields [1]. LiDAR sensors are commonly used in perception for autonomous vehicles because of their high accuracy, speed, and range. These characteristics make the sensors suitable for integration into the perception layer of controllers which have the capacity to avoid collisions with unpredicted obstacles [2]. LiDAR technology is also applied to field Autonomous Land Vehicles (ALVs) to detect potential obstacles. With a novel 3D LiDAR setup, the blind area around the vehicle is greatly reduced and the density of LiDAR data is greatly improved, which are critical for ALVs [3]. In addition to autonomous land vehicle applications, LiDAR is also used for navigation of unmanned aircraft systems [4]. The authors combined LiDAR to automatically identify ground objects that pose navigation restrictions such as airports and high-rises.

Meanwhile, in the field of agriculture and forestry, by combining field and LiDAR data in forests with coexisting evergreen and deciduous species, researchers modelled common forest stand variables (height, diameter, volume and biomass) with high accuracy [5]. At the same time, LiDAR point clouds data is used for comparative classification analysis of post-harvest growth detection in precision agriculture [6], while other researchers have proposed a new approach for discriminating maize and weed plants from soil surface, evaluating the accuracy and performance of a LiDAR sensor for vegetation detection using distance and reflection values [7].

In other respects, LiDAR technology contributes to detect flood protection structures, natural or artificial in river floodplains and in coastal zones [8]. Also, 3D information derived from image dense matching or airborne LiDAR is very effective for building change detection [9]. Furthermore, for many robotics and intelligent vehicle applications, detection and tracking multiple objects based on LiDAR is one of the most important components [10].

1.2. Tracking Algorithms Based on LiDAR

LiDAR systems are commonly used for pedestrian recognition in ALVs, compared with cameras and can provide accurate range information and larger field of view [11]. For years, Kalman filters (KF) and Monte Carlo particle filters (PF) have been the two commonly used approaches to estimate motions of a target. In some early works, Song et al. [12] proposed a novel sparse learning-based object tracking algorithm utilizing 3D LiDAR data to realize moving object tracking of vehicles. The 3D point clouds acquired from LiDAR are first resampled on a virtual image plane, where the hypothesis of the targets is generated under the particle filtering framework. Guo et al. [13] proposed a pedestrian tracking algorithm initializing a KF to predict the possible position of the pedestrian centroid in the future frame. Meanwhile, Dewan et al. [14] have presented a novel model-free approach for detecting and tracking dynamic objects in 3D LiDAR scans obtained by a moving sensor. They sequentially detected multiple motions in the scene and segment objects using a Bayesian approach. Allodi et al. [15] have presented an obstacle detection, tracking and fusion algorithm which allows to reconstruct the environment surrounding the vehicle. An Unscented Kalman Filter (UKF) managing a variable number of observations, arbitrarily composable, allows to correctly address the combined tracking and fusion challenge. Moreover, Wasik et al. [16] have proposed a method based on the detection of circular features with least-squares fitting and filtering out outliers using a map-based selection. They have improved the estimate of the relative robot position and reduce its uncertainty by feeding measurements into a KF, resulting in an accurate tracking system.

For detecting and tracking moving objects in more complex cases, an occupancy grid tracking system based on particles [17] has been proposed. The proposed occupancy grid tracking solution can be classified as using the Descartes probability model of the reverse sensor and it generates a fully dynamic grid. To resolve ambiguities in complex dynamic scenes, Tuncer et al. [18] proposed a novel method for integrated tracking and segmentation of 3D LiDAR data with a non-parametric Bayesian method to combine segmentation and tracking components. In [19], Asvadi et al. proposed a 3D object tracking algorithm using a 3D-LiDAR, an RGB (Red, Green, Blue) camera and INS (Inertial Navigation System) (GPS (Global Position System)/IMU (Inertial Measurement Unit)) sensors data by analyzing sequential 2D-RGB, 3D point-cloud, and the ego-vehicle’s localization data and outputs the trajectory of the tracked object, an estimation of its current velocity, and its predicted location in the 3D world coordinate system in the next time-step while in [20], feature matching, Iterative Closest Point (ICP), Kalman filtering, and dynamic mapping are combined together to estimate motions.

As mentioned above, the KF, the PF UKF and non-parametric Bayesian are used in detecting and tracking moving targets and there is no straightforward extension of their approach to a moving spherical object.

1.3. Application of Kalman Filter

The KF has achieved notable success in the areas of guidance, navigation, and control of vehicles, particularly aircraft and spacecraft. Srilekha et al. [21] introduced a new technique for detecting, tracking and counting the vehicles based on Kalman filtering. Huang et al. [22] proposed the Robust Strong Tracking Cubature Kalman Filter (RSTCKF) for spacecraft attitude estimation with a quaternion constraint. Furthermore, the KF is a widely applied concept in time series analysis used in fields such as signal processing and econometrics. Jain et al. [23] investigated the use of KF to estimate and track both the laser PN (Phase Noises) and the NLPN (Nonlinear Phase Noises) in 100-Gb/s single channel coherent optical phase-modulated systems. The KF is one of the main topics in the field of robotic motion planning and control. Gulalkari et al. [24] proposed an object tracking and following six-legged robot (6LR) system that uses a Kinect camera based on KF and back-stepping control method. Lim et al. [25] proposed incorporating dead-reckoning using only encoder measurements, and a Kalman filter-based Gaussian Process to compensate the uncertainty. As for other aspects, Moon [26] developed a human skeleton tracking system using the Kalman filter framework, in which multiple Kinect sensors are used to correct inaccurate tracking data from a single Kinect sensor.

In order to solve the real-time tracking process for a moving sphere at indoor environments and in the future for spherical fruit identification and positioning with varying illumination, this paper first introduces the processes of sphere detection with 3D LiDAR, and then discusses the principles of the KF and PF algorithms. Next, with experimental modeling, data analysis of two tracking methods is compared, and finally we reach a conclusion. The tracking flowchart is as shown in Figure 1 below.

2. Detection of Moving Spherical Object

2.1. The Velodyne System

The Velodyne VLP-16 3D LiDAR sensor obtains a 360-degree scene capture through the rotation of its internal motor. It is composed of 16 laser beams, which scan thousands of times per second. Each beam has a fixed pitch angle. The experimental scene and its visualization result are shown in Figure 2.

2.2. Outliers and Noise Filtering

To reduce the calculation of segmentation after the 3D data acquisition, it is necessary to eliminate some of the noise, outliers, holes, etc. by filtering according to some motion cues. Here we remove the coordinate origin (0, 0, 0) and then use outlier filter proposed by Rusu et al. [27] which works well for indoor scenes. Firstly, we compute the average distance of each point between its nearest $k$ neighbors. Next, we compute the mean $\mu$, and standard deviation $\sigma$ of all these distances to determine a distance threshold. The standard deviation coefficient $\alpha$ depends on the size of the analyzed neighborhood $k$. The distance threshold $t$ will be equal to:

$$t=\mu +\alpha \times \sigma$$

(1)

In Equation (1), $\alpha$ is set to 1 and $k$ is set to 30 here with the empiric and experimental threshold, especially for the indoor scene. Finally, the points can be classified as inliers or outliers if their average neighbor distance is below or above this threshold respectively. The results of filtering are shown below in Figure 3, where most of the noises and outliers marked by red ellipse are removed.

2.3. Fast Ground Segmentation

For the indoor object motion tracking, the ground segmentation is essential to cancelling the ground background information. The fast ground segmentation algorithm proposed by Himmelsbach et al. [28] is used to remove the ground noise and to reduce the amount of subsequent calculations, which requires less runtime and obtains good segmentation results.

2.3.1. 3D Point-Cloud Data Set Mapping

• Define the unordered 3D point clouds from a scan time $t$ of the LiDAR sensor as $P_t=\left\{p_1, \dots \dots , p_{N_t}\right\}$, where $N_t$ denotes the number of 3D point clouds. The $p_i={\left(x_i,y_i,z_i\right)}^T$ denotes a 3D point, given by the Euclidean coordinates to the ego-coordinate system with original point at the center of the LiDAR sensor.
• The x-o-y plane denotes a circle with a radius of $R$, and then cut the circle equally into multiple discrete sectors, as shown in Figure 4. The $\Delta \alpha$ denotes the angle of each sector plane, so the number of sectors $M=2\pi /\Delta \alpha$.
• $S_i$ represents each sector, where $1\le \mathrm{i}\le M$, so that each point can be classified into a sector plane according to its projection on the x-o-y plane, expressed as a segment ($p_i$):

  $$\mathrm{segment}\left(p_i\right)=\frac{atan2\left(y_i,x_i\right)}{\Delta \alpha },$$

  (2)

  where $\mathrm{atan}2(y_i,x_i)$ represents the angle within [0, 2π) between the positive direction of x-axis and x-o-y plane, $y_i$ representing y-value of $p_i$, $x_i$ representing x-value of $p_i$, and $\Delta \alpha$ representing the angle of each sector plane.

We denote the set of all points mapped to the same segment $s$ by $P_s$:

$$P_s=\left\{p_i\in P|segment\left(p_i\right)=s\right\},$$

(3)

Define a mapping of all points $P_s$ of the same segment to one of many bins $b_j^s$,$j=1\dots B$ discretizing the range component of the points, while the superscript $s$ denotes the sector that the bin belongs to. The minimum or maximum range that a bin covers is expressed respectively by $r_j^{min}$ and $r_j^{max}$. Obviously, a point $p_i\in P_s$ maps to bin $b_j^s$:

$$r_j^{min}\le \sqrt{x_i^2+y_i^2}\le r_j^{max},$$

(4)

The $P_{b_j^s}$ is denoted by the set of all points mapping to $b_j^s$. Given a set of $P_{b_j^s}$ of 3D points mapped to the same bin, a new set of 2D points $P_{b_j^s}^{\prime }$ is simply defined as:

$$P_{b_j^s}^{\prime }=\left\{p_i^{\prime }|p_i\in P_{b_j^s}\right\},$$

(5)

where $p_i$ representing any point in 3D space, $p_i={\left(x_i,y_i,z_i\right)}^T$, $P_i^{\prime }={\left(\sqrt{x_i^2+y_i^2},z_i\right)}^T$.

All points have been mapped to a segment and a corresponding segment bin. With the above mapping method of 3D point clouds data set, sorting from small to large by distance, the processed 3D space point set is partially of order. A prototype point $p_{b_j^s}^{\prime }$ is calculated for every non-empty bin points $P_{b_j^s}^{\prime }$ whose point with lowest z-coordinate and most likely belonging to the ground plane.

2.3.2. Fast Ground Segmentation

On the basis of the above data mapping method of 3D point clouds data set, the ground model in a sectorial area S can be expressed as a set $L_s$ of a line segment shown in Figure 5.

Calculate the distance between a point and the line $L$ as shown in Figure 6. Calculate the two straight lines $L_a$, and $L_b$, which pass through both ends of line segment $AB$, and perpendicular to line segment $L$. Then determine whether the point $P$ is between the straight lines $L_a$, and $L_b$ (such as $P_1$ point) or outside the two lines (such as $P_2$ point, $P_3$ point). If the point is at the position $P_1$, the distance$L$ and $P_1$ can be directly calculated. If the point is at the position $P_2$, then the distance between $P_2$ and point $A$ is calculated. In the same way, if at point $P_3$, the distance between $P_3$ and $L$ is equal to the distance between $P_3$ and point $B$. The process of extracting lines for a segment is expressed in Algorithm 1 as follows.

The thresholds mentioned in the algorithm are tested in the experiment with the settings shown in

Table 1. Threshold Setting.

${\mathit{T}}_{\mathit{m}}$	${\mathit{T}}_{\mathit{b}}$	${\mathit{T}}_{\mathit{R}\mathit{M}\mathit{S}\mathit{E}}$	${\mathit{T}}_{\mathit{d}\mathit{p}\mathit{r}\mathit{e}\mathit{v}}$	${\mathit{T}}_{\mathit{g}\mathit{r}\mathit{o}\mathit{u}\mathit{n}\mathit{d}}$
0.08	0.2	1	0.04	0.08

, while the threshold $T_{ground}$ determines whether the point belongs to a ground point. It remains to formulate necessary conditions for a line $y=mx+b$ to be considered part of the ground plane:

• The line’s slope $m$ must not exceed a certain threshold $T_m$.
• The line’s absolute y-intercept b must not exceed a certain threshold $T_b$.
• The root mean square error of the fit must not exceed a certain threshold $T_{RMSE}$.
• The distance of the first point of a line to the line previously fitted must not exceed $T_{dprev}$, enforcing smooth transitions between pairs of successive lines.

With the above bins and segments calculation, and the experimental thresholds trial selection, the fast ground segmentation results are shown in Figure 7, where the most of ground information can be effectively cancelled.

Algorithm 1. Extraction of lines for one segment SS

1: ${\mathbf{L}}_{\mathbf{s}}=\mathbf{\varnothing }$, c = 0, ${\mathbf{p}}_{\mathbf{l}}=\mathbf{\varnothing }$

% ${\mathbf{L}}_{\mathbf{s}}$ denotes a set of line set in segment S, c denotes count of loop,${\mathbf{p}}_{\mathbf{l}}$ denotes a point of line

2: for j = 0 in MAX_B do  % for each bin

3:  if ${\mathbf{p}}_{{\mathbf{b}}_{\mathbf{j}}^{\mathbf{s}}}^{\prime }\ne \mathbf{\varnothing }$ then     % if there is a point mapping in ${\mathbf{b}}_{\mathbf{j}}^{\mathbf{s}}$

4:   if |${\mathbf{p}}_{\mathbf{l}}$| >= 2 then   % if the number of points in ${\mathbf{L}}_{\mathbf{s}}$ bigger than two

5:    (${\mathbf{m}}_{\mathbf{c}},{\mathbf{b}}_{\mathbf{c}}$) = fitline(${\mathbf{p}}_{\mathbf{l}}\cup {\mathbf{p}}_{{\mathbf{b}}_{\mathbf{j}}^{\mathbf{s}}}^{\prime }$)  % line fit to get ${\mathbf{m}}_{\mathbf{c}},{\mathbf{b}}_{\mathbf{c}}$ of L

6:    if $\left|{\mathbf{m}}_{\mathbf{c}}\right|$ <= ${\mathbf{T}}_{\mathbf{m}}$ ∧ (${\mathbf{m}}_{\mathbf{c}}$ > |${\mathbf{T}}_{{\mathit{m}}_{\mathit{s}\mathit{m}\mathit{a}\mathit{l}\mathit{l}}}$| ∨ $\left|{\mathbf{b}}_{\mathbf{c}}\right|$ <= ${\mathbf{T}}_{\mathbf{b}}$) ∧ fitError(${\mathbf{m}}_{\mathbf{c}},{\mathbf{b}}_{\mathbf{c}},{\mathbf{p}}_{\mathbf{l}}\cup {\mathbf{p}}_{{\mathbf{b}}_{\mathbf{j}}^{\mathbf{s}}}^{\prime }$) < ${\mathbf{T}}_{\mathbf{RMSE}}$ then

% if match condition of thresholds

7:       ${\mathbf{p}}_{\mathbf{l}}\leftarrow {\mathbf{p}}_{\mathbf{l}}\cup {\mathbf{p}}_{{\mathbf{b}}_{\mathbf{j}}^{\mathbf{s}}}^{\prime }$      % add ${\mathbf{p}}_{{\mathbf{b}}_{\mathbf{j}}^{\mathbf{s}}}^{\prime }$ to ${\mathbf{p}}_{\mathbf{l}}$

8:    else

9:       (${\mathbf{m}}_{\mathbf{c}},{\mathbf{b}}_{\mathbf{c}}$) $\leftarrow$ fitline (${\mathbf{p}}_{\mathbf{l}}$)    % line fit to get ${\mathbf{m}}_{\mathbf{c}},{\mathbf{b}}_{\mathbf{c}}$ of L

10:      ${\mathbf{L}}_{\mathbf{s}}\leftarrow {\mathbf{L}}_{\mathbf{s}}\cup \{\left({\mathbf{m}}_{\mathbf{c}},{\mathbf{b}}_{\mathbf{c}}\right)\}$

11:      ${\mathbf{p}}_{\mathbf{l}}\leftarrow \mathbf{\varnothing }$           % clear ${\mathbf{p}}_{\mathbf{l}}$

12:      c$\leftarrow$c + 1        % next line segment

13:      j$\leftarrow$j − 1         % next distance

14:   else               % if the number of points in ${\mathbf{L}}_{\mathbf{s}}$ smaller than two

15:    if c = 0 ∨ (${\mathbf{p}}_{\mathbf{l}}\ne \mathbf{\varnothing }$) ∨ $\mathbf{d}\mathbf{i}\mathbf{s}\mathbf{t}\mathbf{p}\mathbf{o}\mathbf{i}\mathbf{n}\mathbf{t}\mathbf{l}\mathbf{i}\mathbf{n}\mathbf{e}({\mathbf{p}}_{{\mathbf{b}}_{\mathbf{j}}^{\mathbf{s}}}^{\prime },{\mathbf{m}}_{\mathbf{c}-1},{\mathbf{b}}_{\mathbf{c}-1})\le {\mathit{T}}_{\mathit{d}\mathit{p}\mathit{r}\mathit{e}\mathit{v}}$ then      % if the first point or the distance of point and line match thresholds

16:      ${\mathbf{p}}_{\mathbf{l}}={\mathbf{p}}_{\mathbf{l}}{{\displaystyle \cup }}^{\text{}}{\mathbf{p}}_{{\mathbf{b}}_{\mathbf{j}}^{\mathbf{s}}}^{\prime }$        % add ${\mathbf{p}}_{{\mathbf{b}}_{\mathbf{j}}^{\mathbf{s}}}^{\prime }$ to ${\mathbf{p}}_{\mathbf{l}}$

2.4. Object Segmentation

On the basis of fast ground segmentation to cancel the noises, the Euclidean clustering segmentation algorithm is used to segment the sphere target, and its point-cloud data needs to be trained to recognize the target.

2.4.1. Euclidean Clustering of Point Clouds

The Euclidean clustering algorithm is used to segment other disperse irrelevant background sorting point clouds data of same similarity within certain threshold. The basic idea is dividing n points into m classes randomly at first, and then making the points in each class have comparatively high similarity while the points in different classes have comparatively low similarity. Then we calculate the Euclidean distance between clusters. The two clusters with a minimum distance could be merged into one cluster. We repeat the calculation of distances between clusters, and subsequent merging. With the repeated iteration until the distance between any two clusters is over than the given threshold, or the number of clusters is less than the given number, the segmentation is completed and the target sphere object is obtained. The distance threshold is set as 0.06 according to the number of sphere object’s point clouds, the minimum number of points in each class as 100, and the maximum number as 2000. The results of Euclidean clustering are shown in Figure 8, where the surrounding of the target sphere can be clearly clustered. Figure 8e shows the basic contour of the target sphere. With the Euclidean clustering, we could extract and match the moving sphere during the indoor motion tracking process.

2.4.2. VFH Descriptor Extraction

The numbers of clusters have been segmented in the process of Euclidean clustering. Subsequently the feature extraction descriptor for each cluster is employed to match the target sphere with Fast Library with Approximate Nearest Neighbors (FLANN). The feature description here describes the geometry and topology of the local or global of point clouds data sets, which can be easily understood as a point-cloud feature. The feature description of point-clouds generally consists of local and global feature descriptions. The local features describe the local geometry and shape characteristics of the point-cloud data, while the global features describe the global topological structure of the point clouds. Only for the motion tracking at the whole indoor environment to distinguish different poses, the global feature descriptor of Viewpoint Feature Histograms (VFH) is employed to estimate the feature of clusters and to extract the target. The visualization of sphere object and its VFH are shown in Figure 9.

2.4.3. Feature Match of FLANN

The point clouds feature model library is constructed with the FLANN and the extraction steps are listed as follows:

(1)

Acquire the point clouds data sets using the LiDAR sensor at different distance between the sphere object and sensor, and then extract VFH features for each point clouds model.

(2)

Load the above VFH features into memories and convert the data into matrix format.

(3)

Create the k-d (k-dimensional) tree with the converted matrix data, and save the index of k-d tree for the direct search match.

(4)

Input the VFH feature and the index of k-d, and search the nearest neighbor along the k-d tree for the input data.

(5)

Achieve the target point clouds if the difference between the searching results and VFH is less than the given threshold.

3. Tracking

3.1. Kalman Filtering

For the motion tracking of a moving sphere, the KF [29] provides a highly computable method in the recursive way to estimate the state of the process and minimize the estimated mean square error. The state and measurement equations are used to describe a dynamic system. The state vector $s_k$ of the system of time moment $k$ is determined by both the state $s_{k-1}$ at time moment $k-1$ and the observed noise. The measurement vector $m_k$ is also determined by these two, which is by the observation function of state vector $s_k$ at time moment k and the noise. The target motion tracking process with KF is shown in Figure 10. The state variables are the positions and velocities of the sphere object in the X, Y, and Z coordinate, expressed as the matrix ${\left[x,y,z,v_x,v_y,v_z\right]}^T$ and the observed variable z is the objects’ position data real-time sensed by LiDAR sensor and real-time processed, denoted as ${\left[x,y,z\right]}^T$. Here, the state function is expressed as Equation (6):

$$s_k=A{s}_{k-1}+w_{k-1},$$

(6)

and the measurement function is given by Equation (7):

$$m_k=H{m}_k+v_k$$

(7)

The random variables $w_{k-1}$ and $v_k$ represent the process and the measurement noise respectively, denoting the noises and disturbances of the moving sensing data. They are assumed to be independent of each other, and with normal probability distributions:

$$p\left(w\right)~N\left(0,Q\right)$$

(8)

$$p\left(v\right)~N\left(0,R\right)$$

(9)

The matrix $Q$ denotes the process noise covariance and $R$ the measurement noise covariance. Here assumed that $Q$ and $R$ are redefined as constant shown as follows:

$$Q=\left[\begin{array}{cccccc}0.05& 0& 0& 0& 0& 0\\ 0& 0.05& 0& 0& 0& 0\\ 0& 0& 0.05& 0& 0& 0\\ 0& 0& 0& 0.05& 0& 0\\ 0& 0& 0& 0& 0.05& 0\\ 0& 0& 0& 0& 0& 0.05\end{array}\right]$$

(10)

$$R=\left[\begin{array}{ccc}0.05& 0& 0\\ 0& 0.05& 0\\ 0& 0& 0.05\end{array}\right]$$

(11)

Define the state variable $s_k$ as a six-dimensional vector shown in Equation (12):

$$s_k={\left[x_k,y_k,z_k,v_{x,k},v_{y,k},v_{z,k}\right]}^T,$$

(12)

where $x_k,y_k,z_k$ are respectively the coordinate value of the center sphere object in the x, y, z coordinate system, and $v_{x,k},v_{y,k},v_{z,k}$ is respectively the speed of the center coordinates in the x, y, z direction.

The time interval $dt$ between the two frames of LiDAR is only one second, which is relatively short, and the motion could be considered as a uniform motion, so the state transition matrix A is expressed as:

$$A=\left[\begin{array}{cccccc}1& 0& 0& dt& 0& 0\\ 0& 1& 0& 0& dt& 0\\ 0& 0& 1& 0& 0& dt\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$$

(13)

The measurement vector $m_k$ is used to observe the center position of the moving spherical objects shown in Equation (14):

$$m_k={\left[x_k,y_k,z_k\right]}^T,$$

(14)

and the corresponding observation matrix is as follows:

$$H=\left[\begin{array}{cccccc}1& 0& 0& -dt& 0& 0\\ 0& 1& 0& 0& -dt& 0\\ 0& 0& 1& 0& 0& -dt\end{array}\right].$$

(15)

The variable ${\widehat{s}}_k^{-}$ (− represents priori, and ^ represents the estimate) denotes k-th priori state estimation when the k-th preceding state is known, and ${\widehat{s}}_k$ is the known posterior state estimation of measurement variable $z_k$ at the k-th period.

The following Equation (16) predicts the current state value with the result of the previous finest state:

$${\widehat{s}}_k^{-}=A{\widehat{s}}_{k-1}+w,$$

(16)

where ${\widehat{s}}_k^{-}$ is the a priori state estimate at step k given knowledge of the process prior to step k. A is the state transition matrix of the system shown in Equation (13), ${\widehat{s}}_{k-1}$is the optimal estimate at time moment t − 1, and w is the system noise following Gaussian distribution.

The uncertainty of each moment is represented by the covariance matrix P, and the update formula is expressed in Equation (17):

$$P_k^{-}=A{P}_{t-1}{A}^T+Q,$$

(17)

where $P_k^{-}$ is the covariance of ${\widehat{x}}_k^{-}$, $P_{t-1}$ is the covariance of ${\widehat{x}}_{k-1}$, and Q is the covariance of the random signals $w_{k-1}$ and $v_k$. The prediction Equations (16) and (17) update time t.

We define a priori and a posteriori estimate error as Equations (18) and (19), respectively;

$$e_k^{-}\equiv x_k-{\widehat{x}}_k^{-},$$

(18)

$$e_k\equiv x_k-{\widehat{x}}_k,$$

(19)

Then, the a priori estimated error covariance is given by Equation (20):

$$P_k^{-}=E\left[e_k^{-}{e}_k^{-}{}^T\right],$$

(20)

and the s posteriori estimate error covariance is:

$$P_k=E\left[e_k{e}_k{}^T\right],$$

(21)

The initial state $P_0$ of covariance matrix $P_k$ is redefined as:

$$P_0=\left[\begin{array}{cccccc}10& 0.1& 0.1& 0.1& 0.1& 0.1\\ 0.1& 10& 0.1& 0.1& 0.1& 0.1\\ 0.1& 0.1& 10& 0.1& 0.1& 0.1\\ 0.1& 0.1& 0.1& 10& 0.1& 0.1\\ 0.1& 0.1& 0.1& 0.1& 10& 0.1\\ 0.1& 0.1& 0.1& 0.1& 0.1& 10\end{array}\right]$$

(22)

The posteriori state estimate ${\widehat{s}}_k$ as a linear combination of an a priori estimate ${\widehat{s}}_k^{-}$ and a weighted difference between an actual measurement $m_k$ and a measurement prediction $H{\widehat{s}}_k^{-},$ calculated as shown in Equation (23):

$${\widehat{s}}_k={\widehat{s}}_k^{-}+K_k\left(m_k-H{\widehat{s}}_k^{-}\right),$$

(23)

where the matrix $K_k$ is the gain that minimizes the posteriori error covariance. The difference $\left(m_k-H{\widehat{s}}_k^{-}\right)$ is the measurement innovation (residual) that reflects the discrepancy between the predicted measurement $H{\widehat{s}}_k^{-}$ and the actual measurement $z_k$.

To accomplish minimization, the Equations (18) and (23) are firstly substituted into Equation (19), with a unit matrix $I$, then we get Equation (24):

$$e_k=\left[I-K_{k}H\right]e_k^{-}-K_k{v}_k,$$

(24)

Substituting the above Equation (24) into Equation (21), then we get:

$$P_k=\left(I-K_{k}H\right)P_k^{-}{\left(I-K_{k}H\right)}^T+K_{k}R{K}_k{}^T,$$

(25)

Obtaining the indicated expectations, we take the derivative of the tracking result with respect to $K_k$. Set the result to zero, and then solve for $K_k$ as below:

$$K_k=P_k^{-}{H}^T{\left(H{P}_k^{-}{H}^T+R\right)}^{-1}=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R},$$

(26)

With the equations above, the smaller the observed noise covariance R, the larger the gain $K_k$. The smaller the covariance $P_k^{-}$, the smaller the gain $K_k$. The Kalman gain $K_k$ works on two aspects, firstly, it weighs the size of the a priori estimated error of the covariance$P_k^{-}$and the observed noise covariance matrix R to determine the more convincing model between the prediction and observation model; secondly, it transforms the representation form of the residuals from the observation domain to the state domain.

To make the KF rung down till the end of the whole system running process, the covariance ${\widehat{x}}_k$ at state t needs to be updated as follows:

$$P_k=\left(I-K_{k}H\right)P_k^{-},$$

(27)

After the time updating calculation of the Equations (14) and (19) and the measurement updating Equations (20), (23) and (24), the whole Kalman tracking process repeats again. A posteriori estimation of Equation (20) obtained from the previous calculation of Equation (18) is taken as the a priori estimation of Equation (24) of the next computation. The whole process of KF is shown in Algorithm 2 as follows.

Algorithm 2. Kalman Filter

Input:${\mathit{m}}_{\mathit{k}}$,  % object position for time step t from sensor

Output: ${\widehat{\mathit{s}}}_{\mathit{k}}$,  % a position estimation of object

1: initialize t, ${\widehat{\mathit{s}}}_{\mathit{k}-\mathbf{1}}$, A, P, Q, R  % t represents prediction time moment, ${\widehat{\mathit{s}}}_{\mathit{k}-\mathbf{1}}$ is the known posterior        % state estimation at time moment k − 1; A represents state transition matrix;        % P represents the covariance matrix, Q denotes covariance of the random        % signals, and R is the matrix of observation noise covariance.

2: if filterStop = false then  % end with convergence of click the ‘stop’ button.

3:  ${\widehat{\mathit{s}}}_{\mathit{k}}^{-}\leftarrow \mathit{A}{\widehat{\mathit{s}}}_{\mathit{k}-\mathbf{1}}$    % calculate predicting position estimation, according to Equation (16)

4:  P ← APAT + Q  % calculate priori covariance matrix, according to Equation (17)

5:  K ← PCT(CPCT + R)  % calculate Kalman Gain matrix, according to Equation (26)

6:  ${\widehat{\mathit{s}}}_{\mathit{k}}$ ← ${\widehat{\mathit{s}}}_{\mathit{k}}^{-}$ + K(${\mathit{m}}_{\mathit{k}}$ − C${\widehat{\mathit{s}}}_{\mathit{k}}^{-}$)  % calculate optimal estimation value, according to Equation (23)

7:  P ← (I − KC)P  % calculate ${\widehat{\mathit{x}}}_{\mathit{k}}$ covariance, according to Equation (27), I denotes unit matrix

8:  t ← t + 1

9: end if

3.2. Particle Filtering

Particle filtering is a non-parametric Monte Carlo method used to simulate the realization of the recursive Bayesian filter, which is applicable to any state space model for the non-linear non-Gaussian case, and its accuracy can reach the optimal estimate. Filtered particles are possibilities to describe the target state. The purpose of filtering is the most probable state of the filtered target. In the Bayesian estimation theory, the current state of the target is estimated using the previous state and the current measured value. The arbitrary probability distribution $p(x_k)$ can be Monte Carlo approximated using the discrete particle set as follows:

$$p\left(x_k|y_{1:k}\right)\approx {\Sigma }_{i=1}^{N_k}{w}_k^{\left(i\right)}\delta \left(x_k-x_k^{\left(i\right)}\right),$$

(28)

where $x_k^{\left(i\right)}$, $w_k^{\left(i\right)}$, $N_k$ are respectively expressed as particle state, weight and total number under $k$ time, where $\delta$ is Dirac’s delta function.

The most basic and common PF implementation framework is Sequential Importance Sampling and Resampling (SISR) or Sampling Importance Resampling (SIR) filter, and the algorithm is shown below:

Step 1: For i = 1, 2, …, N, Initializing the particle set, $k=0$:

Generating the sampled particles ${\{x_0^{\left(i\right)}\}}_{i=1}^N$ based on the priori distribution $p\left(x_0\right);$

Step 2: For $k=1,2,\dots ,N$, Executing the follow steps circularly:

Sequential importance sampling: for $i=1,2,\dots ,N$, generating the sampled particles ${\{{\tilde{x}}_k^{\left(i\right)}\}}_{i=1}^N$ from the importance probability density, then calculating the particle weights, finally normalizing the weights so that the sum of the weights of the particles is 1;

Resampling: resampling the particles set $\{{\tilde{x}}_k^{\left(i\right)},{\tilde{w}}_k^{\left(i\right)}\}$, and the resampled set is $\left\{x_k^{\left(i\right)},1/N\right\}$;

Printing: calculating the estimated state value: ${\widehat{x}}_k={\Sigma }_{i=1}^N{\tilde{x}}_k^{\left(i\right)},{\tilde{w}}_k^{\left(i\right)}$.

Sequential importance sampling is the basis of particle filtering, which applies the sequential analysis method in statistics to the Monte Carlo method, so as to realize the recursive estimation of the probability density of posterior filtering. Assumed that the importance probability density function $q(x_{0:k}|y_{1:k})$ can be decomposed into:

$$q\left(x_{0:k}|y_{1:k}\right)=q(x_{0:k-1}|y_{1:k-1})q(x_k|x_{0:k-1},y_{1:k}),$$

(29)

Let the system state be a Markov process, and the given system state is independent of each observation so that there is:

$$p\left(x_{0:k}\right)=\mathrm{p}\left(x_0\right){\prod }_{i=1}^{k}p(x_i|x_{i-1}),$$

(30)

$$p\left(y_{1:k}|x_{1:k}\right)={\prod }_{i=1}^{k}p(y_i|x_i),$$

(31)

The recursive form of the posterior probability density function can be expressed as:

$$\begin{array}{cc}\hfill p\left(x_{0:k}|Y_k\right)& =\frac{p(y_k|x_{0:k},Y_{k-1})p(x_{0:k}|Y_{k-1})}{p(y_k|Y_{k-1})}\hfill \\ & =\frac{p(y_k|x_{0:k},Y_{k-1})p(x_k|x_{0:k-1},Y_{k-1})p(x_{0:k}|Y_{k-1})}{p(y_k|Y_{k-1})}\\ & =\frac{p(y_k|x_k)p(x_k|x_{k-1})p(x_{0:k-1}|Y_{k-1})}{p(y_k|Y_{k-1})}\end{array}$$

(32)

In the update phase, the particles’ weights are recalculated according to the likelihood function $p\left(x_{0:k}|Y_k\right)$:

$$\begin{array}{cc}\hfill w_k^{\left(i\right)}& \infty \frac{p(x_k^{\left(i\right)}|Y_k)}{q(x_k^{\left(i\right)}|Y_k)},\hfill \\ & =\frac{p(y_k|x_k^{\left(i\right)})p(x_k^{\left(i\right)}|x_{k-1}^{\left(i\right)})p(x_{0:k-1}^{\left(i\right)}|Y_{k-1})}{q(x_k^{\left(i\right)}|x_{0:k-1}^{\left(i\right)},Y_k)q(x_{0:k-1}^{\left(i\right)}|Y_{k-1})}\\ & =w_{k-1}^{\left(i\right)}\frac{p(y_k|x_k^{\left(i\right)})p(x_k^{\left(i\right)}|x_{k-1}^{\left(i\right)})}{q(x_k^{\left(i\right)}|x_{0:k-1}^{\left(i\right)},Y_k)}\end{array}$$

(33)

In general, it is necessary to normalize the weight of the particle:

$${\tilde{w}}_k^{\left(i\right)}=\frac{w_k^{\left(i\right)}}{{{\displaystyle \Sigma }}_{i=1}^N{w}_k^{\left(i\right)}},$$

(34)

This results in an approximate representation of the posterior probability density function expressed by Equation (32). In practical application, the use of too many samples will result in a sharp increase in the computational complexity and the deterioration of the performance of the particle filter. However, it is very difficult to correctly approximate the posterior probability with a small amount of sampling, and the resampling process may also lead to particle deficiency. Therefore, it is necessary to determine the appropriate sampling quantity and improve the efficiency of sampling according to the state of the system, under the condition of ensuring the diversity of the particles. In this paper, adaptive particle filter based on Kullback-Leibler Distance (KLD) sampling proposed by Fox [30] is adopted to resample the particles.

The core idea of the KLD sampling is that in each iteration of the particle filter, using the probability $1-\delta$ to make the error between the true posterior probability and the estimated probability density based on the sample less than ε:

$$n=\frac{1}{2ϵ}{\mathcal{X}}_{k-1,1-\delta }^2$$

(35)

where $\delta$ is set as 0.99 and $ϵ$ as 0.2. So that the number of resampled samples is determined. The error is determined by calculating the KLD. The KLD is used to represent the approximation error between the two probability distributions $p$ and $q$:

$$K\left(p,q\right)={{\displaystyle \Sigma }}_{x}p\left(x\right)lg\frac{p\left(x\right)}{q\left(x\right)},$$

(36)

In the resampling, the smaller particles are neglected and the larger particles are copied. The number of particles in the resampling is determined by KLD sampling in the process of particle duplication, and the number of particles of the next importance is determined, adjusting the number of particles on-line and reducing the computational complexity. The process of KLD sampling is shown in Algorithm 3.

Algorithm 3. KLD sampling algorithm

Input: ${\mathit{s}}_{\mathit{t}-\mathbf{1}}=\{\langle {\mathit{x}}_{\mathit{t}-\mathbf{1}}^{\left(\mathit{i}\right)},{\mathit{w}}_{\mathit{t}-\mathbf{1}}^{\left(\mathit{i}\right)}\rangle |\mathit{i}=\mathbf{1},\dots ,\mathit{n}\}$ , observations${\mathit{z}}_{\mathit{t}}$ , limits$\mathit{\epsilon }$ and$\mathit{\delta }$;

Output: ${\mathit{S}}_{\mathit{t}}$

1: ${\mathit{S}}_{\mathit{t}}≔\mathbf{\varnothing },\mathit{n}=\mathbf{0},\mathit{k}=\mathbf{0},\mathit{\alpha }=\mathbf{0}$    % initializing

2: do                % generating samples

3:  sampling from discrete distributions under the weight of known ${\mathit{s}}_{\mathit{t}-\mathbf{1}}$ , the sequence is$\mathit{j}\left(\mathit{n}\right)$

4:  sampling ${\mathit{x}}_{\mathit{t}}^{\left(\mathit{n}\right)}$ from$\mathbf{p}({\mathit{x}}_{\mathit{t}}|{\mathit{x}}_{\mathit{t}-\mathbf{1}})$ using ${\mathit{x}}_{\mathit{t}-\mathbf{1}}^{\left(\mathit{j}\left(\mathit{n}\right)\right)}$

5:  ${\mathit{w}}_{\mathit{t}}^{\left(\mathit{n}\right)}≔\mathit{p}({\mathit{z}}_{\mathit{t}}|{\mathit{x}}_{\mathit{t}}^{\left(\mathit{n}\right)})$       % calculate the importance weights

6:  $\mathit{\alpha }≔\mathit{\alpha }+{\mathit{w}}_{\mathit{t}}^{\left(\mathit{n}\right)}$          % update the normalization factor

7:  ${\mathit{S}}_{\mathit{t}}≔{\mathit{S}}_{\mathit{t}}{{\displaystyle \cup }}^{\text{}}\{\langle {\mathit{x}}_{\mathit{t}}^{\left(\mathit{n}\right)},{\mathit{w}}_{\mathit{t}}^{\left(\mathit{n}\right)}\rangle \}$    % insert the sample into the sample set

8:  if (${\mathit{x}}_{\mathit{t}}^{\left(\mathit{n}\right)}$ fall in $\mathit{b}\mathit{i}\mathit{n}\mathit{b}$ ) then    % update the number of $\mathit{b}\mathit{i}\mathit{n}\mathit{s}$

9:   $\mathit{k}≔\mathit{k}+\mathbf{1}$

10:   $\mathit{b}≔\mathit{n}\mathit{o}\mathit{n}-\mathit{e}\mathit{m}\mathit{p}\mathit{t}\mathit{y}$

11:  $\mathit{n}≔\mathit{n}+\mathbf{1}$          % update the number of generated samples

12: while ($\mathit{n}<\frac{\mathbf{1}}{\mathbf{2}\mathit{ϵ}}{\mathcal{X}}_{\mathit{k}-\mathbf{1},\mathbf{1}-\mathit{\delta }}^{\mathbf{2}}$)     % stop when come to the limits K-L with Equation (35)

13: for i: = 1, …, n do         % normalize importance weights

14:  ${\mathit{w}}_{\mathit{t}}^{\left(\mathit{i}\right)}={\mathit{w}}_{\mathit{t}}^{\left(\mathit{i}\right)}/\mathit{\alpha }$

4. Experimentation and Discussions

In order to validate the robustness and real-time performance of the two tracking algorithms (KF and adaptive PF), some experiments were carried out on the moving spherical target with or without occlusion, obstacles, different speeds and different trajectories. We tied the ball with a rope and pulled the string to move the ball.

4.1. Target Tracking with Occlusion and Obstacle in the Environment

In target tracking processes, occlusion is a very common phenomenon. When the target is blocked, the valid information will be reduced and the tracking difficulty will be increased. Considering the partial occlusion of the spherical target with a long piece of wood, as shown in Figure 11a, the experimental results are shown in Figure 12 and Figure 13 with two different tracking algorithms, respectively.

In Figure 12, when the target is partially occluded, the KF clusters the wood point clouds together as the next frame’s measurement, and some tracking loss occurs. However, in Figure 13, the PF algorithm can track the object effectively where the occluded wood board does not cluster with the target.

Obstacles in front of the moving target are common in practical applications, and when multiple targets appear in motion, they may interfere with each other. The carton box as an obstacle is shown in Figure 11b. The experimental results are shown in Figure 14.

The two methods can track the spherical target continuously in the case of an obstacle. In Figure 14, the KF is susceptible to near point-cloud in the tracking process, and the PF has strong robustness for the motion tracking.

4.2. Target Tracking at Different Moving Speeds

Whether the tracker can track moving targets at different speeds is also an important indicator of the performance. Due to the limitations of our laboratory environment and the point-cloud density of LiDAR equipment, the basketball position swings easily in the process of manually moving the basketball, especially at low speed, and a uniform speed can hardly be obtained. At the experimental process, the observed speed of movement of the basketball is 0.054 m/s at its low speed, and 0.125 m/s at its high speed. In Figure 15, the continuity of low-speed trajectory expressed by the light blue dots shows that the KF is more suitable for tracking at relative lower speeds. The continuity and density of high speed trajectory expressed by the red and brown fork shows the adaptive PF has better tracking performance. Basically, with more trials of different speed experiments, the adaptive PF has better tracking effects for the different moving speed than the KF.

4.3. Target Tracking in Different Motion Trajectories

In an actual situation, the target may move in a variety of trajectories. The spherical target’s real-time tracking is tested in three kinds of trajectories including straight line, curve and three-dimensional trajectory. The linear motion trajectory and error analysis are shown in Figure 16. Figure 16a,b show the actual trajectory and the estimated value of the spherical target in the rectilinear movement under the Kalman filter and the adaptive particle filter, respectively. The results indicate that the target moves as a curve in the same situation are shown in Figure 17. Obviously, the trajectory with the Kalman filter is smoother. However, the effect of PF tracking is better. The error comparison is shown in Figure 16c and Figure 17c. Errors and fluctuations of the KF are greater than the adaptive PF.

The results of the trajectory tracking of the spherical target in the three-dimensional space are shown in Figure 18. It is easy to see that the KF and the PF can basically track the moving target in three-dimensional space, but fluctuate in the Z-axis direction.

5. Conclusions

This paper has proposed a systematic real-time detection and tracking method for an indoor moving sphere using the VLP-16 3D LiDAR sensor. After a series of preprocessing steps of point-cloud data and global feature extraction, the Kalman filter and the adaptive particle filter method are used to estimate the real-time motion state of a spherical object. With three different kinds of indoor comparison experiments and analysis, the results show that the adaptive PF has better tracking performance. The specific work allows us to put forth two conclusions:

Firstly, the real-time detection of the spherical target is accomplished by acquiring the real-timepoint-cloud data of moving sphere at indoor, preprocessing with fast ground segmentation algorithm to remove outliers, and ground points clustering with Euclidean cluster algorithm, extracting target feature with VFH to establish model library and matching to detect spherical targets.

Secondly, the KF is used to real-time track the object, and the object position is estimated sequentially by real-time acquisition of the measured value, prediction and correction, while the adaptive PF is used to track the target, and the state of the target is estimated by sampling, calculating the weight and resampling. The efficiency of KF and adaptive PF in 3D lidar tracking is verified by indoor basketball tracking experiments, with a moving spherical object with or without occlusion and obstacles, respectively, at different speeds and over different trajectories. The experimental results show that adaptive PF has a small tracking error and strong robustness.

The motion tracking of a dynamic environment is one of the key components for intelligent agricultural harvesters to operate in real-world conditions. We will continue to exploit the 3D semantic perception with transfer learning and real-time location method of natural fruits for a better tracking performance.