Algorithm 1: TGHM Exploration Algorithm | |
Input: Laser data X generated by LIDAR; Odometer data O generated by the mobile robot; | |
Process: | |
1: | Initialize the topology node set Tt with the origin position P0: T0 = {P0}; |
2: | Initialize the grid map M according to X and O and the topology map T according to T0; |
3: | Initialize the candidate target point set C0, the candidate topology node set N0: C0, N0 = ∅; |
4: | Initialize t as the round number of the exploration: t = 1; |
5: | repeat |
6: | Update Ct with candidate target points generated by geometry rules; |
7: | if Ct ≠ ∅ |
8: | Update Nt with filtered candidate target points meet from Ct:Nt = Nt−1 ∪ Ct; |
9: | Filter the candidate topology nodes according to Equations (10) and (11): Nt = Nt\{Nt}unqualified; |
10: | else |
11: | Nt = Nt−1; |
12: | end if |
13: | if Nt ≠ ∅ |
14: | Choose the node with the highest value from Nt by Equation (9) as the next target point Pt; |
15: | else |
16: | The exploration finishes: return M and T; |
17: | end if |
18: | Motivate the robot towards Pt and update M according to Xt and Ot; |
19: | Turn Pt into a topology node and update Tt accordingly: Tt = Tt−1 ∪ {Pt}, Nt = Nt\{Pt}; |
20: | Update the topology map T with Tt; |
21: | t = t + 1; |
22: | until return called |
Output: The established map M and the topology map T; |