CN111428334A - 激光雷达测量中机器人站位规划方法 - Google Patents

Abstract

本发明公开了一种激光雷达测量中机器人站位规划方法，用于解决现有激光雷达测量中测量视点规划方法测量站位点多的技术问题。技术方案是首先构建CAD仿真模型并建立坐标系，再构建视点可达圆锥模型，根据测量精度要求对视点可达圆锥模型进行离散处理，利用激光雷达测量约束和工业机器人手臂可达空间范围约束对小球集合进行筛选，将满足约束条件的小球保留，取包含小球种类最多的相交区域，以相交区域的型心作为激光雷达测量站位点。本发明使用离散小球进行测量可达域计算，根据测量精度确定离小球半径，使测量速率与测量精度相适应。针对不同的测量精度，算法都保持较高的计算效率，激光雷达测量站位点总数减少20～30％。

Description

激光雷达测量中机器人站位规划方法

技术领域

本发明涉及一种激光雷达测量中测量视点规划方法，特别涉及一种激光雷达测量中机器人站位规划方法。

背景技术

激光雷达自动化三维测量是由多自由度机器人调整激光雷达位姿，从多个测量视点对零件进行测量，从而获得准确的测量数据。测量视点的生成过程影响检测的整体 效率和准确率。

文献“CN109163674A一种面结构光自动化三维测量中传感器测量视点规划方法”提出了一种基于面结构光测量的测量视点规划方法。通过对复杂零件分块处理，将数 量众多的被检测点分配到单个体积块中。使用一个测量视点检测多个类似的被检测点， 从而提高检测效率。然而对于复杂装配体而言，装配体的空间结构更加复杂，需要的 测量视点更多。检测任务也是复杂零件的数倍。该方法无法满足复杂的装配体的检测 需求。

国内外已存在关于测量视点自动规划技术的多项研究。其中较为先进的方法是分析检测任务中的各种约束之间的相互关系，基于这些关系生成一个符合限制条件的测 量视点。对于复杂装配体的检测而言，这种方法的计算量大、耗时高。缺乏对数量众 多的测量视点聚类优化的过程。

综上所述，目前复杂装配体测量过程中，存在算法效率低，测量点冗余等问题。

发明内容

为了克服现有激光雷达测量中测量视点规划方法测量站位点多的不足，本发明提供一种激光雷达测量中机器人站位规划方法。该方法首先构建CAD仿真模型并建立 坐标系，再构建视点可达圆锥模型，根据测量精度要求对视点可达圆锥模型进行离散 处理，利用激光雷达测量约束和工业机器人手臂可达空间范围约束对小球集合进行筛 选，将满足约束条件的小球保留，取包含小球种类最多的相交区域，以相交区域的型 心作为激光雷达测量站位点。将从测量站位点能测量的测量点所对应的离散小球，从 所有测量点所对应的离散小球集合中移除。对其余测量点继续上述过程，直至生成与 所有测量点相对应的激光雷达测量站位点。本发明使用离散小球进行测量可达域计算， 根据测量精度确定离小球半径，使测量速率与测量精度相适应。针对不同的测量精度， 算法都可以保持较高的计算效率。本发明方法的柔性高于背景技术方法。对测量可达 域进行几何求交运算，实现用数量最少激光雷达测量站位点完成检测任务，将激光雷 达测量站位点总数减少20～30％。

本发明解决其技术问题所采用的技术方案：一种激光雷达测量中机器人站位规划方法，其特点是包括以下步骤：

(a)构建CAD仿真模型并建立坐标系。采用三维造型软件，将已知的激光雷达、 机器人手臂模型和零件模型装配在检测平台上。建立检测世界坐标系S_w，任选检测平 台上一点作为S_w的原点，用三维移动平台的三个正交运动方向分别作为S_w的X,Y,Z轴 的方向。以机器人手臂基座中心点O_b为基点建立运动坐标系S_b，三个坐标轴的方向与 S_w的三个坐标轴的方向相同。标注出所有测量点的坐标和表面法式。

(b)构建视点可达圆锥模型。从被测模型中提取出每个测量点的坐标和表面单位法矢。根据每个测量点P_w,i的坐标(x_w,i,y_w,i,z_w,i)和表面单位法矢

i＝1,2,...,m，m是测量点总个数。以P_w,i(x_w,i,y_w,i,z_w,i)为视点可达圆锥顶点，

为轴线作一个顶角为θ_k的视点可达圆锥。取视点可达圆锥上一条母 线ζ_i,0，ζ_i,0的方向矢量为

将ζ_i,0绕轴线旋转一个角度

后得到站位可达圆锥上的另一条母线ζ_i,l，

ζ_i,l的方向矢量表示为:

其中I为3×3单位矩阵，

用多条离散的母线表示视点可达圆锥模型。

(c)根据测量精度要求对视点可达圆锥模型进行离散处理。小球半径

C 表示测量特征的精度要求。对站位可达圆锥分层离散，每层高度h＝2×r_q，共分为J层，

第j层的圆半径

再将圆离散成圆环，相邻圆环间距d＝2×r_q， 共分为K层，

第j层圆台的第k层圆环记为ring_j,k，圆环半径表示为

计算圆环的周长

用圆环ring_j,k的周长C_c,j,k除以 小球直径d_q＝2·r_q，结果向下取整，得到圆环ring_j,k上离散小球的数量L。离散小球记 作q_j,_k,l。计算得出小球q_j,_k,l的圆心坐标(x_j,_k,l,y_j,_k,l,z_j,_k,l)。

由测量点P_w,i建立的视点可达圆锥用离散小球的集合表示，记为:

S_i＝{q_j,k,l|j∈[1,J],k∈[1,K],l∈[1,L],N⁺} (4)

(d)激光雷达测量约束定义。根据每个测量点P_w,i特征类型及测量精度要求，激 光雷达站位点M_w,i与测量点P_w,i满足距离约束、角度约和干涉约束。

距离约束:激光雷达站位M_w,i与测量点P_w,i之间的距离L_i满足有效范围要求，即L_min＜L_i＜L_max。其中L_min，L_max分别是在满足测量精度的要求下，允许的最小和最大距 离。

角度约束：由测量点P_w,i指向激光雷达站位M_w,i的向量

与测量点P_w,i的法矢

夹角θ_i满足有效范围要求，即θ_min＜θ_i＜θ_max。其中θ_min，θ_max是满足 测量精度要求所允许的最小和最大角度。由检测对象的特征类型决定。

测量约束边界用离散的母线矢量表示，其中圆锥顶角θ_k＝2θ_i。根据距离约束L_min，L_max在母线ζ_i,0上截取线段μ_i,0，线段μ_i,0的端点为

和

P_A,w,iP_B,w,i以表面单位法矢

为轴旋转360°得到激光雷达的测量约束边界。

(e)工业机器人手臂可达空间范围约束定义。采用经典的D-H方法建立连杆坐 标系，通过连杆坐标系R_i相对于连杆坐标系R_i-1的坐标变换矩阵

得到工业机器人前三个关节形成的工作空间的方程

其中

c_θi＝cos(θ_i)，s_θi＝sin(θ_i)，c_αi＝cos(α_i)，

s_αi＝sin(α_i)

机器人手臂工作可达区域由前三个关节的工作区域决定。根据工业机器人的结构参数 关节变量θ_i满足

对关节变量θ₂、θ₃采用极限组合原理，可得到当θ₁＝0时工业机器人腕关节端点

在机器人坐标系中的工作空间边界，再根据工作空间边界 求出工作空间

关键点的z坐标，这些关键点为θ₁＝0时工作空间内外边界z坐标 最大和最小的点以及边界表达式发生变化处点的z坐标，记为Z₁,Z₂......,Z₇。然后求出 腕关节端点

到机器人坐标系z轴的距离D_i，以及在对应

的z坐标处工作空间 W₀(P_w)的内外边界到机器人坐标系z轴的距离

和

如果有

成 立，则说明

在工作空间

内部。

的参数方程分别为：

式中，

c₁＝cos(θ₁),c₂＝cos(θ₂),c₃＝cos(θ₃)；

s₁＝sin(θ₁),s₂＝sin(θ₂),s₃＝sin(θ₃)；

s₂₃＝sin(θ₂+θ₃)；

c₂₃＝cos(θ₂+θ₃)；

d₄是工业机器人连杆4的关节偏置距离；θ₁是工业机器人连杆1的关节转角；θ₂是工业机器人连杆2的关节转角；

θ₃是工业机器人连杆3的关节转角；a₁是工业机器人连杆1的长度；a₂是工业机器人连杆2的长度；

a₃是工业机器人连杆3的长度；机器人手臂运动空间边界用方程

表示。

(f)利用(d)、(e)中的激光雷达测量约束和工业机器人手臂可达空间范围约束对小球集合S_i＝{q_j,k,l|j∈[1,J],k∈[1,K],l∈[1,L],N⁺}进行筛选。将满足约束条件的小球保 留。对S_i＝{q_j,k,l|j∈[1,J],k∈[1,K],l∈[1,L],N⁺}筛选处理得到测量可达域S_i'。

(h)将每个测量点的测量可达域S_i'求交。取包含小球种类最多的相交区域T_i，以相交区域T_i的型心作为激光雷达测量站位点Q_w,i。将从测量站位点Q_w,i能测量的测量点 P_w,_i所对应的离散小球，从所有测量点所对应的离散小球集合中移除。对其余测量点继 续上述过程，直至生成与所有测量点P_w,i相对应的激光雷达测量站位点Q_w,i。

本发明的有益效果是：该方法首先构建CAD仿真模型并建立坐标系，再构建视 点可达圆锥模型，根据测量精度要求对视点可达圆锥模型进行离散处理，利用激光雷 达测量约束和工业机器人手臂可达空间范围约束对小球集合进行筛选，将满足约束条 件的小球保留，取包含小球种类最多的相交区域，以相交区域的型心作为激光雷达测 量站位点。将从测量站位点能测量的测量点所对应的离散小球，从所有测量点所对应 的离散小球集合中移除。对其余测量点继续上述过程，直至生成与所有测量点相对应 的激光雷达测量站位点。本发明使用离散小球进行测量可达域计算，根据测量精度确 定离小球半径，使测量速率与测量精度相适应。针对不同的测量精度，算法都可以保 持较高的计算效率。本发明算法的柔性高于目前所使用的算法。对测量可达域进行几 何求交运算，实现用数量最少激光雷达测量站位点完成检测任务，将激光雷达测量站 位点总数减少20～30％。

下面结合附图和具体实施方式对本发明作详细说明。

附图说明

图1是本发明激光雷达测量中机器人站位规划方法的流程图。

图2是本发明方法中激光雷达测量约束定义。

图3是本发明方法中测量可达圆锥离散化模型。

图4是本发明方法中机器人手臂运动范围约束。

图5是本发明方法中测量可达域求交示意图。

具体实施方式

参照图1-5。本发明激光雷达测量中机器人站位规划方法具体步骤如下：

步骤1、构建CAD仿真模型并建立坐标系。

使用UG软件，将已知的激光雷达、机器人手臂模型和零件模型装配在检测平台上。建立检测世界坐标系S_w，任选检测平台上一点作为S_w的原点，用三维移动平台的 三个正交运动方向分别作为S_w的X,Y,Z轴的方向。以机器人手臂基座中心点O_b为基点 建立运动坐标系S_b，三个坐标轴的方向与S_w的三个坐标轴的方向相同。标注出所有测 量点的坐标和表面法式。

步骤2、构建视点可达圆锥模型。

根据每个测量点P_w,i的坐标(x_w,i,y_w,i,z_wi,)和表面单位法矢

m是测量点总个数。以P_w,i(x_w,i,y_w,i,z_w,i)为视点可达圆锥顶点，

