基于线结构光扫描的长圆柱形物体三维识别与定位方法与流程

技术特征：

1.一种基于线结构光扫描的长圆柱形物体三维识别与定位方法，用于实现料箱中长圆柱形物体位置的测量，其特征在于，包括以下步骤：

利用线结构光测量传感器对料箱中的长圆柱形物体以固定步长分m次进行轴向扫描，得到m个剖面的结构光测量数据；

对每个剖面的结构光测量数据分别进行数据分割，使得属于同一物体的数据分割在一段圆弧中；

对每段分割数据进行圆弧拟合，得到圆心坐标；

根据同一物体上的圆弧应满足的约束条件，匹配m个剖面的圆弧圆心；

利用线性插值算法计算长圆柱形物体的三维坐标；

确定抓取的物体标号。

2.根据权利要求1所述的基于线结构光扫描的长圆柱形物体三维识别与定位方法，其特征在于，所述m的设定个剖面基本能覆盖整个长圆柱形物体的轴向，需要根据实际情况自行确定。

3.根据权利要求1所述的基于线结构光扫描的长圆柱形物体三维识别与定位方法，其特征在于，所述线结构光测量传感器所投射的射线是1个。

4.根据权利要求1所述的基于线结构光扫描的长圆柱形物体三维识别与定位方法，其特征在于，所述数据分割过程中，同一段圆弧的数据点应该同时满足以下条件：

|x_i-x_i-1|+|z_i-z_i-1|＜k₁ (1)

|z_i-z_i-5|＜k₂ (2)

(z_i-10-z_i≤k₃)||(z_i+10-z_i≤k₃) (3)

其中，(x_i,z_i)为结构光测量数据在第i点处x、z方向上的坐标值，(x_i-1,z_i-1)为结构光测量数据在第i-1点处x、z方向上的坐标值，z_i-5、z_i-10、z_i+10分为结构光测量数据在第i-5、i-10、i+10点处z方向上的坐标值，k₁、k₂、k₃是预设常数。

5.根据权利要求4所述的基于线结构光扫描的长圆柱形物体三维识别与定位方 法，其特征在于，所述数据分割后排除满足以下条件的结构光测量数据的干扰：

$|z_i-z_m^{*}|\>k_4---\left(4\right)$

$|z_i-z_n^{*}|\<k_5---\left(5\right)$

其中，(x_i,z_i)为结构光测量数据在第i点处x、z方向上的坐标值，为圆弧上最后一点的结构光测量数据，k₄、k₅是预设常数，且存在k₄＜k₅。

6.根据权利要求1所述的基于线结构光扫描的长圆柱形物体三维识别与定位方法，其特征在于，所述进行圆弧拟合要求同一段圆弧上数据的点数同时满足以下条件：

n＞r/T_sample (6)

n＜2*r/T_sample (7)其中，n为同一段圆弧上数据的点数，r为圆弧半径，T_sample为结构光分辨率。

7.根据权利要求1所述的基于线结构光扫描的长圆柱形物体三维识别与定位方法，其特征在于，所述圆弧拟合采用高斯-牛顿迭代法进行，目标函数为：

f(x₀,z₀)＝(x-x₀)²+(z-z₀)²-r² (8)

其中，x、z为圆弧上的数据点坐标，x₀、z₀为圆弧圆心坐标，即为待求参数，其求解过程如下：

第一步：设置x₀、z₀的初始值

$x_0^{\left(0\right)}=\frac{1}{n}\underset{k=1}{\overset{n}{\Σ}}{x}_k---\left(9\right)$

$z_0^{\left(0\right)}=\underset{max}{\Σ}{z}_k-r---\left(10\right)$

其中，x_k为圆弧上x方向上的第k个数据，n为圆弧上的数据量，z_k为圆弧上数据z方向上的最大值，r为圆弧半径；

第二步：对函数f(x₀,z₀)求二阶偏导数，即

$b_{11}=\underset{k=1}{\overset{n}{\Σ}}\frac{\∂f_k}{\∂x_0}\·\frac{\∂f_k}{\∂x_0}---\left(11\right)$

