CN103895023A - 一种基于编码方位装置的机械臂末端跟踪测量系统及方法 - Google Patents

Abstract

一种基于编码方位装置的机械臂末端跟踪测量系统及方法。系统包括机械臂、多面体编码点特征测量靶标、机械臂末端执行器、机械臂控制柜、测量像机和上位机。本发明提供的基于编码方位装置的机械臂末端跟踪测量系统和方法采用单目视觉姿态估计以满足大视场的要求，并且结构简单、造价低。采用多面体编码点特征测量靶标避免了复杂工业环境下光污染造成的对定位精度的影响。

Description

一种基于编码方位装置的机械臂末端跟踪测量系统及方法

技术领域

本发明属于机械臂跟踪测量技术领域，特别是涉及一种基于编码方位装置的机械臂末端跟踪测量系统及方法。

背景技术

传统的工业机械臂一般采用示教或离线编程的方式对加工任务进行路径规划和运动编程。随着技术的发展，工业生产对加工质量、加工精度、装配效率提出了更高的要求。而利用视觉系统，可以实时反馈机械臂末端的精确位置信息，根据位置信息可以合理地调整机械臂的运动轨迹，提高生产效率，进一步提高产品的加工质量。如何提高机械臂末端位置姿态检测和跟踪精度是目前国内外研究的热点，对机械臂末端位置姿态检测跟踪目前主要有以下几种方法：基于激光跟踪仪的视觉测量跟踪补偿系统，与跟踪靶标、扫描头、位姿测头等组成视觉伺服系统，实现工业机械臂在线监控和测量并补偿，该方法主要存在价格昂贵和工业环境复杂造成光路遮挡等问题。基于视觉测程法的机械臂测量跟踪方法，该方法通过对特征点提取和特征点向量描述，建立关联规则的特征点匹配策略，最后用数学方法求解刚体运动方程。此方法能够跟踪机械臂并绘制实时的运动轨迹，该方法难以精确建立动力学和运动状态模型，特征点识别匹配受工业环境影响，误匹配严重。基于Indoor GPS的测量补偿系统，主要由发射站、接收器、信号处理器以及终端计算机组成，通过跟踪安装在需要跟踪定位部件上的接收器或接收器组实现跟踪头坐标测量或跟踪目标位姿测量。该系统主要存在局部组网造价高，精度低，可靠性差等缺点。

发明内容

为了解决上述问题，本发明的目的在于提供一种基于编码方位装置的机械臂末端跟踪测量系统及方法。

为了达到上述目的，本发明提供的基于编码方位装置的机械臂末端跟踪测量系统及方法包括：机械臂、多面体编码点特征测量靶标、机械臂末端执行器、机械臂控制柜、测量像机和上位机；其中：机械臂为电动多关节工业机械臂，其活动末端装有多面体编码点特征测量靶标和机械臂末端执行器；

多面体编码点特征测量靶标为带有编码特征点的测量识别装置，其安装在机械臂与机械臂末端执行器的连接部位；

机械臂末端执行器为机械臂的操作执行部件，机械臂通过安装在其末端的机械臂末端执行器完成预定动作；

机械臂控制柜为工业机械臂的驱动控制部件，其与机械臂相连接，用于控制机械臂工作；

测量像机为单目CCD图像采集装置，其与上位机相连接，用于采集整个操作区的图像信息，并传送给上位机；

上位机为控制计算机，为本系统的控制核心；其与机械臂控制柜相连接。

所述的多面体编码点特征测量靶标为多面体编码方位装置，其为一多面体形状，其中轴部位设有连接轴，连接轴的两端分别与机械臂的末端和机械臂末端执行器相连接；多面体编码点特征测量靶标上具有多个矩形侧面，每一侧面构成一个测量面，每个测量面的表面粘贴有一至多个编码特征点；每个编码特征点包含个相对位置固定的特征位和3个编码位。

所述的编码特征点采用黑色背景，在黑色背景上根据排列规则粘贴由高反射率反光材料制成的特征位和编码位。

本发明提供的基于编码方位装置的机械臂末端跟踪测量系统的跟踪测量方法包括按顺序执行的下列步骤：

步骤一、工作空间整体布局和装置固定的S01阶段：根据机械臂的运动轨迹、工作范围，将测量像机固定在待加工工件上；

