# A Method of Dual-AGV-Ganged Path Planning Based on the Genetic Algorithm
## 3 Proposed Dual-AGV-Ganged Path-Planning Method  
### Introducing the Starting-Point and Goal-Point Pose for Path Initialization  
The initialization process plays a crucial role in the convergence speed and final outcomes of the algorithm. This paper proposes dual-AGV-ganged safety-steering rules  
Figure 1: Two-dimensional grid map. Figure 2: Parameters of the dual-AGV-ganged system. to guide the direction of path initialization. Additionally, the poses of the starting and goal points were incorporated to more comprehensively consider the possible steering collisions of dual AGVs. First, the angle of the starting point was calculated. Using the dual-AGV-ganged system's safe turning radius, the vector angles between the starting point and pre-selected path points, as well as the turning cost of the dual-AGV system, were computed to select the suitable path points for generating a path that considers the starting-point's pose. Next, based on this initial path, the angle of the goal point was calculated, and the vector angles and turning costs between the pre-selected path points and the goal point were determined in reverse. This ensures the selection of appropriate path points to generate a path that accounts for both the starting and goal points' poses, thereby completing the population initialization. The specific steps are detailed below. Part I: Initialization of the Path Starting-Point Poses  
(1) Obtain the angle of the starting-point: By subscribing to the ROS topic published by AMCL, the pose information of the leader AGV's starting point is obtained, including the orientation represented by the quaternion (\(\mathbf{q}=[w,x,y,z]\)). In this context, a quaternion is a four-dimensional vector used to represent 3D rotations. The component www is related to the rotation angle, while x, y, and z represent the components of the rotation axis along the x, y, and z directions, respectively [32]. The quaternion is converted to Euler angles using Equations (3)-(5). The Euler angle along the z-axis of the AGV is mapped from \((-\pi,\pi)\) to \((0,2\pi)\), and, after a 180\({}^{\circ}\) clockwise rotation, the starting-point angle \(\theta_{\text{start}}\) is obtained. Finally, the starting point is set as the current node of the leader AGV, and \(\theta_{\text{start}}\) is set as the current angle of the follower AGV:  
\[\text{Roll}(\phi)=\text{atan2}(2(wx+yz),1-2(x^{2}+y^{2})), \tag{3}\]  
\[\text{Pitch}(\theta)=\text{asin}(2(wy-zx)), \tag{4}\]  
\[\text{Yaw}(\psi)=\text{atan2}(2(wz+xy),1-2(y^{2}+z^{2})), \tag{5}\]  
where \(\text{Roll}(\phi)\) is the roll angle, \(\text{Pitch}(\theta)\) is the pitch angle, \(\text{Yaw}(\psi)\) is the yaw angle, \(\text{atan2}\) is the arctangent function, and \(\text{asin}\) is the arcsine function. (2) Conduct a search of the surrounding free nodes and sort: The free nodes around the current node of the leader AGV are identified, and the coordinates of the goal point with the current point are compared to determine the direction of the goal point relative to the current node. Next, the free nodes are sorted and then prioritized by selecting those located in or near this direction as candidate path points, setting the current node as \(P_{A}\) and the candidate path points as \(P_{B}\);  
(3) Calculate the angle forming a vector: By subtracting the x and y coordinates of \(P_{A}\) from the x and y coordinates of \(P_{B}\), respectively, \(dx\) and \(dy\) are obtained. Then, using Equation (6), the angle of vector is \(AB\) formed by \(P_{A}\) and \(P_{B}\) is calculated. This angle is then rotated 180\({}^{\circ}\) clockwise to set the target angle for the follower AGV:  
\[\theta_{\text{point}}= \begin{cases}0&\text{if }dx=0\text{ and }dy>0\\ \pi&\text{if }dx=0\text{ and }dy<0\\ \frac{\pi}{2}&\text{if }dx>0\text{ and }dy=0\\ \frac{3\pi}{2}&\text{if }dx<0\text{ and }dy=0\\ \text{atan2}(dy,dx)&\text{if }dx\
eq 0\text{ and }dy>0\\ \text{atan2}(dy,dx)+2\pi&\text{if }dx\
eq 0\text{ and }dy<0\\ \end{cases} , \tag{6}\]  
where \(\theta_{\text{point}}\) is the angle of the vector \(AB\).