JP6847336B2 - 計測装置、計測方法及び計測プログラム - Google Patents

Description

この発明は、複数のセンサを用いて物体の検出項目の検出値を計算する技術に関する。

車両に搭載された複数のセンサを用いて、車両の周辺の物体の位置及び速度といった検出項目の検出値を特定し、車両の制御を行う技術がある。
この技術では、各センサによって検出された物体が同一の物体であるか否かを判定することがある。この判定では、各センサによって検出された物体についての各検出項目の値を要素とするベクトルが類似しているか否かを判定することにより、各センサによって検出された物体が同一の物体であるか否かが判定される。

特許文献１には、センサにより得られたデータから算出された位置と、地図データが示す位置との間の尤度をマハラノビス距離を用いて計算することが記載されている。

特開２０１１−００２３２４号公報

複数のセンサによって検出された物体が同一の物体であると判定された場合に、その物体の各検出項目の検出値を特定する必要がある。この際、各センサによって検出された物体についての各検出項目の値を要素とするベクトルから、尤もらしいベクトルを選択して、選択されたベクトルが示す各検出項目の値を検出値とすることが考えられる。ベクトルの尤もらしさが適切に計算されなければ、物体についての各検出項目の検出値を適切に特定することができない。
この発明は、物体についての検出項目の検出値を適切に特定可能にすることを目的とする。

この発明に係る計測装置は、
複数のセンサそれぞれを対象のセンサとして、前記対象のセンサによって物体が対象時刻に観測されて得られた前記物体の検出項目についての観測値に基づき、前記物体の前記検出項目についての前記対象時刻における検出値をカルマンフィルタを用いて計算する追尾部と、
前記複数のセンサそれぞれを対象のセンサとして、前記対象のセンサによって得られた前記観測値と、前記観測値に基づき前記検出値が前記追尾部によって計算された計算時に用いられた、前記対象時刻の前の時刻に予測された前記対象時刻における前記物体の前記検出項目の値である予測値との間のマハラノビス距離に加え、前記計算時に得られたカルマンゲインを用いて、前記対象のセンサにより得られた前記観測値に基づき計算された前記検出値の信頼度を計算する信頼度計算部と、
前記複数のセンサにより得られた前記観測値に基づき計算された前記検出値のうち、前記信頼度計算部によって計算された前記信頼度が高い前記検出値を選択する値選択部と
を備える。

この発明では、複数のセンサそれぞれに基づき計算された検出値のうち、マハラノビス距離とカルマンゲインとから計算された信頼度の高い検出値が選択される。これにより、直近の情報の信頼性の高さと、時系列の情報の信頼性の高さとの両方を考慮して、適切な検出値を選択することが可能である。

実施の形態１に係る計測装置１０の構成図。 実施の形態１に係る計測装置１０の動作を示すフローチャート。 実施の形態１に係る計測装置１０の動作の説明図。 変形例１に係る計測装置１０の構成図。 実施の形態２に係る計測装置１０の構成図。 実施の形態２に係る計測装置１０の動作を示すフローチャート。 実施の形態２に係るラップ率の説明図。 実施の形態２に係るラップ率の計算方法の説明図。 実施の形態２に係るＴＴＣの計算方法の説明図。 実施の形態３に係るカルマンゲインの具体例を示す図。 実施の形態３に係るマハラノビス距離の具体例を示す図。 実施の形態３に係る信頼度の具体例を示す図。 実施の形態３に係る検出値の具体例を示す図。

実施の形態１．
＊＊＊構成の説明＊＊＊
図１を参照して、実施の形態１に係る計測装置１０の構成を説明する。
計測装置１０は、移動体１００に搭載され、移動体１００の周辺の物体についての検出値を計算するコンピュータである。実施の形態１では、移動体１００は車両である。移動体１００は、車両に限らず、船舶といった他の種類であってもよい。
計測装置１０は、移動体１００又は図示された他の構成要素と、一体化した形態又は分離不可能な形態で実装されても、あるいは、取り外し可能な形態又は分離可能な形態で実装されてもよい。

計測装置１０は、プロセッサ１１と、メモリ１２と、ストレージ１３と、センサインタフェース１４とのハードウェアを備える。プロセッサ１１は、信号線を介して他のハードウェアと接続され、これら他のハードウェアを制御する。

プロセッサ１１は、プロセッシングを行うＩＣ（Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）である。プロセッサ１１は、具体例としては、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、ＤＳＰ（Ｄｉｇｉｔａｌ Ｓｉｇｎａｌ Ｐｒｏｃｅｓｓｏｒ）、ＧＰＵ（Ｇｒａｐｈｉｃｓ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）である。

メモリ１２は、データを一時的に記憶する記憶装置である。メモリ１２は、具体例としては、ＳＲＡＭ（Ｓｔａｔｉｃ Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＤＲＡＭ（Ｄｙｎａｍｉｃ Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）である。

ストレージ１３は、データを保管する記憶装置である。ストレージ１３は、具体例としては、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）である。また、ストレージ１３は、ＳＤ（登録商標，Ｓｅｃｕｒｅ Ｄｉｇｉｔａｌ）メモリカード、ＣＦ（ＣｏｍｐａｃｔＦｌａｓｈ，登録商標）、ＮＡＮＤフラッシュ、フレキシブルディスク、光ディスク、コンパクトディスク、ブルーレイ（登録商標）ディスク、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｋ）といった可搬記録媒体であってもよい。

センサインタフェース１４は、センサと接続するためのインタフェースである。センサインタフェース１４は、具体例としては、Ｅｔｈｅｒｎｅｔ（登録商標）、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）、ＨＤＭＩ（登録商標，Ｈｉｇｈ−Ｄｅｆｉｎｉｔｉｏｎ Ｍｕｌｔｉｍｅｄｉａ Ｉｎｔｅｒｆａｃｅ）のポートである。

実施の形態１では、計測装置１０は、センサインタフェース１４を介して、ＬｉＤＡＲ（Ｌａｓｅｒ Ｉｍａｇｉｎｇ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ）用のＥＣＵ３１（Ｅｌｅｃｔｒｏｎｉｃ Ｃｏｎｔｒｏｌ Ｕｎｉｔ）と、Ｒａｄａｒ用のＥＣＵ３２と、カメラ用のＥＣＵ３３と接続されている。
ＬｉＤＡＲ用のＥＣＵ３１は、移動体１００に搭載されたセンサであるＬｉＤＡＲ３４と接続されており、ＬｉＤＡＲ３４によって得られたセンサデータから物体の観測値４１を計算する装置である。Ｒａｄａｒ用のＥＣＵ３２は、移動体１００に搭載されたセンサであるＲａｄａｒ３５と接続されており、Ｒａｄａｒ３５によって得られたセンサデータから物体の観測値４２を計算する装置である。カメラ用のＥＣＵ３３は、移動体１００に搭載されたセンサであるカメラ３６と接続されており、カメラ３６によって得られた画像データから物体の観測値４３を計算する装置である。

計測装置１０は、機能構成要素として、追尾部２１と、融合部２２と、信頼度計算部２３と、値選択部２４とを備える。計測装置１０の各機能構成要素の機能はソフトウェアにより実現される。
ストレージ１３には、計測装置１０の各機能構成要素の機能を実現するプログラムが格納されている。このプログラムは、プロセッサ１１によりメモリ１２に読み込まれ、プロセッサ１１によって実行される。これにより、計測装置１０の各機能構成要素の機能が実現される。

図１では、プロセッサ１１は、１つだけ示されていた。しかし、プロセッサ１１は、複数であってもよく、複数のプロセッサ１１が、各機能を実現するプログラムを連携して実行してもよい。

＊＊＊動作の説明＊＊＊
図２及び図３を参照して、実施の形態１に係る計測装置１０の動作を説明する。
実施の形態１に係る計測装置１０の動作は、実施の形態１に係る計測方法に相当する。また、実施の形態１に係る計測装置１０の動作は、実施の形態１に係る計測プログラムの処理に相当する。

（図２のステップＳ１１：追尾処理）
追尾部２１は、複数のセンサそれぞれを対象のセンサとして、対象のセンサによって移動体１００の周辺に存在する物体が対象時刻に観測されて得られた物体の複数の検出項目それぞれについての観測値を取得する。そして、追尾部２１は、観測値に基づき、物体の複数の検出項目それぞれについての対象時刻における検出値をカルマンフィルタを用いて計算する。
実施の形態１では、センサは、ＬｉＤＡＲ３４と、Ｒａｄａｒ３５と、カメラ３６とである。センサは、これらのセンサに限らず、音波センサといった他のセンサであってもよい。実施の形態１では、検出項目は、水平方向の位置Ｘと、奥行方向の位置Ｙと、水平方向の速度Ｘｖと、奥行方向の速度Ｙｖとである。検出項目は、これらの項目に限らず、水平方向の加速度と、奥行方向の加速度といった他の項目であってもよい。

具体的には、追尾部２１は、ＬｉＤＡＲ用のＥＣＵ３１からＬｉＤＡＲ３４に基づく各検出項目の観測値４１を取得する。また、追尾部２１は、Ｒａｄａｒ用のＥＣＵ３２からＲａｄａｒ３５に基づく各検出項目の観測値４２を取得する。また、追尾部２１は、カメラ用のＥＣＵ３３からカメラ３６に基づく各検出項目の観測値４３を取得する。観測値４１，４２，４３は、それぞれ、水平方向の位置Ｘと、奥行方向の位置Ｙと、水平方向の速度Ｘｖと、奥行方向の速度Ｙｖとを示す。そして、追尾部２１は、ＬｉＤＡＲ３４とＲａｄａｒ３５とカメラ３６とのそれぞれを対象のセンサとして、対象のセンサに基づく観測値（観測値４１、観測値４２又は観測値４３）を入力として、各検出項目の検出値をカルマンフィルタを用いて計算する。

具体例としては、追尾部２１は、対象のセンサの対象の検出項目について、数１に示す物体の運動モデルと、数２に示す物体の観測モデルとに対して、カルマンフィルタを用いることにより、検出値を計算する。

ここで、Ｘ_{ｔ｜ｔ−１}は、時刻ｔ−１における時刻ｔの状態ベクトルである。Ｆ_{ｔ｜ｔ−１}は、時刻ｔ−１から時刻ｔにおける遷移行列である。Ｘ_{ｔ−１｜ｔ−１}は、時刻ｔ−１における物体の状態ベクトルの現在値である。Ｇ_{ｔ｜ｔ−１}は、時刻ｔ−１から時刻ｔにおける駆動行列である。Ｕ_ｔ−１は、時刻ｔ−１における平均が０であり、共分散行列Ｑ_ｔ−１の正規分布に従うシステム雑音ベクトルである。Ｚ_ｔは、時刻ｔにおけるセンサの観測値を示す観測ベクトルである。Ｈ_ｔは、時刻ｔにおける観測関数である。Ｖ_ｔは、時刻ｔにおける平均が０であり、共分散行列Ｒ_ｔの正規分布に従う観測雑音ベクトルである。

追尾部２１は、拡張カルマンフィルタを用いる場合には、対象のセンサの対象の検出項目について、数３から数４に示す予測処理と、数５から数１０に示す平滑処理とを実行することにより、検出値を計算する。

ここで、Ｘ＾_{ｔ｜ｔ−１}は、時刻ｔ−１における時刻ｔの予測ベクトルである。Ｘ＾_{ｔ−１｜ｔ−１}は、時刻ｔ−１における平滑ベクトルである。Ｐ_{ｔ｜ｔ−１}は、時刻ｔ−１における時刻ｔの予測誤差共分散行列である。Ｐ_{ｔ−１｜ｔ−１}は、時刻ｔ−１における平滑誤差共分散行列である。Ｓ_ｔは、時刻ｔにおける残差共分散行列である。θ_ｔは、時刻ｔにおけるマハラノビス距離である。Ｋ_ｔは、時刻ｔにおけるカルマンゲインである。Ｘ＾_ｔ｜ｔは、時刻ｔにおける平滑ベクトルであり、時刻ｔにおける各検出項目の検出値を示す。Ｐ_ｔ｜ｔは、時刻ｔにおける平滑誤差共分散行列である。Ｉは、単位行列である。なお、行列に上付きで示されているＴは転置行列であることを示し、−１は逆行列であることを示している。

追尾部２１は、マハラノビス距離θ_ｔと、カルマンゲインＫ_ｔと、時刻ｔにおける平滑ベクトルＸ＾_ｔ｜ｔといった、計算によって得られる各種データをメモリ１２に書き込む。

（図２のステップＳ１２：融合処理）
融合部２２は、各センサに基づく対象時刻における観測値間のマハラノビス距離を計算する。実施の形態１では、融合部２２は、ＬｉＤＡＲ３４に基づく観測値とＲａｄａｒ３５に基づく観測値との間のマハラノビス距離と、ＬｉＤＡＲ３４に基づく観測値とカメラ３６に基づく観測値との間のマハラノビス距離と、Ｒａｄａｒ３５に基づく観測値とカメラ３６に基づく観測値との間のマハラノビス距離とを計算する。マハラノビス距離の計算方法は、ステップＳ１１でのマハラノビス距離の計算方法と計算対象となるデータが異なるだけである。
融合部２２は、マハラノビス距離が閾値以下の場合に、２つのセンサによって得られた観測値が同一の物体を観測して得られた観測値であるとして、２つのセンサによって得られた観測値を同じグループに分類する。

なお、ＬｉＤＡＲ３４に基づく観測値とＲａｄａｒ３５に基づく観測値との間のマハラノビス距離と、ＬｉＤＡＲ３４に基づく観測値とカメラ３６に基づく観測値との間のマハラノビス距離とは閾値以下であり、Ｒａｄａｒ３５に基づく観測値とカメラ３６に基づく観測値との間のマハラノビス距離は閾値よりも長い場合が起こり得る。この場合には、ＬｉＤＡＲ３４に基づく観測値との関係から見ると、ＬｉＤＡＲ３４に基づく観測値とＲａｄａｒ３５に基づく観測値とカメラ３６に基づく観測値とは同一の物体を検出した観測値となる。しかし、Ｒａｄａｒ３５に基づく観測値との関係から見ると、Ｒａｄａｒ３５に基づく観測値とＬｉＤＡＲ３４に基づく観測値とは同一の物体を検出した観測値となるが、Ｒａｄａｒ３５に基づく観測値とカメラ３６に基づく観測値とは異なる物体を検出した観測値となる。
このような場合には、判定基準を事前に定めておき、融合部２２は判定基準に従いどのセンサに基づく観測値が同一の物体を検出した観測値となるかを判定すればよい。例えば、判定基準は、いずれか１つのセンサに基づく観測値との関係から見た場合に同一の物体を検出した観測値となる場合には、同一の物体を検出した観測値とするといったものである。また、判断基準は、全てのセンサに基づく観測値との関係から見た場合に同一の物体を検出した観測値となる場合にのみ、同一の物体を検出した観測値とするといったものでもよい。

