CN116352710A - 一种用于大型航天构件的机器人自动标定与三维测量方法 - Google Patents

Abstract

本发明公开了一种用于大型航天构件的机器人自动标定与三维测量方法。首先，基于Kronecker积和Moore‑Penrose逆对手眼关系方程进行线性求解，在保证更准确的封闭解的同时更有针对性地减少标定过程中系统噪声、计算带来的误差影响，并且采用Rodrigues公式对所求解手眼关系矩阵H_cg的旋转部分进行正交化以消除测量噪声的影响，提高算法运行效率；然后，为了进一步提高手眼标定精度，提升点云配准误差，引入基于最小化欧式变换误差的差分进化(DE)算法应用于手眼标定，以解决由于缺少对旋转矩阵的约束条件导致手眼方程解的不稳定性；最后，采集大型航天构件的多幅单元测量点云数据，完成大型航天构件作为待测物的三维形貌测量任务。

Description

一种用于大型航天构件的机器人自动标定与三维测量方法

技术领域

本发明属于三维形貌测量技术领域，更为具体地讲，涉及一种用于大型航天构件的机器人自动标定与三维测量方法。

背景技术

航空航天领域的关键零部件通常为尺寸较大、形状复杂的大型航天构件，一般的结构光相机或者激光扫描仪等三维测量设备的视场有限，单一视角下往往只能采集到大型测量对象的局部点云数据。为了重构出大型航天构件的三维形貌，需要将每个单元测量所得的局部点云数据统一在同一基准坐标系下。因此，基于工业机器人约束位姿的三维重建技术为大型航天构件的非接触式测量提供了新思路。

基于工业机器人的三维重建技术利用工业机器人作为运动载体，通过机器人各轴关节坐标系之间的约束来确定各视角的单元测量坐标系的位姿关系，使测量系统在保留视觉测量技术非接触、快速等特点的同时，机器人快速灵活的特性增强了整个测量系统的柔性。

工业机器人目前手眼标定的方式主要通过相机拍摄标定物进行位姿识别，获取相机参数以及机器人姿态信息，建立手眼标定方程，然后进行手眼方程解算。目前主要通过不同的数学工具如旋量、四元数、对偶四元数、来描述空间运动，或者是加不同的运动约束如纯移动或者纯转动来将旋转和平移解耦开来，获取手眼关系矩阵。然而，上述方法只考虑到了最小化代数误差来计算手眼关系矩阵，完成手眼标定，并未考虑到图像噪声对手眼标定精度的影响，这就导致线性求解得到姿态关系的标定精度低，从而影响三维形貌测量的准确性和完整性。

此外，手眼关系的求解精度所带来的问题以及相关的测量任务的完成效率等方面，是本类测量装置当前面临的一个很严峻的问题。为了进一步解决三维测量任务的精度需求，需要对手眼方程结算结果进行补偿。由于实际世界坐标和标定结果计算得到的世界坐标之间存在误差，如按照2022年12月30日公布的、公布号为CN115546289A的中国发明专利申请《一种基于机器人的复杂结构件三维形貌测量方法》，只是对该误差进行均值补差达不到测量任务需要。

发明内容

本发明的目的在于克服现有技术的不足，提供一种用于大型航天构件的机器人自动标定与三维测量方法，以线性求解出更准确的封闭解，简化计算并提高了效率，并更有针对性地减少标定过程中系统噪声、计算带来的误差影响，同时修正手眼关系精度，以保证多视角点云数据的配准精度，很大程度上提高多视角三维形貌测量的效率。

为实现上述发明目的，本发明用于大型航天构件的机器人自动标定与三维测量方法，其特征在于，包括：

(1)、建立手眼标定方程

将双目相机安装于机器人末端法兰处，控制机器人运动至第i个拍摄位姿，对棋盘格标定板上一角点进行拍摄，在该拍摄位姿获得标定板图片，同时记录机器人位姿信息以及角点在相机坐标系中的位置P_i，这样对n个拍摄位姿进行拍摄，得到n组标定板图片和相对应机器人姿态信息；

根据张氏标定法获得每组标定板图片中标定板相对于相机的旋转向量R_ci和平移向量T_ci，并转换成旋转平移矩阵，即得到相机外参矩阵H_ci,i＝1,2,...,n，根据机器人位姿信息计算相对于基座的旋转向量R_gi和平移向量T_gi，得到机器人位姿矩阵H_gi,i＝1,2,...,n，其中：

对于任意两次变换拍摄位姿u,v之间相机外参矩阵、机器人位姿矩阵，建立手眼标定方程：

其中：

其中，R_gu,v表示矩阵

中的旋转矩阵，T_gu,v表示矩阵

中的平移向量，R_cg表示手眼关系矩阵H_cg中的旋转矩阵，T_cg表示手眼关系矩阵中的平移向量，R_cu,v表示矩阵

的旋转矩阵，T_cu,v表示矩阵

中的平移向量；

(2)、利用Kronecker积和Moore-Penrose逆将手眼标定方程转化为最小二乘问题并利用奇异值分解计算出手眼关系矩阵H_cg

建立线性方程组：

其中，I为单位矩阵，

表示Kronecker积，vec表示向量化操作；

将所有两次变换拍摄位姿u,v下的矩阵：

按列放置，得到一个矩阵R；

对矩阵R进行奇异值分解，得到U矩阵即左奇异矩阵、奇异值矩阵∑以及V矩阵即右奇异矩阵；

将所有两次变换拍摄位姿u,v下的矩阵：

按列放置，得到一个矩阵T_g；

取列向量V∑^-1U^TT_g的前9个元素还原成3×3矩阵形式，得到矩阵

然后，对矩阵

进行正交化奇异值分解即

其中，U_R为右奇异矩阵，∑_R为奇异值矩阵，V_R为左奇异矩阵，得到手眼关系矩阵H_cg的旋转矩阵R_cg即

将所有两次变换拍摄位姿u,v下的矩阵(R_gu,v-I)按列放置，得到一个矩阵R_g，将所有两次变换拍摄位姿u,v下的矩阵(R_cgT_cu,v-T_gu,v)按列放置，得到一个矩阵T，

根据旋转矩阵R_cg计算得到手眼关系矩阵H_cg的平移向量T_cg：

T_cg＝R_g ^-1T

(3)、对计算出的手眼关系矩阵H_cg进行修正，得到用于多视角点云粗配准的手眼关系矩阵

3.1)、获取J组角点真实坐标和点云坐标之间的对应点集

3.1.1)、用探针装置固定于机器人末端法兰上，通过五点法和示教器建立工具坐标，得到探针末端相对于机器人末端法兰坐标系的位置坐标ΔP₀，机器人带动探针移动至标定板左上角的第一个方格的四个角点正上方处，从示教器上记录此时机器人末端法兰相对于基座坐标系的坐标，不断带动探针移动到标定板的方格的四个角点正上方处，得到角点正上方处机器人末端法兰相对于基座坐标系的坐标

并且推算出标定板所有角点的真实坐标：

其中，

为第l个角点正上方处机器人末端法兰相对于基座坐标系的三维坐标，Δx₀,Δy₀,Δz₀为探针末端相对于机器人末端法兰坐标系的位置坐标，L标定板上角点的数量，T表示转置，B表示机器人基座坐标系；

3.1.2)、在一确定的机器人位姿下拍摄并重建标定板三维点云，重建得到标定板三维点云，依次点击并获取所述标定板左上角的第一个方格的四个角点相对于双目相机坐标系的点云坐标，根据推算可以获得同所述角点真实坐标相对应的所有标定板角点的点云坐标P₁ ^l：

其中，

为点云坐标P₁ ^l的三维坐标表示，C表示双目相机坐标系；

3.1.3)、根据机器人位姿信息，得到拍摄标定板点云时的机器人位姿矩阵为H_g，由已知的角点真实坐标P₀ ^l，得到转换至该机器人姿态的法兰坐标系下的角点坐标

其中，

为角点坐标

的三维坐标表示，G表示机器人末端法兰坐标系；

由此得到一组对应点集

3.1.4)、改变机器人位姿J-1次，重复步骤3.1.2)、3.1.3)，得到第j组对应点集重新记为

这样得到J组角点真实坐标和点云坐标之间的对应点集的集合P；

3.2)、基于最小化欧式变换误差优化手眼关系矩阵H_cg

3.2.1)、以手眼关系矩阵H_cg初始化种群

构建规模为M的种群，并初始化每个个体x^h为：

其中，h表示为个体编号，h＝1,2,…M，0表示第0代种群，下标数字为个体中分量编号，[r_ox,r_oy,r_oz]为手眼关系矩阵H_cg中旋转矩阵R_cg对应的旋转向量，[x_o,y_o,z_o]为手眼关系矩阵H_cg中的平移向量T_cg的转置，即：

其中，Rodrigues()表示旋转向量转旋转矩阵操作；

rand(0,1)表示取0到1之间的随机数操作；

r_xmaxmin,r_ymaxmin,r_zmaxmin,x_maxmin,y_maxmin,z_maxmin分别表示手眼关系矩阵中旋转向量x、y、z以及手眼关系矩阵中平移向量x、y、z的最大值最小值之差；

3.2.2)、个体变异与交叉

对种群中个体进行变异和交叉：

其中，

为第t代种群中第h个个体的第k个分量，Mutation()为变异操作，

为第t代第h个变异个体的第k个分量，Crossover()为交叉操作，

为第t代第h个实验个体的第k个分量，k＝1,2,…,6；

3.2.3)、利用贪婪算法来选择进入下一代种群的个体x^h(t+1)：

其中：

其中，

分别为第t代种群中第h个个体x^h(t)对应的旋转矩阵：

其中，

分别为第t代种群中第h个个体u^h(t)对应的旋转矩阵：

3.2.4)、t＝t+1，返回步骤3.2.2)，直到达到设定的终止条件为止；

3.2.5)、比较M个个体x^h(t+1),h＝1,2,…,M的适应度函数值f(x^h+1(t),P)，将最小适应度函数值对应个体记为x′(t)＝{x′₁(t),x′₂(t),x′₃(t),x′₄(t),x′₅(t),x′₆(t)}，则修正后手眼标定矩阵

为：

R′_cg＝Rodrigues([x′₁(t),x′₂(t),x′₃(t)])，T′_cg＝[x′₄(t),x′₅(t),x′₆(t)]^T

(4)、三维形貌测量

调试所述双目相机，使其能清晰地拍摄到测量对象即需要进行测量的大型航天构件，使得左右相机保持在同一水平位置，并且保留一定距离，并完成双目标定；调试所述机器人，使其能承载双目相机进行三维测量任务，保证拍摄完测量对象的全貌；

在每个测量位姿进行结构光图像拍摄，并重构出测量对象的单元测量点云数据

记录测量位姿并计算得到该测量位姿机器人的位姿矩阵

其中，m为对测量对象进行三维形貌测量的测量位姿数量

根据获得的单元测量点云数据

各视角位姿矩阵

以及步骤(4)得到的优化后的手眼关系矩阵

计算出每幅单元测量点云数据从双目相机坐标系到机器人基座坐标系的刚体变换矩阵：

将双目相机获取的单元测量点云数据转换至机器人基座统一坐标系下，得到配准后的第q个测量位姿下的单视角点云

实现多视角点云数据的配准，从而完成大型航天构件的三维形貌测量。

本发明的发明目的是这样实现的：

为了提高大型航天构件的三维形貌测量的准确性和完整性，本发明用于大型航天构件的机器人自动标定与三维测量方法包括：首先，基于Kronecker积和Moore-Penrose逆对手眼关系方程进行线性求解，在保证更准确的封闭解的同时更有针对性地减少标定过程中系统噪声、计算带来的误差影响，并且采用Rodrigues公式对所求解手眼关系矩阵H_cg的旋转部分进行正交化以消除测量噪声的影响，提高算法运行效率；然后，为了进一步提高手眼标定精度，提升点云配准误差，引入基于最小化欧式变换误差的差分进化(DE)算法应用于手眼标定，以解决由于缺少对旋转矩阵的约束条件导致手眼方程解的不稳定性，将标定板角点点云坐标P₁ ^l和其对应角点真实坐标

之间的刚体变换关系作为构成来源，建立最小化欧式变换误差代价函数，对初始手眼矩阵进行收敛迭代，其旋转部分和平移部分同时进行修正，得到用于多视角点云粗配准的手眼关系矩阵

最后，采集大型航天构件的多幅单元测量点云数据，完成单元测量点云数据到机器人基坐标系的点云配准，从而实现更加快速高效地获得轮廓更贴近真实的整体点云的功能，完成大型航天构件作为待测物的三维形貌测量任务。

附图说明

图1是本发明用于大型航天构件的机器人自动标定与三维测量方法一种具体实施方式的流程图；

图2是标定板角点欧式变换示意图；

图3是角点真实坐标和点云坐标示意图；

图4是手眼标定修正前后结果对比图；

图5是一局部单元测量点云数据示意图；

图6是多幅单元测量点云数据转换至同一坐标系示意图；

图7是一大型航天构件三维形貌测量结果示意图。

具体实施方式

下面结合附图对本发明的具体实施方式进行描述，以便本领域的技术人员更好地理解本发明。需要特别提醒注意的是，在以下的描述中，当已知功能和设计的详细描述也许会淡化本发明的主要内容时，这些描述在这里将被忽略。

图1是本发明用于大型航天构件的机器人自动标定与三维测量方法一种具体实施方式的流程图。

在本实施例中，如图1所示，本发明用于大型航天构件的机器人自动标定与三维测量方法包括：

步骤S1：建立手眼标定方程

将双目相机安装于机器人末端法兰处，控制机器人运动至第i个拍摄位姿，对棋盘格标定板上一角点进行拍摄，在该拍摄位姿获得标定板图片，同时记录机器人位姿信息以及角点在相机坐标系中的位置P_i，这样对n个拍摄位姿进行拍摄，得到n组标定板图片和相对应机器人姿态信息。

根据张氏标定法获得每组标定板图片中标定板相对于相机的旋转向量R_ci和平移向量T_ci，并转换成旋转平移矩阵，即得到相机外参矩阵H_ci,i＝1,2,...,n，根据机器人位姿信息计算相对于基座的旋转向量R_gi和平移向量T_gi，得到机器人位姿矩阵H_gi,i＝1,2,...,n，其中：

由于n组拍摄位姿中任意两次变换拍摄位姿u,v之间，其中u,v＝1,2,...,n，并且u≠v，根据机器人基座到标定板之间的相对位姿关系H_BW固定不变，相机与机器人末端法兰之间的相对位姿关系即手眼关系矩阵H_cg固定不变，结合相机外参矩阵和机器人姿态矩阵之间的坐标转换关系，可以得到以下关系：

H_guH_cgH_cu＝H_BW＝H_BW＝H_gvH_cgH_cv

即：

H_guH_cgH_cu＝H_gvH_cgH_cv

做矩阵变化，即左右两边前后各乘以矩阵

可以得到：

即手眼标定方程：

其中：

其中，R_gu,v表示矩阵

中的旋转矩阵，T_gu,v表示矩阵

中的平移向量，R_cg表示手眼关系矩阵H_cg中的旋转矩阵，T_cg表示手眼关系矩阵中的平移向量，R_cu,v表示矩阵

的旋转矩阵，T_cu,v表示矩阵

中的平移向量。

令

X＝H_cg，则手眼标定方程可以表示为：

AX＝XB

由n组标定数据可以建立

个手眼标定方程。

步骤S2：利用Kronecker积和Moore-Penrose逆将手眼标定方程转化为最小二乘问题并利用奇异值分解计算出手眼关系矩阵H_cg

基于Kronecker积的性质将待解手眼标定方程AX＝XB展开，可得齐次方程：

AX＝XB

即公式一、公式二：

其中，R_cu,v,R_cg,R_gu,v都是旋转矩阵，属于特殊正交群SO₍₃₎，在乘法上是封闭的。

对公式一、二利用矩阵直积的性质将矩阵方程转化为线性代数方程组，再进行求解，可以得到以下两个公式：

于是，可以将以上方程组构造成如下矩阵方程形式：

根据矛盾方程组的性质，该方程组左边的系数矩阵为不可逆矩阵，在求解该矛盾方程组的解时将用到广义逆的思想。所以需要求取方程组的极小范数最小二乘解，则该方程组的极小范数最小二乘解就等于C⁺d。而将旋转矩阵与平移向量进行耦合，将会在计算过程中减少系统噪声、计算带来的误差影响。具体步骤如下。

原矩阵方程转换为线性最小二乘问题：

由SVD分解可以得到

根据矩阵U,V的正交性，可以得到：

再令

代入上式，并且将矩阵展开得到：

其中，σ_i,z_i,u_i为各矩阵的向量部分。因此，原问题可以整理为:

显然，当

时。该问题取得最小值，则最小二乘解和最小值为：

由

可得

因此，通过SVD来求解线性最小二乘解的公式为：

因此，本发明中计算手眼关系矩阵H_cg为：

建立线性方程组：

其中，I为单位矩阵，

