一种充电口定位方法及充电口对准方法与流程

本发明涉及自动充电和机器视觉，尤其涉及一种充电口定位方法及充电口对准方法。

背景技术：

1、在自动充电技术领域，利用充电机器人对电动车辆的充电口进行充电。在充电之前需要对充电口进行识别和定位，并使得充电机器人的机械臂对准充电口。

2、传统技术中，需要在充电口位置添加标记点，以帮助视觉系统完成充电口识别和定位，引导机械臂携带充电插头对准充电口。然而，这种技术需要对车辆充电口位置进行局部改造，否则视觉系统无法正常识别。为了能够适用所有车辆，现有技术也有通过识别充电口的圆形特征来识别车辆充电口位置，实现了无需借助标记点即可自动识别出充电口位置的目的。然而，这种技术需要借助外部光源，也很容易被外界光线干扰，导致识别出的圆形特征容易出现偏差，最终导致识别出的充电口位置误差较大，且识别具有偶然性，即有时能够识别出有时无法识别出，从而导致多次识别的一致性差。

技术实现思路

1、本发明的一个目的在于避免充电口识别过程被外界光线干扰而导致的识别出的充电口位置误差较大且识别一致性差。

2、本发明的一个进一步的目的在于精准定位出充电口的空间位置。

3、特别地，本发明提供了一种充电口定位方法，包括如下步骤：

4、采集包含有充电口的第一图像；

5、从所述第一图像中提取出用于定位所述充电口的特征圆，基于所述特征圆形成以所述特征圆为内圆的基准圆环，并以所述基准圆环建立与所述基准圆环同心的目标圆环，所述目标圆环的区域不超出所述基准圆环；

6、根据所述目标圆环内至少部分点的三维坐标进行平面拟合，得到所述目标圆环的平面方程；

7、根据所述平面方程和所述特征圆的圆心的三维坐标确定出所述充电口的初步空间位置。

8、可选地，所述根据所述平面方程和所述特征圆的圆心的三维坐标确定出所述充电口的初步空间位置之后，还包括如下步骤：

9、获取模板点云和所述充电口的实际点云；

10、使得所述模板点云根据所述平面方程和所述特征圆的圆心的三维坐标进行运动；

11、将运动结束之后的所述模板点云与所述实际点云进行匹配，得到优化后圆心的三维坐标和优化后平面方程；

12、根据所述优化后圆心的三维坐标和优化后平面方程确定出所述充电口的最终空间位置。

13、可选地，所述使得所述模板点云根据所述平面方程和所述特征圆的圆心的三维坐标进行运动，包括如下步骤：

14、根据所述平面方程得到所述充电口所在平面的实际法向量；

15、将所述实际法向量转换为预设的目标法向量，得到旋转变换关系，所述目标法向量为充电插头垂直插入所述充电口时所述充电口所在平面的法向量；

16、使得所述模板点云根据所述圆心的三维坐标和所述旋转变换关系进行运动。

17、可选地，所述将运动结束之后的所述模板点云与所述实际点云进行匹配，得到优化后圆心的三维坐标和优化后平面方程，所述优化后平面方程按照如下方法获得：

18、将运动结束之后的所述模板点云与所述实际点云进行匹配，得到优化后圆心的三维坐标和优化后旋转变换关系；

19、根据所述目标法向量和所述优化后旋转变换关系，得到所述优化后平面方程。

20、可选地，所述将所述实际法向量转换为预设的目标法向量，得到旋转变换关系的步骤中，利用如下公式得到所述旋转变换关系：

21、

22、

23、

24、

25、其中，表示所述实际法向量，表示所述目标法向量，rv表示所述旋转变换关系。

26、可选地，所述根据所述目标圆环内至少部分点的三维坐标进行平面拟合，得到所述目标圆环的平面方程的步骤中，利用像素坐标和相机坐标之间的关系式计算获得所述目标圆环上的点在相机坐标系下的三维坐标，利用ransac算法进行平面拟合。