（図２のステップＳ１３：信頼度計算処理）
信頼度計算部２３は、複数のセンサそれぞれを対象のセンサとし、複数の検出項目それぞれを対象の検出項目として、ステップＳ１１で対象のセンサによる観測値に基づき計算された対象の検出項目の検出値の信頼度を計算する。

具体的には、信頼度計算部２３は、ステップＳ１１で得られた対象のセンサによる対象の検出項目の観測値と、ステップＳ１１でこの観測値に基づき検出値が計算された計算時に用いられた、対象時刻の前の時刻に予測された対象時刻における物体の検出項目の値である予測値との間のマハラノビス距離を取得する。つまり、信頼度計算部２３は、Ｘ＾_ｔ｜ｔが計算された際に、ステップＳ１１で計算されたマハラノビス距離θ_ｔをメモリ１２から読み出し取得する。また、信頼度計算部２３は、ステップＳ１１で対象のセンサによる対象の検出項目の観測値に基づき検出値が計算された計算時に得られたカルマンゲインを取得する。つまり、信頼度計算部２３は、Ｘ＾_ｔ｜ｔが計算された際に、ステップＳ１１で計算されたカルマンゲインＫ_ｔをメモリ１２から読み出し取得する。
信頼度計算部２３は、マハラノビス距離θ_ｔと、カルマンゲインＫ_ｔとを用いて、対象のセンサによる観測値に基づき計算された対象の検出項目の検出値の信頼度を計算する。具体的には、信頼度計算部２３は、数１１に示すように、マハラノビス距離θ_ｔとカルマンゲインＫ_ｔとを乗じて、対象のセンサによる観測値に基づき計算された対象の検出項目の検出値の信頼度を計算する。

ここで、Ｍ_Ｘは、水平方向の位置Ｘについての信頼度であり、Ｍ_Ｙは、奥行方向の位置Ｙについての信頼度であり、Ｍ_Ｘｖは、水平方向の速度Ｘｖについての信頼度であり、Ｍ_Ｙｖは、奥行方向の速度Ｙｖについての信頼度である。Ｋ_Ｘは、水平方向の位置Ｘについてのカルマンゲインであり、Ｋ_Ｙは、奥行方向の位置Ｙについてのカルマンゲインであり、Ｋ_Ｘｖは、水平方向の速度Ｘｖについてのカルマンゲインであり、Ｋ_Ｙｖは、奥行方向の速度Ｙｖについてのカルマンゲインである。

なお、信頼度計算部２３は、マハラノビス距離θ_ｔとカルマンゲインＫ_ｔとの少なくとも一方に重み付けした上で、マハラノビス距離θ_ｔとカルマンゲインＫ_ｔとを乗じて信頼度を計算してもよい。

（ステップＳ１４：値選択処理）
値選択部２４は、ステップＳ１２で同一の物体を検出した観測値として設定された各観測値に基づき計算された複数の検出値のうち、ステップＳ１３で計算された信頼度が最も高い検出値を選択する。信頼度が高いとは、マハラノビス距離とカルマンゲインとを乗じて得られた値が小さいという意味である。

信頼度は、ステップＳ１４で同一の物体を検出した観測値として設定された各観測値に基づき計算された複数の検出値から、採用する検出値を選択する際に用いられるものである。そのため、ステップＳ１３では、信頼度計算部２３は、全てのセンサを対象のセンサとして信頼度を計算する必要はない。ステップＳ１３では、信頼度計算部２３は、ステップＳ１２で複数の観測値が１つのグループに分類されている場合に、そのグループに分類された各観測値の取得元であるセンサを対象のセンサとして信頼度を計算すればよい。

図３を参照して具体例を説明する。
ＬｉＤＡＲ３４によって得られたある観測値４１である観測値Ｘと、Ｒａｄａｒ３５によって得られたある観測値４２である観測値Ｙとの間のマハラノビス距離が閾値以下であったとする。そのため、ステップＳ１２で融合部２２は、観測値Ｘと観測値Ｙとを、同一の物体を検出して得られたものとして、１つのグループ５１に分類する。
観測値Ｘと観測値Ｙとが１つのグループ５１に分類されたため、ステップＳ１３で信頼度計算部２３は、観測値Ｘの取得元のセンサであるＬｉＤＡＲ３４を対象のセンサとして、各検出項目についての検出値Ｍの信頼度Ｍ’を計算する。同様に、信頼度計算部２３は、観測値Ｙの取得元のセンサであるＲａｄａｒ３５を対象のセンサとして、各検出項目についての検出値Ｎの信頼度Ｎ’を計算する。なお、図３では、マハラノビス距離とカルマンゲインとを乗じて得られた値を０以上１以下になるように正規化した上で、正規化した値を１から減算して、信頼度Ｍ’及び信頼度Ｎ’を計算している。そのため、図３では、値が大きいほど信頼度が高くなっている。
そして、ステップＳ１４で値選択部２４は、グループ５１が表す物体について、検出項目毎に、信頼度Ｍ’と信頼度Ｎ’とを比較して、検出値Ｍと検出値Ｎとのうち信頼度が高い方を選択する。つまり、図３に示す信頼度Ｍ’及び信頼度Ｎ’の場合には、値選択部２４は、水平方向の位置Ｘについては、検出値Ｎ“０．１４”を選択し、奥行方向の位置Ｙについては、検出値Ｍ“２０．０”を選択し、水平方向の速度Ｘｖについては、検出値Ｎ“−０．１２”を選択し、奥行方向の速度Ｙｖについては、検出値Ｍ“−４．５０”を選択する。

