非结构环境中四轮差速无人车的三维重建研究

0引言

本文主要介绍一种适用于非结构环境的四轮差速无人车,然后使用其实现了三维地图的建立和同时 定位,并进行了相应的实验。该无人车基于ROS(Robot Operating System)进行开发,ROS起源于斯坦福大学与机器人技术公司Willow Garage的个人机器人项目。它提供类似操作系统所提供的功能,包括硬件抽象描述、底层驱动程序管理、共用功能的执行、程序间的消息传递、程序发行包管理,也提供一些工具程序和库用于获取、建立、编写和运行多机整合的程序^[1]。车辆以NVIDIA Jetson TX2控制板作为上位机进行控制和数据处理,使用 自制工控机作为下位机进行数据采集和控制驱动器,装备有编码器、三维激光雷达、高精度IMU(Inertial Measurement Unit)、ZED双目立体相机等多种传感器。另外,提出了一种基于编码器和IMU融合的六自由度状态估计方法,然后使用改进后的开源算法,实现了高精度三维地图的建立和同时定位。

1四轮差速无人车

本文介绍的四轮差速无人车基于ROS开发,硬件架构如图1所示,把具备多种接口的工控机作为下位机进行数据采集,将计算能力强大的TX2控制板作为上位机。工控机和TX2都装有通用操作系统 Ubuntu 16.04和Kinetic版本的ROS系统,工控机和TX2通过网线进行通信。为提高下位机和驱动器之间的通信速率,提升车辆响应速度,特采用CAN(Controller Area Network)总线通信。无人车装备了多种传感器,包括Velodyne 16线三维激光雷达、ZED双目立体相机、Crossbow NAV440高精度IMU等。

图2是四轮差速无人车实物图,共分为三层,第一层放置通用外设、激光雷达、GPS天线和急停开关;第二层放置IMU、ZED相机、工控机、TX2控制板、 CAN分析仪、供电端口和超声波雷达;第三层放置电池、铭朗MLDS4810型驱动器、伺服电机、ROTARY OVW2—10—2MD型编码器。

2 里程计融合

里程计是车辆定位和建图的重要依据之一,所以精确的里程计是高精度地图和准确状态估计的重要保证。当车辆行驶在非结构路面上时,在世界坐标下车辆不仅有x、y方向的位移,还有z方向的位移。基于视觉的里程计和基于雷达的里程计通过帧间匹配的方式都可得到六自由度的车辆状态估计。近年来,专家学者提出了很多基于视觉的里程计算法:文献[2]使用双目相机,通过基线计算车辆运动;文献[3]使用深度相机,通过视觉和深度计算里程计;文献[4]通过使用三维激光雷达进行了里程计计算。

但是使用单一的传感器进行里程计计算会有诸多缺点。例如,视觉里程计必须在中等且稳定的光照条件下进行匹配,而雷达里程计需要通过大量的变量对运动进行解算,以保证运动不失真。为了得到高精度里程计,zhang Ji等人率先提出一种融合视觉和雷达的里程计计算方法^[5],但是这种方法需要大量的计算资源,实时性不好。

本文主要介绍一种基于编码器和IMU融合的六自由度状态估计方法,这种方法具有实时性好、精度高等特点。为简化计算,本章在未特别注明使用世界坐标系时,默认使用车辆的自有坐标系,如图3所示。

3基于编码器和IMU融合的里程计

四轮差速无人车采用四个编码器分别采集四个车轮的转速,忽略同侧前后两车轮转速差,记左侧两车轮的转速为n_L,右侧两车轮的转速为n_R。由式(1)和(2)可得左侧车轮线速度VL和右侧车轮线速度V_R,其中R为车轮半径,因左右两侧编码器安装方向相反,则测得的旋转方向相反,所以V_R为速度的相反数:

因同侧前后车轮转速近似相等,可将四轮差速模型简化为如图4所示的两轮差分模型,d为轮距的一半,r为车辆中心点到转弯中心点的距离,因左轮和右轮转弯时绕转弯中心点的角速度相同,可得式(3):

式中:W_L、W_R为左轮角速度和右轮角速度。

用V_L、V_R和d表示r,可得式(4):

则有式(5):

车辆x方向的速度Vx可由式(6)得出:

式中:W为角速度。

四轮差速无人车装备了Crossbow NAV440系列的高精度IMU,Crossbow NAV440是一款GPS/INS组合导航的产品,它综合利用了基于MEMS的惯性器件和GPS技术,即使在没有GPS的状态下,角速度测量精度也小于0.02°，加速度测量精度在运动过程中小于1 mGal。通过IMU测得车辆绕xyz旋转的角速度,对三个角速度进行积分,得到车辆的姿态,以欧拉角进行表示,具体表示为滚动角θ_R、俯仰角θ_P和航向角θ_Y。则在世界坐标系下,车辆的位移Lx、Ly和Lz分别为:

4 三维地图构建及状态估计实验

目前,有许多开源三维地图构建及同时定位的算法,比如LOAM、hdl_graph_slam等。笔者对hdl_graph_slam算法进行了改进,原有算法使用单一的三维激光雷达点云进行抽稀(Downsample)后再进行帧间匹 配,得出里程计。但这种方法需要大量的特征点进行匹配,还需要消耗大量的计算资源,而且在车速较高时会产生畸变;另外,经多次实验,用帧间匹配得到的里程计在z方向上的位移计算不准确。针对这种情况,改进为采用编码器和IMU融合计算里程计。

hdl_graph_slam算法主要分为四个部分:

1)滤波,就是通过PCL(Point Cloud Library)对原始激光点云数据进行抽稀,对不在有效范围内的无效激光进行去除(Outlier Removal)。

2)帧间匹配,每隔一段预先设置好的时间提取一帧点云作为关键帧(keyframe),在预先设置好的范围内提取特征与关键帧进行匹配,计算出有六自由度的里程计。

3)地面探测,设置激光高度阈值,将高于阈值的激光滤除,得到地面的点云数据。

4)建图,将所有匹配过的点云帧保存,形成点云地图,使用g2o图优化算法对全局地图进行优化。g2o是一个通用图优化函数库,带有多种求解器,其将优化问题以图的形式表达。把关键帧作为g2o中的“node”,将与关键帧进行匹配的点云帧作为g2o的“edge”。

具体实验步骤如下:

步骤一:使用手柄向上位机发送速度指令,使车辆绕某工厂大楼行驶一周,速度指令以Twist消息格式传递。

步骤二:工控机接收到速度指令后执行相应运动,同时采集三维环境点云数据,消息以点Pointcloud2格式传递;与此同时,根据周围环境及里程计信息,估计当前位姿信息,如图5所示。

步骤三:上位机对点云数据进行处理。

步骤四:以PCD格式保存三维点云地图,如图6所示。

本次实验建立了高精度稠密点云地图,在车辆围绕工厂大楼行驶一周后,地图形成了闭环,没有发生偏移,这说明使用编码器和IMU融合的里程计表现出很高的精度。

5结论

本文提出了一种适用于室外环境的四轮差速无人车,车辆采用ROS系统,很好地解决了机器人领域代码可移植性和复用性的问题;在硬件方面,装备了机器人行走领域所需传感器;另外,还提出了一种融合编码器和IMU状态估计的方法,并成功应用到3D SLAM (Simultaneous Localization and Mapping)算法中,然后进行了相应实验,在该实验中,车辆表现出了精度高、响应速度快的特点,可作为室外移动机器人同时定位与建图的参考。

[参考文献]

[1]胡春旭,熊枭,任慰,等.基于嵌入式系统的室内移动机器人定位与导航[J].华中科技大学学报(自然科学版), 2013,41(增刊1):254—257.

[2]NISTERD,NAR0DITSKY0,BERGENJ.visual odometryforgroundvehicleapplications [J].Journalof FieldRobotics,2006,23(1): 3—20.

[3] Huang A S,Bachrach A,Henry P,et al.visual odometry and  mapping  for  autonomous  flight  using  an  RGB—D
camera[c]//Robotics Research,The 15th International Symposium  ISRR  2017 : 235—252.

[4]ZHANG  J , SINGH   S. Low—drift   and  real—time   lidar odometry  and  mapping[J].Autonomous  Robots,2017,41
(2) :401—416.

[5] ZHANG  J,SINGH  S.visual—lidar odometry and mapping :  low—drift,robust,and  fast[c]//2015  IEEE  Interna— tional   conference   on   Robotics   and   Automation (IcRA),2015 : 2174—2181.

2024年第19期第12篇