一种基于视觉里程计的松耦合多传感器融合定位算法的制作方法

1.本发明涉及智能设备技术领域，具体为一种基于视觉里程计的松耦合多传感器融合定位算法。


背景技术：

2.视觉里程计与传统的里程计不同，不使用码盘等设备，只利用摄像头拍摄的连续图像帧就可以计算里程，非常方便，因而用途广泛。和车轮里程计相比，视觉里程计的优势是不受车轮在恶劣环境下(如不平整的路面、水面或沙漠)打滑的影响。汽车拐弯时，左右车轮的拐弯半径是不同的，这会增加车轮里程计的误差。而视觉里程计可以提供更为精确的轨迹估计，相对位姿误差范围为0.1％到2％(2012年数据)。视觉里程计在某些特殊场景下是非常重要且必须的，如无法使用车轮里程计的环境(如无人机)下，在gps失效的特殊环境(如水下、外太空)下。
3.可见光通信技术(简称vlc)以其精度高、成本低、易于实现的优点极大填补了商业化室内定位的刚需。同时，由于可见光通信技术不会产生任何的射频干扰，可见光定位算法在射频辐射被严格限制的环境中(如医院、核电站等)同样有良好的表现。可见光定位技术(简称vlp)是通过对led进行调制，使各个led以不同频率明暗闪烁并通过空气传播其位姿信息。移动终端的接收装置对vlp信息进行捕获，通过图像处理技术对捕获的图片进行处理，解调出led的位姿信息，再利用几何学等原理，计算出移动终端的位姿信息。
4.基于相机的vlp系统采用安装在已知位姿(例如天花板上)的调制led灯作为人工地标，将每个led通过基于rse的occ测量与id坐标相关联。若要减少vlp中所需的指示灯的数量，则不再将捕获的led图像作为点，而是作为利用几何特征确定接收器方向和位姿的图像。此方法需要在led上放置一个附加的标记。
5.slam定位技术对于在未知环境中构建和更新地图很有用，而移动终端则维护有关其位姿的信息。它包括同时构建的环境模型(地图)和在其内部移动的终端的姿态的估计。slam可根据传感器分为两类：基于视觉和基于激光雷达。与其他距离测量装置相比，激光雷达传感器以较大的扫描角和高角分辨率进行其周围的测量。此外，它对照明是不变的。高可靠性和精度使激光雷达传感器成为姿态估计的流行选择。
6.针对上述问题，本发明公开了一种基于视觉里程计的松耦合多传感器融合定位算法，提供一种定位误差小、精度高，可实时校正，可靠性高且能适应复杂场景的定位算法。


技术实现要素：

