## 3 Autonomous Mapping and Navigation with Satellite Imagery and Cadastral Information
### Second Stage: Path Planning
#### 3.2.3 Determine Navigation Route from Reference Points  
In this stage, the set with the intermediate points is extracted from the reference points. They correspond to the points where the robot should navigate. The main idea is first to move the robot to the closest corner, and then, the robot navigates through the grove (path row by path row), alternating the direction of navigation (i.e., a zig-zag movement). Firstly, the closest corner of the parcel is determined by the coordinates provided by the robotic system using the Euclidean distance. The corners are the vertexes of the lines extracted from the first and last rows containing trees previously detected. The idea is to move the robot from one side of the parcel to the opposite side through the row paths. The sequence of intermediate points is generated according to the first point (corner closest to the initial position of the robot) being the opposite corner the destination. Intermediate points are calculated using Algorithm 9. Finally, this stage is not directly performed by path planning but Algorithm 10) will be used to determine the sequence of individual points. Those points will be the basis for the next stage where the paths for any robotic system that composes the team are determined. ```
**Algorithm 9** IntermediatePoints[Position,MiddlePaths,Vertexes]
``` Extract corners from Vertexes: \(\mathit{Corners}=[Vertexes(1),Vertexes(2),Vertexes(Penultimate),Vertexes(Last)]\) Detect closest corner to \(\mathit{CurrentPosition}\): Corner Add \(\mathit{Corner}\) to \(\mathit{ListIntermediatePoints}\)\(ListPoints=\mathit{SequencePoints}\{Corner,\mathit{MiddlePaths},Vertexes\}\) Add \(\mathit{ListPoints}\) to \(\mathit{ListIntermediatePoints}\) Return:\(ListIntermediatePoints\) ```
**Algorithm 10** SequencePoints[Corner,MiddlePaths,Vertexes]
``` Let be \(N_{points}\) the number of points in \(\mathit{MiddlePath}\) if\(\mathit{Corner}\) is \(Vertexes(1)\)then for i = 1 to (\(N_{points}\)-1) do  Add MiddlePaths(i) to \(\mathit{ListPoints}\)  Add \(Vertexes(i\cdot 2-module(i+1,2))\) to \(\mathit{ListPoints}\) endfor  Add MiddlePaths(\(N_{points}\)) to \(\mathit{ListPoints}\)  Add Vertexes(\(last\)) to \(\mathit{ListPoints}\) endif if\(\mathit{Corner}\) is \(Vertexes(2)\)then for i = 1 to (\(N_{points}\)-1) do  Add MiddlePaths(i) to \(\mathit{ListPoints}\)  Add \(Vertexes(i\cdot 2-module(i,2))\) to \(\mathit{ListPoints}\) endfor  Add MiddlePaths(\(N_{points}\)) to \(\mathit{ListPoints}\)  Add Vertexes(\(penultimate\)) to \(\mathit{ListPoints}\) endif if\(\mathit{Corner}\) is \(Vertexes(penultimate)\)then for i = 1 to (\(N_{points}\)-1) do \(j=N_{points}+1-i\)  Add MiddlePaths(\(j\)) to \(\mathit{ListPoints}\)  Add \(Vertexes((j-1)\cdot 2-module(i,2))\) to \(\mathit{ListPoints}\) endfor  Add MiddlePaths(1) to \(\mathit{ListPoints}\)  Add Vertexes(2) to \(\mathit{ListPoints}\) endif if\(\mathit{Corner}\) is \(Vertexes(last)\)then for i = 1 to (\(N_{points}\)-1) do \(j=N_{points}+1-i\)  Add MiddlePaths(\(j\)) to \(\mathit{ListPoints}\)  Add MiddlePaths(\(j\)) to \(\mathit{ListPoints}\)  Add \(Vertexes((j-



#### 3.2.4 Path Planning Using Processed Satellite Imagery

Once the initial and intermediate points and their sequence are known, the path planning map is determined. This means calculating the full path that the robotic system should follow. Firstly, the _Speed_ map is calculated with Equation (4). This map is related to the distance to the obstacles (where the robot cannot navigate). The novel equation introduces a parameter, \(\alpha\), to penalize those parts of the path close to obstacles and to give more priority to those areas which are far from obstacles. Theoretically, the robotic system will navigate faster in those areas far from obstacles due to the low probability of collision with the obstacles. Therefore, the equation has been chosen to penalize the robotic system for navigating close to obstacles. \[Speed(x,y)=\left\{ \begin{array}{cc}100+distance(x,y)^{\alpha}&\text{if } distance(x,y)>0\\ 1&\text{otherwise}\end{array} \right.