CN117516515A - 一种足履惯导/头戴视觉协同的人员定位装置和方法 - Google Patents

Abstract

一种足履惯导/头戴视觉协同的人员定位装置和方法，属于多传感器融合和行人导航领域。卫星信号良好时，观测量处理单元处理卫星导航定位单元及足部惯导的数据，经过融合滤波器处理，得到行人的定位结果；卫星信号薄弱时，观测量处理单元对足部惯导及头戴视觉的数据进行处理，将视觉的定位和零速信息作为观测量，通过融合滤波器对INS推算的导航状态量进行修正,得到行人定位结果。本发明适用于多传感器融合和行人导航领域，通过北斗、头戴视觉和足履惯导的多传感器融合定位方案，实现室内外行人无缝导航的目的，提升导航精度，降低设备投入成本。

Description

一种足履惯导/头戴视觉协同的人员定位装置和方法

技术领域

本发明涉及一种足履惯导/头戴视觉协同的人员定位装置和方法，属于多传感器融合和行人导航领域。

背景技术

随着科学技术的飞速发展以及人们生活水平的提高，人们对于导航和定位服务的需求越来越高。现代社会中，人们的日常活动已经不再受限于固定的场所，而是随着时间的推移发生复杂的变化。在一些专业领域，例如执勤、消防救援、电气安装和管道维护等，人员的位置变化必须得到及时响应和指挥调度。

随着微型计算机性能的提高和物联网技术的发展，行人导航定位产品越来越多。根据不同的技术方案和应用场景，可以将与行人导航定位相关的场景分为室内和室外两种。在室外场景中，全球导航卫星***可以提供高精度的三维位置坐标，但在大型建筑、城市峡谷、深山等室外环境中，卫星信号被严重遮挡，导致卫星与接收机通信中断，只依靠卫星导航无法及时更新位置信息和提供导航服务。近年来，超宽带(UWB)、蓝牙、ZigBee、WIFI等技术得到了快速发展。这些技术可以通过将基站放置在室内或特定位置，利用测距原理或指纹匹配等方法来估计行人的位置。这类技术已经比较成熟，部分技术已经开始应用于商业领域，但局限性也较明显：首先，在特定环境中必须先放置基站，但是设备的成本较高，维护起来也比较困难；其次，室内环境相对复杂，存在定位盲区，易受非视距影响。此外，在森林区域、应急救援场景等室外遮蔽物较多的区域，上述技术较难满足定位需求。

发明内容

针对目前室内定位成本较高且难以满足遮蔽物较多的室内定位需求的问题，本发明的主要目的是提供一种足履惯导/头戴视觉协同的人员定位装置和方法，通过头戴视觉和足履惯导的多传感器融合定位，实现室内外行人无缝导航的目的，提升导航精度，降低设备投入成本。

本发明的目的是通过以下技术方案实现的：

本发明公开的一种足履惯导/头戴视觉协同的人员定位装置包括：穿戴式智能设备、观测量处理单元及融合滤波器；穿戴式智能设备通过硬件获取惯性测量单元(InertialMeasurement Unit，IMU)、相机及北斗卫星导航定位单元的原始数据。在室外卫星信号良好时，观测量处理单元分别对北斗卫星导航定位单元以及足部惯导的数据进行处理，通过融合滤波器融合北斗定位结果与足部惯导数据，得到行人的定位结果；在室内卫星信号薄弱时，观测量处理单元对足部惯导及头戴视觉的数据进行处理，融合滤波器通过卡尔曼滤波将视觉的定位和零速探测模块探测的零速信息作为观测量对INS推算的导航状态误差进行估计，然后对INS推算的导航状态量进行修正,最终得到行人定位结果。

穿戴式智能设备包括：IMU、相机及北斗卫星导航定位单元；IMU与行人的足部进行捷联，提供加速度和角速度的原始数据；相机与头盔进行捷联，提供视频流的原始数据；卫星导航定位单元提供卫星导航***的原始观测数据；

观测量处理模块包括：IMU处理单元、视频流处理单元、北斗卫星导航处理单元；IMU处理单元包括用户运动模式识别模块以及零速探测模块，对IMU提供的加速度和角速度数据进行处理得到速度、位置及姿态并传递给融合滤波器；视频流处理单元处理头部相机传输的视频流，检测头戴视觉摄像头检测到空间视野中存在的视觉标签，提取的人员三维绝对位置信息并传送给所述融合滤波器；北斗卫星导航处理单元处理伪距、载波相位观测数据得到位置和速度信息并传送给所述融合滤波器。

融合滤波器包括观测模型和***模型；***模型对导航状态误差进行预测，观测模型利用观测量对***模型预测的导航状态误差进行融合处理并对INS推算的行人的速度、位置、姿态进行修正

本发明公开的一种足履/头戴协同的人员定位方法，基于一种足履/头戴协同的人员定位装置实现，包括以下步骤：

步骤1、穿戴式设备利用自身硬件获取IMU、相机及北斗卫星导航定位单元的原始数据；

步骤2、观测量处理单元处理步骤1所获取的原始数据，向融合滤波器提供速度、位置及姿态数据；

当室外卫星信号良好时，卫星导航处理单元处理卫星导航定位单元的原始数据获得行人的位置信息，该位置信息用于修正足部惯导的误差，提高足部惯导的精度；当室内卫星信号薄弱时，视频流处理单元处理头部相机传输的视频流，检测头戴视觉摄像头检测到空间视野中存在的视觉标签，提取人员的三维绝对位置信息，该位置信息用于修正足部惯导的误差，提高足部惯导的精度。

为充分发挥头戴视觉装备的环境感知功能，结合标签识别的快速重定位技术获取到的精准绝对位置信息，消除足履惯性装备在连续相对定位中产生的累积误差并针对偏差定位轨迹进行重新平滑滤波，基于卡尔曼滤波框架实现信息融合定位。

首先给出卡尔曼滤波器的***状态δX的定义：

δr为位置误差，δv为速度误差，为姿态误差，δb_g为陀螺零偏误差，δb_a为加速度计零偏误差。

以下分别为导航坐标系n系下的位置、速度、姿态、陀螺传感器零偏和加速度计传感器零偏的连续时间误差方程：

其中为载体坐标系转化至导航坐标系的姿态转移矩阵；f^b为加速度计在b系下测量的比力；×表示反对称矩阵；/>为失准角，即姿态误差；gⁿ示当地重力矢量在n系中的投影；/>中的下标n表示的是运动坐标系为n系，描述载体的运动；下标e表示的是参考坐标系为地球坐标系e系，即运动所参照的坐标系；上标n表示投影坐标系，指定投影坐标系后，能够写出向量在该坐标系下的坐标值；/>表示n系相对于i系的旋转角速度向量在n系下的投影；/>表示b系相对于i系的旋转角速度向量在b系下的投影；/>表示e系相对于i系的旋转角速度向量在n系下的投影。

传感器零偏误差建模为一阶高斯马尔可夫过程。式中：

载体移动时，导航坐标系会随之变化，导航坐标系转移速率定义为：

为大地纬度，v_E为东向速度，v_N为北向速度，h表示高程。式中R_M和R_N由下列公式计算：

R_N为卯酉圈主曲率半径，R_M为子午圈主曲率半径；a为地球椭球半长轴，e为地球模型第一偏心率。地球的自转速度在n系下表示为：

ω_e是一个标量，代表地球自转角速率。

为得到***状态方程，首先需要得到的连续时间微分方程：

其中F(t)为***动态矩阵，w(t)为***噪声向量，G(t)为***噪声驱动矩阵，其中w＝[w_g w_a w_bg w_ba]且有：

E(w(t)w(τ)^T)＝q(t)δ(t-τ)

0表示三零矩阵，I表示三阶单位矩阵，w_g是陀螺测量噪声，w_a是加速度计测量噪声。w_bg是陀螺零偏马尔可夫过程激励白噪声，w_ba是加速度计零偏马尔可夫过程激励白噪声。

其中：

T_g为述陀螺仪零偏马尔可夫过程的相关时间矩阵，T_a为加速度计零偏马尔可夫过程的相关时间矩阵：

为方便计算，在此给出四个辅助矩阵：

其中：

所述融合滤波器通过卡尔曼滤波来实现，所述融合滤波器中的观测模型定义如下：

Z_k＝H_kδX_k+e_k,E(e_k ^Te_k)＝R

Z_k为观测向量，H_k为观测矩阵，量测噪声用e_k表示，其相应的量测噪声方差协方差矩阵用R表示。Z_k，H_k随着观测量的不同变化。

当处于零速区间时，零速区间的速度相当于直接对速度误差的观测，因此观测矩阵为：

H_k＝[I 0 0 0 0]

当头戴视觉摄像头检测到空间视野中存在的视觉标签，提取到人员三维绝对位置信息时，对应的观测向量表示为INS推算的位置与人员三维绝对位置之差，观测矩阵为：

l^b为IMU测量中心指向头部相机相位中心的向量在b系下的投影，表示方向余弦矩阵，能够将三维向量在n系下的投影变换到b系下的投影。

