基於浮點型dsp的運動載體姿態傾角測量裝置的製作方法
2023-05-29 16:36:06
專利名稱：基於浮點型dsp的運動載體姿態傾角測量裝置的製作方法
技術領域：
本實用新型涉及載體姿態傾角測量技術，尤其涉及一種用於對載體運動過程中動態姿態傾角的測量裝置的技術。
背景技術：
姿態傾角測量技術運用在許多領域中，如機器人、智能小車、航模控制、遊戲設備、個人導航設備、圖像穩定系統等；靜態的姿態傾角地測量較為容易且測量效果較為滿意，但動態的姿態傾角地測量，尤其是被測設備動態效果較為強烈的情況下的姿態傾角測量，其測量技術和測量效果則顯得差強人意。目前，對運動載體的姿態傾角測量裝置主要使用高精度陀螺儀包括液浮陀螺儀、光纖陀螺儀或雷射陀螺儀等，這些陀螺儀的精度很高，但結構複雜，使用壽命短，成本高，一般僅限於導航方面，而難以在一般的運動控制系統中應用。MEMS (微機電系統)在融合多種微細加工技術，並應用現代信息技術的最新成果的基礎上發展起來的高科技前沿學科，目前MEMS市場的主導產品為壓力傳感器、加速度計、微陀螺儀等。滿意的測量技術尤其是動態測量技術限定了傳感器要滿足實時性好、精度高、易於集成、成本功耗低、體積小等特點。近幾年，基於MEMS技術的加速度計或陀螺儀被用來測量載體的姿態傾角，具有成本低、體積小、集成高、精度較高，但單一使用某種類型的傳感器測量載體在運動情況下的動態姿態傾角時便會呈現很大的不足，主要原因如下使用單一加速度計(一個或多個加速度計傳感器)測量載體動態姿態傾角，其原理是通過測量載體的線性加速度，利用數學關係推出載體姿態角度值。加速度計是通過重力加速度在各軸上的分量值來推出載體傾角值，測量的角度值長期較穩定可靠，但是加速度計動態響應慢，敏捷性不夠，且由於傳感器噪聲會隨帶寬的增大而增加，在跟蹤動態效果較強烈的動態姿態傾角時得到的數據將會大大增加出錯的機會，其測量的範圍將會得到很大的限制，這使得使用單一加速度計檢測運動載體的動態姿態傾角並不合適；使用陀螺儀(一個或多個陀螺儀傳感器)測量運動載體的角速度，通過不斷積分得到載體姿態傾角。陀螺儀在跟蹤動態環境下的載體姿態角，其敏捷性很高、動態響應較快，但自身的先天性機械噪聲無可避免地會產生測量誤差，並且隨著時間的推移，累計誤差會越來越大，最終造成測量偏差。單獨使用陀螺儀傳感器測量運動載體的動態傾角，其結果也很難讓人滿意。另外從處理器硬體平臺選擇上考慮，一般的單片機或定點型處理器，在大量浮點運算和正餘弦運算時程序執行效率低下，對複雜算法執行周期較長，不能滿足對強烈動態環境下運動載體姿態角的實時跟蹤，同時動態表示的精確度和範圍也較低。
實用新型內容本實用新型的目的在於提供一種對動態姿態傾角跟蹤效果好、實時性高、精確度高、表示範圍大、數據處理效率高的基於浮點型DSP的運動載體姿態傾角測量方法及其裝置。實現本實用新型目的的技術解決方案為一種基於浮點型DSP的運動載體姿態傾角測量裝置，包括慣性測量單元MU、電源電路、浮點型DSP電路、SCI轉USB通信電路、上位機，電源電路分別與IMU模塊、浮點型DSP電路、SCI轉USB通信電路進行連接，IMU模塊通過IIC總線與浮點型DSP電路進行連接，浮點型DSP電路通過SCI接口與SCI轉USB電路進行連接，SCI轉USB通信電路與上位機進行連接；慣性測量單元MU包括陀螺儀及其處理電路和加速度計及其處理電路電路，陀螺儀及其處理電路包括依次連接的陀螺儀、模數轉換電路ADC、數字濾波電路、帶FIFO的IIC接口電路，加速度計及其處理電路包括依次連接的加速度計、模數轉換電路ADC、數字濾波電路、帶FIFO的IIC接口電路。 本實用新型與現有技術相比，其顯著優點I、本實用新型是不僅可以對載體靜態姿態角進行準確測量，也可以對運動強烈環境下的載體動態姿態角進行精確且跟蹤效果好的動態測量；2、本實用新型採用陀螺儀和加速度計結合的方式檢測運動載體的姿態角，將兩種不同類型的傳感器進行性能上互補，充分利用DSP強大的數據處理優勢，通過卡爾曼濾波消除單一傳感器的隨機噪聲和漂移誤差，有效地避免了錯誤數據的輸出，獲得動態跟蹤效果好、實時性高且更接近實際真值的姿態角；3、本實用新型使用的數據信號處理器為採用32位浮點型DSP，在對大量的浮點運算和正餘弦運算時具有很大的優勢，對複雜算法程序執行效率高，得到的動態姿態傾角精度高；從節約成本、產品開發、數據處理效率和精確度等方面考慮，開發人員可在此基礎上進行多功能擴展和二次開發，如用於高精度伺服控制上；4、本實用新型的上位機軟體採用NI (美國國家儀器)的LabVIEW軟體進行編寫，底層驅動使用VISA (虛擬儀器軟體架構)進行編寫，該軟體界面友好，可移植性高，以虛擬示波器非常直觀地將數據實時、動態地進行顯示，並具有波形數據本地保存和回放的功能，在實際使用時非常直觀、方便，代替了體積笨重、攜帶不便且成本昂貴的傳統示波器。
圖I為本實用新型一種基於浮點型DSP的運動載體姿態傾角測量裝置的總體結構框圖。圖2為本實用新型實施例一種基於浮點型DSP的運動載體姿態傾角測量裝置的SCI轉USB電路原理圖。圖3為本實用新型實施例一種基於浮點型DSP的運動載體姿態傾角測量裝置的IMU模塊的原理圖，即陀螺儀和加速度計的外圍電路原理圖。圖4為本實用新型實施例一種基於浮點型DSP的運動載體姿態傾角測量裝置的電源電路原理圖。圖5為本實用新型實施例一種基於浮點型DSP的運動載體姿態傾角測量裝置的DSP電路原理圖。圖6為本實用新型實施例一種基於浮點型DSP的運動載體姿態傾角測量裝置的DSP構成最小系統所需的振蕩器、復位、JTAG仿真口、外擴RAM電路原理圖。圖7為本實用新型實施例一種基於浮點型DSP的運動載體姿態傾角測量裝置的上位機軟體層疊顯示波形圖。圖8為本實用新型實施例一種基於浮點型D SP的運動載體姿態傾角測量裝置的上位機軟體測試分立顯示波形圖。圖9為本實用新型實施例一種基於浮點型DSP的運動載體姿態傾角測量裝置的軟體流程圖。
具體實施方式
本實用新型一種基於浮點型DSP的運動載體姿態傾角測量裝置，包括慣性測量單元IMU、電源電路201、浮點型DSP電路301、SCI轉USB通信電路401、上位機501，電源電路201分別與IMU模塊、浮點型DSP電路301、SCI轉USB通信電路401進行連接，IMU模塊通過IIC總線與浮點型DSP電路301進行連接，浮點型DSP電路301通過SCI接口與SCI轉USB電路401進行連接，SCI轉USB通信電路401與上位機501進行連接；其中，慣性測量單元IMU包括陀螺儀及其處理電路101和加速度計及其處理電路102電路，陀螺儀及其處理電路101包括依次連接的陀螺儀、模數轉換電路ADC、數字濾波電路、帶FIFO的IIC接口電路，加速度計及其處理電路102包括依次連接的加速度計、模數轉換電路ADC、數字濾波電路、帶FIFO的IIC接口電路。所述的浮點型DSP電路301，其內部自帶一個單精度浮點單元，支持硬體浮點運
笪
ο所述的上位機501中的虛擬示波器是使用虛擬儀器軟體和虛擬儀器軟體架構編寫的以虛擬示波器方式顯示波形的上位機軟體。
以下結合附圖說明對本實用新型的實施作詳細描述。結合圖I所示，本實用新型所提供的一種基於浮點型DSP的運動載體姿態傾角測量裝置包括陀螺儀及其信號處理電路101、加速度計及其信號處理電路102，兩者構成慣性測量單元(頂U);浮點型DSP電路301與MU模塊通過SPI接口或IIC接口電路連接進行數據通信；數據處理模塊302在DSP內部，其負責將SPI總線或IIC總線採集的信息進行讀取和計算、卡爾曼濾波、數據格式轉換，然後通過DSP的SCI發送出去；SCI轉USB通信電路401連接的是DSP電路301和上位機501 ；電源電路201電路連接到陀螺儀電路101、加速度計電路102、DSP電路301、SCI轉USB通信電路401 ;上位機501上使用LabVIEW和VISA編寫用戶軟體，實現波形的實時動態記錄、保存和顯示。本實用新型所提供一種基於浮點型DSP的運動載體姿態傾角測量裝置，既可以測量靜態環境下的姿態角，也可以用來準確測量和跟蹤動態環境下的姿態角。圖2為本測量裝置的SCI轉USB電路原理圖；圖3為本測量裝置的MU模塊的原理圖，即陀螺儀和加速度計的外圍電路原理圖；圖4為本測量裝置的電源電路原理圖；圖5為本測量裝置的DSP電路原理圖；圖6為本測量裝置的DSP構成最小系統所需的振蕩器、復位、JTAG仿真口、外擴RAM電路原理圖。本實用新型實施例一種基於浮點型DSP的運動載體姿態傾角測量裝置，包括陀螺儀及其處理電路101、加速度計及其處理電路102、電源電路201、浮點型DSP電路301、數據處理模塊302、通信電路401和上位機501，進一步說明I)採用陀螺儀和加速度計兩種傳感器構成慣性測量單元(MU)固定於被測運動載體上，用來實時測量運動載體的姿態角度和角速度信息；陀螺儀晶片周圍需要外加特定數值的電容、電阻以配置二階低通濾波電路，用來同步驅動和感應接口，如圖3所示，取Cl=O. 47uF，C2=0. OluF, R3=10K ;陀螺儀集成晶片型號為L3G4200D，加速度計集成晶片型號
為ADXL345，佔為模式選擇端，高
電平時為IIC通信模式，低電平時為SPI通信模式，圖3使用IIC模式進行通信；2)採用由TI (德州儀器)公司生產的型號為TMS320F28335的32位浮點型DSP用作本裝置的主晶片，DSP在對大量的浮點運算和正餘弦運算時具有很大的優勢，對複雜算法程序執行效率高，得到的動態姿態傾角精度高。從節約成本、產品開發、數據處理效率 和精確度等方面考慮，在實施中可採用核心板加底板構成裝置測量板將主晶片DSP的最小系統做成核心板，其他的各子模塊電路以底板形式製作，這樣既可以節約成本又方便日後的維修、多功能擴展和二次開發；DSP通過IIC總線與IMU模塊進行IIC連接通信，結合圖9所示，具體流程有1、DSP對SCI、IIC、定時器初始化、中斷初始化，DSP對加速計和陀螺儀進行校正；2、DSP每2ms進行一次中斷，浮點型DSP內部進行實時數據處理，包括DSP
對MU進行校正和讀寫、DSP使用公式―肌加rr^·.*EuIarlDsg得到加速
'丨(oc z) +(U)-
度計測量的 X 軸角度值、DSP 使用公SCgfrojratw—x-i—asis—init—up) *L3G4200D SCALE
得到陀螺儀測量的X軸角速度；3、DSP採用卡爾曼濾波將兩種信號進行融合，取初始化值
0003O
Q =
為L0 a.0013], Ts=O. 02s ; 4、將得到的浮點型數據轉成四字節整型數據；5、制 Jt=[rm] = Qm
定通信協議，其格式為A0XXXXB0A1XXXXB1A2XXXXB2，其中XXXX表示四字節型角度值，總共有三組有效數據分別為陀螺儀測量的動態姿態角度值、加速度計測量的動態姿態角度值、DSP進行數據處理、計算和濾波融合後的動態姿態角度；6、DSP控制SCI，將上述格式的18個字節角度信息通過SCI轉USB電路發送給上位機；3)上位機使用NI (美國國家儀器)公司生產的LabVIEW8. 2版本的軟體進行編寫，底層驅動使用VISA (虛擬儀器軟體架構)和下位機進行數據通信。使用簡單，僅需一根USB數據線連接，上位機收到18個字節數據進行數據格提取、校驗，對不符合格式的數據進行踢除，再進行整型數據到浮點型數據格式的轉換，得到三個浮點型角度值分別為陀螺儀測量的角度、加速度計測量的角度、基於DSP數據處理、濾波融合和轉換的角度，並用虛擬示波器進行動態實時顯示、記錄，還可以進行波形的保存和回放，省去了體積笨重、攜帶不便且成本昂貴的傳統示波器。圖7為本實用新型實施例一種基於浮點型DSP的運動載體姿態傾角測量裝置的上位機軟體層疊顯示波形圖。三種曲線分別是陀螺儀測量的角度、加速度計測量的角度、基於DSP數據處理、濾波融合和轉換之後的角度，並可以實時顯示、保存具體的角度值，也可以保存曲線和進行波形回放。[0037]圖8為本實用新型實施例一種基於浮點型DSP的運動載體姿態傾角測量裝置的上位機軟體測試分立顯示波形圖。從圖7、圖8上可以看出加速度計採集的波形較為平滑，但動態響應不夠，在跟蹤很陡角度信號時誤差較大；陀螺儀動態效果較好，但因先天性噪聲幹擾，隨著時間的推移，經過積分後得到的角度值不準確，時常有一些跳變。本裝置利用兩種不同類型的傳感器進行優勢上實質性互補，消除了單一傳感器的隨機噪聲和漂移誤差，有效地避免了錯誤數據的輸出，尤其是跟蹤動態姿態角上的效果較單一傳感器更敏捷、精確。本實用新型採用加速度計和陀螺儀結合的方式檢測運動載體的姿態角，利用陀螺儀短時測量準確且反應敏捷的優勢和加速度計長期穩定的特點，使用浮點型DSP作為核心處理器，充分發掘其對複雜算法的處理能力較強、實時性和精確度高的優勢，對這兩種類型的傳感器進行實時數據採集、分析和處理，加入卡爾曼濾波實現兩者信號進行融合，消除單一傳感器的隨機噪聲和漂移誤差，有效地避免了錯誤數據的輸出，得到動態跟蹤效果好、實時性高且更接近實際真值的姿態角，為高性能控制系統提高控制精度提供了強有力的數據支撐。上位機使用LabVIEW軟體和VISA進行編寫，示波器以虛擬儀器的方式顯示出來，將 傳統體積笨重、攜帶不便的示波器裝進普通的PC機，節省了昂貴的測量調試成本。
權利要求1.一種基於浮點型DSP的運動載體姿態傾角測量裝置，包括慣性測量單元IMU、電源電路(201)、浮點型DSP電路(301 )、SCI轉USB通信電路(401)、上位機(501)，電源電路(201)分別與IMU模塊、浮點型DSP電路(301)、SCI轉USB通信電路(401)進行連接，IMU模塊通過IIC總線與浮點型DSP電路(301)進行連接，浮點型DSP電路(301)通過SCI接口與SCI轉USB電路(401)進行連接，SCI轉USB通信電路(401)與上位機(501)進行連接；其特徵在於慣性測量單元MU包括陀螺儀及其處理電路(101)和加速度計及其處理電路(102)電路，陀螺儀及其處理電路(101)包括依次連接的陀螺儀、模數轉換電路ADC、數字濾波電路、帶FIFO的IIC接口電路，加速度計及其處理電路(102)包括依次連接的加速度計、模數轉換電路ADC、數字濾波電路、帶FIFO的IIC接口電路。
2.根據權利要求I所述的一種基於浮點型DSP的運動載體姿態傾角測量裝置，其特徵在於所述的浮點型DSP電路(301)，其內部自帶一個單精度浮點單元，支持硬體浮點運算。
專利摘要本實用新型公開了一種基於浮點型DSP的運動載體姿態傾角測量裝置，該裝置包括慣性測量單元IMU、電源電路、浮點型DSP電路、SCI轉USB通信電路、上位機，其中，慣性測量單元IMU包括陀螺儀及其處理電路和加速度計及其處理電路電路，陀螺儀及其處理電路包括依次連接的陀螺儀、模數轉換電路ADC、數字濾波電路、帶FIFO的IIC接口電路，加速度計及其處理電路包括依次連接的加速度計、模數轉換電路ADC、數字濾波電路、帶FIFO的IIC接口電路；本實用新型對動態姿態傾角跟蹤效果好、實時性高、精確度高、表示範圍大、數據處理效率高。
文檔編號G01C1/00GK202710030SQ20122012537
公開日2013年1月30日 申請日期2012年3月29日 優先權日2012年3月29日
發明者高建龍, 潘曉偉, 單梁, 戴躍偉, 李穎, 韓成祥, 楊陽, 李曦 申請人:南京理工大學