动态稀疏环境下机器人SLAM物体状态检测方法技术

本发明专利技术涉及动态稀疏环境下机器人SLAM物体状态检测方法，首先，通过视觉传感器对环境进行图像采集，使用SURF描述子获得图像的特征向量集合；其次，基于最近邻算法对当前时刻与历史时刻的图像进行匹配并通过RANSAC算法检验匹配是否成功以判断两个时刻的物体是否一致；然后，利用视差法获得物体的深度信息，并根据平面几何关系进一步得到物体两个时刻的全局坐标和相对位置差；最后，结合假设检验的思想得到接受区域，并通过检验相对位置差是否落在接受区域内来判断物体的状态。本发明专利技术在对环境中物体状态进行检测时，考虑移动机器人的定位和测量误差对物体位置观测值的影响，提高对物体状态判断的准确度。

【技术实现步骤摘要】
动态稀疏环境下机器人SLAM物体状态检测方法
本专利技术属于移动机器人导航
，涉及一种动态稀疏环境下面向移动机器人同步定位与地图构建(simultaneouslocalizationandmapping,SLAM)问题的物体状态检测方法。
技术介绍
SLAM技术被称为自主移动机器人领域的“圣杯(Holygrail)”，是移动机器人实现完全智能化的关键。随着移动机器人在物流、探测、服务等领域的广泛应用，SLAM问题成为了移动机器人领域的研究热点，并日益受到学术界及工程界密切关注。当移动机器人在动态环境中工作时，有必要区别对待静态物体和动态物体：仅有静态物体才能作为路标为机器人提供定位信息，而动态物体只能被当作障碍物进行避障处理。当机器人在稀疏环境（即能够利用的路标比较稀少的工作环境）下工作时，从环境中获得的信息大大减少，导致地图构建和定位的精度显著下降，从而加大了动静态物体的区分难度，同时又反作用于机器人的地图构建和定位的精度，于是就陷入一个恶性循环。因此，一个有效的动态稀疏环境下移动机器人物体状态检测方法具有重要的理论意义及应用价值。Ila等人利用Pose-SLAM方法，采用机器人的历史位姿构建地图，从而解决了由于路标稀缺导致环境信息减少的问题；而Yaghmaie等人提出利用三态地图(thethree-statemap)作为动态路标检测的依据，从而解决了动态环境下的SLAM问题。然而，前者只考虑了静态环境，无法解决动态环境下移动机器人的SLAM问题；而后者虽然考虑到了动态环境，但其前提是路标足够多，当路标比较稀缺时，由环境信息减少而导致定位精度下降容易使得静态物体被错误地判断成动态物体。
技术实现思路
移动机器人在路标稀少情况下难以准确判断物体状态的问题，提供一种动态稀疏环境下面向移动机器人SLAM问题的物体状态检测方法，在对环境中物体状态进行检测时，考虑移动机器人的定位和测量误差对物体位置观测值的影响，提高物体状态检测的准确性。动态稀疏环境下机器人SLAM物体状态检测方法，包括以下步骤：步骤1：采集环境图像，获取图像的特征向量集合。步骤1.1，移动机器人通过双目视觉传感器采集环境图像。步骤1.2，利用多尺度多方向Gabor函数构建能量图像空间，并采用非极大抑制方法对8点邻域范围内检测的极值点进行筛选。步骤1.3，通过SURF描述子构建向量以表示特征点附近的局部区域，获得当前图像的特征向量集合。步骤2：判断当前图像与历史图像中的物体是否一致。步骤2.1，求当前图像与某时刻历史图像的匹配点。利用最近邻算法计算当前图像的每一个特征点对应的特征向量与历史图像所有特征向量的余弦相似度。取余弦相似度最大值点作为图像匹配点，求出当前图像与历史图像的所有匹配点。步骤2.2，判断两个时刻图像中物体的一致性。通过RANSAC算法检验匹配是否成功以判断这两个时刻图像中的物体是否一致，如果一致则进行步骤3；如果不一致但当前图像的视觉显著度大于阈值则保存并返回步骤1，否则，直接返回步骤1。步骤3：利用视差法获取图像中物体的深度信息，并计算该物体两个时刻的全局坐标和相对位置差。步骤3.1：获取图像中物体的深度信息。在极线几何约束下，根据三角测量原理，并利用双目视觉传感器获取匹配位置下的视差图像来提取物体的深度信息。步骤3.2：求物体在历史时刻和当前时刻的坐标及其相对位置差。步骤4：判断物体的状态。步骤4.1：假设物体为静止状态，求其相对位置差的期望和协方差矩阵。步骤4.2：根据协方差矩阵构造一个大小随之改变的区域A，以及一个以非当前路标为圆心、r为半径的固定区域B，取两者的并集C＝A∪B作为接受区域判断物体的状态。步骤4.3：判断物体的状态。如果相对位置差落在区域C内，则接受原假设，认为物体是静态；否则，则拒绝原假设，认为物体是动态。与现有技术相比，本专利技术具有以下有益效果：本专利技术在对环境中物体状态进行检测时，考虑了移动机器人的定位和测量误差对物体位置观测值的影响，从而提高了对物体状态判断的准确度，解决了以往方法在动态稀疏环境中由于机器人定位误差而引起的物体状态误判问题，为后续的地图构建与定位打下良好的基础。附图说明图1是本专利技术所涉及方法的流程图；图2是本专利技术双目视觉传感器模型示意图；图3是本专利技术实施例所建立的全局和局部坐标系示意图；图4是本专利技术中的图形匹配的结果图：（a）为当前时刻图像，（b）为历史图像；图5是图4（a）的图像视觉BoW直方图；图6是本专利技术中的移动机器人的SLAM仿真结果图。具体实施方式本专利技术所涉及方法的流程如图1所示，包括以下步骤：步骤1：采集环境图像，获取图像的特征向量集合。步骤1.1，移动机器人通过双目视觉传感器采集环境图像。步骤1.2，利用多尺度多方向Gabor函数构建能量图像空间，并采用非极大抑制方法对8点邻域范围内检测的极值点进行筛选。（1）基于Gabor函数设计一组多方向的奇对称与偶对称滤波器：式中，σ为图像所在的尺度，x，y，θ分别为图像点所在坐标及其方向，gsin和gcos分别为奇、偶对称滤波器的传输函数。（2）对图像进行多尺度多方向滤波：式中，I(x,y)为图像，Weven(x,y,σ,θ)、Wodd(x,y,σ,θ)分别为图像与偶对称及奇对称滤波器的滤波输出。（3）对于各个方向的滤波器输出求二范数得到各个尺度下各方向的能量函数：E(x,y,σ,θ)＝||W(x,y,σ,θ)||式中，E(x,y,σ,θ)为能量函数。（4）对于同一尺度下各方向能量函数采用统一权值求和，得到图像的多尺度能量图像：式中，k＝1,2,......,N，N为方向总数，本专利技术中取N=8，8个方向分别为θk=0°，22.5°，45°，67.5°，90°，112.5°，135°，157.5°。（5）在能量图像中的点邻域内寻找能量图像中的极大值点，并提取能量极大值大于阈值的能量点为特征点。所述点邻域通常取8点邻域，所述阈值通常取最大能量的一半。步骤1.3，通过SURF描述子构建向量以表示特征点附近的局部区域，获得当前图像的特征向量集合。（1）对以特征点为圆心、以6σ为半径的邻域中的点计算X和Y方向上的Haar小波响应。（2）以60°的扇形滑动窗口绕圆心遍历整个圆，计算窗口内的响应总和，并取最长向量作为特征点主方向。（3）以特征点为中心，主方向为X方向构造一个大小为20σ的方框，将其分成4×4个子区域。对每一个子区域分别计算Σdx，Σdy，Σ|dx|和Σ|dy|，其中，dx、dy分别为子区域内各点X和Y方向上的Haar小波响应。则每一个子区域可用向量v＝(Σdx,Σdy,Σ|dx|,Σ|dy|)T表示，这样特征点附近的局部区域可以用一个4×4×4＝64维的特征向量描述。步骤2：判断当前图像与历史图像中的物体是否一致。步骤2.1，求当前图像特征点与某时刻历史图像的匹配点。求当前图像某一特征点对应的特征向量与历史图像所有特征点对应特征向量的余弦相似度，相似度最大的点即为所求的匹配点。匹配示意图如图4所示。利用最近邻算法计算两特征向量v1,v2的余弦相似度S，公式如下：式中，v1,v2均为单位向量。按照上述方法求出当前图像所有特征点的匹配点。步骤2.2，判断两个时刻图像中物体的一致性。（1）随机选取两个时刻图像的3对匹配本文档来自技高网...

【技术保护点】
动态稀疏环境下机器人SLAM物体状态检测方法，其特征在于，在检测物体状态时考虑移动机器人的定位和测量误差对物体位置观测值的影响；所述方法包括以下步骤：步骤1：采集环境图像，获取图像的特征向量集合；步骤1.1，移动机器人通过双目视觉传感器采集环境图像；步骤1.2，利用多尺度多方向Gabor函数构建能量图像空间，并采用非极大抑制方法对8点邻域范围内检测的极值点进行筛选；步骤1.3，通过SURF描述子构建向量以表示特征点附近的局部区域，获得当前图像的特征向量集合；（1）对以特征点为圆心、以6σ为半径的邻域中的点计算X和Y方向上的Haar小波响应；（2）以60°的扇形滑动窗口绕圆心遍历整个圆，计算窗口内的响应总和，并取最长向量作为特征点主方向；（3）以特征点为中心，主方向为X方向构造一个大小为20σ的方框，将其分成4×4个子区域；对每一个子区域分别计算Σdx，Σdy，Σ|dx|和Σ|dy|，其中，dx、dy分别为子区域内各点X和Y方向上的Haar小波响应；则每一个子区域可用向量v＝(Σdx,Σdy,Σ|dx|,Σ|dy|)T表示，这样特征点附近的局部区域可以用一个4×4×4＝64维的特征向量描述；步骤2：判断当前图像与历史图像中的物体是否一致；步骤2.1，求当前图像特征点与某时刻历史图像的匹配点；求当前图像某一特征点对应的特征向量与历史图像所有特征点对应特征向量的余弦相似度，相似度最大的点即为所求的匹配点；利用最近邻算法计算两特征向量v1,v2的余弦相似度S，公式如下：S=cos<v1,v2>=v1·v2|v1||v2|=v1·v2]]>式中，v1,v2均为单位向量；按照上述方法求出当前图像所有特征点的匹配点；步骤2.2，判断两个时刻图像中物体的一致性；（1）随机选取两个时刻图像的3对匹配点，并求出它们之间的变换矩阵H；（2）由变换矩阵H计算其它匹配点的误差，当误差小于阈值时，认为该匹配点支持当前的变换矩阵H；所述阈值通常取0.002；（3）如果有超过2/3匹配点支持当前的变换矩阵H，则认为图像匹配成功；否则，返回步骤（1），若循环N次仍未满足条件则认为图像匹配失败；N通常取1000；（4）如果匹配失败，计算当前图像的视觉显著性，若当前图像的视觉显著性大于阈值则保存此图像；图像匹配失败说明了移动机器人到了一个新的场景，如果这个新场景的特征足够丰富，可将此图像作为路标图像予以保存；新场景的特征是否足够丰富，根据图像的视觉显著性进行判断；图像的视觉显著性按下面方法求得：首先利用余弦相似度对图像中的特征进行分类，将余弦相似度大于阈值的两个特征向量划为同一类；阈值通常取0.6；然后，构建图像的视觉BoW模型，此时图像的特征丰富程度可用其视觉BoW直方图的熵来描述：Hi=-Σk=1W(t)p(wk)log2p(wk)]]>p(wk)=wkΣk=1W(t)wk]]>其中，p(wk)为图像视觉BoW特征的经验分布，wk为第k个直方图的长度，W(t)为此视觉BoW空间的大小；对上式进行归一化得到视觉显著度为：SLi=Hilog2W(t)]]>按照上式计算图像的视觉显著度，如果大于设定的阈值则认为该图像的具有足够丰富的视觉特征予以保存；阈值通常取0.4；步骤3：利用视差法获取图像中物体的深度信息，并计算该物体两个时刻的坐标和相对位置差；步骤3.1：获取图像中物体的深度信息；A，B为安装在机器人身上的两个摄像头，方向与机器人朝向方向一致；O为A、B的连线中点；由几何关系求得物体的深度信息y为：y=L×fd1-d2]]>其中，D1、D2分别为物体与两个摄像头光学中心线的距离，L＝D1‑D2为基线宽度，d1、d2分别为物体在图像平面上的投影与光学中心线的距离；步骤3.2：计算物体两个时刻的坐标和相对位置差；以机器人初始时刻两摄像头A、B连线中点O为坐标原点，以机器人朝向为X轴方向，以过O点平行于地面且垂直于X轴的方向为Y轴方向建立全局直角坐标系XOY；以机器人当前时刻两摄像头连线中点p＝(px,py)为坐标原点p，以机器人朝向方向为X’轴方向，以过p点平行于地面且垂直于X’轴的方向为Y’轴方向建立局部直角坐标系X’pY’；根据步骤3.1中获得的物体深度信息y可得物体与机器人的相对位置差，即物体的局部坐标m'为：m′=(y,-yf(d1+d22))]]>当前时刻物体的全局坐标m为：m＝p+ΔmΔm＝(m'ysinθ+m'xcosθ,m'ycosθ‑m'xsinθ)其中，Δm为物体与机器人的位置差在全局坐标系中的坐标，p＝(px,py)为机器人在全局坐标系中的坐标，θ为机器人的方向角；将物体两个时刻的位置做差可得此物体在i、j两个时刻的相对位置差Δmij＝mi‑mj；i...

