自动充电机械臂的定位对接系统的控制方法和装置

摘要

本发明提供了一种自动充电机械臂的定位对接系统的控制方法和装置，系统包括：设置在车载端的M个超声波发射探头和设置在自动充电机械臂上的N个超声波接收探头，其中，M个超声波发射探头的探测区域构成工作区域，所述工作区域包括定位区域和非定位区域；方法包括：控制自动充电机械臂工作，以使N个超声波接收探头运动至工作区域；根据N个超声波接收探头接收到探测信号的时间计算自动充电机械臂的第一运动参数，并根据第一运动参数对自动充电机械臂进行相应控制，以使N个超声波接收探头运动至定位区域。由此，能够准确地控制自动充电机械臂上的超声波接收探头运动至定位区域，从而确保自动充电机械臂与车载端连接器能够进行准确地对接。

权利要求

1.一种自动充电机械臂的定位对接系统的控制方法，其特征在于，所述自动充电机械臂的定位对接系统包括：M个超声波发射探头，所述M个超声波发射探头设置在车载端，所述M个超声波发射探头用于发射探测信号，其中，所述M个超声波发射探头的探测区域构成工作区域，所述工作区域包括定位区域和非定位区域，M为大于或等于4的正整数；N个超声波接收探头，所述N个超声波接收探头依次设置在地面端的所述自动充电机械臂上，所述N个超声波接收探头用于接收所述M个超声波发射探头发射的探测信号，其中，N为大于或等于2的正整数；其中，所述自动充电机械臂的定位对接系统的控制方法包括以下步骤：控制所述自动充电机械臂工作，以使所述N个超声波接收探头运动至所述工作区域；

根据所述N个超声波接收探头接收到所述探测信号的时间计算所述自动充电机械臂的第一运动参数，并根据所述第一运动参数对所述自动充电机械臂进行相应控制，以使所述N个超声波接收探头运动至所述定位区域；其中，所述根据所述N个超声波接收探头接收到所述探测信号的时间计算所述自动充电机械臂的第一运动参数，并根据所述第一运动参数对所述自动充电机械臂进行相应控制，以使所述N个超声波接收探头运动至所述定位区域，包括：获取所述N个超声波接收探头中第一个超声波接收探头接收到所述探测信号的时间；

根据所述第一个超声波接收探头接收到所述探测信号的时间判断所述第一个超声波接收探头是否处于定位区域；

如果否，则根据所述第一个超声波接收探头接收到所述探测信号的时间获取所述第一个超声波接收探头在所述工作区域内的方位信息，并根据所述第一个超声波接收探头在所述工作区域内的方位信息计算所述自动充电机械臂的第一运动参数，以及根据所述第一运动参数对所述自动充电机械臂进行相应控制，以使所述第一个超声波接收探头运动至所述定位区域；

如果是，则获取所述N个超声波接收探头中第二个超声波接收探头接收到所述探测信号的时间，并根据所述第二个超声波接收探头接收到所述探测信号的时间判断所述第二个超声波接收探头是否处于定位区域；其中，所述自动充电机械臂的定位对接系统的控制方法还包括：在所述N个超声波接收探头运动至所述定位区域内时，获取n个超声波接收探头的坐标信息；

根据所述n个超声波接收探头的坐标信息获取车载端连接器中心点的坐标信息，以及根据所述车载端连接器中心点的坐标信息计算所述自动充电机械臂的第二运动参数，其中，n为大于或等于2且小于或等于N的正整数；

根据所述第二运动参数对所述自动充电机械臂进行相应控制，以使所述自动充电机械臂与所述车载端连接器对接。

2.根据权利要求1所述的自动充电机械臂的定位对接系统的控制方法，其特征在于，所述根据所述N个超声波接收探头接收到所述探测信号的时间计算所述自动充电机械臂的第一运动参数，并根据所述第一运动参数对所述自动充电机械臂进行相应控制，以使所述N个超声波接收探头运动至所述定位区域，还包括：如果判断所述第二个超声波接收探头处于定位区域，则获取所述N个超声波接收探头中第i个超声波接收探头接收到所述探测信号的时间，并根据所述第i个超声波接收探测接收到所述探测信号的时间判断所述第i个超声波接收探头是否处于所述定位区域，直至判断出所述N个超声波接收探头均处于所述定位区域，其中，i为大于或等于3且小于或等于N的正整数。

3.根据权利要求1所述的自动充电机械臂的定位对接系统的控制方法，其特征在于，所述获取n个超声波接收探头的坐标信息，包括：获取所述n个超声波接收探头在地面端坐标系中的第一坐标，并获取所述n个超声波接收探头在车载端坐标系中的第二坐标。

4.根据权利要求3所述的自动充电机械臂的定位对接系统的控制方法，其特征在于，所述根据所述n个超声波接收探头的坐标信息获取车载端连接器中心点的坐标信息，包括：根据所述第一坐标和所述第二坐标采用向量转换的方式计算所述车载端连接器中心点在所述地面端坐标系中的坐标。

5.一种自动充电机械臂的定位对接系统的控制装置，其特征在于，所述自动充电机械臂的定位对接系统包括：M个超声波发射探头，所述M个超声波发射探头设置在车载端，所述M个超声波发射探头用于发射探测信号，其中，所述M个超声波发射探头的探测区域构成工作区域，所述工作区域包括定位区域和非定位区域，M为大于或等于4的正整数；N个超声波接收探头，所述N个超声波接收探头依次设置在地面端的所述自动充电机械臂上，所述N个超声波接收探头用于接收所述M个超声波发射探头发射的探测信号，其中，N为大于或等于2的正整数；其中，所述自动充电机械臂的定位对接系统的控制装置包括：第一控制模块，所述第一控制模块用于控制所述自动充电机械臂工作，以使所述N个超声波接收探头运动至所述工作区域；

第二控制模块，所述第二控制模块用于根据所述N个超声波接收探头接收到所述探测信号的时间计算所述自动充电机械臂的第一运动参数，并根据所述第一运动参数对所述自动充电机械臂进行相应控制，以使所述N个超声波接收探头运动至所述定位区域，其中，所述第二控制模块具体用于：获取所述N个超声波接收探头中第一个超声波接收探头接收到所述探测信号的时间；

根据所述第一个超声波接收探头接收到所述探测信号的时间判断所述第一个超声波接收探头是否处于定位区域；

如果否，则根据所述第一个超声波接收探头接收到所述探测信号的时间获取所述第一个超声波接收探头在所述工作区域内的方位信息，并根据所述第一个超声波接收探头在所述工作区域内的方位信息计算所述自动充电机械臂的第一运动参数，以及根据所述第一运动参数对所述自动充电机械臂进行相应控制，以使所述第一个超声波接收探头运动至所述定位区域；

如果是，则获取所述N个超声波接收探头中第二个超声波接收探头接收到所述探测信号的时间，并根据所述第二个超声波接收探头接收到所述探测信号的时间判断所述第二个超声波接收探头是否处于定位区域；

获取模块，所述获取模块用于在所述N个超声波接收探头运动至所述定位区域内时，获取n个超声波接收探头的坐标信息；

计算模块，所述计算模块用于根据所述n个超声波接收探头的坐标信息获取车载端连接器中心点的坐标信息，以及根据所述车载端连接器中心点的坐标信息计算所述自动充电机械臂的第二运动参数，其中，n为大于或等于2且小于或等于N的正整数；

第三控制模块，所述第三控制模块用于根据所述第二运动参数对所述自动充电机械臂进行相应控制，以使所述自动充电机械臂与所述车载端连接器对接。

6.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时，实现根据权利要求1‑4中任一项所述的自动充电机械臂的定位对接系统的控制方法。

7.一种非临时性计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现根据权利要求1‑4中任一项所述的自动充电机械臂的定位对接系统的控制方法。

说明书

自动充电机械臂的定位对接系统的控制方法和装置

技术领域

[0001] 本发明涉自动充电技术领域，具体涉及一种自动充电机械臂的定位对接系统的控制方法和一种自动充电机械臂的定位对接系统的控制装置。

背景技术

[0002] 目前，工业上机械臂的运动定位主要采用视觉定位的方式，不仅大大增加了生产成本，而且对探测进行防护设计的难度也较大。

[0003] 相关技术中，可采用超声波定位的方式实现机械臂的定位控制，然而，在实际应用中，由于超声波探头的波束角的限制，因此构成的可定位区域是有限的，存在一定的定位盲区，从而无法准确地控制机械臂运动至可定位区域，进而导致机械臂与车载端连接器无法实现准确的对接。

发明内容

[0004] 本发明为解决上述技术问题，提供了一种自动充电机械臂的定位对接系统的控制方法，在定位盲区内，能够准确地控制自动充电机械臂上的超声波接收探头运动至定位区域，从而确保自动充电机械臂与车载端连接器能够进行准确地对接。

[0005] 本发明采用的技术方案如下：

[0006] 一种自动充电机械臂的定位对接系统的控制方法，自动充电机械臂的定位对接系统包括：M个超声波发射探头，M个超声波发射探头设置在车载端，M个超声波发射探头用于发射探测信号，其中，M个超声波发射探头的探测区域构成工作区域，工作区域包括定位区域和非定位区域，M为大于或等于4的正整数；N个超声波接收探头，N个超声波接收探头依次设置在地面端的自动充电机械臂上，N个超声波接收探头用于接收M个超声波发射探头发射的探测信号，其中，N为大于或等于2的正整数；其中，自动充电机械臂的定位对接系统的控制方法包括以下步骤：控制自动充电机械臂工作，以使N个超声波接收探头运动至工作区域；根据N个超声波接收探头接收到探测信号的时间计算自动充电机械臂的第一运动参数，并根据第一运动参数对自动充电机械臂进行相应控制，以使N个超声波接收探头运动至定位区域。

[0007] 自动充电机械臂的定位对接系统的控制方法，还包括：在所述N个超声波接收探头运动至所述定位区域内时，获取n个超声波接收探头的坐标信息；根据所述n个超声波接收探头的坐标信息获取车载端连接器中心点的坐标信息，以及根据所述车载端连接器中心点的坐标信息计算所述自动充电机械臂的第二运动参数，其中，n为大于或等于2且小于或等于N的正整数；根据所述第二运动参数对所述自动充电机械臂进行相应控制，以使所述自动充电机械臂与所述车载端连接器对接。

[0008] 所述根据所述N个超声波接收探头接收到所述探测信号的时间计算所述自动充电机械臂的第一运动参数，并根据所述第一运动参数对所述自动充电机械臂进行相应控制，以使所述N个超声波接收探头运动至所述定位区域，包括：获取所述N个超声波接收探头中第一个超声波接收探头接收到所述探测信号的时间；根据所述第一个超声波接收探头接收到所述探测信号的时间判断所述第一个超声波接收探头是否处于定位区域；如果否，则根据所述第一个超声波接收探头接收到所述探测信号的时间获取所述第一个超声波接收探头在所述工作区域内的方位信息，并根据所述第一个超声波接收探头在所述工作区域内的方位信息计算所述自动充电机械臂的第一运动参数，以及根据所述第一运动参数对所述自动充电机械臂进行相应控制，以使所述第一个超声波接收探头运动至所述定位区域；如果是，则获取所述N个超声波接收探头中第二个超声波接收探头接收到所述探测信号的时间，并根据所述第二个超声波接收探头接收到所述探测信号的时间判断所述第二个超声波接收探头是否处于定位区域。

[0009] 所述根据所述N个超声波接收探头接收到所述探测信号的时间计算所述自动充电机械臂的第一运动参数，并根据所述第一运动参数对所述自动充电机械臂进行相应控制，以使所述N个超声波接收探头运动至所述定位区域，还包括：如果判断所述第二个超声波接收探头处于定位区域，则获取所述N个超声波接收探头中第i个超声波接收探头接收到所述探测信号的时间，并根据所述第i个超声波接收探测接收到所述探测信号的时间判断所述第i个超声波接收探头是否处于所述定位区域，直至判断出所述N个超声波接收探头均处于所述定位区域，其中，i为大于或等于3且小于或等于N的正整数。

[0010] 所述获取n个超声波接收探头的坐标信息，包括：获取所述n个超声波接收探头在地面端坐标系中的第一坐标，并获取所述n个超声波接收探头在车载端坐标系中的第二坐标。

[0011] 所述根据所述n个超声波接收探头的坐标信息获取车载端连接器中心点的坐标信息，包括：根据所述第一坐标和所述第二坐标采用向量转换的方式计算所述车载端连接器中心点在所述地面端坐标系中的坐标。

[0012] 一种自动充电机械臂的定位对接系统的控制装置，所述自动充电机械臂的定位对接系统包括：M个超声波发射探头，所述M个超声波发射探头设置在车载端，所述M个超声波发射探头用于发射探测信号，其中，所述M个超声波发射探头的探测区域构成工作区域，所述工作区域包括定位区域和非定位区域，M为大于或等于4的正整数；N个超声波接收探头，所述N个超声波接收探头依次设置在地面端的所述自动充电机械臂上，所述N个超声波接收探头用于接收所述M个超声波发射探头发射的探测信号，其中，N为大于或等于2的正整数；其中，所述自动充电机械臂的定位对接系统的控制装置包括：第一控制模块，所述第一控制模块用于控制所述自动充电机械臂工作，以使所述N个超声波接收探头运动至所述工作区域；第二控制模块，所述第二控制模块用于根据所述N个超声波接收探头接收到所述探测信号的时间计算所述自动充电机械臂的第一运动参数，并根据所述第一运动参数对所述自动充电机械臂进行相应控制，以使所述N个超声波接收探头运动至所述定位区域。

