0 引言
随着科学技术的不断进步,新的作战方式层出不穷,智能化武器装备在现代战争中的作用日益增强。为适应现代化新型作战需求,弹药补给车正朝着自动化、无人化、信息化和智能化方向迅速发展。在无人弹药补给车定位方式中,使用单一传感器很难获得准确定位,因此,多传感器融合定位成为研究者的重点。最初,IS Weon和SG Lee333[1]提出了一种将GPS和IMU首先进行坐标校正,然后融合激光雷达和视觉数据;2019年,马争光等[2]提出激光SLAM的关键问题是进行传感器的联合标定;付心如等[3]研究了GPS系统和IMU惯导系统融合方式,提出一种非线性化误差模型,改进了无迹卡尔曼滤波算法;章大勇等[4]研究的组合定位导航基于地标量测的IMU惯导和激光雷达数据融合定位。金峰等[5]研究了基于卡尔曼滤波的定位导航,该算法处理IMU惯导和激光雷达融合数据,较好地解决了内部传感器漂移问题。
因此,本文中针对现有的单传感器算法建图精度低,实时性不足的问题,为了适应未来战争、实现自主定位、快速准确到达目的地的要求,在激光雷达的基础上引入IMU,并对IMU与激光雷达进行外参标定与时间同步,解决在建图过程中因激光雷达与IMU坐标系未统一产生的建图漂移现象,完成了多传感器融合建图的效果,提升了建图精度。设计了一种能够实现自主定位及安全运输的火炮自动补给系统,并以此来提升我国无人作战设备的综合作战能力,推动无人弹药补给车的研究发展。
1 IMU与激光雷达融合SLAM方案设计
1.1 总体方案设计
根据功能需求分析,将IMU与3D激光雷达融合,设计时需考虑定位与建图的精度、实时性,室外未知环境下的可靠性,以及平台资源和计算消耗的平衡性等。设计方案主要包括硬件系统和软件系统。
硬件系统包括无人弹药补给车本体、阿克曼底盘、3D激光雷达、IMU与车载工控机。无人车结构设计时,要考虑电池部分、驱动部分,以及激光雷达扫描范围不受遮挡等。
软件系统主要包括软件运行环境即Ubuntu操作系统和ROS机器人操作系统、传感器驱动模块、通信模块以及建图模块。软件系统需具有运算速度快、数据传输稳定、可视化、远程控制等特点,因此软件系统的运行环境需具有稳定性好、能够在不同设备上分布式运行、部署简单等优点。
系统总体框图如图1所示。
图1 系统总体框图
Fig.1 Overall system block diagram
1.2 无人弹药补给车IMU与激光雷达传感器模型
1.2.1 雷达传感器模型
为满足系统的研究条件,激光扫描面要可以覆盖比较广的视野范围,测距能力和精度都有较高要求,这就需要三维多线激光雷达。本文中使用的是速腾RS-Helios(32线)激光雷达,激光雷达主要参数的具体数值如表1所示。由表1可知雷达的扫描频率和精度都很高,且测量范围又广又远,满足对激光传感器的要求。
表1 雷达主要参数
Table 1 Main parameters of radar
参数数值线数32水平视场角/(°)360激光波长/nm905垂直视场角/(°)+15～-55激光发射角(全角)水平1.4 mrad 垂直 2.6 mrsd精度(典型值)0.2～1 m:±3 cm 1～100 m:±2 cm测距能力/m0.2～150帧率/Hz10/20
1.2.2 IMU传感器模型
IMU是惯性测量单元(inertial measurement unit,IMU)的缩写,应用在自动驾驶车上的IMU,其关键特点是成本低、体积小,并且有足够高的测量精度。根据IMU 的工作特点,可以看出其工作条件对环境的依赖程度较小,因此,IMU 常用于辅助激光雷达导航系统,在缺乏卫星信号和激光退化的场景,IMU能在短时间内获得载体的运动消息。本文中使用的是XW-GI5691导航系统,其主要参数的具体数值如表2所示。由表2可知IMU的稳定性和更新频率都很高,满足设计要求。
表2 惯性测量单元主要参数
Table 2 Main parameters of IMU
参数数值陀螺仪量程±400(°)/s 零偏稳定性1(°)/h(1σ 10 s平均)0.3(°)/h(Allan Variance)加速度计量程±10 g零偏稳定性<20 μg(1σ 10 s平均)<10 μg(Allan Variance)GNSS板卡数据更新速率5 Hz
2 激光SLAM算法设计
LeGO-LOAM是一种轻型、地面优化的激光雷达建图方法,用于实时六自由度姿态估计,为应对可变地面进行了地面优化[6]。它的算法相较于LOAM算法有以下几点改进:
1) 增加地面分割(segmentation)操作,把点云投影为距离图像,分离出地面点与非地面点。
2) 平滑度计算公式不同,原始LOAM中为使用点集中的坐标相减,而LeGO-LOAM中为使用点集中的欧式距离作差。
3) 特征点选取中原始LOAM为按照平滑度的值及已有特征点数量分为平面点/边缘点两类,而LeGO -LOAM中不仅考虑到平滑度的值还同时考虑到点的类型为“地面点/分割点”,而是分别进行提取不同的特征点集。
4) 雷达里程计(lidar odometry)模块相邻帧之间特征点的对应关系也分别按照“地面点/分割点”进行寻找。
5) 雷达建图(lidar mapping)模块中LeGO-LOAM提供了基于传感器视野范围与基于图优化获取的2种获取特征对应点方法。此外,还加入了iSAM2进行后端优化(闭环检测)。LeGO-LOAM框图如图2所示。
图2 LeGO-LOAM框图
Fig.2 LeGO-LOAM block diagram
3 基于激光雷达与IMU融合的前端里程计
对2种传感器进行坐标系标定,以实现数据对齐。进而利用两传感器的优势,融合 IMU 测量数据与激光特征点观测信息[7],实现无人弹药补给车位姿的实时准确估计。
3.1 IMU与激光雷达时间同步标定
2种传感器数据的测量频率和时间间隔的准确度存在差异,因此采用时间戳插值[8]的方法实现不同测量频率的两类数据在时间坐标上的对齐。
假设t1、t2始终代表IMU数据队列里的前2个数据q1、q2的时间戳。t0为待同步点云的时间戳。由于数据传输至工控机的时间并不同步,需对t0时刻附近IMU数据的时间戳信息进行选择判新,将时间戳相对于t0时刻较早的IMU数据不断推出队列,直到获得该时刻最近的2个数据,如式(1)所示,根据最近邻2个IMU数据进行插值,进而获得激光帧对应的IMU数据:
(1)
3.2 IMU与激光雷达外参标定
外参是指传感器在空间中的相对位姿,分为平移和旋转外参,外参标定即确定不同传感器之间位姿的相对转换关系,是融合不同传感器数据的基础。2种传感器采集的数据均基于自身坐标系,利用求解的外参将IMU 数据统一至激光雷达坐标系下,再转换至移动机器人坐标系下便可实现数据统一[9],如图3所示。
图3 手眼标定
Fig.3 Hand-eye calibration
使用手眼标定原理标定外参[10],需要获得2个传感器在相同时间的位姿变化量。利用NICP算法对点云进行匹配,获得一段时间间隔Δt内雷达位姿变化量Δt1。 通过对IMU的角速度、加速度积分求得相应时间间隔姿态变化量Δt1。令四元数TL1表示IMU坐标系相当于激光雷达旋转外参,根据手眼标定原理可得:
ΔT1TL1=TL1ΔT1
(2)
由于变换矩阵T由旋转矩阵R以及平移量t构成,将式(2)展开可得:
ΔR1RL1=RL1ΔR1
(3)
ΔR1tL1+Δt1=RL1Δt1+tL1
(4)
求解出外参RL1后,并可求得平移外参。因此,构建关于旋转外参的齐次超正定方程组:
(5)
利用SVD 分解[11]对该问题进行求解,便可获得旋转外参RL1。利用求解的RL1构建关于式(4)的线性最小二乘问题,便可求解平移矩阵TL1。采集数据时,令机器人沿x抽方向进行平移,对z抽进行25 cm左右的旋转,以及绕z轴的旋转运动录制 20 s 左右试验数据,按照手眼标定原理进行外参求解。在求解获得新的外参后,利用该外参作为初值,重新迭代计算,直到外参不再变化。
标定结果如表3所示。IMU相对于激光雷达的外参的平移量与测量值相差较小,但旋转外参[12]由于难以测量,产生了较大偏差。
表3 外参标定结果
Table 3 External calibration results
外参标定前标定后横滚角(°)00.923俯仰角(°)00.800航向角(°)01.481x平移/cm0-0.023y平移/cm00.033z平移/cm-13.800-15.033
4 地图构建和因子图优化
4.1 地图构建
SLAM中构建地图[13]首先是将当前帧点云转换到世界坐标系下:
(6)
式(6)中:
和
是当前点云转换到世界坐标系下的旋转矩阵和平移向量;pi是
中的点;
是pi点转换到世界坐标系后的点。当有新的点云帧到来时需要更新旋转矩阵和平移向量,假设新进来的帧为pk+1,pk和pk+1两帧之间的变换旋转矩阵为
平移向量为
根据它们两帧之间的变换关系来更新地图坐标,旋转的更新方式为
(7)
经过上述的计算完成旋转矩阵的更新,平移向量的更新方式为
(8)
地图的构建就是每当新的一帧进来就根据位姿的相对变换关系来更新Rw和tw,然后将其交换到世界坐标系下并且和历史点云进行融合,不断地重复该过程即完成点云地图的拼接。地图的构建存在噪声,导致构建出来的地图存在偏差,通过回环检测发现闭环[14],采用因子图优化的方法对地图进行全局一致性优化。
4.2 因子图优化
因子图是一种图模型,非常适合于复杂的估计问题的建模。目前在SLAM[15]领域内广泛应用的因子图优化库是 GTSAM[16]。如图4所示,节点x4和x5对应于机器人在2个不同时刻的位姿,他们之间链接构成的边是相邻帧的里程计约束,节点x2和x5构成的边是闭环约束,将无人车的位姿信息作为变量,不同的位姿之间的帧间约束或者闭环约束作为因子。将各个约束加入到因子图中,利用因子图优化来对算法估计出的轨迹和构建的地图进行优化,使得轨迹和地图精度提高。
图4 图优化原理示意图
Fig.4 Schematic diagram of optimization principle
5 试验与结果分析
在试验中,通过无人弹药补给车实时采集校园环境数据,分别对LeGO-LOAM方案和融合IMU方案[17]测试效果,通过对比试验,将分别验证2种传感器融合前后的建图和定位精度。对LeGO-LOAM方案和融合IMU方案进行定性定量分析。
5.1 融合定位精度评价标准
本文采取定位和建图2种方式作为评价标准:
1) 在一个闭环环境中,进行激光点云建图,定性对比LeGO-LOAM方案和融合IMU方案的建图效果,以及观察运动轨迹的起始点是否闭合。
2) 定性对比2个方案各自的轨迹。
5.2 结果分析
5.2.1 建图效果
由图5可以看出,通过LeGO-LOAM方案得到的点云地图会出现重影现象,且地图出现了倾斜。对于LeGO-LOAM方案来说,IMU惯导数据作为融合定位源之一时,针对传感器的各种特性进行数据融合,得到融合后的精确定位,对定位建图有着较大影响。与未融合IMU方案相比,点云地图完整清晰,没有叠层和重影,并且可以真实反映实际环境。
图5 融合IMU前后建图效果对比
Fig.5 Comparison of imaging effects before and after IMU fusion
Lego-LOAM算法与融合IMU后的位姿绝对误差如表4所示。融合IMU后的SLAM算法的位姿绝对误差的平均值为2.597 m,根误差代表了误差的总体数值情况,融合IMU 后的SLAM算法的均方根误差为2.669 m,小于光有激光雷达的Lego-LOAM SLAM算法的4.031 m。
表4 位姿绝对误差
Table 4 Absolute pose error m
算法中位数最小值平均值均方根误差Lego-LOAM算法3.6830.194 03.7864.031融合IMU算法2.6330.5522.5972.669
5.2.2 定位效果
由图6可以看出,通过LeGO-LOAM方案得到的运动轨迹显示出轨迹的起点和终点没有重合,出现了断节。从开始行驶,位姿误差逐渐增大,在最后一个转弯处,轨迹开始出现明显的漂移,导致终点和起点相差很大,且漂移量较大。融合后的轨迹更接近优化后的轨迹。
图6 融合IMU前后轨迹对比图
Fig.6 Comparison of trajectories before and after IMU fusion
表5为不同算法在轨迹图上获得的运行终点坐标、运行轨迹长度、图上起点与终点的距离Δd以及实际距离真值。融合IMU后的SLAM算法能够准确进行回环检测,后端优化算法对位姿的修正作用明显,大幅度降低在室外大范围场景中长距离运行产生的累计误差,位姿轨迹具有一致性。
表5 融合IMU前后距离误差对比
Table 5 Comparison of distance errors before and after IMU fusion
算法算法终点坐标轨迹长度/m距离误差Δd/m距离真值/mLego-LOAM算法(-3.986,11.032,3.201)804.65211.236融合IMU算法(0.324,0.184,0.102)816.720.9820.1
6 结论
针对缺乏卫星导航信号时建图困难的问题,搭建了无人弹药补给车IMU和激光雷达融合的SLAM系统及其模型。
1) 完成了IMU和激光雷达有效选型,在ROS环境下实现了包括传感器信息采集、实时位姿估计与全局优化及建图等功能。
2) 完成了多传感器坐标变换,使用手眼标定原理和时间戳插值方法解决了2种传感器时间与空间不统一问题,可以更好地融合 IMU 测量数据与激光特征点观测信息。
3) 建图时采用基于因子图优化框架实现全局位姿优化,在前端位姿估计的基础上,进一步提高了位姿估计精度和全局地图一致性。
最后在校园里采集实时数据,验证方案效果。测试结果表明,融合IMU后的激光雷达SLAM算法的建图精度和终点距离误差与融合前相比都有明显提高,能消除在长距离运行下位姿的累计误差。
[1] WEON I S,LEE S G.Environment recognition based on multi-sensor fusion for autonomous driving vehicles[J].Journal of Institute of Control,2019,25(2):125-131.
[2] 马争光,赵永国,刘成业,等.激光和视觉融合SLAM方法研究综述[J].导航定位学报,2017,5(2):111-116.MA Zhengguang,ZHAO Yongguo,LIU Chengye,et al A review of SLAM methods for laser and visual fusion[J].Journal of Navigation and Positioning,2017,5 (2):111-116.
[3] 付心如,徐爱功,孙伟.抗差自适应UKF的INS/GNSS组合导航算法[J].导航定位学报,2017,5(2):111-116.FU Xinru,XU Aigong,SUN Wei.INS/GNSS integrated navigation algorithm for robust adaptive UKF[J].Journal of Navigation and Positioning,2017,5(2):111-116.
[4] 章大勇.激光雷达/惯性组合导航系统的一致性与最优估计问题研究[D].长沙:国防科技大学,2010.ZHANG Dayong.Research on consistency and optimal estimation of lidar/inertial integrated navigation system[D].Changsha:National University of Defense Science and Technology,2010.
[5] 金峰,蔡鹤皋.机器人 IMU 与激光扫描测距传感器数据融合[J],机器人,2000,22(6):470-473.JIN Feng,CAI Hegao.Data fusion of robot IMU and laser scanning ranging sensor[J],Robotics,2000,22(6):470-473.
[6] ZHIQIANG D,JINGYI Z,TIANCI L,et al.An intensity-enhanced LiDAR SLAM for unstructured environments[J].Measurement Science and Technology,2023,34(12).
[7] SIMEN S,TERJE D.Comparison of GPS and IMU systems for total distance,velocity,acceleration and deceleration measurements during small-sided games in soccer[J].Cogent Social Sciences,2023,9(1).
[8] JIANG Y,WANG T,SHAO S,et al.3D SLAM based on NDT matching and ground constraints for ground robots in complex environments[J].The Industrial Robot,2023,50(1).
[9] 王昌云,李利君.基于四元数的机器人手眼标定算法[J].传感器与微系统,2019,38(12):133-135.WANG Changyun,LI Lijun.Robot hand eye calibration algorithm based on quaternion[J].Sensors and Microsystems,2019,38 (12):133-135.
[10] 付瑶.激光雷达、IMU联合标定及实时点云建图方法研究[D].北京:北京建筑大学,2023.FU Yao.Research on the joint calibration and real-time point cloud mapping methods of LiDAR and IMU[D].Beijing:Beijing University of Architecture and Architecture,2023.
[11] PAWEL S,PIOTR K.LIDAR-based SLAM implementation using Kalman filter[J].Radioelectronic Systems Conference,2019,2020:11442.
[12] S
OWAK P,KANIEWSKI P.LIDAR-based SLAM implementation using Kalman filter[P].Other Conferences,2020.
[13] 云军英.基于三维激光雷达的室内地图构建算法研究[D].郑州:郑州轻工业大学,2023.YUN Junying Research on indoor map construction algorithm based on 3D LiDAR[D].Zhengzhou:Zhengzhou University of Light Industry,2023.
[14] RYOHEI S,KOICHI H.Mapping and localization for autonomous ship using LiDAR SLAM on the sea[J].Journal of Marine Science and Technology,2023,28(2).
[15] CAN W,ZHIBIN L,YEFEI K,et al.Applying SLAM algorithm based on nonlinear optimized monocular vision and IMU in the positioning method of power inspection robot in complex environment[J].Mathematical Problems in Engineering,2022,2022.
[16] DELLAERT F.Factor graphs and GTSAM:A hands-on instroduction[R].Georgia Institute of Technology,2012.
[17] 王忠博.基于激光雷达和IMU的同时定位与建图算法研究[D].鞍山:辽宁科技大学,2022.WANG Zhongbo.Research on simultaneous localization and mapping algorithms based on lidar and IMU[D].Anshan:Liaoning University of Science and Technology,2022.