## 3 The Navigation Strategy with Path Priority
### Step 4: Velocity Control Strategy  
To have a real simulation to verify the feasibility of the proposed NSPP, we have the following arrangement for each robot. Let us establish a robot polar coordinate [31; 32] between the current position of the robot and the next navigation point (or the target) position as shown in Figure 7, where the origin of the coordinate is the next navigation point (or the target) position and the position of the robot is denoted by \(\mathrm{R}(\rho,\theta)\), where \(\rho\) is the distance between the robot and the next navigational point (or the target), and \(\theta\) is the orientation angle of the robot in the polar coordinate. Let \(\phi\) be the angle between the moving direction of the robot and the horizontal axis, and \(\beta=\pi-\theta\) and \(\alpha=-\beta-\phi\) are defined. Here, \(u_{1}\) is the speed of the robot moving, \(u_{2}\) is the rotational speed of the robot, \(l\) is the half-length of the distance between two wheels, and \(v_{l}\) and \(v_{r}\) indicate the velocities of the left and right wheels, respectively, of the robot. Therefore, we have Equation (1) [31; 32].  
\[\left[\begin{array}{c}\dot{\rho}\\ \dot{\alpha}\\ \dot{\beta}\end{array}\right]=\left[\begin{array}{cc}-\cos\alpha&0\\ \sin\alpha/\rho&-1\\ -\sin\alpha/\rho&0\end{array}\right]\left[\begin{array}{c}u_{1}\\ u_{2}\end{array}\right]\text{.} \tag{1}\]  
where  
\[u_{1}=(v_{r}+v_{l})/2\text{,} \tag{2}\]  
and  
\[u_{2}=(v_{r}-v_{l})/2l\text{.} \tag{3}\]  
Furthermore, let \(u_{1}=k_{\rho}\times\rho\) denote that the forward moving speed depends on the distance from the robot to the next navigational point (target), and \(u_{2}=k_{\alpha}\times\alpha\) means the rotational speed is determined by the angles of \(\alpha\). From Equations (2) and (3), we can find the velocities of the left wheel as \(v_{l}=k_{\rho}\times\rho-l\times k_{\alpha}\times\alpha\) and of the right wheel as \(v_{r}=k_{\rho}\times\rho+l\times k_{\alpha}\times\alpha\), respectively. Finally, let the left and right wheel speeds be normalized as Equations (4) and (5) to avoid speed reduction when the robot moves closer to the navigational point.  
\[v_{r\_nor}=k\times\frac{v_{r}}{|v_{r}|+|v_{l}|}\text{,} \tag{4}\]  
\[v_{l\_nor}=k\times\frac{v_{l}}{|v_{r}|+|v_{l}|}\text{,} \tag{5}\]  
where \(k\) is a constant.  
Figure 7: The polar coordinate of the wheeled robot (the purple arrow denotes the moving direction of the robot).  
Since \(v_{l}\) and \(v_{r}\) depend on the value of \(\rho\), the speed reduction happens when the robot is close to the navigation point (such as point 1). However, the robot still needs to move to the next navigation point (such as point 2), and then it will start to speed up its speed due to new \(\rho\) from point 1 to point 2. Therefore, the speed of the robot will be changed frequently between any two navigation points. This situation is not efficient for the robot moving and should be avoided. Since the speed of the robot is \(v=v_{r}+v_{l}\), then by the normalization in Equations (4) and (5), the final \(v\) will be \(v_{r\_nor}+v_{l\_nor}=k\), which is a constant speed.  
There are many parameters to be set for the above equations. We set \(k_{\rho}=0\) and \(k_{\alpha}=1\), which can be called the mode 1 (rotation mode) with \(|\alpha|>0.03\). Let \(k_{\rho}=1\) and \(k_{\alpha}=0.01\), which can be called the mode 2 (forward mode) with \(|\alpha|\leq 0.03\). In mode 1, we are only concerned about the direction in which the robot is moving. Mode 2 forces the robot to move toward the target regardless of its orientation on reaching the target point.