一种人机协作机器人手臂的防抖控制方法与流程
本发明涉及机器人领域技术，尤其是指一种人机协作机器人手臂的防抖控制方法。
背景技术：
传统的人机协作机器人手臂，为了提高安全性，一般采用力传感器，或者皮肤传感器，来检测外界的冲击，进行相应的避障控制动作。人机协作的机器人手臂的防抖控制，目前文献中还未发现相关的专利。目前以机器人和防抖作为关键字，在中国专利文献数据库中搜索，仅仅能搜索到两个专利。其中一个是“移动机器人视觉系统防抖装置与防抖补偿控制方法”(公开号：cn101612735a,2009年12月公开)，该专利设计了一种机器人防抖的机械装置，并且提出了pd反馈的防抖的控制方法，所使用的pd反馈控制算法设计主要是针对特定一个频段进行相位和增益进行补偿，而且该专利主要是针对机器人视觉的补偿。另外一个“一种智能化多功能植保机器人喷药臂用防抖动机构”(公开号：cn201510485208.1,2016年5月公开)提出了一个主要是用于机器人手臂的防抖机构，虽然使用了微控制器，但是所设计的控制算法极其简化，并且欠缺控制器稳定性方面的考量，而且仅仅局限于喷雾机器人手臂。
此外，目前防抖控制在数码相机以及智能手机领域中用应用的比较多。一般的思想都是通过传感器检测人手的抖动，然后通过单片机的算法来计算相应的补偿量，通过移动镜头组中的用于防抖的镜片，来补偿手抖动造成的光成像偏差。这种系统的部分技术可以应用到人机协作机器人手臂防抖控制上去，人机协作机器人的手臂防抖控制防抖系统和数码相机还是很不相同的：(1)本身控制的对象不同，一个是机器人的机械手臂，一个是数码相机的镜头；(2)应用场景不同，数码相机的防抖控制主要是为了在人手不经意的抖动下，也能拍出不模糊的照片，而人机协作机器人手臂的防抖控制主要是在外界扰动情况下，保持机器人手臂的稳定性；(3)控制器不同，正因为数码相机的抖动来源是手，所以需要针对手抖动的主要抖动频率设计校正相应的控制器，而人机协作机器手臂的抖动来源可能是手以外的，所以必须要在更广的频率范围来设计不同的控制器去校正相位和增益的偏差。
技术实现要素：
有鉴于此，本发明针对现有技术存在之缺失，其主要目的是提供一种人机协作机器人手臂的防抖控制方法，其能有效校正相位和增益的偏差。
为实现上述目的，本发明采用如下之技术方案：
一种人机协作机器人手臂的防抖控制方法，主要包括角速度传感器增益调整、相位与增益分频段处理,其过程包括以下步骤：
(1)传感器增益调整：
针对角速度传感器的不同的增益，测量系统的抖动残留率，这里残留率定义为能反应机器人手臂抖动程度大小的一个量，必须要设计一个好的系统以满足比较低的残留率要求，为使残留率满足要求，将调整过程划分为两个阶段：
(1.1)粗调整处理：
通过计算所获取角速度的残留率，设定阈值r，如果残留率小于r，判定粗调整后的残留率符合要求，输出结果；如果残留率大于r，则针对残留率进行步骤(1.2)中进行精调整，以使其满足要求；
(1.2)精调整处理：
对于粗调整处理后不满足要求的残留率，进行精调整过程，其具体过程如下：
(1.2.1)对于步骤(1.1)中不满足要求的残留率，对其相应的增益进行加h处理，重复此过程n次，对处理后的结果，同样进行与阈值r比较，如果小于r，则符合要求，否则转入步骤(1.2.2)进行处理；
(1.2.2)针对步骤(1.2.1)中不满足要求的残留率，首先在模拟谷中根据不同的点，依据两点确定直线的原理，求出交叉点，并定义为暂定谷；其次，在暂定谷的左右两侧两点中选择残留率较低的点，并求出此点与暂定谷之间的点k,比较暂定谷与k的残留率，取其残留率最小的作为精调整处理的最终结果；
(2)相位与增益阶段分频段处理：
根据步骤(1)处理后得到的增益之积输入到相位与增益处理阶段，进行相位与增益补偿，使其满足系统稳定性的要求。
优选的，所述步骤(2)所述的相位与增益处理阶段的具体步骤如下：
(2.1)对频段进行划分，将其划分为低频段、中间频段和高频段，使其在不同频段内进行相位和增益补偿；
(2.2)在步骤(2.1)进行频段划分以后，首先考虑在低频段进行设计第一一阶滤波控制器；
(2.2.1)设计第一一阶滤波控制器，根据梅森公式，得到该第一一阶滤波控制器的一节环节以z为参数的传递函数如下公式(1)
其中a，b，c为不同的常数；
(2.2.2)根据定义，有z和s的关系如下公式(2),其中s是拉普拉斯变量，ts是采样周期；
而时滞的次数是用n来表示的，时间滞后的量是1，因此得到如下公式(3)
(2.2.3)因为仅仅靠公式(3)，很难进行工程上的仿真以及实际的计算，本步骤和下一步骤分别做两个很重要的转换-双线性变换和泰勒展开近似，双线性变换如下公式(4)所表；
(2.2.4)本步骤对
(2.2.5)将步骤2.2.4中的公式(5)和(6)代入到公式(4)，可以得到公式(7)
根据定义，已经知道有公式(8)和(9)
s＝jω(8)
ω＝2πf(9)
其中f是频率，采样周期和采样频率的关系式如下面公式(10)
ts＝1/fs(10)
其中fs是系统的采样频率，因为得到公式
因此，一阶滤波控制器的传递函数可以表示为如下频率的函数，
(2.2.6)根据公式(12)中的传递函数，很容易直接得到不同频率f下的增益和相位；
(2.3)其次考虑针对中间频段设计第二一阶滤波控制器，其设计推导过程与步骤(2.2)中的步骤(2.2.1)到步骤(2.2.6)的内容基本相同，其中不同的是，最后需要调整a、b、c参数以补偿中间频段的相位和增益；
(2.4)其次考虑针对高频段设计第三一阶滤波控制器，其设计推导过程与步骤(2.2)中的步骤(2.2.1)到步骤(2.2.6)的内容基本相同，其中不同的是，最后需要调整a、b、c参数以补偿中间频段的相位和增益。
本发明与现有技术相比具有明显的优点和有益效果，具体而言，由上述技术方案可知：
通过配合利用角速度传感器增益调整和相位与增益分频段处理，实现了在更广的频率范围来设计不同的控制器去校正相位和增益的偏差，有效防止了人机协作机器人手臂发生抖动，为使用带来便利。
为更清楚地阐述本发明的结构特征和功效，下面结合附图与具体实施例来对本发明进行详细说明：
附图说明
图1是本发明的机器人手臂关节防抖控制硬件电路构成图；
图2是本发明的传感器增益调整流程图；
图3是本发明的补偿相位增益的控制器框图；
图4是本发明中用于调整低频段相位增益的控制器lbf频率特性图；
图5是本发明中用于调整中间频段相位增益控制器mbf频率特性图；
图6是本发明中用于调整高频段相位和增益控制器hbf频率特性图。
具体实施方式
本发明揭示了一种人机协作机器人手臂的防抖控制方法，主要包括角速度传感器增益调整(sensorgain)、相位与增益分频段处理,其过程包括以下步骤：
(1)传感器增益调整：
针对角速度传感器的不同的增益(sensorgain)，测量系统的抖动残留率,这里残留率定义为能反应机器人手臂抖动程度大小的一个量，必须要设计一个好的系统以满足比较低的残留率要求。为使残留率满足要求，将调整过程划分为两个阶段：
(1.1)粗调整处理：
通过计算所获取角速度的残留率，设定阈值r，如果残留率小于r，判定粗调整后的残留率符合要求，输出结果；如果残留率大于r，则针对残留率进行步骤1.2中进行精调整，以使其满足要求；
(1.2)精调整处理：
对于粗调整处理后不满足要求的残留率，进行精调整过程，其具体过程如下：
(1.2.1)对于步骤(1.1)中不满足要求的残留率，对其相应的增益进行加h处理，重复此过程n次，对处理后的结果，同样进行与阈值r比较，如果小于r，则符合要求，否则转入步骤(1.2.2)进行处理；
(1.2.2)针对步骤(1.2.1)中不满足要求的残留率，首先在模拟谷中根据不同的点，依据两点确定直线的原理，求出交叉点，并定义为暂定谷；其次，在暂定谷的左右两侧两点中选择残留率较低的点，并求出此点与暂定谷之间的点k,比较暂定谷与k的残留率，取其残留率最小的作为精调整处理的最终结果；
(2)相位与增益阶段分频段处理：
根据步骤(1)处理后得到的增益之积输入到相位与增益处理阶段，进行相位与增益补偿，使其满足系统稳定性的要求；
步骤(2)所述的相位与增益处理阶段的具体步骤如下：
(2.1)对频段进行划分，将其划分为低频段、中间频段和高频段，使其在不同频段内进行相位和增益补偿。
比如针对人手的抖动，主要的抖动频率集中在6hz附近，因此对于低频段的防抖控制器的设计，可以考虑以6hz为中心进行设计。而比较高频的抖动的频率可能达到500hz-800hz，我们可以考虑高频以600hz为中心设计防抖控制器。中间的频段的频率可以考虑300hz为中心进行设计防抖控制器。
(2.2)在步骤(2.1)进行频段划分以后，首先考虑在低频段进行设计第一一阶滤波控制器。
(2.2.1)设计第一一阶滤波控制器。一个一节的滤波框图可以参见图3，其中d是时滞环节，可以看作是z变换中的z-1。根据梅森公式，可以得到该第一一阶滤波控制器一节环节以z为参数的传递函数如下公式(1)
其中a，b，c为不同的常数。
(2.2.2)根据定义，有z和s的关系如下公式(2),其中s是拉普拉斯变量，ts是采样周期。
而时滞的次数是用n来表示的，图一中时间滞后的量是1，因此可以得到如下公式(3)
(2.2.3)因为仅仅靠公式(3)，很难进行工程上的仿真以及实际的计算。本步骤和下一步骤分别做两个很重要的转换-双线性变换和泰勒展开近似。双线性变换如下公式(4)所表。
(2.2.4)本步骤对
(2.2.5)将步骤2.2.4中的公式(5)和(6)代入到公式(4)，可以得到公式(7)
根据定义，已经知道有公式(8)和(9)
s＝jω(8)
0＝2πf(9)
其中f是频率，采样周期和采样频率的关系式如下面公式(10)
ts＝1/fs(10)
其中fs是系统的采样频率。因为可以得到公式
因此，一阶滤波控制器的传递函数可以表示为如下频率的函数，
(2.2.6)根据公式(12)中的传递函数，很容易直接得到不同频率f下的增益和相位。很显然，在采样频率固定的情况下，系统的频率响应特性是由参数a，b，c决定的。根据需要可以设计不同的参数，来调整需要补偿的频段的相位和增益。图4是用于调整低频段相位和增益的控制器lbf频率特性的一个实例。
(2.3)其次考虑针对中间频段设计第二一阶滤波控制器。其设计推导过程可以参考步骤(2.2)中的步骤(2.2.1)到步骤(2.2.6)的内容。其中不同的是，最后需要调整a、b、c参数以补偿中间频段的相位和增益。图5是用于调整中间频段相位和增益的控制器mbf频率特性。
(2.4)最后考虑针对高频段设计第三一阶滤波控制器。其设计推导过程可以参考步骤(2.2)中的步骤(2.2.1)到步骤(2.2.6)的内容。其中不同的是，最后需要调整a、b、c参数以补偿高频段的相位和增益。图6用于调整高频段相位和增益的控制器hbf频率特性图，通过图6，可以明显观察到在高频的时候，相位的补偿比较大。
本发明的设计重点是：通过配合利用角速度传感器增益调整和相位与增益分频段处理，实现了在更广的频率范围来设计不同的控制器去校正相位和增益的偏差，有效防止了人机协作机器人手臂发生抖动，为使用带来便利。
以上结合具体实施例描述了本发明的技术原理。这些描述只是为了解释本发明的原理，而不能以任何方式解释为对本发明保护范围的限制。基于此处的解释，本领域的技术人员不需要付出创造性的劳动即可联想到本发明的其它具体实施方式，这些方式都将落入本发明的保护范围之内。
- 还没有人留言评论。精彩留言会获得点赞！