CN102288957A - 角速度推测装置以及角速度推测方法 - Google Patents

Abstract

提供一种角速度推测装置以及角速度推测方法，高精度地推测移动体的角速度。周边物体观测装置(811；雷达)针对移动体的周边存在的物体，反复观测以移动体为基准的物体的相对位置。相对位置取得部(211)取得周边物体观测装置(811)观测到的观测结果。静止物体判定部(220；静止物体识别部)判定周边物体观测装置(811)观测到相对位置的物体是否静止。物体相关部(230；静止物体用追踪部)从由周边物体观测装置(811)观测到的多个相对位置中，判定针对同一物体由周边物体观测装置(811)观测到的多个相对位置。角速度推测部(状态推测部240、轨道推测部)推测移动体的角速度。

Description

角速度推测装置以及角速度推测方法

技术领域

本发明涉及推测移动体的角速度的角速度推测装置。

背景技术

在陀螺传感器等对移动体的角速度进行观测的角速度观测装置中，有偏移误差(bias error)等误差。有如下方式：使用全球定位***(GPS)等位置观测装置，观测移动体的位置，并根据观测结果来推测移动体的角速度。另外，有如下方式：使用根据位置观测装置的观测结果推测出的角速度，来校正角速度观测装置观测到的角速度。

【专利文献1】日本特开平6-148319号公报

【专利文献2】日本特开平9-49875号公报

【专利文献3】日本特开2007-333385号公报

发明内容

如GPS那样依靠外部的***来对移动体的位置进行观测的位置观测装置有时根据电波状况等而无法使用。

本发明是为了解决例如所述那样的课题而完成的，目的在于不用依靠GPS那样的位置观测装置，而高精度地推测移动体的角速度。

本发明的角速度推测装置的特征在于，具有相对位置取得部、静止物体判定部、物体相关部、以及角速度推测部，

所述相对位置取得部针对存在于移动体的周边的物体，取得周边物体观测装置观测到的观测结果，其中，所述周边物体观测装置反复观测以所述移动体为基准的所述物体的相对位置，

所述静止物体判定部根据所述相对位置取得部所取得的观测结果，判定所述周边物体观测装置观测到相对位置的物体是否静止，

所述物体相关部根据所述相对位置取得部所取得的观测结果，从所述周边物体观测装置观测到的多个相对位置中，判定针对同一物体由所述周边物体观测装置观测到的多个相对位置，

所述角速度推测部根据所述相对位置取得部所取得的观测结果、所述静止物体判定部所判定的判定结果、以及所述物体相关部所判定的判定结果，推测所述移动体的角速度。

根据本发明的角速度推测装置，即使没有GPS那样的位置观测装置，也可以高精度地推测移动体的角速度。

附图说明

图1是示出实施方式1中的偏航率(yaw rate)偏移校正装置800的结构的框图。

图2是示出直至时刻t₁～t_N在时间方向上连续地取得了相关性的静止物体的情形的图。

图3是示出实施方式1中的相关静止物体选择部141的处理的处理流程图。

图4是示出向静止物体基准坐标系的坐标变换的图。

图5是示出静止物体基准坐标系中的本车轨道的图。

图6是示出实施方式1中的偏移校正部840的处理的处理流程图。

图7是示出时刻对准的图。

图8是示出实施方式2中的偏航率偏移校正装置800的结构的框图。

图9是示出多个静止物体的相对位置的一个例子的图。

图10是时序的说明图。

图11是示出实施方式3中的偏航率偏移校正装置800的结构的框图。

图12是示出实施方式3中的轨道综合部150的动作的处理流程图。

图13是示出实施方式4中的静止物体用追踪部120的结构的框图。

图14是示出实施方式5中的角速度推测装置200的硬件结构的一个例子的硬件结构图。

图15是示出实施方式5中的角速度推测装置200的功能块的结构的一个例子的块结构图。

图16是用于说明移动体801的移动速度、与物体701、702相对移动体801的相对速度的关系的图。

图17是用于说明物体相对移动体的相对速度的观测时刻781～783、与移动体的移动速度的观测时刻791～799的关系的图。

图18是用于说明移动体801的移动轨迹751、与物体703～708相对移动体801的相对位置的轨迹761～766的关系的图。

图19是用于说明实施方式5中的物体相关部230的动作的图。

图20是用于说明以移动体801为基准的相对坐标系、与以静止点为基准的绝对坐标系之间的关系的图。

图21是示出实施方式5中的角速度推测处理S500的流程的一个例子的流程图。

图22是示出实施方式5中的观测结果取得工序S510的流程的一个例子的流程图。

图23是示出实施方式5中的静止判定工序S520的流程的一个例子的流程图。

图24是示出实施方式5中的物体相关工序S530的流程的一个例子的流程图。

图25是示出实施方式5中的状态推测工序S550的流程的一个例子的流程图。

图26是用于说明物体相关部230在相关处理中使用的平行移动量以及旋转角度、与移动体801的移动量的关系的图。

图27是示出实施方式7中的角速度推测装置200的功能块的结构的一个例子的块结构图。

图28是示出实施方式8中的角速度推测装置200的功能块的结构的一个例子的块结构图。

附图标记说明

110：静止物体识别部；120：静止物体用追踪部；121：相关部；122：平滑部；123：预测部；124：延迟部；130：静止物体存储部；140：本车轨道计算部；141：相关静止物体选择部；142：静止物体基准坐标变换部；143：轨道推测部；150：轨道综合部；200：角速度推测装置；211：相对位置取得部；212：速度观测值取得部；213：角速度观测值取得部；220：静止物体判定部；230：物体相关部；240：状态推测部；251：角速度误差存储部；252：角速度校正部；253：角速度检验部；254：角速度误差计算部；800：偏航率偏移校正装置；810：雷达；811：周边物体观测装置；820：车速传感器；821：移动速度观测装置；830：偏航率传感器；831：角速度观测装置；840：偏移校正部；901：输出装置；902：输入装置；911：处理装置；914：存储装置。

具体实施方式

说明在使用车辆中搭载的雷达装置来监视车辆前方时对车辆前方的物体进行检测的装置。

有如下的车辆用前方监视装置，即，该车辆用前方监视装置搭载于车辆，向车辆前方的规定角度照射光波、毫米波等发送波，并接收其反射波，从而识别在本车辆的前方行驶的车辆、本车辆的前方的障碍物。在车辆用前方监视装置中，需要从所检测出的物体中，不依赖于直线路径行驶时、转弯路径行驶时而将本车辆的行进方向上存在的车辆识别为先行车辆。此时，作为对在本车行进方向上存在的先行车辆进行识别的方式，使用车速和偏航率(角速度)传感器来计算道路曲率，识别先行车辆。

但是，在为了计算道路曲率而使用的偏航率传感器中产生与电压偏置量相当量的零点偏移误差，而且伴随着温度、时间变化，零点偏移误差会漂移，所以如果原样地使用，则弄错先行车判定的可能性高。因此，需要进行传感器的零点偏移校正。

有如下方式：为了计算本车的偏航率(以下，称为偏航角速度)，取得由本车辆的雷达检测出的多个静止物体的前一时刻下的角度与当前时刻下的角度的相关性。作为相关方式有如下方式：使在前一时刻检测出的静止物体的角度偏移Δθ，将与在当前时刻检测出的静止物体的角度最具有相关性的Δθ视为角度变化量，计算偏航角速度。但是，在实际的道路行驶环境中，在时间方向上未必能够连续地检测出静止物体。例如，由于在途中发生检测故障、再出现、或者多径波(multipathwave)所致的无用信号的混入等，所以在所述方法中无法正确地推测偏航角速度。

另外，有如下方式：为了检测本车辆的停止状态(速度为0的情况)，使用与检测物体的相对速度成为0的条件，将低速时的偏航角速度视为偏航角速度零点偏移值而进行校正。但是，如果本车并非低速时，则无法进行偏移校正。

另外，有如下方式：根据利用GPS得到的本车的绝对位置以及利用雷达检测出的与静止物体的相对位置，计算包括偏航角速度的本车辆的运动。在该方式中，还包括与本车的运动无关的静止物体而计算本车轨道，因此如果没有利用GPS得到的绝对位置，就无法正确地计算出偏航角速度。

这样，在偏航率传感器中产生电压偏置量的零点偏移误差，而且伴随着温度、时间变化，零点偏移误差会漂移，所以如果原样地使用，则弄错先行车判定的可能性高。

因此，利用追踪滤波器从多个静止物体中抽取在时间方向上具有相关性的静止物体，根据该静止物体与本车辆的相对位置的时间序列数据，来计算基于本车的位置和运动(在本文中称为轨道)的偏航角速度推测值。而且，根据偏航角速度观测值与推测值之差来计算偏移误差，校正偏航角速度。

实施方式1.

使用图1～图7，说明实施方式1。

图1是示出本实施方式中的偏航率偏移校正装置800的结构的框图。

偏航率偏移校正装置800具备雷达810、车速传感器820、偏航率传感器830、静止物体识别部110、静止物体用追踪部120、静止物体存储部130、本车轨道计算部140、以及偏移校正部840。

雷达810(周边物体观测装置)向车辆前方的规定角度范围内照射雷达波，并接收物体反射的雷达波，检测该物体的位置以及相对速度。

车速传感器820(移动速度观测装置)检测车辆的速度。

偏航率传感器830(角速度观测装置)检测偏航角速度。

静止物体识别部110(相对位置取得部、速度观测值取得部、静止物体判定部)根据由所述雷达810检测出的物体的相对速度和由所述车速传感器820检测出的车辆的速度，识别该物体是否为静止物体。

静止物体用追踪部120(物体相关部)利用追踪滤波器，从由所述静止物体识别部110识别为静止物体的多个静止物体中抽取在时间方向上具有相关性的静止物体。静止物体用追踪部120例如具有相关部121、平滑部122、预测部123、以及延迟部124。

静止物体存储部130存储由所述静止物体用追踪部120抽取的已相关的静止物体。

本车轨道计算部140(角速度推测部)将由所述静止物体存储部130得到的静止物***置作为输入，将所述静止物***置变换到静止物体基准的坐标系从而得到本车位置，根据所述本车位置来推测包含偏航角速度的本车轨道。本车轨道计算部140例如具有相关静止物体选择部141、静止物体基准坐标变换部142、以及轨道推测部143。

偏移校正部840使用由偏航率传感器830检测出的偏航角速度和从所述本车轨道计算部140得到的偏航角速度推测值，计算偏航角速度的偏移误差并进行校正。

图2是示出直至时刻t₁～t_N在时间方向上连续地取得了相关性的静止物体的情形的图。

当追踪静止物体时，可知根据本车辆的运动而移动。

在静止物体用追踪部120中，如该图所示利用追踪滤波器(卡尔曼滤波器、α-β(-γ)滤波器等公知的滤波器)，从多个静止物体中抽取在时间方向上取得相关性的有效的静止物体。该图的白圆意味着在时间方向上取得了相关性的同一静止物体，带斜线的圆意味着所述静止物体(白圆)的最新时刻的位置，黑圆意味着在时间方向上没有连续性而在静止物体用追踪部120中没有取得相关性的静止物体。从静止物体用追踪部120输出取得了相关性的静止物体的位置(如果有多普勒速度则还包括多普勒速度)和该静止物体编号(表示同一静止物体的编号)。

在静止物体存储部130中，记录静止物体编号和其已相关静止物体的时间序列数据，并将所记录的相关静止物体的时间序列数据和该静止物体编号输出到相关静止物体选择部141。

在相关静止物体选择部141中，将每个同一静止物体的相关静止物体时间序列数据作为输入，从所述相关静止物体时间序列数据中，仅抽取进入预先设定的距离R_min(例如30m)以内的相关静止物体，并对相关次数N_cor进行计数。而且，在所述相关次数N_cor是相关次数阈值N_min以上的情况下，输出所述相关静止物体的位置。

图3是示出本实施方式中的相关静止物体选择部141的处理的处理流程图。

在步骤1中，相关静止物体选择部141抽取进入距离阈值以内的相关静止物体。

在步骤2中，相关静止物体选择部141判定相关静止物体数是否为阈值以上。

在相关静止物体数是阈值以上的情况下，在步骤3中，相关静止物体选择部141输出所述相关静止物体。

此处，对于相关次数阈值N_min，使用本车速V_M、雷达观测速率T、距离R_min如下式那样决定上限。

【式11】

$N_{\min }\≤\frac{R_{\min }}{V_M\·T}$

例如，如果设成R_min＝30[m]、V_M＝16[m/s]、T＝0.2[s]，则虽然通过雷达可进行9次左右的观测，但无法进行其以上的观测。因此，通过设定满足式11的条件的阈值N_min，从而可靠地执行以后的处理。

在静止物体基准坐标变换部142中，从以雷达位置为原点的坐标系变换为以某时刻的静止物体为原点的坐标系。原本，在以雷达为原点的坐标系中利用与雷达的相对位置而得到静止物体的位置。因此，通过以静止物体在某时刻的位置为坐标系的原点而对本车辆的位置进行坐标变换，从而使用从静止物体观察的本车的位置，计算出本车辆的轨道。以下使用例子来进行说明。

图4是示出向静止物体基准坐标系的坐标变换的图。

如该图所示，在以本车行进方向为Y、以车轴方向为X的本车辆基准XY坐标系中定义了静止物***置的情况下，将在时刻t₁、t₂下观测到的某静止物体的位置分别设成(x₁，y₁)、(x₂，y₂)(虽然本来在雷达中可根据距离和角度而得到静止物***置，但如果进行坐标变换，则容易变换为xy坐标系的位置，所以省略该步骤)。接下来，在以时刻t₁下的所述静止物体的位置为原点的坐标系中，时刻t₁下的本车的位置矢量z_o1以及时刻t₂下的位置矢量z_o2成为如下式那样。

【式12】

z_o1＝|-x₁-y₁|^T

z_o2＝[-x₂-y₂]^T

在静止物体基准坐标变换部142中，这样将本车位置输出到轨道推测部143。

图5是示出静止物体基准坐标系中的本车轨道的图。

在轨道推测部143中，将所述本车位置的时间序列数据作为输入，使用基于协调旋转模型(coordinated turn model)等转弯模型的扩展卡尔曼滤波器，来计算本车的轨道推测值。此处，轨道推测值

(也记为X hat)如下式那样由位置(x，y)、速度((也记为x dot)，

(也记为y dot))、偏航角速度ω构成。

【式13】

