首页 / 基于UKF-FGO算法的PDRUWB室内融合定位方法

基于UKF-FGO算法的PDRUWB室内融合定位方法实质审查 发明

技术领域

[0001] 本发明涉及室内定位技术领域,尤其涉及一种基于UKF‑FGO算法的PDRUWB室内融合定位方法。

相关背景技术

[0002] 室内环境的建筑物和障碍物会限制全球导航卫星系统(Global Navigation Satellite System,GNSS)信号的穿透能力,导致信号质量下降。此外,室内环境中容易发生多径效应,即信号在建筑物内部反射、折射,造成接收器收到多个路径上的信号,从而影响定位准确性。这些因素导致GNSS无法满足人们对室内定位的需求。
[0003] 为解决该技术问题,现阶段多采用超宽带技术(Ultra Wide Band,UWB)和行人航位推算算法(Pedestrian Dead Reckoning,PDR)来实现室内定位。对于UWB技术来说,其利用基站与移动标签之间的脉冲信号的到达时间进行定位,定位精度高,但是容易受到非视距、多径效应等多种因素的影响。PDR算法主要是利用惯性测量元件,如加速度传感器、陀螺仪、磁力计等,基于步数、步长、方向等参数实现对室内人员的定位,定位短时精度高且表现出良好的稳定性,但是长距离定位存在积分误差。
[0004] 也即,现阶段所采用的室内定位算法受到各种因素的影响,使得最终确定的定位数据存在偏差,进而导致最终确定的室内定位结果不准确。

具体实施方式

[0064] 以下描述中,为了说明而不是为了限定,提出了诸如特定系统结构、技术之类的具体细节,以便透彻理解本发明实施例。然而,本领域的技术人员应当清楚,在没有这些具体细节的其它实施例中也可以实现本发明。在其它情况中,省略对众所周知的系统、装置、电路以及方法的详细说明,以免不必要的细节妨碍本发明的描述。
[0065] 为使本发明的目的、技术方案和优点更加清楚,下面将结合附图通过具体实施例来进行说明。
[0066] 图1为本发明实施例提供的基于UKF‑FGO算法的PDRUWB室内融合定位方法的实现流程图。如图1所示,该方法可以包括:
[0067] 步骤110:采集目标人员在室内的运动数据。
[0068] 在本实施例中,运动数据可以包括通过加速度传感器测得的目标人员在每个时刻对应的加速度、通过陀螺仪测得的目标人员在每个时刻对应的转向角度、姿态以及朝向、通过磁力计测得的目标人员在每个时刻对应的地磁强度、通过陀螺仪测得的目标人员在每个时刻对应的角速度、通过定位技术测得的目标人员在每个时刻的地系坐标等数据。
[0069] 步骤120:基于运动数据确定第一定位结果和第二定位结果;其中,第一定位结果基于PDR算法确定,第二定位结果基于UWB算法确定。
[0070] 在本实施例中,两种不同的算法可以分别选择PDR算法和UWB算法,作为两种不同的定位算法,以得到第一定位结果和第二定位结果。相应的,可以根据运动数据中的每个时刻对应的加速度和二维位置,通过PDR算法确定第一定位结果;根据运动数据中的目标人员在每个时刻的地系坐标,通过UWB算法确定第二定位结果;其中,第一定位结果可以包括PDR算法输出的目标人员的步长、航向角和每一时刻的位姿变量顶点;第二定位结果可以包括UWB算法输出的每个时刻目标人员对应的坐标量。
[0071] 下面分别通过一个可选的实施例,对确定第一定位结果和第二定位结果的过程进行说明:
[0072] 在一个可选的实施例中,基于运动数据确定第一定位结果,可以包括:
[0073] 根据每个时刻对应的加速度,确定目标人员的每一步所对应的步态数据;其中,步态数据包括每一步对应的起始时刻、中间时刻和每一刻对应的加速度。
[0074] 基于步态数据确定目标人员的步长。
[0075] 分别将每个时刻对应的加速度和重力加速度进行坐标系转换,得到b系坐标下每个时刻对应的的标准加速度和重力加速度。
[0076] 将标准加速度和重力加速度相乘,得到标准加速度误差。
[0077] 将标准加速度误差输入到PI滤波器中,得到加速度补偿项。
[0078] 基于加速度补偿项对每个时刻对应的加速度进行修正,得到更新后的航向角。
[0079] 基于更新后的航向角和二维位置,确定每一时刻的位姿变量顶点。
[0080] 图2为本发明实施例提供的基于UKF‑FGO算法的PDRUWB室内融合定位方法的步态检测实现流程图,下面结合图2,对本实施例中所记载的如何确定目标人员的每一步所对应的步态数据进行说明。
[0081] 在本实施例中,可以基于有限状态机的峰值检测法,将目标人员跨出一步的动作划分为有限个状态,当行人完成了所有状态,则认为此时判断为完成的一步。以目标人员的任意一步为例,本实施例中将目标人员跨出一步的动作划分为六个状态,也即从S1到S6,下面对S1到S6进行详细说明:
[0082] 状态S1:该状态认为目标人员处于静止站立状态,加速度存在波动,不断穿越零点。将零点划分为上零点与下零点,上零点即加速度信号从负值转入正值所经过的零点,即单步期间加速度信号上升趋势的起始点;下零点即加速度信号从正值转入负值所经过的零点,即单步期间加速度信号下降趋势的中间点。
[0083] 上零点与下零点的判别公式如下:
[0084] szero={+1|acck≥0&&acck‑1≤0}(1)
[0085] xzero={+1|acck≤0&&acck‑1≥0}(2)
[0086] 其中,szero为上零点的计数,xzero为下零点的计数,acck为k时刻的合加速度,acck‑1为k‑1时刻的合加速度。在此状态中时,将szero和xzero置零。
[0087] 当零点标记满足公式(3)时,判断目标人员进入S2状态。
[0088] szero‑xzero=1(3)
[0089] 状态S2:该状态下认为目标人员开始行走,此时为脚后跟开始离开地面,加速度整体处于上升趋势。根据公式(4)确认加速度是否处于上升趋势。根据公式(5)确认加速度是否大于波动阈值accT。如果满足公式(5),则判断进入状态S3。其中,公式(4)、(5)如下:
[0090] acci>acci‑1(4)
[0091] acci>accT(5)
[0092] 状态S3:该状态下认为此时目标人员脚部离开地面,且加速度acc到达最大,即满足公式(6),同时记录此时的peakacc=acci。接下来进行公式(7)的判定,若满足公式(7)则进入状态记录S4。其中,公式(6)、(7)如下:
[0093] peaki={acci|acci>acci‑1&&acci>acci+1}(6)
[0094] szero‑xzero=0(7)
[0095] 状态S4:该状态下认为此时目标人员脚部由升至最高开始下降,此时满足公式(8)。当满足公式(8)后,根据公式(9)确认加速度是否大于波动阈值accD。如果满足公式(9),则判断进入状态S5。其中,公式(8)、(9)如下:
[0096] acci
[0097] acci≤accD(9)
[0098] 状态S5:该状态下认为此时目标人员脚部接近地面,且加速度acc到达最小值,即满足公式(10),同时记录此时的valleyacc=acci。根据公式(11)进行判定,若峰谷差大于阈值σ,则进入状态S6。其中,公式(10)、(11)如下:
[0099] valleyj={accj|accj
[0100] peakacc‑valleyacc>σ  (11)
[0101] 状态S6:该状态下目标人员完成跨出一步动作,步数加一,返回S1。
[0102] 基于该状态,可以得到目标人员每一步对应的步态数据,也即每一步对应的起始时刻的加速度、中间时刻的加速度,以及其它任一时刻对应的状态及加速度。
[0103] 在确定目标人员的步态数据以后,可以根据步态数据中对应的起始时刻的加速度、中间时刻的加速度,以及其它任一时刻对应的状态及加速度,计算目标人员的步长,其中,步长计算公式可以为:
[0104]
[0105] 其中,Sk为k时刻的步长;p为权重因子,在本实施例中可以设置为0.4;Sk‑1为k‑1时刻的步长;K1、K2为常数,在试验中通过测试确定;amax、amin分别为k时刻对应的步态中的最大加速度和最小加速度;wk为k时刻对应的步态中加速度的方差,f为步频;ts和tx代表单步的起始时刻及中间时刻,即步态检测中的上零点及下零点时刻。
[0106] 图3为本发明实施例提供的基于UKF‑FGO算法的PDRUWB室内融合定位方法的航向角估计流程图,下面结合图3,对本实施例中确定航向角进行具体说明。
[0107] 在本实施例中,可以基于PI互补滤波进行航向角估计,具体的,可以定义b系(大地b坐标系中的大地维度)中加速度传感器的输出值为a,对其进行归一化处理,得到b系下的标准加速度数据:
[0108] ab=[ax ay az]T(14)
[0109] 将重力加速度由n系转换为b系:
[0110]
[0111] 将分别通过公式(14)和(15)进行转换的加速度作为标准加速度和重力加速度。
[0112] 对ab和gb做向量叉乘,可得到误差ea将其作为PI滤波器的输入,其中,误差ea的计算公式如下:
[0113]
[0114] 使用PI(比例‑积分)调节器对误差ea进行滤波处理,以消除漂移误差。其中,补偿值δ的表达式如下:
[0115] δ=(kp+ki)·ea(17)
[0116] 式中,kp为比例系数,控制着系统改正的强弱。ki用于调节误差积分,以达到修正稳态误差的作用。
[0117] 将δ与陀螺仪输出的角速度数据wg可得基于加速度的角速度改正值w如下:
[0118] w=wg+δ(18)
[0119] 将w用于更新四元数,最终求出实时更新的航向角。
[0120] 之后,根据基于更新后的航向角和二维位置,还可以确定每一时刻的位姿变量顶T点,其中,位姿变量顶点,可以表示为[xk,yk,θk]。
[0121] 在得到第一定位结果后,下面对第二定位结果的确定过程进行描述,相应的,在一个可选的实施例中,基于运动数据确定第二定位结果,可以包括:
[0122] 针对每个时刻,利用双边双向测距法,计算第k个基站与目标人员之间的电磁波飞行时间。
[0123] 基于电磁波飞行时间计算第k个基站与目标人员之间的距离。
[0124] 基于目标人员与预设数量基站之间的距离,得到非线性方程组。
[0125] 对非线性方程组进行至少两次解算,并得到至少两次解算对应的定位误差。
[0126] 当定位误差小于预设阈值时,将该至少两次解算得到的初始坐标量的平均值作为UWB算法输出的每个时刻目标人员对应的坐标量。
[0127] 在本实施例中,实际使用时,需要设置多个UWB基站,并且,每个UWB基站的位置不同,目标行人携带标签,以进行定位检测。具体的,在目标行人携带标签的前提下,利用双边双向测距法计算得到第k个基站与目标人员之间的电磁波飞行时间,基于电磁波飞行时间,确定第k个基站与目标人员之间的距离。其中,第k个基站与目标人员之间的距离可以通过以下方式进行计算:rk=tk*c,c为电磁波波速。
[0128] 预设数量的基站可以设置为4个基站,相应的,当检测到标签的基站数量不小于4时,基于目标人员与预设数量基站之间的距离,利用TDOA算法得到非线性方程组:
[0129]
[0130] 为保证定位结果的准确性,可以利用最小二乘法对公式(19)进行至少两次结算,并得到至少两次解算结果,基于两次结果计算这两次解算对应的定位误差。当定位误差小于预设阈值时,将这两次解算得到的初始坐标量的平均值作为UWB算法输出的每个时刻目标人员对应的坐标量。
[0131] 也即,当 时,将这两次解算得到的初始坐标量的平均值作为UWB算法输出的每个时刻目标人员对应的坐标量。
[0132] 其中,(x01,y01,z01)和(x02,y02,z02)分别为两次结算得到的初始坐标量,Dthr为预设阈值,本实施例中可以设置为0.3m。
[0133] 在得到第一定位结果和第二定位结果后,可以通过步骤130确定第一目标,通过步骤140确定第二目标,具体可以参考以下相应的实施例:
[0134] 步骤130:将第一定位结果输入到第一方程中,确定第一目标。
[0135] 在本实施例中,为进行室内融合定位,本实施例采用融合无迹卡尔曼滤波的因子图优化(Unscented Kalman Filter Factor Graph Optimization,UKF‑FGO)算法进行,基于此,本实施例为第一定位结果设置了PDR约束方程,也即第一方程。
[0136] 位姿变量顶点包含三个因素:二维位置和转角,即[xk,yk,θk]T。目标人员的每一步位姿变化通常由步长和行进方向的航向角两个参数来表征。通过对PDR系统采集的数据进行相应的处理和分析而获得。由于PDR数据源自传感器的直接测量,基于此数据计算出的位姿变化参数可被视为观测值。该位姿变化Lk可以写成如下形式:
[0137]
[0138] 其中,Lk为k时刻位姿变化;sk为k时刻对应的步长;Δθk为k时刻航向角的变化量。
[0139] PDR约束用于描述PDR算法中相邻位姿之间的关系,其误差项与前一时刻位姿变量、当前位姿变量以及观测到的位姿变化有关。由此PDR位姿顶点的误差项epdr定义如下:
[0140]
[0141] 其中,qk‑1为误差项与k‑1时刻位姿变量;qk为k时刻姿变量;xk为k时刻二维位置中的横向坐标;xk‑1为k‑1时刻二维位置中的横向坐标;yk为k时刻二维位置中的纵向坐标;yk‑1为k‑1时刻二维位置中的纵向坐标;θk为k时刻航向角;θk‑1为k‑1时刻航向角。
[0142] 相应的,基于PDR位姿顶点的误差项epdr,最小二乘形式的第一方程可以为:
[0143]
[0144] 其中,Ωpdr为平方误差项的权重矩阵。
[0145] 步骤140:将第二定位结果输入到第二方程中,确定第二目标。
[0146] 由于UWB系统通过双边测距(Double‑sided two‑way ranging,DS‑TWR)方法可以测量移动标签到UWB基站的距离。利用距离信息,可以构造UWB基站与移动标签之间的距离约束方程,也即第二方程。
[0147] 相应的,在k时刻,目标人员的坐标为uk,第n个基站的坐标为Bn=(xn,yn)。k时刻通过UWB传感器得到的测量值是目标人员(标签)到第n个基站的距离,记为dkn。UWB测量值形成的误差函数euwb可以定义为:
[0148] euwb(uk,Bn,dkn)=norm(Bn‑uk)‑dkn(24)
[0149] 其中,euwb为UWB测量值形成的误差函数;uk为k时刻目标人员的坐标Bn为第n个基站坐标;dkn为k时刻目标人员到第n个基站的距离;norm(·)代表向量的模;norm(Bn‑uk)表示预测值,用于预测k时刻目标人员到第n个基站的距离。
[0150] 基于误差函数euwb,第二方程可以表示为:
[0151]
[0152] 其中,Ωuwb为权重矩阵。
[0153] 步骤150:基于UKF‑FGO算法,将第一目标和第二目标融合,得到目标方程,对目标方程求解,得到目标人员在室内的融合定位结果。
[0154] 在本实施例中,由于本实施例采用的是融合无迹卡尔曼滤波的因子图优化算法进行融合定位,相应的,在一个可选的实施例中,将第一目标和第二目标融合,得到目标方程,对目标方程求解,得到目标人员在室内的融合定位结果,可以包括:
[0155] 将运动数据输入到融合无迹卡尔曼滤波的因子图优化算法中,得到UKF每个时刻输出的目标人员的位置信息。
[0156] 将UKF每个时刻输出的目标人员的位置信息输入到第三方程中,确定第三目标。
[0157] 将第一目标、第二目标和第三目标融合,得到目标方程,对目标方程求解,得到目标人员在室内的融合定位结果。
[0158] 图4是本发明实施例提供的基于UKF‑FGO算法的PDRUWB室内融合定位方法的融合UKF的因子图优化流程图,下面结合图4对本实施例进行说明。
[0159] 在本实施例中,本实施例所提供的方法包括定位信息处理以及融合定位两个过程。其中,定位信息处理可以参考步骤120,融合定位可以参考步骤130‑步骤150。步骤130和步骤140已经对第一方程(PDR约束)和第二方程(UWB约束)进行说明,下面在步骤150中,对第三方程(UKF约束)以及目标方程及其求解进行说明。
[0160] 相应的,在本实施例中,UKF每个时刻输出的目标人员的位置信息可以表示为:
[0161]
[0162] 其中, 为k时刻UKF输出的目标人员的位置信息;pk为k时刻UKF输入的运动数据; 为观测噪声。
[0163] 第三方程(UKF约束)用于描述算法中状态变量与量测值的关系,其误差项与状态参数有关。由此UKF顶点的误差项定义如下:
[0164]
[0165] 相应的,第三方程可以表示为:
[0166]
[0167] 其中,Ωukf为权重矩阵。
[0168] 在确定三个方程(约束方程)以后,将对应的数值输入到方程中,得到第一目标、第二目标和第三目标;基于到第一目标、第二目标和第三目标建立的目标方程可以表示为:
[0169] F(·)=Fpdr(·)+Fuwb(·)+Fukf(·)(29)
[0170] 对目标方程可以采用Levenberg‑Marquardt算法求解,得到系统状态最优的估计值,将得到的最优的估计值作为目标人员在室内的融合定位结果。
[0171] 综上,本发明实施例提供了一种基于UKF‑FGO算法的PDRUWB室内融合定位方法,该方法针对传统PDR算法拥有较为明确的步态周期信息,提高了步态检测和步长估计的准确性,具有抗信号噪声和干扰能力强的优点,借助融合算法进一步提高了室内定位准确性和鲁棒性。具体的,本实施例利用加速度计、陀螺仪和磁力计采集运动数据,采用PDR算法检测目标人员步态,估算步长和航向角;在非视距等因素干扰小的情况下,基于到达时间差(Time Difference of Arrival,TDOA)算法进行UWB定位;利用融合无迹卡尔曼滤波的因子图优化(UKF‑FGO)算法融合PDR与UWB定位结果,获得人员的最优定位结果。可见,相比于传统方法,本发明实施例采用两种不同的定位算法,确定两种定位结果,通过结合二者的优点,能够提高定位精度;对于二者之间的局限性,则采用融合算法,对得到的两种定位结果重新进行处理,以对定位数据进行修正,减少偏差,进而提高室内定位结果的准确性。
[0172] 下面通过一个实施例对本发明实施例所提供的基于UKF‑FGO算法的PDRUWB室内融合定位方法的有效性进行验证,请参见图5‑图7。图5为本发明实施例提供的基于UKF‑FGO算法的PDRUWB室内融合定位方法的试验场地俯视图;图6为本发明实施例提供的基于UKF‑FGO算法的PDRUWB室内融合定位方法的融合算法定位轨迹图;图7为本发明实施例提供的基于UKF‑FGO算法的PDRUWB室内融合定位方法的融合算法定位误差图。
[0173] 在图5中,选择的试验场地的走廊宽度为3.6m,长度为15m。其中四角处可以设置监测设备。目标人员由①号点按照其正常的行进速度走到②号点;从②号点按照其加快后的行进速度走到③号点;从③号点按照其减慢后的行进速度走到④号点。在这个过程中通过四角处设置的监测设备采集目标人员的运动数据。
[0174] 运动数据的处理结果可以参见图6,图6中横坐标代表走廊宽度,纵坐标表示走廊长度,所有数值取正值,以①号点为原点,针对图6中三段不同的走势曲线,第一段表示①‑②阶段,第二段表示②‑③阶段,第三段③‑④阶段。图6中,叉号、星号、三角、空心圆和方形分别代表了PDR、UWB、UKF、因子图路线轨迹和本实施例的轨迹,而黑色线段表示真实轨迹。分别对①‑②、②‑③、③‑④三个阶段进行局部放大,从前①‑②和②‑③阶段的局部放大图来看本实施例所提供的算法轨迹较其他轨迹更靠近真实轨迹,③‑④阶段的局部放大图中UKF轨迹略好于本实施例所提供的算法轨迹。
[0175] 在图7的融合算法定位误差图中,横坐标为步数,纵坐标为误差。第0‑22步为①‑②阶段,第22‑31为②‑③阶段,第31‑40步为③‑④阶段。图7中黑色方块线代表PDR定位误差,②‑③阶段由于步伐加快,设备由于震动频率增加,且在②号点转弯时误差达到最大值,③‑④阶段由于步态发生变化,步长变小,导致初始时误差较常速走略小。星形线代表UWB定位误差,由于开始阶段处于基站附近,所以误差较小,随着远离基站,误差开始增大,处于一号点与二号点的中点时误差达到最大0.2m。三角线与圆形线分别代表因子图、UKF融合定位的误差,UKF初期误差曲线优于因子图误差曲线,30步之后,因子图误差曲线优于UKF误差曲线。方框线代表本实施例的误差,相较于因子图、UKF融合定位的误差,本实施例的曲线鲁棒性更好、定位误差有所下降。
[0176] 应理解,上述实施例中各步骤的序号的大小并不意味着执行顺序的先后,各过程的执行顺序应以其功能和内在逻辑确定,而不应对本发明实施例的实施过程构成任何限定。
[0177] 以下为本发明的装置实施例,对于其中未详尽描述的细节,可以参考上述对应的方法实施例。
[0178] 图8示出了本发明实施例提供的基于UKF‑FGO算法的PDRUWB室内融合定位装置的结构示意图,为了便于说明,仅示出了与本发明实施例相关的部分,详述如下:
[0179] 如图8所示,基于UKF‑FGO算法的PDRUWB室内融合定位装置8包括:
[0180] 采集模块81,用于采集目标人员在室内的运动数据;
[0181] 定位模块82,用于基于运动数据确定第一定位结果和第二定位结果;其中,第一定位结果基于PDR算法确定,第二定位结果基于UWB算法确定;
[0182] 确定模块83,用于将第一定位结果输入到第一方程中,确定第一目标;
[0183] 确定模块83,还用于将第二定位结果输入到第二方程中,确定第二目标;
[0184] 融合模块84,用于基于UKF‑FGO算法,将第一目标和第二目标融合,得到目标方程,对目标方程求解,得到目标人员在室内的融合定位结果。
[0185] 在一种可能的实现方式中,融合模块84,具体用于:
[0186] 将运动数据输入到融合无迹卡尔曼滤波的因子图优化算法中,得到UKF每个时刻输出的目标人员的位置信息;
[0187] 将UKF每个时刻输出的目标人员的位置信息输入到第三方程中,确定第三目标;
[0188] 将第一目标、第二目标和第三目标融合,得到目标方程,对目标方程求解,得到目标人员在室内的融合定位结果。
[0189] 在一种可能的实现方式中,第三方程为:
[0190]
[0191] 其中,eukf为UKF顶点的误差项; 为k时刻UKF输出的目标人员的位置信息;pk为k时刻UKF输入的运动数据;Ωukf为权重矩阵; 为观测噪声。
[0192] 在一种可能的实现方式中,第一定位结果包括PDR算法输出的目标人员的步长、航向角和每一时刻的位姿变量顶点;
[0193] 第二定位结果包括UWB算法输出的每个时刻目标人员对应的坐标量。
[0194] 在一种可能的实现方式中,第一方程为:
[0195]
[0196] 其中,epdr为PDR位姿顶点的误差项;qk‑1为误差项与k‑1时刻位姿变量;qk为k时刻姿变量;Ωpdr为平方误差项的权重矩阵;Lk为k时刻位姿变化;xk为k时刻二维位置中的横向坐标;xk‑1为k‑1时刻二维位置中的横向坐标;yk为k时刻二维位置中的纵向坐标;yk‑1为k‑1时刻二维位置中的纵向坐标;θk为k时刻航向角;θk‑1为k‑1时刻航向角;sk为k时刻对应的步长;Δθk为k时刻航向角的变化量;
[0197] 第二方程为:
[0198]
[0199] euwb(uk,Bn,dkn)=norm(Bn‑uk)‑dkn
[0200] 其中,euwb为UWB测量值形成的误差函数;uk为k时刻目标人员的坐标Bn为第n个基站坐标;dkn为k时刻目标人员到第n个基站的距离;Ωuwb为权重矩阵;norm(·)代表向量的模;norm(Bn‑uk)表示预测值,用于预测k时刻目标人员到第n个基站的距离;
[0201] 目标方程为:
[0202] F(·)=Fpdr(·)+Fuwb(·)+Fukf(·)。
[0203] 在一种可能的实现方式中,运动数据包括每个时刻对应的加速度和二维位置;
[0204] 定位模块82,具体用于:
[0205] 根据每个时刻对应的加速度,确定目标人员的每一步所对应的步态数据;其中,步态数据包括每一步对应的起始时刻、中间时刻和每一刻对应的加速度;
[0206] 基于步态数据确定目标人员的步长;
[0207] 分别将每个时刻对应的加速度和重力加速度进行坐标系转换,得到b系坐标下每个时刻对应的的标准加速度和重力加速度;
[0208] 将标准加速度和重力加速度相乘,得到标准加速度误差;
[0209] 将标准加速度误差输入到PI滤波器中,得到加速度补偿项;
[0210] 基于加速度补偿项对每个时刻对应的加速度进行修正,得到更新后的航向角;
[0211] 基于更新后的航向角和二维位置,确定每一时刻的位姿变量顶点。
[0212] 在一种可能的实现方式中,定位模块82,具体用于:
[0213] 针对每个时刻,利用双边双向测距法,计算第k个基站与目标人员之间的电磁波飞行时间;
[0214] 基于电磁波飞行时间计算第k个基站与目标人员之间的距离;
[0215] 基于目标人员与预设数量基站之间的距离,得到非线性方程组;
[0216] 对非线性方程组进行至少两次解算,并得到至少两次解算对应的定位误差;
[0217] 当定位误差小于预设阈值时,将该至少两次解算得到的初始坐标量的平均值作为UWB算法输出的每个时刻目标人员对应的坐标量。
[0218] 图9是本发明实施例提供的电子设备的示意图。如图9所示,该实施例的电子设备9包括:处理器90、存储器91以及存储在所述存储器91中并可在所述处理器90上运行的计算机程序92。所述处理器90执行所述计算机程序92时实现上述各个基于UKF‑FGO算法的PDRUWB室内融合定位方法实施例中的步骤,例如图1所示的步骤110至步骤150。或者,所述处理器90执行所述计算机程序92时实现上述各装置实施例中各模块/单元的功能,例如图8所示各模块/单元的功能。
[0219] 示例性的,所述计算机程序92可以被分割成一个或多个模块/单元,所述一个或者多个模块/单元被存储在所述存储器91中,并由所述处理器90执行,以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段,该指令段用于描述所述计算机程序92在所述电子设备9中的执行过程。例如,所述计算机程序92可以被分割成图8所示的各模块/单元。
[0220] 所述电子设备9可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述电子设备9可包括,但不仅限于,处理器90、存储器91。本领域技术人员可以理解,图9仅仅是电子设备9的示例,并不构成对电子设备9的限定,可以包括比图示更多或更少的部件,或者组合某些部件,或者不同的部件,例如所述电子设备还可以包括输入输出设备、网络接入设备、总线等。
[0221] 所称处理器90可以是中央处理单元(Central Processing Unit,CPU),还可以是其他通用处理器、数字信号处理器(Digital Signal Processor,DSP)、专用集成电路(Application Specific Integrated Circuit,ASIC)、现场可编程门阵列(Field‑Programmable Gate Array,FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
[0222] 所述存储器91可以是所述电子设备9的内部存储单元,例如电子设备9的硬盘或内存。所述存储器91也可以是所述电子设备9的外部存储设备,例如所述电子设备9上配备的插接式硬盘,智能存储卡(Smart Media Card,SMC),安全数字(Secure Digital,SD)卡,闪存卡(Flash Card)等。进一步地,所述存储器91还可以既包括所述电子设备9的内部存储单元也包括外部存储设备。所述存储器91用于存储所述计算机程序以及所述电子设备所需的其他程序和数据。所述存储器91还可以用于暂时地存储已经输出或者将要输出的数据。
[0223] 所属领域的技术人员可以清楚地了解到,为了描述的方便和简洁,仅以上述各功能单元、模块的划分进行举例说明,实际应用中,可以根据需要而将上述功能分配由不同的功能单元、模块完成,即将所述装置的内部结构划分成不同的功能单元或模块,以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中,上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。另外,各功能单元、模块的具体名称也只是为了便于相互区分,并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程,可以参考前述方法实施例中的对应过程,在此不再赘述。
[0224] 在上述实施例中,对各个实施例的描述都各有侧重,某个实施例中没有详述或记载的部分,可以参见其它实施例的相关描述。
[0225] 本领域普通技术人员可以意识到,结合本文中所公开的实施例描述的各示例的单元及算法步骤,能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行,取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能,但是这种实现不应认为超出本发明的范围。
[0226] 在本发明所提供的实施例中,应该理解到,所揭露的装置/电子设备和方法,可以通过其它的方式实现。例如,以上所描述的装置/电子设备实施例仅仅是示意性的,例如,所述模块或单元的划分,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式,例如多个单元或组件可以结合或者可以集成到另一个系统,或一些特征可以忽略,或不执行。另一点,所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口,装置或单元的间接耦合或通讯连接,可以是电性,机械或其它的形式。
[0227] 所述作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
[0228] 另外,在本发明各个实施例中的各功能单元可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。
[0229] 所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本发明实现上述实施例方法中的全部或部分流程,也可以通过计算机程序来指令相关的硬件来完成,所述的计算机程序可存储于一计算机可读存储介质中,该计算机程序在被处理器执行时,可实现上述各个基于UKF‑FGO算法的PDRUWB室内融合定位方法实施例的步骤。其中,所述计算机程序包括计算机程序代码,所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括:能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(Read‑Only Memory,ROM)、随机存取存储器(Random Access Memory,RAM)、电载波信号、电信信号以及软件分发介质等。
[0230] 以上所述实施例仅用以说明本发明的技术方案,而非对其限制;尽管参照前述实施例对本发明进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围,均应包含在本发明的保护范围之内。

当前第1页 第1页 第2页 第3页
相关技术
室内融合相关技术
算法室内相关技术
邢卉发明人的其他相关专利技术