$b_{21}=b_{12}=\underset{k=1}{\overset{n}{\Σ}}\frac{\∂f_k}{\∂x_0}\·\frac{\∂f_k}{\∂z_0}---\left(12\right)$

$b_{21}=\underset{k=1}{\overset{n}{\Σ}}\frac{\∂f_k}{\∂z_0}\·\frac{\∂f_k}{\∂z_0}---\left(13\right)$

同时令

$B_1=\underset{k=1}{\overset{n}{\Σ}}\frac{\∂f_k}{\∂x_0}(r^2-f_k)---\left(14\right)$

$B_2=\underset{k=1}{\overset{n}{\Σ}}\frac{\∂f_k}{\∂z_0}(r^2-f_k)---\left(15\right)$

b₁₁Δ₁+b₁₂Δ₂＝B₁ (16)

b₂₁Δ₁+b₂₂Δ₂＝B₂ (17)

其中，f_k为圆弧上第k个数据的目标函数值，Δ₁、Δ₂分别为圆心坐标的增量，根据式(16)、(17)，可得：

${\mathrm{\Δ}}_1=\frac{B_1\·b_{22}-B_2\·b_{12}}{b_{11}\·b_{22}-b_{21}\·b_{12}}---\left(18\right)$

${\mathrm{\Δ}}_2=\frac{B_1\·b_{21}-B_2\·b_{11}}{b_{12}\·b_{21}-b_{22}\·b_{11}}---\left(19\right)$

第三步：更新x₀、z₀，即：

$x_0^i=x_0^{i-1}+{\mathrm{\Δ}}_1---\left(20\right)$

$z_0^i=z_0^{i-1}+{\mathrm{\Δ}}_2---\left(21\right)$

其中，分别为第i-1次迭代时的圆弧圆心坐标，分别为第i次迭代时的圆弧圆心坐标；

第四步：计算均方误差：

$MS=\frac{1}{n}\underset{k=1}{\overset{n}{\Σ}}{\left(f_k\right(x_0,z_0)-r^2)}^2---\left(22\right)$

如果MS＜T，T为最大均方误差值，停止迭代，得到圆弧圆心坐标x₀、z₀；否则转到第二步。

8.根据权利要求1所述的基于线结构光扫描的长圆柱形物体三维识别与定位方 法，其特征在于，所述同一物体上的圆弧应满足的约束条件为：

$|x_q-x_p^0|\<k_6---\left(23\right)$

$|z_q-z_p^0|\<k_7---\left(24\right)$

其中，(x_q,z_q)为最后匹配的圆弧圆心坐标，为未匹配剖面的圆弧圆心坐标，k₆、k₇是预设常数。

9.根据权利要求1所述的基于线结构光扫描的长圆柱形物体三维识别与定位方法，其特征在于，所述匹配m个剖面的圆弧圆心后，该段圆弧对应的物体即为候选抓取物体。

10.根据权利要求1所述的基于线结构光扫描的长圆柱形物体三维识别与定位方法，其特征在于，所述利用线性插值算法计算长圆柱形物体的三维坐标，具体为：

已知物体抓取位置处的y值，计算离该值最近的两个剖面，得到相应剖面物体对应圆弧的圆心坐标(x₁,z₁)、(x₂,z₂)，利用线性插值方法计算得到y值对应的x、z值分别为：

$x=\frac{y-y_1}{y_2-y_1}\·(x_2-x_1)+x_1---\left(27\right)$

$z=\frac{y-y_1}{y_2-y_1}\·(z_2-z_1)+z_1---\left(28\right)$

其中，y₁、y₂分别为两个剖面对应的y值。

11.根据权利要求1所述的基于线结构光扫描的长圆柱形物体三维识别与定位方法，其特征在于，所述确定抓取的物体标号，具体为：

随机选择最上层可以抓取的物体，如果以下不等式成立：

那么标号为s的物体即为待抓取的物体；其中，rand()为随机数，分别为标号为s的物体两个抓取位置处对应的z值，分别为标号为k的物体两个抓取位置处对应的z值，m为候选抓取物体数目。