具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种基于位姿跟踪系统机械臂目标点在线追踪方法及系统，实现了柔性化、高精度的加工制造，同时也有效提高了机械臂自动化水平。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

图1为本发明所提供的一种基于位姿跟踪系统机械臂目标点在线追踪方法流程示意图，如图1所示，本发明所提供的一种基于位姿跟踪系统机械臂目标点在线追踪方法，包括：

S101，确定机械臂加工场景，并如图2所示；所述机械臂加工场景为将待加工工件和刚体安装在夹具上，在机械臂末端的刀具上安装相应的刚体，并将机械臂安装在加工平台上；所述位姿跟踪系统用于测量刚体在空间中的位置与姿态；所述位姿跟踪系统包括：光学跟踪系统；所述光学跟踪系统包括：两个相机；

S102，在所述机械臂加工场景中，利用所述位姿跟踪系统，确定待加工工件基于位姿追踪系统的位姿以及刀具基于位姿跟踪系统的位姿；并根据几何关系以及参照物变化关系确定刀具基于机械臂末端的位姿、刀具末端相对于刀具的位姿以及机械臂机座相对于位姿跟踪系统的位姿。

S102，之前还包括：

在位姿跟踪系统中建立测量坐标系；以所述待加工工件上安装的刚体为原点建立工件坐标系；以刀具上安装的刚体为原点建立刀具坐标系；以机械臂末端为原点建立械臂末端坐标系；以机械臂机座为原点建立机械臂机座坐标系；以刀具末端为原点建立刀具末端坐标系。

S102具体包括：

根据所述位姿跟踪系统确定待加工工件基于位姿跟踪系统的位姿以及刀具基于位姿跟踪系统的位姿；

根据所述机械臂末端与刀具的几何关系，确定所述刀具基于所述机械臂末端的位姿；

根据机械臂的当前位型以及机械臂运动学正解确定机械臂末端基于机械臂机座的位姿；

利用公式T_MB＝T_MT(T_BET_ET)^-1确定机械臂机座相对于位姿跟踪系统的位姿；

利用公式T_BW＝(T_MB)^-1T_MW确定待加工工件相对于机械臂机座的位姿；

根据所述刀具基于所述机械臂末端的位姿、机械臂末端基于机械臂机座的位姿以及待加工工件相对于机械臂机座的位姿确定刀具末端相对于刀具的位姿；加工时，刀具末端的位姿与所述待加工工件位姿相同；即刀具末端的位姿与工件待加工工位位姿重合。

其中，T_MB为机械臂机座相对于位姿跟踪系统的位姿，T_MT为刀具基于位姿跟踪系统的位姿，T_BE为机械臂末端基于机械臂机座的位姿，T_ET为刀具基于所述机械臂末端的位姿，T_BW为待加工工件相对于机械臂机座的位姿，T_MW为待加工工件基于位姿跟踪系统的位姿。

即基于位姿跟踪系统可以准确获得工件在空间中的位姿为T_MW，其中{M}代表测量坐标系，位姿跟踪系统主要由两相机构成，测量坐标系建立在两相机之间，{W}代表工件坐标系，由于工件安装于夹具上，夹具上安装了相应的刚体，因此可以准确获得工件与夹具之间的位姿关系，通过位姿跟踪系统，可以准确获得刚体相对于测量坐标系的位姿，进而可以获得工件坐标系相对于测量坐标系的位姿；同时刀具上也安装了相应的刚体，因此基于位姿跟踪系统同样可以准确获得刀具在空间中的位姿为T_MT，其中{T}代表刀具坐标系。在位姿跟踪系统的测量范围内，随机选取一组机械臂的初始位型，通过位姿跟踪系统可以获得刀具坐标系相对于测量坐标系之间的相对位姿关系T_MT；根据几何关系，可以获得末端刀具基于机械臂末端的位姿为T_ET，其中{E}代表机械臂末端坐标系；基于机械臂的当前位型，通过机械臂运动学正解，可以获得机械臂末端基于机械臂机座的位姿为T_BE，其中{B}代表机械臂机座坐标系。各个坐标系的关系及建立如图3所示。

