JP2014026516A - 目標経路生成装置及び目標経路生成方法 - Google Patents

Abstract

【課題】自動運行ビークルの追従性の低下及び自動運行ビークルの振動を抑制する。
【解決手段】目標経路生成装置及び目標経路生成方法は、自動運行ビークルの初期位置Ｎｓから目標位置Ｎｅまでの目標経路を生成する。目標経路生成装置及び目標経路生成方法は、初期位置Ｎｓから目標位置Ｎｅまでを繋ぐ躍度最小化軌道のうち、初期位置Ｎｓ及び目標位置Ｎｅにおける曲率ρの変化率が零である躍度最小化軌道を三角関数で近似して目標経路を算出する。
【選択図】図１

Description

本発明は、目標経路生成装置及び目標経路生成方法に関する。

従来から、クロソイド曲線、直線、及び円弧を組み合わせて車両の経路を生成する方法が知られている（特許文献１参照）。クロソイド曲線は曲率ρが曲線長Ｌに比例する曲線である。クロソイド曲線、直線、及び円弧の各々は、曲率の変化率がそれぞれ一定であるため、車両の追従性が良い。このため、道路線形や自動駐車システムの経路生成に用いられている。

特開２０１０−１８０７４号公報

しかし、従来の方法では、クロソイド曲線と直線、円弧を組み合わせて経路を算出しているため、これらの線の接続点において曲率の変化率が不連続となる。したがって、クロソイド曲線と直線、曲線を組み合わせた経路を自動操舵の目標経路として用いると、その接続点において操舵サーボ系に対して大きなトルクが要求され、車両の追従性が低下し、車両に振動が発生して乗員に違和感を与えてしまう場合がある。

本発明は上記した課題に鑑みて成されたものであり、その目的は、自動運行ビークルの追従性の低下及び自動運行ビークルの振動を抑制する目標経路生成装置及び目標経路生成方法を提供することである

上記目的を達成するため、本発明の一態様に係わる目標経路生成装置及び目標経路生成方法では、初期位置から目標位置までを繋ぐ躍度最小化軌道のうち、初期位置及び目標位置における曲率の変化率が零である躍度最小化軌道を三角関数で近似して目標経路を算出する。

本発明の一態様に係わる目標経路生成装置及び目標経路生成方法によれば、クロソイド曲線と直線、曲線を組み合わせた経路に比べて、自動運行ビークルの追従性の低下及び自動運行ビークルの振動を抑制することができる。

図１は、本発明の実施形態に係わる目標経路生成装置の全体構成を示すブロック図である。 図２は、図１の経路生成部３３の詳細な構成を示すブロック図である。 図３は、図２の経路生成部３３が実行する目標経路生成方法の手順を示すフローチャートである。 図４は、図３のステップＳ０１の詳細な手順を示すフローチャートである。 図５は、障害物１０１が存在する領域を図示する障害物マップの一例を示す平面図である。 図６は、図５の障害物マップに示された障害物１０１を、正方形の凸領域１０２で分割した例を示す平面図である。 図７は、図６の凸領域１０２の重心点を母点１０３とするボロノイ図である。 図８は、図７の母点１０３をドロネー点とするドロネー図である。 図９は、図４のステップＳ１０７において選択された初期位置Ｎｓ及び目標位置Ｎｅを例示する平面図である。 図１０は、初期位置Ｎｓから目標位置Ｎｅまでを繋ぐ経路候補（Ｒ_１、Ｒ_２）を示す平面図である。 図１１は、連続する３つの経由点（Ｎ_１、Ｎ_２、Ｎ_３）を通る円Ｃｒ及び円Ｃｒに接するベクトル（Ｖ_１、Ｖ_２、Ｖ_３）を示す平面図である。 図１２は、経由点Ｎ_３について算出される３つのベクトル（Ｖａ、Ｖｂ、Ｖｃ）及び車両の姿勢角の検索範囲ＧＮを示す平面図である。 図１３は、経由点Ｎ_２の車両位置ＣＡ２から経由点Ｎ_３の車両位置ＣＡ３までのコリドーＫＬを示す平面図である。 図１４は、直線探索法（黄金分割法）を説明するためのグラフである。 図１５は、ドロネー辺１０５において経由点を移動させて経路を探索する方法を説明するための平面図である。 図１６（ａ）は、姿勢角の探索範囲ＧＮ内で直線検索法を用いて姿勢角の最適値を検索するフローチャートを示し、図１６（ｂ）は、ドロネー辺１０５を探索範囲とした直線検索法を用いて経由点の最適値を検索するフローチャートを示す。 図１７は、互いに接する２つの円弧により区分経路を近似する方法を説明するための平面図である。 図１８は、円周Ｃ_１及び円周Ｃ_２の組合せ可能な範囲を示す平面図である。 図１９は、円軌道における姿勢角θ_Ｐと行程角φ_Ｐとの関係を示す平面図である。 図２０（ａ）は、「三角波」、「躍度最小化軌道」及び「三角関数近似」の各曲線の曲率ρを示すグラフであり、図２０（ｂ）は、「三角波」、「躍度最小化軌道」及び「三角関数近似」の各曲線を示す平面図である。 図２１（ａ）は、「台形波」、「躍度最小化軌道」及び「三角関数近似」の各曲線の曲率ρを示すグラフであり、図２１（ｂ）は、「台形波」、「躍度最小化軌道」及び「三角関数近似」の各曲線を示す平面図である。 図２２は、初期位置Ｓから目標位置Ｅまでの間に任意に設定された経由点Ｗ_ｉ（ｉ＝１〜ｎ−１）を繋ぐ目標経路を示す平面図である。 図２３（ａ）は水平面ＨＰ及び鉛直面ＶＰへそれぞれ投影された３次元空間における目標経路を示すグラフであり、図２３（ｂ）は互いに接する２つの回転楕円体Ｅ_１、Ｅ_２を用いた経路近似モデルを示す立体図である。

以下図面を参照して、本発明の実施形態を説明する。図面の記載において同一部分には同一符号を付し説明を省略する。

[目標経路生成装置]
図１を参照して、実施形態に係わる目標経路生成装置の全体構成を説明する。目標経路生成装置は、車両、船舶、航空機を含む自動操舵が可能な移動体（以後、「自動運行ビークル」という）における、２次元空間或いは３次元空間における初期位置から目標位置までの目標経路を生成する装置である。実施形態では、自動運行ビークルの一例として車輪による自動操舵が可能な車両について説明する。

実施形態に係わる目標経路生成装置は、図１に示すように、目標経路生成装置全体の制御するＥＣＵ（電子制御装置）１１、外部とデータ通信を行う通信部１２、カメラやソナーを含む外界認識センサ１３、車両の乗員に対して画像情報を提供するためのモニタ１４、音声情報を提供するためのスピーカ１５、乗員からの指示入力を受け付ける入力部１６、車両の車輪の回転速度から車速を求める車輪速センサ１７、操舵角度を測定する舵角センサ１８、車輪を操舵する操舵アクチュエータ１９、車両の駆動力を制御する駆動力制御部２０、及び車両を駆動する駆動アクチュエータ２１を備える。

ＥＣＵ１１は、ＣＰＵ（中央処理装置）、ＲＯＭ、ＲＡＭ、外部記憶装置を備えるマイクロコンピュータからなり、外部記憶装置に記憶されたコンピュータプログラムを実行することにより、以下に示す機能的な構成を実現する。ＥＣＵ１１は、通信部１２及び外界認識センサ１３から得られる初期位置から目標位置までの領域の画像情報から初期位置から目標位置までの領域に存在する障害物及びその位置を認識或いは判定する認識判定部３１、後述する経路生成部３３により生成された目標経路を車両が通過するように操舵を制御するための制御信号を操舵アクチュエータ１９に送信する操舵制御部３２、認識判定部３１により認識或いは判定された障害物及びその位置に基づき目標経路を生成する経路生成部３３、車輪速センサ１７及び舵角センサ１８から得られる車輪速及び舵角から車両の現在位置を推定する自己位置推定部３４、及び車両の位置を決定するための制御信号を駆動力制御部２０へ送信する位置決め制御部３５を実現する。

通信部１２は、車両の外部に設置された固定点カメラにより撮影された画像データを受信する。カメラやソナーを含む外界認識センサ１３では得ることが出来ない画像情報を通信部１２を介して得ることができる。このため、認識判定部３１はより広い範囲における障害物及びその位置を求めることができる。経路生成部３３は、障害物及びその位置の情報から車両周囲に存在する障害物の地図（障害物マップ）を作成する。

モニタ１４は、経路生成部３３により作成された障害物マップを表示する。更に、モニタ１４は、経路生成部３３により生成された目標経路、及び自己位置推定部３４により推定された車両の現在位置を、障害物マップに重畳して表示することができる。モニタ１４の種類は特に限定されないが、タッチパネルディスプレイを適用した場合、モニタ１４の画面にソフトキーボードやその他の文字列を表示すれば、入力部１６とモニタ１４とを一体化することができる。スピーカ１５は、目標経路に沿って車両を自動運行させる際の運転操作の手順を案内する音声を出力する。

自己位置推定部３４は、車輪速センサ１７により求められた車速を積分して車両の移動距離を求め、舵角センサ１８により求められた舵角を移動距離で積分することにより車両のヨー角を求めることができる。

図２を参照して、図１の経路生成部３３の詳細な構成を説明する。経路生成部３３は、経由点設定部４１、円周近似部４２、及び三角関数近似部４３を備える。

経由点設定部４１は、初期位置から目標位置までを繋ぐ目標経路の途中で通過する少なくとも１以上の経由点及び経由点における車両の姿勢角を設定する。経由点設定部４１は、初期位置から目標位置までの間に存在する障害物を複数の凸領域に分割して各凸領域の重心点の位置を求める。そして、重心点を母点とするボロノイ図及びドロネー図を用いて少なくとも１以上の経由点及び経由点における車両の姿勢角を設定する。車両の姿勢角には、例えば、車両の進行方向が成す角度が含まれる。経由点設定部４１が実施する詳細な処理内容は、図４〜図１６を参照して後述する。

円周近似部４２は、経由点設定部４１により設定された経由点の位置、初期位置及び目標位置、及び経由点、初期位置及び目標位置の各々における車両の姿勢角に基づいて、初期位置から経由点を経由して目標位置までを繋ぐ経路を、円弧、クロソイド曲線、或いは線分の組合せにより近似する。初期位置を最初の経由点とし、目標位置を最後の経由点とした場合、円周近似部４２は、最後の経由点を除く他の経由点の各々について、経由点から次の経由点までの区分経路を１つの円弧もしくは互いに接する２つの円弧により近似する。区分経路は、初期位置から目標位置までを繋ぐ経路を経由点で区切った時の１区分に相当する。経由点から次の経由点までの区分経路を互いに接する２つの円弧により近似する「２円モデル」の具体例を、図１７〜図１８を参照して後述する。区分経路を互いに接する２つの円弧により近似した場合、円周近似部４２は、２つの円弧の接点を新たな経由点として設定し、２つの円弧の接線方向が成す角度を新たな経由点における車両の姿勢角として設定する。

三角関数近似部４３は、経由点設定部４１及び円周近似部４２により設定された経由点の位置及び経由点における車両の姿勢角に基づいて、経由点から次の経由点までの躍度最小化軌道を三角関数で近似した目標区分経路を算出する。三角関数近似部４３は、初期位置から目標位置に至る経由点の各々について、三角関数で近似した目標区分経路を算出し、これらの目標区分経路を繋ぎ合わせて、初期位置から目標位置までの目標経路を生成する。三角関数近似部４３が実施する詳細な処理内容は、図１９〜図２０を参照して後述する。

[目標経路生成方法]
図３のフローチャートを参照して、図２の経路生成部３３が実行する目標経路生成方法の手順を説明する。

先ず、ステップＳ０１において、経由点設定部４１は、障害物マップに基づいて、初期位置及び目標位置、初期位置から目標位置までの経路が通る経由点の位置、及び初期位置、目標位置、経由点における車両の姿勢角を設定する。ステップＳ０１の詳細な手順は、図４のフローチャートを参照して後述する。

ステップＳ０３に進み、円周近似部４２は、経由点から次の経由点までの区分経路を互いに接する２つの円弧により近似する。その後、ステップＳ０５に進み、三角関数近似部４３は、経由点の位置及び経由点における車両の姿勢角に基づいて、経由点から次の経由点までの躍度最小化軌道を三角関数で近似した目標区分経路を算出する。

[経由点及び姿勢角の設定]
図４〜図１６を参照して、図３のステップＳ０１の詳細な手順を説明する。先ず、ステップＳ１０１において、経由点設定部４１は、認識判定部３１により求められた障害物及びその位置に基づいて、障害物マップを作成する。作成された障害物マップの一例を図５に示す。障害物マップには、障害物１０１が存在する領域と、障害物１０１が存在しない、車両が通行可能な他の領域とが示されている。

ステップＳ１０３に進み、経由点設定部４１は、初期位置から目標位置までの間に存在する障害物を複数の凸領域に分割する。凸領域の形状としては、２次元空間の場合、正三角形、正方形、正六角形を含む正平面充填形を用いることができる。また、３次元空間の場合、凸領域の形状としては、平行六面体、切頂八面体、菱形十二面体を含む空間充填多面体を用いることができる。図６は、図５の障害物マップに示された障害物１０１を、正方形の凸領域１０２で分割した例を示す。

ステップＳ１０５に進み、経由点設定部４１は、各凸領域１０２の重心点の位置を求め、各重心点を母点１０３とするボロノイ図及びドロネー図を作成する。図７は、図６の凸領域１０２の重心点を母点１０３とするボロノイ図を示す。ボロノイ図には、ボロノイ境界１０４及びボロノイ点１０６が含まれる。ボロノイ境界１０４は、隣接する２つの母点１０３の二等分線の一部であり、ボロノイ点１０６は、２以上のボロノイ境界１０４の交点である。ボロノイ境界１０４及びボロノイ点１０６は、母点１０３に対して一義的に定まる。

ステップＳ１０５において、次に、経由点設定部４１は、図７のボロノイ図から、図８に示すドロネー図を作成する。ドロネー図には、ドロネー点、及びドロネー辺１０５が含まれる。ドロネー点は、ボロノイ図における母点１０３に相当する。ドロネー辺１０５は、ドロネー点１０３を結ぶ線分である。

ステップＳ１０７に進み、経由点設定部４１は、初期位置及び目標位置を選択する。初期位置及び目標位置は、自己位置推定部３４により推定された車両の現在位置及び入力部１６を介してユーザにより入力された目標到達位置に基づいて、ステップＳ１０５で求められたボロノイ点１０６の中からそれぞれ選択される。例えば、車両の現在位置及び目標到達位置の各々に最寄りのボロノイ点１０６を初期位置Ｎｓ及び目標位置Ｎｅとすることができる。図９には、ステップＳ１０７において選択された初期位置Ｎｓ及び目標位置Ｎｅを例示する。また、経由点設定部４１は、初期位置及び目標位置の各々における姿勢角を設定することができる。

ステップＳ１０９に進み、経由点設定部４１は、図１０に示すように、初期位置Ｎｓから目標位置Ｎｅまでを繋ぐ経路候補（Ｒ_１、Ｒ_２）を算出する。ただし、経路候補（Ｒ_１、Ｒ_２）は、障害物１０１が存在しない領域にあるボロノイ点１０６を経由点（ノード）とし、障害物１０１が存在しない領域にあるボロノイ境界１０４をブランチとする。経由点設定部４１は、算出された経路候補（Ｒ_１、Ｒ_２）の各々について、経路候補（Ｒ_１、Ｒ_２）が通過する経由点のリスト（ノードリスト）を作成することができる。

ステップＳ１１１に進み、経由点設定部４１は、複数の経路候補（Ｒ_１、Ｒ_２）が存在する場合、複数の経路候補（Ｒ_１、Ｒ_２）の中から、最短経路を探索してこれを最適経路として決定する。最短経路の探索には、ダイキストラ法或いはＡ^＊（エースター）検索アルゴリズムを含む経路検索アルゴリズムを用いることができる。図１０に示す例において、経由点設定部４１は、複数の経路候補（Ｒ_１、Ｒ_２）の中から、経路候補Ｒ_１を最適経路として決定する。

ステップＳ１１３に進み、経由点設定部４１は、最適経路Ｒ_１上の各経由点における車両の姿勢角の検索範囲ＧＮを算出する。具体的には、先ず、経由点設定部４１は、図１１に示すように、連続する３つの経由点（Ｎ_１、Ｎ_２、Ｎ_３）を通る円Ｃｒを算出する。そして、３つの経由点（Ｎ_１、Ｎ_２、Ｎ_３）において円Ｃｒに接するベクトル（Ｖ_１、Ｖ_２、Ｖ_３）を算出する。但し、連続する３つの経由点が同一直線上に存在するとき、経由点設定部４１は、その直線を３つの経由点におけるベクトルとして算出する。円Ｃｒ及びベクトル（Ｖ_１、Ｖ_２、Ｖ_３）の算出を各経由点について実行することにより、図１２に示すように、各経由点（経由点Ｎ_３）について３つのベクトル（Ｖａ、Ｖｂ、Ｖｃ）が算出される。経由点設定部４１は、３つのベクトル（Ｖａ、Ｖｂ、Ｖｃ）が含まれる角度範囲を、車両の姿勢角の検索範囲ＧＮとして算出する。

ステップＳ１１５に進み、経由点設定部４１は、最適経路Ｒ_１上で連続する経由点の間で、姿勢角の変化が最小となるように、各経由点における姿勢角の組み合わせを算出する。この時、初期位置Ｎｓにおける初期姿勢角と目標位置Ｎｅにおける目標姿勢角を拘束条件とする。算出された各経由点における姿勢角を、姿勢角の初期解とする。経由点設定部４１は、各経由点における姿勢角の初期解に基づいて、各経由点間を車両が追従可能な滑らかな経路で接続する。

ステップＳ１１７に進み、経由点設定部４１は、車両が追従可能な滑らかな経路に対して、移動する車両の軌跡（コリドー）を算出し、図１３に示すように、コリドーＫＬと障害物が存在する凸領域１０２とが衝突するか否かを判定する。図１３は、経由点Ｎ_２の車両位置ＣＡ２から経由点Ｎ_３の車両位置ＣＡ３までのコリドーＫＬを示す。コリドーＫＬが凸領域１０２に衝突せずに次の経由点に至る場合（Ｓ１１９でＹＥＳ）、ステップＳ１２３に進み、次の経由点についてもコリドーＫＬの算出及び衝突判定を行う。一方、コリドーＫＬと凸領域１０２が衝突する場合（Ｓ１１９でＮＯ）、経由点設定部４１は、車両の姿勢角の検索範囲ＧＮ内において姿勢角を変更して（Ｓ１２１でＮＯ）、再度、コリドーＫＬの算出及び衝突判定を行う（Ｓ１１７）。このようにして、姿勢角の探索範囲ＧＮにおいて姿勢角を変更しながら、衝突が起こらない経路を検索する。この作業を初期位置Ｎｓから目標位置Ｎｅまでの各経由点間について実施する。

また、コリドーＫＬが凸領域１０２と衝突せずに次の経由点に至る滑らかな経路が複数ある場合、コリドーＫＬを算出する６点と凸領域１０２の最近点との距離の和の移動経路長に関する積分値が最大となる経路を選択する。

車両の姿勢角の検索範囲ＧＮ内において、凸領域１０２と衝突せずに次の経由点に至る滑らかな経路が見つからない場合（Ｓ１２１でＹＥＳ）、経路を生成することができないと判断して（Ｓ１２５）、図４のフローチャートは終了する。

以上の手順を経て、経由点設定部４１は、初期位置Ｎｓから目標位置Ｎｅまでの最適経路Ｒ_１が通る経由点の位置、及び各経由点における車両の姿勢角を決定することができる。

[ドロネー辺上の直線検索]
なお、図４のフローチャートの変形例として、車両の姿勢角の検索範囲ＧＮ内において、凸領域１０２と衝突せずに次の経由点に至る滑らかな経路が見つからない場合（Ｓ１２１でＹＥＳ）、ドロネー辺１０５において経由点を移動させて経路を探索することができる。

この検索では、直線検索法（黄金分割法）を用いる。与えられた制約条件の下で、ある目的関数を最大または最小にする解を求める事を一般に最適化問題と呼ぶ。この最適化問題において，探索範囲内の最適値を反復的に探索する手法の一つとして直線探索法（黄金分割法）が挙げられる。

図１４を参照して直線探索法（黄金分割法）を説明する。与えられた探索範囲（図９ではｘ_ｍｉｎ≦ｘ≦ｘ_ｍａｘ）内で目的関数ｆ(ｘ)を最小にするｘを探索する際に、探索範囲（ｘ_ｍｉｎ，ｘ_ｍａｘ）を黄金分割比１：（１＋５^1/2）／２で分割した値ｘ_１を算出し、探索範囲（ｘ_ｍｉｎ，ｘ_ｍａｘ）を黄金分割比で分割した値ｘ_２を算出する。この処理を繰り返すことによってｆ（ｘ）の最小値（最適値）を探索する。直線探索法（黄金分割法）は、そのアルゴリズムが単純で効率的であるため、最適化問題を扱う際によく用いられる。

図１５を参照して、ドロネー辺１０５において経由点を移動させて経路を探索する方法を説明する。経由点Ｎ_２において、凸領域１０２と衝突せずに次の経由点Ｎ_３まで至る滑らかな経路が見つからない場合、経由点Ｎ_２とその次の経由点Ｎ_３との間を横切るドロネー辺１０５上を探索範囲として、経由点Ｎ_２を移動して経路を探索する。この際、先に述べた直線探索法を用いて探索範囲を分割して最適値を探索する。図１５中の符号ＣＡ２は経由点Ｎ_２における車両の外形を示し、符号ＣＡ２１は経由点Ｎ_２１における車両の外形を示し、符号ＣＡ２２は経由点Ｎ_２２における車両の外形を示す。

図１６（ａ）は姿勢角の探索範囲ＧＮ内で直線検索法を用いて姿勢角の最適値を検索するフローチャートを示し、図１６（ｂ）はドロネー辺１０５を探索範囲とした直線検索法を用いて経由点の最適値を検索するフローチャートを示す。姿勢角の探索範囲ＧＮ内において滑らかな経路の範囲を算出し（Ｓ２０１）、凸領域１０２との衝突が無い経路（解）が存在する場合（Ｓ２０３でＹＥＳ）、直線検索を行うことにより車両が追従可能な滑らかな経路を探索する（Ｓ２０５）。この時、ドロネー辺上で経由点を移動させることはない。

一方、車両の姿勢角の検索範囲ＧＮ内において、凸領域１０２と衝突せずに次の経由点に至る滑らかな経路が見つからない場合（Ｓ３０５でＹＥＳ）、ステップＳ３０９に進み、図１５に示したように、経由点Ｎ_２とその次の経由点Ｎ_３との間を横切るドロネー辺１０５上を探索範囲として経由点Ｎ_２を移動させて、凸領域１０２と衝突せずに次の経由点に至る滑らかな経路を探索する。

なお、経由点Ｎ_２において姿勢角の検索範囲ＧＮ内を直線探索して経路を求める図１６（ａ）のループと、経由点Ｎ_２を経由する経路が存在しない場合にドロネー辺１０５上で直線探索して経由点Ｎ_２を変更する図１６（ｂ）のループとが入れ子構造になっている。つまり、姿勢角の検索範囲ＧＮ内を直線探索して解が求まらない場合（図１６（ａ）のループで解無しの場合）、ドロネー辺１０５上を探索範囲として経由点Ｎ_２を移動させる（（図１６（ｂ）のループを実行する）。その上で、再度、図１６（ａ）のループに戻り、姿勢角の検索範囲ＧＮ内を直線探索する。また、図１６（ａ）及び図１６（ｂ）に示す処理は、図４のステップＳ１１７からＳ１２５までの処理に対応している。

[２円モデルによる区分経路近似]
次に、図１７及び図１８を参照して、経由点から次の経由点までの区分経路を互いに接する２つの円弧により近似する「２円モデル」の具体例を説明する。

図１７に示すように、経由点をＸＹ座標軸における原点Ｏ（０，０）とし、経由点における車両の姿勢角をθ_０＝０とする。一方、次の経由点を座標Ｑ（Ｘ_Ｑ，Ｙ_Ｑ）とし、経由点における車両の姿勢角をθ_Ｑとする。ここでは、車両の姿勢角θは、Ｘ軸の正方向に対して成す角度を示す。また、図１７において、原点Ｏ及び座標Ｑから伸びる矢印は、それぞれ車両の姿勢角を示すベクトル（姿勢角ベクトル）である。

原点Ｏから座標Ｑに至る経路は、点Ｐで接する２つの円Ｃ_１、Ｃ_２の一部分を構成する円弧ＯＰ及び円弧ＰＱにより近似することができる。円Ｃ_１は、原点Ｏにおける車両の姿勢角ベクトルに接し、円Ｃ_２は、座標Ｑにおける車両の姿勢角ベクトルに接する。

円Ｃ_１の半径をＲ_１とすると、円Ｃ_１の中心ＲはＹ軸上の点Ｒ（０，Ｒ_１）である。円Ｃ_２の半径をＲ_２とすると、円Ｃ_２の中心Ｓの座標は、（Ｘ_Ｑ±Ｒ_２ｓｉｎθ_Ｑ，Ｙ_Ｑ±Ｒ_２ｃｏｓθ_Ｑ）と表せる。なお、中心Ｓの座標において、Ｘ座標の±が＋符号である場合、Ｙ座標の±は−符号となり、Ｘ座標の±が−符号である場合、Ｙ座標の±は＋符号となる。よって、２つの円Ｃ_１、Ｃ_２が点Ｐで接する場合、円Ｃ_１の中心Ｒから円Ｃ_２の中心Ｓまでの距離（ＲＳ）について、（１）式が成り立つ。

（１）式をＲ_１及びＲ_２に関してまとめ、式を変形すると、（２）式及び（３）式が得られる。

車両の最小旋回半径をＲ_ｍｉｎとした場合、（２）式の右辺のＲ_２にＲ_ｍｉｎを代入し、（３）式の右辺のＲ_１にＲ_ｍｉｎを代入すると、Ｒ_１及びＲ_２がとり得る範囲を（４）式でそれぞれ表すことができる。

円周Ｃ_２の半径Ｒ_２にＲ_ｍｉｎを代入した場合、図１８に示す円周Ｃ_１及び円周Ｃ_２の組合せによって原点Ｏから座標Ｑまでの経路を近似することができる。円周Ｃ_１及び円周Ｃ_２の接点をＰ_２とする。一方、円周Ｃ_１の半径Ｒ_１にＲ_ｍｉｎを代入した場合、図１８に示す円周Ｃ_１’及び円周Ｃ_２’の組合せによって原点Ｏから座標Ｑまでの経路を近似することができる。円周Ｃ_１’及び円周Ｃ_２’の接点をＰ_１とする。円周近似部４２は、円周Ｃ_１及び円周Ｃ_２の組合せから、円周Ｃ_１’及び円周Ｃ_２’の組合せまでの範囲内において、原点Ｏから座標Ｑまでの経路を近似することができる。接点Ｐ_１と接点Ｐ_２とを両端とする線ｌ_ｐを接点Ｐの解曲線と呼ぶ。円周近似部４２は、解曲線ｌ_ｐ上において接点Ｐを設定することができる。

Ｒ_１／（Ｒ_１＋Ｒ_２）＝βとした場合、この式に（３）式を代入することにより、（５）式を得ることができる。

また、円Ｃ_２の中心Ｓの座標（Ｘ_Ｓ＝Ｘ_Ｑ±Ｒ_２ｓｉｎθ_Ｑ，Ｙ_Ｓ＝Ｙ_Ｑ±Ｒ_２ｃｏｓθ_Ｑ）に（３）式を代入することにより、（６）式が得られる。なお、中心Ｓの座標において、Ｘ座標の±が＋符号である場合、Ｙ座標の±は−符号となり、Ｘ座標の±が−符号である場合、Ｙ座標の±は＋符号となる。

したがって、円周Ｃ_１と円周Ｃ_２の接点Ｐの座標（βＸ_Ｓ，βＹ_Ｓ＋（１−β）Ｒ_１）は、（５）式及び（６）式を用いることにより、半径Ｒ_１の関数として表すことができる。

また、図１９に示すように、円軌道における姿勢角θ_Ｐと行程角φ_Ｐの間にはθ_Ｐ＝２φ_Ｐの関係がある。この関係を利用すると、接点Ｐの座標（βＸ_Ｓ，βＹ_Ｓ＋（１−β）Ｒ_１）から(７)式を得ることができる。（７）式に（５）式を代入することにより、円周Ｃ_１及び円周Ｃ_２の接点Ｐにおける車両の姿勢角θ_Ｐを半径Ｒ_１の関数として表すことができる。

なお、図１７には、経由点をＸＹ座標軸における原点Ｏ（０，０）とし、経由点における車両の姿勢角をθ_０＝０とした場合に、次の経由点の座標Ｑ（Ｘ_Ｑ，Ｙ_Ｑ）が、ＸＹ座標軸の第１象限に位置する場合を説明した。この場合、原点Ｏにおける姿勢角ベクトルに接する２つの円周のうち、第１象限或いは第２象限側の円周を用い、座標Ｑにおける姿勢角ベクトルに接する２つの円周のうち、第３象限或いは第４象限側の円周を用いる。座標Ｑ（Ｘ_Ｑ，Ｙ_Ｑ）が、ＸＹ座標軸の第２象限に位置する場合も同様である。これに対して、座標Ｑ（Ｘ_Ｑ，Ｙ_Ｑ）が、ＸＹ座標軸の第３象限或いは第４象限に位置する場合、原点Ｏにおける姿勢角ベクトルに接する２つの円周のうち、第３象限或いは第４象限側の円周を用い、座標Ｑにおける姿勢角ベクトルに接する２つの円周のうち、第１象限或いは第２象限側の円周を用いることができる。

以上説明したように、円周近似部４２は、経由点から次の経由点までの区分経路を互いに接する２つの円弧により近似し、２つの円弧の接点Ｐの位置及び接点Ｐにおける車両の姿勢角θ_Ｐを設定することができる。そして、円周近似部４２は、接点Ｐを新たな経由点として設定し、２つの円弧の接線方向を新たな経由点における車両の姿勢角θ_Ｐとして設定する。

ただし、２つの円弧軌道の組み合わせでは、接点Ｐにおいて軌道の曲率が不連続に変化する。したがって、２つの円弧軌道の組み合わせた区分経路を自動操舵の目標区分経路として用いると、その接続点において操舵サーボ系に対して大きなトルクが要求され、車両の追従性が低下し、車両に振動が発生して乗員に違和感を与えてしまう場合がある。そこで、三角関数近似部４３は、以下に述べる手順により、座標Ｏから接点Ｐまでの間、及び接点Ｐから座標Ｑまでの間を、躍度最小化軌道を三角関数で近似した曲線でそれぞれ接続する。これにより、座標Ｏから座標Ｑまでの経路を、車両が追従可能な、曲率が滑らかに変化する曲線で表すことができる。

[三角関数で近似した目標区分経路の算出]
接点Ｐにおいて曲率の変化率が不連続となるという問題は、座標Ｏ及び接点Ｐにおいて曲率の変化率が零とする拘束条件を付加した躍度最小化軌道を用いることにより解決される。しかし、その代償として、大きく旋回する軌道が生成されてしまい、円弧やクロソイド曲線に比べて軌道が膨らむという問題が生じる。この問題は、障害物に接触することなく目標位置へ至る経路の自由度を狭めてしまう。そこで、三角関数近似部４３は、経由点（例えば、座標Ｏ）から次の経由点（例えば、接点Ｐ）までを繋ぐ躍度最小化軌道のうち、経由点及び次の経由点における曲率の変化率が零である躍度最小化軌道を、三角関数で近似して目標区分経路を算出する。これにより、追従性悪化や振動が発生を回避し、大きな旋回を伴う軌道において軌道が膨らむことを抑制することが可能となる。

図１９の原点Ｏ、原点Ｏにおける姿勢角θ_Ｏ＝０°から接点Ｐ、接点Ｐにおける姿勢角θ_Ｐ＝９０°に至る曲線のうち、原点Ｏ及び接点Ｐにおいて車両の操舵角が零である曲線を考える。図２０（ｂ）は、これらの曲線の曲線長Ｌに対する曲線の曲率ρを示すグラフである。図２０（ａ）は、これらの曲線を示すグラフである。

図２０（ｂ）の「三角波」は、曲率ρが曲線長Ｌに比例する２つのクロソイド曲線を組み合わせたものの曲率を示す。具体的には、図２０（ｂ）の「三角波」は、「Ｂ１」から「Ｂ２」までの間のクロソイド曲線と、「Ｂ２」から「Ｂ３」までの間のクロソイド曲線とを組み合わせたものである。「Ｂ１」、「Ｂ２」及び「Ｂ３」において曲率の変化率が不連続となる。ただし、図２０（ａ）に示すように、以下に述べる他の２つの曲線に比べて小回りな軌道となる。

図２０（ｂ）の「躍度最小化軌道」は、原点Ｏ及び接点Ｐにおいて曲率の変化率が零である躍度最小化軌道の曲率を示す。原点Ｏ及び接点Ｐにおいて曲率の変化率は連続しているが、「三角波」に比べて曲率ρの最大値が大きくなる。よって、図２０（ａ）に示すように、他の２つの曲線に比べて大回りな軌道となる。

これに対して、図２０（ｂ）の「三角関数近似」は、原点Ｏ及び接点Ｐにおいて曲率の変化率が零である躍度最小化軌道を三角関数で近似した曲線の曲率を示す。原点Ｏ及び接点Ｐにおける曲率ρの変化率は、「三角波」に比べて緩やかであり、且つ「躍度最小化軌道」に比べて曲率ρの最大値が小さくなる。よって、図２０（ａ）に示すように、以下に示す「躍度最小化軌道」に比べて小回りな軌道となる。詳細には、「三角波」の曲線を基準として、軌道の膨らみを「躍度最小化軌道」の約半分程度に低減することができる。小旋回半径で大きな旋回量が必要な縦列駐車の目標軌道においては、この差による生成可能な軌道の自由度が与える影響は大きい。このように、図２０（ｂ）の「三角関数近似」は、他の２つの曲線である「三角波」及び「躍度最小化軌道」の両方の利点を兼ね備えた曲線であると言える。

図２０（ｂ）の「躍度最小化軌道」に示す曲率ρは、（８）式に示すように、曲線長（ｌ）の関数として表すことができる。ここで、「Ｌ」は、原点Ｏから接点Ｐまでの曲線全体の長さを示す。

図２０（ｂ）の「三角関数近似」も、（９）式に示すように、曲線長（ｌ）の関数として表すことができる。（９）式は、（８）式の一部を三角関数の一例としての余弦関数で近似したものである。

なお、図２０（ｂ）の「三角波」、「躍度最小化軌道」、及び「三角関数近似」の３つの曲線とグラフの縦軸とで囲まれる面積は、（１０）式で表され、それぞれの曲線について全て同じ値となる。すなわち、原点Ｏから接点Ｐまで進んだ時のヨー角（姿勢角の差θ_Ｐ−θ_Ｏ）は、３つの曲線について同じになる。

次に、図２１（ｂ）に示すように、原点Ｏと接点Ｐの間で最大曲率を維持する部分、つまり一定の操舵角を保持する部分（操舵角保持部分ＲＣ）が含まれる曲線を考える。この場合、三角関数近似部４３は、原点Ｏから操舵角保持部分ＲＣの開始点まで、つまり図２１（ｂ）の「Ｂ４」から「Ｂ５」までを繋ぐ躍度最小化軌道のうち、原点Ｏ及び操舵角保持部分ＲＣの開始点における曲率の変化率が零である躍度最小化軌道を、三角関数で近似して目標区分経路を算出する。そして、三角関数近似部４３は、操舵角保持部分ＲＣの終了点から接点Ｐまで、つまり図２１（ｂ）の「Ｂ６」から「Ｂ７」までを繋ぐ躍度最小化軌道のうち、操舵角保持部分ＲＣの終了点及び接点Ｐにおける曲率の変化率が零である躍度最小化軌道を、三角関数で近似して目標区分経路を算出する。そして、これらの目標区分経路を操舵角保持部分ＲＣを介して繋ぎ合わせることにより、原点Ｏから接点Ｐまでの目標区分経路が生成される。