[0013] 一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时，实现上述的自动充电机械臂的定位对接系统的控制方法。

[0014] 一种非临时性计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述的自动充电机械臂的定位对接系统的控制方法。

[0015] 本发明的有益效果：

[0016] 本发明在定位盲区内，能够准确地控制自动充电机械臂上的超声波接收探头运动至定位区域，从而确保自动充电机械臂与车载端连接器能够进行准确地对接。

附图说明

[0017] 图1为本发明实施例的自动充电机械臂的定位对接系统的控制方法的流程图；

[0018] 图2a为本发明一个具体实施例的将车载端作为定位发射端所获取的定位区域的范围的示意图；

[0019] 图2b为本发明一个具体实施例的将车载端作为定位接收端所获取的定位区域的范围的示意图；

[0020] 图3为本发明一个具体实施例的4个超声波发射探头的探测区域所构成的工作区域的示意图；

[0021] 图4为本发明一个具体实施例的自动充电机械臂的运动模型的示意图；

[0022] 图5为本发明一个具体实施例的各超声波接收探头、两级旋转机械臂各相关节点以及地面端连接器中心点在地面端坐标系中的坐标的示意图；

[0023] 图6为本发明一个具体实施例的车载端坐标系与地面端坐标系之间的转换示意图；

[0024] 图7为本发明实施例的自动充电机械臂的定位对接系统的控制装置的方框示意图；

[0025] 图8为本发明一个实施例的自动充电机械臂的定位对接系统的控制装置的方框示意图。

具体实施方式

[0026] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

[0027] 图1是根据本发明实施例的自动充电机械臂的定位对接系统的控制方法的流程图其中，本发明实施例中的自动充电机械臂可为多级旋转机械臂，例如，可为两级旋转机械臂。

[0028] 目前，为了解决工业上采用视觉定位的方式对自动充电机械臂进行运动定位导致生成成本大、防护设计难度高的问题，可以采用超声波定位的方式对自动充电机械臂进行定位控制，例如，采用三发一收(车载端设置三个超声波发射探头，地面端设置一个超声波接收探头)，或者三收一发(车载端设置三个超声波接收探头，地面端设置一个超声波发射探头)的方式进行定位控制。

[0029] 在实际应用过程中，超声波发射探头具有波束角限制，其构成的可定位区域是有限的，存在一定的定位盲区，因此，自动充电机械臂需要进行一定时间的盲动运行至可定位区域，才能对自动充电机械臂进行定位。

[0030] 然而，相关技术中，在定位盲区内，无法准确地控制自动充电机械臂进行运行至可定位区域，从而无法实现对自动充电机械臂进行准确的定位，进而无法确保自动充电机械臂与车载端连接器能够进行准确地对接。

[0031] 为此，本发明实施例提出了一种自动充电机械臂的定位对接系统的控制方法，在定位盲区内，能够准确地控制自动充电机械臂上的超声波接收探头运动至定位区域，从而确保自动充电机械臂与车载端连接器能够进行准确地对接。

[0032] 需要说明的是，在结构上车载端(车载单元)适合安装多个超声波探头，构成车载定位坐标系(车载端坐标系)，车载端既可以做定位发射端也可以做定位接收端，其中，如图2a和2b所示，图2a为将车载端作为定位发射端所获取的定位区域的范围的示意图，图2b为将车载端作为定位接收端所获取的定位区域的范围的示意图，由对比可以发现，车载做定位发射端时所获取的定位区域的范围明显大于车载做定位接收端所获取的定位区域的范围，因此，在本发明实施例的自动充电机械臂的定位对接系统可包括：M个超声波发射探头和N个超声波接收探头，其中，M个超声波发射探头设置在车载端，M个超声波发射探头用于发射探测信号，其中，M个超声波发射探头的探测区域构成工作区域，工作区域包括定位区域和非定位区域，M为大于或等于4的正整数；N个超声波接收探头依次设置在地面端的自动充电机械臂上，N个超声波接收探头用于接收M个超声波发射探头发射的探测信号，其中，N为大于或等于2的正整数。举例而言，可采用4个超声波发射探头设置在车载端，2个超声波接收探头依次设置在地面端的自动充电机械臂上。

[0033] 如图1所示，本发明实施例的自动充电机械臂的定位对接系统的控制方法可包括以下步骤：

[0034] S1，控制自动充电机械臂工作，以使N个超声波接收探头运动至工作区域。