S103，根据所述待加工工件基于光学追踪系统的位姿、机械臂机座相对于位姿跟踪系统的位姿、刀具基于所述机械臂末端的位姿以及刀具末端相对于刀具的位姿构建系统运行学模型；

S103具体包括：

利用公式y_MC＝f(y_MB,y_ET,y_TC,g(p_n,x_r))确定系统运行学模型；

其中，y_MC为刀具末端相对于刀具的位姿向量，y_MB为机械臂机座相对于位姿跟踪系统的位姿向量，y_ET为刀具基于所述机械臂末端的位姿向量，y_TC为刀具末端相对于刀具的位姿，f代表系统正运动学，g代表机械臂正运动学，p_n代表机械臂名义参数，x_r代表机械臂的关节角度。

S104，根据所述系统运动学模型，利用位姿跟踪系统建立系统误差模型。

S104具体包括：

基于采样频率获取机械臂对应的关节角度值以及位姿跟踪系统测量获得的刀具位姿；

根据当前时刻的所述关节角度值、位姿跟踪系统测量获得的当前时刻的刀具位姿以及所述系统运动学模型确定当前时刻的输出的刀具末端坐标系相对于位姿跟踪系统的位姿；

将所述当前时刻的输出的刀具末端坐标系相对于位姿跟踪系统的位姿与当前时刻的实际测量的位姿进行比较，确定当前时刻的刀具末端的定位误差；

根据所述当前时刻的刀具末端的定位误差确定当前时刻的系统运动学参数误差；在机械臂运动过程中，持续迭代更新，将所述当前时刻的所述关节角度值替换为下一时刻的所述关节角度值，将当前时刻的刀具位姿替换为下一时刻的刀具位姿，并返回所述根据当前时刻的所述关节角度值、位姿跟踪系统测量获得的当前时刻的刀具位姿以及所述系统运动学模型确定当前时刻的输出的刀具末端坐标系相对于位姿跟踪系统的位姿的步骤，直至机械臂到达机械臂的目标点的目标时刻，确定目标时刻的刀具末端的定位误差和系统运动学参数误差；所述系统运动学参数误差包括：机械臂参数误差、刀具的安装误差以及机械臂的机座定位误差；

根据目标时刻的刀具末端的定位误差和系统运动学参数误差，利用公式y_MC+Δy_MC＝f(y_MB+Δy_MB,y_ET+Δy_ET,y_TC,g(p_n+Δp_n,x_r))确定系统误差模型；

其中，Δy_MC为目标时刻的刀具末端的定位误差，Δp_n为目标时刻的机械臂参数误差、Δy_ET为目标时刻的刀具的安装误差、Δy_MB为目标时刻的机械臂的机座定位误差。

由于机械臂受机械加工误差、装配误差等因素的影响，导致机械臂名义D-H参数存在偏差，本专利定义该偏差值为Δp_n；虽然刀具的几何尺寸相对较为准确，但在装配过程中存在一定的装配误差，本专利定义该误差值为Δy_ET；同样，机械臂机座相对于位姿跟踪系统存在定位误差，本专利定义该误差值为Δy_MB。由于上述误差的存在，最终导致刀具无法准确到达目标点位，刀具末端的定位误差为Δy_MC。

基于同一时刻，可采集到机械臂对应的关节角度值以及位姿跟踪系统测量获得的刀具位姿信息，将当前的关节角度值代入系统运动学方程中，即可求解出名义的刀具末端坐标系相对于位姿跟踪系统的位姿，将求得的名义位姿与实际测量的位姿进行比较即可获得当前的定位误差，即ΔyMC。基于当前的定位误差在线估计系统运动学参数误差,即[Δpn,ΔyET,ΔyMB]。同理，基于一定的采样频率，如此反复进行数据采集并在线估计系统运动学参数误差，最终能够实现对系统误差模型的准确估计。

