CN106705964A - 全景相机融合imu、激光扫描仪定位与导航系统及方法 - Google Patents

Abstract

本发明涉及的全景相机融合IMU、激光扫描仪定位与导航系统及方法，包括：六个模拟相机模组、相机固定架、六路图像采集卡、六个镜头、电源、主机、IMU模块、Arduino板、激光扫描仪，模拟相机模组呈环形排列构架在相机固定架上，图像采集卡分别与模拟相机模组电性相连，镜头分别固定在模拟相机模组上，电源与模拟相机模组均电性相连，主机上安装有图像采集卡驱动，主机连接激光扫描仪，主机连接Arduino板。本发明不仅能够获得较为准确的初始定位信息，而且有效消减IMU模块与激光扫描仪的累积定位误差，较大程度上提高了机器人的定位与导航精度。不仅如此，保存的全景图像包含机器人的位置与姿态信息，可作为机器人行进过程中精确地图像记录数据，用于机器人行进过程中问题排查与解决。

Description

全景相机融合IMU、激光扫描仪定位与导航系统及方法

技术领域：

本发明涉及机器人定位与导航技术领域，具体是一种全景相机融合IMU、激光扫描仪定位与导航系统及方法。

背景技术：

随着机器人技术的不断发展，传统的机器人定位方法已难以满足机器人快速精准的定位需求。基于激光扫描仪的定位与导航方法虽然能实现精确地定位，但是运算复杂度高，特别是在空旷的环境中，由于没有障碍物作为反射体生成点云，空旷环境下难以实现准确的即时定位与地图生成。基于IMU的定位与导航能够获取准确的机器人位置与姿态参数，但是在长距离定位的时候容易造成误差累积。采用IMU与激光扫描仪在一定程度上能够实现较为准确的实时定位与制图效果，但是在长距离定位与导航时依然存在累积误差与航向偏移，特别是在机器人初始位置的确定上，传统的导航与定位方式难以实现准确的初始位置的确定。然而基于视觉的定位方式可以有效解决定位与导航中的误差积累，并且在一定程度上能有效解决初始位置的确定，但是基于视觉的定位方式精度受到极大限制，定位精度难以达到IMU与激光扫描仪的定位精度，并且受到环境光照条件影响较大。

发明内容

本发明为了克服现有技术中存在的上述缺陷，提供了一种全景相机融合IMU、激光扫描仪定位与导航系统及方法，采用6相机360°全景构架，在机器人采用IMU、激光扫描仪对周围环境进行建图的同时，采集周围环境360°全景图像，并记录图像采集时刻机器人的位置与姿态数据，生成带定位信息的全景图像序列与激光扫描仪构建的地图进行融合，生成带视觉标记的融合地图。采用带视觉标记的融合地图对机器人进行定位与导航。

为解决上述问题，本发明提出一种全景相机融合IMU、激光扫描仪定位与导航系统，包括：六个模拟相机模组、相机固定架、六路图像采集卡、六个镜头、电源、主机、IMU模块、Arduino板、激光扫描仪，所述模拟相机模组呈环形排列并固定设置在所述相机固定架上，所述六路图像采集卡分别与所述六个模拟相机模组电性相连，所述六个镜头分别固定在所述六个模拟相机模组上，所述电源与六个模拟相机模组均电性相连，所述主机上安装有图像采集卡驱动，图像采集卡驱动输出端连接所述图像采集卡，所述图像采集卡设置有配套的SDK开发接口，所述SDK开发接口的输入端连接模拟相机模组的输出端，所述IMU模块与Arduino板相连，所述Arduino板与主机相连，所述激光扫描仪与主机相连。

在上述技术方案中，所述模拟相机模组采用索尼super hard 1/3英寸CCD，额定电压为12V，额定电流为50mA，有效像素为720(H)*576(V)。

在上述技术方案中，所述电源的输出电压为12V，输出电流为1A。

在上述技术方案中，所述主机的CPU为酷睿双核或以上，内存等于或大于2G，硬盘等于或大于256G，主机的系统为Windows 10、windows7以及windows xp中的任意一种。

在上述技术方案中，所述相机固定架包括六个环形分布的固定板，相邻固定板固定相连且呈120°夹角，所述固定板的中心均设置有一相机安装孔。

在上述技术方案中，所述相机固定架采用3D打印机打印。

在上述技术方案中，所述IMU模块硬件组成包括MPU6050芯片集成3轴陀螺仪和3轴加速度计、3轴HMC5883L地磁传感器、BPM180气压高度计以及STM32F103T8 32-bit ARMCortexM3处理器，所述IMU模块额定电压为5V，额定电流为40mA,数据更新频率最高为60hz，IMU模块硬件预写各芯片的驱动和姿态结算程序，上位机通过串口直接获取当前模块的俯仰、横滚、航向、气压高度及温度数据。

在上述技术方案中，所述Arduino板型号为Mega2560，采用USB接口的核心电路板，具有54路数字输入输出，Arduino Mega2560的处理器核心是ATmega2560，同时具有54路数字输入/输出口，其中16路作为PWM输出，16路模拟输入，4路UART接口，一个16MHz晶体振荡器，一个USB口，一个电源插座，一个ICSP header和一个复位按钮，其工作电压为5V。

在上述技术方案中，所述激光扫描仪采用北洋公司的UST-10LX激光扫描仪，UST-10LX扫描距离为10m，扫描广角270°，工作电压为DC12V/24V，单圈扫描时间25msec，测距精度为±40mm，角度分辨率为0.25，防护等级为IP65，上位机通过以太网口读取扫描数据。

本发明同时提供一种全景相机融合IMU、激光扫描仪定位与导航系统及方法，包括以下步骤：

步骤S1、通过IMU获取机器人运行过程中的位置与姿态参数，融合激光扫描仪采集的周围环境的三维点云信息，创建周围环境地图；

步骤S2、通过全景相机获取以环形框架为中心的360°环境图像，其图像与机器人定位信息进行绑定，与激光扫描仪生成地图融合生成带视觉标记的地图；

步骤S3、利用融合生成的带视觉标记的地图对机器人进行定位于导航；

上述技术方案中，所述步骤一具体包括以下子步骤：

子步骤S11、根据IMU所获取的数据，利用航迹推算算法解算出机器人当前的位置和姿态参数；

子步骤S12、利用激光传感器所采集的点云数据来表征探测环境中障碍物的空间分布；

子步骤S13、采用Rao-Blackwellized滤波、卡尔曼滤波或扩展性卡尔曼滤波等滤波算法进行并发定位与制图解算，实时生成增量式地图；

所述步骤二具体包括以下子步骤：

子步骤S21、首先采用全景相机保存当前图像，并记录当前图像保存时刻机器人的位置与姿态信息；

子步骤S22、在后台对图像帧提取SIFT特征及特征描述，作为融合地图的视觉标记；

子步骤S23、对图像帧提取的SIFT特征以及对应的图像拍摄时机器人的位置姿态信息建立索引；

所述步骤三具体包括以下子步骤：

子步骤S31、首先获取全景相机图像，对图像进行SIFT特征提取及描述；

子步骤S32、在步骤S23中生成的索引进行检索，匹配对应的SIFT特征，获得检索得到的机器人位置与姿态数据；

子步骤S33、利用激光扫描仪获取的周围场景的三维点云数据与生成的地图进行ICP匹配，获得机器人的位置与姿态数据；

子步骤S34、利用步骤S32获取检索的地图中机器人位置与姿态数据与步骤S33中激光扫描仪的定位数据进行融合，获取准确的机器人位置与姿态参数。

本发明与现有技术方案相比具有以下有益效果和优点：

本发明能够实现机器人较为准确的初始位置确定，在激光扫描仪生成的地图中融合视觉标记，增强了地图信息，有效降低了IMU、激光扫描仪累积误差，提高了机器人的定位与导航精度。同时有效记录了机器人行进过程中各个位置点的周围环境的全景图像，充当高精度机器人行进记录仪，有助于排查和解决机器人行径过程中出现的问题。

附图说明

图1是本发明中相机固定架的结构示意图。

图中编号说明：1、固定板；2、安装孔。

具体实施方式

以下结合附图和具体实施例对本发明作进一步的详细描述：

本实施例中，本发明提出的高清多相机全景立体成像系统，包括：六个模拟相机模组、相机固定架、六路图像采集卡、六个镜头、电源、主机、服务器、客户端，模拟相机模组呈环形排列构架在相机固定架上，图像采集卡分别与模拟相机模组电性相连，镜头分别固定在模拟相机模组上，电源与模拟相机模组均电性相连，主机上安装有图像采集卡驱动，采用图像采集卡配套的SDK开发接口，编程同步获取六个模拟相机模组的采集图像，主机与服务器相连，服务器与客户端相连。

模拟相机模组采用索尼super hard 1/3英寸CCD，额定电压为12V，额定电流为50mA，有效像素为720(H)*576(V)。

电源的输出电压为12V，输出电流为1A。

主机的CPU为酷睿双核或以上，内存等于或大于2G，硬盘等于或大于256G，主机的系统为Windows 10、windows7以及windows xp中的任意一种。

相机固定架包括六个环形分布的固定板1，相邻固定板1之间呈120°固定相连，固定板1的中心均设置有一相机安装孔2。

相机固定架采用3D打印机打印。

