[发明专利]一种用于机器人钻孔中优化钻头位姿的方法

摘要

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

主权项

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)，直至所有路径点的视觉伺服收敛完成，方法结束。