(57)【特許請求の範囲】
【請求項１】
制御対象に対応する制御モデルと、
前記制御モデルから出力される第１の出力信号の目標値と、前記制御モデルに仮想外乱を入力させた状態で前記制御モデルから出力された第２の出力信号と、に基づいて、前記制御モデルから出力された前記第１の出力信号が前記目標値となるような制御信号を生成して前記制御モデルに出力する制御モデル用コントローラと、
前記制御モデルから出力された前記第１の出力信号を、前記制御対象を制御する制御対象用コントローラに出力するための出力信号出力部と、
を備えたフィードバック制御シミュレーション装置と、
前記フィードバック制御シミュレーション装置の前記制御モデルから出力された前記第１の出力信号と、前記制御対象から出力されたフィードバック制御用出力信号であって前記制御対象から出力された制御対象出力信号及び前記制御対象出力信号以外の観測量を含むフィードバック制御用出力信号と、に基づいて、前記制御対象から出力された制御対象出力信号が前記制御モデルから出力された前記第１の出力信号に追従するような制御信号を生成して前記制御対象に出力することにより、前記制御対象を制御する制御対象用コントローラと、
を備えた制御装置。
【請求項２】
前記制御対象用コントローラが、前記制御対象の逆モデルである
請求項１記載の制御装置。
【請求項３】
前記フィードバック制御シミュレーション装置は、
前記制御モデル用コントローラから出力された前記制御信号を前記制御対象に出力するための制御信号出力部
を備えた請求項１又は請求項２記載の制御装置。
【請求項４】
前記フィードバック制御シミュレーション装置は、
前記仮想外乱に基づいて、前記制御モデルに仮想外乱を入力させた状態と等価な状態を得るために前記制御信号に加算する信号である等価仮想外乱を生成する等価仮想外乱生成部を備え、
前記制御信号出力部は、前記制御信号と前記等価仮想外乱とを加算した制御信号を前記制御対象に出力する
請求項３記載の制御装置。
【請求項５】
前記制御対象は移動体であり、前記目標値は前記移動体の目標軌道であり、前記仮想外乱は前記移動体が移動する際に障害となる障害物を仮想した外乱であり、前記第１の出力信号は前記障害物を回避した障害物回避軌道である
請求項１〜４の何れか１項に記載の制御装置。
【請求項６】
前記仮想外乱は、ポテンシャル関数から得られた外乱である
請求項５記載の制御装置。
【請求項７】
前記移動体は車両であり、前記ポテンシャル関数は、前記車両に斥力を与える関数である
請求項６記載の制御装置。
【請求項８】
前記ポテンシャル関数は、前記車両が進入する交差点入り口で前記斥力が最大となるように設定された関数である
請求項７記載の制御装置。
【請求項９】
前記移動体は２つのアームであり、互いのアームを前記障害物とみなして前記ポテンシャル関数を設定する
請求項６記載の制御装置。
【請求項１０】
コンピュータが、
制御対象に対応する制御モデルから出力される第１の出力信号の目標値と、前記制御モデルに仮想外乱を入力させた状態で前記制御モデルから出力された第２の出力信号と、に基づいて、前記制御モデルから出力された前記第１の出力信号が前記目標値となるような制御信号を生成して前記制御モデルに出力するステップと、
前記制御モデルから出力された前記第１の出力信号を、前記制御対象を制御する制御対象用コントローラに出力するステップと、
前記制御モデルから出力された前記第１の出力信号と、前記制御対象から出力されたフィードバック制御用出力信号であって前記制御対象から出力された制御対象出力信号及び前記制御対象出力信号以外の観測量を含むフィードバック制御用出力信号と、に基づいて、前記制御対象から出力された制御対象出力信号が前記制御モデルから出力された前記第１の出力信号に追従するような制御信号を生成して前記制御対象に出力することにより、前記制御対象を制御するステップと、
を含む制御方法。
【請求項１１】
コンピュータを、
請求項１〜９の何れか１項に記載の制御装置を構成する各手段として機能させるための制御プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、制御装置、制御方法、及び制御プログラムに関する。
【背景技術】
【０００２】
従来、出力の目標値と実際の出力値を比較して自動的に出力値と目標値が一致するように制御するフィードバック制御シミュレーション装置が知られている（例えば特許文献１、２参照）。また、出力に変動を起こさせるような外乱を予測し、これを前もって打ち消すように制御するフィードフォワード制御装置が知られている。さらに、フィードバック制御とフィードフォワード制御を組み合わせた装置が知られている（例えば特許文献３〜５参照）。
【０００３】
フィードバック制御やフィードフォワード制御は、様々な分野の技術に適用され、例えば車両の運転支援システムにも適用される。近年、運転支援システムでは、先行車追従や前方車両との衝突回避、車線維持などが実用化されている。このように、運転支援の適用範囲は拡がってきているが、事故のない運転の実現には到っていない。事故の発生原因には、危険の見落としや急な飛び出しに対する注意不足といったものがある。これらの原因に対処するには、広い範囲に複数存在する注意点を同時に考慮した運転を行う必要がある。このことから、複数の注意点を考慮するために、対処すべき要素を足し合わせることができる人工ポテンシャル法を用いた制御が考案されている。しかしながら、運転支援システムの最終目標といえる全自動運転を考える場合、経路計画を行う人工ポテンシャル法では、自由な経路や速度設定を行うのが困難である。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特表２００９−２１３３４号公報
【特許文献２】特表２００９−５２１７５１号公報
【特許文献３】特開２０１０−２８２６１８号公報
【特許文献４】特開２０１０−２１８００８号公報
【特許文献５】特開２０１４−１３６０７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
例えば従来では、まずポテンシャル法により障害物を回避するための軌道を計算するステップ、計算された軌道に制御対象を追従させるような動作（ハンドル操作量等）を決定するステップ、の２ステップの制御を行う場合が多く、計算が複雑である、という問題がある。
【０００６】
本発明は、複雑な計算をすることなく制御対象を精度良く制御するための制御信号を生成することができる制御装置、制御方法、及び制御プログラムを提供することを目的としている。
【課題を解決するための手段】
【０００７】
上記課題を解決するため、請求項１記載の発明の制御装置は、制御対象に対応する制御モデルと、前記制御モデルから出力される第１の出力信号の目標値と、前記制御モデルに仮想外乱を入力させた状態で前記制御モデルから出力された第２の出力信号と、に基づいて、前記制御モデルから出力された前記第１の出力信号が前記目標値となるような制御信号を生成して前記制御モデルに出力する制御モデル用コントローラと、前記制御モデルから出力された前記第１の出力信号を、前記制御対象を制御する制御対象用コントローラに出力するための出力信号出力部と、を備えたフィードバック制御シミュレーション装置と、前記フィードバック制御シミュレーション装置の前記制御モデルから出力された前記第１の出力信号と、前記制御対象から出力されたフィードバック制御用出力信号であって前記制御対象から出力された制御対象出力信号及び前記制御対象出力信号以外の観測量を含むフィードバック制御用出力信号と、に基づいて、前記制御対象から出力された制御対象出力信号が前記制御モデルから出力された前記第１の出力信号に追従するような制御信号を生成して前記制御対象に出力することにより、前記制御対象を制御する制御対象用コントローラと、を備える。
請求項２記載の発明は、前記制御対象用コントローラが、前記制御対象の逆モデルである。
【０００８】
請求項３記載の発明は、前記制御モデル用コントローラから出力された前記制御信号を前記制御対象に出力するための制御信号出力部を備える。
【０００９】
請求項４記載の発明は、前記仮想外乱に基づいて、前記制御モデルに仮想外乱を入力させた状態と等価な状態を得るために前記制御信号に加算する信号である等価仮想外乱を生成する等価仮想外乱生成部を備え、前記制御信号出力部は、前記制御信号と前記等価仮想外乱とを加算した制御信号を前記制御対象に出力する。
【００１１】
請求項５記載の発明は、前記制御対象は移動体であり、前記目標値は前記移動体の目標軌道であり、前記仮想外乱は前記移動体が移動する際に障害となる障害物を仮想した外乱であり、前記第１の出力信号は前記障害物を回避した障害物回避軌道である。
【００１２】
請求項６記載の発明は、前記仮想外乱は、ポテンシャル関数から得られた外乱である。
【００１３】
請求項７記載の発明は、前記移動体は車両であり、前記ポテンシャル関数は、前記車両に斥力を与える関数である。
【００１４】
請求項８記載の発明は、前記ポテンシャル関数は、前記車両が進入する交差点入り口で前記斥力が最大となるように設定された関数である。
【００１５】
請求項９記載の発明は、前記移動体は２つのアームであり、互いのアームを前記障害物とみなして前記ポテンシャル関数を設定する。
【００１８】
請求項１０記載の発明の制御方法は、コンピュータが、制御対象に対応する制御モデルから出力される第１の出力信号の目標値と、前記制御モデルに仮想外乱を入力させた状態で前記制御モデルから出力された第２の出力信号と、に基づいて、前記制御モデルから出力された前記第１の出力信号が前記目標値となるような制御信号を生成して前記制御モデルに出力するステップと、前記制御モデルから出力された前記第１の出力信号を、前記制御対象を制御する制御対象用コントローラに出力するステップと、前記制御モデルから出力された前記第１の出力信号と、前記制御対象から出力されたフィードバック制御用出力信号であって前記制御対象から出力された制御対象出力信号及び前記制御対象出力信号以外の観測量を含むフィードバック制御用出力信号と、に基づいて、前記制御対象から出力された制御対象出力信号が前記制御モデルから出力された前記第１の出力信号に追従するような制御信号を生成して前記制御対象に出力することにより、前記制御対象を制御するステップと、を含む。
【００１９】
請求項１１記載の発明の制御プログラムは、コンピュータを、請求項１〜９の何れか１項に記載の制御装置を構成する各手段として機能させるための制御プログラムである。
【発明の効果】
【００２０】
本発明によれば、複雑な計算をすることなく制御対象を精度良く制御するための制御信号を生成することができる、という効果を有する。
【図面の簡単な説明】
【００２１】
【図１】第１実施形態に係る制御装置１０のブロック図である。
【図２】フィードバック制御シミュレーション装置で実行されるフィードバック制御シミュレーション処理のフローチャートである。
【図３】第２実施形態に係るフィードバック制御シミュレーション装置のブロック図である。
【図４】第３実施形態に係るフィードバック制御シミュレーション装置のブロック図である。
【図５】第４実施形態に係るフィードバック制御シミュレーション装置のブロック図である。
【図６】障害物からの斥力について説明するための図である。
【図７】障害物の周りに発生するポテンシャル場について説明するための図である。
【図８】ポテンシャル関数について説明するためのグラフである。
【図９】駐車車両追い抜きのシミュレーション結果について説明するための図である。
【図１０】無信号交差点通過のシミュレーション結果について説明するための図である。
【図１１】第５実施形態に係るモジュールのコンセプトについて説明するための図である。
【図１２】第５実施形態に係るフィードバック制御シミュレーション装置のブロック図である。
【図１３】アームの衝突回避における偏差抑制入力について説明するための図である。
【図１４】アームの衝突回避について説明するための図である。
【図１５】ポテンシャル場の一例を示す図である。
【図１６】アームの衝突回避のシミュレーション結果を示す図である。
【図１７】アームの衝突回避のシミュレーション結果を示す図である。
【図１８】第６実施形態に係る制御装置１０のブロック図である。
【図１９】第７実施形態に係るフィードバック制御シミュレーション装置のブロック図である。
【図２０】第８実施形態に係る制御装置のブロック図である。
【図２１】第９実施形態に係る制御装置のブロック図である。
【発明を実施するための形態】
【００２２】
以下、図面を参照して本発明の実施の形態の一例について説明する。
【００２３】
（第１実施形態）
【００２４】
図１には、本実施形態に係る制御装置１０のブロック図を示した。図１に示すように、制御装置１０は、フィードバック制御シミュレーション装置１２及び制御対象用コントローラ１４を備える。
【００２５】
フィードバック制御シミュレーション装置１２は、制御モデル１６及び制御モデル用コントローラ１８を備える。
【００２６】
制御モデル１６は、制御対象２０に対応する制御モデル、すなわち制御対象２０のシミュレーションモデルである。
【００２７】
制御対象２０としては、例えば自動車、飛翔体（ドローン等）、産業用ロボット、ヒューマノイドロボット等の移動体が適用されるが、これに限られるものではない。本実施形態では、制御対象２０が移動体の場合について説明する。
【００２８】
制御モデル用コントローラ１８は、制御モデル１６から出力される第１の出力信号としての出力信号ｙＭの目標値ｒと、制御モデル１６に仮想外乱ｄｖを入力させた状態で制御モデル１６から出力されたフィードバック制御用の第２の出力信号としての出力信号ｙｍと、に基づいて、制御モデル１６から出力された出力信号ｙＭが目標値ｒに追従するような制御信号ｕＭを生成して制御モデル１６に出力する。なお、フィードバック制御用の出力信号ｙｍは、出力信号ｙＭだけでなく、出力信号ｙＭ以外の観測量も含む信号である。
【００２９】
すなわち、制御モデル用コントローラ１８は、制御対象２０のシミュレーションモデルである制御モデル１６に対してフィードバック制御を行い、出力信号ｙＭが目標値ｒを追従するような制御信号ｕＭを生成して制御モデル１６に出力する。
【００３０】
ここで、目標値ｒは、一例として移動体である制御対象２０の目標軌道である。また、仮想外乱ｄｖは、一例として制御対象２０が移動する際に障害となる障害物を仮想した外乱である。また、出力信号ｙＭは、障害物を回避した障害物回避軌道である。なお、仮想外乱は、ポテンシャル関数から得られた外乱を適用することができるが、制御対象２０が移動する際に障害となる障害物を仮想した外乱であれば、これに限られるものではない。
【００３１】
また、フィードバック制御シミュレーション装置１２は、制御モデル１６から出力された出力信号ｙＭを、制御対象２０を制御する制御対象用コントローラ１４に出力するための制御信号出力部２２を備える。
【００３２】
制御対象用コントローラ１４は、フィードバック制御シミュレーション装置１２から出力された出力信号ｙＭと、制御対象２０から出力されたフィードバック制御用の出力信号ｙｒと、に基づいて、制御対象２０から出力された出力信号ｙＲが制御モデル１６から出力された出力信号ｙＭに追従するような制御信号ｕＲを生成して制御対象２０に出力する。なお、フィードバック制御用の出力信号ｙｒは、出力信号ｙＲだけでなく、出力信号ｙＲ以外の観測量も含む信号である。
【００３３】
すなわち、制御対象用コントローラ１４は、制御モデル１６から出力された出力信号ｙＭを用いて制御対象２０に対してフィードバック制御を行う。
【００３４】
制御モデル用コントローラ１８は、例えばＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）、及び不揮発性メモリ等を含むコンピュータで構成することができる。この場合、後述するフィードバック制御シミュレーション処理をコンピュータに実行させるフィードバック制御シミュレーションプログラムを、例えばＲＯＭに記憶しておき、ＣＰＵにこれを読み込んで実行させる。なお、フィードバック制御シミュレーションプログラムは、ＣＤ−ＲＯＭやＤＶＤ−ＲＯＭ等の記録媒体により提供するようにしてもよい。
【００３５】
次に、制御モデル用コントローラ１８において実行されるフィードバック制御シミュレーション処理を図２に示すフローチャートを参照して説明する。
【００３６】
まず、ステップＳ１００では、制御モデル１６から出力される出力信号ｙＭの目標値ｒを入力する。
【００３７】
ステップＳ１０２では、制御モデル１６に仮想外乱ｄｖを入力させた状態で制御モデル１６から出力されたフィードバック制御用の出力信号ｙｍを入力する。
【００３８】
ステップＳ１０４では、ステップＳ１００で入力した目標値ｒと、ステップＳ１０２で入力した出力信号ｙｍと、に基づいて、出力信号ｙＭが目標値ｒを追従するような制御信号ｕＭを生成して制御モデル１６に出力する。
【００３９】
これにより、制御モデル１６は、制御信号ｕＭに応じた出力信号ｙＭを制御対象用コントローラ１４に出力する。そして、制御対象用コントローラ１４は、制御モデル１６から出力された出力信号ｙＭと、制御対象２０から出力されたフィードバック制御用の出力信号ｙｒと、に基づいて、制御対象２０から出力された出力信号ｙＲが制御モデル１６から出力された出力信号ｙＭに追従するような制御信号ｕＲを生成して制御対象２０に出力する。
【００４０】
このように、制御対象２０は、シミュレーションによるフィードバック制御によって生成された出力信号ｙＭに基づいてフィードバック制御される。
【００４１】
制御モデル１６が制御対象２０に精度良く対応していれば、仮想外乱ｄｖが無い場合は、実際の制御対象２０の出力信号ｙＲを目標値ｒに精度良く追従させることができる。また、適切な仮想外乱ｄｖを与えることにより、移動体を障害物から回避させるための軌道である出力信号ｙＭがフィードバック制御シミュレーションにより自動的に計算され、移動体である制御対象２０の軌道である出力信号ｙＲがこれに追従することにより、前記移動体は精度よく障害物を回避できる。シミュレーション環境は、フィードバック制御にとって理想的な環境であるため、仮想外乱ｄｖを与えてもフィードバック制御シミュレーションは一般に安定である。
【００４２】
（第２実施形態）
【００４３】
次に、本発明の第２実施形態について説明する。なお、第１実施形態と同一部分には同一符号を付し、その詳細な説明は省略する。
【００４４】
図３には、本実施形態に係るフィードバック制御シミュレーション装置１２Ａのブロック図を示した。図３のフィードバック制御シミュレーション装置１２Ａが図１のフィードバック制御シミュレーション装置１２と異なるのは、制御モデル用コントローラ１８から出力された制御信号ｕＭを制御対象２０に出力するための出力信号出力部２４を備えている点、制御モデル１６から出力された出力信号ｙＭを制御対象用コントローラ１４に出力するための制御信号出力部２２が省略されている点である。その他の構成は図１のフィードバック制御シミュレーション装置１２と同様なので、詳細な説明は省略する。
【００４５】
また、出力信号ｙＭを制御対象用コントローラ１４に出力するための制御信号出力部２２が省略されているため、図３の構成では、制御対象用コントローラ１４も省略されている。
【００４６】
このように、本実施形態では、シミュレーションによるフィードバック制御によって制御モデル用コントローラ１８により生成された制御信号ｕＭが制御対象２０に出力される。すなわち、制御対象２０は、制御モデル用コントローラ１８により生成された制御信号ｕＭに基づいてフィードフォワード制御されることとなる。
【００４７】
制御対象２０が例えば自動運転が可能な自動車である場合、制御信号ｕＭは、例えば自動車の向きを操作するハンドルのハンドル操作量とすることができる。この場合、従来のように障害物を回避するための軌道を計算してから、計算された軌道に制御対象を追従させるためのハンドル操作量を決定する等の複雑な計算をする必要がなく、シミュレーションによるフィードバック制御により自動車が障害物に衝突するのを回避することが可能なハンドル操作量を少ない計算量でダイレクトに求めることができる。
【００４８】
（第３実施形態）
【００４９】
次に、本発明の第３実施形態について説明する。なお、第１実施形態及び第２実施形態と同一部分には同一符号を付し、その詳細な説明は省略する。
【００５０】
図４には、本実施形態に係るフィードバック制御シミュレーション装置１２Ｂのブロック図を示した。図４のフィードバック制御シミュレーション装置１２Ｂは、図１のフィードバック制御シミュレーション装置１２と、図３のフィードバック制御シミュレーション装置１２Ａと、を組み合わせた装置である。すなわち、フィードバック制御シミュレーション装置１２Ｂは、制御モデル１６から出力された出力信号ｙＭを制御対象用コントローラ１４に出力するための制御信号出力部２２と、制御モデル用コントローラ１８から出力された制御信号ｕＭを制御対象２０に出力するための出力信号出力部２４と、を備えている。
【００５１】
そして、制御モデル用コントローラ１８から出力された制御信号ｕＭと、制御対象用コントローラ１４から出力された制御信号ｕＲと、が加算されて制御対象２０に出力される。
【００５２】
このように、本実施形態では、シミュレーションによるフィードバック制御によって生成された出力信号ｙＭが制御対象用コントローラ１４に入力され、制御対象用コントローラ１４から出力された制御信号ｕＲと、制御対象用コントローラ１４により生成された制御信号ｕＭとが加算された制御信号により制御対象２０が制御されるため、制御対象２０の出力信号ｙＲを出力信号ｙＭに精度良く追従させることができる。
【００５３】
（第４実施形態）
【００５４】
次に、本発明の第４実施形態について説明する。第４実施形態では、障害物回避を考慮した自動運転制御システムに本発明を適用した場合について説明する。
【００５５】
図５には、本実施形態に係るフィードバック制御シミュレーション装置３０のブロック図を示した。なお、図５の構成は、第３実施形態で説明した図４の構成に相当する。
【００５６】
以下では、「ＰＤ」を制御モデル用コントローラＰＤと称し、「ＰＭ」を制御モデルＰＭと称し、「ＰＩＤ」を制御対象用コントローラＰＩＤと称し、「ＰＲ」を制御対象ＰＲと称する。
【００５７】
図５に示すように、目標値決定部３２から出力された目標値ｒから、制御モデルＰＭから出力された出力信号ｑ−（以下では、ｑの上付きバーを便宜上このように記載する）を減算した信号が偏差として制御モデルＰＤに出力される。なお、制御モデル用コントローラＰＤと、目標値ｒから出力信号ｑ−を減算する部分と、が図４の制御モデル用コントローラ１８に相当する。
【００５８】
制御モデル用コントローラＰＤから出力された制御信号は、仮想外乱発生部３４から出力された仮想外乱ｄｖと加算されて座標変換部３６に出力される。座標変換部３６により座標変換された制御信号ｕ−（以下では、ｕの上付きバーを便宜上このように記載する）は、制御モデルＰＭに出力される。
【００５９】
制御対象ＰＲは、制御信号ｕによって制御される。制御対象ＰＲから出力された出力信号には、外乱ｄが加算されて出力信号ｑとして出力される。なお、制御対象ＰＲは、図４の制御対象２０に相当する。
【００６０】
制御対象用コントローラＰＩＤは、出力信号ｑ−から出力信号ｑを減算した信号に基づいて制御信号を生成し、座標変換部３８に出力する。座標変換部３８で座標変換された制御信号は、フィードバック制御シミュレーション装置３０の座標変換部３６から出力された制御信号ｕ−と加算されて制御信号ｕとして制御対象ＰＲに出力される。なお、制御対象用コントローラＰＩＤと、出力信号ｑ−から出力信号ｑを減算する部分と、が図４の制御対象用コントローラ１４に相当する。
【００６１】
ここで、制御対象ＰＲは実車両、制御モデルＰＭは実車両に対応するシミュレーションモデルである。従って、目標値ｒは実車両の位置及び速度を表す目標値であり、制御信号ｕは実車両の駆動制動力及び舵角を制御する制御信号であり、出力信号ｑは実車両の位置及び速度を表す出力信号である。なお、以下では、制御モデルＰＭを単に車両と称する場合がある。
【００６２】
仮想外乱発生部３４は、シミュレーション上で走行する車両に障害物からの斥力を与えるための仮想外乱ｄｖを出力する。仮想外乱ｄｖ、すなわち障害物からの斥力は、予め設定したポテンシャル関数から計算することができ、障害物が複数存在する場合は、それぞれの障害物からの斥力の和をとる。ポテンシャル関数をＵ、位置をｑ＾とおくと、力Ｆ＾は下記（１）式のように表すことができる。なお、数式内のボールド表示の記号はベクトルであることを表しており、本文中では該当の記号に「＾」を付して表している。
【００６３】
【数１】
0006664807.tif 000002
・・・（１）
【００６４】
制御モデル用コントローラＰＤには、経路を辿らせつつ仮想外乱ｄｖの影響を受けるものを用いる。実車両である制御対象ＰＲには、フィードバック制御シミュレーション装置３０において計算された制御信号ｕ−に基づく制御信号ｕが入力されるため、制御対象用コントローラＰＩＤには、外乱ｄや制御モデルＰＭの誤差を抑えることを目的としたものを用いる。図６には、目標値及び制御モデルＰＭに加える力を決定する際の決定方法を示した。障害物４０からの斥力、すなわちポテンシャル関数による斥力４２（Ｆｏｒｃｅ  ｏｆ  ｐｏｔｅｎｔｉａｌ）の影響で車両４４が目標位置４６から遅れることを防ぐため、車両４４が目標経路４８に沿った方向に進んだ距離だけ目標位置を目標経路４８上で進ませる。そして、その目標位置４６に設定された目標速度を車両４４の目標速度とする。次に、目標値、すなわち目標位置及び目標速度と車両４４の位置及び速度との偏差からＰＤ制御（比例微分制御）により、目標経路４８を維持する力５０（Ｃｏｕｒｃｅ  ｋｅｅｐｉｎｇ  ｆｏｒｃｅ）を計算する。次に、ポテンシャル関数からの斥力４２を加える。そして、これらの力を足し合わせ、足し合わせた力を制御モデルＰＭの座標に変換し、前方向を駆動力、横方向の力をコーナリングパワーで除算したものを舵角とし、これらを制御モデルＰＭ及び制御対象ＰＲに出力する。
【００６５】
次に、駐車車両や歩行者を追い抜く場合の例について説明する。
【００６６】
道の端に車が駐車しているとき、安全な距離を置いて追い抜く運転が求められる。また、駐車車両の影からの飛び出しを考慮した減速が必要になる。歩行者がいる場合においても同様に、安全な距離での追い抜きと飛び出しを考慮した減速を行う必要がある。そこで、障害物から離れる向きに力を加えるポテンシャル関数を考える。下記（２）式に、障害物に近いほど大きくなるポテンシャル関数を示した。ここで、Ｋは障害物までの距離、Ａ、αは定数である。
【００６７】
【数２】
0006664807.tif 000003
・・・（２）
【００６８】
上記（２）式のポテンシャル関数では、車両が近くに存在する場合にのみ大きな斥力をかけるため、安全な距離を保つことはできるものの、離れた場所から近づく場合に、早い段階から回避行動を起こすことが困難である。そこで、図７に示すように、障害物４０を車両の幅だけ大きくした長方形とし、車両の進行方向である図７において右方向と反対向き、すなわち左向きに突起したエッジ５２を取り付ける。図７は道路を上から見た図であり、車両は図７において右方向に進んでおり、車両と接触する位置に障害物４０があるものとする。この場合、上記（２）式のポテンシャル関数を用いると、図７のような等高線でポテンシャル場５４が描ける。ポテンシャル場５４は内側に行くほど対数的に大きくなる。このようなポテンシャル関数を用いることにより、障害物４０に近づくほど大きな斥力が車両にかかるようになるが、このままでは急ブレーキがかかるようになってしまう。そこで、障害物４０との相対速度が大きくなるほど、障害物４０に近づいた位置を仮の位置としてポテンシャル関数を計算する。その結果、ポテンシャル場５４は図７のようになり、早い段階から障害物を回避することができる。
【００６９】
具体的には、エッジ５２の突起側に車両がある場合、障害物４０との相対速度に応じて小さくなる擬距離を用いる。擬距離は、位置ｑ＾＝［Ｘ  Ｙ］を下記（３）式と置き換えた場合における障害物４０との距離である。このとき、車両の進行方向は図７においてＸ軸の方向であり、Ｖは車両の速度、Ｖｏｂは障害物の速度、Ｘｏｂは車両進行方向における障害物４０の位置、βは定数である。
【００７０】
【数３】
0006664807.tif 000004
・・・（３）
【００７１】
擬距離を用いることにより、障害物４０に早く近づくほど障害物４０とより近くにあった場合におけるポテンシャル関数の値が適用される。
【００７２】
図７では、上記（２）、（３）式から、車両が４０ｋｍ／ｈで走行中の場合における障害物４０のポテンシャル関数が表すポテンシャル場５４を、障害物４０から離間するに従って１０５から対数的に減らしていった等高線で表している。
【００７３】
次に、無信号交差点を通過する場合の例について説明する。
【００７４】
無信号交差点の優先側について考える。無信号交差点では歩行者の急な飛び出しがあったときにおいても停止することができるように、交差点に入る前に減速を行う必要がある。そのため、ポテンシャル関数によって交差点に入るまでに制御モデルＰＭに進行方向と逆向きの力を加えることを考える。無信号交差点におけるポテンシャル関数の式を下記（４）式に示した。また、図８には、制限速度が２５ｋｍ／ｈの場合における下記（４）式をグラフで表したものを示した。ここで、速度の比例ゲインをＰＶ、目標速度をＶｒ、道幅をｗ、交差点の中心をＸｕ、交差点に入るときに求められる最小速度をＶｍ、Ｘｕ通過時の速度をＶＸ＝Ｘｕとする。図８では、交差点の中心Ｘｕ＝０としている。また、ａは定数である。
【００７５】
【数４】
0006664807.tif 000005
・・・（４）
【００７６】
このポテンシャル関数は、力の最大値が目標速度に比例した距離だけ交差点入り口より前になる。こうすることで、交差点入り口付近で最小速度とすることができる。また、このポテンシャルにおいても擬距離を用いて、車両の速度が速い場合において早くから減速できるようにする。
【００７７】
次に、駐車車両を追い抜く場合のシミュレーションについて説明する。
【００７８】
駐車車両がある場合において、図５のフィードバック制御シミュレーション装置３０において実行されるシミュレーション部分のみについて検証した結果を図９に示した。図９に示すように、車両の軌道が、目標軌道５６（Ｒｅｆｅｒｅｎｃｅ  ｃｏｕｒｃｅ）上に存在する障害物４０としての駐車車両を回避した回避軌道５８（Ｓｉｍｕｌａｔｉｏｎ）となっていることが判る。このことから、本実施形態に係るフィードバック制御シミュレーション装置３０と仮想外乱としてポテンシャル関数を用いることにより、追い抜き運転が可能であることが確認された。
【００７９】
次に、無信号交差点を通過する場合のシミュレーションについて説明する。
【００８０】
無信号交差点を通過する場合において、図５のフィードバック制御シミュレーション装置３０において実行されるシミュレーション部分のみについて検証した結果を図１０に示した。
【００８１】
図１０は、交差点の中心Ｘｕ＝１００とした場合のシミュレーション結果（Ｓｉｍｕｌａｔｉｏｎ）の他に、比較対象として、道路の制限速度を目標速度とした結果（Ｒｅｇｕｌａｔｉｏｎ  ｓｐｅｅｄ）、実際の人間がドライバーの場合の運転データ（Ｄｒｉｖｉｎｇ  ｄａｔａ）を示した。
【００８２】
なお、運転データは、３１個のデータから、対向車両を考慮した減速があるデータ、減速の少ないデータを除いた５個のデータの平均値を用いた。
【００８３】
図１０に示すように、シミュレーション結果（Ｓｉｍｕｌａｔｉｏｎ）は、実際の運転データ（Ｄｒｉｖｉｎｇ  ｄａｔａ）と同様の減速が再現できていることが判る。加速時に差が見られるが、このときはポテンシャル関数からの力が０であることから、速度維持のためのＰＤ制御が、人間が運転する際の加速動作を再現できていないためであると考えられる。
【００８４】
このように、ポテンシャル関数によりシミュレーション上で車両に力を加える自動運転制御システムに本発明を適用可能であることが判った。また、障害物の追い抜きや無信号交差点の通過時におけるポテンシャル関数を設定し、シミュレーションによってその有効性が確認できた。
【００８５】
なお、本実施形態では、第３実施形態で説明した形態を障害物回避を考慮した自動運転制御に適用した場合の具体例について説明したが、図５において、制御対象用コントローラＰＩＤ及び座標変換部３８を含む制御部３９を省略した構成としてもよい。この場合、第２実施形態で説明した形態を自動運転制御に適用した構成となる。
【００８６】
（第５実施形態）
【００８７】
次に、本発明の第５実施形態について説明する。第５実施形態では、モジュール化された多体システムの衝突回避制御に本発明を適用した場合の具体例について説明する。
【００８８】
まず、本実施形態に係るモジュールのコンセプトについて説明する。
【００８９】
コンセプトの概要を図１１に示した。ハードウェアのモジュールに対して、それを動作させるための動力学モデルやコントローラをソフトウェア上でモジュール化してハードウェアに与え、ハードウェアとソフトウェアを併せて１つのモジュールとして扱う。このモジュールをＭｏｄｅｌ−Ｃｏｎｔａｉｎｅｄ  Ｍｏｄｕｌｅ（ＭＣＭ）と定義する。以下では、このモジュールをＭＣＭと称する。
【００９０】
図１１では、回転ジョイント（Ｒｏｔａｔｉｏｎａｌ  ｊｏｉｎｔ）のモジュールをＭＣＭ１としている。ＭＣＭ１は、回転ジョイントのハードウェアＨＲ、ハードウェアＨＲをモデル化したモデルＰＲ、モデルＰＲを制御するコントローラＣＲをモジュール化したものである。
【００９１】
同様に、ＭＣＭ２は、直動ジョイント（Ｌｉｎｅａｒ  ｊｏｉｎｔ）のハードウェアＨＬ、ハードウェアＨＬをモデル化したモデルＰＬ、モデルＰＬを制御するコントローラＣＬをモジュール化したものである。そして、ＭＣＭ１及びＭＣＭ２を結合することにより、ハードウェアＨＲ、ＨＬを組み合わせたハードウェアが構成されると共に、モデルＰＲ、ＰＬを組み合わせたソフトウェアが構成される。
【００９２】
ＭＣＭでハードウェアを構成した場合、ソフトウェア上でもモジュールを結合することによって、ハードウェアに対応した動力学モデルやコントローラを得ることができる。これにより、モデル作成や制御器設計の工程が簡略化される。本実施形態では、ＭＣＭの検証用に回転ジョイントと直動ジョイントのハードウェアを作成し、専用の動力学モデルを与えてＭＣＭとして扱う。
【００９３】
次に、ＭＣＭに与えるモデルについて説明する。
【００９４】
従来、ＭＣＭに逆動力学モデルを与えて制御を行う方法が知られているが、本実施形態では、ＭＣＭのコンセプトの拡張性を検証するために、ポテンシャル法を用いた制御器をＭＣＭに与えて制御を行う。ポテンシャル法は、障害物にポテンシャル関数を持たせ、ポテンシャル関数の勾配に応じた反発力を仮想空間で加えることにより衝突回避を考慮した経路設計を行う手法である。本実施形態では、ＭＣＭで２つのアームを構成し、互いに障害物とみなして扱う。ＭＣＭに与えるポテンシャル関数Ｕを下記（５）式に示す。
【００９５】
【数５】
0006664807.tif 000006
・・・（５）
【００９６】
上記（５）式におけるｒは、ＭＣＭ同士の距離、ａはポテンシャル場の勾配を調整する定数である。ＭＣＭ同士の距離が小さくなるほどポテンシャル関数の勾配が大きくなる。反発力ｆ＾は、上記（５）式の勾配である下記（６）式で表される。
【００９７】
【数６】
0006664807.tif 000007
・・・（６）
【００９８】
また、上記（５）式のＡを下記（７）式のように定義することにより、ＭＣＭ同士の相対速度ｖｒに応じてポテンシャル関数の勾配を変化させる。ここで、Ｂはポテンシャル場の勾配を調整する定数、ｃは相対速度の影響を調整する定数である。
【００９９】
【数７】
0006664807.tif 000008
・・・（７）
【０１００】
ＭＣＭが高速で近づくとポテンシャル場が大きくなり、より早い段階で減速が始まる。また、互いに静止している場合はポテンシャル場が小さくなり、影響が軽減される。
【０１０１】
次に、本実施形態に係るフィードバック制御シミュレーション装置の構成について説明する。
【０１０２】
図１２には、本実施形態に係るフィードバック制御シミュレーション装置７０のブロック図を示した。なお、図１２の構成は、第１実施形態で説明した図１のフィードバック制御シミュレーション装置１２に相当する。以下では、「ＰＤ」を制御モデル用コントローラＰＤと称し、「Ｐ」を制御モデルＰと称する。
【０１０３】
一般的なポテンシャル法は、目的地に引力ポテンシャルを与えることで自動的に経路設計をする。ロボットアームではユーザが所望する軌道を自由に設計して追従させることを目的とすることが多い。そのため、自動で経路を決定してしまう引力ポテンシャルとの相性が悪い。そこで、本実施形態では、ユーザが自らアームの目標関節角度θｉｎを与えて軌道を生成し、逆動力学モデルＰ−１でアームの駆動に必要なトルクτを求めてプラントモデルである制御モデルＰに出力する。制御モデルＰからはアームの出力関節角度θｏｕｔが出力される。制御モデル用コントローラＰＤは、出力関節角度θｏｕｔと目標関節角度θｉｎとの偏差に基づいてＰＤ制御を行うことにより制御モデルＰを制御する。このように、２自由度系で制御を行う。また、制御モデル用コントローラＰＤから出力されたトルクτに、ポテンシャル場から発生する外力ｆをトルク変換器で変換した仮想外乱ｄｖを加えてプラントモデルＰに与えることにより、衝突回避を考慮した動作の補正を行う構成としている。ここで、制御モデル用コントローラＰＤ及び逆動力学モデルＰ−１が、図１の制御モデル用コントローラ１８に相当する。また、制御モデルＰは図１の制御モデル１６に相当する。
【０１０４】
なお、出力関節角度θｏｕｔを実機のプラントを制御するコントローラに入力することにより第１実施形態と同様の形態となり、トルクτを実機のプラントに入力することにより第２実施形態と同様の形態となり、これらの形態を組み合わせることで第３実施形態と同様の形態となる。
【０１０５】
次に、出力関節角度θｏｕｔと目標関節角度θｉｎとの偏差を抑制するための偏差抑制入力について説明する。
【０１０６】
例えばアームの先端の目標軌道を、図１３に示すような開始点Ｓｔａｒｔから終了点Ｇｏａｌまでの目標軌道７２とした場合、障害物４０のポテンシャル場５４からの外力Ｆにより出力関節角度θｏｕｔと目標関節角度θｉｎとの偏差が拡大し過ぎる場合がある。この偏差を制御モデル用コントローラＰＤのＰＤ制御により修正する場合、偏差が大きいために制御モデルＰに過度なトルクτが入力され、オーバーシュートが起こり得る。
【０１０７】
そこで、偏差の拡大を防ぐ方法として、偏差に応じて、目標関節角度θｉｎの進行速度を遅延させる。目標関節角度θｉｎを生成する際に使用する５次多項式補間で用いる多項式を下記（８）式のように設定する。ここで、θ０は初期角度、θｆは最終角度、ｔ０は開始時間、ｔｆは終了時間、Δｔはサンプリング時間、ｄは定数である。
【０１０８】
【数８】
0006664807.tif 000009
・・・（８）
【０１０９】
上記（８）式の変数ｔを目標値誤差ｅ、すなわち偏差ｅの大きさに応じて、入力する目標関節角度θｉｎの進行速度を変える。これにより、偏差ｅが大きい場合は目標関節角度θｉｎの進行速度が遅くなり、偏差ｅが無い場合は目標関節角度θｉｎの進行速度が元の速度となるようにする。これにより、オーバーシュートが抑制される。
【０１１０】
次に、シミュレーション結果について説明する。
【０１１１】
前述した相対速度によって変化するポテンシャル場と偏差抑制入力を組み合わせて、減速しながら衝突回避動作をし、オーバーシュートが軽減されるか否かを検証するために２  つのシミュレーションを行った。
【０１１２】
第１のシミュレーションでは、上記（５）式の係数Ａを定数にした変化しないポテンシャル場を用いてシミュレーションを行った。また、第２のシミュレーションでは、上記（７）式の係数Ａを相対速度によって変化するポテンシャル場と偏差抑制入力とを組み合わせてシミュレーションを行った。
【０１１３】
図１４に示すように、回転ジョイント７４を３つ組み合わせた２本のアーム７６Ａ、７６Ｂを交差させた場合における衝突回避動作をシミュレーションした。
【０１１４】
回転ジョイント７４は、ＭＣＭの検証用として製作した実機のパラメータを用いた。ポテンシャル関数を与えたアーム１つ分のポテンシャル場は図１５のようになった。
【０１１５】
第１、第２のシミュレーションの結果を図１６、１７に示した。図中の小さい番号から順にアーム７６Ａ、７６Ｂは動作する。ただし、７番より大きい番号の記載は省略している。また、左のアーム７６Ｂの先端の最高速度とアーム７６Ａ、７６Ｂ間の最短距離を以下の表１に示した。
【０１１６】
【表１】
0006664807.tif 000010
【０１１７】
図１６、１７に示すように、どちらも衝突せずに初期姿勢から目標姿勢に移動できている。また、上記表１に示すように、衝突回避動作時の最高速度は第２のシミュレーションの方が遅い。これは、相対速度によってポテンシャル場が大きくなり、アームが早めの段階で外力を受けたからである。また、表１のアーム間の最短距離は第２のシミュレーションの方が大きい。これは、相対速度に応じて変化するポテンシャル場による減速と偏差抑制入力による適度なＰＤ制御によるものである。さらに、ＰＤ制御が適度に作用しているので、目標姿勢に注目すると図１７は、図１６に比べてオーバーシュートが抑えられていることが分かる。以上のことから、アーム同士の間隔を広く保ちつつ、かつ、減速しながら衝突回避動作を行うことができることが判った。
【０１１８】
このように、相対速度によって変化するポテンシャル場と偏差によって進行速度が変化する偏差抑制入力を組み合わせることにより、シミュレーション上で良好な衝突回避動作が可能であることが確認できた。
【０１１９】
（第６実施形態）
【０１２０】
次に、本発明の第６実施形態について説明する。なお、第１実施形態と同一部分には同一符号を付し、その詳細な説明は省略する。
【０１２１】
図１８には、本実施形態に係る制御装置１０Ａのブロック図を示した。図１８の制御装置１０Ａが図１の制御装置１０と異なるのは、制御対象２０を制御する制御対象用コントローラが、制御対象の逆モデル１４Ａで構成されている点である。その他の構成は図１のフィードバック制御シミュレーション装置１２と同様なので、詳細な説明は省略する。
【０１２２】
このように、本実施形態では、制御対象の逆モデル１４Ａが制御対象２０を制御する。ここで、「制御対象の逆モデル」とは、制御対象からの出力信号（例えば制御対象２０からの出力信号ｙＲ）を入力すると、その出力信号が得られるような、制御対象へ入力される制御信号（例えば制御対象２０へ入力される制御信号ｕＲ）を出力するモデルである。精度の良い制御対象の逆モデルがあれば、外乱等が無い環境では、これをフィードフォワード的に用いることにより制御量を目標値に精度良く一致させることができる。すなわち、図１８の制御装置１０Ａは、図１の制御装置１０において実際に制御対象２０のフィードバック制御を行っている部分を、制御対象の逆モデルを用いたフィードフォワード制御で置き換えたものである。なお、制御対象の逆モデル１４Ａとしては、例えば制御モデル１６の逆モデルを用いることができるが、これに限られるものではない。
【０１２３】
（第７実施形態）
【０１２４】
次に、本発明の第７実施形態について説明する。なお、第２実施形態と同一部分には同一符号を付し、その詳細な説明は省略する。
【０１２５】
図１９には、本実施形態に係るフィードバック制御シミュレーション装置１２Ｃのブロック図を示した。
【０１２６】
図１９に示すように、本実施形態に係るフィードバック制御シミュレーション装置１２Ｃは、等価仮想外乱生成部６０を備えている。
【０１２７】
等価仮想外乱生成部６０には、仮想外乱ｄｖが入力される。そして、等価仮想外乱生成部６０は、入力された仮想外乱ｄｖに基づいて等価仮想外乱ｄｖ１を生成する。ここで、等価仮想外乱ｄｖ１は、制御モデル１６に仮想外乱ｄｖを入力させた状態と等価な状態を得るために、制御モデル用コントローラ１８から出力された制御信号ｕＭに加算する信号である。
【０１２８】
制御信号出力部２４は、制御モデル用コントローラ１８から出力された制御信号ｕＭに等価仮想外乱ｄｖ１を加算した制御信号ｕｍを制御モデル１６及び制御対象２０に出力する。
【０１２９】
このように、本実施形態では、シミュレーションによるフィードバック制御によって制御モデル用コントローラ１８により生成された制御信号ｕＭに等価仮想外乱ｄｖ１を加算した制御信号ｕｍが制御対象２０に出力される。すなわち、制御対象２０は、制御信号ｕｍに基づいてフィードフォワード制御されることとなる。
【０１３０】
制御対象２０が例えば自動運転が可能な自動車である場合、制御信号ｕｍは、例えば自動車の向きを操作するハンドルのハンドル操作量とすることができる。この場合、従来のように障害物を回避するための軌道を計算してから、計算された軌道に制御対象を追従させるためのハンドル操作量を決定する等の複雑な計算をする必要がなく、シミュレーションによるフィードバック制御により自動車が障害物に衝突するのを回避することが可能なハンドル操作量を少ない計算量でダイレクトに求めることができる。
【０１３１】
（第８実施形態）
【０１３２】
次に、本発明の第８実施形態について説明する。なお、第３実施形態及び第７実施形態と同一部分には同一符号を付し、その詳細な説明は省略する。
【０１３３】
図２０には、本実施形態に係る制御装置１０Ｂのブロック図を示した。図２０の制御装置１０Ｂは、図４に示す制御装置１０のフィードバック制御シミュレーション装置１２Ｂに代えて図１９のフィードバック制御シミュレーション装置１２Ｃが設けられた構成である。また、制御モデル１６から出力された出力信号ｙＭは、制御信号出力部２２から制御対象用コントローラ１４に出力される。
【０１３４】
制御対象用コントローラ１４は、制御モデル１６から出力された出力信号ｙＭと、制御対象２０から出力された第３の出力信号としての出力信号ｙｒと、に基づいて、制御信号ｕＲを生成して出力する。
【０１３５】
制御対象２０には、制御対象用コントローラ１４から出力された制御信号ｕＲと、制御モデル用コントローラ１８から出力された制御信号ｕＭに等価仮想外乱ｄｖ１を加算した制御信号ｕｍと、が加算された制御信号が入力される。
【０１３６】
このように、本実施形態では、シミュレーションによるフィードバック制御によって制御モデル用コントローラ１８により生成された制御信号ｕＭに等価仮想外乱ｄｖ１が加算された制御信号ｕｍが、制御対象用コントローラ１４から出力された制御信号ｕＲに加算されて制御対象２０に出力される。これにより、制御対象２０の出力信号ｙＲを出力信号ｙＭに精度良く追従させることができる。
【０１３７】
（第９実施形態）
【０１３８】
次に、本発明の第９実施形態について説明する。なお、第８実施形態と同一部分には同一符号を付し、その詳細な説明は省略する。
【０１３９】
図２１には、本実施形態に係る制御装置１０Ｃのブロック図を示した。図２１に示すように、制御装置１０Ｃは、等価仮想外乱生成部６０及び制御対象コントローラ１４を備えた構成である。
【０１４０】
このように、本実施形態では、制御対象用コントローラ１４から出力された制御信号ｕＲに等価仮想外乱生成部６０から出力された等価仮想外乱ｄｖ１が加算された制御信号が制御対象２０に出力される。これにより、制御対象２０を精度良く制御することができる。
【０１４１】
以上、本発明の実施形態について図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、本発明の要旨を逸脱しない範囲の設計変更等も含まれる。
【符号の説明】
【０１４２】
１０ 制御装置
１２、１２Ａ、１２Ｂ、３０、７０ フィードバック制御シミュレーション装置
１４ 制御対象用コントローラ
１６ 制御モデル
１８ 制御モデル用コントローラ
２０ 制御対象
２２ 制御信号出力部
２４ 出力信号出力部
７０ フィードバック制御シミュレーション装置
【図１】
0006664807.tif 000011
【図２】
0006664807.tif 000012
【図３】
0006664807.tif 000013
【図４】
0006664807.tif 000014
【図５】
0006664807.tif 000015
【図６】
0006664807.tif 000016
【図７】
0006664807.tif 000017
【図８】
0006664807.tif 000018
【図９】
0006664807.tif 000019
【図１０】
0006664807.tif 000020
【図１１】
0006664807.tif 000021
【図１２】
0006664807.tif 000022
【図１３】
0006664807.tif 000023
【図１４】
0006664807.tif 000024
【図１５】
0006664807.tif 000025
【図１６】
0006664807.tif 000026
【図１７】
0006664807.tif 000027
【図１８】
0006664807.tif 000028
【図１９】
0006664807.tif 000029
【図２０】
0006664807.tif 000030
【図２１】
0006664807.tif 000031