JP2007249363A - 移動体の軌道追従制御システム及び軌道追従制御方法 - Google Patents

Abstract

【課題】
目標経路に対する追従性が高い軌道追従制御システム及び軌道追従制御方法を提供すること。
【解決手段】
本発明にかかる軌道追従制御システムは、目標経路１００に対してロボット１が追従するように制御するものである。このシステムでは、まず、目標経路１００を円弧近似し、円弧の曲率を算出する。このとき、本発明では、ロボット１の移動速度に応じて曲率を決定している。次に、算出された曲率に応じてロボット１の制御を決定する。
【選択図】 図１

Description

本発明は、移動体の軌道追従制御システム及び軌道追従制御方法に関し、特にロボットの軌道追従を制御するシステム及びその方法に関する。

従来より、ロボットや車両等の移動体を目標経路に対して自動追従させる技術が提案されている。例えば、目標経路に対する、移動体の位置の横ずれ量や方位角のずれ量をフィードバック制御することによって自動追従する技術がある。しかしながら、かかるフィードバック制御だけでは、曲率への変化に対して移動体の応答が遅れる場合があるため、様々な曲率が混在するカーブを含む目標経路を追従することは困難である。

特許文献１では、この問題点を解決するために、目標経路からの相対的な横ずれ及び方位角のずれ量に応じたフィードバック制御による操作量に加えて、目標経路の曲率や横ずれ量に応じたフィードフォワード制御による操作量により操舵角を決定する技術が提案されている。これにより、曲率が変化するような任意の目標経路に対して精度良く追従することができる。
特開２００２−２１５２３９号公報

しかしながら、特許文献１に開示された追従技術では、目標経路の曲率を制御ループに直接入れているため、目標経路の曲率の不連続な変化が移動体の挙動に直接反映され、移動体の動作が不安定になるという問題が発生する。さらに、フィードフォワード制御による操作が常に機能するため、移動体が目標経路から大きく外れたときに、フィードフォワード制御による操作量がフィードバック制御による操作量に対する外乱となり、軌道追従性が劣化するという問題も生じる。

本発明は、かかる課題を解決するためになされたものであり、目標経路に対する追従性が高い軌道追従制御システムを提供することを目的とする。

本発明にかかる軌道追従制御システムは、目標経路に対して移動体が追従するように制御する軌道追従制御システムであって、前記目標経路を円弧近似し、円弧の曲率を算出する曲率算出手段と、前記曲率算出手段により算出された曲率に応じて前記移動体の制御を決定するフィードフォワード制御手段とを備え、前記曲率算出手段は、当該移動体の移動速度に応じて前記曲率を決定するものである。本発明によれば、このような構成を有するため、目標経路に対する追従性を高くすることができる。

より具体的には、前記曲率算出手段は、前記移動体の移動速度及び制御周期に基づいて当該曲率を決定する。特に制御周期に応じて曲率を決定しているので、移動体を移動制御可能な単位で制御することができ、安定した追従動作を実現できる。

ここで、前記曲率算出手段は、前記移動体の位置に対する、前記目標経路上の最寄点を決定する最寄点決定手段と、前記目標経路上において前記最寄点決定手段により決定された最寄点の前後に二つの隣接点を決定する隣接点決定手段と、前記最寄点決定手段により決定された最寄点と、前記隣接点決定手段により決定された隣接点を通過する円弧を決定する円弧決定手段とを備えることによって、簡易な計算により曲率を算出することができる。

このとき、前記隣接点決定手段は、前記最寄点と前記隣接点間の距離を前記移動体の移動速度に応じて決定することとして、移動速度に応じた追従動作を実現できる。

前記隣接点決定手段は、前記最寄点と前記隣接点間の距離を前記移動体の移動速度及び制御周期に応じて決定することが好ましい。特に制御周期に応じて距離を決定しているので、移動体を移動制御可能な単位で制御することができ、安定した追従動作を実現できる。

具体的には、前記隣接点決定手段は、前記移動体の移動速度をＶとし、前記移動体の制御周期をΔＴとしたとき、前記最寄点と前記隣接点間の距離ＬをＶΔＴに応じて決定することが望ましい。

さらに、前記フィードフォワード制御手段は、目標経路に対する移動体の位置誤差又は姿勢誤差が予め定められた誤差を越える場合に当該フィードフォワード制御を停止することが好ましい。これにより、軌道追従性を高めることができる。好適な実施の形態における移動体はロボットである。

他方、本発明にかかる軌道追従制御方法は、目標経路に対して移動体が追従するように制御する軌道追従制御方法であって、前記目標経路を円弧近似し、円弧の曲率を算出するステップと、算出された曲率に応じて前記移動体の制御を決定するステップとを備え、前記曲率算出ステップでは、当該移動体の移動速度に応じて前記曲率を決定するものである。本発明によれば、目標経路に対する追従性を高くすることができる。

ここで、前記曲率算出ステップでは、前記移動体の移動速度及び制御周期に基づいて当該曲率を決定することが望ましい。特に制御周期に応じて曲率を決定しているので、移動体を移動制御可能な単位で制御することができ、安定した追従動作を実現できる。

前記曲率算出ステップでは、前記移動体の位置に対する、前記目標経路上の最寄点を決定するステップと、前記最寄点の前後の二つの隣接点を決定するステップと、前記最寄点と前記隣接点を通過する円弧を決定するステップとを備えることが望ましい。これらのステップにより簡易な計算で曲率を算出することができる。

このとき、前記隣接点は、前記最寄点と前記隣接点間の距離を前記移動体の移動速度に応じて決定することにより、移動速度に応じた追従動作を実現できる。

前記最寄点と前記隣接点間の距離を前記移動体の移動速度及び制御周期に応じて決定することが好ましい。特に制御周期に応じて距離を決定しているので、移動体を移動制御可能な単位で制御することができ、安定した追従動作を実現できる。

具体的には、前記移動体の移動速度をＶとし、前記移動体の制御周期をΔＴとしたとき、前記最寄点と前記隣接点間の距離ＬをＶΔＴに応じて決定するとよい。

目標経路に対する移動体の位置誤差又は姿勢誤差が予め定められた誤差を越える場合には、前記曲率に応じた移動体の制御を停止することが望ましい。これにより、軌道追従性を高めることができる。好適な実施の形態における移動体はロボットである。

本発明にかかる軌道追従制御プログラムは、目標経路に対して移動体が追従するように制御する軌道追従制御プログラムであって、コンピュータに対して、前記目標経路を円弧近似し、前記移動体の移動速度に応じて曲率を決定するステップと、算出された曲率に応じて前記移動体の制御を決定するステップとを実行させるものである。本発明によれば、目標経路に対する追従性を高くすることができる。

本発明によれば、目標経路に対する追従性が高い軌道追従制御システム及び軌道追従制御方法を提供することができる。

本発明の実施の形態にかかる軌道追従制御システムでは、移動体としてロボットを用いている。当該軌道追従制御システムは、例えば、ロボットに搭載されたコンピュータやロボットとは別に設けられたコンピュータにより実現される。このコンピュータは、例えば、中央処理装置（ＣＰＵ）、ＲＯＭ、ＲＡＭ、ハードディスク等の補助記憶装置、ＣＤ−ＲＯＭ等の可搬型記憶媒体が挿入される記憶媒体駆動装置、入力手段や出力手段を備えている。ＲＯＭ、補助記憶装置、可搬型記憶媒体等の記憶媒体には、オペレーティングシステムと協働してＣＰＵ等に命令を与え、アプリケーションプログラムを記録することができ、ＲＡＭにロードされることによって実行される。このアプリケーションプログラムは、本発明にかかる軌道追従制御システムを実現する特有のコンピュータプログラムを含む。軌道追従制御システムによる追従制御は、中央処理装置がアプリケーションプログラムをＲＡＭ上に展開した上で当該アプリケーションプログラムに従った処理を補助記憶装置に格納されたデータを読み出し、また格納を行なうことにより、実行される。

