JP6603197B2 - 連続値最適化問題の非線形最適化プログラム、経路探索プログラム、及び経路探索装置 - Google Patents

Description

本発明は、連続値最適化問題の非線形最適化プログラム、経路探索プログラム、及び経路探索装置に関する。

近年、例えば、運転支援装置が事故防止対応のために開発されており、特にドライバーへの運転負担を軽減するために開発されている。この種の運転支援装置が経路探索するときには、この経路の位置及び速度を時間に応じた関数とし、これらの位置及び速度に応じた関数に基づく評価関数を導入し、この評価関数の極値（例えば最小値）を満たす位置及び速度を求めることで、最適な経路を探索できることが発明者らにより確認されている。

なお、本願に関連する技術が特許文献１に開示されている。この特許文献１には、評価関数と制約条件とを独立に扱い、評価関数を極小化するための変数更新処理と制約条件を満足させるための変数更新処理とを分け、これらの２つの更新処理を交互に実行することにより、制約条件付きの評価関数を極小化する技術が開示されている。

特開平１１−６６０３９号公報

発明者らは、次のことを検討している。例えば、移動体の経路の評価関数の値（以下、評価値と称す）は、位置と速度（例えば他車との相対位置、現在速度と制限速度との差分）に応じて設定でき、評価関数は、位置ｘ（ｔ）及び速度ｖ（ｔ）を変数とした関数で表すことができる。ここで位置ｘ（ｔ）と速度ｖ（ｔ）は、ｘ（ｔ＋１）＝ｘ（ｔ）＋ｖ（ｔ）・Δｔで表される等式制約の関係性を備える。この等式制約を満たさない経路は移動体の経路として非現実的であるため、常に等式制約を満たす経路を出力する必要がある。しかし、経路探索はリアルタイム処理であり、極値化が終了する前に解を出力しなければならない状況も想定される。このため、極値化の途中でも常に等式制約を満たす極値化の手法が必要となる。特許文献１には等式制約をもつ場合の手法について記載されているが、極値化処理の途中で常に等式制約を満足させることはできない。

また、速度ｖ（ｔ）を位置ｘ（ｔ）の関数とみなし、評価関数を位置ｘ（ｔ）の関数として極値化する方法も考えられるが、評価関数を位置ｘ（ｔ）だけの関数として表した場合、絶対的な位置ｘ（ｔ）に関して行う最適化処理と、時間的変化を伴う位置の差分ｘ（ｔ＋１）−ｘ（ｔ）に関して行う最適化処理と、を同時に実行することになり、評価関数の極値への収束に時間を必要以上に要してしまうことになる可能性が高い。この種の課題は、経路探索処理に限られるものではなく、様々な種類の問題に生じる。

本発明の目的は、与えられた等式制約を常に満足させながら高速処理できるようにした連続値最適化問題の非線形最適化プログラム、経路探索プログラム、及び経路探索装置を提供することにある。

請求項１に記載した発明は、変数に応じた等式制約が付与された評価関数が極値に近接した条件を満たす変数または評価関数の最適値を導出する最適化問題に係る探索プログラムを対象としている。この探索プログラムは、評価関数にラグランジュ定数を用いて等式制約付きのラグランジュ未定乗数法を適用するときに等式制約の変数と独立的に分離した評価関数の変数を用意し、評価関数の変数と等式制約の変数とを一致させる追加等式制約を用いて処理するプログラムである。

このプログラムによれば、最適化装置に、評価関数が極値に近くなる条件を探索しながら等式制約に徐々に近づくように評価関数の変数を更新する手順と、等式制約を満足したまま評価関数が極値に近くなる条件に徐々に近づくように等式制約の変数を更新する手順と、を交互に繰り返し実行させるようにしている。このようにすることで、等式制約の変数が常に等式制約を満たしながら最適制御を高速処理できるようになる。

一実施形態における経路探索装置の電気的構成図 経路探索装置の機能構成図 ある時刻における移動体の最適経路を示す鳥瞰図（その１） ある時刻における移動体の最適経路を示す鳥瞰図（その２） 複数の移動体の走行時の位置関係を概略的に示す鳥瞰図（その１） 複数の移動体の走行時の位置関係を概略的に示す鳥瞰図（その２） 処理の流れを説明するフローチャート 補助関数法の説明図

以下、本発明の連続値最適化問題の非線形最適化プログラム、経路探索プログラム及び経路探索装置の一実施形態について図面を参照して説明する。経路探索装置１は、図１Ａに示すように、制御器２を備え、位置検出器３、速度センサ４、地図情報取得部５、等を接続して構成される。

この経路探索装置１は、移動体が未来時刻において最適位置を通過する経路を最適化する最適化装置としても構成される。制御器２は、ＣＰＵ６と、ＲＯＭ、ＲＡＭ、不揮発性メモリ等によるメモリ７と、入出力インタフェース（Ｉ／Ｏ）８と、をバス接続して構成されたマイクロコンピュータ（以下マイコンと略す）９などを用いて構成され、最適化問題に係る最適化プログラムがメモリ７に記憶されている。

制御器２が、最適化プログラムを実行することにより実現される機能としては、図１Ｂに示すように、評価関数更新部１０、等式制約変数更新部１１、及び、経路探索部１２としての機能が挙げられる。また、図示していないが、拡張ラグランジュ未定乗数更新部、及び、極値化部としての機能を備えるようにしても良い。

マイコン９は、非遷移的実体的記録媒体としてのメモリ７に記憶された最適化プログラムを実行することで各種処理を実行し、評価関数（後述ではＨ（｛ｘ_ｉα，ｖ_ｉα｝））が極値に近接した条件を満たす変数（後述では位置変数ｘ_ｉα，速度変数ｖ_ｉα）または、当該評価関数の最適値を導出し出力するようになっている。

位置検出器３は、例えばＧＰＳ（Global Positioning System）等を用いて現在位置の情報を検出し制御器２に出力する。速度センサ４は、移動体（例えば自動車）Ｃの移動速度を検出し制御器２に出力する。地図情報取得部５は、所定の記憶装置（例えばＨＤＤ）に予め記憶される地図情報、又は、外部の地図サーバ（図示せず）から地図情報を取得する。

図２及び図３は、ある連続した時刻における移動体Ｃと障害物Ｏｂ１、Ｏｂ２との配置関係を鳥瞰図で表している。制御器２のマイクロコンピュータ９は、メモリ７に記憶された最適化プログラムとしての例えば経路探索プログラムを実行することで、位置検出器３から取得される現在位置情報、速度センサ４から取得される移動体Ｃの移動速度情報、及び、地図情報取得部５により取得される地図情報に基づいてモデル予測制御を行う。なお、その他のセンサ、例えば、車両周辺の移動体（例えば自動車、オートバイ、人）または障害物Ｏｂ１、Ｏｂ２を検知するためのカメラ、超音波センサ、車両周辺の移動体や障害物Ｏｂ１、Ｏｂ２との距離を測定するための測距装置、などにより取得される各種情報を用いてモデル予測制御を行っても良い。

このモデル予測制御は、未来予測を行う技術であり、未来を含む一定期間の最適制御を行い、これらの最適制御を所定の時間毎（例えばｔ＝０〜Ｔ）に更新する技術である。経路探索装置１は、モデル予測制御技術を用いることで未来を含む一定期間における移動体Ｃの最適な走行経路を導出したり、障害物Ｏｂ１、Ｏｂ２に衝突することなく所定のスペースに導くように運転支援できる。

制御器２が、障害物Ｏｂ１、Ｏｂ２への衝突を避けながら経路探索処理を行うときには、図２に時刻ｔ０における鳥瞰図を示すように、現在時刻ｔ０から未来時刻ｔ１〜ｔ６までの位置Ｌ１、Ｌ２、Ｌ３、Ｌ４、Ｌ５、Ｌ６を結合した移動体Ｃの最適経路Ｒｏ０を導出することで、時刻ｔ１における最適移動位置Ｌ１を決定する。

図３に、次の経路探索処理を行う時刻ｔ１における鳥瞰図を示している。この図３に示すように、制御器２は、現在時刻ｔ１の位置Ｌ１と共に新たに現在時刻ｔ１の後の未来時刻ｔ２〜ｔ７における位置Ｌ１、Ｌ２１、Ｌ３１、Ｌ４１、Ｌ５１、Ｌ６１、Ｌ７１を結合した最適経路Ｒｏ１を導出するが、時刻ｔ１における最適経路Ｒｏ１は、時刻ｔ０における最適経路Ｒｏ０と近い軌跡となることが考えられる。このとき制御器２が、経路探索処理を行うときには、下記の（１）式のように等式制約を設けることで最適経路を計算することが望ましい。

ここで、ｘ（ｔ）は移動体の位置を表し、ｖ（ｔ）は移動体の速度を表す。これは、移動体が現実的に走行可能な経路を考慮して導出される等式制約となる。仮に、経路探索処理の途中で出力される解が等式制約を満たさないときには、物理的に実現不可能な経路を出力することになる。このため、この（１）式の等式制約を満たすように最適経路を計算することが望ましい。さて、経路探索の評価関数は、位置変数ｘ（ｔ）及び速度変数ｖ（ｔ）の関数として、下記の（２）式のように一般的に表すことができる。

図４及び図５に示す自身の移動体（以下、自動車を想定し自車と称す）Ｃ１の移動経路は、他の移動体（以下、自動車を想定し他車と称す）Ｃ２及びＣ３の移動経路にも依存する。このため、経路探索装置１は、自車Ｃ１の最適経路を探索すると同時に他車Ｃ２及びＣ３の最適経路も探索処理し、その条件下にて自車Ｃ１の移動経路を決定することが望ましい。例えば、図４に示すように、片道２車線Ｒ１及びＲ２の道路を想定し、自車Ｃ１が走行車線Ｒ１を走行中に同一の走行車線Ｒ１を走行中の他車Ｃ２を安全に追い越す場合の経路探索処理を考慮する。

図４に示すように、走行車線Ｒ１を走行中の他車Ｃ２は、自車Ｃ１がどのように走行したとしても走行車線Ｒ１を走行することと仮定する。自車Ｃ１の後方に位置すると共に追越車線Ｒ２を走行中の他車Ｃ３が、自車Ｃ１の走行速度よりも速い場合、他車Ｃ３が自車Ｃ１の後方から当該自車Ｃ１を追い越すことが想定される。

このとき、自車Ｃ１が走行車線Ｒ１から追越車線Ｒ２に車線変更すると衝突する虞がある。このため、自車Ｃ１に搭載される経路探索装置１の制御器２は、他車Ｃ３の移動経路を予測し、図５に示すように、速度の速い他車Ｃ３が自車Ｃ１を追い越したことを確認した後、走行車線Ｒ１から追越車線Ｒ２に車線変更する経路Ｒａを最適経路として探索することが望ましい。このとき、制御器２は、経路探索の最適化変数となる位置変数ｘ（ｔ）、速度変数ｖ（ｔ）として、下記の（３）式のように行列式で用意する。

ここで、例えば位置変数ｘ_１１、ｘ_１２、速度変数ｖ_１１、ｖ_１２等に付与された最初の添え字「１」「２」…「ｉ」は、自車Ｃ１であるか他車Ｃ２、Ｃ３であるか、また他車Ｃ２、Ｃ３であっても、自車Ｃ１の周囲に位置する複数の他車Ｃ２、Ｃ３の識別符号を示す。この（３）式では、この添え字「１」を自車Ｃ１に対応した識別符号とし、添え字「２」〜「ｉ」を他車Ｃ２、Ｃ３に対応した識別符号としている。

また、位置変数ｘ_１１、ｘ_１２、速度変数ｖ_１１、ｖ_１２等に付与された２番目の添え字「１」「２」は、例えば次元の軸を示すものであり、例えばｘ座標であるかｙ座標であるかを示す識別符号である。本形態では、最大２次元の鳥瞰図で経路探索処理を行うことを想定しているため、この（３）式では「１」をｘ座標とし、「２」をｙ座標としている。

説明の簡単化のため、２次元で示しているが３次元以上で考慮しても良い。また、例えば位置変数ｘ_１１（０）、ｘ_１１（１）…ｘ_１１（Ｔ）等のうち、括弧内の数字の「０」は現在時点を表し、「１、２、…、Ｔ」は現在時刻から相対的に未来となる時刻を表す。すなわち、ｘ_ｉα（ｔ）は時刻ｔにおける車両ｉのα番目の位置変数を示し、ｖ_ｉα（ｔ）は時刻ｔにおける車両ｉのα番目の速度変数を示す。

評価関数を示す（２）式、等式制約を示す（１）式を一般化して行列式（３）式を適用すると、（４）式のように示すことができる。

ここで、係数行列Ｗは（５−１）式、位置変数行列ｘ_ｉαは（５−２）式、時刻０〜Ｔの間の速度変数行列ｖ_ｉαは（５−３）式で表すことができ、時刻０〜Ｔ−１の間の速度変数行列ｖ_ｉα＾（−）を（５−４）式のように定義している。

前述の（４）式の等式制約ｖ_ｉα＾（−）＝Ｗｘ_ｉαは、例えば現在時刻（例えば０）から次回の時刻（例えば１）に至るまでの位置変数ｘ_ｉαの時間的変化を速度変数ｖ_ｉαとして定義付けることを表しており、（４）式は、この等式制約を条件として評価関数Ｈを最小化する位置変数行列ｘ_ｉα及び速度変数行列ｖ_ｉαを出力することを示している。

さて、このような最適化問題を処理するためラグランジュ未定乗数法を適用する。そして、さらに等式制約の変数（ｘ_ｉα，ｖ_ｉα）と独立的に分離した評価関数Ｈの変数（ｐ_ｉα，ｑ_ｉα）を用意し、評価関数の変数（ｐ_ｉα，ｑ_ｉα）と等式制約の変数（ｘ_ｉα，ｖ_ｉα）とを一致させる等式制約（追加等式制約相当）を追加する。すなわち、評価関数Ｈに係る一対の変数ｐ_ｉα，ｑ_ｉαと、等式制約に係る位置変数ｘ_ｉα、速度変数ｖ_ｉαとを別々の変数として置き換えると共に、これらの変数がそれぞれ一致するような等式制約条件を設ける。

（４）式の等式制約付きの評価関数Ｈについてラグランジュ未定乗数法を適用すると、（６）式のように表すことができる。

