CN110869808B - 方位推定装置 - Google Patents

Abstract

本发明涉及方位推定装置。具备：卫星数据获取部(101)，获取方位推定装置、导航信号的多普勒频移频率；传感器数据获取部(102)，从偏航率传感器获取偏航率；多普勒方位推定部(103)，推定移动体的方位；状态变量决定部(S14)，决定表示方位角的可靠度的状态变量(S)；阈值选择部(S16)，从用于决定是否采用上述方位作为基准方位的多个组的评价参数阈值群选择上述评价参数阈值群；评价参数计算部(S17)，计算用于评价方位的可靠度的多个评价参数；以及采用与否判断部(S18、S19)，对多个上述评价参数和上述阈值选择部选择的上述评价参数阈值群进行比较，并基于判断为满足采用条件而决定为采用上述方位作为上述基准方位。

Description

方位推定装置

相关申请的交叉引用

本申请基于2017年7月12日申请的日本专利申请号2017－136433号，并在此引用其记载内容。

技术领域

本公开涉及推定移动体移动的方位的方位推定装置。

背景技术

在专利文献1中公开了按照时间序列积蓄导航卫星发送的导航信号的频率位移量亦即多普勒频移频率，并且按时间序列获取车速以及偏航率的一方或者两方，且根据它们高精度地推定方位的方法。

另外，广泛周知被称为自主导航的当前位置推定方法。在自主导航中，将根据偏航率传感器检测到的值所决定的方位角变化量与基准方位相加来更新方位。另外，速度也是根据速度传感器或加速度传感器的检测值来推定。

专利文献1:日本专利第5879977号公报

若使用专利文献1所公开的方法则能够高精度地推定方位。因此，考虑将通过专利文献1所公开的方法推定的方位设为自主导航所使用的基准方位。

然而，当然推定的方位有误差，且该误差在每次推定方位时变化。因此，在能够利用专利文献1所公开的方法推定方位的情况下，若必须将该方位设为基准方位，则推定的方位的误差有可能增加。

另一方面，广泛周知若自主导航继续，则偏航率传感器检测的偏航率的误差被累计，由此方位推定的误差增加。因此，即使不更新基准方位的时间变长，方位推定误差较大的状态也继续。

发明内容

本公开的目的在于提供能够抑制在自主导航中推定的方位的误差增加的方位推定装置。

本公开的第一方式所涉及的方位推定装置是搭载在移动体上，依次推定移动体移动的方位的方位推定装置，具备：卫星数据获取部，按时间序列获取GNSS接收机接收的导航信号的多普勒频移频率；传感器数据获取部，从移动体中所使用的偏航率传感器按时间序列获取移动体的偏航率；多普勒方位推定部，基于方程式来推定移动体的方位，方程式是基于按时间序列获取的多个多普勒频移频率建立的推定各时刻的移动体的方位的方程式，且在每个时刻变化的未知参数被基于偏航率计算的方位角变化量约束；状态变量决定部，决定状态变量，状态变量根据在自主导航中加上方位角变化量的基准方位被更新之后的经过时间而增加，且表示方位角的可靠度；阈值选择部，状态变量决定部决定的状态变量越是表示方位角的可靠度低的值，则从用于决定是否采用多普勒方位推定部推定的方位作为基准方位的多个组的评价参数阈值群选择即使方位角的可靠度较低也容易满足采用条件的评价参数阈值群；评价参数计算部，在多普勒方位推定部推定出方位的情况下，计算用于评价该方位的可靠度的多个评价参数；以及采用与否判断部，对多个评价参数和阈值选择部选择的评价参数阈值群进行比较，并基于判断为满足采用条件，决定为采用多普勒方位推定部推定的方位作为基准方位。

另外，本公开的第二方式所涉及的方位推定装置是搭载在移动体上，依次推定移动体移动的方位的方位推定装置，具备：卫星数据获取部，按时间序列获取GNSS接收机接收的导航信号的多普勒频移频率；传感器数据获取部，从移动体中所使用的速度传感器按时间序列获取移动体的速度的大小，并且从移动体中所使用的偏航率传感器按时间序列获取移动体的偏航率；多普勒方位推定部，基于方程式来推定移动体的方位，方程式是基于按时间序列获取的多个多普勒频移频率建立的推定各时刻的移动体的方位的方程式，且在每个时刻变化的未知参数被移动体的速度的大小以及时钟漂移的时间变化的线形化约束；状态变量决定部，决定状态变量，状态变量在自主导航中，根据加上基于偏航率计算的方位角变化量的基准方位被更新之后的经过时间而增加，且表示方位角的可靠度；阈值选择部，状态变量决定部决定的状态变量越是表示方位角的可靠度低的值，则从用于决定是否采用多普勒方位推定部推定的方位作为基准方位的多个组的评价参数阈值群选择即使方位角的可靠度较低也容易满足采用条件的评价参数阈值群；评价参数计算部，在多普勒方位推定部推定出方位的情况下，计算用于评价该方位的可靠度的多个评价参数；以及采用与否判断部，对多个评价参数和阈值选择部选择的评价参数阈值群进行比较，并基于判断为满足采用条件，决定为采用多普勒方位推定部推定的方位作为基准方位。

上述这些方位推定装置决定表示方位角的可靠度的大小的状态变量，该状态变量越表示方位角的可靠度低，则将与多个评价参数比较的评价参数阈值群设为即使方位角的可靠度较低也容易满足采用条件的评价参数阈值群。由此，多普勒方位推定部推定的方位即使可靠度足够高也被采用为基准方位。因而，由于容易提前更新基准方位，所以能够抑制在自主导航中推定的方位的误差增加。

附图说明

关于本公开的上述目的以及其它目的、特征及优点，参照附图并通过下述的详细描述会变得更加明确。在其附图中：

图1是表示方位推定装置的结构的框图。

图2是表示图1的多普勒方位推定部执行的处理的流程图。

图3是表示图1的INS方位决定部执行的处理的流程图。

图4是表示在图3的一部分处理中所使用的方位可靠度等级决定表的图。

图5是表示在图3的一部分处理中所使用的阈值群选择表的图。

图6是表示评价参数阈值群列表的图。

具体实施方式

以下，基于附图对实施方式进行说明。图1所示的方位推定装置100与GNSS接收机2、车速传感器3、陀螺仪传感器4、档位传感器5一起搭载在作为移动体的车辆1上。若车辆1的电源接通，则在方位推定装置100也被供给电源，在向方位推定装置100供给电源的期间，方位推定装置100依次推定方位角θ。

＜结构的概要＞

GNSS接收机2接收GNSS(GlobalNavigationSatelliteSystem：全球导航卫星系统)具备的导航卫星Si发送的导航信号。此外，i是导航卫星的编号。GNSS例如是GPS。导航信号叠加在载波上并作为电波从导航卫星Si发送。以下，将导航卫星Si发送的电波称为GNSS电波。GNSS接收机2对接收到的GNSS电波进行解调来取出导航信号。

而且，根据取出的导航信号来决定伪距ρ_i、多普勒频移频率D_i、卫星位置(X_si，Y_si，Z_si)、卫星的状态、导航数据等。导航数据包括导航卫星Si的卫星编号、作为导航卫星Si的轨道信息的星历表、导航卫星Si发送电波的时刻等。

对于各导航卫星Si的卫星位置(X_si，Y_si，Z_si)而言，基于各导航卫星Si的星历表以及发送GNSS电波的时刻进行计算。对于伪距ρ_i而言，通过导航卫星Si发送GNSS电波的时刻与GNSS接收机2接收GNSS电波的时刻的时刻差即电波传播时间乘以光速C来计算。

多普勒频移频率D_i是导航卫星Si发送的电波的载波的频率与接收到的GNSS电波的载波的频率的频率差。预先决定导航卫星Si发送的电波的载波频率，该频率预先存储在GNSS接收机2具备的规定的存储部中。

GNSS接收机2将它们与作为接收信号的导航信号的S/N、接收导航信号的时刻一起按一定周期输出至方位推定装置100。GNSS接收机2输出至方位推定装置100的信息是在卫星导航中运算位置前的信息。以下，将在卫星导航中运算位置前的信息设为卫星数据。GNSS接收机2输出卫星信息的一定周期例如是200毫秒～400毫秒之间。存在多个导航卫星Si。GNSS接收机2根据能够从GNSS电波解调的全部导航信号来决定卫星数据，并将决定的全部卫星数据输出至方位推定装置100。

车速传感器3检测车辆1的车轮的旋转速度。车速传感器3将表示车轮的旋转速度的信号输出至方位推定装置100。

陀螺仪传感器4检测绕车辆1的横摆轴、俯仰轴、横摇轴的旋转角速度，并将表示检测到的旋转角速度的信号输出至方位推定装置100。由于陀螺仪传感器4检测绕横摆轴的旋转角速度即偏航率，所以作为偏航率传感器发挥作用。

档位传感器5检测车辆1的换档位置，并将表示该换档位置的信号输出至方位推定装置100。通过档位传感器5输出的信号决定车辆1的移动方向是前进以及后进的哪一个。

车速传感器3、陀螺仪传感器4、档位传感器5是输出表示车辆1的运动即举动的信号的举动检测传感器6。

方位推定装置100是具备未图示的CPU、ROM、RAM等的计算机。CPU利用RAM的临时存储功能并执行存储在ROM等非迁移实体记录介质中的程序。由此，方位推定装置100执行作为卫星数据获取部101、传感器数据获取部102、多普勒方位推定部103、INS方位决定部104的功能。若执行这些功能，则执行与存储在非迁移实体记录介质中的程序对应的方法。此外，也可以通过一个或多个IC等以硬件的方式构成方位推定装置100执行的功能的一部分或者全部。

＜方位推定装置100执行的处理的概要＞

接下来，对方位推定装置100执行的处理的概要进行说明。此外，对于方位推定装置100执行的一部分的处理，使用图2、图3所示的流程图进行详述。

卫星数据获取部101从GNSS接收机2以卫星数据获取周期按时间序列获取卫星数据，并将获取的卫星数据存储至卫星数据积蓄部110。卫星数据获取周期与GNSS接收机2输出卫星数据的周期相同。卫星数据积蓄部110是可以写入的存储介质，可以是易失性，也可以是非易失性。卫星数据积蓄部110例如可以使用RAM。

传感器数据获取部102以传感器值获取周期按时间序列获取举动检测传感器6检测到的信号。传感器值获取周期比GNSS接收机2输出导航信号的周期短，例如是数十毫秒。传感器数据获取部102将获取的信号存储至传感器数据积蓄部111。传感器数据积蓄部111是可写入的存储介质，可以是易失性，也可以是非易失性。传感器数据积蓄部111例如可以使用RAM。另外，传感器数据积蓄部111能够使用与卫星数据积蓄部110相同的存储介质。

多普勒方位推定部103使用(1)式所示的方位推定式，来计算初始时刻的车辆1的行进方向的方位角(即方位角初始值)θ⁰。此外，初始时刻是指更新了方位角初始值θ⁰的时刻。

[数1]

Vs^t _i＝V^t _wheelGx^t _icos(θ⁰+θ^t _gyro)+V^t _wheelGy^t _isin(θ⁰+θ^t _gyro)-Cbv⁰-At   (1)

(1)式是在专利文献1中作为(6)式所记载的式子。在(1)式中，Vs_i是卫星方向速度，t是时刻，V_wheel是车速传感器3的检测值，θ_gyro是车辆1的行进方向的方位角的变化量即相对方位角，Cbv⁰是初始时刻的时钟漂移，A是时钟漂移Cbv的时间变化的斜率，Gx、Gy是从车辆1到导航卫星Si的视线向量的x成分、y成分。

处于(1)式的左边的卫星方向速度Vs_i根据(2)式来计算。

[数2]

(2)式的右边的第一项意味着车辆1相对于导航卫星Si的相对速度Vr_i，第二项、第三项、第四项是导航卫星Si在车辆1的方向上的速度。它们的和意味着车辆1在导航卫星Si的方向上的速度，所以(2)式成立。在(2)式中，D_i是多普勒频移频率。因而，(1)式使用多普勒频移频率D_i来计算方位角初始值θ⁰。

使用图2对多普勒方位推定部103的处理进行说明。在S1中，根据卫星数据积蓄部110中存储的各导航卫星Si的卫星位置(X_si，Y_si，Z_si)的时间序列数据来计算各导航卫星Si的速度向量(Vxs_i，Vys_i，Vzs_i)。

在S2中，将卫星数据积蓄部110中存储的卫星数据所包含的多普勒频移频率D_i代入到(3)式中，计算本车辆相对于导航卫星Si的当前的相对速度Vr_i。此外，在(3)式中，C是光速，F是导航卫星Si发送的GNSS电波的频率。

[数3]

Vr_i＝-D_i·C/F  (3)

在S3中，计算当前的车辆1的位置P(X_v，Y_v，Z_v)。车辆1的位置P用于计算从车辆1到导航卫星Si的视线向量(Gx_i，Gy_i，Gz_i)。

如广泛周知那样，如果接收到四个以上的质量良好的导航信号，则使用导航信号建立四个以上的联立方程式，由此能够计算作为未知数的位置P(X_v，Y_v，Z_v)和时钟漂移Cbv。然而，此处的车辆1的位置用于在接下来的S4中计算从车辆1到导航卫星Si的视线向量(Gx_i，Gy_i，Gz_i)。由于导航卫星Si存在于远方，所以为了求出导航卫星Si和车辆1的角度而使用的当前位置的精度可以低。因此，可以不接收四个以上的质量良好的信号。

因此，例如在该S3中，能够也包括不能够判断为质量良好的信号而使用四个以上的导航信号，来计算当前的车辆1的坐标(X_v，Y_v，Z_v)。在根据四个以上的导航信号来计算当前的车辆1的坐标(X_v，Y_v，Z_v)的情况下，计算伪距ρ_i、和导航卫星Si的卫星位置(X_si，Y_si，Zs)，并决定车辆1的坐标(X_v，Y_v，Z_v)，以使得伪距ρ_i的残差最小。

另外，也可以通过使用伪距ρ_i的位置决定以外的精度低的方法决定车辆1的位置P(X_v，Y_v，Z_v)。取决于系统等允许的推定精度，但如果车辆1的位置误差是数百m的范围，则速度推定误差成为1m/sec以下，没有大的问题。因此，例如，可以从地图等决定位置，另外，也可以根据过去的位置的测定历史、信标等信息等决定车辆1的位置P(X_v，Y_v，Z_v)。

在S4中，计算从车辆1到导航卫星Si的视线向量(Gx_i，Gy_i，Gz_i)。视线向量的x成分、y成分、z成分根据(4)式计算。

[数4]

在(4)式中，ρ^t _i是时刻t的导航卫星Si的伪距，(X^t _si，Y^t _si，Z^t _si)是时刻t的导航卫星Si的卫星位置。(X^t _V，Y^t _V，Z^t _V)是时刻t的车辆1的当前位置，在S3中进行计算。

在S5中，在前述的(2)式中代入在S2中计算出的相对速度Vr_i、S4中计算出的视线向量(Gx_i，Gy_i，Gz_i)、在S1中计算出的导航卫星Si的速度向量(Vxs_i，Vys_i，Vzs_i)，来计算车辆1在导航卫星Si的方向上的卫星方向速度Vs_i。

在S6中，在(1)式中代入在S5中计算出的卫星方向速度Vs_i、使用从传感器数据积蓄部111获取的偏航率更新后的相对方位角θ_gyro、在S4中计算出的视线向量(Gx_i，Gy_i，Gzi)。由此，在(1)式中，未知参数成为θ⁰、Cbv⁰以及A这三个。

因此，建立三个以上的在(1)式中代入这些卫星方向速度Vs_i、相对方位角θ_gyro、视线向量(Gx_i，Gy_i，Gz_i)后的式子。而且，解开由那三个以上的式子构成的联立方程式。由此，能够求出(1)式中成为未知参数的方位角初始值θ⁰。

如果未知参数θ⁰、Cbv⁰、A是在初始时刻以后，则即使时刻不同也相同。因此，不需要在同一时刻建立三个式子，只要在多个时刻建立的式子数合计是3个式子以上，则能够求出未知参数。例如，即使假设3个时刻(t₀，t₁，t₂)的观测卫星数分别是1，也能够使用观测到的卫星数据来求出方位角初始值θ⁰。

INS方位决定部104通过根据作为惯性传感器的陀螺仪传感器4的检测值求出的偏航率乘以传感器值获取周期来计算方位角变化量(即相对方位角θ_gyro)。将该方位角变化量与基准方位相加来决定方位角θ。基准方位是多普勒方位推定部103推定的方位角初始值θ⁰。

由于以传感器值获取周期获得偏航率，所以INS方位决定部104按传感器值获取周期更新方位角θ。方位角θ是通过对作为基准方位的方位角初始值θ⁰加上相对方位角θ_gyro来求出。

但是，INS方位决定部104不一定在每次多普勒方位推定部103更新方位角初始值θ⁰时将基准方位更新为最新的方位角初始值θ⁰。是因为方位角初始值θ⁰也有误差，使用至此所使用的方位角初始值θ⁰也有可能能够决定误差较少的方位角θ。

使用图3，对INS方位决定部104执行的处理进行说明。INS方位决定部104以传感器值获取周期执行图3所示的处理。

在S11中，判断多普勒方位推定部103是否计算出方位角初始值θ⁰。为了计算方位角初始值θ⁰，需要卫星数据。卫星数据以比传感器值获取周期长的卫星数据获取周期获取。另外，若电波环境较差，则也有时不能够接收导航信号。因此，S11的判断也有时为否。前一次执行该图3以后，多普勒方位推定部103计算方位角初始值θ⁰的情况下，将S11的判断设为是。在S11的判断为是的情况下，进入S12。

在S12中，判断多普勒方位推定是否在继续。多普勒方位意味着多普勒方位推定部103推定的方位，即，方位角初始值θ⁰。多普勒方位推定在继续意味着多普勒方位推定部103是能够继续推定方位的状态。在多普勒方位推定部103是能够每隔卫星数据获取周期连续地进行一定时间以上的方位推定的情况下，设为多普勒方位推定在继续。一定时间设定为卫星数据获取周期的多次的以上的时间，例如1秒。

若多普勒方位推定在继续，则推定的方位的误差变小，即，推定的方位的可靠度变高。因此，在S12的判断为是的情况下，在S13中将方位可靠度等级设为最高等级。此外，将状态变量S的值设为0。状态变量S在S14中进行说明。执行S13后进入S19。

在S12的判断为否的情况下，进入S14。在S14中计算状态变量S。在本实施方式中，状态变量S使用(5)式来计算。

[数5]

在(5)式中，S⁰是状态变量S的初始值。该初始值S⁰是方位推定装置100的电源接通后，到执行S13为止所使用的值。若执行S13，则状态变量S变为0。因而，若执行S13，则该初始值S⁰变为0。

在存前一次方位推定装置100的电源断开时的状态变量S的值亦即前一次结束值的情况下，初始值S⁰是该前一次结束值。在不存在前一次结束值时，设为预先设定的初始值。

在(5)式中，tc是INS方位决定部104使用于方位角θ的计算的方位角初始值θ⁰被更新、且此时的方位可靠度等级成为最高等级的时刻。但是，在方位推定装置100的电源接通后，到执行S13为止，tc是电源被接通的时刻。

t是当前的时刻，v是车速，A(v)是将车速作为变量规定的值。具体而言，速度v相对高时，A(v)相对小，速度v相对低时，A(v)相对大。例如，A(v)能够设为如下的函数：在车速为0时，A(v)为0，在速度v高于某个速度v1时，A(v)成为A1，在速度v低于该速度v1且高于0时，A(v)成为A2(＞A1)。

并不限于此，也能够将A(V)设为随着速度v增加，值连续地减少的函数。另外，也能够将A(v)设为决定与速度v对应的值的表形式。

INS方位决定部104更新使用于方位角θ的计算的方位角初始值θ⁰之后的经过时间越长，则状态变量S的值越大。更新方位角初始值θ⁰之后的经过时间越长，则偏航率的误差被累计，所以方位角θ的误差越大。状态变量S是用于评价该误差的变量。因此，以状态变量S的值与方位角θ的误差相关的方式决定A(v)。如前述那样，速度v相对大时相对减小A(v)的理由是因为速度v越高则偏航率的误差越小。

在S15中，根据状态变量S进而图4所示的方位可靠度等级决定表来决定方位可靠度等级。在图4中，DIRLVn＿LMT(n为1～5)是针对于状态变量S的阈值。因而，图4所示的方位可靠度等级决定表是根据状态变量S的值决定方位可靠度等级的表。

在图4中，方位推定运算继续中意味着前述的多普勒方位推定在继续。自主导航意味着根据INS方位决定部104决定的方位角θ和速度v依次运算车辆1的移动量，对车辆1的当前位置进行更新的定位方法。

在图4中，方位可靠度等级的数值越大则方位角θ的可靠度越高，换言之，意味着方位角θ的误差较小。在图4中，方位可靠度等级为7时，意味着误差是0.5度以下。

方位可靠度等级为最高等级时的误差是何种程度通过预先计测与参照值的差收敛至何种程度来决定。参照值是在车辆1上搭载构成为包括飞机中所使用的高精度陀螺仪传感器、高分辨率车速计等的高精度位置计测系统并测定出的值。在电波环境良好的场所中，该参照值与多普勒方位推定部103推定的方位的差收敛至某个范围时，将该差的范围设为最高等级下的误差。

决定低于最高等级的方位可靠度等级的状态变量S的阈值也通过实际测定与上述的高精度位置计测系统测定出的参照值的差来决定。

在紧接着的S16中，根据在S15中决定的方位可靠度等级和当前的速度v、以及图5所示的阈值群选择表选择评价参数阈值群P。阈值群选择表是用于从多个组的评价参数阈值群P选择一个评价参数阈值群P的表。

各评价参数阈值群P具备针对于多个评价参数的阈值(以下，评价参数阈值)。评价参数阈值是用于评价多普勒方位推定部103推定的方位的误差的阈值。评价参数例如是每个导航卫星Si的多普勒频移频率D_i的残差的最大值、平均、方差、协方差、时间序列方差等。如从(1)式、(2)式所知那样，由于多普勒频移频率D_i给方位角初始值θ⁰的值带来影响，所以它们成为评价多普勒方位推定部103推定的方位的误差的评价参数。

图5是根据方位可靠度等级和速度等级来决定评价参数阈值群P的表。图5所示的评价参数阈值群P1～P4分别由多个评价参数阈值构成。图6示出评价参数阈值群P1～P4具备的评价参数阈值的例子。在图6中，THRE＿VAR1＿A、THRE＿VAR1＿B等是评价参数阈值。

在图5所示的阈值群选择表中，方位可靠度等级越低，换句话说，方位可靠度等级越是表示方位可靠度低的等级，则评价参数阈值成为表示越低可靠度的值。另外，速度等级越是低速侧的等级，则评价参数阈值成为越表示低可靠度的值。

构成图5所示的评价参数阈值群P的各评价参数阈值与图4同样地通过在车辆1上搭载高精度位置计测系统，计测出的参照值与多普勒方位推定部103推定的方位的差分别收敛至图4所示的方位可靠度等级的状态下，根据实际获得的多普勒频移频率D_i等来计算并决定。

因而，在多普勒方位推定部103推定方位时，通过将根据此时的多普勒频移频率D_i等计算出的评价参数与评价参数阈值进行比较，能够评价多普勒方位推定部103推定的方位的可靠度。

因此，在S17中，根据多普勒方位推定部103推定方位时的多普勒频移频率D_i等来计算评价参数。在S18中，对构成在S16中选择出的评价参数阈值群P的各评价参数阈值和在S17中计算出的评价参数进行比较，判断是否满足采用条件。

例如，能够设为全部评价参数实现评价参数阈值这个条件。此外，评价参数实现评价参数阈值意味着评价参数与评价参数阈值相比，成为方位的可靠度较高的侧的值。

采用条件除此以外，也可以在多个评价参数中的规定比例(例如8成)实现评价参数阈值的情况下满足采用条件。另外，也可以针对各评价参数的实现程度设置等级并进行评分，使用全部评价参数的合计分数来判断是否满足采用条件。

在S18的判断为是的情况下，能够认为多普勒方位推定部103推定的方位的可靠度高于当前时刻的方位角θ的可靠度。因此，如果S18的判断为是则进入S19，采用多普勒方位推定部103计算出的方位角初始值θ⁰，作为INS方位决定部104所使用的基准方位。即，对INS方位决定部104所使用的方位角初始值θ⁰进行更新。另外，在执行S13的情况下，也执行该S19。在S20中，计算从更新方位角初始值θ⁰到当前时刻为止的方位角变化量，并将该方位角变化量与方位角初始值θ⁰相加，从而更新方位角θ。

在S21中，将在S20中更新的方位角θ、和最新的方位可靠度等级输出至使用它们的应用程序。作为应用程序的例子，有使用该方位角θ和车辆1的速度v来进行自主导航的位置推定应用程序。

此外，在图3的处理中，S14相当于状态变量决定部，S15相当于可靠度等级决定部，S16相当于阈值选择部，S17相当于评价参数计算部，S18、S19相当于采用与否判断部。

＜实施方式的总结＞

本实施方式的方位推定装置100计算表示方位角θ的可靠度的大小的状态变量S(S14)，该状态变量S表示方位角θ的可靠度越低，则将与多个评价参数进行比较的评价参数阈值群P设为即使方位角θ的可靠度较低也容易满足采用条件的评价参数阈值群P。因而，即使多普勒方位推定部103推定的方位的可靠度不足够高，也被采用为基准方位。由此，由于容易提前更新基准方位，所以能够抑制在自主导航中推定的方位的误差增加。

以上，对实施方式进行了说明，但公开的技术并不限于上述的实施方式，如下的变形例也包含在公开的范围中，并且除了下述以外也能够在不脱离主旨的范围内实施各种变更。

该申请所记载的流程图或流程图处理由多个部(或称作步骤)构成，各部例如表现为S2。进而，各部可以分割成多个子部，另一方面，也可以将多个部合成一个部。进而，这样构成的各部可以称作为设备、模块、方法。

＜变形例1＞

前述的实施方式所示的(1)式是如专利文献1也记载那样，采用如下的约束条件1～3，对卫星方向速度Vs_i和车辆1的速度向量的关系式进行约束而获得的式子。此外，Vx、Vy分别是车辆1的速度向量的x成分、y成分。

[数6]

约束条件1

约束条件2Vz^t＝O

约束条件3Cbv^t＝Cbv⁰+At

如专利文献1也公开那样，作为约束条件，无需全部使用这三个约束条件。也可以设为仅使用车速数据、方位角变化量、时钟漂移Cbv的时间变化中任意一个的约束条件。另外，也可以设为适当地组合各个而成的约束条件。

作为约束条件，在仅使用方位角变化量的情况下，在(1)式中，V^t _wheel、θ⁰、以及Cbv^t成为未知参数。因而，时间序列数据的全部时刻中的未知参数数为“时刻数×2+1”。

另外，作为约束条件，在仅使用时钟漂移Cbv的时间变化的情况下，在(1)式中，V^t _wheel、θ⁰、θ^t _gyro、Cbv⁰以及A成为未知参数，所以时间序列数据的全部时刻的未知参数数为“时刻数×2+3”。

另外，作为约束条件，在使用时钟漂移Cbv的时间变化以及车速数据的情况下，在(1)式中，θ⁰、θ^t _gyro、Cbv⁰以及A成为未知参数，所以时间序列数据的全部时刻的未知参数数为“时刻数×1+3”。

＜变形例2＞

在前述的实施方式中，移动体是车，但移动体也可以是车以外。

Claims (10)

1.一种方位推定装置，搭载于移动体，依次推定上述移动体移动的方位，具备：

卫星数据获取部，按时间序列获取GNSS接收机接收的导航信号的多普勒频移频率；

传感器数据获取部，从在上述移动体中所使用的偏航率传感器按时间序列获取上述移动体的偏航率；

多普勒方位推定部，基于方程式来推定上述移动体的方位，其中，该方程式是基于按时间序列获取的多个上述多普勒频移频率来建立的推定各时刻的上述移动体的方位的方程式，且是在每个时刻变化的未知参数被基于上述偏航率计算的方位角变化量所约束的方程式；

状态变量决定部，决定状态变量，其中，该状态变量根据在自主导航中加上上述方位角变化量的基准方位被更新之后的经过时间来增加，且该状态变量表示方位角的可靠度；

阈值选择部，上述状态变量决定部决定的上述状态变量越是表示上述方位角的可靠度低的值，则从用于决定是否采用上述多普勒方位推定部推定的方位作为上述基准方位的多个组的评价参数阈值群，选择即使上述方位角的可靠度低也容易满足采用条件的上述评价参数阈值群；

评价参数计算部，在上述多普勒方位推定部推定出方位的情况下，计算用于评价该方位的可靠度的多个评价参数；以及

采用与否判断部，对多个上述评价参数与上述阈值选择部选择的上述评价参数阈值群进行比较，并基于判断为满足上述采用条件这一情况，决定采用上述多普勒方位推定部推定的方位作为上述基准方位。

2.根据权利要求1所述的方位推定装置，其中，

上述状态变量在上述移动体的速度相对大的情况下，与上述移动体的速度相对小的情况相比，与上述经过时间的增加伴随的增加量少。

3.根据权利要求2所述的方位推定装置，其中，

还具备可靠度等级决定部，该可靠度等级决定部基于上述状态变量决定部决定的上述状态变量来决定方位可靠度等级，

上述阈值选择部基于上述可靠度等级决定部决定的上述方位可靠度等级和速度等级来选择上述评价参数阈值群，上述方位可靠度等级越是表示上述方位角的可靠度低的等级，并且上述速度等级越是速度低的等级，则以越低的上述方位角的可靠度选择满足上述采用条件的上述评价参数阈值群。

4.根据权利要求3所述的方位推定装置，其中，

在上述多普勒方位推定部处于连续一定时间以上能够推定出上述方位时，上述可靠度等级决定部将上述方位可靠度等级决定为可靠度最高的等级。

5.根据权利要求1～4中的任意一项所述的方位推定装置，其中，

上述状态变量决定部将上述方位推定装置被接通电源后的上述状态变量的初始值，设为前一次的被断开电源时的上述状态变量的值亦即前一次结束值，并在没有上述前一次结束值的情况下，设为预先设定的初始值。

6.一种方位推定装置，搭载于移动体，依次推定上述移动体移动的方位，具备：

卫星数据获取部，按时间序列获取GNSS接收机接收的导航信号的多普勒频移频率；

传感器数据获取部，从在上述移动体中所使用的速度传感器按时间序列获取上述移动体的速度的大小，并且从在上述移动体中所使用的偏航率传感器按时间序列获取上述移动体的偏航率；

多普勒方位推定部，基于方程式来推定上述移动体的方位，其中，该方程式是基于按时间序列获取的多个上述多普勒频移频率来建立的推定各时刻的上述移动体的方位的方程式，且是在每个时刻变化的未知参数被上述移动体的速度的大小以及时钟漂移的时间变化的线形化所约束的方程式；

状态变量决定部，决定状态变量，其中，该状态变量根据在自主导航中加上基于上述偏航率计算的方位角变化量的基准方位被更新之后的经过时间来增加，且该状态变量表示方位角的可靠度；

阈值选择部，上述状态变量决定部决定的上述状态变量越是表示上述方位角的可靠度低的值，则从用于决定是否采用上述多普勒方位推定部推定的方位作为上述基准方位的多个组的评价参数阈值群，选择即使上述方位角的可靠度低也容易满足采用条件的上述评价参数阈值群；

评价参数计算部，在上述多普勒方位推定部推定出方位的情况下，计算用于评价该方位的可靠度的多个评价参数；以及

采用与否判断部，对多个上述评价参数与上述阈值选择部选择的上述评价参数阈值群进行比较，并基于判断为满足上述采用条件这一情况，决定采用上述多普勒方位推定部推定的方位作为上述基准方位。

7.根据权利要求6所述的方位推定装置，其中，

上述状态变量在上述移动体的速度相对大的情况下，与上述移动体的速度相对小的情况相比，与上述经过时间的增加伴随的增加量少。

8.根据权利要求7所述的方位推定装置，其中，

还具备可靠度等级决定部，该可靠度等级决定部基于上述状态变量决定部决定的上述状态变量来决定方位可靠度等级，

上述阈值选择部基于上述可靠度等级决定部决定的上述方位可靠度等级和速度等级来选择上述评价参数阈值群，上述方位可靠度等级越是表示上述方位角的可靠度低的等级，并且上述速度等级越是速度低的等级，则以越低的上述方位角的可靠度选择满足上述采用条件的上述评价参数阈值群。

9.根据权利要求8所述的方位推定装置，其中，

在上述多普勒方位推定部处于连续一定时间以上能够推定出上述方位时，上述可靠度等级决定部将上述方位可靠度等级决定为可靠度最高的等级。

10.根据权利要求6～9中的任意一项所述的方位推定装置，其中，

上述状态变量决定部将上述方位推定装置被接通电源后的上述状态变量的初始值，设为前一次的被断开电源时的上述状态变量的值亦即前一次结束值，并在没有上述前一次结束值的情况下，设为预先设定的初始值。

Images