$\hat{X}={\left[\begin{array}{ccccc}x& y& \stackrel{\·}{x}& \stackrel{\·}{y}& \mathrm{\ω}\end{array}\right]}^T$

轨道推测部143将轨道推测值和推测误差协方差矩阵输出到偏移校正部840。

在偏移校正部840中，通过从本车轨道计算部140输入的偏航角速度，计算偏航角速度观测值的偏移误差，输出偏移校正后的偏航角速度。

图6是示出本实施方式中的偏移校正部840的处理的处理流程图。

首先，在步骤1中，从本车轨道计算部140输出的轨道推测值和推测误差协方差矩阵被输入到偏移校正部840。另外，偏航角速度观测值从偏航率传感器830输入到偏移校正部840。此时，也可以在通过追踪滤波器等而对偏航角速度观测值进行了平滑处理之后，输入该偏航角速度观测值。但是，由于并非始终从本车轨道计算部140输出偏航角速度推测值，所以如该处理流程那样在无法得到偏航角速度推测值的采样时刻，从后述的偏移校正值存储器中使用所保存的偏移校正值来校正偏航角速度观测值(步骤2)。此处，将偏移校正值的初始值设成0。

接下来，在步骤3中，使用从本车轨道计算部140输出的轨道推测值和推测误差协方差矩阵，计算由偏航角速度观测值得到的基于转弯模型的预测值和预测误差协方差、以及由偏航角速度推测值得到的基于转弯模型的预测值和预测误差协方差。但是，通常，由雷达810对静止物体进行检测的采样速率、与从本车的偏航率传感器830对偏航角速度进行检测的速率不同，偏航率传感器830的采样速率更细致，所以在偏航角速度的观测时刻t与偏航角速度推测值的更新时刻t’不同的情况下需要进行时刻对准。

图7是示出时刻对准的图。

为此，通过直至时刻t为止的时间外插处理，计算出时刻t的推测值和推测误差协方差矩阵。

【式14】

$\hat{X}\left(t\right)=F({\mathrm{\ω}}_{\mathrm{est}}\left(t^{\′}\right),t-t^{\′})\hat{X}\left(t^{\′}\right)$

P_t＝F(ω_est(t′)，t-t′)^TP_t′F(ω_est(t′)，t-t′)+Q(t-t′)

此处，F(ω_est(t’)，t-t’)意味着将偏航角速度设成时刻t’的偏航角速度推测值ω_est的情况下的转弯模型的从时刻t’向时刻t的状态推移矩阵。另外，Q(t-t’)是***噪声的协方差矩阵。

使用所述的推测值和推测误差协方差，计算出由从时刻t起ΔT秒后的偏航角速度观测值得到的基于转弯模型的预测值

和预测误差协方差P_OBS，t+ΔT。而且，计算出由偏航角速度推测值得到的基于转弯模型的ΔT秒后的预测值

和预测误差协方差P_EST，t+ΔT。

然后，在步骤4中，在不满足基于卡方(chi-square)检验的如下判定式的情况下，由偏航角速度观测值得到的预测值与由偏航角速度推测值得到的预测值不同，所以在偏航角速度观测值中包含偏移误差。

【式15】

$({\hat{X}}_{\mathrm{OBS},t+\mathrm{\ΔT}}-{\hat{X}}_{\mathrm{EST},t+\mathrm{\ΔT}})(P_{\mathrm{OBS},t+\mathrm{\ΔT}}+P_{\mathrm{EST},t+\mathrm{\ΔT}})({\hat{X}}_{\mathrm{OBS},t+\mathrm{\ΔT}}-{\hat{X}}_{\mathrm{EST},t+\mathrm{\ΔT}})$

$\≤{\mathrm{\ϵ}}_{\mathrm{th}}$

此处，根据卡方分布表求出ε_th。

此处，偏航角速度观测值的观测速率比偏航角速度推测值的输出速率(或者，雷达的检测速率)更细致，所以在得到偏航角速度观测值的每一时刻，进行使用了判定式的判定，在N次中有M次不满足上述判定式的情况下，实施以下的偏移误差推测处理。在满足的情况下，视为在偏航角速度观测值中不包含偏移误差，使用偏移校正值存储器中保存的偏移误差来进行校正(步骤2)。

接下来，在步骤5中，使用所述偏航角速度推测值和偏航角速度观测值，推测偏航角速度观测值的偏移误差。此处，使用例如卡尔曼滤波器等来推测偏移误差。

在偏移校正值存储器中将偏移误差推测值作为偏移校正值而进行保存(步骤6)。最后，输出使用偏移校正值进行了校正的偏航角速度(步骤7)。

在以上说明的偏航率偏移校正装置800中，本车轨道计算部140通过将从搭载于本车的雷达810得到的静止物体的位置变换到静止物体基准的坐标系，由此计算出静止物体基准坐标系中的本车位置，使用所述本车位置来推测本车轨道。

本车轨道计算部140仅抽取距离雷达810在规定的距离以内存在的静止物体，并在得到规定数以上的所述静止物体的情况下，推测本车轨道。

本车轨道计算部140使用基于转弯模型的扩展卡尔曼滤波器，根据静止物体基准坐标系中的本车位置来推测本车轨道。

偏移校正部840通过使用了预测误差协方差矩阵的卡方判定，没有将在从偏航率传感器830得到的偏航角速度观测值以及由本车轨道计算部140计算出的偏航角速度推测值的各个中进行时间外插而计算出的预测值的差分视作相同的情况下，进行偏航角速度观测值中包含的偏移误差推测。

偏移校正部840在偏航角速度观测值的观测时刻与由本车轨道计算部140计算出的偏航角速度推测值的推测时刻不同的情况下，通过对观测时刻与推测时刻之差进行时间外插，使偏航角速度推测值的推测时刻对准偏航角速度观测值的观测时刻。

这样，通过从包含无用信号的静止物体中抽取在时间方向上具有连续性的准确的静止物体，并使用该静止物体与本车的位置来推测包含偏航角速度的本车轨道，可以正确地计算偏航角速度。而且，通过使用所述偏航角速度推测值来校正由偏航率传感器检测出的偏航角速度观测值的偏移误差，由此可以利用以高采样速率得到的偏航角速度观测值，进行正确的先行车判定。

实施方式2.

使用图8～图10说明实施方式2。

另外，对于与实施方式1共同的部分，附加同一符号，省略说明。

图8是示出本实施方式中的偏航率偏移校正装置800的结构的框图。

在实施方式1中，使用某1个静止物体对偏航角速度进行了校正，但实际上由于观测到多个单独的静止物体(如果是道路诱导标(delineator)，则是多个道路诱导标等)，所以在本实施方式2中示出使用了多个静止物体的偏航率偏移校正装置800。

在本车轨道计算部140中，将多个静止物体作为输入而推测本车轨道。另外，从静止物体用追踪部120将已相关静止物体输入到本车轨道计算部140的时刻针对每个静止物体不同。

图9是示出多个静止物体的相对位置的一个例子的图。

图10是时序的说明图。

例如，如图9那样在得到静止物体1和静止物体2这2个静止物体的位置的情况下，依照图10那样的时序，进行静止物体1的追踪(相当于图10的静止物体用追踪1)和静止物体2的追踪(相当于图10的静止物体用追踪2)的处理。

首先，依照图3的流程，如图10那样在时刻t₄从静止物体用追踪1向本车轨道计算部140输出已相关静止物体。在静止物体用追踪2中由于是在时刻t₅输出，所以在以先前向本车轨道计算部140进行了输出的静止物体1为基准的静止物体基准坐标系中，进行本车轨道计算部的处理。所述本车轨道计算部140的处理与实施方式1相同，所以省略。

从时刻t₅，从静止物体用追踪2输出已相关静止物体，所以将所述静止物体变换到以静止物体1为基准的坐标系。此时，使用时刻t₅下的静止物体1与静止物体2的相对位置矢量进行坐标变换即可(图9)。将该相对位置矢量设成由静止物体用追踪部120输出的静止物体1推测位置与静止物体2推测位置的相对位置即可。

即使在时刻t₆下静止物体1的静止物体缺少的情况下，由于使用静止物体2的静止物体来更新，所以会提高精度。

当静止物体1在时刻t₉出到雷达覆盖区域外的情况下，其以后在以其他静止物体为基准的坐标系中继续进行本车轨道计算部140的处理。关于时刻t₉以后的通过本车轨道计算部140得到的本车辆的位置，从以静止物体1为基准的坐标系变换为以静止物体2为基准的坐标系。

以后，在检测出其他静止物体的情况下也反复进行同样的处理，从而在由于静止物体是1个而观测次数少、本车轨道的推测精度不佳的情况下，通过使用多个静止物体来继续进行推测，从而具有提高推测精度的效果。

其他处理与实施方式1相同，所以省略。

进一步使用图8来说明偏航率偏移校正装置800的输入输出。

从静止物体用追踪部120输出已相关静止物体的位置(如果能观测到多普勒速度，则还包含多普勒速度)、静止物体编号、以及静止物体的推测值(位置和速度)。在静止物体基准坐标变换中，使用各静止物体的推测位置，将各静止物***置(以及多普勒速度)的时间序列数据变换到以基准静止物体(在所述例子中相当于静止物体1)为原点的坐标系。在轨道推测部中，使用依次输入的静止物***置来推测包含偏航角速度推测值的轨道，并输出到偏移校正部。

在以上说明的偏航率偏移校正装置800中，静止物体用追踪部120将该静止物体的推测位置输出到静止物体存储部130，所述本车轨道计算部140使用从多个静止物体得到的位置的时间序列数据，在某个成为基准的静止物体的坐标系中根据多个静止物体来计算出本车位置，并使用所述本车位置来推测本车轨道。

实施方式3.

使用图11～图12来说明实施方式3。

另外，对于与实施方式1～实施方式2共同的部分，附加同一符号，省略说明。

图11是示出本实施方式中的偏航率偏移校正装置800的结构的框图。

在本车轨道计算部140a～140c中，计算出各静止物体的轨道推测值和推测误差方差矩阵，并与各静止物体的推测值(通过静止物体用追踪而计算出的推测值)一起输出到轨道综合部150。在轨道综合部150中，使用各静止物体的推测值来对准坐标，使用推测误差协方差矩阵，对轨道推测值进行加权综合。

图12是示出本实施方式中的轨道综合部150的动作的处理流程图。

在步骤P1中输入到轨道综合部150的轨道的数量N_TRK是2以上的情况下，进入到选择成为坐标系的基准的静止物体的步骤P2。将相关次数N_cor多的轨道选择为基准静止物体，在步骤P3中将其他轨道的位置变换到基准静止物体的坐标系。

步骤P4的作为对轨道进行综合的方式，也可以使用已有的航迹综合方式。例如，考虑有色性使用协方差交叉法来进行综合。或者，不考虑有色性，可以使用最小二乘综合法。

为了抑制运算负荷，也可以使用下式，用预定的参数a(0≤a≤1)进行加权综合。

【式16】

x^SYS＝ax^TRK1+(1-a)x^TRK2

或者，也可以使用下式，根据航迹的更新状况进行加权综合。

【式17】

$x^{\mathrm{SYS}}=\frac{n_{\mathrm{TRK}1}}{n_{\mathrm{TRK}1}+n_{\mathrm{TRK}2}}{x}^{\mathrm{TRK}1}+\frac{n_{\mathrm{TRK}2}}{n_{\mathrm{TRK}1}+n_{\mathrm{TRK}2}}{x}^{\mathrm{TRK}2}$

此处，根据静止物体1和2分别得到轨道x^TRK1、x^TRK2。另外，x^SYS相当于综合后的轨道，n_TRK1相当于静止物体1的相关次数N_cor，n_TRK2相当于静止物体2的相关次数N_cor。

其他处理与实施方式1或者2相同，所以省略。

以上说明的偏航率偏移校正装置800具备雷达810、车速传感器820、偏航率传感器830、静止物体识别部110、静止物体用追踪部120、静止物体存储部130、本车轨道计算部140、轨道综合部150、以及偏移校正部840。

雷达810向车辆前方的规定角度范围内照射雷达波，并接收被物体所反射的雷达波，检测该物体的位置以及相对速度。

车速传感器820检测车辆的速度。

偏航率传感器830检测偏航角速度。

静止物体识别部110根据由所述雷达810检测出的物体的相对速度和由所述车速传感器820检测出的车辆的速度，识别该物体是否为静止物体。

静止物体用追踪部120通过追踪滤波器，从由所述静止物体识别部110识别为静止物体的多个静止物体中，抽取在时间方向上具有相关性的静止物体，计算该静止物体推测位置。

静止物体存储部130存储从所述静止物体用追踪部120得到的该静止物体的推测位置以及已相关静止物体。

本车轨道计算部140a～140c将从所述静止物体存储部130得到的已相关静止物***置的时间序列数据作为输入，将所述静止物***置变换到静止物体基准的坐标系，从而得到本车位置的时间序列数据，根据所述本车位置来推测包含偏航角速度的本车轨道。

轨道综合部150对从所述本车轨道计算部140a～140c的各个输出的多个轨道推测值进行加权综合，计算出本车轨道。

偏移校正部840使用由偏航率传感器830检测出的偏航角速度和从所述本车轨道计算部140a～140c得到的偏航角速度推测值，计算出偏航角速度的偏移误差并进行校正。

轨道综合部150从多个静止物体的各轨道推测值中，在以相关静止物体最多的静止物体为基准的坐标系中对轨道推测值进行综合而计算本车轨道。

实施方式4.

使用图13来说明实施方式4。

另外，对与实施方式1～实施方式3共同的部分，附加同一符号，省略说明。

图13是示出本实施方式中的静止物体用追踪部120的结构的框图。

静止物体用追踪部120还具有对静止物体进行分组(clustering)的静止物体分组部。包括相关部、平滑部、预测部和延迟部的追踪滤波器对由静止物体分组部进行分组得到的组进行追踪。

在实际的道路行驶环境下有来自视线引导标(道路诱导标)、护栏(guard rail)、壁等静止物体的反射。特别是从护栏、壁等表面积宽的静止物体，得到多个反射点，有时难以追踪1个反射点。因此，通过在静止物体用追踪部120的追踪滤波器的前级实施静止物体的分组，集中从表面积大的静止物体得到的多个反射点，并通过追踪其组，从而提高静止物体用追踪部的相关性能。

其他处理与实施方式1～3相同，所以省略。