通过位姿跟踪系统，可以准确获得工件上对应的待加工工位在空间中的位姿，基于机械臂末端刀具当前的初始状态进行路径规划。对于机械臂的路径规划，主要是通过机械臂的名义D-H参数，初始化的机械臂机座坐标系相对于测量系统坐标系的位姿，以及刀具坐标系相对于机械臂末端坐标系的位姿，根据系统逆运动学求解路径中各个点位对应的机械臂关节角度。将求解的关节角度下发至机械臂控制系统中，在机械臂运动过程中，通过位姿跟踪系统测量机械臂末端刀具的位姿。将机械臂末端刀具的实际位姿与定义的规划路径位姿进行比较获得位姿误差，即Δy_MC，基于位姿跟踪系统的测量误差在线估计系统运动学参数误差，即[Δp_n、Δy_ET、Δy_MB]，通过在线估计系统运动学参数误差，最终实现测量误差Δy_MC逐渐减小，以至于达到加工定位的精度要求。

S105，利用所述系统误差模型对所述机械臂的目标点进行在线位姿误差补偿。

在机械臂通过路径规划实时运动的过程中，基于位姿跟踪系统的测量误差可在线估计系统运动学参数误差，因此在机械臂运动至目标点后，通过估计的系统运动学参数误差进行系统逆运动学求解，进行在线位姿误差补偿，以实现机械臂较高的定位精度，同时通过机械臂运动过程中进行在线估计系统运动学参数误差，在机械臂到达目标点后，基于估计的参数误差实现了快速位姿误差补偿，提高了加工效率。

下面结合附图对本发明专利作进一步的详细描述。

参照图2，在该实体案例中，通过UR10机械臂进行轴孔装配实验，将刚体分别安装于刀具与工件上，通过位姿跟踪系统可实时测得两刚体在空间中的位置与姿态，为了表明实验的有效性，将工件安装在一个可移动的平台上。

通过位姿跟踪系统，可以准确获得工件相对于位姿跟踪系统的位姿关系为：

y_MW＝[x_MB，y_MB，z_MB，α_MB，β_MB，γ_MB]

＝[295.64，-537.85，-3440.09，17837，0.36，179.25]；

其中x，y，z的单位为mm，α，β，γ的单位为度。同时可获得机械臂初始状态下，当前位型下刀具末端的位姿为：

y_MC＝[-194.98，-151.31，-3792.28，178，37，0.36，179.25]

通过计算可获得机械臂机座坐标系相对于测量坐标系的位姿为：

y_MB＝[-104.89，-357.92，-4241.71，-90.50，-1.08，-1.44]

建立系统误差模型，由于该实体案例采用的是UR10机械臂，D-H的参数误差共有24个；同时存在刀具的安装误差以及机械臂机座的定位误差，因此该系统的误差模型参数共计26个。

对机械臂当前位姿到目标点进行路径规划，在该过程中共划分了50个点。在机械臂运动过程中，通过位姿跟踪系统测量刀具当前位姿，将机械臂末端刀具的实际位姿与定义的规划路径位姿进行比较，基于位姿跟踪系统的测量误差，使用扩展卡尔曼滤波算法在线估计系统运动学参数误差，具体可分为预测更新与测量更新阶段：

预测更新：

测量更新：

其中预测更新阶段表示运动学参数误差，k|k-1表示在k时刻的预测值，k-1|k-1表示在k-1时刻的估计值，表示估计协方差矩阵；测量更新阶段H表示测量矩阵，R表示高斯白噪声的协方差矩阵，K表示卡尔曼增益，z表示位姿误差。参照图4～11为通过50个点实时估计的参数误差。

由于系统误差的存在，最终导致机械臂无法准确到达目标点。通过估计的系统运动学参数误差进行系统逆运动学求解，进行在线位姿误差补偿，表1为到达目标点补偿前后机械臂对应的关节角度，表2为补偿前后的定位误差。参照图12为补偿前后轴孔装配的效果图。

表1在线位姿误差补偿前后机械臂关节角度

表2在线位姿误差补偿前后的定位误差

由于采用上述方案，本发明的优势有以下方面：所述方法具有较强的通用性，不限定机械臂自由度类型，能够普遍应用于机械臂的制造加工领域；引入位姿跟踪系统，可以通过测量刚体在空间中的位置和姿态，间接获得研究目标在空间中的位姿信息；将工件安装于定位夹具上，同时在定位夹具上安装刚体，因此通过位姿跟踪系统可获得工件在空间中的具体位姿，不仅如此，本专利提出在机械臂末端刀具上也安装相应刚体，通过该方式可初步确定工件相对机械臂之间的位姿关系，规避了使用示教的方式确定工件与机械臂之间的位姿关系，该套系统可以应用于不同工件的制造加工，实现了柔性化生产制造的加工模式；基于位姿跟踪系统的测量误差实现了机械臂在运动过程中对系统运动学参数误差的估计，在机械臂到达目标点后通过估计的参数进行在线位姿误差补偿，提高了机械臂的绝对定位精度，同时也提高了自动化水平。

