运动状态判定装置的制作方法

本发明涉及判定目标的运动状态的运动状态判定装置。

背景技术：

以往，已知一种从观测数据中分类目标的运动状态的方法的技术。这里，目标定义为车辆、船舶、飞机、机器人、人、自行车等可移动的物体。例如，在专利文献1中，公开了一种装置，从表示车辆位置的观测数据中提取车辆横向运动的朝向变化的点，如果变化点之间的距离在一定值以上，则判定该车辆的运动状态为蛇行。

现有技术文献

专利文献

专利文献1：日本专利特开2014－6576号公报

技术实现要素：

发明所要解决的技术问题

在目标的观测数据中包含误差的情况下，存在由于该观测误差而频繁发生运动状态判定错误的问题。在专利文献1所记载的现有技术中，由于根据包含观测误差在内的观测数据判定车辆的蛇行，因此在横向位置的变化点的距离小于一定值的情况下，即车辆的横向位置发生细微变化的情况下，视为该运动方向的变化是由于观测误差引起的，并判定该辆车没有蛇行。但是，通常，从进行观测的传感器到目标的距离越远，位置的观测误差变得越大。因此，在如专利文献1那样仅根据目标的位置来判定蛇行的技术中，越是远离传感器的目标，越容易产生将观测误差判定为蛇行的错误。另外，为了减少这样的判定错误，在观测误差大的前提下，越接近传感器的目标，越容易产生将目标的微小蛇行判定为观测误差的错误。

本发明是为了解决上述问题而完成的，其目的在于，即使在观测数据中包含观测误差的情况下，特别是在传感器与目标的距离发生变动而观测误差并非一定的环境中，也能减少与目标的运动状态相关的判定的错误频率。

解决技术问题所采用的技术方案

本发明所涉及的运动状态判定装置的特征在于，包括：传感器部，该传感器部获取目标的观测数据；路径数据存储部，该路径数据存储部存储表示所述目标能够移动的路径的形状的路径数据；路径分量预测部，该路径分量预测部分别计算所述目标的运动要素的预测值及预测误差的与所述路径平行的分量和垂直的分量；第一坐标变换部，该第一坐标变换部将由所述路径分量预测部计算出的运动要素的预测值及预测误差变换为与表示所述观测数据的第一坐标系相同的坐标系；滤波部，该滤波部使用由所述传感器部获取到的观测数据、在所述路径数据存储部中存储的路径数据、由所述第一坐标变换部变换后的运动要素的预测值及预测误差，来计算所述第一坐标系下的所述目标的运动要素的推定值及推定误差；第二坐标变换部，该第二坐标变换部将由所述滤波部计算出的所述目标的运动要素的推定值及推定误差变换为将与所述路径平行的分量和垂直的分量作为坐标轴的第二坐标系；以及状态判定部，该状态判定部使用由所述第二坐标变换部变换后的所述目标的运动要素的推定值及推定误差，来判定所述目标的运动状态。

发明效果

根据本发明，能够减少因观测误差引起的观测数据上的目标的位置变动而错误判定目标的运动状态的频率。

附图说明

图1是表示实施方式1所涉及的运动状态判定装置100的结构的框图。

图2是表示实施方式1中与对象车辆21相关的路径平行方向的位置和路径垂直方向的位置的示意图。

图3是表示实施方式1中路径坐标与观测坐标之间的坐标变换的示意图。

图4是表示实施方式1所涉及的运动状态判定装置100的硬件结构的框图。

图5是表示实施方式1所涉及的运动状态判定装置100的动作的流程图。

图6是表示对象车辆21的观测坐标中的运动要素向量的示例的示意图。

图7是表示对象车辆21的路径坐标中的运动要素向量的示例的示意图。

图8是表示实施方式2所涉及的运动状态判定装置100的结构的框图。

图9是表示实施方式2所涉及的运动状态判定装置100的动作的流程图。

具体实施方式

实施方式1.

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

图1是表示实施方式1所涉及的运动状态判定装置100的结构的框图。在该实施方式中，特别是将目标种类设为汽车，将目标的运动状态设为“车辆蛇行”“车辆没有蛇行”这两种。另外，将判定运动状态的对象的汽车设为在远离搭载有进行观测的传感器的汽车(本车辆)的位置上行驶的汽车。另外，判定运动状态的对象的汽车也可以是本车辆。以下，将判定运动状态的对象的目标记载为“对象车辆”，并对对象车辆的运动状态是“有蛇行”“没有蛇行”这两种情况进行说明。

如图1所示，运动状态判定装置100包括路径平行分量预测部10、路径垂直分量预测部11、路径数据存储部12、第一坐标变换部13、传感器部14、滤波部15、第二坐标变换部16、状态判定部17。这里，路径平行分量预测部10、路径垂直分量预测部11可以汇总作为路径分量预测部1来处理。运动状态判定装置100基于从传感器获取到的观测数据和路径数据存储部12中保存的路径数据，判定对象车辆21有无蛇行。

这里，观测数据定义为表示对象车辆21的运动要素(位置、速度以及它们的时间微分量)的信息。以下，说明观测数据是表示对象车辆21的位置的数据的情况。另外，路径数据定义为表示对象车辆21的典型移动路径的形状的信息。以下，说明路径数据是表示对象车辆21正在行驶的车道形状的数据的情况。特别地，在下文中，在将通过车道中央的线近似为点的集合的情况下，将各个中央点的位置数据称为路径数据。另外，目标除了汽车以外，还可以是船舶、飞机、机器人、人、自行车等。例如，在目标是船舶的情况下，观测数据可以设为表示船舶位置的通信信号(自动船舶识别装置的信号等)，路径数据可以设为典型的航线。另外，例如在目标是人的情况下，观测数据可以设为拍摄了人的视频，路径数据可以设为人行道的形状。

接着，定义“路径平行方向”和“路径垂直方向”作为运动状态判定装置100的说明中的术语。图2是表示实施方式1所涉及的运动状态判定装置100中的路径平行方向和路径垂直方向的一个示例的示意图。