在以上说明的偏航率偏移校正装置800中，静止物体用追踪部120对静止物体进行分组，并追踪组。

实施方式5.

使用图14～图25来说明实施方式5。

另外，对于与实施方式1～实施方式4共同的部分，附加同一符号，省略说明。

图14是示出本实施方式中的角速度推测装置200的硬件结构的一个例子的硬件结构图。

角速度推测装置200是例如具有处理装置911、存储装置914、输入装置902、以及输出装置901的计算机。

处理装置911通过执行计算机程序来处理数据，对角速度推测装置200整体进行控制。

存储装置914存储处理装置911执行的计算机程序、处理装置911处理的数据等。存储装置914例如是易失性存储器、非易失性存储器、闪存存储器、磁盘装置、光盘装置等。

输入装置902从角速度推测装置200的外部输入信号、信息，变换为处理装置911可处理的形式的数据。输入装置902所变换的数据既可以由处理装置911直接处理，也可以由存储装置914临时存储。关于输入装置902，例如有：键盘、鼠标等输入利用者的操作的操作输入装置、将模拟信号变换为数字数据的模拟数字变换装置等变换装置、接收其他装置所发送的信号的接收装置等接口装置等。

输出装置901将处理装置911处理了的数据、存储装置914存储了的数据变换为可以向角速度推测装置200的外部进行输出的形式而输出。关于输出装置901，例如有：对图像进行显示的显示装置或扬声器等将数据变换为人的五官可感知的形式而输出的装置、将数字数据变换为模拟信号的数字模拟变换装置等变换装置、对其他装置发送信号的发送装置等接口装置等、将数据变换为可向其他装置进行输入的形式而输出的装置等。

通过由处理装置911执行存储装置914所存储的计算机程序。来实现角速度推测装置200的功能块。另外，角速度推测装置200的功能块也可以未必通过计算机来实现，既可以通过数字电路、模拟电路等电子电路来实现，也可以通过机械性结构等的电气性结构以外的结构来实现。

另外，实施方式1～实施方式4中说明的偏航率偏移校正装置800的功能块也同样地既可以通过计算机来实现，也可以通过其他结构来实现。

图15是示出本实施方式中的角速度推测装置200的功能块的结构的一个例子的块结构图。

角速度推测装置200搭载于汽车等移动体中。在移动体中，除了角速度推测装置200以外，还搭载了周边物体观测装置811、移动速度观测装置821、角速度观测装置831等。

周边物体观测装置811对在移动体的周边存在的物体的位置、速度等进行观测。周边物体观测装置811例如是雷达。例如，周边物体观测装置811通过对放射电波、激光等放射波并被物体反射的反射波进行探测，测定传输时间，计算出直到物体为止的距离。周边物体观测装置811对放射放射波的方向或者接收反射波的方向进行扫描等，判定物体存在的方向。周边物体观测装置811对放射波进行1次扫描所需的时间是例如0.2秒。因此，在移动体与物体之间的相对位置关系不变化的情况下，周边物体观测装置811例如每隔0.2秒反复观测同一物体。周边物体观测装置811根据所计算出的距离和所判定的方向，计算物体相对移动体的相对位置。另外，周边物体观测装置811通过检测例如由多普勒效应引起的放射波与反射波之间的波长的偏差，由此检测物体相对移动体的相对速度。周边物体观测装置811输出表示物体的相对位置、相对速度等观测结果的信号。

移动速度观测装置821观测移动体的移动速度。移动速度观测装置821例如通过安装于汽车车轴的转速计来测定车轴的转速，计算出移动速度。移动速度观测装置821对例如0.1秒期间的车轴的转速进行计数。移动速度观测装置821通过对所计数的转速乘以轮胎的一周的长度，由此计算出在0.1秒的期间移动体行进的距离。移动速度观测装置821通过使所计算出的距离成为10倍，计算出移动体的秒速。在该例子的情况下，移动速度观测装置821每隔0.1秒重复观测1次移动体的移动速度。移动速度观测装置821输出表示对移动体的移动速度进行观测得到的观测结果的信号。

另外，移动速度观测装置821也可以不计算移动体的移动速度，而输出原样地表示所测定的车轴的转速的信号。在该情况下，后述的速度观测值取得部212代替移动速度观测装置821来执行所述计算，计算移动体的移动速度。

角速度观测装置831观测移动体的角速度。角速度观测装置831例如是偏航率传感器、陀螺传感器等。角速度观测装置831既可以是对移动体的三维方向(水平方向、前后方向、扭转方向)的角速度进行观测的结构，也可以是仅对水平方向的角速度进行观测的结构。另外，角速度观测装置831也反复观测移动体的角速度。观测间隔例如是10毫秒。角速度观测装置831输出表示对移动体的角速度进行观测而得到的观测结果的信号。

角速度推测装置200根据周边物体观测装置811、移动速度观测装置821、角速度观测装置831等观测结果，推测移动体的角速度。因为在角速度观测装置831所观测的角速度中包含有偏移误差、其他误差。角速度推测装置200通过综合周边物体观测装置811、移动速度观测装置821、角速度观测装置831等观测结果，推测更正确的角速度。另外，角速度推测装置200也可以不仅推测移动体的角速度，而且还推测移动体的速度、移动方向、位置等。

另外，角速度推测装置200也可以根据所推测出的角速度以及角速度观测装置831所观测的角速度，计算出角速度观测装置831观测到的角速度的误差。角速度推测装置200也可以预先存储所计算出的角速度的误差，在没有得到周边物体观测装置811等的观测结果的情况下、或所得到的观测结果的可靠性低的情况下，使用所存储的角速度的误差，校正角速度观测装置831观测到的角速度，由此推测移动体的角速度。

角速度推测装置200具有相对位置取得部211、速度观测值取得部212、角速度观测值取得部213、静止物体判定部220、物体相关部230、以及状态推测部240。

相对位置取得部211使用输入装置902，输入由周边物体观测装置811所输出的信号，取得周边物体观测装置811观测到的观测结果。

相对位置取得部211使用处理装置911，判定输入了由周边物体观测装置811所输出的信号的时刻，计算出周边物体观测装置811观测到物体的观测时刻。相对位置取得部211既可以将输入了信号的时刻原样地视为观测时刻，也可以考虑周边物体观测装置811中的延迟时间而将与输入了信号的时刻相比回退延迟时间的时刻设成观测时刻。另外，也可以构成为在周边物体观测装置811输出的信号中包含有表示周边物体观测装置811观测到物体的观测时刻的信息。在该情况下，相对位置取得部211使用处理装置911，从所输入的信号取得观测时刻。

相对位置取得部211使用存储装置914，存储如下数据，其中，该数据表示周边物体观测装置811观测到物体的观测时刻、所观测到的物体的相对位置以及相对速度。将相对位置取得部211所存储的数据称为“周边物体观测数据”。在“周边物体观测数据”之中，将表示观测时刻的数据称为“物体观测时刻数据”，将表示物体的相对位置的数据称为“相对位置观测值数据”，将表示物体的相对速度的数据称为“相对速度观测值数据”。

速度观测值取得部212使用输入装置902，输入由移动速度观测装置821所输出的信号，取得移动速度观测装置821观测到的观测结果。速度观测值取得部212使用处理装置911，进行与相对位置取得部211同样的处理，求出移动速度观测装置821观测到移动体的移动速度的观测时刻。速度观测值取得部212使用存储装置914，存储如下数据，其中，该数据表示移动速度观测装置821观测到移动体的移动速度的观测时刻、所观测到的移动体的移动速度。将速度观测值取得部212所存储的数据称为“移动速度数据”。在“移动速度数据”之中，将表示观测时刻的数据称为“移动速度观测时刻数据”，将表示移动体的移动速度的数据称为“移动速度观测值数据”。

角速度观测值取得部213使用输入装置902，输入由角速度观测装置831所输出的信号，取得角速度观测装置831观测到的观测结果。角速度观测值取得部213使用处理装置911，进行与相对位置取得部211、速度观测值取得部212同样的处理，求出角速度观测装置831观测到移动体的角速度的观测时刻。角速度观测值取得部213使用存储装置914，存储如下数据，其中，该数据表示角速度观测装置831观测到移动体的角速度的观测时刻、所观测到的移动体的角速度。将角速度观测值取得部213所存储的数据称为“移动体角速度数据”。在“移动体角速度数据”之中，将表示观测时刻的数据称为“角速度观测时刻数据”、将表示移动体的角速度的数据称为“角速度观测值数据”。

静止物体判定部220使用处理装置911，判定周边物体观测装置811观测到的物体是否静止。静止物体判定部220使用存储装置914，存储表示所判定的判定结果的数据。将静止物体判定部220所存储的数据称为“静止判定结果数据”。

例如，静止物体判定部220使用处理装置911，输入相对位置取得部211所存储的周边物体观测数据和速度观测值取得部212所存储的移动速度数据。静止物体判定部220使用处理装置911，从所输入的周边物体观测数据中，取得表示周边物体观测装置811观测到的物体相对移动体的相对速度的相对速度观测值数据。静止物体判定部220使用处理装置911，从所输入的移动速度数据中，取得表示移动体的移动速度的移动速度观测值数据。静止物体判定部220使用处理装置911，根据相对速度观测值数据所表示的相对速度和移动速度观测值数据所表示的移动速度，判定周边物体观测装置811观测到的物体是否静止。

图16是用于说明移动体801的移动速度、与物体701、702相对移动体801的相对速度的关系的图。

箭头711、713分别表示移动体801、物体702的移动速度。设物体701静止。箭头721、722分别表示物体701、702相对移动体801的相对速度。

由于物体701静止，所以如果以移动体801为基准，则物体701看起来向与移动体801的移动速度正好相反朝向的方向，以与移动体801的移动速度正好相同的速度进行移动。与此相对，由于物体702移动，所以如果以移动体801为基准，则物体702看起来以将由移动体801的移动所致的表观速度(apparent speed)(箭头723)与由物体702的移动所致的实际的速度(箭头713)进行合成得到的速度进行移动。因此，物体相对移动体的相对速度(表观速度)可以用下式表示。

【式18】

v_r＝v_t-v_s

其中，v_r是表示物体相对移动体的相对速度的矢量。v_t是表示物体的移动速度的矢量。v_s是表示移动体的移动速度的矢量。

例如，静止物体判定部220使用处理装置911，将表示物体相对移动体的相对速度的矢量v_r、与表示移动体的移动速度的矢量v_s进行相加，计算表示物体的移动速度的矢量v_t。静止物体判定部220使用处理装置911，计算所计算出的矢量v_t的绝对值。静止物体判定部220使用处理装置911，将所计算出的绝对值与规定的阈值进行比较。考虑物体相对移动体的相对速度、移动体的移动速度的观测误差而预先设定阈值。在矢量v_t的绝对值小于阈值的情况下，静止物体判定部220使用处理装置911，判定为该物体静止。

另外，在利用多普勒效应来观测物体相对移动体的相对速度的情况下，所观测的相对速度仅为物体的表观速度(箭头721、722)中的、物体与移动体的距离方向的分量(箭头725、726)，而观测不到以移动体801为轴的旋转方向的分量(箭头727、728)。所观测的相对速度可以用下式表示。

【式19】

υ_d＝|v_τ|cosα

其中，v_d是表示物体相对移动体的相对速度的距离方向分量的实数。α是表示物体相对移动体的移动速度的方向、与从物体观看的能看到移动体的方向之间的角度的实数。

由于观测不到物体相对移动体的相对速度中的、以移动体801为轴的旋转方向的分量(箭头727、728)，所以角度α是未知的。但是，由于物体701是静止的，所以物体701的角度α(733)与角度731一致，其中，该角度731是移动体801的移动方向与从移动体801观看的能看到物体701的方向之间的角度。与此相对，由于物体702移动，所以物体702的角度α(734)未必与角度732一致，其中，该角度732是移动体801的移动方向与从移动体801观看的能看到物体701的方向之间的角度。

例如，静止物体判定部220使用处理装置911，计算移动体的移动方向与从移动体观看的能看到物体的方向之间的角度的余弦(cosine)。静止物体判定部220使用处理装置911，计算表示移动体的移动速度的矢量v_s的绝对值、与所计算出的余弦之积。静止物体判定部220使用处理装置911，计算从所计算出的积中减去表示物体相对移动体的相对速度的距离方向分量的实数v_d而得到的差。静止物体判定部220使用处理装置911，将所计算出的差与规定的阈值进行比较。考虑物体相对移动体的相对速度、移动体的移动速度的观测误差而预先设定阈值。在差小于阈值的情况下，静止物体判定部220使用处理装置911，判定为该物体静止。

图17是用于说明物体相对移动体的相对速度的观测时刻781～783、与移动体的移动速度的观测时刻791～799的关系的图。

如上所述，周边物体观测装置811对物体相对移动体的相对位置、相对速度进行观测的观测周期771、与移动速度观测装置821对移动体的移动速度进行观测的观测周期772不同。另外，周边物体观测装置811对物体相对移动体的相对位置、相对速度进行观测的观测时刻781～783是对物体存在的方向进行了扫描的时刻，所以如果物体存在的方向发生变化，则1次的观测周期中的定时(timing)会变化，是不定期的。与此相对，移动速度观测装置821对移动体的移动速度进行观测的观测时刻791～799的间隔是一定的，与观测周期772相等。

因此，物体相对移动体的相对速度的观测时刻781～783、与移动体的移动速度的观测时刻791～799一致的可能性几乎不存在。但是，需要在相同的时刻观测：静止物体判定部220为了判定物体是否静止而使用的物体相对移动体的相对速度、与移动体的移动速度。

在通过对车轴的转速进行计数而观测移动体的移动速度的情况下，所观测的移动速度是从开始计数的时刻至结束计数的时刻的移动速度的平均值。例如，在时刻793观测到的移动速度是从时刻792至时刻793的移动速度的平均值。因此，静止物体判定部220将在时刻781观测到的物体相对移动体的相对速度、与在时刻793观测到的移动体的移动速度进行比较即可。

例如，静止物体判定部220使用处理装置911，每当速度观测值取得部212取得移动速度观测装置821的观测结果时执行处理。静止物体判定部220使用处理装置911，输入由速度观测值取得部212所存储的本次的移动速度数据、和前一个移动速度数据。静止物体判定部220使用处理装置911，从所输入的2个移动速度数据中，取得分别表示移动速度观测装置821观测到移动体的移动速度的本次的观测时刻和上次的观测时刻的2个移动速度观测时刻数据。

接下来，静止物体判定部220使用处理装置911，从相对位置取得部211所存储的周边物体观测数据之中，输入未处理的周边物体观测数据。静止物体判定部220使用处理装置911，从所输入的周边物体观测数据中，取得表示周边物体观测装置811观测到物体的观测时刻的物体观测时刻数据。静止物体判定部220使用处理装置911，对所取得的物体观测时刻数据表示的观测时刻、与所取得的2个移动速度观测时刻数据表示的观测时刻进行比较。

在周边物体观测装置811观测到物体的观测时刻是移动速度观测装置821观测到移动体的移动速度的上次的观测时刻之前的情况下，使用移动速度观测装置821上次观测到的移动体的移动速度，判定周边物体观测装置811观测到的物体是否静止。静止物体判定部220使用处理装置911，从所输入的前一个移动速度数据中，取得表示移动速度观测装置821观测到的移动速度的移动速度观测值数据。

在周边物体观测装置811观测到物体的观测时刻是移动速度观测装置821观测到移动体的移动速度的上次的观测时刻之后，并且是本次的观测时刻之前的情况下，使用移动速度观测装置821本次观测到的移动体的移动速度，判定周边物体观测装置811观测到的物体是否静止。静止物体判定部220使用处理装置911，从所输入的本次的移动速度数据中，取得表示移动速度观测装置821观测到的移动速度的移动速度观测值数据。

在周边物体观测装置811观测到物体的观测时刻是移动速度观测装置821观测到移动体的移动速度的本次的观测时刻之后的情况下，使用移动速度观测装置821下次观测的移动体的移动速度，判定周边物体观测装置811观测到的物体是否静止。由于尚未取得移动体的移动速度的下次的观测值，所以静止物体判定部220在本次不对其周边物体观测数据进行处理。

例如，由此静止物体判定部220从移动速度观测装置821观测到的观测结果之中，选择为了判定周边物体观测装置811观测到的物体是否静止而使用的移动体的移动速度。或者，静止物体判定部220也可以根据移动速度观测装置821观测到的观测结果，计算为了判定周边物体观测装置811观测到的物体是否静止而使用的移动体的移动速度。

例如，静止物体判定部220使用处理装置911，根据速度观测值取得部212所存储的移动速度数据表示的观测时刻和移动速度，进行平滑处理。在周边物体观测装置811观测到物体的观测时刻是移动速度观测装置821观测到移动体的移动速度的最新的观测时刻之前的情况下，静止物体判定部220使用处理装置911，根据平滑处理的结果，推测周边物体观测装置811观测到物体的观测时刻下的移动体的移动速度。或者，在周边物体观测装置811观测到物体的观测时刻是移动速度观测装置821观测到移动体的移动速度的最新的观测时刻之前的情况下，静止物体判定部220使用处理装置911，根据平滑处理的结果，预测周边物体观测装置811观测到物体的观测时刻下的移动体的移动速度。静止物体判定部220使用处理装置911，使用所推测或者预测出的移动体的移动速度，判定周边物体观测装置811观测到的物体是否静止。

物体相关部230使用处理装置911，对于周边物体观测装置811观测到的物体中的静止物体判定部220判定为静止的物体，进行相关处理。即，物体相关部230使用处理装置911，从周边物体观测装置811观测到物体的观测结果之中，判定多次观测所静止的相同物体而得到的观测结果。物体相关部230使用存储装置914，存储表示所判定的判定结果的数据。将物体相关部230存储的数据称为“相关结果数据”。

例如，物体相关部230使用处理装置911，输入静止物体判定部220所存储的静止判定结果数据。物体相关部230使用处理装置911，根据所输入的静止判定结果数据表示的判定结果，从相对位置取得部211所存储的周边物体观测数据之中，输入关于静止物体判定部220判定为静止的物体的周边物体观测数据，其中，该周边物体观测数据是未处理的周边物体观测数据。物体相关部230使用处理装置911，从所输入的周边物体观测数据，取得表示周边物体观测装置811观测到物体的观测时刻的物体观测时刻数据、和表示周边物体观测装置811观测到的物体相对移动体的相对位置的相对位置观测值数据。

当存在根据已处理的周边物体观测数据推测出的物体相对移动体的相对位置的轨迹的情况下，物体相关部230使用处理装置911，判定所取得的数据表示的观测时刻以及相对位置是否适合于其轨迹。在有多个适合的轨迹的情况下，物体相关部230使用处理装置911，判定所取得的数据表示的观测时刻以及相对位置最适合的轨迹。物体相关部230使用处理装置911，将所取得的数据表示的观测时刻以及相对位置编入到(最)适合的轨迹而更新轨迹。

当不存在所取得的数据表示的观测时刻以及相对位置适合的轨迹的情况下，物体相关部230使用处理装置911，判定在不存在适合的轨迹的其他周边物体观测数据中是否有可以成为新的轨迹的周边物体观测数据。在有可以成为新的轨迹的周边物体观测数据的情况下，物体相关部230使用处理装置911，生成新的轨迹。

物体相关部230使用存储装置914，存储表示所生成或者更新了的轨迹的数据。

另外，在该时间点，还不知道移动体的位置、朝向，所以根据物体相对移动体的相对位置(表观位置)来进行物体相关部230中的相关处理。

图18是用于说明移动体801的移动轨迹751、与物体703～708相对移动体801的相对位置的轨迹761～766的关系的图。

例如，设移动体801如移动轨迹751所示蜿蜒地移动。另外，设物体703～708静止。

在时刻785，周边物体观测装置811针对物体703～708，观测相对移动体801的相对位置741a～741f。

在时刻786，周边物体观测装置811针对物体703～708，观测相对移动体801的相对位置742a～742f。

在时刻787，周边物体观测装置811针对物体703～708，观测相对移动体801的相对位置743a～743f。

物体相关部230根据这些观测结果进行相关处理，生成轨迹761～766。

这样，静止的物体703～708相对移动体801的相对位置伴随着移动体801的移动而变化。特别是在移动体801转弯了的情况下，能看到物体703～708的方向较大地变化，所以物体703～708相对移动体801的相对位置也较大地变化。

因此，静止的物体703～708相对移动体801的相对位置的轨迹761～766有时这样成为复杂的形状。

在周边物体观测装置811观测到多个静止的物体703～708的情况下，各个物体703～708相对移动体801的相对位置伴随着移动体801的移动、转弯而较大地变化，但物体703～708之间的位置关系不会变化。关于物体703～708相对移动体801的相对位置，不仅方向伴随着移动体801的转弯而发生变化，而且方向、距离伴随移动体801的移动而还发生变化。与此相对，由于物体703～708是静止的，所以即使移动体801进行移动、转弯，物体703～708之间的距离也不会变化，而只是物体703～708之间的方向伴随着移动体801的转弯而发生变化。

物体相关部230利用该情形，使用处理装置911，预测周边物体观测装置811观测的物体相对移动体801的相对位置。物体相关部230使用处理装置911，使用所预测的结果来进行相关处理。

例如，物体相关部230使用处理装置911，从在1次的扫描周期内由周边物体观测装置811观测到的多个物体之中，选择1个静止的物体。物体相关部230使用处理装置911，以使所选择的物体的坐标与原点一致的方式，对周边物体观测装置811在本次的扫描周期内观测到的多个静止的物体相对移动体801的相对位置的坐标进行平行移动。另外，物体相关部230使用处理装置911，从在上次的扫描周期内由周边物体观测装置811观测到的多个物体之中，也同样选择1个静止的物体。物体相关部230使用处理装置911，以使所选择的物体的坐标与原点一致的方式，对周边物体观测装置811在上次的扫描周期内观测到的多个静止的物体相对移动体801的相对位置的坐标进行平行移动。

物体相关部230使用处理装置911，使对周边物体观测装置811在本次的扫描周期内观测到的物体的相对位置进行了平行移动的坐标，以原点为中心进行旋转。物体相关部230使用处理装置911，计算出所旋转的各个坐标、与对周边物体观测装置811在上次的扫描周期内观测到的物体的相对位置进行了平行移动的各个坐标之间的距离。在所计算出的距离小于规定的阈值的情况下，物体相关部230使用处理装置911，视作与该2个坐标对应的物体是同一物体，算出视作同一物体的坐标的组的数量。

物体相关部230使用处理装置911，改变所旋转的角度而重复进行上述处理，求出视作同一物体的坐标的组的数量最多的角度。

物体相关部230使用处理装置911，改变从周边物体观测装置811在本次的扫描周期内观测到的多个物体中选择的物体、与从周边物体观测装置811在上次的扫描周期内观测到的多个物体中选择的物体的组合而重复进行上述处理，求出视作同一物体的坐标的组的数量最多的组合和旋转角度。

图19是用于说明本实施方式中的物体相关部230的动作的图。

物体相关部230使用处理装置911，从周边物体观测装置811在上次的扫描周期内观测到的静止的物体相对移动体801的相对位置741a～741f中，例如选择相对位置741c。物体相关部230使用处理装置911，以使相对位置741c的坐标与原点一致的方式，对相对位置741a～741f的坐标进行平行移动。

接下来，物体相关部230使用处理装置911，从周边物体观测装置811在本次的扫描周期内观测到的静止的物体相对移动体801的相对位置742a～742f中，例如选择相对位置742c。物体相关部230使用处理装置911，以使相对位置742c的坐标与原点一致的方式，对相对位置742a～742f的坐标进行平行移动。

物体相关部230使用处理装置911，使平行移动了的相对位置742a～742f的坐标以原点为中心而进行旋转。物体相关部230使用处理装置911，对旋转了的相对位置742a～742f的坐标、与平行移动了的相对位置741a～741f的坐标进行比较，算出视为同一物体的坐标的组的数量。

在物体相关部230所选择的相对位置的组合是周边物体观测装置811实际上观测到同一物体的相对位置的组、且旋转了的角度与移动体801转弯了的角度一致的情况下，视为同一物体的坐标的组的数量最多。

物体相关部230使用处理装置911，将视为同一物体的坐标的组的数量最多的相对位置的组合，判定为是周边物体观测装置811观测到同一物体的相对位置。另外，此时对于与视为同一物体的坐标的组对应的相对位置的组合，物体相关部230也判定为是周边物体观测装置811观测到同一物体的相对位置。

这样，由于物体相关部230仅针对静止物体判定部220判定为静止的物体进行相关处理，所以即使不知道移动体801的移动轨迹751，也可以对周边物体观测装置811观测到的物体进行相关追踪。

另外，物体相关部230既可以没有遗漏地尝试所有的相对位置的组合，也可以使用移动速度观测装置821观测到的移动体801的移动速度等信息来缩小所尝试的相对位置的组合。

另外，物体相关部230既可以不限定使平行移动了的坐标进行旋转的角度的范围而进行尝试，也可以使用角速度观测装置831观测到的移动体801的角速度等信息来缩小所尝试的角度。

另外，物体相关部230也可以并非对视为同一物体的坐标的组的数量进行比较，而是对以视为同一物体的坐标的组为顶点的凸多边形的面积进行比较，判定面积最宽的相对位置的组合、旋转角度。因为在视为同一物体的坐标的组集中到窄范围的情况下，有可能是1个物体的细微部分。

状态推测部240(角速度推测部)使用处理装置911，推测移动体的位置、速度、移动方向、角速度、移动速度观测装置821观测到的移动速度的增益误差、角速度观测装置831观测到的角速度的偏移误差等状态量。状态推测部240使用存储装置914，存储表示所推测的结果的数据。状态推测部240使用输出装置901，将所推测的结果输出到外部。

状态推测部240每当推测状态量时，利用由物体相关部230判定为是周边物体观测装置811观测到静止的同一物体得到的观测结果的物体相对移动体801的相对位置。

另外，在多个扫描周期的期间观测到1个物体的情况下，观测到该物体的扫描周期的数量越多，利用由周边物体观测装置811观测到该物体的相对移动体801的相对位置的价值越高。因此，也可以仅在观测到该物体的扫描周期的数量比规定的阈值多的情况下，利用由周边物体观测装置811观测到该物体的相对移动体801的相对位置。或者，也可以仅在连续地观测到该物体的扫描周期的数量比规定的阈值多的情况下，利用由周边物体观测装置811观测到该物体的相对移动体801的相对位置。例如，状态推测部240使用处理装置911，根据由物体相关部230所判定的判定结果，计算出(连续地)观测到该物体的扫描周期的数量，并将所计算出的扫描周期的数量与阈值进行比较，仅在(连续地)观测到该物体的扫描周期的数量比阈值多的情况下，利用由周边物体观测装置811观测到的该物体相对移动体801的相对位置。

关于周边物体观测装置811观测到的物体相对移动体的相对位置，可以考虑为以移动体的绝对位置为原点、并以移动体的移动方向为基准的相对坐标系中的坐标。

图20是用于说明以移动体801为基准的相对坐标系、与以静止点为基准的绝对坐标系之间的关系的图。

X轴以及Y轴是以移动体801为基准的相对坐标系中的坐标轴。x轴以及y轴是绝对坐标系中的坐标轴。另外，绝对坐标系的原点只要静止则可以是任意处。

如果将绝对坐标系中的移动体801的位置设为(x_s，y_s)、将移动体801的移动方向与绝对坐标系的y轴所成的角度设为θ，则在相对坐标系中的坐标(X，Y)与绝对坐标系中的坐标(x，y)之间，存在如下关系。

【式20】

$\left(\begin{array}{c}X\\ Y\end{array}\right)=R_{\mathrm{\θ}}\left(\begin{array}{c}x-x_s\\ y-y_s\end{array}\right),$ $R_{\mathrm{\θ}}=\left(\begin{array}{cc}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right)$

其中，R_θ是表示角度θ的旋转变换的矩阵。

如果将静止的物体的绝对坐标系中的坐标设为(x_t，y_t)、将周边物体观测装置811观测到的该物体相对移动体的相对位置的坐标设为(x_r、y_r)，不考虑观测误差，则可以原样地适用式20的坐标变换，所以下式成立。

【式21】

$\left(\begin{array}{c}{x}_r\\ y_r\end{array}\right)=R_{\mathrm{\θ}}\left(\begin{array}{c}{x}_t-x_s\\ y_t-y_s\end{array}\right)$

状态推测部240例如使用扩展卡尔曼滤波器，使用下式所示的模型来进行推测。

【式22】

