特開 2024-64032 経路計画方法及び経路計画システム

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

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024064032

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

(54)【発明の名称】経路計画方法及び経路計画システム

(51)【国際特許分類】

   G05D 1/43 20240101AFI20240507BHJP

   B60W 30/10 20060101ALI20240507BHJP

   G01C 21/36 20060101ALI20240507BHJP

   G08G 1/16 20060101ALI20240507BHJP

【ＦＩ】

G05D1/02 H

G05D1/02 S

B60W30/10

G01C21/36

G08G1/16 C

【審査請求】未請求

【請求項の数】4

【出願形態】ＯＬ

(21)【出願番号】P 2022172323

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

(71)【出願人】

【識別番号】000003207

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

(71)【出願人】

【識別番号】504132881

【氏名又は名称】国立大学法人東京農工大学

(74)【代理人】

【識別番号】110003199

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

(72)【発明者】

【氏名】岩井 秀成

(72)【発明者】

【氏名】井上 慎太郎

(72)【発明者】

【氏名】ラクシンチャラーンサク ポンサトーン

【テーマコード（参考）】

2F129

3D241

5H181

5H301

【Ｆターム（参考）】

2F129AA03

2F129BB03

2F129BB20

2F129BB23

2F129BB26

2F129BB55

2F129DD13

2F129DD15

2F129DD53

2F129DD65

2F129EE55

2F129EE65

2F129EE67

2F129EE78

2F129EE79

2F129EE95

2F129GG04

2F129GG05

2F129GG06

2F129GG11

2F129GG17

2F129GG18

3D241BA15

3D241DB02Z

3D241DB05Z

3D241DB12Z

3D241DC46Z

3D241DC49Z

5H181AA01

5H181CC03

5H181CC04

5H181CC14

5H181EE13

5H181FF04

5H181FF14

5H181LL01

5H181LL09

5H181LL11

5H301BB05

5H301BB10

5H301BB14

5H301CC03

5H301CC06

5H301LL01

5H301LL06

5H301LL11

5H301LL15

5H301LL17

(57)【要約】

【課題】移動体の適切な目標経路を計画することができる技術を提供する。
【解決手段】移動体の目標経路が計画される。より詳細には、移動体に搭載された認識センサによる認識結果に基づいて、移動体の周囲の路面の標高を示す標高情報が取得される。標高情報に基づいて、経路候補上の評価地点に位置する移動体の複数の車輪の各々の標高が算出される。更に、複数の車輪の各々の標高に基づいて、評価地点に位置する移動体のアンダーパネルの位置が算出される。そして、標高情報とアンダーパネルの位置に基づいて、アンダーパネルが路面と接触すると予測される評価地点を通過しないように目標経路が計画される。
【選択図】図４

【特許請求の範囲】

【請求項1】

移動体の目標経路を計画する経路計画方法であって、
前記移動体に搭載された認識センサによる認識結果に基づいて、前記移動体の周囲の路面の標高を示す標高情報を取得することと、
前記標高情報に基づいて、経路候補上の評価地点に位置する前記移動体の複数の車輪の各々の標高を算出することと、
前記複数の車輪の各々の前記標高に基づいて、前記評価地点に位置する前記移動体のアンダーパネルの位置を算出することと、
前記標高情報と前記アンダーパネルの前記位置に基づいて、前記アンダーパネルが前記路面と接触すると予測される前記評価地点を通過しないように前記目標経路を計画することと
を含む
経路計画方法。

【請求項2】

請求項１に記載の経路計画方法であって、
前記複数の車輪の各々の前記標高に基づいて、前記評価地点に位置する前記移動体の傾きを算出することと、
前記移動体の前記傾きに基づいて、前記移動体が転倒すると予測される前記評価地点を通過しないように前記目標経路を計画することと
を更に含む
経路計画方法。

【請求項3】

請求項１又は２に記載の経路計画方法であって、
前記評価地点における前記移動体の速度、加速度、及びヨーレートに基づいて、前記移動体がスリップすると予測される前記評価地点を通過しないように前記目標経路を計画することを更に含む
経路計画方法。

【請求項4】

移動体の目標経路を計画する経路計画システムであって、
１又は複数のプロセッサを備え、
前記１又は複数のプロセッサは、
前記移動体に搭載された認識センサによる認識結果に基づいて、前記移動体の周囲の路面の標高を示す標高情報を取得し、
前記標高情報に基づいて、経路候補上の評価地点に位置する前記移動体の複数の車輪の各々の標高を算出し、
前記複数の車輪の各々の前記標高に基づいて、前記評価地点に位置する前記移動体のアンダーパネルの位置を算出し、
前記標高情報と前記アンダーパネルの前記位置に基づいて、前記アンダーパネルが前記路面と接触すると予測される前記評価地点を通過しないように前記目標経路を計画する
ように構成された
経路計画システム。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、移動体の目標経路を計画する技術に関する。また、本開示は、目標経路に追従するように移動体を制御する技術に関する。

【背景技術】

【0002】

特許文献１は、プラント内の既設設備の異常時に、支援設備を待機位置から所定位置に移動させるための経路を生成する経路計画システムを開示している。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】国際公開第２０１６／１５８８００号

【発明の概要】

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

【0004】

移動体の目標経路を計画し、その目標経路に追従するように移動体を制御することについて考える。このとき、移動体が問題なく通過することができる適切な目標経路を計画することが望まれる。例えば、目標経路を走行する移動体のアンダーパネルが路面と接触しないように目標経路を計画することが望まれる。

【0005】

本開示の１つの目的は、移動体の適切な目標経路を計画することができる技術を提供することにある。

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

【0006】

第１の観点は、移動体の目標経路を計画する経路計画方法に関連する。
経路計画方法は、
移動体に搭載された認識センサによる認識結果に基づいて、移動体の周囲の路面の標高を示す標高情報を取得することと、
標高情報に基づいて、経路候補上の評価地点に位置する移動体の複数の車輪の各々の標高を算出することと、
複数の車輪の各々の標高に基づいて、評価地点に位置する移動体のアンダーパネルの位置を算出することと、
標高情報とアンダーパネルの位置に基づいて、アンダーパネルが路面と接触すると予測される評価地点を通過しないように目標経路を計画することと
を含む。

【0007】

第２の観点は、移動体の目標経路を計画する経路計画システムに関連する。
経路計画システムは、１又は複数のプロセッサを備える。
１又は複数のプロセッサは、
移動体に搭載された認識センサによる認識結果に基づいて、移動体の周囲の路面の標高を示す標高情報を取得し、
標高情報に基づいて、経路候補上の評価地点に位置する移動体の複数の車輪の各々の標高を算出し、
複数の車輪の各々の標高に基づいて、評価地点に位置する移動体のアンダーパネルの位置を算出し、
標高情報とアンダーパネルの位置に基づいて、アンダーパネルが路面と接触すると予測される評価地点を通過しないように目標経路を計画する
ように構成される。

【発明の効果】

【0008】

本開示によれば、移動体の適切な目標経路を計画することが可能となる。

【図面の簡単な説明】

【0009】

【図1】車両制御の概要を説明するための概念図である。

【図2】車両制御システムの構成例を示すブロック図である。

【図3】経路計画処理の一例を説明するための概念図である。

【図4】経路計画処理の改善例を説明するための概念図である。

【図5】経路計画処理の改善例を説明するための概念図である。

【図6】経路計画処理の改善例を説明するための概念図である。

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

【0010】

添付図面を参照して、本開示の実施の形態を説明する。

【0011】

１．目標経路とリスク回避
自律的に移動可能な移動体について考える。移動体としては、車両、ロボット、飛翔体、等が例示される。車両としては、自動運転車両、月面探索車（月面ローバー）、等が例示される。ロボットとしては、物流ロボット、等が例示される。飛翔体としては、飛行機、ドローン、等が例示される。

【0012】

一例として、以下の説明においては、移動体が車両である場合について考える。一般化する場合には、以下の説明における「車両」を「移動体」で読み替え、「車両の走行」を「移動体の移動」で読み替えるものとする。

【0013】

図１は、車両１の制御の概要を説明するための概念図である。車両１は、出発地から目的地まで自律的に走行する。より詳細には、目的地に到達するための目標経路ＰＴがリアルタイムに算出され、その目標経路ＰＴに追従するように車両１は制御される。

【0014】

車両１の周囲には、車両１の走行を妨げるリスクが存在する可能性がある。典型的には、障害物が、車両１の走行を妨げるリスクとなる。そのようなリスクは、車両１に搭載された認識センサによって認識される。そして、認識センサによって認識されたリスクを回避するように目標経路ＰＴが算出される。

【0015】

車両１が未知の環境を走行する場合もある。例えば、車両１が広大な不整地（uneven surface, uneven terrain）を走行する場合もある。不整地をオフロードと呼ぶこともできる。未舗装の不整地には、白線は存在しない。その代わり、不整地には、岩石、樹木、急坂、崖、丘陵、谷、穴、陥没、クレーター等、車両１の走行を妨げるリスクが存在する可能性がある。また、光源と地形により形成される影も、認識センサによる認識精度を低下させるため、車両１の走行を妨げるリスクとなる。尚、移動体が飛翔体である場合は、洞窟等が不整地に相当する。

【0016】

リスク値Ｕ_ｒｉｓｋは、車両走行に関するリスクの大きさである。リスクポテンシャルは、リスク値Ｕ_ｒｉｓｋを位置の関数として表す。言い換えれば、リスクポテンシャルは、リスク値Ｕ_ｒｉｓｋの分布を示す。例えば、障害物ＯＢＳに関するリスクポテンシャルでは、リスク値Ｕ_ｒｉｓｋは、障害物ＯＢＳの位置で最大となり、障害物ＯＢＳから離れるにつれて小さくなる。例えば、リスクポテンシャルは、ガウス分布（正規分布）で表される。分布の分散は、障害物ＯＢＳの種類毎に異なっていてもよい。分布の分散は、車両１の速度に応じて変動してもよい。例えば、速度が高くなるほど、分布の分散は大きくなってもよい。車両１の周囲の所定範囲が格子状に区切られ、各グリッド上でリスク値Ｕ_ｒｉｓｋが与えられてもよい。その場合、リスクポテンシャルをリスクグリッドと呼ぶこともできる。

【0017】

車両１は、認識センサを用いて車両１の周囲のリスクを認識し、その認識結果に基づいて車両１の周囲のリスクポテンシャルをリアルタイムに算出する。更に、車両１は、リスクポテンシャルに基づいて、リスクを回避しつつ目的地へ向かう目標経路ＰＴをリアルタイムに算出する。そして、車両１は、目標経路ＰＴに追従するように自律的に走行する。

【0018】

２．車両制御システムの例
図２は、車両１を制御する車両制御システム１０の構成例を示すブロック図である。典型的には、車両制御システム１０は、車両１に搭載される。車両制御システム１０は、センサ群２０、走行装置３０、及び制御装置１００を含んでいる。

【0019】

センサ群２０は、車両１に搭載されている。センサ群２０は、車両１の周囲の状況を認識する認識センサ２１を含んでいる。認識センサ２１としては、カメラ、ＬＩＤＡＲ（Laser Imaging Detection and Ranging）、レーダ、等が例示される。また、センサ群２０は、車両１の状態を検出する車両状態センサ２２を含んでいる。車両状態センサ２２は、速度センサ、加速度センサ、ヨーレートセンサ、舵角センサ、等を含んでいる。

【0020】

走行装置３０は、車両１を走行させる。より詳細には、走行装置３０は、操舵装置、駆動装置、及び制動装置を含んでいる。操舵装置は、車両１の車輪を転舵する。駆動装置としては、エンジン、電動機、インホイールモータ、等が例示される。制動装置は、制動力を発生させる。走行装置３０は、キャタピラにより車両１を走行させてもよい。

【0021】

制御装置１００は、車両１を制御するコンピュータである。制御装置１００は、１又は複数のプロセッサ１１０（以下、単にプロセッサ１１０と呼ぶ）と１又は複数の記憶装置１２０（以下、単に記憶装置１２０と呼ぶ）を含んでいる。プロセッサ１１０は、各種処理を実行する。記憶装置１２０は、プロセッサ１１０による処理に必要な各種情報を格納する。

【0022】

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

【0023】

制御装置１００は、センサ群２０を用いて、車両１の運転環境を示す運転環境情報２００を取得する。運転環境情報２００は、記憶装置１２０に格納される。運転環境情報２００は、周辺状況情報、車両状態情報、地図情報、等を含んでいる。

【0024】

周辺状況情報は、認識センサ２１による認識結果を示す情報であり、車両１の周囲の状況を示す。例えば、周辺状況情報は、カメラによって撮像される画像情報を含む。他の例として、周辺状況情報は、ＬＩＤＡＲによって得られる点群情報を含む。周辺状況情報は、更に、車両１の周囲の物体に関する物体情報を含んでいる。物体は、車両１の走行を妨げる障害物を含む。物体情報は、車両１に対する物体の相対位置及び相対速度を示す。例えば、カメラによって得られた画像情報を解析することによって、物体を識別し、その物体の相対位置を算出することができる。また、ＬＩＤＡＲによって得られた点群情報に基づいて、物体を識別し、その物体の相対位置と相対速度を取得することもできる。

【0025】

車両状態情報は、車両１の状態を示す情報であり、車両状態センサ２２から得られる。車両１の状態としては、速度、加速度、ヨーレート、舵角、等が挙げられる。

【0026】

地図情報は、車両１が走行するエリアの地図である。車両１が走行するエリアは、少なくとも車両１の出発地と目的地を含んでいる。地図情報は、３次元地形情報を含んでいてもよい。車両１が広大な不整地を走行する場合、地図情報はその不整地の地図を含む。地図情報は、既知のリスクの大まかな位置を示していてもよい。

【0027】

制御装置１００は、車両１の走行を制御する車両走行制御を実行する。車両走行制御は、操舵制御、加速制御、及び減速制御を含む。制御装置１００は、走行装置３０を制御することによって車両走行制御を実行する。

【0028】

また、制御装置１００は、運転環境情報２００に基づいて自動運転制御を行う。より詳細には、制御装置１００は、運転環境情報２００に基づいて、車両１の走行プランを生成し、車両１が走行プランに従って走行するために必要な目標経路ＰＴを算出する。そして、制御装置１００は、車両１が目標経路ＰＴに追従するように車両走行制御を行う。

【0029】

尚、ＧＮＳＳ（Global Navigation Satellite System）を利用できない環境では、初期位置（出発地）に対する車両１の位置は、例えばデッドレコニングを通して把握される。具体的には、車両１の速度、舵角、等が車両状態情報から得られる。車両１が走行する路面の勾配は、加速度センサにより検出される加速度から算出可能である。速度、舵角、勾配、等に基づいて、車両１の移動距離や移動方向を算出することができる。移動距離や移動方向を一定周期毎に繰り返し算出することにより、車両１の位置が更新される。

【0030】

３．経路計画処理の例
車両１の目標経路ＰＴを計画（算出、決定）する処理を、以下、「経路計画処理」と呼ぶ。車両制御システム１０を、経路計画処理を実行する「経路計画システム」と呼ぶこともできる。

【0031】

経路計画処理において、制御装置１００は、認識センサ２１による認識結果、すなわち周辺状況情報（物体情報）に基づいて、車両１の周囲のリスクポテンシャルをリアルタイムに算出する（図１参照）。車両１の周囲に複数のリスクが存在する場合、それぞれのリスクに関するリスクポテンシャルが加算される。そして、制御装置１００は、リスクポテンシャルに基づいて、リスクを回避しつつ目的地へ向かう目標経路ＰＴをリアルタイムに算出する。

【0032】

