0 引言

计算机技术的发展带动了图像技术和视频跟踪技术的发展，各种新型的技术被用于物体的运动跟踪和行为预测领域^[1-2]。而卡尔曼算法在运动信息的处理上，具有非常明显的优势，采用此算法的跟踪技术，能够非常有效地克服非线性干扰带来的影响，同时也能够加强运动行为的预测能力，本文重点研究此算法在船舶航行轨迹预测中的应用。

1 卡尔曼滤波状态的分析

对航迹行为建立基于卡尔曼滤波的状态方程：

$ {X_k} = {\mathit{\boldsymbol{A}}}{X_{k- 1}} + {\mathit{\boldsymbol{B}}}{U_k} + {W_k}, $

(1)

上述表达式中，X_k表示t=k时的系统状态，为M × 1的向量，M ×M维矩阵A表示状态转移矩阵，该转移矩阵表示该系统从t=n时的状态到n + 1的状态之间的转移关系，此变量是已知的；M ×M维矩阵B表示动态系统的参数；W_k表示噪声大小。

同时，矩阵A拥有3个重要特征。

1）状态分布的转移特性：

$ {{\mathit{\boldsymbol{A}}}_{a,b}} = {{\mathit{\boldsymbol{A}}}_{a,s}} \times {{\mathit{\boldsymbol{A}}}_{s,b}}, $

(2)

式中，A_{a, b}为从a状态到b状态的转移矩阵；A_{a, s}为从状态a到s的转移矩阵；s为转移过程中的中间状态；A_{s, b}为从状态s到b的转移矩阵。

2）状态转移的互逆性：

$ {{\mathit{\boldsymbol{A}}}_{a,b}} = {{\mathit{\boldsymbol{A}}}_{b,a}}^{- 1}, $

(3)

上面的互逆方程中，A_{a, b}为从状态a转移到b状态的转移矩阵。A_{b, a}为从状态b变换到a状态的矩阵。

3）在相同时刻，状态转移矩阵具有相对不变性：

$ {{\mathit{\boldsymbol{A}}}_{a,a}} = {\mathit{\boldsymbol{I}}}。 $

(4)

式中，A_{a, a}为从a状态变换到a状态的矩阵。其中I为单位矩阵。

2 船舶航行轨迹的滤波处理和行为预测 2.1 非线性系统的建立

由于船舶的航迹运动轨迹受到多种非线性因素的影响，因此可以转化简单的线性化处理方程，通过泰勒级数展开，将系统的状态方程进行变换：

$ \begin{array}{l} f\left( x \right) = \sum\limits_{n = 0}^\infty {\frac{{{f^{(n)}}(\bar x)\Delta {x^n}}}{{n!}}} = \\ f\left( {\bar x} \right) + f'\left( {\bar x} \right)\Delta x + f''\left( {\bar x} \right)\Delta {x^2}/2 + \cdots 。 \end{array} $

(5)

其中，$\Delta x = x- \overline x $，${f^{(n)}}(\overline x )$为f（x）在$x = \overline x $时的n阶导数。

上述的化简步骤，可以认为，在x=0处展开泰勒级数f（x）=cos（x）。此时，cos（x）的导数为-sin（x）；sin（x）的导数即为cos（x）^[3-4]。此时可以把cos（x）的泰勒展开式描述成

$ \cos \left( x \right) = \cos \left( {\overline x } \right)- \sin \left( {\overline x } \right)\Delta x- \cos \left( {\overline x } \right)\Delta {x^2} + \cdots 。 $

(6)

若让cos（x）在x=0处进行泰勒级数的展开，令$\overline x = 0$，$\Delta x = x- \overline x = x$，则cos（x）的泰勒级数式变换成：

$ \begin{array}{l} \cos \left( x \right) = \cos \left( 0 \right)- \sin \left( x \right)x- \cos \left( 0 \right){x^2}/2 + \cdots = \\ 1- {x^2}/2 + \cdots 。 \end{array} $

(7)

对cos（x）进行2阶泰勒级数的展开，cos（x）等于1-x²/2，通过对其他高次项的展开，可以发现高次项对整个系统的影响非常小。

2.2 扩展卡尔曼滤波

下面建立航行轨迹预测系统的有关方程：

系统的状态方程为${x_{k + 1}} = f\left( {{x_k},{u_k}} \right) + {w_k}$。

预测系统的输出方程为${y_k} = h\left( {{x_k}} \right) + {v_k}$。

对上述的每一步计算设置相应的导数矩阵与状态估计矩阵。

$ \begin{array}{l} {{\mathit{\boldsymbol{A}}}_k} = f'\left( {\widehat {{x_k}},{u_k}} \right),\\ {{\mathit{\boldsymbol{C}}}_k} = h'\left( {\widehat {{x_k}}} \right)。 \end{array} $

(8)

进行卡尔曼滤波方程的变换，得到：

$ \begin{array}{*{20}{l}} {{{\mathit{\boldsymbol{K}}}_k} = {{\mathit{\boldsymbol{P}}}_k}{{\mathit{\boldsymbol{C}}}^{\rm{T}}}{{\left( {{\mathit{\boldsymbol{C}}}{{\mathit{\boldsymbol{P}}}_k}{{\mathit{\boldsymbol{C}}}^{\rm{T}}} + {\mathit{\boldsymbol{R}}}} \right)}^{- 1}}},\\ {\widehat {{x_{k + 1}}} = f\left( {\widehat {{x_k}},{u_k}} \right) + {{\mathit{\boldsymbol{K}}}_k}\left[ {{y_k}- h\left( {\widehat {{x_k}}} \right)} \right]},\\ {{{\mathit{\boldsymbol{P}}}_{k + 1}} = {\mathit{\boldsymbol{A}}}\left( {{\mathit{\boldsymbol{I}}}- {{\mathit{\boldsymbol{K}}}_k}{\mathit{\boldsymbol{C}}}} \right){{\mathit{\boldsymbol{P}}}_k}{{\mathit{\boldsymbol{A}}}^{\rm{T}}} + {\mathit{\boldsymbol{Q}}}}。 \end{array} $

(9)

上述方程，实现了从非线性状态到线性状态的转换，因此卡尔曼滤波在加快预测数据的处理和抑制噪声干扰方面具有非常明显的优势^[5-6]。

2.3 船舶运动规律仿真

假设仿真系统中的船舶处于加速度运动状态，整个运动系统的状态向量为：

$ {x_n} = \left[ {{x_n},{v_n},{a_n}} \right], $

(10)

式中，n为运动轨迹的采样点。其中加速度、速度和空间位移之间的关系表示为

$ {v_t} = {v_0} + at,s = {v_0}t + \frac{1}{2}a{t^2}, $

(11)

变换得到

$ \left\{ {\begin{array}{*{20}{c}} {{x_{n + 1}} = {x_n} + {v_n}T + \frac{1}{2}{a_n}{T^2}},\\ {{v_{n + 1}} = {v_n} + {a_n}T},\\ {a{}_{n + 1} = {a_n}}。 \end{array}} \right. $

(12)

运动系统的状态转移矩阵为：${\mathit{\boldsymbol{A}}}\!=\!\left[\! {\begin{array}{*{20}{c}} 1 & T & {\frac{1}{2}{T^2}}\\ 0 & 1 & T\\ 0 & 0 & 1 \end{array}}\!\right],$

输出方程为：${y_n} = {x_n} + \sqrt {\mathit{\boldsymbol{Q}}} $，

观测矩阵表示为：C=E，

系统的噪声协方差矩阵为：E=Q，

其中，设置系统的初始协方差：p=1。

如图 1所示为船舶航行系统中的加速度预测曲线，其中绿线表示卡尔曼最优估计曲线，虚线表示理论速度。可以发现两者和实际测量的结果非常接近。同理，如图 2所示为船舶的位移仿真曲线，同样采用卡尔曼滤波实现了较好的预测和跟随。

3 结语

在船舶驾驶系统高度自动化和智能化的今天，人们对各种航行轨迹预测技术非常依赖，因此本文提出的基于卡尔曼滤波算法的轨迹预测技术，在一定程度上满足了船舶工业的控制需求，也有利于航运事业的发展。