CN111311680B - 一种三维激光雷达与机械臂间的自动联合标定方法 - Google Patents
一种三维激光雷达与机械臂间的自动联合标定方法 Download PDFInfo
- Publication number
- CN111311680B CN111311680B CN202010090092.2A CN202010090092A CN111311680B CN 111311680 B CN111311680 B CN 111311680B CN 202010090092 A CN202010090092 A CN 202010090092A CN 111311680 B CN111311680 B CN 111311680B
- Authority
- CN
- China
- Prior art keywords
- joint
- mechanical arm
- coordinate system
- laser radar
- dimensional laser
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10032—Satellite or aerial image; Remote sensing
- G06T2207/10044—Radar image
Landscapes
- Engineering & Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明公开了一种三维激光雷达与机械臂间的自动联合标定方法,其方法为:第一步、随机生成目标位姿;第二步、三维激光雷达原始点云预处理及优化:第三步、机械臂末端抓取位置及各关节位姿计算;第四步、利用最小二乘法求解最优旋转矩阵R和平移矩阵T。有益效果:降低了对标定参照物的依赖,标定过程简单、技术难度小、易于广泛推广与实现。简化点云数据等进行预处理和优化,大大降低了点云误差,提高而标定精度,有助于轮式机器人对目标的识别、定位和抓取等。能够应用在轮式机器人的复杂场景理解、大范围目标物体识别、定位、抓取及人机交互等人工智能了领域,对智能汽车领域的传感器自动联合标定具有重要意义。
Description
技术领域
本发明涉及一种自动联合标定方法,特别涉及一种三维激光雷达与机械臂间的自动联合标定方法。
背景技术
目前,随着人工智能的快速发展,自动驾驶汽车等轮式机器人技术逐渐火热起来。通过传感器去感知周围环境,并对要抓取的物体进行识别、定位、抓取等,这其中离不开感知传感器与机械臂之间的坐标***一,这就要求需要对多传感器进行联合标定,使各个传感器的信息数据融合在一起,从而实现轮式机器人通过三维激光雷达去感知周围环境,并对要抓取的物体进行识别、定位,并将物体在三维激光雷达坐标系中的位置信息转换成机械臂所在坐标系的位置信息,从而实现机械臂对物体的抓取过程。同时,根据机械臂末端的坐标系,可以通过基于ROS的MoveIt!实现对机械臂各关节的反向运动学求解得到各关节的坐标位置。通过上述标定可以将激光雷达和机械臂联合起来使用,可以根据周围环境信息自主做出分析、决策等,对轮式机器人的技术发展等具有重要的意义。
在轮式机器人领域,如何解决三维激光雷达和机械臂之间的坐标变换问题显得尤为重要。同时,由于线束较少的三维激光雷达所扫出来的点云较为稀疏,有时候会出现点云空洞等现象,如果直接利用原始点云信息进行标定会带来较大的误差。现有的标定方法中,标定精度受标定对象、标定距离的限制,所得到的标定误差较大,无法满足轮式机器人对三维环境感知的需求。
发明内容
本发明的目的是为了解决三维激光雷达和机械臂之间的坐标变换问题而提供的一种三维激光雷达与机械臂间的自动联合标定方法。
本发明提供的三维激光雷达与机械臂间的自动联合标定方法,其方法如下所述:
第一步、随机生成目标位姿:通过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,同理,也可根据上述最小二乘法来求出各关节坐标系转换到三维激光雷达坐标系下的最优旋转矩阵和平移矩阵。
本发明的有益效果:
本发明提供的三维激光雷达与机械臂间的自动联合标定方法实现了轮式机器人的三维激光雷达与机械臂之间的自动联合标定,使用标准立方体作为标定参照物,降低了对标定参照物的依赖,标定过程简单、技术难度小、易于广泛推广与实现。同时,对于三维激光雷达的原始点云数据通过剔除孤立噪声点、修补点云数据空洞、简化点云数据等进行预处理和优化,大大降低了点云误差,提高而标定精度,有助于轮式机器人对目标的识别、定位和抓取等。本方法既可用于三维激光雷达与机械臂的初次标定也可用于标定后在工作场景中的标定修正过程,可以一边抓取物体一边自动修正之前的标定结果,具有自动化、智能化等优点,具有广泛的应用前景。本发明可以应用在轮式机器人的复杂场景理解、大范围目标物体识别、定位、抓取及人机交互等人工智能了领域,对智能汽车领域的传感器自动联合标定具有重要意义。
附图说明
图1为本发明所述K-邻域示意图。
图2为本发明所述剔除孤立噪声点流程图。
图3为本发明所述修补点云空洞流程图。
图4为本发明所述曲率采样法流程图。
图5为本发明所述三维激光雷达坐标系、机械臂坐标系和各关节坐标系转换示意图。
图6为本发明所述点云中的顶点个数为7个及提取上平面的示意图。
图7为本发明所述点云中的顶点个数为6个及提取上平面的示意图。
图8为本发明所述点云中的顶点个数为4个及提取上平面的示意图。
具体实施方式
请参阅图1至图8所示:
本发明提供的三维激光雷达与机械臂间的自动联合标定方法,其方法如下所述:
第一步、随机生成目标位姿:通过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,同理,也可根据上述最小二乘法来求出各关节坐标系转换到三维激光雷达坐标系下的最优旋转矩阵和平移矩阵。
Claims (1)
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,同理,也可根据上述最小二乘法来求出各关节坐标系转换到三维激光雷达坐标系下的最优旋转矩阵和平移矩阵。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010090092.2A CN111311680B (zh) | 2020-02-13 | 2020-02-13 | 一种三维激光雷达与机械臂间的自动联合标定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010090092.2A CN111311680B (zh) | 2020-02-13 | 2020-02-13 | 一种三维激光雷达与机械臂间的自动联合标定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111311680A CN111311680A (zh) | 2020-06-19 |
CN111311680B true CN111311680B (zh) | 2021-03-30 |
Family
ID=71154509
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010090092.2A Active CN111311680B (zh) | 2020-02-13 | 2020-02-13 | 一种三维激光雷达与机械臂间的自动联合标定方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111311680B (zh) |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112082483B (zh) * | 2020-09-09 | 2021-12-03 | 易思维(杭州)科技有限公司 | 仅有棱边特征工件的定位方法、应用及精度评价方法 |
CN112361982B (zh) * | 2020-10-29 | 2022-02-01 | 山东省科学院激光研究所 | 一种大幅面工件三维数据提取方法及*** |
CN112484725B (zh) * | 2020-11-23 | 2023-03-21 | 吉林大学 | 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法 |
CN116416319B (zh) * | 2022-11-17 | 2023-11-24 | 南京理工大学 | 面向智能驾驶多类型传感器标定的一次性联合标定方法 |
CN117475002A (zh) * | 2023-12-27 | 2024-01-30 | 青岛亿联建设集团股份有限公司 | 基于激光扫描技术的建筑倾斜度测量方法 |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110497373A (zh) * | 2019-08-07 | 2019-11-26 | 大连理工大学 | 一种移动作业机器人的三维激光雷达与机械臂间的联合标定方法 |
-
2020
- 2020-02-13 CN CN202010090092.2A patent/CN111311680B/zh active Active
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110497373A (zh) * | 2019-08-07 | 2019-11-26 | 大连理工大学 | 一种移动作业机器人的三维激光雷达与机械臂间的联合标定方法 |
Non-Patent Citations (1)
Title |
---|
装车机器人激光雷达测量***及其标定方法;王春梅 等;《光电工程》;20190715;第46卷(第7期);第190002-1到190002-8页 * |
Also Published As
Publication number | Publication date |
---|---|
CN111311680A (zh) | 2020-06-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111311680B (zh) | 一种三维激光雷达与机械臂间的自动联合标定方法 | |
CN110340891B (zh) | 基于点云模板匹配技术的机械臂定位抓取***及方法 | |
CN109755995B (zh) | 基于ros机器人操作***的机器人自动充电对接方法 | |
CN109794963B (zh) | 一种面向曲面构件的机器人快速定位方法 | |
CN108827155B (zh) | 一种机器人视觉测量***及方法 | |
CN108549084B (zh) | 一种基于稀疏二维激光雷达的目标检测与姿态估计方法 | |
CN108994844B (zh) | 一种打磨操作臂手眼关系的标定方法和装置 | |
CN111267095A (zh) | 一种基于双目视觉的机械臂抓取控制方法 | |
CN104040590A (zh) | 用于估计物体的姿态的方法 | |
CN113111887A (zh) | 一种基于相机和激光雷达信息融合的语义分割方法及*** | |
CN104463108A (zh) | 一种单目实时目标识别及位姿测量方法 | |
CN111243017A (zh) | 基于3d视觉的智能机器人抓取方法 | |
CN112045655B (zh) | 用于大尺度多站点场景的移动机器人位姿测量方法及*** | |
CN112508895B (zh) | 一种基于曲面配准的螺旋桨叶片质量评估方法 | |
CN111522022B (zh) | 基于激光雷达的机器人进行动态目标检测方法 | |
CN113706710A (zh) | 基于fpfh特征差异的虚拟点多源点云融合方法及*** | |
CN111986219A (zh) | 一种三维点云与自由曲面模型的匹配方法 | |
CN115965960A (zh) | 一种基于深度学习与3d点云的焊缝识别方法 | |
CN114299039B (zh) | 一种机器人及其碰撞检测装置和方法 | |
CN116466712A (zh) | 基于cml-avg激光雷达小车的自主巡航方法及*** | |
CN114545400A (zh) | 基于毫米波雷达的水面机器人的全局重定位方法 | |
CN113378647B (zh) | 基于三维点云的实时轨道障碍物检测方法 | |
CN113978297A (zh) | 基于3d视觉与力柔顺控制的电动汽车自动充电*** | |
Xu et al. | A new welding path planning method based on point cloud and deep learning | |
CN114897967B (zh) | 一种面向挖掘装备自主作业的物料形态识别方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |