机械臂装置的闭环控制系统及方法与流程

本发明涉及自动化控制技术领域，特别涉及一种机械臂装置的闭环控制系统及方法。

背景技术

机械臂装置广泛应用于工业自动化、无人作业及演示玩具等领域。机械臂装置通常由基座、若干组连接杆、传动齿轮组、伺服电机、末端负载装置和控制器组成，其功能结构类似人的手臂。机械臂装置的动作通常表现为末端负载装置按照设计的路径和设计的姿态进行运动。机械臂装置的动作需要通过机械臂的控制系统采用一定的控制方法来实现。控制系统通常由控制器、伺服电机、传动齿轮组构成，其基本过程为控制器输出指令控制伺服电机运转，伺服电机运转带动传动齿轮组转动，传动齿轮组转动驱动连接杆产生运动。为实现末端负载装置的三维运动和姿态变化，机械臂装置一般具有多组由连接杆、传动齿轮组和伺服电机构成的关节臂连接而成，每组机械关节臂具备一定范围的运动自由度，相互连接后使处于连接最末端的负载装置能够进行三维运动和姿态变化。为使末端负载装置能够按照设计的路径和姿态进行运动，机械臂的控制系统需使用特定的控制方法。机械臂装置的控制方法一般是根据末端负载装置的路径规划和姿态规划解算出每组机械关节臂运动的参数，这些参数通常表现为伺服电机带动下传动齿轮组转动的角度，控制器利用参数作为输出量控制各组伺服电机转动，以达到控制目的。

机械臂装置因用途不同，所以其末端负载装置具体的功能也有所不同，但其末端负载装置运动路径和姿态的精确度、稳定度和响应速度等参数是机械臂装置的重要性能指标。在不同用途和场合中，有不同的具体要求，而这些参数受到很多因素的影响，主要包括：控制系统和控制方法、机械关节臂的加工精度、末端负载装置的重量尺寸及震动等。这些因素中，机械关节臂加工和末端负载装置由机械臂装置本身特点决定，改善空间不大，因此控制系统和控制方法成为提升机械臂装置性能的重要途径。

为提升末端负载装置运动路径和姿态的精确度和稳定度，目前控制系统的主要方法是精确控制每组机械臂关节转动的角度。在自动控制领域中提高控制精确度和稳定度的基本方法是采用闭环反馈控制。目前机械臂装置的控制系统通常采用增加编码器或标尺等器件实时测定每组关节转动的角度量作为反馈量，将测定的角度量与系统解算出的角度目标参量进行实时比较，将比较误差量换算为继续控制伺服电机运转的控制量，伺服电机在控制量的控制下产生运转，从而使机械臂关节角度发生进一步的变化，这种变化又被测量器件实时测定得到新的角度量，上述步骤不断重复。这一过程中选用合适的误差量换算方法使比较误差最终趋近于零，从而使每组机械臂关节的转动角度与系统解算角度匹配，达到精确控制和稳定控制的目的。这种通过测定每组机械臂关节转动角度量作为反馈量形成的闭环反馈控制方法是目前机械臂装置控制系统主要控制方法，其主要问题在于每组机械臂关节的角度误差会积累到机械臂的末端后形成较大的误差，随着机械关节臂组数量的增加，积累在末端的误差会以几何级数增加。

技术实现要素：

本发明解决的问题是现有的机械臂装置的控制系统通过测定每组机械臂关节转动角度量作为反馈量形成的闭环反馈控制方法，由于每组机械臂关节的角度误差会积累到机械臂的末端后形成较大的误差，随着机械关节臂组数量的增加，积累在末端的误差会以几何级数增加。

为解决上述问题，本发明实施例提供一种机械臂装置的闭环控制系统，包括：

设置于机械臂装置负载端的定位源，用于发射声波信号；

多个定位接收器和计时器；其中，所述定位接收器用于在接收到同步命令信号时启动计时器，并在接收到声波信号时关闭计时器，以确定声波信号从定位源到各个定位接收器的传送时间；其中，所述同步命令信号是与声波信号同步发射的电磁波信号或电信号；

控制器，用于基于声波信号从定位源到各个定位接收器的传送时间或各个定位接收器接收声波信号的相位来实时确定所述定位源的位置，并根据所述定位源与机械臂装置负载端的相对位置关系确定机械臂装置负载端的位置；

所述控制器，还用于对机械臂装置的机械臂运动进行控制，并将实时确定的所述机械臂装置负载端的位置或者所述定位源的位置作为控制反馈量与期望运动位置作比对以得到误差量，再将误差量通过控制算法转换为机械臂运动的控制量以实现对所述机械臂装置的闭环控制。

可选的，所述定位源包括声波发射器、声源发生电路以及第一通信同步电路；其中，所述声波发生电路用于产生声波信号；所述声波发射器用于发射声波信号；所述第一通信同步电路用于处理所述定位源与控制器之间的通信。

可选的，所述定位源包括一个声波发射器，根据该声波发射器的位置确定所述定位源的位置，并基于定位源与机械臂装置负载端的相对位置关系确定机械臂装置负载端的位置。

可选的，所述定位源包括至少两个声波发射器，根据至少两个声波发射器的位置确定所述定位源的位置和姿态，并基于定位源与机械臂装置负载端的相对位置关系和姿态确定机械臂装置负载端的位置和姿态。

可选的，机械臂装置的闭环控制系统包括至少两个定位接收器；所述控制器根据定位接收器接收到的声波信号的相位来计算所述定位源与各个定位接收器之间的矢量角度，再结合各个定位接收器的位置确定所述定位源的位置。

可选的，机械臂装置的闭环控制系统包括至少三个定位接收器，且其中至少一个定位接收器与其余定位接收器不在同一直线上；所述控制器根据各个定位接收器接收到的声波信号到达的时间来测算所述定位源与各个定位接收器之间的距离，再结合各个定位接收器的位置确定所述定位源的位置。

可选的，机械臂装置的闭环控制系统包括至少四个定位接收器，且其中至少一个定位接收器与其余定位接收器不在同一平面中；所述控制器根据各个定位接收器接收到的声波信号到达的时间差来测算所述定位源与各个定位接收器之间的距离差，再结合各个定位接收器的位置确定所述定位源的位置。

可选的，所述定位接收器包括声波接收器、声波接收放大处理电路、声波信号相位预处理单元以及第二通信同步电路；其中，所述声波接收器用于接收声波信号、所述声波接收放大处理电路用于对接收到的声波信号的功率进行放大处理、所述声波信号相位预处理单元用于从接收到的声波信号提取相位信息、所述第二通信同步电路用于处理所述定位接收器与控制器之间的通信。

可选的，所述控制器包括控制处理单元和机械臂装置动作控制接口；其中，所述控制处理单元用于与所述定位源和各个定位接收器进行通信；所述机械臂装置动作控制接口与机械臂装置的驱动器连接，从而将所述控制器产生的机械臂运动的控制量传送给所述机械臂装置的驱动器以驱动机械臂装置进行运动。

可选的，所述声波信号的频率范围为50赫兹~100000赫兹。

基于上述机械臂装置的闭环控制系统，本发明实施例还提供了一种机械臂装置的闭环控制方法，包括如下步骤：

在机械臂装置运动过程中，所述定位源发射声波信号；

所述定位接收器在接收到同步命令信号时启动计时器，并在接收到声波信号时关闭计时器，以确定声波信号从定位源到各个定位接收器的传送时间；其中，所述同步命令信号是与声波信号同步发射的电磁波信号或电信号；

所述控制器基于声波信号从定位源到各个定位接收器的传送时间或各个定位接收器接收声波信号的相位来实时确定所述定位源的位置，以及根据所述定位源与机械臂装置负载端的相对位置关系确定机械臂装置负载端的位置；

所述控制器将实时确定的所述机械臂装置负载端的位置或者所述定位源的位置作为控制反馈量与期望运动位置作比对以得到误差量，再将误差量通过控制算法转换为机械臂运动的控制量以实现对所述机械臂装置的闭环控制。

与现有技术相比，本发明技术方案具有以下有益效果：

根据本发明实施例提供的机械臂装置的闭环控制系统，利用了机械臂负载端的位置信息（或者负载端上定位源的位置信息）作为反馈实现闭环自动控制，使机械臂各关节的控制可以相互协同，减少了对单一关节控制的依赖，并可以解决机械臂各连接杆长度的变化所引起的负载端控制精度和稳定度问题。本闭环控制系统可以利用非固定在机械臂装置上的同类型定位源预设机械臂负载端的运动轨迹，控制器自动记录轨迹各点位置坐标，并将其逐一设置为系统期望位置，通过闭环控制方法使机械臂装置负载端按预设轨迹运动。

