基于双目视觉的导弹装填位姿偏差检测系统和方法技术方案

本发明专利技术提供了一种基于双目视觉的导弹装填位姿偏差检测系统和方法，包括双目相机、吊具靶标组、导弹托架靶标组、相机控制器，本发明专利技术能够有效解决车载导弹自动装填系统面临的装填作业高精度定位难题，并且布置方式灵活、操作方便、自动化程度高，能够为导弹自动装填控制提供实时状态反馈信息。制提供实时状态反馈信息。制提供实时状态反馈信息。

【技术实现步骤摘要】
基于双目视觉的导弹装填位姿偏差检测系统和方法


[0001]本专利技术涉及导弹装填控制
，具体地，涉及一种基于双目视觉的导弹装填位姿偏差检测系统和方法。

技术介绍

[0002]现有技术中的战术导弹装填系统多采用人工或半自动装填模式，装填效率低且操控技术要求高，因此对于发展适应野战环境的车载全自动导弹装填系统的需求愈发强烈。
[0003]在导弹装填过程中，需将导弹集装模块的销孔与导弹托架上的定位销精确对准，对自动装填系统的定位精度提出了很高的要求。然而，自动装填系统由结构复杂的多自由度机械臂和吊具构成，因大负载造成的弹性形变和各关节控制误差将逐级传递并在末端耦合放大，导致系统开环精度难以满足装填对位要求。又因野战条件下，装填车与发射车的停车位置随机，进一步加大了装填位姿偏差的不确定性。

技术实现思路

[0004]针对现有技术中的缺陷，本专利技术的目的是提供一种基于双目视觉的导弹装填位姿偏差检测系统和方法。
[0005]根据本专利技术提供的一种基于双目视觉的导弹装填位姿偏差检测方法，包括：
[0006]步骤S1、标定吊具数据：测量吊具靶标组与吊具坐标系之间的位姿关系，记录在相机控制器中；
[0007]步骤S2、标定导弹托架数据：测量导弹托架靶标组与导弹托架坐标系之间的位姿关系，记录在相机控制器中；
[0008]步骤S3、标定装填参考位姿：当吊具抓持导弹模块定位在导弹托架上时，使用双目相机同时拍摄吊具靶标组和导弹托架靶标组，由相机控制器计算并记录吊具坐标系和导弹托架坐标系的相对位姿，作为导弹装填作业时的参考位姿；
[0009]步骤S4、装填作业时检测实时位姿：在导弹装填作业时，使用双目相机同时拍摄吊具靶标组和导弹托架靶标组，由相机控制器计算吊具坐标系和导弹托架坐标系的实时位姿；
[0010]步骤S5、计算装填位姿偏差：使用相机控制器根据标定的装填作业参考位姿和检测得到的装填实时位姿，计算得到吊具坐标系实时位姿与参考位姿之间的位姿偏差。
[0011]优选地，在所述步骤S1中：在相机坐标系下测量并构造吊具坐标系，将此时吊具坐标系在相机坐标系下的位姿矩阵记作设吊具靶标组的靶标个数为m，将此时吊具靶标组的各靶标在相机坐标系下的三维坐标记作
[0012]在所述步骤S2中：在相机坐标系下测量并构造导弹托架坐标系，将此时导弹托架坐标系在相机坐标系下的位姿矩阵记作设导弹托架靶标组的靶标个数为n，将此时导弹托架靶标组的各靶标在相机坐标系下的三维坐标记作
[0013]优选地，在所述步骤S3中，将此时吊具靶标组的各靶标在相机坐标系下的三维坐标记作导弹托架靶标组的各靶标在相机坐标系下的三维坐标记作使用如下步骤计算吊具坐标系和导弹托架坐标系的参考相对位姿：
[0014]步骤S3.1、坐标归一化：将各靶标的标定值与实测值按照如下公式进行归一化处理：
[0015][0016][0017]表示出厂前，吊具靶标组与吊具坐标系之间的位姿关系下，吊具靶标组的第i个靶标在相机坐标系下的归一化坐标；
[0018]表示出厂前，吊具靶标组与吊具坐标系之间的位姿关系下，吊具靶标组的第i个靶标在相机坐标系下的三维坐标；
[0019]表示出厂前，吊具靶标组与吊具坐标系之间的位姿关系下，吊具靶标组的第k个靶标在相机坐标系下的三维坐标；
[0020]表示出厂前，吊具抓持导弹模块定位在导弹托架上时，吊具靶标组的第i个靶标在相机坐标系下的归一化坐标；
[0021]表示出厂前，吊具抓持导弹模块定位在导弹托架上时，吊具靶标组的第i个靶标在相机坐标系下的三维坐标；
[0022]表示出厂前，吊具抓持导弹模块定位在导弹托架上时，吊具靶标组的第k个靶标在相机坐标系下的三维坐标；
[0023]表示出厂前，导弹托架靶标组与导弹托架坐标系之间的位姿关系下，导弹托架靶标组的第j个靶标在相机坐标系下的归一化坐标；
[0024]表示出厂前，导弹托架靶标组与导弹托架坐标系之间的位姿关系下，导弹托架靶标组的第j个靶标在相机坐标系下的三维坐标；
[0025]表示出厂前，导弹托架靶标组与导弹托架坐标系之间的位姿关系下，导弹托架靶标组的第k个靶标在相机坐标系下的三维坐标；
[0026]表示出厂前，吊具抓持导弹模块定位在导弹托架上时，导弹托架靶标组的第j个靶标在相机坐标系下的归一化坐标；
[0027]表示出厂前，吊具抓持导弹模块定位在导弹托架上时，导弹托架靶标组的第j个靶标在相机坐标系下的三维坐标；
[0028]表示出厂前，吊具抓持导弹模块定位在导弹托架上时，导弹托架靶标组的第k个靶标在相机坐标系下的三维坐标；
[0029]步骤S3.2、计算吊具坐标系及导弹托架坐标系在相机坐标系下的位姿矩阵H
s
、H
f
：由归一化坐标构造如下矩阵和分别进行奇异值分解得：
[0030]H
s
＝U
s
Σ
s
V
sT
,H
f
＝U
f
Σ
f
V
fT (3)
[0031]U
s
、U
f
表示左奇异矩阵；
[0032]Σ
s
、Σ
f
表示对角矩阵；
[0033]V
s
、V
f
表示右奇异矩阵；
[0034]然后，分别计算当前时刻吊具坐标系、导弹托架坐标系与步骤S1和步骤S2标定时的相对位姿矩阵
[0035][0036]其中：
[0037][0038][0039]进而得到当前时刻吊具坐标系及导弹托架坐标系在相机坐标系下的位姿矩阵
[0040][0041]步骤S3.3、计算装填参考位姿：根据下式计算得到装填对准时，吊具坐标系与导弹托架坐标系之间的位姿关系，记作装填参考位姿
[0042][0043]优选地，在所述步骤S4中：
[0044]将此时吊具靶标组的各靶标在相机坐标系下的三维坐标记作导弹托架靶标组的各靶标在相机坐标系下的三维坐标记作分别计算得到装填作业时吊具坐标系、导弹托架坐标系在相机坐标系下的位姿矩阵进而得到装填作业是吊具坐标系与导弹托架坐标系之间的位姿关系
[0045][0046]在所述步骤S5：计算装填位姿偏差
[0047][0048]根据本专利技术提供的一种基于双目视觉的导弹装填位姿偏差检测系统，包括：
[0049]模块M1、标定吊具数据：测量吊具靶标组与吊具坐标系之间的位姿关系，记录在相机控制器中；
[0050]模块M2、标定导弹托架数据：测量导弹托架靶标组与导弹托架坐标系之间的位姿关系，记录在相机控制器中；
[0051]模块M3、标定装填参考位姿：当吊具抓持导弹模块定位在导弹托架上时，使用双目相机同时拍摄吊具靶标组和导弹托架靶标组，由相机控制器计算并记录吊具坐标系和导弹托架坐标系的相对位本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于双目视觉的导弹装填位姿偏差检测方法，其特征在于，包括：步骤S1、标定吊具数据：测量吊具靶标组与吊具坐标系之间的位姿关系，记录在相机控制器中；步骤S2、标定导弹托架数据：测量导弹托架靶标组与导弹托架坐标系之间的位姿关系，记录在相机控制器中；步骤S3、标定装填参考位姿：当吊具抓持导弹模块定位在导弹托架上时，使用双目相机同时拍摄吊具靶标组和导弹托架靶标组，由相机控制器计算并记录吊具坐标系和导弹托架坐标系的相对位姿，作为导弹装填作业时的参考位姿；步骤S4、装填作业时检测实时位姿：在导弹装填作业时，使用双目相机同时拍摄吊具靶标组和导弹托架靶标组，由相机控制器计算吊具坐标系和导弹托架坐标系的实时位姿；步骤S5、计算装填位姿偏差：使用相机控制器根据标定的装填作业参考位姿和检测得到的装填实时位姿，计算得到吊具坐标系实时位姿与参考位姿之间的位姿偏差。2.根据权利要求1所述的基于双目视觉的导弹装填位姿偏差检测方法，其特征在于，在所述步骤S1中：在相机坐标系下测量并构造吊具坐标系，将此时吊具坐标系在相机坐标系下的位姿矩阵记作设吊具靶标组的靶标个数为m，将此时吊具靶标组的各靶标在相机坐标系下的三维坐标记作在所述步骤S2中：在相机坐标系下测量并构造导弹托架坐标系，将此时导弹托架坐标系在相机坐标系下的位姿矩阵记作设导弹托架靶标组的靶标个数为n，将此时导弹托架靶标组的各靶标在相机坐标系下的三维坐标记作3.根据权利要求2所述的基于双目视觉的导弹装填位姿偏差检测方法，其特征在于，在所述步骤S3中，将此时吊具靶标组的各靶标在相机坐标系下的三维坐标记作导弹托架靶标组的各靶标在相机坐标系下的三维坐标记作使用如下步骤计算吊具坐标系和导弹托架坐标系的参考相对位姿：步骤S3.1、坐标归一化：将各靶标的标定值与实测值按照如下公式进行归一化处理：步骤S3.1、坐标归一化：将各靶标的标定值与实测值按照如下公式进行归一化处理：步骤S3.1、坐标归一化：将各靶标的标定值与实测值按照如下公式进行归一化处理：表示出厂前，吊具靶标组与吊具坐标系之间的位姿关系下，吊具靶标组的第i个靶标在相机坐标系下的归一化坐标；表示出厂前，吊具靶标组与吊具坐标系之间的位姿关系下，吊具靶标组的第i个靶标在相机坐标系下的三维坐标；
表示出厂前，吊具靶标组与吊具坐标系之间的位姿关系下，吊具靶标组的第k个靶标在相机坐标系下的三维坐标；表示出厂前，吊具抓持导弹模块定位在导弹托架上时，吊具靶标组的第i个靶标在相机坐标系下的归一化坐标；表示出厂前，吊具抓持导弹模块定位在导弹托架上时，吊具靶标组的第i个靶标在相机坐标系下的三维坐标；表示出厂前，吊具抓持导弹模块定位在导弹托架上时，吊具靶标组的第k个靶标在相机坐标系下的三维坐标；表示出厂前，导弹托架靶标组与导弹托架坐标系之间的位姿关系下，导弹托架靶标组的第j个靶标在相机坐标系下的归一化坐标；表示出厂前，导弹托架靶标组与导弹托架坐标系之间的位姿关系下，导弹托架靶标组的第j个靶标在相机坐标系下的三维坐标；表示出厂前，导弹托架靶标组与导弹托架坐标系之间的位姿关系下，导弹托架靶标组的第k个靶标在相机坐标系下的三维坐标；表示出厂前，吊具抓持导弹模块定位在导弹托架上时，导弹托架靶标组的第j个靶标在相机坐标系下的归一化坐标；表示出厂前，吊具抓持导弹模块定位在导弹托架上时，导弹托架靶标组的第j个靶标在相机坐标系下的三维坐标；表示出厂前，吊具抓持导弹模块定位在导弹托架上时，导弹托架靶标组的第k个靶标在相机坐标系下的三维坐标；步骤S3.2、计算吊具坐标系及导弹托架坐标系在相机坐标系下的位姿矩阵H
s
、H
f
：由归一化坐标构造如下矩阵和分别进行奇异值分解得：H
s
＝U
s
Σ
s
V
sT
,H
f
＝U
f
Σ
f
V
fT
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)U
s
、U
f
表示左奇异矩阵；Σ
s
、Σ
f
表示对角矩阵；V
s
、V
f
表示右奇异矩阵；然后，分别计算当前时刻吊具坐标系、导弹托架坐标系与步骤S1和步骤S2标定时的相对位姿矩阵对位姿矩阵其中：
进而得到当前时刻吊具坐标系及导弹托架坐标系在相机坐标系下的位姿矩阵进而得到当前时刻吊具坐标系及导弹托架坐标系在相机坐标系下的位姿矩阵步骤S3.3、计算装填参考位姿：根据下式计算得到装填对准时，吊具坐标系与导弹托架坐标系之间的位姿关系，记作装填参考位姿坐标系之间的位姿关系，记作装填参考位姿4.根据权利要求3所述的基于双目视觉的导弹装填位姿偏差检测方法，其特征在于，在所述步骤S4中：将此时吊具靶标组的各靶标在相机坐标系下的三维坐标记作导弹托架靶标组的各靶标在相机坐标系下的三维坐标记作分别计算得到装填作业时吊具坐标系、导弹托架坐标系在相机坐标系下的位姿矩阵进而得到装填作业是吊具坐标系与导弹托架坐标系之间的位姿关系业是吊具坐标系与导弹托架坐标系之间的位姿关系在所述步骤S5：计算装填位姿偏差在所述步骤S5：计算装填位姿偏差5.一种基于双目视觉的导弹装填位姿偏差检测系统，其特征在于，包括：模块M1、标定吊具数据：测量吊具靶标组与吊具坐标系之间的位姿关系，记录在相机控制器中；模块M2、标定导弹托架数据：测量导弹托架靶标组与导弹托架坐标系之间的位姿关系，记录在相机控制器中；模块M3、标定装填参考位姿：当吊具抓...

【专利技术属性】
技术研发人员：曾远帆，庄国平，殷翔，缪东辉，蒋道民，鲁勇军，范文晶，牛远征，刘禹华，张胜，
申请(专利权)人：上海机电工程研究所，
类型：发明
国别省市：