基于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)＝λt²+p1t²-p2t²-p3t²，t12(t)＝2(p1tp2t+λtp3t)，t13(t)＝2(p1tp3t-λtp2t)，t23(t)＝2(p2tp3t+λtp1t)，t33(t)＝λt²+p3t²-p1t²-p2t²；

步骤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+wt

zt＝hxt+vt

其中，a为状态转移矩阵，a、b均为常数矩阵，ut为控制量，wt、vt分别为第一噪声向量、第二噪声向量，h为观测矩阵；

步骤4.2：估计第t时刻平仓机器人位姿的状态变量为

步骤4.3：计算第t时刻平仓机器人位姿的协方差为

pt'＝apt-1a^t+q

其中，q为状态方程的方差；

步骤4.4：计算第t时刻平仓机器人位姿的卡尔曼滤波增益为

kt＝pt'h^t[hpt'h^t+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)；其中，γt、θt、ψt分别为第t时刻平仓机器人相对导航坐标系的俯仰角、翻滚角、偏航角，(xt,yt)为第t时刻平仓机器人的位置坐标。

所述步骤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时刻的粮仓图像的所有特征点。

其中，imu即惯性测量单元(inertialmeasurementunit)。粮仓图像的特征点包括角点、边缘和区块。本实施例中，α＝70％，t＝20％ip，m＝10。

步骤2：对imu进行初始化。

其中，对imu进行初始化包括：进行陀螺仪偏置估计，在tof深度相机坐标系和imu坐标系的变换间加入尺度因子s，进行尺度粗略估计、加速度偏置估计、尺度精确估计、速度估计。

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

机器人坐标系相对参考坐标系的方位可以完全由机器人坐标系依次绕3个不同的轴转动的3个角度来确定：

步骤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)＝λt²+p1t²-p2t²-p3t²，t12(t)＝2(p1tp2t+λtp3t)，t13(t)＝2(p1tp3t-λtp2t)，t23(t)＝2(p2tp3t+λtp1t)，t33(t)＝λt²+p3t²-p1t²-p2t²；

步骤3.6：根据四元数和姿态矩阵将加速度计采集的平仓机器人在机器人坐标系下的比力分量转换到导航坐标系中，在导航坐标系下通过积分求解比力方程，得到平仓机器人相对地球的速度，并由平仓机器人的速度积分得到第t时刻平仓机器人的位置(xt,yt)。

步骤4：将步骤1中通过tof深度相机解算出的平仓机器人的位姿作为状态向量xt，将步骤3中通过imu解算出的平仓机器人的位姿作为观测向量zt，对步骤1和步骤3中解算出的位姿进行卡尔曼滤波融合，得到平仓机器人的位姿，完成平仓机器人的自主定位。

卡尔曼滤波实际由两个过程组成：预测与校正。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的预测。在校正阶段，滤波器利用对当前状态的观测值修正在预测阶段获得的预测值，以获得一个更接近真实的新估计值。在本发明中，将视觉传感器即tof深度相机估计解算得到的位姿数据作为预测值，将imu解算得到的位姿数据作为校正值。

所述步骤4包括下述步骤：

步骤4.1：构建平仓机器人定位的状态方程和观测方程分别为

xt＝axt-1+but+wt

zt＝hxt+vt

其中，a为状态转移矩阵，a、b均为常数矩阵，ut为控制量，wt、vt分别为第一噪声向量、第二噪声向量，h为观测矩阵；

步骤4.2：估计第t时刻平仓机器人位姿的状态变量为

步骤4.3：计算第t时刻平仓机器人位姿的协方差为

pt'＝apt-1a^t+q

其中，q为状态方程的方差；

步骤4.4：计算第t时刻平仓机器人位姿的卡尔曼滤波增益为

kt＝pt'h^t[hpt'h^t+ra]^-1

其中，ra为观测方程的方差；

步骤4.5：用第t时刻平仓机器人位姿的观测量对第t时刻平仓机器人位姿的估计量进行修正，得到第t时刻平仓机器人位姿的最优估计值为

将作为平仓机器人在第t时刻的最终位姿；

步骤4.6：更新协方差为：

pt＝[i-kth]pt'

其中，i为单位矩阵。

显然，上述实施例仅仅是本发明的一部分实施例，而不是全部的实施例。上述实施例仅用于解释本发明，并不构成对本发明保护范围的限定。基于上述实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，也即凡在本申请的精神和原理之内所作的所有修改、等同替换和改进等，均落在本发明要求的保护范围内。