光源编码+PDR组合的室内行人定位方法

闫 伟，牛小骥，旷 俭

(武汉大学卫星导航定位技术研究中心，湖北 武汉 430079)

在室外环境中，全球卫星导航系统(global navigation satellite system，GNSS)能够提供分米级甚至厘米级精度的定位；但在室内环境中，GNSS及相关技术无法提供高精度的定位服务[1]。针对室内高精度定位的需求，国内外研究机构主要利用视觉、蓝牙天线阵列、超宽带、音频、光源[1-2]等方法实现定位。其中，随着基础照明设施的普及，光源定位技术在室内迅速发展[3]。

目前，光源定位技术主要包括基于LED灯信号特征提取的定位技术[4]、基于光强模型的定位技术[5]、基于红外光视觉捕捉的定位技术[6]、基于特殊光源的定位技术[7]等。光源编码定位是一种基于特殊光源的定位技术：光源基站进行编码设计，使地面不同位置具有不同的光信号；接收端对光信号进行解码后，准确计算自身位置，其定位精度可以达到分米级甚至厘米级[2-3]。但单独利用光源编码定位可能会遇到以下问题：①在非视距(non line of sight，NLOS)环境中，接收端被障碍物或行人部分遮挡，接收到的光强降低，噪声增大，影响位置精度；②接收端将反射光信号误判为直射光信号，造成定位粗差；③接收端因某些原因，若被完全遮挡，导致定位失败。这些问题影响了光源编码定位在室内场景中的发展。

近年来，微机电系统(micro-electro-mechanical system，MEMS)快速发展，智能手机都带有MEMS传感器[8]。基于智能手机的行人航迹推算(pedestrian dead reckoning，PDR)技术利用MEMS惯性测量单元(inertial measurement unit，IMU)，独立、连续地推算行人位置，短期精度高。但MEMS传感器误差较大，且导航算法中存在积分运算，故PDR的位置误差会随时间迅速累积[9]。

针对上述问题，本文基于智能手机平台，运用扩展卡尔曼滤波(extended Kalman filter，EKF)将光源编码位置与PDR位置相融合。光源编码+PDR组合既能利用光源编码定位结果实时修正IMU误差参数，控制PDR结果的发散；又能在光源编码定位无效时，利用PDR的短期结果提供较高精度的导航信息，从而保证定位结果的连续性和准确性。

光源编码+PDR组合算法的完整框图如图1所示。

1 光源编码定位系统

光源编码定位系统由光源基站、接收端与相应软件组成。系统的编码体现在外壳的编码设计：将半球形外壳沿经纬度方向均匀划分成多块，每一块分别进行开孔、闭孔的设计。开孔表示编码位1，即某部分外壳被去除，可使光线通过；闭孔表示编码位0，即某部分外壳未被去除，阻止光线通过。外壳沿纬度方向进行不同的开孔、闭孔顺序设计。

图2为光源编码定位原理示意图。光源基站悬挂在天花板上，工作时内部电机带动编码外壳旋转，LED光源通过开孔形成光束，使地面产生明暗变化。接收端记录光强的时间序列测量值，根据光源基站坐标、外壳编码参数和光信号解码信息，判断自身的粗略位置；之后通过小波变化降噪、优化的编码策略等，精确测量自身位置，达到厘米级的定位精度。具体的光源编码基站设计和定位算法参考文献[3]。

2 基于捷联惯导的PDR算法

基于捷联惯导的PDR算法框架如图1所示。利用IMU数据进行惯性导航系统(inertial navigation system，INS)机械编排，得到用户实时的导航信息(三维位置、速度和姿态)。为了减缓捷联惯导的误差累计速度，算法实时检测行人运动状态：当检测到行人向前迈出一个脚步时，利用行进的距离构造速度观测值进行EKF速度更新[9]。

2.1 INS机械编排

已知物体上一时刻的导航信息，利用INS机械编排，对IMU数据进行积分，可以得到当前时刻的导航信息。由于智能手机IMU的传感器误差较大，积分后位置误差会迅速增大，因此：①进行机械编排时可以忽略影响极小的误差项(如地球自转引入的误差)而不影响算法精度[9]；②需要引入其他观测量，利用EKF进行数据融合以提高精度。

导航坐标系(n系)下INS机械编排方程的离散化形式[10]如下

(1)

2.2 EKF模型

EKF状态方程、量测方程的离散化形式见式(2)，状态向量δxk、状态转移矩阵Φk/k-1见式(3)、式(4)[11]。

(2)

(3)

(4)

式中，δpn、δvn、φ分别为位置、速度、姿态的误差。

根据EKF状态方程和量测方程，利用式(5)进行状态更新和量测更新，得到行人的导航信息[1]。

(5)

2.3 行走速度更新

本文针对行人运动特征，进行如下合理假设：多数时候行人面朝的方向就是行走的方向[12]；短时间内行人的速度是一个常值[13]。即在一步周期内，只存在恒定的前向速度，侧向和垂向的速度几乎为0。基于该假设计算行走速度，行人坐标系(l系)下的速度观测值[13]如下

(6)

