一种三维激光雷达与机械臂间的自动联合标定方法

摘要

本发明公开了一种三维激光雷达与机械臂间的自动联合标定方法，其方法为：第一步、随机生成目标位姿；第二步、三维激光雷达原始点云预处理及优化：第三步、机械臂末端抓取位置及各关节位姿计算；第四步、利用最小二乘法求解最优旋转矩阵R和平移矩阵T。有益效果：降低了对标定参照物的依赖，标定过程简单、技术难度小、易于广泛推广与实现。简化点云数据等进行预处理和优化，大大降低了点云误差，提高而标定精度，有助于轮式机器人对目标的识别、定位和抓取等。能够应用在轮式机器人的复杂场景理解、大范围目标物体识别、定位、抓取及人机交互等人工智能了领域，对智能汽车领域的传感器自动联合标定具有重要意义。

权利要求

1.一种三维激光雷达与机械臂间的自动联合标定方法，其特征在于：包括如下步骤：第一步、随机生成目标位姿：通过MoveIt！中的“randomvalid”选项，在机器人的工作范围内随机生成目标姿态，使机械臂末端每次在三维激光雷达的检测范围内具有不同的姿态；

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

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

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

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

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

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

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

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

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

说明书

一种三维激光雷达与机械臂间的自动联合标定方法

技术领域

[0001] 本发明涉及一种自动联合标定方法，特别涉及一种三维激光雷达与机械臂间的自动联合标定方法。

背景技术

[0002] 目前，随着人工智能的快速发展，自动驾驶汽车等轮式机器人技术逐渐火热起来。通过传感器去感知周围环境，并对要抓取的物体进行识别、定位、抓取等，这其中离不开感
知传感器与机械臂之间的坐标系统一，这就要求需要对多传感器进行联合标定，使各个传
感器的信息数据融合在一起，从而实现轮式机器人通过三维激光雷达去感知周围环境，并
对要抓取的物体进行识别、定位，并将物体在三维激光雷达坐标系中的位置信息转换成机
械臂所在坐标系的位置信息，从而实现机械臂对物体的抓取过程。同时，根据机械臂末端的
坐标系，可以通过基于ROS的MoveIt！实现对机械臂各关节的反向运动学求解得到各关节的
坐标位置。通过上述标定可以将激光雷达和机械臂联合起来使用，可以根据周围环境信息
自主做出分析、决策等，对轮式机器人的技术发展等具有重要的意义。

[0003] 在轮式机器人领域，如何解决三维激光雷达和机械臂之间的坐标变换问题显得尤为重要。同时，由于线束较少的三维激光雷达所扫出来的点云较为稀疏，有时候会出现点云
空洞等现象，如果直接利用原始点云信息进行标定会带来较大的误差。现有的标定方法中，
标定精度受标定对象、标定距离的限制，所得到的标定误差较大，无法满足轮式机器人对三
维环境感知的需求。

发明内容

[0004] 本发明的目的是为了解决三维激光雷达和机械臂之间的坐标变换问题而提供的一种三维激光雷达与机械臂间的自动联合标定方法。

[0005] 本发明提供的三维激光雷达与机械臂间的自动联合标定方法，其方法如下所述：

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

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

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

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

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

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

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

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

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

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

[0016] 本发明的有益效果：

[0017] 本发明提供的三维激光雷达与机械臂间的自动联合标定方法实现了轮式机器人的三维激光雷达与机械臂之间的自动联合标定，使用标准立方体作为标定参照物，降低了
对标定参照物的依赖，标定过程简单、技术难度小、易于广泛推广与实现。同时，对于三维激
光雷达的原始点云数据通过剔除孤立噪声点、修补点云数据空洞、简化点云数据等进行预
处理和优化，大大降低了点云误差，提高而标定精度，有助于轮式机器人对目标的识别、定
位和抓取等。本方法既可用于三维激光雷达与机械臂的初次标定也可用于标定后在工作场
景中的标定修正过程，可以一边抓取物体一边自动修正之前的标定结果，具有自动化、智能
化等优点，具有广泛的应用前景。本发明可以应用在轮式机器人的复杂场景理解、大范围目
标物体识别、定位、抓取及人机交互等人工智能了领域，对智能汽车领域的传感器自动联合
标定具有重要意义。

附图说明

[0018] 图1为本发明所述K‑邻域示意图。

[0019] 图2为本发明所述剔除孤立噪声点流程图。

[0020] 图3为本发明所述修补点云空洞流程图。

[0021] 图4为本发明所述曲率采样法流程图。

[0022] 图5为本发明所述三维激光雷达坐标系、机械臂坐标系和各关节坐标系转换示意图。

[0023] 图6为本发明所述点云中的顶点个数为7个及提取上平面的示意图。

[0024] 图7为本发明所述点云中的顶点个数为6个及提取上平面的示意图。

[0025] 图8为本发明所述点云中的顶点个数为4个及提取上平面的示意图。

具体实施方式

[0026] 请参阅图1至图8所示：

[0027] 本发明提供的三维激光雷达与机械臂间的自动联合标定方法，其方法如下所述：

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

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

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

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

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

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

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

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

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

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