CN111198563A - 一种用于足式机器人动态运动的地形识别方法及系统 - Google Patents
一种用于足式机器人动态运动的地形识别方法及系统 Download PDFInfo
- Publication number
- CN111198563A CN111198563A CN201911402644.2A CN201911402644A CN111198563A CN 111198563 A CN111198563 A CN 111198563A CN 201911402644 A CN201911402644 A CN 201911402644A CN 111198563 A CN111198563 A CN 111198563A
- Authority
- CN
- China
- Prior art keywords
- contour line
- plane
- point
- obtaining
- point cloud
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 46
- 230000000007 visual effect Effects 0.000 claims abstract description 16
- 238000004364 calculation method Methods 0.000 claims abstract description 14
- 238000011156 evaluation Methods 0.000 claims description 12
- 238000006243 chemical reaction Methods 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 7
- 239000011159 matrix material Substances 0.000 claims description 6
- 238000012163 sequencing technique Methods 0.000 claims description 6
- 230000008030 elimination Effects 0.000 claims description 3
- 238000003379 elimination reaction Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 11
- 230000005021 gait Effects 0.000 description 6
- 238000003384 imaging method Methods 0.000 description 5
- 230000003287 optical effect Effects 0.000 description 4
- OYPRJOBELJOOCE-UHFFFAOYSA-N Calcium Chemical compound [Ca] OYPRJOBELJOOCE-UHFFFAOYSA-N 0.000 description 3
- 230000006978 adaptation Effects 0.000 description 3
- 229910052791 calcium Inorganic materials 0.000 description 3
- 239000011575 calcium Substances 0.000 description 3
- 238000013135 deep learning Methods 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 3
- 230000007246 mechanism Effects 0.000 description 3
- 241001031135 Aristea ecklonii Species 0.000 description 1
- 241000238631 Hexapoda Species 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
- G05D1/0251—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Aviation & Aerospace Engineering (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Electromagnetism (AREA)
- Image Processing (AREA)
- Image Analysis (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种用于足式机器人动态运动的地形识别方法及系统，其中，所述方法包括：获取RGB双目左右图像；获得点云图和轮廓线；将所述轮廓线投影到所述点云图上，截取所述轮廓线内的点云；获得共面点云团；采用MSAC的算法进行平面估计，得到平面估计的参数化模型，并剔除不符合所述参数化模型的匹配对；获得三维地形图；根据参数平面的高度和深度进行排序；通过计算得到平面交线，进而获得平面横截图；将所述平面横截图与所述得到的平面估计的参数化模型进行对比，评估识别的准确性；将所述三维地形图的点云数据视觉坐标系转化到机器人坐标系。在本发明实施中，提高机器人运动规划的可靠性，提高轮廓识别的准确性，提高地形平面参数的准确性。
Description
技术领域
本发明涉及机器人地形识别的技术领域，尤其涉及一种用于足式机器人动态运动的地形识别方法及系统。
背景技术
腿足式机器人在运动中只需离散的落脚点而不需要连续的路径，因此较轮式和履带式机器人，其具有更为优秀的灵活性和环境适应性.在该类机器人的研究中，野外复杂地形环境下的自主运动一直是研究的重点，而地形识别及路径规划技术又是解决这一问题的关键.最近几年，国外许多研究机构已对四足机器人在复杂环境下的自主运动进行了研究，而国内由于对四足机器人的研究较晚，对地形识别及路径规划部分的研究还处于起步阶段。
而一种面向六足机器人的地形分类方法提出一种用于面向足式机器人的地形分类方法，通过离线训练已知地形类别图片，从而对所处的环境进行识别判断，自动选择步态，该方法需要对地形有先验信息进行判断，而实际中地形往往不可预测，且地形的高度差差异对步态规划产生的影响无法自适应调整，环境适应性差。
足式机器人自身的稳定步态控制通常在平坦地形条件下达到稳定状态，然而实际场景往往是非结构化、非平坦、非平滑的地面，特别是有巨大、尖锐的地形凸起，比如盒子或者楼梯。如果地形凸起高度低于足式机器人抬腿高度或者出现坡度不大的缓坡，足式机器人将在自身稳定步态控制器的调整下也将最终稳定，而实际场景中障碍物常高于足式机器人抬腿高度，机器人遇到障碍物，无法跨越，同时撞到障碍物也无法达到平稳状态。
发明内容
本发明的目的在于克服现有技术的不足，本发明提供了一种用于足式机器人动态运动的地形识别方法及系统，基于双目视觉的楼梯地形识别和三维地图重构，通过楼梯台阶的识别，机器人将能够修改其摆动脚轨迹，向稳定步态控制器提供地形信息，已达到跨越障碍物目的，提高足式机器人的环境适应性。
为了解决上述技术问题，本发明实施例提供了一种用于足式机器人动态运动的地形识别方法，所述方法包括：
获取RGB双目左右图像；
基于所述RGB双目左右图像，分别获得点云图和轮廓线；
将所述轮廓线投影到所述点云图上，截取所述轮廓线内的点云；
通过所述截取所述轮廓线内的点云，获得共面点云团；
基于所述共面点云团，采用MSAC的算法进行平面估计，得到平面估计的参数化模型，并剔除不符合所述参数化模型的匹配对；
基于所述平面估计的参数化模型，获得三维地形图；
基于所述三维地形图，根据参数平面的高度和深度进行排序；
在所述排序之后，通过计算得到平面交线，进而获得平面横截图；
将所述平面横截图与所述得到的平面估计的参数化模型进行对比，评估识别的准确性；
在所述评估之后，将所述三维地形图的点云数据视觉坐标系转化到机器人坐标系。
可选的，所述基于所述RGB双目左右图像，获得点云图包括：
基于所述RGB双目左右图像，采用三角测量的算法求得视差图；
通过投影模型从所述视差图中获得深度图；
结合所述深度图和像素坐标，转化成点云图。
可选的，所述基于所述RGB双目左右图像，获得轮廓线包括：
基于RGB双目左右图像，采用轮廓线扫描的算法检测轮廓线，并提取所述检测到的轮廓线；
对所述检测到的轮廓线进行面积计算并排序；
判断所述检测到的轮廓线包围面积是否小于设定阈值；
若是，则进行交叉重叠计算，用大轮廓线代替后得到所需要的轮廓线；
若否，则直接得到所需要的轮廓线。
可选的，所述基于RGB双目左右图像，采用轮廓线扫描的算法检测轮廓线，并提取所述检测到的轮廓线包括：
基于RGB双目左右图像，找到一个初始起点，并设置所述初始起点的方向为上；
若像素点P1是前景，则将所述初始起点移动到像素点P2，再移动到像素点P1；
若像素点P1不是前景，则判断像素点P2是否是前景；
若像素点P2是前景，则将所述初始起点移动到像素点P2；
若像素点P1和像素点P2都不是前景，则判断像素点P3是否为前景；
若像素点P3是前景，则将所述初始起点先右转，再移动到像素点P3；
返回至若像素点P1是前景，则将所述初始起点移动到像素点P2，再移动到像素点P1，直到在同一个位置右转三次以上或者返回到初始起点三次以上结束。
可选的，所述基于所述共面点云团，采用MSAC的算法进行平面估计，得到平面估计的参数化模型，并剔除不符合所述参数化模型的匹配对包括：
基于所述共面点云团，随机选取一部分粗糙结果中的匹配对；
基于三点算法，通过所述匹配对来计算得出平面；
基于所述平面，反推模型并通过计算所述模型对于所述匹配对的代价；
通过反复迭代所述模型，最终得到平面估计的参数化模型，并剔除不符合所述参数化模型的匹配对。
可选的，所述通过计算所述模型对于所述匹配对的代价包括：
计数所述模型中粗糙匹配对的组数；
在所述计数之后，对代价值进行初始化；
基于所述代价值初始化，对所述粗糙匹配对中的每一匹配对进行统计并分析；
若所述匹配对误差小于等于误差容许值，则代价为所计算的误差；
若所述匹配对误差大于误差容许值，则代价为误差容值。
可选的，所述在所述评估之后，将所述三维地形图的点云数据视觉坐标系转化到机器人坐标系包括：
在所述评估之后，从相机坐标系上的点经过转换矩阵，将所述三维地形图的点云数据视觉坐标系转化到机器人坐标系；
基于所述机器人坐标系，机器人获得障碍物距离本体的距离。
可选的，所述从相机坐标系上的点经过转换矩阵，将所述三维地形图的点云数据视觉坐标系转化到机器人坐标系的具体公式如下：
P_r＝H·P_v；
其中，H为相机坐标系与机器人本体坐标系的位姿转换关系，P_v为相机坐标系的点，P_r为机器人本体坐标系的点。
另外，本发明实施例还提供了一种用于足式机器人动态运动的地形识别系统，所述系统包括：
图像获取模块：用于获取RGB双目左右图像；
点云图和轮廓线获得模块：用于基于所述RGB双目左右图像，分别获得点云图和轮廓线；
截取模块：用于将所述轮廓线投影到所述点云图上，截取所述轮廓线内的点云；
共面点云团获得模块：用于通过所述截取所述轮廓线内的点云，获得共面点云团；
估计与剔除模块：用于基于所述共面点云团，采用MSAC的算法进行平面估计，得到平面估计的参数化模型，并剔除不符合所述参数化模型的匹配对；
三维地形图获得模块：用于基于所述平面估计的参数化模型，获得三维地形图；
排序模块：用于基于所述三维地形图，根据参数平面的高度和深度进行排序；
平面横截图获得模块：用于在所述排序之后，通过计算得到平面交线，进而获得平面横截图；
对比与评估模块：用于将所述平面横截图与所述得到的平面估计的参数化模型进行对比，评估识别的准确性；
坐标系转化模块：用于在所述评估之后，将所述三维地形图的点云数据视觉坐标系转化到机器人坐标系。
可选的，所述点云图和轮廓线获得模块还包括：用于基于所述RGB双目左右图像，采用三角测量的算法求得视差图；通过投影模型从所述视差图中获得深度图；结合所述深度图和像素坐标，转化成点云图；以及用于基于RGB双目左右图像，采用轮廓线扫描的算法检测轮廓线，并提取所述检测到的轮廓线；对所述检测到的轮廓线进行面积计算并排序；判断所述检测到的轮廓线包围面积是否小于设定阈值；若是，则进行交叉重叠计算，用大轮廓线代替后得到所需要的轮廓线；若否，则直接得到所需要的轮廓线。
在本发明实施中，针对传统足式机器人采用模板匹配识别地形的环境适应差问题，采用双目视觉实时获取地形信息，实现楼梯地形的实时识别与三维地形图重构，提高环境适应性的同时实时为步态控制器提供障碍物准确位姿信息，提高机器人运动规划的可靠性；针对轮廓线扫描法提取楼梯轮廓易受机器人本体抖动重新重影现象，提出交叉重叠替换机制对易产生错误轮廓线区域和现象进行修正，提高轮廓识别的准确性；针对地形信息的获取复杂，通过深度学习进行场景识别效率低，实时性差的问题，通过平面轮廓投影截取点云方法获得目标地形信息，采用剔除外点的平面估计法提高地形平面参数的准确性和精度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见的，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
图1是本发明实施中的用于足式机器人动态运动的地形识别方法的流程示意图；
图2是本发明实施中的用于足式机器人动态运动的地形识别系统的结构组成示意图；
图3是本发明实施中的相机成像的模型图；
图4是本发明实施中的P1，P2，P3像素的定义解释图；
图5是本发明实施中的轮廓线扫描的算法的步骤解释图a；
图6是本发明实施中的轮廓线扫描的算法的步骤解释图b；
图7是本发明实施中的轮廓线扫描的算法的步骤解释图c；
图8是本发明实施中的三维地形图的示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
实施例
请参阅图1，图1是图1是本发明实施中的用于足式机器人动态运动的地形识别方法的流程示意图。
如图1所示，一种用于足式机器人动态运动的地形识别方法，所述方法包括：
S11：获取RGB双目左右图像；
具体的，结合附图3所示，附图3示出本发明实施中的相机成像的模型，经过双目相机标定和校准后，双目相机的主光轴可到达平行，如图所示是双目相机模型，世界坐标系中的任意一点都满足，该点与它在左右相机的成像点在同一个极平面上。P为空间中的点，PL和PR是点P在左右像平面上的成像点，OR和OL是左右相机的光心，长为L的两条线段(端点为蓝色星星的线段)表示的是左右相机的像面，光心到像面的最短距离就是焦距长度f。若P是世界坐标系中的一点，它在左右像面上的成像点是PL和PR。PL和PR距各自像面的左边缘的距离是XL和XR。视差就是XR-XL或者是XL-XR。标定和匹配后f，b，XR，XL都能够得到。
其中，物体的景深Z与视差关系与求解：
在三角形OL-OR-P中。三角形PL-PR-P相似于三角形OL-OR-P，则有比例关系：
可求得：
可得只要求得XR-XL就可以求得Z。
S12：基于所述RGB双目左右图像，分别获得点云图和轮廓线；
在本发明实施过程中，所述基于所述RGB双目左右图像，获得点云图包括：基于所述RGB双目左右图像，采用三角测量的算法求得视差图；通过投影模型从所述视差图中获得深度图；结合所述深度图和像素坐标，转化成点云图。
在本发明实施过程中，所述基于所述RGB双目左右图像，获得轮廓线包括：基于RGB双目左右图像，采用轮廓线扫描的算法检测轮廓线，并提取所述检测到的轮廓线；对所述检测到的轮廓线进行面积计算并排序；判断所述检测到的轮廓线包围面积是否小于设定阈值；若是，则进行交叉重叠计算，用大轮廓线代替后得到所需要的轮廓线；若否，则直接得到所需要的轮廓线。
具体的，所述轮廓线提取的算法：当前像素位置，当前方向，P1，P2，P3三个像素点：当前像素位置，即任意一个左边像素为背景的前景边界像素都可以作为起始像素，由算法来决定移动的下个位置；当前方向，即当前像素有4个方向，左，上，右，下；每次移动后方向可能会改变，例如从(i，j)移动到(i，j+1)，那么方向就变成了下；P1，P2，P3像素，即有了当前像素和方向，P1，P2，P3像素的定义结合附图4所示，附图4示出本发明实施中的P1，P2，P3像素的定义解释；
其中，所述基于RGB双目左右图像，采用轮廓线扫描的算法检测轮廓线，并提取所述检测到的轮廓线包括：基于RGB双目左右图像，找到一个初始起点，并设置所述初始起点的方向为上；结合附图5所示，附图5示出本发明实施中的轮廓线扫描的算法的步骤解释图a，若像素点P1是前景，则将所述初始起点移动到像素点P2，再移动到像素点P1；若像素点P1不是前景，则判断像素点P2是否是前景；结合附图6所示，附图6是本发明实施中的轮廓线扫描的算法的步骤解释图b，若像素点P2是前景，则将所述初始起点移动到像素点P2；若像素点P1和像素点P2都不是前景，则判断像素点P3是否为前景；结合附图7所示，附图7是本发明实施中的轮廓线扫描的算法的步骤解释图c，若像素点P3是前景，则将所述初始起点先右转，再移动到像素点P3；返回至若像素点P1是前景，则将所述初始起点移动到像素点P2，再移动到像素点P1，直到在同一个位置右转三次以上或者返回到初始起点三次以上结束。
S13：将所述轮廓线投影到所述点云图上，截取所述轮廓线内的点云；
S14：通过所述截取所述轮廓线内的点云，获得共面点云团；
S15：基于所述共面点云团，采用MSAC的算法进行平面估计，得到平面估计的参数化模型，并剔除不符合所述参数化模型的匹配对；
在本发明具体实施过程中，所述基于所述共面点云团，采用MSAC的算法进行平面估计，得到平面估计的参数化模型，并剔除不符合所述参数化模型的匹配对包括：基于所述共面点云团，随机选取一部分粗糙结果中的匹配对；基于三点算法，通过所述匹配对来计算得出平面；基于所述平面，反推模型并通过计算所述模型对于所述匹配对的代价；通过反复迭代所述模型，最终得到平面估计的参数化模型，并剔除不符合所述参数化模型的匹配对。
具体的，所述通过计算所述模型对于所述匹配对的代价包括：计数所述模型中粗糙匹配对的组数；在所述计数之后，对代价值进行初始化；基于所述代价值初始化，对所述粗糙匹配对中的每一匹配对进行统计并分析；若所述匹配对误差小于等于误差容许值，则代价为所计算的误差；若所述匹配对误差大于误差容许值，则代价为误差容值。
其中，所述MSAC的算法伪代码如下：
ConsensusMinCost＝+inf//给定代价大值；
For(Trails＝0；Trails＜maxTrails&&ConsensusMinCost＞Threshold；++Trails){//迭代周期内，如最小代价值大于设定阈值，则继续迭代；
Select a Random Sample；//随机选择样本；
Calculate Model；//本实例采用三点法计算平面模型；
Calculate Error；//计算误差；
Calculate ConsensusCost；//计算代价，具体代价算法将在下面给出；
If(ConsensusCost＜consensusMinCost){//如代价小于上一循环的最小代价；
consensusMinCost＝consensusCost；//则更新最小代价；
consensusMinCostModel＝Model；//更新最小代价模型；
}；
}；
Calculate the Error for consensusMinCostModel；//对根据最小代价模型计算每一组匹配对的误差；
Calculate set of Inliers；//通过误差容许值Tolerence选择内点；Error＜＝Tolerence，Inliers就是经过筛选后的精确匹配对。
所述代价cost计算的代码如下：
count＝number of Data Points//粗糙匹配对的组数计数；
cost＝0；//代价值初始化；
For(n＝0；n＜count；n++){//对粗匹配的每一匹配对进行统计；
cost+＝Error[n]<＝Tolerance？Error[n]：Tolerance；//某一匹配对误差小于等于误差容许值，则代价为所计算误差，如大于误差容许值，则代价为误差容值；
}。
S16：基于所述平面估计的参数化模型，获得三维地形图；
具体的，结合附图8所示，附图8示出本发明实施中的三维地形图的示意图。
S17：基于所述三维地形图，根据参数平面的高度和深度进行排序；
S18：在所述排序之后，通过计算得到平面交线，进而获得平面横截图；
S19：将所述平面横截图与所述得到的平面估计的参数化模型进行对比，评估识别的准确性；
S20：在所述评估之后，将所述三维地形图的点云数据视觉坐标系转化到机器人坐标系。
在本发明具体实施过程中，所述在所述评估之后，将所述三维地形图的点云数据视觉坐标系转化到机器人坐标系包括：在所述评估之后，从相机坐标系上的点经过转换矩阵，将所述三维地形图的点云数据视觉坐标系转化到机器人坐标系；基于所述机器人坐标系，机器人获得障碍物距离本体的距离。
具体的，所述从相机坐标系上的点经过转换矩阵，将所述三维地形图的点云数据视觉坐标系转化到机器人坐标系的具体公式如下：
P_r＝H·P_v；
其中，H为相机坐标系与机器人本体坐标系的位姿转换关系，P_v为相机坐标系的点，P_r为机器人本体坐标系的点。
在本发明实施中，针对传统足式机器人采用模板匹配识别地形的环境适应差问题，采用双目视觉实时获取地形信息，实现楼梯地形的实时识别与三维地形图重构，提高环境适应性的同时实时为步态控制器提供障碍物准确位姿信息，提高机器人运动规划的可靠性；针对轮廓线扫描法提取楼梯轮廓易受机器人本体抖动重新重影现象，提出交叉重叠替换机制对易产生错误轮廓线区域和现象进行修正，提高轮廓识别的准确性；针对地形信息的获取复杂，通过深度学习进行场景识别效率低，实时性差的问题，通过平面轮廓投影截取点云方法获得目标地形信息，采用剔除外点的平面估计法提高地形平面参数的准确性和精度。
实施例
请参阅图2，图2是本发明实施中的用于足式机器人动态运动的地形识别系统的结构组成示意图。
如图2所示，一种用于足式机器人动态运动的地形识别系统，所述系统包括：
图像获取模块21：用于获取RGB双目左右图像；
点云图和轮廓线获得模块22：用于基于所述RGB双目左右图像，分别获得点云图和轮廓线；
在本发明具体实施过程中，所述点云图和轮廓线获得模块还包括：用于基于所述RGB双目左右图像，采用三角测量的算法求得视差图；通过投影模型从所述视差图中获得深度图；结合所述深度图和像素坐标，转化成点云图；以及用于基于RGB双目左右图像，采用轮廓线扫描的算法检测轮廓线，并提取所述检测到的轮廓线；对所述检测到的轮廓线进行面积计算并排序；判断所述检测到的轮廓线包围面积是否小于设定阈值；若是，则进行交叉重叠计算，用大轮廓线代替后得到所需要的轮廓线；若否，则直接得到所需要的轮廓线。
截取模块23：用于将所述轮廓线投影到所述点云图上，截取所述轮廓线内的点云；
共面点云团获得模块24：用于通过所述截取所述轮廓线内的点云，获得共面点云团；
估计与剔除模块25：用于基于所述共面点云团，采用MSAC的算法进行平面估计，得到平面估计的参数化模型，并剔除不符合所述参数化模型的匹配对；
三维地形图获得模块26：用于基于所述平面估计的参数化模型，获得三维地形图；
排序模块27：用于基于所述三维地形图，根据参数平面的高度和深度进行排序；
平面横截图获得模块28：用于在所述排序之后，通过计算得到平面交线，进而获得平面横截图；
对比与评估模块29：用于将所述平面横截图与所述得到的平面估计的参数化模型进行对比，评估识别的准确性；
坐标系转化模块30：用于在所述评估之后，将所述三维地形图的点云数据视觉坐标系转化到机器人坐标系。
具体地，本发明实施例的装置相关功能模块的工作原理可参见方法实施例的相关描述，这里不再赘述。
在本发明实施中，针对传统足式机器人采用模板匹配识别地形的环境适应差问题，采用双目视觉实时获取地形信息，实现楼梯地形的实时识别与三维地形图重构，提高环境适应性的同时实时为步态控制器提供障碍物准确位姿信息，提高机器人运动规划的可靠性；针对轮廓线扫描法提取楼梯轮廓易受机器人本体抖动重新重影现象，提出交叉重叠替换机制对易产生错误轮廓线区域和现象进行修正，提高轮廓识别的准确性；针对地形信息的获取复杂，通过深度学习进行场景识别效率低，实时性差的问题，通过平面轮廓投影截取点云方法获得目标地形信息，采用剔除外点的平面估计法提高地形平面参数的准确性和精度。
本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(ROM，Read Only Memory)、随机存取存储器(RAM，RandomAccess Memory)、磁盘或光盘等。
另外，以上对本发明实施例所提供的一种用于足式机器人动态运动的地形识别方法及系统进行了详细介绍，本文中应采用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。
Claims (10)
1.一种用于足式机器人动态运动的地形识别方法，其特征在于，所述方法包括：
获取RGB双目左右图像；
基于所述RGB双目左右图像，分别获得点云图和轮廓线；
将所述轮廓线投影到所述点云图上，截取所述轮廓线内的点云；
通过所述截取所述轮廓线内的点云，获得共面点云团；
基于所述共面点云团，采用MSAC的算法进行平面估计，得到平面估计的参数化模型，并剔除不符合所述参数化模型的匹配对；
基于所述平面估计的参数化模型，获得三维地形图；
基于所述三维地形图，根据参数平面的高度和深度进行排序；
在所述排序之后，通过计算得到平面交线，进而获得平面横截图；
将所述平面横截图与所述得到的平面估计的参数化模型进行对比，评估识别的准确性；
在所述评估之后，将所述三维地形图的点云数据视觉坐标系转化到机器人坐标系。
2.根据权利要求1所述的一种用于足式机器人动态运动的地形识别方法，其特征在于，所述基于所述RGB双目左右图像，获得点云图包括：
基于所述RGB双目左右图像，采用三角测量的算法求得视差图；
通过投影模型从所述视差图中获得深度图；
结合所述深度图和像素坐标，转化成点云图。
3.根据权利要求1所述的一种用于足式机器人动态运动的地形识别方法，其特征在于，所述基于所述RGB双目左右图像，获得轮廓线包括：
基于RGB双目左右图像，采用轮廓线扫描的算法检测轮廓线，并提取所述检测到的轮廓线；
对所述检测到的轮廓线进行面积计算并排序；
判断所述检测到的轮廓线包围面积是否小于设定阈值；
若是，则进行交叉重叠计算，用大轮廓线代替后得到所需要的轮廓线；
若否，则直接得到所需要的轮廓线。
4.根据权利要求3所述的一种用于足式机器人动态运动的地形识别方法，其特征在于，所述基于RGB双目左右图像，采用轮廓线扫描的算法检测轮廓线，并提取所述检测到的轮廓线包括：
基于RGB双目左右图像，找到一个初始起点，并设置所述初始起点的方向为上；
若像素点P1是前景，则将所述初始起点移动到像素点P2，再移动到像素点P1；
若像素点P1不是前景，则判断像素点P2是否是前景；
若像素点P2是前景，则将所述初始起点移动到像素点P2；
若像素点P1和像素点P2都不是前景，则判断像素点P3是否为前景；
若像素点P3是前景，则将所述初始起点先右转，再移动到像素点P3；
返回至若像素点P1是前景，则将所述初始起点移动到像素点P2，再移动到像素点P1，直到在同一个位置右转三次以上或者返回到初始起点三次以上结束。
5.根据权利要求1所述的一种用于足式机器人动态运动的地形识别方法，其特征在于，所述基于所述共面点云团，采用MSAC的算法进行平面估计，得到平面估计的参数化模型，并剔除不符合所述参数化模型的匹配对包括：
基于所述共面点云团，随机选取一部分粗糙结果中的匹配对；
基于三点算法，通过所述匹配对来计算得出平面；
基于所述平面，反推模型并通过计算所述模型对于所述匹配对的代价；
通过反复迭代所述模型，最终得到平面估计的参数化模型，并剔除不符合所述参数化模型的匹配对。
6.根据权利要求5所述的一种用于足式机器人动态运动的地形识别方法，其特征在于，所述通过计算所述模型对于所述匹配对的代价包括：
计数所述模型中粗糙匹配对的组数；
在所述计数之后，对代价值进行初始化；
基于所述代价值初始化，对所述粗糙匹配对中的每一匹配对进行统计并分析；
若所述匹配对误差小于等于误差容许值，则代价为所计算的误差；
若所述匹配对误差大于误差容许值，则代价为误差容值。
7.根据权利要求1所述的一种用于足式机器人动态运动的地形识别方法，其特征在于，所述在所述评估之后，将所述三维地形图的点云数据视觉坐标系转化到机器人坐标系包括：
在所述评估之后，从相机坐标系上的点经过转换矩阵，将所述三维地形图的点云数据视觉坐标系转化到机器人坐标系；
基于所述机器人坐标系，机器人获得障碍物距离本体的距离。
8.根据权利要求1所述的一种用于足式机器人动态运动的地形识别方法，其特征在于，所述从相机坐标系上的点经过转换矩阵，将所述三维地形图的点云数据视觉坐标系转化到机器人坐标系的具体公式如下：
P_r＝H·P_v；
其中，H为相机坐标系与机器人本体坐标系的位姿转换关系，P_v为相机坐标系的点，P_r为机器人本体坐标系的点。
9.一种用于足式机器人动态运动的地形识别系统，其特征在于，所述系统包括：
图像获取模块：用于获取RGB双目左右图像；
点云图和轮廓线获得模块：用于基于所述RGB双目左右图像，分别获得点云图和轮廓线；
截取模块：用于将所述轮廓线投影到所述点云图上，截取所述轮廓线内的点云；
共面点云团获得模块：用于通过所述截取所述轮廓线内的点云，获得共面点云团；
估计与剔除模块：用于基于所述共面点云团，采用MSAC的算法进行平面估计，得到平面估计的参数化模型，并剔除不符合所述参数化模型的匹配对；
三维地形图获得模块：用于基于所述平面估计的参数化模型，获得三维地形图；
排序模块：用于基于所述三维地形图，根据参数平面的高度和深度进行排序；
平面横截图获得模块：用于在所述排序之后，通过计算得到平面交线，进而获得平面横截图；
对比与评估模块：用于将所述平面横截图与所述得到的平面估计的参数化模型进行对比，评估识别的准确性；
坐标系转化模块：用于在所述评估之后，将所述三维地形图的点云数据视觉坐标系转化到机器人坐标系。
10.根据权利要求9所述的一种用于足式机器人动态运动的地形识别系统，其特征在于，所述点云图和轮廓线获得模块还包括：用于基于所述RGB双目左右图像，采用三角测量的算法求得视差图；通过投影模型从所述视差图中获得深度图；结合所述深度图和像素坐标，转化成点云图；以及用于基于RGB双目左右图像，采用轮廓线扫描的算法检测轮廓线，并提取所述检测到的轮廓线；对所述检测到的轮廓线进行面积计算并排序；判断所述检测到的轮廓线包围面积是否小于设定阈值；若是，则进行交叉重叠计算，用大轮廓线代替后得到所需要的轮廓线；若否，则直接得到所需要的轮廓线。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911402644.2A CN111198563B (zh) | 2019-12-30 | 2019-12-30 | 一种用于足式机器人动态运动的地形识别方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911402644.2A CN111198563B (zh) | 2019-12-30 | 2019-12-30 | 一种用于足式机器人动态运动的地形识别方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111198563A true CN111198563A (zh) | 2020-05-26 |
| CN111198563B CN111198563B (zh) | 2022-07-29 |
Family
ID=70744510
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911402644.2A Active CN111198563B (zh) | 2019-12-30 | 2019-12-30 | 一种用于足式机器人动态运动的地形识别方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111198563B (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113287969A (zh) * | 2021-04-12 | 2021-08-24 | 安克创新科技股份有限公司 | 清扫表面类型的判断方法、清扫模式调整方法及清扫设备 |
| US20220270323A1 (en) * | 2021-02-19 | 2022-08-25 | Insurance Services Office, Inc. | Computer Vision Systems and Methods for Supplying Missing Point Data in Point Clouds Derived from Stereoscopic Image Pairs |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20130181983A1 (en) * | 2010-06-25 | 2013-07-18 | Kabushiki Kaisha Topcon | Point cloud data processing device, point cloud data processing system, point cloud data processing method, and point cloud data processing program |
| CN103868460A (zh) * | 2014-03-13 | 2014-06-18 | 桂林电子科技大学 | 基于视差优化算法的双目立体视觉自动测量方法 |
| CN104484648A (zh) * | 2014-11-27 | 2015-04-01 | 浙江工业大学 | 基于轮廓识别的机器人可变视角障碍物检测方法 |
| US20150379766A1 (en) * | 2013-02-21 | 2015-12-31 | Isis Innovation Limted | Generation of 3d models of an environment |
| CN107730585A (zh) * | 2017-11-06 | 2018-02-23 | 济南市市政工程设计研究院（集团）有限责任公司 | 一种地形三维模型生成方法及系统 |
| CN108846888A (zh) * | 2018-04-23 | 2018-11-20 | 北京建筑大学 | 一种古木建筑构件精细尺寸信息自动化提取方法 |
| CN109410183A (zh) * | 2018-10-08 | 2019-03-01 | 香港理工大学 | 基于点云数据的平面提取方法、系统、装置及存储介质 |
| WO2019110824A1 (en) * | 2017-12-07 | 2019-06-13 | Imra Europe S.A.S. | Using silhouette for fast object recognition |
-
2019
- 2019-12-30 CN CN201911402644.2A patent/CN111198563B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20130181983A1 (en) * | 2010-06-25 | 2013-07-18 | Kabushiki Kaisha Topcon | Point cloud data processing device, point cloud data processing system, point cloud data processing method, and point cloud data processing program |
| US20150379766A1 (en) * | 2013-02-21 | 2015-12-31 | Isis Innovation Limted | Generation of 3d models of an environment |
| CN103868460A (zh) * | 2014-03-13 | 2014-06-18 | 桂林电子科技大学 | 基于视差优化算法的双目立体视觉自动测量方法 |
| CN104484648A (zh) * | 2014-11-27 | 2015-04-01 | 浙江工业大学 | 基于轮廓识别的机器人可变视角障碍物检测方法 |
| CN107730585A (zh) * | 2017-11-06 | 2018-02-23 | 济南市市政工程设计研究院（集团）有限责任公司 | 一种地形三维模型生成方法及系统 |
| WO2019110824A1 (en) * | 2017-12-07 | 2019-06-13 | Imra Europe S.A.S. | Using silhouette for fast object recognition |
| CN108846888A (zh) * | 2018-04-23 | 2018-11-20 | 北京建筑大学 | 一种古木建筑构件精细尺寸信息自动化提取方法 |
| CN109410183A (zh) * | 2018-10-08 | 2019-03-01 | 香港理工大学 | 基于点云数据的平面提取方法、系统、装置及存储介质 |
Non-Patent Citations (2)
| Title |
|---|
| XIAOMING ZHAO等: "Real-Time Stairs Geometric Parameters Estimation for Lower Limb Rehabilitation Exoskeleton", 《THE 30TH CHINESE CONTROL AND DECISION CONFERENCE (2018 CCDC)》 * |
| 李刚等: "3D视觉技术在机器人抓取作业中的应用", 《自动化博览》 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20220270323A1 (en) * | 2021-02-19 | 2022-08-25 | Insurance Services Office, Inc. | Computer Vision Systems and Methods for Supplying Missing Point Data in Point Clouds Derived from Stereoscopic Image Pairs |
| CN113287969A (zh) * | 2021-04-12 | 2021-08-24 | 安克创新科技股份有限公司 | 清扫表面类型的判断方法、清扫模式调整方法及清扫设备 |
| CN113287969B (zh) * | 2021-04-12 | 2022-07-01 | 安克创新科技股份有限公司 | 清扫表面类型的判断方法、清扫模式调整方法及清扫设备 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111198563B (zh) | 2022-07-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111563442B (zh) | 基于激光雷达的点云和相机图像数据融合的slam方法及系统 | |
| CN110568447B (zh) | 视觉定位的方法、装置及计算机可读介质 | |
| CN106940704B (zh) | 一种基于栅格地图的定位方法及装置 | |
| CN110176032B (zh) | 一种三维重建方法及装置 | |
| CN112785643A (zh) | 一种基于机器人平台的室内墙角二维语义地图构建方法 | |
| CN109903331B (zh) | 一种基于rgb-d相机的卷积神经网络目标检测方法 | |
| CN111862201B (zh) | 一种基于深度学习的空间非合作目标相对位姿估计方法 | |
| CN110879994A (zh) | 基于形状注意力机制的三维目测检测方法、系统、装置 | |
| Muñoz-Bañón et al. | Targetless camera-LiDAR calibration in unstructured environments | |
| CN111274943A (zh) | 一种检测方法、装置、电子设备及存储介质 | |
| CN111998862B (zh) | 一种基于bnn的稠密双目slam方法 | |
| CN110487274B (zh) | 用于弱纹理场景的slam方法、系统、导航车及存储介质 | |
| GB2612029A (en) | Lifted semantic graph embedding for omnidirectional place recognition | |
| CN109033989A (zh) | 基于三维点云的目标识别方法、装置及存储介质 | |
| CN112818925A (zh) | 一种城市建筑和树冠识别方法 | |
| CN111161334B (zh) | 一种基于深度学习的语义地图构建方法 | |
| CN111144349A (zh) | 一种室内视觉重定位方法及系统 | |
| CN111198563B (zh) | 一种用于足式机器人动态运动的地形识别方法及系统 | |
| CN110567441A (zh) | 基于粒子滤波的定位方法、定位装置、建图及定位的方法 | |
| CN113052907A (zh) | 一种动态环境移动机器人的定位方法 | |
| CN113112547A (zh) | 机器人及其重定位方法、定位装置及存储介质 | |
| CN116879870A (zh) | 一种适用于低线束3d激光雷达的动态障碍物去除方法 | |
| CN109636897B (zh) | 一种基于改进RGB-D SLAM的Octomap优化方法 | |
| Hu et al. | R-CNN based 3D object detection for autonomous driving | |
| Buck et al. | Capturing uncertainty in monocular depth estimation: Towards fuzzy voxel maps |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |