## 4 Robot Action Skills
### Navigation
#### 4.1.2 Local Planning
```
1:params\(\leftarrow\)GetLocalPlannerParameters()
2:\(\mathbf{x}_{\text{goal}}\leftarrow\)GetLocalGoal(params)
3:\(\mathbf{p}_{\text{global}}\leftarrow\)GetGlobalPose()\(\triangleright\)Using odometry
4:whileLocalGoalNotReached(\(\mathbf{p},\mathbf{x}_{\text{goal}}\))do
5:procedureDW_update()
6:\(\mathcal{O}_{\text{local}}\leftarrow\)UpdateLocalMap(\(\mathbf{p}_{\text{global}}\))\(\triangleright\)Obstacle set
7:\(\mathcal{V}_{\text{admissible}}\leftarrow\)SpanVelocityGrid(params)\(\triangleright\)Set of admissible velocity vectors
8:\(\mathbf{v}_{\text{safe}}\leftarrow\)\(\triangleright\)Set of safe velocity vectors
9:for\(\mathbf{v}_{i}\in\mathcal{V}_{\text{admissible}}\)do
10:\(t_{i}\leftarrow\)TimeToCollision(\(\mathcal{O}_{\text{local}},\mathbf{v}_{i}\))
11: FLAG_coll\(\leftarrow\)LeadToCollision(\(\mathcal{O}_{\text{local}},\mathbf{v}_{i}\),\(t_{i}\))
12: FLAG_danger\(\leftarrow\)IsDangerous(\(\mathcal{O}_{\text{local}},\mathbf{v}_{i}\))
13:ifnot(FLAG_coll and FLAG_danger)then
14:\(\mathbf{v}_{\text{safe}}.\)add(\((\mathbf{v}_{i},t_{i})\))
15:endif
16:endfor
17:if\(\mathcal{V}_{\text{safe}}==\varnothing\)then
18: StopAndReplan()
19:else
20:\(\mathbf{v}_{i}^{*}\leftarrow\)Optimize(\(\mathcal{V}_{\text{safe}}\))
21:endif
22:endprocedure
23:\(\mathbf{p}_{\text{global}}\leftarrow\)GetGlobalPose()
24:endwhile
25:PublishVelocityCommand(\(\mathbf{v}_{i}^{*}\))
```  
**Algorithm 1** Pseudo code of the dynamic window procedure. Selecting the optimal velocity vector within \(\mathcal{V}_{\text{safe}}\) requires the designing of a value function that accounts for several factors. We define the following heuristic utility functions as its components--heading, speed and distance, with respect to each safe \(\mathbf{v}_{i}\). Their effects are the following:  
Figure 6: The robot collision space is projected onto the ground as a convex polygon (green surface) to locally navigate around obstacles (blue boxes). The arrows indicate the robot’s navigational DoF. * heading--Rewards goal directed motions
* speed--Rewards high linear velocities to enforce fast goal directed motions whenever possible
* distance--Rewards long predicted times \(t_{i}\) until collision  
Effects of each utility function are given as weights \(w_{i}\) and summed into a value function:  
\[V=w_{1}H+w_{2}S+w_{3}D \tag{1}\]  
The combination of heading and speed generates an attractor behavior towards the local goal. In the presence of obstacles, the distance utility shifts the optimum of the total value function towards a deviating motion around the obstacle. Figure 7 illustrates such a scenario--the robot is placed in front of an obstacle in the \(x\)-direction of the robot coordinate system (see Figure 6), and it is shown how the value drops in the direction of the obstacle. Nevertheless, in situations when the robot detects dynamic obstacles, a frequently updated global path re-planning is performed to ensure smooth robot motion. The global plan update frequency is set to 5 Hz constituting a descent compromise of unnecessary computational burden and smooth change in robot heading considering the robot maximum velocity.