CN111289944B - 一种基于uwb定位的无人艇位置航向测定方法 - Google Patents

Abstract

本发明公开了一种基于UWB定位的无人艇位置航向测定方法，现有技术无法实现无人艇定位与航向测定的同期同时测量，而且也很难兼顾定位的测量范围与测量精度。本发明如下：1、建立定位基站坐标系；2、开启第一定位基站、第二定位基站、第三定位基站、船头定位主机和船尾定位主机。3、获取六个距离时间序列。4、对步骤3所得的六个距离时间序列值进行混合滤波。5、计算船头定位主机坐标(x_h,y_h)和船尾定位主机坐标(x_t,y_t)。6、计算无人艇的航向数据θ。本发明涉及的基于无人艇位置测量与航向测量系统采用了船坞与无人艇之间坐标系变换方法与公式，在无人艇中心位置的基础上实时计算无人艇的航向数据。

Description

一种基于UWB定位的无人艇位置航向测定方法

技术领域

本发明属于基于无线通信的海面定位技术领域，具体涉及一种基于长距离UWB测距技术的无人艇定位与航向测定的方法。

背景技术

无人艇作为海上智能体的代表，以其自主能动性和智能性等特点广泛应用于军事侦查、科研考察、环境监测等方面，无人艇可替代人力去完成海上复杂而又危险的任务。相比其他舰艇，无人艇具有小型、机动灵活、易潜匿等特点，可以有效打击目标，掩护船只撤离，特定范围巡逻巡视，侦查监视敌军动向。面对海上恶劣环境的救援场景，中大型船只行动有所不便，无人艇可以到达大船无法靠近的救援区域，依靠自身携带的救援设备快速对目标进行救援。无人艇定位技术主要分为卫星定位、惯性导航定位、超声波定位和激光定位技术等。卫星定位技术是一种使用多个轨道卫星对移动设备准确定位的技术，但是定位精度在米数量级附近。惯性导航定位技术是利用敏感元件测量载体在惯性空间的线运动和角运动参数，根据已知的初始条件，通过牛顿力学等原理融合成定位数据的技术，但是这种方法存在着严重的数据偏移问题。超声波定位是一种利用超声波的物理特性进行测距的定位方式，但是无线通信距离较近，不适合范围较大的海面场景。激光雷达定位技术是一种利用激光穿透性和反射性来实现对远端物体定位的技术，精度虽然高但是不能有遮挡，非视距环境下无法使用。除此之外，卫星定位、惯性导航定位、超声波定位和激光定位技术等四种方法中也只有惯性导航定位方法能测量无人艇的航向，其余几种都无法实时测得无人艇的航向数据。

表1.无人艇6定位技术比较

上述方法能够在一定范围下满足无人艇的定位需求，但是无法实现无人艇定位与航向测定的同期同时测量，而且也很难兼顾定位的测量范围与测量精度。经检索，2016年8月 24日公开的公开号CN105898698A为的发明专利“一种基于UWB的船舶停泊辅助方法和系统”，虽然其采用了利用UWB超宽带定位技术实现船舶靠泊辅助方法和系统，但是只提出了获取码头和船舶的轮廓、船舶到码头的距离、角度和相对速度，但是无法根据此公开的发明专利清晰获取如何获取船舶的航向计算公式，只是笼统地提出了一个定位模型，而且也没有提出针对海面波浪环境的数据处理方法。本发明创新地利用了长距离UWB测距方式实现无人艇的位置测量及航向测量，提出了一种用于无人艇6航向测量的方法以及针对该环境的数据滤波处理方法，实现了无人艇相对于船坞坐标系位置的精确测量，同时还可以实现获取无人艇的实时航向数据。

发明内容

本发明的目的在于提供一种基于UWB定位的无人艇航向测定方法。

本发明一种基于UWB定位的无人艇位置航向测定方法，包括如下步骤：

步骤1、在船坞上设置第一定位基站、第二定位基站和第三定位基站；在无人艇的头端和尾端分别设置船头定位主机和船尾定位主机。各定位主机和各定位基站均采用UWB测距模块。建立定位基站坐标系，第一定位基站、第二定位基站、第三定位基站的坐标分别为(x₁,y₁)，(x₂,y₂)，(x₃,y₃)；船头定位主机、船尾定位主的坐标分别为(x_h,y_h)，(x_t,y_t)。

步骤2、开启第一定位基站、第二定位基站、第三定位基站、船头定位主机和船尾定位主机。

步骤3、船头定位主机分别测得自身与第一定位基站、第二定位基站、第三定位基站之间距离d₁₁，d₁₂，d₁₃的时间序列值，船尾定位主机分别测得自身与第一定位基站、第二定位基站、第三定位基站之间距离d₂₁，d₂₂，d₂₃的时间序列值。同一时刻测到的d₁₁，d₁₂， d₁₃，d₂₁，d₂₂，d₂₃为一个距离数组。

步骤4、对步骤3所得的六个距离时间序列值进行混合滤波。

4-1.用窗口宽度为N的滑动窗口在六个时间序列值上滑动；每次滑动后均对滑动窗口框选的N个距离数组进行中值滤波；通过滑动窗口的持续滑动形成经过中值滤波后的六个时间序列值。

中值滤波的过程如下：

①.将距离数组内的六个距离值与前一组距离数组内的对应数值分别求差，得到六个偏差值；若任意一个偏差值大于偏差阈值，将该距离数组内的六个距离值作为异常值删除。

②.对经步骤①筛选后保留下来的各个距离数组按照平均值从大到小排序；取处于中间的多个距离数组作为优选距离数组，分别计算d₁₁，d₁₂，d₁₃，d₂₁，d₂₂，d₂₃在各优选距离数组中的平均值，得到当前窗口对应的六个距离值。

4-2.将步骤4-1中经过中值滤波所得的六个时间序列值输入到卡尔曼滤波器，输出平滑的测距数据序列值。

步骤5、计算船头定位主机坐标(x_h,y_h)和船尾定位主机坐标(x_t,y_t)。

5-1.建立距离求解方程组如式(1)所示；若式(1)在解算精度内有解，则得到无人艇船头和船尾的定位主机位置(x_h,y_h)和(x_t,y_t)，直接进入步骤5-4；若式(1)在解算精度内无解，则进入步骤5-2。

5-2.建立扩展方程组如式(2)所示；

其中，δ为误差幅度值，初值为解算精度值。

若仍然出现无解情况，则持续增大误差幅度值δ的大小，并重新计算，直到式(2)有解或循环次数超出计算次数阈值N_cnt；误差幅度值δ每次均增大一个解算精度的数值。若在计算次数阈值N_cnt内式(2)有解，则进入步骤5-3；否则，抛弃该组距离值。

5-3.记录当前的误差幅度值δ，并将误差幅度值δ减小一个解算精度数值后重新代入式 (2)进行解算。若无解，则采用步骤5-2中求出的解作为无人艇的船头定位主机坐标(x_h,y_h)、船尾定位主机坐标(x_t,y_t)；若有解，则将所得解作为无人艇的船头定位主机坐标(x_h,y_h)、船尾定位主机坐标(x_t,y_t)。

5-4.将船头定位主机和船尾定位主机的中点

作为无人艇的坐标位置。

步骤6、计算无人艇的航向数据θ。

6-1以定位基站坐标系的Y轴正方向作为无人艇的0度航向角方向，逆时针方向为航向角的负方向，顺时针方向为航向角的正方向。

6-2.计算出船头定位主机与船尾定位主机坐标的X轴偏差Δx和Y轴偏差Δy如式(3) 所示。

6-3.计算无人艇偏转弧度ω_Rad如式(4)所示。

ω_Rad＝arctan|Δx/Δy|，Δx≠0&Δy≠0 式(4)

式(4)中，|·|为绝对值计算符。

6-4.计算无人艇的弧度航向角θ如式(5)所示。

步骤7、循环执行步骤3到步骤6，持续更新无人艇的坐标位置和航向角。

作为优选，所述的第一定位基站、第二定位基站、第三定位基站分别设置在船坞外端的两侧、内端的中部，呈等边三角形分布。定位基站坐标系的原点O为第一定位基站与第二定位基站的中点，X轴正方向为第一定位基站向第二定位基站的方向，Y轴正方向为原点O远离第三定位基站的方向。

D为第一定位基站与第二定位基站的距离的二分之一。

作为优选，所述船头定位主机、船尾定位主机的连线与无人艇的中心轴线重合。

作为优选，所述的无人艇上还设置有定位计算处理模块。定位计算处理模块采用单片机处理器。定位计算处理模块分别与船头定位主机和船尾定位主机通过线缆连接。

本发明具有的有益效果是：

1.本发明涉及的基于无人艇位置测量与航向测量系统采用了船坞与无人艇之间坐标系变换方法与公式，在无人艇中心位置的基础上实时计算无人艇的航向数据。目前UWB定位一般都只是获取位置信息，没有根据位置信息对航向进行计算的现成方案。