IMU模块硬件组成包括MPU6050集成3轴陀螺仪和3轴加速度计、3轴HMC5883L地磁传感器、BPM180气压高度计以及STM32F103T8 32-bit ARM CortexM3处理器。模块额定电压为5V，额定电流为40mA,数据更新频率最高可达60hz。硬件内部预写各芯片的驱动和姿态结算程序，上位机可通过串口直接获取当前模块的俯仰、横滚、航向、气压高度及温度数据。

Arduino板型号为Mega2560，采用USB接口的核心电路板，具有多达54路数字输入输出，特别适合需要大量IO接口的设计。Arduino Mega2560的处理器核心是ATmega2560，同时具有54路数字输入/输出口(其中16路可作为PWM输出)，16路模拟输入，4路UART接口，一个16MHz晶体振荡器，一个USB口，一个电源插座，一个ICSP header和一个复位按钮，其工作电压为5V。

激光扫描仪采用北洋公司的UST-10LX激光扫描仪。UST-10LX扫描距离为10m，扫描广角270°。工作电压为DC12V/24V。单圈扫描时间25msec，测距精度为±40mm，角度分辨率为0.25，防护等级为IP65。上位机通过以太网口读取扫描数据。

通过全景相机获取以环形框架为中心的360°环境图像，采用IMU模块、激光扫描仪生成周围环境地图，记录机器人各个位置与姿态处的周围环境360°全景图像，构建融合视觉标记的环境地图，用于机器人的定位与导航；在机器人定位与导航时，首先通过全景相机拍摄环境的全景图像进行视觉标记检索识别，获得初始定位信息，然后融合激光扫描仪的定位信息做出更加精确的导航与定位。

本发明提出的全景相机融合IMU、激光扫描仪定位与导航方法，包括以下步骤：

步骤S1、通过IMU获取机器人运行过程中的位置与姿态参数，融合激光扫描仪采集的周围环境的三维点云信息，创建周围环境地图；

步骤S2、通过全景相机获取以环形框架为中心的360°环境图像，其图像与机器人定位信息进行绑定，与激光扫描仪生成地图融合生成带视觉标记的地图；

步骤S3、利用融合生成的带视觉标记的地图对机器人进行定位于导航；

上述技术方案中，所述步骤一具体包括以下子步骤：

子步骤S11、根据IMU所获取的数据，利用航迹推算算法解算出机器人当前的位置和姿态参数；

子步骤S12、利用激光传感器所采集的点云数据来表征探测环境中障碍物的空间分布；

子步骤S13、采用Rao-Blackwellized滤波、卡尔曼滤波或扩展性卡尔曼滤波等滤波算法进行并发定位与制图解算，实时生成增量式地图；

所述步骤二具体包括以下子步骤：

子步骤S21、首先采用全景相机保存当前图像，并记录当前图像保存时刻机器人的位置与姿态信息；

子步骤S22、在后台对图像帧提取SIFT特征及特征描述，作为融合地图的视觉标记；

子步骤S23、对图像帧提取的SIFT特征以及对应的图像拍摄时机器人的位置姿态信息建立索引；

所述步骤三具体包括以下子步骤：

子步骤S31、首先获取全景相机图像，对图像进行SIFT特征提取及描述；

子步骤S32、在步骤S23中生成的索引进行检索，匹配对应的SIFT特征，获得检索得到的机器人位置与姿态数据；

子步骤S33、利用激光扫描仪获取的周围场景的三维点云数据与生成的地图进行ICP匹配，获得机器人的位置与姿态数据；

子步骤S34、利用步骤S32获取检索的地图中机器人位置与姿态数据与步骤S33中激光扫描仪的定位数据进行融合，获取准确的机器人位置与姿态参数。

本发明不仅能够获得较为准确的初始定位信息，而且有效消减IMU模块与激光扫描仪的累积定位误差，较大程度上提高了机器人的定位与导航精度。不仅如此，保存的全景图像包含机器人的位置与姿态信息，可作为机器人行进过程中精确地图像记录数据，用于机器人行进过程中问题排查与解决。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围中。

Claims (10)

1.一种全景相机融合IMU、激光扫描仪定位与导航系统，其特征在于，包括：六个模拟相机模组、相机固定架、六路图像采集卡、六个镜头、电源、主机、IMU模块、Arduino板、激光扫描仪，所述模拟相机模组呈环形排列并固定设置在所述相机固定架上，所述六路图像采集卡分别与所述六个模拟相机模组电性相连，所述六个镜头分别固定在所述六个模拟相机模组上，所述电源与六个模拟相机模组均电性相连，所述主机上安装有图像采集卡驱动，图像采集卡驱动输出端连接所述图像采集卡，所述图像采集卡设置有配套的SDK开发接口，所述SDK开发接口的输入端连接模拟相机模组的输出端，所述IMU模块与Arduino板相连，所述Arduino板与主机相连，所述激光扫描仪与主机相连。

2.根据权利要求1所述的全景相机融合IMU、激光扫描仪定位与导航系统，其特征在于，所述模拟相机模组采用索尼super hard 1/3英寸CCD，额定电压为12V，额定电流为50mA，有效像素为720(H)*576(V)。

3.根据权利要求1所述的全景相机融合IMU、激光扫描仪定位与导航系统，其特征在于，所述电源的输出电压为12V，输出电流为1A。

4.根据权利要求1所述的全景相机融合IMU、激光扫描仪定位与导航系统，其特征在于，所述主机的CPU为酷睿双核或以上，内存等于或大于2G，硬盘等于或大于256G，主机的系统为Windows10、windows7以及windows xp中的任意一种。

5.根据权利要求1所述的全景相机融合IMU、激光扫描仪定位与导航系统，其特征在于，所述相机固定架包括六个环形分布的固定板，相邻固定板固定相连且呈120°夹角，所述固定板的中心均设置有一相机安装孔。

6.根据权利要求1所述的全景相机融合IMU、激光扫描仪定位与导航系统，其特征在于，所述相机固定架采用3D打印机打印。

7.根据权利要求1所述的全景相机融合IMU、激光扫描仪定位与导航系统，其特征在于，所述IMU模块硬件组成包括MPU6050芯片集成3轴陀螺仪和3轴加速度计、3轴HMC5883L地磁传感器、BPM180气压高度计以及STM32F103T8 32-bit ARM CortexM3处理器，所述IMU模块额定电压为5V，额定电流为40mA,数据更新频率最高为60hz。

8.根据权利要求1所述的种全景相机融合IMU、激光扫描仪定位与导航系统，其特征在于，所述Arduino板型号为Mega2560，采用USB接口的核心电路板，具有54路数字输入输出，Arduino Mega2560的处理器核心是ATmega2560，同时具有54路数字输入/输出口，其中16路作为PWM输出，16路模拟输入，4路UART接口，一个16MHz晶体振荡器，一个USB口，一个电源插座，一个ICSP header和一个复位按钮，其工作电压为5V。

9.根据权利要求1所述全景相机融合IMU、激光扫描仪定位与导航系统，其特征在于，所述激光扫描仪采用北洋公司的UST-10LX激光扫描仪，UST-10LX扫描距离为10m，扫描广角270°，工作电压为DC12V/24V，单圈扫描时间25msec，测距精度为±40mm，角度分辨率为0.25，防护等级为IP65，上位机通过以太网口读取扫描数据。

10.根据权利要求1所述的全景相机融合IMU、激光扫描仪定位与导航系统及方法，其特征在于，包括以下步骤：

步骤S1、通过IMU获取机器人运行过程中的位置与姿态参数，融合激光扫描仪采集的周围环境的三维点云信息，创建周围环境地图；

步骤S2、通过全景相机获取以环形框架为中心的360°环境图像，其图像与机器人定位信息进行绑定，与激光扫描仪生成地图融合生成带视觉标记的地图；

步骤S3、利用融合生成的带视觉标记的地图对机器人进行定位于导航；

上述技术方案中，所述步骤一具体包括以下子步骤：

子步骤S11、根据IMU所获取的数据，利用航迹推算算法解算出机器人当前的位置和姿态参数；

子步骤S12、利用激光传感器所采集的点云数据来表征探测环境中障碍物的空间分布；

子步骤S13、采用Rao-Blackwellized滤波、卡尔曼滤波或扩展性卡尔曼滤波等滤波算法进行并发定位与制图解算，实时生成增量式地图；

所述步骤二具体包括以下子步骤：

子步骤S21、首先采用全景相机保存当前图像，并记录当前图像保存时刻机器人的位置与姿态信息；

子步骤S22、在后台对图像帧提取SIFT特征及特征描述，作为融合地图的视觉标记；

子步骤S23、对图像帧提取的SIFT特征以及对应的图像拍摄时机器人的位置姿态信息建立索引；

所述步骤三具体包括以下子步骤：

子步骤S31、首先获取全景相机图像，对图像进行SIFT特征提取及描述；

子步骤S32、在步骤S23中生成的索引进行检索，匹配对应的SIFT特征，获得检索得到的机器人位置与姿态数据；

子步骤S33、利用激光扫描仪获取的周围场景的三维点云数据与生成的地图进行ICP匹配，获得机器人的位置与姿态数据；

子步骤S34、利用步骤S32获取检索的地图中机器人位置与姿态数据与步骤S33中激光扫描仪的定位数据进行融合，获取准确的机器人位置与姿态参数。