一种无人艇安全航行船位推算方法.pdf

本发明提供的是一种无人艇安全航行船位推算方法。步骤一，获取无人艇数据；步骤二，利用最小二乘法多项式拟合原理对获取的原始数据进行滤波，消除野值；步骤三，进行强跟踪卡尔曼滤波；步骤四，从GPS信号丢失时刻的位置开始，根据该点的航行速度、航行方向和航行时间对下一时刻的位置信息进行推算。本发明能够实现对无人艇获取的数据信息进行基于最小二乘法的数据预处理；能够实现无人艇在GPS信号丢失的情况下，采用基于强跟踪卡尔曼滤波的船位推算方法对下一时刻位置信息进行推测。本发明的方法可以对危险进行规避，具有实用性。

1.一种无人艇安全航行船位推算方法，其特征是：
步骤一，获取无人艇数据；
步骤二，利用最小二乘法多项式拟合方法对获取的原始数据进行滤波，消除野值；
步骤三，进行强跟踪卡尔曼滤波；
步骤四，从GPS信号丢失时刻的位置开始，根据该点的航行速度、航行方向和航行时间
对下一时刻的位置信息进行推算。
2.根据权利要求1所述的无人艇安全航行船位推算方法，其特征是所述消除野值的具
体步骤为：
①对已知的测量数据(xi,yi)，i＝1,2,...,n，选取合适的次数n进行最小二乘多项式拟
合；
其中xi为海流测量时间，yi为海流速度大小；
②求出系数矩阵α0,α1,…,αn，得到拟合多项式
③通过求得的多项式推算出相对应的拟合值pi，i＝1,2,...,n；
④将求出的拟合值与实际值做差，生成拟合残差序列y＝pi-yj，j＝1,2,...n，
⑤计算拟合残差序列的均方误差σ：
$\mathrm{\σ}=\sqrt{\frac{y_1^2+y_2^2+\mathrm{......}+y_n^2}{n}}=\sqrt{\frac{{\mathrm{\Σy}}_j^2}{n}}$
⑥用3σ准则判断并剔除野值,剔除野值后的数据为yj，j＝1,2,...,m,m≤n：

3.根据权利要求2所述的无人艇安全航行船位推算方法，其特征是所述强跟踪卡尔曼
滤波的滤波器具有如下一般结构:
$\left\{\begin{array}{c}\hat{X}(k+1)=\hat{X}(k+1/k)+K(k+1)V(k+1)\\ V(k+1)=Y(k+1)-H(k+1)\hat{X}(k+1/k)\end{array}\right.$
其中k——采样时刻
Y(k+1)——为滤波器的量测值；
V(k+1)——为估计残差；
K(k+1)——为时变增益阵；
通过引入渐消因子λ(k+1)来实现强跟踪滤波器的特性。
4.根据权利要求3所述的无人艇安全航行船位推算方法，其特征是所述根据该点的航
行速度、航行方向和航行时间对下一时刻的位置信息进行推算具体包括：
设已知起始位置已知，无人艇根据无人艇的运动参数经过系统的采样时间，计算得到
无人艇下一时刻的位置，起始位置用(x0,y0)表示，通过GPS得到在tn时刻的位置用(xn,yn)来
表示，则无人艇载体航行速度在正东方向和正北方向的分量如下：
$\left\{\begin{array}{c}{v}_E=v_x{\mathrm{sin\ψ}}_i-v_y{\mathrm{cos\ψ}}_i\\ v_F=v_x{\mathrm{cos\ψ}}_i+v_y{\mathrm{sin\ψ}}_i\end{array}\right.$
结合图2通过下式得到：
$\left\{\begin{array}{c}{x}_n=x_0+\underset{i=0}{\overset{n-1}{\Σ}}{\mathrm{\Δx}}_i=x_0+\underset{i=0}{\overset{n-1}{\Σ}}(v_x{\mathrm{sin\ψ}}_i-v_y{\mathrm{cos\ψ}}_i)T\\ y_n=y_0+\stackrel{n-1}{\mathrm{\Σ}}{\mathrm{\Δy}}_i=y_0+\underset{i=0}{\overset{n-1}{\Σ}}(v_x{\mathrm{cos\ψ}}_i-v_y{\mathrm{sin\ψ}}_i)T\end{array}\right.$
式中vE、vN——分别为载体航行速度在正东方向和正北方向的分量；
vx、vy——分别为DVL测得的载体相对大地的前向速度和左向速度；
ψi——无人艇载体的航向角,顺时针为正，逆时针为负，可由罗经测得。
Δx,Δy——分别表示在时刻ti-1和ti单位时间内无人艇分别在X轴和Y轴方向上的位移
差；
T——船位推算的时间间隔；
利用强跟踪卡尔曼滤波后：
$\left\{\begin{array}{c}{x}_{n+1}=x_n+T\·u_{\left(x\right)}cos\mathrm{\ψ}+T\·v_{\left(x\right)}\sin \mathrm{\ψ}\\ y_{n+1}=y_n+T\·u_{\left(x\right)}cos\mathrm{\ψ}+T\·v_{\left(x\right)}\sin \mathrm{\ψ}\end{array}\right.$
则状态方程为：
$\left[\begin{array}{c}{x}_{n+1}\\ y_{n+1}\\ u_{(x+1)}\\ v_{(x+1)}\end{array}\right]=\left[\begin{array}{cccc}1& 0& T\·cos\mathrm{\ψ}& T\·sin\mathrm{\ψ}\\ 0& 1& T\·cos\mathrm{\ψ}& T\·sin\mathrm{\ψ}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\·\left[\begin{array}{c}{x}_n\\ y_n\\ u_{\left(x\right)}\\ v_{\left(x\right)}\end{array}\right].$
5.根据权利要求4所述的无人艇安全航行船位推算方法，其特征是获取无人艇数据包
括利用姿态传感器测量船的航向、回转率、纵摇、横摇、垂向加速度；利用位置参考系统测量
无人艇的位置；利用风传感器测量风速和风向器。

一种无人艇安全航行船位推算方法

技术领域

本发明涉及的是一种无人艇安全航行船位推算方法。

背景技术

无人艇作为一种新型高性能船，随着其在航运和科学研究等领域越来越广泛的应
用，各种各样的无人艇也应运而生。因为无人艇通常在较高速度下航行，所以其容易受到恶
劣环境的影响，稳定性能不好，在航行轨迹上产生偏离导致实际航行路线和预期航行轨迹
出现误差。GPS信号用来对船舶进行定位，针对其航行误差进行修正，但是在环境干扰下容
易导致无人艇的GPS信号数据丢失，若在GPS信号丢失的情况下船舶面临碰撞，触礁等危险
情况时，无人艇很难进行避让。现阶段主要是通过基于卡尔曼滤波进行船位推算来对航行
轨迹进行预估，船位推算指的是通过无人艇之前的历史航行数据参数(实船的航行方向、航
行速度以及航行时间等)来进行船体位置推算，将推算出来的结果当成信号丢失时刻GPS信
号所对应的航行运动参数，这样就可以使船舶正常行驶。

发明内容

本发明的目的在于提供一种可以在GPS信号丢失情况下推算出无人艇的安全航行
船位的无人艇安全航行船位推算方法。

本发明的目的是这样实现的：

步骤一，获取无人艇数据；

步骤二，利用最小二乘法多项式拟合原理对获取的原始数据进行滤波，消除野值；

步骤三，进行强跟踪卡尔曼滤波；

步骤四，从GPS信号丢失时刻的位置开始，根据该点的航行速度、航行方向和航行
时间对下一时刻的位置信息进行推算。

本发明还可以包括：

1、所述消除野值的具体步骤为：

①对已知的测量数据(xi,yi)(i＝1,2,...,n)，选取合适的次数n进行最小二乘多
项式拟合。其中xi为海流测量时间，yi为海流速度大小；

②求出系数矩阵α0,α1,…,αn，得到拟合多项式

③通过求得的多项式推算出相对应的拟合值pi(i＝1,2,...,n)；

④将求出的拟合值与实际值做差，生成拟合残差序列y＝pi-yj(j＝1,2,...n)，

⑤计算拟合残差序列的均方误差σ：

⑥用3σ准则判断并剔除野值,剔除野值后的数据为yj(j＝1,2,...,m,m≤n)：

2、所述强跟踪卡尔曼滤波的滤波器具有如下一般结构:

其中k——采样时刻；

Y(k+1)——为滤波器的量测值；

V(k+1)——为估计残差；

K(k+1)——为时变增益阵。

最后通过引入渐消因子λ(k+1)来实现强跟踪滤波器的特性。

3、所述根据该点的航行速度、航行方向和航行时间对下一时刻的位置信息进行推
算具体包括：

设已知起始位置已知，无人艇根据无人艇的运动参数经过系统的采样时间，计算
得到无人艇下一时刻的位置，起始位置用(x0,y0)表示，通过GPS得到在tn时刻的位置用(xn,
yn)来表示，则无人艇载体航行速度在正东方向和正北方向的分量如下：

结合图2通过下式得到：

式中vE、vN——分别为载体航行速度在正东方向和正北方向的分量；

vx、vy——分别为DVL测得的载体相对大地的前向速度和左向速度；

ψi——无人艇载体的航向角,顺时针为正，逆时针为负，可由罗经测得。

Δx,Δy——分别表示在时刻ti-1和ti单位时间内无人艇分别在X轴和Y轴方向上
的位移差；

T——船位推算的时间间隔；

利用强跟踪卡尔曼滤波后：

则状态方程为：

4、获取无人艇数据包括利用姿态传感器测量船的航向、回转率、纵摇、横摇、垂向
加速度；利用位置参考系统测量无人艇的位置；利用风传感器测量风速和风向器。

本发明提供了一种无人艇可以在GPS信号丢失情况下安全航行的船位推算方法。
主要包括：

1、无人艇数据获取

无人艇由外壳、推进器、DVL、姿态传感器、罗经、GPS和控制计算机组成，在运动时
可通过以下几种系统实时获取数据：利用姿态传感器测量船的航向、回转率、纵摇、横摇、垂
向加速度；利用位置参考系统测量无人艇的位置；利用风传感器测量风速和风向器。

2、数据预处理

利用最小二乘法多项式拟合原理对获取的原始数据进行滤波，消除野值。

3、强跟踪卡尔曼滤波

利用卡尔曼滤波原理，对无人艇设计一个强跟踪卡尔曼滤波，根据此方法可以对
无人艇进行船位推算。

4、无人艇位置信息的推算

无人艇在GPS信号丢失的情况下为了正常安全航行，从GPS信号丢失时刻的位置开
始，根据该点的航行速度、航行方向和航行时间对下一时刻的位置信息进行合理推算。

5、模拟无人艇航行

通过Matlab软件模拟GPS信号正常传输和丢失情况下无人艇的航行情况，分析基
于强跟踪卡尔曼滤波的船位推算方法是否适用与无人艇在水面的航行。

本发明相对现有技术具有如下的优点及效果：

1.本发明能够实现对无人艇获取的数据信息进行基于最小二乘法的数据预处理；

2.本发明能够实现无人艇在GPS信号丢失的情况下，采用基于强跟踪卡尔曼滤波
的船位推算方法对下一时刻位置信息进行推测。

3.通过仿真可以看出此方法可以对危险进行规避，具有实用性。

附图说明

图1是本发明的无人艇船位推算系统图；

图2是本发明的无人艇位置推算示意图；

图3是本发明的无人艇的GPS信号正常传输时的航迹图。

具体实施方式

下面结合附图举例对本发明作进一步的描述。

附图2是本发明的无人艇位置推算示意图，已知起始位置(x0,y0)、总速度Vi和对应
的大地坐标系下的航向角ψi，可以推测下一时刻的位置信息。

无人艇运动学方程以向量形式表示：

其中位置向量为η＝[x y ψ]T。通常在描述船舶或半潜平台的水面运动时只需用
到纵荡、横荡和艏摇三个方向的运动，因此简化后的无人艇三自由度运动学方程如下：

式中x，y——无人艇在横纵轴位置

u，v——无人艇在横纵方向的速度

r——无人艇的艏摇角速度

ψ——无人艇的艏摇角度

利用无人艇的运动数学模型模拟实船的航行情况，为后续的研究提供理论基础。

本发明的无人艇安全航行船位推算方法主要包括如下步骤：

步骤一：无人艇数据获取

利用姿态传感器测量船的航向、回转率、纵摇、横摇、垂向加速度；位置参考系统包
括GPS，北斗导航系统，利用位置参考系统测量无人艇的位置；利用风传感测量风速和风向。

步骤二：数据预处理

无人艇在水面航行的时候，由于仪器的自噪声以及姿态、位置等传感器的误差，特
别是海面上的风浪对无人艇的航行有较为明显的影响，使得获取的数据中有异常数据(野
值)和随机误差。为了保证得到的数据更接近真实数据，则必须对这些原始数据进行滤波，
消除野值，并对测量的随机误差进行平滑修正，提高数据的精度。

利用最小二乘法多项式拟合原理对获取的原始数据进行滤波，需要对已知的观测
数据拟合一个多项式，再利用三倍拟合残差序列的均方误差准则进行判别剔除，较其它方
法易于操作。该方法对数据野值剔除步骤如下：

①利用步骤一中的测量值(xi,yi)(i＝1,2,...,n)，选取合适的次数n进行最小二
乘多项式拟合，其中xi为海流测量时间，yi为海流速度大小；

②求出系数矩阵α0,α1,…,αn，得到拟合多项式

③通过求得的多项式推算出相对应的拟合值pi(i＝1,2,...,n)；

④将求出的你何止与实际值做差，生成拟合残差序列y＝pi-yj(j＝1,2,...n)，

⑤计算拟合残差序列的均方误差σ：

⑥用3σ准则判断并剔除野值,剔除野值后的数据为yj(j＝1,2,...,m,m≤n)：

根据式(2)可知，若残差值小于门限值，为正常值，此值不变；若大于或等于门限
值，则判断为野值，用与其相邻的六个数据的均值代替此野值。门限值3σ可根据实验具体情
况做出适当的更改，注意在第一步求解过程中多项式系数n的确定需要结合实际的数据情
况，选取最终拟合曲线结果最好的一组。

采用最小二乘法拟合的数据预处理方法对采集数据中的野值进行提出，保证采集
数据更符合真实情况，减小误差对实船航行的影响。

步骤三：强跟踪卡尔曼滤波

卡尔曼滤波的滤波可以分为两个部分，分别是状态空间模型和观测模型。状态模
型可以通过状态方程来形容相近两时刻内的状态变化规律，是用来对状态变化规律进行反
映的模型。用观测模型来对真实观测量和状态变量的关系进行反映。强跟踪滤波器的概念,
它与通常的滤波器相比具有以下优良特性:

1)较强的关于模型参数失配的鲁棒性.

2)较低的关于噪声及初值统计特性的敏感性.

3)极强的关于突变状态的跟踪能力,并在滤波器达到稳态时保持这种能力.

4)适中的计算复杂性

一类强跟踪卡尔曼滤波器应具有如下一般结构:

式中k——采样时刻；

Y(k+1)——为滤波器的量测值；

V(k+1)——为估计残差；

K(k+1)——为时变增益阵。

最后通过引入渐消因子λ(k+1)来实现强跟踪滤波器的特性。

卡尔曼滤波问题就是利用线性系统状态方程来进行最优估计。

步骤四：无人艇位置信息的推算

无人艇航行时经常使用基于多普勒测速仪(DVL)、惯性测量单元(IMU)和全球定位
系统(GPS)数据的传统航位推算算法。DVL测得的速度一般是船体坐标系下的速度，为了推
算无人艇航行时载体的经纬度，需要把船体坐标系下无人艇的速度转化为北东坐标系下的
速度。假设已知起始位置已知，无人艇可以根据无人艇的运动参数(船体的航行方向以及船
体的航行速度)经过系统的采样时间(0.3～0.5秒)计算得到无人艇下一时刻的位置。起始
位置用(x0,y0)表示，通过GPS可以得到在tn时刻的位置，该时刻的位置用(xn,yn)来表示。

无人艇载体航行速度在正东方向和正北方向的分量计算如下：

结合图2通过下式得到：

式中vE、vN——分别为载体航行速度在正东方向和正北方向的分量；

vx、vy——分别为DVL测得的载体相对大地的前向速度和左向速度；

ψi——无人艇载体的航向角,顺时针为正，逆时针为负，可由罗经测得。

Δx,Δy——分别表示在时刻ti-1和ti单位时间内无人艇分别在X轴和Y轴方向上
的位移差；

T——船位推算的时间间隔；

如果想利用强跟踪卡尔曼滤波对船位推算进行过预测，可以先推导出正确的状态
方程和测量方程。根据船位推算的原理方程式(7)，可以得到最终结果方程式：

则状态方程为：

利用本发明的推算结果模拟无人艇航行

GPS信号用来对船舶位置进行定位，在GPS信号正常传输的情况下，无人艇根据GPS
信号给出的运动参数可以正常按照预定的航行轨迹航行(如图3所示)。假设当在A点处GPS
信号丢失，无人艇运行的位置无法确定，此时可以通过船位推算针对初始使其无人艇的航
行方向、速度以及时间进行估计下一时刻无人艇的位置，从无人艇GPS信号丢失到恢复GPS
信号的过程中无人艇能够平稳的按照预期的估计滑行。经过船位推算后的曲线和GPS信号
正常传输时候的航行曲线是没有太明显的差异的，也可以理解为在GPS信号丢失的时候可
以根据基于强跟踪卡尔曼滤波的船位推算原理对下一时间的航行位置进行估计，防止GPS
信号丢失到恢复的过程中无人艇无法航行或者出现其他危险。通过仿真验证，可以确定选
择此方法可以使无人艇安全航行，在GPS信号丢失时间内让无人艇可以按照预期的轨迹航
行。