１２．４．　経路追従運動のサンプル値制御２００８−１０−７

（実行ファイル 3-12-4AGVSmplC0exe ）

・移動ロボット（ＡＧＶ）が外界の情景をTVカメラで見て自身の位置姿勢を知る場合，その計測に時間がかかりフィードバック制御に影響を及ぼすことがある．

・計測値を推定して制御量を計算し出力している間に，ＡＧＶが進んで状態が変わってしまい真の制御量と異なるからである．

・サンプル時間による制御系の特性や安定性についてはサンプル値制御問題として定式化されている．

・ここでは12,3,0節で述べた経路制御（連続制御）と比較しながらサンプル値制御による走行制御をシミュレーションする．

［目次］

１．計測データのサンプリングと時間遅れ

２．AGV経路追従のサンプル値制御の伝達関数

３．各AGV機構の運動方程式とサンプル値制御の安定性

（１）2DW1C機構−PI制御−

（２）1FDW1FS機構−P制御−

（１）1RDW1FS機構−P制御−

［シミュレーションの使い方］

［AGV経路追従散布理知制御の特徴］

１．計測データのサンプリングと時間遅れ

・TVカメラを使った経路ずれの計測と経路ずれを修正するフィードバック量の計算には少なからず時間がかかる．

・状態量をサンプルした後計算して出力する過程は図12.4.1の如くである．

・連続量の系への入力をx(t)，出力をy(t)=Ax(t)とする．Aは一般的な線形関数である．

・サンプル時間をTsとすると，時刻tで計測した値x(t)を基に計算した値y(t)=Ax(t)をTs秒後に出力し，０次ホールドとしてTs から2 Tsまで一定に保つ．

・計測に時間がかからない連続系では，時刻tで計測した結果はtで出力するが，サンプル値制御系では時刻tでの結果をt+Tsに出力する．

・すなわちTs秒前の値を出力する．

・これらをLaplace変換して表現すると

　　　，　，　　　　　　　　　(1-1)

　　　　　　　　　　　　　　　　　　　　　　　(1-2)

・すなわち時間遅れとサンプリングを併せた系の伝達関数は

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　(1-3)

　である．

［註］サンプル値制御の理論では０次ホールドについて説明しているが，時間遅れについては言及していない．この時間遅れが系の安定性に大きく影響することがこのAGV制御のシミュレーションによって明らかになる．この影響を小さくするために１次ホールド，または「先読み（外挿）」を使うのが効果的であるが，ここでは述べない．

２．AGV経路追従のサンプル値制御の伝達関数

・AGVの走行制御系は図12.4.3で表される．

・経路Rはの変わりに走行距離s，経路からの横ずれd，AGVの向きθで表す．横ずれの指令値はdc=0である．

・各AGVの連続制御式を12.30AGVフィードバック制御の基本（ファイル名 3-12-30AGVFBC0 ）から引用して再掲する．

・Rcに対する応答Rの関係は

　　　，　　　　　　　　　　　　　(2-1)

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　(2-2)

・連続制御ではであったが，サンプル値制御ではGh(s)が直列に加わっている．

・上式のｚ変換を求め，系の安定性を論じる．

・行列表示の時間遅れとホールドの伝達関数は

　　　，　(Iは２×２または３×３の単位行列)　　　　　　　　　　 　(2-2)

・一般的にはこのように表現されるが，ベクトルと行列の計算はこの場合煩雑なので，12.30節と同様に１変数に直して式を扱う．

３．各AGV機構の運動方程式とサンプル値制御の安定性

・制御変数とAGVの速度，またはの関係式とその伝達関数表示Gw(s)を求める．

・走行速度vsまたは駆動輪速度vdは指令値速度のままとし，経路誤差のフィードバックは行わない．

・経路誤差は旋回速度Ωまたはステアリング速度ωにフィードバックする．

（１）2DW1C機構−PI制御−

・連続制御では定常偏差をなくすために誤差の積分要素が必要であることが分かったので，サンプル値制御でもPI制御で考える(12.30節参照)．

・簡単のために走行速度vsと旋回速度Ωを制御変数にとる．(vs,Ω)と本来の制御変数である２つの駆動輪速度との換算は容易である．

・走行速度は一定とする．（どんな急旋回でもできる）

・AGVの運動方程式は

　　　，　　　，　　　                               (2-3-1-1)

・を独立に扱い，についてを求める．

・誤差のフィードバックは，12.30節のシミュレーション結果を踏まえて，PI型のフィードバックとする（定常偏差をなくすため）．

　　　                  (2-3-1-3)

　　　，　　　　　　　　　　　　　　 　　　　　　　(2-3-1-4)

・誤差が小さいとして線形化する．Δθに関する伝達関数は12.30節(2-1-6)式を参照して

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　(2-3-1-5)

　　　                         (2-3-1-6)

・系の安定性を論じるためにz変換し，パルス伝達関数を求める．

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　(2-3-1-7)

　　　 　 (2-3-1-8)

・留数計算によるz変換の公式によれば

　　　，　                      (2-3-1-9)

　　　，　　                          (2-3-1-10)

　であるから

　　　

　　　

　　　    (2-3-1-11)

                                                      (2-3-1-12)

，　　，　　

　　　，　　 (2-3-1-13)

・特性方程式の根がz平面上で単位円内にあることが安定条件である．

・Hurwitzの安定判別によれば

　　　                                        (2-3-1-14)

　　　                                       (2-3-1-15)

　　　                                             (2-3-1-16)

　　　                                      (2-3-1-17)

　　　                                       (2-3-1-18)

　　　                                           (2-3-1-19)

　が安定条件である．

（２）1FDW1FS機構−P制御−

・連続制御では誤差の積分要素が必要でないことが分かったので，サンプル値制御でもP制御で考える．

・駆動輪速度vdとステアリング(ST)速度ωを制御変数にとる．

・経路は既知であり，経路に沿うST角と曲率の変化率は分かっているとする．

・駆動輪速度vdは曲率の急変化に着いていけるように可変とする．その変化は分かっており，それを駆動輪速度の指令値とする．

・経路誤差ΔRは経路からのずれΔd・旋回角度誤差Δθ・ST誤差Δφであり，この誤差をST速度ωにフィードバックする．

・AGVの運動方程式は

　　　，　　，　　，　        (2-3-2-1)

・を独立に扱い，についてを求める．

・誤差のフィードバックは，12.30節のシミュレーション結果を踏まえて，P型のフィードバックとする．

　　　　　　　　　　　　 　　　　　　　　　 (2-3-2-2)

　　　，　　，　　　　　　　　 　　　　　　 　(2-3-2-3)

・誤差が小さいとして線形化する．Δθに関する伝達関数は12.30節(1-2-8)式を参照して

　　　　　　　　　　　　　　　　　　　　　　　　　 　(2-3-2-4)

　　　                               (2-3-2-5)

　vsは(2-3-2-1)式に見るように，一定とは限らない．

・系の安定性を論じるためにz変換し，パルス伝達関数を求める．

　　　　　　　　　　　　　　　　　　　　　　　　　　 　(2-3-2-6)

　　　 　　 　 (2-3-2-7)

・前述留数計算によるz変換の公式(2-3-1-9)，(2-3-1-10)式を参照して

　　　

　　　

　　　　　　(2-3-2-8)

　　　

　　　　　　　　　　　　　　　　　　　　　　　　　　　　(2-3-2-9)

，　　，　　

　　　，　　       (2-3-2-10)

・Hurwitzによる安定条件は前述の(2-1-3-14)〜(2-1-3-19)式と同じである．

・走行速度はでありφcによって変わり，従ってゲインもサンプル値制御の安定条件の値も変わる．シミュレーションではφc=0としてゲインを不変とする．φcが０でないときはその時点のゲインは小さめに設定したことになり，サンプル値制御でも限界サンプル値時間が大きくなり，安定側に移動する．

・限界のサンプル時間の計算の詳細は12.4Cサンプル値制御におけるサンプル時間と安定限界（ファイル名 3-12-4CAGVSMPCC ）を参照のこと．

（３）1RDW1FS機構−P制御−

・連続制御では誤差の積分要素が必要でないことが分かったので，サンプル値制御でもP制御で考える．

・駆動輪速度（=走行速度）vsとステアリング(ST)速度ωを制御変数にとる．

・経路は既知であり，経路に沿うST角は分かっているとする．

・駆動輪速度vsは一定であり，それを駆動輪速度の指令値とする．フィードバック制御は行わない．

・経路誤差ΔRは経路からのずれΔd・旋回角度誤差Δθ・ST誤差Δφであり，この誤差をST速度ωにフィードバックする．

・AGVの運動方程式は

　　　，　　，　　，　        (2-3-3-1)

・を独立に扱い，についてを求める．

・誤差のフィードバックは，12.30節のシミュレーション結果を踏まえて，P型のフィードバックとする．

　　　　　　　　　　　　 　　　　　　　　　 (2-3-3-2)

　　　，　　，　　　　　　　　 　　　　　　 　(2-3-3-3)

・誤差が小さいとして線形化する．Δφに関する伝達関数は12.30節(1-3-10)式を参照して

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　 　(2-3-3-4)

　　　 　 (2-3-3-5)

　ただし

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　(2-3-3-6)

・これは1DW1FS機構のLをlに置き換えて，式は全く同じである．

・パルス伝達関数は

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　 　(2-3-3-7)

　　　

　　　　　　(2-3-3-8)

　　　

　　　　　　　　　　　　　　　　　　　　　　　　　　　(2-3-3-9)

，　　，　　

　　　，　　       (2-3-3-10)

