特許 7601074 制御装置、制御方法、制御プログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-12-09

(45)【発行日】2024-12-17

(54)【発明の名称】制御装置、制御方法、制御プログラム

(51)【国際特許分類】

   G05D 1/43 20240101AFI20241210BHJP

   G05D 1/24 20240101ALI20241210BHJP

   G08G 1/16 20060101ALI20241210BHJP

   G01C 21/28 20060101ALI20241210BHJP

   B60W 30/10 20060101ALI20241210BHJP

【ＦＩ】

G05D1/43

G05D1/24

G08G1/16 C

G01C21/28

B60W30/10

【請求項の数】 5

(21)【出願番号】P 2022166989

(22)【出願日】2022-10-18

(65)【公開番号】P2024059360

(43)【公開日】2024-05-01

【審査請求日】2024-03-05

(73)【特許権者】

【識別番号】000003207

【氏名又は名称】トヨタ自動車株式会社

(74)【代理人】

【識別番号】110003199

【氏名又は名称】弁理士法人高田・高橋国際特許事務所

(72)【発明者】

【氏名】保海 佳佑

(72)【発明者】

【氏名】河内 太一

【審査官】田中 友章

(56)【参考文献】

【文献】特開２０２１－１７６０５２（ＪＰ，Ａ）

【文献】特開２０１８－１６９３１９（ＪＰ，Ａ）

【文献】国際公開第２０１８／１８１９７４（ＷＯ，Ａ１）

【文献】国際公開第２０１８／２１２２９２（ＷＯ，Ａ１）

【文献】米国特許出願公開第２０２０／０１２４４２１（ＵＳ，Ａ１）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０５Ｄ １／４３

Ｇ０５Ｄ １／２４

Ｇ０８Ｇ １／１６

Ｇ０１Ｃ ２１／２８

Ｂ６０Ｗ ３０／１０

(57)【特許請求の範囲】

【請求項1】

移動体の自己位置推定を行う処理と、
前回処理で推定された前記移動体の位置に対する変位量が所定範囲内となるように前記自己位置推定で得られた推定位置を補正する処理と、
前記推定位置の補正により得られた補正推定位置を今回処理の前記移動体の位置とみなし、前記補正推定位置に基づいて前記移動体の制御を行う処理と、
前記推定位置と前記補正推定位置の差分の大きさに基づいて前記自己位置推定の精度不良を検出する処理と、
前記自己位置推定の精度不良が検出されたことを受けて、前記差分の大きさに応じた非常制御を行う処理と、
を実行するように構成されたことを特徴とする制御装置。

【請求項2】

請求項１に記載の制御装置であって、
前記推定位置を補正することは、前回処理で推定された前記移動体の位置に対する変位率に一定の上限値を設けて、前記推定位置を制限するように補正することを含む
ことを特徴とする制御装置。

【請求項3】

請求項１又は２に記載の制御装置であって、
前記自己位置推定の精度不良を検出する処理は、前記差分の大きさが第１閾値を超えたことを受けて、前記自己位置推定が精度不良であると検出することを含み、
前記非常制御は、
前記差分の大きさが前記第１閾値を超えて第２閾値を超えないときに実行される警報を発する第１制御と、
前記差分の大きさが前記第２閾値を超えて第３閾値を超えないときに実行される減速による退避を行わせる第２制御と、
前記差分の大きさが前記第３閾値を超えるときに実行される停止による退避を行わせる第３制御と、
を含む
ことを特徴とする制御装置。

【請求項4】

移動体の自己位置推定を行うことと、
前回処理で推定された前記移動体の位置に対する変位量が所定範囲内となるように前記自己位置推定で得られた推定位置を補正することと、
前記推定位置の補正により得られた補正推定位置を今回処理の前記移動体の位置とみなし、前記補正推定位置に基づいて前記移動体の制御を行う処理と、
前記推定位置と前記補正推定位置の差分の大きさに基づいて前記自己位置推定の精度不良を検出することと、
前記自己位置推定の精度不良が検出されたことを受けて、前記差分の大きさに応じた非常制御を行うことと、
を含むことを特徴とする制御方法。

【請求項5】

移動体の制御を行う処理をコンピュータに実行させる制御プログラムであって、
前記移動体の自己位置推定を行う処理と、
前回処理で推定された前記移動体の位置に対する変位量が所定範囲内となるように前記自己位置推定で得られた推定位置を補正する処理と、
前記推定位置の補正により得られた補正推定位置を今回処理の前記移動体の位置とみなし、前記補正推定位置に基づいて前記移動体の制御を行う処理と、
前記推定位置と前記補正推定位置の差分の大きさに基づいて前記自己位置推定の精度不良を検出する処理と、
前記自己位置推定の精度不良が検出されたことを受けて、前記差分の大きさに応じた非常制御を行う処理と、
を前記コンピュータに実行させるように構成されたことを特徴とする制御プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、推定位置に基づいて移動体を制御する技術に関する。

