[发明专利]一种基于惯性传感器和雷达的行人航位推算方法和系统

权利要求书

1.一种基于惯性传感器和雷达的行人航位推算方法，其特征在于，该方法包括以下步骤：

S1.惯性传感器按照设定的采集频率持续地收集行人行走过程中的加速度和角速度，所述惯性传感器装备在行人身上，雷达按照设定的采集频率持续地收集行人行走过程中行人到墙壁距离，所述雷达装备在行人身上，测距方向与行人前进方向的夹角处于区间[45°,135°]，在行走过程中保持不变；

S2.根据采集到的数据进行步态分割，得到各步伐对应的时段，并按照划分结果得到各步伐对应的加速度序列、角速度序列和墙壁距离序列；

S3.对于每个步伐，进行以下处理：

A.基于当前步伐对应的加速度序列进行步长估计，得到当前步伐的步长，基于当前步伐对应的角速度序列进行航向估计，得到当前步伐的航向，其与上一步伐的航向的差值作为当前步伐的航向增量；

B.对当前步伐的步长、航向增量和墙壁距离以及上一步伐的行人姿态进行多源信息融合，估计出当前步伐的行人姿态；具体包括：

进行航位推算采用如下递推公式进行行人姿态估计和墙壁参数初始化；

其中，s_n表示当前步伐的行人姿态，它包括航向h_n和二维平面坐标(x_n,y_n)，l_n,θ_n分别表示当前步伐区间的步长和航向增量；递推结果s_n确定为本区间最终的位置估计结果；

对同一步伐区间，利用前后行人姿态及本区间内的距离观测计算观测对应的墙壁点，墙壁点计算式表示如下：

其中，表示第n个步伐区间里的第m个墙壁点，(x_n,m,y_n,m)为对应的观测点，即距离观测d_n,m产生时行人所在的位置，假设M个观测点在行人的相邻两个位置间均匀分布，再利用本区间内的所有墙壁点通过最小二乘法(y＝kx+b)对墙壁参数进行拟合，得到墙壁参数的初始化值(k_i,b_i)；

所述多源信息融合：基于当前步伐的墙壁距离和上一步伐的行人姿态对墙壁参数进行更新，将当前步伐的步长、航向增量、墙壁距离、更新后的墙壁参数和上一步伐的行人姿态输入至粒子滤波器，估计出当前步伐的行人姿态，其中，上一步伐的行人姿态作为状态，当前步伐的步长和航向增量作为控制量，当前步伐的墙壁距离作为观测，更新后的墙壁参数作为观测模型参数。

2.如权利要求1所述的方法，其特征在于，在多源信息融合之前，采用以下方式进行墙壁距离异常值修正，得到修正后的墙壁距离：

(1)对墙壁距离序列进行卡尔曼滤波，得到滤波后墙壁距离序列；

(2)对滤波后墙壁距离序列的每个墙壁距离，当距离变化率估计值大于某一预先设定的阈值T_v时，判定当前墙壁距离为异常观测值，进入步骤(3)，否则，判定为正常观测值，进入步骤(4)；

(3)采用以下修正式进行修正：

其中，表示时刻n对应的距离修正值，表示时刻n对应的距离变化趋势预测，γ是平滑参数，0γ1；

(4)直接使用标准卡尔曼滤波结果作为距离修正结果。

3.如权利要求2所述的方法，其特征在于，基于当前步伐的墙壁距离和上一步伐的行人姿态，采用扩展卡尔曼滤波算法对墙壁参数进行更新，其中，上一步伐的墙壁参数作为状态，当前步伐的墙壁距离作为观测，上一步伐的行人姿态作为观测模型参数。

4.如权利要求1至3任一项所述的方法，其特征在于，步骤A之后，通过以下方式估计出当前步伐的行人姿态：

(1)当前步伐的航向增量大于设定阈值时，标记为转角步伐，使用转角步伐划分行人轨迹，将划分后的每段行动轨迹的前N_i个步伐标记为初始化步伐，将每段行动轨迹的剩余步伐标记为融合步伐；

(2)对于转角步伐，利用转角步伐对应的步长和航向增量，通过递推式得到当前行人姿态；对于初始化步伐，利用当前步伐对应的步长和航向增量，通过递推公式得到当前行人姿态，此外，基于当前行人姿态、前一行人姿态及当前步伐的墙壁距离序列进行墙壁参数初始值估计。