为轴线作一个顶角为θ_k的视点可达圆锥。取视点可达圆锥上一条母 线ζ_i,0，ζ_i,0的方向矢量为

其中

将ζ_i,0绕轴线旋转一个角度

后得到站位可达圆锥上的另一条母线ζ_i,l，

ζ_i,l的方向矢量可表示为

其中I为3×3单位矩阵，

用多条离散的母线表示视点可达圆锥模型。

步骤3、视点可达圆锥模型离散处理。

根据测量精度要求对视点可达圆锥模型进行离散处理。用离散小球表示视点可达圆锥。小球半径

C表示测量特征的精度要求。对站位可达圆锥分层离散，每 层高度h＝2×r_q，共分为J层，

第j层的圆半径

再将圆离散 成圆环，相邻圆环间距d＝2×r_q，共分为K层，

第j层圆台的第k层圆环记 为ring_j,k，圆环半径表示为

计算圆环的周长

用圆环ring_j,k的周长C_c,j,k除以小球直径d_q＝2·r_q，结果向下取 整，得到圆环ring_j,k上离散小球的数量L。离散小球记作q_j,k,l。计算得出小球q_j,k,l的圆 心坐标(x_j,_k,l,y_j,_k,l,z_j,_k,l)。

由测量点P_w,i建立的视点可达圆锥用离散小球的集合表示，记为 S_i＝{q_j,k,l|j∈[1,J],k∈[1,K],l∈[1,L],N+}。

步骤4、激光雷达测量约束定义。

根据每个测量点P_w,i特征类型及测量精度要求，激光雷达站位点M_w,i与测量点P_w,i满足距离约束、角度约束，干涉约束。

距离约束:激光雷达站位M_w,i与测量点P_w,i之间的距离L_i满足有效范围要求，即L_min＜L_i＜L_max。其中L_min，L_max分别是在满足测量精度的要求下，允许的最小和最大距 离。

角度约束：由测量点P_w,i指向激光雷达站位M_w,i的向量

与测量点P_w,i的法矢

夹角θ_i满足有效范围要求，即θ_min＜θ_i＜θ_max。其中θ_min，θ_max是满足 测量精度要求所允许的最小和最大角度。由检测对象的特征类型决定。

将ζ_i,0绕轴线旋转一个角度

后得到站位可达圆锥上的另一条母线ζ_i,l，

l＝0,1,...L-1，ζ_i,l的方向矢量可表示为

其中I为3×3单位矩阵，

步骤5、工业机器人手臂可达空间范围约束定义。

采用经典的D-H方法建立连杆坐标系，通过连杆坐标系R_i相对于连杆坐标系R_i-1的坐标变换矩阵

得到工业机器人前三个关节形成的工作空间的方程

其中

c_θi＝cos(θ_i)，s_θi＝sin(θ_i)，c_αi＝cos(α_i)，

s_αi＝sin(α_i)

机器人手臂工作可达区域由前三个关节的工作区域决定。根据工业机器人的结构参数 关节变量θ_i满足

对关节变量θ₂、θ₃采用极限组合原理，可得到当θ₁＝0时工业机器人腕关节端点

在机器人坐标系中的工作空间边界，再根据工作空间边界 求出工作空间

关键点的z坐标，这些关键点为θ₁＝0时工作空间内外边界z坐标 最大和最小的点以及边界表达式发生变化处点的z坐标，记为Z₁,Z₂......,Z₇。然后求出 腕关节端点

到机器人坐标系z轴的距离D_i，以及在对应

的z坐标处工作空间 W₀(P_w)的内外边界到机器人坐标系z轴的距离

和

如果有

成 立，则说明

在工作空间

内部。

的参数方程分别为：

式中：

c₁＝cos(θ₁),c₂＝cos(θ₂),c₃＝cos(θ₃)；

s₁＝sin(θ₁),s₂＝sin(θ₂),s₃＝sin(θ₃)；

s₂₃＝sin(θ₂+θ₃)；

c₂₃＝cos(θ₂+θ₃)；

