无人车粮库清扫系统的定位与建图方法、定位与建图装置

技术领域

本发明属于定位与建图技术领域，具体涉及一种无人车粮库清扫系统的定位与建图方法、定位与建图装置。

背景技术

粮库的粮食清扫是粮食现代物流的一个重要环节。粮库的粮食清扫对于维护粮食质量、预防火灾和爆炸、保障储存设施运行和工作人员健康安全都是非常必要的。定期清扫粮食粉尘可以减少潜在问题的发生，提高粮食储存管理的效果和安全性。

目前我国粮食清扫主要采用的是机械清扫技术：

机械清扫是最常见和传统的粮食清扫方式。使用各种清扫设备和工具，如扫帚、刮板、清扫车等，对粮食仓库、储存设施进行清理和除尘。这种技术主要依赖于人工操作，通过机械设备辅助人工清理。机械清扫技术需要依靠人工操作，劳动强度较大，需要大量的人力投入。同时，机械清扫的清扫效率相对较低，需要更多时间和人力资源。

传统的机械清扫技术在中国粮食清扫中仍然广泛使用，尤其在小型仓库和传统储存设施中占有较大比例。为解决现有技术的不足，现提出改进方案，将智能自动驾驶技术应用于粮食清扫技术中。

发明内容

本发明提供一种无人车粮库清扫系统的定位与建图方法及定位与建图装置，旨在解决传统粮食清扫作业效率低下、人力成本高、时间耗费长的问题。

为解决上述技术问题，本发明通过以下技术方案实现：

本发明提供了一种无人车粮库清扫系统的定位与建图方法，无人车粮库清扫系统，包括粮库清扫无人车和激光雷达、工控机以及车辆惯性导航系统；所述定位与建图方法具体包括以下步骤：

步骤1：通过卫星定位获取粮食堆积区域的位置信息，并将该位置信息传输给工控机；

步骤2：控制工控机驱动粮库清扫无人车前进至粮食堆积区域，同时开启激光雷达和车辆惯性导航系统；

步骤3：基于步骤2同时开启的工控机和激光雷达进行点云数据扫描，对激光雷达得到的点云数据，使用基于点云中每个点的关键帧来提取特征点，再进行点云数据的处理及运动畸变校正；

步骤4：基于步骤3的运动畸变校正后，再使用因子图增量式平滑优化后的点云数据，进行局部地图的构建；

步骤5：基于步骤4构建的局部地图进行姿态估计融合、运动估计及环境建模，使粮库清扫无人车能够在无需人工干预的情况下，实现连续作业。

进一步的，所述步骤2开启外接的激光雷达具体为，激光雷达和车辆惯性导航系统的点云数据按照时间戳进行同步，使得点云数据在每个时间点均同步。

进一步的，所述步骤3对激光雷达得到的点云数据进行处理具体为，对于每个时间点的激光雷达点云数据，进行预处理步骤，以提高后续步骤的效果。

进一步的，所述步骤3运动畸变校正具体为，用车辆惯性导航系统得到的高频率数据估计粮库清扫无人车在每个时间点的运动，得到估计值，并根据估计值对激光雷达得到的与预处理后的点云数据进行运动畸变校正，以恢复真实的几何形状。

进一步的，所述步骤4具体为，使用基于运动畸变校正后的点云数据中每个点的高度变化来提取特征点；

使用LIO-SLAM的特征匹配追踪算法，通过追踪关键帧中的特征点，并将特征点与当前帧中的特征点进行匹配，通过匹配特征点之间的几何关系和描述子，得到特征点之间的对应关系，再根据这些特征点使用NDT算法构建局部地图。

进一步的，所述NDT算法基于概率分布函数对运动畸变校正后的点云数据进行建模，并通过匹配两个点云的概率分布函数来估计刚体变换矩阵，依次通过迭代优化、及最小化匹配点之间的距离或误差，来寻找最佳的刚体变换。

进一步的，所述关键帧的选取具体为，采用帧描述符来匹配点云之间的相似度，对一次扫描得到的点云P，以扫描中心O为圆心，最大扫描距离Lmax为半径，由内而外分割成x个环面，径向分割为y个扇形区域，共分为x乘y个区域，每个区域内运动畸变校正后的点云数据的个数记为n，每个面元的点云为P

把区域内运动畸变校正后的点云数据去掉一个最高值和一个最低值，取平均值然后进行平方，

其中p∈P

将运动畸变校正后的点云数据平均值的平方构成矩阵I：

I＝(a

将运动畸变校正后的点云数据构成矩阵，矩阵即为当前帧的空描述，有效描述周围环境结构在垂直方向上的形状；然后根据列向量的余弦相似度来定义两帧I

随机选取两帧点云，取平均值作为两帧点云的相似度，得到第一个关键帧，由于两帧点云扫描时为随机扫描，因此需要进行循环比对；

当新的候选帧到来时，和上一个关键帧进行比对，如果相似度大于阈值，则将此帧作为新的关键帧。

进一步的，所述步骤5的姿态估计融合具体为，将激光雷达得到的姿态估计结果与车辆惯性导航系统得到的姿态估计结果进行融合；

所述步骤5的运动估计具体为，根据姿态估计的结果，利用激光雷达得到的运动信息和车辆惯性导航系统得到的线加速度数据，估计粮库清扫无人车的运动状态；

所述步骤5的环境建模具体为，利用姿态估计融合结果和运动估计的结果，将运动畸变校正后的点云数据转换到全局坐标系中，并进行点云地图的构建。

本发明还提供了一种无人车粮库清扫系统的定位与建图装置，所述定位与建图装置使用如上述无人车粮库清扫系统的定位与建图方法，所述定位与建图装置具体包括激光雷达数据处理及校正模块、局部地图构建模块、估计及环境建模模块；

所述激光雷达数据处理及校正模块，通过卫星定位获取粮食堆积区域的位置信息，并将该信息传输给工控机，控制工控机驱动粮库清扫无人车前进至粮食堆积区域，同时开启工控机和激光雷达进行点云数据扫描，对激光雷达得到的点云数据，使用基于点云中每个点的关键帧来提取特征点，进行处理及运动畸变校正；

所述局部地图构建模块，用于对运动畸变校正后，再使用因子图增量式平滑优化后的点云数据，进行构建局部地图的构建；

所述估计及环境建模模块，用于对构建的局部地图进行姿态估计融合、运动估计及环境建模，使粮库清扫无人车能够在无需人工干预的情况下，实现连续作业。

本发明的有益效果是：

本发明能提高系统可靠性和数据可信度：粮库清扫无人车配备单目相机、激光雷达、组合导航和超声波雷达等多种传感器，利用多传感器数据融合技术解决探测、跟踪和目标识别等问题，从而提高整个系统的可靠性、鲁棒性和数据的可信度。

本发明能实现准确和鲁棒的姿态估计和运动估计：采用紧耦合的LIDAR/车辆惯性导航系统融合技术，通过将激光雷达和车辆惯性导航系统的数据进行紧密耦合，实现更准确和鲁棒的车辆姿态估计和运动估计。这种技术能够提供精确的车辆姿态信息，为后续的路径规划和环境建模提供可靠的基础。

本发明能实现精确的车辆轨迹和环境地图解算：采用帧描述符和因子图的增量式平滑优化方法，利用因子图的结构，通过迭代优化变量的值，实现对车辆轨迹和环境地图的精确解算。这种方法能够有效地提高轨迹和地图的准确性，为自主导航和清扫任务提供可靠的参考。

本发明能实现高效、安全的自主导航和清扫任务：通过采用改进的路径规划算法和避障算法，粮库清扫无人车能够根据动态环境和障碍物信息自主规划路径并进行避障，以实现高效、安全的自主导航和清扫任务。这消除了人力操作的限制和误差，提高了清扫的准确性和效率，同时降低了劳动成本和安全风险。

本发明能实现自动高效的清扫操作：粮库清扫无人车配备先进的工控机，并采用ROS2系统+Autoware自动驾驶开发系统，能够方便地进行建图、定位和路径规划，实现自动高效的清扫操作。这种自动化的操作方式能够提高清扫的效率，并减少人工操作的工作量。

附图说明

图1是本发明的粮库清扫无人车的结构示意图。

图2是本发明的粮库清扫无人车运行的示意图。

图3是本发明的工控机的示意图。

图4是本发明的清扫装置的示意图。

图5是本发明采用的点云帧分割方法的示意图。

图6是本发明的因子图优化示意图。

图7是本发明的方法流程图。

1、粮库清扫无人车；2、激光雷达；3、工控机；4、清扫装置；5、RTK；6、储物仓。

具体实施方式

下面将结合本发明实施例中的附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1至图7所示，本发明提供了一种无人车粮库清扫系统的定位与建图方法，无人车粮库清扫系统，包括粮库清扫无人车1和激光雷达2、工控机3以及车辆惯性导航系统(图中未示出)；所述定位与建图方法具体包括以下步骤：

步骤1：通过卫星定位获取粮食堆积区域的位置信息，并将该位置信息传输给工控机3；

步骤2：控制工控机3驱动粮库清扫无人车1前进至粮食堆积区域，同时开启激光雷达2和车辆惯性导航系统；

步骤3：基于步骤2同时开启的工控机3和激光雷达2进行点云数据扫描，对激光雷达2得到的点云数据使用基于点云中每个点的关键帧来提取特征点，再进行点云数据的处理及运动畸变校正；

步骤4：基于步骤3的运动畸变校正后，再使用因子图增量式平滑优化后的点云数据，进行局部地图的构建；

步骤5：基于步骤4构建的局部地图进行姿态估计融合、运动估计及环境建模，使粮库清扫无人车1能够在无需人工干预的情况下，实现连续作业。

进一步的，所述步骤2开启外接的激光雷达2具体为，激光雷达2和车辆惯性导航系统的点云数据按照时间戳进行同步，使得点云数据在每个时间点均同步。

进一步的，所述步骤3对激光雷达2得到的点云数据进行处理具体为，对于每个时间点的激光雷达2得到的点云数据，进行预处理步骤，以提高后续步骤的效果。

进一步的，所述步骤3运动畸变校正具体为，用车辆惯性导航系统得到的高频率数据估计粮库清扫无人车1在每个时间点的运动，得到估计值，并根据估计值对激光雷达2得到的与预处理后的点云数据进行运动畸变校正，以恢复真实的几何形状。

进一步的，所述步骤4具体为，根据经过运动畸变校正后的点云数据，使用基于运动畸变校正后的点云数据中每个点的高度变化来提取特征点；

粮库清扫无人车1使用过程中，会遇到凸出来的地面，会有高程变化的表面，使用LIO-SLAM的特征匹配追踪(Feature Tracking)算法；该算法通过追踪关键帧中的特征点，并将其与当前帧中的特征点进行匹配；通过匹配特征点之间的几何关系和描述子，可以得到特征点之间的对应关系，再根据这些特征点使用NDT算法构建局部地图；

进一步的，所述NDT算法基于概率分布函数对点云数据进行建模，并通过匹配两个点云的概率分布函数来估计刚体变换矩阵；它通过迭代优化的方式，通过最小化匹配点之间的距离或误差，来寻找最佳的刚体变换。

进一步的，所述因子图增量式平滑优化具体为，在普通的图优化中，最终要计算的是一个增量式方程，每次新增节点都要对整个图进行优化；在因子图优化中，如图6所示：当新的变量和因子加入时，首先分析因子图之间的连接和影响关系，考虑之前存储的信息有哪些可以继续利用，哪些必须重新计算，最后只对新增节点相关的变量进行优化；采用增量式平滑优化方法实现通过优化因子图中的变量，使得图像的平滑性得到改善。这种方法的基本思想是通过迭代地调整变量的值，以最小化一个目标函数，该目标函数衡量了图像的平滑性和符合观察数据的程度；

增量式平滑优化方法的优势在于它可以逐步更新变量的值，以逐步改善图像的平滑性；通过在因子图中建立适当的因子和变量之间的关系，可以利用因子图的结构进行高效的优化，转化为最小二乘法的问题，并在每个迭代步骤中更新变量的值，以逐步收敛到最优解。

进一步的，所述关键帧的选取具体为，LIO-SLAM本身采用的是一种比较简单的方法来提取关键帧，若当前帧与上一帧之间的位移超过1米或者旋转角度超过10°，则把当前帧作为新的关键帧；考虑到有着粮食、粮食谷壳、粮食粉尘等的粮仓，粮库清扫无人车1在运行过程中，会行驶不平稳角度不断发生变化，且角度偏移比较少多为小弧度曲线，为获得稳定高质量的关键帧，本发明采用帧描述符来匹配点云之间的相似度，对关键帧的信息提取如图5所示，对一次扫描得到的点云P，以扫描中心O为圆心，最大扫描距离Lmax为半径，由内而外分割成x个环面，径向分割为y个扇形区域，共分为x乘y个区域，每个区域内运动畸变校正后的点云数据的个数记为n，每个面元的点云为Pi

把区域内运动畸变校正后的点云数据去掉一个最高值和一个最低值，取平均值然后进行平方，

其中p∈P

将运动畸变校正后的点云数据平均值的平方构成矩阵I：

I＝(a

将运动畸变校正后的点云数据构成矩阵，矩阵即为当前帧的空描述，有效描述周围环境结构在垂直方向上的形状，然后根据列向量的余弦相似度来定义两帧I

随机选取两帧点云，取平均值作为两帧点云的相似度，得到第一个关键帧，由于两帧点云扫描时为随机扫描，因此需要进行循环比对；当新的候选帧到来时，和上一个关键帧进行比对，如果相似度大于阈值，则将此帧作为新的关键帧；这样选取的关键帧信息更丰富，降低了冗余关键帧的数量，同时后面图优化也会用到关键帧的帧描述符(矩阵)信息，不会增加太多计算量。

进一步的，所述步骤5的行姿态估计具体为，融合将激光雷达2的姿态估计结果与车辆惯性导航系统的姿态估计结果进行融合，以得到更准确和稳定的姿态估计；使用传感器融合算法，扩展卡尔曼滤波器法；

所述步骤5的运动估计具体为，根据姿态估计的结果，利用激光雷达2的运动信息和车辆惯性导航系统的线加速度数据，估计车辆的运动状态；

所述步骤5的环境建模具体为，利用姿态和运动估计的结果，将激光雷达2的点云数据转换到全局坐标系中，并进行点云地图构建。

本发明采用第一液压缸(图中未视出)，通过控制第一液压缸的活塞杆缩回，使粮库清扫无人车1底部的清扫装置4与地面接触，随后通过第一液压缸使清扫装置4进行水平移动，有效地清扫粮食堆积区域的表面，如图1和图2、以及图4所示。同时，通过储水部分内部的水泵为清扫装置4提供清洗水源，确保清扫效果的同时，避免对粮食造成污染。

清扫的粮食粉尘会存储到右边的储物仓6里(如图1所示)，可以通过遥控器一键自动倒出储物仓6中清扫掉的粮食粉尘并且一键自动收回储物仓6，送到集中处理中心一并处理，高效方便。

清扫完毕后，粮库清扫无人车1可以自动返回就近的储放区域或者指定的充电区域进行自动充电。这样的设计使得粮库清扫无人车1能够在无需人工干预的情况下，实现连续作业，大幅提升了清扫作业的效率。

其中，图3为工控机3实际使用过程中的操作界面示意图，操作界面左侧包含各种参数，操作界面右面为实际工控机3的实际扫描情况。

该粮库清扫无人车1的构造如图1，粮库清扫无人车1的顶部为RTK5，顶部中间是激光雷达2，下方是清扫装置4，粮库清扫无人车1的侧面设有工控机3的操作界面、以及储物仓6的仓门。

所述RTK5在开阔无遮挡的情况下可以实现GNNS厘米级精度定位，用于室外和激光雷达定位、惯性导航系统等融合实现更精确地多场景定位。

车头部分是粮库清扫无人车1的控制室，里面有方向盘等来控制粮库清扫无人车1运动，车中央是可触摸式工控机3，可以进行ros2和autoware环境下对粮库清扫无人车1进行控制，实现激光雷达2、车辆惯性导航系统、gps以及视觉相机等传感器的融合导航，同时可以监控粮库清扫无人车1运行状态，车后方是储物仓6，用来储存清扫到的粮食粉尘和垃圾。

该用于无人车粮库清扫系统的采用帧描述符的匹配和优化的定位与建图，相较于传统的人力清扫，本发明的粮食粮库清扫无人车1具有以下优势：配备单目相机、激光雷达2、组合导航和超声波雷达等多种传感器，利用多传感器数据融合技术解决探测、跟踪和目标识别等问题，从而提高整个系统的可靠性、鲁棒性和数据的可信度；使用紧耦合的激光雷达2(LiDAR)和车辆惯性导航系统融合，通过将激光雷达2和车辆惯性导航系统的数据进行紧密耦合，实现更准确和鲁棒的车辆姿态估计和运动估计；采用帧描述符合因子图的增量式平滑优化方法，利用因子图的结构，通过迭代优化变量的值，实现对车辆轨迹和环境地图的精确解算；通过采用改进的混合A*算法和改进的TEB算法(局部路径规划方法)，粮库清扫无人车1能够根据动态环境和障碍物信息自主规划路径并进行避障，以实现高效、安全的自主导航和清扫任务，避免了人力操作的限制和误差，提高了清扫的准确性和效率，降低了劳动成本和安全风险；配备先进的工控机3，并采用ROS2系统+Autoware自动驾驶开发系统，能够方便地进行建图、定位和路径规划，实现自动高效的清扫操作；粮库清扫无人车1配备紧急制动按键、遥控器紧急制动、车身急停和远程急停开关等紧急制动装置，以确保安全操作。

激光雷达2/车辆惯性导航系统紧耦合的解算方法是一种将激光雷达2和车辆惯性导航系统数据进行融合的技术，以提高环境感知和定位的准确性。这种方法通过将激光雷达2和车辆惯性导航系统的测量数据进行时间同步，并利用它们之间的互补信息，实现更准确和鲁棒的姿态估计、运动估计和地图构建。本文将车辆惯性导航系统紧耦合于LIO-SLAM框架中实现精准的轨迹解算：

本发明还提供了一种无人车粮库清扫系统的定位与建图装置，所述定位与建图装置使用如上述用于粮库清扫无人车1的采用帧描述符的匹配和优化的定位与建图方法，所述定位与建图装置具体包括激光雷达2数据处理及校正模块、局部地图构建模块、估计及环境建模模块及优化与校正模块；

所述激光雷达数据处理及校正模块，通过卫星定位获取粮食堆积区域的位置信息，并将该信息传输给就近的平台车工控机3；工控机3驱动粮库清扫无人车1前进至粮食堆积区域，同时开启工控机3和激光雷达2进行点云数据扫描，对激光雷达2得到的点云数据使用基于点云中每个点的关键帧来提取特征点，进行处理及运动畸变校正；

所述局部地图构建模块，用于对运动畸变校正后，再使用因子图增量式平滑优化后的点云数据，进行构建局部地图的构建；

所述估计及环境建模模块，用于对构建的局部地图进行姿态估计融合、运动估计及环境建模，使粮库清扫无人车1能够在无需人工干预的情况下，实现连续作业。

由上可见，本发明实施例基于定位与建图装置采用定位与建图方法，实现高效、安全的自主导航和清扫任务，通过采用改进的路径规划算法和避障算法，粮库清扫无人车1能够根据动态环境和障碍物信息自主规划路径并进行避障，以实现高效、安全的自主导航和清扫任务，提高了清扫的准确性和效率。

显然，上述实施例仅仅是为清楚地说明所作的举例，而并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引伸出的显而易见的变化或变动仍处于本发明创造的保护范围之中。