[发明专利]一种基于超声波和惯性传感器的三维笔式定位方法

摘要

本发明涉及一种基于超声波和惯性传感器的三维笔式定位方法，适用于虚拟现实、人机交互、信息化教育、三维多媒体交互控制等应用领域。电子笔集成超声波发射器、红外发射器和九轴惯性传感器，通过三维超声波无线定位技术获得电子笔笔尖的三维坐标，在超声信号受到遮挡时，利用九轴传感器的进行三维定位，并根据姿态信息，计算出电子笔超声波传感器的坐标，解决超声波在遮挡情况下的无法对电子笔定位的问题，并利用超声波三维定位坐标实时校正九轴传感器定位坐标的累积误差，从而实现超声定位与九轴惯性传感器定位的互相弥补，实现三维交互空间的无遮挡轨迹跟踪和连续操控。

主权项

1.一种基于超声波和惯性传感器的三维笔式定位方法，其特征在于，包括下列步骤：(1)触发电子笔按键，电子笔周期性发射红外和超声波信号；(2)接收器判断是否接收到红外，若收到红外信号，跳转到步骤(3)，否则跳转到步骤(6)；(3)通过三个接收器分别接收超声波信号，以红外信号作为超声波传播时间测量的基准提取超声波的传播时间，根据控制器提供的环境温度参数计算超声波传播的速度，用传播时间乘以传播速度得到电子笔到各个接收器的传播距离l₁、l₂、l₃，并送给主控制器；(4)控制器判断是否完整收到三路超声波距离，若完整收到三路超声距离，跳转到(5)，否则跳转到(6)；(5)控制器三路超声波传播距离l₁、l₂、l₃距离后，利用几何定位，计算出超声波定位的三维坐标，第1个超声波定位周期的三维定位坐标表示为P₁(x₁,y₁,z₁)，若第n个定位周期，超声波的三维定位坐标表示为P_n(x_n,y_n,z_n)，下标n表示第n个超声波定位周期，每连续成功完成一次超声波定位后，对n用n+1更新，并通过无线通信模块读取电子笔的方向角θ_n、俯仰角Φ_n和翻滚角γ_n，并保存为Q_n(θ_n,Φ_n,γ_n)，跳转到步骤(2)继续执行；(6)判断超声波联系成功定位周期数n是否大于2，若大于2则跳转到步骤(7)，否则跳转到步骤(2)；(7)对n用n+1更新，通过无线通信模块读取电子笔姿态信息：四元数q₀、q₁、q₂、q₃和加速度信息：ax_bn、ay_bn、az_bn，计算惯性传感器的三维定位坐标I_n(x_in,y_in,z_in)，具体方法为：读取的姿态信息为九轴惯性传感器输出的姿态信息—四元数q₀、q₁、q₂、q₃和加速度信息A_bn(ax_bn、ay_bn、az_bn)，构造惯性传感器坐标系—超声波定位坐标系的四元数方向变换矩阵，将惯性传感器定位坐标系的加速度ax_bn、ay_bn、az_bn变换到超声波定位坐标系的加速度ax_on、ay_on、az_on，根据电子笔在第n‑1周期的姿态角Q_n‑1(θ_n‑1,Φ_n‑1,γ_n‑1)和超声波三维定位坐标P_n‑1(x_n‑1,y_n‑1,z_n‑1)计算出I_n‑1(x_in‑1,y_in‑1,z_in‑1)，根据电子笔在第n‑2周期的姿态角Q_n‑2(θ_n‑2,Φ_n‑2,γ_n‑2)和超声三维定位坐标P_n‑2(x_n‑2,y_n‑2,z_n‑2)计算出I_n‑2(x_in‑2,y_in‑2,z_in‑2)，利用I_n‑1(x_in‑1,y_in‑1,z_in‑1)和I_n‑2(x_in‑2,y_in‑2,z_in‑2)计算惯性传感器在第n周期的初始速度V_n0(vx_n0,vy_n0,vz_n0)，以V_n0(vx_n0,vy_n0,vz_n0)为初始速度，对加速度ax_on、ay_on、az_on分别进行积分并消除积分过程中产生的一次趋势项，得到惯性传感器的速度V_n，以I_n‑1(x_in‑1,y_in‑1,z_in‑1)为初始坐标，对V_n进行积分并消除积分过程中产生的一次趋势项，得到第n周期的惯性传感器的三维坐标I_n(x_in,y_in,z_in)；(8)根据电子笔姿态和超声波传感器与惯性传感器之间的距离，基于惯性传感器的三维坐标I_n(x_in,y_in,z_in)计算超声波的三维定位坐标P_n(x_n,y_n,z_n)，获得P_n(x_n,y_n,z_n)后，跳转到步骤(2)继续执行。