首先，路径平行方向的轴定义为将作为路径数据的车道的中央点前后连接的线段的集合。并且，将从某个车道的中央点的原点到与对象车辆21的距离为最小的路径平行方向的轴上的点(路径坐标轴交点)的距离定义为与对象车辆21相关的路径平行方向的位置。另外，将该路径平行方向的位置及对该位置进行时间微分后得到的运动要素称为“运动要素的路径平行方向分量”。

接着，路径垂直方向的轴定义为与路径坐标轴交点的路径平行方向的轴垂直的直线。并且，将从路径坐标轴交点到路径垂直方向的轴与对象车辆21的位置相交的点的距离定义为与对象车辆21相关的路径垂直方向的位置。另外，将该路径垂直方向的位置及对该位置进行时间微分后得到的运动要素称为“运动要素的路径垂直方向分量”。

基于上述定义，使用图3说明运动状态判定装置100的动作。图3是表示实施方式1所涉及的运动状态判定装置100中的动作的一个示例的示意图。

图3左侧是表示在以搭载有传感器的本车辆20为基准的坐标系中对象车辆21进行蛇行的情况下，能得到伴随着蛇行的观测数据23的情况的示意图。在图3的左侧，将与本车辆20的行进方向相关的位置设为“纵向位置”，将与纵向垂直的方向的位置设为“横向位置”，观测数据23被表示为这些纵向、横向的位置。以下，将该观测数据23中的坐标系称为“观测坐标”。

另一方面，图3右侧是在路径平行方向和路径垂直方向上表示与左侧同样地进行蛇行的对象车辆21的推定位置的时间变化时的示意图。以下，将路径平行方向和路径垂直方向上表示运动要素的坐标系称为“路径坐标”。另外，如图3中央的箭头所示，由“第一坐标变换部13”进行从观测坐标到路径坐标的坐标变换，由“第二坐标变换部16”进行从路径坐标到观测坐标的变换。运动状态判定装置100在路径坐标上进行下一时刻帧中对象车辆21的运动要素的预测，将预测值变换到观测坐标上后，在观测坐标上基于观测数据23进行对象车辆21的运动要素的推定(滤波)。然后，再次通过坐标变化将推定值变换为路径坐标，基于路径坐标上的推定值判定对象车辆21有无蛇行。

通过这样的动作，例如即使在对象车辆21沿着如图3所示那样弯曲的路径进行蛇行的情况下，也能够在路径坐标上提取出与路径正交的方向的蛇行。另外，特别是，运动状态推定装置的特征在于，所推定出的运动要素的误差(推定误差)还与运动要素一起进行坐标变换，利用该推定误差来判定有无蛇行。

以下，表示图1中的各功能块之间的关系。另外，在下文中，将传感器输出观测数据的时间的划分记载为“时刻帧”。另外，将执行处理的时刻帧记载为“当前时刻”，将当前时刻的前一个时刻帧记载为“前一时刻”。

路径分量预测部1中的路径平行分量预测部10从第二坐标变换部16接受对象车辆21在前一时刻的运动要素的路径平行分量推定值和路径平行分量的推定误差，将对象车辆21在当前时刻的运动要素的路径平行分量预测值和路径平行分量的预测误差发送给第一坐标变换部13。

路径分量预测部1中的路径垂直分量预测部11从第二坐标变换部16接受对象车辆21在前一时刻的运动要素的路径垂直分量推定值和路径垂直分量的推定误差，将对象车辆21在当前时刻的运动要素的路径垂直分量预测值和路径垂直分量的预测误差发送给第一坐标变换部13。

路径数据存储部12在第一坐标变换部13及第二坐标变换部16执行路径数据时，将路径数据发送到第一坐标变换部13及第二坐标变换部16。另外，即使预先保存了路径数据，但在用于处理时也可以每次都通过通信方式等获取并保存。可以使用例如与定位卫星进行通信等现有技术作为获取路径数据的通信方式。

第一坐标变换部13从路径平行分量预测部10接受对象车辆21在当前时刻的运动要素的路径平行分量预测值和路径平行分量的预测误差，从路径垂直分量预测部11接受对象车辆21在当前时刻的运动要素的路径垂直分量预测值和路径垂直分量的预测误差，从路径数据存储部12接受路径数据。然后，将对象车辆21在当前时刻的观测坐标上运动要素的预测值和观测坐标上运动要素的预测误差发送给滤波部15。

传感器部14由用于观测对象车辆21的位置的设备构成。例如是单眼摄像头、立体摄像头、毫米波雷达、激光传感器、无线信号接收器等现有设备。传感器部14将对象车辆21在当前时刻的观测数据发送给滤波部15。另外，从观测设备获取到的图像或测距值等获取观测数据(对象车辆21的位置等)的方式使用例如从视频中提取车辆的特征形状的算法等每个观测设备的现有技术。

滤波部15从第一坐标变换部13接受对象车辆21在当前时刻帧中的观测坐标上运动要素的预测值和观测坐标上运动要素的预测误差，从传感器部14接受对象车辆21在当前时刻的观测数据。然后，将对象车辆21在当前时刻的观测坐标上运动要素的推定值和观测坐标上运动要素的推定误差发送给第二变换部。

第二坐标变换部16从滤波部15接受对象车辆21在当前时刻的观测坐标上运动要素的推定值和观测坐标上运动要素的推定误差，从路径数据存储部12接受路径数据。然后，向状态判定部17发送对象车辆21在当前时刻的路径坐标上运动要素的推定值和路径坐标上运动要素的推定误差。另外，在下一时刻帧中，向路径平行分量预测部10发送对象车辆21在前一时刻的运动要素的路径平行分量推定值和路径平行分量的推定误差，向路径垂直分量预测部11发送对象车辆21在前一时刻的运动要素的路径垂直分量推定值和路径垂直分量的推定误差。

状态判定部17从第二坐标变换部16接受对象车辆21在当前时刻的路径坐标上运动要素的推定值和路径坐标上运动要素的推定误差，输出对象车辆21在当前时刻有无蛇行以作为运动状态判定装置100的输出。

图4是表示实施方式1所涉及的硬件结构的一个示例的图。各硬件通过总线等有线电路或网络相互连接。运动状态判定装置100中的路径平行分量预测部10、路径垂直分量预测部11、路径数据存储部12、第一坐标变换部13、滤波部15、第二坐标变换部16、状态判定部17被存储在记录装置42中，路径平行分量预测部10、路径垂直分量预测部11、第一坐标变换部13、滤波部15、第二坐标变换部16、状态判定部17由运算装置41来执行。

另外，路径平行分量预测部10、路径垂直分量预测部11、第一坐标变换部13、滤波部15、第二坐标变换部16、状态判定部17在执行中向记录装置42进行输入输出值以及处理中间数据的写入、读出、删除。运算装置41是cpu(centralprocessingunit：中央处理单元)、ecu(electroniccontrolunit：电子控制单元)或gpu(graphicsprocessingunit：图像处理单元)等。记录装置42是hdd(harddisckdrive：硬盘驱动器)、ssd(solidstatedrive：固态硬盘)、dram(dynamicrandomaccessmemory：动态随机存取存储器)或闪存等。输出接口43是dvi(digitalvisualinterface：数字视频接口、注册商标)、hdmi(highdefinitionmultimdiainterface：高清多媒体接口、注册商标)、usb(universalserialbus：通用串行总线)、以太网(注册商标)、can(controllerareanetwork：控制器局域网)总线等。

接着，使用图5的流程图，说明实施方式1所涉及的运动状态判定装置100的动作的详细情况。图5是表示路径平行分量预测部10、路径垂直分量预测部11、第一坐标变换部13、传感器部14、滤波部15、第二坐标变换部16、状态判定部17在当前时刻的动作的流程图。

作为动作说明的准备，定义表示观测数据的位置向量、表示路径数据的位置向量、表示对象车辆21的运动要素的推定值的向量、表示运动要素的推定误差的矩阵。这里，观测数据如上所述，设为表示对象车辆21的位置的数据。以下，将表示对象车辆21在某个第k个时刻帧的位置的观测数据由式(1)定义的2行1列的位置向量zk表示。

这里，将zk设为观测坐标上的位置向量，将zk⁽¹⁾设为表示对象车辆21的横向位置的观测数据，将zk⁽²⁾设为表示对象车辆21的纵向位置的观测数据。另外，t上标表示矩阵的转置。

这里，路径数据如上所述，设为表示对象车辆21正在行驶的车道的中央点的位置的数据。以下，由式(2)定义的2行1列的位置向量si表示对象车辆21正在行驶的车道的第i个中央点的位置。

这里，将si设为观测坐标上的位置向量，si⁽¹⁾表示中央点的横向位置，si⁽²⁾表示中央点的纵向位置。将各中央点的连续标号即i设为1以上且i以下(i为2以上的整数)，路径数据设为由i个位置向量si表示。另外，作为各中央点的连续标号的i设为按纵向位置的升序排列。即对于任意的连续标号i，设si⁽²⁾＜si 1⁽²⁾始终成立。

另外，设与本车辆20到对象车辆21的距离相比，在足够远的远方获取作为路径数据的车道的中央点，在任一个中央点的纵向方向的中间观察到其他车辆。即，将以下情况设为前提，即：在各中央点的连续标号i为1以上且i以下的情况下(i为2以上的整数)，在任意的第k时刻帧下述关系始终成立。

接着，分别对观测坐标和路径坐标定义表示对象车辆21的运动要素的推定值的向量。首先，在时刻帧k下的观测坐标中的对象车辆21的运动要素推定值由以下4行1列的向量xk^(obs)表示。

这里，xk^(obs，1)表示对象车辆21的横向位置的推定值，xk^(obs，2)表示对象车辆21的纵向位置的推定值，xk^(obs，3)表示对象车辆21的横向速度的推定值，xk^(obs，4)表示对象车辆21的纵向速度的推定值。

接着，在时刻帧k下的路径坐标中的对象车辆21的运动要素推定值由表示路径平行分量的运动要素推定值的以下2行1列的向量xk^(para)和表示路径垂直分量的运动要素推定值的以下2行1列的向量xk^(vert)表示。分别定义如下。

这里，xk^(para，1)表示对象车辆21的路径平行方向的位置的推定值，xk^(para，2)表示对象车辆21的路径平行方向的速度的推定值，xk^(vert，1)表示对象车辆21的路径垂直方向的位置的推定值，xk^(vert，2)表示对象车辆21的路径垂直方向的速度的推定值。

对于如上所述定义的运动要素推定值的向量，其示意图在图6和图7中表示。在图6中表示观测坐标中的运动要素推定值xk^(obs)的位置分量，图7中表示路径坐标中的运动要素推定值xk^(para)、xk^(vert)的位置分量。其中，关于图7的路径坐标，为了简化图示，将路径数据si图示为路径平行方向的原点。

接着，对表示运动要素的推定误差的矩阵进行定义。推定误差以表示运动要素的向量的误差协方差矩阵的形式来表示。以下，将观测坐标中的运动要素推定值向量xk^(obs)的误差表示为pk^(obs)。另外，路径坐标中的运动要素推定值向量xk^(para)、xk^(vert)的误差分别表示为pk^(para)、pk^(vert)。这里，作为示例，将运动要素设为位置和速度这两种。然而，运动要素也可以不一定是位置和速度这两种，例如也可以设为使用增加了加速度、急动分量等3种以上的运动要素。

下面，说明图5的各步骤。在步骤st1中，路径平行分量预测部10预测当前时刻的路径平行分量的运动要素，计算该运动要素预测值的误差。在该步骤中，将前一时刻的运动要素推定值xk^(para)和推定误差pk^(para)作为输入，基于对象车辆21的路径平行位置匀速移动这一运动模型，通过以下公式计算运动要素预测值xk|k-1^(para)和预测误差pk|k-1^(para)。

这里，qk^(para)是表示运动模型的误差的2行2列的误差协方差矩阵，是预先设定的参数。另外，φk^(para)是基于运动模型使运动要素向量从前一时刻转移到当前时刻的转移矩阵，设为以下矩阵。

在这里，τk表示从前一时刻到当前时刻的经过时间。另外，在上述公式中假设匀速移动以作为运动模型，但是也可以设为其他运动模型，例如基于特定加速度的等加速度运动模型等。

