[发明专利]激光雷达测量中机器人站位规划方法

权利要求书

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，ζ_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_minL_iL_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的坐标变换矩阵得到工业机器人前三个关节形成的工作空间的方程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满足对关节变量θ₂、θ₃采用极限组合原理，可得到当θ₁＝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。