根据当前量测和状态向量，下一时刻的状态向量和协方差矩阵通过卡尔曼滤波更新公式得到：

K为增益矩阵，P_k+1∣k为k+1时刻的状态最优估计的方差协方差阵的预测值，H为观测矩阵，R_k为量测噪声方差阵，X_k+1为最优估计，X_k+1∣k为k+1时刻的状态向量预测值，P_k+1为误差协方差阵。

步骤3、经过融合滤波器的处理最终得到行人导航结果，实现室内外行人无缝导航的目的，提升导航精度，降低设备投入成本。

有益效果：

本发明公开的一种足履惯导/头戴视觉协同的人员定位装置和方法，将卫星导航、视觉及惯性导航，通过融合滤波组合在一起，解决卫星信号薄弱时定位精度差的问题，以及足履惯性导航误差累计的问题，实现室内外行人无缝导航的目的，提升导航精度，降低设备投入成本。

附图说明

图1为本发明公开的一种足履惯导/头戴视觉协同的人员定位装置的结构示意图；

附图标记：1-穿戴式设备、2-观测量处理模块、3-及融合滤波器、4-惯性测量单元、5-卫星导航定位单元以及视觉单元、6-IMU处理单元、7-位置信息处理单元、8-***模型、9-观测模型、10-INS机械编排模块、11-IMU误差补偿模块、12-零速探测模块、13-速度观测模型、14-位置观测模型、15-卫星导航处理单元、16-视频流处理单元。

图2为本发明公开的一种足履惯导/头戴视觉协同的人员定位方法的流程示意图。

具体实施方式

下面将结合附图和实施例对本发明加以详细说明。同时也叙述了本发明技术方案解决的技术问题及有益效果，需要指出的是，所描述的实施例仅旨在便于对本发明的理解，而对其不起任何限定作用。

如图1所示，一种基于足履/头戴协同的人员定位装置和方法，包括：穿戴式智能设备、观测量处理模以及融合滤波器。穿戴式智能设备利用自身硬件获取惯性测量单元(Inertial Measurement Unit,IMU)、相机、和北斗全球导航卫星***的原始数据，观测量处理单元处理穿戴式设备提供的原始数据以提供位置或速度等观测量给融合滤波器，经过融合滤波器的处理最终得到行人导航结果。上述一种基于足履/头戴协同的人员定位装置IMU与行人的足部进行固联，相机与头盔进行固联。

上述穿戴式智能设备1包括现有智能设备常见的IMU、相机、北斗等；IMU4提供加速度和角速度的原始数据；相机提供视频流的原始数据；北斗提供速度和位置数据。穿戴式智能设备任何可以提供观测信息的其他传感器都可以包含在所提出的一种基于足履/头戴协同的人员定位装置和方法中。

上述观测量处理单元包括：IMU处理单元，北斗处理单元，视频流处理单元；IMU处理单元处理IMU提供的原始加速度以及角速度数据，并对行人的运动模式进行识别，然后将解算出的位置、速度信息传递给融合滤波器；卫星信号良好时，北斗处理单元处理北斗原始数据得到行人的位置和速度信息；当处于室内环境时，视频流处理单元处理头部相机传输的视频流，检测头戴视觉摄像头检测到空间视野中存在的视觉标签，提取的人员三维绝对位置信息并传送给所述融合滤波器。

所述用户运动模式识别模块根据所述穿戴式智能设备的IMU和其他可选的硬件提供的原始数据识别出静止、行走、跑步等用户运动模式；IMU处理单元解算出IMU位置信息传送给所述融合滤波器。

为充分发挥头戴穿戴式视觉装备的环境感知功能，结合标签识别的快速重定位技术获取到的精准绝对位置信息，消除足履惯性装备在连续相对定位中产生的累积误差并针对偏差定位轨迹进行重新平滑滤波，基于KF滤波框架实现信息融合定位。

首先给出***状态的定义：

分别为n系下的位置、速度、姿态和传感器零偏的连续时间误差方程：

传感器零偏误差建模为一阶高斯马尔可夫过程，式子中：

载体移动时，导航坐标系的轴会随之变化，导航坐标系转移速率定义为：

为大地维度，v_E为东向速度，v_N为北向速度，h表示高程。式中R_M和R_N由下列公式计算：

R_N为卯酉圈主曲率半径，R_M为子午圈主曲率半径；a为地球椭球半长轴，e为地球模型第一偏心率，对于WGS84椭球，a＝6378137.0m，e＝0.006694379988635。在n系下，将地球的自转速度定义为：

ω_e是一个标量，代表地球自转角速率，约15度每小时。

为载体坐标系转化至导航坐标系的姿态转移矩阵，f^b为加速度计在b系下测量的比力，×表示反对称矩阵，/>为失准角，即姿态误差，gⁿ示当地重力矢在n系中的投影，/>中的下标n表示的是运动坐标系为n系(描述载体的运动)，下标e表示的是参考坐标系为e系(运动所参照的坐标系)。上标n表示投影坐标系，指定投影坐标系后，可以写出向量在该坐标系下的坐标值，/>同上述解释。

***连续时间状态空间模型如下：

其中w＝[w_g w_a w_bg w_ba]且有：

E(w(t)w(τ)^T)＝q(t)δ(t-τ)

0表示三零矩阵，I表示三阶单位矩阵，w_g是陀螺测量噪声，w_a是加速度计测量噪声。w_bg是陀螺零偏马尔可夫过程激励白噪声，w_ba是加速度计零偏马尔可夫过程激励白噪声。

T_g为述陀螺仪零偏马尔可夫过程的相关时间矩阵，T_a为加速度计零偏马尔可夫过程的相关时间矩阵：

为方便计算，在此给出四个辅助矩阵：

其中：

GRS80椭球重力场参数如下：

则F矩阵中的部分元素可以表示为：

融合滤波器实现为KF的测量模型定义如下：

Z_k为观测向量，H_k为观测矩阵，量测噪声用e_k表示，其相应的量测噪声方差协方差矩阵用R表示。Z_k，H_k随着观测量的不同变化。

当处于零速区间时，零速区间的速度相当于直接对速度误差的观测，因此观测矩阵为：

H_k＝[I 0 0 00]

当头戴视觉摄像头检测到空间视野中存在的视觉标签，提取到人员三维绝对位置信息时，对应的观测向量表示为INS推算的位置与人员三维绝对位置之差，观测矩阵为：

l^b为IMU测量中心指向头部相机相位中心的向量在b系下的投影，表示方向余弦矩阵，可以将三维向量在n系下的投影变换到b系下的投影。

根据当前量测和状态向量，下一时刻的状态向量可表示为：

然后将卡尔曼滤波器输出的位置误差、速度误差、姿态误差、陀螺零偏误差、加速度计零偏误差反馈给IMU处理单元，

一种足履惯导/头戴视觉协同的人员定位装置及方法，将IMU与足部捷联，将相机固定在头部，实现了北斗/惯导/视觉融合的定位***，实现了对行人室内的准确定位。

以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明，任何熟悉本专业的技术人员，在不脱离本发明技术方案范围内，当可利用上述揭示的技术内容作出些许更动或修饰为等同变化的等效实施例，但凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。

Claims (3)

1.一种足履惯导/头戴视觉协同的人员定位装置，其特征在于：包括穿戴式智能设备、观测量处理单元及融合滤波器；穿戴式智能设备通过硬件获取惯性测量单元即IMU、相机及北斗卫星导航定位单元的原始数据；观测量处理单元处理穿戴式智能设备获取的数据，经过融合滤波器处理，得到行人的定位结果。

2.如权利要求1所述的一种足履惯导/头戴视觉协同的人员定位装置，其特征在于：

穿戴式智能设备包括：IMU、相机及北斗卫星导航定位单元；IMU与行人的足部进行捷联，提供加速度和角速度的原始数据；相机与头盔进行捷联，提供视频流的原始数据；卫星导航定位单元提供卫星导航***的原始观测数据；

观测量处理模块包括：IMU处理单元、视频流处理单元、北斗卫星导航处理单元；IMU处理单元包括用户运动模式识别模块以及零速探测模块，对IMU提供的加速度和角速度数据进行处理得到速度、位置及姿态并传递给融合滤波器；视频流处理单元处理头部相机传输的视频流，检测头戴视觉摄像头检测到空间视野中存在的视觉标签，提取的人员三维绝对位置信息并传送给所述融合滤波器；北斗卫星导航处理单元处理伪距、载波相位观测数据得到位置和速度信息并传送给所述融合滤波器；

融合滤波器包括观测模型和***模型；***模型对导航状态误差进行预测，观测模型利用观测量对***模型预测的导航状态误差进行融合处理并对INS推算的行人的速度、位置、姿态进行修正。

3.一种足履/头戴协同的人员定位方法，基于一种足履/头戴协同的人员定位装置实现，其特征在于：包括以下步骤：

步骤1、穿戴式设备利用自身硬件获取IMU、相机及北斗卫星导航定位单元的原始数据；

步骤2、观测量处理单元处理步骤1所获取的原始数据，向融合滤波器提供速度、位置及姿态数据；

当室外卫星信号良好时，卫星导航处理单元处理卫星导航定位单元的原始数据获得行人的位置信息，该位置信息用于修正足部惯导的误差，提高足部惯导的精度；当室内卫星信号薄弱时，视频流处理单元处理头部相机传输的视频流，检测头戴视觉摄像头检测到空间视野中存在的视觉标签，提取人员的三维绝对位置信息，该位置信息用于修正足部惯导的误差，提高足部惯导的精度；

为充分发挥头戴视觉装备的环境感知功能，结合标签识别的快速重定位技术获取到的精准绝对位置信息，消除足履惯性装备在连续相对定位中产生的累积误差并针对偏差定位轨迹进行重新平滑滤波，基于卡尔曼滤波框架实现信息融合定位；

首先给出卡尔曼滤波器的***状态δX的定义：

δr为位置误差，δv为速度误差，为姿态误差，δb_g为陀螺零偏误差，δb_a为加速度计零偏误差；

以下分别为导航坐标系n系下的位置、速度、姿态、陀螺传感器零偏和加速度计传感器零偏的连续时间误差方程：

其中为载体坐标系转化至导航坐标系的姿态转移矩阵；f^b为加速度计在b系下测量的比力；×表示反对称矩阵；/>为失准角，即姿态误差；gⁿ示当地重力矢量在n系中的投影；中的下标n表示的是运动坐标系为n系，描述载体的运动；下标e表示的是参考坐标系为地球坐标系e系，即运动所参照的坐标系；上标n表示投影坐标系，指定投影坐标系后，能够写出向量在该坐标系下的坐标值；/>表示n系相对于i系的旋转角速度向量在n系下的投影；/>表示b系相对于i系的旋转角速度向量在b系下的投影；/>表示e系相对于i系的旋转角速度向量在n系下的投影；

传感器零偏误差建模为一阶高斯马尔可夫过程；式中：

载体移动时，导航坐标系会随之变化，导航坐标系转移速率定义为：

为大地纬度，v_E为东向速度，v_N为北向速度，h表示高程；式中R_M和R_N由下列公式计算：

R_N为卯酉圈主曲率半径，R_M为子午圈主曲率半径；a为地球椭球半长轴，e为地球模型第一偏心率；地球的自转速度在n系下表示为：

ω_e是一个标量，代表地球自转角速率；

为得到***状态方程，首先需要得到的连续时间微分方程：

其中F(t)为***动态矩阵，w(t)为***噪声向量，G(t)为***噪声驱动矩阵，其中w＝[w_g w_a w_bg w_ba]且有：

E(w(t)w(τ)^T)＝q(t)δ(t-τ)

0表示三零矩阵，I表示三阶单位矩阵，w_g是陀螺测量噪声，w_a是加速度计测量噪声；w_bg是陀螺零偏马尔可夫过程激励白噪声，w_ba是加速度计零偏马尔可夫过程激励白噪声；

其中：

T_g为述陀螺仪零偏马尔可夫过程的相关时间矩阵，T_a为加速度计零偏马尔可夫过程的相关时间矩阵：

为方便计算，在此给出四个辅助矩阵：

其中：

所述融合滤波器通过卡尔曼滤波来实现，所述融合滤波器中的观测模型定义如下：

Z_k＝H_kδX_k+e_k,

Z_k为观测向量，H_k为观测矩阵，量测噪声用e_k表示，其相应的量测噪声方差协方差矩阵用R表示；Z_k，H_k随着观测量的不同变化；

当处于零速区间时，零速区间的速度相当于直接对速度误差的观测，因此观测矩阵为：

H_k＝[I 0 0 0 0]

当头戴视觉摄像头检测到空间视野中存在的视觉标签，提取到人员三维绝对位置信息时，对应的观测向量表示为INS推算的位置与人员三维绝对位置之差，观测矩阵为：

l^b为IMU测量中心指向头部相机相位中心的向量在b系下的投影，表示方向余弦矩阵，能够将三维向量在n系下的投影变换到b系下的投影；

根据当前量测和状态向量，下一时刻的状态向量和协方差矩阵通过卡尔曼滤波更新公式得到：

K为增益矩阵，P_k+1∣k为k+1时刻的状态最优估计的方差协方差阵的预测值，H为观测矩阵，R_k为量测噪声方差阵，X_k+1为最优估计，X_k+1∣k为k+1时刻的状态向量预测值，P_k+1为误差协方差阵；

步骤3、经过融合滤波器的处理最终得到行人导航结果，实现室内外行人无缝导航的目的，提升导航精度，降低设备投入成本。