基于四元数姿态算法的机械手臂系统开发*

闽南理工学院 陶庆凤

虚拟交互新技术正在进行快速的发展，传统使用人机交互的输入设备如手柄、鼠标和键盘等已经满足不了人们生产生活的需求，所以人们急需一种更加符合人体动力学的方式来实现全新的人机交互。手势控制器输入法是一种全新的人机交互方式，具有生动、直观和形象的特点，可以给操作者带来非常自然的人机交互体验，在促进虚拟现实体验的临场感种具有很重要的意义。在上述背景下，本项目设计出一个基于四元数姿态算法的机械手臂系统，来满足当今虚拟现实技术的交互需求。

1 开发背景

科学技术以及社会经济快速发展，促进虚拟现实技术逐渐向各行各业渗透，并因此衍生出了很多新的高科技的研究领域。而在此当中，虚拟交互行业是一个综合性的领域，它代表着时代的需求，并集合了多种新技术、新算法于一体，它的研究方向包含多学科交叉的内容，比如人机交互的技术、计算机图形的技术、人工智能技术、信息技术、传感技术等。

为了促进虚拟交互技术的发展，国家先后出台了多项政策与规划措施，保障行业朝着健康有序的方向发展，所以未来虚拟交互行业一定会迎来蓬勃发展的阶段。

2 基于互补滤波器的四元数姿态解算方法

在系统开发过程中使用的是标准化的四元数，它的三角函数式可以表示为：，通过该表达式可以计算刚体旋转运动的数据，表达式中代表旋转的瞬轴，∂代表刚体旋转的角度[1]。

通常惯性导航过程中，都具有初始状态时导航坐标系m和载体坐标系a重合的现象，且载体坐标系a是动态坐标系，也就是上述四元数的矢量变换中提到的OX'Y'Z'这个新坐标系，而导航坐标系m对应上述四元数的矢量变换中提到的OXYZ这个旧坐标系。假设a坐标系下有定坐标(xa，ya，za)在m坐标系中的投影是(xm，ym，zm)，将其代入到矩阵表达式中就得出姿态矩阵的表达式为：

四元数的微分方程可以用如下方程式表达：

把表达式写成矩阵的形式如下：

求解这个四元数的微分方程式，可以得出四元数p0、p1、p2、p3。

3 传感器的设计

3.1 人体手部骨骼关节分析

人类生产生活中、和大自然的接触中，手是极其重要的媒介，手部动作有着很高的精度和灵敏度。手部结构也相当复杂，手的结构是由17个关节以及27块骨骼所组成的，如图1所示，它相当于一个多关节组成的运动系统。手部结构的自由度等于每根手指的自由度相加，单手一共是二十四个自由度，包括小指、无名指、中指和食指等四指各指头四个自由度，以及拇指上的五个自由度，除此之外还有手腕的曲度、外展、手掌的弧度等3个自由度。

图1 人体手部结构图Fig.1 Structure of the human hand

除拇指外，其余4指的运动状态都是相似的，例如食指，当它做出伸展的动作时，食指的指掌部位的关节可以完成环转、弯曲伸展、侧向摆动等动作。而拇指的结构跟其余四指比就相对复杂，当拇指做出伸展的动作时，它的指掌关节、远指关节可以完成弯曲伸展的动作，手掌腕部的关节可以完成对掌、弯曲伸展以及环转等动作。

3.2 传感器的布局

在机械手臂的开发中，一个非常重要的事情就是传感器的布局，优良的传感器布局方法不但可以确保实现系统的基本功能，还可以进一步精简系统的内容以及提高系统稳定性、安全性[2]。

在人体手部骨骼分析中，知道了人体手部结构图的关节特点、自由度以及运动过程。由此可知食指能够灵活地做出弯曲伸展的活动，依赖于各个关节以及指骨的相互配合，整个动作过程如下：首先根关节也就是指掌关节，它是食指活动的起点，有着指挥官的作用；其次近指关节属于根关节底下的子关节，作用是连接远指和指掌2个关节；最后远指关节也是子关节底下的次子关节，它是最灵敏的一个关节。如图2所示可以看出远指骨依赖于远指关节来活动，近指骨依赖于指掌关节来进行弯曲伸缩活动，而中指骨可以围绕着近指关节展开相关活动。在运动过程中，近指骨和中指骨围绕着指掌和近指关节产生的动作是由人的意识来决定的，是属于主动形式的动作，而远指骨则相反，远指骨不是独立产生的动作，它是附属远指关节的动作，是人的意识无法操控的，也就是在没有外力的时候，它不能自己独立产生动作，一旦远指骨运动必然会引起中指骨也做出相应的动作[3]。

