# Motion Planning for Mobile Robot with Modified BIT+
## 2 Modified BIT* Planner
### Modified BIT\({}^{\star}\)  
BIT\({}^{\star}\) algorithm construct a tree that explores from start state to goal state. It maintains a vertex queue and an edge queue, then continuously select edges that have the potential to improve the current path from the edge queue [15]. We present a modified BIT\({}^{\star}\) with the informed sample strategy as well as stretch method and the process of this modified BIT\({}^{\star}\) is illustrated in Algorithm 3.  
There are some notations used in the algorithm. The exploring tree consists of 'Vertices' and 'Edges'. \(g_{\tau}(v)\) is the cost from start to \(v\) through the current tree. '\(c(v,x)\)' is the cost of an edge \((v,x)\). \(\widehat{h}(x)\)' is the heuristic function that guide the tree to explore towards the goal and we use the distance between \(x\) and goal to estimate it.  
Many factors can be considered in path cost. In Section 2.2, the stretch method have been introduced to reduce the twists and turns of the path. And in our simulation, we use differential wheeled mobile robot to verify our modified BIT\({}^{\star}\). So we only consider the path length in \(g_{\tau}(v)\) and we use the path length from start state to \(v\) to calculate it. Actually, for other robots which have steering angle limits, it is necessary to consider the curvature of the path in the cost function \(g(x)\) and \(c(v,x)\).  
In Algorithm 3, 'BestValueInVQ' is to get the lowest estimated cost in 'VetexQueue' according to \(g_{\tau}(x)+\widehat{h}(x)\), and 'BestVertex' return the vertex in 'VertexQueue' with lowest estimated cost. 'BestValueInEQ' is to get the lowest estimated cost in 'EdgeQueue' according to \(g_{\tau}(v)+c(v,x)+\widehat{h}(x)\). Line 8 means that the best vertex has the potential to expand to get a better edge, so 'Explore' is to connect the best vertex and the near points in 'Samples' with radius \(r\) to get new edges, and then new edge \((v,x)\) that satisfy \(g_{\tau}(v)+c(v,x)+\widehat{h}(x)<C_{best}\) is added to 'EdgeQueue'. \(r\) can be defined as a variable and the specific expression is given in [15]. Then, the algorithm have to check if the best edge is able to improve the path (line 12). If the best edge can not improve the path, the current EdgeQueue and VertexQueue would be cleared and 'Samples' would be updated in next iteration. Algorithm 3 shows a simple description of our proposed modified BIT*.  
Compared to original BIT*, we use a simple stretch method shown in Algorithm 2 when getting a new path, which is able to reduce the twists and turns as well as the path cost. The cost of stretched path will be used as \(C_{best}\) to limit the sample area in next sampling process, accelerating the converging rate. In real implementation, the collision checking is necessary though it is not shown in Algorithm 3. Besides, the stretch method would be adopted only when a new path is found to reduce computational cost.  
```
1 Vertices \(\leftarrow\{x_{start}\}\); Edges \(\leftarrow\)\(\emptyset\); Samples \(\leftarrow\{x_{goal}\}\);
2 EdgeQueue \(\leftarrow\)\(\emptyset\); VertexQueue \(\leftarrow\)\(\emptyset\); \(C_{best}=\infty\);
3 while i++ < Iterations do
4 ifEdgeQueue \(\equiv\)\(\emptyset\) and VertexQueue \(\equiv\)\(\emptyset\)then
5 Samples \(\leftarrow\) InformedSample(\(x_{start}\), \(x_{goal}\), \(C_{best}\));
6\(V_{old}\leftarrow\) Vertices;
7 VertexQueue \(\leftarrow\) Vertices;
8 while BestValueInVQ \(\leq\) BestValueInEQ do
9 VertexQueue \(\leftarrow\) BestVertex;
10 Explore(BestVertex);
11 Get BestEdge from EdgeQueue and remove it;
12 if The BestEdge can improve current path then
13 Add BestEdge to Edges and update current tree;
14 ifGetNewPath then
15 NewPath = GetPath(Edges, Vertices);
16 StretchedPath = Stretch(NewPath);
17\(C_{best}\) = CalculateCost(NewPath);
18
19 else
20 EdgeQueue = \(\emptyset\);VertexQueue = \(\emptyset\);
21
22 return StretchedPath;
```  
**Algorithm 3**Modified BIT*.