曲面轮廓恒力跟踪的非线性双闭环控制

张铁 胡广

摘 要：针对机器人在跟踪未知曲面轮廓运动时，由于缺乏位置信息导致的接触力不恒定的问题，提出了非线性双闭环控制方法。该方法中的双闭环包括运动倾角的实时反馈校正器，以及基于非线性PID的法向接触力控制器。非线性PID控制器是由非线性跟踪微分器（TD）和状态误差反馈（SEF）构造的，基于线性PID框架的非线性控制器。首先，根据受力模型得到基于力反馈的运动倾角校正策略；另外，考虑到多维力传感器维间耦合的影响，对Maxwell三维力传感器的解耦矩阵进行了标定；同时，提出了力信号的自适应惯性滤波方法，改善了滤波器的灵敏度和稳定性，综合性能提高了34.42%；最后，实验验证了双闭环控制方法能有效提高力控制精度和稳定性，具有较强的可执行性和适应能力。

关键词：工业机器人；力反馈；双闭环控制；轮廓跟踪；力传感器

DOI：

中图分类号：TP242.2文献标志码：A文章编号：

Nonlinear dual-loop force controller of contour following

ZHANGTie， HUGuang

（School of Mechanical and Automotive Engineering， South China University of Technology， Guangzhou 510641， China）

Abstract：Aiming at the problem that the contact force caused by the lack of position information is not constant， the nonlinear double closed-loop control method is proposed for the robot to track the unknown surface contour.Double-loop in this method including movement angle adjuster based on real-time feedback， and the nonlinear PID normal contact force controller. This method is a nonlinear controller based on classical PID frame， composed of nonlinear tracking differentiator （TD） and state error feedbacks （SEF）. First， according to the force model， the motion inclination correction strategy based on force feedback is proposed.In consideration of the cross axis coupling of multi-dimensional force sensor， the decoupling matrix of Maxwell force sensor is calibrated. Meanwhile， adaptive inertia filtering method is proposed to improve the sensitivity and stability of filter， and the comprehensive performance is improved by 34.42%. Finally， the dual-loop control method is verified by experiments. The proposed method can effectively improve the precision and stability， with a strong ability to adapt and enforceability.

Keywords： industrialrobot； force feedback；dual-loop control； contour following； force sensor

引言

力控制輪廓跟踪通常用于机器人的表面加工作业中，比如抛光、打磨、去毛刺等。在这些应用中，机器人末端工具与工件之间应该保持期望的接触力[1-2]。如果接触力过大会损坏工件或工具，相反，如果接触力过小则会脱离接触。为了实现恒力轮廓跟踪，Antonio Lopes和Fernando Almeida[3]设计了一种机器人主动辅助力-阻抗控制装置，该装置采用六自由度并联设计，可以实现插孔、轮廓跟踪等任务。采用辅助装置的方式虽然可以增加机器人的柔顺性，但其机械结构较复杂；Heiko[4]等设计了基于视觉和力传感器切换的机器人轮廓跟踪系统，视觉/力切换控制能够稳定的进行轮廓跟踪任务，但需要用到昂贵的激光视觉传感器。KazuoKiguchi[5]在机器人跟踪未知曲面工件的任务中，提出了一种模糊力向量的方法，用来确定力/位置混合控制中的力控制方向。模糊向量法可以确定力控制方向的范围，但其精确程度受到传感器干扰或噪声的影响；Alexander Winkler和JozefSuchy[6]通过在力/位置控制器中加入双积分器，降低了力控制的稳态误差。双积分器的力控制方向固定，不能跟随轮廓的改变而调整力控制方向。

本文提出的轮廓跟踪方法是一种非线性双闭环控制方法。双闭环包括运动倾角的实时反馈校正器，以及基于非线性PID（nonlinear PID， NLPID）的法向接触力控制器。两个闭环控制，通过实时调整力控制方向，同时实现了未知轮廓跟踪和法向接触力恒定两个目的。只要给定任意的初始运动倾角，机器人末端工具都能以恒定的法向接触力跟踪未知曲面轮廓运动，该方法具有较强的可执行性和适应能力。

曲面轮廓跟踪运动

基于恒定接触力的曲面轮廓跟踪操作，通常在机器人的抛光、打磨、去毛刺等工业应用中有非常重要的作用。曲面轮廓跟踪运动的目的是根据力传感器检测到的接触力，控制机器人末端工具沿未知轮廓的曲面运动，并施加恒定的法向压力。