図２１（ｂ）の「台形波」は、曲率ρが曲線長Ｌに比例するクロソイド曲線と円弧を組み合わせたものの曲率を示す。具体的には、図２１（ｂ）の「台形波」は、「Ｂ４」から「Ｂ５」までの間のクロソイド曲線と、「Ｂ５」から「Ｂ６」までの間の円弧と、「Ｂ６」から「Ｂ７」までの間のクロソイド曲線とを組み合わせたものである。「Ｂ４」〜「Ｂ７」において曲率の変化率が不連続となる。

図２１（ｂ）の「躍度最小化軌道」は、原点Ｏ、操舵角保持部分ＲＣの開始点及び終了点、及び接点Ｐにおいて曲率の変化率が零である躍度最小化軌道の曲率を示す。操舵角保持部分ＲＣの曲率は「台形波」と同じである。原点Ｏから接点Ｐまでの間で曲率の変化率は連続する。また、曲率ρの最大値は、「台形波」と同じになる。

これに対して、図２１（ｂ）の「三角関数近似」は、原点Ｏから操舵角保持部分ＲＣの開始点までの躍度最小化軌道、及び操舵角保持部分ＲＣの終了点から接点Ｐまでの躍度最小化軌道をそれぞれ三角関数で近似した曲線の曲率を示す。「Ｂ４」〜「Ｂ７」における曲率ρの変化率は、「台形波」に比べて緩やかである。

図２１（ｂ）の「躍度最小化軌道」のうち、原点Ｏから操舵角保持部分ＲＣの開始点までの躍度最小化軌道の曲率ρは、（１１）式で表すことができる。ここで、「ρ_ｍａｘ」は操舵角保持部分ＲＣの最大曲率を示し、「Ｌ_１」は原点Ｏから操舵角保持部分ＲＣの開始点までの曲線長を示す。

図２１（ｂ）の「三角関数近似」のうち、原点Ｏから操舵角保持部分ＲＣの開始点までの曲率ρは、（１２）式で表すことができる。（１２）式は、（１１）式の一部を三角関数の他の例としての正弦関数で近似したものである。

以上説明したように、三角関数近似部４３は、経由点設定部４１及び円周近似部４２により設定された経由点の位置及び経由点における車両の姿勢角に基づいて、経由点から次の経由点までの躍度最小化軌道を三角関数で近似した目標区分経路を算出することができる。そして、三角関数近似部４３は、最後の経由点を除く他の経由点の各々について目標区分経路を算出し、これらの目標区分経路を繋ぎ合わせる。これにより、図１０に示した初期位置Ｎｓから最適経路Ｒ_１上の各経由点を経由して目標位置Ｎｅまで到達する目標経路を生成することができる。

なお、実施形態では、初期位置Ｎｓから目標位置Ｎｅの間に１以上の経由点を設定し、経由点毎に目標区分経路を算出し、これらの目標区分経路を繋ぎ合わせて１つの目標経路を生成した。しかし、これに限らず、初期位置Ｎｓと目標位置Ｎｅとの間に経由点を設けることなく、三角関数近似部４３が、初期位置Ｎｓと目標位置Ｎｅの間を繋ぐ目標経路を直接、生成することも可能である。この場合、図２の経路生成部３３において、経由点設定部４１は不要である。

また、図２２に示すように、初期位置Ｓから目標位置Ｅまでを繋ぐ任意の経路において、その経路が経由するｎ−１個の経由点をＷ_ｉ（ｉ＝１〜ｎ−１）とし、初期位置Ｓを最初の経由点Ｗ０とし、目標位置Ｅを最後の経由点Ｗｎとする。総ての経由点Ｗ_０〜Ｗ_ｎにおける姿勢角θ_ｉが与えられた場合、図２の経路生成部３３は、図２２に示すように、それぞれの経由点Ｗ_ｉ間をつなぐ目標区分経路を生成し、これらを繋ぎ合わせた任意の目標経路を生成することができる。

更に、航空機のような３次元空間内を移動する自動運行ビークルでは、一般に水平面上のダイナミクスと鉛直面上のダイナミクスが異なる。そのため、図２３（ａ）に示すように３次元空間における目標経路を生成する場合、水平面（ＸＹ平面）ＨＰと鉛直面（ＹＺ平面）ＶＰでは異なる曲率の変化率を用いる必要がある。ここで、先に述べた２次元平面における目標経路の生成を３次元空間における目標経路へ拡張する場合には、３次元空間における目標経路を水平面ＨＰ及び鉛直面ＶＰへそれぞれ投影し、水平面ＨＰ及び鉛直面ＶＰの各々において、２次元平面における目標経路の生成処理を実行すればよい。具体的には、三角関数近似部４３は、先ず、３次元空間における初期位置Ｎｓ及び目標位置Ｎｅを鉛直面及び水平面の各々へ投影する。そして、投影された鉛直面及び水平面の各々について、初期位置Ｎｓ及び目標位置Ｎｅにおける曲率の変化率を零とする躍度最小化軌道を三角関数でそれぞれ近似して目標投影経路を算出する。そして、これらの目標投影経路を合成して３次元空間における目標経路を算出する。

また、３次元空間の場合、円周近似部４２は、互いに接する２つの円周の代わりに、互いに接する２つの回転楕円体Ｅ_１、Ｅ_２を用いて原点Ｏから座標Ｑまでの経路を近似する。２つの回転楕円体Ｅ_１、Ｅ_２は、自動運行ビークルの鉛直方向のダイナミクスおよび水平方向のダイナミクスに基づいて決定した長軸及び短軸を持つ楕円体である。２つの回転楕円体Ｅ_１、Ｅ_２が接する接点Ｐ上には、接点Ｐを含み、且つ２つの回転楕円体Ｅ_１、Ｅ_２が接する平面Ｓを設定することができる。そして、接点Ｐにおける自動運行ビークルの姿勢角ベクトルを、平面Ｓ内に設定することができる。

以上説明したように、実施形態によれば、以下の作用効果が得られる。

