CN108020223A - 一种基于惯性测量装置的力反馈设备手柄的姿态测量方法 - Google Patents

Abstract

本发明提供了一种基于惯性测量装置的力反馈设备手柄的姿态测量方法，首先由于惯性测量装置的惯性特质，使得仅通过单个测量装置就能获得手柄在空间中的任意姿态；其次通过使用姿态校准算法和姿态插值算法，使得测得的姿态能够满足力觉渲染对姿态信息高频率、高精度的需求；同时在带有力反馈的虚拟现实模拟环境下，验证了该方法能够满足力觉交互的需求。本发明使用惯性测量装置测量姿态，仅需要单个装置就能测得末端三自由度的姿态，能够满足力反馈设备手柄机构简化的需求；而且惯性测量装置直接和力反馈设备手柄相连，不需要通过机械结构和转动关节相连，测得的方位角仅与手柄的空间姿态有关，与力反馈设备移动平台的放置方式无关，测量更加精确。

Description

一种基于惯性测量装置的力反馈设备手柄的姿态测量方法

技术领域

本发明涉及一种力反馈设备手柄姿态的测量方法，尤其是涉及一种采用惯性测量装置测量力反馈设备手柄姿态的方法。

背景技术

触觉是人类的基本感觉之一，通过触觉，我们能够感知物体的软硬、轻重、冷热、干湿、凹凸等外界环境信息。通过触觉与视觉、听觉等其它感官共同配合，人对外部世界的认知效率会明显提升。目前，人类已经实现利用视觉和听觉信息与计算机进行交互，进入多媒体和虚拟现实的世界。不过视觉信息与听觉信息难以表达操作对象的深浅、软硬及其运动状态等，若通过触觉交互设备将触觉信息反馈给操作者，可以增加操作者得到的信息量，从而增强人机交互的逼真度和真实感。

随着触觉交互技术与计算机技术的进步，触觉信息正被逐渐引入虚拟现实系统，使操作者在对虚拟环境中的虚拟物体进行操作时，感受到虚拟物体之间作用力及运动，体验到如同操作真实物体的触觉感知效果。

在力觉渲染中，需要通过姿态测量装置获得力反馈设备手柄的姿态信息，用于虚拟工具的图形绘制、碰撞检测和碰撞响应，实现频率大于30Hz的图形反馈和大于1000Hz的力反馈。为满足力觉渲染高频率、高精度的需求，力觉渲染算法对力反馈设备测量装置测量姿态信息的精度(<0.1°)和刷新频率(>1000Hz)有着苛刻的要求。

目前的力反馈设备主要采用机械装置测量姿态信息。机械测量装置在工作时，通过机械臂和编码器/电位计的连接架构测量姿态信息。因为六自由度力反馈设备的末端手柄具有三个关节，因此需要将三个机械测量装置连接于各个转动关节，才能测出完整的姿态信息。机械测量装置具有性能可靠、延时短，无潜在干扰源等优点，但采用机械测量装置的手柄系统比较笨重。实践证明，机械跟踪装置复杂的机械连接是制约末端手柄结构简化的瓶颈所在，增大了力反馈设备手柄结构设计的难度。此外，机械测量装置的活动范围受到机械连接的限制，工作空间有限。

与机械测量装置相比，惯性跟踪器具有设备轻便、价格低、不怕遮挡、有无限大的工作空间、低延迟等显著优点，适用于姿态的测量。尽管惯性跟踪是虚拟现实领域的研究热点之一，但在力反馈设备的研发中的应用却缺乏相应的探究。在测量物体的空间姿态时，惯性测量装置相较于传统的机械跟踪装置最突出的优点在于连接简单，但是如何获得力反馈设备转动部分三自由度的姿态信息还亟待研究。

在此背景下，本发明提供了方法研究的重点在于如何从惯性测量装置和力反馈设备手柄的连接、姿态信息的获取和处理、力反馈算法的设计等方面出发，实现利用惯性测量装置测量力反馈设备手柄的姿态。

发明内容

