[發明專利]一種基于特征反向投影的自主空中加油錐套位姿解算方法

有效

• 申請號: 202111139017.1
• 申請日: 2021-09-27
• 公開（公告）號: CN113850866B
• 公開（公告）日: 2023-06-30
• 發明（設計）人: 馬躍博;顏坤;趙汝進;劉恩海;朱自發;易晉輝;曾思康;唐雨萍
• 申請（專利權）人: 中國科學院光電技術研究所
• 主分類號: G06T7/73
• 分類號: G06T7/73;G06T7/13
• 代理公司: 北京科迪生專利代理有限責任公司 11251
• 代理人: 江亞平

1.一種基于特征反向投影的自主空中加油錐套位姿解算方法，其特征在于：

該方法包括如下步驟：

步驟(1)：對錐套目標進行成像，并獲取目標圖像；

所述錐套目標具有錐臺構型，該錐臺構型的上下平行的兩個端面分別為圓面和圓環，即頂面圓和底面圓，已知該兩個端面的圓的半徑和中心所在目標坐標系中的三維坐標，根據中心坐標和半徑獲取錐套的結構特征參數如下：

p_cb＝(x_cb，y_cb，z_cb)z_cb＝0

p_ct＝(0,0,d)

其中，x_w、y_w和z_w為錐套的目標坐標系坐標，r_b為錐套底面圓半徑，r_t為錐套頂面圓半徑，d為錐臺高度，p_cb為底面圓中心坐標，p_ct為頂面圓中心坐標；

步驟(2)：提取錐套結構特征：

將步驟(1)獲取的目標圖像進行特征識別，提取圖像中目標的結構特征點坐標集合，所述結構特征點坐標集合包括：底面圓特征點集{P_b}、底面圓中心點成像位置集{P_cb}、頂面圓中心點成像位置集{P_ct}；

步驟(3)：實施特征空間反向投影：

已知單目相機等效焦距為f，定義所述目標的結構特征點坐標集合在相機坐標系下的坐標集分別為底面圓邊緣點坐標集合{A_b}、底面圓中心點坐標{A_cb}和頂面圓中心點坐標{A_ct}，錐套目標坐標系到相機坐標系的位姿轉移矩陣為T，由步驟(2)獲取的目標結構特征點的坐標為P_i＝(u_i,v_i)，則相機坐標系下的三維坐標表示為(P_i,f)＝(u_i,v_i,f)，相機坐標系下的特征點坐標根據小孔成像原理，結合位姿轉移矩陣T和相機的內參數矩陣K將特征點反向投影到三維空間；

其中，

其中u₀和v₀為相機主點位置，R為旋轉矩陣，t為平移向量，T為轉移矩陣；

特征點反向投影到三維空間的表達式為：

根據先驗知識，由于錐臺結構的旋轉不變性，設置旋轉角為0，得到反向投影方程為：

f(θ,φ,t_x,t_y,t_z,x′_w,y′_w,z′_w|u,v)＝0

上式中，θ、φ分別為目標坐標系相對于相機坐標系的俯仰角和偏航角，能構成旋轉姿態矩陣R，t_x、t_y和t_z構成平移向量t＝[t_x,t_y,t_z]^T，p_w＝(x′_w,y′_w,z′_w)為圖像特征點反向投影在三維空間中坐標值，(u,v)為圖像中錐套結構特征的坐標；

同時根據錐臺構型頂面圓的中心圖像特征點(u_ct,v_ct)和目標坐標系錐臺高度d對所述反向投影方程化簡得到：

f₁(θ,φ,t_z,x′_w,y′_w,z′_w|u,v)＝0

根據該底面圓的約束方程可知z′_w＝0，結合簡化后的反向投影方程計算得到反向投影方程為：

f₂(θ,φ,t_z,x′_w,y′_w|u,v)＝0；

步驟(4)：基于錐套結構約束關系解算位姿：

基于錐套結構的約束關系構建目標函數：

其中，r_b為底面圓半徑，為估計底面圓半徑，x′_w和y′_w為估計的目標坐標系下底面圓輪廓坐標；

通過對待求位姿參數變量X＝[θ,φ,t_z]^T的求導得到目標函數的雅可比矩陣J，從而計算得到位姿參數的修正量ΔX，過程表示如下：

J_XΔX＝J

其中，J_X為雅可比矩陣對X的偏導；

根據上述計算得到的修正量ΔX，對位姿參數進行迭代解算：

X^k＝X^k-1+ΔX

經過k次迭代，當所有的底面圓約束和中心點位置約束都能同時滿足時，目標函數迭代過程逐漸收斂為零，從而解算得到最優的位姿參數變量X；

在得到X的基礎之上根據上述反向投影方程簡化方程計算得到目標的姿態角和平移向量t＝(t_x,t_y,t_z)。