一种越野环境下的四足机器人激光自主定位建图方法技术

本发明专利技术公开了一种越野环境下的四足机器人激光自主定位建图方法，获取野外环境下的激光雷达原始点云，在原始点云的基础上提取少于100个的三维特征点来进行配准，将备选的三维特征点聚焦为边缘点和平面点，并通过的值来进行边缘点和平面点的提取；得到上述三维特征点以后，对获得的两帧点云进行配准；相关点云之间建立几何关系，进行巡视器的定位工作，对激光雷达运动产生的畸变进行运动补偿；通过L

【技术实现步骤摘要】
一种越野环境下的四足机器人激光自主定位建图方法


[0001]本专利技术属于无人系统
，特别涉及一种越野环境下的四足机器人激光自主定位建图方法以及存储介质和电子设备。

技术介绍

[0002]随着机器人同时定位与地图构建(Simultaneous Localization and Mapping，SLAM)技术的成熟，越来越多的学者们也致力于将SLAM技术应用至野外无人系统巡视任务中，如N.B.Joseph等人针对沙漠巡视器测试了数种SLAM算法，并提出利用SLAM算法所生成的三维地貌模型设计巡视器可达域；Affan Shaukat等人利用巡视器双目相机检测到的障碍构成特征组合，并在EKF
‑
SLAM框架下设计了完整的自主定位算法，该方法值得借鉴之处在于将导航路标视为代表障碍的特征点组合，相比传统定位方法提高了特征识别成功率；Mark Woods等人分析与总结了历代火星巡视器的工作模式与环境，针对欧空局未来火星表面巡视任务(ExoMars2.0)设计了基于SLAM技术的火星车沙漠实地自主定位实验，验证了SLAM算法在火星巡视器长时、欠通讯工作条件下的有效性。
[0003]SLAM问题可以简单概括为将机器人放置于一个未知的环境中，如何利用敏感器的测量信息进行自身定位及周围环境描述的问题。Dissanayake等人于上世纪80年代建立了移动机器人导航的最初框架，提出了以Kalman Filter为基础的KF
‑
SLAM导航算法，S.Thrun等人对KF
‑
SLAM算法进行了综述。Montemerlo等人于2002年提出了著名的Fast
‑
SLAM算法，提出了利用粒子滤波(Particle filter)估计机器人状态，扩展卡尔曼滤波估计地图特征这一思想，并参考Rao
‑
Blackwellize估计结构将EKF对地图的估计归属到单个粒子中。Montemerlo与S.Thrun在2003年对FastSLAM算法进一步完善，提出在状态预测过程中引入测量概率密度(Probability Density Function)重新赋权，新的FastSLAM 2.0算法即使在使用1个粒子时也能达到较好的估计效果。近年来随着计算机算力的提升，基于图优化的SLAM方法逐渐成为了研究热点，一些优秀的算法如TreeMap，TORO，HOG_MAN，G2O等方法均取得了优于基于滤波体系的SLAM方法，但这些方法的计算量随时间的增长而显著增大，目前仍处于离线式优化处理，针对此问题，Kaess等人提出了一种增量式的平滑建图方法iSAM(Incremental Smoothing and Mapping)。iSAM在计算非线性优化问题时，对信息矩阵进行QR分解，当新的量测信息到来时，算法应用上一步的矩阵信息增量地对信息矩阵进行更新，避免对所有的相关元素重新进行计算，从而提高算法效率。
[0004]SLAM算法的核心是通过多次特征的重观测(re
‑
visiting)来控制累计误差，能够在精确定位的同时输出周边地图，因此对野外探索导航任务具有潜在应用价值。然而，目前的自主定位建图方法在野外单一环境下缺少鲁棒性，从而导致状态估计错误率较高，且检测精度有待提高。

技术实现思路

