Laser-Based Pedestrian Tracking in Outdoor Environments by Multiple Mobile Robots

Abstract

This paper presents an outdoors laser-based pedestrian tracking system using a group of mobile robots located near each other. Each robot detects pedestrians from its own laser scan image using an occupancy-grid-based method, and the robot tracks the detected pedestrians via Kalman filtering and global-nearest-neighbor (GNN)-based data association. The tracking data is broadcast to multiple robots through intercommunication and is combined using the covariance intersection (CI) method. For pedestrian tracking, each robot identifies its own posture using real-time-kinematic GPS (RTK-GPS) and laser scan matching. Using our cooperative tracking method, all the robots share the tracking data with each other; hence, individual robots can always recognize pedestrians that are invisible to any other robot. The simulation and experimental results show that cooperating tracking provides the tracking performance better than conventional individual tracking does. Our tracking system functions in a decentralized manner without any central server, and therefore, this provides a degree of scalability and robustness that cannot be achieved by conventional centralized architectures.

1. Introduction

Tracking (i.e., estimating the motion) of pedestrians is important to ensure safe navigation of mobile robots and vehicles. There has been much interest in the use of stereo vision or a laser range scanner (LRS) in mobile robotics and vehicle automation [1–5]. We previously presented a pedestrian tracking method using LRS mounted on mobile robots and automobiles [6–8].

Recently, many studies related to multi-robot coordination and cooperation have been conducted [9,10]. When these robots and vehicles are located near each other, they can share their sensing data. This implies that the robots and vehicles are considered to be a multi-sensor system. Therefore, even if pedestrians are located outside the sensing area of any individual robot or vehicle, it can detect pedestrians using the tracking data received from other robots and vehicles in the vicinity, and thus, multiple robots can improve the accuracy and reliability of pedestrian tracking.

In an intelligent transport system (ITS), if the tracking data is shared with neighboring vehicles through vehicle-to-vehicle communication, each vehicle can detect pedestrians efficiently. This facilitates the construction of an advanced driver-assistance-system. Even if pedestrians suddenly run into roads, the vehicles can detect them, and hence drivers can stop their vehicles to prevent an accident.

This paper presents a pedestrian tracking method employing multiple mobile robots and vehicles. Most studies of cooperative tracking by multiple mobile robots focus on motion planning and controlling issues [11–13]. These studies attempt to keep many moving objects visible to the mobile robots at all times while consuming as little motion energy as possible. In this paper, we address sensor-data fusion, through which pedestrian tracking is achieved by combining the tracking data from multiple mobile robots located in their vicinity.

There has been considerable research in cooperative pedestrian tracking using multiple static sensors located in the environment [14–18] and multi-sensors on robots [19,20]. Our previous work [8] presented a pedestrian tracking method using in-vehicle multi-laser range scanners; pedestrians were tracked by each LRS based on a Kalman filter. In order to enhance the tracking performance, the tracking data were blended based on covariance intersection (CI) method [21].

In this paper, we extend our previous method to pedestrian tracking with multiple mobile robots in the proximity to each other. As illustrated in Figure 1, our method contributes toward building a cooperative pedestrian tracking system using vehicles such as mobile robots, cars, and electric personal assistive mobility devices (EPAMD) in future urban city environments.

Figure 1.

Pedestrian tracking system using multiple vehicles such as mobile robots, cars, and EPAMD.

Recent studies [22,23] in cooperative pedestrian tracking by multiple mobile robots require centralized data fusion with a central server; sensing data captured by each robot are sent to a central server for subsequent data fusion. The centralized data fusion reduces system robustness and scalability. Our cooperative tracking system proposed in this paper functions in a decentralized manner without any central server. This paper is organized as follows: in Section 2, we present an overview of our experimental system. In Sections 3 and 4, we present methods of pedestrian tracking and robot localization. In Section 5, we describe simulation and experiment of pedestrian tracking to validate our method, followed by our conclusions.

2. Experimental Mobile Robots

Figure 2 shows our mobile robot system used in the experiments. We use a Okatech Mecrobot wheeled mobile robot platform. Three robots each have two independent drive-wheels. A wheel encoder is attached to each of the drive wheels to measure the wheel velocity. A fiber-optic yaw rate gyro (Tamagawa Seiki, TA7319N3) is attached to the robot's chassis to measure the turn velocity. This information is used to estimate the robot's posture based on dead reckoning. Moreover, each robot is equipped with an RTK-GPS (Novatel ProPak-V3 GPS receiver) to identify its own posture in outdoor environments. The RTK-GPS provides three types of solution: fixed, float, and single solutions. Fixed solution offers range accuracy of less than 0.2 m, and float solution achieves range accuracy of about 0.2 to 1 m. In outdoor environments causing GPS multipath problems and bad weather conditions, we get single solutions with range accuracies of several meters.

Figure 2.

Overview of the mobile robot system.

The robot is equipped with a single-layered LRS (Sick LMS100). The LRS captures laser scan images that are represented by a sequence of distance samples in a horizontal plane of 270 deg. The angular resolution of the LRS is 0.5 deg, and the number of distance samples is 541 in one scan image. The onboard computer is a Lenovo ThinkPad R500 with a 2.4 GHz Intel core 2 duo processor, and the operating system used is Microsoft Windows Vista. The sampling frequency of the sensors is 10 Hz.

Broadcast communication via a wireless LAN is used to exchange information among the robots. It takes approximately 40 ms to exchange information between robots. We employ a ring-type network structure in which the robots transmit information in the sequence: robot #1, #2, and #3.

3. Pedestrian Tracking

3.1. Overview

We define two coordinate frames: the world coordinate frame, Σ_w(O_w : X_wY_w) and the i-th robot coordinate frame, Σ_i(O_i : X_iY_i) attached at the robot body, where i = 1, 2, 3. Each robot independently detects pedestrians using its own laser image based on an occupancy-grid-based method. Table 1 briefly shows our occupancy grid algorithm in the pseudo-code format. Our detection method is detailed in [6,7].

Table 1.

Occupancy grid algorithm.

Let C [Xmax, Ymax] be a two dimensional array of cells counting the number of observations, where Xmax and Ymax are the maximum X and Y coordinates. Intialize all cells in C to zero. Make an observation with laser range scanner. Determine which cells in C are occupied in the current laser scan image, and increment the occupied cells C [X, Y]. If C [X, Y] == 0, then we have no information on the cell—free space. If C [X, Y] ≧ 7, then the cell is “static cell”—static object. If 0 < C [X, Y] < 7, then the cell is “moving cell”—moving object; pedestrian. Repeat from step 3.

The detected pedestrians are tracked using the following two tracking modes (Figure 3):

1. Individual tracking by a single robot: Each robot individually tracks pedestrians without any tracking data from other robots. The robot can only track pedestrians inside its LRS sensing area.

2. Cooperative tracking by multiple robots: The robots track pedestrians by sharing their own tracking data so that each robot can track pedestrians both inside and outside its LRS sensing area.

Figure 3.

Tracking mode; robots #1 and #2 track a pedestrian in cooperative tracking mode, and robot #3 tracks a pedestrian in individual tracking mode. The red arc indicates the LRS sensing area.

3.2. Individual Tracking

A pedestrian position in Σ_w is denoted by (x,y). If the pedestrian is assumed to move at almost constant velocity, the rate kinematics is given by:

$$\begin{array}{c}{\mathit{\text{x}}}_{(t)}={\mathit{\text{Fx}}}_{(t-1)}+\mathit{\text{G}}\mathit{\Delta }{\mathit{\text{x}}}_{(t-1)}\\ =\left(\begin{array}{cccc}1& \tau & 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& \tau \\ 0& 0& 0& 1\end{array}\right){\mathit{\text{x}}}_{(t-1)}+\left(\begin{array}{cc}{\tau }^2/2& 0\\ \tau & 0\\ 0& {\tau }^2/2\\ 0& \tau \end{array}\right)\mathit{\Delta }{\mathit{\text{x}}}_{(t-1)}\end{array}$$ (1)

where x = (x, ẋ, y, ẏ)^T. Δx = (Δẍ, Δÿ)^T is an unknown acceleration (plant noise). τ is a sampling period of sensors; in our experimental system, τ is 0.1 s.

The measurement model related to the pedestrian is then:

$$\begin{array}{c}{\mathit{\text{z}}}_{(t)}={\mathit{\text{H}}}_{i^{(t)}}{\mathit{\text{x}}}_{(t)}+{\mathit{\text{H}}}_{i^{(t)}}^{\prime }{\mathit{\text{u}}}_{i^{(t)}}+\mathit{\Delta }{\mathit{\text{z}}}_{(t)}\\ =\left(\begin{array}{cccc}cos{\psi }_{i^{(t)}}& 0& -sin{\psi }_{i^{(t)}}& 0\\ sin{\psi }_{i^{(t)}}& 0& cos{\psi }_{i^{(t)}}& 0\end{array}\right){\mathit{\text{x}}}_{(t)}+\left(\begin{array}{cc}cos{\psi }_{i^{(t)}}& -sin{\psi }_{i^{(t)}}\\ sin{\psi }_{i^{(t)}}& cos{\psi }_{i^{(t)}}\end{array}\right){\mathit{\text{u}}}_{i^{(t)}}+\mathit{\Delta }{\mathit{\text{z}}}_{(t)}\end{array}$$ (2)

where z = (z_x,z_y)^T is the measurement represented in Σ_i. Δz is the measurement noise. u_i = (x_i, y_i)^T is the position of the i-th robot in Σ_w. ψ_i is the orientation of the i-th robot in Σ_w. The posture (position and orientation) x_i = (x_i, y_i, ψ_i)^T is determined using the localization system described in Section 4.

From Equation (1), the pedestrian's posture x̂ and its associated error covariance P are predicted using a Kalman filter [24]:

$$\{\begin{array}{l}{\widehat{\mathit{\text{x}}}}_{(t/t-1)}=\mathit{\text{F}}{\widehat{\mathit{\text{x}}}}_{(t-1)}\hfill \\ {\mathit{\text{P}}}_{(t/t-1)}=\mathit{\text{F}}{\mathit{\text{P}}}_{(t-1)}{\mathit{\text{F}}}^T+{\mathit{\text{GQ}}}_{(t-1)}{\mathit{\text{G}}}^T\hfill \end{array}$$ (3)

where Q is the covariance of the plant noise Δx.

To track multiple pedestrians, as shown in Figure 4(a), a validation region with a constant radius is set around the predicted position (x^, ŷ) of each tracked pedestrian. The measurements inside the validation region are considered to be obtained from the tracked pedestrian, and it is applied to the track updated with the Kalman filter. On the other hand, the measurements outside the validation region are considered to be false alarms, and are therefore, discarded. From Equations (2) and (3), the posture of the tracked pedestrian and its associated error covariance are updated by:

$$\{\begin{array}{c}{\widehat{\mathit{\text{x}}}}_{(t)}={\widehat{\mathit{\text{x}}}}_{(t/t-1)}+{\mathit{\text{K}}}_{(t)}({\mathit{\text{z}}}_{(t)}-{\mathit{\text{H}}}_{i^{(t)}}{\widehat{\mathit{\text{x}}}}_{(t/t-1)}+{\mathit{\text{H}}}_{i^{(t)}}^{\prime }{\mathit{\text{u}}}_{i^{(t)}})\\ {\mathit{\text{P}}}_{(t)}={\mathit{\text{P}}}_{(t/t-1)}+{\mathit{\text{K}}}_{(t)}{\mathit{\text{H}}}_{i^{(t)}}{\mathit{\text{P}}}_{(t/t-1)}\end{array}$$ (4)

where K(t) = P(t/t−1)Hi(t)TS(t/t−1)−1, and S(t/t−1) = Hi(t)P(t/t−1)Hi(t)T + R(t). R is the covariance of the measurement noise Δz.

Figure 4.

Tracking condition; the red circle and black diamond indicate the tracked pedestrian and the measurement, respectively. The dashed circle indicates the validation region. (a) Case 1. (b) Case 2. (c) Case 3. (d) Case 4. (e) Case 5.

In our simulation and experiment described in Section 5, the radius of the validation region is set at 1.0 m. The covariances of the plant and measurement noises in Equations (3) and (4) are set at Q = diag (1.0 m²/s⁴, 1.0 m²/s⁴) and R = diag (0.01 m², 0.01 m²), respectively.

In crowded environments, as shown in Figures 4(b–d), multiple measurements exist inside a validation region; multiple tracked pedestrians also compete for measurements. To achieve a reliable data association (matching of tracked pedestrians and measurements), we apply a global-nearest-neighbor (GNN) algorithm [25].

We consider that, in a validation region, J pedestrians exist and K measurements are received, where J does not necessarily equal K. We then define the distance measure λ_jk from the j-th tracked pedestrian to the k-th measurement, where j = 1,2, …, J and k = 1,2, …, K as:

$${\lambda }_{jk}=\sqrt{{({\mathit{\text{z}}}_{k^{(t)}}-{\widehat{\mathit{\text{u}}}}_{j^{(t/t-1)}})}^T({\mathit{\text{S}}}_{j^{(t/t-1)}}){}^{-1}({\mathit{\text{z}}}_{k^{(t)}}-{\widehat{\mathit{\text{u}}}}_{j^{(t/t-1)}})}$$ (5)

where Sj(t/t−1) = Hj(t)Pj(t/t−1)Hj(t)T + R(t), and uj(t/t−1) is the predicted position of the j-th tracked pedestrian.

We then define the following cost matrix Λ:

$$\mathit{\text{Λ}}=\left(\begin{array}{cccc}{\lambda }_{11}& {\lambda }_{12}& \cdots & {\lambda }_{1K}\\ {\lambda }_{21}& {\lambda }_{22}& \cdots & {\lambda }_{2K}\\ ⋮& ⋮& \ddots & ⋮\\ {\lambda }_{J1}& {\lambda }_{J2}& \cdots & {\lambda }_{JK}\end{array}\right)$$ (6)

We assume that the a(j)-th measurement is assigned to the j-th pedestrian. The data association is achieved by finding the a(j) based on the Munkres algorithm [26] so that ${\sum }_{j=1}^J{\lambda }_{ja(j)}$ can be minimized. It is noted that if the k-th measurement does not exist inside the validation of the j-th tracked person, we set the distance measure at λ_jk = ∞.

Pedestrians always appear in and disappear from the LRS sensing area. They also face interaction and occlusion issues. In order to handle such conditions, we implement a tracking management system based on the following rules:

1. Track initiation: As shown in Figures 4(a–c), the measurements that are not matched with any tracked pedestrians are considered to come from new pedestrians or false alarms, which disappear soon. Therefore, we tentatively initiate tracking of the measurements with Kalman filter. If the measurements are always visible in more than N₁ s, they are considered to come from new pedestrians, and the tracking is continued. If the measurements disappear within N₁ s, they are considered to be the false alarms, and the tentative tracking is terminated.

2. Track termination: When the tracking pedestrians exit the sensing area of the LRS or they meet occlusion, no measurements exist within their validation regions. If no measurements arise from the temporal occlusion, the measurements appear again. We thus predict the positions of the tracking pedestrians with the Kalman filter. If the measurements appear again within N₂ s, we proceed with the tracking. Otherwise (see Figure 4(e)), we terminate the tracking. In our simulation and experiment described in Section 5, we set N₁ = 1.5 and N₂ = 3.0 by trial and error.

For simplicity, in this paper, pedestrians are assumed to move at an almost constant velocity, and they are tracked using the usual Kalman filter. If the pedestrians move randomly, such as walking, running, going or stopping suddenly, and turning suddenly, using multi-model-based tracking can improve the tracking performance [14,15].

3.3. Cooperative Tracking

When the robots are located near to each other, the tracking mode is switched to cooperative tracking. They communicate with each other and exchange their own tracking data, which consist of estimated positions and velocities of tracked pedestrians and their associated error covariances. Because tracking data are shared, each individual robot constantly tracks pedestrians both inside and outside its own LRS sensing area.

To elucidate cooperative tracking in detail, we consider two robots #1 and #2, as shown in Figure 5. The tracking data for the m-th pedestrian tracked by robot #1 is denoted by ${\mathit{\text{I}}}_m^{(1)}=\{{\widehat{\mathit{\text{x}}}}_m^{(1)},{\mathit{\text{P}}}_m^{(1)}\}$, where m = 1,2, …. ${\widehat{\mathit{\text{x}}}}_m^{(1)}$ denotes the estimate (position estimate ${\widehat{\mathit{\text{q}}}}_m^{(1)}$ and velocity estimate ${\widehat{\dot{\mathit{\text{q}}}}}_m^{(1)}$); ${\mathit{\text{P}}}_m^{(1)}$ is its associated error covariance. Similarly, the tracking data for the n-th pedestrian tracked by robot #2 is denoted by ${\mathit{\text{I}}}_n^{(2)}=\{{\widehat{\mathit{\text{x}}}}_n^{(2)},{\mathit{\text{P}}}_n^{(2)}\}$, where n = 1,2, …. We consider that robot #1 combines the tracking data sent from robot #2 with its own tracking data. Combining the tracking data of robot #1 with that of robot #2 can be achieved similarly.

Figure 5.

Conditions in cooperative tracking. (a) Case 1. (b) Case 2. (c) Case 3.

First, we set a validation region with a constant radius around the position estimate ${\widehat{\mathit{\text{q}}}}_m^{(1)}$ of the m-th pedestrian tracked by robot #1. We consider the position estimate ${\widehat{\mathit{\text{q}}}}_n^{(2)}$ of the n-th pedestrian tracked by robot #2 as the measurement, and then, we can determine data association (one-to-one matching of pedestrians tracked by robots #1 and #2) using the GNN algorithm. The GNN-based data association in cooperative tracking is similar as one in individual tracking mentioned in Section 3.1. In our simulation and experiment described in Section 5, the radius of the validation region is set at 1.2 m.

As shown in Figure 5(a), when a pedestrian is detected inside the sensing areas of both robots #1 and #2, the two estimates, q̂⁽¹⁾ and q̂⁽²⁾, of the pedestrian can be matched. For the matched pedestrian, robot #1 updates its own tracking data by the CI method [21]:

$$\{\begin{array}{l}{\widehat{\mathit{\text{x}}}}_{(t)}^{(1)+}={\mathit{\text{P}}}_{(t)}^{(1)+}\left\{\omega {\mathit{\text{P}}}_{(t)}^{(1)-1}{\widehat{\mathit{\text{x}}}}_{(t)}^{(1)}+(1-\omega ){\mathit{\text{P}}}_{(t)}^{(2)-1}{\widehat{\mathit{\text{x}}}}_{(t)}^{(2)}\right\}\\ {\mathit{\text{P}}}_{(t)}^{(1)+}={\left\{\omega {\mathit{\text{P}}}_{(t)}^{(1)-1}+(1-\omega ){\mathit{\text{P}}}_{(t)}^{(2)-1}\right\}}^{-1}\end{array}$$ (7)

where I⁽¹⁾ = {x̂⁽¹⁾, P⁽¹⁾} and I⁽²⁾ = {x̂⁽²⁾, P⁽²⁾} denote the tracking data of the matched pedestrian. x̂⁽¹⁾⁺ and P⁽¹⁾⁺ denote the updated tracking data and its associated error covariance, respectively. The weight ω is selected using the Golden selection search (GSS) method so that the determinant of P⁽¹⁾⁺ can be minimized under the constraint 0 ≤ ω ≤ 1. In simulation and experiment described in Section 5, the convergence threshold of the weight ω is set at 1.0*10⁻⁴. In this case, we can determine the appropriate weight ω by iterative calculation of less than twenty times.

As shown in Figure 5(b,c), for non-matched pedestrian, robot #1 updates its own tracking data as follows:

1. When a pedestrian appears inside the sensing area of robot #1 but outside that of robot #2, as shown in Figure 5(b), robot #1 has the tracking data I⁽¹⁾, but robot #2 does not have I⁽²⁾. Then, robot #1 sets I⁽¹⁾⁺ = I⁽¹⁾.

2. When a pedestrian appears inside the sensing area of robot #2 but outside that of robot #1 as shown in Figure 6(c), robot #2 has the tracking data I⁽²⁾, but robot #1 does not have I⁽¹⁾. Then, robot #1 sets I⁽¹⁾⁺ = I⁽²⁾.

Figure 6.

Robot localization by relative-scan matching; the solid and open circles indicate images scanned by robots #1 and #2, respectively.

Cooperative tracking with three or more robots can be achieved in a similar manner. Decentralized data fusion provides better system scalability and reliability than centralized data fusion [21]. Therefore, we combine the tracking data in a decentralized manner. Statistically, the tracking data are highly correlated. The conventional Kalman filter-based fusion hampers the development of a decentralized system because it needs to calculate the degree of their correlation. The CI method allows accurate fusion of the tracking data in a decentralized manner without the knowledge of the degree of their correlation. Therefore, we apply the CI algorithm.

Data association is important in pedestrian tracking. In this paper, we apply GNN-based data association to match the current measurement scan to the existing tracks. An alternative effective data association algorithm is multiple hypothesis tracking (MHT) [27,28]. In MHT, the feasible measurement-to-track association hypotheses are enumerated and evaluated up to a certain time depth. The MHT-based data association may outperform the GNN data association in crowded environments; however, in our experience, MHT data association makes real-time tracking difficult in crowded environments because it requires the evaluation of an exponentially increasing number of feasible data association hypotheses. The MHT data association also requires centralized data fusion with a central server [22]. Therefore, we apply GNN data association.

4. Estimation of Robot Posture

To achieve cooperative tracking, each robot must always identify its own posture (position and orientation) with a high degree of accuracy in a world coordinate frame Σ_w and map the tracking data onto Σ_w, for which we apply RTK-GPS. The robot also determines its own posture by a scan matching based localization to improve the accuracy of its posture. If the robot cannot retrieve RTK-GPS information, only the scan matching based localization is applied to determine its own posture.

4.1. RTK-GPS Based Localization

The robot estimates its own velocity (linear/turning velocity) based on dead reckoning using the wheel encoders and gyro. The robot is assumed to move at nearly constant velocity. Motion and measurement models of the i-th robot are then given by Equations (8) and (9), respectively:

$${\mathit{\text{V}}}_{i^{(t)}}={\mathit{\text{V}}}_{i^{(t-1)}}+\mathit{\Delta }{\mathit{\text{V}}}_{i^{(t-1)}}$$ (8)

$${\mathit{\text{z}}}_{i^{(t)}}=\left(\begin{array}{cc}1& -b/2\\ 1& b/2\\ 0& 1\end{array}\right){\mathit{\text{V}}}_{i^{(t)}}+\mathit{\Delta }{\mathit{\text{z}}}_{i^{(t)}}$$ (9)

where V_i = (v_i, ψ̇_i)^T; v_i is the linear velocity and ψ̇_i is the turning velocity. z_i = (z_l, z_r, z_ψ)^T; z_l and z_r are the velocities of the left and right wheels, respectively, measured by the wheel encoders , and z_ψ is the gyro output. ΔV_i and Δz_i are unknown acceleration (disturbance) and the sensor noise, respectively. b is the tread length of the robot.

From Equations (8) and (9), the robot velocity V_i is estimated using Kalman filter. Based on the velocity estimate V̂_i, we can determine the posture of the i-th robot x_i = (x_i, y_i, ψ_i)^T and its associated covariance by Equations (10) and (11), respectively:

$${\widehat{\mathit{\text{x}}}}_{i^{(t/t-1)}}=\left(\begin{array}{c}{\widehat{x}}_{i^{(t-1)}}+{\widehat{V}}_{i^{(t-1)}}\tau cos({\widehat{\psi }}_{i^{(t-1)}}+\frac{{\widehat{\dot{\psi }}}_{i^{(t-1)}}}{2}\tau )\\ {\widehat{y}}_{i^{(t-1)}}+{\widehat{V}}_{i^{(t-1)}}\tau sin({\widehat{\psi }}_{i^{(t-1)}}+\frac{{\widehat{\dot{\psi }}}_{i^{(t-1)}}}{2}\tau )\\ {\widehat{\psi }}_{i^{(t-1)}}+{\widehat{\dot{\psi }}}_{i^{(t-1)}}\tau \end{array}\right)$$ (10)

$${\mathit{\text{P}}}_{i^{(t/t-1)}}=\nabla {\mathit{\text{f}}}_{(t-1)}{\mathit{\text{P}}}_{i^{(t-1)}}\nabla {{\mathit{\text{f}}}_{(t-1)}}^T+\nabla {{\mathit{\text{f}}}^{\prime }}_{(t-1)}{\mathit{\text{Q}}}_{i^{(t-1)}}\nabla {{{\mathit{\text{f}}}^{\prime }}_{(t-1)}}^T$$ (11)

where x̂_i^(t/t−1) and P_i^(t/t−1) are the posture estimate and its error covariance, respectively. Q_i is the error covariance of V̂_i. ∇f and ∇f′ are the Jacobian matrices of Equation (10) at x̂_i^(t/t−1) and V̂_i, respectively.τ is a sampling period of sensors.

The measurement model related to the RTK-GPS is given by:

$$\begin{array}{c}{\mathit{\text{z}}}_{{\mathit{\text{GPS}}}^{(t)}}=\mathit{\text{H}}{\mathit{\text{x}}}_{i^{(t)}}+\mathit{\Delta }{\mathit{\text{z}}}_{{\mathit{\text{GPS}}}^{(t)}}\\ =\left(\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right){\mathit{\text{x}}}_{i^{(t)}}+\mathit{\Delta }{\mathit{\text{z}}}_{{\mathit{\text{GPS}}}^{(t)}}\end{array}$$ (12)

where z_GPS is the measurement; position of the i-th robot in Σ_w, and Δz_GPS is the measurement noise.

If the robot obtains posture information from the RTK-GPS, the robot can update its own posture and its associated covariance using Kalman filter as follows:

$$\{\begin{array}{l}{\widehat{\mathit{\text{x}}}}_{i^{(t)}}={\widehat{\mathit{\text{x}}}}_{i^{(t/t-1)}}+{\mathit{\text{K}}}_{(t)}[{\mathit{\text{z}}}_{{\mathit{\text{GPS}}}^{(t)}}-\mathit{\text{H}}{\widehat{\mathit{\text{x}}}}_{i^{(t/t-1)}}]\hfill \\ {\mathit{\text{P}}}_{i^{(t)}}={\mathit{\text{P}}}_{i^{(t/t-1)}}-{\mathit{\text{K}}}_{(t)}\mathit{\text{H}}{\mathit{\text{P}}}_{i^{(t/t-1)}}\hfill \end{array}$$ (13)

where K_(t) = P_i^(t/t−1)H_(t)^T (HP_i^(t/t−1)H^T + R_GPS^(t))⁻¹, and R_GPS is the covariance of the measurement noise Δz_GPS.

4.2. Scan Matching Based Localization

When multiple robots are located near each other, they have an overlapping sensing area. They improve their own posture accuracy by exchanging their laser-scan images and matching them in their overlapping sensing area.

To elucidate scan matching based localization in detail, we consider two robots #1 and #2, as shown in Figure 6, where the two robots are located near each other and their sensing areas partially overlap. We define the posture of robot #2 relative to robot #1 by ²z₁ = (²x₁,²y₁,²ψ₁)^T in Σ_w. Robot #1 broadcasts its own posture and a laser scan image obtained by its own LRS to robot #2. Robot #2 determines the relative posture, ²z₁, by matching its own laser scan image with that sent from robot #1 (Appendix). Hereafter, we call the laser scan matching for estimating relative posture as relative-scan matching.

Measurement model related to the relative-scan matching is given by:

$$\begin{array}{c}{{}^2\mathit{\text{z}}}_{1^{(t)}}=g({\widehat{\mathit{\text{x}}}}_{1^{(t)}},{\mathit{\text{x}}}_{2^{(t)}})+\mathit{\Delta }{{}^2\mathit{\text{z}}}_{1^{(t)}}\\ =\left(\begin{array}{ccc}cos{\psi }_{2^{(t)}}& -sin{\psi }_{2^{(t)}}& 0\\ sin{\psi }_{2^{(t)}}& cos{\psi }_{2^{(t)}}& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}{\widehat{x}}_{1^{(t)}}-x_{2^{(t)}}\\ {\widehat{y}}_{1^{(t)}}-y_{2^{(t)}}\\ {\widehat{\psi }}_{1^{(t)}}-{\psi }_{2^{(t)}}\end{array}\right)+\mathit{\Delta }{{}^2\mathit{\text{z}}}_{1^{(t)}}\end{array}$$ (14)

where x̂₁ is the posture of robot #1 estimated by the RTK-GPS-based localization. Δ²z₁ is the error of the relative posture. From Equations (13) and (14), robot #2 can determine its own posture x̂₂ and its associated error covariance P₂ using Kalman filter:

$$\{\begin{array}{l}{\widehat{\mathit{\text{x}}}}_{2^{(t)}}={\widehat{\mathit{\text{x}}}}_{2^{(t/t-1)}}+{\mathit{\text{K}}}_{(t)}[{{}^2\mathit{\text{z}}}_{1^{(t)}}-\mathit{\text{g}}({\widehat{\mathit{\text{x}}}}_{1^{(t)}},{\widehat{\mathit{\text{x}}}}_{2^{(t/t-1)}})]\hfill \\ {\mathit{\text{P}}}_{2^{(t)}}={\mathit{\text{P}}}_{2^{(t/t-1)}}-{\mathit{\text{K}}}_{(t)}\text{∇}{\mathit{\text{g}}}_{(t)}{\mathit{\text{P}}}_{2^{(t/t-1)}}\hfill \end{array}$$ (15)

where K_(t) = P_2^(t/t−1)∇g_(t)^T (∇g_(t)P_2^(t/t−1)∇g_(t)^T+²R_1^(t))⁻¹. ∇g is the Jacobian matrix of g in Equation (14) at x̂_2^(t/t−1), and ²R₁ is covariance of Δ²z₁.

5. Simulation and Experimental Results

5.1. Simulation Results

In the experiment described in Section 5.2, it is very difficult to recognize the true positions of the tracked pedestrians. Therefore, we evaluate the performance of the proposed method by simulating four-pedestrian tracking by two robots. As shown in Figure 7, two robots stops at the coordinates (x,y)=(−8.0,3.0)m and (7.0,−1.0)m, and pedestrians move at the velocity of 0.1–1.7 m/s; pedestrians #1 and #2 move side-by-side at the distance of 0.8 m from start point A, and pedestrians #3 and #4 move side-by-side at the distance of 0.8 m from start point B. Four pedestrians meet each other at point C. In the simulation, the pedestrians are assumed to be always detected correctly, and measurement noise of LRS is assumed to be uniform distribution between −0.05 m and 0.05 m. Simulation tool/software is self-produced using C++ language.

Figure 7.

Simulation condition. Red, blue, green and black lines indicate moving path of pedestrians #1, #2, #3 and #4, respectively.

Figure 8(a) shows the effect of tracking mode and data association methods to the tracking error; GNN and conventional nearest neighbor (NN) [24] methods are applied for the data association. Tracking error is evaluated by the following root mean squared error (RMS):

$${\mathit{\text{RMS}}}_{(t)}=\sqrt{\frac{1}{4}\mathbf{\text{∑}}_{i=1}^4{({\widehat{\mathit{\text{u}}}}_{i^{(t)}}-{\mathit{\text{u}}}_{i^{(t)}})}^T({\widehat{\mathit{\text{u}}}}_{i^{(t)}}-{\mathit{\text{u}}}_{i^{(t)}})}$$ (16)

where û_i^(t)=(x̂_i^(t), ŷ_i^(t))^T and u_i^(t)=(x_i^(t), y_i^(t))^T denote the position estimate and the true position, respectively, of the i-th pedestrian, where i = 1, 2, 3, 4, at the t-th laser scan.

Figure 8.

Tracking error. (a) Effect of data association method and tracking mode. (b) Effect of data fusion method. In (a), red, green, blue and black lines indicate the results by cooperative tracking using GNN, individual tracking using GNN, cooperative tracking using NN, and individual tracking using NN, respectively. In (b), red, green and black lines indicate the results by CI, Kalman filter and averaging method, respectively.

First of all, we compare the result of cooperative tracking by GNN data association with that of individual tracking by GNN data association. Both tracking modes have the similar tracking error before 200 scan (20 s); however, individual tracking mode causes large tracking error after 200 scan. Because pedestrian #2 is shadowed by pedestrian #1 around 183 scan, robot #1 loses pedestrian #2 and large tracking error occurs in individual tracking mode after 200scan. However, pedestrian #2 is visible by robot #2 even around 183 scan, and thus cooperative tracking can maintain the accurate tracking after 200scan.

Both tracking modes temporarily cause large tracking error around 160 scan (around (x,y) = (−3.0,0.7)m in Figure 7). This reason is why pedestrian #3 is temporarily shadowed by pedestrian #4 and track lost then occurs.

NN-based data association very often causes incorrect matching of tracked pedestrians and LRS measurements, and this results in the track being lost. On the other side, GNN data association reduces the track lost. Therefore, the tracking error by GNN data association becomes smaller than that by NN data association. As the result, it is clear from Figure 8(a) that cooperative tracking by GNN data association provides better tracking performance than other methods.

Next, we simulate the effect of the data fusion methods for the cooperative tracking to the tracking error. For comparison purpose, we consider three data fusion methods: CI method, Kalman filter, and averaging method. In the Kalman filter, data fusion is achieved by considering the tracking data sent from other robots to be measurements. Based on the averaging method, each robot tracks pedestrians by simply averaging its own tracking data with the tracking data sent from other robots; the averaging method equals CI method by setting the weight ω = 0.5. In this simulation, GNN data association is always applied for the data association.

Figure 8(b) shows the results. The Kalman filter and averaging method cause large tracking errors around 120 scans (around point C in Figure 7). The data fusion method is closely related to the data association method; the performance in data fusion affects that in data association, and vice versa. Compared to Kalman filter and average methods, CI method maintains the accurate tracking performance. From these simulations, we confirmed that cooperative tracking based on CI and GNN methods provides tracking performance better.

5.2. Experimental Results

To evaluate the tracking method, we conducted an experiment in an outdoor environment shown in Figure 9. Three robots and three pedestrians move around in the environment as shown in Figure 10. The moving speed of the robots is less than 0.3 m/s. The walking speeds of pedestrians #1, #2, and #3 are less than 1.5 m/s, 1.5 m/s, and 3.7 m/s, respectively: At first, pedestrian #3 walks at the same speed of pedestrians #1 and #2, and he runs at a speed of 3.7 m/s on the way. The experimental time is 188 scans (18.8 s).

Figure 9.

View of experimental environments.

Figure 10.

Movement path of robots and pedestrians. (a) Robot path. (b) Pedestrian path.

Figure 11 shows the results of pedestrian tracking only by individual tracking; figures (a), (b) and (c) show the tracks of three pedestrians estimated by robot #1, #2, and #3, respectively. Each robot partially tracks pedestrians because the pedestrians exist inside and outside the sensing area of the LRS.

Figure 11.

Pedestrian tracks estimated by individual tracking. (a) Robot #1. (b) Robot #2. (c) Robot #3. Red, blue and green lines indicate paths of pedestrians #1, #2 and #3, respectively.

Figure 12 shows the tracks of three pedestrians estimated by individual and cooperative tracking; because the three robots share the tracking data with each other, all three robots can track the three pedestrians for an extended period.

Figure 12.

Pedestrian tracks estimated by individual and cooperative tracking. Red, blue and green lines indicate paths of pedestrians #1, #2 and #3, respectively.

Figure 13 shows the duration of pedestrian tracking; figures (a), (b) and (c) show the times during which robots #1, #2 and #3, respectively, track pedestrians using individual tracking. Figure 14 shows the duration of pedestrian tracking by the individual and cooperative tracking. From these results, cooperative tracking provides a better tracking performance than individual tracking; for example, cooperative tracking detects pedestrian #3 who runs into the road 34 scan (3.4 s) faster than individual tracking. The faster the pedestrians can be detected, the safer becomes robot's navigation.

Figure 13.

Duration of individual tracking. (a) Robot #1. (b) Robot #2. (c) Robot #3. The thin line indicates the time during which the pedestrian exits the sensing area of the robot. The bold line indicates the time during which the robot tracks pedestrians using the individual tracking.

Figure 14.

Duration of individual and cooperative tracking. The thin line indicates the time during which the pedestrian exits the sensing area of each robot. The bold line indicates the time during which the robot tracks pedestrians using the individual and cooperative tracking.

6. Conclusions

This paper presents a laser-based pedestrian tracking method using multiple mobile robots. Pedestrians were tracked by each robot using Kalman filter and GNN based data association. The tracking data obtained by each robot was broadcast to others robots and was combined by the CI method. Our method shares the pedestrian tracking data with all robots, and thus, collectively they can always recognize pedestrians that may be invisible to individual robots. The method was validated by simulation and experiment. Our tracking system worked effectively in a decentralized manner without any central server.

In the experiment, three pedestrians were tracked in a sparse environment. We will next conduct pedestrian tracking experiments in crowded environments. To achieve cooperative tracking, the robots must always identify their own postures with a high degree of accuracy in a common coordinate frame, for which, in this paper, we applied two localization methods: RTK-GPS-based and relative-scan matching-based. However, in outdoor environments such as areas surrounded by high buildings and roadside trees, it is difficult for robots to obtain posture information accurately by GPS due to GPS multipath and diffraction problems and so on. To cope with this problem, we will embed a simultaneously localization and mapping (SLAM) method into our tracking system; SLAM-GPS fusion based localization will always maintain a high degree of positioning accuracy, and therefore, it will enhance the robustness of our cooperative pedestrian tracking system in GPS-denied environments such as urban cities.

Appendix

Relative-Scan Matching

We determine the posture of robot #2 ²z₁ = (²x₁,²y₁,²ψ₁)^T relative to that of robot #1 based on laser-scan matching method. The matching is based on point-to-point scan matching by the iterative closest point (ICP) algorithm [29].

From the laser scan images taken by robots #1 and #2, we compute the relative posture ²z₁ using the weighted least-squares method; the cost function is given as:

$$J={\mathbf{\text{∑}}_{i=1}^{541}{w}_i\{{\mathit{\text{q}}}_j-(\mathit{\text{R}}{\mathit{\text{p}}}_i+\mathit{\text{T}})\}}^2$$ (A1)

where $\mathit{\text{R}}=\left(\begin{array}{cc}{cos}^2{\psi }_1& -{sin}^2{\psi }_1\\ {sin}^2{\psi }_1& {cos}^2{\psi }_1\end{array}\right)$ and $\mathit{\text{T}}=\left(\begin{array}{c}{{}^{2}x}_1\\ {{}^{2}y}_1\end{array}\right)$.

Here, p_i = (p_xi, p_yi)^T , where i = 1, 2, …, 541, denotes the distance sample (scan image) from robot #1; q_j = (q_xj, q_yj)^T, where j = 1, 2, … 541, denotes the distance sample from robot #2, as shown in Figure A1.

Figure 15.

Relative-scan matching. Solid and open circles indicate scan images taken by robots #1 and #2, respectively.

Each sample p_i corresponds to the minimum distance sample q_j of all samples in the scan by robot #2; w_i denotes the weight. R and T denote the rotational matrix and translational vector, respectively.

To reduce the effects of correspondence errors in the distance samples in the two laser images, we define weight w_i according to the errors between correspondence points w_i = (1+d_ij /C)⁻¹, where d_ij denotes the distance error between the two laser images and C is a constant. In our experiment described in Section 5, C is set at 0.1.

From Equation (A1), the iterative least-squares method is used to update the relative posture ${{}^2\mathit{\text{z}}}_1^{(m-1)}$as follows:

$${{}^2\mathit{\text{z}}}_1^{(m)}={{}^2\mathit{\text{z}}}_1^{(m-1)}+{({\mathit{\text{H}}}^T\mathit{\text{WH}})}^{-1}{\mathit{\text{H}}}^T\mathit{\text{W}}(\mathit{\text{q}}-\mathit{\text{p}})$$ (A2)

where $\mathit{\text{p}}={({\mathit{\text{p}}}_1^T,{\mathit{\text{p}}}_2^T,\dots ,{\mathit{\text{p}}}_{541}^T)}^T$, $\mathit{\text{q}}={({\mathit{\text{q}}}_1^T,{\mathit{\text{q}}}_2^T,\dots ,{\mathit{\text{q}}}_{541}^T)}^T$, W = diag(w₁, w₂, …, w₅₄₁) and H=∂p/∂²z₁.

The convergent value of ${{}^2\mathit{\text{z}}}_1^{(m)}$ gives the relative posture ²z₁.