CN105180942B - 一种无人船自主导航方法及装置 - Google Patents

一种无人船自主导航方法及装置 Download PDF

Info

Publication number
CN105180942B
CN105180942B CN201510579857.8A CN201510579857A CN105180942B CN 105180942 B CN105180942 B CN 105180942B CN 201510579857 A CN201510579857 A CN 201510579857A CN 105180942 B CN105180942 B CN 105180942B
Authority
CN
China
Prior art keywords
unmanned boat
coordinates
level
point coordinates
point
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.)
Active
Application number
CN201510579857.8A
Other languages
English (en)
Other versions
CN105180942A (zh
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.)
Shenzhen Zhongzhi Weian Robot Technology Co.,Ltd.
Original Assignee
China Security and Surveillance Technology PRC Inc
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 China Security and Surveillance Technology PRC Inc filed Critical China Security and Surveillance Technology PRC Inc
Priority to CN201510579857.8A priority Critical patent/CN105180942B/zh
Publication of CN105180942A publication Critical patent/CN105180942A/zh
Application granted granted Critical
Publication of CN105180942B publication Critical patent/CN105180942B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • 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/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明提供一种无人船自主导航方法包括：预设无人船的运行轨迹，采集无人船运行轨迹上的原始坐标，并通过原始坐标得到一级参考点坐标；获取无人船的当前位置坐标，并根据当前位置坐标判断无人船是否偏离预设的运行轨迹；若未偏离，则利用卡尔曼滤波预测出下一个预测点坐标；再判断下一个预测点坐标与对应的一级参考点坐标的偏差是否小于预设的第一阈值，若是，则使无人船向下一个预测点坐标运行；否则，根据当前位置坐标与一级参考点坐标确定所述无人船进一步的运行方向。本发明的无人船自主导航方法解决了人为遥控或定位误差因素造成的S型曲线运动的问题，能更加的精确对无人船进行导航。另外，本发明还提供了一种无人船自主导航装置。

Description

一种无人船自主导航方法及装置
技术领域
本发明涉及无人导航技术领域，具体涉及一种无人船自主导航方法及装置。
背景技术
在水产养殖领域，因为水质状况对水生物的生存、生长影响很大，故水产养殖户对水质信息的获取较为迫切，但传统的定点监测方法存在监测不全面的问题，要实现多处监测，还存在要么需要人为调整，要么增加传感器设备从而增加成本等问题。
现有技术中，也有使用小型无人船搭载水质监测传感器来实现对水体信息的监测，通常是基于GPS定位加导航算法来实现自主导航，但这种方案适用于水域范围较大的场景，大部分的水产养殖池塘的面积不是很大，因此现有的导航方法无法满足无人船自主导航的精度要求，存在因人为遥控或定位误差因素造成的S型曲线运动的问题，造成导航系统的不稳定。
发明内容
本发明的目的在于提出一种无人船自主导航方法及装置，能够对无人船进行更加精准的导航，并对直线段和曲线段采用不同的预测校正导航方法，解决了人为遥控或定位误差因素造成的S型曲线运动的问题。
为达此目的，本发明采用以下技术方案：
一种无人船自主导航方法，包括：
预设无人船的运行轨迹，采集无人船运行轨迹上的原始坐标，并通过所述原始坐标得到至少两个一级参考点坐标；
获取所述无人船的当前位置坐标，并根据所述当前位置坐标判断所述无人船是否偏离预设的运行轨迹；
若所述无人船未偏离预设的运行轨迹，则利用卡尔曼滤波预测出下一个预测点坐标；
判断所述下一个预测点坐标与对应的一级参考点坐标的偏差是否小于预设的第一阈值，若是，则使所述无人船向所述下一个预测点坐标运行；
否则，根据所述当前位置坐标与所述一级参考点坐标确定所述无人船进一步的运行方向；
所述根据所述当前位置坐标及所述一级参考点坐标确定所述无人船进一步的运行方向具体为：
将每间隔预设数量的所述一级参考点坐标标记为二级参考点坐标；
根据所述当前位置坐标与所述一级参考点坐标、所述二级参考点坐标的位置关系确定所述无人船进一步的运行方向。
优选地，还包括：
若所述无人船偏离预设的运行轨迹，则根据所述当前位置坐标与所述一级参考点坐标确定所述无人船进一步的运行方向。
优选地，所述根据所述当前位置坐标与所述一级参考点坐标、所述二级参考点坐标的位置关系确定所述无人船进一步的运行方向具体为：
判断所述无人船当前所处轨迹段的轨迹状态；
若所述无人船当前所处轨迹段为直线段轨迹，则获取与所述当前位置坐标相邻的下一个二级参考点坐标，使所述无人船向所述下一个二级参考点坐标运行；
若所述无人船当前所处轨迹段为曲线段轨迹，则获取与所述当前位置坐标相邻的下一个一级参考点坐标，并计算得到所述下一个一级参考点坐标和下一个二级参考点坐标的中间坐标，使所述无人船向所述中间坐标运行。
优选地，所述根据所述当前位置坐标与所述一级参考点坐标、所述二级参考点坐标的位置关系确定所述无人船进一步的运行方向具体为：
获取与所述当前位置坐标相邻的下一个二级参考点坐标，使所述无人船向所述下一个二级参考点坐标运行。
优选地，所述采集无人船运行轨迹上的原始坐标，并通过所述原始坐标得到一级参考点坐标包括：
通过超宽带标签以预设的频率采集所述无人船运行轨迹上的原始坐标数组；
从所述原始坐标数组中选取一个原始坐标作为第一个一级参考点坐标，沿着所述预设的运行轨迹方向，获取与所述第一个一级参考点坐标的距离为预设距离差值的至少两个原始坐标，计算所获取的所述至少两个原始坐标的平均值，将所述平均值定义为第二个一级参考点坐标，以此类推，将所有原始坐标数组搜索完毕，即获得一级参考点坐标数组。
优选地，所述判断所述无人船当前所处轨迹段的轨迹状态具体为：
获取与所述当前位置坐标前后相邻的两个二级参考点坐标；
计算所述相邻的两个二级参考点坐标所形成的直线的斜率；
根据计算所得的斜率值来判断所述无人船当前所处轨迹段的轨迹状态。
优选地，所述根据计算所得的斜率值来判断所述无人船当前所处轨迹段的轨迹状态具体为：
当-0.2＜Rn-1,n＜0.2时，确定所述无人船处于X轴直线段轨迹；
当(0.2＜Rn-1,n＜20&&xn＞xn-1)||(-20＜Rn-1,n＜-0.2&&xn＜xn-1)时，确定所述无人船处于左转弯状态；
当(0.2＜Rn-1,n＜20&&xn＜xn-1)||(-20＜Rn-1,n＜-0.2&&xn＞xn-1)时，确定所述无人船处于右转弯状态；
当(Rn-1,n＞20)||(Rn-1,n＜-20时，确定所述无人船处于Y轴直线段轨迹；
其中，Rn-1,n为所述相邻的两个二级参考点坐标所形成直线的斜率值，n为正整数，xn-1和xn对应为所述相邻的两个二级参考点坐标的X轴坐标；所述无人船处于X轴直线段轨迹和处于Y轴直线段轨迹为所述直线段轨迹的两种形式；所述无人船处于左转弯状态和处于右转弯状态为所述曲线段轨迹的两种形式。
优选地，所述利用卡尔曼滤波预测出下一个预测点坐标具体为：
1)定义Xk为卡尔曼滤波器k时刻的状态向量也即所需要预测的无人船的下一个预测点，为k时刻的状态先验估计，根据k-1时刻的状态向量Xk-1也即无人船的当前坐标点，计算k时刻的状态先验估计公式为：Xk -＝AXk-1；并计算k时刻的误差协方差先验估计公式为：Pk -＝APk-1AT+Q；
2)计算卡尔曼滤波增益Kk，公式为：Kk＝Pk-HT(HPk-HT+R)-1；
3)定义Zk为观测向量，也即无人船在k时刻实际获取到的位置坐标，根据Zk计算得到k时刻的状态向量也即状态后验估计Xk，公式为Xk＝Xk - +Kk(Zk-HXk -)；
4)将Xk根据公式Xk＝[px(k)py(k)vx(k)vy(k)]T以向量形式展开，其中，px(k)、py(k)分别是k时刻在x、y轴上的位置分量，vx(k)vy(k)分别是k时刻在x、y轴上的速度分量，根据px(k)、py(k)即可获得所需要预测的无人船的下一个预测点的坐标；
其中，A为状态转移矩阵，H为观测矩阵，
Q、R为协方差矩阵：
作为本发明的另一个方面，提供一种无人船自主导航装置，包括：
轨迹绘制模块，用于预设无人船的运行轨迹，采集无人船运行轨迹上的原始坐标，并通过所述原始坐标得到至少两个一级参考点坐标；
获取判断模块，用于获取所述无人船的当前位置坐标，并根据所述当前位置坐标判断所述无人船是否偏离预设的运行轨迹；
预测模块，用于当无人船未偏离预设的运行轨迹时，则利用卡尔曼滤波预测出下一个预测点坐标；
偏差判断模块，用于判断所述下一个预测点坐标与对应的一级参考点坐标的偏差是否小于预设的第一阈值；
第一导航模块，用于当所述下一个预测点坐标与对应的一级参考点坐标的偏差小于预设的第一阈值时，则使所述无人船向所述下一个预测点坐标运行；
第二导航模块，用于当所述下一个预测点坐标与对应的一级参考点坐标的偏差大于预设的第一阈值时，根据所述当前位置坐标与所述一级参考点坐标确定所述无人船进一步的运行方向。
本发明的有益效果为：一种无人船自主导航方法包括：预设无人船的运行轨迹，采集无人船运行轨迹上的原始坐标，并通过原始坐标得到一级参考点坐标；获取无人船的当前位置坐标，并根据当前位置坐标判断无人船是否偏离预设的运行轨迹；若未偏离，则利用卡尔曼滤波预测出下一个预测点坐标；再判断下一个预测点坐标与对应的一级参考点坐标的偏差是否小于预设的第一阈值，若是，则使无人船向下一个预测点坐标运行；否则，根据当前位置坐标与一级参考点坐标确定所述无人船进一步的运行方向。本发明的无人船自主导航方法解决了人为遥控或定位误差因素造成的S型曲线运动的问题，能更加的精确对无人船进行导航。
附图说明
图1是本发明实施例一提供的一种无人船自主导航方法的流程图。
图2是本发明实施例一提供的一种坐标轴建立示意图。
图3是本发明实施例二提供的一种轨迹绘制方法的流程图。
图4是本发明实施例二提供的一种轨迹绘制方法的流程图。
图5是本发明实施例二提供的一种轨迹绘制方法的流程图。
图6是本发明实施例三提供的一种轨迹绘制方法的流程图。
图7是本发明实施例四提供的卡尔曼滤波原理示意图。
图8是本发明实施例四提供的一种导航调整示意图。
图9是本发明实施例四提供的另一种导航调整示意图。
图10是本发明实施例五提供的一种无人船自主导航装置的结构图。
图11是图10中第二导航模块的结构图。
图12是图11中导航单元的结构图。
图13是图10中轨迹绘制模块的结构图。
图14是图12中轨迹状态判断单元的结构图。
具体实施方式
下面结合图1-图14并通过具体实施方式来进一步说明本发明的技术方案。
实施例一
图1是本发明实施例一提供的一种无人船自主导航方法的流程图。
一种无人船自主导航方法，包括：
S10、预设无人船的运行轨迹，采集无人船运行轨迹上的原始坐标，并通过所述原始坐标得到至少两个一级参考点坐标；
S20、获取所述无人船的当前位置坐标，并根据所述当前位置坐标判断所述无人船是否偏离预设的运行轨迹；
S30、若所述无人船未偏离预设的运行轨迹，则利用卡尔曼滤波预测出下一个预测点坐标；
S40、判断所述下一个预测点坐标与对应的一级参考点坐标的偏差是否小于预设的第一阈值，若是，则进入步骤S41、使所述无人船向所述下一个预测点坐标运行；
S50、否则，根据所述当前位置坐标与所述一级参考点坐标确定所述无人船进一步的运行方向。
在本实施例中，超宽带(UWB，Ultra Wideband)是一种不用载波，而采用时间间隔极短(小于1ns)的脉冲进行通信的方式，通过在较宽的频谱上传送极低功率的信号，UWB能在10米左右的范围内实现数百Mbit/s至数Gbit/s的数据传输速率，使用超宽带技术实现定位，相对于传统的GPS导航方式，能更加的精确对无人船进行导航，并对直线段和曲线段采用不同的预测校正导航方法，解决了人为遥控或定位误差因素造成的S型曲线运动的问题，能更加的精确对无人船进行导航。
在本实施例中，所述步骤S10之前还包括：
确定坐标轴，所述坐标轴分为X轴和Y轴。
如图2所示，以基站a为圆点，a与b的连线方向为x轴方向，与其垂直的方向为y轴，这样三个基站的坐标相对而言比较简单，可以使标签坐标的计算略微简化。(用UWB进行二维平面坐标的定位，至少需要3个基站；如果要进行三维坐标的定位，就至少需要四个基站，且四个基站不在同一平面上。其他定位技术，如GPS、北斗，通常只考虑三维定位，因此利用它们进行定位时，要求至少有四颗卫星进行工作。)坐标轴确定之后，进行轨迹的绘制。
在本实施例中，所述步骤S20之后还包括：
若所述无人船偏离预设的运行轨迹，则进入步骤S50，根据所述当前位置坐标与所述一级参考点坐标确定所述无人船进一步的运行方向。
实施例二
如图3所示，在本实施例中，所述步骤S50包括：
S51、将每间隔预设数量的所述一级参考点坐标标记为二级参考点坐标；
S52、根据所述当前位置坐标与所述一级参考点坐标、所述二级参考点坐标的位置关系确定所述无人船进一步的运行方向。
在本实施例中，将一级参考坐标标记为Cn,，其中n为正整数，即从第一个一级参考坐标到第n个一级参考坐标可分别标记为C1…Cn。
将每间隔预设数量的所述一级参考点坐标标记为二级参考点坐标，若所述预设数量为10，即每隔10个一级参考点坐标选取一个一级参考点坐标作为二级参考点坐标。具体地，将数组Cn每十个取一个出来形成一个新的数组Kn，则数组Kn即对应着二级参考点的坐标。这边应该是每隔10个，取出一个，比如说取C1，C11，C21…这种形式的，若将二级参考坐标标记为Kn,，其中n为正整数，即K1与C1相对应，K2与C11相对应，以此类推。
作为另一种实施例，所述预设数量可以为9或11，或其他任意个数量，若预设数量为9或11，即每隔9个或11个一级参考点坐标选取一个一级参考点坐标作为二级参考点坐标。如图4所示，在本实施例中，所述步骤S52包括：
S521、判断所述无人船当前所处轨迹段的轨迹状态；
S522、若所述无人船当前所处轨迹段为直线段轨迹，则获取与所述当前位置坐标相邻的下一个二级参考点坐标，使所述无人船向所述下一个二级参考点坐标运行；
S523、若所述无人船当前所处轨迹段为曲线段轨迹，则获取与所述当前位置坐标相邻的下一个一级参考点坐标，并计算得到所述下一个一级参考点坐标和下一个二级参考点坐标的中间坐标，使所述无人船向所述中间坐标运行。
在本实施例中，对直线段和曲线段采用不同的校正导航方法，解决了人为遥控或定位误差因素造成的S型曲线运动的问题。
如图5所示，在本实施例中，所述步骤S521包括：
S5211、获取与所述当前位置坐标前后相邻的两个二级参考点坐标；
S5212、计算所述相邻的两个二级参考点坐标所形成的直线的斜率；
S5213、根据计算所得的斜率值来判断所述无人船当前所处轨迹段的轨迹状态。
在本实施例中，所述S5213的判断方法具体为：
当-0.2＜Rn-1,n＜0.2时，确定所述无人船处于X轴直线段轨迹；
当(0.2＜Rn-1,n＜20&&xn＞xn-1)||(-20＜Rn-1,n＜-0.2&&xn＜xn-1)时，确定所述无人船处于左转弯状态；
当(0.2＜Rn-1,n＜20&&xn＜xn-1)||(-20＜Rn-1,n＜-0.2&&xn＞xn-1)时，确定所述无人船处于右转弯状态；
当(Rn-1,n＞20)||(Rn-1,n＜-20时，确定所述无人船处于Y轴直线段轨迹；
其中，Rn-1,n为所述相邻的两个二级参考点坐标所形成直线的斜率值，n为正整数，xn-1和xn对应为所述相邻的两个二级参考点坐标的X轴坐标；所述无人船处于X轴直线段轨迹和处于Y轴直线段轨迹为所述直线段轨迹的两种形式；所述无人船处于左转弯状态和处于右转弯状态为所述曲线段轨迹的两种形式。
通过上面的分析，若二级参考点坐标K1为起点，K2为与K1相邻的第二个二级参考点坐标，如要判断从K1到K2段的轨迹状态，则只需计算出K1到K2的直线对应的斜率为R12即可获取K1到K2段的轨迹航行状态，由于K1对应一级参考点坐标C1，K2对应C11，则C1到C10的坐标状态即对应K1到K2段的航行状态。如K1到K2段的状态是x直行，则C1、C2…C10的S值都对应的是x直行状态。继续对K2到K3段、K3到K4段…Kn到Kn+1段进行分析，即可获得最终扩展后的坐标集[C'n]＝[(xn,yn,sn)]。
作为另一种实施例，所述步骤S52具体为：
获取与所述当前位置坐标相邻的下一个二级参考点坐标，使所述无人船向所述下一个二级参考点坐标运行。
在本实施例中，不管当前无人船的轨迹状态是直线段还是曲线段，只要下一个预测点坐标与对应的预设的一级参考点坐标的偏差大于预设的第一阈值，都向下一个二级参考点坐标运行。
实施例三
如图6所示，在本实施例中，所述步骤S10包括：
S11、通过超宽带标签以预设的频率采集所述无人船运行轨迹上的原始坐标数组；
在本实施例中，可通过超宽带标签每秒向基站发送100次信息，三个基站将测得的与标签的距离信息通过wifi汇聚到后台处理器，处理器计算出标签每次发送信息时对应的坐标点，并存储到数据库中。本实施例中，由于考虑是在小范围水产养殖塘，如果GPS定位，误差10m左右，肯定行不通。而UWB定位精度高，cm级别(能做到15cm以内)的能满足养殖塘导航的要求，因此优选UWB定位技术进行定位。
考虑到小型无人船的速度不会太快，这里暂且假设最大速度为1m/s,如果忽略无线传输的时间，则每次间隔最大的距离为1cm。这里选择每隔10cm设置一个参考点，可以保证每走一步可以获取足够多的定位信息。首先，1m/s的速度肯定是能符合我们应用的要求了；其次，按1m/s的速度，现在每隔10cm差不多能收到10个定位数据，即每秒发送100个数据，也就是每运动1m，至少发送100个数据，每运动10cm，至少获得10个数据；可以理解的是，也可以将超宽带标签发送消息的次数及无人船速度设置为其他任意数值，如果小船速度慢了，将收到更多的数据，更能满足要求。
S12、从所述原始坐标数组中选取一个原始坐标作为第一个一级参考点坐标，沿着所述预设的运行轨迹方向，获取与所述第一个一级参考点坐标的距离为预设距离差值的至少两个原始坐标，计算所获取的所述至少两个原始坐标的平均值，将所述平均值定义为第二个一级参考点坐标，以此类推，将所有原始坐标数组搜索完毕，即获得一级参考点坐标数组。
在本实施例中，若定义原始坐标数组为Sn，预设距离差值为10cm，首先以S1为第一个一级参考坐标，标记为C1，然后以C1为参照，搜索下一个与C1距离10cm左右参考点标记为C2；以此类推，直至将原始数据全部搜索结束，找出所有的一级参考点坐标。
作为另一种实施例，所述第一个参考坐标也可以为S1之外的任意一个原始坐标。
在本实施例中，确定一级参考点坐标的方法以下述Cn+1为例进行说明：
本实施例，若上一个一级参考坐标Cn对应的原始坐标为Sm，若要确定Cn的下一个一级参考坐标Cn+1，并设置误差阈值为1cm，需从原始坐标数组中选出下一个原始坐标点Sm+1，并计算它们之间的距离：判断|d-10|<1是否成立，成立，则标记为T1，不成立，则忽略，并选出下一个原始坐标点Sm+2继续分析；如果连续找出多个成立的原始坐标点，则分别对它们进行记录，并标记为T1、T2…Tn。如果在找到一个或多个成立的原始坐标点之后，再判断这些点是否满足|d-10|<1，若不满足，则确定为不符合要求的坐标点；不符合要求和不满足成立条件的点都是指不符合判断标准的点，则继续选择下一个坐标点进行判断，直至发现连续3个不符合的坐标点，结束此轮搜索判断，以此来消除个别噪点的影响。最终，取数组Tn中数据的平均值作为下一个一级参考坐标点Cn+1。这样做的目的是要把所有距离上一个一级参考点的距离差为10cm，误差在1cm的点全部找出来，然后取这些所有原始坐标点坐标值的平均值作为下一个一级参考坐标点。通常和一级参考坐标点距离10cm左右的坐标点，肯定是连续的，也就是这几个连续点的原始坐标和前一一级参考坐标点|d-10|的值都是小于1的，但定位有时可能存在误差，比如说前五个都满足：|d-10|<1，而第六个由于定位误差|d-10|>1了，而后面的原始坐标点与上一个一级参考坐标点|d-10|的值还是小于1的；但是如果连续几个原始坐标点与上一个一级参考坐标点|d-10|都大于1了，基本上就可以保证后面的原始坐标点与上一个一级参考坐标点|d-10|也大于1，而不是由于误差导致的结果，因为产生大的误差，应该是小概率事件。
实施例四
在本实施例中，所述步骤S30具体为：
1)定义Xk为卡尔曼滤波器k时刻的状态向量也即所需要预测的无人船的下一个预测点，为k时刻的状态先验估计，根据k-1时刻的状态向量Xk-1也即无人船的当前坐标点，计算k时刻的状态先验估计公式为：Xk -＝AXk-1；并计算k时刻的误差协方差先验估计公式为：Pk -＝APk-1AT+Q；
2)计算卡尔曼滤波增益Kk，公式为：Kk＝Pk-HT(HPk-HT+R)-1；
3)定义Zk为观测向量，也即无人船在k时刻实际获取到的位置坐标，在本实施例中，具体为在k时刻实际获取到的位置坐标在x、y轴上的位置分量，根据Zk计算得到k时刻的状态向量也即状态后验估计Xk，公式为Xk＝Xk - +Kk(Zk-HXk -)；
4)将Xk根据公式Xk＝[px(k)py(k)vx(k)vy(k)]T以向量形式展开，其中，px(k)、py(k)分别是k时刻在x、y轴上的位置分量，vx(k)vy(k)分别是k时刻在x、y轴上的速度分量，根据px(k)、py(k)即可获得所需要预测的无人船的下一个预测点的坐标；
其中，A为状态转移矩阵，H为观测矩阵，
Q、R为协方差矩阵：
在本实施例中，当无人船位于直线段轨迹时，根据前三个轨迹点计算出初始运行轨迹，之后利用卡尔曼滤波来对预测点进行预测，如图7所示，卡尔曼滤波器用反馈控制的方法估计过程状态，可分为预测和校正两个部分，预测主要目的是为下一时刻构造先验估计，而校正是结合先验估计和新的测量变量得出改进后的后验估计。
如果预测的轨迹点与对应的一级参考点坐标偏差小于阈值，则往预测轨迹坐标点方向运动，否则，沿下一个二级参考点坐标方向运动。
整个运动过程是：每到一个一级参考点坐标，如果没有偏离，即进行一次预测。虽然由于偏离轨迹，当前状态可能是向二级参考点方向运动，但如果在到达下个一级参考点坐标后，无人船已经回到轨迹中来，即没有偏离，就又可以开始预测了。如果发现无人船偏离预设轨迹，则不进行预测轨迹，直接向下一个二级参考点方向运动。否则，按之前的预测方式进行运动。
预测的作用：总的来说就是，在无人船没有偏离运行轨迹时，用预测指导无人船运动，能让无人船运动的更加稳定；而当无人船偏离了运动轨迹，再预测就没有意义了，则直接根据当前位置坐标与一级参考点坐标、二级参考点坐标的位置关系确定无人船进一步的运行方向。
如图8所示，本实施例中，圆点为一级参考点坐标，矩形所示为无人船经过的位置，若预设距离差值为10cm，则每两个圆点(一级参考点坐标)之间相隔10cm，就如图8所示，无人船运动到A点，此时与它对应的参考y轴距离大于预设第二距离阀值，如在本实例中设置为10cm，如果此时直接向它的下一个一级参考点坐标方向运动，就可能导致图8的结果，这是调整过度的结果。此时设置为向它的二级参考点坐标方向运动，就得到图9的结果，则能取得较好的调整效果。
在本实施例中，当无人船位于曲线段轨迹时，首先根据前三个一级参考点坐标计算出下一时刻运动的方向，并向该方向进行运动。当无人船到达下一个一级参考点坐标对应位置时，利用卡尔曼预测出下一个轨迹点坐标，如果与对应一级参考点坐标偏差小于预设第一阈值，则往预测轨迹坐标点方向运动；否则，选出两个坐标，一个是当前一级参考点坐标对应的下一个一级参考点坐标，另个是下一个二级参考点坐标，得到这两个坐标的中点坐标，并以向它的方向为目标方向运动，采取与直线段不同的预测方式是因为这样可以让船在曲线段调整的更平滑一点，其处理的细节也是不一样的。对于无人船在曲线段是否到达参考点对应位置的判断依据如下：
当实际当前位置坐标点与一级参考点坐标的距离差值发生由小变大，即认为到达了对应位置，并判断距离值是否小于预设第三阀值，如设置为10cm。若该距离值小于预设第三阀值，则认为没有偏离设定的轨迹，否则，则认为发生偏离。
实施例五
如图10所示，一种无人船自主导航装置，包括：
轨迹绘制模块10，用于预设无人船的运行轨迹，采集无人船运行轨迹上的原始坐标，并通过所述原始坐标得到至少两个一级参考点坐标；
获取判断模块20，用于获取所述无人船的当前位置坐标，并根据所述当前位置坐标判断所述无人船是否偏离预设的运行轨迹；
预测模块30，用于当无人船未偏离预设的运行轨迹时，则利用卡尔曼滤波预测出下一个预测点坐标；
偏差判断模块40，用于判断所述下一个预测点坐标与对应的一级参考点坐标的偏差是否小于预设的第一阈值；
第一导航模块50，用于当所述下一个预测点坐标与对应的一级参考点坐标的偏差小于预设的第一阈值时，则使所述无人船向所述下一个预测点坐标运行；
第二导航模块60，用于当所述下一个预测点坐标与对应的一级参考点坐标的偏差大于预设的第一阈值时，根据所述当前位置坐标与所述一级参考点坐标确定所述无人船进一步的运行方向。
在本实施例中，所述第二导航模块60，还用于当所述无人船偏离预设的运行轨迹时，根据所述当前位置坐标与所述一级参考点坐标确定所述无人船进一步的运行方向。
如图11所示，在本实施例中，所述第二导航模块60包括：
标记单元61，用于将每间隔预设数量的所述一级参考点坐标标记为二级参考点坐标；
导航单元62，用于根据所述当前位置坐标与所述一级参考点坐标、所述二级参考点坐标的位置关系确定所述无人船进一步的运行方向。
如图12所示，在本实施例中，所述导航单元62包括：
轨迹状态判断单元621，用于判断所述无人船当前所处轨迹段的轨迹状态；
直线段导航单元622，用于当所述无人船当前所处轨迹段为直线段轨迹时，获取与所述当前位置坐标相邻的下一个二级参考点坐标，使所述无人船向所述下一个二级参考点坐标运行；
曲线段导航单元623，用于当所述无人船当前所处轨迹段为曲线段轨迹时，获取与所述当前位置坐标相邻的下一个一级参考点坐标，并计算得到所述下一个一级参考点坐标和下一个二级参考点坐标的中间坐标，使所述无人船向所述中间坐标运行。
作为另一种实施例，所述导航单元62包括：
统一导航单元，用于当所述无人船当前所处轨迹段为直线段轨迹或曲线段轨迹时，皆获取与所述当前位置坐标相邻的下一个二级参考点坐标，使所述无人船向所述下一个二级参考点坐标运行。
如图13所示，在本实施例中，所述轨迹绘制模块10包括：
原始坐标采集单元11，用于通过超宽带标签以预设的频率采集所述无人船运行轨迹上的原始坐标数组；
一级参考点坐标采集单元12，用于从所述原始坐标数组中选取一个原始坐标作为第一个一级参考点坐标，沿着所述预设的运行轨迹方向，获取与所述第一个一级参考点坐标的距离为预设距离差值的至少两个原始坐标，计算所获取的所述至少两个原始坐标的平均值，将所述平均值定义为第二个一级参考点坐标，以此类推，将所有原始坐标数组搜索完毕，即获得一级参考点坐标数组。
如图14所示，在本实施例中，所述轨迹状态判断单元621包括：
二级坐标获取单元6211，用于获取与所述当前位置坐标前后相邻的两个二级参考点坐标；
斜率计算单元6212，用于计算所述相邻的两个二级参考点坐标所形成的直线的斜率；
轨迹状态计算单元6213，用于根据计算所得的斜率值来判断所述无人船当前所处轨迹段的轨迹状态。
在本实施例中，所述轨迹状态计算单元6213的计算公式为：
当-0.2＜Rn-1,n＜0.2时，确定所述无人船处于X轴直线段轨迹；
当(0.2＜Rn-1,n＜20&&xn＞xn-1)||(-20＜Rn-1,n＜-0.2&&xn＜xn-1)时，确定所述无人船处于左转弯状态；
当(0.2＜Rn-1,n＜20&&xn＜xn-1)||(-20＜Rn-1,n＜-0.2&&xn＞xn-1)时，确定所述无人船处于右转弯状态；
当(Rn-1,n＞20)||(Rn-1,n＜-20时，确定所述无人船处于Y轴直线段轨迹；
其中，Rn-1,n为所述相邻的两个二级参考点坐标所形成直线的斜率值，n为正整数，xn-1和xn对应为所述相邻的两个二级参考点坐标的X轴坐标；所述无人船处于X轴直线段轨迹和处于Y轴直线段轨迹为所述直线段轨迹的两种形式；所述无人船处于左转弯状态和处于右转弯状态为所述曲线段轨迹的两种形式。
在本实施例中，所述预测模块的计算公式为：
1)定义Xk为卡尔曼滤波器k时刻的状态向量也即所需要预测的无人船的下一个预测点，为k时刻的状态先验估计，根据k-1时刻的状态向量Xk-1也即无人船的当前坐标点，计算k时刻的状态先验估计公式为：Xk -＝AXk-1；并计算k时刻的误差协方差先验估计公式为：Pk -＝APk-1AT+Q；
2)计算卡尔曼滤波增益Kk，公式为：Kk＝Pk -HT(HPk -HT+R)-1；
3)定义Zk为观测向量，也即无人船在k时刻实际获取到的位置坐标，根据Zk计算得到k时刻的状态向量也即状态后验估计Xk，公式为Xk＝Xk - +Kk(Zk-HXk -)；
4)将Xk根据公式Xk＝[px(k)py(k)vx(k)vy(k)]T以向量形式展开，其中，px(k)、py(k)分别是k时刻在x、y轴上的位置分量，vx(k)vy(k)分别是k时刻在x、y轴上的速度分量，根据px(k)、py(k)即可获得所需要预测的无人船的下一个预测点的坐标；
其中，A为状态转移矩阵，H为观测矩阵，
Q、R为协方差矩阵：
以上所述仅为本发明的具体实施方式，这些描述只是为了解释本发明的原理，而不能以任何方式解释为对本发明保护范围的限制。基于此处的解释，本领域的技术人员不需要付出创造性的劳动即可联想到本发明的其它具体实施方法，这些方式都将落入本发明的保护范围之内。

Claims (10)

1.一种无人船自主导航方法，其特征在于，包括：
预设无人船的运行轨迹，采集无人船运行轨迹上的原始坐标，并通过所述原始坐标得到至少两个一级参考点坐标；
获取所述无人船的当前位置坐标，并根据所述当前位置坐标判断所述无人船是否偏离预设的运行轨迹；
若所述无人船未偏离预设的运行轨迹，则利用卡尔曼滤波预测出下一个预测点坐标；
判断所述下一个预测点坐标与对应的一级参考点坐标的偏差是否小于预设的第一阈值，若是，则使所述无人船向所述下一个预测点坐标运行；
否则，根据所述当前位置坐标与所述一级参考点坐标确定所述无人船进一步的运行方向；
所述根据所述当前位置坐标及所述一级参考点坐标确定所述无人船进一步的运行方向具体为：
将每间隔预设数量的所述一级参考点坐标标记为二级参考点坐标；
根据所述当前位置坐标与所述一级参考点坐标、所述二级参考点坐标的位置关系确定所述无人船进一步的运行方向。
2.根据权利要求1所述的一种无人船自主导航方法，其特征在于，还包括：
若所述无人船偏离预设的运行轨迹，则根据所述当前位置坐标与所述一级参考点坐标确定所述无人船进一步的运行方向。
3.根据权利要求1或2所述的一种无人船自主导航方法，其特征在于，所述根据所述当前位置坐标与所述一级参考点坐标、所述二级参考点坐标的位置关系确定所述无人船进一步的运行方向具体为：
判断所述无人船当前所处轨迹段的轨迹状态；
若所述无人船当前所处轨迹段为直线段轨迹，则获取与所述当前位置坐标相邻的下一个二级参考点坐标，使所述无人船向所述下一个二级参考点坐标运行；
若所述无人船当前所处轨迹段为曲线段轨迹，则获取与所述当前位置坐标相邻的下一个一级参考点坐标，并计算得到所述下一个一级参考点坐标和下一个二级参考点坐标的中间坐标，使所述无人船向所述中间坐标运行。
4.根据权利要求1或2所述的一种无人船自主导航方法，其特征在于，所述根据所述当前位置坐标与所述一级参考点坐标、所述二级参考点坐标的位置关系确定所述无人船进一步的运行方向具体为：
获取与所述当前位置坐标相邻的下一个二级参考点坐标，使所述无人船向所述下一个二级参考点坐标运行。
5.根据权利要求1或2所述的一种无人船自主导航方法，其特征在于，所述采集无人船运行轨迹上的原始坐标，并通过所述原始坐标得到一级参考点坐标包括：
通过超宽带标签以预设的频率采集所述无人船运行轨迹上的原始坐标数组；
从所述原始坐标数组中选取一个原始坐标作为第一个一级参考点坐标，沿着所述预设的运行轨迹方向，获取与所述第一个一级参考点坐标的距离为预设距离差值的至少两个原始坐标，计算所获取的所述至少两个原始坐标的平均值，将所述平均值定义为第二个一级参考点坐标，以此类推，将所有原始坐标数组搜索完毕，即获得一级参考点坐标数组。
6.根据权利要求3所述的一种无人船自主导航方法，其特征在于，所述判断所述无人船当前所处轨迹段的轨迹状态具体为：
获取与所述当前位置坐标前后相邻的两个二级参考点坐标；
计算所述相邻的两个二级参考点坐标所形成的直线的斜率；
根据计算所得的斜率值来判断所述无人船当前所处轨迹段的轨迹状态。
7.根据权利要求6所述的一种无人船自主导航方法，其特征在于，所述根据计算所得的斜率值来判断所述无人船当前所处轨迹段的轨迹状态具体为：
当-0.2＜Rn-1,n＜0.2时，确定所述无人船处于X轴直线段轨迹；
当(0.2＜Rn-1,n＜20&&xn＞xn-1)||(-20＜Rn-1,n＜-0.2&&xn＜xn-1)时，确定所述无人船处于左转弯状态；
当(0.2＜Rn-1,n＜20&&xn＜xn-1)||(-20＜Rn-1,n＜-0.2&&xn＞xn-1)时，确定所述无人船处于右转弯状态；
当(Rn-1,n＞20)||(Rn-1,n＜-20) 时，确定所述无人船处于Y轴直线段轨迹；
其中，Rn-1,n为所述相邻的两个二级参考点坐标所形成直线的斜率值，n为正整数，xn-1和xn对应为所述相邻的两个二级参考点坐标的X轴坐标；所述无人船处于X轴直线段轨迹和处于Y轴直线段轨迹为所述直线段轨迹的两种形式；所述无人船处于左转弯状态和处于右转弯状态为所述曲线段轨迹的两种形式。
8.根据权利要求7所述的一种无人船自主导航方法，其特征在于，所述利用卡尔曼滤波预测出下一个预测点坐标具体为：
1)定义Xk为卡尔曼滤波器k时刻的状态向量也即所需要预测的无人船的下一个预测点，为k时刻的状态先验估计，根据k-1时刻的状态向量Xk-1也即无人船的当前坐标点，计算k时刻的状态先验估计公式为：Xk -＝AXk-1；并计算k时刻的误差协方差先验估计公式为：Pk -＝APk-1AT+Q；
2)计算卡尔曼滤波增益Kk，公式为：Kk＝Pk -HT(HPk -HT+R)-1；
3)定义Zk为观测向量，也即无人船在k时刻实际获取到的位置坐标，根据Zk计算得到k时刻的状态向量也即状态后验估计Xk，公式为Xk＝Xk -+Kk(Zk-HXk -)；
4)将Xk根据公式Xk＝[px(k)py(k)vx(k)vy(k)]T以向量形式展开，其中，px(k)、py(k)分别是k时刻在x、y轴上的位置分量，vx(k)vy(k)分别是k时刻在x、y轴上的速度分量，根据px(k)、py(k)即可获得所需要预测的无人船的下一个预测点的坐标；
其中，A为状态转移矩阵，H为观测矩阵，Q、R为协方差矩阵：
9.一种无人船自主导航装置，其特征在于，包括：
轨迹绘制模块，用于预设无人船的运行轨迹，采集无人船运行轨迹上的原始坐标，并通过所述原始坐标得到至少两个一级参考点坐标；
获取判断模块，用于获取所述无人船的当前位置坐标，并根据所述当前位置坐标判断所述无人船是否偏离预设的运行轨迹；
预测模块，用于当无人船未偏离预设的运行轨迹时，则利用卡尔曼滤波预测出下一个预测点坐标；
偏差判断模块，用于判断所述下一个预测点坐标与对应的一级参考点坐标的偏差是否小于预设的第一阈值；
第一导航模块，用于当所述下一个预测点坐标与对应的一级参考点坐标的偏差小于预设的第一阈值时，则使所述无人船向所述下一个预测点坐标运行；
第二导航模块，用于当所述下一个预测点坐标与对应的一级参考点坐标的偏差大于预设的第一阈值时，根据所述当前位置坐标与所述一级参考点坐标确定所述无人船进一步的运行方向；
所述第二导航模块包括：
标记单元，用于将每间隔预设数量的所述一级参考点坐标标记为二级参考点坐标；
导航单元，用于根据所述当前位置坐标与所述一级参考点坐标、所述二级参考点坐标的位置关系确定所述无人船进一步的运行方向。
10.根据权利要求9所述的一种无人船自主导航装置，其特征在于，所述第二导航模块，还用于当所述无人船偏离预设的运行轨迹时，根据所述当前位置坐标与所述一级参考点坐标确定所述无人船进一步的运行方向。
CN201510579857.8A 2015-09-11 2015-09-11 一种无人船自主导航方法及装置 Active CN105180942B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510579857.8A CN105180942B (zh) 2015-09-11 2015-09-11 一种无人船自主导航方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510579857.8A CN105180942B (zh) 2015-09-11 2015-09-11 一种无人船自主导航方法及装置

Publications (2)

Publication Number Publication Date
CN105180942A CN105180942A (zh) 2015-12-23
CN105180942B true CN105180942B (zh) 2018-07-20

Family

ID=54903193

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510579857.8A Active CN105180942B (zh) 2015-09-11 2015-09-11 一种无人船自主导航方法及装置

Country Status (1)

Country Link
CN (1) CN105180942B (zh)

Families Citing this family (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107278262B (zh) * 2016-11-14 2021-03-30 深圳市大疆创新科技有限公司 飞行轨迹的生成方法、控制装置及无人飞行器
CN107065858A (zh) * 2017-02-13 2017-08-18 四川超影科技有限公司 基于超宽带的巡检机器人导航方法
CN107703934A (zh) * 2017-08-24 2018-02-16 北京臻迪科技股份有限公司 一种无人船的控制方法及装置
CN108917752B (zh) * 2018-03-30 2022-11-11 深圳一清创新科技有限公司 无人船导航方法、装置、计算机设备和存储介质
CN109186605B (zh) * 2018-09-01 2022-03-18 哈尔滨工程大学 一种基于uwb室内定位的无人艇船载测速测向方法
CN109163726B (zh) * 2018-09-21 2020-12-18 江苏大学 一种回状的全覆盖轨迹规划方法
CN109874101A (zh) * 2019-04-15 2019-06-11 东北大学 一种基于卡尔曼滤波的超宽带室内定位系统及方法
CN110351752B (zh) * 2019-06-27 2021-11-30 珠海云洲智能科技股份有限公司 一种无人艇及其网络优化方法、装置和存储介质
CN110672075A (zh) * 2019-09-26 2020-01-10 暨南大学 一种基于三维立体成像的远程水域检测系统和方法
CN111679669B (zh) * 2020-06-01 2023-08-08 陕西欧卡电子智能科技有限公司 一种无人船自主精准停靠方法及系统
CN112465856A (zh) * 2020-11-18 2021-03-09 珠海大横琴科技发展有限公司 基于无人机的船只轨迹修正方法、装置及电子设备
TWI829992B (zh) * 2021-01-21 2024-01-21 鎮鑫科技股份有限公司 無人船偏移復位系統
CN114802616B (zh) * 2022-04-15 2023-08-04 浙江科技学院 小型水面清污无人艇及其控制方法、定位方法
CN115047868A (zh) * 2022-05-16 2022-09-13 江苏科技大学 一种无人船自主航行系统及其方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102176285A (zh) * 2011-02-28 2011-09-07 江苏怡和科技股份有限公司 视频流中的车辆行为模式判定方法
CN102788591A (zh) * 2012-08-07 2012-11-21 郭磊 基于视觉信息的机器人沿引导线巡线导航方法
EP2827210A1 (en) * 2013-07-15 2015-01-21 BAE Systems PLC Route planning
CN104567860A (zh) * 2014-12-29 2015-04-29 深圳市科松电子有限公司 一种机器人自主导航方法、装置及系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8275493B2 (en) * 2009-07-30 2012-09-25 Ruffa Anthony A Bow riding unmanned water-borne vehicle

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102176285A (zh) * 2011-02-28 2011-09-07 江苏怡和科技股份有限公司 视频流中的车辆行为模式判定方法
CN102788591A (zh) * 2012-08-07 2012-11-21 郭磊 基于视觉信息的机器人沿引导线巡线导航方法
EP2827210A1 (en) * 2013-07-15 2015-01-21 BAE Systems PLC Route planning
CN104567860A (zh) * 2014-12-29 2015-04-29 深圳市科松电子有限公司 一种机器人自主导航方法、装置及系统

Also Published As

Publication number Publication date
CN105180942A (zh) 2015-12-23

Similar Documents

Publication Publication Date Title
CN105180942B (zh) 一种无人船自主导航方法及装置
CN105704652B (zh) 一种wlan/蓝牙定位中的指纹库采集和优化方法
CN106912018B (zh) 基于信令轨迹的地图匹配方法及系统
CN103869824B (zh) 基于生物触角模型的多机器人水下目标搜寻方法及装置
CN108508913B (zh) 基于数据驱动的自主式水下航行器海底路径规划方法
CN110933599B (zh) 一种融合uwb与wifi指纹的自适应定位方法
CN105372628A (zh) 一种基于Wi-Fi的室内定位导航方法
CN106291517A (zh) 基于位置与视觉信息优化的室内云机器人角度定位方法
CN109672973A (zh) 一种基于最强ap法的室内定位融合算法
CN110035384A (zh) 一种融合多传感器信号滤波优化的室内定位方法
CN109459052B (zh) 一种扫地机全覆盖路径规划方法
CN112881979B (zh) 一种基于ekf滤波的初始状态自适应融合定位方法
CN107421527A (zh) 一种基于磁场信息与运动传感器的室内定位方法
CN109511085A (zh) 一种基于MeanShift和加权k近邻算法的UWB指纹定位方法
CN110187372A (zh) 一种低速无人车园区内组合导航方法及系统
CN106772466B (zh) 一种基于形状特征搜索的近地卫星目标自动捕获算法
EP3696574A1 (en) Robotic vehicle for soil cultivation
CN109657928B (zh) 一种车载传感器系统的闭环协同调度框架的协同调度方法
CN102914786B (zh) 水下自主航行器auv自主定位方法
CN107843260A (zh) 一种基于电磁指纹信息的低空无人飞行器定位导航方法
CN111007460B (zh) 直升机协同定位方法
CN103905994A (zh) 一种基于容错思想的室内Wi-Fi指纹定位方法
Chen et al. Deep neural network based on feature fusion for indoor wireless localization
CN107659918B (zh) 一种智能跟随的方法及系统
CN103220778A (zh) 一种基于无线传感器网络的移动节点队形变换方法及实现装置

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CP03 Change of name, title or address

Address after: 518000 room 1306, Press Plaza, Shennan Avenue, Futian District, Shenzhen, Guangdong

Patentee after: ANKE ROBOT CO.,LTD.

Address before: 518054 room 1306, Press Plaza, Shennan Avenue, Futian District, Shenzhen, Guangdong

Patentee before: ANKE SMART CITY TECHNOLOGY (PRC) Co.,Ltd.

CP03 Change of name, title or address
TR01 Transfer of patent right

Effective date of registration: 20180904

Address after: 518000 Shenzhen, Guangdong Guangming New District Guangming Street Wan Dai Heng hi tech Industrial Park 5, 5 floor.

Patentee after: SHENZHEN ZHONGZHI KECHUANG ROBOT Co.,Ltd.

Address before: 518000 room 1306, Press Plaza, Shennan Avenue, Futian District, Shenzhen, Guangdong

Patentee before: ANKE ROBOT CO.,LTD.

TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20210517

Address after: 518000 Room 201, building a, No.1 Qianhai 1st Road, Qianhai Shenxiang cooperation zone, Shenzhen City, Guangdong Province

Patentee after: Shenzhen Zhongzhi Weian Robot Technology Co.,Ltd.

Address before: 518000 Shenzhen, Guangdong Guangming New District Guangming Street Wan Dai Heng hi tech Industrial Park 5, 5 floor.

Patentee before: SHENZHEN ZHONGZHI KECHUANG ROBOT Co.,Ltd.

TR01 Transfer of patent right