基于惯性系统的室内行人航向融合约束算法的制作方法

本发明涉及导航、制导与控制技术领域，尤其涉及一种基于惯性系统的室内行人航向融合约束算法。

背景技术：

随着微机电技术的发展，体积小、质量轻、功耗低的mems微惯性测量单元(microinertialmeasurementunit，mimu)的导航定位技术逐渐成为一个热门的研究方向。这种基于自包含传感器微惯性导航系统无需依靠外部设备(如全球卫星定位系统，测距设备等)，即可实时地提供行人位置、速度、姿态等导航定位信息，从而独立自主的实现导航定位和轨迹跟踪。

在传统的室内行人导航定位算法中，大多只采取了单一的航向角约束算法。主要有基于地图匹配的航向角约束算法，利用磁力计输出值抑制零速区间航向误差算法、行人行走方向航向约束算法，航向自观测、自约束算法等，这些算法都能够在一定的程度上抑制航向漂移，从而修正航向误差实现导航定位功能，但都存在明显的局限性。室内地图匹配算法仅适用于行人行走方向与道路一致的情况下，当行人行走偏离道路路向，如存在避让行人、非直线行走等特殊情况时，基于地图匹配的航向角约束算法不再实用。利用磁力计输出可以有效抑制零速区间航向误差，但室内金属材料较多，对磁力计的干扰较大，因此会对航向误差造成一定影响。当行人行走方向与设定方向相匹配时，使用该条道路主方向约束行人行走航向，仅限于抑制行人直线行走时的航向误差，具体应用场景比较少。航向自观测、自约束算法利用零速区间除第一个采样外的其余点的航向角等于第一个采样点对应的航向角，或航向角等于零速区间航向角均值，从而提高航向角精度，这种方法虽不受使用场景的限制，但精度不高。

技术实现要素：

为解决现有技术存在的局限和缺陷，本发明提供一种基于惯性系统的室内行人航向融合约束算法，包括：

形成室内环境信息地图，所述室内环境信息地图使用xml语言对道路信息进行存储，建立室内无向地图；

根据行人运动在零速区间的误差方程，获得卡尔曼滤波器的状态变量，计算公式如下：

其中，姿态角误差为位置误差为δγ，速度误差为δν；

根据行人的行走航向与行人所处道路航向之差的模值设置阈值ω1；

获得所述卡尔曼滤波器的观测量，计算公式如下：

其中，测量矩阵为

观测噪声矩阵为r＝diag{(0.03)²(0.01)²(0.01)²(0.01)²}

根据所述观测量与所述阈值ω1判断行人是否偏离所述室内环境信息地图的道路中心线航向。

可选的，所述根据行人运动在零速区间的误差方程，获得卡尔曼滤波器的状态变量的步骤还包括：

获得线性化系统状态转移矩阵，计算公式如下：

其中，观测量为行人处于步态周期之中的站立期的实际速度值，计算公式为：观测矩阵为h＝[o3×3o3×3i3×3]；

通过更新姿态变换矩阵获得姿态误差补偿，计算公式如下：

其中，

可选的，所述根据行人运动在零速区间的误差方程，获得卡尔曼滤波器的状态变量的步骤还包括：

当行人处于步态周期之中的站立期时进行时间更新和量测信息更新；

当行人处于步态周期之中的非站立期时进行时间更新；

根据系统状态方程和量测信息方程获得惯性导航系统的数学模型，计算公式如下：

其中，xk是(k)时刻的系统n维状态变量，φk,k-1是系统从(k-1)时刻到(k)时刻的n×n维状态转移矩阵，wk-1是(k-1)时刻系统的过程噪声，zk是(k)时刻系统的观测量，hk是(k)时刻系统的观测矩阵，vk是(k)时刻系统的观测噪声。

可选的，还包括：

当所述观测量小于所述阈值ω1时，所述室内环境信息地图匹配航向约束算法；

当所述观测量大于所述阈值ω1时，根据单个复步周期之内行人的行走航向角φ(k)的变换量，判断行人是否处于直线行走状态；

