《一种基于HSUB∞/SUB滤波的机械臂外力估计方法.pdf》由会员分享，可在线阅读，更多相关《一种基于HSUB∞/SUB滤波的机械臂外力估计方法.pdf（18页珍藏版）》请在专利查询网上搜索。
1、10申请公布号CN104050355A43申请公布日20140917CN104050355A21申请号201410112717522申请日20140325G06F19/0020110171申请人浙江工业大学地址310014浙江省杭州市下城区潮王路18号72发明人徐建明丁毅杨金桥倪洪杰臧永灿74专利代理机构杭州天正专利事务所有限公司33201代理人王兵黄美娟54发明名称一种基于H滤波的机械臂外力估计方法57摘要本发明所述的基于H滤波的机械臂外力估计方法，包括采用模拟低通滤波器对动力学方程中信号进行滤波，推导出不显含加速度的动力学模型；将外力看作是状态空间方程的状态变量，基于H滤波估计外力；再用双。
2、线性变换的方法设计IIR数字滤波器，从而实现在实际机械臂系统中应用。51INTCL权利要求书3页说明书12页附图2页19中华人民共和国国家知识产权局12发明专利申请权利要求书3页说明书12页附图2页10申请公布号CN104050355ACN104050355A1/3页21一种基于H滤波的机械臂外力估计方法，其特征在于，具体步骤如下（1）由模拟低通滤波器对动力学方程中信号进行滤波，推导出不显含加速度的机械臂动力学模型；串联机械臂的动力学方程的一般形式如下式Q是关节角度的矢量,MQ为机械臂的惯量矩阵,是离心力和哥氏力矢量,GQ是重力矢量,D表示未知的扰动力,JQ是机械臂的雅克比矩阵,FEXT代表施。
3、加在机械臂末端的外力，是关节驱动器的输出力矩，一般是由测量电机电流计算得到，所以夹杂着噪声。以下都以S表示拉普拉斯变换复变量，Z表示Z变换复变量，选用的模拟低通滤波器的传递函数为C是模拟低通滤波器的3DB截止频率，冲激响应为滤波之后的1式如下所示上式中R代表时间积分变量,由分部积分公式UDVUVVDU设UFTRMQ,可得假设机械臂的初始状态是静止的,即而F0C,将5代入4,可得6式中出现了新的卷积令假设我们用符号FS表示信号XT经过传递函数为FS的滤波器滤波之后的结果，则由6可导出不显含加速度的机械臂动力学模型权利要求书CN104050355A2/3页3（2）用双线性变换方法由模拟低通滤波器设。
4、计IIR数字滤波器；双线性变换实现从S平面到Z平面的映射为T表示采样周期，则传递函数为FS的滤波器经过双线性变换后的系统函数为同理，设期望的数字滤波器截止频率C,由S域的频率变量和Z域的频率变量的关系计算得到参数C，得到IIR滤波器的系统函数FZ，使设计的数字滤波器的截止频率可以映射到C；用设计出的IIR数字滤波器对动力学方程信号进行滤波，得如果外力FEXT变化相对平缓,上式可以写成令FZHKFZVKK0,1,2，代表各个时刻的离散值，可以得到加入外力随机游动情况下，外力FEXT的状态方程可得状态空间方程权利要求书CN104050355A3/3页4其中，WK,VK分别为状态空间方程的系统噪声和。
5、量测噪声；（3）基于建立的状态空间方程，采用H滤波方法估计状态变量FEXT,选择估计变量其中算法步骤为并且在每个时刻K必须满足条件设计H滤波中初始估计误差,系统噪声WK和量测噪声VK的权重矩阵P0,QK和RK，使代价函数的界1/满足条件。2根据权利要求1所述的方法，其特征在于步骤（2）中，由模拟滤波器的传递函数FS通过双线性变换设计IIR滤波器FZ，由S域的频率变量和Z域的频率变量的关系，设定期望的数字滤波器截止频率C得到参数C，使IIR滤波器的截止频率映射到期望截止频率。3根据权利要求1所述的方法，其特征在于步骤（3）中，将经过数字滤波器的动力学模型看作观测方程，加入外力变化的状态方程，基于。
6、状态空间方程设计H滤波器,实时估计外力。权利要求书CN104050355A1/12页5一种基于H滤波的机械臂外力估计方法（一）技术领域0001本发明属于H滤波和数字滤波技术在机械臂外力估计中的应用。（二）背景技术0002近年来，随着机器人技术的发展，机器人被应用于工业和服务业的许多领域。机器人的任务可以大致分为两类一类是机器人在运行过程中不需要与环境相接触，单纯的位置控制就可以解决问题；另一类是要求机器人与环境相接触，例如打磨，抛光，修边等，对于这类任务单纯的位置控制已经不能胜任。这类任务一般对接触力的大小有严格的要求，因为机器人末端微小的位置误差都可能导致巨大的接触力，易对机器人和目标物造成。
7、损伤。工业机器人对接触力进行控制，首先需要测量机械臂在运行过程中末端受到的外力信息。工业应用中一般在机器人末端执行器安装力传感器来测量所受到的外力，但是力传感器的价格昂贵，且在高温和潮湿环境下可能无法正常工作。由于成本和使用条件的限制，考虑基于机械臂动力学方程，由机械臂关节驱动器的力矩去估计机械臂末端受到的外力将变得非常有实际应用价值。（三）发明内容0003本发明针对实际的工业机器人中通常没有安装关节加速度传感器，使用数字低通滤波技术在基于动力学方程估计末端受到外力时避免直接使用加速度的信息，并平滑夹杂高频噪声的关节驱动器力矩信号。0004本发明由模拟低通滤波器对动力学方程中信号进行滤波，推导。
8、出不显含加速度的动力学模型，将外力看作是状态空间方程的状态变量，基于H滤波估计外力；再用双线性变换的方法设计IIR数字滤波器，从而实现在实际机器人系统中应用。0005一种基于H滤波的机械臂外力估计方法，其特征在于，具体步骤如下0006（1）由模拟低通滤波器对动力学方程中信号进行滤波，推导出不显含加速度的机械臂动力学模型；0007串联机械臂的动力学方程的一般形式如下式00080009Q是关节角度的矢量,MQ为机械臂的惯量矩阵,是离心力和哥氏力矢量,GQ是重力矢量,D表示未知的扰动力,JQ是机械臂的雅克比矩阵,FEXT代表施加在机械臂末端的外力，是关节驱动器的输出力矩，一般是由测量电机电流计算得到。
9、，所以夹杂着噪声。0010以下都以S表示拉普拉斯变换复变量，Z表示Z变换复变量，选用的模拟低通滤波器的传递函数为0011说明书CN104050355A2/12页60012C是模拟低通滤波器的3DB截止频率，冲激响应为00130014滤波之后的1式如下所示00150016上式中R代表时间积分变量,由分部积分公式0017UDVUVVDU0018设UFTRMQ,可得00190020假设机械臂的初始状态是静止的,即而F0C,将5代入4,可得002100226式中出现了新的卷积00230024令00250026假设我们用符号FS表示信号XT经过传递函数为FS的滤波器滤波之后的结果，则由6可导出不显含加速。
10、度的机械臂动力学模型00270028（2）用双线性变换方法由模拟低通滤波器设计IIR数字滤波器；0029双线性变换实现从S平面到Z平面的映射为00300031T表示采样周期，则传递函数为FS的滤波器经过双线性变换后的系统函数为00320033同理，说明书CN104050355A3/12页700340035设期望的数字滤波器截止频率C,由S域的频率变量和Z域的频率变量的关系00360037计算得到参数C，得到IIR滤波器的系统函数FZ，使设计的数字滤波器的截止频率可以映射到C；用设计出的IIR数字滤波器对动力学方程信号进行滤波，得00380039如果外力FEXT变化相对平缓,上式可以写成0040。
11、0041令00420043FZHK0044FZVK0045K0,1,2，代表各个时刻的离散值，可以得到00460047加入外力随机游动情况下，外力FEXT的状态方程00480049可得状态空间方程00500051其中，WK,VK分别为状态空间方程的系统噪声和量测噪声；0052（3）基于建立的状态空间方程，采用H滤波方法估计状态变量FEXT,选择估计变量00530054其中00550056算法步骤为说明书CN104050355A4/12页800570058并且在每个时刻K必须满足条件00590060设计H滤波中初始估计误差,系统噪声WK和量测噪声VK的权重矩阵P0,QK和RK，使代价函数的界1/。
12、满足条件。0061再由双线性变换的方法由模拟低通滤波器设计IIR数字滤波器方法，其特征在于步骤（2）中，由模拟滤波器的传递函数FS通过双线性变换设计IIR滤波器FZ，由S域的频率变量和Z域的频率变量的关系，设定期望的数字滤波器截止频率C得到参数C，使IIR滤波器的截止频率映射到期望截止频率。0062步骤（3）中，将经过数字滤波器的动力学模型看作观测方程，加入外力变化的状态方程，基于状态空间方程设计H滤波器,实时估计外力。0063本发明中不显含加速度的动力学模型推导过程为0064串联机械臂的动力学方程的一般形式如下式00650066Q是关节角度的矢量,MQ为机械臂的惯量矩阵,是离心力和哥氏力矢量。
13、,GQ是重力矢量,D表示未知的扰动力,JQ是机械臂的雅克比矩阵,FEXT代表施加在机械臂末端的外力,是关节驱动器的输出力矩,一般是由测量电机电流计算得到,所以夹杂着噪声0067以下都以S表示拉普拉斯变换复变量，Z表示Z变换复变量。我们使用模拟低通滤波器,它的传递函数为00680069C是模拟低通滤波器的3DB截止频率，冲激响应为00700071由卷积定理可知,在频率域与2相乘等于在时域与3卷积,滤波之后的1式如下所示00720073上式中R代表时间积分变量,由分部积分公式0074UDVUVVDU0075动力学方程1只有等式左边第一项包含着加速度的项,设UFTRMQ,可得说明书CN1040503。
14、55A5/12页900760077此时上式已经不显含加速度的项,我们假设机械臂的初始状态是静止的,即而F0C,将5代入4,可得007800797式中出现了新的卷积,由00800081得到第二个卷积的传递函数。由于6式中项比较多,为了表示方便,令00820083假设我们用符号FS表示信号XT经过传递函数为FS的滤波器滤波之后的结果，则由6可导出不显含加速度的机械臂动力学模型00840085我们利用双线性变换的方法去设计IIR滤波器。双线性变换实现从S平面到Z平面的映射为00860087上式中T表示采样周期,则传递函数为FS的滤波器经过双线性变换后的系统函数为00880089同理,00900091。
15、S域的频率变量和Z域的频率变量有如下关系00920093数字滤波器的截止频率对结果会有影响。理想情况下,截止频率应大于关节力矩说明书CN104050355A6/12页10信号的最高频率,但是过高的截止频率会影响滤除高频噪声的效果。数字滤波器的3DB截止频率设计方法为由期望的数字滤波器截止频率C，代入（8）计算得到参数C，即可得到截止频率为C数字滤波器的系统函数FZ。0094经过数字滤波器后,再整理7,我们可以得到00950096如果外力FEXT变化相对平缓,上式可以写成00970098令00990100FZHK0101FZVK0102可以得到01030104K0,1,2代表各个时刻的离散值，上。
16、式可以看成是状态空间的观测方程,VK是量测误差,又由于外力FEXT随时间变化,只考虑外力是随机游动的情况,可以设状态方程为01050106合并9和10可以得状态空间方程01070108其中，WK,VK分别为状态空间方程的系统噪声和量测噪声。0109本发明推导出不显含加速度的动力学模型，并加入随时间变化外力的状态方程，基于状态空间方程设计H滤波器估计外力FEXT。0110采用H滤波方法估计状态变量FEXT,选择估计变量01110112其中01130114H滤波的代价函数为说明书CN104050355A107/12页1101150116上式中ZK表示变量真值，表示估计值，即向量的加权内积,SK为对。
17、应时刻状态向量的权重矩阵,其余加权内积说明同上,代价函数可以被设定为小于1/,即01170118算法步骤为01190120并且在每个时刻K必须满足条件01210122H滤波中的P0,QK,RK分别是初始估计误差,系统噪声WK和量测噪声VK的权重矩阵,1/是代价函数的界,增大可以提高估计精度,但是必须满足12,所以应根据情况折中选择。0123本发明的优点是可以有效地滤除关节驱动器力矩信号的高频成分，平滑力矩信号；基于不显含动力学方程估计外力，不需要直接测量机器人关节的加速度信号；运用H滤波的方法可以在系统存在干扰的情况下，能有效抑制噪声干扰对力估计精度的影响。在实际应用中，本发明不依赖机械臂关节。
18、加速度信号，由关节驱动器力矩信号估计机械臂末端执行器受到的外力，可替代昂贵的力传感器；可降低机械臂力控制成本，在机器人打磨、抛光、装配和雕刻等加工领域具有应用前景。附图说明0124图1为在MATLAB中搭建的二自由度机械臂仿真模型图。ENV是整个仿真模型的环境模块,在分析模式中我们选择INVERSEDYNAMICS。EXTERNALFORCE模块中是施加在机械臂末端的外力信号。JOINT1MOTION和JOINT2MOTION模块中是驱动关节运动的信号,包括关节角位置,速度和加速度。SENSORQ1和SENSORQ2是位置传感器,输出关节1和关节2的关节角度。SENSORV1和SENSORV2。
19、是速度传感器,输出关节1和关节2的关节角速度。SENSORF1和SENSORF2是关节力矩传感器,输出关节1和关节2的力矩值。0125图2为本发明实施例中施加在机械臂末端的外力信号。0126图3为本发明实施例中在高斯白噪声均值为0，方差为01时，估计外力的效果图。说明书CN104050355A118/12页12具体实施方式0127以下结合附图和实施例对本发明的技术方案作进一步描述。0128一种基于H滤波的机械臂外力估计方法，其特征在于，具体步骤如下0129（1）由模拟低通滤波器对动力学方程中信号进行滤波，推导出不显含加速度的机械臂动力学模型；0130串联机械臂的动力学方程的一般形式如下式013。
20、10132Q是关节角度的矢量,MQ为机械臂的惯量矩阵,是离心力和哥氏力矢量,GQ是重力矢量,D表示未知的扰动力,JQ是机械臂的雅克比矩阵,FEXT代表施加在机械臂末端的外力，是关节驱动器的输出力矩，一般是由测量电机电流计算得到，所以夹杂着很强的噪声；0133以下都以S表示拉普拉斯变换复变量，Z表示Z变换复变量，选用的模拟低通滤波器的传递函数为01340135C是模拟低通滤波器的3DB截止频率，冲激响应为01360137滤波之后的1式如下所示01380139上式中R代表时间积分变量,由分部积分公式0140UDVUVVDU0141设UFTRMQ,可得01420143假设机械臂的初始状态是静止的,即。
21、而F0C,将5代入4,可得014401458式中出现了新的卷积01460147令0148说明书CN104050355A129/12页130149假设我们用符号FS表示信号XT经过传递函数为FS的滤波器滤波之后的结果，则由6可导出不显含加速度的机械臂动力学模型01500151（2）用双线性变换方法由模拟低通滤波器设计IIR数字滤波器；0152双线性变换实现从S平面到Z平面的映射为01530154T表示采样周期，则传递函数为FS的滤波器经过双线性变换后的系统函数为01550156同理，01570158设期望的数字滤波器截止频率C,由S域的频率变量和Z域的频率变量的关系01590160计算得到参数C。
22、，得到IIR滤波器的系统函数FZ，使设计的数字滤波器的截止频率可以映射到C；用设计出的IIR数字滤波器对动力学方程信号进行滤波，得01610162如果外力FEXT变化相对平缓,上式可以写成01630164令01650166FZHK0167FZVK0168K0,1,2，代表各个时刻的离散值，可以得到01690170加入外力随机游动情况下，外力FEXT的状态方程说明书CN104050355A1310/12页1401710172可得状态空间方程01730174其中，WK,VK分别为状态空间方程的系统噪声和量测噪声；0175（3）基于建立的状态空间方程，采用H滤波方法估计状态变量FEXT,选择估计变量。
23、01760177其中01780179算法步骤为01800181并且在每个时刻K必须满足条件01820183设计H滤波中初始估计误差,系统噪声WK和量测噪声VK的权重矩阵P0,QK和RK，使代价函数的界1/满足条件。0184再由双线性变换的方法由模拟低通滤波器设计IIR数字滤波器方法，其特征在于步骤（2）中，由模拟滤波器的传递函数FS通过双线性变换设计IIR滤波器FZ，由S域的频率变量和Z域的频率变量的关系，设定期望的数字滤波器截止频率C得到参数C，使IIR滤波器的截止频率映射到期望截止频率。0185步骤（3）中，将经过数字滤波器的动力学模型看作观测方程，加入外力变化的状态方程，基于状态空间方程。
24、设计H滤波器,实时估计外力。0186考虑如图1所示的二自由度机械臂仿真系统,在JOINT1MOTION和JOINT2MOTION模块中输入驱动信号,使机械臂运行一段轨迹。12T即为SENSORF1和SENSORF2输出关节1和关节2的力矩值,在力矩信号上叠加高斯白噪声,即量测误差VK是高斯白噪声。令量测误差VK的均值为0，设采样周期T001S。其中连杆1的长度L104M,质量M115KG,连杆2的长度L202M,质量M25KG,重力加速度G981M/S2。EXTERNALFORCE模块中所施加的外力信号如图2所示。为了简化二自由度机械臂的动力学方程,假设机械臂每个连杆的质量都集中在连杆的末端,。
25、因此每个连杆质心的惯性张量为零矩阵。根据1式可以推出动力学方程为说明书CN104050355A1411/12页15018701880189019001910192019301940195二自由度平面机械臂受到的外力矢量为01960197具体实施本发明的步骤为0198（1）设期望的数字滤波器截止频率C205RAD/S,由8得C2/T,计算得到IIR滤波器的系统函数为019902000201（2）用设计出的IIR数字滤波器对二自由度的机械臂动力学方程信号进行滤波，建立不显含加速度的动力学模型，得到YK和HK。0202（3）设计H滤波的SK,P0,QK和RK，本例中选择020302040205机械臂。
26、从静止状态开始运行一段轨迹,开始阶段不受到任何外力,令02060207为了满足12,通过预先确定机械臂关节运动轨迹可以得到HK的取值区间,性能上界参数应取得适当小,选择1。由算法步骤在机械臂运行时实时估计机械臂末端受到外力。在噪声方差为01时，外力估计效果如图3所示。0208本发明在外力恒定和外力快速变化阶段都能较精确地估计机械臂末端执行器受说明书CN104050355A1512/12页16到的外力,而且能有效抑制噪声干扰对力估计精度的影响。在实际应用中，本发明不依赖机械臂关节加速度信号，由关节驱动器力矩信号估计机械臂末端执行器受到的外力，可替代昂贵的力传感器；可降低机械臂力控制成本，在机器人打磨、抛光、装配和雕刻等加工领域具有应用前景。说明书CN104050355A161/2页17图1图2说明书附图CN104050355A172/2页18图3说明书附图CN104050355A18。