一种基于约束因子图的三维磁矢量和PDR融合导航方法

一种基于约束因子图的三维磁矢量和pdr融合导航方法
技术领域
1.本发明属于导航定位技术领域，涉及数据融合与分析，具体涉及一种基于约束因子图的三维磁矢量和pdr融合导航方法。


背景技术：

2.区域内磁场信息可用于导航定位。现有技术中利用磁场构建指纹库，基于单点或序列匹配的方式进行导航定位。也有将磁场信息与其他传感器组合互补的方式，以提高导航定位精度的研究。然而，室内狭小环境的磁场分布相较于室外广阔环境的地磁分布更加复杂多变，同时磁场信息具有信息维度低、易受干扰等特点，极易出现区域内多个位置的磁场指纹相似度极高的情况，导致匹配失败。即使是在组合导航的方法中，也会因为受干扰点位置的磁场信息导致整体精度的下降。
3.目前基于磁场信息的导航定位算法没有充分发挥磁场信息的潜力，还存在着轨迹匹配易出现异常、信息融合不充分、易受干扰影响等问题。区域内某一磁场强度的位置有许多可能，这意味着仅仅依靠磁场信息难以实现单点的精准定位。现有的磁场匹配算法包括迭代最近等值线匹配算法(iccp)、动态时间规整算法(dtw)、磁轮廓匹配算法(magcom)和基于磁指纹匹配以及磁标签匹配的算法等。dtw是用于序列与序列之间匹配的算法，难以胜任序列与多维度地磁地图的快速匹配任务，并且行人难以提前获取轨迹。magcom在地磁图出现多条相似等值线时轨迹匹配精度很低。航向角的累计误差也会使iccp算法出现匹配失败的问题。磁标签匹配算法精度低。基于磁指纹匹配的算法依据磁场特征分为：基于磁标量、基于磁矢量等。基于磁标量，也就是磁场强度的模，匹配成功率低，仅仅用到了一维的磁场信息。因此iccp、dtw、magcom、磁标签匹配算法和基于磁标量指纹匹配算法都没有充分利用磁场信息。
4.此外，现有基于磁矢量指纹匹配的算法易受异常的磁场量测值干扰，受异常的姿态信息和惯性导航系统误差影响严重。


技术实现要素：

5.针对现有技术的不足，本发明提出了一种基于约束因子图的三维磁矢量和pdr融合导航方法，引入约束因子图模型重构多传感器组合，为pdr因子加入了强约束限制因子更新区间，从而抑制磁场局部的随机异常，在磁场指纹库中引入姿态矩阵动态地解析系统整体误差，降低了导航算法的误差，提高了整体的导航定位精度和稳定性。
6.一种基于约束因子图的三维磁矢量和pdr融合导航方法，包括以下步骤：
7.步骤一、在导航测试区域使用三轴磁强计按网格采集磁场矢量数据。在采集过程中，传感器的姿态保持不变，即航向角始终保持零。对每一轴磁指纹数据进行时间平均计算，构建包含三轴磁场强度与方向的地磁指纹库，使用bicubic插值函数进行插值，构建地磁指纹接口。行人按照测试轨迹在导航测试区域内连续移动，同时采集三轴磁场数据与惯性器件数据，包括三轴磁强计量测、三轴加速度量测、三轴陀螺仪量测。根据步伐监测算法
结合加速度量测值确定行人的步伐点的时刻，确认每一个步伐时刻对应的三轴陀螺仪量测，其中航向角量测作为该行人步伐点的航向角量测；确认每一个步伐时刻对应的三轴磁强计量测，作为该行人步伐点的磁场量测。使用步长估计算法计算行人的单步步长，作为步长量测。
8.步骤二、对pdr相关量测因子建模，具体步骤如下：
9.s2.1、i时刻组合导航系统状态xi表示为：
10.xi＝[xi,yi]
t
[0011]
其中xi和yi表示在i时刻行人在导航区域内固定的平面直角坐标系中的位置的坐标。根据相邻时刻的两个系统状态确认步长，步长量测函数h
step
()为：
[0012]hstep
(xi,x
i+1
)＝|x
i-x
i+1
|
[0013]
其中|
·
|表示距离计算。
[0014]
假设在完成一次跨步的过程中航向角等于前一个状态时刻的航向角，即航向角仅在即将开始下一步跨步时改变，根据相邻时刻的两个系统状态确定前一个时刻的航向角，航向角量测函数h
angle
()为：
[0015][0016]
则i时刻的步长量测stepleni与航向角量测θi描述为：
[0017][0018][0019]
其中是i时刻的步长量测误差，是i时刻的航向角量测误差。进一步对相邻时刻的航向角进行差分，得到i时刻差分航向角
△
θi的量测模型：
[0020][0021]
其中h
dangle
()是差分航向角量测函数，是i时刻差分航向角量测误差。
[0022]
假定任意两个时刻下的步长量测误差相互独立，则不同时刻下的步长量测误差符合同一概率分布；假定相同时间内三轴陀螺仪的偏移误差符合同一高斯型概率分布，则不同时刻下的差分航向角量测误差符合同一高斯型概率分布；因此步长因子f
isteplen
和差分航向角因子f
idangle
描述为：
[0023][0024][0025]
其中∑
step
是步长误差协方差矩阵，∑
dangle
是差分航向角误差协方差矩阵。由于步
长与差分航向角的维度均为1，则有：
[0026]
∑
step
＝(σ
step
)2[0027]
∑
dangle
＝(σ
dangle
)2[0028]
其中，(σ
step
)2表示步长量测误差的方差为(σ
step
)2，(σ
dangle
)2表示差分航向角量测误差的方差。
[0029]
s2.2、将第i时刻采集到地磁量测描述为：
[0030][0031]
其中mag
i,x
，mag
i,y
和mag
i,z
分别为三轴磁传感器坐标系中x轴、y轴、z轴方向上的地磁量测值。
[0032]
则i时刻系统状态xi对应的三轴磁强估计量表示为：
[0033][0034]
其中h
mag
()是地磁接口函数，根据步骤一中的bicubic插值函数推导得到；()是地磁接口函数，根据步骤一中的bicubic插值函数推导得到；分别表示导航区域坐标系下指向x轴正方向、y轴正方向和z轴方向的地磁估计值，即航向角为零时姿态下的三轴磁强的估计值，和分别为对应的简写形式。
[0035]
由于地磁量测与三轴磁强估计量的值来自不同坐标系，无法直接比对。因此引入旋转矩阵进行动态匹配。根据i时刻的航向角估计值构建如下旋转矩阵：
[0036][0037]
通过使用旋转矩阵将零航向角姿态下的地磁估计矩阵转化为三轴传感器坐标系中，再与地磁量测进行比对，构建三维地磁量测因子f
imag
：
[0038][0039]
其中∑
mag
是地磁误差协方差矩阵：
[0040][0041]
其中，分别表示磁传感器三个坐标轴对应的量测误差标准差。
[0042]
s2.3、为步长因子和差分航向角因子引入如下约束条件：
[0043][0044]
其中，表示单步步长约束边界，即步长的量测与估计的绝对误差的最大边界，表示差分航向角的约束边界，即差分航向角的量测与估计之间绝对误差的最大边界。
[0045]
针对本系统的约束因子图建立如下目标函数：
[0046][0047]
其中，x＝[x1,x2,
…
,xi,
…
]表示包含所有时刻状态的序列。x
map
表示根据最大后验概率准则估计的最优的状态序列。
[0048]
由于三维地磁量测因子、步长因子和差分航向角因子均为指数函数形式，因此将该目标函数转化为有约束的非线性最小二乘形式：
[0049][0050]
步骤三、对步骤二建立的约束因子图模型进行解算，得到预测导航轨迹。具体步骤如下：
[0051]
s3.1、变量空间重映射。
[0052]
在步骤二定义的目标函数中，存在不具备一阶可导条件的非线性函数，直接优化求解有困难，因此将目标函数的约束条件重写为：
[0053][0054][0055]
其中，ci和di的取值范围为实数域。将模型中估计变量显式地表示：
[0056][0057]
[0058]
其中，表示为第i时刻的步长估计，表示第i时刻的差分航向角估计。
[0059]
将第i时刻的航向角估计值表示为：
[0060][0061]
其中θ1为已知的初始航向角。j＜i。
[0062]
使用转移方程递推下一时刻的状态估计值
[0063][0064]
其中，x1为初始状态量，即起始点的位置。
[0065]
通过序列c＝{c1,c2,
…
,c
n-1
,cn}、d＝{d1,d2,
…
,d
n-1
,dn}唯一地确认一条导航轨迹，将目标函数的自变量由x转变为c、d，描述为：
[0066][0067]
其中，n表示轨迹的长度。u(c,d)是以c和d为自变量形式的地磁因子分量：
[0068][0069]
s3.2、使用梯度算法求解目标函数的极小值，首先计算目标函数q对ck的偏导：
[0070][0071]
其中，ck表示序列c中第k个元素。。
[0072]
计算u(c,d)对ck的偏导：
[0073][0074]
将其按照坐标方向分别展开：
[0075][0076]
分别计算和对ck的偏导数：
[0077][0078][0079][0080]
进一步展开为：
[0081][0082][0083]
然后计算目标函数q对dk的偏导：
[0084][0085]
其中dk表示序列d中第k个元素。
[0086]
计算u(c,d)对dk的偏导：
[0087][0088]
将其按照坐标方向分别展开：
[0089][0090]
分别计算和对dk的偏导数：
[0091][0092][0093]
分别计算和对dk的偏导数：
[0094][0095][0096][0097]
进一步展开为：
[0098][0099][0100]
则目标函数q取到极小值的条件为：
[0101][0102]
s3.3、遍历序列c、d中每一个元素，按照s3.2中方法计算目标函数q对ck、dk的偏导数，使用adam快速梯度下降法进行迭代更新，得到新的序列c、d，然后通过使用转移方程依次递推下一时刻的状态估计值输出系统状态的估计序列完
成航迹推测。
[0103]
本发明与现有技术相比有如下优点：
[0104]
1、使用因子图优化模型对三维地磁信息和惯性导航信息深度融合，构造了包含姿态矩阵和三维地磁信息深度关联的地磁因子，与差分航向角因子、步长因子同时进行迭代优化。避免了因航向角累计误差引起姿态偏差从而导致的地磁指纹匹配失误的问题，可以满足各种姿态下的地磁匹配要求与各种形状的轨迹的导航定位。
[0105]
2、对步长量测与步长估计的偏差以及差分航向角量测值与差分航向角估计值的偏差引入硬约束，起到约束可行状态搜索空间的作用。同时硬约束在地磁因子中还有约束姿态的作用，从而降低地磁指纹误匹配率。
[0106]
3、构建的三维地磁指纹库包括区域内地面自定义直角坐标系中三正交轴的方向，相较于地磁标量指纹库具有信息密度更高、不同位置指纹差异性更明显的特点。通过bicubic插值函数对三个维度的磁场强度进行插值，使得指纹库接口函数具有一阶可微的性质，为路径的快速解算奠定基础。
[0107]
4、通过构建新的变量重映射关系，将以坐标(x,y)为自变量的原目标函数转化为以实数序列c和d为自变量的目标函数，避免了原目标函数中非线性且不可微的函数优化困难的问题，将有约束问题等价地转换为无束优化问题，并采用快速的adam梯度算法，使导航解算快速收敛。
附图说明
[0108]
图1是融合导航方法系统框图；
[0109]
图2是实施例中磁场矢量采集场景图；
[0110]
图3是实施例中约束因子图的结构示意图；
[0111]
图4是实施例中多种导航定位解算轨迹绝对误差对比图。
具体实施方式
[0112]
以下结合附图对本发明作进一步的解释说明；
[0113]
一种基于约束因子图的三维磁矢量和pdr融合导航方法，构建了三维地磁指纹库，并使用约束因子图建模，与pdr动态融合实现室内复杂轨迹导航定位。通过引入航向角与步长约束条件既缩小了搜索空间，提高了系统抵抗磁异常的能力，又约束了姿态匹配，提高了地磁指纹匹配的成功率，实现行人任意姿态下的动态地磁指纹匹配。通过变量重映射的方式将不可微分的有约束的目标函数转化为可微的无约束的等价函数求解，并使用梯度算法建模，提高了求解速度。如图1所示，具体导航流程包括以下步骤：
[0114]
步骤1、如图2所示，在导航测试区域按网格采集磁场矢量数据，图中的黑色圆形标记点即为磁场矢量采集点。在导航测试区域按横向布置7列采集点，纵向布置21行采集点。采集过程中磁传感器的姿态保持一致，传感器的三轴始终指向测试区域直角坐标系的三轴正方向。对每一轴磁指纹数据进行时间平均计算，构建包含三轴磁场强度与方向的地磁指纹库，并分别使用bicubic插值函数进行插值，构建地磁指纹接口。
[0115]
步骤2、测试人员按照测试轨迹在导航测试区域内连续步行并同时采集三轴磁场数据与惯性器件数据，包括三轴磁强计的量测、三轴加速度量测、三轴陀螺仪量测。根据步
伐监测算法结合加速度量测值确定行人的步伐点的时刻，确认每一个步伐时刻对应的陀螺仪量测，其中航向角量测作为该行人步伐点的航向角量测；确认每一个步伐时刻对应的磁强计量测，作为该行人步伐点的磁场量测，包含磁传感器的三轴量测。使用步长估计算法计算行人的单步步长，作为步长量测。
[0116]
步骤3、分别对地磁因子、差分航向角因子、步长因子建模，其中地磁因子包含使用航向角估计值构造的旋转矩阵表示姿态，用于优化时动态匹配。对差分航向角量测偏差、步长量测偏差加入局部约束，限制因子更新可行域范围。建模完成的因子图结构如图3所示。
[0117]
步骤4、将目标函数通过变量重映射转化为可微分无约束函数求解，使用梯度算法快速迭代。迭代结束时反映射回原变量空间输出导航预测轨迹。
[0118]
使用普通因子图算法(factor graph,fg)、迭代最近等值线匹配算法(iccp)计算轨迹与本方法进行对比实验，结果如图4所示，本方法的误差始终低于其他现有方法，证明本方法可以提高室内导航的精度。
[0119]
以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

技术特征：
1.一种基于约束因子图的三维磁矢量和pdr融合导航方法，其特征在于：具体包括以下步骤：步骤1、在导航测试区域按网格采集磁场矢量数据，采集过程中磁传感器的航向角始终保持为零；对每一轴磁指纹数据进行时间平均计算，构建包含三轴磁场强度与方向的地磁指纹库，使用bicubic插值函数进行插值，构建地磁指纹接口；行人按照测试轨迹在导航测试区域内连续步行，同时采集三轴磁强计量测、三轴加速度量测、三轴陀螺仪量测；根据步伐监测算法结合加速度量测值确定行人的步伐点的时刻，确认每一个步伐时刻对应的陀螺仪量测，其中航向角量测作为该行人步伐点的航向角量测；确认每一个步伐时刻对应的磁强计量测，作为该行人步伐点的磁场量测，包含磁传感器的三轴量测；使用步长估计算法计算行人的单步步长，作为步长量测；步骤2、分别对地磁因子、差分航向角因子、步长因子建模，其中地磁因子通过使用航向角估计值构造的旋转矩阵来表示姿态；为差分航向角量测偏差、步长量测偏差加入局部约束，限制因子更新的可行域范围，建立目标函数并转化为有约束的非线性最小二乘形式；步骤3、通过变量空间重映射，将步骤2建立的目标函数转化为可微分的无约束函数，通过梯度算法求解目标函数的极小值，迭代更新后反映射回原变量空间，推导下一时刻的状态估计值，输出系统状态的估计序列，即导航预测轨迹。2.如权利要求1所述一种基于约束因子图的三维磁矢量和pdr融合导航方法，其特征在于：差分航向角因子、步长因子的建模方法为：将i时刻组合导航系统状态x
i
表示为：x
i
＝[x
i
,y
i
]
t
其中x
i
和y
i
表示在i时刻行人在导航区域内固定的平面直角坐标系中的位置的坐标；根据相邻时刻的两个系统状态确认步长，步长量测函数h
step
()为：h
step
(x
i
,x
i+1
)＝|x
i-x
i+1
|其中|
·
|表示距离计算；假设在完成一次跨步的过程中航向角等于前一个状态时刻的航向角，即航向角仅在即将开始下一步跨步时改变，根据相邻时刻的两个系统状态确定前一个时刻的航向角，航向角量测函数h
angle
()为：则i时刻的步长量测steplen
i
与航向角量测θ
i
描述为：描述为：
其中是i时刻的步长量测误差，是i时刻的航向角量测误差；进一步对相邻时刻的航向角进行差分，得到i时刻差分航向角
△
θ
i
的量测模型：其中h
dangle
()是差分航向角量测函数，是i时刻差分航向角量测误差；假定任意两个时刻下的步长量测误差相互独立，则不同时刻下的步长量测误差符合同一概率分布；假定相同时间内三轴陀螺仪的偏移误差符合同一高斯型概率分布，则不同时刻下的差分航向角量测误差符合同一高斯型概率分布；因此步长因子f
isteplen
和差分航向角因子f
idangle
描述为：描述为：其中∑
step
是步长误差协方差矩阵，∑
dangle
是差分航向角误差协方差矩阵；由于步长与差分航向角的维度均为1，则有：∑
step
＝(σ
step
)2∑
dangle
＝(σ
dangle
)2其中，(σ
step
)2表示步长量测误差的方差为(σ
step
)2，(σ
dangle
)2表示差分航向角量测误差的方差。3.如权利要求1或2所述一种基于约束因子图的三维磁矢量和pdr融合导航方法，其特征在于：为步长因子和差分航向角因子引入如下约束条件：其中，x
i
为i时刻组合导航系统状态，steplen
i
、θ
i
、
△
θ
i
分别为i时刻的步长量测、航向角量测与差分航向角量测，h
step
()、h
dangle
()分别为步长量测函数和差分航向角量测函数，表示单步步长约束边界，表示差分航向角的约束边界。4.如权利要求3所述一种基于约束因子图的三维磁矢量和pdr融合导航方法，其特征在于：地磁因子的建模方法为：将第i时刻采集到地磁量测描述为：其中mag
i,x
，mag
i,y
和mag
i,z
分别为三轴磁传感器坐标系中x轴、y轴、z轴方向上的地磁量测值；则i时刻系统状态x
i
对应的三轴磁强估计量表示为：
其中h
mag
()是地磁接口函数，分别表示导航区域坐标系下指向x轴正方向、y轴正方向和z轴方向的地磁估计值，和分别为对应的简写形式；根据i时刻的航向角估计值构建如下旋转矩阵：通过使用旋转矩阵将零航向角姿态下的地磁估计矩阵转化为三轴传感器坐标系中，再与地磁量测进行比对，构建三维地磁量测因子f
imag
：其中∑
mag
是地磁误差协方差矩阵：其中，分别表示磁传感器三个坐标轴对应的量测误差标准差。5.如权利要求4所述一种基于约束因子图的三维磁矢量和pdr融合导航方法，其特征在于：针对约束因子图建立如下目标函数：其中，x＝[x1,x2,
…
,x
i
,
…
]表示包含所有时刻状态的序列；x
map
表示根据最大后验概率准则估计的最优的状态序列；将目标函数转化为有约束的非线性最小二乘形式：
对其进行解算，得到预测导航轨迹；具体步骤如下：s3.1、变量空间重映射；将目标函数的约束条件重写为：将目标函数的约束条件重写为：其中，c
i
和d
i
的取值范围为实数域；将模型中估计变量显式地表示：的取值范围为实数域；将模型中估计变量显式地表示：其中，表示为第i时刻的步长估计，表示第i时刻的差分航向角估计；将第i时刻的航向角估计值表示为：其中θ1为已知的初始航向角；j＜i；使用转移方程递推下一时刻的状态估计值使用转移方程递推下一时刻的状态估计值其中，x1为初始状态量，即起始点的位置；通过序列c＝{c1,c2,
…
,c
n-1
,c
n
}、d＝{d1,d2,
…
,d
n-1
,d
n
}唯一地确认一条导航轨迹，将目标函数的自变量由x转变为c、d，描述为：其中，n表示轨迹的长度；u(c,d)是以c和d为自变量形式的地磁因子分量：
s3.2、使用梯度算法求解目标函数的极小值，首先计算目标函数q对c
k
的偏导：其中，c
k
表示序列c中第k个元素；；计算u(c,d)对c
k
的偏导：将其按照坐标方向分别展开：分别计算和对c
k
的偏导数：的偏导数：的偏导数：进一步展开为：
然后计算目标函数q对d
k
的偏导：其中d
k
表示序列d中第k个元素；计算u(c,d)对d
k
的偏导：将其按照坐标方向分别展开：分别计算和对d
k
的偏导数：的偏导数：分别计算和对d
k
的偏导数：的偏导数：
进一步展开为：进一步展开为：则目标函数q取到极小值的条件为：s3.3、遍历序列c、d中每一个元素，按照s3.2中方法计算目标函数q对c
k
、d
k
的偏导数，使用adam快速梯度下降法进行迭代更新，得到新的序列c、d，然后通过使用转移方程依次递推下一时刻的状态估计值输出系统状态的估计序列完成航迹推测。

技术总结
本发明公开了一种基于约束因子图的三维磁矢量和PDR融合导航方法，使用三维磁矢量数据构建地磁指纹接口，估算行人步长和航向角的量测值，以及地磁量测值。引入包含航向角估计量的旋转矩阵表示姿态，构建约束因子图模型，将各种量测值进行融合，并引入约束条件缩小搜索空间。利用变量重映射，将不可微分有约束的原目标函数转化为可微分无约束的等价函数，在新的变量空间中利用Adam梯度优化算法求解参数，输出导航轨迹的预测值。该方法利用旋转矩阵估计姿态实现地磁指纹的动态匹配，并限制了因子更新范围，约束姿态矩阵的可行域，提高匹配成功率与抗地磁异常的能力。配成功率与抗地磁异常的能力。配成功率与抗地磁异常的能力。


技术研发人员：李泽华 尚俊娜 施浒立 应娜
受保护的技术使用者：杭州电子科技大学
技术研发日：2023.07.04
技术公布日：2023/10/6