CN110702134B - 基于slam技术的车库自主导航装置及导航方法 - Google Patents

Abstract

本发明公开一种基于SLAM技术的车库自主导航装置及导航方法，该装置包括激光雷达装置、惯性测量单元、微处理器、存储器、显示设备、SLAM子系统和路径规划子系统。本发明利用SLAM技术和导航装置自身携带的传感器，由SLAM子系统对激光雷达获取的距离信息和经IMU数据校正后的激光里程信息进行处理，生成未知环境地图；利用路径规划子系统，调用激光里程信息和SLAM子系统生成地图数据，对汽车驶出车库的路径进行规划，因此无需借助网络等方式获取导航地图，即可实现对车库汽车的自主导航。本发明在卫星信号接收不到或较弱的场景，作为对现有网络导航设备的补充，也可以获得精确的导航效果。

Description

基于SLAM技术的车库自主导航装置及导航方法

技术领域

本发明属于车载导航技术领域，尤其涉及一种基于SLAM技术的车库自主导航装置及导航方法。

背景技术

现有的常规导航仪大多数依赖于GPS、北斗卫星等，如现有的北斗卫星车载导航仪。在卫星信号较弱或接受不到的车库中，导航仪大多依赖于已经建立好的车库平面图、无线网络或蓝牙等其他通信方式实现定位及导航。中国专利CN201810341079.2公开了北斗卫星车载导航仪，它包括导航仪主机及导航仪底座，通过接受北斗卫星信号实现导航，并能实现语音播报；中国专利CN201510735677.4公开了一种汽车GPS导航仪，它包括GPS导航装置、显示单元、娱乐播放系统等，通过接收GPS信号完成导航，并将GPS导航装置与娱乐播放系统合二为一；中国专利CN201610876638.0公开了一种基于无网络条件下的车库导航方法，通过智能终端扫描二维码的方式下载相关应用程序车库平面图及起点编码、终点编码的方式实现无网络导航；中国专利CN201220313482.2公开了基于ZigBee网络的车库LED智能导航系统，只要由主控制器、ZigBee无线网络、网络化LED灯具构成，通过ZigBee网络连接若干网络化LED灯具实现导航。上述发明的不足之处在于卫星定位类导航装置在类似于车库场景中易丢失导航信号，通信类及通过标签实现车库导航的方式需要定时维护，且定位导航缺乏自主性。

SLAM(simultaneous localization and mapping)，也称为CML(ConcurrentMapping and Localization),即时定位与地图构建，或并发建图与定位。近年来，随着人工智能的不断发展，SLAM技术无论是在工业还是在家庭等多个领域被广泛应用，例如智能移动机器人作为一种典型的自动化装置，更加强调移动能力的自主性，常常通过SLAM技术完成对周围环境的感知识别以及自身相对周围环境的位置标定。

发明内容

本发明要解决的技术问题是提出一种能够在车库中实现汽车在无网络条件下的自主导航装置及导航方法。

为解决上述问题，本发明首先提出了一种基于SLAM技术的车库自主导航装置，该装置包括微处理器、存储器、激光雷达装置、惯性测量单元(IMU)、显示设备、SLAM子系统和路径规划子系统；

所述微处理器用于总体协调所述自主导航装置的工作并对所述激光雷达装置测量数据和所述惯性测量单元测量数据进行处理；在汽车入库时，所述微处理器启动所述激光雷达装置、惯性测量单元并启动所述SLAM子系统，所述微处理器根据所述激光雷达的测量数据构建激光里程信息，并利用所述惯性测量单元测量的数据对所述激光里程信息进行矫正，然后将所述激光雷达的数据和所述矫正后的激光里程信息输入到所述SLAM子系统；在所述汽车停车时，所述微处理器将所述激光里程信息、所述SLAM子系统生成的地图数据保存到所述存储器中后，关闭所述SLAM子系统；在所述汽车离开车库时，所述微处理器调用路径规划子系统对所述汽车驶出车库的路径进行规划；

