## 4 Collision Avoidance Algorithm
### Motion Adjustment via SBUIS Control Signal Modification
\begin{table}
\begin{tabular}{c c} \hline \hline
**Pseudo-Code for Object Detection and Collision Avoidance Algorithm:** \\ \hline
1 & Initialize sensorArray \(\leftarrow\) [sensor1, sensor2, \(\ldots\), sensor8] \\
2 & loop \(\longrightarrow\) Repeats periodically during flight \\
3 & \# Get original user command raw distance measurements \\
4 & Input: \(\mathbf{U_{c}}\) = get\_user\_control\_signal \\
5 & for each sensor in sensorArray do: \\
6 & raw\_data = sensor.get\_distance\_data \\
7 & \# Filter and compute minimum distance of angular segment \\
8 & \(D_{n}\) = Minimum of angular segment distance data (raw\_data) \\
9 & \# Get obstacle direction \\
10 & \(\Theta_{n}\) = sensor.get\_direction \\
11 & \# Compute repulsive force using gaussian model \\
12 & \(\mathbf{f_{n}}=\mathit{Kexp}\left(-\frac{D_{n}^{2}}{2\sigma^{2}}\right)\left[ \begin{matrix}cos(\Theta_{n}+\pi)\\ sin(\Theta_{n}+\pi)\end{matrix}\right]\) \\
13 & end for \\
14 & \# Calculate total repulsive force vector \\
15 & \(\mathbf{F_{t}}=\frac{1}{N}\sum\limits_{n=1}^{N}\mathbf{f_{n}}\) \\
16 & \# Modify control signals to adjust flight direction and speed \\
17 & \(\mathbf{U_{mod}}=\mathbf{U_{c}}+\mathbf{F_{t}}\) \\
18 & apply\_control\_signal (\left(\mathbf{U_{mod}}\right))\) \\
19 & end loop \\ \hline \hline \end{tabular}
\end{table} 
Table 4: Pseudo-code of the proposed collision avoidance algorithm. Figure 16: Graphical representation of control signal modification: (**a**) original user command (\(\mathbf{U_{c}}\)) and generated repulsive force (\(\mathbf{F_{t}}\)); (**b**) resultant modified control signal vector (\(\mathbf{U_{mod}}\)). Figure 17: Front direction obstacle detection example. Figure 18: Control command modification in response to front obstacle detection: (**a**) As the drone moves forward, it detects an obstacle at decreasing distances, from approximately 400 cm down to 60 cm. (**b**) Then a virtual repulsive force is computed according to Equation (6). (**c**) The original user command (blue) indicates continuous forward motion. Initially, the modified control command (green) matches the user’s original command since no obstacle is present. Once the obstacle is detected, the modified control signal is gradually diverted to avoid a collision. (**d**) Finally, the summary shows the relationship between distance, generated repulsive force, raw user command, and modified control command. Figure 18 provides a set of corresponding graphs that visualize how the system processes this sensor feedback and modifies the control signals:  
* Figure 18a plots the measured distance between the drone and the obstacle, which decreases as the drone moves forward. * Figure 18b displays the resulting repulsive force generated by the system using the Gaussian model, which increases as the obstacle comes closer. * Figure 18c compares the original user command (blue line), representing continuous forward motion, with the modified control command (green line), which increases in magnitude as the repulsive force increases. * Figure 18d summarizes all three signals: distance, repulsive force, and control commands on a single graph for comprehensive comparison. This example highlights how the system selectively intervenes to override manual input when necessary. Initially, when no obstacle is present, the modified command follows the user input. As the obstacle enters the effective zone range, the computed repulsive force causes the control signal to deviate smoothly, ensuring collision avoidance without abrupt maneuvers. For clarity, the graphs were generated using data from a single angular segment.