ロボットの目標軌道となる目標経路は、しばしば座標の集合として与えられる。座標は、例えば、図１に示されるようにグリッド状に離散化されたグリッド座標で表現される。グリッド座標を利用するメリットは、最短経路探索を容易に実行できる点や、確実に目的地に到達できる経路を得ることができる点にある。しかしながら、グリッド座標において表現される目標経路は直線と直線が接続された不連続な経路であるため、ロボットがこのような経路を追従することは難しい。そのため、与えられた離散的な経路座標から、ロボットの追従制御に利用可能な滑らかな目標経路を、例えば、曲線近似等の手法を利用して曲線経路を求めることにより作成することができる。

自由曲線からなる目標経路を図１に実線で示す。このような目標経路１００上を、図２に示されるような構成のロボット１が追従移動する場合を考える。ロボット１は、二つの車輪１１，１２を備えている。これらの車輪１１，１２はそれぞれモータ等の駆動機構によって回転数が制御されている。

このロボット１に関し、次式で示されるような制御則が成り立つ。

これらの式及び図２に示される各制御パラメータは、次の通りである。
Ｖ_ｌ：左車輪１１の移動速度
Ｖ_ｒ：右車輪１２の移動速度
Ｖ：ロボット（台車）１の接線速度
ｄ：左車輪１１と右車輪１２間の距離
Ｒ：最寄点の曲率半径
ｙ_error：目標経路からの位置誤差
θ_error：目標経路からの姿勢誤差
ｋ_ｙ：位置誤差のフィードバックゲイン
ｋ_θ：姿勢誤差のフィードバックゲイン

上記各式において１−（ｄ／２Ｒ），１＋（ｄ／２Ｒ）がフィードフォワード項であり、ｋ_ｙｙ_error＋ｋ_θθ_errorがフィードバック項である。フィードフォワード項により、ロボット１が特定の曲率で曲がることを前提として予測したカーブに沿うようにロボット１の両車輪１１，１２を制御する。フィードバック項により、目標経路からの位置誤差及び姿勢誤差が少なくなりゼロとなるように制御する。

図１では、目標経路上の任意の点Ａや点Ｂにおける曲率半径Ｒ_Ａ，Ｒ_Ｂ及び曲率中心Ｏ_Ａ，Ｏ_Ｂを有する円が示されている。かかる曲率半径Ｒ_Ａ，Ｒ_Ｂが上記式（１）によりロボット１の経路追従制御に用いられる。

続いて、図３に示すフローチャートを用いて、本発明の実施の形態にかかる追従制御の流れについて説明する。
まず、目標経路上のロボット１の最寄点を探索する（Ｓ１０１）。最寄点の探索処理については、種々の方法を用いることができるが、本例では、分割法が用いられている。

図４に最寄点の探索処理の詳細を示す。図５に示すように、ロボット１が点Ａにいるときに目標経路１００上の最寄点を算出する場合には、節点ｎの座標と点Ａ間の距離を求める（Ｓ２０１）。このようにして求めた距離が過去に算出した点Ａといずれかの節点間の距離の中で最小の距離かどうかを判定する（Ｓ２０２）。判定の結果、最小の距離であると判定された場合には、その節点ｎを記憶する（Ｓ２０３）。判定の結果、最小の距離でないと判定された場合や、ステップＳ２０３により節点ｎを記憶した場合には、全ての節点を調べたかどうかを判定する（Ｓ２０４）。

