位置推定装置、移动体控制系统、位置推定方法以及程序

摘要

本发明的实施方式提供位置推定装置、移动体控制系统、位置推定方法以及程序，能够精度良好地推定移动体装置的自身位置。位置推定装置对设置了多个传感器的移动体装置的自身位置进行推定。位置推定装置具备状态量推定部、可靠度算出部、自身位置推定部。状态量推定部基于多个传感器各自的观测值，推定多个传感器各自的状态量。可靠度算出部算出多个传感器各自的可靠度。自身位置推定部基于多个传感器各自的可靠度，选择多个传感器中的对象传感器，基于所选择的对象传感器的观测值以及状态量，推定自身位置。

说明书

技术领域

本发明的实施方式涉及位置推定装置、移动体控制系统、位置推定方法以及程序。

背景技术

已知推定拍摄装置的自身位置的位置推定装置。位置推定装置使用通过拍摄装置拍摄到的图像以及由安装于拍摄装置的传感器观测到的观测值来推定自身位置。进一步，位置推定装置有时也在由传感器观测到的观测值的基础上使用表示传感器的状态的状态量来推定自身位置。例如，角速度传感器的状态量是角速度所包含的偏置(bias)。加速度传感器的状态量是加速度所包含的偏置、重力加速度以及速度。

位置推定装置基于由传感器观测到的观测值来推定传感器的状态量。但是，所推定的状态量的精度会根据传感器的移动轨迹等而变化。例如，在进行等速直线运动的情况下，加速度传感器的观测值大致成为一定。因此，在这样的情况下，加速度偏置、重力加速度以及速度的精度会变低。在使用了这样的精度低的状态量的情况下，位置推定装置的自身位置的推定精度会变差。

发明内容

本发明要解决的技术课题在于精度良好地推定移动体装置的自身位置。

实施方式涉及的位置推定装置对设置了一个或者多个传感器的移动体装置的自身位置进行推定，所述一个或者多个传感器观测与移动有关的信息。所述位置推定装置具备状态量推定部、可靠度算出部以及自身位置推定部。所述状态量推定部基于所述一个或者多个传感器各自的观测值，推定表示所述一个或者多个传感器各自的状态的状态量。所述可靠度算出部算出表示所述一个或者多个传感器各自的所述状态量的可靠程度的可靠度。所述自身位置推定部基于所述一个或者多个传感器各自的所述状态量的所述可靠度，选择所述一个或者多个传感器中的一个或者多个对象传感器，基于所选择的所述一个或者多个对象传感器各自的所述观测值以及所述状态量，推定所述自身位置。

根据上述位置推定装置，能够精度良好地推定移动体装置的自身位置。

附图说明

图1是表示实施方式涉及的移动体的图。

图2是移动体控制系统的构成图。

图3是位置推定装置的构成图。

图4是表示位置推定装置的处理的流程的流程图。

图5表示自身位置推定部的流程的流程图。

图6是表示基于第1方式的推定处理的流程的流程图。

图7是表示基于第2方式的推定处理的流程的流程图。

图8是表示基于第3方式的推定处理的流程的流程图。

图9是变形例涉及的位置推定装置的构成图。

标号说明

10 移动体装置

12 移动体控制系统

21 拍摄装置

22 传感器

23 存储装置

24 输入装置

25 显示装置

26 通信装置

27 移动体控制装置

40 信息处理装置

41 存储器

42 硬件处理器

43 ROM

44 RAM

50 位置推定装置

52 自身位置存储部

54 图像取得部

56 图像存储部

58 观测值取得部

60 状态量推定部

62 可靠度算出部

64 自身位置推定部

74 运动类别确定部

76 异常判定部

78 复位部

具体实施方式

以下，参照附图对本发明的实施方式进行说明。

图1是表示实施方式涉及的移动体装置10的图。在本实施方式中，移动体装置10是汽车或者摩托车等车辆。但是，移动体装置10不限于车辆，只要自身能够移动、或者能够由人或机器人等运动移动，也可以是任何的装置。例如，移动体装置10既可以是机器人，也可以是船舶，还可以是无人机等飞行物体。

移动体装置10搭载移动体控制系统12。移动体控制系统12例如是包括专用或者通用计算机的装置。移动体控制系统12也可以为：其信息处理功能不搭载于移动体装置10，而是搭载于经由网络与移动体装置10连接的云等的装置。

图2是表示实施方式涉及的移动体控制系统12的构成的一个例子的图。移动体控制系统12具备拍摄装置21、一个或者多个传感器22、存储装置23、输入装置24、显示装置25、通信装置26、移动体控制装置27、信息处理装置40。

拍摄装置21设置于移动体装置10。拍摄装置21从移动体装置10中的被固定了的位置以被固定了的姿势对移动体装置10的周围进行拍摄。对于拍摄装置21，只要能够测定与移动体装置10的相对的姿势，则也可以不是被固定了的姿势。在本实施方式中，拍摄装置21是单眼摄像头。拍摄装置21既可以是立体摄像头，也可以是深度摄像头。另外，拍摄装置21既可以是预定视角的摄像头，也可以是全方位摄像头。拍摄装置21按预定的时间间隔而对移动体装置10的周围进行拍摄，生成拍摄图像。

一个或者多个传感器22设置于移动体装置10。一个或者多个传感器22各自例如以被固定了的姿势安装于移动体装置10中的被固定了的位置。一个或者多个传感器22分别观测与移动体装置10的移动有关的信息。

在本实施方式中，一个或者多个传感器22包括角速度传感器31以及加速度传感器32。

角速度传感器31检测移动体装置10的角速度来作为观测值。角速度传感器31检测角速度的X方向分量、角速度的Y方向分量以及Z方向分量。X方向分量、Y方向分量以及Z方向分量相互正交。

加速度传感器32检测移动体装置10的平动方向的加速度来作为观测值。加速度传感器32检测加速度的X方向分量、加速度的Y方向分量以及加速度的Z方向分量。

一个或者多个传感器22也可以包括角速度传感器31和加速度传感器32以外的传感器。例如，一个或者多个传感器22也可以包括检测移动体装置10的速度的速度传感器、基于来自卫星的信号等来检测移动体装置10的位置的位置传感器。另外，一个或者多个传感器22例如也可以包括LiDAR等的对与移动体装置10周围的物体之间距离进行测定的距离传感器、通过声波对移动体装置10周围的物体进行探知的声纳以及地磁传感器等。

存储装置23是硬盘驱动器、光盘驱动器或者闪速存储器等的半导体存储器元件等。存储装置23存储由移动体控制系统12执行的程序以及移动体控制系统12所使用的数据。

输入装置24受理来自用户的指示以及信息输入。输入装置24例如是操作面板、鼠标或者轨迹球等指示(pointing)设备或者键盘等输入设备。

显示装置25对用户显示各种信息。显示装置25例如是液晶显示装置等显示设备。通信装置26通过无线通信与外部装置收发信息。

移动体控制装置27对用于使移动体装置10移动的驱动机构进行控制。例如，在移动体装置10为自动驾驶车辆的情况下，基于从信息处理装置40得到的信息以及其他信息，判断周边的状况，对加速量、制动量以及操舵角等进行控制。

信息处理装置40例如是专用或者通用的一个或者多个计算机。信息处理装置40对拍摄装置21、一个或者多个传感器22、存储装置23、输入装置24、显示装置25、通信装置26、移动体控制装置27进行管理以及控制。信息处理装置40具有存储器41和一个或者多个硬件处理器42。

存储器41例如包括ROM43(Read Only Memory，只读存储器)和RAM44(RandomAccess Memory，随机访问存储器)。ROM43以无法重写的方式存储用于信息处理装置40的控制的程序以及各种设定信息等。RAM44是SDRAM(Synchronous Dynamic Random AccessMemory，同步动态随机访问存储器)等的易失性存储介质。RAM44作为一个或者多个硬件处理器42的工作区域发挥功能。

一个或者多个硬件处理器42经由总线连接于存储器41(ROM43以及RAM44)。一个或者多个硬件处理器42例如既可以包括一个或者多个CPU(Central Processing Unit，中央处理单元)，也可以包括一个或者多个GPU(Graphics Processing Unit，图形处理单元)。另外，一个或者多个硬件处理器42也可以包括具备用于实现神经网络的专用的处理电路的半导体装置等。

一个或者多个硬件处理器42将RAM44作为工作区域来通过与预先存储于ROM43或者存储装置23的各种程序的协作来执行各种处理，由此作为位置推定装置50发挥功能。关于位置推定装置50，将在以下进行详细的说明。

图3是表示位置推定装置50的构成的图。位置推定装置50推定移动体装置10的自身位置。更具体而言，移动体装置10推定自身位置的平动分量以及自身位置的旋转分量。

自身位置的平动分量表示移动体装置10在三维空间内的坐标(X方向的位置、Y方向的位置以及Z方向的位置)。例如，自身位置的平动分量表示距任意的基准点的距离(X方向的距离、Y方向的距离以及Z方向的距离)。任意的基准点例如可以是开始时刻等的预定时刻的移动体装置10的自身位置的平动分量。

自身位置的旋转分量表示移动体装置10的姿势(绕X轴的旋转角度、绕Y轴的旋转角度以及绕Z轴的旋转角度)。此外，旋转分量可以是从任意的基准旋转角起算的角度差。任意的基准旋转角例如可以是开始时刻等的预定时刻的移动体装置10的自身位置的旋转分量。另外，例如旋转分量也可以由旋转矩阵、表示旋转轴和旋转方向的矢量或者四元数等来表现。

位置推定装置50按预定的时间间隔来推定自身位置。将成为推定自身位置的对象的时刻称为对象时刻。另外，将对象时刻之前、已经推定完自身位置的时刻称为过去时刻。

位置推定装置50具备自身位置存储部52、图像取得部54、图像存储部56、观测值取得部58、状态量推定部60、可靠度算出部62以及自身位置推定部64。

自身位置存储部52存储一个或者多个过去时刻各自的自身位置。图像取得部54取得在对象时刻通过拍摄装置21拍摄到的拍摄图像。图像存储部56存储在一个或者多个过去时刻通过拍摄装置21拍摄到的拍摄图像。

观测值取得部58取得通过一个或者多个传感器22各自观测到的观测值。观测值取得部58例如以时间序列取得观测值。在本实施方式中，观测值取得部58取得通过角速度传感器31观测到的角速度以及通过加速度传感器32观测到的加速度来作为观测值。

状态量推定部60推定表示一个或者多个传感器22各自的状态的状态量。在本实施方式中，状态量推定部60推定表示角速度传感器31的偏置的角速度偏置来作为表示角速度传感器31的状态的状态量。更具体而言，状态量推定部60推定角速度偏置的X方向分量、Y方向分量以及Z方向分量。

进一步，在本实施方式中，状态量推定部60推定移动体装置10的平动方向的速度、重力加速度、以及表示加速度传感器32的偏置的加速度偏置来作为表示加速度传感器32的状态的状态量。更具体而言，状态量推定部60推定速度、重力加速度以及加速度偏置各自的X方向分量、Y方向分量以及Z方向分量。

进一步，状态量推定部60例如也可以推定拍摄装置21与角速度传感器31之间的位置偏离量以及拍摄装置21与角速度传感器31之间的时间戳之差来作为角速度传感器31的状态量。另外，状态量推定部60例如也可以推定拍摄装置21与加速度传感器32之间的位置偏离量以及拍摄装置21与加速度传感器32之间的时间戳之差来作为加速度传感器32的状态量。另外，状态量推定部60也可以推定与拍摄装置21之间的位置偏离量、与特定的传感器22的位置偏离量、与拍摄装置21的时间戳之差、与特定的传感器22的时间戳之差来作为一个或者多个传感器22各自的状态量。

位置推定装置50通过推定与拍摄装置21之间的位置偏离量或者与特定的传感器22的位置偏离量来作为传感器22的状态量，能够抑制因传感器22的位置变化而产生的自身位置的推定精度的恶化。另外，位置推定装置50通过推定与拍摄装置21之间的时间戳之差或者与特定的传感器22的时间戳之差来作为传感器22的状态量，能够抑制因传感器22的时间戳的变动而产生的自身位置的推定精度的恶化。

另外，位置推定装置50也可以推定在过去时刻所推定的自身位置的平动分量与实际世界上的大小的比率来作为状态量。由此，位置推定装置50能够抑制因自身位置的平动分量的大小与实际世界上的大小背离而产生的自身位置的推定精度的恶化。

例如，关于一个或者多个传感器22的各个传感器，状态量推定部60推定使对预定的关系式代入了一个或者多个过去时刻各自的自身位置的情况下的预定的关系式的残差为最小的状态量。预定的关系式是基于过去时刻的自身位置、所对应的传感器22的观测值及所对应的传感器22的状态量以及过去时刻与对象时刻的时间差来算出对象时刻的自身位置的方程式。例如，预定的关系式是用于基于过去时刻的自身位置来算出对象时刻的自身位置的运动方程式。

例如，状态量推定部60通过非线性最小二乘法推定使预定的关系式的残差为最小的状态量。另外，状态量推定部60在状态量在时间上变化的情况下，例如使用Levenberg－Marquardt法、Gauss－Newton法或者共轭梯度法等，对预定的关系式反复代入多个过去时刻各自的自身位置，推定使残差最小化的状态量。另外，在对预定的关系式反复代入多个过去时刻各自的自身位置的情况下，状态量推定部60也可以按每个过去时刻改变权重来调齐残差的大小。此外，在后面参照图4来进一步对角速度传感器31以及加速度传感器32的状态量的推定方法的一个例子进行描述。

可靠度算出部62算出表示一个或者多个传感器22各自的状态量的可靠程度的可靠度。可靠度在值大的情况表示所推定的状态量的可靠程度以及精度高，在值小的情况下表示所推定的状态量的可靠程度以及精度低。

例如，可靠度算出部62算出状态量的误差的期待值的倒数来作为状态量的可靠度。对于状态量的误差的期待值，例如在状态量推定部60推定状态量的情况下，在用于使残差为最小的运算中获得。

例如，在通过非线性最小二乘法使残差为最小的情况下，状态量推定部60推定状态量的协方差矩阵。例如，状态量推定部60推定状态量中的X方向分量、Y方向分量以及Z方向分量的协方差矩阵。在该情况下，可靠度算出部62从状态量推定部60取得状态量的协方差矩阵。并且，可靠度算出部62算出所取得的协方差矩阵的对角分量之和的倒数来作为可靠度。在协方差矩阵的对角分量之和小的情况下，期待为状态量的误差小且精度高，在协方差矩阵的对角分量之和大的情况下，期待为状态量的误差大且精度低。

另外，移动体装置10有时具备观测与状态量相当的观测值的第2传感器。在该情况下，可靠度算出部62也可以对第2传感器的观测值与所推定的状态量进行比较来算出可靠度。例如，在推定速度来作为状态量、且移动体装置10具备基于车轮的转速来观测速度的速度计的情况下，可靠度算出部62在速度计的观测值与所推定的速度接近的情况下提高可靠度，在速度计的观测值与所推定的速度远离的情况下降低可靠度。

另外，在可预测到状态量可取的范围的情况下，可靠度算出部62也可以算出所推定的状态量与预先设定的值或者范围的一致度来作为可靠度。例如，对于角速度传感器31的角速度偏置以及加速度传感器32的加速度偏置可取的范围，可以通过预先对特性等进行评价来进行预测。因此，可靠度算出部62也可以算出所预测到的范围与所推定的角速度偏置以及所推定的加速度偏置的一致度来作为可靠度。另外，对于重力加速度的方向，可以根据加速度传感器32相对于地面的安装姿势来进行预测。因此，可靠度算出部62也可以算出所预测到的重力加速度的方向与所推定的重力加速度的方向的一致度来作为可靠度。

自身位置推定部64基于一个或者多个传感器22各自的状态量的可靠度，选择一个或者多个传感器22中的一个或者多个对象传感器。例如，自身位置推定部64选择一个或者多个传感器22中的其状态量的可靠度为预先确定的阈值以上的一个或者多个传感器22来作为对象传感器。并且，自身位置推定部64基于所选择的一个或者多个对象传感器各自的观测值以及状态量，推定自身位置。

例如，自身位置推定部64基于在对象时刻通过拍摄装置21拍摄到的拍摄图像、过去时刻的拍摄图像、过去时刻的自身位置、所选择的一个或者多个对象传感器各自的观测值以及状态量，推定对象时刻的自身位置。另外，例如在一个或者多个传感器22中不存在状态量的可靠度为阈值以上的一个或者多个对象传感器的情况下等，自身位置推定部64也可以不使用任何传感器22的观测值以及状态量而推定自身位置。即，在该情况下，自身位置推定部64基于对象时刻的拍摄图像、过去时刻的拍摄图像、过去时刻的自身位置，推定对象时刻的自身位置。此外，关于自身位置推定部64的推定处理，将进一步在图5以后进行详细的说明。

并且，自身位置推定部64输出所推定的对象时刻的自身位置。例如，自身位置推定部64向移动体控制装置27输出所推定的对象时刻的自身位置。移动体控制装置27取得按预定的时间间隔而推定出的自身位置。移动体控制装置27基于所推定的自身位置，控制移动体装置10的移动。

图4是表示位置推定装置50的处理的流程的流程图。位置推定装置50按预定的时间间隔而执行图4所示的处理，推定对象时刻的自身位置。

首先，在S11中，图像取得部54取得对象时刻的拍摄图像。接着，在S12中，观测值取得部58取得通过一个或者多个传感器22各自观测到的观测值。例如，观测值取得部58取得与对象时刻的拍摄图像的拍摄定时同步地测定的观测值。

接着，在S13中，状态量推定部60推定表示一个或者多个传感器22各自的状态的状态量。在本实施方式中，状态量推定部60推定表示角速度传感器31的偏置的角速度偏置来作为表示角速度传感器31的状态的状态量。

例如，状态量推定部60推定使对旋转分量运动方程式代入了过去时刻的自身位置的旋转分量的情况下的残差(第1残差)为最小的角速度偏置。旋转分量运动方程式是用于基于过去时刻的自身位置的旋转分量来算出对象时刻的自身位置的旋转分量的关系式。

具体而言，旋转分量运动方程式由下述的式(1)表示。