＊＊＊実施の形態１の効果＊＊＊
以上のように、実施の形態１に係る計測装置１０は、マハラノビス距離とカルマンゲインとを用いて、検出値の信頼度を計算する。
マハラノビス距離は、過去の予測値と現在の観測値との間の一致度合を示す。また、カルマンゲインは、時系列での予測の正しさを示す。そのため、マハラノビス距離とカルマンゲインとを用いて信頼度を計算することにより、過去の予測値と現在の観測値との間の一致度合と、時系列での予測の正しさとの両方を考慮した信頼度を計算することが可能である。つまり、リアルタイムの情報と、過去の時系列の情報との両方を考慮した信頼度を計算することが可能である。

また、実施の形態１に係る計測装置１０は、検出項目毎に、信頼度の高い検出値を選択する。つまり、実施の形態１に係る計測装置１０は、同一の物体を検出したセンサが複数ある場合に、全ての検出項目についてある１つのセンサに基づき得られた検出値を採用するのではなく、検出項目毎に、どのセンサに基づき得られた検出値を採用するかを決定する。
センサは、検出項目及び状況毎に、精度よく検出値が得られるか否かが変化する。そのため、あるセンサは、ある状況において、一部の検出項目については精度よく検出値が得られるが、他の検出項目については精度よく検出値が得られない場合がある。そのため、検出項目毎に、信頼度の高い検出値を選択することにより、全ての検出項目について精度のよい検出値を得ることが可能である。

＊＊＊他の構成＊＊＊
＜変形例１＞
実施の形態１では、各機能構成要素がソフトウェアで実現された。しかし、変形例１として、各機能構成要素はハードウェアで実現されてもよい。この変形例１について、実施の形態１と異なる点を説明する。

図４を参照して、変形例１に係る計測装置１０の構成を説明する。
各機能構成要素がハードウェアで実現される場合には、計測装置１０は、プロセッサ１１とメモリ１２とストレージ１３とに代えて、電子回路１５を備える。電子回路１５は、各機能構成要素と、メモリ１２と、ストレージ１３との機能とを実現する専用の回路である。

電子回路１５としては、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ロジックＩＣ、ＧＡ（Ｇａｔｅ Ａｒｒａｙ）、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）、ＦＰＧＡ（Ｆｉｅｌｄ−Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）が想定される。
各機能構成要素を１つの電子回路１５で実現してもよいし、各機能構成要素を複数の電子回路１５に分散させて実現してもよい。

＜変形例２＞
変形例２として、一部の各機能構成要素がハードウェアで実現され、他の各機能構成要素がソフトウェアで実現されてもよい。

プロセッサ１１とメモリ１２とストレージ１３と電子回路１５とを処理回路という。つまり、各機能構成要素の機能は、処理回路により実現される。

実施の形態２．
実施の形態２は、検出された物体の検出値に基づき、移動体１００を制御する点が実施の形態１と異なる。実施の形態２では、この異なる点を説明し、同一の点については説明を省略する。

＊＊＊構成の説明＊＊＊
図５を参照して、実施の形態２に係る計測装置１０の構成を説明する。
計測装置１０は、ハードウェアとして、制御インタフェース１６を備える点が異なる。計測装置１０は、制御インタフェース１６を介して、制御用のＥＣＵ３７と接続されている。制御用のＥＣＵ３７は、移動体１００に搭載されたブレーキアクチュエータといった機器３８に接続されている。
また、計測装置１０は、機能構成要素として、移動体制御部２５を備える点が図１に示す計測装置１０と異なる。

＊＊＊動作の説明＊＊＊
図６から図９を参照して、実施の形態２に係る計測装置１０の動作を説明する。
実施の形態２に係る計測装置１０の動作は、実施の形態２に係る計測方法に相当する。また、実施の形態２に係る計測装置１０の動作は、実施の形態２に係る計測プログラムの処理に相当する。

図６のステップＳ２１からステップＳ２４の処理は、図２のステップＳ１１からステップＳ１４の処理と同じである。
（ステップＳ２５：移動体制御処理）
移動体制御部２５は、移動体１００の周辺に存在する物体について、ステップＳ２４で選択された各検出項目の検出値を取得する。そして、移動体制御部２５は、移動体１００を制御する。
具体的には、移動体制御部２５は、移動体１００の周辺に存在する物体についての各検出項目の検出値に応じて、移動体１００に搭載されたブレーキ、ステアリングといった機器を制御する。
例えば、移動体制御部２５は、移動体１００の周辺に存在する物体についての各検出項目の検出値に基づき、移動体１００が物体と衝突する可能性が高いか否かを判定する。移動体制御部２５は、移動体１００が物体と衝突する可能性が高いと判定した場合には、ブレーキを制御して移動体１００を減速又は停止させる、あるいは、ステアリングを制御して物体を避けるように制御する。

図７から図９を参照して、具体的な制御方法の例としてブレーキの制御方法を説明する。
移動体制御部２５は、移動体１００の周辺に存在する物体についての各検出項目の検出値に基づき、移動体１００の予測進行路と物体とのラップ率と、衝突までの時間（以下、ＴＴＣ）とを計算する。移動体制御部２５は、基準割合（例えば５０％）以上のラップ率がある物体に対して、基準時間（例えば１．６秒）以下のＴＴＣである場合に、移動体１００がその物体に衝突する可能性が高い判定する。そして、移動体制御部２５は、制御インタフェース１６を介して、ブレーキアクチュエータに対して制動の指令を出力し、ブレーキを制御することにより、移動体１００を減速又は停止させる。ブレーキアクチュエータに対する制動の指令とは、具体的には、ブレーキ液圧値を指定することである。

図７に示すように、ラップ率は、移動体１００の予測進行路と物体とが重なる割合である。
移動体制御部２５は、例えば、アッカーマンの軌道計算を用いて、移動体１００の予測進行路を計算する。つまり、移動体制御部２５は、車速Ｖ［メートル／秒］と、ヨーレートＹｗ（角速度）［角度／秒］と、ホイールベースＷｂ［メートル］と、操舵角Ｓｔ［角度］とした場合に、数１２により予測軌道Ｒを計算する。予測軌道Ｒは、旋回半径Ｒの円弧である。

ここで、Ｒ_１は、車速と角速度とから計算される旋回半径であり、Ｒ_１＝Ｖ／Ｙｗである。Ｒ_２は、操舵角とホールベースとから計算される旋回半径であり、Ｒ_２＝Ｗｂ／ｓｉｎ（Ｓｔ）である。Ｒは、Ｒ_１とＲ_２とのハイブリッド値である。αは、Ｒ_１とＲ_２との重みの比率である。αは、角速度から計算される軌道を重視する場合には、例えば、０．９８である。

ヨーレートとステアリングの制御といった要因に基づく、移動体１００の予測進行路の変化による衝突予測位置は、時間経過に伴いばらつく。そのため、ある１時点におけるラップ率を単純に計算し、計算結果に基づきブレーキ制御を行うか否かを判定すると、判定結果が安定しない場合がある。
そこで、図８に示すように、移動体制御部２５は、移動体１００の全面を横方向に一定の区分毎に分割し、各区分が物体と重なるか否かを判定する。移動体制御部２５は、重なる区分の数が基準数以上である場合に、ラップ率が基準割合以上であると判定する。これにより、判定結果をある程度安定させることが可能である。

図９に示すように、移動体制御部２５は、移動体１００と物体との相対距離［メートル］を、相対速度［メートル／秒］で除して、ＴＴＣを計算する。相対速度Ｖ３は、物体の速度Ｖ２から、移動体１００の速度Ｖ１を減算して計算される。

＊＊＊実施の形態２の効果＊＊＊
以上のように、実施の形態２に係る計測装置１０は、選択された物体の各検出項目の検出値に基づき、移動体１００を制御する。実施の形態１で説明した通り、各検出項目の検出値は精度が高い。そのため、移動体１００を適切に制御することが可能である。

実施の形態３．
実施の形態３は、信頼度の計算方法が実施の形態１と異なる。実施の形態３では、この異なる点を説明し、同一の点については説明を省略する。

＊＊＊動作の説明＊＊＊
実施の形態３では、図２のステップＳ１３で信頼度計算部２３は、マハラノビス距離θ_ｔとカルマンゲインＫ_ｔとの一方を他方から得られる値に対する重みとして与えることにより信頼度を計算する。つまり、信頼度計算部２３は、マハラノビス距離θ_ｔとカルマンゲインＫ_ｔとを用いて数１３又は数１４のように信頼度Ｍを計算する。

ここで、ｇ（θ_ｔ）は、マハラノビス距離θ_ｔから得られる値である。

ここで、ｈ（Ｋ_ｔ）は、カルマンゲインＫ_ｔから得られる値である。

具体例としては、信頼度計算部２３は、信頼度計算部２３は、数１５に示すように、マハラノビス距離θ_ｔの単調減少関数ｆ（θ_ｔ）とカルマンゲインＫ_ｔとを乗じて、対象のセンサによる観測値に基づき計算された対象の検出項目の検出値の信頼度を計算する。

