[发明专利]机械臂装置的闭环控制系统及方法

权利要求书

1.一种机械臂装置的闭环控制系统，其特征在于，包括：

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

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

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

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

2.如权利要求1所述的机械臂装置的闭环控制系统，其特征在于，所述定位源包括声波发射器、声源发生电路以及第一通信同步电路；其中，所述声波发生电路用于产生声波信号；所述声波发射器用于发射声波信号；所述第一通信同步电路用于处理所述定位源与控制器之间的通信。

3.如权利要求2所述的机械臂装置的闭环控制系统，其特征在于，所述定位源包括一个声波发射器，根据该声波发射器的位置确定所述定位源的位置，并基于定位源与机械臂装置负载端的相对位置关系确定机械臂装置负载端的位置。

4.如权利要求2所述的机械臂装置的闭环控制系统，其特征在于，所述定位源包括至少两个声波发射器，根据至少两个声波发射器的位置确定所述定位源的位置和姿态，并基于定位源与机械臂装置负载端的相对位置关系和姿态确定机械臂装置负载端的位置和姿态。

5.如权利要求1所述的机械臂装置的闭环控制系统，其特征在于，机械臂装置的闭环控制系统包括至少两个定位接收器；所述控制器根据定位接收器接收到的声波信号的相位来计算所述定位源与各个定位接收器之间的矢量角度，再结合各个定位接收器的位置确定所述定位源的位置。

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

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

8.如权利要求1所述的机械臂装置的闭环控制系统，其特征在于，所述定位接收器包括声波接收器、声波接收放大处理电路、声波信号相位预处理单元以及第二通信同步电路；其中，所述声波接收器用于接收声波信号、所述声波接收放大处理电路用于对接收到的声波信号的功率进行放大处理、所述声波信号相位预处理单元用于从接收到的声波信号提取相位信息、所述第二通信同步电路用于处理所述定位接收器与控制器之间的通信。

9.如权利要求1所述的机械臂装置的闭环控制系统，其特征在于，所述控制器包括控制处理单元和机械臂装置动作控制接口；其中，所述控制处理单元用于与所述定位源和各个定位接收器进行通信；所述机械臂装置动作控制接口与机械臂装置的驱动器连接，从而将所述控制器产生的机械臂运动的控制量传送给所述机械臂装置的驱动器以驱动机械臂装置进行运动。

10.如权利要求1所述的机械臂装置的闭环控制系统，其特征在于，所述声波信号的频率范围为50赫兹~100000赫兹。

11.一种机械臂装置的闭环控制方法，用于权利要求1至10中任一项所述的机械臂装置的闭环控制系统，其特征在于，该闭环控制方法包括如下步骤：

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

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

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

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