# An INS-UWB Based Collision Avoidance System for AGV
## 3 Collision Avoidance System Design
### AGV Collision Avoidance System  
Based on the accurate localization, the workflow of the UWB and INS based AGV collision avoidance system is shown in Figure 5. After the AGV UWB node sends a ranging request, the UWB anchor nodes on the warehouse shelves will receive the signal and reply to it. Through TDOA(Time Difference of Arrival), the range between the AGV and the anchor nodes could be estimated. Then, the AGV might receive data from various anchor nodes, and could estimate the distances from them:  
\[R=\left[\begin{array}{cccc}r_{1},&r_{2},&r_{3},&...,&r_{n},\end{array}\right] \tag{20}\]  
where \(r_{n}\) represents the current estimated distance between AGV and anchor node \(n\).  
In order to reduce the computational complexity, the nearest three UWB anchor nodes are considered as reference nodes in the positioning of the AGV. In addition, the estimated distance from the AGV and the three anchor nodes are:  
\[R=\left[\begin{array}{cccc}r_{N1},&r_{N2},&r_{N3}\end{array}\right]. \tag{21}\]  
At the same time, the system will record the node id of the three UWB anchor nodes and find the corresponding location coordinates according to the pre-established electronic map. The coordinates could be represented as follows:  
\[A_{n}=[x_{pn},y_{pn}]\quad n\in\{1,2,3\}. \tag{22}\]  
With the UWB ranging result \(R_{N}\) and the coordinates of the UWB anchor nodes \(P_{n}\), the accurate localization result could be achieved with EKF integrating the INS data. The coordinate of the AGV at time k can be represented with \([x_{k},y_{k}]\).  
In order to avoid the collision of the AGV, the real-time distance between the coordinate of the AGV and the anchor nodes should be measured. The movement of the AGV shall be predicted at the same time to prevent the collision resulting from the control delay of the AGV. The movement prediction of the AGV could be accomplished with the positioning estimation, direction of motion and current velocity. The direction and velocity information could be obtained from the INS sensors.  
As shown in Figure 6, there could be several kinds of collision situations, no collision (a), about to collide (b), and will have collision (c).  
Figure 5: Flowchart of collision avoidance.  
In the warehouse scenario, once the position coordinate \([x_{k},y_{k}]\), the velocity \(v_{pn}\) and the angel \(\theta\) between the AGV and \(+y\) direction of the coordinate are known, the left and right side position can be calculated with the size of AGV as follows:  
\[\left[\begin{array}{c}l_{x_{k}}\\ l_{y_{k}}\end{array}\right]=\left[\begin{array}{c}x+\frac{w}{2}\cos\theta\\ y-\frac{w}{2}\sin\theta\end{array}\right];\left[\begin{array}{c}r_{x_{k}}\\ r_{y_{k}}\end{array}\right]=\left[\begin{array}{c}x-\frac{w}{2}\cos\theta \\ y+\frac{w}{2}\sin\theta\end{array}\right], \tag{23}\]  
where \(w\) is the width of the AGV.  
Considering the control delay of the AGV, assuming that the AGV keeps its current state for time \(\Delta t\), the position of the left and right side of the AGV could be calculated and predicted.  
If the area covering the four points \([l_{x_{k}},l_{y_{k}}]\), \([r_{x_{k}},r_{y_{k}}]\), \([l_{x_{k+1}},l_{y_{k+1}}]\), \([r_{x_{k+1}},r_{y_{k+1}}]\) does not contain obstacles in the electronic map, no collision happens. When the area has obstacles, collisions will happen and the AGV adjusts its direction until it meets the demand for non-collision.