本发明解决的技术问题是：为了解决了力反馈设备手柄三自由度姿态测量的难题，本发明提供了一种基于惯性测量装置的力反馈设备手柄的姿态测量方法。首先由于惯性测量装置的惯性特质，使得仅通过单个测量装置就能获得手柄在空间中的任意姿态；其次通过使用姿态校准算法和姿态插值算法，使得测得的姿态能够满足力觉渲染对姿态信息高频率、高精度的需求；同时在带有力反馈的虚拟现实模拟环境下，验证了该方法能够满足力觉交互的需求。

本发明提供的一种基于惯性测量装置的力反馈设备手柄的姿态测量方法，包括以下步骤：

步骤(1)、将惯性测量装置和力反馈设备的手柄固连：利用惯性测量装置的欧拉角表示力反馈设备的手柄的姿态，需要实现力反馈设备手柄和惯性测量装置的同步运动，并且两者的设备坐标系较为接近，力反馈设备的坐标系的x/y/z轴和惯性测量装置的x/y/z轴朝向相同，即可以用惯性测量装置基于自身的x/y/z轴测得欧拉角表示力反馈设备手柄的偏移；首先将惯性测量装置和力反馈设备的手柄固连，实现两者的同步运动；然后调整惯性测量装置的连接方式，使得惯性测量装置的坐标系的x/y/z轴与力反馈设备的坐标系的x/y/z轴朝向相同，实现惯性测量装置的设备坐标系和力反馈设备手柄的设备坐标系的贴合；惯性测量装置测量得到东北天坐标系下的欧拉角，其欧拉角表示姿态时的坐标系旋转顺序定义为ZYX，可理解为手柄先绕Z轴转，再绕Y轴转，再绕X轴转。

步骤(2)、上位机获取姿态信息：惯性测量装置(如通过USB-TTL模块)连接到上位机，通信方式采用串行通信。惯性测量装置测得的数据按照16进制的方式向上位机发送，每个数据分低字节和高字节依次传送，二者组合成一个有符号的short类型。上位机收到数据后，计算欧拉角的公式如下所示：

angle_x＝((wxH＜＜8)|wxL)/32768*2000

angle_y＝((wyH＜＜8)|wyL)/32768*2000

angle_z＝((wzH＜＜8)|wzL)/32768*2000

其中angle_x、angle_y和angle_z为惯性测量装置的欧拉角，wxH、wyH、wzH、wxL、wyL和wzL为惯性测量装置向上位机发送的数据；

步骤(3)、基于多线程的姿态信息处理：力觉渲染操作启动后，上位机开启力觉渲染线程进行反馈力的计算；确定惯性测量装置连接正常后，上位机开启姿态信息处理的线程，对惯性测量装置传输的欧拉角进行姿态处理，以满足力觉渲染的精度要求和频率要求；上位机建立不同线程间的通信，确保处理完成的姿态信息可以发送至力觉渲染线程，进行反馈力的计算；两者可以同步进行的，前后顺序不需要进行区分。

步骤(4)、基于姿态匹配的姿态校准：因为机械连接误差，所以惯性传感的设备坐标系和力反馈设备手柄的设备坐标系不完全重叠；当获取惯性测量装置发送的欧拉角后，需要进行姿态信息的校准；设定力反馈设备手柄的初始校准状态，通过测量得到该状态下力反馈设备手柄的方位角，求出标准初始姿态矩阵；同时测得该状态下惯性测量装置传入的欧拉角，求出实际初始姿态矩阵；通过两个矩阵间的矩阵运算，求得姿态校准矩阵，对后续传入的欧拉角进行左乘处理，将惯性测量装置的欧拉角变换到力反馈设备的坐标系。

步骤(5)、基于姿态信息插值的姿态频率校准：针对校准后的姿态信息，在下次姿态更新前，根据欧拉角姿态插值的原理，通过前两次的姿态信息预测下次循环的姿态信息，提高姿态测量的频率，满足力觉渲染姿态信息刷新频率(如1000Hz)的要求；

