室外带电作业机器人的融合定位方法研究*

徐伟 王勇军*

(1 深圳亿嘉和科技研发有限公司 智能清洁事业部,广东 深圳 518055;2 桂林航天工业学院 无人机遥测重点实验室,广西 桂林 541004)

在高压输电线剥线、接线等带电操作中,通常依靠人工完成,但存在效率低下、劳动强度大和高压安全不足等问题[1-4]。为了解决这些问题,工业界研制了带电作业机器人[2,5-6]。这些室外带电作业机器人多采用激光点云技术对电线等作业对象进行建模,并计算出最佳的目标作业位姿。通过定位系统实时反馈精准定位信息,从而引导机器人达到指定位置。室外带电作业机器人的作业场景通常是半高空场景,且周围环境比较复杂。在这种工作环境下,准确定位是带电作业机器人顺利完成作业任务的基础[7]。

诸多学者对室外带电作业机器人半高空场景下的精准定位技术开展了研究。文献[8]使用单目视觉定位方法,文献[9]使用激光定位方法,文献[10]使用激光雷达建立场景语义来获取定位,文献[11]采用的是双目立体视觉的定位方法。但是,半高空场景下的电线等关键目标点云特征稀疏,视觉成像易受强光光照干扰,因而该场景下的点云匹配和视觉应用定位方法受限较多。借鉴全球导航卫星系统(Global Navigation Satellite System, GNSS)技术可实现一种不依赖环境特征的绝对定位方法,来解决环境受限场景下带电作业机器人的定位问题[12-18]。但是该技术多数都以GPS数据为主要基准,其单点定位精度较差,无法满足带电作业机器人的厘米级定位精度要求。为此,可在卫星导航基础上,进一步采用载波相位差分(Real-Time Kinematic,RTK)技术,可使得定位精度能达到厘米级[19]。但RTK定位性能通常易受环境遮挡的影响,且需要考虑并解决工程上的电磁干扰问题。文献[20-21]使用RTK与激光雷达进行融合定位,可在一定程度上解决干扰问题,但在工程上增加了全局建图的工序。

结合以上分析,设计了一种基于双天线RTK/激光雷达/IMU的误差滤波器,采用以双天线RTK与惯性测量单元(Inertial Measurement Unit,IMU)的组合导航技术[22]为主,激光点云地图匹配技术为辅的融合定位方法,充分发挥RTK、激光雷达和IMU三种传感器的优势互补特性,不仅有效解决场景受限时的定位问题,而且大大提升了该场景下的定位稳定性。同时,构建了一种适应半高空场景的机器人运动约束模型,可有效解决机器人本体电磁干扰导致航向估计误差大甚至发散问题,大大提升了抗干扰能力。

在本方法中,RTK和IMU进行数据融合,考虑到单天线RTK无法静止定向,因此采用了双天线RTK方案,并推导出了双天线RTK位置、速度和航向观测模型。RTK和激光雷达亦进行数据融合,RTK在空旷场景表现优异,而激光雷达在遮挡场景表现优异,两者在室外复杂场景互补融合,同时考虑到半高空场景以空旷为主,因而以RTK为主要定位传感器。最后,考虑到高压线路作业过程中,大电流和高电压产生的强烈电磁场可能会干扰GPS和RTK的工作,导致定位失效或误差增大,还引入了抗干扰处理方法。为了解决干扰问题,本文引入了机器人优化运动约束模型。通过对机器人的运动模式进行深入分析,并结合IMU的高频数据,实时调整和优化模型参数,从而有效抵消电磁干扰的影响,保证了定位的准确性和稳定性。

1 双天线RTK/激光雷达/IMU组合系统

如图1所示的双天线RTK/激光雷达/IMU组合系统,测量IMU坐标系相对于东北天坐标系[15]的三维位姿变换。要求IMU、激光雷达与双天线严格刚性安装,且RTK航向观测方程随IMU安装位置和姿态不同而不同,设置RTK主天线指向副天线方向与IMU传感器y轴正向一致。误差滤波器以IMU误差传播建立预测方程、以双天线RTK位置、速度和航向建立RTK观测方程、以激光点云地图匹配位姿建立激光观测方程。注意的是,当RTK观测质量不佳时,才会融合激光位姿。

IMU通常包括三轴加速度计和三轴陀螺仪,分别测量物体的线性加速度和角速度。捷联惯性导航系统(Strapdown Inertial Navigation System,SINS)使用加速度和角速度信息并进行积分,估计位置、速度和姿态[15,23],并根据IMU精度推导误差协方差[24]。

RTK主要由基站和移动站组成,基站和移动站通过电台或网络传输差分数据,实现对移动站位置的精确计算。通常将基站架高并静止放置于室外空旷环境,移动站固连于机器人本体并随机器人运动。因此,其位姿用两种不同的坐标系描述p点在地心地固坐标系(Earth-Centered Earth-Fixed,ECEF)中的位置,即大地坐标系和直角坐标系[14],如图2所示。RTK位置测量采用纬度,经度,高度(φ,λ,h)大地坐标系表示,RTK航向测量为主天线指向副天线方向与真北的夹角。为方便数学处理,将经纬高度转换到东北天坐标系[15]。

激光雷达用于测量和获取目标物体的距离、位置和形状等信息,它通过发射激光束并接受激光束的反射信号来实现测量。对电线等作业对象建模,同时对环境进行激光扫描建图[25],并生成三维点云地图。根据激光点云地图数据,并结合点云精匹配算法(Iterative Closest Point,ICP)可求解位姿和置信度[26]。

1.1 IMU误差滤波器

滤波状态xt=[pt,vt,qt,abt,wbt]由预测状态x=[p,v,q,ab,wb]和误差状态δx=[δp,δv,δq,δab,δwb]组成。其中,p,v,q,ab,wb分别为三维位置、速度、姿态四元数、加速度零偏、角速度零偏。x通过三维角速度和加速度积分,δx通过建立IMU误差方程并结合双天线RTK观测修正估计。三者的关系用式(1)表示, ⊕ 表示向量运算,如加减法、四元数乘法。

xt=x⊕δx

(1)

1.1.1 预测方程

预测状态x,用式(2)对三维角速度ωm和加速度am进行积分。

(2)

1.1.2 误差方程

建立误差方程式(3)。

δp←δp+δvΔt

δv←δv+(-(R(am-ab))δθ-Rδab)Δt+vi

δθ←δθ-RδωbΔt+θi

δab←δab+ai

δωb←δωb+ωi

(3)

其中,vi,θi,ai,ωi分别为速度,角度,加速度偏置,角速度偏置高斯白噪声。

设误差状态和扰动向量的雅可比矩阵分别为Fx和Fi,扰动向量的协方差矩阵为Q,建立误差预测方程(4)。

(4)

为了让滤波器快速收敛,通常取较大初始协方差P0[15]。

1.2 双天线RTK观测器

1.2.1 位置观测方程

(5)

其关于δθ的雅可比矩阵为:

(6)

位置观测雅可比矩阵为:

(7)

图3 RTK位置观测模型

1.2.2 速度观测方程

对式(5)进行微分,建立RTK速度观测方程式(8)。

(8)

其关于δθ的雅可比矩阵为:

(9)

其关于δωb的雅可比矩阵为:

(10)

速度观测雅可比矩阵为:

(11)

1.2.3 航向观测方程

因旋转矩阵R提取的航向φ′∈[-π,π],而双天线RTK测量的真北方向角φ∈[0,2π),将其归一化到区间[-π,π],如图4所示。

图4 双天线RTK航向归一化

考虑IMU的y轴指向机头方向,φ关于旋转四元数q=[q0,q1,q2,q3]T的方程为:

(12)

(13)

其中,φ关于q的雅可比为:

(14)

q关于δθ的雅可比为:

(15)

航向观测雅可比矩阵为:

(16)

1.3 激光位姿观测器

(17)

其中T为4×4矩阵,包含了三维旋转矩阵R和平移向量p。则激光位置观测方程为:

(18)

位置观测雅可比矩阵同式(7)。

1.4 有害数据剔除与滤波

E{rrT}=HP-HT+R

(19)

同时选择一个阈值λ使得

Prob{r2>λ(HP-HT+R)}=μ

(20)

此处μ∈(0,1)通常较小。当式(21)被满足时,

r2=λ(HP-HT+R)

(21)

测量值是无效的且相应的观测修正更新可以被忽略[27]。

该作业机器人的位置、速度和航向观测过程各自进行更新,且各自进行有害数据检测和剔除。需要注意的是,通常RTK可以反馈定位和定向状态,根据其状态亦可判断测量值是否有效。

2 基于运动约束模型的抗干扰处理

2.1 电磁干扰描述

带电作业机器人电子设备复杂,难免会产生高频杂波等电磁干扰,影响RTK接收卫星信号并进行定位解算,特别是RTK副天线对干扰信号尤为敏感,若处理不好,将大大降低RTK解算性能。可通过架高天线等远离干扰源的方法可改善电磁干扰问题,但这样会增大产品体积,应用空间受限。

2.2 基于IMU的运动约束

室外带电作业机器人主要靠机械臂操作,且本体通常处于静态执行建模、接线、剥线等作业任务,因而本体不会有高频振动,对IMU非常友好。因此,可利用IMU实时检测机器人运动状态[28]。

设三维加速度am=[amx,amy,amz],三维陀螺仪ωm=[ωmx,ωmy,ωmz],若满足式 (22)、(23)、(24),则认为其处于稳态。

(22)

(23)

|ωmi-δωbi|<0.2

(24)