7.本发明的目的在于提供一种基于视觉里程计的松耦合多传感器融合定位算法，以解决上述背景技术中提出的技术问题。为实现上述目的，本发明提供如下技术方案：一种基于视觉里程计的松耦合多传感器融合定位算法，包括：
8.步骤s1：获取从slo
‑
vlp计算得到的移动终端观测位姿s
t
(x
t
,y
t
,z
t
，θ
t
)，其中偏航角θ
t
通过激光雷达匹配器进行校正；
9.步骤s2：将步骤s1中由slo
‑
vlp计算得到的移动终端位姿作为自适应蒙特卡罗定位算法中粒子的初始化位姿，由自适应蒙特卡罗定位算法估计的位姿记作(x
i
,y
i
)；
10.步骤s3：获取由视觉里程计基于orb特征匹配的移动终端的位姿p
t
；
11.步骤s4：将步骤s1、s2获得的观测位姿作为卡尔曼滤波算法的观测值以校正由步骤s3获得的视觉里程计预测的移动终端的位姿p
t
。
12.优选的，所述步骤s1中，获取从slo
‑
vlp计算并得到校正后的移动终端观测位姿s
t
(x
t
,y
t
,z
t
，θ
t
)的计算方法包括：
13.步骤s11：调制led灯光，用于传递led灯体id以及获得与所述灯体id相对应的led灯体位姿信息；
14.步骤s12：获取某一led灯体在世界坐标系中的位姿(x
i
，y
i
,z
i
)，并计算位于移动终端上的相机图像中心在世界坐标系中的位姿p(x
s
，y
s
,z
s
)；
15.步骤s13：通过位于移动终端上的相机的坐标系与该移动终端坐标系的坐标系变换关系计算得到该移动终端的姿态p
t
(x
t
,y
t
,z
t
)；
16.步骤s14：由移动终端上的视觉里程计获取估计的偏航角γ
odom
、激光雷达匹配器检测的的相对方向变换关系γ
lidar
→
map
,可以得到校正后的偏航角θ
t
＝γ＝γ
odom
γ
lidar
→
map
；并由步骤s12获得的p可得到移动终端的slo
‑
vlp位姿s
t
(x
t
,y
t
,z
t
，θ
t
)。
17.优选的，所述步骤s2中，获取由自适应蒙特卡罗定位算法得到的位姿(x
i
,y
i
)，其方法包括：
18.步骤s21：将权利要求2获得的slo
‑
vlp位姿s
t
作为自适应蒙特卡罗定位算法滤波器粒子的初始化位姿；
19.步骤s22：把视觉里程计传感器获取的位姿信息和激光雷达数据输入到自适应蒙特卡罗定位算法定位器，输出移动终端在地图坐标系上的位姿(x
i
,y
i
)。
20.优选的，所述步骤s3中，获取由视觉里程计基于orb特征匹配的移动终端的位姿的方法包括：
21.步骤s31：特征检测：从相机拍摄的图片中进行兴趣点检测，提取关键点；
22.步骤s32：计算由步骤s31获得的关键点的描述子，根据描述子进行特征点匹配；
23.步骤s33：根据对极几何约束，并由八点法估计出本质矩阵e，并由e通过奇异值分解svd恢复旋转矩阵和平移矩阵r和t；
24.步骤s34：由步骤s33获得的r,t估计相机的相对运动，从而估计得到相机的当前位姿；
25.步骤s35：由相机坐标系与移动终端坐标系的tf变换关系可以计算得到移动终端的位姿
26.优选的，所述步骤s4中，步骤s1、s2获得的观测位姿作为卡尔曼滤波算法的观测值以校正由步骤s3获得的视觉里程计预测的移动终端的位姿的方法如下：
27.由slo
‑
vlp计算的观测位姿和激光雷达的观测位姿可以获得位姿观测值z
t
,并通过卡尔曼滤波器对所述步骤s3获得的位姿进行校正，得到最终的移动终端的位姿p
t
。
28.与现有技术相比，本发明的有益效果是：
29.1.由视觉里程计估计的移动终端的位姿的误差明显降低，并且可以应用在车轮里
程计无法工作的特殊环境，定位精度明显提高。
30.2.本发明通过为激光slam技术提供比较可靠的姿态观测值，可以校正视觉里程计、激光雷达等传感器的累积误差，提供更为精准的位姿，增强算法的鲁棒性。
31.3.通过多传感器的融合，本发明可以弥补独立传感器的不足，并提供更可靠的姿态估计；在led短缺/故障时为终端提供准确而可靠的定位，算法的鲁棒性使移动终端不仅适用于照明设施完善的场合，在信源短缺的情况下也能提供高精度的定位服务，为路径规划和自主导航打下坚实基础。
附图说明
32.图1为本发明说明基于视觉里程计的松耦合多传感器融合定位算法的总体框；
33.图2为世界、相机和图像坐标系之间的变换示意图；
34.图3为由视觉里程计获取移动终端位姿的流程图。
具体实施方式
35.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
36.请参阅图1、图2、图3，本发明提供了一种基于视觉里程计的松耦合多传感器融合定位算法，该方法包括：
37.步骤s1：获取从slo
‑
vlp计算得到的移动终端观测位姿(x
t
,y
t
,z
t
，θ
t
)，而其方向角θ
t
通过激光雷达匹配器(amcl)校正。
38.步骤s11：获取某一led灯体在世界坐标系中的位姿(x
i
，y
i
,z
i
)，并计算终端相机图像中心在世界坐标系中的位姿p(x
s
，y
s
,z
s
)。其方法包括：
39.步骤s111：调制led灯光，用于传递led灯体id以及获得与所述灯体id相对应的led灯体位姿信息。调制led灯光是指让led灯的发光芯片按照一定的频率闪烁，闪烁频率对应于调制信号；在调制的过程中对每个led灯体调制分配一个id编码，同时调制入led灯体的位姿信息。
40.步骤s112：获取移动终端捕获的某一led灯体的感光区域roi并识别其身份(led
‑
id)，并从注册的灯光数据库(预制的led地标地图)检索led的3d位姿p
led
(x
i
，y
i
,z
i
)。
41.步骤s113：由步骤s112获取的p
led
(xi，yi,zi)可以计算该led灯体在相机坐标系中获得的位姿为(x
i
,y
i
)，其方法包括：
[0042][0043]
λ为所述移动终端与所述led灯体之间在z轴上的高度差，根据计算的所述led像平面的直径和所述led灯体实际直径的比例关系、三角形性质及成像原理，求得所述移动终端与所述led灯体之间在z轴上的高度差，
[0044]
即
[0045]
其中d为led的直径，d
pixel
为相机的像素距离，p
d
是像素距离到物理距离的转换，f为相机的焦距。
[0046]
f(焦距),dx/dy(像素距离),u0/v0(图像中心像素)组成了特征矩阵k；
[0047]
t是平移矩阵，相当于移动终端在三维世界坐标系中的位姿；
[0048]
r是从世界坐标系到相机坐标系的旋转矩阵。
[0049][0050]
当只考虑二维平面时，α,β视为常数，而γ由里程计传感器获取。
[0051]
步骤s114：由步骤s112得到的p
led
(xi，yi,zi)，可以计算:
[0052]
zs＝zi
‑
λ
ꢀꢀ
(2)
[0053]
步骤s115：相机中心在世界坐标系的位姿计算方法包括：
[0054]
由
[0055]
得到ps[xs,ys,zs,γ]，k为由f(焦距),dx/dy(像素距离),u0/v0(图像中心像素)组成的特征矩阵。
[0056]
步骤s115：移动终端在世界坐标系的位姿st可以由以下方法获取：
[0057][0058]
其中r
x
，r
y
，r
z
和t
x
，t
y
，t
z
是从相机坐标系到移动终端的base_link坐标系的坐标变换的旋转和平移的系数。
[0059]
步骤s12:由slo
‑
vlp和里程计输入的偏航角θ计算得到的γ，通过激光雷达匹配器(amcl)进行tf坐标关系校正，其方法包括：
[0060]
激光雷达通过将扫描数据与以前的扫描相匹配，获得两个时刻之间的相对位姿变换关系，以方向γ的坐标变换关系来修正里程计传感器得到的移动终端的方向，具体方法如下：
[0061]
θ
t
＝γ＝γ
odom
γ
lidar
→
map
[0062]
步骤s2：将步骤s1得到的slo
‑
vlp位姿作为自适应蒙特卡罗定位算法(amcl)粒子的初始化位姿，得到由amcl估计的位姿(xi,yi)。其方法包括：
[0063]
步骤s21：将步骤s1获得的slo
‑
vlp姿态st作为amcl滤波器粒子的初始化位姿；
[0064]
步骤s22：把里程计传感器获取的位姿信息和激光雷达数据输入到amcl定位器，输出终端在地图上的位姿；
[0065]
步骤s3：获取由视觉里程计基于orb特征匹配的移动终端的位姿.
[0066]
步骤s31：特征检测：从相机拍摄的图片中进行兴趣点检测，提取关键点,方法如
下：采用fast(features from accelerated segment test)算法来检测关键点。这个定义基于关键点周围的图像灰度值，检测候选关键点周围一圈的像素值，如果候选点周围领域内有足够多的像素点与该候选点的灰度值差别够大，则认为该候选点为一个关键点。
[0067]
步骤s32：计算由步骤s31获得的关键点的描述子，并根据描述子进行特征点匹配，方法如下：
[0068]
步骤s321：根据由步骤s31获得的关键点计算brief描述子：
[0069]
使用矩法来确定fast特征点的方向。也就是说通过矩来计算特征点以r为半径范围内的质心，特征点坐标到质心进行连线，求出该直线与横坐标轴的夹角，即为该特征点的方向。具体方法如下：
[0070]
矩定义：m
pq
＝∑
x,y
x
p
y
q
i(x,y)
[0071]
质心定义：
[0072]
然后求取向量oc的方向，同时如果把x,y的范围保持在[
‑
r,r]之间(r为该特征点邻域的半径)，以特征点为坐标原点，则得到的方向角(即fast特征点的方向)为：θ＝arctan(m
01
,m
10
)。
[0073]
然后从关键点p周围(半径为r的圆)随机挑选n个点对，对于每个点对中的两个点，如果前一个点的灰度值大于后一个点，则取1，反之取0，由此计算得到关键点p的描述子。
[0074]
步骤s322：:使用hamming距离对两幅图像中的brief描述子进行匹配。
[0075]
步骤s33：根据对极几何约束，假设p2为相机位姿运动r(旋转),t(平移)后与前一帧特征点p1匹配的特征点，s1,s2是空间点深度，p为世界坐标系下某点的坐标，k为内参，在标定好的相机下，k已知。则有:
[0076]
s1p1＝kp
ꢀꢀ
(1)
[0077]
s2p2＝k(rp t)
ꢀꢀ
(2)
[0078]
在使用齐次坐标时，一个向量将等于它自身乘上任意的非零常数。这通常用于表达一个投影关系。例如s1p1和p1成投影关系，它们在齐次坐标的意义下是相等的。我们称这种相等关系为尺度意义下相等，记作：则(1)(2)式可以写成：
[0079][0080]
取x
14
＝k
‑1p1,x2＝k
‑1p2,x1,x2是两个像素点的归一化平面上的坐标，代入上式得：
[0081]
两边同乘t^(相当于两侧同时与t做外积),并两侧同乘x
2t
，
[0082]
化简得到：x
2t
t^rx1＝0
[0083]
令e＝t^r，e称为本质矩阵
[0084]
根据八点法估计出本质矩阵e，并由e通过奇异值分解(svd)恢复旋转向量和平移向量r和t.
[0085]
步骤s34：由步骤s33获得的r,t估计相机的相对运动，计算得到相机的位姿：
[0086][0087]
步骤s35：由相机坐标系与移动终端坐标系(base_link)的tf变换关系可以计算得
到移动终端的位姿
[0088][0089]
其中r
x
，r
y
，r
z
和t
x
，t
y
，t
z
是从相机坐标系到移动终端的base_link坐标系的坐标变换的旋转和平移的系数。
[0090]
步骤s4：将步骤s1、s2得到的观测位姿作为ekf算法的输入值以纠正由步骤s3获得的视觉里程计计算的移动终端位姿估计值其方法包括：
[0091]
步骤s41：计算卡尔曼增益k＝p
t|t
‑1h
tt
(h
t
p
t|t
‑1h
tt
r
t
)
‑1ꢀꢀ
(3)
[0092]
p
t|t
‑1＝fp
t
‑1f
t
,其中
[0093]
其中h
t
是观测矩阵，r
t
是观测噪声的误差协方差矩阵，且假设观测噪声服从正态分布。
[0094]
步骤s42：以步骤s41获得的卡尔曼增益k去更新位姿向量和协方差矩阵，具体做法如下：
[0095][0096]
p
t
＝p
t|t
‑1‑
kh
t
p
t|t
‑1ꢀꢀꢀꢀꢀꢀ
(5)
[0097]
以上中，为对现状态真实值x
t
的估计；为基于t
‑
1次对t次的预估计；假设输入噪声和观测噪声均服从正态分布，q为输入噪声的协方差，r为观测噪声的协方差,p
t
为协方差矩阵。
[0098]
至此，通过视觉里程计预测方程式和多传感器融合的估计姿态可以利用卡尔曼滤波算法更新预测姿态和纠正视觉里程计的累积误差。