所述存储器用于存储所述微处理器、所述SLAM子系统和所述路径规划子系统工作过程中处理和生成的数据；

所述激光雷达装置用于感知汽车周围环境信息，生成所述汽车距离障碍物的距离信息；

所述惯性测量单元，用于测量汽车瞬时角速度和瞬时加速度数据；

所述显示设备用于显示所述微处理器、所述SLAM子系统和所述路径规划子系统工作过程中生成的图形信息和标识；

所述SLAM子系统，对所述激光雷达获取的距离信息和经IMU数据校正后的所述激光里程信息进行处理，生成未知环境地图；

所述路径规划子系统，调用所述激光里程信息和所述SLAM子系统生成地图数据，对所述汽车驶出车库的路径进行规划。

优选的，所述里程计信息里面包括所述车库入口位置信息、所述汽车的停车点位置信息和位姿信息。

优选的，所述激光雷达通过三角测距法实现对障碍物距离的测量。

优选的，对所述激光里程信息的校准由微处理器中的递归滤波器实现。

优选的，所述递归滤波器采用扩展卡尔曼滤波算法完成对所述激光里程信息的校准。

优选的，所述路径规划子系统设定所述SLAM子系统建图的起点为所述车库的入口和出口、建图结束时所述汽车的位姿为所述路径规划时的起始位姿。

优选的，所述SLAM子系统采用开源的Gmapping算法完成建图。

优选的，所述路径规划子系统，采用amcl算法匹配完成动态汽车定位，并对所述地图信息、汽车位姿及汽车出口位置进行处理，采用djikstra算法完成动态的路径规划。

本发明还提出了一种基于所述车库自主导航装置的导航方法，具体包括以下步骤：

步骤1：在所述汽车进入车库时，打开所述自主导航装置，由所述SLAM子系统保存所述汽车当前位置并开始建图；

步骤2：在所述汽车停车时，所述自主导航装置将所述汽车位姿信息和所述SLAM子系统构建的地图保存到所述存储器中，关闭所述SLAM子系统；

步骤3：在所述汽车离开车库时，打开所述自主导航装置，由所述路径规划子系统调用所述存储器中的位姿信息和地图，实时对所述汽车进行动态定位，实现自主导航。

本发明的有益效果是：

本发明利用SLAM技术和导航装置自身携带的传感器，由SLAM子系统，对激光雷达获取的距离信息和经IMU数据校正后的激光里程信息进行处理，生成未知环境地图；利用路径规划子系统，调用激光里程信息和SLAM子系统生成地图数据，对汽车驶出车库的路径进行规划，因此无需借助网络等方式获取导航地图，即可实现对车库汽车的自主导航。本发明在卫星信号接收不到或较弱的场景，作为对现有网络导航设备的补充，也可以获得很好的导航效果。

附图说明

图1为本发明实施例的系统整体框图；

图2为本发明实施例的系统装置示意图；

图3为本发明实施例递归滤波器的算法流程图；

图4为本发明实施例的SLAM子系统框图；

图5为本发明实施例的SLAM子系统算法流程图；

图6为本发明实施例路径规划子系统框图；以及

图7为本发明实施例的工作流程图；

主要附图标记：1-激光雷达装置，2-惯性测量单元，3-微处理器。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。

基于SLAM技术的车库自主导航装置，如图1所示该系统包括处理器、激光雷达装置1、惯性测量单元2、显示设备、SLAM子系统和路径规划子系统。

本实施例的装置示意图如图2所示，所示处理器安装于汽车顶部；所示惯性测量单元2安装于处理器正前方；所示激光雷达装置1安装于处理器上部；存储器置于处理器内部；显示装置安装于汽车内部与处理器通过串口连接。

处理器包括微处理器3和存储器，其中微处理器3作为整个系统的核心，用于协调系统各个部分的工作，在汽车入库时，对激光雷达数据和惯性测量单元2数据进行处理，并将处理后的数据作为SLAM子系统输出，生成地图数据，在汽车出库时，调用路径规划子系统进行路径规划；存储器用于对微处理器3所处理的数据和生成的地图数据进行存储；