d₄是工业机器人连杆4的关节偏置距离；θ₁是工业机器人连杆1的关节转角；θ₂是工业机器人连杆2的关节转角；θ₃是工业机器人连杆3的关节转角；a₁是工业机器人连 杆1的长度；a₂是工业机器人连杆2的长度；a₃是工业机器人连杆3的长度；

机器人手臂运动空间边界用方程

表示。

步骤6、根据约束筛选满足条件的小球生成测量可达域。

利用步骤4、5中的激光雷达测量约束和工业机器人手臂可达空间范围约束对小球集合S_i＝{q_j,k,l|j∈[1,J],k∈[1,K],l∈[1,L],N+}进行筛选。将满足约束条件的小球保留。 对S_i＝{q_j,k,l|j∈[1,J],k∈[1,K],l∈[1,L],N+}筛选处理得到测量可达域S_i'。

步骤7、测量可达域求交并计算得出测量站位点。

将每个测量点的测量可达域S_i'求交。取包含小球种类最多的相交区域T_i，以相交区域T_i的型心作为激光雷达测量站位点Q_w,i。将从测量站位点Q_w,i能测量的测量点P_w,i所 对应的离散小球，从所有测量点所对应的离散小球集合中移除。对其余测量点继续上 述过程，直至生成与所有测量点P_w,i相对应的激光雷达测量站位点Q_w,i。

Claims (1)

1.一种激光雷达测量中机器人站位规划方法，其特征在于包括以下步骤：

(a)构建CAD仿真模型并建立坐标系；采用三维造型软件，将已知的激光雷达、机器人手臂模型和零件模型装配在检测平台上；建立检测世界坐标系S_w，任选检测平台上一点作为S_w的原点，用三维移动平台的三个正交运动方向分别作为S_w的X,Y,Z轴的方向；以机器人手臂基座中心点O_b为基点建立运动坐标系S_b，三个坐标轴的方向与S_w的三个坐标轴的方向相同；标注出所有测量点的坐标和表面法式；

(b)构建视点可达圆锥模型；从被测模型中提取出每个测量点的坐标和表面单位法矢；根据每个测量点P_w,i的坐标(x_w,i,y_w,i,z_w,i)和表面单位法矢

m是测量点总个数；以P_w,i(x_w,i,y_w,i,z_w,i)为视点可达圆锥顶点，

为轴线作一个顶角为θ_k的视点可达圆锥；取视点可达圆锥上一条母线ζ_i,0，ζ_i,0的方向矢量为

其中

将ζ_i,0绕轴线旋转一个角度

后得到站位可达圆锥上的另一条母线ζ_i,l，

l＝0,1,...L-1，ζ_i,l的方向矢量表示为:

其中I为3×3单位矩阵，

用多条离散的母线表示视点可达圆锥模型；

(c)根据测量精度要求对视点可达圆锥模型进行离散处理；小球半径

C表示测量特征的精度要求；对站位可达圆锥分层离散，每层高度h＝2×r_q，共分为J层，

第j层的圆半径

再将圆离散成圆环，相邻圆环间距d＝2×r_q，共分为K层，

第j层圆台的第k层圆环记为ring_j,k，圆环半径表示为

计算圆环的周长

用圆环ring_j,k的周长C_c,j,k除以小球直径d_q＝2·r_q，结果向下取整，得到圆环ring_j,k上离散小球的数量L；离散小球记作q_j,k,l；计算得出小球q_j,k,l的圆心坐标(x_j,k,l,y_j,k,l,z_j,k,l)；

由测量点P_w,i建立的视点可达圆锥用离散小球的集合表示，记为:

S_i＝{q_j,k,l|j∈[1,J],k∈[1,K],l∈[1,L],N⁺} (4)

(d)激光雷达测量约束定义；根据每个测量点P_w,i特征类型及测量精度要求，激光雷达站位点M_w,i与测量点P_w,i满足距离约束、角度约和干涉约束；

距离约束:激光雷达站位M_w,i与测量点P_w,i之间的距离L_i满足有效范围要求，即L_min＜L_i＜L_max；其中L_min，L_max分别是在满足测量精度的要求下，允许的最小和最大距离；