步骤(6)、构建虚拟力觉交互场景，测试员屏蔽掉了力反馈设备自身的姿态信息；通过步骤(1)-(5)对惯性测量装置测得的姿态信息进行了优化，得到了力反馈设备手柄的姿态角；将姿态角发送到力觉渲染计算线程，配合力反馈设备测得的力反馈设备手柄的位置信息进行力觉计算，最后通过力反馈设备向用户输出反馈力。

上述姿态测量方法，步骤(2)中，上位机收到惯性测量装置发送的数据后，先将高位字节转换为一个有符号的short类型的数据，然后再移位，获得惯性测量装置测得的欧拉角。

上述姿态测量方法，步骤(2)中，惯性测量装置测得的数据按照16进制的方式向上位机发送，每个数据分低字节和高字节依次传送，二者组合成一个有符号的short类型；上位机收到数据后，计算欧拉角的公式如下所示：

angle_x＝((wxH＜＜8)|wxL)/32768*2000

angle_y＝((wyH＜＜8)|wyL)/32768*2000

angle_z＝((wzH＜＜8)|wzL)/32768*2000

其中angle_x(俯仰)、angle_y(偏航)和angle_z(横滚)为惯性测量装置的欧拉角，wxH、wyH、wzH、wxL、wyL和wzL为惯性测量装置向上位机发送的角度数据，每个数据分低字节和高字节依次传送，二者组合成一个有符号的short类型的数据。

上述姿态测量方法，步骤(3)中，上位机在力觉计算线程外开启新的姿态处理线程对姿态信息进行处理，同时保持姿态处理线程和力觉计算线程间的通信；在不阻塞力觉计算的同时，确保处理后的姿态信息能够用于反馈力的计算。两者可以同步进行的，前后顺序不需要进行区分。

本发明的原理在于：

(1)将惯性测量装置与力反馈设备手柄固连，使两者的设备坐标系贴合，从而将力反馈设备手柄的空间姿态用惯性测量装置的方位角表示。

(2)为了处理力反馈设备手柄的空间姿态，在上位机开启多个线程，同时进行力觉计算和接收到的姿态信息的处理，并建立线程间通信，使得力觉计算线程可以及时获得处理后的姿态信息，进行工具姿态的刷新、碰撞检测和碰撞响应。

(3)为了使测得的姿态信息能满足力觉交互对姿态刷新高频率、高精度的需求，设计姿态校准算法和姿态插值算法，提高惯性测量装置测得的姿态信息的精度和频率。

本发明与现有技术相比的优点在于：

1、使用惯性测量装置测量姿态，仅需要单个装置就能测得测得末端三自由度的姿态，能够满足力反馈设备手柄机构简化的需求。

2、惯性测量装置测量姿态时，惯性测量装置直接和力反馈设备手柄相连，不需要通过机械结构和转动关节相连，测得的方位角仅与手柄的空间姿态有关，与力反馈设备移动平台的放置方式无关，测量更加精确。

附图说明

图1为基于惯性测量装置的力反馈设备手柄的姿态测量方法的流程图；

图2为在交互过程中测得的反馈力刷新曲线；

图3为用户对使用惯性测量装置和机械测量装置模拟效果评对比价图。

具体实施方式

图1给出了基于惯性测量装置测量力反馈设备手柄的姿态的具体处理流程，下面结合其他附图及具体实施方式进一步说明本发明。

本发明提供一种基于惯性测量装置的力反馈设备手柄的姿态测量方法，主要步骤介绍如下：

1、基于坐标系重叠的惯性测量装置与力反馈设备手柄连接

用惯性测量装置的欧拉角表示力反馈设备手柄的姿态；将惯性测量装置和力反馈设备手柄相连；调整惯性测量装置的连接方式，使得惯性测量装置的坐标系和力反馈设备手柄的坐标系贴合；当力反馈设备的手柄在空间运动时，与其固连的惯性测量装置也回转动相同的角度；获取惯性测量装置的欧拉角后，可以利用ZYX的转动方式，获取惯性测量装置的旋转矩阵。

2、上位机获取姿态信息

