面向林地环境的四足机器人自主定位方法*

夏文强,王书涵,曾理湛,罗 欣

(华中科技大学机械科学与工程学院,湖北 武汉 430074)

1 引言

林地是一种典型的野外环境,是四足机器人常见的作业场景,实现四足机器人在野外林地环境中的自主作业对于发挥和拓展其应用有着重要的价值和意义[1,2]。林地环境具有树木多且间距小、地形复杂、地面松软、凹凸不平和光线明暗变化大等特点。在这种具有挑战性的环境中,四足机器人需要更为准确和高频的定位信息来实现按设定路线的快速导航。四足机器人获取其定位信息的方法是利用传感器数据进行里程计推算,常用的传感器主要有2种:一种是内部传感器,例如惯性测量单元IMU(Intertial Measurement Unit)和编码器等;另一种是外部传感器,例如激光雷达LiDAR(Light Detection And Ranging)和相机等。在林地环境中采用基于内部传感器的方法[3,4,5]往往会因为林地地面松软、凹凸不平等使得落足点滑动进而使得定位估计值出现较大误差,因此需要借助外部传感器对腿部里程计进行修正,例如,Hartley和Wisth等[6,7]考虑采用加入视觉信息进行因子图构建的融合,结果表明惯性运动学视觉方法优于融合信息较少的方法。然而,由于基于因子图的方法使用沿整个轨迹的测量值进行平滑处理,对于实时导航控制而言,更新频率较低。在满足高频率的定位下,通常考虑采用卡尔曼滤波的方式融合观测信息,Ma等[8]提出了一种误差状态卡尔曼滤波器,将惯性测量单元、腿部运动学与立体视觉里程计相融合,但该方法未考虑林地环境特征,仍是以腿部里程计作为先验,接收外部数据后需要构建观测模型,更新协方差矩阵,模型和计算复杂度较大。

近年来,随着同步定位与建图SLAM(Simultaneous Localization And Mapping)技术的发展,在机器人上使用激光雷达或者视觉相机作为主要传感器,可以实现精确的定位,比较有代表性的SLAM方法有LOAM[9]、LIO-SAM[10]、ORB-SLAM[11]和ORB-SLAM2[12]等。采用视觉信息的ORB-SLAM类方法易受光线条件影响,无法满足机器人在林地环境执行任务的需求。因此,林地导航的首选SLAM方案是3D激光雷达,其可以不受光线影响,但输出频率较低,无法满足实时导航的需求。

因此,面向林地环境的四足机器人设定路线的快速导航问题,本文提出了一种适用于林地环境的自主定位方法,考虑了在林地环境激光雷达定位的适用性以及腿部里程计的高频连续性,改进LOAM方法,采用腿部里程计去除激光雷达的点云畸变,并分别提取林地地面和树干信息进行匹配,提高激光雷达定位精度;在激光雷达2次定位之间采用中值和窗口滤波融合腿部里程计的插值数据,提高定位频率,降低计算复杂度低的同时保证了精度和频率。

2 四足机器人自主定位总体框架

激光雷达在林地环境中可以获取丰富的特征信息,但在扫描过程中由于机器人的抖动,获取的点云会产生运动畸变。因此,本文在构建腿部里程计的同时采用其短时间位姿估计的准确性去除点云畸变,且由于林地中存在较多的树叶或杂草等,其不能作为特征提取的一部分,考虑提取较为明显的树干特征和地面特征进行匹配,可以提高激光雷达精度。在保证激光雷达精度和时间对齐的基础上,推导2次定位之间腿部里程计多次位姿变换,采用中值和窗口滤波融合其插值数据,可以提高其频率,从而满足四足机器人设定路线快速导航的定位频率和精度。本文方法的框架如图1所示,主要分为3部分:第1部分是通过扩展卡尔曼滤波构建腿部里程计;第2部分是对于激光雷达三维点云,去除畸变,构建激光里程计,图1中,tx、ty和tz分别表示激光里程计在x、y和z方向上的位移,θroll、θpitch和θyaw分别表示激光里程计的横滚角、俯仰角和偏航角;第3部分是将两者融合得到最终的里程计。

Figure 1 Framework of quadruped robot autonomous localization method图1 四足机器人自主定位方法框架

3 四足机器人自主定位方法实现

3.1 基于卡尔曼滤波的腿部里程计

四足机器人结构简图和简化坐标系如图2所示,机器人的质心位置和IMU基本重合。图中,l1、l2、l3分别表示为对应关节之间的连杆长度。机器人的质心坐标系为{OB}。以右后腿为例分析,设腿部坐标系为{OH},髋部横滚关节坐标系{O0}与坐标系{OH}之间存在绕x轴的横滚角θ1,髋部俯仰关节坐标系{O1}与{O0}之间存在绕z轴的俯仰角θ2,膝部俯仰关节坐标系{O2}与{O1}之间存在绕z轴的俯仰角θ3,足端坐标系{O3}与{O2}不存在角度变换。每个角度均可以通过编码器获取。

Figure 2 Structural diagram of quadruped robot图2 四足机器人结构简图

(1)

(2)

其中,si表示第i条腿相对于机器人质心坐标系的位置向量,equ(·)表示基于上述运动学求解si所建立的方程,ni表示噪声,将四足机器人存在的滑移等效为噪声[4,14],假定滑动的大小为0.3～0.5步长的高斯噪声。在本文中不考虑加工、装配和编码器噪声等误差。

同样式(2)也可以表示为在世界坐标系下机器人落足点的位置pi减去机器人质心位置r的差值,即:

si=RT(pi-r)+ni

(3)

其中,R表示机器人质心在世界坐标系下的姿态。

用v表示机器人质心的速度,定义机器人的状态向量和观测变量分别如式(4)和式(5)所示:

x=[r,v,p1,p2,p3,p4]

(4)

z=[r-p1,r-p2,r-p3,r-p4,v1,v2,v3,v4]

(5)

其中,v1～v4分别表示四足机器人中的第1～4条腿的速度。

由IMU预积分可得到第k次迭代的先验预测方程为:

(6)

(7)

由式(3)可以得到观测方程的更新为:

zk+1=Ck+1xk+1+wk+1

(8)

其中,C表示状态观测矩阵,wk+1表示观测误差。由此可以得到2种不同传感器信息的更新方程,采用卡尔曼滤波[15]融合,状态向量中的r与R即表示机器人在世界坐标系下的速度和位姿。

3.2 基于特征点云匹配的激光雷达定位

3.2.1 点云畸变去除

树林环境特征丰富,可以获取大量稳定有效的点云信息,但由于四足机器人在林地环境运动中的机身抖动和滑移,且无法保证匀速运动,因此需要对激光雷达点云进行运动畸变去除,采用3.1节中获取的腿部里程计进行点云畸变去除。相应符号设定如下:

(1)Pk表示在第k次扫描周期的所有点云集合。

(2)一次扫描的时间段为[t0,t1],在这个时间段内得到时间对齐后的腿部里程计位姿集合为T={Tj|j=0,1,…,m},其中j=0时对齐时间为t0,j=m时对齐时间为t1。

由集合T可以推导得到在不同时刻下相对t0时刻的位姿变换如下:

(9)

Figure 3 Time series diagram of point cloud by LiDAR图3 激光雷达采样点云时间序列图

Pk中每个点的采样时刻很难精确获取,使用其在扫描中所对应的水平角来估计在时间段[t0,t1]中的线性占比,即可对应至集合T中相对位姿变换下标j为:

(10)

Figure 4 Schematic diagram before and after point cloud distortion removal图4 点云畸变去除前后示意图

3.2.2 林地地面环境和树干特征提取

激光雷达在林地环境中获取稳定有效的特征,对实现其精确定位有较大的影响[16]。因此,在林地环境中获取准确可靠的地面和树干特征,去除如树叶、草丛等不稳定的特征,可以避免错误匹配,提高定位精度。

对于地面特征,本文采用以模型拟合为基础的RANSAC(RANdom SAmple Consensus)算法[17]进行获取,机器人搭载16线激光雷达离地面高度h大约为0.5 m,取激光雷达下面6根线束,则前方地面点云x方向的范围为z/tan 15°～z/tan 3°,即1.87 m～9.54 m。根据此距离进行范围限定并对选定点云的z方向排序,选取高度较小的部分点云进行平面拟合得到方程a0x+b0y+c0z+d0=0。

在激光雷达前后限定范围内的第l个点云pl=[x,y,z],如果是平面点则与平面的距离满足一定阈值dmax,且由于地面估计参数的不确定性,随着点云与坐标原点p0=[x0,y0,z0]的距离越远,其为平面点的可能性越低,则该点与原点的连线和拟合平面的夹角也应该满足一定阈值θmax,即存在以下约束公式:

(11)

在林地环境中另一个重要特征是树干,属于一类特征。对于激光雷达光束而言,如果从同一个物体反射得到较多近邻点则这些点云属于相同分割。将激光雷达点云按照线束投影为二维深度图像,其中图像的行数为线束,列数为总旋转角除以水平分辨率,像素为点的深度。采用聚类分割方法[18]进行树干的点云分割,原理如图5所示。在图5中右侧部分为示意的树干特征,A和B表示任意2个邻近点,d1和d2表示连续且相邻的2道激光束。连接A、B为直线LAB,α为OA与OB的夹角,β为OA与LAB的夹角,设定阈值为βmin,如果β>βmin且A和B之间的欧氏距离较小,则认为A和B均属于同一个物体,也即存在约束公式如式(12)所示:

β=arctan(d2sinα/(d1-d2cosα))>βmin

(12)

Figure 5 Schematic diagram of tree trunk feature point cloud segmentation图5 树干特征点云分割原理图

由上述方法得到分割后的地面点云和树干点云分别如图6和图7所示。可以看出分割得到的地面特征和树干特征较为明显且合理,为激光雷达点云匹配提供了可靠和明确的点云。

Figure 6 Woodland environment feature point cloud图6 林地地面环境特征点云图

Figure 7 Point cloud map of tree trunk characteristics in woodland environments图7 林地环境树干特征点云图

3.2.3 前端里程计

本文借鉴LOAM思想[9],采用点的曲率评估局部表面的平滑度,并以此提取当前的特征信息,点的曲率计算公式如式(13)所示:

(13)

假设Ek和Hk分别表示第k次扫描中曲面特征点集合和平面特征点集合,Ek+1和Hk+1分别表示第k+1次扫描提取到的曲面特征点集合和平面特征点集合。第k+1帧相对于第k帧的旋转运动采用3-2-1欧拉角表示,变换矩阵为:

(14)

(15)

其中,c′=cos(·),s′=sin(·)。R表示旋转矩阵,t表示3×1的位移向量。为了求解平移和旋转6个变量,对于曲面特征匹配,采取点到直线的匹配来估计[tz,β,γ];对于平面特征匹配,采取点到平面的匹配来估计[tx,ty,α],其中β,γ和α分别表示为横滚角、俯仰角和偏航角。构建距离约束关系如式(16)所示:

(16)

其中,fE(·)和fH(·)表示的是约束关系,优化变量是dE和dH。构建约束关系如图8所示,其中,图8a表示曲面特征点r和q到上一帧最邻近点p组成直线的距离约束关系,图8b表示平面特征点p到上一帧3个邻近点s、q、r组成平面的距离约束关系。根据点到直线和点到平面所对应的求解公式即可建立优化方程。

Figure 8 Schematic diagram of feature point distance constraints图8 特征点距离约束示意图

(17)

其中,0表示起始帧,激光雷达以10 Hz频率不断获取每一帧的点云信息,计算2帧之间的位姿变换,根据上述公式递推得到前端里程计。

3.2.4 后端里程计

为了更多利用每一帧得到的信息,在后端考虑利用若干历史帧(在这里选取前面5帧)的信息,从效果上相当于一个滑动窗口,里面存放了当前的局部地图,其不局限于相邻帧的间距,而是扩展到了相邻若干帧的间距,帧图匹配精度比帧间匹配精度高很多。

后端优化的思路与前面雷达前端里程计的思路基本一致,对于当前帧的边缘特征点与平面特征点,同样是从第k-5帧到第k帧所有特征点云所组成的地图中寻找当前帧的匹配特征点建立距离约束的残差表达式。最终通过高斯牛顿迭代求解优化得到6个变量值,获取雷达后端优化的齐次变换矩阵。后端输出的激光雷达里程计频率较低,可以结合前端里程计进行融合,具体计算如式(18)所示:

(18)

3.3 基于中值和窗口滤波的融合里程计

4 机器人林地环境仿真与实验

4.1 仿真实验与分析

本文仿真环境为Webots,在仿真中可以获取四足机器人搭载激光雷达的点云数据、IMU数据、编码器数据和真实位姿值。基于ROS(Robot Operating System)发布和接收腿部里程计、激光雷达里程计(LOAM方法)、融合里程计以及真实值。四足机器人林地仿真环境如图10所示。分析四足机器人自主定位的效果,控制四足机器人直线运动,在这里对比四足机器人定位变量x,y,如图11和图12所示,其中不同的线型表示不同的里程计值,从图11和图12中可以看出,在前面时间中有一部分省略,因为在此方向上基本无运动。

Figure 10 Establishment of simulation environment in Webots图10 Webots仿真环境的搭建

Figure 11 Variation of localization variable x with time图11 定位变量x随时间变化图

Figure 12 Variation of localization variable y with time图12 定位变量y随时间变化图

由图11和图12可以得到,融合里程计的定位值不会像腿部里程计一样累计误差,在某些时刻接近真实值。计算得到不同里程计下x,y方向绝对定位均方根误差RMSE(Root Mean Square Error)如表1所示,可以得到在y方向上融合定位的效果比激光雷达定位效果较好,在x方向上定位效果接近,整体上提高了激光雷达定位精度,且融合频率接近腿部里程计的频率,在仿真中为500 Hz。

Table 1 Root mean square error of location with different odometers in x and y directions表1 不同里程计在x,y方向定位的均方根误差

4.2 实验验证与分析

本文采用宇树公司出品的四足机器人AI进行实验。图13所示是系统的整体硬件框架。编码器角度通过接口程序获取,激光雷达是速腾聚创公司的RS-LiDAR-16,IMU型号为MTi-G-710,AGX与交换机通过网线连接四足机器人,远程终端调试使用代码,编译后发送控制指令,实现四足机器人的运动切换,实验环境和平台如图14所示。

Figure 13 Hardware configuration framework图13 硬件配置框架

Figure 14 Experimental environment and platform图14 实验环境和平台

根据实际情况,设定路线为x方向10 m,y方向4.5 m,控制机器人按照长方形路径行走4圈(实际中会存在小范围的偏差),得到不同定位下的曲线如图15所示。

Figure 15 Location curves with different odometers图15 不同里程计下的定位曲线图

计算得到不同里程计下的绝对轨迹误差ATE(Absolute Trajectory Error)曲线如图16所示(以第3圈为例),其中不同线型表示不同里程计。可以得到腿部里程计绝对误差均大于激光雷达里程计和融合里程计,融合里程计绝对误差小于激光雷达里程计。腿部里程计在不同的转弯处与实际值的偏差会存在一定的波动,因为打滑存在不确定性,且在直线运动时会存在某个方向的偏差较小。

Figure 16 Absolute error of the third circle in robot walking with different localization odometers图16 不同里程计下机器人行走第3圈的绝对误差

每一圈回到起点的绝对轨迹误差如表2所示。从表2中可以看出,激光雷达里程计和融合里程计最大均方根误差分别为1.36 m和0.19 m,均优于腿部里程计,且最终融合里程计精度相较于激光雷达里程计提升了94%。从图15中也可以得到融合里程计更接近真实值,符合机器人实际行走轨迹。

Table 2 Absolute trajactory error with different odometers returning to the starting point at each turn表2 不同里程计在每一圈回到起点的绝对轨迹误差

根据林地实际状况,设定的直线路线与上述验证方法相同,路线上存在树木并且需要进行穿越,设定四足机器人行走速度为1.0 m/s,转弯半径为1.5 m,最大转弯速度为0.6 rad/s,四足机器人采用相同的控制方法,得到在不同里程计下的定位数据如图17所示。由图17可以得出,在四足机器人设定路线快速导航中且在相同的控制方法和规划下,通过激光雷达定位在前期能保持较好的定位精度。但是在避开树木转弯的过程中,由于定位频率较低,局部导航无法顺利调节,导致转弯性能较差,最终由于撞到树木而无法完成任务要求。通过融合里程计和腿部里程计的定位方法,能够进行顺利调节,完成相应的工作任务,但从图中得到腿部里程计误差偏移较多,机器人在实际实验运动过程中非常接近树木,最大误差为0.8 m,而融合后的里程计最大偏移为0.2 m,在保证高频率的前提下,仍然有着较高的精度,顺利地完成了四足机器人在林地环境中的设定路线快速导航任务。

Figure 17 Location values and expected location values of quadruped robot with different localization odometers图17 不同里程计下的四足机器人定位值和期望定位值

5 结束语

面向林地环境的四足机器人自主定位问题,本文利用多种传感器的数据,对于编码器和IMU采用卡尔曼滤波的方法,而对于激光雷达的点云数据,考虑了去除畸变,提取林地地形和树干特征进行匹配,并采用中值和窗口滤波的融合方法,该方法可以较好满足四足机器人自主定位的实时性和精确性,弥补了激光雷达定位频率低和腿部里程计定位累计误差大的不足。本文通过仿真和实验验证了该定位方法的有效性,并在设定路线的快速导航中验证了该方法的实用性。

由于林地环境的复杂性、机器人续航等因素限制,本文将打滑简化处理为高斯噪声,但是实际过程中四足机器人运动中加减速的不对称,是一个有偏的模型,尽管如此并不影响本文结论的正确性。对于腿部里程计可能存在的打滑以及长距离下定位漂移等问题,应考虑建模进一步减少位姿估计误差或者融合其他传感器信息修正。在实际导航中期望定位值与里程计定位值的偏差存在一定波动,后续考虑紧耦合并和结合滤波的方法进行多传感器数据的融合,可以达到更好的效果和实用性。