$x_1=\left(\begin{array}{c}{x}_t\\ y_t\\ x_s\\ y_s\\ \mathrm{\ν}\\ \mathrm{\θ}\\ \mathrm{\ω}\end{array}\right),$ $f_1\left(x_1\right)=\left(\begin{array}{c}{x}_t\\ y_t\\ x_s+\mathrm{\ν\Δ}t\sin \mathrm{\θ}\\ y_s+\mathrm{\ν\Δ}t\cos \mathrm{\θ}\\ \mathrm{\ν}\\ \mathrm{\θ}+\mathrm{\ω\Δt}\\ \mathrm{\ω}\end{array}\right),$

$z_1=\left(\begin{array}{c}{x}_r\\ y_r\end{array}\right),$ $h_1\left(x_1\right)=R_{\mathrm{\θ}}\left(\begin{array}{c}{x}_t-x_s\\ y_t-y_s\end{array}\right)$

其中，x₁是表示状态量的矢量。x_t是周边物体观测装置811观测到的静止物体的绝对坐标系中的x坐标。y_t是周边物体观测装置811观测到的静止物体的绝对坐标系中的y坐标。x_s是绝对坐标系中的移动体的x坐标。y_s是绝对坐标系中的移动体的y坐标。v是移动体的移动速度。θ是移动体的移动方向。ω是移动体的角速度。f₁是表示状态迁移模型的函数。Δt是经过时间。z₁是表示观测量的矢量。x_r是周边物体观测装置811观测到的静止物体相对移动体的相对位置的x坐标。y_r是周边物体观测装置811观测到的静止物体相对移动体的相对位置的Y坐标。h₁是表示观测模型的函数。

另外，也可以构成为如下：作为扩展卡尔曼滤波器的观测值，状态推测部240不仅使用由周边物体观测装置811观测到的物体相对移动体的相对位置的坐标(x_r，y_r)，而且还使用由移动速度观测装置821观测到的移动体的移动速度、角速度观测装置831观测到的移动体的角速度。在该情况下，状态推测部240例如使用下式所示的模型来进行推测。

【式23】

$x=\left(\begin{array}{c}{x}_1\\ {\mathrm{\ϵ}}_{\mathrm{\ν}}\\ {\mathrm{\ϵ}}_{\mathrm{\ω}}\end{array}\right),$ $f\left(x\right)=\left(\begin{array}{c}{f}_1\left(x_1\right)\\ {\mathrm{\ϵ}}_{\mathrm{\ν}}\\ {\mathrm{\ϵ}}_{\mathrm{\ω}}\end{array}\right),$

$z=\left(\begin{array}{c}{z}_1\\ {\mathrm{\ν}}_o\\ {\mathrm{\ω}}_o\end{array}\right),$ $h\left(x\right)=\left(\begin{array}{c}{h}_1\left(x_1\right)\\ (1+{\mathrm{\ϵ}}_{\mathrm{\ν}})\mathrm{\ν}\\ \mathrm{\ω}+{\mathrm{\ϵ}}_{\mathrm{\ω}}\end{array}\right)$

其中，x是表示状态量的矢量。ε_v是移动速度观测装置821观测的移动体的移动速度的增益误差。ε_ω是角速度观测装置831观测的移动体的角速度的偏移误差。f是表示状态迁移模型的函数。z是表示观测量的矢量。v_o是移动速度观测装置821观测到的移动体的移动速度。ω_o是角速度观测装置831观测到的移动体的角速度。x₁、f₁、z₁、h₁遵从式22中的定义。

另外，周边物体观测装置811观测物体的时刻、移动速度观测装置821观测移动体的移动速度的时刻、以及角速度观测装置831观测移动体的角速度的时刻分别不同，但在各个观测装置观测的观测值的误差之间没有相关性。因此，例如，针对各个观测装置所观测的每个观测值分配观测函数，每当某个观测装置进行观测时，状态推测部240进行更新处理。

另外，在将多个物体相对移动体的相对位置利用于状态量的推测的情况下，状态推测部240也可以使用以包含多个物体在绝对坐标系中的坐标的方式进行了扩展的状态量矢量(x或者x₁)。

但是，如果由扩展卡尔曼滤波器计算的矩阵的次数变高，则计算量增加，所以在处理装置911的处理能力低的情况下，有时难以进行实时的计算。

因此，状态推测部240也可以针对各个物体并行地执行扩展卡尔曼滤波。在该情况下，由多个扩展卡尔曼滤波器分别推测移动体的绝对坐标、角速度等状态量，所以得到多个推测值。例如，状态推测部240使用处理装置911，将由多个扩展卡尔曼滤波器推测出的多个推测值进行平均，而设成移动体的绝对坐标、角速度等状态量的推测值。在每当将推测值进行平均时，状态推测部240也可以使用由多个扩展卡尔曼滤波器计算出的多个误差协方差矩阵，进行与推测精度对应的加权。

图21是示出本实施方式中的角速度推测处理S500的流程的一个例子的流程图。

在角速度推测处理S500中，角速度推测装置200推测移动体的角速度等。角速度推测处理S500例如具有观测结果取得工序S510、静止判定工序S520、物体相关工序S530、以及状态推测工序S550。

在观测结果取得工序S510中，相对位置取得部211、速度观测值取得部212以及角速度观测值取得部213分别取得周边物体观测装置811、移动速度观测装置821以及角速度观测装置831观测到的观测结果。

在静止判定工序S520中，静止物体判定部220根据在观测结果取得工序S510中取得的观测结果，判定周边物体观测装置811观测到的物体是否静止。

在物体相关工序S530中，物体相关部230对于在静止判定工序S520中由静止物体判定部220判定为静止的物体，进行相关处理。

在状态推测工序S550中，状态推测部240根据在观测结果取得工序S510中取得的观测结果、在物体相关工序S530中由物体相关部230进行了相关处理的结果，推测移动体的角速度等。

角速度推测装置200使处理返回观测结果取得工序S510，反复进行处理。

图22是示出本实施方式中的观测结果取得工序S510的流程的一个例子的流程图。

观测结果取得工序S510例如具有相对位置取得工序S511、移动速度取得工序S512、以及角速度取得工序S513。

在相对位置取得工序S511中，相对位置取得部211使用处理装置911，判定周边物体观测装置811是否输出了表示观测结果的信号。在判定为周边物体观测装置811输出了信号的情况下，相对位置取得部211使用输入装置902，输入周边物体观测装置811输出的信号。相对位置取得部211使用处理装置911，取得所输入的信号表示的观测结果。相对位置取得部211使用处理装置911，生成表示所取得的观测结果的周边物体观测数据。相对位置取得部211生成的周边物体观测数据例如包括物体观测时刻数据、相对位置观测值数据、相对速度观测值数据。

物体观测时刻数据表示周边物体观测装置811观测该物体的时刻。例如，物体观测时刻数据是以毫秒为单位表示从规定的时刻起的经过时间的整数值数据。或者，物体观测时刻数据是表示帧的编号的整数值数据、和以毫秒为单位表示从该帧的开始时刻起的经过时间的整数值数据的组。另外，1个帧是周边物体观测装置811的1次的扫描周期。1个帧的长度是例如0.2秒。

相对位置观测值数据表示周边物体观测装置811观测到的物体相对移动体的相对位置。例如，相对位置观测值数据是在以移动体为原点、以移动体的行进方向为Y轴、且相对于移动体的行进方向以直角右方向为X轴的移动体基准的相对坐标系中，以米为单位表示周边物体观测装置811观测到的物体的坐标的2个实数值数据的组。或者，相对位置观测值数据是以米为单位表示周边物体观测装置811观测到的物体与移动体之间的距离的实数值数据、和以弧度(radian)为单位表示从移动体观看能看到周边物体观测装置811观测到的物体的方向与移动体的行进方向所成的角度的实数值数据的组。

相对速度观测值数据表示周边物体观测装置811观测到的物体相对移动体的相对速度。例如，相对速度观测值数据是在移动体基准的相对坐标系中，以米每秒为单位表示周边物体观测装置811观测到的物体相对移动体的相对速度矢量的分量的2个实数值数据的组。或者，相对速度观测值数据是以米每秒为单位表示周边物体观测装置811观测到的物体相对移动体的相对速度的物体与移动体的距离方向分量的实数值数据。

相对位置取得部211使用存储装置914，存储所生成的周边物体观测数据。

在移动速度取得工序S512中，速度观测值取得部212使用处理装置911，判定移动速度观测装置821是否输出了表示观测结果的信号。在移动速度观测装置821输出了信号的情况下，速度观测值取得部212使用输入装置902，输入由移动速度观测装置821输出的信号。速度观测值取得部212使用处理装置911，取得所输入的信号表示的观测结果。速度观测值取得部212使用处理装置911，生成表示所取得的观测结果的移动速度数据。速度观测值取得部212生成的移动速度数据例如包括移动速度观测时刻数据和移动速度观测值数据。

移动速度观测时刻数据表示移动速度观测装置821观测到移动体的移动速度的时刻。例如，移动速度观测时刻数据是以毫秒为单位表示从规定的时刻起的经过时间的整数值数据。

移动速度观测值数据表示移动速度观测装置821观测到的移动体的移动速度。例如，移动速度观测值数据是以米每秒为单位表示移动速度观测装置821观测到的移动体的移动速度的实数值数据。

速度观测值取得部212使用存储装置914，存储所生成的移动速度数据。

在角速度取得工序S513中，角速度观测值取得部213使用处理装置911，判定角速度观测装置831是否输出了表示观测结果的信号。在角速度观测装置831输出了信号的情况下，角速度观测值取得部213使用输入装置902，输入由角速度观测装置831所输出的信号。角速度观测值取得部213使用处理装置911，取得所输入的信号表示的观测结果。角速度观测值取得部213使用处理装置911，生成表示所取得的观测结果的角速度数据。角速度观测值取得部213生成的角速度数据例如包括角速度观测时刻数据和角速度观测值数据。

角速度观测时刻数据表示角速度观测值取得部213观测到移动体的角速度的时刻。例如，角速度观测时刻数据是以毫秒为单位表示从规定的时刻起的经过时间的整数值数据。

角速度观测值数据表示角速度观测值取得部213观测到的移动体的角速度。例如，角速度观测值数据是以弧度每秒为单位表示角速度观测装置831观测到的移动体的角速度的实数值数据。

角速度观测值取得部213使用存储装置914，存储所生成的角速度数据。

另外，物体观测时刻数据、移动速度观测时刻数据、以及角速度观测时刻数据中的观测时刻的表现形式只要可以相互变换，就也可以是不同的形式。

图23是示出本实施方式中的静止判定工序S520的流程的一个例子的流程图。

静止判定工序S520例如具有移动速度观测时刻取得工序S521、物体选择工序S522、观测时刻比较工序S523、以及速度比较工序S524。

在移动速度观测时刻取得工序S521中，静止物体判定部220使用处理装置911，取得在移动速度取得工序S512中由速度观测值取得部212所存储的移动速度数据中的、观测时刻最新的移动速度数据。静止物体判定部220使用处理装置911，从所取得的移动速度数据中包含的移动速度观测时刻数据中，取得移动速度观测装置821观测到移动体的移动速度的最新的观测时刻。另外，静止物体判定部220使用处理装置911，从所取得的移动速度数据中包含的移动速度观测值数据中，取得移动速度观测装置821观测到的移动体的移动速度。

另外，也可以在上次执行了静止判定工序S520之后移动速度观测装置821没有观测到新的移动速度的情况下，静止物体判定部220不进行以后的处理而结束静止判定工序S520。例如，静止物体判定部220使用处理装置911，对在上次执行了移动速度观测时刻取得工序S521时取得的最新的观测时刻、与本次取得的最新的观测时刻进行比较。在观测时刻相同的情况下，静止物体判定部220结束静止判定工序S520。

在物体选择工序S522中，静止物体判定部220使用处理装置911，从在相对位置取得工序S511中由相对位置取得部211所存储的周边物体观测数据之中，选择还没有判定该物体是否静止的周边物体观测数据。

例如，在后述的速度比较工序S524中，静止物体判定部220使用存储装置914，针对某个周边物体观测数据判定了是否静止的情况下，将表示其判定结果的静止判定结果数据与该周边物体观测数据对应起来进行存储。

静止物体判定部220使用处理装置911，针对某个周边物体观测数据判定是否存储了对应起来的静止判定结果数据，由此判定是否针对该周边物体观测数据已判定了是否静止。

当不存在应选择的周边物体观测数据的情况下，静止物体判定部220使用处理装置911，结束静止判定工序S520。

当存在1个以上的应选择的周边物体观测数据的情况下，静止物体判定部220使用处理装置911，从其中选择1个周边物体观测数据。静止物体判定部220使用处理装置911，使处理进入到观测时刻比较工序S523。

在观测时刻比较工序S523中，静止物体判定部220使用处理装置911，从在物体选择工序S522中选择的周边物体观测数据中包含的物体观测时刻数据中，取得周边物体观测装置811观测到该物体的观测时刻。静止物体判定部220使用处理装置911，对所取得的观测时刻、与在移动速度观测时刻取得工序S521中取得的观测时刻进行比较。

在周边物体观测装置811观测到物体的观测时刻是移动速度观测装置821观测到移动体的移动速度的最新的观测时刻之后的情况下，静止物体判定部220针对该周边物体观测数据尚未进行是否静止的判定。静止物体判定部220使用处理装置911，使处理返回到物体选择工序S522，选择接下来的周边物体观测数据。

在周边物体观测装置811观测到物体的观测时刻是移动速度观测装置821观测到移动体的移动速度的最新的观测时刻之前的情况下，静止物体判定部220针对该周边物体观测数据进行是否静止的判定。静止物体判定部220使用处理装置911，使处理进入到速度比较工序S524。

在速度比较工序S524中，静止物体判定部220使用处理装置911，从在物体选择工序S522中选择的周边物体观测数据中包含的相对速度数据，取得周边物体观测装置811观测到的物体相对移动体的相对速度。静止物体判定部220使用处理装置911，根据所取得的相对速度和在移动速度观测时刻取得工序S521中取得的移动体的移动速度，判定该物体是否静止。静止物体判定部220使用处理装置911，生成表示所判定的判定结果的静止判常数据。静止物体判定部220使用存储装置914，将所生成的静止判常数据与在物体选择工序S522中选择的周边物体观测数据对应起来进行存储。

静止物体判定部220使用处理装置911，使处理返回到物体选择工序S522，选择接下来的周边物体观测数据。

图24是示出本实施方式中的物体相关工序S530的流程的一个例子的流程图。

