CN111830529B - 一种基于灯光标定信息融合的激光slam方法及装置 - Google Patents

一种基于灯光标定信息融合的激光slam方法及装置 Download PDF

Info

Publication number
CN111830529B
CN111830529B CN202010657650.9A CN202010657650A CN111830529B CN 111830529 B CN111830529 B CN 111830529B CN 202010657650 A CN202010657650 A CN 202010657650A CN 111830529 B CN111830529 B CN 111830529B
Authority
CN
China
Prior art keywords
data
laser
frame
transformation matrix
pose
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.)
Active
Application number
CN202010657650.9A
Other languages
English (en)
Other versions
CN111830529A (zh
Inventor
王强
曾勇
高川琦
卢镇宇
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuhan University of Technology WUT
Original Assignee
Wuhan University of Technology WUT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Wuhan University of Technology WUT filed Critical Wuhan University of Technology WUT
Priority to CN202010657650.9A priority Critical patent/CN111830529B/zh
Publication of CN111830529A publication Critical patent/CN111830529A/zh
Application granted granted Critical
Publication of CN111830529B publication Critical patent/CN111830529B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders

Abstract

本发明公开一种基于灯光标定信息融合的激光SLAM方法、装置及计算机可读存储介质，属于激光SLAM与Li‑Fi信息传输技术领域，解决了现有技术中位姿漂移过大及建立的地图准确性较低的技术问题。一种基于灯光标定信息融合的激光SLAM方法，包括以下步骤：获取最优链路链接，根据所述最优链路链接，确定基于Li‑Fi的帧间齐次变换矩阵；获取数据结合帧数据，对所述数据结合数据进行配准，得到配准后的数据结合帧数据，确定基于激光的帧间齐次变换矩阵，获取融合Li‑Fi和激光数据帧之间的位姿变换矩阵，根据所述融合Li‑Fi和激光数据帧之间的位姿变换矩阵及配准后的数据结合帧数据，对当前帧与地图进行匹配并更新地图。抑制了位姿漂移，提高了建立的地图的准确性。

Description

一种基于灯光标定信息融合的激光SLAM方法及装置
技术领域
本发明涉及激光SLAM与Li-Fi信息传输技术领域，尤其是涉及一种基于灯光标定信息融合的激光SLAM方法、装置及计算机可读存储介质。
背景技术
即时定位与地图构建技术(Simultaneous localization and mapping，SLAM)是移动机器人在未知环境中进行探索、侦查、定位导航等各项任务的基础和关键，而激光SLAM作为搭载激光雷达的主体，能在运动中获得环境状态信息，估计自身或机器人的位姿，同时建立并更新周围的环境地图；SLAM的信息处理方式可分为滤波法和图优化法；由于二维激光SLAM的定位、映射和导航算法比视觉SLAM更成熟，因此二维激光SLAM更适合室内移动机器人；实际上，机器人外部运行环境是无法预测的，不同环境的不确定程度不同，机器人获取的信息也受传感器限制，因此，在几何特征不明显的环境或复杂的环境中，移动机器人很难通过单个传感器了解自身精确位置和周围环境，为了使移动机器人能在未知环境中稳定运行，基于多传感器结合的激光SLAM方案便成为了新的研究热点。
近年来，发光二极管由于其长寿命、高亮度和快响应的特性被广泛用于区域照明。可见光通信技术(Light Fidelity，Li-Fi)则是基于LED的新一代通信技术，它通过快速开关LED达到通信的目的；而基于可见光通信技术的室内定位系统，如RSS由于无需借助电磁波通信，可实现室内高精度定位；在SLAM中，回环检测是一个递推过程，即根据传感器信息判断机器人当前所处位置，并与全局地图中的上一位置进行比较。回环检测是未知环境地图创建中的一个难点，如，移动机器人在长走廊等特征点不明显的环境中运行时，传感器可获得的环境特征信息少，回环检测的难度增加，位姿漂移过大；移动机器人在存在大量相同或相似物体的复杂环境中，如存在相似的桌椅、墙角等物体时，会造成感知歧义，在对这些相似特征点进行重复判断和概率估计时，要处理的数据会成倍增加，导致建立的地图准确性较低。
发明内容
有鉴于此，本发明提供了一种基于灯光标定信息融合的激光SLAM方法、装置及计算机可读存储介质，解决现有技术中位姿漂移过大及建立的地图准确性较低的技术问题。
一方面，本发明提供了一种基于灯光标定信息融合的激光SLAM方法，包括以下步骤：
获取LED的标定坐标、LED发射机的编码以及接受机的估计坐标，根据所述LED的标定坐标、LED发射机的编码以及接受机的估计坐标，获取最优链路链接；
根据所述最优链路链接，获取基于Li-Fi的当前数据帧，根据所述基于Li-Fi的当前数据帧，确定基于Li-Fi的帧间齐次变换矩阵；
获取当前激光帧数据，根据当前激光帧数据及基于Li-Fi的帧间齐次变换矩阵，获取数据结合帧数据，对所述数据结合数据进行配准，得到配准后的数据结合帧数据，根据所述配准后的结合帧数据，确定基于激光的帧间齐次变换矩阵；
根据基于Li-Fi的帧间齐次变换矩阵及基于激光的帧间齐次变换矩阵，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵，根据所述融合Li-Fi和激光数据帧之间的位姿变换矩阵及配准后的数据结合帧数据，对当前帧与地图进行匹配并更新地图。
进一步地，根据所述LED的标定坐标、LED发射机的编码以及接受机的估计坐标，获取最优链路链接，具体包括，建立最优链路链接机制模型，通过所述最优链路链接机制模型，最优链路链接，其中，最优链路链接机制模型为
Figure BDA0002577329480000021
Txi＝(xi,yi,i)，(xi,yj)为LED的标定坐标，i＝1,2,3是LED发射机编码，Rxt＝(xe,ye,t)，t是观测时刻，(xe,ye)为接受机的估计坐标，E＝(1,1,0)T，
Figure BDA0002577329480000022
是Li-Fi链路数量。
进一步地，根据当前激光帧数据及基于Li-Fi的帧间齐次变换矩阵，获取数据结合数据，具体包括，根据所述Li-Fi齐次变换矩阵T1将当前激光帧数据进行映射变换得到虚拟帧数据，对虚拟帧数据做空间相邻顺序预处理，得到预处理后的虚拟帧数据，将当前激光帧数据变换到参考帧坐标系下，得到参考帧数据，将参考帧数据加入到虚拟帧数据中得到数据结合帧数据。
进一步地，将当前激光帧数据变换到参考帧坐标系下，得到参考帧数据，具体包括，通过帧间齐次变换将当前激光帧数据变换到参考帧坐标系下，所述帧间齐次变换的变化关系为
Figure BDA0002577329480000031
其中，Tj＝(xj,yj,θj)∈R2×[0,2π]是位姿变换矩阵，j＝1时，Tj为基于Li-Fi的帧间齐次变换矩阵，j＝2时，Tj为基于激光的帧间齐次变换矩阵，
Figure BDA0002577329480000032
表示数据点ql经过变换矩阵Tj变换到ql,f，f(Tj,ql)为对应的映射变换函数，ql＝(qlx,qly)T为观测坐标。
进一步地，确定基于Li-Fi的帧间齐次变换矩阵与基于激光的帧间齐次变换矩阵，具体包括，通过相对变换求解公式，确定基于Li-Fi的帧间齐次变换矩阵T1与基于激光的帧间齐次变换对应的位姿变换矩阵T2，所述相对变换求解公式为
Figure BDA0002577329480000033
其中，
Figure BDA0002577329480000034
为第k次迭代的位姿变换矩阵，
Figure BDA0002577329480000035
表示数据点ql经过变换矩阵
Figure BDA0002577329480000036
变换到ql,f，
Figure BDA0002577329480000037
进一步地，对所述数据结合数据进行配准，具体包括，计算当前数据与相邻数据的欧式距离，取欧式距离最小的两个数据作为配准数据，实现对所述数据结合数据的配准。
进一步地，根据基于Li-Fi的帧间齐次变换矩阵及基于激光的帧间齐次变换矩阵，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵，具体包括，根据基于Li-Fi的帧间齐次变换矩阵、基于激光的帧间齐次变换矩阵及基于概率的权重系数，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵。
进一步地，基于灯光标定信息融合的激光SLAM方法，还包括，根据基于Li-Fi数据的观测概率模型获取基于Li-Fi数据的观测概率，根据基于激光数据的观测概率模型获取基于激光数据的观测概率，根据基于Li-Fi数据的观测概率和所述基于激光数据的观测概率获取基于概率的权重系数，其中，基于Li-Fi数据的观测概率模型为
Figure BDA0002577329480000038
基于激光数据的观测概率模型为
Figure BDA0002577329480000039
其中，δt是t时刻观测的高斯白噪声，均值为0，方差为Qt，xt是t时刻的机器人位姿，zt,1是t时刻机器人基于Li-Fi最优链路的观测量，p(zt,1|xt)为位姿xt下对于观测量zt,1的观测概率；
Figure BDA0002577329480000041
K为激光传感器采集的激光观测数据的编号，
Figure BDA0002577329480000042
为t时刻采集的编号为K的激光观测数据，zt，2是t时刻机器人观测到的激光数据集，m是占用概率地图，p(zt,2|xt,m)为位姿xt下对于观测量zt,2的观测概率。
另一方面，本发明还提供了一种基于灯光标定信息融合的激光SLAM装置，包括处理器以及存储器，所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现如上述任一技术方案所述的基于灯光标定信息融合的激光SLAM方法。
另一方面，本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机该程序被处理器执行时，实现如上述任一技术方案所述的基于灯光标定信息融合的激光SLAM方法。
与现有技术相比，本发明的有益效果包括：通过获取LED的标定坐标、LED发射机的编码以及接受机的估计坐标，根据所述LED的标定坐标、LED发射机的编码以及接受机的估计坐标，获取最优链路链接；根据所述最优链路链接，获取基于Li-Fi的当前数据帧，根据所述基于Li-Fi的当前数据帧，确定基于Li-Fi的帧间齐次变换矩阵；获取当前激光帧数据，根据当前激光帧数据及基于Li-Fi的帧间齐次变换矩阵，获取数据结合帧数据，对所述数据结合数据进行配准，得到配准后的数据结合帧数据，根据所述配准后的结合帧数据，确定基于激光的帧间齐次变换矩阵；根据基于Li-Fi的帧间齐次变换矩阵及基于激光的帧间齐次变换矩阵，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵，根据所述融合Li-Fi和激光数据帧之间的位姿变换矩阵及配准后的数据结合帧数据，对当前帧与地图进行匹配并更新地图；抑制了位姿漂移，提高了建立的地图的准确性。
附图说明
图1为本发明实施例1所述的基于灯光标定信息融合的激光SLAM方法的其流程示意图；
图2为本发明实施例1所述的距离-区域模型示意图；
图3为本发明实施例1所述的机器人示意图；
图4为本发明实施例1所述的Li-Fi的链路链接数量与使用RSS室内定位得到机器人的定位误差关系图；
图5为本发明实施例1所述的raw odometry及pIC方法建立的环境地图；
图6为本发明实施例1所述的基于灯光标定信息融合的激光SLAM方法建立的环境地图；
图7为本发明实施例1所述的基于灯光标定信息融合的激光SLAM方法和Cartographer方法的回环精度、召回率对比的示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
实施例1
本发明实施例提供了一种基于灯光标定信息融合的激光SLAM方法，其流程示意图，如图1所示，所述方法包括以下步骤：
S1、获取LED的标定坐标、LED发射机的编码以及接受机的估计坐标，根据所述LED的标定坐标、LED发射机的编码以及接受机的估计坐标，获取最优链路链接；
S2、根据所述最优链路链接，获取基于Li-Fi的当前数据帧，根据所述基于Li-Fi的当前数据帧，确定基于Li-Fi的帧间齐次变换矩阵；
S3、获取当前激光帧数据，根据当前激光帧数据及基于Li-Fi的帧间齐次变换矩阵，获取数据结合帧数据，对所述数据结合数据进行配准，得到配准后的数据结合帧数据，根据所述配准后的结合帧数据，确定基于激光的帧间齐次变换矩阵；
S4、根据基于Li-Fi的帧间齐次变换矩阵及基于激光的帧间齐次变换矩阵，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵，根据所述融合Li-Fi和激光数据帧之间的位姿变换矩阵及配准后的数据结合帧数据，对当前帧与地图进行匹配并更新地图。
优选的，根据所述LED的标定坐标、LED发射机的编码以及接受机的估计坐标，获取最优链路链接，具体包括，建立最优链路链接机制模型，通过所述最优链路链接机制模型，最优链路链接，其中，最优链路链接机制模型为
Figure BDA0002577329480000051
Txi＝(xi,yi,i)，(xi,yj)为LED的标定坐标，i＝1,2,3是LED发射机编码，Rxt＝(xe,ye,t)，t是观测时刻，(xe,ye)为接受机的估计坐标，E＝(1,1,0)T，
Figure BDA0002577329480000061
是Li-Fi链路数量；
需要说明的是，通过筛选Li-Fi数据，保证最优链路链接，接收端接收到的Li-Fi数据会有多个，由于Li-Fi数据的准确性与接受端和发射端之间的空间距离有关，所以要筛选出与接收端距离最近并且被接收到的Linkn个发射端发出的Li-Fi数据，Linkn值与具体环境有关；
优选的，根据当前激光帧数据及基于Li-Fi的帧间齐次变换矩阵，获取数据结合数据，具体包括，根据所述Li-Fi齐次变换矩阵T1将当前激光帧数据进行映射变换得到虚拟帧数据，对虚拟帧数据做空间相邻顺序预处理，得到预处理后的虚拟帧数据，将当前激光帧数据变换到参考帧坐标系下，得到参考帧数据，将参考帧数据加入到虚拟帧数据中得到数据结合帧数据；
需要说明是，进行相邻帧匹配，根据激光雷达返回的质量信号Q可以得到测距结果的可信度，由于激光雷达的工作特点，当实际测量距离超过一定值L时，质量Q会下降，测距信息变得不可靠，为了减少测量误差，需要对每帧点云数据进行筛选，舍弃低质量数据点，然后通过迭代方式结合Li-Fi信息配准点集；通过距离—区域模型加速最近邻点的查找以及回环检测时的数据预处理，在增加数据帧信息维的同时保证系统的实时性；
具体实施时，基于Li-Fi的数据点进行配准，由于灯源信息是人为标定的，灯源投影信息是相对准确的，机器人接收到同一灯源的信号相同，而测量、信号处理及计算引起的误差已经在Li-Fi位置概率模型中有所体现，基于位置概率的Li-Fi数据帧的齐次变换是准确的；计算出基于Li-Fi的齐次变换矩阵T1，并依据变换矩阵T1将当前帧数据(Snew,i＝{qm,m＝1,L,Mj}，表示第i帧数据集中的m个观测量，包括基于Li-Fi和激光的观测量)，进行映射变换得到虚拟帧，以加快基于激光的点云数据的相对变换矩阵T2的求解，并对虚拟帧中的数据做空间相邻顺序排列的预处理，虚拟帧形式如下
Figure BDA0002577329480000062
其中，f为映射变换函数，它通过帧间齐次变换将当前帧(目标帧)中的数据点ql变换到参考帧(Sref,i＝{pn,n＝1,L,Nj}中，表示第i帧数据集中的n个观测量)坐标系下，得到数据点ql,f；
优选的，将当前激光帧数据变换到参考帧坐标系下，得到参考帧数据，具体包括，通过帧间齐次变换将当前激光帧数据变换到参考帧坐标系下，所述帧间齐次变换的变化关系为
Figure BDA0002577329480000071
其中，Tj＝(xj,yj,θj)∈R2×[0,2π]是位姿变换矩阵，j＝1时，Tj为基于Li-Fi的帧间齐次变换矩阵，j＝2时，Tj为基于激光的帧间齐次变换矩阵，
Figure BDA0002577329480000072
表示数据点ql经过变换矩阵Tj变换到ql,f，f(Tj,ql)为对应的映射变换函数，ql＝(qlx,qly)T为观测坐标；
将参考帧数据集加入到虚拟帧中，得到数据结合帧，如式下
Figure BDA0002577329480000073
其中，pl′为参考帧中的第l′个数据。
一个具体实施例中，根据距离-区域模型从当前数据中心向数据轴的两端搜索，图2为距离-区域模型示意图，即首先对帧数据进行空间相对原点距离排序，再对一个数据的相邻域的数据进行遍历排序，根据排序结果，计算当前数据与相邻数据的欧式距离，距离最小的两个数据即为对应配准数据；通过距离-区域模型来减少数据匹配的计算量，可以加快寻找最近邻点，在帧-图匹配时也能更快寻找出关键帧，图2中，空白圆圈表示融合帧中的数据点，三角形表示参考帧中的数据点，涂黑圆圈表示灯光标定信息，第一行代表虚拟帧，第二行代表数据融合帧，箭头表示距离值增大的方向；
优选的，确定基于Li-Fi的帧间齐次变换矩阵与基于激光的帧间齐次变换矩阵，具体包括，通过相对变换求解公式，确定基于Li-Fi的帧间齐次变换矩阵T1与基于激光的帧间齐次变换对应的位姿变换矩阵T2，所述相对变换求解公式为
Figure BDA0002577329480000074
其中，
Figure BDA0002577329480000075
为第k次迭代的位姿变换矩阵，
Figure BDA0002577329480000076
表示数据点ql经过变换矩阵
Figure BDA0002577329480000077
变换到数据点ql,f，
Figure BDA0002577329480000078
需要说明是，在找到使得激光点云数据对应点之间匹配度最高的的齐次变换点后，通过改变变换矩阵使得对应点之间的距离平方和最小；
优选的，对所述数据结合数据进行配准，具体包括，计算当前数据与相邻数据的欧式距离，取欧式距离最小的两个数据作为配准数据，实现对所述数据结合数据的配准。
优选的，根据基于Li-Fi的帧间齐次变换矩阵及基于激光的帧间齐次变换矩阵，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵，具体包括，根据基于Li-Fi的帧间齐次变换矩阵、基于激光的帧间齐次变换矩阵及基于概率的权重系数，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵。
基于激光和基于Li-Fi的点云数据是由不同的传感器所测量的，通过观测概率估计来确定T1和T2的权重，以融合基于激光的点云信息和基于Li-Fi的点云信息，然后重建激光帧之间的变换，并得到融合Li-Fi数据帧和激光数据帧之间的位姿变换Tk，
Figure BDA0002577329480000081
其中，k是迭代次数，T1 k、
Figure BDA0002577329480000082
分别为第k次迭代时的基于Li-Fi和激光的位姿变换，wprop是基于概率的权重系数，其数学模型如式
Figure BDA0002577329480000083
其中，ρj是归一化系数，使得所有观测量zt,j(包括zt,1：Li-Fi数据和zt,2：激光数据)的概率和为1，m是由观测量得到的占用概率地图；
优选的，所述基于灯光标定信息融合的激光SLAM方法还包括，根据基于Li-Fi数据的观测概率模型获取基于Li-Fi数据的观测概率，根据基于激光数据的观测概率模型获取基于激光数据的观测概率，根据基于Li-Fi数据的观测概率和所述基于激光数据的观测概率获取基于概率的权重系数，其中，基于Li-Fi数据的观测概率模型为
Figure BDA0002577329480000084
基于激光数据的观测概率模型为
Figure BDA0002577329480000085
其中，δt是t时刻观测的高斯白噪声，均值为0，方差为Qt，xt是t时刻的机器人位姿，zt,1是t时刻机器人基于Li-Fi最优链路的观测量，p(zt,1|xt)为位姿xt下对于观测量zt,1的观测概率；
Figure BDA0002577329480000086
K为激光传感器采集的激光观测数据的编号，
Figure BDA0002577329480000087
为t时刻采集的编号为K的激光观测数据，zt，2是t时刻机器人观测到的激光数据集，m是占用概率地图，p(zt,2|xt,m)为位姿xt下对于观测量zt,2的观测概率；
一个具体实施例中，建立数据观测概率模型，利用Extended Kalman Filter(EKF)构造Li-Fi数据的虚拟一致性状态分量，对激光传感器的扫描信息进行状态向量增广做概率估计，建立基于Li-Fi数据的观测概率模型，如下式所示
Figure BDA0002577329480000088
其中，δt是t时刻观测的高斯白噪声，均值为0，方差为Qt，xt是t时刻的机器人位姿，zt,1是t时刻机器人基于Li-Fi最优链路的观测量，p(zt,1|xt)为位姿xt下对于观测量zt,1的观测概率；基于激光数据的观测概率模型如下
Figure BDA0002577329480000091
其中，
Figure BDA0002577329480000092
K为激光传感器采集的激光观测数据的编号，
Figure BDA0002577329480000093
为t时刻采集的编号为K的激光观测数据，zt，2是t时刻机器人观测到的激光数据集，m是地图(由观测量得到的占用概率地图)，p(zt,2|xt,m)为位姿xt下对于观测量zt,2的观测概率。
需要说明的是，通过建立了接收端与LED发射端之间的最优链路链接，保证基于Li-Fi的数据集误差最小，通过基于Li-Fi和激光的相邻帧间的位姿变换矩阵，并通过建立的Li-Fi数据的观测概率以及激光数据的观测概率，重构帧间齐次变换矩阵得到Tk；相邻时刻机器人的位姿通过位姿变换Tk建立联系，从而确定不同时刻机器人的位姿，并结合观测数据计算出地图每个栅格的占用值；在当前帧与地图进行匹配时，将当前帧数据从本体坐标系变换到世界坐标系下，并调取当前帧的数据集Snew,i，与地图数据集SM做交集运算，若Snew,i∩SM＝Snew,i且集合元素数目不少于Linkn，说明当前帧Li-Fi链路无遮挡，否则说明取交集运算无意义，应该只依据激光点云数据进行配准；然后建立当前帧与子图的数据对应关系，具体步骤与上述对Li-Fi数据点及数据结合帧的配准相同，如果对应点之间的空间欧式距离小于阈值，则将当前帧定义为回环帧，并进行相对变换的求解，这时的两帧数据在时间上不一定相邻，但在空间上是相邻的，结合在相邻空间位置上采集的观测数据计算栅格占用值并更新地图；否则，进入下一帧与地图的匹配；
一个具体实施例中，采用的装载光电传感器和180度HOKUYO激光传感器的Komodo机器人作为基于灯光标定信息融合的激光SLAM方法的载体，所述机器人示意图，如图3所示；Li-Fi的链路链接数量与使用RSS室内定位得到机器人的定位误差关系图，如图4所示，图4中横坐标为链路数量，纵坐标为每厘米的错误值，通过改变Li-Fi的链路链接数量，使用RSS室内定位得到机器人的定位误差，确定Linkn的值为4，
通过raw odometry、pIC以及基于灯光标定信息融合的激光SLAM方法按相同轨迹控制机器人行驶，建立环境地图，raw odometry及pIC方法建立的环境地图，如图5所示，图5左边为pIC方法建立的环境地图，图5右边为raw odometry方法建立的环境地图；基于灯光标定信息融合的激光SLAM方法建立的环境地图，如图6所示，可以看出，基于灯光标定信息融合的激光SLAM方法的视觉效果要比raw odometry、pIC更好，尤其是，它可以利用Li-Fi配准在实施环境中通透的地方进行帧配准，随后，通过标记起点和行驶路径，使机器人使用Cartographer和基于灯光标定信息融合的激光SLAM方法分别运行，每次都从同一点开始，然后返回到起点，重复多次，统计数据；基于灯光标定信息融合的激光SLAM方法和Cartographer方法(二维激光SLAM中主要的闭环检测方法)的回环精度、召回率对比的示意图，如图7所示，图7显示了基于灯光标定信息融合的激光SLAM方法(Li-Fi based SLAM)和Cartographer的回环精度和召回率；基于灯光标定信息融合的激光SLAM方法有着可靠的、稳定的综合性能，提高了回环检测的准确率，从而有效抑制位姿漂移。
实施例2
本发明实施例提供了一种基于灯光标定信息融合的激光SLAM装置，包括处理器以及存储器，所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现如上述实施例1所述的基于灯光标定信息融合的激光SLAM方法。
实施例3
本发明实施例提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机该程序被处理器执行时，实现如上述实施例1所述的基于灯光标定信息融合的激光SLAM方法。
本发明公开了一种基于灯光标定信息融合的激光SLAM方法、装置及计算机可读存储介质，通过获取LED的标定坐标、LED发射机的编码以及接受机的估计坐标，根据所述LED的标定坐标、LED发射机的编码以及接受机的估计坐标，获取最优链路链接；根据所述最优链路链接，获取基于Li-Fi的当前数据帧，根据所述基于Li-Fi的当前数据帧，确定基于Li-Fi的帧间齐次变换矩阵；获取当前激光帧数据，根据当前激光帧数据及基于Li-Fi的帧间齐次变换矩阵，获取数据结合帧数据，对所述数据结合数据进行配准，得到配准后的数据结合帧数据，根据所述配准后的结合帧数据，确定基于激光的帧间齐次变换矩阵；根据基于Li-Fi的帧间齐次变换矩阵及基于激光的帧间齐次变换矩阵，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵，根据所述融合Li-Fi和激光数据帧之间的位姿变换矩阵及配准后的数据结合帧数据，对当前帧与地图进行匹配并更新地图；抑制了位姿漂移，提高了建立的地图的准确性；
本发明所述技术方案构建了Li-Fi信息的虚拟一致状态，能提供更多环境信息，而距离区域模型的建立可以适当减少扫描匹配过程中的计算量；信息融合关闭机制得建立可以减少Li-Fi链路受阻塞的影响，从而使得系统更加平滑稳定；提出了用于Li-Fi数据校准的最短链路链接机制，使得系统链接Li-Fi链路保持最优，减少校准误差；能够利用激光和光电传感器采集的数据建立概率权重，来重建帧之间的变换，以减少数据关联误差；通过在走廊和室内办公环境的实验，验证了其能够在提高回环检测的准确率的同时及时发现回环帧、保证召回率，有效抑制位姿漂移。
以上所述本发明的具体实施方式，并不构成对本发明保护范围的限定。任何根据本发明的技术构思所做出的各种其他相应的改变与变形，均应包含在本发明权利要求的保护范围内。

Claims (10)

1.一种基于灯光标定信息融合的激光SLAM方法，其特征在于，包括以下步骤：
获取LED的标定坐标、LED发射机的编码以及接受机的估计坐标，根据所述LED的标定坐标、LED发射机的编码以及接受机的估计坐标，获取最优链路链接；
根据所述最优链路链接，获取基于Li-Fi的当前数据帧，根据所述基于Li-Fi的当前数据帧，确定基于Li-Fi的帧间齐次变换矩阵；
获取当前激光帧数据，根据当前激光帧数据及基于Li-Fi的帧间齐次变换矩阵，获取数据结合帧数据，对所述数据结合帧数据进行配准，得到配准后的数据结合帧数据，根据所述配准后的数据结合帧数据，确定基于激光的帧间齐次变换矩阵；
根据基于Li-Fi的帧间齐次变换矩阵及基于激光的帧间齐次变换矩阵，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵，根据所述融合Li-Fi和激光数据帧之间的位姿变换矩阵及配准后的数据结合帧数据，对当前帧与地图进行匹配并更新地图。
2.根据权利要求1所述的基于灯光标定信息融合的激光SLAM方法，其特征在于，根据所述LED的标定坐标、LED发射机的编码以及接受机的估计坐标，获取最优链路链接，具体包括，建立最优链路链接机制模型，通过所述最优链路链接机制模型，最优链路链接，其中，最优链路链接机制模型为
Figure FDA0003990638700000011
Txi＝(xi,yi,i)，(xi,yj)为LED的标定坐标，i＝1,2,3是LED发射机编码，Rxt＝(xe,ye,t)，t是观测时刻，(xe,ye)为接受机的估计坐标，E＝(1,1,0)T，
Figure FDA0003990638700000012
Linkn是Li-Fi链路数量。
3.根据权利要求1所述的基于灯光标定信息融合的激光SLAM方法，其特征在于，根据当前激光帧数据及基于Li-Fi的帧间齐次变换矩阵，获取数据结合帧数据，具体包括，根据所述Li-Fi齐次变换矩阵T1将当前激光帧数据进行映射变换得到虚拟帧数据，对虚拟帧数据做空间相邻顺序预处理，得到预处理后的虚拟帧数据，将当前激光帧数据变换到参考帧坐标系下，得到参考帧数据，将参考帧数据加入到虚拟帧数据中得到数据结合帧数据。
4.根据权利要求3所述的基于灯光标定信息融合的激光SLAM方法，其特征在于，将当前激光帧数据变换到参考帧坐标系下，得到参考帧数据，具体包括，通过帧间齐次变换将当前激光帧数据变换到参考帧坐标系下，所述帧间齐次变换的变化关系为
Figure FDA0003990638700000021
其中，Tj＝(xj,yj,θj)∈R2×[0,2π]是位姿变换矩阵，j＝1时，Tj为基于Li-Fi的帧间齐次变换矩阵，j＝2时，Tj为基于激光的帧间齐次变换矩阵，
Figure FDA0003990638700000022
表示数据点ql经过变换矩阵Tj变换到ql,f，f(Tj,ql)为对应的映射变换函数，ql＝(qlx,qly)T为观测坐标。
5.根据权利要求4所述的基于灯光标定信息融合的激光SLAM方法，其特征在于，确定基于Li-Fi的帧间齐次变换矩阵与基于激光的帧间齐次变换矩阵，具体包括，通过相对变换求解公式，确定基于Li-Fi的帧间齐次变换矩阵T1与基于激光的帧间齐次变换对应的位姿变换矩阵T2，所述相对变换求解公式为
Figure FDA0003990638700000023
其中，
Figure FDA0003990638700000024
为第k次迭代的位姿变换矩阵，
Figure FDA0003990638700000025
表示数据点ql经过变换矩阵
Figure FDA0003990638700000026
变换到数据点ql,f，
Figure FDA0003990638700000027
6.根据权利要求1所述的基于灯光标定信息融合的激光SLAM方法，其特征在于，对所述数据结合帧数据进行配准，具体包括，计算当前数据与相邻数据的欧式距离，取欧式距离最小的两个数据作为配准数据，实现对所述数据结合帧数据的配准。
7.根据权利要求1所述的基于灯光标定信息融合的激光SLAM方法，其特征在于，根据基于Li-Fi的帧间齐次变换矩阵及基于激光的帧间齐次变换矩阵，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵，具体包括，根据基于Li-Fi的帧间齐次变换矩阵、基于激光的帧间齐次变换矩阵及基于概率的权重系数，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵。
8.根据权利要求1所述的基于灯光标定信息融合的激光SLAM方法，其特征在于，还包括，根据基于Li-Fi数据的观测概率模型获取基于Li-Fi数据的观测概率，根据基于激光数据的观测概率模型获取基于激光数据的观测概率，根据基于Li-Fi数据的观测概率和所述基于激光数据的观测概率获取基于概率的权重系数，其中，基于Li-Fi数据的观测概率模型为
Figure FDA0003990638700000031
基于激光数据的观测概率模型为
Figure FDA0003990638700000032
其中，δt是t时刻观测的高斯白噪声，均值为0，方差为Qt，xt是t时刻的机器人位姿，zt,1是t时刻机器人基于Li-Fi最优链路的观测量，p(zt,1|xt)为位姿xt下对于观测量zt,1的观测概率；
Figure FDA0003990638700000033
K为激光传感器采集的激光观测数据的编号，
Figure FDA0003990638700000034
为t时刻采集的编号为K的激光观测数据，zt，2是t时刻机器人观测到的激光数据集，m是占用概率地图，p(zt,2|xt,m)为位姿xt下对于观测量zt,2的观测概率。
9.一种基于灯光标定信息融合的激光SLAM装置，其特征在于，包括处理器以及存储器，所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现如权利要求1-8任一所述的基于灯光标定信息融合的激光SLAM方法。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机该程序被处理器执行时，实现如权利要求1-8任一所述的基于灯光标定信息融合的激光SLAM方法。
CN202010657650.9A 2020-07-09 2020-07-09 一种基于灯光标定信息融合的激光slam方法及装置 Active CN111830529B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010657650.9A CN111830529B (zh) 2020-07-09 2020-07-09 一种基于灯光标定信息融合的激光slam方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010657650.9A CN111830529B (zh) 2020-07-09 2020-07-09 一种基于灯光标定信息融合的激光slam方法及装置

Publications (2)

Publication Number Publication Date
CN111830529A CN111830529A (zh) 2020-10-27
CN111830529B true CN111830529B (zh) 2023-04-14

Family

ID=72900737

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010657650.9A Active CN111830529B (zh) 2020-07-09 2020-07-09 一种基于灯光标定信息融合的激光slam方法及装置

Country Status (1)

Country Link
CN (1) CN111830529B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112612034B (zh) * 2020-12-24 2023-10-13 长三角哈特机器人产业技术研究院 基于激光帧和概率地图扫描的位姿匹配方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106662453A (zh) * 2014-08-27 2017-05-10 高通股份有限公司 用于根据传感器和射频测量进行基于移动的实时定位的方法和设备
CN109358342A (zh) * 2018-10-12 2019-02-19 东北大学 基于2d激光雷达的三维激光slam系统及控制方法
DE102019128799A1 (de) * 2018-10-26 2020-04-30 Ford Global Technologies, Llc Systeme und verfahren zum bestimmen eines fahrzeugstandorts in parkstrukturen

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106662453A (zh) * 2014-08-27 2017-05-10 高通股份有限公司 用于根据传感器和射频测量进行基于移动的实时定位的方法和设备
CN109358342A (zh) * 2018-10-12 2019-02-19 东北大学 基于2d激光雷达的三维激光slam系统及控制方法
DE102019128799A1 (de) * 2018-10-26 2020-04-30 Ford Global Technologies, Llc Systeme und verfahren zum bestimmen eines fahrzeugstandorts in parkstrukturen

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
"A review and insight on various indoor positioning and navigation techniques";Mehta J et al.;《National Conference on Emerging Trends in Computer, Electrical and Electronics》;20151231;全文 *
"基于双目立体视觉的特征点提取与定位方法研究";吴哲岑;《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》;20150915;全文 *

Also Published As

Publication number Publication date
CN111830529A (zh) 2020-10-27

Similar Documents

Publication Publication Date Title
CN109613549B (zh) 一种基于卡尔曼过滤的激光雷达定位方法
Guan et al. Robot localization and navigation using visible light positioning and SLAM fusion
CN110031817B (zh) 一种激光雷达反光板的快速匹配方法
CN111413970A (zh) 超宽带与视觉融合的室内机器人定位与自主导航方法
CN111751785B (zh) 一种隧道环境下车辆可见光定位方法
Wang et al. Machine learning and its applications in visible light communication based indoor positioning
CN114111774B (zh) 车辆的定位方法、系统、设备及计算机可读存储介质
CN111830529B (zh) 一种基于灯光标定信息融合的激光slam方法及装置
Morawska et al. Transfer learning-based UWB indoor localization using MHT-MDC and clusterization-based sparse fingerprinting
Ren et al. LiDAR‐based robust localization for field autonomous vehicles in off‐road environments
CN114264297B (zh) Uwb和视觉slam融合算法的定位建图方法及系统
Irish et al. Probabilistic 3D mapping based on GNSS SNR measurements
CN115728803A (zh) 一种城市驾驶车辆连续定位系统及方法
WO2022111723A1 (zh) 道路边缘检测方法及机器人
CN112762929B (zh) 一种智能导航方法、装置和设备
CN115103299B (zh) 一种基于rfid的多传感器融合定位方法
CN116466712A (zh) 基于cml-avg激光雷达小车的自主巡航方法及系统
e Silva et al. Challenges and solutions in received signal strength-based seamless positioning
KR20230056690A (ko) 다중 에이전트 맵 생성
Li et al. 3D semantic map construction based on point cloud and image fusion
HU et al. Integration of WiFi, laser, and map for robot indoor localization
CN114758364A (zh) 基于深度学习的工业物联网场景融合定位方法及系统
CN112987720A (zh) 一种用于移动机器人的多尺度地图构建方法及构建装置
Zhang et al. A robust regression model for simultaneous localization and mapping in autonomous mobile robot
Liu et al. A Review of Sensing Technologies for Indoor Autonomous Mobile Robots

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