一种基于定滑轮的上肢康复机器人：基于四元数的前臂假肢手部自平衡的设计

一种基于定滑轮的上肢康复机器人：基于四元数的前臂假肢手部自平衡的设计0 引言英文引用格式：Fan Binghui，Zhang Kaili，Wang Chuanjiang，et al. Design of hand self-balance for the forearm prosthetic based on quaternion[J].Application of Electronic Technique，2016，42(5)：78-81.文献标识码：ADOI：10.16157/j.issn.0258-7998.2016.05.022中文引用格式：樊炳辉，张凯丽，王传江，等. 基于四元数的前臂假肢手部自平衡的设计[J].电子技术应用，2016，42(5)：78-81.

樊炳辉1，张凯丽1，王传江1，刘圭圭1，张芳丽2

（1.山东科技大学机器人研究中心，山东 青岛266590；2.友邦电力设备有限公司，黑龙江 哈尔滨150000）

摘 要：针对残疾人的前臂假肢在端水等运动过程中需要手部虎口一直与大地保持平行，即手部实现姿态自平衡问题，提出采用四元数的姿态解算算法，并采用卡尔曼滤波融合MPU6050传感器中陀螺仪和加速度计的输出数据，补偿了陀螺仪输出数据的随机漂移误差，得到前臂假肢手部姿态的正确估计。该系统由安卓手机、STM32单片机、MPU6050传感器、高性能数字舵机组成，通过安卓手机语音识别向STM32单片机发送是否保持手部自平衡指令，由STM32实时采集MPU6050中陀螺仪和加速度计的数据并经过算法处理后，通过串口向舵机发送信号。实验结果表明，这种姿态估计算法有效地解决了前臂假肢在运动过程中手部自平衡问题，为前肢缺失的残疾人提供了便利。

中图分类号：TP242.6

文献标识码：A

DOI：10.16157/j.issn.0258-7998.2016.05.022

中文引用格式：樊炳辉，张凯丽，王传江，等. 基于四元数的前臂假肢手部自平衡的设计[J].电子技术应用，2016，42(5)：78-81.

英文引用格式：Fan Binghui，Zhang Kaili，Wang Chuanjiang，et al. Design of hand self-balance for the forearm prosthetic based on quaternion[J].Application of Electronic Technique，2016，42(5)：78-81.

0 引言

世界上每年因战争、自然灾害、交通事故等原因引起的残疾人数多达千万人以上，其中大部分为肢体残疾，而目前市场上的大部分假肢都是作为装饰性用品，真正实现功能代偿的假肢数量较少。为满足残疾人日常生活的需求，本系统对前臂假肢做了姿态规划，即控制前臂旋转和手腕俯仰两个电机，使得假肢末端手爪的虎口平面与大地水平面始终保持平行，如图1所示。这样假肢在抓取和移动目标物体的过程中，可以保证运动平稳，对移动水杯等类似操作尤其重要。在前臂假肢的平衡控制过程中，姿态传感器将检测到的前臂假肢的姿态信号送入STM32，经算法运算得到控制量驱动舵机来控制前臂的扭转和手部的俯仰，从而使前臂假肢的手部恢复到平衡状态。可以看出，获取实时的前臂假肢的姿态是保持手部平衡的前提。因此，实现一种实时可靠的前臂假肢姿态估计算法尤为重要，它直接影响其手部自平衡性能。

姿态解算的算法有多种，常用的算法有欧拉角法、方向余弦法与四元数法，由于欧拉角法求解时，方程中存在奇点，方向余弦法计算量大，因此现在常用的方法为四元数法，其计算量小，无奇点^[1-2]。姿态估计算法必须考虑到传感器容易受温度及噪声的影响而产生不同程度的漂移，它会影响姿态估计的准确性。针对这一问题，本文采用卡尔曼滤波的方法来融合陀螺仪和加速度计输出的数据，修正了陀螺仪输出量中的随机漂移分量^[3]。

1 系统硬件设计

随着安卓智能手机的发展与普及，为了让前臂假肢的控制更加方便和标准化，提出了以安卓手机离线识别语音及应用蓝牙通信技术实现安卓手机无线控制假肢运动的方案。本文介绍的前臂假肢可实现手部平衡，除此之外，在不需要手部保持平衡的环境下，安卓手机通过语音识别可控制前臂假肢关闭手部自平衡功能和实现手部上抬、下落、内转、外转和张合等动作。该系统由安卓手机、蓝牙模块、MPU6050、STM32微控制器、MX-28高性能舵机组成，系统的控制流程如图2所示。

其中，选用MPU6050做为前臂假肢的姿态检测系统，MPU6050在单个产品封装内集成了3轴陀螺仪和3轴加速度计。由于前臂假肢不需获知偏航角，所以本文采用了MPU6050来获取前臂假肢的俯仰角和横滚角。整个控制过程由MPU6050测量姿态变化，并通过前臂扭转以及手腕俯仰来根据姿态变化实时进行调整，使其达到自平衡状态。

MPU6050安装在假肢的腕部，其安装位置如图3所示。将MPU6050安装在控制前臂旋转关节的上部并固定在连杆上，左右位置可以变化，但要确保其所在的平面与手爪虎口所在平面平行。安装完成后，前臂绕MPU6050的X轴旋转，手腕绕MPU6050的Y轴俯仰。

2 四元数姿态解算算法

四元数的数学概念是1843年由哈密顿首先提出的。它是由一个实部单位和3个虚部单位i、j、k组成，其形式为：q=q₀ q₁i q₂j q₃k。四元数可以表示旋转，其标量部分表示旋转角度的大小，矢量部分表示转轴的方向^[4-5]，应用四元数时必须进行归一化，也就是令其模值为1。

前臂假肢的定位由固定的内部的三维坐标确定，一个坐标系到另一个坐标系的变换，可以通过绕不同坐标轴的3次连续转动来实现。顺时针绕横滚轴，即绕载体坐标系的X轴旋转，得到横滚角φ；顺时针绕俯仰轴，即绕载体坐标系的Y轴旋转，得到俯仰角θ；顺时针绕偏航轴，即绕载体坐标系的?爪轴旋转，得到偏航角γ^[6]。

四个参数的初始值是由欧拉角的初始值经过以下计算得到的^[6-9]：

利用四元数微分方程可以实现四元数的更新，其中，ω是陀螺仪测量的角速度。

应用四阶龙格—库塔算法解微分方程，得到4个参数的更新值。具体过程如下^[12-14]：

设：

则：

其中，T为更新周期，q(t)为初始四元数，q(t T)为更新后的四元数，K₁是时间段T开始时的斜率，K₂是时间段中点的斜率，K₃也是中点的斜率，K₄是时间段终点的斜率。

计算得到这4个参数之后，可由姿态矩阵T更新欧拉角[15]：

得到姿态角，即可得到前臂假肢的姿态估计。

前臂假肢在平衡的情况下，欧拉角的理想输出值应该是0°，但由于陀螺仪有随机漂移误差，欧拉角会一直积累^[16]。实验结果图4所示。

针对这一问题，提出了应用卡尔曼滤波的数据融合算法来解决。卡尔曼滤波可以将陀螺仪的输出数据和加速度计的输出数据融合起来。陀螺仪用于测量角速度，动态跟踪性能好，但陀螺仪受温度、不稳定力矩等因素的影响，会产生随机漂移误差；加速度计可用于测量加速度，通过测得的加速度值与重力加速度的比值得到倾角值，加速度计静态性能好，但是动态跟踪性能差。可以看出，陀螺仪和加速度计优缺点互补，结合起来会有好的效果^[17-18]。

3 实验结果分析

3.1 四元数法和卡尔曼滤波实验结果分析

将MPU6050静止平放在桌上，采集其经四元数姿态解算和卡尔曼滤波后输出的数据，通过MATLAB工具对实测数据进行仿真分析，如图5所示，俯仰角和横滚角都在附近，对比图4发现卡尔曼滤波有效解决了陀螺仪的随机漂移误差，可以准确地获取静态时前臂假肢的姿态。

将MPU6050安装到前臂假肢上，令前臂假肢绕MPU6050的X轴从平衡位置顺时针逐步旋转到-40°，一段时间后，回到平衡位置，再沿X轴逆时针逐步旋转到40°，采集这一过程MPU6050经四元数姿态解算和卡尔曼滤波后输出的数据，通过MATLAB工具对实测数据进行仿真分析，横滚角发生相应的变化，俯仰角基本保持不变，如图6(a)所示。令前臂假肢绕MPU6050的Y轴重复以上实验，根据图6(b)可以看出，俯仰角发生相应的变化，横滚角基本保持不变。由于实验过程中手动旋转前臂假肢，操作者偶有抖动和难做到匀速转动，因此，图6中的波形中偶有小的波动。实验表明，本文采用的姿态估计可以实现前臂假肢动作时姿态的正确估计，经过STM32运算处理后，实现前臂假肢在运动过程中保持手部平衡。

