基于位姿融合优化的室内地图构建方法、系统及存储介质与流程

1.本发明涉及一种室内地图构建方法，尤其涉及一种在室内环境卫星信号缺失的情况下， 在贫特征、高动态、弱光影的场景中，基于kinect/imu位姿融合优化的室内地图构建的 方法、系统及存储介质。


背景技术：

2.精确高效的位姿估计与建图系统是机器人实现运动控制与路径规划的关键，在室内环境 卫星信号缺失的情况下，同时定位与地图构建(simultaneous localization and mapping， slam)技术是解决机器人环境认知与定位导航的有效手段之一。slam是指搭载特定传感 器的主体(一般为机器人等)，在没有环境先验信息的情况下，在运动中建立环境模型，同 时估计自身运动的过程。根据搭载传感器的不同，slam可以分为视觉slam、激光slam 和多传感器融合slam。基于视觉传感器的slam(visual slam)主要由传感器信息读取、 视觉里程计、后端优化、回环检测和建图等部分组成。随着计算机视觉、图像处理、人工智 能等技术的发展，视觉slam的可用性不断得到挖掘，并且尝试融合其他传感器信息用于 构建更加稳健的视觉slam系统。随着rgb
‑
d相机的出现，许多学者提出了基于kinect 的视觉slam方法，并且运用到三维建模、人体识别、系统交互等领域。
3.但是，视觉slam方法在贫特征、高动态、弱光影的场景时，算法受到很多限制。 imu传感(惯性传感器)由于自身的特点，使得它与视觉slam方法形成了较好的互补性。 例如，视觉传感器在高速运动的状态下，基于特征匹配的位姿估计成功率较低，而imu依 赖自身加速度和陀螺仪仍然能够保持较精确的位姿估计；视觉传感器可以有效限制运动过程 中imu存在的累计漂移误差；相机数据与imu数据的融合可以提高位姿估计算法的鲁棒性。 这种结合相机和imu的方法被称为视觉惯性slam(visual
‑
inertial slam，vi
‑
slam)。与 视觉slam相似，vi
‑
slam框架可以分为基于滤波和基于优化两种。基于滤波的vi
‑
slam 方法将状态和参数作为在线计算的一部分，输入不同的滤波框架，更新校准并输出实时位姿。 基于优化的vi
‑
slam方法也被称为平滑方法，它们将imu数据作为先验信息，提取匹配图 像特征，再根据图像特征定义重投影误差，构造代价函数，解算机器人位姿。很多学者对此 方面进行了研究：weiss等设计的ssf(single
‑
sensor fusion)算法以扩展卡尔曼滤波 (extended kalmanfilter，ekf)为基本框架，将imu解算的位姿作为预测函数，将ptam 算法计算的位姿作为测量数据，有效地提升了算法的性能，并且当视觉解算位姿短时间失败 时，该算法中imu的运动预测仍然能够提供精确的估计。刘振彬等使用一种改进的非线性 方法优化单目惯导slam方案，该方案基于vins
‑
mono系统，在初始化的过程中添加了对 加速度贝叶斯偏差的初始化，并借鉴orb
‑
slam，使用orb(一种快速特征点提取和描述 的算法)特征点在前端对图像进行特征检测匹配，在提高方案鲁棒性的同时实现了较高精度 的实时定位与稀疏地图构建。campos等提出一种基于视觉惯导的slam系统viorb
‑ꢀ
slam，能够支持单目、双目、rgbd等多种相机类型。vi orb
‑
slam系统基于两种传感 器设计了一种混合地图数据关联机制，能够对短期、中期、长期的传感器采集数据进行数据 关联。同时基于特
征的紧耦合vio系统，vi orb
‑
slam算法依赖最大后验估计，对关键帧 进行几何一致性检测，提高了地图定位的精确性。
4.但是，融合两种传感器数据的位姿估计，虽然可以有效提高估计精度，其同样存在一些 技术缺陷：使用松耦合的方式融合位姿，主体仍然独立地使用视觉算法实现机器人的位姿估 算，imu的测量数据仅仅被添加到一些独立的位姿估算过程当中，直接导致了相机与imu 内部状态之间的相关性被忽略，缺乏对位姿估算有效的约束和补偿，甚至会出现对相机与 imu位姿误差估算失败的结果，因此，松耦合方法并不能取得最优的位姿估计精度；紧耦 合的方法是将视觉算法和imu数据融合在一起，共同构建运动方程与状态方程，对机器人 的位姿状态进行解算，但是采用全局优化或局部平滑的紧耦合方法过度依赖初始化的位姿估 计，计算相机与imu数据的过程中复杂度较高，实时性有所欠缺；同时，单目相机无法直 接获取尺度信息，只能依赖imu计算的尺度信息，紧耦合方法并不能有效增强位姿尺度的 估算精度。此外，还需要考虑到基于滤波的vi
‑
slam方法的马尔科夫性，即在某一时刻的 状态与之前所有时刻的状态关系无法确定，算法框架存在缺陷，精度提升空间有限。


技术实现要素：

5.为了解决上述技术问题，本发明的目的是提供一种基于kinect/imu位姿融合优化的 室内地图构建的方法，该方法具有更高的位姿估计精度、更高的二维地图建模精度以及更优 的建模效果，能够应用于贫特征、高动态、弱光影的场景中。
6.基于上述目的，本发明的一个方面，提供一种基于位姿融合优化的室内地图构建方法， 该方法包括：
7.根据kinect和imu获取的实时数据解算机器人的位姿数据；
8.获取机器人当前运动状态；
9.若机器人处于静止状态，则采用扩展卡尔曼滤波算法对所述位姿数据进行融合处理，若 机器人处于运动状态，则采用动态加权方法对所述位姿数据进行融合处理；
10.根据融合处理结果构建室内地图。
11.作为优选，所述利用kinect获取的实时数据解算机器人位姿数据的具体方法为：获 取图像数据；对两张部分重叠图像的同名特征点进行匹配，获得同名特征点之间的对应关系， 该对应关系与机器人位姿对应。
12.作为优选，所述利用imu获取的实时数据解算机器人位姿数据的具体方法为：采用方 向余弦矩阵算法进行位姿解算。
13.作为优选，所述获取机器人当前运动状态的方法为：
14.若imu的线加速度为0，且里程计数据没有增加，则判定机器人当前为静止状态；
15.若imu的线加速度不为0，和/或里程计数据有增加，则判定机器人当前为运动状态。
16.作为优选，采用扩展卡尔曼滤波算法对所述位姿数据进行融合处理的具体方法为：
17.获取系统状态量，计算误差状态预测值和系统状态噪声协方差矩阵；
18.由计算得到观测矩阵和噪声协方差矩阵，计算系统的增益矩阵；
19.由增益矩阵，更新系统状态量、系统协方差矩阵；
20.将系统状态量计算得到的四元数进行归一化处理；
21.将四元数转化为方便位姿显示的欧拉角，以此表示机器人的位姿状态。
22.作为优选，采用动态加权方法对所述位姿数据进行融合处理的方法为：
23.通过kinect根据关键帧获取对应的图像；
24.根据kinect获取的两个关键帧之间特征点的匹配成功率μ设置权重值阈值；若匹配 成功率μ降低，则提高imu姿态的权重；若匹配成功率μ升高，则提高kinect姿态的权 重。
25.作为优选，根据所述融合处理结果创建室内地图的具体方法为：
26.获取激光点与雷达本身经过距离与方位角的解算后获得相对坐标；
27.预先将融合优化后的位姿估计替换建图算法中的位姿估计，并将该位姿估计作为新的数 据源计算激光点与雷达的相对关系，以更新激光点的距离与方位角信息；
28.使用基于高斯牛顿的扫描匹配法匹配激光点坐标，建立高精度的室内二维地图。
29.本发明的另一个方面，提供一种采用上述的基于位姿融合优化的室内地图构建方法的进 行室内地图构建的系统，其特征在于，包括：
30.位姿数据解算单元，根据kinect和imu获取的实时数据解算机器人的位姿数据；
31.运动状态判断单元，根据imu的线加速度数据和里程计数据判断机器人当前运动状态；
32.位姿数据融合处理单元，分别与位姿数据解算单元和运动状态判断单元连接，当机器人 处于静止状态时，采用扩展卡尔曼滤波算法对所述位姿数据进行融合处理，当机器人处于运 动状态时，采用动态加权方法对所述位姿数据进行融合处理；
33.二维地图构建单元，与位姿数据融合处理单元连接，根据所述融合处理结果创建室内地 图。
34.作为优选，所述位姿数据解算单元包括：
35.kinect数据解算子单元，其用于获取图像数据；对两张部分重叠图像的同名特征点进 行匹配，获得同名特征点之间的对应关系，该对应关系与机器人位姿对应。
36.imu数据解算子单元，其采用方向余弦矩阵算法对imu获取的实时数据进行位姿解算。
37.本发明的再一个方面，提供一种存储介质，该存储介质内存储有计算机程序，所述计算 机程序被处理执行时实现如上所述的基于位姿融合优化的室内地图构建方法。
38.与现有技术相比，本发明的有益效果为：
39.该方法通过首先判定机器人的运动状态，针对机器人静止状态，使用扩展卡尔曼滤波算 法进行位姿数据的融合，针对机器人运动状态，采用特征匹配加权策略融合kinect与 imu位姿数据，这样，有效提高融合后的位姿估计精确度，使得二维地图建模精度更好且建 模效果更优，该方法可有效提高贫特征、高动态、弱光影的场景中的位姿估计精度。
附图说明
40.构成本技术的一部分的说明书附图用来提供对本技术的进一步理解，本技术的示意性实 施例及其说明用于解释本技术，并不构成对本技术的限定。
41.图1是本发明实施例中基于位姿融合优化的室内地图构建方法的流程图；
42.图2(a)
‑
(c)分别是是本发明实施例中过道、墙角和桌椅特征点选取匹配示意图；
43.图3是本发明实施例中部分自然场景与人为布设场景；
44.图4是本发明实施例中匹配成功率与位姿误差关系；
45.图5是本发明实施例中搭建的机器人平台与传感器；
46.图6是本发明实施例中x、y、z方向位移偏离图；
47.图7是本发明实施例中x、y方向位移偏离图；
48.图8(a)
‑
(d)是本发明实施例中局部建图效果对比；
49.图9(a)是本发明实施例中cartographer算法构建的二维地图；
50.图9(b)是本发明实施例中本发明算法构建的二维地图。
具体实施方式
51.下面结合附图与实施例对本发明作进一步说明。
52.应该指出，以下详细说明都是示例性的，旨在对本技术提供进一步的说明。除非另有指 明，本实施例使用的所有技术和科学术语具有与本技术所属技术领域的普通技术人员通常理 解的相同含义。
53.需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申 请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图 包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括
”ꢀ
时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
54.本实施例提供一种基于位姿融合优化的室内地图构建方法，如图1所示，该方法包括：
55.根据kinect和imu获取的实时数据解算机器人的位姿数据；
56.获取机器人当前运动状态；
57.若机器人处于静止状态，则采用扩展卡尔曼滤波算法对所述位姿数据进行融合处理，若 机器人处于运动状态，则采用动态加权方法对所述位姿数据进行融合处理；
58.根据融合处理结果构建室内地图。
59.作为一种较优的实施方式，所述利用kinect获取的实时数据解算机器人位姿数据的 具体方法为：获取图像数据；对两张部分重叠图像的同名特征点进行匹配，获得同名特征点 之间的对应关系，该对应关系与机器人位姿对应。具体地：基于kinect获取的图像数据 解算机器人的位姿，通常根据两张部分重叠图像的同名特征点的匹配而实现。首先，使用 oriented
‑
fast算法提取图像特征点；然后，使用快速近似最近邻方法(fast library forapproximate nearest neighbors，flann)实现特征点之间的匹配。根据两张图像中的同名 特征点的对应关系，实现相机运动参数的解算。例如：假设相机运动的刚体变换量为(r， t)，其中，r为旋转向量，t为平移向量，那么特征点对应的坐标与运动过程的关系如式(1) 所示。
60.p
pm
＝rp
lm
t
ꢀꢀꢀ
(1)
61.其中，p
pm
，和rp
lm
为当前帧图像与上一帧图像第m个特征点的三维坐标，m＝1，2， 3
……
n。使用最近邻方法匹配特征点的同时需要剔除误匹配的特征点对，随机选取至少4对 特征点对，计算刚体变换量(r，t)，将其坐标代入||p
pm
‑
(rp
lm
t)||2，筛选小于阈值的点 代入式(1)解算。最后，通过迭代计算获得特征点对应坐标与运动过程关系的最小二乘值， 用
于估计机器人的当前位姿。
62.作为一种较优的实施方式，所述利用imu获取的实时数据解算机器人位姿数据的具体 方法为：采用方向余弦矩阵算法进行位姿解算。具体地：
63.imu主要包含三轴加速度计与三轴陀螺仪，通过对加速度计测得的线速度进行二次积 分，可以获得imu的速度与位置数据；对陀螺仪测得的角速度进行积分，可以获得imu的 姿态数据。为避免当俯仰角为90
°
时，欧拉角微分方程会出现奇点的问题，导致无法获得 imu的全姿态解算，本实施例采用方向余弦矩阵算法(direction cosine matrix，dcm)使 用矢量的方向余弦表示姿态矩阵，有效地避免了使用欧拉角表示姿态遇到的奇点问题，具体 地，本实施例中imu姿态的解算采用dcm算法，定义dcm矩阵为c，如式(2)所示。
[0064][0065]
其中，i
′
,j
′
,k
′
为机器人坐标的单位向量。对i
′
,j
′
,k
′
求导可得式(3)。
[0066][0067]
再对矩阵c求导可得如式(4)所示。
[0068][0069]
求解式(4)方程，得到imu与机器人的相对位姿变换关系。
[0070]
此外，由于在实际操作中，imu的高测量频率占用了大量的计算资源，为避免在每个 imu测量时都添加一个新的状态量，通常在两帧之间加入重新参数化的过程，实现运动约 束，避免重复积分，这种重新参数化的过程称为imu预积分。具体地：假设imu测量得到 的加速度与角速度分别表示为那么，imu测量模型如式(5)所示。
[0071][0072]
其中，w为世界坐标系，b为imu坐标系，
b
ω
wb
(t)为b相对于w的瞬时角速度， 为世界坐标系到imu坐标系的旋转矩阵，w
α
(t)为世界坐标系下的瞬时加速度，b
g
(t)、 b
α
(t)为陀螺仪与加速度的偏差，η
g
(t)、η
α
(t)为随机噪声。引入动力学积分模型，模型包含旋 转关系速度和平移如式(6)所示。
[0073][0074]
对式(6)进行积分可以得δt时间内的旋转量r
wb
(t δt)、速度量
w
v(t δt)和平移量 w
p(t δt)
w
p(t δt)，结合imu测量模型中的加速度与角速度得到
imu 在δt时间内相对于世界坐标系的运动状态r
wb
(t δt)、
w
v(t δt)和
w
p(t δt)，即两个时刻 imu数据之间的相对关系。imu与kinect的更新率并不同步，积分两个关键帧之间的多 帧imu数据可以有效约束关键帧。考虑到旋转矩阵随着时间的不断变化，因此相对运动 增量δr
ij
、δv
ij
、ap
ij
，如式(7)所示。
[0075][0076]
其中，其中，式(7)左侧为相对运动增量，右 侧为imu数据，进一步将两个关键帧的相对运动增量δr
ij
、δv
ij
、δp
ij
表示为式(8)。
[0077][0078]
如此一来，得到两帧之间的相对运动增量δr
ij
、δv
ij
、δp
ij
，即两帧之间使用imu预积分 重新参数化的结果，避免了重复积分imu观测量，提高了算法的可靠性。
[0079]
作为一种较优的实施方式，所述获取机器人当前运动状态的方法为：
[0080]
若imu的线加速度为0，且里程计数据没有增加，则判定机器人当前为静止状态；
[0081]
若imu的线加速度不为0，或里程计数据有增加，则判定机器人当前为运动状态。
[0082]
作为一种较优的实施方式，采用扩展卡尔曼滤波算法对所述位姿数据进行融合处理的具 体方法为：
[0083]
获取系统状态量，计算误差状态预测值和系统状态噪声协方差矩阵；
[0084]
由计算得到观测矩阵和噪声协方差矩阵，计算系统的增益矩阵；
[0085]
由增益矩阵，更新系统状态量、系统协方差矩阵；
[0086]
将系统状态量计算得到的四元数进行归一化处理；
[0087]
将四元数转化为方便位姿显示的欧拉角，以此表示机器人的位姿状态。
[0088]
具体地：
[0089]
为了更加精确地估计机器人的位姿信息，可以使用扩展卡尔曼滤波融合imu和 kinect位姿。ekf分为预测过程和更新过程。对于预测过程，定义误差状态向量如式 (9)所示。
[0090][0091]
其中，为状态向量期望值。同时，使用表示状态噪声n，根据误差状
因此，位姿融合过程中可以适当考虑两者之间的权重关系，分析关键帧特征点的匹配成功率， 用以确定kinect和imu位姿对于特征点匹配的依赖程度，进而实现面向不同位姿状态采 取不同的加权策略融合处理imu和kinect位姿。
[0108]
当机器人处于运动状态时，imu的测量值包括线速度、加速度和角速度，kinect根据 关键帧获取对应的图像。根据两个kinect获取的关键帧之间特征点匹配的成功率设置权 重值阈值；如果匹配成功率低，那么当前姿态和kinect姿态的关联程度低，应当提高 imu姿态的权重；如果匹配成功率高，那么当前姿态和kinect姿态的关联程度高，应当 提高kinect姿态的权重。
[0109]
假设两个关键帧匹配成功的特征点与总特征点的比值定义为匹配成功率μ，那么加权融 合位姿的关键在于，根据μ的取值区间，确定kinect和imu位姿融合的权重值，即位姿加 权融合的权参数。
[0110]
作为一种较优的实施方式，根据所述融合处理结果创建室内地图的具体方法为：
[0111]
获取激光点与雷达本身经过距离与方位角的解算后获得相对坐标；
[0112]
预先将融合优化后的位姿估计替换建图算法中的位姿估计，并将该位姿估计作为新的数 据源计算激光点与雷达的相对关系，以更新激光点的距离与方位角信息；
[0113]
使用基于高斯牛顿的扫描匹配法匹配激光点坐标，建立高精度的室内二维地图。
[0114]
室内二维地图的构建过程中，非结构化的场景可能导致二维激光雷达的构图平面倾斜， 降低建图质量。本实施例采用激光三角测距技术实现lidar建图，测得的激光点与lidar 本身经过距离与方位角的解算后获得相对坐标，构图前将融合优化后的位姿估计替换建图算 法中的位姿估计，将其作为新的数据源计算激光点与雷达的相对关系，以此更新激光点的距 离与方位角信息，再使用基于高斯牛顿的扫描匹配法匹配激光点坐标，建立高精度的室内二 维地图。
[0115]
室内环境大多呈现垂直结构，实验中需要将倾斜的雷达数据做近似投影处理。扫描获得 的激光数据(ρ，θ)按照地图坐标表达形式表示为(p
x
，p
y
，p
z
)，如式(16)所示。
[0116][0117]
其中，(θ
x
，θ
y
，θ
z
)表示翻滚角、俯仰角、偏航角。根据初始位姿与角度变换求得扫描激光 点的坐标(x，y，z)，如式(17)所示。
[0118][0119]
对应优化后的位姿估计与激光点相对坐标后，使用高斯牛顿法匹配激光点，提高建图精 度与效果。首先对刚体变换t取最小值t
*
，如式(18)所示。
[0120][0121]
其中，s
i
(t)为激光点的平面坐标，g(s
i
(t))为s
i
(t)的占用值，当所有激光点完全匹配时， g(s
i
(t))的值为1。之后优化一段时间内的雷达数据测量误差，如式(19)所示：
[0122][0123]
通过泰勒展开与求偏导，将求解δt转化为高斯牛顿的最小化问题，如式(20)所示。
[0124][0125]
其中，输出基于牛顿高斯的最小二乘解后，根 据高斯牛顿法求得光束点匹配的结果，将与优化位姿对应点的距离与方位角作为数据源代入 建图过程，构建室内二维地图，有效去除了因二维激光雷达运动带来的畸变，提升了建图效 果。
[0126]
根据上述内容，基于kinect/imu的位姿融合优化的室内地图构建方法根据机器人的 运动状况，选择不同的位姿融合策略。当机器人处于静止状态时，使用ekf方法融合 kinect和imu的位姿；当机器人处于运动状态时，使用加权方法融合kinect和imu的 位姿。对于加权融合方法而言，确定kinect位姿和imu位姿以何种比例关系参与加权是 关键。本实施例依据两个关键帧匹配成功的特征点与总特征点的比值μ(简称匹配成功率μ) 确定kinect位姿和imu位姿的权重关系，即位姿加权融合的权参数λ。
[0127]
为了得到准确的位姿加权融合的权参数λ，本实施例还设计了位姿加权融合权参数确定 实验。实验区域选取某实验室室内环境。实验设备选取kinect v2相机与ah100b惯性测 量单元。由于惯性测量单元采集数据的频率过高，为同步相机与惯性测量单元采集数据的频 率，前期使用imu预积分步骤，将相机与惯性测量单元的采集频率统一为kinect v2相机 的30hz。
[0128]
实验步骤主要包括如下步骤：
[0129]
(1)获取原始位姿数据。
[0130]
分别选取26个室内环境场景，使用kinect v2相机拍摄图像，匹配图像之间的特征点， 解算位姿；使用imu积分记录加速度与角速度数据，解算位姿；常用的位姿解算方法都直 接使用扩展卡尔曼滤波方法获得机器人的位姿，因此，可以将用作实验比较的真值。
[0131]
(2)计算均方根误差。
[0132]
分别计算kinect v2相机和imu解算的位姿与扩展卡尔曼滤波解算的位姿之间的均方 根误差。
[0133]
(3)分析匹配成功率μ与位姿均方根误差的关系。当匹配成功率μ取不同的值时，分 析kinect v2相机和imu计算的均方根误差的关系，进而确定kinect位姿和imu位姿 的权重关系。
[0134]
匹配成功率μ是指某一关键帧的两幅重叠图像的匹配成为同名特征点的数量与重叠图像 获取的总特征点之间的比值，本技术中匹配成功率μ的高低决定了环境特征的显著程度，进 而决定了位姿解算的精度。
[0135]
本实施例首先选取了19个不同的场景，实验场景包含了具有代表性的过道、墙角、桌 椅等位置，基于orb特征点检测使用快速近似最邻近方法匹配特征点。
[0136]
如图2(a)
‑
图2(c)所示，左侧图像分别表示实验室环境的过道、墙角、桌椅场景， 右侧3幅图像表示从不同角度获取的过道、墙角、桌椅图像，其中图中横线连线表示两幅图 像成功匹配的orb特征点对。实验过程中，过道、墙角、桌椅场景分别检测到533、149、 653个特征点，其中过道场景成功匹配50对特征点对，墙角场景成功匹配4对特征点对， 桌椅场景成功匹配76对特征点对。过道、墙角、桌椅场景的匹配成功率分别为9.38％、 2.68％、11.63％，这说明纹理丰富、结构明显、特征性强的场景能够检测到更多的特征点并 且匹配成功率较高；相反，弱纹理、结构单一的场景检测到的特征点少并且匹配成功率低。 更为重要的是，通过这19个不同场景的实验，匹配成功率最大为43％，始终没有超过50％。 针对上述情况，本实施例增加7个布设了人为特征的实验场景，使得特征点匹配成功率达到 了87.37％，图3为部分自然场景和人为布设场景。
[0137]
其次，如图4所示，分别计算kinect v2相机和imu解算的位姿与扩展卡尔曼滤波解 算的位姿之间的均方根误差，实验结果表明：(1)当kinect相机特征点匹配的成功率越 低时，kinect相机与扩展卡尔曼滤波之间的位姿均方根误差就越高，说明此时kinect相 机解算位姿的可靠性较低；(2)当kinect相机特征点匹配的成功率越高时，kinect相机 与扩展卡尔曼滤波之间的位姿均方根误差就越低，说明此时kinect相机解算位姿的可靠 性较高；(3)但是，考虑到匹配成功率越高的原因是增加了许多人为的特征点，因此在适当 时候可能需要忽略这种影响。如果单纯观察匹配成功率低于50％的情况，还可以发现 kinect解算的位姿精度逐渐降低，imu解算的位姿精度始终没有太大变化，这种现象是符 合实际情况的。因此，当机器人处于运动状态时，本技术按照加权参数融合kinect解算 的姿态和imu解算的姿态，用作优化的融合位姿。
[0138]
1)0.6≤μ≤1时，kinect解算的姿态的权参数λ为0.5；imu解算的姿态的权参数μ为 0.5；
[0139]
2)0.3≤μ<0.6时，kinect解算的姿态的权参数λ为0.33；imu解算的姿态的权参数μ 为0.67；
[0140]
3)0≤μ<0.3时，kinect解算的姿态的权参数λ为0.2；imu解算的姿态的权参数μ为 0.8。
[0141]
4kinect/imu位姿融合实验：
[0142]
kinect/imu位姿融合实验的实验平台选用autolabor机器人，它搭载了kinect v2 相机、ah100b惯性测量单元和rplidar a2二维激光雷达，三个传感器与autolabor机器 人之间采用固联的联接方式，如图5所示。
[0143]
实验数据：
[0144]
为了验证位姿融合加权策略的有效性、kinect/imu融合位姿的精度以及优化后的 lidar建图效果，本实施例设计了两组不同实验，第一组实验选用了euroc公开数据集作 为数据来源，另一组实验采集某实验室环境数据作为数据来源。
[0145]
euroc公开数据集是苏黎世联邦理工学院基于无人飞行器在礼堂与空房间两种环境内 采集的数据，无人飞行器搭载的传感器主要包括相机与惯性测量单元，相机频率为20hz， 惯性测量单元频率为200hz。实验数据分为4个文件夹，每个文件夹都包含传感器相对于坐 标系的变换情况。euroc公开数据集主要用于位姿精度的比较分析。
[0146]
第二组数据集使用autolabor机器人采集某实验室的环境数据。实验室场景为有
障碍物 的室内平坦地面，面积为15m
×
10m。机器人搭载的传感器主要包括kinect v2相机与 ah100b惯性测量单元，相机频率为30hz，惯性测量单元频率为37hz。实时采集的数据集 通过bag数据包记录，然后根据不同的话题将bag数据包导出对应的位姿数据集。
[0147]
实验步骤：
[0148]
(1)实验数据获取
[0149]
第一组实验选用了euroc公开数据集作为数据来源，第二组数据集使用autolabor机器 人采集实验室的环境数据。
[0150]
(2)位姿解算
[0151]
提取并匹配两帧重叠图像的orb特征点，根据特征点对的匹配关系解算相机的运动参 数。二次积分imu记录的线速度，计算速度与位置信息，积分角速度计算姿态信息，获得 速度、位置、姿态信息之后，使用方向余弦矩阵计算imu与实验平台的相对位姿关系。
[0152]
(3)位姿估计优化
[0153]
判断机器人运动状态。如果机器人为静止状态，那么使用扩展卡尔曼滤波融合相机与惯 导传感器解算的位姿；如果机器人为运动状态，那么通过特征点匹配的依赖程度，使用加权 策略融合位姿，输出优化后的位姿估计。
[0154]
(4)二维地图构建
[0155]
优化更新后的位姿纠正rplidar a2获取激光点的距离与方位角信息，使用基于高斯 牛顿的扫描匹配，构建二维环境地图。
[0156]
(5)实验分析
[0157]
首先，基于euroc公开数据集，比较rovio算法和本实施例算法在位姿融合优化方面 的精度。rovio算法紧耦合视觉信息和imu信息，通过迭代卡尔曼滤波融合数据，更新滤 波状态与位姿，在算法框架和数据融合方式方面与本实施例类似。其次，基于实验室场景的 采集数据，同样比较viorb
‑
slam算法和比本实施例算法在位姿融合优化方面的精度。 viorb
‑
slam算法由orb
‑
slam和imu模块组成，通过最大后验估计，对关键帧进行几 何一致性检测，得到位姿最优估计，因此，viorb
‑
slam算法和本实施例算法具有较高的 对比度。最后，在二维地图构建方面，比较cartographer算法和本实施例算法的建模精度和 效果。cartographer算法是基于二维激光雷达构建地图的经典算法，根据imu和里程计信息 通过滤波的方式预测位姿，将扫描点进行体素滤波转换为栅格点坐标构建地图，建图效果稳 定，鲁棒性高。
[0158]
第一组实验基于euroc公开数据集，比较分析rovio算法和本实施例算法的位姿估计 精度。实验时间总长为149.96s，共选取了29993个位姿。实验误差如表5所示，分别选取 最大误差(max)、最小误差(min)、平均值(mean)、均方根误差(rmse)和标准差 (std)作为精度衡量的指标。同时分析了机器人在x轴方向、y轴方向和z轴方向的偏移图 (图6)。
[0159]
表5本实施例算法与rovio实验误差分析(pose:29993)
[0160][0161]
从实验结果可以看出：本实施例算法与rovio算法对比，均方根绝对误差为
9(b))，实际墙面在二维平面地图上的映射直线相互垂直，边与边的相对关系基本没有畸变。
[0171]
因此，从建图精度与可视化效果的角度来说，本实施例算法相较于cartographer算法都 有一定程度的提升。
[0172]
本实施例以室内环境为实验背景，结合机器人室内建图的位姿精度需求与室内环境特点， 提出了一种基于kinect/imu融合位姿优化的方法，将其运用于二维地图的建模。首先， 通过kinect、imu解算机器人的位姿；其次，通过判定机器人的运动状态，使用扩展卡尔 曼滤波或特征匹配加权策略融合imu与kinect数据，形成新的位姿估计；再次，将融合 优化后的高精度位姿和同一时刻测得的激光点一一对应，纠正激光点在坐标系内的距离与方 位角信息，将其代入基于牛顿高斯扫面点的匹配过程，构建室内二维地图。最后，设计了 kinect/imu位姿融合实验，分别运用euroc公开数据集和实测数据集验证算法的有效性， 实验结果表明，算法较robvio算法和vi orb
‑
slam算法具有更高位姿估计精度，较 cartographer算法具有更高的二维地图建模精度和建模效果。
[0173]
本发明的另一个方面，提供一种采用上述的基于位姿融合优化的室内地图构建方法的进 行室内地图构建的系统，其特征在于，包括：
[0174]
位姿数据解算单元，根据kinect和imu获取的实时数据解算机器人的位姿数据；
[0175]
运动状态判断单元，根据所述位姿数据判断机器人当前运动状态；
[0176]
位姿数据融合处理单元，分别与位姿数据解算单元和运动状态判断单元连接，当机器人 处于静止状态时，采用扩展卡尔曼滤波算法对所述位姿数据进行融合处理，当机器人处于运 动状态时，采用动态加权方法对所述位姿数据进行融合处理；
[0177]
二维地图构建单元，与位姿数据融合处理单元连接，根据所述融合处理结果创建室内地 图。
[0178]
作为一种较优的实施方式，所述位姿数据解算单元包括：
[0179]
kinect数据解算子单元，其用于获取图像数据；对两张部分重叠图像的同名特征点进 行匹配，获得同名特征点之间的对应关系，该对应关系与机器人位姿对应。
[0180]
imu数据解算子单元，其采用方向余弦矩阵算法对imu获取的实时数据进行位姿解算。
[0181]
本发明的再一个方面，提供一种存储介质，该存储介质内存储有计算机程序，所述计算 机程序被处理执行时实现如上所述的基于位姿融合优化的室内地图构建方法。
[0182]
为了提高室内地图构建的精度，通常可以融合kinect、imu等多种传感器获得的高精 度数据。本技术通过判断机器人的运动状态，根据特征点匹配的依赖程度，分别使用ekf 和加权融合两种方式实现kinect和imu两种传感器位姿的优化，实现了室内地图的高精 度建模。这一过程中，首先判定机器人的运动状态，针对机器人静止状态，使用扩展卡尔曼 滤波算法进行位姿数据的融合，针对机器人运动状态，采用特征匹配加权策略融合kinect 与imu位姿数据，这样，有效提高融合后的位姿估计精确度，使得二维地图建模精度更好 且建模效果更优，该方法可有效提高贫特征、高动态、弱光影的场景中的位姿估计精度。
[0183]
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的， 不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下 在本发明的范围内可以对上述实施例进行变化、修改、替换和变型，凡是依据本
发明的技术 实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围 内。