# Path Planning and Collision Avoidance in Unknown Environments for USVs Based on an Improved D* Lite
## 2 D* Lite  
### Introduction of D* Lite  
D* Lite searches reversely from the goal node to the start node. The key point of the algorithm is the assumption that all unknown areas are free space. Based on this assumption, it can incrementally plan the path. It finds a node from the adjacent nodes that is closest to the goal node by minimizing the distance (\(rhs(s)\)) between the current node and the goal node. When starting to move according to the path, it sets the current node as the start node. Thus, the path planning time is continuously reduced while moving towards the goal node.  
We assume that \(Succ(s)\) denotes the set of successor nodes of node \(s\), \(Pred(s)\) represents the set of predecessor nodes of node \(s\), \(c(s,s^{\prime})\) denotes the cost from node \(s\) to node \(s^{\prime}\), \(s_{start}\) is the start node, and \(s_{goal}\) is the goal node. D* Lite represents the cost from a node to the goal node \(s_{goal}\) with two estimated values, \(g(s)\) and \(rhs(s)\). The \(g(s)\) denotes the estimated minimum cost from the goal node to the current node \(s\). When expanding to the adjacent nodes of the current node \(s\), the \(rhs(s)\) of each adjacent node is recalculated to ensure that it is always the minimal estimated value. When \(rhs(s)\) is calculated, it will be assigned to \(g(s)\), as shown in Equation (1).  
\[g(s)=rhs(s) \tag{1}\]  
\(rhs(s)\) is calculated by its successor's estimated minimum cost of \(g(s^{\prime})\) and the cost of \(c(s,s^{\prime})\) between node \(s\) and \(s^{\prime}\), which is shown in the following equation:  
\[rhs(s)=\begin{cases}0&s=s_{goal}\\ min_{s\subseteq\text{Pred(s)}}(g(s^{\prime})+c(s,s^{\prime}))&otherwise\end{cases} \tag{2}\]  
where \(g(s^{\prime})\) is the minimal cost from the successor node \(s^{\prime}\) to the goal node \(s_{goal}\), and \(c(s,s^{\prime})\) is the edge cost from node \(s\) to node \(s^{\prime}\).  
D* Lite stores searched nodes in a priority queue \(U\) and ranks these nodes according to their \(key(s)\) value. The \(key(s)\) contains two values of \(k_{1}(s)\) and \(k_{2}(s)\). The node with the smallest key value will be selected as the next node to be searched. The definitions of \(k_{1}(s)\) and \(k_{2}(s)\) are as follows:  
\[k_{1}(s)=min(g(s),rhs(s))+h(s_{start},s)+k_{m} \tag{3}\]  
\[k_{2}(s)=min(g(s),rhs(s)) \tag{4}\]  
where \(k_{m}\) denotes the distance that the robot (USV in this paper) has moved. We use \(k_{m}\) to avoid recalculating the priority queue when replanning the path, which can improve the computing efficiency.  
We let \(h(s_{start},s)\) be the estimated value from the current node \(s\) to the start node \(s_{start}\), and we obtain the equation as follows:  
\[h(s_{start},s)=\begin{cases}0&s=s_{start}\\ c(s,s^{\prime})+h(s^{\prime},s_{start})&otherwise\end{cases} \tag{5}\]  
During the process of path planning, D* Lite chooses an adjacent node of the current node \(s\) as the next node to expand, which has the smallest \(key(s^{\prime})\) value within all the adjacent nodes. The following equation shows the comparison of key values:  
\[key(s)\leq key(s^{\prime})\Rightarrow\begin{cases}k_{1}(s)<k_{1}(s^{\prime} )\\ k_{1}(s)=k_{1}(s^{\prime})\text{ and }k_{2}(s)\leq k_{2}(s^{\prime})\end{cases} \tag{6}\]