## 4 Proposed Method
### Lidar and Vision Data Fusion Method for Autonomous Docking  
In recent research, Lidar sensors and cameras have commonly been used together in autonomous driving applications, because a Lidar sensor can collect 3D spatial information. In contrast, a low-cost camera captures the appearance and texture of the corresponding area in 2D images. Therefore, the fusion of Lidar and the camera data can improve the object detection performance. Lidar-camera calibration estimates a transformation matrix that gives the relative rotation and translation between the 2D coordinates obtained from the Hikvision camera and the 3D spatial coordinates obtained from the Lidar, as demonstrated in Equation (1) [29]. The 3D coordinates of the charging station can be calculated using Equations (2)-(4) [29] based on the predicted bounding box in the image domain:  
\[z_{c} \begin{bmatrix}u\\ v\\ 1\end{bmatrix} = \begin{bmatrix}\frac{f_{x}}{dx}&0&u_{0}\\ 0&\frac{f_{y}}{dy}&v_{0}\\ 0&0&1\end{bmatrix}\begin{bmatrix}1&0&0&0\\ 0&1&0&0\\ 0&0&1&0\end{bmatrix}\begin{bmatrix}X\\ Y\\ z_{c}\\ 1\end{bmatrix}  \tag{1}\]  
\[X=\frac{u-u_{0}\cdot z_{c}\cdot dx}{f_{x}} \tag{2}\]  
\[Y=\frac{v-v_{0}\cdot z_{c}\cdot dy}{f_{y}} \tag{3}\]  
\[Z=z_{c} \tag{4}\]  
where \(u\) and \(v\) are the 2D coordinates from the camera; \(u_{0}\) and \(v_{0}\) are the origins of the coordinate system in the image domain; \(f_{x}\) and \(f_{y}\) are the focal lengths along the \(x\) and \(y\) directions, respectively; \(X\), \(Y\), and \(Z\) are the 3D global coordinates from the Lidar; and \(z_{c}\) is the distance between the detected object and the camera. An illustration of the transformation process is presented in Figure 5.  
Figure 5: Illustration of the transformation process. An Ouster Lidar sensor is utilized to calculate the distances from the robot frame of reference to the side wall and the depth or the distance to the charger. It is assumed that the charging station is enclosed within walls to simplify the pose estimation of the robot for the docking process. Two scenarios are considered for the implementation of the Lidar-vision docking method: docking in an environment with only one charger and in one with three chargers, as shown by the Gazebo virtual environment setups in Figure 6.  
In the case of the scenario with three different chargers, the vision-based method will aid the robot in identifying the correct charger and autonomously docking with it. Rviz software is used to visualize the Lidar point cloud data of the charging stations for both the one-charger and three-charger setups, as demonstrated in Figure 6. The pose estimation and navigation for docking are used with the Lidar sensor data based on the information given in Figure 7. After the correct charging station is identified using computer vision algorithms, the Lidar point cloud data are filtered to obtain two diagonal and two straight lines, called, respectively, Front_laser, Back_laser, Wall_laser, and Charger_laser. Based on this information, a series of rotations and linear motions can be applied to the robot to move it to the desired location in front of the charger. The pseudo-code Algorithm 1 used to carry out the Lidar-based docking procedure is described as follows:  
Figure 6: Docking station Gazebo virtual environment setup with one charger (**top left**) and three chargers (**top right**), and Rviz Lidar point cloud visualization for one charger (**bottom left**) and three chargers (**bottom right**). The above algorithm was tested on the Husky robot and gave a fairly accurate pose estimation and localization for docking, except for systematic errors based on the Lidar data readings.