CN103824080A - 动态稀疏环境下机器人slam物体状态检测方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于移动机器人导航技术领域，涉及一种动态稀疏环境下面向移动机器人同步定位与地图构建(simultaneous localization and mapping,SLAM)问题的物体状态检测方法。

背景技术

SLAM技术被称为自主移动机器人领域的“圣杯(Holy grail)”，是移动机器人实现完全智能化的关键。随着移动机器人在物流、探测、服务等领域的广泛应用，SLAM问题成为了移动机器人领域的研究热点，并日益受到学术界及工程界密切关注。

当移动机器人在动态环境中工作时，有必要区别对待静态物体和动态物体：仅有静态物体才能作为路标为机器人提供定位信息，而动态物体只能被当作障碍物进行避障处理。当机器人在稀疏环境（即能够利用的路标比较稀少的工作环境）下工作时，从环境中获得的信息大大减少，导致地图构建和定位的精度显著下降，从而加大了动静态物体的区分难度，同时又反作用于机器人的地图构建和定位的精度，于是就陷入一个恶性循环。因此，一个有效的动态稀疏环境下移动机器人物体状态检测方法具有重要的理论意义及应用价值。

Ila等人利用Pose-SLAM方法，采用机器人的历史位姿构建地图，从而解决了由于路标稀缺导致环境信息减少的问题；而Yaghmaie等人提出利用三态地图(the three-state map)作为动态路标检测的依据，从而解决了动态环境下的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函数设计一组多方向的奇对称与偶对称滤波器：

$g_{\sin }=\frac{1}{2{\mathrm{\π\σ}}^2}\exp (-\frac{x^2+y^2}{2{\mathrm{\σ}}^2})\sin \left(\frac{\mathrm{\σ}}{\mathrm{\π}}(x\cos \mathrm{\θ}+y\sin \mathrm{\θ})\right)$

$g_{\cos }=\frac{1}{2{\mathrm{\π\σ}}^2}\exp (-\frac{x^2+y^2}{2{\mathrm{\σ}}^2})\cos \left(\frac{\mathrm{\σ}}{\mathrm{\π}}(x\cos \mathrm{\θ}+y\sin \mathrm{\θ})\right)$

式中，σ为图像所在的尺度，x，y，θ分别为图像点所在坐标及其方向，g_sin和g_cos分别为奇、偶对称滤波器的传输函数。

（2）对图像进行多尺度多方向滤波：

$W_{\mathrm{even}}(x,y,\mathrm{\σ},\mathrm{\θ})=I(x,y)\⊗g_{\cos }(x,y,\mathrm{\σ},\mathrm{\θ})$

$W_{\mathrm{odd}}(x,y,\mathrm{\σ},\mathrm{\θ})=I(x,y)\⊗g_{\sin }(x,y,\mathrm{\σ},\mathrm{\θ})$

式中，I(x,y)为图像，W_even(x,y,σ,θ)、W_odd(x,y,σ,θ)分别为图像与偶对称及奇对称滤波器的滤波输出。

（3）对于各个方向的滤波器输出求二范数得到各个尺度下各方向的能量函数：

E(x,y,σ,θ)＝||W(x,y,σ,θ)||

式中，E(x,y,σ,θ)为能量函数。

（4）对于同一尺度下各方向能量函数采用统一权值求和，得到图像的多尺度能量图像：

$E(x,y,\mathrm{\σ})=\underset{k=1}{\overset{N}{\mathrm{\Σ}}}E(x,y,\mathrm{\σ},{\mathrm{\θ}}_k)$

式中，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个子区域。对每一个子区域分别计算Σd_x，Σd_y，Σ|d_x|和Σ|d_y|，其中，d_x、d_y分别为子区域内各点X和Y方向上的Haar小波响应。则每一个子区域可用向量v＝(Σd_x,Σd_y,Σ|d_x|,Σ|d_y|)^T表示，这样特征点附近的局部区域可以用一个4×4×4＝64维的特征向量描述。

步骤2：判断当前图像与历史图像中的物体是否一致。

步骤2.1，求当前图像特征点与某时刻历史图像的匹配点。

求当前图像某一特征点对应的特征向量与历史图像所有特征点对应特征向量的余弦相似度，相似度最大的点即为所求的匹配点。匹配示意图如图4所示。

利用最近邻算法计算两特征向量v₁,v₂的余弦相似度S，公式如下：

$S=\cos <v_1,v_2>=\frac{v_1\&CenterDot;v_2}{\left|v_1\right|\left|v_2\right|}=v_1\&CenterDot;v_2$

式中，v₁,v₂均为单位向量。

按照上述方法求出当前图像所有特征点的匹配点。

步骤2.2，判断两个时刻图像中物体的一致性。

（1）随机选取两个时刻图像的3对匹配点，并求出它们之间的变换矩阵H。

（2）由变换矩阵H计算其它匹配点的误差，当误差小于阈值时，认为该匹配点支持当前的变换矩阵H。阈值通常取0.002。

（3）如果有超过2/3匹配点支持当前的变换矩阵H，则认为图像匹配成功；否则，返回步骤（1），若循环N次仍未满足条件则认为图像匹配失败。N通常取1000。

（4）如果匹配失败，计算当前图像的视觉显著性，若当前图像的视觉显著性大于阈值则保存此图像。

图像匹配失败说明了移动机器人到了一个新的场景，如果这个新场景的特征足够丰富，可将此图像作为路标图像予以保存。新场景的特征是否足够丰富，根据图像的视觉显著性进行判断。图像的视觉显著性按下面方法求得：

首先利用余弦相似度对图像中的特征进行分类，将余弦相似度大于阈值的两个特征向量划为同一类。阈值通常取0.6。然后，构建图像的视觉BoW模型，此时图像的特征丰富程度可用其视觉BoW直方图（如图5所示）的熵来描述：

$H_i=-\underset{k=1}{\overset{W\left(t\right)}{\mathrm{\&Sigma;}}}p\left(w_k\right){\log }_{2}p\left(w_k\right)$

$p\left(w_k\right)=\frac{w_k}{\underset{k=1}{\overset{W\left(t\right)}{\mathrm{\&Sigma;}}}{w}_k}$

其中，p(w_k)为图像视觉BoW特征的经验分布，w_k为第k个直方图的长度，W(t)为此视觉BoW空间的大小。对上式进行归一化得到视觉显著度为：

${S_L}_i=\frac{H_i}{{\log }_{2}W\left(t\right)}$

按照上式计算图像的视觉显著度，如果大于设定的阈值则认为该图像的具有足够丰富的视觉特征予以保存。阈值通常取0.4。

步骤3：利用视差法获取图像中物体的深度信息，并计算该物体两个时刻的坐标和相对位置差。

步骤3.1：获取图像中物体的深度信息。

如图2所示，A，B为安装在机器人身上的两个摄像头，方向与机器人朝向方向一致。O为A、B的连线中点。由几何关系求得物体的深度信息y为：

$y=\frac{L\&times;f}{d_1-d_2}$

其中，D₁、D₂分别为物体与两个摄像头光学中心线的距离，L＝D₁-D₂为基线宽度，d₁、d₂分别为物体在图像平面上的投影与光学中心线的距离。

步骤3.2：计算物体两个时刻的坐标和相对位置差。

如图3所示，以机器人初始时刻两摄像头A、B连线中点O为坐标原点，以机器人朝向为X轴方向，以过O点平行于地面且垂直于X轴的方向为Y轴方向建立全局直角坐标系XOY；以机器人当前时刻两摄像头连线中点p＝(p_x,p_y)为坐标原点p，以机器人朝向方向为X’轴方向，以过p点平行于地面且垂直于X’轴的方向为Y’轴方向建立局部直角坐标系X’pY’。

根据步骤3.1中获得的物体深度信息y可得物体与机器人的相对位置差，即物体的局部坐标m'为：

$m^{\&prime;}=(y,-\frac{y}{f}\left(\frac{d_1+d_2}{2}\right))$

当前时刻物体的全局坐标m为：

m＝p+Δm

Δm＝(m'_ysinθ+m'_xcosθ,m'_ycosθ-m'_xsinθ)

其中，Δm为物体与机器人的位置差在全局坐标系中的坐标，p＝(p_x,p_y)为机器人在全局坐标系中的坐标，θ为机器人的方向角。将物体两个时刻的位置做差可得此物体在i、j两个时刻的相对位置差Δm_ij＝m_i-m_j。i为当前时刻，j为匹配成功的历史时刻。

步骤4：判断物体的状态。

步骤4.1：假设物体为静止状态，计算在此假设下Δm_ij的期望μ_m与方差M_m。

假设 $p_x~N({\stackrel{\&OverBar;}{p}}_x,{\mathrm{\&sigma;}}_x^2),p_y~N({\stackrel{\&OverBar;}{p}}_y,{\mathrm{\&sigma;}}_y^2),{\mathrm{\&Delta;m}}_x~N(\mathrm{\&Delta;}{\stackrel{\&OverBar;}{m}}_x,{\mathrm{\&delta;}}_x^2),\mathrm{\&Delta;}{m}_y~N(\mathrm{\&Delta;}{\stackrel{\&OverBar;}{m}}_y,{\mathrm{\&delta;}}_y^2),$ “-”表示求期望。由于p_x和Δm_x相互独立，p_y和Δm_y相互独立，且

则：

$m_x~N({\stackrel{\&OverBar;}{m}}_x,{\mathrm{\&sigma;}}_x^2+{\mathrm{\&delta;}}_x^2)$

$m_y~N({\stackrel{\&OverBar;}{m}}_y,{\mathrm{\&sigma;}}_y^2+{\mathrm{\&delta;}}_y^2)$

假设i时刻和j时刻物体的全局坐标分别为m_i和m_j，机器人的全局坐标分别为(p_ix,p_iy)和(p_jx,p_jy)。如果物体是静态的，即

设X＝m_ix-m_jx，Y＝m_iy-m_jy，则：

$X~N(0,{\mathrm{\&sigma;}}_{\mathrm{ix}}^2+{\mathrm{\&sigma;}}_{\mathrm{jx}}^2+{\mathrm{\&delta;}}_{\mathrm{ix}}^2+{\mathrm{\&delta;}}_{\mathrm{jx}}^2-2\mathrm{cov}(p_{\mathrm{ix}},p_{\mathrm{jx}}))$

$Y~N(0,{\mathrm{\&sigma;}}_{\mathrm{iy}}^2+{\mathrm{\&sigma;}}_{\mathrm{jy}}^2+{\mathrm{\&delta;}}_{\mathrm{iy}}^2+{\mathrm{\&delta;}}_{\mathrm{jy}}^2-2\mathrm{cov}(p_{\mathrm{iy}},p_{\mathrm{jy}}))$

因此，写成矩阵形式有：

$\left[\begin{array}{c}X\\ Y\end{array}\right]~N\left(\begin{array}{c}\left[\begin{array}{c}0\\ 0\end{array}\right],{\mathrm{\&Sigma;}}_{\mathrm{ii}}+{\mathrm{\&Sigma;}}_{\mathrm{jj}}-{\mathrm{\&Sigma;}}_{\mathrm{ij}}-{\mathrm{\&Sigma;}}_{\mathrm{ji}}+2R\end{array}\right)$

其中，Σ_m,_n为机器人m时刻与n时刻位置的协方差矩阵，Σ_m,_n为2×2方阵，m,n＝i或j，R是测量白噪声的协方差矩阵。

物体为静态时Δm_ij的期望μ_m与方差M_m分别为：

${\mathrm{\&mu;}}_m=\left[\begin{array}{c}0\\ 0\end{array}\right],$ M_m＝Σ_ii+Σ_jj-Σ_ij-Σ_ji+2R

步骤4.2：构建接受区域。

步骤4.1中的分布可以写成二维联合正态分布的形式：

$(X,Y)~N(0,{\mathrm{\&zeta;}}_X^2;0,{\mathrm{\&zeta;}}_Y^2;\mathrm{\&rho;})$

其中， ${\mathrm{\&zeta;}}_X^2=M_m\left(\mathrm{1,1}\right),{\mathrm{\&zeta;}}_Y^2=M_m\left(\mathrm{2,2}\right),$ $\mathrm{\&rho;}=\frac{M_m\left(\mathrm{1,2}\right)}{\sqrt{M_m\left(\mathrm{1,1}\right)}\sqrt{M_m\left(\mathrm{2,2}\right)}}.$

对于正态分布，期望值所在的概率密度最大，因此可以用某点的概率密度与期望值所在的概率密度的比值与阈值β(β＜1)比较，所述阈值β通常取0.1。如果所述比值大于β，则认为事件在此点邻域发生的概率比较大，认为这个物体是静态的，接受原来的假设。物体是静态的条件表示如下：

$e^{\frac{-1}{2(1-{\mathrm{\&rho;}}^2)}[x^2-2\mathrm{\&rho;xy}+y^2]}>\mathrm{\&beta;}\&DoubleLeftRightArrow;\frac{1}{2(1-{\mathrm{\&rho;}}^2)}[x^2-2\mathrm{\&rho;xy}+y^2]<-\ln \mathrm{\&beta;}$

其中，

令α＝-lnβ，上式变为：

$\frac{1}{2(1-{\mathrm{\&rho;}}^2)}[x^2-2\mathrm{\&rho;xy}+y^2]<\mathrm{\&alpha;}$

令上式表示的区域为A。由于区域A的大小随方差改变，当方差较小时区域A也较小，这时只要物体的位置读数发生微小的变化就会被判断为动态，稳定性比较差，因此需构建一个以历史路标为圆心、r为半径的固定区域B：X²+Y²＜r²，本发明r取0.2。并取A、B的并集C＝A∪B作为最终的接受区域以弥补上述的缺陷。

步骤4.3：判断物体的状态。

通过步骤3获得的物体位置差Δm_ij与区域C的关系判断物体的状态：如果Δm_ij∈C则认为物体为静态，反之则认为物体为动态。

为了证明本发明的有效性，进行了仿真实验，仿真结果如图6所示。地图上存在着动态物体和静态物体，机器人在地图上移动，在移动的过程中判断物体的状态，并利用静态物体当做路标，利用pose-SLAM的方法对自身的位置进行估计，估计轨迹与实际轨迹基本重合，最大误差不超过4m。

Claims (3)

1.动态稀疏环境下机器人SLAM物体状态检测方法，其特征在于，在检测物体状态时考虑移动机器人的定位和测量误差对物体位置观测值的影响；所述方法包括以下步骤：

步骤1：采集环境图像，获取图像的特征向量集合；

步骤1.1，移动机器人通过双目视觉传感器采集环境图像；

步骤1.2，利用多尺度多方向Gabor函数构建能量图像空间，并采用非极大抑制方法对8点邻域范围内检测的极值点进行筛选；

步骤1.3，通过SURF描述子构建向量以表示特征点附近的局部区域，获得当前图像的特征向量集合；

（1）对以特征点为圆心、以6σ为半径的邻域中的点计算X和Y方向上的Haar小波响应；

（2）以60°的扇形滑动窗口绕圆心遍历整个圆，计算窗口内的响应总和，并取最长向量作为特征点主方向；

（3）以特征点为中心，主方向为X方向构造一个大小为20σ的方框，将其分成4×4个子区域；对每一个子区域分别计算Σd_x，Σd_y，Σ|d_x|和Σ|d_y|，其中，d_x、d_y分别为子区域内各点X和Y方向上的Haar小波响应；则每一个子区域可用向量v＝(Σd_x,Σd_y,Σ|d_x|,Σ|d_y|)^T表示，这样特征点附近的局部区域可以用一个4×4×4＝64维的特征向量描述；

步骤2：判断当前图像与历史图像中的物体是否一致；

步骤2.1，求当前图像特征点与某时刻历史图像的匹配点；

求当前图像某一特征点对应的特征向量与历史图像所有特征点对应特征向量的余弦相似度，相似度最大的点即为所求的匹配点；

利用最近邻算法计算两特征向量v₁,v₂的余弦相似度S，公式如下：

$S=\cos <v_1,v_2>=\frac{v_1\&CenterDot;v_2}{\left|v_1\right|\left|v_2\right|}=v_1\&CenterDot;v_2$

式中，v₁,v₂均为单位向量；

按照上述方法求出当前图像所有特征点的匹配点；

步骤2.2，判断两个时刻图像中物体的一致性；

（1）随机选取两个时刻图像的3对匹配点，并求出它们之间的变换矩阵H；

（2）由变换矩阵H计算其它匹配点的误差，当误差小于阈值时，认为该匹配点支持当前的变换矩阵H；所述阈值通常取0.002；

（3）如果有超过2/3匹配点支持当前的变换矩阵H，则认为图像匹配成功；否则，返回步骤（1），若循环N次仍未满足条件则认为图像匹配失败；N通常取1000；

（4）如果匹配失败，计算当前图像的视觉显著性，若当前图像的视觉显著性大于阈值则保存此图像；

图像匹配失败说明了移动机器人到了一个新的场景，如果这个新场景的特征足够丰富，可将此图像作为路标图像予以保存；新场景的特征是否足够丰富，根据图像的视觉显著性进行判断；图像的视觉显著性按下面方法求得：

首先利用余弦相似度对图像中的特征进行分类，将余弦相似度大于阈值的两个特征向量划为同一类；阈值通常取0.6；然后，构建图像的视觉BoW模型，此时图像的特征丰富程度可用其视觉BoW直方图的熵来描述：

$H_i=-\underset{k=1}{\overset{W\left(t\right)}{\mathrm{\&Sigma;}}}p\left(w_k\right){\log }_{2}p\left(w_k\right)$

$p\left(w_k\right)=\frac{w_k}{\underset{k=1}{\overset{W\left(t\right)}{\mathrm{\&Sigma;}}}{w}_k}$

其中，p(w_k)为图像视觉BoW特征的经验分布，w_k为第k个直方图的长度，W(t)为此视觉BoW空间的大小；对上式进行归一化得到视觉显著度为：

${S_L}_i=\frac{H_i}{{\log }_{2}W\left(t\right)}$

按照上式计算图像的视觉显著度，如果大于设定的阈值则认为该图像的具有足够丰富的视觉特征予以保存；阈值通常取0.4；

步骤3：利用视差法获取图像中物体的深度信息，并计算该物体两个时刻的坐标和相对位置差；

步骤3.1：获取图像中物体的深度信息；

A，B为安装在机器人身上的两个摄像头，方向与机器人朝向方向一致；O为A、B的连线中点；由几何关系求得物体的深度信息y为：

$y=\frac{L\&times;f}{d_1-d_2}$

其中，D₁、D₂分别为物体与两个摄像头光学中心线的距离，L＝D₁-D₂为基线宽度，d₁、d₂分别为物体在图像平面上的投影与光学中心线的距离；

步骤3.2：计算物体两个时刻的坐标和相对位置差；

以机器人初始时刻两摄像头A、B连线中点O为坐标原点，以机器人朝向为X轴方向，以过O点平行于地面且垂直于X轴的方向为Y轴方向建立全局直角坐标系XOY；以机器人当前时刻两摄像头连线中点p＝(p_x,p_y)为坐标原点p，以机器人朝向方向为X’轴方向，以过p点平行于地面且垂直于X’轴的方向为Y’轴方向建立局部直角坐标系X’pY’；

根据步骤3.1中获得的物体深度信息y可得物体与机器人的相对位置差，即物体的局部坐标m'为：

$m^{\&prime;}=(y,-\frac{y}{f}\left(\frac{d_1+d_2}{2}\right))$

当前时刻物体的全局坐标m为：

m＝p+Δm

Δm＝(m'_ysinθ+m'_xcosθ,m'_ycosθ-m'_xsinθ)

其中，Δm为物体与机器人的位置差在全局坐标系中的坐标，p＝(p_x,p_y)为机器人在全局坐标系中的坐标，θ为机器人的方向角；将物体两个时刻的位置做差可得此物体在i、j两个时刻的相对位置差Δm_ij＝m_i-m_j；i为当前时刻，j为匹配成功的历史时刻；

步骤4：判断物体的状态；

步骤4.1：假设物体为静止状态，计算在此假设下Δm_ij的期望μ_m与方差M_m；

