一种基于UWB与GPS融合的无人车循迹方法及终端与流程

一种基于uwb与gps融合的无人车循迹方法及终端
技术领域
1.本发明涉及无人驾驶技术领域，尤其涉及一种基于uwb与gps融合的无人车循迹方法及终端。


背景技术：

2.无人驾驶汽车是汽车领域今后发展的主要趋势，汽车循迹驾驶是无人车自动驾驶的一种方式。其中，循迹驾驶是指汽车根据预先设置好的路线进行自动循迹驾驶。目前无人循迹驾驶采用的定位导航技术大都是gps(global positioning system，全球定位系统)、北斗等，但是当这些信号被屏蔽时，或者处于信号弱、信号受干扰的情况下，仅通过单独的gps模块无法提供精确的gps定位导航，造成无人循迹驾驶功能适用的区域范围较小，无人循迹驾驶功能的应用场景比较局限。
3.此外，gps的定位精度通常在3米左右，而uwb(ultra wideband，超宽带)定位技术的测距精度则为10厘米，相比于gps模块的定位精度，uwb的测距精度更高，且uwb标签模块价格经济实惠，相对于gps或北斗的定位芯片成本降低很多。


技术实现要素：

4.本发明所要解决的技术问题是：提供一种基于uwb与gps融合的无人车循迹方法及终端，避免单一定位模块无法提供精确的定位信息，提高无人循迹驾驶的精确度。
5.为了解决上述技术问题，本发明采用的技术方案为：
6.一种基于uwb与gps融合的无人车循迹方法，包括：
7.s1、根据多个uwb基站得到所述uwb标签的实时定位信息，所述uwb标签位于无人车上；
8.s2、获取所述无人车的gps定位信息，根据所述gps定位信息得到所述无人车的第一行驶数据，同时根据所述实时定位信息得到所述无人车的第二行驶数据，判断所述第一行驶数据与所述第二行驶数据是否一致，若否，则记录所述第二行驶数据；
9.s3、根据目标无人车路径轨迹与所述第二行驶数据得到所述无人车的转向信息，重复执行所述s1-s3，直至所述实时定位信息与所述目标无人车路径轨迹中预设的终点信息一致。
10.为了解决上述技术问题，本发明采用的另一种技术方案为：
11.一种基于uwb与gps融合的无人车循迹终端，包括存储器、处理器及存储在所述存储器上并在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现所述一种基于uwb与gps融合的无人车循迹方法的各个步骤。
12.本发明的有益效果在于：通过uwb(ultra wideband，超宽带)定位技术与gps定位技术同时对无人车进行实时定位，基于uwb定位技术验证gps定位技术的准确性，使得无人车可以在gps等卫星信号较弱或丢失的情况，甚至在不使用gps卫星信号的情况下完成预设的无人车驾驶循迹任务，避免单一定位模块无法提供精确的定位信息，提高无人循迹驾驶
的普遍实用性。
附图说明
13.图1为本发明实施例提供的一种基于uwb与gps融合的无人车循迹方法的步骤流程图；
14.图2为本发明实施例提供的一种基于uwb与gps融合的无人车循迹方法的空间坐标图；
15.图3为本发明实施例提供的一种基于uwb与gps融合的无人车循迹终端的结构示意图；
16.标号说明：
17.1、一种基于uwb与gps融合的无人车循迹终端；2、存储器；3、处理器。
具体实施方式
18.为详细说明本发明的技术内容、所实现目的及效果，以下结合实施方式并配合附图予以说明。
19.请参照图1，本发明实施例提供的一种基于uwb与gps融合的无人车循迹方法，包括：
20.s1、根据多个uwb基站得到所述uwb标签的实时定位信息，所述uwb标签位于无人车上；
21.s2、获取所述无人车的gps定位信息，根据所述gps定位信息得到所述无人车的第一行驶数据，同时根据所述实时定位信息得到所述无人车的第二行驶数据，判断所述第一行驶数据与所述第二行驶数据是否一致，若否，则记录所述第二行驶数据；
22.s3、根据目标无人车路径轨迹与所述第二行驶数据得到所述无人车的转向信息，重复执行所述s1-s3，直至所述实时定位信息与所述目标无人车路径轨迹中预设的终点信息一致。
23.从上述描述可知，本发明的有益效果在于：通过uwb(ultra wideband，超宽带)定位技术与gps定位技术同时对无人车进行实时定位，基于uwb定位技术验证gps定位技术的准确性，使得无人车可以在gps等卫星信号较弱或丢失的情况，甚至在不使用gps卫星信号的情况下完成预设的无人车驾驶循迹任务，避免单一定位模块无法提供精确的定位信息，提高无人循迹驾驶的普遍实用性。
24.进一步的，所述uwb基站的数量至少为三个；所述实时定位信息包括即时经纬度信息；
25.所述s1具体为：
26.判断所述uwb标签是否同时接收到所述多个uwb基站的测距信号，若是，则根据所述测距信号得到所述uwb标签与每一uwb基站的定位距离；
27.基于所述定位距离得到所述uwb标签的即时经纬度信息。
28.由上述描述可知，基于超宽带定位技术对待测运动平台之间进行测距，并以距离数据及站点位置信息为基础，最后通过三边定位算法得到标签节点的定位估计；由于卫星信号在室内会被严重干扰，从而导致gps或是北斗无法发送定位信息，因此无人车在此情况
下，使用超宽带模块进行测距，可以获得厘米级精度的距离数据，避免单一定位模块无法提供精确的定位信息，提高定位信息的准确性。此外，超宽带标签模块相对于卫星定位芯片价格便宜很多，降低了无人车成本。
29.进一步的，所述第一行驶数据包括第一移动距离和第一航向角，所述第二行驶距数据包括第二移动距离和第二航向角；
30.所述s2具体为：
31.获取所述无人车的gps定位信息，其中，所述gps定位信息包括前一时刻和当前时刻的gps定位信息；
32.根据所述前一时刻和当前时刻的gps定位信息得到所述无人车的第一移动距离和第一航向角；
33.同时获取所述无人车的实时定位信息，其中，所述实时定位信息包括前一时刻和当前时刻的实时定位信息；
34.根据所述前一时刻和当前时刻的实时定位信息得到所述无人车的第二移动距离和第二航向角；
35.判断所述第一移动距离和第一航向角与所述第二移动距离和第二航向角是否一致，若否，则记录所述第二移动距离和第二航向角。
36.由上述描述可知，超宽带定位与卫星定位同时获取无人车的实时定位信息，基于超宽带定位从而验证卫星定位的精准性，避免在卫星定位被干扰的情况下，无人车无法精准定位，造成无人车循迹出现偏差，提高了无人车循迹的精准度。
37.进一步的，所述s3具体为：
38.获取所述目标无人车路径轨迹的点集合，根据所述当前时刻的实时定位信息和第二航向角确定所述点集合中的目标点；
39.计算所述当前时刻的实时定位信息相对于所述目标点的目标航向角；
40.根据所述目标航向角与所述第二航向角得到所述无人车的转向信息；
41.控制所述无人车根据所述转向信息进行转向；
42.重复执行所述s1-s3，直至所述实时定位信息与所述目标无人车路径轨迹中预设的终点信息一致。
43.由上述描述可知，在结合超宽带定位技术与卫星定位技术下获取的实时定位信息精确度高，从而保证后续无人车进行循迹任务的成功率。
44.进一步的，计算所述当前时刻的实时定位信息相对于所述目标点的目标航向角具体为：
45.目标航向角ret
目标
＝ar cosθ；
46.其中，所述当前时刻的实时定位信息为(lon0，lat0)，所述目标点为(lon1，lat1)；其中lon表示所述uwb标签的经度坐标，lat表示所述uwb标签的纬度坐标；dist表示所述实时定位信息与所述目标点之间的距离，cosθ表示目标航向角的余弦值；
47.所述根据所述目标航向角与所述第二航向角得到所述无人车的转向信息具体为：
48.若所述目标航向角与所述第二航向角的差值大于零，则所述无人车执行左转；
49.若所述目标航向角与所述第二航向角的差值等于零，则所述无人车保持直行；
50.若所述目标航向角与所述第二航向角的差值小于零，则所述无人车执行右转。
51.由上述描述可知，无人车每获取一次实时定位信息则表示无人车发生了移动，因此基于无人车的实时定位信息逐一获取下一个需到达的目标点，由无人车的当前位置和需到达的目标点即可精准计算无人车下一步的航向。且实时定位信息可通过卫星定位或超宽带定位得到，精确性较高，提高了无人车循迹任务的成功率。
52.进一步的，所述基于所述定位距离得到所述uwb标签的即时经纬度信息具体为：
53.获取每一uwb基站的站点经纬度信息，根据所述站点经纬度信息计算对应uwb基站所在位置的地球半径；
54.根据所述地球半径获取任意两个uwb基站之间的三维空间距离；
55.根据所述三维空间距离计算所述两个uwb基站之间的二维空间距离以及相对航向角，基于所述uwb基站建立坐标系；
56.根据所述二维空间距离和定位距离得到所述uwb标签的即时标签坐标；
57.将所述即时标签坐标与所述相对航向角代入坐标旋转公式，得到所述uwb标签的即时经纬度信息。
58.由上述描述可知，基于定位技术获取的三维数据经过对应计算后得到二维数据，将无人车的移动路径由三维转化为二维，降低了循迹算法的复杂度，提高了循迹算法处理数据速度，从而提高无人车循迹任务的效率。
59.进一步的，所述地球半径包括对应uwb基站所在的球半径和所在纬度的纬度圈半径；
60.所述根据所述站点经纬度信息计算对应uwb基站所在位置的地球半径具体为：
61.所述球半径为
62.所述纬度圈半径为
63.其中，ea为地球赤道半径，eb为地球极半径，所述站点经纬度信息分别为为(lon
z0
，lat
z0
)和(lon
z1
，lat
z1
)；其中lonz为所述uwb基站的经度坐标，latz为所述uwb基站的纬度坐标。
64.由上述描述可知，通过超宽带基站的经纬度信息进行三维空间计算，得到所述站点与地球球心的距离以及所述站点与地球极轴的距离，便于以该站点建立坐标系，从而将三维空间计算转换为二维空间计算，简化算法。
65.进一步的，所述三维空间距离为所述任意两个uwb基站之间的弧长；
66.所述根据所述地球半径获取任意两个uwb基站之间的三维空间距离具体为：
[0067][0068]
其中，ee为任意两个uwb基站所在球半经的圆的弧长，ef为任意两个uwb基站所在纬度圈的圆的弧长。
[0069]
由上述描述可知，通过经纬度信息获取的站点距离为三维空间距离，两点之间的连线为一圆弧，因此，通过任意两个站点所在球半径的圆以及纬度圈的圆计算其三维空间
距离；基于三维空间距离计算两个站点的相对关系。
[0070]
进一步的，所述根据所述三维空间距离计算所述两个uwb基站之间的二维空间距离以及相对航向角具体为：
[0071]
所述二维空间距离为
[0072]
所述相对航向角为
[0073]
由上述描述可知，根据超宽带基站之间的相对关系，建立对应坐标系，从而将三维空间数据转化为二维空间数据，降低循迹算法复杂度，同时基于基站建立坐标系，定位数据精确度高，可适用性强，只要无人车周围建设有超宽带基站，即可采用本技术实现无人车循迹，提高无人循迹驾驶的普遍实用性。
[0074]
请参照图3，本发明另一实施例提供了一种基于uwb与gps融合的无人车循迹终端，包括存储器、处理器及存储在所述存储器上并在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现所述一种基于uwb与gps融合的无人车循迹方法的各个步骤。
[0075]
从上述描述可知，本发明的有益效果在于：通过uwb(ultra wideband，超宽带)定位技术与gps定位技术同时对无人车进行实时定位，基于uwb定位技术验证gps定位技术的准确性，使得无人车可以在gps等卫星信号较弱或丢失的情况，甚至在不使用gps卫星信号的情况下完成预设的无人车驾驶循迹任务，避免单一定位模块无法提供精确的定位信息，提高无人循迹驾驶的普遍实用性。
[0076]
本发明实施例提供的一种基于uwb与gps融合的无人车循迹方法及终端，可应用于无人车辆的循迹驾驶，避免单一定位模块无法提供精确的定位信息，提高无人循迹驾驶的精确性，以下通过具体实施例来说明：
[0077]
请参照图1至图2，本发明的实施例一为：
[0078]
一种基于uwb与gps融合的无人车循迹方法，包括：
[0079]
s1、根据多个uwb基站得到所述uwb标签的实时定位信息，所述uwb标签位于无人车上。
[0080]
其中，所述uwb基站的数量至少为三个，所述实时定位信息包括即时经纬度信息。
[0081]
所述s1具体为：
[0082]
s11、判断所述uwb标签是否同时接收到所述多个uwb基站的测距信号，若是，则根据所述测距信号得到所述uwb标签与每一uwb基站的定位距离。
[0083]
s12、基于所述定位距离得到所述uwb标签的即时经纬度信息；
[0084]
具体地，获取每一uwb基站的站点经纬度信息，根据所述站点经纬度信息计算对应uwb基站所在位置的地球半径。
[0085]
其中，所述地球半径包括对应uwb基站所在的球半径和所在纬度的纬度圈半径：
[0086]
所述球半径为
[0087]
所述纬度圈半径为
[0088]
其中，ea为地球赤道半径，eb为地球极半径，所述站点经纬度信息分别为为(lon
z0
，lat
z0
)和(lon
z1
，lat
z1
)；其中lonz为所述uwb基站的经度坐标，latz为所述uwb基站的纬度坐标。
[0089]
具体地，ea＝6378137米，eb＝6356752.2142米。
[0090]
s13、根据所述地球半径获取任意两个uwb基站之间的三维空间距离。
[0091]
其中，所述三维空间距离为所述任意两个uwb基站之间的弧长：
[0092]
具体地，
[0093]
其中，ee为任意两个uwb基站所在球半经的圆的弧长，ef为任意两个uwb基站所在纬度圈的圆的弧长。
[0094]
s14、根据所述三维空间距离计算所述两个uwb基站之间的二维空间距离以及相对航向角，基于所述uwb基站建立坐标系。
[0095]
具体地，所述二维空间距离为
[0096]
所述相对航向角为
[0097]
s15、根据所述二维空间距离和定位距离得到所述uwb标签的即时标签坐标。
[0098]
s16、将所述即时标签坐标与所述相对航向角代入坐标旋转公式，得到所述uwb标签的即时经纬度信息。
[0099]
s2、获取所述无人车的gps定位信息，根据所述gps定位信息得到所述无人车的第一行驶数据，同时根据所述实时定位信息得到所述无人车的第二行驶数据，判断所述第一行驶数据与所述第二行驶数据是否一致，若否，则记录所述第二行驶数据。
[0100]
其中，所述第一行驶数据包括第一移动距离和第一航向角，所述第二行驶距数据包括第二移动距离和第二航向角。
[0101]
所述s2具体为：
[0102]
s21、获取所述无人车的gps定位信息，其中，所述gps定位信息包括前一时刻和当前时刻的gps定位信息；
[0103]
s22、根据所述前一时刻和当前时刻的gps定位信息得到所述无人车的第一移动距离和第一航向角；
[0104]
s23、同时获取所述无人车的实时定位信息，其中，所述实时定位信息包括前一时刻和当前时刻的实时定位信息；
[0105]
s24、根据所述前一时刻和当前时刻的实时定位信息得到所述无人车的第二移动距离和第二航向角；
[0106]
s25、判断所述第一移动距离和第一航向角与所述第二移动距离和第二航向角是否一致，若否，则记录所述第二移动距离和第二航向角。
[0107]
在一种可选的实施方式中，所述s3之前包括：操控无人车运行采集轨迹点，组成目标无人车路径轨迹。
[0108]
s3、根据目标无人车路径轨迹与所述第二行驶数据得到所述无人车的转向信息，重复执行所述s1-s3，直至所述实时定位信息与所述目标无人车路径轨迹中预设的终点信
息一致。
[0109]
所述s3具体为：
[0110]
s31、获取所述目标无人车路径轨迹的点集合，根据所述当前时刻的实时定位信息和第二航向角确定所述点集合中的目标点。
[0111]
s32、计算所述当前时刻的实时定位信息相对于所述目标点的目标航向角。
[0112]
具体地，目标航向角为ret
目标
＝arcosθ；
[0113]
其中，所述当前时刻的实时定位信息为(lon0，lat0)，所述目标点为(lon1，lat1)；其中lon表示所述uwb标签的经度坐标，lat表示所述uwb标签的纬度坐标；dist表示所述实时定位信息与所述目标点之间的距离，cosθ表示目标航向角的余弦值。
[0114]
s33、根据所述目标航向角与所述第二航向角得到所述无人车的转向信息。
[0115]
具体地，若所述目标航向角与所述第二航向角的差值大于零，则所述无人车执行左转；
[0116]
若所述目标航向角与所述第二航向角的差值等于零，则所述无人车保持直行；
[0117]
若所述目标航向角与所述第二航向角的差值小于零，则所述无人车执行右转。
[0118]
s34、控制所述无人车根据所述转向信息进行转向。
[0119]
s35、重复执行所述s1-s3，直至所述实时定位信息与所述目标无人车路径轨迹中预设的终点信息一致。
[0120]
请参照图1至图2，本发明的实施例二为：
[0121]
一种基于uwb与gps融合的无人车循迹方法应用于实际场景中，包括：
[0122]
d0、分别记录无人车周围3个uwb基站的站点经纬度信息；其中，基站0的站点经纬度信息为(lon
z0
，lat
z0
)，基站1的站点经纬度信息为(lon
z1
，lat
z1
)，基站2的站点经纬度信息为(lon
z2
，lat
z2
)。
[0123]
d1、判断uwb标签是否同时接收到3个uwb基站的测距信号，若是，则根据所述测距信号得到所述uwb标签与每一uwb基站的定位距离，并基于所述定位距离得到所述uwb标签的即时经纬度信息；其中，uwb标签与基站0的定位距离为dist_0，uwb标签与基站1的定位距离为dist_1。
[0124]
具体地，参照图2，d11、以基站0为坐标原点，建立直角坐标系，并根据3个uwb基站的站点经纬度信息计算基站0和基站1之间的二维空间距离以及相对航向角：
[0125]
(1)计算基站1所在的球半径为
[0126]
计算基站1所在纬度的纬度圈半径为
[0127]
(2)计算基站0和基站1所在球半径的圆的弧长
[0128]
计算基站0和基站1所在纬度圈的圆的弧长
[0129]
(3)计算基站0和基站1之间的二维空间距离
[0130]
计算基站0指向基站1的相对航向角
[0131]
d12、根据所述二维空间距离和定位距离得到所述uwb标签的即时标签坐标，并将所述即时标签坐标与所述相对航向角代入坐标旋转公式，得到所述uwb标签的即时经纬度信息：
[0132]
(1)计算uwb标签的即时标签坐标；
[0133]
基于三角形正余弦定理cosa＝(dist_02+dist_1
2-dist
二维2
)；
[0134][0135]
即时标签坐标的纵坐标为横坐标为
[0136]
(2)计算uwb标签的即时经纬度信息；
[0137]
uwb标签的即时经度为lon
tag
＝xcos(ret
相对
)+ysin(ret
相对
)；
[0138]
uwb标签的即时纬度为lat
tag
＝ycos(ret
相对
)-xsin(ret
相对
)。
[0139]
d2、获取所述无人车的gps定位信息，根据所述gps定位信息得到所述无人车的第一行驶数据，同时根据所述实时定位信息得到所述无人车的第二行驶数据，判断所述第一行驶数据与所述第二行驶数据是否一致，若否，则记录所述第二行驶数据。其中，所述第一行驶数据包括第一移动距离dist1和第一航向角ret1，所述第二行驶距数据包括第二移动距离dist2和第二航向角ret2。
[0140]
d3、根据目标无人车路径轨迹与所述第二行驶数据得到所述无人车的转向信息，重复执行所述d1-d3，直至所述实时定位信息与所述目标无人车路径轨迹中预设的终点信息一致。其中，所述实时定位信息为即时经纬度信息(lon
tag
，lat
tag
)。
[0141]
具体地，d31、获取所述目标无人车路径轨迹的点集合，根据所述当前时刻的实时定位信息(lon
tag
，lat
tag
)和第二航向角ret2确定所述点集合中距离无人车前方5米的轨迹点为目标点；其中，目标点坐标为(lon
obj
，lat
obj
)。
[0142]
d32、计算所述当前时刻的实时定位信息相对于所述目标点的目标航向角:
[0143][0144]
目标航向角为ret
目标
＝arcosθ。
[0145]
d33、计算所述目标航向角ret
目标
与所述第二航向角ret2差值得到所述无人车的转向信息：
[0146]
若ret
目标-ret2》0，则所述无人车执行左转；
[0147]
若ret
目标-ret2＝0，则所述无人车保持直行；
[0148]
若ret
目标-ret2《0，则所述无人车执行右转。
[0149]
d34、控制所述无人车根据所述转向信息进行转向。
[0150]
d35、重复执行所述d1-d3，直至所述实时定位信息与所述目标无人车路径轨迹中预设的终点信息一致。
[0151]
请参照图3，本发明的实施例三为：
[0152]
一种基于uwb与gps融合的无人车循迹终端1，包括存储器2、处理器3及存储在所述
存储器2上并在所述处理器3上运行的计算机程序，所述处理器3执行所述计算机程序时实现实施例一和实施例二中的各个步骤。
[0153]
综上所述，本发明提供的一种基于uwb与gps融合的无人车循迹方法及终端，通过uwb(ultra wideband，超宽带)定位技术与gps定位技术同时对无人车进行实时定位，由于卫星信号在室内会被严重干扰，从而导致gps或是北斗无法发送定位信息，且单独的gps定位精度一般为3米左右，uwb的测距精度为10厘米，融合gps和uwb能够大幅度提高无人车的定位精度，因此无人车在此情况下，使用超宽带模块进行测距，可以获得厘米级精度的距离数据，避免单一定位模块无法提供精确的定位信息，提高定位信息的准确性。且基于uwb定位技术验证gps定位技术的准确性，使得无人车可以在gps等卫星信号较弱或丢失的情况，甚至在不使用gps卫星信号的情况下完成预设的无人车驾驶循迹任务，拓宽了无人循迹驾驶的应用场景。此外，超宽带标签模块相对于卫星定位芯片价格便宜很多，降低了无人车成本。
[0154]
以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等同变换，或直接或间接运用在相关的技术领域，均同理包括在本发明的专利保护范围内。