# Topological Navigation and Localization in
## 3 Topological Navigation and Localization for LHD
### Navigation
#### 3.4.3 Guidance  
This module performs the task of selecting the appropriate commands for the machine's actuators, given the high-level general directives of the expected motion and, at the same time, ensuring that the LHD will not hit any obstacles or mine infrastructure. For that purpose, a model-based predictive control (MPC) scheme was implemented using the vehicle's kinematic equations and a cost function that simultaneously considers the following: the high level reference commands, the distance to the walls of the tunnel, and the smooth variation of the actuator commands over time. Figure 10: Graphic representation of the kinematic variables of the vehicle. The kinematic model of a center-articulated vehicle has been presented in a number of previous publications, such as in [11]. Equations (10) and (11) show an incremental model for the machine's pose. \[\Delta[x,y,\theta]=\Delta t\cdot\left[v\ \cos(\theta),v\ \sin(\theta),(v\ \sin( \gamma)+L_{r}\omega)/\left(L_{f}\cos(\gamma)+L_{r}\right)\right] \tag{10}\]  
\[\Delta\gamma=\Delta t\cdot\omega \tag{11}\]  
where:  
\([x,y,\theta]=\) pose of the LHD (2D position and angle). \(\Delta t=\) sampling time of the discrete model. \(v=\) linear speed of the LHD. \(\gamma=\) steering angle in the LHD's pivot. \(\omega=\dot{\gamma}=\) steering speed in the LHD's pivot. \(L_{r}=\) length from the LHD's pivot to the rear wheel axis. \(L_{f}=\) length from the LHD's pivot to the front wheel axis. The previous model is used in the MPC to predict the trajectory of the machine over a predefined timespan. Then, the optimization process is carried out, in which the best actuator command \((u=[u_{v},u_{\omega}])\) for each time step is selected to minimize the following cost function:  
\[Q=Q_{map}+Q_{steering}+Q_{smooth} \tag{12}\]  
This equation shows that the cost function is composed of three parts: one for keeping the vehicle away from the tunnel walls (\(Q_{map}\)), another (\(Q_{steering}\)) for following the high-level reference commands, and a final one to smooth the optimization result over time (\(Q_{smooth}\)). In Equation (13), it can be seen that the cost associated with keeping the machine away from the walls relies on maximizing the distance between certain key points of the vehicle and the closest data point in the registered point cloud of the environment. These key points are the corners of the front and rear vehicle bodies. \[Q_{map}=\sum_{i=1}^{n}R_{F}\frac{|D_{FL,i}-D_{FR,i}|}{D_{FL,i}^{2} \ D_{FR,i}^{2}}+R_{M}\frac{|D_{ML,i}-D_{MR,i}|}{D_{ML,i}^{2}\ D_{MR,i}^{2}}+R_{R} \frac{|D_{RL,i}-D_{RR,i}|}{D_{RL,i}^{2}\ D_{RR,i}^{2}} \tag{13}\]  
With \(D_{FL,i}\) the distance between the front left corner of the machine and the closest point of the tunnel walls, predicted at time step \(i\) of the optimization process. Similarly, \(D_{FR}\), \(D_{ML}\), \(D_{MR}\), \(D_{RL}\), and \(D_{RR}\), refer to the distances from the front right, middle left, middle right, rear left, and rear right corners of the vehicle, respectively. The cost function weights, \(R_{F}\), \(R_{M}\), and \(R_{R}\), are selected to obtain proper behavior. Equation (14) details the cost related to following the command directives issued from the high-level software modules. Here, only the reference for the steering speed (\(\overline{\omega}\)) is considered, since the reference for the machine's maximum speed (\(v_{MAX}\)) is directly set as an upper bound restriction for the optimization function. Again, the cost function weight \(R_{\omega}\) is selected to obtain proper behavior. \[Q_{steering}=\sum_{i=1}^{n}R_{\omega}\ |\omega_{i}-\overline{\omega}| \tag{14}\]  
Finally, the smoothing component of the cost function (\(Q_{smooth}\)), is intended to ensure that the command has a controlled variation (i.e., limits the change in the command between time steps), and that a newly computed optimal command vector has some degree of continuity after the time span for which it was selected. Namely, the \(Q_{smooth}\) component comprises, in turn, two other terms, as stated above.