同步控制机械手臂的研究*

（沈阳航空航天大学计算机学院 沈阳 110136）

1 引言

2016年美国19所大学在美国科学基金会的赞助下联合发布了《美国机器人技术路线图：从互联网到机器人》，该报告预言机器人是将会对人类未来产生革命性影响的新技术，在不久的将来机器人将会应用于人类社会的各个角落。现阶段机器人是力学、材料学、自动控制、传感器、机械等传统学科交叉的结晶，未来机器人将融合传感技术、智能控制、人机交互、计算机视觉、仿生机械材料等新研究领域的研究成果，机器人将从传统工业行业走向医疗、家庭、个人等更广阔的应用领域。

根据机器人领域代表期刊《International Journal of Robotics Research》上发表的部分研究方向的统计数据，智能控制、人机交互与传感技术是目前成果最多的领域，现阶段是传统机器人向未来智能机器人演进的过度阶段，通过初步应用智能控制、传感器融合、人机交互等新技术使得机器人相比于传统机器人更加易用、安全和人性化，未来随着各领域研究的深入和应用需求的提升，机器人在人工智能与认知融合等领域将会有更多的进展。

目前尽管机器人已经广泛应用于各大门类的工业领域，但主要还是从事于结构化环境中执行各类确定性任务，操作不灵活、感知不强与应用场景单一等是现阶段机器人亟待解决的问题。随着应用要求的提高和相关领域的成熟，协作机器人出现了。协作机器人是初步融合人工智能领域研究成果的产物，协作机器人具备一定的外界感知能力和人性化的操作方式。

本文探究利用惯性测量单元（IMU）对机器人进行体感操作的全新人机交互方式，让机器人实现与人的非接触同步运动。根据设计目标，这套系统分为三个模块：穿戴式人体运动数据采集模块，驱动机械手臂模块，机械臂运动反馈模块。穿戴式人体运动数据采集模块负责实时测量人体手臂的运动姿态，并根据机械臂运动反馈模块的数据实时反馈给人体；驱动机械手臂模块负责根据人体运动数据驱动机械手做相应动作；机械臂运动反馈模块负责测量机械手臂的运动状态并实时反馈给人体穿戴式设备。

2 穿戴式人体运动数据采集模块

本系统可以简化为主从遥操作机械手臂系统，遥操作技术利用机械手臂代替操作者完成各种操作任务。遥操作机械手臂系统的关键是将本地操作者的主观行为运动通过遥操作系统投射到机械手臂，因此需要一种设备能实时采集操控者的运动数据。

目前采集人体手臂运动数据的研究方向大致分为两类：基于光学传感器的计算机视觉技术设备和基于惯性传感器的穿戴式设备。这两种方式各有优劣，但相比于光学传感器的严格应用环境，基于惯性传感器的方式更加便捷，且应用更广泛。

2.1 惯性传感器

得益于微电子技术的进步，MEMS器件的集成度越来越高，原来复杂的惯性传感器也变得越来越小，于是MEMS惯性传感器由于其廉价、小巧有了更广泛和灵活的应用。

穿戴式人体运动数据采集设备的硬件部分主要是利用MEMS惯性传感器来测量人体姿态，进而求解出人体运动数据。系统采用MPU9250作为惯性测量单元，MPU9250包括三轴加速度计、三轴陀螺仪和三轴磁力计，如图1所示。

图1 MPU9250惯性传感器

2.2 基于MPU9250的姿态解算

MPU9250中加速度计和磁力计具有高频噪声，所以加速度计和磁力计输出信号瞬时值不够精确，直接解算出来的姿态会震荡，但是加速度计和磁力计的长期数据是稳定的。另一方面陀螺仪具有低频噪声，即每一时刻得到的角速度是比较准确的，但是由于使用积分计算旋转角度会有积分累计误差，因此陀螺仪长期数据是不准确的，也就是陀螺仪漂移现象。根据加速度计、磁力计和陀螺仪在频域上的互补特性，可以融合这三种传感器数据，以提高精度和系统的动态特性。

对MPU9250进行姿态融合和滤波，需要三个步骤：1）校正；2）坐标转换；3）滤波。如图2所示。

图2 MPU9250姿态融合与滤波流程

