一种基于kinect的室内机器人楼梯攀爬位姿快速估计方法技术

本发明专利技术公开了一种基于kinect的室内机器人楼梯攀爬位姿快速估计方法，获取楼梯RGB图像和深度图像，检测图像边缘；从获取的边缘图像，提取出边缘直线；检测边沿直线交点，获得楼梯里角边沿点；由获得的边沿点，找到楼梯边沿线，提取其对应Hough线段的中点为代表点的像素坐标；根据相机像素坐标系和相机坐标系的关系解出内参矩阵,获得边沿点相机坐标系下的坐标；通过获得的边沿点的相机坐标建立楼梯坐标系；获得相机光心在楼梯坐标系下坐标；在楼梯坐标系下，估算相机光心到楼梯坐标系原点的距离，并与阈值比较，进行楼梯计数，获得姿态信息。本发明专利技术以可接受的运行时间和精度估算了机器人攀爬楼梯区域时的位姿，具有鲁棒性和有效性。

【技术实现步骤摘要】
一种基于kinect的室内机器人楼梯攀爬位姿快速估计方法
本专利技术属于机器人三维空间定位
，尤其涉及一种基于kinect的室内机器人楼梯攀爬位姿快速估计方法。
技术介绍
为了以可预测和安全的方式与一般室内环境进行交互，机器人在很大程度上依赖于识别环境和估计自身相对于环境的位姿。楼梯是室内移动接入遇到的典型障碍环境，该问题可分为三个步骤：(1)检测楼梯，(2)获得机器人自身位姿，(3)爬楼梯。论文《Efficientstairwaydetectionandmodelingforautonomousrobotclimbing》中提出一种将二维特征嵌入到三维数据利用八叉树向下采样获取楼梯点云数据的方法。该方法能够得到较为准确的楼梯模型，但点云采集和建模需要较大计算量，难以部署到小型低成本移动平台上。(ChanDS,SilvaRK,MonteiroJC,etal.Efficientstairwaydetectionandmodelingforautonomousrobotclimbing[J].2017)。大多数室内机器人位姿估计方法均是在二维平面空间的定位，论文《High-precisionindoorpositioningfusionalgorithmbasedonWiFifingerprint》中提出了一种利用梯度提升决策树和机器学习相结合的方法，解决了在室内用WiFi指纹定位方法的非线性问题，并利用粒子滤波技术，有效解决了动态目标跟踪中的误差累积问题，提高了位姿估计的精度，但指纹收集工作量很大，且WiFi设备移动后需要重新收集。(KaXiaofei,LiMengmeng,QiaoWei.High-precisionindoorpositioningfusionalgorithmbasedonWiFifingerprint[J].JournalofXi'anUniversityofScienceandTechnology,2020,40(3):470-476.)。机器人在室内跨层攀爬楼梯是在三维空间的运动，而以上的定位方法均为在二维平面空间的定位(机器人没有高度上的位移)并利用特征点匹配的方法进行位姿估计。但楼梯场景的结构特征重复且攀爬时有高度上的位移，用特征点匹配的方法会造成较大的误匹配率。
技术实现思路
针对
技术介绍
中现有楼梯攀爬位姿估计方法的不足，提出一种在三维空间的基于楼梯几何特征的快速估计位姿的方法，该方法以可接受的运行时间和精度估算了机器人攀爬楼梯区域时的位姿，具有鲁棒性和有效性。为解决上述技术问题，本专利技术采用如下技术方案：一种基于kinect的室内机器人楼梯攀爬位姿快速估计方法，包括以下步骤：S1.基于kinect传感器获取楼梯RGB图像和深度图像，使用gabor滤波器进行图像处理，获取ROI区域，然后应用canny算子检测图像边缘；S2.利用Hough线检测从S1中获取的边缘图像，提取出边缘直线；S3.首先滤除重复与偏离的直线，然后利用三线相交法检测边沿直线交点，获得楼梯里角边沿点，即楼梯台阶的水平边沿和台阶高度和宽度所在边沿的相交点；S4.由S3中获得的楼梯里角边沿点，找到它旁边的三条楼梯边沿线，提取其对应Hough线段的中点为代表点，提取出4个边沿点的像素坐标；S5.对相机进行标定，根据相机像素坐标系和相机坐标系的关系解出内参矩阵，通过彩色图与深度图对齐，由S4中提取的4个边沿点的像素坐标，获得其在相机坐标系下的坐标；S6.通过S5中获得的4个边沿点的相机坐标建立楼梯坐标系；并通过坐标变换，获得相机光心在楼梯坐标系下坐标；S7.在楼梯坐标系下，通过估算相机光心到楼梯坐标系原点的距离，并与阈值比较，进行楼梯计数，获得姿态信息。进一步，步骤S3中使用三线相交法优化检测到的楼梯边沿点的准确性。进一步，所述步骤S3中滤除重复与偏离的直线具体步骤为：比较检测直线两两的斜率差的绝对值，若满足0≤|ki+1-ki|≤1/5，则找到拟合这两条直线的Hough线段，比较它们的长度大小，如果length(i+1)≥length(i)，则去除length(i)所在的直线；若length(i+1)＜length(i)，则去除length(i+1)所在的直线；这样重复的和偏离边沿的直线将被滤除，边沿直线将被保留，其中ki+1和ki分别为第i+1条检测直线和第i条检测直线的斜率，length(i+1)和length(i)分别为第i+1条检测直线和第i条检测直线Hough线段的长度。进一步，所述三线相交法的具体步骤为：先找边沿直线两两之间在图像内的交点，假设三个交点坐标为(xn+1，yn+1)，(xn，yn)，(xn-1，yn-1)，如果0≤|xn+1-xn|≤8且0≤|xn-xn-1|≤8且0≤|xn+1-xn-1|≤8且0≤|yn+1-yn|≤8且0≤|xn-yn-1|≤8且0≤|yn+1-yn-1|≤8，则认为这三个点为楼梯边沿的交点，则取三线连接点坐标，即楼梯里角边沿点坐标这三个点所在直线即为楼梯边沿线，提取出这三个点所在楼梯边沿线上的Hough线段的中点为边沿点，其中n-1、n、n+1分别表示边沿直线在图像内交点的序号。进一步，所述步骤S5中，相机坐标系下边沿点坐标由下式获得：Xc＝Zc(u-u0)/fxYc＝Zc(v-v0)/fy式中，(u，v)为图像中的像素坐标，(Xc，Yc，Zc)为相机坐标系下的坐标，fx、fy、u0、v0均为相机内参，fx、fy为相机焦距，u0、v0为成像面中心坐标。进一步，所述步骤S6中，从相机坐标系到楼梯坐标系的转换为刚体变换，可以用一个旋转矩阵R和一个平移矩阵T来表示，其中平移矩阵T可用楼梯坐标原点在相机坐标系中的坐标表示，利用S5中获得的边沿点在相机坐标下的坐标构造向量单位化后，以0为原点，这三个单位向量的方向为Z，X，Y轴的方向建立楼梯坐标系，(v1，v2，v3)即为楼梯坐标系到相机坐标系的旋转矩阵，求逆即可得到相机坐标系到楼梯坐标系的旋转矩R；其中O(x0，y0，z0)，P1(x1，y1，z1)，，P2(x2，y2，z2)，P3(x3，y3，z3)为S5中得到的4个楼梯边沿点在相机坐标系下的坐标。进一步，所述步骤S7中，一直到接近楼梯时都以0阶楼梯边沿即Ow0为参考系，直到靠近第一阶楼梯时，此时当相机相对于0阶楼梯的位姿处于一定阈值内时，此处将位姿量化为Oc到Ow0的距离来衡量，选取第一阶楼梯边沿Ow1为参考系并估算相机到Ow1的距离；若该距离也处于一定阈值范围内，此时机器人开始爬第一阶楼梯并计数；开始爬第一阶楼梯时便以第二阶楼梯边沿Ow2为参考系估算相机位姿，直至爬上第一阶楼梯，若此刻距离Ow2也在一定阈值范围内，此时开始爬第二阶楼梯并计数；依次类推，爬第N阶楼梯便以第N+1阶楼梯为参考系，直到视野中没有楼梯出现，此时停止攀爬，由此，即可得到机器人楼梯攀爬的位姿信息。本文档来自技高网...

【技术保护点】
1.一种基于kinect的室内机器人楼梯攀爬位姿快速估计方法，其特征在于，包括以下步骤：/nS1.基于kinect传感器获取楼梯RGB图像和深度图像，使用gabor滤波器进行图像处理，获取ROI区域，然后应用canny算子检测图像边缘；/nS2.利用Hough线检测从S1中获取的边缘图像，提取出边缘直线；/nS3.首先滤除重复与偏离的直线，然后利用三线相交法检测边沿直线交点，获得楼梯里角边沿点，即楼梯台阶的水平边沿和台阶高度和宽度所在边沿的相交点；/nS4.由S3中获得的楼梯里角边沿点，找到它旁边的三条楼梯边沿线，提取其对应Hough线段的中点为代表点，提取出4个边沿点的像素坐标；/nS5.对相机进行标定，根据相机像素坐标系和相机坐标系的关系解出内参矩阵,通过彩色图与深度图对齐，由S4中提取的4个边沿点的像素坐标，获得其在相机坐标系下的坐标；/nS6.通过S5中获得的4个边沿点的相机坐标建立楼梯坐标系；并通过坐标变换，获得相机光心在楼梯坐标系下坐标；/nS7.在楼梯坐标系下，通过估算相机光心到楼梯坐标系原点的距离，并与阈值比较，进行楼梯计数，获得姿态信息。/n

【技术特征摘要】
1.一种基于kinect的室内机器人楼梯攀爬位姿快速估计方法，其特征在于，包括以下步骤：
S1.基于kinect传感器获取楼梯RGB图像和深度图像，使用gabor滤波器进行图像处理，获取ROI区域，然后应用canny算子检测图像边缘；
S2.利用Hough线检测从S1中获取的边缘图像，提取出边缘直线；
S3.首先滤除重复与偏离的直线，然后利用三线相交法检测边沿直线交点，获得楼梯里角边沿点，即楼梯台阶的水平边沿和台阶高度和宽度所在边沿的相交点；
S4.由S3中获得的楼梯里角边沿点，找到它旁边的三条楼梯边沿线，提取其对应Hough线段的中点为代表点，提取出4个边沿点的像素坐标；
S5.对相机进行标定，根据相机像素坐标系和相机坐标系的关系解出内参矩阵,通过彩色图与深度图对齐，由S4中提取的4个边沿点的像素坐标，获得其在相机坐标系下的坐标；
S6.通过S5中获得的4个边沿点的相机坐标建立楼梯坐标系；并通过坐标变换，获得相机光心在楼梯坐标系下坐标；
S7.在楼梯坐标系下，通过估算相机光心到楼梯坐标系原点的距离，并与阈值比较，进行楼梯计数，获得姿态信息。


2.根据权利要求1所述的一种基于kinect的室内机器人楼梯攀爬位姿快速估计方法，其特征在于，步骤S3中使用三线相交法优化检测到的楼梯边沿点的准确性。


3.根据权利要求2所述的一种基于kinect的室内机器人楼梯攀爬位姿快速估计方法，其特征在于，所述步骤S3中滤除重复与偏离的直线具体步骤为：比较检测直线两两的斜率差的绝对值，若满足0≤|ki+1-ki|≤1/5,则找到拟合这两条直线的Hough线段,比较它们的长度大小，如果length(i+1)≥length(i)，则去除length(i)所在的直线；若length(i+1)<length(i),则去除length(i+1)所在的直线；这样重复的和偏离边沿的直线将被滤除，边沿直线将被保留，其中ki+1和ki分别为第i+1条检测直线和第i条检测直线的斜率，length(i+1)和length(i)分别为第i+1条检测直线和第i条检测直线Hough线段的长度。


4.根据权利要求2所述的一种基于kinect的室内机器人楼梯攀爬位姿快速估计方法，其特征在于，所述三线相交法的具体步骤为：先找边沿直线两两之间在图像内的交点，假设三个交点坐标为(xn+1,yn+1)，(xn,yn)，(xn-1,yn-1),如果0≤|xn+1-xn|≤8且0≤|xn-xn-1|≤8且0≤|xn+1-xn-1|≤...

【专利技术属性】
技术研发人员：刘清宇，阮禹，白永康，周诗洋，
申请(专利权)人：武汉科技大学，
类型：发明
国别省市：湖北;42