具有非视距识别功能的室内行人定位方法与流程

本发明涉及室内定位领域，尤其涉及一种具有非视距识别功能的室内行人定位方法。

背景技术：

在室外环境中，通常采用全球定位系统(globalpositioningsystem,gps)对目标进行定位。但是由于室内环境较室外更为复杂，障碍物(如墙壁和橱柜等)的阻挡会造成卫星信号的多路径和nlos效应，进而导致定位误差较大，不能满足室内定位的要求，因此需要采用其他定位技术对室内环境下的行人目标进行定位。目前，uwb技术是一种非常有前景的室内定位技术，基于uwb技术的常用定位方法有toa、到达角度(directionofarrival,doa)、到达时间差(timedifferenceofarrival,tdoa)和接收信号强度(receivedsignalstrength,rss)以及它们的混合参数定位方法。

toa定位具有极高时间分辨率的特性，定位精度能够达到厘米级。在对动态未知目标进行定位时，由于使用单一方法进行定位，会存在各自的优缺点，所以常使用uwb与行人航位推算法(pedestriandeadreckoning,pdr)进行联合定位。toa是利用多边测量原理，即根据距离定位物体。对于具有已知信号传播速度的信号，可以通过测量从信号的发射端(称为标签)到接收端(称为基站)所经过的时间来计算基站和标签之间的距离。当标签与基站之间的直接传播路径没有任何障碍物遮挡，即处于视距情况下，其误差较小。但当直接路径受到障碍物的干扰时，即nlos情况下，信号是经过了透射、反射和散射等后才被基站接收到的，此时存在较大的时延误差，导致定位发生偏差。pdr是利用步长和方位定位，根据从室内预先已知的初始位置，利用速度、时间和方向来估计当前的位置，即通过利用惯性传感器来检测人在行走时的步长、步数以及行进方向来进行位置估计，是借助imu实现的。imu可以通过感知载体运动时的姿态信息，运用牛顿力学和运动微积分方程来推算速度信息和位置信息，因此利用imu进行定位是一种成本更低的选择。

imu通常由三轴加速度计和三轴陀螺仪组成，具有良好的抗干扰性，加之信息更新率很高，所以在短期内具有非常好的定位精度，但是使用imu进行采集数据时，会随着时间的增长产生累计误差即积分运算的递归，造成大的漂移。为获得更为准确的定位，需要对其进行定期校准。在使用imu进行定位时还必须给定它的初始位置信息，这样才能与其他定位技术联合使用(如uwb技术)。

chenp等提出一种融合uwb的基于人体运动对称特性的具有误差自校正功能的室内动态定位方法，采用无迹卡尔曼滤波器(unscentedkalmanfilter，ukf)对惯性传感器和uwb数据进行融合，uwb定位克服了惯性定位的误差累积，但该方法imu是一直介入的，即使有uwb可以对其纠正，也会导致一定的误差累积。zhangh等提出了一种利用扩展卡尔曼滤波器对imu和超声定位的数据进行融合，但其受到环境的制约比较大。

技术实现要素：

本发明所要解决的技术问题是提出了一种在室内复杂环境下具有nlos识别功能的toa和pdr相融合的室内定位算法。在定位过程中，利用提出的三角不等式定理进行los/nlos鉴别。当检测行人目标处于nlos状态时使imu介入，与toa定位相结合，在短时间内提供惯性参数和姿态信息，结合行人行走规律进行pdr计算，修正行人目标的定位信息，消除了uwb信号多路径效应和nlos的影响，减少了累积误差。

本发明采用的技术方案为：

一种具有非视距识别功能的室内行人定位方法，包括以下步骤：

步骤一：利用信号到达时间法toa对待测行人目标进行定位，得到任意时刻下待测行人的位置坐标；

步骤二：根据待测行人的位置坐标，由三角不等式定理判断待测行人的当前状态是处于视距状态还是非视距状态；若待测行人处于视距状态，则继续执行步骤一；若待测行人处于非视距状态，则执行步骤三；

步骤三：若待测行人处于非视距状态，则采用imu惯性测量仪和行人航位推算法联合定位的方法，推算待测行人在上一时刻t-1到当前时刻t的位移量δs′；

步骤四：由上一时刻t-1待测行人的位置坐标、位移量δs′和imu陀螺仪输出的方向信息，重新计算当前时刻t下待测行人的位置坐标；

步骤五：重复以上步骤，计算得到所有时刻下的待测行人的定位信息，并利用粒子滤波器对数据进行筛选，去掉异常值；

完成具有非视距识别功能的室内行人定位。

其中，步骤一具体包括以下步骤：

(101)设定任意时刻下待测行人的位置坐标为(x0,y0)，基站的位置坐标为(xi,yi)，待测行人到第i个基站的真实距离为di，1≤i≤n，n为基站总数，则有：

(102)采用以下公式计算得到待测目标位置坐标：

将方程组中所有的方程均减去第一个方程，得如下方程组：

将上述方程组改写为矩阵形式，则有如下形式：

hx＝b(5)

矩阵方程的最小方差解为：

x＝(h^th)^-1h^tb(6)

由此，计算得到任意时刻下待测行人的位置坐标。

其中，步骤二中根据待测行人的位置坐标，由三角不等式定理判断待测目标的当前状态是处于视距状态还是非视距状态，具体包括以下步骤：

(201)由步骤一得到待测行人在当前时刻t的位置坐标(xt,yt)，在t-1时刻的位置坐标为(xt-1,yt-1)，在t-2时刻的位置坐标为(xt-2,yt-2)；

(202)根据加速度测量及待测行人在t-1和t-2时刻的位置，计算待测行人从t-1时刻移动到t时刻的距离δs；

式中，vt-1是t-1时刻的速度，是t-2到t-1时刻的平均速度，st-1,t-2是t-2到t-1时刻的移动距离，at-1和at-2分别表示t-1时刻的加速度和t-2时刻的加速度，t为当前时刻；

(203)计算待测行人在t时刻与基站(xq,yq)之间的距离dt，目标在t-1时刻与基站(xq,yq)之间的距离dt-1，1≤q≤n，n为基站总数，计算公式如下：

(204)由三角不等式定理可得以下关系：

δs+dt-dt-1＞0(15)

δs+dt-1-dt＞0(16)

dt+dt-1-δs＞0(17)

δs-||dt-dt-1||＞0(18)

由于在实际中存在测量误差和估计误差，所以式(18)可改写为：

δs-||dt-dt-1||＝δs-||dt+εi+bi-dt-1-δμt-1||＞0(19)

εi+bi是t时刻的测量误差，δμt-1是前一次位置的估计误差；

(205)若式(18)或式(19)成立，则待测行人处于视距状态；否则，待测行人处于非视距状态。

其中，步骤三具体包括以下步骤：

(301)根据待测行人的运动特点，建立行人腰部运动模型；

(302)计算一个周期待测行人在行走中腰部在水平方向上的位移量sx；

δy1＝l1(1-cosδ)(20)

δy2＝l2(1-cosδ)(21)

sright＝2l1sinδ(22)

sleft＝2l2cosδ(23)

由以上式子可得：

则一个周期行走中腰部的位移量为：

sx＝sright+sleft(26)

在总的步数下腰部的位移量sd为：

式中，s0为水平方向的初始位移量，sright和sleft分别表示待测行人在支撑脚为右脚和左脚时人体行进的位移量，将行人垂直站立时惯性测量单元imu与脚底的距离设为单摆摆长，其中l1和l2分别表示右脚和左脚单脚支撑时行进过程中倒置单摆的摆长；δy1和δy2分别表示支撑脚为右脚和左脚时垂直方向上的最大和最小位移量；δ表示行进过程中腿部向前迈出的最大角度，即在单腿和双腿支撑之间划分临界点地时刻，支撑腿与垂直地面之间的夹角角度；

(303)计算一个周期待测行人在行走中腰部在垂直方向上的位移量sy；

待测行人在每次行进步态中，设定imu输出的三轴加速度分别为ax_i，ay_i，az_i，三轴角速度分别为ωx_i，ωy_i，ωz_i，其中i＝1,2,…,n，n为总的步数；行进过程中支撑腿在垂直方向上的夹角为θi，则有：

其中，θ0是θi的初值，δt为imu的采样时间间隔，利用θi将其转换到水平和垂直方向上，则有：

其中，axi和ayi分别为行进过程中在水平和垂直方向上的加速度值，g为重力加速度；

设定待测行人最开始是静止的，即θ0＝0，对加速度进行积分得到垂直方向上的速度：

利用“零速修正zupt”修正原理，得vy0为零，通过对imu垂直方向上的速度进行进一步积分运算，得到其在垂直方向上的位移量：

其中，sy0为zupt时刻在垂直方向上的初始位移，将其设置为零；求每一步中以zupt时刻为基准，实时位移syi的最大最小值，即可求得δy1、δy2。以此，通过式(27)得到一段时间内行人的总位移。

本发明相比现有技术的优点为：

本发明能够消除uwb信号多路径效应和nlos影响，减少累积误差。当行人在处于非视距的情况下，对比其它算法本发明的算法能够有效的提高定位精度。

附图说明

图1为本发明行人腰部运动模型。

具体实施方式

下面结合附图对本发明作进一步解释说明。

步骤一：先利用信号到达时间法(timeofarrival,toa)对待测行人目标进行定位，可得到某时刻下的行人位置坐标；

(101)假设某时刻下待测行人的位置坐标为(x0,y0)，其到第i个基站(xi,yi)的测量距离为d′i，其到第i个基站的真实距离为di，1≤i≤n，n为基站总数，则有：

(102)测量距离d′i和真实距离di的差值即为测距噪声ρi。为保证待测行人的定位结果更加准确，则要求测距噪声最小化，采用以下方法计算得到待测行人位置坐标：

将方程组中所有的方程均减去第一个方程，可得如下方程组：

