一种基于UWB的三维定位方法及定位装置与流程

本发明涉及uwb定位，更具体地说，它涉及一种基于uwb的三维定位方法及定位装置。

背景技术：

1、随着港口和码头的快速发展，码头吊机和码头的散货漏斗越来越广泛地被使用；其中，码头吊机能够快速、大量地抓取货物，具有升降高度大、搬运能力强、搬运速度快等有点；散货漏斗用于与吊机相互配合，能够接收吊机移送过来的货物，并且转移到传输带上，将堆放状态的货物依次从底部输出，以使货物相互分散，便于工人搬运转移。

2、但是采用吊机和散货漏斗的组合，也存在一定的危险：码头吊机驾驶员对抓斗视角存在视角误判跟盲区死角，导致对货物抓取、物料卸货、吊机抓斗高度无法精准的控制。且由于码头散货可移动漏斗并非固定地方使用，需要灵活走动，在散货漏斗上布设网线跟电线存在困难性。现有的技术大多数都是使用uwb设备之间数据通过有线连接实现数据交互，使用poe交换机给uwb设备供电使用tcp服务通讯。这种方法在散货漏斗上实现存在安全隐患，散货漏斗需要灵活走动就没办法实现。

3、吊机驾驶员需要控制抓斗上升跟下降对货物进行抓取，在抓斗上铺设电线也存在铺设电线难，存在安全隐患问题。

4、现有技术uwb设备设备层跟服务器服务层采用网线连接使用tcp通讯，没有考虑到现场环境跟布线难度，所以在现场铺设网线这种方式没办法使用在码头可移动散货漏斗上，服务器显示画面也没办法直接显示到码头吊机驾驶舱内。

技术实现思路

1、针对现有技术存在的不足，本发明的目的在于提供一种基于uwb的三维定位方法及定位装置，以克服现有的技术中存在的吊机驾驶员难以准确观察到散货漏斗内的情况的缺点。

2、本发明的上述技术目的是通过以下技术方案得以实现的：一种基于uwb的三维定位方法，应用于至少四个uwb定位基站、uwb定位标签以及服务器；用于测量散货漏斗与吊机之间的相对位置；

3、至少四个所述uwb定位基站均固定设置在散货漏斗上；所述uwb定位标签固定设置在吊机上；所述uwb定位标签分别与各个uwb定位基站通信连接；各个uwb定位基站分别与服务器通信连接；

4、所述定位方法包括：

5、获取各个uwb定位基站的位置；利用各个uwb定位基站分别计算与uwb定位标签之间的长度l；

6、接收各个uwb定位基站发送的长度l；

7、根据各个uwb定位基站对应的长度l以及各个uwb定位基站的位置，计算所述uwb定位标签在若干uwb定位基站形成的平面上的投影点坐标(x,y)以及uwb定位标签与若干uwb定位基站形成的平面之间的距离z；

8、输出uwb定位标签的三维坐标(x,y,z)。

9、可选的，所述利用各个uwb定位基站分别计算与uwb定位标签之间的长度l；包括：

10、分别利用各个uwb定位基站接收通讯范围内的uwb定位标签发送的uwb定位信号；

11、分别计算uwb定位信号到达各个uwb定位基站的飞行时间tprop；

12、根据uwb定位信号到达各个uwb定位基站的飞行时间tprop，分别计算uwb定位标签与各个uwb定位基站之间的长度l。

13、可选的，所述分别计算uwb定位信号到达各个uwb定位基站的飞行时间tprop，包括：

14、利用uwb定位基站接收uwb标签发射的第一测距信号，获取所述第一测距信号的第一发射时间t0以及接收第一测距信号的第一接收时间t1；

15、利用uwb定位基站向所述uwb标签发射第二测距信号，获取所述第二测距信号的第二发射时间t2以及第二测距信号到达uwb标签的第二接收时间t3；

16、利用uwb定位基站接收uwb标签发射的第三测距信号，获取所述第三测距信号的第三发射时间t4以及接收第三测距信号的第三接收时间t5；

17、根据所述第一发射时间t0、第一接收时间t1、第二发射时间t2、第二接收时间t3、第三发射时间t4以及第三接收时间t5，计算uwb信号的飞行时间tprop，包括：

18、

19、其中，tround1＝t3-t0，tround2＝t5-t2，treplay1＝t2-t1，treplay2＝t4-t3。

20、可选的，所述根据uwb定位信号到达各个uwb定位基站的飞行时间tprop，分别计算uwb定位标签与各个uwb定位基站之间的长度l，包括：

21、l＝c*tprop；

22、其中，其中c为电磁波在真空中的传播速度，c＝3*108m/s。

23、可选的，所述根据uwb定位标签与各个uwb定位基站之间的长度l以及各个uwb定位基站的位置，计算所述uwb定位标签在若干uwb定位基站形成的平面上的投影点坐标(x,y)，包括：

24、从四个uwb基站中取三个uwb定位基站记作基站组，共得到四个基站组；

25、在uwb定位基站形成的平面上构建坐标系；

26、分别利用每一个基站组中的三个基站与uwb定位标签之间的长度，计算uwb标签在uwb定位基站形成的平面上的质心坐标(xki,yki)，得到四个质心坐标(xk1,yk1)、(xk2,yk2)、(xk3,yk3)以及(xk4,yk4)；

27、计算四个质心坐标的平均值，作为投影点初始坐标(xk,yk)，包括：

28、

29、

30、根据所述投影点初始坐标(xk,yk)，计算投影点坐标(x,y)，包括：

31、

32、其中，δx,δy为预设的差值。

33、可选的，所述分别利用每一个基站组中的三个基站与uwb定位标签之间的长度，计算uwb标签在uwb定位基站形成的平面上的质心坐标(xki,yki)，包括：

34、获取uwb定位标签于第一uwb定位基站之间的第一长度l1；获取uwb定位标签与第二uwb定位基站之间的第二长度l2；获取uwb定位标签与第三uwb定位基站之间的第三长度l3；

35、在若干uwb定位基站形成的平面上构建坐标系，分别获取基站组中第一uwb定位基站对应的第一坐标(x1,y1)、基站组中第二uwb定位基站对应的第二坐标(x2,y2)以及基站组中第三uwb定位基站对应的第三坐标(x3,y3)；

36、第一坐标(x1,y1)为圆心、第一长度l1为半径，在坐标系中构建第一圆形区域；第二坐标(x2,y2)为圆心、第二长度l2为半径，在坐标系中构建第二圆形区域；第三坐标(x3,y3)为圆心、第三长度l3为半径，在坐标系中构建第三圆形区域；

37、判断第一圆形区域、第二圆形区域以及第三圆形区域是否存在相互重叠的重叠区域，若是，则计算所述重叠区域的质心坐标(xki,yki)。

38、可选的，所述计算所述重叠区域的质心坐标(xki,yki)，包括：

39、获取第一圆形区域方程：(x-x1)2+(y-y1)2＝l12；

40、获取第二圆形区域方程：(x-x2)2+(y-y2)2＝l22；

41、获取第三圆形区域方程：(x-x3)2+(y-y3)2＝l32；

42、根据三个圆形区域的方程计算重叠区域的三个顶点的坐标(x4,y4)、(x5,y5)以及(x6,y6)；

43、根据三个顶点的坐标(x4,y4)、(x5,y5)以及(x6,y6)，计算重叠区域的质心坐标(x,y)，包括：引入线性方程：

44、aq＝b；

45、分别将第一圆形区域的方程、第二圆形区域的方程以及第三圆形区域的方程进行线性化，得到方程组：

46、

47、

48、

49、其中a为可逆矩阵，满足：

50、(x4-x6)(y5-y6)-(y4-y6)(x5-x6)≠0；

51、通过上述方程组，解算q值，得到质心坐标(xki,yki)；

52、还包括：在质心坐标(xki,yki)为多个结果的情况下，利用最小二乘法解算q，包括：

53、q＝(ata)-1atb。

54、可选的，所述计算所述重叠区域的质心坐标(xki,yki)，包括：

55、获取第一圆形区域方程：(x-x1)2+(y-y1)2＝l12；

56、获取第二圆形区域方程：(x-x2)2+(y-y2)2＝l22；

