基于惯性测量辅助视觉的运输车空间定位方法及系统技术方案

本发明专利技术公开了基于惯性测量辅助视觉的运输车空间定位方法及系统，采集AGV小车的IMU传感器数据；采集AGV小车周围环境的连续图像；将IMU传感器数据按照时间块进行累加得到IMU传感器数据块；对图像进行预处理，得到特征点匹配数据；利用得到的IMU传感器数据块和的特征点匹配数据，估计出相机的位置和方向；根据相机的位置和方向，绘制相机的运动轨迹图；地图匹配：将相机所在的世界坐标系与真实场景所在的世界坐标系进行坐标转换，使得AGV得到的位置和路径能与真实场景地图在位置和路径上进行匹配；获得AGV小车在真实场景中的位置以及所有可能的运行路径。本发明专利技术能达到AGV小车精准空间定位的目的。

【技术实现步骤摘要】
基于惯性测量辅助视觉的运输车空间定位方法及系统
本专利技术涉及基于惯性测量辅助视觉的运输车空间定位方法及系统。
技术介绍
AGV(AutomatedGuidedVehicle，自动导引运输车)系统，也称AGV小车，包括定位和导航两部分。定位主要有两大主流：基于激光雷达的定位系统、基于视觉的定位系统。基于激光雷达的定位系统的原理是激光测距，即向环境中发射红外激光和接收红外激光，获得时间差，进而得到物体的距离。激光雷达定位系统运行稳定、精度较高，但激光雷达价格过于昂贵，并且激光雷达不能感知周围的一些视觉信息，比如周围环境的颜色以及某些特殊标志。基于视觉的定位系统是通过图像采集设备收集近距离的环境信息，并利用计算机视觉技术进行图像处理获得环境信息，实现定位。在图像采集设备中，单目相机因其简单的结构和原理，而被广泛应用。但基于单目相机作为视觉传感器，仍有一些缺陷：1.没有深度信息，导致定位不准确；2.根据特征点追踪估计出的相机位置和方向存在误差，并不断累积，导致小车偏离路径。
技术实现思路
基于以上问题，我们采用惯性测量单元(IMU,InertialMeasurementUnit)辅助单目相机实现空间定位，提出了一种基于惯性测量辅助视觉的运输车空间定位方法及系统，其中IMU的估计加视觉特征的精准匹配，使得系统定位精度很高。本专利技术能够在比较复杂的环境中也可以实现AGV的精准定位功能。第一方面，本专利技术提供基于惯性测量辅助视觉的运输车空间定位方法，包括：步骤(1)：采集AGV小车的IMU传感器数据，所述IMU传感器数据，包括：AGV小车角速度和加速度；步骤(2)：采集AGV小车周围环境的连续图像；步骤(3)：将IMU传感器数据按照时间块进行累加得到IMU传感器数据块，使得IMU传感器数据块输出的频率与采集的连续图像的输出频率保持一致，并且在时间上对齐；步骤(4)：对步骤(2)的图像进行预处理，得到特征点匹配数据；步骤(5)：利用步骤(3)得到的IMU传感器数据块和步骤(4)得到的特征点匹配数据，估计出相机的位置和方向；步骤(6)：根据步骤(5)得到的相机的位置和方向，绘制相机的运动轨迹图；步骤(7)：地图匹配：将相机所在的世界坐标系与真实场景所在的世界坐标系进行坐标转换，使得AGV得到的位置和路径能与真实场景地图在位置和路径上进行匹配；获得AGV小车在真实场景中的位置以及所有可能的运行路径。所述步骤(3)的步骤为：假设从IMU传感器获得的角速度为ωr，加速度为αr,但由于传感器受外界噪声的干扰以及自身硬件的限制，存在测量偏差σ，外界噪声干扰η，实际获得的IMU传感器数据：其中，σg是角速度偏差，σα是加速度偏差，ηα表示外界噪声对角速度的干扰，ηg表示外界噪声对加速度的干扰，ωr表示带有误差的角速度，αr表示带有误差的加速度；假设[t,t+Δt]表示第k帧与第k+1帧两幅图像之间的时间间隔，按照公式(2)对IMU传感器数据进行预积分，得到两帧图像之间的IMU传感器数据块：所述步骤(4)图像预处理的步骤如下：步骤(41)：特征点检测：采用FAST算法对步骤(2)的每一帧图像进行特征点检测；步骤(42)：特征点匹配：对所有的连续两帧的图像的特征点进行匹配，得到同一个特征点在连续两帧图像中的相对位置；步骤(43)：关键帧提取：如果当前帧图像的视差大于设定阈值，或者当前帧图像比前一帧图像存在大于设定阈值个数的新特征点，则当前帧视为关键帧；步骤(44)：提取关键帧图像的特征点的BRIEF描述算子，将关键帧数据按照时间先后顺序存入关键帧数据库中，所述关键帧数据包括：特征点、BRIEF描述算子和IMU传感器数据块；利用DBOW2算法，将BRIEF描述算子以字典的形式存入BoW词袋模型中；所述BRIEF描述算子用于描述特征点，将关键帧图像中的特征点的描述算子以字典的形式存入BoW词袋模型，目的是做为后续的回(闭)环检测的数据库。回环检测需要对比当前的关键帧与之前存入的关键帧是否为同一帧，进而判断小车是否重复在某个位置。判断是否为同一帧的方法就是判断两个关键帧图像的特征点的描述算子是否近似。BoW词袋模型存储描述算子的效果：由于描述子维度比较高，词袋存储简单；由于词袋是树形结构，便于对描述算子相似度的计算，回环检测时有利。所述步骤(5)的步骤为：步骤(51)：初始化阶段:初始化阶段根据连续两幅图像的相互匹配的特征点的关系，利用对极几何约束获得初始阶段的相机的估计旋转量和平移量；再根据估计出来的旋转量与平移量，基于三角测量的方法估计出特征点的三维坐标；所述步骤(51)对极几何约束：若空间中的点P在图像平面上对应的是点p，根据小孔成像原理可知，空间中的点P、图像中的点p以及相机的光心在一条直线上。那么两幅图像上相匹配的两个特征点就得到一个对极几何约束方程：其中，空间中的点P在两幅图像中的像素点分别为p1和p2；求解对极几何约束方程就获得相机的估计旋转量R和相机的估计平移量t。利用三角测量的方法获得图像的深度信息s1和s2，进而获得像素的三维坐标。步骤(52)：运动阶段，根据像素的三维坐标和像素二维坐标的对应关系，利用透视n点法PnP估计相机的位置和方向；利用BundleAdjustment方式求解PnP问题，得到相机的位置和方向。所述步骤(52)的步骤为：n个三维空间点P及其投影p，利用李代数ξ表示希望得到的相机旋转量R和平移量t，K是相机的内参，假设三维空间点坐标为Pi＝[Xi,Yi,Zi]T,像素点坐标为εi＝[μi,υi]T，根据相机成像原理得到像素二维坐标与三维空间点坐标的关系：像素二维坐标与三维空间点坐标的关系存在一个误差；把误差求和，构建最小二乘问题，寻找最好的相机位姿，使误差最小化：误差就是将像素二维坐标与三维空间点坐标按照当前估计的位姿进行投影得到的位置相比较得到的误差，其中φi＝[μi,υi,1]T。总之，通过特征点匹配，p1和p2是同一个空间点P的投影，初始化阶段得到像素点的三维坐标，经过投影得到p2与实际的p2之间存在距离，通过调整相机位姿，使得投影得到p2与实际的p2之间存在距离消失，从而得到相机的位置和方向。所述步骤(7)的步骤为：在真实场景地图中固定A、B、C三个点，相机从A点到B点，得到运动轨迹并获得B点相对于A的位置和方向。通过坐标系之间的转换矩阵，获得A、B点在真实场景地图中的位置和真实距离。假设A为初始点，B点在以A点为原点的世界坐标系下的坐标为在真实场景中的坐标系为两个坐标系之间相差一个尺度M和平移ΔS＝[ΔX,ΔY,ΔZ]T、旋转R0，转换关系如下：公式(5)中有三个未知数：尺度M、平移ΔS和旋转R0，通过A、B、C坐标，求解三个未知数，然后通过公式(5)随时求出相机在真实场景中的位置坐标。所述步骤(6)和步骤(7)之间还包括：步骤(70)：闭环检测；如果当前帧是关键帧，将关键帧中特征点的BRIEF描述算子与之前BoW词袋模型中关键帧的描述算子进行对比，关键帧中特征点的BRIEF描述算子与之前BoW词袋模型中关键帧的描述算子相似度大于设定比例，就认为两个关键帧相似，如果获得到连续设定个数的关键帧相似，说明AGV小车之前到达过当前位置。第二方面，本专利技术提供一种计算机可读存储介质，其上存储有计算本文档来自技高网...

