# Collision Avoidance from Multiple Passive Agents with Partially Predictable Behavior
## 5 Navigational Approach
### Avoiding Multiple Passive Agents  
We will present the optimization procedure to navigate a robot among multiple passive agents, from a random start point to next (sub) goal \(p_{g}\), in an open environment. This paper refers the goal as a point extracted from some global way point plan. For the considered environment, the algorithm should be able to quickly re-plan motion for an arbitrary starting point. Instead of conforming to any specific path, it requires the robot to avoid collision with passive agents while moving towards the goal. The proposed approach is based on computing optimal control input that brings robot closer to goal, while its margin should be greater than or equal to some allowable minimum safety margin, represented by \(\beta\). The control input \(u_{s}^{*}\) that is actually given to a robot has margin greater than or equal to \(\beta\), (14) and it minimizes the cost in (15) :  
\[mrg(u_{s}^{*},UO^{\tau})\geq\beta \tag{14}\]  
\[u_{s}^{*}=arg\min_{u_{s}\in U^{*}_{safe}}||f(h(x_{a},u_{s},\tau))-p_{g}|| \tag{15}\]  
That is, the navigation problem can be formulated as the problem of finding \(u_{s}\in U^{\tau}_{safe}\) for which position \(f(h(x_{a},u,\tau))\) is closest to \(p_{g}\) in terms of Euclidean distance, and the margin \(mrg(u_{s}^{*},UO^{\tau})\) is greater than or equal to minimum safety margin \(\beta\). In case, if no control input in \(U^{\tau}_{safe}\) satisfies (14) then control input with the biggest margin is selected, thus giving priority to safety. In this optimization problem, the interaction time horizon \(\tau\) can be set equal to time horizon over which the behavior  
Figure 3: The green region in (**a**) is a safe control space \(U^{\tau=5}_{safe}\) obtained by taking the complement of control obstacle \(UO^{\tau=5}\) in \(U_{ad}\). In (**b**), the paths undertaken by a robot for selected control inputs in \(U^{\tau=5}_{safe}\) are shown. The velocity in red lies is in control obstacle and its associated path leads robot to penetrate into obstacle within 5 s. Velocity in yellow lies on the boundary of control obstacle thus, have margin zero. It leads the robot to graze the obstacle within 5 s. Velocity in blue has margin greater than zero and it leads the robot to navigate from a distance to the obstacle. of obstacles is predicted, that is typically in the range of 3 to 10 s. The computed control input is applied for a short time step unless a new control loop begins. It results in continuous sense-plan-act navigation framework. The rest of the section will present the procedure of solving the optimization problem. Explicit construction of control obstacle and safe control space is computationally challenging. We can earn computational savings by adopting a sampling based optimization procedure. Algorithm 1 summarizes the procedure for obtaining a set of safe control inputs and a set of inputs in the control obstacle. The sampling function on line 4 generates uniformly distributed samples in admissible control space for simplicity. However, more intelligent sampling procedure can be used. Each sampled control input is tested against temporary control obstacle at discrete time steps to obtain a set of safe control inputs and a set of control inputs in the control obstacle. ```
1:\(U_{safe}^{\mathrm{r}}\longleftarrow\varnothing\)
2:\(UU^{\mathrm{r}}\longleftarrow\varnothing\)
3:for\(i=0\) to \(n\)do
4:\(u_{i}\longleftarrow\mathrm{Sample}(U_{ad})\)
5:for\(t=0:\delta t:\tau\)do
6:if\(u_{i}\in\bigcup_{j\in N}UU_{j}(t)\)then
7:\(UU^{\mathrm{r}}=UU^{\mathrm{r}}\bigcup u_{i}\)
8: break loop
9:endif
10:endfor
11:if\(u_{i}\
otin UU^{\mathrm{r}}\)then
12:\(U_{safe}^{\mathrm{r}}=U_{safe}^{\mathrm{r}}\bigcup u_{i}\)
13:endif
14:endfor
```  
**Algorithm 1** Sample Control Space  
Algorithm 2 summarizes the procedure used for selecting a best safe control input to be given to a robot. A sampling based margin of \(u_{s}\) can be computed, which will be the weighted Euclidean distance of \(u_{s}\) to the nearest sampled control input within control obstacle.