# RJ-RRT: Improved RRT for Path Planning in Narrow Passages
## 3 Rj-Rrt
### Environmental Judgment  
The main purpose of environmental judgment is to identify narrow passages and set the expansion space of subtrees (Line 4 in Algorithm 3). Environmental judgment will mainly occur during part of the sampling process, and special cases where environmental judgment is not performed will be described in the next section. When randomly generating a sampling point \(x_{rand}\) in the sampling space, the environment around \(x_{rand}\) will be judged. As shown in Figure 3. Firstly, if \(x_{rand}\) is located in \(X_{free}\), the subsequent judgment will be omitted. If \(x_{rand}\) is located in \(X_{obs}\), then perform local sampling in the circle with \(x_{rand}\) as the center and \(R1\) as the radius to obtain \(N1\) sampling points and determine the space where these sampling points are located (Line 1 in Algorithm 5). If there are sample points located in \(X_{free}\), it can be inferred that \(x_{rand}\) is at the boundary of the obstacle and close to free space. Randomly pick a sampling point in \(X_{free}\), denoted as \(x_{cir}\). Divide the circle with \(x_{cir}\) as the center and \(R2\) as the radius into eight equal parts and obtain eight marked points on the boundary of the circle, denoted as \(M1\), \(M2\), \(M3\), \(M4\), \(M5\), \(M6\), \(M7\), and \(M8\) (Lines 2-3 in Algorithm 5). Through the location of the marked points, it is further identified whether the current surrounding environment of \(x_{cir}\) is the inside of the narrow passage or the entrance of the narrow passage. The process of judging the surrounding environment of the sampling point is given by Algorithm 5. ```
1:\(x_{cir}\leftarrow\mathit{LocalSample}(x_{rand})\)
2:\(\mathit{MarkedPoints}\leftarrow\mathit{Local}\mathit{ judgement}(x_{cir})\)
3:\(\mathit{SubTreeExpandSpace}\leftarrow\mathit{Local}\mathit{ judgement}(\mathit{MarkedPoints})\)
```  
**Algorithm 5** EnvironmentalJudgment  
Figure 2: The sampling space A is reduced to B through P1 and then reduced to C through P2. The expansion fails in C. The first fall back sampling and then forward sampling. **Inside the narrow passage**: as shown in Figure 4, when two or four marked points are located in \(X_{free}\), the remaining marked points are located in \(X_{obs}\), and where each marked point located in free space is matched with another free point with an angle of 180\({}^{\circ}\). Then, we inferred that \(x_{cir}\) is located inside the narrow passage. In this case, the constructed subtree expansion space is a rectangle (If the configuration space is 3D, will be a cube). The center of the rectangle is at \(x_{cir}\), where the long side is parallel to the line formed by the 180\({}^{\circ}\) marked points in free space, and the length of the side is \(L1\). The other short side of the rectangle is \(L2\). This stretched rectangle constitutes the sampling space of the subtree. **Entrance of the narrow passage**: when the marked points are distributed similarly to Figure 5, that is, there are three or four consecutive marked points and an additional isolated marked point (in Figure 5, the isolated marked point is M3) in the free space, while the adjacent to the isolated marked point or remaining marked points are located in the obstacle space then it is inferred that \(x_{cir}\) is located at the entrance of the narrow passage, and isolated marked point is used to identify narrow passage entrance and guide exploration. The constructed subtree expansion space is like that inside the narrow space. Figure 4: (**a**,**b**) are examples of \(x_{cir}\) inside the narrow passage. Figure 3: Locally sample around \(xrand\). \(xrand1\), \(xrand2\), \(xrand3\), and \(xrand4\) show four different cases, respectively. When there are sampling points in the free space in the local sampling, such as \(xrand1\) and \(xrand2\), then the surrounding environment of \(xrand\) is further judgments, \(xrand3\) and \(xrand4\) will not make further judgments.