步骤二、测量像机与工件的预标定、建立靶标和末端执行器校准数据库的S02阶段：将工件空间坐标预先校准，经工件工装定位与预定标，像机坐标系(OCXCYCZC)在工件坐标系（OWXWYWZW）下的位姿（TCW,RCW）为固定值且已知；通过对基准校准球进行校准来确定机械臂末端执行器的位置在靶标坐标系下的坐标转换关系，从而确定末端执行器坐标系（ODXDYDZD）在靶标坐标系（OEXEYEZE）下的位姿（TDE,RDE）；

步骤三、工作空间坐标系建立的S03阶段：在机械臂基座处建立机械臂坐标系（ORXRYRZR）；靶标坐标系（OEXEYEZE）在编码特征点或靶标其他位置上选取；末端执行器坐标系（ODXDYDZD）建立在机械臂末端执行器的末端；

步骤四、机械臂到达预定位置的S04阶段：通过上位机输入目标空间坐标参数，利用机械臂控制柜控制机械臂到达预定位置；

步骤五、靶标实时成像测量的S05阶段：测量像机对安装在机械臂与机械臂末端执行器之间的多面体编码点特征测量靶标进行实时成像测量，然后将图像数据传递到上位机；

步骤六、获取机械臂末端执行器姿态的S06阶段：上位机对上述测量图像经过编码特征点扫描、质心定位、编码特征点识别，后方交会求解像机外方位和相对方位，利用外极线约束外极角匹配方法进行编码特征点匹配，基于共线性约束的优化平差求解编码特征点空间三维坐标；像机基站成像中利用空间存在多于点的相互之间空间距离已知的特征点与成像点之间的关系，采用基于空间共角约束条件下的交会姿态估计算法和多外部方位装置优化确定像机基站的绝对外部方位（TCW,RCW），即像机坐标系(OCXCYCZC)在工件坐标系（OWXWYWZW）下的位姿（TCW,RCW）；通过坐标逆变换获得靶标坐标系在像机坐标系(OCXCYCZC)下的位姿（TEC，REC）；

步骤七、获取机械臂末端执行器精确空间坐标的S07阶段：利用机械臂末端执行器与多面体编码点特征测量靶标之间的刚体连接关系和预校准并建立的末端执行器数据库，最终获得机械臂末端执行器在工件坐标系下的精确坐标位置及姿态；

步骤八、补偿机械臂末端执行器位姿偏差的S08阶段：通过位姿检测补偿系统计算程序位姿和测量位姿偏差值，由上位机发送补偿位移给机械臂控制柜，从而精确控制机械臂的动作；

步骤九、判断预定动作是否完成的S09阶段：判断本次操作的预定动作是否全部完成，如果判断结果为“是”，则本操作流程结束，否则下一步返回S03阶段的入口处，继续下一步动作。

由于工业机械臂装配现场环境复杂，加工精度高，本发明提供的基于编码方位装置的机械臂末端跟踪测量系统和方法采用单目视觉姿态估计以满足大视场的要求，并且结构简单、造价低。采用多面体编码点特征测量靶标避免了复杂工业环境下光污染造成的对定位精度的影响。

本发明与已有技术相比具有以下特点：

1.提出基于单目视觉姿态估计算法和多面体编码点特征靶标的机械臂末端跟踪测量方法，单目视觉测量解决了现场空间的限制，可实现复杂现场环境下的实时跟踪测量。

2.采用多面体编码点测量靶标作为测量靶标，多面体结构能够确保机械臂任意位姿均在单主测面或双主侧面出现，从而实现全工况实时监测。

3.多面体编码点特征测量靶标的每一测量面上均包含1到多个编码特征点，从而能够确保编码特征点具有唯一识别特性，提高了测量的稳定性和可靠性。另外，主侧面上多个编码特征点增加了控制点的数量，编码特征点上特征位之间的固定约束可使控制点在位姿估计解算过程中相互校准，可在提高三维不确定度指标的同时进一步消除伪特征像点造成的粗大误差。

附图说明

图1为本发明提供的基于编码方位装置的机械臂末端跟踪测量系统结构示意图。

图2为本发明提供的基于编码方位装置的机械臂末端跟踪测量系统中多面体编码点特征测量靶标结构示意图。

图3为本发明提供的基于编码方位装置的机械臂末端跟踪测量系统中各坐标系示意图。

图4为采用本发明提供的基于编码方位装置的机械臂末端跟踪测量方法时通过基准校准球校准机械臂末端执行器过程示意图。

图5为本发明提供的基于编码方位装置的机械臂末端跟踪测量方法流程图。

具体实施方式

下面结合附图和具体实施例对本发明提供的基于编码方位装置的机械臂末端跟踪测量系统及方法进行详细说明。

如图1所示，本发明提供的基于编码方位装置的机械臂末端跟踪测量系统包括：机械臂1、多面体编码点特征测量靶标2、机械臂末端执行器3、机械臂控制柜4、测量像机5和上位机6；其中：机械臂1为电动多关节工业机械臂，其活动末端装有多面体编码点特征测量靶标2和机械臂末端执行器3；

多面体编码点特征测量靶标2为带有编码特征点的测量识别装置，其安装在机械臂1与机械臂末端执行器3的连接部位；

机械臂末端执行器3为机械臂1的操作执行部件，机械臂1通过安装在其末端的机械臂末端执行器3完成预定动作；

机械臂控制柜4为工业机械臂1的驱动控制部件，其与机械臂1相连接，用于控制机械臂1工作；

测量像机5为单目CCD图像采集装置，其与上位机6相连接，用于采集整个操作区的图像信息，并传送给上位机6；

上位机6为控制计算机，为本系统的控制核心；其与机械臂控制柜4相连接。

如图2所示，所述的多面体编码点特征测量靶标2为多面体编码方位装置，其为一多面体形状，其中轴部位设有连接轴123，连接轴123的两端分别与机械臂1的末端和机械臂末端执行器3相连接。多面体编码点特征测量靶标2上具有多个矩形侧面，每一侧面构成一个测量面，测量面的个数需根据所使用的机械臂1的尺寸确定（如图2采用八面体结构），每个测量面的表面粘贴有一至多个编码特征点（图2所示的为两个编码特征点）；每个编码特征点包含5个相对位置固定的特征位121和3个编码位120，为了易于识别和解算，各个测量面上编码特征点的标号应各不相同，以避免编码特征点解算时出现误匹配现象；每一个编码特征点均构成外部方位装置，主侧面上多个编码特征点增加了控制点的数量，编码特征点上特征位之间的固定约束可使控制点在位姿估计解算过程中相互校准，利用空间多于3点的相互之间空间距离已知的特征点与之成像点之间的关系即可确定像机坐标系相对于靶标坐标系之间的方位关系。

所述的编码特征点采用黑色背景，在黑色背景上根据排列规则粘贴由高反射率反光材料制成的特征位121和编码位120。

所述的多面体编码点特征测量靶标2的外形可根据实际情况进行设计，并应保证机械臂1和机械臂末端执行器3之间刚性连接；同时应合理设计多棱柱体体积，使其不会影响机械臂1末端负重比。

机械臂末端执行器坐标系与机械臂基座坐标系之间的坐标变换关系由机械臂1的正向运动学模型及各关节变量值获得，由于不存在完美工业机械臂运动学模型，并且机械加工及安装也存在较大误差，因此需要外部测量系统对机械臂1进行校准和补偿。测量像机5应与待加工工件7处在同一工作空间内且保证工件7和机械臂1处于测量像机5的视场范围内，测量像机5位置固定后，经工件7工装定位和预标定，测量像机5的像机坐标系在工件坐标系下的位姿为固定值。测量像机5的内部方位参数、有效焦距、像机原点坐标、像机畸变参数等都会直接影响单目视觉姿态估计解算结果，从而影响在工件坐标系下多面体编码点特征测量靶标2上编码特征点解算，结果直接影响定位精度，因此测量像机5必须要经过像机内部参数高精度校准；测量像机5将利用调整光圈大小来设置曝光时间，并将采集到的图像传送给上位机6。上位机6对采集到的图像进行数据处理，之后扫描采集图像并进行质心定位，对编码特征点进行识别，后方交会求解像机外方位及相对方位，经过前方交会获取编码特征点三维坐标，通过优化平差精确求解编码特征点三维坐标，经过坐标系转换，确定机械臂1末端在工件坐标系下的位置姿态，与已经标定的工件坐标求取偏差，最后利用机械臂控制柜4对机械臂1末端进行姿态位置补偿。另外，多面体编码点特征测量靶标2固定在机械臂1的末端，应保证刚性连接，以避免工作过程中的不稳定性，多面体编码点特征测量靶标2的尺寸设计还要保证机械臂1末端不能超过额定负重比。根据机械臂运动学理论合理设计多面体编码点特征测量靶标2布局对提高机械臂1末端定位精度具有直接作用。

如图4所示，所述的基于编码方位装置的机械臂末端跟踪测量系统还包括基准校准球8；基准校准球8为基准校准装置，一般由碳化硅材料制成，球体表面具有较高的精度及表面清洁度，并且在球体表面设置具有一定尺寸精度及光洁度的孔作为基准孔。在本测量系统的测量范围内安装基准校准球8，安装后其位置始终保持不变。利用测量像机5对同一基准孔分别进行机械臂1不同位姿成像的测量，并将像机坐标系下的坐标值通过坐标变换转换到工件坐标下。通过机械臂1对设置在测量现场的基准校准球8某一固定点进行多次不同位姿成像的测量，并通过多面体编码点特征测量靶标2的空间约束可以求解出测量像机5到多面体编码点特征测量靶标2之间的空间位置关系，通过球面拟合，获得机械臂末端执行器3的位置在靶标坐标系下的坐标转换关系。

如图5所示，本发明提供的基于编码方位装置的机械臂末端跟踪测量方法包括按顺序执行的下列步骤：

步骤一、工作空间整体布局和装置固定的S01阶段：应选取震动强度小的工作环境，合理规划测量布局及测量网络，以保证测量精度。根据机械臂1的运动轨迹、工作范围，合理设置机械臂1与待加工工件7的工作布局，将测量像机5固定在待加工工件7上，其安装应保证多面体编码点特征测量靶标2与测量像机5之间具有良好的交会角度。

步骤二、测量像机与工件的预标定、建立靶标和末端执行器校准数据库的S02阶段：将工件空间坐标预先校准，经工件工装定位与预定标，像机坐标系(OCXCYCZC)在工件坐标系（OWXWYWZW）下的位姿（TCW,RCW）为固定值且已知。由于机械臂末端执行器3与多面体编码点特征测量靶标2为刚体连接且定标，如图4所示，通过对基准校准球8进行校准可以确定机械臂末端执行器3的位置在靶标坐标系下的坐标转换关系，从而确定末端执行器坐标系（ODXDYDZD）在靶标坐标系（OEXEYEZE）下的位姿（TDE,RDE）。

步骤三、工作空间坐标系建立的S03阶段：如图3所示，由于机械臂1基座是固定的，所以在机械臂1基座处建立机械臂坐标系（ORXRYRZR）。测量像机5位置固定后，像机坐标系(OCXCYCZC)和工件坐标系（OWXWYWZW）的建立可根据工件工装定位与预定标确定，靶标坐标系（OEXEYEZE）可在编码特征点上选取，也可选在靶标其他位置，因为视觉测量中的空间刚体变换建立了物方空间内不同坐标系间的转换关系，通过空间刚体变换所建立的坐标系相对位姿关系可以用旋转矩阵和平移矩阵来描述。末端执行器坐标系（ODXDYDZD）建立在机械臂末端执行器3的末端。

步骤四、机械臂到达预定位置的S04阶段：由于工件坐标预先校准已知，首先通过上位机6输入目标空间坐标参数，利用机械臂控制柜4控制机械臂1到达预定位置；

步骤五、靶标实时成像测量的S05阶段：测量像机5对安装在机械臂1与机械臂末端执行器3之间的多面体编码点特征测量靶标2进行实时成像测量，然后将图像数据传递到上位机6；

步骤六、获取机械臂末端执行器姿态的S06阶段：上位机6对上述测量图像经过编码特征点扫描、质心定位、编码特征点识别，后方交会求解像机外方位和相对方位，利用外极线约束外极角匹配方法进行编码特征点匹配。基于共线性约束的优化平差求解编码特征点空间三维坐标。像机基站成像中利用空间存在多于3点的相互之间空间距离已知的特征点与成像点之间的关系，采用基于空间共角约束条件下的交会姿态估计算法和多外部方位装置优化可以确定像机基站的绝对外部方位（TCW,RCW），即像机坐标系(OCXCYCZC)在工件坐标系（OWXWYWZW）下的位姿（TCW,RCW）。通过坐标逆变换则可获得靶标坐标系在像机坐标系(OCXCYCZC)下的位姿（TEC，REC）。由于像机坐标系在工件坐标系下的位姿已经预先标定且已知，通过坐标变换可以获得机械臂末端的靶标坐标系在工件坐标系下的位姿（TEW,REW）。

由于机械臂末端执行器3与多面体编码点特征测量靶标2可作为刚体连接且定标，如图4所示，通过对基准校准球8校准可以确定机械臂末端执行器3在靶标坐标系下的坐标转换关系，因此末端执行器坐标系（ODXDYDZD）在靶标坐标系（OEXEYEZE）下的位姿（TDE,RDE）确定。同样经靶标坐标系（OEXEYEZE）、像机坐标系(OCXCYCZC)转换至工件坐标系（OWXWYWZW），该位姿（TDW,RDW）即确定了测量时刻的机械臂末端执行器3的位置及姿态。

