建筑垃圾分拣机械手抓取控制方法及装置

1.本发明涉及智能抓取控制领域，特别是涉及一种建筑垃圾分拣机械手抓取控制方法及装置。


背景技术：

2.随着智能制造的发展，越来越多的领域应用到机械手等抓取控制装置完成抓取操作，比如，建筑领域应用机械手抓取建筑垃圾，由于建筑垃圾材质多样，表面刚度各异，当机械手的抓取力较小时，建筑垃圾易滑脱；当机械手的抓取力较大时，建筑垃圾易损坏。因此，抓取控制装置对抓取的控制是其中的关键点。
3.现有技术中，抓取控制装置对抓取的控制主要采用阻抗控制法，该方法根据设定的目标阻抗模型调节机械手末端执行器的位置和接触力的动态关系，广泛用于果蔬抓取、患肢康复、工作表面打磨、轴孔装配等方面；在建筑领域中，当应用阻抗控制法进行抓取分拣时，若待抓取物的位置和表面刚度都能精确获得，则可以实现既快又稳的抓取；但实际应用中，由于抓取控制装置通过传感器预估的待抓取物的位置和表面刚度等信息往往带有偏差；并且在抓取过程中待抓取物的表面刚度也可能发生变化，这时应用阻抗控制法并不能得到良好的控制效果；反而会影响待抓取物的分拣效率。
4.因此，抓取控制装置如何更精确地进行抓取是一个亟待解决的问题。


技术实现要素：

