## 4 Proposed Path and Shape Planning Method
### Narrow Spaces Detection and Covering
}{\text{argmin}} (\sqrt{\left(x_{n}^{i}-x_{ns}\right)^{2}+\left(y_{n}^{i}-y_{ns}\right)^{2}}, \tag{7}\] \[M(x_{ns},y_{ns})=200\]  
\[(\hat{x}_{ns},\hat{y}_{ns})= \underset{ \begin{subarray}{c}(x_{ns},y_{ns})\in\Omega\\ M(x_{ns},y_{ns})=100\end{subarray} }{\text{argmin}} (\sqrt{\left(x_{n}^{i}-x_{ns}\right)^{2}+\left(y_{n}^{i}-y_{ns}\right)^{2}}, \tag{8}\]  
Assuming that global path and shape planner is now sliding from left to right connecting \(W_{n}(bl)\) and \(W_{n}(br)\), the nearest narrow pixel \((\hat{x}_{ns},\hat{y}_{ns})\) within a window area \(\Omega\) having the center at \(W_{n}^{i}\) is already defined by the above algorithm. If there is no obstacle pixel in a range \((x_{n}^{i}:x_{n}^{br},y_{n}^{i}-w_{r}/2:y_{n}^{i}+w_{r}/2)\), the path and shape are planned as follows. Global shape planner will change the morphology plan of \(W_{n}^{i}\) to the morphology having \(w_{r}\) smaller than \(v_{l}+v_{r}\). Then the global path planner will use A-star to traverse narrow space. When path \(W_{n}^{i}\) reaches the last pixel point on the narrow space in front of boundary waypoint, the global shape planner is signaled that it is out of the narrow area. The shape plan at \(W_{n}^{i}\) whose location offsets with \(w_{r}\) to the right from the last narrow space is reshaped to the shape before entering the narrow area to keep the default morphology of hTetro. Then global path planner makes a path to connect the boundary waypoint as Figure 14a. In the case of existing any obstacle pixel in range \((x_{n}^{i}:x_{n}^{br},y_{n}^{i}-w_{m}/2:y_{n}^{i}+w_{m}/2)\) as Figure 14b, the global planner will define waypoints based on the nearest obstacle within \(\Omega\) with current global planner location \(W_{n}^{i}\) as follows. The position of the nearest obstacle pixel \((\hat{x}_{no},\hat{y}_{no})\) can be found by Equation (8). After changing the shape at the position in front of the narrow space of distance \(w_{m}\), the global planner will find the path by traversing the narrow area to the point \((\hat{x}_{no}-w_{m}/2,\hat{y}_{no})\) then returning to the point where the shape has changed and transform back to the default state. Since the path planning are based on the zigzag pattern, to simplify the zigzag pattern where each zigzag segment have the predefined width of O shape width, hTetro are transformed to default O shape after cover the narrow space. Moreover, transform from line I into a square O to avoid the collision during the pivot turn since square shape has the half-length comparing to the length of I shape. Then, to  
Figure 14: Covering narrow space constrains the proposed method. (**a**) Proposed method without obstacle from narrow space to waypoint, (**b**) proposed method with obstacle from narrow space to the waypoint. complete the trajectory from this point to boundary waypoint, the path plan will be calculated by using A-star as Figure 14b. The same mechanism can be applied for the case of moving from right to left. Note that when moving into the narrow area, the proposed A-star based zigzag scanning algorithm described above can be applied to cover the entire space such as a scenario where a robot passes through narrow gate to new space.