[发明专利]基于足部航姿参考和肩部电子罗盘的个人导航系统及方法

权利要求书

1.一种基于足部航姿参考和肩部电子罗盘的个人导航方法，其特征是，包括以下步骤：

(1)将两个低成本AHRS分别安装于行人的鞋上和肩上，其中安装于鞋上的AHRS主要负责导航解算，肩上的AHRS作为电子罗盘主要负责行人航向测量；

(2)肩上的AHRS将其测量得到的行人航向信息直接输入到鞋上的AHRS，作为其解算姿态转移矩阵时所需的航向信息；鞋上的AHRS通过自身携带的加速度计和陀螺仪以及肩上的AHRS提供的行人航向信息对行人的轨迹进行解算；

(3)将人的行走分为静止和运动两个状态，对行人的当前运动状态进行判断；

(4)一旦行人处于静止状态，将鞋上的AHRS通过将自身携带的加速度传感器和陀螺仪传感器所采集到的数据作为卡尔曼滤波器的观测量，并对足部AHRS采集到的数据进行误差补偿，最终得到当前时刻最优的导航信息预估；

(5)将足部AHRS解算的速度和姿态信息与卡尔曼滤波器输出的误差预估分别作为人工智能算法的培训输入和目标输入，对二者的映射关系进行构建；

(6)一旦行人处于运动状态，人工智能算法进入预估阶段；在这一阶段，足部AHRS中输出的速度和姿态信息作为预估输入传送到人工智能算法中，通过构建的映射关系，对卡尔曼滤波器失锁的观测向量进行预估，保证卡尔曼滤波器的正常工作，进而保证整个导航系统的无缝导航。

2.如权利要求1所述的一种基于足部航姿参考和肩部电子罗盘的个人导航方法，其特征是，所述步骤(3)中判断行人的当前运动状态的方法为：

鞋上的AHRS通过将自身携带的加速度和陀螺仪传感器所采集到的数据计算当前时刻鞋子的加速度模值，通过加速度模值的大小判断鞋子的运动状态。

3.如权利要求2所述的一种基于足部航姿参考和肩部电子罗盘的个人导航方法，其特征是，通过加速度模值的大小判断鞋子的运动状态的具体方法为：

其中，加速度模值：a_x、a_y、a_z分别为在载体坐标系下的x、y和z三个方向的加速度值。

4.如权利要求1所述的一种基于足部航姿参考和肩部电子罗盘的个人导航方法，其特征是，所述步骤(4)中一旦当前时刻鞋子处于静止状态，所采用的卡尔曼滤波器的状态方程具体为：

φk+1δVk+1nδPk+1n▿k+1nϵk+1n=I3×303×303×303×3I3×3·TS(fkn)·TI3×303×3I3×3·T03×303×3I3×3·TI3×303×303×303×303×303×3I3×303×303×303×303×303×3I3×3φkδVknδPkn▿knϵkn+ωk---(1)]]>

其中，为AHRS测量得到的导航坐标系下的东向、北向和天向三个方向的加速度误差；分别为k时刻及k+1时刻AHRS测量得到的导航坐标系下的东向、北向和天向三个方向的速度误差；φ_k、φ_k+1分别为k时刻及k+1时刻AHRS测量得到的导航坐标系下的横摇、纵摇和航向角误差；分别为k时刻及k+1时刻AHRS测量得到的导航坐标系下的东向、北向和天向三个方向的位置误差；分别为k时刻及k+1时刻导航坐标系下的东向、北向和天向三个方向的加速度误差；分别为k时刻及k+1时刻导航坐标系下的东向、北向和天向三个方向的角速度误差；ω_k为状态噪声；T为采样周期；I为单位矩阵。