# Field Navigation Using Fuzzy Elevation Maps Built with Local 3D Laser Scans
## 7 Field Navigation
### Direction Selection  
Every time an FEM and its FRM are delivered by a Map node, the Dir_selection node computes a new motion direction (see Figure 4). A total of 32 possible directions \(d_{i}\) from the projection of the current \(XYZ\) frame onto the \(X_{p}Y_{p}\) plane are checked (see Figure 15). The chosen direction \(d_{j}\) will be the one that minimizes the following cost function:  
\[J(d_{i})=\frac{\left|\Delta_{i}\right|+g\left|\delta_{i}\right|}{D_{i}}, \tag{11}\]  
where \(\Delta_{i}\) is the difference between \(d_{i}\) and the goal point direction, \(\delta_{i}\) is the angular difference between \(d_{i}\) and the projection of the \(X\)-axis, \(g=0.02\) is a factor that penalizes changes of direction, and \(D_{i}\) is the maximum distance that the robot could travel over the FEM following \(d_{i}\) (see Figure 15).  
To compute \(D_{i}\), several points \((x,y)\) are evaluated in the FEM and FRM along \(d_{i}\) with steps of 0.1 m starting from the current position of the robot. Whenever the checked point has a reliability value on the FRM less than 0.75, or has a gradient magnitude on the FEM greater than 0.35, or reach the borders of the FEM, the distance \(D_{i}\), which is initially zero, is not increased more.  
Figure 14: Magnetic field measurements before (**left**) and after calibration (**right**).  
Figure 15: Illustration of direction selection over the FEM.  
The gradient of the FEM \(\
abla H\) is employed to detect non traversable areas [18]. The magnitude of the gradient at a point with cylindrical coordinates \((\alpha,r)\) on the FEM can be calculated as:  
\[|\
abla H(\alpha,r)|=\sqrt{\left(\frac{\partial H(\alpha,r)}{\partial x}\right)^ {2}+\left(\frac{\partial H(\alpha,r)}{\partial y}\right)^{2}}, \tag{12}\]  
where:  
\[\frac{\partial H(\alpha,r)}{\partial x} =\sum_{\forall i,j}\left[z_{i,j}\left(\mu_{i}(\alpha)\,\frac{ \partial\mu_{j}(r)}{\partial x}+\mu_{j}(r)\,\frac{\partial\mu_{i}(\alpha)}{ \partial x}\right)\right], \tag{13}\] \[\frac{\partial H(\alpha,r)}{\partial y} =\sum_{\forall i,j}\left[z_{i,j}\left(\mu_{i}(\alpha)\,\frac{ \partial\mu_{j}(r)}{\partial y}+\mu_{j}(r)\,\frac{\partial\mu_{i}(\alpha)}{ \partial y}\right)\right], \tag{14}\]  
and:  
\[\frac{\partial\mu_{i}(\alpha)}{\partial x} =\left\{\begin{array}{ll}\frac{r}{r^{2}}\,\frac{1}{\alpha^{i-1} -a^{i}},&\text{if }\alpha^{i-1}\leq\alpha<\alpha^{i},\\ \frac{y}{r^{2}}\,\frac{1}{\alpha^{i+1}-a^{i}},&\text{if }\alpha^{i}\leq \alpha<\alpha^{i+1},\\ 0,&\text{otherwise},\end{array}\right.\quad\frac{\partial\mu_{j}(r)}{\partial x }=\left\{\begin{array}{ll}\frac{r}{r^{2}}\,\frac{1}{r^{i-1}-r^{i+1}},&\text{if }r^{j-1}\leq r<r^{j},\\ \frac{y}{r^{2}}\,\frac{1}{r^{i-1}-r^{i+1}},&\text{if }r^{j}\leq r<r^{j+1},\\ 0,&\text{otherwise},\end{array}\right. \tag{15}\] \[\frac{\partial\mu_{i}(\alpha)}{\partial y} =\left\{\begin{array}{ll}\frac{r}{r^{2}}\,\frac{1}{a^{i-1}-a^{i+1}},& \text{if }\alpha^{i-1}\leq\alpha<\alpha^{i},\\ \frac{x}{r^{2}}\,\frac{1}{a^{i-1}-a^{i+1}},&\text{if }\alpha^{i}\leq \alpha<\alpha^{i+1},\\ 0,&\text{otherwise}.\end{array}\right.\quad\frac{\partial\mu_{j}(r)}{\partial y }=\left\{\begin{array}{ll}\frac{y}{r^{2}}\,\frac{1}{r^{i-1}-r^{i+1}},&\text{if }r^{j-1}\leq r<r^{j},\\ \frac{y}{r^{2}}\,\frac{1}{r^{i-1}-r^{i+1}},&\text{if }r^{j}\leq r<r^{j+1},\\ 0,&\text{otherwise}.\end{array}\right. \tag{16}\]