曲面轮廓跟踪的受力分析

曲面轮廓跟踪运动模型如图1所示，图中坐标系{T}表示末端工具的运动坐标系和力传感器坐标系，坐标系{C}的X轴与轮廓切线同向，Y轴与轮廓表面法线同向， 表示两个坐标系的X轴之间夹角。r表示运动方向，r与XT之间的夹角为 。为了保证法向接触力恒定，需要沿法向进行调整，即 ，所以 。当工具末端以 的速度沿轮廓切向运动时，工具与工件之间的法向和切向作用力分别为 和 。由传感器检测到的兩个方向的力分别是 和 ，它们的大小等于 和 在传感器坐标系下的投影，于是可得：

（1）

通过上式可以得到法向力 和切向力 ：

（2）

图1 受力分析

Fig.1 Stress analysis

由图1可知，曲面跟踪的运动时，工具沿曲面切线方向以匀速 运动的同时，要保证法向接触力恒定。为保证法向接触力恒定，在法向上由测量得到的法向接触力，根据控制算法产生一个法向调整速度 。将曲面法向和切向运动 和 ，转换成末端运动坐标系的运动速度 和 得：

（3）

双闭环恒力控制方法

工件与工具末端之间的接触力是由系统的变形产生的，图1中 表示法向形变量。当由形变量 产生的法向接触力与期望的法向接触力相等时，即当 时，其中k表示法向刚度，则工具沿法线方向不需要调整，即 ，则 。 是 与 的合运动方向与XT之间的夹角，即工具末端的运动倾角，有：

（4）

接触状态时，期望的法向接触力： ，力反馈控制周期为： 。当 ，实际法向力：

（5）

当 时，有：

（6）

式中：v表示末端运动速度。则可知实际法向接触力与 及 之间的关系为：

（7）

由此可知： 时， ， 时， 。即在图1中，当运动方向为r1时，实际法向接触力 大于期望接触力 ；当运动方向为r2时，实际法向接触力 小于期望接触力 。利用这个特点，可以根据反馈的实际法向接触力 来调整运动倾角 ，使得 的同时 。

由上式求解出 为：

（8）

可知偏转角 的大小与法向接触力误差 ，以及末端运动倾角 有关。因此，可以根据力传感器反馈得到的法向接触力误差和当前时刻的运动倾角 ，来校正运动倾角，使得 。

由此提出了如图2所示的基于NLPID的双闭环力控制方法。控制器中的双闭环包括基于力反馈的运动倾角校正器，以及基于NLPID的法向接触力控制器。图中 表示期望的法向接触力， 表示实际法向接触力，u表示控制量， 表示坐标系{C}到{T}的运动变换矩阵，如式（3）所示。跟踪微分器（tracking differentiator， TD）和状态误差反馈（state error feedback， SEF）构成了NLPID控制器。误差 与当前运动倾角 用来校正运动倾角，如式（9）所示。

图2双闭环控制

Fig.2 Double loop control

NLPID控制器的输出是法向的运动调整量 ，运动控制器根据输入的 将法向的运动向量 ，根据坐标关系转换成工具坐标系下的运动向量 。用传感器测量得到的两个轴向力 和 ，以及 来计算实际测量得到的法向接触力 ，并将 作为控制器和校正器的反馈信号。如果初始运动倾角 越接近实际 ，那么运动倾角 就能更快收敛到轮廓倾角 。校正器根据输入的 和 得到校正之后的运动倾角 ，由式（8）可知校正公式为：

（9）

式中： 表示n时刻的运动倾角。

图2中，SEF将误差 、误差的积分及误差的微分 的线性组合作为控制器的输出，与PID控制的结构相同，所以SEF的传递函数为：

（10）

式中： 分别表示SEF的控制参数。

控制对象 的输入为法相速度信号 ，输出为法相接触力 ，根据弹簧滑块模型忽略阻尼作用， 可以简化为一个积分环节：

（11）

式中：τ表示积分常数。

由式（10）和（11）得到的系统开环传递函数为：

（12）

开环传递函数 在S平面右半部无极点，为了得到稳定的闭环系统，开环幅相频率特性曲线不能包围 点，即幅相频率特性曲线与实轴的交点大于-1：

（13）

由式（13）可知，适当调整控制参数，使得稳定条件 成立，闭环系统就能稳定。

非线性PID控制

NLPID力控制器的基本结构如图3所示。图中所示的NLPID控制用到了两个非线性跟踪微分器，分别用来安排过渡过程以及对系统输出进行跟踪微分处理。图中 表示期望的接触力； 和 分别代表输入信号的跟踪信号及其微分信号； 和 分别表示输出信号的跟踪信号及其微分信号； ， ，分别表示状态误差及其微分；u是由误差的反馈组合得到的控制量； 和 分别表示对象模型及干扰； 表示滤波器； 表示测量得到的实际法向接触力。

带有惯性滤波的非线性PID控制算法流程如下：

首先，以 作为输入安排过度过程[7]：

（14）

式中： 是最速控制综合函数[7]；需要调整的参数为 和 ； 是中间变量。其中 代表跟踪速度的快慢；滤波因子 ，其中h是控制步长。

将系统的输出信号进行惯性滤波：

（15）

式中： 是惯性滤波系数， 是原始输出信号， 是滤波之后的输出信号。

利用跟踪微分器提取输出信号的微分[7]：

（16）

式中： 是输入， 、 是输出， 是中间变量。力传感器信号中一般都有噪声，取滤波因子 。

最后，产生误差信号和误差反馈控制信号：

（17）

式中： 是需要调整的控制参数。因为安排了过度过程以及对输出信号进行了跟踪微分处理，使得控制参数可以在较大的范围内取值，而不影响控制效果[8]。

力信号的测量与处理

三维力传感器的解耦标定

实验中采用的力传感器是Maxwell的A3D46-20kg三维力傳感器。其非线性小于 ，重复性小于 。由于没有能够检测力矩信息的通道，三维力传感器只能在固定点受力，如果受力点改变，必须重新进行标定，否则会产生力耦合使测得的力值不正确，导致机器人误操作[9]。

三维力传感器在校验出厂时，给出的是基于O点的标定矩阵，即受力点必须设定在O点，如图4所示。三维力解耦方程为： 。其中 是三个方向的力向量； 是三个通道输出的电压向量； 是O点的解耦矩阵。

如图4所示，当在 点受到X方向的力 时，传感器相当于在O点受到 的力和 的扭矩。如果在O点单独作用力 时输出的电压是 ，而在O点同时作用 和 时输出的电压是 。由于三维力传感器没有测量力矩的通道，由 产生的变形影响到其他通道的输出电压，使得 。而利用公式 进行计算的时候，由 导致的输出电压误差就会叠加到各轴的测量力值上，导致三个轴的测量值都不准确。

采用的解耦标定方法是在受力点附近的三个方向悬挂已知重量的砝码，分别测量三个通道的输出电压，得到矩阵方程：

（18）

式中： 表示对角矩阵 ， 是解耦矩阵， 表示加载力矩阵 时，三个通道的输出电压矩阵。由以上方程可以得到解耦矩阵： 。

标定过程中，在受力点处，分别于三个受力方向加载21.76N的力，并测量传感器三个通道输出的电压值，求取电压的平均值，得到解耦矩阵：

（19）

为了验证该解耦矩阵能否准确测量各轴的力，在X轴与Y轴之间 的方向加载12N的力，并在6s左右增加到13.5N。采样周期为 。结果如图5所示，在0-5s时X和Y轴的理论值是8.49N，7-13s时X和Y轴的理论值是9.55N。图中5-7s的波动是在增加重量时引起的振动。稳定状态时，X和Y轴的测量误差不超过 ，小于厂家给出的标定误差 。结果表明，三维力传感器通过标定得到的解耦矩阵能够准确测量各轴的力，且具有较高的精度，满足实际应用要求。

图5解耦效果

Fig.5 Decoupling effect

自适应惯性滤波

力信号中包含测量误差与干扰信号，需要进行滤波处理。设计滤波器时要考虑到力控制系统中实时性的要求，故采用较简单的惯性滤波，即一阶滞后滤波器。惯性滤波器的传递函数是一个时间常数为T的惯性环节，即：

（20）

式中： 和 分别表示输入和输出。

设采样时间间隔为 ，那么惯性滤波在时域上可以表示成：

（21）

式中： 和 分别表示当前采样值和当前滤波值， 表示上一时刻滤波值。设：

（22）

则惯性滤波的离散表达式可以简单的表示成：

