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].

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

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.

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: (1) x ( t ) = Fx ( t − 1 ) + G Δ x ( t − 1 ) = ( 1 τ 0 0 0 1 0 0 0 0 1 τ 0 0 0 1 ) x ( t − 1 ) + ( τ 2 / 2 0 τ 0 0 τ 2 / 2 0 τ ) Δ x ( t − 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: (2) z ( t ) = H i ( t ) x ( t ) + H i ( t ) ′ u i ( t ) + Δ z ( t ) = ( cos ψ i ( t ) 0 − sin ψ i ( t ) 0 sin ψ i ( t ) 0 cos ψ i ( t ) 0 ) x ( t ) + ( cos ψ i ( t ) − sin ψ i ( t ) sin ψ i ( t ) cos ψ i ( t ) ) u i ( t ) + Δ z ( t )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]: (3) { x ^ ( t / t − 1 ) = F x ^ ( t − 1 ) P ( t / t − 1 ) = F P ( t − 1 ) F T + GQ ( t − 1 ) G Twhere 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: (4) { x ^ ( t ) = x ^ ( t / t − 1 ) + K ( t ) ( z ( t ) − H i ( t ) x ^ ( t / t − 1 ) + H i ( t ) ′ u i ( t ) ) P ( t ) = P ( t / t − 1 ) + K ( t ) H i ( t ) P ( t / t − 1 )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.

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: (5) λ j k = ( z k ( t ) − u ^ j ( t / t − 1 ) ) T ( S j ( t / t − 1 ) ) ( z k ( t ) − u ^ j ( t / t − 1 ) ) − 1where 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 Λ: (6) Λ = ( λ 11 λ 12 ⋯ λ 1 K λ 21 λ 22 ⋯ λ 2 K ⋮ ⋮ ⋱ ⋮ λ J 1 λ J 2 ⋯ λ J K )

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 ∑ j = 1 J λ j a ( 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:

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.

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].

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 I m ( 1 ) = { x ^ m ( 1 ) , P m ( 1 ) }, where m = 1,2, …. x ^ m ( 1 ) denotes the estimate (position estimate q ^ m ( 1 ) and velocity estimate q ˙ ^ m ( 1 )); P m ( 1 ) is its associated error covariance. Similarly, the tracking data for the n-th pedestrian tracked by robot #2 is denoted by I n ( 2 ) = { x ^ n ( 2 ) , 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.

First, we set a validation region with a constant radius around the position estimate q ^ m ( 1 ) of the m-th pedestrian tracked by robot #1. We consider the position estimate 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]: (7) { x ^ ( t ) ( 1 ) + = P ( t ) ( 1 ) + { ω P ( t ) ( 1 ) − 1 x ^ ( t ) ( 1 ) + ( 1 − ω ) P ( t ) ( 2 ) − 1 x ^ ( t ) ( 2 ) } P ( t ) ( 1 ) + = { ω P ( t ) ( 1 ) − 1 + ( 1 − ω ) P ( t ) ( 2 ) − 1 } − 1where 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:

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⁽¹⁾.

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.

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: (8) V i ( t ) = V i ( t − 1 ) + Δ V i ( t − 1 ) (9) z i ( t ) = ( 1 − b / 2 1 b / 2 0 1 ) V i ( t ) + Δ z i ( t )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: (10) x ^ i ( t / t − 1 ) = ( x ^ i ( t − 1 ) + V ^ i ( t − 1 ) τ cos ( ψ ^ i ( t − 1 ) + ψ ˙ ^ i ( t − 1 ) 2 τ ) y ^ i ( t − 1 ) + V ^ i ( t − 1 ) τ sin ( ψ ^ i ( t − 1 ) + ψ ˙ ^ i ( t − 1 ) 2 τ ) ψ ^ i ( t − 1 ) + ψ ˙ ^ i ( t − 1 ) τ ) (11) P i ( t / t − 1 ) = ∇ f ( t − 1 ) P i ( t − 1 ) ∇ f ( t − 1 ) T + ∇ f ′ ( t − 1 ) Q i ( t − 1 ) ∇ f ′ ( t − 1 ) Twhere 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: (12) z GPS ( t ) = H x i ( t ) + Δ z GPS ( t ) = ( 1 0 0 0 1 0 ) x i ( t ) + Δ z GPS ( t )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: (13) { x ^ i ( t ) = x ^ i ( t / t − 1 ) + K ( t ) [ z GPS ( t ) − H x ^ i ( t / t − 1 ) ] P i ( t ) = P i ( t / t − 1 ) − K ( t ) H P i ( t / t − 1 )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.

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: (14) z 2 1 ( t ) = g ( x ^ 1 ( t ) , x 2 ( t ) ) + Δ z 2 1 ( t ) = ( cos ψ 2 ( t ) − sin ψ 2 ( t ) 0 sin ψ 2 ( t ) cos ψ 2 ( t ) 0 0 0 1 ) ( x ^ 1 ( t ) − x 2 ( t ) y ^ 1 ( t ) − y 2 ( t ) ψ ^ 1 ( t ) − ψ 2 ( t ) ) + Δ z 2 1 ( t )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: (15) { x ^ 2 ( t ) = x ^ 2 ( t / t − 1 ) + K ( t ) [ z 2 1 ( t ) − g ( x ^ 1 ( t ) , x ^ 2 ( t / t − 1 ) ) ] P 2 ( t ) = P 2 ( t / t − 1 ) − K ( t ) ∇ g ( t ) P 2 ( t / t − 1 )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₁.

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 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): (16) RMS ( t ) = 1 4 ∑ i = 1 4 ( u ^ i ( t ) − u i ( t ) ) T ( u ^ i ( t ) − u i ( t ) )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.

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.

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 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 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 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.