工业机器人基于工作空间的力/位PID控制系统研究

陈浩文 ，张文辉 ，，钟秋波 ，周书华 ，叶晓平

（1.南京工程学院自动化学院，江苏南京211167；2.机械制造系统工程国家重点实验室，陕西西安710000；3.浙江经济职业技术学院汽车技术学院，浙江杭州310000；4.浙江省文创产品数字化设计与智能制造重点实验室，浙江丽水323000）

随着工业自动化进程的不断发展，工业机器人将扮演越来越重要的角色，大量工业机器人被用来代替人类进行工作，以此来提高生产效率，这也使得工业机器人领域一直是研究的热点。与传统的工业机器人不同的是，当今工业机器人面临打磨、抛光和装配等任务时，单一的位置控制已经无法满足生产的实际要求，因此能够进行力位置混合控制的工业机器人成为了现在科研人员研究的方向。这类机器人能够完成更多复杂的工作任务，具有更高的灵活性、安全性、智能化等特点[1]。力/位置混合控制与阻抗控制是机器人力位控制的两种基本方法。

Mason[2]于1979年提出了同时控制力和位置的概念以及关于机械臂关节柔顺的思想，并在柔顺控制中引入了自然约束与人为约束的概念，将任务空间分为互补正交的力子空间和速度子空间，提出在运动约束方向上进行力控制，在力约束方向上进行位置控制。在这基础上，1981年Raibertt与Craig[3]第一个提出了力/位置混合控制器，该控制器通过雅可比矩阵将工作空间中任意方向的力与位置分配到各个关节的控制器上，使得机器人以独立的形式同时控制力与位置。此后杨宗泉[4]再提出了基于模糊补偿的机器人力/位置控制策略。

阻抗控制是一种间接力控制。其是在阻尼控制和刚度控制的基础上发展起来的。Hogan[5]于1987年首次系统地阐述了阻抗控制的概念，并且提出了基于机器人动力学模型的阻抗控制策略，对机器人柔顺控制方向产生了深远的影响。周晓东等[6]提出了基于粒子群算法的阻抗控制，解决了阻抗参数选取的不确定性问题，提高了系统的鲁棒性。王宇驰等[7]针对机器人建模中的不确定性，提出了一种对机器人阻抗控制中不确定性进行补偿的方法，能够表现出很好的力跟踪效果。

本文将研究工业机器人在执行复杂工作任务中的力/位置控制问题，根据关节空间中工业机器人的动力学模型推导出工作空间中动力学模型，并分别采用PID控制算法设计力/位置混合控制系统和阻抗控制系统，且证明了控制系统的稳定性。与以往研究不同，为了深入研究两种控制方法的系统特性，本文将首次基于对等工业机器人模型，利用Matlab/Simulink进行对比数值实验研究[8]。

1 工作空间机器人动力学建模

当N自由度机械臂末端与外界环境表面接触时，关节空间中动力学方程为

式中：q、q、q分别为机械臂关节角度向量、角速度向量与角加速度向量；D（q）为n×n阶正定惯性矩阵；C（q，q）q为 n×n 阶离心力与哥氏力项；G（q）为重力矩向量；τ为关节力矩向量；J为Jacobian矩阵；F为机械臂末端与环境之间的作用力。

为了对末端位置进行控制，因此需要将关节空间中的动力学方程转换为工作空间中的动力学方程。首先由虚功原理得

因此得到工作空间中的动力学方程

综上所述，机械臂动力学模型是一个高度复杂、强耦合的非线性时变方程，而且其复杂程度会随着自由度的增加而上升[9]。

2 力/位置混合控制系统设计

首先当忽略机械臂动力学模型中的重力项时，方程为

且传统的PD控制律

下面构建李雅普诺夫函数[10]进行系统稳定性分析：

经过微分得

半负定的，所以能量是耗散的。当V˙=0 时，有θ˙=0 和θ¨=0，即e˙≡0，≡0。在此情况下，由闭环系统误差方程可得Kpe=0。由于Kp是可逆的，因此只有e=0。由LaSalle定理可知，系统全局渐进稳定的平衡点，即从任意初始条件（θ0，θ˙0）出发，均有 θ0→θd，θ˙0→0。

力/位混合控制系统是由力控制环与位置控制环来对力与位置进行控制的，位置控制器采用PD控制（参数为Kp、Kd），力控制器采用PID控制（参数为Kfp、Kfi、Kfd）。力/位混合控制系统框图如图1所示。

图1 力/位混合控制系统框图

在工作空间中设计控制律为

3 阻抗控制系统设计

对于基于位置的阻抗控制系统来说，其主要是由位置控制器、阻抗控制模型以及被控对象的动力学模型等组成的。阻抗控制系统框图如图2所示。

图2 阻抗控制系统框图

由上面分析得到工作空间中机械臂末端与环境接触后的动力学模型为：

根据机械手末端与环境表面接触时产生动态接触力作用的情况，建立如下阻抗模型：

式中xc、x为末端指令轨迹与实际轨迹，M、B和K分别为质量、阻尼与刚度系数矩阵。通过加入末端期望接触力Fd，来计算Fd与环境表面对机械手末端的反作用力Fe之间的差值。根据阻抗模型选择适当的阻抗系数，计算出误差E（E为阻抗轨迹xd与输入指令轨迹xc之间的差值），再将其与输入的指令轨迹相加得出我们所期望的阻抗轨迹xd，即：

通过传统的PD控制对xd与实际反馈的位置x的误差e进行控制，设计控制律为

进而来实现机械臂的力控制。

4 控制系统实验与分析

为了降低动力学模型的复杂程度，提高系统效率与稳定性，以二自由度工业机器人为控制对象（如图3所示），分别采用阻抗控制与力/位混合控制两种基本方法对机械臂力控制系统进行数值实验研究。

图3 二自由度工业机器人

4.1 阻抗控制

机械臂连杆长度设为l1+l2=1；在工作空间中末端的理想跟踪轨迹取 xc=cosπt、yc=sinπt，其为一个半径为1，圆心为（0，0）的圆；接触环境表面的位置设在xe=0.5处，且在x轴向上的期望接触力为Fd=10 N；阻抗系数选择M=diag［ 1.0］、B=diag［ 10］、K=diag［ 200 ］，分别为质量、阻尼和刚度系数矩阵；位置控制器中PD参数设置：Kp=diag［ 15 ］、Kd=diag［ 10 ］。数值实验结果如图4～图7所示。

图4 末端关节的位置跟踪

图5 对末端轨迹的跟踪

图6 末端接触力跟踪效果

图7 关节实际控制输入

从图中可以看出机械臂末端在t=0.3～1.7 s之间，未与环境表面接触时，x与y方向上的实际轨迹均能实现对指令轨迹xc与yc的跟踪并做逆时针圆周运动，且末端接触力Fe恒为0N；而当t=1.8s后，在x轴方向上的位置跟踪出现了误差，这是因为其在x方向受到了位置约束，即设置的接触面位置x=0.5，此时接触力Fe上升并跟踪为10N，之后沿着接触面向上运动；当t=2.5s时，机械臂末端再次与接触面分离，x方向上恢复精确的位置跟踪，接触力恢复为0N，此后反复循环运动。该实验进一步验证了各参数选择的合理性。通过位置控制间接地实现了对末端接触力的控制，保证了机械臂末端不会因为力过大而对接触面或机械臂本体的结构造成破坏。

4.2 力/位置混合控制

力/位置混合控制同时也是机器人主动柔顺控制的重要方法之一，主要是通过位置控制环与力控制环分别对位置和力进行控制。被控对象取同种二自由度机械臂，定义第一个连杆的质量m1和第二个连杆的质量m2都为1，连杆长度l1与l2都设为1，q为关节角度；期望末端轨迹设为工作空间的圆，即末端期望轨迹输入为xc=cos（πt），yc=sin（πt）；接触环境表面的位置设在 xe=0.5 处，期望力Fd=10 N，力控制器PID参数取Kp=80、Ki=2、Kd=10，位置控制器PD参数取Kp=19000、Kd=1500。根据图1力/位混合控制结构图，利用Simulink建立模型并仿真，数值实验结果如图8～图11所示。

图8 末端关节的位置跟踪

图9 对末端轨迹的跟踪

图10 末端接触力跟踪效果

图11 关节实际控制输入

从图8、图9可以看出机械臂末端在未与环境表面接触时，机械臂能够通过位置控制环对末端期望轨迹xc、yc进行跟踪控制，验证了PD控制参数选取的合理性与可靠性；当t=0.4～1.5 s之间时，机器人末端与环境表面接触，受到了位置约束，此时机械臂末端在x方向上位置跟踪失效，沿着接触表面运动，通过力控制环对机械臂末端在x方向上产生的接触力按照期望力的大小进行调节控制，使得接触力Fe达到10 N，而在方向上依然进行位置控制；当t=2.4 s时，机械臂末端离开接触面，此时接触力Fe为0 N，这就实现了机械臂末端的力与位置的同时控制。

通过数值实验对比研究，得出力/位置混合控制能够完成对机械臂末端力与位置的跟踪，但由于力控制器与位置控制器PID参数的合理性很难保证，使得控制器的精度达不到期望的要求，同时机械臂末端的力传感器对末端力进行检测并反馈时容易受外界环境的干扰而不够精确，且该方法对任务与环境的依赖性特别大。根据外界环境的变化，机械臂需要在力控制与位置控制模式之间不停的切换，运算过程十分繁杂。

而阻抗控制的本质属于位置控制，即间接力控制，因此避免了不确定性作用力对系统产生的干扰。从图3、图4可以看出，其具有较强的鲁棒性，也能够完成从无约束运动到有约束运动之间的转换，但是阻抗模型是由阻抗参数确定的，而阻抗参数选择的合理性很难得到，同时也无法精确获得接触环境的位置与刚度，这就使得系统在运行时无法避免一些误差的产生。

5 总结

针对复杂工作任务中工业机器人力/位控制问题，推导了工作空间的工业机器人动力学模型，分别基于PID算法设计了力/位置混合控制系统和阻抗控制系统，并基于Lyapunov稳定性理论证明了控制系统的稳定性。为了深入研究两种控制方法的系统特性，首次基于对等工业机器人模型进行了比较系统的实验研究，研究结果表明，在期望接触力为10 N的情况下，阻抗控制表现出更加优越的柔顺性，其在力/位置控制方面精度更高，易于实现。研究结果对于相关力/位置控制技术的工程应用具有一定的借鉴意义。