上位机获取的姿态信息可以利用下面的公式进行计算：

angle_x＝((wxH＜＜8)|wxL)/32768*2000

angle_y＝((wyH＜＜8)|wyL)/32768*2000

angle_z＝((wzH＜＜8)|wzL)/32768*2000

其中anglex、angley和anglez为惯性测量装置的欧拉角，wxH、wyH、wzH、wxL、wyL和wzL为惯性测量装置通过串行通信向上位机发送的数据；

3、基于多线程的姿态信息处理

上位机开启力觉渲染线程进行反馈力的计算；上位机开启姿态信息处理的线程，对惯性测量装置的欧拉角进行姿态处理，满足力觉渲染的精度和频率要求；上位机建立不同线程间的通信，当姿态信息处理完毕后，可以发送至力觉渲染线程，进行反馈力的计算。

4、基于姿态匹配的姿态校准

当获取姿态校准后，需要进行姿态信息的校准；设定初始校准状态，测得该状态下力反馈设备手柄的姿态矩阵；根据惯性测量装置传入的欧拉角，根据自ZYX变换顺序，求得姿态校准前的变换矩阵；通过矩阵相乘，求得姿态校准矩阵，对后续传入的欧拉角进行处理，将惯性测量装置的欧拉角变换到力反馈设备的设备坐标系。

5、基于插值的姿态频率校准

针对校准后的姿态信息，通过姿态信息插值对姿态刷新频率进行补偿。

算法：姿态频率的插值

输入：校准后的惯性测量装置测得的姿态信息

初始化：x^t、x^t-1分别为当前时刻和上一时刻存储的姿态信息，x^I为输入的姿态信息。

输出：插值后的姿态信息x^I

6、构建虚拟力觉交互场景

虚拟场景为由牙齿、舌头、脸颊等口腔组织和探针等工具组成的口腔手术场景，所有模型的数据都可以通过计算机图形学方法在程序中直接绘制。在交互时，我们通过力反馈设备手柄控制虚拟场景中的探针等工具同步运动，和不同的口腔组织进行交互。在进行模拟时，探针可以实现空间六自由度的运动(三自由度移动+三自由度转动)，其移动位置通过力反馈设备自带的测量装置获得，转动姿态通过惯性测量装置获得。当进行力觉交互时，探针姿态获取的频率和精度直接影响着模拟效果：如果精度较低，则力反馈设备手柄和虚拟探针的姿态会出现明显的差别，降低虚拟环境的沉浸感；如果频率较低，则力觉计算的频率达不到1000Hz，用户会感到明显的震动。

在一个典型的口腔手术操作中，医生使用牙周探针等工具在狭小的腔体中操作，比如伸入到牙齿与包裹在其周围牙龈之间的牙周袋中。在工具和口腔组织的交互过程中，工具的姿态变换较为频繁，比如当医生在牙周袋内转动探针时，在探针和牙齿、牙龈之间将产生多区域接触。因此测得的姿态信息对模拟效果有着显著的影响。

图2、3为采用惯性测量装置测得的姿态信息模拟上述交互时，获得的实验结果。图2为力反馈设备的输出力结果，横轴标识时间，纵轴表示力的幅值，红绿蓝三种颜色分别代表x、y、z轴的输出力，改图表明采用惯性测量装置可以获得连续地输出力，不会发生明显的震动；图3为用户主观性实验的输出结果，分别表示当采用惯性测量装置测量角度时，画面上视觉的连续性(画面是否延迟)、力觉连续性(力是否平稳)和角度一致性(角度测量的准确度)，与采用机械测量装置测量角度时是否有明显的差异，得分越高代表效果越好。结果表明，采用惯性测量装置测量角度有着较好的效果。

本发明所述的惯性测量装置可以为由传感器、敏感部件、跟踪器组成的测量装置。

本发明未详细阐述的技术内容属于本领域技术人员的公知技术。

尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

Claims (4)

1.一种基于惯性测量装置的力反馈设备手柄的姿态测量方法，其特征在于，包括以下六个步骤：

