一种可见光定位与激光雷达惯性里程计的紧耦合优化方法

1.本发明涉及智能设备技术领域，特别是涉及一种可见光定位与激光雷达惯性里程计的紧耦合优化方法。


背景技术：

2.状态估计、定位和建图是一个成功的智能移动机器人的基本先决条件，这是反馈控制、避障和规划等能力所必需的。利用基于视觉和基于激光雷达的传感，人们一直在努力实现高性能的实时同步定位和建图(slam)。基于视觉的方法通常会使用单眼或立体相机，并在连续图像上使用三角特征来确定相机的运动。虽然基于视觉的方法特别适合于位置识别，但它们对初始化、照明和范围的敏感性使得它们在单独用于支持自动导航系统时不可靠。另一方面，基于激光雷达的方法在很大程度上对照明变化是不变的。特别是随着最近出现的远程、高分辨率的三维激光雷达，激光雷达传感器能够以较大的扫描角和高角分辨率进行其周围的测量，所以激光雷达更适合在三维空间中直接捕捉环境的精细细节。
3.可见光通信技术(简称vlc)以其精度高、成本低、易于实现的优点极大填补了商业化室内定位的刚需。同时，由于可见光通信技术不会产生任何的射频干扰，可见光定位算法在射频辐射被严格限制的环境中(如医院、核电站等)同样有良好的表现。可见光定位技术(简称vlp)是通过对led进行调制，使各个led以不同频率明暗闪烁并通过空气传播其位置信息。移动终端的接收装置对vlp信息进行捕获，通过图像处理技术对捕获的图片进行处理，解调出led的位置信息，再利用几何学等原理，计算出移动终端的位置信息。
4.基于相机的vlp系统采用安装在已知位置(例如天花板上)的调制led灯作为人工地标，将每个led通过基于rse的occ测量与id坐标相关联。若要减少vlp中所需的指示灯的数量，则不再将捕获的led图像作为点，而是作为利用几何特征确定接收器方向和位置的图像，此方法需要在led上放置一个附加的标记。
5.现有的基于激光雷达传感器和惯性传感器等多传感器松耦合的slam定位技术不能有效校正由于传感器运动而带来的点云畸变或轮式里程计漂移以及缺乏可靠准确的绝对位姿测量的全局检测的问题，因此亟需一种可见光定位与激光雷达惯性里程计的紧耦合优化方法成为本领域人员关注的热点问题。


技术实现要素：