（23）

式中：滤波系数 。

滤波系数 的取值越靠近0，则滤波结果越平稳，越不容易被干扰所影响；但是延时也较大，跟随信号变化的速度缓慢；而 取值越靠近1时，灵敏度虽然高，但滤波效果不明显，波动较大。为了达到稳定性好灵敏度又高的滤波效果，采用参数自适应调整策略来优化惯性滤波器。优化后的滤波器应该有如下特点：

1）当输入信号趋于稳定，在一个值上下振荡时，应该采取稳定性优先策略，取小滤波系数 。

2）当信号朝一个方向快速变化时，滤波结果需要及时跟进，这时就要放弃稳定性而采取灵敏度优先策略，即选择较大的滤波系数 。

于是提出了滤波系数的自适应调整策略如下：

（24）

式中： 和 表示连续两次采样信号的变化方向， 表示滤波系数的调整速度，M表示信号变化阀值， 表示两次输入信号的差， 是符号函数。

为了验证式（24）提出的自适应惯性滤波的滤波效果，用力传感器测量的数据进行了滤波实验。分别用固定滤波系数和自适应滤波系数处理同一段采样数据并进行对比。滤波参数如表1所示：

滤波效果如图6所示，上图为滤波信号，下图为滤波系数变化情况。可以看出在0-9s及11-20s的时候，采样信号趋于平稳， 的值在0附近波动，自适应惯性滤波信号也非常平滑，没有产生波动；而在 左右，采样值朝一个方向出现剧烈变化时， 自动增大，达到 左右，滤波值能及时跟踪采样的变化。结果说明，自适应惯性滤波能够根据实际采样数据调整滤波系数，从而达到灵敏度高且稳定性好的滤波效果。

图6自适应惯性滤波

Fig.6 Adaptiveinertial filtering

曲面轮廓恒力跟踪实验

曲面轮廓跟踪实验装置如图7所示。实验时假定初始处于接触状态，且曲面轮廓未知。力传感器的信号通过放大器后输出模拟量信号，通过端子板的A/D端口输入到控制器。控制器接收到电压信号后，通过解耦矩阵（19）计算得到三维力传感器在各方向上检测到的力信号，将力信号由式（24）滤波后作为系统的反馈信号。同时控制器根据控制算法输出控制量，通过速度控制模式控制工具末端的运动。

斜面恒力跟踪控制

为了验证基于力反馈的运动倾角调整方法的有效性，进行了力控制斜面轮廓跟踪实验。采样周期 ，斜面的倾角 ，任意选取初始的 ，期望的法向接触力 ，取跟踪运动速度 ，经过检测得到的接触刚度 。非线性PID控制器的主要参数如表2所示。图8所示为运动倾角 与法向接触力 的实时调整情况。

运动控制开始后，运动倾角 迅速调整到轮廓倾角 的同时，法向接触力 也逐渐稳定到期望接触力 。调整时间 ，稳态时 ，平均跟踪误差 ，平均法向接触力 ，平均力控制误差 。由此说明基于力反馈的运动倾角调整方法，可以实现恒定法向接触力的斜面跟踪运动控制。

未知曲面轮廓跟踪

如果工件表面轮廓是未知曲面时，轮廓倾角 是时变的。为了验证这种情况下，能否通过实时校正运动倾角 的方式控制恒定的法向接触力，利用双闭环控制方法对图9所示的曲面工件进行了轮廓跟踪实验。实验过程中任意选取初始的 ，期望的法向接触力 ，曲面跟踪运动的合速度 ，其他运动控制参数与4.1节相同。

图9曲面工件

Fig.9 Curved surface

工具末端的运动轨迹与曲面轮廓之间的关系如图10所示，最大轮廓跟踪误差 。控制器的控制目标是恒定的法向接触力，而机器人在各个位姿的刚度都不一样，所以产生了误差。

运动倾角与实际轮廓倾角之间的关系如图11所示。由图中可以看出，经过反馈校正后的运动倾角 能够跟踪实际轮廓倾角 的变化。图12表示轮廓跟踪运动过程中实际法向接触力的变化情况，由图可以看出在整个运动过程中工具末端一直与工件表面接触，而且实际法向接触力稳定在期望接触力附近。平均法向接触力 ，法向接触力控制误差为 ，处于力传感器测量误差范围之内。由此可以说明，非线性双闭环控制方法，在对未知曲面轮廓的恒力跟踪运动上是可行的，且保证了一定的法向接触力控制精度。