27、可选地，所述从所述第一图像中提取出用于定位所述充电口的特征圆，基于所述特征圆形成以所述特征圆为内圆的基准圆环，并以所述基准圆环建立与所述基准圆环同心的目标圆环的步骤中，从所述第一图像中提取出位于同一平面的至少两个特征圆，所述至少两个特征圆形成至少两个基准圆环，并对应建立至少两个目标圆环；

28、所述根据所述目标圆环内至少部分点的三维坐标进行平面拟合，得到所述目标圆环的平面方程的步骤中，每次分别从至少两个所述目标圆环中抽取至少一个点，并根据抽取的点的三维坐标进行平面拟合。

29、可选地，所述基准圆环的内圆的半径为r，所述基准圆环的外圆的半径为2r；

30、所述目标圆环的内圆的半径为，所述基准圆环的外圆半径为1.1r-1.3r，所述基准圆环的外圆的半径为1.7r-1.9r。

31、特别地，本发明提供了一种充电机器人控制方法，包括如下步骤：

32、获取由前述的充电口定位方法得到的所述充电口的最终空间位置；

33、根据所述最终空间位置调整充电机器人的充电插头的位置和姿态，以使所述充电插头对准所述充电口。

34、可选地，所述根据所述最终空间位置调整充电机器人的充电插头的位置和姿态，以使所述充电插头对准所述充电口的步骤中，控制所述充电机器人的机械臂根据所述圆心三维坐标进行平移，并根据由优化后平面方程确定出的旋转变换关系调整充电插头的姿态，从而使得所述充电插头对准所述充电口。

35、可选地，所述充电机器人包括机器人本体和设置在所述机器人本体上且相对所述机器人本体可移动的深度相机，所述机器人本体的末端具有所述充电插头，所述充电机器人控制方法还包括如下步骤：

36、获取所述深度相机反馈的在所述深度相机视野范围内是否检测到充电口的检测结果；

37、在所述检测结果为未检测到充电口时，控制所述深度相机相对所述机器人本体转动，直至所述充电口检测结果为检测到充电口。

38、可选地，所述充电机器人控制方法还包括如下步骤：

39、在所述检测结果为检测到充电口时，获取包含有充电口的第二图像和对应的深度图；

40、根据所述第二图像和对应的深度图计算得到所述充电口与所述深度相机之间的当前距离；

41、在所述当前距离为预设距离范围内时，执行所述充电口定位方法中各步骤，否则控制所述深度相机朝向靠近或远离所述充电口的方向移动，以调整与所述充电口之间的距离。

42、可选地，所述获取所述深度相机反馈的在所述深度相机视野范围内是否检测到充电口的检测结果的步骤中，所述深度相机是基于深度学习目标检测模型进行充电口检测结果的反馈的，所述深度学习目标检测模型为训练后的yolov5s模型，所述yolov5s模型的训练样本为在不同场景下的充电口图片

43、根据本发明的方案，通过形成以特征圆为内圆的基准圆环，并以基准圆环建立与基准圆环同心的目标圆环，再根据目标圆环内部分点的三维坐标进行平面拟合，得到平面方程，最后根据平面方程和特征圆的圆心坐标来确定出充电口的初步空间位置。由于拟合时是以目标圆环内的点的三维坐标进行拟合的，相比于现有技术中仅以特征圆或其轮廓拟合或计算出的结果更加精准，且不会受到外界光线或其他因素干扰，从而确定出的初步空间位置误差较小，且一次就可以识别定位出较为精准的初步空间位置，多次识别的一致性非常好。并且，本发明方案也无需借助标记点或外界光照就可以实现自动识别。

44、进一步地，通过将模板点云与实际点云进行匹配，得到优化后圆心的三维坐标和优化后平面方程，再根据优化后的数据确定出充电口的最终空间位置，由此可以获得非常精准的充电口位置。

45、根据下文结合附图对本发明具体实施例的详细描述，本领域技术人员将会更加明了本发明的上述以及其他目的、优点和特征。