CN107102749B - 一种基于超声波和惯性传感器的三维笔式定位方法 - Google Patents
一种基于超声波和惯性传感器的三维笔式定位方法 Download PDFInfo
- Publication number
- CN107102749B CN107102749B CN201710270094.8A CN201710270094A CN107102749B CN 107102749 B CN107102749 B CN 107102749B CN 201710270094 A CN201710270094 A CN 201710270094A CN 107102749 B CN107102749 B CN 107102749B
- Authority
- CN
- China
- Prior art keywords
- ultrasonic wave
- dimensional
- coordinate
- inertial sensor
- ultrasonic
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F3/00—Input arrangements for transferring data to be processed into a form capable of being handled by the computer; Output arrangements for transferring data from processing unit to output unit, e.g. interface arrangements
- G06F3/01—Input arrangements or combined input and output arrangements for interaction between user and computer
- G06F3/03—Arrangements for converting the position or the displacement of a member into a coded form
- G06F3/033—Pointing devices displaced or positioned by the user, e.g. mice, trackballs, pens or joysticks; Accessories therefor
- G06F3/0346—Pointing devices displaced or positioned by the user, e.g. mice, trackballs, pens or joysticks; Accessories therefor with detection of the device orientation or free movement in a 3D space, e.g. 3D mice, 6-DOF [six degrees of freedom] pointers using gyroscopes, accelerometers or tilt-sensors
Landscapes
- Engineering & Computer Science (AREA)
- General Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- Human Computer Interaction (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
- Length Measuring Devices Characterised By Use Of Acoustic Means (AREA)
Abstract
本发明涉及一种基于超声波和惯性传感器的三维笔式定位方法，适用于虚拟现实、人机交互、信息化教育、三维多媒体交互控制等应用领域。电子笔集成超声波发射器、红外发射器和九轴惯性传感器，通过三维超声波无线定位技术获得电子笔笔尖的三维坐标，在超声信号受到遮挡时，利用九轴传感器的进行三维定位，并根据姿态信息，计算出电子笔超声波传感器的坐标，解决超声波在遮挡情况下的无法对电子笔定位的问题，并利用超声波三维定位坐标实时校正九轴传感器定位坐标的累积误差，从而实现超声定位与九轴惯性传感器定位的互相弥补，实现三维交互空间的无遮挡轨迹跟踪和连续操控。
Description
技术领域
本发明涉及一种三维笔式定位方法，具体涉及一种基于超声波和九轴(三轴加速度、三轴陀螺仪和三轴地磁)惯性传感器的三维联合定位技术，以超声波三维定位为主，在超声波传感器受到遮挡时由九轴传感器实现电子笔的定位，九轴传感器的累积误差由超声波三维定位坐标进行实时校正，可实现电子笔的三维无遮挡实时定位与跟踪，可用于虚拟现实(VR)、人机交互、信息化教育、三维多媒体交互控制等应用领域。
背景技术
随着三维人机交互、虚拟现实的迅速发展，移动目标(操作杆、电子笔、头部、手等)的实时三维定位跟踪技术成为必需技术。
目前用于三维交互和虚拟现实空间定位技术主要包括如下几种：
(1)激光定位技术
基本原理：在空间内安装数个可发射激光的装置，对空间发射横竖两个方向扫射的激光，被定位的物体上放置了多个激光感应接收器，通过计算两束光线到达定位物体的角度差，从而得到物体的三维坐标，物体在移动时三维坐标也会跟着变化，便得到了动作信息，完成动作的捕捉。
优缺点：激光定位技术的优势在于定位精度高，同时可支持多个目标定位，可移动范围广。缺点是其利用机械方式来控制激光扫描，稳定性和耐用性较差，随着使用时间的加长，机械结构磨损，也会导致定位失灵等故障，虽然被跟踪目标上放置了多个激光感应接收器，仍存在因遮挡而无法定位问题。
(2)红外光学定位技术
基本原理：通过在空间内安装多个红外发射摄像头，从而对整个空间进行覆盖拍摄，被定位的物体表面安装了红外反光点，摄像头发出的红外光经反光点反射，随后捕捉到这些经反射的红外光，配合多个摄像头工作再通过后续程序计算后便能得到被定位物体的空间坐标。
优缺点：红外光学定位技术的优点是有着较高的定位精度，延迟率低，不足的是这全套设备加起来成本非常高，而且使用起来很麻烦，需要在空间内搭建非常多的摄像机，存在遮挡而无法定位问题。
(3)可见光定位技术
基本原理：可见光定位技术的原理和红外光学定位技术有点相似，同样采用摄像头捕捉被追踪物体的位置信息，只是其不再利用红外光，而是直接利用可见光，在不同的被追踪物体上安装能发出不同颜色的发光灯，摄像头捕捉到这些颜色光点从而区分不同的被追踪物体以及位置信息。
优缺点：可见光定位技术的优点是造价成本最低，而且无需后续复杂的算法，技术实现难度不大。不足的是这种技术定位精度相对较差，抗遮挡性差，而且对环境也有一定的使用限制，假如周围光线太强，灯光被削弱，可能无法定位，如果使用空气有相同色光则可能导致定位错乱，同时也由于摄像头视角原因，可移动范围小，灯光数量有限，可追踪目标不多。
(4)计算机视觉动作捕捉技术
基本原理：基于计算机视觉原理，其由多个高速相机从不同角度对运动目标进行拍摄，当目标的运动轨迹被多台摄像机获取后，通过后续程序的运算，便能在电脑中得到目标的轨迹信息，也就完成了移动目标的定位、跟踪以及动作捕捉。
优缺点：该技术的优点是利用少量的摄像机对监测区域的多目标进行动作捕捉，大物体定位精度高，同时被监测对象不需要穿戴和拿取任何定位设备，约束性小，更接近真实的体感交互体验。不足的是，这种技术需要庞大的程序计算量，对硬件设备有一定配置要求，同时受外界环境影响大，比如环境光线昏暗、背景杂乱、有遮挡物等都无法很好的完成目标跟踪；此外捕捉的动作如果不是合理的摄像机视角以及程序处理影响等，对于比较精细的动作可能无法准确捕捉。
(5)基于惯性传感器的跟踪技术
基本原理：被追踪目标需要集成加速度计，陀螺仪和磁力计等惯性传感器设备，当目标在运动时，这些元器件的位置和姿态信息被改变，通过姿态、加速度等信息，可以获得目标运动的坐标和轨迹，实现移动目标的定位。
优缺点：基于惯性传感器的动作捕捉技术的优点受外界的影响小，不用在使用空间上安装“灯塔”、摄像头等杂乱部件，而且可获取的动作信息量大、灵敏度高、动态性能好、可移动范围广，定位跟踪无遮挡，体感交互也完全接近真实的交互体验。不足的是惯性传感器无法得到绝对坐标，并且轨迹累积误差较大。
(6)超声波三维定位技术
基本原理：一般利用三个不在同一条直线上的接收器(含超声波接收传感器和红外接收器)，移动目标以红外信号作为发射超声波信号的时间基准，接收到红外信号后，开始计时，直到接收器接收到超声波信号后，提取超声波的传播时间，三路超声波传播时间乘以声速，得打移动目标到达三个接收器的距离，通过几何计算即可得到移动目标的位置，从而实现移动目标的定位和跟踪。
优缺点：该技术优点是定位精度高，实习性高，成本低，结构简单，通过布置多个接收器实现大范围跟踪定位。不足的是超声波定位易受遮挡。
针对上述各种三维空间定位技术的不足，本发明公开一种基于超声波和九轴惯性传感器的三维笔式定位系统，该系统通过超声波三维定位技术实现对电子笔的三维定位，在超声波收到遮挡时由九轴惯性传感器(三轴加速度、三轴陀螺仪和三轴地磁)实现电子笔的实时定位与轨迹跟踪，九轴惯性传感器的轨迹跟踪的坐标由电子笔的超声波三维定位坐标实时校准，降低累积误差的影响，实现电子笔的三维无遮挡实时定位与跟踪，并提供电子笔三维姿态的识别，为三维交互操作和虚拟现实中移动目标的定位和跟踪提供一种人机交互工具。
中国专利“一种超声波定位装置及其定位方法”(申请号200710026504.0)公开了一种超声波定位装置及方法，利用4个以上超声接收模块，实现对发射笔的三维定位，支持非接触触控功能，但未考虑超声遮挡问题，电子笔不具备姿态识别功能。
中国专利“大范围多目标超声跟踪定位系统和方法”(申请号201010546442.8)公开了一种大范围多目标超声跟踪定位系统及方法，采用不同频率和不同时隙实现多目标识别，采用空间分割法实现大范围定位和跟踪，但未考虑超声遮挡问题，也不具备姿态识别功能。
中国专利“一种三维交互电子白板系统及方法”(申请号201310404513.4)公开了一种三维交互电子白板系统及方法，基于三维超声波无线定位技术，利用频分复用技术和陷波技术实现多目标实时的分离与识别。根据超声波接收信号的幅值，自动选取幅值较大且不在同一条直线上的4个接收器为定位接收器，利用信噪比较高的超声波信号进行目标定位。基于超声波到达时间差方法由主控制器完成移动目标的三维定位，但未考虑超声遮挡问题，也不具备姿态识别功能。
中国专利“一种多感知便捷式智能笔”(申请号201420470088.90)公开了一种多感知便捷式智能笔，包括笔壳、电容屏触摸笔头、NFC感应线圈、条码扫描引擎模组、加速度/陀螺仪/地磁九轴传感器、温度/湿度传感器、控制芯片、蓝牙/WIFI通信模块等，能够在触摸屏上通过电容触摸头实现笔迹书写，通过九轴传感器获得智能笔姿态，并通过蓝牙/WIFI通信模块将姿态、条码信息发送到互连设备，但该智能笔仅能在触摸屏上实现二维定位，定位范围小，不能实现对智能笔的三维定位。
中国专利“一种基于惯性姿态与超声波测距的三维定位方法及装置”(申请号201610081788.2)公开了一种基于惯性姿态与超声波测距的定位方法及装置，在该发明中，地面定位参照模块包括超声波发射单元及无线同步单元，待定位目标模块包括惯性单元、超声波接收单元及无线同步单元，利用地面参照模块到待定位目标模块上的不少于三个超声波距离值，结合待定位目标模块的姿态数据，计算获得待定位目标模块的三维坐标值，并利用惯性传感器获得姿态，在待定位模块(四轴飞行器)倾斜时，对地面定位参考模块实现定位，该发明仅选取四个接收器中的三个来减少对超声波信号的遮挡，只能部分减小遮挡问题，并未考虑惯性传感器与超声波信号的联合定位问题。
发明内容
本发明提供一种基于超声波和惯性传感器的三维笔式定位方法，以降低空间三维定位系统的复杂性，降低成本，消除移动目标对定位信号的遮挡问题，目的是要提供一种低成本、低复杂度、无遮挡的三维笔式定位方法，该方法基于三维笔式定位系统，三维笔式定位系统主要包括三维笔式交互系统，该系统主要包括主机和电子笔(移动目标)两部分，其中主机包括接收器、控制器两部分，各部分组成及功能如下：
(1)电子笔
电子笔集成微处理器A(MCU1)、PVDF压电薄膜、红外发射管、MEMS九轴(三轴加速度、三轴陀螺仪和三轴地磁)惯性传感器、无线通信模块、按键等；
PVDF压电薄膜为超声波发射器，发射定位所需的超声波信号，红外发射管发射超声波三维定位所需的同步信号，微处理器通过MEMS九轴(三轴加速度、三轴陀螺仪和三轴地磁)惯性传感器提取加速度信号和姿态识别，并利用加速度和姿态信息获得电子笔的辅助坐标(九轴惯性传感器的坐标)，可以采用单片机、ARM、DSP，无线通信模块主要负责将电子笔的姿态信息、惯性传感器数据以及发送给控制器以及接收控制器发送的惯性传感器校准数据，无线模块可以采用蓝牙模块或者ISM频段射频模块，按键主要为电子笔提供各种功能的切换；
(2)主机
1)接收器
本系统采用3个接收器，每个接收器均包括超声波传感器、超声波放大滤波电路、红外传感器、红外放大滤波电路以及距离提取电路；
2)控制器
主要包括微处理器B(MCU2)、无线通信模块、通信接口电路、温度传感器以及电源电路；
微处理器B主要完成超声三维定位和惯性传感器三维定位，并完成二者的定位融合；
无线通信模块主要负责接收电子笔的姿态信息和惯性传感器数据，并发送电子笔的惯性传感器校准数据；
通信接口电路与上位机通信，完成被跟踪目标三维定位坐标数据的传送并接收系统软件命令和系统配置信息；
温度传感器提供环境温度，用于计算超声波的传播速度；
电源电路为主机提供所需的电源。
基于上述三维笔式定位系统，本发明基于超声波和惯性传感器的三维笔式定位方法，包括下列步骤：
(1)触发电子笔按键，电子笔周期性发射红外和超声波信号；
(2)接收器判断是否接收到红外，若收到红外信号，跳转到步骤(3)，否则跳转到步骤(6)；
(3)通过三个接收器分别接收超声波信号，以红外信号作为超声波传播时间测量的基准提取超声波的传播时间，根据控制器提供的环境温度参数计算超声波传播的速度，用传播时间乘以传播速度得到电子笔到各个接收器的传播距离l1、l2、l3，并送给主控制器；
(4)控制器判断是否完整收到三路超声波距离，若完整收到三路超声距离，跳转到(5)，否则跳转到(6)；
(5)控制器三路超声波传播距离l1、l2、l3距离后，利用几何定位，计算出超声波定位的三维坐标，第1个超声波定位周期的三维定位坐标表示为P1(x1,y1,z1)，若第n个定位周期，超声波的三维定位坐标表示为Pn(xn,yn,zn)，下标n表示第n个超声波定位周期，每连续成功完成一次超声波定位后，对n用n+1更新，并通过无线通信模块读取电子笔的方向角θn、俯仰角Φn和翻滚角γn，并保存为Qn(θn,Φn,γn)，跳转到步骤(2)继续执行；
(6)判断超声波联系成功定位周期数n是否大于2，若大于2则跳转到步骤(7)，否则跳转到步骤(2)；
(7)对n用n+1跟新，通过无线通信模块读取电子笔姿态信息：四元数q0、q1、q2、q3和加速度信息axbn、aybn、azbn，计算惯性传感器的三维定位坐标In(xin,yin,zin)；
(8)根据电子笔姿态和超声波传感器与惯性传感器之间的距离，基于惯性传感器的三维坐标In(xin,yin,zin)计算PVDF压电薄膜的三维坐标Pn(xn,yn,zn)，获得Pn(xn,yn,zn)后，跳转到步骤(2)继续执行。
本发明所述步骤(7)中计算惯性传感器的三维定位坐标In(xin,yin,zin)的具体方法为：
读取的姿态信息为九轴惯性传感器输出的姿态信息—四元数q0、q1、q2、q3和加速度信息Abn(axbn、aybn、azbn)，构造惯性传感器坐标系—超声波定位坐标系的四元数方向变换矩阵，将惯性传感器定位坐标系的加速度axbn、aybn、azbn变换到超声波定位坐标系的加速度axon、ayon、azon，根据电子笔在第n-1周期的姿态角Qn-1(θn-1,Φn-1,γn-1)和超声波三维定位坐标Pn-1(xn-1,yn-1,zn-1)计算出In-1(xin-1,yin-1,zin-1)，根据电子笔在第n-2周期的姿态角Qn-2(θn-2,Φn-2,γn-2)和超声三维定位坐标Pn-2(xn-2,yn-2,zn-2)计算出In-2(xin-2,yin-2,zin-2)，利用In-1(xin-1,yin-1,zin-1)和In-2(xin-2,yin-2,zin-2)计算惯性传感器在第n周期的初始速度Vn0(vxn0,vyn0,vzn0)，以Vn0(vxn0,vyn0,vzn0)为初始速度，对加速度axon、ayon、azon分别进行积分并消除积分过程中产生的一次趋势项，得到惯性传感器的速度Vn，以In-1(xin-1,yin-1,zin-1)为初始坐标，对Vn进行积分并消除积分过程中产生的一次趋势项，得到第n周期的惯性传感器的三维坐标In(xin,yin,zin)。
本发明所述惯性传感器坐标系—超声波定位坐标系的四元数方向变换矩阵：
本发明采用(2)式将惯性传感器定位坐标系的加速度axbn、aybn、azbn变换到超声波定位坐标系的加速度axon、ayon、azon：
本发明利用In-1(xin-1,yin-1,zin-1)和In-2(xin-2,yin-2,zin-2)计算惯性传感器在第n周期的初始速度Vn0(vxn0,vyn0,vzn0)，方法是：设电子笔超声波发射的周期和惯性传感器姿态信息和加速度信息的采集周期均为To，计算公式如下：
vxn0＝(xin-2-xin-1)/To (6)
vyn0＝(yin-2-yin-1)/To (7)
vzn0＝(zin-2-zin-1)/To (8)。
本发明所述步骤(7)中用超声波三维定位坐标为惯性传感器三维定位提供绝对参考，定期自动消除惯性传感器的累积误差。
本发明的优点在于：基于超声波和惯性传感器联合完成电子笔的三维空间定位，利用三路超声波实现电子笔的三维空间定位，当超声波受到遮挡，采用惯性传感器进行空间三维定位，两种定位方式自动切换，实现电子笔三维空间无遮挡定位，系统结构简单，成本低、定位范围大、定位精度高、实时性好。
附图说明
图1是本发明三维笔式定位系统组成示意图；
图2是本发明电子笔组成示意图；
图3是本发明主机组成原理框图；
图4是本发明定位方法流程图；
图5是本发明惯性传感器定位方法流程图；
图6是本发明超声波定位坐标与惯性传感器定位坐标转换示意图。
具体实施方式
本发明提供的基于超声波和惯性传感器的三维笔式定位方法基于如图1所示的三维笔式交互系统，该系统主要包括主机和电子笔(移动目标)两部分：
电子笔的结构图如图2所示，电子笔集成微处理器A(MCU1)202、PVDF压电薄膜101、红外发射管102、MEMS九轴(三轴加速度、三轴陀螺仪和三轴地磁)惯性传感器103、无线通信模块201、按键203、笔尖204；
所述PVDF压电薄膜101为超声波发射器，发射定位所需的超声波信号，红外发射管发射超声波三维定位所需的同步信号；微处理器A202通过MEMS九轴(三轴加速度、三轴陀螺仪和三轴地磁)惯性传感器103(以后简称惯性传感器)提取加速度信号和姿态信息，可以采用单片机、ARM、DSP；无线通信模块201主要负责将电子笔的四元数(q0、q1、q2、q3)姿态信息、加速度传感器数据发送给主机，以及接收主机发送的惯性传感器校准数据，无线通信模块201可以采用蓝牙模块或者ISM频段射频模块；按键203主要为电子笔提供各种功能的切换；笔尖204主要用于电子笔在平面接触书写和操控时使用，PVDF压电薄膜101与MEMS九轴惯性传感器103距离为b；
主机主要包括控制器300和3个不在同一条直线上的接收器，具体原理如图3所示：
每个接收器均包括超声波传感器、超声波放大滤波电路、红外传感器、红外放大滤波电路以及距离提取电路；
接收器具体组成分别为接收器A301、接收器B302、接收器C303；
控制器300主要包括微处理器B(MCU2)、无线通信模块、通信接口电路、温度传感器以及电源电路；微处理器B主要完成超声三维定位和惯性传感器三维定位，并完成二者的定位融合；无线通信模块主要负责接收电子笔的姿态信息和惯性传感器数据，并发送电子笔的惯性传感器校准数据；通信接口电路与上位机通信，完成被跟踪目标三维定位坐标数据的传送并接收系统软件命令和系统配置信息；温度传感器提供环境温度，用于计算超声波的传播速度；电源电路为主机提供所需的电源；
定义超声波三维定位坐标系为O-XoYoZo，九轴惯性传感器的坐标系为B-XbYbZb，如图1所示。
如图4所示，基于上述笔式交互系统，本发明一种基于超声波和惯性传感器的三维笔式定位方法，包括下列步骤：
(1)触发电子笔按键，电子笔周期性发射红外和超声波信号；
(2)接收器判断是否接收到红外信号，若收到红外信号，跳转到步骤(3)，否则跳转到步骤(6)；
(3)通过三个接收器分别接收超声波信号，以红外信号作为超声波传播时间测量的基准提取超声波的传播时间，根据控制器提供的环境温度参数计算超声波传播的速度，用传播时间乘以传播速度得到电子笔到各个接收器的传播距离l1、l2、l3，并送给控制器；
(4)控制器判断是否完整收到三路超声波距离，若完整收到三路超声距离，跳转到(5)，否则跳转到(6)；
(5)控制器三路超声波传播距离l1、l2、l3距离后，利用几何定位，计算出超声波定位的三维坐标，第1个超声波定位周期的三维定位坐标表示为P1(x1,y1,z1)，若第n个定位周期，超声波的三维定位坐标表示为Pn(xn,yn,zn)，下标n表示第n个超声波定位周期，每连续成功完成一次超声波定位后，对n用n+1更新，并通过无线通信模块读取电子笔的方向角θn、俯仰角Φn和翻滚角γn，并保存为Qn(θn,Φn,γn)，跳转到步骤(2)继续执行；
(6)判断超声波联系成功定位周期数n是否大于2，若大于2则跳转到步骤(7)，否则跳转到步骤(2)；
(7)对n用n+1更新，通过无线通信模块读取电子笔姿态信息(四元数q0、q1、q2、q3)和加速度(axbn、aybn、azbn)信息，计算惯性传感器的三维定位坐标In(xin,yin,zin)，具体方法流程如图5所示，详细描述如下：
为提高实时性，只读取的姿态信息为九轴惯性传感器输出的姿态信息—四元数q0、q1、q2、q3和加速度信息Abn(axbn、aybn、azbn)，构造惯性传感器坐标系—超声波定位坐标系的四元数方向变换矩阵：
利用公式(1)将惯性传感器定位坐标系的加速度axbn、aybn、azbn变换到超声波定位坐标系，得到：
上式中axon、ayon、azon为超声波定位坐标系下电子笔三个轴的加速度；
超声波三维定位坐标到惯性传感器三维定位坐标的转换关系如图6所示，图6中，定义方向角为θ(-180°～180°)，俯仰角为Φ(-90°～90°)，方向角θ和俯仰角Φ可以从四元数变换矩阵公式(1)中计算得到，超声波传感器与惯性传感器的距离为b，第n周期的超声波三维定位坐标Pn(xn,yn,zn)与惯性传感器三维定位坐标In(xin,yin,zin)的转换关系为：
xin＝xn+bsinΦnsinθn (3)
yin＝yn+bsinΦncosθn (4)
zin＝zn+bcosΦn (5)
利用公式(3)、公式(4)和公式(5)，根据电子笔在第n-1周期的姿态角Qn-1(θn-1,Φn-1,γn-1)和超声波三维定位坐标Pn-1(xn-1,yn-1,zn-1)计算出In-1(xin-1,yin-1,zin-1)，根据电子笔在第n-2周期的姿态角Qn-2(θn-2,Φn-2,γn-2)和超声三维定位坐标Pn-2(xn-2,yn-2,zn-2)计算出In-2(xin-2,yin-2,zin-2)，利用In-1(xin-1,yin-1,zin-1)和In-2(xin-2,yin-2,zin-2)计算惯性传感器在第n周期的初始速度Vn0(vxn0,vyn0,vzn0)，设电子笔超声波发射的周期和惯性传感器姿态信息和加速度信息的采集周期均为To，计算公式如下：
vxn0＝(xin-2-xin-1)/To (6)
vyn0＝(yin-2-yin-1)/To (7)
vzn0＝(zin-2-zin-1)/To (8)
以Vn0(vxn0,vyn0,vzn0)为初始速度，对加速度axon、ayon、azon分别进行积分并消除积分过程中产生的一次趋势项，得到惯性传感器的速度Vn，以In-1(xin-1,yin-1,zin-1)为初始坐标，对Vn进行积分并消除积分过程中产生的一次趋势项，得到第n周期的惯性传感器的三维坐标In(xin,yin,zin)；
Vn0(vxn0,vyn0,vzn0)和In-1(xin-1,yin-1,zin-1)均由超声波三维定位坐标计算得到，给惯性传感器提供绝对参考，消除了累积误差；
(8)根据电子笔姿态和超声波传感器与惯性传感器之间的距离，基于惯性传感器的三维坐标In(xin,yin,zin)计算PVDF压电薄膜的三维坐标Pn(xn,yn,zn)，具体如下：
根据图6所示超声波三维定位坐标到惯性传感器三维定位坐标的转换关系，可由惯性传感器的三维坐标In(xin,yin,zin)计算得到等效的超声波定位坐标Pn(xn,yn,zn)：
xn＝xin-bsinΦnsinθn (8)
yn＝yin-bsinΦncosθn (9)
zn＝zin-bcosΦn (10)
获得Pn(xn,yn,zn)后，跳转到步骤(2)继续执行。
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内所做的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
Claims (5)
1.一种基于超声波和惯性传感器的三维笔式定位方法，其特征在于，包括下列步骤：
(1)触发电子笔按键，电子笔周期性发射红外和超声波信号；
(2)接收器判断是否接收到红外，若收到红外信号，跳转到步骤(3)，否则跳转到步骤(6)；
(3)通过三个接收器分别接收超声波信号，以红外信号作为超声波传播时间测量的基准提取超声波的传播时间，根据控制器提供的环境温度参数计算超声波传播的速度，用传播时间乘以传播速度得到电子笔到各个接收器的传播距离l1、l2、l3，并送给主控制器；
(4)控制器判断是否完整收到三路超声波距离，若完整收到三路超声距离，跳转到(5)，否则跳转到(6)；
(5)控制器三路超声波传播距离l1、l2、l3距离后，利用几何定位，计算出超声波定位的三维坐标，第1个超声波定位周期的三维定位坐标表示为P1(x1,y1,z1)，若第n个定位周期，超声波的三维定位坐标表示为Pn(xn,yn,zn)，下标n表示第n个超声波定位周期，每连续成功完成一次超声波定位后，对n用n+1更新，并通过无线通信模块读取电子笔的方向角θn、俯仰角Φn和翻滚角γn，并保存为Qn(θn,Φn,γn)，跳转到步骤(2)继续执行；
(6)判断超声波联系成功定位周期数n是否大于2，若大于2则跳转到步骤(7)，否则跳转到步骤(2)；
(7)对n用n+1更新，通过无线通信模块读取电子笔姿态信息：四元数q0、q1、q2、q3和加速度信息：axbn、aybn、azbn，计算惯性传感器的三维定位坐标In(xin,yin,zin)，具体方法为：
读取的姿态信息为九轴惯性传感器输出的姿态信息—四元数q0、q1、q2、q3和加速度信息Abn(axbn、aybn、azbn)，构造惯性传感器坐标系—超声波定位坐标系的四元数方向变换矩阵，将惯性传感器定位坐标系的加速度axbn、aybn、azbn变换到超声波定位坐标系的加速度axon、ayon、azon，根据电子笔在第n-1周期的姿态角Qn-1(θn-1,Φn-1,γn-1)和超声波三维定位坐标Pn-1(xn-1,yn-1,zn-1)计算出In-1(xin-1,yin-1,zin-1)，根据电子笔在第n-2周期的姿态角Qn-2(θn-2,Φn-2,γn-2)和超声三维定位坐标Pn-2(xn-2,yn-2,zn-2)计算出In-2(xin-2,yin-2,zin-2)，利用In-1(xin-1,yin-1,zin-1)和In-2(xin-2,yin-2,zin-2)计算惯性传感器在第n周期的初始速度Vn0(vxn0,vyn0,vzn0)，以Vn0(vxn0,vyn0,vzn0)为初始速度，对加速度axon、ayon、azon分别进行积分并消除积分过程中产生的一次趋势项，得到惯性传感器的速度Vn，以In-1(xin-1,yin-1,zin-1)为初始坐标，对Vn进行积分并消除积分过程中产生的一次趋势项，得到第n周期的惯性传感器的三维坐标In(xin,yin,zin)；
(8)根据电子笔姿态和超声波传感器与惯性传感器之间的距离，基于惯性传感器的三维坐标In(xin,yin,zin)计算超声波的三维定位坐标Pn(xn,yn,zn)，获得Pn(xn,yn,zn)后，跳转到步骤(2)继续执行。
2.根据权利要求1所述的一种基于超声波和惯性传感器的三维笔式定位方法，其特征在于，惯性传感器坐标系—超声波定位坐标系的四元数方向变换矩阵：
3.根据权利要求1所述的一种基于超声波和惯性传感器的三维笔式定位方法，其特征在于，采用(2)式将惯性传感器定位坐标系的加速度axbn、aybn、azbn变换到超声波定位坐标系的加速度axon、ayon、azon：
4.根据权利要求1所述的一种基于超声波和惯性传感器的三维笔式定位方法，其特征在于，利用In-1(xin-1,yin-1,zin-1)和In-2(xin-2,yin-2,zin-2)计算惯性传感器在第n周期的初始速度Vn0(vxn0,vyn0,vzn0)，方法是：设电子笔超声波发射的周期和惯性传感器姿态信息和加速度信息的采集周期均为To，计算公式如下：
vxn0＝(xin-2-xin-1)/To (6)
vyn0＝(yin-2-yin-1)/To (7)
vzn0＝(zin-2-zin-1)/To (8)。
5.根据权利要求1所述的一种基于超声波和惯性传感器的三维笔式定位方法，其特征在于，步骤(7)中用超声波三维定位坐标为惯性传感器三维定位提供绝对参考，定期自动消除惯性传感器的累积误差。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710270094.8A CN107102749B (zh) | 2017-04-23 | 2017-04-23 | 一种基于超声波和惯性传感器的三维笔式定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710270094.8A CN107102749B (zh) | 2017-04-23 | 2017-04-23 | 一种基于超声波和惯性传感器的三维笔式定位方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107102749A CN107102749A (zh) | 2017-08-29 |
| CN107102749B true CN107102749B (zh) | 2019-11-08 |
Family
ID=59657882
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710270094.8A Active CN107102749B (zh) | 2017-04-23 | 2017-04-23 | 一种基于超声波和惯性传感器的三维笔式定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107102749B (zh) |
Families Citing this family (18)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107608541B (zh) * | 2017-10-17 | 2021-03-05 | 宁波视睿迪光电有限公司 | 三维姿态定位方法、装置及电子设备 |
| CN107704106B (zh) * | 2017-10-17 | 2021-04-09 | 宁波视睿迪光电有限公司 | 姿态定位方法、装置及电子设备 |
| CN107728788A (zh) * | 2017-10-31 | 2018-02-23 | 吉林大学 | 一种基于红外超声三维定位体感交互装置 |
| EP3721322B1 (en) * | 2017-12-06 | 2024-05-22 | Invensense, Inc. | System for fusing acoustic and inertial position determination |
| CN108205373B (zh) * | 2017-12-25 | 2021-08-13 | 北京致臻智造科技有限公司 | 一种交互方法及系统 |
| CN108387871B (zh) * | 2018-01-30 | 2021-10-29 | 吉林大学 | 一种实现六自由度测量的超声三维定位系统和定位方法 |
| CN108801246B (zh) * | 2018-04-26 | 2021-12-07 | 北京凌宇智控科技有限公司 | 一种空间精确定位的方法及系统 |
| CN108553105A (zh) * | 2018-05-08 | 2018-09-21 | 上海逸动医学科技有限公司 | 关节运动检测的组合式传感器、关节运动检测系统及方法 |
| CN110488996A (zh) * | 2018-05-15 | 2019-11-22 | 名硕电脑(苏州)有限公司 | 绘图笔及具有其的互动系统 |
| CN108958483A (zh) * | 2018-06-29 | 2018-12-07 | 深圳市未来感知科技有限公司 | 基于交互笔的刚体定位方法、装置、终端设备及存储介质 |
| CN109143214B (zh) * | 2018-07-26 | 2021-01-08 | 上海乐相科技有限公司 | 一种采用激光扫描的目标定位方法及装置 |
| CN109738860B (zh) * | 2018-11-23 | 2020-09-08 | 青岛小鸟看看科技有限公司 | 外接设备的定位方法及装置、虚拟现实头戴设备及系统 |
| CN109621401A (zh) * | 2018-12-29 | 2019-04-16 | 广州明朝互动科技股份有限公司 | 一种互动游戏系统及控制方法 |
| CN110132257B (zh) * | 2019-05-15 | 2023-03-24 | 吉林大学 | 基于多传感器数据融合的人体行为预测方法 |
| CN110262667B (zh) * | 2019-07-29 | 2023-01-10 | 上海乐相科技有限公司 | 一种虚拟现实设备及定位方法 |
| CN111580686B (zh) * | 2020-05-10 | 2021-07-20 | 吉林大学 | 一种融合超声定位和惯性测量单元的三维笔势识别方法 |
| CN112197769A (zh) * | 2020-10-23 | 2021-01-08 | 徐工集团工程机械股份有限公司道路机械分公司 | 一种工程机械隧道山坳环境声音回波+惯导组合定位方法 |
| CN112433628B (zh) * | 2021-01-28 | 2021-06-08 | 深圳市瑞立视多媒体科技有限公司 | 双光球交互笔的刚体位姿确定方法、装置和计算机设备 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7526944B2 (en) * | 2004-01-07 | 2009-05-05 | Ashok Sabata | Remote monitoring of pipelines using wireless sensor network |
| CN103353304A (zh) * | 2013-06-25 | 2013-10-16 | 深圳市宇恒互动科技开发有限公司 | 一种对三维惯性运动感测进行补偿的方法及装置 |
| US8564551B2 (en) * | 2008-10-24 | 2013-10-22 | Samsung Electronics Co., Ltd. | Input device for foldable display device and input method thereof |
| CN103455171A (zh) * | 2013-09-09 | 2013-12-18 | 吉林大学 | 一种三维交互电子白板系统及方法 |
| CN103501537A (zh) * | 2013-09-24 | 2014-01-08 | 北京大学 | 基于智能手机和Wi-Fi无线网络的楼宇内部定位方法和系统 |
| CN104385808A (zh) * | 2014-10-27 | 2015-03-04 | 邓乐安 | 基于惯性测量的原笔迹记录智能笔 |
| CN105572638A (zh) * | 2016-02-06 | 2016-05-11 | 向杰 | 一种基于惯性姿态与超声波测距的三维定位方法及装置 |
-
2017
- 2017-04-23 CN CN201710270094.8A patent/CN107102749B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7526944B2 (en) * | 2004-01-07 | 2009-05-05 | Ashok Sabata | Remote monitoring of pipelines using wireless sensor network |
| US8564551B2 (en) * | 2008-10-24 | 2013-10-22 | Samsung Electronics Co., Ltd. | Input device for foldable display device and input method thereof |
| CN103353304A (zh) * | 2013-06-25 | 2013-10-16 | 深圳市宇恒互动科技开发有限公司 | 一种对三维惯性运动感测进行补偿的方法及装置 |
| CN103455171A (zh) * | 2013-09-09 | 2013-12-18 | 吉林大学 | 一种三维交互电子白板系统及方法 |
| CN103501537A (zh) * | 2013-09-24 | 2014-01-08 | 北京大学 | 基于智能手机和Wi-Fi无线网络的楼宇内部定位方法和系统 |
| CN104385808A (zh) * | 2014-10-27 | 2015-03-04 | 邓乐安 | 基于惯性测量的原笔迹记录智能笔 |
| CN105572638A (zh) * | 2016-02-06 | 2016-05-11 | 向杰 | 一种基于惯性姿态与超声波测距的三维定位方法及装置 |
Non-Patent Citations (1)
| Title |
|---|
| 超声波惯性传感器定位误差校准方法;刘瑜等;《安庆师范大学学报》;20170331;1-4页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107102749A (zh) | 2017-08-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107102749B (zh) | 一种基于超声波和惯性传感器的三维笔式定位方法 | |
| US10191559B2 (en) | Computer interface for manipulated objects with an absolute pose detection component | |
| EP3721284B1 (en) | Selective tracking of a head-mounted display | |
| US7826641B2 (en) | Apparatus and method for determining an absolute pose of a manipulated object in a real three-dimensional environment with invariant features | |
| US9229540B2 (en) | Deriving input from six degrees of freedom interfaces | |
| Meyer et al. | A survey of position trackers | |
| JP4278979B2 (ja) | ジェスチャーに基づいた入力及びターゲット指示のための単一カメラシステム | |
| US9201513B2 (en) | Method of controlling a cursor by measurements of the attitude of a pointer and pointer implementing said method | |
| CN106933385B (zh) | 一种基于三维超声波定位的低功耗空鼠笔的实现方法 | |
| CN106933390A (zh) | 惯性技术和超声波相结合的触控笔位姿检测方法及系统 | |
| KR102130892B1 (ko) | 착용형 디스플레이 장치 및 이를 이용한 이미지 제공 방법 | |
| CN101911162A (zh) | 用于扫描光束显示器的输入装置 | |
| EP2304527A1 (en) | Gesture-based control system for vehicle interfaces | |
| US20140194742A1 (en) | Ultrasound imaging system and method | |
| US20220155880A1 (en) | Interacting with a smart device using a pointing controller | |
| CN101226437A (zh) | 可用于空间遥控鼠标及动作捕捉的三维传感系统 | |
| US10949579B2 (en) | Method and apparatus for enhanced position and orientation determination | |
| WO2017021902A1 (en) | System and method for gesture based measurement of virtual reality space | |
| TW201101234A (en) | Three-dimensional space motion reconstruction method and apparatus constructed thereby | |
| US20230349693A1 (en) | System and method for generating input data from pose estimates of a manipulated object by using light data and relative motion data | |
| TW200415368A (en) | Method and system for obtaining positioning data | |
| US11475177B2 (en) | Method and apparatus for improved position and orientation based information display | |
| KR20190060625A (ko) | 관성 측정 유닛 센서의 위치 보정 장치 및 그 보정 방법 | |
| CN103034345A (zh) | 一种真实空间中的地理虚拟仿真三维鼠标笔 | |
| Reunanen et al. | Näprä-Affordable Fingertip Tracking with Ultrasound. |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |