基于摄影测量的移动机器人末端标定方法研究*

王逸峰，逯永康，李俊卿，张 洋，刘 巍，王福吉

（大连理工大学，大连 116024）

大型飞机机身、火箭壳体、航天器舱体、风机叶片等大型构件是工程装备的重要组成部分，此类构件往往具有尺寸大、结构复杂、精度要求高等特点[1–3]。随着装备制造需求的进一步提高，现有的数控机床加工能力难以适用于此类构件的加工与装配。移动加工与装配机器人以其加工范围可拓展、高效率、柔性加工等优势，被逐渐应用于此类构件制造[4]。大型复杂构件制造过程中，移动机器人末端的高鲁棒性准确定位是保障制造精度与效率的基础。因此，如何在大尺寸复杂现场环境下对移动机器人末端进行高效率、高鲁棒性准确定位，已成为国内外学者的研究热点。

常用的机器人末端定位技术有激光跟踪定位、双目视觉在线定位和室内GPS定位等。其中，激光跟踪定位是通过跟踪测量机器人末端合作靶标以解算末端空间位姿[5]，但该方法存在易受现场复杂结构遮挡及测量效率低等问题。双目视觉在线定位方法是将视觉系统与机器人刚性连接并标定[6]，采用三角测量原理获取被测目标三维坐标求解末端位置，多用于局部小范围测量，易受振动干扰且对测量角度有较高的要求。室内GPS定位模式是利用室内激光发射装置与接收装置进行测量[7]，10 m范围内测量精度达0.125 mm，测量现场布局复杂且精度有限。除上述测量模式以外，近年来摄影测量系统以测量范围大、精度高（4 μm+4 μm/m）、鲁棒性高等优势[8]，被逐渐应用于航空航天、能源交通等领域的大型构件测量，只需相机在不同位置获取被测目标图像信息即可实现坐标与位姿解算。

目前，李文龙等[9]对移动机器人末端定位方法开展了广泛的研究。任永杰等[10]将提升机器人精度的方法分为标定法与外部定位法两类，其中标定法无法克服机器人连续加工中的定位漂移问题；外部定位法通过高精度外部观测设备对机器人末端进行定位，因此更具优越性。Moeller等[11]采用移动激光跟踪仪对机器人末端进行实时测量定位，在800 mm×800 mm×800 mm范围内定位精度为0.2 mm。Wang等[12]建立了基于深度置信网络的机器人理论位姿误差与实际位姿误差的映射模型，采用双目视觉对移动机器人进行末端定位，其定位精度为0.29 mm，能够满足机器人自动钻铆系统的要求。上述方法通过眼在手外的方式对机器人末端进行定位，大连理工大学课题组曾利用双目视觉实现机器人的随动测量[13]，但设备体积较大，难以适应位姿变换复杂的场景。

上述研究为本文提供了良好的借鉴思路，但难以适用于测量范围大、现场易遮挡及位姿变化等复杂制造工况。因此，本文提出一种利用单目相机实现复杂工况下机器人末端在位标定方法，以解决大型构件机器人加工中的定位难题。该方法通过空间后方交会、多坐标系基准转换实现移动机器人位姿求解。

1 机器人末端定位总体方案

本文考虑到机器人结构参数、测量范围和工况特点等，提出了离线全局标定–在线定位点摄影–位姿反算的机器人末端定位方法。其中，离线全局标定包括分布于待测区域的定位点标定与单目相机–机器人末端相对位置的标定，定位点标定通过单目摄影相机多摄影站点测量与相片拼接求解定位点坐标的方法实现；单目相机与机器人末端相对位置的标定是通过多坐标系基准转换方法实现，然后利用摄影测量方法实现在线定位点的像点坐标提取，基于空间后方交会原理求解相机姿态，最后基于相机与机器人末端的标定参数反求机器人末端位姿。设计了基于摄影测量的移动机器人末端定位方案，该方案主要由机器人、摄影测量相机、激光跟踪仪、公共靶标点和摄影测量合作靶标等组成，如图1所示。测量区域上贴有公共靶标点与摄影测量合作靶标，其中摄影测量合作靶标用于相机在多个不同位置拍摄数字图像并提取的同名点；公共靶标点是摄影测量与激光跟踪仪的合作靶点，经过替换后能够分别被摄影测量相机与激光跟踪仪所测量，二者统称为定位点。摄影测量相机固连在机器人末端，末端上的控制点装有激光跟踪仪反射靶标，能够被激光跟踪仪所测量。摄影测量相机被固定在末端后，与末端控制点之间形成刚性连接，因此标定摄影测量相机与末端控制点的相对位置关系后，求得摄影测量相机的位姿即可反求机器人末端空间位姿。

图1 机器人末端定位示意图Fig.1 Schematic diagram of robot end positioning