步骤(1)、将惯性测量装置和力反馈设备手柄固连：首先将惯性测量装置和力反馈设备手柄固连，实现两者的同步运动；然后调整惯性测量装置的连接方式，使得惯性测量装置的坐标系的x/y/z轴与力反馈设备的坐标系的x/y/z轴朝向相同；惯性测量装置测量得到东北天坐标系下的欧拉角，用惯性测量装置测得的欧拉角表示力反馈设备手柄的姿态时的坐标系旋转顺序定义为ZYX，表示力反馈设备手柄先绕Z轴转，再绕Y轴转，再绕X轴转；

步骤(2)、上位机获取姿态信息：惯性测量装置连接到上位机，通信方式采用串行通信；

步骤(3)、基于多线程的姿态处理：力觉渲染操作启动后，上位机开启力觉渲染计算线程进行反馈力的计算；确定惯性测量装置连接正常后，上位机开启姿态信息处理线程，对惯性测量装置传输的欧拉角进行姿态处理，以满足力觉渲染计算的精度要求和频率要求；上位机建立不同线程间的通信，确保处理完成的姿态信息可以发送至力觉渲染计算线程，进行反馈力的计算；

步骤(4)、基于姿态匹配的姿态校准：当获取惯性测量装置测得的欧拉角后，对姿态信息进行校准；设定力反馈设备手柄的初始校准状态，通过测量得到该状态下力反馈设备手柄的方位角，求出标准初始姿态矩阵；同时测得该状态下惯性测量装置传入的欧拉角，求出实际初始姿态矩阵；通过两个矩阵间的矩阵运算，求得姿态校准矩阵，对后续传入的欧拉角进行左乘处理，将惯性测量装置的欧拉角变换到力反馈设备的坐标系；

步骤(5)、基于姿态信息插值的姿态频率校准：针对校准后的姿态信息，在下次姿态更新前，根据欧拉角姿态插值的原理，通过前两次的姿态信息预测下次循环的姿态信息，提高姿态测量的频率，满足力觉渲染姿态信息预设的刷新频率的要求；

步骤(6)、构建虚拟力觉交互场景，屏蔽掉力反馈设备自身的姿态信息；通过步骤(1)-(5)对惯性测量装置测得的姿态信息进行优化，得到力反馈设备手柄的姿态角；将姿态角发送到力觉渲染计算线程，配合力反馈设备测得的力反馈设备手柄的位置信息进行力觉计算，最后通过力反馈设备向用户输出反馈力。

2.根据权利要求1所述的基于惯性测量装置的力反馈设备手柄的姿态测量方法，其特征在于：所述步骤(2)中，上位机收到惯性测量装置发送的数据后，先将高位字节转换为一个有符号的short类型的数据，然后再移位，获得惯性测量装置测得的欧拉角。

3.根据权利要求2所述的基于惯性测量装置的力反馈设备手柄的姿态测量方法，其特征在于：所述步骤(2)中，惯性测量装置测得的数据按照16进制的方式向上位机发送，每个数据分低字节和高字节依次传送，二者组合成一个有符号的short类型；上位机收到数据后，计算欧拉角的公式如下所示：

angle_x＝((wxH＜＜8)|wxL)/32768*2000

angle_y＝((wyH＜＜8)|wyL)/32768*2000

angle_z＝((wzH＜＜8)|wzL)/32768*2000

其中angle_x(俯仰)、angle_y(偏航)和angle_z(横滚)为惯性测量装置的欧拉角，wxH、wyH、wzH、wxL、wyL和wzL为惯性测量装置向上位机发送的角度数据，每个数据分低字节和高字节依次传送，二者组合成一个有符号的short类型的数据。

4.根据权利要求1所述的基于惯性测量装置的力反馈设备手柄的姿态测量方法，其特征在于：所述步骤(3)中，上位机在力觉计算线程外开启新的姿态处理线程对姿态信息进行处理，同时保持姿态处理线程和力觉计算线程间的通信；在不阻塞力觉计算的同时，确保处理后的姿态信息能够用于反馈力的计算。