图2 食指活动的各个关节Fig.2 The joints of the index finger

远指骨和中指骨之间动作互相影响、互相带动，这种关系可以用一组数学表达式来体现，表达式如下所示：

通过表达式（5）我们知道，远指骨的活动状态可以依据中指骨的活动状态推算出来，那么对于食指来说，我们可以在近指骨以及中指骨的地方分别布局一个传感器，用来测量近指骨与中指骨活动产生动作的数据，得出相关数据之后，再使用表达式（5）来得出远指骨活动产生的数据，至此整个食指的活动状态的数据都可以完整获取。而拇指数据的获取，一般来说我们只需要采集近指骨与远指骨活动产生的数据，所以在近指骨与远指骨的地方分别布局一个传感器，来获取相关数据即可。腕关节是整个手掌的底部，要获取其数据，只需在手背的地方布局一个传感器即可。

综上可以得出整个手部结构传感器设置的方案：先在手背上布局1个传感器的节点，然后在四指的中指骨与近指骨的地方各布局1个节点传感器，最后是大拇指，在其近指骨处与远指骨处各布局1个节点传感器，该布局设计能够完整地计算出手部的运动数据。

3.3 微处理器设计

传感器的节点是设置在机械手臂相关骨骼上的，是用来计算骨骼运动状态的设备，其构成主要由磁力计、陀螺仪、加速度计以及微处理器组成。要符合安全流畅的运动状态，必须要将传感器的节点大小设计成微型化，所以设计过程采用的是微处理器，它是整个传感器最核心的器件，在工作时通过IIC的总线来读取磁力计、加速度计以及陀螺仪的原始数据，然后经过数据融合的操作得到机械手臂的姿态信息，再把该姿态信息的数据发送给CAN总线，最后实现数据采集以及数据处理与数据传输等三大功能。

4 系统软件设计

4.1 系统开发的部分代码[4]

4.2 系统操作界面

有两个主要的操作模式界面，分别为滚动条控制界面和按钮点击控制界面，可根据用户的实际需求来进行切换。在进入操作界面前需打开手机蓝牙，进入操作界面后点击右边的第二个按钮使得手机蓝牙与手势控制器的蓝牙配对，配对之前蓝牙的初始值为红色停止状态的图标，配对成功后红色蓝牙图标变成了蓝色工作状态中的图标，此时方可进行有效手势控制器的操作，系统开发过程的主要界面如图3、图4所示。

图3 滚动条操作界面Fig.3 Scroll bar operation interface

图4 按钮点击操作界面Fig.4 Button click operation interface

(1)滚动条操作界面

滚动条控制界面：开始工作前五根手指对应的五根滚动条分别处于各滚动条的中间位置，可手动向上或向下调节滚动条的位置。向上是调节对应手指伸展的状态，向下是调节对应手指弯曲的状态，右边第三个按钮为恢复初始状态的按钮，手势控制器使用完毕后可点此按钮恢复初始状态。

(2)按钮点击操作界面

按钮点击控制界面：开启时手势控制器处于初始状态，分别点击大拇指、食指、中指、无名指、小指等按钮，则对应的机械手指开始产生弯曲，点击持续时间越久弯曲的程度越深。松开按钮时，相应的机械手指会逐渐伸展开来，调整完毕后可按右边第三个按钮使其恢复初始状态。

5 结语

本系统开发过程中采用了基于互补滤波器的四元数姿态解算方法，该计算方法能精确地得出机械手臂的运动姿态。同时分析了人体手部结构，包括对小指、无名指、中指、食指和大拇指的各个关节以及指骨进行了细致地分析，对机械手臂上传感器的布局提供了方案。然后对机械手臂的软件开发部分进行了介绍，该系统对机械手臂进行运动控制，控制过程中具有良好的负载能力和控制精度，能自动调节手势控制器的握力大小，该控制器的功能可以进行扩展和挖掘，对后续的二次开发提供了良好的延展性。