三角関数近似部４３は、初期位置Ｎｓから目標位置Ｎｅまでを繋ぐ躍度最小化軌道のうち、初期位置Ｎｓ及び目標位置Ｎｅにおける曲率ρの変化率が零である躍度最小化軌道を、三角関数で近似して目標経路を算出することができる。これにより、クロソイド曲線と円弧、直線を組み合わせた経路に比べて、車両の追従性の低下や車両の振動の発生を回避することができる。また、初期位置Ｎｓ及び目標位置Ｎｅにおける曲率ρの変化率が零である躍度最小化軌道に比べて、軌道が膨らむことを抑制することができる。

三角関数近似部４３は、躍度最小化軌道を正弦関数で近似して目標経路を算出することができる。これにより、操舵角保持部分ＲＣを含む場合において、車両の追従性の低下や車両の振動の発生を回避し、軌道の膨らみを抑制することができる。

三角関数近似部４３は、躍度最小化軌道を余弦関数で近似して目標経路を算出することができる。これにより、操舵角保持部分ＲＣを含まない場合において、車両の追従性の低下や車両の振動の発生を回避し、軌道の膨らみを抑制することができる。

目標経路は初期位置Ｎｓから目標位置Ｎｅまでの間に少なくとも１以上の経由点を通り、初期位置Ｎｓは最初の経由点であり、目標位置Ｎｅは最後の経由点である場合、三角関数近似部４３は、初期位置から目標位置に至る経由点の各々について、経由点から次の経由点までの躍度最小化軌道を三角関数で近似して目標区分経路を算出し、目標区分経路を繋ぎ合わせて目標経路を生成する。これにより、任意に設定された１以上の経由点を通る目標経路を設定することができる。

目標経路は３次元空間における目標経路であり、三角関数近似部４３は、初期位置Ｎｓ及び目標位置Ｎｅが投影された鉛直面及び水平面の各々について、初期位置Ｎｓ及び目標位置Ｎｅにおける曲率ρの変化率を零とする躍度最小化軌道を三角関数でそれぞれ近似して目標投影経路を算出することができる。そして、目標投影経路を合成して３次元空間における目標経路を算出することができる。これにより、３次元空間における目標経路の生成においても、自動運行ビークルの追従性の低下や自動運行ビークルの振動の発生を回避し、軌道の膨らみを抑制することができる。

目標経路生成装置は、少なくとも１以上の経由点及び経由点における自動運行ビークルの姿勢角を設定する経由点設定部４１を更に備えていてもよい。経由点設定部４１は、初期位置Ｎｓから目標位置Ｎｅまでの間に存在する障害物を複数の凸領域１０２に分割して各凸領域１０２の重心点の位置を求め、重心点を母点１０３とするボロノイ図及びドロネー図を用いて経由点及び自動運行ビークルの姿勢角を設定することができる。これにより、任意に作成された障害物マップに対して、障害物１０１と衝突せずに初期位置Ｎｓから目標位置Ｎｅまで到達する最適経路Ｒ_１を検索することができる。そして、最適経路Ｒ_１が経由する経由点及び各経由点における姿勢角を設定することができる。

以上、実施形態に沿って本発明の内容を説明したが、本発明はこれらの記載に限定されるものではなく、種々の変形及び改良が可能であることは、当業者には自明である。

３３…経路生成部
４１…経由点設定部
４２…円周近似部
４３…三角関数近似部
１０１…障害物
１０２…凸領域
１０３…ドロネー点（母点）
１０４…ボロノイ境界
１０５…ドロネー辺
１０６…ボロノイ点
θ…姿勢角
ＨＰ…水平面
Ｎｅ…目標位置
Ｎｓ…初期位置
ＶＰ…鉛直面

Claims (7)

1. 自動運行ビークルの初期位置から目標位置までの目標経路を生成する目標経路生成装置であって、
   初期位置から目標位置までを繋ぐ躍度最小化軌道のうち、前記初期位置及び目標位置における曲率の変化率が零である躍度最小化軌道を、三角関数で近似して前記目標経路を算出する三角関数近似部
   を備えることを特徴とする目標経路生成装置。
2. 請求項１に記載の目標経路生成装置において、
   前記三角関数近似部は、前記躍度最小化軌道を正弦関数で近似して前記目標経路を算出することを特徴とする目標経路生成装置。
3. 請求項１に記載の目標経路生成装置において、
   前記三角関数近似部は、前記躍度最小化軌道を余弦関数で近似して前記目標経路を算出することを特徴とする目標経路生成装置。
4. 請求項１〜請求項３のいずれか一項に記載の目標経路生成装置において、
   前記目標経路は前記初期位置から前記目標位置までの間に少なくとも１以上の経由点を通り、前記初期位置は最初の経由点であり、前記目標位置は最後の経由点であり、
   前記三角関数近似部は、前記最後の経由点を除く他の経由点の各々について、経由点から次の経由点までの前記躍度最小化軌道を三角関数で近似して前記目標区分経路を算出し、前記目標区分経路を繋ぎ合わせて前記目標経路を生成する
   ことを特徴とする目標経路生成装置。
5. 請求項１〜請求項４のいずれか一項に記載の目標経路生成装置において、
   前記目標経路は３次元空間における目標経路であり、
   前記三角関数近似部は、前記初期位置及び目標位置が投影された鉛直面及び水平面の各々について、前記初期位置及び目標位置における曲率の変化率を零とする躍度最小化軌道を三角関数でそれぞれ近似して目標投影経路を算出し、前記目標投影経路を合成して前記３次元空間における目標経路を算出する
   ことを特徴とする目標経路生成装置。
6. 請求項４に記載の目標経路生成装置において、
   前記少なくとも１以上の経由点及び前記経由点における前記自動運行ビークルの姿勢角を設定する経由点設定部を更に備え、
   前記経由点設定部は、前記初期位置から前記目標位置までの間に存在する障害物を複数の凸領域に分割して前記各凸領域の重心点の位置を求め、前記重心点を母点とするボロノイ図及びドロネー図を用いて前記少なくとも１以上の経由点及び前記姿勢角を設定する
   ことを特徴とする目標経路生成装置。
7. 自動運行ビークルの初期位置から目標位置までの目標経路を生成する目標経路生成方法であって、
   初期位置から目標位置までを繋ぐ躍度最小化軌道のうち、前記初期位置及び目標位置における曲率の変化率が零である躍度最小化軌道を三角関数で近似して前記目標経路を算出する
   ことを特徴とする目標経路生成方法。