激光雷达装置1用于感知环境信息，通过三角测距法实现对障碍物距离的测量，获取的测量数据由微处理器3通过RF2O算法构建激光里程计，本实例采用的RF2O算法是一种开源的激光里程计算法。

惯性测量单元2，用于获得汽车瞬时角速度和瞬时加速度数据，递归滤波器用于对激光里程计数据的偏航角速度进行矫正，以获取更精确的里程信息，减少在导航过程中的里程误差。激光里程计数据和IMU数据的融合过程如图3所示，本发明实施例采用扩展卡尔曼滤波算法，该算法通过接收激光里程计数据，并更新协方差矩阵及里程计状态，如果满足预设条件则接收IMU数据进行再次更新，如果迭代次数达到设定值或者协方差已经满足要求，则输出最新的里程信息；

显示设备用于显示微处理器3、SLAM子系统和路径规划子系统工作过程中生成的图形信息和标识，包括地图信息、路径信息、汽车位置以及车头朝信息；

如图4所示，SLAM子系统，将获取激光雷达获取地距离信息和经IMU数据校正后的激光里程信息作为输入，用于计算未知环境地图。本发明实施例使用的激光雷达装置1为2D激光雷达，通过感知周围环境测量到障碍物的距离信息作为SLAM子系统的输入；所示惯性测量单元2和激光里程信息由微处理器3中构建的递归滤波器完成对激光里程信息的校准，校准后的里程信息作为SLAM子系统的输入，该递归滤波器通过扩展卡尔曼滤波算法完成对激光里程信息的校准，校准过程如图3所示；其中SLAM子系统采用开源的Gmapping算法完成建图，算法流程如图5所示，首先以经IMU数据校正后的激光里程信息为依据抽取粒子，其中每一个粒子代表一条运动轨迹，再通过匹配当前激光雷达测量数据对每个粒子进行打分，选取得分最高的粒子更新汽车运动轨迹，进而更新未知环境的地图所示车库地图为SLAM子系统通过Gmapping算法建立的车库平面地图。

路径规划子系统，认为建图的起点即为车库的入口和出口，建图结束时汽车的位姿即为路径规划时的起始位姿，通过amcl算法进行匹配完成动态汽车定位，并对地图信息、汽车位姿及出口位置进行处理，采用djikstra算法完成动态的路径规划，如图5所示，所示车库地图由SLAM子系统建立，可从存储器获得，并通过车库地图与激光雷达数据的特征校准进行实时定位；所示路径与位置信息为路径规划子系统的输出信息，通过处理器经串口传输到显示装置。

本实施例中自主导航装置的工作流程如图7所示，在汽车进入车库时，打开自主导航装置，运行激光雷达装置1和IMU，同时SLAM子系统保存当前位置并开始建图，停车时处理器将停车点位置、停车处车头朝向和SLAM子系统构建的车库平面地图保存到存储器中；在汽车离开车库时，打开路径规划子系统，调用先前存储的入口位置、停车点位置、停车点汽车车头朝向和SLAM子系统构建的车库平面地图，进行路径规划并通过动态定位算法实时定位完成导航；起点位置为建图的原点位置，在开始建图时获得；停车点位置及汽车位姿由里程信息计算得到。

本发明实施例所建立的基于SLAM技术的车库导航装置通过2D激光雷达在进入车库时打开系统随着汽车移动完成对未知场景的平面地图建立，停车时保存地图，在离开车库时调用地图通过路径规划系统规划出汽车到车库出口的路径完成导航。与已有的车库导航装置相比，该技术不依赖于地下停车场的环境构造，无需基于已知地图且无需网络、蓝牙等其他通讯方式即可实现在车库中的自主定位导航，既具有普适性、自主性，又便于推广，也逐渐成为当前社会的一种趋势。