判定の結果、全ての節点を調べていないと判定された場合には、ｎに１を加えた節点についてさらにステップＳ２０１の処理を実行する。また、判定の結果、全ての節点を調べたと判定された場合には、距離が最小となる節点（最小節点）に対して隣接する二つの節点（隣接節点）のうち、最小節点との間の距離が小さい方の隣接節点を選択し、選択された隣接節点と最小節点間を探索範囲とする（Ｓ２０６）。そして、図６に示されるように、探索範囲とされた節点間の中点を始点Ｐ_０とし、探索範囲ｗ１を節点間距離Ｌの１／２とする（Ｓ２０７）。図６に示された例では、始点Ｐ_０から（１／２）ｗ１、即ち（１／４）Ｌだけｎ点側に延長した点Ｐ_Ｌ１から、始点Ｐ_０から（１／２）ｗ１、即ち（１／４）Ｌだけｎ＋１点側に延長した点Ｐ_Ｒ１までを探索範囲ｗ１としている。そして、始点Ｐ_０について（即ち、ｎがＰ_０に場合について）、ｎ−（１／２）ｗ１の節点Ｐ_Ｌ１と、ｎ＋（１／２）ｗ１の節点Ｐ_Ｒ１のいずれが点Ａから近いかどうかが判定される（Ｓ２０８）。判定の結果、近い方の節点を近傍点Ｐとし、さらに近傍点Ｐからｗ１の半分のｗ２を探索範囲として同様に点Ａから近いかどうかが判定され、同様の処理を繰り返す（Ｓ２０９）。このような処理の結果、収束した場合には、収束した結果、最も点Ａから近いと判定された近傍点Ｐが最寄点と特定され、その座標が求められる。

図３に示すフローチャートの説明に戻る。最寄点を探索した後に、ロボット１の移動速度に応じて最寄点の前後２点を決定する（Ｓ１０２）。図７に示されるように、最寄点をＦとすると、その前後の目標経路上の２点からなる隣接点Ｅ、Ｇを決定する。

このとき、ＥＦ間の距離とＦＧ間は等距離とし、その距離Ｌは、ロボット１の移動速度をＶ、制御周期ΔＴとしたとき、Ｌ＝ＶΔＴで表わされる。即ち、距離Ｌは、移動速度Ｖ及び制御周期ΔＴのそれぞれに正比例する。このように、ＥＦＧの３点が決まれば、これら３点を通る円を一意に決定することができる。

次に、最寄点とその前後の隣接点、即ち点Ｅ，Ｆ，Ｇから、円の方程式を解き、円弧の半径Ｒを算出する（Ｓ１０３）。ここで、円の方程式は次式（２）で表わすことができる。

ｘ^２＋ｙ^２＋ａｘ＋ｂｙ＋ｃ＝０ （２）

Ｅ（ｘ_０，ｙ_０），Ｆ（ｘ_１，ｙ_１），Ｇ（ｘ_２，ｙ_２）とすると、円が通過する、これら３点の座標より連立方程式（３）が得られる。

式（３）においてＬＵ分解等の算法を用いて左辺の逆行列を求め、未知数ａ，ｂ，ｃを算出する。さらに得られたａ，ｂ，ｃより、点Ａに対する曲率半径Ｒ及び曲率中心Ｏ（ｘ_Ｏ，ｙ_Ｏ）は以下のように与えられる。

Ｒ＝{（ａ^２＋ｂ^２）／４ − ｃ^２}^１／２
ｘ_ｏ＝ａ／２
ｙ_ｏ＝ｂ／２

続いて、最寄点Ｆに対するロボット１の位置誤差及び姿勢誤差から、式（１）におけるフィードバック項を算出する（Ｓ１０４）。

さらに、ステップＳ１０３において算出された円弧の半径Ｒから、式（１）におけるフィードフォワード項を算出する（Ｓ１０５）。

次に、位置誤差及び姿勢誤差が、予め決定された指定誤差以下かどうかを判定する（Ｓ１０６）。判定の結果、位置誤差と姿勢誤差の双方が指定誤差以下であると判定された場合には、フィードフォワード制御及びフィードバック制御の双方を実行する（Ｓ１０７）。即ち、ステップＳ１０４において算出されたフィードバック項の値と、ステップＳ１０５において算出されたフィードフォワード項の値を用いて、左右の車輪の移動速度を決定し、これに基づいた制御を実行する。

他方、判定の結果、位置誤差と姿勢誤差のうちいずれか一方若しくは双方が指定誤差を超えると判定された場合には、フィードフォワード制御は実行せずに、フィードバック制御のみを実行する（Ｓ１０８）。即ち、ステップＳ１０４において算出されたフィードバック項の値のみを用いて、左右の車輪の移動速度を決定し、これに基づいた制御を実行する。

このように、本実施の形態では、フィードフォワード制御の適用範囲を、軌道追従誤差が一定量以下の状況に限定している。図８で示されるように、ロボット１と目的経路の距離が離れている状況では、ロボット１にフィードフォワード制御を適用すると、フィードフォワード制御は、ロボット１が目標経路上を進んでいることを前提条件として計算されるため、フィードフォワード制御の入力が追従誤差を補正するフィードバック制御に対する外乱となり、ロボット１の軌道追従性が低下する。そのため、経路とロボット間が離れている、即ち経路追従誤差が指定誤差以上の場合は、フィードフォワード制御を適用せずに、フィードバック制御のみで動作することとした。例えば、ロボット１が進むべき方向と反対方向を進んでいるような場合にはフィードフォワード制御を停止することとした。

また、本実施の形態では、移動経路を円弧近似する場合の曲率半径を、移動速度が大きくなれば曲率半径が長くなるように、また移動速度が小さくなれば曲率半径が短くなるように、ロボットの移動速度に応じて決定している。即ち、ロボットの移動速度が大きく、ロボットが速く移動するような場合には、微小単位でみれば急に曲がるべきところも緩やかに曲がるようにロボットの移動を制御し、ロボットの移動速度が小さく、ロボットが遅く移動するような場合には、逆に微小単位に従って急に曲がるように経路を設定している。従って、ロボットは安定して目標経路を追従することが可能となる。

本実施の形態では、Ｌ＝ＶΔＴで最寄点と隣接点間の距離を決定している。ここで、ロボットの移動速度Ｖに制御周期ΔＴを乗算した値は、制御周期ΔＴがロボットの方向及び速度を変更できない期間であるから、ロボットの方向及び速度を変更できない距離に相当する。従って、ロボットの方向及び速度を変更できない距離よりも短い距離を単位として円弧に近似したとすると、そのような微小単位でロボットを移動制御することができない。それ故に、最寄点と隣接点間の距離を、Ｌ＝ＶΔＴにより決定することにより、制御周期以下の曲率変化にロボットが影響を受けることなく安定的に軌道追従制御を実現することができる。

なお、本実施の形態では、最寄点と隣接点間の距離をＬ＝ＶΔＴにより決定したが、Ｌ＝ＶΔＴ以上であればよい。例えば、Ｌ＝ａＶΔＴ＋ｂの一般式（ａ，ｂ：定数）で表わされるような距離としてもよい。

その他の実施の形態．
上述の例では、軌道追従制御システムを２輪走行のロボットに適用したが、これに限らず、２輪よりも多い、例えば４輪走行のロボットにも適用でき、さらには、１輪走行のロボットや２足歩行・４足歩行のロボットにも適用できる。歩行ロボットの場合には歩幅を制御することにより移動方向を変えて目標経路を追従することができる。さらには、ロボットに限らず、２輪走行や４輪走行の車両であっても適用できる。

本発明にかかる軌道追従制御システムにおけるロボットの移動経路を示すグリッド座標と目標経路を示す図である。 本発明にかかる軌道追従制御システムにおける制御パラメータの幾何学的関係を示す説明図である。 本発明にかかる軌道追従制御システムにおける処理の流れを示すフローチャートである。 本発明にかかる軌道追従制御システムにおける処理の流れを示すフローチャートである。 本発明にかかる軌道追従制御システムにおける、最寄点の探索処理を説明するための説明図である。 本発明にかかる軌道追従制御システムにおける、最寄点の探索処理を説明するための説明図である。 本発明にかかる軌道追従制御システムにおける、円弧近似処理を説明するための説明図である。 本発明にかかる軌道追従制御システムにおける処理を説明するための説明図である。

符号の説明

１ ロボット
１１ 右車輪
１２ 左車輪
１００ 目標経路

Claims (17)

1. 目標経路に対して移動体が追従するように制御する軌道追従制御システムであって、
   前記目標経路を円弧近似し、円弧の曲率を算出する曲率算出手段と、
   前記曲率算出手段により算出された曲率に応じて前記移動体の制御を決定するフィードフォワード制御手段とを備え、
   前記曲率算出手段は、当該移動体の移動速度に応じて前記曲率を決定する軌道追従制御システム。
2. 前記曲率算出手段は、前記移動体の移動速度及び制御周期に基づいて当該曲率を決定することを特徴とする請求項１記載の軌道追従制御システム。
3. 前記曲率算出手段は、
   前記移動体の位置に対する、前記目標経路上の最寄点を決定する最寄点決定手段と、
   前記目標経路上において、前記最寄点決定手段により決定された最寄点の前後に二つの隣接点を決定する隣接点決定手段と、
   前記最寄点決定手段により決定された最寄点と、前記隣接点決定手段により決定された隣接点を通過する円弧を決定する円弧決定手段とを備えたことを特徴とする請求項１又は２記載の軌道追従制御システム。
4. 前記隣接点決定手段は、前記最寄点と前記隣接点間の距離を前記移動体の移動速度に応じて決定することを特徴とする請求項３記載の軌道追従制御システム。
5. 前記隣接点決定手段は、前記最寄点と前記隣接点間の距離を前記移動体の移動速度及び制御周期に応じて決定することを特徴とする請求項３記載の軌道追従制御システム。
6. 前記隣接点決定手段は、前記移動体の移動速度をＶとし、前記移動体の制御周期をΔＴとしたとき、前記最寄点と前記隣接点間の距離ＬをＶΔＴに応じて決定することを特徴とする請求項３記載の軌道追従制御システム。
7. 前記フィードフォワード制御手段は、目標経路に対する移動体の位置誤差又は姿勢誤差が予め定められた誤差を越える場合に当該フィードフォワード制御を停止することを特徴とする請求項１乃至６いずれかに記載の軌道追従制御システム。
8. 前記移動体はロボットであることを特徴とする請求項１乃至７いずれかに記載の軌道追従制御システム。
9. 目標経路に対して移動体が追従するように制御する軌道追従制御方法であって、
   前記目標経路を円弧近似し、円弧の曲率を算出するステップと、
   算出された曲率に応じて前記移動体の制御を決定するステップとを備え、
   前記曲率算出ステップでは、当該移動体の移動速度に応じて前記曲率を決定する軌道追従制御方法。
10. 前記曲率算出ステップでは、前記移動体の移動速度及び制御周期に基づいて当該曲率を決定することを特徴とする請求項９記載の軌道追従制御方法。
11. 前記曲率算出ステップでは、
    前記移動体の位置に対する、前記目標経路上の最寄点を決定するステップと、
    前記最寄点の前後の二つの隣接点を決定するステップと、
    前記最寄点と前記隣接点を通過する円弧を決定するステップとを備えたことを特徴とする請求項９又は１０記載の軌道追従制御方法。
12. 前記隣接点は、前記最寄点と前記隣接点間の距離を前記移動体の移動速度に応じて決定することを特徴とする請求項１１記載の軌道追従制御方法。
13. 前記最寄点と前記隣接点間の距離を前記移動体の移動速度及び制御周期に応じて決定することを特徴とする請求項１１記載の軌道追従制御方法。
14. 前記移動体の移動速度をＶとし、前記移動体の制御周期をΔＴとしたとき、前記最寄点と前記隣接点間の距離ＬをＶΔＴに応じて決定することを特徴とする請求項１１記載の軌道追従制御方法。
15. 目標経路に対する移動体の位置誤差又は姿勢誤差が予め定められた誤差を越える場合には、前記曲率に応じた移動体の制御を停止することを特徴とする請求項９乃至１４いずれかに記載の軌道追従制御方法。
16. 前記移動体はロボットであることを特徴とする請求項９乃至１５いずれかに記載の軌道追従制御方法。
17. 目標経路に対して移動体が追従するように制御する軌道追従制御プログラムであって、
    コンピュータに対して、
    前記目標経路を円弧近似し、前記移動体の移動速度に応じて曲率を決定するステップと、
    算出された曲率に応じて前記移動体の制御を決定するステップとを実行させる軌道追従制御プログラム。

Images