基于單目視覺的非合作航天器相對(duì)位置和姿態(tài)測量算法

張勁鋒，孫承啟，蔡 偉

（1.北京控制工程研究所，北京100190；2.空間智能控制技術(shù)國家級(jí)重點(diǎn)實(shí)驗(yàn)室，北京100190）

基于單目視覺的非合作航天器相對(duì)位置和姿態(tài)測量算法

張勁鋒1，2，孫承啟1，2，蔡 偉1

（1.北京控制工程研究所，北京100190；2.空間智能控制技術(shù)國家級(jí)重點(diǎn)實(shí)驗(yàn)室，北京100190）

對(duì)非合作航天器的相對(duì)位置和姿態(tài)測量問題進(jìn)行了研究，提出了一種新的單目視覺測量算法——相似迭代算法.該算法根據(jù)相似三角形原理推導(dǎo)出一種新的深度迭代機(jī)制，通過深度估計(jì)和絕對(duì)方位問題求解兩個(gè)步驟對(duì)相對(duì)位置和姿態(tài)參數(shù)進(jìn)行迭代計(jì)算.數(shù)值仿真結(jié)果表明了該算法的有效性.

單目視覺；非合作航天器；相對(duì)位置和姿態(tài)

在空間交會(huì)對(duì)接、在軌服務(wù)等航天任務(wù)的最后逼近段，通常采用光學(xué)成像敏感器來測量追蹤航天器與目標(biāo)航天器之間的相對(duì)位置和姿態(tài)（以下簡稱相對(duì)位姿），國內(nèi)外許多學(xué)者已經(jīng)對(duì)合作目標(biāo)航天器（目標(biāo)航天器上安裝有目標(biāo)標(biāo)識(shí)器）的視覺測量算法進(jìn)行了大量研究［1-3］.針對(duì)合作目標(biāo)的相對(duì)位姿測量方法雖有精度高、抗干擾能力強(qiáng)的特點(diǎn)，但其作用距離受限于合作目標(biāo)尺寸，且只能對(duì)裝有目標(biāo)標(biāo)識(shí)器的航天器進(jìn)行測量.而針對(duì)非合作目標(biāo)的相對(duì)位姿測量方法，不僅可對(duì)合作目標(biāo)測量方法形成冗余，而且可對(duì)沒有安裝目標(biāo)標(biāo)識(shí)器的非合作或敵對(duì)目標(biāo)（統(tǒng)稱為非合作目標(biāo)）航天器的相對(duì)位姿進(jìn)行測量.該方法本身具有重要的實(shí)際應(yīng)用價(jià)值和潛在的軍事用途，已經(jīng)引起了國內(nèi)外許多學(xué)者的關(guān)注［4-5］.

本文從非合作目標(biāo)測量的實(shí)際需求出發(fā)，對(duì)非合作航天器相對(duì)位姿測量問題進(jìn)行了研究，提出了一種新的單目視覺測量算法.由于該算法的主要特點(diǎn)是根據(jù)三角形相似原理提出一種新的深度迭代機(jī)制，因此本文稱之為相似迭代算法.該算法的每一個(gè)步驟均有多種實(shí)現(xiàn)方法，本文利用直接線性變換算法［6］來求解深度初值，利用單位四元數(shù)求解絕對(duì)方位問題［7］.

1 問題描述

假設(shè)攝像機(jī)已經(jīng)過標(biāo)定，如圖1所示，目標(biāo)坐標(biāo)系為OoXoYoZo，攝像機(jī)坐標(biāo)系為OcXcYcZc，焦距為f，z軸垂直于圖像平面G.平面H通過目標(biāo)坐標(biāo)系原點(diǎn)Oo且平行于圖像平面G，H與攝像機(jī)坐標(biāo)系原點(diǎn)Oc的距離為zc0，即點(diǎn)Oo在攝像機(jī)坐標(biāo)系中的z軸坐標(biāo).目標(biāo)上點(diǎn)Pi在平面H上的正交投影點(diǎn)為Mi，Mi在圖像平面上的透視投影為點(diǎn)mi，即點(diǎn)Pi的弱透視投影點(diǎn)為mi，其透視投影為點(diǎn)pi.分別從點(diǎn)Pi、Mi和pi向軸OcZc作垂線，垂足分別為點(diǎn)E、D和C.

現(xiàn)給定三維目標(biāo)上的一組特征點(diǎn)Pi，i＝1，2，…，n，n≥6均位于攝像機(jī)的視場內(nèi)，令特征點(diǎn)Pi在目標(biāo)坐標(biāo)系中的坐標(biāo)poi＝［xoi，yoi，zoi］T，在攝像機(jī)坐標(biāo)系中的坐標(biāo)為pci＝［xci，yci，zci］T，則

其中，

圖1 相關(guān)坐標(biāo)系和攝像機(jī)成像模型

展開式（1），可得

在攝像機(jī)坐標(biāo)系中，點(diǎn)Pi經(jīng)過透視變換，得到對(duì)應(yīng)的2維投影圖像點(diǎn)pi，令點(diǎn)pi在圖像坐標(biāo)系中的坐標(biāo)為［ui，vi］T，則

將式（3）代入式（4），可得

要解決的問題就是在已知目標(biāo)特征點(diǎn)坐標(biāo)及其對(duì)應(yīng)的圖像坐標(biāo)的條件下，根據(jù)式（5）所表達(dá)的非線性約束，求解未知的姿態(tài)旋轉(zhuǎn)矩陣R和平移向量t.

2 相似迭代算法

本文提出的相似迭代算法包括深度估計(jì)和絕對(duì)方位問題求解兩個(gè)步驟.在深度估計(jì)步驟，利用新的深度迭代公式估計(jì)新的深度值；在絕對(duì)方位問題求解步驟，由上一步得到的深度值構(gòu)造所謂的絕對(duì)方位問題，求解出相對(duì)位姿參數(shù).對(duì)上述兩個(gè)步驟進(jìn)行反復(fù)迭代，直至滿足一定的收斂條件.對(duì)于迭代初值可以用多種算法來求解，由于直接線性變換算法［6］較為簡單，速度快，因此本文采用該算法用于求解迭代初值.

2.1 深度估計(jì)

由圖1所示，在攝像機(jī)坐標(biāo)系中，令di表示向量的長度，這里稱為點(diǎn)Pi的深度值.在ΔOcEPi中，由于Cpi∥EPi，根據(jù)三角形的相似性，有

又已知

可得

顯然，若已計(jì)算出r3和t3，則可利用上式迭代計(jì)算各個(gè)特征點(diǎn)的深度值di.

2.2 絕對(duì)方位問題求解

令si表示向量方向上的單位向量，即

則點(diǎn)Pi在攝像機(jī)坐標(biāo)系中的坐標(biāo)為

現(xiàn)在已知目標(biāo)特征點(diǎn)Pi在目標(biāo)坐標(biāo)系中的坐標(biāo)poi及其在攝像機(jī)坐標(biāo)系中的坐標(biāo)pci，要求解目標(biāo)坐標(biāo)系與攝像機(jī)坐標(biāo)系之間的剛體變換，即旋轉(zhuǎn)矩陣R與平移向量t，該問題常稱之為絕對(duì)方位問題［7］.

求解絕對(duì)方位問題有多種算法，文獻(xiàn)［7］利用單位四元數(shù)表達(dá)旋轉(zhuǎn)矩陣，給出了問題的閉式解，本文采用該算法來求解絕對(duì)方位問題.

1）計(jì)算兩個(gè)三維點(diǎn)集的均值，將數(shù)據(jù)歸一化

2）構(gòu)造反對(duì)稱矩陣

式中（·）×表示向量對(duì)應(yīng)的叉乘矩陣.

3）對(duì)矩陣A進(jìn)行特征值分解，則矩陣A的最小特征值對(duì)應(yīng)的特征向量即為所求的單位四元數(shù)q.將單位四元數(shù)q轉(zhuǎn)化為旋轉(zhuǎn)矩陣R，則所求的平移向量為

2.3 求解深度初值

本文利用直接線性變換算法［6］來求解初始的相對(duì)位姿參數(shù)R和t，然后由式（9）計(jì)算深度的初始值.

將相對(duì)位姿參數(shù)R和t組合成一個(gè)12維向量h，即

則式（5）可改寫為如下線性方程組：

式中2n×12系數(shù)矩陣A為

直接線性變換算法就是求解合適的h，使得‖Ah‖為最小，一般約束條件取‖h‖＝1，利用拉格朗日乘子法，可得下面的無約束優(yōu)化目標(biāo)函數(shù)

對(duì)上式求h的偏導(dǎo)數(shù)，并令其為0，可得

可得

可知，所求的向量h是對(duì)應(yīng)于對(duì)稱矩陣ATA的最小特征值λ的特征向量.

利用上述算法求得旋轉(zhuǎn)矩陣R和平移向量t后，就可由式（9）對(duì)深度值進(jìn)行迭代更新.如果前后兩次迭代的深度誤差小于某個(gè)閾值，則停止迭代，并輸出相對(duì)位姿參數(shù)R和t；否則繼續(xù)進(jìn)行迭代循環(huán).

3 數(shù)值仿真

為了驗(yàn)證本文提出的相似迭代算法的有效性，利用Matlab軟件進(jìn)行了數(shù)值仿真.

為了簡化計(jì)算，取非合作航天器的本體為1×1×1的立方體，航天器帶兩個(gè)太陽帆板，如圖2所示.

圖2 仿真用的目標(biāo)航天器模型

將航天器的18個(gè)頂點(diǎn)作為目標(biāo)特征點(diǎn)，各特征點(diǎn)在目標(biāo)坐標(biāo)系中的坐標(biāo)如表1所示，這里不考慮遮擋問題.

表1 特征點(diǎn)在目標(biāo)航天器坐標(biāo)系中的坐標(biāo)

固定平移分量t1＝5，t2＝5，以t3為變量，取步長為1，在5～500內(nèi)變化；在每個(gè)距離點(diǎn)處，3個(gè)旋轉(zhuǎn)角θ1、θ2和θ3按照均勻分布在－30°～30°間隨機(jī)取值，并產(chǎn)生1 000個(gè)旋轉(zhuǎn)矩陣.由得到的旋轉(zhuǎn)矩陣和平移向量分別對(duì)特征點(diǎn)進(jìn)行剛體變換和透視投影變換，得到對(duì)應(yīng)的圖像特征點(diǎn).假設(shè)圖像噪聲的方差σ與信噪比τ的關(guān)系為

取信噪比為80 dB，由式（21）算出不同距離處的圖像噪聲方差σ，在圖像點(diǎn)坐標(biāo)中加入均值為0、方差為σ的高斯隨機(jī)噪聲.利用本文提出的相似迭代算法計(jì)算出3個(gè)旋轉(zhuǎn)角和3個(gè)平移分量在不同距離處的平均誤差，仿真結(jié)果如圖3和圖4所示.

圖3 相對(duì)姿態(tài)誤差隨目標(biāo)距離的變化

圖4 相對(duì)位置誤差隨目標(biāo)距離的變化

由圖3和圖4可知，相對(duì)位姿的平均誤差均隨著目標(biāo)距離的增大而增大，3個(gè)旋轉(zhuǎn)角的最大平均誤差不超過0.5°，平移分量的最大相對(duì)誤差不超過1%，能夠基本滿足非合作航天器相對(duì)位姿參數(shù)的測量要求.如果星載計(jì)算機(jī)運(yùn)算能力和時(shí)間允許，還可以采用更為精確的迭代初值或適當(dāng)增加迭代次數(shù)來獲得更高的測量精度.

4 結(jié) 論

本文對(duì)非合作航天器的相對(duì)位姿測量問題進(jìn)行了研究，提出了一種新的單目視覺測量算法——相似迭代算法.數(shù)值仿真結(jié)果表明該算法具有精度高、抗噪聲能力強(qiáng)的優(yōu)點(diǎn)，能夠基本滿足非合作航天器相對(duì)位姿測量的需求.此外，該算法還提供了一種靈活的分步實(shí)現(xiàn)框架，其中的每個(gè)步驟可分別用多種方法來實(shí)現(xiàn)，有利于算法的進(jìn)一步改進(jìn)和推廣.

［1］ Ho C C，Mc-Clamroch N H.Aspacecraft docking problem：position estimation using a computer vision approach［C］.AIAA Guidance，Navigation and Control Conference，Portland，USA，Aug 1990

［2］ Calhoun P C，Dabney R.Solution to the problem of determining the relative 6 DOF state for spacecraft automated rendezvous and docking［J］.SPIE，1995，2466：175-184

［3］ 林來興，李燦.交會(huì)對(duì)接逼近階段CCD相機(jī)的測量方法［J］.宇航學(xué)報(bào)，1994，15（2）：24-34

［4］ Cropp A，Palmer P L，Mc-Lauchlan P，et al.Estimating the pose of a known target satellite［J］.IEE Electronic Letters，2000，36（15）：1331-1332

［5］ 張世杰，曹喜濱，陳閩.非合作航天器間相對(duì)位姿的單目視覺確定算法［J］.南京理工大學(xué)學(xué)報(bào)，2006，30（5）：564-568

［6］ Hartley R，Zisserman A.Multiple view geometry in computer vision［M］.Cambridge：Cambridge University Press，2003

［7］ Horn B K.Closed-form solution of absolute orientation using unit quaternion［J］.Journal of the Optical Society of America，1987，4（4）：629-642

Monocular Vision-Based Relative Position and Attitude Measurement Algorithm for Non-Cooperative Spacecrafts

ZHANG Jinfeng1，2，SUN Chengqi1，2，CAIWei1
（1.Beijing Institute of Control Engineering，Beijing 100190，China；2.National Key Laboratory of Science and Technology on Space Intelligent Control，Beijing 100190，China）

Anovel monocular vision-based relative position and attitudemeasurement algorithm named as the Sim ilar Iteration algorithm is presented for non-cooperative spacecrafts in this paper.The algorithm derives a new depth iteration mechanism from principle of triangular similitude and involves two steps，i.e.the depth estimation and solving the absolute orientation problem.Numerical simulations have demonstrated the effectiveness of the proposed algorithm.

monocular vision；non-cooperative spacecraft；relative position and attitude

V448.2

A

1674-1579（2009）06-0050-04

2009-02-25

張勁鋒（1979—），男，河南人，博士研究生，研究方向?yàn)榭臻g目標(biāo)識(shí)別與相對(duì)測量（e-mail：zjf519＠yahoo.com.cn）.