步骤七、获取机械臂末端执行器精确空间坐标的S07阶段：利用机械臂末端执行器3与多面体编码点特征测量靶标2之间的刚体连接关系和预校准并建立的末端执行器数据库，最终获得机械臂末端执行器3在工件坐标系下的精确坐标位置及姿态；

步骤八、补偿机械臂末端执行器位姿偏差的S08阶段：通过位姿检测补偿系统计算程序位姿和测量位姿偏差值，由上位机6发送补偿位移给机械臂控制柜4，从而精确控制机械臂1的动作；

步骤九、判断预定动作是否完成的S09阶段：判断本次操作的预定动作是否全部完成，如果判断结果为“是”，则本操作流程结束，否则下一步返回S03阶段的入口处，继续下一步动作。

本跟踪测量系统的工作原理：

利用上位机6通过机械臂控制柜4控制机械臂1到达预定位置，然后利用测量像机5对安装在机械臂1与机械臂末端执行器3之间的多面体编码点特征测量靶标2进行实时成像测量，然后将图像数据传递到上位机6，上位机6对获得的图像进行编码特征点解算，并根据编码特征点之间的已知空间坐标约束利用姿态估计方法获得靶标坐标系与像机坐标系之间的相对位姿，利用机械臂末端执行器3与多面体编码点特征测量靶标2之间的刚体连接关系和预校准并建立的末端执行器数据库，最终获得机械臂末端执行器3在工件坐标系下的精确坐标位置及姿态。通过位姿检测补偿系统计算程序位姿和测量位姿偏差值，从而达到精确控制机械臂1动作的目的。

对工业机械臂运动学分析，确定其运动轨迹优化、工作空间、误差补偿模型，合理设计像机与被测工件距离，确定测量布局优化。在此基础上合理设计多面体编码点特征测量靶标2与测量像机5布局优化，从而获得工作空间高精度全局实时测量。另外，由于机械臂装配现场环境复杂，测量视场范围大，实时性要求高，本发明采用单目视觉姿态估计方法结合多面体编码特征测量靶标2进行机械臂1的位姿补偿测量。

测量现场环境的光污染决定了编码特征点具有唯一识别特性，本发明以多面体编码点特征测量靶标2作为测量靶标，可避免编码特征点的误识别引起测量粗大误差。采用多面体结构能够确保机械臂1在任意工作位姿均有在单目视觉三维不确定度指标满足范围内的单主测面或双主侧面出现，实现全工况测量。

通过识别编码方位装置确定测量像机5与多面体编码点特征测量靶标2的位姿转换关系。多面体编码点特征测量靶标2上每一个编码特征点包含相对位置固定且已知的5个特征位121和3个编码位120，每一个编码特征点均构成外部方位装置，主侧面上多个编码特征点增加了控制点的数量，编码特征点上特征位之间的固定约束使控制点在位姿估计解算过程中互校准，在提高三维不确定度指标的同时，进一步消除了伪特征像点造成的粗大测量误差的可能。

多面体编码点特征测量靶标2的每一测量面上均包含1到多个编码特征点，由于每一编码特征点的唯一性，在特征像点聚类分析及编码解码中，编码特征点区域中和外部的伪特征像点对其均不会造成影响，因此可以确保测量的稳定性和可靠性。另外，为了易于识别和解算编码特征点布局，可采用每一主测面和相邻测面大小不同，各个测量面上编码特征点编号不同的方式。

Claims (4)

1.一种基于编码方位装置的机械臂末端跟踪测量系统，其特征在于：其包括：机械臂（1）、多面体编码点特征测量靶标（2）、机械臂末端执行器（3）、机械臂控制柜（4）、测量像机（5）和上位机（6）；其中：机械臂（1）为电动多关节工业机械臂，其活动末端装有多面体编码点特征测量靶标（2）和机械臂末端执行器（3）；

多面体编码点特征测量靶标（2）为带有编码特征点的测量识别装置，其安装在机械臂（1）与机械臂末端执行器（3）的连接部位；

机械臂末端执行器（3）为机械臂（1）的操作执行部件，机械臂（1）通过安装在其末端的机械臂末端执行器（3）完成预定动作；

机械臂控制柜（4）为工业机械臂（1）的驱动控制部件，其与机械臂（1）相连接，用于控制机械臂（1）工作；