【背景技術】

【0002】

特許文献１は、車両の周囲の物標の認識結果から算出される観測位置と車両の内部センサの検出結果から算出される予測位置とに基づいて、車両の地図上の位置を推定する自車位置推定装置を開示している。自車位置推定装置は、推定結果のふらつきが生じているか否かを判定する処理と、推定結果のふらつきが生じていると判定された場合、推定結果が予測位置に近くなるように位置の推定における予測位置の重み付けを重くする処理と、を実行する。

【0003】

その他、本開示に係る技術分野の技術レベルを示す文献として、以下の特許文献２及び特許文献３がある。

【先行技術文献】

【特許文献】

【0004】

【文献】特開２０２０－０３４４４１号公報

【文献】特開２０２０－００１６６８号公報

【文献】特開２０２１－０９９３８４号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

移動体の位置を推定する自己位置推定は、自律走行制御等の移動体の制御において重要な処理の１つである。自己位置推定による推定位置は、センサの異常等を要因として急激な変動が生じることがある。推定位置に急激な変動が生じた場合、移動体の制御は、推定位置に追従できなくなる虞がある。延いては、移動体の位置を誤ったまま移動体の制御が継続される虞がある。このような制御が継続されることは、制御性能を確保する上で望ましくない。また乗客を乗せて走行する車両等の移動体においては、安全性の観点でも問題となる。

【0006】

特許文献１に開示される技術によれば、推定位置に急激な変動が生じた場合、推定結果がふらついているとして、位置の推定における予測位置の重み付けが重くなる。しかしながら、内部センサの検出結果から算出される予測位置は、一般に履歴的な誤差が生じる。このため、予測位置の重み付けを大きくすると、移動体の位置を大きく誤ったまま移動体の制御が継続される虞がある。

【0007】

本開示の１つの目的は、上記の課題を鑑み、推定位置に基づく移動体の制御に関して、移動体の位置を誤ったまま制御が継続されることを抑止することができる技術を提供することにある。

【課題を解決するための手段】

【0008】

本開示の第１の観点は、制御装置に関する。

【0009】

第１の観点に係る制御装置は、移動体の自己位置推定を行う処理と、前回処理で推定された移動体の位置に対する変位量が所定範囲内となるように自己位置推定で得られた推定位置を補正する処理と、推定位置の補正により得られた補正推定位置を今回処理の移動体の位置とみなし、補正推定位置に基づいて移動体の制御を行う処理と、推定位置と補正推定位置の差分の大きさに基づいて自己位置推定の精度不良を検出する処理と、自己位置推定の精度不良が検出されたことを受けて、推定位置と補正推定位置の差分の大きさに応じた非常制御を行う処理と、を実行するように構成されたことを特徴とする。

【0010】

本開示の第２の観点は、制御方法に関する。

【0011】

第２の観点に係る制御方法は、移動体の自己位置推定を行うことと、前回処理で推定された移動体の位置に対する変位量が所定範囲内となるように自己位置推定で得られた推定位置を補正することと、推定位置の補正により得られた補正推定位置を今回処理の移動体の位置とみなし、補正推定位置に基づいて移動体の制御を行う処理と、推定位置と補正推定位置の差分の大きさに基づいて自己位置推定の精度不良を検出することと、自己位置推定の精度不良が検出されたことを受けて、差分の大きさに応じた非常制御を行うことと、を含むことを特徴とする。

【0012】

本開示の第３の観点は、移動体の制御を行う処理をコンピュータに実行させる制御プログラムに関する。

【0013】

第３の観点に係る制御プログラムは、移動体の自己位置推定を行う処理と、前回処理で推定された移動体の位置に対する変位量が所定範囲内となるように自己位置推定で得られた推定位置を補正する処理と、推定位置の補正により得られた補正推定位置を今回処理の移動体の位置とみなし、補正推定位置に基づいて移動体の制御を行う処理と、推定位置と補正推定位置の差分の大きさに基づいて自己位置推定の精度不良を検出する処理と、自己位置推定の精度不良が検出されたことを受けて、推定位置と補正推定位置の差分の大きさに応じた非常制御を行う処理と、をコンピュータに実行させるように構成されたことを特徴とする。

【発明の効果】

【0014】

本開示によれば、自己位置推定で得られた推定位置は、前回処理で推定された移動体の位置に対する変位量が所定範囲内となるように補正される。そして、補正により得られた補正推定位置に基づいて移動体の制御が行われる。これにより、推定位置に急激な変動が生じた場合においても移動体の制御の追従性を担保することができる。さらに本開示によれば、推定位置と補正推定位置の差分の大きさに基づいて自己位置推定の精度不良が検出される。そして、自己位置推定の精度不良が検出されたことを受けて、差分の大きさに応じた非常制御が実行される。これにより、移動体の位置を誤ったまま制御が継続されることを抑止することができる。

【図面の簡単な説明】

【0015】

【図1】本実施形態に係る制御システムの構成を示すブロック図である。

【図2】推定位置の補正の例を示す概念図である。

【図3】推定位置と補正推定位置の差分の大きさに応じた非常制御の切り換え判断の例を示す概念図である。

【図4】制御装置が実行する処理の一例を示すフローチャートである。

【図5】本実施形態に係る制御システムの実施例を示す概念図である。

【発明を実施するための形態】

【0016】

本開示は、移動体の制御に関連する。移動体としては、車両、ロボット、等が例示される。移動体は、自律走行可能であってもよい。車両は、自動運転車両であってもよい。一例として、以下の説明においては、移動体が車両である場合について考える。一般化する場合には、以下の説明における「車両」を「移動体」で読み替えるものとする。

【0017】

１．制御システム
図１は、本実施形態に係る制御システム１０の構成を示すブロック図である。制御システム１０は、車両１を制御する。制御システム１０は、内部センサ２０、外部センサ３０、ＧＮＳＳ（Global Navigation Satellite System）センサ４０、走行装置５０、ＨＭＩ６０、及び制御装置１００を含んでいる。

【0018】

内部センサ２０は、車両１に搭載され、車両１の走行状態を検出する。内部センサ２０としては、車速センサ（車輪速センサ）、操舵角センサ、加速度センサ、ヨーレートセンサ、等が例示される。

【0019】

外部センサ３０は、車両１に搭載され、車両１の周囲の状況を検出する。外部センサ３０としては、ＬｉＤＡＲ（Laser Imaging Detection and Ranging）、カメラ、レーダ、等が挙げられる。

【0020】

ＧＮＳＳセンサ４０は、衛星から信号を受信することにより車両１の位置を測定する。

【0021】

走行装置５０は、操舵装置、駆動装置、及び制動装置を含んでいる。操舵装置は、車輪を転舵する。駆動装置は、駆動力を発生させる。制動装置は、制動力を発生させる。

【0022】

ＨＭＩ６０は、車両１のＨＭＩ機能を提供する装置を含んでいる。ＨＭＩ６０として、ディスプレイ、スピーカ、タッチパネル、等が例示される。

【0023】

制御装置１００は、車両１を制御するコンピュータである。少なくとも、制御装置１００は、車両１の位置を推定し、推定された車両１の位置に基づいて車両１の走行を制御する機能を提供する。例えば、制御装置１００は、車両１の自動運転機能を提供するＥＣＵ（Electronic Control Unit）である。

【0024】

制御装置１００は、１又は複数のプロセッサ１１０（以下、単にプロセッサ１１０と呼ぶ）と１又は複数の記憶装置１２０（以下、単に記憶装置１２０と呼ぶ）を含んでいる。プロセッサ１１０は、後述する各種処理を実行する。例えば、プロセッサ１１０は、演算装置やレジスタ等を含むＣＰＵ（Central Processing Unit）で構成することができる。記憶装置１２０は、各種情報を格納する。例えば、記憶装置１２０は、揮発性メモリ、不揮発性メモリ、ＨＤＤ（Hard Disk Drive）、ＳＳＤ（Solid State Drive）等の記録媒体で構成することができる。典型的には、制御装置１００は、車両１に搭載される。あるいは、制御装置１００の一部は、車両１の外部の外部装置に配置され、リモートで車両１を制御してもよい。

【0025】

記憶装置１２０には、制御プログラム２１０、車両走行情報２２０、センサ検出情報２３０、地図情報２４０、推定位置情報２５０、等が格納される。

【0026】

制御プログラム２１０は、車両１を制御するためのコンピュータプログラムである。プロセッサ１１０が制御プログラム２１０を実行することにより、制御装置１００による各種処理が実現される。制御プログラム２１０は、コンピュータ読み取り可能な記録媒体に記録されていてもよい。

【0027】