假设 $p_x~N({\stackrel{\&OverBar;}{p}}_x,{\mathrm{\&sigma;}}_x^2),p_y~N({\stackrel{\&OverBar;}{p}}_y,{\mathrm{\&sigma;}}_y^2),{\mathrm{\&Delta;m}}_x~N(\mathrm{\&Delta;}{\stackrel{\&OverBar;}{m}}_x,{\mathrm{\&delta;}}_x^2),\mathrm{\&Delta;}{m}_y~N(\mathrm{\&Delta;}{\stackrel{\&OverBar;}{m}}_y,{\mathrm{\&delta;}}_y^2),$ “-”表示求期望；由于p_x和Δm_x相互独立，p_y和Δm_y相互独立，且

则：

$m_x~N({\stackrel{\&OverBar;}{m}}_x,{\mathrm{\&sigma;}}_x^2+{\mathrm{\&delta;}}_x^2)$

$m_y~N({\stackrel{\&OverBar;}{m}}_y,{\mathrm{\&sigma;}}_y^2+{\mathrm{\&delta;}}_y^2)$

假设i时刻和j时刻物体的全局坐标分别为m_i和m_j，机器人的全局坐标分别为(p_ix,p_iy)和(p_jx,p_jy)；如果物体是静态的，即

设X＝m_ix-m_jx，Y＝m_iy-m_jy，则：

$X~N(0,{\mathrm{\&sigma;}}_{\mathrm{ix}}^2+{\mathrm{\&sigma;}}_{\mathrm{jx}}^2+{\mathrm{\&delta;}}_{\mathrm{ix}}^2+{\mathrm{\&delta;}}_{\mathrm{jx}}^2-2\mathrm{cov}(p_{\mathrm{ix}},p_{\mathrm{jx}}))$

$Y~N(0,{\mathrm{\&sigma;}}_{\mathrm{iy}}^2+{\mathrm{\&sigma;}}_{\mathrm{jy}}^2+{\mathrm{\&delta;}}_{\mathrm{iy}}^2+{\mathrm{\&delta;}}_{\mathrm{jy}}^2-2\mathrm{cov}(p_{\mathrm{iy}},p_{\mathrm{jy}}))$

因此，写成矩阵形式有：

$\left[\begin{array}{c}X\\ Y\end{array}\right]~N\left(\begin{array}{c}\left[\begin{array}{c}0\\ 0\end{array}\right],{\mathrm{\&Sigma;}}_{\mathrm{ii}}+{\mathrm{\&Sigma;}}_{\mathrm{jj}}-{\mathrm{\&Sigma;}}_{\mathrm{ij}}-{\mathrm{\&Sigma;}}_{\mathrm{ji}}+2R\end{array}\right)$

其中，Σ_m,n为机器人m时刻与n时刻位置的协方差矩阵，Σ_m,n为2×2方阵，m,n＝i或j，R是测量白噪声的协方差矩阵；

物体为静态时Δm_ij的期望μ_m与方差M_m分别为：

${\mathrm{\&mu;}}_m=\left[\begin{array}{c}0\\ 0\end{array}\right],$ M_m＝Σ_ii+Σ_jj-Σ_ij-Σ_ji+2R

步骤4.2：构建接受区域；

步骤4.3：判断物体的状态；

通过步骤3获得的物体位置差Δm_ij与区域C的关系判断物体的状态：如果Δm_ij∈C则认为物体为静态，反之则认为物体为动态。

2.根据权利要求1所述的动态稀疏环境下机器人SLAM物体状态检测方法，其特征在于，所述步骤1.2构建能量图像空间并采用非极大抑制方法对8点邻域范围内检测的极值点进行筛选的方法包括以下步骤：

（1）基于Gabor函数设计一组多方向的奇对称与偶对称滤波器：

$g_{\sin }=\frac{1}{2{\mathrm{\&pi;\&sigma;}}^2}\exp (-\frac{x^2+y^2}{2{\mathrm{\&sigma;}}^2})\sin \left(\frac{\mathrm{\&sigma;}}{\mathrm{\&pi;}}(x\cos \mathrm{\&theta;}+y\sin \mathrm{\&theta;})\right)$

$g_{\cos }=\frac{1}{2{\mathrm{\&pi;\&sigma;}}^2}\exp (-\frac{x^2+y^2}{2{\mathrm{\&sigma;}}^2})\cos \left(\frac{\mathrm{\&sigma;}}{\mathrm{\&pi;}}(x\cos \mathrm{\&theta;}+y\sin \mathrm{\&theta;})\right)$

式中，σ为图像所在的尺度，x，y，θ分别为图像点所在坐标及其方向，g_sin和g_cos分别为奇、偶对称滤波器的传输函数；

（2）对图像进行多尺度多方向滤波：

$W_{\mathrm{even}}(x,y,\mathrm{\&sigma;},\mathrm{\&theta;})=I(x,y)\&CircleTimes;g_{\cos }(x,y,\mathrm{\&sigma;},\mathrm{\&theta;})$

$W_{\mathrm{odd}}(x,y,\mathrm{\&sigma;},\mathrm{\&theta;})=I(x,y)\&CircleTimes;g_{\sin }(x,y,\mathrm{\&sigma;},\mathrm{\&theta;})$

式中，I(x,y)为图像，W_even(x,y,σ,θ)、W_odd(x,y,σ,θ)分别为图像与偶对称及奇对称滤波器的滤波输出；

（3）对于各个方向的滤波器输出求二范数得到各个尺度下各方向的能量函数：

E(x,y,σ,θ)＝||W(x,y,σ,θ)||

式中，E(x,y,σ,θ)为能量函数；

（4）对于同一尺度下各方向能量函数采用统一权值求和，得到图像的多尺度能量图像：

$E(x,y,\mathrm{\&sigma;})=\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}E(x,y,\mathrm{\&sigma;},{\mathrm{\&theta;}}_k)$

式中，k＝1,2,......,N，N为方向总数，本发明中取N=8，8个方向分别为θ_k=0°，22.5°，45°，67.5°，90°，112.5°，135°，157.5°；

（5）在能量图像中的点邻域内寻找能量图像中的极大值点，并提取能量极大值大于阈值的能量点为特征点；所述点邻域通常取8点邻域，所述阈值通常取最大能量的一半。

3.根据权利要求1所述的动态稀疏环境下机器人SLAM物体状态检测方法，其特征在于，所述步骤4.2构建接受区域的方法包括以下内容：

所述步骤4.1中的分布可以写成二维联合正态分布的形式：

$(X,Y)~N(0,{\mathrm{\&zeta;}}_X^2;0,{\mathrm{\&zeta;}}_Y^2;\mathrm{\&rho;})$

其中， ${\mathrm{\&zeta;}}_X^2=M_m\left(\mathrm{1,1}\right),{\mathrm{\&zeta;}}_Y^2=M_m\left(\mathrm{2,2}\right),$ $\mathrm{\&rho;}=\frac{M_m\left(\mathrm{1,2}\right)}{\sqrt{M_m\left(\mathrm{1,1}\right)}\sqrt{M_m\left(\mathrm{2,2}\right)}};$

对于正态分布，期望值所在的概率密度最大，因此可以用某点的概率密度与期望值所在的概率密度的比值与阈值β比较，所述阈值β通常取0.1；如果所述比值大于β，则认为事件在此点邻域发生的概率比较大，认为这个物体是静态的，接受原来的假设；物体是静态的条件表示如下：

$e^{\frac{-1}{2(1-{\mathrm{\&rho;}}^2)}[x^2-2\mathrm{\&rho;xy}+y^2]}>\mathrm{\&beta;}\&DoubleLeftRightArrow;\frac{1}{2(1-{\mathrm{\&rho;}}^2)}[x^2-2\mathrm{\&rho;xy}+y^2]<-\ln \mathrm{\&beta;}$

其中，

令α＝-lnβ，上式变为：

$\frac{1}{2(1-{\mathrm{\&rho;}}^2)}[x^2-2\mathrm{\&rho;xy}+y^2]<\mathrm{\&alpha;}$

令上式表示的区域为A；由于区域A的大小随方差改变，当方差较小时区域A也较小，这时只要物体的位置读数发生微小的变化就会被判断为动态，稳定性比较差，因此需构建一个以历史路标为圆心、r为半径的固定区域B：X²+Y²＜r²，r通常取0.2；并取A、B的并集C＝A∪B作为最终的接受区域以弥补上述的缺陷。

Images