基于腕力传感器的绳驱动拟人臂的力/位混合控制研究

唐振山，陈伟海

（北京航空航天大学自动化科学与电气工程学院，北京 100191）

0 引 言

绳驱动拟人臂样机是一种串并联混合结构，样机由并联驱动肩关节、键驱动肘关节和并联驱动腕关节组成[1]，采用钢丝绳代替连杆作为驱动元件并将驱动电机安装于基座上。它结合了并联结构高刚度、高精度、高负载能力和绳驱动结构重量轻、灵活性好的优点，会在广泛的领域拥有较好的应用前景。但由于绳索固有的柔性[2]，机构安装的初始定位误差、电机卷扬机构缠绕误差的存在，导致这种非直接驱动系统在任务空间的开环精度和刚度较低，从而使其在微操作或精度要求较高场合的应用受到了限制。本文采用3个相交于一点的单轴机构来实现自由度球关节的功能，通过安装在关节处的角度传感器，实现高精度的位置控制。

机器人在从事装配、打磨、抛光及轮廓跟踪等任务时，不但需要高精度的位置控制，还需要实时的控制其与环境之间的接触力，本文主要是在阻抗控制[3]算法的基础上进行位置控制和力控制。

1 运动学模型分析

绳驱动拟人臂的示意图如图1所示，由3个关节组成，即3-DOF的并联肩关节、1-DOF的肘关节、3-DOF的并联腕关节。建立如图1的坐标系，基坐标系为{Ko}，肩关节坐标系为{Ks}，肘关节坐标系{Ke}，腕关节坐标系为{Kw}，末端操作手坐标系{KT}。

图1 七自由度拟人臂示意图

机械臂的前向运动学公式为

式中：{TOS}，{TSE}，{TEW}，{TWF}——肩关节坐标系相对基坐标系，肘关节坐标系相对肩关节坐标系，腕关节坐标系相对肘关节坐标系及工具坐标系相对腕关节坐标系的变化矩阵，它们分别如式（2）～式（5）所示

式中：po，pa，pb，pc——关节之间连杆的长度；

q1，q2，q3——肩关节姿态的欧拉角；

q4——肘关节的角度；

q5，q6，q7——腕关节姿态的欧拉角；

s1，s2，s3，s4，s5，s6，s7——7个关节角对应的7个旋量[4]。

2 阻抗模型分析

阻抗控制中的阻抗包括操作机物理上内在的阻抗和采用主动控制带来的阻抗两部分。操作机物理上内在的阻抗是不变的，阻抗控制的目的就是通过选用主动控制参数来实现理想的目标阻抗。阻抗控制中，阻抗可以为任意的函数形式：f=Z（x），但考虑到实现的可能，采用式（6）的2阶线性阻抗[5]

Md∈R3×3，Bd∈R3×3——系统的理想质量矩阵和阻尼矩阵；

F∈R3——工具与环境的接触力向量；

Kf∈R3×3——力反馈增益矩阵；，Fd——末端操作手理想的速度向量，加速度向量和作用力向量；

Md，Bd，Kf均设定为正定的对角阵。

由式（7）可解得

X=e-Md-1BdtX（0）+

为了进行实际控制，把式（8）进行离散化处理，采样时间为 Δt，假设 Md，Bd，Kf，F 和 Fd在采样间隔 Δt·（k-1）≤t＜Δt·k 是连续的，令，则式（8）可变换为

式中：Ki∈R3×3——正定对角的积分增益矩阵。

3 轨迹规划

3.1 标准环境的轨迹规划

当环境能精确建模时，如图2所示，采用解析的方式得到环境的信息，从而得到机器人的准确参考轨迹。例如末端跟踪的轨迹为图2所示的X_Y平面中圆心为B的圆弧时，令机器人末端操作手的法向量平行于Z轴，即垂直于X_Y平面，用式（11）参数方程进行轨迹建模

图2 X-Y平面上的圆弧轨迹

式中：R——圆弧半径；

θ——圆弧上点对应的角度值。

假设机器人末端执行器做圆弧运动时，角速度曲线为加速、匀速、减速的梯形曲线，加速、减速时间为Ta，总时间为T，则最大角速度为

式中：θ0——整个圆弧对应的角度。

最大角加速度为

可得角速度方程为

为了进行数字控制，把式（14）离散化，设采样时间为 Δt，令

轨迹方程变换为

式中：n——采样点数。

可得末端的位置向量为

设末端的姿态保持不变一直为Ra∈R3×3，则末端的位置可以表示为

3.2 未知环境轨迹规划

当机器人与一些环境比如平面、圆弧面等标准环境接触时，环境一般能被精确建模，环境的每一点的切向和法向能通过理论计算得到；但当机器人与未知环境发生接触时，例如对不规则物体的表面打磨、轮廓跟踪等，机器人末端的参考运动轨迹并不能提前给定，这时就需要通过力、位置等反馈信息进行在线轨迹规划，根据当前点的位置和与环境作用力进行在线计算得到下一点的参考值[6]。

当机器人与未知环境接触时，如图3所示，Fx与Fy为安装机器人末端的多维力传感器测得的与环境之间的接触力，p为垂直于运动方向的单位法向量，w和t分别为工具与环境接触点处的单位法向量和切向量，忽略工具与环境之间的摩擦[7]。

图3 不确定性环境下的轨迹模型

w和t可由式（19）得出

由于安装在末端的力传感器上工具坐标系为动坐标系，从而测得的多维力信息需要一定的矩阵变换才能得到未知环境的信息。

式中：Re——当前点的末端的姿态矩阵；

F′——力传感器测得的力向量；

