CN103492903A - 状态推定装置 - Google Patents

Abstract

本发明的目的在于，提供一种能够高精度地推定观测对象的状态的状态推定装置。状态推定装置（1）通过进行将由LIDAR（2）得到的对象车辆的测定数据应用到状态推定用模型而运算的卡尔曼滤波器更新处理，来推定在本车辆的周边存在的车辆的状态。然后，状态推定装置（1）基于与对象车辆的位置关系、对象车辆的状态，来变更卡尔曼滤波器更新处理所使用的状态推定用模型。

Description

状态推定装置

技术领域

本发明涉及通过将测定数据应用到状态推定用模型来推定观测对象的状态的推定装置。

背景技术

以往，作为推定动态观测对象的状态的技术，公知有日本特开2002－259966号公报中记载的装置。日本特开2002－259966号公报中记载的装置具备多个识别单元，通过按照规定条件切换识别手法，来实现推定的高精度化。

专利文献1：日本特开2002－259966号公报

但是，即便是日本特开2002－259966号公报所记载的技术，也不能获得足够的推定精度，因此要求更高精度的推定手法。

鉴于此，近年来采用了使用卡尔曼滤波器（Kalman Filter）等滤波器的状态推定手法。在卡尔曼滤波器中，首先设定观测模型、观测噪声模型、运动模型以及运动噪声模型等状态推定用模型。然后，在卡尔曼滤波器中通过将观测对象的测定数据应用到该设定的状态推定用模型中，来高精度地推定动态观测对象的状态。

然而，在以往的使用了卡尔曼滤波器的状态推定手法中，由于与观测对象的状态时刻变化无关，状态推定用模型都固定，所以存在未必能够高精度地推定观测对象的状态这一问题。

发明内容

鉴于此，本发明的目的在于，提供一种能够更高精度地推定观测对象的状态的状态推定装置。

本发明涉及的状态推定装置是将由测定观测对象的测定装置测定出的测定数据应用到状态推定用模型，来推定观测对象的状态的状态推定装置，具有基于与观测对象的位置关系或者观测对象的状态来变更状态推定用模型的变更单元。

根据本发明涉及的状态推定装置，由于基于与观测对象的位置关系或者观测对象的状态来变更状态推定用模型，所以能够更高精度地推定动态观测对象的状态。

该情况下，优选观测对象是在测定装置的周边存在的车辆，变更单元基于观测对象的中心位置相对于测定装置的方向来变更状态推定用模型。如果观测对象的中心位置相对于测定装置的方向不同，则观测对象的可测定面不同。因此，如果与观测对象的中心位置相对于测定装置的方向无关地使用同一状态推定用模型，则无法恰当地进行测定数据与状态推定用模型的对应。其结果，不能高精度地推定观测对象的状态。鉴于此，通过基于观测对象的中心位置相对于测定装置的方向来变更状态推定用模型，能够使测定数据与状态推定用模型恰当地建立对应。由此，能够进一步提高观测对象的状态的推定精度。

另外，优选观测对象是在测定装置的周边存在的车辆，变更单元基于观测对象的朝向来变更状态推定用模型。如果观测对象的朝向不同，则观测对象的可测定面不同。因此，如果与观测对象的朝向无关地使用同一状态推定用模型，则无法恰当地进行测定数据与状态推定用模型的对应。其结果，不能高精度地推定观测对象的状态。鉴于此，由于通过基于观测对象的朝向来变更状态推定用模型，能够使测定数据与状态推定用模型恰当地建立对应，所以能够进一步提高观测对象的状态的推定精度。

另外，优选观测对象是在测定装置的周边存在的车辆，变更单元基于观测对象的中心位置相对于测定装置的方向、以及观测对象的朝向这双方来变更状态推定用模型。能够通过观测对象的中心位置相对于测定装置的方向、以及观测对象的朝向这双方，来确定观测对象与本车辆对置的面。因此，由于通过基于这双方的信息来变更状态推定用模型，能够使测定数据与状态推定用模型恰当地建立对应，所以能够进一步提高观测对象的状态的推定精度。

另外，优选变更单元基于在上次的推定中使用的状态推定用模型，来筛选应用测定数据的状态推定用模型。通常，由于观测对象的举动变化是连续的，所以通过基于在上次的推定中使用的状态推定用模型来筛选状态推定用模型，能够降低错误的状态推定用模型被选择的情况。

另外，优选变更单元基于上次推定出的观测对象的状态，来推定观测对象的中心位置相对于测定装置的方向、或者观测对象的朝向。这样，由于通过利用上次推定出的信息可确保推定的连续性，所以能够进一步提高观测对象的状态的推定精度。

另外，优选变更单元基于观测对象存在的位置的地图信息，来推定观测对象的朝向。在观测对象静止的情况下、刚检测出观测对象之后的情况下等，无法根据测定数据求出观测对象的朝向。鉴于此，通过利用观测对象存在的位置的地图信息，即使在这样的情况下，也能推定观测对象的朝向。

另外，优选变更单元根据测定数据生成观测对象的模型，基于构成模型的边的数量来变更状态推定用模型。这样，由于通过基于根据测定数据生成的模型的边的数量来变更状态推定用模型，可明确状态推定用模型的变更基准，所以能够进一步提高观测对象的状态的推定精度。

另外，优选状态推定用模型包含以方差值表示由于测定装置的测定而产生的观测噪声的观测噪声模型，变更单元基于相对于观测对象的面的朝向来变更观测噪声模型的方差值。通常，在与观测对象的面垂直的方向测定数据的观测噪声小，在与观测对象的面水平的方向测定数据的观测噪声大。鉴于此，通过基于相对于测定对象的面的朝向来变更观测噪声模型的方差值，能够进一步提高观测对象的状态的推定精度。

另外，优选变更单元基于距观测对象的距离来变更观测噪声模型。由于若距观测对象的距离近，则观测对象的测定对象区域大，所以观测噪声变小。另一方面，由于若距观测对象的距离远，则观测对象的测定对象区域小，所以观测噪声变大。鉴于此，通过根据距测定对象的距离来变更观测噪声模型，能够进一步提高观测对象的状态的推定精度。

另外，优选观测对象是在测定装置的周边存在的车辆，状态推定用模型包含表示周边车辆的运动状态的运动模型、和表示运动模型中的转向角度的变化量的运动噪声模型，若观测对象的速度高，则与观测对象的速度低时相比，变更单元使运动噪声模型中的转向角度的变化量减小。通常，若车辆的速度快，则大幅转动转向的可能性低。鉴于此，通过当观测对象的速度高时减小运动噪声模型中的转向角度的变化量，能够进一步提高观测对象的状态的推定精度。

另外，优选使用多个不同的状态推定用模型来推定观测对象的状态，并且计算出观测对象的状态的推定方差值，输出推定方差值最小的观测对象的状态。由此，即便在与观测对象的位置关系、观测对象的状态不明的情况下，也能输出使用恰当的状态推定用模型推定出的观测对象的状态。

根据本发明，能够高精度地推定观测对象的状态。

附图说明

图1是表示本实施方式涉及的状态推定装置的框图。

图2是表示想要推定的变量的图。

图3是表示第1实施方式涉及的状态推定装置的推定处理的图。

图4是表示重心位置的方位角与重心位置的速度朝向的图。

图5是表示观测模型的变更基准例的图。

图6是用于对右斜后面观测模型进行说明的图。

图7是用于对后面观测模型进行说明的图。

图8是表示第2实施方式涉及的状态推定装置的推定处理的图。

图9是表示第3实施方式涉及的状态推定装置的推定处理的图。

图10是表示第4实施方式涉及的状态推定装置的推定处理的图。

图11是表示第5实施方式涉及的状态推定装置的推定处理的图。

图12是表示图11的模型选择处理的图。

图13是表示第6实施方式涉及的状态推定装置的推定处理的图。

图14是表示对象车辆与分组点群数据之间的关系的图。

图15是表示观测噪声模型的概念的图。

图16是表示第7实施方式涉及的状态推定装置的推定处理的图。

图17是表示第8实施方式涉及的状态推定装置的推定处理的图。

图18是表示第9实施方式涉及的状态推定装置的推定处理的图。

具体实施方式

以下，参照附图对本发明涉及的状态推定装置的优选实施方式详细地进行说明。其中，在所有的图中，对相同或者相当的部分赋予相同附图标记。

图1是表示本实施方式涉及的状态推定装置的框图。本实施方式涉及的状态推定装置1被搭载于车辆，与LIDAR（Light Detection andRanging）2电连接。

LIDAR2是利用激光来测定其他车辆的雷达，作为测定装置发挥功能。LIDAR2通过射出激光并且接收该射出的激光的反射光，来检测反射点的点列。而且，LIDAR2根据激光的速度、激光的射出时刻以及反射光的受光时刻，来计算该检测出的点列的测定数据。测定数据中例如包括与本车辆的相对距离、相对于本车辆的相对方向、根据与本车辆的相对距离和相对于本车辆的相对方向计算出的坐标等。而且，LIDAR2将检测出的点列的测定数据发送给状态推定装置1。

状态推定装置1通过使用了卡尔曼滤波器的推定处理，来推定在本车辆的周边存在的其他车辆的状态。

若具体说明，则状态推定装置1首先将在本车辆的周边存在的其他车辆作为观测对象即对象车辆，设定该对象车辆的状态作为想要推定的变量。图2是表示想要推定的变量的图。如图2所示，想要推定的变量例如为中心位置（x）、中心位置（y）、速度（v）、朝向（θ）、轮胎角（ξ）、轴距（b）、长度（l）、宽度（w）。

而且，状态推定装置1通过将从LIDAR2发送来的测定数据应用到规定的状态推定用模型进行运算，来推定上述的各变量，输出该推定出的变量作为对象车辆的状态推定值。其中，在本实施方式中，将如此推定变量的处理称为“卡尔曼滤波器更新处理”。

并且，状态推定装置1基于与对象车辆的位置关系、对象车辆的状态，来变更卡尔曼滤波器更新处理所使用的状态推定用模型。因此，状态推定装置1还作为变更状态推定用模型的变更单元发挥功能。其中，卡尔曼滤波器更新处理所使用的状态推定用模型如后述那样，由观测模型、观测噪声模型、运动模型、运动噪声模型表示。

这里，对卡尔曼滤波器的概念简单进行说明。其中，由于卡尔曼滤波器本身是公知的技术，所以省略详细说明。

卡尔曼滤波器在只有观测量（观测向量）z_k被观测时，推定观测对象的状态（状态向量）x_k。因此，x_k成为想要通过推定求出的变量。其中，在本实施方式中，由LIDAR2测定的测定数据与观测量对应。

时刻k的观测量z_k由以下（1）式所示的观测模型表示。

[数1]

z_k＝Hx_k+υ_k…(1)

这里，v_k是表示进入到观测模型的观测噪声的观测噪声模型。观测噪声例如是因LIDAR2的特性而产生的误差、LIDAR2的读取误差等由于观测而产生的误差。该观测噪声模型v_k按照平均0方差R的正态分布，由下述的（2）式或者（3）式表示。

[数2]

$p_{{\mathrm{\υ}}_k}\left(\mathrm{\υ}\right)~\exp \{-{\mathrm{\υ}}^T{R}^{-1}\mathrm{\υ}\}\·\·\·\left(2\right)$

E(υ_k υ_k ^T)=R…(3)

时刻k的状态x_k由下述（4）式所示的运动模型表示。

[数3]

x_k＝Ax_k-1+Bu_k-1+ω_k-1…(4)

这里，u_k是操作量。另外，w_k是表示进入到运动模型的运动噪声的运动噪声模型。运动噪声是因进行与运动模型所设想的运动状态不同的运动状态而产生的误差。例如，在是进行等速直线运动的运动模型的情况下，存在因进行加减速而在观测对象的速度中产生的误差、因转向被转动而在观测对象的速度方向产生的误差等。该运动噪声模型w_k按照平均0方差Q的正态分布，由下述（5）式或者（6）式表示。

[数4]

$p_{{\mathrm{\ω}}_k}\left(\mathrm{\ω}\right)=\exp \{-{\mathrm{\ω}}^T{Q}^{-1}\mathrm{\ω}\}\·\·\·\left(5\right)$

E(ω_k ω_k ^T)=Q…(6)

而且，在卡尔曼滤波器中，假定概率p（xk｜z1，···，zk）为高斯分布，逐次运算下一时刻的概率p（xk＋1｜z1，···，zk＋1）。于是，状态x_k的分布由下述（7）式以及（8）式表示。

[数5]

${{\hat{x}}_k}^{-}=A{\hat{x}}_{k-1}+{\mathrm{Bu}}_{k-1}\·\·\·\left(7\right)$

${P_k}^{-}={\mathrm{AP}}_{k-1}{A}^T+Q\·\·\·\left(8\right)$

平均值

方差值

而且，以观测量z_k更新了的状态x_k的分布由下述（9）式以及（10）表示。

[数6]

${\hat{x}}_k={(H^T{R}^{-1}H+{\left({P_k}^{-}\right)}^{-1})}^{-1}(H^T{R}^{-1}{z}_k+{\left({P_k}^{-}\right)}^{-1}{{\hat{x}}_k}^{-})\·\·\·\left(9\right)$

$P_k={(H^T{R}^{-1}H+{\left({P_k}^{-}\right)}^{-1})}^{-1}\·\·\·\left(10\right)$

以下，针对第1实施方式～第9实施方式涉及的状态推定装置详细进行说明。其中，各实施方式涉及的状态推定装置与实施方式的编号对应被赋予11～19的附图标记。

［第1实施方式］

对第1实施方式涉及的状态推定装置11的推定处理进行说明。图3是表示第1实施方式涉及的状态推定装置的推定处理的图。

如图3所示，第1实施方式涉及的状态推定装置11基于对象车辆的中心位置相对于LIDAR2的方向和对象车辆的朝向，来变更卡尔曼滤波器更新处理所使用的观测模型。作为观测模型，有下述8个：以对象车辆的后面作为对象的后面观测模型、以对象车辆的后面以及左面作为对象的左斜后面观测模型、以对象车辆的左面作为对象的左面观测模型、以对象车辆的前面以及左面作为对象的左斜前面观测模型、以对象车辆的前面作为对象的前面观测模型、以对象车辆的前面以及右面作为对象的右斜前面观测模型、以对象车辆的右面作为对象的右面观测模型、和以对象车辆的后面以及右面作为对象的右斜后面观测模型。鉴于此，状态推定装置11从这8个观测模型中选择恰当的观测模型。

首先，状态推定装置11根据由LIDAR2发送来的点列的测定数据来生成分组点群数据（S1）。若详细说明，则状态推定装置11在LIDAR2检测出反射点的点列后，生成对位于规定距离内的点列进行了分组的分组点群数据。由于该分组点群数据对应于各车辆而生成，所以当在本车辆的周边存在多个车辆时，生成多个分组点群数据。

接下来，状态推定装置11求出在S1中生成的分组点群数据的重心位置（S2）。分组点群数据的重心位置与对象车辆的中心位置对应。因此，分组点群数据的重心位置例如可以通过根据分组点群数据生成车辆的模型，并计算出该模型的重心位置来求出。

然后，状态推定装置11计算出从LIDAR2观察到的在S2中求出的重心位置的方位角（S3）。即，状态推定装置11在S3中计算出对象车辆的重心位置相对于LIDAR2的方向。

另一方面，状态推定装置11在过去多次中跟踪通过S2求出的重心位置，来推定在S2中求出的重心位置的速度（S4）。然后，状态推定装置11基于S4的跟踪以及速度的推定，来计算出在S2中求出的重心位置的速度朝向（S5）。即，状态推定装置11在S5中计算出对象车辆的速度朝向。

接下来，状态推定装置11根据在S3中计算出的重心位置的方位角与在S5中计算出的重心位置的速度朝向的差值，来选择观测模型（S6）。

这里，参照图4以及图5对S6的处理详细进行说明。图4是表示重心位置的方位角与重心位置的速度朝向的图。图5是表示观测模型的变更基准例的图。其中，在图4中，O（X0，Y0）表示LIDAR2的原点，C（x，y）表示在S2中求出的重心位置。另外，θ表示在S5中计算出的重心位置C的速度朝向，Ψ表示重心位置C相对于原点O的方向、即在S3中计算出的方向。

如图4所示，状态推定装置11首先计算出从在S5中计算出的速度朝向θ减去了在S3中计算出的方向Ψ而得到的角度

该角度

由

＝θ－Ψ表示，为0～2π（360°）的范围。然后，如图5所示，状态推定装置11基于该计算出的角度

来选择观测模型。

在角度

为20°以下的情况下，由于从LIDAR2仅能看到对象车辆的后面，所以状态推定装置11选择后面观测模型。

在角度大于20°且为70°以下的情况下，由于从LIDAR2仅能看到对象车辆的后面以及左面，所以状态推定装置11选择左斜后面观测模型。

在角度大于70°且为110°以下的情况下，由于从LIDAR2仅能看到对象车辆的左面，所以状态推定装置11选择左面观测模型。

在角度

大于110°且为160°以下的情况下，由于从LIDAR2仅能看到对象车辆的前面以及左面，所以状态推定装置11选择左斜前面观测模型。

在角度大于160°且为200°以下的情况下，由于从LIDAR2仅能看到对象车辆的前面，所以状态推定装置11选择前面观测模型。

在角度

大于200°且为250°以下的情况下，由于从LIDAR2仅能看到对象车辆的前面以及右面，所以状态推定装置11选择右斜前面观测模型。

在角度

大于250°且为290°以下的情况下，由于从LIDAR2仅能看到对象车辆的右面，所以状态推定装置11选择右面观测模型。

在角度大于290°且为340°以下的情况下，由于从LIDAR2仅能看到对象车辆的后面以及右面，所以状态推定装置11选择右斜后面观测模型。

在角度大于340°的情况下，由于从LIDAR2仅能看到对象车辆的后面，所以状态推定装置11选择后面观测模型。

这里，参照图6以及图7对观测模型的一个例子详细进行说明。图6是用于对右斜后面观测模型进行说明的图。图7是用于对后面观测模型进行说明的图。

考虑如图6所示，从LIDAR2仅能看到对象车辆的后面以及右面的情况。该情况下，如果将直线应用到在S1中生成的分组点群数据，则该分组点群数据被分成由配置在右侧的点列构成的右分组、和由配置在左侧的点列构成的左分组。其中，由于分组点群数据由反射点的点列构成，所以被应用到分组点群数据的直线对应于对象车辆的前面、后面、右面、左面。

如上所述，想要推定的变量为中心位置（x）、中心位置（y）、速度（v）、朝向（θ）、轮胎角（ξ）、轴距（b）、长度（l）、宽度（w）（参照图2）。因此，右斜后面观测模型中的变量为：

右分组的中心位置（X_R）

右分组的中心位置（Y_R）

右分组中的长轴的长度（L_R）

右分组中的长轴的方位（θ_R）

左分组的中心位置（X_L）

左分组的中心位置（Y_L）

左分组中的长轴的长度（L_L）

左分组中的长轴的方位（Θ_L）。

另外，右斜后面观测观测模型成为：

XR＝x－l／2×cos（θ）

Y_R＝y－l／2×sin（θ）

L_R＝w

Θ_R＝mod（θ＋π／2，π）

X_L＝x＋w／2×sin（θ）

Y_L＝y－w／2×cos（θ）

L_L＝l

Θ_L＝mod（θ，π）。

考虑如图7所示，从LIDAR2仅能看到对象车辆的后面的情况。该情况下，如果将直线应用到在S1中生成的分组点群数据，则被分组为一个。

如上所述，想要推定的变量为中心位置（x）、中心位置（y）、速度（v）、朝向（θ）、轮胎角（ξ）、轴距（b）、长度（l）、宽度（w）（参照图2）。因此，右斜后面观测观测模型中的变量成为：

分组的中心位置（X）

分组的中心位置（Y）

分组中的长轴的长度（L）

分组中的长轴的方位（Θ）。

而且，右斜后面观测观测模型成为：

X＝x－l／2×cos（θ）

Y＝y－l／2×sin（θ）

L＝w

Θ＝mod（θ＋π／2，π）。

然后，状态推定装置11将在S6中选择出的观测模型决定为这次的推定所使用的观测模型（S7）。

接下来，状态推定装置11使用在S1中生成的分组点群数据和在S7中决定了的观测模型，进行卡尔曼滤波器更新处理（S8）。此时，状态推定装置11推定中心位置（x）、中心位置（y）、速度（v）、朝向（θ）、轮胎角（ξ）、轴距（b）、长度（l）、宽度（w）的变量，并且计算推定出的各变量的方差（以下称为“推定方差值”）。推定方差值与上述的用（9）式表示的方差值P_k对应。然后，状态推定装置11输出通过S8的卡尔曼滤波器更新处理计算出的变量作为对象车辆的状态推定值（S9）。

这样，根据本实施方式涉及的状态推定装置11，由于基于与对象车辆的位置关系、对象车辆的状态来变更状态推定用模型，所以能够更高精度地推定动态对象车辆的状态。

而且，由于通过基于对象车辆的中心位置相对于LIDAR2的方向与对象车辆的朝向的差值来变更观测模型，能够恰当地将测定数据与观测模型建立对应，所以能够进一步提高对象车辆的状态的推定精度。

［第2实施方式］

接下来，对第2实施方式涉及的状态推定装置12的推定处理进行说明。第2实施方式基本上与第1实施方式相同，但与第1实施方式的不同之处在于观测模型的选择手法。因此，以下仅对与第1实施方式不同的部分进行说明，省略与第1实施方式同样的部分的说明。

图8是表示第2实施方式涉及的状态推定装置的推定处理的图。如图8所示，第2实施方式涉及的状态推定装置12基于在上次的推定处理中使用的观测模型，来筛选在这次的推定处理中使用的观测模型。

通常，车辆的举动变化是连续的。因此，即使与对象车辆的位置关系、对象车辆的状态伴随着时间的经过而发生变化，从LIDAR2看到的车辆的面也只会按照后面、左斜后面、左面、左斜前面、前面、右斜前面、右面、右斜后面的顺序，或者其相反的顺序变化。

鉴于此，状态推定装置12基于在上次的推定处理的S7中决定的观测模型，来筛选在这次的推定处理的S6中选择的观测模型（S11）。

若详细说明，则状态推定装置12首先确定在上次的推定处理的S7中决定的观测模型。并且，状态推定装置12以上述的顺序或者相反顺序确定与该观测模型具有邻接关系的2个观测模型。而且，状态推定装置12将在这次的推定处理的S6中选择的观测模型筛选为该确定出的3个观测模型。例如，当在上次的推定处理的S7中决定的观测模型是后面观测模型的情况下，以后面观测模型、右斜后面模型以及左斜后面模型这3个观测模型来筛选在这次的推定处理的S6中选择的观测模型。

而且，在S6中，当根据在S3中计算出的重心位置的方位角与在S5中计算出的重心位置的速度朝向的差值选择的观测模型是在S11中筛选后的观测模型时，状态推定装置12与第1实施方式同样地继续处理。

另一方面，在S6中，当根据在S3中计算出的重心位置的方位角与在S5中计算出的重心位置的速度朝向的差值选择的观测模型不是在S11中筛选后的观测模型时，状态推定装置12判定为这次的观测模型的选择存在错误的可能性高。然后，状态推定装置12将在S6中选择的观测模型变更成在上次的推定处理的S7中决定的观测模型，或者将通过这次的推定处理而输出的观测对象的状态推定值处理成欠缺可靠性的值。

这样，根据第2实施方式涉及的状态推定装置12，由于基于在上次的推定处理中使用的观测模型来筛选这次的推定处理所使用的观测模型，所以能够降低错误的观测模型被选择的情况。

［第3实施方式］

接下来，对第3实施方式涉及的状态推定装置13的推定处理进行说明。第3实施方式基本上与第1实施方式相同，但是与第1实施方式的不同之处在于观测模型的选择手法。因此，以下仅对与第1实施方式的不同部分进行说明，省略与第1实施方式同样的部分的说明。

图9是表示第3实施方式涉及的状态推定装置的推定处理的图。如上所述，在第1实施方式中，基于在S1中生成的分组点群数据求出了对象车辆的中心位置相对于LIDAR2的方向、以及对象车辆的朝向。与此相对，如图9所示，在第3实施方式中，基于在上次的推定处理中输出的对象车辆的状态推定值来求出对象车辆的中心位置相对于LIDAR2的方向、以及对象车辆的朝向。