【技术保护点】
基于惯性测量辅助视觉的运输车空间定位方法，其特征是，包括：步骤(1)：采集AGV小车的IMU传感器数据，所述IMU传感器数据，包括：AGV小车角速度和加速度；步骤(2)：采集AGV小车周围环境的连续图像；步骤(3)：将IMU传感器数据按照时间块进行累加得到IMU传感器数据块，使得IMU传感器数据块输出的频率与采集的连续图像的输出频率保持一致，并且在时间上对齐；步骤(4)：对步骤(2)的图像进行预处理，得到特征点匹配数据；步骤(5)：利用步骤(3)得到的IMU传感器数据块和步骤(4)得到的特征点匹配数据，估计出相机的位置和方向；步骤(6)：根据步骤(5)得到的相机的位置和方向，绘制相机的运动轨迹图；步骤(7)：地图匹配：将相机所在的世界坐标系与真实场景所在的世界坐标系进行坐标转换，使得AGV得到的位置和路径能与真实场景地图在位置和路径上进行匹配；获得AGV小车在真实场景中的位置以及所有可能的运行路径。

【技术特征摘要】
1.基于惯性测量辅助视觉的运输车空间定位方法，其特征是，包括：步骤(1)：采集AGV小车的IMU传感器数据，所述IMU传感器数据，包括：AGV小车角速度和加速度；步骤(2)：采集AGV小车周围环境的连续图像；步骤(3)：将IMU传感器数据按照时间块进行累加得到IMU传感器数据块，使得IMU传感器数据块输出的频率与采集的连续图像的输出频率保持一致，并且在时间上对齐；步骤(4)：对步骤(2)的图像进行预处理，得到特征点匹配数据；步骤(5)：利用步骤(3)得到的IMU传感器数据块和步骤(4)得到的特征点匹配数据，估计出相机的位置和方向；步骤(6)：根据步骤(5)得到的相机的位置和方向，绘制相机的运动轨迹图；步骤(7)：地图匹配：将相机所在的世界坐标系与真实场景所在的世界坐标系进行坐标转换，使得AGV得到的位置和路径能与真实场景地图在位置和路径上进行匹配；获得AGV小车在真实场景中的位置以及所有可能的运行路径。2.如权利要求1所述的基于惯性测量辅助视觉的运输车空间定位方法，其特征是，所述步骤(3)的步骤为：假设从IMU传感器获得的角速度为ωr，加速度为αr,但由于传感器受外界噪声的干扰以及自身硬件的限制，存在测量偏差σ，外界噪声干扰η，实际获得的IMU传感器数据：其中，σg是角速度偏差，σα是加速度偏差，ηα表示外界噪声对角速度的干扰，ηg表示外界噪声对加速度的干扰，ωr表示带有误差的角速度，αr表示带有误差的加速度；假设[t,t+Δt]表示第k帧与第k+1帧两幅图像之间的时间间隔，按照公式(2)对IMU传感器数据进行预积分，得到两帧图像之间的IMU传感器数据块：3.如权利要求1所述的基于惯性测量辅助视觉的运输车空间定位方法，其特征是，所述步骤(4)图像预处理的步骤如下：步骤(41)：特征点检测：采用FAST算法对步骤(2)的每一帧图像进行特征点检测；步骤(42)：特征点匹配：对所有的连续两帧的图像的特征点进行匹配，得到同一个特征点在连续两帧图像中的相对位置；步骤(43)：关键帧提取：如果当前帧图像的视差大于设定阈值，或者当前帧图像比前一帧图像存在大于设定阈值个数的新特征点，则当前帧视为关键帧；步骤(44)：提取关键帧图像的特征点的BRIEF描述算子，将关键帧数据按照时间先后顺序存入关键帧数据库中，所述关键帧数据包括：特征点、BRIEF描述算子和IMU传感器数据块；利用DBOW2算法，将BRIEF描述算子以字典的形式存入BoW词袋模型中。4.如权利要求3所述的基于惯性测量辅助视觉的运输车空间定位方法，其特征是，所述步骤(5)的步骤为：步骤(51)：初始化阶段:初始化阶段根据连续两幅图像的相互匹配的特征点的关系，利用对极几何约束获得初始阶段的相机的估计旋转量和平移量；再根据估计出来的旋转量与平移量，基于三角测量的方法估计出特征点的三维坐标；步骤(52)：运动阶段，根据像素的三维坐标和像素二维坐标的对应关系，利用透视n点法PnP估计相机的位置和方向；利用BundleAdjustment方式求解PnP问题，得到相机的位置和方向。5.如权利要求4所述的基于惯性测量辅助视觉的运输...

【专利技术属性】
技术研发人员：刘宇，杨卓峰，朱利霞，张玉娟，
申请(专利权)人：上海文什数据科技有限公司，
类型：发明
国别省市：上海,31