一種多特征點位置姿態(tài)冗余解算方法
【專利摘要】本發(fā)明屬于物體位置姿態(tài)測量【技術(shù)領(lǐng)域】，具體涉及一種多特征點位置姿態(tài)冗余解算方法。方法以特征點在視覺測量系統(tǒng)中的三維坐標(biāo)為輸入條件，獲取數(shù)據(jù)后通過計算特征點到空間虛擬點的距離尋找匹配點對，若所有特征點均能找到匹配點對，則直接進(jìn)行去重心化操作；若無法找到匹配點對，則該點被自動舍棄，記錄匹配點對個數(shù)，動態(tài)調(diào)整后續(xù)算法數(shù)據(jù)入口大小，由剩余點解算該時刻的姿態(tài)位置信息；完成匹配后，通過去重心化實現(xiàn)平移信息和旋轉(zhuǎn)信息的分離，單獨解算旋轉(zhuǎn)矩陣。本發(fā)明解決了物體空間位置姿態(tài)測量的六自由度解算問題，提高算法精度的同時使算法保持較高的實時性能。
【專利說明】一種多特征點位置姿態(tài)冗余解算方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于物體位置姿態(tài)測量【技術(shù)領(lǐng)域】，具體涉及一種多特征點位置姿態(tài)冗余解 算方法。

【背景技術(shù)】
[0002] 隨著航空航天領(lǐng)域的迅猛發(fā)展，對飛行器運(yùn)動過程位置姿態(tài)測量的要求已經(jīng)越來 越廣泛，如航天器對接、級間分離姿態(tài)的測量等等。目前，主流方法是通過立體攝影測量技 術(shù)實現(xiàn)對飛行器上特征點的三維坐標(biāo)動態(tài)測量，進(jìn)而通過特征點的三維坐標(biāo)反推物體運(yùn)動 的六自由度姿態(tài)參數(shù)，這個過程即為姿態(tài)解算。
[0003] 傳統(tǒng)解算方法，按照特征點在運(yùn)動前后兩個坐標(biāo)系中的幾何關(guān)系建立方程求取姿 態(tài)的閉式解或通過優(yōu)化方法求取數(shù)值解。
[0004] 根據(jù)剛體運(yùn)動原理，只需三個特征點即可解算物體的姿態(tài)參數(shù)，但由于該方法的 求解方程建立在嚴(yán)格準(zhǔn)確的幾何約束基礎(chǔ)上，對噪聲干擾的敏感性高，一般采用增加特征 點的方式，提高數(shù)值解對噪聲干擾的免疫力。但特征點數(shù)目的增加又給不同時刻間的特征 點匹配帶來了困難，極易造成解算結(jié)果的錯誤；同時，使用數(shù)目較多的特征點來進(jìn)行迭代計 算，嚴(yán)重增加了計算耗時，限制了該方法的應(yīng)用范圍。


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

[0005] 本發(fā)明的目的在于提供一種求解物體六自由度位置姿態(tài)的冗余解算方法，此方法 根據(jù)剛體運(yùn)動學(xué)原理利用物體上多個特征點坐標(biāo)作為輸入條件，通過特征點匹配、去重心 化、多點平差等步驟，實現(xiàn)對物體位置姿態(tài)的快速精確求解。
[0006] 為達(dá)到上述目的，本發(fā)明所采取的技術(shù)方案為：
[0007] -種多特征點位置姿態(tài)冗余解算方法，根據(jù)剛體變換原理，物體姿態(tài)變化時物體 上的點坐標(biāo)滿足：
[0008] P/ = R*Pi+t
[0009] 其中R為旋轉(zhuǎn)矩陣，t為平移矩陣；

【權(quán)利要求】
1. 一種多特征點位置姿態(tài)兀余解算方法，其特征在于： 根據(jù)剛體變換原理，物體姿態(tài)變化時物體上的點坐標(biāo)滿足： P/ = R*Pi+t 其中R為旋轉(zhuǎn)矩陣，t為平移矩陣；
其中a，目，y分別為繞X，y，Z軸的旋轉(zhuǎn)角
，x,，y,，z,分別為沿X，y，z軸 的平移量； 該方法包括如下步驟： 步驟一：獲取初始點坐標(biāo)；獲取物體上特征點的H維坐標(biāo)，特征點的個數(shù)大于3個，設(shè) Ti時亥IJ測得的N點坐標(biāo)分別為： Pi (Xi，Yi, Zi)，尺2 (X2, 72, Z2)......Pn(X。，y。，zj， T2時亥Ij測得的N點坐標(biāo)分別為：
步驟二；特征點匹配；該匹配過程采用計算特征點到空間虛擬點的距離實現(xiàn)；設(shè)Ti時 刻空間虛擬點？。咕，y。，Z。)，取
，分別計算N個 點到P。的距離iPiPj、iPaPj......iPnPj ;設(shè)T2時刻空間虛擬點P。'（X。'，y。'，z。'），取
分別計算N個點到P。'的距離 P/P。' I、I P2'P。' I......IPn'P。' I;將 IPi'P。' I 與 IPAUIP2P」......|P"P」分別進(jìn) 行比較，距離相同或相差小于誤差限的點視為匹配點對，若無匹配點對，則舍棄該特征點； 將匹配好的特征點按照統(tǒng)一的順序進(jìn)行排列，后續(xù)計算過程依次讀取即可； 步驟H ;去重也化；物體姿態(tài)變化時物體上的點坐標(biāo)滿足： P/ = R沖 i+t (1) 上式對上述虛擬點亦成立，則； Pc' = R*Pc+t (2) 式（1)減去式（2)得： P/ -Pc' =帖化-Pc) 做 N點聯(lián)立后可得方程組：



步驟四：實現(xiàn)多點間的平差優(yōu)化。
2. 根據(jù)權(quán)利要求1所述的多特征點位置姿態(tài)兀余解算方法，其特征在于：所述步驟四 采用奇異值分解方法求解方程（4)得到旋轉(zhuǎn)矩陣R，在矩陣病態(tài)的情況下，求取最小二乘解 中存在的唯一最小范數(shù)解，實現(xiàn)多點間的平差優(yōu)化。
3. 根據(jù)權(quán)利要求2所述的多特征點位置姿態(tài)兀余解算方法，其特征在于：所述奇異值 分解具體方法為：將矩陣b=巧,奇異值分解，則r = u*vt，u、v為奇異值分解后的兩正 交矩陣，由式（1)求得平移矩陣t。
4. 根據(jù)權(quán)利要求1所述的多特征點位置姿態(tài)兀余解算方法，其特征在于：由旋轉(zhuǎn)矩陣R 求解旋轉(zhuǎn)角的過程通過非線性方程的優(yōu)化求解實現(xiàn)。
【文檔編號】G06F19/00GK104424382SQ201310367522
【公開日】2015年3月18日 申請日期:2013年8月21日 優(yōu)先權(quán)日:2013年8月21日
【發(fā)明者】孫增玉, 梁雅軍, 宋金城, 郭磊, 劉柯, 陳曉暉 申請人:北京航天計量測試技術(shù)研究所, 中國運(yùn)載火箭技術(shù)研究院