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

摘要

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

主权项

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·v_2}{\left|v_1\right|\left|v_2\right|}=v_1·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)}{\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)}{\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({\bar{p}}_x,{\sigma }_x^2),p_y~N({\bar{p}}_y,{\sigma }_y^2),{\mathrm{\Delta m}}_x~N(\Delta {\bar{m}}_x,{\delta }_x^2),\Delta m_y~N(\Delta {\bar{m}}_y,{\delta }_y^2),$“-”表示求期望；由于p_x和Δm_x相互独立，p_y和Δm_y相互独立，且则：$m_x~N({\bar{m}}_x,{\sigma }_x^2+{\delta }_x^2)$$m_y~N({\bar{m}}_y,{\sigma }_y^2+{\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,{\sigma }_{\mathrm{ix}}^2+{\sigma }_{\mathrm{jx}}^2+{\delta }_{\mathrm{ix}}^2+{\delta }_{\mathrm{jx}}^2-2\mathrm{cov}(p_{\mathrm{ix}},p_{\mathrm{jx}}))$$Y~N(0,{\sigma }_{\mathrm{iy}}^2+{\sigma }_{\mathrm{jy}}^2+{\delta }_{\mathrm{iy}}^2+{\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],{\Sigma }_{\mathrm{ii}}+{\Sigma }_{\mathrm{jj}}-{\Sigma }_{\mathrm{ij}}-{\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分别为：${\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则认为物体为静态，反之则认为物体为动态。