CN110125455B - 一种用于机器人钻孔中优化钻头位姿的方法 - Google Patents

Abstract

本发明提出一种用于机器人钻孔中优化钻头位姿的方法，属于机器人钻孔作业中优化调整钻头位姿的领域。该方法首先搭建基于条纹投影法和视觉伺服控制的机器人钻孔系统并进行系统参数标定，然后进行机器人钻孔作业的离线规划得到一系列路径点；在每个路径点，计算钻头在该路径点期望位姿下的相位图，设计视觉特征并推导交互矩阵和视觉伺服控制律；最后利用视觉伺服控制律对离线规划的每个路径点的钻头位姿进行优化，直到每个路径点的视觉伺服收敛完成，以提高钻头轴线与待钻孔点处的切面的垂直度，进而提高钻孔质量和表面平整度。本发明结合条纹投影轮廓测量法测量精度高和视觉伺服控制精度高的优点，能有效优化实际机器人钻孔作业中钻头的位姿。

Description

一种用于机器人钻孔中优化钻头位姿的方法

技术领域

本发明属于机器人钻孔中优化调整钻头位姿的领域，尤其涉及一种用于机器人钻孔中优化钻头位姿的方法。

背景技术

钻孔是指在实体材料上加工出孔的手段。钻孔主要用来加工用于铆钉、螺钉螺母连接的孔，因此孔的质量和表面平整度对于连接的稳定性至关重要。传统的钻孔主要依赖钻床来保证孔的质量和表面平整度，而钻床钻孔需要夹持工件，而且无法处理复杂曲面的钻孔问题。为了解决这些问题，机器人钻孔技术应运而生。但机器人钻孔技术受限于离线规划中建模误差、工件变形、安装误差、工件与机器人坐标系标定误差以及机器人的绝对精度不高等缺点，难以保证钻孔时钻头轴线于钻孔点切面保持垂直。因此优化离线规划的钻头位姿，消除以上误差带来的影响，进而提高钻孔的质量和表面平整度就有了重要的技术意义。但当前尚没有一种通过测量钻孔位置局部三维形貌来调整机器人钻头位姿的方法，因此本领域尚属于空白。

发明内容

本发明的目的是针对机器人钻孔中离线规划的钻头位姿的精度受限于建模误差、工件变形、安装误差、工件与机器人坐标系标定误差以及机器人的绝对精度不高等缺点，提出一种用于机器人钻孔中优化钻头位姿的方法。本发明是结合条纹投影轮廓测量法的测量精度高，视觉伺服控制精度高的优点来优化离线规划的钻头位姿，提高钻头轴线与接触点的切面的垂直度，进而提高钻孔的质量和表面平整度。

本发明提出一种用于机器人钻孔中优化钻头位姿的方法，其特征在于，包括以下步骤：

1)搭建基于条纹投影法和视觉伺服控制的机器人钻孔系统；所述系统包括：机器人基座、机器人末端执行器、固定底座、钻孔装置、投影仪、相机和计算机；所述机器人基座固定在安装平台上，机器人末端执行器通过螺栓连接副连接固定底座，钻孔装置、投影仪和相机分别通过螺纹连接固定在固定底座上，使得待钻孔工件的待钻孔位置同时处在相机和投影仪的视野和景深范围以内；六自由度工业机器人、投影仪和相机分别连接计算机；

2)将待钻孔工件通过螺纹连接固定在光学平台上，将激光跟踪仪和机器人分别放置在待钻孔工件两侧设定距离处；分别构建各个坐标系如下：F_b表示机器人基座坐标系，F_e表示机器人末端执行器坐标系，F_p表示投影仪坐标系，F_c表示相机坐标系，F_d表示钻头坐标系，F_w表示待钻孔工件的工件坐标系，F_L表示激光跟踪仪坐标系；

3)通过相机投影仪内外参数标定法标定投影仪成像模型的内参矩阵M_p，标定相机成像模型的内参矩阵M_c，标定相机坐标系F_c与投影仪坐标系F_p之间的变换矩阵^pT_c,通过手眼标定标定相机坐标系F_c与机器人末端执行器坐标系F_e之间的变换矩阵^cT_e，利用激光跟踪仪标定固定在待钻孔工件坐标系F_w与激光跟踪仪坐标系F_L的变换矩阵^wT_L，利用激光跟踪仪标定机器人基座坐标系F_b与激光跟踪仪坐标系F_L的变换矩阵^bT_L，进而求出工件坐标系F_w与机器人基座坐标系F_b之间的变换矩阵^bT_w；根据固定底座三维模型和钻孔装置的三维模型测量和计算得到钻头坐标系F_d相对于机器人末端坐标系F_e的变换矩阵^eT_d；

4)在待钻孔工件的三维模型上确定对应的一个或多个钻孔点的位置，利用步骤3)中得到的变换矩阵^bT_w和^eT_d，进行机器人钻孔的离线规划，得到每个钻孔点对应的路径点，并计算机器人在每个路径点对应的六个关节的关节角度；具体方法如下：

在待钻孔工件的的三维模型上选定任意一个钻孔点的位置，读取该钻孔点在工件坐标系下的坐标,并生成该点处切面特征，然后读取该切面特征的法向向量，约束钻头坐标系F_d的Z轴于该法向向量同轴，同时使钻头坐标系原点到该钻孔点处于设定距离，得到钻头坐标系F_d相对工件坐标系F_w的变换矩阵^dT_w，再结合变换矩阵^bT_w和^eT_d求得对应钻头坐标系下机器人末端坐标系F_e相对于基座坐标系F_b的变换矩阵^bT_e＝^bT_w·^dT_w ^-1·^eT_d ^-1，进而通过机器人逆运动学求得对应此变换矩阵下机器人在此钻孔点对应路径点的各个关节的关节角度；

5)将钻头移动至步骤3)离线规划得到的第一个路径点，将该路径点记为当前路径点，在当前路径点的钻头的位姿即为该路径点待优化的钻头位姿；

6)计算钻头在当前路径点期望位姿下的相位图；具体步骤如下：

6-1)在当前路径点，利用条纹投影轮廓测绘法，投影仪投影N张不同相位的单位频率正弦条纹图和N张不同相位的高频率正弦条纹图到待钻孔工件表面，同时利用相机采集待钻孔工件表面通过投影得到的2N张变形的条纹图，通过解码算法得到此钻头位姿下的对应相位图；然后利用基于三角测量原理的三维重构算法，从相位图中重构出待钻孔工件表面的三维点云，该三维点云位于投影仪坐标系F_p中；

6-2)利用钻头坐标系F_d相对于机器人末端执行器坐标系F_e的变换矩阵^dT_e以及投影仪坐标系F_p相对于机器人末端执行器坐标系F_e的变换矩阵^pT_e的逆矩阵进行矩阵相乘，得到钻头坐标系F_d相对于机器人末端执行器坐标系F_e的变换矩阵^dT_p：^dT_p＝^dT_e·^pT_e ^-1；

计算钻头轴线与步骤6-1)得到的三维点云的交点坐标，交点为三维点云中到钻头轴线的距离最近的点；

6-3)取交点坐标附近的M个点进行二次曲面拟合得到待钻孔工件表面的二次曲面方程，然后求得该方程对应的曲面在该交点处的切面及切面的法向向量，该法向向量即为期望的钻头轴线的方向；

6-4)根据步骤6-1)得到的位于投影仪坐标系F_p中的三维点云，以及步骤6-3)得到的期望的钻头轴线的方向，在仿真环境下将三维点云整体移动到使钻头轴线和期望的钻头轴线重合且待钻孔点与钻头坐标系原点距离符合设定距离的位置，得到待钻孔工件表面新的三维点云；以该新的三维点云为待测量表面，通过模拟投影过程的算法计算待测量表面对应的相位图，该相位图即为钻头在期望位姿下的相位图；

7)设计视觉特征并推导交互矩阵和视觉伺服控制律；

将钻头在期望位姿下，即钻头轴线垂直待钻孔点切面时，钻头轴线与待钻孔工件新的三维点云的交点在步骤6-4)得到的相位图中对应的位置作为该相位图中待钻孔点，将该待钻孔点附近的圆形区域的元素值整体排列为视觉特征向量：X_p＝(x_p1,…,x_pn)，其中x_pi为圆形区域内第i个元素的值，圆形区域内元素的总数量为n；

建立对应视觉特征的视觉伺服的交互矩阵为：

其中

其中，

r₁,r₂,r₃,t₁,t₂,t₃来自相机坐标系和投影仪坐标系的变换矩阵^pT_c＝(r₁,r₂,r₃,(t₁,t₂,t₃)^T)，x_p和x_c分别为空间点在投影仪和相机成像平面的投影的横坐标，y_p和y_c分别为空间点在投影仪和相机成像平面的投影的纵坐标，

对应的视觉伺服控制律为：

其中V_c为相机的六维速度，λ是增益系数，

是交互矩阵的伪逆的近似估计量，令

其中

为交互矩阵的伪逆，e＝(s-s^*)为误差项，其中s^*为期望位姿下对应的视觉特征；

8)将当前路径点作为视觉伺服的初始状态，利用步骤7)中得到视觉伺服控制律对在该路径点上钻头的位姿进行优化，直到该路径点的视觉伺服收敛完成，该路径点对应的钻头位姿优化完毕；

9)机器人根据优化后的钻头位姿在当前路径点完成钻孔，然后移动至下一路径点，并将下一路径点当成新的当前路径点，然后重新返回步骤6)，直至所有路径点的视觉伺服收敛完成，方法结束。

本发明的特点及有益效果在于：

1)本方法结合了条纹投影轮廓测量法测量精度高以及视觉伺服控制精度高的优点，可以消除交互矩阵标定误差，建模误差等引起的离线规划误差，有效提高实际钻孔作业过程中钻头的位姿精度，进而提高钻孔质量和表面平整度。

2)本方法中的视觉伺服控制是直接以相位图作为视觉特征，而不对其进行特征提取，因此避免了繁琐的图像处理过程，同时能充分利用相位图的信息，保证视觉伺服控制的精度和鲁棒性。

3)本方法不依赖对象的表面特征，不仅可以适用于带钻孔表面为平面的情况，也可以适用于待钻孔工件表面为复杂曲面的情况。

附图说明

图1为本发明实施例中基于条纹投影法和视觉伺服控制的机器人钻孔系统结构示意图。

图2为本发明实施例中的单位频率条纹图和高频条纹图。

图中：1-机器人基座，2-机器人末端执行器，3-固定底座，4-钻孔装置，5-投影仪，6-相机，7-待钻孔工件，8-激光跟踪仪。

具体实施方式

本发明提出一种用于机器人钻孔中优化钻头位姿的方法，下面结合附图和具体实施例对本发明进一步详细说明如下。以下实施例用于说明本发明，但不限于本发明的范围。

本发明提出一种用于机器人钻孔中优化钻头位姿的方法，包括以下步骤：

1)搭建基于条纹投影法和视觉伺服控制的机器人钻孔系统；

本发明实施例的基于条纹投影法和视觉伺服控制的机器人钻孔系统结构如图1所示，所述系统包括：六自由度工业机器人(以下简称机器人)基座1，机器人末端执行器2，固定底座3，钻孔装置4，投影仪5，相机6和计算机。所述六自由度工业机器人基座1通过螺纹连接固定在厚度为20mm的安装平台上，机器人末端执行器通过螺栓连接副连接固定底座3，钻孔装置4、投影仪5和相机6分别通过螺纹连接固定在固定底座3上，其中投影仪5与相机6的位置关系需要保证：工件的待钻孔位置同时处在相机和投影仪的视野和景深范围以内；六自由度工业机器人的控制器通过网线连接计算机，投影仪和相机分别通过USB3.0连接线连接计算机。

2)将待钻孔工件7通过螺纹连接固定在光学平台上，为保证激光跟踪仪8测量过程中机器人不干涉对靶标的检测，激光跟踪仪和机器人分别放置在待钻孔工件两侧设定距离处；激光跟踪仪放置过远会导致测量精度受影响，放置过近会导致能测量的范围有限，因此一般距离待钻孔工件50～200cm，即可符合要求，本实施例中激光跟踪仪8放置在离待钻孔工件80cm左右。待钻孔工件放置在机器人的操作空间内即可，但为保证机器人末端执行器的刚度，防止钻孔过程中产生太大的阻力导致钻头振动，工件不宜离机器人太远。本实施例中，工件水平放置在距地面100cm，距机器人150cm左右的位置。需要指出的是，激光跟踪仪在本方法仅来标定工件相对机器人基座的变换矩阵，它可以被其他的精密测量仪器替代。分别构建各个坐标系如图1所示，图1中各个坐标系分别由如下符号表示：F_b表示机器人基座坐标系，F_e表示机器人末端执行器坐标系，F_p表示投影仪坐标系，F_c表示相机坐标系，F_d表示钻孔装置中的钻头坐标系，F_w表示待钻孔工件的工件坐标系，F_L表示激光跟踪仪坐标系。