表示Kronecker积，vec表示向量化操作；

将所有两次变换拍摄位姿u,v下的矩阵：

按列放置，得到一个矩阵R；

对矩阵R进行奇异值分解，得到U矩阵即左奇异矩阵、奇异值矩阵∑以及V矩阵即右奇异矩阵；

将所有两次变换拍摄位姿u,v下的矩阵：

按列放置，得到一个矩阵T_g；

取列向量V∑^-1U^TT_g的前9个元素还原成3×3矩阵形式，得到矩阵

然后，对矩阵

进行正交化奇异值分解即

其中，U_R为右奇异矩阵，∑_R为奇异值矩阵，V_R为左奇异矩阵，得到手眼关系矩阵H_cg的旋转矩阵R_cg即

将所有两次变换拍摄位姿u,v下的矩阵(R_gu,v-I)按列放置，得到一个矩阵R_g，将所有两次变换拍摄位姿u,v下的矩阵(R_cgT_cu,v-T_gu,v)按列放置，得到一个矩阵T，

根据旋转矩阵R_cg计算得到手眼关系矩阵H_cg的平移向量T_cg：

T_cg＝R_g ^-1T

这样，求得相机坐标系相对于机器人末端法兰坐标系的旋转平移矩阵，即手眼关系矩阵H_cg，并以此进行初始化，得到DE算法第0代种群。

步骤S3：对计算出的手眼关系矩阵H_cg进行修正，得到用于多视角点云粗配准的手眼关系矩阵

步骤S3.1：获取J组角点真实坐标和点云坐标之间的对应点集

步骤S3.1.1：用探针装置固定于机器人末端法兰上，通过五点法和示教器建立工具坐标，得到探针末端相对于机器人末端法兰坐标系的位置坐标ΔP₀，机器人带动探针移动至标定板左上角的第一个方格的四个角点正上方处，从示教器上记录此时机器人末端法兰相对于基座坐标系的坐标，不断带动探针移动到标定板的方格的四个角点正上方处，得到角点正上方处机器人末端法兰相对于基座坐标系的坐标

并且推算出标定板所有角点的真实坐标：

其中，

为第l个角点正上方处机器人末端法兰相对于基座坐标系的三维坐标，Δx₀,Δy₀,Δz₀为探针末端相对于机器人末端法兰坐标系的位置坐标，L标定板上角点的数量，T表示转置，B表示机器人基座坐标系；

步骤S3.1.2：在一确定的机器人位姿下拍摄并重建标定板三维点云，重建得到标定板三维点云，依次点击并获取所述标定板左上角的第一个方格的四个角点相对于双目相机坐标系的点云坐标，根据推算可以获得同所述角点真实坐标相对应的所有标定板角点的点云坐标P₁ ^l：

其中，

为点云坐标P₁ ^l的三维坐标表示，C表示双目相机坐标系；

步骤S3.1.3：根据机器人位姿信息，得到拍摄标定板点云时的机器人位姿矩阵为H_g，由已知的角点真实坐标P₀ ^l，得到转换至该机器人姿态的法兰坐标系下的角点坐标

其中，

为角点坐标

的三维坐标表示，G表示机器人末端法兰坐标系；

由此得到一组对应点集

步骤S3.1.4：集重新记为

这样得到J组角点真实坐标和点云坐标之间的对应点集的集合P；

步骤S3.2：基于最小化欧式变换误差优化手眼关系矩阵H_cg

步骤S3.2.1：以手眼关系矩阵H_cg初始化种群

构建规模为M的种群，并初始化每个个体x^h为：

其中，h表示为个体编号，h＝1,2,…M，0表示第0代种群，下标数字为个体中分量编号，[r_ox,r_oy,r_oz]为手眼关系矩阵H_cg中旋转矩阵R_cg对应的旋转向量，[x_o,y_o,z_o]为手眼关系矩阵H_cg中的平移向量T_cg的转置，即：

其中，Rodrigues()表示旋转向量转旋转矩阵操作；

rand(0,1)表示取0到1之间的随机数操作；

r_xmaxmin,r_ymaxmin,r_zmaxmin,x_maxmin,y_maxmin,z_maxmin分别表示手眼关系矩阵中旋转向量x、y、z以及手眼关系矩阵中平移向量x、y、z的最大值最小值之差；

步骤S3.2.2：个体变异与交叉

对种群中个体进行变异和交叉：

