基于视觉惯导融合的移动机器人地图构建方法及相关设备

1.本发明属于同步定位与地图创建(simultaneous location and mapping，slam)技术领域，涉及基于视觉惯导融合的移动机器人地图构建方法及相关设备。


背景技术：

2.同步定位与地图创建(simultaneous location and mapping，slam)的核心问题是，要求机器人在一个陌生的环境中，首先要探索环境从而了解环境(构建地图)，同步运用地图追踪机器人在该环境中的位置(定位)。对slam问题的传统解决方法主要是基于数学概率方法，其中，卡尔曼滤波器、粒子滤波器和极大期望算法等是机器人slam问题的基本解决方法。虽然这些传统的slam算法中仍使用激光测距和声呐测距采集信息，但是这些传感器采集信息，往往得到的数据不精确且使用有一定的局限性，更多情况下激光测距传感器已被视觉传感器所替代。
3.然而传统视觉slam算法仅依靠单一传感器已无法满足科研的需求，也无法在复杂快速运动的环境中及时捕捉机器人的运动信息并反馈给主控机进行运动补偿。科研工作者开始融合两种不同传感器的信息意图互补两者的不足，期望惯性测量单元(inertial measurement unit，imu)可以在快速运动时替代相机捕捉相机运动的物理信息提高视觉slam定位精度，而相机也可以弥补imu在慢速运动下因使用时间过长使得累计误差漂移的问题。
4.但目前的融合方法在构建地图时，并未实现实时对运动的姿态趋势进行分析，不能确定惯导融合的局部实时最优状态，因此相机和imu数据无法准确融合，导致读取当前地图点位姿信息效率低、误差大。同时具有后端计算量较大，运行时间长，处理误差实时性差等问题。


技术实现要素：