【技术特征摘要】
1.动态稀疏环境下机器人SLAM物体状态检测方法，其特征在于，在检测物体状态时考虑移动机器人的定位和测量误差对物体位置观测值的影响；所述方法包括以下步骤：步骤1：采集环境图像，获取图像的特征向量集合；步骤1.1，移动机器人通过双目视觉传感器采集环境图像；步骤1.2，利用多尺度多方向Gabor函数构建能量图像空间，并采用非极大抑制方法对8点邻域范围内检测的极值点进行筛选；步骤1.3，通过SURF描述子构建向量以表示特征点附近的局部区域，获得当前图像的特征向量集合；(1)对以特征点为圆心、以6σ为半径的邻域中的点计算X和Y方向上的Haar小波响应，σ为高斯滤波的方差；(2)以60°的扇形滑动窗口绕圆心遍历整个圆，计算窗口内的响应总和，并取最长向量作为特征点主方向；(3)以特征点为中心，主方向为X方向构造一个大小为20σ的方框，将其分成4×4个子区域；对每一个子区域分别计算Σdx，∑dy，∑|dx|和∑|dy|，其中，dx、dy分别为子区域内各点X和Y方向上的Haar小波响应；则每一个子区域可用向量v＝(∑dx,∑dy,∑|dx|,∑|dy|)T表示，这样特征点附近的局部区域可以用一个4×4×4＝64维的特征向量描述；步骤2：判断当前图像与历史图像中的物体是否一致；步骤2.1，求当前图像特征点与某时刻历史图像的匹配点；求当前图像某一特征点对应的特征向量与历史图像所有特征点对应特征向量的余弦相似度，相似度最大的点即为所求的匹配点；利用最近邻算法计算两特征向量v1,v2的余弦相似度S，公式如下：式中，v1,v2均为单位向量；按照上述方法求出当前图像所有特征点的匹配点；步骤2.2，判断两个时刻图像中物体的一致性；(1)随机选取两个时刻图像的3对匹配点，并求出它们之间的变换矩阵H；(2)由变换矩阵H计算其它匹配点的误差，当误差小于阈值时，认为该匹配点支持当前的变换矩阵H；所述阈值通常取0.002；(3)如果有超过2/3匹配点支持当前的变换矩阵H，则认为图像匹配成功；否则，返回步骤(1)，若循环N次仍未满足条件则认为图像匹配失败；N通常取1000；(4)如果匹配失败，计算当前图像的视觉显著性，若当前图像的视觉显著性大于阈值则保存此图像；图像匹配失败说明了移动机器人到了一个新的场景，如果这个新场景的特征足够丰富，可将此图像作为路标图像予以保存；新场景的特征是否足够丰富，根据图像的视觉显著性进行判断；图像的视觉显著性按下面方法求得：首先利用余弦相似度对图像中的特征进行分类，将余弦相似度大于阈值的两个特征向量划为同一类；阈值通常取0.6；然后，构建图像的视觉BoW模型，此时图像的特征丰富程度可用其视觉BoW直方图的熵来描述：其中，p(wk)为图像视觉BoW特征的经验分布，wk为第k个直方图的长度，W(t)为此视觉BoW空间的大小；对上式进行归一化得到视觉显著度为：按照上式计算图像的视觉显著度，如果大于设定的阈值则认为该图像的具有足够丰富的视觉特征予以保存；阈值通常取0.4；步骤3：利用视差法获取图像中物体的深度信息，并计算该物体两个时刻的坐标和相对位置差；步骤3.1：获取图像中物体的深度信息；A，B为安装在机器人身上的两个摄像头，方向与机器人朝向方向一致；O为A、B的连线中点；由几何关系求得物体的深度信息y为：其中，D1、D2分别为物体与两个摄像头光学中心线的距离，L＝D1-D2为基线宽度，d1、d2分别为物体在图像平面上的投影与左右摄像头光学中心线的距离，f表示焦距；步骤3.2：计算物体两个时刻的坐标和相对位置差；以机器人初始时刻两摄像头A、B连线中点O为坐标原点，以机器人朝向为X轴方向，以过O点平行于地面且垂直于X轴的方向为Y轴方向建立全局直角坐标系XOY；以机器人当前时刻两摄像头连线中点p＝(px,py)为坐标原点p，以机器人朝向方向为X’轴方向，以过p点平行于地面且垂直于X’轴的方向为Y’轴方向建立局部直角坐标系X’pY’；根据步骤3.1中获得的物体深度信息y可得物体与机器人的相对位置差，即物体的局部坐标m'为：当前时刻物体的全局坐标m为：m＝p+ΔmΔm＝(m'ysinθ+m'xcosθ,m'ycosθ-m'xsinθ)其中，Δm为物体与机器人的位置差在全局坐标系中的坐标，p＝(px,py)为机器人在全局坐标系中的坐标，θ为机器人的方向角；将物体两个时刻的位置做差可得此物体在i、j两个时刻的相对位置差Δmij＝mi-mj；i为当前时刻，j为匹配成功的历史时刻；步骤4：判断物体的状态；步骤4.1：假设物体为静止状态，计算在此假设下Δmij的期望μm与方差Mm；假设“-”表示求期望；...

【专利技术属性】
技术研发人员：曹政才，黄志明，付宜利，马逢乐，陈嵩，翁志萍，王炅，
申请(专利权)人：北京化工大学，
类型：发明
国别省市：北京;11