1 引言

当前,大空间精密测量定位已成为航天、航空、航海等大型装备数字化制造和装配的基础支撑技术^[1-3]。传统单站式测量系统(如激光跟踪仪、全站仪)受串行单目标测量模式的限制,无法满足多设备协同、智能工装自主定位的多点多任务并行、高效动态跟踪测量需求。近年来出现的分布式测量系统将多个测量站组成空间分布的整体测量网络,在保证测量精度的基础上通过并行自动化测量提高了测量效率,是智能制造的关键环节。目前,大空间分布式测量网络已发展出多种传感器形式,如基于视觉原理的相机网络^[4]、基于光电扫描的室内空间定位网络^[5-8]、基于声学测距的超声定位网络^[9-10]和基于接收强度信息的射频定位网络^[11]。其中,室内空间测量定位系统(wMPS)^[12]是基于光电扫描原理的大尺寸三维坐标测量系统,具有精度高、多任务并行自动测量等优点,被广泛应用于工业坐标测量领域,是分布式测量系统的典型代表。

分布式测量网络的组网定向,是获取网络中多个关联测量仪器相对位姿关系的重要过程和共性关键技术,以wMPS网络为例,为了对工作区域实现全覆盖,系统包含的发射站数量以及需要标定的定向参数(外参)通常较多,且发射站交会关系复杂,对组网定向方法的要求更高。目前常用的方法需要在测量场内数十个位置(标定点位)放置接收器,每个接收器需同时采集至少两个发射站的信号,利用扫描交会关系构造约束方程^[12],迭代求解定向参数。通常应保证一部分接收器能同时采集多个发射站的信号,才能将解算误差控制在一定范围。现有的方法主要依靠主观经验,判断测量场条件及各个发射站的交会区域,并粗略设置标定点位、每个接收器的朝向。但发射站数量较多时,该方法难度较大。其次,接收器存在最大接收角和距离限制,只能接收到以其正对方向为轴线的一个锥体范围内的激光信号。现有的组网方法通过试错法反复手动微调接收器的位置和方向,以确保所有接收器和发射站在彼此的观测视野内。除此之外,交会角度也是影响定位精度的因素^[13],而现有的组网方法依赖主观经验估计每个标定点位的交会角度,存在潜在的不确定性。

为满足智能测量的要求,近几年工业现场的自动引导车(AGV)应用越来越广泛,AGV容易集成深度相机、激光雷达等传感器,结合同步定位与地图构建(SLAM)等技术后,具有地图构建、全局定位等能力^[5]。一方面可以获取测量场的客观先验信息,如测量场的几何信息、测量网络布局等,对信号盲区和交会角度等参数进行准确计算,从而合理规划标定点位的位置和最佳朝向。另一方面AGV具有定位和导航的能力,可减少人力参与。完成组网的分布式测量网络具有高精度定位能力,给AGV定位和导航提供了有效支撑^[5]。

本文首先介绍了wMPS测量模型,研究了结合自主探索建图、全局定位技术及wMPS交会测量模型的组网定向算法,并提出了针对不同测量环境的标定点位规划策略。经实验验证,该方法相比现有方法具有更高的自动化程度和更快的组网定向速度。

2 基本原理

2.1 测量模型

wMPS发射站的旋转头向空间内发射两个形状已知的旋转激光平面,如图1所示,激光平面的初始方程可表示为

amx+bmy+cmz+dm=0,m=1,2,(1)

式中,a_m、b_m、c_m、d_m为平面初始参数。当接收器接收到两个激光平面信号时,两平面的扫描角度 θmt=ω(t_m-t₀),其中,ω为发射站旋转头的转速,t_m为激光平面m扫描至接收器的时间,t₀为激光平面扫描至码盘零点的时间,此时激光平面参数为

amtbmtcmtdmt=cosθmt-sinθmt00sinθmt cosθmt0000100001ambmcmdm,m=1,2。(2)

图 1. wMPS原理图

Fig. 1. Schematic diagram of the wMPS

下载图片 查看所有图片

假设接收器在某时刻接收到多个发射站的信号,且全局坐标系到每个发射站的变换关系[R_k,T_k]已知,则每个发射站都满足方程

[amkbmkcmk] cosθmktsinθmkt0-sinθmktcosθmkt0001·Rkxyz+Tk+dmk=0,(3)

式中,k=1,2,3,…为发射站序号, [xyz]T为接收器的全局坐标。当接收器同时接收到2个或2个以上发射站的信号时,用最小二乘算法就能得到接收器三维坐标 [xyz]T的最优估计。

2.2 基于长度约束的组网定向算法

