航迹预测装置、航迹预测方法及航迹预测程序与流程

本发明涉及对移动体的移动位置进行预测的航迹预测装置、航迹预测方法及航迹预测程序。

背景技术：

以往，各种具有显示预测航迹的功能的导航装置被考虑且被实用化。以往的导航装置对从当前时刻到预测目标时刻的时间乘以当前的速度从而计算距离。以往的导航装置将该计算的距离加到当前位置上，从而计算预测位置。并且，以往的导航装置通过依次反复进行该预测位置的计算，从而计算预测航迹。

作为这样的导航装置，专利文献1中记载的导航装置(船舶用显示装置)还使用加速度的双重积分来计算预测位置。

现有技术文献

专利文献

专利文献1：日本特许第0428636号说明书

技术实现要素：

发明要解决的课题

但是，在专利文献1中记载的导航装置中，由于使用对时间的平方乘以1/2再乘以加速度后的值，加速度的观测误差等导致的噪声成分增大。由此，预测位置及预测航迹的误差变大。

从而，本发明的目的在于，提供高精度地计算预测位置的航迹预测装置、航迹预测方法及航迹预测程序。

用于解决课题的手段

本发明的航迹预测装置具备水平对地速度计算部、角速度计算部及预测位置计算部。水平对地速度计算部根据移动体的姿势角、对地去路及对地船速而计算水平对地速度。角速度计算部对移动体的角速度进行计测或者计算。预测位置计算部使用从当前时刻至预测目标时刻为止的时间和水平对地速度，若角速度超过转动检测阈值则使用角速度的积分运算来计算预测位置。

根据该结构，在移动体正在进行旋转运动时，使用所取得的角速度的积分值来计算预测位置。

发明效果

根据本发明，能够高精度地计算预测位置。

附图说明

图1是本发明的第一实施方式所涉及的航迹预测装置的框图。

图2是用于说明本发明的第一实施方式的预测位置的计算概念的图。

图3是对本实施方式的航迹预测装置和比较例的航迹预测装置的航迹预测结果进行了模拟的图。

图4是示出了本实施方式的航迹预测装置和比较例的航迹预测装置的预测航迹相对于实际航迹的标准差的图。

图5是本发明的实施方式所涉及的航迹预测的流程图。

图6是本发明的实施方式所涉及的预测位置的计算的流程图。

图7是本发明的第二实施方式所涉及的航迹预测装置的框图。

具体实施方式

关于本发明的第一实施方式所涉及的航迹预测装置、航迹预测方法及航迹预测程序，参照附图进行说明。另外，以下示出使用船舶作为移动体的方式，但也能够对于其他水上、水中移动体、陆上移动体、或者空中移动体应用本申请发明的结构。

图1是本发明的第一实施方式所涉及的航迹预测装置的框图。图2是用于说明本发明的第一实施方式的预测位置的计算概念的图。

如图1所示，航迹预测装置10具备当前位置计算部20、角速度计算部30、姿势角计算部40、对地去路计算部50、对地船速计算部60、水平对地速度计算部70及预测位置计算部80。

当前位置计算部20计算装备航迹预测装置10的移动体的当前位置p。当前位置p(0)具有纬度分量plat(0)和经度分量plon(0)。当前位置计算部20例如使用定位信号的码伪距等，计算ecef正交坐标系的当前位置pec(0)。当前位置计算部20使用由在船体上安装的至少一个天线接收到的定位信号，计算当前位置pec(0)。当前位置计算部20使用ecef正交坐标系和enu坐标系的坐标变换矩阵，将ecef正交坐标系的当前位置pec(0)变换为enu坐标系的当前位置p，计算当前位置p的纬度分量plat(0)和经度分量plon(0)。当前位置计算部20将当前位置p(0)输出至预测位置计算部80。

角速度计算部30计算船体的角速度ω(t)。角速度计算部30作为一例，是作为惯性传感器的陀螺传感器等，对角速度ω(t)进行计测并输出。角速度计算部30此外作为其他一例，使用定位信号的载波相位差来计算角速度ω。在该情况下，角速度计算部30使用由在移动体的不同的位置上配置的至少2个天线接收到的载波相位的差，计算角速度ω(t)。角速度计算部30将角速度ω(t)输出至预测位置计算部80。通过使用载波相位，能够以简单的结构得到高精度的角速度。

姿势角计算部40计算船体的姿势角。姿势角通常由俯仰角横滚角θ(t)及偏航角ψ(t)构成。姿势角计算部40至少计算偏航角ψ(t)。姿势角计算部40使用角速度计算部30、和惯性传感器的计测值或者载波相位差，计算至少包含偏航角ψ(t)的姿势角。另外，姿势角计算部40也可以根据由角速度计算部30计算出的角速度而计算偏航角ψ(t)。通过使用载波相位，能够以简单的结构得到高精度的姿势角。

对地去路计算部50使用船体的姿势角等，计算对地去路cog(t)。对地去路计算部50将对地去路cog(t)输出至水平对地速度计算部70。

对地船速计算部60使用多普勒声呐的输出等，计算对地船速sog(t)。对地船速计算部60将对地船速sog(t)输出至水平对地速度计算部70。

水平对地速度计算部70使用接着示出的方法，计算船体坐标系的水平对地速度vb(t)。水平对地速度vb(t)是矢量，由x方向分量vbx(t)、和y方向分量vby(t)构成。x方向是如图2所示那样与船舶100的船首方向平行的分量，将从船尾朝向船首的方向设为正方向。y方向与船首方向(x方向)正交，将从左舷朝向右舷的方向设为正的方向。

如图2所示，水平对地速度vb(t)的x方向分量vbx(t)和y方向分量vby(t)设为偏角β(t)根据下式而得到。

vbx(t)＝sog(t)·cosβ(t)-(式1)

vby(t)＝sog(t)·sinβ(t)-(式2)

在此，偏角β根据对地去路cog和偏航角ψ而由下式得到。

β(t)＝cog(t)-ψ(t)-(式3)

水平对地速度计算部70使用下式，计算enu坐标系的水平对地速度vn(t)。

如图2所示，enu坐标系(ned坐标系)的水平对地速度vn(t)和船体坐标系的水平对地速度vb(t)处于将偏航角ψ(t)设为2个坐标系所成的角的关系。从而，enu坐标系(ned坐标系)的水平对地速度vn(t)的北方向分量vnn(t)和东方向分量vne(t)根据船体坐标系的水平对地速度vb(t)的x方向分量vbx(t)和y方向分量vby(t)及偏航角ψ(t)而被计算。

vnn(t)＝vbx(t)·cosψ(t)-vby(t)·sinψ(t)-(式4)

vne(t)＝vbx(t)·sinψ(t)+vby(t)·cosψ(t)-(式5)

水平对地速度计算部70将enu坐标系的水平对地速度vn(t)＝(vnn(t)，vne(t))输出至预测位置计算部80。

预测位置计算部80使用下面的方法，计算预测时刻τ的预测位置p(τ)。

在时刻t中，在预测位置p(t)＝(plat(t)，plon(t))、和水平对地速度vn(t)＝(vnn(t)，vne(t))之间，以下的关系成立。

dplat(t)/dt＝vnn(t)-(式6)

dplon(t)/dt＝vne(t)-(式7)

此外，预测时刻的偏航角ψ(τ)使用初始时刻的偏航角ψ(0)、和从初始时刻t＝0至预测时刻t＝τ为止的角速度ω(t)的积分运算而被得到。若设想为船舶100稳态回旋，则从初始时刻t＝0至预测时刻t＝τ为止，ω(t)成为固定值ω。从而，预测时刻的偏航角ψ(τ)由下式得到。

ψ(τ)＝ψ(0)+ω·τ-(式8)

此外，预测位置计算部80根据角速度ω，判断船舶100为平进运动中还是旋转运动中。预测位置计算部80根据各个情况，计算p(τ)＝(plat(τ)，plon(τ))。

若旋转运动中，即、偏航角ψ(τ)超过转动检测阈值，则预测位置计算部80使用预测位置(式6)、(式7)的积分运算，计算预测位置p(τ)＝(plat(τ)，plon(τ))。

plat(τ)＝plat(0)+(积分运算(vnn))

＝plat(0)+(vnn(0)·sinωτ)/ω-(vne(0)·(1-cosωτ))/ω-(式9)

plon(τ)＝plon(0)+(积分运算(vne))

＝plon(0)+(vnn(0)·(1-cosωτ))/ω-(vne(0)·sinωτ)/ω-(式10)

即，若船舶100为旋转运动中，则预测位置计算部80使用包含作为转头方向的加速度的角速度的积分值的运算，计算预测位置p(τ)。

另一方面，若平进运动中，即、偏航角ψ(τ)为转动检测阈值以下，则预测位置计算部80使用不利用加速度(角速度)的下面的运算，计算预测位置p(τ)。

plat(τ)＝plat(0)+vnn(0)·τ-(式11)

plon(τ)＝plon(0)+vne(0)·τ-(式12)

预测位置计算部80输出所计算出的预测位置p(τ)＝(plat(τ)，plon(τ))。此外，预测位置计算部80持续执行该预测位置p(τ)的计算，计算多个时刻τ的预测位置p(τ)。并且，预测位置计算部80连结这多个时刻τ的预测位置p(τ)，从而能够得到预测航迹。

通过这样的结构及处理，航迹预测装置10能够根据船舶100的举动，抑制对计算预测位置p(τ)而言不需要的加速度项的利用。由此，使用该加速度项导致的误差的增加被抑制。从而，航迹预测装置10能够高精度地计算预测位置p(τ)。进而，若有速度传感器、角速度传感器等惯性传感器在船体中的位置信息(相对于重心位置的船体坐标)则根据船体的姿势角而校正为船体的重心位置，能够更高精度地计算预测位置p(τ)。

图3是模拟了本实施方式的航迹预测装置和比较例的航迹预测装置的航迹预测结果的图。图3的(a)示出使用了本申请的结构的预测航迹，图3的(b)示出始终使用了速度和速度的变化量的预测航迹，图3的(c)示出仅使用了速度的预测航迹，图3的(d)示出始终使用了速度和加速度的预测航迹。在图3的(a)、图3的(b)、图3的(c)、图3的(d)中，点线示出实际航迹，实线示出预测航迹。

如图3的(a)、图3的(b)、图3的(c)、图3的(d)所示，通过在本实施方式中使用航迹预测装置10的结构，从而能够将实际航迹和预测航迹的差变小。

图4是示出了本实施方式的航迹预测装置和比较例的航迹预测装置的预测航迹相对于实际航迹的标准差的图。图4的(a)示出纬度中的预测航迹的标准差，图4的(b)示出经度中的预测航迹的标准差。在图4的(a)、图4的(b)中，实线是本申请的预测航迹的标准差，点线是仅使用了速度的预测航迹的标准差，点划线是始终使用了速度和速度的变化量的预测航迹的标准差，二点划线是始终使用了速度和加速度的预测航迹的标准差。

如图4的(a)、图4的(b)、图4的(c)、图4的(d)所示，通过在本实施方式中使用航迹预测装置10的结构，从而能够将预测航迹的标准差变小。即，能够将预测航迹相对于实际航迹的误差变小。

另外，在上述的说明中，示出了将由航迹预测装置10执行的处理通过多个功能部来实现的方式。但是，也可以将上述的多个处理程序化而存储至存储介质，由计算机等运算处理器读出并执行该程序。在该情况下，运算处理器执行与图5、图6所示的流程图相应的处理即可。

图5是本发明的实施方式所涉及的航迹预测的流程图。图6是本发明的实施方式所涉及的预测位置的计算的流程图。另外，各处理的具体的实现方法与上述的各功能部的处理相同，因此省略说明。

如图5所示，运算处理器计算当前位置p(0)(s11)。运算处理器计算角速度ω(t)。运算处理器使用姿势角的偏航角ψ(t)、对地去路cog(t)及对地船速sog(t)，计算水平对地速度vn(s13)。运算处理器使用当前位置p(0)、角速度ω、及预测时刻τ的水平对地速度vn(τ)，计算预测位置p(τ)。

更具体而言，如图6所示，运算处理器若检测到角速度ω大致为0，即、为转动检测阈值以下(s41：是)，则计算预测位置p(τ)而不使用加速度(s42)。运算处理器若检测到角速度ω与0较大地不同，即、超过转动检测阈值(s41：否)，则通过利用了作为转头方向的加速度的角速度的积分运算，计算预测位置p(τ)(s43)。

接着，关于第二实施方式所涉及的航迹预测装置、航迹预测方法及航迹预测程序，参照附图进行说明。图7是本发明的第二实施方式所涉及的航迹预测装置的框图。

如图7所示，本发明的第二实施方式所涉及的航迹预测装置10a相对于第一实施方式所涉及的航迹预测装置10，在追加了显示器90的点上不同。航迹预测装置10a的其他结构与航迹预测装置10同样，省略同样的地方的说明。

预测位置计算部80将所计算出的预测位置输出至显示器90。显示器90显示该预测位置。此外，预测位置计算部80在计算出预测航迹的情况下，将该预测航迹输出至显示器90。显示器90显示预测航迹。

通过设为这样的结构，操作者能够容易地视觉辨认预测位置、预测航迹。

另外，相对于上述的航迹预测装置10、10a，姿势角计算部40、对地去路计算部50及对地船速计算部60也可以是分体的。进而，相对于航迹预测装置10、10a，当前位置计算部20及角速度计算部30也可以是分体的。

标号说明

10、10a：航迹预测装置

20：当前位置计算部

30：角速度计算部

40：姿势角计算部

50：对地去路计算部

60：对地船速计算部

70：水平对地速度计算部

80：预测位置计算部

90：显示器

100：船舶