車両走行情報２２０は、内部センサ２０によって検出される情報であり、車両１の走行状態を示す。車両１の走行状態としては、車速、操舵角、加速度、ヨーレート、等が挙げられる。

【0028】

センサ検出情報２３０は、外部センサ３０によって検出される情報である。例えば、センサ検出情報２３０は、ＬｉＤＡＲによって得られる点群情報を含む。他の例として、センサ検出情報２３０は、カメラによって撮影される画像を含んでいてもよい。

【0029】

更に、センサ検出情報２３０は、車両１の周囲の物体に関する物体情報を含む。車両１の周囲の物体としては、歩行者、他車両、特徴物、等が例示される。特徴物としては、白線、縁石、ポール、電信柱、看板、標識、建物の角、等が例示される。特徴物は、後述される自己位置推定において利用される。物体情報は、少なくとも車両１に対する物体の相対位置を含む。例えば、ＬｉＤＡＲによって得られた点群情報に基づいて、物体を認識し、その物体の相対位置及び相対速度を取得することができる。

【0030】

地図情報２４０は、一般的なナビゲーション地図を含む。また後述される自己位置推定を行うため、地図情報２４０は、地図上の特徴物の情報を管理する特徴物地図情報２４１を含んでいる。特徴物地図情報２４１は、少なくとも、地図上の絶対座標系における特徴物の位置（絶対位置）の情報を管理する。さらに特徴物地図情報２４１は、特徴物を特定するための情報（分類、属性、形状等）を管理しても良い。特徴物地図情報２４１は、本実施形態に係る制御システム１０を適用する環境に応じてあらかじめ与えられて良い。

【0031】

その他、地図情報２４０は、車両１の制御に用いられる情報を含んでいて良い。例えば、地図情報２４０は、地図上の路面の形状、高さ、輝度等の情報や、地図上の構造物の形状や属性等の情報を含んでいて良い。

【0032】

後述するように、プロセッサ１１０は、所定の周期で車両１の位置を推定する処理を繰り返し実行する。推定位置情報２５０は、プロセッサ１１０が実行する処理により推定された車両１の位置の時系列データを管理する。

【0033】

２．処理
以下、制御装置１００が実行する処理、より具体的にはプロセッサ１１０が実行する処理について説明する。

【0034】

２－１．自己位置推定処理
プロセッサ１１０は、車両１の位置（絶対位置）を推定する自己位置推定に係る処理（自己位置推定処理）を実行する。車両１の位置は、以下、単に「車両位置」と称する。車両位置の初期値は、例えば、ＧＮＳＳセンサ４０により得られる。

【0035】

ここで、車両位置は、典型的には、地図上の絶対座標系における座標位置で表される。さらに車両位置は、車両１の姿勢（ロール角、ピッチ角、及びヨー角）を含んでいても良い。この場合、車両位置は、一般に、６つの要素をもつベクトルで表されることとなる。あるいは、車両位置は、基準となる座標位置及び姿勢に対する剛体変換行列で表すことも可能である。例えば、基準となる３次元の座標位置に対する平行移動を表す３×１のサイズの並進ベクトルをｒとし、基準となる３軸の車両１の姿勢に対する回転を表す３×３のサイズの回転行列をＲとすると、車両位置は、以下の式の４×４のサイズの剛体変換行列Ｍで表すことができる。この場合、車両位置の座標位置のベクトル表現は、基準となる座標位置を表すベクトルに剛体変換行列を作用させることで得られる。また車両位置の車両１の姿勢は、基準となる車両１の姿勢に回転行列Ｒを作用させることで得られる。

【0036】

【数1】

【0037】

以下、プロセッサ１１０が実行する自己位置推定処理の一例について説明する。

【0038】

まずプロセッサ１１０は、車両走行情報２２０と、前回処理で推定された車両位置とに基づいて、大まかな車両位置（以下、「事前推定位置」と称する。）を算出する。例えば、プロセッサ１１０は、車両１の操舵角及び車速に基づいて車両１の移動量（変位量）を算出し、その移動量に基づいて事前推定位置を算出する。また例えば、プロセッサ１１０は、車両位置を状態とし車両走行情報２２０を説明変数として構成された状態方程式から事前推定位置を算出する。なお、プロセッサ１１０は、推定位置情報２５０を参照することにより、前回処理で推定された車両位置を取得することができる。前回処理で推定された車両位置を、以下単に「前回位置」と称する。

【0039】

