本发明涉及室内定位算法领域,尤其涉及一种基于激光的室内半结构化环境特征定位方法。
背景技术:
1、随着科技的发展,定位技术得到了快速发展与普及,从航空航天进入到千家万户。如果按照环境来划分,定位技术可分为室外定位技术与室内定位技术两大类。室外定位技术多用于航空航天和自动驾驶等相关领域,室内定位技术多用于扫地机器人、仓储机器人、送餐机器人以及医疗运输机器人等相关领域。
2、室内定位技术主要包含了激光定位、视觉定位、无线定位和循迹定位等技术领域。不同的公司根据自己的业务需求和使用场景搭配组合其中的一种或多种技术方案。
3、基于激光的定位技术,由于激光的特性,不易受环境光照影响,可以稳定运行,单次测量上具有较高的精度,能够提供精确的环境信息,但是由于信息量较少,较为缺乏回环检测的能力。
4、基于视觉的定位技术,可以充分利用丰富的纹理信息,具有超强的场景辨识能力。可以较为容易地被用来跟踪和预测场景中的动态目标,但是容易受到光照影响,计算量相对较大,对cpu、gpu处理器的要求比较高。
5、基于无线的定位技术,典型代表如uwb定位技术,由于不同企业研发的uwb定位系统精度略有差异,普遍的精度是10cm,其优势在于工作时间短,功耗低。不足之处在于需要提前部署定位网络,成本高,而且不能有遮挡无线信号的障碍物,一旦被遮挡,精度会受到很大影响。
6、目前国家大力提倡智能建造,建筑环境纹理较少不宜用视觉的方式,更不适合提前部署昂贵的定位网络,基于激光的定位技术是比较好的选择。由于室内建筑具有典型的半结构化环境特征,所有本发明提出了一种基于激光的半结构化环境定位方法,主要应用于室内建筑场景,可提供稳定、高精度的定位信息。
技术实现思路
1、本发明的目的是克服背景技术中存在的上述缺陷,提供一种基于激光的室内半结构化环境特征定位方法,以解决当前现有的激光定位算法在半结构化的建筑环境中,定位精度不足以及稳定性不足等问题。
2、本发明为解决上述技术问题采用以下技术方案:
3、一种基于激光的室内半结构化环境特征定位方法,具体包括以下步骤:
4、步骤a:在对环境直线提取之前,读取激光雷达原始数据,将其转换为点云坐标;
5、步骤b:提取点云数据的角点,把原始点云数据根据角点分成多个共线族;
6、步骤c:对每个共线族,计算误差最小的拟合直线方程参数;
7、步骤d:对点云数据提取到的直线与特征地图进行匹配,计算相对位姿;
8、步骤e:把新获取到的直线插入特征地图,与原有特征地图融合更新。
9、进一步的,所述步骤b中,寻找角度的方法如下:
10、提取点云数据里面每一个点的前后五个点,拟合直线,计算每个点云前后五个点所拟合直线的夹角,进而可以近似出该点的曲率;曲率大于阈值的点定义为角点;
11、首先,假设通过5个点拟合出来的直线方程为:
12、y=ax+c (1)
13、根据最小二乘的原理,分别计算拟合直线的参数a和c:
14、
15、
16、根据上述公式,分别求得点云前面5个点的拟合直线l1的参数a1和c1,后面5个连续点的拟合直线l2参数a2,c2;
17、计算l1的角度θ1和l2的角度θ2:
18、θ1=arctan(a1) (4)
19、θ2=arctan(a2) (5)
20、点云前后两点直线角度求差值,当θ1与θ2的角度差值大于所设定的阈值时,说明该点云的角度变化率较大,设置该点为角点;
21、对于相邻的角点,取角度变化率最大的点作为角点。
22、进一步的,所述步骤c中,采用了最小二乘的方法对共线族点云进行直线拟合,具体包含以下步骤:
23、两个角点之间的点云个数为n,如果n大于设定的拟合直线点云数量最小阈值,根据角点,计算直线方程l,两个角点之间的所有点云数据到直线l的距离,如果大于距离阈值,则不是一条直线,如果小于阈值,则为了直线方程的参数更加精确,根据公式(2)和公式(3)重新计算n个点云的拟合直线方程,角点坐标为直线的两个端点,记为线段li。
24、进一步的,所述步骤d中,使用了基于直线的特征地图,提取雷达扫描点云数据里面的直线与特征地图中的直线进行匹配,计算相对位姿,具体包含以下步骤:
25、提取最新一帧点云中的所有线段,与特征地图做比较,在地图中找到与新的点云数据所提取线段距离最近的特征地图线段,如果两个线段的距离和角度同时小于所设定的阈值,认为他们是一对对应的线段,找到所有对应的线段后,计算对应直线之间相差的角度,然后累加求角度差的均值,就是雷达扫描数据与地图匹配的旋转角度,然后把所有的直线旋转这个角度,再计算新点云数据和特征地图中的相邻线段的交点,作为对应匹配点,求取所有对应匹配点之间的x方向差值和y方向差值,累加求平均值,就是新点云数据与特征地图之间的位姿变换,进而得出机器的位姿。
26、与现有技术相比,本发明具有如下有益效果:
27、(1)本发明提供的一种基于激光的室内半结构化环境特征定位方法,通过提取半结构化环境中墙体的直线特征,利用多个共线的激光点所拟合出来的直线进行定位,相比于单个激光点,直线更接近真实环境,从而有效提高了定位精度。
28、(2)本发明提供的一种基于激光的室内半结构化环境特征定位方法,通过对雷达数据提取墙体特征,可以有效解决局部激光点跳动对定位稳定性的影响。由于半结构化环境中动态移动物体较少,墙体固定性较高,因此墙体特征具有较好的稳定性,可以提供更可靠的定位信息。
29、(3)本发明提供的一种基于激光的室内半结构化环境特征定位方法,根据实验结果表明,该方法具有较高的稳定性和精度,能够有效解决当前现有的激光定位算法在半结构化建筑环境中定位精度不足以及稳定性不足等问题。
1.一种基于激光的室内半结构化环境特征定位方法,其特征在于,具体包括以下步骤:
2.根据权利要求1所述的一种基于激光的室内半结构化环境特征定位方法,其特征在于,所述步骤b中,寻找角度的方法如下:
3.根据权利要求1所述的一种基于激光的室内半结构化环境特征定位方法,其特征在于,所述步骤c中,采用了最小二乘的方法对共线族点云进行直线拟合,具体包含以下步骤:
4.根据权利要求1所述的一种基于激光的室内半结构化环境特征定位方法,其特征在于,所述步骤d中,使用了基于直线的特征地图,提取雷达扫描点云数据里面的直线与特征地图中的直线进行匹配,计算相对位姿,具体包含以下步骤: