JP7628242B2 - 駐車経路生成装置及び駐車制御装置 - Google Patents

Description

本発明は、駐車経路生成装置及び駐車制御装置に関する。

例えば特許文献１には、トラクタ及びトレーラを具備した連結車の後退駐車を支援する後退駐車支援装置が記載されている。特許文献１に記載の後退駐車支援装置は、自車位置から駐車目標位置までの駐車目標経路を作成し、連結車が駐車目標経路に沿って移動するように連結車の操舵角を設定する。このとき、駐車目標経路について、Ｐｕｒｅ Ｐｕｒｓｕｉｔ法を用いて追従制御した場合の連結車の走行軌跡が作成される。

特開２０１８－１９９４５８号公報

ところで、駐車場等における連結車の後退駐車では、前進と後退との折返しが行われることが多い。しかし、上記従来技術においては、後退経路のみしか生成されない。このため、前進と後退との折返しを含むような駐車経路を自動的に生成することができない。

本発明の目的は、前進と後退との折返しを含むような駐車経路を実環境に応じて簡易に生成することができる駐車経路生成装置及び駐車制御装置を提供することである。

本発明の一態様は、トラクタ及びトレーラを具備した連結車を駐車させるための駐車経路を生成する駐車経路生成装置であって、連結車の現在位置を検出する位置検出部と、連結車の周辺環境を検知する環境検知部と、位置検出部により検出された連結車の現在位置を用いて、連結車の駐車開始位置を取得する駐車開始位置取得部と、位置検出部により検出された連結車の現在位置を含む情報を用いて、連結車の駐車目標位置を設定する駐車目標位置設定部と、駐車開始位置取得部により取得された駐車開始位置と駐車目標位置設定部により設定された駐車目標位置と環境検知部により検知された連結車の周辺環境とを用いて、連結車の折返し位置を設定する折返し位置設定部と、駐車開始位置取得部により取得された駐車開始位置と駐車目標位置設定部により設定された駐車目標位置と折返し位置設定部により設定された折返し位置とに基づいて、駐車開始位置から折返し位置までのトレーラの第１目標経路と折返し位置から駐車目標位置までのトレーラの第２目標経路とを生成する目標経路生成部と、目標経路生成部により生成された第１目標経路及び第２目標経路に基づいて、トラクタの移動経路を算出する移動経路算出部とを備え、目標経路生成部は、第１目標経路として、駐車開始位置から折返し位置までの間で曲率が連続的に変化するような曲線部を有する幾何経路を生成し、第２目標経路として、折返し位置から駐車目標位置までの間で曲率が連続的に変化するような曲線部を有する幾何経路を生成する。

このような駐車経路生成装置においては、連結車の現在位置を用いて、連結車の駐車開始位置が取得され、連結車の現在位置を含む情報を用いて、連結車の駐車目標位置が設定される。また、連結車の駐車開始位置及び駐車目標位置と連結車の周辺環境とを用いて、連結車の折返し位置が設定される。このとき、連結車の周辺環境を考慮して、連結車が周辺に存在する障害物と接触しないような折返し位置が設定される。そして、連結車の駐車開始位置、駐車目標位置及び折返し位置に基づいて、駐車開始位置から折返し位置までのトレーラの第１目標経路と折返し位置から駐車目標位置までのトレーラの第２目標経路とが生成される。そして、トレーラの第１目標経路及び第２目標経路に基づいて、トラクタの移動経路が算出される。ここで、トレーラの第１目標経路として、駐車開始位置から折返し位置までの間で曲率が連続的に変化するような曲線部を有する幾何経路が生成される。トレーラの第２目標経路として、折返し位置から駐車目標位置までの間で曲率が連続的に変化するような曲線部を有する幾何経路が生成される。このため、連結車の姿勢を考慮して、駐車開始位置から駐車目標位置まで滑らかな第１目標経路及び第２目標経路が生成されることになる。これにより、前進と後退との折返しを含むような駐車経路が実環境に応じて簡易に生成される。

駐車経路生成装置は、移動経路算出部によりトラクタの移動経路が算出された後、連結車の幾何モデルを用いて、連結車を駐車開始位置及び駐車目標位置から折返し位置までそれぞれ前進走行で追従制御を行う車両運動シミュレーションを実施することで、トラクタの操舵角のデータ系列とトラクタ及びトレーラの各姿勢角のデータ系列とを算出するデータ系列算出部を更に備えてもよい。

このような構成では、連結車を駐車開始位置及び駐車目標位置から折返し位置までそれぞれ前進走行で追従制御を行う車両運動シミュレーションを実施することにより、連結角の安定が確保されるトラクタの操舵角のデータ系列とトラクタ及びトレーラの各姿勢角のデータ系列とを算出することができる。また、連結車を駐車目標位置に駐車させる際には、トラクタの操舵角のデータ系列とトラクタ及びトレーラの各姿勢角のデータ系列とに従って、トラクタの操舵制御を行うことにより、連結車を駐車経路に沿って駐車目標位置まで精度良く誘導することができる。

駐車経路生成装置は、第１目標経路及び第２目標経路である幾何経路の曲率最大値がトレーラの旋回曲率最大値以下であるかどうかを判断する曲率制約判定部を更に備え、移動経路算出部は、曲率制約判定部により幾何経路の曲率最大値が旋回曲率最大値以下であると判断されたときに、トラクタの移動経路を算出し、折返し位置設定部は、曲率制約判定部により幾何経路の曲率最大値が旋回曲率最大値以下でないと判断されたときは、折返し位置を再設定してもよい。

このような構成では、トレーラの第１目標経路及び第２目標経路である幾何経路の曲率最大値は、トラクタの最大舵角とトラクタとトレーラとの最大連結角とで決まるトレーラの旋回曲率最大値以下となる。従って、駐車開始位置から駐車目標位置まで連結車特有の複雑な挙動が考慮された第１目標経路及び第２目標経路が得られる。

駐車経路生成装置は、第１目標経路及び第２目標経路に基づいて、トレーラの車体の通過軌跡を予測すると共に、移動経路に基づいて、トラクタの車体の通過軌跡を予測する車体軌跡予測部と、環境検知部により検知された連結車の周辺環境と車体軌跡予測部により予測された車体の通過軌跡とに基づいて、車体が連結車の周辺に存在する障害物と接触するかどうかを判断する接触判定部を更に備え、折返し位置設定部は、接触判定部により連結車が障害物と接触すると判断されたときは、折返し位置を再設定してもよい。

このような構成では、連結車が周辺に存在する障害物と接触する可能性があるときは、連結車の折返し位置が再設定されることになる。従って、連結車の周辺に存在する障害物との接触を回避するような第１目標経路及び第２目標経路が確実に生成される。

駐車経路生成装置は、トラクタとトレーラとの連結角を検出する連結角検出部を更に備え、駐車開始位置取得部は、連結車の現在位置と連結角とに基づいて、駐車開始位置としてのトレーラの位置座標、方位角及び旋回曲率を取得し、駐車目標位置設定部は、連結車の現在位置を含む情報と連結角とに基づいて、駐車目標位置としてのトレーラの位置座標、方位角及び旋回曲率を設定し、折返し位置設定部は、駐車開始位置と駐車目標位置と連結車の周辺環境と連結角とに基づいて、折返し位置としてのトレーラの位置座標、方位角及び旋回曲率を設定してもよい。

このような構成では、連結車の駐車開始位置、駐車目標位置及び折返し位置として、トレーラの位置座標、方位角及び旋回曲率が得られる。従って、トレーラの第１目標経路及び第２目標経路を形成する曲線として、例えば演算負荷が少ないトライ・クロソイド曲線等を採用することができる。

本発明の他の態様は、トラクタ及びトレーラを具備した連結車を駐車させる駐車制御装置であって、連結車の現在位置を検出する位置検出部と、連結車の周辺環境を検知する環境検知部と、位置検出部により検出された連結車の現在位置を用いて、連結車の駐車開始位置を取得する駐車開始位置取得部と、位置検出部により検出された連結車の現在位置を含む情報を用いて、連結車の駐車目標位置を設定する駐車目標位置設定部と、駐車開始位置取得部により取得された駐車開始位置と駐車目標位置設定部により設定された駐車目標位置と環境検知部により検知された連結車の周辺環境とを用いて、連結車の折返し位置を設定する折返し位置設定部と、駐車開始位置取得部により取得された駐車開始位置と駐車目標位置設定部により設定された駐車目標位置と折返し位置設定部により設定された折返し位置とに基づいて、駐車開始位置から折返し位置までのトレーラの第１目標経路と折返し位置から駐車目標位置までのトレーラの第２目標経路とを生成する目標経路生成部と、目標経路生成部により生成された第１目標経路及び第２目標経路に基づいて、トラクタの移動経路を算出する移動経路算出部と、目標経路生成部により生成された第１目標経路及び第２目標経路と移動経路算出部により算出された移動経路とに沿って連結車を駐車目標位置に駐車させるように、トラクタの操舵制御を行う追従制御部とを備え、目標経路生成部は、第１目標経路として、駐車開始位置から折返し位置までの間で曲率が連続的に変化するような曲線部を有する幾何経路を生成し、第２目標経路として、折返し位置から駐車目標位置までの間で曲率が連続的に変化するような曲線部を有する幾何経路を生成する。

このような駐車制御装置においては、連結車の現在位置を用いて、連結車の駐車開始位置が取得され、連結車の現在位置を含む情報を用いて、連結車の駐車目標位置が設定される。また、連結車の駐車開始位置及び駐車目標位置と連結車の周辺環境とを用いて、連結車の折返し位置が設定される。このとき、連結車の周辺環境を考慮して、連結車が周辺に存在する障害物と接触しないような折返し位置が設定される。そして、連結車の駐車開始位置、駐車目標位置及び折返し位置に基づいて、駐車開始位置から折返し位置までのトレーラの第１目標経路と折返し位置から駐車目標位置までのトレーラの第２目標経路とが生成される。そして、トレーラの第１目標経路及び第２目標経路に基づいて、トラクタの移動経路が算出される。そして、トレーラの第１目標経路及び第２目標経路とトラクタの移動経路とに沿って連結車を駐車目標位置に駐車させるように、連結車の操舵制御が行われる。ここで、トレーラの第１目標経路として、駐車開始位置から折返し位置までの間で曲率が連続的に変化するような曲線部を有する幾何経路が生成される。トレーラの第２目標経路として、折返し位置から駐車目標位置までの間で曲率が連続的に変化するような曲線部を有する幾何経路が生成される。このため、連結車の姿勢を考慮して、駐車開始位置から駐車目標位置まで滑らかな第１目標経路及び第２目標経路が生成されることになる。これにより、前進と後退との折返しを含むような駐車経路が実環境に応じて簡易に生成される。

駐車制御装置は、移動経路算出部によりトラクタの移動経路が算出された後、連結車の幾何モデルを用いて、連結車を駐車開始位置及び駐車目標位置から折返し位置までそれぞれ前進走行で追従制御を行う車両運動シミュレーションを実施することで、トラクタの操舵角のデータ系列とトラクタ及びトレーラの各姿勢角のデータ系列とを算出するデータ系列算出部を更に備え、追従制御部は、データ系列算出部により算出されたトラクタの操舵角のデータ系列とトラクタ及びトレーラの各姿勢角のデータ系列とに従って連結車を駐車目標位置に駐車させるように、トラクタの操舵制御を行ってもよい。

このような構成では、連結車を駐車開始位置及び駐車目標位置から折返し位置までそれぞれ前進走行で追従制御を行う車両運動シミュレーションを実施することにより、連結角の安定が確保されるトラクタの操舵角のデータ系列とトラクタ及びトレーラの各姿勢角のデータ系列とを算出することができる。また、連結車を駐車目標位置に駐車させる際には、トラクタの操舵角のデータ系列とトラクタ及びトレーラの各姿勢角のデータ系列とに従って、トラクタの操舵制御を行うことにより、連結車を駐車経路に沿って駐車目標位置まで精度良く誘導することができる。

駐車制御装置は、トラクタとトレーラとの連結角を検出する連結角検出部を更に備え、追従制御部は、トラクタの操舵角のデータ系列とトラクタ及びトレーラの各姿勢角のデータ系列とに従って連結車を駐車目標位置に駐車させるように、トラクタの操舵のフィードフォワード制御を行うと共に、連結角と連結車の現在位置とに基づいて連結車を駐車目標位置に駐車させるように、トラクタの操舵のフィードバック制御を行ってもよい。

このような構成では、トラクタの操舵角のデータ系列とトラクタ及びトレーラの各姿勢角のデータ系列とに従って、トラクタの操舵のフィードフォワード制御を行うだけでなく、トラクタとトレーラとの連結角と連結車の現在位置とに基づいて、トラクタの操舵のフィードバック制御を行うことにより、外乱等があっても、連結車を駐車経路に沿って駐車目標位置まで精度良く誘導することができる。

本発明によれば、前進と後退との折返しを含むような駐車経路を実環境に応じて簡易に生成することができる。

本発明の一実施形態に係る駐車経路生成装置を備えた駐車制御装置を概略的に示すブロック図である。 図１に示された駐車制御装置が搭載された連結車の側面図である。 図２に示された連結車が駐車する様子を示す平面図である。 図１に示された経路生成ＥＣＵにより実行される経路生成処理の手順の詳細を示すフローチャートである。 図２に示された連結車をモデル化して示す概略図である。 駐車開始位置から折返し位置までのトレーラの第１目標経路と折返し位置から駐車目標位置までのトレーラの第２目標経路とを示す平面図である。 トライ・クロソイド曲線により得られるトレーラの第２目標経路の一例をトラクタの移動経路と共に示すグラフである。 トレーラの車体及びトラクタの車体の通過軌跡の一例を示すグラフである。

以下、本発明の実施形態について図面を参照して詳細に説明する。

図１は、本発明の一実施形態に係る駐車経路生成装置を備えた駐車制御装置を概略的に示すブロック図である。図１において、本実施形態の駐車制御装置１は、図２及び図３に示されるように、駐車場等において連結車２を自動的に駐車させる装置である。

連結車２は、ここではセミトレーラ式の大型トラックである。なお、連結車２は、中型トラックまたは小型トラックでもよい。連結車２は、トラクタ３と、このトラクタ３の後側に配置されたトレーラ４とを具備している。トレーラ４は、連結部５においてトラクタ３に回動可能に連結されている。

トラクタ３は、車体６と、この車体６の前側に回転可能に支持された左右１対の前輪７と、車体６の後側に回転可能に支持された左右１対の後輪８とを有している。トラクタ３には、エンジン（図示せず）が搭載されている。トレーラ４は、車体９と、この車体９の後側に回転可能に支持された左右１対の車輪１０とを有している。

図１に戻り、駐車制御装置１は、連結車２に搭載されている。駐車制御装置１は、カーナビゲーション１１と、連結角センサ１２と、カメラ１３と、経路生成ＥＣＵ１４と、駐車制御ＥＣＵ１５とを備えている。

カーナビゲーション１１、連結角センサ１２、カメラ１３及び経路生成ＥＣＵ１４は、本実施形態の駐車経路生成装置１６を構成している。駐車経路生成装置１６は、駐車制御装置１により連結車２を駐車させるための駐車経路を生成する装置である。

駐車経路生成装置１６は、図３に示されるように、駐車スペースＨの横から前進し、１回の折返しを行って駐車スペースＨまで後退するような駐車経路を生成する。具体的には、駐車経路生成装置１６は、駐車開始位置Ａ１から折返し位置Ａ３まで前進した後、折返し位置Ａ３から駐車目標位置Ａ２まで後退するような駐車経路を生成する。駐車開始位置Ａ１、駐車目標位置Ａ２及び折返し位置Ａ３については、後で詳述する。

カーナビゲーション１１は、ＧＰＳ衛星から送信される電波を受信して、連結車２の現在位置を測定するＧＰＳ受信機１７と、地図データを記憶する地図記憶部１８と、連結車２の現在位置を地図と共に表示するディスプレイ１９と、連結車２の目的地等のデータを設定入力する入力部２０とを有している。

ＧＰＳ受信機１７は、連結車２の現在位置を検出する位置検出部を構成している。ＧＰＳ受信機１７は、連結車２の現在位置として、トラクタ３の２次元座標（Ｘ，Ｙ座標）の位置座標及び方位角を検出する。

連結角センサ１２は、トラクタ３とトレーラ４との連結角θ（図５参照）を検出するセンサ（連結角検出部）である。連結角θは、トラクタ３の前後方向に延びる仮想線Ｌ１とトレーラ４の前後方向に延びる仮想線Ｌ２とが交差する角度である。トラクタ３及びトレーラ４が同一直線上に配置されている状態では、連結角θは０度である。

カメラ１３は、連結車２の周囲を撮像する撮像部である。カメラ１３は、例えばトラクタ３及びトレーラ４にそれぞれ設けられている。カメラ１３は、障害物５０（図３及び図６参照）の有無等といった連結車２の周辺環境を検知する環境検知部を構成している。

障害物５０としては、他の駐車車両５１（図３参照）、建物の壁５２（図６参照）または縁石等が挙げられる。建物の壁５２がカーナビゲーション１１に内蔵された地図データから分かる場合には、カーナビゲーション１１の地図記憶部１８も環境検知部を構成する。

経路生成ＥＣＵ１４は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。経路生成ＥＣＵ１４は、カーナビゲーション１１の情報、連結角センサ１２の検出値及びカメラ１３の撮像画像を入力し、連結車２の駐車経路の生成に関する処理を実行し、連結車２の駐車経路を含むデータを駐車制御ＥＣＵ１５に出力する。

経路生成ＥＣＵ１４は、駐車開始位置取得部２１と、駐車目標位置設定部２２と、折返し位置設定部２３と、目標経路生成部２４と、曲率制約判定部２５と、移動経路算出部２６と、車体軌跡予測部２７と、接触判定部２８と、データ系列算出部２９と、データ出力部３０とを有している。

駐車開始位置取得部２１は、カーナビゲーション１１のＧＰＳ受信機１７により検出された連結車２の現在位置を用いて、連結車２の駐車開始位置Ａ１（図３参照）を取得する。具体的には、駐車開始位置取得部２１は、連結車２の現在位置と、連結角センサ１２により検出されたトラクタ３とトレーラ４との連結角θとに基づいて、連結車２の駐車開始位置Ａ１を取得する。駐車開始位置取得部２１は、連結車２の駐車開始位置Ａ１として、トレーラ４の位置座標、方位角及び旋回曲率を取得する。

駐車目標位置設定部２２は、ＧＰＳ受信機１７により検出された連結車２の現在位置を含むカーナビゲーション１１の情報を用いて、連結車２の駐車目標位置Ａ２（図３参照）を設定する。具体的には、駐車目標位置設定部２２は、連結車２の現在位置を含むカーナビゲーション１１の情報と、連結角センサ１２により検出されたトラクタ３とトレーラ４との連結角θと、カメラ１３の撮像画像とに基づいて、連結車２の駐車目標位置Ａ２を設定する。駐車目標位置設定部２２は、連結車２の駐車目標位置Ａ２として、トレーラ４の位置座標、方位角及び旋回曲率を取得する。

折返し位置設定部２３は、駐車開始位置取得部２１により取得された連結車２の駐車開始位置Ａ１と駐車目標位置設定部２２により設定された連結車２の駐車目標位置Ａ２とカメラ１３により検知された連結車２の周辺環境とを用いて、連結車２の折返し位置Ａ３（図３参照）を設定する。具体的には、折返し位置設定部２３は、連結車２の駐車開始位置Ａ１及び駐車目標位置Ａ２と、連結角センサ１２により検出されたトラクタ３とトレーラ４との連結角θと、カメラ１３の撮像画像とに基づいて、連結車２の折返し位置Ａ３を設定する。折返し位置Ａ３は、前進から後退に切り換わる位置である。折返し位置設定部２３は、連結車２の折返し位置Ａ３として、トレーラ４の位置座標、方位角及び旋回曲率を取得する。

目標経路生成部２４は、駐車開始位置取得部２１により取得された連結車２の駐車開始位置Ａ１と、駐車目標位置設定部２２により設定された連結車２の駐車目標位置Ａ２と、折返し位置設定部２３により設定された連結車２の折返し位置Ａ３とに基づいて、トレーラ４の第１目標経路Ｒ１及び第２目標経路Ｒ２（図６参照）を生成する。第１目標経路Ｒ１は、駐車開始位置Ａ１から折返し位置Ａ３までのトレーラ４の目標経路である。第２目標経路Ｒ２は、折返し位置Ａ３から駐車目標位置Ａ２までのトレーラ４の目標経路である。

目標経路生成部２４は、トレーラ４の第１目標経路Ｒ１として、連結車２の駐車開始位置Ａ１から折返し位置Ａ３までの間で曲率が連続的に変化するような曲線部を有する幾何経路を生成する。目標経路生成部２４は、トレーラ４の第２目標経路Ｒ２として、連結車２の折返し位置Ａ３から駐車目標位置Ａ２までの間で曲率が連続的に変化するような曲線部を有する幾何経路を生成する。

曲率制約判定部２５は、目標経路生成部２４により生成されたトレーラ４の第１目標経路Ｒ１及び第２目標経路Ｒ２である幾何経路が車両構造に基づくトレーラ４の旋回曲率最大値の制約を満たしているかどうかを判定する。具体的には、曲率制約判定部２５は、トレーラ４の第１目標経路Ｒ１及び第２目標経路Ｒ２である幾何経路の曲率最大値がトレーラ４の旋回曲率最大値以下であるかどうかを判断する。

移動経路算出部２６は、曲率制約判定部２５により幾何経路の曲率最大値がトレーラ４の旋回曲率最大値以下であると判断されたときに、目標経路生成部２４により生成されたトレーラ４の第１目標経路Ｒ１及び第２目標経路Ｒ２に基づいて、連結車２の駐車開始位置Ａ１から駐車目標位置Ａ２までのトラクタ３の移動経路Ｑを算出する。

車体軌跡予測部２７は、目標経路生成部２４により生成されたトレーラ４の第１目標経路Ｒ１及び第２目標経路Ｒ２に基づいて、トレーラ４の車体９の通過軌跡を予測すると共に、移動経路算出部２６により算出されたトラクタ３の移動経路Ｑに基づいて、トラクタ３の車体６の通過軌跡を予測する。

接触判定部２８は、カーナビゲーション１１の情報と、カメラ１３の撮像画像と、車体軌跡予測部２７により予測されたトレーラ４の車体９及びトラクタ３の車体６の通過軌跡とに基づいて、車体６，９が障害物５０（図３及び図６参照）と接触するかどうかを判断する。カーナビゲーション１１の情報及びカメラ１３の撮像画像は、環境検知部の検知データに相当する。

データ系列算出部２９は、移動経路算出部２６によりトラクタ３の移動経路Ｑが算出された後、連結車２の幾何モデルを用いて、連結車２を駐車開始位置Ａ１及び駐車目標位置Ａ２から折返し位置Ａ３までそれぞれ前進走行で追従制御を行う車両運動シミュレーションを実施することで、トラクタ３の操舵角のデータ系列とトラクタ３及びトレーラ４の各姿勢角のデータ系列とを算出する。なお、データ系列は、履歴データである。

データ出力部３０は、接触判定部２８により車体６，９が障害物５０と接触しないと判断されたときに、トレーラ４の第１目標経路Ｒ１及び第２目標経路Ｒ２とトラクタ３の移動経路Ｑとを駐車経路データとして駐車制御ＥＣＵ１５に出力すると共に、トラクタ３の操舵角のデータ系列とトラクタ３及びトレーラ４の各姿勢角のデータ系列とを駐車制御ＥＣＵ１５に出力する。

図４は、経路生成ＥＣＵ１４により実行される経路生成処理の手順の詳細を示すフローチャートである。本処理は、連結車２の運転者によって駐車支援の開始が指示されると、実行される。このとき、運転者は、連結車２の駐車目標位置候補をカーナビゲーション１１の入力部２０により設定入力してもよい。

図４において、経路生成ＥＣＵ１４は、まずＧＰＳ受信機１７を含むカーナビゲーション１１の情報及び連結角センサ１２の検出値に基づいて、連結車２の駐車開始位置Ａ１としてのトレーラ４の位置座標、方位角及び旋回曲率を取得する（手順Ｓ１０１）。

トレーラ４の位置座標は、例えば図５に示されるように、トレーラ４の後軸中心Ｏ２の位置座標（Ｘ２，Ｙ２）である。トレーラ４の後軸中心Ｏ２は、図３に示されるように、トレーラ４の後側に配置された左右１対の車輪１０に連結された車軸１０ａの軸方向（トレーラ４の車幅方向）の中心である。トレーラ４の方位角は、トレーラ４のヨー角Ψ２に相当する。なお、トラクタ３の方位角は、トラクタ３のヨー角Ψ１に相当する。

また、連結車２の定常旋回を仮定する条件下では、トレーラ４の旋回曲率は、トラクタ３に対するトレーラ４の連結角と一意に対応する。このため、トラクタ３とトレーラ４との連結角θは、トレーラ４の旋回曲率を入力とする代数式で求められる。従って、トレーラ４の旋回曲率は、トラクタ３とトレーラ４との連結角θから得られる。トラクタ３とトレーラ４との連結角θが０度である状態では、トレーラ４の旋回曲率は０である。なお、図５では、連結車２がモデル化して示されている。

続いて、経路生成ＥＣＵ１４は、ＧＰＳ受信機１７を含むカーナビゲーション１１の情報、連結角センサ１２の検出値及びカメラ１３の撮像画像に基づいて、連結車２の駐車目標位置Ａ２としてのトレーラ４の位置座標、方位角及び旋回曲率を設定する（手順Ｓ１０２）。

続いて、経路生成ＥＣＵ１４は、連結車２の駐車開始位置Ａ１及び駐車目標位置Ａ２、連結角センサ１２の検出値及びカメラ１３の撮像画像に基づいて、連結車２の折返し位置Ａ３としてのトレーラ４の位置座標、方位角及び旋回曲率を設定する（手順Ｓ１０３）。このとき、経路生成ＥＣＵ１４は、連結車２の駐車開始位置Ａ１及び駐車目標位置Ａ２から、連結車２の折返し位置Ａ３の候補となるエリアを算出し、そのエリア内において障害物５０に接触しないような折返し位置Ａ３を設定する。

続いて、経路生成ＥＣＵ１４は、連結車２の駐車開始位置Ａ１、駐車目標位置Ａ２及び折返し位置Ａ３に基づいて、トレーラ４の後軸中心Ｏ２の第１目標経路Ｒ１及び第２目標経路Ｒ２を生成する（手順Ｓ１０４）。トレーラ４の後軸中心Ｏ２の第１目標経路Ｒ１は、図６に示されるように、連結車２の駐車開始位置Ａ１から折返し位置Ａ３までの間で曲率が連続的に変化するような曲線部を有する幾何経路である。トレーラ４の後軸中心Ｏ２の第２目標経路Ｒ２は、図６に示されるように、連結車２の折返し位置Ａ３から駐車目標位置Ａ２までの間で曲率が連続的に変化するような曲線部を有する幾何経路である。曲率が連続的に変化するような曲線としては、トライ・クロソイド曲線が採用される。

トライ・クロソイド曲線は、曲率の急激な変化が生じない曲線である。トライ・クロソイド曲線は、例えば曲率変化率が一定な曲線である。トライ・クロソイド曲線は、３本の経路長が等しいクロソイド曲線を組み合わせることにより、初期及び終端の曲線及び位置を任意に指定することが可能な滑らかな曲線である。トライ・クロソイド曲線は、２変数の非線形方程式を数値的に解くことで簡単に求められる。なお、トライ・クロソイド曲線については、日本機械学会論文集（Vol.85,No.878,2019）の「クロソイド曲線による初期と終端の曲率が指定可能な経路生成手法」に詳細に記載されている。

図７は、トライ・クロソイド曲線により得られるトレーラ４の後軸中心Ｏ２の第２目標経路Ｒ２の一例を示すグラフである。なお、図７は、セミトレーラ式の連結車の経路生成についてのシミュレーション結果を示している。

図７において、トライ・クロソイド曲線で与えられるトレーラ４の第２目標経路Ｒ２の一端は、連結車２の折返し位置Ａ３におけるトレーラ４の位置を表している。トライ・クロソイド曲線で与えられる第２目標経路Ｒの他端は、連結車２の駐車目標位置Ａ２におけるトレーラ４の位置を表している。折返し位置Ａ３では、位置座標は（Ｘａ，Ｙａ）であり、方位角は０度であり、旋回曲率は０である。駐車目標位置Ａ２では、位置座標は（Ｘｂ，Ｙｂ）であり、方位角は９０度であり、旋回曲率は０である。また、第２目標経路Ｒ２の駐車目標位置Ａ２側の端部は直線部であり、第２目標経路Ｒ２の他の部分は曲線部となっている。なお、第２目標経路Ｒ２は、全体的に曲線状であってもよい。

続いて、経路生成ＥＣＵ１４は、トレーラ４の後軸中心Ｏ２の第１目標経路Ｒ１及び第２目標経路Ｒ２である幾何経路の曲率最大値がトレーラ４の旋回曲率最大値以下であるかどうかを判断する（手順Ｓ１０５）。トレーラ４の旋回曲率最大値は、連結車２の車両構造上の最大舵角及び最大連結角に応じて予め決まっている。なお、連結車２の舵角は、トラクタ３の前輪７の舵角δ（図５参照）である。

経路生成ＥＣＵ１４は、トレーラ４の後軸中心Ｏ２の第１目標経路Ｒ１及び第２目標経路Ｒ２である幾何経路の曲率最大値がトレーラ４の旋回曲率最大値以下でないと判断したときは、上記の手順Ｓ１０３を再度実行し、連結車２の折返し位置Ａ３を再設定する。

経路生成ＥＣＵ１４は、トレーラ４の後軸中心Ｏ２の第１目標経路Ｒ１及び第２目標経路Ｒ２である幾何経路の曲率最大値がトレーラ４の旋回曲率最大値以下であると判断したときは、トレーラ４の後軸中心Ｏ２の第１目標経路Ｒ１及び第２目標経路Ｒ２とトレーラ４の車体９の寸法とに基づいて、トラクタ３の後軸中心Ｏ１の移動経路Ｑを算出する（手順Ｓ１０６）。

このとき、経路生成ＥＣＵ１４は、連結車２の定常旋回を仮定したうえで、トレーラ４の後軸中心Ｏ２の第１目標経路Ｒ１及び第２目標経路Ｒ２の曲率からトラクタ３とトレーラ４との連結角θを代数式で求め、トラクタ３の後軸中心Ｏ１の移動経路Ｑをトレーラ４の位置に対する相対関係から代数的に求める。トレーラ４の後軸中心Ｏ２の第１目標経路Ｒ１及び第２目標経路Ｒ２である幾何経路としては、曲率が連続変化する値が用いられるので、連結車２の定常旋回を仮定して連結角θを求めても差し支えなく、また得られる連結角θも連続変化する値である。このため、一般的に微分方程式で表される複雑な連結車ダイナミクスを解くことなく、トレーラ４とトラクタ３とのスムーズな相対挙動が近似的に表現される。

トラクタ３の後軸中心Ｏ１は、図３に示されるように、トラクタ３の左右１対の後輪８に連結された車軸８ａの軸方向（トラクタ３の車幅方向）の中心である。なお、トラクタ３の左右１対の前輪７には、車軸７ａが連結されている。また、トラクタ３の後軸中心Ｏ１の位置座標は、（Ｘ１，Ｙ１）である（図５参照）。

図７に示されるシミュレーションでは、トライ・クロソイド曲線で与えられるトレーラ４の後軸中心Ｏ２の第２目標経路Ｒ２から、トラクタ３の後軸中心Ｏ１の移動経路Ｑが得られる。移動経路Ｑは、トレーラ４の後軸中心Ｏ２の第２目標経路Ｒ２に応じた特徴的な曲線となっている。

続いて、経路生成ＥＣＵ１４は、トレーラ４の後軸中心Ｏ２の第１目標経路Ｒ１及び第２目標経路Ｒ２とトレーラ４の車体９の形状及び寸法とに基づいて、トレーラ４の車体９の通過軌跡を予測すると共に、トラクタ３の後軸中心Ｏ１の移動経路Ｑとトラクタ３の車体６の形状及び寸法とに基づいて、トラクタ３の車体６の通過軌跡を予測する（手順Ｓ１０７）。

図８は、トレーラ４の車体９及びトラクタ３の車体６の通過軌跡の一例を示すグラフであり、図７に示されるグラフに対応している。図８において、トレーラ４の車体９の通過軌跡Ｂ１は、図７に示されるトレーラ４の後軸中心Ｏ２の第２目標経路Ｒ２を基準として、幾何的に求められる。トラクタ３の車体６の通過軌跡Ｂ２は、図７に示されるトラクタ３の後軸中心Ｏ１の移動経路Ｑを基準として、幾何的に求められる。

続いて、経路生成ＥＣＵ１４は、カーナビゲーション１１の情報及びカメラ１３の撮像画像に基づいて、連結車２の車体６，９が周囲の障害物５０（図６参照）と接触しないかどうかを判断する（手順Ｓ１０８）。経路生成ＥＣＵ１４は、連結車２の車体６，９が周囲の障害物５０と接触すると判断したときは、上記の手順Ｓ１０３を再度実行し、連結車２の折返し位置Ａ３を再設定する。

経路生成ＥＣＵ１４は、連結車２の車体６，９が周囲の障害物５０と接触しないと判断したときは、連結車２の簡易幾何モデルを用いて、連結車２を駐車開始位置Ａ１から折返し位置Ａ３まで第１目標経路Ｒ１及び移動経路Ｑに沿って前進走行で追従制御を行うと共に、連結車２を駐車目標位置Ａ２から折返し位置Ａ３まで第２目標経路Ｒ２及び移動経路Ｑに沿って前進走行で追従制御を行う車両運動シミュレーションを実施することで、トラクタ３の操舵角のデータ系列とトラクタ３及びトレーラ４の各姿勢角のデータ系列とを算出する（手順Ｓ１０９）。なお、トラクタ３の操舵角は、トラクタ３の前輪７の舵角δに相当する。このとき、経路生成ＥＣＵ１４は、例えばトラクタ３の移動経路Ｑである幾何経路に対し、経路追従制御手法を用いて追従制御を行う車両運動シミュレーションを実施してもよい。

実際の駐車では、連結車２を折返し位置Ａ３から駐車目標位置Ａ２まで後退させる。ただし、エンジン等の動力源は、トラクタ３に搭載されており、トレーラ４には搭載されていない。従って、連結車２を後退させる際には、トラクタ３がトレーラ４を押して走行することになるため、トラクタ３とトレーラ４との連結角θの挙動は不安定である。このため、追従制御を行う場合には、制御器の数値パラメータの調整が複雑にならざるを得ない。

一方、連結車２を前進させる際には、トラクタ３がトレーラ４を引っ張って走行することになるため、トラクタ３とトレーラ４との連結角θの挙動は安定である。このため、追従制御に使用される数値パラメータを比較的簡単に設定することができる。また、上記の手順Ｓ１０５において、目標経路である幾何経路の曲率最大値が連結車２の旋回曲最大値以下であることが確保されているため、車両運動シミュレーション上では、連結車２の前進走行を逆再生させても、連結車２の前進走行時と同じ軌跡が得られる。

そこで、本車両運動シミュレーションでは、折返し位置Ａ３と駐車目標位置Ａ２との間でも、駐車目標位置Ａ２を出発点とし、折返し位置Ａ３を到着点とする前進走行シミュレーションが実施される。

続いて、経路生成ＥＣＵ１４は、トレーラ４の後軸中心Ｏ２の第１目標経路Ｒ１及び第２目標経路Ｒ２とトラクタ３の後軸中心Ｏ１の移動経路Ｑとを駐車経路データとして駐車制御ＥＣＵ１５に出力すると共に、トラクタ３の操舵角のデータ系列とトラクタ３及びトレーラ４の各姿勢角のデータ系列とを駐車制御ＥＣＵ１５に出力する（手順Ｓ１１０）。

ここで、駐車開始位置取得部２１は、手順Ｓ１０１を実行する。駐車目標位置設定部２２は、手順Ｓ１０２を実行する。折返し位置設定部２３は、手順Ｓ１０３を実行する。目標経路生成部２４は、手順Ｓ１０４を実行する。曲率制約判定部２５は、手順Ｓ１０５を実行する。移動経路算出部２６は、手順Ｓ１０６を実行する。車体軌跡予測部２７は、手順Ｓ１０７を実行する。接触判定部２８は、手順Ｓ１０８を実行する。データ系列算出部２９は、手順Ｓ１０９を実行する。データ出力部３０は、手順Ｓ１１０を実行する。

図１に戻り、駐車制御ＥＣＵ１５は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。駐車制御ＥＣＵ１５は、経路生成ＥＣＵ１４からの駐車経路データ及びデータ系列とカーナビゲーション１１の情報と連結角センサ１２の検出値とを入力し、連結車２の自動駐車に関する処理を実行する。

駐車制御ＥＣＵ１５は、記憶部３１と、追従制御部３２とを有している。記憶部３１は、経路生成ＥＣＵ１４のデータ出力部３０から出力された駐車経路データ及びデータ系列を記憶する。

追従制御部３２は、記憶部３１に記憶された駐車経路データ及びデータ系列に従って連結車２を駐車目標位置Ａ２に駐車させるように、連結車２の操舵制御及び速度制御を行う。追従制御部３２は、トラクタ３の後軸中心Ｏ１の移動経路Ｑ及びトレーラ４の後軸中心Ｏ２の第１目標経路Ｒ１及び第２目標経路Ｒ２に沿って連結車２を追従走行させるように、連結車２の操舵制御及び速度制御を行う。追従制御部３２は、連結車２の操舵制御においては連結車２のステアリングを制御し、連結車２の速度制御においてはアクセル及びブレーキを制御する。

このとき、追従制御部３２は、トラクタ３の操舵角のデータ系列とトラクタ３及びトレーラ４の各姿勢角のデータ系列とに従って、連結車２を駐車目標位置Ａ２に駐車させるように、トラクタ３の操舵のフィードフォワード制御を行う。つまり、追従制御部３２は、トラクタ３の操舵角のデータ系列とトラクタ３及びトレーラ４の各姿勢角のデータ系列とを目標値として、トラクタ３の操舵のフィードフォワード制御を行う。また、追従制御部３２は、連結角センサ１２により検出されたトラクタ３とトレーラ４との連結角θとＧＰＳ受信機１７により検出された連結車２の現在位置とに基づいて、連結車２を駐車目標位置Ａ２に駐車させるように、トラクタ３の操舵のフィードバック制御を行う。

追従制御部３２は、例えばトライ・クロソイド曲線で与えられたトレーラ４の目標経路上に仮想的に目標姿勢の車両を考え、その目標値に対する実車両の相対誤差を状態量として０に近づけるような追従制御を行う。誤差状態量を０に近づけるような追従制御としては、例えばＬＱ制御が挙げられる。

以上のように本実施形態にあっては、連結車２の現在位置を用いて、連結車２の駐車開始位置Ａ１が取得され、連結車２の現在位置を含む情報を用いて、連結車２の駐車目標位置Ａ２が設定される。また、連結車２の駐車開始位置Ａ１及び駐車目標位置Ａ２と連結車２の周辺環境とを用いて、連結車２の折返し位置Ａ３が設定される。このとき、連結車２の周辺環境を考慮して、連結車２が周辺に存在する障害物５０と接触しないような折返し位置Ａ３が設定される。そして、連結車２の駐車開始位置Ａ１、駐車目標位置Ａ２及び折返し位置Ａ３に基づいて、駐車開始位置Ａ１から折返し位置Ａ３までのトレーラ４の第１目標経路Ｒ１と折返し位置Ａ３から駐車目標位置Ａ２までのトレーラ４の第２目標経路Ｒ２とが生成される。そして、トレーラ４の第１目標経路Ｒ１及び第２目標経路Ｒ２に基づいて、トラクタ３の移動経路Ｑが算出される。そして、トレーラ４の第１目標経路Ｒ１及び第２目標経路Ｒ２とトラクタ３の移動経路Ｑとに沿って連結車２を駐車目標位置Ａ２に駐車させるように、連結車２の操舵制御が行われる。ここで、トレーラ４の第１目標経路Ｒ１として、連結車２の駐車開始位置Ａ１から折返し位置Ａ３までの間で曲率が連続的に変化するような曲線部を有する幾何経路が生成される。トレーラ４の第２目標経路Ｒ２として、連結車２の折返し位置Ａ３から駐車目標位置Ａ２までの間で曲率が連続的に変化するような曲線部を有する幾何経路が生成される。このため、連結車２の姿勢を考慮して、駐車開始位置Ａ１から駐車目標位置Ａ２まで滑らかな第１目標経路Ｒ１及び第２目標経路Ｒ２が生成されることになる。これにより、前進と後退との折返しを含むような駐車経路が実環境に応じて簡易に生成される。また、車両パラメータに応じた事前のオフライン演算が不要となるため、トレーラ４の繋ぎ替え等の車両パラメータの変更への対応を容易に行うことができる。その結果、量産車両への導入を安価に実現することが可能となる。

また、本実施形態では、連結車２の駐車開始位置Ａ１から駐車目標位置Ａ２まで滑らかな駐車経路が生成されることになる。このような駐車経路に対して例えば経路曲率をフィードフォワード情報として利用する操舵制御を組み合わせることで、駐車経路に対して連結車２の逸脱量が少ない経路追従制御を実現することができる。また、例えばトレーラ４の第１目標経路Ｒ１及び第２目標経路Ｒ２の値を代数式へ代入演算することで、トレーラ４の車体９及びトラクタ３の車体６の通過軌跡を容易に予測することができる。従って、演算処理を簡素化しつつ、連結車２の周囲に存在する障害物５０との接触を回避するような駐車経路を生成することができる。

また、本実施形態では、連結車２の幾何モデルを用いて、連結車２を駐車開始位置Ａ１及び駐車目標位置Ａ２から折返し位置Ａ３までそれぞれ前進走行で追従制御を行う車両運動シミュレーションを実施することにより、連結角θの安定が確保されるトラクタ３の操舵角のデータ系列とトラクタ３及びトレーラ４の各姿勢角のデータ系列とを算出することができる。また、連結車２を駐車目標位置Ａ２に駐車させる際には、トラクタ３の操舵角のデータ系列とトラクタ３及びトレーラ４の各姿勢角のデータ系列とに従って、トラクタ３の操舵制御を行うことにより、連結車２を駐車経路に沿って駐車目標位置Ａ２まで精度良く誘導することができる。

また、本実施形態では、トラクタ３の操舵角のデータ系列とトラクタ３及びトレーラ４の各姿勢角のデータ系列とに従って、トラクタ３の操舵のフィードフォワード制御を行うだけでなく、トラクタ３とトレーラ４との連結角θと連結車２の現在位置とに基づいて、トラクタ３の操舵のフィードバック制御を行うことにより、外乱等があっても、連結車２を駐車経路に沿って駐車目標位置Ａ２まで精度良く誘導することができる。

また、本実施形態では、トレーラ４の第１目標経路Ｒ１及び第２目標経路Ｒ２である幾何経路の曲率最大値がトレーラ４の旋回曲率最大値以下であると判断されたときに、トラクタ３の移動経路Ｑが算出され、幾何経路の曲率最大値がトレーラ４の旋回曲率最大値以下でないと判断されたときは、連結車２の折返し位置Ａ３が再設定される。このため、トレーラ４の第１目標経路Ｒ１及び第２目標経路Ｒ２である幾何経路の曲率最大値は、トラクタ３の最大舵角とトラクタ３とトレーラ４との最大連結角とで決まるトレーラ４の旋回曲率最大値以下となる。従って、駐車開始位置Ａ１から駐車目標位置Ａ２まで連結車２特有の複雑な挙動が考慮された第１目標経路Ｒ１及び第２目標経路Ｒ２が得られる。

また、本実施形態では、トレーラ４の第１目標経路Ｒ１及び第２目標経路Ｒ２に基づいて、トレーラ４の車体９の通過軌跡が予測されると共に、トラクタ３の移動経路Ｑに基づいて、トラクタ３の車体６の通過軌跡が予測される。そして、カメラ１３の撮像画像と車体６，９の通過軌跡とに基づいて、車体６，９が障害物５０と接触するかどうかが判断され、車体６，９が障害物５０と接触すると判断されたときは、連結車２の折返し位置Ａ３が再設定される。従って、連結車２の周囲に存在する障害物５０との接触を回避するような第１目標経路Ｒ１及び第２目標経路Ｒ２が確実に生成される。

また、本実施形態では、連結車２の駐車開始位置Ａ１、駐車目標位置Ａ２及び折返し位置Ａ３として、トレーラ４の位置座標、方位角及び旋回曲率が得られる。従って、トレーラ４の第１目標経路Ｒ１及び第２目標経路Ｒ２を形成する曲線として、例えば演算負荷が少ないトライ・クロソイド曲線等を採用することができる。

なお、本発明は、上記実施形態には限定されない。例えば上記実施形態では、カーナビゲーション１１のＧＰＳ受信機１７によって連結車２の現在位置が測定されているが、連結車２の現在位置を検出する位置検出部としては、特にＧＰＳ受信機１７には限られない。例えば、連結車２の周囲にレーザを照射し、レーザの反射光を受光するレーザセンサのデータを利用したレーザＳＬＡＭ手法を用いて、連結車２の現在位置を推定してもよいし、或いはカメラの撮像画像を利用したＶｉｓｕａｌ ＳＬＡＭ手法を用いて、連結車２の現在位置を推定してもよい。

また、上記実施形態では、カメラ１３を使用して、連結車２の周辺環境が検知されているが、特にその形態には限られず、カメラ１３に代えて又はカメラ１３と共にレーザセンサ及びレーダセンサ等を使用して、連結車２の周辺環境を検知してもよい。

また、上記実施形態では、トレーラ４の後軸中心Ｏ２の第１目標経路Ｒ１及び第２目標経路Ｒ２が生成され、トラクタ３の後軸中心Ｏ１の移動経路Ｑが算出されているが、特にそのような形態には限られない。トレーラ４の第１目標経路Ｒ１及び第２目標経路Ｒ２としては、トレーラ４の任意の点（例えば車体９の中心点）の目標経路を生成してもよい。トラクタ３の移動経路Ｑとしては、トラクタ３の任意の点（例えば車体６の中心点）の移動経路を算出してもよい。

また、上記実施形態では、トライ・クロソイド曲線を採用して、トレーラ４の第１目標経路Ｒ１及び第２目標経路Ｒ２が生成されているが、特にその形態には限られない。トレーラ４の第１目標経路Ｒ１としては、連結車２の駐車開始位置Ａ１から折返し位置Ａ３までの間で曲率が連続的に変化するような曲線を採用すればよい。トレーラ４の第２目標経路Ｒ２としては、連結車２の折返し位置Ａ３から駐車目標位置Ａ２までの間で曲率が連続的に変化するような曲線を採用すればよい。そのような曲線としては、例えばBiarc曲線、Clothoid曲線、Bezier曲線及びSpline曲線等が挙げられる。

また、上記実施形態では、トレーラ４の第１目標経路Ｒ１及び第２目標経路Ｒ２である幾何経路の曲率最大値がトレーラ４の旋回曲率最大値以下でないと判断されたとき、または連結車２が周辺に存在する障害物５０と接触すると判断されたときは、連結車２の折返し位置Ａ３が再設定されているが、折返し位置Ａ３の再設定を一定回数繰り返し行っても、判断制約の条件を満たさないときは、与えられた駐車目標位置Ａ２への連結車２の誘導は不可であるとして、ドライバに対して駐車目標位置Ａ２または駐車開始位置Ａ１を変更するように通知してもよい。

また、上記実施形態では、トレーラ４の第１目標経路Ｒ１及び第２目標経路Ｒ２である幾何経路の曲率最大値がトレーラ４の旋回曲率最大値以下でないと判断されたときは、連結車２の折返し位置Ａ３が再設定されているが、折返し位置Ａ３と共に駐車目標位置Ａ２を再設定してもよい。連結車２が周辺に存在する障害物５０と接触すると判断されたときも、同様である。

また、上記実施形態では、連結車２は、セミトレーラ式のトラックであるが、本発明は、フルトレーラ式のトラックにも適用可能である。また、本発明は、特にトラックには限られず、バス等の連結車にも適用可能である。

また、上記実施形態の駐車経路生成装置１６は、自動運転により連結車２を駐車させる駐車制御装置１に具備されているが、本発明は、特にその形態には限られず、駐車支援システムを利用して連結車２の駐車を手動運転で実施するような場合にも適用可能である。

１…駐車制御装置、２…連結車、３…トラクタ、４…トレーラ、６…車体、９…車体、１２…連結角センサ（連結角検出部）、１３…カメラ（環境検知部）、１６…駐車経路生成装置、１７…ＧＰＳ受信機（位置検出部）、２１…駐車開始位置取得部、２２…駐車目標位置設定部、２３…折返し位置設定部、２４…目標経路生成部、２５…曲率制約判定部、２６…移動経路算出部、２７…車体軌跡予測部、２８…接触判定部、２９…データ系列算出部、３２…追従制御部、５０…障害物、Ａ１…駐車開始位置、Ａ２…駐車目標位置、Ａ３…折返し位置、Ｒ１…第１目標経路、Ｒ２…第２目標経路。

Claims (6)

1. トラクタ及びトレーラを具備した連結車を駐車させるための駐車経路を生成する駐車経路生成装置であって、
   前記連結車の現在位置を検出する位置検出部と、
   前記連結車の周辺環境を検知する環境検知部と、
   前記位置検出部により検出された前記連結車の現在位置を用いて、前記連結車の駐車開始位置を取得する駐車開始位置取得部と、
   前記位置検出部により検出された前記連結車の現在位置を含む情報を用いて、前記連結車の駐車目標位置を設定する駐車目標位置設定部と、
   前記駐車開始位置取得部により取得された前記駐車開始位置と前記駐車目標位置設定部により設定された前記駐車目標位置と前記環境検知部により検知された前記連結車の周辺環境とを用いて、前記連結車の折返し位置を設定する折返し位置設定部と、
   前記駐車開始位置取得部により取得された前記駐車開始位置と前記駐車目標位置設定部により設定された前記駐車目標位置と前記折返し位置設定部により設定された前記折返し位置とに基づいて、前記駐車開始位置から前記折返し位置までの前記トレーラの第１目標経路と前記折返し位置から前記駐車目標位置までの前記トレーラの第２目標経路とを生成する目標経路生成部と、
   前記目標経路生成部により生成された前記第１目標経路及び前記第２目標経路に基づいて、前記トラクタの移動経路を算出する移動経路算出部と、
   前記移動経路算出部により前記トラクタの移動経路が算出された後、前記連結車の幾何モデルを用いて、前記連結車を前記駐車開始位置及び前記駐車目標位置から前記折返し位置までそれぞれ前進走行で追従制御を行う車両運動シミュレーションを実施することで、前記トラクタの操舵角のデータ系列と前記トラクタ及び前記トレーラの各姿勢角のデータ系列とを算出するデータ系列算出部とを備え、
   前記目標経路生成部は、前記第１目標経路として、前記駐車開始位置から前記折返し位置までの間で曲率が連続的に変化するような曲線部を有する幾何経路を生成し、前記第２目標経路として、前記折返し位置から前記駐車目標位置までの間で曲率が連続的に変化するような曲線部を有する幾何経路を生成する駐車経路生成装置。
2. トラクタ及びトレーラを具備した連結車を駐車させるための駐車経路を生成する駐車経路生成装置であって、
   前記連結車の現在位置を検出する位置検出部と、
   前記連結車の周辺環境を検知する環境検知部と、
   前記位置検出部により検出された前記連結車の現在位置を用いて、前記連結車の駐車開始位置を取得する駐車開始位置取得部と、
   前記位置検出部により検出された前記連結車の現在位置を含む情報を用いて、前記連結車の駐車目標位置を設定する駐車目標位置設定部と、
   前記駐車開始位置取得部により取得された前記駐車開始位置と前記駐車目標位置設定部により設定された前記駐車目標位置と前記環境検知部により検知された前記連結車の周辺環境とを用いて、前記連結車の折返し位置を設定する折返し位置設定部と、
   前記駐車開始位置取得部により取得された前記駐車開始位置と前記駐車目標位置設定部により設定された前記駐車目標位置と前記折返し位置設定部により設定された前記折返し位置とに基づいて、前記駐車開始位置から前記折返し位置までの前記トレーラの第１目標経路と前記折返し位置から前記駐車目標位置までの前記トレーラの第２目標経路とを生成する目標経路生成部と、
   前記目標経路生成部により生成された前記第１目標経路及び前記第２目標経路に基づいて、前記トラクタの移動経路を算出する移動経路算出部と、
   前記トラクタと前記トレーラとの連結角を検出する連結角検出部とを備え、
   前記駐車開始位置取得部は、前記連結車の現在位置と前記連結角とに基づいて、前記駐車開始位置としての前記トレーラの位置座標、方位角及び旋回曲率を取得し、
   前記駐車目標位置設定部は、前記連結車の現在位置を含む情報と前記連結角とに基づいて、前記駐車目標位置としての前記トレーラの位置座標、方位角及び旋回曲率を設定し、
   前記折返し位置設定部は、前記駐車開始位置と前記駐車目標位置と前記連結車の周辺環境と前記連結角とに基づいて、前記折返し位置としての前記トレーラの位置座標、方位角及び旋回曲率を設定し、
   前記目標経路生成部は、前記第１目標経路として、前記駐車開始位置から前記折返し位置までの間で曲率が連続的に変化するような曲線部を有する幾何経路を生成し、前記第２目標経路として、前記折返し位置から前記駐車目標位置までの間で曲率が連続的に変化するような曲線部を有する幾何経路を生成する駐車経路生成装置。
3. 前記第１目標経路及び前記第２目標経路である幾何経路の曲率最大値が前記トレーラの旋回曲率最大値以下であるかどうかを判断する曲率制約判定部を更に備え、
   前記移動経路算出部は、前記曲率制約判定部により前記幾何経路の曲率最大値が前記旋回曲率最大値以下であると判断されたときに、前記トラクタの移動経路を算出し、
   前記折返し位置設定部は、前記曲率制約判定部により前記幾何経路の曲率最大値が前記旋回曲率最大値以下でないと判断されたときは、前記折返し位置を再設定する請求項１または２記載の駐車経路生成装置。
4. 前記第１目標経路及び前記第２目標経路に基づいて、前記トレーラの車体の通過軌跡を予測すると共に、前記移動経路に基づいて、前記トラクタの車体の通過軌跡を予測する車体軌跡予測部と、
   前記環境検知部により検知された前記連結車の周辺環境と前記車体軌跡予測部により予測された前記車体の通過軌跡とに基づいて、前記車体が前記連結車の周辺に存在する障害物と接触するかどうかを判断する接触判定部を更に備え、
   前記折返し位置設定部は、前記接触判定部により前記連結車が前記障害物と接触すると判断されたときは、前記折返し位置を再設定する請求項１～３の何れか一項記載の駐車経路生成装置。
5. トラクタ及びトレーラを具備した連結車を駐車させる駐車制御装置であって、
   前記連結車の現在位置を検出する位置検出部と、
   前記連結車の周辺環境を検知する環境検知部と、
   前記位置検出部により検出された前記連結車の現在位置を用いて、前記連結車の駐車開始位置を取得する駐車開始位置取得部と、
   前記位置検出部により検出された前記連結車の現在位置を含む情報を用いて、前記連結車の駐車目標位置を設定する駐車目標位置設定部と、
   前記駐車開始位置取得部により取得された前記駐車開始位置と前記駐車目標位置設定部により設定された前記駐車目標位置と前記環境検知部により検知された前記連結車の周辺環境とを用いて、前記連結車の折返し位置を設定する折返し位置設定部と、
   前記駐車開始位置取得部により取得された前記駐車開始位置と前記駐車目標位置設定部により設定された前記駐車目標位置と前記折返し位置設定部により設定された前記折返し位置とに基づいて、前記駐車開始位置から前記折返し位置までの前記トレーラの第１目標経路と前記折返し位置から前記駐車目標位置までの前記トレーラの第２目標経路とを生成する目標経路生成部と、
   前記目標経路生成部により生成された前記第１目標経路及び前記第２目標経路に基づいて、前記トラクタの移動経路を算出する移動経路算出部と、
   前記移動経路算出部により前記トラクタの移動経路が算出された後、前記連結車の幾何モデルを用いて、前記連結車を前記駐車開始位置及び前記駐車目標位置から前記折返し位置までそれぞれ前進走行で追従制御を行う車両運動シミュレーションを実施することで、前記トラクタの操舵角のデータ系列と前記トラクタ及び前記トレーラの各姿勢角のデータ系列とを算出するデータ系列算出部と、
   前記目標経路生成部により生成された前記第１目標経路及び前記第２目標経路と前記移動経路算出部により算出された前記移動経路とに沿って前記連結車を前記駐車目標位置に駐車させるように、前記トラクタの操舵制御を行う追従制御部とを備え、
   前記目標経路生成部は、前記第１目標経路として、前記駐車開始位置から前記折返し位置までの間で曲率が連続的に変化するような曲線部を有する幾何経路を生成し、前記第２目標経路として、前記折返し位置から前記駐車目標位置までの間で曲率が連続的に変化するような曲線部を有する幾何経路を生成し、
   前記追従制御部は、前記データ系列算出部により算出された前記トラクタの操舵角のデータ系列と前記トラクタ及び前記トレーラの各姿勢角のデータ系列とに従って前記連結車を前記駐車目標位置に駐車させるように、前記トラクタの操舵制御を行う駐車制御装置。
6. 前記トラクタと前記トレーラとの連結角を検出する連結角検出部を更に備え、
   前記追従制御部は、前記トラクタの操舵角のデータ系列と前記トラクタ及び前記トレーラの各姿勢角のデータ系列とに従って前記連結車を前記駐車目標位置に駐車させるように、前記トラクタの操舵のフィードフォワード制御を行うと共に、前記連結角と前記連結車の現在位置とに基づいて前記連結車を前記駐車目標位置に駐車させるように、前記トラクタの操舵のフィードバック制御を行う請求項５記載の駐車制御装置。

Images