将上述方程组改写为矩阵形式，则有如下形式：

hx＝b(5)

矩阵方程的最小方差解为：

x＝(h^th)^-1h^tb(6)

由此，可计算得到任意时刻下待测目标的位置坐标。

步骤二：由三角不等式定理判断待测行人的当前状态是处于视距状态还是非视距状态。若待测行人处于视距状态，则继续进行步骤一；若待测行人处于非视距状态，则进行步骤三；

(201)由toa法计算得到待测行人在t时刻的位置坐标(xt,yt)，在t-1时刻的位置坐标为(xt-1,yt-1)，在t-2时刻的位置坐标为(xt-2,yt-2)；

(202)根据加速度测量及待测行人在t-1和t-2时刻的位置，可计算待测行人从t-1时刻移动到t时刻的距离δs；

vt-1是t-1时刻的速度，是t-2到t-1时刻的平均速度，st-1,t-2是t-2到t-1时刻的移动距离，at-1和at-2分别表示t-1时刻的加速度、t-2时刻的加速度；

(203)待测行人在t时刻与基站(xq,yq)之间的距离为dt，待测行人在t-1时刻与基站(xq,yq)之间的距离为dt-1，计算公式如下：

(204)由三角不等式定理可得以下关系：

δs+dt-dt-1＞0(15)

δs+dt-1-dt＞0(16)

dt+dt-1-δs＞0(17)

δs-||dt-dt-1||＞0(18)

由于在实际中存在测量误差和估计误差，所以式(18)可改写为：

δs-||dt-dt-1||＝δs-||dt+εi+bi-dt-1-δμt-1||＞0(19)

εi+bi是t时刻的测量误差，δμt-1是前一次位置的估计误差；

(205)若式(18)或式(19)成立，则待测行人处于视距状态。反之，待测行人处于非视距状态。

步骤三：若待测行人标处于非视距状态，则利用toa技术定位得到的位置数据误差较大，需要采用惯性测量仪(inertialmeasurementunit，imu)和行人航位推算法(pedestriandeadreckoning，pdr)联合定位的方法，推算行人目标在t-1时刻在t时刻的位移量δs′；

(301)针对行人运动特点，建立行人腰部运动模型，如图1所示。黑色实线是行人在行走过程中腰部的运动轨迹。sright和sleft分别表示行人在支撑脚为右脚和左脚时人体行进的位移量。当左脚和右脚交替地作为支撑脚时，周期性地不断重复该过程以改变行人位置。

将行人垂直站立时imu与脚底的距离设为单摆摆长，其中l1和l2分别表示右脚和左脚单脚支撑时行进过程中倒置单摆的摆长；δy1和δy2分别表示支撑脚为右脚和左脚时垂直方向上的最大和最小位移量；δ表示行进过程中腿部向前迈出的最大角度，即在单腿和双腿支撑之间划分临界点地时刻，支撑腿与垂直地面之间的夹角角度。

(302)计算一个周期待测行人在行走中腰部在水平方向上的位移量sx；

δy1＝l1(1-cosδ)(20)

δy2＝l2(1-cosδ)(21)

sright＝2l1sinδ(22)

sleft＝2l2cosδ(23)

由以上式子可得：

则一个周期行走中腰部的位移量为：

sx＝sright+sleft(26)

在总的步数下腰部的位移量为：

(303)计算一个周期待测行人在行走中腰部在垂直方向上的位移量sy；

在每次行进步态中，设imu输出的三轴加速度分别为ax_i，ay_i，az_i，三轴角速度分别为ωx_i，ωy_i，ωz_i，其中i＝1,2,…,n。忽略行人在行进过程中左右摇摆的运动特性，假设行进过程中支撑腿在垂直方向上的夹角为θi，所以有：

其中，θ0是θi的初值，δt为imu的采样时间间隔。因为imu所采集的加速度值是在机体坐标系下的观测值，可利用θi将其转换到水平和垂直方向上，则有：

其中，axi和ayi为行进过程中在水平和垂直方向上的加速度值，g为重力加速度。假设行人最开始是静止的，即θ0＝0，对加速度进行积分可以得到垂直方向上的速度：

此时利用“零速修正(zupt)”修正原理，即为了提高速度和位移的计算精度，将导航计算速度重置为零，或者将非零速度作为反推参数来修正之前的导航计算过程，可得vy0为零。通过对imu垂直方向上的速度进行进一步积分运算，可以得到其在垂直方向上的位移量：

其中，sy0为zupt时刻在垂直方向上的初始位移，可将其设置为零；求每一步中以zupt时刻为基准，实时位移syi的最大最小值，即可求得δy1、δy2。以此，通过式(27)得到一段时间内行人的总位移。

步骤四：由t-1时刻目标的定位坐标、位移量δs′和imu陀螺仪输出的方向信息，重新计算t时刻下，待测行人目标的位置坐标；

步骤五：重复以上步骤，计算得到所有时刻下的待测行人目标定位信息后，利用粒子滤波器对数据进行筛选，去掉个别异常值。