2.本发明涉及的基于无人艇位置测量与航向测量系统采用了滑动窗口中值和卡尔曼混合滤波数据处理方法，滑动窗口中值滤波方法具有快速求解的优点，可以达到计算时钟的频率，卡尔曼滤波方法具有目前公认较高的定位精度，因此本发明兼顾了上述两种滤波方法的优点，实现了测距数值的异常剔除与数据平滑，提高定位精确性。

3.本发明涉及的基于无人艇位置测量与航向测量系统采用了递增幅度和反馈确认的方法来确定定位解算的精度，递增幅度和反馈确认的方法在降低无法定位的情况的前提下，提高定位精度及其可靠性，相比较现有数学公式的计算方法简单高效。

附图说明

图1为本发明采用的无人艇位置-航向测量系统的安装示意图；

图2为本发明步骤4中滑动窗口中值和卡尔曼混合滤波方法流程图；

图3为本发明中无人艇的定位解算计算原理示意图；

图4为本发明中无人艇的航向计算原理示意图。

具体实施方式

以下结合附图对本发明作进一步说明。

如图1所示，一种基于UWB定位的无人艇位置航向测定方法，所采用的无人艇位置-航向测量系统包括定位计算处理模块、两个定位主机(分别为船头定位主机1和船尾定位主机2)和三个定位基站(分别为第一定位基站3、第二定位基站4和第三定位基站5)。船头定位主机1和船尾定位主机2可以输出各自与第一定位基站3、第二定位基站4、第三定位基站5之间的距离值。定位计算处理模块设置在无人艇6上。船头定位主机1、船尾定位主机2分别设置在无人艇6的首端和尾端，且连线与无人艇6的中心轴线重合；第一定位基站3、第二定位基站4、第三定位基站5分别设置在船坞7外端的两侧、内端的中部，呈等边三角形分布。定位主机和定位基站均采用UWB测距模块，定位计算处理模块采用单片机处理器。定位计算处理模块分别与船头定位主机1和船尾定位主机2通过线缆连接。船头定位主机1和船尾定位主机2输出距离序列值给定位计算处理模块，定位计算处理模块可以根据测得的距离值算出无人艇6的坐标位置和航向数据。

该一种基于UWB定位的无人艇6航向测定方法，包括如下步骤：

步骤1、以第一定位基站3与第二定位基站4的中点作为原点O，以第一定位基站3向第二定位基站4的方向为X轴正方向，以原点O远离第三定位基站5的方向为Y轴正方向，建立定位基站坐标系。

船坞7上按上述要求部署的第一定位基站3、第二定位基站4和第三定位基站5，坐标分别为(x₁,y₁)，(x₂,y₂)，(x₃,y₃)；测得第一定位基站3与第二定位基站4之间的距离2D，从而得到：

无人艇6上按上述要求部署的船头定位主机1 和船尾定位主机2，坐标分别为(x_h,y_h)和(x_t,y_t)，其值为待求量。；

步骤2、开启第一定位基站3、第二定位基站4、第三定位基站5、船头定位主机1、船尾定位主机2、定位计算处理模块，并输入D数值给定位计算处理模块。

步骤3、船头定位主机1采用TOA或TDOA等方法测得与第一定位基站3、第二定位基站4、第三定位基站5之间距离d₁₁，d₁₂，d₁₃的时间序列值，船尾定位主机2采用TOA 或TDOA等方法测得与第一定位基站3、第二定位基站4、第三定位基站5之间距离d₂₁， d₂₂，d₂₃的时间序列值。同一时刻测到的d₁₁，d₁₂，d₁₃，d₂₁，d₂₂，d₂₃为一个距离数组。

步骤4、如图2所示，对d₁₁，d₁₂，d₁₃，d₁₁，d₂₂，d₂₃这六项时间序列值作为一组值采用滑动窗口中值和卡尔曼混合滤波方法，先进行异常值剔除，再取时间序列窗口的中间平均值，然后进行卡尔曼滤波，最终实现测距时间序列的异常剔除和平滑处理。具体过程如下：

4-1.用窗口宽度为N的滑动窗口在六个时间序列值上滑动；每次滑动后均对滑动窗口框选的N个距离数组进行中值滤波；通过滑动窗口的持续滑动形成经过中值滤波后的六个时间序列值。

中值滤波的过程如下：

①.将距离数组内的d₁₁，d₁₂，d₁₃，d₂₁，d₂₂，d₂₃与前一组距离数组(即前一个采样时刻采集到的六个距离值)内的对应数值分别求差，得到六个偏差值；若任意一个偏差值大于偏差阈值，将该距离数组内的六个距离值作为异常值删除；

②.对经步骤①筛选后保留下来的各个距离数组按照平均值从大到小排序；取处于中间的多个距离数组作为优选距离数组，分别计算d₁₁，d₁₂，d₁₃，d₂₁，d₂₂，d₂₃在各优选距离数组中的平均值，得到当前窗口对应的六个距离值d₁₁，d₁₂，d₁₃，d₂₁，d₂₂，d₂₃。

4-2.将步骤4-1中经过中值滤波所得的六个时间序列值输入到卡尔曼滤波器，输出平滑的测距数据序列值。

步骤5、定位计算处理模块获取到经过步骤4混合滤波处理后的d₁₁，d₁₂，d₁₃，d₂₁，d₂₂，d₂₃时间序列值，并将其输入到计算方程得到无人艇6的船头定位主机坐标(x_h,y_h)、船尾定位主机坐标(x_t,y_t)，最终获取无人艇6的坐标位置；具体过程如下：

5-1.根据传统的三边定位方法，建立距离求解方程组如式(1)所示；若式(1)在解算精度内有解，则得到无人艇6船头和船尾的定位主机位置(x_h,y_h)和(x_t,y_t)，直接进入步骤 5-4；若式(1)在解算精度内无解，则进入步骤5-2。

5-2.如图3所示降低精度要求形成解区域，建立扩展方程组如式(2)所示；

其中，δ为误差幅度值，初值为解算精度值。

若仍然出现无解情况，则持续增大误差幅度值δ的大小，并重新计算，直到式(2)有解或循环次数超出计算次数阈值N_cnt；误差幅度值δ每次均增大一个解算精度的数值。若在计算次数阈值N_cnt内式(2)有解，则进入步骤5-3；否则，抛弃该组距离值，结果保存为零值，并将解算结果标志位置零，等待下一组距离值的到来。

5-3.记录当前的误差幅度值δ，并将误差幅度值δ减小一个解算精度数值后重新代入式 (2)进行解算。若无解，则采用步骤5-2中求出的解作为无人艇6的船头定位主机坐标(x_h, y_h)、船尾定位主机坐标(x_t,y_t)；若有解，则将所得解作为无人艇6的船头定位主机坐标(x_h, y_h)、船尾定位主机坐标(x_t,y_t)。之后，直接保存结果，置位解算结果标志位并返回。

5-4.将船头定位主机和船尾定位主机的中点作为无人艇6的坐标位置

步骤6、如图4所示，定位计算处理模块根据步骤5的计算结果，输入到计算方程获取无人艇6的航向数据。

6-1定义定位基站坐标系下Y轴正方向作为无人艇6的0度航向角方向，从Y轴正方向逆时针旋转到Y轴负方向为航向角的负方向，从Y轴正方向顺时针旋转到Y轴负方向为航向角的正方向。

6-2.在计算时首先判断是否接收到船头定位主机和船尾定位主机的数据并且已经计算出定位数据，然后计算出船头定位主机与船尾定位主机坐标的X轴偏差Δx和Y轴偏差Δy 如式(3)所示。

6-3.计算无人艇6偏转弧度ω_Rad如式(4)所示。

ω_Rad＝arctan|Δx/Δy|，Δx≠0&Δy≠0 式(4)

式(4)中，|Δx/Δy|为Δx/Δy的绝对值。

6-4.计算无人艇6的弧度航向角θ如式(5)所示。

步骤7、由于作业中的无人艇6在持续移动，所以循环执行步骤3到步骤6，持续计算出无人艇6的坐标位置和航向角。

对本发明的精准性测试如下：

设置九个测量位置，分别为(14.05,15.05)、(0,15.05)、(-14.05,15.05)、(14.05,34.07)、(0,34.07)、(-14.05,34.07)、(14.05,53.42)、(0,53.42)、(-14.05,53.42)。对九个测量位置分别进行多次定位计算，误差值小于1米。

Claims (4)

1.一种基于UWB定位的无人艇位置航向测定方法，其特征在于：步骤1、在船坞上设置第一定位基站、第二定位基站和第三定位基站；在无人艇的头端和尾端分别设置船头定位主机和船尾定位主机；各定位主机和各定位基站均采用UWB测距模块；建立定位基站坐标系，第一定位基站、第二定位基站、第三定位基站的坐标分别为(x₁,y₁)，(x₂,y₂)，(x₃,y₃)；船头定位主机、船尾定位主的坐标分别为(x_h,y_h)，(x_t,y_t)；

步骤2、开启第一定位基站、第二定位基站、第三定位基站、船头定位主机和船尾定位主机；