2.3 姿态解算预处理

在MPU9250静止时，理论上加速度计/磁力计的XYZ轴测量的数值投射到三维坐标系中是球心在原点一个椭球，但由于传感器做工误差等导致球心偏离原点，所以需要先进行椭球拟合求出球心偏移量。椭球拟合问题就是根据坐标点求解椭球的球心，简化成下列矩阵形式：

图3 MPU9250椭球拟合

磁力计的基坐标系和加速度计的基坐标系是不同的，因此需要将磁力计坐标系转换到加速度计坐标系。假设加速度计测量到的重力加速度为ag，磁力计测量到的磁通量向量是Φb，先将磁力计基坐标旋转到加速度计的基坐标，即Φa=AΦb，A为旋转和将椭球转换成正球的矩阵。由于在理论上磁力计向量Φa与重力加速度向量ag垂直，故对向量进行归一化后可得：

再次利用最小二乘法求解出A矩阵，之后就可以通过矩阵A将磁力计测量的磁通量向量转换到加速度坐标系中。

2.4 姿态解算与数据融合

对加速度计和磁力计做完预处理后，在实际测量时还需要使用各种滤波算法做姿态融合。目前基于MEMS惯性传感器的姿态解算主要有三种方法：欧拉角、方向余弦和四元数。欧拉角在求解姿态时存在万向节锁问题，不能用于全姿态解算；方向余弦可用于全姿态解算但是含有三角函数计算量大，故也不能满足实时性要求；四元数，计算量小，无万向节锁问题，实时性好，所以选择基于四元数的姿态解算。

通过罗德里格旋转可以推导出四元数的旋转公式，根据四元数旋转公式可以推出载体坐标系b转换到地理坐标系R的坐标转换矩阵：

求解出四元数后，可以通过四元数的值反解出载体坐标系的旋转角，公式如下：

载体姿态的改变即为四元数的改变，因此对于载体的姿态解算需要实时更新四元数。通过构建四元数关于时间的微分方程来表示陀螺仪三个轴的角速度与四元数之间的关联，最后化简可得四元数的更新方程：

通过上述方程就可以解算出载体相对于地理坐标系旋转的角度，但实际应用时直接解算MPU9250载体会出现误差，且误差随着时间积累，所以需要通过融合加速度计和磁力计对陀螺仪进行校正和补偿。根据上文中加速度计、磁力计和陀螺仪的不同特性，互补滤波器对加速度计、磁力计使用低频滤波，对陀螺仪使用高频滤波，加速度计低通滤波如下：

由于地理坐标中重力加速度向量是恒定不变的，所以利用理论重力加速度向量和实际重力加速度向量的外积来计算向量方向差值θ。另利用磁力计测量出的磁通量向量和理论磁通量向量组成

对偏航角进行校正的向量组。构建PI控制器来控制补偿值的大小，故Mahony滤波主要过程如下：

其中error≈θ，表示估计的四元数，δ为陀螺仪角速度补偿值，Kp控制加速度计或磁力计和陀螺仪之间的交叉频率，Ki用于校正陀螺仪误差。

Mahony滤波分为预测和校正两步，第一步通过三轴陀螺仪测得的角速度带入式（5）计算出四元数姿态预测；第二步通过加速度计或磁力计利用式（6）和式（7）校正四元数。最后通过获得的新四元数带入式（4）求解出欧拉角，即可求解出载体相对于地理坐标系得偏转角度。

2.5 穿戴设备设计

参考人体手臂运动模型和刚体运动理论，可将人体手臂简化成为五自由度的连杆模型，如图4。

图4 人体手臂连杆模型

其中Oi和zi分别为关节i的原点和旋转轴，肩关节有三个关节(i=1，2，3)，肘关节有两个关节(i=4，5)，l1和l2分别为上臂和前臂的长度，d1为肩关节的肩宽，x0y0z0为人体坐标系，人体手臂D-H参数表见表1。

表1 人体手臂D-H参数

其中θi，αi，ai，di分别表示关节i的关节角、扭转角、连杆长度和连杆偏移量。

根据人体手臂运动模型和测量要求，需要两个MPU9250传感器分别安置在大臂和前臂上，安装示意如图5。

图5 MEMS惯性传感器安装示意图

根据安装位置，假设人体是沿着地理坐标系的轴站立，则只用上臂MEMS惯性传感器测量的数据就可以直接解算出肩关节相对于地理坐标系的旋转角度。前臂MEMS惯性传感器测量的是小臂相对于地理坐标系的姿态数据，由于前臂和上臂是连杆结构，所以前臂MEMS惯性传感器的姿态数据还需要利用上臂的姿态数据转换成肘关节的角度数据。肘关节的弯曲角度是上臂载体坐标系的x轴与前臂载体坐标系的x轴的夹角，即绕z轴的Yaw偏航角之差，而前臂旋转角度等于前臂解算的Pitch欧拉角减去上臂的Pitch欧拉角。肘关节旋转角度解算如式（8）。

其中θ、α分别为肘关节弯曲角度和前臂旋转角度，分别表示两个惯性传感器解算出的Pitch欧拉角，分别为两个惯性传感器解算出的Yaw欧拉角。

3 驱动机械手臂模块

利用穿戴式人体运动数据采集模块实时采集人手臂运动数据，然后解算出肩关节和肘关节旋转角度，将关节的旋转角度作为输入信号，驱动机械手臂上相应关节的伺服电机。在ROS中建立机械手臂仿真运动模型，各关节对应位置与手臂模型如图6所示。

图6 机械手臂模拟仿真

在肩关节出有三个伺服电机，肘关节有两个伺服电机，每个伺服电机分别对应图3中的关节点。驱动机械手臂模块可以分为两个部分：主控制器和关节控制器。主控制器即穿戴式人体运动时数据采集设备，负责测量人体运动姿态和解算出关节旋转角度；关节控制器即控制机械臂运动的伺服系统，根据主控制器传输过来的各关节旋转角度数据驱动相应电机旋转。

主控制器周期性向关节控制器发送各关节旋转指令，关节控制器周期性读控制指令，然后控制相应伺服电机运作。系统设计要求人体手臂与机械手臂同步运动，所以主控制器和关节控制器之间的通信报文需要统一指令格式，控制过程如图7。

图7 驱动机械手臂流程

控制报文中序列号为主控制器周期性发送控制报文所加上的序列号，关节控制器根据序列号判断机械手臂运动轨迹，关节控制器收到报文后会先检测序列号的连续性，若是在一定时间ΔT内收到的报文序列无法组成连续的一段控制报文，则表示通信出现故障，若是报文序列正常则开始解析报文命令和关节位置信息，并将关节信息转换成伺服电机控制信号。

4 机械臂运动反馈模块

主控人在遥控机械臂运动时，机械手臂也需要实时反馈机械手臂的运动状态，目前一般使用机械手臂现场监控来反馈机械手臂的运动信息，这种布置方便简单，但是对于主控人缺乏主观感知，所以在此基础上再设计一套外骨骼穿戴设备负责反馈给主控人机械手臂的状态。

机械手臂上安装力矩传感器和编码器，编码器提供关节的旋转角度θi(°)，扭力传感器提供关节电机的力矩τi(N·m)。反馈模块收集机械手臂各关节的旋转角度和力矩，然后将机械臂的力矩同比例换算成外骨骼电机的力矩，最后驱动外骨骼电机做相应运动将机械臂运动的角度和力矩反馈给主控人。

上肢外骨骼设计，如图8所示。

图8 上肢外骨骼设计图

5 结语

研究初衷是探索一种新的控制机械手臂的方式，利用多传感器的数据融合和仿生机械设计，让人更容易更便捷地操控机械手臂。整套系统可以简化成主从控制系统，主控制是通过惯性传感器测量人体的运动轨迹；然后解算出控制信号，从系统负责接受主系统的控制信号然后驱动机械手臂做相应运动；反馈模块根据机械臂反馈的运行状态数据，通过机械外骨骼直接反馈给主控人。目前系统初步实现预定功能，下一步将继续优化惯性传感器的姿态解算算法和主控制与关节控制系统之间的通信协议，以提高整体系统的精度和稳定性。同时也将完善机械和传感器设计，增强主控人对整套系统的感知，提升主控人操作使用的舒适度和精准度。