面向NLOS建筑工地的施工人员定位与跟踪算法的制作方法

本发明涉及建筑工地施工安全领域，具体涉及一种面向nlos建筑工地的施工人员定位与跟踪算法。

背景技术：

1、nlos建筑工地是一种采用管理系统对现场人员分布情况和活动轨迹进行实时监控的建筑施工场景，其通过定位系统可以实时了解工地上的人数和位置，从而可以对现场情况进行监控，并且对可能出现以及已经出现的安全事故根据事先设置好的预警规则来进行报警和处理。从而大大提高了建筑工地的安全性。

2、在nlos建筑工地中，施工人员是活动的，因此对于施工人员的定位和跟踪极为关键，在现有的技术手段中，均采用基站与uwb标签配合，uwb标签给于施工人员随身携带，通过数据通讯实时掌握uwb标签的位置，从而推断施工人员的位置。

3、然而，在定位过程中，标签通过超宽带信号与基站进行实时通信，并且通信后通过获取到的超宽带信号发送与接收的时间戳信息，得到测距信息，通过算法得到标签与相应基站之间的距离，在计算过程中，跟踪算法是基于通讯数据采用算法计算得到施工人员位置信息，由于计算方式的不同，施工人员计算位置信息与施工人员实际位置信息具有较大的偏差，导致了定位结果与实际位置偏差较大，极大影响了对施工人员的定位跟踪。

技术实现思路

1、本发明要解决的技术问题是提供一种面向nlos建筑工地的施工人员定位与跟踪算法，能够准确的计算施工人员实际位置实现对施工人员的精准定位与跟踪。

2、为了解决上述技术问题，本发明提供了一种面向nlos建筑工地的施工人员定位与跟踪算法，采用定位系统，所述定位系统包括施工人员所佩戴的标签、固定已知位置的至少三个基站、用于接收测距信息且计算标签位置信息的服务器，基站与服务器之间进行网络数据交互的交换机，所述基站安装于建筑工地的施工现场，交换机用于对基站进行供电；包括以下步骤：

3、标签通过超宽带信号与基站进行实时通信，通信后，基站通过获取到的超宽带信号发送与接收的时间戳信息，并由基站上搭载的微型处理单元处理，得到测距信息，即得到标签与相应基站之间的距离，多个基站得到的测距信息通过交换机发送至服务器，服务器获得关于某一标签的所有测距信息后，根据测距信息，运用加权最小二乘算法对taylor算法运算需要的初值进行优化，随后根据优化后的初值运用taylor算法求得初步定位结果，最后运用容积kalman滤波算法对初步定位结果优化，得到最终估计位置。

4、进一步的，标签与相应基站之间的距离通过标签与基站之间连续的6个时间戳求得飞行时间t，根据飞行时间t计算距离d＝t·c，其中c为空气中电磁波传播速度。

5、进一步的，6个时间戳由以下方式获得：标签向基站发送一条测距请求信息，记录此时发送时间戳tsp，基站记录收到来自标签的接收时间戳trp，延迟一定时间后，发送回复信息至标签，记录发送时间戳tsr，标签收到回复信息时记录当前时间戳trr，延迟一段时间后，发送一条测距信息给基站，记录发送时间戳tsf，基站记录接收时间戳trf，得到6个时间戳。

6、进一步的，获取taylor算法的初值方法如下：

7、将基站的三维坐标(xi,yi,zi)(i＝1,2…n)投影至二维水平平面进行计算，得到

8、将上述式中前n-1个方程与最后一个方程相减，可得ax＝b，其中，

9、

10、

11、x为标签位置坐标值；

12、设n-1维的定位误差矩阵en则en＝ax-b，基于最小二乘原理，令f(x)＝|ax-b|2，对f(x)求导，并令导数为0，可得x最优估计值，公式如下：

13、

14、随后对每一基站测得的距离附加不同权重，加权权重矩阵为w，基于加权最小二乘算法可得标签位置估计值

15、当w＝r-1时，的均方误差最小，其中r为测距误差方差，采用公式对加权权重矩阵的元素进行定义，公式为式中，i表示的是对应标签测距的基站编号，j表示的是标签编号，n为测距总数；

16、随后计算估计值误差统计参数pk，pk表示第k个估计值与其余估计值间的差异程度，并设置野值剔除阈值p，当pk＞p说明该组的标签位置估计值误差较大，予以剔除，取剔除结束后，剩标签位置估计值t个，根据标签位置估计值计算标签估计坐标：

17、即初值。

18、进一步的，根据所得的标签估计坐标(x0,y0)，设标签真实坐标为(x,y)，则定位误差方程为其中(δx,δy)为初值与真值间误差；

19、根据距离公式建立定位函数：ki为定位标签与各基站水平面内投影距离，将上式在(x0,y0)处taylor展开可得：

20、其中

21、ki|0(i＝1,2...n)为输入的初值与各基站水平投影间距，进一步可得，

22、

23、进而可得εi＝hi-ξiδ，其中

24、

25、由最小二乘原理，可得δ的最优估计值

26、通过判断定位误差的大小与预设阈值之间的关系，推断算法是否需进一步迭代计算以减少定位误差，若定位误差大于预设阈值，则将当前计算结果作为新的初值带入计算，直到定位误差小于预设阈值或达到预设最大迭代次数，得到标签坐标估计值为(x0+δx,y0+δy)。

27、进一步的，将量测噪声方差作为待估计参数加入容积kalman滤波算法的系统模型中，将标签坐标估计值作为容积kalman滤波算法的观测值并对标签位置进行平滑。

28、进一步的，系统模型为：

29、其中k表示k时刻，zk和xk+1分别是k时刻的量测向量及状态向量，其中状态向量包含待估计位置参数及量测噪声方差；hk(·)和fk(·)分别为量测函数和状态转移函数；wk和vk分别为过程噪声及量测噪声，满足零均值高斯分布，且相互独立。

30、本发明的有益效果：

31、定位与跟踪算法首先运用加权最小二乘算法对taylor算法的初值进行优化，优化后的初值运用能够大大提高定位精度，进而运用taylor求得的初步定位结果与实际位置偏差大大减小，最后运用容积kalman滤波算法对初步定位结果进行进一步优化，得到最终估计位置，实现高精度的施工人员的定位与跟踪功能。

技术特征：

1.一种面向nlos建筑工地的施工人员定位与跟踪算法，其特征在于，采用定位系统，所述定位系统包括施工人员所佩戴的标签、固定已知位置的至少三个基站、用于接收测距信息且计算标签位置信息的服务器，基站与服务器之间进行网络数据交互的交换机，所述基站安装于建筑工地的施工现场，交换机用于对基站进行供电；包括以下步骤：

2.如权利要求1所述的面向nlos建筑工地的施工人员定位与跟踪算法，其特征在于，标签与相应基站之间的距离通过标签与基站之间连续的6个时间戳求得飞行时间t，根据飞行时间t计算距离d＝t·c，其中c为空气中电磁波传播速度。

3.如权利要求2所述的面向nlos建筑工地的施工人员定位与跟踪算法，其特征在于，6个时间戳由以下方式获得：标签向基站发送一条测距请求信息，记录此时发送时间戳tsp，基站记录收到来自标签的接收时间戳trp，延迟一定时间后，发送回复信息至标签，记录发送时间戳tsr，标签收到回复信息时记录当前时间戳trr，延迟一段时间后，发送一条测距信息给基站，记录发送时间戳tsf，基站记录接收时间戳trf，得到6个时间戳。

4.如权利要求1所述的面向nlos建筑工地的施工人员定位与跟踪算法，其特征在于，获取taylor算法的初值方法如下：

5.如权利要求4所述的面向nlos建筑工地的施工人员定位与跟踪算法，其特征在于，根据所得的标签估计坐标(x0,y0)，设标签真实坐标为(x,y)，则定位误差方程为其中(δx,δy)为初值与真值间误差；

6.如权利要求5所述的面向nlos建筑工地的施工人员定位与跟踪算法，其特征在于，将量测噪声方差作为待估计参数加入容积kalman滤波算法的系统模型中，将标签坐标估计值作为容积kalman滤波算法的观测值并对标签位置进行平滑。

7.如权利要求6所述的面向nlos建筑工地的施工人员定位与跟踪算法，其特征在于，系统模型为：

技术总结
本发明公开了一种面向NLOS建筑工地的施工人员定位与跟踪算法，采用定位系统，定位系统包括施工人员所佩戴的标签、固定已知位置的至少三个基站、用于接收测距信息且计算标签位置信息的服务器，基站与服务器之间进行网络数据交互的交换机，基站安装于建筑工地的施工现场；根据基站通过获取到的超宽带信号发送与接收的时间戳信息，得到测距信息，多个基站得到的测距信息通过交换机发送至服务器，根据测距信息，运用加权最小二乘算法对Taylor算法运算需要的初值进行优化，随后根据优化后的初值运用Taylor算法求得初步定位结果，运用容积Kalman滤波算法对初步定位结果优化，得到最终估计位置。本发明能够准确的定位施工人员的位置，提高施工安全性。

技术研发人员：钱小毛,杨威,杨叶青,马绍刚,顾国
受保护的技术使用者：江苏金麟技术检测鉴定集团有限公司
技术研发日：
技术公布日：2024/3/27