クレーンの障害物回避のための軌道生成と軌道追従
M2014SC006 伊藤大智
指導教員：大石泰章

1

はじめに

クレーンは建設や輸送，製造などで頻繁に使用される機械であるが，いつも一定の環境で使用されるとは限らない．例えば工場の製造ラインでは，扱う部品やその数などに応じて，ラインの配置を変更することがある．その際の配置によっては，クレーンの可動範囲内に障害物である他の機械が存在し，可動範囲が限定されてしまうため，障害物を回避する制御を行う必要がある．
これまでのクレーンの障害物回避の研究では，軌道計画に三次元ポテンシャル関数を用いた研究 [1] や，スプライン関数を用いた研究 [2]，リアルタイムで障害物を観測し障害物を回避する研究 [3] がある．
本研究では，ロボット工学で使用される軌道生成手法をクレーンに適用する．具体的には，人工ポテンシャル法を用いて，目標位置と障害物の位置からポテンシャル関数を形成し，最急降下法によって勾配の逆方向に沿った軌道を求める [4, 5]．さらに，この軌道に沿ってペイロードが運搬されるように，ジブの旋回とトロリーの移動の2 つの自由度を使い，計算トルク法を使った制御を行う．
計算トルク法は，マニピュレータの制御でよく使用される [6]．
通常計算トルク法は，全ての状態変数に入力を加えることができる場合に使用されている．しかし本研究で使用するクレーンは，スチールケーブルの振れ角には直接入力を加えることができないため，別途振れ止め制御を行う必要がある．本研究では，計算トルク法による軌道追従制御に加え，振れ止め制御として最適ロバストサー
ボを適用した手法を提案する．本研究の検証によって，障害物が存在する場合の生成軌道への追従制御，およびペイロードの安定した運搬を実現できると考えられる．

2

制御対象と目的

2.1

制御対象

図 1 クレーンの概略図

p[m]M [kg]
Jθ[kgm2 ]
Kj
[N·m/A]
Kt[N·m/A]
Ij
[A]
It[A]
x[m]
y[m]
z[m]
m[kg]
θ[rad]
α[rad]
γ[rad]
l[m]g[m/s2 ]表 1 本論文で用いる記号
トロリーの位置0 ≤ p ≤ 0.6
トロリーの質量0.6ジブの旋回運動0.8771の慣性モーメント
ジブの旋回用モータ
0.0650のトルク定数
トロリー移動用モータ
0.0436のトルク定数
ジブ移動用モータへの入力電流
トロリー移動用モータへの入力電流
ペイロードの x 座標
ペイロードの y 座標
ペイロードの z 座標
ペイロードの質量0.8680
ジブの旋回角度
スチールケーブルの円周方向の振れ角
スチールケーブルの半径方向の振れ角
スチールケーブルの長さ
0.5
重力加速度
9.81

本研究では，Quanser 社の 3 DOF Crane[7] を使用する．このクレーンの概略図を図 1 に示す．
このクレーンは，鉛直方向に伸びたタワーの上部から，水平方向にジブが伸び，ジブに沿ってトロリーが移動する構造をしている．ここで制御できるのは，ジブの旋回角度，トロリーの位置，スチールケーブルの長さの 3 つの自由度である．本研究では，スチールケーブルの長さ (x , y ) ，i 番目の障害物の位置を (x(i) , y (i) ) ，目標位置ss00を固定し，ジブの旋回角度とトロリーの位置の 2 つの自 を (x , y ) とする．
gg
由度を使用し，ペイロードの位置を制御する．
トロリーから吊り下げられたペイロードを，ジブの旋回モデリングに用いる記号を表 1 に示す．
運動とトロリーの位置を変化させることによって，初期

2.2

制御目的

本研究のクレーンにおいては，回転軸上から見た平面上での障害物回避を行うことを考える．図 2 に障害物が2 つある場合の概略図を示す．ペイロードの初期位置を位置から目標位置まで運搬する．
障害物回避を行う上で，以下の仮定をおく：(1) 障害物は，ジブの高さより低い位置にある；(2) 障害物は移動しない．