本发明中所有部件均可采用常规型号。本实施例中，相机为分辨率为2048X2560的灰度工业相机，型号为JAI GO5000；投影仪为分辨率为912X140的德州仪器的DLP 4500Pro；固定底座同时将相机，投影仪，钻孔装置固定到机器人末端执行器上，固定底座的材料为15mm厚的钢板；六自由度工业机器人为ABB IRB 4600，钻孔装置的伺服控制器和机器人控制柜一样属于支撑硬件，对建模过程没有影响，所以钻孔装置的伺服控制器和机器人控制柜都没有在图中描述，放置在安全的位置即可；计算机为联想Y720。上述所有硬件均无特殊要求。

3)参数标定；

通过相机投影仪内外参数标定法标定投影仪成像模型的内参矩阵M_p，标定相机成像模型的内参矩阵M_c，标定相机坐标系F_c与投影仪坐标系F_p之间的变换矩阵^pT_c,通过手眼标定标定相机坐标系F_c与机器人末端执行器坐标系F_e之间的变换矩阵^cT_e，利用激光跟踪仪标定固定在待钻孔工件坐标系F_w与激光跟踪仪坐标系F_L的变换矩阵^wT_L，利用激光跟踪仪标定机器人基座坐标系F_b与激光跟踪仪坐标系F_L的变换矩阵^bT_L，进而求出工件坐标系F_w与机器人基座坐标系F_b之间的变换矩阵^bT_w；根据一个自己设计的连接相机、投影仪、钻孔装置和机器人末端执行器的固定底座三维模型和钻孔装置的三维模型(底座的三维模型为自己在SolidWorks绘制所得，钻孔装置的三维模型可以向厂家索取或自己测量得到)测量和计算得到钻头坐标系F_d相对于机器人末端坐标系F_e的变换矩阵^eT_d。

4)离线规划；

根据实际需求确定一个或多个钻孔点的位置，并在待钻孔工件的三维模型(这里的三维模型是带钻孔工件的三维模型，根据不同零件而异，而且可以是用于加工这一工件的三维模型，也可以是利用逆工程学测量工件从而三维重建得到的三维模型)上确定对应的一个或多个钻孔点的位置，利用步骤3)中得到的变换矩阵^bT_w和^eT_d，进行机器人钻孔的离线规划，计算机器人在规划的每个路径点(每一个路径点对应一个钻孔点的位置)对应的六个关节的关节角度。具体方法如下：

首先根据实际需求，在CATIA环境下的待钻孔工件的的三维模型上选定一个或多个钻孔点的位置，本实施例取一个钻孔点，并读取该钻孔点在工件坐标系下的坐标,并生成该点处切面特征，然后读取该切面特征的法向向量，约束钻头坐标系F_d的Z轴于该法向向量同轴，同时使钻头坐标系原点到该钻孔点的距离为5-10mm最合适，本实施例取5mm，该距离视钻头坐标系原点与钻头尖点的位置关系而定，本实施例将钻头坐标系原点与钻头尖点重合。由上述约束可以得到钻头坐标系F_d与工件坐标系F_w的变换矩阵^dT_w，再结合变换矩阵^bT_w和^eT_d，可以求得对应钻头坐标系下机器人末端坐标系F_e相对于基座坐标系F_b的变换矩阵^bT_e＝^bT_w·^dT_w ^-1·^eT_d ^-1，进而通过机器人逆运动学求得对应此变换矩阵下机器人在此钻孔点对应路径点的各个关节的关节角度。

5)将钻头移动至步骤3)离线规划得到的第一个路径点，将该路径点记为当前路径点，在当前路径点的钻头的位姿即为该路径点待优化的钻头位姿；

6)计算钻头在当前路径点期望位姿下的相位图；具体步骤如下：