为了适应复杂加工场景下机器人末端定位需求与现场约束，本文采用“多测量坐标系基准转换–相机空间位姿解算–相机与末端控制点位置标定–末端空间位姿求解”等关键步骤实现末端高鲁棒性准确定位，关键步骤如图2所示。首先基于多测量坐标系基准转换的方法标定摄影测量相机与机器人末端控制点的位置关系；机器人末端移动后，由于摄影测量靶点坐标已知，基于空间后方交会原理求解相机空间位姿；再利用相机–机器人末端标定参数求解机器人末端位姿；最后判断机器人末端是否达到预定位置，姿态角度是否正确，刀具是否对准，若否则需要根据测量所得机器人末端位姿进行调整并重新定位。

图2 机器人末端定位流程图Fig.2 Positioning flowchart of robot end

多源测量数据基准的有效传递是机器人位姿准确解算的关键，末端定位关键坐标系及转换关系如图3所示。定义如下：全局坐标系{W}、相机坐标系{P}、末端控制点坐标系{M}、物方空间坐标系{C}。

图3 多坐标系基准转换示意图Fig.3 Schematic diagram of multi-coordinate system datum conversion

激光跟踪仪测量坐标系作为所有测量数据的统一基准，以跟踪头中心为原点，视为全局坐标系。物方空间坐标系是摄影测量所拍摄定位点的坐标系。末端控制点坐标系是由至少3个控制点建立的一个局部坐标系。任意一个坐标系{G1}相对于另一个坐标系{G2}的空间位姿采用矢量Pst=（x，y，z，ω，φ，κ）表示，其中矢量T=（x，y，z）T用于表示坐标系{G1}原点在坐标系{G2}中的位置= （fω，φ，κ）用于表示坐标系{G1}相对于坐标系{G2}的欧拉转角。空间点在{G1}坐标系下的坐标G1i=（X1i，Y1i，Z1i）T与在{G2}坐标系下的坐标G2i=（X2i，Y2i，Z2i）T存在如下转换关系：

2 多坐标系标定与基准转换

2.1 多测量坐标系基准转换方法

摄影测量获取的定位点坐标与相机位姿均在物方空间坐标系下，须将其转换至全局坐标系下参与位姿求解。本文利用多个公共点，采用奇异值分解的坐标转换方法实现摄影测量坐标系与全局坐标系的统一[14]。由于摄影测量光束法平差优化算法的随机性，物方空间坐标系的原点坐标会发生变动，但被测点与点之间的几何距离关系并未改变，此时仍需采用上述坐标转换方法将测点三维坐标统一至原始物方坐标系。

为了清楚表达不同坐标系的转换关系，假设两个测量坐标系内公共点集合分别为P与Q，各个测点在各自测量坐标系下的坐标分别为pi和qi。其转换关系可表示为pi= Rqi+ T + ε。其中，R为旋转矩阵；T为平移矩阵；ε为误差向量。基于最小二乘原理，求解R与T，需满足式（3）：

利用质心位移向量，求解矩阵H并对其进行奇异值分解：

式中，U为左奇异矩阵；Σ为奇异值矩阵，在主对角线上有值，其他元素为0；V为右奇异矩阵。基于左奇异矩阵U与右奇异矩阵V，计算旋转矩阵R。

最后计算位移矩阵T：

2.2 摄影相机空间位姿解算

摄影测量每个站位的相片中含有部分被测点[15]，根据相机透视原理建立共线方程式，通过光束法平差求解被测点空间坐标和相机站位的内外参数。

摄影相机的投影成像模型是小孔成像模型，被测点经投影中心于像平面成像，共线方程式为

式中，x、y为像点坐标；x0、y0为相机主点坐标；Δx、Δy为像点坐标偏差；f为相机主距；X、Y、Z为定位点坐标；XS、YS、ZS为投影中心坐标；a、b、c为与ω、φ、κ有关的三角函数。

后方交会的原理如图4所示，每一个定位点与相机投影中心的连线作为一条光束。定位点的坐标已知，在相机内参数经过标定的情况下，求解相机的空间位姿。将共线方程式线性化，得到后方交会误差方程式：

图4 摄影测量后方交会原理图Fig.4 Principle diagram of photogrammetry resection

式中，υx、υy为像点坐标的误差；为相机投影中心坐标的偏差；、、为相机角度的偏差为用于近似计算的像点坐标。需要代入初值才能解算出相机的空间位姿，否则极易陷入局部最优解，因此常采用角锥法、直接线性变换法等进行解算。

2.3 摄影相机机器人末端控制点位置标定

标定的目的是求解相机坐标系与机器人末端控制点坐标系之间的相对位置关系。首先根据后方交会原理，计算出相机坐标系与物方空间坐标系之间的转换参数，因此求得RPC与TPC，将相机测得的公共靶标点坐标转移至相机坐标系下；然后由激光跟踪仪测得末端控制点在全局坐标系下的坐标，以其中一个控制点为原点，该点与另一个控制点连线为轴，建立局部控制点坐标系。根据控制点在控制点坐标系下的坐标与在全局坐标系下的坐标，基于奇异值分解的方法求解坐标系间的转换参数RMW与TMW。根据公共靶标点分别在相机坐标系与全局坐标系下的坐标，求解相机坐标系相对于全局坐标系的旋转矩阵RWP与平移矩阵TWP。相机坐标系—全局坐标系—控制点坐标系构成坐标转换的链条，通过公式（11）计算相机坐标系与控制点坐标系之间的旋转矩阵RMP与平移矩阵TMP，由此建立相机坐标系与机器人末端控制点坐标系之间的相对位置关系。