进一步，若定位源包括至少两个声波发射器，还可以根据至少两个声波发射器的位置确定所述定位源的位置和姿态，并基于定位源与机械臂装置负载端的相对位置关系和姿态确定机械臂装置负载端的位置和姿态。在实际应用中，通常机械臂负载端装置是机械手，根据上述方式确定机械手的朝向等姿态对于执行的任务也非常重要。

附图说明

图1是本发明实施例中构建的空间三维坐标系o-xyz以及机械臂装置的闭环控制系统的示意图；

图2是本发明实施例的一种机械臂装置的闭环控制方法的流程示意图。

具体实施方式

发明人发现现有的机械臂装置的控制系统通过测定每组机械臂关节转动角度量作为反馈量形成的闭环反馈控制方法，由于每组机械臂关节的角度误差会积累到机械臂的末端后形成较大的误差，随着机械关节臂组数量的增加，积累在末端的误差会以几何级数增加。

针对上述问题，发明人经过研究，提供了一种机械臂装置的闭环控制系统及方法。利用了机械臂负载端上定位源的位置信息作为反馈实现闭环自动控制，使机械臂各关节的控制可以相互协同，减少了对单一关节控制的依赖，并可以解决机械臂各连接杆长度的变化所引起的负载端控制精度和稳定度问题。本闭环控制系统可以利用非固定在装置上的同类型定位源预设机械臂负载端运动轨迹，控制器自动记录轨迹各点位置坐标，并将其逐一设置为系统期望位置，通过闭环控制方法使机械臂装置负载端按预设轨迹运动。

为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施方式做详细的说明。

如图1所示的是本发明实施例中构建的空间三维坐标系o-xyz以及机械臂装置的闭环控制系统的示意图。参考图1，在空间三维坐标系o-xyz及机械臂装置的闭环控制系统中的图示标号说明如下：

空间的三维坐标系o-xyz，其中o为坐标系原点；

标号1~11是机械臂装置的基本功能结构，具体包括机械臂基座1，机械臂第一转动关节2，机械臂第一连杆3，机械臂第二转动关节4，机械臂第二连杆5，机械臂第三转动关节6，机械臂第n连杆7，机械臂第n转动关节8，机械臂第n+1连杆9，机械臂第n+1转动关节10，机械臂末端负载装置11。

需要说明的是，机械臂装置应用于不同场景，其关节数和连杆数至少为一组，具体数量由应用所决定，且在实际应用中机械臂装置可以有不同的具体设计，但不影响本发明实施例中闭环控制系统对其功能结构的控制实施。

标号12是定位源；

标号13、14、15和16是定位接收器；

标号18是控制处理单元、标号19是机械臂装置动作控制接口，两者是控制器的组成部分；

标号20是定位源与控制处理单元之间的通信通道；

标号21、22、23和24是定位接收器与控制处理单元之间的通信通道；

标号25是机械臂装置动作控制接口与机械臂装置的驱动器29之间的连接通道；

标号29是机械臂装置的驱动器；

标号30是声波信号。

本实施例中，所述闭环控制系统用于对机械臂装置进行运动控制。其中，所述机械臂装置的基本功能结构如图1所示以及上文对标号1~11各个组成部分的描述，但在实际应用中并不限于这样图示的结构，这并不影响本发明的实质，在此不再赘述。

在所述机械臂装置负载端上设置有定位源12，所述定位源用于发射声波信号。

具体的，所述定位源包括声波发射器、声源发生电路以及第一通信同步电路（图1中均未示出）；其中，所述声波发生电路用于产生声波信号；所述声波发射器用于发射声波信号；所述第一通信同步电路用于处理所述定位源与控制器之间的通信。

所述声波信号的频率设定范围为50赫兹~100000赫兹，其中常用声波是频率值为40000赫兹的超声波，相应的声波发射器为超声波换能器。