以上所述，仅为本发明的具体实施方式的实例，本发明的局限并不局限于此。熟悉该技术领域的技术人员在本发明揭露的技术范围之内，可轻易找到变化和替换方式，这些都应涵盖在本发明的保护范围之内。为此，本发明的保护范围应以权利要求的保护范围为准。

Claims (6)

1.一种基于SLAM技术的车库自主导航装置，其特征在于：其包括微处理器、存储器、激光雷达装置、惯性测量单元、显示设备、SLAM子系统和路径规划子系统；

所述微处理器对所述激光雷达装置测量数据和所述惯性测量单元测量数据进行处理，在汽车入库时，所述微处理器启动所述激光雷达装置、惯性测量单元并启动所述SLAM子系统，所述微处理器根据所述激光雷达的测量数据构建激光里程信息，并利用所述惯性测量单元测量的数据对所述激光里程信息进行矫正，然后将所述激光雷达的数据和所述矫正后的激光里程信息输入到所述SLAM子系统；在所述汽车停车时，所述微处理器将所述激光里程信息、所述SLAM子系统生成的地图数据保存到所述存储器中后，关闭所述SLAM子系统；在所述汽车离开车库时，所述微处理器调用路径规划子系统对所述汽车驶出车库的路径进行规划；

所述存储器用于存储所述微处理器、所述SLAM子系统和所述路径规划子系统工作过程中处理和生成的数据；

所述激光雷达装置用于感知汽车周围环境信息，生成所述汽车距离障碍物的距离信息；

所述惯性测量单元，用于测量汽车瞬时角速度和瞬时加速度数据；

所述显示设备用于显示所述微处理器、所述SLAM子系统和所述路径规划子系统工作过程中生成的图形信息和标识；

所述SLAM子系统，对所述激光雷达获取的距离信息和经IMU数据校正后的所述激光里程信息进行处理生成未知环境地图；具体为：首先以经IMU数据校正后的激光里程信息为依据抽取粒子，其中每一个粒子代表一条运动轨迹，再通过匹配当前激光雷达测量数据对每个粒子进行打分，选取得分最高的粒子更新汽车运动轨迹，生成未知环境地图；

所述路径规划子系统，调用所述激光里程信息和所述SLAM子系统生成地图数据，对所述汽车驶出车库的路径进行规划，具体为：建图的起点即为车库的入口和出口，建图结束时汽车的位姿即为路径规划时的起始位姿，通过amcl算法进行匹配完成动态汽车定位，并对地图信息、汽车位姿及出口位置进行处理，采用djikstra算法完成动态的路径规划。

2.根据权利要求1所述的基于SLAM技术的车库自主导航装置，其特征在于：所述激光雷达通过三角测距法实现对障碍物距离的测量。

3.根据权利要求1所述的基于SLAM技术的车库自主导航装置，其特征在于：对所述激光里程信息的校准由微处理器中的递归滤波器实现。

4.根据权利要求3所述的基于SLAM技术的车库自主导航装置，其特征在于：所述递归滤波器采用扩展卡尔曼滤波算法完成对所述激光里程信息的校准。

5.根据权利要求1所述的基于SLAM技术的车库自主导航装置，其特征在于：所述SLAM子系统采用开源的Gmapping算法完成建图。

6.一种用于根据权利要求1-5中任一项所述的基于SLAM技术的车库自主导航装置的导航方法，其特征在于：具体包括以下步骤：

步骤1：在所述汽车进入车库时，打开所述自主导航装置，由所述SLAM子系统保存所述汽车当前位置并开始建图；

步骤2：在所述汽车停车时，所述自主导航装置将所述汽车位姿信息和所述SLAM子系统构建的地图保存到所述存储器中，关闭所述SLAM子系统；

步骤3：在所述汽车离开车库时，打开所述自主导航装置，由所述路径规划子系统调用所述存储器中的位姿信息和地图，实时对所述汽车进行动态定位，实现自主导航。

Images