【技术实现步骤摘要】
基于双窗口的激光里程计方法、系统、装置
[0001]本专利技术涉及服务机器人
，具体涉及一种基于双窗口的激光里程计方法、系统、装置。
技术介绍
[0001]随着机器人技术的发展，服务机器人已经进入人们生产生活的各个方面。为了完成任务，机器人需要首先确定自身在环境中的位置。机器人定位受到国内外研究人员的普遍关注。通过激光雷达传感器实现机器人定位的方法称为激光里程计。激光里程计的常用方法是通过匹配当前帧和地图中的点云数据来优化当前帧位姿，称之为帧图匹配，其中地图一般通过之前的多帧点云数据创建，代表方法包括LOAM(LiDAR Odometry and Mapping)、LeGO
‑
LOAM(Lightweight and Ground
‑
Optimized Lidar Odometry and Mapping)以及基于有向几何点DGP(Directed Geometry Point)的方法等。上述方法仅优化当前帧位姿，这使得长期的累积误差较大。为了降低累积误差，同时优化多帧位姿的平滑方法...
【技术保护点】
【技术特征摘要】
中的最佳关联特征。3.根据权利要求2所述的基于双窗口的激光里程计方法，其特征在于，基于第二特征集的特征及其在第二环境特征集中的最佳关联特征，构建位姿优化函数，其方法为：其中，R
wl
和t
wl
分别是T
wl
的旋转矩阵和平移向量，T
wl
是激光雷达传感器当前帧L
l
的坐标系到世界坐标系W的位姿，是传感器当前帧L
l
的坐标系到世界坐标系W的优化后位姿，p
a
是第二特征集P2中的特征在第一特征集P1中对应特征的位置，和分别是特征在第二环境特征集Q2中的最佳关联特征的位置和方向，是矩阵转置算子，‖
·
‖表示向量欧式范数，为位姿所在的流形空间，|P2|为第二特征集P2中的特征数量，a表示第二特征集P2中的特征编号。4.根据权利要求3所述的基于双窗口的激光里程计方法，其特征在于，根据第二特征集的特征及其在第二环境特征集中的最佳关联特征，利用第一特征集更新第一环境特征集，其方法为：如果当前帧是新的关键帧，记为则根据第二特征集P2中的特征及其在第二环境特征集Q2中的最佳关联特征将与对应的第一特征集P1中的特征(p
a
，n
a
)加入与对应的第一环境特征集Q1中的特征所在的第i
*
组，记为其中如果第二特征集P2中的特征在第二环境特征集Q2中没有找到最佳关联特征，则直接用对应的第一特征集P1中的特征(p
a
，n
a
)，在第一环境特征集Q1中创建新的观测特征观测特征其中全部赋值为Null，然后将N1赋值为N1+1；对于第二环境特征集Q2中未选为最佳关联的特征找到其对应的第一环境特征集Q1中的特征(p
i，j
，n
i，j
)所在的第i组，在第一环境特征集Q1中创建新的观测特征(p
i，N+1
，n
i，N+1
)并将它赋值为Null；如果(N+1)≤N
large
，将N赋值为N+1；否则删除第一环境特征集Q1中所有特征在外部大窗口中最旧关键帧中的观测特征(p
i，1
，n
i，1
)，然后将其余观测特征的观测帧编号减1，即将(p
i，j
，n
i，j
)(j＝2，3，...，N+1)的观测帧编号j全部减1，并删除关键帧同时将关键帧的编号j也均减1，N为外部大窗口中的关键帧的数量，N
large
为第一预设数量，即外部大窗口中关键帧的最大数量。
5.根据权利要求1所述的基于双窗口的激光里程计方法，其特征在于，基于第一环境特征集，构建内部小窗口中的关键帧的多帧位姿优化函数，其方法为：其中，其中，是关键帧的坐标系到世界坐标系W的优化后位姿，N1是第一环境特征集Q1中特征的数量，s
i
是第一环境特征集Q1中的第i个特征在之前N个关键帧中的最旧观测帧编号，即关键帧中的最旧观测帧编号，即是第一环境特征集Q1中的第i个特征在外部大窗口的第j个关键帧中的观测特征q
i，j
＝(p
i，j
，n
i，j
)在世界坐标系W中的对应特征的位置，即对应特征的位...
【专利技术属性】
技术研发人员：李忠辉，曹志强，谭民，梁爽，亢晋立，
申请(专利权)人：中国科学院自动化研究所，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。