物体相关工序S530具有上次静止物体取得工序S531、本次静止物体取得工序S532、上次物体选择工序S533、上次物体平行移动工序S534、本次物体选择工序S535、本次物体平行移动工序S536、旋转角度选择工序S537、本次物体旋转工序S538、距离计算工序S539、同一物体判定工序S540、阈值判定工序S541、以及同一物体存储工序S542。

在上次静止物体取得工序S531中，物体相关部230使用处理装置911，从相对位置取得部211所存储的周边物体观测数据之中，取得所有关于在前一个帧中由周边物体观测装置811观测到的物体的周边物体观测数据，且该周边物体观测数据是在静止判定工序S520中由静止物体判定部220判定为该物体静止的周边物体观测数据。

物体相关部230使用处理装置911，针对所取得的各个周边物体观测数据，求出如下帧的数量，其中，该帧是在与2个以上之前的帧中的周边物体观测数据之间存在由物体相关部230判定为周边物体观测装置811观测到同一物体的周边物体观测数据的帧。

例如，在后述的同一物体存储工序S542中，物体相关部230使用处理装置911，判定为关于在某个帧中由周边物体观测装置811观测到的物体的周边物体观测数据、与关于在其之前的帧中由周边物体观测装置811观测到的物体的周边物体观测数据是观测到同一物体而得到的数据的情况下，生成表示相关次数的相关次数数据。相关次数是判定为同一物体的帧的数量。物体相关部230使用存储装置914，与该周边物体观测数据对应起来存储相关次数数据。当不存在判定为观测到同一物体的与对方的周边物体观测数据对应起来的相关次数数据的情况下，物体相关部230生成使相关次数成为“1”的相关次数数据。当存在判定为观测到同一物体的与对方的周边物体观测数据对应起来的相关次数数据的情况下，物体相关部230生成将对该相关次数数据所表示的相关次数加上“1”而得到的结果作为相关次数的相关次数数据。

物体相关部230使用处理装置911，取得与所取得的周边物体观测数据对应起来存储的相关次数数据所表示的相关次数，由此求出判定为同一物体的帧的数量。

物体相关部230使用处理装置911，按照所取得的帧的数量从多到少的顺序，排列所取得的所有的周边物体观测数据。取得了相关性的帧的数量越多，该周边物体观测数据因噪声等所致的误检测的可能性越低，并且将移动的物体错误地识别为静止的可能性也越低。因此，与本次的帧中的周边物体观测数据取得相关性的可能性高。通过预先按照取得相关性的可能性从高到低的顺序排列周边物体观测数据，可以抑制物体相关工序S530的计算量。

在本次静止物体取得工序S532中，物体相关部230使用处理装置911，从相对位置取得部211所存储的周边物体观测数据中，取得所有关于在最新的帧中由周边物体观测装置811观测到的物体的周边物体观测数据，且该周边物体观测数据是在静止判定工序S520中由静止物体判定部220判定为该物体静止的周边物体观测数据。

在上次物体选择工序S533中，物体相关部230使用处理装置911，从在上次静止物体取得工序S531中取得的周边物体观测数据中，按照在上次静止物体取得工序S531中排列的顺序，选择1个周边物体观测数据。

在上次静止物体取得工序S531中取得的周边物体观测数据全部选择完毕、且不存在应选择的周边物体观测数据的情况下，物体相关部230使用处理装置911，使处理进入到同一物体存储工序S542。

在存在未选择的周边物体观测数据的情况下，物体相关部230使用处理装置911，选择1个未选择的周边物体观测数据。物体相关部230使用处理装置911，预测对于与所选择的周边物体观测数据相同的物体在本次的帧中由周边物体观测装置811所观测的相对位置。例如，物体相关部230使用处理装置911，根据在直到前一个帧为止的期间取得了相关性的物体相对移动体的相对位置的轨迹、状态推测部240所推测的移动体的角速度等状态量，进行预测。

在上次物体平行移动工序S534中，物体相关部230使用处理装置911，对于在上次静止物体取得工序S531中取得的所有的周边物体观测数据，从该周边物体观测数据中包含的相对位置观测值数据表示的周边物体观测装置811观测到的物体相对移动体的相对位置的坐标，减去在本次静止物体取得工序S532中选择的周边物体观测数据中包含的相对位置观测值数据表示的该物体相对移动体的相对位置的坐标，由此对物体的坐标进行平行移动。物体相关部230使用存储装置914，针对在上次静止物体取得工序S531中取得的各个周边物体观测数据，存储表示平行移动后的坐标的数据。

在本次物体选择工序S535中，物体相关部230使用处理装置911，将在本次静止物体取得工序S532中取得的周边物体观测数据，按照该周边物体观测数据中包含的相对位置观测值数据表示的周边物体观测装置811观测到的物体相对移动体的相对位置与在上次物体选择工序S533中预测的相对位置接近的顺序进行排列。因为与所预测的相对位置间的距离越短，是观测到同一物体的观测结果的可能性越高。另外，物体相关部230也可以仅抽取与所预测的相对位置间的距离比规定的阈值短的部分，并按照由近到远的顺序排列。

物体相关部230使用处理装置911，从在本次静止物体取得工序S532中取得的周边物体观测数据(或者，仅抽取了与在上次物体选择工序S533中预测的相对位置间的距离比规定的阈值短的部分的周边物体观测数据)中，按照与在上次物体选择工序S533中预测的相对位置间的距离接近的顺序，选择1个周边物体观测数据。

在本次静止物体取得工序S532中取得的周边物体观测数据(或者，仅抽取了与在上次物体选择工序S533中预测的相对位置间的距离比规定的阈值短的部分的周边物体观测数据)全部选择完毕、且不存在应选择的周边物体观测数据的情况下，物体相关部230使用处理装置911，使处理返回到上次物体选择工序S533，从在上次静止物体取得工序S531中选择的周边物体观测数据中，选择接下来的周边物体观测数据。

当存在未选择的周边物体观测数据的情况下，物体相关部230使用处理装置911，选择1个未选择的周边物体观测数据，使处理进入到本次物体平行移动工序S536。

在本次物体平行移动工序S536中，物体相关部230使用处理装置911，针对在本次静止物体取得工序S532中取得的所有的周边物体观测数据，从该周边物体观测数据中包含的相对位置观测值数据表示的周边物体观测装置811观测到的物体相对移动体的相对位置的坐标，减去在本次物体选择工序S535中选择的周边物体观测数据中包含的相对位置观测值数据表示的该物体相对移动体的相对位置的坐标，由此对物体的坐标进行平行移动。物体相关部230使用存储装置914，针对在本次静止物体取得工序S532中取得的各个周边物体观测数据，存储表示平行移动后的坐标的数据。

在旋转角度选择工序S537中，假设在上次物体选择工序S533中选择的周边物体观测数据、和在本次物体选择工序S535中选择的周边物体观测数据是表示观测到同一物体的观测结果的周边物体观测数据，由物体相关部230使用处理装置911，推测移动体的转弯角度。物体相关部230也可以将状态推测部240推测的状态量、角速度观测装置831观测到的角速度等利用于转弯角度的推测。物体相关部230使用处理装置911，根据所推测的转弯角度，决定所尝试的多个旋转角度。

物体相关部230使用处理装置911，从所决定的多个旋转角度中，按照与所推测出的转弯角度接近的顺序，选择1个旋转角度。

在所决定的多个旋转角度全部选择完毕、且不存在应选择的旋转角度的情况下，物体相关部230使用处理装置911，使处理返回到本次物体选择工序S535，从在本次静止物体取得工序S532中取得的周边物体观测数据中选择接下来的周边物体观测数据。

当存在未选择的旋转角度的情况下，物体相关部230使用处理装置911，选择1个未选择的旋转角度，使处理进入到本次物体旋转工序S538。

在本次物体旋转工序S538中，物体相关部230使用处理装置911，使针对在本次静止物体取得工序S532中取得的所有的周边物体观测数据在本次物体平行移动工序S536中进行了平行移动的坐标，以原点为中心，旋转移动在旋转角度选择工序S537中选择的旋转角度。

在距离计算工序S539中，物体相关部230使用处理装置911，对于在上次静止物体取得工序S531中取得的所有的周边物体观测数据、与在本次静止物体取得工序S532中取得的所有的周边物体观测数据的所有组合，计算出如下距离，其中，该距离是针对在上次静止物体取得工序S531中取得的周边物体观测数据在上次物体平行移动工序S534中进行了平行移动的坐标、与针对在本次静止物体取得工序S532中取得的周边物体观测数据在本次物体旋转工序S538中进行了旋转移动的坐标之间的距离。物体相关部230使用处理装置911，按照所计算出的距离由近到远的顺序，生成在上次静止物体取得工序S531中取得的周边物体观测数据、与在本次静止物体取得工序S532中取得的周边物体观测数据的对。

在同一物体判定工序S540中，物体相关部230使用处理装置911，算出在距离计算工序S539中生成的对中的、所计算出的距离比规定的阈值短的对的数量。物体相关部230使用处理装置911，在本次执行了物体相关工序S530的过程中将所算出的对数与此前在同一物体判定工序S540中算出的对数的最大值进行比较。

在本次算出的对数小于此前的对数的最大值的情况下，物体相关部230使处理返回到旋转角度选择工序S537，选择接下来的旋转角度。

在本次算出的对数大于此前的对数的最大值的情况下，物体相关部230使用存储装置914，存储如下数据，其中，该数据表示在距离计算工序S539中生成的对中的所计算出的距离比阈值短的对、和所算出的对数。物体相关部230使用处理装置911，使处理进入到阈值判定工序S541。

在阈值判定工序S541中，物体相关部230使用处理装置911，将在距离计算工序S539中算出的对数与规定的阈值进行比较。

在所算出的对数小于阈值的情况下，物体相关部230使用处理装置911，使处理返回到旋转角度选择工序S537，选择接下来的旋转角度。

在所算出的对数大于阈值的情况下，物体相关部230使用处理装置911，使处理进入到同一物体存储工序S542。因为按照取得相关性的可能性从高到低的顺序试行，所以在较早的阶段发现正确的组合和旋转角度的可能性高，不用进行其以后的试行，由此能够抑制计算量。

另外，成为比较的对象的阈值也可以是预定的常数。另外，也可以根据在上次静止物体取得工序S531中取得的周边物体观测数据的数量、在本次静止物体取得工序S532中取得的周边物体观测数据的数量，由物体相关部230计算阈值。例如，物体相关部230对在上次静止物体取得工序S531中取得的周边物体观测数据的数量、与在本次静止物体取得工序S532中取得的周边物体观测数据的数量进行比较而求出小的数量，并对所求出的数量乘以规定的常数(例如0.6)而计算出阈值。

在同一物体存储工序S542中，物体相关部230使用处理装置911，判定为在同一物体判定工序S540中存储的数据表示的对是表示周边物体观测装置811观测到同一物体的观测结果的周边物体观测数据。物体相关部230使用处理装置911，生成所述相关次数数据作为表示所判定的结果的相关结果数据。另外，物体相关部230使用处理装置911，生成哪个周边物体观测数据是表示周边物体观测装置811观测到同一物体的观测结果的周边物体观测数据的数据，而作为相关结果数据。例如，物体相关部230使用处理装置911，对周边物体观测装置811观测到的物体分配编号。物体相关部230使用存储装置914，将表示所分配的编号的数据与周边物体观测数据对应起来进行存储。物体相关部230对于判定为表示周边物体观测装置811观测到与前一个帧中的周边物体观测数据相同的物体的观测结果的周边物体观测数据，分配与前一个帧中的该周边物体观测数据相同的编号。物体相关部230对其以外的周边物体观测数据，分配与任意一个周边物体观测数据都不同的编号。

图25是示出本实施方式中的状态推测工序S550的流程的一个例子的流程图。

状态推测工序S550具有相关物体选择工序S551、相关次数判定工序S552、预测工序S553、更新工序S554、以及平均工序S555。

在相关物体选择工序S551中，状态推测部240使用处理装置911，从由相对位置取得部211所存储的周边物体观测数据中，选择1个关于在最新的帧中由周边物体观测装置811观测到的物体的周边物体观测数据，且该周边物体观测数据是在物体相关工序S530中由物体相关部230判定为表示观测到与关于在其之前的帧中由周边物体观测装置811观测到的物体的周边物体观测数据相同的物体的观测结果的周边物体观测数据。例如，状态推测部240使用处理装置911，从相对位置取得部211所存储的周边物体观测数据中，选择由物体相关部230存储了对应起来的相关次数数据的周边物体观测数据。

在取得了相关性的周边物体观测数据全部选择完毕、且不存在应选择的周边物体观测数据的情况下，状态推测部240使用处理装置911，使处理进入到平均工序S555。

在取得了相关性的周边物体观测数据中存在未选择的周边物体观测数据的情况下，状态推测部240使用处理装置911，选择1个未选择的周边物体观测数据，使处理进入到相关次数判定工序S552。

在相关次数判定工序S552中，状态推测部240使用处理装置911，将在相关物体选择工序S551中选择的周边物体观测数据的相关次数与规定的阈值进行比较。例如，状态推测部240使用处理装置911，与在相关物体选择工序S551中选择的周边物体观测数据相对应地取得由物体相关部230所存储的相关次数数据。状态推测部240使用处理装置911，将所取得的相关次数数据表示的相关次数与阈值进行比较。

在相关次数小于阈值的情况下，状态推测部240使用处理装置911，使处理返回到相关物体选择工序S551，选择接下来的周边物体观测数据。

在相关次数大于阈值的情况下，状态推测部240使用处理装置911，使处理进入到预测工序S553。

在预测工序S553中，状态推测部240使用处理装置911，使在相关物体选择工序S551中选择的周边物体观测数据表示的物体的绝对位置包含在状态量中，并使用状态迁移函数，预测在相关物体选择工序S551中选择的周边物体观测数据中包含的物体观测时刻数据表示的观测时刻下的状态量。

在更新工序S554中，状态推测部240使用处理装置911，将在相关物体选择工序S551中选择的周边物体观测数据中包含的物体相对移动体的相对位置作为观测量，例如使用扩展卡尔曼滤波器来更新状态量的推测值。状态推测部240使用存储装置914，存储表示更新后的状态量的推测值、误差协方差矩阵的数据。

在平均工序S555中，状态推测部240使用处理装置911，针对取得了相关性的各个周边物体观测数据，将在更新工序S554中推测的状态量的推测值进行平均，而计算作为全体的状态量的推测值。