若详细说明，则状态推定装置13从在上次的推定处理的S9中输出的对象车辆的状态推定值提取出对象车辆的位置（x，y），根据该提取出的对象车辆的位置来计算对象车辆的中心位置相对于LIDAR2的方向（S13）。另外，状态推定装置13从在上次的推定处理的S9中输出的对象车辆的状态推定值提取出对象车辆的速度朝向（θ）（S14）。

然后，状态推定装置13根据在S13中计算出的对象车辆的中心位置相对于LIDAR2的方向、与在S14中提取出的对象车辆的速度朝向的差值，来选择观测模型（S6）。

这样，根据第3实施方式涉及的状态推定装置13，由于通过利用在上次的推定处理中输出的对象车辆的状态推定值，来确保推定的连续性，所以能够进一步提高对象车辆的状态的推定精度。

［第4实施方式］

接下来，对第4实施方式涉及的状态推定装置14的推定处理进行说明。第4实施方式基本上与第1实施方式相同，但是与第1实施方式的不同之处在于观测模型的选择手法。因此，以下仅对与第1实施方式不同的部分进行说明，省略与第1实施方式同样的部分的说明。

图10是表示第4实施方式涉及的状态推定装置的推定处理的图。如上所述，在第1实施方式中，基于在S1中生成的分组点群数据求出了对象车辆的朝向。与此相对，如图10所示，在第4实施方式中，基于地图信息求出对象车辆的朝向。

若详细说明，则状态推定装置14首先取得地图信息（S16）。该地图信息例如可以是如导航系统等那样搭载于车辆的存储装置中存储的信息，也可以是通过路车间通信等而从车外取得的信息。

接下来，状态推定装置14通过将在S2中计算出的重心位置重叠到在S16中取得的地图信息，来确定地图信息中的对象车辆所存在的位置。然后，状态推定装置14计算该确定出的位置在地图上的道路的朝向，推定为该计算出的地图上的道路的朝向是对象车辆的速度朝向（S17）。

此外，在第4实施方式中，也可以在S2中不仅计算分组点群数据的重心位置，还根据分组点群数据推定对象车辆的位置，在S17中基于该推定出的对象车辆的位置，确定地图上的对象车辆所存在的位置。

这样，根据第4实施方式涉及的状态推定装置14，由于基于对象车辆存在的位置来推定对象车辆的朝向，所以即使例如在对象车辆静止的情况下、刚检测出对象车辆之后的情况下等，也能够推定对象车辆的朝向。

［第5实施方式］

接下来，对第5实施方式涉及的状态推定装置15的推定处理进行说明。第5实施方式基本上与第1实施方式相同，但是与第1实施方式的不同之处在于观测模型的选择手法。因此，以下仅对与第1实施方式不同的部分进行说明，省略与第1实施方式相同的部分的说明。

图11是表示第5实施方式涉及的状态推定装置的推定处理的图，图12是表示图11的模型选择处理的图。如上所述，在第1实施方式中，基于从分组点群数据计算出的对象车辆的中心位置相对于LIDAR2的方向、以及对象车辆的朝向，选择了观测模型。与此相对，如图11以及图12所示，在第5实施方式中，基于从分组点群数据计算出的边的数量，来选择观测模型。

若详细说明，则如图10所示，状态推定装置15在S1中生成分组点群数据后，进行接下来说明模型选择处理（S19）。

参照图11，对S19的模型选择处理详细进行说明。

状态推定装置15首先计算出在S1中生成的分组点群数据的凸包（S21）。在该凸包计算中，首先从分组点群数据中确定右端的点和左端的点。然后，从右端（或者左侧）的点开始朝向左侧（或者右侧）将分组点群数据的点依次连结，如果到达左端（或者右侧）的点，则结束点的连结。其中，由于分组点群数据由反射点的点列构成，所以在凸包计算中连结的线成为与对象车辆的侧面对应的1条或者2条直线。

接下来，状态推定装置15对在S21中计算出的凸包的边进行分割（S22）。如上所述，由于分组点群数据由反射点的点列构成，所以在S21的凸包计算中连结的线成为与对象车辆的侧面对应的1条或者2条直线。因此，通过在S21中对凸包的边进行分割，能够判断从LIDAR2能够看到对象车辆的几个面。

接下来，状态推定装置15判定边的数量是否为1（S23）。然后，如果状态推定装置15判定为边的数量为1（S23：是），则判定边的长度是否比规定的阈值短（S24），如果判定为边的数量不是1（S23：否），则判定左边的边是否比右边的长（S31）。其中，S24的阈值是用于区别车辆的前面以及后面和左面以及右面的值。因此，S24的阈值成为一般车辆的、前面以及后面的宽度与左面以及右面的长度之间的值。

进入到S24的状态推定装置15若判定为边的长度比规定的阈值短（S24：是），则判定对象车辆的速度朝向是否是相对于本车辆逐渐远离的方向（S25），如果判定为边的长度不比规定的阈值短（S24：否），则判定对象车辆的速度朝向从本车辆观察是否是右朝向（S28）。其中，对象车辆的速度朝向可通过各种手法来检测。例如，可以如第1实施方式那样通过跟踪分组点群数据的重心位置来求取，另外，也可以如第3实施方式那样根据在上次的推定处理中输出的状态推定值来求取。

进入到S25的状态推定装置15若判定为对象车辆的速度朝向是相对于本车辆逐渐远离的方向（S25：是），则选择后面观测模型（S26），若判定为对象车辆的速度朝向不是相对于本车辆逐渐远离的方向（S25：否），则选择前面模型（S27）。

进入到S28的状态推定装置15若判定为对象车辆的速度朝向从本车辆观察是右朝向（S28：是），则选择右面观测模型（S29），若判定为对象车辆的速度朝向从本车辆观察不是右朝向（S28：否），则选择左面观测模型。

进入到S31的状态推定装置15若在上述S31的判定中判定为左边比右边长（S31：是），则判定对象车辆的速度朝向是否是相对于本车辆逐渐远离的方向（S32），若判定为左边不比右边长（S31：否），则判定对象车辆的速度朝向是否是相对于本车辆逐渐远离的方向（S35）。