在步骤st2中，路径垂直分量预测部11预测当前时刻的路径平行分量的运动要素，计算该运动要素预测值的误差。在该步骤中，将前一时刻的运动要素推定值xk^(vert)和推定误差pk^(vert)作为输入，基于对象车辆21的路径垂直位置匀速移动这一运动模型，通过以下公式计算运动要素预测值xk|k-1^(vert)和预测误差pk|k-1^(vert)。

这里，qk^(vert)是表示运动模型的误差的2行2列的误差协方差矩阵，是预先设定的参数。另外，φk^(vert)是基于运动模型使运动要素向量从前一时刻转移到当前时刻的转移矩阵，设为以下矩阵。

另外，在上述公式中假设匀速移动以作为运动模型，但是也可以设为其他运动模型，例如基于特定加速度的等加速度运动模型等。另外，步骤st1和步骤st2假设的运动模型和参数也可以不同。

在步骤st3中，第一坐标变换部13从路径坐标的运动要素预测值变换为观测坐标的运动要素预测值。另外，从路径坐标的运动要素预测误差变换为观测坐标的运动要素预测误差。在变换中，从路径数据存储部12中使用路径数据si。在该步骤中，将xk|k-1^(para)、pk|k-1^(para)、xk|k-1^(vert)、pk|k-1^(vert)、si设为输入，通过以下公式计算观测坐标的运动要素预测值xk|k-1^(obs)和观测坐标的运动要素预测误差pk|k-1^(obs)。

这里，旋转矩阵gj^(para)、gj^(vert)和平移向量gj定义如下。

上述式中，与构成路径数据的线段平行的单位向量ej^(para)、与ej^(para)垂直的单位向量ej^(vert)、从路径方向原点到sj的路径方向的长度λj计算如下。

针对向量的绝对值符号(||)定义为向量的欧几里德距离。另外，上述的路径数据的连续标号j定义为将对象车辆21前后夹住的中央点，且满足以下条件的1以上i以下的j。

在步骤st4中，传感器部14获取观测数据，滤波部15进行观测数据与对象车辆21的对应关联。不管传感器的种类如何，通常，在通过传感器观测对象车辆21的位置的情况下，除了表示对象车辆21的位置的观测数据以外，还可能发生获取来自对象车辆21以外的错误的观测数据的情况。例如，在传感器部14是基于诸如摄像头等可见光图像的车辆观测传感器的情况下，变成看板等类似于车辆的物体的位置错误的观测数据。另外，例如在传感器部14是基于雷达等电磁波的反射强度的车辆观测传感器的情况下，变成护栏等电波反射物体的位置错误的观测数据。

另外，不论传感器的种类如何，通常，在通过传感器观测对象车辆21的位置的情况下，可能发生在观测数据中不包含来自对象车辆21的观测数据的情况。例如，在传感器部14是摄像头的情况下，当从摄像头看到的对象车辆21被遮蔽物遮住时，不获取来自对象车辆21的观测数据。另外，例如在传感器部14是雷达的情况下，在对象车辆21的反射电波暂时被信号噪声埋没时，不获取来自对象车辆21的观测数据。

根据上述理由，在该步骤st4中，通过当前时刻获取到的观测数据和运动要素预测值的比较，进行是否存在来自对象车辆21的观测数据、以及若存在则是哪个观测数据的对应关联。在对应关联中，对于各个观测数据zk，计算表示观测数据和预测位置之间的剩余差的以下马氏距离δ。

这里，sk是与对象车辆21的预测位置和观测数据的剩余差相关的误差协方差矩阵，由

计算出。这里，r是观测数据zk的误差协方差矩阵，是预先设定的参数。此外，h是用于从观测坐标中的运动要素中提取位置向量的矩阵，定义为

由于表示根据式(22)计算出的剩余差δ越小，其观测数据越与对象车辆21的预测值一致，因此，将多个观测数据zk中δ为最小的观测数据与来自于对象车辆21相对应关联，并将该观测数据作为在步骤st5中处理的观测数据zk。另外，如上所述，也可能发生来自对象车辆21的观测数据一个也没有获取到的情况，因此，在剩余差δ小于一定值的观测数据一个也没有的情况下，或者在当前时刻获取到的观测数据为0个的情况下，将无效值代入步骤st5中处理的观测数据中。

在步骤st5中，滤波部15基于观测数据计算对象车辆21在当前时刻的运动要素推定值和推定误差。将此处使用的观测数据zk设为在步骤st4中与对象车辆21对应关联的观测数据。

对象车辆21在当前时刻的运动要素推定值xk^(obs)和运动要素推定值的误差协方差矩阵pk^(obs)通过以下公式来计算。

此处，h与式(24)设为相同。另外，kk定义如下。

此处，sk设为与式(23)相同，上标的“－1”表示逆矩阵。另外，在观测数据zk为无效值的情况下，认为在当前时刻下没有观测到对象车辆21，如下所述，将预测值作为推定值。

在步骤st6中，第二坐标变换部16从观测坐标的运动要素推定值变换为路径坐标的运动要素推定值。另外，从观测坐标的运动要素推定误差变换为路径坐标的运动要素推定值。在变换中，从路径数据存储部12中使用路径数据si。

在该步骤中，将xk^(obs)、pk^(obs)、si设为输入，通过以下公式计算路径坐标的运动要素推定值xk^(para)、xk^(vert)以及路径坐标的运动要素推定误差pk^(para)、pk^(vert)。

这里，旋转矩阵旋转矩阵gj^(para)、gj^(vert)的定义与步骤st3中的式(15)和(16)相同，并且平移向量gj的定义也与步骤st3中的式(17)相同。其中，路径数据的连续标号j的计算方法中，将j定义为：设为在路径平行方向的位置上将对象车辆21前后夹住的中央点，且满足以下条件的1以上i以下的j。

另外，λj的定义与步骤st3的式(20)相同。

在步骤st7中，状态判定部17基于路径坐标的运动要素推定值和推定误差，判定对象车辆21有无蛇行以作为对象车辆21的运动状态。每次判定，计算下面的马氏距离δk。

其中h是用于从路径垂直方向的运动要素中提取位置分量的矩阵，定义为

h＝[10](36)

表示了式(35)的δk越大，从路径的中心到对象车辆21推定位置的距离就越明显地远离位置的推定误差。因此δk的时间序列图如果以某个一定以上的振度振动，则判定为对象车辆21进行蛇行。在判定δk的振幅时，使用例如将在某个一定时刻帧之间的δk的最大值和最小值之差设为振幅、对δk进行短时间傅立叶变换并将频率分量的峰值设为振幅等的方法。

另外，在上述中，仅基于路径垂直方向的运动要素及其推定误差来判定有无蛇行，但也可以使用路径平行方向的运动要素及其推定误差。例如，可以根据路径平行方向的位置与该推定误差相比大多少、即对象车辆21与传感器远离多少，来使判定蛇行的δk的振幅阈值连续地增高等。

在如上所述构成的实施方式1所涉及的运动状态判定装置100中，获得以下效果。首先，在实施方式1所涉及的运动状态判定装置100中，依次计算运动要素的推定值的误差，基于该误差判定对象车辆21的运动状态。用于判定的运动要素推定值的误差中反映有滤波部15中假定的观测数据的误差。即，在状态判定部17中，基于使用运动要素的预测值而计算出的观测数据的误差，判定对象车辆21的运动状态。通过该结构，能够区分由观测数据的误差引起的运动要素的变动与真正的运动要素的变动。与专利文献1那样的仅基于对象车辆21的位置来判定运动状态的现有技术相比，该效果在下述点上具有显著的效果，即：在对象车辆21在距离传感器远的地方行驶而观测数据的误差大的情况下，能够减少由观测误差引起的观测数据上的对象车辆21的位置变动被误判定为对象车辆21的蛇行的情况的频率。

此外，在实施方式1所涉及的运动状态判定装置100中，对于路径垂直方向的分量和路径平行方向的分量分别计算对象车辆21的运动要素和运动要素的推定误差，并基于这些路径坐标上的运动要素推定值和推定误差值来判定对象车辆21的运动状态。通过该结构，即使在对象车辆21行驶的路径以任意形状弯曲的情况下，也能基于时间序列数据来进行状态判定，上述时间序列数据与垂直于路径的方向的运动要素推定值及其推定误差、平行于路径的方向的运动要素及其推定误差相关。在对象车辆21行驶的路径本身为蛇行的情况下，该效果突出地表现在，与基于例如以方位为基准的坐标系上的运动要素推定值和推定误差来判定对象车辆21的蛇行这一简单的方式相比，通过简单的方式无法区分沿着路径的蛇行和偏离路径的方向的蛇行，但在一个实施方式1所涉及的运动状态判定装置100中能够区分它们。

此外，在实施方式1所涉及的运动状态判定装置100中，分别预测平行于路径的方向的运动要素和垂直于路径的方向的运动要素。一般来说，汽车或二轮车等中，与平行于路径的方向的加速减速相比，在垂直于路径的方向上加速减速的频率少。因此，通过在平行于路径的方向和垂直于路径的方向上设定不同的运动模型、不同的运动模型的误差，从而能够实现更适于实际的对象车辆21运动的运动要素的预测值。例如，该效果突出地表现在，与在例如以方位为基准的坐标系上预测运动要素的简单的方式相比，能降低运动要素的预测误差，其结果是推定误差降低，通过与推定误差的比较能对运动要素的微小变动进行判定，最终能降低运动状态判定的错误频率。

由此，实施方式1所涉及的运动状态判定装置100的特征在于，包括：传感器部14，该传感器部14获取目标(例如对象车辆21)的观测数据；路径数据存储部12，该路径数据存储部12存储表示所述目标能够移动的路径的形状的路径数据；路径分量预测部1，该路径分量预测部1分别计算所述目标的运动要素的预测值及预测误差的与所述路径平行的分量和垂直的分量；第一坐标变换部13，该第一坐标变换部13将由所述路径分量预测部1计算出的运动要素的预测值及预测误差变换为与表示所述观测数据的第一坐标系相同的坐标系；滤波部15，该滤波部15使用由所述传感器部14获取到的观测数据、在所述路径数据存储部12中存储的路径数据、由所述第一坐标变换部13变换后的运动要素的预测值及预测误差，来计算所述目标的运动要素的推定值及推定误差；第二坐标变换部16，该第二坐标变换部16将由所述滤波部15计算出的所述目标的运动要素的推定值及推定误差变换为将与所述路径平行的分量和垂直的分量作为坐标轴的第二坐标系；以及状态判定部17，该状态判定部17使用由所述第二坐标变换部16变换后的所述目标的运动要素的推定值及推定误差，来判定所述目标的运动状态。根据本发明，能够减少因观测误差引起的观测数据上的目标位置变动而错误判定目标的运动状态的频率。尤其是，本发明中，即使在观测数据中存在误差的情况下也进行目标的运动要素的预测，因此能够使目标的运动要素的推定误差变小。其结果是相对于仅使用观测数据的现有技术，能够减少错误判定目标的运动状态的频率。另外，即使在目标行驶的路径以任意形状弯曲的情况下，也能基于时间序列数据来判定状态，上述时间序列数据与垂直于路径的方向的运动要素推定值及其推定误差、以及与平行于路径的方向的运动要素及其推定误差相关，因此，能够减小错误判定目标的运动状态的频率。

另外，在实施方式1所涉及的运动状态判定装置100中，其特征在于，路径分量预测部1在与所述路径平行的方向和垂直的方向上设定不同的运动模型，分别计算与所述路径平行的分量和垂直的分量。通过使用该结构，能够实现更适于实际的对象车辆21的运动的运动要素的预测值。

实施方式2.

在实施方式1中，基于一个运动模型预测路径平行方向上的运动要素，并基于另一个运动模型预测路径垂直方向的运动要素。然而，对象车辆21可以采取的运动是多种多样的，路径方向和垂直方向的运动并不限于能够分别通过一种运动模型来适当地表现。

例如，在路径上由于对象车辆21以外的车辆而混乱的情况下，为了调整车辆间距，对象车辆21频繁发生路径平行方向的加速减速，但是在路径上空闲的情况下，对象车辆21的路径平行方向的运动维持匀速的可能性高。另外，路径垂直方向的运动取决于对象车辆21的重量或路面环境等，例如在对象车辆21是大型车辆的情况下，与小型车辆相比难以发生路径垂直方向的加速减速。另外，本车辆20不总是能够获取以对象车辆21的运动为特征的拥挤状态、车辆重量、路面环境等。因此，在对象车辆21的属性或路面环境等多种多样且不能获取这些条件的情况下，如果分别假设一种运动模型来预测路径平行方向的运动要素和路径垂直方向的运动要素则预测误差变大，最终基于推定误差的运动状态的判定错误频繁发生。

因此，在实施方式2中，对于路径平行方向的运动要素和路径垂直方向的运动要素，分别计算运动模型不同的多个预测值。另外，对于各个预测值计算运动要素的推定值，基于其中适当的推定值来判定对象车辆21的运动状态。通过该结构，即使当对象车辆21的运动模型多样化到不能用一种来表示时，也能够减少错误地判定运动状态的频率。

图8是表示实施方式2所涉及的运动状态判定装置100的结构的框图。如图8所示，实施方式2所涉及的运动状态判定装置100由路径平行分量预测部10、路径垂直分量预测部11、路径数据存储部12、第一坐标变换部13、传感器部14、滤波部15、第二坐标变换部16、状态判定部17构成。其中，路径平行分量预测部10、路径垂直分量预测部11、第一坐标变换部13、滤波部15、第二坐标变换部16分别并排有n个(n为2以上的整数)。这里，路径数据存储部12和传感器部14与实施方式1相同。另外，第一坐标变换部13和第二坐标变换部16并排有n个这一点与实施方式1不同，但各自的内部处理与实施方式1相同。

以下，表示图8中的各功能块之间的关系。另外，关于存在n个的路径平行分量预测部10、路径垂直分量预测部11、第一坐标变换部13、滤波部15、第二坐标变换部16，对第n个功能块进行说明(n为1以上且n以下的整数)。

第n个的路径平行分量预测部10从第n个第二坐标变换部16接受对象车辆21在前一时刻的第n个运动要素的路径平行分量推定值和路径平行分量的推定误差，将对象车辆21在当前时刻的第n个运动要素的路径平行分量预测值和路径平行分量的预测误差发送给第n个第一坐标变换部13。

第n个的路径垂直分量预测部11从第n个第二坐标变换部16接受对象车辆21在前一时刻的第n个运动要素的路径垂直分量推定值和路径垂直分量的推定误差，将对象车辆21在当前时刻的第n个运动要素的路径垂直分量预测值和路径垂直分量的预测误差发送给第n个第一坐标变换部13。

第n个第一坐标变换部13从第n个路径平行分量预测部10接受对象车辆21在当前时刻的第n个运动要素的路径平行分量预测值和路径平行分量的预测误差，从第n个路径垂直分量预测部11接受对象车辆21在当前时刻的第n个运动要素的路径垂直分量预测值和路径垂直分量的预测误差，从路径数据存储部12接受路径数据。然后，将对象车辆21在当前时刻的第n个观测坐标上运动要素的预测值和观测坐标上运动要素的预测误差发送给第n个滤波部15。

第n个滤波部15从第n个第一坐标变换部13接受对象车辆21在当前时刻帧的第n个观测坐标上运动要素的预测值和观测坐标上运动要素的预测误差，从传感器部14接受对象车辆21在当前时刻的观测数据。然后，将对象车辆21在当前时刻的第n个观测坐标上运动要素的推定值和观测坐标上运动要素的推定误差发送给第n个第二变换部。另外，将对象车辆21在当前时刻的第n个预测位置与观测数据的剩余差发送给状态判定部17。

第n个第二坐标变换部16从第n个滤波部15接受对象车辆21在当前时刻的第n个观测坐标上运动要素的推定值和观测坐标上运动要素的推定误差，从路径数据存储部12接受路径数据。然后，向状态判定部17发送对象车辆21在当前时刻的第n个路径坐标上运动要素的推定值和路径坐标上运动要素的推定误差。另外，在下一时刻帧中，向第n个路径平行分量预测部10发送对象车辆21在前一时刻的第n个运动要素的路径平行分量推定值和路径平行分量的推定误差，向第n个路径垂直分量预测部11发送对象车辆21在前一时刻的第n个运动要素的路径垂直分量推定值和路径垂直分量的推定误差。

状态判定部17从第1～n个第二坐标变换部16接受对象车辆21在当前时刻的第1～n个路径坐标上运动要素的推定值和路径坐标上运动要素的推定误差，并且，从第1～n个滤波部15接受对象车辆21在当前时刻的第1～n个预测位置与观测数据的剩余差。然后，将对象车辆21在当前时刻有无蛇行作为运动状态判定装置100的输出而输出。

实施方式2所涉及的硬件结构与实施方式1相同，在图4中表示。实施方式2所涉及的运动状态判定装置100中的第1～n个路径平行分量预测部10、第1～n个路径垂直分量预测部11、第1～n个第一坐标变换部13、第1～n个滤波部15、第1～n个第二坐标变换部16、状态判定部17存储在记录装置42中，第1～n个路径平行分量预测部10、第1～n个路径垂直分量预测部11、第1～n个第一坐标变换部13、第1～n个滤波部15、第1～n个第二坐标变换部16、状态判定部17由运算装置41来执行。另外，路径数据存储部12存储在记录装置42中。

另外，第1～n个路径平行分量预测部10、第1～n个路径垂直分量预测部11、第1～n个第一坐标变换部13、第1～n个滤波部15、第1～n个第二坐标变换部16、状态判定部17在执行中向记录装置42进行输入输出值以及处理中间数据的写入、读出、删除。

接着，使用图9的流程图，说明实施方式2所涉及的运动状态判定装置100的动作的详细情况。图9是表示第1～n个路径平行分量预测部10、第1～n个路径垂直分量预测部11、第1～n个第一坐标变换部13、传感器部14、第1～n个滤波部15、第1～n个第二坐标变换部16、状态判定部17的当前时刻的动作的流程图。另外，在以下的说明中的符号以及标记中，没有说明的意味着与实施方式1相同。

作为动作说明的准备，定义由第n个路径平行分量预测部10、第n个路径垂直分量预测部11、第n个第一坐标变换部13、第n个滤波部15、第n个第二坐标变换部16计算出的与对象车辆21的第n个运动要素有关的符号(n是1以上n以下的整数)。

首先，对象车辆21在时刻帧k的观测坐标中的第n个运动要素推定值由以下向量xk，n^(obs)表示。

这里，xk，n^(obs，1)表示对象车辆21的横向位置的推定值，xk，n^(obs，2)表示对象车辆21的纵向位置的推定值，xk，n^(obs，3)表示对象车辆21的横向速度的推定值，xk，n^(obs，4)表示对象车辆21的纵向速度的推定值。

另外，对象车辆21在时刻帧k的路径坐标中的第n个运动要素推定值由表示路径平行分量的运动要素推定值的以下2行1列的向量xk，n^(para)和表示路径垂直分量的运动要素推定值的以下2行1列的向量xk，n^(vert)表示。分别定义如下。

这里，xk，n^(para，1)表示对象车辆21的路径平行方向的位置的推定值，xk，n^(para，2)表示对象车辆21的路径平行方向的速度的推定值，xk，n^(vert，1)表示对象车辆21的路径垂直方向的位置的推定值，xk，n^(vert，2)表示对象车辆21的路径垂直方向的速度的推定值的推定值。

接着，也对各运动要素的推定误差进行定义。观测坐标中的运动要素推定值向量xk，n^(obs)的误差表示为pk，n^(obs)。另外，路径坐标中的运动要素推定值向量xk，n^(para)、xk，n^(vert)的误差分别表示为pk，n^(para)、pk，n^(vert)。

下面，说明图9的各步骤。在步骤st8中，运动状态判定装置100在当前时刻的处理中选择未选择的1以上且n以下的整数标号。另外，选择未选择的标号中的哪一个标号的方法是任意的。在以后的步骤st9～13的说明中，说明这里选择了n这个标号的情况下的处理。

在步骤st9中，第n个路径平行分量预测部10预测当前时刻的第n个路径平行分量的运动要素，计算该运动要素预测值的误差。

在该步骤中，将前一时刻的第n个运动要素推定值xk，n^(para)和推定误差pk，n^(para)作为输入，基于对象车辆21的路径平行位置匀速移动这一第n个运动模型，通过以下公式计算第n个运动要素预测值xk|k-1，n^(para)和预测误差pk|k-1，n^(para)。

这里，qk，n^(para)是表示与路径平行方向有关的第n个运动模型的误差的2行2列的误差协方差矩阵，是预先设定的参数。另外，φk，n^(para)是基于与路径平行方向有关的第n个运动模型使运动要素向量从前一时刻转移到当前时刻的转移矩阵，例如设为以下矩阵。

另外，在上述式中，作为与路径平行分量有关的第n个运动模型，假设了匀速移动，但也可以设为其他运动模型。例如，可以假设等加速度运动，以代替式(40)等。

这里，将an^(para)设为表示与路径平行分量有关的加速度的参数。

在步骤st10中，第n个路径垂直分量预测部11预测当前时刻的第n个路径垂直分量的运动要素，计算该运动要素预测值的误差。在该步骤中，将前一时刻的第n个运动要素推定值xk，n^(vert)和推定误差pk，n^(vert)作为输入，基于对象车辆21的路径垂直位置匀速移动这一第n个运动模型，通过以下公式计算第n个运动要素预测值xk|k-1，n^(vert)和预测误差pk|k-1，n^(vert)。

这里，qk，n^(vert)是表示与路径垂直方向有关的第n个运动模型的误差的2行2列的误差协方差矩阵，是预先设定的参数。另外，φk，n^(vert)是基于与路径垂直方向有关的第n个运动模型使运动要素向量从前一时刻转移到当前时刻的转移矩阵，例如设为以下矩阵。

另外，在上述式中，作为与路径平行分量有关的第n个运动模型，假设了匀速移动，但也可以设为其他运动模型。例如，可以假设等加速度运动，以代替式(44)等。

这里，将an^(vert)设为表示与路径垂直分量有关的加速度的参数。另外，步骤st9和步骤st10假设的运动模型和参数也可以不同。

在步骤st11中，第n个第一坐标变换部13从路径坐标的第n个运动要素预测值变换为观测坐标的第n个运动要素预测值。另外，从路径坐标的第n个运动要素预测误差变换为观测坐标的第n个运动要素预测误差。在变换中，从路径数据存储部12中使用路径数据si。在该步骤中，将xk|k-1，n^(para)、pk|k-1，n^(para)、xk|k-1，n^(vert)、pk|k-1，n^(vert)、si设为输入，通过以下公式计算观测坐标的运动要素预测值xk|k-1，n^(obs)和观测坐标的运动要素预测误差pk|k-1，n^(obs)。

这里，旋转矩阵gj^(para)、gj^(vert)和平移向量gj的定义与实施方式1的步骤st3相同。另外，上述的路径数据的连续标号j定义为：设为将对象车辆21前后夹住的中央点，且满足以下条件的1以上i以下的j。

λj的定义与步骤st3的式(20)相同。

在步骤st12中，传感器部14获取观测数据，滤波部15进行观测数据与对象车辆21的对应关联。在对应关联中，对于各个观测数据zk，计算表示观测数据和预测位置之间的剩余差的以下的马氏距离δn。

这里，sk，n是与对象车辆21的第n个预测位置和观测数据的剩余差有关的误差协方差矩阵，由

计算出。

多个观测数据zk中δn为最小的观测数据与来自对象车辆21对应关联，并将该观测数据作为在紧接着之后的步骤st13中处理的观测数据zk。另外，在剩余差δn小于一定值的观测数据一个也没有的情况下，或者在当前时刻获取到的观测数据为0个的情况下，将无效值代入步骤st13中处理的观测数据中。另外，在实施方式2中这里计算出的剩余差δn也用于步骤st16的运动状态判定。

在步骤st13中，第n个滤波部15基于观测数据计算对象车辆21在当前时刻的第n个运动要素推定值和推定误差。将此处使用的观测数据zk设为在步骤st4中与对象车辆21对应关联的观测数据。对象车辆21在当前时刻的第n个运动要素推定值xk，n^(obs)和运动要素推定值的误差协方差矩阵pk，n^(obs)通过以下公式来计算。

这里，kk，n定义如下。

这里，sk，n与式(52)设为相同。

另外，在观测数据zk为无效值的情况下，视为在当前时刻下没有观测到对象车辆21，如下所述，将预测值作为推定值。

在步骤st14中，第n个第二坐标变换部16从观测坐标的第n个运动要素推定值变换为路径坐标的第n个运动要素推定值。另外，从观测坐标的第n个运动要素推定误差变换为路径坐标的第n个运动要素推定值。在变换中，从路径数据存储部12中使用路径数据si。

在该步骤中，将xk，n^(obs)、pk，n^(obs)、si设为输入，通过以下公式计算路径坐标的运动要素推定值xk，n^(para)、xk，n^(vert)以及路径坐标的运动要素推定误差pk，n^(para)、pk，n^(vert)。

这里，旋转矩阵旋转矩阵gj^(para)、gj^(vert)和平移向量gj的定义与实施方式1的步骤st6相同。另外，路径数据的连续标号j的计算方法中，定义为：设为在路径平行方向的位置将对象车辆21前后夹住的中央点，且满足以下条件的为1以上i以下的j。

λj的定义与步骤st3的式(20)相同。

在步骤st15中，运动状态判定装置100在当前时刻的处理中判定是否存在未选择的1以上且小于n的整数标号。如果存在未选择的标号，则转移到步骤st8，如果不存在，则转移到步骤st16。

在步骤st16中，状态判定部17基于n个路径坐标的运动要素推定值和推定误差、以及预测位置与观测数据的剩余差，判定对象车辆21有无蛇行以作为对象车辆21的运动状态。

首先，使用根据式(51)计算出的n个预测位置与观测数据之间的剩余差δn，求出以下标号n*。

从上述δn的定义可明确，δn越小，表示由第n个运动模型预测的位置越接近观测数据。因此，给出最小的δn的第n*个预测值能够视为是由n种运动模型中最适于对象车辆21的实际状态的运动模型计算出的预测值。

计算出n*后，求出以下的马氏距离δk,*。

该δk,*越大，则表示对象车辆21的位置越明显地远离路径的中心。因此，δk，*的时间序列图如果以某个一定以上的振幅振动，则判定为对象车辆21进行蛇行。在判定δk，*的振幅时，使用例如将在某个一定时刻帧之间的δk，*的最大值和最小值之差设为振幅、对δk进行短时间傅立叶变换并将频率分量的峰值设为振幅等的方法。

另外，在上述中，仅基于路径垂直方向的运动要素及其推定误差来判定有无蛇行，但也可以使用路径平行方向的运动要素及其推定误差。例如，可以设为根据路径平行方向的位置与其推定误差相比大多少、即对象车辆21与传感器远离多少，来使判定蛇行的δk，*的振幅阈值连续地增高等。

另外，在上述中基于表示剩余差的δn的大小，计算出n*，但是也可以使用表示运动模型的有效性的指标、例如aic(akaikeinformationcriteria：赤池信息准则)、bic(bayesianinformationcriteria：贝叶斯信息准则)等指标来计算n*。

另外，在图9的流程图中，虽然设为按顺序执行n次步骤st9～14的处理的动作，但是取而代之地，也可以在多个线程中并行处理步骤st9～14，在n个路径平行分量预测部10、路径垂直分量预测部11、第一坐标变换部13、滤波部15、第二坐标变换部16的处理完成之后，执行步骤st16。

在如上所述构成的实施方式2所涉及的运动状态判定装置100中，获得以下效果。在实施方式2所涉及的运动状态判定装置100中构成为，通过n个路径平行分量预测部10和n个路径垂直分量预测部11计算预测中运动模型的前提或参数值不同的n种运动要素预测值，基于各自的预测值来推定运动要素，基于它们的预测值和预测误差、以及预测值与观测数据之间的剩余差来判定对象车辆21的运动状态。通过该结构，即使在对象车辆21能够进行各种运动的情况下，也能够基于适于实际的对象车辆21的运动的运动模型来预测运动要素，能够基于根据该预测值推定出的运动要素来判定运动状态。另外，通过n个路径平行分量预测部10计算n种运动要素预测值及预测误差也能够作为通过路径平行分量预测部10计算n组运动要素预测值的状态来处理。

该效果尤其在下述点上表现出显著的效果，例如在对象车辆21的属性、对象车辆21行驶的路面状态等多种多样，且不能获取这些信息的情况下，与从基于一种运动模型推定出的运动要素中判定出运动状态的情况相比，由于能使用预测误差和推定误差小的运动要素来判定运动状态，因此最终能够减少错误判定运动状态的频率。

由此，实施方式2所涉及的运动状态判定装置100中，所述路径分量预测部1对n组所述目标的运动要素的预测值及预测误差分别计算与所述路径平行的分量和垂直的分量，所述第一坐标变换部13将由所述路径分量预测部1计算出的n组运动要素的预测值及预测误差变换为所述第一坐标系，所述滤波部15对由所述第一坐标变换部13变换后的n组运动要素的预测值及预测误差各个组，计算所述目标的运动要素的推定值及推定误差，所述第二坐标变换部16将由所述滤波部15计算出的n组所述目标的运动要素的推定值及推定误差分别变换为所述第二坐标系，所述状态判定部17使用由所述第二坐标变换部16变换后的n组所述目标的运动要素的推定值及推定误差，来判定所述目标的运动状态。通过该结构，即使在目标能够进行各种运动的情况下，也能够基于适于实际的目标的运动的运动模型来预测运动要素，能够基于根据该预测值推定出的运动要素来判定运动状态。

另外，在实施方式2所涉及的运动状态判定装置100中，其特征在于，所述路径分量预测部1对n组的所述目标的运动要素的预测值和预测误差分别设定不同的运动模型，计算与所述路径平行的分量和垂直的分量。通过该结构，能够从各种运动模型中选择适于实际的目标运动的运动模型，并且能够最终减少错误判定运动状态的频率。

标号说明

1路径分量预测部

10路径平行分量预测部

11路径垂直分量预测部

12路径数据存储部

13第一坐标变换部

14传感器部

15滤波部

16第二坐标变换部

17状态判定部

20本车辆

21对象车辆

23观测数据

41运算装置

42记录装置

43输出接口

100运动状态判定装置。