# An INS-UWB Based Collision Avoidance System for AGV
## 3 Collision Avoidance System Design  
### UWB Positioning  
The UWB localization of the AGV collision avoidance system is based on three positioning points. Considering the traditional factory warehouse environment, the UWB anchor nodes are deployed on the corners of the warehouse shelves to decrease NLOS (none line-of-sight) effects as shown in Figure 2. After deploying the UWB anchor nodes in the warehouse, an electronic map with a coordinate will be established, which will accurately note each UWB anchor nodes and obstacles. With the deployed UWB nodes and the electronic map, the AGV could get its coordinate and calculate the distances between the shelves and obstacles in the path for collision avoidance.  
Figure 1: Warehouse of the factory.  
The movement of AGV is in a 2D plane. whereas the UWB nodes are commonly installed in high places in order to decrease the interferences of NLOS. The solution is the projection of the anchor nodes onto the ground plane, as shown in Figure 3. The smaller the distance, the higher the accuracy of UWB ranging; thus, the three anchor nodes closest to the AGV are utilized in its positioning.  
Denoting the three UWB anchor nodes as \(N1\), \(N2\), \(N3\), the distances between the AGV and the three anchor nodes as \((l_{1},l_{2},l_{3})\), the heights of the anchor nodes as \((h_{1},h_{2},h_{3})\), and the height of the UWB device on the AGV as \(h_{0}\), the vertical projections of the three UWB anchor nodes \((d_{1},d_{2},d_{3})\) are  
\[\left[\begin{array}{c}d_{1}\\ d_{2}\\ d_{3}\end{array}\right]=\left[\begin{array}{c}\sqrt{l_{1}^{2}-(h_{1}-h_{0}) ^{2}}\\ \sqrt{l_{2}^{2}-(h_{2}-h_{0})^{2}}\\ \sqrt{l_{3}^{2}-(h_{3}-h_{0})^{2}}\end{array}\right]. \tag{1}\]  
The locations in the coordinate system of the anchor nodes \(N1\), \(N2\), \(N3\) and the AGV node \(D\) are \((x_{1},y_{1},\,(x_{2},y_{2}),\,(x_{3},y_{3}),\,(x,y)\), respectively. Thus,  
\[\left\{\begin{array}{c}(x-x_{1})^{2}+(y-y_{1})^{2}=d_{1}^{2}\\ (x-x_{2})^{2}+(y-y_{2})^{2}=d_{2}^{2}\\ (x-x_{3})^{2}+(y-y_{3})^{2}=d_{3}^{2}\end{array}\right\}. \tag{2}\]  
Figure 3: Range projection.  
Figure 2: Deployment of UWB anchor nodes.  
If \(\Delta=(x_{1}-x_{2})(y_{1}-y_{3})-(x_{1}-x_{3})(y_{1}-y2)\
eq 0\), the linearization method could be adopted to get a coordinate of the point \(D\):  
\[\left[\begin{array}{c}x\\ y\end{array}\right]=A^{-1}b=\left[\begin{array}{c}\frac{K_{1}}{K}(y_{2}-Y_{3 })+\frac{K_{2}}{K}(y_{3}-y_{1})\\ \frac{K_{1}}{K}(x_{3}-x_{2})+\frac{K_{2}}{K}(x_{1}-x_{3})\end{array}\right], \tag{3}\]  
\[K=2(x_{1}-x_{3})(y_{2}-Y_{3})-2(x_{2}-x_{3})(y_{1}-y_{3}), \tag{4}\]  
\[K_{1}=x_{1}^{2}-x_{3}^{2}+y_{1}^{2}-y_{3}^{2}-d_{1}^{2}+d_{3}^{2}, \tag{5}\]  
\[K_{2}=x_{2}^{2}-x_{3}^{2}+y_{2}^{2}-y_{3}^{2}-d_{2}^{2}+d_{3}^{2}. \tag{6}\]  
The coordinate of unknown point \(D(x,y)\) can be calculated. The three point positioning method is an easy method;however, due to the distance estimation error in real applications, it may become an unsolved problem. There are several conditions which could result in unsolved problems such as those illustrated in Figure 4a,b which are the most common. In order to get better localization results, the UWB ranging errors should be addressed.