57、获取第三圆形区域方程：(x-x3)2+(y-y3)2＝l32；

58、根据三个圆形区域的方程计算重叠区域的三个顶点的坐标；根据三个顶点的坐标计算三个坐标所形成的三角形的质心，包括：

59、将第一圆形区域方程、第二圆形区域方程以及第三圆形区域方程展开，得到：

60、

61、设：

62、

63、

64、

65、利用最小二乘法解算p，得到对应的质心坐标(xki,yki)，包括：

66、p＝(ctc)-1ctd。

67、可选的，所述根据各个uwb定位基站对应的长度l以及各个uwb定位基站的位置，计算uwb定位标签与若干uwb定位基站形成的平面之间的距离z，包括：

68、计算质心坐标(xki,yki)与基站组中的第一uwb定位基站的之间的第四长度l4，包括：

69、(xki-x1)2+(yki-y1)2＝l42；

70、计算质心坐标(xki,yki)与基站组中的第二uwb定位基站的之间的第五长度l5，包括：

71、(xki-x2)2+(yki-y2)2＝l52；

72、计算质心坐标(xki,yki)与基站组中的第三uwb定位基站的之间的第六长度l6，包括：

73、(xki-x3)2+(yki-y3)2＝l62；

74、根据第四长度l4以及第一长度l1，计算uwb定位标签与第一uwb定位基站所在的平面之间的第一距离z1，包括：

75、l12-l42＝z12；

76、根据第五长度l5以及第二长度l2，计算uwb定位标签与第二uwb定位基站所在的平面之间的第二距离z1，包括：

77、l22-l52＝z22；

78、根据第四长度l6以及第三长度l3，计算uwb定位标签与第三uwb定位基站所在的平面之间的第三距离z3，包括：

79、l32-l62＝z32；

80、计算uwb定位标签与基站组中的三个uwb定位基站形成的平面之间的距离zki，包括：

81、zki＝(z1+z2+z3)/3。

82、可选的，所述根据各个uwb定位基站对应的长度l以及各个uwb定位基站的位置，计算uwb定位标签与若干uwb定位基站形成的平面之间的距离z，还包括：

83、分别计算uwb标签到四个基站组对应的平面之间的距离zk1、zk2、zk3以及zk4；

84、计算四个距离的平均值作为实际距离：

85、z＝(zk1+zk2+zk3+zk4)/4。

86、可选的，还包括：在散货漏斗上设置第一太阳能充电板以及第一储能模块；所述第一太阳能充电板与第一储能模块电连接；所述第一储能模块分别与各个uwb定位基站电连接；

87、还包括：在吊机上设置第二太阳能充电板、第二储能模块、加速度传感器以及用于根据加速度传感器的信号控制uwb定位标签是否工作的控制组件；所述第二太阳能充电板与第二储能模块电连接；所述第二储能模块与控制组件电连接；所述加速度传感器与控制组件电连接；所述uwb定位标签与控制组件电连接。

88、一种基于uwb的三维定位装置，包括：至少三个uwb定位基站、uwb定位标签以及服务器；所述服务器包括：

89、长度计算模块：用于获取各个uwb定位基站的位置；利用各个uwb定位基站分别计算与uwb定位标签之间的长度l；

90、长度接收模块：用于接收各个uwb定位基站发送的长度l；

91、坐标计算模块：用于根据各个uwb定位基站对应的长度l以及各个uwb定位基站的位置，计算所述uwb定位标签在若干uwb定位基站形成的平面上的投影点坐标(x,y)以及uwb定位标签与若干uwb定位基站形成的平面之间的距离z；

92、三维坐标输出模块：用于输出uwb定位标签的三维坐标(x,y,z)。

93、综上所述，本发明具有以下有益效果：通过在某一个平面上布置若干个用于定位的uwb定位基站，能够对通信范围内的uwb标签进行三维定位，不仅能够确定uwb标签的投影位置，还能够计算出uwb定位标签的高度，进而确定uwb定位标签的空间坐标，根据空间坐标能够在预设的3d图像中对uwb标签的位置进行显示，以实现对安装有uwb标签的吊机进行准确定位，避免在转移货物过程中发生危险。