[0035] 具体而言，定位流程启动后，可先通过控制模块控制自动充电机械臂进行旋转工作，以使自动充电机械臂上的接收探头运动到车位中标定的工作区域内，即M个超声波发射探头的探测区域所构成工作区域，其中，在工作区域内超声波接收探头能够接收到至少一个超声波发射探头的探测信号。举例而言，当M为4时，如图3所示，超声波发射探头#1、#2、#3和#4(雷达#1、#2、#3和#4)的探测区域可构成工作区域，其中，至少三个超声波发射探头共同探测的区域可构成定位区域，其他区域可为非定位区域(包括单雷达探测区域和双雷达探测区域)。

[0036] S2，根据N个超声波接收探头接收到探测信号的时间计算自动充电机械臂的第一运动参数，并根据第一运动参数对自动充电机械臂进行相应控制，以使N个超声波接收探头运动至定位区域。

[0037] 根据本发明的一个实施例，根据N个超声波接收探头接收到探测信号的时间计算自动充电机械臂的第一运动参数，并根据第一运动参数对自动充电机械臂进行相应控制，以使N个超声波接收探头运动至定位区域，包括：获取N个超声波接收探头中第一个超声波接收探头接收到探测信号的时间；根据第一个超声波接收探头接收到探测信号的时间判断第一个超声波接收探头是否处于定位区域；如果否，则根据第一个超声波接收探头接收到探测信号的时间获取第一个超声波接收探头在工作区域内的方位信息，并根据第一个超声波接收探头在工作区域内的方位信息计算自动充电机械臂的第一运动参数，以及根据第一运动参数对自动充电机械臂进行相应控制，以使第一个超声波接收探头运动至定位区域；如果是，则获取N个超声波接收探头中第二个超声波接收探头接收到探测信号的时间，并根据第二个超声波接收探头接收到探测信号的时间判断第二个超声波接收探头是否处于定位区域。

[0038] 根据本发明的一个实施例，根据N个超声波接收探头接收到探测信号的时间计算自动充电机械臂的第一运动参数，并根据第一运动参数对自动充电机械臂进行相应控制，以使N个超声波接收探头运动至定位区域，还包括：如果判断第二个超声波接收探头处于定位区域，则获取N个超声波接收探头中第i个超声波接收探头接收到探测信号的时间，并根据第i个超声波接收探测接收到探测信号的时间判断第i个超声波接收探头是否处于定位区域，直至判断出N个超声波接收探头均处于定位区域，其中，i为大于或等于3且小于或等于N的正整数。

[0039] 具体地，如果自动充电机械臂上的各超声波接收探头不都在定位区域内，那么将无法对各超声波接收探头进行准确地定位，因此，本发明实施例中，可根据各超声波接收探头接收到探测信号的时间逐个判断各超声波接收探头是否处于定位区域，并将未处于定位区域的超声波接收探头运动至定位区域。

[0040] 其中，可先获取据N个超声波接收探头中最前端的超声波接收探头，即第一个超声波接收探头检测到标志接收到超声波发射探头信号的时间，并根据第一个超声波接收探头接收到探测信号的时间判断第一个超声波接收探头是否处于定位区域。其中，根据第一个超声波接收探头接收到探测信号的时间可获取第一个超声波接收探头与各超声波发射探头之间的距离，从而可获取第一个超声波接收探头在工作区域内的方位信息，即可判断出第一个超声波接收探头是否处于定位区域。

[0041] 如果第一个超声波接收探头不处于定位区域，则可根据第一个超声波接收探头接收到探测信号的时间获取第一个超声波接收探头在工作区域内的方位信息，例如，仍以超声波发射探头为4个为例，可判断第一个超声波接收探头处于某个超声波发射探头的探测区域(覆盖区域)，或者处于某两个超声波发射探头的探测区域的交叉区域，此时，可根据该方位信息计算自动充电机械臂的第一运动参数，以根据该第一运动参数控制第一个超声波接收探头运动至定位区域。

[0042] 其中，如果判断出第一个超声波接收探头处于某个超声波发射探头的探测区域，例如，如图3所示，当判断第一个超声波接收探头(图3中的星型标记)处于超声波发射探头#1的单雷达探测区域时，可向车载端坐标系的Y轴负方向运动第一预设步进距离(可根据实际情况进行标定)，此时，根据运动方向、第一预设步进距离以及第一个超声波接收探头当前所处地面端坐标系中的坐标可计算出第一个超声波接收探头的下一个目标点在地面端坐标系中的坐标。然后，根据第一个超声波接收探头的下一个目标点在地面端坐标系中的坐标计算自动充电机械臂的第一运动参数，即计算自动充电机械臂的旋转运动量。

[0043] 具体地，如图4所示，其运动模型可以简化为以第一个超声波接收探头的下一个目标点C为圆心，以第一个超声波接收探头的下一个目标点到二级机械臂旋转中心A的距离为半径的圆与一级机械臂旋转轨迹相交，其中，得到的交点为在到达第一个超声波接收探头的下一个目标点时二级臂旋转中心A的位置。其中，求解的结果中，根据地面端坐标系，yA的值为正，xA的坐标值计算下来存在双解，此时，取xA的坐标值较大的点。

[0044] 可以理解的是，如图5所示，当两级旋转机械臂在地面端坐标系中运转时，各超声波接收探头、两级旋转机械臂各相关节点以及地面端连接器中心点在地面端坐标系中的坐标，可通过以下公式进行计算：

[0045]

[0046]

[0047]

[0048] 其中，O为一级旋转机械臂与底座底盘的连接节点(安装有一级机械臂绝对值旋转编码器，以测量机械臂的旋转角度)，A为二级臂旋转中心(安装有二级机械臂绝对值旋转编码器，以测量机械臂的旋转角度)，C为地面端连接器的中心点，D为第一个超声波接收探头，E为第二个超声波接收探头。

[0049] 其中，A点的坐标(xA，yA)可通过以下公式获取：

[0050]

[0051] 其中，L1为一级旋转机械臂的长度，θ1为一级旋转机械臂张开的角度。

[0052] B点的坐标(xB，yB)可通过以下公式获取：

[0053]

[0054] 其中，L3为二级旋转机械臂到旋转中心的距离，θ2为二级旋转机械臂张开的角度。

[0055] C点的坐标(xC，yC)可通过以下公式获取：

[0056]

[0057] 其中，L2为二级旋转机械臂的长度。

[0058] D点的坐标(xD，yD)可通过以下公式获取：

[0059]

[0060] 其中，L4为地面端连接器的中心点C与第一个超声波接收探头D之间的距离。

[0061] E点的坐标(xE，yE)可通过以下公式获取：

[0062]

[0063] 其中，L5为第一个超声波接收探头D与第二个超声波接收探头E之间的距离。由此，在确定出二级臂旋转中心A的坐标时，可计算出一级机械臂的旋转角度θ1为：

[0064]

[0065] AC相对于A点的方位角β1为：

[0066]

[0067] BC的方位角β2为：

[0068]

[0069] 则二级机械臂的旋转角度θ2可为：

[0070] θ2＝π+θ1‑β2，   (12)

[0071] 由此，可获取将第一个超声波接收探头运动到下一个目标点时自动充电机械臂的旋转运动量，即θ1和θ2，并根据该旋转运动量对自动充电机械臂进行相应控制，以使第一个超声波接收探头运动至下一个目标点。

[0072] 进一步而言，判断移动后的第一个超声波接收探头是否处于定位区域，如果判断第一个超声波接收探头处于某两个超声波发射探头的探测区域的交叉区域，例如，判断第一个超声波接收探头处于超声波发射探头#1和超声波发射探头#2构成的双雷达探测区域，则再向车载端坐标系的X轴负方向和Y轴负方向各运行第二预设步进距离(可根据实际情况进行标定)，然后，再根据运动方向、第二预设步进距离计算下一个目标点在地面端坐标系中的坐标，进而计算自动充电机械臂的旋转运动量，以使第一个超声波接收探头继续运动，直至判断出第一个超声波接收探头处于定位区域，具体的计算过程参见上述实施例，为避免冗余，在此不再详述。

[0073] 当第一个超声波接收探头处于定位区域时，可获取N个超声波接收探头中第二个超声波接收探头接收到探测信号的时间，并根据第二个超声波接收探头接收到探测信号的时间判断第二个超声波接收探头是否处于定位区域，以及在判断第二个超声波接收探头未处于定位区域时，根据第二个超声波接收探头接收到探测信号的时间获取第二个超声波接收探头在工作区域内的方位信息，并根据第二个超声波接收探头在工作区域内的方位信息计算自动充电机械臂的第一运动参数，以及根据第一运动参数对自动充电机械臂进行相应控制，以使第二个超声波接收探头运动至定位区域，具体的计算过程参见上述实施例，为避免冗余，在此不再详述。

[0074] 当第二个超声波接收探头处于定位区域时，可获取N个超声波接收探头中第i个超声波接收探头接收到探测信号的时间，并根据第i个超声波接探头接收到探测信号的时间判断第i个超声波接收探头是否处于定位区域，直至判断出N个超声波接收探头均处于定位区域。

[0075] 需要说明的是，在判断第i个超声波接收探头是否处于定位区域的同时，还需判断前i‑1个超声波接收探头是否仍处于定位区域，如果判断出前i‑1个超声波接收探头中出现超声波接收探头移出定位区域，则继续进行调整，以确保各超声波接收探头均处于定位区域。

