二子樣旋轉(zhuǎn)矢量姿態(tài)算法ip核的制作方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于姿態(tài)測量組件技術(shù)領(lǐng)域，特別是一種二子樣旋轉(zhuǎn)矢量姿態(tài)算法IP核。
【背景技術(shù)】
[0002] 在研制姿態(tài)測量組件原理樣機時，對所采用的姿態(tài)解算算法通常會考慮其在圓錐 運動時產(chǎn)生的誤差。對捷聯(lián)慣導(dǎo)姿態(tài)更新算法來說，圓錐運動是最惡劣的環(huán)境條件，它會誘 發(fā)數(shù)學(xué)平臺的嚴(yán)重漂移，所以在旋轉(zhuǎn)矢量優(yōu)化算法中常以錐運動作為環(huán)境條件。如果能確 保錐運動環(huán)境條件下的算法漂移最小，就一定能確保在其余環(huán)境條件下的算法漂移最小。 引起圓錐誤差的因素很多，對于陀螺帶寬、量化誤差、安裝誤差等引起的錐誤差，可以通過 提高陀螺相應(yīng)性能來克服。對于姿態(tài)更新率過低，理應(yīng)通過提高更新率來解決，但算法的實 時性又制約著更新率的提高，解決這一矛盾的有效途徑是采用等效旋轉(zhuǎn)矢量法實現(xiàn)姿態(tài)更 新。傳統(tǒng)的使用DSP作為處理器實現(xiàn)該算法運算時間接近lms，如處理器為TMS320VC5509A 時，算法運行時間為0. 77ms。這個速度不能滿足對速度要求較高的姿態(tài)測量組件的設(shè)計。
[0003] 目前，二子樣旋轉(zhuǎn)矢量姿態(tài)算法在通用MCU或者DSP中運行周期過長，導(dǎo)致運算時 間不能進(jìn)一步減少，無法實現(xiàn)高速和高精度。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明的目的在于提供一種速度快、精度高的二子樣旋轉(zhuǎn)矢量姿態(tài)算法IP核，為 高速姿態(tài)測量組件的實現(xiàn)提供可行性。
[0005] 實現(xiàn)本發(fā)明目的的技術(shù)解決方案為：一種二子樣旋轉(zhuǎn)矢量姿態(tài)算法IP核，包括 422接口模塊、角增量模塊、等效旋轉(zhuǎn)矢量模塊、變換四元數(shù)模塊、四元數(shù)更新模塊、計算姿 態(tài)角模塊，其中422接口模塊又包括分頻模塊、422接收模塊、幀校驗?zāi)K、FIFO存儲發(fā)送 模塊、姿態(tài)角輸出模塊、運算結(jié)果分解模塊、422輸出模塊；所述分頻模塊分別與422接收模 塊、運算結(jié)果分解模塊、422輸出模塊相連；422接收模塊通過幀校驗?zāi)K與FIFO存儲發(fā)送 模塊相連，F(xiàn)IFO存儲發(fā)送模塊接入角增量模塊；角增量模塊與等效旋轉(zhuǎn)矢量模塊、變換四 元數(shù)模塊、四元數(shù)更新模塊順次連接，四元數(shù)更新模塊分別接入計算姿態(tài)角模塊和角增量 模塊；計算姿態(tài)角模塊依次通過姿態(tài)角輸出模塊、運算結(jié)果分解模塊接入422輸出模塊；
[0006] 所述422接口模塊負(fù)責(zé)從外部讀取422串口數(shù)據(jù)及各個時刻的軸向角速度，并將 經(jīng)過二子樣旋轉(zhuǎn)矢量姿態(tài)算法運算的結(jié)果按422串口標(biāo)準(zhǔn)輸出到外部存儲；
[0007] 所述角增量模塊根據(jù)FIFO存儲發(fā)送模塊輸入的三組數(shù)據(jù)格式為IEEE754單精度 浮點的三個軸向的角速度Thetaji= 1，2, 3)，按照積分公式求得兩組二子樣姿態(tài)角增量 ΘuΘ2作為等效旋轉(zhuǎn)矢量模塊的輸入信號；
[0008] 所述等效旋轉(zhuǎn)矢量模塊將二子樣姿態(tài)角增量ΘΘ2根據(jù)優(yōu)化的二子樣旋轉(zhuǎn)矢量 公式處理得到等效旋轉(zhuǎn)矢量Φ作為變換四元數(shù)模塊的輸入信號；
[0009] 所述變換四元數(shù)模塊將等效旋轉(zhuǎn)矢量Φ按照四元數(shù)變換公式進(jìn)行處理得到四元 數(shù)作為四元數(shù)更新模塊的輸入信號；
[0010] 所述四元數(shù)更新模塊將四元數(shù)按照姿態(tài)更新方程進(jìn)行處理得到更新后的四元數(shù) 作為計算姿態(tài)角模塊的輸入信號；
[0011] 所述計算姿態(tài)角模塊根據(jù)更新后的四元數(shù)構(gòu)建姿態(tài)矩陣，求得姿態(tài)角即俯仰角 1橫滾角^、航向角4輸出至422接口模塊的姿態(tài)角輸出模塊。
[0012] 本發(fā)明與現(xiàn)有技術(shù)相比，其顯著優(yōu)點為：（1)采用等效旋轉(zhuǎn)矢量的二子樣優(yōu)化算 法進(jìn)行算法IP核的設(shè)計，減少了姿態(tài)解算算法在圓錐運動時的漂移；(2)采用FPGA作為 處理器，使用Xilinx提供的算法IP核搭建，全程并行處理，使得實現(xiàn)該算法只需要十幾微 秒，大大提高姿態(tài)測量組件的運行速度，為高速姿態(tài)測量組件的實現(xiàn)提供了可能性；（3)采 用422進(jìn)行串口通信，以差動方式發(fā)送和接收，并且全雙工工作模式使得收發(fā)不影響，使得 它傳輸距離遠(yuǎn)，速度快。
【附圖說明】
[0013]圖1本發(fā)明二子樣旋轉(zhuǎn)矢量姿態(tài)算法IP核的模塊結(jié)構(gòu)示意圖。
[0014]圖2為本發(fā)明二子樣旋轉(zhuǎn)矢量姿態(tài)算法的流程圖。
[0015]圖3為本發(fā)明二子樣旋轉(zhuǎn)矢量姿態(tài)算法IP核中422接收模塊狀態(tài)轉(zhuǎn)換圖。
[0016]圖4為本發(fā)明二子樣旋轉(zhuǎn)矢量姿態(tài)算法IP核中幀校驗?zāi)K狀態(tài)轉(zhuǎn)換圖。
[0017]圖5為本發(fā)明二子樣旋轉(zhuǎn)矢量姿態(tài)算法IP核中運算結(jié)果分解模塊狀態(tài)轉(zhuǎn)換圖。
[0018]圖6為本發(fā)明二子樣旋轉(zhuǎn)矢量姿態(tài)算法IP核中422輸出模塊狀態(tài)轉(zhuǎn)換圖。
[0019]圖7為本發(fā)明二子樣旋轉(zhuǎn)矢量姿態(tài)算法IP核總體模塊電路封裝符號圖。
【具體實施方式】
[0020]結(jié)合圖1，本發(fā)明二子樣旋轉(zhuǎn)矢量姿態(tài)算法IP核，包括422接口模塊、角增量模塊、 等效旋轉(zhuǎn)矢量模塊、變換四元數(shù)模塊、四元數(shù)更新模塊、計算姿態(tài)角模塊，其中422接口模 塊又包括分頻模塊、422接收模塊、幀校驗?zāi)K、FIFO存儲發(fā)送模塊、姿態(tài)角輸出模塊、運算 結(jié)果分解模塊、422輸出模塊；所述分頻模塊分別與422接收模塊、運算結(jié)果分解模塊、422 輸出模塊相連；422接收模塊通過幀校驗?zāi)K與FIFO存儲發(fā)送模塊相連，F(xiàn)IFO存儲發(fā)送模 塊接入角增量模塊；角增量模塊與等效旋轉(zhuǎn)矢量模塊、變換四元數(shù)模塊、四元數(shù)更新模塊順 次連接，四元數(shù)更新模塊分別接入計算姿態(tài)角模塊和角增量模塊；計算姿態(tài)角模塊依次通 過姿態(tài)角輸出模塊、運算結(jié)果分解模塊接入422輸出模塊；所述角增量模塊、等效旋轉(zhuǎn)矢量 模塊、變換四元數(shù)模塊、四元數(shù)更新模塊、計算姿態(tài)角模塊均由Xilinx提供的CORDICIP 核、浮點操作器IP核、加/減法器IP核、乘法器IP核和除法器IP核搭建而成，整個算法的 流程如圖2,各個模塊的功能如下：
[0021] -、422 接 口模塊
[0022] 所述422接口模塊負(fù)責(zé)從外部讀取422串口數(shù)據(jù)及各個時刻的軸向角速度，將其 進(jìn)行和校驗后，并將經(jīng)過二子樣旋轉(zhuǎn)矢量姿態(tài)算法運算的結(jié)果按422串口標(biāo)準(zhǔn)輸出到外部 存儲。
[0023] 所述422接口模塊中的分頻模塊作用是將系統(tǒng)時鐘分為422接收模塊、運算結(jié)果 分解模塊和422發(fā)送模塊所需的時鐘；422接收模塊采用16倍頻采樣接收，設(shè)計采用狀態(tài) 機，分為5個狀態(tài)：等待起始位、求中點，等待采樣，采樣，停止位接收，422接收模塊的狀態(tài) 轉(zhuǎn)換如圖3所示；幀校驗?zāi)K設(shè)計采用狀態(tài)機，分為4個狀態(tài)：接收同步字其中字節(jié)Oxaa、 接收同步字其中字節(jié)0x55、接收保留字節(jié)、接收一組角速度，幀校驗?zāi)K的狀態(tài)轉(zhuǎn)換如圖4 所示；FIFO存儲發(fā)送模塊用于存儲轉(zhuǎn)發(fā)接收的角速度數(shù)據(jù)；運算結(jié)果分解模塊設(shè)計采用狀 態(tài)機，分為8個狀態(tài)：空閑發(fā)送同步字字節(jié)Oxaa、發(fā)送同步字字節(jié)0x55、等待、422使能信號 歸' 〇'、分解航向角、分解橫滾角、分解俯仰角、停止發(fā)送校驗字節(jié)，運算結(jié)果分解模塊狀態(tài) 轉(zhuǎn)換如圖5所示；422輸出模塊設(shè)計采用狀態(tài)機，分為5個狀態(tài)：空閑、發(fā)送起始位、移位等 待、移位、發(fā)送停止位，422輸出模塊的狀態(tài)轉(zhuǎn)移如圖6所示。
[0024] 所述422接口模塊中分頻模塊、422接收模塊、幀校驗?zāi)K、FIFO存儲發(fā)送模塊、姿 態(tài)角輸出模塊、運算結(jié)果分解模塊、422輸出模塊，具體功能如下：
[0025] (1)分頻模塊根據(jù)所需波特率將系統(tǒng)時鐘進(jìn)行分頻處理獲得所需的422接收模塊 的輸入時鐘，422接收模塊在輸入時鐘上升沿時接收422接收端傳來的比特，接收到一個字 節(jié)后輸入幀校驗?zāi)K；
[0026] (2)幀校驗?zāi)K接收到同步字0x55aa時開始接收新的一幀數(shù)據(jù)，該幀的最后一 個字節(jié)為前面所有數(shù)據(jù)和較驗的低八位數(shù)據(jù)，通過和校驗的一組數(shù)據(jù)將輸出口使能信號 cheken置'Γ作為FIFO存儲發(fā)送模塊的敏感信號，一幀數(shù)據(jù)包括該時刻的三個軸向角速 度；
[0027] (3)FIFO存儲發(fā)送模塊當(dāng)使能信號cheken上升沿時存入一組數(shù)據(jù)，在存儲數(shù)據(jù)大 于3時同時輸出三組數(shù)據(jù)用于二子樣旋轉(zhuǎn)矢量姿態(tài)算法IP核的運算，下面每新存入兩組新 數(shù)據(jù)，加上之前最近時刻的一組數(shù)據(jù)為角增量模塊提供輸入信號；
[0028] (4)運算結(jié)果分解模塊將最近更新的姿態(tài)角包括俯仰角、橫滾角、航向角分解成一 個個字節(jié)并加上同步字〇x55aa以及和校驗的低八位按時鐘周期輸出；
[0029] (5)422輸出模塊按照一定的波特率將運算結(jié)果分解模塊輸出的字節(jié)按順序發(fā)送 出去，422輸出模塊的時鐘由分頻模塊分頻得到。
[0030] 二、角增量模塊
[0031] 所述角增量模塊根據(jù)FIFO存儲發(fā)送模塊輸入的三組數(shù)據(jù)格式為IEEE754單精度 浮點的三個軸向的角速度Thetaji= 1，2, 3)，按照積分公式求得兩組二子樣姿態(tài)角增量ΘuΘ2作為等效旋轉(zhuǎn)矢量模塊的輸入信號；所述積分公式如式（1)所示：
[0034] 其中，h為姿態(tài)更新周期，在二子樣旋轉(zhuǎn)矢量姿態(tài)算法中取0.02,Thetaji= 1，2, 3)為三個軸向角速度。
[0035] 三、等效旋轉(zhuǎn)矢量模塊
[0036] 所述等效旋轉(zhuǎn)矢量模塊將二子樣姿態(tài)角增量ΘΘ2根據(jù)優(yōu)化的二子樣旋轉(zhuǎn)矢量 公式處理得到等效旋轉(zhuǎn)矢量Φ作為變換四元數(shù)模塊的輸入信號；優(yōu)化的二子樣旋轉(zhuǎn)矢量 公式如式（2)所示：
[0038]其中Φ為等效旋轉(zhuǎn)矢量。
[0039] 四、變換四元數(shù)模塊
[0040] 所述變換四元數(shù)模塊將等效旋轉(zhuǎn)矢量Φ按照四元數(shù)變換公式通過CORDICIP核、 浮點操作器IP核、加/減法器IP核、乘法器IP核和除法器IP核進(jìn)行處理得到四元數(shù)作為 四元數(shù)更新模塊的輸入信號；四元數(shù)變換公式如式（3)所示：
[0042]其中，Φχ，Φγ，Φζ分別為等效旋轉(zhuǎn)矢量Φ的三個軸向