図３は、経路計画処理の一例を説明するための概念図である。本例では、目標経路ＰＴは、「グローバル経路ＰＧ」、「準グローバル経路ＰＳ」、及び「ローカル経路ＰＬ」の三種類を含んでいる。

【0033】

グローバル経路ＰＧは、目的地への大まかな目標経路ＰＴであり、予め決定される。

【0034】

準グローバル経路ＰＳは、車両１の周囲のリスクを回避しつつ目的地へ向かう目標経路ＰＴである。つまり、準グローバル経路ＰＳは、所定のグローバル経路ＰＧになるべく沿いつつも、リスクを回避することができる目標経路ＰＴである。制御装置１００は、グローバル経路ＰＧと認識センサ２１による認識結果に基づいて、準グローバル経路ＰＳを算出することができる。図３に示されるように、例えば、準グローバル経路ＰＳは、一定間隔で並ぶ複数の経路候補点（経由点）Ｃの集合で表される。つまり、準グローバル経路ＰＳは、一定間隔で並ぶ複数の経路候補点Ｃをつなぎ合わせたものである。

【0035】

ローカル経路ＰＬは、準グローバル経路ＰＳよりも粒度の高い目標経路ＰＴである。制御装置１００は、準グローバル経路ＰＳに基づいて、より粒度の高いローカル経路ＰＬを算出する。ローカル経路ＰＬは、準グローバル経路ＰＳになるべく沿いつつも、車両１の走行安定性が確保されるように算出される。

【0036】

例えば、制御装置１００は、複数の目標ヨーレート候補を設定し、複数の目標ヨーレート候補と車速（一定と仮定）に基づいて複数の経路候補を設定する。そして、制御装置１００は、車両１の質点モデルに基づいて、複数の経路候補（目標ヨーレート候補）の各々に対する車両挙動を評価する。評価期間Ｔ_ｐ＿ｙは、車両挙動を評価する期間である。その評価期間Ｔ_ｐ＿ｙに所定数の評価地点Ｅが設定される。例えば、評価期間Ｔ_ｐ＿ｙは単位期間Δｔ_ｐ＿ｙ毎に分割され、単位期間Δｔ_ｐ＿ｙ毎に評価地点Ｅが設定される。Δｔ_ｐ＿ｙは、隣接する評価地点Ｅ間の時間間隔（タイムステップ）であると言える。制御装置１００は、各経路候補（目標ヨーレート候補）に関して、所定の評価関数に基づいて評価値を算出する。評価値は、リスク値Ｕ_ｒｉｓｋが小さくなるにつれて小さくなる。また、評価値は、準グローバル経路ＰＳと経路候補との間の偏差が小さくなるにつれて小さくなる。更に、評価値は、現在のヨーレートと目標ヨーレート候補との間の差が小さくなるにつれて、すなわち、操舵が滑らかになるにつれて小さくなる。そして、制御装置１００は、評価値が最小となる経路候補をローカル経路ＰＬとして選択する。このようにして、準グローバル経路ＰＳよりも滑らかなローカル経路ＰＬが得られる。すなわち、準グローバル経路ＰＳになるべく沿いつつ車両１の走行安定性を確保することができるローカル経路ＰＬが得られる。

【0037】

このように、制御装置１００は、所定のグローバル経路ＰＧを基準として、準グローバル経路ＰＳ及びローカル経路ＰＬを段階的に算出する。そして、制御装置１００は、ローカル経路ＰＬを目標経路ＰＴとし、ローカル経路ＰＬに追従するように車両走行制御を行う。

【0038】

４．経路計画処理の改善
経路計画処理には改善の余地がある。例えば、目標経路ＰＴを走行する車両１のアンダーパネルが路面と接触しないように目標経路ＰＴを計画することが望まれる。他の例として、目標経路ＰＴを走行する車両１が転倒しないように目標経路ＰＴを計画することが望まれる。更に他の例として、目標経路ＰＴを走行する車両１がスリップしないように目標経路ＰＴを計画することが望まれる。以下、経路計画処理の改善の様々な例について説明する。

【0039】