5.鉴于以上所述现有技术的缺点，本发明的目的在于提供一种建筑垃圾分拣机械手抓取控制方法及装置，用于解决现有技术中存在的未能更精确地进行抓取的问题。
6.为实现上述目的及其他相关目的，本发明提供一种抓取控制方法，包括以下步骤：获取抓取控制装置的实际抓取力、实际角度、实际位置以及待抓取物的期望抓取力；基于所述实际抓取力和所述期望抓取力，应用阻抗控制算法，确定待抓取物的位置修正量；基于所述实际抓取力、所述实际位置以及所述期望抓取力，应用预设的参数估计算法，确定待抓取物的期望位置；基于所述位置修正量和所述期望位置，应用逆运动学计算法，确定抓取控制装置的期望角度；所述期望角度用于和所述实际角度比对以调整所述抓取控制装置的实际角度从而完成抓取操作。
7.于本发明的一实施例中，所述基于所述实际抓取力、所述实际位置以及所述期望抓取力，应用预设的参数估计算法，确定待抓取物的期望位置，包括：基于所述实际抓取力、所述实际位置，应用预设的参数估计算法，确定待抓取物的位置估计值、刚度估计值；基于所述位置估计值、所述刚度估计值以及所述期望抓取力，确定待抓取物的期望位置。
8.于本发明的一实施例中，所述逆运动学计算法的公式表达式为：
9.10.其中，θ1为抓取控制装置单侧的第一关节处的关节角度，θ2为抓取控制装置单侧的第二关节处的关节角度；l1为所述第一关节处和所述第二关节处的连接长度；l2为第二关节处至抓取端的连接长度；x为以所述第一关节处为原点的二维坐标的横坐标值；y为以所述第一关节处为原点的二维坐标的纵坐标值。
11.于本发明的一实施例中，所述x根据所述位置修正量和所述期望位置求和得出；所述y根据所述实际位置得出。
12.对应地，本发明提供一种抓取控制装置，包括获取模块，用于获取抓取控制装置的实际抓取力、实际角度、实际位置以及待抓取物的期望抓取力；第一处理模块，用于基于所述实际抓取力和所述期望抓取力，应用阻抗控制算法，确定待抓取物的位置修正量；第二处理模块，用于基于所述实际抓取力、所述实际位置以及所述期望抓取力，应用预设的参数估计算法，确定待抓取物的期望位置；关节角度计算模块，用于基于所述位置修正量和所述期望位置，应用逆运动学计算法，确定抓取控制装置的期望角度；所述期望角度用于和所述实际角度比对以调整所述抓取控制装置的实际角度从而完成抓取操作。
13.于本发明的一实施例中，所述第二处理模块，具体用于：基于所述实际抓取力、所述实际位置，应用预设的参数估计算法，确定待抓取物的位置估计值、刚度估计值；基于所述位置估计值、所述刚度估计值以及所述期望抓取力，确定待抓取物的期望位置。
14.于本发明的一实施例中，所述逆运动学计算法的公式表达式为：
[0015][0016]
其中，θ1为抓取控制装置单侧的第一关节处的关节角度，θ2为抓取控制装置单侧的第二关节处的关节角度；l1为所述第一关节处和所述第二关节处的连接长度；l2为第二关节处至抓取端的连接长度；x为以所述第一关节处为原点的二维坐标的横坐标值；y为以所述第一关节处为原点的二维坐标的纵坐标值。
[0017]
于本发明的一实施例中，所述x根据所述位置修正量和所述期望位置求和得出；所述y根据所述实际位置得出。
[0018]
本发明提供一种存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述的抓取控制方法。
[0019]
本发明提供一种抓取控制设备，其特征在于，包括：抓取控制模块、前端传感器、位置传感器、力传感器、角度传感器、阻抗控制器、参数估计器、比例积分微分控制器；所述前端传感器，用于获取待抓取物的位置点信息、待抓取物的期望抓取力，并反馈至所述抓取控制模块；所述位置传感器，用于获取所述抓取控制设备的抓取端的实际位置，并反馈至所述抓取控制模块；所述力传感器，用于获取所述抓取端的实际抓取力，并反馈至所述抓取控制模块；所述角度传感器，用于获取所述抓取控制设备的关节处的实际角度，并反馈至所述抓取控制模块；所述阻抗控制器，用于基于所述实际抓取力和所述期望抓取力，确定待抓取物的位置修正量，并反馈至所述抓取控制模块；所述参数估计器，用于基于所述实际抓取力、所述实际位置以及所述期望抓取力，确定待抓取物的期望位置，并反馈至所述抓取控制模块；所述抓取控制模块，用于基于所述位置点信息和所述期望抓取力，控制抓取控制设备移
动至所述待抓取物处以准确抓取操作；还用于将所述实际抓取力和所述期望抓取力输入所述阻抗控制器；还用于将所述实际位置、所述实际抓取力以及所述期望抓取力输入所述参数估计器；还用于基于所述位置修正量和所述期望位置，应用逆运动学计算法，确定抓取控制装置的期望角度；并将所述期望角度、所述实际角度输入所述比例积分微分控制器；所述比例积分微分控制器，用于基于所述期望角度、所述实际角度，确定所述抓取控制设备的角度调整量；并反馈至所述抓取控制模块；所述抓取控制模块，还用于根据所述角度调整量控制所述抓取控制设备完成抓取操作。
[0020]
如上所述，本发明的建筑垃圾分拣机械手抓取控制方法及装置，具有以下有益效果：
[0021]
(1)参数估计算法的应用，确保了抓取控制装置在预估待抓取物的位置和表面刚度有偏差或抓取过程中待抓取物的表面刚度有变化时仍可作出自适应调整以实现稳定抓取。
[0022]
(2)将阻抗控制算法和参数估计算法结合，显著提高了算法的鲁棒性，保证了待抓取物的分拣效率。
附图说明
[0023]
图1显示为本发明的抓取控制方法于一实施例中的流程图。
[0024]
图2显示为本发明的抓取控制方法于一实施例中的简化模型的坐标示意图。
[0025]
图3显示为本发明的抓取控制方法于一实施例中的工作流程实例图。
[0026]
图4显示为本发明的抓取控制方法于一实施例中的预估的待抓取物的位置和表面刚度都没有偏差时的抓取力变化示意图。
[0027]
图5显示为本发明的抓取控制方法于一实施例中的预估的待抓取物的表面刚度有偏差时的抓取力变化示意图。
[0028]
图6显示为本发明的抓取控制方法于一实施例中的预估的待抓取物的位置有偏差时的抓取力变化示意图。
[0029]
图7显示为本发明的抓取控制方法于一实施例中的抓取过程中待抓取物的表面刚度有变化时的抓取力变化示意图。
[0030]
图8显示为本发明的抓取控制装置于一实施例中的结构示意图。
[0031]
图9显示为本发明的抓取控制装置于一实施例中的抓取控制设备。
[0032]
元件标号说明
[0033]
81
ꢀꢀꢀꢀꢀꢀꢀꢀ
获取模块
[0034]
82
ꢀꢀꢀꢀꢀꢀꢀꢀ
第一处理模块
[0035]
83
ꢀꢀꢀꢀꢀꢀꢀꢀ
第二处理模块
[0036]
84
ꢀꢀꢀꢀꢀꢀꢀꢀ
关节角度计算模块
[0037]
91
ꢀꢀꢀꢀꢀꢀꢀꢀ
抓取控制模块
[0038]
92
ꢀꢀꢀꢀꢀꢀꢀꢀ
前端传感器
[0039]
93
ꢀꢀꢀꢀꢀꢀꢀꢀ
位置传感器
[0040]
94
ꢀꢀꢀꢀꢀꢀꢀꢀ
力传感器
[0041]
95
ꢀꢀꢀꢀꢀꢀꢀꢀ
角度传感器
[0042]
96
ꢀꢀꢀꢀꢀꢀꢀꢀ
阻抗控制器
[0043]
97
ꢀꢀꢀꢀꢀꢀꢀꢀ
参数估计器
[0044]
98
ꢀꢀꢀꢀꢀꢀꢀꢀ
比例积分微分控制器
具体实施方式
[0045]
以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。
[0046]
需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。
[0047]
本发明的建筑垃圾分拣机械手抓取控制方法及装置应用参数估计算法，确保抓取控制装置在预估待抓取物的位置和表面刚度有偏差或抓取过程中待抓取物的表面刚度有变化时仍可作出自适应调整以实现稳定抓取；并且将阻抗控制算法和参数估计算法结合，显著提高了算法的鲁棒性，保证了待抓取物的分拣效率。
[0048]
如图1所示，于本实施例中，本发明的抓取控制方法包括以下步骤：
[0049]
步骤s1、获取抓取控制装置的实际抓取力、实际角度、实际位置以及待抓取物的期望抓取力。
[0050]
具体地，抓取控制装置设置有前端传感器，通过前端传感器获取待抓取物的位置点信息和待抓取物的期望抓取力；此外，前端传感器还能获取待抓取物的体积、材质信息；根据获取的待抓取物的位置点信息，抓取控制装置移动至待抓取物的附近；同时，抓取控制装置的抓取端设置有力传感器、位置传感器和角度传感器，通过力传感器测出抓取端与抓取物之间的实际抓取力；通过位置传感器测出抓取端的实际位置；通过角度传感器测出抓取控制装置的关节处的实际角度。
[0051]
步骤s2、基于所述实际抓取力和所述期望抓取力，应用阻抗控制算法，确定待抓取物的位置修正量。
[0052]
具体地，将实际抓取力和期望抓取力进行比较，得到抓取力误差；对抓取力误差应用阻抗控制算法，确定出待抓取物的位置修正量，这里的阻抗控制算法的应用可以采用阻抗控制器的形式。
[0053]
进一步具体的，定义实际抓取力为f，期望抓取力为f
d
，阻抗控制算法的应用过程如下。
[0054]
第一步，将抓取控制装置的抓取端与环境系统等效为线性弹簧，公式表达式为：
[0055][0056]
其中：x表示抓取控制装置的抓取端的实际位置；x
e
表示待抓取物的位置；k
e
表示待抓取物的刚度。
[0057]
第二步，设置目标阻抗控制模型，公式表达式为：
[0058][0059]
其中：x、分别表示抓取控制装置的抓取端的实际位置、实际速度、实际加速度；x
d
、分别表示抓取控制装置的抓取端的目标位置、目标速度、目标加速度；m
d
表示目标惯性；b
d
表示目标阻尼；k
d
表示目标刚度。阻抗控制的目的就是选用主动控制参数来实现理想的目标阻抗。
[0060]
第三步，定义位置修正向量δx＝x
‑
x
d
，抓取力误差e
f
＝f
d
‑
f，由公式表达式(2)得出位置修正量δx与抓取力误差e
f
满足的公式表达式为：
[0061][0062]
第四步，对公式表达式(3)进行拉普拉斯变换，得到阻抗控制器的传递函数为：
[0063][0064]
公式表达式(4)得到的即为待抓取物的位置修正量。
[0065]
步骤s3、基于所述实际抓取力、所述实际位置以及所述期望抓取力，应用预设的参数估计算法，确定待抓取物的期望位置。
[0066]
具体地，基于所述实际抓取力、所述实际位置，应用预设的参数估计算法，确定待抓取物的位置估计值、刚度估计值；基于所述位置估计值、所述刚度估计值以及所述期望抓取力，确定待抓取物的期望位置。定义实际位置为x、位置估计值为刚度估计值为以及期望位置为xd，这里参数估计算法的应用可以采用参数估计器的形式。
[0067]
进一步具体地，参数估计算法的应用过程如下。
[0068]
第一步，根据公式表达式(1)，定义期望位置xd的公式表达式为：
[0069][0070]
定义预测抓取力的公式表达式为：
[0071][0072]
在实际操作中，期望位置xd和预测值可根据当前时刻的待抓取物的刚度估计值和位置估计值实时计算得到。
[0073]
第二步，预先定义的关系等式的公式表达式为：
[0074][0075]
φ＝[φ
k φ
x
]
t
ꢀꢀꢀ
(7)
[0076]
其中，为待抓取物的刚度估计值和刚度的差值；为待抓取物的位置估计值和位置的差值乘以刚度估计值；为由和组成的一维数组的转置。
[0077]
再将公式表达式(6)与公式表达式(1)的第一式相减，并应用公式表达式(7)中的
进行表示，得到预测抓取力和实际抓取力的误差的公式表达式为：
[0078][0079]
这里根据误差调整待抓取物的刚度估计值和位置估计值使得当时间t
→
∞时即预测抓取力能够收敛于实际抓取力。
[0080]
进一步具体地，由于预设的实际抓取力、预测抓取力以及期望抓取力三者之间的关系为：若能够证明可以收敛于f，则f
→
f
d
，即在预测抓取力收敛于实际抓取力的条件下，实际抓取力是能够趋向于期望抓取力的。因此，基于抓取力三者之间的关系，使用李亚普诺夫稳定性定理构造的能量函数v的公式表达式为：
[0081]
v＝φ
t
πφ
ꢀꢀꢀ
(9)
[0082]
其中，π为给定的二阶正定矩阵，为的转置。
[0083]
假设对求导的的公式表达式为：
[0084][0085]
综合上述公式表达式(8)、(9)、(10)得到的对能量函数v求导的的公式表达式为：
[0086][0087]
其中，是一个半负定矩阵；结合半负定矩阵的概念可知，如果φ能够满足公式表达式式(10)，那么当t
→
∞时，能够收敛于f，从而f
→
f
d
。
[0088]
第三步，结合上述公式和证明结论，得到的对的求导和对的求导的公式表达式为：
[0089][0090]
其中,ξ1和ξ2为正常数。
[0091]
第四步，综合上述三步中公式表达式，得到的完整参数估计的公式表达式为：
[0092][0093]
其中，和分别为t＝0时刻，待抓取物的刚度估计值和位置估计值，都为
预设值。
[0094]
通过上述步骤s3中的参数估计算法的应用，能够对待抓取物的刚度和位置进行在线估计，得到待抓取物的刚度估计值和位置估计值，进而根据这些估计值实时计算得到待抓取物的期望位置。
[0095]
步骤s4、基于所述位置修正量和所述期望位置，应用逆运动学计算法，确定抓取控制装置的期望角度；所述期望角度用于和所述实际角度比对以调整所述抓取控制装置的实际角度从而完成抓取操作。
[0096]
具体地，所述逆运动学计算法的公式表达式为：
[0097][0098]
其中，θ1为抓取控制装置单侧的第一关节处的关节角度，θ2为抓取控制装置单侧的第二关节处的关节角度；l1为所述第一关节处和所述第二关节处的连接长度；l2为第二关节处至抓取端的连接长度；x为以所述第一关节处为原点的二维坐标的横坐标值；y为以所述第一关节处为原点的二维坐标的纵坐标值。如图2所示，于本实施例中，本发明的简化模型的坐标示意图，图中的抓取控制装置的抓取端是二指两自由度平面机械手的模式，设定机械手两指和待抓取物均关于y轴对称，因此只对左、右侧中的单侧进行分析即可。
[0099]
进一步具体地，所述x根据所述位置修正量和所述期望位置求和得出；所述y根据所述实际位置得出，比如在考虑一维模式的抓取情况下，y可以保持不变。
[0100]
进一步具体地，在确定出抓取控制装置的期望角度后，将期望角度和实际角度相减得到角度误差，再应用比例积分微分控制法，实时确定各关节处的控制量，从而保证抓取端能够无静差地根据各关节的期望角度，并按照确定的控制量实时进行抓取调整，以使得实际抓取力达到期望抓取力，从而达到自适应地对出现的抓取控制装置在预估待抓取物的位置和表面刚度有偏差或抓取过程中待抓取物的表面刚度有变化的情况下的灵活调整以更精确地完成抓取操作。比如在建筑领域，通过上述方法抓取建筑垃圾后，能够按照预设的轨迹移动到指定位置以释放建筑垃圾，从而完成一次分拣任务。
[0101]
通过上述步骤s1
‑
s4的方法，不但确保抓取控制装置在预估待抓取物的位置和表面刚度有偏差或抓取过程中待抓取物的表面刚度有变化时仍可作出自适应调整以实现稳定抓取；而且保证了待抓取物的分拣效率；如图3所示，于本实施例中，本发明的工作流程实例图。
[0102]
如图4所示，于本实施例中，本发明的预估的待抓取物的位置和表面刚度都没有偏差时的抓取力变化示意图；仿真结果表明，这种条件设定下，抓取控制装置快速稳定地完成了抓取任务。
[0103]
如图5所示，于本实施例中，本发明的预估的待抓取物的表面刚度有偏差时的抓取力变化示意图；这里设定的条件为待抓取物的位置信息没有偏差，而表面刚度信息存在3000n/m的偏差；仿真结果表明，这种条件设定下，抓取控制装置通过上述方法的自适应参数调节，抓取力的超调量略微增大，但同样能够快速稳定地完成抓取任务。
[0104]
如图6所示，于本实施例中，本发明的预估的待抓取物的位置有偏差时的抓取力变
化示意图；这里设定的条件为待抓取物的刚度信息没有偏差，而位置信息存在0.1mm的偏差；仿真结果表明，这种条件设定下，抓取控制装置通过上述方法的自适应参数调节，抓取力的超调量增加明显且抓取时间有所延长，但两者都在允许范围内，同样能够快速稳定地完成抓取任务。
[0105]
如图7所示，于本实施例中，本发明的抓取过程中待抓取物的表面刚度有变化时的抓取力变化示意图；这里设定的条件为在抓取控制装置稳定抓取待抓取物时，待抓取物的刚度发生突变，比如在1.5s至1.6s内刚度增加3000n/m，仿真结果表明，这种条件设定下，抓取控制装置通过上述方法的自适应参数调节能够迅速反应并及时调整，使抓取力快速稳定至期望值上。
[0106]
如图8所示，于一实施例中，本发明的抓取控制装置包括：
[0107]
获取模块81，用于获取抓取控制装置的实际抓取力、实际角度、实际位置以及待抓取物的期望抓取力；
[0108]
第一处理模块82，用于基于所述实际抓取力和所述期望抓取力，应用阻抗控制算法，确定待抓取物的位置修正量；
[0109]
第二处理模块83，用于基于所述实际抓取力、所述实际位置以及所述期望抓取力，应用预设的参数估计算法，确定待抓取物的期望位置；
[0110]
关节角度计算模块84，用于基于所述位置修正量和所述期望位置，应用逆运动学计算法，确定抓取控制装置的期望角度；所述期望角度用于和所述实际角度比对以调整所述抓取控制装置的实际角度从而完成抓取操作。
[0111]
其中，所述第二处理模块83，具体用于：
[0112]
基于所述实际抓取力、所述实际位置，应用预设的参数估计算法，确定待抓取物的位置估计值、刚度估计值；
[0113]
基于所述位置估计值、所述刚度估计值以及所述期望抓取力，确定待抓取物的期望位置。
[0114]
所述逆运动学计算法的公式表达式为：
[0115][0116]
其中，θ1为抓取控制装置单侧的第一关节处的关节角度，θ2为抓取控制装置单侧的第二关节处的关节角度；l1为所述第一关节处和所述第二关节处的连接长度；l2为第二关节处至抓取端的连接长度；x为以所述第一关节处为原点的二维坐标的横坐标值；y为以所述第一关节处为原点的二维坐标的纵坐标值。所述x根据所述位置修正量和所述期望位置求和得出；所述y根据所述实际位置得出。
[0117]
本实施例的抓取控制装置具体实现的技术特征与实施例1中抓取控制方法中的各步骤的原理基本相同，方法和装置之间可以通用的技术内容不作重复赘述。
[0118]
本发明的存储介质上存储有计算机程序，该程序被处理器执行时实现上述的抓取控制方法。
[0119]
如图9所示，于一实施例中，本发明的抓取控制设备包括：
[0120]
抓取控制模块91、前端传感器92、位置传感器93、力传感器94、角度传感器95、阻抗控制器96、参数估计器97、比例积分微分控制器98；
[0121]
所述前端传感器92，用于获取待抓取物的位置点信息、待抓取物的期望抓取力，并反馈至所述抓取控制模块；
[0122]
所述位置传感器93，用于获取所述抓取控制设备的抓取端的实际位置，并反馈至所述抓取控制模块；
[0123]
所述力传感器94，用于获取所述抓取端的实际抓取力，并反馈至所述抓取控制模块；
[0124]
所述角度传感器95，用于获取所述抓取控制设备的关节处的实际角度，并反馈至所述抓取控制模块；
[0125]
所述阻抗控制器96，用于基于所述实际抓取力和所述期望抓取力，确定待抓取物的位置修正量，并反馈至所述抓取控制模块；
[0126]
所述参数估计器97，用于基于所述实际抓取力、所述实际位置以及所述期望抓取力，确定待抓取物的期望位置，并反馈至所述抓取控制模块；
[0127]
所述抓取控制模块91，用于基于所述位置点信息和所述期望抓取力，控制抓取控制设备移动至所述待抓取物处以准确抓取操作；还用于将所述实际抓取力和所述期望抓取力输入所述阻抗控制器；还用于将所述实际位置、所述实际抓取力以及所述期望抓取力输入所述参数估计器；还用于基于所述位置修正量和所述期望位置，应用逆运动学计算法，确定抓取控制装置的期望角度；并将所述期望角度、所述实际角度输入所述比例积分微分控制器；
[0128]
所述比例积分微分控制器98，用于基于所述期望角度、所述实际角度，确定所述抓取控制设备的角度调整量；并反馈至所述抓取控制模块；
[0129]
所述抓取控制模块91，还用于根据所述角度调整量控制所述抓取控制设备完成抓取操作。
[0130]
综上所述，本发明的建筑垃圾分拣机械手抓取控制方法及装置，应用参数估计算法，确保了抓取控制装置在预估待抓取物的位置和表面刚度有偏差或抓取过程中待抓取物的表面刚度有变化时仍可作出自适应调整以实现稳定抓取；并且将阻抗控制算法和参数估计算法结合，显著提高了算法的鲁棒性，保证了待抓取物的分拣效率。所以，本发明有效克服了现有技术中的种种缺点而具高度产业利用价值。
[0131]
上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。