6-1)在当前路径点，利用条纹投影轮廓测绘法(Fringe ProjectionProfilometry)，投影仪投影N张不同相位的单位频率正弦条纹图和N张不同相位的高频率正弦条纹图到待钻孔工件表面，本实施例的单位频率正弦条纹图和高频率正弦条纹图如图2所示，其中，图2(a)为单位频率正弦条纹图，图2(b)为高频正弦条纹图。同时利用相机采集待钻孔工件表面通过投影得到的2N张变形的条纹图，通过解码算法得到此钻头位姿下的对应相位图；N的值越高，条纹投影轮廓测绘法的精度就越高，但同时测量时间也会增加，为了兼顾精度和速度，本实施例的N取为20；相位图为一个和相机的分辨率相同大小的矩阵，其中每个元素的值是对应该元素位置的空间点在投影仪成像平面下的像点的横坐标；然后利用基于三角测量原理的三维重构算法，从相位图中重构出待钻孔工件表面的三维点云，该三维点云位于投影仪坐标系F_p中。

6-2)利用钻头坐标系F_d相对于机器人末端执行器坐标系F_e的变换矩阵^dT_e以及投影仪坐标系F_p相对于机器人末端执行器坐标系F_e的变换矩阵^pT_e的逆矩阵进行矩阵相乘，可以得到钻头坐标系F_d相对于机器人末端执行器坐标系F_e的变换矩阵^dT_p：^dT_p＝^dT_e·^pT_e ^-1；由于钻头坐标系F_d被定义为其Z轴与钻头轴线方向重合，由此可以计算得到钻头轴线与步骤6-1)中得到的三维点云的交点坐标，此交点即为该三维点云中的待钻孔点；考虑到点云并不连续，因此交点取为点云中到钻头轴线的距离最近的点。

6-3)取交点坐标附近的M个点进行二次曲面拟合得到待钻孔工件表面的二次曲面方程，然后求得该方程对应的曲面在该交点处的切面及切面的法向向量，该法向向量即为期望的钻头轴线的方向。M的取值视相机的分辨率和待钻孔区域的大小而定，M太小无法保证拟合精度，M太大则不能很好地反映待钻孔区域的形貌，一般为1000到10000，本实施例取M为5000。

6-4)根据步骤6-1)得到的位于投影仪坐标系F_p中的三维点云，以及步骤6-3)得到的期望的钻头轴线的方向，在仿真环境下将三维点云整体移动到使钻头轴线和期望的钻头轴线重合，且待钻孔点与钻头坐标系原点(即钻头尖点)距离一定距离的位置，此距离为实际钻孔中钻头尖点到待钻孔点的距离，不宜太远，否则钻孔时容易产生跳动，同样不宜太近，否则钻头在移动时可能会与工件表面发生碰撞，一般10mm至30mm，本实施例取20mm；然后以这一位置的三维点云为待测量表面，通过模拟投影过程的算法计算出其对应的相位图，该相位图即为钻头在期望位姿下的相位图。

7)设计视觉特征并推导交互矩阵和视觉伺服控制律。

为了充分利用相位图的全部信息，保证方法的精确性和鲁棒性，同时免于复杂的特征提取过程，将步骤6-4)得到的相位图中待钻孔点(此点为在期望位姿时，即钻头轴线垂直待钻孔点切面时，钻头轴线与待钻孔工件新的三维点云的交点在相位图中对应的位置)附近的圆形区域的全部元素值直接作为视觉特征，圆形区域的半径一般取50-100个像素，本实施例取60个像素。将上述圆形区域的元素值整体排列为视觉特征向量：X_p＝(x_p1,…,x_pn)，其中x_pi为圆形区域内第i个元素的值，圆形区域内元素的总数量为n。经过相应推导，建立对应视觉特征的视觉伺服的交互矩阵被推导为：

其中

其中，

其中的参数r₁，r₂，r₃，t₁，t₂，t₃来自相机坐标系和投影仪坐标系的变换矩阵^pT_c＝(r₁，r₂，r₃，(t₁,t₂,t₃)^T)，x_p和x_c分别为空间点在投影仪和相机成像平面的投影的横坐标，y_p和y_c分别为空间点在投影仪和相机成像平面的投影的纵坐标，

对应的视觉伺服控制律为：

其中V_c为相机的六维速度，λ是增益系数，

是交互矩阵的伪逆的近似估计量，本发明取

其中

为交互矩阵的伪逆，e＝(s-s^*)为误差项，其中s^*为期望位姿下对应的视觉特征；

8)进行视觉伺服控制，优化钻头位姿。

将当前路径点作为视觉伺服的初始状态，利用步骤7)中得到视觉伺服控制律对在该路径点上钻头的位姿进行优化，直到该路径点的视觉伺服收敛完成，该路径点对应的钻头位姿优化完毕；

9)机器人根据优化后的钻头位姿在当前路径点完成钻孔，然后移动至下一路径点，并将下一路径点当成新的当前路径点，然后重新返回步骤6)，直至所有路径点的视觉伺服收敛完成，方法结束。

由于本发明结合了条纹投影轮廓测量法测量精度高和视觉伺服控制精度高的优点，所以可以消除交互矩阵标定误差，建模误差等引起的离线规划误差，提高实际钻孔作业过程中钻头的位姿精度，进而提高钻孔质量和表面平整度。此外，本方法中的视觉伺服控制是直接以相位图作为视觉特征，而不对其进行特征提取，因此避免了繁琐的图像处理过程，同时能充分利用相位图的信息，保证视觉伺服控制的精度和鲁棒性

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员，在不脱离本发明技术原理的前提下，还可以做出若干改进和替换，这些改进和替换也应视为本发明的保护范围。

Claims (1)

1.一种用于机器人钻孔中优化钻头位姿的方法，其特征在于，包括以下步骤：

1)搭建基于条纹投影法和视觉伺服控制的机器人钻孔系统；所述系统包括：机器人基座、机器人末端执行器、固定底座、钻孔装置、投影仪、相机和计算机；所述机器人基座固定在安装平台上，机器人末端执行器通过螺栓连接副连接固定底座，钻孔装置、投影仪和相机分别通过螺纹连接固定在固定底座上，使得待钻孔工件的待钻孔位置同时处在相机和投影仪的视野和景深范围以内；六自由度工业机器人、投影仪和相机分别连接计算机；

2)将待钻孔工件通过螺纹连接固定在光学平台上，将激光跟踪仪和机器人分别放置在待钻孔工件两侧设定距离处；分别构建各个坐标系如下：F_b表示机器人基座坐标系，F_e表示机器人末端执行器坐标系，F_p表示投影仪坐标系，F_c表示相机坐标系，F_d表示钻头坐标系，F_w表示待钻孔工件的工件坐标系，F_L表示激光跟踪仪坐标系；

3)通过相机投影仪内外参数标定法标定投影仪成像模型的内参矩阵M_p，标定相机成像模型的内参矩阵M_c，标定相机坐标系F_c与投影仪坐标系F_p之间的变换矩阵^pT_c,通过手眼标定标定相机坐标系F_c与机器人末端执行器坐标系F_e之间的变换矩阵^cT_e，利用激光跟踪仪标定固定在待钻孔工件坐标系F_w与激光跟踪仪坐标系F_L的变换矩阵^wT_L，利用激光跟踪仪标定机器人基座坐标系F_b与激光跟踪仪坐标系F_L的变换矩阵^bT_L，进而求出工件坐标系F_w与机器人基座坐标系F_b之间的变换矩阵^bT_w；根据固定底座三维模型和钻孔装置的三维模型测量和计算得到钻头坐标系F_d相对于机器人末端坐标系F_e的变换矩阵^eT_d；

4)在待钻孔工件的三维模型上确定对应的一个或多个钻孔点的位置，利用步骤3)中得到的变换矩阵^bT_w和^eT_d，进行机器人钻孔的离线规划，得到每个钻孔点对应的路径点，并计算机器人在每个路径点对应的六个关节的关节角度；具体方法如下：