进入到S32的状态推定装置15若判定为对象车辆的速度朝向是相对于本车辆逐渐远离的方向（S32：是），则选择左斜后面观测模型（S33），若判定为对象车辆的速度朝向不是相对于本车辆逐渐远离的方向（S32：否），则选择右斜前面模型（S34）。

进入到S35的状态推定装置15若判定为对象车辆的速度朝向是相对于本车辆逐渐远离的方向（S35：是），则选择右斜后面观测模型（S36），若判定为对象车辆的速度朝向是相对于本车辆逐渐远离的方向（S35：否），则选择左斜后面观测模型（S37）。

此外，在S35中，也可以判定对象车辆的速度朝向从本车辆观察是否是右朝向。该情况下，可以是状态推定装置15若判定为对象车辆的速度朝向从本车辆观察是右朝向，则选择右斜后面观测模型（S36），若判定为对象车辆的速度朝向从本车辆观察不是右朝向，则选择左斜后面观测模型（S37）。

若如此选择了观测模型，则如图10所示，状态推定装置15将在S19中选择出的观测模型决定为这次的推定所使用的观测模型（S7）。

这样，根据第5实施方式涉及的状态推定装置15，由于通过基于从分组点群数据求出的边的数量来变更观测模型，使得观测模型的选择基准明确化，所以能进一步提高对象车辆的状态的推定精度。

［第6实施方式］

接下来，对第6实施方式涉及的状态推定装置16的推定处理进行说明。第6实施方式基本上与第1实施方式相同，但是与第1实施方式的不同之处仅在于变更观测模型的观测噪声模型。因此，以下仅对与第1实施方式不同的部分进行说明，省略与第1实施方式相同的部分的说明。

图13是表示第6实施方式涉及的状态推定装置的推定处理的图。如上所述，在第1实施方式中，基于从分组点群数据计算出的对象车辆的中心位置相对于LIDAR2的方向、以及对象车辆的朝向，选择了观测模型。与此相对，如图13所示，在第6实施方式中，基于从分组点群数据计算出的边的方位角来变更观测噪声模型。

这里，对相对于对象车辆的面的朝向与观测误差之间的关系进行说明。

一般情况下，由于LIDAR2具有10cm左右的分辨率，所以点列p本身的测定误差小。另一方面，由于LIDAR2具有难以从端部检测出点列这一特性，所以由LIDAR2检测出的点列的中心成为从对象车辆的面的中心偏离的位置。因此，与对象车辆3的面垂直的方向的观测噪声小，但与对象车辆3的面水平的方向的观测噪声比与对象车辆3的面垂直的方向的观测噪声大。

图14是表示对象车辆与分组点群数据的关系的图，图15是表示观测噪声模型的概念的图。其中，图14的箭头表示对象车辆的行进方向。

如图14所示，从LIDAR2可看到对象车辆3的前面3_A以及左面3_B，考虑在对象车辆3的前面3_A以及左面3_B检测到从LIDAR2射出的激光的反射点的点列p的情况。

该情况下，从前面3_A的右部（在图14中为左上的部分）与左面3_B的后部（在图14中为右上的部分）没有检测出点列p。因此，在前面3_A中的点列p的中心P_A’与前面3_A的中心P_A相比偏移向前面3_A的左侧（在图14中为右下侧）。另外，在左面3_B中的点列p的中心P_B’与左面3_B的中心P_B相比偏移向左面3_B的前侧（在图14中为左下侧）。

如上所述，中心位置（x，y）是观测模型的变量。因此，如果基于由LIDAR2检测出的点列p计算前面3_A的中心位置，则与对象车辆3的前面3_A水平的方向的观测噪声比与前面3_A垂直的方向的观测噪声大。另外，如果基于由LIDAR2检测出的点列p计算左面3_B的中心位置，则与对象车辆3的左面3_B水平的方向的观测噪声比与左面3_B垂直的方向的观测噪声大。

鉴于此，如图15所示，虽然通常观测噪声模型中的中心位置的方差值R’用正圆表示，但在第6实施方式中，按照与对象车辆的面水平的方向的观测噪声比与对象车辆的面垂直的方向的观测噪声大的方式，变更观测噪声模型中的中心位置的方差值R。

若具体说明，则若将与对象车辆的面垂直的方向的误差设为σ_y，将与对象车辆的面水平的方向的误差设为σ_x，将旋转行列设为R_θ，则观测噪声模型中的中心位置的方差值R由下述（11）式表示。其中，将（11）式的计算方法记载于（12）式。

[数7]

$R=R_{\mathrm{\θ}}\left[\begin{array}{cc}{{\mathrm{\σ}}_x}^2& 0\\ 0& {{\mathrm{\σ}}_y}^2\end{array}\right]{R_{\mathrm{\θ}}}^T...\left(11\right)$

[数8]

$R=E\[\left(\begin{array}{c}x\\ y\end{array}\right)\left(\mathrm{xy}\right)\]$

$R=E\[R_{\mathrm{\θ}}\left(\begin{array}{c}X\\ Y\end{array}\right)\left(\mathrm{XY}\right){R_{\mathrm{\θ}}}^T\]$

$=R_{\mathrm{\θ}}E\[\left(\begin{array}{c}X\\ Y\end{array}\right)\left(\mathrm{XY}\right)\]{R_{\mathrm{\θ}}}^T$

$=R_{\mathrm{\θ}}{R}_0{R_{\mathrm{\θ}}}^T$

其中 $R_0=E\[\left(\begin{array}{c}X\\ Y\end{array}\right)\left(\mathrm{xy}\right)\]=\left[\begin{array}{cc}{{\mathrm{\σ}}_x}^2& 0\\ 0& {{\mathrm{\σ}}_y}^2\end{array}\right]\·\·\·\left(12\right)$

接下来，参照图13对状态推定装置16的处理进行说明。

状态推定装置16首先计算出在S1中生成的分组点群数据的凸包（S41），然后对该计算出的凸包的边进行分割（S42）。其中，S41的凸包计算与第5实施方式涉及的状态推定装置16所进行的S21的凸包计算（参照图12）同样。

