[发明专利]一种在线校准步长、修正运动偏差角和自适应能量管理的行人导航方法

权利要求书

1.一种在线校准步长、修正运动偏差角和自适应能量管理的行人导航方法，具体包括以下步骤：

步骤1：采用MARG传感器获取行人位置、速度、姿态数据；

步骤2：行人开始运动后进入“捷联惯导推算”阶段，基于惯性推算方法进行行人位置姿态更新；

在每一时刻接收到MARG元件数据后，进行位置速度姿态更新，具体如下：

进行k时刻的位置速度姿态推算：

上标n表示导航坐标系n系，下标k表示第k时刻，表示从b系到n系的姿态旋转矩阵，f表示比力，I₃表示3维的单位阵，Δt表示采样时间间隔，g表示重力加速度，v表示速度矢量，ω表示角速度，加速度计与陀螺仪的测量误差将导致位置、速度、姿态误差逐渐积累，通过构建滤波器进行实时误差估计与补偿x^I：

其中上标T表示矩阵转置，表示失准角，δp表示位置误差，δv表示速度误差，δω表示角速率偏差，δa表示加速度偏差；

建立滤波状态方程：

其中，w表示过程噪声，服从高斯分布w～N(0,Q)，转移矩阵为

进行一步预测：

P表示位置矢量，建立量测更新方程：

其中H表示观测涉及矩阵，ε表示观测噪声，服从高斯分布ε～N(0,R)；

基于零速检测结果及磁力计解算的偏航角，确定量测矢量与量测设计矩阵

其中，ψ_mag,k表示借助磁力计计算的航向角，ψ_gyro,k表示借助陀螺仪计算的航向角，D表示偏航误差角与失准角间的转移关系，D＝[-sinψtanγ -cosψtanγ 1]，γ表示姿态俯仰角；

进行量测更新，得到最终状态估计量及其方差：

其中，K表示滤波器增益矩阵；

将误差量返回公式(1)中进行修正，由此完成了一个周期的位置更新；

步骤3：进行行人步态检测，以每一步为单位进行分割；

步骤4：进行行人步长在线标定；

步骤5：进行行人航向偏差角修正；

对航向偏差角一阶导数作如下假设关系：

取滤波状态矢量状态模型可写为：

状态转移矩阵为

建立观测模型：

其中，观测设计矩阵观测矢量θ_i为第i步的实际运动方向，根据每一步的运动位移方向、速度方向，其计算公式可选：

其中，Δp_x,i,Δp_y,i分别为捷联惯导解算的第i步x、y方向位移，分别为第i步x、y方向速度的均值；ψ_i为捷联惯导解算的第i步落地时的偏航角；修正方法与步骤2中修正方法完全相同；

根据步长节点划分，每一步结束进行一次修正，在初始化结束阶段获得最终方向偏差估计值δθ；

步骤6：检测总位移是否达预设长度，如否，则转至步骤2；如是，MARG传感器中陀螺仪进行休眠，转至步骤7；

步骤7：进入“步态航位推算”阶段位置推算。

2.如权利要求1所述的一种在线校准步长、修正运动偏差角和自适应能量管理的行人导航方法，其特征在于所述步骤7的具体步骤为：

步骤7-1：基于加速度数据进行步态检测与分割；

步骤7-2：对行为模式进行检测，如检测到该模式已标定，则执行7-3，否则执行步骤2；

步骤7-3：利用标定完成的步长公式计算每一步的步长；

步骤7-4：借助磁力计计算每一步的偏航角；

步骤7-5：对每一步的航向进行偏差修正，得到实际运动方向；

步骤7-6：利用每一步的步长与修正后的方向，进行位置推算。