测量像机（5）为单目CCD图像采集装置，其与上位机（6）相连接，用于采集整个操作区的图像信息，并传送给上位机（6）；

上位机（6）为控制计算机，为本系统的控制核心；其与机械臂控制柜（4）相连接。

2.根据权利要求1所述的基于编码方位装置的机械臂末端跟踪测量系统，其特征在于：所述的多面体编码点特征测量靶标（2）为多面体编码方位装置，其为一多面体形状，其中轴部位设有连接轴（123），连接轴（123）的两端分别与机械臂（1）的末端和机械臂末端执行器（3）相连接；多面体编码点特征测量靶标（2）上具有多个矩形侧面，每一侧面构成一个测量面，每个测量面的表面粘贴有一至多个编码特征点；每个编码特征点包含（5）个相对位置固定的特征位（121）和3个编码位（120）。

3.根据权利要求2所述的基于编码方位装置机械臂末端跟踪测量系统，其特征在于：所述的编码特征点采用黑色背景，在黑色背景上根据排列规则粘贴由高反射率反光材料制成的特征位（121）和编码位（120）。

4.一种如权利要求1所述的基于编码方位装置的机械臂末端跟踪测量系统的跟踪测量方法，其特征在于：所述的测量方法包括按顺序执行的下列步骤：

步骤一、工作空间整体布局和装置固定的S01阶段：根据机械臂（1）的运动轨迹、工作范围，将测量像机（5）固定在待加工工件（7）上；

步骤二、测量像机与工件的预标定、建立靶标和末端执行器校准数据库的S02阶段：将工件空间坐标预先校准，经工件工装定位与预定标，像机坐标系(OCXCYCZC)在工件坐标系（OWXWYWZW）下的位姿（TCW,RCW）为固定值且已知；通过对基准校准球（8）进行校准来确定机械臂末端执行器（3）的位置在靶标坐标系下的坐标转换关系，从而确定末端执行器坐标系（ODXDYDZD）在靶标坐标系（OEXEYEZE）下的位姿（TDE,RDE）；

步骤三、工作空间坐标系建立的S03阶段：在机械臂（1）基座处建立机械臂坐标系（ORXRYRZR）；靶标坐标系（OEXEYEZE）在编码特征点或靶标其他位置上选取；末端执行器坐标系（ODXDYDZD）建立在机械臂末端执行器（3）的末端；

步骤四、机械臂到达预定位置的S04阶段：通过上位机（6）输入目标空间坐标参数，利用机械臂控制柜（4）控制机械臂（1）到达预定位置；

步骤五、靶标实时成像测量的S05阶段：测量像机（5）对安装在机械臂（1）与机械臂末端执行器（3）之间的多面体编码点特征测量靶标（2）进行实时成像测量，然后将图像数据传递到上位机（6）；

步骤六、获取机械臂末端执行器姿态的S06阶段：上位机（6）对上述测量图像经过编码特征点扫描、质心定位、编码特征点识别，后方交会求解像机外方位和相对方位，利用外极线约束外极角匹配方法进行编码特征点匹配，基于共线性约束的优化平差求解编码特征点空间三维坐标；像机基站成像中利用空间存在多于3点的相互之间空间距离已知的特征点与成像点之间的关系，采用基于空间共角约束条件下的交会姿态估计算法和多外部方位装置优化确定像机基站的绝对外部方位（TCW,RCW），即像机坐标系(OCXCYCZC)在工件坐标系（OWXWYWZW）下的位姿（TCW,RCW）；通过坐标逆变换获得靶标坐标系在像机坐标系(OCXCYCZC)下的位姿（TEC，REC）；

步骤七、获取机械臂末端执行器精确空间坐标的S07阶段：利用机械臂末端执行器（3）与多面体编码点特征测量靶标（2）之间的刚体连接关系和预校准并建立的末端执行器数据库，最终获得机械臂末端执行器（3）在工件坐标系下的精确坐标位置及姿态；

步骤八、补偿机械臂末端执行器位姿偏差的S08阶段：通过位姿检测补偿系统计算程序位姿和测量位姿偏差值，由上位机（6）发送补偿位移给机械臂控制柜（4），从而精确控制机械臂（1）的动作；

步骤九、判断预定动作是否完成的S09阶段：判断本次操作的预定动作是否全部完成，如果判断结果为“是”，则本操作流程结束，否则下一步返回S03阶段的入口处，继续下一步动作。

Images