*3.2. Kinematics of Vehicles*

Intelligent vehicle path planning needs to consider the kinematics to realize steering and obstacle avoidance. Articulated vehicles are considered in this paper, which are composed of a front body and rear body, and the vehicle bodies are connected through the articulated points. Articulated vehicles in underground mines are usually rear-wheel drives, and the steering is completed by controlling the relative position between the front and rear bodies through the expansion and contraction of the steering cylinders. Nonarticulated vehicles can be abstracted as articulated vehicles with a rear body length of 0 to achieve the universality of this article. Assuming that the tire and the ground have pure rolling friction, the movement process of the vehicles can be simplified to rigid body plane movement, as shown in Figure 6.

**Figure 6.** The simplified movement process of the vehicles.

In order to describe the position of the vehicle, the kinematics formula needs to be established. We established a Cartesian coordinate system for articulated vehicles. We set the instantaneous steering center as *PC*(*xC*, *yC*), the axle center of the front body as *Pf xf* , *yf* , the axle center of the rear body as *Pr*(*xr*, *yr*), the articulated point as *PO*(*xO*, *yO*), the front and rear linear velocities as *vf* and *vr*, respectively, the headings as *θ <sup>f</sup>* and *θr*, the radius of the front and rear body as *Rf* and *Rr*, respectively, the steering angle as *γ*, and the angular velocity as *ωγ*. Then the kinematics model of the articulated vehicles can be described as follows [45].

$$
\begin{bmatrix}
\dot{\mathbf{x}}\_{r} \\
\dot{y}\_{r} \\
\dot{\theta}\_{f} \\
\dot{\gamma}\_{f}
\end{bmatrix} = \begin{bmatrix}
\cos\theta\_{f} \\
\sin\theta\_{f} \\
\frac{\sin\gamma}{R\_{f}\cos\gamma + R\_{r}} \\
0
\end{bmatrix} v\_{f} + \begin{bmatrix}
0 \\
0 \\
\frac{R\_{r}}{R\_{f}\cos\gamma + R\_{r}} \\
1
\end{bmatrix} \omega\_{r} \tag{1}
$$

Then, the position equation of the vehicles can be derived to avoid collision with drifts or obstacles; that is, the collision detection should be performed on the geometric shape of the vehicles, drifts, and obstacles. The Oriented Bounding Box (OBB) method was used to transform each entity into multiple bounding boxes in different directions for intersection testing. On the premise of authenticity, it is assumed that the front and rear bodies of the articulated vehicles are two rectangles, the width is *w*, and the length of the front and rear bodies are *lf* and *lr*, respectively, as shown in Figure 7.

**Figure 7.** The geometric movement state of vehicles.

The OBB of the articulated vehicles (*Polygoncar*) can be represented by polygon *ABCDEFG*. According to the geometric relationship, the coordinates of each point of *Polygoncar* can be expressed as the formulas

$$\mathbf{x}\_A = \mathbf{x}\_o - l\_r \cos \theta\_r + \frac{w \sin \theta\_r}{2} \tag{2}$$

$$y\_A = y\_\vartheta - l\_r \sin \theta\_r - \frac{w \cos \theta\_r}{2} \tag{3}$$

$$\mathbf{x}\_B = \mathbf{x}\_0 - l\_r \cos \theta\_r - \frac{w \sin \theta\_r}{2} \tag{4}$$

$$y\_B = y\_o - l\_r \sin \theta\_r + \frac{w \cos \theta\_r}{2} \tag{5}$$

$$\mathbf{x}\_{\mathbb{C}} = \mathbf{x}\_{o} - \frac{w \sin \theta\_{r}}{2} \tag{6}$$

$$y\_C = y\_o + \frac{w \cos \theta\_r}{2} \tag{7}$$

$$\mathbf{x}\_D = \mathbf{x}\_\sigma - \frac{w \sin \theta\_f}{2} \tag{8}$$

$$y\_D = y\_o + \frac{w \cos \theta\_f}{2} \tag{9}$$

$$\mathbf{x}\_E = \mathbf{x}\_o + l\_f \cos \theta\_f - \frac{w \sin \theta\_f}{2} \tag{10}$$

$$y\_E = y\_o + l\_f \sin \theta\_f + \frac{w \cos \theta\_f}{2} \tag{11}$$

$$\mathbf{x}\_F = \mathbf{x}\_o + l\_f \cos \theta\_f + \frac{w \sin \theta\_f}{2} \tag{12}$$

$$y\_F = y\_o + l\_f \sin \theta\_f - \frac{w \cos \theta\_f}{2} \tag{13}$$

$$\begin{array}{l} \chi\_{\rm G} = \frac{w\cos\theta\_f^{-2}\cos\theta\_r - w\cos\theta\_f\cos\theta\_r^2 - w\cos\theta\_f\sin\theta\_r^2 + w\cos\theta\_r\sin\theta\_f}{2\left(\cos\theta\_f\sin\theta\_r - \cos\theta\_r\sin\theta\_f\right)}\\ +\frac{\chi\_o\cos\theta\_f\sin\theta\_r - \chi\_o\cos\theta\_r\sin\theta\_f}{\left(\cos\theta\_f\sin\theta\_r - \cos\theta\_r\sin\theta\_f\right)}\end{array} \tag{14}$$

The collision can be detected by the intersection area between the map and the OBB of vehicles. If the vehicle is just within the feasible area of the map, it can be defined as no collision. We constructed the collision detection formula of the vehicle according to the *Polygonmap* and *Polygoncar*, as shown in Formula (15).

$$\text{Collision} = \begin{cases} 1, \text{ } \mathcal{Q} < \text{Polyggon} \text{map} \cap \text{Polyggon}\_{\text{carr}} < \text{Polyggon}\_{\text{carr}}\\ 1, \quad \text{Polyggon}\_{\text{map}} \cap \text{Polyggon}\_{\text{carr}} = \mathcal{Q} \\ 0, \text{ } \text{Polyggon}\_{\text{map}} \cap \text{Polyggon}\_{\text{carr}} = \text{Polyggon}\_{\text{carr}} \end{cases} \tag{15}$$

#### **4. Improved RRT\* Algorithm for Intelligent Vehicles**

The RRT\* algorithm has great advantages in search efficiency and search quality and has been successfully applied in unmanned vehicle driving, UAV navigation, etc.

For underground mines, the application of the RRT\* algorithm must consider the following aspects:


Aiming to adopt the use of intelligent vehicles in underground mines, this paper makes the following improvements to the RRT\* algorithm:

(1) Dynamic step size

The classic RTT\* algorithm adopts a fixed step size expansion strategy. When the step size is small, the convergence speed is slow. When the step size is large, the vehicle easily collides with the drift wall, causing sampling failure and indirectly affecting the solution speed. The strategy of a fixed step size is: first, we randomly sampled *xrand* in the map; secondly, we obtained its neighbor *xnear*; then, we connected *xrand* and *xnear*, and took the

length of the StepSize from *xnear* to obtain the point *xnew*; if the collision detection was valid, an expansion was completed. Collision detection with a fixed step has a higher probability of failure. To solve this problem, a dynamic step size strategy was designed, and the step size was taken as a dynamic random function of CollisionSize (the distance from *xnear* to the collision point). When far from the obstacle, a larger step size was taken to ensure the speed of convergence; when the obstacle was closer, a smaller step size was taken to ensure the effectiveness of collision detection, as shown in Formula (16) and Figure 8.

**Figure 8.** Comparison of fixed step size and dynamic step size. (**a**) Fixed step size; (**b**) dynamic step size.

#### (2) Steering angle constraints

The steering process of vehicles is strictly constrained by the max steering angle *β*. Therefore, during the sampling process of the RRT\* algorithm, the angle *θ* between the new path and the parent path should be less than *β*, as shown in Formula (17) and Figure 9.

$$\theta = \left| \dot{\gamma}\_f \right| = \arccos \left( \frac{\overbrace{\left| \overbrace{\left| \overbrace{\left| \overbrace{\left| \overbrace{\left| \overleftlangle \overleftlangle \overleftlangle \over \\ \right| \overleft< marc \right]}^{\times} \right|}^{\times \times \times \times \times \times} \right|}^{\times \times \times \times \times} \right|}{\left| \left| \overbrace{\left| \overbrace{\left| \over \\ \right| \overleft. marc \right>}^{\times \times \times \times \times} \right|}^{\times \times \times \times \times} \right|} \right| \right) \leq \beta \tag{17}$$

**Figure 9.** Geometric relation of steering angle constraints.

(3) Optimal tree reconnection

The classic RRT\* algorithm uses random sampling, so the obtained path usually has certain turns, which lead to the deceleration of vehicles. Therefore, unnecessary turns should be avoided to lead the vehicles to drive straight. This will reduce the control difficulty of unmanned driving while reducing the path distance. The optimal tree reconnection process is as follows: we straightened and optimized the feasible path when the RRT\* algorithm found a solution; we continuously traversed from the root node to the child

node; we searched for the child nodes that were directly connected without obstacles; we connected the two nodes and deleted the intermediate nodes. This process turned the path into a curve by reducing the number of nodes, as shown in Figures 10 and 11.

**Figure 10.** Process of optimal tree reconnection.

**Figure 11.** Geometric relation of optimal tree reconnection.

The pseudo code of the improved Algorithm 1 is as follows:

