[發明專利]一種多特征點位置姿態冗余解算方法

有效

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

技術領域

本發明屬于物體位置姿態測量技術領域，具體涉及一種多特征點位置姿態冗余解算方法。

背景技術

隨著航空航天領域的迅猛發展，對飛行器運動過程位置姿態測量的要求已經越來越廣泛，如航天器對接、級間分離姿態的測量等等。目前，主流方法是通過立體攝影測量技術實現對飛行器上特征點的三維坐標動態測量，進而通過特征點的三維坐標反推物體運動的六自由度姿態參數，這個過程即為姿態解算。

傳統解算方法，按照特征點在運動前后兩個坐標系中的幾何關系建立方程求取姿態的閉式解或通過優化方法求取數值解。

根據剛體運動原理，只需三個特征點即可解算物體的姿態參數，但由于該方法的求解方程建立在嚴格準確的幾何約束基礎上，對噪聲干擾的敏感性高，一般采用增加特征點的方式，提高數值解對噪聲干擾的免疫力。但特征點數目的增加又給不同時刻間的特征點匹配帶來了困難，極易造成解算結果的錯誤；同時，使用數目較多的特征點來進行迭代計算，嚴重增加了計算耗時，限制了該方法的應用范圍。

發明內容

本發明的目的在于提供一種求解物體六自由度位置姿態的冗余解算方法，此方法根據剛體運動學原理利用物體上多個特征點坐標作為輸入條件，通過特征點匹配、去重心化、多點平差等步驟，實現對物體位置姿態的快速精確求解。

為達到上述目的，本發明所采取的技術方案為：

一種多特征點位置姿態冗余解算方法，根據剛體變換原理，物體姿態變化時物體上的點坐標滿足：

P_i’＝R*P_i+t

其中R為旋轉矩陣，t為平移矩陣；

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

該方法包括如下步驟：

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

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

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

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

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

P_i’＝R*P_i+t(1)

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

P_c’＝R*P_c+t(2)

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