CN113902828A - 一种以墙角为关键特征的室内二维语义地图的构建方法 - Google Patents

一种以墙角为关键特征的室内二维语义地图的构建方法 Download PDF

Info

Publication number
CN113902828A
CN113902828A CN202111127160.9A CN202111127160A CN113902828A CN 113902828 A CN113902828 A CN 113902828A CN 202111127160 A CN202111127160 A CN 202111127160A CN 113902828 A CN113902828 A CN 113902828A
Authority
CN
China
Prior art keywords
corner
angle
wall
semantic
indoor
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.)
Pending
Application number
CN202111127160.9A
Other languages
English (en)
Inventor
蒋林
万乐
刘奇
赵慧
胡雨欣
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuhan University of Science and Engineering WUSE
Wuhan University of Science and Technology WHUST
Original Assignee
Wuhan University of Science and Engineering WUSE
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Wuhan University of Science and Engineering WUSE filed Critical Wuhan University of Science and Engineering WUSE
Priority to CN202111127160.9A priority Critical patent/CN113902828A/zh
Publication of CN113902828A publication Critical patent/CN113902828A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T11/00—2D [Two Dimensional] image generation
    • G06T11/20—Drawing from basic elements, e.g. lines or circles
    • G06T11/206—Drawing of charts or graphs
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Theoretical Computer Science (AREA)
  • Manipulator (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明针对AMCL算法在重定位过程中收敛速度慢，在相似和变化环境下容易收敛错误等问题，提出了一种以墙角为关键特征的室内二维语义地图的构建方法。首先利用激光雷达进行栅格地图构建，其次使用深度相机获取墙角语义信息和非墙角物体语义标签，同时利用电子罗盘和激光雷达数据进行墙角方向性判断获取墙角类别标签，最后利用贝叶斯估计构建带有墙角类别标签和非墙角物体语义标签的语义地图。通过试验证明该方法能够克服相似和变化环境下AMCL定位不准的问题，并且在大环境场景下也能快速定位。

Description

一种以墙角为关键特征的室内二维语义地图的构建方法
技术领域
本发明涉及机器人平台建图定位导航学习领域，尤其涉及一种以墙角为关键特征的室内二维语义地图的构建方法。
背景技术
基于粒子滤波的蒙特卡洛定位算法(Monte Carlo Localization,MCL)通过在地图中撒布粒子来代表机器人的位姿，然后观测模型来更新粒子权重并进行重采样，最后粒子聚集的位置就是机器人的位姿。考虑到该算法在机器人遭遇“绑架”或者全局定位错误时不能够重新定位，于是提出自适应蒙特卡洛定位算法(adaptive Monte CarloLocalization,AMCL)AMCL是在MCL的基础上增加长期和短期的指数滤波器衰减率来计算随机采样概率，在粒子的置信度较低时可以重新分布粒子来优化机器人定位失败的情况。
本申请发明人在实施本发明的过程中，发现现有技术中存在如下技术问题：
AMCL算法建立的的栅格地图和重定位过程虽然简单，但其只有环境的轮廓信息，机器人由于先验信息的不足，容易在相似环境和变化环境中迷失自身位姿，在重定位过程中容易收敛错误以及收敛速度慢。
由此，可知现有的方法存在定位不准的技术问题。
发明内容
本发明提出了一种以墙角为关键特征的室内二维语义地图的构建方法，用于解决或者至少部分解决现有技术中的方法存在的定位不准的技术问题。
为了解决上述技术问题，本发明提供了一种以墙角为关键特征的室内二维语义地图的构建方法，包括：
S1：利用激光雷达实时采集室内物体与机器人平台的距离、室内物体与机器人平台的方向角以及墙面与激光雷达的深度信息，利用里程计获得先验位姿信息，使用深度相机获取墙角语义信息和非墙角物体语义标签，同时利用电子罗盘获取航向角；
S2：基于室内物体与机器人平台的距离、室内物体与机器人平台的方向角以及先验位姿信息，利用预设算法处理得到栅格地图；
S3：根据墙面与激光雷达的深度信息以及电子罗盘获取的航向角，对室内墙角进行方向性判断，基于方向性判断结果以及墙角语义信息，获得墙角类别标签；
S4：将获取到的墙角类别标签和非墙角物体语义标签从相机坐标系转化到世界坐标系下，然后利用贝叶斯估计从多帧图像中获取非墙角物体语义标签，增量式地构建带有墙角类别标签和非墙角物体语义标签的语义图；
S5：将带有墙角类别标签和非墙角物体语义标签的语义图与栅格地图通过原点重合、位姿对齐融合得到以墙角为关键特征的室内二维语义地图。
在一种实施方式中，步骤S3中根据墙面与激光雷达的深度信息以及电子罗盘获取的航向角，对室内墙角进行方向性判断，包括：
通过激光雷达击中墙面的激光点采用IEPF算法进行直线拟合，得到墙面与电子罗盘获取的航向角之间的夹角；
当每类墙角不存重叠航向角时，识别到的墙角为对应的墙角类别；
当存在重叠航向角时，判断墙角位于航向角的左侧还是右侧，若该侧只有一种墙角类型，则该墙角为对应墙角类型，如果该侧有两类墙角，则计算航向角与墙角的该侧墙面之间的夹角θ，并将θ＜35°时的墙角类型为航向角的该侧墙角，θ＞55°时的墙角类型为航向角的另一侧墙角。
在一种实施方式中，通过激光雷达击中墙面的激光点采用IEPF算法进行直线拟合，包括：
(1)对于一帧激光数据点Z(P1，P2，…，Pn)，根据端点坐标P1和Pn求取直线l，计算中间激光点距离直线l的最大距离hi；
(2)若hi大于阈值H，则在该点位置将激光数据点拆分为Z1(P1，P2，…，Pi)和Z2(Pi，Pi+1，…，Pn)；
(3)重复上述步骤(1)和(2)，将激光雷达数据分成若干段，直至所有点集与对应直线距离小于阈值H；
(4)对步骤(3)中得到每一段激光数据使用最小二乘法求取拟合直线，获取墙面激光数据拟合之后的直线。
在一种实施方式中，墙角类别包括墙角1和墙角2，当存在重叠航向角时，
电子罗盘获取的航向角从起始角度顺时针旋转35°，达到相机左极限视角范围，相机只能看到右上角的墙角1，机器人航向角为35°；当电子罗盘获取的航向角从起始角度顺时针旋转55°，达到相机右极限视角范围，相机只能看到墙角1；此外，电子罗盘获取的航向角从起始角度顺时针旋转305°和从起始角度旋转325°达到当前相机的左右极限视角，只能看到左上角的墙角2，此时航向角的范围为305°至325°。
在一种实施方式中，步骤S4包括：
S4.1：获取相机坐标系和机器人坐标系之间的转换矩阵Tr,k和机器人坐标系与世界坐标系之间的转换矩阵Tw,r，如下所示：
Figure BDA0003278980310000031
Figure BDA0003278980310000032
式中，R为旋转矩阵，t为平移向量，(xt,yt,θt)为机器人在世界坐标系下的实时位姿，由slam算法得到；
S4.2：获得相机的像素坐标点转换相机坐标系下的公式以及相机坐标下的点(Xk,Yk,Zk)转换到世界坐标系下的点(Xw,Yw,Zw)的坐标，其中，相机的像素坐标点转换相机坐标系下的公式为：
Figure BDA0003278980310000033
式中，(Xk,Yk,Zk)为相机坐标系下点的坐标，(u,v)T为像素坐标点，fx，fy，cx，cy是相机内参，通过相机标定得到，Zk＝d/1000，d是像素点(u，v)T的深度值，则相机坐标下的点(Xk,Yk,Zk)转换到世界坐标系下的点(Xw,Yw,Zw)的坐标为：
Figure BDA0003278980310000041
Figure BDA0003278980310000042
S4.3：将点的坐标映射到栅格坐标系下，其中，栅格地图分辨率为0.05m，计算结果向上取整，计算公式如下：
Figure BDA0003278980310000043
S4.4：使用贝叶斯估计从多个关键帧获取语义标签，增量式地构建语义地图，给定机器人位姿为x1:t＝{x1,x2,…,xt}，相机器数据为z1:t＝{z1,z2,…,zt}，则语义地图m表示为每一个语义栅格mi的概率累积，采用贝叶斯估计更新每个语义栅格中存在障碍物的概率：机器人遍历整个室内环境，判断每个栅格的最终后验概率是否大于设定阈值，得到最终的物体语义标签，构建语义地图。
在一种实施方式中，所述方法还包括：
加载建立好的以墙角为关键特征的室内二维语义地图，将非墙角物体形状采用最小包围矩阵和最小包围圆圈表示，得到优化之后的以墙角为关键特征的室内二维语义地图。
本申请实施例中的上述一个或多个技术方案，至少具有如下一种或多种技术效果：
本发明提供的一种以墙角为关键特征的室内二维语义地图的构建方法，使用深度相机获取墙角语义信息和非墙角物体语义标签，可以通过深度学习获取环境语义信息，同时借助电子罗盘进行墙角方向性判断，同步机器人位姿进行坐标映射，构建带有墙角类别标签和非墙角物体语义标签的二维语义地图。机器人在进行重定位时，通过视觉进行机器人位姿估计，计算粒子状态分布与机器人位姿估计匹配概率以及激光雷达观测数据与地图匹配概率，共同更新粒子权重，提高机器人对环境的辨识度，试验结果表明，在相似环境和变化环境下本发明算法能够快速定位成功，增强了机器人全局重定位的鲁棒性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例中提供的以墙角为关键特征的室内二维语义地图的构建方法流程图；
图2为本发明实施例中构建的二维栅格地图；
图3为本发明实施例中直线拟合原理示意图；
图4为本发明实施例中非墙角物体语义图；
图5为本发明实施例中墙角语义图；
图6为本发明实施例中非墙角物体语义地图；
图7为本发明实施例中墙角语义地图；
图8为本发明实施例中以墙角为关键特征的室内二维语义地图；
图9为本发明实施例中墙角类别和传感器示意图；
图10为本发明实施例中不存在重叠航向角下墙角类别判断示意图；
图11为本发明实施例中存在重叠航向角下墙角类别判断示意图；
图12为本发明实施例中墙角1和墙角2类别判断流程图；
图13为本发明实施例中优化语义地图。
具体实施方式
本发明提供了一种以墙角为关键特征的室内二维语义地图的构建方法，能够克服相似和变化环境下AMCL定位不准的问题，并且在大环境场景下也能快速定位。
本发明的主要发明构思如下：
针对AMCL在全局重定位时，只用激光雷达数据进行地图匹配，一旦环境相似或者环境发生变化时，机器人收敛错误。本发明算法通过深度学习获取环境语义信息，同时借助电子罗盘进行墙角方向性判断，同步机器人位姿进行坐标映射，构建带有墙角类别标签和非墙角物体语义标签的二维语义地图。机器人在进行重定位时，通过视觉进行机器人位姿估计，计算粒子状态分布与机器人位姿估计匹配概率以及激光雷达观测数据与地图匹配概率，共同更新粒子权重，提高机器人对环境的辨识度，试验结果表明，在相似环境和变化环境下本发明算法能够快速定位成功，增强了机器人全局重定位的鲁棒性。
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
下面结合图1至图13与表1至表3介绍本发明的具体实施方式为本发明的具体实施例为以墙角为关键特征的室内二维语义地图的构建方法。需要说明的是，涉及到采用色彩区分的附图，具体参见实审参考材料。
本发明实施例提供了一种以墙角为关键特征的室内二维语义地图的构建方法，包括：
S1：利用激光雷达实时采集室内物体与机器人平台的距离、室内物体与机器人平台的方向角以及墙面与激光雷达的深度信息，利用里程计获得先验位姿信息，使用深度相机获取墙角语义信息和非墙角物体语义标签，同时利用电子罗盘获取航向角；
S2：基于室内物体与机器人平台的距离、室内物体与机器人平台的方向角以及先验位姿信息，利用预设算法处理得到栅格地图；
S3：根据墙面与激光雷达的深度信息以及电子罗盘获取的航向角，对室内墙角进行方向性判断，基于方向性判断结果以及墙角语义信息，获得墙角类别标签；
S4：将获取到的墙角类别标签和非墙角物体语义标签从相机坐标系转化到世界坐标系下，然后利用贝叶斯估计从多帧图像中获取非墙角物体语义标签，增量式地构建带有墙角类别标签和非墙角物体语义标签的语义图；
S5：将带有墙角类别标签和非墙角物体语义标签的语义图与栅格地图通过原点重合、位姿对齐融合得到以墙角为关键特征的室内二维语义地图。
本发明提出以墙角为关键特征的室内二维语义地图的构建方法，使用KinectV2相机获取非墙角物体语义标签以及使用电子罗盘和激光雷达数据进行墙角方向性判断获取墙角类别标签，对于室内非墙角物体使用DeeplabV2网络进行语义信息提取，由于墙角不好使用语义分割进行提取，因此对于墙角采用SSD算法进行语义信息提取，该算法同时兼顾检测速度和精度，是目标检测算法中较好的选择。
具体实施时，本发明的方法依托的硬件平台包括：机器人平台底盘、主控机、激光雷达传感器、深度相机、电子罗盘；所述主控机分别与所述的机器人平台底盘、激光雷达传感器、深度相机、电子罗盘依次连接。
具体示例中，主控机选用的是酷睿M4I7-D迷你主机；
机器人平台底盘选用的是pibot品牌的Arduino驱动板底盘；
激光雷达传感器选用性能稳定的SICK lms111激光雷达；
深度相机选用的是微软的Kinect V2；
电子罗盘选用的是瑞芬科技型号为DCM302B电子罗盘。
请参见图1，为本发明实施例中提供的以墙角为关键特征的室内二维语义地图的构建方法流程图；墙角语义信息和非墙角物体语义标签是通过Kinect相机的RGB-D彩色摄像头通过深度学习中的目标检测获取的。
通过主控机控制所述机器人平台在室内运动，首先利用激光雷达传感器实时采集室内物体与机器人平台的距离和室内物体与机器人平台的方向角，并传输至主控机，利用Gmapping或Cartographer算法处理得到栅格地图(如图2所示)，其次使用深度相机获取墙角语义信息和非墙角物体语义标签，并利用电子罗盘获取航向角，激光雷达获取相应的数据(墙面与激光雷达的深度信息)。
可以通过激光雷达构建栅格地图，同时得到机器人的实时位姿，最后使用KinectV2获取非墙角物体语义标签以及使用电子罗盘和激光雷达数据进行墙角方向性判断。
具体实施过程中，本发明中采用的电子罗盘可以获得航向角，机器人搭载的电子罗盘可以得到机器人朝向，根据识别到墙角对应的机器人朝向来进行墙角方向性判断。机器人搭载的KinectV2相机，其深度图最大视角范围是70o。
在进行室内墙角方向性判断时，通过激光雷达击中墙面的激光点采用IEPF算法进行直线拟合(如图3的(a)部分和(b)部分所示，其中(a)表示直线拟合原理，(b)表示激光拟合直线)。室内墙角有着四个朝向，每一类墙角都有着独特的标签，因此对墙角进行方向性判断，获取墙角类别标签。
需要说明的是，本发明获取的每一帧图像都是关键帧，由于存在传感器误差和环境的不稳定性，会出现重复累加非墙角物体语义标签，因此使用贝叶斯估计计算出概率最大的图像帧，抛弃掉概率小的图像帧。
由于根据上述方法获取到的非墙角物体语义标签和四类墙角信息(墙角类别标签)都是在KinectV2相机坐标系下，因此需要将相机坐标系转化到世界坐标系下，然后利用贝叶斯估计从多帧图像里面获取非墙角物体语义标签，增量式地构建带有墙角类别标签和非墙角物体语义标签的语义图。
其中，以墙角为关键特征构建出非墙角物体语义图(如图4所示)和墙角语义图(如图5所示)，构建的图中背景色为黑色，可以用不同颜色代表不同的物体语义信息(彩色附图见实审参考材料)。然后得到非墙角物体语义图与栅格地图融合后的地图(如图6所示)和墙角语义图与栅格地图融合后的地图(如图7所示)，图中的物体轮廓基本与栅格障碍物区域相吻合，墙角类别也是对应在正确的墙角处。最终将墙角语义图、非墙角物体语义图与栅格地图通过原点重合、位姿对齐融合得到以墙角为关键特征的室内二维语义地图(如图8所示)，由图可见非墙角物体位置正确，能正确反映环境中的语义信息，有利于机器人正确理解环境与定位。
在一种实施方式中，步骤S3中根据墙面与激光雷达的深度信息以及电子罗盘获取的航向角，对室内墙角进行方向性判断，包括：
通过激光雷达击中墙面的激光点采用IEPF算法进行直线拟合，得到墙面与电子罗盘获取的航向角之间的夹角；
当每类墙角不存重叠航向角时，识别到的墙角为对应的墙角类别；
当存在重叠航向角时，判断墙角位于航向角的左侧还是右侧，若该侧只有一种墙角类型，则该墙角为对应墙角类型，如果该侧有两类墙角，则计算航向角与墙角的该侧墙面之间的夹角θ，并将θ＜35°时的墙角类型为航向角的该侧墙角，θ＞55°时的墙角类型为航向角的另一侧墙角。
具体来说，室内墙角在相机KinectV2视角范围内和电子罗盘获取的航向角范围内进行判断，如果每类墙角都存在不重叠的航向角范围，则在不重叠的航向角范围内识别到的墙角是对应的墙角类别。但是对于一个相邻的墙角，都会有着重叠的航向角部分，则需要采用其他的判断方式。
在一种实施方式中，通过激光雷达击中墙面的激光点采用IEPF算法进行直线拟合，包括：
(1)对于一帧激光数据点Z(P1，P2，…，Pn)，根据端点坐标P1和Pn求取直线l，计算中间激光点距离直线l的最大距离hi；
(2)若hi大于阈值H，则在该点位置将激光数据点拆分为Z1(P1，P2，…，Pi)和Z2(Pi，Pi+1，…，Pn)；
(3)重复上述步骤(1)和(2)，将激光雷达数据分成若干段，直至所有点集与对应直线距离小于阈值H；
(4)对步骤(3)中得到每一段激光数据使用最小二乘法求取拟合直线，获取墙面激光数据拟合之后的直线。
其中，激光雷达发射激光打中墙面返回的信息，通过IEPF算法将各个激光点(每一段激光数据)拟合，然后拟合得到墙面与电子罗盘获取的航向角之间的夹角。
作为可选，机器人搭载电子罗盘可以得到机器人朝向，根据识别到墙角对应的机器人朝向来进行墙角方向性判断。第一次使用时对电子罗盘进行校正，使得电子罗盘航向角起始角度垂直于墙面(如图9所示)，正北方向的航向角为0°，航向角按照顺时针方向增加，范围在0°到360°之间，正北方向的右边墙角设定为墙角1，按照逆时针顺序依次定位墙角2、墙角3和墙角4。
在一种实施方式中，墙角类别包括墙角1和墙角2，当存在重叠航向角时，
电子罗盘获取的航向角从起始角度顺时针旋转35°，达到相机左极限视角范围，相机只能看到右上角的墙角1，机器人航向角为35°；当电子罗盘获取的航向角从起始角度顺时针旋转55°，达到相机右极限视角范围，相机只能看到墙角1；此外，电子罗盘获取的航向角从起始角度顺时针旋转305°和从起始角度旋转325°达到当前相机的左右极限视角，只能看到左上角的墙角2，此时航向角的范围为305°至325°。
具体实施过程中，电子罗盘获取的航向角从起始角度顺时针旋转35°(如图10的(a)部分所示)，此时达到相机左极限视角范围，相机只能看到右上角的墙角1，机器人航向角为35°。当电子罗盘获取的航向角从起始角度顺时针旋转55°(如图10的(b)部分所示)，此时达到相机右极限视角范围，相机也只能看到墙角1。同理，电子罗盘获取的航向角从起始角度顺时针旋转305°和从起始角度旋转325°，达到当前相机的左右极限视角，只能看到左上角的墙角2，此时航向角的范围为305°至325°。根据相同视角分析可以得到其他墙角对应的航向角(如表1所示)，在对应的航向角范围内，相机识别到的墙角是对应类别墙角。
表1
墙角类别 墙角1 墙角2 墙角3 墙角4
航向角(°) 35-55 305-325 215-235 125-145
对于每一个相邻的墙角，它们都有着重叠的航向角部分(如图11所示)，对于墙角1和墙角2，在航向角325°到35°之间(325°到360°和360°到35°)，机器人相机能够看到的墙角既可能是墙角1也可能是墙角2。
对墙角1和墙角2在重叠航向角下进行类别判断流程图(如图12所示)，具体流程如下：
(1)将机器人随机放在地图任意位置(如图11的(a)部分和(b)部分所示)，由上述分析证明，当机器人搭载电子罗盘获取的航向角范围在325°到360°之间，墙角1只可能在航向角右侧，墙角2可能在航向角的左侧或者右侧，因此出现在航向角左侧的墙角一定是墙角2。由上述分析证明，当机器人搭载的电子罗盘获取的航向角范围在0°到35°之间(如图11的(b)部分和(c)部分所示)，墙角2只可能在航向角左侧，墙角1可能在航向角的左侧或者右侧，因此出现在航向角右侧的墙角一定是墙角1。
(2)如果该侧有两类墙角，则计算航向角与墙角的该侧墙面之间夹角θ。例如，随机放置机器人位置(如图11的(a)部分和(b)部分)，当机器人搭载的电子罗盘获取的航向角范围在325°到360°之间，墙角1和墙角2都可能出现在航向角右侧，此时计算航向角与墙角右侧墙面(例如用蓝色表示)之间的夹角θ。当机器人搭载的电子罗盘获取的航向角在0°到35°之间(如图11的(b)部分和(c)部分)，墙角1和墙角2都可能出现在航向角左侧，此时计算航向角与墙角左侧墙面(例如用红色表示)之间的夹角θ。
(3)判断航向角与墙角的该侧墙面之间的夹角θ，当θ＜35°时，墙角类型为航向角的该侧墙角，θ＞55°时，墙角类型为航向角的另一侧墙角。
根据该判断方法，可以得到重叠航向角下的墙角类型判断(如表2)，用①、②、③、④表示对应的墙角1到墙角4，用左和右表示墙角位于航向角的左右两侧。
表2
Figure BDA0003278980310000111
ⅰ当机器人搭载的电子罗盘获取的航向角范围在325°到360°之间，墙角1和墙角2都可能出现在航向角右侧，计算航向角与墙角右侧墙面之间的夹角θ，若θ＜35°时，墙角类型为航向角右侧墙角1；若θ＞55°，墙角类型为航向角左侧墙角2。当机器人搭载的电子罗盘获取的航向角范围在0°到35°之间，墙角1和墙角2都可能出现在航向角左侧，计算航向角与墙角右侧墙面之间的夹角θ，若θ＜35°时，墙角类型为航向角左侧墙角2；若θ＞55°，墙角类型为航向角右侧墙角1。
ⅱ当机器人搭载的电子罗盘获取的航向角范围在235°到270°之间，墙角2和墙角3都可能出现在航向角右侧，计算航向角与墙角右侧墙面之间的夹角θ，若θ＜35°时，墙角类型为航向角右侧墙角2；若θ＞55°，墙角类型为航向角左侧墙角3。当机器人搭载的电子罗盘获取的航向角范围在270°到305°之间，墙角2和墙角3都可能出现在航向角左侧，计算航向角与墙角右侧墙面之间的夹角θ，若θ＜35°时，墙角类型为航向角左侧墙角3；若θ＞55°，墙角类型为航向角右侧墙角2。
ⅲ当机器人搭载的电子罗盘获取的航向角范围在145°到180°之间，墙角3和墙角4都可能出现在航向角右侧，计算航向角与墙角右侧墙面之间的夹角θ，若θ＜35°时，墙角类型为航向角右侧墙角3；若θ＞55°，墙角类型为航向角左侧墙角4。当机器人搭载的电子罗盘获取的航向角范围在180°到215°之间，墙角3和墙角4都可能出现在航向角左侧，计算航向角与墙角右侧墙面之间的夹角θ，若θ＜35°时，墙角类型为航向角左侧墙角4；若θ＞55°，墙角类型为航向角右侧墙角3。
ⅳ当机器人搭载的电子罗盘获取的航向角范围在55°到90°之间，墙角4和墙角1都可能出现在航向角右侧，计算航向角与墙角右侧墙面之间的夹角θ，若θ＜35°时，墙角类型为航向角右侧墙角4；若θ＞55°，墙角类型为航向角左侧墙角1。当机器人搭载的电子罗盘获取的航向角范围在90°到125°之间，墙角4和墙角1都可能出现在航向角左侧，计算航向角与墙角右侧墙面之间的夹角θ，若θ＜35°时，墙角类型为航向角左侧墙角1；若θ＞55°，墙角类型为航向角右侧墙角4。
在一种实施方式中，步骤S4包括：
S4.1：获取相机坐标系和机器人坐标系之间的转换矩阵Tr,k和机器人坐标系与世界坐标系之间的转换矩阵Tw,r，如下所示：
Figure BDA0003278980310000121
Figure BDA0003278980310000122
式中，R为旋转矩阵，t为平移向量，(xt,yt,θt)为机器人在世界坐标系下的实时位姿，由slam算法得到；旋转矩阵在乘以一个向量时，只改变向量的方向但不改变大小。转换矩阵是进行不同坐标系之间的坐标变换，Tw,r和Tw,r为转换矩阵例如，空间中有一点p，它在xyz坐标系下对应的向量为v，而另一个坐标系XYZ下对应的是V，如果存在矩阵T，使得V＝T*v，称xyz中的向量v变换到XYZ中的向量V。
S4.2：获得相机的像素坐标点转换相机坐标系下的公式以及相机坐标下的点(Xk,Yk,Zk)转换到世界坐标系下的点(Xw,Yw,Zw)的坐标，其中，相机的像素坐标点转换相机坐标系下的公式为：
Figure BDA0003278980310000131
式中，(Xk,Yk,Zk)为相机坐标系下点的坐标，(u,v)T为像素坐标点，fx，fy，cx，cy是相机内参，通过相机标定得到，Zk＝d/1000，d是像素点(u，v)T的深度值，则相机坐标下的点(Xk,Yk,Zk)转换到世界坐标系下的点(Xw,Yw,Zw)的坐标为：
Figure BDA0003278980310000132
Figure BDA0003278980310000133
S4.3：将点的坐标映射到栅格坐标系下，其中，栅格地图分辨率为0.05m，计算结果向上取整，计算公式如下：
Figure BDA0003278980310000134
S4.4：使用贝叶斯估计从多个关键帧获取语义标签，增量式地构建语义地图，给定机器人位姿为x1:t＝{x1,x2,…,xt}，相机器数据为z1:t＝{z1,z2,…,zt}，则语义地图m表示为每一个语义栅格mi的概率累积，采用贝叶斯估计更新每个语义栅格中存在障碍物的概率：机器人遍历整个室内环境，判断每个栅格的最终后验概率是否大于设定阈值，得到最终的物体语义标签，构建语义地图。
具体来说，由于传感器存在误差和环境的不确定性，可能导致连续帧获取的语义标签不一致，在建图上就表现出重叠构建语义地图，地图中会重复累加物体语义标签，因此使用贝叶斯估计从多个关键帧获取非墙角物体语义标签，增量式地构建语义地图。给定机器人位姿为x1:t＝{x1,x2,…,xt}，传感器数据(KinectV2相机的数据)为z1:t＝{z1,z2,…,zt}，则语义地图m表示为每一个语义栅格mi的概率累积：
p(m|x1:t,z1:t)＝Πp(mi|x1:t,z1:t) (7)
采用贝叶斯估计更新每个语义栅格中存在障碍物的概率，公式如下：
Figure BDA0003278980310000141
机器人遍历整个室内环境，判断每个栅格的最终后验概率是否大于设定阈值，得到最终的物体语义标签，构建语义地图。其中，mi表示第i个栅格的状态，x1:t表示1到t时刻的机器人位姿，z1:t表示1到t时刻的激光观测数据，p(m|x1:t,z1:t)表示在知道机器人位姿和激光观测数据的条件下估计环境地图，p(mi|x1:t,z1:t)表示在知道机器人位姿和激光观测数据的条件下估计第i个栅格的状态。其他符号类似，可以根据条件概率的定义推导出来，
在一种实施方式中，所述方法还包括：
步骤S6：加载建立好的以墙角为关键特征的室内二维语义地图，将非墙角物体形状采用最小包围矩阵和最小包围圆圈表示，得到优化之后的以墙角为关键特征的室内二维语义地图。
步骤S5建立的以墙角为关键特征的室内二维语义地图是为了能够快速查询墙角类别信息，于是对该语义地图进行初始化，将墙角类别标签和周围非墙角物体语义标签进行存储。
本实施方式中，首先加载建立好的以墙角为关键特征的室内二维语义地图，将非墙角物体形状采用最小包围矩阵和最小包围圆圈进行表示，从而得到优化之后的以墙角为关键特征的室内二维语义地图(如图13所示)。从该图中提取每个非墙角物体的中心坐标，并与非墙角物体的语义标签进行对应。本发明机器人搭载的相机KinectV2的深度图最大视角范围是70°，其在视距0.5m到4.5m之间较准，同时考虑地图大小，因此以墙角为圆心2.5m为半径画圆，中心坐标位于圆内的非墙角物体都视为墙角附近物体。图中只选取部分的墙角点进行示意，在每个墙角点进行搜索，如果有非墙角物体在墙角附近，则将墙角类别信息和非墙角物体类别信息以及相对距离信息进行存储，用d1，d2，d3，d4分别表示出墙角附近的椅子、柜子、垃圾桶、门的中心与墙角的距离。通过遍历整个以墙角为关键特征的室内二维语义地图，得到墙角类别信息及周围非墙角物体语义信息(语义标签)(如表3)。
Figure BDA0003278980310000151
本发明具有如下优点：
针对AMCL在全局重定位时，只用激光雷达数据进行地图匹配，一旦环境相似或者环境发生变化时，机器人收敛错误。本发明算法通过深度学习获取环境语义信息，同时借助电子罗盘进行墙角方向性判断，同步机器人位姿进行坐标映射，构建带有墙角类别标签和非墙角物体语义标签的二维语义地图。机器人在进行重定位时，通过视觉进行机器人位姿估计，计算粒子状态分布与机器人位姿估计匹配概率以及激光雷达观测数据与地图匹配概率，共同更新粒子权重，提高机器人对环境的辨识度，试验结果表明，在相似环境和变化环境下本发明算法能够快速定位成功，增强了机器人全局重定位的鲁棒性。
本发明的保护范围并不限于上述的实施例，显然，本领域的技术人员可以对本发明进行各种改动和变形而不脱离本发明的范围和精神。倘若这些改动和变形属于本发明权利要求及其等同技术的范围内，则本发明的意图也包含这些改动和变形在内。

Claims (6)

1.一种以墙角为关键特征的室内二维语义地图的构建方法，其特征在于，包括：
S1：利用激光雷达实时采集室内物体与机器人平台的距离、室内物体与机器人平台的方向角以及墙面与激光雷达的深度信息，利用里程计获得先验位姿信息，使用深度相机获取墙角语义信息和非墙角物体语义标签，同时利用电子罗盘获取航向角；
S2：基于室内物体与机器人平台的距离、室内物体与机器人平台的方向角以及先验位姿信息，利用预设算法处理得到栅格地图；
S3：根据墙面与激光雷达的深度信息以及电子罗盘获取的航向角，对室内墙角进行方向性判断，基于方向性判断结果以及墙角语义信息，获得墙角类别标签；
S4：将获取到的墙角类别标签和非墙角物体语义标签从相机坐标系转化到世界坐标系下，然后利用贝叶斯估计从多帧图像中获取非墙角物体语义标签，增量式地构建带有墙角类别标签和非墙角物体语义标签的语义图；
S5：将带有墙角类别标签和非墙角物体语义标签的语义图与栅格地图通过原点重合、位姿对齐融合得到以墙角为关键特征的室内二维语义地图。
2.如权利要求1所述的室内二维语义地图的构建方法，其特征在于，步骤S3中根据墙面与激光雷达的深度信息以及电子罗盘获取的航向角，对室内墙角进行方向性判断，包括：
通过激光雷达击中墙面的激光点采用IEPF算法进行直线拟合，得到墙面与电子罗盘获取的航向角之间的夹角；
当每类墙角不存重叠航向角时，识别到的墙角为对应的墙角类别；
当存在重叠航向角时，判断墙角位于航向角的左侧还是右侧，若该侧只有一种墙角类型，则该墙角为对应墙角类型，如果该侧有两类墙角，则计算航向角与墙角的该侧墙面之间的夹角θ，并将θ＜35°时的墙角类型为航向角的该侧墙角，θ＞55°时的墙角类型为航向角的另一侧墙角。
3.如权利要求2所述的室内二维语义地图的构建方法，其特征在于，通过激光雷达击中墙面的激光点采用IEPF算法进行直线拟合，包括：
(1)对于一帧激光数据点Z(P1，P2，…，Pn)，根据端点坐标P1和Pn求取直线l，计算中间激光点距离直线l的最大距离hi；
(2)若hi大于阈值H，则在该点位置将激光数据点拆分为Z1(P1，P2，…，Pi)和Z2(Pi，Pi+1，…，Pn)；
(3)重复上述步骤(1)和(2)，将激光雷达数据分成若干段，直至所有点集与对应直线距离小于阈值H；
(4)对步骤(3)中得到每一段激光数据使用最小二乘法求取拟合直线，获取墙面激光数据拟合之后的直线。
4.如权利要求2所述的室内二维语义地图的构建方法，其特征在于，墙角类别包括墙角1和墙角2，当存在重叠航向角时，
电子罗盘获取的航向角从起始角度顺时针旋转35°，达到相机左极限视角范围，相机只能看到右上角的墙角1，机器人航向角为35°；当电子罗盘获取的航向角从起始角度顺时针旋转55°，达到相机右极限视角范围，相机只能看到墙角1；此外，电子罗盘获取的航向角从起始角度顺时针旋转305°和从起始角度旋转325°达到当前相机的左右极限视角，只能看到左上角的墙角2，此时航向角的范围为305°至325°。
5.如权利要1所述的室内二维语义地图的构建方法，其特征在于，步骤S4包括：
S4.1：获取相机坐标系和机器人坐标系之间的转换矩阵Tr,k和机器人坐标系与世界坐标系之间的转换矩阵Tw,r，如下所示：
Figure FDA0003278980300000021
Figure FDA0003278980300000022
式中，R为旋转矩阵，t为平移向量，(xt,yt,θt)为机器人在世界坐标系下的实时位姿，由slam算法得到；
S4.2：获得相机的像素坐标点转换相机坐标系下的公式以及相机坐标下的点(Xk,Yk,Zk)转换到世界坐标系下的点(Xw,Yw,Zw)的坐标，其中，相机的像素坐标点转换相机坐标系下的公式为：
Figure FDA0003278980300000031
式中，(Xk,Yk,Zk)为相机坐标系下点的坐标，(u,v)T为像素坐标点，fx，fy，cx，cy是相机内参，通过相机标定得到，Zk＝d/1000，d是像素点(u，v)T的深度值，则相机坐标下的点(Xk,Yk,Zk)转换到世界坐标系下的点(Xw,Yw,Zw)的坐标为：
Figure FDA0003278980300000032
Figure FDA0003278980300000033
S4.3：将点的坐标映射到栅格坐标系下，其中，栅格地图分辨率为0.05m，计算结果向上取整，计算公式如下：
Figure FDA0003278980300000034
S4.4：使用贝叶斯估计从多个关键帧获取语义标签，增量式地构建语义地图，给定机器人位姿为x1:t＝{x1,x2,…,xt}，相机器数据为z1:t＝{z1,z2,…,zt}，则语义地图m表示为每一个语义栅格mi的概率累积，采用贝叶斯估计更新每个语义栅格中存在障碍物的概率：机器人遍历整个室内环境，判断每个栅格的最终后验概率是否大于设定阈值，得到最终的物体语义标签，构建语义地图。
6.如权利要1所述的室内二维语义地图的构建方法，其特征在于，所述方法还包括：
加载建立好的以墙角为关键特征的室内二维语义地图，将非墙角物体形状采用最小包围矩阵和最小包围圆圈表示，得到优化之后的以墙角为关键特征的室内二维语义地图。
CN202111127160.9A 2021-09-26 2021-09-26 一种以墙角为关键特征的室内二维语义地图的构建方法 Pending CN113902828A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111127160.9A CN113902828A (zh) 2021-09-26 2021-09-26 一种以墙角为关键特征的室内二维语义地图的构建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111127160.9A CN113902828A (zh) 2021-09-26 2021-09-26 一种以墙角为关键特征的室内二维语义地图的构建方法

Publications (1)

Publication Number Publication Date
CN113902828A true CN113902828A (zh) 2022-01-07

Family

ID=79029202

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111127160.9A Pending CN113902828A (zh) 2021-09-26 2021-09-26 一种以墙角为关键特征的室内二维语义地图的构建方法

Country Status (1)

Country Link
CN (1) CN113902828A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115326048A (zh) * 2022-07-08 2022-11-11 武汉科技大学 基于墙角族为主特征的语义地图构建方法
WO2023173243A1 (zh) * 2022-03-14 2023-09-21 罗伯特·博世有限公司 二维激光雷达扫描图的语义标注生成

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023173243A1 (zh) * 2022-03-14 2023-09-21 罗伯特·博世有限公司 二维激光雷达扫描图的语义标注生成
CN115326048A (zh) * 2022-07-08 2022-11-11 武汉科技大学 基于墙角族为主特征的语义地图构建方法
CN115326048B (zh) * 2022-07-08 2024-06-07 武汉科技大学 基于墙角族为主特征的语义地图构建方法

Similar Documents

Publication Publication Date Title
CN111563442B (zh) 基于激光雷达的点云和相机图像数据融合的slam方法及系统
WO2021233029A1 (en) Simultaneous localization and mapping method, device, system and storage medium
CN111486855B (zh) 一种具有物体导航点的室内二维语义栅格地图构建方法
CN113537208B (zh) 一种基于语义orb-slam技术的视觉定位方法及系统
Nieto et al. Recursive scan-matching SLAM
Hähnel et al. Learning compact 3D models of indoor and outdoor environments with a mobile robot
CN113483747B (zh) 基于具有墙角信息的语义地图改进amcl定位方法及机器人
CN113902828A (zh) 一种以墙角为关键特征的室内二维语义地图的构建方法
CN111522022B (zh) 基于激光雷达的机器人进行动态目标检测方法
CN113108773A (zh) 一种融合激光与视觉传感器的栅格地图构建方法
Li et al. Automatic targetless LiDAR–camera calibration: a survey
Tungadi et al. Robust online map merging system using laser scan matching and omnidirectional vision
CN111678516A (zh) 一种基于激光雷达的有界区域快速全局定位方法
CN117115784A (zh) 目标数据融合的车辆检测方法及检测装置
Betge-Brezetz et al. Object-based modelling and localization in natural environments
CN116844124A (zh) 三维目标检测框标注方法、装置、电子设备和存储介质
Maffei et al. Global localization over 2d floor plans with free-space density based on depth information
CN109816726B (zh) 一种基于深度滤波器的视觉里程计地图更新方法和系统
CN110570473A (zh) 一种基于点线融合的权重自适应位姿估计方法
Ruiz-Mayor et al. Perceptual ambiguity maps for robot localizability with range perception
CN115457130A (zh) 一种基于深度关键点回归的电动汽车充电口检测定位方法
CN111915632B (zh) 一种基于机器学习的贫纹理目标物体真值数据库构建方法
CN111951337B (zh) 图像检测目标空间定位方法及系统
Cupec et al. Global localization based on 3d planar surface segments
CN108549089B (zh) 一种用于slam的镂空障碍物检测装置和方法

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