非合作空間圓目標(biāo)的相對(duì)位姿測(cè)量方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及空間圓目標(biāo)的測(cè)量技術(shù)，特別涉及的是非合作空間圓目標(biāo)的相對(duì)位姿 測(cè)量技術(shù)。
【背景技術(shù)】
[0002] 隨著空間技術(shù)的發(fā)展，對(duì)故障或失效衛(wèi)星的空間在軌服務(wù)和碎片清除等，非合作 空間飛行器的近程操作活動(dòng)的需求越來(lái)越多。其發(fā)展趨勢(shì)是研制面向非合作目標(biāo)的自主空 間飛行器，通過(guò)自主接近非合作空間飛行器，以執(zhí)行空間在軌服務(wù)或清除任務(wù)。目前，非合 作空間飛行器中可供識(shí)別和對(duì)接抓捕的部位主要包括星箭對(duì)接環(huán)、遠(yuǎn)地點(diǎn)發(fā)動(dòng)機(jī)噴管、太 陽(yáng)帆板、對(duì)地通信天線支架等。星箭對(duì)接環(huán)、發(fā)動(dòng)機(jī)噴管等具有明顯的空間圓特征，為自主 完成空間服務(wù)任務(wù)，迫切需要針對(duì)這一類具有明顯空間圓特征非合作空間飛行器的超近距 離相對(duì)位姿測(cè)量技術(shù)。
[0003] 現(xiàn)有技術(shù)中，通過(guò)從二維圖像中提取橢圓來(lái)估計(jì)空間圓的姿態(tài)。橢圓檢測(cè)一般采 用基于非線性最小二乘擬合法實(shí)現(xiàn)，橢圓方程的一般數(shù)學(xué)模型如公式(9)所示：
[0004] Ax2+2Bxy+Cy2+2Dx+2Ey+l = 0 (9)
[0005] 其中要求AC-B2>0，A，B，C，D，E為待定的一般橢圓方程系數(shù)。將上式寫成矩陣形式 有：
[0006]
[0007] 假設(shè)(Xl，yi) 4 = 1，…，N為橢圓上的數(shù)據(jù)點(diǎn)即對(duì)應(yīng)圖像上二維坐標(biāo)點(diǎn)，橢圓擬合即 估計(jì)橢圓方程中的各個(gè)參數(shù)。如公式（11)所示，通過(guò)非線性最小二乘法即可解算出未知參 數(shù)A，B，C，D，E。
[0008]
[0009 ]令世界坐標(biāo)系0w-XwYwZw的原點(diǎn)位于圓心位置，其Z軸與圓所在平面的法向平行。參 看圖1，將世界坐標(biāo)系中的圓投影到相機(jī)成像坐標(biāo)系，世界坐標(biāo)系相機(jī)成像坐標(biāo) 系0-ΧΥΖ的變換關(guān)系如下：
[0010]
[0011 ]其中，[u，v，1 ]7為成像平面上點(diǎn)的齊次坐標(biāo)，P = [x，y，z，1 ]7為空間點(diǎn)的齊次坐標(biāo)， 多=[Wjf" H=K[rir2r3t]為平面單應(yīng)矩陣，K為相機(jī)的內(nèi)參數(shù)矩陣，R= [η，Γ2，Γ3]為空 間旋轉(zhuǎn)矩陣，t為平移向量，S為比例因子。
[0012] 圓在世界坐標(biāo)系下方程可表示為如（13)的矩陣形式：
[0013]
[0014]其中，r為圓的半徑。[0015]記Q = diag{l，l，-r2}，于是，圓投影方程為
[0016]
[0017]
[0018]
[0019] 其中，s為一比例因子。由方程(14)可知，圓投影方程中隱含了圓位姿參數(shù)，包括圓 心的位置t及圓所在平面的法向量Γ3。
[0020] 將方程(14)寫如下形式：
[0021] ^ + (t / r)(t / rf = 1- sK^lMK^r
[0022] 注意到等式左端矩陣秩<2且等式右端矩陣為實(shí)對(duì)稱矩陣，因此可作如下分解:1_ sK_1MK_ T=Ud i ag {λι, λ2,0} UT
[0023] 其中，U為3X3正交矩陣。通過(guò)解算可得到圓的三維姿態(tài)參數(shù)如下：
[0024] (15) _ ( 16,
[0026] 其中λχ,λ〗為I-sKHmIT1"的特征值，U為3*3的正交矩陣，Θ和φ為方便表示r3和t/r而 采用的數(shù)學(xué)符號(hào)，且滿足_=1，0砷=31/2,331/2，由于^3二1 >因此有如下等式成立：
[0027] Aicos29+A2cos2it= 1 ,Aicos29+A2sin29 = 1
[0028] 根據(jù)上述兩個(gè)等式可進(jìn)一步得到：
[0029] sin29 =(入1_1)/(入1_入2)
[0030] 由上式可確定Θ和φ，從而進(jìn)一步解算出姿態(tài)參數(shù)和位置參數(shù)。
[0031] 然而，在未引入其他輔助信息情況下，僅通過(guò)從二維圖像中提取橢圓來(lái)估計(jì)空間 圓的姿態(tài)，結(jié)果會(huì)產(chǎn)生歧義性，即會(huì)得到兩組視覺(jué)上均存在合理解釋的兩個(gè)解(法向量和圓 心位置）。其次，為獲取空間圓位置信息，需要已知空間圓半徑信息。然而在實(shí)際位姿測(cè)量過(guò) 程中，由于目標(biāo)為非合作目標(biāo)，難以得到先驗(yàn)信息，因此，傳統(tǒng)的方法在姿態(tài)解算過(guò)程中存 在二義性問(wèn)題;在位置解算過(guò)程中需要已知空間圓半徑。

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

[0032] 本發(fā)明所要解決的技術(shù)問(wèn)題是提供一種非合作空間圓目標(biāo)的相對(duì)位姿測(cè)量方法， 可解決傳統(tǒng)的方法在姿態(tài)解算過(guò)程中存在二義性問(wèn)題。
[0033] 此外，本發(fā)明的方法無(wú)需非合作空間圓目標(biāo)的先驗(yàn)信息即可實(shí)現(xiàn)其位置解算。
[0034] 為解決上述問(wèn)題，本發(fā)明提出一種非合作空間圓目標(biāo)的相對(duì)位姿測(cè)量方法，包括 以下步驟：
[0035] S1:提供左右相機(jī)拍攝空間圓目標(biāo)形成的左圖像和右圖像，其中，空間圓目標(biāo)在所 述左圖像、右圖像的成像坐標(biāo)系下分別成像為左橢圓、右橢圓；
[0036] S2:采用非線性最小二乘法擬合所述左圖像和右圖像，檢測(cè)出其中的橢圓或近似 橢圓，所述橢圓或近似橢圓作為候選橢圓；
[0037] S3:根據(jù)所述左橢圓和右橢圓之間的極線約束關(guān)系，從所述候選橢圓中識(shí)別出所 述左橢圓和右橢圓；
[0038] S4:根據(jù)步驟S2中擬合所述左橢圓和右橢圓而生成的左橢圓方程系數(shù)和右橢圓方 程系數(shù)，分別確定空間坐標(biāo)系到成像坐標(biāo)系的空間圓左投影方程和空間圓右投影方程，從 而獲得兩組空間圓姿態(tài)解；
[0039] S5:根據(jù)左右相機(jī)之間的位置姿態(tài)關(guān)系，將其中一組空間圓姿態(tài)解轉(zhuǎn)換到另一組 空間圓姿態(tài)解的空間坐標(biāo)系下，根據(jù)夾角最小原則確定空間圓目標(biāo)的最優(yōu)姿態(tài)解。
[0040] 根據(jù)本發(fā)明的一個(gè)實(shí)施例，還包括步驟S6:根據(jù)所述最優(yōu)姿態(tài)解確定空間圓目標(biāo) 的相對(duì)位置信息。
[0041] 根據(jù)本發(fā)明的一個(gè)實(shí)施例，在步驟S3中，所述極線約束關(guān)系為所述左橢圓和右橢 圓的圓心在圖像像素坐標(biāo)系下具有的確定位置關(guān)系。
[0042] 根據(jù)本發(fā)明的一個(gè)實(shí)施例，在步驟S1中，左右相機(jī)拍攝空間圓目標(biāo)形成的左圖像 和右圖像時(shí)，左右相機(jī)處于同一水平高度，左右相機(jī)之間保持一定基線長(zhǎng)度，左右相機(jī)光軸 分別內(nèi)傾相同角度，形成一拍攝夾角，所述拍攝夾角小于90度。
[0043] 根據(jù)本發(fā)明的一個(gè)實(shí)施例，在步驟S3中，所述極線約束關(guān)系為所述左橢圓和右橢 圓的圓心在圖像像素坐標(biāo)系下的y軸上的數(shù)值相同。
[0044] 根據(jù)本發(fā)明的一個(gè)實(shí)施例，在步驟S4中，確定任意一組空間圓姿態(tài)解的方法包括： 根據(jù)相應(yīng)投影方程推算出用以投影的空間圓所在平面的法向量r 3及圓心位置t與半徑r比 值，公式如（1)和(2):
[0045]
[0046]
[0047] 其中，心，\2為矩陣特征值，U為3X3正交矩陣，Θ和φ為用以表示r3和t/r而采用的參 數(shù)符號(hào)，且θ+Φ = π/2，3π/2，Γ/Γ3 二 1，UTU = I，因而1，A1cos29+A2sin29 = 1， 從而確定兩組Θ和Φ，得到兩個(gè)法向量解，作為一組空間圓姿態(tài)解。
[0048] 根據(jù)本發(fā)明的一個(gè)實(shí)施例，將根據(jù)空間圓左投影方程獲得的空間圓姿態(tài)解記為 rn，r12，將根據(jù)空間圓右投影方程獲得的空間圓姿態(tài)解記為m，rr2,在步驟S5中，將其中一 組空間圓姿態(tài)解的法向量轉(zhuǎn)換到另一組空間圓姿態(tài)解的空間坐標(biāo)系下的公式如(4):
[0049] riri = R〇rn
[0050] rir2 = R〇ri2 (4)
[0051] 根據(jù)兩組最優(yōu)法向量之間的夾角最小原則，如公式（5)，確定一組最優(yōu)法向量，記 為Γ3，
[0052]
[0053] 空間圓法向量r3確定后根據(jù)公式(6)確定出偏航角f和俯仰角Φ，作為空間圓目標(biāo) 的最優(yōu)姿態(tài)解，
[0054]
[0055] 還包括步驟S6:由公式（6)確定空間圓目標(biāo)的圓心位置t與半徑r比值，根據(jù)公式 (7)和(8)確定空間圓目標(biāo)相對(duì)于左相機(jī)或右相機(jī)的圓心位置矢量，作為相對(duì)位置信息，
[0060] ti = r*[tix tiy tiz]T
[0061] tr = r*[trx try trz]T (8)
[0062] 其中，^」（1 = 0，1，2，]_ = 0，1，2)為旋轉(zhuǎn)矩陣辦中的元素，[乜\七^(guò)七12]7為左相機(jī) 空間坐標(biāo)系下的空間圓目標(biāo)的圓心位置坐標(biāo)與半徑之比之后的三維