1. はじめに
これまでの記事で、Open Robotics の静止モデル tower_crane を Blender で分割・関節化し、
円筒座標 (θ, r, h) の3軸——旋回・横行・巻上げ——で動くタワークレーンを Gazebo Classic 上に作りました。
- 旋回（yaw）化:
- 横行（trolley）・巻上げ（hoist）追加:
ここからは以下のリンクの論文「Automating the Tower Crane: Integrating the Development and Simulation of Path Planning and Trajectory Tracking of Tower Crane in ROS Framework」(ISARC 2024) に倣って、ROS/ROS2 で自動搬送の再現に本格的に取り組んでいきたいと思います。
ところが着手してすぐに、いまのモデルではそもそも論文の問題が発生しないことが分かりました。
論文が扱うタワークレーンは 5自由度のシステムです。
- 作動する3自由度：旋回 γ、横行 x、巻上げ l
- 作動しない2自由度：吊荷の振れ φ（半径方向）、θ（接線方向）
吊荷を直接押して止める装置は存在しません。トロリとジブの動かし方だけで間接的に振れを消すしかない。
これが難しいので論文になっているわけです。
一方、私がここまで作ってきたモデルはフックが固定しているので、どれだけ乱暴に動かしても荷は揺れません。
そこでこの記事では、論文の再現に入る前の土台づくりとして次の3つを実施しました。
- 既存の SDF モデルを URDF 化し、振れ2自由度を追加して5自由度にする
- ROS 2 Humble + gazebo_ros2_control で力（トルク）指令を受け付けるようにする
- 追加した振り子の物理が正しいことを、3つの実験で実測して確かめる
制御そのものは次の記事で扱おうと思います。この記事は「クレーンで吊った時の吊荷の揺れ」を作るところまでです。
先にお断りしておくと、私は制御工学を専門に学んだわけではありません。作って動かしながら覚えている段階なので、特に理論の解釈まわりには間違った理解が混じっている可能性があります。おかしなところに気づかれた方は、ぜひコメントで教えていただけると助かります。
なお、途中でいくつか派手にハマりました。特に 「非作動関節を <ros2_control> に登録してはいけない」 と 「メッシュを package:// で書くと Gazebo の描画が止まる」 の2つは、症状が分かりにくく原因にたどり着くまで時間がかかったので、切り分けの過程も含めて書いておきます。
完成すると、こんな動きになります:
2. 実行環境
| 項目 | 内容 |
|---|---|
| OS | Ubuntu 22.04 LTS |
| シミュレータ | Gazebo Classic 11.10.2 / SDF 1.6 |
| 物理エンジン | ODE (Open Dynamics Engine) |
| 主要パッケージ | gazebo_ros_pkgs, gazebo_ros2_control, ros2_control |
| 解析 | Python 3.10 + NumPy 1.26 / SciPy 1.15 / matplotlib 3.10 |
ワークスペースの構成は次のとおりです。既存の Gazebo 単体用モデル（models/tower_crane_rot/）
はそのまま残し、ROS 2 用のパッケージを src/ に追加する形にしました。
tower_crane_ws/
├─ models/tower_crane_rot/ これまでの3自由度 SDF モデル（メッシュの単一ソース）
│ ├─ model.sdf model.config
│ └─ meshes/ base.obj upper.obj trolley.obj hook.obj
├─ src/
│ ├─ tower_crane_description/ 5自由度 URDF・ワールド・起動 launch
│ └─ tower_crane_control/ 保持制御・ロガー・実験 launch・解析
3. 5自由度モデルの設計
3.1 関節チェーン
追加する振れ2関節は revolute 2つで作ります。ダミーリンクを挟んで直交2軸にすることで、自在継手（ユニバーサルジョイント）相当の球面振り子になります。
world -(fixed)- base_link -(yaw γ)- upper_link -(trolley x)- trolley_link
-(swing_radial φ)- swing_r_link -(swing_tangential θ)- swing_t_link
-(hoist d)- hook_link
ここで大事なのは、振れ2関節を巻上げのプリズマティック関節より「上」に置くことです。こうすると振り子の支点がトロリになり、振り子長 $l$ がケーブル長そのものになります。逆にフックの下に振れ関節を置いてしまうと、巻上げても振り子長が変わらない別物のモデルになってしまいます。
もう一つ大事なのが 2関節の順序 です。論文[1]の式(1) は吊荷の位置を
\mathbf{x}_P =
\begin{bmatrix}
x - l \cos\theta \sin\phi \\
l \sin\theta \\
- l \cos\theta \cos\phi
\end{bmatrix}
と書いています。これは支点から下ろした鉛直ベクトルに θ を先に、φ を後に掛けた形、つまり φ を外側（トロリ側）、θ を内側（フック側） に置いた場合の式です。上のチェーンはこの順序になっています。
ただし接線方向成分だけは、論文の式(1) に対して $-l\sin\theta$ と符号が反転します。これは次節の符号の約束によるもので、意図的です。
3.2 座標系と符号の約束
メッシュの都合で、このモデルはジブの長手方向（外向き）がモデル座標の -Y です。したがって接線方向は $\hat{t} = \hat{z} \times \hat{r}$ より +X になります。
振れ角の正の向きは、論文[1]の式(8)と(10)の運動方程式に合わせて 「支点の運動に対して荷が遅れる向き」を正と定義しました。
\begin{aligned}
l \ddot{\phi} + g \phi - \ddot{x} &= 0 && (8) \\
l \ddot{\theta} + g \theta - x \ddot{\gamma} &= 0 && (10)
\end{aligned}
一定の加速度で走り続けて、振れ角が落ち着いた状態（$\ddot\phi \simeq 0,\ \ddot\theta \simeq 0$）では、次の関係になります。
\phi \simeq \frac{\ddot{x}}{g}, \qquad
\theta \simeq \frac{x \ddot{\gamma}}{g}
トロリが外向きに加速すれば φ が正（荷は内側に残る）、ジブが正方向に加速すれば θ が正（荷は回転の後ろ側に残る）です。この定義に合わせると、φ の回転軸は +X、θ の回転軸は +Y になります。
論文[1]の式(1) と式(10) の不整合
式(1) の θ の定義（$+\theta$ で荷が接線加速度と同じ向き＝進行側に出る）で運動方程式を立て直すと、接線方向の式は
l \ddot{\theta} + g \theta + x \ddot{\gamma} = 0
となるはずで、論文[1]の式(10) とは $x\ddot\gamma$ の符号が逆です。一方 φ 側は、式(1)（$+\phi$ で荷が遅れ側に出る）と式(8) が整合しています。つまりズレているのは θ だけで、式(10) は式(1) と逆の「遅れ側を正」という定義を暗黙に使っていることになります。
制御則の設計に使われるのは式(7)〜(10) なので、この記事では式(10) 側、すなわち φ・θ とも遅れ側を正に統一しました。後述の実験A3 で、実装がこの符号どおりになっていることを実測で確認します。
3.3 質量と寸法
寸法は既存の model.sdf からそのまま移植しました。メッシュの実測値で検算しています。
| 項目 | 記号 | 値 | 備考 |
|---|---|---|---|
| マスト高さ | — | 約 25.99 m |
base.obj 1039.6 units × 0.025 |
| 全高 | — | 約 33.45 m | |
| トロリ半径 | $x$ | 2.0 〜 22.0 m | 可動範囲 |
| ケーブル長 | $l$ | 3.4 〜 26.4 m | 可動範囲 |
| 吊荷質量 | $m$ | 1000 kg | |
| トロリ質量 | $M$ | 500 kg | |
| 上部旋回体 質量 | — | 3000 kg | |
| 上部旋回体 慣性モーメント | $J_o$ | $1 \times 10^6$ kg·m² | 旋回軸まわり（izz） |
論文[1]はトロリ質量・慣性モーメント・吊荷質量を明示していないため、実寸のクレーンとして妥当な値を独自に設定しました。
そのため入力（F_x, T_γ, F_l）の絶対値は論文と一致しませんが、振れ角 φ, θ は質量に依存しません。
式(8),(10) を見れば分かるとおり、振れの運動方程式に質量が入っていないためです。
つまり論文の Fig.5/6 と比較するとき、振れ角はそのまま比較できるということになります。これは次の記事で効いてきます。
4. URDF を書く
SDF のままでも Gazebo は動きますが、ros2_control も MoveIt も URDF が前提なので、ここで URDF に移行します。
src/tower_crane_description/urdf/tower_crane.urdf.xacro の要点を抜粋します。
4.1 メッシュは package:// ではなく file:// で書く
これが後述する描画停止の原因になった箇所です。先に結論を書いておきます。
<!-- メッシュの場所。package:// は使わないこと -->
<xacro:property name="mesh_dir"
value="file://$(find tower_crane_description)/meshes"/>
URDF の教科書どおりに package://tower_crane_description/meshes/base.obj と書くと、
物理は完全に正常に動くのに描画だけが死にます。詳細は 9 章に書きます。
4.2 振れ関節
<!-- 半径方向の振れ φ（非作動）。軸 +X: 正 = 荷が内側へ遅れる -->
<link name="swing_r_link"><xacro:dummy_inertial/></link>
<joint name="swing_radial_joint" type="revolute">
<parent link="trolley_link"/>
<child link="swing_r_link"/>
<origin xyz="0 0 ${pivot_dz}" rpy="0 0 0"/>
<axis xyz="1 0 0"/>
<limit lower="-1.5708" upper="1.5708" effort="1.0e7" velocity="10.0"/>
<dynamics damping="${damp_swing}"/>
</joint>
<!-- 接線方向の振れ θ（非作動）。軸 +Y -->
<link name="swing_t_link"><xacro:dummy_inertial/></link>
<joint name="swing_tangential_joint" type="revolute">
<parent link="swing_r_link"/>
<child link="swing_t_link"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-1.5708" upper="1.5708" effort="1.0e7" velocity="10.0"/>
<dynamics damping="${damp_swing}"/>
</joint>
effort を 1.0e7 と極端に大きく取っているのは、Gazebo が Joint::SetForce をこの値でクランプするからです。
既定値のまま（例えば 1000 Nm）だと、外部から /apply_joint_effort などで加えた励起トルクが勝手に切り詰められます。
非作動関節なのでリミット自体に物理的な意味はなく、大きくしても害はありません。
もっとも、その励起方法は 7.1 節でやめたので、いまは名残です。
4.3 ダミーリンクの質量
振れ関節を作るためのダミーリンクは、質量ゼロに近づけたくなります。しかし ODE はこれを嫌います。
<xacro:macro name="dummy_inertial">
<inertial>
<origin xyz="0 0 0"/>
<mass value="50.0"/>
<inertia ixx="50" iyy="50" izz="50" ixy="0" ixz="0" iyz="0"/>
</inertial>
</xacro:macro>
最初 1 kg にしていたところ、1000 kg の吊荷と直結した瞬間に発散しました。
ODE の quickstep ソルバは隣接リンクの質量比が大きいと解が破綻します。50 kg に上げたら安定しました。
ダミーリンクの重心は振れ支点の上にあるので、質量を足しても振り子の復元トルクは増えません。慣性 50 も、振り子の慣性 m·l² ≈ 2.3e5 kg·m² に比べれば無視できます。
4.4 重力の扱い
既存の model.sdf では ODE を安定させるために全リンクを gravity=false にしていました。
振れを再現するには荷に重力が効くことが必須なので、hook_link だけ true にします。
<gazebo reference="base_link"> <gravity>false</gravity></gazebo>
<gazebo reference="upper_link"> <gravity>false</gravity></gazebo>
<gazebo reference="trolley_link"> <gravity>false</gravity></gazebo>
<gazebo reference="swing_r_link"> <gravity>false</gravity></gazebo>
<gazebo reference="swing_t_link"> <gravity>false</gravity></gazebo>
<!-- 振り子の復元力を生む唯一のリンク -->
<gazebo reference="hook_link"> <gravity>true</gravity><material>Gazebo/Orange</material></gazebo>
マストやジブを false のままにしておくことで、旋回関節にかかる拘束力を最小限に保ち、
これまで積み上げてきた安定条件（スケール 0.025 倍・上部旋回体の重心をジブ高さに置く・小さい step size）
を壊さずに済みます。
なお、構造材には <material> を指定しないでください。
Gazebo/Grey を指定したところ、このマテリアルは ambient が 0.3 と低いため、
ラチス（格子）構造の陰の面が潰れてクレーン全体が真っ黒に見えました。
旧 model.sdf が明るく写っていたのはマテリアル無指定だったからです。
色は URDF 側の <material> に一本化すると、RViz と Gazebo で同じ見た目になります。
5. ros2_control をつなぐ
論文の入力は力とトルク（F_x, T_γ, F_l）なので、effort_controllers/JointGroupEffortController を使います。
controller_manager:
ros__parameters:
update_rate: 200 # [Hz]
crane_effort_controller:
type: effort_controllers/JointGroupEffortController
crane_effort_controller:
ros__parameters:
joints:
- yaw_joint # T_γ [Nm]
- trolley_joint # F_x [N]
- hoist_joint # F_l [N]
5.1 非作動関節を <ros2_control> に書いてはいけない
ここが一番ハマったところです。
φ, θ を観測したいので、素直に考えると <ros2_control> に状態インタフェースだけ持つ関節として登録したくなります。実際そう書いても /joint_states に値は出ます。ところが、自由振動が異常な速さで減衰しました。初期振れ 0.15 rad が 5 秒でほぼ止まります。減衰比に直すと $\zeta \simeq 0.55$ 相当です。
エネルギー収支を取ってみると妙なことになっていました。
| 収支項目 | 値 |
|---|---|
| 振り子の力学的エネルギー（t = 0 s） | 1651.8 J |
| 振り子の力学的エネルギー（t = 6 s） | 19.3 J |
| 差し引き | −1632.5 J |
| アクチュエータ3軸がした仕事の合計 | −30.7 J |
| 説明のつかない散逸 | −1601.8 J |
1632 J 失われているのに、アクチュエータが吸収したのは 31 J だけ。どこにも行かずに消えています。そこで犯人を絞り込みました。
| 試したこと | 結果 |
|---|---|
関節の <dynamics damping> を 1.0 → 0.0
|
結果がビット単位で同一 |
同上を 1.0 → 2.0e5
|
結果がビット単位で同一 |
ODE ソルバを quick → world
|
ほぼ同一（ただし振れ関節を ros2_control に載せたままの比較） |
implicitSpringDamper を外す |
同一 |
| ros2_control を使わない純 Gazebo の同じ振り子 | 39 秒間で振幅 0.14998 → 0.14997 rad 周期は理論値と +0.27 % で一致 |
決め手は上の2行です。減衰係数を 0 にしても 2e5 にしても結果が1ビットも変わりません。 ソルバの精度の問題であれば、関節の減衰値を変えれば結果は必ず変わるはずです。1ビットも動かないということは、URDF に書いた値がそもそも物理エンジンに届いておらず、何かが関節を上書きしているということになります。そして ros2_control を使わない純 Gazebo では、同じ振り子がまったく減衰しません。
<ros2_control> に登録した関節は毎周期 GazeboSystem::write() の対象になり、コマンドインタフェースを持たせていなくても動力学が上書きされてしまうようです。この挙動は上流でも「stuck passive joints（刺さった非作動関節）の修正」として直っているようなので、バージョンによっては再現しないかもしれません。
そこで振れ関節は ros2_control に載せず、状態の publish だけを Gazebo のプラグインに任せることにしました。
<!-- 5関節すべての状態を publish する。joint_state_broadcaster は使わない -->
<gazebo>
<plugin name="crane_joint_state_publisher"
filename="libgazebo_ros_joint_state_publisher.so">
<ros><remapping>~/out:=/joint_states</remapping></ros>
<update_rate>200</update_rate>
<joint_name>yaw_joint</joint_name>
<joint_name>trolley_joint</joint_name>
<joint_name>hoist_joint</joint_name>
<joint_name>swing_radial_joint</joint_name>
<joint_name>swing_tangential_joint</joint_name>
</plugin>
</gazebo>
joint_state_broadcaster は使いません。/joint_states の publisher をこのプラグイン1つにしておけば、robot_state_publisher にも5関節ぶんの完全な情報が渡り、RViz の表示も正しくなります。
上の表の「ソルバを quick → world」は、この上書きが起きている状態での比較です。上書きが支配的な状態では下流のソルバの差が埋もれてしまうので、ソルバ側が効いていたかどうかはこの実験では判定できていません。
5.2 URDF のコメントに「:」を書くと controller_manager が起動しない
もう1つ、原因が分かりにくいものがありました。日本語コメントを充実させた URDF にしたところ、
gazebo_ros2_control が次のエラーを吐いて controller_manager が立ち上がらなくなりました。
[ERROR] [gazebo_ros2_control]: parser error Couldn't parse parameter override rule:
'--param robot_description:=<?xml version="1.0" ?>
...
gazebo_ros2_control は URDF の全文を
--ros-args --param robot_description:=<URDF全文> というコマンドライン引数として
controller_manager ノードに渡します。rcl はこの値を YAML として解釈するため、
コメント中の「〜:」（コロン+改行や空白）がマッピングと誤認され、パースに失敗していました。
実行時にコメントは不要なので、xacro を展開したあとコメントを落として渡すようにしました。
#!/usr/bin/env python3
"""xacro を展開し、XML コメントを除去した URDF を標準出力に書く。"""
import sys
import xacro
def strip_comments(node):
for child in list(node.childNodes):
if child.nodeType == child.COMMENT_NODE:
node.removeChild(child)
child.unlink()
elif child.hasChildNodes():
strip_comments(child)
def main(argv):
doc = xacro.process_file(argv[1],
mappings=dict(a.split(':=', 1) for a in argv[2:] if ':=' in a))
strip_comments(doc)
sys.stdout.write(doc.toxml())
return 0
if __name__ == '__main__':
sys.exit(main(sys.argv))
launch 側では xacro の代わりにこれを呼びます。これで .xacro 側のコメントは自由に書けます。
robot_description = ParameterValue(Command([
xacro_urdf, ' ', xacro_file,
' payload_mass:=', LaunchConfiguration('payload_mass'),
' hoist_init:=', LaunchConfiguration('hoist_init'),
]), value_type=str)
6. 起動シーケンス — 低速で起動して、あとから等速に戻す
ここも素直に書くと詰みます。順番に説明します。
まず、Gazebo を一時停止（pause:=true）で起動して、コントローラを有効化してから再開したくなります。
ところがこれは動きません。
[ERROR] [controller_manager]: Switch controller timed out after 5.000000 seconds!
[ERROR] [spawner]: Failed to activate controller : crane_effort_controller
ros2_control のコントローラ有効化は controller_manager の update() の中で実行されるので、
物理が止まっていると switch が完了せずタイムアウトします。
では等速で起動すればよいかというと、今度は effort controller が有効になるまでの数秒間、
1000 kg の吊荷が自由落下します。2 秒で約 20 m 落ちて関節リミットに激突し、ODE が壊れます。
そこで、わざと低速でシミュレーションを開始し、コントローラが有効になってから等速に戻すことにしました。
ワールド側で更新レートを 25 Hz（0.0125 倍速）にしておきます。
<physics type="ode">
<max_step_size>0.0005</max_step_size>
<!-- わざと低速(25Hz)で開始する。
pause 状態だとコントローラの activate が完了せずタイムアウトし、
等速で開始すると effort controller が有効になるまでに吊荷が自由落下する。
低速起動なら両方を避けられる。 -->
<real_time_update_rate>25</real_time_update_rate>
<ode>
<solver><type>world</type><iters>300</iters><sor>1.3</sor></solver>
<constraints><cfm>0.0</cfm><erp>0.8</erp></constraints>
</ode>
</physics>
ソルバを既定の quick（PGS の反復解法）ではなく world（直接解法）にしています。リンク6個の小さな系なので直接解法でも速度は問題になりません。5.1 節の減衰問題を追っていた時期に変えたもので、こちらがどれだけ効いたかは切り分けきれていません。
そして launch 側で、コントローラの spawner が終了したら gz physics で等速に引き上げます。
eff_spawner = Node(
package='controller_manager', executable='spawner', output='screen',
arguments=['crane_effort_controller', '--controller-manager', '/controller_manager'],
)
# コントローラ有効化後、低速起動(25Hz)から所定の更新レートへ引き上げる
speedup = ExecuteProcess(
cmd=['gz', 'physics', '-u', LaunchConfiguration('update_rate')],
output='screen',
)
return LaunchDescription([
# ...
RegisterEventHandler(OnProcessExit(target_action=spawn, on_exit=[eff_spawner])),
RegisterEventHandler(OnProcessExit(target_action=eff_spawner, on_exit=[speedup])),
])
低速で走っている 10 秒程度の間にシミュレーション時間は 0.1 秒ほどしか進まないので、
荷の落下も 5 cm 程度で済みます。2000 Hz × step 0.0005 s で実時間比 1.0 です。
7. 保持制御ノード
検証には「支点を固定したまま荷だけ振らせる」必要があるので、作動3関節を PD で保持するノードを書きます。
実機でいえばブレーキに相当します。
PD 制御は位置を保ついちばん簡単なやり方で、中身はばねとダンパーそのものです。目標位置からずれたぶんに比例した力で引き戻し（P＝比例）、動いている速さに比例したブレーキをかける（D＝微分）。この2つを足すだけです。ゲイン $k_p$ がばねの硬さ、$k_d$ がダンパーの効きにあたります。
巻上げはプリズマティック関節なので、重力補償を入れないと荷が落ちます。
論文の巻上げ PID（式(18)）の最小版でもあります。
def on_joint_state(self, msg):
s = CraneState.from_joint_state(msg)
# 初回は実測値を目標に取り込む（目標値が指定されていなければ）
if not self.ref:
self.ref = {'x': s.x, 'gamma': s.gamma, 'l': s.l}
m = self.p('payload_mass')
# x_ref, gamma_ref, l_ref はパルス/ランプ/デモの処理で決まる（省略）
u_yaw = self.p('kp_yaw') * (gamma_ref - s.gamma) - self.p('kd_yaw') * s.gammad
u_x = self.p('kp_x') * (x_ref - s.x) - self.p('kd_x') * s.xd
# hoist_joint は +d = 下向き。重力は +d 側に働くので保持力は -m*g
u_l = -m * G + self.p('kp_l') * (l_ref - s.l) - self.p('kd_l') * s.ld
cmd = Float64MultiArray()
cmd.data = [_clamp(u_yaw, self.p('sat_yaw')),
_clamp(u_x, self.p('sat_x')),
_clamp(u_l, self.p('sat_l'))]
self.pub.publish(cmd)
目標値を渡さなかった場合は初回に観測した姿勢をそのまま目標にするようにしています。
最初これを入れておらず、起動直後に「現在位置 10 m、目標 20 m」という 10 m のステップ指令が入り、
飽和した力でトロリが吹っ飛んで ODE が発散しました。地味ですが効きます。
保持が効いていることは、巻上げの指令値で確認できます。
u_l ≈ -9800 N （吊荷 1000 kg × 9.80665 m/s² = 9807 N）
x = 10.0000 m, γ = 0.0000 rad, l = 15.0000 m のまま静止
7.1 振れの励起はトロリの往復パルスで行う
初期振れ角を与える方法として、gazebo_ros2_control の initial_value や
Gazebo の /apply_joint_effort サービスを試しましたが、いずれも 5.1 節の問題（非作動関節に
ros2_control が触れない構成にした）と噛み合いませんでした。
最終的に、トロリの目標値を半周期だけずらして戻す方法にしました。物理的にも自然で、自前のコードだけで完結します。
支点を A だけ動かして T/2 後に戻すと、振れ振幅はおよそ 2A/l になります。
つまり狙いの振幅 φ0 に対して A = φ0·l/2 とすればよいことになります。
pulse_amp = PythonExpression([phi0, ' * ', l_ref, ' / 2'])
half_period = PythonExpression(['3.14159265 * (', l_ref, ' / 9.80665)**0.5'])
8. 検証：振り子の運動は正しいか
制御を載せる前に、追加した振り子が物理的に正しいことを確かめます。
ここが間違っていると、この先の制御の結果がすべて信用できなくなります。
8.1 実験A1：自由振動の周期
作動3関節を PD で固定し、トロリの往復パルスで振らせて、その後の $\phi(t)$ に「だんだん小さくなるサインカーブ」
\phi(t) = A e^{-\zeta \omega_n t} \cos(\omega_d t + \psi) + c
をフィットして周期を求めます。ケーブル長 $l$ を 5, 10, 15, 20 m と変えて4本走らせます。
for L in 5.0 10.0 15.0 20.0; do
ros2 launch tower_crane_control exp_a1_free_swing.launch.py \
l_ref:=$L phi0:=0.10 duration:=70.0 out:=out/a1_l$L.csv
done
ros2 run tower_crane_control analyze_a1 -- out/a1_*.csv --t-start 12 \
--fig out/a1_waveform.png --fig-summary out/a1_period.png
比較対象は2つです。$T_\text{point}$ は教科書どおりの質点近似、$T_\text{rigid}$ は荷の自転慣性 $I$ を含む剛体振り子です。
T_\text{point} = 2\pi\sqrt{\frac{l}{g}}, \qquad
T_\text{rigid} = 2\pi\sqrt{\frac{m l^2 + I}{m g l}}
結果です。
| $l$ [m] | $T_\text{meas}$ [s] | $T_\text{point}$ [s] | 誤差 [%] | $T_\text{rigid}$ [s] | 誤差 [%] | $\zeta$ | $A$ [rad] |
|---|---|---|---|---|---|---|---|
| 5.00 | 4.550 | 4.487 | +1.41 | 4.522 | +0.60 | 0.00083 | 0.0953 |
| 10.00 | 6.378 | 6.345 | +0.52 | 6.358 | +0.32 | 0.00036 | 0.0980 |
| 15.00 | 7.796 | 7.771 | +0.32 | 7.778 | +0.24 | 0.00021 | 0.0986 |
| 20.00 | 8.995 | 8.973 | +0.25 | 8.978 | +0.20 | 0.00014 | 0.0988 |
質点近似との誤差は最大でも 1.41 %、剛体振り子と比べれば 0.60 % 以下に収まりました。なお、この実測は保持ゲイン $k_p = 3\times10^5$ N/m で走らせた結果です（この値が効いてくる理由は後述します）。
周期 vs ケーブル長と相対誤差
実測 φ(t) と、周期だけ理論値に置き換えた波形の重ね描き
残った誤差の内訳
残った誤差も説明がつきます。効いているのは2つです。
1. 荷の自転慣性
荷は完全な質点ではありません。URDF では慣性を $I = 400$ kg·m² と置いています（$I = 0$ は ODE が嫌がるため、$m l^2 \approx 2.3\times10^5$ に対して十分小さい値として選んだもの）。この $I$ のぶんだけ剛体振り子として振る舞うので、$l$ が短いほど周期が伸びます。$T_\text{rigid}/T_\text{point} - 1$ がその寄与で、$l = 5$ m では +0.80 % 効きます。
2. 支点のばね性
トロリは力制御なので、位置を保っているといっても、実際は PD ゲイン $k_p$ ぶんのばねで支えられています。荷が横に振れると支点を押すので、そのぶん支点がわずかに逃げる。振り子が実質的に少しだけ長くなるということです。
逃げる量は「荷の重さ ÷ ばねの硬さ」です。荷は 1000 kg なので 9807 N、$k_p = 3\times10^5$ N/m で割ると 32.7 mm。周期は長さの平方根で効くので、伸びが 32.7 mm でも周期への影響はその半分の割合になり、$l = 5$ m で +0.33 %、$l = 20$ m で +0.08 % です。短い振り子ほど 32.7 mm の重みが大きくなるので、$l$ が小さいほど効きます。
この2つを足すとこうなります。
| $l$ [m] | 自転慣性 | 支点ばね | 小計 | 実測誤差 | 残差 |
|---|---|---|---|---|---|
| 5.00 | +0.80 % | +0.33 % | +1.13 % | +1.41 % | +0.28 % |
| 10.00 | +0.20 % | +0.16 % | +0.36 % | +0.52 % | +0.16 % |
| 15.00 | +0.09 % | +0.11 % | +0.20 % | +0.32 % | +0.12 % |
| 20.00 | +0.05 % | +0.08 % | +0.13 % | +0.25 % | +0.12 % |
支点ばねのモデルは、ゲインを振ると裏が取れます。保持ゲインを $k_p = 3\times10^4$ に下げると、ばねが10倍柔らかくなるので逃げる量も10倍の 327 mm。同じ計算をすると $l = 15$ m で +1.09 % となり、自転慣性の +0.09 % と合わせて +1.18 % を予測します。実測も +1.18 % でした。$3\times10^5$ に戻すと実測は +0.32 % まで下がり、これも内訳表の $l = 15$ m の行のとおりです。
残差 0.1〜0.3 % は、ros2_control を使わない純 Gazebo の振り子で測った +0.27 % と同程度なので、ODE が有限のステップで積分していることによる癖（周期がわずかに長めに出る）とみています。ただし残差が $l$ とともに小さくなる点は説明しきれていません。トロリ質量 $M$ の慣性や制御周期の遅れが効いている可能性がありますが、切り分けきれていません。
減衰比 $\zeta$ はどれも $10^{-4}$ 台で、ほぼ無減衰です。5.1 節の状態（$\zeta \simeq 0.55$）と比べると別物になったことが分かります。
8.2 実験A2：可変長振り子
$l = 20$ m で振らせたまま、0.5 m/s で 8 m まで巻き上げます。
ros2 launch tower_crane_control exp_a23_swing.launch.py \
l_ref:=20.0 pulse_amp:=1.0 l_ramp_at:=30.0 l_target:=8.0 l_rate:=0.5 \
duration:=110.0 out:=out/a2.csv
ros2 run tower_crane_control analyze_a23 -- a2 out/a2.csv --t-start 12 --fig out/a2.png
ゼロ交差の間隔から瞬時周期を求めて、その時刻の $2\pi\sqrt{l(t)/g}$ と比べます。
| 指標 | 実測 | 理論 |
|---|---|---|
| 周期誤差（平均） | +0.79 % | — |
| 周期誤差（最大） | +2.69 % | — |
| 振幅スケーリング指数 | $\phi \propto l^{-0.738}$ | $l^{-3/4}$（ゆっくり巻き上げた場合） |
φ(t)、l(t)、瞬時周期と理論曲線
周期は理論曲線にきれいに乗ります。最大誤差 2.69 % は巻上げが止まる瞬間のもので、その区間だけ周期が急変しているためです。
振幅は 0.10 rad から 0.19 rad まで増えました。
揺れている振り子の糸をゆっくり引き上げると、速く揺れるようになり、同時に振れ角も大きくなる。 理科の実験で見たことがある方もいると思います。ゆっくり引き上げるかぎり、振れ角の振幅とケーブル長のあいだには
\phi_0 \propto l^{-3/4}
という関係が成り立ちます。実測の指数は $-0.738$ で、この $-0.75$ とよく一致しました。
ここで1つ落とし穴がありました。この指数は巻上げ中の区間だけを使ってフィットした値です。記録には巻上げ前後の「$l$ が一定のまま振幅だけがゆっくり減衰する」区間も入っていて、そこまで含めて回帰すると $-0.685$ になってしまいます。$l$ が動いていない点は「$l$ を変えたら振幅がどう変わるか」の情報を持たないので、除いて測る必要があります。
この節の冒頭で挙げた analyze_a23 は記録した全点で回帰するので、そのまま実行すると $-0.685$ が出ます。$-0.738$ は巻上げ中の点だけを抜き出して測り直した値です。
同じデータでも、フィットに使う区間で傾きが変わる
左が全35点、右が巻上げ中の10点だけを使った場合です。左の灰色の点が「$l$ が動いていない区間」で、$l$ = 8 m と 20 m の2箇所に25点ぶんが重なっています。これを混ぜると傾きが $-0.685$ に引っぱられる。右のように巻上げ中だけで測ると $R^2 = 1.0000$ のきれいなべき乗則になり、理論の $l^{-3/4}$（橙の破線）とほぼ重なります。
「ゆっくり」の目安についても書いておきます。ケーブル長が1秒でどれだけの割合縮むかを、振れ自体の速さと比べた比で見ます。今回は 0.5 m/s なので、長さの縮む割合は毎秒 2.5 %（$l$ = 20 m のとき）から 6.3 %（$l$ = 8 m のとき）。いっぽう振れの速さは 0.70〜1.11 rad/s です。比にすると 0.04〜0.06 で、1 よりずっと小さいので「ゆっくり」の範囲に入ります。逆に巻上げが速すぎるとこの関係は崩れるので、次の記事で巻上げ速度を決めるときの目安になります。
なお $l^{-3/4}$ は振れ角についての指数です。水平変位の振幅 $l\phi_0$ で見ると $l^{+1/4}$、つまり巻き上げると水平の振れ幅はむしろ狭くなります。角度が増えるのに実距離は減る、というのは直感に反するので、荷の実際の暴れ方を語るときは注意が必要です。
8.3 実験A3：旋回励起と符号の検証
3.2 節で決めた符号の約束が正しいかを確認します。$x = 20$ m、$l = 15$ m でジブに 0.05 rad の旋回パルスを入れ、接線方向の振れ $\theta$ を測ります。
そのうえで、実測の $x(t)$ と $\ddot\gamma(t)$ を使って論文の式(10)
l\ddot{\theta} + g\theta - x\ddot{\gamma} = 0
を数値積分し、実測の $\theta(t)$ と重ねます。 $\ddot\gamma$ は $\dot\gamma$ の数値微分（Savitzky-Golay フィルタで平滑化）から求めました。
ros2 launch tower_crane_control exp_a23_swing.launch.py \
l_ref:=15.0 x_hold:=20.0 pulse_axis:=gamma pulse_amp:=0.05 \
duration:=70.0 out:=out/a3.csv
ros2 run tower_crane_control analyze_a23 -- a3 out/a3.csv --t-start 4 --fig out/a3.png
結果です。
| 指標 | 値 |
|---|---|
| 接線振れ $\theta$ 実測振幅 | 0.0504 rad |
| 式(10) モデルの振幅 | 0.0504 rad |
| 実測とモデルの相関 | +0.9994 |
| RMS 誤差 | 0.00047 rad（振幅の 0.9 %） |
上が励振項 x·γ̈、下が実測 θ と式(10) の数値解
相関が +0.9994、つまり実測とモデルが同位相です。ここで符号を逆に取っていた場合、すなわち URDF の swing_tangential_joint の軸を逆向きに定義していた場合は、同じ物理現象に対して $\theta$ の測定値だけが反転するので、相関は $-0.9994$ になります。したがってこの符号は、URDF の関節軸の向きが式(10) の θ の定義と一致していることを示しています。
ただしこれは「式(10) が正しい」ことの証明ではありません。3.2 節で述べたとおり、論文[1]の式(1) と式(10) は $\theta$ の符号定義が食い違っており、この実験が確認しているのは実装が式(10) 側に揃っていることです。式(1) 側の定義を採用するなら、相関は $-0.9994$ になるのが正解ということになります。
振幅も小数点以下4桁まで一致し、RMS 誤差は振幅の 0.9 % です。
この実験にはもう1つ意味があります。式(8), (10) は微小角を仮定した線形化モデルですが、それが 1 % の精度で成立していることが同時に示されたことになります。次の記事で設計する LQR はこの線形モデルの上に載るので、この結果が土台になります。
9. メッシュを package:// で書くと Gazebo の描画が止まる
ここまでの検証はすべてヘッドレス（gui:=false）で行っていました。最後に GUI を立ち上げたところ、gzclient のウィンドウが小さいまま固まりました。
厄介なのは、物理はまったく正常に動いていることです。このモデルは collision を持たず visual だけなので、物理エンジンはメッシュを一切読みません。つまりメッシュ由来の問題は、シミュレーション結果を見ているかぎり絶対に気づけません。8章までの実験がすべて正しく通っていたのは、そのためです。
gzclient は環境要因でも落ちるので、切り分けには GUI を使わない方法を使いました。Gazebo のカメラセンサに JPEG を書き出させ、1枚でも出力されるかだけを見ます。
<model name="photographer">
<static>true</static>
<pose>45 32 24 0 0.12 -2.52</pose>
<link name="link">
<sensor name="shot_cam" type="camera">
<camera>
<horizontal_fov>1.05</horizontal_fov>
<image><width>1280</width><height>720</height><format>R8G8B8</format></image>
<clip><near>0.1</near><far>500</far></clip>
<save enabled="true"><path>frames</path></save>
</camera>
<always_on>1</always_on>
<update_rate>30</update_rate>
</sensor>
</link>
</model>
カメラ設定を揃えたまま、置くモデルだけを差し替えて4通り試しました。条件1〜3 は同じ短い収録時間で、条件4 は修正後の確認なので本番収録（110 秒）をそのまま完走させています。
| # | 置いたモデル | メッシュ URI | 出力枚数 | 判定 |
|---|---|---|---|---|
| 1 | なし（空のワールド） | — | 19 枚 | ✅ |
| 2 | 旧モデル | model:// |
15 枚 | ✅ |
| 3 | 新モデル | package:// |
0 枚 | ❌ |
| 4 | 新モデル |
file://（絶対パス） |
3299 枚（110 s ぶんを完走） | ✅ |
差し替えたのは URI 記法だけで、メッシュファイルそのものは 3 と 4 で同一です。それでも 3 だけが 1 枚も出力しません。package:// が単独の原因です。
package:// は Gazebo Classic 側のレンダリング（gzclient およびカメラセンサ）で解決できず、メッシュ読み込みのところで描画スレッドごと止まってしまうようです。file:// の絶対パスであれば Gazebo でも RViz でも解決できます。
<!-- NG: Gazebo のレンダリングが止まる -->
<mesh filename="package://tower_crane_description/meshes/base.obj"/>
<!-- OK -->
<mesh filename="file://$(find tower_crane_description)/meshes/base.obj"/>
$(find) は xacro が展開するので、生成後の URDF には絶対パスが入ります。修正後は gzclient も正常に描画するようになりました。
なお、この方法で検証するときは DISPLAY に注意してください。gzclient を起動しなくても、カメラセンサのレンダリングには GL コンテキストが必要です。環境変数の DISPLAY をそのまま使うと、GL の効かないディスプレイを掴んで無言で 0 枚になることがあります。これは package:// の症状と見分けがつかないので、まず条件 1（空のワールド）で1枚でも出ることを確かめてから、モデルを足すのが確実です。
10. 動かしてみる
5自由度版のデモを用意しました。無限ループで動き続けます。
cd ~/tower_crane_ws
colcon build --symlink-install
source install/setup.bash
DISPLAY=:1 ros2 launch tower_crane_control demo_gui.launch.py
Ctrl-C で止めるまで、次の行程を繰り返します。
| # | 行程 | 動作 |
|---|---|---|
| 1 | 横行：先端へ | $x$: 10 → 20 m |
| 2 | 巻下げ | $l$: 15 → 24 m |
| 3 | 巻上げ | $l$: 24 → 10 m |
| 4 | 横行：根元へ | $x$: 20 → 6 m |
| 5 | 右旋回 | $\gamma$: 0 → +0.6 rad |
| 6 | 複合動作 | 左旋回しながら横行＋巻下げ |
| 7 | 正面へ復帰 | — |
各行程は速度制限付き（横行 0.8 m/s、旋回 0.08 rad/s、巻上げ 0.8 m/s）で動き、到達後に 6 秒静止します。
この静止時間がポイントで、動きを止めても荷（オレンジの箱）が振れ続けるのが見えます。振れ2自由度は非作動で、減衰比も $10^{-4}$ 台しかないためです。8.1 節で測った $\zeta$ がそのまま目に見える形で出てきます。
速度や荷の重さは引数で変えられます。
| 引数 | 既定値 | 効果 |
|---|---|---|
demo_rate_x |
0.8 | 横行速度 [m/s] |
demo_dwell |
6.0 | 各行程後の静止時間 [s] |
payload_mass |
1000.0 | 吊荷質量 [kg] |
# ゆっくり見せる
DISPLAY=:1 ros2 launch tower_crane_control demo_gui.launch.py \
demo_rate_x:=0.4 demo_dwell:=10.0
# 荷を重くする
DISPLAY=:1 ros2 launch tower_crane_control demo_gui.launch.py payload_mass:=2000.0
なお、起動してから（Gazebo の立ち上げと spawn を含めて）最初の 15 秒ほどはクレーンが動きません。これは仕様です。 6 章で書いたとおり、低速でシミュレーションを開始し、コントローラを有効化してから等速に戻しているためです。ターミナルに demo step ... が出はじめたら本番です。
11. まとめ
論文の再現に向けた土台として、次のところまで進みました。
- 既存の3自由度 SDF モデルを URDF 化し、吊荷の振れ2自由度を追加して論文と同じ5自由度にした
- ROS 2 Humble + gazebo_ros2_control で力・トルク指令を受け付けるようにした
- 追加した振り子の物理が正しいことを3つの実験で確認した
- 自由振動の周期が、荷の自転慣性を入れた理論値と 0.20〜0.60 % で一致（教科書どおりの質点近似と比べても 0.25〜1.41 %）
- 巻上げ中の瞬時周期が
2π√(l(t)/g)に平均 +0.79 % で追従、振幅は φ ∝ l^(-0.738) - 接線振れが論文 式(10) の数値解と相関 +0.9994、振幅も一致
次回は、論文[1]の中身に入っていきたいと思います。式(11)〜(17) の時変LQR（トロリ・旋回）と式(18)の巻上げPID、そして5次スプラインによる軌道生成を実装して、論文[1]の Fig.5/6 の再現を目指したいと思っています。
今回のデモ動画は「制御なしだとこうなる」というベースラインとしてそのまま使えるので、
同じ動作で before / after を並べたいなと考えています。
参考文献
- M. Muddassir, M. A. A. Abdelkareem, T. Zayed, Z. Lafhaj,
"Automating the Tower Crane: Integrating the Development and Simulation of
Path Planning and Trajectory Tracking of Tower Crane in ROS Framework",
Proceedings of the 41st International Symposium on Automation and Robotics
in Construction (ISARC 2024), pp. 1065–1072, 2024.
（この記事で「論文」と呼んでいるのはこれです。
デモ動画: https://github.com/muddassir93/ISARC2024_Demo/raw/main/Demo.mp4 ）
ライセンス / クレジット
本記事および関連リポジトリで使用している3Dモデル tower_crane は、
Gazebo モデルデータベース（osrf/gazebo_models）に含まれるもので、
Creative Commons Attribution 3.0 Unported (CC BY 3.0) で提供されています。
| 項目 | 内容 |
|---|---|
| 原著作物 | tower_crane |
| 著作権表示 | Copyright 2012 Nathan Koenig（リポジトリ LICENSE より） |
| 原著作者 | Nate Koenig（model.config の author 表記） |
| 収録先 | Gazebo モデルデータベース osrf/gazebo_models（運営: Open Robotics） |
| 出典 | https://github.com/osrf/gazebo_models/tree/master/tower_crane |
| ライセンス | CC BY 3.0 |
| 改変 |
あり。base / upper / トロリ / フックへの分割、旋回・横行・巻上げ関節の追加、.obj への変換、および本記事での振れ2関節の追加と URDF 化 |
本記事中のコード（URDF・launch・制御ノード・解析スクリプト）は私が書いたもので、
モデルデータとはライセンスが別です。CC BY 3.0 はシェアアライク条項を持たないため、
二次的著作物に元と同じライセンスを適用する義務はありません。
私が書いたコードは MIT ライセンスとします。