3.2 前臂假肢手部自平衡实验结果分析

对前臂假肢系统组装并调试，通过安卓手机软件向假肢发出保持手部平衡的命令。具体测试步骤:连接好线路，打开电源，观察指示灯正常亮起，触击控制界面的蓝牙连接按钮，观察指示灯，以确保手机蓝牙和蓝牙模块的配对连接成功；否则，检查系统的连接是否正确。蓝牙配对成功后，向安卓手机喊出“平衡”指令，手机通过蓝牙将指令传到单片机，从而控制前臂假肢运动。实验结果如图7、图8所示。

图7(a)～图7(e)展示了前臂假肢从下往上转动过程中，其手部可以实现自平衡，手部虎口一直保持与大地平行。

图8中以箭头指向的地点为参考点，可以看出图8(a)～图8(c)为前臂假肢由内往外转动过程，同样假肢手部能实现自平衡。

4 结论

此前臂假肢采用四元数法的姿态解算算法，并利用卡尔曼滤波融合MPU6050中陀螺仪和加速度计的数据，得到其正确的姿态估计。经实验测试，该系统运行稳定、可靠，所设计的姿态估计算法能够很好地跟踪前臂假肢的真实姿态，实现了假肢手部的姿态自平衡控制。此前臂假肢可以辅助残疾人完成喝水等动作，方便了他们的生活，也减轻了家属的负担。

参考文献

[1] 杜海龙，张荣辉，刘平，等.捷联惯导系统姿态解算模块的实现[J].光学精密工程，2008，16(10)：1956-1962.

[2] 张荣辉，贾宏光，陈涛，等.基于四元数法的捷联式惯性导航系统的姿态解算[J].光学精密工程，2008，16(10)：1963-1970.

[3] 杨凌霄，李晓阳.基于卡尔曼滤波的两轮自平衡车姿态解算方法[J].计算机仿真，2014，31(6)：406-409.

[4] 叶锃锋，冯恩信.基于四元数和卡尔曼滤波的两轮车姿态稳定方法[J].传感技术学报，2012，25(4)：524-528.

[5] 吕印新，肖前贵，胡寿松.基于四元数互补滤波的无人机姿态解算[J].燕山大学学报，2014 38（2）：175-180.

[6] 张思奇，赵忆文.便携式移动机器人姿态解算方法[J].微计算机信息，2007，23(10-2)：188-190.

[7] 祝乔，汪旭东，崔家瑞.基于四元数的航天器姿态非线性控制器[J].信息与控制，2012，41(2)：170-179.

[8] 贾秋玲，闫建国，王建民.空间多刚体系统姿态的协同控制[J].信息与控制，2005，34(4)：385-388.

[9] 赵国文.机器人运动学中的四元数矩阵和基变换矩阵[J].机器人，1993，15（2）：59-64.

[10] 张天光，王秀萍，王丽霞，等，译.捷联惯性导航技术(第二版)[M].北京：国防工业出版社，2007：29-33.

[11] 秦永元.惯性导航[M].北京：科学出版社，2006：287-305.

[12] 崔璐璐.基于MEMS器件的姿态测量系统研究与实现[D].大连：大连理工大学，2009.

[13] 王励扬，霍昆明，何文涛，等.四阶龙格库塔算法在捷联惯性导航中的应用[J].计算机仿真，2014，31(11)：56-59.

[14] 张慧春，吴简彤，何昆鹏，等.四阶龙格-库塔法在捷联惯导系统姿态解算中的应用[J].应用科技，2005，32(6)：37-39.

[15] 邓正隆.惯性技术[M].哈尔滨：哈尔滨工业大学出版社，2006：14-164.

[16] 金光明，张国良，陈林鹏.MEMS陀螺仪静态漂移模型与滤波方法研究[J].传感器与微系统，2007，26(11)：48-50.

[17] 冯智勇，曾瀚，张力，等.基于陀螺仪及加速度计信号融合的姿态角度测量[J].西南师范大学学报(自然科学版)，2011，36(4)：137-141.

[18] 刘星.多维MEMS惯性传感器的姿态解算算法研究[D].哈尔滨：哈尔滨工程大学，2013.