なお、信頼度計算部２３は、マハラノビス距離θ_ｔの単調減少関数ｆ（θ_ｔ）とカルマンゲインＫ_ｔとの少なくとも一方に重み付けした上で、マハラノビス距離θ_ｔの単調減少関数ｆ（θ_ｔ）とカルマンゲインＫ_ｔとを乗じて信頼度を計算してもよい。
正規化のため、マハラノビス距離θ_ｔの単調減少関数ｆ（θ_ｔ）として、ローレンツ関数と、ガウス関数と、指数関数と、べき乗関数といったマハラノビス距離θ_ｔの積分区間が無限大までの定積分が収束する被積分関数を採用してもよい。また、関数ｆ（θ_ｔ）には、計算に必要なパラメータが含まれてもよい。

＊＊＊実施の形態３の効果＊＊＊
以上のように、実施の形態３に係る計測装置１０は、マハラノビス距離θ_ｔとカルマンゲインＫ_ｔとの一方を他方から得られる値に対する重みとして与えることにより信頼度を計算する。
これにより、適切な信頼度が計算される。その結果、適切な検出値が採用される。

図１０から図１３を参照して、実施の形態３で説明した信頼度の計算方法を用いて検出値を選択した具体例を説明する。
ここでは、ＬｉＤＡＲ３４によって得られた観測値と、Ｒａｄａｒ３５によって得られた観測値とが同一グループであったとする。図１０では、横軸が移動体１００から周辺に存在する物体までの距離を示し、縦軸がそれぞれのセンサで得られた物体の相対的な奥行方向の位置Ｙに関するカルマンゲインを示している。図１１では、横軸が移動体１００から周辺に存在する物体までの距離を示し、縦軸がそれぞれのセンサで得られた物体の相対的な奥行方向の位置Ｙに関するマハラノビス距離を示している。

図１０に示すカルマンゲインと図１１に示すマハラノビス距離とに基づき計算された、奥行方向の位置Ｙについての信頼度を計算すると図１２に示すようになる。ここでは、信頼度は、マハラノビス距離θ_ｔの単調減少関数ｆ（θ_ｔ）とカルマンゲインＫ_ｔとを乗じて、計算されている。マハラノビス距離θ_ｔの単調減少関数ｆ（θ_ｔ）としては、数１６に示すローレンツ関数が用いられている。

なお、ここでは、パラメータγとして１が用いられた。しかし、パラメータγは、０＜γ＜∞の範囲で設定すればよい。これにより、信頼度に対するカルマンゲインの影響が大きくなるように設定されてもよいし、マハラノビス距離の影響が大きくなるように設定されてもよい。

図１２に示す信頼度を参照して、各時刻、すなわち各距離において、奥行方向の位置Ｙに関する信頼度を比較し、信頼度が高い検出値を選択すると、図１３に示すようになる。図１３に示すように、奥行方向の位置Ｙがばらつくことなく、時刻の変化に合わせて一定の変化をしており、精度が高い結果が得られたことが分かる。

＊＊＊他の構成＊＊＊
＜変形例３＞
なお、実施の形態３で計算された信頼度に基づき特定された検出値を用いて、実施の形態２で説明したように移動体１００を制御してもよい。

以上、本発明の実施の形態について説明した。これらの実施の形態及び変形例のうち、いくつかを組み合わせて実施してもよい。また、いずれか１つ又はいくつかを部分的に実施してもよい。なお、本発明は、以上の実施の形態及び変形例に限定されるものではなく、必要に応じて種々の変更が可能である。

１０ 計測装置、１１ プロセッサ、１２ メモリ、１３ ストレージ、１４ センサインタフェース、１５ 電子回路、１６ 制御インタフェース、２１ 追尾部、２２ 融合部、２３ 信頼度計算部、２４ 値選択部、２５ 移動体制御部、３１ ＬｉＤＡＲ用のＥＣＵ、３２ Ｒａｄａｒ用のＥＣＵ、３３ カメラ用のＥＣＵ、３４ ＬｉＤＡＲ、３５ Ｒａｄａｒ、３６ カメラ、３７ 制御用のＥＣＵ、３８ 機器、４１ 観測値、４２ 観測値、４３ 観測値、５１ グループ、１００ 移動体。

Claims (9)

1. 複数のセンサそれぞれを対象のセンサとして、前記対象のセンサによって物体が対象時刻に観測されて得られた前記物体の検出項目についての観測値に基づき、前記物体の前記検出項目についての前記対象時刻における検出値をカルマンフィルタを用いて計算する追尾部と、
   前記複数のセンサそれぞれを対象のセンサとして、前記対象のセンサによって得られた前記観測値と、前記観測値に基づき前記検出値が前記追尾部によって計算された計算時に用いられた、前記対象時刻の前の時刻に予測された前記対象時刻における前記物体の前記検出項目の値である予測値との間のマハラノビス距離に加え、前記計算時に得られたカルマンゲインを用いて、前記マハラノビス距離と前記カルマンゲインとの一方を他方から得られる値に対する重みとして与えることにより、前記対象のセンサにより得られた前記観測値に基づき計算された前記検出値の信頼度を計算する信頼度計算部と、
   前記複数のセンサにより得られた前記観測値に基づき計算された前記検出値のうち、前記信頼度計算部によって計算された前記信頼度が高い前記検出値を選択する値選択部と
   を備える計測装置。
