專利名稱：一種組合式空間位姿精密動態(tài)測量裝置及方法
技術(shù)領(lǐng)域：
本發(fā)明屬于大尺度(超過300米)精密測量技術(shù)領(lǐng)域，涉及一種空 間位姿精密動態(tài)測量技術(shù)。
背景技術(shù)：
精密測量技術(shù)和裝置是先進(jìn)制造主要支撐技術(shù)之一，體現(xiàn)國家工業(yè) 與科技水平。國民經(jīng)濟(jì)和先進(jìn)制造的不斷發(fā)展對精密測量提出新的要求， 因此提高精密測量水平對提高我國先進(jìn)制造能力，促進(jìn)國民經(jīng)濟(jì)發(fā)展具 有重要意義。針對大尺度(達(dá)到300米)空間的高精度、高采樣率、長 時(shí)間運(yùn)行的實(shí)時(shí)測量問題，還沒有哪一種技術(shù)具有絕對的競爭優(yōu)勢。目 前，可選技術(shù)主要包括全球定位系統(tǒng)(GPS)、慣性測量技術(shù)(IPS)和 全站儀測量技術(shù)(TS)。綜合評價(jià)各具優(yōu)勢和不足。GPS具有全天候、速度快、精度高、點(diǎn)間相互不用通視等優(yōu)點(diǎn)。也 有受美國控制制約、要求視空條件好、周圍不能有強(qiáng)功率發(fā)射電臺等限 制。測量速度和精度可達(dá)到20Hz， lcm。60年代以來，隨著激光的出現(xiàn)，激光陀螺和光纖陀螺問世，以其優(yōu) 良的性能受到關(guān)注，迅速進(jìn)入慣性測量領(lǐng)域。慣性測量元件的發(fā)展，為 慣性測量裝置和技術(shù)的發(fā)展奠定了良好的基礎(chǔ)。慣性測量單元僅依靠慣 性裝置本身就能在目標(biāo)固聯(lián)體內(nèi)部獨(dú)立地完成測量任務(wù)，具有高度的自 主性，不受測量范圍限制，采樣率高給出近似連續(xù)的測量數(shù)據(jù)，能提供 速度、加速度等豐富的動態(tài)測量信息。由于慣性測量單元具有高度自主 性，不從外部獲得信息進(jìn)行校準(zhǔn)，單獨(dú)采用慣性單元進(jìn)行測量時(shí)會使得 定位定向誤差發(fā)生累積，其定位誤差隨測量時(shí)間增加而變大，無法進(jìn)行 長時(shí)間連續(xù)工作。有著"測量機(jī)器人"之稱的全站儀測量技術(shù)測程大、價(jià)格低、環(huán)境適 應(yīng)能力強(qiáng)，靜態(tài)測量精度高(測角精度可達(dá)到0.5"，靜態(tài)測距精度5lmm+lppm)，但動態(tài)測量能力較弱。隨著全站儀技術(shù)的發(fā)展，功能的 不斷完善，全站儀已能進(jìn)行自動識別、鎖定和跟蹤。盡管全站儀測量系 統(tǒng)實(shí)現(xiàn)了動態(tài)跟蹤測量和智能化，但仍存在時(shí)滯和采樣率低(5Hz)等問 題，尤其存在較大的測量時(shí)滯，對動態(tài)測量精度造成很大的影響。發(fā)明內(nèi)容為了解決現(xiàn)有技術(shù)的問題，本發(fā)明針對大型裝置對大尺度空間的高 精度、高采樣率、長時(shí)間運(yùn)行的實(shí)時(shí)測量需求，提出一種組合式空間位 姿精密動態(tài)測量裝置及方法。為達(dá)成所述目的，本發(fā)明組合式空間位姿精密動態(tài)測量裝置，該系 統(tǒng)包括慣性測量單元、全站儀測量單元、數(shù)據(jù)采集處理單元以及計(jì)算機(jī); 慣性測量單元和全站儀測量單元分別與數(shù)據(jù)采集處理單元相連接，慣性 測量單元輸出目標(biāo)固連體角速度和加速度信息；全站儀測量單元輸出對 應(yīng)目標(biāo)固連體上靶標(biāo)的位置信息；數(shù)據(jù)采集處理單元分別與慣性測量單 元和全站儀測量單元相連接，數(shù)據(jù)采集處理單元接收、存儲并輸出目標(biāo) 固連體角速度和加速度信息和目標(biāo)固連體上靶標(biāo)的位置信息；計(jì)算機(jī)與 數(shù)據(jù)采集處理單元相連接，用于將角速度和加速度信息和靶標(biāo)的位置信 息進(jìn)行數(shù)據(jù)融合處理，并輸出目標(biāo)固連體的位置和姿態(tài)。其中慣性測量單元是用于目標(biāo)固連體全位姿測量所需的按平臺式或 捷聯(lián)式工作方式工作的慣性儀表組。其中全站儀測量單元包括全站儀群和靶標(biāo)組，每個(gè)全站儀與對應(yīng) 的靶標(biāo)非接觸連接，靶標(biāo)組接收并反射所述全站儀群發(fā)射的信號。其中計(jì)算機(jī)的數(shù)據(jù)融合處理，采用時(shí)間回溯算法修正消除全站儀測 量單元實(shí)時(shí)動態(tài)跟蹤測量的時(shí)滯，融合處理慣性測量單元和全站儀測量 單元的測量數(shù)據(jù)，計(jì)算出目標(biāo)固連體在全局坐標(biāo)系的位置和姿態(tài)信息。為達(dá)成所述目的，本發(fā)明組合式空間位姿精密動態(tài)測量方法，包括 如下步驟步驟S1:設(shè)備啟動并進(jìn)行初始化，使設(shè)備進(jìn)入穩(wěn)定運(yùn)行階段； 步驟S2:自檢和標(biāo)定確定各靶標(biāo)在目標(biāo)固連體坐標(biāo)系內(nèi)坐標(biāo)；慣性測量單元平臺坐標(biāo)系與目標(biāo)固連體坐標(biāo)系之間的變換關(guān)系；全站儀群 間的位置關(guān)系，定義全局坐標(biāo)系；測量得到全站儀組時(shí)滯參數(shù)；步驟S3:慣性測量單元測出目標(biāo)固聯(lián)體加速度、角速度在其敏感軸 上的輸出值，通過平臺坐標(biāo)系與目標(biāo)固連體坐標(biāo)系的相應(yīng)坐標(biāo)變換得到 目標(biāo)固連坐標(biāo)系內(nèi)的加速度和角速度值，發(fā)送給數(shù)據(jù)采集處理單元；步驟S4:各全站儀測量計(jì)算出對應(yīng)耙標(biāo)在全站儀坐標(biāo)系內(nèi)的坐標(biāo)信 息，通過坐標(biāo)轉(zhuǎn)換得到目標(biāo)固連體上各靶標(biāo)在全局坐標(biāo)系下的坐標(biāo)值， 發(fā)送給數(shù)據(jù)采集處理單元；步驟S5:數(shù)據(jù)采集處理單元接收、存儲慣性測量單元發(fā)送來的目標(biāo) 固連體的加速度和角速度值和全站儀測量單元發(fā)送來的各靶標(biāo)在全局坐 標(biāo)系下的坐標(biāo)值，記錄數(shù)據(jù)的同時(shí)記錄數(shù)據(jù)到達(dá)時(shí)刻，并根據(jù)計(jì)算機(jī)單 元需求轉(zhuǎn)發(fā)相應(yīng)的數(shù)據(jù)；步驟S6:判斷全站儀數(shù)據(jù)是否更新到達(dá)，如果全站儀數(shù)據(jù)到達(dá)后，則執(zhí)行步驟S7，如果全站儀數(shù)據(jù)沒有到達(dá)，則執(zhí)行步驟S8;步驟S7:根據(jù)全站儀測量數(shù)據(jù)解算目標(biāo)固聯(lián)體位置和姿態(tài)；根據(jù)全站儀時(shí)滯參數(shù)，在計(jì)算機(jī)記錄的慣性測量單元測量的數(shù)據(jù)序列中尋找起算時(shí)間點(diǎn)；更新時(shí)間回溯時(shí)刻并初始化解算參數(shù)；作為參數(shù)輸入下一步 的解算程序中；步驟S8:計(jì)算機(jī)單元根據(jù)慣性測量解算原理，以步驟S7回溯的時(shí) 刻為數(shù)據(jù)處理的時(shí)間起算點(diǎn)，全站儀測量單元更新的數(shù)據(jù)為初始參數(shù),解 算出目標(biāo)固聯(lián)體位置和姿態(tài)；步驟S9: KALMAN濾波器分別將步驟S2、步驟S7、步驟S8各 步計(jì)算出的參數(shù)數(shù)據(jù)，和步驟S3、步驟S4所測原始數(shù)據(jù)，作為卡爾曼 濾波算法的系統(tǒng)信息和測量信息，通過對誤差進(jìn)行最優(yōu)估計(jì)修正，從而 計(jì)算出目標(biāo)固連坐標(biāo)系相對于全局坐標(biāo)系的位置和姿態(tài)信息，輸出最優(yōu) 位姿值；步驟S10:判斷是否收到測量結(jié)束指令，如無收到，繼續(xù)執(zhí)行步驟6 進(jìn)行測量。其中，所述各靶標(biāo)在目標(biāo)固連體坐標(biāo)系內(nèi)坐標(biāo)，是預(yù)先標(biāo)定出靶標(biāo) 與目標(biāo)固聯(lián)體之間的幾何位置尺寸關(guān)系，定義目標(biāo)固連體坐標(biāo)系，從而 得到各靶標(biāo)在目標(biāo)固連體坐標(biāo)系內(nèi)坐標(biāo)；其中，所述慣性測量單元平臺坐標(biāo)系與目標(biāo)固連體坐標(biāo)系之間的變換關(guān)系是預(yù)先標(biāo)定出慣性測量單元所構(gòu)成的平臺坐標(biāo)系與目標(biāo)固聯(lián)體 坐標(biāo)系之間的幾何位置和尺寸關(guān)系，從而得到平臺坐標(biāo)系與目標(biāo)固連體 坐標(biāo)系之間的變換關(guān)系。其中，所述全局坐標(biāo)系是預(yù)先標(biāo)定出全站儀群之間的位置關(guān)系， 定義全局坐標(biāo)系，從而得到各全站儀自身坐標(biāo)系與全局坐標(biāo)系之間的變 換關(guān)系以及目標(biāo)固連體坐標(biāo)系相對全局坐標(biāo)系的初始狀態(tài)。其中，所述全站儀組時(shí)滯參數(shù)是利用目標(biāo)固連體運(yùn)行一理論軌跡， 同時(shí)用全站儀群進(jìn)行測量，用測量數(shù)據(jù)與理論數(shù)據(jù)進(jìn)行相關(guān)處理得到全 站儀組時(shí)滯參數(shù)。其中，所述數(shù)據(jù)融合處理，是根據(jù)全站儀實(shí)時(shí)動態(tài)跟蹤測量時(shí)存在 的時(shí)滯，采用時(shí)間回溯算法，當(dāng)全站儀位姿修正數(shù)據(jù)到達(dá)時(shí)，將慣性測 量數(shù)據(jù)回溯到全站儀測量數(shù)據(jù)的真實(shí)時(shí)間點(diǎn)，以該時(shí)間點(diǎn)作為數(shù)據(jù)處理 起算點(diǎn)，以全站儀到達(dá)數(shù)據(jù)作為起算初始參數(shù)，用于消除時(shí)滯，從而計(jì) 算出目標(biāo)固連體在全局坐標(biāo)系的位置和姿態(tài)信息。本發(fā)明的技術(shù)效果或優(yōu)點(diǎn)在全站儀測量系統(tǒng)中引入慣性自主測量 方法提高測量質(zhì)量，由陀螺與加速度計(jì)實(shí)時(shí)測量出被測目標(biāo)的角速度與 加速度，再通過積分分別給出姿態(tài)和位置信息。由全站儀每隔一段時(shí)間 給出修正信號，采用時(shí)間回溯算法修正時(shí)滯。充分發(fā)揮全站儀測量和慣 性測量各自的優(yōu)勢，相互補(bǔ)充。本發(fā)明兩種技術(shù)融合測量的優(yōu)勢如下1、 測程達(dá)到全站儀測程，達(dá)到100米到1000米以上。2、 得益于慣性測量優(yōu)勢提高了采樣率，得到近似連續(xù)信號。3、 利用慣性測量優(yōu)勢直接提供速度、加速度動態(tài)信息。4、 慣性測量單元與全站儀測量單元優(yōu)勢互補(bǔ)，采用時(shí)間回溯算法修 正全站儀測量單元時(shí)滯，使組合測量單元測量精度超過全站儀動態(tài)測量 精度，達(dá)到全站儀靜態(tài)測量精度。5、 全站儀測量單元的引入，克服了慣性測量單元測量誤差隨時(shí)間累計(jì)的弱點(diǎn)，使組合測量誤差不隨時(shí)間增加而累積與系統(tǒng)運(yùn)行時(shí)間長短無 關(guān)，延長了連續(xù)工作時(shí)間。兩種測量技術(shù)的融合實(shí)現(xiàn)了大尺度、高采樣率、高精度、可長期運(yùn) 行的精密實(shí)時(shí)動態(tài)測量，為國家重大裝備制造領(lǐng)域的精密測量提供了一種低成本的測量手段。


圖l是本發(fā)明裝置的結(jié)構(gòu)2是現(xiàn)有技術(shù)500米口徑球面射電望遠(yuǎn)鏡示意3確認(rèn)時(shí)滯量仿真說明4是本發(fā)明時(shí)間回溯算法運(yùn)行示意5是本發(fā)明時(shí)間回溯法對一組實(shí)測數(shù)據(jù)的處理結(jié)果圖6是本發(fā)明的所述方法的運(yùn)行流程圖具體實(shí)施方式
下面結(jié)合附圖詳細(xì)說明本發(fā)明技術(shù)方案中所涉及的各個(gè)細(xì)節(jié)問題。 應(yīng)指出的是，所描述的實(shí)施例僅旨在便于對本發(fā)明的理解，而對其不起 任何限定作用。本發(fā)明是針對世界上最大的500米口徑球面射電望遠(yuǎn)鏡(FAST)中 饋源支撐系統(tǒng)的精密測量需求(工作范圍300米，采樣率10Hz，精度 RMS2mm)提出的。以該關(guān)鍵技術(shù)研究為背景，對各種相關(guān)測量技術(shù)進(jìn) 行了深入研究，提出研制一種組合式空間位姿精密動態(tài)測量裝置和方法， 可實(shí)現(xiàn)大尺度、超大尺度空間位姿的高精度實(shí)時(shí)測量。FAST是利用貴州喀斯特地區(qū)的洼坑作為望遠(yuǎn)鏡臺址建造的世界上 最大的球面射電望遠(yuǎn)鏡，圖2是現(xiàn)有技術(shù)500米口徑球面射電望遠(yuǎn)鏡示 意圖。具體工程實(shí)現(xiàn)是將500米口徑的主反射面分成4600塊小球面單元 拼合并安裝在洼地內(nèi)，形成初始中性球面。觀測時(shí)，反射面下方的驅(qū)動 裝置C在計(jì)算機(jī)控制下使小球面單元塊沿徑向運(yùn)動，使對準(zhǔn)被觀測的天 體Stl的300米口徑內(nèi)的球面調(diào)整成拋物面，隨天體St2運(yùn)動不同時(shí)刻對 準(zhǔn)的區(qū)域不同；在反射面周邊約600米尺度上建造數(shù)個(gè)一百多米高的塔 B，饋源倉A由過塔鋼索在計(jì)算機(jī)控制下拖動，在焦面上運(yùn)動；索拖動 下的饋源倉需在150米高、206米口徑的焦面上運(yùn)動；實(shí)現(xiàn)高精度的指 向跟蹤，饋源倉的高精度實(shí)時(shí)測量是關(guān)鍵，定位測量精度達(dá)到RMS 2mm， 測量頻率大于10Hz。這是極具挑戰(zhàn)性的難題。為驗(yàn)證FAST各項(xiàng)關(guān)鍵技術(shù)的可行性，建造了 50米整體縮尺模型。綜合考慮各種技術(shù)的應(yīng)用前景，F(xiàn)AST 50米模型上采用三臺徠卡 TCRA1101型全站儀，測量饋源倉上三個(gè)360度棱鏡為靶標(biāo)，計(jì)算得出 位置和姿態(tài)，由于存在采樣率低和時(shí)滯等問題，動態(tài)測量精度無法滿足 FAST工作要求。對由三臺全站儀組成的位姿動態(tài)測量系統(tǒng)性能進(jìn)行了大 量的測試，所有116組動態(tài)實(shí)驗(yàn)數(shù)據(jù)表明1、全站儀組合位姿動態(tài)測量系統(tǒng)存在時(shí)滯；2、在確定工況下時(shí)滯 穩(wěn)定不變；3、動態(tài)測量時(shí)時(shí)滯是測量誤差的主要來源約占80%。 FAST 在測量范圍、測量精度和測量速度的高要求以及大量的實(shí)驗(yàn)結(jié)果催生了 這種"組合式空間位姿精密動態(tài)測量裝置和方法"的問世。請參閱圖1，本發(fā)明的組合式空間位姿精密動態(tài)測量裝置，包括慣 性測量單元l、全站儀測量單元2、數(shù)據(jù)釆集處理單元3以及計(jì)算機(jī)4;慣性測量單元1和全站儀測量單元2分別與數(shù)據(jù)釆集處理單元3相 連接，慣性測量單元1輸出目標(biāo)固連體角速度和加速度信息；全站儀測 量單元2輸出對應(yīng)目標(biāo)固連體上靶標(biāo)的位置信息；所述慣性測量單元是 用于目標(biāo)固連體全位姿測量所需的按平臺式或捷聯(lián)式工作方式工作的慣 性儀表組。慣性測量單元h裝有三個(gè)單自由度陀螺和三個(gè)加速度計(jì)。 陀螺的三個(gè)敏感軸與加速度計(jì)的三個(gè)敏感軸嚴(yán)格保持方向一致，并構(gòu)成 一個(gè)直角坐標(biāo)系，即平臺坐標(biāo)系。在具體實(shí)施例中，可選用封裝完整的 成品慣性儀表組。它與目標(biāo)固連體通過螺栓剛性連接，并由加工過程精 確保證慣性儀表組所建平臺坐標(biāo)系與目標(biāo)固連體坐標(biāo)系間的轉(zhuǎn)換精度。 慣性儀表組輸出目標(biāo)固聯(lián)體的加速度和角速度測量值。全站儀測量單元2包括全站儀群和靶標(biāo)組，每個(gè)全站儀與對應(yīng)的 靶標(biāo)非接觸連接，靶標(biāo)組接收并反射所述全站儀群發(fā)射的信號。全站儀 測量單元2:由三個(gè)結(jié)構(gòu)相同的全站儀和對應(yīng)目標(biāo)固連體上三個(gè)靶標(biāo)組 成。全站儀融光、機(jī)、電等先進(jìn)技術(shù)于一身，其整個(gè)系統(tǒng)主要部分為電 子測距單元或稱之為測距儀；電子測角及微處理器單元或稱之為電子經(jīng) 緯儀；電子記錄單元或稱存儲單元。全站儀的測量原理和激光跟蹤儀一 樣為極坐標(biāo)法，需要測量一個(gè)斜距和二個(gè)角度可得到被測點(diǎn)的三維坐標(biāo)， 但是測距方式、跟蹤方式及結(jié)構(gòu)設(shè)計(jì)不同，決定了跟蹤儀測量精度和采樣率好于全站儀，而測程則無法與全站儀相比，跟蹤儀采用干涉法和絕 對測距法，測程幾十米，全站儀采用多測尺相位法測距測程達(dá)到公里量級?？蛇x用封裝完整的徠卡公司的TCA系列全站儀，如TCA2003和 TCRA1101等。靶標(biāo)可選為三個(gè)空心角隅棱鏡、實(shí)心角隅棱鏡、或360 棱鏡等，它們與目標(biāo)固聯(lián)體通過螺栓剛性連接，并由加工過程保證反射 鏡中心點(diǎn)在運(yùn)動目標(biāo)固連坐標(biāo)系內(nèi)的坐標(biāo)精度。數(shù)據(jù)采集處理單元3分別與慣性測量單元1和全站儀測量單元2相 連接，數(shù)據(jù)采集處理單元3接收、存儲并輸出目標(biāo)固連體角速度和加速 度信息和目標(biāo)固連體上靶標(biāo)的位置信息；計(jì)算機(jī)4與數(shù)據(jù)采集處理單元 3相連接，用于將角速度和加速度信息和靶標(biāo)的位置信息進(jìn)行數(shù)據(jù)融合 處理，并輸出目標(biāo)固連體的位置和姿態(tài)。所述計(jì)算機(jī)4的數(shù)據(jù)融合處理， 是根據(jù)全站儀實(shí)時(shí)動態(tài)跟蹤測量時(shí)存在的時(shí)滯，釆用時(shí)間回溯算法修正 消除全站儀測量單元2實(shí)時(shí)動態(tài)跟蹤測量的時(shí)滯，融合處理慣性測量單 元1和全站儀測量單元2的測量數(shù)據(jù)，計(jì)算出目標(biāo)固連體在全局坐標(biāo)系 的位置和姿態(tài)信息。數(shù)據(jù)釆集處理單元3以及計(jì)算機(jī)4:計(jì)算機(jī)4內(nèi)安裝數(shù)據(jù)采集處理 單元3和存儲多傳感器數(shù)據(jù)融合算法軟件程序，數(shù)據(jù)采集處理單元3可 選用市場成型產(chǎn)品Moxa卡，計(jì)算機(jī)4選擇帶有安裝Moxa卡插槽的PC 機(jī)即可?，F(xiàn)將本實(shí)施例中實(shí)施辦法和部分算法公式闡述如下確定時(shí)滯確定工況下，驅(qū)動目標(biāo)固聯(lián)體沿已知軌跡運(yùn)動，用全站儀測量單元2測量目標(biāo)固聯(lián)體位姿，測量結(jié)果與理論軌跡進(jìn)行相關(guān)比較可得到具體時(shí)滯量At。圖3中e是理論軌跡，f是全站儀實(shí)測軌跡，在時(shí)間軸上將全站儀測量數(shù)據(jù)左移并計(jì)算e和f曲線之差的均方根誤差，當(dāng)誤差達(dá)到最小值時(shí)時(shí)間軸移動的數(shù)據(jù)量即時(shí)滯量。慣性測量單元1:慣性測量單元1由三個(gè)陀螺構(gòu)成的陀螺組件及三個(gè)加速度計(jì)構(gòu)成的加速度計(jì)組件，分別感測對應(yīng)敏感方位的角速度及加速度，加速度和角速度經(jīng)過積分等運(yùn)算，獲取目標(biāo)固聯(lián)體的位置及旋轉(zhuǎn)角度從而獲得目標(biāo)固聯(lián)體的位姿信息。目標(biāo)固聯(lián)體的速度取決于它的初速度、加速度和作用時(shí)間，即速度是對加速度的積分，可表示為11u(O =《)+ J"(XW,式中速度為u，時(shí)間為"加速度為"，u(/。)為初始時(shí)刻目標(biāo)固聯(lián)體 的運(yùn)動速度向量。而目標(biāo)固聯(lián)體的位置取決于起始位置、速度和運(yùn)行時(shí) 間，也就是說位置就等于對速度的積分，可寫成0式中《。)為初始時(shí)刻目標(biāo)固聯(lián)體的位置向量。 同理，對目標(biāo)固聯(lián)體的角度測定也同樣如此，角速度變化經(jīng)積分可 得到旋轉(zhuǎn)角度。p(，0目標(biāo)固聯(lián)坐標(biāo)系和全局坐標(biāo)系之間通過坐標(biāo)變換矩陣來求得，陀螺儀測量得到目標(biāo)固聯(lián)體的姿態(tài)角具體算法包括目標(biāo)固聯(lián)坐標(biāo)系統(tǒng)三軸定義分別為xb為運(yùn)行方向，前向?yàn)檎?，yb為運(yùn)行方向的交叉方向，右側(cè)為正，Zb為目標(biāo)固聯(lián)體垂直方向，向下為正。加速度計(jì)測量得到的沿運(yùn)目標(biāo)固聯(lián)坐標(biāo)系的比力信息。全局坐標(biāo)系統(tǒng)三軸定義分別為Xn為北方向(大地北)，yn為東方向， Z"為鉛垂線方向，向下為正。陀螺儀測量可得到的沿全局坐標(biāo)系的角加 速度。目標(biāo)固聯(lián)坐標(biāo)系和全局坐標(biāo)系之間通過坐標(biāo)變換矩陣來求得。變換 矩陣由三個(gè)歐拉角組成，航空工程中通常把^、 ^和^稱為目標(biāo)固聯(lián)體姿態(tài)的方位Raw、俯仰pitch和側(cè)滾角roll。其中^俯仰角(pitch),即目標(biāo)固 聯(lián)坐標(biāo)系xb軸與水平線的夾角，頭朝上為正。-為側(cè)滾角(roll)，是目標(biāo) 固聯(lián)坐標(biāo)系yb軸與水平線的夾角，右測朝下為正。^為偏航角(Raw)， 是在水平面內(nèi)，目標(biāo)固聯(lián)坐標(biāo)系xb軸與北方向之間的夾角，右偏為正。 陀螺儀測量得到目標(biāo)固聯(lián)體的姿態(tài)角(Roll、 Pitch、 Yaw)。引入四元數(shù)向:《=《0《輛足限制條件^^"。2+》、=1。其中，cos二《。=2為標(biāo):5 = i《i + j《2 + k《3 = f sin A2為矢量。單位矢量；方向?yàn)樾?和四元數(shù)相乘法則，姿態(tài)矩陣可以用四元數(shù)表示為1 —2&22+&2) 2(仏《2—^仏)2(仏仏+《o&)2(糾廣暢)2(《2《3+鶴)1-2(《X)1-2(《22+《32) 2(貓+憾3) 2(貓-暢)2(暢-鋪3) 1-2(W+^) 2(^3+幅) _2(暢+貓)2(狄廣鏈)1-2(A2+^)_ 四元數(shù)向量滿足微分方程7 = 5即_ 0—<yt叫二 10，《=一 2，—wz0w乂《250、仏乂<5J =并且，其中，符號0表示四元數(shù)乘積。注意，式中，6 = —+^+—為目標(biāo)固聯(lián)坐標(biāo)系xbybzb相對全局坐標(biāo)系坐標(biāo)系xyz的 轉(zhuǎn)動角速度，即_w7w
隊(duì)式中，L"」為目標(biāo)固聯(lián)體的絕對角速度；L^」為北—東—下全局坐 標(biāo)系的牽連角速度眠a — +-tan義式中，Q = l5.04107°/h，地球自轉(zhuǎn)角速度； 2=目標(biāo)固聯(lián)體緯度；、=東向速度； =北向速度；A=6378km，地球半徑； ^=目標(biāo)固聯(lián)體高度。 在短時(shí)間飛行的低速目標(biāo)固聯(lián)體上，地球表觀運(yùn)動角速度 [ , "2 A]T可忽略不計(jì)。因此，[A A A了a^《rf，即，安裝在目標(biāo)固聯(lián)體上的三只速率陀螺輸出可作為相對角速度矢量6的直接測姿態(tài)角可由姿態(tài)矩陣C的元素^計(jì)算如下= arc tan 方位角 & 俯仰角 S = -arcsiiiC13^ = arctan"^~測滾角 & 。全站儀測量單元2:將靶標(biāo)安裝在被測目標(biāo)固聯(lián)體上，每個(gè)靶標(biāo)由 一臺全站儀跟蹤測量，靶標(biāo)在全局坐標(biāo)系下的坐標(biāo)[兀KZ]^,靶標(biāo)在目 標(biāo)固聯(lián)坐標(biāo)系中的坐標(biāo)[XJ，4(s標(biāo))已知。運(yùn)動剛體，動態(tài)測量需要確定它 在空間的六個(gè)自由度，即要確定目標(biāo)固聯(lián)坐標(biāo)系與全局坐標(biāo)系的坐標(biāo)轉(zhuǎn)換參數(shù)^6^,z。,y。,z。。通過對目標(biāo)剛體上三個(gè)觀測點(diǎn)進(jìn)行觀測，三個(gè)觀測點(diǎn)在剛體本身坐 標(biāo)系的坐標(biāo)已知。由如下公式解算兩坐標(biāo)系統(tǒng)轉(zhuǎn)換的旋轉(zhuǎn)參數(shù)和平移參 數(shù)^e,y,z。時(shí)間回溯如圖4示出消除時(shí)滯的時(shí)間回溯算法，T為慣性測量單 元l數(shù)據(jù)采樣周期，丁1=111*丁為全站儀測量采樣周期，^=11*丁為全站儀 測量時(shí)滯，i即t2為當(dāng)前時(shí)刻，j全站儀數(shù)據(jù)到達(dá)點(diǎn)，t,時(shí)間回溯后解算 起始時(shí)刻，tQ時(shí)間回溯后解算起始時(shí)刻的前一時(shí)刻以計(jì)算初速度。全站 儀一組數(shù)據(jù)到達(dá)后至下組數(shù)據(jù)到達(dá)前任意時(shí)刻t2，用由全站儀測量單元2數(shù)據(jù)修正后的慣性測量數(shù)據(jù)解算出目標(biāo)固聯(lián)體在該時(shí)刻位姿。具體時(shí)間 回溯辦法為，全站儀數(shù)據(jù)在j時(shí)到達(dá)設(shè)k二O，在全站儀下組數(shù)據(jù)到達(dá)前， 慣性測量單元1每運(yùn)行一個(gè)周期T， k值加l;在已經(jīng)記錄的慣性測量數(shù)據(jù)中，回溯慣性測量數(shù)據(jù)到t, :(i-k-n^T時(shí)亥U，將該時(shí)刻作為數(shù)據(jù)融合處理起算點(diǎn)，將全站儀最新到達(dá)數(shù)據(jù)作為該起算時(shí)刻的位置和姿態(tài)初始 參數(shù)，用該時(shí)刻t,與前一時(shí)刻t。兩時(shí)刻實(shí)測位置信息進(jìn)行差分計(jì)算解算 出速度作為該時(shí)刻速度初始參數(shù)，初始參數(shù)完備，可解算位姿，借此方 法消除了全站儀測量時(shí)滯影響；如圖5示出用時(shí)間回溯辦法對其中一組全站儀實(shí)測數(shù)據(jù)的處理結(jié)果，展示了消除時(shí)滯對測量精度的改善，圖5a中曲線a是理論軌跡，曲線b是全站儀測量實(shí)測曲線，反應(yīng)了存在時(shí)滯和采樣率低問題，圖5b是將圖5a中曲線b按時(shí)滯量進(jìn)行時(shí)間移位，使之與曲線a重合；圖5c曲線c代表圖5a中曲線a與曲線b之差，圖5c中曲線d代表圖5b中曲線a與曲線b之差，(RMS曲線d-RMS曲線c)/RMS曲線d=0.802顯示出消除時(shí)滯影響對動態(tài)測量精度的改善效果。如圖6示出算法流程圖，計(jì)算機(jī)采集慣性測量單元1與全站儀測量單元2的測量輸出值，經(jīng)過相應(yīng)初步的數(shù)據(jù)變換處理后，送入擴(kuò)展卡爾曼濾波器。相應(yīng)的算法流程主要由以下幾部分組成(1) 開始，初始化設(shè)備啟動并進(jìn)行初始化，使設(shè)備進(jìn)入穩(wěn)定運(yùn)行 階段；(2) 自檢和標(biāo)定作為系統(tǒng)參數(shù)送入計(jì)算機(jī)，在循環(huán)測量解算過程中保持不變。a. 確定各靶標(biāo)在目標(biāo)固連體坐標(biāo)系內(nèi)坐標(biāo)，是預(yù)先標(biāo)定出靶標(biāo)與目 標(biāo)固聯(lián)體之間的幾何位置尺寸關(guān)系，定義目標(biāo)固連體坐標(biāo)系，從而得到 各耙標(biāo)在目標(biāo)固連體坐標(biāo)系內(nèi)坐標(biāo)；b. 慣性測量單元1平臺坐標(biāo)系與目標(biāo)固連體坐標(biāo)系之間的變換關(guān)系, 是預(yù)先標(biāo)定出慣性測量單元1所構(gòu)成的平臺坐標(biāo)系與目標(biāo)固聯(lián)體坐標(biāo)系 之間的幾何位置和尺寸關(guān)系，從而得到平臺坐標(biāo)系與目標(biāo)固連體坐標(biāo)系之間的變換關(guān)系；c. 全站儀群間的位置關(guān)系，定義全局坐標(biāo)系；是預(yù)先標(biāo)定出全站儀 群之間的位置關(guān)系，定義全局坐標(biāo)系，從而得到各全站儀自身坐標(biāo)系與全局坐標(biāo)系之間的變換關(guān)系以及目標(biāo)固連體坐標(biāo)系相對全局坐標(biāo)系的初 始狀態(tài)；d.測量得到全站儀組時(shí)滯參數(shù)；目標(biāo)固連體運(yùn)行一理論軌跡，同時(shí) 用全站儀群進(jìn)行測量，用測量數(shù)據(jù)與理論數(shù)據(jù)進(jìn)行相關(guān)處理得到全站儀 組時(shí)滯參數(shù)；(3) 慣性測量數(shù)據(jù)慣性測量單元1測出目標(biāo)固聯(lián)體加速度、角速 度在其敏感軸上的輸出值，通過平臺坐標(biāo)系與目標(biāo)固連體坐標(biāo)系的相應(yīng) 坐標(biāo)變換得到目標(biāo)固連坐標(biāo)系內(nèi)的加速度和角速度值，發(fā)送給數(shù)據(jù)采集 處理單元；(4) 全站儀測量單元2的全站儀測量數(shù)據(jù)TS:各全站儀測量計(jì)算 出對應(yīng)耙標(biāo)在全站儀坐標(biāo)系內(nèi)的坐標(biāo)信息，通過坐標(biāo)轉(zhuǎn)換得到目標(biāo)固連 體上各靶標(biāo)在全局坐標(biāo)系下的坐標(biāo)值，發(fā)送給數(shù)據(jù)采集處理單元3;(5) 采集記錄數(shù)據(jù)采集處理單元3接收并存儲慣性測量單元1發(fā) 送來的目標(biāo)固連的加速度和角速度值和全站儀測量單元2發(fā)送來的各靶 標(biāo)在全局坐標(biāo)系下的坐標(biāo)值，記錄數(shù)據(jù)的同時(shí)記錄數(shù)據(jù)到達(dá)時(shí)刻，并根 據(jù)計(jì)算機(jī)4需求轉(zhuǎn)發(fā)相應(yīng)的數(shù)據(jù)；(6) 判斷全站儀數(shù)據(jù)是否到達(dá)更新，如果全站儀數(shù)據(jù)到達(dá)后，則執(zhí) 行步驟(7)，如果全站儀數(shù)據(jù)沒有到達(dá)，則執(zhí)行步驟(8)。(7) 根據(jù)全站儀測量數(shù)據(jù)解算目標(biāo)固聯(lián)體位置和姿態(tài)；根據(jù)全站儀 時(shí)滯參數(shù)，在計(jì)算機(jī)記錄的慣性測量單元1測量的數(shù)據(jù)序列中尋找起算 時(shí)間點(diǎn)；更新時(shí)間回溯時(shí)刻并初始化解算參數(shù)；作為參數(shù)輸入下一步的 解算程序中；(8) 計(jì)算機(jī)4根據(jù)慣性測量解算原理，以該時(shí)間點(diǎn)為數(shù)據(jù)處理的時(shí) 間起算點(diǎn)，全站儀測量單元2更新的數(shù)據(jù)為初始參數(shù)，解算出目標(biāo)固聯(lián)體 位置和姿態(tài)；(9) KALMAN濾波器分別將(2) (7) (8)各步計(jì)算出的參數(shù) 和數(shù)據(jù)，和慣性測量單元1和全站儀測量單元2所測原始數(shù)據(jù)(3) (4)， 作為卡爾曼濾波算法的系統(tǒng)信息和測量信息，通過對誤差進(jìn)行最優(yōu)估計(jì) 修正，從而計(jì)算出目標(biāo)固連坐標(biāo)系相對于全局坐標(biāo)系的位置和姿態(tài)信息， 輸出最優(yōu)位姿值；(10) 結(jié)束判斷是否收到測量結(jié)束指令，如無收到，繼續(xù)執(zhí)行步驟(3)進(jìn)行測量。以上所述，僅為本發(fā)明中的具體實(shí)施方式
，但本發(fā)明的保護(hù)范圍并 不局限于此，任何熟悉該技術(shù)的人在本發(fā)明所揭露的技術(shù)范圍內(nèi)，可理 解想到的變換或替換，都應(yīng)涵蓋在本發(fā)明的包含范圍之內(nèi)，因此，本發(fā) 明的保護(hù)范圍應(yīng)該以權(quán)利要求書的保護(hù)范圍為準(zhǔn)。
權(quán)利要求
1、一種組合式空間位姿精密動態(tài)測量裝置，其特征在于，該系統(tǒng)包括慣性測量單元、全站儀測量單元、數(shù)據(jù)采集處理單元以及計(jì)算機(jī)；慣性測量單元和全站儀測量單元分別與數(shù)據(jù)采集處理單元相連接，慣性測量單元輸出目標(biāo)固連體角速度和加速度信息；全站儀測量單元輸出對應(yīng)目標(biāo)固連體上靶標(biāo)的位置信息；數(shù)據(jù)采集處理單元分別與慣性測量單元和全站儀測量單元相連接，數(shù)據(jù)采集處理單元接收、存儲并輸出目標(biāo)固連體角速度和加速度信息和目標(biāo)固連體上靶標(biāo)的位置信息；計(jì)算機(jī)與數(shù)據(jù)采集處理單元相連接，用于將角速度和加速度信息和靶標(biāo)的位置信息進(jìn)行數(shù)據(jù)融合處理，并輸出目標(biāo)固連體的位置和姿態(tài)。
2、 根據(jù)權(quán)利要求1所述的空間位姿精密動態(tài)測量裝置，其特征在于， 所述慣性測量單元是用于目標(biāo)固連體全位姿測量所需的按平臺式或捷聯(lián) 式工作方式工作的慣性儀表組。
3、 根據(jù)權(quán)利要求1所述的空間位姿精密動態(tài)測量裝置，其特征在于， 所述全站儀測量單元包括全站儀群和靶標(biāo)組，每個(gè)全站儀與對應(yīng)的靶 標(biāo)非接觸連接，靶標(biāo)組接收并反射所述全站儀群發(fā)射的信號。
4、 根據(jù)權(quán)利要求1所述的空間位姿精密動態(tài)測量裝置，其特征在于， 所述計(jì)算機(jī)的數(shù)據(jù)融合處理，釆用時(shí)間回溯算法修正消除全站儀測量單 元實(shí)時(shí)動態(tài)跟蹤測量的時(shí)滯，融合處理慣性測量單元和全站儀測量單元 的測量數(shù)據(jù)，計(jì)算出目標(biāo)固連體在全局坐標(biāo)系的位置和姿態(tài)信息。
5、 —種組合式空間位姿精密動態(tài)測量的方法，其特征在于，包括以 下步驟步驟S1:設(shè)備啟動并進(jìn)行初始化，使設(shè)備進(jìn)入穩(wěn)定運(yùn)行階段； 步驟S2:自檢和標(biāo)定確定各靶標(biāo)在目標(biāo)固連體坐標(biāo)系內(nèi)坐標(biāo)；慣性測量單元平臺坐標(biāo)系與目標(biāo)固連體坐標(biāo)系之間的變換關(guān)系；全站儀群間的位置關(guān)系，定義全局坐標(biāo)系；測量得到全站儀組時(shí)滯參數(shù)；步驟S3:慣性測量單元測出目標(biāo)固聯(lián)體加速度、角速度在其敏感軸 上的輸出值，通過平臺坐標(biāo)系與目標(biāo)固連體坐標(biāo)系的相應(yīng)坐標(biāo)變換得到目標(biāo)固連坐標(biāo)系內(nèi)的加速度和角速度值，發(fā)送給數(shù)據(jù)采集處理單元；步驟S4:各全站儀測量計(jì)算出對應(yīng)靶標(biāo)在全站儀坐標(biāo)系內(nèi)的坐標(biāo)信息，通過坐標(biāo)轉(zhuǎn)換得到目標(biāo)固連體上各靶標(biāo)在全局坐標(biāo)系下的坐標(biāo)值，發(fā)送給數(shù)據(jù)采集處理單元；步驟S5:數(shù)據(jù)采集處理單元接收、存儲慣性測量單元發(fā)送來的目標(biāo)固連體的加速度和角速度值和全站儀測量單元發(fā)送來的各靶標(biāo)在全局坐 標(biāo)系下的坐標(biāo)值，記錄數(shù)據(jù)的同時(shí)記錄數(shù)據(jù)到達(dá)時(shí)刻，并根據(jù)計(jì)算機(jī)單元需求轉(zhuǎn)發(fā)相應(yīng)的數(shù)據(jù)；步驟S6:判斷全站儀數(shù)據(jù)是否更新到達(dá)，如果全站儀數(shù)據(jù)到達(dá)后，則執(zhí)行步驟S7，如果全站儀數(shù)據(jù)沒有到達(dá)，則執(zhí)行步驟S8;步驟S7:根據(jù)全站儀測量數(shù)據(jù)解算目標(biāo)固聯(lián)體位置和姿態(tài)；根據(jù)全站儀時(shí)滯參數(shù)，在計(jì)算機(jī)記錄的慣性測量單元測量的數(shù)據(jù)序列中尋找起算時(shí)間點(diǎn)；更新時(shí)間回溯時(shí)刻并初始化解算參數(shù)；作為參數(shù)輸入下一步 的解算程序中；步驟S8:計(jì)算機(jī)單元根據(jù)慣性測量解算原理，以步驟S7回溯的時(shí) 刻為數(shù)據(jù)處理的時(shí)間起算點(diǎn)，全站儀測量單元更新的數(shù)據(jù)為初始參數(shù),解 算出目標(biāo)固聯(lián)體位置和姿態(tài)；步驟S9: KALMAN濾波器分別將步驟S2、步驟S7、步驟S8各 步計(jì)算出的參數(shù)數(shù)據(jù)，和步驟S3、步驟S4所測原始數(shù)據(jù)，作為卡爾曼 濾波算法的系統(tǒng)信息和測量信息，通過對誤差進(jìn)行最優(yōu)估計(jì)修正，從而 計(jì)算出目標(biāo)固連坐標(biāo)系相對于全局坐標(biāo)系的位置和姿態(tài)信息，輸出最優(yōu) 位姿值；步驟S10:判斷是否收到測量結(jié)束指令，如無收到，繼續(xù)執(zhí)行步驟6 進(jìn)行測量。
6. 根據(jù)權(quán)利要求5所述的動態(tài)測量的方法，其特征在于所述各靶 標(biāo)在目標(biāo)固連體坐標(biāo)系內(nèi)坐標(biāo)，是預(yù)先標(biāo)定出靶標(biāo)與目標(biāo)固聯(lián)體之間的 幾何位置尺寸關(guān)系，定義目標(biāo)固連體坐標(biāo)系，從而得到各靶標(biāo)在目標(biāo)固 連體坐標(biāo)系內(nèi)坐標(biāo)。
7. 根據(jù)權(quán)利要求5所述的動態(tài)測量的方法，其特征在于所述慣性 測量單元平臺坐標(biāo)系與目標(biāo)固連體坐標(biāo)系之間的變換關(guān)系是預(yù)先標(biāo)定出慣性測量單元所構(gòu)成的平臺坐標(biāo)系與目標(biāo)固聯(lián)體坐標(biāo)系之間的幾何位 置和尺寸關(guān)系，從而得到平臺坐標(biāo)系與目標(biāo)固連體坐標(biāo)系之間的變換關(guān) 系。
8. 根據(jù)權(quán)利要求5所述的動態(tài)測量的方法，其特征在于所述全局 坐標(biāo)系是預(yù)先標(biāo)定出全站儀群之間的位置關(guān)系，定義全局坐標(biāo)系，從 而得到各全站儀自身坐標(biāo)系與全局坐標(biāo)系之間的變換關(guān)系以及目標(biāo)固連 體坐標(biāo)系相對全局坐標(biāo)系的初始狀態(tài)。
9. 根據(jù)權(quán)利要求5所述的動態(tài)測量的方法，其特征在于所述全站 儀組時(shí)滯參數(shù)是利用目標(biāo)固連體運(yùn)行一理論軌跡，同時(shí)用全站儀群進(jìn)行 測量，用測量數(shù)據(jù)與理論數(shù)據(jù)進(jìn)行相關(guān)處理得到全站儀組時(shí)滯參數(shù)。
10. 根據(jù)權(quán)利要求5所述的動態(tài)測量的方法，其特征在于所述數(shù) 據(jù)融合處理，是根據(jù)全站儀實(shí)時(shí)動態(tài)跟蹤測量時(shí)存在的時(shí)滯，采用時(shí)間 回溯算法，當(dāng)全站儀位姿修正數(shù)據(jù)到達(dá)時(shí)，將慣性測量數(shù)據(jù)回溯到全站 儀測量數(shù)據(jù)的真實(shí)時(shí)間點(diǎn)，以該時(shí)間點(diǎn)作為數(shù)據(jù)處理起算點(diǎn)，以全站儀 到達(dá)數(shù)據(jù)作為起算初始參數(shù)，用于消除時(shí)滯，從而計(jì)算出目標(biāo)固連體在 全局坐標(biāo)系的位置和姿態(tài)信息。
全文摘要
本發(fā)明公開組合式空間位姿精密動態(tài)測量裝置及方法，裝置包括慣性測量單元、全站儀測量單元以及分別與數(shù)據(jù)采集處理單元連接，數(shù)據(jù)采集處理單元與計(jì)算機(jī)連接。慣性測量單元測出目標(biāo)固聯(lián)體加速度、角速度，全站儀測量計(jì)算出目標(biāo)固連體位姿，通過數(shù)據(jù)采集處理單元發(fā)送給計(jì)算機(jī)，計(jì)算機(jī)根據(jù)全站儀數(shù)據(jù)到達(dá)時(shí)刻、測量時(shí)滯和慣性測量解算原理，采用時(shí)間回溯算法和KALMAN濾波，融合處理兩種數(shù)據(jù)，消除全站儀測量時(shí)滯，解算出最優(yōu)估計(jì)的目標(biāo)固聯(lián)體位置和姿態(tài)。本發(fā)明提高了現(xiàn)有全站儀測量系統(tǒng)的穩(wěn)定性和實(shí)時(shí)位姿測量精度，豐富現(xiàn)有動態(tài)跟蹤測量系統(tǒng)的測量輸出信息，滿足了大型裝置對大尺度空間的高精度、高采樣率、長時(shí)間運(yùn)行的實(shí)時(shí)測量需求。
文檔編號G01C3/00GK101608920SQ200810115160
公開日2009年12月23日 申請日期2008年6月18日 優(yōu)先權(quán)日2008年6月18日
發(fā)明者南仁東, 張志偉, 朱麗春, 王啟明, 翟學(xué)兵, 胡金文, 蔣志乾 申請人:中國科學(xué)院國家天文臺