1.一种基于UWB定位的无人艇位置航向测定方法,其特征在于:步骤1、在船坞上设置第一定位基站、第二定位基站和第三定位基站;在无人艇的头端和尾端分别设置船头定位主机和船尾定位主机;各定位主机和各定位基站均采用UWB测距模块;建立定位基站坐标系,第一定位基站、第二定位基站、第三定位基站的坐标分别为(x1,y1),(x2,y2),(x3,y3);船头定位主机、船尾定位主的坐标分别为(xh,yh),(xt,yt);
步骤2、开启第一定位基站、第二定位基站、第三定位基站、船头定位主机和船尾定位主机;
步骤3、船头定位主机分别测得自身与第一定位基站、第二定位基站、第三定位基站之间距离d11,d12,d13的时间序列值,船尾定位主机分别测得自身与第一定位基站、第二定位基站、第三定位基站之间距离d21,d22,d23的时间序列值;同一时刻测到的d11,d12,d13,d21,d22,d23为一个距离数组;
步骤4、对步骤3所得的六个距离时间序列值进行混合滤波;
4-1.用窗口宽度为n的滑动窗口在六个时间序列值上滑动;每次滑动后均对滑动窗口框选的n个距离数组进行中值滤波;通过滑动窗口的持续滑动形成经过中值滤波后的六个时间序列值;
中值滤波的过程如下:
①.将距离数组内的六个距离值与前一组距离数组内的对应数值分别求差,得到六个偏差值;若任意一个偏差值大于偏差阈值,将该距离数组内的六个距离值作为异常值删除;
②.对经步骤①筛选后保留下来的各个距离数组按照平均值从大到小排序;取处于中间的多个距离数组作为优选距离数组,分别计算d11,d12,d13,d21,d22,d23在各优选距离数组中的平均值,得到当前窗口对应的六个距离值;
4-2.将步骤4-1中经过中值滤波所得的六个时间序列值输入到卡尔曼滤波器,输出平滑的测距数据序列值;
步骤5、计算船头定位主机坐标(xh,yh)和船尾定位主机坐标(xt,yt);
5-1.建立距离求解方程组如式(1)所示;若式(1)在解算精度内有解,则得到无人艇船头和船尾的定位主机位置(xh,yh)和(xt,yt),直接进入步骤5-4;若式(1)在解算精度内无解,则进入步骤5-2;
5-2.建立扩展方程组如式(2)所示;
其中,δ为误差幅度值,初值为解算精度值;
若仍然出现无解情况,则持续增大误差幅度值δ的大小,并重新计算,直到式(2)有解或循环次数超出计算次数阈值Ncnt;误差幅度值δ每次均增大一个解算精度的数值;若在计算次数阈值Ncnt内式(2)有解,则进入步骤5-3;否则,抛弃该组距离值;
5-3.记录当前的误差幅度值δ,并将误差幅度值δ减小一个解算精度数值后重新代入式(2)进行解算;若无解,则采用步骤5-2中求出的解作为无人艇的船头定位主机坐标(xh,yh)、船尾定位主机坐标(xt,yt);若有解,则将所得解作为无人艇的船头定位主机坐标(xh,yh)、船尾定位主机坐标(xt,yt);
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定位的无人艇位置航向测定方法,其特征在于:所述的无人艇上还设置有定位计算处理模块;定位计算处理模块采用单片机处理器;定位计算处理模块分别与船头定位主机和船尾定位主机通过线缆连接。