(57)【要約】【課題】現実的な軌道が取得される可能性を高めることができる技術の提供。【解決手段】自車両と他車両について、予測期間内に車両が達し得る各位置において前記車両で実現し得る複数の状態を取得し、前記自車両が前記位置および前記状態を変化させる際のコストであって前記他車両が前記自車両と接触する可能性が高いほど大きくなる自車両コストと、前記他車両が前記位置および前記状態を変化させる際のコストであって前記他車両が前記自車両と接触する可能性が高いほど大きくなる他車両コストと、に対して優先度が高い車両の重み付けが相対的に小さい値である係数を乗じて加えた和を取得し、前記和の累積値が最小である軌道を前記自車両の予定軌道として取得する、予定軌道取得システムを構成する。【選択図】図１
2020139774.tif 000002
【特許請求の範囲】
【請求項１】
予測期間内に自車両が達し得る各位置において前記自車両で実現し得る複数の状態を取得する自車両位置状態取得部と、
前記予測期間内に前記自車両以外の他車両が達し得る各位置において前記他車両で実現し得る複数の前記状態を取得する他車両位置状態取得部と、
前記自車両が前記位置および前記状態を変化させる際のコストであって前記他車両が前記自車両と接触する可能性が高いほど大きくなる自車両コストと、前記他車両が前記位置および前記状態を変化させる際のコストであって前記他車両が前記自車両と接触する可能性が高いほど大きくなる他車両コストと、に対して優先度が高い車両の重み付けが相対的に小さい値である係数を乗じて加えた和を取得するコスト取得部と、
前記自車両が達し得る複数の前記位置および各位置における前記自車両の前記状態によって規定される軌道であって、当該軌道に含まれる前記位置のそれぞれにおける前記和の累積値が最小である軌道を前記自車両の予定軌道として取得する予定軌道取得部と、
を備える予定軌道取得システム。
【請求項２】
前記状態は、車両の速度である、
請求項１に記載の予定軌道取得システム。
【請求項３】
前記優先度が高い車両は、
交通規則によって優先される車両である、
請求項１または請求項２に記載の予定軌道取得システム。
【請求項４】
前記コスト取得部は、
前記位置および前記状態の少なくとも一方が、前記自車両に対する規制に反する場合、前記規制に反していない場合よりも大きい前記自車両コストを取得し、
前記位置および前記状態の少なくとも一方が、前記他車両に対する規制に反する場合、前記規制に反していない場合よりも大きい前記他車両コストを取得する、
請求項１〜請求項３のいずれかに記載の予定軌道取得システム。
【請求項５】
予測期間内に自車両が達し得る各位置において前記自車両で実現し得る複数の状態を取得する自車両位置状態取得部と、
前記予測期間内に前記自車両以外の他車両が達し得る各位置において前記他車両で実現し得る複数の前記状態を取得する他車両位置状態取得部と、
前記自車両が前記位置および前記状態を変化させる際のコストであって前記他車両が前記自車両と接触する可能性が高いほど大きくなる自車両コストと、前記他車両が前記位置および前記状態を変化させる際のコストであって前記他車両が前記自車両と接触する可能性が高いほど大きくなる他車両コストと、に対して優先度が高い車両の重み付けが相対的に小さい値である係数を乗じて加えた和を取得するコスト取得部と、
前記自車両が達し得る複数の前記位置および各位置における前記自車両の前記状態によって規定される軌道であって、当該軌道に含まれる前記位置のそれぞれにおける前記和の累積値が最小である軌道を前記自車両の予定軌道として取得する予定軌道取得部と、
前記予定軌道を風景に重畳させて表示する表示制御部と、
を備える予定軌道表示システム。
【請求項６】
コンピュータを、
予測期間内に自車両が達し得る各位置において前記自車両で実現し得る複数の状態を取得する自車両位置状態取得部、
前記予測期間内に前記自車両以外の他車両が達し得る各位置において前記他車両で実現し得る複数の前記状態を取得する他車両位置状態取得部、
前記自車両が前記位置および前記状態を変化させる際のコストであって前記他車両が前記自車両と接触する可能性が高いほど大きくなる自車両コストと、前記他車両が前記位置および前記状態を変化させる際のコストであって前記他車両が前記自車両と接触する可能性が高いほど大きくなる他車両コストと、に対して優先度が高い車両の重み付けが相対的に小さい値である係数を乗じて加えた和を取得するコスト取得部、
前記自車両が達し得る複数の前記位置および各位置における前記自車両の前記状態によって規定される軌道であって、当該軌道に含まれる前記位置のそれぞれにおける前記和の累積値が最小である軌道を前記自車両の予定軌道として取得する予定軌道取得部、
として機能させる予定軌道取得プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、車両の予定軌道を取得する予定軌道取得システム、予定軌道表示システムおよび予定軌道取得プログラムに関する。
【背景技術】
【０００２】
従来、車両の予定軌道を取得する技術が知られている。例えば、特許文献１には、自車の位置および内部状態に基づいて、自車が取り得る進路を網羅的に所定の本数生成する構成が開示されている。また、特許文献１には、自車の進路と予測された障害物の進路に基づいて、衝突確率を計算し、衝突確率が所定の閾値を超えた場合に警告音を発生する構成が開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００９−６４０８８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
従来の技術においては、他車両の運転者の操作の確率分布等に基づいて他車両の進路を確率的に予測し、確率分布は、例えば、直進方向を最大とする確率分布である。従来技術においては、予測された他車両の進路に基づいて自車両の軌道が取得される。この構成においては、他車両の現在の状態から他車両の進路が予測されており、他車両と周囲との関係、例えば、他車両と自車両との関係や、他車両とその周囲の道路との関係が考慮されていない。従って、他車両ではなく自車両が優先であったとしても、他車両の進路は自車両を考慮せずに予測される。このため、自車両が優先される道路において、本来は自車両の動作に基づいて他車両の動作が決められるべきである場合であっても、他車両の動作に基づいて自車両の軌道が取得されることがあり、非現実的な軌道が取得されることがあった。
本発明は、前記課題にかんがみてなされたもので、現実的な軌道が取得される可能性を高めることができる技術の提供を目的とする。
【課題を解決するための手段】
【０００５】
上記の目的を達成するため、予定軌道取得システムは、予測期間内に自車両が達し得る各位置において自車両で実現し得る複数の状態を取得する自車両位置状態取得部と、予測期間内に自車両以外の他車両が達し得る各位置において他車両で実現し得る複数の状態を取得する他車両位置状態取得部と、自車両が位置および状態を変化させる際のコストであって他車両が自車両と接触する可能性が高いほど大きくなる自車両コストと、他車両が位置および状態を変化させる際のコストであって他車両が自車両と接触する可能性が高いほど大きくなる他車両コストと、に対して優先度が高い車両の重み付けが相対的に小さい値である係数を乗じて加えた和を取得するコスト取得部と、自車両が達し得る複数の位置および各位置における自車両の状態によって規定される軌道であって、当該軌道に含まれる位置のそれぞれにおける和の累積値が最小である軌道を自車両の予定軌道として取得する予定軌道取得部と、を備える。
【０００６】
さらに、上記の目的を達成するため、予定軌道取得プログラムは、コンピュータを、予測期間内に自車両が達し得る各位置において自車両で実現し得る複数の状態を取得する自車両位置状態取得部、予測期間内に自車両以外の他車両が達し得る各位置において他車両で実現し得る複数の状態を取得する他車両位置状態取得部、自車両が位置および状態を変化させる際のコストであって他車両が自車両と接触する可能性が高いほど大きくなる自車両コストと、他車両が位置および状態を変化させる際のコストであって他車両が自車両と接触する可能性が高いほど大きくなる他車両コストと、に対して優先度が高い車両の重み付けが相対的に小さい値である係数を乗じて加えた和を取得するコスト取得部、自車両が達し得る複数の位置および各位置における自車両の状態によって規定される軌道であって、当該軌道に含まれる位置のそれぞれにおける和の累積値が最小である軌道を自車両の予定軌道として取得する予定軌道取得部、として機能させる。
【０００７】
車両がある位置に達する際に車両で実現し得る状態は１個ではない。例えば、状態が速度である場合、異なる車速によって当該位置に達し得る。従って、自車両において選択し得る軌道を網羅的に考慮するためには、位置のみを網羅的に考慮しても不充分である。そこで、予定軌道取得システムおよび予定軌道取得プログラムにおいては、各位置において自車両および他車両で実現し得る複数の状態を取得し、複数の位置および各位置における状態によって軌道を規定し、コストの和の累積値を最小化することによって自車両の軌道を最適化する。この結果、自車両および他車両における状態が異なる場合も考慮して自車両の軌道を最適化することができる。
【０００８】
そして、自車両と他車両との関係においていずれかの車両の優先度が高い場合、優先度が高い車両は現実の世界において優先度が低い車両よりも軌道選択の自由度が高い。一方、優先度が低い車両は、優先度が高い車両の動作に合わせて（避けるように）軌道を選択すべきであり、現実の世界において軌道選択の自由度が低い。そこで、優先度が高い車両のコストに対して、優先度が低い車両のコストよりも相対的に小さい値である係数を乗じて軌道を選択すれば、優先度が高い車両の軌道を、優先度が低い車両の軌道よりも自由に選択することができる。従って、優先度が高い車両が優先的に移動するような現実的な軌道が取得される可能性を高めることができる。
【０００９】
さらに、予定軌道表示システムは、予測期間内に自車両が達し得る各位置において前記自車両で実現し得る複数の状態を取得する自車両位置状態取得部と、前記予測期間内に前記自車両以外の他車両が達し得る各位置において前記他車両で実現し得る複数の前記状態を取得する他車両位置状態取得部と、前記自車両が前記位置および前記状態を変化させる際のコストであって前記他車両が前記自車両と接触する可能性が高いほど大きくなる自車両コストと、前記他車両が前記位置および前記状態を変化させる際のコストであって前記他車両が前記自車両と接触する可能性が高いほど大きくなる他車両コストと、に対して優先度が高い車両の重み付けが相対的に小さい値である係数を乗じて加えた和を取得するコスト取得部と、前記自車両が達し得る複数の前記位置および各位置における前記自車両の前記状態によって規定される軌道であって、当該軌道に含まれる前記位置のそれぞれにおける前記和の累積値が最小である軌道を前記自車両の予定軌道として取得する予定軌道取得部と、前記予定軌道を風景に重畳させて表示する表示制御部と、を備える。この構成によれば、自車両および他車両における状態が異なる場合も考慮して最適化された自車両の軌道を利用者に対して案内することができる。また、優先度が高い車両が優先的に移動するような現実的な軌道を利用者に対して案内することができる。
【図面の簡単な説明】
【００１０】
【図１】ナビゲーションシステムのブロック図である。
【図２】ツリー構造の例を示す図である。
【図３】任意の位置および速度から遷移可能な位置および速度を示す図である。
【図４】自車両と他車両において遷移可能な位置および速度を示す図である。
【図５】図５Ａは予定軌道の表示例を示す図、図５Ｂ、図５Ｃは予定軌道の例を示す図である。
【図６】予定軌道取得処理のフローチャートである。
【図７】軌道候補探索処理のフローチャートである。
【図８】ツリー構造を利用した探索の例を示す図である。
【発明を実施するための形態】
【００１１】
ここでは、下記の順序に従って本発明の実施の形態について説明する。
（１）ナビゲーションシステムの構成：
（２）予定軌道取得処理：
（３）軌道候補探索処理：
（４）他の実施形態：
【００１２】
（１）ナビゲーションシステムの構成：
図１は、本発明の一実施形態にかかる予定軌道取得システム（および予定軌道表示システム）を含むナビゲーションシステム１０の構成を示すブロック図である。ナビゲーションシステム１０は、自車両に備えられており、ＣＰＵ，ＲＡＭ，ＲＯＭ等を備える制御部２０、記録媒体３０を備えている。ナビゲーションシステム１０は、記録媒体３０やＲＯＭに記憶されたプログラムを制御部２０で実行することができる。記録媒体３０には、予め地図情報３０ａおよびツリー構造データ３０ｂが記録されている。
【００１３】
地図情報３０ａは、交差点の位置の特定や、経路案内等に利用される情報であり、自車両が走行する道路上に設定されたノードの位置等を示すノードデータ，ノード間の道路の形状を特定するための形状補間点の位置等を示す形状補間点データ，ノード同士の連結を示すリンクデータ，道路やその周辺に存在する地物の位置等を示す地物データ等を含んでいる。なお、本実施形態においてノードは交差点を示している。また、リンクデータには、当該リンクデータが示す道路区間上の車線の数および幅を示す情報が対応づけられている。
【００１４】
また、本実施形態において、地図情報３０ａには自車両に対する規制を示す規制情報が含まれている。例えば、地図情報３０ａには、車線毎の制限速度、道路上の位置毎の禁止事項（車線変更禁止、右左折等の禁止、追い越しの禁止等）、同一の車線を双方向に通行可能な道路上での優先的な進行方向等を示す情報等が含まれている。規制は、各種の態様で定義され、例えば、路面上のペイントや標識等によって定義される。従って、規制情報は、位置や車線毎の規制の内容を示す情報であっても良いし、位置や車線毎のペイントの文字や標識の種類等を示す情報であっても良い。
【００１５】
ツリー構造データ３０ｂは、自車両および他車両が到達し得る位置と、各位置において自車両で実現し得る複数の状態とを示す情報である。本実施形態においては、既定の時間ステップ毎の自車両の位置が特定され、各位置における自車両の状態としての自車両の速度が特定されることによって自車両の軌道が取得される。
【００１６】
このように、本実施形態において軌道は位置および速度の推移で特定される。各位置において自車両および他車両で実現し得る速度（ベクトル）は複数個存在する。本実施形態においては、位置と速度との組み合わせの中で、実現可能な位置と速度の遷移をツリー構造によって示している。図２は、ツリー構造データ３０ｂによって特定可能なツリー構造を模式的に示す図である。図２においては、自車両の位置、速度、および他車両の位置、速度の組み合わせを白丸のノードで示しており、遷移可能なノードを矢印によって示している。また、図２においては、時間ステップをＴで示しており、初期状態をＴ＝０とし、以後、時間ステップが１増加するたびにＴ＝１，２と数値を増加させて時間ステップを示している。なお、時間ステップは、任意の長さであってよい。
【００１７】
図２は、初期状態Ｔ＝０において自車両が位置（Ｘ01，Ｙ01）に存在し、速度（Ｖx01，Ｖy01）であり、かつ、他車両が位置（Ｘ11，Ｙ11）に存在し、速度（Ｖx11，Ｖy11）である場合に遷移可能な位置および速度の組み合わせを示している。図２においては、当該初期状態における位置および速度の組み合わせを示すノードをノード１と呼ぶ。ツリー構造データ３０ｂにおいては、自車両および他車両の位置、速度の組み合わせがノードによって表現される。ここで、初期状態であるＴ＝０から１時間ステップ変化してＴ＝１となる過程で自車両が位置（Ｘ01，Ｙ01）から位置（Ｘ02，Ｙ02）に移動し、速度（Ｖx01，Ｖy01）から速度（Ｖx02，Ｖy02）に変化可能すると同時に、他車両が位置（Ｘ11，Ｙ11）から位置（Ｘ12，Ｙ12）に移動し、速度（Ｖx11，Ｖy11）から速度（Ｖx12，Ｖy12）に変化可能である場合、変化後の位置および速度がノード２となる。図２においては、初期状態のＴ＝０におけるノード１から遷移可能であることがノード１からノード２に延びる矢印によって示されている。ツリー構造は、このように、自車両および他車両において遷移可能な位置および速度の組み合わせの関係を示す。
【００１８】
なお、ツリー構造データ３０ｂは、複数の時間ステップに渡って遷移可能な位置および速度の組み合わせを特定できるように構成されていれば良く、本実施形態においては、１ステップ分のデータが保持され、複数の時間ステップに渡る遷移関係は軌道生成の際に都度取得される。ある位置および速度の組み合わせから他の位置および速度の組み合わせに遷移可能であるか否かを示す情報は予め作成されていれば良く、種々の手法で作成されて良い。
【００１９】
図３は、任意の位置および速度から遷移可能な位置および速度を示す図である。図３においては、車両の現在の位置を位置Ｐ0とし、位置Ｐ0の周囲における車両の移動先の候補位置を黒丸で示している。車両を一定速度で、または加減速して移動させることにより、車両は１回の時間ステップにおいて、破線で示す範囲内に移動可能である。すなわち、本実施形態においては、車両に作用する加速度および減速度の大きさの上限値が決められており、この範囲内の加速度で車両を移動させた場合に到達し得る位置が破線で示されている。
【００２０】
例えば、位置Ｐ0における進行方向に向けた速度が速度Ｖ0であり、１回の時間ステップ内において最大の大きさの減速度で減速しても位置Ｐ1を越えてしまう場合には、破線で示す範囲に位置Ｐ1が含まれない。この場合、ツリー構造データ３０ｂにおいて、位置Ｐ0、速度Ｖ0の車両が遷移可能な位置に位置Ｐ1は含まれない。また、位置Ｐ0における進行方向に向けた速度が速度Ｖ0であり、１回の時間ステップ内において最大の大きさの加速度で加速しても位置Ｐ2に到達できない場合には、破線で示す範囲に位置Ｐ2が含まれない。この場合、ツリー構造データ３０ｂにおいて、位置Ｐ0、速度Ｖ0の車両が遷移可能な位置に位置Ｐ2は含まれない。
【００２１】
さらに、位置Ｐ0における進行方向に向けた速度が速度Ｖ0であり、１回の時間ステップ内において最大の大きさ以下の減速度で減速すれば位置Ｐ3に到達する場合には、破線で示す範囲に位置Ｐ3が含まれる。この場合、ツリー構造データ３０ｂにおいて、位置Ｐ0、速度Ｖ0の車両が遷移可能な位置に位置Ｐ3が含まれる。さらに、位置Ｐ0における進行方向に向けた速度が速度Ｖ0であり、１回の時間ステップ内において最大の大きさ以下の加速度で加速すれば位置Ｐ4に到達する場合には、破線で示す範囲に位置Ｐ4が含まれる。この場合、ツリー構造データ３０ｂにおいて、位置Ｐ0、速度Ｖ0の車両が遷移可能な位置に位置Ｐ4が含まれる。
【００２２】
以上のように、加速度および減速度の大きさが上限値以内である場合に遷移可能な位置および速度を決める処理を、位置Ｐ0において想定し得る複数の速度について定義すれば、位置Ｐ0における各速度から遷移可能な位置および速度を網羅的に定義することができる。そこで、本実施形態におけるツリー構造データ３０ｂでは、１回の時間ステップで任意の位置および速度から遷移可能な位置および速度を対応づけている。すなわち、当該データに基づいて、初期状態のＴ＝０におけるノード１から遷移可能なノードを特定すれば、再度当該データを参照することにより、次の時間ステップで遷移可能なノードを特定可能である。
【００２３】
図４は、図３と同一の位置Ｐ0に自車両が存在し、速度Ｖ0である例において、位置Ｐ7に他車両が存在する場合の例を示している。また、図４においては次の時間ステップで他車両が遷移可能な範囲が二点鎖線で示されている。すなわち、図４に示す例においては、他車両の速度Ｖ7が自車両の速度Ｖ0よりも小さく、次の時間ステップで他車両が遷移可能な範囲は自車両より狭い。この例であれば、初期状態のＴ＝０におけるノード１は、自車両が位置Ｐ0、速度Ｖ0であり、他車両が位置Ｐ7、速度Ｖ7である状態を示す。この状態から、自車両において遷移可能な位置および速度と、他車両において遷移可能な位置および速度と、の組み合わせがＴ＝１におけるノードに該当する。
【００２４】
ツリー構造データ３０ｂは、１回の時間ステップで任意の位置および速度から遷移可能な位置および速度を対応づけているため、当該ツリー構造データ３０ｂに基づいて、図４に示す破線内に示す位置と各位置での自車両の速度と、二点鎖線内に示す位置と各位置での他車両の速度の組み合わせが網羅的に特定可能であり、特定されたノードがＴ＝１におけるノードとなる。このような処理をＴ＝１より後の時間ステップにおいても実行することで、図２に示すツリー構造が得られる。このため、ツリー構造データ３０ｂは、実質的には任意の時間ステップ後に遷移可能なノードの関係、すなわちツリー構造を示していると言える。
【００２５】
むろん、ツリー構造データ３０ｂのフォーマットは他のフォーマットであって良く、例えば、予測期間内の全時間ステップ分の遷移関係が予め特定され、ツリー構造データ３０ｂとして記録媒体３０に記録されていても良い。なお、図３に示す例は、ツリー構造データ３０ｂの概念を模式的に示す図であり、破線で示す範囲はより複雑に変化し得る。また、図３に示す例は遷移し得る位置を車両の前方に限定しているが、車両の速度が小さい場合や０である場合、負の加速度（後進）によって後方の位置に達し得るため、車両が後方に移動する軌道が考慮されても良い。
【００２６】
本実施形態における自車両は、図１に示すようにカメラ４０とＧＮＳＳ受信部４１と車速センサ４２とジャイロセンサ４３とユーザＩ／Ｆ部４４とを備えている。ＧＮＳＳ受信部４１は、Global Navigation Satellite Systemの信号を受信する装置であり、航法衛星からの電波を受信し、図示しないインタフェースを介して自車両の現在地を算出するための信号を出力する。制御部２０は、この信号を取得して自車両の現在地を取得する。車速センサ４２は、自車両が備える車輪の回転速度に対応した信号を出力する。制御部２０は、図示しないインタフェースを介してこの信号を取得し、車速を取得する。ジャイロセンサ４３は、自車両の水平面内の旋回についての角加速度を検出し、自車両の向きに対応した信号を出力する。制御部２０は、この信号を取得して自車両の進行方向を取得する。車速センサ４２およびジャイロセンサ４３等は、自車両の走行軌跡を特定するために利用され、本実施形態においては、自車両の出発地と走行軌跡とに基づいて現在地が特定され、当該出発地と走行軌跡とに基づいて特定された自車両の現在地がＧＮＳＳ受信部４１の出力信号に基づいて補正される。
【００２７】
カメラ４０は、自車両の前方に向けられた視野内の画像を取得する装置である。制御部２０は、当該カメラ４０の出力する画像を取得し、特徴量の抽出等によって画像を解析することによって自車両の周辺に存在し、接触を避けるべき物体（他車両、歩道、電柱等の障害物等）を検出することができる。
【００２８】
ユーザＩ／Ｆ部４４は、利用者の指示を入力し、また利用者に各種の情報を提供するためのインタフェース部であり、図示しないタッチパネル方式のディスプレイやスピーカー等の出力音の出力部を備えている。
【００２９】
制御部２０は、図示しないナビゲーションプログラムの機能により図示しないユーザＩ／Ｆ部４４の入力部を介して利用者による目的地の入力を受け付け、地図情報３０ａに基づいて自車両の現在地から目的地までの走行予定経路を探索する。また、制御部２０は、当該ナビゲーションプログラムの機能によりユーザＩ／Ｆ部４４を制御し、走行予定経路に沿って走行するための案内を実行する。本実施形態においては、当該ナビゲーションプログラムの付加機能として、自車両の前方において周囲の物体に接触せずに自車両を走行させるための予定軌道を生成し、表示する機能を実施することが可能であり、当該表示は予定軌道取得プログラム２１によって実現される。
【００３０】
予定軌道取得プログラム２１は、当該予定軌道の生成および表示を実現するため、自車両位置状態取得部２１ａと他車両位置状態取得部２１ｂとコスト取得部２１ｃと予定軌道取得部２１ｄと表示制御部２１ｅとを備えている。
【００３１】
自車両位置状態取得部２１ａは、予測期間内に自車両が達し得る各位置において自車両で実現し得る複数の状態を取得する機能を制御部２０に実行させるプログラムモジュールである。すなわち、制御部２０は、自車両位置状態取得部２１ａの機能により、ツリー構造データ３０ｂを参照し、自車両の現在地および速度から遷移可能な位置および速度を取得する。具体的には、制御部２０は、ＧＮＳＳ受信部４１，車速センサ４２，ジャイロセンサ４３の出力信号に基づいて自車両の現在地を取得する。また、制御部２０は、車速センサ４２，ジャイロセンサ４３等の出力に基づいて自車両の現在の速度を取得する。
【００３２】
他車両位置状態取得部２１ｂは、予測期間内に自車両以外の他車両が達し得る各位置において他車両で実現し得る複数の状態を取得する機能を制御部２０に実行させるプログラムモジュールである。すなわち、制御部２０は、他車両位置状態取得部２１ｂの機能により、ツリー構造データ３０ｂを参照し、他車両の現在地および速度から遷移可能な位置および速度を取得する。具体的には、制御部２０は、カメラ４０によって撮影された画像に対して画像処理を行い、他車両の特徴（例えば、路面から上に延びる黒いタイヤやナンバープレートの特徴等）を抽出する。むろん、他車両の抽出は種々の手法で実行されてよく、例えば、ＹＯＬＯ（You Only Look Once）等の人工知能を使って抽出しても良い。
【００３３】
他車両が検出されると、制御部２０は、他車両の位置を特定する。すなわち、本実施形態においては、カメラ４０が撮影した画像内の位置毎に当該位置において撮影された他車両とカメラ４０を搭載した自車両との相対位置関係（自車両からの距離および方向）が予め特定されている。そこで、制御部２０は、カメラ４０が撮影した画像内で抽出された各他車両における画像内での位置に基づいて、自車両と他車両との相対速度を取得する。なお、カメラ４０による撮影は既定のフレーム間隔で実行されるため、撮影された各画像に基づいて自車両から他車両までの距離を特定することにより、自車両と他車両との相対速度を取得することができる。
【００３４】
以上のようにして、自車両の位置および速度と、他車両の位置および速度とが取得されると、制御部２０は、自車両位置状態取得部２１ａおよび他車両位置状態取得部２１ｂの機能により、自車両の現在地および速度と、他車両の現在地および速度との組み合わせから、１回の時間ステップにおいて遷移可能な組み合わせを取得する。さらに、制御部２０は、取得された位置および速度から、次の時間ステップにおいて遷移可能な位置および速度の組み合わせを取得する処理を繰り返し、予測期間内に自車両および他車両が達し得る任意の位置と各位置において自車両および他車両で実現し得る速度を取得する。この結果、予測期間に渡る図２に示すような遷移関係が取得される。
【００３５】
コスト取得部２１ｃは、自車両が位置および状態を変化させる際のコストであって他車両が自車両と接触する可能性が高いほど大きくなる自車両コストと、他車両が位置および状態を変化させる際のコストであって他車両が自車両と接触する可能性が高いほど大きくなる他車両コストと、に対して優先度が高い車両の重み付けが相対的に小さい値である係数を乗じて加えた和を取得する機能を制御部２０に実行させるプログラムモジュールである。すなわち、制御部２０は、コスト取得部２１ｃにより、好ましくない位置および速度への遷移が多くなるほどコストが大きくなるように定義されたコストを取得する。
【００３６】
コストは、軌道として好ましい遷移であるか否かを示していれば良く、本実施形態においては、複数の要素によってコストが決められる。本実施形態におけるコストは、自車両からみて好ましくない位置を示す自車両コストと他車両からみて好ましくない位置を示す他車両コストとの和で規定される。自車両コストは、自車両が周辺の物体と接触する可能性が高いほど大きくなるコストと、自車両が遷移後の位置に達する際に自車両に作用する加速度が大きいほど大きくなるコストと、自車両が規制に反する場合に大きくなるコストとの和である。他車両コストは、他車両が周辺の物体と接触する可能性が高いほど大きくなるコストと、他車両が遷移後の位置に達する際に他車両に作用する加速度が大きいほど大きくなるコストと、他車両が規制に反する場合に大きくなるコストとの和である。
【００３７】
なお、自車両および他車両の周辺の物体には車両も含まれる。本実施形態において、自車両の周辺の物体は、カメラ４０によって撮影された画像に基づいて特定される。すなわち、制御部２０は、カメラ４０によって撮影された自車両の前方の画像に基づいて、自車両の周辺の予め決められた物体についての特徴量を抽出する。予め決められた物体は、自車両との衝突が防止されるべき物体であり、例えば、他車両、歩道、電柱等の障害物である。なお、他車両から見ると自車両も接触を防止すべき物体である。
【００３８】
制御部２０は、カメラ４０によって撮影された画像に対して画像処理を行い、上述の処理によって他車両の位置および自車両と他車両との相対速度を取得する。また、制御部２０は、当該画像処理に基づいて、歩道（例えば、自車両が走行する道路の幅方向の端部に道路方向に延びる直線等）を抽出する。さらに、制御部２０は、当該画像処理に基づいて、電柱（例えば、路面から鉛直上方に延びるほぼ平行な直線等）を抽出する。むろん、ここでも物体の抽出は種々の手法で実行されてよく、例えば、ＹＯＬＯ（You Only Look Once）等の人工知能を使って抽出しても良い。いずれにしても、各種の画像処理に基づいて、物体の特徴が抽出されると、制御部２０は、自車両の周辺に物体が存在すると特定する。
【００３９】
物体が特定されると、制御部２０は、物体の位置を特定する。すなわち、本実施形態においては、カメラ４０が撮影した画像内の位置毎に当該位置において撮影された物体とカメラ４０を搭載した自車両との相対位置関係（自車両からの距離および方向）が予め特定されている。そこで、制御部２０は、カメラ４０が撮影した画像内で抽出された各物体における画像内での位置に基づいて、自車両と各物体との相対速度を取得する。なお、カメラ４０による撮影は既定のフレーム間隔で実行されるため、撮影された各画像に基づいて自車両から物体までの距離を特定することにより、自車両と各物体との相対速度を取得することができる。
【００４０】
制御部２０は、自車両が現在の位置および速度から他の位置および速度に遷移した場合において、当該位置に他の物体が存在するか否かを相対速度に基づいて特定する。当該位置に他の物体が存在する場合、制御部２０は、自車両と接触する可能性が高いとみなす。
【００４１】
自車両と接触する可能性が高いほど大きくなるコストは、自車両が他の物体に接触する可能性に基づいて値が決められて良いが、本実施形態においては、任意の時間ステップにおいて、物体が存在する位置に遷移するためのコストが無限大（もしくは非常に大きい値）に設定される。すなわち、コストの観点からは、任意の時間ステップにおいて、物体が存在する位置に遷移できないように構成されている。
【００４２】
図４においては、自車両および他車両の周辺に存在する歩道を一点鎖線で模式的に示している。このように、自車両および他車両の周辺に歩道が存在する場合、歩道に相当する位置のコストは無限大になる。従って、図４に示す例においては一点鎖線内に存在する位置に遷移するためのコストが無限大になる。具体的には、一点鎖線で示す歩道により、位置Ｐ5，Ｐ6に遷移するためのコストが無限大になり、位置Ｐ0に存在する自車両は次の時間ステップにおいて位置Ｐ5，Ｐ6に移動し得ない。また、自車両の周辺に他車両が存在し、次の時間ステップにおいて他車両が移動し得る位置に自車両が移動し得る場合、その位置におけるコストは無限大になる。従って、自車両が遷移し得る破線で示された範囲と他車両が遷移し得る二点鎖線で示された範囲との双方に含まれる位置Ｐ9に自車両および他車両が遷移するためのコストが無限大になり、位置Ｐ0に存在する自車両は位置Ｐ9に移動し得ない。
【００４３】
このようなコストは、自車両コスト、他車両コストとなる。すなわち、自車両が位置Ｐ0に存在する場合、図４の破線で示される範囲外の位置は遷移可能な位置とされず、範囲内において他の物体と接触し得る位置（位置Ｐ5，Ｐ6および位置Ｐ9）が無限大となるように各位置への遷移に関する自車両コストが設定される。また、他車両が位置Ｐ7に存在する場合、図４の二点鎖線で示される範囲外の位置は遷移可能な位置とされず、範囲内において他の物体と接触し得る位置（位置Ｐ9）が無限大となるように各位置への遷移に関する他車両コストが設定される。
【００４４】
遷移後の位置に達する際に車両に作用する加速度が大きいほど大きくなるコストは、遷移前の位置および速度と、遷移後の位置および速度とに基づいて特定される。例えば、自車両が図４に示す位置Ｐ0から破線で示す範囲内の各位置に遷移する場合の距離と、両位置での速度を特定すると、運動方程式等に基づいて位置および速度の遷移に必要な加速度を特定することができる。他車両が図４に示す位置Ｐ7から二点鎖線で示す範囲内の各位置に遷移する場合の距離と、両位置での速度を特定すると、運動方程式等に基づいて位置および速度の遷移に必要な加速度を特定することができる。
【００４５】
そこで、制御部２０は、ツリー構造が示す遷移のために必要な加速度を自車両および他車両のそれぞれについて取得する。コストは加速度が大きいほど大きくなるように予め加速度毎のコストの値が決められている。制御部２０は、加速度に基づいて各遷移の際の自車両コストおよび他車両コストを取得する。
【００４６】
規制に反する場合に大きくなるコストは、規制に反する軌道が選択されないようにするために組み込まれるコストである。本実施形態において、制御部２０は、地図情報３０ａに含まれる規制情報を参照し、ツリー構造が示す遷移を行った場合の移動先の位置が規制に反する場合、当該位置に遷移するコストを無限大にする。例えば、移動先の位置が規制に反する場合とは、例えば、車線変更禁止区域での車線変更に相当する場合、右左折禁止交差点での右左折に相当する場合、追い越し禁止区域での追い越しに相当する場合等である。従って、自車両がある位置に遷移すると規制に反する場合、その位置に遷移する際の自車両コストが無限大に設定され、他車両がある位置に遷移すると規制に反する場合、その位置に遷移する際の他車両コストが無限大に設定される。
【００４７】
また、制御部２０は、地図情報３０ａに含まれる規制情報を参照し、ツリー構造が示す遷移を行った場合の遷移後の速度が規制に反する場合、当該速度に遷移するコストを無限大にする。なお、遷移後の速度が規制に反する場合とは、例えば、遷移後の速度が遷移後の位置での制限速度以上である場合等が該当する。従って、自車両がある速度に遷移すると規制に反する場合、その速度に遷移する際の自車両コストが無限大に設定され、他車両がある速度に遷移すると規制に反する場合、その速度に遷移する際の他車両コストが無限大に設定される。
【００４８】
自車両コストと他車両コストとが設定されると、制御部２０は、自車両コストと他車両コストの和が遷移のコストであるとみなす。ただし、本実施形態においては、自車両コストと他車両コストとのそれぞれに重み係数が乗じられる。すなわち、ある遷移のコストＣは、自車両コストＣｔ1、他車両コストＣｔ2に基づいて、以下のように表現される。
Ｃ＝ｋ1×Ｃｔ1＋ｋ2×Ｃｔ2
ただし、ｋ1，ｋ2のそれぞれは、自車両コストの重み係数、他車両コストの重み係数である。
【００４９】
そして、本実施形態において重み付け係数の大きさは、自車両と他車両の優先度に基づいて決められ、優先度が高い車両のコストの重み付け係数が相対的に小さい値になるように決められている。すなわち、優先度が高い車両は当該車両が主体となって（他に道を譲ることなく）軌道を決定して良く、優先度が低い車両は優先度が高い車両の動作に合わせて（道を譲って）軌道を決定する必要がある。例えば、車両が道路を走行する際、自車両と他車両との相対関係によっていずれか一方が優先されることが交通規則によって決められている場合がある。図５Ｂ、図５Ｃはこのような例を示している。図５Ｂ、図５Ｃにおいて、道路Ｒは双方向通行道路であり、自車両Ｃ0が走行中の道路の幅は車両１台分程度であり、他車両Ｃ1が走行中の道路の幅は車両２台分程度である。このような道路において、互いに逆向きに自車両Ｃ0と他車両Ｃ1が走行している場合、幅が細い部分を走行している車両が優先される車両になるように交通規則が決められている場合がある。
【００５０】
この場合、他車両Ｃ1は自車両Ｃ0に道を譲り、自車両Ｃ0が通過するのを待たなければならない。従って、この例において他車両Ｃ1が自車両Ｃ0に道を譲り、自車両Ｃ0が先に直進する軌道は現実的であるが、自車両Ｃ0が他車両Ｃ1に道を譲る軌道は現実的ではない。従って、自車両の軌道である予定軌道を探索する際に、自車両と他車両の優先度が考慮されていなければ、非現実的な軌道が探索されてしまう。
【００５１】
そこで、本実施形態において制御部２０は、地図情報３０ａが示す規制情報を参照し、自車両および他車両の前方において、自車両と他車両との相対関係によって優先度に差異が生じるような交通規則（同一の車線を双方向に通行可能な道路上での優先的な進行方向等を示す情報）が存在するか否か判定する。自車両と他車両との相対関係によって優先度に差異が生じるような交通規則が存在する場合、制御部２０は、当該交通規則に基づいて、自車両と他車両の優先度を特定する。なお、他の交通規則（例えば、速度制限等）は、規則に違反しないようにするためのコストを特定するために利用され、重み付けは、自車両と他車両との相対関係によって優先度に差異が生じるような交通規則に基づいて行われる。
【００５２】
制御部２０は、自車両と他車両の優先度に基づいて、優先度が高い車両のコストの重み付け係数が相対的に小さい値になるように重み付け係数を決定する。重み付け係数の値は予め決められていれば良く、固定の大きさであっても良いし、優先度の強度に基づいて変化しても良い。重み付け係数が決定されると、制御部２０は、自車両コストと他車両コストのそれぞれに重み付け係数を乗じて各遷移のコストを決定する。図２においては、各ノード間での遷移を実線の矢印で示しており、当該実線の矢印にコストを付記している。例えば、ノード１からノード２への遷移はコストが１、ノード２からノード５への遷移はコストが５である。本実施形態においては、当該コスト１やコスト５が重み付け係数を乗じられた自車両コストと他車両コストとの和である。
【００５３】
予定軌道取得部２１ｄは、自車両が移動すべき複数の位置および各位置における状態によって規定される軌道であって、当該軌道に含まれる位置のそれぞれにおけるコストの和の累積値が最小である軌道を予定軌道として取得する機能を制御部２０に実行させるプログラムモジュールである。すなわち、制御部２０は、ツリー構造データ３０ｂに基づいて、現在以後、既定期間（複数の時間ステップ）以内に遷移可能なノードの順列を特定し、順列に従って到達される複数の位置および各位置における状態が軌道であるとみなす。さらに、制御部２０は、ノードの順列毎にコストの和の累積値を取得し、最小のものを予定軌道として取得する。
【００５４】
例えば、図２に示す例においては、２回の時間ステップ後（Ｔ＝２）でのコストの和の累積値を各ノードについて累積コストとして示している。この例におけるノード１，２，５という順列の遷移においては、ノード１からノード２への遷移における自車両コストと他車両コストの和が１、ノード２からノード５への遷移における自車両コストと他車両コストの和が５である。そこで、制御部２０は、ノード１，２，５という順列の遷移のコストの和の累積値が６と取得する。制御部２０は、予測期間に相当する長さの時間ステップにおいてコストの和の累積値が最も小さい順列を取得し、当該順列で位置および速度が変化する軌道を予測軌道とみなす。なお、コストの和の累積値が最も小さい順列を探索するための手法は、種々の手法を採用可能であり、例えば、動的計画法等を採用可能である。
【００５５】
表示制御部２１ｅは、予定軌道を風景に重畳させて表示する機能を制御部２０に実行させるプログラムモジュールである。すなわち、本実施形態においては、自車両の周辺に存在する他車両を含む物体に接触せず、加速度が小さく（自車両の挙動が安定しており）、規制に適合し、車両同士の優先関係からみて現実的な軌道を予定軌道として表示させることで、利用者に対して好ましい軌道を認識させるように構成されている。
【００５６】
具体的には、制御部２０は、カメラ４０で撮影した画像をユーザＩ／Ｆ部４４のディスプレイに表示させる。この結果、ユーザＩ／Ｆ部４４のディスプレイに自車両の前方の風景が表示された状態になる。さらに、制御部２０は、当該風景に対して予定軌道を示す画像を重畳して表示する。すなわち、本実施形態においては、カメラ４０が撮影した画像内の位置毎に当該位置において撮影された物体とカメラ４０を搭載した自車両との相対位置関係が予め特定されている。
【００５７】
そこで、制御部２０は、予定軌道上の位置と自車両との相対関係に基づいて、各位置を通る軌道を示す矢印をディスプレイ上に描画する。図５Ａは、自車両前方の風景に重畳された予定軌道Ｏｒを模式的に示している。なお、図５Ａに示す例において、予定軌道Ｏｒは予定軌道に沿って走行するために必要な位置を示しているが、むろん、位置毎の速度が各種態様で示されてもよい。
【００５８】
以上の構成によれば、位置に加えて、自車両の状態としての速度が異なる場合も考慮して軌道を最適化することができ、自車両において選択し得る軌道を網羅的に考慮することができる。図５Ｂおよび図５Ｃは、自車両Ｃ0の前方に対向車両としての他車両Ｃ1が走行している例を示している。図５Ｂおよび図５Ｃに示す例においては、自車両Ｃ0の方が他車両Ｃ1よりも優先であるため、自車両Ｃ0が道路を直進する軌道が予定軌道として生成される。
【００５９】
通常は、図５Ｂに破線の矢印で示されるように、道路の優先関係に従って他車両Ｃ1が道を譲って破線で示されるように遷移したり、図５Ｃに破線の矢印で示されるように、道路の優先関係に従って他車両Ｃ1が後進して道を譲るように遷移したりする。従って、自車両Ｃ0は予定軌道に従って走行すれば、優先関係に適合した軌道で道路を走行することができる。
【００６０】
このように、本実施形態においては、車両同士の優先関係が考慮されて予定軌道が生成されるため、他車両Ｃ1のみの動作を主にして自車両Ｃ0の軌道が生成されることはない。従って、優先度が高い自車両Ｃ0が後進して他車両Ｃ1に道を譲る軌道などの非現実的な軌道が予定軌道になることはなく、現実的な軌道を予定軌道にすることができる。なお、万一、他車両が優先関係を無視して自車両Ｃ0に道を譲らなかったとしても、自車両Ｃ0と他車両Ｃ1とが接近すると、やがて、自車両Ｃ0が他車両Ｃ1に接触しない軌道が予定軌道として生成される状態になる。
【００６１】
（２）予定軌道取得処理：
次に、図６に示すフローチャートに基づいて制御部２０が実行する予定軌道取得処理を説明する。ナビゲーションシステム１０が起動されると、制御部２０は、一定期間毎（例えば、１００ｍｓ毎）に予定軌道取得処理を実行する。予定軌道取得処理が実行されると、制御部２０は、自車両位置状態取得部２１ａの機能により、ツリー構造を取得する（ステップＳ１００）。すなわち、制御部２０は、ＧＮＳＳ受信部４１、車速センサ４２、ジャイロセンサ４３の出力信号に基づいて、自車両の現在地および自車両の現在の速度を取得する。
【００６２】
また、制御部２０は、カメラ４０の出力画像に基づいて、自車両の前方の他車両の現在地および他車両の現在の速度を取得する。そして、制御部２０は、ツリー構造データ３０ｂを参照し、自車両の現在の位置および現在の速度と、他車両の現在の位置および現在の速度との組み合わせから１回の時間ステップで遷移可能な他の組み合わせを取得する。次に、制御部２０は、当該時間ステップにおける位置および速度の組み合わせから遷移可能な位置および速度の組み合わせを取得する。制御部２０は、以上の処理を繰り返し、予め決められた期間である予測期間内に遷移可能な位置および速度の組み合わせの遷移関係を取得する。この結果、図２に示すようなツリー構造が取得される。
【００６３】
次に、制御部２０は、コスト取得部２１ｃの機能により、コストを取得する（ステップＳ１０５）。すなわち、制御部２０は、カメラ４０の出力画像に基づいて、自車両の周辺に存在し、接触を避けるべき物体（他車両を含む）の位置を検出する。さらに、制御部２０は、当該物体と自車両との相対速度を取得し、当該物体が存在する位置への遷移がステップＳ１００で取得された遷移に含まれる場合、当該遷移における自車両コストに対して無限大のコストを対応づける。また、制御部２０は、他車両から見た物体（自車両を含む）の位置と相対速度とを取得し、当該物体が存在する位置への遷移がステップＳ１００で取得された遷移に含まれる場合、当該遷移における他車両コストに対して無限大のコストを対応づける。
【００６４】
また、制御部２０は、ステップＳ１００で取得した各位置および速度の組み合わせから遷移可能な位置および速度の組み合わせに遷移するために必要な加速度を、自車両および他車両のそれぞれで取得し、加速度に応じた大きさのコストを、当該遷移の自車両コストおよび他車両コストとして取得する。さらに、制御部２０は、地図情報３０ａに含まれる規制情報を参照し、ステップＳ１００で取得した位置および速度の組み合わせから遷移可能な位置および速度の組み合わせに遷移する場合に規制に反するか否か判定する。そして、自車両において規制に反する位置または速度への遷移が行われる場合には、その遷移に対して無限大の自車両コストを対応づける。また、他車両において規制に反する位置または速度への遷移が行われる場合には、その遷移に対して無限大の他車両コストを対応づける。
【００６５】
以上の処理により、各遷移について自車両コストと他車両コストとが算出されている。そこで、制御部２０は、地図情報３０ａの規制情報を参照し、自車両と他車両との相対関係によって優先度に差異が生じるような交通規則が存在するか否か判定する。当該交通規則が存在する場合、制御部２０は、優先度が高い方の車両の重み係数を相対的に小さく、優先度が低い方の車両の重み係数を相対的に大きくする。そして、制御部２０は、
Ｃ＝ｋ1×Ｃｔ1＋ｋ2×Ｃｔ2
の式によって自車両コストと他車両コストとを重み付けして足すことで各遷移のコストの和を取得する。この結果、図２に示すツリー構造において実線で示す矢印に対応づけられたように、遷移毎のコストが算出された状態となる。
【００６６】
次に、制御部２０は、予定軌道取得部２１ｄの機能により、現在ノードテーブル、直線ノードテーブル、累積コストテーブルを作成する（ステップＳ１１０）。各テーブルはコストの和の累積値を算出する過程で参照される変数を格納するためのテーブルであり、累積コストテーブルは、ノードの順列に従って遷移した場合のコストの和の累積値を格納するためのテーブルである。現在ノードテーブルは、順列の最後に存在するノードを示す情報（本例ではノードの番号）を格納するためのテーブルである。直前ノードテーブルは、順列の最後に存在するノードに遷移する直前のノードを示す情報（本例ではノードの番号）を格納するためのテーブルである。
【００６７】
例えば、図２においては、２回の時間ステップＴ＝２で到達する各ノードについて累積コストテーブルに格納された値を累積コストとして示し、現在ノードテーブルに格納された値を現在ノードとして示し、直前ノードテーブルに格納された値を直前ノードとして示している。なお、各テーブルは互いに関連づけられており、現在ノードテーブルの値に対して累積コストテーブルの値と直前ノードテーブルの値が対応づけられる。例えば、図２に示す例において、ノード１，２，５の遷移を考えると、累積コストは６であり、最後のノードは５，最後のノードの直前のノードは２である。この場合、値が５であるデータが作成されて現在ノードテーブルに格納される。さらに、値６のデータが作成されて累積コストテーブルに格納されて現在ノードテーブルの値５に対応づけられ、値２のデータが作成されて直前ノードテーブルに格納されて現在ノードテーブルの値５に対応づけられる。ステップＳ１１０において、制御部２０は、このようなデータを格納するためのデータを作成して初期化する。
【００６８】
次に、制御部２０は、予定軌道取得部２１ｄの機能により、時間ステップを示す変数Ｔを０に初期化する（ステップＳ１１５）。次に、制御部２０は、予定軌道取得部２１ｄの機能により、変数Ｔにおける軌道候補を探索する（ステップＳ１２０：詳細は後述）。そして、制御部２０は、予定軌道取得部２１ｄの機能によって変数Ｔを１回の時間ステップ分進行させ（Ｔ＝Ｔ＋ΔＴ）、変数Ｔが最大値Ｔｍａｘを超えるまでステップＳ１２０，Ｓ１２５を繰り返す。この結果、変数Ｔが最大値Ｔｍａｘとなるまでの期間の軌道候補（遷移可能な順列）が特定される。
【００６９】
次に、制御部２０は、予定軌道取得部２１ｄの機能により、コストの和の累積値が最小の軌道を予定軌道として取得する（ステップＳ１３５）。すなわち、制御部２０は、ステップＳ１２０〜Ｓ１３０のループを経て生成されたＴｍａｘまでの軌道の中で、累積コストテーブルに格納された値が最小の軌道を予定軌道として取得する。
【００７０】
次に、制御部２０は、表示制御部２１ｅの機能により、予定軌道を表示する（ステップＳ１４０）。すなわち、制御部２０は、ステップＳ１３５で取得された予定軌道をカメラ４０の撮影画像に重畳して表示する画像を描画し、ユーザＩ／Ｆ部４４のディスプレイを制御して予定軌道を表示させる。
【００７１】
（３）軌道候補探索処理：
図７は、ステップＳ１２０で実行される軌道候補探索処理を示すフローチャートである。ここでは、効率的な探索を行うためにビームサーチを行う例を説明するが、むろん、充分なリソースを利用可能である場合、ビームサーチが行われず全ての候補について探索されても良い。また、他の方法で候補を絞っても良い。
【００７２】
軌道候補探索処理が開始されると、制御部２０は、変数Ｔが０であるか否かを判定する（ステップＳ２００）。すなわち、制御部２０は、処理対象が初期ノードであるか否かを判定する。例えば、図２に示す例において、処理対象のノードがＴ＝０におけるノード１であれば、制御部２０は、ステップＳ２００において変数Ｔが０であると判定する。
【００７３】
ステップＳ２００において、変数Ｔが０であると判定された場合、制御部２０は、ビームサーチによってコストの低いノードを抽出し、累積コストテーブルを更新する（ステップＳ２０５）。ビームサーチは採用し得る候補の中から採用される可能性が高い上位の候補のみを抽出した探索を行うアルゴリズムであり、本実施形態においては、遷移のコストが小さい上位のＮ個（Ｎは２以上、ノード数以下の整数）が抽出される。
【００７４】
図２に示す例において変数Ｔ＝０のノード１が処理対象である場合、遷移のコスト（自車両コストと他車両コストとの重み付け和）はＴ＝１のノード２，３，４のそれぞれに遷移する際のコストである。図２においては、ビームサーチにおいてノードが抽出される個数Ｎが３の例が想定されている。従って、ステップＳ２０５においては、ノード２，３，４が抽出されて累積コストテーブルが更新される。すなわち、制御部２０は、ノード１からノード２，３，４のそれぞれに遷移する際のコストを取得してコストの和の累積値とする。例えば、ノード２に遷移する際のコストは１，ノード３に遷移する際のコストは２である。
【００７５】
次に、制御部２０は、現在ノードテーブルを更新し（ステップＳ２１０）、直前ノードテーブルを更新する（ステップＳ２１５）。すなわち、制御部２０は、Ｔ＝０の次の時間ステップであるＴ＝１において遷移可能なノードのそれぞれを現在ノードとした場合に、各現在ノードに累積コストと直前ノードとを対応づける。例えば、図２に示す例においては、ノード２，３，４のそれぞれについてテーブルの更新が実施され、現在ノードがノード２である場合、現在ノードテーブルに値２が格納され、現在ノードテーブルの値２に対して累積コスト１と直前ノード１とが対応づけられる。現在ノードがノード３である場合、現在ノードテーブルに値３が格納され、現在ノードテーブルの値３に対して累積コスト２と直前ノード１とが対応づけられる。
【００７６】
一方、ステップＳ２００において、変数Ｔが０であると判定されない場合、制御部２０は、処理対象の現在ノードを設定する（ステップＳ２２０）。すなわち、変数Ｔが０でない場合、変数Ｔ−１から変数Ｔへの時間ステップで遷移し得るノードは複数個存在する。例えば、図２に示す例においてＴ＝１であれば、Ｔ＝０から遷移し得るノードのノード数は３個（ノード２，３，４）である。制御部２０は、このように複数個存在し得るノードの中から、ステップＳ２２０〜Ｓ２３０のループ処理の対象になっていないノードを１個選択し、処理対象に設定する。
【００７７】
次に、制御部２０は、処理対象の現在ノードから遷移可能なノードへのコストの和を累積コストに加算する（ステップＳ２２５）。すなわち、制御部２０は、変数Ｔの次の時間ステップにかけて処理対象の現在ノードから遷移可能なノードを特定する。例えば、図２に示す例において、処理対象のノードがノード２であれば、制御部２０は、ノード５，６，７のそれぞれに対して遷移する際のコストの和５，６，１０を取得する。さらに、制御部２０は、各コストの和を累積コストに加算する。例えば、ノード５，６，７のそれぞれであれば、累積コストは６，７，１１となる。処理対象のノードがノード３であれば、制御部２０は、ノード８，９のそれぞれに対して遷移する際のコストの和４，３を取得する。さらに、制御部２０は、各コストの和を累積コストに加算する。例えば、ノード８，９であれば、累積コストは６，５となる。
【００７８】
次に、制御部２０は、全てのノードについて終了したか否かを判定する（ステップＳ２３０）。すなわち、制御部２０は、変数Ｔ−１から変数Ｔへの時間ステップで遷移し得るノードの全てを処理対象としてステップＳ２２０〜Ｓ２３０のループ処理が終了したか否かを判定する。例えば、図２に示す例においてＴ＝１であれば、ノード２，３，４のそれぞれを処理対象としてステップＳ２２０〜Ｓ２３０のループ処理を行ったか否かを判定する。
【００７９】
ステップＳ２３０において、全てのノードについて終了したと判定されない場合、制御部２０は、ステップＳ２２０以降の処理を繰り返す。一方、ステップＳ２３０において、全てのノードについて終了したと判定された場合、制御部２０は、ビームサーチによってコストの低いノードを抽出し、累積コストテーブルを更新する（ステップＳ２３５）。ここでも、遷移のコストが小さい上位のＮ個（Ｎは２以上、ノード数以下の整数）が抽出される。
【００８０】
図２に示す例において変数Ｔ＝１である場合、制御部２０は、ノード２，３，４から遷移し得るノード５〜１０のそれぞれについて遷移する際のコストを取得する。すなわち、制御部２０は、ノード５，６，７，８，９，１０のそれぞれに遷移する際のコストを５，６，１０，４，３，３と取得する。ここでも、ビームサーチにおいてノードが抽出される個数Ｎが３の例を想定すると、上位３個のコストは３，３，４であり、それぞれのコストで遷移し得るノード９，１０，８が抽出される。
【００８１】
図８においては、抽出後のノードが示されている。ノードが抽出されると、ステップＳ２２５で算出された累積コストに基づいて各ノードについての累積コストテーブルが更新される。例えば、ノード８が抽出された場合、制御部２０は、ノード３からノード８に遷移する際のコストを取得してコストの和の累積値６を取得する。
【００８２】
次に、制御部２０は、現在ノードテーブルを更新し（ステップＳ２４０）、直前ノードテーブルを更新する（ステップＳ２４５）。例えば、図２および図７に示す例において変数Ｔ＝１である場合、Ｔ＝１の次の時間ステップであるＴ＝２において遷移可能なノード８，９，１０のそれぞれについてテーブルの更新が実施される。すなわち、現在ノードがノード８である場合、現在ノードテーブルに値８が格納され、現在ノードテーブルの値８に対して累積コスト６と直前ノード３とが対応づけられる。現在ノードがノード１０である場合、現在ノードテーブルに値１０が格納され、現在ノードテーブルの値１０に対して累積コスト４と直前ノード４とが対応づけられる。
【００８３】
制御部２０は、ステップＳ１２０において、以上のようにして変数Ｔについての軌道候補を探索する。ステップＳ１２０が終了して図５に示すフローチャートに戻ると、制御部２０は、ステップＳ１２０〜Ｓ１３０のループ処理を実行することによって変数Ｔｍａｘまで軌道候補を探索する。図８においては、Ｔｍａｘが２である場合の例が示されている。すなわち、変数Ｔ＝２である場合にステップＳ２２０〜Ｓ２３０が行われると、ノード８，９，１０から遷移可能なノード１１〜１５のそれぞれに遷移する際のコストの和の累積値が特定される。
【００８４】
さらに、ステップＳ２３５が実行されると、ビームサーチによって遷移する際のコストが６，２，３であるノード１１，１２，１３への遷移が抽出され、抽出されたノードについて累積コストテーブル、現在ノードテーブル、直前ノードテーブルが更新される。図８においては、それぞれの値の例が示されている。この状態で図５の処理に戻ると、制御部２０は、ステップＳ１３０の判定を経てステップＳ１３５を実行する。図８に示す例であれば、ノード１１，１２，１３への遷移が候補として残っているが、累積コストが最小の遷移は累積コストの値が８であるノード１，３，８，１２という遷移である。従って、制御部２０は、ノード１，３，８，１２の順列で特定される軌道を予定軌道として取得する。以上の構成によれば、高速に最小のコストになる軌道を探索することができる。
【００８５】
（４）他の実施形態：
以上の実施形態は本発明を実施するための一例であり、他にも種々の実施形態を採用可能である。例えば、予定軌道取得システムは、自車両等に搭載された装置であっても良いし、可搬型の端末によって実現される装置であっても良いし、複数の装置（例えば、クライアントとサーバや、ナビゲーション装置内の制御部とユーザＩ／Ｆ部内の制御部等）によって実現されるシステムであっても良い。むろん、予定軌道取得システムが他車両に搭載されていても良い。
【００８６】
予定軌道取得システムを構成する自車両位置状態取得部２１ａと他車両位置状態取得部２１ｂとコスト取得部２１ｃと予定軌道取得部２１ｄと表示制御部２１ｅとの少なくとも一部が複数の装置に分かれて存在しても良い。むろん、上述の実施形態の一部の構成が省略されてもよいし、処理の順序が変動または省略されてもよい。
【００８７】
例えば、上述の実施形態において、コストは、車両と接触する可能性が高いほど大きくなるコストと、遷移後の位置に達する際に車両に作用する加速度が大きいほど大きくなるコストと、規制に反する場合に大きくなるコストの和であったが、これらのコストの少なくとも１個に基づいて軌道が特定されても良い。さらに、予定軌道は、前方に向けて走行する軌道に限定されず、後方に後進する軌道が候補となっても良い。
【００８８】
自車両位置状態取得部は、予測期間内に自車両が達し得る各位置において自車両で実現し得る複数の状態を取得することができればよい。すなわち、自車両位置状態取得部は、予測開始時刻から予測終了時刻までの予測期間において自車両が達し得る位置と各位置での状態の組み合わせを取得する。予測期間は、種々の長さであって良く、予定軌道の用途等によって変動して良い。
【００８９】
自車両が達し得る位置は、予定軌道を構成する位置の候補となり得る位置であればよく、連続的に存在しても良いし、離散的に存在しても良い。自車両で実現し得る状態は、軌道の選び方や各位置に達する過程が異なることによって異なり得る要素であれば良く、速度以外にも、加速度や進行方向等が挙げられる。すなわち、予定軌道として最適化すべき自車両の状態を自車両位置状態取得部によって取得すれば、位置とともに状態を最適化した予定軌道を生成することができる。
【００９０】
他車両位置状態取得部は、予測期間内に他車両が達し得る各位置において他車両で実現し得る複数の状態を取得することができればよい。すなわち、他車両位置状態取得部は、自車両位置状態取得部と同様の情報を他車両について取得することができればよい。自車両における位置毎の状態と他車両における位置毎の状態は同種であっても良いし、異種であっても良い。例えば、自車両の状態、他車両の状態がともに速度である構成であっても良いし、一方が速度、他方が加速度である構成であっても良い。なお、自車両は予定軌道取得システムが利用される車両であり、他車両は当該自車両の周辺に存在する自車両以外の車両である。むろん、他車両は複数台存在しても良い。
【００９１】
コスト取得部は、自車両が位置および状態を変化させる際のコストであって他車両が自車両と接触する可能性が高いほど大きくなる自車両コストと、他車両が位置および状態を変化させる際のコストであって他車両が自車両と接触する可能性が高いほど大きくなる他車両コストと、に対して優先度が高い車両の重み付けが相対的に小さい値である係数を乗じて加えた和を取得することができればよい。すなわち、自車両コストは、自車両の軌道として好ましいか否かを測る指標であり、コストが小さい程好ましい軌道であることを示す。また、他車両コストは、他車両の軌道として好ましいか否かを測る指標であり、コストが小さい程好ましい軌道であることを示す。
【００９２】
従って、自車両コストは、自車両の位置の変化に伴って発生するコストの累積値が最小化されることによって自車両の軌道として最も好ましい軌道が生成されるように定義されていれば良い。また、他車両コストは、他車両の位置の変化に伴って発生するコストの累積値が最小化されることによって他車両の軌道として最も好ましい軌道が生成されるように定義されていれば良い。
【００９３】
自車両コストおよび他車両コストは、少なくとも、自車両と他車両とが接触する可能性が高いほど大きくなるように、すなわち、自車両が他車両と接触しない可能性が高い軌道が自車両の軌道になり、他車両が自車両と接触しない可能性が高い軌道が他車両の軌道になるように定義されていれば良い。このため、他車両と接触し得る位置に到達し得る自車両コストは他の位置と比較して大きく、自車両と接触し得る位置に到達し得る他車両コストは他の位置と比較して大きい。
【００９４】
むろん、他車両と接触する位置が自車両位置状態取得部で取得されない構成や、自車両と接触する位置が他車両位置状態取得部で取得されない構成も採用し得るが、これらの構成は、接触し得る位置に移動する際のコストが無限大であることと等価である。位置毎のコストは段階的に変化しても良いし、連続的に変化しても良い。むろん、車両以外の物体（例えば、歩道や電柱等）についてのコストが取得され、自車両や他車両が他の物体に接触しないように軌道が取得されても良い。
【００９５】
コストは、種々の要素によって決められて良く、車両同士の接触を防止するためのコスト以外にも種々の要素でコストが決められて良い。例えば、自車両や他車両に作用する加速度、自車両や他車両に対する規制によってコストが決められても良いし、自車両や他車両の位置や車速、移動距離、ヨーレート、ブレーキ開度、アクセル開度などによってコストが決められても良い。さらに、自車両周辺の物体の位置を特定するための構成はカメラ４０の撮影画像の解析が利用される構成に限定されない。例えば、地図情報３０ａに歩道や電柱の情報等が含まれており、当該情報に基づいて自車両の周辺の物体の位置が特定されても良い。
【００９６】
優先度は、自車両と他車両との相対的な関係であり、優先度が高い車両は低い車両と比較して主体的に軌道を選択可能な自由度が高い。優先度が高い車両は、上述のように交通規則によって優先される車両である構成に限定されない。例えば、自車両と他車両との動作（速度や加速度等）に基づいて、車両の優先度が決められても良い。具体的には、速度が速い車両の方が優先される構成等であっても良い。また、車両内の人物の動作に基づいて、車両の優先度が決められても良い。具体的には、車両内の人物が道を譲るジェスチャーをした場合、ジェスチャーされた車両の優先度が高く、当該人物が乗る車両の優先度が低いとみなされる構成等が想定される。この構成は、例えば、自車両や予定軌道取得システムが備えるカメラで撮影された画像の画像処理に基づいてジェスチャーを特定するなどによって実現可能である。
【００９７】
重み付けは、優先度が高い車両の軌道が主導的に特定され、優先度が低い車両の軌道が従属的に特定されるように定義できれば良く、種々の手法で定義されて良い。また、重み付けの値は予め決められた値に限定されず、例えば、優先度の強度等に応じて重み付けの値が決められても良い。
【００９８】
予定軌道取得部は、自車両が達し得る複数の位置および各位置における自車両の状態によって規定される軌道であって、当該軌道に含まれる位置のそれぞれにおける和の累積値が最小である軌道を自車両の予定軌道として取得することができればよい。すなわち、予定軌道取得部は、自車両の位置と当該位置における状態との組み合わせであって、連続的に到達し得る位置と当該位置での状態の順列を自車両の軌道とする。各位置における状態は複数個存在し得るため、軌道を構成する位置が同一であっても状態は異なり得る。この状況で、予定軌道取得部は、位置と状態で規定される軌道の中から自車両コストと他車両コストの和の累積値が最小である軌道を自車両の予定軌道として取得する。すなわち、コストの和には他車両コストが含まれるため、他車両の動作も考慮された状態で自車両の軌道が最適化される。
【００９９】
コストの和の累積値が最小である軌道を探索する手法は、種々の手法を採用可能である。すなわち、上述の実施形態においては、動的計画法によってコストの和の累積値が最小である軌道を探索したが、むろん、他の手法、例えば、機械学習による探索等が行われてもよい。また、ツリー構造のデータからコストの和の累積値が最小である軌道を検索する手法もビームサーチに限定されず、各種の検索が行われて良い。リソースが充分である場合には総当たりで検索が行われてもよい。
【０１００】
予定軌道は、種々の目的で利用されて良い。すなわち、上述の実施形態のように、予定軌道がディスプレイに表示されるシステムが構成されても良いし、予定軌道に基づいて自車両が自動運転されるシステムが構成されても良く、他にも種々の構成を採用可能である。
【０１０１】
さらに、本発明のように、自車両と他車両について位置および状態の組み合わせとして取り得る組み合わせを探索する際に車両の優先度を考慮することで予定軌道を取得する手法は、プログラムや方法としても適用可能である。また、以上のような予定軌道取得システム、プログラム、方法は、単独の装置として実現される場合もあれば、自車両に備えられる各部と共有の部品を利用して実現される場合もあり、各種の態様を含むものである。さらに、以上のような予定軌道取得システムの少なくとも一部を備えたナビゲーション装置や方法、プログラムを提供することが可能である。また、一部がソフトウェアであり一部がハードウェアであったりするなど、適宜、変更可能である。さらに、予定軌道取得システムを制御するプログラムの記録媒体としても発明は成立する。むろん、そのソフトウェアの記録媒体は、磁気記録媒体であってもよいし半導体メモリであってもよいし、今後開発されるいかなる記録媒体においても全く同様に考えることができる。
【符号の説明】
【０１０２】
１０…ナビゲーションシステム、２０…制御部、２１…予定軌道取得プログラム、２１ａ…自車両位置状態取得部、２１ｂ…他車両位置状態取得部、２１ｃ…コスト取得部、２１ｄ…予定軌道取得部、２１ｅ…表示制御部、３０…記録媒体、３０ａ…地図情報、３０ｂ…ツリー構造データ、４０…カメラ、４１…ＧＮＳＳ受信部、４２…車速センサ、４３…ジャイロセンサ、４４…ユーザＩ／Ｆ部
【図１】
2020139774.tif 000003
【図２】
2020139774.tif 000004
【図３】
2020139774.tif 000005
【図４】
2020139774.tif 000006
【図５】
2020139774.tif 000007
【図６】
2020139774.tif 000008
【図７】
2020139774.tif 000009
【図８】
2020139774.tif 000010