[發明專利]一種嵌入式的雙目非合作目標相對位姿測量方法

有效

• 申請號: 202010324661.5
• 申請日: 2020-04-23
• 公開（公告）號: CN111536981B
• 公開（公告）日: 2023-09-12
• 發明（設計）人: 段慧仙;裴浩東;劉云猛;周姍姍;項慶;陸佳琪
• 申請（專利權）人: 中國科學院上海技術物理研究所
• 主分類號: G01C21/24
• 分類號: G01C21/24
• 代理公司: 北京恒律知識產權代理有限公司 11416
• 代理人: 劉鳳玲

1.一種嵌入式的雙目非合作目標相對位姿測量方法，其特征在于，所述的方法包括以下步驟：

1)：利用黑白棋盤格對雙目相機進行離線標定，得到雙目相機的內外參數和畸變參數，具體步驟如下：

1-1)：坐標系的建立主要包括雙目相機坐標系和非合作目標主體坐標系，雙目相機坐標系{O_c-X_cY_cZ_c}：左相機的光心作為原點O_c；左右相機光心連線，并指向右相機作為X_c軸；左相機光軸作為Z_c軸；通過原點O_c，X_c軸和Z_c軸的叉乘作為Y_c軸；

非合作目標主體坐標系{O_s-X_sY_sZ_s}：非合作目標主體上對接環中心作為原點O_s；經過原點O_s，平行于對接環表面的法向量并指向雙目相機作為Z_s軸；經過原點O_s，對接環中心與左上角邊框角點A的連線為X_s軸；經過原點O_s，X_s軸和Z_s軸的叉乘作為Y_s軸；

1-2)：離線標定是指利用張正友提出的基于黑白棋盤格的雙目立體視覺系統標定算法獲取雙目相機的內外參數和畸變參數，左相機內參數包括焦距和主點坐標和斜率s^l；右相機內參數包括焦距和主點坐標和斜率s^r；雙目相機外參數包括右相機相對于左相機的旋轉矩陣R和平移向量T；畸變參數包括徑向畸變參數{k₁，k₂，k₃}和切向畸變參數{p₁，p₂}；

徑向畸變模型為：

切向畸變模型為：

其中，(x，y)為圖像平面上的點，(x_corrected，y_corrected)為畸變校正后的點，r²＝x²+y²，{k₁，k₂，k₃}為徑向畸變參數，{p₁，p₂}為切向畸變參數；

2)：利用兩塊圖像處理板分別對左右相機采集的非合作目標圖像進行處理，提取非合作目標主體的邊框角點和對接環；具體步驟如下：

2-1)：左右圖像預處理是指對左右相機采集的非合作目標圖像進行畸變矯正、二值化、模糊化、邊緣檢測、提取邊緣、加粗邊緣和邊緣內部填充；

2-2)：主體邊框角點提取是指在圖像預處理基礎上利用霍夫直線變換尋找目標主體邊框的四條邊緣及其角點，首先，利用霍夫直線變換篩選累加器大于閾值的所有直線；其次，根據直線的角度分為平行直線和垂直直線，在平行直線和垂直直線中分別利用聚類成兩類；然后，在兩類直線中選取其中累加器值最大的直線，即為目標主體的四條邊框直線；之后，在尋找到目標主體的四條邊框直線后，計算出四個角點；最后，以左上角點作為第一個點，按順時針順序排列四個角點，其中左圖像四個角點記為右圖像四個角點記為

2-3)：主體對接環提取是指在圖像預處理基礎上利用霍夫橢圓檢測方法提取目標主體上的對接環，并得到對接環中心；根據邊緣檢測擬合對接環時，去掉邊緣點太少的橢圓，去掉長軸和短軸之比太大的橢圓，去掉長短軸太長和太短的橢圓，去掉橢圓中心點距離太近的橢圓，去掉邊緣點在橢圓上的點占邊緣點總數較少的點，得到的橢圓即為目標主體上的對接環；最后，根據提取的目標主體上對接環，計算出左圖像對接環中心坐標和右圖像對接環中心坐標

2-4)：主體表面特征點提取是指利用特征點提取算法快速提取左圖像目標主體表面特征點和右圖像目標主體表面特征點i＝1,2,…,N，其中N為特征點的個數；

3)：利用一塊主控板對2)中提取的左右圖像特征進行匹配，結合1)中得到的雙目相機參數恢復匹配特征的三維坐標；

3-1)：左右圖像特征匹配是指將圖像處理板傳輸到主控板上的左右圖像特征點，基于雙目極線約束找到左右圖像中對應的匹配特征點；

3-2)：恢復匹配特征的三維坐標是指利用已標定的雙目相機參數計算出匹配特征在雙目相機坐標系下的三維坐標；和分別為左右圖像中一對匹配的特征點，其在雙目相機坐標系下的三維坐標記為M_i；則

其中，和是常數，和分別為左右相機內參數矩陣，I是單位矩陣，T₀＝[0?0?0]^T，R和T是右相機相對于左相機坐標系的旋轉矩陣和平移向量；

記

由上式可得

其中，A_i為系數矩陣，則通過最小二乘法解算出匹配特征點對在雙目相機坐標系下的三維坐標M_i，從而，計算出對接環中心對應的三維坐標為M_doc；邊框角點對應的三維坐標為M_cori，i＝1,2,3,4；

4)：在主控板上，根據3)中匹配特征的三維坐標，計算得到非合作目標相對于雙目相機的位置和姿態；

相對位姿實時計算是指通過匹配特征點的三維坐標計算出目標坐標系與雙目相機坐標系之間的相對位姿{R_rel?T_rel}；通過1-1)建立的坐標系可知，在非合作目標主體坐標系下，對接環中心坐標為O_s，邊框四個角點的坐標為M_corsi，i＝1,2,3,4；則

M_cori＝R_relM_corsi+T_rel，i＝1,2,3,4

T_rel＝-M_doc

即解算出非合作目標在雙目相機坐標系下的相對位姿。