(2)(2)の障害物の位置を (xs , ys ) = (0.3, 0.35) とし，また重み h1 , h2 はそれぞれ，h1 = 35, h2 = 30 とした．
図 3 と図 4 から，目標位置に近づくほどポテンシャル関数 U の値が小さくなっていることが確認でき，また障害物の位置に近づくほどポテンシャル関数 U の値が大きくなっていることが確認できる．

図 2 クレーンを回転軸上から見た概略図

3人工ポテンシャル法

本章では，障害物のある空間での軌道決定でよく用いられる，人工ポテンシャル法について述べる．
人工ポテンシャル法とは，目標位置と障害物の位置からポテンシャル関数を定義し，現在の位置からポテンシャル関数の勾配が最も急な降下方向に進み，軌道生成を行う手法である．本研究では，最急降下法を用いて，ペイロードの目標位置であるポテンシャル関数の最適解に収束する軌道を求める．

3.1

図 3 ポテンシャル関数のグラフ

ポテンシャル関数

本研究では，文献 [5, 8] に従い，次のようなポテンシャル関数を考える．ポテンシャル関数は，目標位置に関する関数と障害物の位置に関する関数を足し合わせたものである．また，目標位置に関する関数は目標位置で最小値をとり，障害物の位置に関する関数は障害物の位置で+∞ に発散するようなものである．
目標位置に関する関数は，

Ug = (x − xg )2 + (y − yg )2(1)と与える．
障害物の位置に関する関数は，

Us =n∑

1√(i) 2(i)i=1(x − xs ) + (y − ys )2(2)と与える．ただし，n は障害物の数である．
ポテンシャル関数は，

図 4 ポテンシャル関数の等高線図

3.2

最急降下法

最急降下法は，与えられた関数が最小となる位置を求める方法であり，初期位置から出発して最も急な勾配で関数値が減少する方向を求め，その方向に適当なステップ幅で進むことを繰り返す [9]．本研究では，前節で考えたポテンシャル関数に最急降下法を適用することで，障害物を避けつつ，初期位置から目標位置に至る軌道を求める．

3.3

軌道の生成

3.1 節で形成したポテンシャル関数と 3.2 節のアルゴリズムによって得られた，初期位置から目標位置までの軌道で表される [5]．h1 , h2 > 0 はそれぞれ，目標位置と障害 を図 5 に示す．図 5 は，3.1 節の図 3，図 4 の場合の軌道を求めた．ペイロードの初期位置は (x0 , y0 ) = (0.4, 0.4)
物の位置に関する関数にかかる重みである．
図 3 と図 4 にポテンシャル関数 U の例を示す．図 3 は である．
図 5 から，ペイロードは初期位置 (x0 , y0 ) = (0.4, 0.4)
ポテンシャル関数のグラフ，図 4 はポテンシャル関数のから目標位置(xg , yg ) = (0.1, 0.1) に収束し，かつ 2 つの等高線図である．今回の例では障害物を 2 つ置いた場合を考え，ペイロードの目標位置を (xg , yg ) = (0.1, 0.1)， 障害物の位置を考慮し回避する軌道を描いていることが(1)(1)
1 つ目の障害物の位置を (xs , ys ) = (0.3, 0.2)，2 つ目 確認できる．
U = h1 Ug + h2 Us

(3)

4.2

計算トルク法の比例定数最適化

式 (6) で用いられる Kv , Kp は，試行錯誤で正の定数を選ぶが，本研究では，以下の評価関数が最小となる比例定数を用いる：∫ T
Qp ∥ξd (t) − ξ(t)∥ + Rp ∥τ ∥ dt．

Jp =(7)0

ただし，Qp , Rp > 0 は重み，∥ · ∥ はユークリッドノルムを示す．

4.3
4.3.1
図 5 最急降下法が生成したペイロードの軌道制御系設計

4

本研究では，軌道追従の制御手法として計算トルク法を用いる．通常計算トルク法は，全ての状態変数に入力を加えられることを前提としているが，本研究で用いるクレーンの場合，振れ角に対して入力を加えることができず，計算トルク法が適用できない．そのため，ペイロードを軌道に追従させる為には，別途振れ角の制御を行う必要がある．
ここで，軌道追従の課題を 2 つに分けて考える．スチー
ルケーブルが振れず，トロリー下にペイロードが常に存在するという仮定の下，3.3 節で生成した軌道を極座標に変換し，それぞれジブの旋回運動とトロリー位置の目標軌道とし，計算トルク法によって追従させる．加えて，軌道追従を行う際に生じるスチールケーブルの振れ角を打ち消す制御手法として最適ロバストサーボを用いる1 ．

4.1

計算トルク法による軌道追従制御(4)のように整理する．ただし，τ (t) = [Ij (t) It (t)]T ，M (ξ)˙ は 2 × 1 の遠心力やコリオは 2 × 2 の慣性行列，V (ξ, ξ)リ力を示す項，G(ξ) は 2 × 1 の重力を示す項である．
計算トルク法では，˙τ (t) = M (ξ(t))τ ′ (t) + V (ξ(t), ξ(t))+ G(ξ(t))

最適サーボによる振れ止め制御を行うため，状態変数
ζ(t) を ζ(t) = [α(t) γ(t) α̇(t) γ̇(t)]T とし，モデル化する．
以下の仮定をおく：(1) クレーンとスチールケーブルは剛体である；(2) ペイロードは質点である； (3) θ ≃ 0，p ≃ 0，α ≃ 0，γ ≃ 0 として線形化を行う；(4) 入力 τ とスチールケーブルの振れ角 α, γ の積は 0 とする．
上記の条件より，状態空間表現{
ζ̇(t) = Aζ(t) + Bτ (t),(8)η(t) = Cζ(t)を得る．ただし，
001 0
 000 1
A= g,− l00 0m0 − gl (1 + M) 0 0[]T[−Kj p0 001Jθ lB=,C=Kt
00 00Ml]
0100004.3.2

最適ロバストサーボ

スチールケーブルの円周方向の振れ角の目標値 αref = 0との偏差と半径方向の振れ角の目標値 γref = 0 との偏差を e(t) = [αref (t) − α(t) γref (t) − γ(t)]，偏差の積分を∫tw(t) = 0 e(t)dt で表すと，状態変数の拡大系は

ζe = [α(t) γ(t) α̇(t) γ̇(t) w(t)]T(9)となる．また，w(t), ζ(t), τ (t) の定常値をそれぞれw∞ , ζ∞ , τ∞ とし，ζ˜e = [ζ(t) − ζ∞ w(t) − w∞ ]T , τ̃ (t) =τ (t) − τ∞ と定義すると，拡大偏差システム(5)とし，さらに式 (5) の τ ′ を入力と考え，

˙ + Kp (ξd − ξ)
τ ′ = ξ¨d + Kv (ξ˙d − ξ)モデリングである．

本研究で用いたクレーンの一般化座標を
θ(t), p(t), α(t), γ(t) と し ，ラ グ ラ ン ジュの 運 動 方 程式よりクレーンの数学モデルを求める．
状態変数 ξ(t) を ξ(t) = [θ(t) p(t)]T とし，動力学モデル¨ + V (ξ(t), ξ(t))˙
τ (t) = M (ξ(t))ξ(t)+ G(ξ(t))

最適ロバストサーボによる振れ止め制御(6)とすることで，軌道追従制御を行う．ただし，Kv , Kp > 0は速度，位置に対する比例定数，ξ¨d , ξ˙d , ξd は加速度，速度，位置の目標値を示す [10]．
1 振れ角を零に収束させるのが目的であるので，本来最適レギュレータを使えばよいはずであるが，適用してみたところ十分な性能が得られなかったので，最適サーボを使用する．

{˙ζ˜e (t) = Ae ζ˜e (t) + Be τ̃ (t)e(t) = Ce ζ˜e (t)(10)を得る [11]．ただし，[][][]
A O4×2B
Ae =, Be =, Ce = −C O2×2
−C O2×2O2×2である．Om×n は，m 行 n 列の零行列である．
式 (10) の Be は，トロリーの位置 p に依存するため，pの変動範囲を考慮する必要がある．ここでは，Be を行列多面体とし，ロバスト安定性を考慮した最適ロバストサー
ボを提案する．
以下の LMI 条件のもと，X, F, Z の存在する範囲でtrace Z を最小化するように，最適サーボのフィードバッ
クゲイン Ke = [K G] を求める [11]：[]
−He Ae X + Be F
XQh F T R
(11)Qh X
I0  > 0,比べて，赤いラインの計算トルク法に最適ロバストサーボを加えた場合は，赤のラインの方が目標軌道との誤差が少なくなっており，振れ止め制御の効果も確認できる．

用いる Qp , Rp をそれぞれ Qp = 26000, Rp = 1 とすると
き，得られたゲイン Kv , Kp は参考文献

5

おわりに

本研究では人工ポテンシャル法を用いて，目標位置と障害物の位置からポテンシャル関数を形成し，最急降下法によって直線探索を行い，クレーンのペイロードが障害物を回避するための軌道を求めた．また，クレーンのペRF0Rイロードを求めた軌道に追従させる制御手法として，計[]Z I
算トルク法と最適ロバストサーボを適用し，障害物回避> 0.
(12)
I Xに成功した．
本研究では，人工ポテンシャル法による軌道生成を行っ
ただし，Qh > 0 は状態に対する重み行列の行列平方根，たが，クレーンの動特性を考慮してポテンシャル関数をR > 0 は入力に対する重み行列である．
作ることができればよりよいと思われる．また，振れ止
4.4 シミュレーション結果
め制御で用いた最適ロバストサーボにおいては，動特性4.1, 4.2, 4.3 節の方法で制御することにし，シミュレー を線形化してしまったため，振れ止めの精度が良いとはいえない．
ションを行う．
今後の課題として，クレーンの動特性を考慮したポテ
図 6 に，計算トルク法のみの場合と最適ロバストサー
ボによる振れ止め制御を適用した場合のシミュレーショ ンシャル関数の形成，かつ振れ止め制御の際に非線形項ン結果を示す．今回，計算トルク法の比例定数最適化で を考慮した入力を求めることが挙げられる．

Kv = [150 50], Kp = [1510 140]と なった ．ま た ，最 適 ロ バ ス ト サ ー ボ に よ る 振れ 止 め 制 御 の 重 み 行 列 Qh , R を そ れ ぞ れ Qh =diag(10000, 10000, 8200, 8700, 300000, 1000000), R=diag(2, 400)，ト ロ リ ー の 位 置 p の 変 動 範 囲 を 0.1〜
0.57[m] とし，得られたゲイン Ke = [K G] は[11.0505
Ke =0.00000.0000 −42.1180−0.6809
0.0000−0.0000
3.761949.9449−0.0000]
0.0000−6.4037となった．

[1] 鈴木薪雄，寺嶋一彦：天井クレーンのポテンシャル法を用いた三次元パスプランニング，日本ロボット学会
誌，Vol. 18，No. 5，pp. 728–736，2000．
[2] 林喜章，山本元司：旋回クレーンの軌道追従制御と衝突回避軌道の生成，ロボティクス・メカトロニクス
講演会講演概要集，1P1-B02(1)–1P1-B02(3)，東京，2006．
[3] 永井 他：天井クレーンの 3 次元オンライン経路計画，
第 54 回自動制御連合講演会予稿集，pp. 973–976，豊橋，2012．
[4] 辻 他：収束時間を調節可能な人工ポテンシャル法によるマニピュレータの動作計画，日本ロボット学会誌，Vol. 13，No. 2，pp. 285–290，1995．
[5] 彌城 他：ポテンシャル法によるロボット製品の障害物回避技術の開発，三菱重工技報，Vol. 51，No. 1，2014．
[6] 今村智貴：マニュピレータの軌道追従制御，南山大学
大学院理工学研究科修士論文，2015．
[7] 3 DOF Crane,
http://www.quaser.com/products/3dofcrane[8] 中野 他：『高知能移動ロボティクス』．講談社サイエ
ンティフィク，東京，2004．

図 6 シミュレーション結果
図 6 の結果から，スチールケーブルの振れによる影響によって多少目標軌道との誤差はあるものの，ほぼ軌道に追従できており，障害物回避の軌道追従に成功したといえる．また，青のラインの計算トルク法のみの場合に[9] 福島雅夫：『新版数理計画入門』．朝倉書店，東京，2012．
[10] J．J．Craig：『ロボティクス—機構・力学・制御—』．
共立出版，東京，1991．
[11] 川田昌克：『MATLAB/Simulink による現代制御入門』．森北出版，2011．