# Robot Navigation Based on Human Trajectory Prediction and Multiple Travel Modes
## 2 Methods
### Navigation Based on Multiple Travel Modes and Predicted Pedestrians' Positions
#### 2.3.1 Follow Target Detection and Follow Mode Switching Method for Smooth and Slow-Moving States  
The ideal follow-up target should have the following characteristics: The speed direction should be pointing to the robot's goal position; the speed magnitude should be similar to the robot's normal moving speed; the position should be within a safe and comfortable distance and reasonable angle to the robot. Given the current position of the robot, \(q_{r}\), inspired by the artificial potential field [22], we define a function (Equation (12)) to evaluate the followability of a pedestrian, \(p_{i}\). The function includes two penalty terms: The deviation from the ideal position penalty term, \(U_{rep}(p)\), (10) and the deviation from the ideal speed penalty term (11). In Equation (10), the function, \(d(q_{r}p_{i})\), represents the distance between the robot and the pedestrian, \(p_{i}\); \(d_{0}\) represents the ideal distance; \(\theta\) represents the angle between the vector of \(p_{i}\) to \(q_{r}\) and the vector of \(q_{r}\) to the goal position; and \(N(\cdot)\) is a normalization function. The variation of function, \(U_{rep}(p_{i})\), with \(d(q_{r}p_{i})\) and \(\theta\) is shown in Figure 5a. When the distance of a pedestrian in DATS is too small (less than 0.8 m in this paper), the function is given a large penalty value of 1000. In Equation (11), \(v_{0}\) represents the ideal walking speed of the pedestrian, \(\alpha\) represents the direction angle of the vector of \(p_{i}\) to the goal point (ideal speed direction), and \(\gamma\) represents the speed direction of the pedestrian. The variation of the function, \(U_{rep}(p_{i})\), with the walking speed deviation from the ideal walking speed, \(v_{pi}-v_{0}\), and the direction deviation from the desired angle, \(\gamma\)-\(\alpha\), is shown in Figure 5b. When the pedestrian's speed is too small (\(<\)0.1 m/s in this paper) or the direction deviates from the ideal direction by more than \(\pi/18\), a large penalty value of 1000 will be assigned to the function. Compared with the position, the speed of the pedestrian has a greater influence on followability performance. Therefore, the minimum value of the speed penalty function is 0, and the minimum value of the position penalty function is designed to be 1. \[\mathrm{U}_{rep}(p_{i})= \begin{cases}\exp\left(N\left(\left(\frac{1}{d(q_{r},p _{i})}-\frac{1}{d_{0}}\right)^{2}\right)+N\left(sin^{2}\theta\right)\right)& \text{when }0.8\leq d(q_{r},p_{i})\leq 5\text{ and }-\frac{\pi}{4}\leq\theta\leq\frac{\pi}{4}\\ 1000&\text{otherwise}\end{cases}  \tag{10}\]  
\[\mathrm{U}_{rep}(p_{i})= \begin{cases}N\left(\left(v_{pi}-v_{0}\right)^{2} \right)+N\left(\left(\gamma-\alpha\right)^{2}\right)&\text{when }\alpha-\frac{\pi}{18}\leq\gamma\leq\alpha+\frac{\pi}{18}\text{ and }0.1\text{ m/s}\leq v_{pi}\leq 1.8\text{ m/s}\\ 1000&\text{otherwise}\end{cases}  \tag{11}\]  
\[\mathrm{U}_{total}(p_{i})=\mathrm{U}_{rep}(p_{i}).\mathrm{U}_{rep}(p_{i}) \tag{12}\]  
If the value of \(U_{total}(p_{i})\) is within 2, the pedestrian, \(p_{i}\), will be treated as a candidate high-speed-follow target; if it is in the interval (2, 12), \(p_{i}\) will be treated as a low-speed-follow target. Figure 5: (**a**) The variation of function, \(U_{rep}(p_{i})\), with \(d(q_{r},p_{i})\) and \(\theta\); (**b**) the variation of the function, \(U_{rep}(p_{i})\), with the walking speed deviation from the ideal walking speed, \(v_{pi}\)-\(v_{0}\), and the direction deviation from the desired angle, \(\gamma\)-\(\alpha\). If it is greater than 12, \(p_{i}\) cannot be regarded as a candidate following target.