空間軌跡實時追蹤裝置及方法
【專利摘要】本發明提供一種空間軌跡實時追蹤裝置及方法，涉及三維空間軌跡追蹤【技術領域】。該裝置包括：傳感器、微控制器、通用異步收發控制器，傳感器包括三軸加速度傳感器、三軸陀螺儀和三軸電子羅盤，傳感器通過IIC接口將傳感數據發送給微控制器；微控制器，用于接收所述傳感器發送的傳感數據并進行處理，并向通過IIC總線向傳感器發送控制命令；同時將處理過后的傳感數據通過UART外設發送給通用異步收發控制器；通用異步收發控制器，將微控制器發送的處理過后的傳感數據進行轉換，并通過USB接口發送給上位機；同時通用異步收發控制器對上位機發送的命令進行轉換后發送給所述微控制器。本發明成本低廉；具備高穩定性；具備高精確度；且裝置尺寸小，攜帶方便。
【專利說明】空間軌跡實時追蹤裝置及方法
【技術領域】
[0001]本發明涉及三維空間軌跡追蹤【技術領域】，具體涉及一種空間軌跡實時追蹤裝置及方法。
【背景技術】
[0002]隨著社會的經濟發展，空間軌跡實時追蹤技術能夠應用的領域越來越廣。如各大型購物中心遍布在城市的各個商業區，在大型購物中心購物的時候，消費者會發現琳瑯滿目的商鋪讓人眼花繚亂，如對地下施工的工作人員和對地下事故進行救援的救援人員來說，非常需要一個裝置能夠幫助他們在進行定位和導航；再如說有些礦井和油田的裝置需要跟蹤裝置的移動距離，使用標尺來測量不能迅速的判定裝置的工作狀態和性能指標；還有大學做的各種物理實驗，需要跟蹤物體的運動軌跡，從而需要準確快捷地得到相應的實驗結果。
[0003]現行所采用的方法是通過移動基站和GPS信號的方式來進行空間軌跡實時追蹤的。
[0004]然而，在某些特殊的情況下，沒有移動基站或者GPS信號時，現有的方法就很難對空間軌跡實時追蹤，故而急需一種能夠對三維空間中的軌跡進行實時追蹤的新的裝置。
【發明內容】
[0005](一)解決的技術問題
[0006]針對現有技術的不足，本發明提供了一種空間軌跡實時測量裝置及方法，能夠解決在沒有移動基站或者GPS信號時對三維空間的軌跡實時追蹤問題。
[0007](二)技術方案
[0008]為實現以上目的，本發明通過以下技術方案予以實現:
[0009]一種空間軌跡實時追蹤裝置，其特征在于，該裝置包括:傳感器、微控制器、通用異步收發控制器，
[0010]所述傳感器包括三軸加速度傳感器、三軸陀螺儀和三軸電子羅盤，所述三軸加速度傳感器用于采集三個互相垂直軸向上的加速度，所述三軸陀螺儀用于采集三個互相垂直軸向上的角速度，所述三軸電子羅盤用于采集三個互相垂直軸向上的姿態角，所述傳感器通過Iic接口將傳感數據發送給微控制器；
[0011]所述微控制器，用于接收所述傳感器發送的傳感數據并進行處理，同時通過IIC總線向所述傳感器發送控制命令；然后將處理過后的傳感數據通過UART外設發送給通用異步收發控制器；
[0012]所述通用異步收發控制器，將所述微控制器發送的處理過后的傳感數據進行轉換，并通過USB接口發送給上位機；同時把上位機發送的命令進行轉換后發送給所述的微控制器。
[0013]優選的，所述的微控制器包括精簡指令處理器核心、只讀存儲器、隨機存儲器、IIC接口、定時器、中斷源和USART等外設；
[0014]所述只讀存儲器與所述精簡指令處理器核心通過指令總線相連，用于存儲該裝置的指令信息；
[0015]所述隨機存儲器與所述精簡指令處理器核心通過數據總線相連，用于存儲該裝置的數據信息；
[0016]所述精簡處理器核心通過APB總線與IIC接口、定時器、中斷源和USART等裝置連接；IIC接口與所述傳感器相連，用于對所述傳感器與所述微控制器的數據和命令的交互；
[0017]所述定時器，用于所述微控制器對所述傳感器施行采樣周期時間間隔的采樣；
[0018]所述中斷源，用于在定時器產生定時器中斷的情況下，所述中斷源將所述精簡指令處理器核心的程序指針指向采樣程序的第一條指令，使得在采樣周期到來的時候所述微控制器對所述傳感器進行采樣。
[0019]優選的，該裝置進一步包括:通用下載接口模塊，用于將所述微控制器的連接到仿真器，通過仿真器對所述微控制器的程序進行下載和調試。
[0020]優選的，該裝置進一步包括:電源模塊，所述電源模塊包括2個LD0，一個LDO負責對所述微控制器供電，另一個LDO負責對所述傳感器供電。
`[0021]優選的,該裝置進一步包括:LED指示燈,所述LED指示燈通過IO接口與所述微控制器相連，所述微控制器通過控制LED指示燈的亮滅來指示裝置運行狀態。
[0022]本發明還提供了一種空間軌跡實時追蹤方法，該方法包括步驟:
[0023]S1、裝置初始化，微控制器開始定時，預訂延時時間后，微控制器接收三軸加速度傳感器、三軸陀螺儀和三軸電子羅盤將采集到的傳感器數據；
[0024]S2、所述微控制器對所述傳感器數據進行處理，得到裝置的初始姿態角，并依據所述姿態角確定初始旋轉四元數(q°, q1，q2，q3);
[0025]S3、依據所述三軸陀螺儀傳輸的角速度數據更新所述初始旋轉四元數，得到旋轉矩陣，并通過所述旋轉矩陣將所述三軸加速度傳感器傳輸的三軸加速度數據分解到導航坐標系;
[0026]S4、對所述導航坐標系下的三軸加速度分別進行一次積分，得到導航坐標系下的三軸速度；
[0027]S5、對所述導航坐標系下的三軸速度進行一次積分，得到導航坐標系下的三軸位移；
[0028]S6、合成所述導航坐標系下的三軸位移，得到對物體進行實時追蹤的空間軌跡。
[0029]優選的，步驟S2中的初始姿態角包括:初始俯仰角Pitch、初始橫滾角Roll和初始航向角Yaw ；
[0030]其中，初始俯仰角Pitch的計算表達式為:
[0031]Pitch=asin (AccY/(AccX*AccX+AccY*AccY+AccZ*AccZ)2)；
[0032]其中，初始橫滾角Roll的計算表達式為:
[0033]Roll=asin (AccX/(AccX*AccX+AccY*AccY+AccZ*AccZ)2)；
[0034]其中，初始航向角Yaw的計算表達式為:
[0035]Yaw = arctan ( (HyCosRoll^HzSinRoll1) / (HxCosPitch^HySinPitch1SinRoll1-HzSinpitch1CosRoll1))；[0036]式中，ACCX、ACCY和AccZ為三軸加速度傳感器傳輸的x、y和ζ軸傳感數據；Hx、Hy、Hz為三軸電子羅盤傳輸的X、y和ζ軸傳感數據。
[0037]優選的，步驟S3中更新后的所述旋轉四元數為:
【權利要求】
1.一種空間軌跡實時追蹤裝置，其特征在于，該裝置包括:傳感器、微控制器、通用異步收發控制器， 所述傳感器包括三軸加速度傳感器、三軸陀螺儀和三軸電子羅盤，所述三軸加速度傳感器用于采集三個互相垂直軸向上的加速度，所述三軸陀螺儀用于采集三個互相垂直軸向上的角速度，所述三軸電子羅盤用于采集三個互相垂直軸向上的姿態角，所述傳感器通過IIC接口將傳感數據發送給微控制器； 所述微控制器，用于接收所述傳感器發送的傳感數據并進行處理，同時通過Iic總線向所述傳感器發送控制命令；然后將處理過后的傳感數據通過UART外設發送給通用異步收發控制器； 所述通用異步收發控制器，將所述微控制器發送的處理過后的傳感數據進行轉換，并通過USB接口發送給上位機；同時把上位機發送的命令進行轉換后發送給所述的微控制器。
2.如權利要求1所述的空間軌跡實時追蹤裝置，其特征在于，所述的微控制器包括精簡指令處理器核心、只讀存儲器、隨機存儲器、IIC接口、定時器、中斷源和USART等外設； 所述只讀存儲器與所述精簡指令處理器核心通過指令總線相連，用于存儲該裝置的指令信息； 所述隨機存儲器與所述精簡指令處理器核心通過數據總線相連，用于存儲該裝置的數據信息； 所述精簡處理器核心通過APB總線與IIC接口、定時器、中斷源和USART等裝置連接；IIC接口與所述傳感器相連，用于對所述傳感器與所述微控制器的數據和命令的交互； 所述定時器，用于所述微控制器對所述傳感器施行采樣周期時間間隔的采樣； 所述中斷源，用于在定時器產生定時器中斷的情況下，所述中斷源將所述精簡指令處理器核心的程序指針指向采樣程序的第一條指令，使得在采樣周期到來的時候所述微控制器對所述傳感器進行采樣。
3.如權利要求1所述的空間軌跡實時追蹤裝置，其特征在于，該裝置進一步包括:通用下載接口模塊，用于將所述微控制器的連接到仿真器，通過仿真器對所述微控制器的程序進行下載和調試。
4.如權利要求1所述的空間軌跡實時追蹤裝置，其特征在于，該裝置進一步包括:電源模塊，所述電源模塊包括2個LDO, —個LDO負責對所述微控制器供電,另一個LDO負責對所述傳感器供電。
5.如權利要求1所述的空間軌跡實時追蹤裝置，其特征在于，該裝置進一步包括:LED指示燈，所述LED指示燈通過IO接口與所述微控制器相連，所述微控制器通過控制LED指示燈的亮滅來指示裝置運行狀態。
6.一種空間軌跡實時追蹤方法，其特征在于，該方法包括步驟: S1、裝置初始化，微控制器開始定時，預訂延時時間后，微控制器接收三軸加速度傳感器、三軸陀螺儀和三軸 電子羅盤將采集到的傳感器數據； S2、所述微控制器對所述傳感器數據進行處理，得到裝置的初始姿態角，并依據所述姿態角確定初始旋轉四元數(q°, q1，q2，q3); S3、依據所述三軸陀螺儀傳輸的角速度數據更新所述初始旋轉四元數，得到旋轉矩陣，并通過所述旋轉矩陣將所述三軸加速度傳感器傳輸的三軸加速度數據分解到導航坐標系; S4、對所述導航坐標系下的三軸加速度分別進行一次積分，得到導航坐標系下的三軸速度； S5、對所述導航坐標系下的三軸速度進行一次積分，得到導航坐標系下的三軸位移； S6、合成所述導航坐標系下的三軸位移，得到對物體進行實時追蹤的空間軌跡。
7.如權利要求6所述的空間軌跡實時追蹤方法，其特征在于，步驟S2中的初始姿態角包括:初始俯仰角Pitch、初始橫滾角Roll和初始航向角Yaw ； 其中，初始俯仰角Pitch的計算表達式為:
Pitch=asin(AccY/(AccX*AccX+AccY*AccY+AccZ*AccZ)2)； 其中，初始橫滾角Roll的計算表達式為:
Roll=asin(AccX/(AccX*AccX+AccY*AccY+AccZ*AccZ)2)； 其中，初始航向角Yaw的計算表達式為: Yaw = arctan ( (HyCosRoll^HzSinRoll1) / (HxCOSPitch^HySinPitch1SinRoll1-HzSinpitch1CosRoll1))； 式中，AccX、AccY和AccZ 為三軸加速度傳感器傳輸的x、y和z軸傳感數據；HX、Hy、Hz為三軸電子羅盤傳輸的X、y和z軸傳感數據。
8.如權利要求6所述的空間軌跡實時追蹤方法，其特征在于，步驟S3中更新后的所述旋轉四元數為:
9.如權利要求6所述的空間軌跡實時追蹤方法，其特征在于，步驟S4中，對所述導航坐標系下的三軸加速度分別進行一次積分的表達
10.如權利要求6所述的空間軌跡實時追蹤方法，其特征在于，步驟S6中，合成所述導航坐標系下的三軸位移的表示式為:
S — Sxi+Syj+Szk 式中，Sx, Sy和Sz為物體在空間坐標系的三個軸向上的移動矢量，1、j和k為空間坐標系三個軸向上的單位向量。
【文檔編號】G01C21/16GK103884338SQ201410060655
【公開日】2014年6月25日 申請日期:2014年2月21日 優先權日:2014年2月21日
【發明者】王劍秦, 倪廣元, 孫龍清, 郭少鑫, 段青玲 申請人:中國農業大學, 北京中恒永信科技有限公司