基于激光与RGB-D相机的异构多机器人协作定位

,, ,

(浙江工业大学,信息工程学院,浙江 杭州 310023)

多机器人协同定位是利用系统中某些机器人的高精度定位,精确的导航能力,和引入相对观测量(相对距离与相对方位),通过彼此之间的信息交互，使装备低精度定位设备的机器人提高定位精度。当某机器人由于自身传感器故障导致自身定位失效时,协同定位可以在一定程度上恢复其定位能力。在多机器人协同工作的实际场景中,考虑经济成本,不可能为每台机器人都配备高精度的定位传感器。通过给部分机器人安装高精度传感器,使用协同定位方法可以提高机器人平台整体的定位精度。因此研究多机器人协同定位具有重要的理论价值与实际意义[1-2]。目前,对于多机器人协同定位技术研究常用的方法:1) 同构分布式协同定位,这些方法使用的都是同构系统,要求是相同的传感器,每个机器人各自处理自己所观测的信息,这样可以充分地利用各个机器人的观测信息。提高整个平台的精度。Roumeliotis等[3]提出了一种分布式的扩展卡尔曼滤波(EKF)形式[4-5],Hage等[6]提出的容错协同定位也是使用扩展卡尔曼的形式,这些文献都使用这种方法。但是这种方法需要实时更新每一个机器人的观测信息,计算量增大,计算复杂度高。2) 异构集中式协同定位,将所有机器人的信息同一集中处理,可以快速地处理信息,去除冗余的信息。如Wanasinghe等[7]提出的异构机器人系统的分布式协同定位使用的是异构集中式定位方法。这种方法虽然计算快,实现简单，但是一旦系统崩溃,很难恢复,鲁棒性较差。所以针对这种问题,蒋荣欣等[8-9]使用多传感器融合方式进行协同定位。利用多传感器融合技术弥补由于单个传感器问题导致的系统崩溃问题。传统的融合方法如联邦滤波的方式[10]信息分配系数是固定值,实际中各子系统的定位精度每时每刻都不同,导致整个系统的精度下降。分布式矩阵加权的方式[11]要计算各个子系统之间的互协方差,计算复杂,实时性较差。

针对上面研究存在的问题,提出了一种激光与RGB-D相机融合的多传感器异构定位系统,克服单传感器精度不高的缺点。首先建立移动机器人运动模型,再根据移动机器人配置的传感器,结合不同传感器的测量原理分别建立了RGB-D相机观测模型与激光传感器观测模型,得到系统状态观测方程。由于移动机器人运动模型是高度非线性模型,而传统的EKF线性化处理导致非线性误差较大,采用无迹卡尔曼滤波器获得局部估计值。为了处理多传感器数据,使用协方差交叉融合方法(CI)[12-15],CI融合不需要计算各子系统的互协方差,计算量小,鲁棒性强,实时性高,克服了计算复杂的互协方差的缺点。最后搭建了实验平台并在该平台上验证了所提出算法的可行性与优越性。

1 问题描述与建模

1.1 系统描述

所考虑的异构机器人协作定位系统如图1所示,其中的机器人i配备了高精度里程计,以及外部传感器激光雷达和RGB-D相机。机器人j只配备低精度里程计。当机器人i与机器人j同时运动时,为了使低精度定位能力的机器人j提高定位精度,机器人i通过RGB-D相机、激光雷达获取机器人j的位置信息,协同机器人j获取更高精度的定位信息。针对该问题,提出了激光与视觉融合的异构多机器人协作定位方法。

当前采样时刻,机器人j利用自身的里程计信息与机器人运动模型估计出自身的位置信息。机器人i先利用RGB-D相机获取机器人j与机器人i的相对距离、相对方位角,通过RGB-D观测模型与机器人j的运动模型组成RGB-D局部估计器,得出第1组关于机器人j的位姿估计与估计方差,如图1中灰色三角与虚线椭圆所示,再通过激光雷达获取机器人j与机器人i的相对距离、相对方位角。通过激光观察模型与机器人j的运动模型组成激光局部估计器,得出第2组关于机器人j的位姿估计与估计方差,如图1中灰色三角与实线椭圆所示。由于噪声的影响,两组估计都不准确。为了获得更高精度的位姿估计,将两组位姿估计值进行二次融合,如图1虚线三角所示,从而得到精度更高的机器人j的位姿估计。为了实现异构多机器人协作定位方法需要建立机器人的运动模型,并且针对不同传感器特性建立各自的观测模型。

图1 异构多机器人协作定位框架图Fig.1 Framework of cooperative localization for heterogeneous multi robots

1.2 机器人运动模型

考虑两轮差速轮式移动机器人,假设机器人的质地均匀,质心为几何中心点,且机器人运动过程中左右过程中左右两轮与地面纯滚动无滑动(图2)。图2中:R为车体运动的圆弧半径;Δx,Δy,Δθ分别为单位时间Δt内车体位置横、纵坐标和方位角的变化量。当走直线或采样时间足够短时,里程计通过记录左右轮的转动里程,得到移动机器人的角速度与速度。

图2 运动模型Fig.2 Motion model

定义移动机器人的位姿为Xk=[xk,yk,θk]T为k时刻移动机器人的横坐标、纵坐标和航向角。如图2所示,状态方程[16]可描述为

X(k+1)=f(X(k),u(k))

(1)

(2)

式中:u(k)=[v(k),w(k)]T，其中v(k)=ΔL=(ΔSl+ΔSr)/2，w(k)=Δθ=(ΔSl-ΔSr)/d，而ΔSl,ΔSr分别为移动机器人在k时刻的左右轮里程计的变化值，d为2轮轴距。

2 协作定位方法

图3 协同定位算法框架Fig.3 Framework of cooperative location algorithm

2.1 基于RGB-D相机的局部估计器1

2.1.1 RGB-D相机传感器模型

使用RGB-D相机的红外相机可以获得场景的深度信息,从而求得空间中某点的坐标,如图4所示。

图4 RGB-D测距原理Fig.4 RGB-D camera measuring principle

以RGB-D红外相机中心为坐标原点建立坐标系,取红外相机指向红外发射的方向为X轴,Z轴垂直于图像平面,当物体恰好位于参考平面时,记录下它的散斑图案。当物体位于平面时,它的散斑图像在X轴上产生一个视差,通过三角形相似原理可知表达式为

(3)

(4)

式中:Z2为物体在物理平面的深度值;b为基线长度;f为红外相机的焦距;D为物体在物理平面的位移;d为散斑图案在图像平面的视差。由式(3,4)可得

(5)

式中:Z1,f,b通过校准得到。场景中某点的深度值和相机的焦距f共同决定了该点的成像度,可得出X和Y轴的坐标为

(6)

(7)

式中:x,y分别为该点在图像坐标系上的坐标;x0,y0分别为图像坐标系的原点坐标。

x0,y0可以通过上述原理求出(x0,y0是以RGB-D为原点的局部坐标),如图5所示可得

(8)

(9)

所以局部估计器1在k+1时刻对机器人j的量测信息为

(10)

图5 RGB-D观测模型Fig.5 The observation model of RGB-D camera

2.1.2 RGB-D局部估计器设计

UKF滤波方法是一种基于UT(Unscented transform)变换的非线性滤波方法。UKF是一种递归式贝叶斯估计方法。相比于EKF,UKF不必计算Jacobi矩阵并且估计精度和收敛速度都得到了提高,也更容易实现。

算法1RGB-D相机局部估计器

步骤1状态预测。其表达式为

(11)

Xj(k+1|k)=f(X(k),u(k))

(12)

(13)

(14)

步骤2量测更新。其表达式为

Zj(k+1|k)=H(Xj(k+1|k))

(15)

(16)

(17)

P1(k+1|k+1)=P1(k+1|k)-KPZZKT

(18)

(19)

P1(k+1|k+1)=P1(k+1|k+1)

(20)

(21)

(22)

(23)

根据RGB-D测距原理将k+1时刻机器人i的位置[x(k+1)i,y(k+1)i]T代入RGB-D观测模型中得到k+1时刻的量测信息Z(k+1)=[ρij,βij]T。

2.2 基于激光传感器的局部估计器2

2.2.1 激光传感器模型

如图6所示,由于激光传感器坐标系的形式为极坐标系统,采集的数据用T={ρi,i|i=1,2,…,n},其中ρi为激光测量的长度值,i为每束激光的数据集的下标。

图6 激光测量模型Fig.6 Laser measurement model

(24)

2.2.2 激光局部估计器设计

算法2激光局部估计器

步骤1状态预测。其表达式为

(25)

Xj(k+1|k)=f(X(k),u(k))

(26)

(27)

(28)

步骤2量测更新。其表达式为

Zj(k+1|k)=H(Xj(k+1|k))

(29)

(30)

(31)

(32)

P2(k+1|k+1)=P2(k+1|k)-KPZZKT

(33)

(34)

P1(k+1|k+1)=P1(k+1|k+1)

(35)

式中：Zj(k+1|k)=H(Xj(k+1|k))因传感器的测量原理不同,计算的方法也不同。激光观测模型的量测信息通过下面方式进行更新:

1) 在k+1时刻得到机器人i的位置[x(k+1)i,y(k+1)i]T。

5) 得到[ρ(k+1)ij,β(k+1)ij]T,其中ρ(k+1)ij=ρcenter,β(k+1)ij=icenterΔθ-π/2。

6) 将所得到的量测信息Z(k+1)=[ρij,βij]T以及将k+1时刻机器人i的位置信息[x(k+1)i,y(k+1)i]T代入激光观测模中。

2.3 全局融合算法

得到两个局部估计之后,将两个局部估计值进一步融合。由于受到共同的过程噪声影响,两个局部估计结果是相关的,而各局部估计间的互协方差计算复杂,考虑到移动机器人的实时性要求高的特点,采用CI融合算法得到更精确的估计。基于激光传感器与RGB-D相机的全局估计器设计算法为

PCI(k+1|k+1)=[ωP1(k+1|k+1)-1+
(1-ω)P2(k+1|k+1)-1]-1

(35)

xCI(k+1|k+1)=PCI(k+1|k+1)[ωP1(k+1|k+
1)-1×x1(k+1|k+1)+(1-ω)P2(k+1|k+
1)-1x2(k+1|k+1)]

(36)

式中:x1(k+1|k+1),P1(k+1|k+1)分别通过RGB-D相机局部估计器算法得到;x2(k+1|k+1),P2(k+1|k+1)分别通过激光局部估计器算法得到。使用黄金分割法[17]等方法快速求最小值点ω=0.542 5。得到机器人j最优估计位姿为xCI(k+1|k+1),与估计的方差阵PCI(k+1|k+1)。

3 实 验

3.1 实验硬件与环境

本实验使用的是两台DFROBOT，如图7所示.2台DFROBT都装备里程计,其中一台DFROBOT为机器人i装备RGB-D相机;型号为华硕Xtion-pro live,装备型号为HOKUYO URG-04LX-UG01的激光雷达;使用的系统环境是ubuntu 14.04,采用indigo版本的ROS系统[18-19],opencv 3.0,深度相机的驱动库openNI。为了能够有良好的人机交互,将ROS与QT5.7结合使用,编写上位机程序,直观的实时体现机器人的位姿信息。

图7 DFROBOT机器人Fig.7 DFROBOT

首先建立1个名为/Robot的ROS节点(用于接受ROS发布的其他节点信息),利用ROS订阅/M1Speed的左轮线速度、/M2Speed右轮线速度的节点信息。然后为了方便识别目标机器人j,在机器人j尾部安置1个绿色矩形标志物。通过图像处理检测出绿色标志物(图8),提取RGB-D量测信息。接着订阅激光节点/hokuyo_node的/scan话题,提取激光量测信息。最后使用所提方法估计出机器人j的位姿,并在上位机上实时显示。

图8 标志物检测与选定Fig.8 Detection and selection of signs

3.2 实验场景与结果

实验选择为空旷的室内环境,两台DFROBOT都按照指定的轨迹进行移动,两个移动机器人移动速度均≤200 mm/s,机器人j从坐标(0,-1)开始做半径为1.2 m的圆周运动;机器人i从坐标(-1,-1.5)开始,跟踪并观测机器人j进行实验。实验对比如图9所示。

图9 实验结果Fig.9 Experimental results

图9(a)为融合估计与RGB-D估计器估计轨迹,RGB-D滤波器估计轨迹一直在真实轨迹周围。说明RGB-D相机识别出了机器人j,没有丢失目标。因为受到光照强度的影响,在提取机器人j尾部标志物中心点时数据不稳定,容易跳变,所以导致了RGB-D滤波器估计值方差大,轨迹波动明显。而融合后的轨迹方差比RGB-D滤波器的估计方差要小,融合后轨迹更加贴合真实轨迹。

图9(b)为融合估计与激光估计器估计轨迹,虽然激光雷达没有大的数据跳变,但是激光只能测量到机器人的边框。因为需要定位的是机器人的几何中心点,机器人本身宽28 cm,所以无法准确地定位到目标中心,从而导致了定位结果有一定的偏差。雷达精度比视觉高,所估计的轨迹波动不大。融合后的轨迹比激光滤波器估计的轨迹更加贴近真实轨迹,很好地结合了两个子滤波器的优点,抑制了其缺点。

图9(c)为RGB-D滤波器、激光滤波器与融合估计的RMS均方根误差。RGB-D滤波器的均方根误差最大E(RMS)=0.391 6 m,激光滤波器E(RMS)=0.196 3 m,运用全局融合算法(CI)计算后的E(RMS)=0.178 1 m。

4 结 论

提出的基于激光与RGB-D相机的异构多机器人协作定位方法达到了精确定位的目的,验证了算法的可行性。融合视觉与激光传感器,获得了视觉提取目标容易的优点,又抑制了视觉收到光照等环境影响导致的估计方差过大的缺点,同时拥有激光估计器的方差小的特点,克服了激光识别目标困难以及无法定位到机器人几何中心的缺点。全局融合(CI)算法比其他的融合方法计算简单,实现容易,在保证定位精度的同时还提高了系统的实时性。