实施方式6.

使用图26来说明实施方式6。

另外，对于与实施方式5共同的部分，附加同一符号，省略说明。

本实施方式中的角速度推测装置200的硬件结构、功能块的结构与实施方式5相同，所以此处省略说明。

状态推测部240并非将由物体相关部230进行了相关处理的物体相对移动体的相对位置作为观测值而输入，而是将在由物体相关部230进行相关处理时所计算出的平行移动量、旋转角度作为观测值而输入，推测移动体的角速度等状态量。

图26是用于说明物体相关部230在相关处理中使用的平行移动量以及旋转角度、与移动体801的移动量的关系的图。

设物体相关部230取得了在某帧中由周边物体观测装置811观测到的静止物体相对移动体801的相对位置741a～741f、与在其他帧中由周边物体观测装置811观测到的静止物体相对移动体801的相对位置742a～742f的相关性。物体相关部230使用矢量755对相对位置741a～741f的坐标进行平行移动，使相对位置741c的坐标与原点对准。另外，设物体相关部230使用矢量756对相对位置742a～742f的坐标进行平行移动，使相对位置742c的坐标与原点对准，旋转移动旋转角度757，由此取得了相关性。

在该情况下，从使矢量756旋转了旋转角度757的矢量758减去矢量755而得到的差矢量759是这2个帧之间的移动体基准的相对坐标系中的移动体801的移动量。

另外，旋转角度757是这2个帧之间的移动体801的转弯角度。

例如，物体相关部230在图24中说明的同一物体判定工序S540中，在本次算出的对数大于此前的对数的最大值的情况下，使用存储装置914存储：在上次物体选择工序S533中选择的周边物体观测数据、在本次物体选择工序S535中选择的周边物体观测数据、以及表示在旋转角度选择工序S537中选择的旋转角度的数据。

状态推测部240根据由物体相关部230所存储的2个周边物体观测数据和表示旋转角度的数据，计算移动体的移动量以及转弯角度的观测值。例如，状态推测部240使用处理装置911，从在本次物体选择工序S535中由物体相关部230选择的周边物体观测数据中，取得在最新的帧中由周边物体观测装置811观测到的该物体相对移动体的相对位置。状态推测部240使用处理装置911，使表示所取得的相对位置的坐标以原点为中心，旋转移动在旋转角度选择工序S537中由物体相关部230所选择的旋转角度。状态推测部240使用处理装置911，从在上次物体选择工序S533中由物体相关部230所选择的周边物体观测数据中，取得在前一个帧中由周边物体观测装置811观测到的该物体相对移动体的相对位置。状态推测部240使用处理装置911，计算从表示所取得的相对位置的坐标减去旋转移动后的坐标而得到的差，作为移动体的移动量的观测值。另外，状态推测部240使用处理装置911，将在旋转角度选择工序S537中由物体相关部230所选择的旋转角度作为移动体的转弯角度的观测值。

状态推测部240使用处理装置911，将这样计算出的移动体的移动量以及转弯角度的观测值作为例如扩展卡尔曼滤波器的输入而进行推测值的更新处理。在该情况下，状态推测部240例如使用下式所示的模型来进行推测。

【式24】

$x_2=\left(\begin{array}{c}{x}_s\\ y_s\\ \mathrm{\ν}\\ \mathrm{\θ}\\ \mathrm{\ω}\end{array}\right),$ $f_2\left(x_2\right)=\left(\begin{array}{c}{x}_s+\mathrm{\ν\Δ}t\sin \mathrm{\θ}\\ y_s+\mathrm{\ν\Δ}t\cos \mathrm{\θ}\\ \mathrm{\ν}\\ \mathrm{\θ}+\mathrm{\ω\Δt}\\ \mathrm{\ω}\end{array}\right),$

$z_2=\left(\begin{array}{c}\mathrm{\Δx}\\ \mathrm{\Δy}\\ \mathrm{\Δ\θ}\end{array}\right),$ $h_2\left(x_2\right)=\left(\begin{array}{c}\frac{\mathrm{\ν}}{\mathrm{\ω}}(1-\cos \mathrm{\ω\Δt})\\ \frac{\mathrm{\ν}}{\mathrm{\ω}}\sin \mathrm{\ω\Δt}\\ \mathrm{\ω\Δt}\end{array}\right)$

其中，x₂是表示状态量的矢量。f₂是表示状态迁移模型的函数。z₂是表示观测量的矢量。Δx是状态推测部240计算出的移动体的移动量的观测值的横向分量。Δy是状态推测部240计算出的移动体的移动量的观测值的纵向分量。Δθ是状态推测部240计算出的移动体的转弯角度的观测值。h₂是表示观测模型的函数。

此处，表示状态量的矢量x₂不包含静止物体的绝对坐标系中的坐标。因此，状态推测部240无需针对每个静止物体进行推测，而整体进行1个推测即可。

另外，状态推测部240也可以使用用式24的x₂、f₂、z₂、h₂置换了式23的x₁、f₁、z₁、h₁而得到的模型来进行推测。

实施方式7.

使用图27说明实施方式7。

另外，对于与实施方式5～实施方式6共同的部分，附加同一符号，省略说明。

图27是示出本实施方式中的角速度推测装置200的功能块的结构的一个例子的块结构图。

周边物体观测装置811不观测物体相对移动体的相对速度，而仅观测物体相对移动体的相对位置。

在实施方式5中，静止物体判定部220根据周边物体观测装置811观测到的物体相对移动体的相对速度、和移动速度观测装置821观测到的移动体的移动速度，判定物体是否静止。与此相对，本实施方式的周边物体观测装置811不观测物体相对移动体的相对速度，所以静止物体判定部220使用处理装置911，根据作为由物体相关部230取得了相关的结果而得到的物体相对移动体的相对位置的轨迹，计算物体相对移动体的相对速度。静止物体判定部220使用处理装置911，根据所计算出的物体相对移动体的相对速度、和移动速度观测装置821观测到的移动体的移动速度，判定物体是否静止。

另外，在静止物体判定部220判定物体是否静止之前，需要由物体相关部230进行相关处理，所以物体相关部230至少在初始阶段，无法利用静止物体判定部220的判定结果。因此，物体相关部230使用处理装置911，首先对于周边物体观测装置811观测到的所有物体，实施相关处理。另外，也可以根据由物体相关部230进行的相关处理的结果，在由静止物体判定部220判定了物体是否静止之后，物体相关部230对于由静止物体判定部220判定为没有静止的物体，中止相关处理。

物体相关部230由于对不清楚是否静止的物体实施相关处理，因此利用由状态推测部240推测出的移动体的位置、朝向。例如，物体相关部230使用处理装置911，根据由周边物体观测装置811观测到物体的观测时刻下的移动体的位置、朝向，将周边物体观测装置811观测到的物体相对移动体的相对位置的坐标变换为绝对坐标系中的坐标。物体相关部230使用处理装置911，根据变换后的坐标，进行物体的相关处理。

由此，即使在周边物体观测装置811不观测物体相对移动体的相对速度的情况下，也可以推测移动体的角速度等。

实施方式8.

使用图28说明实施方式8。

另外，对于与实施方式1～实施方式7共同的部分，附加同一符号，省略说明。

图28是示出本实施方式中的角速度推测装置200的功能块的结构的一个例子的块结构图。

角速度推测装置200除了实施方式6中说明的功能块以外，还具有角速度误差存储部251、角速度校正部252、角速度检验部253、以及角速度误差计算部254。

状态推测部240不使用由角速度观测装置831观测到的移动体的角速度，而根据其以外的观测值，推测状态量。

角速度误差存储部251使用存储装置914存储如下数据，其中，该数据表示偏移误差等由角速度观测装置831观测的角速度的误差。将角速度误差存储部251存储的数据称为“角速度误差数据”。在最初的阶段，不清楚角速度观测装置831观测的角速度的误差，所以也可以例如假设为没有误差，角速度误差存储部251使用存储装置914，将表示“0”的数据作为角速度误差数据来存储。

角速度校正部252使用处理装置911，校正由角速度观测装置831观测到的移动体的角速度的误差。角速度校正部252使用存储装置914，存储表示所计算出的角速度的数据。将角速度校正部252存储的数据称为“已校正角速度数据”。

例如，角速度校正部252使用处理装置911，输入角速度观测值取得部213存储了的移动体角速度数据、和角速度误差存储部251存储了的角速度误差数据。角速度校正部252使用处理装置911，从所输入的移动体角速度数据，取得角速度观测值数据。角速度校正部252使用处理装置911，计算从所取得的角速度观测值数据表示的角速度减去所输入的角速度误差数据表示的误差而得到的差，计算校正后的角速度。

角速度检验部253使用处理装置911，检验角速度校正部252校正了的角速度是否正确。角速度检验部253使用存储装置914，存储表示所检验的结果的数据。将角速度检验部253存储的数据称为“角速度检验结果数据”。

例如，角速度检验部253使用处理装置911，根据状态推测部240推测出的状态量，检验角速度校正部252校正了的角速度是否正确。

角速度误差计算部254使用处理装置911，计算角速度观测装置831观测到的移动体的角速度的误差。角速度误差计算部254使用存储装置914，存储表示所计算出的误差的数据。将角速度误差计算部254存储的数据称为“角速度误差推测数据”。

例如，角速度误差计算部254使用处理装置911，输入表示状态推测部240推测出的状态量的数据、和角速度观测值取得部213存储了的移动体角速度数据。角速度误差计算部254使用处理装置911，从所输入的数据，取得表示状态推测部240推测出的移动体的角速度的数据。角速度误差计算部254使用处理装置911，从所输入的移动体角速度数据，取得角速度观测值数据。角速度误差计算部254使用处理装置911，计算从所取得的数据表示的移动体的角速度的推测值减去所取得的角速度观测值数据表示的角速度的观测值而得到的差，计算角速度的误差。

在角速度检验部253判定为角速度校正部252校正了的角速度不正确的情况下，角速度误差存储部251更新所存储的角速度误差数据，将角速度误差计算部254存储了的角速度误差推测数据作为新的角速度误差数据，使用存储装置914进行存储。

接下来，说明由角速度检验部253进行的检验处理。

例如，角速度检验部253使用状态推测部240推测出的状态量，预测物体的位置。此时，对于移动体的角速度，不使用由状态推测部240推测出的状态量，而使用角速度校正部252校正了的角速度。由此，在预测精度降低了的情况下，角速度检验部253判定为由角速度校正部252校正了的角速度不正确。

为了用角速度校正部252校正了的角速度来置换状态推测部240推测出的角速度，需要使状态推测部240推测出的状态量的时刻、与角速度观测装置831观测到移动体的角速度的时刻一致。状态推测部240在周边物体观测装置811观测到物体的情况、或移动速度观测装置821观测到移动体的移动速度的情况下，更新状态量的推测值。因此，周边物体观测装置811观测到物体的时刻、或移动速度观测装置821观测到移动体的移动速度的时刻成为状态推测部240推测出的状态量的时刻。

此处，将角速度观测装置831观测到移动体的角速度的时刻设为t₁。将周边物体观测装置811观测到物体的时刻或者移动速度观测装置821观测到移动体的移动速度的时刻设为t₂。另外，设时刻t₁是在时刻t₂之后的时刻。

角速度检验部253例如根据时刻t₂下的状态量的推测值进行外插处理，计算时刻t₁下的状态量的推测值。

例如，角速度检验部253使用处理装置911，计算下式的右边，由此计算时刻t₁下的状态量的推测值。

【式25】

$\hat{X}\left(t_1\right)=F(t_1-t_2)\hat{X}\left(t_2\right)$

其中，带山形的x(

以下记载为“x^”)是推测值矢量。推测值矢量x^是k次的列矢量。推测值矢量x^是时刻t的函数。推测值矢量x^的要素是时刻t下的状态量的推测值。推测值矢量x^的次数k是推测值的数量。

F是状态迁移矩阵。状态迁移矩阵F是k次的正方矩阵。状态迁移矩阵F是时间Δt的函数。状态迁移矩阵F表示如下映射，其中，该映射是在规定的运动模型中将表示某个时刻下的移动体的位置等的矢量映射到表示经过了时间Δt之后的移动体的位置等的矢量的映射。

即，该式意味着：使表示时间(t₁-t₂)的经过的状态迁移矩阵F(t₁-t₂)作用于时刻t₂下的推测值矢量x^(t₂)，计算出时刻t₁下的推测值矢量x^(t₁)。

另外，状态迁移矩阵F的运动模型例如是假设为移动体以某个角速度进行运动的转弯运动模型等。

角速度检验部253根据所计算出的时刻t₁下的状态量的推测值，依照移动体以角速度校正部252校正了的时刻t₁下的角速度进行转弯运动这样的运动模型，预测从时刻t₁经过了规定的时间ΔT的时刻t₃(即，t₃＝t₁+ΔT)下的状态量。

例如，角速度检验部253使用处理装置911，计算下式的右边，由此计算时刻t₃下的状态量的预测值。

【式26】

${\hat{x}}_J\left(t_3\right)=F_J\left(\mathrm{\ΔT}\right){\hat{x}}_J\left(t_1\right)$

其中，x^_J是用角速度校正部252校正了的时刻t₁下的角速度置换状态推测部240推测出的角速度而得到的推测值矢量。F_J是基于移动体以角速度校正部252校正了的时刻t₁下的角速度进行转弯运动这样的运动模型的状态迁移矩阵。

即，该式意味着：使基于移动体以角速度校正部252校正了的时刻t₁下的角速度进行转弯运动这样的运动模型的表示时间ΔT的经过的状态迁移矩阵F_J作用于时刻t₁下的推测值矢量x^_J(t₁)，计算出时刻t₃下的推测值矢量x^_J(t₃)。

另外，角速度检验部253根据状态推测部240计算出的误差方差、误差协方差，推测所预测的状态量的误差方差、误差协方差。

为了与状态量的推测值同样地，使误差方差等时刻与校正后的角速度的时刻一致，角速度检验部253例如根据时刻t₂下的误差方差等，计算时刻t₁下的误差方差等。

例如，角速度检验部253使用处理装置911，计算下式的右边，由此计算时刻t₁下的误差方差等。

【式27】

P(t₁)＝F(t₁-t₂)^TP(t₂)F(t₁-t₂)+Q(t₁-t₂)

