自行走式地下掘进机器人姿态测量系统的设计*

陈双叶，牛经龙，杨汝军

(北京工业大学电子信息与控制工程学院，北京100124)

自行走式地下掘进机器人主要应用于城市隧(管)道的挖掘，采用自增阻四段式结构［1］，，具有急曲线施工能力及不依赖管壁提供掘进推进反力的技术特点，是一种创新发展的新机型。在其掘进过程中受到各种力的作用和一些不定性因素影响，这会与设计轴线出现偏差。为了将掘进路线与隧道设计曲线的误差控制在一定的范围内，需要及时的获得机器人的位置和姿态，此时就必须通过导向系统测量机器人在绝对空间中的坐标，具体来说就是获得机器人各段首尾的中心坐标，计算掘进机器人轴线与隧道设计轴线的位置偏差(水平方向和垂直方向)和角度偏差(水平偏航角和俯仰角)［2－4］，随后反馈给位姿控制系统做出控制来减小偏差。

对于掘进中的机器人，要实时的获取空间位置和姿态难度较大。主要原因有机器人的动态性、测量标志设置困难、测量环境恶劣等。目前测量方法主要分为人工测量和自动测量两类。人工测量工作量大、人力投入多、测量频率低无法实时获取机器人姿态。但由于其实现方式有多种、成本较低和操作简单，国内施工中仍被采用。自动导向测量系统主要有德国的VMT，日本Enzan公司的ROBOTEC和日本的GYRO系统［5］，等。前两者为全站仪激光导航，后者为陀螺仪惯性导航。德国VMT激光导向系统功能齐全、精度高，但只能与德国海瑞克公司的盾构配套使用而无法移植，并且操作复杂，无法大规模采用。对于日本ROBOTEC，采用三个反射棱镜与一个自动全站仪测量，通过测量三个棱镜的坐标计算机器人位姿。但在同一时刻全站仪只能检测一个点的坐标，而测量动态行进的机器人姿态需要检测同一时刻离散分布的若干点的空间坐标，所以它更适合掘进机器人静态位姿检测。对于日本GYRO惯性陀螺仪导航系统，定位误差会随时间而增大，不适用于长距离长时间的测量，需要定期的进行校正［6－7］，在施工中陀螺仪仅作为辅助参考。为了使自行走式地下掘进机完成地下自动控制掘进行走，对其地下姿态的定位提出高的要求，高精度，智能化和开放化。基于此，本文设计出一种新的姿态定位系统，可对多目标进行实时动态的精确测量。图1为测量系统在工程中的应用结构图。

图1 工程控制图

1 姿态测量系统设计

1.1 测量系统的组成

(1)发射器［8－10］

系统中存在两个发射器，每台发射器以不同角速度旋转，同时发射带有位置和角速度等信息的激光信号，包括两束扇形光和一束选通光。两束扇形光在水平面的夹角θ为90°，与垂直面的夹角φ为±30°，扇面水平约夹角±30°(如图2)。同时发射器内部还集成有电子罗盘模块，配合整平基座可以建立与世界坐标系相符的测量坐标系，南北为坐标系x轴，东西为坐标系y轴，垂直于水平面的轴为z。

图2 扇面夹角

(2)整平基座

整平基座对发射器进行整平操作，使发射器的中心轴线垂直于水平面。

(3)接收器

接收器主要由激光传感器和放大电路组成［11］，用于接收发射器发射出的激光信号，并反馈信息给发射器，告知其对选通光束的接收，通过发射器与接收器输出的信号来实现空间定位。其数量可以根据要求进行配置。

(4)坐标计算处理器

坐标计算处理器用于处理发射器与接收器反馈回来的数据，计算得到与目标相关的角度信息，再根据角度信息计算得到目标点的三维空间坐标。

(5)上位机软件

上位机软件用于汇总各个接收器反馈回来的坐标信息，根据用户的需要计算得到机器人空间位姿的信息，显示于客户端，方便用户掌握机器人的路线偏差并对其进行控制。

1.2 测量系统的布局

自行走掘进机器人外径为1 100 mm，内径为900 mm，共分为4节，每节之间通过液压缸两两相连，液压缸全部处于收缩状态时机器全长5 200 mm其中第1节长度为700 mm，第2、3、4节长度均为1 500 mm(为机器外侧测量长度，不包括机器各节搭接部分长度)，当液压缸全部处于伸长状态时机器全长5 560 mm［1］(如图3)。根据机器人的机械结构和发射器的测量范围对接收器和发射器进行布局。

图3 自行走掘进机器人结构图

在掘进机尾部离内壁一定距离处安装4个接收器，接收器位置即测量点位置，要满足4个测量点在同一断面(图4)且关于中心对称。根据3点确定一个平面的原理，机器人第4段尾部断面方程和中心点O的坐标可以确定。由于掘进机各节的材质均是圆形钢桶，便可根据刚体性质通过数学公式依次推导出各段首尾断面中心坐标完成位姿的测量［12－14］。

图4 接收器安放位置

发射器的扇形光束与水平面的夹角φ为±30°，加上激光的测量距离构成发射器的测量空间(图5)，测量过程中要求接收器处在发射器的测量范围内，且两个发射器相距一定距离且分布于掘进路线内壁的两侧，则可完成接收器空间坐标的测量。

图5 发射器空间测量范围

1.3 测量原理

利用对自行走式掘进机器人尾段机身上不在同一条直线上的三点坐标的测量，根据三点确定一个平面和掘进机的刚体性质，自行走式掘进机器人的空间位置就可以确定。对于机身上点坐标的测量，采用前方交会原理(图6)。

图6 前方交会原理图

计算公式

通过对 A、B 两点空间坐标，αA、αB、βA、βB角度的测量，可以得到目标点P的坐标。

其中A、B两点空间坐标即测量系统两个发射器的坐标，在掘进开始前通过全站仪对两点进行测量。αk(k=A，B)是通过接收装置接收到选通光后反馈给发射装置，记录此时选通光与电子罗盘正北方向的夹角，得出水平方向夹角。βk(k=A，B)是利用两束扇形光被接收器接收到的不同时间差值Δt计算得出的(图7)。

计算公式:

其中t1是发射器第1束扇形光被接收到的时间，t2是第2束扇形光被接收到的时间。

图7 角度测量

1.4 系统工作原理

测量系统启动后，两台发射器以不同角速度ωk(k=A，B)旋转，旋转过程中各自发射出一束选通光和两束扇形光，A、B两发射器发出的选通光先后到达接收器，接收器提取A、B选通光所带的角速度信息和位置信息，以此判断不同时刻接收到的选通光来自哪个发射器。通过无线电模块将接收到的A、B选通光的时刻信息反馈给发射器，发射器记录此时刻选通光与电子罗盘正北方向的夹角，得到水平角度信息 αA、αB。

以接收器接收到选通光的时刻为计时零点，存在两个计时零点A0、B0，4束扇形光先后到达接收器，根据扇形激光所载的信息特征判断该时刻以哪个计时零点计时。得到4组时刻其中为A发射器的两束扇形激光被接收时刻，得到为B发射器的两束扇形激光被接收的时刻，得到根据式(2)计算出βA、βB。同理多个接收器计算方法相同，但反馈给发射器的信息有标识，可以区分不同接收器反馈回的信息。当 αA、αB、βA、βB测量完毕后，一周期的测量结束，开始下一周期的测量。图8为系统结构框图，图9为系统运行流程图。

图8 系统框图

图9 系统流程图

2 系统测量误差分析

系统的坐标测量误差从原理上可分为静态误差和动态误差。静态误差主要源于发射器和接收器本身的观测误差，是仪器观测误差通过系统测量相互作用，相互影响的结果。动态误差主要是接收器接收发射器信号时间上不同步和载体运动共同作用的结果。考虑到测量技术的成熟，发射器空间坐标在开始用全站仪定位所产生的误差可以忽略不计。

2.1 静态误差分析

静态误差是目标点即接收器在静止条件下观测，由环境、零部件不稳定等因素造成的，具有随机性，所以将静态误差视为随机误差处理。设发射器和接收器观测量 αA、αB、βA、βB的观测精度为 σ1、σ2、σ3、σ4，误差为 Δα1、Δα2、Δβ1、Δβ2。静态误差ΔK1=(Δx1，Δy1，Δz1)，测量精度为 σk1=(σx1，σy1，σz1)。对式(1)全微分得:

由于4个角度的测量分别通过两个发射器共4种光束独立测量得出，所以4个观测量彼此独立，互不相关，根据方差合成定理［15］得:

2.2 动态误差分析

在系统测量过程中，由于目标点接收器的运动和发射器A、B不同角速度的旋转，对4个角的测量会出现2个时刻的不同步，即测量水平角αA、αB时2发射器各自的选通光束在不同时刻被接收器接收，接收时间间隔为ΔT1，同理各自扇形光束测量俯仰角βA、βB，被接收器接收的时间间隔为ΔT2，测量水平角和俯仰角之间的时间间隔记为ΔT3，建立动态误差模型(图10)。测量过程中目标点相对于接收器静止，接收器又是测量系统的一部分。所以将接收器的运动作为测量装置内部的运动，它所造成的误差等同于系统本身测量装置的误差，而对目标点的测量则可看作在目标点静止时的测量。又由于动态误差的大小和方向随接收器的载体即掘进机器人的掘进距离，掘进方向和掘进速度大小变化而变化，具有不确定性，不能确切的掌握，所以将动态误差视为未定系统误差处理。

图10 模型动态误差原理

模型图表明时间差 ΔT1、ΔT2、ΔT3的存在引起测量系统观测量的误差，实际测量值为 α'A、α'B、β'A、β'B，分别对应的测量点 p1、p2、p3、p4。由于地下掘进机器人行进速度缓慢，对于以秒级为测量周期的测量定位系统而言，测量周期内的4点在一条直线上，设目标点速度v方向为测量采样点之间的连线方向(xn+1－xn，yn+1－yn，zn+1－ zn)，方向角为 γ、ρ，精度为sk2=(sx2，sy2，sz2)。

可求得 p1，p2，p3，p44 个测量点坐标:

根据上述公式可求得各测量角:

将式(11)～式(14)代入式(1)得目标点的实际测量值(x'，y'，z')。进一步计算得:

动态误差 ΔK2=(Δx2，Δy2，Δz2)。

3 系统误差仿真

ΔK1为静态误差，即随机误差，ΔK2为动态误差，即系统误差。根据系统误差与随机误差合成定理［15］:

为保证测量的精度，应选择合适的结构。对于时间同步则受限于硬件设备性能，所以需要适当缩小观测误差范围，扩大时间同步误差范围。

根据式(4)进行仿真计算，当0＜αA+αB＜180°，0 ＜βA，βB＜30°时，测角精度为 σ≤10″，b≤1 000 mm时，σx1＜27.78 mm，σy1＜43.9 mm，σz1＜38.75 mm。测角精度为 σ≤5″，b≤1 000 mm 时，σx1＜13.89 mm，σy1＜20.87 mm，σz1＜17.49 mm。考虑到目前系统的激光测角精度和在掘进工程中隧道标高误差需控制在－20 mm～80 mm，平面误差控制在±50 mm，水工隧道误差控制在±150 mm［16］等因素，取测角精度σ≤5″进行分配后得:σx1≤20 mm，σy1≤25 mm，σz1≤20 mm。sx2≤45 mm，sy2≤43 mm，sz2≤10 mm。

通过动态测量误差模型可知，目标点速度与误差成正比，发射器角速度与误差成反比，对模型进行仿真，对模型中的输入参量取极限值，设定实际路线。目标行进速度为v≤1.5 mm/s，发射器A的角速度为ωA≥π/4 rad/s，发射器B的角速度为ωB≥π/3 rad/s。A 点坐标为(xA，yA，zA)，B 点坐标为(xB，yB，zB)。其中|yB－yA|≤1 000 mm。图11 为极限值情况下的仿真误差。由图可知，动态误差满足上述误差分配要求。该导航定位方法在掘进过程位姿测量中可行，且精度较高。

图11 系统仿真

惯性导航定位方法与其相比运用的是陀螺仪对掘进机进行方位和倾斜角检测，同时利用其中的加速度计测量掘进机的加速度并对其进行两次积分求得掘进距离。通过对方位角，倾斜角和掘进距离的计算得到掘进机器人的位置坐标。图12表示机器人位置的计算模型。其中θi表示测量得到的方位角与倾斜角。从已知点A1出发，沿θ1方向掘进距离L1，由此算出点A2的坐标。再从A2出发，用同样方法算出A3。以此类推，可算出直到An的坐标。以日本Tolimec公司制造的MG－32B系统为例，系统方位角静止点精度 ±0.2°，俯仰角精度 ±0.1°［17］。设定实际路线与上述相同，不考虑陀螺仪加速度计误差对时间积分的影响，对惯性系统进行仿真。图13可知随着掘进距离的增加，定位误差逐渐增大。该仿真是在较理想的情况下，在实际测量过程中由于加工工艺误差、信号检测误差引起的陀螺漂移会严重影响测量的精度。此外，环境温度的变化对陀螺的机械、电学性能影响也较大，有的甚至是质的变化，这无疑也会导致陀螺精度的下降。这些都会使得测量结果的误差大于图中仿真结果。与上述测量系统相比它的精度和稳定性显得不足。

图12 惯性系统位置计算模型

图13 惯性导航测量系统仿真

4 结论

针对目前自行走式地下掘进机器人地下姿态测量方法的不足，本文提出的新型地下姿态测量方法更加合理。该系统操作简便，实时性好，精度较高，对地下机器人掘进过程中的姿态定位具有一定的应用价值。

［1］ 卢清国，王庆学，张延钦，等.自行走式地下管道掘进机工业样机试验［J］.建筑机械，2010(7):81－86.

［2］ 唐争气，赵俊三，彭国新.盾构机实时姿态测量和计算方法的研究［J］.土木工程学报，2007，40(11):92 －97.

［3］ 冯宝新，王解先.盾构姿态测量方法［J］.公路隧道，2011(1):63－65.

［4］ 潘国荣，王穗辉，陈传林，等.盾构姿态自动测量系统的开发与应用［J］.同济大学学报，2010，38(3):459 －463.

［5］ 潘明华.盾构自动导向系统的研究与实现［D］.湖北:华中科技大学，2005.

［6］ 马建仓，陈静.MEMS SINS-GPS组合导航系统设计［J］.传感技术学报，2009，22(10):1437 －1441.

［7］ 王晶晶，童敏明，刘彬.组合导航定位系统研究［J］.Software，2011，32(5):82 －84.

［8］ Thomas M Hedges，Hiro Takgl，Michael J Sobel.Position Measurement System and Using Cone Math Calibration［P］.美国专利:6535282B2，2003－3－18.

［9］ Schwendemann J，Tilman Müller，Krautschneider R.Indoor Navigation of Machines and Measuring Devices with iGPS［C］//IEEE International Conference on Indoor Positioning and Indoor Navigation，Zürich，Swizerland，15 －17 September 2010.

［10］ Robert Schmitt，Nish S，Schonberg A，et al.Performance Evaluation of iGPS for Industrial Application［C］//IEEE International Conference on Indoor Positioning and Indoor Navigation，Zürich，Swizerland，15 －17 September 2010.

［11］李孝轩.传感器微弱信号低噪声斩波放大器［J］.传感技术学报，1994，7(3):44 －48.

［12］张厚美，古力.盾构机姿态参数的测量及计算方法研究［J］.现代隧道技术，2004，41(2):14 －20.

［13］李鹏程.盾构机姿态测量方法的精度比较［J］.安徽建筑，2008，15(6):174 －176.

［14］冯东健，潘庆林，张凤梅.地铁盾构施工中盾构机姿态定位测量的研究［J］.工程勘探，2005，5:57 －61.

［15］吴石林，张玘.误差分析与数据处理［M］.北京:清华大学出版社，2010:108－113.

［16］孙更生等.中国土木工程师手册(中)［M］.上海:上海科学技术出版社，2001:55 －60.

［17］于睿坤.盾构掘进机掘进过程中姿态的智能控制系统研究［D］.上海:同济大学，2008.