４－１．アンダーパネルと路面の接触の回避
図４は、車両１の４輪ばねモデルを示している。車両１は、複数の車輪２を備えている。複数の車輪２は、左前輪２ｆｌ、右前輪２ｆｒ、左後輪２ｒｌ、右後輪２ｒｒを含んでいる。更に、車両１は、アンダーパネル３を備えている。複数の車輪２とアンダーパネル３は、ばねを介して接続されている。言い換えれば、アンダーパネル３は、ばねを介して複数の車輪２によって支持されている。各ばねのばね定数は等しいとする。Ｚ_ｇは、路面（地面）の標高である。添え字ｆｌ、ｆｒ、ｒｌ、ｒｒは、それぞれ、左前輪２ｆｌ、右前輪２ｆｒ、左後輪２ｒｌ、右後輪２ｒｒを意味する。例えば、Ｚ_ｇｒｒは、右後輪２ｒｒの位置における路面の標高を意味する。

【0040】

このような車両１の４輪ばねモデルに関して、図４中の式［Ａ１］と式［Ａ２］が成り立つ。Ｚ_{ｌｏｃａｌ}は、車両固定座標系における各車輪２の標高である。例えば、Ｚ_{ｌｏｃａｌ＿ｒｒ}は、車両固定座標系における右後輪２ｒｒの標高である。各車輪２の標高Ｚ_{ｌｏｃａｌ}は、各車輪２の位置における路面の標高Ｚ_ｇから算出可能である。ΔＬは、各車輪２のばね長である。ｈ_ｍｉｎは、最低地上高である。

【0041】

図５は、原点Ｏが車両１の重心位置にある座標系における各車輪２の平面位置（ｘ，ｙ）を示している。ｌ_ｆは、原点Ｏから前輪軸までの距離である。ｌ_ｒは、原点Ｏから後輪軸までの距離である。ｄは車両１のトレッド幅である。ψは車体のヨー角である。各車輪２の平面位置（ｘ、ｙ）は、ｌ_ｆ、ｌ_ｒ、ｄ、ψに基づいて定義される。原点Ｏ（車両重心）周りのモーメントは、図５中の式［Ａ３］と式［Ａ４］で表される。上記式［Ａ１］～［Ａ４］から、各車輪２のばね長ΔＬを表す式［Ａ５］が得られる。

【0042】

再度図４を参照して、アンダーパネル３の位置を表す方程式について説明する。アンダーパネル３は平面の剛体であるとする。この場合、アンダーパネル３の位置は、図４中の式［Ａ６］で表される。ベクトルＡ、Ｂ、Ｃは、それぞれ、原点Ｏから右前輪２ｆｒ、左前輪２ｆｌ、右後輪２ｒｒの位置でのアンダーパネル３の位置へのベクトルである。

【0043】

上記式［Ａ６］を用いることによって、任意の平面位置（ｘ_ｄ，ｙ_ｄ）におけるアンダーパネル３の標高を算出することができる。一方、Ｚ_ｇ（ｘ_ｄ、ｙ_ｄ）は、平面位置（ｘ_ｄ，ｙ_ｄ）における路面の標高Ｚ_ｇである。平面位置（ｘ_ｄ，ｙ_ｄ）において車両１のアンダーパネル３が路面に接触しない条件（第１条件）は、下記式（１）で表される。

【0044】

【数1】

【0045】

制御装置１００は、以上に説明された式を利用することによって、アンダーパネル３が路面と接触すると予測される評価地点Ｅを通過しないように目標経路ＰＴを計画することが可能となる。

【0046】

より詳細には、「標高情報」は、車両１の周囲の路面（地面）の標高Ｚ_ｇを示す。制御装置１００は、上述の周辺状況情報（すなわち、認識センサ２１による認識結果）に基づいて標高情報を取得することができる。例えば、制御装置１００は、ＬＩＤＡＲによって得られた点群情報に基づいて標高情報を取得することができる。車両構成情報（ｌ_ｆ、ｌ_ｒ、ｄ、ｈ_ｍｉｎ、等）は予め与えられる。ヨー角ψは車両位置情報から得られる。

【0047】

制御装置１００は、経路候補上の評価地点Ｅ（図３参照）に車両１が位置すると仮定して、上述の式に基づいて各種パラメータを算出する。例えば、評価地点Ｅに位置する車両１の各車輪２の位置は、車両構成情報に基づいて算出される。各車輪２の位置における路面の標高Ｚ_ｇは、標高情報から得られる。各車輪２の標高Ｚ_{ｌｏｃａｌ}は、各車輪２の位置における路面の標高Ｚ_ｇから算出される。各車輪２のばね長ΔＬは、図５中の式［Ａ５］に基づいて算出される。評価地点Ｅに位置する車両１のアンダーパネル３の位置は、図４中の式［Ａ６］に基づいて算出（推定）される。評価地点Ｅの周辺の路面の標高Ｚ_ｇは、標高情報から得られる。

【0048】

そして、制御装置１００は、アンダーパネル３によってカバーされる平面位置（Ｘ_ｄ，ｙ_ｄ）において上記式（１）で表される条件が成立するか否かを判定する。第１条件が成立する場合、評価地点Ｅに位置する車両１のアンダーパネル３は路面と接触しない。一方、第１条件が成立しない場合、評価地点Ｅに位置する車両１のアンダーパネル３が路面と接触すると予測される。従って、その場合、制御装置１００は、当該評価地点Ｅを含む経路候補を採用しない。

【0049】

このように、制御装置１００は、標高情報とアンダーパネル３の位置に基づいて、アンダーパネル３が路面と接触すると予測される評価地点Ｅを通過しないように目標経路ＰＴを計画する。これにより、安全性の観点から目標経路ＰＴの精度が向上する。言い換えれば、目標経路ＰＴに沿って走行する車両１の安全性が向上する。

【0050】

４－２．転倒の回避
図６は、車両１の転倒の回避を説明するための概念図である。右輪のピッチ角θ_ｒ及び左輪のピッチ角θ_ｌは、図６中の式［Ｂ１］で表される。前輪のロール角φ_f及び後輪のロール角φ_rは、図６中の式［Ｂ２］で表される。ｌは、車両１のホイールベースである。

【0051】

図６中の式［Ｂ３」は、内輪の接地点周りのモーメントのつり合いを表している。ここで、Ｆ_{ｚ＿ｏｕｔ}は外輪の垂直抗力である。ｍは車両１の重量である。ａ_ｙは車両１の横運動により生じる横加速度である。ｇは重力加速度である。φは、前輪のロール角φ_f及び後輪のロール角φ_rのうち大きい方である。ｈは車両１の重心高さである。ｄは車両１のトレッド幅である。ｕは車両１の速度である。ｒは車両１のヨーレートである。車両１が横転しない条件は、外輪の垂直抗力Ｆ_{ｚ＿ｏｕｔ}が０より大きいことである。従って、車両１が横転しない条件（第２条件）は、下記式（２）で表される。

【0052】

【数2】

【0053】

また、上述の通り、右輪のピッチ角θ_ｒ及び左輪のピッチ角θ_ｌは、図６中の式［Ｂ１］で表される。車両１が前後方向に転倒しない条件（第３条件）は、下記式（３）で表される。θ_ｍａｘは許容最大ピッチ角である。

【0054】

【数3】

【0055】

制御装置１００は、以上に説明された式を利用することによって、車両１が転倒すると予測される評価地点Ｅを通過しないように目標経路ＰＴを計画することが可能となる。

【0056】

より詳細には、上述のセクション４－１の場合と同様に、制御装置１００は、車両１の周囲の路面（地面）の標高Ｚ_ｇを示す標高情報を取得する。車両構成情報（ｍ、ｌ、ｄ、ｈ、等）は予め与えられる。車両１の速度ｕ及びヨーレートｒは、車両状態情報及び目標経路に基づいて得られる。上述のセクション４－１の場合と同様に、制御装置１００は、経路候補上の評価地点Ｅに車両１が位置すると仮定して、上述の式に基づいて各種パラメータを算出する。すなわち、制御装置１００は、標高情報に基づいて、評価地点Ｅに位置する車両１の傾き（ロール角、ピッチ角）を算出する。そして、制御装置１００は、評価地点Ｅにおいて上記式（２）で表される第２条件及び上記式（３）で表される第３条件が成立するか否かを判定する。第２条件及び第３条件が成立する場合、評価地点Ｅにおいて車両１は転倒しない。一方、第２条件と第３条件の少なくとも一方が成立しない場合、評価地点Ｅにおいて車両１は転倒すると予測される。従って、その場合、制御装置１００は、当該評価地点Ｅを含む経路候補を採用しない。

【0057】

このように、制御装置１００は、標高情報に基づいて、評価地点Ｅに位置する車両１の傾き（ロール角、ピッチ角）を算出する。そして、制御装置１００は、車両１の傾きに基づいて、車両１が転倒すると予測される評価地点Ｅを通過しないように目標経路ＰＴを計画する。これにより、安全性の観点から目標経路ＰＴの精度が向上する。言い換えれば、目標経路ＰＴに沿って走行する車両１の安全性が向上する。

【0058】

４－３．スリップの回避
摩擦円の観点から、車両１がスリップしない条件について検討する。車両１がスリップしない条件は、「タイヤ横力と前後駆動力のベクトル和が摩擦力より小さい」ということである。摩擦力は、タイヤの垂直荷重と摩擦係数μの積で与えられる。例えば、車両１がスリップしない条件（第４条件）は、下記式（４）で与えられる。

【0059】

【数4】

【0060】

ここで、ａ_ｘ＿ｐは、評価地点Ｅにおける車両１の加速度である。θ_ｐは、評価地点Ｅにおける路面の勾配である。ｕ_ｐは、評価地点Ｅにおける車両１の速度である。ｒ_ｐは、評価地点Ｅにおける車両１のヨーレートである。加速は前輪のみで行うとする。また、マージンを考慮するため、摩擦力（μｍｇ）に１未満の係数（例：０．７５）が掛けられる。

【0061】

上述のセクション４－１の場合と同様に、制御装置１００は、経路候補上の評価地点Ｅに車両１が位置すると仮定して、上述の式に基づいて各種パラメータを算出する。評価地点Ｅにおける路面の勾配θ_ｐは、標高情報に基づいて算出される。車両１の速度ｕ_ｐ、加速度ａ_ｘ＿ｐ、及びヨーレートｒ_ｐは、車両状態情報及び目標経路に基づいて得られる。制御装置１００は、評価地点Ｅにおいて上記式（４）で表される第４条件が成立するか否かを判定する。第４条件が成立する場合、評価地点Ｅにおいて車両１はスリップしない。一方、第４条件が成立しない場合、評価地点Ｅにおいて車両１はスリップすると予測される。従って、その場合、制御装置１００は、当該評価地点Ｅを含む経路候補を採用しない。

【0062】

このように、制御装置１００は、車両１がスリップすると予測される評価地点Ｅを通過しないように目標経路ＰＴを計画する。これにより、車両１が路面をグリップして走行できることが保証される。言い換えれば、目標経路ＰＴに沿って走行する車両１の安全性が向上する。

【0063】

４－４．効果
以上に説明されたように、本実施の形態によれば、車両１の適切な目標経路ＰＴを計画することが可能となる。上述のセクション４－１～４－３で説明された観点の少なくとも一つを考慮することによって、少なくとも効果は得られる。上述のセクション４－１～４－３で説明された観点の全てが考慮されると、特に高い効果が得られる。

【符号の説明】

【0064】

１…車両， ２…車輪， ３…アンダーパネル， １０…車両制御システム， ２０…センサ群， ２１…認識センサ， ２２…車両状態センサ， ３０…走行装置， １００…制御装置， １１０…プロセッサ， １２０…記憶装置， １３０…車両制御プログラム， ２００…運転環境情報， ＰＴ…目標経路， ＰＧ…グローバル経路， ＰＳ…準グローバル経路， ＰＬ…ローカル経路

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】