5.本发明的目的在于提供基于视觉惯导融合的移动机器人地图构建方法，以解决现有技术中相机和imu数据无法准确融合导致读取当前地图点位姿信息效率低、误差大以及后端处理误差实时性差的技术问题。
6.所述的基于视觉惯导融合的移动机器人地图构建方法，所述方法包括：
7.步骤s1，根据视觉姿态和惯性姿态融合状态确定融合达成度，完成视觉初始化、视觉惯性校准对齐以及惯性初始化；
8.步骤s2，构建情形评估函数实时分析姿态趋势；
9.步骤s3，确定融合状态局部期望值并输出该状态：通过评估函数值与阈值比较判断此时机器人的运动状态观测数据是否具有高确定性，同时增加时间阈值设定双重约束评估状态以确定是否终止视觉惯性联合初始化并输出当前状态；
10.步骤s4，根据公共地图点稀疏程度将关键帧分为强共视关键帧和弱共视关键帧；
11.步骤s5，根据分级结果将强共视关键帧送入滑动窗口并利用帧间位姿密集连续性
优化强共视关键帧，借助观测地图点的联系对自感知地图进行绘制和校正。
12.优选的，所述步骤s1中，imu测量值由真实值、随机游走偏差和白噪声三个部分组成，切加速度计仍受到重力加速度的影响，将imu运动状态变量利用中值法离散化以方便相机跟imu进行状态融合，离散化的运动状态变量算式如下：
[0013][0014]
p
pre
、v
pre
和q
pre
分别代表imu位置、速度和姿态四元数的初始值；和分别代表第i和i+1个时刻下imu在世界坐标系下的加速度；和分别代表第i和i+1个时刻下imu在世界坐标系下的角速度；为内积的运算法则；和表示在第i和i+1个时刻的时间段里imu加速度偏差和角速度偏差的变化量；是从imu坐标系转移到世界坐标系下的旋转矩阵，δt为相邻时刻的间隔时间；
[0015]
上述运动状态变量算式能计算出在i+1个时刻下的计算结果为z
i+1
，z1为初始值，在第一次计算时直接利用相机采集的信息确定，之后的计算利用imu上一时刻的信息计算得到当前时刻的观测数据。
[0016]
优选的，所述步骤s1中，用z
f
表示机器人的运动状态观测数据，则存在z
f
＝{z1,z2,...,z
n
}，z
i
为z
f
中具体的第i个时刻下的观测数据；若机器人的运动状态观测数据z
f
＝{z1,z2,...,z
n
}服从概率分布f(z
f
；y)，其中y是需要求解的机器人状态变量目标函数，得到的状态估计函数如下式：
[0017][0018]
构建机器人状态变量目标函数y来度量机器人观测状态数据并估计观测的位姿随时间变化的趋向的置信度，机器人运动状态置信度表达如下式：
[0019][0020]
其中ω(z
f
；y)为机器人运动状态观测数据z
f
的置信度，e(
·
)为计算机器人运动状态观测数据z
f
的期望，s(
·
)为计算机器人运动状态观测数据z
f
的标准差，var(
·
)为计算机器人运动状态观测数据z
f
的方差，通过机器人运动状态置信度确定融合达成度。
[0021]
优选的，所述步骤s2中，定义视觉惯性观测样本情形评估函数如下式：
[0022][0023]
其中，f
λ
(z
f
)为机器人运动状态观测数据情形评估函数，ω(z
f
；y)是从关于评估机器人运动状态观测数据z
f
的位置、速度、旋转四元数(尺度、重力矢量)和偏差等相关状态获得的信息矩阵置信度，为机器人运动状态中某个关键帧f观测数据z
f
对应的协方差矩阵，f为运行过程中某个图像帧，δτ是与图像帧相关的图像信息矩阵。
[0024]
优选的，所述步骤s2中，构建机器人运动状态观测数据z
f
情形评估函数的性能度量如下式
[0025][0026]
其中f
det
(z
f
)为移动机器人在运动时拍摄的某个包含惯性测量单元状态信息和视觉状态信息的图像帧f的观测数据情形性能度量，ω(z
f
；y)为机器人运动状态中某个图像帧f状态观测数据置信度，det(
·
)是指求解矩阵的行列式，为机器人运动状态中某个图像帧f观测数据z
f
对应的协方差矩阵，δτ为与图像帧有关的图像信息矩阵，log(
·
)为对数函数。
[0027]
优选的，所述步骤s3中，定义情形评估函数值小于阈值s
th
来衡量初始化状态具有较高的置信度，认为此时的融合状态具有高确定性，终止初始化，将相应的预积分和初始化数据融合，进入下一步的视觉惯性实时优化；同时在初始化过程中额外增加一个时间阈值设定，若时间阈值内情形评估函数未达到较高的置信度，在到达时间阈值时直接终止初始化，将当前初始化结果送入下一阶段实时优化。
[0028]
优选的，所述步骤s4中，构建基于共视约束的关键帧局部优化概率图模型，按共视强弱对关键帧进行分级处理，将帧间观测信息及关键帧上地图点等信息作为约束项优化强共视关键帧；共视约束概率图模型可以看成一个无向加权图，其联合概率分布即可分解为
多个团q势能函数的乘积，如下式：
[0029][0030]
其中f
k
＝{f
k1
,f
k2
,...,f
kn
}为关键帧，p(f
k
)为联合概率，为包含共同观测地图点的关键帧的稀疏权重函数，ψ为正则化因子，通过正则化满足联合概率的有效性；其中f
k
＝{f
k1
,f
k2
,...,f
kn
}为关键帧，p(f
k
)为联合概率，为包含共同观测地图点的关键帧的稀疏权重函数。
[0031]
优选的，所述步骤s5中，滑动窗口优化时，仅优化强共视关键帧，同时保留弱共视关键帧的位姿信息作为约束项；视觉位姿估计误差项、imu预积分误差项分量构建一个目标函数中，最小化重投影误差和imu误差优化状态量，目标函数如下式所示：
[0032][0033]
其中，e
reproj
为两相邻关键帧之间的重投影误差；e
imu
为imu在时间段δt的误差，由视觉和imu共同影响下的偏差测量组成。e
observe
为相机观测误差，为相机关于运行状态z
f
＝{z1,z2,...,z
n
}的信息矩阵，e
p
、e
v
和e
q
分别为imu在运行时的位置、速度和姿态四元数的误差，为imu关于运行状态z
f
＝{z1,z2,...,z
n
}的信息矩阵，ρ(
·
)为鲁棒核函数，e
b
为imu某个时间点的随机偏差集合，为相机和imu共同影响下关于运行状态z
f
＝{z1,z2,...,z
n
}的信息矩阵。
[0034]
本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如上所述的基于视觉惯导融合的移动机器人地图构建方法的步骤。
[0035]
本发明还提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上所述的基于视觉惯导融合的移动机器人地图构建方法的步骤。
[0036]
本发明的技术效果：1、本发明使用基于费雪信息矩阵为主导的双重视觉惯导初始化方法，针对状态观测值构建观测样本情形评估函数，评估状态误差信息矩阵，判断视觉惯导融合的局部实时最优状态作为终止条件，有效保证了相机和imu数据之间能准确融合，系统读取当前地图点位姿信息效率高、误差小。为防止评估时间过长，增加时间阈值设定双重约束评估状态观测变化趋势，缩短初始化时间，提高系统鲁棒性。
[0037]
2、使用基于包含共同观测地图点的稀疏程度分级关键帧，优化强共视关键帧，保留弱共视关键帧的地图点信息进入滑动窗口作为约束项，与重投影误差和imu误差一同约束强共视关键帧上的位姿观测状态。这样在保证对误差处理可靠性的前提下，减少了计算量，大大缩短耗时，解决了后端处理误差实时性差的问题。
附图说明
[0038]
图1为基于双重阈值影响的视觉惯导移动机器人融合地图构建方法的流程示意图。
[0039]
图2为构建视觉惯导初始化阶段中imu初始化的流程示意图。
[0040]
图3为imu初始化情形评估的流程示意图。
[0041]
图4为本发明在视觉惯性联合初始化阶段的偏差变化。
[0042]
图5为本发明滑动优化阶段采用共视分级方法的效果图。
[0043]
图6为本发明最终的定位效果图。
具体实施方式
[0044]
下面对照附图，通过对实施例的描述，对本发明具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。
[0045]
根据相机传感器和惯性测量单元两种传感器对于外界和自在的环境信息读取，并构建两种信息相互交换的实现。利用该原理对现有的slam技术进行改进得到本发明方案。
[0046]
实施例一：
[0047]
本发明提供了基于视觉惯导融合的移动机器人地图构建方法，应用该方法的系统同时包括有模拟视觉系统和模拟惯性系统。
[0048]
模拟视觉系统包括：两个视觉感知标定器、信号调理电路、模数(ad)转换器。其中，两个视觉标定器对称安置，主控芯片优选为stm32主控芯片。
[0049]
模拟惯性系统包括：陀螺仪、加速度计、信号调理电路、模数模数(ad)转换器。其中，惯性测量单元选择集成性较强的mpu6050芯片，内含三轴陀螺仪、三轴加速度计以及三轴磁力计。
[0050]
如图1
‑
3所示，本方法具体包括；
[0051]
步骤s1，根据视觉姿态和惯性姿态融合状态确定融合达成度，完成视觉初始化、视觉惯性校准对齐以及惯性初始化。
[0052]
所述步骤s1中，首先定义imu测量值(包括陀螺仪和加速度计)由真实值、随机游走偏差和白噪声三个部分组成，除此之外，加速度计仍受到重力加速度的影响。为方便相机跟imu的状态融合，将imu运动状态变量利用中值法离散化，得到状态变量表示如下式：
[0053][0054]
p
pre
、v
pre
和q
pre
分别代表imu位置、速度和姿态四元数的初始值；和分别代表第i和i+1个时刻下imu在世界坐标系下的加速度；和分别代表第i和i+1个时刻下imu在世界坐标系下的角速度；为内积的运算法则；和表示在第i和i+1个时刻的时间段里imu加速度偏差和角速度偏差的变化量；是从imu坐标系转移到世界坐标系下的旋转矩阵，δt为相邻时刻的间隔时间。
[0055]
上述运动状态变量算式能计算出在i+1个时刻下的计算结果为z
i+1
，z1为初始值，在第一次计算时直接利用相机采集的信息确定，之后的计算利用imu上一时刻的信息计算得到当前时刻的观测数据。
[0056]
用z
f
表示机器人的运动状态观测数据，则存在z
f
＝{z1,z2,...,z
n
}，z
i
为z
f
中具体的第i个时刻下的观测数据。依据步骤s1中状态变量的算式能得到在i+1个时刻下的计算结果为z
i+1
，而z1为初始值，在第一次计算时直接利用相机采集的信息确定，之后的计算利用imu上一时刻的信息计算得到当前时刻的观测数据。若机器人的运动状态观测数据z
f
＝{z1,z2,...,z
n
}服从概率分布f(z
f
；y)，y是步骤s1得到的需要求解的机器人状态变量目标函数，得到的状态估计函数如下式
[0057][0058]
构建机器人状态变量目标函数y来度量机器人观测状态数据并估计观测的位姿(位置、速度、旋转四元数以及偏差)随时间变化的趋向的置信度，机器人运动状态置信度表达如下式
[0059][0060]
其中ω(z
f
；y)为机器人运动状态观测数据z
f
的置信度，e(
·
)为计算机器人运动状态观测数据z
f
的期望，s(
·
)为计算机器人运动状态观测数据z
f
的标准差，var(
·
)为计算机器人运动状态观测数据z
f
的方差，通过机器人运动状态置信度确定融合达成度。
[0061]
步骤s1完成视觉初始化、视觉惯性校准对齐以及惯性初始化，具体包括：
[0062]
s1.1视觉初始化。相机读取视觉信息并计算重投影误差；
[0063]
s1.2视觉惯导对齐配准。同步相机与imu采集频率，保证采集图像帧的同时能采集到该图像帧的物理位姿信息；
[0064]
s1.3陀螺仪偏置求解。最小化预积分值与视觉旋转差值求解陀螺仪偏置；
[0065]
s1.4尺度、速度与重力加速度初始化。构建尺度、加速度与重力加速度线性方程进行求解；
[0066]
s1.5重力加速度提炼。通过在重力向量切面上不断迭代微调重力向量；
[0067]
s1.6设置世界坐标系。根据初始化求得的重力矢量设置世界坐标系。
[0068]
步骤s2，根据融合情况实时分析姿态趋势。
[0069]
定义视觉惯性观测样本情形评估函数如下式：
[0070][0071]
其中，f
λ
(z
f
)为机器人运动状态观测数据情形评估函数，ω(z
f
；y)是从关于评估机器人运动状态观测数据z
f
的位置、速度、旋转四元数(尺度、重力矢量)和偏差等相关状态获得的信息矩阵置信度。为机器人运动状态中某个关键帧f观测数据z
f
对应的协方差矩阵，f为运行过程中某个图像帧，δτ是与图像帧相关的图像信息矩阵。
[0072]
为了最小化机器人运动状态观测数据信息矩阵置信度的局域半径，需要最小化信息矩阵协方差的行列式，由于得到的最小化置信度局域半径与最大化信息矩阵的对数行列式相同，从而得到机器人运动状态观测数据情形评估函数的性能度量：
[0073][0074]
其中f
det
(z
f
)为移动机器人在运动时拍摄的某个包含惯性测量单元状态信息和视觉状态信息的关键帧f的观测数据情形性能度量，det(
·
)是指求解运动状态观测数据信息
矩阵的行列式，为机器人运动状态中某个关键帧f观测数据z
f
对应的协方差矩阵，δτ为与图像帧有关的图像信息矩阵，log(
·
)为对数函数。
[0075]
步骤s3，确定融合状态期望值并输出该状态。
[0076]
定义情形评估函数值小于阈值s
th
来衡量初始化状态具有较高的置信度，认为此时的具有高确定性，终止初始化，将相应的预积分和初始化数据融合，进入下一步的视觉惯性实时优化。为避免因情形评估函数计算时间过长使得imu误差累积，故在初始化过程中额外增加一个15s的时间阈值设定，若在15s内未达到较高的置信度，在第15s时直接终止初始化，将当前初始化结果送入下一阶段实时优化。
[0077]
步骤s3具体包括：
[0078]
s3.1开始视觉惯性联合初始化；
[0079]
s3.2判断初始化时间是否达到设定的时间阈值。若未达到，执行步骤s3.3；若达到，执行步骤s3.5；
[0080]
s3.3构建最差情形评估函数实时计算状态观测值的状态期望值；
[0081]
s3.4判断状态期望值ω(z)是否达到状态期望阈值s
th
。若未达到，执行步骤s3.1；若达到，执行步骤s3.5；
[0082]
s3.5结束视觉惯性联合初始化。
[0083]
步骤s4，根据公共地图点稀疏程度将关键帧分为强共视关键帧和弱共视关键帧。
[0084]
构建基于共视约束的关键帧局部优化概率图模型，按共视强弱对关键帧进行分级处理，保留强共视关键帧，忽略弱共视关键帧，将帧间观测信息及关键帧上地图点等信息作为约束项优化强共视关键帧。此时共视约束概率图模型可以看成一个无向加权图，其联合概率分布即可分解为多个团q势能函数(因子)的乘积，如下式：
[0085][0086]
其中f
k
＝{f
k1
,f
k2
,...,f
kn
}为关键帧，p(f
k
)为联合概率，ψ为正则化因子，通过正则化满足联合概率的有效性。其中f
k
＝{f
k1
,f
k2
,...,f
kn
}为关键帧，p(f
k
)为联合概率，ψ为正则化因子，通过正则化满足联合概率的有效性，为包含共同观测地图点的关键帧的稀疏权重函数。
[0087]
步骤s5，根据分级结果将强共视关键帧送入滑动窗口并利用帧间位姿密集连续性优化强共视关键。
[0088]
滑动窗口优化时，仅优化强共视关键帧，同时保留弱共视关键帧的位姿信息作为约束项。视觉位姿估计误差项、imu预积分误差项分量构建一个目标函数中，最小化重投影误差和imu误差优化状态量，目标函数如下式所示：
[0089][0090]
其中，e
reproj
为两相邻关键帧之间的重投影误差；e
imu
为imu在时间段δt的误差，由视觉和imu共同影响下的偏差测量组成。e
observe
为相机观测误差，为相机关于运行状态z
f
＝{z1,z2,...,z
n
}的信息矩阵，e
p
、e
v
和e
q
分别为imu在运行时的位置、速度和姿态四元数的误差，为imu关于运行状态z
f
＝{z1,z2,...,z
n
}的信息矩阵，ρ(
·
)为鲁棒核函数，e
b
为imu某个时间点的随机偏差集合，为相机和imu共同影响下关于运行状态z
f
＝{z1,z2,...,z
n
}的信息矩阵。
[0091]
imu测量误差包括传统的位姿误差e
p
、e
v
和e
q
及偏差e
b
，各误差分量如下式所示：
[0092][0093][0094][0095]
e
b
＝δ(b
g
,b
a
)
(i,j)
[0096]
其中，为imu状态在时刻i从自身坐标系(body)转移到世界坐标系(world)下的旋转矩阵，和是在imu预积分阶段计算陀螺仪残差得出的位置、速度和旋转四元数雅克比矩阵，是在imu预积分阶段计算加速度残差得出的速度雅克比矩阵，δt
ij
为从时刻i到时刻j之间的时间段，表示时刻j下的角速度偏差，为imu在时刻i从世界坐标系转到时刻j自身坐标系的位置变化值，为imu在时刻i从世界坐标系转到时刻j自身坐标系的速度变化值，为imu状态在时刻j从世界坐标系(world)转移到自身坐标系(body)下的旋转矩阵。δr
(i,j)
为时刻j与时刻i之间的旋转四元数差值，δv
(i,j)
为时刻j与时刻i之间的速度变化值，δp
(i,j)
为时刻j与时刻i之间的位置变化值，δ(b
g
,b
a
)
(i,j)
为时刻j与时刻i之间的偏差变化值。
[0097]
最后，利用优化好的强共视关键帧储存层、位姿信息储存层、视觉感知储存层进行信息交融，对自感知地图进行绘制或对之前的自感知地图进行修正与删减。
[0098]
下面以结合具体实验对上述构建双重阈值变化视觉惯性里程计的过程进行说明。
[0099]
图4给出了本发明在视觉惯性联合初始化阶段的偏差变化。本发明初始化算法能实现偏差的快速收敛，构建了情形评估函数实时评估当前偏差的状态，因此精度和收敛性高于其他方法。加速度收敛过程整体上与陀螺仪偏置收敛过程类似，最终趋向定值，仅个别估计值存在误差。设定情形评估函数可以满足待估计参数收敛的要求，能够使得imu达到快速收敛的目的，在一定程度上有效的抑制了imu偏差的增大有效地节约了初始化时间，减少了imu测量值误差，提高了后续阶段的优化精度。
[0100]
图5给出了本发明滑动窗口优化阶段采用共视分级方法的效果图(采用公开数据集euroc数据集)，对于mh_01_easy数据集和mh_03_medium数据集来说，经过滑动窗口优化以后，位置漂移最大值在
±
100mm左右，对于复杂数据集(如mh_05_difficult)位置误差在
±
200mm以内。
[0101]
表1
[0102][0103]
euroc测试中运行时间的比较如表1所示，在测试中，从表中可以得出，在不同的环境下，本发明运行时间跟随效率达到了90％，能够及时计算相机的图像信息并传送给主控机进行实时运动控制，取得较好的跟随精度和效率。
[0104]
表2
[0105][0106]
euroc数据集下轨迹精度的比较如表2所示。从表中数据分析可得，针对环境的不同，轨迹误差rmse也会不同，总体误差不高于0.14m。其中，在数据集v1_01_easy序列上的测试轨迹精度误差可达0.027m。
[0107]
本发明最终的定位效果图如图6所示。
[0108]
当存在复杂未知环境时，使用基于双重阈值影响的移动机器人视觉惯导融合地图构建方法具有明显的实时性优势，提高定位精度，同时利用共视分级关系减少了运行时间，增强了跟随效率。
[0109]
实施例二：
[0110]
与本发明实施例一对应，本发明实施例二提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时依照实施例一的方法实现以下步骤：
[0111]
步骤s1，根据视觉姿态和惯性姿态融合状态确定融合达成度，完成视觉初始化、视觉惯性校准对齐以及惯性初始化。
[0112]
步骤s2，构建情形评估函数实时分析姿态趋势。
[0113]
步骤s3，确定融合状态局部期望值并输出该状态。
[0114]
步骤s4，根据公共地图点稀疏程度将关键帧分为强共视关键帧和弱共视关键帧。
[0115]
步骤s5，根据分级结果将强共视关键帧送入滑动窗口并利用帧间位姿密集连续性优化强共视关键帧，借助观测地图点的联系对自感知地图进行绘制和校正。
[0116]
上述存储介质包括：u盘、移动硬盘、只读存储器(rom，read
‑
only memory)、随机存取存储器(ram，random access memory)、光盘等各种可以存储程序代码的介质。
[0117]
上述关于计算机可读存储介质中程序执行后实现步骤的具体限定可以参见实施例一，在此不再做详细说明。
[0118]
实施例三：
[0119]
与本发明实施例一对应，本发明实施例三提供一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现以下步骤：
[0120]
步骤s1，根据视觉姿态和惯性姿态融合状态确定融合达成度，完成视觉初始化、视觉惯性校准对齐以及惯性初始化。
[0121]
步骤s2，构建情形评估函数实时分析姿态趋势。
[0122]
步骤s3，确定融合状态局部期望值并输出该状态。
[0123]
步骤s4，根据公共地图点稀疏程度将关键帧分为强共视关键帧和弱共视关键帧。
[0124]
步骤s5，根据分级结果将强共视关键帧送入滑动窗口并利用帧间位姿密集连续性优化强共视关键帧，借助观测地图点的联系对自感知地图进行绘制和校正。
[0125]
上述关于计算机设备实现步骤的具体限定可以参见实施例一，在此不再做详细说明。
[0126]
需要说明的是，本发明的说明书附图中的框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与获得机指令的组合来实现。
[0127]
上面结合附图对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的发明构思和技术方案进行的各种非实质性的改进，或未经改进将本发明构思和技术方案直接应用于其它场合的，均在本发明保护范围之内。