道路形状推定装置、道路形状推定方法及びプログラム
【課題】道路形状を正確に推定することができるようにする。
【解決手段】形状補間点の補間点データを取得するデータ取得処理手段と、補間点データに基づいて各形状補間点における曲率半径を算出する半径算出処理手段と、前記各曲率半径に基づいてコーナを検出し、開始候補点及び終了候補点を設定するコーナ検出処理手段と、前記コーナの開始候補点及び終了候補点に基づいて中心仮想点を、該中心仮想点より開始候補点側に前方仮想点を、前記中心仮想点より終了候補点側に後方仮想点を設定し、コーナの仮想曲率半径を最小半径として算出する最小半径算出処理手段とを有する。中心仮想点、前方仮想点及び後方仮想点に基づいて仮想曲率半径が算出されるので、道路形状を正確に推定することができる。
【解決手段】形状補間点の補間点データを取得するデータ取得処理手段と、補間点データに基づいて各形状補間点における曲率半径を算出する半径算出処理手段と、前記各曲率半径に基づいてコーナを検出し、開始候補点及び終了候補点を設定するコーナ検出処理手段と、前記コーナの開始候補点及び終了候補点に基づいて中心仮想点を、該中心仮想点より開始候補点側に前方仮想点を、前記中心仮想点より終了候補点側に後方仮想点を設定し、コーナの仮想曲率半径を最小半径として算出する最小半径算出処理手段とを有する。中心仮想点、前方仮想点及び後方仮想点に基づいて仮想曲率半径が算出されるので、道路形状を正確に推定することができる。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、道路形状推定装置、道路形状推定方法及びプログラムに関するものである。
【背景技術】
【0002】
従来、ナビゲーション装置においては、例えば、GPS(グローバルポジショニングシステム)によって、自車である車両の現在の位置、すなわち、現在地が検出され、データ記録部から地図データが読み出され、表示部に地図画面が形成され、該地図画面に、現在地を表す自車位置、該自車位置の周辺の地図等が表示されるようになっている。したがって、運転者は、前記地図画面に表示された自車位置等に従って車両を走行させることができる。
【0003】
また、運転者が目的地を入力し、探索条件を設定すると、該探索条件に基づいて、経路探索処理が行われ、前記地図データに従って、現在地で表される出発地から目的地までの経路が探索される。そして、探索された経路、すなわち、探索経路は、前記地図画面に自車位置と共に表示され、探索経路の案内、すなわち、経路案内が行われる。したがって、運転者は、表示された探索経路に沿って車両を走行させることができる。
【0004】
ところで、前記ナビゲーション装置においては、ナビゲーション装置において取得された情報を利用して自動変速機の変速段を変更したり、エンジンの出力を変更したりして、車両の走行制御を行うようにした車両制御システムが提供されている。
【0005】
そのために、該車両制御システムに道路形状推定装置が配設され、該道路形状推定装置によって道路形状が推定されるようになっている。そして、前記道路形状推定装置は、前記ナビゲーション装置のデータ記録部に配設されたデータベースから道路データを読み出すことによって、データベース上の道路に沿った所定の道路リンクを形状推定道路とし、該形状推定道路において自車位置より前方における道路形状を複数の点で表すために設定された補間点、すなわち、形状補間点に関する補間点データを取得し、互いに隣接する3点の形状補間点に対する3点計算法によって各形状補間点における道路の曲率半径(以下、単に「半径」という。)を算出し、各半径に基づいて、データベース上でコーナを検出し、設定するとともに、コーナの開始点を表す開始候補点、及びコーナの終了点を表す終了候補点を設定する。そして、前記形状推定道路内において、各形状補間点における半径のうちの最も小さい半径をそのコーナにおける最小半径とし、最小半径に基づいてクロソイド係数を推定し、該クロソイド係数に基づいて、コーナの近似式によって表されるクロソイド曲線を算出し、該クロソイド曲線に基づいて、開始候補点及び終了候補点の位置を補正し、道路形状を推定し、補正された開始候補点及び終了候補点の位置を、道路形状を表すデータとしてデータ記録部に記録する(例えば、特許文献1参照。)。
【特許文献1】特開2005−214839号公報
【発明の開示】
【発明が解決しようとする課題】
【0006】
しかしながら、前記従来の道路形状推定装置においては、互いに隣接する3点の形状補間点に対する3点計算法によって算出された半径のうちの最も小さい半径をコーナの曲率半径として算出すると、曲率半径の大きいコーナにおいては、実際の道路上の曲率半径と算出されたコーナの曲率半径との差が大きくなってしまうので、道路形状を正確に推定することができない。
【0007】
本発明は、前記従来の道路形状推定装置の問題点を解決して、道路形状を正確に推定することができる道路形状推定装置、道路形状推定方法及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【0008】
そのために、本発明の道路形状推定装置においては、道路に沿って設定され、道路の形状を表す複数の形状補間点の補間点データを取得するデータ取得処理手段と、前記道路における所定の区間内の前記補間点データに基づいて各形状補間点における曲率半径を算出する半径算出処理手段と、前記各曲率半径に基づいて、前記所定の区間内のコーナを検出するとともに、該検出されたコーナの開始候補点及び終了候補点を設定するコーナ検出処理手段と、前記コーナの開始候補点及び終了候補点に基づいて中心仮想点を、該中心仮想点より開始候補点側に前方仮想点を、前記中心仮想点より終了候補点側に後方仮想点を設定し、中心仮想点、前方仮想点及び後方仮想点に基づいて前記コーナの仮想曲率半径を最小半径として算出する最小半径算出処理手段とを有する。
【発明の効果】
【0009】
本発明によれば、コーナ内の中心付近に中心仮想点が、該中心仮想点より開始候補点側に前方仮想点が、前記中心仮想点より終了候補点側に後方仮想点が設定され、中心仮想点、前方仮想点及び後方仮想点に基づいて仮想曲率半径が最小半径として算出されるので、道路形状を正確に推定することができる。
【発明を実施するための最良の形態】
【0010】
以下、本発明の実施の形態について図面を参照しながら詳細に説明する。
【0011】
図1は本発明の実施の形態における車両制御システムのブロック図、図2は本発明の実施の形態における道路形状推定処理手段の動作を示すメインフローチャートである。
【0012】
図において、14は情報端末、例えば、車両に搭載された車載装置としてのナビゲーション装置であり、該ナビゲーション装置14は、車両の現在地を自車位置として、車両の方位を自車方位として検出する現在地検出部としてのGPSセンサ15、図示されない地図データのほかに各種の情報が記録された情報記録部としてのデータ記録部16、ナビゲーション処理等の各種の演算処理を行うナビゲーション処理部17、操作者である運転者が操作することによって所定の入力を行うための第1の入力部としての操作部34、図示されない画面に表示された画像によって各種の表示を行い、運転者に通知するための第1の出力部としての表示部35、運転者が音声によって所定の入力を行うための第2の入力部としての音声入力部36、音声出力を行い、各種の情報を運転者に通知するための第2の出力部としての音声出力部37、及び通信端末として機能する送受信部としての通信部38を備え、前記ナビゲーション処理部17に、GPSセンサ15、データ記録部16、操作部34、表示部35、音声入力部36、音声出力部37及び通信部38が接続される。また、前記ナビゲーション処理部17には車速を検出する車速検出部としての車速センサ44等が接続される。そして、前記GPSセンサ15は、自車位置及び自車方位のほかに時刻を検出する。なお、GPSセンサ15と独立させて図示されない方位センサを配設し、該方位センサによって自車方位を検出することもできる。
【0013】
前記データ記録部16には、地図データファイルから成るデータベースが配設され、該データベースに地図データが記録される。該地図データには、各交差点(分岐点も含む。)間を結ぶ道路に関する道路データ、前記道路の端点(始点及び終点)を表すノードに関するノードデータ、前記交差点に関する交差点データ、探索用に加工された探索データ、施設に関する施設データ等が含まれるほか、道路上の地物に関する地物データが含まれる。なお、前記道路データには、前記データベース上で道路の始点から終点までの道路リンクを表すデータ、及び道路リンク上において、道路形状を表すために道路に沿って設定された複数の設定点としての形状補間点のデータ(以下「補間点データ」という。)が含まれる。補間点データには、各形状補間点の番号、座標等が含まれる。なお、各ノードは、道路リンク上の始点又は終点を表すので、形状補間点でもある。
【0014】
さらに、前記データ記録部16には、統計データファイルから成るデータベース、走行履歴データファイルから成るデータベース等が配設され、前記統計データファイルに統計データが、前記走行履歴データファイルに走行履歴データが、いずれも実績データとして記録される。
【0015】
そして、前記データ記録部16は、前記各種のデータを記録するために、ハードディスク、CD、DVD、光ディスク等の図示されないディスクを備えるほかに、各種のデータを読み出したり、書き込んだりするための読出・書込ヘッド等の図示されないヘッドを備える。また、前記データ記録部16にメモリカード等を使用することができる。
【0016】
前記ナビゲーション処理部17は、ナビゲーション装置14の全体の制御を行う制御装置としての、かつ、演算装置としてのCPU31、該CPU31が各種の演算処理を行うに当たってワーキングメモリとして使用されるRAM32、制御用のプログラムのほか、目的地までの経路の探索、経路案内等を行うための各種のプログラムが記録されたROM33、各種のデータ、プログラム等を記録するために使用される図示されないフラッシュメモリ等を備える。
【0017】
前記操作部34として、表示部35とは独立させて配設された図示されないキーボード、マウス等を使用することができる。また、前記操作部34として、前記表示部35に形成された画面に画像で表示された各種のキー、スイッチ、ボタン等の画像操作部をタッチ又はクリックすることによって、所定の入力操作を行うことができるようにしたタッチパネルを使用することができる。
【0018】
前記表示部35としてディスプレイが使用され、表示部35に形成された各種の画面に、自車位置、自車方位等を表示したり、地図、探索経路、該探索経路に沿った案内情報、交通情報等を表示したり、探索経路における次の交差点までの距離、次の交差点における進行方向を表示したりすることができる。
【0019】
また、音声入力部36は、図示されないマイクロホン等によって構成され、音声によって必要な情報を入力することができる。さらに、音声出力部37は、図示されない音声合成装置及びスピーカを備え、前記探索経路の経路案内を音声出力によって行う。
【0020】
前記通信部38は、道路交通情報センタから送信された現況の交通情報、一般情報等の各種の情報を受信するための図示されないビーコンレシーバ、FM放送局を介して前記各種の情報をFM多重放送として受信するための図示されないFM受信機等を備える。また、通信部38は、図示されない情報センタから、交通情報、一般情報等の情報のほかに、地図データ、統計データ、走行履歴データ等のデータを図示されないネットワークを介して受信することができる。
【0021】
なお、ナビゲーション処理部17、CPU31等は、単独で、又は二つ以上組み合わせることによってコンピュータとして機能し、各種のプログラム、データ等に基づいて演算処理を行う。また、前記データ記録部16、RAM32、ROM33、フラッシュメモリ等によって記憶装置及び記録媒体が構成される。そして、演算装置として、CPU31に代えてMPU等を使用することもできる。
【0022】
なお、10は自動変速機、11は自動変速機制御装置、51はエンジン制御装置、52はエンジンである。また、前記自動変速機制御装置11、ナビゲーション装置14、エンジン制御装置51等によって車両制御システムが構成される。
【0023】
次に、前記構成のナビゲーション装置14の基本動作について説明する。
【0024】
まず、運転者によって操作部34が操作され、ナビゲーション装置14が起動されると、CPU31の図示されない現在地読込処理手段は、現在地読込処理を行い、GPSセンサ15によって検出された自車位置及び自車方位を読み込む。次に、前記CPU31の図示されないマッチング処理手段は、マッチング処理を行い、読み込まれた自車位置の軌跡、及び自車位置の周辺の道路を構成する各道路リンクの形状、配列等に基づいて、車両がいずれの道路リンク上に位置するかの判定を行うことによって、自車位置を特定する。
【0025】
続いて、CPU31の図示されない基本情報取得処理手段は、基本情報取得処理を行い、前記地図データをデータ記録部16から読み出して取得する。なお、地図データを前記情報センタ等から取得することもでき、その場合、前記基本情報取得処理手段は、受信した地図データをフラッシュメモリにダウンロードする。
【0026】
そして、前記CPU31の図示されない表示処理手段は、表示処理を行い、前記表示部35に各種の画面を形成する。例えば、表示処理手段の地図表示処理手段は、地図表示処理を行い、表示部35に地図画面を形成し、該地図画面に周囲の地図を表示するとともに、自車位置及び自車方位を表示する。
【0027】
したがって、運転者は、前記周囲の地図、自車位置及び自車方位に従って車両を走行させることができる。
【0028】
また、運転者が操作部34を操作して目的地を入力すると、CPU31の図示されない目的地設定処理手段は、目的地設定処理を行い、目的地を設定する。なお、必要に応じて出発地を入力し、設定することもできる。また、あらかじめ所定の地点を登録しておき、登録された地点を目的地として設定することができる。続いて、運転者が操作部34を操作して探索条件を入力すると、CPU31の図示されない探索条件設定処理手段は、探索条件設定処理を行い、探索条件を設定する。
【0029】
このようにして目的地及び探索条件が設定されると、CPU31の図示されない経路探索処理手段は、経路探索処理を行い、前記自車位置、自車方位、目的地、探索条件等を読み込むとともに、データ記録部16から探索データ等を読み出し、自車位置、自車方位、目的地、探索データ等に基づいて、自車位置で表される出発地から目的地までの経路を前記探索条件で探索し、探索経路を表す経路データを出力する。このとき、経路探索処理においては、各道路リンクごとに付与されたリンクコストの合計が最も小さい経路が探索経路とされる。なお、出発地を、自車位置に代えて、運転者が設定した所定の地点とし、該所定の地点から目的地までの経路を探索することもできる。
【0030】
続いて、前記CPU31の図示されない案内処理手段は、案内処理を行い、経路案内を行う。そのために、前記案内処理手段の案内表示処理手段は、案内表示処理を行い、前記経路データを読み込み、経路データに基づいて前記地図画面に探索経路を表示する。
【0031】
なお、例えば、経路案内の対象となる地点である交差点で車両を右左折させる必要がある場合、前記交差点が、車両を進行させる方向の案内を行う対象となる案内交差点として設定され、前記案内処理手段の音声出力処理手段は、音声出力処理を行い、前記案内交差点に車両が到達する前に、音声出力によって経路案内を行う。
【0032】
また、前記案内処理手段の案内点拡大図形成処理手段は、案内点拡大図形成処理を行い、車両が案内交差点に到達する前に、地図画面の所定の領域に案内交差点の拡大図、すなわち、案内点拡大図としての交差点拡大図を形成し、交差点拡大図による経路案内を行う。そのために、探索経路上の前記案内交差点より手前(自車位置側)の、設定された距離だけ離れた箇所に車両が到達すると、前記交差点拡大図が表示される。この場合、該交差点拡大図に、案内交差点の周辺の地図、探索経路、案内交差点において目印になる施設等の陸標が表示される。
【0033】
ところで、本実施の形態においては、前記ナビゲーション装置14において取得された情報を、自動変速機制御装置11に送信して、自車位置より前方の道路形状に対応させて自動変速機10の変速段を変更したり、エンジン制御装置51に送信して、自車位置より前方の道路形状に対応させてエンジン52の出力を変更したりして、車両の走行制御を行うようにしている。
【0034】
そのために、前記CPU31は道路形状推定装置として機能し、CPU31の図示されない道路形状推定処理手段は、道路形状推定処理を行い、後述される方法で道路形状を推定する。そして、CPU31の図示されない走行制御処理手段は、走行制御処理を行い、推定された道路形状に従って、自動変速機10の変速段を変更したり、エンジン52の出力を変更したりするための信号を前記自動変速機制御装置11、エンジン制御装置51等に送信する。
【0035】
次に、図2に従って、前記道路形状推定処理手段の動作について説明する。
【0036】
まず、前記道路形状推定処理手段のデータ取得処理手段は、データ取得処理を行い、道路形状を推定する対象となる道路を含む、自車位置より前方の所定の領域を道路形状推定範囲として設定し、データ記録部16から道路データを読み出し、前記道路形状推定範囲内のノードデータを取得する。
【0037】
この場合、道路形状推定範囲内において、ノードデータで表される各ノードのうちの最初のノードから最後のノードまでの、互いに隣接する各ノード間の道路リンクについて、順に道路形状の推定が行われる。なお、この場合、データベース上の道路において道路形状の推定が行われる各道路リンクを形状推定道路とする。
【0038】
そこで、前記データ取得処理手段は、各形状推定道路において道路形状の推定を行うために、自車位置より前方の、形状推定道路上における形状補間点の補間点データを取得する。なお、本実施の形態において、前記データ取得処理手段は、ノードデータ及び補間点データを、データ記録部16から読み出すことによって取得するようになっているが、前記情報センタ等からネットワークを介して受信することによって取得することもできる。
【0039】
次に、前記道路形状推定処理手段の半径算出処理手段は、半径算出処理を行い、自車位置より前方の形状推定道路上における形状補間点の補間点データに基づいて、3点計算法によって形状推定道路の各形状補間点における半径(曲率半径)を算出する。続いて、前記道路形状推定処理手段のコーナ検出処理手段は、コーナ検出処理を行い、前記算出された半径に基づいて、データベース上の形状推定道路内においてコーナを検出し、コーナが開始される形状補間点を開始候補点として、データベース上でコーナが終了する形状補間点を終了候補点として設定する。
【0040】
そして、前記道路形状推定処理手段のコーナ結合処理手段は、実際の道路上では一つのコーナとして見なされるべきコーナが、データベース上における形状補間点の設定のされ方によって、複数のコーナとして検出された場合に、コーナ結合処理を行い、検出されたコーナの結合を行う。
【0041】
次に、前記道路形状推定処理手段の第1、第2のコーナ分割処理手段は、実際の道路上では複数のコーナとして見なされるべきコーナが、データベース上における形状補間点の設定のされ方によって、一つのコーナとして検出された場合に、第1、第2のコーナ分割処理を行い、検出されたコーナの分割を行う。この場合、第1のコーナ分割処理においては、コーナ内の所定の形状補間点が、コーナの分割を行うための閾(しきい)値以上の半径を有する場合に、第2のコーナ分割処理においては、コーナ内の所定の形状補間点間におけるセグメントが閾値以上の長さを有する場合に、検出されたコーナの分割が行われる。
【0042】
このようにして、コーナの結合が行われたり、分割が行われたりして、形状補間点に基づく適正なコーナが設定されると、前記道路形状推定処理手段の最小半径算出処理手段は、最小半径算出処理を行い、各コーナについて、クロソイド係数を推定するための適正な最小半径を算出する。
【0043】
続いて、前記道路形状推定処理手段のクロソイド係数推定処理手段は、クロソイド係数推定処理を行い、前記最小半径に基づいてクロソイド係数を推定し、道路形状推定処理手段のクロソイド曲線算出処理手段は、クロソイド曲線算出処理を行い、クロソイド係数に基づいてコーナの近似式を形成し、コーナの道路形状を表すクロソイド曲線を算出する。
【0044】
このようにして、該クロソイド曲線が算出されると、道路形状推定処理手段のフィッティング処理手段は、フィッティング処理を行い、クロソイド曲線をコーナ上の各形状補間点に合わせることによって、開始候補点及び終了候補点を補正し、実際の道路上でコーナが開始される点、及びコーナが終了する点に近似する位置にコーナの開始点及び終了点を設定する。
【0045】
そして、道路形状推定処理手段の開始点・終了点記録処理手段は、開始点・終了点記録処理を行い、前記開始点及び終了点を道路形状を表すデータとしてデータ記録部16に記録する。このようにして、道路形状が推定される。
【0046】
次に、フローチャートについて説明する。
ステップS1 半径算出処理を行う。
ステップS2 コーナ検出処理を行う。
ステップS3 コーナ結合処理を行う。
ステップS4 第1のコーナ分割処理を行う。
ステップS5 第2のコーナ分割処理を行う。
ステップS6 最小半径算出処理を行う。
ステップS7 クロソイド係数推定処理を行う。
ステップS8 クロソイド曲線算出処理を行う。
ステップS9 フィッティング処理を行う。
ステップS10 開始点・終了点記録処理を行い、処理を終了する。
【0047】
次に、図2のステップS1における半径算出処理のサブルーチンについて説明する。
【0048】
図3は本発明の実施の形態における半径算出処理のサブルーチンを示す図、図4は本発明の実施の形態における半径を算出する原理を説明する図である。
【0049】
図4に示されるように、半径算出処理の対象となる形状補間点を対象点maとし、該対象点maの一つ手前(自車位置側)の形状補間点を前隣接点mbとし、対象点maの一つ先(自車位置から離れる側)の形状補間点を後隣接点mcとするとともに、対象点maと前隣接点mbとを結ぶ、対象点maより手前側のセグメント(以下「前側セグメント」という。)のセグメント長をL1とし、対象点maと後隣接点mcとを結ぶ、対象点maより先のセグメント(以下「後側セグメント」という。)のセグメント長をL2とする。また、前記対象点ma、前隣接点mb及び後隣接点mcを通過する円をCrとしたときの、円Crの中心をQとし、円Crの半径をRとする。さらに、対象点maにおいて、前側セグメントに対して後側セグメントの成す角度を方位角θとする。
【0050】
そして、対象点maと後隣接点mcとを結ぶ方向をx軸方向とし、該x軸方向に対する直角の方向をy軸方向としたときの、x軸方向における対象点maと前隣接点mbとの間の距離をXとし、x軸方向における対象点maと中心Qとの間の距離をAとし、x軸方向における前隣接点mbと中心Qとの間の距離をGとするとともに、y軸方向における対象点maと前隣接点mb間の距離をYとし、y軸方向における前隣接点mbと中心Qとの間の距離をHとし、y軸方向における対象点maと中心Qとの間の距離をFとすると、
X=L1cosθ
A=L2/2
G=X+A
Y=L1sinθ
F=√(R2 −A2 )
H=F−Y
になる。
【0051】
この場合、距離Gの二乗と距離Hの二乗との和が半径Rの二乗と等しいので、式(1)の関係が成り立つ。
【0052】
R2 =G2 +H2 ……(1)
次に、該式(1)の距離G、Hに前記各値を代入すると式(2)を得ることができる。
【0053】
R2 =X2 +2XA+A2 +R2 −A2
−2Y√(R2 −A2 )+Y2 ……(2)
続いて、該式(2)を変形すると式(3)〜(6)を得ることができる。
【0054】
2Y√(R2 −A2 )=X2 +Y2 +2XA ……(3)
R2 −A2 ={(X2 +Y2 +2XA)/2Y}2 ……(4)
R2 ={(X2 +Y2 +2XA)/2Y}2 +A2 ……(5)
R2 =(L12 +2L1・L2・cosθ+L22 )
/(2sinθ)2 ……(6)
該式(6)から、半径Rは、セグメント長L1 、L2 及び方位角θの関数であることが分かる。
【0055】
そこで、前記半径算出処理手段は、前記補間点データを読み込み、対象点ma、前隣接点mb及び後隣接点mcの各座標に基づいて、セグメント長L1、L2を算出するとともに、方位角θを算出する。
【0056】
続いて、前記半径算出処理手段は、セグメント長L1、長L2及び方位角θに基づいて前記式(6)によって円Crの半径Rを算出する。このようにして、形状推定道路上に対象点maにおける半径Rを算出することができる。
【0057】
次に、フローチャートについて説明する。
ステップS1−1 前側セグメントのセグメント長L1を算出する。
ステップS1−2 後側セグメントのセグメント長L2を算出する。
ステップS1−3 方位角θを算出する。
ステップS1−4 半径Rを算出し、リターンする。
【0058】
なお、本実施の形態においては、前記半径算出処理手段によって方位角θ及び半径Rを計算に基づいて算出するようになっているが、あらかじめ算出された方位角θ及び半径Rを、道路データの一部としてデータ記録部16のデータベースに記録しておき、データ記録部16から読み出すことによって算出することもできる。
【0059】
次に、図2のステップS2におけるコーナ検出処理について説明する。
【0060】
図5は本発明の実施の形態におけるコーナ検出処理の第1の説明図、図6は本発明の実施の形態におけるコーナ検出処理の第2の説明図、図7は本発明の実施の形態におけるコーナ検出処理の第3の説明図、図8は本発明の実施の形態におけるコーナ検出処理の第4の説明図、図9は本発明の実施の形態におけるコーナ検出処理の第5の説明図、図10は本発明の実施の形態におけるコーナ検出処理の第6の説明図、図11は本発明の実施の形態におけるコーナ検出処理の第7の説明図、図12は本発明の実施の形態におけるコーナ検出処理の第8の説明図である。
【0061】
図において、ri(i=1、2、…)は図示されない二つのノード間に設定された形状推定道路、mi(i=1、2、…)は該形状推定道路riに沿って、車両の進行方向(矢印G方向)における手前側から順に設定された複数の形状補間点、θi(i=1、2、…)は各形状補間点miにおける方位角である。なお、該方位角θiは、前側セグメントに対して後側セグメントの成す角度であり、前側セグメントに対して後側セグメントが車両の進行方向における右側に位置する場合、正の値を採り、前側セグメントに対して後側セグメントが車両の進行方向における左側に位置する場合、負の値を採る。
【0062】
まず、前記コーナ検出処理手段の開始候補点・終了候補点設定処理手段は、開始候補点・終了候補点設定処理を行い、前記各形状補間点miにおける半径Ri(i=1、2、…)を読み込み、該半径Riが、あらかじめ設定されたコーナ検出用の閾値Rth(本実施の形態においては、1000〔m〕)以下であるかどうかを判断し、半径が閾値Rth以下の形状補間点を抽出する。図5においては、形状補間点m3〜m9が、直線に近く、閾値Rth以下であると判断され、抽出される。
【0063】
続いて、前記開始候補点・終了候補点設定処理手段は、抽出された各形状補間点において、複数の連続する形状補間点(以下「連続形状補間点群」という。)があるかどうかを判断し、連続形状補間点群がある場合、連続形状補間点群における最も手前(自車位置側)の形状補間点を開始候補点とする。図5及び6においては、形状補間点m3〜m9によって連続形状補間点群が構成され、形状補間点m3が開始候補点s1にされる。
【0064】
また、前記開始候補点・終了候補点設定処理手段は、抽出された形状補間点について、前記半径算出処理手段によって算出された方位角θを手前側から順に読み込み、所定の形状補間点において方位角の正負が反転する(一つ手前の形状補間点に対して方位角の方向が逆になる。)かどうかを判断し、所定の形状補間点において方位角の正負が反転する場合に、一つ先の形状補間点において正負が反転するかどうかを判断する。そして、所定の形状補間点において方位角の正負が反転し、かつ、一つ先の形状補間点において正負が反転しない場合は、前記所定の形状補間点を開始候補点s1とする。図7に示されるような形状を有する形状推定道路r2において、形状補間点m11〜m16が設定されている場合、各形状補間点m12〜m15において、各方位角θ12及びθ13は負の値を採り、方位角θ14及びθ15は正の値を採る。すなわち、形状補間点m14において方位角θ14の正負が負から正に反転し、一つ先の形状補間点m15において方位角θ15の正負が正のままで反転しないので、形状補間点m14が開始候補点s1にされる。
【0065】
次に、前記開始候補点・終了候補点設定処理手段は、抽出された各形状補間点において、連続形状補間点群があるかどうかを判断し、連続形状補間点群がある場合、連続形状補間点群における最も先側(自車位置から離れる側)の形状補間点を終了候補点とする。図5及び8においては、形状補間点m3〜m9によって連続形状補間点群が構成され、形状補間点m9が終了候補点e1にされる。
【0066】
また、前記開始候補点・終了候補点設定処理手段は、所定の形状補間点において方位角の正負が反転するかどうかを判断し、所定の形状補間点において方位角の正負が反転しない場合に、一つ先の形状補間点において正負が反転するかどうかを判断する。そして、所定の形状補間点において方位角の正負が反転せず、一つ先の形状補間点において正負が反転する場合は、前記所定の形状補間点を終了候補点e1とする。図9に示されるような形状を有する形状推定道路r3において、形状補間点m21〜m26が設定されている場合に、各形状補間点m22〜m25における方位角θ22及びθ23は正の値を採り、方位角θ24及びθ25は負の値を採る。すなわち、形状補間点m23においては方位角θ23の正負が正のままで反転せず、一つ先の形状補間点m24においては方位角θ24の正負が正から負に反転するので、形状補間点m23が終了候補点e1にされる。
【0067】
このようにして開始候補点及び終了候補点が設定されると、前記コーナ検出処理手段のコーナ設定処理手段は、コーナ設定処理を行い、開始候補点と終了候補点との間にコーナを設定する。図10に示されるような形状を有する形状推定道路r4においては、形状補間点m1〜m10のうちの形状補間点m2が開始候補点s1にされ、形状補間点m9が終了候補点e1にされ、開始候補点s1と終了候補点e1との間にコーナcn1が設定される。
【0068】
なお、所定の形状補間点の半径が閾値Rth以下であり、一つ手前の形状補間点の半径、及び一つ先の形状補間点の半径がいずれも閾値Rthより大きい場合、前記開始候補点・終了候補点設定処理手段は、前記所定の形状補間点を、開始候補点及び終了候補点を兼ねる単発点として設定し、前記コーナ設定処理手段は、前記所定の形状補間点にコーナを設定する。また、図11に示されるような形状を有する形状推定道路r5においては、形状補間点m31〜m35のうちの形状補間点m33が単発点f1に設定され、形状補間点m33にコーナcn1が設定される。
【0069】
さらに、所定の形状補間点において方位角の正負が反転し、一つ先の形状補間点における方位角の正負が反転する場合、前記開始候補点・終了候補点設定処理手段は、前記所定の形状補間点を単発点として設定する。
【0070】
図12に示されるような形状を有する形状推定道路r6においては、形状補間点m41〜m48のうちの形状補間点m42〜m44、m46及びm47の方位角θ42〜θ44、θ46及びθ47は正の値を採り、形状補間点m45の方位角θ45は負の値を採る。この場合、形状補間点m45において方位角θ45の正負が正から負に反転し、一つ先の形状補間点m46において方位角θ46の正負が負から正に反転するので、形状補間点m45が、単発点f1に設定され、形状補間点m45にコーナcn1が設定される。
【0071】
続いて、前記コーナ設定処理手段は、単発点f1の前後、及び単発点f1において三つのコーナcn1〜cn3を設定する。なお、この場合、コーナの番号は手前側から順次付与され、コーナの番号に対応させて開始候補点の番号及び終了候補点の番号が付与される。
【0072】
このようにして形状推定道路においてコーナが設定されると、前記コーナ検出処理手段の記録処理手段は、記録処理を行い、形状推定道路内のコーナの番号、開始候補点の番号、終了候補点の番号等をRAM32(図1)に記録する。
【0073】
ところで、前述されたように、クロソイド係数推定処理においては、形状推定道路において検出されたコーナにおける最小半径に基づいてクロソイド係数が推定されるようになっているが、前記半径算出処理で3点計算法によって算出された半径のうちの最も小さい半径を最小半径として使用すると、該最小半径は本来使用すべき値より小さくなってしまう傾向がある。
【0074】
図13は3点計算法によって算出される半径を説明する第1の図、図14は3点計算法によって算出される半径を説明する第2の図である。
【0075】
図において、m51〜56、m61〜m71は形状補間点、s1は開始候補点、e1は終了候補点である。
【0076】
前記半径算出処理において算出された半径のうちの最も小さい半径を最小半径として使用する場合、図13においては、半径算出処理の対象となる形状補間点m53が対象点とされ、該対象点の一つ手前の形状補間点m52が前隣接点とされ、対象点の一つ先の形状補間点m54が後隣接点とされ、対象点、前隣接点及び後隣接点を通過する円CLcの半径が形状補間点m53における半径とされる。ここで、対象点、前隣接点及び後隣接点の3点だけでなく、形状補間点m51〜m56の座標に基づいて近似曲線を算出し、曲率半径を算出すると、該曲率半径は実際の道路の曲率半径に近い値を採る。そして、前記曲率半径を有する円を描くとCLdになり、このことから、3点計算法によって算出された半径は、実際の道路の曲率半径より小さくなる。
【0077】
また、図14においては、半径算出処理の対象となる形状補間点m66が対象点とされ、該対象点の一つ手前の形状補間点m65が前隣接点とされ、対象点の一つ先の形状補間点m67が後隣接点とされ、対象点、前隣接点及び後隣接点を通過する円CLcの半径が形状補間点m66における半径とされる。ここで、対象点、前隣接点及び後隣接点の3点だけでなく、形状補間点m61〜m71の座標に基づいて近似曲線を算出し、曲率半径を算出すると、該曲率半径は実際の道路の曲率半径に近い値を採る。そして、前記曲率半径を有する円を描くとCLdになり、この場合も、3点計算法によって算出された半径は、実際の道路の曲率半径より小さくなる。しかも、コーナの形状が大きくなると、その分、3点計算法によって算出された半径と実際の道路の曲率半径との差は大きくなる。
【0078】
したがって、前記半径算出処理手段によって算出された半径のうちの最も小さい半径を最小半径として使用すると、該最小半径は本来使用すべき値より小さくなるので、前記最小半径に基づいてクロソイド係数を推定すると、クロソイド曲線を適正に算出することができず、道路形状を正確に推定することができなくなってしまう。
【0079】
そこで、本実施の形態においては、検出されたコーナ内において、三つの仮想点を設定し、各仮想点の座標に基づいて、半径を算出し、算出された半径を最小半径とするようにしている。
【0080】
次に、図2のステップS6における最小半径算出処理のサブルーチンについて説明する。この場合、前述されたように、コーナ結合処理においてコーナが結合され、第1、第2のコーナ分割処理においてコーナが分割された後のコーナにおいて最小半径を算出する場合について説明する。
【0081】
図15は本発明の実施の形態における最小半径算出処理のサブルーチンを示す図、図16は本発明の実施の形態における最小半径算出処理の第1の説明図、図17は本発明の実施の形態における最小半径算出処理の第2の説明図、図18は本発明の実施の形態における最小半径算出処理の第3の説明図、図19は本発明の実施の形態における最小半径算出処理の第4の説明図、図20は本発明の実施の形態における最小半径算出処理の第5の説明図、図21は本発明の実施の形態における最小半径算出処理の第6の説明図、図22は本発明の実施の形態における最小半径算出処理の第7の説明図、図23は本発明の実施の形態における最小半径算出処理の第8の説明図、図24は本発明の実施の形態における最小半径算出処理の第9の説明図、図25は本発明の実施の形態における最小半径算出処理の第10の説明図、図26は本発明の実施の形態における最小半径算出処理の第11の説明図、図27は本発明の実施の形態における最小半径算出処理の第12の説明図である。
【0082】
図16において、r7は形状推定道路、cn1はコーナ、miは形状補間点、s1はコーナcn1の開始候補点、e1はコーナcn1の終了候補点である。
【0083】
まず、前記最小半径算出処理手段の中心仮想点設定処理手段は、中心仮想点設定処理を行い、第1の仮想点として中心仮想点Pvcを設定する。そのために、前記中心仮想点設定処理手段のコーナ中心付近設定処理手段は、コーナ中心付近設定処理を行い、各形状補間点m2〜m9間のセグメント長を加算することによって、図17に示されるコーナcn1の開始候補点slから終了候補点e1までの距離をコーナ長Lseとして算出する。
【0084】
続いて、前記コーナ中心付近設定処理手段は、開始候補点slより終了候補点e1側に、開始候補点slからコーナ長Lseの半分の距離Lf
Lf=Lse/2
だけ離れた位置、すなわち、開始候補点slと終了候補点e1との間の中心の位置にコーナ中間地点Pcを、該コーナ中間地点Pcより手前側の開始候補点slとの間において、コーナ中間地点Pcから距離Lf/ε1だけ離れた位置にコーナ前方地点Pfを、前記コーナ中間地点Pcより先側の終了候補点e1との間において、コーナ中間地点Pcから距離Lf/ε2だけ離れた位置にコーナ後方地点Pbを設定し、前記コーナ前方地点Pfからコーナ後方地点Pbまでの範囲をコーナ中心付近AR1として設定する。
【0085】
なお、本実施の形態において、ε1、ε2は、コーナ中間地点Pcより開始候補点sl側に前方地点Pfを、コーナ中間地点Pcより終了候補点e1側にコーナ後方地点Pbを設定するための設定値であり、本実施の形態においては、2に設定される。
【0086】
次に、前記中心仮想点設定処理手段の中心仮想点決定処理手段は、中心仮想点決定処理を行い、コーナ中心付近AR1内に、前記半径算出処理で算出された半径Riのうちの最も小さい半径を有する形状補間点があるかどうかを判断し、最も小さい半径を有する形状補間点がある場合、該形状補間点を中心仮想点Pvcとして設定する。図19に示されるように、例えば、形状補間点m5が、半径Riのうちの最も小さい半径を有する場合、形状補間点m5はコーナ中心付近AR1内にあるので、中心仮想点決定処理手段は、形状補間点m5を中心仮想点Pvcとする。
【0087】
また、前記コーナ中心付近AR1内に、最も小さい半径を有する形状補間点がない場合、中心仮想点決定処理手段は、前記コーナ中間地点Pcを中心仮想点Pvcとする。図20に示されるように、例えば、形状補間点m3が、半径Riのうちの最も小さい半径を有する場合、形状補間点m3はコーナ中心付近AR1内にないので、中心仮想点決定処理手段は、コーナ中間地点Pcを中心仮想点Pvcとする。
【0088】
このように、中心仮想点Pvcが設定されると、前記最小半径算出処理手段の前方仮想点設定処理手段は、前方仮想点設定処理を行い、第2の仮想点として前方仮想点Pvfを設定する。そのために、前記前方仮想点設定処理手段の距離設定処理手段は、距離設定処理を行い、中心仮想点Pvcから開始候補点slまでの距離をLsとしたとき、図21に示される距離Lsの半分の距離Ls/2と閾値Lsth(例えば、35〔m〕)とを比較する。そして、前記距離設定処理手段は、距離Ls/2が閾値Lsth以上である場合、距離Ls/2を、中心仮想点Pvcから前方仮想点Pvfを設定するための前方仮想点距離Lvfに設定し、距離Ls/2が閾値Lsth未満である場合、閾値Lsthを前方仮想点距離Lvfに設定する。
【0089】
続いて、前記距離設定処理手段は、前方仮想点距離Lvfと前記距離Lsとを比較する。図22に示されるように、前方仮想点距離Lvfが距離Ls未満である場合、前記前方仮想点設定処理手段の前方仮想点決定処理手段は、前方仮想点決定処理を行い、中心仮想点Pvcより手前側の、中心仮想点Pvcから前方仮想点距離Lvfだけ離れた位置に前方仮想点Pvfを設定する。また、図23に示されるように、前方仮想点距離Lvfが距離Ls以上である場合、前記前方仮想点決定処理手段は、中心仮想点Pvcより手前側の、中心仮想点Pvcから距離Lsだけ離れた位置(開始候補点sl)に前方仮想点Pvfを設定する。
【0090】
この場合、開始候補点slと中心仮想点Pvcとの中間点を第1の候補点P1とし、中心仮想点Pvcより手前側の、中心仮想点Pvcから閾値Lthfだけ離れた位置を第2の候補点P2としたときに、前方仮想点Pvfは、第1、第2の候補点P1、P2、及び、開始候補点slのうちのいずれか一つの点に設定される。
【0091】
すなわち、前方仮想点Pvfは、開始候補点slより中心仮想点Pvcから離れる側には設定されず、また、第1の候補点P1より中心仮想点Pvc側には設定されない。
【0092】
次に、前記最小半径算出処理手段の後方仮想点設定処理手段は、後方仮想点設定処理を行い、第3の仮想点として後方仮想点Pvbを設定する。そのために、前記後方仮想点設定処理手段の距離設定処理手段は、距離設定処理を行い、中心仮想点Pvcから終了候補点e1までの距離をLeとしたとき、図24に示される距離Leの半分の距離Le/2と閾値Leth(例えば、35〔m〕)とを比較する。そして、前記距離設定処理手段は、距離Le/2が閾値Leth以上である場合、距離Le/2を、中心仮想点Pvcから後方仮想点Pvbを設定するための後方仮想点距離Lvbに設定し、距離Le/2が閾値Leth未満である場合、閾値Lethを後方仮想点距離Lvbに設定する。
【0093】
続いて、前記距離設定処理手段は、後方仮想点距離Lvbと前記距離Leとを比較する。図25に示されるように、後方仮想点距離Lvbが距離Le未満である場合、前記後方仮想点設定処理手段の後方仮想点決定処理手段は、後方仮想点決定処理を行い、中心仮想点Pvcより先側の、中心仮想点Pvcから後方仮想点距離Lvbだけ離れた位置に後方仮想点Pvbを設定する。また、図26に示されるように、仮想点距離Lvbが距離Le以上である場合、前記後方仮想点決定処理手段は、中心仮想点Pvcより先側の、中心仮想点Pvcから距離Leだけ離れた位置(終了候補点e1)に後方仮想点Pvbを設定する。
【0094】
この場合、終了候補点e1と中心仮想点Pvcとの中間点を第1の候補点P11とし、中心仮想点Pvcより先側の、中心仮想点Pvcから閾値Lthbだけ離れた位置を第2の候補点P12としたときに、後方仮想点Pvbは、第1、第2の候補点P11、P12、及び、終了候補点e1のうちのいずれか一つの点に設定される。
【0095】
すなわち、後方仮想点Pvbは、終了候補点e1より中心仮想点Pvcから離れる側には設定されず、また、第1の候補点P11より中心仮想点Pvc側には設定されない。
【0096】
続いて、前記最小半径算出処理手段の仮想半径算出処理手段は、仮想半径算出処理を行い、中心仮想点Pvc、前方仮想点Pvf及び後方仮想点Pvbの座標に基づいて、3点計算法によって仮想曲率半径としての半径を算出し、最小半径とする。
【0097】
このように、本実施の形態においては、コーナcn1の中心付近AR1に、中心仮想点Pvcが設定され、第1、第2の候補点P1、P2及び開始候補点s1のうちのいずれか一つの点に前方仮想点Pvfが設定され、第1、第2の候補点P11、P12及び終了候補点e1のうちのいずれか一つの点に後方仮想点Pvfが設定され、図27に示されるように、中心仮想点Pvc、前方仮想点Pvf及び後方仮想点Pvbに基づいて、3点計算法によって最小半径が算出されるので、最小半径を実際の道路上の最小半径に近似する値にすることができる。
【0098】
すなわち、中心仮想点Pvc、前方仮想点Pvf及び後方仮想点Pvbを通過する円をCLcとし、形状補間点m1〜m11の座標に基づいて曲率半径を算出し、該曲率半径を有する円を描くとCLdになり、このことから、各円CLc、CLdは類似する形状を有する。
【0099】
また、本実施の形態において、前方仮想点Pvfは、第1の候補点P1より中心仮想点Pvc側には設定されず、また、後方仮想点Pvbは、第1の候補点P11より中心仮想点Pvc側には設定されないので、前記半径算出処理手段によって最小半径を算出する場合に比べて最小半径が小さくなるのを防止することができる。
【0100】
次に、フローチャートについて説明する。
ステップS6−1 中心仮想点Pvcを設定する。
ステップS6−2 前方仮想点Pvfを設定する。
ステップS6−3 後方仮想点Pvbを設定する。
ステップS6−4 仮想半径算出処理を行い、リターンする。
【0101】
次に、図2のステップS7におけるクロソイド係数推定処理について説明する。
【0102】
図28は本発明の実施の形態におけるクロソイド係数マップを示す図である。
【0103】
この場合、最小半径Rminとクロソイド曲線のクロソイド係数Aの2乗の値A2 とが、互いに対応させてROM33(図1)にクロソイド係数マップとして記録され、前記クロソイド係数推定処理手段は、前記ステップS6で算出された最小半径Rminを読み込み、クロソイド係数マップを参照し、最小半径Rminに対応する値A2 を読み出す。続いて、前記クロソイド係数推定処理手段は、クロソイド係数Aを算出する。なお、前記クロソイド係数マップは、実際の道路における半径と、前記道路を設計する際に使用されたクロソイド曲線のクロソイド係数との関係に基づいて作成されたものである。
【0104】
次に、図2のステップS8におけるクロソイド曲線算出処理のサブルーチンについて説明する。
【0105】
図29は本発明の実施の形態におけるクロソイド曲線算出処理のサブルーチンを示す図である。
【0106】
ところで、クロソイド曲線は、始点からの長さが大きくなるほど曲率半径が小さくなる曲線であり、一般的には、道路を設計する際に、クロソイド曲線の一部がコーナの開始点及び終了点に当てられるようになっている。前記クロソイド曲線をx−y座標で表すと、クロソイド曲線の開始点(原点)から所定の点Sまでの距離、すなわち、曲線長をLとし、前記点Sにおける曲線半径をRcとすると、
L・Rc=A2 ……(7)
になる。また、通常、クロソイド曲線のX座標及びY座標は、それぞれ、式(8)及び(9)で示される多項式によって算出することができる。
【0107】
X=L×(1−L2 /(40Rc2 )+L4 /(3456Rc4 )
−L6 /(599040Rc6 )
+L8 /(175472640Rc8 )…) ……(8)
X=L2 /(6Rc)×(1−L2 /(56Rc2 )+
L4 /(7040Rc4 )−L6 /(1612800Rc6 )
+L8 /(588349440Rc8 )…) ……(9)
この場合、クロソイド曲線を前記式(8)及び(9)で算出することが考えられるが、式(8)及び(9)は、高次の多項式であり、多数の点をプロットしてクロソイド曲線を算出しようとすると、クロソイド曲線算出処理手段によってCPU31に加わる負荷が大きくなってしまう。
【0108】
そこで、本実施の形態においては、前記クロソイド曲線算出処理手段によって、クロソイド係数推定処理において算出されたクロソイド係数Aに基づいてクロソイド曲線を算出するようにしている。そのために、前記クロソイド曲線算出処理手段は、クロソイド曲線(曲線長L(k)の点S)のX座標及びY座標を、シュミレーションに基づいて得られた式(10)及び(11)によって近似させ、算出する。
【0109】
【数1】
【0110】
ただし、X0 及びY0 は、クロソイド曲線の開始点のX座標及びY座標である。また、角度φは次の式(12)で表される。
【0111】
φ=α+2k・L ……(12)
そして、αはクロソイド曲線の開始点の方位であり、値kは式(13)で表される。
【0112】
k=28/A2 ……(13)
次に、フローチャートについて説明する。
ステップS8−1 クロソイド曲線の曲線長L(k)でループを開始する。
ステップS8−2 曲線長L(k)の点SのX座標を算出する。
ステップS8−3 曲線長L(k)の点SのY座標を算出する。
ステップS8−4 クロソイド曲線の曲線長L(k)でループを終了させ、リターンする。
【0113】
次に、図2のステップS9におけるフィッティング処理について説明する。
【0114】
図30は本発明の実施の形態におけるフィッティング処理の説明図である。
【0115】
図において、r7は形状推定道路、cn1は前記コーナ結合処理及び第1、第2のコーナ分割処理によって設定されたコーナ、m1〜m5はコーナcn1を構成する形状補間点、s1はコーナcn1の開始候補点である。
【0116】
この場合、前記フィッティング処理手段のクロソイド曲線移動処理手段は、クロソイド曲線移動処理を行い、形状補間点m1、m2間を結ぶセグメントSg1の延長線上に、クロソイド曲線Qの始点Qsを置き、該始点Qsにおける接線とセグメントSg1の延長線とが一致するようにクロソイド曲線Qを矢印E方向に移動させる。
【0117】
このとき、前記フィッティング処理手段の誤差算出処理手段は、誤差算出処理を行い、形状補間点m2以降の各形状補間点m2〜m5に対するクロソイド曲線Qの位置の誤差の合計を算出する。この場合、各形状補間点m2〜m5に対するクロソイド曲線Qの位置の誤差は、各形状補間点m2〜m5とクロソイド曲線Qを描く際にプロットされる各点との前記X軸上の距離及びY軸上の距離に基づいて算出される。
【0118】
そして、前記クロソイド曲線移動処理手段は、誤差の合計が最も小さくなるように前記クロソイド曲線Qを各形状補間点m2〜m5に合わせ、クロソイド曲線Qの位置を設定する。
【0119】
続いて、前記フィッティング処理手段の開始点・終了点抽出処理手段は、開始点・終了点抽出処理を行い、クロソイド曲線Qの位置が設定されると、クロソイド曲線Qの始点Qsを実際のコーナの開始点Sa1として設定する。
【0120】
また、クロソイド曲線移動処理手段、誤差算出処理手段及び開始点・終了点抽出処理手段は、コーナcn1の終点側において同様の処理を行い、クロソイド曲線Qの位置を設定し、クロソイド曲線Qの始点を実際のコーナの終了点として設定する。
【0121】
このようにして、クロソイド曲線Qによって開始候補点及び終了候補点を補正し、補正された開始候補点及び終了候補点を、それぞれコーナcn1の開始点Sa1及び終了点として設定することができるので、コーナcn1の開始点Sa1及び終了点を実際の道路上のコーナの開始点及び終了点に近づけることができる。
【0122】
次に、図2のステップS10における開始点・終了点記録処理について説明する。
【0123】
前記開始点・終了点記録処理手段は、抽出された開始点及び終了点の位置を、道路形状を表すデータとしてデータ記録部16に記録する。
【0124】
このように、本実施の形態においては、道路形状を正確に推定することができ、正確に推定された道路形状に基づいてコーナcn1の開始点Sa1及び終了点を設定するようになっているので、道路形状に対応させて自動変速機10の変速段を変更したり、道路形状に対応させてエンジン52の出力を変更したりして、車両の走行制御を精度良く行うことができる。
【0125】
本実施の形態において、道路形状の推定は、ナビゲーション装置14のCPU31を動作させることによって行われるようになっているが、自動変速機制御装置11の図示されないCPUを作動させたり、又は前記情報センタの制御装置としてのCPUを動作させたりすることによって道路形状の推定を行うことができる。なお、車両の全体の制御を行うために、自動変速機制御装置11及びエンジン制御装置51の上位の制御装置として車両制御装置が配設されるが、該車両制御装置のCPUを動作させることによって道路形状の推定を行うこともできる。
【0126】
また、本実施の形態においては、隣接する各ノード間の形状推定道路について道路形状の推定を行うようになっているが、実際の道路上において、コーナ内にノードがある場合、各ノード間の形状推定道路ごとに道路形状を推定すると、コーナ内におけるノードの前後において処理が異なってしまう。そこで、他の実施の形態においては、道路データにおいて、各道路リンク間に道なりデータが付与されていて、ノードを通過した後に進入することが予測される道路リンクが明らかな場合、又は経路探索処理によって探索経路が設定されている場合等においては、二つ以上の連続する形状推定道路について道路形状の推定を行うことができる。
【0127】
なお、本発明は前記実施の形態に限定されるものではなく、本発明の趣旨に基づいて種々変形させることが可能であり、それらを本発明の範囲から排除するものではない。
【図面の簡単な説明】
【0128】
【図1】本発明の実施の形態における車両制御システムのブロック図である。
【図2】本発明の実施の形態における道路形状推定処理手段の動作を示すメインフローチャートである。
【図3】本発明の実施の形態における半径算出処理のサブルーチンを示す図である。
【図4】本発明の実施の形態における半径を算出する原理を説明する図である。
【図5】本発明の実施の形態におけるコーナ検出処理の第1の説明図である。
【図6】本発明の実施の形態におけるコーナ検出処理の第2の説明図である。
【図7】本発明の実施の形態におけるコーナ検出処理の第3の説明図である。
【図8】本発明の実施の形態におけるコーナ検出処理の第4の説明図である。
【図9】本発明の実施の形態におけるコーナ検出処理の第5の説明図である。
【図10】本発明の実施の形態におけるコーナ検出処理の第6の説明図である。
【図11】本発明の実施の形態におけるコーナ検出処理の第7の説明図である。
【図12】本発明の実施の形態におけるコーナ検出処理の第8の説明図である。
【図13】3点計算法によって算出される半径を説明する第1の図である。
【図14】3点計算法によって算出される半径を説明する第2の図である。
【図15】本発明の実施の形態における最小半径算出処理のサブルーチンを示す図である。
【図16】本発明の実施の形態における最小半径算出処理の第1の説明図である。
【図17】本発明の実施の形態における最小半径算出処理の第2の説明図である。
【図18】本発明の実施の形態における最小半径算出処理の第3の説明図である。
【図19】本発明の実施の形態における最小半径算出処理の第4の説明図である。
【図20】本発明の実施の形態における最小半径算出処理の第5の説明図である。
【図21】本発明の実施の形態における最小半径算出処理の第6の説明図である。
【図22】本発明の実施の形態における最小半径算出処理の第7の説明図である。
【図23】本発明の実施の形態における最小半径算出処理の第8の説明図である。
【図24】本発明の実施の形態における最小半径算出処理の第9の説明図である。
【図25】本発明の実施の形態における最小半径算出処理の第10の説明図である。
【図26】本発明の実施の形態における最小半径算出処理の第11の説明図である。
【図27】本発明の実施の形態における最小半径算出処理の第12の説明図である。
【図28】本発明の実施の形態におけるクロソイド係数マップを示す図である。
【図29】本発明の実施の形態におけるクロソイド曲線算出処理のサブルーチンを示す図である。
【図30】本発明の実施の形態におけるフィッティング処理の説明図である。
【符号の説明】
【0129】
16 データ記録部
31 CPU
cn1 cn11 cn21 コーナ
e1 終了候補点
ma 対象点
mi、m1〜m16、m21〜m26、m31〜m35、m51〜m56、m61〜m71 形状補間点
Pvb 後方仮想点
Pvc 中心仮想点
Pvf 前方仮想点
Q クロソイド曲線
Rmin 最小半径
s1 開始候補点
【技術分野】
【0001】
本発明は、道路形状推定装置、道路形状推定方法及びプログラムに関するものである。
【背景技術】
【0002】
従来、ナビゲーション装置においては、例えば、GPS(グローバルポジショニングシステム)によって、自車である車両の現在の位置、すなわち、現在地が検出され、データ記録部から地図データが読み出され、表示部に地図画面が形成され、該地図画面に、現在地を表す自車位置、該自車位置の周辺の地図等が表示されるようになっている。したがって、運転者は、前記地図画面に表示された自車位置等に従って車両を走行させることができる。
【0003】
また、運転者が目的地を入力し、探索条件を設定すると、該探索条件に基づいて、経路探索処理が行われ、前記地図データに従って、現在地で表される出発地から目的地までの経路が探索される。そして、探索された経路、すなわち、探索経路は、前記地図画面に自車位置と共に表示され、探索経路の案内、すなわち、経路案内が行われる。したがって、運転者は、表示された探索経路に沿って車両を走行させることができる。
【0004】
ところで、前記ナビゲーション装置においては、ナビゲーション装置において取得された情報を利用して自動変速機の変速段を変更したり、エンジンの出力を変更したりして、車両の走行制御を行うようにした車両制御システムが提供されている。
【0005】
そのために、該車両制御システムに道路形状推定装置が配設され、該道路形状推定装置によって道路形状が推定されるようになっている。そして、前記道路形状推定装置は、前記ナビゲーション装置のデータ記録部に配設されたデータベースから道路データを読み出すことによって、データベース上の道路に沿った所定の道路リンクを形状推定道路とし、該形状推定道路において自車位置より前方における道路形状を複数の点で表すために設定された補間点、すなわち、形状補間点に関する補間点データを取得し、互いに隣接する3点の形状補間点に対する3点計算法によって各形状補間点における道路の曲率半径(以下、単に「半径」という。)を算出し、各半径に基づいて、データベース上でコーナを検出し、設定するとともに、コーナの開始点を表す開始候補点、及びコーナの終了点を表す終了候補点を設定する。そして、前記形状推定道路内において、各形状補間点における半径のうちの最も小さい半径をそのコーナにおける最小半径とし、最小半径に基づいてクロソイド係数を推定し、該クロソイド係数に基づいて、コーナの近似式によって表されるクロソイド曲線を算出し、該クロソイド曲線に基づいて、開始候補点及び終了候補点の位置を補正し、道路形状を推定し、補正された開始候補点及び終了候補点の位置を、道路形状を表すデータとしてデータ記録部に記録する(例えば、特許文献1参照。)。
【特許文献1】特開2005−214839号公報
【発明の開示】
【発明が解決しようとする課題】
【0006】
しかしながら、前記従来の道路形状推定装置においては、互いに隣接する3点の形状補間点に対する3点計算法によって算出された半径のうちの最も小さい半径をコーナの曲率半径として算出すると、曲率半径の大きいコーナにおいては、実際の道路上の曲率半径と算出されたコーナの曲率半径との差が大きくなってしまうので、道路形状を正確に推定することができない。
【0007】
本発明は、前記従来の道路形状推定装置の問題点を解決して、道路形状を正確に推定することができる道路形状推定装置、道路形状推定方法及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【0008】
そのために、本発明の道路形状推定装置においては、道路に沿って設定され、道路の形状を表す複数の形状補間点の補間点データを取得するデータ取得処理手段と、前記道路における所定の区間内の前記補間点データに基づいて各形状補間点における曲率半径を算出する半径算出処理手段と、前記各曲率半径に基づいて、前記所定の区間内のコーナを検出するとともに、該検出されたコーナの開始候補点及び終了候補点を設定するコーナ検出処理手段と、前記コーナの開始候補点及び終了候補点に基づいて中心仮想点を、該中心仮想点より開始候補点側に前方仮想点を、前記中心仮想点より終了候補点側に後方仮想点を設定し、中心仮想点、前方仮想点及び後方仮想点に基づいて前記コーナの仮想曲率半径を最小半径として算出する最小半径算出処理手段とを有する。
【発明の効果】
【0009】
本発明によれば、コーナ内の中心付近に中心仮想点が、該中心仮想点より開始候補点側に前方仮想点が、前記中心仮想点より終了候補点側に後方仮想点が設定され、中心仮想点、前方仮想点及び後方仮想点に基づいて仮想曲率半径が最小半径として算出されるので、道路形状を正確に推定することができる。
【発明を実施するための最良の形態】
【0010】
以下、本発明の実施の形態について図面を参照しながら詳細に説明する。
【0011】
図1は本発明の実施の形態における車両制御システムのブロック図、図2は本発明の実施の形態における道路形状推定処理手段の動作を示すメインフローチャートである。
【0012】
図において、14は情報端末、例えば、車両に搭載された車載装置としてのナビゲーション装置であり、該ナビゲーション装置14は、車両の現在地を自車位置として、車両の方位を自車方位として検出する現在地検出部としてのGPSセンサ15、図示されない地図データのほかに各種の情報が記録された情報記録部としてのデータ記録部16、ナビゲーション処理等の各種の演算処理を行うナビゲーション処理部17、操作者である運転者が操作することによって所定の入力を行うための第1の入力部としての操作部34、図示されない画面に表示された画像によって各種の表示を行い、運転者に通知するための第1の出力部としての表示部35、運転者が音声によって所定の入力を行うための第2の入力部としての音声入力部36、音声出力を行い、各種の情報を運転者に通知するための第2の出力部としての音声出力部37、及び通信端末として機能する送受信部としての通信部38を備え、前記ナビゲーション処理部17に、GPSセンサ15、データ記録部16、操作部34、表示部35、音声入力部36、音声出力部37及び通信部38が接続される。また、前記ナビゲーション処理部17には車速を検出する車速検出部としての車速センサ44等が接続される。そして、前記GPSセンサ15は、自車位置及び自車方位のほかに時刻を検出する。なお、GPSセンサ15と独立させて図示されない方位センサを配設し、該方位センサによって自車方位を検出することもできる。
【0013】
前記データ記録部16には、地図データファイルから成るデータベースが配設され、該データベースに地図データが記録される。該地図データには、各交差点(分岐点も含む。)間を結ぶ道路に関する道路データ、前記道路の端点(始点及び終点)を表すノードに関するノードデータ、前記交差点に関する交差点データ、探索用に加工された探索データ、施設に関する施設データ等が含まれるほか、道路上の地物に関する地物データが含まれる。なお、前記道路データには、前記データベース上で道路の始点から終点までの道路リンクを表すデータ、及び道路リンク上において、道路形状を表すために道路に沿って設定された複数の設定点としての形状補間点のデータ(以下「補間点データ」という。)が含まれる。補間点データには、各形状補間点の番号、座標等が含まれる。なお、各ノードは、道路リンク上の始点又は終点を表すので、形状補間点でもある。
【0014】
さらに、前記データ記録部16には、統計データファイルから成るデータベース、走行履歴データファイルから成るデータベース等が配設され、前記統計データファイルに統計データが、前記走行履歴データファイルに走行履歴データが、いずれも実績データとして記録される。
【0015】
そして、前記データ記録部16は、前記各種のデータを記録するために、ハードディスク、CD、DVD、光ディスク等の図示されないディスクを備えるほかに、各種のデータを読み出したり、書き込んだりするための読出・書込ヘッド等の図示されないヘッドを備える。また、前記データ記録部16にメモリカード等を使用することができる。
【0016】
前記ナビゲーション処理部17は、ナビゲーション装置14の全体の制御を行う制御装置としての、かつ、演算装置としてのCPU31、該CPU31が各種の演算処理を行うに当たってワーキングメモリとして使用されるRAM32、制御用のプログラムのほか、目的地までの経路の探索、経路案内等を行うための各種のプログラムが記録されたROM33、各種のデータ、プログラム等を記録するために使用される図示されないフラッシュメモリ等を備える。
【0017】
前記操作部34として、表示部35とは独立させて配設された図示されないキーボード、マウス等を使用することができる。また、前記操作部34として、前記表示部35に形成された画面に画像で表示された各種のキー、スイッチ、ボタン等の画像操作部をタッチ又はクリックすることによって、所定の入力操作を行うことができるようにしたタッチパネルを使用することができる。
【0018】
前記表示部35としてディスプレイが使用され、表示部35に形成された各種の画面に、自車位置、自車方位等を表示したり、地図、探索経路、該探索経路に沿った案内情報、交通情報等を表示したり、探索経路における次の交差点までの距離、次の交差点における進行方向を表示したりすることができる。
【0019】
また、音声入力部36は、図示されないマイクロホン等によって構成され、音声によって必要な情報を入力することができる。さらに、音声出力部37は、図示されない音声合成装置及びスピーカを備え、前記探索経路の経路案内を音声出力によって行う。
【0020】
前記通信部38は、道路交通情報センタから送信された現況の交通情報、一般情報等の各種の情報を受信するための図示されないビーコンレシーバ、FM放送局を介して前記各種の情報をFM多重放送として受信するための図示されないFM受信機等を備える。また、通信部38は、図示されない情報センタから、交通情報、一般情報等の情報のほかに、地図データ、統計データ、走行履歴データ等のデータを図示されないネットワークを介して受信することができる。
【0021】
なお、ナビゲーション処理部17、CPU31等は、単独で、又は二つ以上組み合わせることによってコンピュータとして機能し、各種のプログラム、データ等に基づいて演算処理を行う。また、前記データ記録部16、RAM32、ROM33、フラッシュメモリ等によって記憶装置及び記録媒体が構成される。そして、演算装置として、CPU31に代えてMPU等を使用することもできる。
【0022】
なお、10は自動変速機、11は自動変速機制御装置、51はエンジン制御装置、52はエンジンである。また、前記自動変速機制御装置11、ナビゲーション装置14、エンジン制御装置51等によって車両制御システムが構成される。
【0023】
次に、前記構成のナビゲーション装置14の基本動作について説明する。
【0024】
まず、運転者によって操作部34が操作され、ナビゲーション装置14が起動されると、CPU31の図示されない現在地読込処理手段は、現在地読込処理を行い、GPSセンサ15によって検出された自車位置及び自車方位を読み込む。次に、前記CPU31の図示されないマッチング処理手段は、マッチング処理を行い、読み込まれた自車位置の軌跡、及び自車位置の周辺の道路を構成する各道路リンクの形状、配列等に基づいて、車両がいずれの道路リンク上に位置するかの判定を行うことによって、自車位置を特定する。
【0025】
続いて、CPU31の図示されない基本情報取得処理手段は、基本情報取得処理を行い、前記地図データをデータ記録部16から読み出して取得する。なお、地図データを前記情報センタ等から取得することもでき、その場合、前記基本情報取得処理手段は、受信した地図データをフラッシュメモリにダウンロードする。
【0026】
そして、前記CPU31の図示されない表示処理手段は、表示処理を行い、前記表示部35に各種の画面を形成する。例えば、表示処理手段の地図表示処理手段は、地図表示処理を行い、表示部35に地図画面を形成し、該地図画面に周囲の地図を表示するとともに、自車位置及び自車方位を表示する。
【0027】
したがって、運転者は、前記周囲の地図、自車位置及び自車方位に従って車両を走行させることができる。
【0028】
また、運転者が操作部34を操作して目的地を入力すると、CPU31の図示されない目的地設定処理手段は、目的地設定処理を行い、目的地を設定する。なお、必要に応じて出発地を入力し、設定することもできる。また、あらかじめ所定の地点を登録しておき、登録された地点を目的地として設定することができる。続いて、運転者が操作部34を操作して探索条件を入力すると、CPU31の図示されない探索条件設定処理手段は、探索条件設定処理を行い、探索条件を設定する。
【0029】
このようにして目的地及び探索条件が設定されると、CPU31の図示されない経路探索処理手段は、経路探索処理を行い、前記自車位置、自車方位、目的地、探索条件等を読み込むとともに、データ記録部16から探索データ等を読み出し、自車位置、自車方位、目的地、探索データ等に基づいて、自車位置で表される出発地から目的地までの経路を前記探索条件で探索し、探索経路を表す経路データを出力する。このとき、経路探索処理においては、各道路リンクごとに付与されたリンクコストの合計が最も小さい経路が探索経路とされる。なお、出発地を、自車位置に代えて、運転者が設定した所定の地点とし、該所定の地点から目的地までの経路を探索することもできる。
【0030】
続いて、前記CPU31の図示されない案内処理手段は、案内処理を行い、経路案内を行う。そのために、前記案内処理手段の案内表示処理手段は、案内表示処理を行い、前記経路データを読み込み、経路データに基づいて前記地図画面に探索経路を表示する。
【0031】
なお、例えば、経路案内の対象となる地点である交差点で車両を右左折させる必要がある場合、前記交差点が、車両を進行させる方向の案内を行う対象となる案内交差点として設定され、前記案内処理手段の音声出力処理手段は、音声出力処理を行い、前記案内交差点に車両が到達する前に、音声出力によって経路案内を行う。
【0032】
また、前記案内処理手段の案内点拡大図形成処理手段は、案内点拡大図形成処理を行い、車両が案内交差点に到達する前に、地図画面の所定の領域に案内交差点の拡大図、すなわち、案内点拡大図としての交差点拡大図を形成し、交差点拡大図による経路案内を行う。そのために、探索経路上の前記案内交差点より手前(自車位置側)の、設定された距離だけ離れた箇所に車両が到達すると、前記交差点拡大図が表示される。この場合、該交差点拡大図に、案内交差点の周辺の地図、探索経路、案内交差点において目印になる施設等の陸標が表示される。
【0033】
ところで、本実施の形態においては、前記ナビゲーション装置14において取得された情報を、自動変速機制御装置11に送信して、自車位置より前方の道路形状に対応させて自動変速機10の変速段を変更したり、エンジン制御装置51に送信して、自車位置より前方の道路形状に対応させてエンジン52の出力を変更したりして、車両の走行制御を行うようにしている。
【0034】
そのために、前記CPU31は道路形状推定装置として機能し、CPU31の図示されない道路形状推定処理手段は、道路形状推定処理を行い、後述される方法で道路形状を推定する。そして、CPU31の図示されない走行制御処理手段は、走行制御処理を行い、推定された道路形状に従って、自動変速機10の変速段を変更したり、エンジン52の出力を変更したりするための信号を前記自動変速機制御装置11、エンジン制御装置51等に送信する。
【0035】
次に、図2に従って、前記道路形状推定処理手段の動作について説明する。
【0036】
まず、前記道路形状推定処理手段のデータ取得処理手段は、データ取得処理を行い、道路形状を推定する対象となる道路を含む、自車位置より前方の所定の領域を道路形状推定範囲として設定し、データ記録部16から道路データを読み出し、前記道路形状推定範囲内のノードデータを取得する。
【0037】
この場合、道路形状推定範囲内において、ノードデータで表される各ノードのうちの最初のノードから最後のノードまでの、互いに隣接する各ノード間の道路リンクについて、順に道路形状の推定が行われる。なお、この場合、データベース上の道路において道路形状の推定が行われる各道路リンクを形状推定道路とする。
【0038】
そこで、前記データ取得処理手段は、各形状推定道路において道路形状の推定を行うために、自車位置より前方の、形状推定道路上における形状補間点の補間点データを取得する。なお、本実施の形態において、前記データ取得処理手段は、ノードデータ及び補間点データを、データ記録部16から読み出すことによって取得するようになっているが、前記情報センタ等からネットワークを介して受信することによって取得することもできる。
【0039】
次に、前記道路形状推定処理手段の半径算出処理手段は、半径算出処理を行い、自車位置より前方の形状推定道路上における形状補間点の補間点データに基づいて、3点計算法によって形状推定道路の各形状補間点における半径(曲率半径)を算出する。続いて、前記道路形状推定処理手段のコーナ検出処理手段は、コーナ検出処理を行い、前記算出された半径に基づいて、データベース上の形状推定道路内においてコーナを検出し、コーナが開始される形状補間点を開始候補点として、データベース上でコーナが終了する形状補間点を終了候補点として設定する。
【0040】
そして、前記道路形状推定処理手段のコーナ結合処理手段は、実際の道路上では一つのコーナとして見なされるべきコーナが、データベース上における形状補間点の設定のされ方によって、複数のコーナとして検出された場合に、コーナ結合処理を行い、検出されたコーナの結合を行う。
【0041】
次に、前記道路形状推定処理手段の第1、第2のコーナ分割処理手段は、実際の道路上では複数のコーナとして見なされるべきコーナが、データベース上における形状補間点の設定のされ方によって、一つのコーナとして検出された場合に、第1、第2のコーナ分割処理を行い、検出されたコーナの分割を行う。この場合、第1のコーナ分割処理においては、コーナ内の所定の形状補間点が、コーナの分割を行うための閾(しきい)値以上の半径を有する場合に、第2のコーナ分割処理においては、コーナ内の所定の形状補間点間におけるセグメントが閾値以上の長さを有する場合に、検出されたコーナの分割が行われる。
【0042】
このようにして、コーナの結合が行われたり、分割が行われたりして、形状補間点に基づく適正なコーナが設定されると、前記道路形状推定処理手段の最小半径算出処理手段は、最小半径算出処理を行い、各コーナについて、クロソイド係数を推定するための適正な最小半径を算出する。
【0043】
続いて、前記道路形状推定処理手段のクロソイド係数推定処理手段は、クロソイド係数推定処理を行い、前記最小半径に基づいてクロソイド係数を推定し、道路形状推定処理手段のクロソイド曲線算出処理手段は、クロソイド曲線算出処理を行い、クロソイド係数に基づいてコーナの近似式を形成し、コーナの道路形状を表すクロソイド曲線を算出する。
【0044】
このようにして、該クロソイド曲線が算出されると、道路形状推定処理手段のフィッティング処理手段は、フィッティング処理を行い、クロソイド曲線をコーナ上の各形状補間点に合わせることによって、開始候補点及び終了候補点を補正し、実際の道路上でコーナが開始される点、及びコーナが終了する点に近似する位置にコーナの開始点及び終了点を設定する。
【0045】
そして、道路形状推定処理手段の開始点・終了点記録処理手段は、開始点・終了点記録処理を行い、前記開始点及び終了点を道路形状を表すデータとしてデータ記録部16に記録する。このようにして、道路形状が推定される。
【0046】
次に、フローチャートについて説明する。
ステップS1 半径算出処理を行う。
ステップS2 コーナ検出処理を行う。
ステップS3 コーナ結合処理を行う。
ステップS4 第1のコーナ分割処理を行う。
ステップS5 第2のコーナ分割処理を行う。
ステップS6 最小半径算出処理を行う。
ステップS7 クロソイド係数推定処理を行う。
ステップS8 クロソイド曲線算出処理を行う。
ステップS9 フィッティング処理を行う。
ステップS10 開始点・終了点記録処理を行い、処理を終了する。
【0047】
次に、図2のステップS1における半径算出処理のサブルーチンについて説明する。
【0048】
図3は本発明の実施の形態における半径算出処理のサブルーチンを示す図、図4は本発明の実施の形態における半径を算出する原理を説明する図である。
【0049】
図4に示されるように、半径算出処理の対象となる形状補間点を対象点maとし、該対象点maの一つ手前(自車位置側)の形状補間点を前隣接点mbとし、対象点maの一つ先(自車位置から離れる側)の形状補間点を後隣接点mcとするとともに、対象点maと前隣接点mbとを結ぶ、対象点maより手前側のセグメント(以下「前側セグメント」という。)のセグメント長をL1とし、対象点maと後隣接点mcとを結ぶ、対象点maより先のセグメント(以下「後側セグメント」という。)のセグメント長をL2とする。また、前記対象点ma、前隣接点mb及び後隣接点mcを通過する円をCrとしたときの、円Crの中心をQとし、円Crの半径をRとする。さらに、対象点maにおいて、前側セグメントに対して後側セグメントの成す角度を方位角θとする。
【0050】
そして、対象点maと後隣接点mcとを結ぶ方向をx軸方向とし、該x軸方向に対する直角の方向をy軸方向としたときの、x軸方向における対象点maと前隣接点mbとの間の距離をXとし、x軸方向における対象点maと中心Qとの間の距離をAとし、x軸方向における前隣接点mbと中心Qとの間の距離をGとするとともに、y軸方向における対象点maと前隣接点mb間の距離をYとし、y軸方向における前隣接点mbと中心Qとの間の距離をHとし、y軸方向における対象点maと中心Qとの間の距離をFとすると、
X=L1cosθ
A=L2/2
G=X+A
Y=L1sinθ
F=√(R2 −A2 )
H=F−Y
になる。
【0051】
この場合、距離Gの二乗と距離Hの二乗との和が半径Rの二乗と等しいので、式(1)の関係が成り立つ。
【0052】
R2 =G2 +H2 ……(1)
次に、該式(1)の距離G、Hに前記各値を代入すると式(2)を得ることができる。
【0053】
R2 =X2 +2XA+A2 +R2 −A2
−2Y√(R2 −A2 )+Y2 ……(2)
続いて、該式(2)を変形すると式(3)〜(6)を得ることができる。
【0054】
2Y√(R2 −A2 )=X2 +Y2 +2XA ……(3)
R2 −A2 ={(X2 +Y2 +2XA)/2Y}2 ……(4)
R2 ={(X2 +Y2 +2XA)/2Y}2 +A2 ……(5)
R2 =(L12 +2L1・L2・cosθ+L22 )
/(2sinθ)2 ……(6)
該式(6)から、半径Rは、セグメント長L1 、L2 及び方位角θの関数であることが分かる。
【0055】
そこで、前記半径算出処理手段は、前記補間点データを読み込み、対象点ma、前隣接点mb及び後隣接点mcの各座標に基づいて、セグメント長L1、L2を算出するとともに、方位角θを算出する。
【0056】
続いて、前記半径算出処理手段は、セグメント長L1、長L2及び方位角θに基づいて前記式(6)によって円Crの半径Rを算出する。このようにして、形状推定道路上に対象点maにおける半径Rを算出することができる。
【0057】
次に、フローチャートについて説明する。
ステップS1−1 前側セグメントのセグメント長L1を算出する。
ステップS1−2 後側セグメントのセグメント長L2を算出する。
ステップS1−3 方位角θを算出する。
ステップS1−4 半径Rを算出し、リターンする。
【0058】
なお、本実施の形態においては、前記半径算出処理手段によって方位角θ及び半径Rを計算に基づいて算出するようになっているが、あらかじめ算出された方位角θ及び半径Rを、道路データの一部としてデータ記録部16のデータベースに記録しておき、データ記録部16から読み出すことによって算出することもできる。
【0059】
次に、図2のステップS2におけるコーナ検出処理について説明する。
【0060】
図5は本発明の実施の形態におけるコーナ検出処理の第1の説明図、図6は本発明の実施の形態におけるコーナ検出処理の第2の説明図、図7は本発明の実施の形態におけるコーナ検出処理の第3の説明図、図8は本発明の実施の形態におけるコーナ検出処理の第4の説明図、図9は本発明の実施の形態におけるコーナ検出処理の第5の説明図、図10は本発明の実施の形態におけるコーナ検出処理の第6の説明図、図11は本発明の実施の形態におけるコーナ検出処理の第7の説明図、図12は本発明の実施の形態におけるコーナ検出処理の第8の説明図である。
【0061】
図において、ri(i=1、2、…)は図示されない二つのノード間に設定された形状推定道路、mi(i=1、2、…)は該形状推定道路riに沿って、車両の進行方向(矢印G方向)における手前側から順に設定された複数の形状補間点、θi(i=1、2、…)は各形状補間点miにおける方位角である。なお、該方位角θiは、前側セグメントに対して後側セグメントの成す角度であり、前側セグメントに対して後側セグメントが車両の進行方向における右側に位置する場合、正の値を採り、前側セグメントに対して後側セグメントが車両の進行方向における左側に位置する場合、負の値を採る。
【0062】
まず、前記コーナ検出処理手段の開始候補点・終了候補点設定処理手段は、開始候補点・終了候補点設定処理を行い、前記各形状補間点miにおける半径Ri(i=1、2、…)を読み込み、該半径Riが、あらかじめ設定されたコーナ検出用の閾値Rth(本実施の形態においては、1000〔m〕)以下であるかどうかを判断し、半径が閾値Rth以下の形状補間点を抽出する。図5においては、形状補間点m3〜m9が、直線に近く、閾値Rth以下であると判断され、抽出される。
【0063】
続いて、前記開始候補点・終了候補点設定処理手段は、抽出された各形状補間点において、複数の連続する形状補間点(以下「連続形状補間点群」という。)があるかどうかを判断し、連続形状補間点群がある場合、連続形状補間点群における最も手前(自車位置側)の形状補間点を開始候補点とする。図5及び6においては、形状補間点m3〜m9によって連続形状補間点群が構成され、形状補間点m3が開始候補点s1にされる。
【0064】
また、前記開始候補点・終了候補点設定処理手段は、抽出された形状補間点について、前記半径算出処理手段によって算出された方位角θを手前側から順に読み込み、所定の形状補間点において方位角の正負が反転する(一つ手前の形状補間点に対して方位角の方向が逆になる。)かどうかを判断し、所定の形状補間点において方位角の正負が反転する場合に、一つ先の形状補間点において正負が反転するかどうかを判断する。そして、所定の形状補間点において方位角の正負が反転し、かつ、一つ先の形状補間点において正負が反転しない場合は、前記所定の形状補間点を開始候補点s1とする。図7に示されるような形状を有する形状推定道路r2において、形状補間点m11〜m16が設定されている場合、各形状補間点m12〜m15において、各方位角θ12及びθ13は負の値を採り、方位角θ14及びθ15は正の値を採る。すなわち、形状補間点m14において方位角θ14の正負が負から正に反転し、一つ先の形状補間点m15において方位角θ15の正負が正のままで反転しないので、形状補間点m14が開始候補点s1にされる。
【0065】
次に、前記開始候補点・終了候補点設定処理手段は、抽出された各形状補間点において、連続形状補間点群があるかどうかを判断し、連続形状補間点群がある場合、連続形状補間点群における最も先側(自車位置から離れる側)の形状補間点を終了候補点とする。図5及び8においては、形状補間点m3〜m9によって連続形状補間点群が構成され、形状補間点m9が終了候補点e1にされる。
【0066】
また、前記開始候補点・終了候補点設定処理手段は、所定の形状補間点において方位角の正負が反転するかどうかを判断し、所定の形状補間点において方位角の正負が反転しない場合に、一つ先の形状補間点において正負が反転するかどうかを判断する。そして、所定の形状補間点において方位角の正負が反転せず、一つ先の形状補間点において正負が反転する場合は、前記所定の形状補間点を終了候補点e1とする。図9に示されるような形状を有する形状推定道路r3において、形状補間点m21〜m26が設定されている場合に、各形状補間点m22〜m25における方位角θ22及びθ23は正の値を採り、方位角θ24及びθ25は負の値を採る。すなわち、形状補間点m23においては方位角θ23の正負が正のままで反転せず、一つ先の形状補間点m24においては方位角θ24の正負が正から負に反転するので、形状補間点m23が終了候補点e1にされる。
【0067】
このようにして開始候補点及び終了候補点が設定されると、前記コーナ検出処理手段のコーナ設定処理手段は、コーナ設定処理を行い、開始候補点と終了候補点との間にコーナを設定する。図10に示されるような形状を有する形状推定道路r4においては、形状補間点m1〜m10のうちの形状補間点m2が開始候補点s1にされ、形状補間点m9が終了候補点e1にされ、開始候補点s1と終了候補点e1との間にコーナcn1が設定される。
【0068】
なお、所定の形状補間点の半径が閾値Rth以下であり、一つ手前の形状補間点の半径、及び一つ先の形状補間点の半径がいずれも閾値Rthより大きい場合、前記開始候補点・終了候補点設定処理手段は、前記所定の形状補間点を、開始候補点及び終了候補点を兼ねる単発点として設定し、前記コーナ設定処理手段は、前記所定の形状補間点にコーナを設定する。また、図11に示されるような形状を有する形状推定道路r5においては、形状補間点m31〜m35のうちの形状補間点m33が単発点f1に設定され、形状補間点m33にコーナcn1が設定される。
【0069】
さらに、所定の形状補間点において方位角の正負が反転し、一つ先の形状補間点における方位角の正負が反転する場合、前記開始候補点・終了候補点設定処理手段は、前記所定の形状補間点を単発点として設定する。
【0070】
図12に示されるような形状を有する形状推定道路r6においては、形状補間点m41〜m48のうちの形状補間点m42〜m44、m46及びm47の方位角θ42〜θ44、θ46及びθ47は正の値を採り、形状補間点m45の方位角θ45は負の値を採る。この場合、形状補間点m45において方位角θ45の正負が正から負に反転し、一つ先の形状補間点m46において方位角θ46の正負が負から正に反転するので、形状補間点m45が、単発点f1に設定され、形状補間点m45にコーナcn1が設定される。
【0071】
続いて、前記コーナ設定処理手段は、単発点f1の前後、及び単発点f1において三つのコーナcn1〜cn3を設定する。なお、この場合、コーナの番号は手前側から順次付与され、コーナの番号に対応させて開始候補点の番号及び終了候補点の番号が付与される。
【0072】
このようにして形状推定道路においてコーナが設定されると、前記コーナ検出処理手段の記録処理手段は、記録処理を行い、形状推定道路内のコーナの番号、開始候補点の番号、終了候補点の番号等をRAM32(図1)に記録する。
【0073】
ところで、前述されたように、クロソイド係数推定処理においては、形状推定道路において検出されたコーナにおける最小半径に基づいてクロソイド係数が推定されるようになっているが、前記半径算出処理で3点計算法によって算出された半径のうちの最も小さい半径を最小半径として使用すると、該最小半径は本来使用すべき値より小さくなってしまう傾向がある。
【0074】
図13は3点計算法によって算出される半径を説明する第1の図、図14は3点計算法によって算出される半径を説明する第2の図である。
【0075】
図において、m51〜56、m61〜m71は形状補間点、s1は開始候補点、e1は終了候補点である。
【0076】
前記半径算出処理において算出された半径のうちの最も小さい半径を最小半径として使用する場合、図13においては、半径算出処理の対象となる形状補間点m53が対象点とされ、該対象点の一つ手前の形状補間点m52が前隣接点とされ、対象点の一つ先の形状補間点m54が後隣接点とされ、対象点、前隣接点及び後隣接点を通過する円CLcの半径が形状補間点m53における半径とされる。ここで、対象点、前隣接点及び後隣接点の3点だけでなく、形状補間点m51〜m56の座標に基づいて近似曲線を算出し、曲率半径を算出すると、該曲率半径は実際の道路の曲率半径に近い値を採る。そして、前記曲率半径を有する円を描くとCLdになり、このことから、3点計算法によって算出された半径は、実際の道路の曲率半径より小さくなる。
【0077】
また、図14においては、半径算出処理の対象となる形状補間点m66が対象点とされ、該対象点の一つ手前の形状補間点m65が前隣接点とされ、対象点の一つ先の形状補間点m67が後隣接点とされ、対象点、前隣接点及び後隣接点を通過する円CLcの半径が形状補間点m66における半径とされる。ここで、対象点、前隣接点及び後隣接点の3点だけでなく、形状補間点m61〜m71の座標に基づいて近似曲線を算出し、曲率半径を算出すると、該曲率半径は実際の道路の曲率半径に近い値を採る。そして、前記曲率半径を有する円を描くとCLdになり、この場合も、3点計算法によって算出された半径は、実際の道路の曲率半径より小さくなる。しかも、コーナの形状が大きくなると、その分、3点計算法によって算出された半径と実際の道路の曲率半径との差は大きくなる。
【0078】
したがって、前記半径算出処理手段によって算出された半径のうちの最も小さい半径を最小半径として使用すると、該最小半径は本来使用すべき値より小さくなるので、前記最小半径に基づいてクロソイド係数を推定すると、クロソイド曲線を適正に算出することができず、道路形状を正確に推定することができなくなってしまう。
【0079】
そこで、本実施の形態においては、検出されたコーナ内において、三つの仮想点を設定し、各仮想点の座標に基づいて、半径を算出し、算出された半径を最小半径とするようにしている。
【0080】
次に、図2のステップS6における最小半径算出処理のサブルーチンについて説明する。この場合、前述されたように、コーナ結合処理においてコーナが結合され、第1、第2のコーナ分割処理においてコーナが分割された後のコーナにおいて最小半径を算出する場合について説明する。
【0081】
図15は本発明の実施の形態における最小半径算出処理のサブルーチンを示す図、図16は本発明の実施の形態における最小半径算出処理の第1の説明図、図17は本発明の実施の形態における最小半径算出処理の第2の説明図、図18は本発明の実施の形態における最小半径算出処理の第3の説明図、図19は本発明の実施の形態における最小半径算出処理の第4の説明図、図20は本発明の実施の形態における最小半径算出処理の第5の説明図、図21は本発明の実施の形態における最小半径算出処理の第6の説明図、図22は本発明の実施の形態における最小半径算出処理の第7の説明図、図23は本発明の実施の形態における最小半径算出処理の第8の説明図、図24は本発明の実施の形態における最小半径算出処理の第9の説明図、図25は本発明の実施の形態における最小半径算出処理の第10の説明図、図26は本発明の実施の形態における最小半径算出処理の第11の説明図、図27は本発明の実施の形態における最小半径算出処理の第12の説明図である。
【0082】
図16において、r7は形状推定道路、cn1はコーナ、miは形状補間点、s1はコーナcn1の開始候補点、e1はコーナcn1の終了候補点である。
【0083】
まず、前記最小半径算出処理手段の中心仮想点設定処理手段は、中心仮想点設定処理を行い、第1の仮想点として中心仮想点Pvcを設定する。そのために、前記中心仮想点設定処理手段のコーナ中心付近設定処理手段は、コーナ中心付近設定処理を行い、各形状補間点m2〜m9間のセグメント長を加算することによって、図17に示されるコーナcn1の開始候補点slから終了候補点e1までの距離をコーナ長Lseとして算出する。
【0084】
続いて、前記コーナ中心付近設定処理手段は、開始候補点slより終了候補点e1側に、開始候補点slからコーナ長Lseの半分の距離Lf
Lf=Lse/2
だけ離れた位置、すなわち、開始候補点slと終了候補点e1との間の中心の位置にコーナ中間地点Pcを、該コーナ中間地点Pcより手前側の開始候補点slとの間において、コーナ中間地点Pcから距離Lf/ε1だけ離れた位置にコーナ前方地点Pfを、前記コーナ中間地点Pcより先側の終了候補点e1との間において、コーナ中間地点Pcから距離Lf/ε2だけ離れた位置にコーナ後方地点Pbを設定し、前記コーナ前方地点Pfからコーナ後方地点Pbまでの範囲をコーナ中心付近AR1として設定する。
【0085】
なお、本実施の形態において、ε1、ε2は、コーナ中間地点Pcより開始候補点sl側に前方地点Pfを、コーナ中間地点Pcより終了候補点e1側にコーナ後方地点Pbを設定するための設定値であり、本実施の形態においては、2に設定される。
【0086】
次に、前記中心仮想点設定処理手段の中心仮想点決定処理手段は、中心仮想点決定処理を行い、コーナ中心付近AR1内に、前記半径算出処理で算出された半径Riのうちの最も小さい半径を有する形状補間点があるかどうかを判断し、最も小さい半径を有する形状補間点がある場合、該形状補間点を中心仮想点Pvcとして設定する。図19に示されるように、例えば、形状補間点m5が、半径Riのうちの最も小さい半径を有する場合、形状補間点m5はコーナ中心付近AR1内にあるので、中心仮想点決定処理手段は、形状補間点m5を中心仮想点Pvcとする。
【0087】
また、前記コーナ中心付近AR1内に、最も小さい半径を有する形状補間点がない場合、中心仮想点決定処理手段は、前記コーナ中間地点Pcを中心仮想点Pvcとする。図20に示されるように、例えば、形状補間点m3が、半径Riのうちの最も小さい半径を有する場合、形状補間点m3はコーナ中心付近AR1内にないので、中心仮想点決定処理手段は、コーナ中間地点Pcを中心仮想点Pvcとする。
【0088】
このように、中心仮想点Pvcが設定されると、前記最小半径算出処理手段の前方仮想点設定処理手段は、前方仮想点設定処理を行い、第2の仮想点として前方仮想点Pvfを設定する。そのために、前記前方仮想点設定処理手段の距離設定処理手段は、距離設定処理を行い、中心仮想点Pvcから開始候補点slまでの距離をLsとしたとき、図21に示される距離Lsの半分の距離Ls/2と閾値Lsth(例えば、35〔m〕)とを比較する。そして、前記距離設定処理手段は、距離Ls/2が閾値Lsth以上である場合、距離Ls/2を、中心仮想点Pvcから前方仮想点Pvfを設定するための前方仮想点距離Lvfに設定し、距離Ls/2が閾値Lsth未満である場合、閾値Lsthを前方仮想点距離Lvfに設定する。
【0089】
続いて、前記距離設定処理手段は、前方仮想点距離Lvfと前記距離Lsとを比較する。図22に示されるように、前方仮想点距離Lvfが距離Ls未満である場合、前記前方仮想点設定処理手段の前方仮想点決定処理手段は、前方仮想点決定処理を行い、中心仮想点Pvcより手前側の、中心仮想点Pvcから前方仮想点距離Lvfだけ離れた位置に前方仮想点Pvfを設定する。また、図23に示されるように、前方仮想点距離Lvfが距離Ls以上である場合、前記前方仮想点決定処理手段は、中心仮想点Pvcより手前側の、中心仮想点Pvcから距離Lsだけ離れた位置(開始候補点sl)に前方仮想点Pvfを設定する。
【0090】
この場合、開始候補点slと中心仮想点Pvcとの中間点を第1の候補点P1とし、中心仮想点Pvcより手前側の、中心仮想点Pvcから閾値Lthfだけ離れた位置を第2の候補点P2としたときに、前方仮想点Pvfは、第1、第2の候補点P1、P2、及び、開始候補点slのうちのいずれか一つの点に設定される。
【0091】
すなわち、前方仮想点Pvfは、開始候補点slより中心仮想点Pvcから離れる側には設定されず、また、第1の候補点P1より中心仮想点Pvc側には設定されない。
【0092】
次に、前記最小半径算出処理手段の後方仮想点設定処理手段は、後方仮想点設定処理を行い、第3の仮想点として後方仮想点Pvbを設定する。そのために、前記後方仮想点設定処理手段の距離設定処理手段は、距離設定処理を行い、中心仮想点Pvcから終了候補点e1までの距離をLeとしたとき、図24に示される距離Leの半分の距離Le/2と閾値Leth(例えば、35〔m〕)とを比較する。そして、前記距離設定処理手段は、距離Le/2が閾値Leth以上である場合、距離Le/2を、中心仮想点Pvcから後方仮想点Pvbを設定するための後方仮想点距離Lvbに設定し、距離Le/2が閾値Leth未満である場合、閾値Lethを後方仮想点距離Lvbに設定する。
【0093】
続いて、前記距離設定処理手段は、後方仮想点距離Lvbと前記距離Leとを比較する。図25に示されるように、後方仮想点距離Lvbが距離Le未満である場合、前記後方仮想点設定処理手段の後方仮想点決定処理手段は、後方仮想点決定処理を行い、中心仮想点Pvcより先側の、中心仮想点Pvcから後方仮想点距離Lvbだけ離れた位置に後方仮想点Pvbを設定する。また、図26に示されるように、仮想点距離Lvbが距離Le以上である場合、前記後方仮想点決定処理手段は、中心仮想点Pvcより先側の、中心仮想点Pvcから距離Leだけ離れた位置(終了候補点e1)に後方仮想点Pvbを設定する。
【0094】
この場合、終了候補点e1と中心仮想点Pvcとの中間点を第1の候補点P11とし、中心仮想点Pvcより先側の、中心仮想点Pvcから閾値Lthbだけ離れた位置を第2の候補点P12としたときに、後方仮想点Pvbは、第1、第2の候補点P11、P12、及び、終了候補点e1のうちのいずれか一つの点に設定される。
【0095】
すなわち、後方仮想点Pvbは、終了候補点e1より中心仮想点Pvcから離れる側には設定されず、また、第1の候補点P11より中心仮想点Pvc側には設定されない。
【0096】
続いて、前記最小半径算出処理手段の仮想半径算出処理手段は、仮想半径算出処理を行い、中心仮想点Pvc、前方仮想点Pvf及び後方仮想点Pvbの座標に基づいて、3点計算法によって仮想曲率半径としての半径を算出し、最小半径とする。
【0097】
このように、本実施の形態においては、コーナcn1の中心付近AR1に、中心仮想点Pvcが設定され、第1、第2の候補点P1、P2及び開始候補点s1のうちのいずれか一つの点に前方仮想点Pvfが設定され、第1、第2の候補点P11、P12及び終了候補点e1のうちのいずれか一つの点に後方仮想点Pvfが設定され、図27に示されるように、中心仮想点Pvc、前方仮想点Pvf及び後方仮想点Pvbに基づいて、3点計算法によって最小半径が算出されるので、最小半径を実際の道路上の最小半径に近似する値にすることができる。
【0098】
すなわち、中心仮想点Pvc、前方仮想点Pvf及び後方仮想点Pvbを通過する円をCLcとし、形状補間点m1〜m11の座標に基づいて曲率半径を算出し、該曲率半径を有する円を描くとCLdになり、このことから、各円CLc、CLdは類似する形状を有する。
【0099】
また、本実施の形態において、前方仮想点Pvfは、第1の候補点P1より中心仮想点Pvc側には設定されず、また、後方仮想点Pvbは、第1の候補点P11より中心仮想点Pvc側には設定されないので、前記半径算出処理手段によって最小半径を算出する場合に比べて最小半径が小さくなるのを防止することができる。
【0100】
次に、フローチャートについて説明する。
ステップS6−1 中心仮想点Pvcを設定する。
ステップS6−2 前方仮想点Pvfを設定する。
ステップS6−3 後方仮想点Pvbを設定する。
ステップS6−4 仮想半径算出処理を行い、リターンする。
【0101】
次に、図2のステップS7におけるクロソイド係数推定処理について説明する。
【0102】
図28は本発明の実施の形態におけるクロソイド係数マップを示す図である。
【0103】
この場合、最小半径Rminとクロソイド曲線のクロソイド係数Aの2乗の値A2 とが、互いに対応させてROM33(図1)にクロソイド係数マップとして記録され、前記クロソイド係数推定処理手段は、前記ステップS6で算出された最小半径Rminを読み込み、クロソイド係数マップを参照し、最小半径Rminに対応する値A2 を読み出す。続いて、前記クロソイド係数推定処理手段は、クロソイド係数Aを算出する。なお、前記クロソイド係数マップは、実際の道路における半径と、前記道路を設計する際に使用されたクロソイド曲線のクロソイド係数との関係に基づいて作成されたものである。
【0104】
次に、図2のステップS8におけるクロソイド曲線算出処理のサブルーチンについて説明する。
【0105】
図29は本発明の実施の形態におけるクロソイド曲線算出処理のサブルーチンを示す図である。
【0106】
ところで、クロソイド曲線は、始点からの長さが大きくなるほど曲率半径が小さくなる曲線であり、一般的には、道路を設計する際に、クロソイド曲線の一部がコーナの開始点及び終了点に当てられるようになっている。前記クロソイド曲線をx−y座標で表すと、クロソイド曲線の開始点(原点)から所定の点Sまでの距離、すなわち、曲線長をLとし、前記点Sにおける曲線半径をRcとすると、
L・Rc=A2 ……(7)
になる。また、通常、クロソイド曲線のX座標及びY座標は、それぞれ、式(8)及び(9)で示される多項式によって算出することができる。
【0107】
X=L×(1−L2 /(40Rc2 )+L4 /(3456Rc4 )
−L6 /(599040Rc6 )
+L8 /(175472640Rc8 )…) ……(8)
X=L2 /(6Rc)×(1−L2 /(56Rc2 )+
L4 /(7040Rc4 )−L6 /(1612800Rc6 )
+L8 /(588349440Rc8 )…) ……(9)
この場合、クロソイド曲線を前記式(8)及び(9)で算出することが考えられるが、式(8)及び(9)は、高次の多項式であり、多数の点をプロットしてクロソイド曲線を算出しようとすると、クロソイド曲線算出処理手段によってCPU31に加わる負荷が大きくなってしまう。
【0108】
そこで、本実施の形態においては、前記クロソイド曲線算出処理手段によって、クロソイド係数推定処理において算出されたクロソイド係数Aに基づいてクロソイド曲線を算出するようにしている。そのために、前記クロソイド曲線算出処理手段は、クロソイド曲線(曲線長L(k)の点S)のX座標及びY座標を、シュミレーションに基づいて得られた式(10)及び(11)によって近似させ、算出する。
【0109】
【数1】
【0110】
ただし、X0 及びY0 は、クロソイド曲線の開始点のX座標及びY座標である。また、角度φは次の式(12)で表される。
【0111】
φ=α+2k・L ……(12)
そして、αはクロソイド曲線の開始点の方位であり、値kは式(13)で表される。
【0112】
k=28/A2 ……(13)
次に、フローチャートについて説明する。
ステップS8−1 クロソイド曲線の曲線長L(k)でループを開始する。
ステップS8−2 曲線長L(k)の点SのX座標を算出する。
ステップS8−3 曲線長L(k)の点SのY座標を算出する。
ステップS8−4 クロソイド曲線の曲線長L(k)でループを終了させ、リターンする。
【0113】
次に、図2のステップS9におけるフィッティング処理について説明する。
【0114】
図30は本発明の実施の形態におけるフィッティング処理の説明図である。
【0115】
図において、r7は形状推定道路、cn1は前記コーナ結合処理及び第1、第2のコーナ分割処理によって設定されたコーナ、m1〜m5はコーナcn1を構成する形状補間点、s1はコーナcn1の開始候補点である。
【0116】
この場合、前記フィッティング処理手段のクロソイド曲線移動処理手段は、クロソイド曲線移動処理を行い、形状補間点m1、m2間を結ぶセグメントSg1の延長線上に、クロソイド曲線Qの始点Qsを置き、該始点Qsにおける接線とセグメントSg1の延長線とが一致するようにクロソイド曲線Qを矢印E方向に移動させる。
【0117】
このとき、前記フィッティング処理手段の誤差算出処理手段は、誤差算出処理を行い、形状補間点m2以降の各形状補間点m2〜m5に対するクロソイド曲線Qの位置の誤差の合計を算出する。この場合、各形状補間点m2〜m5に対するクロソイド曲線Qの位置の誤差は、各形状補間点m2〜m5とクロソイド曲線Qを描く際にプロットされる各点との前記X軸上の距離及びY軸上の距離に基づいて算出される。
【0118】
そして、前記クロソイド曲線移動処理手段は、誤差の合計が最も小さくなるように前記クロソイド曲線Qを各形状補間点m2〜m5に合わせ、クロソイド曲線Qの位置を設定する。
【0119】
続いて、前記フィッティング処理手段の開始点・終了点抽出処理手段は、開始点・終了点抽出処理を行い、クロソイド曲線Qの位置が設定されると、クロソイド曲線Qの始点Qsを実際のコーナの開始点Sa1として設定する。
【0120】
また、クロソイド曲線移動処理手段、誤差算出処理手段及び開始点・終了点抽出処理手段は、コーナcn1の終点側において同様の処理を行い、クロソイド曲線Qの位置を設定し、クロソイド曲線Qの始点を実際のコーナの終了点として設定する。
【0121】
このようにして、クロソイド曲線Qによって開始候補点及び終了候補点を補正し、補正された開始候補点及び終了候補点を、それぞれコーナcn1の開始点Sa1及び終了点として設定することができるので、コーナcn1の開始点Sa1及び終了点を実際の道路上のコーナの開始点及び終了点に近づけることができる。
【0122】
次に、図2のステップS10における開始点・終了点記録処理について説明する。
【0123】
前記開始点・終了点記録処理手段は、抽出された開始点及び終了点の位置を、道路形状を表すデータとしてデータ記録部16に記録する。
【0124】
このように、本実施の形態においては、道路形状を正確に推定することができ、正確に推定された道路形状に基づいてコーナcn1の開始点Sa1及び終了点を設定するようになっているので、道路形状に対応させて自動変速機10の変速段を変更したり、道路形状に対応させてエンジン52の出力を変更したりして、車両の走行制御を精度良く行うことができる。
【0125】
本実施の形態において、道路形状の推定は、ナビゲーション装置14のCPU31を動作させることによって行われるようになっているが、自動変速機制御装置11の図示されないCPUを作動させたり、又は前記情報センタの制御装置としてのCPUを動作させたりすることによって道路形状の推定を行うことができる。なお、車両の全体の制御を行うために、自動変速機制御装置11及びエンジン制御装置51の上位の制御装置として車両制御装置が配設されるが、該車両制御装置のCPUを動作させることによって道路形状の推定を行うこともできる。
【0126】
また、本実施の形態においては、隣接する各ノード間の形状推定道路について道路形状の推定を行うようになっているが、実際の道路上において、コーナ内にノードがある場合、各ノード間の形状推定道路ごとに道路形状を推定すると、コーナ内におけるノードの前後において処理が異なってしまう。そこで、他の実施の形態においては、道路データにおいて、各道路リンク間に道なりデータが付与されていて、ノードを通過した後に進入することが予測される道路リンクが明らかな場合、又は経路探索処理によって探索経路が設定されている場合等においては、二つ以上の連続する形状推定道路について道路形状の推定を行うことができる。
【0127】
なお、本発明は前記実施の形態に限定されるものではなく、本発明の趣旨に基づいて種々変形させることが可能であり、それらを本発明の範囲から排除するものではない。
【図面の簡単な説明】
【0128】
【図1】本発明の実施の形態における車両制御システムのブロック図である。
【図2】本発明の実施の形態における道路形状推定処理手段の動作を示すメインフローチャートである。
【図3】本発明の実施の形態における半径算出処理のサブルーチンを示す図である。
【図4】本発明の実施の形態における半径を算出する原理を説明する図である。
【図5】本発明の実施の形態におけるコーナ検出処理の第1の説明図である。
【図6】本発明の実施の形態におけるコーナ検出処理の第2の説明図である。
【図7】本発明の実施の形態におけるコーナ検出処理の第3の説明図である。
【図8】本発明の実施の形態におけるコーナ検出処理の第4の説明図である。
【図9】本発明の実施の形態におけるコーナ検出処理の第5の説明図である。
【図10】本発明の実施の形態におけるコーナ検出処理の第6の説明図である。
【図11】本発明の実施の形態におけるコーナ検出処理の第7の説明図である。
【図12】本発明の実施の形態におけるコーナ検出処理の第8の説明図である。
【図13】3点計算法によって算出される半径を説明する第1の図である。
【図14】3点計算法によって算出される半径を説明する第2の図である。
【図15】本発明の実施の形態における最小半径算出処理のサブルーチンを示す図である。
【図16】本発明の実施の形態における最小半径算出処理の第1の説明図である。
【図17】本発明の実施の形態における最小半径算出処理の第2の説明図である。
【図18】本発明の実施の形態における最小半径算出処理の第3の説明図である。
【図19】本発明の実施の形態における最小半径算出処理の第4の説明図である。
【図20】本発明の実施の形態における最小半径算出処理の第5の説明図である。
【図21】本発明の実施の形態における最小半径算出処理の第6の説明図である。
【図22】本発明の実施の形態における最小半径算出処理の第7の説明図である。
【図23】本発明の実施の形態における最小半径算出処理の第8の説明図である。
【図24】本発明の実施の形態における最小半径算出処理の第9の説明図である。
【図25】本発明の実施の形態における最小半径算出処理の第10の説明図である。
【図26】本発明の実施の形態における最小半径算出処理の第11の説明図である。
【図27】本発明の実施の形態における最小半径算出処理の第12の説明図である。
【図28】本発明の実施の形態におけるクロソイド係数マップを示す図である。
【図29】本発明の実施の形態におけるクロソイド曲線算出処理のサブルーチンを示す図である。
【図30】本発明の実施の形態におけるフィッティング処理の説明図である。
【符号の説明】
【0129】
16 データ記録部
31 CPU
cn1 cn11 cn21 コーナ
e1 終了候補点
ma 対象点
mi、m1〜m16、m21〜m26、m31〜m35、m51〜m56、m61〜m71 形状補間点
Pvb 後方仮想点
Pvc 中心仮想点
Pvf 前方仮想点
Q クロソイド曲線
Rmin 最小半径
s1 開始候補点
【特許請求の範囲】
【請求項1】
道路に沿って設定され、道路の形状を表す複数の形状補間点の補間点データを取得するデータ取得処理手段と、前記道路における所定の区間内の前記補間点データに基づいて各形状補間点における曲率半径を算出する半径算出処理手段と、前記各曲率半径に基づいて、前記所定の区間内のコーナを検出するとともに、該検出されたコーナの開始候補点及び終了候補点を設定するコーナ検出処理手段と、前記コーナの開始候補点及び終了候補点に基づいて中心仮想点を、該中心仮想点より開始候補点側に前方仮想点を、前記中心仮想点より終了候補点側に後方仮想点を設定し、中心仮想点、前方仮想点及び後方仮想点に基づいて前記コーナの仮想曲率半径を最小半径として算出する最小半径算出処理手段とを有することを特徴とする道路形状推定装置。
【請求項2】
前記開始候補点と終了候補点との間の中心の位置にコーナ中間地点が、前記開始候補点とコーナ中間地点との間にコーナ前方地点が、前記終了候補点とコーナ中間地点との間にコーナ後方地点が設定され、前記コーナ前方地点からコーナ後方地点までの範囲に前記中心仮想点が設定される請求項1に記載の道路形状推定装置。
【請求項3】
前記開始候補点と中心仮想点との中間点を第1の候補点とし、中心仮想点より手前側の、中心仮想点から閾値だけ離れた位置を第2の候補点としたときに、前記前方仮想点は、第1、第2の候補点、及び、前記開始候補点のうちのいずれか一つの点に設定される請求項1又は2に記載の道路形状推定装置。
【請求項4】
前記終了候補点と中心仮想点との中間点を第1の候補点とし、中心仮想点より先側の、中心仮想点から閾値だけ離れた位置を第2の候補点としたときに、前記後方仮想点は、第1、第2の候補点、及び、前記終了候補点のうちのいずれか一つの点に設定される請求項1又は2に記載の道路形状推定装置。
【請求項5】
道路に沿って設定され、道路の形状を表す複数の形状補間点の補間点データを取得し、前記道路における所定の区間内の前記補間点データに基づいて各形状補間点における曲率半径を算出し、該各曲率半径に基づいて、前記所定の区間内のコーナを検出するとともに、該検出されたコーナの開始候補点及び終了候補点を設定し、前記コーナの開始候補点及び終了候補点に基づいて中心仮想点を、該中心仮想点より開始候補点側に前方仮想点を、前記中心仮想点より終了候補点側に後方仮想点を設定し、中心仮想点、前方仮想点及び後方仮想点に基づいて前記コーナの仮想曲率半径を最小半径として算出することを特徴とする道路形状推定方法。
【請求項6】
コンピュータを、道路に沿って設定され、道路の形状を表す複数の形状補間点の補間点データを取得するデータ取得処理手段、前記道路における所定の区間内の前記補間点データに基づいて各形状補間点における曲率半径を算出する半径算出処理手段、前記各曲率半径に基づいて、前記所定の区間内のコーナを検出するとともに、該検出されたコーナの開始候補点及び終了候補点を設定するコーナ検出処理手段、並びに前記コーナの開始候補点及び終了候補点に基づいて中心仮想点を、該中心仮想点より開始候補点側に前方仮想点を、前記中心仮想点より終了候補点側に後方仮想点を設定し、中心仮想点、前方仮想点及び後方仮想点に基づいて前記コーナの仮想曲率半径を最小半径として算出する最小半径算出処理手段として機能させることを特徴とするプログラム。
【請求項1】
道路に沿って設定され、道路の形状を表す複数の形状補間点の補間点データを取得するデータ取得処理手段と、前記道路における所定の区間内の前記補間点データに基づいて各形状補間点における曲率半径を算出する半径算出処理手段と、前記各曲率半径に基づいて、前記所定の区間内のコーナを検出するとともに、該検出されたコーナの開始候補点及び終了候補点を設定するコーナ検出処理手段と、前記コーナの開始候補点及び終了候補点に基づいて中心仮想点を、該中心仮想点より開始候補点側に前方仮想点を、前記中心仮想点より終了候補点側に後方仮想点を設定し、中心仮想点、前方仮想点及び後方仮想点に基づいて前記コーナの仮想曲率半径を最小半径として算出する最小半径算出処理手段とを有することを特徴とする道路形状推定装置。
【請求項2】
前記開始候補点と終了候補点との間の中心の位置にコーナ中間地点が、前記開始候補点とコーナ中間地点との間にコーナ前方地点が、前記終了候補点とコーナ中間地点との間にコーナ後方地点が設定され、前記コーナ前方地点からコーナ後方地点までの範囲に前記中心仮想点が設定される請求項1に記載の道路形状推定装置。
【請求項3】
前記開始候補点と中心仮想点との中間点を第1の候補点とし、中心仮想点より手前側の、中心仮想点から閾値だけ離れた位置を第2の候補点としたときに、前記前方仮想点は、第1、第2の候補点、及び、前記開始候補点のうちのいずれか一つの点に設定される請求項1又は2に記載の道路形状推定装置。
【請求項4】
前記終了候補点と中心仮想点との中間点を第1の候補点とし、中心仮想点より先側の、中心仮想点から閾値だけ離れた位置を第2の候補点としたときに、前記後方仮想点は、第1、第2の候補点、及び、前記終了候補点のうちのいずれか一つの点に設定される請求項1又は2に記載の道路形状推定装置。
【請求項5】
道路に沿って設定され、道路の形状を表す複数の形状補間点の補間点データを取得し、前記道路における所定の区間内の前記補間点データに基づいて各形状補間点における曲率半径を算出し、該各曲率半径に基づいて、前記所定の区間内のコーナを検出するとともに、該検出されたコーナの開始候補点及び終了候補点を設定し、前記コーナの開始候補点及び終了候補点に基づいて中心仮想点を、該中心仮想点より開始候補点側に前方仮想点を、前記中心仮想点より終了候補点側に後方仮想点を設定し、中心仮想点、前方仮想点及び後方仮想点に基づいて前記コーナの仮想曲率半径を最小半径として算出することを特徴とする道路形状推定方法。
【請求項6】
コンピュータを、道路に沿って設定され、道路の形状を表す複数の形状補間点の補間点データを取得するデータ取得処理手段、前記道路における所定の区間内の前記補間点データに基づいて各形状補間点における曲率半径を算出する半径算出処理手段、前記各曲率半径に基づいて、前記所定の区間内のコーナを検出するとともに、該検出されたコーナの開始候補点及び終了候補点を設定するコーナ検出処理手段、並びに前記コーナの開始候補点及び終了候補点に基づいて中心仮想点を、該中心仮想点より開始候補点側に前方仮想点を、前記中心仮想点より終了候補点側に後方仮想点を設定し、中心仮想点、前方仮想点及び後方仮想点に基づいて前記コーナの仮想曲率半径を最小半径として算出する最小半径算出処理手段として機能させることを特徴とするプログラム。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【図20】
【図21】
【図22】
【図23】
【図24】
【図25】
【図26】
【図27】
【図28】
【図29】
【図30】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【図20】
【図21】
【図22】
【図23】
【図24】
【図25】
【図26】
【図27】
【図28】
【図29】
【図30】
【公開番号】特開2009−243948(P2009−243948A)
【公開日】平成21年10月22日(2009.10.22)
【国際特許分類】
【出願番号】特願2008−88165(P2008−88165)
【出願日】平成20年3月28日(2008.3.28)
【出願人】(000100768)アイシン・エィ・ダブリュ株式会社 (3,717)
【Fターム(参考)】
【公開日】平成21年10月22日(2009.10.22)
【国際特許分類】
【出願日】平成20年3月28日(2008.3.28)
【出願人】(000100768)アイシン・エィ・ダブリュ株式会社 (3,717)
【Fターム(参考)】
[ Back to top ]