2.4 机器人末端空间位姿解算

基于摄影相机与末端控制点之间的标定结果，建立摄影相机与末端控制点之间的刚性转换关系。当移动机器人移动到另一个位置后拍摄一张像片，基于后方交会解算出相机位姿；然后根据公共靶标点转换到激光跟踪仪全局坐标系；最后反求出末端位姿。假定在某一位置，全局坐标系相对于相机坐标系的转换参数为RWP1与TWP1，则全局坐标系与控制点坐标系的基准转换表达式如下：

然后，根据刚体位姿转换原理，求得控制点坐标系与全局坐标系的转换矩阵参数RWM1与TWM1为

式中，TWM1中3个参数代表末端控制点坐标系原点在全局坐标系下的坐标，即机器人末端在全局坐标系下的位置（XM1，YM1，ZM1）T；RWM1为旋转矩阵，由RWM1求得机器人末端的3个转角（ωM1，φM1，κM1）。通过上述计算，最终求解出移动机器人末端空间位姿六维参数。

3 试验与精度评价

3.1 相机–机器人末端标定

实验室内开展了移动机器人末端模拟定位测量试验，试验系统主要由Leica AT960激光跟踪仪（15 μm+ 6 μm/m）、CIM–3工业摄影相机（4 μm+4 μm/m）等组成，如图5所示。末端控制点与相机固连并可被激光跟踪仪测得，定位点分布于1000 mm×1000 mm的平面内。

图5 机器人末端定位试验平台Fig.5 Positioning experiment platform of robot end

首先采用激光跟踪仪测量部分定位点三维坐标，建立全局坐标系；然后摄影测量相机在4 m×4 m空间范围、测量物距1~4 m内，拍摄30余张照片，剔除拍摄点数少于6的无效照片，求解出定位点在物方空间坐标系下的坐标；最后将相机固定，此时相机的位置记为PL1，使用激光跟踪仪测量末端3个控制点在全局坐标系下的坐标，建立局部控制点坐标系并解算全局坐标系相对于控制点坐标系的转换参数RMW与TMW：

相机在PL1处拍摄一张照片，对相片进行二值化处理，提取像点中心坐标并识别编码信息，利用编码信息进行像点与物方点的匹配，基于后方交会原理求解相机在物方空间系下的坐标，求得相机坐标系相对于全局坐标系的转换参数RWP与TWP：

对相机与机器人末端转换参数进行标定，参数RMP与TMP通过式（11）计算，完成相机与机器人末端参数标定。

3.2 定位精度评价

评价数字工业摄影测量精度的指标主要有内符合精度与外符合精度，其中外符合精度通过更高精度等级的测量方法得到，如与激光跟踪仪测量数据对比；内符合精度也称为重复测量精度，其计算方式与均方根误差RMSE的计算方式一致，对n组数据（e1，e2，…，en）计算其平均数为，有

鉴于激光跟踪仪单点测量精度可达（15 μm+6 μm/m），其测量值可视为参考值，摄影测量测得的末端空间位姿作为测量值。模拟机械臂末端的移动，相机在1.2 m空间范围内进行移动，移动过程中物距保持在1 m内。移动相机至PL2处，用激光跟踪仪测量末端控制点坐标用于后续计算定位误差；相机在PL2处重复拍摄10次，进行数据处理与分析。

取第1张相片计算定位精度，激光跟踪仪测得的末端位姿作为参考值，摄影测量测得的末端位姿作为测量值。重复标定末端位姿测量过程两次，定位精度如表1所示。

对机器人末端空间位姿测量结果进行重复性精度验证，重复测量10次得到机器人末端的重复定位精度，如图6所示。

由表1和图6（a）可知，机器人末端三维各向位置精度优于0.19 mm，其中Y轴定位精度较低是由于摄影测量相机在景深方向误差较大引起的；由表1和图6（b）可知，姿态角定位精度优于0.011°；由式（17）和图6（a）计算得到位置重复测量精度为0.02 mm。后方交会误差和相机标定误差是影响机器人末端定位精度的敏感因素，仍需深入探索后方交会误差与相机测量角度、测量距离的关联关系。

图6 机器人末端的定位精度Fig.6 Positioning accuracy of mobile robot end

表1 机器人末端定位参数Table 1 Positioning parameters of robot end

4 结论

针对机器人末端定位过程存在测量范围大、视线易遮挡、动态位姿复杂变化等问题，本文利用摄影测量离线获取全局坐标系下定位点坐标，然后基于后方交会求解相机空间位姿并标定相机与机器人末端位置关系，最后通过机器人末端相机的在位测量获取移动机器人末端姿态参数。试验结果表明该方法在物距为1000 mm时，机器人末端三维各向位置精度优于0.19 mm，姿态角精度优于0.011°，对大型构件加工场景下的机器人定位具有参考意义。