In order to meet the battery replacement demand of electric vehicles, a 6D pose estimation method of battery package locking mechanism based on point cloud segmentation is proposed to solve the positioning problem of locking mechanism during battery package docking in battery swapping station. This method uses YOLOv5 network to segment the point cloud of locking mechanism from the scene, and uses voxel filtering and moving least square fitting to filter and smooth the point cloud. The point cloud labels are predicted by the point cloud segmentation network, and the global semantic feature is added to the Fast Point Feature Histograms (FPFH) feature to make up for the defect that the FPFH has only the local feature of the point cloud. According to this feature, the Random Sample Consensus (RANSAC) rigid point cloud registration is leveraged, and the 6D pose of the locking mechanism point cloud is estimated. Finally, the Iterative Closest Point(ICP) algorithm is used to correct the pose estimation results. The experimental results show that the 6D pose estimation algorithm of locking mechanism based on point cloud segmentation has high accuracy, and can overcome the mismatching caused by environmental noise, and accurately obtain the position and attitude of locking mechanism. The angle error of position and attitude estimation can reach 1.90°, the displacement error can reach 1.4 mm, and the RMSE can reach 1.5 mm, which provides an effective solution for battery docking positioning in battery swapping station.
ZHOU R, LIU Y F, QI N M, et al. Overview of visual pose estimation methods for space missions[J]. Optics and Precision Engineering, 2022, 30(20): 2538-2553.(in Chinese). doi: 10.37188/OPE.20223020.2538http://dx.doi.org/10.37188/OPE.20223020.2538
YANG N, SHEN J SH, ZHANG J D, et al. Autonomous measurement of relative attitude and positionfor spatial non-cooperative spacecraft based on stereo vision[J]. Optics and Precision Engineering, 2017, 25(5):1331-1339. (in Chinese). doi: 10.3788/ope.20172505.1331http://dx.doi.org/10.3788/ope.20172505.1331
LI Y, YE P J, PENG J, et al. Artificial target recognition and location based on Mars exploration[J]. Optics and Precision Engineering, 2015, 23(2):566-572.(in Chinese). doi: 10.3788/ope.20152302.0566http://dx.doi.org/10.3788/ope.20152302.0566
DROST B, ULRICH M, NAVAB N, et al. Model globally, match locally: efficient and robust 3D object recognition[C].2010 IEEE Computer Society Conference on Computer Vision and Pattern Recognition. 13-18,2010, San Francisco, CA, USA. IEEE, 2010: 998-1005. doi: 10.1109/cvpr.2010.5540108http://dx.doi.org/10.1109/cvpr.2010.5540108
HINTERSTOISSER S, LEPETIT V, ILIC S, et al. Model Based Training, Detection and Pose Estimation of Texture-less 3D Objects in Heavily Cluttered Scenes[M].Computer Vision - ACCV 2012. Berlin, Heidelberg: Springer Berlin Heidelberg, 2013: 548-562. doi: 10.1007/978-3-642-37331-2_42http://dx.doi.org/10.1007/978-3-642-37331-2_42
OGUN P S, USMAN Z, DHARMARAJ K, et al. 3D vision assisted flexible robotic assembly of machine components[C].Proc SPIE 9875, Eighth International Conference on Machine Vision (ICMV 2015), 2015, 9875: 342-348.
BOCHKOVSKIY A, WANG C Y, LIAO H. YOLOv4: optimal speed and accuracy of object detection[C]. IEEE Conference on Computer Vision and Pattern Recognition(CVPR), 2020.
WU D, LIAO M W, ZHANG W T, et al. YOLOP: You only look once for panoptic driving perception[J].Machine Intelligence Research, 2022, 19(6): 550-562. doi: 10.1007/s11633-022-1339-yhttp://dx.doi.org/10.1007/s11633-022-1339-y
艾卫涛. 三维激光扫描点云去噪及在建模中的应用[D]. 西安: 西安科技大学, 2017.
AI W T. Denoising of 3D Laser Scanning Point Cloud and Its Application In Modeling[D]. Xi'an: Xi'an University of Science and Technology, 2017. (in Chinese)
ZENG Q H, LU D. Curve and surface fitting based on moving least-squares methods[J]. Journal of Engineering Graphics, 2004(1): 84-89.(in Chinese). doi: 10.3969/j.issn.1003-0158.2004.01.017http://dx.doi.org/10.3969/j.issn.1003-0158.2004.01.017
ZHOU Q Y, PARK J, KOLTUN V. Fast Global Registration[M].Computer Vision - ECCV 2016. Cham: Springer International Publishing, 2016: 766-782. doi: 10.1007/978-3-319-46475-6_47http://dx.doi.org/10.1007/978-3-319-46475-6_47
RUSU R B, BLODOW N, MARTON Z C, et al. Aligning point cloud views using persistent feature histograms[C].2008 IEEE/RSJ International Conference on Intelligent Robots and Systems. 22-26,2008, Nice, France. IEEE, 2008: 3384-3391. doi: 10.1109/iros.2008.4650967http://dx.doi.org/10.1109/iros.2008.4650967
RUSU R B, BLODOW N, BEETZ M. Fast point feature histograms (FPFH) for 3D registration[C].2009 IEEE International Conference on Robotics and Automation. 12-17,2009, Kobe, Japan. IEEE, 2009: 3212-3217. doi: 10.1109/robot.2009.5152473http://dx.doi.org/10.1109/robot.2009.5152473
QI C R , YI L, SU H, et al. PointNet++: Deep Hierarchical Feature Learning on Point Sets in A Metric Space[EB/OL]. 2017: arXiv: 1706.02413. https://arxiv.org/abs/1706.02413.pdfhttps://arxiv.org/abs/1706.02413.pdf.
CHARLES R Q, HAO S, MO K C, et al. PointNet: deep learning on point sets for 3D classification and segmentation[C].2017 IEEE Conference on Computer Vision and Pattern Recognition (CVPR).21-26,2017, Honolulu, HI, USA. IEEE, 2017: 77-85. doi: 10.1109/cvpr.2017.16http://dx.doi.org/10.1109/cvpr.2017.16
FISCHLER M A, BOLLES R C. Random Sample Consensus: A Paradigm for Model Fitting With Applications to Image Analysis and Automated Cartography[M].Readings in Computer Vision. Amsterdam: Elsevier, 1987: 726-740. doi: 10.1016/b978-0-08-051581-6.50070-2http://dx.doi.org/10.1016/b978-0-08-051581-6.50070-2
HUBER P J. Robust estimation of a location parameter[J]. The Annals of Mathematical Statistics, 1964, 35(1): 73-101. doi: 10.1214/aoms/1177703732http://dx.doi.org/10.1214/aoms/1177703732
BESL P J, MCKAY N D. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992, 14(2): 239-256. doi: 10.1109/34.121791http://dx.doi.org/10.1109/34.121791