【技术实现步骤摘要】
本专利技术属于slam、机器人运动控制，涉及一种轻量级slam方法，尤其涉及一种基于激光雷达的轻量级slam方法。
技术介绍
1、同步定位与建图(simultaneous localization and mapping，slam)是自主移动机器人和自动驾驶的关键技术之一。根据slam系统使用的主要传感器的不同，slam技术可以分为激光slam、视觉slam和多源融合slam等等。其中激光slam通过激光雷达感知环境，可以直接获取环境的三维信息，具有定位精确、系统可靠性高的特点，广泛应用于室内导航、自动驾驶等领域。激光slam根据前端里程计算法的原理，可以分为基于迭代最近点(iterative closest point,icp)的slam算法、基于高斯混合模型(gaussian mixturemodels，gmm)的slam算法以及基于图优化的slam算法。其中基于icp的slam算法精度较高，但运算量也比较大，无法部署在微处理器等低成本、算力受限的嵌入式设备上，限制了slam在低成本、小型化机器人系统的应用。
技术实现思路
1、本专利技术的目的在于针对目前slam算法运算量大、占用内存大，无法部署在微处理器上的问题，提出了一种轻量级slam方法。
2、为了实现上述目的，本专利技术采取如下技术方案。
3、一种轻量级slam方法，依托于获取的二维点云；包括如下步骤：
4、step1、根据曲率提取特征并计算每个特征的特征参数，具体为：
6、step11所述二维点云是传感器极坐标系下的点集；step11具体为：取待求数据点前后各nc个数据点，将这些数据点极径与待求数据点极径的差值加和除以待求数据点的极径，再除以这些数据点的极角范围，得到待求数据点的近似曲率；所述nc大于等于1小于等于20。
7、step12、对所有数据点近似曲率取绝对值再进行非极大值抑制，筛选出极大和极小值作为备选数据点；
8、step13、依据设定的线特征识别阈值、角特征识别阈值和异常点识别阈值从备选数据点中提取特征；
9、step14、计算每个特征的特征参数；
10、所述特征参数，包括哈希值、邻近哈希值和邻近距离；
11、step2、将特征与地图特征进行匹配，得到匹配好的特征对；
12、所述特征即为step1提取的特征称为扫描特征；所述地图特征为特征地图中的特征；
13、对于每个扫描特征，遍历其哈希值对应哈希桶中的所有地图特征计算并得到该哈希桶的最小特征距离，记为第一距离；判断该遍历操作是否满足如下条件：1)邻近哈希值等于哈希值；2)第一距离小于邻近距离；3)邻近距离大于搜索半径；若满足1)或2)或3)者中之一，则找出第一距离对应的地图特征与该扫描特征一起即为匹配好的特征对；若1)2)3)均不满足，则遍历邻近哈希值对应哈希桶中的所有地图特征并得到该哈希桶的最小特征距离，记为第二距离；找出第一距离和第二距离的较小值对应的地图特征与该扫描特征一起即为匹配好的特征对；
14、step3、计算每个特征对的误差函数并求和，得到构造好的优化函数，再依据该优化函数通过牛顿迭代法更新位姿；
15、step4、收敛判断：判断优化函数的雅可比矩阵的l2范数是否小于设定的收敛阈值，若小于跳至step5，否则跳至step14；
16、step5、更新失配扫描特征、更新特征对以及更新失配地图特征。
17、step13所述特征包括线特征和角特征；曲率绝对值小于线特征识别阈值的曲率极小值点为线特征；曲率绝对值大于角特征识别阈值，但曲率的负数不大于异常点识别阈值的曲率极大值点为角特征。
18、step14所述哈希值采用局部敏感哈希函数计算，包括线特征的哈希值和角特征的哈希值；
19、step14所述线特征的哈希值通过将地图坐标的和除以倍哈希桶宽度加偏置，经过向下取整和限幅得到。
20、step14所述角特征哈希值为将线特征哈希值再加整数hmax；所述hmax是线特征哈希值上限。
21、step14所述邻近哈希值是除了该特征本身的哈希值外最接近的哈希值；所述邻近距离是该特征与邻近哈希值的距离。
22、step14所述特征参数，还包括地图坐标、特征方向；所述地图坐标的计算方式为：将传感器极坐标系下的特征点通过坐标变换得到地图坐标系下的直角坐标；所述特征方向，包括线特征的特征方向和角特征的特征方向；所述线特征的特征方向为该特征前后各nc个数据点组成近似直线的方向；所述角特征的特征方向为该特征前后各nc个数据点与该特征连线的向量和的方向。
23、step2所述特征地图是一个哈希表，被分为2hmax-1个哈希桶，每个哈希桶对应一个哈希值且被表示为一个链表；每个链表包括表头和表节点；所述表头为该哈希桶的哈希值，表节点储存地图特征的地图坐标、特征方向和权重。
24、step3所述特征对的误差函数，包括线特征对的误差函数和角特征对的误差函数；所述线特征对的误差函数的计算方式为扫描特征到地图特征所在直线的距离的平方与两个特征的特征方向差值的平方再做加权和；所述角特征对误差函数的计算方式为扫描特征到地图特征的特征距离的平方。
25、step3所述采用牛顿迭代法更新位姿，具体为：更新后位姿等于更新前位姿加上位姿增量，所述位姿增量为优化函数的雅可比矩阵与逆海塞矩阵的矩阵乘。
26、step5所述更新失配扫描特征，具体为：将失配扫描特征根据其哈希值存入特征地图哈希表中对应的哈希桶并将权重赋值为k；所述k的取值范围为1到20。
27、step5所述更新特征对，具体为：将特征点对的地图特征坐标与扫描特征坐标加权平均作为新的地图特征坐标，并将其权重加δk；所述δk的取值范围为(0-1]；地图坐标更新后，重新计算其哈希值，并根据哈希值重新将更新后的地图特征存入特征地图哈希表中对应的哈希桶。
28、step5所述更新失配地图特征，具体为：将失配地图特征映射到传感器极坐标系；查找极角与失配地图特征最接近的传感器数据点，并判断该传感器数据点与失配地图特征极径的大小；若失配地图特征的极径小于传感器数据点且极径的差值大于传感器测距误差，说明该失配地图特征非稳定存在；因此将该地图特征的权重减δk；若该地图特征的权重小于0，则将其剔除出特征地图哈希表。
29、有益效果
30、1、所述方法只需要激光雷达作为传感器，不需要其他传感器辅助，降低了传感器的成本；
31、2、所述方法通过近似曲率计算公式在极坐标系下获得数据点的曲率，避免了极坐标系到直角坐标系变换的庞大计算量，同时采用非极大值抑制的方法显著降低特征的数量，从而节省运算开销；
32、3、所述方法构建周围环境的特征地图，避免了稠密栅格地图的大量信息冗余，节省了存储本文档来自技高网...
【技术保护点】
1.一种轻量级SLAM方法，依托于获取的二维点云；其特征在于，包括：
2.根据权利要求1所述的一种轻量级SLAM方法，其特征在于，所述哈希值采用局部敏感哈希函数计算，包括线特征的哈希值和角特征的哈希值；
3.根据权利要求1所述的一种轻量级SLAM方法，其特征在于，Step13所述特征包括线特征和角特征；曲率绝对值小于线特征识别阈值的曲率极小值点为线特征；曲率绝对值大于角特征识别阈值，但曲率的负数不大于异常点识别阈值的曲率极大值点为角特征。
4.根据权利要求1或2所述的一种轻量级SLAM方法，其特征在于，Step14所述特征参数，还包括地图坐标、特征方向；所述地图坐标的计算方式为：将传感器极坐标系下的特征点通过坐标变换得到地图坐标系下的直角坐标；所述特征方向，包括线特征的特征方向和角特征的特征方向；所述线特征的特征方向为该特征前后各NC个数据点组成近似直线的方向；所述角特征的特征方向为该特征前后各NC个数据点与该特征连线的向量和的方向。
5.根据权利要求1所述的一种轻量级SLAM方法，其特征在于，Step2所述特征地图是一个哈希表，被
6.根据权利要求1所述的一种轻量级SLAM方法，其特征在于，Step3所述特征对的误差函数，包括线特征对的误差函数和角特征对的误差函数；所述线特征对的误差函数的计算方式为扫描特征到地图特征所在直线的距离的平方与两个特征的特征方向差值的平方再做加权和；所述角特征对误差函数的计算方式为扫描特征到地图特征的特征距离的平方。
7.根据权利要求1所述的一种轻量级SLAM方法，其特征在于，所述采用牛顿迭代法更新位姿，具体为：更新后位姿等于更新前位姿加上位姿增量，所述位姿增量为优化函数的雅可比矩阵与逆海塞矩阵的矩阵乘。
8.根据权利要求1所述的一种轻量级SLAM方法，其特征在于，Step5所述更新失配扫描特征，具体为：将失配扫描特征根据其哈希值存入特征地图哈希表中对应的哈希桶并将权重赋值为k；所述k的取值范围为1到20。
9.根据权利要求1或8所述的一种轻量级SLAM方法，其特征在于，Step5所述更新特征对，具体为：将特征点对的地图特征坐标与扫描特征坐标加权平均作为新的地图特征坐标，并将其权重加Δk；所述Δk的取值范围为(0-1]；地图坐标更新后，重新计算其哈希值，并根据哈希值重新将更新后的地图特征存入特征地图哈希表中对应的哈希桶。
10.根据权利要求1或9所述的一种轻量级SLAM方法，其特征在于，Step5所述更新失配地图特征，具体为：将失配地图特征映射到传感器极坐标系；查找极角与失配地图特征最接近的传感器数据点，并判断该传感器数据点与失配地图特征极径的大小；若失配地图特征的极径小于传感器数据点且极径的差值大于传感器测距误差，说明该失配地图特征非稳定存在；因此将该地图特征的权重减Δk；若该地图特征的权重小于0，则将其剔除出特征地图哈希表。
...【技术特征摘要】
1.一种轻量级slam方法，依托于获取的二维点云；其特征在于，包括：
2.根据权利要求1所述的一种轻量级slam方法，其特征在于，所述哈希值采用局部敏感哈希函数计算，包括线特征的哈希值和角特征的哈希值；
3.根据权利要求1所述的一种轻量级slam方法，其特征在于，step13所述特征包括线特征和角特征；曲率绝对值小于线特征识别阈值的曲率极小值点为线特征；曲率绝对值大于角特征识别阈值，但曲率的负数不大于异常点识别阈值的曲率极大值点为角特征。
4.根据权利要求1或2所述的一种轻量级slam方法，其特征在于，step14所述特征参数，还包括地图坐标、特征方向；所述地图坐标的计算方式为：将传感器极坐标系下的特征点通过坐标变换得到地图坐标系下的直角坐标；所述特征方向，包括线特征的特征方向和角特征的特征方向；所述线特征的特征方向为该特征前后各nc个数据点组成近似直线的方向；所述角特征的特征方向为该特征前后各nc个数据点与该特征连线的向量和的方向。
5.根据权利要求1所述的一种轻量级slam方法，其特征在于，step2所述特征地图是一个哈希表，被分为2hmax-1个哈希桶，每个哈希桶对应一个哈希值且被表示为一个链表；每个链表包括表头和表节点；所述表头为该哈希桶的哈希值，表节点储存地图特征的地图坐标、特征方向和权重。
6.根据权利要求1所述的一种轻量级slam方法，其特征在于，step3所述特征对的误差函数，包括线特征对的误差函数和角特征对的误差函数；所述线特征对的误差函数的...
【专利技术属性】
技术研发人员：张延军，王卓，周禹彤，卢继华，
申请(专利权)人：北京理工大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。