目標軌道での水平方向への直進能力の確認

1つ目の実験では，提案手法による実験結果と従来提案されている姿勢制御のみの 手法[4]による実験結果を比較する．これにより，提案手法によって，姿勢制御のみ の手法では困難な目標軌道での水平方向への直進が可能となることを確認する．同 時に，前節で提案した窓枠位置情報を利用した位置検出法によって，ロボットの位 置^Wx, ^Wy，姿勢θが検出可能であることを確認する．まず，ロボットが目標軌道で の水平方向への直進が可能である状態について具体的に議論する．

目標軌道を，図2.13に示すように時刻tにおける目標速度 V_dをt < 0.5(s)の ときV_d = 0.2t(m/s)，t ≥ 0.5(s)のときV_d = 0.1(m/s)とし，目標位置と姿勢を y_d= 0.0(m),θ_d= 0(deg)としてロボットに与える．また，ロボットの初期位置と姿 勢を^Wx= 0.0(m),^Wy= 0.0(m), θ = 0(deg)とし，ロボットは図2.14に示すように位 置，姿勢の初期誤差がない状態から移動を開始する．ロボットが目標軌道を追従し て移動するときの位置^Wx, ^Wy，姿勢θを，ポジションセンサ（浜松ホトニクス社製 C5949）によって測定する．

ロボットの姿勢θについて検討する．本節の実験では，ロボットに清掃機構は搭載 しておらず，ロボットは清掃しないが，清掃を想定した移動動作を行うものとする．

図1.14に示す本研究で採用する清掃機構の長さL_cを，ロボットの直径2R = 0.34m を考慮して，図2.15に示すようにL_c = 0.15mと設定する．また，ロボットの中心

x y

Σ

W W

θ = 0

Fig. 2.14Initial position and orientation without initial position error

Cleaning unit Sponge+water Squeegee

L_c=0.15m

R_=0.17m L_s=0.12m

Fig. 2.15 Placement of the cleaning mechanism

から清掃機構までの距離をL_s= 0.12mとする．この距離があるため，ロボットの姿 勢θの変動が大きくなると，清掃機構が通る場所が波状となり，意図した場所を清 掃できなくなる．そこで，目標とする水平な直線に対して，実際のロボットの姿勢 θと目標姿勢θdの差が±10^◦以内となるように，軌道追従制御ゲインkx,ky,kθを設 定した．

ロボットの位置^Wyについて検討する．ロボットは図1.17の(a)に示す水平パラレ ル動作軌道を追従して，水平な一列ごとに清掃を行う．このとき，清掃機構が通ら ない場所をなくすために，清掃機構が通る場所が重複するように，図2.16に示す水 平な列間の距離L_lを設定する．ただし，重複が大きくなるほど直進する回数が増え，

清掃時間が長くなる．そこで，清掃機構の長さL_c = 0.15mを考慮して，水平な列 間の距離をL_l= 0.07mと設定する．姿勢θの誤差による清掃機構の鉛直方向の位置 の変動も考慮すると，実際のロボットの位置^Wyと目標とする水平な直線に対する鉛 直方向の目標位置ydの差が±(Lc−Ll−2Lssin 10^◦)/2 =±0.02m以内なら，ロボッ トは清掃機構が通らない場所がないように移動可能である．

ロボットの位置^Wxについて検討する．実際のロボットの位置^Wxと水平方向の目標 位置x_dの差は清掃には大きく影響しないため，高い制御精度は必要でない．ロボッ トの半径がR = 0.17mであることを考慮して，実際のロボットの位置^Wxと水平方

Direction of gravity Robot

W indow frame Glass

L_l L_l

Fig. 2.16 Distance between horizontal lines

向の目標位置x_dの差が±0.17m以内となるよう，軌道追従制御ゲインk_x, k_y, k_θを 設定した．

ロボットの位置^Wx,^Wyと姿勢θについての検討より，ロボットが目標とする水平 な直線を，水平方向の位置の誤差が±0.17m以内，鉛直方向の位置の誤差が±0.02m 以内，姿勢の誤差が±10^◦以内で追従するとき，ロボットが目標軌道での水平方向 への直進が可能であるとする．提案する軌道追従制御法によって，ロボットが目標 軌道での水平方向への直進が可能であることを確認する．

次に，ロボットの位置^Wx, ^Wy，姿勢θが検出可能である状態について，具体的に 議論する．前節で提案した窓枠位置情報を利用した位置検出法によって検出したロ ボットの位置，姿勢と，ポジションセンサ（浜松ホトニクス社製C5949）によって 測定した結果を比較する．

ロボットが目標軌道での水平方向への直進が可能である状態についての議論を考 慮して，鉛直方向の位置の検出誤差∆^Wyを，鉛直方向の位置の制御誤差の半分以下

として±0.01m以内とした．同様に，姿勢の検出誤差∆θを，姿勢の制御誤差の半

分以下として±0.01m以内とした．また，水平方向の位置の検出誤差∆^Wxについて は，水平方向の位置の要求制御精度が鉛直方向の位置の要求制御精度と比較して低 いため，∆^Wyの2倍以内として±0.02m以内とした．これらが実現できたとき，ロ ボットの位置^Wx, ^Wy，姿勢θが検出可能であるとする．前節で提案した窓枠位置情 報を利用した位置検出法によって，ロボットの位置^Wx, ^Wy，姿勢θが検出可能であ ることを確認する．

Table 2.2Parameters of motors

Gear ratio γ 72

Gear efficiency η_G 0.732

Armature resistance R_M 2.8Ω Torque constant kM 0.0193N·m/A Back electromotive force constant K_e 0.009V·s/rad

Moment of inertia J_M 20.6g·cm²

式(2.13)に示す，重力とその他の動力学による影響を補償する軌道追従制御入力

トルクτ を，以下のDCモータのダイナミクスに代入する．

v = R_M

k_M (J_Mγ¨q+ τ

γη_G) +K_eγq˙

= R_M

k_M (J_Mγ¨q+ 1

γη_G{M J⁻¹[¨p_c+K_p( ˙p_c−p) +˙ K_i(p_c−p)] +D(p,q)˙ })

+K_eγq˙ (2.43)

提案手法では，この入力電圧v = [vL, vR]^T をモータに印加する．pおよびp˙ は，左 右車輪のエンコーダから得られるqとその微分q˙ を式(2.8)により変換して得られ る．また，使用したDCモータの諸元を表2.2に示す．試行錯誤により，目標軌道 に対する鉛直方向の位置の誤差と姿勢の誤差が小さくなるように，軌道追従制御ゲ インをk_x = 0.1(1/s), k_y = 200(rad/m²), k_θ = 50(rad/m)と，速度制御ゲインを k_p = 1000(1/s), k_i = 750(1/s²)と決定した．

比較のための姿勢制御のみの手法の制御入力速度を以下のようにする．

p˙_ca= [

V_ca Ω_ca

]

= [

V_d Ω_d+k_θaθ_e

]

(2.44) ここで，kθaは正の値をもつ姿勢制御ゲインである．提案手法の式(2.12)と同様に速 度のPI制御を追加して，モータへの入力電圧va = [vaL, vaR]^T を以下のようにする．

v_a=J⁻¹{K_pa( ˙p_ca−p) +˙ K_ia(p_ca−p)} (2.45) ここで，Kpa = diag(k_pa, k_pa), K_ia = diag(k_ia, k_ia)であり，kpa, k_iaは正の値をも つゲインである．試行錯誤により目標軌道に対する姿勢の誤差が小さくなるよう

t(s) 0

Proposed control Attitude control 0.0

2 4 6 8 14

0.05 0.1 0.15 0.2 0.25 0.3

-0.05 -0.1 -0.15 -0.2 -0.25

-0.3 10 12

Wx-x

d(m)

Fig. 2.17Horizontal position error of the robot without initial position error

に，姿勢制御ゲインをk_θa = 5.75(rad/s)と，速度制御ゲインをk_pa = 2500(Vs), k_ia = 35000(V)と決定した．

提案した軌道追従制御法による試行と，姿勢制御のみの手法による試行をそれぞ れ5回ずつ行い，ポジションセンサ（浜松ホトニクス社製C5949）によって，ロボッ トの位置^Wx,^Wy，姿勢θを測定した．時刻tにおける，ロボットの位置^Wx,^Wy，姿勢 θと，ロボットの目標位置x_d, y_d，姿勢θ_dの差の平均値と標準偏差を，それぞれ図 2.17∼2.19に示す．横軸は時刻tを，縦軸はそれぞれ(^Wx−x_d), (^Wy−y_d), (θ −θ_d) を表す．赤色の実線が提案手法による結果の平均値を，青色の破線が姿勢制御のみ の手法による結果の平均値を示す．また，範囲を示すI 印がそれぞれの標準偏差を 表す．そして，このときのロボットの軌跡の平均値と標準偏差を図2.20に示す．横 軸はロボットの水平方向の位置^Wxを，縦軸はロボットの鉛直方向の位置^Wyを表す．

