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

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

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