基于粒子滤波器的移动机器人多传感器融合定位方法
技术领域
本发明涉及移动机器人定位技术领域,尤其涉及一种基于粒子滤波器的移动机器人多传感器融合定位方法。
背景技术
移动机器人定位是确定其在已知环境中所处位置的过程,是实现移动机器人自动导航能力的关键。依据机器人所采用传感器类型的不同,其定位方式有所不同。目前应用较广泛的传感器有里程计、超声波、激光器、摄像机、红外线、深度相机、GPS定位系统等等。使用单一传感器,定位效果往往具有不确定性,尤其是服务机器人在人流量较多的场所进行定位时,定位系统的准确性和鲁棒性经受不起考验。
因此,现有技术需要进一步改进和完善。
发明内容
本发明的目的在于克服现有技术的不足,提供一种基于粒子滤波器的移动机器人多传感器融合定位方法。
本发明的目的通过下述技术方案实现:
一种基于粒子滤波的移动机器人多传感器融合定位方法,该定位方法主要包括以下步骤:
步骤S1:在定位场所的各个角落安装多个外部摄像机,使得外部摄像机的视场范围均匀地分布在整个定位场所;在定位场所的天花板安装VLC定位系统LED灯具;在移动机器人上安装2D激光定位仪、VLC定位系统信号接收器、罗盘。
步骤S2:通过外部摄像机网络使用SLAM算法构造环境的地图。
步骤S3:将机器人移动到环境中的各个位置来收集2D激光定位仪、VLC定位系统信号接收端(光电传感器)、罗盘、外部摄像机网络反馈的校准数据。
进一步的,本发明所述步骤S3中还包括以下步骤:
步骤S31:收集2D激光定位仪反馈的数据。在任何时刻,二维激光测距仪都提供了一个机器人和附近的物体之间的距离的测量数据。所有测到的距离都在同一个平面上,每个都指向一个不同的角度,并按逆时针顺序组成一个矢量信号,然后与时间戳关联作为反馈的数据。
步骤S32:收集VLC定位系统信号接收端反馈的数据。接收端经自适应滤波器分辨出各LED的ID码和对应的信号强度,按固定的ID顺序记录对应的信号强度组成一个矢量信号,然后与时间戳关联作为反馈的数据。
步骤S33:收集罗盘反馈的数据。将罗盘所显示的机器人的角度,即机器人的方向偏离当前磁场北极的角度,与时间戳关联作为反馈的数据。
步骤S34:收集外部摄像机网络反馈的数据。移动机器人上带有特定颜色的指示灯,当移动机器人进入某一外部摄像机的视野时,可通过图像分析确认移动机器人进入当前外部摄像机的视野范围,按固定的顺序记录每一外部摄像机的侦查信号(逻辑信号,1代表侦查到移动机器人,0代表没有)并组成一个矢量信号,与时间戳关联作为反馈的数据。
步骤S4:计算机器人移动的轨迹并将每个姿势与时间戳关联起来。
步骤S5:为每个传感器建立一个训练集。每个训练集包含一个与相应的姿势相关联的传感器捕捉的数据。训练每个传感器的观察模型。
进一步的,本发明所述步骤S5中还包括以下步骤:
步骤S51:建立2D激光定位仪的观察模型。根据已有的激光距离矢量与对应的位置信息,通过人工神经网络的方法建立一个以位置信息为输入,以激光距离矢量为输出的模型。
步骤S52:建立VLC定位系统的观察模型。根据已有的各LED光信号在接收端的强度与对应的位置信息,通过高斯回归的方法建立一个以位置信息为输入,以各LED光信号在接收端的强度为输出的模型。
步骤S53:建立罗盘的观察模型。根据已有的罗盘显示的机器人角度与对应的位姿信息(包括机器人的角度),通过高斯回归的方法建立一个以位置信息为输入,以角度差(罗盘显示的机器人角度与真实机器人角度之差)为输出的模型。
步骤S54:建立外部摄像机网络的观察模型。根据已有的摄像机侦查信息与对应的位置信息,通过高斯回归的方法建立一个以位置信息为输入,以每个摄像机的侦查信息为输出的模型。
步骤S6:在定位时,使用粒子滤波算法计算当前的移动机器人位姿,即在初始时刻设置位姿随机、权值相等的粒子群,在每一时刻对每一个粒子使用里程计提供的数据和该粒子上一时刻的位姿预测该粒子当前的位姿,并使用每个传感器的观察模型和当前时刻得来的传感器测量数据更新粒子的权值,根据权值对所有粒子进行重采样,以权值最大的粒子的位姿作为机器人当前时刻的定位数据。其中,在更新粒子的权值的过程中,使用每个传感器的观察模型和当前时刻得来的传感器测量数据更新粒子的权值,只取当前时刻能够收到传感器测量数据参与权值更新计算。即并不硬性规定传感器以相同的数据速率同步更新数据,允许传感器测量周期不一致。
与现有技术相比,本发明还具有以下优点:
(1)本发明所提供的基于粒子滤波器的移动机器人多传感器融合定位方法采用多种传感器的信息进行定位,即使在相似度很高的不同区域也准确识别出当前位置,大大提高了定位的准确性。
(2)本发明所提供的基于粒子滤波器的移动机器人多传感器融合定位方法采用使用多种传感器定位,避免了使用单一传感器在特殊定位区域无法提供有效信息的缺陷,使得机器人在人群拥挤的环境中仍然能实现定位,提高了定位的鲁棒性。
(3)本发明所提供的基于粒子滤波器的移动机器人多传感器融合定位方法在定位时,只考虑在那一瞬间接收到的信息,即不需要传感器以相同的数据速率同步更新数据。当一个传感器故障时,定位的准确性会降低,但定位不至于失效。这大大提高定位系统的定位效率、稳定性和可靠性。同时,因为不需要传感器以相同的数据速率同步更新数据,本发明所提供的融合定位方法能够容易地加入更多的传感器或更换其它传感器。
附图说明
图1是本发明所提供的基于粒子滤波器的移动机器人多传感器融合定位方法的流程示意图。
图2是本发明所提供的基于粒子滤波器的移动机器人多传感器安装示意图。
图3是本发明所提供的基于粒子滤波器的移动机器人多传感器融合定位方法的数据采集和模型训练示意图。
图4是本发明所提供的粒子滤波算法示意图。
上述附图中的标号说明:
1-2D激光定位仪,2-VLC定位系统,3-LED灯具,4-外部摄像机,5-罗盘。
具体实施方式
为使本发明的目的、技术方案及优点更加清楚、明确,以下参照附图并举实施例对本发明作进一步说明。
科技术语解析:
SLAM是同步定位与地图构建(Simultaneous Localization And Mapping)的缩写,主要用于解决移动机器人在未知环境中运行时定位导航与地图构建的问题。SLAM通常包括如下几个部分,特征提取,数据关联,状态估计,状态更新以及特征更新等。对于其中每个部分,均存在多种方法。
可见光通信技术(Visible Light Communication,VLC)是指利用可见光波段的光作为信息载体,不使用光纤等有线信道的传输介质,而在空气、水等各类条件下直接传输光信号的通信方式。由于发光二极管(Light Emitting Diode,LED)作为半导体器件,利用其脉冲电流响应速度快的特点,可将调制信号转化为基础的点亮、熄灭,通过高频的点亮、熄灭来实现高速光信号调制和传输。然后利用光电二极管等光电转换器件接收光载波信号并获得信息。其物理结构上都包含光信号发射和光信号接收两部分。可广泛利用现有配备LED的室内外大型显示屏、信号灯、照明灯具、指示器和汽车前尾灯等实现高速信号传输,接收装置可利用现有的光电器件(如手机摄像头)来实现接收。
实施例1:
如图1至图4所示,本实施例公开了一种基于粒子滤波的移动机器人多传感器融合定位方法,该定位方法主要包括以下步骤:
步骤S1:在定位场所的各个角落安装多个外部摄像机4,使得外部摄像机4的视场范围均匀地分布在整个定位场所;在定位场所的天花板安装VLC定位系统LED灯具3;在移动机器人上安装2D激光定位仪1、VLC定位系统2信号接收器、罗盘。
步骤S2:通过外部摄像机4网络使用SLAM算法构造环境的地图Mo。
步骤S3:将机器人移动到环境中的各个位置来收集2D激光定位仪1、VLC定位系统2信号接收端(光电传感器)、罗盘、外部摄像机4网络反馈的校准数据其中t为时间,i=1,2,3,4为传感器标记,Z为传感器反馈的数据。
进一步的,本发明所述步骤S3中还包括以下步骤:
步骤S31:收集2D激光定位仪1反馈的数据。在任何时刻,二维激光测距仪都提供了一个机器人和附近的物体之间的距离的测量数据。所有测到的距离都在同一个平面上,每个都指向一个不同的角度,并按逆时针顺序组成一个矢量信号,然后与时间戳关联作为反馈的数据。
步骤S32:收集VLC定位系统2信号接收端反馈的数据。接收端经自适应滤波器分辨出各LED的ID码和对应的信号强度,按固定的ID顺序记录对应的信号强度组成一个矢量信号,然后与时间戳关联作为反馈的数据。
步骤S33:收集罗盘反馈的数据。将罗盘所显示的机器人的角度,即机器人的方向偏离当前磁场北极的角度,与时间戳关联作为反馈的数据。
步骤S34:收集外部摄像机4网络反馈的数据。移动机器人上带有特定颜色的指示灯,当移动机器人进入某一外部摄像机4的视野时,可通过图像分析确认移动机器人进入当前外部摄像机4的视野范围,按固定的顺序记录每一外部摄像机4的侦查信号(逻辑信号,1代表侦查到移动机器人,0代表没有)并组成一个矢量信号,与时间戳关联作为反馈的数据。
步骤S4:计算机器人移动的轨迹并将每个姿势与时间戳关联起来(t,st),其中st为机器人的位姿信息,即st=(xt,yt,θt)。
步骤S5:为每个传感器建立一个训练集。每个训练集包含一个与相应的姿势相关联的传感器捕捉的数据训练每个传感器的观察模型Zi=gi(s)。
进一步的,本发明所述步骤S5中还包括以下步骤:
步骤S51:建立2D激光定位仪1的观察模型。根据已有的激光距离矢量与对应的位置信息,通过人工神经网络的方法建立一个以位置信息为输入,以激光距离矢量为输出的模型。
步骤S52:建立VLC定位系统2的观察模型。根据已有的各LED光信号在接收端的强度与对应的位置信息,通过高斯回归的方法建立一个以位置信息为输入,以各LED光信号在接收端的强度为输出的模型。
步骤S53:建立罗盘的观察模型。根据已有的罗盘显示的机器人角度与对应的位姿信息(包括机器人的角度),通过高斯回归的方法建立一个以位置信息为输入,以角度差(罗盘显示的机器人角度与真实机器人角度之差)为输出的模型。
步骤S54:建立外部摄像机网络的观察模型。根据已有的摄像机侦查信息与对应的位置信息,通过高斯回归的方法建立一个以位置信息为输入,以每个摄像机的侦查信息为输出的模型。
步骤S6:在定位时,使用粒子滤波算法计算当前的移动机器人位姿,即在初始时刻设置位姿随机、权值相等的粒子群在每一时刻对每一个粒子使用里程计提供的数据和该粒子上一时刻的位姿预测该粒子当前的位姿并使用每个传感器的观察模型和当前时刻得来的传感器测量数据更新粒子的权值其中,在更新粒子的权值的过程中,只取当前时刻能够收到传感器测量数据参与权值更新计算,即当前时刻没有更新数据的传感器对应的为零。每个传感器的观察模型的推测结果与当前时刻得来的传感器测量数据相似度越高,则对应的越高,权值越高。根据权值对所有粒子进行重采样得以权值最大的粒子的位姿作为机器人当前时刻的定位数据。
上述实施例为本发明较佳的实施方式,但本发明的实施方式并不受上述实施例的限制,其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化,均应为等效的置换方式,都包含在本发明的保护范围之内。