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

一种基于UWB室内定位的无人艇船载测速测向方法，属于水面无人艇的室内定位信息获取技术领域。该方法基于UWB室内定位装置，充分考虑定位数据的传输特性，经历阈值处理和卡尔曼滤波器的初步过滤，利用速度解算算法F_outvel(i)得到稳定的速度输出；利用多个定位标签之间的耦合关系和无人艇自身几何特性，分别设计出艏向判定算法F_faigate(n，i)和艏向解算算法F_outfai(i)，从而获得实时的艏向输出，上位机解算之后通过串口经由无线电实时将解算结果发送到目标无人艇，由无人艇上相应的无线电通信模块进行接收，形成信息交互。本发明专利技术设计的测速测向系统满足了无人艇运动控制点的需要、稳定、低成本、实时的要求；实现了无人艇的室内定位、速度、艏向多元信息的获取。

A Vehicle-borne Velocity and Direction Finding Method for Unmanned Vehicle Based on UWB Indoor Positioning

A method of velocity and direction finding based on UWB indoor positioning for unmanned aerial vehicle is proposed, which belongs to the field of indoor positioning information acquisition technology for surface unmanned aerial vehicle. This method is based on UWB indoor positioning device, fully considering the transmission characteristics of positioning data, experiencing threshold processing and Kalman filter preliminary filtering, using the speed resolution algorithm F_outvel(i) to obtain stable speed output; using the coupling relationship between multiple positioning tags and the geometric characteristics of unmanned aerial vehicle itself, the heading determination algorithm F_faigate(n, i) and heading calculation are designed respectively. The method F_outfai(i) is used to obtain the real-time bow output. After the upper computer calculates, the solution results are sent to the target UAV through the serial port in real time through radio. The corresponding radio communication module on the UAV receives the results and forms the information exchange. The speed and direction finding system designed by the invention meets the requirements of UAV motion control point, stability, low cost and real-time, and achieves indoor positioning, speed and heading multi-information acquisition of UAV.

【技术实现步骤摘要】
一种基于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(本文档来自技高网...

【技术保护点】
1.一种基于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)，再次等待定位装置获取新的定位信息。...

【技术特征摘要】
1.一种基于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)，再次等待定位装置获取新的定位信息。2.根据权利要求1所述的一种基于UWB室内定位的无人艇船载测速测向方法，其特征在于：所述的步骤(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...

【专利技术属性】
技术研发人员：贾知浩，廖煜雷，李晔，贾琪，陈启贤，张伟，苏玉民，沈海龙，武皓微，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江,23