其中,i=x,y,z,0.1、0.01、0.2是根据实验设定的经验值,应用时要根据使用的IMU精度进行参数调整。δωb=[δωbx,δωby,δωbz]是陀螺仪零偏偏置,在处于稳态时可用式(25)去更新。

δωb←δωb+0.01(ωm-δωb)

(25)

2.3 滤波处理

应用时架高双天线就可忽略电磁干扰对定位天线的影响,仅需考虑如何剔除航向有害数据。若式(21)生效,则可剔除当前RTK航向测量值。此时若检测到机器人处于静态,则使用当前时刻滤波器航向估计值作为后续观测量,直到稳态退出或定向状态恢复。

3 实验验证

图5所示的是自行研制的双天线RTK/IMU组合导航系统。

图5 双天线RTK/IMU组合导航系统

将组合导航和3D激光雷达(32线,测量精度:±1 cm,FOV:360°×90°,分辨率:水平0.2°、垂直2.81°)分别固定安装于室外带电作业机器人,双天线架高0.3米,主副天线间隔1米,分别在半高空无遮挡和半遮挡场景进行作业。图6展示了半高空无遮挡和半遮挡场景下的点云地图及其定位轨迹,并在这两种场景下测试定位和定向精度。

图6 室外半高空场景点云地图及定位轨迹

整个实验可分六个步骤:1)将自研RTK/IMU组合导航设备固定安装在带电作业机器人。2)利用功分器让两者共用双天线卫星信号。3)使用自研基站发送差分电文。4)分别在半高空无遮挡和半遮挡两种场景均执行下述步骤。5)3支激光笔分别以不同的位姿固定安装在机器人本体上,3条激光束打到地面标签纸上。6)在三维空间中任意选取A1,A2,…An点,并在地面上贴上3个标签纸。6)将机器人从三维空间中任意位置操控到A1,A2,…An点,每个点反复操作10次及以上,并记录结果。

实验测试流程如图7所示。

图7 实验测试流程图

若机器人处于静态状态,激光雷达对电线等作业对象按照1 m/s速度进行动态扫描,使用LOAM系列[27]建图算法就可生成点云地图,如图8所示。与此同时,通过滤波器计算的位姿将激光点云转换到地图坐标系,可确定转换后的点云与地图的匹配重合度。不难发现,图8所示的匹配重合度较好,这也从侧面能反映出滤波器能提供精准定位和定向,证明了本文方法的有效性。

图8 转换后的激光点云与地图重合情况

实际应用过程中因存在各种干扰,如果仅采用RTK数据来解算机器人定向信息,会发现RTK航向测量值波动较大,解算的航向角会逐渐发散,如图9所示原始RTK航向值。

图9 航向滤波前后对比曲线

为提高定向精度,解决航向发散问题,结合机器人“运动约束模型”所设计的滤波器可修正RTK解算数据,获得平稳光滑的航向滤波结果(图9航向滤波值)。这也表明,优化的运动约束模型能提升RTK/IMU组合滤波的精度和稳定性,该方法可提升滤波器的抗干扰能力。

图6(b)所示的半高空半遮挡场景是应用中出现最多的场景,存在较多的遮挡物,RTK定位状态存在“假固定”现象,且本体的电磁干扰也会导致RTK定向状态出现浮动解。在该环境下进行零位静态重复定位和定向精度测试,10次实验结果如表1所示。结果表明,RTK组合导航融合了激光位姿以及运动学约束模型,能够为室外带电作业机器人提供高于3cm的定位精度和高于0.3°的定向精度,满足机器人作业要求。

表1 重复定位精度测试结果

4 结 论

本文采用室外带电作业机器人为载体,以解决带电作业机器人定位问题以及干扰情况下引起的航向误差大甚至发散问题为目标,设计了双天线RTK/激光雷达/IMU组合滤波器,充分利用双天线RTK、激光雷达、IMU三种传感器的优势互补特性,解决了应用场景下精准定位问题;同时,还建立了基于室外带电作业机器人的运动约束模型,充分发挥IMU在该场景下的优势,利用IMU数据进行稳态检测,解决了航向误差发散的问题。经过实验验证,基于双天线RTK组合导航,融合激光点云地图匹配技术,引入运动学约束模型,不仅能够提供精准定位和定向信息,而且大大提升定位稳定性和抗干扰能力。

基于双天线RTK/IMU的激光点云数据融合滤波定位方法,不仅提高了带电作业机器人在复杂环境下的定位精度,也降低了因环境变化和电磁干扰引起的定位失效风险,为带电作业机器人的广泛应用打下了坚实的基础。该方法已经在公司承接的多个任务场景下进行了长时间的作业测试,结果符合预期,具备较高的商业推广价值。

需要注意的是,双天线RTK与IMU的杆臂值、激光雷达与IMU的静态位姿,均直接采用结构初值,应用时存在旋转外参误差,下一步可结合该误差的补偿研究进而提高系统的定位定向精度。