常用的wMPS组网定向方法有基于基准尺和基于控制场两种方法^[12]。通过基准尺构建长度约束,并利用联合平差迭代求解文献[ 12]中的最优化目标函数F

F=∑m=12∑k=1K∑j=1M∑i=12(dmkij2)+λ·[(xU-xL)2+(yU-yL)2+(zU-zL)2-L]2,(4)

式中,d_mkij为标定点位j处的第i个接收器距离发射站k发出的激光平面m的距离,U和L分别表示同一标定点位处基准尺两端的接收器,L为基准尺长度,λ为惩罚系数。

实验基于(4)式求解定向参数,(4)式一般利用非线性最优化方法求解。为了防止局部极小点的影响,提高收敛速度,有必要对迭代初值的计算进行研究。根据文献[ 12],需要提供的初值分为外参初值和位置初值、全局坐标系到发射站坐标系的旋转矩阵和平移矢量以及所有标定点位处每个接收器在全局坐标系下的坐标。对于外参初值,可以应用后方交会模型^[14]估计各个发射站坐标系相对于某一全局坐标系的变换关系。对于位置初值,现有方法假设发射站轴线与接收器连线垂直^[12],但实际中难以严格满足该假设,令初值计算困难,因此提出了一种新的基于全局定位的初值计算方法。

2.3 自动组网系统设计

实验使用的系统原理如图2所示,wMPS发射站发射的激光平面基本能覆盖测量空间。将一组接收器固定在AGV车身上组成接收器靶标,接收发射站发出的激光信号并经信号处理器处理后发送至上位机。

图 2. 系统结构图

Fig. 2. Structure diagram of the system

下载图片 查看所有图片

SLAM可构建或更新环境地图,同时估计传感器的运动状态,其中GMapping^[15-16]是较为成熟的SLAM方案。基于SLAM的主动探索^[17-18]可在无任何先验信息的前提下,自主运动AGV并构建测量场的地图,结合后方交会模型计算发射站的外参初值。根据发射站的布局信息和地图信息计算wMPS的交会区域、交会角度、障碍物、信号盲区等客观先验信息,并以此作为规划标定点位分布的依据。完成地图构建后,利用自适应蒙特卡罗定位(AMCL)法^[19-21],输入激光雷达的实时测量数据,跟踪和估计全局位姿。AGV自动导航至标定点位处采集wMPS数据^[12],在每个点位处AGV上接收器的位置初值可用AMCL模型计算得到。将迭代初值(位置初值和外参初值)和wMPS数据代入(4)式可求解定向参数。

3 融合自主导航信息的组网算法

3.1 算法流程

所用算法的具体步骤。1)结合后方交会模型和自主探索建图算法获取客观先验信息和外参初值。2)根据客观先验信息,基于标定点位规划策略,自动规划合适的标定点位分布,计算每个点位的朝向。3) AGV行驶经过所有标定点位,通过AMCL得到布置于车身上的wMPS接收器的坐标,并将其作为位置迭代初值,同时采集wMPS标定数据。最后将迭代初值、wMPS标定数据代入迭代优化方程解算定向参数,算法整体流程如图3所示。

图 3. 所提算法流程图

Fig. 3. Flow chart of the proposed algorithm

下载图片 查看所有图片

3.2 初值估计算法

后方交会模型能估算出由一组相对位置已知的接收器构成的靶标坐标系tar到发射站坐标系k的旋转矩阵 ktarR˙和平移矩阵 ktarR˙,其中k=1,2,3,…。在AGV自主探索构建地图的过程中,靶标会随车身在测量场内往复移动与转动,并持续地采集发射站的激光信号,利用后方交会模型实时计算发射站坐标系相对于靶标的变换关系,同时利用SLAM算法在全局地图坐标系上实时定位靶标位姿,得到发射站相对于全局地图坐标系的转换关系,并将其作为外参初值。

假设靶标和激光雷达的坐标系关系已知,由SLAM算法得到的地图坐标系map到靶标坐标系tar的转换关系为 maptarR^和 maptarT^,则地图坐标系map到发射站坐标系k的转换关系[ kmapR^kmapT^],即外参初值为

mapkR^=tarkR^maptarR^,(5)mapkT^=tarkT^+tarkR^maptarT^。(6)

发射站在测量场中的位置信息是标定点位规划的重要依据,因此需要计算发射站的分布。坐标系k的原点在自身坐标系下的坐标可以表示为

p^kk=mapkR^p^mapk+mapkT^=[000]T,(7)

式中, p^kmap为发射站k的原点在map坐标系的位置,可表示为

