[發(fā)明專利]一種基于超聲波和慣性傳感器的三維筆式定位方法

有效

• 申請?zhí)枺?/td>: 201710270094.8
• 申請日: 2017-04-23
• 公開（公告）號: CN107102749B
• 公開（公告）日: 2019-11-08
• 發(fā)明（設(shè)計）人: 陳建;楊鵬;孫曉穎;燕學(xué)智;孫銘會;林琳;溫泉;康凱;于嘉鑫
• 申請（專利權(quán)）人: 吉林大學(xué)
• 主分類號: G06F3/0346
• 分類號: G06F3/0346
• 代理公司: 吉林長春新紀(jì)元專利代理有限責(zé)任公司 22100
• 代理人: 魏征驥

1.一種基于超聲波和慣性傳感器的三維筆式定位方法，其特征在于，包括下列步驟：

(1)觸發(fā)電子筆按鍵，電子筆周期性發(fā)射紅外和超聲波信號；

(2)接收器判斷是否接收到紅外，若收到紅外信號，跳轉(zhuǎn)到步驟(3)，否則跳轉(zhuǎn)到步驟(6)；

(3)通過三個接收器分別接收超聲波信號，以紅外信號作為超聲波傳播時間測量的基準(zhǔn)提取超聲波的傳播時間，根據(jù)控制器提供的環(huán)境溫度參數(shù)計算超聲波傳播的速度，用傳播時間乘以傳播速度得到電子筆到各個接收器的傳播距離l₁、l₂、l₃，并送給主控制器；

(4)控制器判斷是否完整收到三路超聲波距離，若完整收到三路超聲距離，跳轉(zhuǎn)到(5)，否則跳轉(zhuǎn)到(6)；

(5)控制器三路超聲波傳播距離l₁、l₂、l₃距離后，利用幾何定位，計算出超聲波定位的三維坐標(biāo)，第1個超聲波定位周期的三維定位坐標(biāo)表示為P₁(x₁,y₁,z₁)，若第n個定位周期，超聲波的三維定位坐標(biāo)表示為P_n(x_n,y_n,z_n)，下標(biāo)n表示第n個超聲波定位周期，每連續(xù)成功完成一次超聲波定位后，對n用n+1更新，并通過無線通信模塊讀取電子筆的方向角θ_n、俯仰角Φ_n和翻滾角γ_n，并保存為Q_n(θ_n,Φ_n,γ_n)，跳轉(zhuǎn)到步驟(2)繼續(xù)執(zhí)行；

(6)判斷超聲波聯(lián)系成功定位周期數(shù)n是否大于2，若大于2則跳轉(zhuǎn)到步驟(7)，否則跳轉(zhuǎn)到步驟(2)；

(7)對n用n+1更新，通過無線通信模塊讀取電子筆姿態(tài)信息：四元數(shù)q₀、q₁、q₂、q₃和加速度信息：ax_bn、ay_bn、az_bn，計算慣性傳感器的三維定位坐標(biāo)I_n(x_in,y_in,z_in)，具體方法為：

讀取的姿態(tài)信息為九軸慣性傳感器輸出的姿態(tài)信息—四元數(shù)q₀、q₁、q₂、q₃和加速度信息A_bn(ax_bn、ay_bn、az_bn)，構(gòu)造慣性傳感器坐標(biāo)系—超聲波定位坐標(biāo)系的四元數(shù)方向變換矩陣，將慣性傳感器定位坐標(biāo)系的加速度ax_bn、ay_bn、az_bn變換到超聲波定位坐標(biāo)系的加速度ax_on、ay_on、az_on，根據(jù)電子筆在第n-1周期的姿態(tài)角Q_n-1(θ_n-1,Φ_n-1,γ_n-1)和超聲波三維定位坐標(biāo)P_n-1(x_n-1,y_n-1,z_n-1)計算出I_n-1(x_in-1,y_in-1,z_in-1)，根據(jù)電子筆在第n-2周期的姿態(tài)角Q_n-2(θ_n-2,Φ_n-2,γ_n-2)和超聲三維定位坐標(biāo)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分別進(jìn)行積分并消除積分過程中產(chǎn)生的一次趨勢項，得到慣性傳感器的速度V_n，以I_n-1(x_in-1,y_in-1,z_in-1)為初始坐標(biāo)，對V_n進(jìn)行積分并消除積分過程中產(chǎn)生的一次趨勢項，得到第n周期的慣性傳感器的三維坐標(biāo)I_n(x_in,y_in,z_in)；

(8)根據(jù)電子筆姿態(tài)和超聲波傳感器與慣性傳感器之間的距離，基于慣性傳感器的三維坐標(biāo)I_n(x_in,y_in,z_in)計算超聲波的三維定位坐標(biāo)P_n(x_n,y_n,z_n)，獲得P_n(x_n,y_n,z_n)后，跳轉(zhuǎn)到步驟(2)繼續(xù)執(zhí)行。