基于TOF深度相机与IMU的平仓机器人自主定位方法技术

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

Autonomous positioning method of warehouse robot based on TOF depth camera and IMU

【技术实现步骤摘要】
基于TOF深度相机与IMU的平仓机器人自主定位方法
本专利技术涉及粮仓机器人定位
，特别是涉及一种基于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和平移向量(xt,yt,1)，由旋转矩阵R求解出平仓机器人的姿态角(γt,θt,ψt)，得到平仓机器人的位姿(xt,yt,γt,θt,ψt)；其中，γt、θt、ψt分别为第t时刻平仓机器人相对导航坐标系的俯仰角、翻滚角、偏航角，(xt,yt)为第t时刻平仓机器人的位置坐标；步骤2：对IMU进行初始化；步骤3：通过初始化后的IMU解算平仓机器人的位姿；步骤3.1：将导航坐标系作为参考坐标系，计算第t时刻导航坐标系到机器人坐标系的关系为其中，为第t时刻平仓机器人在导航坐标系下的三轴角速度，为第t时刻陀螺仪采集的平仓机器人在机器人坐标系下的三轴角速度；步骤3.2：计算平仓机器人的初始四元数为其中，(γ0,θ0,ψ0)为平仓机器人的初始姿态；步骤3.3：采用二阶龙格库塔法，计算第t时刻平仓机器人的四元数为(λt，p1t，p2t，p3t)；步骤3.4：计算第t时刻平仓机器人的姿态矩阵为步骤3.5：计算第t时刻平仓机器人的姿态角为其中，T11(t)＝λt2+p1t2-p2t2-p3t2，T12(t)＝2(p1tp2t+λtp3t)，T13(t)＝2(p1tp3t-λtp2t)，T23(t)＝2(p2tp3t+λtp1t)，T33(t)＝λt2+p3t2-p1t2-p2t2；步骤3.6：根据四元数和姿态矩阵将加速度计采集的平仓机器人在机器人坐标系下的比力分量转换到导航坐标系中，在导航坐标系下通过积分求解比力方程，得到平仓机器人相对地球的速度，并由平仓机器人的速度积分得到第t时刻平仓机器人的位置(xt,yt)；步骤4：将步骤1中通过TOF深度相机解算出的平仓机器人的位姿作为状态向量Xt，将步骤3中通过IMU解算出的平仓机器人的位姿作为观测向量Zt，对步骤1和步骤3中解算出的位姿进行卡尔曼滤波融合，得到平仓机器人的位姿，完成平仓机器人的自主定位。所述步骤1.1中，提取第t时刻的粮仓图像的特征点的方法为FAST关键点方法，具体包括下述步骤：步骤1.1.1：在第t时刻的粮仓图像中随机选取像素点p；其中，像素点p的亮度为Ip；步骤1.1.2：以像素点p为中心，选取半径为3个像素的圆上的16个像素点；步骤1.1.3：若16个像素点中有连续的M个像素点的亮度大于Ip+T或小于Ip-T，则像素点p为特征点；步骤1.1.4：在第t时刻的粮仓图像中选取下一个像素点，重复上述步骤1.1.2至步骤1.1.3，直至第t时刻的粮仓图像中的所有像素点都被选取，得到第t时刻的粮仓图像的所有特征点。所述步骤4包括下述步骤：步骤4.1：构建平仓机器人定位的状态方程和观测方程分别为Xt＝AXt-1+But+WtZt＝HXt+Vt其中，A为状态转移矩阵，A、B均为常数矩阵，ut为控制量，Wt、Vt分别为第一噪声向量、第二噪声向量，H为观测矩阵；步骤4.2：估计第t时刻平仓机器人位姿的状态变量为步骤4.3：计算第t时刻平仓机器人位姿的协方差为Pt'＝APt-1AT+Q其中，Q为状态方程的方差；步骤4.4：计算第t时刻平仓机器人位姿的卡尔曼滤波增益为Kt＝Pt'HT[HPt'HT+Ra]-1其中，Ra为观测方程的方差；步骤4.5：用第t时刻平仓机器人位姿的观测量对第t时刻平仓机器人位姿的估计量进行修正，得到第t时刻平仓机器人位姿的最优估计值为将作为平仓机器人在第t时刻的最终位姿；步骤4.6：更新协方差为：Pt＝[I-KtH]Pt'其中，I为单位矩阵。本专利技术的有益效果为：本专利技术通过TOF深度相机和IMU分别解算平仓机器人的位姿，并将通过TOF深度相机解算出的平仓机器人的位姿作为状态向量、将通过IMU解算出的平仓机器人的位姿作为观测向量，对TOF深度相机和IMU分别解算出的位姿进行卡尔曼滤波融合，得到平仓机器人的位姿，能够提高平仓机器人在环境复杂的粮仓环境中定位的准确性。附图说明图1为本专利技术基于TOF深度相机与IMU的平仓机器人自主定位方法的流程图。具体实施方式下面将结合附图和具体实施方式，对本专利技术作进一步描述。如图1所示，为本专利技术基于TOF深度相机与IMU的平仓机器人自主定位方法的流程图。本专利技术的基于TOF深度相机与IMU的平仓机器人自主定位方法，所述平仓机器人上配置有TOF深度相机与IMU，其特征在于，包括下述步骤：步骤1：通过TOF深度相机解算平仓机器人的位姿：步骤1.1：通过TOF深度相机采集第t时刻的粮仓图像，提取第t时刻的粮仓图像的特征点，构建二维点云粮仓环境；步骤1.2：将第t-1时刻的粮仓图像作为参考帧，将第t时刻的粮仓图像作为当前帧，将当前帧与参考帧进行特征匹配，若当前帧与参考帧中有比例为α的特征点的位置坐标相同，则当前帧为关键帧；步骤1.3：通过当前帧与参考帧之间多组对应点的帧间匹配，计算本质矩阵E，对本质矩阵E进行分解，求出旋转矩阵R和平移向量(xt,yt,1)，由旋转矩阵R求解出平仓机器人的姿态角(γt,θt,ψt)，得到平仓机器人的位姿(xt,yt,γt,θt,ψt本文档来自技高网...

【技术保护点】
1.一种基于TOF深度相机与IMU的平仓机器人自主定位方法，所述平仓机器人上配置有TOF深度相机与IMU，其特征在于，包括下述步骤：步骤1：通过TOF深度相机解算平仓机器人的位姿：步骤1.1：通过TOF深度相机采集第t时刻的粮仓图像，提取第t时刻的粮仓图像的特征点，构建二维点云粮仓环境；步骤1.2：将第t‑1时刻的粮仓图像作为参考帧，将第t时刻的粮仓图像作为当前帧，将当前帧与参考帧进行特征匹配，若当前帧与参考帧中有比例为α的特征点的位置坐标相同，则当前帧为关键帧；步骤1.3：通过当前帧与参考帧之间多组对应点的帧间匹配，计算本质矩阵E，对本质矩阵E进行分解，求出旋转矩阵R和平移向量(xt,yt,1)，由旋转矩阵R求解出平仓机器人的姿态角(γt,θt,ψt)，得到平仓机器人的位姿(xt,yt,γt,θt,ψt)；其中，γt、θt、ψt分别为第t时刻平仓机器人相对导航坐标系的俯仰角、翻滚角、偏航角，(xt,yt)为第t时刻平仓机器人的位置坐标；步骤2：对IMU进行初始化；步骤3：通过初始化后的IMU解算平仓机器人的位姿；步骤3.1：将导航坐标系作为参考坐标系，计算第t时刻导航坐标系到机器人坐标系的关系为...

【技术特征摘要】
1.一种基于TOF深度相机与IMU的平仓机器人自主定位方法，所述平仓机器人上配置有TOF深度相机与IMU，其特征在于，包括下述步骤：步骤1：通过TOF深度相机解算平仓机器人的位姿：步骤1.1：通过TOF深度相机采集第t时刻的粮仓图像，提取第t时刻的粮仓图像的特征点，构建二维点云粮仓环境；步骤1.2：将第t-1时刻的粮仓图像作为参考帧，将第t时刻的粮仓图像作为当前帧，将当前帧与参考帧进行特征匹配，若当前帧与参考帧中有比例为α的特征点的位置坐标相同，则当前帧为关键帧；步骤1.3：通过当前帧与参考帧之间多组对应点的帧间匹配，计算本质矩阵E，对本质矩阵E进行分解，求出旋转矩阵R和平移向量(xt,yt,1)，由旋转矩阵R求解出平仓机器人的姿态角(γt,θt,ψt)，得到平仓机器人的位姿(xt,yt,γt,θt,ψt)；其中，γt、θt、ψt分别为第t时刻平仓机器人相对导航坐标系的俯仰角、翻滚角、偏航角，(xt,yt)为第t时刻平仓机器人的位置坐标；步骤2：对IMU进行初始化；步骤3：通过初始化后的IMU解算平仓机器人的位姿；步骤3.1：将导航坐标系作为参考坐标系，计算第t时刻导航坐标系到机器人坐标系的关系为其中，为第t时刻平仓机器人在导航坐标系下的三轴角速度，为第t时刻陀螺仪采集的平仓机器人在机器人坐标系下的三轴角速度；步骤3.2：计算平仓机器人的初始四元数为其中，(γ0,θ0,ψ0)为平仓机器人的初始姿态；步骤3.3：采用二阶龙格库塔法，计算第t时刻平仓机器人的四元数为(λt，p1t，p2t，p3t)；步骤3.4：计算第t时刻平仓机器人的姿态矩阵为步骤3.5：计算第t时刻平仓机器人的姿态角为其中，T11(t)＝λt2+p1t2-p2t2-p3t2，T12(t)＝2(p1tp2t+λtp3t)，T13(t)＝2(p1tp3t-λtp2t)，T23(t)＝2(p2tp3t+λtp1t)，T33(t)＝λt2+p3t2-p1t2-p2t2；步骤3.6：根据四元数和姿态矩阵将加速度计采集的平仓机器人在机器人坐标系下的比力分量转换到导航坐标系中，在导航坐标系下通过积分求解比力方程，得到...

【专利技术属性】
技术研发人员：田进波，贾烘腾，
申请(专利权)人：东北大学，
类型：发明
国别省市：辽宁,21