2. 前記追尾部は、前記対象のセンサによって物体が対象時刻に観測されて得られた前記物体の複数の検出項目それぞれを対象の検出項目として、前記対象の検出項目についての観測値に基づき、前記物体についての前記対象の検出項目の検出値を計算し、
   前記信頼度計算部は、前記複数の検出項目それぞれを対象の検出項目として、前記対象のセンサによって得られた前記対象の検出項目の前記観測値と、前記物体の前記対象の検出項目の予測値との間のマハラノビス距離に加え、前記計算時に得られたカルマンゲインを用いて、前記マハラノビス距離と前記カルマンゲインとの一方を他方から得られる値に対する重みとして与えることにより、前記対象のセンサにより得られた前記観測値に基づき計算された前記対象の検出項目の前記検出値の信頼度を計算し、
   前記値選択部は、前記複数の検出項目それぞれを対象の検出項目として、前記対象の検出項目について前記複数のセンサにより得られた前記観測値に基づき計算された前記検出値のうち、前記信頼度計算部によって計算された前記信頼度が高い前記検出値を選択する
   請求項１に記載の計測装置。
3. 前記信頼度計算部は、前記マハラノビス距離と前記カルマンゲインとを乗じて、前記信頼度を計算する
   請求項１又は２に記載の計測装置。
4. 前記信頼度計算部は、前記マハラノビス距離の単調減少関数と前記カルマンゲインとを乗じて、前記信頼度を計算する
   請求項１又は２に記載の計測装置。
5. 前記信頼度計算部は、前記マハラノビス距離のローレンツ関数とガウス関数と指数関数とべき乗関数とのいずれかと前記カルマンゲインとを乗じて、前記信頼度を計算する
   請求項４に記載の計測装置。
6. 前記計測装置は、さらに、
   前記複数のセンサそれぞれによって得られた前記観測値間のマハラノビス距離を計算して、計算された前記マハラノビス距離が閾値以下の観測値を同一の物体を観測して得られた観測値として同じグループに分類する融合部
   を備え、
   前記値選択部は、前記融合部によって同じグループに分類された各観測値に基づき計算された前記検出値のうち、前記信頼度が高い前記検出値を選択する
   請求項１から５までのいずれか１項に記載の計測装置。
7. 前記物体は、移動体の周辺に存在する物体であり、
   前記計測装置は、さらに、
   前記値選択部によって選択された前記検出値に基づき、前記移動体を制御する移動体制御部
   を備える請求項１から６までのいずれか１項に記載の計測装置。
8. 追尾部が、複数のセンサそれぞれを対象のセンサとして、前記対象のセンサによって物体が対象時刻に観測されて得られた前記物体の検出項目についての観測値に基づき、前記物体の前記検出項目についての前記対象時刻における検出値をカルマンフィルタを用いて計算し、
   信頼度計算部が、前記複数のセンサそれぞれを対象のセンサとして、前記対象のセンサによって得られた前記観測値と、前記観測値に基づき前記検出値が計算された計算時に用いられた、前記対象時刻の前の時刻に予測された前記対象時刻における前記物体の前記検出項目の値である予測値との間のマハラノビス距離に加え、前記計算時に得られたカルマンゲインを用いて、前記マハラノビス距離と前記カルマンゲインとの一方を他方から得られる値に対する重みとして与えることにより、前記対象のセンサにより得られた前記観測値に基づき計算された前記検出値の信頼度を計算し、
   値選択部が、前記複数のセンサにより得られた前記観測値に基づき計算された前記検出値のうち、前記信頼度が高い前記検出値を選択する計測方法。
9. 複数のセンサそれぞれを対象のセンサとして、前記対象のセンサによって物体が対象時刻に観測されて得られた前記物体の検出項目についての観測値に基づき、前記物体の前記検出項目についての前記対象時刻における検出値をカルマンフィルタを用いて計算する追尾処理と、
   前記複数のセンサそれぞれを対象のセンサとして、前記対象のセンサによって得られた前記観測値と、前記観測値に基づき前記検出値が前記追尾処理によって計算された計算時に用いられた、前記対象時刻の前の時刻に予測された前記対象時刻における前記物体の前記検出項目の値である予測値との間のマハラノビス距離に加え、前記計算時に得られたカルマンゲインを用いて、前記マハラノビス距離と前記カルマンゲインとの一方を他方から得られる値に対する重みとして与えることにより、前記対象のセンサにより得られた前記観測値に基づき計算された前記検出値の信頼度を計算する信頼度計算処理と、
   前記複数のセンサにより得られた前記観測値に基づき計算された前記検出値のうち、前記信頼度計算処理によって計算された前記信頼度が高い前記検出値を選択する値選択処理と
   を行う計測装置としてコンピュータを機能させる計測プログラム。

Images