一种基于UWB室内定位的无人艇船载测速测向方法与流程

本发明属于水面无人艇的室内定位信息获取技术领域，具体涉及一种基于uwb室内定位的无人艇船载测速测向方法。

背景技术：

为了对无人艇实现有效的运动控制，必须使其能够获取控制对象，即无人艇速度和艏向的实时反馈，才能使控制算法实现闭环，并有效地进行运动控制。

在室外往往可以通过gps和磁罗经来获取此类信息，然而在室内水池，沟渠等遮蔽空间，gps不能或难以收到稳定的信号，磁罗经受建筑物内局部磁场的影响严重，其测向数据易发生突变，难以用于室内无人艇的艏向控制，而一般的室内定位装置例如红外线、超声波、蓝牙等却只能反馈有效的位置信息，因此结合室内定位装置设计出稳定高效的无人艇实时测速测向系统是非常有必要的。

公开日2017年2月15日，公开号cn205958595u，发明名称为“一种车辆测速测向装置及系统”的专利申请，提供一种紧贴车辆车轮设置且具有无线通信功能的集成式车辆测速测向装置，可无线采集车辆的运动速度和运动方向并无线上传到车辆中控台或移动终端，降低了安装和维修难度,并且结构简单、易于实现、成本低廉。但该方法是利用测量车轮的转速和转向进行测速测向，而车辆与水面无人艇的机动机构不同，无法通过测量螺旋桨转速和舵机转向实现无人艇的测速测向，因此无法应用于无人艇的测速测向工作中。

公开日2017年5月10日，公开号cn106627673a，发明名称为“一种多传感器融合的列车定位方法及系统”的专利申请，采集至少两个测速设备的数据，并分别将至少两个测速设备的数据转换为速度信息，包括速度的大小和方向、以及状态信息，再将至少两个测速设备对应的速度信息采取预设的融合算法进行融合，获得融合后的速度信息及距离信息并输出。但该方法所述的两个以上的测速设备中至少包含一个轴段测速设备和一个非轴段测速设备，这是因为考虑了列车的机动结构特征，其融合算法也是针对列车设计的，而无人艇的机动结构与列车大相径庭，因此该方法同样无法直接应用于无人艇的室内测速测向。

公开日2008年4月16日，公开号cn201047846，发明名称为“船舶尾轴测速测向装置”的专利申请，较好地解决了非接触式传感器脉冲测速测向技术的不足和缺陷，使用单个传感器与测速齿轮配合，利用测速齿轮中空位的配置来完成测向的功能,使得安装更为方便。同时测速单元利用微处理器来完成，使得单传感器方案能同时检测转速和转向，使得性能和功能大大提高。但是该方法是针对船舶尾轴设计的间接测速测向方法，忽略了船舶受环境、载重量、吃水等变化的影响，其结果无法代表船舶的实际速度和艏向，因此同样无法满足无人艇进行自主运动控制的需要。

技术实现要素：

本发明的目的在于提供一种基于uwb室内定位的无人艇船载测速测向方法。以uwb室内定位装置为基础，充分结合其数据流反馈特点，结合无人艇的几何结构特征，设计出一种满足无人艇运动控制需要的实时测速测向算法，其中包括速度解算算法，艏向判定算法和艏向解算算法，对无人艇的速度艏向信息在上位机上进行实时解算，并将得到的速度艏向信息同位置信息一起通过串口经由无线电发送给目标无人艇，从而对无人艇进行有效控制。

本发明的目的是这样实现的，包括以下步骤：

(1)通过uwb室内定位装置的上位机软件获取无人艇上定位标签的位置信息pos(xi,yi)；

(2)对定位数据进行阈值处理；判断当前时刻的位置pos(xi,yi)与上一时刻的位置prepos(xi,yi)之间的距离是否小于阈值threshold，若小于阈值则直接执行步骤(3)，若大于阈值，则说明该值为装置野值，此时令pos(xi,yi)＝prepos(xi,yi)+t*prevel，继续执行步骤(3)；

(3)对pos(xi,yi)进行卡尔曼滤波，得到滤波后的pos(xi,yi)；其中卡尔曼滤波器的模型函数设计为pos＝pos+t*prevel，其中t为室内定位装置的数据反馈时间间隔，由于t一般很小，因此忽略二阶加速度项；

(4)利用速度解算算法f_outvel(i)解算输出速度outvel和速度标志flag_vel，若速度标志flag_vel等于1则执行步骤(5)，否则返回步骤(1)；

(5)令上一时刻的速度prevel等于输出速度outvel，即保存速度信息用于阈值过滤和卡尔曼滤波；

(6)利用艏向判定算法f_faigate(n，i)，判断艏向判定标志flag_fai是否等于1，是则执行步骤(7)，否则返回步骤(1)；

(7)根据无人艇上的定位标签数目n和当前标签号i利用艏向解算算法f_outfai(i)计算无人艇当前的艏向角outfai并输出，执行步骤(8)，否则返回步骤(1)；

(8)最后上位机软件通过串口经由无线电将解算结果发送给水面无人艇，实现信息交互，并返回步骤(1)，再次等待定位装置获取新的定位信息。

本发明还包含以下结构特征：

所述步骤(4)中速度解算算法f_outvel(i)主要计算方法为：

(4.1)判断速度消息队列qvel是否存在，不存在则创建qvel并设为空；

(4.2)判断qvel是否为空，是则令当前定位信息curpos和上一时刻定位信息prepos都等于此刻输入的定位信息(xi,yi)，并令输出标号flag_out等于当前定位数据标号i，否则令上一时刻定位信息prepos等于当前定位信息curpos，再令当前定位信息curpos等于此刻输入的定位信息(xi,yi)；

(4.3)判断当前定位信息标号i是否等于输出标号flag_out，是则求解当前速度信息curvel等于(curpos-prepos)/t，其中t为信息反馈的时间间隔，并转步骤(4.4)，否则令速度标志flag_vel等于0并结束；

(4.4)将当前速度信息curvel存入消息队列qvel，若qvel已满则执行步骤(4.5)，否则令速度标志flag_vel等于0，结束；

(4.5)计算输出速度outvel等于qvel中所有元素的求和取平均，然后删除qvel中的首个消息元素，最后令速度标志flag_vel等于1，结束。

所述步骤(6)中艏向判定算法f_faigate(n，i)主要计算方法为：

(6.1)判断标签标志位flag(i)是否已初始化过，并将未初始化的flag(i)初始化为0；

(6.2)判断当前时刻反馈的定位数据标号i，i等于0时，执行步骤(6.3)，i等于1时，执行步骤(6.4)，i等于2时，执行步骤(6.5)，i等于3时，执行步骤(6.6)；

(6.3)若flag(0)等于0，令其等于1，否则令其等于0；

(6.4)若flag(1)等于0，令其等于1，否则令其等于0；

(6.5)若flag(2)等于0，令其等于1，否则令其等于0；

(6.6)若flag(3)等于0，令其等于1，否则令其等于0；

(6.7)当且仅当flag(0)和flag(1)都等于1或flag(2)和flag(3)都等于1时，令艏向判定标志flag_fai等于1，否则flag_fai等于0。

所述步骤步骤(7)中艏向解算算法f_outfai(i)主要计算方法为：

(7.1)若flag(0)＝flag(1)＝1且当前定位数据标签号i等于0，计算输出艏向outfai等于arctan(curpos-prepos)，然后令flag(0)＝flag(1)＝0；

(7.2)若flag(0)＝flag(1)＝1且当前定位数据标签号i等于1，计算输出艏向outfai等于arctan(prepos-curpos)，然后令flag(0)＝flag(1)＝0；

(7.3)若flag(2)＝flag(3)＝1且当前定位数据标签号i等于2，计算输出艏向outfai等于arctan(curpos-prepos)；然后令flag(2)＝flag(3)＝0；

(7.4)若flag(2)＝flag(3)＝1且当前定位数据标签号i等于3，计算输出艏向outfai等于arctan(prepos-curpos)；然后令flag(2)＝flag(3)＝0。

本发明的有益效果在于：

(1)一般的测速测向系统没有考虑无人艇的机动特性和结构特征，本发明针对无人艇的这些特点，基于uwb室内定位装置，在上位机软件上实现速度艏向的解算之后，通过串口经由无线电将位置速度艏向信息发送给无人艇上对应的无线电接收模块，从而设计出满足无人艇运动控制需要的、稳定的、低成本的、实时无人艇室内测速测向系统；

(2)一般的室内定位装置不具备实时反馈目标速度艏向的功能，本发明在此基础上，结合uwb室内定位装置的数据流反馈特性，设计出无人艇速度解算算法f_outvel(i)和艏向判定算法f_faigate(n，i)，同时结合无人艇的几何结构特征，设计出无人艇艏向解算算法f_outfai(i)，从而实现了无人艇的室内定位、速度、艏向多元信息的获取。