F——相对于全局坐标系的力向量。

当末端操作手运动到 P（k）点时，P（k+1）点的位置信息可通过式（21）计算得到

4 位置控制算法

拟人臂在任务空间的定位操作是实现拟人臂各种复杂运动的基础，只有样机的位置控制精度达到要求，轨迹跟踪、微动等任务才能快速顺利完成。实际中样机模型参数的准确性很大程度上影响了末端执行器的定位精度，样机在加工和装配过程中，间隙、回差、非垂直连接等因素，加之绳索本身的柔性，都导致系统精度的降低。为了消除各种不确定因素带来的误差，采用关节角闭环控制的方式，这样可以不必对每个误差源进行建模并补偿消除，直接通过PID控制进行每个关节的控制。样机由3个关节构成：2个三自由度的球关节和1个一自由度的肘关节，球关节是1个多输入多输出的耦合结构，底层驱动由4个伺服电机完成。伺服电机的一个缺点是输入命令为电机的相对或绝对位置，需要根据关节误差实时的改变每个电机的最终位置，但由于耦合关节非线性的因素及绳索弹性的影响，并不能确定电机的最终位置，这样传统的PID控制器不能顺利完成，为此提出了一种改进的PD算法。

底层电机控制采用了一种可以在电机运动中改变目标位置的技术，如图4所示，这种技术避免了电机不同运动之间的停顿（图5），使运动速度更加平滑。这种技术是首先给定电机一个虚拟目标位置点，在没有到达这个位置点的过程中，实时监测关节处编码器的反馈值，通过解耦计算实时地修正这个虚拟目标点，采样时间为10ms，这样在一个运动过程中，一般为几百毫秒到几秒，足以把个各种误差消除到理想范围内，之后的实验也表明算法的有效性。

图4 改进的PD算法示意图

图5 传统的控制算法示意图

5 实 验

为了得到较好的控制效果，运用类似于生物驱动机制的分级控制系统作为控制器，分级控制系统框图如图6所示。

图6 控制系统整体框图

本文在样机控制系统的基础上进行实验来验证算法的正确性：

实验一：拟人臂各个关节从初始位置运动到给定的角度，设定值如下：肩关节的欧拉角Z_1、Y_1、X_1 分别为 10°、15°、20°，肘关节角度 M 为 60°，腕关节的欧拉角为 15°、15°、15°，运动过程中实时监测关节角的角度值并绘制成曲线，如图7，图8，图9所示，可见机器人各关节定位精度均达到码盘的分辨率0.18°以内，采用上面闭环控制算法得到的精度是满足系统要求的。

实验二：使机器人在X_Y平面内绘制一条规划好的曲线，曲线跟踪算法采用的是自学习算法，结果如图10所示，可以看出机械臂对于空间规划的曲线具有很高的跟踪能力，位置精度较高[8]。

图7 肩关节角度响应曲线

图8 肘关节角度响应曲线

图9 腕关节角度响应曲线

6 仿真

采用式（22）计算力传感器信号：

图10 圆弧轨迹

其中noise为传感器噪声，为与工具末端与环境z坐标的差值，设定环境刚度为Km=40N/mm，反馈力增益矩阵：积分矩阵取环境作用力Fd设定为Fd=[0 0 10]N，采用阻抗模型进行控制仿真得到图11与图12中的曲线。

从图12中的力响应曲线可以看出，阻抗控制算法能快速地实现力跟踪并且具有较高的跟踪精度，Z方向上的位置在参考位置的基础上产生一定的偏差来满足接触力控制的要求，通过位置和力之间的这种动态关系调节，实现了系统的力/位混合控制。

7 结束语

（1）在一种具有关节角反馈的绳驱动拟人臂样机的基础上，利用关节角反馈信息提出一种适合耦合球关节的闭环控制算法，克服了绳驱动技术多种误差源不能建模的问题，提高了整机的运动精度。

（2）结合多维力传感器提出了一种基于阻抗控制的力控制算法，并在此基础上进行了仿真实验，验证了阻抗模型的正确性。

（3）对机器人的两种不同的任务环境进行分析，并利用多维力传感器对环境信息进行估计与建模，在此基础上对所要实施的任务进行了轨迹规划。

（4）采用基于PC+PMAC分级控制器系统实现控制系统，有效利用了PC资源丰富、开发调试和运动控制器实时运算能力强的优点，同时系统具有很好的开放性和可扩展性。

图11 接触力响应曲线

图12 Z方向位置响应曲线

[1]陈泉柱.绳驱动拟人臂机器人的机构设计与运动学研究[D].北京：北京航空航天大学，2006.

[2]王洪光，赵明扬，郭立新，等.一种柔索驱动并联机构及其运动学分析[J].机器人，2002，24（7）：634-636.

[3]Hogan N.Impedance control，an approach to manipulation[C]∥American Control Conference.1984：304-314.

[4]John J.Craig，贠超.机器人学导论[M].3版.北京：机械工业出版社，2006：136-150.

[5]Fusaomi Nagata， Tetsuo Hase，Zenku Haga，et al.Watanabe CAD/CAM based position/force controller for a mold polishing robot[J].Mechatronics，2007（17）：207-216.

[6]乔兵，吴洪涛，朱建英，等.面向位控机器人的力/位混合控制[J].机器人，1999，21（7）：7.

[7]Yu K G，Kieffer J.Robotic force velocity control for followingunknown contours，ofgranularmaterials[J].Control Engineering Practice，1999（7）：1249-1256.

[8]Burdet E，Rey L，Codourey A.A trivial and efficient learning method for motion and force control[J].Engineering Applications of Artificial Intelligence，2001（14）：487-496.