一种充电设备以及控制机械臂充电的方法

1.本技术涉及车辆技术领域，尤其涉及一种充电设备以及控制机械臂充电的方法。


背景技术：

2.随着新能源汽车的发展和规模化的使用，移动充电设备因其具备灵活的充电资源、并且不受环境、车位条件的制约而越来越受到用户的青睐。移动充电设备具体表现为充电桩是可移动的形式，或者可移动底盘搭载电池包形式，比如当移动充电设备是一种移动充电车时，该移动充电车的底盘上可搭载多个电池包，在移动充电车接收到用户的请求后，驶向用户所在地，并为用户驾驶的车辆提供充电服务，完成充电任务。
3.在移动充电设备为待充电车辆充电之前，需要移动到待充电车辆附近，以定位待充电车辆上的定位口区域，并调整自身的位姿(所处的位置以及充电臂的位姿)进而向充电口充电。
4.目前主流的充电机器人方案均采用结构光或双目或超声结合机械臂规划方式通过位置控制实现插拔。首先通过结构光相机在其感知范围内对充电口拍照识别，获取充电座相对于相机的位姿，再将位姿坐标转换为机械臂末端充电插口的位姿，即可获取机械臂的控制指令，进行机械臂运动规划完成插拔。
5.由于为了实现充电插口向充电口的准确插入，要求具备亚毫米级(例如小于等于2
°
)的位姿定位精度，才可保证充电插口向充电口的顺利插入，且对接不损坏连接器件，因此基于视觉反馈的方案就要求视觉检测模块就要达到位置识别亚毫米级，位姿识别2
°
的要求，然而当前结构光技术基本精度可以达到1～2mm，位姿3
°
左右，还会受到距离视场角的限制，本身就很难保证顺利插拔，即使插入，由于位姿误差的存在也会导致较大的插拔力，容易破坏设备，甚至损坏机械臂。因此，仅基于视觉反馈技术进行插拔很难达到充电插口向充电口的顺利插入的精度要求。


技术实现要素：

6.本技术提供了一种机械臂、充电设备以及控制机械臂充电的方法，可以提高插拔任务的准确性以及可靠性。
7.第一方面，本技术提供了一种机械臂，包括：控制结构以及机械臂主体；所述机械臂主体上设置有力传感器以及充电插头，所述充电插头包括多个充电头，所述力传感器用于采集所述充电插头和所述充电口之间接触面的作用力信息，所述接触面包括所述多个充电头中至少一个充电头的侧面，所述侧面为和所述充电头的轴向方向平行的表面；所述控制结构用于根据所述作用力信息，控制所述机械臂主体的位姿，以便将所述充电插头插入至所述充电口。
8.其中，充电头可以为圆柱状的结构，可以包括底面和侧面，底面可以为充电时需要和充电孔底部相接触的面，侧面可以为充电时需要和充电孔侧壁相接触的面。
9.在一种可能的实现中，所述作用力信息包括所述充电头的径向方向的力。
10.在现有的只基于视觉进行位姿控制的方案中，由于视觉本身的误差，而且在充电插头在充电口附近的时候视觉相机已无法继续观测到充电口，如果只采用在最佳观测点观测到的视觉位姿进行充电插头的插拔，可能会发生不能插到底、楔牢的情况，本技术通过获取到力传感器采集的作用力信息来进行机械臂的位姿控制，可在充电插头已部分插入(例如侧面和充电口接触)，视觉信息无法获取误差的情况下，根据力觉调整位置和位姿，让充电插头适应充电口的插入方向，从而提高了插拔任务的准确性以及可靠性。减少了由于充电头侧面和充电口的内壁挤压或者摩擦时对充电头的磨损(或者是卡住而无法达到底部)的情况。
11.在一种可能的实现中，所述作用力信息包括所述充电口对所述充电插头的力矩信息。
12.其中，作用力信息可以指示充电口对所述充电插头的6维力，6维力可以包括在三维空间内(例如沿着x轴、y轴、z轴方向)的力以及在三维空间内(例如绕着x轴、y轴、z轴旋转)的力矩。由于其中包括了力矩信息，则可以更精准更细粒度的识别出充电口对于充电头的阻碍方向，进而提供更精准的力控来将充电头准确插入充电孔。
13.在一种可能的实现中，所述力传感器固定于所述至少一个充电头的侧面；或者，所述机械臂包括具备旋转自由度的关节，所述控制结构用于通过控制所述关节的旋转来控制所述机械臂主体的位姿，所述力传感器固定于所述关节上；或者，所述力传感器固定在所述充电插头和所述机械臂主体之间。
14.在一种可能的实现中，所述充电口包括多个充电孔，所述多个充电头与所述多个充电孔之间一一匹配；所述控制结构具体用于：
15.根据所述作用力信息，确定位姿调整参数；
16.根据所述位姿调整参数，将所述机械臂主体的位姿由第一位姿矫正为第二位姿；其中，所述第一位姿和所述第二位姿不同，且所述第二位姿为每个所述充电头均和相匹配的充电孔对齐时所述机械臂主体的位姿。
17.在一种可能的实现中，基于作用力信息，可以确定出机械臂主体是否处于正确的位姿(例如第二位姿可以为正确的位姿)。例如，在作用力信息指示充电口对充电插头存在非轴向方向的压力(或者是存在力矩)时，可以确定机械臂主体未处于正确的位姿，在这种情况下，所述多个充电头中的至少一个充电头未和相匹配的充电孔对齐，也就是充电头的中心轴与相匹配的充电孔的中心轴不重合或者偏差大于阈值的情况。
18.也就是说，第一位姿可以为充电头的中心轴与相匹配的充电孔的中心轴不重合或者偏差大于阈值的情况(也就是上述所述的未处于正确的位姿)。
19.本技术实施例中，在机械臂主体未处于正确的位姿时，可以基于作用力信息来调整机械臂主体的位姿，以便机械臂主体处于正确的位姿。
20.在一种可能的实现中，控制机构可以在基于机械臂主体处于第一位姿时，根据作用力信息对机械臂主体的位姿进行矫正(至第二位姿，也就是正确的位姿)，第二位姿为机械臂主体可以将充电插头以较小阻力的正确方式插入到充电口底部的位姿。
21.在一种可能的实现中，所述第一位姿为所述多个充电头的底部端面未和所述充电孔的底部接触时的位姿，所述底部端面为和所述充电头的轴向方向垂直的表面。
22.在一种可能的实现中，所述对齐包括：充电头沿相匹配的充电孔方向的投影范围
在相匹配的充电孔的范围内，且充电头的中心轴与相匹配的充电孔的中心轴重合或者偏差小于阈值。
23.在一种可能的实现中，所述控制结构还用于：在将所述机械臂主体的位姿由第一位姿矫正为第二位姿之后，控制所述机械臂主体的位姿，以便所述充电插头朝向所述充电孔底部且沿着所述充电头的中心轴的方向移动。
24.在一种可能的实现中，在所述由第一位姿矫正为第二位姿的过程中，所述充电口对所述充电插头在目标方向上的阻力存在降低的趋势，所述目标方向为所述充电头的轴向方向或径向方向。
25.应理解，在从第一位姿调整至第二位姿时，充电口对所述充电头在目标方向上的阻力可以存在降低的趋势，目标方向可以径向方向和/或轴向状态(例如可以为径向方向、轴向状态、或者径向方向以及轴向方向)。这里所谓的存在降低的趋势，可以理解为充电口对所述充电头在目标方向上的阻力可以不是一直降低，而是可以存在局部的增加，但是从趋势上，充电口对所述充电头在目标方向上的阻力是降低的。
26.在将所述机械臂主体的位姿由第一位姿矫正为第二位姿之后，充电插头已经处于和充电口的对齐位姿，在这种情况下，可以将充电插头朝向所述充电孔底部且沿着所述充电头的中心轴的方向移动直到底部，在一种可能的实现中，所述控制结构可以基于所述作用力信息指示所述充电口对所述充电插头仅存在沿所述充电插头的轴向方向的阻力，控制所述机械臂主体保持静止状态，进而完成了向充电口的插入任务。
27.在一种可能的实现中，在所述由第一位姿矫正为第二位姿的过程中，所述充电口对所述充电插头之间的力矩存在降低的趋势。
28.在一种可能的实现中，所述位姿调整参数具体用于控制所述充电插头的移动方向以及移动速率。
29.在一种可能的实现中，所述控制机构可以实时获取视觉传感器针对于所述充电口的区域所采集的图像信息；根据所述图像信息中所述充电口的位置信息，得到所述机械臂主体的实时位姿；根据所述实时位姿，控制所述机械臂主体的位姿，以便将所述机械臂主体上的充电插头引导至所述充电口周围。
30.由于视觉伺服中采集的图像信息是实时获取的，基于实时获取的图像信息可以求解得到机械臂主体的实时位姿(该实时位姿可以为机械臂主体末端和待充电车辆的充电口之间的相对位姿)，由于在控制所述机械臂主体的位姿时，可能会出现底座移动、障碍物的阻碍等情况，导致机械臂主体的位姿出现误差，本技术可以基于实时位姿来修正机械臂主体的位姿，可以提高机械臂主体的位姿识别精度，进而可以更准确的将机械臂主体上的充电插头引导至所述充电口周围。
31.在一种可能的实现中，视觉伺服是视觉结合运动进行的闭环控制方法。主要功能是实现机械臂末端充电枪对于充电口的导引运功。大致流程是将充电枪/充电座视觉识别位姿结果作为输入，采用视觉伺服反馈控制思想计算驱动误差，采用基于位姿的视觉伺服实现机械臂末端向目标位姿的精确导引。采用自适应增益方法，加快在小误差时的调整速度，实现快速的视觉伺服。此步骤中伺服结束时机械臂将处于最佳观测点，此处已获取最精准的充电口的位姿。此时机械臂将根据把充电插口导引至充电口附近，为下一步力控插入做准备。
32.在一种可能的实现中，由于最佳观测点要保证一定的稳定性，才能保证目标观测位姿的精度。然而，移动机器人基座相对充电座的位姿可能会不断地发生变化。为实现在上述变化下，保障最佳观测点相对充电座的位姿一致，采用位姿的快速视觉伺服实现此目标。在一种可能的实现中，可以采用位姿反馈的视觉伺服思想，基于李氏稳定判据设计关节速度控制律，实现机械臂末端向目标位置的精确导引。
33.可选的，在一种可能的实现中，所述机械臂主体上设置有所述视觉传感器。
34.可选的，在一种可能的实现中，视觉传感器还可以不设置在机械臂主体上而是设置在第三方视角对应的位置上。
35.在一种可能的实现中，在进行上述相对位姿的确定时，需要视觉传感器采集到针对于充电口的图像信息，并基于图像信息来确定充电口相对于视觉传感器之间的相对位姿。
36.在一种可能的实现中，所述图像信息包括2d图像以及3d点云，所述控制机构还用于：根据所述2d图像以及预设的充电口3d模型，提取所述充电口对应的第一3d位置；
37.根据所述3d点云，提取所述充电口对应的第二3d位置；将所述第一3d位置和所述第二3d位置进行融合，得到所述充电口的位姿。
38.根据2d图像，可以得到充电口中心点二维像素坐标，通过中心点的相互位置关系得到每个点的编号；根据充电座标准cad模型得到圆孔中心点在目标坐标系下的三维物理坐标，进而得到各个充电孔的三维物理坐标。
39.在一种可能的实现中，所述第一3d位置包括所述充电口上m个第一特征点的3d位置；所述第二3d位置包括所述充电口上m个第二特征点的3d位置；所述m个第一特征点和所述m个第二特征点一一对应；所述将所述第一3d位置和所述第二3d位置进行融合，包括：将所述m个第一特征点的3d位置分别和对应的所述第二特征点的3d位置进行融合。其中，第一特征点和第二特征点可以为充电孔。
40.本技术提出了一种融合2d图像-3d点云信息的充电口位姿精确识别方法，可以采用2d深度图像检测网络对充电口的二维关键点进行亚像素级的精确检测，采用三维关键点检测对充电口三维关键点进行基于3d霍夫网络的偏移量可靠检测，通过融合(例如采用非线性优化)使得融合二维关键点及三维关键点的投影误差最小，进而求得充电口的精确位姿，以此解决充电口在光照变化大、目标对比度低情况下的位姿高精度识别问题。
41.在一种可能的实现中，所述机械臂主体还包括目标接口，所述目标接口用于可拆卸连接所述充电插头。可选的，目标接口可以为基于电磁性来进行清扫工具的可拆卸连接的部件。采用磁吸式结构抓取充电枪，保证充电插头和机械臂的可靠对接，然后进行下一步充电操作。
42.在一种可能的实现中，在所述控制结构根据所述作用力信息，控制所述机械臂主体的位姿之前，所述目标接口与初始充电插头连接；
43.所述控制结构还用于控制所述机械臂主体的位姿、以及所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆卸并替换为所述充电插头；其中，
44.所述目标充电插头为支持快充充电的插头，所述充电插头为支持慢充充电的插头；或者，
45.所述目标充电插头为支持慢充充电的插头，所述充电插头为支持快充充电的插
头。
46.本技术实施例中，充电设备可以兼容快充枪和慢充枪两种充电方式，可以具备快换机构，可根据用户任务需求选取充电枪。
47.在一种可能的实现中，所述充电设备还包括：插头收纳区域，所述插头收纳区域包括用于设置所述充电插头的目标收纳位置；
48.所述控制所述机械臂主体的位姿、以及所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆卸并替换为所述充电插头，包括：
49.控制所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆卸；
50.控制所述机械臂主体的位姿，将所述目标接口移动至所述目标收纳位置，并控制所述目标接口的连接状态，将所述目标接口与所述充电插头进行连接。
51.第二方面，本技术提供了一种充电设备，包括：机械臂、力传感器以及充电插头；
52.所述机械臂包括控制结构以及机械臂主体；
53.所述机械臂主体上设置有所述力传感器以及所述充电插头，所述充电插头包括多个充电头，所述力传感器用于采集所述充电插头和所述充电口之间接触面的作用力信息，所述接触面包括所述多个充电头中至少一个充电头的侧面，所述侧面为和所述充电头的轴向方向平行的表面；
54.所述控制结构用于根据所述作用力信息，控制所述机械臂主体的位姿，以便将所述充电插头插入至所述充电口。
55.在一种可能的实现中，所述作用力信息包括所述充电口对所述充电插头的力矩信息。
56.在一种可能的实现中，所述力传感器固定于所述至少一个充电头的侧面；或者，
57.所述机械臂包括具备旋转自由度的关节，所述控制结构用于通过控制所述关节的旋转来控制所述机械臂主体的位姿，所述力传感器固定于所述关节上；或者，
58.所述力传感器固定在所述充电插头和所述机械臂主体之间。
59.在一种可能的实现中，所述充电口包括多个充电孔，所述多个充电头与所述多个充电孔之间一一匹配；所述控制结构具体用于：
60.根据所述作用力信息，确定位姿调整参数；
61.根据所述位姿调整参数，将所述机械臂主体的位姿由第一位姿矫正为第二位姿；其中，所述第一位姿和所述第二位姿不同，且所述第二位姿为每个所述充电头均和相匹配的充电孔对齐时所述机械臂主体的位姿。
62.在一种可能的实现中，所述第一位姿为所述多个充电头的底部端面未和所述充电孔的底部接触时的位姿，所述底部端面为和所述充电头的轴向方向垂直的表面。
63.在一种可能的实现中，所述对齐包括：充电头沿相匹配的充电孔方向的投影落在所述相匹配的充电孔的范围内，且充电头的中心轴与相匹配的充电孔的中心轴重合或者偏差小于阈值。
64.在一种可能的实现中，所述控制结构还用于：
65.在将所述机械臂主体的位姿由第一位姿矫正为所述第二位姿之后，控制所述机械臂主体的位姿，以便所述充电插头朝向所述充电孔底部且沿着所述充电头的中心轴的方向
移动。
66.在一种可能的实现中，在所述由第一位姿矫正为第二位姿的过程中，所述充电口对所述充电插头之间的力矩存在降低的趋势。
67.在一种可能的实现中，所述位姿调整参数具体用于控制所述充电插头的移动方向以及移动速率。
68.在一种可能的实现中，所述控制结构具体用于：
69.基于所述作用力信息指示所述充电口对所述充电插头仅存在沿所述充电插头的轴向方向的阻力，控制所述机械臂主体保持静止状态。
70.在一种可能的实现中，所述控制机构还用于：
71.根据所述作用力信息，控制所述机械臂主体的位姿之前，实时获取视觉传感器针对于所述充电口的区域所采集的图像信息；
72.根据所述图像信息中所述充电口的位置信息，得到所述机械臂主体的实时位姿；
73.根据所述实时位姿，控制所述机械臂主体的位姿，以便将所述机械臂主体上的充电插头引导至所述充电口周围。
74.在一种可能的实现中，所述机械臂主体上设置有所述视觉传感器。
75.在一种可能的实现中，所述图像信息包括2d图像以及3d点云，所述控制机构还用于：
76.根据所述2d图像以及预设的充电口3d模型，提取所述充电口对应的第一3d位置；
77.根据所述3d点云，提取所述充电口对应的第二3d位置；
78.将所述第一3d位置和所述第二3d位置进行融合，得到所述充电口的位姿。
79.在一种可能的实现中，所述第一3d位置包括所述充电口上m个第一特征点的3d位置；所述第二3d位置包括所述充电口上m个第二特征点的3d位置；所述m个第一特征点和所述m个第二特征点一一对应；
80.所述将所述第一3d位置和所述第二3d位置进行融合，包括：
81.将所述m个第一特征点的3d位置分别和对应的所述第二特征点的3d位置进行融合。
82.在一种可能的实现中，所述机械臂主体还包括目标接口，所述目标接口用于可拆卸连接所述充电插头。
83.在一种可能的实现中，在所述控制结构根据所述作用力信息，控制所述机械臂主体的位姿之前，所述目标接口与初始充电插头连接；
84.所述控制结构还用于控制所述机械臂主体的位姿、以及所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆卸并替换为所述充电插头；其中，
85.所述目标充电插头为支持快充充电的插头，所述充电插头为支持慢充充电的插头；或者，
86.所述目标充电插头为支持慢充充电的插头，所述充电插头为支持快充充电的插头。
87.在一种可能的实现中，所述充电设备还包括：插头收纳区域，所述插头收纳区域包括用于设置所述充电插头的目标收纳位置；
88.所述控制所述机械臂主体的位姿、以及所述目标接口的连接状态，将所述初始充
电插头从所述目标接口上进行拆卸并替换为所述充电插头，包括：
89.控制所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆卸；
90.控制所述机械臂主体的位姿，将所述目标接口移动至所述目标收纳位置，并控制所述目标接口的连接状态，将所述目标接口与所述充电插头进行连接。
91.在一种可能的实现中，所述充电设备为固定式充电桩、移动机器人或者充电车。
92.第三方面，本技术提供了一种控制机器臂充电的方法，所述方法应用于机械臂的控制结构，所述机械臂主体上设置有力传感器以及充电插头，所述充电插头包括多个充电头，所述力传感器用于采集所述充电插头和所述充电口之间接触面的作用力信息，所述接触面包括所述多个充电头中至少一个充电头的侧面，所述侧面为和所述充电头的轴向方向平行的表面；
93.所述方法包括：
94.根据所述作用力信息，控制所述机械臂主体的位姿，以便将所述充电插头插入至所述充电口。
95.在一种可能的实现中，所述作用力信息包括所述充电口对所述充电插头的力矩信息。
96.在一种可能的实现中，所述作用力信息包括所述充电头的径向方向的力。
97.在一种可能的实现中，所述力传感器固定于所述至少一个充电头的侧面；或者，
98.所述机械臂包括具备旋转自由度的关节，所述控制结构用于通过控制所述关节的旋转来控制所述机械臂主体的位姿，所述力传感器固定于所述关节上；或者，
99.所述力传感器固定在所述充电插头和所述机械臂主体之间。
100.在一种可能的实现中，所述充电口包括多个充电孔，所述多个充电头与所述多个充电孔之间一一匹配；所述根据所述作用力信息，控制所述机械臂主体的位姿，包括：
101.根据所述作用力信息，确定位姿调整参数；
102.根据所述位姿调整参数，将所述机械臂主体的位姿由第一位姿矫正为第二位姿；其中，所述第一位姿和所述第二位姿不同，且所述第二位姿为每个所述充电头均和相匹配的充电孔对齐时所述机械臂主体的位姿。
103.在一种可能的实现中，所述第一位姿为所述多个充电头的底部端面未和所述充电孔的底部接触时的位姿，所述底部端面为和所述充电头的轴向方向垂直的表面。
104.在一种可能的实现中，所述对齐包括：充电头沿相匹配的充电孔方向的投影落在所述相匹配的充电孔的范围内，且充电头的中心轴与相匹配的充电孔的中心轴重合或者偏差小于阈值。
105.在一种可能的实现中，在将所述机械臂主体的位姿由第一位姿矫正为第二位姿之后，所述方法还包括：
106.控制所述机械臂主体的位姿，以便所述充电插头朝向所述充电孔底部且沿着所述充电头的中心轴的方向移动。
107.在一种可能的实现中，在所述由第一位姿矫正为第二位姿的过程中，所述充电口对所述充电插头在目标方向上的阻力存在降低的趋势，所述目标方向为所述充电头的轴向方向或径向方向。
108.在一种可能的实现中，在所述由第一位姿矫正为第二位姿的过程中，所述充电口对所述充电插头之间的力矩存在降低的趋势。
109.在一种可能的实现中，所述方法还包括：
110.基于所述作用力信息指示所述充电口对所述充电插头仅存在沿所述充电插头的轴向方向的阻力，控制所述机械臂主体保持静止状态。
111.在一种可能的实现中，所述位姿调整参数具体用于控制所述充电插头的移动方向以及移动速率。
112.在一种可能的实现中，所述方法还包括：
113.根据所述作用力信息，控制所述机械臂主体的位姿之前，实时获取视觉传感器针对于所述充电口的区域所采集的图像信息；
114.根据所述图像信息中所述充电口的位置信息，得到所述机械臂主体的实时位姿；
115.根据所述实时位姿，控制所述机械臂主体的位姿，以便将所述机械臂主体上的充电插头引导至所述充电口周围。
116.在一种可能的实现中，所述机械臂主体上设置有所述视觉传感器。
117.在一种可能的实现中，所述图像信息包括2d图像以及3d点云，所述方法还包括：
118.根据所述2d图像以及预设的充电口3d模型，提取所述充电口对应的第一3d位置；
119.根据所述3d点云，提取所述充电口对应的第二3d位置；
120.将所述第一3d位置和所述第二3d位置进行融合，得到所述充电口的位姿。
121.在一种可能的实现中，所述第一3d位置包括所述充电口上m个第一特征点的3d位置；所述第二3d位置包括所述充电口上m个第二特征点的3d位置；所述m个第一特征点和所述m个第二特征点一一对应；
122.所述将所述第一3d位置和所述第二3d位置进行融合，包括：
123.将所述m个第一特征点的3d位置分别和对应的所述第二特征点的3d位置进行融合。
124.在一种可能的实现中，所述机械臂主体还包括目标接口，所述目标接口用于可拆卸连接所述充电插头。
125.在一种可能的实现中，在所述控制结构根据所述作用力信息，控制所述机械臂主体的位姿之前，所述目标接口与初始充电插头连接；
126.所述方法还包括：控制所述机械臂主体的位姿、以及所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆卸并替换为所述充电插头；其中，
127.所述目标充电插头为支持快充充电的插头，所述充电插头为支持慢充充电的插头；或者，
128.所述目标充电插头为支持慢充充电的插头，所述充电插头为支持快充充电的插头。
129.在一种可能的实现中，所述充电设备还包括：插头收纳区域，所述插头收纳区域包括用于设置所述充电插头的目标收纳位置；
130.所述控制所述机械臂主体的位姿、以及所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆卸并替换为所述充电插头，包括：
131.控制所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆
卸；
132.控制所述机械臂主体的位姿，将所述目标接口移动至所述目标收纳位置，并控制所述目标接口的连接状态，将所述目标接口与所述充电插头进行连接。
133.在一种可能的实现中，所述充电设备为固定式充电桩、移动机器人或者充电车。
134.第四方面，本技术实施例提供了一种计算机可读存储介质，其特征在于，包括计算机可读指令，当该计算机可读指令在计算机设备上运行时，使得该计算机设备执行上述第三方面及其任一可选的方法。
135.第五方面，本技术实施例提供了一种计算机程序产品，其特征在于，包括计算机可读指令，当该计算机可读指令在计算机设备上运行时，使得该计算机设备执行上述第三方面及其任一可选的方法。
136.第六方面，本技术提供了一种芯片系统，该芯片系统包括处理器，用于支持执行控制机构实现上述方面中所涉及的功能，例如，发送或处理上述方法中所涉及的数据；或，信息。在一种可能的设计中，该芯片系统还包括存储器，该存储器，用于保存执行设备或训练设备必要的程序指令和数据。该芯片系统，可以由芯片构成，也可以包括芯片和其他分立器件。
137.第七方面，本技术提供了一种控制机器臂充电的装置，所述装置包括一个或多个处理器和存储器；其中，所述存储器中存储有计算机可读指令；所述一个或多个处理器读取所述计算机可读指令，执行上述第三方面及其任一可选的方法。
138.本技术提供了一种充电设备，其特征在于，包括：机械臂、力传感器以及充电插头；所述机械臂包括控制结构以及机械臂主体；所述机械臂主体上设置有所述力传感器以及所述充电插头，所述充电插头包括多个充电头，所述力传感器用于采集所述充电插头和所述充电口之间接触面的作用力信息，所述接触面包括所述多个充电头中至少一个充电头的侧面，所述侧面为和所述充电头的轴向方向平行的表面；所述控制结构用于根据所述作用力信息，控制所述机械臂主体的位姿，以便将所述充电插头插入至所述充电口。在现有的只基于视觉进行位姿控制的方案中，由于视觉本身的误差，而且在充电插头在充电口附近的时候视觉相机已无法继续观测到充电口，如果只采用在最佳观测点观测到的视觉位姿进行充电插头的插拔，可能会发生不能插到底、楔牢的情况，本技术通过获取到力传感器采集的作用力信息来进行机械臂的位姿控制，可在充电插头已部分插入(例如侧面和充电口接触)，视觉信息无法获取误差的情况下，根据力觉调整位置和位姿，让充电插头适应充电口的插入方向，从而提高了插拔任务的准确性以及可靠性。减少了由于充电头侧面和充电口的内壁挤压或者摩擦时对充电头的磨损(或者是卡住而无法达到底部)的情况。
附图说明
139.图1为本技术实施例提供的场景示意；
140.图2为本技术实施例提供的场景示意；
141.图3为本技术实施例提供的场景示意；
142.图4为本技术实施例提供的场景示意；
143.图5为本技术实施例提供的场景示意；
144.图6为本技术实施例提供的场景示意；
145.图7为本技术实施例提供的场景示意；
146.图8为本技术实施例提供的一个充电设备的结构示意；
147.图9为本技术实施例提供的一个搜索方法示意；
148.图10为本技术实施例提供的一个检测结果示意；
149.图11为本技术实施例提供的一个检测结果示意；
150.图12为本技术实施例提供的一个位姿计算方法示意；
151.图13为本技术实施例提供的一个信息融合算法示意；
152.图14为本技术实施例提供的一个模型结构示意；
153.图15为本技术实施例提供的一个检测结果示意；
154.图16为本技术实施例提供的一个坐标系转换示意；
155.图17为本技术实施例提供的一个3d信息提取示意；
156.图18为本技术实施例提供的一个控制机械臂充电的方法示意；
157.图19为本技术实施例提供的一个机械臂示意；
158.图20为本技术实施例提供的一个控制机械臂充电的方法示意；
159.图21为本技术实施例提供的一个rcc设备示意；
160.图22为本技术实施例提供的一个控制机械臂充电的示意；
161.图23为本技术实施例提供的一个受力示意；
162.图24为本技术实施例提供的一个控制机械臂充电的装置示意；
163.图25为本技术实施例提供的芯片的一种结构示意图。
具体实施方式
164.下面结合本技术实施例中的附图对本技术实施例进行描述。本技术的实施方式部分使用的术语仅用于对本技术的具体实施例进行解释，而非旨在限定本技术。
165.本技术的说明书和权利要求书及上述附图中的术语“第一”、第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的术语在适当情况下可以互换，这仅仅是描述本技术的实施例中对相同属性的对象在描述时所采用的区分方式。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，以便包含一系列单元的过程、方法、系统、产品或设备不必限于那些单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它单元。
166.新能源车代表了汽车行业绿色发展的新方向，在我国和全世界出现了爆发性发展。随着新能源汽车销量陡增，车主们需要在路途中进行“加电”。然而，现有的服务区或停车场充电桩相对严重不足，导致经常出现用户大量排队、排队等待时间远超过充电时间的现象。在短时间无法大量提供充电桩的情况下，采用移动充电设备(例如充电机器人)为新能源车进行自动充电，为新能源车提供”移动充电宝”，可在短时间内有效缓解充电桩严重不足的问题。另一方面，即便停车场里有空闲的充电桩，在停车场里找到电车充电桩也特别费时间，同时快充充电桩普遍存在充电枪和线缆重量重、硬度较高的问题，导致驾驶员人工进行充电操较为困难。充电机器人可以自行导航到车主跟前，并代替人进行充电枪的插拔，会给用户带来更高效的充电服务和更好的充电体验。
167.如图1所示，移动充电设备具体可以表现为充电桩是可移动的形式，或者可移动底
盘搭载电池包形式，比如当移动充电设备是一种移动充电车时，该移动充电车的底盘上可搭载多个电池包，在移动充电车接收到用户的请求后，驶向用户所在地，并为用户驾驶的车辆提供充电服务，完成充电任务。
168.本技术的技术方案可应用于一种移动充电系统，如图2所示，该系统可以包括服务器11、至少一个车辆系统12、移动充电设备13和充电桩14等。其中，每个车辆系统12中可以包括用户121、终端设备122和待充电车辆123，如图3所示。车辆123为用户121正在驾驶的需要充电的车辆，终端设备122的持有者是用户121。
169.其中，用户121可以是驾驶员，当驾驶员发现其驾驶的车辆123电量不足时，通过终端设备122向服务器发送一个请求消息，所述请求消息用于请求为车辆123充电。或者，用户121在终端设备122的app上生成一个请求，然后将该请求发送给服务器11。
170.终端设备122可以是一种便携式设备，比如智能终端、手机、笔记本电脑、平板电脑、个人计算机(personal computer，pc)、个人数字助理(personal digital assistant，pda)，可折叠终端、具备无线通讯功能的可穿戴设备(例如智能手表或手环)、用户设备(user device)或用户设备(user equipment，ue)、以及增强现实(augmented reality，ar)或者虚拟现实(virtual reality，vr)设备等，本技术的实施例对终端设备的具体设备形态不做限定。
171.车辆123可以是一种电动汽车(electric vehicle，ev)，所述ev包括显示屏、车机处理器和通信模块，此外，ev还可以包括其他部件或单元，本实施例对此不予限制。
172.可选的，在一种可能的实施方式中，上述车辆系统12中可以不包括终端设备122，所述终端设备122的功能由ev中的车机处理器和通信模块来实现，比如车机处理器在接收到用户触发的指令之后，通过通信模块向服务器11发送请求消息。
173.参见图2，服务器11可接收一个或多个车辆系统12发送的请求消息，对系统中的至少一个移动充电设备13进行调度，并向相应的移动充电设备派发充电任务，指示其驶向发请求消息的用户所在位置，对请求的车辆进行充电。另外，在调度和派发充电任务之前，服务器11还用于获取路况信息，根据路况信息进行高级驾驶辅助系统(advanced driver assistance systems，adas)计算，生成调度策略等。
174.可选的，所述服务器11为云服务器，所述云服务器可以是一个服务器，或者是由多个服务器组成的服务器集群。
175.移动充电设备13，用于接收服务器11发送的指示信号，并根据指示信号驶向待充电车辆(本技术实施例也可以称之为目标车辆)的位置，并为其充电，当移动充电设备13完成充电任务后，返回充电桩14进行补电。此外，移动充电设备13还用于在执行充电任务的过程中实时地上报当前的所携带的电池包的电量情况，以及在行驶过程中实时地上报自己的位置信息给服务器11，以便为服务器11能够实时地掌握系统中各个移动充电设备的情况，为后续充电任务的派发和电能资源的调度提供依据。
176.另外，每个移动充电设备13中包括车机处理器、通信模块、充电电池或者电池包，所述电池包的数量可以是一个或者多个，每个电池包携带的一定电量，本实施例对电池包的数量以及每个电池包的电量不予限制。
177.可选的，所述移动充电设备13可以是一种ev，或者燃油汽车。
178.接下来介绍移动充电设备13的架构示意。
179.本技术实施例可以应用于如图4所示的移动充电设备13中。如图4所示，该移动充电设备13可以包括：传感器模块110、驱动装置120、充电装置140和主控平台130。
180.其中，传感器模块110可以包括一颗或者多颗视觉传感器111(例如摄像头)(本技术实施例也可以简称为传感器)组成，例如可以包括普通的光学摄像头，也可以是红外摄像头、结构光传感器或飞行时间(time-of-flight，tof)传感器等。示例地，传感器模块110可以包括一颗普通的rgb摄像头或红黄黄蓝(red yellow yellow blue，ryyb)摄像头，摄像装置模块也可以包括多颗摄像头或者传感器组成rgb-d深度摄像头方案。示例地，rgb-d深度摄像头方案可以包括两颗rgb摄像头组成双目方案，可以包括一颗rgb摄像头和一颗结构光传感器组成结构光方案，也可以是包括一颗rgb摄像头和一颗tof传感器组成tof方案，本技术实施例对此不做具体限定。另外，视觉传感器111(例如摄像头)可以是定焦摄像头，也可以是变焦摄像头，例如具备相位对焦、激光对焦等能力。
181.应理解，视觉传感器111(例如摄像头)可以承载于一个运动单元上，运动单元用于承载视觉传感器111(例如摄像头)，并且驱动视觉传感器111(例如摄像头)产生旋转。在一个实施例中，运动单元可以驱动视觉传感器111(例如摄像头)产生两自由度旋转，若z轴所指的方向为摄像头11正前方，那么上述两自由度旋转可以包括视觉传感器111(例如摄像头)以x轴为旋转轴的旋转和视觉传感器111(例如摄像头)以y轴为旋转轴的旋转。运动单元驱动视觉传感器111(例如摄像头)产生旋转可以通过舵机或者伺服电机的转动来实现，示例地，当驱动装置用于驱动视觉传感器111(例如摄像头)产生两自由度旋转时，运动单元可以包含两个驱动机构driver 1和driver 2，例如两个舵机或者两个伺服电机，其中一个舵机1或者伺服电机1用于控制视觉传感器111(例如摄像头)以x轴为旋转轴的旋转，另一个舵机2或者伺服电机2用于控制视觉传感器111(例如摄像头)以y轴为旋转轴的旋转。在另一些实施例中，运动单元可以驱动视觉传感器111(例如摄像头)产生三自由度旋转，即增加视觉传感器111(例如摄像头)以z轴为旋转轴的旋转，相应地，运动单元还可以包含三个驱动机构driver 1、driver 2和driver 3，例如三个舵机或者三个伺服电机，其中一个舵机1或者伺服电机1用于控制视觉传感器111(例如摄像头)以x轴为旋转轴的旋转，另一个舵机2或者伺服电机2用于控制视觉传感器111(例如摄像头)以y轴为旋转轴的旋转，另一个舵机3或者伺服电机3用于控制视觉传感器111(例如摄像头)以z轴为旋转轴的旋转。
182.传感器模块110还可以包括运动传感器112，运动传感器112可以为里程计、加速度计、速度计、惯性测量单元等等，用于采集移动充电设备13在行驶过程中的里程信息，如行程、轨迹、速度等信息。
183.力传感器113可以为用于检测对机械臂142末端施加力的传感器。作为力传感器113，可以利用可以检测单轴方向的力的压力传感器和可以检测多个轴方向的力的成分的力觉传感器或力矩传感器。在本实施方式中，作为力传感器113，可以使用六轴的力觉传感器。六轴的力觉传感器检测与固有的传感器坐标系中互相正交的三个检测轴平行的力的大小和绕三个检测轴的力矩的大小。需要说明的是，力传感器113可以设置在机械臂142末端的位置以外的位置，例如，可以设置在机械臂142中的一个以上的关节上。
184.驱动装置120可包括为移动充电设备13提供动力运动的组件。在一个实施例中，驱动装置120可包括引擎、能量源、传动装置和车轮/轮胎。引擎可以是内燃引擎、电动机、空气压缩引擎或其他类型的引擎组合，例如气油发动机和电动机组成的混动引擎，内燃引擎和
空气压缩引擎组成的混动引擎。引擎将能量源转换成机械能量。
185.能量源的示例包括汽油、柴油、其他基于石油的燃料、丙烷、其他基于压缩气体的燃料、乙醇、太阳能电池板、电池和其他电力来源。能量源也可以为移动充电设备13的其他系统提供能量。
186.传动装置可以将来自引擎的机械动力传送到车轮。传动装置可包括差速器和驱动轴。在一个实施例中，传动装置还可以包括其他器件，比如离合器。其中，驱动轴可包括可耦合到一个或多个车轮的一个或多个轴。
187.主控平台130是该装置的数据处理和控制中心，主控平台130与充电装置140、传感器模块110和驱动装置120建立通信连接，例如能够接收传感器模块110采集的图像数据，对图像数据进行处理，以及向驱动装置120发送移动的指令等。其中，充电装置140可以包括充电头141(或者称之为充电插头)和机械臂142。在一些实施例中，主控平台130可以是嵌入式的计算机平台，包括单元不限于基于x86指令集、arm指令集、risc-v指令集或者mips指令集等设计的计算机芯片和软件系统等。
188.在一个实施例中，上述计算机芯片例如可以包括处理器131和存储器132，其中，处理器131例如可以包括：中央处理器(central processing unit，cpu)、系统芯片(system on a chip，soc)、应用处理器(application processor，ap)、微控制器(microcontroller)、神经网络处理器(neural-network processing unit，npu)和/或图形处理器(graphics processing unit，gpu)等；存储器132例如可以包括非易失性存储器和易失性存储器，非易失性存储器例如快闪存储器(flash memory)，包括nand flash、固态硬盘等，易失性存储器例如同步动态随机存取內存(synchronous dynamic random-access memory，sdram)等。
189.在一个实施例中，上述软件系统可以包括操作系统以及运行在该操作系统中的程序指令133。当处理器执行上述程序指令时，使得图3或图4所示的装置执行本技术实施例提供的控制机械臂充电的方法的各个步骤。
190.在一些实施例中，存储器132可包含程序指令133(例如，程序逻辑)，程序指令133可被处理器131执行来执行移动充电设备13的各种功能，包括以上描述的那些功能。存储器132也可包含额外的指令，包括向驱动装置120、传感器模块110、控制系统和外围设备中的一个或多个发送数据、从其接收数据、与其交互和/或对其进行控制的指令。
191.除了程序指令133以外，存储器132还可存储数据，例如道路地图、路线信息，自动驾驶装置的位置、方向、速度以及其它这样的自动驾驶装置数据，以及其他信息。这种信息可在移动充电设备13在自主、半自主和/或手动模式中操作期间被移动充电设备13使用。
192.无线通信系统150可以直接地或者经由通信网络来与一个或多个设备(例如服务器11)无线通信。例如，无线通信系统150可使用3g蜂窝通信，例如码分多址(code division multiple access，cdma)、evd0、全球移动通信系统(global system for mobile communications，gsm)/是通用分组无线服务技术(general packet radio service，gprs)，或者4g蜂窝通信，例如长期演进(long term evolution，lte)，或者5g蜂窝通信。无线通信系统150可利用wifi与无线局域网(wireless local area network，wlan)通信。在一些实施例中，无线通信系统150可利用红外链路、蓝牙或zigbee与设备直接通信。其他无线协议，例如各种自动驾驶装置通信系统，例如，无线通信系统150可包括一个或多个专用
短程通信(dedicated short range communications，dsrc)设备，这些设备可包括自动驾驶装置和/或路边台站之间的公共和/或私有数据通信。
193.可选地，上述这些组件中的一个或多个可与移动充电设备13分开安装或关联。例如，存储器132可以部分或完全地与移动充电设备13分开存在。上述组件可以按有线和/或无线方式来通信地耦合在一起。
194.可选地，上述组件只是一个示例，实际应用中，上述各个模块中的组件有可能根据实际需要增添或者删除，图4不应理解为对本技术实施例的限制。
195.本技术实施例中控制机械臂充电的方法的执行主体可以为移动充电设备13，也可以为云侧的服务器11，参照图5，图5示出的系统中可以移动充电设备13和服务器11，移动充电设备13和服务器11通信连接(例如通过无线通信系统150实现)，其中，移动充电设备13可以将传感器采集的数据传递至服务器11，由服务器11基于移动充电设备13将传感器采集的数据来实现本技术实施例中的控制机械臂充电的方法，并将控制机械臂充电的相关处理结果传递至移动充电设备13。
196.接下来介绍服务器11的架构示意。
197.本技术实施例还提供了一种服务器，请参阅图6，图6是本技术实施例提供的服务器一种结构示意图，具体的，服务器600由一个或多个服务器实现，服务器600可因配置或性能不同而产生比较大的差异，可以包括一个或一个以上中央处理器(central processing units，cpu)66(例如，一个或一个以上处理器)和存储器632，一个或一个以上存储应用程序642或数据644的存储介质630(例如一个或一个以上海量存储设备)。其中，存储器632和存储介质630可以是短暂存储或持久存储。存储在存储介质630的程序可以包括一个或一个以上模块(图示没标出)，每个模块可以包括对服务器中的一系列指令操作。更进一步地，中央处理器66可以设置为与存储介质630通信，在服务器600上执行存储介质630中的一系列指令操作。
198.服务器600还可以包括一个或一个以上电源626，一个或一个以上有线或无线网络接口650，一个或一个以上输入输出接口658；或，一个或一个以上操作系统641，例如windows servertm，mac os xtm，unixtm,linuxtm，freebsdtm等等。
199.目前主流的充电机器人方案均采用结构光或双目或超声结合机械臂规划方式通过位置控制实现插拔。首先通过结构光相机在其感知范围内对充电口拍照识别，获取充电座相对于相机的位姿，再将位姿坐标转换为机械臂末端充电插口的位姿，即可获取机械臂的控制指令，进行机械臂运动规划完成插拔。
200.由于为了实现充电插口向充电口的准确插入，要求具备亚毫米级(例如小于等于2
°
)的位姿定位精度，才可保证充电插口向充电口的顺利插入，且对接不损坏连接器件，因此基于视觉反馈的方案就要求视觉检测模块就要达到位置识别亚毫米级，位姿识别2
°
的要求，然而当前结构光技术基本精度可以达到1～2mm，位姿3
°
左右，还会受到距离视场角的限制，本身就很难保证顺利插拔，即使插入，由于位姿误差的存在也会导致较大的插拔力，容易破坏设备，甚至损坏机械臂。因此，仅基于视觉反馈技术进行插拔很难达到充电插口向充电口的顺利插入的精度要求。
201.本技术实施例提供的用于进行充电的机械臂可以高精度的实现充电插口向充电口的插拔。
202.接下来从产品的形态上介绍本技术实施例中的充电设备。
203.参照图7，图7为本技术实施例中的充电设备的充电场景示意，其中，充电设备可以包括机械臂。机械臂可以采用多轴串联型机械臂，实现对于充电枪的位置和位姿控制，机械臂形态不限于串联，只要可以实现末端充电枪六维位姿导的引即可。充电设备可以包括力传感器(或者可以为力矩传感器)，在一种可能的实现中，其能够感知枪头末端所受的力和力矩(fx，fy，fz，tx，ty，tz)，能够感知侧向力传感器均可。充电设备可以包括摄像头，能够获取充电口的2d图像和/或3d点云，用于识别充电座的6d位姿。充电设备可以包括充电枪(包括充电插头)及固定装置，充电设备可以包括移动平台，扩展充电装置的移动范围。
204.下面结合本技术实施例中的附图对本技术实施例进行描述。本技术的实施方式部分使用的术语仅用于对本技术的具体实施例进行解释，而非旨在限定本技术。
205.参照图8，图8为本技术实施例提供的一种充电设备的实施例示意图，其中，移动充电设备可以为固定式充电桩、移动充电车辆、移动充电机器人等产品。
206.如图8示出的那样，本技术实施例提供的充电装置可以包括机械臂802，其中机械臂802可以包括：控制结构以及机械臂802主体。其中，控制机构可以为上述介绍的处理器131。
207.在一种可能的实现中，控制机构可以控制机械臂802主体的位姿，来实现充电任务，充电任务例如可以为：将机械臂802末端的充电插头803插入至车辆的充电口中。
208.在一种可能的实现中，在充电任务的开始阶段，可选的，充电设备可以接收充电指令(例如可以接收到来自服务器发送的充电指令，或者是直接接收到用户的终端设备发送的充电指令)，可选的，充电指令可以携带有被充电车的位置、被充电车辆的车型信息、快慢充信息等信息。充电设备可以基于接收到充电指令而开启充电任务的执行，触发开启移动状态。
209.在一种可能的实现中，充电设备需要移动至被充电车辆充电口附近才能开始机械臂802的操作，保证充电口在机械臂802可达空间范围内，此部分可以采用导航定位模式或者自动驾驶技术实现。
210.在一种可能的实现中，充电设备可以兼容快充枪和慢充枪两种充电方式，因此可以具备快换机构，可根据用户任务需求选取充电枪，这里采用磁吸式结构抓取充电枪，保证充电枪和机械臂802的可靠对接，然后进行下一步充电操作。
211.在一种可能的实现中，充电设备选择停靠点之后，移动底盘的任务基本达成，即将进行机械臂802的操作，那么需要知道充电座(包括充电口，本实施例为了方便描述，充电座也可以称之为充电口)的大概位置，才能控制机械臂802在该区域进行充电口的位置搜索。充电口的大致位置可以根据车型检测，也可以通过充电设备相机对被充电车辆进行车型识别，获取到被充电车辆的视觉信息结合车辆模型信息即可解算出充电口大致位置，将此位置发送给机械臂802，机械臂802即可在此区域进行下一步流程，即搜索阶段。
212.在一种可能的实现中，由于移动平台的停靠位置存在位置误差，相机视野，例如视场角(field of view，fov)也会存在限制。很难保证在机械臂802末端的相机范围内保证充电口可见，因此有必要进行充电口的“搜索”策略，进行充电口的初步定位或者为粗定位。可选的，搜索方案可以基于机械臂802运动学中的扇形路径规划以及视觉感知中目标检测算法来共同实现。
213.示例性的，搜索过程的流程图可以参照图9。具体的，由于充电车停靠位置存在偏差，并且待充电车辆品牌、型号及停靠位置等的不同，加之机械臂802末端camera视角存在限制，因此为了保证充电口可见首先控制机械臂802进行指定区域的搜索。可以通过已知充电口的大致位置信息(如车辆停靠位置以及充电口高度、倾角等)生成搜索路径，控制机械臂802完成路径跟随。搜索过程中同步进行充电口的目标检测与跟踪。
214.接下来介绍本技术实施例中的扇形轨迹搜索，通常情况下，新能源汽车的充电口高度分布在70c～100cm范围内，因此可在笛卡尔空间内结合相机fov在此高度进行扇形轨迹搜索，搜索过程伴随实时充电口检测算法，检测到目标则驱动机械臂802运动使得目标处于视野中心，这样的方式有利于进入下一阶段的视觉伺服过程。
215.在一种可能的实现中，可以基于深度学习来实现充电口的目标快速和高精度检测与跟踪。为了提升任务快速性，高效率实现充电口搜索，可选的，可以采用改进的yolov4深度学习目标检测算法结合template tracking跟踪算法的方法。
216.其中，改进的yolov4算法主要是将原网络结构中的特征提取网络在深度上分别对卷积层数和卷积核数进行压缩，改进后的模型特征提取网络的卷积层数下降了(例如可以由原本的73层下降至22层)，因此模型的加载和推理过程速度都有很大提升，并且精度几乎保持不变。
217.表1：改进的yolov4算法与yolov4算法对比
[0218][0219]
改进的yolov4目标检测算法可准确地检测出图像中充电口区域(box坐标)即(x,y,w,h)，将此区域图像作为目标区域传递给template tracking跟踪算法，当下一帧图像到来时，跟踪算法将计算图像中与目标区域相关系数最高的区域作为跟踪结果输出。
[0220]
传统的图像检测算法无法应对未知且昏暗的地下车库与品种繁多的车辆品牌、型号，而深度学习则不同，深度学习受海量数据驱动，具有极强的鲁棒性和泛化能力，但同时，深度学习算法的预测推理过程会进行大量计算，需要硬件设备(显卡)的算力支持。因此单一的深度学习算法或传统图像检测算法均无法得到我们预期的效果。而此方案利用深度学习算法对环境变化的鲁棒性与对陌生环境的泛化能力，确保复杂环境中可以成功检测到充电口目标，同时对原有算法模型进行了精简和压缩，提高了算法运行速度，并且结合了传统算法中的template tracking算法弥补了深度学习算法在便携式的工控机上因算力不足导致的无法实时检测的问题，实现了复杂环境中充电口目标的实时检测与跟踪。其中，检测结果的示意可以参照图10所示，方块区域(box)为检测结果输出，坐标为(x,y,w,h)，代表图像中的像素坐标。追踪效果可以参照图11所示，每一帧图像中在box为追踪区域，确保该区域存在充电口的检测结果。
[0221]
通过上述针对于充电口的搜索，控制机构可以控制机械臂802的位姿，以便将充电口落到相机的视野范围内(可选的，落到相机的视野中心区域)。之后，控制机构可以可以基
于视觉伺服的方式控制机械臂802的位姿，以便将机械臂802主体上的充电插头803引导至所述充电口周围。
[0222]
需要说明的是，位姿(position and orientation)可以包括位置和位姿，位置可以可以由三维坐标系中的三个坐标值表示，位姿可以由绕各坐标轴旋转所规定的状态来表示。
[0223]
在将机械臂802主体上的充电插头803引导至所述充电口周围时，需要知晓充电口相对于机械臂802主体(具体可以为机械臂802主体上设置充电插口的位置)的相对位姿，基于视觉传感器804采集的信息确定的位姿可以为视觉传感器804相对于充电口的位姿，而由于视觉传感器804在机械臂802上的位置是固定的，也就是说视觉传感器804和充电插头803之间的相对位置关系是固定的，因此在知晓视觉传感器804相对于充电口的位姿之后，相当于知道了充电插头803和充电口之间的相对位姿。
[0224]
应理解，在获取充电口相对于视觉传感器804的相对位姿时，由于标定误差的存在，可能并不等同于充电座相对于机械臂802的相对位姿，这之间还会存在系统误差，且随着机械臂802基座与充电座相对位姿不同而发生变化，本技术可以通过标定解决两个坐标系之间存在的系统误差。
[0225]
具体的，充电机器人要完成充电枪的插拔，应满足在完全插入时，充电枪在机械臂802坐标系下的位姿与在充电座坐标系下的位姿代表同一空间位姿。这首先要求知道充电座坐标系相对于机械臂802坐标系的位姿关系。由于机械臂802基座可能随时发生变化，因此这个位姿关系也是动态发生变化的。我们可以通过视觉感知的方式得到充电座相对于相机坐标系的位姿，然后通过手眼参数变换到机械臂802末端坐标系，再通过正运动学得到充电座相对于机械臂802坐标系的位姿。由于机械臂802系统误差、手眼标定误差和视觉识别误差的客观存在，通过这种方法得到的充电座位姿，和充电座位姿的真值是存在误差的，通常不能保证充电枪的插入。由于充电座位姿的真值无法通过其他方法获得，而本技术实施例中的机械臂802上可以安装有力传感器801，因此可以通过力传感器801采集的信息来进行基于力觉的自标定方法，来实现充电座真值的获取，并构建视觉获取的充电座位姿补偿矩阵进行观测位姿补偿。
[0226]
在一种可能的实现中，假设充电插头803和充电口没有间隙，由于充电连接件为过渡配合，因此假设成立，认为充电插头803坐标系与充电口坐标系重合，定义世界坐标系为充电设备的基座w，末端坐标系定义在充电插头803的最末端e，充电口坐标系为o，相机坐标系为c，于是可以有：
[0227][0228][0229]
第一步，在对接成功时，可得到：
[0230]
[0231][0232]
利用正运动学的计算方法：
[0233][0234]
可见：
[0235][0236]
为允许一定的容错性，充电口和充电插头803之间可以存在一定的但非常小的误差，可以通过力觉自标定方法来确定正确的插入位姿完成对正确的真值插入位姿的搜索。具体办法可以示例性的为：
[0237]
(1)利用充电插头803插入充电口到底。
[0238]
(2)利用柔性调整充电插头803的位姿，使得所受力矩最小。
[0239]
(3)程序控制调整充电插头803的位姿，使得除充电插头803轴向外，其他两个坐标方向受力和最小。
[0240]
(4)在垂直于充电插头803轴向的两个方向(x，y方向)进行平移，得到x，y方向的最小值和最大值
[0241]
(5)平移机械臂802末端充电插头803，使其停止在x，y方向的中值附近。
[0242]
这样，通过力反馈可以找到真值插入位姿
[0243]
在第二步，在机械臂802基座与第一步保持一致的前提下，可以让机械臂802末端移动到最佳观测点处。
[0244]
在最佳观测点处，可得到正运动学计算的末端位姿：
[0245]wte＝wt6(θ1)6te[0246]
通过在最佳观测点观测到充电口位姿通过视觉得到的末端位姿：
[0247][0248]
其中是视觉测量误差，是手眼参数的误差，是手眼标定的测量值，令上面两个公式相等，可以得到补偿量：
[0249][0250]
得到补偿量之后，可以在最佳观测点观测到目标充电口，用如下公式来求取期望充电插头803位姿：
[0251][0252]
结合公式：
[0253][0254]
令上述两式相等，可得：
[0255]
[0256]
进而可以求解出插入后的期望关节角(在机械臂802主体由多个关节组成的情况下，可以通过关节角来控制机械臂802主体的位姿)。
[0257]
本技术提出了一种机械臂802与充电口相对位姿误差的力觉标定方法，通过力觉自标定搜索的技术得到机械臂802充电插头803与充电口完全插入的零位，并根据该零位给出定量补偿视觉观测到的充电口与机械臂802相对位姿误的误差的方法，以此解决机械臂802形变误差、视觉识别误差、手眼标定误差等汇集起来的的充电口相对机械臂802的绝对位姿误差导致不能插入充电插头803的问题。
[0258]
在一种可能的实现中，机械臂802可以包括基台(或者称之为基座)以及机械臂802主体，机械臂802主体可以称之为手臂。手臂可以包括多个关节，关节之间依次连接，且相互之间具备旋转自由度。在机械臂802主体的末端(远离基座的一端)上，可以安装有力传感器801以及充电插口(例如可以顺序安装)。本技术实施例中，控制机构可以可以基于视觉伺服的方式控制机械臂802主体的位姿，以便将机械臂802主体上的充电插头803引导至所述充电口周围。
[0259]
在一种可能的实现中，所述控制机构可以实时获取视觉传感器804针对于所述充电口的区域所采集的图像信息；根据所述图像信息中所述充电口的位置信息，得到所述机械臂802主体的实时位姿；根据所述实时位姿，控制所述机械臂802主体的位姿，以便将所述机械臂802主体上的充电插头803引导至所述充电口周围。
[0260]
由于视觉伺服中采集的图像信息是实时获取的，基于实时获取的图像信息可以求解得到机械臂802主体的实时位姿(该实时位姿可以为机械臂802主体末端和待充电车辆的充电口之间的相对位姿)，由于在控制所述机械臂802主体的位姿时，可能会出现底座移动、障碍物的阻碍等情况，导致机械臂802主体的位姿出现误差，本技术可以基于实时位姿来修正机械臂802主体的位姿，可以提高机械臂802主体的位姿识别精度，进而可以更准确的将机械臂802主体上的充电插头803引导至所述充电口周围。
[0261]
其中，可以通过控制机械臂802进行快速视觉伺服，实现对于充电座的快速导引，这里需要设计稳定的控制律实现精确而快速的闭环控制，机械臂802控制和视觉识别结果需要高度融合从系统角度设计控制算法，使得机械臂802运动进而保证末端充电枪能够快速逼近充电口，为下一步力控插拔做好准备。
[0262]
在一种可能的实现中，充电口的位姿识别(例如6d位姿识别)是实现充电插拔功能的最重要环节，精确的位置识别是进行插拔的前提条件和基础。同时要求算法还需要具有较高的鲁棒性，比如针对车库场景的昏暗条件。6d位置主要是指充电口在机械臂802末端camera坐标系中的位置(x，y，z)和位姿角度(roll，pitch，yaw)；可选的，可以采用rgbd深度相机结合神经网络算法实现充电口的6d位姿识别，识别精度要求达到
±
1～2mm，角度精度
±3°
以内(坐标系之间的转换关系可以参照图16所示)。
[0263]
在一种可能的实现中，采用三维关键点检测结合二维关键点检测网络的方法，实现低对比度充电口检测环境中获取高精度识别结果。
[0264]
其中，视觉伺服是视觉结合运动进行的闭环控制方法。主要功能是实现机械臂802末端充电枪对于充电口的导引运功。大致流程是将充电枪/充电座视觉识别位姿结果作为输入，采用视觉伺服反馈控制思想计算驱动误差，采用基于位姿的视觉伺服实现机械臂802末端向目标位姿的精确导引。采用自适应增益方法，加快在小误差时的调整速度，实现快速
的视觉伺服。此步骤中伺服结束时机械臂802将处于最佳观测点，此处已获取最精准的充电口的位姿。此时机械臂802将根据把充电插口导引至充电口附近，为下一步力控插入做准备。
[0265]
更具体的，由于最佳观测点要保证一定的稳定性，才能保证目标观测位姿的精度。然而，移动机器人基座相对充电座的位姿可能会不断地发生变化。为实现在上述变化下，保障最佳观测点相对充电座的位姿一致，采用位姿的快速视觉伺服实现此目标。在一种可能的实现中，可以采用位姿反馈的视觉伺服思想，基于李氏稳定判据设计关节速度控制律，实现机械臂802末端向目标位置的精确导引。
[0266]
具体实现方法为，系统在完成对充电座的搜索时，充电座在手眼相机的视野中，由此可以得到充电座到相机的位姿矩阵，这个位姿矩阵与期望的所设定的最佳观测点的位姿矩阵相比较，得到了一个误差差值。假设当前位姿向量其中表示充电座坐标系相对于相机坐标系的平移向量，θu表示充电坐标系相对相机坐标系的方向矢量，期望位姿向量0表示相机的方向和充电坐标系的方向在收敛时将保持一致。则定义误差值：e＝s
*-s。
[0267]
通过构建误差的微分与误差成-λ比例关系的方程，保证误差以指数形式衰减。这是较为简单的视觉伺服控制律：
[0268]
这种控制律通常会收敛速度太慢，在误差比较小的时候，可以设定自适应增益的伺服控制律：
[0269]
其中λ(||e||)为误差||e||的函数。如表2所示，可以构建正态型模糊隶属函数a(||e||)进行模糊控制，通过实验调整模糊控制律的参数进行自适应增益控制。依据所制定的模糊集合从极小、较小、中等、较大、极大的变化，其模糊规则数值满足a1》a2》a3》a4》a5，数字a1～a5表示对误差||e||进行态势评分，误差越小评分越高。将误差输入值分别代入隶属函数求解隶属度，并按照隶属度最大原则进行反模糊化。若误差输入值在某一集合中隶属度最大，则选择相应的规则值作为控制输出量，进而实现在误差小的时候衰减值变大，误差大的时候衰减值变小。
[0270]
表2模糊隶属函数及模糊规则
[0271][0272]
在每个伺服周期内，可以根据视觉感知的位姿误差，通过指数衰减方程得到位姿误差微分的值，位姿误差微分可表达为交互矩阵与相机的速度的乘积：
[0273]
则相机速度与位姿向量的误差关系可表达为：其中为相机速度，为相机角速度：
[0274][0275]
而相机速度又可以由雅可比矩阵与机械臂802关节速度的乘积表示，这样，就可以根据位姿误差求解处关节的伺服速度
[0276][0277]
可选的，可以对机械臂802周围进行障碍物检测，并通过势场法根据与障碍物的距离得到的反作用力作为减速度vo，在有障碍物的时候，障碍物产生的势能将把机械臂802推开。定义：
[0278][0279]
其中d是机械臂802末端到环境之间的最短距离，距离越小，势能越大，μ是常数。定义：
[0280][0281]
上述公式表明势场法产生的加速度将指向降低势场势能方向。机械臂802关节总速度可求得为：
[0282][0283]
即可将角速度作为控制输入给机械臂802发送控制指令，进而形成闭环控制，驱动机械臂802运行一个视觉伺服控制周期。
[0284]
上述视觉伺服一直不断运行直到收敛，这时机械臂802带动相机移动到最佳观测点。
[0285]
可选的，视觉伺服流程可以如图12所示。
[0286]
本技术可以在视觉伺服中采用期望位姿补偿以及雅可比计算的补偿。
[0287]
(1)期望位姿补偿。视觉伺服收敛的时候，得到的是相机实际位姿到达的期望观测位姿。即：
[0288][0289]
此处代表相机的测量位姿，和理想相机c位姿存在补偿量因此期望值的设定根据补偿量进行补偿后作为实际期望值。
[0290][0291]
(2)雅可比计算的补偿。由于采用的是实际相机位姿作为控制参数，因此雅可比矩阵可以为：
[0292][0293]
抵达最佳观测点之后的观测补偿已在上述实施例中说明，这里不再赘述。
[0294]
可选的，在一种可能的实现中，所述机械臂802主体上设置有所述视觉传感器804。
[0295]
可选的，在一种可能的实现中，视觉传感器804还可以不设置在机械臂802主体上而是设置在第三方视角对应的位置上。
[0296]
在一种可能的实现中，在进行上述相对位姿的确定时，需要视觉传感器804采集到针对于充电口的图像信息，并基于图像信息来确定充电口相对于视觉传感器804之间的相对位姿。
[0297]
在一种可能的实现中，所述图像信息可以包括2d图像以及3d点云，进而控制机构可以根据所述2d图像以及预设的充电口3d模型，提取所述充电口对应的第一3d位置；根据所述3d点云，提取所述充电口对应的第二3d位置；将所述第一3d位置和所述第二3d位置进行融合，得到所述充电口的位姿。其中，充电口的位姿可以理解为充电口相对于视觉传感器804之间的相对位姿。
[0298]
在一种可能的实现中，充电设备上的视觉传感器804可以为用于采集深度图像的深度传感器。深度传感器可以例如但不限于如下介绍：深度相机、飞行时间(time of fight，tof)相机、或激光雷达、照相式扫描仪、或激光雷达探测与测量(light detection and ranging，lidar)传感器。控制机构采集3d点云可以通过但不限于如下方式：
[0299]
a、基于获取的rgb图像和深度图像得到3d点云。
[0300]
在本技术实施例中，控制机构可以通过部署于该机械臂802主体上的传感器采集rgb图像和深度图像，rgb图像和深度图像就是指传感器在当前时刻、当前位置能感知到的周围环境形成的rgb图像和深度图像。例如，可以是通过部署于该充电设备上的深度相机采集得到rgb图像和对应的深度图像，也可以是通过部署于该充电设备上的深度传感器采集深度图像，并通过部署于该充电设备上的摄像模块(如，摄像头)采集到对应的rgb图像。
[0301]
传感器采集到充电设备在当前时刻的rgb图像和深度图像之后，先对该rgb图像进行实例分割，如，可采用solov2、blendmask等算法对该rgb图像进行实例分割，输出实例分割结果，实例分割结果包括该rgb图像上每个区域所属的分类类别、属于该分类类别的置信度(不大于1)以及分割的掩膜(mask)，可将实例分割结果定义为mi。之后，可以选择属于充电口的数据，并将实现了实例分割且重新定义了像素值的rgb图像(可称为处理后的rgb图像)与深度图像进行叠加，该处理后的rgb图像中的每个局部区域就具有了深度信息，具体地，由于处理后的rgb图像每个局部区域对应的mask在该rgb图像上占据的区域定义像素为1，其余区域定义像素为0，那么将该处理后的rgb图像与深度图像相乘，由于像素值为0的区域相乘后依然是0，因此相乘后的结果只保留了rgb图像中每个区域对应的区域以及每个区域对应的深度信息。之后，再通过相机内参恢复出充电口的3d点云。
[0302]
需要说明的是，在本技术的一些实施方式中，还可以对实例分割结果以及深度图像分别先进行滤波操作，如，通过形态学滤波降低实例分割结果的外点、通过深度平滑消除深度图像的空洞部分等，总之，滤波的目的是为了去除噪声。
[0303]
b、基于获取的rgb图像和原始激光点云数据得到多个3d点云。
[0304]
在本技术实施例中，充电设备可以通过部署于该充电设备上的传感器采集、rgb图像和原始激光点云数据，例如，充电设备上可部署有激光雷达、普通的摄像头等传感器，通过摄像头采集rgb图像，同时通过该激光雷达采集原始激光点云数据，在这种情况下，则不需要获取深度图像，只需按照上述类似过程将rgb图像进行实例分割，然后将实例分割结果对应到同时获取的原始激光点云数据上，从而得到充电口对应的3d点云。
[0305]
在一种可能的实现中，可有采用视觉传感器804来获取到二维图像和三维深度点云数据，二维图像可以通过检测关键点并通过与三维模型的同名关键点进行对应，可以求取目标的6d位姿，但这依赖对二维关键点的高精度检测，同时在通过2d-3d对应恢复6d信息的时候会引入重建误差。车库场景存在弱光线条件，不同充电口深度也有所不同，还存在一定色差。同时，三维点云也可以与目标物体的三维模型进行匹配得到目标6d位姿，但点云的精度略微差一些，需要采用融合二维图像和三维点云各自的重建信息，实现更高精度的定位。为实现对充电口的精确插入，需要得到充电口在机械臂802坐标系中的精确位姿，这可以通过传感器观测到充电口相对于传感器的位姿，再根据传感器安装位置由传感器相对于机械臂802末端的位姿折算到机械臂802末端，再由机械臂802末端通过正运动学折算到相对于机械臂802基座的相对位姿关系。其中，卷积神经网络结构的示意可以参照图14所示。
[0306]
表3 2d关键点及序号检测卷积神经网络结构
[0307]
[0308][0309]
例如，可以通过第1到18层构成的特征提取器在图像上提取出高表征能力的特征图集合(feature maps)，而后利用三层全连接层，将特征图集合转换为维度为14的向量。计算该向量与图像上2d关键点真值组成的向量之间的均方差(mse)作为网络模型的loss函数，不断输入数据、迭代模型直至loss收敛。最终可以获得一个高精度的关键点检测算法模型。
[0310]
示例性的，可以得到七个关键点在充电口坐标系下的三维坐标见下表所示：
[0311]
表4充电口七个关键点充电口坐标系下的三维坐标
[0312][0313]
针对于3d点云关键点检测网络，在实际观测到的三维点云中，所期望的关键点可能并没有观测到，通过关键点检测网络，检测现有各点到关键点的方向向量，并通过投票的方式间接求得关键点的精确坐标。首先，从三维点云模型中选取若干个标准关键点，这通过
最远点采样法求得。具体办法是，第一次通过随机采样的方式得到第一个点，其他每次都在剩余的点中选取与已选点集的距离最远的点，这样求取n个点作为标准关键点。其次，得到了关键点之后，由标准数据关键点以及人工生成变换后的点云生成训练数据集进行训练。输入的点云首先通过pointnet++网络进行特征提取，针对每一个点，有一个多层的全连接神经网络用以计算每个点到k个关键点的偏差，然后根据这些偏差采用3d霍夫投票网络进行投票聚类，并由聚类的点集求解得到精确的三维关键点(具体流程可以参照图17所示)。在进行模型训练时，可以把两个函数根据二维检测和三维检测的误差分别以α，β来进行加权，得到总的损失函数如下：
[0314]
loss＝∑
r,t
(α||p
2d-proj(rp
3dgt
+t)||2+β||p
p3d-(rp
p3dgt
+t)||2)；
[0315]
其中，p
2d
是图像检测的关键点，p
3dgt
是充电口坐标系下的三维点坐标，r，t是从充电口坐标系到相机坐标系的转移矩阵，p
p3d
是相机坐标系下的三维点云中检测的关键点三维坐标，p
p3dgt
三维模型中的关键点三维坐标。算法可以通过图像的2d-3d对应计算r，t初值，然后采用l-m法通过梯度下降求取最优位姿矩阵的解(例如可以参照图13所示)。
[0316]
在一种可能的实现中，充电座的6dpose(位置和姿态)识别是无人充电任务的关键环节，确保充电口的6d位姿准确才能进行机械臂的精确导引，进而采用力控技术进行柔顺插拔。由于标准慢充充电口由七个孔组成，因此选取充电口的七个孔的圆心作为关键点，通过人工智能算法对上文中的目标区域进行关键点检测。此方案将二维图像上的7个关键点检测问题转化为14个数值的回归问题，即7个关键点坐标(x，y)构成的14维向量。基于vgg16网络结构，修改网络结构中的最后三层全连接层参数，使最后一层输出维度为14，将原本的softmax loss改为均方差loss。最终网络的输入为一张彩色图像，输入为维度为14的向量，即为图像上7个关键点的坐标值。
[0317]
采用最远点采样法对点云进行降采样，该方法除了第一次随机采样之外，其他每次都在剩余的点中选取与已选点集的距离最远的点，采样后的点作为稠密点云的降采样版本，并和标准点云进行三维配准，得到三维点对应序列，并以标准三维关键点与目标坐标系关键点的通过r,t变换之后的距离为三维目标函数；
[0318]
采用改进的vgg二维关键点检测网络，得到充电座圆孔中心点二维像素坐标，通过中心点的相互位置关系得到每个点的编号(例如可以参照图15所示)；根据充电座标准cad模型得到圆孔中心点在目标坐标系下的三维物理坐标。二维关键像素点(像素坐标系下)、三维物理点(目标坐标系下)通过编号顺序一一对应，我们根据三维物理点反投影到像素坐标系下得到对应的反投影像素点，并比对二维关键点与反投影点之间的距离为二维损失函数项；
[0319]
把两个函数根据二维检测和三维检测的误差分别以α，β来进行加权，得到总的损失函数。
[0320]
本技术提出了一种融合2d图像-3d点云信息的充电口位姿精确识别方法，可以采用2d深度图像检测网络对充电口的二维关键点进行亚像素级的精确检测，采用三维关键点检测神经网络对充电口三维关键点进行基于3d霍夫网络的偏移量可靠检测，采用非线性优化使得融合二维关键点及三维关键点的投影误差最小的技术来求得充电口的精确位姿，以此解决充电口在光照变化大、目标对比度低情况下的位姿高精度识别问题。
[0321]
通过上述视觉伺服的方式，可以将充电插头803引导至充电口附近，例如精度范围
在1至2mm，2
°
的误差范围内，由于前一段规划基于视觉存在一定误差，同时考虑碰撞安全性，此时控制机械臂802不能与充电口接触，因此需要根据视觉信息进行一段运动，能让机械臂802主体上安装的力觉传感器感知到与充电口接触，然后可以进行基于力感知的自适应调整阶段(例如可以参照图18所示)。
[0322]
本技术提出了一种自适应误差增益的视觉伺服导引方法，提出了结合势场避障的视觉伺服速度指令生成。视觉伺服在实际应用中要和环境交互，但又不能与环境发生碰撞导致破坏，采用结合势场避障的方法引入新的排斥速度因子，有效地保障了视觉伺服的安全性。二是提出了自适应增益的视觉伺服控制律，通过设定衰减指数与误差幅值之间的反向模糊关系，实现了在小误差下的大衰减系数，有效加快了视觉伺服的收敛速度。三是采用了基于位姿的视觉伺服，由于把位置和位姿信息作为特征向量，比基于图像的视觉伺服包含了更多更精确的信息，可有效地导引机械臂802到期望的最佳观测点处，通过上述方法解决了机械臂802与充电口位姿可能不断变化的情况下无碰撞导引相机停留在位姿相对固定的最佳观测点的问题。
[0323]
在充电插头803接触到充电口之后，控制机构可以获取到力传感器801采集的作用力信息，该作用力信息可以指示充电口对充电插头803的作用力(或者力矩)，并根据所述作用力信息，控制所述机械臂802主体的位姿，以便将所述充电插头803插入至所述充电口。
[0324]
在现有的只基于视觉进行位姿控制的方案中，由于视觉本身的误差，而且在充电插头803在充电口附近的时候视觉相机已无法继续观测到充电口，如果只采用在最佳观测点观测到的视觉位姿进行充电插头803的插拔，可能会发生不能插到底、楔牢的情况，本技术通过获取到力传感器801采集的作用力信息来进行机械臂802的位姿控制，可在充电插头803已部分插入，视觉信息无法获取误差的情况下，根据力觉调整位置和位姿，让充电插头803适应充电口的插入方向，从而实现了插拔任务的准确性以及可靠性。
[0325]
接下来介绍本技术实施例中的力传感器801。
[0326]
在一种可能的实现中，所述机械臂802主体上设置有力传感器801，所述力传感器801用于采集所述充电插头和所述充电口之间接触面的作用力信息，所述接触面包括所述多个充电头中至少一个充电头的侧面，所述侧面为和所述充电头的轴向方向平行的表面。
[0327]
在一种可能的实现中，所述作用力信息包括所述充电头的径向方向的力。例如作用力信息可以包括6维的作用力信息。
[0328]
在一种可能的实现中，所述作用力信息包括所述充电口对所述充电插头的力矩信息。
[0329]
其中，作用力信息可以指示充电口对所述充电插头的6维力，6维力可以包括在三维空间内(例如沿着x轴、y轴、z轴方向)的力以及在三维空间内(例如绕着x轴、y轴、z轴旋转)的力矩。由于其中包括了力矩信息，则可以更精准更细粒度的识别出充电口对于充电头的阻碍方向，进而提供更精准的力控来将充电头准确插入充电孔。
[0330]
在一种可能的实现中，所述力传感器801固定于所述充电插头803上(具有可以在至少一个充电头的侧面上)；或者，所述机械臂802包括具备旋转自由度的关节，所述控制结构用于通过控制所述关节的旋转来控制所述机械臂802主体的位姿，所述力传感器801固定于所述关节上；或者，所述力传感器固定在所述充电插头和所述机械臂主体之间。
[0331]
在一种可能的实现中，力传感器801可以具有一个或多个力检测单元、a-d转换电
路和信号处理电路。力检测单元各自包含力检测元件和放大力检测元件的输出信号的放大电路。放大电路将放大后的信号va向a-d转换电路供给。a-d转换电路将数字转换后的信号vd向信号处理电路供给。力检测单元的个数根据由力传感器801检测的检测轴的数量以及关于各检测轴的力的种类设定。例如，力传感器801在检测与互相正交的三个检测轴平行的力的大小和绕三个检测轴的力矩的大小时，使用十二个力检测单元。
[0332]
在一种可能的实现中，作为力检测元件，例如可以使用压电式、应变片式或光学式等各种元件。
[0333]
在一种可能的实现中，放大电路构成为作为将从力检测元件输出的电荷积分并转换成电压信号va的积分电路。具体地，放大电路具有运算放大器、电容器和开关元件。运算放大器的负极输入端子与力检测元件的电极连接，运算放大器的正极输入端子与地(基准电位点)接地。而且，运算放大器的输出端子与a-d转换电路连接。电容器连接在运算放大器的负极输入端子与输出端子之间。开关元件与电容器并列连接。开关元件根据从信号处理电路供给的接通/断开信号执行切换操作。
[0334]
在一种可能的实现中，开关元件是断开时，从力检测元件输出的电荷在电容器中存储，其电压va向a-d转换电路输出。另一方面，开关元件成为接通时，电容器的两端子间短路。其结果是电容器中存储的电荷被放电并成为零，向a-d转换电路输出的电压va成为0伏特。将开关元件接通的操作相当于力传感器801的重置。
[0335]
在一种可能的实现中，力转换部可以将多个力检测单元的输出转换为表示关于多个检测轴的力的f信号(i)并输出。需要说明的是，也将力信号f(i)简单地称为“力f(i)”。可选的，这些力f(i)包含与互相正交的三个检测轴平行的力fx、fy、fz和绕三个检测轴的力矩tx、ty、tz。力f(i)此外也包含将与三个检测轴平行的力fx、fy、fz作为三个轴方向成分的力向量的大小fmag和将绕三个检测轴的力矩tx、ty、tz作为三个轴方向成分的力矩向量的大小tmag。力向量的大小fmag是三个力fx、fy、fz的二次方和的平方根，力矩向量的大小tmag是三个力矩tx、ty、tz的二次方和的平方根(x、y、z轴的示意可以参照图19所示)。
[0336]
在一种可能的实现中，所述充电插头803包括多个充电头，所述充电口包括多个充电孔，所述多个充电头与所述多个充电孔之间一一匹配；所述控制结构可以根据所述作用力信息，确定位姿调整参数，并根据所述位姿调整参数，将所述机械臂802主体的位姿由第一位姿矫正为第二位姿；其中，所述第一位姿和所述第二位姿不同，且所述第二位姿为每个所述充电头均和相匹配的充电孔对齐时所述机械臂802主体的位姿。
[0337]
在一种可能的实现中，基于作用力信息，可以确定出机械臂802主体是否处于正确的位姿(例如第二位姿可以为正确的位姿)。例如，在作用力信息指示充电口对充电插头803存在非轴向方向的压力时，可以确定机械臂802主体未处于正确的位姿，在这种情况下，所述多个充电头中的至少一个充电头未和相匹配的充电孔对齐，也就是充电头的中心轴与相匹配的充电孔的中心轴不重合或者偏差大于阈值的情况。例如可以参照图22中的a、b、c、d。
[0338]
在一种可能的实现中，所述第一位姿为部分所述充电插头803已插入至所述充电口，且所述多个充电头中的至少一个充电头未和相匹配的充电孔对齐时所述机械臂802主体的位姿。
[0339]
在一种可能的实现中，所述第一位姿为所述多个充电头的底部端面未和所述充电孔的底部接触时的位姿，所述底部端面为和所述充电头的轴向方向垂直的表面。
[0340]
在一种可能的实现中，所述对齐包括：充电头向充电孔方向的投影范围在相匹配的充电孔的范围内，且充电头的中心轴与相匹配的充电孔的中心轴重合或者偏差小于阈值。
[0341]
也就是说，第一位姿可以为充电头的中心轴与相匹配的充电孔的中心轴不重合或者偏差大于阈值的情况(也就是上述所述的未处于正确的位姿)。
[0342]
在一种可能的实现中，所述第一位姿为所述多个充电头的底部端面未和所述充电孔的底部接触时的位姿，所述底部端面为和所述充电头的轴向方向垂直的表面。
[0343]
本技术实施例中，在机械臂802主体未处于正确的位姿时，可以基于作用力信息来调整机械臂802主体的位姿，以便机械臂802主体处于正确的位姿。
[0344]
在一种可能的实现中，控制机构可以在基于机械臂802主体处于第一位姿时，根据作用力信息对机械臂802主体的位姿进行矫正(至第二位姿，也就是正确的位姿)，第二位姿为机械臂802主体可以将充电插头803以较小阻力的正确方式插入到充电口底部的位姿。
[0345]
示例性的，可以参照图22，图22中的a、b、c、d可以认为是第一位姿，e可以认为的第二位姿。
[0346]
应理解，在从第一位姿调整至第二位姿时，充电口对所述充电头在目标方向上的阻力可以存在降低的趋势，目标方向可以径向方向和/或轴向状态(例如可以为径向方向、轴向状态、或者径向方向以及轴向方向)。这里所谓的存在降低的趋势，可以理解为充电口对所述充电头在目标方向上的阻力可以不是一直降低，而是可以存在局部的增加，但是从趋势上，充电口对所述充电头在目标方向上的阻力是降低的。
[0347]
在一种可能的实现中，在所述由第一位姿矫正为第二位姿的过程中，所述充电口对所述充电插头之间的力矩存在降低的趋势。
[0348]
参照图23，图23中三条曲线分别为充电插头803x，y，z三个方向上受到的合力，由接触力曲线图可以看出，使用力控算法后，x，y方向接触力逐渐减小，随着插入过程缩减到零点附近，说明自适应位姿调整大大降低了x，y轴方向方向充电口对枪体产生的压力，使用力控算法后，z方向插入接触力由从起始时刻的80n逐渐降低至60n，说明自适应调整阶段之后插拔力逐渐减小并保持稳定，直至可靠插入。从受力曲线可以看出，整个插入过程充电插头803根据力感知进行了快速的自适应位姿调整，并且各个方向合力处于收敛状态，保证具备较好的柔顺特性。
[0349]
在一种可能的实现中，控制机构可以基于作用力信息来确定位姿调整参数，该位姿调整参数具体可以用于控制所述充电插头803的移动方向和/或移动速率(例如移动方向、移动速率、或移动方向以及移动速率)。
[0350]
接下来结合一个具体示例介绍控制机构如何基于作用力信息来确定位姿调整参数，并基于位姿调整参数来控制机械臂802主体的位姿。
[0351]
在一种可能的实现中，可以采用直接力反馈结合改进型导纳控制技术，参考远柔顺中心设备的原理进行柔顺参数调节，实现不同型号充电插头803在插入和拔出过程中的六自由度柔顺特性，保证对于充电口的可靠安全快速插拔操作。其中，可以将插入过程分为二个阶段，停泊点到充电口端口的自由运动，充电口端口到底部的柔顺插入运动。在自由运动阶段，利用位置伺服控制精确地将插头送入充电口端口内；然后，利用导纳控制产生充电插头803末端的柔顺运动，通过力/力矩反馈和设计的导纳矩阵获取机械臂802工具末端速
度的修正量，从而改变理想的插入速度产生柔顺插入行为。整个插入过程，融合视觉感知到的位姿信息、力、力矩、位置、速度等信息，实现可靠插拔。
[0352]
在一种可能的实现中，插拔环节是智能充电的核心环节，由于涉及到充电插头803和充电口的物理接触，因此就存在力的变化，尽管前序流程视觉已经给出了较为精准的充电口目标位姿，但是依然存在一定的误差会造成较大的接触力，同时插入过程中由于不同加工精度，疲劳程度，磨损程度等产生的力交互更为复杂，因此为了保证最后插入过程安全可靠，有必要进行基于力感知的主动柔顺控制。整个力控插拔流程要承接前序视觉信息，并要结合运动状态和力感知进行接触状态判断，保证可以进行插入后采用主动柔顺控制算法实现充电插头803头的自适应调整。
[0353]
示例性的，主动柔顺插拔坐标系可以定义为，坐标原点位于充电插头803端面的几何中心，z轴垂直于端面并指向充电插头803插入方向，y轴位于端面内朝上，x轴由右手法则确定。
[0354]
导纳控制的基本原理为，设充电插头803的插入理想速度为v
ideal
＝[0,0,vz,0,0,0]
t
，即，以速度vz沿z轴做直线运动。如果接触力/力矩部位，则按下列公式在cartesian空间计算实际的充电插头803参考速度：
[0355]vactual
＝v
ideal
+a
admittancewcontact
；
[0356]
式中，v
actual
是实际的充电插头803参考速度，w
contact
是接触力/力矩，a
admittance
是导纳矩阵。
[0357]
充电设备实现导纳算法的原理如图20所示。选定导纳矩阵a
admittance
，依据由力/力矩传感器给出接触力/力矩测量计算出充电插头803在cartesian空间的柔性速度修正量，与理想插入速度相加后得到充电设备在cartesian空间的实际参考速度。然后，乘以逆jacobi矩阵获得每一个关节的参考速度，从而在速度表征上控制充电设备产生柔顺插入运动。
[0358]
当存在接触力/力矩时，rcc设备可以通过自身柔性调整插头位姿，实现与插孔对准，避免产生过大的接触力/力矩。通过选择a
admittance
具有rcc设备的导纳矩阵的形式，即，
[0359][0360]
式中，k是标准rcc设备三个对称弹簧的刚度，α是三个对称弹簧形成的正四面体的中心轴线与弹簧所在边的夹角，l是柔顺远中心与充电插头803端面的距离，分别为绕位于柔顺远中心上与主动柔顺插拔坐标系平行的rcc设备坐标系x轴、y轴、z轴的旋
转刚度。图21是标准rcc设备示意图。
[0361]
因此，通过采用上述改进型导纳控制算法，充电插头803产生与rcc设备类似的柔顺行为，实现柔顺的插入过程，避免产生过大的接触力以及卡死、阻塞等现象。
[0362]
本技术实施例中，结合力感知模型以及自适应调整导纳控制参数，实现根据(力和力矩)插拔力摩擦力自适应调整tcp末端位姿和速度，实现对于位置误差的高容限特性，提高插拔成功率，同时柔顺特性实现“最小”插拔力，提升插拔可靠性和安全性，具有较强的参数自适应能力，能够有效保障插拔完成时间；融合视觉末段位置感知以及插拔力感知，解算“完全插入”数学表征模型，有效判断是否完全插入，提升任务可靠性；同理拔出过程采用柔顺力控技反向实现可靠拔出，保证整体插拔任务可靠性。
[0363]
图22展示了自适应主动位姿调整插入过程中，充电插头803枪头和充电口的关系，a状态为视觉伺服给出的充电插头803头停靠位置，可看出，存在一定的位置和位姿误差(示意图，实际误差值较小)，然后通过自由运动(此处也可以进行一定的搜索路径设计增强接触范围和速度)到达b状态，即接触检测，接触状态检测之后即将进入位姿的自适应调整阶段如图c，最终达到e的完全可靠插入状态，此过程中c和d流程将持续循环运行，直至满足可靠插入条件完成任务。因此整个插入过程中充电插头803是随着接触状态和接触了进行自适应调整的，进而保证插入过程持续减小摩擦力，降低侧向阻力保证可靠性安全性。
[0364]
在一种可能的实现中，所述控制结构可以在将所述机械臂802主体的位姿由第一位姿矫正为第二位姿之后，控制所述机械臂802主体的位姿，以便所述充电插头803朝向所述充电孔底部且沿着所述充电头的中心轴的方向移动。
[0365]
在将所述机械臂802主体的位姿由第一位姿矫正为第二位姿之后，充电插头803已经处于和充电口的对齐位姿，在这种情况下，可以将充电插头803朝向所述充电孔底部且沿着所述充电头的中心轴的方向移动直到底部，在一种可能的实现中，所述控制结构可以基于所述作用力信息指示所述充电口对所述充电头仅存在沿所述充电插头803的轴向方向的阻力，控制所述机械臂802主体保持静止状态，进而完成了向充电口的插入任务。
[0366]
完成充电后，可以进行插入过程的逆向运动，同样采用主动柔性技术，自适应调整拔出过程中充电插头803的位姿。拔出完成后，机械臂802将根据设定完成归位，保证机械臂802收缩到较小的空间，避免底盘移动过程中对其他车辆等物体产生碰撞。机械臂802收回后即完成插拔任务，根据系统任务设定，充电充电设备移动至指定地点。等待下一次任务指令。
[0367]
在一种可能的实现中，所述机械臂主体还包括目标接口，所述目标接口用于可拆卸连接所述充电插头。可选的，目标接口可以为基于电磁性来进行清扫工具的可拆卸连接的部件。采用磁吸式结构抓取充电枪，保证充电插头和机械臂的可靠对接，然后进行下一步充电操作。
[0368]
在一种可能的实现中，在所述控制结构根据所述作用力信息，控制所述机械臂主体的位姿之前，所述目标接口与初始充电插头连接；所述控制结构还用于控制所述机械臂主体的位姿、以及所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆卸并替换为所述充电插头；其中，所述目标充电插头为支持快充充电的插头，所述充电插头为支持慢充充电的插头；或者，所述目标充电插头为支持慢充充电的插头，所述充电插头为支持快充充电的插头。
[0369]
本技术实施例中，充电设备可以兼容快充枪和慢充枪两种充电方式，可以具备快换机构，可根据用户任务需求选取充电枪。
[0370]
在一种可能的实现中，所述充电设备还包括：插头收纳区域，所述插头收纳区域包括用于设置所述充电插头的目标收纳位置；控制机构可以控制所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆卸；控制所述机械臂主体的位姿，将所述目标接口移动至所述目标收纳位置，并控制所述目标接口的连接状态，将所述目标接口与所述充电插头进行连接。
[0371]
本技术提供了一种充电设备，其特征在于，包括：机械臂、力传感器以及充电插头；所述机械臂包括控制结构以及机械臂主体；所述机械臂主体上设置有所述力传感器以及所述充电插头，所述充电插头包括多个充电头，所述力传感器用于采集所述充电插头和所述充电口之间接触面的作用力信息，所述接触面包括所述多个充电头中至少一个充电头的侧面，所述侧面为和所述充电头的轴向方向平行的表面；所述控制结构用于根据所述作用力信息，控制所述机械臂主体的位姿，以便将所述充电插头插入至所述充电口。在现有的只基于视觉进行位姿控制的方案中，由于视觉本身的误差，而且在充电插头在充电口附近的时候视觉相机已无法继续观测到充电口，如果只采用在最佳观测点观测到的视觉位姿进行充电插头的插拔，可能会发生不能插到底、楔牢的情况，本技术通过获取到力传感器采集的作用力信息来进行机械臂的位姿控制，可在充电插头已部分插入(例如侧面和充电口接触)，视觉信息无法获取误差的情况下，根据力觉调整位置和位姿，让充电插头适应充电口的插入方向，从而提高了插拔任务的准确性以及可靠性。减少了由于充电头侧面和充电口的内壁挤压或者摩擦时对充电头的磨损(或者是卡住而无法达到底部)的情况。
[0372]
本技术实施例通过视觉和力觉融合，可有效降低对视觉定位精度的要求，降低了成本。这是因为力柔顺控制可对充电枪的位姿根据力反馈的状况进行调整，即便充电枪和充电座存在误差，从力反馈可进行辨识并通过调整充电枪的位姿，消除了充电座识别误差带来的不利影响。因此，视觉力觉的融合避免采用昂贵的结构光相机，降低成本同时降低末端机构的体积和重量。此外，本技术实施例还克服了拖拽线缆的影响。由于拖拽线缆会对充电枪的位姿造成影响，结合力觉的方法能通过辨识定量观测到拖拽线缆力的影响并在控制的时候施加力补偿，有效克服拖拽线缆的影响。采用视觉方法可快速找到充电口并通过融合2d-3d信息实现充电座位姿的高精度识别，降低了机械臂802到充电口的全过程时间，同时由于视觉精度较高，也减少了力控搜索正确插拔位姿的时间，使得全流程速度有较大提升。
[0373]
此外，本技术实施例还提供了一种控制机器臂充电的方法，所述方法可以应用于上述实施例中所描述的机械臂802的控制结构，所述机械臂802主体上设置有力传感器801以及充电插头803，所述充电插头包括多个充电头，所述力传感器用于采集所述充电插头和所述充电口之间接触面的作用力信息，所述接触面包括所述多个充电头中至少一个充电头的侧面，所述侧面为和所述充电头的轴向方向平行的表面；
[0374]
所述方法包括：
[0375]
根据所述作用力信息，控制所述机械臂802主体的位姿，以便将所述充电插头803插入至所述充电口。
[0376]
在一种可能的实现中，所述作用力信息包括所述充电口对所述充电插头的力矩信
息。
[0377]
在一种可能的实现中，所述作用力信息包括所述充电头的径向方向的力。
[0378]
在一种可能的实现中，所述力传感器固定于所述至少一个充电头的侧面；或者，
[0379]
所述机械臂包括具备旋转自由度的关节，所述控制结构用于通过控制所述关节的旋转来控制所述机械臂主体的位姿，所述力传感器固定于所述关节上；或者，
[0380]
所述力传感器固定在所述充电插头和所述机械臂主体之间。
[0381]
在一种可能的实现中，所述充电口包括多个充电孔，所述多个充电头与所述多个充电孔之间一一匹配；所述根据所述作用力信息，控制所述机械臂主体的位姿，包括：
[0382]
根据所述作用力信息，确定位姿调整参数；
[0383]
根据所述位姿调整参数，将所述机械臂主体的位姿由第一位姿矫正为第二位姿；其中，所述第一位姿和所述第二位姿不同，且所述第二位姿为每个所述充电头均和相匹配的充电孔对齐时所述机械臂主体的位姿。
[0384]
在一种可能的实现中，所述第一位姿为所述多个充电头的底部端面未和所述充电孔的底部接触时的位姿，所述底部端面为和所述充电头的轴向方向垂直的表面。
[0385]
在一种可能的实现中，所述对齐包括：充电头沿相匹配的充电孔方向的投影落在所述相匹配的充电孔的范围内，且充电头的中心轴与相匹配的充电孔的中心轴重合或者偏差小于阈值。
[0386]
在一种可能的实现中，在将所述机械臂主体的位姿由第一位姿矫正为第二位姿之后，所述方法还包括：
[0387]
控制所述机械臂主体的位姿，以便所述充电插头朝向所述充电孔底部且沿着所述充电头的中心轴的方向移动。
[0388]
在一种可能的实现中，在所述由第一位姿矫正为第二位姿的过程中，所述充电口对所述充电插头在目标方向上的阻力存在降低的趋势，所述目标方向为所述充电头的轴向方向或径向方向。
[0389]
在一种可能的实现中，在所述由第一位姿矫正为第二位姿的过程中，所述充电口对所述充电插头之间的力矩存在降低的趋势。
[0390]
在一种可能的实现中，所述方法还包括：
[0391]
基于所述作用力信息指示所述充电口对所述充电插头仅存在沿所述充电插头的轴向方向的阻力，控制所述机械臂主体保持静止状态。
[0392]
在一种可能的实现中，所述位姿调整参数具体用于控制所述充电插头的移动方向以及移动速率。
[0393]
在一种可能的实现中，所述方法还包括：
[0394]
根据所述作用力信息，控制所述机械臂主体的位姿之前，实时获取视觉传感器针对于所述充电口的区域所采集的图像信息；
[0395]
根据所述图像信息中所述充电口的位置信息，得到所述机械臂主体的实时位姿；
[0396]
根据所述实时位姿，控制所述机械臂主体的位姿，以便将所述机械臂主体上的充电插头引导至所述充电口周围。
[0397]
在一种可能的实现中，所述机械臂主体上设置有所述视觉传感器。
[0398]
在一种可能的实现中，所述图像信息包括2d图像以及3d点云，所述方法还包括：
[0399]
根据所述2d图像，提取所述充电口对应的2d位置；
[0400]
根据所述3d点云，提取所述充电口对应的3d位置；
[0401]
将所述2d位置和所述3d位置进行融合，得到所述充电口的位置信息。
[0402]
在一种可能的实现中，所述机械臂主体还包括目标接口，所述目标接口用于可拆卸连接所述充电插头。
[0403]
在一种可能的实现中，在所述控制结构根据所述作用力信息，控制所述机械臂主体的位姿之前，所述目标接口与初始充电插头连接；
[0404]
所述方法还包括：控制所述机械臂主体的位姿、以及所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆卸并替换为所述充电插头；其中，
[0405]
所述目标充电插头为支持快充充电的插头，所述充电插头为支持慢充充电的插头；或者，
[0406]
所述目标充电插头为支持慢充充电的插头，所述充电插头为支持快充充电的插头。
[0407]
在一种可能的实现中，所述充电设备还包括：插头收纳区域，所述插头收纳区域包括用于设置所述充电插头的目标收纳位置；
[0408]
所述控制所述机械臂主体的位姿、以及所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆卸并替换为所述充电插头，包括：
[0409]
控制所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆卸；
[0410]
控制所述机械臂主体的位姿，将所述目标接口移动至所述目标收纳位置，并控制所述目标接口的连接状态，将所述目标接口与所述充电插头进行连接。
[0411]
在一种可能的实现中，所述充电设备为固定式充电桩、移动机器人或者充电车。
[0412]
关于控制机构的描述，可以参照上述实施例中关于机械臂802的控制结构的描述，这里不再赘述。
[0413]
本技术实施例还提供了一种控制机器臂充电的装置，所述装置应用于机械臂802的控制结构，所述机械臂主体上设置有力传感器以及充电插头，所述充电插头包括多个充电头，所述力传感器用于采集所述充电插头和所述充电口之间接触面的作用力信息，所述接触面包括所述多个充电头中至少一个充电头的侧面，所述侧面为和所述充电头的轴向方向平行的表面；
[0414]
所述装置包括：
[0415]
控制模块，用于根据所述作用力信息，控制所述机械臂802主体的位姿，以便将所述充电插头803插入至所述充电口。
[0416]
在一种可能的实现中，所述作用力信息包括所述充电头的径向方向的力。
[0417]
在一种可能的实现中，所述作用力信息包括所述充电口对所述充电插头的力矩信息。
[0418]
在一种可能的实现中，所述力传感器固定于所述至少一个充电头的侧面；或者，
[0419]
所述机械臂包括具备旋转自由度的关节，所述控制结构用于通过控制所述关节的旋转来控制所述机械臂主体的位姿，所述力传感器固定于所述关节上；或者，
[0420]
所述力传感器固定在所述充电插头和所述机械臂主体之间。
[0421]
在一种可能的实现中，所述充电口包括多个充电孔，所述多个充电头与所述多个充电孔之间一一匹配；所述控制模块，具体用于：
[0422]
根据所述作用力信息，确定位姿调整参数；
[0423]
根据所述位姿调整参数，将所述机械臂802主体的位姿由第一位姿矫正为第二位姿；其中，所述第一位姿和所述第二位姿不同，且所述第二位姿为每个所述充电头均和相匹配的充电孔对齐时所述机械臂802主体的位姿。
[0424]
在一种可能的实现中，所述第一位姿为所述多个充电头的底部端面未和所述充电孔的底部接触时的位姿，所述底部端面为和所述充电头的轴向方向垂直的表面。
[0425]
在一种可能的实现中，所述对齐包括：充电头沿相匹配的充电孔方向的投影落在所述相匹配的充电孔的范围内，且充电头的中心轴与相匹配的充电孔的中心轴重合或者偏差小于阈值。
[0426]
在一种可能的实现中，在将所述机械臂802主体的位姿由第一位姿矫正为第二位姿之后，所述控制模块，还用于：
[0427]
控制所述机械臂802主体的位姿，以便所述充电插头803朝向所述充电孔底部且沿着所述充电头的中心轴的方向移动。
[0428]
在一种可能的实现中，在所述由第一位姿矫正为第二位姿的过程中，所述充电口对所述充电头在目标方向上的阻力存在降低的趋势，所述目标方向为所述充电插头803的轴向方向或径向方向。
[0429]
在一种可能的实现中，在所述由第一位姿矫正为第二位姿的过程中，所述充电口对所述充电插头之间的力矩存在降低的趋势。
[0430]
在一种可能的实现中，所述控制模块，还用于：
[0431]
基于所述作用力信息指示所述充电口对所述充电头仅存在沿所述充电插头803的轴向方向的阻力，控制所述机械臂802主体保持静止状态。
[0432]
在一种可能的实现中，所述位姿调整参数具体用于控制所述充电插头803的移动方向以及移动速率。
[0433]
在一种可能的实现中，所述装置还包括：
[0434]
获取模块，用于根据所述作用力信息，控制所述机械臂802主体的位姿之前，实时获取视觉传感器804针对于所述充电口的区域所采集的图像信息；
[0435]
根据所述图像信息中所述充电口的位置信息，得到所述机械臂802主体的实时位姿；
[0436]
所述控制模块，还用于：
[0437]
根据所述实时位姿，控制所述机械臂802主体的位姿，以便将所述机械臂802主体上的充电插头803引导至所述充电口周围。
[0438]
在一种可能的实现中，所述机械臂802主体上设置有所述视觉传感器804。
[0439]
在一种可能的实现中，所述图像信息包括2d图像以及3d点云，所述获取模块，具体用于：
[0440]
根据所述2d图像，提取所述充电口对应的2d位置；
[0441]
根据所述3d点云，提取所述充电口对应的3d位置；
[0442]
将所述2d位置和所述3d位置进行融合，得到所述充电口的位置信息。
[0443]
在一种可能的实现中，所述机械臂主体还包括目标接口，所述目标接口用于可拆卸连接所述充电插头。
[0444]
在一种可能的实现中，在所述控制结构根据所述作用力信息，控制所述机械臂主体的位姿之前，所述目标接口与初始充电插头连接；
[0445]
所述方法还包括：控制所述机械臂主体的位姿、以及所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆卸并替换为所述充电插头；其中，
[0446]
所述目标充电插头为支持快充充电的插头，所述充电插头为支持慢充充电的插头；或者，
[0447]
所述目标充电插头为支持慢充充电的插头，所述充电插头为支持快充充电的插头。
[0448]
在一种可能的实现中，所述充电设备还包括：插头收纳区域，所述插头收纳区域包括用于设置所述充电插头的目标收纳位置；
[0449]
所述控制所述机械臂主体的位姿、以及所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆卸并替换为所述充电插头，包括：
[0450]
控制所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆卸；
[0451]
控制所述机械臂主体的位姿，将所述目标接口移动至所述目标收纳位置，并控制所述目标接口的连接状态，将所述目标接口与所述充电插头进行连接。
[0452]
在一种可能的实现中，所述充电设备为固定式充电桩、移动机器人或者充电车。
[0453]
接下来介绍本技术实施例提供的一种控制机械臂802充电的装置，请参阅图24，图24为本技术实施例提供的控制机械臂802充电的装置的一种结构示意图。具体的，控制机械臂802充电的装置2400包括：接收器2401、发射器2402、处理器2403和存储器2404(其中控制机械臂802充电的装置2400中的处理器2403的数量可以一个或多个，图24中以一个处理器为例)，其中，处理器2403可以包括应用处理器24031和通信处理器24032。在本技术的一些实施例中，接收器2401、发射器2402、处理器2403和存储器2404可通过总线或其它方式连接。
[0454]
存储器2404可以包括只读存储器和随机存取存储器，并向处理器2403提供指令和数据。存储器2404的一部分还可以包括非易失性随机存取存储器(non-volatile random access memory，nvram)。存储器2404存储有处理器和操作指令、可执行模块或者数据结构，或者它们的子集，或者它们的扩展集，其中，操作指令可包括各种操作指令，用于实现各种操作。
[0455]
处理器2403控制雷达系统(包括天线、接收器2401以及发射器2402)的操作。具体的应用中，雷达系统的各个组件通过总线系统耦合在一起，其中总线系统除包括数据总线之外，还可以包括电源总线、控制总线和状态信号总线等。但是为了清楚说明起见，在图中将各种总线都称为总线系统。
[0456]
上述本技术实施例揭示的控制机械臂802充电的方法(图7所示的)可以应用于处理器2403中，或者由处理器2403实现。处理器2403可以是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器2403中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器2403可以是通用处理器、数字信号处理器
(digital signal processing，dsp)、微处理器或微控制器，还可进一步包括专用集成电路(application specific integrated circuit，asic)、现场可编程门阵列(field-programmable gate array，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。该处理器2403可以实现或者执行本技术实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本技术实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器2404，处理器2403读取存储器2404中的信息，结合其硬件完成上述实施例提供的控制机械臂802充电的方法的步骤。
[0457]
接收器2401可用于接收输入的数字或字符信息，以及产生与雷达系统的相关设置以及功能控制有关的信号输入。发射器2402可用于通过第一接口输出数字或字符信息；发射器2402还可用于通过第一接口向磁盘组发送指令，以修改磁盘组中的数据。
[0458]
本技术实施例中还提供一种包括计算机程序产品，当其在计算机上运行时，使得计算机执行上述实施例中描述的控制机械臂802充电的方法。
[0459]
本技术实施例中还提供一种计算机可读存储介质，该计算机可读存储介质中存储有用于进行信号处理的程序，当其在计算机上运行时，使得计算机执行如上述实施例中描述的控制机械臂802充电的方法。
[0460]
本技术实施例提供的控制机械臂802充电的装置具体可以为芯片，芯片包括：处理单元和通信单元，该处理单元例如可以是处理器，该通信单元例如可以是输入/输出接口、管脚或电路等。该处理单元可执行存储单元存储的计算机执行指令，以使执行设备内的芯片执行上述实施例描述的图像增强方法，或者，以使训练设备内的芯片执行上述实施例描述的图像增强方法。可选地，该存储单元为该芯片内的存储单元，如寄存器、缓存等，该存储单元还可以是该无线接入设备端内的位于该芯片外部的存储单元，如只读存储器(read-only memory，rom)或可存储静态信息和指令的其他类型的静态存储设备，随机存取存储器(random access memory，ram)等。
[0461]
具体的，请参阅图25，图25为本技术实施例提供的芯片的一种结构示意图，该芯片可以表现为神经网络处理器npu250，npu 250作为协处理器挂载到主cpu(host cpu)上，由host cpu分配任务。npu的核心部分为运算电路2503，通过控制器2504控制运算电路2503提取存储器中的矩阵数据并进行乘法运算。
[0462]
在一些实现中，运算电路2503内部包括多个处理单元(process engine,pe)。在一些实现中，运算电路2503是二维脉动阵列。运算电路2503还可以是一维脉动阵列或者能够执行例如乘法和加法这样的数学运算的其它电子线路。在一些实现中，运算电路2503是通用的矩阵处理器。
[0463]
举例来说，假设有输入矩阵a，权重矩阵b，输出矩阵c。运算电路从权重存储器2502中取矩阵b相应的数据，并缓存在运算电路中每一个pe上。运算电路从输入存储器2501中取矩阵a数据与矩阵b进行矩阵运算，得到的矩阵的部分结果或最终结果，保存在累加器(accumulator)2508中。
[0464]
统一存储器2506用于存放输入数据以及输出数据。权重数据直接通过存储单元访
问控制器(direct memory access controller，dmac)2505，dmac被搬运到权重存储器2502中。输入数据也通过dmac被搬运到统一存储器2506中。
[0465]
biu为bus interface unit即，总线接口单元2510，用于axi总线与dmac和取指存储器(instruction fetch buffer，ifb)2509的交互。
[0466]
总线接口单元2510(bus interface unit，简称biu)，用于取指存储器2509从外部存储器获取指令，还用于存储单元访问控制器2505从外部存储器获取输入矩阵a或者权重矩阵b的原数据。
[0467]
dmac主要用于将外部存储器ddr中的输入数据搬运到统一存储器2506或将权重数据搬运到权重存储器2502中或将输入数据数据搬运到输入存储器2501中。
[0468]
向量计算单元2507包括多个运算处理单元，在需要的情况下，对运算电路的输出做进一步处理，如向量乘，向量加，指数运算，对数运算，大小比较等等。主要用于神经网络中非卷积/全连接层网络计算，如batch normalization(批归一化)，像素级求和，对特征平面进行上采样等。
[0469]
在一些实现中，向量计算单元2507能将经处理的输出的向量存储到统一存储器2506。例如，向量计算单元2507可以将线性函数和/或非线性函数应用到运算电路2503的输出，例如对卷积层提取的特征平面进行线性插值，再例如累加值的向量，用以生成激活值。在一些实现中，向量计算单元2507生成归一化的值、像素级求和的值，或二者均有。在一些实现中，处理过的输出的向量能够用作到运算电路2503的激活输入，例如用于在神经网络中的后续层中的使用。
[0470]
控制器2504连接的取指存储器(instruction fetch buffer)2509，用于存储控制器2504使用的指令；
[0471]
统一存储器2506，输入存储器2501，权重存储器2502以及取指存储器2509均为on-chip存储器。外部存储器私有于该npu硬件架构。
[0472]
其中，上述任一处提到的处理器，可以是一个通用中央处理器，微处理器，asic，或一个或多个用于控制上述实施例中描述的控制机械臂802充电的方法相关步骤的程序执行的集成电路。
[0473]
另外需说明的是，以上所描述的装置实施例仅仅是示意性的，其中该作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。另外，本技术提供的装置实施例附图中，模块之间的连接关系表示它们之间具有通信连接，具体可以实现为一条或多条通信总线或信号线。
[0474]
通过以上的实施方式的描述，所属领域的技术人员可以清楚地了解到本技术可借助软件加必需的通用硬件的方式来实现，当然也可以通过专用硬件包括专用集成电路、专用cpu、专用存储器、专用元器件等来实现。一般情况下，凡由计算机程序完成的功能都可以很容易地用相应的硬件来实现，而且，用来实现同一功能的具体硬件结构也可以是多种多样的，例如模拟电路、数字电路或专用电路等。但是，对本技术而言更多情况下软件程序实现是更佳的实施方式。基于这样的理解，本技术的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在可读取的存储介质
中，如计算机的软盘、u盘、移动硬盘、rom、ram、磁碟或者光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，训练设备，或者网络设备等)执行本技术各个实施例该的方法。
[0475]
在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时，可以全部或部分地以计算机程序产品的形式实现。
[0476]
该计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行该计算机程序指令时，全部或部分地产生按照本技术实施例该的流程或功能。该计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。该计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一计算机可读存储介质传输，例如，该计算机指令可以从一个网站站点、计算机、训练设备或数据中心通过有线(例如同轴电缆、光纤、数字用户线(dsl))或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、训练设备或数据中心进行传输。该计算机可读存储介质可以是计算机能够存储的任何可用介质或者是包含一个或多个可用介质集成的训练设备、数据中心等数据存储设备。该可用介质可以是磁性介质，(例如，软盘、硬盘、磁带)、光介质(例如，dvd)、或者半导体介质(例如固态硬盘(solid state disk，ssd))等。

