基于激光雷达数据的特征地图创建

基于激光雷达数据的特征地图创建

0创建二维特征地图环境建模是移动机器在未知环境中执行独立研究任务的必要过程。环境模型的表达分为平面地图和三维地图。平面地图包括资源地图、网格地图和作战地图，三维地图包括三维几何地图和可视化地图。文中介绍了移动机器人在室内环境下创建二维特征地图的详细过程。特征地图主要通过点、直线、角等几何图样来表示环境,该移动机器人创建的地图以直线特征作为主要的环境描述特征。在相对结构化的室内环境如走廊、办公室等,直线特征明显且易于提取,具有计算量小和地图所占存储空间小的特点。移动机器人采用二维激光雷达获取环境信息,通过里程计和电子罗盘推算机器人的位姿,绘制环境特征地图。每获取一帧激光雷达数据,首先对数据进行滤波预处理和坐标变换,然后采用创建直线模板的方法进行数据点分簇,得到一系列直线模板,再对每组模板的数据进行最小二乘法拟合,得到相应的直线参数,最后进行线段相关性分析,融合相关性强的线段,从而实现全局地图更新。1机器人运动学模型与传感器模型的分析1.1带横向刮动地面该移动机器人采用左右两侧履带差分驱动形式,与通过全方位轮或者导向轮来实现转向的轮式移动机器人在运动学上有着较大的区别,履带式移动机器人的转向通过左右两侧履带差分运动实现的,即转向时接触地面的履带要横向刮动地面,使地面对机器人产生一个较大的摩擦力。此外履带式机器人转向时履带存在滑转和滑移的现象,这是不可忽略的。假设机器人在水平地面运动,其在XOY坐标系中的运动示意图如图1所示。机器人的位姿可由向量P=[xC,yC,θC]T表示,其中(xC,yC)是其质心C在XOY参考平面的投影坐标,θC为航向角。左右履带的理论速度分别为νL和νR,实际速度分别为ν′L和ν′R,滑移率分别为kL和kR,D是机器人主体的宽度,b是单履带的宽度,则机器人以质心为参考点的运动学模型为:其中,kL=νL-ν′LνL,kR=νR-ν′RνRkL=νL−ν′LνL,kR=νR−ν′RνR。1.2位姿的变化里程计的工作原理是根据安装在驱动电机的编码器来检测机器人在一定时间内转过的弧度,进而推算机器人相对位姿的变化。设机器人在k时刻和k+1时刻位姿分别为Xk=[xk,yk,θk]T和Xk+1=[xk+1,yk+1,θk+1]T,机器运动的距离ΔS=(ΔSR+ΔSL)/2,转过的角度Δθ=(ΔSR-ΔSL)/(D+2b),其中ΔSL和ΔSR分别为左右履带考虑滑移后的运动距离,得出里程计的圆弧模型为:1.3激光雷达坐标系中的坐标系激光雷达在其扫描平面按一定的角度分辨率进行距离扫描,扫描数据通过极坐标形式sn=(dn,ϕn)T表示,转换为雷达直角坐标系中的坐标为:ΡnL=(xnL,ynL)Τ=(dncosϕn,dnsinϕn)Τ,n=1,...,Ν(3)PnL=(xnL,ynL)T=(dncosϕn,dnsinϕn)T,n=1,...,N(3)通过坐标变换得到扫描点在机器人坐标系和全局坐标系中的坐标PnRnR和PnGnG,见式(4)和(5),其中[a,b,θL]T是激光雷达在机器人坐标系中的坐标,[xC,yC,θC]T是机器人在全局坐标系中的坐标。1.4方位角变化量电子罗盘和里程计通过加权方式提供机器人方位角变化量,即Δθ=λΔθc+(1-λ)Δθe,其中λ和1-λ为各自权重。罗盘提供的俯仰角和横滚角可防止机器人侧翻。2创建特征地图2.1直线模板分簇该激光雷达量程为20mm～5600mm,其数据可能包含部分无效数据,如超出量程测得的数据和孤值噪声等,需对数据进行必要的预处理,通过设定一阈值dth来处理超出量程的数据,即:dn=5600mm,当dn<dth。滤波除噪的公式见式(6)。然后通过创建直线模板对雷达扫描数据点进行分簇,不同的簇用于拟合不同的线段。直线模板分簇算法的伪代码如表1所示。数据分簇之后,首先剔除点数过少以及首尾点距离过短的的直线模板,再通过公式(7)利用最小二乘法拟合直线成y=kx+b形式,然后让模板的首尾点向直线做垂线,取垂足作为新线段的端点,计算新线段的长度并剔除过短的线段,最后将新线段添加到局部地图的线段链表末尾,进而获得本次扫描的局部地图。图2是由某帧雷达数据经直线特征提取后创建的局部地图。2.2旧线路组成折中段新提取的线段应首先与先前地图中的线段进行合并,这通过两个步骤进行:一是分析线段的相关性,二是对相关性强的线段实行合并。线段之间的相关性主要取决于两线段之间的距离和线段的方向,如图3所示,其中x和y是两线段长度,a、b、c、d是线段端点的连线长度,T是设定的阈值。两线段之间的接近程度可通过式(8)中的4个不等式来判断,当至少满足其中两个条件时,即认为两线段距离足够接近,可进行下一步的线段方向判断。a+b<x+Τ,c+d<x+Τ,a+c<x+Τ,b+d<x+Τ(8)a+b<x+T,c+d<x+T,a+c<x+T,b+d<x+T(8)线段方向的判断主要考虑线段所在直线的两个参数:斜率k和截距b。对于两条斜率截距分别为(k1,b1)和(k2,b2)的直线,令Δk%=|k1-k2k1|×100%Δk%=∣∣k1−k2k1∣∣×100%,Δb%=|b1-b2b1|×100%Δb%=∣∣b1−b2b1∣∣×100%,正常情况下若满足Δk%<A且Δb%<B(A和B是设定的接近0的阈值),即认为两直线方向基本一样。当两直线与x轴接近平行或y轴接近平行时,则用直线与x正轴的夹角来判断二者方向是否接近,令θ1=arctan(k1),θ2=arctan(k2),Δθ=|θ1-θ2|Δθ=|θ1−θ2|,若满足Δθ<C(C是设定的阈值),则认为两直线方向基本一致。相关性分析完后,若两条件同时满足,则认为两线段的相关性强,可进行合并,反之作为两条独立的线段。线段的合并工作是生成一条新的折中线段替代两条旧线段。新线段所在直线的斜率k′和截距b′由旧线段加权得到,权重由旧线段长度决定,令α=dis1dis1+dis2,β=dis2dis1+dis2,其中dis1和dis2分别为两条旧线段的长度,则k′=αk1+βk2,b′=αb1+βb2。进一步确定新线段端点,分别从旧线段端点向新线段所在直线作垂线,共4个垂足,取最外端两垂足作为新线段的端点,从而生成新线段。重复相同过程,把局部地图融合到先前生成的地图中,实现全局地图的更新。3激光雷达根据上面提出的特征地图创建算法,实验室自制履带式移动机器人在办公室内自主运动探索环境,同时创建环境的特征地图。该机器人尺寸为100cm×50cm×40cm,以0.3m/s的速度运动,激光雷达采样频率为5Hz,罗盘采样频率为12Hz。在软件设计方面采用VC++6.0开发环境。图4(a)展示了特征地图的创建过程,图4(b)是办公室的最终特征地图,实验结果表明特征地图比较精准,能够较好的表示环境的轮廓,可为机器人提供路径规划等任务提供度量信息,创建的特征地图具有存储量小和计算量小的特点,适合于相对结构化的室内环境的表征。4环境特征地图构建