Algorithm 1 Obstacle collision model

Input: $\left(x_{orec}^i,y_{orec}^i\right)$, $le{n}_{orec}^i$, $wi{d}_{orec}^i$, $\alpha$, $\left(x_{ocir}^i,y_{ocir}^i\right)$, $r_{ocir}^i$ Output: The distances measured by the three distance sensors to the nearest obstacles $d_s$, $d_l$, $d_r$. 1:Obtain the line-segment model of the range sensors and the line-segment model of each obstacle $\left\{P_o{P}_s\right\}$, $\left\{P_o{P}_l\right\}$, $\left\{P_o{P}_r\right\}$. 2:For rectangular obstacles i, the set of boundary models is $\left\{A^i{B}^i,A^i{D}^i,C^i{B}^i,C^i{D}^i\right\}$, whose slope and intercept are $k_j^i$ and $c_j^i$, $j=AB,AD,CB,CD$. 3:Determine whether $\left\{P_o{P}_s\right\}$ and $\left\{A^i{B}^i,A^i{D}^i,C^i{B}^i,C^i{D}^i\right\}$ are parallel. 4:if parallel then 5:    It indicates no intersection points. 6:else 7:    Calculate the intersection point $\left(x_c,y_c\right)=\left({\displaystyle \frac{c_j^i-c_l}{k_l-k_j^i}},{\displaystyle \frac{k_l\left(c_j^i-c_l\right)}{k_l-k_j^i}}+c_l\right)$, and obtain the distance $d_l$. 8:end if 9:Sequentially verify whether the intersection point lies on the two intersecting line segments simultaneously. 10:if True then 11:    Output $d_l$. 12:else 13:    It indicates safety. 14:end if 15:For circular obstacle i, 16:if  $∥P_l{O}_{cir}∥>r_{ocir}^i$ then 17:    It indicates safety. 18:else 19:    Compute the intersection points between the line and the circle and determine the one closer to the robot as the obstacle intersection point. Next, obtain the distance $d_l$. 20:end if 21:Verify whether the intersection point lies on the line segment. 22:if True then 23:    Output $d_l$. 24:else 25:    It indicates safety. 26:end if 27:For the forward and right sensors, use the same steps as the left sensor.