在机械臂装置及其所处物理空间（如图1所示构建的三维空间坐标系）的不同位置上设置有多个定位接收器（标号13、14、15和16）和计时器（图1中未示出），其中计时器可以设置于定位接收器上或者控制器上。所述定位接收器用于在接收到同步命令信号时启动计时器，并在接收到声波信号时关闭计时器，以确定声波信号从定位源到各个定位接收器的传送时间。

具体来说，所述同步命令信号是与声波信号同步发射的电磁波信号或电信号，同步命令信号可以由定位源发出或者由控制器中的控制处理单元发出。而同步发射声波信号和同步命令信号包括如下两种情形：1）所述定位源器或定位器和控制处理单元分别同时发射声波信号和同步命令信号，即两者发射信号的时刻没有时间差；2）所述定位源发射声波信号的时间与定位源或控制处理单元发射同步命令信号的时间具有相对时延，即两者发射信号的时刻之间有一个时延，只要各个定位接收器获得这个时延，则不影响后续计算声波信号的传送时间。

在实际应用中，所述定位接收器包括声波接收器、声波接收放大处理电路、声波信号相位预处理单元以及第二通信同步电路（图1中均未示出）。其中，所述声波接收器用于接收声波信号、所述声波接收放大处理电路用于对接收到的声波信号的功率进行放大处理、所述声波信号相位预处理单元用于从接收到的声波信号提取相位信息、所述第二通信同步电路用于处理所述定位接收器与控制器之间的通信。

本领域技术人员理解，利用电磁波的传播速度在较近（<100米）的空间距离内远远大于声波的传递速度的特征，在空间内设置多个定位接收器，在同步发射电磁波信号和声波信号的前提下，各个定位接收器以接收到电磁波信号的时间作为声波信号发射时间，并在接收到声波信号后确定声波信号从定位源到定位接收器的传送时间。

控制器用于基于声波信号从定位源到各个定位接收器的传送时间或各个定位接收器接收声波信号的相位来实时确定所述定位源的位置，并根据所述定位源与机械臂装置负载端的相对位置关系确定机械臂装置负载端的位置。

具体来说，所述控制器包括控制处理单元18和机械臂装置动作控制接口19。其中，所述控制处理单元18用于与所述定位源和各个定位接收器进行通信；所述机械臂装置动作控制接口19与机械臂装置的驱动器29连接，从而将所述控制器产生的机械臂运动的控制量传送给所述机械臂装置的驱动器29以驱动机械臂装置进行运动。

也就是说，所述控制器的机械臂装置动作控制接口通过与机械臂装置的驱动器连接，可以实现对机械臂装置的机械臂运动进行控制，并将实时确定的所述机械臂装置负载端的位置或者所述定位源的位置作为控制反馈量与期望运动位置作比对以得到误差量，再将误差量通过控制算法（例如，pid（proportion、integral、derivative）控制算法）转换为机械臂运动的控制量，上述过程得以不断重复，直至误差量逼近零或达到最小稳态值，以实现对所述机械臂装置的闭环控制。

其中，所述控制处理单元18包括与定位源和各个定位接收器进行通信的通信同步电路、本地算法处理器或与上位机的通信电路（图1中均未示出）。机械臂装置动作控制接口19连接机械臂装置的驱动器（或者执行机构），将控制器产生的机械臂运动的控制量传送给机械臂装置的驱动器（或者执行机构）从而产生相应动作。所述控制处理单元18通过与定位源有线或者无线连接的通信同步电路触发定位器发射声波信号，通过与各个定位接收器有线或者无线连接的通信同步电路记录定位接收器接收到的相位信息或时间信息，所述控制处理单元18利用接收到的相位信息或时间信息结合所有定位接收器的位置，在本地算法处理器中或者上传至上位机中解算出定位源的位置，进而利用上述方法产生的控制量，通过机械臂装置动作控制接口19驱动机械臂装置产生相应动作。

根据定位源中声波发射器的不同个数以及闭环控制系统内定位接收器的不同个数，确定定位源的位置的方式也有差异，具体如下所述：

1）定位源中声波发射器的数目不同

若所述定位源包括一个声波发射器，根据该声波发射器的位置确定所述定位源的位置（在三维空间坐标系中为一个点坐标），并基于定位源与机械臂装置负载端的相对位置关系确定机械臂装置负载端的位置。

若所述定位源包括至少两个声波发射器，根据至少两个声波发射器的位置确定所述定位源的位置和姿态，并基于定位源与机械臂装置负载端的相对位置关系和姿态确定机械臂装置负载端的位置和姿态。也就是说，在这种情形下，根据两个及以上不同位置的点坐标，这些点坐标可以连接成线段，进而得到描述定位源的位置和姿态的一组矢量坐标，这组矢量坐标经过数值修正即可得到描述机械臂负载端的位置和姿态的信息。

可见，与传统的机械臂控制系统通过构建机械臂关节转动空间计算负载端的位置信息和姿态信息的方法不同，本实施例提供的闭环控制系统是直接利用定位技术获取相应的信息数据。

2）闭环控制系统中定位接收器的数目不同

若机械臂装置的闭环控制系统包括至少两个定位接收器。所述控制器根据定位接收器接收到的声波信号的相位来计算所述定位源与各个定位接收器之间的矢量角度（且必须两个及以上不同位置的定位接收器符合波长对其位置的要求限制），再结合各个定位接收器的位置确定所述定位源的位置。

若机械臂装置的闭环控制系统包括至少三个定位接收器，且其中至少一个定位接收器与其余定位接收器不在同一直线上。所述控制器根据各个定位接收器接收到的声波信号到达的时间来测算所述定位源与各个定位接收器之间的距离，再结合各个定位接收器的位置确定所述定位源的位置。

若机械臂装置的闭环控制系统包括至少四个定位接收器，且其中至少一个定位接收器与其余定位接收器不在同一平面中。所述控制器根据各个定位接收器接收到的声波信号到达的时间差来测算所述定位源与各个定位接收器之间的距离差，再结合各个定位接收器的位置确定所述定位源的位置。

在实际应用中，定位接收器的数目可以根据具体采用的空间几何算法来确定，本领域技术人员可以根据空间几何原理，根据若干个已知位置点（即定位接收器的位置）坐标，求解出待确定位置点（即定位源的位置）的坐标。

基于上述机械臂装置的闭环控制系统，本发明实施例还提供了一种机械臂装置的闭环控制方法。

如图2所示的是机械臂装置的闭环控制方法的流程示意图。参考图2，该闭环控制方法包括如下步骤：

步骤s1：在机械臂装置运动过程中，所述定位源发射声波信号；

步骤s2：所述定位接收器在接收到同步命令信号时启动计时器，并在接收到声波信号时关闭计时器，以确定声波信号从定位源到各个定位接收器的传送时间；其中，所述同步命令信号是与声波信号同步发射的电磁波信号或电信号；

步骤s3：所述控制器基于声波信号从定位源到各个定位接收器的传送时间或各个定位接收器接收声波信号的相位来实时确定所述定位源的位置，以及根据所述定位源与机械臂装置负载端的相对位置关系确定机械臂装置负载端的位置；

步骤s4：所述控制器将实时确定的所述机械臂装置负载端的位置或者所述定位源的位置作为控制反馈量与期望运动位置作比对以得到误差量，再将误差量通过控制算法转换为机械臂运动的控制量以实现对所述机械臂装置的闭环控制。

本实施例所述闭环控制方法的具体实施过程可以参照上文对闭环控制系统实施例的描述，在此不再赘述。

综上所述，根据本发明实施例提供的机械臂装置的闭环控制系统，利用了机械臂负载端的位置信息（或者负载端上定位源的位置信息）作为反馈实现闭环自动控制，使机械臂各关节的控制可以相互协同，减少了对单一关节控制的依赖，并可以解决机械臂各连接杆长度的变化所引起的负载端控制精度和稳定度问题。本闭环控制系统可以利用非固定在装置上的同类型定位源预设机械臂负载端运动轨迹，控制器自动记录轨迹各点位置坐标，并将其逐一设置为系统期望位置，通过闭环控制方法使机械臂装置负载端按预设轨迹运动。

进一步，若定位源包括至少两个声波发射器，还可以根据至少两个声波发射器的位置确定所述定位源的位置和姿态，并基于定位源与机械臂装置负载端的相对位置关系和姿态确定机械臂装置负载端的位置和姿态。在实际应用中，通常机械臂负载端装置是机械手，根据上述方式确定机械手的朝向等姿态对于执行的任务也非常重要。

本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。