CN110866927A - 一种基于垂足点线特征结合的ekf-slam算法的机器人定位与构图方法 - Google Patents
一种基于垂足点线特征结合的ekf-slam算法的机器人定位与构图方法 Download PDFInfo
- Publication number
- CN110866927A CN110866927A CN201911149587.1A CN201911149587A CN110866927A CN 110866927 A CN110866927 A CN 110866927A CN 201911149587 A CN201911149587 A CN 201911149587A CN 110866927 A CN110866927 A CN 110866927A
- Authority
- CN
- China
- Prior art keywords
- point
- coordinate system
- line segment
- robot
- ekf
- 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
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/10—Segmentation; Edge detection
- G06T7/11—Region-based segmentation
-
- 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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/22—Matching criteria, e.g. proximity measures
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Electromagnetism (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Evolutionary Computation (AREA)
- Evolutionary Biology (AREA)
- General Engineering & Computer Science (AREA)
- Bioinformatics & Computational Biology (AREA)
- Artificial Intelligence (AREA)
- Life Sciences & Earth Sciences (AREA)
- Computer Networks & Wireless Communication (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本发明公开了一种基于垂足点线特征结合的EKF‑SLAM算法的机器人定位与构图方法。步骤1:将激光雷达获取的点云数据进行区域划分,使得激光雷达的点云能够分割开;步骤2:对分割开的点云进行线段特征的提取,作为EKF算法的观测值;步骤3:将观测值进行数据融合处理,进行一个特征的匹配;步骤4:里程计的信息输入作为EKF的预测阶段,而匹配之后的数据信息作为EKF的更新阶段,完成一次完整的预测更新;步骤5:输出机器人的位姿和地图的信息。本发明采用线段特征作为环境特征,提出了端点迭代的算法进行线段的拟合,基于EKF算法进行优化,利用几何地图作为地图表示,得到基于线段特征的激光雷达SLAM算法,经验证,该算法在实际运用中价值高。
Description
技术领域:
本发明涉及一种基于垂足点线特征结合的EKF-SLAM算法的机器人定位与构图方法。
背景技术:
随着自动化和智能化时代的到来,移动机器人作为一个日益普遍的智能体,已经在不断充斥在人们的科技和生活领域,小到家庭扫地机,大到无人驾驶和星球表面探测,移动机器人都扮演着十分重要的作用;不论处于什么样的环境中,定位与构图是自主移动机器人相对导航的两个重要问题,能够在各种位置的复杂环境中进行自主定位和导航是完成任务的前提,在某些封闭场景中,环境相对单一化,结构化,视觉特征点少,信息不充分,而且在黑暗环境下无法正常工作。
传统的激光雷达SLAM方法有:基于卡尔曼滤波器的KF-SLAM、基于扩展卡尔曼滤波的EKF-SLAM算法、基于粒子滤波的PF-SLAM算法,以及基于图优化的graph-SLAM算法等;卡尔曼滤波(Kalman Filter,KF)的导航算法,该方法是一种线性离散状态估计滤波器,通过系统的观测数据实现对机器人位姿的最优估计,该方法假设系统状态模型噪声、控制输入的噪声以及传感器的噪声均满足高斯分布,但是实际的过程中,大多数机器人的运动都是非线性的,因此,卡尔曼滤波存在着一定的局限性,故而推出了扩展卡尔曼滤波器的算法,这样会比KF得到更加精确的位姿估计;基于粒子滤波器(Particle Filter,PF)算法的导航方式,这种方法则基于贝叶斯估计的后验概率估计,使用非高斯分布的随机样本来近似表示状态,之后通过加权来确定最终的状态向量的概率分布;这种方法在上述噪声非高斯分布的非线性系统定位估计中得到大量的使用;图优化的SLAM算法通过位姿节点和节点之间的约束来构建一个位姿图,最后采用最小二乘的优化方法来优化整个图结构,进而完成对位姿和地图的优化;在以上方法中,KF-SLAM和EKF-SLAM多采用点特征作为地图表示,而PF-SLAM和graph-SLAM多采用占据栅格地图作为地图标识,点特征的地图只有稀疏的路标点,不能作为导航的依据,而栅格地图需要维护庞大的网格,除此之外,这两种SLAM方法都需要对点云数据进行匹配处理,点云数据相对巨大,而且粒子滤波需要维护大量的粒子。
发明内容:
本发明的目的是提供一种基于垂足点线特征结合的EKF-SLAM算法的机器人定位与构图方法,采用线段特征作为环境特征,提出了端点迭代的算法进行线段的拟合,基于EKF算法进行优化,利用几何地图作为地图表示,得到基于线段特征的激光雷达SLAM算法,经验证,该算法在实际运用中价值高。
上述的目的通过以下的技术方案实现:
一种基于垂足点线特征结合的EKF-SLAM算法的机器人定位与构图方法,所述算法包括以下步骤:
步骤1:将激光雷达获取的点云数据输入到区域分割模块进行区域划分,使得激光雷达的点云能够分割开;
步骤2:对分割开的区域里的点云进行线段特征的提取,作为EKF算法的观测值;
步骤3:将观测值进行数据融合处理,进行一个特征的匹配;
步骤4:里程计的信息输入作为EKF的预测阶段,而匹配之后的数据信息作为EKF的更新阶段,完成一次完整的预测更新;
步骤5:输出机器人的位姿和地图的信息。
进一步的,所述步骤1点云数据输入到区域分割模块进行区域划分具体为:激光雷达在每次扫描的过程中会提供一系列关于障碍物的距离信息ds=[ds-90°,...,ds90°]和角度信息θs=[-90°,...,90°];但是由于激光雷达获得的点都是在极坐标系下表示的,为了能够描绘出环境的位置,需要将扫描的点转换到世界坐标系下进行拟合;对于每个扫描点mi(ds(i),θs(i)),将其映射到世界坐标系下可表示为:
X(i)=ds(i)*cos(θs(i)+θr)+xr (1)
Y(i)=ds(i)*sin(θs(i)+θr)+yr (2)
假设激光雷达每次可以获得到的数据点的个数为n,并以这n个点作为一个初始区域,对这个区域的数据进行处理,采用距离分割的方法,从第一个点开始,首先通过欧式距离判断相邻两个点的距离Dj,通过设定一个阈值δ,判断Dj和δ的大小,确定是否从这一点分开;当Dj大于δ时,确定点(Xj,Yj)是区域的一个分割点,以此点将数据划分成两个部分,可以得到第一个区域A1{(Xi,Yi)|i=1,2,...,j}和剩下的待分割区域,以此类推,假设得到N个互相不连通的区域{A1,A2,...,AN}。
进一步的,所述步骤2线段特征的提取,在区域分割的基础之上使用迭代端点的拟合算法,具体为:对于每一次激光雷达扫描获得的激光点云,通过阈值的设定对得到的扫描点进行区域A1{(x1,y1),...,(xN,yN)}的分割,将这个区域分解成区域A11{(x1,y1),...,(xl,yl)}和区域A12{(xl+1,yl+1),...,(xN,yN)},如此循环下去,直至细分的所有区域都满足能够拟合成直线的要求。
进一步的,所述线段表示选择机器人坐标系的原点到线段所在直线的垂足点作为参考,并将这个点坐标转换为世界坐标系下的原点到线段所在直线的垂足点坐标,用这两个参数来当作线段的地标特征点,由于线段是有限的,所以另外四个参数分别表示的是线段的起点和终点在机器人坐标系下的数值,并将其转换到世界坐标系下:βst(Xstl,Ystl),βet(Xetl,Yetl)另外由于机器人在始终的移动,而垂足点在机器人坐标系下的坐标(xrl,yrl)经过机器人坐标系到世界坐标系之间的转换后的点并不是世界系下的原点到线段所在直线的投影坐标(XWl,YWl),故而,这里建立一个中间坐标系ORxy,该坐标系的原点与机器人坐标系的原点重合,x轴y轴指向分别与世界坐标系下的X,Y方向重合,同时令移动机器人到线段所在的投影点在中间坐标系下的坐标为(xl,yl),最后使用以下参数来表示线段:
[XWl,YWl,βst(Xstl,Ystl),βet(Xetl,Yetl)] (7)
求解出(xrl,yrl)、(xl,yl)参数在各自坐标系下的表达式:
将中间坐标系下的移动机器人投影点坐标转换至世界坐标系下可以求解出世界坐标系原点到线段所在直线的投影点坐标:
进一步的,所述步骤4具体为:
步骤4.1:假设地标的位置是不发生变化的,根据移动机器人的运动方程,利用运动模型对由t-1时刻的状态和控制量预测t时刻的运动状态,通过前面的模型可以得到:
步骤4.2:地标环境预测:
[XWli(k+1),YWli(k+1)]=Xi(k+1)=Xi(k)=[XWli(k),YWli(k)] (11)
故而系统的状态预测方程可以写为:
步骤4.3:量测更新:
每次扫描激光雷达都可以获得一系列连续的点,利用前文提到的数据处理算法可以将这些连续的点拟合成多条直线,进而在利用上一节提到的方法提取出线段的地标,状态中的地标位置用原点到线段所在直线的投影来表示;
步骤4.4:状态增广:
当检测到状态中不曾出现过的路标的时候,就认为出现了新的地标,这时需把新的地标加入到状态中,根据量测方程得到新地标的位置;
步骤4.5:更新系统的状态和协方差的初始值。
有益效果:
利用本算法,对在结构化环境中的移动机器人定位精度在5%以内。
附图说明:
附图1是本发明的算法整体结构示意图。
附图2是本发明(a)激光雷达扫描点云分布图,(b)区域分割示意图。
附图3是本发明(a)A1区域示意图,(b)A1区域分割示意图。
附图4是本发明的迭代端点算法原理图及数据仿真验证图。
附图5是本发明线段地标在机器人坐标系下和世界坐标系下的表示图。
附图6是本发明两线段间的匹配图。
附图7是本发明(a)观测线段与匹配地标线段不重叠,(b)观测线段与匹配地标线段部分重叠图,(c)观测线段包含于匹配地标线段,(d)观测线段包含匹配地标线段。
附图8是本发明扩充系统协方差矩阵图。
附图9是本发明(a)环境地图无拐角时的线段提取图,(b)机器人的运动图。
附图10是本发明(a)无拐角位置激光点在机器人坐标系下的分布图,(b)全局坐标系下的机器人运动图。
附图11是本发明(a)拐角处激光点在机器人坐标系下的分布图,(b)全局坐标系下的机器人运动图。
附图12是本发明(a)初始时刻的位姿估计图,(b)t=10s时刻的位姿估计图,(c)t=20s时刻的位姿估计图,(d)t=35s时刻的位姿估计图,(e)t=50s时刻的位姿估计图,(f)t=65s时刻的位姿估计。
附图13是本发明(a)t=0s时刻EKF-SLAM绘制的地图,(b)t=10s时刻EKF-SLAM绘制的地图,(c)t=20s时刻EKF-SLAM绘制的地图,(d)t=45s时刻EKF-SLAM绘制的地图,(e)t=65s时刻EKF-SLAM绘制的地图。
附图14是本发明(a)机器人全局行走运动图,(b)对环境的绘制地图
附图15是本发明轨迹图及机器人各参数的误差曲线。
具体实施方式:
下述说明并非是对本发明的限制,本发明也并不仅限于下述举例,本技术领域的技术人员在本发明的实质范围内所做出的变化、改型、添加或替换,也应属于本发明的保护范围。
实施例1
相比于传统的SLAM,该算法采用区域分割和迭代端点的方法进行折线拟合,代替传统的最小二乘,能够实现折角处的线段分割和提取,除此之外,利用垂足点,结合线段的长度和夹角信息作为匹配依据,完成EKF-SLAM的更新。
本算法采用SICK激光雷达,可以获取18米内,分辨率为0.5的180°范围内的激光点云;首先将激光雷达获取的点云数据输入到区域分割模块进行区域划分,使得激光雷达的点云能够分割开来;然后对分割开的区域里的点云进行线段特征的提取,作为EKF算法的观测值;将观测值进行数据融合处理,进行一个特征的匹配;里程计的信息输入作为EKF的预测阶段,而匹配之后的数据信息作为EKF的更新阶段,完成一次完整的预测更新,最后输出机器人的位姿和地图的信息。
一种基于垂足点线特征结合的EKF-SLAM算法的机器人定位与构图方法,所述算法包括以下步骤,
步骤1:将激光雷达获取的点云数据输入到区域分割模块进行区域划分,使得激光雷达的点云能够分割开;
步骤2:对分割开的区域里的点云进行线段特征的提取,作为EKF算法的观测值;
步骤3:将观测值进行数据融合处理,进行一个特征的匹配;
步骤4:里程计的信息输入作为EKF的预测阶段,而匹配之后的数据信息作为EKF的更新阶段,完成一次完整的预测更新;
步骤5:输出机器人的位姿和地图的信息。
进一步的,所述步骤1点云数据输入到区域分割模块进行区域划分具体为:由于结构化环境中存在较多的直线和线段,线段的特征通常是通过激光雷达获取的数据点来表示,而激光雷达根据自己的扫描范围,在此之内被照射到的物体都会返回给雷达,得到一个扫描点;激光雷达在每次扫描的过程中会提供一系列关于障碍物的距离信息ds=[ds-90°,...,ds90°]和角度信息θs=[-90°,...,90°];如图2(a)所示的是激光雷达的可视化范围;但是由于激光雷达获得的点都是在极坐标系下表示的,为了能够描绘出环境的位置,需要将扫描的点转换到世界坐标系下进行拟合;对于每个扫描点mi(ds(i),θs(i)),将其映射到世界坐标系下可表示为:
X(i)=ds(i)*cos(θs(i)+θr)+xr (1)
Y(i)=ds(i)*sin(θs(i)+θr)+yr (2)
假设激光雷达每次可以获得到的数据点的个数为n,并以这n个点作为一个初始区域,对这个区域的数据进行处理,采用距离分割的方法,从第一个点开始,首先通过欧式距离判断相邻两个点的距离Dj,通过设定一个阈值δ,判断Dj和δ的大小,确定是否从这一点分开;当Dj大于δ时,确定点(Xj,Yj)是区域的一个分割点,以此点将数据划分成两个部分,可以得到第一个区域A1{(Xi,Yi)|i=1,2,...,j}和剩下的待分割区域,以此类推,假设得到N个互相不连通的区域{A1,A2,...,AN}。
进一步的,所述步骤2线段特征的提取与表示模块,角点是一个十分常见的特征,能否实现直线的准确拟合关系到环境的准确与否,否则会影响匹配的精度,带来定位的不准确,所以很有必要对这样的数据进行折线的拟合;为了解决这个问题,在区域分割的基础之上使用迭代端点的拟合算法,具体为:对于每一次激光雷达扫描获得的激光点云,通过阈值的设定对得到的扫描点进行区域A1{(x1,y1),...,(xN,yN)}的分割,将这个区域分解成区域A11{(x1,y1),...,(xl,yl)}和区域A12{(xl+1,yl+1),...,(xN,yN)},如此循环下去,直至细分的所有区域都满足能够拟合成直线的要求。
以A1{(x1,y1),...,(xN,yN)}区域为例,为了拟合出折现,首先选择区域的起点(x1,y1)和终点(xN,yN),连成一条直线,在这两点间,判断每一个点(xi,yi)|i=2,...,N-1到该直线的距离,找出距离的最大值所对应的那个点(xl,yl),如果这个最大值小于一个让A1区域是直线的阈值的时候,直接对这个区域进行拟合得到直线,否则,就从这一点处将这个区域再一次分割,这样A1就被分开成A11{(x1,y1),...,(xl,yl)}和A12{(xl+1,yl+1),...,(xN,yN)}两个部分,对每个部分再进行上述的距离判断,如此循环下去,直至细分的所有区域都满足能够拟合成直线的要求,这样就把A1这个大区域分段拟合成折线了。
以区域A1为例:
步骤201:设定判断是否是直线的阈值δ;
步骤202:连接该区域的起点和终点形成一条直线L;
步骤203:计算该区域起点和终点之间的每一个点到直线L的距离d;
步骤204:寻找所有计算的距离中的最大值dmax及其对应的点,判断这个距离是否大于阈值δ,如果dmax大于,那么从这点开始将这个区域分成A11和A12;
步骤205:每个新生成的区域,重复计算步骤201~步骤204,直到所有的d都小于设定的阈值δ为止。
进一步的,所述线段表示选择机器人坐标系的原点到线段所在直线的垂足点作为参考,并将这个点坐标转换为世界坐标系下的原点到线段所在直线的垂足点坐标,用这两个参数来当作线段的地标特征点,由于线段是有限的,所以另外四个参数分别表示的是线段的起点和终点在机器人坐标系下的数值,并将其转换到世界坐标系下:βst(Xstl,Ystl),βet(Xetl,Yetl)另外由于机器人在始终的移动,而垂足点在机器人坐标系下的坐标(xrl,yrl)经过机器人坐标系到世界坐标系之间的转换后的点并不是世界系下的原点到线段所在直线的投影坐标(XWl,YWl),故而,这里建立一个中间坐标系ORxy,该坐标系的原点与机器人坐标系的原点重合,x轴y轴指向分别与世界坐标系下的X,Y方向重合,同时令移动机器人到线段所在的投影点在中间坐标系下的坐标为(xl,yl),最后使用以下参数来表示线段:
[XWl,YWl,βst(Xstl,Ystl),βet(Xetl,Yetl)] (7)
其中,XWl,YWl为线段垂足点在世界系下的表示,βst(Xstl,Ystl),βet(Xetl,Yetl)为线段起始点和终点,
求解出(xrl,yrl)、(xl,yl)参数在各自坐标系下的表达式:
其中,xrl,yrl为线段垂足点在机器人坐标系下的表示,
将中间坐标系下的移动机器人投影点坐标转换至世界坐标系下可以求解出世界坐标系原点到线段所在直线的投影点坐标:
进一步的,所述步骤4具体为:
步骤4.1:状态预测既要预测移动机器人的姿态,也要预测环境地标的位置,由于是在静态环境下,假设地标的位置是不发生变化的,根据移动机器人的运动方程,利用运动模型对由t-1时刻的状态和控制量预测t时刻的运动状态,通过前面的模型可以得到:
其中,(x,y,θr)机器人位姿信息中的θr和上式中的θt都是转角,
步骤4.2:地标环境预测:
[XWli(k+1),YWli(k+1)]=Xi(k+1)=Xi(k)=[XWli(k),YWli(k)] (11)
其中,XWli,YWli为第i个线段垂足点在世界系下的表示,
故而系统的状态预测方程可以写为:
其中,f(X,u)为状态方程;有t-1时刻的状态量和控制量,通过状态方程f,来求t时刻的状态,X是状态量,u是控制量;
步骤4.3:量测更新:
每次扫描激光雷达都可以获得一系列连续的点,利用前文提到的数据处理算法可以将这些连续的点拟合成多条直线,进而在利用上一节提到的方法提取出线段的地标,状态中的地标位置用原点到线段所在直线的投影来表示;由此可以建立出系统的量测方程:
其中,hi(X)为观测方程,
通过计算该方程的雅克比矩阵就可以求得方程对于地标i的测量矩阵:
通过数据匹配的方法去判断观测的地标数据是否是之前已经观测到的,如果说观测的数据在之前都被观测过,那么,这些地标就可以当作观测量去更新状态。可以利用EKF的量测更新过程去估计机器人的位姿和周围环境的地标位置;
步骤4.4:状态增广:
当检测到状态中不曾出现过的路标的时候,就认为出现了新的地标,这时候就需把新的地标加入到状态中,根据量测方程可以很容易得到新地标的位置:
利用上式分别计算新地标位置对机器人运动状态和测量值的雅克比矩阵如下:
其中,JR为新观测对机器人位姿的雅可比矩阵,Jz为新观测对测量的雅可比矩阵;
同时,状态增广不仅仅只是增加状态量的维度,还要对协方差矩阵进行增广,就是在原有的协方差基础上增加图中阴影部分的内容:如图8所示,根据状态变量的协方差和测量误差矩阵可以利用刚刚求得的雅克比矩阵更新协方差:
其中,PRN为机器人和扩充新地标协方差矩阵,PNN为扩充地标协方差矩阵,PNL为扩充新地标和之前地标协方差矩阵,PLN为之前地标和扩充地标方差矩阵,Jz为新观测对测量的雅可比矩阵,PRR为机器人协方差矩阵,PLN与PNL互为逆运算;
步骤4.5:更新系统的状态和协方差的初始值,为步骤5的基础。
实施例2
对在结构化环境中的移动机器人完成实验,设置机器人的运动速度为0.3米每秒。
实验中让机器人运行在一个矩形框架中,框架里头带有另一个斜矩形,构成一个结构化的环境。实验中激光雷达的扫描距离范围是30米,扫描的角度范围是180°,分辨率是0.5°,那么激光雷达每次扫描能够得到361个点,实验中以三角形表示机器人的位置,三角形的指向表示机器人的方位,因为激光雷达的坐标中心与机器人坐标系中心重合,所以激光雷达测出的数据就可以直接反映出机器人的坐标中心到探测点的距离和角度,如图11所示。
SLAM的问题关键在于能否精确定位,机器人利用自身传感器对自己的位置进行准确定位,所以有必要对定位进行模拟,实验中为了能够很好地形成对比,同时绘制出了真实的运动情况和经过EKF-SLAM算法后估计的运动情况,进行实时的比对,如图12所示。
SLAM的另一个重要过程就是绘制地图,因为本次实验是基于线段特征进来进行的,所以以实时绘制出线段地图为目的,进行程序的设计,并考察最终的线段匹配的可靠性。为了能够更好的看出实验效果,选择将绘制地图与真实地图进行实时比对,并选取其中某些时间作为比较,如图13所示。
如图15所示,可以看到机器人的定位精度在5%以内。
OWXWYW为世界坐标系、OWxRyR为机器人坐标系、ds/ρ为激光雷达点云距离信息、θs/α为激光雷达点云角度信息、X(i),Y(i)为激光点在世界系坐标的表示。
Claims (5)
1.一种基于垂足点线特征结合的EKF-SLAM算法的机器人定位与构图方法,其特征在于,所述算法包括以下步骤:
步骤1:将激光雷达获取的点云数据输入到区域分割模块进行区域划分,使得激光雷达的点云能够分割开;
步骤2:对分割开的区域里的点云进行线段特征的提取,作为EKF算法的观测值;
步骤3:将观测值进行数据融合处理,进行一个特征的匹配;
步骤4:里程计的信息输入作为EKF的预测阶段,而匹配之后的数据信息作为EKF的更新阶段,完成一次完整的预测更新;
步骤5:输出机器人的位姿和地图的信息。
2.根据权利要求1所述的一种基于垂足点线特征结合的EKF-SLAM算法的机器人定位与构图方法,其特征在于,所述步骤1点云数据输入到区域分割模块进行区域划分具体为:激光雷达在每次扫描的过程中会提供关于障碍物的距离信息ds=[ds-90°,...,ds90°]和角度信息θs=[-90°,...,90°];但是由于激光雷达获得的点都是在极坐标系下表示的,为了能够描绘出环境的位置,需要将扫描的点转换到世界坐标系下进行拟合;对于每个扫描点mi(ds(i),θs(i)),将其映射到世界坐标系下可表示为:
X(i)=ds(i)*cos(θs(i)+θr)+xr (1)
Y(i)=ds(i)*sin(θs(i)+θr)+yr (2)
假设激光雷达每次可以获得到的数据点的个数为n,并以这n个点作为一个初始区域,对这个区域的数据进行处理,采用距离分割的方法,从第一个点开始,首先通过欧式距离判断相邻两个点的距离Dj,再设定一个阈值δ,判断Dj和δ的大小,确定是否从这一点分开;当Dj大于δ时,确定点(Xj,Yj)是区域的一个分割点,以此点将数据划分成两个部分,可以得到第一个区域A1{(Xi,Yi)|i=1,2,...,j}和剩下的待分割区域,以此类推,假设得到N个互相不连通的区域{A1,A2,...,AN}。
3.根据权利要求1所述的一种基于垂足点线特征结合的EKF-SLAM算法的机器人定位与构图方法,其特征在于,所述步骤2线段特征的提取,在区域分割的基础之上使用迭代端点的拟合算法,具体为:对于每一次激光雷达扫描获得的激光点云,通过阈值的设定对得到的扫描点进行区域A1{(x1,y1),...,(xN,yN)}的分割,将这个区域分解成区域A11{(x1,y1),...,(xl,yl)}和区域A12{(xl+1,yl+1),...,(xN,yN)},如此循环下去,直至细分的所有区域都满足能够拟合成直线的要求。
4.根据权利要求3所述的一种基于垂足点线特征结合的EKF-SLAM算法的机器人定位与构图方法,其特征在于,所述线段表示选择机器人坐标系的原点到线段所在直线的垂足点作为参考,并将这个点坐标转换为世界坐标系下的原点到线段所在直线的垂足点坐标,用这两个参数来当作线段的地标特征点,由于线段是有限的,所以另外四个参数分别表示的是线段的起点和终点在机器人坐标系下的数值,并将其转换到世界坐标系下:βst(Xstl,Ystl),βet(Xetl,Yetl)另外由于机器人在始终的移动,而垂足点在机器人坐标系下的坐标(xrl,yrl)经过机器人坐标系到世界坐标系之间的转换后的点并不是世界系下的原点到线段所在直线的投影坐标(XWl,YWl),故而,这里建立一个中间坐标系ORxy,该坐标系的原点与机器人坐标系的原点重合,x轴y轴指向分别与世界坐标系下的X,Y方向重合,同时令移动机器人到线段所在的投影点在中间坐标系下的坐标为(xl,yl),最后使用以下参数来表示线段:
[XWl,YWl,βst(Xstl,Ystl),βet(Xetl,Yetl)] (7)
求解出(xrl,yrl)、(xl,yl)参数在各自坐标系下的表达式:
将中间坐标系下的移动机器人投影点坐标转换至世界坐标系下可以求解出世界坐标系原点到线段所在直线的投影点坐标:
5.根据权利要求1所述的一种基于垂足点线特征结合的EKF-SLAM算法的机器人定位与构图方法,其特征在于,所述步骤4具体为:
步骤4.1:假设地标的位置是不发生变化的,根据移动机器人的运动方程,利用运动模型对由t-1时刻的状态和控制量预测t时刻的运动状态,通过前面的模型可以得到:
步骤4.2:地标环境预测:
[XWli(k+1),YWli(k+1)]=Xi(k+1)=Xi(k)=[XWli(k),YWli(k)] (11)
故而系统的状态预测方程可以写为:
步骤4.3:量测更新:
每次扫描激光雷达都可以获得一系列连续的点,利用前文提到的数据处理算法可以将这些连续的点拟合成多条直线,进而在利用上一节提到的方法提取出线段的地标,状态中的地标位置用原点到线段所在直线的投影来表示;
步骤4.4:状态增广:
当检测到状态中不曾出现过的路标的时候,就认为出现了新的地标,这时需把新的地标加入到状态中,根据量测方程得到新地标的位置;
步骤4.5:更新系统的状态和协方差的初始值。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911149587.1A CN110866927B (zh) | 2019-11-21 | 2019-11-21 | 一种基于垂足点线特征结合的ekf-slam算法的机器人定位与构图方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911149587.1A CN110866927B (zh) | 2019-11-21 | 2019-11-21 | 一种基于垂足点线特征结合的ekf-slam算法的机器人定位与构图方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110866927A true CN110866927A (zh) | 2020-03-06 |
CN110866927B CN110866927B (zh) | 2021-07-20 |
Family
ID=69655653
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911149587.1A Active CN110866927B (zh) | 2019-11-21 | 2019-11-21 | 一种基于垂足点线特征结合的ekf-slam算法的机器人定位与构图方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110866927B (zh) |
Cited By (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111551184A (zh) * | 2020-03-27 | 2020-08-18 | 上海大学 | 一种移动机器人slam的地图优化方法及系统 |
CN111811499A (zh) * | 2020-07-13 | 2020-10-23 | 上海电机学院 | 机器人多传感器混合定位方法 |
CN111856499A (zh) * | 2020-07-30 | 2020-10-30 | 浙江大华技术股份有限公司 | 基于激光雷达的地图构建方法和装置 |
CN112052300A (zh) * | 2020-08-05 | 2020-12-08 | 浙江大华技术股份有限公司 | 一种slam后端处理方法、装置和计算机可读存储介质 |
CN112327325A (zh) * | 2020-09-16 | 2021-02-05 | 安徽意欧斯物流机器人有限公司 | 一种基于特征路标的提升2d-slam精度和稳定性的方法 |
CN112394047A (zh) * | 2020-09-21 | 2021-02-23 | 北京龙德时代技术服务有限公司 | 基于激光遥感的煤矿瓦斯巡检机器人及巡检方法 |
CN112543938A (zh) * | 2020-09-29 | 2021-03-23 | 华为技术有限公司 | 占据栅格地图的生成方法和装置 |
CN112577500A (zh) * | 2020-11-27 | 2021-03-30 | 北京迈格威科技有限公司 | 定位与地图构建方法、装置、机器人及计算机存储介质 |
CN112904358A (zh) * | 2021-01-21 | 2021-06-04 | 中国人民解放军军事科学院国防科技创新研究院 | 基于几何信息的激光定位方法 |
CN113190002A (zh) * | 2021-04-25 | 2021-07-30 | 上海工程技术大学 | 一种高铁箱梁巡检机器人实现自动巡检的方法 |
CN113281775A (zh) * | 2021-05-26 | 2021-08-20 | 珠海市一微半导体有限公司 | 一种基于激光扫描信息的充电座定位方法、芯片及机器人 |
CN113884093A (zh) * | 2020-07-02 | 2022-01-04 | 苏州艾吉威机器人有限公司 | Agv建图和定位的方法、系统、装置及计算机可读存储介质 |
Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20120106828A1 (en) * | 2010-11-03 | 2012-05-03 | Samsung Electronics Co., Ltd | Mobile robot and simultaneous localization and map building method thereof |
CN103278159A (zh) * | 2013-05-23 | 2013-09-04 | 清华大学 | 机载2d激光测距机获取3d点云的方法 |
CN103926912A (zh) * | 2014-05-07 | 2014-07-16 | 桂林赛普电子科技有限公司 | 一种基于家庭服务机器人的智能家庭监控系统 |
CN104062973A (zh) * | 2014-06-23 | 2014-09-24 | 西北工业大学 | 一种基于图像标志物识别的移动机器人slam方法 |
CN105333869A (zh) * | 2015-11-04 | 2016-02-17 | 天津津航计算技术研究所 | 一种基于自适应ekf的无人侦察机同步定位与构图方法 |
CN105556329A (zh) * | 2013-09-27 | 2016-05-04 | 高通股份有限公司 | 混合照片导航及地图绘制 |
CN106444757A (zh) * | 2016-09-27 | 2017-02-22 | 成都普诺思博科技有限公司 | 基于直线特征地图的ekf‑slam算法 |
CN109540126A (zh) * | 2018-12-03 | 2019-03-29 | 哈尔滨工业大学 | 一种基于光流法的惯性视觉组合导航方法 |
CN110119705A (zh) * | 2019-05-09 | 2019-08-13 | 哈尔滨工程大学 | 一种机器人基于改进蚁群优化的线特征数据关联方法 |
CN110243380A (zh) * | 2019-06-26 | 2019-09-17 | 华中科技大学 | 一种基于多传感器数据与角度特征识别的地图匹配方法 |
-
2019
- 2019-11-21 CN CN201911149587.1A patent/CN110866927B/zh active Active
Patent Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20120106828A1 (en) * | 2010-11-03 | 2012-05-03 | Samsung Electronics Co., Ltd | Mobile robot and simultaneous localization and map building method thereof |
CN103278159A (zh) * | 2013-05-23 | 2013-09-04 | 清华大学 | 机载2d激光测距机获取3d点云的方法 |
CN105556329A (zh) * | 2013-09-27 | 2016-05-04 | 高通股份有限公司 | 混合照片导航及地图绘制 |
CN103926912A (zh) * | 2014-05-07 | 2014-07-16 | 桂林赛普电子科技有限公司 | 一种基于家庭服务机器人的智能家庭监控系统 |
CN104062973A (zh) * | 2014-06-23 | 2014-09-24 | 西北工业大学 | 一种基于图像标志物识别的移动机器人slam方法 |
CN105333869A (zh) * | 2015-11-04 | 2016-02-17 | 天津津航计算技术研究所 | 一种基于自适应ekf的无人侦察机同步定位与构图方法 |
CN106444757A (zh) * | 2016-09-27 | 2017-02-22 | 成都普诺思博科技有限公司 | 基于直线特征地图的ekf‑slam算法 |
CN109540126A (zh) * | 2018-12-03 | 2019-03-29 | 哈尔滨工业大学 | 一种基于光流法的惯性视觉组合导航方法 |
CN110119705A (zh) * | 2019-05-09 | 2019-08-13 | 哈尔滨工程大学 | 一种机器人基于改进蚁群优化的线特征数据关联方法 |
CN110243380A (zh) * | 2019-06-26 | 2019-09-17 | 华中科技大学 | 一种基于多传感器数据与角度特征识别的地图匹配方法 |
Non-Patent Citations (3)
Title |
---|
CHOI JW ET AL: "《Robust Sonar Feature Detection for the SLAM of Mobile Robot》", 《2005 IEEE》 * |
伍明: "《未知环境下移动机器人目标跟踪理论与方法》", 31 January 2018, 西北工业大学出版社 * |
张振杰: "《无人机视觉导航位姿估计技术研究与实践》", 《中国博士学位论文全文数据库工程科技Ⅱ辑》 * |
Cited By (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111551184B (zh) * | 2020-03-27 | 2021-11-26 | 上海大学 | 一种移动机器人slam的地图优化方法及系统 |
CN111551184A (zh) * | 2020-03-27 | 2020-08-18 | 上海大学 | 一种移动机器人slam的地图优化方法及系统 |
WO2022000882A1 (zh) * | 2020-07-02 | 2022-01-06 | 苏州艾吉威机器人有限公司 | Agv建图和定位的方法、系统、装置及计算机可读存储介质 |
CN113884093A (zh) * | 2020-07-02 | 2022-01-04 | 苏州艾吉威机器人有限公司 | Agv建图和定位的方法、系统、装置及计算机可读存储介质 |
CN111811499A (zh) * | 2020-07-13 | 2020-10-23 | 上海电机学院 | 机器人多传感器混合定位方法 |
CN111811499B (zh) * | 2020-07-13 | 2024-01-19 | 上海电机学院 | 机器人多传感器混合定位方法 |
CN111856499A (zh) * | 2020-07-30 | 2020-10-30 | 浙江大华技术股份有限公司 | 基于激光雷达的地图构建方法和装置 |
CN112052300A (zh) * | 2020-08-05 | 2020-12-08 | 浙江大华技术股份有限公司 | 一种slam后端处理方法、装置和计算机可读存储介质 |
CN112327325A (zh) * | 2020-09-16 | 2021-02-05 | 安徽意欧斯物流机器人有限公司 | 一种基于特征路标的提升2d-slam精度和稳定性的方法 |
CN112327325B (zh) * | 2020-09-16 | 2024-05-14 | 安歌智慧科技(上海)有限公司 | 一种基于特征路标的提升2d-slam精度和稳定性的方法 |
CN112394047A (zh) * | 2020-09-21 | 2021-02-23 | 北京龙德时代技术服务有限公司 | 基于激光遥感的煤矿瓦斯巡检机器人及巡检方法 |
CN112543938A (zh) * | 2020-09-29 | 2021-03-23 | 华为技术有限公司 | 占据栅格地图的生成方法和装置 |
CN112543938B (zh) * | 2020-09-29 | 2022-02-08 | 华为技术有限公司 | 占据栅格地图的生成方法和装置 |
CN112577500A (zh) * | 2020-11-27 | 2021-03-30 | 北京迈格威科技有限公司 | 定位与地图构建方法、装置、机器人及计算机存储介质 |
WO2022110767A1 (zh) * | 2020-11-27 | 2022-06-02 | 北京迈格威科技有限公司 | 定位与地图构建方法、装置、机器人及计算机存储介质 |
CN112904358A (zh) * | 2021-01-21 | 2021-06-04 | 中国人民解放军军事科学院国防科技创新研究院 | 基于几何信息的激光定位方法 |
CN113190002A (zh) * | 2021-04-25 | 2021-07-30 | 上海工程技术大学 | 一种高铁箱梁巡检机器人实现自动巡检的方法 |
CN113281775A (zh) * | 2021-05-26 | 2021-08-20 | 珠海市一微半导体有限公司 | 一种基于激光扫描信息的充电座定位方法、芯片及机器人 |
Also Published As
Publication number | Publication date |
---|---|
CN110866927B (zh) | 2021-07-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110866927B (zh) | 一种基于垂足点线特征结合的ekf-slam算法的机器人定位与构图方法 | |
CN110927740B (zh) | 一种移动机器人定位方法 | |
CN107239076B (zh) | 基于虚拟扫描与测距匹配的agv激光slam方法 | |
Nieto et al. | Recursive scan-matching SLAM | |
CN109325979B (zh) | 基于深度学习的机器人回环检测方法 | |
CN112882053B (zh) | 一种主动标定激光雷达和编码器外参的方法 | |
CN111982114B (zh) | 一种采用imu数据融合估计三维位姿的救援机器人 | |
CN111578926A (zh) | 一种基于自动驾驶平台的地图生成与导航避障的方法 | |
CN115639823A (zh) | 崎岖起伏地形下机器人地形感知与移动控制方法及系统 | |
CN115540850A (zh) | 一种激光雷达与加速度传感器结合的无人车建图方法 | |
CN116030130A (zh) | 一种动态环境下的混合语义slam方法 | |
CN116429116A (zh) | 一种机器人定位方法及设备 | |
Chang et al. | Robust accurate LiDAR-GNSS/IMU self-calibration based on iterative refinement | |
CN117075158A (zh) | 基于激光雷达的无人变形运动平台的位姿估计方法及系统 | |
CN115950414A (zh) | 一种不同传感器数据的自适应多重融合slam方法 | |
Zeghmi et al. | A Kalman-particle hybrid filter for improved localization of AGV in indoor environment | |
Youssefi et al. | Visual and light detection and ranging-based simultaneous localization and mapping for self-driving cars | |
CN114459483B (zh) | 基于机器人导航用地标导航地图构建与应用方法、系统 | |
Xue et al. | Real-time 3D grid map building for autonomous driving in dynamic environment | |
CN114419118A (zh) | 三维点云配准方法、移动设备及存储介质 | |
Danping et al. | Simultaneous localization and mapping based on Lidar | |
Yang et al. | SLAM self-cruise vehicle based on ROS platform | |
Cui et al. | Simulation and Implementation of Slam Drawing Based on Ros Wheeled Mobile Robot | |
Juang | Humanoid robot runs maze mode using depth-first traversal algorithm | |
US20240192695A1 (en) | Anchoring based transformation for aligning sensor data of a robot with a site model |
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 |