黒色の一点鎖線が目標軌道を，赤色の実線が提案手法による結果の平均値を，青色 の破線が姿勢制御のみの手法による結果の平均値を示す．また，範囲を示すI 印が それぞれの標準偏差を示す．

図2.17∼2.19より，提案した軌道追従制御法によって，ロボットは目標とする水平 な直線を，水平方向の位置の誤差が±0.17m以内，鉛直方向の位置の誤差が±0.02m

t(s) 0

Wy-y_d (m)

Proposed control Attitude control 0.0

2 4 6 8 14

0.02 0.04 0.06 0.08 0.1 0.12

-0.02 -0.04 -0.06 -0.08 -0.1 -0.12

12 10

Fig. 2.18Vertical position error of the robot without initial position error

t(s) 0

- (deg)

Proposed control Attitude control 0

2 4 6 8 14

5 10 15 20 25 30

-5 -10 -15 -20 -25

-30 12

θ

10 θ d

Fig. 2.19Orientation error of the robot without initial position error

以内，姿勢の誤差が±10^◦以内で追従している．これに対して，図2.20より姿勢制 御のみの手法による場合は，水平方向に1.05m進む間に鉛直方向に約0.1(m)下がっ ている．したがって，提案した軌道追従制御法によって，姿勢制御のみの手法では

w

0.0

wy(m) 0.0

0.2 0.4

x(m) Desired path Proposed control Attitude control

0.6 0.8 1.0 1.2

0.02 0.04 0.06 0.08 0.1 0.12

-0.02 -0.04 -0.06 -0.08 -0.1 -0.12

Fig. 2.20 Trajectory for the desired path without initial position error

困難な目標軌道での水平方向への直進が可能となることが分かる．これにより，窓 ガラス清掃ロボットが，窓ガラス全面を少ない重複の軌道で移動可能な水平パラレ ル動作による目標軌道を追従可能となる．

また，提案した軌道追従制御法による5回の試行において，2.4節で提案した窓 枠位置情報を利用した位置検出法によって検出したロボットの位置と姿勢を，ポジ ションセンサによって測定したロボットの位置^Wx, ^Wy，姿勢θと比較する．水平方 向の位置の検出誤差∆^Wx,鉛直方向の位置の検出誤差∆^Wy, 姿勢の検出誤差∆θの平 均値と標準偏差を，それぞれ図2.21∼2.23に示す．横軸は時刻tを，縦軸はそれぞ れ∆^Wx, ∆^Wy, ∆θを表す．赤色の実線がそれぞれ∆^Wx, ∆^Wy, ∆θの平均値を示す．ま た，範囲を示すI 印がそれぞれの標準偏差を示す．

図2.21∼2.23より，水平方向の位置の検出誤差∆^Wxが±0.02m以内，鉛直方向の 位置の検出誤差∆^Wyが±0.01m以内，姿勢の検出誤差∆θが±5^◦以内である．した がって，提案した窓枠位置情報を利用した位置検出法によって，ロボットから鉛直 な窓枠までの垂直な距離の最大値がLfw M AX = 0.6mであり，水平な窓枠までの垂 直な距離の最大値がL_{fh M AX} = 0.35mである本実験の場合は，ロボットの位置^Wx,

t(s) 0

∆ ^Wx(m) 0.0

2 4 6 8 14

0.01 0.02 0.03 0.04 0.05

-0.01 -0.02 -0.03 -0.04 -0.05

12 10

Fig. 2.21Error of horizontal position detection

t(s) 0

∆ ^Wy(m) 0.0

2 4 6 8 14

0.01 0.02 0.03 0.04 0.05

-0.01 -0.02 -0.03 -0.04 -0.05

12 10

Fig. 2.22Error of vertical position detection

Wy，姿勢θが検出可能であることが分かる．これにより，2.2節で提案した軌道追従 制御法を実現可能となる．

t(s) 0

∆ (deg) 0

2 4 6 8 14

5 10 15 20 25 30

-5 -10 -15 -20 -25

-30 12

θ

10

Fig. 2.23Error of orientation detection