この（６）式中のＮは、自車Ｃ１及び他車Ｃ２、Ｃ３を含む車両数、Ｍは次元数（例えば２）を示している。ここでλ_ｉαは、ラグランジュ乗数行列を示し、下記の（７）式のように示すことができる。

そしてさらに、評価関数Ｈの変数となる位置変数行列ｘ_ｉα及び速度変数行列ｖ_ｉαと、等式制約項の変数となる位置変数行列ｘ_ｉα及び速度変数行列ｖ_ｉαと、を分離し、この分離した一対の変数がそれぞれ一致する等式制約を追加すると、下記の（８）式のように示すことができる。

ここで、一対の変数行列ｐ_ｉα、ｑ_ｉαが新たに導入した変数であり、この（８）式の等式制約第１式では、変数行列ｐ_ｉαが位置変数行列ｘ_ｉαに一致することを示し、等式制約第２式では、変数行列ｑ_ｉαが速度変数行列ｖ_ｉαに一致することを示している。

この等式制約付きの最適化問題を解く手法として様々な手法が考えられるが、発明者らは追加した等式制約をさらに拡張ラグランジュ法で取り扱うことが望ましいことを導出している。（８）式に拡張ラグランジュ法を適用すると、下記の（９）式のように示すことができる。

（９）式の第２項及び第３項が（８）式の等式制約第１式のｘ_ｉα−ｐ_ｉα＝０に関して拡張ラグランジュ法を用いた数式展開を示し、（９）式の第４項及び第５項が（８）式の等式制約第２式のｖ_ｉα−ｑ_ｉα＝０に関して拡張ラグランジュ法を用いた数式展開を示している。これらの（９）式の第２項から第５項が、新たな等式制約となる（８）式の等式制約第１式及び第２式に徐々に近づけるために設けられる項となり、ρ_ｘ、ρ_ｖが新たな等式制約からのずれを検出し、拡張ラグランジュ未定定数行列θ_ｘ，ｉα、θ_ｖ，ｉαを新たな等式制約に近づくように更新する。これにより、新たな等式制約を満足しながら評価関数Ｈを極小化できる。この詳細説明は後述する。なお、この（９）式中において、拡張ラグランジュ未定定数行列θ_ｘ，ｉα、θ_ｖ，ｉαは下記の（１０−１）式、（１０−２）式のように表すことができる。

以下、この経路探索処理に係る最適化問題の詳細な解法の説明について、図６以降の図面を参照しながら説明する。図６は、経路探索装置１が実行する経路探索処理に係る最適化問題の解法処理をフローチャートにより示している。

この図６に示すように、まず経路探索装置１の制御器２は、図６のＳ１において初期値を設定する。この初期値は、自車Ｃ１及び他車Ｃ２、Ｃ３の現在位置ｘ_ｉα（０）、自車Ｃ１及び他車Ｃ２、Ｃ３の現在走行速度ｖ_ｉα（０）の情報を蓄積することで得られる値であり、これらの情報は、位置検出器３、速度センサ４、地図情報取得部５等から得られる情報を用いて取得できる。

制御器２は、これらの情報を随時定期的に更新することで時刻０〜Ｔにおける自車Ｃ１及び他車Ｃ２、Ｃ３の位置変数ｘ_ｉα及び速度変数ｖ_ｉαの過去の履歴情報を初期値として蓄積する。これにより（３）式のように、時刻０〜Ｔにおける自車Ｃ１及び他車Ｃ２、Ｃ３の位置変数ｘ_ｉα及び速度変数ｖ_ｉαの過去の履歴情報を初期値として蓄積できる。

制御器２は、これらの情報を用いて、図６のＳ２において（９）式の（ｐ_ｉα、ｑ_ｉα）の一対の値を更新する。このＳ２の処理は、評価関数更新部１０による評価関数Ｈの更新処理に相当するもので、このＳ２の処理は、評価関数Ｈが極小値に近くなる条件を探索しながら等式制約に徐々に近づくように評価関数Ｈの位置変数及び速度変数（ｐ_ｉα、ｑ_ｉα）を更新する処理を示している。これらの（ｐ_ｉα、ｑ_ｉα）を更新する手法としては様々な手法を考慮できるが、発明者らにより補助関数法を適用することが望ましいことが確認されている。この補助関数法は、各種の値を極値化するときに用いられる一方法であり、評価関数Ｈを当該評価関数Ｈに近似した２次関数に置換し、この置換した２次関数に基づいて極値化する方法を示している。

以下、補助関数法について簡単に説明する。補助関数法の詳細イメージを図７に示している。図７に示すように、例えば一次元変数ｘの評価関数をｆmej（ｘ）としたときに、この評価関数ｆmej（ｘ）の変数ｘに現在の解候補ｘ＾＊（図７では例えばｘａ）を代入し、この評価値ｆmej（ｘ＾＊）を通過すると共に、その微分値が評価関数ｆmej（ｘ）の偏微分値∂ｆmej（ｘ＾＊）／∂ｘと等しく、且つ、その解候補ｘ＾＊を含む探索空間内の全ての変数ｘ_ｉにおける評価値ｆmej（ｘ_ｉ）よりも大きな値を得る条件を満たすリプシッツ定数Ｌを用いた２次関数ｆｆを導入する。この２次関数ｆｆを数式化すると、（１１−１）式の右辺のように示すことができる。また、この２次関数ｆｆが極値となる条件を満たす解をｘｚとすると（１１−２）式のように導出できる。

これらの（１１−１）式、（１１−２）式において、Ｌはリプシッツ定数を示し、ｘ＾＊は、変数ｘの現在の解候補を示している。なお、（１１−２）式の右辺を微分すると（１２）式のように導出できる。

このとき、変数ｘに解候補ｘ＾＊を代入することで（１２）式の第２項を０にでき、現在の解候補ｘ＾＊における微分値は（１２）式の右辺の第１項と等しくなる。この（１２）式の右辺の第１項は、解候補ｘ＾＊における評価関数ｆmejの偏微分値∂ｆmej（ｘ＾＊）／∂ｘに一致する。また、この２次関数ｆｆは、解候補ｘ＾＊を含む全ての変数ｘ_ｉで評価関数ｆmejの評価値ｆmej(ｘ_ｉ)よりも大きな値となっている。このような２次関数ｆｆを用いて評価関数ｆmejの極値を求めることが望ましい方法となる。

（１１−２）式に示すように、解候補ｘｚを繰り返し導出することで、図７に示すように、現在の解候補をｘａとしたときに、まず２次関数ｆｆ１を探索し、２次関数ｆｆ１の極値を満たす解を次回の解候補ｘｂとし、その後、この次回の解候補ｘｂについて再度２次関数ｆｆ２の探索を行い、２次関数ｆｆ２の極値を満たす解を解候補ｘｃに更新する。図７の例の場合、解候補はｘａ→ｘｂ→ｘｃとなるように変化する。これらの処理が、例えば所定回数以上繰り返されることで終了条件を満たしたときに、評価関数ｆmej（ｘ）の極値と見做すことが可能な解（例えばｘｃ）を得ることができる。

このような補助関数法を（９）式の評価関数Ｈ（｛ｐ_ｉα、ｑ_ｉα｝）に適用し、２次関数に置き換えると（１３−１）式、（１４−１）式のように示すことができる。

（１３−１）式は、（９）式においてｐ_ｉαを最小化するときに補助関数法を適用した数式を示しており、（１３−２）式の~ｐ_ｉα（ｔ）はｐ_ｉαの次の解候補を示す。（１４−１）式は、（９）式においてｑ_ｉαを最小化するときに補助関数法を適用した数式を示しており、（１４−２）式の~ｑ_ｉα（ｔ）はｑ_ｉαの次の解候補を示す。これらの解候補~ｐ_ｉα（ｔ）、~ｑ_ｉα（ｔ）を順次更新し、例えば所定回数繰り返すことで、評価関数Ｈ（｛ｐ_ｉα、ｑ_ｉα｝）が極小となる条件を満たす一対の値（~ｐ_ｉα，~ｑ_ｉα）を更新できる。

次に、制御器２は、この更新された一対の変数（ｐ_ｉα、ｑ_ｉα）を用いて、図６のＳ３において（９）式の位置変数及び速度変数（ｘ_ｉα、ｖ_ｉα）を更新する。この図６のＳ３の処理は、等式制約変数更新部１１による等式制約変数（ｘ_ｉα、ｖ_ｉα）の更新処理に相当するもので、このＳ３の処理は、等式制約を満足したまま評価関数Ｈが極値に近くなる条件に徐々に近づくように等式制約変数（ｘ_ｉα、ｖ_ｉα）を更新する処理を示している。このとき、（９）式は（ｐ_ｉα、ｑ_ｉα）を定数と見れば、ｘ_ｉα、ｖ_ｉαの各２次関数と見做すことができるため、更新解（~ｘ_ｉα、~ｖ_ｉα）を下記の（１５−１）式、（１５−２）式のように導出できる。

次に、（１５−１）式、（１５−２）式に含まれるラグランジュ定数λ_ｉαを導出する。（４）式に示した元の等式制約ｖ_ｉα＾（−）＝Ｗｘ_ｉαを満たすラグランジュ定数λ_ｉαを導出すると（１６）式に示すように展開できる。

このため、この（１６）式を（１５−１）式、（１５−２）式に代入することで、（１７−１）式、（１７−２）式のように更新解（~ｘ_ｉα，~ｖ_ｉα）の値を導出できる。

ここで（４）式に示した元の等式制約ｖ_ｉα＾（−）＝Ｗｘ_ｉαを満たすようにしているため、更新解（~ｘ_ｉα，~ｖ_ｉα）は元の等式制約を常に満たすように更新できる。

次に、制御器２は、これらの更新解（~ｘ_ｉα，~ｖ_ｉα）（~ｐ_ｉα，~ｑ_ｉα）を算出した後に、図６のＳ４においてこれらの更新解を用いて更新解（~θ_ｘ，ｉα，~θ_ｖ，ｉα）を導出する。このとき、下記の（１８−１）式、（１８−２）式に示すように導出する。

前述のように、更新解（~ｘ_ｉα，~ｖ_ｉα）（~ｐ_ｉα， ~ｑ_ｉα）を算出し、これらの更新解（~ｘ_ｉα，~ｖ_ｉα）（~ｐ_ｉα，~ｑ_ｉα）が追加等式制約を満たすように随時更新するものの、これらの更新解にはズレを生じる。このため、評価関数の変数（ｐ_ｉα，ｑ_ｉα）と等式制約の変数（ｘ_ｉα，ｖ_ｉα）との追加等式制約からのずれを検出するための定数（ρ_ｘ，ρ_ｖ）を導入し、（１８−１）式、（１８−２）式に示すように導出することで、拡張ラグランジュ未定変数（θ_ｘ，θ_ｖ）を追加等式制約に近づくように更新すると良い。これによりズレを極力補正できる。

制御器２は、これらの図６のＳ２〜Ｓ４の処理について終了条件を満たすまで繰り返す。この終了条件は、例えば更新回数が所定上限回数を上回ったか否かを条件としても良いし、例えば得られた位置変数ｘ_ｉα，速度変数ｖ_ｉαを代入した評価関数Ｈの評価値が所定値以下となっているか、など他の条件を用いても良い。

そして制御器２は、この所定の終了条件を満たしたときに、位置変数及び速度変数（ｘ_ｉα，ｖ_ｉα）を出力する。この結果、未来における自車Ｃ１の最適位置変数ｘ_ｉαに応じた経路及び速度変数ｖ_ｉαの最適値を探索できる。この場合、速度変数ｖ_ｉαの最適値は導出対象としなくても良い。これにより、未来時刻における各移動体Ｃ１、Ｃ２、Ｃ３の位置変数ｘ_ｉα及び速度変数ｖ_ｉαを推定して解出力でき、位置変数ｘ_ｉαに応じた経路を最適経路として探索できる。また、位置変数及び速度変数（ｘ_ｉα，ｖ_ｉα）が常に等式制約を満たすように更新されているため、前述した極値化処理の途中で処理が終了し位置変数及び速度変数（ｘ_ｉα，ｖ_ｉα）を解出力したとしても、移動体が現実的に実現可能な変数値を解出力でき、非現実的な変数値、すなわち経路を解出力することはない。このＳ６の処理及び位置変数ｘ_ｉαが、経路探索部１２による最適経路の探索処理に相当する。

本実施形態の特徴を概念的にまとめる。
本実施形態によれば、評価関数Ｈ（｛ｘ_ｉα，ｖ_ｉα｝）が極値に近くなる条件を探索しながら等式制約に徐々に近づくように評価関数Ｈ（｛ｘ_ｉα，ｖ_ｉα｝）の変数ｐ_ｉα，ｑ_ｉαを更新する手順と、等式制約を満足したまま評価関数Ｈ（｛ｘ_ｉα，ｖ_ｉα｝）が極値に近くなる条件に徐々に近づくように等式制約の変数を更新する手順と、を交互に繰り返し実行させるようにしている。このようにすることで、等式制約の変数が常に等式制約を満たしながら最適制御を高速処理できる。

また評価関数Ｈ（｛ｘ_ｉα，ｖ_ｉα｝）の変数ｐ_ｉα，ｑ_ｉαと等式制約の変数ｘ_ｉα，ｖ_ｉαとを一致させる追加等式制約を満たすようにラグランジュ定数λを用いて等式制約の変数ｘ_ｉα，ｖ_ｉαを更新する手順と、を繰り返し実行している。これにより、最適制御を高速処理できるようになる。

特に、経路探索処理に適用したときには、評価関数更新部１０による更新処理と等式制約変数更新部１１による更新処理とを繰り返し行い所定の終了条件を満たして得られた位置変数ｘ_ｉαに応じた経路を最適経路として探索するようにしている。これにより最適経路を探索できる。また、位置変数及び速度変数（ｘ_ｉα，ｖ_ｉα）が常に等式制約を満たしているため、極値化処理の途中で処理が終了し位置変数及び速度変数（ｘ_ｉα，ｖ_ｉα）を解出力したとしても、移動体が現実的に実現可能な変数値を解出力でき、非現実的な変数値、経路を解出力することはない。

拡張ラグランジュ未定乗数（θ_ｘ，θ_ｖ）、及び、評価関数Ｈの変数（ｐ，ｑ）と等式制約の変数（ｘ，ｖ）との追加等式制約からのずれを検出するための定数（ρ_ｘ，ρ_ｖ）を用いた拡張ラグランジュ法を適用して算出するときに、評価関数Ｈの変数（ｐ，ｑ）及び等式制約の変数（ｘ，ｖ）を更新した後には拡張ラグランジュ未定乗数（θ_ｘ，θ_ｖ）を更新するようにしている。これにより、ズレを補正できる。

（他の実施形態）
本発明は、上記した実施形態に限定されるものではなく、以下のように変形又は拡張することができる。

前述実施形態では、位置変数ｘ_ｉα，速度変数ｖ_ｉαの最適値を解出力する形態を説明しているが、位置変数ｘ_ｉα，速度変数ｖ_ｉαに対応した評価値、すなわち、解出力すべき変数ｘ_ｉを評価関数Ｈに代入した評価値を解出力するようにしても良い。位置変数及び速度変数（ｘ_ｉα，ｖ_ｉα）を変数とした等式制約を適用した形態を示したが、一般化された連続値最適化問題の非線形最適化処理に適用できる。極値として最小値、極小値を適用した形態を示したが、極値として最大値、極大値を適用した場合にも適用できる。

また、特許請求の範囲に記載した括弧内の符号は、本発明の一つの態様として前述する実施形態に記載の具体的手段との対応関係を示すものであって、本発明の技術的範囲を限定するものではない。前述実施形態の一部を、課題を解決できる限りにおいて省略した態様も実施形態と見做すことが可能である。また、特許請求の範囲に記載した文言によって特定される発明の本質を逸脱しない限度において、考え得るあらゆる態様も実施形態と見做すことが可能である。

また本発明は、前述した実施形態に準拠して記述したが、本発明は当該実施形態や構造に限定されるものではないと理解される。本発明は、様々な変形例や均等範囲内の変形をも包含する。加えて、様々な組み合わせや形態、さらには、それらに一要素、それ以上、あるいはそれ以下、を含む他の組み合わせや形態をも、本開示の範畴や思想範囲に入るものである。

図面中、１は運転支援装置（経路探索装置、最適化装置）、２は制御器（評価関数更新部、等式制約変数更新部、経路探索部、拡張ラグランジュ未定乗数更新部、極値化部）、１０は評価関数更新部、１１は等式制約変数更新部、１２は経路探索部、を示す。

Claims (9)

1. 変数（ｘ_ｉα，ｖ_ｉα）に応じた等式制約が付与された評価関数（Ｈ（｛ｘ_ｉα，ｖ_ｉα｝））が極値に近接した条件を満たす前記変数または前記評価関数の最適値を導出する最適化問題に係る最適化プログラムであって、
   前記評価関数にラグランジュ定数（λ）を用いて等式制約付きのラグランジュ未定乗数法を適用するときに前記等式制約の変数（ｘ_ｉα，ｖ_ｉα）と独立的に分離した前記評価関数の変数（ｐ_ｉα，ｑ_ｉα）を用意し、前記評価関数の変数と前記等式制約の変数とを一致させる追加等式制約を用いて処理するプログラムであり、
   最適化装置（１）に、
   前記評価関数が極値に近くなる条件を探索しながら等式制約に徐々に近づくように評価関数の変数を更新する手順と、
   前記等式制約を満足したまま前記評価関数が極値に近くなる条件に徐々に近づくように等式制約の変数を更新する手順と、
   を交互に繰り返し実行させる連続値最適化問題の非線形最適化プログラム。
2. 請求項１記載の最適化プログラムにおいて、
   前記追加等式制約に近づくように更新する拡張ラグランジュ未定乗数（θ_ｘ，θ_ｖ）及び前記評価関数の変数と前記等式制約の変数との前記追加等式制約からのずれを検出するための定数（ρ_ｘ，ρ_ｖ）を用いた拡張ラグランジュ法を適用して算出するものであって、
   前記評価関数の変数及び前記等式制約の変数を更新した後には前記拡張ラグランジュ未定乗数（θ_ｘ，θ_ｖ）を更新する連続値最適化問題の非線形最適化プログラム。
3. 請求項２記載の最適化プログラムにおいて、
   前記評価関数の変数の極値を導出するときに、
   前記極値の解候補を求めた後、当該解候補を代入したときの微分値が当該評価関数の微分値と等しく、且つ、前記解候補を含む全ての変数における評価値よりも大きな値を得る条件を満たす２次関数に置換し、当該２次関数の極値を次回の値の解候補として繰り返して更新する補助関数法を用いて極値化する連続値最適化問題の非線形最適化プログラム。
4. 位置変数及び速度変数（ｘ_ｉα，ｖ_ｉα）に応じた等式制約が付与された評価関数（Ｈ（｛ｘ_ｉα，ｖ_ｉα｝））が極値に近接した条件を満たす前記位置変数に応じた経路を最適経路として探索する経路探索プログラムであって、
   前記評価関数に前記位置変数及び速度変数に応じた等式制約付きのラグランジュ未定乗数法を適用するときに前記等式制約の位置変数及び速度変数（ｘ_ｉα，ｖ_ｉα）と独立的に分離した前記評価関数の一対の変数（ｐ_ｉα，ｑ_ｉα）を用意し、前記評価関数の一対の変数と前記等式制約の位置変数及び速度変数とをそれぞれ一致させる追加等式制約を用いて処理するプログラムであり、
   経路探索装置（１）に、
   前記評価関数が極値に近くなる条件を探索しながら前記等式制約に徐々に近づくように前記評価関数の一対の変数を更新する手順と、
   前記等式制約を満足したまま前記評価関数が極値に近くなる条件に徐々に近づくように前記等式制約の位置変数及び速度変数を更新する手順と、
   前記評価関数の一対の変数を更新する手順と等式制約の位置変数及び速度変数を更新する手順とを繰り返し行い所定の終了条件を満たして得られた前記位置変数に応じた経路を最適経路として探索する手順と、
   を実行させる経路探索プログラム。
5. 請求項４記載の経路探索プログラムにおいて、
   前記追加等式制約に近づくように更新する拡張ラグランジュ未定乗数（θ_ｘ，θ_ｖ）及び前記評価関数の一対の変数と前記等式制約の位置変数及び速度変数との前記追加等式制約からのずれを検出するための定数（ρ_ｘ，ρ_ｖ）を用いた拡張ラグランジュ法を適用して算出するものであって、
   前記評価関数の一対の変数及び前記等式制約の位置変数及び速度変数を更新した後には前記拡張ラグランジュ未定乗数（θ_ｘ，θ_ｖ）を更新する経路探索プログラム。
6. 請求項５記載の経路探索プログラムにおいて、
   前記評価関数の一対の変数を更新するときに当該評価関数の極値を導出するときには、
   前記極値の解候補を求めた後、当該解候補を代入したときの微分値が当該評価関数の微分値と等しく、且つ、前記解候補を含む全ての変数における評価値よりも大きな値を得る条件を満たす２次関数に置換し、当該２次関数の極値を次回の値の解候補として繰り返して更新する補助関数法を用いて極値化する経路探索プログラム。
7. 位置変数及び速度変数（ｘ_ｉα，ｖ_ｉα）に応じた等式制約が付与された評価関数（Ｈ（｛ｘ_ｉα，ｖ_ｉα｝））が極値に近接した条件を満たす前記位置変数に応じた経路を最適経路として探索する経路探索装置（１）であって、
   前記評価関数に前記位置変数及び速度変数に応じた等式制約付きのラグランジュ未定乗数法を適用するときに前記等式制約の位置変数及び速度変数（ｘ_ｉα，ｖ_ｉα）と独立的に分離した前記評価関数の一対の変数（ｐ_ｉα，ｑ_ｉα）を用意し、前記評価関数の一対の変数と前記等式制約の位置変数及び速度変数とをそれぞれ一致させる追加等式制約を用いて処理する経路探索装置であり、
   前記評価関数が極値に近くなる条件を探索しながら前記等式制約に徐々に近づくように前記評価関数の一対の変数を更新する評価関数更新部（１０）と、
   前記等式制約を満足したまま前記評価関数が極値に近くなる条件に徐々に近づくように前記等式制約の位置変数及び速度変数を更新する等式制約変数更新部（１１）と、
   前記評価関数更新部による更新処理と前記等式制約変数更新部による更新処理とを繰り返し行い所定の終了条件を満たして得られた前記位置変数に応じた経路を最適経路として探索する経路探索部（１２）と、
   を備える経路探索装置。
8. 請求項７記載の経路探索装置において、
   前記追加等式制約に近づくように更新する拡張ラグランジュ未定乗数（θ_ｘ，θ_ｖ）及び前記評価関数の一対の変数と前記等式制約の位置変数及び速度変数との前記追加等式制約からのずれを検出するための定数（ρ_ｘ，ρ_ｖ）を用いた拡張ラグランジュ法を適用して算出するものであって、
   前記評価関数の一対の変数及び前記等式制約の位置変数及び速度変数を更新した後には前記拡張ラグランジュ未定乗数（θ_ｘ，θ_ｖ）を更新する拡張ラグランジュ未定乗数更新部（２）、をさらに備える経路探索装置。
9. 請求項８記載の経路探索装置において、
   前記評価関数更新部が、前記評価関数の一対の変数を更新するときに当該評価関数の極値を導出するときには、
   前記極値の解候補を求めた後、当該解候補を代入したときの微分値が当該評価関数の微分値と等しく、且つ、前記解候補を含む探索空間の内の全ての変数における評価値よりも大きな値を得る条件を満たす２次関数に置換し、当該２次関数の極値を次回の値の解候補として繰り返して更新する補助関数法を用いて極値化する極値化部（２）、
   をさらに備える経路探索装置。

Images