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