走行体の追従制御システム及びその制御方法
【課題】平面上を自由に走行する先導無人車に後続無人車を追従させる追従制御システムを提供する。
【解決手段】追従システム１は、第一走行体２０に配置され、第一走行体および第二走行体１０の位置と車体方向を特定する走行体位置検出部２と、前記車体方向から複数の進行角度を発生させる進行角度発生部３と、進行角度ごとに将来制御パラメータを計算し、第二走行体の将来位置と第一走行体との距離を計算する第一差異計算部４と、将来制御パラメータと現在制御パラメータの差異を計算する第二差異計算部５と、複数の進行角度の中から最適進行角を選定する最適進行角選定部６と、最適進行角選定部６で選定された制御パラメータを駆動機構１１に出力する走行指令部７で構成される。
【解決手段】追従システム１は、第一走行体２０に配置され、第一走行体および第二走行体１０の位置と車体方向を特定する走行体位置検出部２と、前記車体方向から複数の進行角度を発生させる進行角度発生部３と、進行角度ごとに将来制御パラメータを計算し、第二走行体の将来位置と第一走行体との距離を計算する第一差異計算部４と、将来制御パラメータと現在制御パラメータの差異を計算する第二差異計算部５と、複数の進行角度の中から最適進行角を選定する最適進行角選定部６と、最適進行角選定部６で選定された制御パラメータを駆動機構１１に出力する走行指令部７で構成される。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は自由走行する走行体を追従させる追従制御システムに係り、更に詳しくは非ホロノミック拘束を有する走行体のモデル予測制御に基づく自動追従制御システムに関する。
【背景技術】
【０００２】
二輪移動走行体による自動追従制御は電動車椅子の自律移動などへの応用が期待されているが、走行体の構造上、一般化座標の時間微分、即ち速度や加速度を含む積分不可能な微分方程式で表される非ホロノミック拘束を有するため、システムを平衡点に漸近安定化させる静的連続状態フィードバック則が存在しないことから、制御の困難さを伴うことが知られている。
【０００３】
上記問題点の解決方法として、例えば下記先行技術文献がある。
特許文献１には、ゴルフ場で使用される無人搬送車について、プレイヤーが携帯するコマンダーからの信号を受信し、発信源の方向、距離を測定し、追従制御によりプレイヤーを追従して軌道上を走行する無人搬送車の発明が開示されている。
特許文献２には、ドライバーの好みに応じて周辺車両との車間距離を確保しながら走行する自動車の追従制御装置の発明が開示されている。
また、非特許文献１には、先行車両に追従して安全距離を保持しながら走行する追従技術が開示されている。
しかし、これらの先行技術発明は何れも「配備された軌道上を走行する」、「車両が有人である」などの条件が付加されており、平面上を自由走行する無人車を自律的に追従できる無人車に関する追従制御システムの発明は、これまで開示されていない。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平０６−２６９５２４
【特許文献２】特開２００８−１８９０５５
【非特許文献】
【０００５】
【非特許文献１】IEEE Transactions onControl Systems Technology
【発明の概要】
【発明が解決しようとする課題】
【０００６】
平面上を自由に走行する先導無人車の後を、後続無人車に追従させるときの追従制御システムであって、非ホロノミック拘束を有する系に於いて、非ホロノミック拘束を考慮しながら、なおかつ最適性を保障できる新たな自動追従制御システムおよび、その制御方法を提供する。
【課題を解決するための手段】
【０００７】
請求項１に記載の本発明に係る追従制御システムは、平面上を自由に走行する第一走行体が存在する状況下で、第二走行体は、まず周知技術により第一走行体の位置情報を獲得する。
次いで、第二走行体への制御パラメータ情報が第一走行体の位置に基づいて作成され、第二走行体に指令として与えられる。
この場合、制御パラメータ情報として取り敢えず次の第一〜第三過程を経て複数のパラメータ候補が作成される。
第一過程：前記指令の与えられる時点を現在、前記指令が実行・完了する時点を将来と定義し、第二走行体に複数の進行角度を設定する。進行角度は第二走行体と第一走行体の現在位置により定まる角度に基づき、適宜の方法で複数種類を設定する。
第二過程：第二走行体の複数の進行角度毎に運動モデルを線形近似し、第二走行体の複数の将来制御パラメータ量を求める。
第三過程：前記複数の将来制御パラメータ量に基づき計算される第二走行体の複数の将来位置を求める。
【０００８】
上記過程を経て複数の将来制御パラメータと、この将来制御パラメータに基づき計算される将来位
置が求められる。
次いで、前記将来位置の計算結果を用い、（１式）で定義される第一誤差を複数の進行角度
毎に求める。
【数１】
これにより、複数の進行角度ごとに、複数の第一誤差が求まる。
次に、前記複数の進行角度の中から、（２式）で定義される評価関数を用いて最適進行角度の選定を
行う。
【数２】
この結果、両走行体間の距離を最小とする進行角度の制御パラメータが、最適条件として選定される。本発明によれば、複数設定される第二走行体の進行角度のうち、第一走行体に最も近接して追従する進行角度が常に選定され、第二走行体はこの条件を満たすように制御されるので、非ホロノミック拘束を有する系であったとしても、確実に第一走行体を追従することができる。
【０００９】
請求項２に記載の発明は、前記複数の進行角度の中から最適条件を選定するに際し、前記第一誤差に基づくだけでなく、次の第二誤差を含めて評価を行う。
第二誤差： 第二走行体の現在のパラメータ値 − 第二走行体の将来のパラメータ値
またこのときの評価関数は次の（３式）で表される。
【数３】
ただし、α、βは重み係数（０．０〜１．０）である。
ここに、第二誤差を評価する意味は次に基づく。
【００１０】
制御パラメータ値は、第二走行体に対する加速指令又は減速指令になるが、急激な加減速は走行体の走行を不安定にする。特に走行体が人を乗せる自動車椅子などでは、急加速、急ブレーキは人の乗り心地を著しく損ね、極端な場合人身事故に至ることも想定される。
評価関数（３式）のα、βは追従特性と走行安定性に対する重み付け係数であり、目的に応じ各係数を適宜調整可能である。従って、追従の正確性を重視する評価関数とすることも、また、走行安定性を重視する評価関数とすることもできる。
これにより、非ホロノミック拘束を有する系であったとしても、第二走行体は、第一走行体への確実な追従と、走行安定性確保の両条件を満たしながら追従走行できる。
【００１１】
本発明の追従制御システムは、本発明が適用可能な無人走行体であれば如何なる走行体にも適用できるが、請求項３に記載の発明は、少なくとも第二走行体が二輪の走行体であれば本発明が好適に適用される。即ち、左右２つの駆動輪を有する無人車を、自由走行する走行体に追従させて走行させることができる。ここに二輪の走行体とは、２つの制御駆動輪を装備する走行体であれば、駆動機能を有しない補助的に用いられる従導輪を幾つ装備していても２つの制御駆動輪を装備する走行体と見なせる。
【００１２】
請求項４に記載の発明は自由走行する第一走行体の後を追従させて、第二走行体を走行させる系に適用する追従制御方法に関する。また、本追従制御システムのコントローラから、第二走行体に走行指令が出されて追従制御されるが、このとき、走行指令の出される時点を現在とし、前記走行指令が実行・完了される時点を将来と定める。
追従制御では、まず追従制御システムのコントローラが自由走行する第一走行体の位置を検出・把握する。次いで、次の第一〜第三過程を経て複数の制御パラメータ候補と、これら制御パラメータから求まる第二走行体の将来位置が計算される。
第一過程： 前記第二走行体に複数の進行角度を設定する。
第二過程： 前記複数の進行角度に応じて定まる前記第二走行体の複数の将来制御パラメータ量を求める。
第三過程： 前記複数の将来制御パラメータ量に基づき求められる第二走行体の複数の将来位置を求める。
【００１３】
ここに、第三過程で求められた第二走行体の将来位置と第一走行体の現在位置との差異から
（１式）を用い、第一過程で設定した複数の進行角度毎に第一誤差を求める。次いで（２式）で定義される評価関数を用い最適条件を選定する。この結果、本制御方法により、複数の進行角度のうち常に第一走行体に最も近接して追従する進行角度が選定され、第二走行体はこの条件を満たすよう制御される。
なお、評価関数に（３式）を用いれば、追従特性と走行安定性に対する重み付け係数を自由に設定で
きるので、追従の正確性を重視する制御方法とすることも、また、走行安定性を重視する制御方法と
することも可能で、目的に応じた追従制御方法を設定できる。
【発明の効果】
【００１４】
本発明による追従制御システム及び追従制御方法によれば、第一走行車を追従する第二走行車を至近距離で追従するか、走行安定性を重視して追従するかについて、重みを付けて追従条件を設定できるので、用途に応じた制御形態をフレキシブルに設定することができる。また、これにより、非ホロノミック拘束を有する系であったとしても、第二走行体は、第一走行体への確実な追従と、走行安定性確保の両条件を満たしながら追従できる。
【図面の簡単な説明】
【００１５】
【図１】追従システムの構成説明図。
【図２】実験システムを示す概要図である。
【図３】本発明による実験結果である。
【図４】本発明による実験結果である。
【図５】本発明による実験結果である。
【図６】本発明によるシミュレーション結果である。
【図７】本発明によるシミュレーション結果である。
【図８】本発明によるシミュレーション結果である。
【発明を実施するための形態】
【００１６】
＜実施例１＞
本発明による実施例１の追従システムの構成を図１に示す。
追従システム１は、第２走行体１０に配置され、走行体位置検出部２と、進行角度発生部３と、第一誤差計算部４と第二誤計算部５と、最適進行角選定部６と、走行指令部７で構成される。
走行体位置検出部２はＧＰＳ、超音波センサ、赤外線センサ、ジャイロなどを用い、平面上の第２走行体（自車）の車体方向及び位置を認識し、次いで、ＣＣＤカメラ、超音波センサなどを用い第一走行体２０の存在を認識し、その平面上の位置を特定する。
次に、進行角度発生部３は第一走行体位置と第二走行体の車体方向とに基づいて、複数の進行角度を発生させる。進行角度は例えば第二走行体の車体方向を基準に、第一走行体位置と第二走行体を結ぶ方向に、等間隔に複数個発生させる。
【００１７】
その後、第一差異計算部４は進行角度ごとに将来制御パラメータを計算し、次いでこの将来制御パラメータを基に第二走行体の将来位置を計算する。更に第一差異計算部４は、（１式）による第二走行体の将来位置と第一第走行体の現在位置との距離を進行角度ごとに計算する。
一方第二差異計算部５は、将来制御パラメータと現在制御パラメータとの差異を進行角度毎に計算する。最適進行角選定部６は、（３式）に基づく評価式を用い、複数の進行角度の中か最適進行角を選定する。最適進行角選定部６で選定された制御パラメータは走行指令部に伝達され、追従システム１０からの走行指令として第２走行体１０の駆動機構１１に出力される。
【００１８】
＜実施例２＞
次に図２に示す実施例２の追従制御システムについて図１，２を用い説明する。
なお、実施例２の追従制御システムは、本発明技術の検証を行った検証実験システムでもある。
図２では本発明の追従制御システムが第二走行体に搭載されることなく、外部に取り出されて配置される。即ち、カメラ３１が図１に於ける走行体位置検出部１に該当し、図１に於ける進行角度発生部３、第一誤差計算部４、第二誤計算部５、最適進行角選定部６、走行指令部７は全てコントローラ３２に搭載される、また第二走行体３４への走行指令伝達は無線（Ｂｌｕｅｔｏｏｔｈ）で実行される。
実施例２について、まず図２に示すＳｔｅｐ１の説明を行う。
本方法では、移動走行体の数式モデルを利用して、未来の状態を考慮した最適な制御入力を導出する。（４式）は連続時間系における二輪移動走行体の数式モデルを示している。
【００１９】
【数４】
【００２０】
（４式）において、ｘ、ｙは、移動走行体の座標、θは進行方向とｘ軸のなす角、ｒは車輪半径、ｌは車輪間の距離、ωL、ωRは左右の車輪の回転角速度を表す。
（４式）を離散化し、L個の姿勢角θで線形近似し、それらをハイブリッド動的システムとして表現し、
（５式）と（６式）を用いると（７式）が得られる。
【数５】
【数６】
【数７】
数式２において、θｉは線形近似を行う平衡点、Lは線形近似を行った平衡点の数を表す。
数式３において、Ωｉは動作モードが切り替わる条件を表す。
【００２１】
次にＳｔｅｐ２について説明する。（８式）において、ある制御入力を与えると１ステップ未来の状態が求まる。この操作をもう一度繰り返すと（９式）に示すように２ステップ未来の状態が求まる。
【数８】
【数９】
【００２２】
したがって、あらかじめ予測ステップ数を決めておけば、上記繰り返し計算によりあらゆる入力系列に対する状態が導出可能である．そこで、次に得られた状態と所望の状態指令が最も近く、かつ１ステップ間の制御入力の変化量が大きくならないような入力系列を（１０式）に示す評価関数を用いて決定する。
【数１０】
（１０式）は予測する状態と指令値との誤差（Δｑ）の二乗和およびあるステップに対する入力とその１ステップ前の入力の誤差（Δｕ）の二乗和を予測毎に計算し加算する。この評価関数を用いることで、状態誤差を最小にしつつ、入力の変化量も最小化可能な制御入力を導出することができる。
【００２３】
[実施の形態１]
図２に本実施例の用いた装置を示す。二輪自律移動走行体を制御対象とした。ここで、制御対象の位置検出を行うために上空に取り付けたビデオカメラを使用した。
ここでは、先行する移動走行体を円軌道を描くように走行させ、制御対象をモデル予測制御を用いて追従させた場合に、制御対象の軌跡を図３に、そのときの制御入力および先行する走行体との距離を図４、図５に示す。なお、参考までに図６から図８にシミュレーション結果についても示す。
図６から図８より、先行する走行体の急な減速に対して、近づきすぎることなく指令値に追従しており、また1ステップ間の急激な入力の変化も抑制されている。
【００２４】
また、図３から図５より、シミュレーションと実験結果では脈動に違いが見られるものの、概ね一致しているとはいえ、この脈動の原因としては、観測誤差、摩擦や車輪のすべりにおけるモデル化誤差などが考えられる。
本手法は、移動走行体の数式モデルに基づき制御入力を決定する．したがって上記のモデル化誤差は直接制御性能の劣化につながるが、今後のさらなる研究により移動走行体の特性をより正確にモデル化できるようになれば、それに伴い本手法の制御性能は格段に上昇する。
【産業上の利用可能性】
【００２５】
本発明は、走行体の追従制御が必要とされる用途であれば分野を選ばず適用可能である。典型的にはコンテナヤードや倉庫などにおける貨物や荷物の整列収納や、福祉分野における電動椅子の追従走行などに好適に利用できる。
【符号の説明】
【００２６】
１ 追従制御システム
２ 走行体位置検出部
３ 進行角度発生部
４ 第一誤差計算部
５ 第二誤計算部
６ 最適進行角選定部
７ 走行指令部
１０ 第２走行体
１１ 駆動機構
２０ 第一走行体
【技術分野】
【０００１】
本発明は自由走行する走行体を追従させる追従制御システムに係り、更に詳しくは非ホロノミック拘束を有する走行体のモデル予測制御に基づく自動追従制御システムに関する。
【背景技術】
【０００２】
二輪移動走行体による自動追従制御は電動車椅子の自律移動などへの応用が期待されているが、走行体の構造上、一般化座標の時間微分、即ち速度や加速度を含む積分不可能な微分方程式で表される非ホロノミック拘束を有するため、システムを平衡点に漸近安定化させる静的連続状態フィードバック則が存在しないことから、制御の困難さを伴うことが知られている。
【０００３】
上記問題点の解決方法として、例えば下記先行技術文献がある。
特許文献１には、ゴルフ場で使用される無人搬送車について、プレイヤーが携帯するコマンダーからの信号を受信し、発信源の方向、距離を測定し、追従制御によりプレイヤーを追従して軌道上を走行する無人搬送車の発明が開示されている。
特許文献２には、ドライバーの好みに応じて周辺車両との車間距離を確保しながら走行する自動車の追従制御装置の発明が開示されている。
また、非特許文献１には、先行車両に追従して安全距離を保持しながら走行する追従技術が開示されている。
しかし、これらの先行技術発明は何れも「配備された軌道上を走行する」、「車両が有人である」などの条件が付加されており、平面上を自由走行する無人車を自律的に追従できる無人車に関する追従制御システムの発明は、これまで開示されていない。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平０６−２６９５２４
【特許文献２】特開２００８−１８９０５５
【非特許文献】
【０００５】
【非特許文献１】IEEE Transactions onControl Systems Technology
【発明の概要】
【発明が解決しようとする課題】
【０００６】
平面上を自由に走行する先導無人車の後を、後続無人車に追従させるときの追従制御システムであって、非ホロノミック拘束を有する系に於いて、非ホロノミック拘束を考慮しながら、なおかつ最適性を保障できる新たな自動追従制御システムおよび、その制御方法を提供する。
【課題を解決するための手段】
【０００７】
請求項１に記載の本発明に係る追従制御システムは、平面上を自由に走行する第一走行体が存在する状況下で、第二走行体は、まず周知技術により第一走行体の位置情報を獲得する。
次いで、第二走行体への制御パラメータ情報が第一走行体の位置に基づいて作成され、第二走行体に指令として与えられる。
この場合、制御パラメータ情報として取り敢えず次の第一〜第三過程を経て複数のパラメータ候補が作成される。
第一過程：前記指令の与えられる時点を現在、前記指令が実行・完了する時点を将来と定義し、第二走行体に複数の進行角度を設定する。進行角度は第二走行体と第一走行体の現在位置により定まる角度に基づき、適宜の方法で複数種類を設定する。
第二過程：第二走行体の複数の進行角度毎に運動モデルを線形近似し、第二走行体の複数の将来制御パラメータ量を求める。
第三過程：前記複数の将来制御パラメータ量に基づき計算される第二走行体の複数の将来位置を求める。
【０００８】
上記過程を経て複数の将来制御パラメータと、この将来制御パラメータに基づき計算される将来位
置が求められる。
次いで、前記将来位置の計算結果を用い、（１式）で定義される第一誤差を複数の進行角度
毎に求める。
【数１】
これにより、複数の進行角度ごとに、複数の第一誤差が求まる。
次に、前記複数の進行角度の中から、（２式）で定義される評価関数を用いて最適進行角度の選定を
行う。
【数２】
この結果、両走行体間の距離を最小とする進行角度の制御パラメータが、最適条件として選定される。本発明によれば、複数設定される第二走行体の進行角度のうち、第一走行体に最も近接して追従する進行角度が常に選定され、第二走行体はこの条件を満たすように制御されるので、非ホロノミック拘束を有する系であったとしても、確実に第一走行体を追従することができる。
【０００９】
請求項２に記載の発明は、前記複数の進行角度の中から最適条件を選定するに際し、前記第一誤差に基づくだけでなく、次の第二誤差を含めて評価を行う。
第二誤差： 第二走行体の現在のパラメータ値 − 第二走行体の将来のパラメータ値
またこのときの評価関数は次の（３式）で表される。
【数３】
ただし、α、βは重み係数（０．０〜１．０）である。
ここに、第二誤差を評価する意味は次に基づく。
【００１０】
制御パラメータ値は、第二走行体に対する加速指令又は減速指令になるが、急激な加減速は走行体の走行を不安定にする。特に走行体が人を乗せる自動車椅子などでは、急加速、急ブレーキは人の乗り心地を著しく損ね、極端な場合人身事故に至ることも想定される。
評価関数（３式）のα、βは追従特性と走行安定性に対する重み付け係数であり、目的に応じ各係数を適宜調整可能である。従って、追従の正確性を重視する評価関数とすることも、また、走行安定性を重視する評価関数とすることもできる。
これにより、非ホロノミック拘束を有する系であったとしても、第二走行体は、第一走行体への確実な追従と、走行安定性確保の両条件を満たしながら追従走行できる。
【００１１】
本発明の追従制御システムは、本発明が適用可能な無人走行体であれば如何なる走行体にも適用できるが、請求項３に記載の発明は、少なくとも第二走行体が二輪の走行体であれば本発明が好適に適用される。即ち、左右２つの駆動輪を有する無人車を、自由走行する走行体に追従させて走行させることができる。ここに二輪の走行体とは、２つの制御駆動輪を装備する走行体であれば、駆動機能を有しない補助的に用いられる従導輪を幾つ装備していても２つの制御駆動輪を装備する走行体と見なせる。
【００１２】
請求項４に記載の発明は自由走行する第一走行体の後を追従させて、第二走行体を走行させる系に適用する追従制御方法に関する。また、本追従制御システムのコントローラから、第二走行体に走行指令が出されて追従制御されるが、このとき、走行指令の出される時点を現在とし、前記走行指令が実行・完了される時点を将来と定める。
追従制御では、まず追従制御システムのコントローラが自由走行する第一走行体の位置を検出・把握する。次いで、次の第一〜第三過程を経て複数の制御パラメータ候補と、これら制御パラメータから求まる第二走行体の将来位置が計算される。
第一過程： 前記第二走行体に複数の進行角度を設定する。
第二過程： 前記複数の進行角度に応じて定まる前記第二走行体の複数の将来制御パラメータ量を求める。
第三過程： 前記複数の将来制御パラメータ量に基づき求められる第二走行体の複数の将来位置を求める。
【００１３】
ここに、第三過程で求められた第二走行体の将来位置と第一走行体の現在位置との差異から
（１式）を用い、第一過程で設定した複数の進行角度毎に第一誤差を求める。次いで（２式）で定義される評価関数を用い最適条件を選定する。この結果、本制御方法により、複数の進行角度のうち常に第一走行体に最も近接して追従する進行角度が選定され、第二走行体はこの条件を満たすよう制御される。
なお、評価関数に（３式）を用いれば、追従特性と走行安定性に対する重み付け係数を自由に設定で
きるので、追従の正確性を重視する制御方法とすることも、また、走行安定性を重視する制御方法と
することも可能で、目的に応じた追従制御方法を設定できる。
【発明の効果】
【００１４】
本発明による追従制御システム及び追従制御方法によれば、第一走行車を追従する第二走行車を至近距離で追従するか、走行安定性を重視して追従するかについて、重みを付けて追従条件を設定できるので、用途に応じた制御形態をフレキシブルに設定することができる。また、これにより、非ホロノミック拘束を有する系であったとしても、第二走行体は、第一走行体への確実な追従と、走行安定性確保の両条件を満たしながら追従できる。
【図面の簡単な説明】
【００１５】
【図１】追従システムの構成説明図。
【図２】実験システムを示す概要図である。
【図３】本発明による実験結果である。
【図４】本発明による実験結果である。
【図５】本発明による実験結果である。
【図６】本発明によるシミュレーション結果である。
【図７】本発明によるシミュレーション結果である。
【図８】本発明によるシミュレーション結果である。
【発明を実施するための形態】
【００１６】
＜実施例１＞
本発明による実施例１の追従システムの構成を図１に示す。
追従システム１は、第２走行体１０に配置され、走行体位置検出部２と、進行角度発生部３と、第一誤差計算部４と第二誤計算部５と、最適進行角選定部６と、走行指令部７で構成される。
走行体位置検出部２はＧＰＳ、超音波センサ、赤外線センサ、ジャイロなどを用い、平面上の第２走行体（自車）の車体方向及び位置を認識し、次いで、ＣＣＤカメラ、超音波センサなどを用い第一走行体２０の存在を認識し、その平面上の位置を特定する。
次に、進行角度発生部３は第一走行体位置と第二走行体の車体方向とに基づいて、複数の進行角度を発生させる。進行角度は例えば第二走行体の車体方向を基準に、第一走行体位置と第二走行体を結ぶ方向に、等間隔に複数個発生させる。
【００１７】
その後、第一差異計算部４は進行角度ごとに将来制御パラメータを計算し、次いでこの将来制御パラメータを基に第二走行体の将来位置を計算する。更に第一差異計算部４は、（１式）による第二走行体の将来位置と第一第走行体の現在位置との距離を進行角度ごとに計算する。
一方第二差異計算部５は、将来制御パラメータと現在制御パラメータとの差異を進行角度毎に計算する。最適進行角選定部６は、（３式）に基づく評価式を用い、複数の進行角度の中か最適進行角を選定する。最適進行角選定部６で選定された制御パラメータは走行指令部に伝達され、追従システム１０からの走行指令として第２走行体１０の駆動機構１１に出力される。
【００１８】
＜実施例２＞
次に図２に示す実施例２の追従制御システムについて図１，２を用い説明する。
なお、実施例２の追従制御システムは、本発明技術の検証を行った検証実験システムでもある。
図２では本発明の追従制御システムが第二走行体に搭載されることなく、外部に取り出されて配置される。即ち、カメラ３１が図１に於ける走行体位置検出部１に該当し、図１に於ける進行角度発生部３、第一誤差計算部４、第二誤計算部５、最適進行角選定部６、走行指令部７は全てコントローラ３２に搭載される、また第二走行体３４への走行指令伝達は無線（Ｂｌｕｅｔｏｏｔｈ）で実行される。
実施例２について、まず図２に示すＳｔｅｐ１の説明を行う。
本方法では、移動走行体の数式モデルを利用して、未来の状態を考慮した最適な制御入力を導出する。（４式）は連続時間系における二輪移動走行体の数式モデルを示している。
【００１９】
【数４】
【００２０】
（４式）において、ｘ、ｙは、移動走行体の座標、θは進行方向とｘ軸のなす角、ｒは車輪半径、ｌは車輪間の距離、ωL、ωRは左右の車輪の回転角速度を表す。
（４式）を離散化し、L個の姿勢角θで線形近似し、それらをハイブリッド動的システムとして表現し、
（５式）と（６式）を用いると（７式）が得られる。
【数５】
【数６】
【数７】
数式２において、θｉは線形近似を行う平衡点、Lは線形近似を行った平衡点の数を表す。
数式３において、Ωｉは動作モードが切り替わる条件を表す。
【００２１】
次にＳｔｅｐ２について説明する。（８式）において、ある制御入力を与えると１ステップ未来の状態が求まる。この操作をもう一度繰り返すと（９式）に示すように２ステップ未来の状態が求まる。
【数８】
【数９】
【００２２】
したがって、あらかじめ予測ステップ数を決めておけば、上記繰り返し計算によりあらゆる入力系列に対する状態が導出可能である．そこで、次に得られた状態と所望の状態指令が最も近く、かつ１ステップ間の制御入力の変化量が大きくならないような入力系列を（１０式）に示す評価関数を用いて決定する。
【数１０】
（１０式）は予測する状態と指令値との誤差（Δｑ）の二乗和およびあるステップに対する入力とその１ステップ前の入力の誤差（Δｕ）の二乗和を予測毎に計算し加算する。この評価関数を用いることで、状態誤差を最小にしつつ、入力の変化量も最小化可能な制御入力を導出することができる。
【００２３】
[実施の形態１]
図２に本実施例の用いた装置を示す。二輪自律移動走行体を制御対象とした。ここで、制御対象の位置検出を行うために上空に取り付けたビデオカメラを使用した。
ここでは、先行する移動走行体を円軌道を描くように走行させ、制御対象をモデル予測制御を用いて追従させた場合に、制御対象の軌跡を図３に、そのときの制御入力および先行する走行体との距離を図４、図５に示す。なお、参考までに図６から図８にシミュレーション結果についても示す。
図６から図８より、先行する走行体の急な減速に対して、近づきすぎることなく指令値に追従しており、また1ステップ間の急激な入力の変化も抑制されている。
【００２４】
また、図３から図５より、シミュレーションと実験結果では脈動に違いが見られるものの、概ね一致しているとはいえ、この脈動の原因としては、観測誤差、摩擦や車輪のすべりにおけるモデル化誤差などが考えられる。
本手法は、移動走行体の数式モデルに基づき制御入力を決定する．したがって上記のモデル化誤差は直接制御性能の劣化につながるが、今後のさらなる研究により移動走行体の特性をより正確にモデル化できるようになれば、それに伴い本手法の制御性能は格段に上昇する。
【産業上の利用可能性】
【００２５】
本発明は、走行体の追従制御が必要とされる用途であれば分野を選ばず適用可能である。典型的にはコンテナヤードや倉庫などにおける貨物や荷物の整列収納や、福祉分野における電動椅子の追従走行などに好適に利用できる。
【符号の説明】
【００２６】
１ 追従制御システム
２ 走行体位置検出部
３ 進行角度発生部
４ 第一誤差計算部
５ 第二誤計算部
６ 最適進行角選定部
７ 走行指令部
１０ 第２走行体
１１ 駆動機構
２０ 第一走行体
【特許請求の範囲】
【請求項１】
自由走行する第一走行体の後を追従させて第二走行体を走行させる、第二走行体用追従制御システムであって、
前記第一走行体の位置に基づいて前記第二走行体に制御指令が与えられ、
前記指令の与えられる時点を現在とし、前記指令が実行・完了される時点を将来と定め、
前記第二走行体に複数の進行角度を設定する第一過程と、
前記複数の進行角度に応じて定まる前記第二走行体の複数の将来制御パラメータ量を求め
る第二過程と、
前記複数の将来制御パラメータ量に基づき求められる第二走行体の複数の将来位置を求め
る第三過程と、
前記第二走行体の前記複数の進行角度に対応して求まる、前記第一走行体の現在位置と前記第二
走行体の将来位置との差異に基づき第一差異値を求め、該第一差異値で評価を行なう評価関数に
基づき、前記複数の進行角度から一の進行角度を選定し、前記制御指令とすることを特徴とする追従
制御システム。
【請求項２】
前記評価関数が前記第一差異値と、
現在制御パラメータ値と将来制御パラメータ値との差異に基づき求められる第二差異値と、
の和で表される評価関数に基づき、前記複数の進行角から一の進行角度を選定し、前記制
御指令とすることを特徴とする請求項１に記載の追従制御システム。
【請求項３】
少なくとも前記第二走行体が二輪の走行体であることを特徴とする請求項１若しくは請求項２の何れか１項に記載の追従制御システム。
【請求項４】
自由走行する第一走行体の後を追従させて第二走行体を走行させる、第二走行体用追従制御方法であって、
前記第一走行体の位置に基づいて前記第二走行体に制御指令が与えられ、
前記指令の与えられる時点を現在とし、前記指令が実行・完了される時点を将来と定め、
前記第二走行体に複数の進行角度を設定する第一過程と、
前記複数の進行角度に応じて定まる前記第二走行体の複数の将来制御パラメータ量を求める第二
過程と、
前記複数の将来制御パラメータ量に基づき求められる第二走行体の複数の将来位置を求める第三
過程と、
前記第二走行体の前記複数の進行角度に対応して求まる、前記現在位置と前記将来位置との差異
に基づき第一差異値を求め、該第一差異値で評価を行なう評価関数に基づき、前記複数の進行角
度から一の進行角度を選定し、前記制御指令とすることを特徴とする追従制御方法。
【請求項１】
自由走行する第一走行体の後を追従させて第二走行体を走行させる、第二走行体用追従制御システムであって、
前記第一走行体の位置に基づいて前記第二走行体に制御指令が与えられ、
前記指令の与えられる時点を現在とし、前記指令が実行・完了される時点を将来と定め、
前記第二走行体に複数の進行角度を設定する第一過程と、
前記複数の進行角度に応じて定まる前記第二走行体の複数の将来制御パラメータ量を求め
る第二過程と、
前記複数の将来制御パラメータ量に基づき求められる第二走行体の複数の将来位置を求め
る第三過程と、
前記第二走行体の前記複数の進行角度に対応して求まる、前記第一走行体の現在位置と前記第二
走行体の将来位置との差異に基づき第一差異値を求め、該第一差異値で評価を行なう評価関数に
基づき、前記複数の進行角度から一の進行角度を選定し、前記制御指令とすることを特徴とする追従
制御システム。
【請求項２】
前記評価関数が前記第一差異値と、
現在制御パラメータ値と将来制御パラメータ値との差異に基づき求められる第二差異値と、
の和で表される評価関数に基づき、前記複数の進行角から一の進行角度を選定し、前記制
御指令とすることを特徴とする請求項１に記載の追従制御システム。
【請求項３】
少なくとも前記第二走行体が二輪の走行体であることを特徴とする請求項１若しくは請求項２の何れか１項に記載の追従制御システム。
【請求項４】
自由走行する第一走行体の後を追従させて第二走行体を走行させる、第二走行体用追従制御方法であって、
前記第一走行体の位置に基づいて前記第二走行体に制御指令が与えられ、
前記指令の与えられる時点を現在とし、前記指令が実行・完了される時点を将来と定め、
前記第二走行体に複数の進行角度を設定する第一過程と、
前記複数の進行角度に応じて定まる前記第二走行体の複数の将来制御パラメータ量を求める第二
過程と、
前記複数の将来制御パラメータ量に基づき求められる第二走行体の複数の将来位置を求める第三
過程と、
前記第二走行体の前記複数の進行角度に対応して求まる、前記現在位置と前記将来位置との差異
に基づき第一差異値を求め、該第一差異値で評価を行なう評価関数に基づき、前記複数の進行角
度から一の進行角度を選定し、前記制御指令とすることを特徴とする追従制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２０１１−１８６８９３（Ｐ２０１１−１８６８９３Ａ）
【公開日】平成２３年９月２２日（２０１１．９．２２）
【国際特許分類】
【出願番号】特願２０１０−５２９４４（Ｐ２０１０−５２９４４）
【出願日】平成２２年３月１０日（２０１０．３．１０）
【公序良俗違反の表示】
（特許庁注：以下のものは登録商標）
１．Ｂｌｕｅｔｏｏｔｈ
【出願人】（３０４０２６６９６）国立大学法人三重大学 (270)
【Ｆターム（参考）】
【公開日】平成２３年９月２２日（２０１１．９．２２）
【国際特許分類】
【出願日】平成２２年３月１０日（２０１０．３．１０）
【公序良俗違反の表示】
（特許庁注：以下のものは登録商標）
１．Ｂｌｕｅｔｏｏｔｈ
【出願人】（３０４０２６６９６）国立大学法人三重大学 (270)
【Ｆターム（参考）】
[ Back to top ]