1.一种基于UWB和GPS的定位跟随导航系统的方法,其特征在于,UWB和GPS的定位跟随导航包括:被跟随设备、跟随设备、基站以及以所述基站为中心点对称设置在所述基站两侧的两UWB,所述被跟随设备、跟随设备、基站以及两UWB之间两两之间相互通信连接;
所述方法包括以下步骤:
分别对被跟随设备以及跟随设备进行定位;
在完成被跟随设备与跟随设备的定位后,根据被跟随设备与跟随设备的相对位置在导航地图推算出跟随路线;
所述跟随设备根据所述跟随路线跟随所述被跟随设备移动;
所述跟随设备与被跟随设备的定位步骤相同,对所述被跟随设备进行定位包括:两UWB通过双边双向飞行时间法分别对被跟随设备进行测距,其中,两所述UWB以基站为中点分别对称设置在所述基站两侧;
基站根据两UWB的测距结果以及根据其中一个所述UWB与基站之间的距离基于中线定理计算出基站与被跟随设备之间的距离;
基站根据计算出的基站与被跟随设备之间的距离、基站与UWB之间的距离以及UWB与被跟随设备之间的距离基于余弦定理计算基站与被跟随设备之间的偏角θ,得出被跟随设备位置的两个待选点;
基站获取被跟随设备的GPS位置;
基站根据获取到的被跟随设备的GPS位置从两待选点中选出与被跟随设备的GPS位置最接近的待选点作为最终定位点。
2.根据权利要求1所述的基于UWB和GPS的定位跟随导航系统的方法,其特征在于,所述双边双向飞行时间法包括:被跟随设备在Ta1时刻向UWB发射请求信号;
UWB在Tb1时刻接收到所述请求信号后在Tb2时刻向被跟随设备发射响应信号;
被跟随设备在Ta2时刻接收到响应信号后向在Ta3时刻将Ta1,Ta2,Ta3的时间戳发送给UWB;
UWB在Tb3时刻接收到所述Ta1,Ta2,Ta3的时间戳后将所述Ta1,Ta2,Ta3的时间戳后以及自身存储的Tb1,Tb2,Tb3时间戳发送给基站;
基站基于接收到的所有时间戳计算飞行时间ΔTtotal并且将计算出的飞行时间ΔTtotal与光速C相乘得出UWB与被跟随设备之间的距离Stotal;
其中,计算飞行时间ΔTtotal的公式如下:
ΔTa23=Ta3‑Ta2;
ΔTa12=Ta2‑Ta1;
ΔTb12=Tb2‑Tb1;
ΔTb23=Tb3‑Tb2;
所述ΔTa23为Ta3与Ta2的时间差值;
所述ΔTa12为Ta2与Ta1的时间差值;
所述ΔTb12为Tb2与Tb1的时间差值;
所述ΔTb23为Tb3与Tb2的时间差值;计算UWB与被跟随设备之间的距离Stotal公式为:Stotal=ΔTtotal*C,C为光速。
3.根据权利要求1所述的基于UWB和GPS的定位跟随导航系统的方法,其特征在于,所述基于中线定理计算出基站与被跟随设备之间的距离包括:根据公式对基站与被跟随设备之间的距离进行计算,其中,AC为其中一UWB与所述被跟随设备之间的距离,BC为另一UWB与所述被跟随设备之间的距离,AD为其中一UWB与基站之间的距离,CD为被跟随设备与基站之间的距离。
4.根据权利要求1所述的基于UWB和GPS的定位跟随导航系统的方法,其特征在于,所述基于余弦定理计算基站与被跟随设备之间的偏角θ包括:根据公式:求出所述偏角θ,其中,AC为其中一UWB与所述被跟随设备之间的距离,BC为另一UWB与所述被跟随设备之间的距离,AD为其中一UWB与基站之间的距离,CD为被跟随设备与基站之间的距离。
5.根据权利要求1所述的基于UWB和GPS的定位跟随导航系统的方法,其特征在于,所述基于获取到的GPS信息从两预选点中确认最终定位点包括:计算被跟随设备GPS位置在导航地图上的坐标与两待选点在导航地图上的坐标的差值,取坐标差值最小的待选点为最终定位点。
6.根据权利要求1所述的基于UWB和GPS的定位跟随导航系统的方法,其特征在于,在分别对被跟随设备以及跟随设备进行定位之前,还包括:绘制导航地图,所述绘制导航地图包括:实地调查导航线路,在导航地图上绘制被遮挡物遮挡住的卫星地图上没有显示的道路。
7.根据权利要求6所述的基于UWB和GPS的定位跟随导航系统的方法,其特征在于,所述实地调查导航线路还包括:在导航地图上标出平行跟随区域以及后方跟随区域;其中,所述平行跟随区域为:在该区域内,所述跟随设备与被跟随设备平行跟随移动;
所述后方跟随区域为:在该区域内,所述跟随设备位于被跟随设备后方跟随移动。