[发明专利]一种三维激光雷达与机械臂间的自动联合标定方法

权利要求书

1.一种三维激光雷达与机械臂间的自动联合标定方法，其特征在于：包括如下步骤：

第一步、随机生成目标位姿：通过MoveIt！中的“randomvalid”选项，在机器人的工作范围内随机生成目标姿态，使机械臂末端每次在三维激光雷达的检测范围内具有不同的姿态；

第二步、三维激光雷达原始点云预处理及优化：从点云本身出发，采用内插原理的空洞修补，即依靠与空洞周围网格曲率相关的曲面，结合周边顶点对曲面进行内插新顶点以达到空洞修补的目的，具体步骤如下：

步骤一、在各数据点的K邻域范围内搜索最大角度差，根据角度差Δδ是否大于角度差阈值Δδ_th来判断边界特征点，若Δδ≥Δδ_th则为边界特征点；

步骤二、通过边界追踪法将无序的边界点连接成闭合的空洞边界线；

步骤三、通过空洞邻近域的特征面建立局部坐标系，按移动最小二乘法来拟合二次曲面并按曲面模型将边界点序列进行参数化；

步骤四、通过参数化的边界点求解基函数p_i(x)的待定系数α_i(x)并进行U向和V向插值来得到空洞区域的填充点，针对孤立点噪声，采用K-邻域方法来剔除孤立噪声点，即指定数据点并搜索指定数据点一定半径r范围内的邻点个数，若邻点个数n小于邻点个数阈值n_th，则该指定数据点为孤立噪声点，针对点云数据简化问题，在保持被测物体几何特征的前提下，根据物体的曲率特征对测量数据进行精简，以提高曲面重构的效率和精度，在小曲率区域保留少量的点来表示很大的面积，在大曲率区域保留足够多的点来表达局部特征以精确、完整、精简地表示曲面特征；

第三步、机械臂末端抓取位置及各关节位姿计算：根据点云立体特性和疏密性进行每个平面拟合，根据拟合结果进行判断，长方体的八个顶点在三维激光雷达点云中的顶点个数为4个、6个或7个，具体情况如下：

情况一、当长方体的八个顶点在三维激光雷达点云中的顶点个数为6个或者7个时：根据拟合的平面得到一个完整的长方体，根据长方体的体积V与标准体积V₀之间的误差ΔV＝|V-V₀|是否小于体积差阈值ΔV_th来判断是否为机械臂末端抓取的长方体，如果ΔV≤ΔV_th再根据长方体顶点的三维坐标关系，选择出构成上平面的四个顶点P_i，其中i＝1，2，3，4；假设机械臂末端抓取位置是上平面的中心处，根据平面中心位置坐标公式求出平面中心，即机械臂末端抓取位置，当机械臂末端在P₀处抓取长方体时，读取机械臂在控制器坐标系下的位置坐标P_r及各关节的位置坐标P_jointi；

情况二、当长方体的八个顶点在三维激光雷达点云中的顶点个数为4个时：根据拟合的平面去计算拟合平面的两个边长l_i，其中i＝1，2；在标准长方体的三个边长l_oi中随机取两个边长，其中i＝1，2，3；然后求均方根误差判断S是否小于均方根误差阈值S_th，若S≤S_th，则拟合的平面为长方体三个平面之一，根据拟合平面的四个顶点P_i，其中i＝1，2，3，4；同情况一中所述求出机械臂末端抓取位置并读取机械臂在控制器坐标系下的位置坐标P_r各关节的位置坐标P_jointi；

第四步、利用最小二乘法求解最优旋转矩阵R和平移矩阵T：根据P₀＝RP_r+T，将机械臂在控制器坐标系下的位置坐标转换到三维激光雷达坐标系下，由于三维点云信息精度、机械臂抓取精度等均会影响坐标变换的精度，因此需要通过多次抓取来减小误差，重复N次抓取过程，其中N≥3，得到N组坐标P₀、P_r，通过对最小二乘法的误差函数求解满足成立的R和T，即最优的旋转矩阵和平移矩阵，根据MoveIt！中的IKFAST求解出各关节坐标系转换到机械臂末端坐标系的旋转矩阵R_joint和平移矩阵T_joint，此时通过P_r＝R_jointP_joint+T_joint便可将各关节坐标系转换到机械臂坐标系，再根据机械臂坐标系转换到三维激光雷达坐标系下的旋转矩阵R和平移矩阵T，通过P₀＝RP_r+T＝R(R_jointP_joint+T_joint)+T＝RR_jointP_joint+(RT_joint+T)＝R_totalP_joint+T_total计算出各关节坐标系转换到三维激光雷达坐标系下的旋转矩阵R_total＝RR_joint和平移矩阵T_total＝RT_joint+T，同理，也可根据上述最小二乘法来求出各关节坐标系转换到三维激光雷达坐标系下的最优旋转矩阵和平移矩阵。