# A Vision-Based Approach for Autonomous Motion in Cluttered Environments
## 3 Methods
### Motion-Planning System
#### 3.3.1 Problem Definition  
First, we use the A* algorithm to obtain an initial path, \(P_{init}\), which contains \(N+1\) path points \(\{Q_{0},Q_{1},\ldots,Q_{N}\}\). \(Q_{0}\) is the starting point, and \(Q_{N}\) is the target point. These two points need to remain unchanged. Then, the positions of \(\{Q_{1},Q_{2},\ldots,Q_{N-1}\}\) are adjusted to obtain a smoother path, keeping a safe distance away from obstacles. Afterward, three indicators are used for evaluating the path and expressed by the loss function:  
\[f_{total}=\lambda_{1}f_{s}+\lambda_{2}f_{c}+\lambda_{3}f_{i} \tag{8}\]  
where \(f_{s}\) is the smooth loss, \(f_{c}\) is the collision loss, and \(f_{i}\) is the path-length loss. Smooth loss is to improve the smoothness of the path through optimization, so that the path is more convenient for the robot to execute. The collision loss aims to keep the path points away from obstacles through optimization, enabling the safety of the path. In order to limit the length between two path points, the length-loss function can ensure the safety of the path and obtain a shorter path through length loss. For the \(i\) optimization point, \(Q_{i}\), the penalty function of its smoothing term is shown in Formula (9). The smaller the angle between the vector s \(Q_{i-1},Q_{i}\) and \(Q_{i},\overset{\rightarrow}{Q_{i+1}}\), the smaller the loss of the smooth term. This also means that the angle the robot turns at point \(Q_{i}\) is smaller and the path is smoother. \[f_{s}=||(\ Q_{i+1}-Q_{i})-(Q_{i}-Q_{i-1})\ ||^{2} \tag{9}\]  
The penalty of the collision term is to keep the path point away from the obstacle, so the safety loss function of Formula (10) is designed as follows:  
\[f_{c}= \begin{cases}k(d_{0}-d(Q_{i},Q_{obs}))+\eta\left(\frac{1}{d_{0}}-\frac {1}{d_{1}}\right)&d(Q_{i},Q_{obs})\leq d_{0}\\ \eta\left(\frac{1}{d(Q_{i},Q_{obs})}-\frac{1}{d_{1}}\right)&d_{0}<d(Q_{i},Q_{ obs})<d_{1}\\ 0&d(Q_{i},Q_{obs})\geq d_{1}\end{cases}  \tag{10}\]  
where \(d(Q_{i},Q_{obs})\) is the distance between the path point \(Q_{i}\) and the nearest obstacle \(Q_{obs}\). We set \(k\) to a large value. If \(d(Q_{i},Q_{obs})<d_{0}\), a large gradient value will be obtained to make \(Q_{i}\) move away from the obstacle quickly, so that \(d\) is greater than \(d_{0}\). If \(d_{0}<d(Q_{i},Q_{obs})<d_{1}\), a gradient value will be obtained, and the adjustment direction of the path point will be determined together with other items. If \(d\) is greater than \(d_{1}\), it means that the safety distance has been maintained, and the gradient value will be 0, thereby optimizing other items. The loss of the length term can control the distance between path points. We also designed a loss function (Formula (11)) for the length term. Moreover, \(l\) is the distance between the path points \(Q_{i}\) and \(Q_{i-1}\). If \(l>l_{1}\), a large \(k\) value could be set to make the gradient of the loss function large, thereby achieving \(l\leq l_{1}\) by adjusting \(Q_{i}\). If \(l<l_{0}\), this condition would be considered as satisfying the length condition, and then the function will adjust other loss items. If \(l_{0}<l<l_{1}\), the gradient value is at a normal value, and the adjustment method of the path point \(Q\) will be determined comprehensively together with other items. \[f_{l}= \begin{cases}k(l-l_{1})+\eta(l_{1}-l_{0})&l\geq l_{1}\\ \eta(l-l_{0})&l_{0}<l<l_{1}\\ 0&l\leq l_{0}\end{cases}  \tag{11}\]  
\[l=||(\,Q_{i}-Q_{i-1}\,)|| \tag{12}\]  
The loss of the length term allows the algorithm to obtain a shorter path, which facilitates the robot to reach the target point faster. At the same time, by limiting the distance between two path points, the safety of the entire path can be guaranteed (introduced in Section 3.3.2). We optimize the loss function of Formula (8) by gradient descent to obtain a path. Compared with the initial path obtained by the A* algorithm, the path will be smoother and farther away from obstacles.