CN116430310A - 一种基于uwb技术高精度无人船定位系统、方法及存储介质 - Google Patents

一种基于uwb技术高精度无人船定位系统、方法及存储介质 Download PDF

Info

Publication number
CN116430310A
CN116430310A CN202310493969.6A CN202310493969A CN116430310A CN 116430310 A CN116430310 A CN 116430310A CN 202310493969 A CN202310493969 A CN 202310493969A CN 116430310 A CN116430310 A CN 116430310A
Authority
CN
China
Prior art keywords
positioning
uwb
uwb positioning
unmanned ship
base station
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202310493969.6A
Other languages
English (en)
Inventor
李永正
侯倩雯
王璧蔚
张耀
陈健
薛顺
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiangsu University of Science and Technology
Original Assignee
Jiangsu University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jiangsu University of Science and Technology filed Critical Jiangsu University of Science and Technology
Priority to CN202310493969.6A priority Critical patent/CN116430310A/zh
Publication of CN116430310A publication Critical patent/CN116430310A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0257—Hybrid positioning
    • G01S5/0258—Hybrid positioning by combining or switching between measurements derived from different systems
    • G01S5/02585—Hybrid positioning by combining or switching between measurements derived from different systems at least one of the measurements being a non-radio measurement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/06—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/023—Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/025—Services making use of location information using location based information parameters
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/029—Location-based management or tracking services
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Automation & Control Theory (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开一种基于UWB的无人船定位系统、方法及存储介质，包括在湖泊周边设置至少4个UWB定位基站，建立湖泊定位地图区域，建立定位坐标系，UWB定位标签和IMU惯性测量单元安装于无人船上；UWB定位标签分别将所测量出的其与各UWB定位基站之间的距离信息发送至岸基控制中心内的定位处理器；利用IMU采集无人船的加速度及角速度信息，将无人船抑制振动噪声后的实时姿态信息发送至定位处理器；定位处理器根据距离信息和实时姿态信息实现无人船的精确定位解算和实时姿态信息更新，地图服务器在二维或三维地图中显示无人船在湖中的位置信息和实时姿态信息并进行展示。本发明定位成本低、精度高且受外界干扰小，具有实用推广价值。

Description

一种基于UWB技术高精度无人船定位系统、方法及存储介质
技术领域
本发明涉及无人船定位技术，具体涉及一种基于UWB技术高精度无人船定位系统、方法及存储介质。
背景技术
水面无人船是一个较为智能复杂的机器人系统，涉及到船体设计、智能导航、信号检测、人工智能、模式识别等相关领域的技术。目前，常用的定位技术主要有红外线定位、超声波定位、RFID近距离定位、WiFi定位、蓝牙定位等，但这些定位技术仍存在稳定性较弱、安全性较差、成本较高、定位精度不够等问题。目前因超宽带(UWB)具有脉冲间隔短、时间分辨率高可到达到厘米级别的测距精度，成为这些定位主流方案。超宽带(UWB)是一种无线电技术，在多个波段(3.1GHz-10.6GHz)上同时传输几百MHz的宽带(大于500GHz)。超宽带这一技术特点，在视距条件下，定位精度能到厘米级别；但当存在遮挡非视距(NLOS)时,其提供的定位精度会大大降低。UWB定位常采用最小二乘法解算，但缺点是会因为出现奇异矩阵而无法解算的情况。这些缺点，都会严重影响系统定位精度和稳定性。如何降低成本、提高定位精度是无人船湖泊定位中广泛应用亟需解决的关键问题。
在无人船航行过程中，IMU往往用来测量无人船的姿态角，在姿态角中，航向是最需关注的。但是，载体在运动过程中往往会产生各种冲击和振动，当IMU振动频率较高、振幅较大时，会导致IMU产生零偏漂移和非线性、非高斯随机噪声，造成测量精度大幅下降。因此，在振动环境下保持精度与稳定是IMU应用在无人船上的一个重要问题。
随着计算机运算力更快、智能化水平更高、网络通讯技术更稳定的发展，水面无人船不仅可以在军事防御领域发挥重要作用，而且在民用领域也取得了长足的发展，我国相关部门已关注到了高精度无人船定位这方面发展的重要意义，UWB定位技术对无人船的湖泊定位，拥有广阔的应用前景。
发明内容
发明目的：本发明的一个目的是提供一种基于UWB技术高精度无人船定位系统。
本发明的另一个目的是提供一种基于UWB技术高精度无人船定位方法。
技术方案：本发明的一种基于UWB技术的无人船定位系统，包括至少4个UWB定位基站、UWB定位标签、IMU惯性测量单元和岸基控制中心，其中，至少4个UWB定位基站分散布置于湖泊四周边缘，其中一个UWB定位基站设为主基站，其余的UWB定位基站为从基站，UWB定位标签和IMU惯性测量单元设置于无人船上，岸基控制中心包括地图服务器和定位处理器；UWB定位标签与UWB定位基站进行无线通信，分别测量其与各UWB定位基站之间的测距数据，并通过无线网发送至定位处理器；IMU惯性测量单元测量无人船的实时姿态信息，并进行噪声抑制得到实时姿态校正信息发送至定位处理器；定位处理器解析UWB定位标签与各UWB定位基站之间的测距数据，使用UWB定位方法得出无人船的三维定位，并根据无人船的实时姿态校正信息，实现无人船的精确定位解算和获取实时姿态信息；地图服务器在二维或三维地图中显示无人船在湖中的位置信息并进行展示。
优选的，多个UWB定位基站保持相同水平高度，垂直地面安装，且沿湖泊边缘均匀分布，以主基站为定位参考点，建立湖泊定位地图区域，进一步建立湖泊定位坐标系。
本发明的另一实施例中，一种基于UWB技术的无人船定位方法，包括以下步骤：
S1、沿定位湖泊边缘均匀布置4个及以上的UWB定位基站；其中一个设为UWB定位主基站，其余的为UWB定位从基站，以UWB定位主基站的中心作为定位坐标系的原点，UWB定位主基站和与其位于湖泊同一侧的距离最近的UWB定位从基站的连线作为X轴，在水平面内过原点垂直于X轴方向作为Y轴，过原点垂直于水平面竖直向上为Z轴，建立湖泊定位坐标系O-XYZ；将UWB定位标签和IMU惯性测量单元安装于无人船上。
S2、UWB定位标签与各UWB定位基站分别进行无线通信，UWB定位标签将所测量出的其与各UWB定位基站之间的测距数据通过无线网发送至岸基控制中心内的定位处理器；定位处理器解析UWB定位标签与各UWB定位基站之间的测距数据，使用UWB定位方法得出无人船的三维定位；包括：UWB定位标签通过双边双向法获取无人船与各UWB定位基站之间的测距数据；然后，采用K-means++算法抑制测距数据中的NLOS误差；再次，通过高斯均值滤波处理测距数据中的标准测量误差；利用最小二乘法对误差抑制后的测距数据作解算处理，确定无人船的首次三维定位初值；将无人船三维定位初值作为泰勒级数展开法的输入，确定无人船三维定位估计值，即为无人船最终的精确定位置，实现无人船三维定位；
S3、利用IMU惯性测量单元采集无人船的加速度及角速度信息，作为无人船的实时姿态信息，并使用滤波算法对无人船的实时姿态信息进行噪声抑制，将无人船抑制振动噪声后的实时姿态信息发送至岸基控制中心内的定位处理器；
S4、定位处理器根据步骤S2中得到的无人船的三维定位结果和步骤S3发送来的无人船实时姿态信息，实现无人船的精确定位和实时姿态信息更新；
S5、地图服务器在二维或三维地图中显示无人船在湖泊中的位置信息以及实时姿态信息并进行展示。
进一步的，步骤S2中UWB定位标签通过双边双向测距法获取无人船与各UWB定位基站之间的测距数据，具体为：
UWB定位标签向UWB定位基站发送UWB信号请求数据包，经过飞行时间T后，UWB定位基站接收到请求；UWB定位基站经过TA时间延迟后，向UWB定位标签回复应答数据包，UWB定位标签接收到应答后记录从发送请求到接收到应答的延迟时间Tr1，并经过TB时间延迟后向UWB定位基站再额外回复一个接收应答以及请求结束数据包，经过延迟时间Tr2后UWB定位基站接收到UWB定位标签的接收应答以及请求结束数据包，此时UWB信号传输过程结束；最终获取四个时间差数据TA、Tr1、TB、Tr2，UWB定位标签和UWB定位基站对时间戳进行标记，根据四个时间差可计算UWB信号的飞行时间T，如下：
Figure BDA0004211261960000031
确定UWB信号的飞行时间T后，通过dAB＝cT确定UWB定位标签和UWB定位基站之间的测距数据，其中c为光速；TA为UWB定位基站接收到UWB定位标签发送的UWB请求数据包后回复应答数据包的延迟时间，TB为UWB定位标签向UWB定位基站再额外回复一个接收应答以及请求结束数据包的处理和发送延迟时间；
采用双边双向测距法获得的UWB定位标签和UWB定位基站之间的测距数据中除NLOS误差外，还包括受随机因素影响产生的标准测量误差，该误差近似符合高斯分布，此时误差模型如下：
dAB(k)＝RAB(k)+nAB(k)+NAB(k)
其中，dAB(k)为k时刻UWB定位标签和UWB定位基站之间的测距数据，RAB(k)为k时刻UWB定位标签、UWB定位基站之间距离的实际值，nAB(k)为标准测量误差，NAB(k)为NLOS误差。
进一步的，步骤S2中采用K-means++算法抑制测距数据中的NLOS误差的具体方法为：
(1)采用DS-TWR方法对UWB定位标签、UWB定位基站之间距离进行数次测量，得到测距数据集合D＝{d1,d2,…dk,…dn}，dk为测距数据集合D中第k个测距数据，k＝1,2,…,n，n为测距数据集合D中测距数据的个数，从测距数据集合D中随机选取一个测距数据作为第一个初始聚类中心C1；
(2)计算测距数据集合D中各测距数据与第一个初始聚类中心C1的最小距离D(dk)，计算公式为：
D(dk)＝min{dk-C1}
之后确定测距数据集合D中各测距数据被选为第二个聚类中心C2的概率Pk，计算公式为：
Figure BDA0004211261960000041
(3)将随机数U∈[0,1]依次减去P1,P2,…Pn，取第一个差值小于等于零的测距数据视为第二个聚类中心C2；
(4)计算测距数据集合D中各测距数据分别到聚类中心C1与C2的距离，将各测距数据划分成两组，第一组为距离第一个初始聚类中心C1最近的测距数据，形成相应的第一子集；第二组为距离第二个聚类中心C2最近的测距数据，形成相应的第二子集；
(5)使用第一子集和第二子集的平均值λ1和λ2，分别作为两个新的聚类中心，再根据K-means++算法评价准则公式计算误差平方和E：
Figure BDA0004211261960000051
其中，当误差平方和E达到最小值时，两个新的聚类中心λ1和λ2之间的距离最远，而新的聚类中心λ1和λ2中的测距数据最紧凑；
(6)将两个新的聚类中心λ1和λ2分别更新为第一个初始聚类中心C1和第二个聚类中心C2，再反复执行步骤(4)、步骤(5)，当聚类中心满足收敛条件后结束；
(7)经过步骤(6)，将最终得到的两个新聚类中心分别记为A1，A2；当A1<A2时，则聚类中心A1对应的测距数据只含有标准量测误差的测距数据，若聚类中心A1与聚类中心A2的差值小于30cm，则认定聚类中心A2对应的测距数据中无NLOS误差，反之，则将A2对应的测距数据从D中删除，得到新的测距数据集合D'。
进一步的，步骤S2中通过高斯均值滤波处理标准测量误差，具体包括：
抑制NLOS误差后的新的测距数据集合D'中测距数据近似符合(μ,σ2)正态分布特性，密度函数通过下式进行描述：
Figure BDA0004211261960000052
Figure BDA0004211261960000053
Figure BDA0004211261960000054
其中，di为新的测距数据集合D'中第i个测距数据，m为新的测距数据集合D'中的数据量；μ为新的测距数据集合D'中各测距数据的平均值；σ为新的测距数据集合D'中各测距数据的标准差；
通过高斯滤波法对区间(μ-σ,μ+σ)外的测距数据作滤除后，再采用均值滤波法处理，最终得到高斯均值混合滤波输出值dfilter，其公式描述为：
Figure BDA0004211261960000061
其中，m1为新的测距数据集合D'经高斯滤波处理后的数据量。
进一步的，步骤S2中利用最小二乘法对误差抑制后的测距数据作解算处理，确定无人船的三维定位初值；具体为：
假设各UWB定位基站中心的高度所在平面为Z轴的零平面，第i个UWB定位基站的三维坐标表示为Pi＝(xi,yi,zi)，时间ti描述UWB定位标签发送UWB信号请求数据包到第i个UWB定位基站Pi的飞行时间，第i个UWB定位基站Pi到UWB定位标签间的距离通过下式获得：
Rdi＝c(ti-△t)
其中，△t为第i个UWB定位基站Pi与UWB定位标签时钟间的偏差；c为光速；另外，第i个UWB定位基站Pi与UWB定位标签P＝(x,y,z)的间距还能通过下式计算得到：
Figure BDA0004211261960000062
通过上面两个公式得到下式方程组：
Figure BDA0004211261960000063
进一步作变换处理得到下式：
Figure BDA0004211261960000064
转换成矩阵形式，得：
Figure BDA0004211261960000071
进一步改写为AH＝b形式，采用最小二乘法对上面矩阵公式作解析处理，得：
Figure BDA0004211261960000072
其中，H为最小二乘解；A为系数矩阵；向量b为常数；
获取H的前三维变量，即得到无人船三维定位初值，表示为(x0,y0,z0)。
进一步的，步骤S2中将无人船三维定位初值作为泰勒级数展开法的输入，确定无人船三维定位估计值，即为无人船最终的精确定位置，实现无人船三维定位；具体为：
确定无人船三维定位初值后，采用泰勒级数展开方法对其进行处理，以获取无人船定位估计结果；第1个UWB定位基站P1＝(x1,y1,z1)与UWB定位标签P＝(x,y,z)间的初始距离为Rd1，第i个UWB定位基站Pi＝(xi,yi,zi)至UWB定位标签间的初始距离Rdi与UWB定位基站P1＝(x1,y1,z1)到UWB定位标签P＝(x,y,z)的初始距离Rd1的差值通过下式进行计算：
Figure BDA0004211261960000073
将上式用f(x,y,z,xi,yi,zi)形式表示，采用泰勒级数展开方法对首次定位初值(x0,y0,z0)进行处理，消去二阶、三阶分量后，获得：
Figure BDA0004211261960000074
进一步转换为下式的误差矢量方程组形式：
Figure BDA0004211261960000075
其中，
Figure BDA0004211261960000076
为误差矢量；δ为待求向量；
Figure BDA0004211261960000077
矩阵h、G分别表示为：
Figure BDA0004211261960000081
三维定位初值至各UWB定位基站距离用Ri表示；计算误差矢量方程组公式的加权最小二乘解，得到：
Figure BDA0004211261960000082
其中，Q为加权矩阵，是DS-TWR测距量测值的误差协方差矩阵；确定δ后，设定x0'＝x0+△x，y0'＝y0+△y，z0'＝z0+△z，确定(x0',y0',z0')后，将其作为定位初值再次进行迭代循环，当满足条件|△x|2+|△y|2+|△z|2<ε2时，迭代停止，其中设定阈值表示为ε，迭代结束后再次获得(x0',y0',z0')，该定位坐标即为无人船最终的三维定位结果。
进一步的，步骤S3具体包括以下步骤：
S31、使用LMS自适应滤波算法，对加速度计初始值和陀螺仪输出初始值进行前端处理，算法如下：
Figure BDA0004211261960000083
其中，X(n)表示n时刻的加速度计和陀螺仪输入信号，W(n)表示n时刻滤波器的权重系数，d(n)表示n时刻滤波器的期望输出信号，e(n)表示n时刻加速度计和陀螺仪的误差，y(n)表示滤波器输出，μ(n)表示自适应滤波器的迭代步长；算法的收敛条件为：0<μ(n)<1/λmax，其中λmax为输入信号X(n)的自相关矩阵的最大特征值；
S32、通过加速度信息去滤除角速度的零偏噪声；滤波算法公式如下：
Figure BDA0004211261960000091
其中，gx，gy和gz分别为经过互补滤波后输出的三轴角速度值，mx，my和mz分别为经过LMS滤波后的三轴角速度值，ex，ey和ez表示三个轴向的误差，KP和KI为互补滤波器参数；
S33、采用EKF滤波算法进一步滤波从而提升整体去噪效果；
S34、IMU惯性测量单元测量出无人船欧拉角及姿态，并将所测量的实时姿态信息发送至终端处理器。
本发明的又一实施例中，一种存储介质，所述存储介质上存储有计算机程序，所述计算机程序被至少一个处理器执行时实现如上述一种基于UWB技术的无人船定位方法的步骤。
有益效果：与现有技术相比，本发明的显著技术效果为：
(1)本发明使用一种基于UWB高精度无人船定位方法，能够克服传统的TDOA定位的缺点，具有对测距数据误差进行处理，提高测距数据准确度，定位精度高，可靠性强的优点。同时具备受外界因素干扰小且价格低等优点，对无人船湖泊定位具有普适性。
(2)本发明针对无人船振动影响IMU惯性测量单元航向精度问题，能有效抑制振动噪声从而提升航向精度与稳定性的方法，利用姿态信息对UWB定位系统获取的位置信息进行校正，获取更加精确的姿态数据。
附图说明
图1是无人船定位系统示意图；
图2是本发明方法流程图。
具体实施方式
下面结合附图对本发明的实施例进行详细阐述，以使本发明的优点和特征能更易于被本领域技术人员理解，从而对本发明的保护范围做出更为清楚明确的界定。
下面结合实施例对本发明作进一步地详细说明，但本发明的实施方式不限于此。
如图1所示，本发明的一种基于UWB技术的无人船定位系统，包括分散设置于湖泊周围的至少4个UWB定位基站，设置于无人船上的UWB定位标签和IMU惯性测量单元，以及岸基控制中心，岸基控制中心包括地图服务器和定位处理器；UWB定位标签与UWB定位基站进行无线通信，分别测量其与各UWB定位基站之间的距离信息，并分别将所测量出的各UWB定位基站与UWB定位标签之间的距离信息通过无线网发送至岸基控制中心内的定位处理器；IMU惯性测量单元包括加速度计、陀螺仪，水平安装于无人船上，采取一种能有效抑制振动噪声从而提升航向精度与稳定性的方法来测量无人船的三维加速度和角速度姿态，以作为无人船的实时姿态信息，并将所采集的实时姿态信息发送至定位处理器；定位处理器解析UWB定位标签与各UWB定位基站之间的距离信息，使用UWB定位方法得出无人船的三维定位，并根据无人船抑制振动噪声后的姿态信息，实现无人船的精确定位解算和获取实时姿态信息；地图服务器在二维或三维地图中显示无人船在湖中的位置信息并进行展示。
如图1所示场景中，无人船位于湖泊中，多个UWB定位基站保持相同水平高度，垂直地面安装，且沿定位湖泊边缘分布，以多个UWB定位基站作为定位参考点，建立湖泊定位地图区域；并在湖泊定位地图区域建立湖泊定位坐标系，建立过程为：
沿定位湖泊边缘均匀布置4个及以上的UWB定位基站；其中一个设为UWB定位主基站，剩余的为UWB定位从基站，以UWB定位主基站的中心作为定位坐标系的原点，UWB定位主基站和与其位于湖泊同一侧的距离最近的UWB定位从基站的连线作为X轴，在水平面内过原点垂直于X轴方向作为Y轴，过原点垂直于水平面竖直向上为Z轴，建立湖泊定位坐标系O-XYZ。
本发明实施例中，无人船位于一片椭圆形湖泊中，湖泊周围边缘均匀分布设置有至少4个UWB定位基站，分别为一个UWB定位主基站4、第一UWB定位从基站1、第二UWB定位从基站2和第三UWB定位从基站3，UWB定位标签5附着于无人船6上，无人船6上还设置有IMU惯性测量单元，岸基指控中心7在远处接收数据，其内设置有地图服务器8和定位处理器9。具体的：
4个UWB定位基站保持相同水平高度，垂直地面安装，4个UWB定位基站沿定位湖泊边缘分布，分布设置于湖泊相对的两侧岸边。如图1所示，以UWB定位主基站4的中心为定位坐标系的原点O，以UWB定位主基站4和第三UWB定位从基站3的连线为X轴，第三UWB定位从基站3位于X轴正向，在水平面内过原点垂直于X轴方向作为Y轴，以过原点垂直于水平面竖直向上为Z轴，建立空间直角坐标系作为湖泊定位坐标系O-XYZ。
UWB定位标签5为磁吸型，可吸附于无人船6顶，用于与各个UWB定位基站进行无线通信；UWB定位标签5通过与各UWB定位基站进行无线通信互发数据包，将所测量出的与各UWB定位基站间的距离信息(测距数据)通过无线网发送至岸基控制中心7内的定位处理器9，定位处理器9通过无线通信网络线接收UWB定位标签发送来的测距数据，进行数据解析，清洗和处理定位数据，利用定位算法解算出UWB定位标签(即无人船)的三维位置坐标，同时定位处理器9接收IMU惯性测量单发送的实时姿态信息；IMU惯性测量单元包括加速度计和陀螺仪，水平安装于无人船上，采取一种能有效抑制振动噪声从而提升航向精度与稳定性的方法来测量无人船的三维加速度和角速度姿态，以作为所述无人船的实时姿态信息，并将所采集的实时姿态信息发送至定位处理器；地图服务器8用于实时向用户展示无人船6位置信息，可以在二维或三维地图进行展示。
本发明的一种基于UWB技术的无人船定位方法，包括：首先，在湖泊周边设置4个及以上的UWB定位基站，其中一个设为UWB定位主基站，剩余的为UWB定位从基站，以UWB定位主基站作为定位参考点，建立湖泊定位地图区域，进一步建立定位坐标系，UWB定位标签和IMU惯性测量单元安装于无人船上。其次，UWB定位标签与各UWB定位基站进行无线通信，分别将所测量出的各UWB定位基站与UWB定位标签之间的距离信息(测距数据)通过无线网发送至岸基控制中心内的定位处理器；定位处理器用于解析各UWB定位基站与UWB定位标签之间的距离信息，使用UWB定位方法得出无人船的三维定位，同时定位处理器接收IMU惯性测量单发送的实时姿态信息；然后，利用IMU采集无人船的加速度及角速度信息，将无人船抑制振动噪声后的实时姿态信息发送至岸基控制中心内的定位处理器；再次，定位处理器实现无人船的精确定位解算和实时姿态信息更新；最后，地图服务器在二维或三维地图中显示无人船在湖泊中的位置信息以及实时姿态信息并进行展示。本发明定位成本低、精度高且受外界干扰小，可有效降低测距数据误差，可确定各定位点的位置坐标，可将障碍环境下的定位误差控制在最小范围之内，适用于各种智能无人船的湖泊定位，具有实用推广价值。如图2所示，具体包括以下步骤：
S1、安装无人船定位系统，建立湖泊定位地图区域，并建立湖泊定位坐标系O-XYZ；具体为：
S11、沿定位湖泊边缘均匀布置4个及以上的UWB定位基站，其中一个设为UWB定位主基站，剩余的为UWB定位从基站；
S12、以UWB定位主基站作为定位参考点，建立湖泊定位地图区域，并在湖泊定位地图区域建立湖泊定位坐标系，建立的湖泊定位地图区域如图1中椭圆所示区域，湖泊定位坐标系为O-XYZ；
S13、将UWB定位标签和IMU惯性测量单元安装于无人船上。
S2、UWB定位标签与各UWB定位基站分别进行无线通信，UWB定位标签将所测量出的其与各UWB定位基站之间的距离信息(测距数据)通过无线网发送至岸基控制中心内的定位处理器；定位处理器解析UWB定位标签与各UWB定位基站之间的测距数据，使用UWB定位方法得出无人船的三维定位。UWB定位方法具体步骤为：
S21、首先，UWB定位标签通过双边双向法获取无人船与各UWB定位基站之间的测距数据，测距数据中同时分布有标准量测误差和NLOS误差。
UWB定位标签通常利用双边双向测距法(DS-TWR)对无人船进行定位测距，UWB定位标签向UWB定位基站发送UWB信号请求数据包，经过飞行时间T后，到达UWB定位基站处，由其接收，UWB定位基站接收到请求后回复应答数据包，并记录该段延迟时间为TA，UWB定位标签收到应答后记录从发送请求到接收回复信号的延迟时间Tr1，并向UWB定位基站再额外回复一个接收应答以及请求结束数据包，该段处理和发送时间延迟时间TB，UWB定位基站收到UWB定位标签的接收应答以及请求结束后记录该段发送到接收的延迟时间Tr2，此时信号传输过程结束；最终获取四个时间差数据TA，Tr1，TB，Tr2，UWB定位标签、UWB定位基站对时间戳进行标记，根据四个时间差可计算UWB信号的飞行时间T，如下：
Figure BDA0004211261960000121
式(1)可由下式推导得出：
Figure BDA0004211261960000122
确定UWB信号的飞行时间T后，通过dAB＝cT确定UWB定位标签和UWB定位基站之间的测距数据，其中c为光速；TA为UWB定位基站接收到定位标签发送的UWB信号请求数据包后回复应答数据包的延迟时间，TB为UWB定位标签向UWB定位基站再额外回复一个接收应答以及请求结束数据包的处理和发送延迟时间。UWB定位标签、UWB定位基站的时钟精度需相同，以降低测距误差。在采用DS-TWR对无人船进行测距过程中，发射信号会因受到障碍物的影响，而使信号传输产生延迟，导致测距数据中包含较大非视距(NLOS)误差，其值总为正。除此之外，还包含有随机因素干扰造成的标准测量误差，且服从高斯分布。采用DS-TWR获得的测距数据通过下式进行描述：
dAB(k)＝RAB(k)+nAB(k)+NAB(k) (3)
其中，dAB(k)为k时刻UWB定位标签和UWB定位基站之间的测距数据，RAB(k)为k时刻UWB定位标签和UWB定位基站之间距离的实际值，nAB(k)为标准测量误差，视其为零均值高斯变量；NAB(k)为NLOS误差，视为正随机变量，具有均匀分布特性。
S22、然后，采用K-means++算法抑制测距数据中的NLOS误差。
(1)采用DS-TWR方法对UWB定位标签、UWB定位基站之间距离进行数次测量，得到测距数据集合D＝{d1,d2,...dk,...dn}，dk为测距数据集合D中第k个测距数据，k＝1,2,…,n，n为测距数据集合D中测距数据的个数，从测距数据集合D中随机选取一个测距数据作为第一个初始聚类中心C1；
(2)计算测距数据集合D中各测距数据与第一个初始聚类中心C1的最小距离D(dk)，计算公式为：
D(dk)＝min{|dk-C1|} (4)
之后确定测距数据集合D中各测距数据被选为第二个聚类中心C2的概率Pk，计算公式为：
Figure BDA0004211261960000131
(3)将随机数U∈[0,1]依次减去P1,P2,…Pn，取第一个差值小于等于零的测距数据视为第二个聚类中心C2；
(4)计算测距数据集合D中各测距数据分别到聚类中心C1与C2的距离，将各测距数据划分成两组，第一组为距离第一个初始聚类中心C1最近的测距数据，形成相应的第一子集；第二组为距离第二个聚类中心C2最近的测距数据，形成相应的第二子集；
(5)使用第一子集和第二子集的平均值λ1和λ2，分别作为两个新的聚类中心，再根据K-means++算法评价准则公式计算误差平方和E：
Figure BDA0004211261960000141
其中，当误差平方和E达到最小值时，两个新的聚类中心λ1和λ2之间的距离最远，而新的聚类中心λ1和λ2中的测距数据最紧凑；
(6)将两个新的聚类中心λ1和λ2分别更新为第一个初始聚类中心C1和第二个聚类中心C2，再反复执行步骤(4)、步骤(5)，当聚类中心满足收敛条件后结束；
(7)经过步骤(6)，将最终得到的两个新聚类中心分别记为A1，A2；当A1<A2时，则聚类中心A1对应的测距数据只含有标准量测误差的测距数据，若聚类中心A1与聚类中心A2的差值小于30cm，则认定聚类中心A2对应的测距数据中无NLOS误差，反之，则将A2对应的测距数据从D中删除，得到新的测距数据集合D'。
S23、再次，通过高斯均值滤波处理标准测量误差，其过程包括：
抑制NLOS误差后的新的测距数据集合D'中测距数据近似符合(μ,σ2)正态分布特性，密度函数通过下式进行描述：
Figure BDA0004211261960000142
Figure BDA0004211261960000143
Figure BDA0004211261960000151
其中，di为新的测距数据集合D'中第i个测距数据，m为新的测距数据集合D'中的数据量；μ为新的测距数据集合D'中各测距数据的平均值；σ为新的测距数据集合D'中各测距数据的标准差。
通过高斯滤波法对区间(μ-σ,μ+σ)外的测距数据作滤除后，再采用均值滤波法处理，最终得到高斯均值混合滤波输出值dfilter，其公式描述为：
Figure BDA0004211261960000152
其中，m1为新的测距数据集合D'经高斯滤波处理后的数据量。
S24、利用最小二乘法对误差抑制后的测距数据作解算处理，确定无人船的三维定位初值；
假设各UWB定位基站中心的高度所在平面为Z轴的零平面，第i个UWB定位基站的三维坐标表示为Pi＝(xi,yi,zi)，用时间ti描述UWB定位标签发送UWB信号请求数据包到第i个UWB定位基站Pi的飞行时间，第i个UWB定位基站Pi到UWB定位标签间的距离通过下式获得：
Rdi＝c(ti-△t) (11)
其中，△t为第i个UWB定位基站Pi与UWB定位标签时钟间的偏差；c为光速；另外，第i个UWB定位基站Pi与UWB定位标签P＝(x,y,z)的间距还能通过下式计算得到：
Figure BDA0004211261960000153
通过式(11)、(12)可得到下式方程组：
Figure BDA0004211261960000161
对式(13)作变换处理可得到下式：
Figure BDA0004211261960000162
将式(14)转换成矩阵形式，可得：
Figure BDA0004211261960000163
式(15)可改写为AX＝b形式，采用最小二乘法对公式(15)作解析处理，可得：
Figure BDA0004211261960000164
其中，好、H为最小二乘解；A为系数矩阵；向量b为常数。
获取H的前三维变量，即可得到无人船三维定位初值，表示为(x0,y0,z0)。
S25、将无人船三维定位初值作为泰勒级数展开法的输入，确定无人船三维定位估计值，即为无人船最终的精确定位置，实现无人船三维定位。
确定无人船6首次定位初值后，采用泰勒级数展开方法对其进行处理，以获取无人船定位估计结果。由公式(12)可确定第1个UWB定位基站P1＝(x1,y1,z1)与UWB定位标签P＝(x,y,z)间的初始距离为Rd1，第i个UWB定位基站Pi＝(xi,yi,zi)至UWB定位标签间的初始距离Rdi与第1个UWB定位基站P1＝(x1,y1,z1)到UWB定位标签初始距离Rd1的差值通过下式进行计算：
Figure BDA0004211261960000171
将式(17)用f(x,y,z,xi,yi,zi)形式表示，采用泰勒级数展开方法对首次定位初值(x0,y0,z0)进行处理，消去二阶、三阶分量后，可获得：
Figure BDA0004211261960000172
将式(18)转换为下式的误差矢量方程组形式：
Figure BDA0004211261960000173
其中，
Figure BDA0004211261960000174
为误差矢量；δ为待求向量；
Figure BDA0004211261960000175
矩阵h、G分别表示为：
Figure BDA0004211261960000176
三维定位初值至各UWB定位基站距离用Ri表示，i＝1,…N，x0,x1,x2,x3,...xN分别表示各UWB定位基站的X轴坐标值，y0,y1,y2,y3,…yN分别表示各UWB定位基站的Y轴坐标值，z0,z1,z2,z3,…zN分别表示各UWB定位基站的Z轴坐标值，R21、R31、RN1分别代表第2、3、N个UWB定位基站至UWB定位标签的距离与第1个UWB定位基站至UWB定位标签的距离的差值。计算公式(19)的加权最小二乘解，得到：
Figure BDA0004211261960000177
其中，Q为加权矩阵，是DS-TWR测距量测值的误差协方差矩阵。确定δ后，设定x0'＝x0+△x，y0'＝y0+△y，z0'＝z0+△z，确定(x0',y0',z0')后，将其作为定位初值再次进行迭代循环，当满足条件|△x|2+|△y|2+|△z|2<ε2时，迭代停止，其中设定阈值表示为ε，迭代结束后再次获得(x0',y0',z0')，该定位坐标即为无人船6最终的三维定位结果。
S3、利用IMU惯性测量单元采集无人船的加速度及角速度信息，将无人船抑制振动噪声后的实时姿态信息发送至岸基控制中心内的定位处理器。具体为：
使用一种有效抑制振动噪声从而提升航向精度与稳定性的方法，步骤如下：
S31、首先，使用LMS自适应滤波算法，对加速度计初始值和陀螺仪输出初始值进行进行前端处理，算法如下：
Figure BDA0004211261960000181
其中，X(n)表示n时刻的加速度计和陀螺仪输入信号，W(n)表示n时刻滤波器的权重系数，d(n)表示n时刻滤波器的期望输出信号，e(n)表示n时刻加速度计和陀螺仪的误差，y(n)表示滤波器输出，，μ(n)表示自适应滤波器的迭代步长。算法的收敛条件为：0<μ(n)<1/λmax，其中λmax为输入信号X(n)的自相关矩阵的最大特征值。
S32、加速度计在静止时相较于陀螺仪更加稳定，然后通过加速度信息去滤除角速度的零偏噪声。滤波算法公式如下：
Figure BDA0004211261960000182
其中，gx，gy和gz分别为经过互补滤波后输出的三轴角速度值，mx，my和mz分别为经过LMS滤波后的三轴角速度值，ex，ey和ez表示三个轴向的误差，KP和KI为互补滤波器参数。
Figure BDA0004211261960000191
其中，
Figure BDA0004211261960000192
为姿态转换矩阵，Vx，Vy和Vz分别为归一化后的三轴加速度。
S33、再次，采用EKF滤波算法进一步滤波从而提升整体去噪效果。
S34、最后，IMU惯性测量单元测量出无人船欧拉角及姿态，并将所测量的实时姿态信息发送至定位处理器；
S4、定位处理器实现无人船的精确定位解算和实时姿态信息更新。
定位处理器解析来自UWB定位基站与UWB定位标签之间的距离信息，使用UWB定位方法得出无人船的三维定位结果，接受来自IMU惯性测量单元的数据，并更新无人船实时姿态信息，最终实现无人船的精确定位和实时姿态信息更新。
S5、地图服务器在二维或三维地图中显示无人船在湖中的位置信息以及实时姿态信息并进行展示。
地图服务器实时向用户展示在二维或三维地图中显示无人船在湖中的位置信息。
本发明另一实施例中，一种存储介质，所述存储介质上存储有计算机程序，所述计算机程序被至少一个处理器执行时实现如上述一种基于UWB技术的无人船定位方法的步骤，并能达到与上述方法一致的技术效果。

Claims (10)

1.一种基于UWB技术的无人船定位系统，其特征在于，包括至少4个UWB定位基站、UWB定位标签、IMU惯性测量单元和岸基控制中心，其中，至少4个UWB定位基站分散布置于湖泊四周边缘，其中一个UWB定位基站设为主基站，其余的UWB定位基站为从基站，UWB定位标签和IMU惯性测量单元设置于无人船上，岸基控制中心包括地图服务器和定位处理器；UWB定位标签与UWB定位基站进行无线通信，分别测量其与各UWB定位基站之间的测距数据，并通过无线网发送至定位处理器；IMU惯性测量单元测量无人船的实时姿态信息，并进行噪声抑制得到实时姿态校正信息发送至定位处理器；定位处理器解析UWB定位标签与各UWB定位基站之间的测距数据，使用UWB定位方法得出无人船的三维定位，并根据无人船的实时姿态校正信息，实现无人船的精确定位解算和获取实时姿态信息；地图服务器在二维或三维地图中显示无人船在湖中的位置信息并进行展示。
2.根据权利要求1所述的一种基于UWB技术的无人船定位系统，其特征在于，多个UWB定位基站保持相同水平高度，垂直地面安装，且沿湖泊边缘均匀分布，以主基站为定位参考点，建立湖泊定位地图区域，进一步建立湖泊定位坐标系。
3.一种基于UWB技术的无人船定位方法，其特征在于，包括以下步骤：
S1、沿定位湖泊边缘均匀布置4个及以上的UWB定位基站；其中一个设为UWB定位主基站，其余的为UWB定位从基站，以UWB定位主基站的中心作为定位坐标系的原点，UWB定位主基站和与其位于湖泊同一侧的距离最近的UWB定位从基站的连线作为X轴，在水平面内过原点垂直于X轴方向作为Y轴，过原点垂直于水平面竖直向上为Z轴，建立湖泊定位坐标系O-XYZ；将UWB定位标签和IMU惯性测量单元安装于无人船上。
S2、UWB定位标签与各UWB定位基站分别进行无线通信，UWB定位标签将所测量出的其与各UWB定位基站之间的测距数据通过无线网发送至岸基控制中心内的定位处理器；定位处理器解析UWB定位标签与各UWB定位基站之间的测距数据，使用UWB定位方法得出无人船的三维定位；包括：UWB定位标签通过双边双向法获取无人船与各UWB定位基站之间的测距数据；然后，采用K-means++算法抑制测距数据中的NLOS误差；再次，通过高斯均值滤波处理测距数据中的标准测量误差；利用最小二乘法对误差抑制后的测距数据作解算处理，确定无人船的首次三维定位初值；将无人船三维定位初值作为泰勒级数展开法的输入，确定无人船三维定位估计值，即为无人船最终的精确定位置，实现无人船三维定位；
S3、利用IMU惯性测量单元采集无人船的加速度及角速度信息，作为无人船的实时姿态信息，并使用滤波算法对无人船的实时姿态信息进行噪声抑制，将无人船抑制振动噪声后的实时姿态信息发送至岸基控制中心内的定位处理器；
S4、定位处理器根据步骤S2中得到的无人船的三维定位和步骤S3发送来的实时姿态信息，实现无人船的精确定位和实时姿态信息更新；
S5、地图服务器在二维或三维地图中显示无人船在湖泊中的位置信息以及实时姿态信息并进行展示。
4.根据权利要求3所述的一种基于UWB技术的无人船定位方法，其特征在于，步骤S2中UWB定位标签通过双边双向测距法获取无人船与各UWB定位基站之间的测距数据，具体为：
UWB定位标签向UWB定位基站发送UWB信号请求数据包，经过飞行时间T后，UWB定位基站接收到请求；UWB定位基站经过TA时间延迟后，向UWB定位标签回复应答数据包，UWB定位标签接收到应答后记录从发送请求到接收到应答的延迟时间Tr1，并经过TB时间延迟后向UWB定位基站再额外回复一个接收应答以及请求结束数据包，经过延迟时间Tr2后UWB定位基站接收到UWB定位标签的接收应答以及请求结束数据包，此时UWB信号传输过程结束；最终获取四个时间差数据TA、Tr1、TB、Tr2，UWB定位标签和UWB定位基站对时间戳进行标记，根据四个时间差可计算UWB信号的飞行时间T，如下：
Figure FDA0004211261950000021
确定UWB信号的飞行时间T后，通过dAB＝cT确定UWB定位标签和UWB定位基站之间的测距数据，其中c为光速；TA为UWB定位基站接收到UWB定位标签发送的UWB请求数据包后回复应答数据包的延迟时间，TB为UWB定位标签向UWB定位基站再额外回复一个接收应答以及请求结束数据包的处理和发送延迟时间；
采用双边双向测距法获得的UWB定位标签和UWB定位基站之间的测距数据中除NLOS误差外，还包括受随机因素影响产生的标准测量误差，该误差近似符合高斯分布，此时误差模型如下：
dAB(k)＝RAB(k)+nAB(k)+NAB(k)
其中，dAB(k)为k时刻UWB定位标签和UWB定位基站之间的测距数据，RAB(k)为k时刻UWB定位标签、UWB定位基站之间距离的实际值，nAB(k)为标准测量误差，NAB(k)为NLOS误差。
5.根据权利要求3所述的一种基于UWB技术的无人船定位方法，其特征在于，步骤S2中采用K-means++算法抑制测距数据中的NLOS误差的具体方法为：
(1)采用DS-TWR方法对UWB定位标签、UWB定位基站之间距离进行数次测量，得到测距数据集合D＝{d1,d2,...dk,...dn}，dk为测距数据集合D中第k个测距数据，k＝1,2,…,n，n为测距数据集合D中测距数据的个数，从测距数据集合D中随机选取一个测距数据作为第一个初始聚类中心C1；
(2)计算测距数据集合D中各测距数据与第一个初始聚类中心C1的最小距离D(dk)，计算公式为：
D(dk)＝min{|dk-C1|}
之后确定测距数据集合D中各测距数据被选为第二个聚类中心C2的概率Pk，计算公式为：
Figure FDA0004211261950000031
(3)将随机数U∈[0,1]依次减去P1,P2,…Pn，取第一个差值小于等于零的测距数据视为第二个聚类中心C2；
(4)计算测距数据集合D中各测距数据分别到聚类中心C1与C2的距离，将各测距数据划分成两组，第一组为距离第一个初始聚类中心C1最近的测距数据，形成相应的第一子集；第二组为距离第二个聚类中心C2最近的测距数据，形成相应的第二子集；
(5)使用第一子集和第二子集的平均值λ1和λ2，分别作为两个新的聚类中心，再根据K-means++算法评价准则公式计算误差平方和E：
Figure FDA0004211261950000041
其中，当误差平方和E达到最小值时，两个新的聚类中心λ1和λ2之间的距离最远，而新的聚类中心λ1和λ2中的测距数据最紧凑；
(6)将两个新的聚类中心λ1和λ2分别更新为第一个初始聚类中心C1和第二个聚类中心C2，再反复执行步骤(4)、步骤(5)，当聚类中心满足收敛条件后结束；
(7)经过步骤(6)，将最终得到的两个新聚类中心分别记为A1，A2；当A1<A2时，则聚类中心A1对应的测距数据只含有标准量测误差的测距数据，若聚类中心A1与聚类中心A2的差值小于30cm，则认定聚类中心A2对应的测距数据中无NLOS误差，反之，则将A2对应的测距数据从D中删除，得到新的测距数据集合D'。
6.根据权利要求3所述的一种基于UWB技术的无人船定位方法，其特征在于，步骤S2中通过高斯均值滤波处理标准测量误差，具体包括：
抑制NLOS误差后的新的测距数据集合D'中测距数据近似符合(μ,σ2)正态分布特性，密度函数通过下式进行描述：
Figure FDA0004211261950000042
Figure FDA0004211261950000043
Figure FDA0004211261950000044
其中，di为新的测距数据集合D'中第i个测距数据，m为新的测距数据集合D'中的数据量；μ为新的测距数据集合D'中各测距数据的平均值；σ为新的测距数据集合D'中各测距数据的标准差；
通过高斯滤波法对区间(μ-σ,μ+σ)外的测距数据作滤除后，再采用均值滤波法处理，最终得到高斯均值混合滤波输出值dfilter，其公式描述为：
Figure FDA0004211261950000051
其中，m1为新的测距数据集合D'经高斯滤波处理后的数据量。
7.根据权利要求3所述的一种基于UWB技术的无人船定位方法，其特征在于，步骤S2中利用最小二乘法对误差抑制后的测距数据作解算处理，确定无人船的三维定位初值；具体为：
假设各UWB定位基站中心的高度所在平面为Z轴的零平面，第i个UWB定位基站的三维坐标表示为Pi＝(xi,yi,zi)，时间ti描述UWB定位标签发送UWB信号请求数据包到第i个UWB定位基站Pi的飞行时间，第i个UWB定位基站Pi到UWB定位标签间的距离通过下式获得：
Rdi＝c(ti-△t)
其中，△t为第i个UWB定位基站Pi与UWB定位标签时钟间的偏差；c为光速；另外，第i个UWB定位基站Pi与UWB定位标签P＝(x,y,z)的间距还能通过下式计算得到：
Figure FDA0004211261950000052
通过上面两个公式得到下式方程组：
Figure FDA0004211261950000061
进一步作变换处理得到下式：
Figure FDA0004211261950000062
转换成矩阵形式，得：
Figure FDA0004211261950000063
进一步改写为AH＝b形式，采用最小二乘法对上面矩阵公式作解析处理，得：
Figure FDA0004211261950000064
其中，H为最小二乘解；A为系数矩阵；向量b为常数；
获取H的前三维变量，即得到无人船三维定位初值，表示为(x0,y0,z0)。
8.根据权利要求3所述的一种基于UWB技术的无人船定位方法，其特征在于，步骤S2中将无人船三维定位初值作为泰勒级数展开法的输入，确定无人船三维定位估计值，即为无人船最终的精确定位置，实现无人船三维定位；具体为：
确定无人船三维定位初值后，采用泰勒级数展开方法对其进行处理，以获取无人船定位估计结果；第1个UWB定位基站P1＝(x1,y1,z1)与UWB定位标签P＝(x,y,z)间的初始距离为Rd1，第i个UWB定位基站Pi＝(xi,yi,zi)至UWB定位标签间的初始距离Rdi与UWB定位基站P1＝(x1,y1,z1)到UWB定位标签P＝(x,y,z)的初始距离Rd1的差值通过下式进行计算：
Figure FDA0004211261950000071
将上式用f(x,y,z,xi,yi,zi)形式表示，采用泰勒级数展开方法对首次定位初值(x0,y0,z0)进行处理，消去二阶、三阶分量后，获得：
Figure FDA0004211261950000072
进一步转换为下式的误差矢量方程组形式：
Figure FDA0004211261950000073
其中，
Figure FDA0004211261950000074
为误差矢量；δ为待求向量；
Figure FDA0004211261950000075
矩阵h、G分别表示为：
Figure FDA0004211261950000076
三维定位初值至各UWB定位基站距离用Ri表示；计算误差矢量方程组公式的加权最小二乘解，得到：
Figure FDA0004211261950000077
其中，Q为加权矩阵，是DS-TWR测距量测值的误差协方差矩阵；确定δ后，设定x0'＝x0+△x，y0'＝y0+△y，z0'＝z0+△z，确定(x0',y0',z0')后，将其作为定位初值再次进行迭代循环，当满足条件|△x|2+|△y|2+|△z|2<ε2时，迭代停止，其中设定阈值表示为ε，迭代结束后再次获得(x0',y0',z0')，该定位坐标即为无人船最终的三维定位结果。
9.根据权利要求3所述的一种基于UWB技术的无人船定位方法，其特征在于，步骤S3具体包括以下步骤：
S31、使用LMS自适应滤波算法，对加速度计初始值和陀螺仪输出初始值进行前端处理，算法如下：
Figure FDA0004211261950000081
其中，X(n)表示n时刻的加速度计和陀螺仪输入信号，W(n)表示n时刻滤波器的权重系数，d(n)表示n时刻滤波器的期望输出信号，e(n)表示n时刻加速度计和陀螺仪的误差，y(n)表示滤波器输出，μ(n)表示自适应滤波器的迭代步长；算法的收敛条件为：0<μ(n)<1/λmax，其中λmax为输入信号X(n)的自相关矩阵的最大特征值；
S32、通过加速度信息去滤除角速度的零偏噪声；滤波算法公式如下：
Figure FDA0004211261950000082
其中，gx，gy和gz分别为经过互补滤波后输出的三轴角速度值，mx，my和mz分别为经过LMS滤波后的三轴角速度值，ex，ey和ez表示三个轴向的误差，KP和KI为互补滤波器参数；
S33、采用EKF滤波算法进一步滤波从而提升整体去噪效果；
S34、IMU惯性测量单元测量出无人船欧拉角及姿态，并将所测量的实时姿态信息发送至终端处理器。
10.一种存储介质，其特征在于，所述存储介质上存储有计算机程序，所述计算机程序被至少一个处理器执行时实现如权利要求3-9任一项所述一种基于UWB技术的无人船定位方法的步骤。
CN202310493969.6A 2023-04-28 2023-04-28 一种基于uwb技术高精度无人船定位系统、方法及存储介质 Pending CN116430310A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310493969.6A CN116430310A (zh) 2023-04-28 2023-04-28 一种基于uwb技术高精度无人船定位系统、方法及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310493969.6A CN116430310A (zh) 2023-04-28 2023-04-28 一种基于uwb技术高精度无人船定位系统、方法及存储介质

Publications (1)

Publication Number Publication Date
CN116430310A true CN116430310A (zh) 2023-07-14

Family

ID=87085460

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310493969.6A Pending CN116430310A (zh) 2023-04-28 2023-04-28 一种基于uwb技术高精度无人船定位系统、方法及存储介质

Country Status (1)

Country Link
CN (1) CN116430310A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116699665A (zh) * 2023-08-08 2023-09-05 山东科技大学 一种适用于海上光伏电厂环境的无人船定位系统及方法
CN118295428A (zh) * 2024-06-06 2024-07-05 青岛儒海船舶工程有限公司 一种用于无人驾驶自动投饲船的控制系统及控制组件

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116699665A (zh) * 2023-08-08 2023-09-05 山东科技大学 一种适用于海上光伏电厂环境的无人船定位系统及方法
CN118295428A (zh) * 2024-06-06 2024-07-05 青岛儒海船舶工程有限公司 一种用于无人驾驶自动投饲船的控制系统及控制组件

Similar Documents

Publication Publication Date Title
CN110375730B (zh) 基于imu和uwb融合的室内定位导航系统
CN116430310A (zh) 一种基于uwb技术高精度无人船定位系统、方法及存储介质
CN107817469B (zh) 基于非视距环境下超宽频测距实现室内定位方法
CN107544067B (zh) 一种基于高斯混合近似的高超声速再入飞行器跟踪方法
CN108225258A (zh) 基于惯性单元和激光跟踪仪动态位姿测量装置和方法
CN107063245B (zh) 一种基于5阶ssrckf的sins/dvl组合导航滤波方法
CN112967392A (zh) 一种基于多传感器触合的大规模园区建图定位方法
CN114509069B (zh) 基于蓝牙aoa和imu融合的室内导航定位系统
CN110646783B (zh) 一种水下航行器的水下信标定位方法
CN110749891A (zh) 一种可估计未知有效声速的自适应水下单信标定位方法
CN108387236A (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN113311398B (zh) 强杂波复杂背景高机动弱小目标的跟踪方法
CN115046540A (zh) 一种点云地图构建方法、系统、设备和存储介质
CN110209180A (zh) 一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法
Zhang et al. A dynamic window-based UWB-odometer fusion approach for indoor positioning
CN110703205B (zh) 基于自适应无迹卡尔曼滤波的超短基线定位方法
CN109599674A (zh) 一种基于解耦的相控阵天线稳定角跟踪方法
CN109917373A (zh) 运动补偿搜索的动平台雷达的动态规划检测前跟踪方法
CN117932894A (zh) 一种多模导引头信息融合目标状态估计方法
Yi et al. A UWB location algorithm---Based on adaptive Kalman filter
CN108761384A (zh) 一种抗差的传感器网络目标定位方法
CN111708008A (zh) 一种基于imu和tof的水下机器人单信标导航方法
Liao et al. An integrated navigation method based on an adaptive federal Kalman filter for a rice transplanter
CN118050707A (zh) 激光雷达的标定方法及装置、存储介质、终端设备
CN108226863A (zh) 一种单目卫星跟踪定位方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination