CN104345326A - 状态计算装置、移动体、状态计算方法 - Google Patents

Abstract

一种状态计算装置、移动体、状态计算方法，将能够高精度地计算移动状态的状态计算装置实现小型化。状态计算装置具备：天线装置、相关处理部、ADR计算部、运算部。天线具备天线。相关处理部及ADR计算部计算天线的载波相位测定值并输出至运算部。运算部根据多个载波相位测定值计算多个基线矢量。运算部使用多个基线矢量计算姿态角。这时，运算部按照基线矢量的每个组合，分别计算姿态角的多个各成分即俯仰角、滚转角、以及偏航角。运算部根据多个计算结果来计算各成分的代表值，并通过这些代表值决定姿态角。

Description

状态计算装置、移动体、状态计算方法

技术领域

本发明涉及计算船舶等移动体的姿态角(日语：姿勢角)、速度、位置等移动状态的状态计算装置。

背景技术

以往，提出了计算移动体的航行状态(移动状态)的各种装置。例如、专利文献1所记载的姿态计算装置装配于船舶。姿态计算装置使用配置于船舶上的不同位置的多个天线的位置信息，计算船舶的姿态角。

在专利文献所记载的姿态计算装置中，以使连结天线间的基线矢量的大小及方向不同的方式配置多个天线。专利文献1所记载的姿态计算装置，在接收测位信号的多个天线、即能够进行定位的多个天线中，选择基线长度最长的两个天线。专利文献1所记载的姿态计算装置根据所选择的两个天线的定位位置来计算姿态角。

专利文献1：日本特开2008-14721号公报

如上述那样，以往的姿态计算装置尽可能使用基线长度长的两个天线来计算姿态角，所以姿态计算装置相对于移动体的配置规模变大。这是为了提高姿态角的计算精度，一般来说，如果基线长度变短，则姿态角的计算精度下降。

但是，姿态计算装置相对于移动体的配置规模有时存在限制，这种情况下，不得不牺牲姿态角的计算精度。

发明内容

因此，本发明的目的在于，提供一种能够高精度地计算包含姿态角在内的移动状态的小型状态计算装置。此外，还提供能够实现具有这样的特征的状态计算装置的状态计算方法及状态计算程序。

本发明的状态计算装置具备：多个天线、相关处理部、载波相位测定值计算部、基线矢量计算部及姿态角计算部。多个天线为3个以上，配置在移动体上的不同位置，分别接收测位信号。相关处理部基于测位信号和该测位信号的复制信号(replica signal)的相关处理，按照各个天线分别计算载波相位差。载波相位测定值计算部计算载波相位差的积分值、即载波相位测定值。基线矢量计算部基于载波相位测定值，计算多个基线矢量。姿态角计算部按照每个基线矢量计算偏航角，并基于计算出的多个偏航角来决定代表偏航角。

在该构成中，通过多个基线矢量来计算代表偏航角，所以能够高精度地计算该代表偏航角。因此，能够高精度地计算包含该代表偏航角在内的姿态角。

此外，本发明的状态计算装置的姿态角计算部，按照每个基线矢量，根据代表俯仰角(纵摇角、pitch angle)及代表滚转角(横摇角、roll angle)来计算偏航角(yaw angle)。

在该构成中，示出了使用基线矢量来计算偏航角的具体方法。

此外，本发明的状态计算装置的姿态角计算部，按照每个基线矢量，根据代表俯仰角来计算滚转角，并根据计算出的多个滚转角来计算代表滚转角。

在该构成中，示出了在偏航角的计算中利用的代表滚转角的具体计算方法。

此外，本发明的状态计算装置的姿态角计算部，将多个基线矢量中的每2个作为一组，按照每个组计算俯仰角，并根据计算出的多个俯仰角来计算代表俯仰角。

在该构成中，示出了在偏航角及滚转角的计算中利用的代表俯仰角的具体计算方法。此外，通过将这些具体的计算方法组合，能够基于测位信号的载波相位测定值来高精度地计算姿态角。

此外，在本发明的状态计算装置中，以多个基线矢量的至少1个与移动体的航向(heading)平行的方式配置3个以上的天线。

在该构成中，能够简单且高速地计算偏航角。

此外，本发明的状态计算装置还具备位置计算部，该位置计算部使用姿态角和针对多个测位信号的相关处理的结果，计算移动体的特定位置的坐标。这时，位置计算部在计算特定位置的坐标时进行基于姿态角的修正。

此外，本发明的状态计算装置还具备速度计算部，该速度计算部使用针对多个测位信号的相关处理的结果，计算移动体的特定位置的速度。这时，速度计算部在计算特定位置的速度时进行基于姿态角的修正。

在这些构成中，使用上述那样高精度地计算的姿态角，所以能够高精度地计算位置坐标及速度。

发明的效果

根据本发明，能够实现高精度地计算航行状态的小型的状态计算装置。

附图说明

图1是表示本发明的第1实施方式的状态计算装置的构成的框图。

图2是用于说明本发明的第1实施方式的航行状态的计算原理的图。

图3是表示天线装置的各天线的配置及基线矢量的关系的图。

图4是表示由本实施方式的状态计算装置执行的姿态角计算的概略流程的流程图。

图5是表示由运算部执行的姿态角计算的具体流程的流程图。

图6是表示俯仰角θ的计算流程的流程图。

图7是表示滚转角φ的计算流程的流程图。

图8是表示偏航角ψ的计算流程的流程图。

图9是表示本发明的第2实施方式的状态计算装置的构成的框图。

图10是表示本发明的第2实施方式的速度计算流程的流程图。

图11是表示本发明的第2实施方式的另一速度计算流程的流程图。

图12是表示几何学平均值的计算位置和期望位置的偏差的状况及修正的概念的图。

图13是表示本发明的第2实施方式的位置计算流程的流程图。

图14是表示本发明的第2实施方式的另一位置计算流程的流程图。

图15是表示装配了本实施方式的状态计算装置的船舶的概略构成图。

图16是表示多个天线的派生配置样式的图。

图17是表示多个天线的派生配置样式的图。

图18是表示多个天线的派生配置样式的图。

具体实施方式

参照附图说明本发明的第1实施方式的状态计算装置。在本实施方式中，说明将船舶作为移动体并计算该船舶的航行状态的装置，但是在计算其他海上海中移动体、汽车等陆地移动体、飞机等空中移动体的移动状态的情况下，也能够应用以下的构成。

图1是表示本发明的第1实施方式的状态计算装置的构成的框图。图2是用于说明本发明的第1实施方式的航行状态的计算原理的图。在图2中，记载了天线装置100相对于船舶900的设置形态、以及BODY坐标系(船体坐标系)和NED坐标系(地球坐标系)的关系。图3是表示天线装置的各天线的配置及基线矢量的关系的图。

如图1所示，状态计算装置10具备：天线装置100、相关处理部111A、111B、111C、111D、ADR计算部112A、112B、112C、112D、运算部12。

如图2所示，天线装置100设置在船舶900上的视野好的地方、即对于来自定位卫星的测位信号的接收环境良好的地方。例如，如图2所示，天线装置100设置在设于船舶900的甲板的桅杆的前端。

天线装置100具备天线100A、100B、100C、100D。如图3所示，天线100A、100B、100C、100D在天线装置100的框体内配置于同一平面上。这时，以形成天线100A的中心位置A、天线100B的中心位置B、天线100C的中心位置C及天线100D的中心位置D成为四个角的正方形的方式配置天线100A、100B、100C、100D。另外，以下将由该天线100A、100B、100C、100D的配置位置形成的正方形的中心位置作为配置中心位置O。另外，天线装置100以天线100A、100B、100C、100D所配置的平面与船舶900的水平面平行的方式配置于船舶900。

天线100A、100B、100C、100D接收来自多个定位卫星的测位信号，并分别输出至相关处理部111A、111B、111C、111D。在此，例如定位卫星是GPS(Global Positioning System,全球定位系统)卫星，测位信号是GPS信号。另外，不限于GPS，也可以使用GNSS(Grobal Navigation SatelliteSystems，全球导航卫星系统)的定位卫星及测位信号。

相关处理部111A根据来自天线100A的各测位信号和复制信号(replicasignal)的载波相关处理结果，按照每个测位信号输出载波相位差。ADR计算部112A对载波相位差进行积分，计算载波相位测定值ADR_A，并输出至运算部12。另外，相关处理部111A根据来自天线100A的各测位信号和复制信号的代码相关处理结果，计算模拟(日语：擬似)距离PR_A，并输出至运算部12。

相关处理部111B根据来自天线100B的各测位信号和复制信号的载波相关处理结果，按照每个测位信号输出载波相位差。ADR计算部112B对载波相位差进行积分，计算载波相位测定值ADR_B，并输出至运算部12。另外，相关处理部111B根据来自天线100B的各测位信号和复制信号的代码相关处理结果，计算模拟距离PR_B，并输出至运算部12。

相关处理部111C根据来自天线100C的各测位信号和复制信号的载波相关处理结果，按照每个测位信号输出载波相位差。ADR计算部112C对载波相位差进行积分，计算载波相位测定值ADR_C，并输出至运算部12。另外，相关处理部111C根据来自天线100C的各测位信号和复制信号的代码相关处理结果，计算模拟距离PR_C，并输出至运算部12。

相关处理部111D根据来自天线100D的各测位信号和复制信号的载波相关处理结果，按照每个测位信号输出载波相位差。ADR计算部112D对载波相位差进行积分，计算载波相位测定值ADR_D，并输出至运算部12。另外，相关处理部111D根据来自天线100D的各测位信号和复制信号的代码相关处理结果，计算模拟距离PR_D，并输出至运算部12。

如下述那样，本实施方式的运算部12计算姿态角，省略了位置坐标及速度的计算，所以相关处理部111A、111B、111C、111D、和ADR计算部112A、112B、112C、112D至少计算载波相位测定值ADR_A、ADR_B、ADR_C、ADR_D即可。

运算部12具备基线矢量计算部120和姿态角计算部121。

基线矢量计算部120使用载波相位测定值ADR_A、ADR_B、ADR_C、ADR_D来计算多个基线矢量。基线矢量是将设置的多个天线中的2个天线组合而决定。具体地说，在本实施方式中，计算基线矢量AB、BC、CD、DA、BD、CA这六个基线矢量。基线矢量的计算方法是已知的方法，作为代表性的计算方法之一，有如下的方法：计算针对二个天线和二个定位卫星的二重相位差，决定整数值偏置(偏差、bias)，使用该整数值偏置来决定基线矢量。

基线矢量AB以天线100A的中心位置A为起点，以天线100B的中心位置B为终点。基线矢量BC以天线100B的中心位置B为起点，以天线100C的中心位置C为终点。基线矢量CD以天线100C的中心位置C为起点，以天线100D的中心位置D为终点。基线矢量DA以天线100D的中心位置D为起点，以天线100A的中心位置A为终点。基线矢量BD以天线100B的中心位置B为起点，以天线100D的中心位置D为终点。基线矢量CA以天线100C的中心位置C为起点，以天线100A的中心位置A为终点。

姿态角计算部121选择多个基线矢量，根据该多个基线矢量计算姿态角。由运算部12执行的详细的姿态角的计算方法留待后述。姿态角由俯仰角(纵摇角)θ(θ_h)、滚转角(横摇角)φ(φ_h)及偏航角ψ(ψ_h)的成分构成。俯仰角θ(θ_h)、滚转角φ(φ_h)及偏航角ψ(ψ_h)通过BODY坐标系来表现。如图2所示，BODY坐标系由与船舶900的船头方向平行的b1轴、与船舶900的左右舷方向平行的b2轴、与b1轴及b2轴垂直的b3轴构成。即，b3轴是在船舶900不揺动的状态下与铅垂方向平行的轴。该BODY坐标系的中心Pa设定在船舶900的重心位置。

俯仰角θ(θ_h)是由b1轴及b3轴决定的平面的角度，表示船头尾向铅垂方向的移动。滚转角φ(φ_h)是由b2轴及b3轴决定的平面的角度，表示左右舷的铅垂方向的移动。偏航角ψ(ψ_h)是由b1轴及b2轴决定的平面的角度，表示水平面上的船头方位的移动。

姿态角计算部121概略性地分别计算多个这些俯仰角θ、滚转角φ及偏航角ψ，将作为各自的代表值的代表俯仰角θ_h、代表滚转角φ_h及代表偏航角ψ_h作为姿态角[θ_h，φ_h，ψ_h]输出。

另外，如图1所示，这一系列处理可以按照每个功能块执行，也可以预先程序化而由计算机等信息处理装置执行。

图4是表示由本实施方式的状态计算装置执行的姿态角计算的概略流程的流程图。

如上述那样，通过多个天线100A、100B、100C、100D接收测位信号，按每个天线100A、100B、100C、100D计算载波相位测定值ADR_A、ADR_B、ADR_C、ADR_D(S101)。运算部12使用载波相位测定值ADR_A、ADR_B、ADR_C、ADR_D，计算多个基线矢量AB、BC、CD、DA、BD、CA(S102)。

运算部12使用多个基线矢量计算多个俯仰角θ、滚转角φ及偏航角ψ。运算部12根据多个俯仰角θ、滚转角φ及偏航角ψ决定各成分的代表值，作为姿态角[θ_h，φ_h，ψ_h]输出(S103)。

接下来说明姿态角[θ_h，φ_h，ψ_h]的具体计算方法。

(原理)

对于从图2所示的BODY坐标系向NED坐标系的旋转，按照旋转顺序设定横滚、俯仰、航向(日语：ヘディング方向)，并将各自的角度设为[φ，θ，ψ]。即，设为滚转角φ、俯仰角θ、偏航角ψ。

在此，天线配置如上述那样，位于相对于船舶900水平、即与图2所示的b3轴正交的平面上。这时，作为BODY坐标系中的距离r和相对于船头方向的角Δ，表示一般化的天线配置的矢量x^b时，成为下式。

◎【数式1】

$x^b=\left(\begin{array}{c}r\·\cos \mathrm{\Δ}\\ r\·\sin \mathrm{\Δ}\\ 0\end{array}\right)$     －(式1)

此外，NED坐标系中的基线矢量xⁿ成为下式。

◎【数式2】

$x^n=\left(\begin{array}{c}{x}_N\\ x_E\\ x_D\end{array}\right)$     －(式2)

将旋转矩阵设为C_b ⁿ时，从BODY坐标系向NED坐标系的变换式成为下式。

◎【数式3】

$x^n=C_b^n\·x^b$     －(式3)

将式3以各成分表示并计算右边，成为下式。

◎【数式4】

$\left(\begin{array}{c}{x}_N\\ x_E\\ x_D\end{array}\right)=\left(\begin{array}{c}r\cos \mathrm{\Δ}\cos \mathrm{\ψ}\cos \mathrm{\θ}+r\sin \mathrm{\Δ}(\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}-\sin \mathrm{\ψ}\cos \mathrm{\φ})\\ r\cos \mathrm{\Δ}\sin \mathrm{\ψ}\cos \mathrm{\θ}+r\sin \mathrm{\Δ}(\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}-\cos \mathrm{\ψ}\cos \mathrm{\φ})\\ -r\cos \mathrm{\Δ}\sin \mathrm{\θ}+r\sin \mathrm{\Δ}\cos \mathrm{\θ}\sin \mathrm{\φ}\end{array}\right)$

                                  －(式4)

用BODY坐标系表示2个天线配置时，根据(式1)而能够以下式表示。

◎【数式5】

$x^b=\left(\begin{array}{cc}{r}_1\cos {\mathrm{\Δ}}_1& r_2\cos {\mathrm{\Δ}}_2\\ r_1\sin {\mathrm{\Δ}}_1& r_2\sin {\mathrm{\Δ}}_2\\ 0& 0\end{array}\right)$     －(式5)

此外，将2个基线矢量用NED坐标系表示时，根据(式2)而能够以下式表示。

◎【数式6】

$x^n=\left(\begin{array}{cc}{x}_{N1}& x_{N2}\\ x_{E1}& x_{E2}\\ x_{D1}& x_{D2}\end{array}\right)$     －(式6)

使用与(式4)、(式5)、(式6)的x_D相关的部分，对于滚转角φ求出sinφ，从而得到下式。

◎【数式7】

$\sin \mathrm{\φ}=\frac{\frac{x_{D1}}{r_1}+\cos {\mathrm{\Δ}}_1\sin \mathrm{\θ}}{\sin {\mathrm{\Δ}}_1\cos \mathrm{\θ}}=\frac{\frac{x_{D2}}{r_2}+\cos {\mathrm{\Δ}}_2\sin \mathrm{\θ}}{\sin {\mathrm{\Δ}}_2\cos \mathrm{\θ}}$     －(式7)

进而，由(式7)求出俯仰角θ，从而得到下式。

◎【数式8】

$\mathrm{\θ}={\sin }^{-1}\left(\frac{\frac{x_{D2}}{r_2}\sin {\mathrm{\Δ}}_1-\frac{x_{D1}}{r_1}\sin {\mathrm{\Δ}}_2}{\sin ({\mathrm{\Δ}}_2-{\mathrm{\Δ}}_1)}\right)$     －(式8)

从(式8)可知，只要知道2个基线矢量及该2个基线矢量与船头方向所成的角，就能够计算俯仰角θ。

然后，从(式7)可知，只要知道俯仰角θ和1个基线矢量，就能够计算滚转角φ。

进而，使用与(式4)的x_N，x_E相关的部分，得到下式。

◎【数式9】

$\text{\ψ=}{\tan }^{-1}\left(\frac{-\sin \mathrm{\Δ}\cos \mathrm{\φ}\·x_N+(\cos \mathrm{\Δ}\cos \mathrm{\θ}+\sin \mathrm{\Δ}\sin \mathrm{\θ}\sin \mathrm{\φ})x_E}{(\cos \mathrm{\Δ}\cos \mathrm{\θ}+\sin \mathrm{\Δ}\sin \mathrm{\θ}\sin \mathrm{\φ})x_N+\sin \mathrm{\Δ}\cos \mathrm{\φ}\·x_E}\right)$

                                      －(式9)

从(式9)可知，只要知道俯仰角θ、滚转角φ和1个基线矢量，就能够计算偏航角ψ。

使用以上的原理，本实施方式的运算部12具体地按照下面的流程计算姿态角[θ_h，φ_h，ψ_h]。

图5是表示由运算部执行的姿态角计算的具体流程的流程图。

运算部12使用上述(式8)所示的运算方法，使用2个基线矢量来计算俯仰角θ。更具体地说，通过将计算出的2个基线矢量及该2个基线矢量与船头方向所成的角代入(式8)，计算俯仰角θ。这时，运算部12使选择的2个基线矢量的组合不同，按照基线矢量的每个组计算俯仰角θ(S131)。

运算部12对于多个俯仰角θ计算代表俯仰角θ_h(S132)。具体地说，例如运算部12计算多个俯仰角θ的平均值，作为代表俯仰角θ_h。

运算部12使用上述(式7)所示的运算方法，使用代表俯仰角θ_h和1个基线矢量来计算滚转角φ。更具体地说，将上述处理中计算的代表俯仰角θ_h、计算出的1个基线矢量及该基线矢量所成的角代入(式7)，计算滚转角φ。这时，运算部12使与代表俯仰角θ_h组合的基线矢量不同，按照每个组合计算滚转角φ(S133)。

运算部12计算对于多个滚转角φ的代表滚转角φ_h(S134)。具体地说，例如运算部12计算多个滚转角φ的平均值，作为代表滚转角φ_h。

运算部12使用上述(式9)所示的运算方法，使用代表俯仰角θ_h、代表滚转角φ_h、以及1个基线矢量来计算偏航角ψ。更具体地说，将通过上述处理所计算出的代表俯仰角θ_h、代表滚转角φ_h、以及计算出的1个基线矢量及该基线矢量所成的角代入(式9)，计算偏航角ψ。这时，运算部12使与代表俯仰角θ_h及代表滚转角φ_h组合的基线矢量不同，按照每个组合计算偏航角ψ(S135)。

运算部12对于多个偏航角ψ计算代表偏航角ψ_h(S136)。具体地说，例如运算部12计算多个偏航角ψ的平均值，作为代表偏航角ψ_h。

运算部12将代表俯仰角θ_h、代表滚转角φ_h、代表偏航角ψ_h决定为姿态角[θ_h，φ_h，ψ_h](S137)。

通过使用这样的方法，能够与计算出的基线矢量的个数相应地，分别计算多个姿态角的各成分(俯仰角、滚转角、偏航角)。并且，通过使用这样计算出的多个各姿态角成分的平均值等的代表值，抑制各姿态角成分中包含的观测误差。因此，能够高精度地计算姿态角。

此外，要求同程度的精度的情况下，与以往的构成相比，能够使天线间隔变窄。由此，能够缩小天线的配置区域，进而能够使天线装置小型化。

此外，在上述说明中，将代表值简单记为平均值，但既可以是不加权的平均值，也可以是进行了加权的平均值。进行加权的情况下，根据计算姿态角所利用的各基线矢量的方向和各姿态角的方向的关系等决定权重即可。此外，也可以基于各天线位置的定位精度来进行加权。例如，以越是利用定位精度高的天线的基线矢量则权重越大的方式来设定即可。此外，在上述说明中，示出了以平均值作为代表值来计算的例子，但是也可以使用中央值等其他统计值。

此外，如图3所示，通过配置多个天线，能够整体上高精度地计算姿态角。具体地说，如图3所示，在本实施方式的构成中，多个天线100A、100B、100C、100D配置在与BODY坐标的b3轴正交的平面上，从配置中心位置起配置在相同距离。此外，天线100A、100B、100C、100D以配置中心位置O为基准点，分别处于成90°的旋转对称位置。由此，天线100A、100B、100C、100D成为在b1方向、b2方向上都均等的配置样式。即，天线100A、100B、100C、100D成为具有各向同性的配置。

通过采用这样的配置样式，船舶900倾斜时产生的基线矢量的变化相对于俯仰角θ及滚转角φ为同等程度。因此，能够以大致相同的精度计算俯仰角θ、滚转角φ。由此，对于俯仰角θ和滚转角φ能够以具备各向同性的精度进行计算，使用俯仰角θ和滚转角φ计算的偏航角不依存于船体摇动的方向而能够以同等程度的精度计算。其结果，没有方向引起的偏差，能够在整体上高精度地计算姿态角。

此外，通过采用这样的天线配置，无论天线装置的各天线和船头方向的角度为何种角度，对于俯仰角θ及滚转角φ，都能够以具备各向同性的精度进行计算。由此，不必在意天线装置对船舶的设置方向，所以使天线装置对船舶的设置容易。

另外，在此，虽然采用了具有各向同性的配置，但是也可以大致各向同性。即，多个天线的配置样式可以稍微不同。这种情况下，也能够以与具有各向同性的情况大致同等的精度来计算姿态角。

此外，天线装置100以将天线100A的中心位置A和天线100B的中心位置B连结的基线矢量、以及将天线100C的中心位置C和天线100D的中心位置D连结的基线矢量平行于船舶900的船头方向(BODY坐标系的b1轴方向(航向))的方式配置于船舶900即可。通过采用这样的配置，如后述那样，容易进行姿态角计算处理。

此外，通过采用该配置，平行于船头方向的基线矢量的长度变短。因此，能够高速且高精度地计算平行于船头方向的基线矢量。由此，根据情况能够进而高速地计算姿态角。

接着，在上述的天线配置下平行于船头方向的俯仰角θ、滚转角φ、偏航角ψ的计算，根据多个天线的配置、即基线矢量的设定方法，更具体地可以使用如下的各方法。

(俯仰角θ的计算方法)

图6是表示俯仰角θ的计算流程的流程图。

首先，检测是否确定了1个以上的基线矢量。如果基线矢量1个也没有确定(S311：否)，则判断为不能计算俯仰角θ(S317)。

确定了1个以上基线矢量的情况下(S311：是)，取得所确定的基线矢量的个数N。

基线矢量为1个的情况下(S312：N＝1)，检测该基线矢量是否与船头方向、即b1轴方向平行。如果基线矢量不与船头方向平行(S313：否)，则判断为不能计算俯仰角θ(S317)。另一方面，如果基线矢量与船头方向平行(S313：是)、即图3的天线配置的情况下，若确定的基线矢量是基线矢量AB、CD的某一个，则使用该基线矢量来计算俯仰角θ(S314)。这种情况下，设Δ₁或Δ₂为0°、180°，能够根据上述(式8)计算俯仰角θ。

基线矢量为2个的情况下(S312：N＝2)，计算该2个基线矢量是否与船头方向正交。如果2个基线矢量与船头方向(S315：是)正交、即图3的天线配置的情况下，若确定的基线矢量是基线矢量BC、DA的某一个，则判断为不能计算俯仰角θ(S317)。另一方面，如果2个基线矢量不与船头方向正交(S315：否)，则将该2个基线矢量作为组，使用上述(式8)计算俯仰角θ(S316)。

基线矢量为3个以上的情况下(S312：N≥3)，从确定的多个基线矢量选择2个基线矢量，根据该2个基线矢量，使用上述(式8)计算俯仰角θ(S316)。

(滚转角φ的计算方法)

图7是表示滚转角φ的计算流程的流程图。

首先，判断能否使用代表俯仰角θ_h，如果代表俯仰角θ_h不能使用(S321：否)，则检测是否存在与船头方向正交的基线矢量。如果不存在与船头方向正交的基线矢量(S322：否)，则判断为不能计算滚转角φ(S328)。如果存在与船头方向正交的基线矢量(S322：是)，则使用该基线矢量计算滚转角φ(S323)。另外，这种情况的滚转角φ的计算方法，由于基线矢量在b3方向的运动与滚转角φ对应，所以利用基线矢量在b3方向的运动来计算滚转角φ即可。

如果代表俯仰角θ_h能够使用(S321：是)，则取得基线矢量的个数N。

基线矢量为1个的情况下(S324：N＝1)，检测该基线矢量是否与船头方向、即b1轴方向平行。如果基线矢量与船头方向平行(S326：是)、即图3的天线配置的情况下，若确定的基线矢量是基线矢量AB、CD的某一个，则判断为不能计算滚转角φ(S328)。如果该基线矢量不与船头方向平行(S326：否)，则将该基线矢量和代表俯仰角θ_h作为组，使用上述(式7)来计算滚转角φ(S327)。

基线矢量为2个的情况下(S324：N＝2)，检测该2个基线矢量是否与船头方向平行。如果2个基线矢量与船头方向平行(S325：是)、即图3的天线配置的情况下，若确定的基线矢量是基线矢量BC、DA，则判断为不能计算滚转角φ(S328)。另一方面，如果2个的基线矢量不与船头方向正交(S325：否)，则选择该2个基线矢量的某一个，将选择的基线矢量和代表俯仰角θ_h作为组，使用上述(式7)来计算滚转角φ(S327)。

基线矢量为3个以上的情况下(S324：N≥3)，从确定的多个基线矢量选择1个基线矢量，将该基线矢量和代表俯仰角θ_h作为组，使用上述(式7)来计算滚转角φ(S327)。

(偏航角ψ的计算方法)

图8是表示偏航角ψ的计算流程的流程图。

首先，判断能否使用代表俯仰角θ_h，如果不能使用代表俯仰角θ_h(S331：否)，则判断为不能计算偏航角ψ(S336)。

如果能够使用代表俯仰角θ_h(S321：是)，则判断能否使用代表滚转角φ_h。如果不能使用代表滚转角φ_h(S332：否)，则检测是否存在与船头方向平行的基线矢量。如果不存在与船头方向平行的基线矢量(S334：否)，则判断为不能计算偏航角ψ(S336)。如果存在与船头方向平行的基线矢量(S334：是)，则使用该基线矢量和代表俯仰角θ_h来计算偏航角ψ(S335)。另外，这种情况的偏航角ψ的计算方法，由于基线矢量在b2方向的运动与偏航角ψ对应，所以利用基线矢量在b2方向的运动来计算偏航角ψ即可。

如果能够使用代表滚转角φ_h(S332)，则将代表俯仰角θ_h及代表滚转角φ_h和基线矢量作为组，使用上述(式9)来计算偏航角ψ(S333)。

通过采用这样的构成及处理，根据天线配置，能够以更少的基线矢量来计算姿态角。

接下来，参照附图说明本发明的第2实施方式的状态计算装置。图9是表示本发明的第2实施方式的状态计算装置的构成的框图。本实施方式的状态计算装置10A概略地说，与第1实施方式的状态计算装置10的不同点在于，除了姿态角之外还计算位置(位置坐标)及速度。

运算部12A具备：姿态角运算部121、速度运算部122及位置运算部123。姿态角运算部121是与第1实施方式的运算部12相同的构成，进行相同的处理。

速度运算部122根据载波相位测定值ADR_A，计算天线100A的位置处的速度。速度运算部122根据载波相位测定值ADR_B，计算天线100B的位置处的速度。速度运算部122根据载波相位测定值ADR_C，计算天线100C的位置处的速度。速度运算部122根据载波相位测定值ADR_D，计算天线100D的位置处的速度。

速度运算部122根据天线100A、100B、100C、100D的速度计算船舶900的速度。在速度运算部122中，不限于各天线100A、100B、100C、100D的位置，例如也能够计算船舶900的船头位置等船舶900的特定位置的速度。

速度运算部122接下来使用流程计算速度。图10是表示本发明的第2实施方式的速度计算流程的流程图。

首先，速度运算部122计算已经计算出载波相位测定值的各天线位置处的速度(S401)。速度运算部122根据载波相位测定值的时间变化量来计算速度。

速度运算部122根据姿态角[θ_h，φ_h，ψ_h]的时间变化量，计算各天线位置处的角速度(S402)。

速度计算部122使用计算出的角速度，计算各天线位置处的速度计算用修正值(杆臂修正值)(S403)。速度计算用修正值(杆臂修正值)指的是，用于抑制由杆臂效应产生的角速度给速度带来的影响的修正值。杆臂效应指的是，如果对船舶900施加角速度，则在重心Pa以外的位置，观测到的速度中包含角速度的影响。

速度运算部122使用速度计算用修正值来对各天线位置的速度进行修正(S404)。

速度运算部122使用各天线位置的修正后的速度，计算期望位置的速度。这时，速度运算部122通过基于各天线位置和期望位置的距离的几何学平均值，计算期望位置(例如船舶900的重心Pa位置或船头位置等)的速度(S405)。

然后，使用上述姿态角来计算角速度，所以能够高精度地计算速度计算用修正值。由此，能够高精度地抑制杆臂效应，高精度地计算速度。

速度运算部122也可以通过如下的方法来计算速度。图11是表示本发明的第2实施方式的另一速度计算流程的流程图。

速度运算部122计算已经计算出载波相位测定值的各天线位置处的速度(S411)。速度运算部122根据载波相位测定值的时间变化量来计算速度。

速度运算部122使用各天线位置的速度，计算期望位置的速度。这时，速度运算部122通过基于各天线位置和期望位置的距离的几何学平均值，计算期望位置的速度(S412)。

速度运算部122根据姿态角[θ_h，φ_h，ψ_h]的时间变化量，计算各天线位置处的角速度(S413)。速度计算部122使用计算出的角速度，计算期望位置处的速度计算用修正值(杆臂修正值)(S414)。另外，这些处理可以与上述的速度计算处理并行地进行，也可以在速度计算处理之前进行。

速度运算部122使用针对期望位置的速度计算用修正值，对期望位置的速度进行修正(S415)。

在这样的方法中，也使用上述姿态角来计算角速度，所以能够高精度地计算速度计算用修正值。由此，能够高精度地抑制杆臂效应，高精度地计算速度。

另外，在上述说明中，示出了计算载波相位测定值的天线不变化的情况，但是根据测位信号的接收状态不同，有时也存在无法计算载波相位测定值的天线。

这种情况下，产生计算出几何学平均值的位置与期望位置不同的情况。图12是表示几何学平均值的计算位置与期望位置的偏差的状况及修正的概念的图。在图12中，示出了将多个天线100A、100B、100C、100D的配置中心位置O作为期望位置的情况。

如图12(A)所示，在已经计算出全部天线100A、100B、100C、100D的位置处的载波相位测定值的状况下，全部天线100A、100B、100C、100D的位置处的速度的几何学平均值成为配置中心位置O(期望位置)的速度。

如图12(B)所示，在无法计算天线100D的位置处的载波相位测定值的状态下，不能将该天线100D的位置处的载波相位测定值用于期望位置的速度计算。因此，几何学平均值为天线100A、100B、100C的几何学平均位置O’的值，成为与配置中心位置O不同的位置的值。

这种情况下，天线100A、100B、100C、100D的位置关系是已知的，所以预先存储全部天线100A、100B、100C、100D的几何学平均位置(配置中心位置O)和至少2个天线的各组合的几何学平均位置的位置误差。然后，预先设定对该位置误差进行修正的修正值。这时，对还考虑了速度计算用修正值(杆臂修正值)的变化量的修正值进行设定。

计算期望位置、即配置中心位置O的速度的情况下，检测计算出载波相位测定值的天线，根据天线的组合来修正速度的几何学平均值即可。

另外，在此示出了将全部天线100A、100B、100C、100D的配置中心位置O设定为成为基准的期望位置的例子，但是对于其他位置，如果使用同样的概念进行修正，也能够正确地计算期望位置的速度。

位置运算部123根据模拟距离PR_A或载波相位测定值ADR_A，计算天线100A的位置坐标。位置运算部123根据模拟距离PR_B或载波相位测定值ADR_B，计算天线100B的位置坐标。位置运算部123根据模拟距离PR_C或载波相位测定值ADR_C，计算天线100C的位置坐标。位置运算部123根据模拟距离PR_D或载波相位测定值ADR_D，计算天线100D的位置坐标。

位置运算部123根据天线100A、100B、100C、100D的位置坐标，计算期望位置的位置坐标。期望位置指的是，例如配置中心位置O或船舶900的船头位置等。

位置运算部123接下来使用流程来计算速度。图13是表示本发明的第2实施方式的位置计算流程的流程图。

首先，位置运算部123计算已经计算出模拟距离或载波相位测定值的各天线位置的位置坐标(S501)。位置坐标的计算方法是已知的，所以省略具体的方法，但是在使用模拟距离的情况下，与使用载波相位测定值的情况相比，能够从位置坐标的计算初期起快速地计算位置坐标，在使用载波相位测定值的情况下，与使用模拟距离相比，能够高精度地计算位置坐标。

位置运算部123使用如上述那样根据多个基线矢量计算出的姿态角[θ_h，φ_h，ψ_h]，计算各天线位置的位置计算用修正值(S502)。

位置运算部123使用位置计算用修正值，对各天线位置的位置坐标进行修正(S503)。

位置运算部123使用各天线位置的修正后的位置坐标，计算期望位置的位置坐标。这时，位置运算部123通过基于各天线位置和期望位置的距离的几何学平均值，计算期望位置的位置坐标(S504)。

然后，通过使用上述姿态角，能够高精度地计算位置计算用修正值。由此，能够高精度地抑制揺动所导致的位置计算误差，高精度地计算期望位置的位置坐标。

位置运算部123也可以通过如下的方法来计算速度。图14是表示本发明的第2实施方式的其他的位置计算流程的流程图。

首先，位置运算部123计算已经计算出模拟距离或载波相位测定值的各天线位置的位置坐标(S511)。

位置运算部123通过基于各天线位置和期望位置的距离的几何学平均值，计算期望位置的位置坐标(S512)。

位置运算部123使用如上述那样根据多个基线矢量计算出的姿态角[θ_h，φ_h，ψ_h]，计算期望位置的位置计算用修正值(S513)。

位置运算部123使用期望位置用的位置计算用修正值，对期望位置的位置坐标进行修正(S514)。

在这样的方法中，也使用上述姿态角，所以能够高精度地计算位置计算用修正值。由此，能够高精度地抑制揺动所导致的位置计算误差，高精度地计算期望位置的位置坐标。

另外，进行位置计算的情况下，也与上述的速度计算同样，能够应用图12所示的修正、即通过已经计算出载波相位测定值的天线的组合来进行的修正。

以上所示的状态计算装置、状态计算方法、状态计算程序能够装配在船舶900等移动体而利用。图15是表示装配了本实施方式的状态计算装置的船舶的概略构成图。另外，在此，作为移动体以船舶900为例进行了说明，但是对于其他水上移动体、水中移动体、飞机等空中移动体、汽车等陆地移动体，也能够应用本实施方式的构成。

船舶900具备：状态计算装置10A、控制部901、动力902、舵903。状态计算装置10A使用上述方法，计算位置[P_N，P_E，P_D]、速度[V_N，V_E，V_D]、姿态角[φ_h，θ_h，ψ_h]并输出。

控制部901使用位置[P_N，P_E，P_D]、速度[V_N，V_E，V_D]、姿态角[φ_h，θ_h，ψ_h]来执行航行控制处理。例如，控制部901进行自动航行控制或定点保持控制，该自动航行控制是根据当前的自船位置和目标位置推测最有效率的航行路线的控制，该定点保持控制用于使从自船位置的变化保持于定点位置。

控制部901基于设定的控制内容，产生对动力902及舵903的控制信号。动力902及舵903基于控制信号进行动作。

通过这样的构成，如上述那样高精度地计算姿态角、位置、速度，所以能够高精度地实现各种控制。例如，如果是自动航行控制，则能够使船舶900以最佳的航行路线自动航行。此外，如果是定点保持控制，则能够高精度地进行定点保持。

另外，在上述的说明中，示出了将多个天线具备各向同性地配置的例子，但是也可以使用如下所示的配置样式。图16、图17、图18是表示多个天线的派生配置样式的图。

图16所示的天线装置101具备3个天线100A、100B、100C。天线100A与天线100B、100C相比，向船头方向分离地配置。天线100B、100C沿着与船头方向正交的方向配置。在连结天线100B的位置和天线100C的位置的垂直二等分线上配置天线100A。从天线100B的位置到天线100C的位置的距离比从天线100B的位置和天线100C的位置的中点到天线100A的位置的距离更短。

通过使用这样的天线配置，由天线100A、100B构成的基线矢量及由天线100C、100A构成的基线矢量，成为在船头方向上较长的基线矢量。因此，能够确保多个船头方向的成分较长的基线矢量，所以能够高精度地计算俯仰角θ及偏航角ψ。

图17所示的天线装置102具备4个天线100A、100B、100C、100D。天线100A和天线100C沿着船头方向配置。天线100B和天线100D沿着与船头方向正交的方向配置。从天线100A的位置到天线100C的位置的距离远大于从天线100B的位置到天线100D的位置的距离。天线100A的位置和天线100C的位置的中点与天线100B的位置和天线100D的位置的中点一致。像这样，天线装置102按照平行于船头方向的对角线较长、正交于船头方向的对角线较短的菱形形状配置天线100A、100B、100C、100D。

通过采用这样的天线配置，能够进一步增多船头方向的成分较长的基线矢量数。由此，能够更高精度地计算俯仰角θ及偏航角ψ。

图18所示的天线装置103与图16所示的天线装置101相比，进一步追加了天线100D、100E。天线100D、100E配置在连结天线100B的位置和天线100C的位置的垂直二等分线上。天线100D、100E配置在天线100A的位置和天线100C的位置的中点与天线100A的位置之间。即，天线100A、100D、100E沿着船头方向隔开间隔配置。

通过采用这样的天线配置，能够进一步增多船头方向的成分较长的基线矢量数。由此，能够更高精度地计算俯仰角θ及偏航角ψ。

另外，图16、图18所示的天线配置，也可以是以与航向正交的面为基准面而面对称的配置。

此外，天线的配置样式不限于这些例子，想要高精度地计算特定的姿态角成分的情况下，以构成沿着包含该特定的姿态角成分的二平面所共有的轴方向具有较长的矢量成分的基线矢量的方式配置多个天线即可。

此外，在上述的说明中，示出了根据基线矢量来计算作为姿态角的各成分、即俯仰角θ、滚转角φ及偏航角ψ的例子。但是，将使用测位信号的姿态角计算部(或定位部)和IMU(Inertial Measurement Unit(惯性计测装置))综合的综合系统的情况下，也可以通过如下的方法来计算偏航角。俯仰角θ和滚转角φ通过惯性传感器来计测。运算部使用由惯性传感器计测的俯仰角θ及滚转角φ和基于测位信号的基线矢量来计算偏航角。此外，也可以通过惯性传感器仅计测俯仰角θ，根据由惯性传感器计测的俯仰角θ和基线矢量来依次计算滚转角φ和偏航角ψ。

此外，在上述的说明中，未具体示出各天线间的距离、即基线长度和测位信号的波长的关系，但是如果最短的基线长度为测位信号的半波长以下，则本发明的构成更有效地发挥作用。

具体地说，基线长度为半波长以下时，虽然容易决定整数值偏置，但是基线长度较短，所以基线矢量的计算精度较低。由此，计算的姿态角的精度变低，所以作为姿态角检测传感器的精度不足。

但是，使用本发明的构成及处理的情况下，使用多个基线矢量来计算姿态角，所以无论怎样的基线长度，都能够提高姿态角的精度。因此，即使基线长度为半波长以下的天线配置，也能够高精度地计算姿态角。通过像这样将基线长度控制在测位信号的半波长以下，容易决定整数值偏置，能够快速地得到高精度的姿态角。此外，能够缩短天线的间隔，小型地构成天线装置。

Claims (15)

1.一种状态计算装置，具备：

3个以上的天线，配置在移动体上的不同位置，分别接收测位信号；

相关处理部，基于所述测位信号与该测位信号的复制信号的相关处理，按每个天线分别计算载波相位差；

载波相位测定值计算部，计算所述载波相位差的积分值即载波相位测定值；

基线矢量计算部，基于所述载波相位测定值计算多个基线矢量；以及

姿态角计算部，按照每个所述基线矢量计算偏航角，基于该计算出的多个偏航角计算代表偏航角。

2.如权利要求1所述的状态计算装置，

所述姿态角计算部按照每个所述基线矢量，根据代表俯仰角及代表滚转角计算所述偏航角。

3.如权利要求2所述的状态计算装置，

所述姿态角计算部按照每个所述基线矢量，根据所述代表俯仰角计算所述滚转角，根据该计算出的多个滚转角计算代表滚转角。

4.如权利要求3所述的状态计算装置，

所述姿态角计算部将所述多个基线矢量中的每2个作为组，按照每个组计算俯仰角，根据计算出的多个俯仰角计算所述代表俯仰角。

5.如权利要求1～4中任一项所述的状态计算装置，

所述3个以上的天线，以所述多个基线矢量的至少1个与所述移动体的航向平行的方式配置。

6.如权利要求5所述的状态计算装置，

与所述航向平行的基线矢量，是所述多个基线矢量中的基线长度最短的基线矢量。

7.如权利要求1～4中任一项所述的状态计算装置，

所述3个以上的天线，以使该3个以上的天线的配置位置的中心点到各天线的距离相同的方式配置在所述移动体上。

8.如权利要求1～4中任一项所述的状态计算装置，

所述多个基线矢量中的最短的基线矢量的基线长度是所述测位信号的半波长以下。

9.如权利要求1～4中任一项所述的状态计算装置，

该状态计算装置还具备位置计算部，该位置计算部使用姿态角和针对所述多个测位信号的所述相关处理的结果，计算所述移动体的特定位置的坐标，该姿态角由所述姿态角计算部所计算出的代表俯仰角、代表滚转角、以及代表偏航角组成，

在计算所述特定位置的坐标时，该位置计算部进行基于所述姿态角的修正。

10.如权利要求1～4中任一项所述的状态计算装置，

该状态计算装置还具备速度计算部，该速度计算部使用姿态角和针对所述多个测位信号的所述相关处理的结果，计算所述移动体的特定位置的速度，该姿态角由所述姿态角计算部所计算出的代表俯仰角、代表滚转角、以及代表偏航角组成，

在计算所述特定位置的速度时，该速度计算部进行基于所述姿态角的修正。

11.一种移动体，具备：

权利要求1～10中任一项所述的状态计算装置；以及

控制部，使用所述状态计算装置输出的姿态角进行移动控制。

12.一种状态计算方法，包括：

接收步骤，在移动体上的不同位置分别接收3个以上的测位信号；

载波相位测定值计算步骤，基于所述测位信号与该测位信号的复制信号的相关处理，按照每个所述位置分别计算载波相位差，并计算该载波相位差的积分值即载波相位测定值；

基线矢量计算步骤，基于所述载波相位测定值计算多个基线矢量；以及

姿态角计算步骤，按照每个所述基线矢量计算偏航角，并基于计算出的多个偏航角计算代表偏航角。

13.如权利要求12所述的状态计算方法，

在所述姿态角计算步骤中，按照每个所述基线矢量，根据代表俯仰角及代表滚转角计算所述偏航角。

14.如权利要求13所述的状态计算方法，

在所述姿态角计算步骤中，按照每个所述基线矢量，根据所述代表俯仰角计算所述滚转角，根据该计算出的多个滚转角计算代表滚转角。

15.如权利要求14所述的状态计算方法，

在所述姿态角计算步骤中，将所述多个基线矢量中的每2个作为组，按照每个组计算俯仰角，根据计算出的多个俯仰角计算所述代表俯仰角。