# Mixed Reality Enhanced User Interactive Path Planning for Omnidirectional Mobile Robot
## 5 The Description of VFH* Algorithm
### Modification of VFH* Algorithm
#### 5.2.3 The Cost Function  
After there are several candidate directions, their cost values become the key to which is chosen as the final direction. Taking into account the omnidirectional mobile robot used in this paper--the robot can move in any direction, unlike in Equation (12)--the cost function does not need to consider the current robot angle. Since the VFH* algorithm uses path tree to judge the cost, for a generated path tree, the cost of successor nodes should be related to the cost of previous nodes, which is similar to the following A*algorithm heuristic function:  
\[f(n)=g(n)+h(n) \tag{16}\]  
where \(g(n)\) is the cost from the starting node to the current node \(n\), \(h(n)\) is the estimated cost from the current node \(n\) to the target node, and \(f(n)\) is the last cost of the current node \(n\). The cost function of VFH* should also be heuristic and include the cost of previous nodes in the path tree. Therefore, we used the following function:  
\[\left\{ \begin{aligned} g(c_{n})=&\lambda_{1} \cdot\Delta(c_{n},d_{t})+\lambda_{2}\cdot\Delta(c_{n},d_{n-1})\\ g(c_{n+i})=&\lambda_{3}\cdot g(c_{n+i-1})+\lambda_ {4}\cdot\Delta(c_{n+i},d_{t})\\ &+\lambda_{5}\cdot\Delta(c_{n+i},d_{n+i-1})\end{aligned} \right. \tag{17}\]  
where \(c_{n}\) is the candidate direction from the robot's current position to the next node, \(c_{n+i}\) is the candidate direction of the i node in a path tree when generating the tree, \(\Delta(c_{n},d_{t})\) is the absolute difference of the sector between the candidate direction and the target direction, and \(\Delta(c_{n},d_{n-1})\) is the absolute difference of the sector between the candidate direction and the previous direction. When calculating the first node with zero-depth in the path tree, the first equation in Equation (17) is used. Only the target node and the previous direction need to be considered. When calculating the successor nodes, the second equation in Equation (17) is used to consider not only the target node and the previous direction, but also the cost of the node before the path tree, and then the cost of this node is synthesized. So, \(i\) in Equation (17) should be no more than the depth of the path tree. For parameter selection, the higher \(\lambda_{1}(\lambda_{4})\), the more path selection tends toward the target region; the higher \(\lambda_{2}(\lambda_{5})\), the smoother the path selection tends to be. To achieve the effect of shortest path, parameters should meet the following conditions:  
\[\lambda_{1}>\lambda_{2},\;\lambda_{4}>\lambda_{5} \tag{18}\]  
At the same time, to make the method more sensitive to sudden virtual obstacles and to avoid obstacles in time to generate a better path, \(\lambda_{3}\) parameters should also meet the following conditions:  
\[\lambda_{3}\cdot\lambda_{1}<\lambda_{4},\;\lambda_{3}\cdot\lambda_{2}<\lambda _{5} \tag{19}\]  
Figure 5: The candidate direction selection policy. (**a**) The selection when the passable sector is the whole circle. (**b**) The selection when the passable sector is large. (**c**) The selection when the detected passable sector is small. After the path tree of a node is generated, the mobile robot will move the distance of a node in the direction with the lowest total cost in the tree, and then generate a new path tree at the location of the new node. Repeat the above steps until the robot reaches the target point.