一种基于融合多传感器的因子图室内定位方法

1.本发明涉及机器人定位技术领域，尤其是一种基于融合多传感器的因子图室内定位方法。


背景技术：

2.自主移动机器人已应用于工业生产、卫生、交通及军事等各个领域，而精准的位置服务是机器人控制的关键环节之一，近年来已成为研究热点并已成为一个重要课题。相比于室外定位，室内由于存在物体的遮挡、人员的走动及物品摆放繁杂、存在许多不确定的障碍物等复杂环境，导致实现高精准定位难度更大。与早期以惯性测量单元(inertial measurement unit,imu)为主要传感器的定位技术相比，目前室内机器人定位会配备多种传感器，以获得更精确、更可靠的导航解决方案。虽然采用不同传感器组合可以提高定位精度，但传感器测量的异步性和非线性使来自多传感器的测量信息难以有效融合，定位精度不够高且计算复杂度高。


技术实现要素：

3.针对现有技术中的缺陷，本发明提供了一种基于融合多传感器的因子图室内定位方法，可有效融合多传感器的测量信息，提高定位精度。
4.本发明提供了一种基于融合多传感器的因子图室内定位方法，包括步骤：
5.根据多传感器的测量模型制定传感器因子，将所述传感器因子输入至预设的因子动态适应函数作为因子节点、将机器人导航状态作为变量节点构建因子图框架；
6.对获取的多传感器的测量数据进行非线性融合，得到所述机器人导航状态的最优状态增量估计；
7.结合增量平滑算法计算机器人所有导航状态下的全导航解。
8.优选地，所述多传感器包括imu、里程计和激光雷达。
9.优选地，所述根据多传感器的测量模型制定传感器因子具体包括：
10.根据imu测量模型制定imu因子；
11.根据里程计测量模型制定里程计因子；
12.根据激光雷达测量模型制定激光雷达二元因子和外部因子。
13.优选地，所述对获取的多传感器的测量数据进行非线性融合包括：
14.利用高斯牛顿方法对获取的多传感器的测量数据进行非线性融合。
15.优选地，所述对获取的多传感器的测量数据进行非线性融合包括：
16.利用lm方法对获取的多传感器的测量数据进行非线性融合。
17.优选地，还包括：定义机器人最优导航解为对机器人导航状态的最大后验估计。
18.优选地，还包括：将所述机器人导航状态构建成贝叶斯树。
19.优选地，所述增量平滑算法的步骤包括：
20.当初始因子图框架加入新因子节点时，根据所述新因子节点包含的状态量确定贝
叶斯树中的受影响部分及对应的受影响状态量、未受影响部分及对应的未受影响状态量；
21.将所述受影响状态量与所述新因子节点包含的状态量进行非线性融合，得到新增状态量；
22.将所述新增状态量与所述未受影响状态量相接，得到新因子图框架。
23.本发明的有益效果为：
24.将imu、里程计(odom)和激光雷达(lidar)构成的预积分因子、配准因子和闭环因子插入全局因子图中，并采用基于贝叶斯树的因子图优化算法实现数据的融合和对变量节点进行优化估计。将导航状态作为变量节点、传感器模型作为因子节点构建因子图模型，使用智能优化算法解决非线性问题得到每个时刻的最优状态估计，并结合增量平滑技术有效地计算所有状态下的全导航解。增量方法优化了图中的部分节点，使本发明适用于实时性要求比较高的场景。同时，imu测量数据生成一个简单的因子链，可以非常高效地在该拓扑上运行，能保证全局位姿轨迹和所在环境的全局一致性。本发明提升了定位实时性、鲁棒性和准确性，对室内机器人的自主导航定位具有重大的应用研究意义。
附图说明
25.为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍。在所有附图中，类似的元件或部分一般由类似的附图标记标识。附图中，各元件或部分并不一定按照实际的比例绘制。
26.图1为本发明实施例提供的流程示意图；
27.图2a为本发明实施例提供的具有常规imu和偏差因子的因子图；
28.图2b为本发明实施例提供的使用等效imu因子的因子图；
29.图3为本发明实施例提供的里程计和imu传感器测量的因子图；
30.图4为本发明实施例提供的激光雷达、里程计和imu传感器测量的因子图；
31.图5为本发明实施例提供的初始因子图；
32.图6为本发明实施例提供的雅可比矩阵；
33.图7为本发明实施例提供的加入新节点后的因子图；
34.图8为本发明实施例提供的加入新节点后的雅可比矩阵。
具体实施方式
35.下面将结合附图对本发明技术方案的实施例进行详细的描述。以下实施例仅用于更加清楚地说明本发明的技术方案，因此只作为示例，而不能以此来限制本发明的保护范围。
36.需要注意的是，除非另有说明，本技术使用的技术术语或者科学术语应当为本发明所属领域技术人员所理解的通常意义。
37.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
38.应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”和“包含”指示
所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
39.还应当理解，在此本发明说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本发明。如在本发明说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。
40.还应当进一步理解，在本发明说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。
41.将主动移动机器人应用在搬运货物，以及在没电返回进行无线充电时需要非常精准的定位。将多传感器融合的因子图模型应用到基于slam的自主导航机器人中，在机器人建图时通过三种传感器构建周围环境的地图，并且获得先验信息和路标。机器人开始导航时，在搬动货物和交接货物时，需要非常精准的定位，控制机器人移动到指定的位置。机器人在完成任务或者电量不足时，返回进行初始位置补充电量，只有定位足够精准才行完成充电。
42.基于上述描述，本发明实施例提供了一种基于融合多传感器的因子图室内定位方法，参见图1，包括：
43.步骤1：slam问题的状态构建；
44.a.状态：状态是一组描述载体及其传感器的所有变量，代表性地有位置、方向、速度以及传感器相关的参数。例如imu偏差和激光雷达到imu的变换参数。时间k处的状态表示为xk。在基于地标的slam中，状态还包括地标在环境中的位置。假设状态通过离散时间动力学模型变化，则有:
45.x
k 1
＝f(xk，vk，nk)
46.nk是一个不确定性的随机变量(噪声)编码模型和vk是通常通过车轮速度传感器或imu等传感器获得的测量值。
47.b.观测量：我们得到了一组测量值z1，z2，
…
，zk，来自于公式zk＝hk(x
k1
，
…
，x
kp
，nk)，其中每个状态都在x1，x2，
…
，xk中，hk是一个已知的测量函数，而nk是一个随机变量噪声，传感器缺陷(即观测噪声)产生的测量不确定性。每个传感器的测量功能是固定的，下标k表示可能存在不同的传感器。本发明实施例中，以惯性导航、激光测距和惯性姿态图slam为中心，考虑两种类型的测量：
48.(1)在时间k进行的观测，仅涉及时间k的状态(一元观测)
49.zk＝hk(xk，nk).
50.(2)涉及一对状态的时间k的观测，即
51.zk＝hk(xi，xk，nk)，i＜k
52.就像姿势图slam中的情况一样。这些测量可能与计算不同时间姿势之间的相对变换有关，通常使用立体相机或激光雷达，本发明实施例采用激光雷达。它们既可用作传播状态的里程计，也可用作“循环闭包”。
53.总的来说，z＝z1，z2，...，zk表示所有传感器的测量值。
54.步骤2：构建因子图基本模型；
55.因子图作为一种表示因式分解的建模工具，具有简单通用性，尤其在编码领域、统计学、信号处理和人工智能领域有广泛的应用价值。因子图是一种概率图形模型，与贝叶斯
网或马尔可夫随机场不同，它由变量和因子节点组成的二部图表示。变量节点与系统状态(或其部分)相关联，因子与测量值相关联，因子图对给定所有可用测量值的状态随时间的后验概率进行编码。基于因子图多传感器的组合定位方法可以有效解决导航定位信息融合中传感器的异步问题且可实现对多传感器的灵活配置，使得系统具有即插即用的优势。基于因子图的思想，本发明实施例构建了一个多传感器组合导航系统，包括imu、里程计和激光雷达。在因子图中，因子节点和变量节点分别代表传感器测量和导航状态。通过计算一段时间内导航状态的联合概率分布函数的最大后验估计，可以得到所有可用异步多传感器数据下导航状态的最优估计。具有最大后验概率的状态变量集考虑如下：
[0056][0057]
其中x
0：k
是从t0到tk时刻的状态变量，z
1:k
是所以可用传感器的测量值，是通过最大化右侧概率值计算出的最大后验概率估计。根据贝叶斯公式，上式可用转化为：
[0058][0059]
因为联合概率分布函数可以根据先验信息和单个过程和测量模型进行分解。这种因式分解最终可以写成：
[0060][0061]
这个分解过程类似于因子图中的分解过程，即从全局函数到局部函数的乘积。因此，将因子图应用于组合导航系统中的多传感器信息融合是合理可行的。
[0062]
在因子图中，假设f(.)表示局部概率分布函数，那么根据贝叶斯转换的概率公式可以写成：
[0063][0064]
上式中x
1：k，i
表示状态集合中的子集，局部函数fi和误差函数erri有关，因此fi可用表示为：
[0065]fi
(x
1：k，i
)＝d(erri(x
1：k，i
，zi))
[0066]
式中d(.)表示代价函数。
[0067]
假设一个高斯噪声模型，上式就可以写成：
[0068][0069]
其中表示马氏距离，∑i表示ti时刻测量噪声的协方差矩阵。因此，计算最大后
验概率估计等同于最小化在实际的组合导航系统中，误差函数可以表示为状态估计值减去实际测量值。
[0070]
步骤3：根据多传感器的测量模型制定传感器因子，将传感器因子输入至预设的因子动态适应函数作为因子节点、将机器人导航状态作为变量节点构建因子图框架；
[0071]
a.设定先验信息
[0072]
先验信息可分为不同传感器和不同状态变量的个别先验信息p(x0)，每个先验信息可用单独的先验因子表示，对于某些变量x∈x0的先验因子是一元因子，定义为：
[0073]fprior
(x)＝d(x).
[0074]
假设一个高斯分布，先验信息可用变为μ
x
是x0的平均值，∑
x
是x0的协方差。一般情况下，不同变量之间也会存在先验信息。
[0075]
b.根据imu测量模型制定imu因子；
[0076]
对于惯性测量单元，导航状态x的时间演化可用以下连续非线性微分方程描述：
[0077][0078]
上式中fb，ωb分别是在车体架构中的惯性测量单元得到的比力和角加速度，c是通过假定imu误差模型得到用来补偿imu测量的矫正参数，该imu误差模型通常与导航状态估计一起进行估计。将上式线性化，可生成雅可比矩阵和过程噪声，该过程噪声被假定为零平均高斯噪声。
[0079]
在一般情况下，c的时间传播可以根据其自身的一些非线性模型(例如随机游动和温度非线性补偿)来描述：
[0080][0081]
给定的imu测量值与两个连续时间实例ti和t
i 1
的导航状态相关。因子图框架允许采用简单的欧拉积分预测函数具有相关的集成不确定性。以下的非线性优化将适当调整单个状态估计。连续公式的离散表示为：
[0082][0083]c1 i
＝g(ci) n
bias
.
[0084]
上式中的每个等式都定义了连接因子图中相关节点的因子：连接导航节点xi、x
1 i
和偏置节点ci的imu因子f
imu
，以及连接偏置节点xi和c
1 i
的偏置因子f
bias
。在实践中，连续的imu测量值将组合成单个因子。
[0085]
基于误差函数的表示，给定imu测量z的传统imu因子f
imu
和偏置因子f
bias
定义如下：
[0086][0087][0088]
其中，增加新的变量节点x
1 i
到因子图中需要合理的初始值，它通过预测函数h(.)得到，c
1 i
和ci在因子图中表示为变量节点。因子节点f
bias
和实际校准节点添加到因子图如图2a所示。由于imu校准参数ci的典型低动态，因子节点f
bias
和实际校准节点可以以明显低于imu速率的频率添加到因子图中，如图2b所示。该因子可容纳t1和某些t3之间的所有连续
imu测量。导航和校准节点仅在添加新因子f
eimu
时引入，速率远低于imu速率。
[0089]
其中，由于imu因子与当前状态、先前状态和偏置相关，而偏置因子与当前和先前偏置相关。图2a显示了具有imu和偏置因子的相应因子图。通常，在imu频率非常高的情况下估计状态变量是不可行的。相反，将连续的imu测量组合成两个远程导航和校准节点之间的等效imu因子，这可以通过减少因子图中可变节点的数量显著降低计算复杂性的成本。
[0090]
c.根据里程计(odom)测量模型制定里程计(odom)因子；
[0091]
轮速里程计在当前的导航系统中已经非常流行。它主要使用编码器的脉冲计数测量采样时间间隔内载体的距离增量，并提供载体的速度信息。里程计测量模型可通过以下公式得出：
[0092][0093]
其中n
od
是odom的测量噪声，h
od
(xi)是odom的测量函数，表示载体的速度和测量值之间的关系，因此odom的因子可定义为：
[0094][0095]
它仅连接导航状态的变量节点xi，odom的因子f
odom
可与使用当前预积分δxi的等效imu因子f
eimu
一起添加到因子图中，如图3所示。
[0096]
d.根据激光雷达(lidar)测量模型制定激光雷达(lidar)二元因子和外部因子；
[0097]
根据实际测量方程和先验路标点设置，可在多个层面上整合激光雷达传感器观测量。加入激光雷达的因子图模型的构建明确说明了数据之间的关联，从激光雷达传感器导出的测量值。每个测量描述两个时间实例之间地平面上位置和方向的变化。它实现为在i-1时刻的位姿和i时刻的位姿之间的相对姿态，不影响高度、横滚和俯仰。所以激光雷达可以被视为二元因子，imu运动因子也可以视为二元因子，因此测量模型可被定义为：
[0098]
z＝h(x
i-1
，xi) n
[0099]
在上式中，x
i-1
是第i-1时刻的位姿状态，xi是第i时刻的位姿状态，h(.)是激光雷达相对位姿测量函数，表示载体两个时刻姿态的关系，因此lidar的二元因子可定义为：
[0100]flidar
(x
i-1
，xi)＝d(z
i-h(x
i-1
，xi))
[0101]
此外对于每个lidar的观测量zi，都有对应的路标点l，在一个给定位姿xi下观测一个给定的路标l，可以到方位测量值，这样的涉及一个导航状态和一个未知的外部实体被称为外部因子，带有附加噪声n的测量z模型包括时间i的导航状态和未知地标位置l的状态：
[0102]
z＝h(xi，l) n
[0103]
式中，h(.)是观测预测函数，噪声n由协方差矩阵∑的零均值分布所描述。对观测量z，条件概率密度函数p(z|x，1)如下：
[0104]flidar
(zi，h(xi，l))＝d(z
i-h(xi，l))
[0105]
使用这些测量值同时估计导航状态和地标位置在slam问题公式中非常流行。由于lidar测量通常以较低的频率获得，因此固定滞后的大小会自适应地增加。lidar的二元因子和其他因子之间的相互作用如图4所示。本发明实施例中，lidar采用二元因子，可涉及到两个时刻，从而在一个短的时间变化里，可弥补imu和里程计的问题，使定位更精确。
[0106]
基于因子图的定位方法虽然在处理异步数据方面具有良好的性能，但目前在室内定位的研究工作多见于传感器融合形式单一、低动态slam和应用场景简单等方面，在imu和
odom传感器融合的融合的基础上，我们加上激光雷达，既可以补偿这两种传感器固有的偏差和累计误差，又矫正了本身相似特征重定位问题，本发明运用因子图方法，既有效处理了数据杂而多的问题保证了实时性，又提高的了定位精度，使得室内自主机器人运行更加复杂环境。
[0107]
步骤4：对获取的多传感器的测量数据进行非线性融合，得到机器人导航状态的最优状态增量估计；
[0108]
由于组合导航系统中的许多传感器测量具有非线性特征，因子的函数是一个非线性最小二乘问题。我们使用高斯牛顿迭代法来解决这个优化问题，或者lm方法来解决。通过泰勒级数展开，将这个非线性问题转化为线性问题，得到最优状态增量估计：
[0109][0110]
式中是稀疏雅克比矩阵，δ
0：k
是t0到tk的状态增量矩阵，是传感器测量的残差矩阵。当通过迭代计算出结果时，新的状态估计就可以通过公式更新，然后在下一次迭代中将其用作线性化点。
[0111]
本发明实施例中，可使用高斯牛顿迭代法将因子信息融合。当雅克比矩阵是奇异矩阵时也可用lm算法将因子信息融合。
[0112]
步骤5：结合增量平滑算法计算机器人所有导航状态下的全导航解。
[0113]
当初始因子图框架加入新因子节点时，根据新因子节点包含的状态量确定贝叶斯树中的受影响部分及对应的受影响状态量、未受影响部分及对应的未受影响状态量；
[0114]
将受影响状态量与新因子节点包含的状态量进行非线性融合，得到新增状态量；
[0115]
将新增状态量与未受影响状态量相接，得到新因子图框架。
[0116]
为了更好地描述增量平滑算法的因子图模型增量推理方法，列举以下示例：
[0117]
因子图模型随时间的不断递增直观的表现为x中的变量越来越多，如果每一次进行因子图推理都对x中的所有变量重新求解最优估计值，显然是无法满足系统实时性需求的。
[0118]
计算增量需要将雅可比矩阵分解成等效的上三角形式，例如qr分解和cholesky分解。它和高斯牛顿迭代法一样计算出增量就可以得到新的估计它被设置为下一次迭代的初始估计。
[0119]
在本实施例中，参见图5和图6，初始原子图包含4个因子。当增加一个新的因子时，参见图7和图8。其中，
×
是非零元素。
[0120]
图5的平方根信息矩阵r通过对雅可比矩阵进行qr分解得到，参见图6。
[0121]
加入新的因子节点时，平方根信息矩阵r'改变，参见图8。
[0122]
可知，加入新的节点，仅对部分状态变量的估计产生影响，因此，在本实施例中，只需要对受到影响的部分状态变量及新添加的状态变量进行估计。当因子图中加入新的因子节点时，首先根据新因子节点中包含的状态量确定贝叶斯树中受影响的部分以及涉及到的状态量，原因子图中包含这些状态量的部分与新因子节点构成新的因子图，然后转化为新的贝叶斯树，再与未受影响的部分相接，从而实现机器人状态的增量更新。
[0123]
在本实施例中，首先在机器人未导航之前，从三种传感器中获取先验信息，分别包
括lidar识别的路标特征、imu的加速度信息、odom的距离信息。在自主机器人移动时，三个传感器数据进行处理构建传感器各个因子，并且利用先验信息构建因子图模型，从而获得最优的机器人状态估计。其中，imu因子和odom因子分别通过imu传感器测量和odom测量，并被输入到因子生成器。lidar也将测量数据分别通过时态数据和空间数据生成衍生测量信息，转化为节点因子，因子生成器将因子输入融合器处理非线性信息，最终导出机器人最优的状态。
[0124]
目前机器人在各行各业被运用越多越多，且总是配备各种传感器组合成导航定位系统以获得精确定位。不同传感器频率不同，传感器稳定性也参差不一，为了提高定位的精确度和鲁棒性，本发明实施例提出一种基于因子图的多传感器融合的室内定位，对导航关键参数和传感器的标度因子进行了实时估计和反馈，矫正导航系统的信息。本发明实施例是机器人运动根据模型预测状态与实际测量状态的误差(固有的偏差和噪声)，每一时刻的状态变化可以构建贝叶斯模型再转换为因子图模型，状态估计时生成最大后验概率，将最大后验概率转化为最小二乘问题，用高斯牛顿法解决多元非线性最小二乘问题，假如雅克比矩阵是非线性的，实际计算过程中也难免会有数值上的病态出现，于是就采用lm算法解决。本发明实施例克服了一些定位算法在相似复杂中定位不准的问题，同时提高了定位的鲁棒性。
[0125]
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围，其均应涵盖在本发明的权利要求和说明书的范围当中。