图13为本发明所提供的一种基于位姿跟踪系统机械臂目标点在线追踪系统结构示意图，如图13所示，本发明所提供的一种基于位姿跟踪系统机械臂目标点在线追踪系统，包括：

机械臂加工场景确定模块1301，用于确定机械臂加工场景；所述机械臂加工场景为将待加工工件和刚体安装在夹具上，在机械臂末端的刀具上安装相应的刚体，并将机械臂安装在加工平台上；所述位姿跟踪系统用于测量刚体在空间中的位置与姿态；所述位姿跟踪系统包括：光学跟踪系统；所述光学跟踪系统包括：两个相机；

初始位姿确定模块1302，用于在所述机械臂加工场景中，利用所述位姿跟踪系统，确定待加工工件基于位姿追踪系统的位姿以及刀具基于位姿跟踪系统的位姿；并根据几何关系以及参照物变化关系确定刀具基于机械臂末端的位姿、刀具末端相对于刀具的位姿以及机械臂机座相对于位姿跟踪系统的位姿；

系统运行学模型构建模块1303，用于根据所述待加工工件基于光学追踪系统的位姿、机械臂机座相对于位姿跟踪系统的位姿、刀具基于所述机械臂末端的位姿以及刀具末端相对于刀具的位姿构建系统运行学模型；

系统误差模型建立模块1304，用于根据所述系统运动学模型，利用位姿跟踪系统建立系统误差模型；

位姿误差补偿模块1305，用于利用所述系统误差模型对所述机械臂的目标点进行在线位姿误差补偿。

所述位姿确定模块1302具体包括：

待加工工件和刀具位姿确定单元，用于根据所述位姿跟踪系统确定待加工工件基于位姿跟踪系统的位姿以及刀具基于位姿跟踪系统的位姿；

刀具基于所述机械臂末端的位姿确定单元，用于根据所述机械臂末端与刀具的几何关系，确定所述刀具基于所述机械臂末端的位姿；

机械臂末端基于机械臂机座的位姿确定单元，用于根据机械臂的当前位型以及机械臂运动学正解确定机械臂末端基于机械臂机座的位姿；

机械臂机座相对于位姿跟踪系统的位姿确定单元，用于利用公式T_MB＝T_MT(T_BET_ET)^-1确定机械臂机座相对于位姿跟踪系统的位姿；

待加工工件相对于机械臂机座的位姿确定单元，用于利用公式T_BW＝(T_MB)^-1T_MW确定待加工工件相对于机械臂机座的位姿；

刀具末端相对于刀具的位姿确定单元，用于根据所述刀具基于所述机械臂末端的位姿、机械臂末端基于机械臂机座的位姿以及待加工工件相对于机械臂机座的位姿确定刀具末端相对于刀具的位姿；加工时，刀具末端的位姿与所述待加工工件位姿相同；

其中，T_MB为机械臂机座相对于位姿跟踪系统的位姿，T_MT为刀具基于位姿跟踪系统的位姿，T_BE为机械臂末端基于机械臂机座的位姿，T_ET为刀具基于所述机械臂末端的位姿，T_BW为待加工工件相对于机械臂机座的位姿，T_MW为待加工工件基于位姿跟踪系统的位姿。

本发明所提供的一种基于位姿跟踪系统机械臂目标点在线追踪，还包括：

坐标系建立模块，用于在位姿跟踪系统中建立测量坐标系；以所述待加工工件上安装的刚体为原点建立工件坐标系；以刀具上安装的刚体为原点建立刀具坐标系；以机械臂末端为原点建立械臂末端坐标系；以机械臂机座为原点建立机械臂机座坐标系；以刀具末端为原点建立刀具末端坐标系。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。