电力检修机械臂动作学习与控制研究

兰建军

(福建水利电力职业技术学院电力工程学院,福建 永安 366000)

0 引 言

随着机器人技术的迅速发展,电力检修机械臂已逐步应用于输电线路检修作业中,在提高输电线路检修工作效率的同时,降低了专业人员培养成本,提高了作业安全性[1-2]。但机械臂的运动轨迹需要根据作业任务的不同进行调整[3],因此,电力检修机械臂动作的快速规划成为一种新的发展需求[4]。基于人体手臂动作捕捉及学习的控制方案成为一种新的机械臂路径规划方式[5,6]。黄启友利用惯性数据手套与红外检测杆进行手势识别,识别率可达99%[7],李立早等介绍了一种基于加速度计和弯曲度传感器的人体手臂运动角度测量系统[8]。文献[9]提出一种基于近邻法的进行惯性融合的聋哑手语识别方案,准确率可达96.5%。后续的许多基于惯性方式的动作捕捉研究中,通常采用3Dsuit、Moven等集成惯性捕捉系统实现[10-13],也有利用智能可穿戴人机交互设备实现跌倒检测[14]。上述研究取得了较好成果,但实际应用中存在弊端:部分系统只采用6轴惯性传感器进行测量,精度较低;多数采用类似贝叶斯等较为复杂的算法进行多传感器数据融合,在嵌入式应用时姿态测量实时性差;另外上述研究主要依赖成套集成设备,价格高、失去惯性测量低成本的意义,且不提供底层操作接口。

为实现多自由度电力检修机械臂动作学习和控制的低成本嵌入式应用,方案采用惯性传感器实现人体手臂各关节运动姿态的测量和解算,依据李群理论采用刚体变换方法求解机械臂各舵机控制参数,并在Labview中对6自由度机械臂动作学习和控制方案进行了动态仿真验证。

1 系统方案

系统在结构上主要包含手臂姿态检测系统和机械臂舵机控制系统两部分,图1所示为系统结构框图。其中,手臂姿态检测系统由控制器、惯性传感器和WIFI通信模块组成。两个惯性传感器MPU9250布置于人体的大臂和小臂,用于人体手臂姿态检测,传感器数据经控制器进行解算后,形成各舵机的转动目标角度,并通过WIFI模块发送于机械臂舵机控制系统。机械臂舵机控制系统由控制器、数字舵机、机械臂和WIFI模块组成。WIFI模块接收各舵机目标角度参数后,由控制器依据目标角度参数完成舵机控制,从而实现机械臂对人体手臂姿态的识别和学习。

图1 系统结构框图Fig.1 Block diagram of system structure

2 手臂姿态测量

2.1 坐标系构建

人体手臂姿态测量其本质是在人体手臂上构建一套捷联式惯导系统[15,16],从而需要获取手臂的姿态、位置以及速度信息。通常姿态信息采用载体坐标系描述,位置信息采用导航坐标系(采用地理坐标系)来描述。本次选用东北天ENU坐标系为导航坐标系(n系),载体坐标系为b系,最初n系和b系重合,将b系绕其Z轴旋转角度φ,再绕b系新X轴旋转角度θ,最后再绕b系新Y轴旋转角度γ,从而获取一个新的姿态,旋转关系示意图如图2所示。依据刚体三维旋转群理论,由于旋转是相对动坐标系进行的,因此遵循矩阵右乘原则,两坐标系上的点可通过旋转矩阵进行转换,转换关系如公式(1)所示。

图2 两坐标系旋转关系图Fig.2 Rotation diagram of two coordinate systems

(1)

2.2 姿态解算

四元数法以其计算量小、精度高和可避免奇异性等优点被广泛用于姿态解算[17]。四元数法姿态解算主要包含四元数初始化、四元数更新、四元数转换输出三个部分。通过微分方程的求解来实现四元数更新和转换输出,从而不断获取新的姿态,其具体步骤如下:

Step1 四元数初始化

公式(2)所示为四元数初值计算公式,初始化时需要用到人体手臂的初始姿态数据。因为无法确保每次初始化时人体手臂的姿态都处于同一位置,所以常用的方法是测量一个静态姿态(φ,θ,γ)作为四元数初值计算时的初始姿态数据。

静态姿态解算时,手臂处于静止状态,因为惯性传感器水平放置,加速度计只受重力影响,无法利用加速度计测量偏航角φ。偏航角只能利用磁力计测量,其测量公式如公式(3)所示。

(2)

(3)

公式中:mx,my分别为磁力计在X轴和Y轴上的磁场强度分量输出。

(4)

Step2 四元数更新

获取四元数初始值后,后续需要不断的更新四元数来连续解算姿态,四元数更新过程的核心就是根据姿态变换矩阵和陀螺仪测量值不断迭代的过程,求解姿态变换矩阵和载体坐标系中角速度关系的微分方程,可将四元数的求解转换成公式(5)所示的矩阵表达形式。

(5)

公式中:ωx,ωy,ωz分别为陀螺仪测得的三个方向角速度分量。

Step3 四元数转换输出

采用四元数进行姿态解算时有很多优点,但是描述不够直观,特别是在后续的机械臂舵机控制参数计算时带来较多不便。因此,需要将四元数形式的姿态转换成以欧拉角的形式,其转换公式如公式(6)所示。

(6)

尽管人体手臂实时姿态可以通过单一陀螺仪数据测量完成。但是由于积分特性,陀螺仪的测量误差会随着时间不断积累,所以单纯的采用陀螺仪将无法准确测量手臂姿态。

2.3 卡尔曼滤波

为了准确测量人体手臂运动姿态,通常需要融合陀螺仪、加速度计、磁力计的数据完成。卡尔曼滤波算法作为一种较为常用的数据融合算法,其基本原理是利用线性系统状态方程,通过观测系统的输入和输出,对系统状态进行最优估计[1]。首先构建如公式(7)所示的线性随机微分方程和系统观测方程。

xk=Axk-1+Buk-1+wk-1
zk=Hxk+vk

(7)

公式中:xk为k时刻的状态值;A为转移矩阵;B为控制输入矩阵;uk-1为k-1时刻的输入;wk-1为过程噪声;zk为k时刻观测值;H为观测矩阵;vk为测量噪声。

卡尔曼最优值估计的本质是一个递归滤波过程[19],包括系统预测和测量修正两个环节。系统预测环节中,由于系统初始时并没有当前状态的观测信息,所以需要用k-1时刻的后验估计状态值计算k时刻的先验状态估计值,并计算先验估计协方差。

修正环节中主要根据k时刻的先验状态估计值及先验状态协方差计算当前的后验状态估计值(最优估计值),完成迭代更新。通过动态调整卡尔曼增益从而求解有着最小均方差的最优估计值,其软件流程图如图3所示。

图3 软件流程图Fig.3 Software flow

3 机械臂控制

3.1 姿态映射

人体手臂关节是运动的支点和枢纽,运动机理较为复杂。因此,机械臂对人体手臂动作学习的关键在于姿态映射,其核心问题就是将人体手臂姿态数据转换成机械臂上各舵机的目标角度。设计的机械臂结构图如图4所示,其中1号、2号舵机模拟人体大臂的两个自由度,可完成俯仰和偏航运动;3号、4号、5号舵机模拟人体小臂的三个自由度,可完成俯仰、横滚和偏航运动;6号舵机简单模拟人体手部的抓取动作。

图4 仿人机械臂结构图Fig.4 Structure diagram of humanoid robot arm

3.2 姿态分解

图5 偏航角X-Y平面投影图Fig.5 X-Y plane projection of yaw Angle

4 系统建模与测试

4.1 机械臂建模

机械臂建模分别在Soildworks和Labview软件中完成[20]。Soildworks中主要完成舵机及其机械连接件的3D建模,包括零件草图构建、二维图形特征添加、零件约束关系构建和零件装配4个步骤,最后生成3D模型的WRL文件,设计的机械臂组件3D模型图如图6所示。

图6 机械臂3D模型图Fig.6 3D model of mechanical arm

组件编号分别对应于图4中的舵机编号。在Labview软件中导入Soildworks中生成的WRL文件,完成机械臂组件3D模型的导入,为了保证机械臂中各自由度舵机的独立旋转,需要确定好械臂各组件相对于原点的坐标信息。导入各组件时,需要设置各组件之间的父子关系,并要求严格根据组件编号按照顺序导入。

4.2 静态数据测试

为了验证人体手臂测量方案的可行性,以人体手臂小臂测量为例进行了静态测试效果验证。由于手臂无法精确保持在特定角度,测试时利用机械臂替代人体手臂,在机械臂3号组件中心位置上固定一个MPU9520,由单片机控制3-5号舵机分别旋转,旋转范围均为10°～60°(步进角5°),记录解算出的姿态角度,绘制的误差曲线如图7所示,各测点误差均在±1.5°内,满足测量精度要求。

图7 静态测试误差曲线Fig.7 Static test error curve

4.3 机械臂动态仿真

虽然Labview支持3D对象进行相对旋转、平移和缩放功能,但旋转函数仅支持“旋转对象”绕坐标系原点进行旋转,这样无法满足各自由度舵机的独立旋转。为解决上述问题,采用插入连接点的方式通过“创建对象”函数创建体积相对机械臂可以忽略的小球,并将小球移动到各组件的连接支点处。利用父子关系的约束,执行旋转函数时,会带动下一级机械臂绕支点旋转,从而同时实现独立旋转和公共旋转,其动态旋转效果图如图8所示。

图8 机械臂动态运动效果图Fig.8 Mechanical arm dynamic motion effect

5 结 论

针对电力检修机械臂快速动作规划需求,利用惯性测量方式实现人体手臂运动参数检测,经姿态解算获取手臂各关节运动姿态,分解姿态后求解出对应机械臂各舵机运动控制参数后,实现电力检修机械臂对人体手臂动作的学习。为提高测量精度,采用卡尔曼滤波算法对多传感器数据进行融合,最后在Labview中完成了机械臂的建模和动态测试。实验数据表明,在考虑噪声及传感器误差的情况下,测量误差在±1.5°内,该方案精度高、成本低,可在机械臂动作学习和控制应用场合推广应用。