# Motion Planning for Mobile Robot with Modified BIT+
## 2 Modified BIT* Planner
### Stretch Method  
The process of stretch method is illustrated in Figure 3. Based on informed RRT*, we can obtain that the converging rate partially depends on the size of the ellipse area. In Figure 2, the current path is tortuous and it may lead to a large \(C_{best}\). So we try to revise the current path to reduce it's length. For the example in Figure 3, the black solid line is the original path. If there are no obstacles between the adjacent path points, we firstly connect \(x_{start}\) and \(x_{3}\), and then find the new next path point in the straight line. Actually, the new \(x_{2}^{{}^{\prime}}\) only needs to be on the segment \(x_{start}x_{3}\) to ensure that the new path is shorter than the original path. In order to roughly maintain the density of path points, we choose the position of \(x_{2}^{{}^{\prime}}\) to make  
\[\frac{x_{start}x_{2}^{{}^{\prime}}}{x_{2}x_{3}}=\frac{x_{start}x_{2}}{x_{2}x_{3}} \tag{3}\]  
Figure 2: Informed RRT* (rapidly-exploring random tree): Once a path is generated, an ellipse is determined by the start state, goal state and path cost. and then we replace \(x_{2}\) with new path point \(x_{2}^{{}^{\prime}}\). Similarly, start from \(x_{2}^{{}^{\prime}}\), the new path points \(x_{3}^{{}^{\prime}}\), \(x_{4}^{{}^{\prime}}\) satisfy \(\frac{x_{2}^{{}^{\prime}}x_{3}^{{}^{\prime}}}{x_{3}^{{}^{\prime}}x_{4}}=\frac{ x_{2}^{{}^{\prime}}x_{3}}{x_{3}x_{4}}\) and \(\frac{x_{2}^{{}^{\prime}}x_{4}^{{}^{\prime}}}{x_{4}^{{}^{\prime}}x_{2}^{{}^{ \prime}}}=\frac{x_{2}^{{}^{\prime}}x_{3}}{x_{4}x_{3}^{{}^{\prime}}}\). Finally, we connect \(x_{4}^{{}^{\prime}}\) to the goal directly. The red solid line is the new path after the stretch method. Obviously, it is easy to prove that this new red path is shorter than the original one by using the principle that the sum of any two sides of a triangle is greater than the third side. In most cases, there are obstacles in the environment, so we must consider this common situation. For example, in Figure 2(b), the straight line between \(x_{start}\) to \(x_{3}\) will collide with the obstacles, so we find a point between \(x_{2}\) and \(x_{3}\) that could avoid collision when connecting with \(x_{start}\). Then we select the point that is nearest to the obstacle as the next point and repeat this process until arriving to goal. In Figure 2(a),b, the black line is the original path and the red line denotes the new path after stretching. We can obtain that the new path length is shorter that the original path. The new path length will be used as \(C_{best}\) in the process of InformedSample shown in Algorithm 1. For example, in Figure 4, the black ellipse is the boundary of next sampling area generated by the original path according to the strategy of informed RRT*, and the red ellipse is the boundary generated by the modified path after stretch method. The sampling points in the red ellipse region are more likely to shorten the current path and speed up the convergence rate. The complete algorithm is shown in Algorithm 2. This algorithm processes the stretch method through a loop until arriving to goal. \(P\) is the original path. 'NoCollision' in line 5 is the process to check if there is obstacle between two path points. 'GetNext' is to find a new path point to replace the original as is shown in Figure 2(a), and the principle of selecting is described above. 'Divide' in line 8 is to divide the line between two adjacent path point into several equal parts and then a series of path points are stored in a container (TemPoints in line 8). Next procedure is to find \(x_{t}\) that can avoid collision among these path points. The black arrow in Figure 2(b) indicates the direction of the search. 'NeareatToObs' is to get the nearest point to obstacle between \(x_{current}\) and \(x_{t}\). Figure 4: The boundary of sampling area determined by the modified path after stretch method. Figure 3: The process of stretching method in different situation. In real implementation, it may be difficult to get the nearest point to obstacle from the information of grid map.