(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210962107.9 (22)申请日 2022.08.11 (71)申请人 西安航天时代 精密机电有限公司 地址 710100 陕西省西安市航天西路108号 (72)发明人 罗华　李瑞峰　郭静　杨娜　 张力力　郭超　 (74)专利代理 机构 西安智邦专利商标代理有限 公司 6121 1 专利代理师 徐秦中 (51)Int.Cl. G06T 7/73(2017.01) G06T 7/80(2017.01) (54)发明名称 一种基于对接面特征的高精度位姿测量方 法 (57)摘要 本发明涉及一种基于对接面特征的高精度 位姿测量方法， 以解决现有大型部件的装配方法 设备成本较高、 标定过程复杂及易受工业环境影 响的技术问题。 该方法包括： 1、 搭建组装平台； 2、 对单目视觉传感器进行内参标定； 3、 采用手眼标 定法对单目视觉传感器与运动执行组件进行标 定； 4、 对对应的标靶和销孔进行标定； 5、 测量第 一部件相对于第二部件的位姿。 权利要求书4页 说明书8页 附图4页 CN 115496802 A 2022.12.20 CN 115496802 A 1.一种基于对接面特 征的高精度位姿测量方法， 其特 征在于， 包括以下步骤： S1、 搭建组装平台： 包括相对设置的第一组装单元和第二组装单元， 第 一组装单元包括 第一固定对接车(11)、 设置在第一固定对接车(11)上表 面的可移动的运动执行 组件(12)及 固定设置在运动执行 组件(12)上的第一部件(13)， 第一部件(13)对接面上具有至少四个第 一销孔(15)； 第二组装单元包括第二固定对接车(21)、 设置在第二固定对接车(21)上固定 执行组件(22)及固定设置在固定执行 组件(22)上的第二部件(23)， 第二部件(23)对接面上 具有与第一销孔(15)一一对应的第二销 孔(25)； 建立第一组装单元的三维坐标系O ‑xyz， O 在第一部件(23)对接面的中心； S2、 对视觉传感器(16)进行内参标定， 获取视 觉传感器(16)的标定参数； S3、 将内参标定后的视觉传感器(16)安装在第一组装单元， 采用手眼标定法对视觉传 感器(16)与运动执行组件(12)进行标定， 解算视觉传感器坐标系 与运动执行 组件坐标系的 转换关系， 获取视 觉传感器坐标系到运动执 行组件坐标系的旋转矩阵 和平移矩阵 S4、 第一标靶(14)和第二标靶(24)设置在第一部件(13)和第二部件(23)上， 且第一标 靶(14)和第二标靶(24)均在步骤S3所述视觉传感器(16)的视野 范围内； 采用三 维测量系统 分别对第一标靶(14)和第一销孔(15)位置及第二标靶(24)和第二销孔(25)位置进行标定； 按照排序规则， 获取第一标靶标志点坐标排序为P1w， 第二标靶标志点坐标排序为 第一 销孔坐标排序为 第二销孔 坐标排序为 S5、 采用步骤S3中的视觉传感器(16)同时获取第一标靶图像和第二标靶图像， 通过图 像处理算法及坐标系的转换矩阵， 解算出第一部件(13)相对于第二部件(23)的俯仰角 θ、 偏 航角 ψ、 滚转角φ、 位移x、 为位移y、 位移z， 完成第一部件(13)相对于第二部件(23)的位姿测 量。 2.根据权利要求1所述的基于对接面特 征的高精度位姿测量方法， 其特 征在于： 步骤S5具体为： S5.1、 采用步骤S3中的视觉传感器(16)同时获取第一标靶图像和第二标靶图像， 且第 一标靶(14)和第二标靶(24)上的全部标志点均在 对应图像内， 通过图像处理算法分别提取 第一标靶标志点圆心 坐标和第二标靶标志 点圆心坐标， 并按照步骤S4中的排序规则进行排 序， 获得第一标靶标志点圆心坐标排序p1和第二标靶标志点圆心坐标排序p2； S5.2、 根据步骤S2中的视觉传 感器(16)的标定参数、 第一标靶标志点圆心坐标排序p1、 第二标靶标志点圆心坐标排序p2及标靶世界坐标系下第一标靶标志点坐标排序P1w和标靶 世界坐标系下第二标靶标志点坐标排序 分别计算第一标靶(14)和第二标靶(24)在视 觉传感器坐标系下的坐标 根据坐标 计算视觉传感器坐标系 与标靶世界 坐标系的旋转变换矩阵(Rc,Tc)； S5.3、 根据视觉传感器坐标系与标靶世界坐标系的旋转变换矩阵(Rc,Tc)和标靶世界坐 标系下第一销孔坐标排序 第二销孔坐标排序 分别计算第一销孔(15)在视觉传感器 坐标系下的坐标 第二销孔(25)在视 觉传感器坐标系下的坐标 S5.4、 根据步骤S2中， 视觉传感器坐标系到运动 执行组件坐标系的旋转矩阵 和视觉权　利　要　求　书 1/4 页 2 CN 115496802 A 2传感器坐标系到运动执行组件坐标系的平移矩阵 及第一销孔(15)在视觉传感器坐标系 下的坐标 第二销孔(25)在视觉传感器坐标系下的坐标 计算第一销孔(15)在运动执 行组件坐标系下的坐标 第二销孔(25)在运动执 行组件坐标系下的坐标 S5.5、 构建SVD函数， 求解 计算第一销孔(15)和第二销孔(25)理想对接 下， 第一销孔(15)和第二销孔(25)在 运动执行组件坐标系下的刚体变换旋转矩阵Rb和平移 矩阵tb； S5.6、 根据旋转矩阵Rb和平移矩阵tb， 利用欧拉角与旋转矩阵和平移矩阵关系， 解算出 第一部件(13)相对于第二部件(23)的俯仰角 θ、 偏航角 ψ、 滚转角φ、 位移x、 位移y、 位移z， 完 成第一部件(13)相对于第二部件(23)的位姿测量。 3.根据权利要求2所述的基于对接面特 征的高精度位姿测量方法， 其特 征在于： 还包括步骤S6， S6、 根据步骤S5.6获取的俯仰角 θ、 偏航角 ψ、 滚转角φ、 位移x、 位移y、 位移z， 计算第一 销孔(15)与第二销孔(25)的对准误差值， 若第一销孔(15)与第二销孔(25)对准误差值小于 预设阈值， 则完成第一部件(13)相对于第二部件(23)的位姿调节； 若第一销孔(15)与第二 销孔(25)的对准误差值大于等于预设阈值， 则调节第一部件(13)使第一销孔(15)与第二销 孔(25)的对准， 重复步骤S5.1至步骤S5.6， 直至第一销孔(15)与第二销孔(25)对准误差值 小于预设阈值。 4.根据权利要求1 ‑3任一所述的基于对接面特 征的高精度位姿测量方法， 其特 征在于： 步骤S4中， 采用 三维测量系统分别对第一标靶(14)和第一销孔(15)位置及第二标靶 (24)和第二销孔(25)位置进行 标定； 具体为： S4.1、 采用三维测量系统分别获取第一标靶标志点、 第一销孔(15)、 第二靶标标志点和 第二销孔(25)在标靶世界坐标系下的三维坐标； S4.2、 按照排序规则对步骤S4.1获取的三维坐标进行排序， 获取第一标靶标志点坐标 排序为P1w， 第二标靶标志点坐标排序为 第一销孔坐标排序为 第二销孔坐标排序为 5.根据权利要求 4所述的基于对接面特 征的高精度位姿测量方法， 其特 征在于： 步骤S5.3中， 计算第一销孔(15)在视觉传感器坐标系下的坐标 和第二销孔(25)在视 觉传感器坐标系下的坐标 公式为 6.根据权利要求5所述的基于对接面特 征的高精度位姿测量方法， 其特 征在于： 步骤S5.4中， 计算第一销孔(15)在运动执行组件坐标系下的坐标 和第二销孔(25)在 运动执行组件坐标系下的坐标 具体为 权　利　要　求　书 2/4 页 3 CN 115496802 A 3