次にプロセッサ１１０は、センサ検出情報２３０に基づいて、車両１の周囲の特徴物を認識する。さらに、プロセッサ１１０は、算出された事前推定位置と認識された特徴物の相対位置とを組み合わせることにより、認識された特徴物の絶対位置を推定する。また、プロセッサ１１０は、認識された特徴物の物体情報に基づいて、認識された特徴物と、特徴物地図情報２４１で管理される特徴物と、の紐づけを行う。そしてプロセッサ１１０は、認識された特徴物について、推定された絶対位置と、特徴物地図情報２４１から得られる絶対位置と、がなるべく一致するように事前推定位置を修正する。例えば、プロセッサ１１０は、車両位置を状態、認識された特徴物の絶対位置を観測データとして、パーティクルフィルタやカルマンフィルタ等により事前推定位置を修正する。プロセッサ１１０は、事前推定位置の修正によって得られた車両位置を自己位置推定処理の処理結果とする。

【0040】

一例としてこのように、プロセッサ１１０は、自己位置推定処理を実行する。なお、本実施形態に係る制御システム１０において、プロセッサ１１０は、上記で説明する自己位置推定処理とは異なるアルゴリズムを採用していても良い。プロセッサ１１０が自己位置推定処理を実行することにより得られた車両位置を、以下単に「推定位置」と称する。なお、推定位置は、車両位置の各要素を確率変数とする確率分布で与えられていても良い。

【0041】

２－２．フィルタ処理
本実施形態に係る制御システム１０において、プロセッサ１１０は、さらに推定位置を補正する処理（フィルタ処理）を実行する。フィルタ処理において、プロセッサ１１０は、前回位置に対する変位量が所定範囲内となるように推定位置を補正する。ここで、所定範囲は、本実施形態に係る制御システム１０を適用する環境に応じて好適に与えられて良い。典型的には、所定範囲は、車両１の制御において追従することのできる範囲である。ただし、不必要に所定範囲を狭めることは、制御のノミナル性能を著しく低下させるため望ましくない。プロセッサ１１０がフィルタ処理を実行することにより得られた車両位置を、以下単に「補正推定位置」と称する。

【0042】

以下、プロセッサ１１０が実行するフィルタ処理のいくつかの例について説明する。

【0043】

第１の例は、前回位置の各要素に対する変位量に一定の上限値を設けて、推定位置の各要素を制限するように補正推定位置を算出することである。第１の例によれば、車両位置の１つの要素について、推定位置をｘ_ｅｓｔ、前回位置をｘ_ｌｐ、一定の上限値をｘ_ｍａｘとすると、プロセッサ１１０は、補正推定位置を以下の式のｘ_ｏｕｔで算出することができる。車両位置が複数の要素を含む場合、各要素について以下の式を適用すれば良い。この場合、一定の上限値は、各要素で異なっていても良い。

【0044】

【数2】

【0045】

図２の（Ａ）は、車両位置が２次元の座標位置（ｘ，ｙ）で表される場合において、フィルタ処理の第１の例による推定位置の補正の例を示す概念図である。図２の（Ａ）では、推定位置のいくつかの例（ＥＰｉ（ｉ＝０，１，・・・，９））について、フィルタ処理の第１の例による補正の様子と補正推定位置（ＣＰｉ（ｉ＝０，１，・・・，９））が概念的に示されている。なお、図２の（Ａ）において、ＬＰは、前回位置を示す。

【0046】

図２の（Ａ）に示すように、フィルタ処理の第１の例により、補正推定位置は、前回位置ＬＰに対する変位量が所定範囲ＰＲ内となる。ここで、第１の例において、所定範囲ＰＲは、前回位置ＬＰを中心として、一定の上限値（ｘ_ｍａｘ，ｙ_ｍａｘ）で規定される矩形の範囲となる。なお、推定位置が所定範囲ＰＲ内となっている場合、推定位置と補正推定位置は一致する。例えば図２の（Ａ）において、推定位置ＥＰ１、ＥＰ２、及びＥＰ３は、補正推定位置ＣＰ１、ＣＰ２、及びＣＰ３と一致する。

【0047】

第２の例は、前回位置を含む所定範囲を規定し、規定された所定範囲内となるように補正推定位置を算出することである。前回位置を含む所定範囲は、例えば、前回位置を中心とする楕円が挙げられる。この場合、第２の例によれば、車両位置が２次元の座標位置であるとして、推定位置を（ｘ_ｅｓｔ，ｙ_ｅｓｔ）、前回位置を（ｘ_ｌｐ，ｙ_ｌｐ）、楕円の長軸及び短軸の長さを規定するパラメータを（ｘ_ｍａｘ，ｙ_ｍａｘ）とすると、プロセッサ１１０は、補正推定位置を以下の式の（ｘ_ｏｕｔ，ｙ_ｏｕｔ）で算出することができる。車両位置が３次元の座標位置である場合の拡張は容易である。また、車両位置が車両１の姿勢を含む場合は、車両１の姿勢について同様に以下の式を適用すれば良い。

【数3】

【0048】

図２の（Ｂ）は、車両位置が２次元の座標位置（ｘ，ｙ）で表される場合において、フィルタ処理の第２の例による推定位置の補正の例を示す概念図である。ここで、前回位置ＬＰを含む所定範囲ＰＲは、前回位置ＬＰを中心として、パラメータ（ｘ_ｍａｘ，ｙ_ｍａｘ）で規定される楕円である。図２の（Ｂ）に示すように、第２の例によっても、補正推定位置は、前回位置ＬＰに対する変位量が所定範囲ＰＲ内となる。

【0049】

第２の例において、所定範囲は、前回位置に対する変位率に一定の上限値を与えるように規定することも可能である。例えば、上記の式において、楕円の長軸及び短軸の長さを規定するパラメータを（ｘ_ｍａｘ，ｙ_ｍａｘ）＝（Δｔ×ｖ_ｘｍａｘ，Δｔ×ｖ_ｙｍａｘ）とすれば、車両位置の各要素について前回位置に対する変位率に一定の上限値（ｖ_ｘｍａｘ，ｖ_ｙｍａｘ）を与える所定範囲を規定することができる。なお、Δｔは、前回処理と今回処理の時間差である。

【0050】

第３の例は、前回位置の各要素に対する変位率に一定の上限値を設けて、推定位置の各要素を制限するように補正推定位置を算出することである。第３の例によれば、車両位置の１つの要素について、一定の上限値をｖ_ｍａｘ、前回処理と今回処理の時間差をΔｔとし、その他は第１の例と同様の表現を採用すると、プロセッサ１１０は、補正推定位置を以下の式のｘ_ｏｕｔで算出することができる。

【0051】

【数4】

【0052】

第３の例によっても、補正推定位置は、前回位置に対する変位量を所定範囲内とすることができる。特に、変位率の観点で、車両１の制御において追従することのできる範囲としての所定範囲を与えることができる。第３の例における所定範囲は、図２の（Ａ）に示す場合と同等であり、前回位置ＬＰを中心として、一定の上限値ｖ_ｍａｘで規定される矩形の範囲となる。

【0053】

以上説明したように、プロセッサ１１０は、フィルタ処理を実行することにより、前回位置に対する変位量が所定範囲内となるように推定位置を補正した補正推定位置を算出する。本実施形態に係る制御システム１０では、補正推定位置が最終的に車両１の位置と推定される。つまり、前回位置は、前回処理において算出された補正推定位置である。プロセッサ１１０は、フィルタ処理の実行により算出した補正推定位置を、今回処理において推定された車両１の位置として、記憶装置１２０の推定位置情報２５０に格納する。

【0054】

２－３．精度不良検出処理
プロセッサ１１０は、自己位置推定処理の実行により得られた推定位置とフィルタ処理の実行により得られた補正推定位置の差分の大きさに基づいて自己位置推定の精度不良を検出する処理（精度不良検出処理）を実行する。補正推定位置は、前回位置に対する変位量が所定範囲内となるように算出されている。従って、推定位置と補正推定位置の差分が大きいことは、推定位置が前回位置に対して急激に変動していることを示すと考えられる。このため、推定位置と補正推定位置の差分の大きさは、自己位置推定の精度を評価する指標となる。

【0055】

ここで、推定位置と補正推定位置の差分は、例えば、次のように与えられる。

【0056】

車両位置がベクトルで表される場合、推定位置と補正推定位置の差分は、推定位置を表すベクトルと補正推定位置を表すベクトルの差のベクトルとすることができる。なお、車両位置が座標位置及び車両１の姿勢を要素とするベクトルである場合、推定位置と補正推定位置の差分は、座標位置と車両１の姿勢で別個に算出されて良い。

【0057】

車両位置が剛体変換行列で表される場合、推定位置と補正推定位置の差分は、推定位置を表す剛体変換行列をＭ_ｅｓｔ、補正推定位置を表す剛体変換行列をＭ_ｏｕｔとすると、以下の式のＭ_ｄｉｆで算出することができる。なお、Ｍ_ｄｉｆの並進ベクトルｒを抽出することにより、座標位置に関する推定位置と補正推定位置の差分のベクトル表現を得ることができる。また、Ｍ_ｄｉｆの回転行列Ｒを抽出してオイラー角への変換を行うことにより、車両１の姿勢に関する推定位置と補正推定位置の差分のベクトル表現を得ることができる。

【0058】

【数5】

【0059】

精度不良検出処理において、プロセッサ１１０は、例えば、推定位置と補正推定位置の差分の大きさが所定の閾値（第１閾値）を超えたことを受けて、自己位置推定が精度不良であると検出するように構成することができる。差分の大きさが第１閾値を超えたことは、例えば、第１閾値をスカラー値として、差分を表すベクトルのノルムが第１閾値を超えたことから判断することができる。あるいは、第１閾値をベクトル空間上の領域の境界として、差分を表すベクトルが領域外となることから判断することもできる。また、プロセッサ１１０は、差分の大きさが第１閾値を超えた状態が一定期間継続したこと、あるいは差分の大きさが一定期間の間に第１閾値を所定回数以上超えたことを条件として、自己位置推定が精度不良であると検出するように構成されていても良い。

【0060】

２－４．走行制御処理
プロセッサ１１０は、フィルタ処理の実行により得られた補正推定位置に基づいて車両１の走行を制御する処理（走行制御処理）を実行する。プロセッサ１１０は、走行制御処理の実行により走行装置５０又はＨＭＩ６０に対する制御信号を生成する。例えば、車両１の自動運転を行う場合、プロセッサ１１０は、補正推定位置と地図情報２４０から運転判断や走行軌道を含む走行計画を生成する。そして、プロセッサ１１０は、生成した走行計画に沿って車両１が走行するように加速、制動、操舵に係る制御信号を生成する。

【0061】

特にプロセッサ１１０は、精度不良検出処理において自己位置推定の精度不良が検出されたことを受けて、推定位置と補正推定位置の差分の大きさに応じた非常制御を行う処理を実行する。非常制御として、自動運転の中断手続、車両１のオペレータへの警報の発報、減速による退避を行わせる制御、停止による退避を行わせる制御、等が挙げられる。

【0062】

プロセッサ１１０は、推定位置と補正推定位置の差分の大きさに応じて、これらの非常制御を切り替えるように構成されていても良い。例えば、推定位置と補正推定位置の差分のベクトルについて、プロセッサ１１０は、図３に示すように非常制御の切り換えを判断する。図３において、第１閾値、第２閾値、及び第３閾値は、ベクトル空間上の領域の境界である。図３に示す場合、プロセッサ１１０は、差分のベクトル（ＤＶ１，ＤＶ２，ＤＶ３）の大きさが、第１閾値を超えて第２閾値を超えないとき（ＤＶ１）に第１制御を実行し、第２閾値を超えて第３閾値を超えないとき（ＤＶ２）に第２制御を実行し、第３閾値を超えるとき（ＤＶ３）に第３制御を実行することとなる。なお、第１閾値を超えないときは、プロセッサ１１０は、通常制御を行う。このように構成することで、自己位置推定の精度不良を段階的に判断し、精度不良の程度に応じた適切な制御を実行することができる。例えば、第１制御を警報の発報、第２制御を減速による退避、第３制御を停止による退避とすることで、警報によるオペレータへの対処の求め、減速による制御性能の確保、停止による安全性の確保、を段階的に行うことができる。

【0063】

プロセッサ１１０が走行制御処理の実行により生成する制御信号に従って走行装置５０及びＨＭＩ６０が動作することで、制御装置１００による車両１の制御が実現される。

【0064】

２－５．フローチャート
図４は、プロセッサ１１０が実行する処理の一例を示すフローチャートである。図４に示すフローチャートは、所定の周期で繰り返し実行されて良い。

【0065】

ステップＳ１００で、プロセッサ１１０は、記憶装置１２０に格納される各種情報を取得する。

【0066】

ステップＳ１１０で、プロセッサ１１０は、自己位置推定処理を実行し、推定位置を算出する。

【0067】

ステップＳ１２０で、プロセッサ１１０は、フィルタ処理を実行し、補正推定位置を算出する。

【0068】

ステップＳ１３０で、プロセッサ１１０は、ステップＳ１１０において算出した推定位置とステップＳ１２０において算出した補正推定位置の差分を算出する。

【0069】

ステップＳ１３０において算出した差分の大きさが第１閾値を超えない場合（ステップＳ１４１；Ｎｏ）、プロセッサ１１０は、走行制御処理において通常制御を実行する（ステップＳ１５０）。

【0070】

ステップＳ１３０において算出した差分の大きさが第１閾値を超える場合（ステップＳ１４１；Ｙｅｓ）、プロセッサ１１０は、自己位置推定が精度不良であると検出する。そして、差分の大きさが第１閾値を超えて（ステップＳ１４１；Ｙｅｓ）第２閾値を超えない場合（ステップＳ１４２；Ｎｏ）、プロセッサ１１０は、警報を発する第１制御を実行する（ステップＳ１５１）。また差分の大きさが第２閾値を超えて（ステップＳ１４２；Ｙｅｓ）第３閾値を超えない場合（ステップＳ１４３；Ｎｏ）、減速による退避を行わせる第２制御を実行する（ステップＳ１５２）。また差分の大きさが第３閾値を超える場合（ステップＳ１４３；Ｙｅｓ）、プロセッサ１１０は、停止による退避を行わせる第３制御を実行する（ステップＳ１５３）。

【0071】

このように、制御装置１００は処理を実行する。またこのように制御装置１００が処理を実行することにより、車両１の制御に係る制御方法が実現される。

【0072】

３．効果
以上説明したように、本実施形態によれば、自己位置推定で得られた推定位置は、前回処理で推定された車両１の位置に対する変位量が所定範囲内となるように補正される。そして、補正により得られた補正推定位置に基づいて車両１の制御が行われる。これにより、推定位置に急激な変動が生じた場合においても車両１の制御の追従性を担保することができる。延いては、車両１のなめらかな挙動を実現することができる。さらに本実施形態によれば、推定位置と補正推定位置の差分の大きさに基づいて自己位置推定の精度不良が検出される。そして、自己位置推定の精度不良が検出されたことを受けて、差分の大きさに応じた非常制御が実行される。これにより、推定位置と補正推定位置の差分の大きさが過大であり、車両１の位置を誤ったまま制御が継続される虞がある場合に、自動運転の中断等を行うことができる。延いては、車両１の位置を誤ったまま制御が継続されることを抑止することができる。

【0073】

図５は、本実施形態に係る制御システム１０による車両１の制御の実施例を示す概念図である。図５は、車両１が自動運転により車線変更を行おうとするシーンを示している。図５では、各時刻（ｔ１，・・・，ｔ６）において算出される推定位置ＥＰと補正推定位置ＣＰが示されている。

【0074】

時刻ｔ１では、推定位置ＥＰと補正推定位置ＣＰは一致している。次に時刻ｔ２では、推定位置ＥＰと補正推定位置ＣＰは、多少の差はあるがほぼ一致している。従って、時刻ｔ１からｔ２で、制御装置１００は、通常制御により車両１の制御を行う。

【0075】

次に時刻ｔ３において、推定位置ＥＰに急激な変動が生じている。一方で、制御装置１００は、補正推定位置ＣＰに基づいて制御を行うため、急激な変動の制御への影響は抑制される。さらに時刻ｔ３において、推定位置ＥＰと補正推定位置ＣＰの差分が一定程度大きいことから、制御装置１００は、自己位置推定の精度不良を検出し非常制御により車両１を減速させる。これにより、時刻ｔ３以降において、推定位置ＥＰと補正推定位置ＣＰの差分が小さくなることが期待される。なお、このとき、制御装置１００は、車両１のオペレータへの警報を行うことにより、自己位置推定の精度不良が検出され車両１を減速させる制御が行われていることをオペレータに通知するように構成されていても良い。

【0076】

次に時刻ｔ５において、推定位置ＥＰと補正推定位置ＣＰの差分が一定程度小さくなっている。このとき、制御装置１００は、通常制御に復帰して車両１の制御を行うように構成されていて良い。あるいは、制御装置１００は、オペレータに自己位置推定の精度不良が解消されたことを通知するように構成されていても良い。この場合、オペレータが安全性を考慮して通常制御への復帰の判断を行うことが想定される。そして、制御装置１００は、オペレータによる通常制御への復帰の判断を受けて、通常制御に復帰して車両１の制御を行う。なお、時刻ｔ３以降において、さらに推定位置ＥＰと補正推定位置ＣＰの差分が大きくなる場合は、制御装置１００は、車両１の停止等の安全性を確保するための非常制御を実行して良い。

【0077】

このように、本実施形態によれば、車両１のなめらかな挙動を実現しつつ、車両１の位置を誤ったまま制御が継続されることを抑止することができる。

【符号の説明】

【0078】

１ 車両，１０ 制御システム，１００ 制御装置，１１０ プロセッサ，１２０ 記憶装置，２１０ 制御プログラム，ＥＰ 推定位置，ＣＰ 補正推定位置，ＬＰ 前回位置，ＰＲ 所定範囲

【図1】

【図2】

【図3】

【図4】

【図5】