[发明专利]一种基于微惯导的行人室内自主定位算法

权利要求书

1.一种基于微惯导的行人室内自主定位算法，其特征在于：包括如下步骤：

a、步伐检测；

b、航迹推算求取姿态、位置、速度；

c、基于EKF框的系统误差预测及更新。

2.根据权利要求1所述的一种基于微惯导的行人室内自主定位算法，其特征在于：

所述步骤a，基于基于ZUPT及ZARU的PDR定位，该PDR定位的基础是检测I刚的落地的静止时刻，检测IMU的落地基于加速度计与陀螺仪采用三个条件判断：

1：加速度的模

2：一段时间内加速度模的方差

3：角速度的模

将B1、B2、B3进行逻辑“与”运算，B＝B1&B2&B3，当且仅当B＝1时，才判定脚落地且静止；

所述步骤b，由于由于MEMS存在零偏，因此是在输入EKF框架前，对原始加速度与角速度去除零偏：

其中为k时刻零偏修正后的载体加速度与角速度，为EKF模型中的估计值；

然后，更新方向余弦矩阵，需要说明的是该矩阵是姿态的另一种表达形式，可直接转换为姿态欧拉角；

其中是k-1时刻载体到导航系估计的DCM，是k时刻载体到导航系预测的DCM，I₃是3阶单位矩阵，δΩ_k是的反对称矩阵，Δt是IMU采样时间间隔；

这样，可以得到导航系下推算的速度与位置：

其中，v(k，k-1)，r(k，k-1)分别表示k时刻预测的速度与位置，没有被EKF修正，g表示重力加速度，即g≈9.8m/s²；

之后，根据EKF估计的误差，修正速度、位置、DCM矩阵：

其中，r(k，k)，v(k，k)，分别表示k时刻，EKF根据之前步伐静止检测后，修正的位置、速度及DCM矩阵；δr_k，δv为EKF状态估计误差；δΘ_k是关于状态估计误差δφ_k的反对称矩阵：

所述步骤c，EKF框的误差状态包含15个元素：

这里δX(k，k)状态依次表示为，δφ_k：俯仰、滚转、偏航角度误差；载体三轴角速度误差；δr_k：空间位置误差；δv_k：空间速度误差；载体三轴加速度误差，这样状态转移模型为：

δX(k，k-1)＝Φ_kδX(k-1，k-1)+Q_k-1 (1.15)

其中状态转移矩阵为：

其中，是导航系下加速度的反对称矩阵，Q_k-1为过程噪声；

状态协方差预测模型为：

测量模型为：

Z_k＝HδX(k，k)+R_k (1.20)

Z_k为测量值，H为测量矩阵，R_k为测量噪声；

这样，k时刻的测量更新可表示为：

δX(k，k)＝δX(k-1，k-1)+K_k[Z_k-HδX(k，k-1)] (1.21)

其中，K_k为卡尔曼增益，表示为：

然后，更新状态协方差，完成EKF迭代流程；

在实际使用中，采用ZUPT及ZARU进行更新，即测量值为脚放下时的速度v(k，k-1)及ω(k，k-1)，此时对应的测量值与观测矩阵为：

为了进一步提升精度，在本发明中，利用气压计测量的高度信息与航位推算计算的垂直位移间的误差，并引入磁力计输出航向与算法解算航向误差，联合ZUPT及ZARU共同作为测量值，协同约束状态，提升精度，此时对应的测量值与观测矩阵为：

其中，Δyaw＝δφ(3)-φ_Mag，Δh＝r(3)-H_barometer。

3.根据权利要求2所述的一种基于微惯导的行人室内自主定位算法，其特征在于：还包括：

初始位姿的获取：

[1]、计算初始位置及姿态，设置初始位置为零，初始姿态计算由(1.26)求得：

其中，pitch表示初始俯仰角，roll表示初始滚转角，G表示重力加速度，a_x，a_y分别表示机体x轴与y轴加速度输出值；

[2]、磁力计计算航向：首先根据[1]中计算的俯仰及滚转角，将磁力计输出数据机体坐标系：M_{B_MAG}(m_x，m_y，m_z)，经公式(1.27)转换到导航坐标系下记为M_{N_MAG}(B_x，B_y，B_z)，然后根据公式(1.28)计算航向，实现倾斜补偿：

[3]、将之前求得的俯仰、滚转、偏航角，转化为DCM矩阵，作为初始值：

其中，p代表pitch，r代表roll，φ代表φ_{mag_yaw}。