# Topological Navigation and Localization in
## 3 Topological Navigation and Localization for LHD
### Navigation
#### 3.4.1 Navigation Control  
This module receives a target for the LHD navigation, which could be a relevant location within the mine, such as an extraction/draw point or a dumping point. This target is usually defined by a dispatch system, or in some cases by a human operator. It consists of a destination node and a topological route, which is computed using the TMM as is explained in Section 3.2. This path is represented as a sequence of TMM nodes, each one containing information about the position, orientation, heading direction of the vehicle, and an indication of whether the vehicle must go through the node or come to a full stop on it. Given that the topological route is composed of TMM nodes that are originated from tunnel and intersection nodes, this module has two navigation modes: _tunnel trimming_, used inside tunnels, and _path-following_, used inside intersections. In tunnel trimming mode, the navigation modules follow the path of the tunnel's walls, while in path-following mode, the navigation modules follow a trajectory in an area were more than a single path can be taken. In addition, inside tunnel nodes, intermediate sub-goals may be generated depending on the defined waypoints (See Section 3.2). Figure 9: Diagram of the navigation system. To achieve a smooth navigation across TMM nodes, transitions between different goals must be seamless. A naive implementation to identify when a goal has been reached would be to check when the global localization estimation equals the current goal, but this often makes the movement of the vehicle not continuous and clumsy. A better approach requires that _Navigation Control_ anticipates when the LHD is going to reach a certain goal. In order to do this, a set of conditions are applied in addition to monitoring the global localization estimation:  
\[P\left(\overrightarrow{x}_{LHD}=\overrightarrow{x}_{T}\right)=\frac{1}{2\pi \sqrt{|\sigma|}}e^{-\frac{1}{2}\left[\overrightarrow{x}_{LHD}-\overrightarrow {x}_{T}\right]^{T}\sigma^{-1}\left[\overrightarrow{x}_{LHD}-\overrightarrow {x}_{T}\right]_{1,1}}>P\left(\overrightarrow{x}_{LHD}=\overrightarrow{x}_{T} \right)_{MIN} \tag{2}\]  
\[\dot{d}\left(\overrightarrow{x}_{LHD},\overrightarrow{x}_{T}\right)>\dot{d}_{MIN} \tag{3}\]  
\[|\theta_{LHD}-\theta_{T}|<\Delta\theta_{MAX} \tag{4}\]  
where:  
\(\overrightarrow{x}_{LHD}\) = 2D position estimation of the LHD. \(\overrightarrow{x}_{T}\) = 2D position of the current navigation target. \(\sigma\) = 2D Self-localization estimation variance (without orientation estimation). \(P\left(\overrightarrow{x}_{LHD}=\overrightarrow{x}_{T}\right)_{MIN}\) = Minimum 2D target reached likelihood threshold. \(\dot{d}\) = Euclidean distance function derivative with respect to time. \(\dot{d}_{MIN}\) = Minimum Euclidean distance function derivative threshold. \(\theta_{LHD}\) = LHD orientation (heading) estimation. \(\theta_{T}\) = Current target orientation (heading). \(\Delta\theta_{MAX}\) = Maximum orientation difference threshold. The condition (2) is the probabilistic estimation of actually reaching the desired target position. Condition (3) measures if the LHD is actually getting closer to the target and condition (4) measures the difference between the LHD's orientation and the current target's orientation. When navigating in _tunnel tranming_ mode, only condition (2) is used, but when navigating in _path-following_ mode, conditions (2)-(4) must be met. This way, lower values for \(P\left(\overrightarrow{x}_{LHD}=\overrightarrow{x}_{T}\right)_{MIN}\) on condition (2) can be used (which helps to anticipate transitions and obtain a smooth movement), because conditions (3) and (4) indicate that the vehicle is going to the target goal (often a tunnel entrance) in an intersection.