## 5 Reactive Control for Collision Avoidance
### Find a Safe Velocity Vector of the Robot  
If \(L(\mathbf{q}(k),\mathbf{q}(k)+(\mathbf{v}-\mathbf{v}_{o}(k))*k_{p}*dt)\) crosses the objectCircle centered at \(\mathbf{O}(k)\), then the robot must calculate a safe velocity vector. Algorithm 1 is used to make the robot avoid colliding with objects as well as static map environments. Let \(obsBoundaryPointSet\) denote the set of objectBoundaryPoints measured at sample-step \(k\). Furthermore, let \(mapPointSet\) denote the set of mapPoints measured at sample-step \(k\). Recall that mapPoints denote the 2D points associated with static obstacle boundaries in a known 2D map. Let \(PointSet\) denote the union of \(obsBoundaryPointSet\) and \(mapPointSet\). Let \(detectedPoint\) indicate a point in \(PointSet\). As the safety requirement is not satisfied for at least one \(detectedPoint\in PointSet\), the robot under Algorithm 1 calculates its velocity vector satisfying the safety requirement for every \(detectedPoint\in PointSet\). Once a velocity direction which satisfies the safety requirement is found, then the robot utilizes the found velocity vector as its velocity for a single sample-step. In Algorithm 1, \(colNum=col(\mathbf{v}_{c},PointSet)\) returns the number of \(detectedPoints\in PointSet\) colliding with a candidate heading vector \(\mathbf{v}_{c}\). For collision prediction within \(k_{p}\) sample-steps in the future, we use the safety requirement. If \(colNum==0\), then Lemma 1 proved that moving with velocity \(\mathbf{v}_{c}\) assures collision with every \(detectedPoint\in PointSet\) within \(k_{p}\) sample-steps in the future. Thus, if \(colNum==0\), then one can obtain a safe velocity vector avoiding collision with every \(detectedPoint\in PointSet\). Therefore, one sets \(\mathbf{v}_{c}\) as the robot's velocity vector at sample-step \(k+1\). On the other hand, if \(colNum\
eq 0\), then one cannot derive a safe velocity vector which evades colliding with every \(detectedPoint\in PointSet\) within \(k_{p}\) sample-steps in the future. In this situation, one finds a velocity vector with the minimum collision probability by finding a heading vector which collides with the minimum number of \(detectedPoints\). Algorithm 1 shows how to obtain a safe velocity vector at sample-step \(k+1\). Let \(v_{0}\) denote a safe velocity vector at sample-step \(k+1\). Let \(\psi(k)\) define the robot's heading angle at sample-step \(k\). Under (2), one derives  
\[\mathbf{v}(k)=[\cos(\psi(k))*\|\mathbf{v}(k)\|,\sin(\psi(k))*\|\mathbf{v}(k) \|]^{\mathsf{T}}. \tag{7}\]  
Recall that the robot's speed is limited by \(s_{max}\). Therefore, \(\|\mathbf{v}(k+1)\|\) is limited as  
\[0\leq\|\mathbf{v}(k+1)\|\leq s_{max}. \tag{8}\]  
Algorithm 1 finds a safe speed utilizing the following FOR loop: for \(v_{0}=0\), \(v_{0}=v_{0}+\frac{s_{max}}{N}\), while \(v_{0}\leq s_{max}\). Here, \(N\) is a tuning parameter indicating the search precision. As one increases \(N\), one can increase the precision of the speed search. However, increasing \(N\) increases the computational load. In simulations, we use \(N=10\). This implies that the speed search is performed at \(N=10\) distinct speeds. In Algorithm 1, the FOR statement indicates that a velocity vector with low speed (\(v_{0}=0\)) is examined before examining a velocity vector with high speed (\(v_{0}=s_{max}\)). In this way, the robot does not accelerate abruptly. This is desirable considering the human safety in the building. Algorithm 1 finds a safe heading angle, utilizing the following FOR loop: FOR \(\psi_{0}=0\), \(\psi_{0}=\psi_{0}+\frac{\pi}{N}\), while \(\psi_{0}<\pi\). This FOR loop implies that a safe heading angle is found in the clockwise direction starting from the robot's heading. Once a safe heading angle is not derived using this clockwise direction search, then Algorithm 1 finds a safe heading angle utilizing the following FOR loop: FOR \(\psi_{0}=-\frac{\pi}{N}\), \(\psi_{0}=\psi_{0}-\frac{\pi}{N}\), while \(\psi_{0}>-\pi\).