# Virtual Belt Algorithm for the Management of Isolated Autonomous Intersection
## 6 Virtual Belt Algorithm
### Online Algorithm
#### 6.2.2 Trajectory Planning  
As discussed in previous section, two issues should be addressed: First, a CAV should catch up with the assigned VG within a certain time. Second, the collisions among the CAVs in the same lane should be avoided. These issues should be considered for trajectory planning. Due to physical limits, a CAV may fail to catch up with the candidate VG with the highest order. On this condition, the other candidate VGs should be examined according to their orders to find the suitable one. Two assumptions are made for the algorithm of trajectory planning: First, the speed of a CAV is nonnegative. Second, a CAV without an assigned VG can maintain a safe clearance with the front CAV in the same lane. The problem of trajectory planning is demonstrated in Figure 19. There are three VGs in a VB and two CAVs that move along the same straight lane. The speeds of these VGs are \(v_{e}\). CAV A is the first waiting CAV. CAV B is a passing CAV and has been assigned to VG 3. The trajectory of CAV B has  
Figure 18: VBs at road S.  
been planned completely. VG 1 is a candidate VG for CAV A. \(s_{c}\) means the longitudinal position just before the conflict point. \(s_{e}\) means the longitudinal position of the VG center when the VG reaches to the conflict point. Collisions may occur if the CAV cannot catch up with the assigned VG. At the time of \(t_{s}\), the initial speed and position of CAV A are \(v_{s}\) and \(s_{s}\), respectively. The aim of trajectory planning is to find a suitable trajectory for a waiting CAV to catch up with the candidate VG. The trajectory should be within the dynamic limits of the waiting CAV and avoid collisions with the frontal CAV. Considering these requirements, the trajectory of the CAV is planned using a polynomial based method. The longitudinal trajectory of a CAV can be expressed as  
\[s=f\left(t\right). \tag{4}\]  
There are mainly four constraints for the speeds and the positions of the CAV :  
\[\left\{ \begin{array}{l}\dot{f}\left(t_{s}\right)=v_{s}\\ f\left(t_{s}\right)=s_{s}\\ \dot{f}\left(t_{e}\right)=v_{e}\\ f\left(t_{e}\right)=s_{e}\end{array} \right., \tag{5}\]  
where \(t_{s}\) and \(t_{e}\) are the times at the beginning and end of planned trajectory, respectively. \(v_{s}\) and \(x_{s}\) are the speed and position of the CAV at \(t_{s}\), respectively. \(v_{e}\) and \(s_{e}\) are the speed and position at \(t_{e}\), respectively. To simplify the problem of trajectory planning, the third order polynomial is used to design the trajectory of the CAV  
\[f\left(t\right)=q_{3}\cdot t^{3}+q_{2}\cdot t^{2}+q_{1}\cdot t+q_{0}, \tag{6}\]  
where \(q_{i,i=0,1,2,3}\) denote the parameters of the planned trajectory. There are four parameters need to be obtained. And there are four constraints of the speed and position of the CAV. Substituting Equation (6) into Equation (5), one has  
\[\left[ \begin{array}{cccc}3\cdot t_{s}^{2}&2\cdot t_{s}&1&0\\ t_{s}^{3}&t_{s}^{2}&t_{s}&1\\ 3\cdot t_{e}^{2}&2\cdot t_{e}&1&0\\ t_{e}^{3}&t_{e}^{2}&t_{e}&1\end{array} \right]\left[ \begin{array}{c}q_{3}\\ q_{2}\\ q_{1}\\ q_{0}\end{array} \right]=\left[ \begin{array}{c}v_{s}\\ x_{s}\\ v_{e}\\ x_{e}\end{array} \right]. \tag{7}\]  
The parameters in Equation (7) can be calculated as  
Figure 19: Problem of trajectory planning. \[\left[ \begin{array}{c}q_{3}\\ q_{2}\\ q_{1}\\ q_{0}\end{array} \right]=\left[ \begin{array}{cccc}3\cdot t_{s}^{2}&2\cdot t_{s}&1&0 \\ t_{s}^{3}&t_{s}^{2}&t_{s}&1\\ 3\cdot t_{e}^{2}&2\cdot t_{e}&1&0\\ t_{e}^{3}&t_{e}^{2}&t_{e}&1\end{array} \right]^{-1}\left[ \begin{array}{c}v_{s }\\ x_{s}\\ v_{e}\\ x_{e}\end{array} \right]. \tag{8}\]  
The acceleration and speed of the CAV should be constrained to avoid an unpractical trajectory.