TOP > 国内特許検索 > 連続値最適化問題の非線形最適化プログラム、経路探索プログラム、及び経路探索装置 > 明細書

明細書 :連続値最適化問題の非線形最適化プログラム、経路探索プログラム、及び経路探索装置

発行国 日本国特許庁(JP)
公報種別 公開特許公報(A)
公開番号 特開2018-087753 (P2018-087753A)
公開日 平成30年6月7日(2018.6.7)
発明の名称または考案の名称 連続値最適化問題の非線形最適化プログラム、経路探索プログラム、及び経路探索装置
国際特許分類 G01C  21/34        (2006.01)
G05B  13/02        (2006.01)
G06N  99/00        (2010.01)
G08G   1/0968      (2006.01)
FI G01C 21/34
G05B 13/02 J
G06N 99/00 180
G08G 1/0968
請求項の数または発明の数 9
出願形態 OL
全頁数 18
出願番号 特願2016-231278 (P2016-231278)
出願日 平成28年11月29日(2016.11.29)
発明者または考案者 【氏名】岡田 俊太郎
【氏名】寺部 雅能
【氏名】大関 真之
出願人 【識別番号】000004260
【氏名又は名称】株式会社デンソー
【識別番号】504132272
【氏名又は名称】国立大学法人京都大学
個別代理人の代理人 【識別番号】110000567、【氏名又は名称】特許業務法人 サトー国際特許事務所
審査請求 未請求
テーマコード 2F129
5H004
5H181
Fターム 2F129AA03
2F129BB03
2F129BB20
2F129DD02
2F129EE94
2F129EE95
2F129FF20
2F129FF36
2F129GG09
2F129GG10
2F129GG11
2F129GG17
2F129GG18
2F129HH20
5H004GA18
5H004GB12
5H004HA07
5H004HB07
5H004KC02
5H004KC27
5H181AA01
5H181BB04
5H181BB05
5H181CC04
5H181CC11
5H181CC14
5H181FF04
5H181FF21
5H181FF27
5H181LL01
5H181LL02
5H181LL04
要約 【課題】最適制御を高速処理できるようにした連続値最適化問題の非線形最適化プログラム、経路探索プログラム、及び経路探索装置を提供する。
【解決手段】探索プログラムは、評価関数にラグランジュ定数を用いて等式制約付きのラグランジュ未定乗数法を適用するときに等式制約の変数と独立的に分離した評価関数の変数を用意し、評価関数の変数と等式制約の変数とを一致させる追加等式制約を用いて処理するプログラムである。この探索プログラムによれば、最適化装置に、評価関数が極値に近くなる条件を探索しながら等式制約に徐々に近づくように評価関数の変数を更新する手順と、等式制約を満足したまま前記評価関数が極値に近くなる条件に徐々に近づくように等式制約の変数を更新する手順と、を交互に繰り返し実行させるようにしている。
【選択図】図6
特許請求の範囲 【請求項1】
変数(xiα,viα)に応じた等式制約が付与された評価関数(H({xiα,viα}))が極値に近接した条件を満たす前記変数または前記評価関数の最適値を導出する最適化問題に係る最適化プログラムであって、
前記評価関数にラグランジュ定数(λ)を用いて等式制約付きのラグランジュ未定乗数法を適用するときに前記等式制約の変数(xiα,viα)と独立的に分離した前記評価関数の変数(piα,qiα)を用意し、前記評価関数の変数と前記等式制約の変数とを一致させる追加等式制約を用いて処理するプログラムであり、
最適化装置(1)に、
前記評価関数が極値に近くなる条件を探索しながら等式制約に徐々に近づくように評価関数の変数を更新する手順と、
前記等式制約を満足したまま前記評価関数が極値に近くなる条件に徐々に近づくように等式制約の変数を更新する手順と、
を交互に繰り返し実行させる連続値最適化問題の非線形最適化プログラム。
【請求項2】
請求項1記載の最適化プログラムにおいて、
前記追加等式制約に近づくように更新する拡張ラグランジュ未定乗数(θ,θ)及び前記評価関数の変数と前記等式制約の変数との前記追加等式制約からのずれを検出するための定数(ρ,ρ)を用いた拡張ラグランジュ法を適用して算出するものであって、
前記評価関数の変数及び前記等式制約の変数を更新した後には前記拡張ラグランジュ未定乗数(θ,θ)を更新する連続値最適化問題の非線形最適化プログラム。
【請求項3】
請求項2記載の最適化プログラムにおいて、
前記評価関数の変数の極値を導出するときに、
前記極値の解候補を求めた後、当該解候補を代入したときの微分値が当該評価関数の微分値と等しく、且つ、前記解候補を含む全ての変数における評価値よりも大きな値を得る条件を満たす2次関数に置換し、当該2次関数の極値を次回の値の解候補として繰り返して更新する補助関数法を用いて極値化する連続値最適化問題の非線形最適化プログラム。
【請求項4】
位置変数及び速度変数(xiα,viα)に応じた等式制約が付与された評価関数(H({xiα,viα}))が極値に近接した条件を満たす前記位置変数に応じた経路を最適経路として探索する経路探索プログラムであって、
前記評価関数に前記位置変数及び速度変数に応じた等式制約付きのラグランジュ未定乗数法を適用するときに前記等式制約の位置変数及び速度変数(xiα,viα)と独立的に分離した前記評価関数の一対の変数(piα,qiα)を用意し、前記評価関数の一対の変数と前記等式制約の位置変数及び速度変数とをそれぞれ一致させる追加等式制約を用いて処理するプログラムであり、
経路探索装置(1)に、
前記評価関数が極値に近くなる条件を探索しながら前記等式制約に徐々に近づくように前記評価関数の一対の変数を更新する手順と、
前記等式制約を満足したまま前記評価関数が極値に近くなる条件に徐々に近づくように前記等式制約の位置変数及び速度変数を更新する手順と、
前記評価関数の一対の変数を更新する手順と等式制約の位置変数及び速度変数を更新する手順とを繰り返し行い所定の終了条件を満たして得られた前記位置変数に応じた経路を最適経路として探索する手順と、
を実行させる経路探索プログラム。
【請求項5】
請求項4記載の経路探索プログラムにおいて、
前記追加等式制約に近づくように更新する拡張ラグランジュ未定乗数(θ,θ)及び前記評価関数の一対の変数と前記等式制約の位置変数及び速度変数との前記追加等式制約からのずれを検出するための定数(ρ,ρ)を用いた拡張ラグランジュ法を適用して算出するものであって、
前記評価関数の一対の変数及び前記等式制約の位置変数及び速度変数を更新した後には前記拡張ラグランジュ未定乗数(θ,θ)を更新する経路探索プログラム。
【請求項6】
請求項5記載の経路探索プログラムにおいて、
前記評価関数の一対の変数を更新するときに当該評価関数の極値を導出するときには、
前記極値の解候補を求めた後、当該解候補を代入したときの微分値が当該評価関数の微分値と等しく、且つ、前記解候補を含む全ての変数における評価値よりも大きな値を得る条件を満たす2次関数に置換し、当該2次関数の極値を次回の値の解候補として繰り返して更新する補助関数法を用いて極値化する経路探索プログラム。
【請求項7】
位置変数及び速度変数(xiα,viα)に応じた等式制約が付与された評価関数(H({xiα,viα}))が極値に近接した条件を満たす前記位置変数に応じた経路を最適経路として探索する経路探索装置(1)であって、
前記評価関数に前記位置変数及び速度変数に応じた等式制約付きのラグランジュ未定乗数法を適用するときに前記等式制約の位置変数及び速度変数(xiα,viα)と独立的に分離した前記評価関数の一対の変数(piα,qiα)を用意し、前記評価関数の一対の変数と前記等式制約の位置変数及び速度変数とをそれぞれ一致させる追加等式制約を用いて処理する経路探索装置であり、
前記評価関数が極値に近くなる条件を探索しながら前記等式制約に徐々に近づくように前記評価関数の一対の変数を更新する評価関数更新部(10)と、
前記等式制約を満足したまま前記評価関数が極値に近くなる条件に徐々に近づくように前記等式制約の位置変数及び速度変数を更新する等式制約変数更新部(11)と、
前記評価関数更新部による更新処理と前記等式制約変数更新部による更新処理とを繰り返し行い所定の終了条件を満たして得られた前記位置変数に応じた経路を最適経路として探索する経路探索部(12)と、
を備える経路探索装置。
【請求項8】
請求項7記載の経路探索装置において、
前記追加等式制約に近づくように更新する拡張ラグランジュ未定乗数(θ,θ)及び前記評価関数の一対の変数と前記等式制約の位置変数及び速度変数との前記追加等式制約からのずれを検出するための定数(ρ,ρ)を用いた拡張ラグランジュ法を適用して算出するものであって、
前記評価関数の一対の変数及び前記等式制約の位置変数及び速度変数を更新した後には前記拡張ラグランジュ未定乗数(θ,θ)を更新する拡張ラグランジュ未定乗数更新部(2)、をさらに備える経路探索装置。
【請求項9】
請求項8記載の経路探索装置において、
前記評価関数更新部が、前記評価関数の一対の変数を更新するときに当該評価関数の極値を導出するときには、
前記極値の解候補を求めた後、当該解候補を代入したときの微分値が当該評価関数の微分値と等しく、且つ、前記解候補を含む探索空間の内の全ての変数における評価値よりも大きな値を得る条件を満たす2次関数に置換し、当該2次関数の極値を次回の値の解候補として繰り返して更新する補助関数法を用いて極値化する極値化部(2)、
をさらに備える経路探索装置。
発明の詳細な説明 【技術分野】
【0001】
本発明は、連続値最適化問題の非線形最適化プログラム、経路探索プログラム、及び経路探索装置に関する。
【背景技術】
【0002】
近年、例えば、運転支援装置が事故防止対応のために開発されており、特にドライバーへの運転負担を軽減するために開発されている。この種の運転支援装置が経路探索するときには、この経路の位置及び速度を時間に応じた関数とし、これらの位置及び速度に応じた関数に基づく評価関数を導入し、この評価関数の極値(例えば最小値)を満たす位置及び速度を求めることで、最適な経路を探索できることが発明者らにより確認されている。
【0003】
なお、本願に関連する技術が特許文献1に開示されている。この特許文献1には、評価関数と制約条件とを独立に扱い、評価関数を極小化するための変数更新処理と制約条件を満足させるための変数更新処理とを分け、これらの2つの更新処理を交互に実行することにより、制約条件付きの評価関数を極小化する技術が開示されている。
【先行技術文献】
【0004】

【特許文献1】特開平11-66039号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
発明者らは、次のことを検討している。例えば、移動体の経路の評価関数の値(以下、評価値と称す)は、位置と速度(例えば他車との相対位置、現在速度と制限速度との差分)に応じて設定でき、評価関数は、位置x(t)及び速度v(t)を変数とした関数で表すことができる。ここで位置x(t)と速度v(t)は、x(t+1)=x(t)+v(t)・Δtで表される等式制約の関係性を備える。この等式制約を満たさない経路は移動体の経路として非現実的であるため、常に等式制約を満たす経路を出力する必要がある。しかし、経路探索はリアルタイム処理であり、極値化が終了する前に解を出力しなければならない状況も想定される。このため、極値化の途中でも常に等式制約を満たす極値化の手法が必要となる。特許文献1には等式制約をもつ場合の手法について記載されているが、極値化処理の途中で常に等式制約を満足させることはできない。
【0006】
また、速度v(t)を位置x(t)の関数とみなし、評価関数を位置x(t)の関数として極値化する方法も考えられるが、評価関数を位置x(t)だけの関数として表した場合、絶対的な位置x(t)に関して行う最適化処理と、時間的変化を伴う位置の差分x(t+1)-x(t)に関して行う最適化処理と、を同時に実行することになり、評価関数の極値への収束に時間を必要以上に要してしまうことになる可能性が高い。この種の課題は、経路探索処理に限られるものではなく、様々な種類の問題に生じる。
【0007】
本発明の目的は、与えられた等式制約を常に満足させながら高速処理できるようにした連続値最適化問題の非線形最適化プログラム、経路探索プログラム、及び経路探索装置を提供することにある。
【課題を解決するための手段】
【0008】
請求項1に記載した発明は、変数に応じた等式制約が付与された評価関数が極値に近接した条件を満たす変数または評価関数の最適値を導出する最適化問題に係る探索プログラムを対象としている。この探索プログラムは、評価関数にラグランジュ定数を用いて等式制約付きのラグランジュ未定乗数法を適用するときに等式制約の変数と独立的に分離した評価関数の変数を用意し、評価関数の変数と等式制約の変数とを一致させる追加等式制約を用いて処理するプログラムである。
【0009】
このプログラムによれば、最適化装置に、評価関数が極値に近くなる条件を探索しながら等式制約に徐々に近づくように評価関数の変数を更新する手順と、等式制約を満足したまま評価関数が極値に近くなる条件に徐々に近づくように等式制約の変数を更新する手順と、を交互に繰り返し実行させるようにしている。このようにすることで、等式制約の変数が常に等式制約を満たしながら最適制御を高速処理できるようになる。
【図面の簡単な説明】
【0010】
【図1A】一実施形態における経路探索装置の電気的構成図
【図1B】経路探索装置の機能構成図
【図2】ある時刻における移動体の最適経路を示す鳥瞰図(その1)
【図3】ある時刻における移動体の最適経路を示す鳥瞰図(その2)
【図4】複数の移動体の走行時の位置関係を概略的に示す鳥瞰図(その1)
【図5】複数の移動体の走行時の位置関係を概略的に示す鳥瞰図(その2)
【図6】処理の流れを説明するフローチャート
【図7】補助関数法の説明図
【発明を実施するための形態】
【0011】
以下、本発明の連続値最適化問題の非線形最適化プログラム、経路探索プログラム及び経路探索装置の一実施形態について図面を参照して説明する。経路探索装置1は、図1Aに示すように、制御器2を備え、位置検出器3、速度センサ4、地図情報取得部5、等を接続して構成される。

【0012】
この経路探索装置1は、移動体が未来時刻において最適位置を通過する経路を最適化する最適化装置としても構成される。制御器2は、CPU6と、ROM、RAM、不揮発性メモリ等によるメモリ7と、入出力インタフェース(I/O)8と、をバス接続して構成されたマイクロコンピュータ(以下マイコンと略す)9などを用いて構成され、最適化問題に係る最適化プログラムがメモリ7に記憶されている。

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

【0014】
マイコン9は、非遷移的実体的記録媒体としてのメモリ7に記憶された最適化プログラムを実行することで各種処理を実行し、評価関数(後述ではH({xiα,viα}))が極値に近接した条件を満たす変数(後述では位置変数xiα,速度変数viα)または、当該評価関数の最適値を導出し出力するようになっている。

【0015】
位置検出器3は、例えばGPS(Global Positioning System)等を用いて現在位置の情報を検出し制御器2に出力する。速度センサ4は、移動体(例えば自動車)Cの移動速度を検出し制御器2に出力する。地図情報取得部5は、所定の記憶装置(例えばHDD)に予め記憶される地図情報、又は、外部の地図サーバ(図示せず)から地図情報を取得する。

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

【0017】
このモデル予測制御は、未来予測を行う技術であり、未来を含む一定期間の最適制御を行い、これらの最適制御を所定の時間毎(例えばt=0~T)に更新する技術である。経路探索装置1は、モデル予測制御技術を用いることで未来を含む一定期間における移動体Cの最適な走行経路を導出したり、障害物Ob1、Ob2に衝突することなく所定のスペースに導くように運転支援できる。

【0018】
制御器2が、障害物Ob1、Ob2への衝突を避けながら経路探索処理を行うときには、図2に時刻t0における鳥瞰図を示すように、現在時刻t0から未来時刻t1~t6までの位置L1、L2、L3、L4、L5、L6を結合した移動体Cの最適経路Ro0を導出することで、時刻t1における最適移動位置L1を決定する。

【0019】
図3に、次の経路探索処理を行う時刻t1における鳥瞰図を示している。この図3に示すように、制御器2は、現在時刻t1の位置L1と共に新たに現在時刻t1の後の未来時刻t2~t7における位置L1、L21、L31、L41、L51、L61、L71を結合した最適経路Ro1を導出するが、時刻t1における最適経路Ro1は、時刻t0における最適経路Ro0と近い軌跡となることが考えられる。このとき制御器2が、経路探索処理を行うときには、下記の(1)式のように等式制約を設けることで最適経路を計算することが望ましい。

【0020】
【数1】
JP2018087753A_000003t.gif
ここで、x(t)は移動体の位置を表し、v(t)は移動体の速度を表す。これは、移動体が現実的に走行可能な経路を考慮して導出される等式制約となる。仮に、経路探索処理の途中で出力される解が等式制約を満たさないときには、物理的に実現不可能な経路を出力することになる。このため、この(1)式の等式制約を満たすように最適経路を計算することが望ましい。さて、経路探索の評価関数は、位置変数x(t)及び速度変数v(t)の関数として、下記の(2)式のように一般的に表すことができる。

【0021】
【数2】
JP2018087753A_000004t.gif
図4及び図5に示す自身の移動体(以下、自動車を想定し自車と称す)C1の移動経路は、他の移動体(以下、自動車を想定し他車と称す)C2及びC3の移動経路にも依存する。このため、経路探索装置1は、自車C1の最適経路を探索すると同時に他車C2及びC3の最適経路も探索処理し、その条件下にて自車C1の移動経路を決定することが望ましい。例えば、図4に示すように、片道2車線R1及びR2の道路を想定し、自車C1が走行車線R1を走行中に同一の走行車線R1を走行中の他車C2を安全に追い越す場合の経路探索処理を考慮する。

【0022】
図4に示すように、走行車線R1を走行中の他車C2は、自車C1がどのように走行したとしても走行車線R1を走行することと仮定する。自車C1の後方に位置すると共に追越車線R2を走行中の他車C3が、自車C1の走行速度よりも速い場合、他車C3が自車C1の後方から当該自車C1を追い越すことが想定される。

【0023】
このとき、自車C1が走行車線R1から追越車線R2に車線変更すると衝突する虞がある。このため、自車C1に搭載される経路探索装置1の制御器2は、他車C3の移動経路を予測し、図5に示すように、速度の速い他車C3が自車C1を追い越したことを確認した後、走行車線R1から追越車線R2に車線変更する経路Raを最適経路として探索することが望ましい。このとき、制御器2は、経路探索の最適化変数となる位置変数x(t)、速度変数v(t)として、下記の(3)式のように行列式で用意する。

【0024】
【数3】
JP2018087753A_000005t.gif
ここで、例えば位置変数x11、x12、速度変数v11、v12等に付与された最初の添え字「1」「2」…「i」は、自車C1であるか他車C2、C3であるか、また他車C2、C3であっても、自車C1の周囲に位置する複数の他車C2、C3の識別符号を示す。この(3)式では、この添え字「1」を自車C1に対応した識別符号とし、添え字「2」~「i」を他車C2、C3に対応した識別符号としている。

【0025】
また、位置変数x11、x12、速度変数v11、v12等に付与された2番目の添え字「1」「2」は、例えば次元の軸を示すものであり、例えばx座標であるかy座標であるかを示す識別符号である。本形態では、最大2次元の鳥瞰図で経路探索処理を行うことを想定しているため、この(3)式では「1」をx座標とし、「2」をy座標としている。

【0026】
説明の簡単化のため、2次元で示しているが3次元以上で考慮しても良い。また、例えば位置変数x11(0)、x11(1)…x11(T)等のうち、括弧内の数字の「0」は現在時点を表し、「1、2、…、T」は現在時刻から相対的に未来となる時刻を表す。すなわち、xiα(t)は時刻tにおける車両iのα番目の位置変数を示し、viα(t)は時刻tにおける車両iのα番目の速度変数を示す。

【0027】
評価関数を示す(2)式、等式制約を示す(1)式を一般化して行列式(3)式を適用すると、(4)式のように示すことができる。

【0028】
【数4】
JP2018087753A_000006t.gif
ここで、係数行列Wは(5-1)式、位置変数行列xiαは(5-2)式、時刻0~Tの間の速度変数行列viαは(5-3)式で表すことができ、時刻0~T-1の間の速度変数行列viα^(-)を(5-4)式のように定義している。

【0029】
【数5】
JP2018087753A_000007t.gif
前述の(4)式の等式制約viα^(-)=Wxiαは、例えば現在時刻(例えば0)から次回の時刻(例えば1)に至るまでの位置変数xiαの時間的変化を速度変数viαとして定義付けることを表しており、(4)式は、この等式制約を条件として評価関数Hを最小化する位置変数行列xiα及び速度変数行列viαを出力することを示している。

【0030】
さて、このような最適化問題を処理するためラグランジュ未定乗数法を適用する。そして、さらに等式制約の変数(xiα,viα)と独立的に分離した評価関数Hの変数(piα,qiα)を用意し、評価関数の変数(piα,qiα)と等式制約の変数(xiα,viα)とを一致させる等式制約(追加等式制約相当)を追加する。すなわち、評価関数Hに係る一対の変数piα,qiαと、等式制約に係る位置変数xiα、速度変数viαとを別々の変数として置き換えると共に、これらの変数がそれぞれ一致するような等式制約条件を設ける。

【0031】
(4)式の等式制約付きの評価関数Hについてラグランジュ未定乗数法を適用すると、(6)式のように表すことができる。

【0032】
【数6】
JP2018087753A_000008t.gif
この(6)式中のNは、自車C1及び他車C2、C3を含む車両数、Mは次元数(例えば2)を示している。ここでλiαは、ラグランジュ乗数行列を示し、下記の(7)式のように示すことができる。

【0033】
【数7】
JP2018087753A_000009t.gif
そしてさらに、評価関数Hの変数となる位置変数行列xiα及び速度変数行列viαと、等式制約項の変数となる位置変数行列xiα及び速度変数行列viαと、を分離し、この分離した一対の変数がそれぞれ一致する等式制約を追加すると、下記の(8)式のように示すことができる。

【0034】
【数8】
JP2018087753A_000010t.gif
ここで、一対の変数行列piα、qiαが新たに導入した変数であり、この(8)式の等式制約第1式では、変数行列piαが位置変数行列xiαに一致することを示し、等式制約第2式では、変数行列qiαが速度変数行列viαに一致することを示している。

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

【0036】
【数9】
JP2018087753A_000011t.gif
(9)式の第2項及び第3項が(8)式の等式制約第1式のxiα-piα=0に関して拡張ラグランジュ法を用いた数式展開を示し、(9)式の第4項及び第5項が(8)式の等式制約第2式のviα-qiα=0に関して拡張ラグランジュ法を用いた数式展開を示している。これらの(9)式の第2項から第5項が、新たな等式制約となる(8)式の等式制約第1式及び第2式に徐々に近づけるために設けられる項となり、ρ、ρが新たな等式制約からのずれを検出し、拡張ラグランジュ未定定数行列θx,iα、θv,iαを新たな等式制約に近づくように更新する。これにより、新たな等式制約を満足しながら評価関数Hを極小化できる。この詳細説明は後述する。なお、この(9)式中において、拡張ラグランジュ未定定数行列θx,iα、θv,iαは下記の(10-1)式、(10-2)式のように表すことができる。

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

【0038】
この図6に示すように、まず経路探索装置1の制御器2は、図6のS1において初期値を設定する。この初期値は、自車C1及び他車C2、C3の現在位置xiα(0)、自車C1及び他車C2、C3の現在走行速度viα(0)の情報を蓄積することで得られる値であり、これらの情報は、位置検出器3、速度センサ4、地図情報取得部5等から得られる情報を用いて取得できる。

【0039】
制御器2は、これらの情報を随時定期的に更新することで時刻0~Tにおける自車C1及び他車C2、C3の位置変数xiα及び速度変数viαの過去の履歴情報を初期値として蓄積する。これにより(3)式のように、時刻0~Tにおける自車C1及び他車C2、C3の位置変数xiα及び速度変数viαの過去の履歴情報を初期値として蓄積できる。

【0040】
制御器2は、これらの情報を用いて、図6のS2において(9)式の(piα、qiα)の一対の値を更新する。このS2の処理は、評価関数更新部10による評価関数Hの更新処理に相当するもので、このS2の処理は、評価関数Hが極小値に近くなる条件を探索しながら等式制約に徐々に近づくように評価関数Hの位置変数及び速度変数(piα、qiα)を更新する処理を示している。これらの(piα、qiα)を更新する手法としては様々な手法を考慮できるが、発明者らにより補助関数法を適用することが望ましいことが確認されている。この補助関数法は、各種の値を極値化するときに用いられる一方法であり、評価関数Hを当該評価関数Hに近似した2次関数に置換し、この置換した2次関数に基づいて極値化する方法を示している。

【0041】
以下、補助関数法について簡単に説明する。補助関数法の詳細イメージを図7に示している。図7に示すように、例えば一次元変数xの評価関数をfmej(x)としたときに、この評価関数fmej(x)の変数xに現在の解候補x^*(図7では例えばxa)を代入し、この評価値fmej(x^*)を通過すると共に、その微分値が評価関数fmej(x)の偏微分値∂fmej(x^*)/∂xと等しく、且つ、その解候補x^*を含む探索空間内の全ての変数xにおける評価値fmej(x)よりも大きな値を得る条件を満たすリプシッツ定数Lを用いた2次関数ffを導入する。この2次関数ffを数式化すると、(11-1)式の右辺のように示すことができる。また、この2次関数ffが極値となる条件を満たす解をxzとすると(11-2)式のように導出できる。

【0042】
【数11】
JP2018087753A_000013t.gif
これらの(11-1)式、(11-2)式において、Lはリプシッツ定数を示し、x^*は、変数xの現在の解候補を示している。なお、(11-2)式の右辺を微分すると(12)式のように導出できる。

【0043】
【数12】
JP2018087753A_000014t.gif
このとき、変数xに解候補x^*を代入することで(12)式の第2項を0にでき、現在の解候補x^*における微分値は(12)式の右辺の第1項と等しくなる。この(12)式の右辺の第1項は、解候補x^*における評価関数fmejの偏微分値∂fmej(x^*)/∂xに一致する。また、この2次関数ffは、解候補x^*を含む全ての変数xで評価関数fmejの評価値fmej(x)よりも大きな値となっている。このような2次関数ffを用いて評価関数fmejの極値を求めることが望ましい方法となる。

【0044】
(11-2)式に示すように、解候補xzを繰り返し導出することで、図7に示すように、現在の解候補をxaとしたときに、まず2次関数ff1を探索し、2次関数ff1の極値を満たす解を次回の解候補xbとし、その後、この次回の解候補xbについて再度2次関数ff2の探索を行い、2次関数ff2の極値を満たす解を解候補xcに更新する。図7の例の場合、解候補はxa→xb→xcとなるように変化する。これらの処理が、例えば所定回数以上繰り返されることで終了条件を満たしたときに、評価関数fmej(x)の極値と見做すことが可能な解(例えばxc)を得ることができる。

【0045】
このような補助関数法を(9)式の評価関数H({piα、qiα})に適用し、2次関数に置き換えると(13-1)式、(14-1)式のように示すことができる。

【0046】
【数13】
JP2018087753A_000015t.gif

【0047】
【数14】
JP2018087753A_000016t.gif
(13-1)式は、(9)式においてpiαを最小化するときに補助関数法を適用した数式を示しており、(13-2)式の~piα(t)はpiαの次の解候補を示す。(14-1)式は、(9)式においてqiαを最小化するときに補助関数法を適用した数式を示しており、(14-2)式の~qiα(t)はqiαの次の解候補を示す。これらの解候補~piα(t)、~qiα(t)を順次更新し、例えば所定回数繰り返すことで、評価関数H({piα、qiα})が極小となる条件を満たす一対の値(~piα,~qiα)を更新できる。

【0048】
次に、制御器2は、この更新された一対の変数(piα、qiα)を用いて、図6のS3において(9)式の位置変数及び速度変数(xiα、viα)を更新する。この図6のS3の処理は、等式制約変数更新部11による等式制約変数(xiα、viα)の更新処理に相当するもので、このS3の処理は、等式制約を満足したまま評価関数Hが極値に近くなる条件に徐々に近づくように等式制約変数(xiα、viα)を更新する処理を示している。このとき、(9)式は(piα、qiα)を定数と見れば、xiα、viαの各2次関数と見做すことができるため、更新解(~xiα、~viα)を下記の(15-1)式、(15-2)式のように導出できる。

【0049】
【数15】
JP2018087753A_000017t.gif
次に、(15-1)式、(15-2)式に含まれるラグランジュ定数λiαを導出する。(4)式に示した元の等式制約viα^(-)=Wxiαを満たすラグランジュ定数λiαを導出すると(16)式に示すように展開できる。

【0050】
【数16】
JP2018087753A_000018t.gif
このため、この(16)式を(15-1)式、(15-2)式に代入することで、(17-1)式、(17-2)式のように更新解(~xiα,~viα)の値を導出できる。

【0051】
【数17】
JP2018087753A_000019t.gif
ここで(4)式に示した元の等式制約viα^(-)=Wxiαを満たすようにしているため、更新解(~xiα,~viα)は元の等式制約を常に満たすように更新できる。

【0052】
次に、制御器2は、これらの更新解(~xiα,~viα)(~piα,~qiα)を算出した後に、図6のS4においてこれらの更新解を用いて更新解(~θx,iα,~θv,iα)を導出する。このとき、下記の(18-1)式、(18-2)式に示すように導出する。

【0053】
【数18】
JP2018087753A_000020t.gif
前述のように、更新解(~xiα,~viα)(~piα, ~qiα)を算出し、これらの更新解(~xiα,~viα)(~piα,~qiα)が追加等式制約を満たすように随時更新するものの、これらの更新解にはズレを生じる。このため、評価関数の変数(piα,qiα)と等式制約の変数(xiα,viα)との追加等式制約からのずれを検出するための定数(ρ,ρ)を導入し、(18-1)式、(18-2)式に示すように導出することで、拡張ラグランジュ未定変数(θ,θ)を追加等式制約に近づくように更新すると良い。これによりズレを極力補正できる。

【0054】
制御器2は、これらの図6のS2~S4の処理について終了条件を満たすまで繰り返す。この終了条件は、例えば更新回数が所定上限回数を上回ったか否かを条件としても良いし、例えば得られた位置変数xiα,速度変数viαを代入した評価関数Hの評価値が所定値以下となっているか、など他の条件を用いても良い。

【0055】
そして制御器2は、この所定の終了条件を満たしたときに、位置変数及び速度変数(xiα,viα)を出力する。この結果、未来における自車C1の最適位置変数xiαに応じた経路及び速度変数viαの最適値を探索できる。この場合、速度変数viαの最適値は導出対象としなくても良い。これにより、未来時刻における各移動体C1、C2、C3の位置変数xiα及び速度変数viαを推定して解出力でき、位置変数xiαに応じた経路を最適経路として探索できる。また、位置変数及び速度変数(xiα,viα)が常に等式制約を満たすように更新されているため、前述した極値化処理の途中で処理が終了し位置変数及び速度変数(xiα,viα)を解出力したとしても、移動体が現実的に実現可能な変数値を解出力でき、非現実的な変数値、すなわち経路を解出力することはない。このS6の処理及び位置変数xiαが、経路探索部12による最適経路の探索処理に相当する。

【0056】
本実施形態の特徴を概念的にまとめる。
本実施形態によれば、評価関数H({xiα,viα})が極値に近くなる条件を探索しながら等式制約に徐々に近づくように評価関数H({xiα,viα})の変数piα,qiαを更新する手順と、等式制約を満足したまま評価関数H({xiα,viα})が極値に近くなる条件に徐々に近づくように等式制約の変数を更新する手順と、を交互に繰り返し実行させるようにしている。このようにすることで、等式制約の変数が常に等式制約を満たしながら最適制御を高速処理できる。

【0057】
また評価関数H({xiα,viα})の変数piα,qiαと等式制約の変数xiα,viαとを一致させる追加等式制約を満たすようにラグランジュ定数λを用いて等式制約の変数xiα,viαを更新する手順と、を繰り返し実行している。これにより、最適制御を高速処理できるようになる。

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

【0059】
拡張ラグランジュ未定乗数(θ,θ)、及び、評価関数Hの変数(p,q)と等式制約の変数(x,v)との追加等式制約からのずれを検出するための定数(ρ,ρ)を用いた拡張ラグランジュ法を適用して算出するときに、評価関数Hの変数(p,q)及び等式制約の変数(x,v)を更新した後には拡張ラグランジュ未定乗数(θ,θ)を更新するようにしている。これにより、ズレを補正できる。

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

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

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

【0063】
また本発明は、前述した実施形態に準拠して記述したが、本発明は当該実施形態や構造に限定されるものではないと理解される。本発明は、様々な変形例や均等範囲内の変形をも包含する。加えて、様々な組み合わせや形態、さらには、それらに一要素、それ以上、あるいはそれ以下、を含む他の組み合わせや形態をも、本開示の範畴や思想範囲に入るものである。
【符号の説明】
【0064】
図面中、1は運転支援装置(経路探索装置、最適化装置)、2は制御器(評価関数更新部、等式制約変数更新部、経路探索部、拡張ラグランジュ未定乗数更新部、極値化部)、10は評価関数更新部、11は等式制約変数更新部、12は経路探索部、を示す。
図面
【図1A】
0
【図1B】
1
【図2】
2
【図3】
3
【図4】
4
【図5】
5
【図6】
6
【図7】
7