为了与其他方法进行对比，设计了单方向的接触力闭环控制实验。与双闭环控制相比，该方法不进行运动倾角校正，所以力控制方向不能保证始终处于轮廓法向。实验结果分别如图13和14所示。图中 和 分别表示在传感器坐标系的X方向和Y方向测量的接触力， 表示轮廓法向接触力。

图13所示法向接触力的平均值为： ，平均误差为： ，最大误差为： ；而图14所示法向接触力的平均值为： ，平均误差为： ，最大误差为： 。对比实验结果可以看出，单向力控制可以使Y轴的接触力比较稳定，却忽略了法向接触力的波动；而双闭环控制则可以确保法向接触力的稳定。由此可以看出双闭环控制可以提高法向接触力的稳定性和控制精度。

结论

为了实现未知曲面轮廓的恒力跟踪运动，提出了一种基于运动倾角实时校正及法向接触力控制的非线性双闭环控制方法。鉴于三维力传感器的强耦合特性，对其进行了解耦标定，标定后的测量误差小于 ；设计了自适应惯性滤波方法，同时改善了滤波器的灵敏度和稳定性，综合性能提高34.42%。分析了轮廓跟踪运动模型，推导出接触力反馈校正运动倾角的公式，从而提出双闭环控制结构。实验表明，双闭环控制方法可以实现恒力跟踪控制，且轮廓倾角跟踪误差小于0.5%，平均法向接触力控制误差约为 ，比单向力闭环控制的法向接触力稳定性提高31.2%。本文提出的非线性双闭环控制方法，可用于机器人打磨抛光时对工件轮廓进行跟踪，具有一定的法向接触力控制精度。

参考文献：

[1]JAVIERPliego， MARCO A Arteaga. Adaptive position/f?orce control for robot manipulators in contact witha rigid surface with uncertain parameters[J]. European Journal of Control， 2015，22：1-12.

[2]魏立新，李二超，王洪瑞. 基于混合优化神经网络的机器人力/位置控制[J]. 电机与控制学报，2006，02：151-153，159.

WEILixin，LI Erchao，WANG Hongrui. Hybrid force/position control based on mixed optimal neural n?e?tworks[J]. Electric Machines and Control，2006，02：151-153，159.

[3]ANTONIO Lopes， FERNANDO Almeida. A forcei?m?peda?n?c?e controlled industrial robot using an a?ctiveroboti?caux?i?l?i?arydevice[J]. Robotics and ComputerIntegrat?e?dManufacturing， 2008，24：299-309.

[4]HEIKO Koch， ALEXANDERKonig，ALEXANDRAWeigl-Seitz， et al. Force， acceleration and vision sensor fusion for contour following tasks with an industrial rob?ot[J].IEEE International Symposium on Robotic & Sensors Environments， 2011：1-6.

[5] KAZUOKiguchi. Position/force control of robot ma-nipulators for geometrically unknown objec?t?s usin-gfuzzy neural networks[J]. IEEE Transactions On I-ndustrial Electronics， 2000，47（3）：641-649.

[6]ALEXANDER Winkler， JOZEFSuchy. Force controlled c?o?ntour following on unknownobjects with an ind?u?strialrobot[J]. IEEE International Symposium on R?o?b?otic & Sensors Environments，2013：208-213.

[7]韓京清. 自抗扰控制技术[M]. 北京： 国防工业出版社，2008：17-72.

[8]赵永祥， 夏长亮， 宋战锋， 等. 变速恒频风力发电系统风机转速非线性PID控制[J].中国电机工程学报， 20?08，28（11）：133-138.

ZHAOYongxiang， XIAChangliang， SONGZhanfeng， et al. Nonlinear PID rotating speed c?ontrol of variable speed constant frequency windturbinesystem[J]. Proceedings of the CSEE，2008，28（11）：133-138.

[9]曹会彬， 孙玉香， 刘利民， 等. 多维力传感器耦合分析及解耦方法的研究[J]. 传感技术学报， 2011，24（8）：1136-1140.

CAOHuibin， SUNYuxiang， LIU Limin， et al. Coupling analysis of multi-axis force sensor and research of decoupling method[J]. Chinese Journal of Sensors and Actuators，2011，24（8）：1136-1140.