式中，vl为l系下的速度真值；nvl为观测噪声；SL为步长；Δt为走一步的时间。

n系下速度真值vn与vl存在以下关系

(7)

(8)

(9)

利用EKF融合行走速度，将l系速度预测值与速度观测值之差作为量测信息，量测方程如下

(10)

3 光源编码+PDR组合算法

3.1 光源编码位置更新

光源编码定位系统可以提供二维位置信息，将PDR位置与光源编码位置的差值作为量测信息，得到量测方程如式(11)—式(13)。

δzk,P-l=Hk,P-lδxk+vk

(11)

(12)

(13)

3.2 抗差处理

在实际应用中，光源编码定位偶尔会出现远超限定误差的情况。光源编码+PDR组合算法利用PDR推算结果短期精度高的特点，使用PDR位置预测结果探测光源编码定位结果中的粗差，进行剔除或降权处理以起到抗差的目的[14]。

(14)

(15)

(16)

4 试验与结果分析

4.1 算法参数与试验设计

为了验证光源编码+PDR组合算法的有效性，本文进行了4组试验。测试手机为华为P10 Plus，该手机IMU芯片型号为InvenSense公司的ICM-20690。光源编码定位系统由中国科学院深圳先进技术研究院提供[3]。设置光源编码定位频率为1 Hz，即每1 s从光源编码定位系统获取一次位置与标志位(1表示位置数据有效，0表示无效)；设置IMU采样率为50 Hz。本文利用光源编码+PDR组合反向平滑[10]的位置结果作为轨迹的参考真值。

试验场地为武汉大学诗琳通地球空间信息技术协同创新中心，光源编码基站布置在场地侧上方，可以完整覆盖试验场地，图3为基站实物图。试验路径为5.4 m×5.4 m的矩形，基站、参考轨迹与行走方向如图4所示，(0,0)为试验起点。4组试验分别沿预定轨迹行走5圈、3圈、3圈、3圈，测试人员端平手机向前行走。试验轨迹包含了正对、侧对、背对光源基站的轨迹，可以有效考察LOS与NLOS环境中光源编码+PDR组合算法的有效性。

4.2 光源编码定位结果分析

4组试验光源编码定位结果的标志位如图5所示。每组试验都存在标志位为0的数据，即在试验过程中，由于行人本身的遮挡或其他原因，光源编码定位系统会出现定位失败的情况，特别是第3组试验，有超过20个点定位失败。

图6给出了标志位为1的光源编码定位结果。第2、3、4组试验在背对光源基站的区域，都出现了定位误差大于2 m的粗差点。但对于正常定位点，光源编码定位的精度在1 dm以内，与参考轨迹基本完全符合。

表1统计了4组试验的光源编码定位结果。3组试验的正常定位点占80%以上，但存在少数定位误差大的点；第3组试验有超过1/3的点标志位为0，正常定位结果仅占60%。

表1 光源编码定位结果统计

经过以上分析，在实际应用中，单独依靠光源编码定位系统进行室内行人定位，将无法获得稳定可靠的定位结果。

4.3 光源编码+PDR组合定位结果分析

图7为光源编码+PDR组合的定位结果，组合结果与参考轨迹基本完全符合，可以反映出行人真实的导航信息。在光源编码定位结果良好时，光源编码+PDR组合的定位精度与光源编码定位精度相近，且定位结果连续稳定；当光源编码定位出现粗差点时，如第2、3、4组试验，光源编码+PDR组合的定位结果仍然良好，未出现异常结果；当光源编码正常定位点明显减少、定位结果不连续性时，如图6中参考轨迹西南角的光源编码定位点比较稀疏，光源编码+PDR组合可以提供连续的导航信息，保证系统的可靠性。

表2给出了光源编码+PDR组合定位的误差统计。光源编码+PDR组合定位的水平误差均方根值在1 dm以内，均值在8 cm以内，最大值在3 dm左右。EKF在没有外部信息辅助的情况下，解算结果必然是逐渐发散的；结合图5(c)，即便光源编码定位系统在5 s内没有提供可用的位置结果，光源编码+PDR组合算法也仅仅产生了3 dm左右的误差，这也进一步体现了本文算法的有效性。

表2 光源编码+PDR组合定位误差统计 cm

经过以上分析，在理想情况下，光源编码+PDR组合和光源编码定位都能达到分米级乃至厘米级的定位精度，定位结果连续可靠，能反映行人真实的位置情况；当光源编码定位系统定位不连续或定位性能下降时，单独依靠光源编码定位不能很好地满足实际需要，但光源编码+PDR组合仍能保持1 dm的定位精度，提供连续的导航信息。整体而言，光源编码+PDR组合更适合高精度的室内行人定位。

5 结 语

本文针对光源编码定位不连续、易受遮挡导致定位性能下降等问题，以基于捷联惯导的PDR算法为基础，实现了基于EKF的光源编码+PDR组合算法，并运用新息滤波进行抗差处理。试验结果表明，光源编码+PDR组合在室内环境中能实现高精度的连续定位，具有更高的稳定性和可靠性，适用于高精度室内行人定位。