步骤3、船头定位主机分别测得自身与第一定位基站、第二定位基站、第三定位基站之间距离d₁₁，d₁₂，d₁₃的时间序列值，船尾定位主机分别测得自身与第一定位基站、第二定位基站、第三定位基站之间距离d₂₁，d₂₂，d₂₃的时间序列值；同一时刻测到的d₁₁，d₁₂，d₁₃，d₂₁，d₂₂，d₂₃为一个距离数组；

步骤4、对步骤3所得的六个距离时间序列值进行混合滤波；

4-1.用窗口宽度为n的滑动窗口在六个时间序列值上滑动；每次滑动后均对滑动窗口框选的n个距离数组进行中值滤波；通过滑动窗口的持续滑动形成经过中值滤波后的六个时间序列值；

中值滤波的过程如下：

①.将距离数组内的六个距离值与前一组距离数组内的对应数值分别求差，得到六个偏差值；若任意一个偏差值大于偏差阈值，将该距离数组内的六个距离值作为异常值删除；

②.对经步骤①筛选后保留下来的各个距离数组按照平均值从大到小排序；取处于中间的多个距离数组作为优选距离数组，分别计算d₁₁，d₁₂，d₁₃，d₂₁，d₂₂，d₂₃在各优选距离数组中的平均值，得到当前窗口对应的六个距离值；

4-2.将步骤4-1中经过中值滤波所得的六个时间序列值输入到卡尔曼滤波器，输出平滑的测距数据序列值；

步骤5、计算船头定位主机坐标(x_h,y_h)和船尾定位主机坐标(x_t,y_t)；

5-1.建立距离求解方程组如式(1)所示；若式(1)在解算精度内有解，则得到无人艇船头和船尾的定位主机位置(x_h,y_h)和(x_t,y_t)，直接进入步骤5-4；若式(1)在解算精度内无解，则进入步骤5-2；

5-2.建立扩展方程组如式(2)所示；

其中，δ为误差幅度值，初值为解算精度值；

若仍然出现无解情况，则持续增大误差幅度值δ的大小，并重新计算，直到式(2)有解或循环次数超出计算次数阈值N_cnt；误差幅度值δ每次均增大一个解算精度的数值；若在计算次数阈值N_cnt内式(2)有解，则进入步骤5-3；否则，抛弃该组距离值；

5-3.记录当前的误差幅度值δ，并将误差幅度值δ减小一个解算精度数值后重新代入式(2)进行解算；若无解，则采用步骤5-2中求出的解作为无人艇的船头定位主机坐标(x_h,y_h)、船尾定位主机坐标(x_t,y_t)；若有解，则将所得解作为无人艇的船头定位主机坐标(x_h,y_h)、船尾定位主机坐标(x_t,y_t)；

5-4.将船头定位主机和船尾定位主机的中点

作为无人艇的坐标位置；

步骤6、计算无人艇的航向数据θ；

6-1以定位基站坐标系的Y轴正方向作为无人艇的0度航向角方向，逆时针方向为航向角的负方向，顺时针方向为航向角的正方向；

6-2.计算出船头定位主机与船尾定位主机坐标的X轴偏差Δx和Y轴偏差Δy如式(3)所示；

6-3.计算无人艇偏转弧度ω_Rad如式(4)所示；

ω_Rad＝arctan|Δx/Δy|，Δx≠0&Δy≠0 式(4)

式(4)中，|·|为绝对值计算符；

6-4.计算无人艇的弧度航向角θ如式(5)所示；

步骤7、循环执行步骤3到步骤6，持续更新无人艇的坐标位置和航向角。

2.根据权利要求1所述的一种基于UWB定位的无人艇位置航向测定方法，其特征在于：所述的第一定位基站、第二定位基站、第三定位基站分别设置在船坞外端的两侧、内端的中部，呈等边三角形分布；定位基站坐标系的原点O为第一定位基站与第二定位基站的中点，X轴正方向为第一定位基站向第二定位基站的方向，Y轴正方向为原点O远离第三定位基站的方向；

D为第一定位基站与第二定位基站的距离的二分之一。

3.根据权利要求1所述的一种基于UWB定位的无人艇位置航向测定方法，其特征在于：所述船头定位主机、船尾定位主机的连线与无人艇的中心轴线重合。

4.根据权利要求1所述的一种基于UWB定位的无人艇位置航向测定方法，其特征在于：所述的无人艇上还设置有定位计算处理模块；定位计算处理模块采用单片机处理器；定位计算处理模块分别与船头定位主机和船尾定位主机通过线缆连接。

Images