[發(fā)明專利]一種多特征點位置姿態(tài)冗余解算方法

有效

• 申請?zhí)枺?/td>: 201310367522.0
• 申請日: 2013-08-21
• 公開（公告）號: CN104424382B
• 公開（公告）日: 2017-09-29
• 發(fā)明（設(shè)計）人: 孫增玉;梁雅軍;宋金城;郭磊;劉柯;陳曉暉
• 申請（專利權(quán)）人: 北京航天計量測試技術(shù)研究所;中國運載火箭技術(shù)研究院
• 主分類號: G06F19/00
• 分類號: G06F19/00
• 代理公司: 核工業(yè)專利中心11007
• 代理人: 高尚梅

1.一種多特征點位置姿態(tài)冗余解算方法，其特征在于：

根據(jù)剛體變換原理，物體姿態(tài)變化時物體上的點坐標滿足：

P_i'＝R*P_i+t

其中R為旋轉(zhuǎn)矩陣，t為平移矩陣；

其中α，β，γ分別為繞x，y，z軸的旋轉(zhuǎn)角；x_t,y_t,z_t分別為沿x，y，z軸的平移量；

該方法包括如下步驟：

步驟一：獲取初始點坐標：獲取物體上特征點的三維坐標，特征點的個數(shù)大于3個，設(shè)T₁時刻測得的N點坐標分別為：

P₁(x₁，y₁,z₁)，P₂(x₂,y₂，z₂)......P_n(x_n,y_n，z_n)，

T₂時刻測得的N點坐標分別為：

步驟二：特征點匹配：該匹配過程采用計算特征點到空間虛擬點的距離實現(xiàn)：設(shè)T₁時刻空間虛擬點P_c(x_c，y_c，z_c)，取分別計算N個點到P_c的距離|P₁P_c|、|P₂P_c|......|P_nP_c|;設(shè)T₂時刻空間虛擬點P_c’(x_c’,y_c’,z_c’),取分別計算N個點到P_c'的距離|P₁’P_c’｜、｜P₂’P_c’|......|P_n’P_c’|;將|P₁’P_c’|與|P₁P_c|、 |P₂P_c|......|P_nP_c|分別進行比較，距離相同或相差小于誤差限的點視為匹配點對，若無匹配點對，則舍棄該特征點；將匹配好的特征點按照統(tǒng)一的順序進行排列，后續(xù)計算過程依次讀取即可；

步驟三：去重心化：物體姿態(tài)變化時物體上的點坐標滿足：

P_i'＝R*P_i+t(1)

上式對上述虛擬點亦成立，則：

P_c’＝R*P_c+t(2)

式（1）減去式（2）得：

P_i’-P_c’＝R*(P_i-P_c)(3)

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)冗余解算方法，其特征在于： 所述奇異值分解具體方法為：將矩陣奇異值分解，則R＝U*V^T，U、V為奇異值分解后的兩正交矩陣，由式（1）求得平移矩陣t。

4.根據(jù)權(quán)利要求1所述的多特征點位置姿態(tài)冗余解算方法，其特征在于：由旋轉(zhuǎn)矩陣R求解旋轉(zhuǎn)角的過程通過非線性方程的優(yōu)化求解實現(xiàn)。