一种视觉二维码和激光相结合的充电桩定位方法及系统与流程

本发明涉及无人驾驶智能清洗，具体而言，涉及一种视觉二维码和激光相结合的充电桩定位方法及系统。

背景技术：

1、随着人力成本的上涨以及人们对科技智能化的需求提高，无人驾驶清扫车已经越来越多地应用到商用场景中，例如园区、公园、商场、智能工厂等多种室内外场景，大大节省了对于清洁劳务的开支。

2、目前，市场上已有的无人驾驶清扫车，通过在充电站上打印二维码，依靠相机进行识别和定位，从而提供无人驾驶清扫车相对于充电桩的位置。该方案适用范围小，只有在光线较好并且无人驾驶清扫车距离工作站比较近的时候才可以提供较高的定位精度。而且定位误差随距离增大而增大，例如，在充电桩大小一定的情况下，距离越远，其在相机成像中所占的像素越小，从而导致定位误差随着距离增大而增大，不能保证定位精度在使用范围内的一致性。此外，通过视觉二维码定位的方案，由于角度测量精度不足，导致在充电加水过程中，无人驾驶车辆不能做大角度偏转。

3、因此，本发明提供一种视觉二维码和激光相结合的充电桩定位方法及系统，解决现有定位方案中适用范围小、定位误差随距离增大而增大、以及角度精度不足的问题。

技术实现思路

1、本发明提供一种视觉二维码和激光相结合的充电桩定位方法及系统，用以克服现有技术中存在的至少一个技术问题。

2、一方面，本发明实施例提供一种视觉二维码和激光相结合的充电桩定位方法，包括：

3、在充电桩上粘贴二维码；

4、采集所述充电桩图像，利用所述充电桩图像对相机进行参数预标定，得到充电桩在相机坐标系下的坐标；

5、根据所述充电桩的3d模型生成所述充电桩的3d点云；

6、根据所述充电桩在相机坐标系下的坐标，获取所述充电桩在所述激光坐标系下的坐标；

7、采集激光点云；

8、基于所述充电桩在所述激光坐标系下的坐标，对所述激光点云进行预处理，得到感兴趣点云；

9、获取所述感兴趣点云在所述充电桩坐标系下的坐标；

10、对所述充电桩的3d点云、所述感兴趣点云在所述充电桩坐标系下的坐标，进行点到面的约束，得到优化后的车在充电桩坐标系下的角度修正量和位置修正量。

11、可选地，利用所述充电桩图像对相机进行参数预标定，得到充电桩坐标系与相机坐标系之间的转换关系，具体为：

12、识别所述充电桩图像中二维码的边缘得到所述二维码的四个顶点在所述相机坐标系下的坐标；

13、根据所述二维码的尺寸得到所述二维码的四个顶点在所述二维码坐标系下的坐标；

14、根据所述二维码的四个顶点在所述相机坐标系下的坐标和所述二维码的四个顶点在所述二维码坐标系下的坐标，得到所述二维码坐标系与所述相机坐标系之间的转换关系；

15、根据所述二维码坐标系与所述相机坐标系之间的转换关系、以及所述二维码坐标系与充电桩坐标系之间的转换关系，得到所述相机坐标系与所述充电桩坐标系之间的转换关系；

16、根据所述充电桩在所述充电桩坐标系下的坐标、以及所述相机坐标系与所述充电桩坐标系之间的转换关系，得到所述充电桩在所述相机坐标系下的坐标。

17、可选地，根据所述充电桩的设计模型生成所述充电桩的3d点云，具体为：

18、提取所述充电桩的3d模型的表面网格；

19、将所述表面网格生成表面点云，对所述表面点云均匀化，得到所述充电桩的3d点云。

20、可选地，所述充电桩在所述激光坐标系下的坐标为：其中，plidar表示充电桩在激光坐标系下的坐标，表示充电桩在激光坐标系下的角度修正量，pstation表示相机角度下充电桩在激光坐标系下的坐标，表示充电桩在激光坐标系下的位置修正量。

21、可选地，基于所述充电桩在所述激光坐标系下的坐标，对所述激光点云进行预处理，得到感兴趣点云，具体为：

22、基于所述激光点云，以所述充电桩在所述激光坐标系下的坐标为圆心，将距离所述圆心预定距离之外的点云去除，得到所述感兴趣点云。

23、可选地，所述预定距离为1.2米。

24、可选地，获取所述感兴趣点云在所述充电桩坐标系下的坐标，具体为：

25、根据所述充电桩坐标系与相机坐标系之间的转换关系、以及所述相机坐标系与激光坐标系之间的转换关系，得到所述充电桩坐标系与所述激光坐标系之间的转换关系；

26、根据所述感兴趣点云，以及所述充电桩坐标系与所述激光坐标系之间的转换关系，得到所述感兴趣点云在所述充电桩坐标系下的坐标。

27、可选地，所述感兴趣点云在所述充电桩坐标系下的坐标为：其中，station0充电桩坐标系，pstation0表示车在充电桩坐标系下的坐标，表示车在充电桩坐标系下的角度修正量，pcar表示相机角度下车在充电桩坐标系下的坐标，表示车在充电桩坐标系下的位置修正量。

28、可选地，对所述充电桩的3d点云、所述感兴趣点云在所述充电桩坐标系下的坐标，进行点到面的约束，具体为：

29、对所述充电桩的3d点云、所述感兴趣点云在所述充电桩坐标系下的坐标进行数据采样，得到第一点集和第二点集；

30、根据所述第一点集和所述第二点集，确定初始对应点集p和q；

31、将所述初始对应点集p和q中错误的点集去除，得到优化后的车在充电桩坐标系下的角度修正量和位置修正量。

32、另一方面，本发明还提供一种视觉二维码和激光相结合的充电桩定位系统，包括：

33、设置模块，用于在充电桩上粘贴二维码；

34、标定模块，用于采集所述充电桩图像，利用所述充电桩图像对相机进行参数预标定，得到充电桩在相机坐标系下的坐标；

35、生成模块，用于根据所述充电桩的3d模型生成所述充电桩的3d点云；

36、第一获取模块，用于根据所述充电桩在相机坐标系下的坐标，获取所述充电桩在所述激光坐标系下的坐标；

37、采集模块，用于采集激光点云；

38、预处理模块，用于基于所述充电桩在所述激光坐标系下的坐标，对所述激光点云进行预处理，得到感兴趣点云；

39、第二获取模块，用于获取所述感兴趣点云在所述充电桩坐标系下的坐标；

40、求解模块，用于对所述充电桩的3d点云、所述感兴趣点云在所述充电桩坐标系下的坐标，进行点到面的约束，得到优化后的车在充电桩坐标系下的角度修正量和位置修正量。

41、本发明实施例的创新点包括：

42、1、本实施例中，通过将感兴趣点云与充电桩的3d点云进行匹配，即可得到优化后的位置和角度的修正量，基于优化后的位置和角度的修正量，对车辆的位姿进行修正，解决了仅依靠相机定位时存在的定位误差随距离增大而增大、以及角度测量精度不足的问题，可以有效提高充电桩的定位精度，是本发明实施例的创新点之一。

43、2、本实施例中，通过在视觉定位的基础上加入激光点云定位，可以有效解决视觉定位在光照较暗时定位精度较低的问题，从而能够增加适用范围，拓宽使用场景，是本发明实施例的创新点之一。