[0076] 进一步而言，当各超声波接收探头均处于定位区域内时，可控制自动充电机械臂与所述车载端连接器对接，下面结合具体实施例来详细说明如何控制自动充电机械臂与所述车载端连接器对接。

[0077] 根据本发明的一个实施例，自动充电机械臂的定位对接系统的控制方法还包括：在N个超声波接收探头运动至定位区域内时，获取n个超声波接收探头的坐标信息；根据n个超声波接收探头的坐标信息获取车载端连接器中心点的坐标信息，以及根据车载端连接器中心点的坐标信息计算自动充电机械臂的第二运动参数，其中，n为大于或等于2且小于或等于N的正整数；根据第二运动参数对自动充电机械臂进行相应控制，以使自动充电机械臂与车载端连接器对接。

[0078] 根据本发明的一个实施例，获取n个超声波接收探头的坐标信息，包括：获取n个超声波接收探头在地面端坐标系中的第一坐标，并获取n个超声波接收探头在车载端坐标系中的第二坐标。

[0079] 根据本发明的一个实施例，根据n个超声波接收探头的坐标信息获取车载端连接器中心点的坐标信息，包括：根据第一坐标和第二坐标采用向量转换的方式计算车载端连接器中心点在地面端坐标系中的坐标。

[0080] 具体而言，当各超声波接收探头均处于定位区域内时，可计算出各超声波接收探头在车载端坐标系种的坐标，同时由于地面端的双旋转机械臂的旋转中心均装有绝对值编码器，因此，结合公式(1)‑(8)可计算出各超声波接收探头在地面端坐标系里的坐标，此时，利用两个以上的超声波接收探头，通过向量转换，即可计算车载端连接器中心点在地面端坐标系中的坐标。

[0081] 具体地，如图6所示，以利用两个超声波接收探头计算车载端连接器中心点在地面端坐标系中的坐标为例，机械臂上的两个超声波接收探头在车载端坐标系里的坐标可以通过定位系统获取，则第一个超声波接收探头P01(x01，y01)与第二个超声波接收探头P02(x02，y02)在车载端坐标系里的投影向量可为：

[0082]

[0083] 第一个超声波接收探头P01(x01，y01)与车载端连接器中心点PC(xC，yC)在车载端坐标系里的投影向量可为：

[0084]

[0085] 向量 与向量 之间的夹角Δθ为：Δθ＝θ2‑θ1。

[0086] 在地面运动机构坐标系中，机械臂上的两个超声波接收探头的向量为：

[0087]

[0088] 由于向量 是两个超声波接收探头在车载端坐标系中的投影向量，即地面端坐标系中两个超声波接收探头的连线与向量 的夹角与向量 与向量 的夹角相同，均为Δθ，因此，在地面端坐标系中，车载端连接器中心点PC在地面端坐标系的坐标向量模为方向为 的向量逆时针旋转Δθ所得。由此，可计算出车载端连接器中心点PC在地面端坐标系中的坐标：

[0089]

[0090] 最后，将车载端连接器中心点PC在地面端坐标系中的坐标点设置为目标运动点，以计算自动充电机械臂的第二运动参数，具体地计算方式可参见上述实施例中根据所述N个超声波接收探头接收到所述探测信号的时间计算所述自动充电机械臂的第一运动参数的方式，为避免冗余，在此不再详述。根据第二运动参数对自动充电机械臂进行相应控制，以使地面端连接器运动至车载端连接器的正下方，然后抬升地面端连接器，完成地面端连接器(自动充电机械臂)与车载端连接器的对接。由此，解决了使用超声波定位时，车载端的定位坐标系与地面端的运动机构坐标系的转换，从而能够准确计算出双旋转机械臂的运动量，以确保自动充电机械臂与车载端连接器能够进行准确地对接。

[0091] 综上所述，根据本发明实施例的定位对接系统的控制方法，在车载端设置M个超声波发射探头，以通过M个超声波发射探头发射探测信号，并构成包括定位区域和非定位区域的工作区域，以及在地面端的自动充电机械臂上依次设置N个超声波接收探头，以接收M个超声波发射探头发射的探测信号，控制自动充电机械臂工作，以使N个超声波接收探头运动至工作区域，以及根据N个超声波接收探头接收到探测信号的时间计算自动充电机械臂的第一运动参数，并根据第一运动参数对自动充电机械臂进行相应控制，以使N个超声波接收探头运动至定位区域。由此，在定位盲区内，能够准确地控制自动充电机械臂上的超声波接收探头运动至定位区域，从而确保自动充电机械臂与车载端连接器能够进行准确地对接。

[0092] 对应上述实施例的自动充电机械臂的定位对接系统的控制方法，本发明还提出一种自动充电机械臂的定位对接系统的控制装置。

[0093] 需要说明的是，本发明实施例的自动充电机械臂的定位对接系统可包括：M个超声波发射探头和N个超声波接收探头，其中，M个超声波发射探头设置在车载端，M个超声波发射探头用于发射探测信号，其中，M个超声波发射探头的探测区域构成工作区域，工作区域包括定位区域和非定位区域，M为大于或等于4的正整数；N个超声波接收探头依次设置在地面端的自动充电机械臂上，N个超声波接收探头用于接收M个超声波发射探头发射的探测信号，其中，N为大于或等于2的正整数。

[0094] 如图7所示，本发明实施例的自动充电机械臂的定位对接系统的控制装置可包括：第一控制模块100和第二控制模块200。

[0095] 其中，第一控制模块用于控制自动充电机械臂工作，以使N个超声波接收探头运动至工作区域；第二控制模块用于根据N个超声波接收探头接收到探测信号的时间计算自动充电机械臂的第一运动参数，并根据第一运动参数对自动充电机械臂进行相应控制，以使N个超声波接收探头运动至定位区域。

[0096] 根据本发明的一个实施例，如图8所示，自动充电机械臂的定位对接系统的控制装置还可包括：获取模块300、计算模块400和第三控制模块500。

[0097] 其中，获取模块300用于在N个超声波接收探头运动至定位区域内时，获取n个超声波接收探头的坐标信息；计算模块400用于根据n个超声波接收探头的坐标信息获取车载端连接器中心点的坐标信息，以及根据车载端连接器中心点的坐标信息计算自动充电机械臂的第二运动参数，其中，n为大于或等于2且小于或等于N的正整数；第三控制模块500用于根据第二运动参数对自动充电机械臂进行相应控制，以使自动充电机械臂与车载端连接器对接。

[0098] 需要说明的是，本发明实施例的自动充电机械臂的定位对接系统的控制装置更具体的实施方式可参照上述自动充电机械臂的定位对接系统的控制方法的实施例，在此不再赘述。

[0099] 根据本发明实施例的自动充电机械臂的定位对接系统的控制装置，在车载端设置M个超声波发射探头，以通过M个超声波发射探头发射探测信号，并构成包括定位区域和非定位区域的工作区域，以及在地面端的自动充电机械臂上依次设置N个超声波接收探头，以接收M个超声波发射探头发射的探测信号，通过第一控制模块控制自动充电机械臂工作，以使N个超声波接收探头运动至工作区域，以及通过第二控制模块根据N个超声波接收探头接收到探测信号的时间计算自动充电机械臂的第一运动参数，并根据第一运动参数对自动充电机械臂进行相应控制，以使N个超声波接收探头运动至定位区域。由此，在定位盲区内，能够准确地控制自动充电机械臂上的超声波接收探头运动至定位区域，从而确保自动充电机械臂与车载端连接器能够进行准确地对接。

[0100] 对应上述实施例，本发明还提出一种计算机设备。

[0101] 本发明实施例的计算机设备包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行程序时，实现上述实施例的自动充电机械臂的定位对接系统的控制方法。

[0102] 根据本发明实施例的计算机设备，在定位盲区内，能够准确地控制自动充电机械臂上的超声波接收探头运动至定位区域，从而确保自动充电机械臂与车载端连接器能够进行准确地对接。

[0103] 对应上述实施例，本发明还提出一种非临时性计算机可读存储介质。

[0104] 本发明实施例的非临时性计算机可读存储介质上存储有计算机程序，该程序被处理器执行时实现上述的自动充电机械臂的定位对接系统的控制方法。

[0105] 根据本发明实施例的非临时性计算机可读存储介质，在定位盲区内，能够准确地控制自动充电机械臂上的超声波接收探头运动至定位区域，从而确保自动充电机械臂与车载端连接器能够进行准确地对接。

[0106] 在本发明的描述中，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。“多个”的含义是两个或两个以上，除非另有明确具体的限定。

[0107] 在本发明中，除非另有明确的规定和限定，第一特征在第二特征“上”或“下”可以是第一和第二特征直接接触，或第一和第二特征通过中间媒介间接接触。而且，第一特征在第二特征“之上”、“上方”和“上面”可是第一特征在第二特征正上方或斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”可以是第一特征在第二特征正下方或斜下方，或仅仅表示第一特征水平高度小于第二特征。

[0108] 在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必针对相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

[0109] 流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。

[0110] 在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。

[0111] 应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

[0112] 本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。

[0113] 此外，在本发明各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。

[0114] 上述提到的存储介质可以是只读存储器，磁盘或光盘等。尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。