1 引言（Introduction）

作为一种非接触式的测量技术，线激光轮廓传感器可快速获取被测物体表面的轮廓信息，具有效率高、抗干扰能力强、精度高等特点，目前已广泛地应用在各工程领域中^[1]。但线激光轮廓传感器测量范围小，单次只能测量激光线处的2维信息，依靠固定轨道的自动化设备，只可获取被测物体的局部表面数据。将线激光轮廓传感器搭载在灵活性好、精度高的关节机器人平台上，可以实现对被测物体的整体测量，提升了测量系统的柔性，在3维检测、目标定位、逆向工程等领域具有广阔前景。

为了得到被测物体在机器人基坐标系下的3维数据，需要对机器人末端坐标系与线激光轮廓传感器坐标系之间的关系进行标定，即手眼标定。手眼标定方法一般基于求解$ \mathit{\boldsymbol{AX}}=\mathit{\boldsymbol{XB}} $型齐次变换方程，对于基于摄像机的视觉系统，手眼标定往往采用经典的棋盘格图像标定法^[2-3]。由于线激光轮廓传感器输出的2维点云信息不同于图像信息，无法直接使用传统的手眼标定方法。文[4-5]提出首先对线激光轮廓传感器进行内参标定，得到激光平面与相机坐标系的关系，再利用相机与机器人的手眼标定得到线激光轮廓传感器与机器人的手眼变换矩阵。该方法增大了标定复杂度，对于出厂已进行过内参预标定的线激光轮廓传感器并无必要。文[6-7]利用高精度外部设备对线激光轮廓传感器与机器人进行手眼标定，如三坐标测量仪和激光跟踪仪等，虽然原理简单，但标定设备价格昂贵，存在成本高、操作不便等缺点。针对激光位移传感器与机器人的手眼标定，文[8]提出一种定点变位姿的标定方法（FPDP）。该方法通过改变机器人末端姿态，测出定点在激光位移传感器中的坐标，通过求解$ \mathit{\boldsymbol{AX}}=\mathit{\boldsymbol{XB}} $型齐次变换方程得到手眼变换矩阵。该标定方法原理简单，可实施性强。但标定过程中需要人为判断激光点与定点重合与否，导致标定精度易受人为干扰。文[9]提出一种基于平面模板的标定方法，通过对机器人在不同位姿下采集的激光点数据进行平面约束求解，得到标定参数。但该方法利用罚函数法求解约束优化方程时，对标定参数初始值要求较高，容易陷入局部最优，且标定结果还易受激光点分布、测量距离和测量位姿的影响。对于线激光轮廓传感器与机器人的手眼标定，文[10-11]提出利用标定球对线激光轮廓传感器和机器人进行手眼标定，以标定球的球心作为定点，利用勾股定理求解球心在线激光轮廓传感器中的坐标，再根据FPDP方法求解标定参数。但该方法对线激光与标定球的相交位置有要求，相交位置靠近或远离球心都会影响标定精度，使得标定操作困难。文[12]提出一种完全基于求解线性方程组的标定方法，该方法用线激光轮廓传感器对3个近正交平面进行扫描，将扫描得到的数据利用平面约束求解得到变换矩阵。该方法可以在3个近正交平面上的任意位置进行扫描，方便获取大量可靠的标定数据，但使用3个近正交平面增加了标定的成本和复杂性。在此基础上，文[13]提出基于单平面约束的手眼标定方法，通过对扫描数据的严格约束，仅通过单一平面就可以获得最小二乘法求解条件。但该标定结果易受激光线分布和平面位置的影响，当标定平面与机器人基坐标系平面平行时，标定参数收敛失败。

针对扫描机器人末端搭载线激光轮廓传感器的手眼标定问题，本文提出一种基于圆柱侧面约束的手眼标定方法。首先通过改变扫描机器人末端位姿，获得不同位姿下圆柱侧面扫描数据，然后利用对圆柱侧面扫描数据的约束建立约束优化模型，最后根据PSO算法和广义拉格朗日乘子法的融合算法求解该模型得到标定参数。该方法只需要一块标准的圆柱体，标定过程简单、易操作，标定结果不受标定物位置和标定参数初始值的影响，标定精度高。同时，该标定方法在搭载线激光轮廓传感器的无人机和各类机器人上也具有通用性。

2 方案设计（Schematic design）

对于末端搭载线激光轮廓传感器的关节机器人手眼标定问题，本文提出一种基于标准圆柱的手眼标定方案。

首先将标准圆柱置于关节机器人工作空间任意位置。调整机器人末端位姿，使线激光平面与标准圆柱的侧面相交，相交形状为椭圆。图 1为其标定模型，机器人末端坐标系$ \varSigma_{\rm E} $相对于基坐标系$ \varSigma_{\rm B} $的变换矩阵为$ \mathit{\boldsymbol{T}}_{\text{BE}} $，线激光轮廓传感器坐标系$ \varSigma_{\rm L} $相对于机器人末端坐标系$ \varSigma_{\rm E} $的变换矩阵为$ \mathit{\boldsymbol{T}}_{\text{EL}} $，即手眼标定矩阵。在机器人末端的不同位姿下，线激光平面与标准圆柱侧面相交得到的椭圆中心为$ O_j $与$ O_{j+1} $。由于线激光轮廓传感器的输出是线激光坐标系下的2维轮廓数据，其$ Y $轴垂直于激光平面，故该轮廓点云数据的$ y $值恒为0。第$ j $条轮廓数据为

$ \begin{align} {{\mathit{\boldsymbol{L}}}}_{\rm L}^{j}=& \big[\, {{\mathit{\boldsymbol{p}}}}_1, \; {{\mathit{\boldsymbol{p}}}}_2, \; {{\mathit{\boldsymbol{p}}}}_3, \cdots, \; {{\mathit{\boldsymbol{p}}}}_k\big] \\ =& \begin{bmatrix} x_1 & x_2 & x_3 & \cdots & x_k \\ 0 & 0 & 0 & \cdots & 0 \\ z_1 & z_2 & z_3 & \cdots & z_k \\ \end{bmatrix} \end{align} $

(1)

式中，$ {{\mathit{\boldsymbol{p}}}}_i =[x_i, 0, z_i]^{\rm T} $，$ i=1, 2, \cdots , k $，为2维轮廓点云中的点的坐标。

通过齐次变换将$ {{\mathit{\boldsymbol{L}}}}_{\rm L}^{j} $转换到机器人基坐标系下，得到椭圆轮廓在机器人基坐标系下的3维点云数据$ {{\mathit{\boldsymbol{L}}}}_{\rm B}^{j} $为

$ \begin{align} \begin{bmatrix} {{\mathit{\boldsymbol{L}}}}_{\rm B}^{j} \\ 1 \\ \end{bmatrix} &={{\mathit{\boldsymbol{T}}}}_{\text{BE}} {{\mathit{\boldsymbol{T}}}}_{\text{EL}} \begin{bmatrix} {{\mathit{\boldsymbol{L}}}}_{\rm L}^{j} \\ 1 \\ \end{bmatrix} \\ &= \prod\limits_{a = 1}^{n} {{\mathit{\boldsymbol{T}}}}_{a}^{a-1}({{\mathit{\boldsymbol{q}}}}_{a}) {{\mathit{\boldsymbol{T}}}}_{\rm EL} \begin{bmatrix} {{\mathit{\boldsymbol{p}}}}_{1} & {{\mathit{\boldsymbol{p}}}}_{2} & {{\mathit{\boldsymbol{p}}}}_{3} & \cdots & {{\mathit{\boldsymbol{p}}}}_{k} \\ 1 & 1 & 1 & \cdots & 1 \end{bmatrix} \end{align} $

(2)

式中，$ {{\mathit{\boldsymbol{T}}}}_{a}^{a-1}({{\mathit{\boldsymbol{q}}}}_{a}) $表示第$ a $个关节的齐次变换矩阵，$ {{\mathit{\boldsymbol{q}}}}_{a} $表示关节的旋转量，$ \mathit{\boldsymbol{T}}_{\text{EL}} $为标定矩阵。则轮廓数据$ {\mathit{\boldsymbol{Z}}} $为

$ \begin{align} {{\mathit{\boldsymbol{Z}}}}=\big[ {{\mathit{\boldsymbol{L}}}}_{\rm B}^{j} \big], \; \; j=1, 2, \cdots, m \end{align} $

(3)

由于标准圆柱的位置固定，因此轮廓数据$ {\mathit{\boldsymbol{Z}}} $中的点都被约束在同一圆柱侧面。以圆柱侧面约束原理建立约束优化方程。利用PSO算法和广义拉格朗日乘子法的融合算法求解约束优化问题，最终得到准确的手眼标定矩阵和标准圆柱的位置。标定方案流程见图 2。

3 标定算法（Calibration algorithm） 3.1 椭圆轮廓数据拟合

线激光轮廓传感器采用激光三角反射原理，激光束经过特殊的透镜投射到被测物体表面。反射光经过光学透镜组在感光元件表面形成光斑，不同高度的表面反射形成的光斑位置各不相同。经过相关信号处理与计算，可得到物体表面精确的轮廓数据。其原理图如图 3所示，$ O_{\rm L} $-$ X_{\rm L}Y_{\rm L}Z_{\rm L} $为线激光轮廓传感器坐标系，$ O_{\rm C} $-$ X_{\rm C}Y_{\rm C}Z_{\rm C} $和$ o $-$ uv $分别为相机坐标系和图像坐标系，$ O_{\rm W} $-$ X_{\rm W}Y_{\rm W}Z_{\rm W} $为世界坐标系。

在得到线激光轮廓传感器对标准圆柱侧面的测量数据后，需要利用统计滤波器对数据进行滤波处理，降低测量噪声对数据拟合的影响。由于激光平面与圆柱侧面的相交形状为椭圆，即激光轮廓为椭圆形，因此利用随机抽样一致性算法对滤波后的点云数据进行椭圆拟合，可以得到椭圆轮廓方程：

$ \begin{align} Ax^{2}+Bxz+Cz^{2}+Dx+Ez+1=0 \end{align} $

(4)

式中，$ A, B, C, D, E $是椭圆方程参数。

随机抽样一致性算法^[14]已被广泛应用在计算机视觉领域中。相对于最小二乘法，随机抽样一致性算法具有更好的鲁棒性，能从含有大量噪声点的数据中估计出高精度的模型参数。

3.2 建立标定模型

标定算法基于圆柱侧面的约束，即，使所有激光轮廓数据到圆柱侧面的距离之和最小。为方便计算，利用椭圆轮廓估计中心点到圆柱中轴线的距离表示椭圆轮廓数据到圆柱侧面的距离。根据椭圆轮廓方程可以求得椭圆中心点$ O_j $的坐标为

$ \begin{align} \begin{cases} x_{\rm L} = \dfrac{B E-2 C D}{4 A C-B^{2}} \\ y_{\rm L} = 0 \\ z_{\rm L} = \dfrac{B D-2 A E}{4 A C-B^{2}} \end{cases} \end{align} $

(5)

根据坐标齐次变换，将椭圆中心点$ O_{j} (x_{\rm L}, 0, z_{\rm L}) $转换到机器人基坐标系，得到点$ O_{j} (x_{\rm b}, y_{\rm b}, z_{\rm b}) $，即

$ \begin{align} \begin{bmatrix} {x_{\rm b}} \\ {y_{\rm b}} \\ {z_{\rm b}} \\ 1 \end{bmatrix}= {{\mathit{\boldsymbol{T}}}}_{\text{BE}} {{\mathit{\boldsymbol{T}}}}_{\text{EL}} \begin{bmatrix} {x_{\rm L}} \\ 0 \\ {z_{\rm L}} \\ 1 \end{bmatrix}= {{\mathit{\boldsymbol{T}}}}_{\text{BE}} \begin{bmatrix} {a_{11}} & {a_{12}} & {a_{13}} & {t_{1}} \\ {a_{21}} & {a_{22}} & {a_{23}} & {t_{2}} \\ {a_{31}} & {a_{32}} & {a_{33}} & {t_{3}} \\ 0 & 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} {x_{\rm L}} \\ 0 \\ {z_{\rm L}} \\ 1 \end{bmatrix} \end{align} $

(6)

由于椭圆中心点$ O_j $在传感器坐标系中的$ Y $轴坐标值恒为0，由式(6) 可知，元素$ a_{12} $、$ a_{22} $与$ a_{32} $对求解结果无影响，即待标定参数为$ [a_{11}, $ $ a_{21}, $ $ a_{31}, $ $ a_{13}, $ $ a_{23}, $ $ a_{33}, $ $ t_{1}, $ $ t_{2}, $ $ t_{3}] $。

假设在机器人基坐标系下，已知圆柱轴线上一点$ P(x_0, y_0, z_0) $和轴线的方向向量$ \mathit{\boldsymbol{N}}=[m, l, n] $，则圆柱轴线的方程为

$ \begin{align} \frac{x-x_0}{m}= \frac{y-y_0}{l}= \frac{z-z_0}{n} \end{align} $

(7)

连接椭圆中心点$ O_{j} (x_{\rm b}, y_{\rm b}, z_{\rm b}) $和圆柱轴线上一点$ P(x_0, y_0, z_0) $构成线段$ O_jP $，则线段$ O_jP $在圆柱轴线上的投影长度$ L $为

$ \begin{align} L & = [m \; \; l \; \; n] \begin{bmatrix} x_{\rm b}-x_{0} \\[-2pt] y_{\rm b}-y_{0} \\[-2pt] z_{\rm b}-z_{0} \end{bmatrix} \end{align} $

(8)

则点$ O_{j} $到圆柱轴线的距离的平方$ d_{j}^{2} $为

$ \begin{align} d_{j}^{2} =\;&|O_{j} P|^{2}-L^{2} \\ =\;& [({x}_{\rm b}-{x}_{0})^{2}+(y_{\rm b}-y_{0})^{2}+(z_{\rm b}-z_{0})^{2}]- \\ &[m({x}_{\rm b}-{x}_{0})+l(y_{\rm b}-y_{0})+n(z_{\rm b}-z_{0})]^{2} \end{align} $

(9)

将式(8) 代入式(9) 中，计算$ d_{j}^{2} $的值，此时，由于圆柱轴线的引入，标定参数变为

$ \begin{align} {{\mathit{\boldsymbol{x}}}} =\;[& a_{11}, a_{21}, a_{31}, a_{13}, a_{23}, a_{33}, t_{1}, t_{2}, t_{3}, \\ & {x}_{0}, y_{0}, z_{0}, m, l, n] \end{align} $

(10)

其中有单位向量约束

$ \begin{align} \begin{cases} g_{1}: a_{11}{}^{2}+a_{21}{}^{2}+a_{31}{}^{2} = 1 \\ g_{2}: a_{13}{}^{2}+a_{23}{}^{2}+a_{33}{}^{2} = 1 \\ g_{3}: m^{2}+l^{2}+n^{2} = 1 \end{cases} \end{align} $

(11)

改变机器人末端位姿，得到多条激光轮廓数据，利用数据拟合求出每条激光轮廓的椭圆中心点$ O_{j} $。对手眼变换矩阵参数和圆柱轴线参数进行初始值估计，利用估计的初始值，求出每条轮廓中心点到圆柱轴线的距离的平方$ d_{j}^{2} $。理想状态下，中心点$ O_{j} $位于圆柱轴线上，即$ d_{j}^{2} =0 $。通过不断更新标定参数的估计值，使每条轮廓的$ d_{j}^{2} $趋向于0。因此将手眼标定问题转化为约束优化问题，即

$ \begin{align} \mathop{\arg \min} \limits_{{\mathit{\boldsymbol{x}}}} f({{\mathit{\boldsymbol{x}}}})=\sum\limits_{j=1}^k {d_{j}^{2}, \quad \text{s.t.}\; \; g_{1}, g_{2}, g_{3}} \end{align} $

(12)

当方向向量$ \mathit{\boldsymbol{N}} $ $ = $ $ [m, l, n] $的元素中存在0值时，文[13]算法标定失效，但本文算法仍可通过改变机器人的位姿，得到不同的距离$ d_{j} $。即该标定结果不受标定物位置的影响。

3.3 求解标定模型

对于求解有约束非线性优化问题，往往采用罚函数法，但该方法会使罚因子在迭代过程中趋向于无穷大，进而使增广目标函数变得“病态”，影响无约束优化求解，这是罚函数法的主要缺点^[15]。广义拉格朗日乘子法是将外罚函数法与拉格朗日乘子法结合起来，当罚因子适当大时，通过改变乘子的值，使结果逐次迭代逼近最优解。这样就避开了罚函数法的主要缺点，同时乘子的引入使广义拉格朗日乘子法对初始值的选取要求更低^[16]。但当约束优化方程含有多个极值点时，广义拉格朗日乘子法易陷入局部最优。因此，本文提出一种PSO算法和广义拉格朗日乘子法的融合算法。以广义拉格朗日乘子法的初始值作为PSO算法的粒子，以优化函数的值作为更新粒子的依据。将PSO算法中每代的全局最差粒子$ {{\mathit{\boldsymbol{g}}}}_{\text{worst}}^{n} $作为广义拉格朗日乘子法的最优初始值进行迭代，可以有效避免初始值估计对求解约束优化方程的影响，提高了手眼标定成功率。

3.3.1 广义拉格朗日乘子法

有约束优化问题的标准形式为

$ \begin{align} & {\mathop{\arg \min} \limits_{{\mathit{\boldsymbol{x}}}} f({{\mathit{\boldsymbol{x}}}})} \\ \text{s.t. }\; \;& {{\mathit{\boldsymbol{h}}}}({{\mathit{\boldsymbol{x}}}})=[h_{1} ({{\mathit{\boldsymbol{x}}}}), h_{2} ({{\mathit{\boldsymbol{x}}}}), \cdots, h_{l} ({{\mathit{\boldsymbol{x}}}})]^{\rm T}=0 \end{align} $

(13)

式中，$ f({{\mathit{\boldsymbol{x}}}}) $为优化函数，$ {{\mathit{\boldsymbol{h}}}}({{\mathit{\boldsymbol{x}}}}) $为等式约束方程，$ {\mathit{\boldsymbol{x}}} $为$ n $维待求解的参数向量。则广义拉格朗日乘子法求解有约束优化问题的步骤如下：

(1) 选取初始值

初始点$ {{\mathit{\boldsymbol{x}}}}_{0} \in {\rm R}^{n} $，乘子向量$ {\lambda}_{0} \in {\rm R}^{l} $，罚因子$ \sigma_{0} >0 $，允许误差$ \varepsilon \to 0 $，罚因子修正系数$ \eta >1 $，比例系数$ \alpha \in (0, 1) $，迭代次数$ k=1 $。

(2) 无约束优化问题

构建增广目标函数，求解无约束优化方程的最小点坐标$ {{\mathit{\boldsymbol{x}}}}_{k} $：

$ \begin{align} {{\mathit{\boldsymbol{x}}}}_{k} =& \mathop{\arg \min} \limits_{{\mathit{\boldsymbol{x}}}} \varphi ({{\mathit{\boldsymbol{x}}}}, {\lambda}_{k}, \sigma_{k}) \\ =&f({{\mathit{\boldsymbol{x}}}})-{\lambda}_{k}^{\rm T} {{\mathit{\boldsymbol{h}}}}({{\mathit{\boldsymbol{x}}}})+\frac{\sigma_{k} \| {{{\mathit{\boldsymbol{h}}}}({{\mathit{\boldsymbol{x}}}})} \|^{2}}{2} \end{align} $

(14)

(3) 终止条件

若$ \| {{{\mathit{\boldsymbol{h}}}}({{\mathit{\boldsymbol{x}}}}_{k})} \|\leqslant \varepsilon $，则将$ {{\mathit{\boldsymbol{x}}}}_{k} $作为结果输出；否则，继续下面步骤。

(4) 更新罚因子

若$ \| {{{\mathit{\boldsymbol{h}}}}({{\mathit{\boldsymbol{x}}}}_{k})} \|\geqslant \alpha \| {{{\mathit{\boldsymbol{h}}}}({{\mathit{\boldsymbol{x}}}}_{k-1})} \| $，则令$ \sigma_{k+1} =\eta \sigma_{k} $；否则，$ \sigma_{k+1} =\sigma_{k} $。

(5) 乘子向量

令$ {\lambda}_{k+1} ={\lambda}_{k} -\sigma_{k} {{\mathit{\boldsymbol{h}}}}({{\mathit{\boldsymbol{x}}}}_{k}) $，$ k=k+1 $，返回第(1) 步。

BFGS（Broyden-Fletcher-Goldfarb-Shanno）算法是目前最有效的拟牛顿校正方法之一^[15]，因此利用BFGS算法求解第(2) 步无约束优化问题。

3.3.2 PSO算法

PSO算法是一种基于群体智能理论的随机搜索算法，利用种群间个体的合作与竞争来实现对优化问题的求解^[17]。在PSO算法中，第$ i $个粒子的$ k $维元素在第$ n $次迭代中的位置和速度按式(15) 进行更新。

$ \begin{align} \!\!\!\!\begin{cases} {{\mathit{\boldsymbol{v}}}}_{i k}^{n+1} = w {{\mathit{\boldsymbol{v}}}}_{i k}^{n}+c_{1} r_{1}({{\mathit{\boldsymbol{p}}}}_{i k}^{n}-{{\mathit{\boldsymbol{x}}}}_{i}^{n})+c_{2} r_{2}({{\mathit{\boldsymbol{p}}}}_{{\rm g} k}^{n}-{{\mathit{\boldsymbol{x}}}}_{i k}^{n}) \\ {{\mathit{\boldsymbol{x}}}}_{i k}^{n+1} = {{\mathit{\boldsymbol{x}}}}_{i k}^{n}+{{\mathit{\boldsymbol{v}}}}_{i k}^{n} \end{cases}\!\!\! \end{align} $

(15)

式中，$ w $为惯性因子，$ c_{1} $、$ c_{2} $为非负加速因子，$ r_{1} $、$ r_{2} $为在范围$ [0, 1] $中的随机数，$ {{\mathit{\boldsymbol{x}}}}_{i} $、$ {{\mathit{\boldsymbol{v}}}}_{i} $和$ {{\mathit{\boldsymbol{p}}}}_{i} $分别表示第$ i $个粒子的位置、速度和个体极值，$ {{\mathit{\boldsymbol{p}}}}_{\rm g} $为粒子的全局极值。

PSO算法具有计算速度快、结构简单的特点，被广泛应用于解决各种优化问题。但由于缺乏对粒子速度的动态调节，该算法也易陷入局部极值^[17]。

3.3.3 PSO算法与广义拉格朗日乘子法的融合算法

广义拉格朗日乘子法可以有效解决约束优化问题，但当约束优化方程含有多个极值点时，广义拉格朗日乘子法的初始值选取会对优化结果有影响，使迭代过程不收敛或收敛到局部极值。而PSO算法中，粒子总是向全局最优粒子靠近，则每一代的全局最差粒子$ {{\mathit{\boldsymbol{g}}}}_{\text{worst}}^{n} $最有可能跳出上一代的局部最优点，进入另一个局部最优点。将全局最差粒子$ {{\mathit{\boldsymbol{g}}}}_{\text{worst}}^{n} $作为广义拉格朗日乘子法的最优初始点进行迭代，可以有效避免算法陷入局部最优，提高求解优化方程的成功率。其算法流程图如图 4所示，共有7个步骤：

(1) 利用PSO算法进行初始化。将广义拉格朗日乘子法的初始值$ {{\mathit{\boldsymbol{x}}}}_{0} \in {\rm R}^{n} $以向量的形式作为PSO算法的粒子$ {{\mathit{\boldsymbol{X}}}}_{i} $，以无约束优化函数$ \varphi ({{\mathit{\boldsymbol{x}}}}, {\lambda}_{k}, \sigma_{k}) $为适应度函数$ F({{\mathit{\boldsymbol{X}}}}_{i}) $，迭代次数$ n=1 $，全局最优粒子为$ {{\mathit{\boldsymbol{g}}}}_{\text{best}} $，最优粒子的适应度为$ f_{\text{best}} $，迭代最大次数为$ n_{\max} $，终止误差为$ \psi $。

(2) 计算粒子的适应度。计算第$ n $次迭代的粒子适应度，寻找全局最优粒子$ {{\mathit{\boldsymbol{g}}}}_{\text{best}}^{n} $及其适应度$ f_{\text{best}}^{n} $和全局最差粒子$ {{\mathit{\boldsymbol{g}}}}_{\text{worst}}^{n} $及其适应度$ f_{\text{worst}}^{n} $。

(3) 更新$ {{\mathit{\boldsymbol{g}}}}_{\text{best}} $、$ f_{\text{best}} $。如果$ f_{\text{best}}^{n} <f_{\text{best}} $，则令$ {{\mathit{\boldsymbol{g}}}}_{\text{best}} $ $ = $ $ {{\mathit{\boldsymbol{g}}}}_{\text{best}}^{n} $，$ f_{\text{best}} =f_{\text{best}}^{n} $，并跳到第(5) 步；否则，进行下一步。

(4) 利用广义拉格朗日乘子法求解约束优化问题。以$ {{\mathit{\boldsymbol{g}}}}_{\text{worst}}^{n} $为广义拉格朗日乘子法初始值，求解约束优化问题。如果$ \varphi ({{\mathit{\boldsymbol{g}}}}_{\text{worst}}^{n}, {\lambda}_{k}, \sigma_{k})<f_{\text{best}} $，则优化结果替代$ {{\mathit{\boldsymbol{g}}}}_{\text{best}} $，并用其对应的$ \varphi ({{\mathit{\boldsymbol{g}}}}_{\text{worst}}^{n}, {\lambda}_{k}, \sigma_{k}) $替换$ f_{\text{best}} $。

(5) 判断是否满足终止条件。如果$ (n<n_{m}) $ $ \wedge $ $ (f_{\text{best}} $ $ < $ $ \psi) $为真，则输出$ {{\mathit{\boldsymbol{g}}}}_{\text{best}} $；否则进行下一步。其中“$ \wedge $”为逻辑合取符号。

(6) 更新粒子。按式(15) 更新粒子的速度与位置信息。

(7) 迭代。令$ n=n+1 $，跳到第(2) 步。

4 实验结果（Experimental results）

本文选择的机器人为UR5六轴关节机器人，定位精度为$ \pm 0.1 $ mm；线激光轮廓传感器为SSZN公司的SR6130型传感器，精度为$ \pm 5 $ µm；选择轴类的机加工零件作为标准圆柱，其直径为40 mm，加工精度为$ \pm 0.01 $ mm；与基于平面约束的算法和基于标定球的算法进行仿真对比实验，验证本文算法的有效性，分析了标定参数初始值和标定数据误差对标定结果的影响。

其中标准圆柱直径应根据线激光轮廓传感器的测量范围来选择。标准圆柱直径与线激光轮廓传感器的测量范围的关系可以参考附录。

4.1 仿真验证实验 4.1.1 有效性验证

首先获取标定数据。利用仿真软件创建六轴关节型机器人，并创建相机坐标系，以相机坐标系的$ O $-$ XY $平面为线激光平面。在机器人基坐标系内设置圆柱轴线方程为

$ \begin{align} \frac{x-1}{3}=\frac{y-1}{1}=\frac{z-1}{1} \end{align} $

(16)

设置手眼标定矩阵为

$ \begin{align} {{\mathit{\boldsymbol{T}}}}_{\text{EL}} =\begin{bmatrix} 0.5771 & 0.4940 & 0.6503 & {150} \\ -0.8153 & 0.3021 & 0.4940 & {200} \\ 0.0475 & -0.8153 & 0.5771 & {250} \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{align} $

(17)

假设线激光轮廓传感器的测量误差为$ \pm 5 $ µm，机器人定位误差为$ \pm 0.1 $ mm，则改变机器人末端位姿，记录50组机器人位姿数据$ H_i $和激光平面与圆柱轴线的交点数据$ O_i $。并将上述数据代入约束优化方程，利用PSO算法与广义拉格朗日乘子法的融合算法求解近似最优的标定参数。

设置粒子群的粒子数量为20，终止误差为0.5 mm。随着PSO算法的迭代次数的增加，迭代过程中粒子的适应度$ f_{i} $和广义拉格朗日乘子法输出的最优值$ \varphi_{\text{best}} $的变化趋势如图 5所示，其中PSO算法迭代了4次，广义拉格朗日乘子法分别得到4个不同的局部最优值。可以得出，该融合算法解决了广义拉格朗日乘子法易受初始点选取的影响，使结果陷入局部最优的问题。同时也反映出不同标定参数初始值对广义拉格朗日乘子法的求解结果有很大的影响。

广义拉格朗日乘子法在第4次迭代时，各标定参数$ {{\mathit{\boldsymbol{x}}}}_n $和无约束优化函数$ f({{\mathit{\boldsymbol{x}}}}_{n}) $的变化趋势如图 6所示。仿真实验的标定值与理论值的对比如表 1所示。其中标定参数$ (x_0, y_0, z_0) $为圆柱轴线上的一点，比较标定结果到理论轴线的距离$ d $来验证该标定参数的精度。可以看出，在测量误差和机器人定位误差的干扰下，该算法仍具有收敛速度快、计算精度高的优点。

4.1.2 标定参数初始值对标定结果的影响

为方便研究标定参数初始值对标定结果的影响，不考虑机器人和线激光轮廓传感器的随机误差。改变机器人姿态，使线激光轮廓传感器的激光平面与标准圆柱侧面相交，记录多组数据并代入上述算法中，得到如式(12) 的约束优化方程$ f({{\mathit{\boldsymbol{x}}}}) $。

根据标定参数中各个参数的取值特点设置取值范围，如表 2所示。在标定参数取值范围内随机生成不同标定参数初始值，分别单独利用广义拉格朗日乘子法和利用广义拉格朗日乘子法与PSO算法的融合算法求解约束优化方程$ f({{\mathit{\boldsymbol{x}}}}) $的最小值，重复进行15组对比实验，记录各组求解的值，结果如表 3所示。其中求解结果小于$ 10^{-3} $ mm代表标定成功，大于$ 10^{3} $ mm代表求解结果不收敛，在这之间代表求解收敛到局部最优。

由表 3可以看出，标定参数初始值对单独利用广义拉格朗日乘子法求解约束优化方程有很大的影响。在15组实验中，有4组求解结果陷入局部最优（橘色），6组求解结果无法收敛（红色），只有5组求解约束优化方程$ f({{\mathit{\boldsymbol{x}}}}) $的最小值小于$ 10^{-3} $ mm，表明标定成功，其标定成功率为33.3%。而利用广义拉格朗日乘子法与PSO算法的融合算法的标定成功率为100%，由此可知本文算法不受标定参数初始值的影响。

4.1.3 标定物位置对标定结果的影响

为方便比较目标函数值的变化，将机器人和线激光轮廓传感器的随机误差设为0 mm，设置PSO算法的终止误差为10$ ^{-8} $ mm。改变机器人的位姿，使激光平面与标准圆柱侧面相交，记录多组数据，并将数据代入上述算法中，计算标定结果。改变标准圆柱在机器人基坐标系下的位置，进行多组实验，记录上述算法的迭代过程，结果如表 4所示，每一组的目标函数值都远小于设定的终止误差，成功实现机器人的手眼标定。可以看出，相对于文[13]中的方法，该标定方法不受标定物位置的影响。

4.1.4 标定数据误差对标定结果的影响

为了研究标定数据误差对标定结果的影响，设计离线仿真实验。标定数据误差主要有机器人定位误差、线激光轮廓传感器的测量误差以及椭圆轮廓拟合误差。设椭圆轮廓中心点在机器人基坐标系下的坐标为$ O_{j} ({x}_{\rm b}, y_{\rm b}, z_{\rm b}) $。在本文标定算法中，上述误差都反映在计算$ O_{j} ({x}_{\rm b}, y_{\rm b}, z_{\rm b}) $的误差上，为方便研究标定数据误差对标定结果的影响，将随机误差叠加在$ O_{j} ({x}_{\rm b}, y_{\rm b}, z_{\rm b}) $的理论值中。

设置标定数据误差满足10组不同等级的误差。设置第1组的随机误差满足期望为0 mm、方差为0.02 mm$ ^{2} $的正态分布。其余每组按照组号依次对第1组生成的随机误差乘以每组组号，得到每组的随机误差。设置PSO算法的终止误差为0.5 mm。改变机器人的位姿，使激光平面与标准圆柱侧面相交，记录50次数据，并将随机误差叠加到$ O_{j} ({x}_{\rm b}, y_{\rm b}, z_{\rm b}) $的理论值中，计算手眼标定矩阵。记录各组手眼标定矩阵的欧拉角和平移向量与理论值的偏差$ d $，其结果如图 7所示，其中$ R_x $、$ R_y $和$ R_z $表示手眼标定矩阵的“$ xyz $”型欧拉角与理论值的偏差，$ D $表示手眼标定矩阵的平移向量与理论值的偏差。

由图 7可以看出，标定精度与标定数据误差成线性正相关，则直线斜率大小主要受实验中第1组生成的随机误差分布的影响。由于采集50组数据无法很好地反映均值为0 mm的正态分布随机误差，因此提高实验采集次数和机器人多姿态测量次数可以有效降低标定数据误差对标定精度的影响；由于标定矩阵的偏差与标定数据误差成正相关，因此提高机器人定位精度和线激光轮廓传感器的测量精度都可以提升手眼标定的精度。

4.1.5 标定算法的对比实验

为研究线激光轮廓传感器的测量误差、机器人自身误差以及椭圆轮廓拟合误差对标定算法的影响，对该标定算法与文[10]中算法进行仿真对比实验。设置机器人定位误差满足期望为0 mm、方差为0.033 mm$ ^{2} $的正态分布；椭圆轮廓拟合误差可以叠加到线激光轮廓传感器的测量误差中，设置测量误差满足期望为0 mm、方差为0.033 mm$ ^{2} $的正态分布。

分别在移动机器人的不同位姿下进行50次数据采集，记录每次机器人的末端变换矩阵$ \mathit{\boldsymbol{T}}_{\text{BE}} $和线激光轮廓传感器坐标系$ \varSigma_{\rm L} $中的特征点$ O_{j}^{\rm L} $。对本文方法和文[10]中算法分别重复进行16组采集实验。将机器人定位误差和线激光轮廓传感器的测量误差叠加到其中15组采集实验的理论值中，求解手眼变换矩阵$ \mathit{\boldsymbol{T}}_{\text{EL}} $。利用设定的手眼变换矩阵$ \mathit{\boldsymbol{T}}_{\text{EL}}^S $将第16组中本文算法采集实验所获得的特征点$ O_{j}^{\rm L} $变换到机器人基坐标系下，得到其对应点$ O_{j}^{\rm B} $。分别利用每组求解出的手眼变换矩阵$ \mathit{\boldsymbol{T}}_{\text{EL}} $将第16组中本文算法采集实验所获得的特征点$ O_{j}^{\rm L} $变换到机器人基坐标系中，得到其对应点$ O_{j}^{\rm Bn} $，计算$ O_{j}^{\rm B} $与$ O_{j}^{\rm Bn} $之间的误差$ d $，并记录下来，如图 8所示。

标定误差的大小主要反映手眼标定中平移矩阵的标定精度，而标定误差在每组标定数据中的抖动则主要反映手眼标定中旋转矩阵的标定精度。由图 7可以看出，相比文[10]的标定结果，本文标定结果中的旋转矩阵和平移矩阵都具有更高的精度，标定精度提高了50% 左右，同时本文算法也具有更强的抗干扰能力。

相比于文[10]的标定算法，本文标定算法不需要求解特征点在$ \varSigma_{\rm L} $坐标系中的$ y $坐标值，相应的，也不需要计算旋转矩阵中第2列的值。本文算法降低了标定维度，同时也降低了测量误差和机器人误差对标定结果的影响，实验结果也证实了这一点。

4.2 扫描重建实验 4.2.1 参数标定

对末端搭载线激光轮廓传感器的机器人进行手眼标定实验。如图 9所示，搭建手眼标定实验平台。改变机器人末端位姿，使线激光平面与标准圆柱侧面相交，记录30组机器人末端位姿和激光轮廓数据。为减小机器人定位误差对标定结果的影响，应避免机器人大幅度地运动^[12]，尽量减少移动关节的数量^[6]；记录机器人位姿时，应在机器人控制器中与激光线轮廓数据同时记录。机器人位姿数据如表 5所示。

根据文中的手眼标定算法，求解手眼标定矩阵为

$ \begin{align} {{\mathit{\boldsymbol{T}}}}_{\text{EL}} =\begin{bmatrix} -0.9630 & a_{12} & -0.0030 & 149.6886 \\ -0.2692 & a_{22} & -0.0244 & 4.2657 \\ 0.0114 & a_{32} & -0.9997 & 249.8106 \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{align} $

(18)

圆柱轴线方程为

$ \begin{align} \frac{x-742.7532}{0.6093}=\, &\frac{y-346.7995}{0.7926} = \frac{z-21.4034}{0.0026} \end{align} $

(19)

则根据手眼标定参数计算每条椭圆轮廓中心到圆柱轴线的距离$ d $，如图 10所示，由图可知距离$ d $的最大值将近0.15 mm。

4.2.2 零件扫描重建实验

根据手眼标定结果，进行零件扫描重建实验，验证手眼标定精度。在机器人工作平台上放置如图 11所示的零件。其中，零件的$ A $面、$ B $面和$ D $面互相垂直；$ B $面与$ C $面垂直，其垂直度误差为$ 0.0542^{\circ} $；$ C $面与$ A $面平行，其距离为40.0331 mm（数据由Zeiss三坐标测量仪测得）。移动机器人，分别使线激光平面与零件的$ A $面、$ B $面、$ C $面和$ D $面相交，各记录20次机器人末端位姿和激光轮廓数据。为降低零件表面反光对测量数据的影响，应使传感器与零件表面成一定夹角进行拍摄。利用手眼标定结果，将零件各面上激光轮廓数据转到机器人基坐标系下，并分别计算$ B $面和$ C $面激光轮廓数据所拟合的平面方程，结果如下：

$ \begin{align} \begin{cases} B:\; 1.715\times 10^{2}x+6.545y+z-5.098\times 10^{4}=0 \\ C:\; 1.944x+4.904y-10^{3}z+3.058\times 10^{4}=0 \end{cases} \end{align} $

(20)

则两平面的夹角为90.212$ ^{\circ} $，相交线的方向向量为$ {{\mathit{\boldsymbol{m}}}} = [-0.0382, 0.9993, 0.0048] $。

分别在$ B $面和$ C $面的每条激光轮廓数据中随机选取20个点，计算其到各拟合平面的距离，如图 12所示。由图可知，零件平面重建精度高，拟合误差为$ \pm 0.15 $ mm。

在$ A $面的每条激光轮廓数据中随机选取20个点，计算其到零件$ C $面的拟合平面的距离，如图 13所示。由图可知，在$ A $面上任意位姿的激光轮廓数据到拟合平面的距离都在$ 39.90 $~$ 40.15 $ mm之间，则根据三坐标测量仪对零件$ A $面到$ C $面的测量值，可以得出，3维重建的零件各位置的相对距离精度为$ \pm 0.12 $ mm。

在机器人基坐标系下，$ A $面和$ B $面的交线方向向量$ {\mathit{\boldsymbol{m}}} $垂直于$ D $面，即，向量$ {\mathit{\boldsymbol{m}}} $与$ D $面的每条激光轮廓的夹角为90$ ^{\circ} $。计算$ D $面上每条激光轮廓的方向向量$ {{\mathit{\boldsymbol{n}}}}_j $与$ {\mathit{\boldsymbol{m}}} $的夹角$ \theta_j $，结果如图 14所示。可以看到测量的激光轮廓与向量$ {\mathit{\boldsymbol{m}}} $的夹角与理论值之间的误差不超过$ 0.3^{\circ} $。由此可以看出3维重建的零件各位置的相对角度精度较高。

5 结论（Conclusion）

针对线激光轮廓传感器与机器人的手眼标定问题，提出一种利用标准圆柱侧面约束的手眼标定方法。该方法只需要1块标准的圆柱体，标定过程简单、易操作。仿真验证实验结果表明，该标定方法不受标定物位置和标定参数初始值的影响。

利用标准圆柱侧面约束，将标定问题转化为求解约束优化问题。传统的广义拉格朗日乘子法，求解结果易受初始值的影响而陷入局部最优或无法收敛；本文利用PSO算法和广义拉格朗日乘子法的融合算法求解约束优化方程，避免了初始值对求解结果的影响，提高了手眼标定的成功率和准确性。仿真验证实验证明了该融合算法的有效性。

为研究标定数据误差对标定结果的影响，设计仿真实验，结果表明，标定数据误差与标定精度成线性正相关；通过设计与相关文献的对比仿真实验证明了本文算法具有更高的鲁棒性和标定精度；同时为计算本文算法的标定精度，设计对零件的扫描重建实验，结果表明，该标定方法具有较高的精度，其中标定的平移精度在0.15 mm以内，旋转精度在$ 0.3^{\circ} $以内。

在实际应用中，手眼标定算法需要具有便捷、准确和快速的特点。本文算法虽然具有很好的便捷性和准确性，但利用PSO算法和广义拉格朗日乘子法的融合算法求解约束优化方程，在提高求解成功率的同时，也提高了算法的复杂度，增加了求解手眼变换矩阵的时间成本。因此，进一步研究求解约束优化方程的相关算法，提高求解手眼变换矩阵的效率是后续研究的重点。

附录（Appendix）

为研究线激光轮廓传感器测量范围和标准圆柱尺寸的关系，以线激光平面与标准圆柱相交的极限位置为例进行讨论。假设线激光平面与圆柱侧面相交，得到如图 15所示的椭圆轮廓。图(a) 表示线激光测得的椭圆长轴轮廓，其激光平面与圆柱底面的夹角为$ \alpha $；图(b) 表示线激光测得的椭圆短轴轮廓，其激光平面与圆柱底面的夹角为$ \beta $。点$ P_1 $、$ P_2 $为线激光器发射点，分别位于椭圆轮廓的短轴和长轴上；$ \theta $为线激光器发射范围夹角；$ B_1 $、$ B_2 $为线激光轮廓传感器的测量上限，$ A_1 $、$ A_2 $为线激光轮廓传感器的测量下限。

对图(a) 和图(b) 分别考虑以下2种极限情况：

(1) 线激光发射的激光线与椭圆轮廓相切于点$ A_1 $、$ A_2 $。

(2) 线激光发射的激光线与椭圆轮廓相交于点$ A_1 $、$ A_2 $，并且与直线$ B_1 B_2 $相切于点$ Q $。

已知线激光轮廓传感器的景深为$ h $，最大测量高度为$ D $，横向最大测量距离为$ L $，则标准圆柱的半径$ R $应满足：

$ \begin{align} \begin{cases} R<\dfrac{D\tan (0.5\times \theta)\sqrt{(\cos \alpha \tan (0.5\times \theta))^{2}+4}}{4} \\[5pt] R<\dfrac{4h^{2}+(D\tan (0.5\times \theta)\cos \alpha)^{2}}{8h} \\[5pt] R<\dfrac{D\tan (0.5\times \theta)\sqrt{\tan^{2} (0.5\times \theta) + 4 \times \cos^{2} \beta}}{4\cos \beta} \\[5pt] R<\dfrac{4\times (h\cos \beta)^{2}+(D\tan (0.5\times \theta))^{2}}{8\times h\cos \beta} \end{cases}\end{align} $

(21)

其中夹角$ \theta $由线激光轮廓传感器的横向最大测量距离$ L $和最大测量高度$ D $来确定，为避免上述极限状态，需要对$ \theta $取值进行限定，即$ \theta $的取值应满足：

$ \begin{align} \theta \in [1.2\times \arctan (L/{(2\times D)}), 1.7\times \arctan (L/{(2\times D)})] \end{align} $

(22)

由于椭圆长轴轮廓相比于短轴轮廓曲率变化较平缓，因此，为了降低椭圆轮廓曲率对测量数据的影响，激光平面与圆柱底面的夹角$ \alpha $的取值范围应在50$ ^{\circ} $ ~ 75$ ^{\circ} $之间，$ \beta $的取值范围应在40$ ^{\circ} $ ~60$ ^{\circ} $之间。

根据线激光轮廓传感器的景深$ h $、最大测量高度$ D $和横向最大测量距离$ L $，选择合适的角度参数代入式(21) 就可以计算出参考的标准圆柱的尺寸。理论上选择标准圆柱的直径只要小于参考的圆柱直径都能满足要求，但较大尺寸的标准圆柱难以通过机加工保证加工精度，因而也会增加标定成本；较小尺寸的标准圆柱，使传感器测量数据较少，无法有效进行椭圆拟合。综上，选择标准圆柱的直径在20 mm ~100 mm之间较合适。