・Hurwitzによる安定条件は前述の(2-1-3-14)〜(2-1-3-19)式と同じである．

・でありφcによって変わり，従ってゲインもサンプル値制御の安定条件の値も変わる．シミュレーションではφc=0としてゲインを不変とする．φcが０でないときはその時点のゲインは大きめに設定したことになり，サンプル値制御でも限界サンプル値時間が小さくなり，不安定になり易い．

・以上３種のAGVについて，サンプル値制御系の安定条件を求めた．理論によれば，サンプル時刻点の応答が発散しない条件は上記の通りであるが，中間の応答（かげの応答）が安定であるかどうかは分からない．その解析法もあるがここでは省略する．

・また上記安定条件式からフィードバックゲインの条件を決めることもできるが，このシミュレーションでは連続制御で与えられたゲインを使い，そのゲインでサンプル値制御系が安定であるかどうかのみを判定し，理論では連続制御系・サンプル値制御系とも安定の場合，AGV運動の非線形性のために不安定となることがあるかどうかを調べることにする．

［シミュレーションの使い方］

（１）使用フォルダー名：AGVSMPC0 ( プログラム名AGVSMPC0.c, graph2d.h )

（２）文字画面でのキー操作

　(1)スタート → (2)経路(0,1)の選択 → (3)サンプル時間Ts設定 → (4)FB系の固有振動数fn・減衰比ζ設定

→  (5)AGVの初期状態設定 →（(6)３種走行制御の運動計算）→ 任意キーによって描画画面へ

　　　　　　 → 描画画面からの変更があるとき(2) or (3) or (4) or (5)へ

[註] 描画画面で初めに2DW1C機構のアニメーションを描画するように設定している．

（３）描画画面での表示

　(1) 中央面にAGVの走行経路と運動アニメーション図

　(2) 右上に経路誤差(Δd，Δθ)の位相平面トラジェクトリのグラフ

（４）描画画面でのキー操作

　以下はそのまま描画画面に留まる．

　‘s’：上記運動の時間経過（sを押し続けると時間的に変化する）

　‘0’：運動を始点に戻す

　‘x’ , ‘X’, ‘y’, ‘Y’：アニメーション画面を左右上下に移動

　‘w’, ‘W’：アニメーション画面を縮小拡大

　‘z’, ‘Z’ ：位相平面図を左右に移動

　‘1’, ‘2, ‘3’ ：アニメーション描画のAGV選択

　以下は文字画面に戻る．

　‘c’　 ：経路変更のために文字画面(2)へ

  ‘t’    ：サンプル時間の変更のために(3)へ

　‘f’　 ：FB系のゲイン（固有振動数・減衰比）変更のために文字画面(4)へ

　‘h’ 　：AGVの初期状態変更のために文字画面(5)へ

　‘ESC’：終了

［註］どのAGVもホイールベース・トレッドは約1mであり，変更しない．

　　　２つ目の減衰係数はα=ωnまたは安定範囲内のできだけ大きな値とする．

計算を済ませてから画面表示する．

計算には数秒かかることがある．

・シミュレーションの固定した条件は，AGVのホイールベースとトレッドL=B=1[m]，駆動輪速度vd=1[m/s]，ST限界速度ωmax=3 [rad/s]，ST最大角度φmax=3[rad]である．ωmaxとφmaxが運動の安定性に影響する．

・以下の図は各AGV機構のサンプル値制御による経路追従安定性の例を連続制御と比較して示している．

・図１〜３は連続制御で決めたゲインを基にサンプル時間を限界値以下（安定）としたときの直線追従性である．

・どのAGVも理論通りの安定性を示している．

・ただし図では示していないが，1RDW1FS機構では始点が目標直線と更に遠く離れると連続制御では安定であってもサンプル値制御では不安定となる．他のAGV２機構では遠くても連続制御が安定である限り不安定にならない．

・図７は1RDW1FS機構について，直線経路と同じゲインとサンプル時間で，円弧経路への追従性を示している．直線経路では安定であったが（図３），円弧では不安定である．これはφcが０でないのでその点でゲインは大きめになり，限界サンプル時間Tsmaxが小さくなったためと思われる．このときのAGV運動を見ると，小さな円を描いて旋回しているが，これはST角φが大きいときに連続制御で逆にφを減じる方向にフィードバックする筈なのに，サンプル値制御では増加するフィードバックのままになっており，旋回速度が非常に大きくサンプルホールド中に急速に旋回（急速な円運動）するためである．

・線形化したFB系は安定であるが，誤差が大きい（初期状態が経路から離れている）と，非線形性が強くなり指定経路に収束しない．

・図４〜６はサンプル時間を大きくした場合で，理論的には2DW1C機構では安定，1FDW1FS，1RDW1FS機構では不安定である．しかし1FDW1FS機構では発散していない．これはφcが０でないときはゲインが小さめになり，安定領域に近づいたからと思われる．