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

权利要求书

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)继续执行。