[發(fā)明專利]基于雙目視覺的導(dǎo)彈裝填位姿偏差檢測系統(tǒng)和方法

在審

• 申請?zhí)枺?/td>: 202211377870.1
• 申請日: 2022-11-04
• 公開（公告）號: CN115713554A
• 公開（公告）日: 2023-02-24
• 發(fā)明（設(shè)計）人: 曾遠帆;莊國平;殷翔;繆東輝;蔣道民;魯勇軍;范文晶;牛遠征;劉禹華;張勝
• 申請（專利權(quán)）人: 上海機電工程研究所
• 主分類號: G06T7/70
• 分類號: G06T7/70;G06T7/80
• 代理公司: 上海漢聲知識產(chǎn)權(quán)代理有限公司 31236
• 代理人: 胡晶

1.一種基于雙目視覺的導(dǎo)彈裝填位姿偏差檢測方法，其特征在于，包括：

步驟S1、標定吊具數(shù)據(jù)：測量吊具靶標組與吊具坐標系之間的位姿關(guān)系，記錄在相機控制器中；

步驟S2、標定導(dǎo)彈托架數(shù)據(jù)：測量導(dǎo)彈托架靶標組與導(dǎo)彈托架坐標系之間的位姿關(guān)系，記錄在相機控制器中；

步驟S3、標定裝填參考位姿：當?shù)蹙咦コ謱?dǎo)彈模塊定位在導(dǎo)彈托架上時，使用雙目相機同時拍攝吊具靶標組和導(dǎo)彈托架靶標組，由相機控制器計算并記錄吊具坐標系和導(dǎo)彈托架坐標系的相對位姿，作為導(dǎo)彈裝填作業(yè)時的參考位姿；

步驟S4、裝填作業(yè)時檢測實時位姿：在導(dǎo)彈裝填作業(yè)時，使用雙目相機同時拍攝吊具靶標組和導(dǎo)彈托架靶標組，由相機控制器計算吊具坐標系和導(dǎo)彈托架坐標系的實時位姿；

步驟S5、計算裝填位姿偏差：使用相機控制器根據(jù)標定的裝填作業(yè)參考位姿和檢測得到的裝填實時位姿，計算得到吊具坐標系實時位姿與參考位姿之間的位姿偏差。

2.根據(jù)權(quán)利要求1所述的基于雙目視覺的導(dǎo)彈裝填位姿偏差檢測方法，其特征在于，在所述步驟S1中：在相機坐標系下測量并構(gòu)造吊具坐標系，將此時吊具坐標系在相機坐標系下的位姿矩陣記作設(shè)吊具靶標組的靶標個數(shù)為m，將此時吊具靶標組的各靶標在相機坐標系下的三維坐標記作

在所述步驟S2中：在相機坐標系下測量并構(gòu)造導(dǎo)彈托架坐標系，將此時導(dǎo)彈托架坐標系在相機坐標系下的位姿矩陣記作設(shè)導(dǎo)彈托架靶標組的靶標個數(shù)為n，將此時導(dǎo)彈托架靶標組的各靶標在相機坐標系下的三維坐標記作

3.根據(jù)權(quán)利要求2所述的基于雙目視覺的導(dǎo)彈裝填位姿偏差檢測方法，其特征在于，在所述步驟S3中，將此時吊具靶標組的各靶標在相機坐標系下的三維坐標記作導(dǎo)彈托架靶標組的各靶標在相機坐標系下的三維坐標記作使用如下步驟計算吊具坐標系和導(dǎo)彈托架坐標系的參考相對位姿：

步驟S3.1、坐標歸一化：將各靶標的標定值與實測值按照如下公式進行歸一化處理：

表示出廠前，吊具靶標組與吊具坐標系之間的位姿關(guān)系下，吊具靶標組的第i個靶標在相機坐標系下的歸一化坐標；

表示出廠前，吊具靶標組與吊具坐標系之間的位姿關(guān)系下，吊具靶標組的第i個靶標在相機坐標系下的三維坐標；

表示出廠前，吊具靶標組與吊具坐標系之間的位姿關(guān)系下，吊具靶標組的第k個靶標在相機坐標系下的三維坐標；

表示出廠前，吊具抓持導(dǎo)彈模塊定位在導(dǎo)彈托架上時，吊具靶標組的第i個靶標在相機坐標系下的歸一化坐標；

表示出廠前，吊具抓持導(dǎo)彈模塊定位在導(dǎo)彈托架上時，吊具靶標組的第i個靶標在相機坐標系下的三維坐標；

表示出廠前，吊具抓持導(dǎo)彈模塊定位在導(dǎo)彈托架上時，吊具靶標組的第k個靶標在相機坐標系下的三維坐標；

表示出廠前，導(dǎo)彈托架靶標組與導(dǎo)彈托架坐標系之間的位姿關(guān)系下，導(dǎo)彈托架靶標組的第j個靶標在相機坐標系下的歸一化坐標；

表示出廠前，導(dǎo)彈托架靶標組與導(dǎo)彈托架坐標系之間的位姿關(guān)系下，導(dǎo)彈托架靶標組的第j個靶標在相機坐標系下的三維坐標；

表示出廠前，導(dǎo)彈托架靶標組與導(dǎo)彈托架坐標系之間的位姿關(guān)系下，導(dǎo)彈托架靶標組的第k個靶標在相機坐標系下的三維坐標；

表示出廠前，吊具抓持導(dǎo)彈模塊定位在導(dǎo)彈托架上時，導(dǎo)彈托架靶標組的第j個靶標在相機坐標系下的歸一化坐標；

表示出廠前，吊具抓持導(dǎo)彈模塊定位在導(dǎo)彈托架上時，導(dǎo)彈托架靶標組的第j個靶標在相機坐標系下的三維坐標；

表示出廠前，吊具抓持導(dǎo)彈模塊定位在導(dǎo)彈托架上時，導(dǎo)彈托架靶標組的第k個靶標在相機坐標系下的三維坐標；

步驟S3.2、計算吊具坐標系及導(dǎo)彈托架坐標系在相機坐標系下的位姿矩陣H_s、H_f：由歸一化坐標構(gòu)造如下矩陣和分別進行奇異值分解得：

H_s＝U_sΣ_sV_s^T,H_f＝U_fΣ_fV_f^T (3)

U_s、U_f表示左奇異矩陣；

Σ_s、Σ_f表示對角矩陣；

V_s、V_f表示右奇異矩陣；

然后，分別計算當前時刻吊具坐標系、導(dǎo)彈托架坐標系與步驟S1和步驟S2標定時的相對位姿矩陣

其中：

進而得到當前時刻吊具坐標系及導(dǎo)彈托架坐標系在相機坐標系下的位姿矩陣

步驟S3.3、計算裝填參考位姿：根據(jù)下式計算得到裝填對準時，吊具坐標系與導(dǎo)彈托架坐標系之間的位姿關(guān)系，記作裝填參考位姿