在待钻孔工件的的三维模型上选定任意一个钻孔点的位置，读取该钻孔点在工件坐标系下的坐标,并生成该点处切面特征，然后读取该切面特征的法向向量，约束钻头坐标系F_d的Z轴于该法向向量同轴，同时使钻头坐标系原点到该钻孔点处于设定距离，得到钻头坐标系F_d相对工件坐标系F_w的变换矩阵^dT_w，再结合变换矩阵^bT_w和^eT_d求得对应钻头坐标系下机器人末端坐标系F_e相对于基座坐标系F_b的变换矩阵^bT_e＝^bT_w·^dT_w ^-1·^eT_d ^-1，进而通过机器人逆运动学求得对应此变换矩阵下机器人在此钻孔点对应路径点的各个关节的关节角度；

5)将钻头移动至步骤3)离线规划得到的第一个路径点，将该路径点记为当前路径点，在当前路径点的钻头的位姿即为该路径点待优化的钻头位姿；

6)计算钻头在当前路径点期望位姿下的相位图；具体步骤如下：

6-1)在当前路径点，利用条纹投影轮廓测绘法，投影仪投影N张不同相位的单位频率正弦条纹图和N张不同相位的高频率正弦条纹图到待钻孔工件表面，同时利用相机采集待钻孔工件表面通过投影得到的2N张变形的条纹图，通过解码算法得到此钻头位姿下的对应相位图；然后利用基于三角测量原理的三维重构算法，从相位图中重构出待钻孔工件表面的三维点云，该三维点云位于投影仪坐标系F_p中；

6-2)利用钻头坐标系F_d相对于机器人末端执行器坐标系F_e的变换矩阵^dT_e以及投影仪坐标系F_p相对于机器人末端执行器坐标系F_e的变换矩阵^pT_e的逆矩阵进行矩阵相乘，得到钻头坐标系F_d相对于机器人末端执行器坐标系F_e的变换矩阵^dT_p：

计算钻头轴线与步骤6-1)得到的三维点云的交点坐标，交点为三维点云中到钻头轴线的距离最近的点；

6-3)取交点坐标附近的M个点进行二次曲面拟合得到待钻孔工件表面的二次曲面方程，然后求得该方程对应的曲面在该交点处的切面及切面的法向向量，该法向向量即为期望的钻头轴线的方向；

6-4)根据步骤6-1)得到的位于投影仪坐标系F_p中的三维点云，以及步骤6-3)得到的期望的钻头轴线的方向，在仿真环境下将三维点云整体移动到使钻头轴线和期望的钻头轴线重合且待钻孔点与钻头坐标系原点距离符合设定距离的位置，得到待钻孔工件表面新的三维点云；以该新的三维点云为待测量表面，通过模拟投影过程的算法计算待测量表面对应的相位图，该相位图即为钻头在期望位姿下的相位图；

7)设计视觉特征并推导交互矩阵和视觉伺服控制律；

将钻头在期望位姿下，即钻头轴线垂直待钻孔点切面时，钻头轴线与待钻孔工件新的三维点云的交点在步骤6-4)得到的相位图中对应的位置作为该相位图中待钻孔点，将该待钻孔点附近的圆形区域的元素值整体排列为视觉特征向量：X_p＝(x_pt，…，x_pn)，其中x_pi为圆形区域内第i个元素的值，圆形区域内元素的总数量为n；

建立对应视觉特征的视觉伺服的交互矩阵为：

其中

其中，

r_t，r₂，r₃，t_t，t₂，t₃来自相机坐标系和投影仪坐标系的变换矩阵^pT_c＝(r_t，r₂，r₃，(t₁，t₂，t₃)^T)，x_p和x_c分别为空间点在投影仪和相机成像平面的投影的横坐标，y_p和y_c分别为空间点在投影仪和相机成像平面的投影的纵坐标，

对应的视觉伺服控制律为：

其中V_c为相机的六维速度，λ是增益系数，

是交互矩阵的伪逆的近似估计量，令

其中

为交互矩阵的伪逆，e＝(s-s^*)为误差项，其中s^*为期望位姿下对应的视觉特征；

8)将当前路径点作为视觉伺服的初始状态，利用步骤7)中得到视觉伺服控制律对在该路径点上钻头的位姿进行优化，直到该路径点的视觉伺服收敛完成，该路径点对应的钻头位姿优化完毕；

9)机器人根据优化后的钻头位姿在当前路径点完成钻孔，然后移动至下一路径点，并将下一路径点当成新的当前路径点，然后重新返回步骤6)，直至所有路径点的视觉伺服收敛完成，方法结束。

Images