6.本发明针对现有的基于激光雷达传感器和惯性传感器等多传感器松耦合的slam定位技术不能有效校正由于传感器运动而带来的点云畸变或轮式里程计漂移以及缺乏可靠准确的绝对位姿测量的全局检测的问题，提供了一种可见光定位与激光雷达惯性里程计的紧耦合优化方法。
7.为实现上述目的，本发明提供了如下方案：
8.一种可见光定位与激光雷达惯性里程计的紧耦合优化方法，包括：
9.基于惯性测量单元imu估计移动终端的运动，得到惯性里程计估计的增量位姿变
换；
10.通过所述惯性里程计估计的增量位姿变换对激光雷达获取的点云数据进行预处理，并将通过所述惯性测量单元imu预积分得到的变换帧作为所述激光雷达帧f
i+1
的初始变换，再将所述激光雷达帧f
i+1
的初始变换转换到世界坐标系，得到激光雷达帧f
′
i+1
；
11.对所述激光雷达帧f
′
i+1
进行特征匹配，得到相邻两个激光雷达帧之间的转换关系δt
i,i+1
；
12.通过激光雷达帧转换关系δt
i,i+1
校正对应时间戳的imu里程计的偏差，得到经激光里程计校正后的位姿，获取从vlp定位算法计算得到的所述移动终端观测位姿x
t
,y
t
,z
t
，将其作为绝对测量因子对全局因子图进行优化。
13.优选地，所述基于惯性测量单元imu估计移动终端的运动，得到惯性里程计估计的增量位姿变换的过程包括：
14.步骤s11：从所述imu惯性传感器测量得到移动终端传感器的角速度测量值和加速度测量值
15.步骤s12：由所述步骤s11获得的加速度和角速度加上测量噪声n
t
和偏移量b
t
估计所述移动终端上传感器实际的速度ω
t
和加速度a
t
；
16.步骤s13：在t～(t+δt)时间段内通过惯性里程计运动模型并结合所述步骤s12估计的速度ω
t
和加速度a
t
得到传感器在(t+δt)时刻的位置p
t+δt
、速度v
t+δt
和旋转矩阵r
t+δt
；
17.步骤s14：由所述步骤s13得到的δt时间内的运动换算结果并通过imu预积分方法计算时间ti与tj之间的速度、位置、旋转矩阵的变换关系，得到所述惯性里程计估计的增量位姿变换。
18.优选地，通过所述惯性里程计估计的增量位姿变换对所述激光雷达获取的点云数据进行去倾斜预处理。
19.优选地，得到所述激光雷达帧f
′
i+1
的方法包括：
20.步骤s21：基于所述imu预积分方法得到的移动终端的运动变换关系，对获取的由于激光雷达传感器的运动而产生畸变的点云进行校正，得到去畸变的激光雷达点云；
21.步骤s22：由步骤s21得到的所述去畸变的激光雷达点云，通过待评估区域上的点的粗糙度进行特征提取操作，分别为边缘和平面特征，其中，粗糙度大的点归为边缘特征，粗糙度小的点归为平面特征；所述粗糙度由提取到的特征点的曲率决定，曲率大于设定阈值的点视为边缘特征，曲率小于设定阈值的点视为平面特征；
22.步骤s23：对每一个新的激光雷达扫描帧先提取其所述边缘特征和平面特征，组成关键帧f
i+1
；然后将f
i+1
通过转换到世界坐标系下得到f’i+1
，当与之前的状态相比，所述移动终端姿态的变化超过用户定义的阈值时，f
′
i+1
作为新保存的关键帧，f
′
i+1
与因子图中的一个新的移动终端状态节点x
i+1
相关联，两个关键帧f
′i和f
′
i+1
之间的激光雷达帧被丢弃。
23.优选地，基于高斯-牛顿法gauss-newton对所述激光雷达帧f
′
i+1
进行特征匹配，通过计算得到相邻两个激光雷达帧之间的转换关系δt
i,i+1
。
24.优选地，计算所述δt
i,i+1
的方法包括：
25.步骤s31：在长度为m的滑动窗口内选取m个激光雷达关键帧组成f’i-m
...f’i
，构成体素地图mi，即：
[0026][0027][0028][0029]
步骤s32：对关键帧f’i+1
在体素地图中通过高斯-牛顿方法使d
ek
+d
pk
最小化求得最优变换t
i+1
，其中
[0030][0031][0032][0033]
对于的一个边缘特征和是中形成相应的边缘的点；对于的一个平面特征和是中形成相应的平面区域的点；k，u，v和w是它们集合中的特征点索引值；
[0034]
步骤s33：通过所述步骤s32得到的最优变换t
i+1
，计算δt
i,i+1
＝ti′
t
i+1
。
[0035]
优选地，得到所述经激光里程计校正后的位姿的过程包括：
[0036]
步骤s41：获取所述imu里程计最近一帧激光里程计的位姿变换；
[0037]
步骤s42：对步骤s14计算得到的imu惯性里程计估计的增量位姿变换，乘以所述步骤s41的激光里程计的位姿变换，得到经激光里程计校正后的位姿。
[0038]
优选地，所述从vlp定位算法计算得到的所述移动终端观测位姿x
t
,y
t
,z
t
的计算方法包括：
[0039]
步骤s51：调制led灯光，用于传递led灯体id以及获得与所述灯体id相对应的led灯体位置信息；
[0040]
步骤s52：获取某一led灯体在世界坐标系中的位置xi，yi，zi，并计算位于移动终端上的相机中心在世界坐标系中的位置p(xs，ys，zs)；
[0041]
步骤s53：通过位于移动终端上的相机的坐标系与该移动终端的base_link坐标系的坐标系变换关系tf-tree，计算得到所述移动终端的姿态pt(x
t
,y
t
,z
t
)；
[0042]
步骤s54：由步骤s53得到的姿态pt(x
t
,y
t
,z
t
)作为全局闭环检测因子进行地图优化。
[0043]
本发明的有益效果为：
[0044]
1.本发明通过为激光slam技术提供比较可靠的姿态观测值，可以校正视觉里程
计、激光雷达等传感器的累积误差，提供更为精准的位姿，增强算法的鲁棒性；
[0045]
2.通过多传感器的融合，本发明可以弥补独立传感器的不足，并提供更可靠的姿态估计。在led短缺/故障时为终端提供准确而可靠的定位，算法的鲁棒性使移动终端不仅适用于照明设施完善的场合，在信源短缺的情况下也能提供高精度的定位服务，为路径规划和自主导航打下坚实基础。
附图说明
[0046]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0047]
图1为本发明实施例的一种可见光定位与激光雷达惯性里程计的紧耦合优化方法总体流程示意图；
[0048]
图2为本发明实施例的一种可见光定位与激光雷达惯性里程计的紧耦合优化方法具体流程图；
[0049]
图3为本发明实施例的世界、相机和图像坐标系之间的变换示意图。
具体实施方式
[0050]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0051]
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
[0052]
参照图1-图3所示，本发明提供一种可见光定位与激光雷达惯性里程计的紧耦合优化方法，利用imu惯性里程计、rse照相机、激光雷达实现高精度定位。利用imu测量值估计激光雷达在扫描过程中的运动情况以去偏斜点云，而imu自身估计的运动则可以为激光雷达测距优化提供一个初始优化。然后利用激光雷达测速解来估计imu的偏差。而vlp测量可以通过提供高精度的姿态和定位以提供全局纠正偏移，达到校正姿态的目的。
[0053]
该方法包括：
[0054]
步骤s1：从惯性测量单元(imu)估计移动终端的运动，得到惯性里程计增量位姿变换。
[0055]
步骤s11：从imu惯性传感器测量得到移动终端传感器的角速度测量值和加速度测量值
[0056]
步骤s12：由步骤s11获得的加速度和角速度加上测量噪声n
t
和偏移量b
t
来估计移动终端上传感器实际的速度ω
t
和加速度a
t
。具体计算公式如下：
[0057]
[0058][0059]
其中，是从世界坐标系w到移动终端自身坐标系b的旋转矩阵，g是重力加速度向量。
[0060]
步骤s13：在t～(t+δt)时间段内由imu运动模型并结合步骤s12估计的速度和加速度来估计传感器在(t+δt)时刻的位置p
t+δt
、速度v
t+δt
和旋转矩阵r
t+δt
。
[0061]
具体计算公式如下：
[0062][0063][0064][0065]
步骤s14：由步骤s13得到的δt时间内的运动换算结果并通过imu预积分方法计算时间ti与tj之间的速度δv
ij
、位置δp
ij
、旋转矩阵δr
ij
的变换关系。
[0066]
具体计算公式如下：
[0067]
步骤s2：将步骤s1惯性里程计增量位姿变换步骤s2：将步骤s1惯性里程计增量位姿变换
[0068][0069][0070]
为激光雷达获取的点云做去倾斜的处理，并将通过imu预积分得到的从移动终端主体坐标系b到世界坐标系w的变换作为激光雷达帧f
i+1
的初始变换，将f
i+1
转换到世界坐标系得到f’i+1
，计算方法包括：
[0071]
步骤s21：由imu预积分因子得到的移动终端的运动变换关系，对获取的由于激光雷达传感器的运动而产生畸变的点云进行校正。
[0072]
步骤s22：由步骤s21得到的去畸变的激光雷达点云，通过评估区域上的点的粗糙度来执行特征提取操作，分别为边缘和平面特征。粗糙度大的点归为边缘特征，粗糙度小的点归为平面特征。对每一次扫描的点云划分平面点和边缘点方法如下：
[0073]
步骤s221：选取想要求的曲率点i周围5个点构成一个集合s；
[0074]
步骤s222：按公式计算该点i的曲率；其中c表示计算得到的曲率；s表示计算点云的区域，k表示第k帧点，l表示是在激光坐标下，x表示求差值。
[0075]
步骤s223：将每一次点云分成4份，从每份中选取两个曲率最大的点作为边缘点和四个曲率最小的点作为平面点；
[0076]
步骤s23：对每一个新的激光雷达扫描帧先提取其边缘特征和平面特征以组成关键帧f
i+1
。然后将f
i+1
通过转换到世界坐标系下得到f’i+1
，当与之前的状态xi对应的f’i
相比，移动终端姿态的变化超过用户定义的阈值时，f’i+1
作为新保存的关键帧，f’i+1
与因子图中的一个新的移动终端状态节点x
i+1
相关联,两个关键帧f’i
和f’i+1
之间的激光雷达帧被
丢弃。
[0077]
步骤s3：将步骤s2获取的激光雷达帧f’i+1
使用高斯-牛顿法(gauss-newton)进行特征匹配，计算得到相邻两个激光雷达帧之间的转换关系δt
i,i+1
,计算方法包括：
[0078]
步骤s31：在长度为m的滑动窗口内选取m个激光雷达关键帧组成(f’i-m
...f’i
)，并以此构成体素地图mi。也即：
[0079][0080][0081][0082]
步骤s32：对关键帧f’i+1
在体素地图中通过高斯-牛顿方法使d
ek
+d
pk
最小化来求得最优变换，其中，
[0083][0084][0085][0086]
对于的一个边缘特征和是中形成相应的边缘的点。对于的一个平面特征和是中形成相应的平面区域的点。k，u，v和w是它们集合中的特征点索引值。
[0087]
步骤s33：由步骤s32得到的t
i+1
，可以计算δt
i,i+1
＝t
i’t
i+1
。
[0088]
步骤s4：由步骤s3中获得的激光雷达位姿变换t
i+1
来校正对应时间戳的imu里程计的偏差，得到较为精确的惯性里程计位姿估计。
[0089]
步骤s41：获取imu里程计最近一帧激光里程计的位姿变换。
[0090]
步骤s42：对步骤s1中imu惯性里程计估计的增量位姿变换δv
ij
、δp
ij
和δr
ij
，乘以步骤s41的激光里程计位姿变换，得到经激光里程计校正后的位姿。
[0091]
步骤s5：获取从vlp定位算法计算得到的移动终端观测位姿(x
t
,y
t
,z
t
)，将其作为的绝对测量因子对全局因子图做优化。
[0092]
步骤s51：获取某一led灯体在世界坐标系中的位置(xi，yi，zi)，并计算终端相机图像中心在世界坐标系中的位置p(xs，ys，zs)。其方法包括：
[0093]
步骤s511：调制led灯光，用于传递led灯体id以及获得与所述灯体id相对应的led灯体位置信息。调制led灯光是指让led灯的发光芯片按照一定的频率闪烁，闪烁频率对应于调制信号；在调制的过程中对每个led灯体调制分配一个id编码，同时调制入led灯体的位置信息；
[0094]
步骤s512：获取移动终端捕获的某一led灯体的感光区域roi并识别其身份(led-id)，并从注册的灯光数据库(预制的led地标地图)检索led的3d位置p
led
(xi，yi，zi)；
[0095]
步骤s513：由步骤s512获取的p
led
(xi，yi，zi)可以计算该led灯体在相机图像坐标系中获得的位置为(xi，yi)，其方法包括：
[0096][0097]
λ为所述移动终端与所述led灯体之间在z轴上的高度差，根据计算的所述led像平面的直径和所述led灯体实际直径的比例关系、三角形性质及成像原理，求得所述移动终端与所述led灯体之间在z轴上的高度差，即
[0098]
其中d为led的直径，d
pixel
为相机的像素距离，pd是像素距离到物理距离的转换，f为相机的焦距。
[0099]
f(焦距),dx/dy(像素距离),u0/v0(图像中心像素)组成了特征矩阵k；
[0100]
t是平移矩阵，相当于移动终端在三维世界坐标系中的位置；
[0101]
r是从世界坐标系到相机坐标系的旋转矩阵。
[0102][0103]
当只考虑二维平面时，α,β视为常数，而γ由惯性里程计传感器获取。
[0104]
步骤s514：由步骤s511得到的p
led
(xi，yi,＝zi)，可以计算:
[0105]
zs＝zi-λ
[0106]
步骤s515：相机中心在世界坐标系的位姿计算方法包括：
[0107]
由
[0108]
得到ps[xs,ys,zs]，k为由f(焦距),dx/dy(像素距离),u0/v0(图像中心像素)组成的特征矩阵。
[0109]
步骤s52：移动终端在世界坐标系的位姿st可以由以下方法获取：
[0110][0111]
其中r
x
，ry，rz和t
x
，ty，tz是从相机坐标系到移动终端的base_link坐标系的坐标变换的旋转和平移的系数。
[0112]
步骤s53：根据激光雷达的时间戳对vlp测量进行线性插值，对激光雷达每一个新
的状态节点xi，引入响应时间戳的vlp绝对测量因子与之对应，以此作为地图优化的vlp优化因子。
[0113]
通过惯性里程计的状态预测和激光雷达扫描仪和vlp的测量更新，本发明可以在减少传统轮式里程计本身的漂移的同时，融合高精度的激光雷达测距传感器，并通过vlp定位提供一个较为精准可靠的绝对测量。本发明的融合可见光、激光雷达、惯性里程计多传感器紧耦合的定位方法使移动终端能够最大限度地提高其对环境的感知意识，并获得足够的测量值，在室内定位和导航方面很有前景。
[0114]
本发明具有如下有益效果：
[0115]
1.本发明由惯性里程计估计的移动终端的运动和位姿的误差明显降低，定位精度明显提高；
[0116]
2.本发明通过为激光slam技术提供比较可靠的姿态观测值，可以校正视觉里程计、激光雷达等传感器的累积误差，提供更为精准的位姿，增强算法的鲁棒性；
[0117]
3.通过多传感器的融合，本发明可以弥补独立传感器的不足，并提供更可靠的姿态估计；在led短缺/故障时为终端提供准确而可靠的定位，算法的鲁棒性使移动终端不仅适用于照明设施完善的场合，在信源短缺的情况下也能提供高精度的定位服务，为路径规划和自主导航打下坚实基础。
[0118]
以上所述的实施例仅是对本发明优选方式进行的描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。