[发明专利]基于TOF深度相机与IMU的平仓机器人自主定位方法

说明书

本发明涉及粮仓机器人定位技术领域，提供一种基于TOF深度相机与IMU的平仓机器人自主定位方法。本发明的方法包括：步骤1：通过TOF深度相机构建二维点云粮仓环境，并通过当前帧与参考帧的特征匹配，解算平仓机器人的位姿；步骤2：对IMU进行初始化；步骤3：通过IMU解算平仓机器人的位姿；步骤4：将步骤1中通过TOF深度相机解算出的平仓机器人的位姿作为状态向量，将步骤3中通过IMU解算出的平仓机器人的位姿作为观测向量，对步骤1和步骤3中解算出的位姿进行卡尔曼滤波融合，得到平仓机器人的位姿，完成平仓机器人的自主定位。本发明能够提高平仓机器人在环境复杂的粮仓环境中自主定位的准确性。

技术领域

本发明涉及粮仓机器人定位技术领域，特别是涉及一种基于TOF深度相机与IMU的平仓机器人自主定位方法。

背景技术

粮食储备关系到国家安全和社会稳定，粮食仓储技术的创新和发展备受关注。作为粮食入仓即将进入常规储藏阶段的最后一个处理环节，平仓作业是至关重要的，因为平仓作业决定了粮面平整度，而粮面平整度直接关系到粮食储藏期间通风、熏蒸及粮温测控等储粮技术的实施效果，进而影响到粮食在储藏期的安全。

目前，我国粮食仓储工作急需机器人的场所是仓内的平仓工作，由人工进行作业造成平仓速度慢、劳动强度大、粮食变质等问题，不能满足平仓作业的需要，甚至会给人的生命安全带来严重威胁。随着中国制造2025的到来，机器人悄然走进了粮食仓储行业。目前，机器人在粮仓的巡仓、检仓、报仓等方面已经有了广泛的应用，但是在平仓作业上还未实现使用能够自主工作的机器人，现阶段在平仓中使用的机器人都需要人工去远程操控，这样费时费力并且平仓效果不太理想。

当输送机将粮食送进粮仓时，粮食表面高低不平，如果直接存储会导致粮食变质。平仓机器人的首要工作就是将粮食表面平整。在平仓工作过程中，机器人需要将高出平面的粮食推到低于粮食平面的地方。然而在环境复杂的粮仓环境中，平仓机器人如何才能准确确定自身在粮仓中的位置这是一个急需解决的问题。

发明内容

针对现有技术存在的问题，本发明提供一种基于TOF深度相机与IMU的平仓机器人自主定位方法，能够提高平仓机器人在环境复杂的粮仓环境中自主定位的准确性。

本发明的技术方案为：

一种基于TOF深度相机与IMU的平仓机器人自主定位方法，所述平仓机器人上配置有TOF深度相机与IMU，其特征在于，包括下述步骤：

步骤1：通过TOF深度相机解算平仓机器人的位姿：

步骤1.1：通过TOF深度相机采集第t时刻的粮仓图像，提取第t时刻的粮仓图像的特征点，构建二维点云粮仓环境；

步骤1.2：将第t-1时刻的粮仓图像作为参考帧，将第t时刻的粮仓图像作为当前帧，将当前帧与参考帧进行特征匹配，若当前帧与参考帧中有比例为α的特征点的位置坐标相同，则当前帧为关键帧；

步骤1.3：通过当前帧与参考帧之间多组对应点的帧间匹配，计算本质矩阵E，对本质矩阵E进行分解，求出旋转矩阵R和平移向量(x_t,y_t,1)，由旋转矩阵R求解出平仓机器人的姿态角(γ_t,θ_t,ψ_t)，得到平仓机器人的位姿(x_t,y_t,γ_t,θ_t,ψ_t)；其中，γ_t、θ_t、ψ_t分别为第t时刻平仓机器人相对导航坐标系的俯仰角、翻滚角、偏航角，(x_t,y_t)为第t时刻平仓机器人的位置坐标；

步骤2：对IMU进行初始化；

步骤3：通过初始化后的IMU解算平仓机器人的位姿；