p^mapk=-(mapkR^)-1mapkT^。(8)

位置初值是指参与wMPS标定数据采集过程的接收器在全局坐标系下的坐标。利用AMCL估计AGV在每个标定点位处接收器靶标相对于地图的坐标关系 maptarR^i和 maptarT^i,则位置初值为

r^mapi=maptarR^i-1rtari-maptarT^i,(9)

式中, ritar为第i个接收器相对靶标坐标系的坐标,由提前标定得到。

3.3 标定点位自动规划算法

为了解决现有组网方法在选择标定点位过程中依赖主观经验的问题,提出了一种基于先验信息的标定点位自动规划算法。标定点位分布包括五项原则。

1) 控制标定点位的数量,且标定点位应尽可能覆盖整个测量空间。假设创建的地图是w×h的二维栅格M,一个栅格的状态可以被简化成三种:占用、未占用和未知,分别表示为M(s=1)、M(s=0)和M(s=-1)。为使标定点位覆盖测量场,对M(s=0)均匀采样得到潜在标定位置,记为M'(s=0)。记 pkcal,i为M'(s=0)中发射站k的第i个标定点位在地图坐标系中的位置。增加标定点位的数量虽然能增多冗余方程,但过多的点位数量会增加信息采集和解算收敛的时间。定向一组发射站至少需要6个点位^[12],权衡冗余方程数量和解算效率,每组发射站的点位数应在6~20之间。

2) 避免信号遮挡和碰撞。距离障碍物较近的标定点位容易发生碰撞,障碍物后方的标定点位容易产生信号遮挡,因此标定点位需满足

pcalkpcal,ik∉Fok,‖pcal,ik-pobs‖2≥ds,(10)

式中,p_obs为占用栅格在地图坐标系中的位置,d_s为安全距离, Fko为根据发射站k的位置和障碍物的位置信息共同确定的信号遮挡区域,‖‖₂为2范数。用激光雷达等传感器测量障碍物的三维信息,可以更准确估计 Fko,剔除在该区域内部的点位。

3) 在至少被两台发射站信号覆盖的区域内,能同时接收到多站信号的点位优先。考虑激光平面扫描范围,如图1所示,假设发射站k的最小量程为 r1k,最大量程为 r2k,激光平面旋转范围为0°~360°,扇面角为2β^k,经后方交会模型估计的发射站高度为h^k,则标定点位的位置需满足

minr1k,hkcosβk≤‖pcal,ik-pmapk‖2≤r2k。(11)

标定点位的方向条件为

-αrec2≤arccos(pmapk-pcal,ik)·ncal,ik‖pmapk-pcal,ik‖‖ncal,ik‖≤αrec2,(12)

式中,α_rec为wMPS接收器的信号接收最大角度, nkcal,i为接收器的方向矢量。记F_i为接收器i的可探测范围,则同时满足(11)式和(12)式的点可表示为

pcal,ikpmapk∈Fi。(13)

因接收器须同时观测到至少两个发射站,为了降低坐标系转换的累积误差,提高标定效率,在多个发射站(发射站组)的公共视野F_group内设置点位

pcal,ipmap1,pmap2,pmap3,…∈Fgroup。(14)

实际上每个接收器的最大接收角度与标称值存在少许偏差,因此将朝向设置为发射站组的角度中心以保证角度裕度。

4) 减少无效路径,提高效率。AGV从当前标定点位行驶至下一个点位所需要的时间,远大于在一个标定点位处原地旋转的时间。因此,为了在增加标定点位的数量和约束方程的数量的同时尽量减少增加的标定时间,通过旋转当前点位处的AGV改变接收器的朝向。依次采集不同发射站组的信号,如假设由于接收角度限制最多同时观测2个发射站,在某一位置处先采集发射站组{1,2}的数据,再分别朝向发射站组{2,3}和{3,4}并采集数据,达到在同一位置分别采集多组发射站信号的目的。

综上所述,实验采取基于角度采样的方法决定每个点位的朝向。即对每个标定位置,以5°为间隔进行采样,得到角度数组Φ={5°,10°,15°,…,360°},每个元素分别对应该标定位置处的一个标定点位。检查数组中的所有元素,确认其对应的标定点位是否满足上述原则,剔除不能观测到发射站或只能观测到一个发射站的元素,找到观测发射站数量最多的角度构成数组Φ'。Φ'的元素可能观测同一组发射站或不同组发射站,判断被观测各组发射站的中心角度,分别作为该标定位置的一个朝向。