其中，

为第t代种群中第h个个体的第k个分量，Mutation()为变异操作，

为第t代第h个变异个体的第k个分量，Crossover()为交叉操作，

为第t代第h个实验个体的第k个分量，k＝1,2,…,6；

步骤S3.2.3：利用贪婪算法来选择进入下一代种群的个体x^h(t+1)：

其中：

其中，

分别为第t代种群中第h个个体x^h(t)对应的旋转矩阵：

其中，

分别为第t代种群中第h个个体u^h(t)对应的旋转矩阵：

步骤S3.2.4：t＝t+1，返回步骤S3.2.2，直到达到设定的终止条件为止；

步骤S3.2.5：比较M个个体x^h(t+1),h＝1,2,…,M的适应度函数值f(x^h+1(t),P)，将最小适应度函数值对应个体记为x′(t)＝{x′₁(t),x′₂(t),x′₃(t),x′₄(t),x′₅(t),x′₆(t)}，则修正后手眼标定矩阵

为：

R′_cg＝Rodrigues([x′₁(t),x′₂(t),x′₃(t)])，T′_cg＝[x′₄(t),x′₅(t),x′₆(t)]^T

步骤S4：三维形貌测量

调试所述双目相机，使其能清晰地拍摄到测量对象即需要进行测量的大型复杂零部件，使得左右相机保持在同一水平位置，并且保留一定距离，并完成双目标定；调试所述机器人，使其能承载双目相机进行三维测量任务，保证拍摄完测量对象的全貌；

在每个测量位姿进行结构光图像拍摄，并重构出测量对象的单元测量点云数据

记录测量位姿并计算得到该测量位姿机器人的位姿矩阵

其中，m为对测量对象进行三维形貌测量的测量位姿数量

根据获得的单元点云数据

各视角位姿矩阵

以及步骤(4)得到的优化后的手眼关系矩阵

计算出每幅单元点云数据从双目相机坐标系到机器人基座坐标系的刚体变换矩阵：

将双目相机获取的单元测量点云数据转换至机器人基座统一坐标系下，得到配准后的第q个测量位姿下的单视角点云

实现多视角点云数据的配准，从而完成大型复杂零部件的三维形貌测量。

通过控制机器人运动承载双目相机在多个视角位姿下采集待测物零部件的单元测量点云信息；基于优化后手眼矩阵将多幅点云转换至同一坐标系下，完成多视角点云配准功能，实现待测物三维形貌测量，以提高大型复杂零部件的三维形貌测量效率。

实施例

在实施例中，以某一大型航天构件作为试件进行三维形貌测量，该实施例获得了试件除底部以外的三维点云数据，实现了还原试件的形貌特征。

实施例中所使用的试件样本为1600mm×1525mm矩形板，所用标定板为8×11，15mm标准的黑白棋盘标定板，如图2所示。

在本实施例中，首先控制机器人带动双目相机，在不同的拍摄位置采集了9幅像素分辨率为3000×4096的标定板图像，再通过张氏标定法解算出拍摄9幅标定板图片的相机外参矩阵的齐次表达式H_c；并记录相对应拍摄位置的机器人姿态信息，同样计算得到其机器人姿态矩阵的其次表达式H_g。

然后对于待解手眼方程利用Kronecker积和Moore-Penrose逆的性质将手眼方程转化为线性代数方程组，再通过最小二乘法求解出相机坐标系相对于机器人末端法兰坐标系的旋转平移矩阵即手眼关系矩阵H_cg。

根据手眼标定处置结果产生差分进化(DE)算法的初始种群；在本实例中，初始种群为：

在本实施例中，M＝200，

获取J组角点真实坐标和点云坐标之间的对应点集，如图3所示

修正后手眼标定矩阵

为：

如图4所示，手眼关系矩阵H_cg经过DE算法进行修正之后，同样地，反算出对应标定板原点在机器人基坐标系下地位置。对比优化前后n个拍摄位姿下的标定板原点分布，可以看出优化后的标定板原点(实心原点)较为集中，基本处于重叠状态；而优化前的标定板原点(空心原点)则较为分散。该结果证明了手眼标定结果的改善，其效果将在形貌测量中得到验证。

对于本实施例中进行测量的试件，需要从多个位姿进行单元测量点云数据采集。在采集过程中，保证待测试件被拍摄范围完整。通过移动机器人带动双目相机在多个视角位姿下完成测量对象的左右相机的结构光图像采集，重建单元测量点云数据，