说明书附图

图1为一种基于uwb室内定位的无人艇船载测速测向方法的主流程图；

图2为速度解算算法f_outvel(i)的计算流程图；

图3为艏向判定算法f_faigate(i)的计算流程图；

图4为艏向解算算法f_outfai(i)的计算流程图；

图5为速度消息队列qvel的结构图；

图6为艏向判定算法f_faigate(i)的具体说明图；

图7为室内定位标签的安置方式图；

图8为整个无人艇船载室内定位测速测向系统的结构图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步说明：

本发明提出了一种基于uwb室内定位的无人艇船载测速测向方法及系统。该方法的主要步骤包括：(1)由uwb室内定位装置获取无人艇定位标签的位置信息pos(xi,yi)并进行阈值过滤和卡尔曼滤波；(2)利用速度解算算法f_outvel(i)解算出速度outvel，若成功则执行步骤(3)，否则返回步骤(1)；(3)令上一时刻速度prevel等于当前速度outvel；(4)利用艏向判定算法f_faigate(n，i)，判断艏向判定标志flag_fai是否等于1，是则执行步骤(5)，否则返回步骤(1)；(5)根据无人艇上的定位标签数目n和当前标签号i，利用艏向解算算法f_outfai(i)来计算无人艇当前的艏向角outfai并输出，执行步骤(6)，否则返回步骤(1)；(6)最后上位机通过串口经由无线电将解算结果发送给水面无人艇，实现信息交互。

本发明在uwb室内定位装置的基础上，为满足无人艇在室内对速度艏向进行实时控制的需要，充分考虑室内定位装置的数据反馈特性，结合阈值处理、卡尔曼滤波器和无人艇的几何结构特性，设计出一种稳定、低成本、非接触式的无人艇船载室内实时测速测向系统。

结合图1，该方法基于uwb室内定位装置，充分考虑其定位数据的传输特性，经历阈值处理和卡尔曼滤波器的初步过滤，利用速度解算算法f_outvel(i)得到稳定的速度输出；利用多个定位标签之间的耦合关系和无人艇自身几何特性，分别设计出艏向判定算法f_faigate(n，i)和艏向解算算法f_outfai(i)，从而获得实时的艏向输出，上位机解算之后通过串口经由无线电实时将解算结果发送到目标无人艇，由无人艇上相应的无线电通信模块进行接收，形成信息交互。

结合图2，速度解算算法f_outvel(i)，实现了无人艇速度信息的稳定输出，

结合图3，艏向判定算法f_faigate(n，i)，实现了对室内定位装置反馈数据的判定和筛选

结合图4，艏向解算算法f_outfai(i)，实现了无人艇实时艏向角的解算

结合图5，关于速度消息队列qvel的详细说明，其结构下图5所示：不同于gps的速度计算的方法，该算法基于梯度下降的原理设计得到，其速度输出的稳定性可通过改变速度消息队列qvel的长度l进行调节，l越大速度输出越平缓，l越小速度波动越明显，但过大的l会使输出速度失真，因此应结合实际工况选择合适的l。

结合图6，关于艏向判定算法f_faigate(i)的具体说明如图6所示：该方法仿照逻辑电路中的与或门进行设计，由于uwb室内定位装置本身在每个节拍只能反馈一个定位标签的数据，因此需要对当前数据和上一节拍的数据进行耦合分析，利用两个定位标签间的逻辑关系计算出艏向判定标志flag_fai。

结合图7，艏向解算算法f_outfai(i)的具体说明：该算法根据无人艇的几何结构特点设计，由于无人艇一般为双体或单体船结构，因此设计室内定位标签的安置方式如图7：标签的安置顺序需要严格按照图中所示，这样无论是单体还是双体无人艇，都可利用艏向解算算法f_outfai(i)对无人艇的艏向角进行解算。

结合图8，整个无人艇船载室内定位测速测向系统的结构图，其中虚线表示无线通信，实线表示有线连接。

本发明涉及水面无人艇的室内定位信息获取领域，是一种充分结合uwb定位信号与无人艇几何结构特征的，针对室内环境中无人艇实时速度艏向控制需要，而设计的无人艇船载室内测速测向系统。

本发明充分考虑uwb定位信号的回馈特点和无人艇的一般几何结构特征，在uwb室内定位装置的基础上，设计出实时且稳定的无人艇速度艏向解算方法，充分满足了在室内水池进行无人艇运动控制的迫切需要，为设计复杂、智能、高效的无人艇运动控制算法奠定基础。