角度约束：由测量点P_w,i指向激光雷达站位M_w,i的向量

与测量点P_w,i的法矢

夹角θ_i满足有效范围要求，即θ_min＜θ_i＜θ_max；其中θ_min，θ_max是满足测量精度要求所允许的最小和最大角度；由检测对象的特征类型决定；

测量约束边界用离散的母线矢量表示，其中圆锥顶角θ_k＝2θ_i；根据距离约束L_min，L_max在母线ζ_i,0上截取线段μ_i,0，线段μ_i,0的端点为

和

P_A,w,i P_B,w,i以表面单位法矢

为轴旋转360°得到激光雷达的测量约束边界；

(e)工业机器人手臂可达空间范围约束定义；采用经典的D-H方法建立连杆坐标系，通过连杆坐标系R_i相对于连杆坐标系R_i-1的坐标变换矩阵

得到工业机器人前三个关节形成的工作空间的方程W_i(P_i ^b){W₀(P_i ^b)、W₁(P_i ^b)、W₂(P_i ^b)}，其中

c_θi＝cos(θ_i)，s_θi＝sin(θ_i)，c_αi＝cos(α_i)，s_αi＝sin(α_i)

机器人手臂工作可达区域由前三个关节的工作区域决定；根据工业机器人的结构参数关节变量θ_i满足θ_i ^min＜θ_i＜θ_i ^max，对关节变量θ₂、θ₃采用极限组合原理，可得到当θ₁＝0时工业机器人腕关节端点P_i ^b在机器人坐标系中的工作空间边界，再根据工作空间边界求出工作空间W₀(P_i ^b)关键点的z坐标，这些关键点为θ₁＝0时工作空间内外边界z坐标最大和最小的点以及边界表达式发生变化处点的z坐标，记为Z₁,Z₂......,Z₇；然后求出腕关节端点P_i ^b到机器人坐标系z轴的距离D_i，以及在对应P_i ^w的z坐标处工作空间W₀(P_w)的内外边界到机器人坐标系z轴的距离

和

如果有

成立，则说明P_i ^b在工作空间W₀(P_i ^b)内部；W₀(P_i ^b)，W₁(P_i ^b)W₂(P_i ^b)的参数方程分别为：

式中，

c₁＝cos(θ₁),c₂＝cos(θ₂),c₃＝cos(θ₃)；

s₁＝sin(θ₁),s₂＝sin(θ₂),s₃＝sin(θ₃)；

s₂₃＝sin(θ₂+θ₃)；

c₂₃＝cos(θ₂+θ₃)；

d₄是工业机器人连杆4的关节偏置距离；θ₁是工业机器人连杆1的关节转角；θ₂是工业机器人连杆2的关节转角；

θ₃是工业机器人连杆3的关节转角；a₁是工业机器人连杆1的长度；a₂是工业机器人连杆2的长度；

a₃是工业机器人连杆3的长度；机器人手臂运动空间边界用方程W_i(P_i ^b){W₀(P_i ^b)、W₁(P_i ^b)、W₂(P_i ^b)}表示；

(f)利用(d)、(e)中的激光雷达测量约束和工业机器人手臂可达空间范围约束对小球集合S_i＝{q_j,k,l|j∈[1,J],k∈[1,K],l∈[1,L],N⁺}进行筛选；将满足约束条件的小球保留；对S_i＝{q_j,k,l|j∈[1,J],k∈[1,K],l∈[1,L],N⁺}筛选处理得到测量可达域S′_i；

(h)将每个测量点的测量可达域S′_i求交；取包含小球种类最多的相交区域T_i，以相交区域T_i的型心作为激光雷达测量站位点Q_w,i；将从测量站位点Q_w,i能测量的测量点P_w,_i所对应的离散小球，从所有测量点所对应的离散小球集合中移除；对其余测量点继续上述过程，直至生成与所有测量点P_w,i相对应的激光雷达测量站位点Q_w,i。

Images