如图5所示，圈中部分为一单元测量点云重建结果。并记录各个视角的机器人位姿信息，得到位姿矩阵

根据所获得的单元点云数据

和各视角位姿矩阵

以及修正后的手眼关系矩阵

进而计算出每幅单元测量点云数据从双目相机坐标系到机器人基座坐标系的刚体变换矩阵

将双目相机获取的单元测量点云转换至统一坐标系下，如图6所示为多幅单元测量点云数据转换至同一坐标系示意图，可以看出多幅单元测量点云数据均实现刚体变换，整体点云在变换过程中保持原状，无畸变情况产生。

本实例中，大型航天构件作为待测试件的三维形貌测量结果如图7所示。从图7中可以看出，以该试件的三维形貌测量结果完整，多个单元测量点云数据之间的配准情况良好。

尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术邻域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术邻域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

Claims (1)

1.一种用于大型航天构件的机器人自动标定与三维测量方法，其特征在于，包括：

(1)、建立手眼标定方程

将双目相机安装于机器人末端法兰处，控制机器人运动至第i个拍摄位姿，对棋盘格标定板上一角点进行拍摄，在该拍摄位姿获得标定板图片，同时记录机器人位姿信息以及角点在相机坐标系中的位置P_i，这样对n个拍摄位姿进行拍摄，得到n组标定板图片和相对应机器人姿态信息；

根据张氏标定法获得每组标定板图片中标定板相对于相机的旋转向量R_ci和平移向量T_ci，并转换成旋转平移矩阵，即得到相机外参矩阵H_ci,i＝1,2,...,n，根据机器人位姿信息计算相对于基座的旋转向量R_gi和平移向量T_gi，得到机器人位姿矩阵H_gi,i＝1,2,...,n，其中：

对于任意两次变换拍摄位姿u,v之间相机外参矩阵、机器人位姿矩阵，建立手眼标定方程：

其中：

其中，R_gu,v表示矩阵

中的旋转矩阵，T_gu,v表示矩阵

中的平移向量，R_cg表示手眼关系矩阵H_cg中的旋转矩阵，T_cg表示手眼关系矩阵中的平移向量，R_cu,v表示矩阵

的旋转矩阵，T_cu,v表示矩阵

中的平移向量；

(2)、利用Kronecker积和Moore-Penrose逆将手眼标定方程转化为最小二乘问题并利用奇异值分解计算出手眼关系矩阵H_cg

建立线性方程组：

其中，I为单位矩阵，

表示Kronecker积，vec表示向量化操作；

将所有两次变换拍摄位姿u,v下的矩阵：

按列放置，得到一个矩阵R；

对矩阵R进行奇异值分解，得到U矩阵即左奇异矩阵、奇异值矩阵∑以及V矩阵即右奇异矩阵；

将所有两次变换拍摄位姿u,v下的矩阵：

按列放置，得到一个矩阵T_g；

取列向量V∑^-1U^TT_g的前9个元素还原成3×3矩阵形式，得到矩阵

然后，对矩阵

进行正交化奇异值分解即

其中，U_R为右奇异矩阵，∑_R为奇异值矩阵，V_R为左奇异矩阵，得到手眼关系矩阵H_cg的旋转矩阵R_cg即

将所有两次变换拍摄位姿u,v下的矩阵(R_gu,v-I)按列放置，得到一个矩阵R_g，将所有两次变换拍摄位姿u,v下的矩阵(R_cgT_cu,v-T_gu,v)按列放置，得到一个矩阵T，

根据旋转矩阵R_cg计算得到手眼关系矩阵H_cg的平移向量T_cg：

T_cg＝R_g ^-1T

(3)、对计算出的手眼关系矩阵H_cg进行修正，得到用于多视角点云粗配准的手眼关系矩阵

3.1)、获取J组角点真实坐标和点云坐标之间的对应点集

3.1.1)、用探针装置固定于机器人末端法兰上，通过五点法和示教器建立工具坐标，得到探针末端相对于机器人末端法兰坐标系的位置坐标ΔP₀，机器人带动探针移动至标定板左上角的第一个方格的四个角点正上方处，从示教器上记录此时机器人末端法兰相对于基座坐标系的坐标，不断带动探针移动到标定板的方格的四个角点正上方处，得到角点正上方处机器人末端法兰相对于基座坐标系的坐标

