基于激光跟踪的机器人三维空间定位方法

贺 骥 胥 健 郑 斌 桂仲成

（上海圭目机器人有限公司，上海 200437）

0 引言

目前，机器人在室内常以激光雷达、机器视觉摄像头作为主要传感器，基于SLAM 算法进行定位导航[1]。在室外则常利用GPS 卫星信号，结合编码器里程计和陀螺仪惯导进行定位导航。无论室内还是室外都存在一些特殊情况和场景，导致常见的定位传感器失效（例如，过于空旷或单调的环境会导致激光雷达无法找到足够的参照物；隧道、桥底以及地下室等场景存在没有GPS 信号或GPS 信号微弱等问题），在这种情况下，传统的机器人定位方式无法正常工作，因此需要一种新的基于激光跟踪的机器人三维空间定位方法[2]。

1 工程概况

重庆苏家沟特大桥是重庆沿江高速主城至涪陵段项目的重点工程，全长1 208 m，是全线最长的桥梁，桥体采用整体式30 m×40 m 预制预应力T 型梁搭建。在全桥31 个墩台中，高度在40 m 以上的高墩12个，高差较大，地形条件复杂。

重庆苏家沟特大桥的检测工作采用负压吸附爬壁机器人来对桥墩桥塔进行表观图像采集和病害识别，需要对机器人进行精确定位。在桥梁上GPS 信号很差或者没有GPS 信号，桥墩桥塔表面空旷，特征不明显，颜色单调，激光雷达或机器视觉也没有足够的标识来定位。基于上述背景，该文在机器人背部固定位置安装反射棱镜，让全站仪自动跟踪这个棱镜，通过该文提出的标定方法得到的坐标系转换关系即可实时获得机器人在工作坐标系的坐标，从而精确地对爬壁机器人在桥墩桥塔的位置进行定位。

2 基于激光跟踪的机器人三维空间定位方法

2.1 全站仪坐标系和机器人工作坐标系的构建

将全站仪安装在合适位置，一般放置在三脚架上，然后调水平。全站仪输出的是三维极坐标（d，ha，va），坐标原点在全站仪本体正中央，d为被测点距离原点的距离，ha为全站仪沿垂直地平面的轴旋转的角度（偏航角），va为全站仪的俯仰角度。建立1 个三维直角坐标系作为全站仪坐标系，这个坐标系原点依然在全站仪本体正中央，垂直地平面向上作为z轴正方向，在平行于地平面并通过原点的平面上设定任意一个方向作为y轴正方向，则在该平面上垂直y轴向右的方向是x轴正方向，x轴、y轴和z轴均满足左手定则。

如图1 所示，假设被测点的极坐标为（dt，hat，vat），其对应在全站仪直角坐标系中的坐标为（xt，yt，zt），那么（xt，yt，zt）的计算关系式如公式（1）～公式（3）所示[3]。

图1 被测点极坐标与全站仪坐标之间的换算关系

选取机器人的工作区域，在工作区域建立工作坐标系，机器人的工作区域不一定是地面，也可以是任意表面，例如竖直墙面、斜坡面、隧道拱顶以及桥梁表面等，要在这些区域对机器人进行定位，就需要先建立工作坐标系。工作坐标系的选取需要分别确定原点、x轴和y轴，x轴、y轴可以确定1 个平面，z轴垂直这个平面。如果机器人在平面中运动，那么可以将工作坐标系简化为平面坐标系，z轴坐标恒为0。如果有条件，就可以给工作坐标系的原点、x轴和y轴做上标记，方便全站仪采点[4]。

2.2 基于激光跟踪的机器人三维空间坐标计算

图2 被测点极坐标与全站仪坐标之间的换算关系

此时，点P在2 个坐标系中的坐标也已获取。

此时，点Q在2 个坐标系中的坐标也已获取。

至此，已经有3 组不在同一直线上的点坐标，理论上已经可以计算全站仪坐标系A 和工作坐标系B 的转换矩阵，不过为了使拟合计算的结果更精确，还可以继续采集更多的点坐标，此处假设采集更多的点。

这样，就获得了点R在2 个坐标系中的坐标。需要指出的是，这里为了方便说明，默认将点P取在工作坐标系B 的x轴正半轴上，点Q取在工作坐标系B 的y轴正半轴上，点R取在工作坐标系B 中z轴坐标为正的区域，即dx、dy和dz均大于0，实际上也可以将点P或点Q取到工作坐标系B 的坐标轴负半轴，点R取到z轴坐标为负的区域，那么就需要根据情况改变dx、dy和dz的符号。至此，已经获得了4 组点坐标，如果想获得更精确的转换矩阵，还可以继续采点，重复上述采集点R的步骤。

假设现在已经采集了n个点（n≥3），每个点对应2组坐标，分别是全站仪坐标系A 和工作坐标系B 中的坐标。该文采用一个成熟的SVD 刚体变换方法求解坐标系A到坐标系B 的转换矩阵，简要说明如下。

令W=qB·（qA）T，W是一个3×3 的矩阵，可以证明，只要这n组点中有任意3 点不在一条直线上，则W满秩。当W满秩时，可对W进行SVD 分解，得到W=UΣVS（U为n阶酉矩阵；Σ为n×d阶非负实数对角矩阵；V为右奇异矩阵；S为旋转矩阵），那么旋转矩阵S=U·VE；求解平移向量t=pB-SpA，S和t合成变换矩阵R，如公式（11）所示。

其中，z=（0，0，0）。根据全站仪坐标系中的坐标与工作坐标系中的坐标得出坐标转换矩阵。在机器人表面安装反射棱镜，全站仪通过激光自动跟踪反射棱镜，随着机器人的移动，全站仪输出机器人在全站仪坐标系下的坐标。根据坐标转换矩阵得出机器人在工作坐标系下的坐标，完成机器人的实时定位工作。

3 基于激光跟踪的机器人三维空间定位方法应用效果

为了验证基于跟踪的机器人三维空间定位方法的应用效果，该文结合重庆苏家沟特大桥检测项目，采用负压吸附爬壁机器人对桥墩桥塔进行表观图像采集和病害定位，在机器人背部固定位置安装反射棱镜，让全站仪自动跟踪该棱镜，通过该文提出的标定方法得到的坐标系转换关系即可实时获得机器人在工作坐标系的坐标，选取20 个监测点对爬壁机器人在桥墩桥塔的位置进行精确定位，并与桥梁的工程测绘结果进行对比，结果见表1。由表1 可知，两者的平面坐标误差最大值为4.27 cm，平面坐标误差最小值为0.62 cm，两者的高程误差最大值为4.14 cm，高程误差最小值为0.71 cm。

表1 激光跟踪的机器人定位实测结果与桥梁工程测绘结果对比

4 结语

该文以重庆苏家沟特大桥为例，采用理论分析的方法计算机器人在工作坐标系的坐标，将激光跟踪的机器人定位实测结果与桥梁工程测绘结果进行对比，研究分析方法的可行性，得到以下2 个结论：1）在机器人表面安装反射棱镜，全站仪通过激光自动跟踪反射棱镜，随着机器人的移动，全站仪输出机器人在全站仪坐标系下的坐标，根据提出的标定方法可以得到坐标系转换关系，即可实时获得机器人在工作坐标系的坐标。2）采用激光跟踪的机器人定位实测结果与桥梁工程测绘结果的对比表明，两者的平面坐标误差最大值为4.27 cm，平面坐标误差最小值为0.62 cm，两者的高程误差最大值为4.14 cm，高程误差最小值为0.71 cm。