[0005]专利技术目的：为了克服以上不足，本专利技术的目的是提供一种越野环境下的四足机器
人激光自主定位建图方法，能够让机器人自主对周围环境进行位姿状态估计定位导航，提高目标检测精度，完成四足机器人在野外环境下的巡视探索任务。
[0006]技术方案：为了实现上述技术效果，本专利技术提供了一种越野环境下的四足机器人激光自主定位建图方法，包括：将实时定位与地图描绘分解为两个线程，其中第一个线程为高频线程，主要以10Hz进行巡视器运动估计，并融合IMU里程信息，第二个线程为低频线程，主要负责局部地图描绘；
[0007]具体的建图方法如下：
[0008]S1：获取野外环境下的激光雷达原始点云，在原始点云的基础上提取少于100个的三维特征点来进行配准，将备选的三维特征点聚焦为边缘点和平面点，并通过c的值来进行边缘点和平面点的提取；
[0009]S2：得到上述三维特征点以后，对获得的两帧点云进行配准；
[0010]S3：在相关点云之间建立几何关系，进行巡视器的定位工作，并对激光雷达运动产生的畸变问题进行运动补偿；
[0011]S4：通过Levenberg
‑
Marquardt算法有效处理冗余参数，并在后端利用Levenberg
‑
Marquardt算法进行非线性优化状态估计，由于Levenberg
‑
Marquardt算法对于过参数化问题不敏感，能够有效处理冗余参数问题，从而大大减小了优化函数陷入局部极小值的情况；
[0012]S5：当累计一定帧数目的点云时便可以进行建图，精确估计激光雷达在世界坐标系的位姿，将点云数据融入到世界地图中，建图采用的算法的运行频率比里程计算法低。
[0013]本专利技术中所述的一种越野环境下的四足机器人激光自主定位建图方法，S1中在原始点云的基础上提取少于100个的三维特征点来进行配准，选取的点较少，能够有效避免类似ICP算法对大规模点云的处理，提高数据精准性的同时，还能够减少工作量；。
[0014]本专利技术所述的越野环境下的四足机器人激光自主定位建图方法，步骤S1中通过c的值来进行边缘点和平面点的提取具体方法如下：
[0015][0016]其中，i是p
k
中的一个点，p
k
为激光扫描仪一次扫描获得的点云，i∈p
k
,S是激光扫描仪在同一线扫描中i附近的连续点的集合，S包含i两侧各一半的点，|S|为点的数量；
[0017]为i和S中点的归一化矢量和的模；
[0018]由于边缘点矢量和的模较大，而平面点矢量和的模较小；
[0019]扫描中的点基于c值进行排序，然后将最大c值和最小c值选择为特征点，其中最大c值为边缘点，最小c值为平面点。
[0020]本专利技术所述的越野环境下的四足机器人激光自主定位建图方法，在S1中通过c的值来进行边缘点和平面点的提取的过程中，当选择点i作为边缘点或平面点时，还需要满足以下条件：
[0021]为了均匀分布特征点，需要将每次扫描的到的数据均匀分为四个区域，每个区域最多包含2个边缘点和4个平面点，所选边缘点或平面点的数量不能超过子区域的最大值，点i周围邻域10个点内的点不能再次被选择，且点i不能在与激光束大致平行的平面上，也不能在被遮挡区域的边界上；
[0022]假设点A位于与激光束成一定角度的曲面片上，B点位于与激光束大致平行的曲面片上，B点为不可靠的激光回波，不选择它作为特征点，实线段是激光可观察到的对象，点A在遮挡区域的边界上，可能会被检测为边缘点，但是，如果从不同角度观看，遮挡区域可能会发生变化并变得可观察，点本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种越野环境下的四足机器人激光自主定位建图方法，其特征在于：包括：将实时定位与地图描绘分解为两个线程，其中第一个线程为高频线程，主要以10Hz进行巡视器运动估计，并融合IMU里程信息，第二个线程为低频线程，主要负责局部地图描绘；具体的建图方法如下：S1：获取野外环境下的激光雷达原始点云，在原始点云的基础上提取少于100个的三维特征点来进行配准，将备选的三维特征点聚焦为边缘点和平面点，并通过c的值来进行边缘点和平面点的提取；S2：得到上述三维特征点以后，对获得的两帧点云进行配准；S3：在相关点云之间建立几何关系，进行巡视器的定位工作，并对激光雷达运动产生的畸变问题进行运动补偿；S4：通过Levenberg
‑
Marquardt算法有效处理冗余参数，并在后端利用Levenberg
‑
Marquardt算法进行非线性优化状态估计；S5：当累计一定帧数目的点云时便可以进行建图，精确估计激光雷达在世界坐标系的位姿，将点云数据融入到世界地图中，建图采用的算法的运行频率比里程计算法低。2.根据权利要求1所述的越野环境下的四足机器人激光自主定位建图方法，其特征在于：步骤S1中通过c的值来进行边缘点和平面点的提取具体方法如下：其中，i是p
k
中的一个点，p
k
为激光扫描仪一次扫描获得的点云，i∈p
k
,S是激光扫描仪在同一线扫描中i附近的连续点的集合，S包含i两侧各一半的点，|S|为点的数量；为i和S中点的归一化矢量和的模；由于边缘点矢量和的模较大，而平面点矢量和的模较小；扫描中的点基于c值进行排序，然后将最大c值和最小c值选择为特征点，其中最大c值为边缘点，最小c值为平面点。3.根据权利要求2所述的越野环境下的四足机器人激光自主定位建图方法，其特征在于：在S1中通过c的值来进行边缘点和平面点的提取的过程中，当选择点i作为边缘点或平面点时，还需要满足以下条件：为了均匀分布特征点，需要将每次扫描的到的数据均匀分为四个区域，每个区域最多包含2个边缘点和4个平面点，所选边缘点或平面点的数量不能超过子区域的最大值，点i周围邻域10个点内的点不能再次被选择，且点i不能在与激光束大致平行的平面上，也不能在被遮挡区域的边界上；假设点A位于与激光束成一定角度的曲面片上，B点位于与激光束大致平行的曲面片上，B点为不可靠的激光回波，不选择它作为特征点，实线段是激光可观察到的对象，点A在遮挡区域的边界上，可能会被检测为边缘点，但是，如果从不同角度观看，遮挡区域可能会发生变化并变得可观察，点A不是显著的边缘点，也不会将其选择为特征点。4.根据权利要求2所述的越野环境下的四足机器人激光自主定位建图方法，其特征在于：步骤S2中两帧点云进行配准的具体方法如下：
1)：t
k
表示第k次扫描开始的时间，t
k+1
为扫描结束的时间，将这一次扫描获得的点云p
k
经过畸变矫正后投影到时间戳t
k+1
上；2)：将投影后的点云记为3)：在第k+1次扫描时将使用和新获得的点云p
k+1
进行配准来估计激光雷达的运动；4)：在第k次扫描结束时，将p
k
重新投影到时间戳t
k+1
，以获得其中p
k
是在第k次扫描期间感知到的点云；5)：第k+1次扫描期间，p
k
和新感知的点云p
k+1
一起用于估计激光雷达的运动；6)：对于p
k+1
，可以从激光雷达点云中提取出特征点，记ε
k+1
和H
k+1
分别为其边缘点和平面点的集合；7)：再找到来自的边缘线作为ε
k+1
中点的对应关系，找到平面作为H
k+1
中点的对应关系；8)：使用当前估计的变换将ε
k+1
和H
k+1
重新投影到扫描起始点的坐标系下，记和为重投影点集，这样就可以得到第k+1帧点云中所有点的位姿变换信息；9)：对于和中的每个点，将在中找到对应的关系，的特征点存储在3D KD
‑
tree中，KD
‑
tree常用作范围搜索和最近邻搜索。5.根据权利要求4所述的越野环境下的四足机器人激光自主定位建图方法，其特征在于：步骤S2中两帧点云进行配准，所述三维边缘特征点配准的具体方法如下：设i为中的一个点，需要找到两点来表示边缘线，首先在中找到最接近i的点，记为j，然后在j相邻线的两次连续扫描找到与i的最近邻的点，记为l，则(j,l)形成i的对应关系，其中最近邻点的选取使用的是KD
‑
tree的最近邻索引，为了避免i,j和l三点共线造成无解，j和l必须来自不同线的扫描，对于点如果(j,l)是相应的边缘线，则点到边缘线的距离可以计算为：其中和分别是{L}中点i,j和l的坐标，找一条边缘线作为中的边缘点的对应关系，j是最接近特征点的点，在中找到，第一条线代表与j同一次扫描，第二条线代表相邻两个连续的扫描，为了找到边缘线的对应关系，在第二条线上找到另一个点l，并将对应关系表示为(j,l)；i,j,l三点构建的几何约束关系，d
ε
为点到边缘线的距离。6.根据权利要求4所述的越野环境下的四足机器人激光自主定位建图方法，其特征在于：步骤S2中两帧点云进行配准，所述三维平面特征点配准的具体方法如下：设i为中的一个点，找到三个点来表示平面，首先在中找到最接近i的点，记为j，然后，记另外两个点为l和m，它们是最近邻i的点，l在与j同一线的扫描中，m是j相邻线的两次连续扫描中的点，这就保证了这三个点是非共线的，同理，计算平面点与对应
平面的距离，对于点如果(j,l,m)是对应的平面，则点到平面的距离为：其中是{L}中点m的坐标，找到一个平面作为中的平面点的对应关系，j是最接近特征点的点,能够在中找到，然后分别不同线上找到另外两个点l和m，对应关系表示为(j,l,m)；平面点特征匹配过程中通过i,j,l,m四点构建的几何约束关系，d
H
为点到平面的距离；在上述获得特征点的对应关系以后，便进行巡视器的定位工作，在激光雷达扫描过程中假设载体在短时间内具有恒定的角速度和线速度，可以在扫描中对不同时间接收的点进行线性插值姿态变换，其中，t
k+1
是第k+1次扫描开始的时间，t
k+2
为扫描结束的时间，为[t
k+1
,t
k+2
]之间的激光雷达姿态变换，包含激光雷达六自由度的刚性运动，其中t
x
,t
y
和t
z
是沿{L}的x
‑
,y
‑
和z
‑
轴的平移，而θ
x
,θ
y
和θ
z
是遵循右手定则的旋转角度，t
i
为点i的时间戳，为[t
k+1
,t
i
]之间激光雷达的姿态变换。7.根据权利要求6所述的越野环境下的四足机器人激光自主定位建图方法，其特征在于：所述步骤S3中相关点云之间建立几何关系，从而并进行运动补偿的具体方法如下：根据公式(4)中的变换，可以得出，其中，是ε
k+1
或H
k+1
中点i的坐标，是在和中i的对应点，是的第a至b行，R是由Rodrigues公式定义的旋转矩阵，便于求导：在上式中，θ是旋转幅度...

【专利技术属性】
技术研发人员：贺亮，陈建林，袁建平，宋婷，马川，于洋，
申请(专利权)人：江苏云幕智造科技有限公司，
类型：发明
国别省市：