根据行人的行走航向设置阈值ω2，通过比较相邻两个零速区间的航向角变化率与阈值ω2的大小判断行人是否处于直线行走状态，判定方程如下：

当时，行人处于直线行走状态；

当时，行人处于曲线行走状态。

可选的，还包括：

当行人处于直线行走状态时，所述卡尔曼滤波器的观测量表示为：

可选的，还包括：

当行人处于曲线行走状态时，所述卡尔曼滤波器的观测量表示为：

本发明具有下述有益效果：

(1)本实施例提供的技术方案避免了传统基于惯性系统的室内行人航向约束算法中只采取了单一的航向角约束算法，根据室内多变的道路情况，结合了各种航向约束算法的优点，以基于室内无向图的地图匹配航向约束算法为主，自观测、自约束算法和行人行走方向航向约束为辅，提供了一种根据行人在室内处于不同运动状态，分别采用三种航向约束的融合算法，使航向约束算法能够更广泛的适用于室内地形，有效减少航向角误差，提高系统导航定位精度。

(2)本实施例提供的技术方案利用xml语言编写室内无向地图，存储不同编号道路的相关信息。xml语言可操作性强，标签可自行定义，简单易用，适合存储室内道路地图这种类似网状结构的道路分布图。

(3)本实施例提供的技术方案根据行人的行走航向与行人所处道路航向之差的模值设定阈值，使用阈值来判断行人是否偏离室内地图的道路中心线航向。当行人的行走航向与行人所处道路航向之差的模值小于阈值，继续使用地图道路航向约束。当行人的行走航向与行人所处道路航向之差的模值大于阈值，判断行人是否处于直线行走状态。当行人处于直线行走状态时，采用行人行走方向航向约束。当行人处于非直线行走状态时，采用航向自约束算法，从而实现在不同室内场景地形和行人不同行走状态之下的航向角约束的目的。

附图说明

图1为本发明实施例一提供的室内融合航向约束算法的流程图。

图2为本发明实施例一提供的室内融合航向约束算法的行人行走示意图。

具体实施方式

为使本领域的技术人员更好地理解本发明的技术方案，下面结合附图对本发明提供的基于惯性系统的室内行人航向融合约束算法进行详细描述。

实施例一

本实施例提供一种基于惯性系统的室内行人航向融合约束算法，以基于室内无向图的地图匹配航向约束算法为主，自观测、自约束算法和行人行走方向航向约束为辅，设置一种根据行人在室内处于不同运动状态，分别采用三种航向约束的融合算法，使航向约束算法能够更广泛的适用于室内地形，有效减少航向角误差。

本实施例提供的基于惯性系统的室内行人航向融合约束算法，包括如下步骤：

步骤1，建立室内环境信息地图，考虑到室内环境之中，行人行走道路多为横平竖直的走廊、较为开阔的公共活动区域等道路之间的关系较为简单。因此，本实施例利用xml语言对道路进行存储，建立无向地图。

xml文件主要包括文件头和文件体两个部分，文件头包括文件声明和可省略的文件类型的声明，文件体是文件的主要内容，主要是由多个xml元素组成，每个xml元素由起始标签、标签表征的数据内容和结束标签组成，而且各个元素之间可以存在嵌套关系。在建立室内无向地图时，本实施例使用自定义标签<maproad>、</maproad>表示将要存储的地图道路信息，即每一个<maproad>标签都对应存储了一条道路的相关信息。

可用如下xml语言存储道路信息：

<maproad_1>

<maproad_2>

crossx，crossy，crossz

</maproad_2>

<maproad_1>

startx，starty，startz，endx，endy，endz，roadwidth，maproad_1_，maproad_1_d

</maproad_1>

</maproad_1>

其中，<maproad_1></maproad_1>表示编号为1的道路所存储的信息，在道路1中存储了道路2与道路1的道路中心线连接点的位置坐标信息crossx，crossy，crossz分别表示x轴坐标，y轴坐标和z轴坐标，同时道路1中存储了道路1中心线的起始点和终止点的位置坐标startx，starty，startz，endx，endy，endz(分别表示起始点和终止点的x，y，z轴的坐标)，以及路宽roadwidth，路向maproad_1_φ，路长maproad_1_d。

步骤2，根据行人在站立期进行时间更新和量测信息更新，非站立期只进行时间更新，惯性导航系统的数学模型可用系统状态方程和量测信息方程设置如下：

其中，xk是(k)时刻的系统n维状态变量，φk,k-1是系统从(k-1)时刻到(k)时刻的n×n维状态转移矩阵，wk-1是(k-1)时刻系统的过程噪声，zk是(k)时刻系统的观测量，hk是(k)时刻系统的观测矩阵，vk是(k)时刻系统的观测噪声。

本实施例中，卡尔曼滤波中要求系统过程噪声和观测噪声满足高斯分布。即e[wk]＝0，e[νk]＝0，cov(wk)＝qk，cov(νk)＝rk，其中，qk为系统过程噪声协方差矩阵，rk为观测噪声的协方差矩阵。

qk＝diag{(0.01)²(0.01)²(0.01)²000(0.01)²(0.01)²(0.01)²}

rk＝diag{(0.01)²(0.01)²(0.01)²}

步骤3，根据行人行走航向与行人所处道路航向之差的模值设定阈值的大小来判断行人是否偏离室内地图的道路中心线航向。

优选的，当行人行走航向与行人所处道路航向之差的模值小于阈值，继续用地图道路航向约束，当行人行走航向与行人所处道路航向之差的模值大于阈值，判断行人是否处于直线行走状态。

优选的，当行人处于直线行走状态时，采用行人行走方向航向约束；当行人处于非直线行走状态时，采用航向自约束算法。

图1为本发明实施例一提供的室内融合航向约束算法的流程图。如图1所示，本实施例提供的基于惯性系统的室内行人航向融合约束算法，包括以下步骤：

步骤1，建立室内环境信息地图，考虑至室内环境之中，行人行走道路多为横平竖直的走廊、较为开阔的公共活动区域等道路之间的关系较为简单。本实施例利用xml语言对道路进行存储，建立无向地图。

xml文件主要包括文件头和文件体两个部分，文件头包括文件声明和可省略的文件类型的声明，文件体是文件的主要内容，主要是由诸多xml元素组成，每个xml元素由起始标签、标签表征的数据内容和结束标签组成，各个元素之间可以存在嵌套关系。在建立室内无向地图时，使用自定义标签<maproad>、</maproad>表示将要存储的地图道路信息，即每一个<maproad>标签都对应存储了一条道路的相关信息。

本实施例中，可用如下xml语言存储道路信息：

<maproad_1>

<maproad_2>

crossx＝10，crossy＝0，crossz＝0

</maproad_2>

<maproad_1>

startx＝0，starty＝0，startz＝0，endx＝10，endy＝0，endz＝0，roadwidth＝1.5

maproad_1_＝0，maproad_1_d＝10

</maproad_1>

</maproad_1>

<maproad_2>

<maproad_1>

crossx＝10，crossy＝0，crossz＝0

</maproad_1>

<maproad_2>

startx＝10，starty＝0，startz＝0，endx＝10，endy＝10，endz＝0，roadwidth＝1.5

maproad_1_＝90，maproad_1_d＝10

</maproad_2>

</maproad2>

……

其中，<maproad_1></maproad_1>表示编号为1的道路所存储的信息，在道路1中存储了道路2与道路1的道路中心线连接点的位置坐标信息crossx＝10，crossy＝0，crossz＝0(x轴坐标为10m，y轴坐标为0m，z轴坐标为0m)，同时道路1中存储了道路1中心线的起始点和终止点的位置坐标startx＝0，starty＝0，startz＝0，endx＝10，endy＝0，endz＝0，以及路宽roadwidth＝1.5m，maproad_1_＝90度，maproad_1_d＝10m。可根据室内具体道路场景利用xml语言，按照步骤1的格式编写建立室内无相地图。

步骤2，根据行人运动在零速区间的误差方程，获得卡尔曼滤波器的状态变量，计算公式如下：

其中，姿态角误差为位置误差为δγ，速度误差为δν。

本实施例提供的线性化系统状态转移矩阵如下：

本实施例以行人步态周期之中的站立期实际速度值作为观测量航向角观测量设计可参考步骤3，步骤4，步骤5，步骤6，观测矩阵为h＝[03×303×3i3×3]。

本实施例提供的姿态误差补偿可以通过以下更新姿态变换矩阵完成：

其中，

步骤3，根据行人行走航向与行人所处道路航向之差的模值设定阈值的大小来判断行人是否偏离室内地图中道路中心线航向。

由室内地图可知路1(maproad_1)的路向为maproad_1_φ(k)，路长为maproad_1_d(k),根据单足上的imu测量数据，解算出相邻两个零速区间行人所处位置的航向角φ，以道路的路向maproad_1_φ(k)为参考航向，卡尔曼滤波器的观测量可表示为：

其中，测量矩阵为

观测噪声矩阵为r＝diag{(0.03)²(0.01)²(0.01)²(0.01)²}

当观测量小于设定的阈值ω1时，即地图匹配航向约束算法。当观测量超出设定的阈值ω1时，执行步骤4。

步骤4，当观测量超出设定的阈值ω1时，通过计算一个复步周期行人行走航向角φ(k)的变换量，判断行人是否处于直线行走状态。

图2为本发明实施例一提供的室内融合航向约束算法的行人行走示意图。如图2所示，假设行人装有imu的脚起始处位于1处，当行人处于直线行走(1-2-3)时，经过一个复步周期，航向角变化率基本保持不变。当行人处于曲线行走(1-4-5)时，经过一个复步周期，航向角变化率因此，本实施例根据行人行走航向设置阈值ω2，通过比较相邻两零速区间的航向角变化率与阈值ω2的大小来判断行人是否处于直线行走状态，判定方程如下：

当时，行人处于直线行走状态；

当时，行人处于曲线行走状态。

步骤5，当时，行人处于直线行走状态，可采用行人行走方向航行约束算法。行人行走过程中，足部每一次的站立期对应一个零速区间，将第一个零速区间对应的航行角采样点均值记为如果行人沿当前方向直线行走时，可使用作为参考航向约束后续零速区间航向角此时，卡尔曼滤波器的观测量可以表示为：

测量矩阵h，和观测噪声矩阵r，可参考步骤3。

步骤6，当时，行人处于曲线行走状态，可采用航向自观测、自约束算法。将零速区间第一个采样点对应的航行记为如果行人沿当前方向曲线行走时，可使用作为参考航向约束(k)时刻零速区间其他采样点航向角i＝2,3,4,l；k＝1,2,3,l。此时，卡尔曼滤波器的观测量可以表示为：

测量矩阵h，和观测噪声矩阵r，可参考步骤3。

本实施例提供一种基于惯性系统的室内行人航向融合约束算法，根据行人的行走航向与行人所处道路航向之差的模值设定阈值，使用阈值来判断行人是否偏离室内地图的道路中心线航向。当行人的行走航向与行人所处道路航向之差的模值小于阈值，继续使用地图道路航向约束。当行人的行走航向与行人所处道路航向之差的模值大于阈值，判断行人是否处于直线行走状态。当行人处于直线行走状态时，采用行人行走方向航向约束。当行人处于非直线行走状态时，采用航向自约束算法，从而实现在不同室内场景地形和行人不同行走状态之下的航向角约束的目的。

可以理解的是，以上实施方式仅仅是为了说明本发明的原理而采用的示例性实施方式，然而本发明并不局限于此。对于本领域内的普通技术人员而言，在不脱离本发明的精神和实质的情况下，可以做出各种变型和改进，这些变型和改进也视为本发明的保护范围。