接下来，状态推定装置16将在S42中分割的边应用到1条或者2条直线（S43），计算出该应用的直线的方位角（S44）。

然后，状态推定装置16如上述的（11）式所示那样，基于在S44中计算出的直线的方位角，变更观测噪声模型中的中心位置的方差值R（S45）。

然后，状态推定装置16将装入了在S45中方差值被变更后的观测噪声模型的观测模型决定为这次的推定所使用的观测模型（S46）。

这样，根据第6实施方式涉及的状态推定装置16，由于基于相对于对象车辆的面的朝向来变更观测噪声模型的方差值，所以能够进一步提高对象车辆的状态的推定精度。

［第7实施方式］

接下来，对第7实施方式涉及的状态推定装置17的推定处理进行说明。第7实施方式基本上与第1实施方式相同，但是与第1实施方式的不同之处仅在于变更观测模型的观测噪声模型。因此，以下仅对与第1实施方式不同的部分进行说明，省略与第1实施方式相同的部分的说明。

图16是表示第7实施方式涉及的状态推定装置的推定处理的图。如上所述，在第1实施方式中，基于从分组点群数据计算出的对象车辆的中心位置相对于LIDAR2的方向、以及对象车辆的朝向，选择了观测模型。与此相对，如图16所示，在第7实施方式中基于与对象车辆的距离来变更观测噪声模型。

状态推定装置17首先从在上次的推定处理的S9中输出的对象车辆的状态推定值中提取出对象车辆的位置。此时，状态推定装置17也可以取代在上次的推定处理的S9中输出的状态推定值，而如第1实施方式那样使用根据在这次的推定处理的S1中生成的分组点群数据计算出的重心位置。接下来，状态推定装置17根据该提取出的对象车辆的位置计算从本车辆到对象车辆的距离。然后，状态推定装置17基于该计算出的从本车辆到对象车辆的距离，来变更观测噪声模型中的观测噪声（S48）。

若详细说明，则如果从本车辆到对象车辆的距离近，则由于由LIDAR2得到的对象车辆的测定对象区域变大，所以观测噪声变小。另一方面，如果从本车辆到对象车辆的距离远，则由于由LIDAR2得到的对象车辆的测定对象区域变小，所以观测噪声变大。鉴于此，从本车辆到对象车辆的距离越长，则状态推定装置17使观测噪声模型中的观测噪声越大。此外，观测噪声模型中的观测噪声例如可以根据从本车辆到对象车辆的距离而连续变化，也可以根据从本车辆到对象车辆的距离而分成1个或者多个阶段变化。在后者的情况下，例如可以预先设定1个或者多个距离，每当从本车辆到对象车辆的距离超过该设定的距离时，便增大观测噪声模型中的观测噪声。另外，要变更的观测噪声可以使用对象车辆的面的中心位置、对象车辆的速度、对象车辆的朝向等各种噪声。

然后，状态推定装置17将装入了在S48中被变更后的观测噪声模型的观测模型决定为这次的推定所使用的观测模型（S49）。

这样，根据第7实施方式涉及的状态推定装置17，通过基于距对象车辆的距离来变更观测噪声模型中的观测噪声，能够进一步提高对象车辆的状态的推定精度。

［第8实施方式］

接下来，对第8实施方式涉及的状态推定装置18的推定处理进行说明。第8实施方式基本上与第1实施方式相同，但是与第1实施方式的不同之处仅在于变更运动噪声模型的方面。因此，以下仅对与第1实施方式不同的部分进行说明，省略与第1实施方式相同的部分的说明。

图17是表示第8实施方式涉及的状态推定装置的推定处理的图。如上所述，在第1实施方式中，基于对象车辆的中心位置相对于LIDAR2的方向、以及对象车辆的朝向，变更了观测模型。与此相对，如图17所示，在第8实施方式中基于对象车辆的速度来变更运动模型的运动噪声模型。

这里，对运动噪声模型详细进行说明。如上所述，想要推定的变量为中心位置（x）、中心位置（y）、速度（v）、朝向（θ）、轮胎角（ξ）、轴距（b）、长度（l）、宽度（w）（参照图2）。因此，运动模型由

x：＝x＋v×cos（θ）

y：＝y＋v×sin（θ）

v：＝v

θ：＝θ＋v／b×tan（ξ）

ξ：＝ξ

b：＝b

l：＝l

w：＝w表示。

而且，在运动模型是等速直线运动的情况下，装入这些运动模型的运动噪声模型例如成为

σ（x）＝0

σ（y）＝0

σ（v）＝加减速度

σ（θ）＝0

σ（ξ）＝转向变化量（转向角度的变化量）

σ（b）＝0

σ（l）＝0

σ（w）＝0。

这样，对装入运动模型的运动噪声模型设定转向变化量与加减速度，但以往对运动噪声模型设定了全都固定的值。然而，存在随着车辆的速度变高，大幅转动转向的可能性变低的趋势。

鉴于此，状态推定装置18首先从在上次的推定处理的S9中输出的对象车辆的状态推定值提取出对象车辆的速度。然后，状态推定装置18基于该提取出的对象车辆的速度来变更运动噪声模型中的转向变化量σ（ξ）（S51）。若具体说明，则对象车辆的速度越高，则状态推定装置18使运动噪声模型中的转向变化量σ（ξ）越小。其中，转向变化量σ（ξ）例如可以根据对象车辆的速度连续变化，也可以根据对象车辆的速度而分成1个或者多个阶段变化。在后者的情况下，例如可以预先设定1个或者多个速度，每当对象车辆的速度超过设定的速度时，便减小转向变化量σ（ξ）。

然后，状态推定装置18将装入了在S51中被变更后的运动噪声模型的运动模型决定为这次的推定所使用的运动模型（S52）。

这样，根据第8实施方式涉及的状态推定装置18，通过若对象车辆的速度高则减小运动噪声模型中的转向变化量σ（ξ），能够进一步提高对象车辆的状态的推定精度。

［第9实施方式］

接下来，对第9实施方式涉及的状态推定装置19的推定处理进行说明。在第1实施方式中，变更推定处理所使用的观测噪声模型，来推定对象车辆的状态。与此相对，在第9实施方式中，在多个不同的观测模型中推定对象车辆的状态，输出使用推定方差值最小的观测模型而推定出的观测对象的状态。

图18是表示第9实施方式涉及的状态推定装置的推定处理的图。如图18所示，状态推定装置19准备多个不同的观测模型（S54）。S54中准备的观测模型为后面观测模型、左斜后面观测模型、左面观测模型、左斜前面观测模型、前面观测模型、右斜前面观测模型、右面观测模型以及右斜后面观测模型这8个观测模型。此外，在以下的说明中，说明了S54中准备的观测模型为上述8个模型的情况，但只要准备至少2以上观测模型即可，准备的观测模型的个数没有特别限制。

接下来，状态推定装置19将在S1中生成的分组点群数据分别应用到在S54中准备的8个观测模型，并列地进行卡尔曼滤波器更新处理（S55）。S55的卡尔曼滤波器更新处理与第1实施方式中的S8的卡尔曼滤波器更新处理同样。

然后，状态推定装置19输出在S55的各卡尔曼滤波器更新处理中推定出的中心位置（x）、中心位置（y）、速度（v）、朝向（θ）、轮胎角（ξ）、轴距（b）、长度（l）、宽度（w）这各个变量（S56）。

另外，状态推定装置19计算出在S55的各卡尔曼滤波器更新处理中算出的各变量的推定方差值（S57）。

然后，状态推定装置19将在S56中输出的8个卡尔曼滤波器输出中推定方差值最小的卡尔曼滤波器输出作为最终输出（S59）。

这样，根据第9实施方式涉及的状态推定装置19，即使在与对象车辆的位置关系、对象车辆的状态不明的情况下，也能输出使用恰当的观测模型推定出的对象车辆的状态推定值。

以上，对本发明的优选实施方式进行了说明，但本发明并不限定于上述实施方式。

例如，在上述实施方式中，对采用卡尔曼滤波器作为对象车辆的状态的推定单元的情况进行了说明。然而，只要是将测定数据应用到模型来推定对象车辆的状态的装置即可，可以采用任意的单元、任意的滤波器。例如，也可以采用粒子滤波器（Particle Filter）。

另外，在上述实施方式中，采用了在本车辆的周边存在的周边车辆作为观测对象，但也可以将摩托车、自行车等所有的车辆作为观测对象。

另外，在第1实施方式中，对基于对象车辆的中心位置相对于LIDAR2的方向与对象车辆的朝向的差值变更观测模型的情况进行了说明，但也可以仅基于对象车辆的中心位置相对于LIDAR2的方向来变更观测模型，或者仅基于对象车辆的朝向来变更观测模型。

即使对象车辆的中心位置相对于LIDAR2的方向不同，对象车辆的可测定面也不同，另外，即使对象车辆的朝向不同，对象车辆的可测定面也不同。因此，通过仅基于对象车辆的中心位置相对于LIDAR2的方向、或者对象车辆的朝向中任意一方来变更观测模型，也能够将测定数据与观测模型恰当建立对应。由此，能够进一步提高对象车辆的状态的推定精度。

此外，上述的各实施方式能够适当地组合。例如，可以将第1实施方式与第6实施方式组合，来变更观测模型与观测噪声模型，也可以将第1实施方式与第8实施方式组合，来变更观测模型与运动模型。

产业上的可利用性

本发明能够作为对周边车辆的状态进行推定的状态推定装置来进行利用。

附图标记说明：1（11～19）…状态推定装置，2…LIDAR（测定装置），3…对象车辆。

Claims (12)

1.一种状态推定装置，将由测定观测对象的测定装置测定出的测定数据应用到状态推定用模型来推定上述观测对象的状态，其中，

具有基于与上述观测对象的位置关系或者上述观测对象的状态来变更上述状态推定用模型的变更单元。

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

上述观测对象是在上述测定装置的周边存在的车辆，

上述变更单元基于上述观测对象的中心位置相对于上述测定装置的方向来变更上述状态推定用模型。

3.根据权利要求1所述的状态推定装置，其中，

上述观测对象是在上述测定装置的周边存在的车辆，

上述变更单元基于上述观测对象的朝向来变更上述状态推定用模型。

4.根据权利要求1所述的状态推定装置，其中，

上述观测对象是在上述测定装置的周边存在的车辆，

上述变更单元基于上述观测对象的中心位置相对于上述测定装置的方向以及上述观测对象的朝向这双方，来变更上述状态推定用模型。

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

上述变更单元基于在上次的推定中使用的状态推定用模型，来筛选应用上述测定数据的状态推定用模型。

6.根据权利要求2至5中任意一项所述的状态推定装置，其中，

上述变更单元基于上次推定出的上述观测对象的状态，来推定上述观测对象的中心位置相对于上述测定装置的方向，或者推定上述观测对象的朝向。

7.根据权利要求3或4所述的状态推定装置，其中，

上述变更单元基于上述观测对象所存在的位置的地图信息来推定上述观测对象的朝向。

8.根据权利要求1至7中任意一项所述的状态推定装置，其中，

上述变更单元根据测定数据来生成上述观测对象的模型，基于构成上述模型的边的数量来变更上述状态推定用模型。

9.根据权利要求1至8中任意一项所述的状态推定装置，其中，

上述状态推定用模型包含以方差值表示由于上述测定装置的测定而产生的观测噪声的观测噪声模型，

上述变更单元基于相对于上述观测对象的面的朝向来变更上述观测噪声模型的方差值。

10.根据权利要求9所述的状态推定装置，其中，

上述变更单元基于距上述观测对象的距离来变更上述观测噪声模型。

11.根据权利要求1至10中任意一项所述的状态推定装置，其中，

上述观测对象是在上述测定装置的周边存在的车辆，

上述状态推定用模型包含运动模型和运动噪声模型，其中，上述运动模型表示上述周边车辆的运动状态，上述运动噪声模型表示上述运动模型中的转向角度的变化量，

在上述观测对象的速度高时，与上述观测对象的速度低时相比，上述变更单元减小上述运动噪声模型中的转向角度的变化量。

12.根据权利要求1至11中任意一项所述的状态推定装置，其中，

使用多个不同的上述状态推定用模型来推定上述观测对象的状态，并且计算出上述观测对象的状态的推定方差值，输出上述推定方差值最小的上述观测对象的状态。

Images