并且推算出标定板所有角点的真实坐标：

其中，

为第l个角点正上方处机器人末端法兰相对于基座坐标系的三维坐标，Δx₀,Δy₀,Δz₀为探针末端相对于机器人末端法兰坐标系的位置坐标，L标定板上角点的数量，T表示转置，B表示机器人基座坐标系；

3.1.2)、在一确定的机器人位姿下拍摄并重建标定板三维点云，重建得到标定板三维点云，依次点击并获取所述标定板左上角的第一个方格的四个角点相对于双目相机坐标系的点云坐标，根据推算可以获得同所述角点真实坐标相对应的所有标定板角点的点云坐标

其中，

为点云坐标

的三维坐标表示，C表示双目相机坐标系；

3.1.3)、根据机器人位姿信息，得到拍摄标定板点云时的机器人位姿矩阵为H_g，由已知的角点真实坐标

得到转换至该机器人姿态的法兰坐标系下的角点坐标

其中，

为角点坐标

的三维坐标表示，G表示机器人末端法兰坐标系；

由此得到一组对应点集

3.1.4)、改变机器人位姿J-1次，重复步骤3.1.2)、3.1.3)，得到第j组对应点集重新记为

这样得到J组角点真实坐标和点云坐标之间的对应点集的集合P；

3.2)、基于最小化欧式变换误差优化手眼关系矩阵H_cg

3.2.1)、以手眼关系矩阵H_cg初始化种群

构建规模为M的种群，并初始化每个个体x^h为：

其中，h表示为个体编号，h＝1,2,…M，0表示第0代种群，下标数字为个体中分量编号，[r_ox,r_oy,r_oz]为手眼关系矩阵H_cg中旋转矩阵R_cg对应的旋转向量，[x_o,y_o,z_o]为手眼关系矩阵H_cg中的平移向量T_cg的转置，即：

其中，Rodrigues()表示旋转向量转旋转矩阵操作；

rand(0,1)表示取0到1之间的随机数操作；

r_xmaxmin,r_ymaxmin,r_zmaxmin,x_maxmin,y_maxmin,z_maxmin分别表示手眼关系矩阵中旋转向量x、y、z以及手眼关系矩阵中平移向量x、y、z的最大值最小值之差；

3.2.2)、个体变异与交叉

对种群中个体进行变异和交叉：

其中，

为第t代种群中第h个个体的第k个分量，Mutation()为变异操作，

为第t代第h个变异个体的第k个分量，Crossover()为交叉操作，

为第t代第h个实验个体的第k个分量，k＝1,2,…,6；

3.2.3)、利用贪婪算法来选择进入下一代种群的个体x^h(t+1)：

其中：

其中，

分别为第t代种群中第h个个体x^h(t)对应的旋转矩阵：

其中，

分别为第t代种群中第h个个体u^h(t)对应的旋转矩阵：

3.2.4)、t＝t+1，返回步骤3.2.2)，直到达到设定的终止条件为止；

3.2.5)、比较M个个体x^h(t+1),h＝1,2,…,M的适应度函数值f(x^h+1(t),P)，将最小适应度函数值对应个体记为x′(t)＝{x′₁(t),x′₂(t),x′₃(t),x′₄(t),x′₅(t),x′₆(t)}，则修正后手眼标定矩阵

为：

(4)、三维形貌测量

调试所述双目相机，使其能清晰地拍摄到测量对象即需要进行测量的大型航天构件，使得左右相机保持在同一水平位置，并且保留一定距离，并完成双目标定；调试所述机器人，使其能承载双目相机进行三维测量任务，保证拍摄完测量对象的全貌；

在每个测量位姿进行结构光图像拍摄，并重构出测量对象的单元测量点云数据

记录测量位姿并计算得到该测量位姿机器人的位姿矩阵

其中，m为对测量对象进行三维形貌测量的测量位姿数量

根据获得的单元测量点云数据

各视角位姿矩阵

以及步骤(4)得到的优化后的手眼关系矩阵

计算出每幅单元测量点云数据从双目相机坐标系到机器人基座坐标系的刚体变换矩阵：

将双目相机获取的单元测量点云数据转换至机器人基座统一坐标系下，得到配准后的第q个测量位姿下的单视角点云

实现多视角点云数据的配准，从而完成大型航天构件的三维形貌测量。

Images