R(t+Δt)＝R(t)×Exp((ω(t)－b

其中，Exp是将角速度的值变换为旋转分量的映射。另外，式(1)中的×表示对多个旋转分量进行合成。

R(t+Δt)表示对象时刻的自身位置的旋转分量。R(t)表示过去时刻的自身位置的旋转分量。ω(t)表示所测定的角速度。b

另外，状态量推定部60也可以将多个过去时刻的自身位置的旋转分量分别代入到式(1)的旋转分量运动方程式，通过非线性最小二乘法来推定角速度偏置。在该情况下，状态量推定部60例如使用Levenberg－Marquardt法、Gauss－Newton法或者共轭梯度法等，对旋转分量运动方程式反复代入多个过去时刻各自的自身位置，推定使第1残差最小化的角速度偏置。由此，状态量推定部60能够推定在时间上变化的角速度偏置。

另外，在本实施方式中，状态量推定部60推定移动体装置10的平动方向的速度、重力加速度以及表示加速度传感器32的偏置的加速度偏置来作为表示加速度传感器32的状态的状态量。

例如，状态量推定部60推定使对平动分量运动方程式代入了过去时刻的自身位置的平动分量的情况下的残差(第2残差)为最小的平动方向的速度、重力加速度以及加速度偏置。平动分量运动方程式是用于基于过去时刻的自身位置的平动分量来算出对象时刻的自身位置的平动分量的关系式。

具体而言，平动分量运动方程式由下述的式(2)以及式(3)表示。

p(t+Δt)＝p(t)+v(t)×Δt+(1/2)×g×Δt

v(t+Δt)＝v(t)+g×Δt+R(t)×(a(t)－b

p(t+Δt)表示对象时刻的自身位置的平动分量。p(t)表示过去时刻的自身位置的平动分量。v(t)表示推定对象的速度。Δt表示过去时刻与对象时刻的时间差。g表示推定对象的重力加速度。b

另外，状态量推定部60也可以将多个过去时刻的自身位置的平动分量分别代入到式(2)以及式(3)的平动分量运动方程式，通过非线性最小二乘法推定速度、重力加速度以及加速度偏置。在该情况下，状态量推定部60例如使用Levenberg－Marquardt法、Gauss－Newton法或者共轭梯度法等，对平动分量运动方程式反复代入多个过去时刻各自的自身位置，推定使第2残差最小化的速度、重力加速度以及加速度偏置。由此，状态量推定部60能够推定在时间上变化的速度、重力加速度以及加速度偏置。

接着，在S14中，可靠度算出部62算出一个或者多个传感器22各自的状态量的可靠度。

可靠度算出部62取得所推定出的角速度偏置的、相互正交的X方向分量、Y方向分量以及Z方向分量的协方差矩阵。例如，可靠度算出部62取得在S13中的使用了非线性最小二乘法的角速度偏置的算出过程中状态量推定部60所算出的角速度偏置的协方差矩阵。并且，可靠度算出部62算出所取得的角速度偏置的协方差矩阵的对角分量之和的倒数来作为角速度偏置的可靠度。

另外，可靠度算出部62取得所推定出的加速度偏置的、相互正交的X方向分量、Y方向分量以及Z方向分量的协方差矩阵。例如，可靠度算出部62取得在S13中的使用了非线性最小二乘法的加速度偏置的算出过程中状态量推定部60所算出的加速度偏置的协方差矩阵。并且，可靠度算出部62算出所取得的加速度偏置的协方差矩阵的对角分量之和的倒数来作为加速度偏置的可靠度。

接着，在S15中，自身位置推定部64推定对象时刻的自身位置的平动分量以及旋转分量。此外，关于对象时刻的自身位置的推定方法，将参照图5以及图6来在后面进行描述。

接着，在S16中，自身位置推定部64输出所推定的对象时刻的自身位置。例如，自身位置推定部64向移动体控制装置27输出所推定的对象时刻的自身位置。进一步，自身位置推定部64使所推定的对象时刻的自身位置与时刻信息相关联而存储于自身位置存储部52。

位置推定装置50按预定的时间间隔而反复执行以上的S11～S16的处理。由此，位置推定装置50能够按预定的时间间隔而推定自身位置。

图5是表示自身位置推定部64的流程的流程图。在本实施方式中，自身位置推定部64以图5所示的流程执行处理。

首先，在S21中，自身位置推定部64判断角速度的偏置的可靠度是否为预先设定的阈值以上。自身位置推定部64在角速度偏置的可靠度不为阈值以上的情况下(S21：否)，使处理进入S23。自身位置推定部64在角速度偏置的可靠度为阈值以上的情况下(S21：是)，使处理进入S22。

接着，在S22中，自身位置推定部64判断加速度的偏置的可靠度是否为阈值以上。自身位置推定部64在加速度偏置的可靠度不为阈值以上的情况下(S22：否)，使处理进入S24。自身位置推定部64在加速度偏置可靠度为阈值以上的情况下(S22：是)，使处理进入S25。

在S23中，自身位置推定部64通过第1方式推定对象时刻的自身位置。即，自身位置推定部64在角速度偏置的可靠度不为阈值以上的情况下，选择第1方式。

在第1方式中，自身位置推定部64基于对象时刻的拍摄图像、过去时刻的拍摄图像以及过去时刻的自身位置，推定对象时刻的自身位置。由此，在角速度传感器31的状态量的可靠度不为阈值以上的情况下，自身位置推定部64能够不使用角速度传感器31以及加速度传感器32的观测值以及状态值而推定对象时刻的自身位置。

此外，关于第1方式的更详细的处理内容，将参照图6来在后面进行描述。当结束S23的处理时，自身位置推定部64结束本流程的处理。

在S24中，自身位置推定部64通过第2方式推定对象时刻的自身位置。即，自身位置推定部64在角速度偏置的可靠度为阈值以上、但加速度偏置的可靠度不为阈值以上的情况下，选择第2方式。

在第2方式中，自身位置推定部64基于对象时刻的拍摄图像、所测定的角速度、所推定的角速度偏置、过去时刻的拍摄图像、过去时刻的自身位置，推定对象时刻的自身位置。由此，自身位置推定部64能够选择可靠度为阈值以上的状态量的角速度传感器31来作为对象传感器，基于所选择的角速度传感器31的观测值以及状态量，推定自身位置。

此外，关于第2方式的更详细的处理内容，将参照图7来在后面进行描述。当结束S24的处理时，自身位置推定部64结束本流程的处理。

在S25中，自身位置推定部64通过第3方式推定对象时刻的自身位置。即，自身位置推定部64在角速度偏置的可靠度以及加速度偏置的可靠度均为阈值以上的情况下，选择第3方式。

在第3方式中，自身位置推定部64基于对象时刻的拍摄图像、所测定的角速度、所推定的角速度偏置、所测定的加速度、所推定的速度、所推定的重力加速度、所推定的加速度偏置、过去时刻的拍摄图像、过去时刻的自身位置，推定对象时刻的自身位置。由此，自身位置推定部64能够选择状态量的可靠度为阈值以上的角速度传感器31以及加速度传感器32来作为对象传感器，基于所选择的角速度传感器31以及加速度传感器32的观测值以及状态量，推定自身位置。

此外，关于第3方式的更详细的处理内容，将参照图8来在后面进行描述。当结束S25的处理时，自身位置推定部64结束本流程的处理。

图6是表示S23的基于第1方式的推定处理的流程的流程图。自身位置推定部64在S23中执行图6所示的处理。

首先，在S31中，自身位置推定部64基于过去时刻的自身位置的平动分量，推定对象时刻的自身位置的平动分量。例如，自身位置推定部64将多个过去时刻的自身位置的平动分量应用于等速直线运动模型，通过外插处理推定对象时刻的自身位置的平动分量。此外，自身位置推定部64也可以将多个过去时刻的自身位置的平动分量应用于其他模型而代替等速直线运动模型来算出对象时刻的自身位置的平动分量。另外，自身位置推定部64也可以对多个过去时刻的自身位置的平动分量拟合样条(spline)曲线等的曲线来算出对象时刻的自身位置的平动分量。

接着，在S32中，自身位置推定部64基于过去时刻的自身位置的旋转分量，推定对象时刻的自身位置的旋转分量。例如，自身位置推定部64将多个过去时刻的自身位置的旋转分量应用于等角速度运动模型，通过外插处理推定对象时刻的自身位置的旋转分量。此外，自身位置推定部64也可以将多个过去时刻的自身位置的旋转分量应用于其他模型而代替等角速度运动模型来算出对象时刻的自身位置的旋转分量。另外，自身位置推定部64也可以对多个过去时刻的自身位置的旋转分量拟合样条曲线等的曲线来算出对象时刻的自身位置的旋转分量。

接着，在S33中，自身位置推定部64使对象时刻的拍摄图像所包含的一个或者多个特征点各自与过去时刻的拍摄图像所包含的对应特征点相关联。

例如，自身位置推定部64通过Structure－from－Motion(运动重建)或者SLAM(Simultaneous Localization and Mapping，同时定位与地图构建)等算出过去时刻的拍摄图像所包含的对应特征点的三维位置。自身位置推定部64将所算出的三维位置从在S31以及S32中推定出的对象时刻的自身位置投影到对象时刻的拍摄图像，确定对象时刻的拍摄图像上的候选位置。自身位置推定部64搜索对象时刻的拍摄图像的候选位置的周围的相似纹理(texture)，使过去时刻的拍摄图像所包含的对应特征点与对象时刻的拍摄图像的特征点相关联。

另外，自身位置推定部64基于对象时刻的拍摄图像的特征点的位置和在S31以及S32中推定的自身位置，算出过去时刻的拍摄图像的特征点所存在的候选位置来作为极线(epipolar line)。并且，自身位置推定部64也可以在过去时刻的拍摄图像的极线的附近搜索相似纹理，使过去时刻的拍摄图像所包含的对应特征点与对象时刻的拍摄图像的特征点相关联。

另外，自身位置推定部64例如基于像素的周边区域的亮度值之差来算出纹理的相似度。自身位置推定部64也可以例如基于SIFT、SURF、ORB或者AKAZE等的局部特征描述符(descriptor)来算出纹理的相似度。另外，自身位置推定部64也可以预先制作用于判定两个局部区域是否相互对应的神经网络，使用该神经网络来算出相似度。

另外，自身位置推定部64也可以关于多个过去时刻的拍摄图像的各个拍摄图像，使对象时刻的拍摄图像所包含的一个或者多个特征点各自与其过去时刻的拍摄图像所包含的对应特征点相关联。

接着，在S34中，自身位置推定部64调整通过S31以及S32推定的对象时刻的自身位置，以使再投影误差最小化。再投影误差是对象时刻的拍摄图像所包含的一个或者多个特征点各自的像素位置与基于所推定的对象时刻的自身位置来将过去时刻的拍摄图像所包含的对应特征点的三维位置再投影到对象时刻的拍摄图像的像素位置之间的误差的评价值。例如，自身位置推定部64使用非线性最小二乘法等来调整对象时刻的自身位置以使再投影误差最小化。

并且，自身位置推定部64输出通过S34调整后的对象时刻的自身位置。当结束S34时，自身位置推定部64结束本流程。

图7是表示S24的基于第2方式的推定处理的流程的流程图。自身位置推定部64在S24中执行图7所示的处理。

首先，在S41中，自身位置推定部64基于过去时刻的自身位置的平动分量，推定对象时刻的自身位置的平动分量。例如，在S41中，自身位置推定部64执行与图6的S31同样的处理。

接着，在S42中，自身位置推定部64基于过去时刻的自身位置的旋转分量、所测定的角速度以及所推定的角速度偏置，推定对象时刻的自身位置的旋转分量。更具体而言，自身位置推定部64对式(1)所示的旋转分量运动方程式代入过去时刻的自身位置的旋转分量、所测定的角速度以及所推定的角速度偏置，算出对象时刻的自身位置的旋转分量。此外，在S42中，代替上述的处理，自身位置推定部64也可以执行与图6的S32同样的处理。

接着，在S43中，自身位置推定部64使对象时刻的拍摄图像所包含的一个或者多个特征点各自与过去时刻的拍摄图像所包含的对应特征点相关联。例如，在S43中，自身位置推定部64执行与图6的S33同样的处理。

其中，在S43中，自身位置推定部64也可以基于式(1)所示的旋转分量运动方程式，预测特征点的旋转，将过去时刻的拍摄图像中的特征点的周围的区域的图像在旋转方向上变形，然后算出纹理的相似度。由此，自身位置推定部64能够精度良好地执行特征点的关联。

接着，在S44中，自身位置推定部64对通过S41以及S42推定的对象时刻的自身位置进行调整以使再投影误差与旋转分量运动方程式中的第1残差的合计最小化。旋转分量运动方程式中的第1残差是对旋转分量运动方程式代入了过去时刻的自身位置的旋转分量、所测定的角速度以及所推定的角速度偏置的情况下的残差的评价值。

例如，自身位置推定部64使用非线性最小二乘法等来调整对象时刻的自身位置，以使再投影误差和第1残差的合计最小化。此外，再投影误差和旋转分量运动方程式中的第1残差的单位不同。因此，自身位置推定部64也可以在对再投影误差以及第1残差各自进行了标准化之后算出合计。

另外，自身位置推定部64将状态量(在此为角速度偏置)固定，调整对象时刻的自身位置。代替于此，自身位置推定部64也可以进行调整以使状态量和对象时刻的自身位置这两者最小化。

并且，自身位置推定部64输出通过S44调整后的对象时刻的自身位置。当结束S44时，自身位置推定部64结束本流程。

图8是表示S25的基于第3方式的推定处理的流程的流程图。自身位置推定部64在S25中执行图8所示的处理。

首先，在S51中，自身位置推定部64基于过去时刻的自身位置的平动分量、所测定的加速度、所推定的速度、所推定的重力加速度以及所推定的加速度偏置，推定对象时刻的自身位置的平动分量。更具体而言，自身位置推定部64对式(2)以及式(3)所示的平动分量运动方程式代入过去时刻的自身位置的平动分量、所推定的速度、所推定的重力加速度以及所推定的加速度偏置，算出对象时刻的自身位置的平动分量。此外，在S51中，代替上述的处理，自身位置推定部64也可以执行与图6的S31同样的处理。

接着，在S52中，自身位置推定部64基于过去时刻的自身位置的旋转分量、所测定的角速度以及所推定的角速度偏置，推定对象时刻的自身位置的旋转分量。更具体而言，自身位置推定部64对式(1)所示的旋转分量运动方程式代入过去时刻的自身位置的旋转分量、所测定的角速度以及所推定的角速度偏置，算出对象时刻的自身位置的旋转分量。此外，在S52中，代替上述的处理，自身位置推定部64也可以执行与图6的S32同样的处理。

接着，在S53中，自身位置推定部64使对象时刻的拍摄图像所包含的一个或者多个特征点各自与过去时刻的拍摄图像所包含的对应特征点相关联。例如，在S53中，自身位置推定部64执行与图6的S33同样的处理。

其中，在S53中，自身位置推定部64也可以基于所推定的重力加速度的方向，使过去时刻的拍摄图像的特征点的周围的区域的图像变形，以使得对象时刻的拍摄图像和过去时刻的拍摄图像的重力加速度的方向一致，然后，算出纹理的相似度。由此，自身位置推定部64能够精度良好地执行特征点的关联。

代替于此，在S53中，自身位置推定部64也可以基于式(1)所示的旋转分量运动方程式，预测特征点的旋转，使过去时刻的拍摄图像中的特征点的周围的区域的图像在旋转方向上变形，然后算出纹理的相似度。这样一来，自身位置推定部64也能够精度良好地执行特征点的关联。

另外，在S53中，自身位置推定部64也可以组合重力加速度的方向的一致以及特征点的旋转的预测，使过去时刻的拍摄图像中的特征点的周围的区域的图像在旋转方向上变形，然后算出纹理的相似度。

接着，在S54中，自身位置推定部64对通过S51以及S52推定的对象时刻的自身位置进行调整，以使再投影误差、旋转分量运动方程式中的第1残差以及平动分量运动方程式中的第2残差的合计最小化。平动分量运动方程式中的第2残差是对平动分量运动方程式代入了过去时刻的自身位置的平动分量、所测定的加速度、所推定的速度、所推定的重力加速度以及所推定的加速度偏置的情况下的残差的评价值。

例如，自身位置推定部64使用非线性最小二乘法等，调整对象时刻的自身位置，以使再投影误差、第1残差以及第2残差的合计最小化。此外，再投影误差与第1残差以及第2残差的单位不同。因此，自身位置推定部64也可以在对再投影误差、第1残差以及第2残差各自进行了标准化之后算出合计。

另外，自身位置推定部64将状态量(在此为角速度偏置、速度、重力加速度以及角速度偏置)固定，调整对象时刻的自身位置。代替于此，自身位置推定部64也可以进行调整以使某一个或者多个状态量与对象时刻的自身位置这两者最小化。

并且，自身位置推定部64输出通过S54调整后的对象时刻的自身位置。当结束S54时，自身位置推定部64结束本流程。

以上，说明了第1方式、第2方式以及第3方式，但任何方式都包括自身位置的平动分量的推定处理(S31、S41、S51)、自身位置的旋转分量的推定处理(S32、S42、S52)、特征点的关联处理(S33、S43、S53)以及自身位置的调整处理(S34、S44、S54)这四个处理。

自身位置推定部64例如也可以在第1方式、第2方式以及第3方式共用四个处理中的某一处理，根据状态量的可靠度来切换其他处理。例如，自身位置推定部64也可以在S33的处理中共用特征点的关联处理，根据状态量的可靠度来切换自身位置的平动分量的推定处理、自身位置的旋转分量的推定处理以及自身位置的调整处理。

图9是表示变形例涉及的位置推定装置50的构成的图。位置推定装置50也可以是在图3所示的构成的基础上还具备运动类别确定部74、异常判定部76以及复位部78的构成。

运动类别确定部74基于所推定的自身位置的轨迹，确定移动体装置10的运动类别。例如，运动类别确定部74确定移动体装置10是在进行等速直线运动、还是在进行等加速度直线运动、或者是在进行圆运动、亦或是处于静止。另外，例如运动类别确定部74也可以在进行预先设定的运动类别以外的运动的情况下确定为在进行其他运动。

例如，运动类别确定部74通过对所推定的自身位置的轨迹和预先记述的多个模型各自的残差进行评价来确定运动类别。另外，运动类别确定部74在自身位置的轨迹与多个模型的任何模型相比、残差都要大阈值以上的情况下确定为在进行其他运动。另外，运动类别确定部74也可以在存在多个与自身位置的轨迹的残差小于阈值的模型的情况下确定多个运动类别。另外，运动类别确定部74也可以利用神经网络等的识别器来确定运动类别。

异常判定部76取得通过运动类别确定部74确定的运动类别。另外，异常判定部76取得为了自身位置推定部64推定自身位置推定而选择的一个或者多个对象传感器。即，异常判定部76取得所推定的状态量的可靠度高的传感器22的组合。在本实施方式中，异常判定部76取得选择了第1方式、第2方式以及第3方式中的哪个。

接着，异常判定部76判断所选择的一个或者多个对象传感器的组合是否与所确定的运动类别对应，在不对应的情况下判定为异常，在对应的情况下判定为正常。在本实施方式中，异常判定部76判断所选择的方式是否与所确定的运动类别对应，在不对应的情况下判定为异常，在对应的情况下判定为正常。

例如，异常判定部76按运动类别预先存储被期待的选择方式的模式。并且，异常判定部76在与预先存储的模式一致的情况下，判断为所选择的方式与所确定的运动类别对应。

例如，考虑运动类别为等速直线运动的状况。在该状况下，加速度为0，因此，状态量推定部60难以推定速度。因此，期待被选择的方式不是利用加速度、角速度以及拍摄图像的第3方式，而是第1方式和第2方式中的某个。在该状况下，在选择了第3方式的情况下，认为在基于传感器22的观测值的取得以及用于推定自身位置的运算等中的某一方发生了异常。

因此，异常判定部76在运动类别为等速直线运动的情况下，当选择了第3方式时判定为异常，在运动类别为等速直线运动的情况下，当选择了第1方式或者第2方式时判定为正常。另外，异常判定部76也可以在确定了其他运动类别的情况下，无论选择哪种方式，都判定为正常。另外，异常判定部76在确定了多个运动类别的情况下，关于所确定的多个运动类别的各个运动类别，进行所选择的方式是否与所确定的运动类别对应的判断，综合所确定的多个运动类别各自的判断结果，判定是否发生异常。

复位部78在通过异常判定部76判定为异常的情况下，针对自身位置推定部64，对过去时刻的自身位置进行复位，使之重新推定对象时刻的自身位置。例如，复位部78在通过异常判定部76判定为异常的情况下，针对自身位置推定部64，以当前的自身位置为基准点，使之重新推定对象时刻的自身位置。由此，自身位置推定部64能够使用判定为正常的过去时刻的自身位置，推定的新的自身位置。

这样的变形例涉及的位置推定装置50能够对所选择的自身位置的推定方式和所推定的自身位置的关系性进行评价。并且，位置推定装置50能够基于推定方式和所推定的自身位置的关系性的评价结果，自我诊断位置推定装置50的推定动作是否为异常。

并且，变形例涉及的位置推定装置50能够基于自我诊断的结果，推定新的自身位置。由此，根据变形例涉及的位置推定装置50，能够精度更好地推定自身位置。

如上所述，本实施方式涉及的位置推定装置50推定表示一个或者多个传感器22各自的状态的状态量，算出一个或者多个传感器22各自的状态量的可靠度。并且，位置推定装置50基于一个或者多个传感器22各自的状态量的可靠度，选择一个或者多个传感器22中的一个或者多个对象传感器，基于所选择的一个或者多个对象传感器各自的观测值以及状态量，推定自身位置。

由此，位置推定装置50能够避免因利用了可靠度低的状态量而导致的自身位置的推定精度的恶化。因此，根据位置推定装置50，能够使用状态量的可靠度高的传感器22的观测值以及状态量，精度良好地推定移动体装置10的自身位置。

以上对本发明的实施方式进行了说明，但上述的实施方式是作为例子提示的，并不是意在限定发明的范围。这些新的实施方式能够以其他各种各样的方式来实施，能够在不脱离发明的宗旨的范围内进行各种省略、置换、变更。这些实施方式及其变形包含在发明的范围、宗旨内，并且，包含在权利要求书记载的发明及其等同的范围内。

此外，可以将上述的实施方式总结为以下的技术方案。

技术方案1

一种位置推定装置，对设置了一个或者多个传感器的移动体装置的自身位置进行推定，所述一个或者多个传感器观测与移动有关的信息，所述位置推定装置具备：

状态量推定部，其基于所述一个或者多个传感器各自的观测值，推定表示所述一个或者多个传感器各自的状态的状态量；

可靠度算出部，其算出表示所述一个或者多个传感器各自的所述状态量的可靠程度的可靠度；以及

自身位置推定部，其基于所述一个或者多个传感器各自的所述状态量的所述可靠度，选择所述一个或者多个传感器中的一个或者多个对象传感器，基于所选择的所述一个或者多个对象传感器各自的所述观测值以及所述状态量，推定所述自身位置。

技术方案2

根据技术方案1所述的位置推定装置，

所述移动体装置还设置有对所述移动体装置的周围进行拍摄的拍摄装置，

所述自身位置推定部，

在所述一个或者多个传感器中存在所述可靠度为预先确定的阈值以上的所述一个或者多个对象传感器的情况下，选择所述一个或者多个传感器中的所述状态量的所述可靠度为所述阈值以上的一个或者多个传感器来作为所述一个或者多个对象传感器，

基于在对象时刻通过所述拍摄装置拍摄到的拍摄图像、所述对象时刻之前的过去时刻的所述拍摄图像、所述过去时刻的所述自身位置、和所选择的所述一个或者多个对象传感器各自的所述观测值以及所述状态量，推定所述对象时刻的所述自身位置。

技术方案3

根据技术方案2所述的位置推定装置，

所述自身位置推定部，在所述一个或者多个传感器中不存在所述可靠度为所述阈值以上的所述一个或者多个对象传感器的情况下，基于所述对象时刻的所述拍摄图像、所述过去时刻的所述拍摄图像以及所述过去时刻的所述自身位置，推定所述对象时刻的所述自身位置。

技术方案4

根据技术方案2或者3所述的位置推定装置，

关于所述一个或者多个传感器的各个传感器，所述状态量推定部推定使对预定的关系式代入了一个或者多个所述过去时刻各自的所述自身位置的情况下的残差为最小的所述状态量，

所述关系式是基于所述过去时刻的所述自身位置、对应传感器的所述观测值及所述对应传感器的所述状态量、以及所述过去时刻与所述对象时刻的时间差来算出所述对象时刻的所述自身位置的方程式。

技术方案5

根据技术方案4所述的位置推定装置，

所述状态量推定部通过非线性最小二乘法来推定所述状态量，

所述可靠度算出部，

取得所推定的所述状态量的不同的多个分量的协方差矩阵，

算出所取得的所述协方差矩阵的对角分量之和的倒数来作为所述可靠度。

技术方案6

根据技术方案4所述的位置推定装置，

所述移动体装置还具备对与所述状态量相当的观测值进行观测的第2传感器，

所述可靠度算出部对所述第2传感器的观测值与所推定的所述状态量进行比较，算出所述可靠度。

技术方案7

根据技术方案3所述的位置推定装置，

所述自身位置包括平动分量以及旋转分量，

所述一个或者多个传感器包括角速度传感器和加速度传感器，

所述角速度传感器测定所述对象时刻的所述移动体装置的角速度，

所述加速度传感器测定所述对象时刻的所述移动体装置的加速度。

技术方案8

根据技术方案7所述的位置推定装置，

所述状态量推定部，

推定所述移动体装置的角速度偏置来作为所述角速度传感器的所述状态量，

推定所述移动体装置的速度、重力加速度以及所述移动体装置的加速度偏置来作为所述加速度传感器的所述状态量。

技术方案9

根据技术方案8所述的位置推定装置，

所述自身位置推定部基于第1方式、第2方式或者第3方式来推定所述对象时刻的所述自身位置，

所述第1方式为：基于所述对象时刻的所述拍摄图像、所述过去时刻的所述拍摄图像以及所述过去时刻的所述自身位置，推定所述对象时刻的所述自身位置，

所述第2方式为：基于所述对象时刻的所述拍摄图像、所测定的所述角速度、所推定的所述角速度偏置、所述过去时刻的所述拍摄图像以及所述过去时刻的所述自身位置，推定所述对象时刻的所述自身位置，

所述第3方式为：基于所述对象时刻的所述拍摄图像、所测定的所述角速度、所推定的所述角速度偏置、所测定的所述加速度、所推定的所述速度、所推定的所述重力加速度、所推定的所述加速度偏置、所述过去时刻的所述拍摄图像以及所述过去时刻的所述自身位置，推定所述对象时刻的所述自身位置，

所述自身位置推定部，

在所述角速度偏置的所述可靠度不为所述阈值以上的情况下，选择所述第1方式，

在所述角速度偏置的所述可靠度为所述阈值以上、且所述加速度偏置的所述可靠度不为所述阈值以上的情况下，选择所述第2方式，

在所述角速度偏置的所述可靠度以及所述加速度偏置的所述可靠度均为所述阈值以上的情况下，选择所述第3方式。

技术方案10

根据技术方案9所述的位置推定装置，

在所述第1方式中，所述自身位置推定部，

基于所述过去时刻的所述自身位置，推定所述对象时刻的所述自身位置，

基于所推定的所述对象时刻的所述自身位置，使所述对象时刻的所述拍摄图像所包含的一个或者多个特征点各自与所述过去时刻的所述拍摄图像所包含的对应特征点相关联，

调整所推定的所述对象时刻的所述自身位置，以使再投影误差最小化，

所述再投影误差是所述对象时刻的所述拍摄图像所包含的所述一个或者多个特征点各自的像素位置与基于所推定的所述对象时刻的所述自身位置将所述过去时刻的所述拍摄图像所包含的所述对应特征点再投影到所述对象时刻的所述拍摄图像而得到的像素位置之间的误差的评价值。

技术方案11

根据技术方案10所述的位置推定装置，

在所述第2方式中，所述自身位置推定部，

基于所述过去时刻的所述自身位置的平动分量，推定所述对象时刻的所述自身位置的平动分量，

基于所述过去时刻的所述自身位置的旋转分量、所测定的所述角速度以及所推定的所述角速度偏置，推定所述对象时刻的所述自身位置的旋转分量，

基于所推定的所述对象时刻的所述自身位置，使所述对象时刻的所述拍摄图像所包含的一个或者多个特征点各自与所述过去时刻的所述拍摄图像所包含的对应特征点相关联，

调整所推定的所述对象时刻的所述自身位置，以使所述再投影误差与旋转分量运动方程式中的第1残差的合计最小化，

所述旋转分量运动方程式中的所述第1残差是对所述旋转分量运动方程式代入了所述过去时刻的所述自身位置的旋转分量、所测定的所述角速度以及所推定的所述角速度偏置的情况下的残差的评价值。

技术方案12

根据技术方案11所述的位置推定装置，

在所述第3方式中，所述自身位置推定部，

基于所述过去时刻的所述自身位置的平动分量、所测定的所述加速度、所推定的所述速度、所推定的所述重力加速度以及所推定的所述加速度偏置，推定所述对象时刻的所述自身位置的平动分量，

基于所述过去时刻的所述自身位置的旋转分量、所测定的所述角速度以及所推定的所述角速度偏置，推定所述对象时刻的所述自身位置的旋转分量，

基于所推定的所述对象时刻的所述自身位置，使所述对象时刻的所述拍摄图像所包含的一个或者多个特征点各自与所述过去时刻的所述拍摄图像所包含的对应特征点相关联，

调整所推定的所述对象时刻的所述自身位置，以使所述再投影误差、所述旋转分量运动方程式中的所述第1残差以及平动分量运动方程式中的第2残差的合计最小化，

所述平动分量运动方程式中的所述第2残差是对所述平动分量运动方程式代入了所述过去时刻的所述自身位置的平动分量、所测定的所述加速度、所推定的所述速度、所推定的所述重力加速度以及所推定的所述加速度偏置的情况下的残差的评价值。

技术方案13

根据技术方案12所述的位置推定装置，

所述状态量推定部推定使对所述旋转分量运动方程式代入了所述过去时刻的所述自身位置的旋转分量的情况下的所述第1残差成为最小的所述角速度偏置，

所述旋转分量运动方程式由下述的式(1)表示，

R(t+Δt)＝R(t)×Exp((ω(t)－b

R(t+Δt)表示所述对象时刻的所述自身位置的旋转分量，

R(t)表示所述过去时刻的所述自身位置的旋转分量，

ω(t)表示所测定的所述角速度，

b

Δt表示所述过去时刻与所述对象时刻的时间差。

技术方案14

根据技术方案13所述的位置推定装置，

所述状态量推定部将多个所述过去时刻的所述自身位置的旋转分量代入到所述旋转分量运动方程式，通过非线性最小二乘法来推定所述角速度偏置，

所述可靠度算出部，

取得所述角速度偏置下的、相互正交的X方向分量、Y方向分量以及Z方向分量的协方差矩阵，

算出所取得的所述协方差矩阵的对角分量之和的倒数来作为所述角速度偏置的所述可靠度。

技术方案15

根据技术方案13或者14所述的位置推定装置，

所述状态量推定部推定使对所述平动分量运动方程式代入了所述过去时刻的所述自身位置的平动分量的情况下的所述第2残差成为最小的所述速度、所述重力加速度以及所述加速度偏置，

所述平动分量运动方程式由下述的式(2)以及式(3)表示，

p(t+Δt)＝p(t)+v(t)×Δt+(1/2)×g×Δt

v(t+Δt)＝v(t)+g×Δt+R(t)×(a(t)－b

p(t+Δt)表示所述对象时刻的所述自身位置的平动分量，

p(t)表示所述过去时刻的所述自身位置的平动分量，

v(t)表示所述速度，

Δt表示所述过去时刻与所述对象时刻的时间差，

g表示所述重力加速度，

a(t)表示所测定的所述加速度，

b

v(t+Δt)表示所述对象时刻的所述速度。

技术方案16

根据技术方案15所述的位置推定装置，

所述状态量推定部将多个所述过去时刻的所述自身位置的平动分量代入到所述平动分量运动方程式，通过非线性最小二乘法来推定所述加速度偏置，

所述可靠度算出部，

取得所述加速度偏置下的、相互正交的X方向分量、Y方向分量以及Z方向分量的协方差矩阵，

算出所取得的所述协方差矩阵的对角分量之和的倒数来作为所述加速度偏置的所述可靠度。

技术方案17

根据技术方案2～16中任一项所述的位置推定装置，还具备：

运动类别确定部，其基于所推定的所述自身位置的轨迹，确定所述移动体装置的运动类别；和

异常判定部，其判断所选择的所述一个或者多个对象传感器的组合是否与所确定的所述运动类别对应，在不对应的情况下，判定为发生了异常。

技术方案18

一种移动体控制系统，具备位置推定装置和移动体控制装置，所述位置推定装置对设置了一个或者多个传感器的移动体装置的自身位置进行推定，所述一个或者多个传感器观测与移动有关的信息，所述移动体控制装置基于所推定的所述自身位置，控制所述移动体装置的移动，

所述位置推定装置具有：

状态量推定部，其基于所述一个或者多个传感器各自的观测值，推定表示所述一个或者多个传感器各自的状态的状态量；

可靠度算出部，其算出表示所述一个或者多个传感器各自的所述状态量的可靠程度的可靠度；以及

自身位置推定部，其基于所述一个或者多个传感器各自的所述状态量的所述可靠度，选择所述一个或者多个传感器中的一个或者多个对象传感器，基于所选择的所述一个或者多个对象传感器各自的所述观测值以及所述状态量，推定所述自身位置。

技术方案19

一种位置推定方法，通过信息处理装置对设置了一个或者多个传感器的移动体装置的自身位置进行推定，所述一个或者多个传感器观测与移动有关的信息，

所述信息处理装置，

基于所述一个或者多个传感器各自的观测值，推定表示所述一个或者多个传感器各自的状态的状态量，

算出表示所述一个或者多个传感器各自的所述状态量的可靠程度的可靠度，

基于所述一个或者多个传感器各自的所述状态量的所述可靠度，选择所述一个或者多个传感器中的一个或者多个对象传感器，基于所选择的所述一个或者多个对象传感器各自的所述观测值以及所述状态量，推定所述自身位置。

技术方案20

一种程序，是用于使信息处理装置作为位置推定装置发挥功能的程序，所述位置推定装置对设置了一个或者多个传感器的移动体装置的自身位置进行推定，所述一个或者多个传感器观测与移动有关的信息，

所述程序使所述信息处理装置作为状态量推定部、可靠度算出部以及自身位置推定部发挥功能，

所述状态量推定部基于所述一个或者多个传感器各自的观测值，推定表示所述一个或者多个传感器各自的状态的状态量，

所述可靠度算出部算出表示所述一个或者多个传感器各自的所述状态量的可靠程度的可靠度，

所述自身位置推定部基于所述一个或者多个传感器各自的所述状态量的所述可靠度，选择所述一个或者多个传感器中的一个或者多个对象传感器，基于所选择的所述一个或者多个对象传感器各自的所述观测值以及所述状态量，推定所述自身位置。