5) 保证几何精度因子(GDOP)在可接受范围。GDOP是衡量交会测量角度对测角误差影响的无量纲放大因子^[22-23],GDOP越大,表明定位精度越低。为保证定位精度,多数标定点位应满足交会角度在50°~120°之间,避免交会角度出现过大或过小,保证GDOP在可接受范围内。

4 实验验证

4.1 实验配置

实验使用的AGV最大线速度为0.4 m/s,最大角速度为0.2 rad/s,最大线加速度为1 m/s²,搭载SLAMTEC公司的激光扫描测距雷达RPLIDAR,在半径为18 m的测量范围内,以频率为8 kHz进行测量。事先用激光跟踪仪(LEICA-AT403)标定被固定在车体上表面6个接收器的位置。4个wMPS发射站按图4的进行布局,覆盖尺寸为12 m×7 m×3 m测量场,激光跟踪仪的测角误差为(15±6) μm/m。

图 4. 实验环境及配置

Fig. 4. Experimental environment and setup

下载图片 查看所有图片

4.2 组网定向实验

AGV主动探索测量场,并建立栅格地图,同时利用后方交会模型估算发射站的位姿并实时显示在地图上,结果如图5所示。图中1区域表示已被探索的安全区域,2区域表示未探索区域,柱状标记为发射站的估算位置。获取全局地图和发射站在地图中的位置后,根据标定点位规划策略,自动规划一系列点位和标定路径,结果如图6所示。图6(a)中的不同灰度值表示由发射站测量范围和障碍物遮挡共同造成的信号盲区和交会区域,灰度值越低表示能被更多发射站的信号覆盖;图6(b)中的小点表示标定点位的位置,短线表示该点位的朝向。

图 5. SLAM创建的地图和发射站布局

Fig. 5. Map and transmitter distribution created by SLAM

下载图片 查看所有图片

图 6. 标定点位规划结果。(a)规划结果;(b)局部放大图

Fig. 6. Results of calibration path planning. (a) Results of planning; (b) partial enlargement

下载图片 查看所有图片

AGV依次行驶经过标定点位,在每个点位朝向处约停留3 s,同时采集接收器收到的激光信号,并对一小段时间内的测量信号进行平均滤波,得到wMPS的标定数据。此外,当AGV停留在标定点位时,由AMCL定位接收器的坐标并计算位置初值,并根据(4)式迭代解算定向参数。

为了验证组网定向结果的准确性,进行了精度分析实验。将两个接收器固定在标准长度基准尺的两端,将该基准尺放在空间不同位置处。基于实验计算得到的定向参数,利用wMPS测量网络,定位接收器并计算两接收器间的相对距离d,并与距离真值d₀进行对比,计算绝对误差e_a和相对误差e_r,结果如表1所示。可以发现,8次实验测得的距离平均误差为0.15 mm,满足高精度组网定向的需求。

在保证组网定向精度的同时,该方法合理规划了标定点位,一方面确保了每个点位都能有效采集至少两个发射站发出的信号,减少了反复手动调整点位所需的时间;另一方面在同一位置处设置多个朝向,提高了每一位置的利用率,缩短了采集信号所需的时间,当测量网络复杂时效果尤为明显。分别使用传统手动和AGV自动两种方式在测量场采集相同数量标定点位的激光信号,并记录消耗时间,结果如图7所示。可以发现,结合自动点位规划的AGV自动组网在测量网络复杂时明显提高了组网效率。

图 7. 不同标定方法的标定时间对比

Fig. 7. Comparison of time-consuming using different calibration methods

下载图片 查看所有图片

5 结论

从两种方法的测量精度和测量效率可以看出,本方法在保证组网定向精度的同时,提高了组网定向的效率,主要原因包括:每个标定点位的位置和朝向均依据客观先验信息和规划策略得出,保证了点位的有效性,省略了传统方法中目测与反复试错的环节,大大节省了时间;合理规划了标定点位的分布,在维持标定点位分布密度的基础上避免了AGV在点位间的往复移动;基于全局定位的迭代初值计算降低了对接收器位姿的假设要求,改善了标定算法的迭代收敛过程。

实验结果表明,本方法的测量精度达到了亚mm级,满足装备加工和装配工作对测量精度的要求,同时利用AGV提供的客观先验信息,降低了对操作员个体经验的依赖程度,缩短了复杂测量网络组网工作的耗费时长,具有更高的自动化程度。但该方法在规划标定点位时,没有充分考虑测量场内各种高度、形态的障碍物造成的接收器和测量网络之间的信号遮挡问题。原因是所使用的二维激光雷达只能创建平面地图,缺少了障碍物的高度等信息,这也是下一步需要研究的工作。