[发明专利]一种基于双向滤波平滑技术的准实时步进式行人导航方法

权利要求书

1.一种基于双向滤波平滑技术的准实时步进式行人导航方法，其特征在于，具体步骤如下：

步骤1，采集人体行走数据，人体行走数据包括人体行走时的三维加速度数据和三维角速度数据；人体行走数据通过固联于人体足部的惯性传感器来采集，再通过便携式导航上位机来存储和处理惯性传感器采集的人体行走数据；所述的惯性传感器为微惯性传感器，包括一个三轴加速度计和一个三轴陀螺仪；

步骤2，在第一前向算法通道中，先对采集到的人体行走数据进行误差校正，再对校正后的人体行走数据进行捷联惯导解算，获得人体行走时初步的姿态信息和速度信息；同时进行步态时相的检测和步进区间的划分，当检测到人体足部处于步态周期的支撑相时，在误差域进行姿态误差和速度误差的开环卡尔曼滤波估计；当划分出一个新的步进区间时，第一前向算法通道暂停，运行后向算法通道和第二前向算法通道；

步骤3，在后向算法通道中，对第一前向算法通道中估计出的姿态误差和速度误差进行平滑处理，得到经过平滑处理后的误差估计；

步骤4，在第二前向算法通道中，利用后向算法通道中平滑处理后的误差估计对第一前向算法通道中解算出的姿态信息和速度信息进行误差校正，再对误差校正后的速度信息进行积分，解算出人体行走时的位置信息，并返回步骤2继续运行第一前向算法通道；

所述的步骤2，具体过程如下：

利用公式(1)进行人体行走数据的误差校正，得到误差校正后的人体行走数据：

其中，k表示离散采样点，b表示载体坐标系，表示陀螺仪测量的原始角速度，ε^b表示陀螺仪零偏误差，K_G表示陀螺仪刻度系数误差，表示经过误差校正后的有效角速度；表示加速度计测量的原始加速度，▽^b表示加速度计零偏误差，K_A表示加速度计刻度系数误差，表示经过误差校正后的有效加速度；

利用误差校正后的人体行走数据进行姿态和速度的捷联惯导解算，得到人体行走时初步的姿态信息和速度信息，见公式(2)：

其中，n表示导航坐标系，Δt表示采样时间间隔，I表示适当维数的单位矩阵，gⁿ表示重力加速度；表示b系与n系之间的旋转变换矩阵，称为载体的姿态矩阵，Ω_k表示由构成的反对称矩阵，vⁿ表示速度；

在第一前向算法通道中，所述的步态时相的检测和步进区间的划分，包括：

通过零速检测算法对误差校正后的人体行走数据进行计算，获得用于实现零速检测的检验统计量，将检验统计量与阈值参数进行比较，实现步态时相的检测；阈值参数包含检测阈值和修正阈值，其中，检测阈值提前预设，用于步态时相的粗划分，修正阈值在通过聚类分析方法对粗划分结果进行分类的过程中自适应获得，修正阈值包括幅值阈值和时间阈值，用于步态时相的精划分，先进行摆动相的精划分再进行支撑相的精划分，当支撑相的持续时间满足时间阈值时划分出一个新的步进区间；

当检测到人体足部处于步态周期的支撑相时，在误差域进行姿态误差和速度误差的开环卡尔曼滤波估计，步骤如下：

利用公式(3)得到卡尔曼滤波器的状态模型：

其中，b表示载体坐标系，n表示导航坐标系，表示姿态误差，表示姿态误差导数，表示b系与n系之间的旋转变换矩阵，称为载体的姿态矩阵,表示陀螺仪测量噪声，δvⁿ表示速度误差，表示速度误差导数，表示经过误差校正后的有效加速度，表示加速度计测量噪声；

利用得到的卡尔曼滤波器的状态模型，在误差域进行间接滤波，将卡尔曼滤波器的状态向量降维为六维，并建立离散化的卡尔曼滤波状态方程，见公式(4)：

δx_k＝F_kδx_k-1+G_kw_k-1 (4)

其中，k表示离散采样点，δx表示被估计的状态向量，w表示过程噪声，F_k表示状态转移矩阵，G_k表示噪声传递矩阵，状态转移矩阵F_k中的矩阵S_k表示由n系中的向量构成的反对称矩阵，T表示转置矩阵；

当检测到人体足部处于步态周期的支撑相时，人体行走的速度为零，将捷联惯导算法解算出的速度信息作为速度误差的伪观测值提供给卡尔曼滤波器，对姿态误差和速度误差进行估计，建立离散化的卡尔曼滤波观测方程，见公式(5)：

z_k＝Hδx_k+η_k (5)

其中，z_k表示观测量，η_k表示观测噪声，H表示观测矩阵，H＝[0 I 0]；

惯性传感器每采集一次人体行走数据，都要利用捷联惯导算法对采集的人体行走数据进行解算，并执行卡尔曼滤波时间更新；只有当人体足部处于步态周期的支撑相时，才能执行卡尔曼滤波测量更新；

所述的步骤3，具体过程如下：

利用固定区间平滑算法对姿态误差和速度误差进行最优线性平滑处理，见公式(6)：

其中，k表示离散采样点，k∈(FI_end,FI_start]，FI_start表示步进区间的开始时刻，FI_end表示步进区间的结束时刻，表示状态δx_k+1的先验估计，δx_k+1表示点k+1时的状态向量，表示状态δx_k的后验估计，表示状态δx_k的平滑估计，表示状态δx_k+1的平滑估计，P_k+1|k表示点k+1的先验估计误差协方差矩阵，P_k表示点k的后验估计误差协方差矩阵，表示点k的平滑估计误差协方差矩阵，表示点k+1的平滑估计误差协方差矩阵，A_k表示固定区间平滑增益矩阵，F_k+1表示点k+1的状态转移矩阵，T表示转置矩阵；

固定区间平滑算法得到的为平滑处理后的误差估计，包含姿态误差的和速度误差的平滑估计，见公式(7)：

其中，为姿态误差的平滑估计，为速度误差的平滑估计；

所述的步骤4，具体过程如下：

利用公式(8)进行姿态误差和速度误差校正，得到人体行走时最终的姿态信息和速度信息：

其中，k表示离散采样点，b表示载体坐标系，n表示导航坐标系，I表示适当维数的单位矩阵，表示误差校正后的姿态矩阵，表示捷联惯导算法解算出的初步姿态矩阵，Θ_k表示由姿态误差构成的反对称矩阵，其中表示固定区间平滑算法得到姿态误差的平滑估计；表示误差校正后的速度，表示捷联惯导算法解算出的初步速度，表示固定区间平滑算法得到速度误差的平滑估计；

将经过误差校正后的速度对时间进行积分，得到人体行走时最终的位置信息，见公式(9)：

其中，Δt表示采样时间间隔，表示位置。