Return-to-Base Navigation of Robotic Swarms in Mars Exploration Using DoA Estimation Chen Zhu∗ , Siwei Zhang† , Armin Dammann† , Stephan Sand† , Patrick Henkel∗ , and Christoph G¨unther†∗ ∗ Institute
of Communications and Navigation, Technische Universit¨at M¨unchen, Arcisstr. 21, 80333 Munich, Germany of Communications and Navigation, German Aerospace Center, Oberpfaffenhofen, Germany
[email protected]
† Institute
Abstract—In Mars exploration tasks, robotic swarms containing multiple autonomous units are efficient and robust against hazards in the mission. However, when the swarm travels significantly far away from the base, it is not sufficiently reliable to use the local map generated during the exploration alone to return. We propose a return-to-base navigation system for swarm exploration tasks using a radio link from the base station. With intra-swarm links, a swarm can be controlled into a coplanar formation and cooperatively determine the relative position. Then the swarm exploits the difference of phase measurements for distinct swarm elements to estimate the direction of arrival (DoA). The estimation of directions is shown to be reliable to guide the swarm back to the base. Keywords—Swarm navigation, Return-to-base, Relative positioning, Direction of Arrival
I. INTRODUCTION The exploration of Mars, the neighbor planet of Earth, has attracted significant interest in the past decades. However, it takes several minutes for the radio signals to travel from Earth to Mars. As a result, the lag of remote control from Earth is so large that it is essential to execute the exploration missions autonomously. In the GNSS free environments, autonomous robotic exploration missions usually rely on sensors such as IMUs, laser scanner and cameras to navigate the robots using SLAM (Simultaneously Localization And Mapping) techniques [1] [2]. However, if the autonomous robots travel too far away from the base station, the accumulated biases in the generated map may result in a failure to return to base. Therefore, it is necessary to have a radio link from the landing base station for robust return-to-base navigation. In order to increase the system robustness against hazards in the mission, e.g. strike during landing, and to improve the exploration efficiency, we propose to use a robotic swarm including multiple autonomous units such as quadrocopters and ground rovers [3] [4]. The units in the swarm are equipped with multiple sensors. In addition, radio links are available among them for intra-swarm ranging and data transmission. In the situation of far-field exploration, the swarm can only receive non-line-of-sight (NLOS) signals that propagate This work has been performed in the framework of the project VaMEx (Valles Marineris Explorer) which is partly funded by the Federal Ministry of Economics and Technology administered by DLR Space Agency (FKZ 50NA1212) and the Bavarian Ministry of Economic Affairs, Infrastructure, Transport and Technology administered by IABG GmbH (ZB 20-8-3410.212-2012).
through long distance from the station, so a precise baseswarm range estimate is unavailable. In addition, the directions are more of interest than the distance when the swarm is far away. Therefore, the direction of arrival (DoA) estimation of the signal from the base station is the core part of the return-to-base navigation system for Mars exploration. When a swarm of unmanned airborne vehicles (UAVs) is moving in a coplanar formation, the UAVs can be regarded as a two dimensional antenna array if appropriate robotic spacing and radio signal wavelength are chosen. This property enables the robotic swarm to estimate the DoA of the received signal utilizing the carrier phase difference of the received signals. With phase measurements of the swarm array, the state-of-theart algorithms, e.g. MUSIC (MUltiple SIgnal Classification) [5] can be exploited to estimate the DoA. In this work, we design a return-to-base navigation system for Mars exploration with robotic swarms based on DoA estimation. The system model and the signal designs are proposed in Section II. In Section III we propose the DoA estimation method using a robotic swarm with internal radio links. The simulation results are provided in Section IV. II. R ETURN - TO - BASE NAVIGATION AND S YSTEM M ODEL The system is based on the situation that a robotic swarm has finished the exploration mission far away from the base station on the Mars surface. Now the swarm needs to route back with low frequency signal transmitted from the base. As Anderson et al. reviewed in [6], the UAVs can be controlled to move in a coplanar formation as in Figure 1. Zhang et al. [7] proposed an intra swarm radio system to resolve the relative positions of the UAVs in a cooperative sense. Assuming the swarm contains M coplanar UAVs with intraswarm 2D coordinates ri = (xi , yi )T , i = 1, 2, ..., M , which can be calculated according to the relative positioning demonstrated in Section III. The coordinate PM frame has its origin at the swarm center, i.e. rO = 1/M · i=1 ri = (0, 0)T . Since the signal propagates from significantly far-field, the plane wavefront assumption is valid as long as an appropriate carrier frequency is selected. Assuming the direction of arrival of the signal does not change over short period of time, and the clock drift is also insignificant during the time, the received signal model can be expressed as F (t) = A(θ, α)s(t) + n(t),
(1)
Fig. 1: Coplanar Formation of UAVs on Mars
in which s(t) = g(t)ejϕ(t) ∈ C1×1 denotes the arrival signal at the swarm center from the base station. F (t), n(t) ∈ CM ×1 denote the received signal and the noise respectively for the M UAVs. A(θ, α) ∈ CM ×1 is the steering matrix of the antenna array formed by the UAVs, which is a function of the two angles of arrival as illustrated in Figure 2.
z (xi,yi)
θ O
y
α
x Fig. 2: Angles of Arrivals for UAV Antenna Array
neighboring elements of the array in both x and y dimensions must be no larger than half of the wavelength, i.e. max
kxik+1 − xik k ≤ λ/2
(2)
max
kyjk+1 − yjk k ≤ λ/2
(3)
k=1,...,M −1
k=1,...,M −1
where ik and jk are the indices that sort x and y coordinates so that xi1 ≤ ... ≤ xiM and yj1 ≤ ... ≤ yjM . λ = c0 /fc denotes the carrier wavelength, and c0 the speed of light. Considering the ordinary UAV spacing, the wavelength of the signal should be at least dozens of meters so that the swarm elements can be utilized as an antenna array. On the other hand, a large wavelength results in scale increase in antenna size, and the Cram´er-Rao lower bound (CRLB) of DoA estimation increases as the carrier wavelength becomes larger. As a result, regarding the trade-off of all the criterion, a reasonable wavelength should be around 100 meters, corresponding to the carrier frequency around 3 MHz.
A. Carrier Frequency Selection In order to provide the swarm a robust and consistent signal that keeps essential base-swarm interaction and helps return-to-base navigation, the wave transmitted by the base station should be able to propagate for long distance and retain sufficient signal strength in non-line-of-sight cases. To fulfill the requirements of the task, a frequency band with large wavelength is preferred, which propagates along the Martian ground surface or utilizes the reflection of the ionosphere, both with low attenuation factor over distance. The ionospheric condition on Mars varies at distinct locations, dominantly affected by the sunlight. On the dayside of Mars, the Martian ionosphere has an 4.5 MHz cut-off frequency where waves cannot pass through the ionosphere, which leads to a critical frequency of 4.0 MHz for vertical incidence [8]. The order of frequency can be designed to implement the base-swarm communication. However, on the nightside, the Martian ionosphere is too unstable and rare while insufficient data also results in the lack of model for nightside ionosphere. As a result, the ionospheric reflection channel is only available on the dayside, so that the ground wave with larger wavelength that can diffract over terrestrial obstructions is the main propagation channel. The carrier frequency fc must thus satisfy fc < 4 MHz. The requirements of an antenna array is another criteria of choosing base-swarm carrier frequency. To obtain reliable carrier phase estimates without ambiguity, the spacing of two
B. Intra-Swarm Radio Link Design Swarm elements use the intra-swarm links to measure ranges and to exchange relative position information. For a complete-connected network with M elements, the total links number is M (M − 1)/2. Considering requirements of both low latency tolerance of highly dynamic UAVs and large bandwidth of accurate ranging, we design a two layers hybrid system. For the physical layer, we choose the orthogonal frequency division multiplexing (OFDM) technique as the modulation scheme due to its high flexibility and intersubcarrier interference-free property. For the media access control (MAC) layer, self-organized time division multiplexing access (SO-TDMA) is used to share the channel efficiently. The intra-swarm channel is divided into many time slots. Each slot is exclusively accessible for a particular UAV. A swarm element has three modes: activated, mirror and passive. It is activated only within its own time slot when it broadcasts pilots, obtains measurements and transmits data. Elements switch to mirror mode according to the command from the activated element. They listen to a certain band and mirror the observed signal back with another band to help the activated element obtaining the round-trip delay (RTD) measurements. The rest elements are in the silent mode and only passively listen to the signal from others. We use a PCO-based (PulseCoupled Oscillator) hybrid time division method to achieve slot synchronization and allocation [7].
III. R ELATIVE P OSITIONING AND D IRECTION OF A RRIVAL E STIMATION A. Ranging and Clock Synchronization with OFDM Signal Provided the estimate of the starting point of the arrival signal by OFDM signal synchronization, the time of arrival from transmitter i to receiver j can be estimated as tij = tRx,j − tT x,i . As a result, the corresponding pseudorange can be determined by (a) Unfavorable references
ρij = c0 tij = dij + c0 (δj − δi ),
(4)
where dij = dji denotes the true range, while δi and δj denotes the clock offset of the transmitter i and receiver j respectively. Similarly from the reverse link we can obtain ρji = dji + c0 (δi − δj ). Therefore, the unknown clock biases are eliminated and the range can be estimated by dˆij = (ρij + ρji )/2, which can also be regarded as round trip delay (RTD) measurements. To achieve simultaneous multiple interference-free measurements, an interleaved RTD (I-RTD) scheme is applied. An activated element transmits with only a sparse subcarrier grid and the elements in mirror mode re-transmit the received signal back in another band with different additional frequency shifts [7]. Furthermore, the pseudorange estimates can be utilized to synchronize the clocks between the unit pairs. Intuitively the clock difference can be estimated by δˆij = (ρij −ρji )/(2c0 ). The estimates can be used to synchronize the clocks of the UAVs. Without clock synchronization, the clock difference will result in a bias in the carrier phase measurements that βij = e2πfc δij . The biases significantly affects the performance of the DoA estimation in Section III-C if they are not corrected. B. Relative Positioning Due to the absence of external information, only the element position relative to the swarm local reference system can be estimated. Nevertheless, the relative position information is sufficient to further estimate the DoA of the return-tobase signal. Fig. 3 demonstrates the impact of reference elements choice on the relative positioning accuracy. It can be seen that one criteria is to choose the peer of elements with largest distance as the references. However, for a largescale decentralized system, reference choice negotiation leads to additional delay and communication overhead. For our relative positioning algorithm, instead of fixing the references with chosen elements, all elements generate random initial coordinates and adjust them according to the measurements and the neighbor’s coordinates. After some iterations, the random coordinate systems will converge, and it is easy to translate the converged coordinate system to the frame with origin at the swarm center as defined in Section II. In the meantime, formation estimate is automatically stabilized. C. Direction of Arrival Estimation using MUSIC With intra-swarm cooperative positioning, the coplanar UAVs can be regarded as an antenna array with estimated
(b) Favorable references choice
Fig. 3: Comparison for different reference elements choices: blue dots are the chosen reference elements, reds are the other elements. Green ellipses show the 6σ accuracies of relative positioning drawn from the CRLB.
coordinates rˆ1 , rˆ2 , ..., rˆM , in which rˆi = (ˆ xi , yˆi )T . We can define the unit electrical phase angles ψx and ψy as d0 sin(θ) cos(α) (5) λ d0 ψy = 2π sin(θ) sin(α) (6) λ where d0 = 1 [m] denotes the unit distance, while θ and α are the physical zenith and azimuth angle as shown in Figure 2. As a result, the i-th entry of the steering matrix A is ψx = 2π
ai = ej(ψx xˆi /d0 +ψy yˆi /d0 ) ,
i = 1, 2, ..., M
(7)
In the condition of only one signal source, i.e. s(t) in Eqn. (1) is a scaler as in our case, the MUSIC algorithm is equivalent to a maximum likelihood estimation of the direction of arrival. Based on the steering matrix, the MUSIC spectrum function can be constructed as 1 1 = , f (ψx , ψy ) = 2 H Q QH A(ψ , ψ ) kQH A(ψ , ψ )k A(ψ , ψ ) x y x y n x y n n (8) where Qn is the matrix containing eigenvectors corresponding to the M − 1 small eigenvalues of the covariance matrix ˆ = 1 PN F (t)F (t)H . R, which can be estimated by R t=1 N According to the subspace analysis, the unit electrical phase angles ψx and ψy can be estimated as (ψˆx , ψˆy ) = arg max f (ψx , ψy ). ψx ,ψy
(9)
Knowing the unit electrical phase angles, the zenith angle θ and the azimuth angle α can be estimated by λ q 2 −1 2 ˆ θ = sin ψx + ψy (10) 2πd0 ψy α ˆ = tan−1 . (11) ψx IV. S IMULATION R ESULTS A. System Level Relative Positioning The range estimation accuracies using various simultaneous measurement links are simulated with different SNR condition and compared with the CRLBs (Fig. 4). In the simulation, 40 MHz bandwidth is used for I-RTD measurements. For high SNR (≥ -5 dB) the estimator is unbiased and the
accuracy converges to the CRLB with RMSE below 1 m. In low SNR case, the estimator may find a wrong peak and then the accuracy diverges from the CRLB. Higher number of simultaneous links results in higher efficiency, but the ranging precision also declines to some extent. In practice, we can adjust the transmit power to make sure the estimator is unbiased. Furthermore, we investigate the relative positioning (a) Formation RMSE
(b) Sample-based posterior estimation from DPF after 2 sec.
Fig. 5: System level relative positioning results 1
10
θ α
0
10
−1
RMSE [°]
10
−2
10
−3
10
−4
10
−5
10
Fig. 4: Ranging Performance in Various SNR Conditions −6
accuracy in the system level by introducing a metric called formation root mean square error (RMSE): v u M X M X u 2 eF = t (dij − kˆ ri − rˆj k)2 . (12) M (M − 1) i=1 j>i The formation RMSE shows the similarity between the true and estimated swarm formations regardless the coordinate system. In our simulation, 25 elements establish an SO-TDMA structure with 10 ms frame. Once an element believes the synchronization is achieved, it starts to transmit data and make measurements. Then, a distributed particle filter (DPF) algorithm is used to contribute to the emergence of a joint coordinate system and to estimate its own position in this system. Fig. 5 illustrates the results of the system level relative positioning simulation. The variance of ranging noise is fixed to 1 m2 . We can see that it costs less than 1.5 s to establish the SO-TDMA structure and to estimate the formation. The formation RMSE is stabilized at 0.2 m. After 2 s, the formation posterior can be well represented by the DPF, in which red samples are with high probability while blues with low. B. DoA Estimation The accuracy of DoA estimation is shown in Figure 6. In the simulation, 24 links are simultaneously used for intraswarm positioning with RTD measurements. In low SNR region, the DoA estimation error is large since the precision of the cooperative positioning cannot converge to the Cram´erRao lower bound. Thus, the positions of the UAVs cannot be reliably estimated. Nevertheless, the estimation is sufficiently accurate to guide the robotic swarm to the base direction. The SNR will increase when the swarm becomes closer to the transmitter so that the DoA precision improves as the swarm approaches the base.
10
−10
−8
−6
−4
−2
0 SNR [dB]
2
4
6
8
10
Fig. 6: RMSE of DoA Estimation for Various SNR
V. CONCLUSIONS A robotic swarm enables the detection of the base direction according to DoA estimation algorithms by controlling the UAVs to form a coplanar formation as a 2D antenna array. The carrier frequency of the return-to-base navigation signal can be determined according to the criterion for signal propagation capability and reliable DoA estimation. The DoA estimation is shown to be precise as long as the SNR is sufficiently high for reliable cooperative positioning. R EFERENCES [1] H. Durrant-Whyte and T. Bailey, “Simultaneous localization and mapping: part I,” Robotics Automation Magazine, IEEE, vol. 13, no. 2, pp. 99–110, 2006. [2] T. Bailey and H. Durrant-Whyte, “Simultaneous localization and mapping (SLAM): part II,” Robotics Automation Magazine, IEEE, vol. 13, no. 3, pp. 108–117, 2006. [3] Y. Mohan and S. G. Ponnambalam, “An extensive review of research in swarm robotics,” in Nature Biologically Inspired Computing, 2009. NaBIC 2009. World Congress on, Dec. 2009, pp. 140–145. [4] S. Sand, S. Zhang, M. M¨uhlegg, G. Falconi, C. Zhu, T. Kr¨uger, and S. Nowak, “Swarm exploration and navigation on Mars,” in International Conference on Localization and GNSS, Torino, Italy, 2013. [5] R. Schmidt, “Multiple emitter location and signal parameter estimation,” Antennas and Propagation, IEEE Transactions on, vol. 34, no. 3, pp. 276–280, 1986. [6] B. D. Anderson, B. Fidan, C. Yu, and D. Walle, “UAV formation control: theory and application,” in Recent Advances in Learning and Control. Springer, 2008, pp. 15–33. [7] S. Zhang, S. Sand, R. Raulefs, and E. Staudinger, “Self-organized hybrid channel access method for an interleaved RTD-based swarm navigation system,” in Workshop on Positioning, Navigation and Communication, Dresden, Germany, 2013. [8] C. Ho, N. Golshan, and A. Kliore, Radio Wave Prpagation Handbook for Communication on and Around Mars. Jet Propulsion Laboratory, National Aeronautics and Space Administration, 2002.