技术特征：
1.一种充电设备，其特征在于，包括：机械臂、力传感器以及充电插头；所述机械臂包括控制结构以及机械臂主体；所述机械臂主体上设置有所述力传感器以及所述充电插头，所述充电插头包括多个充电头，所述力传感器用于采集所述充电插头和所述充电口之间接触面的作用力信息，所述接触面包括所述多个充电头中至少一个充电头的侧面，所述侧面为和所述充电头的轴向方向平行的表面；所述控制结构用于根据所述作用力信息，控制所述机械臂主体的位姿，以便将所述充电插头插入至所述充电口。2.根据权利要求1所述的充电设备，其特征在于，所述作用力信息包括所述充电口对所述充电插头的力矩信息。3.根据权利要求1或2所述的充电设备，其特征在于，所述力传感器固定于所述至少一个充电头的侧面；或者，所述机械臂包括具备旋转自由度的关节，所述控制结构用于通过控制所述关节的旋转来控制所述机械臂主体的位姿，所述力传感器固定于所述关节上；或者，所述力传感器固定在所述充电插头和所述机械臂主体之间。4.根据权利要求1至3任一所述的充电设备，其特征在于，所述充电口包括多个充电孔，所述多个充电头与所述多个充电孔之间一一匹配；所述控制结构具体用于：根据所述作用力信息，确定位姿调整参数；根据所述位姿调整参数，将所述机械臂主体的位姿由第一位姿矫正为第二位姿；其中，所述第一位姿和所述第二位姿不同，且所述第二位姿为每个所述充电头均和相匹配的充电孔对齐时所述机械臂主体的位姿。5.根据权利要求4所述的充电设备，其特征在于，所述第一位姿为所述多个充电头的底部端面未和所述充电孔的底部接触时的位姿，所述底部端面为和所述充电头的轴向方向垂直的表面。6.根据权利要求4或5所述的充电设备，其特征在于，所述对齐包括：充电头沿相匹配的充电孔方向的投影落在所述相匹配的充电孔的范围内，且充电头的中心轴与相匹配的充电孔的中心轴重合或者偏差小于阈值。7.根据权利要求4至6任一所述的充电设备，其特征在于，所述控制结构还用于：在将所述机械臂主体的位姿由第一位姿矫正为所述第二位姿之后，控制所述机械臂主体的位姿，以便所述充电插头朝向所述充电孔底部且沿着所述充电头的中心轴的方向移动。8.根据权利要求4至7任一所述的充电设备，其特征在于，在所述由第一位姿矫正为第二位姿的过程中，所述充电口对所述充电插头之间的力矩存在降低的趋势。9.根据权利要求4至8任一所述的充电设备，其特征在于，所述位姿调整参数具体用于控制所述充电插头的移动方向以及移动速率。10.根据权利要求1至9任一所述的充电设备，其特征在于，所述控制结构具体用于：基于所述作用力信息指示所述充电口对所述充电插头仅存在沿所述充电插头的轴向方向的阻力，控制所述机械臂主体保持静止状态。11.根据权利要求1至10任一所述的充电设备，其特征在于，所述控制机构还用于：
根据所述作用力信息，控制所述机械臂主体的位姿之前，实时获取视觉传感器针对于所述充电口的区域所采集的图像信息；根据所述图像信息中所述充电口的位置信息，得到所述机械臂主体的实时位姿；根据所述实时位姿，控制所述机械臂主体的位姿，以便将所述机械臂主体上的充电插头引导至所述充电口周围。12.根据权利要求11所述的充电设备，其特征在于，所述机械臂主体上设置有所述视觉传感器。13.根据权利要求11或12所述的充电设备，其特征在于，所述图像信息包括2d图像以及3d点云，所述控制机构还用于：根据所述2d图像以及预设的充电口3d模型，提取所述充电口对应的第一3d位置；根据所述3d点云，提取所述充电口对应的第二3d位置；将所述第一3d位置和所述第二3d位置进行融合，得到所述充电口的位姿。14.根据权利要求13所述的充电设备，其特征在于，所述第一3d位置包括所述充电口上m个第一特征点的3d位置；所述第二3d位置包括所述充电口上m个第二特征点的3d位置；所述m个第一特征点和所述m个第二特征点一一对应；所述将所述第一3d位置和所述第二3d位置进行融合，包括：将所述m个第一特征点的3d位置分别和对应的所述第二特征点的3d位置进行融合。15.根据权利要求1至14任一所述的充电设备，其特征在于，所述机械臂主体还包括目标接口，所述目标接口用于可拆卸连接所述充电插头。16.根据权利要求15所述的充电设备，其特征在于，在所述控制结构根据所述作用力信息，控制所述机械臂主体的位姿之前，所述目标接口与初始充电插头连接；所述控制结构还用于控制所述机械臂主体的位姿、以及所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆卸并替换为所述充电插头；其中，所述目标充电插头为支持快充充电的插头，所述充电插头为支持慢充充电的插头；或者，所述目标充电插头为支持慢充充电的插头，所述充电插头为支持快充充电的插头。17.根据权利要求16所述的充电设备，其特征在于，所述充电设备还包括：插头收纳区域，所述插头收纳区域包括用于设置所述充电插头的目标收纳位置；所述控制所述机械臂主体的位姿、以及所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆卸并替换为所述充电插头，包括：控制所述目标接口的连接状态，将所述初始充电插头从所述目标接口上进行拆卸；控制所述机械臂主体的位姿，将所述目标接口移动至所述目标收纳位置，并控制所述目标接口的连接状态，将所述目标接口与所述充电插头进行连接。18.根据权利要求1至17任一所述的充电设备，其特征在于，所述充电设备为固定式充电桩、移动机器人或者充电车。19.一种控制机器臂充电的方法，其特征在于，所述方法应用于机械臂的控制结构，所述机械臂主体上设置有力传感器以及充电插头，所述充电插头包括多个充电头，所述力传感器用于采集所述充电插头和所述充电口之间接触面的作用力信息，所述接触面包括所述多个充电头中至少一个充电头的侧面，所述侧面为和所述充电头的轴向方向平行的表面；
所述方法包括：根据所述作用力信息，控制所述机械臂主体的位姿，以便将所述充电插头插入至所述充电口。20.根据权利要求19所述的方法，其特征在于，所述作用力信息包括所述充电口对所述充电插头的力矩信息。21.根据权利要求19或20所述的方法，其特征在于，所述充电口包括多个充电孔，所述多个充电头与所述多个充电孔之间一一匹配；所述根据所述作用力信息，控制所述机械臂主体的位姿，包括：根据所述作用力信息，确定位姿调整参数；根据所述位姿调整参数，将所述机械臂主体的位姿由第一位姿矫正为第二位姿；其中，所述第一位姿和所述第二位姿不同，且所述第二位姿为每个所述充电头均和相匹配的充电孔对齐时所述机械臂主体的位姿。22.根据权利要求21所述的方法，其特征在于，所述第一位姿为所述多个充电头的底部端面未和所述充电孔的底部接触时的位姿，所述底部端面为和所述充电头的轴向方向垂直的表面。23.根据权利要求21或22所述的方法，其特征在于，在将所述机械臂主体的位姿由第一位姿矫正为第二位姿之后，所述方法还包括：控制所述机械臂主体的位姿，以便所述充电插头朝向所述充电孔底部且沿着所述充电头的中心轴的方向移动。24.根据权利要求19至23任一所述的方法，其特征在于，所述方法还包括：基于所述作用力信息指示所述充电口对所述充电插头仅存在沿所述充电插头的轴向方向的阻力，控制所述机械臂主体保持静止状态。25.根据权利要求19至24任一所述的方法，其特征在于，所述方法还包括：根据所述作用力信息，控制所述机械臂主体的位姿之前，实时获取视觉传感器针对于所述充电口的区域所采集的图像信息；根据所述图像信息中所述充电口的位置信息，得到所述机械臂主体的实时位姿；根据所述实时位姿，控制所述机械臂主体的位姿，以便将所述机械臂主体上的充电插头引导至所述充电口周围。26.根据权利要求25所述的方法，其特征在于，所述图像信息包括2d图像以及3d点云，所述方法还包括：根据所述2d图像以及预设的充电口3d模型，提取所述充电口对应的第一3d位置；根据所述3d点云，提取所述充电口对应的第二3d位置；将所述第一3d位置和所述第二3d位置进行融合，得到所述充电口的位姿。27.根据权利要求26所述的方法，其特征在于，所述第一3d位置包括所述充电口上m个第一特征点的3d位置；所述第二3d位置包括所述充电口上m个第二特征点的3d位置；所述m个第一特征点和所述m个第二特征点一一对应；所述将所述第一3d位置和所述第二3d位置进行融合，包括：将所述m个第一特征点的3d位置分别和对应的所述第二特征点的3d位置进行融合。28.一种计算机可读存储介质，其特征在于，包括计算机可读指令，当所述计算机可读
指令在计算机设备上运行时，使得所述计算机设备执行权利要求19至27任一项所述的方法。29.一种计算机程序产品，其特征在于，包括计算机可读指令，当所述计算机可读指令在计算机设备上运行时，使得所述计算机设备执行如权利要求19至27任一所述的方法。

技术总结
本申请实施例公开了一种充电设备，包括：控制结构以及设置有力传感器以及充电插头的机械臂主体，力传感器用于在充电插头与充电口接触后采集充电头的侧面和充电口之间的作用力信息；控制结构可以根据作用力信息，控制机械臂主体的位姿，以便将充电插头插入至充电口。本申请通过获取到力传感器采集的作用力信息来进行机械臂的位姿控制，可在充电插头已部分插入，视觉信息无法获取误差的情况下，根据力觉调整位置和位姿，让充电插头适应充电口的插入方向，从而提高了插拔任务的准确性以及可靠性。靠性。靠性。


技术研发人员：陈辰 楚亚奎 薛景涛 贺亚农 陈晓鹏 黄远灿 张伟民
受保护的技术使用者：北京理工大学
技术研发日：2021.12.20
技术公布日：2023/6/27