[发明专利]一种针对摆臂行人的行人航位推算方法

权利要求书

1.一种针对摆臂行人的行人航位推算方法，其特征在于，包括以下步骤：

S1：利用惯性传感器采集行人的俯仰角序列、方位角序列、时间序列、惯性测量单元频率和定位周期，并计算步伐完成俯仰角和步伐完成手机航向；

S2：根据步伐完成俯仰角和步伐完成手机航向，计算步伐完成航向；

S3：利用惯性传感器采集行人的测量步长、俯仰角幅度和加速度振幅数据，并计算步伐完成步长；

S4：利用PDR算法，根据步伐完成航向和步伐完成步长进行航位推算；

所述步骤S2包括以下子步骤：

S21：根据步伐完成俯仰角计算第一偏移量Δψ₁和第二偏移量Δψ₂，并初始化第一偏移量Δψ₁和第二偏移量Δψ₂；

S22：根据初始化后的第一偏移量Δψ₁和第二偏移量Δψ₂，计算步伐完成时人的航向γ_i；

S23：将步伐完成时人的航向γ_i保存在步伐完成航向中；

所述步骤S21中，当步伐完成俯仰角为完成步行时最大俯仰角时，计算第一偏移量Δψ₁，其计算公式为：

Δψ₁＝ψ₁-ψ_d

其中，ψ₁表示第一次达到最大俯仰角时的手机航向，ψ_d表示初始行进方向的手机航向；

当步伐完成俯仰角中为完成步行时最小俯仰角时，计算第二偏移量Δψ₂，其计算公式为：

Δψ₂＝ψ₂-ψ_d

其中，ψ₂表示第一次达到最小俯仰角时的手机航向；

所述步骤S22中，若俯仰角序列中第i个俯仰角数据θ_i是俯仰角序列中的最小值，则步伐完成时人的航向γ_i的计算公式为：

γ_i＝ψ_i-Δψ₂

其中，ψ_i表示方位角序列中第i个方位角数据，Δψ₂表示第二偏移量；

否则，步伐完成时人的航向γ_i的计算公式为：

γ_i＝ψ_i-Δψ₁

其中，Δψ₁表示第一偏移量；

所述步骤S3包括以下子步骤：

S31：利用惯性传感器采集行人的测量步长、俯仰角幅度AMP_θ和加速度振幅AMP_a，并进行步长拟合；

S32：对步长拟合的结果进行加权处理，得到步长的初步目标函数；

S33：根据步长的初步目标函数，构建最优目标函数；

S34：根据最优目标函数，构建步长目标函数，并根据步长目标函数计算步伐完成步长；

所述步骤S31中，进行步长拟合的计算公式为：

其中，SL1表示通过俯仰角振幅拟合得到的步长函数，SL2表示通过加速度振幅拟合得到的步长函数，N表示拟合阶数，k表示泰勒公式正常表达值，AMP_θ表示俯仰角振幅，AMP_a表示加速度振幅，a_k表示拟合得到的第一系数，b_k表示拟合得到的第二系数；

所述步骤S32中，步长的初步目标函数SL的表达式为：

SL＝A·SL1+B·SL2

其中，SL满足E(SL)＝A·E(SL1)+B·E(SL2)＝(A+B)·l＝l，A表示第一参数，B表示第二参数，l表示真实步长，E(SL)表示步长的初步目标函数SL的平均值，E(SL1)表示通过俯仰角振幅拟合得到的步长函数SL1的平均值，E(SL2)表示通过加速度振幅拟合得到的步长函数SL2的平均值；

所述步骤S33中，最优目标函数minD(SL)的表达式为：

其中，表示第一方差，表示第二方差；

所述步骤S34中，步长目标函数SL′的计算公式为：

2.根据权利要求1所述的针对摆臂行人的行人航位推算方法，其特征在于，所述步骤S1包括以下子步骤：

S11：利用惯性传感器采集行人的俯仰角序列方位角序列时间序列惯性测量单元频率f₁和定位周期T；

S12：从俯仰角序列中获取个俯仰角数据，从时间序列中获取个时间数据；

S13：设置索引阈值Δi、角度阈值Δθ和时间差Δt，并进行初始化；

S14：在俯仰角序列和方位角序列中，标记θ_i取俯仰角序列中最大值或最小值时的索引，并存入第一索引序列中，标记ψ_i取方位角序列中最大值或最小值时的索引，并存入第二索引序列中，其中，θ_i表示俯仰角序列中第i个俯仰角数据，ψ_i表示方位角序列中第i个方位角数据；

S15：根据第一索引序列和第二索引序列分别在俯仰角序列和时间序列中确定步伐完成俯仰角与俯仰角和方位角取得最值的时间并去除步伐完成俯仰角中最大俯仰角和最小俯仰角之差不超过初始化后俯仰角阈值Δθ的俯仰角数据，去除俯仰角和方位角取得最值的时间中步距时间间隔不超过初始化后时间差Δt的时间数据；

S16：选择第二索引序列中满足i-Δiji+Δi的索引，并存入第三索引序列中，若第i个俯仰角数据θ_i是最大值，则标记第三索引序列中第n个方位角数据ψ_n处于最大值的索引，否则，标记第三索引序列中第n个方位角数据ψ_n处于最小值的索引；

S17：基于步骤S16中标记的索引，根据方位角序列和第三索引序列确定步伐完成手机航向