其中，P是推测值矢量x^的误差的方差协方差矩阵。方差协方差矩阵P是k次的正方矩阵。方差协方差矩阵P是时刻t的函数。方差协方差矩阵P的要素是时刻t下的推测值矢量的各要素的误差之间的方差或者协方差。

上标T表示矩阵的转置。

Q是***噪声的方差协方差矩阵。方差协方差矩阵Q是k次的正方矩阵。方差协方差矩阵Q是时间Δt的函数。方差协方差矩阵Q的要素是在经过时间Δt的期间产生的***噪声的方差或者协方差。

即，该式意味着：使表示时间(t₁-t₂)的经过的状态迁移矩阵F(t₁-t₂)的转置矩阵从左作用于时刻t₂下的推测误差的方差协方差矩阵P(t₂)，使表示时间(t₁-t₂)的经过的状态迁移矩阵F(t₁-t₂)从右作用于时刻t₂下的推测误差的方差协方差矩阵P(t₂)，并对由此得到的结果，相加由于时间(t₁-t₂)的经过而产生的***噪声的方差协方差矩阵Q(t₁-t₂)，计算出时刻t₁下的推测误差的方差协方差矩阵P(t₁)。

角速度检验部253根据所计算出的时刻t₁下的误差方差等，依照移动体以角速度校正部252校正了的时刻t₁下的角速度进行转弯运动这样的运动模型，计算时刻t₃下的误差方差等。

例如，角速度检验部253使用处理装置911，计算下式的右边，由此计算时刻t₃下的误差方差等。

【式28】

P_J(t₃)＝F_J(ΔT)^TP(t₁)F_J(ΔT)+Q(ΔT)

其中，P_J是基于移动体以角速度校正部252校正了的时刻t₁下的角速度进行转弯运动这样的运动模型的预测的误差的方差协方差矩阵。

该式意味着：使基于移动体以角速度校正部252校正了的时刻t₁下的角速度进行转弯运动这样的运动模型的表示时间ΔT的经过的状态迁移矩阵F_J(ΔT)的转置矩阵从左作用于时刻t₁下的推测误差的方差协方差矩阵P(t₁)，使基于移动体以角速度校正部252校正了的时刻t₁下的角速度进行转弯运动这样的运动模型的表示时间ΔT的经过的状态迁移矩阵F_J(ΔT)从右作用于时刻t₁下的推测误差的方差协方差矩阵P(t₁)，并对由此得到的结果，相加由于时间ΔT的经过而产生的***噪声的方差协方差矩阵Q(ΔT)，计算出关于时刻t₃的预测误差的方差协方差矩阵P_J(t₃)。

角速度检验部253例如如上所述，预测时刻t₃下的状态量、误差方差等。

另外，角速度检验部253也可以并非根据角速度校正部252校正了的角速度，而是根据由角速度观测值取得部213存储了的移动体角速度数据表示的角速度观测装置831观测到的校正前的角速度，预测状态量、误差方差等。

另外，角速度检验部253不置换角速度，而原样地使用状态推测部240推测出的移动体的角速度，预测状态量、误差方差等。

角速度检验部253例如使用处理装置911，计算下式的右边，由此计算时刻t₃下的状态量、误差方差等。

【式29】

${\hat{x}}_T\left(t_3\right)=F_T(t_3-t_2){\hat{x}}_T\left(t_2\right)$

P_T(t₃)＝F_T(t₃-t₂)^TP(t₂)F_T(t₃-t₂)+Q(t₃-t₂)

其中，x^_T是原样地使用了状态推测部240推测出的移动体的角速度的时刻t₂下的推测值矢量。F_T是基于移动体以状态推测部240推测出的时刻t₂下的角速度进行转弯运动这样的运动模型的状态迁移矩阵。P_T是基于移动体以状态推测部240推测出的时刻t₂下的角速度进行转弯运动这样的运动模型的预测的误差的方差协方差矩阵。

角速度检验部253例如如上所述，预测状态量、误差方差等。

角速度检验部253根据所预测出的2种状态量、误差方差等，判定角速度校正部252校正了的角速度是否正确。

角速度检验部253例如判定所预测出的2种状态量之间的差是否在预想的范围内，在超过了预想的范围的情况下，判定为角速度校正部252校正了的角速度不正确。

在该判定中，可以使用统计学上的手法。角速度检验部253例如通过卡方检验，判定2种状态量之间的差是否在预想的范围内。例如，角速度检验部253通过计算下式的右边，计算检验值。

【式30】

ε＝[x_J(t₃)-x_T(t₃)]^T[P_J(t₃)+P_T(t₃)]^-1[x_J(t₃)-x_T(t₃)]

其中，ε是检验值。

角速度检验部253使用处理装置911，将所计算出的检验值ε与规定的阈值ε_th进行比较。阈值ε_th是根据规定的显著性水准(significance level)，例如从卡方分布表求出的。在检验值ε是阈值ε_th以下的情况下，角速度检验部253判定为2种状态量之间的差在预想的范围内。在检验值ε大于阈值ε_th的情况下，角速度检验部253判定为2种状态量之间的差超过了预想的范围。

另外，角速度检验部253也可以并非比较2种状态量，而是根据基于由角速度校正部252校正了的角速度而预测出的状态量，来预测观测值，并对所预测出的观测值与实际的观测值进行比较，由此判定角速度校正部252校正了的角速度是否正确。

以上，各实施方式中说明的结构是一个例子，也可以组合不同的实施方式中说明的结构。另外，也可以进行将并非主要的部分的结构与已有的技术等其他结构进行置换等的变形。

以上说明的角速度推测装置(200；偏航率偏移校正装置800)具有相对位置取得部(211；静止物体识别部110)、静止物体判定部(220；静止物体识别部110)、物体相关部(230；静止物体用追踪部120)、以及角速度推测部(状态推测部240；轨道推测部143；轨道综合部150)。

所述相对位置取得部(211；110)针对存在于移动体(801)周边的物体，取得周边物体观测装置(811；雷达810)观测到的观测结果，其中，该周边物体观测装置(811；雷达810)反复观测以所述移动体(801)为基准的所述物体的相对位置。

所述静止物体判定部(220；110)根据所述相对位置取得部(211；110)取得的观测结果，判定所述周边物体观测装置(811；810)观测到相对位置的物体是否静止。

所述物体相关部(230；120)根据所述相对位置取得部(211)取得的观测结果，从所述周边物体观测装置(811；810)观测到的多个相对位置中，判定针对同一物体由所述周边物体观测装置(811；810)观测到的多个相对位置。

所述角速度推测部(240；143；150)根据所述相对位置取得部(211)取得的观测结果、所述静止物体判定部(220；110)判定的判定结果、以及所述物体相关部(230；120)判定的判定结果，推测所述移动体(801)的角速度。

由此，即使不存在GPS等对移动体的位置进行观测的观测装置，也可以推测移动体的角速度。另外，通过使用所推测出的角速度，可以推测陀螺传感器等对移动体的角速度进行观测的装置的误差。

角速度推测装置(200；800)具有相对位置计算部(静止物体基准坐标变换部142)。

所述相对位置计算部(142)根据所述相对位置取得部(静止物体识别部110)取得的观测结果、所述静止物体判定部(110)判定的判定结果、以及所述物体相关部(静止物体用追踪部120)判定的判定结果，计算以静止的物体为基准的所述移动体(801)的相对位置。

所述角速度推测部(143；150)根据所述相对位置计算部(142)计算出的相对位置，推测所述移动体(801)的角速度。

由于根据与判定为静止的物体之间的相对位置关系，推测移动体自身的运动，从而推测移动体的角速度，所以即使不存在GPS等观测装置，也可以推测移动体的角速度。

角速度推测装置(200；800)具有角速度观测值取得部(213)、和角速度观测误差计算部(状态推测部240；偏移校正部840)。

所述角速度观测值取得部(213)取得由对所述移动体(801)的角速度进行观测的角速度观测装置(角速度观测装置831；830)观测到的观测结果。

所述角速度观测误差计算部(240；840)根据所述角速度推测部(240；143；150)推测出的推测结果、和所述角速度观测值取得部(213)取得的观测结果，计算所述角速度观测装置(831；830)观测到的角速度的误差。

通过计算角速度观测装置的误差，由此，即使在没有来自周边物体观测装置的输入的情况下，也可以校正角速度观测装置观测到的移动体的角速度。

角速度推测装置(200；800)具有速度观测值取得部(212；静止物体识别部110)。

所述速度观测值取得部(212；110)取得由对所述移动体(801)的移动速度进行观测的移动速度观测装置(821；车速传感器820)观测到的观测结果。

所述静止物体判定部(220；110)根据所述相对位置取得部(211；110)取得的观测结果、和所述速度观测值取得部(212；110)取得的观测结果，判定所述物体是否静止。

通过判定物体是否静止，可以以静止的物体为基准来掌握移动体的运动。

所述周边物体观测装置(811；810)观测以所述移动体(801)为基准的所述物体的相对速度。

所述静止物体判定部(220；110)根据所述移动速度观测装置(821；820)观测到的所述移动体(801)的移动速度、和所述周边物体观测装置(811；810)观测到的所述物体的相对速度，判定所述物体是否静止。

由于根据移动体的移动速度和物体相对移动体的相对速度来判定物体是否静止，所以不用使用移动体的转弯角度、物体的轨迹等，而可以判定物体是否静止。

角速度推测装置(200；800)具有相对速度推测部(静止物体判定部220)。

所述相对速度推测部(220)根据由所述物体相关部(230)判定的判定结果，针对所述周边物体观测装置(811)多次观测到的物体，推测所述物体相对所述移动体(801)的相对速度。

所述静止物体判定部(220)根据所述移动速度观测装置(821)观测到的所述移动体(801)的移动速度、和所述相对速度推测部(220)推测出的所述物体的相对速度，判定所述物体是否静止。

由于根据物体相关部的判定结果来计算物体相对移动体的相对速度，所以即使不存在对物体相对移动体的相对速度进行观测的观测装置，也可以判定物体是否静止。

角速度推测装置(200；800)可以通过计算机执行使计算机作为角速度推测装置而发挥功能的计算机程序来实现。

Claims (9)

1.一种角速度推测装置，其特征在于，具有：

相对位置取得部，取得周边物体观测装置观测到的观测结果，其中，所述周边物体观测装置反复观测存在于移动体的周边的物体，对以所述移动体为基准的所述物体的相对位置进行测定；

静止物体判定部，根据所述相对位置取得部所取得的观测结果，判定所述周边物体观测装置观测到的物体是否静止；

物体相关部，根据所述相对位置取得部所取得的多个观测结果，从所述周边物体观测装置所测定的多个相对位置中，判定所述周边物体观测装置观测同一物体而测定的相对位置的组；以及

角速度推测部，根据所述相对位置取得部所取得的观测结果、所述静止物体判定部所判定的判定结果、以及所述物体相关部所判定的判定结果，推测所述移动体的角速度。

2.根据权利要求1所述的角速度推测装置，其特征在于，

所述角速度推测装置还具有相对位置计算部，该相对位置计算部根据所述相对位置取得部所取得的多个观测结果、所述静止物体判定部所判定的判定结果、以及所述物体相关部所判定的判定结果，计算以静止的物体为基准的所述移动体的相对位置，

所述角速度推测部根据所述相对位置计算部计算出的相对位置，推测所述移动体的角速度。

3.根据权利要求1所述的角速度推测装置，其特征在于，

所述角速度推测装置还具有：

角速度观测值取得部，取得角速度观测装置观测到的观测结果，其中，所述角速度观测装置对所述移动体的角速度进行观测；以及

角速度观测误差计算部，根据所述角速度推测部推测出的推测结果和所述角速度观测值取得部取得的观测结果，计算所述角速度观测装置观测到的角速度的误差。

4.根据权利要求1所述的角速度推测装置，其特征在于，

所述角速度推测装置还具有速度观测值取得部，该速度观测值取得部取得移动速度观测装置观测到的观测结果，其中，所述移动速度观测装置对所述移动体的移动速度进行观测，

所述静止物体判定部根据所述相对位置取得部所取得的观测结果以及所述速度观测值取得部所取得的观测结果，判定所述物体是否静止。

5.根据权利要求4所述的角速度推测装置，其特征在于，

所述周边物体观测装置还测定以所述移动体为基准的所述物体的相对速度，

所述静止物体判定部根据所述移动速度观测装置观测到的所述移动体的移动速度以及所述周边物体观测装置所测定的所述物体的相对速度，判定所述物体是否静止。

6.根据权利要求4所述的角速度推测装置，其特征在于，

所述角速度推测装置还具有相对速度推测部，该相对速度推测部根据所述物体相关部所判定的判定结果，对于由所述周边物体观测装置多次观测到的物体，推测所述物体相对所述移动体的相对速度，

所述静止物体判定部根据所述移动速度观测装置观测到的所述移动体的移动速度以及所述相对速度推测部所推测的所述物体的相对速度，判定所述物体是否静止。

7.根据权利要求1所述的角速度推测装置，其特征在于，

通过由计算机执行计算机程序，来实现所述相对位置取得部、所述静止物体判定部、所述物体相关部以及所述角速度推测部。

8.一种角速度推测方法，推测移动体的角速度，其特征在于，

取得周边物体观测装置观测到的观测结果，其中，所述周边物体观测装置反复观测存在于所述移动体的周边的物体，对以所述移动体为基准的所述物体的相对位置进行测定，

根据所取得的观测结果，判定所述周边物体观测装置观测到的物体是否静止，

根据所取得的多个观测结果，从所述周边物体观测装置所测定的多个相对位置中，判定所述周边物体观测装置观测同一物体而测定的相对位置的组，

根据所取得的观测结果、与所述物体的静止性相关的判定结果、以及与所述相对位置的组相关的判定结果，推测所述移动体的角速度。

9.根据权利要求8所述的角速度推测方法，其特征在于，

由计算机执行计算机程序，其中，该计算机程序具有如下步骤：

取得周边物体观测装置观测到的观测结果的步骤，其中，所述周边物体观测装置反复观测存在于移动体的周边的物体，对以所述移动体为基准的所述物体的相对位置进行测定；

根据所取得的观测结果，判定所述周边物体观测装置观测到的物体是否静止的步骤；

根据所取得的多个观测结果，从所述周边物体观测装置所测定的多个相对位置中，判定所述周边物体观测装置观测同一物体而测定的相对位置的组的步骤；以及

根据所取得的观测结果、与所述物体的静止性相关的判定结果、以及与所述相对位置的组相关的判定结果，推测所述移动体的角速度的步骤。

Images