1. はじめに
前々回でタワークレーンを5自由度（旋回・横行・巻上げ + 吊荷の振れ2自由度）にして、
前回その上に時変LQR を載せ、吊荷の振れを止められるところまで進めました。
👇前々回の記事：5自由度化とプラント検証:
👇前回の記事：時変LQR による振れ止め:
ただ、ここまでだと荷を運べても降ろせません。フックに剛結されたままなので、
クレーンとしての仕事が途中で終わっています。
そこで今回は次の2つをやりました
- Gazebo Classic に吊荷の着脱を実装する
- 「搬送 → 振れが収まるまで待つ → 降ろす → 切り離す」という一連の工程を組み、
振れ止めができると何が嬉しいのかを測る
結論から書くと、同じ工程を時変LQR と PID で回したときの所要時間はこうなりました。
- 時変LQR 振れが 8.4 秒で収束 → t=49.6 切り離し → t=65.8 完了
- PID 振れが収まらず 40 秒でタイムアウト → t=81.4 切り離し（振れたまま設置）
制御の良し悪しを「振れ角 [rad]」ではなく作業時間の差で語れるようになりました。
以下が結果の動画です。
👇動画：左が時変LQR、右が PID
以下、コードは全文ではなく要点だけを抜粋して載せます。
2. Gazebo Classic には着脱できる仕組みが無い？
まずGazebo Classic には「実行時に着脱できる関節」が標準では用意されていません。
apt で探すと ros-humble-gazebo-model-attachment-plugin が見つかりますが（他のロボット関連記事で使ったこともあります）、今回やりたいのは「1箇所を切り離す」だけなので、自分で書いたほうが短く済むかと思いまして、作成してみました。
方針はこうです。
URDF の中に fixed joint で荷をぶら下げておく
↓
ROS サービスが呼ばれたら、その joint を Detach() する
↓
荷は自由物体になり、重力で落ちて地面に載る
プラグインの中身は、サービスを1つ立ててそこで Detach() を呼ぶだけです。
class PayloadRelease : public gazebo::ModelPlugin
{
public:
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override
{
model_ = model;
joint_name_ = sdf->Get<std::string>("joint", "payload_joint").first;
const auto service = sdf->Get<std::string>("service", "release_payload").first;
ros_node_ = gazebo_ros::Node::Get(sdf);
release_srv_ = ros_node_->create_service<std_srvs::srv::Trigger>(
service,
[this](const std_srvs::srv::Trigger::Request::SharedPtr,
std_srvs::srv::Trigger::Response::SharedPtr res) {
auto joint = model_->GetJoint(joint_name_);
if (!joint || released_) {
res->success = false;
res->message = released_ ? "already released" : "joint not found";
return;
}
joint->Detach();
released_ = true;
RCLCPP_INFO(ros_node_->get_logger(), "payload released");
res->success = true;
res->message = "released";
});
}
...
};
GZ_REGISTER_MODEL_PLUGIN(PayloadRelease)
URDF 側にはこう書きます。
<gazebo>
<plugin name="payload_release" filename="libpayload_release.so">
<joint>payload_joint</joint>
<service>release_payload</service>
</plugin>
</gazebo>
これで、任意のタイミングで切り離せます。
ros2 service call /release_payload std_srvs/srv/Trigger
ビルドは ament_cmake に gazebo_dev を足すだけです。
find_package(gazebo_dev REQUIRED)
find_package(gazebo_ros REQUIRED)
find_package(std_srvs REQUIRED)
add_library(payload_release SHARED src/payload_release.cpp)
target_include_directories(payload_release PUBLIC ${GAZEBO_INCLUDE_DIRS})
target_link_libraries(payload_release ${GAZEBO_LIBRARIES})
ament_target_dependencies(payload_release gazebo_ros rclcpp std_srvs)
install(TARGETS payload_release DESTINATION lib)
3. どう構築していったか その１（最初の設計では動かなかった・・・・）
上の形に落ち着くまでに、一度まったく違う設計を試して失敗しています。
最初は「荷を別モデルにして、起動時にプラグインが実行時 joint を作って結合する」という
方式にしました。荷を独立したモデルにしておけば、切り離したあとの扱いが自然になると
考えたからです。Gazebo Classic では物理エンジンに直接 joint を作らせられます。
joint_ = world_->Physics()->CreateJoint("fixed", model_);
joint_->Attach(parent, child); // hook_link ← payload::link
joint_->Load(parent, child, ignition::math::Pose3d());
joint_->SetModel(model_);
joint_->Init();
これは結合自体は成立します。ログにも正しく出ます。
payload attached: parent=tower_crane::hook_link child=payload::link
巻上げ力を見ても荷の重量ぶんを保持しているので、ぶら下がってはいます。
ところが動かしてみると、振り子がまったく振れませんでした。またトロリは止まり、2600 N を出し続けているのに 1600 kg のトロリが動かない、という奇妙な状態となりました。
3.1 荷の実座標を測って切り分ける
最初は「荷が世界座標に固定されているのでは」と疑いました。
トロリが 20 m から 16.9 m へ 3.1 m 動いたとき、荷がその場に留まっていれば
ケーブルの傾きは atan(3.1/20) = 0.153 rad になります。実測の 0.149 rad とほぼ一致します。
ただ、これは推測でしたので、Gazebo は模型の位置を取得できるので、測ってみました。
gz model -m payload -p
0.059993 -19.9867 7.69254 -0.158844 ...
0.059989 -19.9780 7.71497 -0.152382 ...
0.059985 -19.9702 7.73638 -0.151103 ...
荷の y は -19.95 付近です。ここでトロリは y = -16.9 にいます。
一見「荷が置いていかれている」ように見えますが、計算すると違いました。
支点 (トロリ) y = -16.9, z = 27.9
フック原点 = 支点 + R(φ)·(0,0,-16.6) → y = -16.9 + 16.6·sin(-0.149) = -19.36
荷の中心 = フック + R(φ)·(0,0,-3.63) → y = -19.36 + 3.63·sin(-0.149) = -19.90
実測 -19.95 とほぼ一致します。つまり荷はフックに正しく追従していました。
推測は外れていたわけです。
では何が起きていたのか。φ ≠ 0 で静止するには、重力の復元トルク
m·g·l·sin(φ) を打ち消す何かが必要です。それが見当たらない。
モデルをまたぐ拘束を ODE が解ききれず、実質的に固まっていたと考えています。
3.2 同一モデル内の joint を切る方式へ
そこで方針を変えました。
変更前 クレーン(モデルA) ←実行時に生成した joint→ 荷(モデルB)
変更後 クレーン(モデルA) ←URDF 内の fixed joint→ 荷リンク ← これを Detach する
同一モデル内なら関節ツリーは読み込み時に構築されるので、
搬送中の動力学は検証済みの一体モデルとまったく同じになります。
実際、この変更だけで振れが正常に戻り、整定も 1.8 秒で決まりました。
つまり実行時に構造を変えるより、最初から構造に入れておいて壊すほうが素直でした。
4. どう構築していったか その2（URDF の fixed joint は SDF 変換で消える）
方式を変えたことで、新しい課題が出てきました。
URDF に fixed joint を書いても、Gazebo に渡る SDF では親リンクに畳み込まれて消えます。
これは URDF → SDF 変換の標準的な最適化で、普段は害がありません。
しかし今回は、その joint こそが Detach() の対象なので、消えると何もできなくなります。
抑止するタグがあります。
<gazebo reference="payload_joint">
<disableFixedJointLumping>true</disableFixedJointLumping>
<preserveFixedJoint>true</preserveFixedJoint>
</gazebo>
変換結果は必ず確認したほうがいいですね。
xacro tower_crane.urdf.xacro detachable_payload:=true > /tmp/d.urdf
gz sdf -p /tmp/d.urdf | grep payload_joint
payload_joint : あり
payload_link : あり（mass 900、collision あり）
hook_link : mass 100
5. どう構築していったか その3（分離してもプラントを壊さない）
ここまでの記事で、振り子の周期や制御の効きを実測で検証してきました。
荷をリンクとして切り出すとその前提が変わるので、影響を先に見積もりました。
やったのは質量配分の設計です。フックと荷を分けても、合成重心を一致させます。
一体のとき 1000 kg が z = -3.400
分離したとき フック 100 kg が z = -1.4
荷 900 kg が z = -3.63
→ 合成重心 z = -3.407
差は 7 mm です。ケーブル長 20 m に対して 0.04 % なので、振り子としては同じものです。
支点まわりの慣性も計算すると 400,000 に対して 400,730 で、ほぼ変わりません。
そのうえで、切り離し機能はオプションにしました。
detachable_payload:=false （既定） フックと荷は一体。これまでの実験はこちら
detachable_payload:=true 荷リンクが生え、切り離せる
こうしておけば、前回までの検証結果はそのまま再現できます。
新機能を足すたびに過去の実験が壊れると、何を信じてよいか分からなくなります。
6. どう構築していったか その4（切り離しは本当に成立しているか）
「見た目に落ちた」だけでは不十分なので、力で確認します。
巻上げの指令値には吊っている質量がそのまま出ます。
切り離し前 u_l = -9868 N フック 100 kg + 荷 900 kg = 1000 kg ぶん
切り離し後 u_l = -980 N フック 100 kg だけ（100 × 9.8 = 980 N）
荷の重量ぶんがちょうど消えています。物理的に外れていると言えます。
7. どう構築していったか その5（ミッションを組む）
工程を自動で進めるノードを書きました。制御そのものは前回のコントローラに任せ、
シーケンサは目標値を切り替えるだけにしています。
1. 搬送 x を目的地へ（5次スプライン 12 秒）
2. 整定 振れが収まるまで待つ（上限 40 秒）
3. 巻下げ 荷の下端が地面に触れる高さまで l を伸ばす
4. 切り離し /release_payload を呼ぶ
5. 退避 フックだけ巻き上げて旋回
接地する高さは幾何から出せます。荷の中心は z = 27.67 - l、
箱（1.5 m 立方）の下端はさらに 0.75 m 下なので、
l = 26.4 m → 下端 0.52 m 少し浮く
l = 26.9 m → 下端 0.02 m ほぼ接地
l = 26.9 m まで伸ばせるよう、巻上げ関節のリミットを広げました。
工程2の「振れが収まるまで待つ」が今回の肝です。ここに判断が入るので、
制御の良し悪しが工程の進み方に直結します。
8. どう構築していったか その6（整定判定を角度だけで書いてはいけない）
その整定判定で、またハマりました。最初はこう書いていました。
quiet = abs(phi) < tol and abs(theta) < tol
これで走らせると、PID でも 8.8 秒で「収束」と判定されます。
前回の記事で PID は永久に振れ続けると測ったばかりなので、明らかにおかしい。
原因は単純で、振り子は1周期に2回ゼロを通過するからです。
そしてゼロを通過する瞬間は角速度が最大、つまり大きく振れている時です。
角度だけを見ていると、その瞬間を「止まった」と誤判定します。
角速度を条件に足すと直ります。
quiet = (abs(phi) < tol and abs(theta) < tol
and abs(phid) < rate and abs(thetad) < rate)
判定時の値を並べると違いが分かります。
修正前 PID |φ| = 0.0048 rad で「収束」 ← 振幅は 0.1 rad のまま
修正後 PID |φ| = 0.0407 rad, |φ̇| = 0.0661 rad/s で「タイムアウト」
LQR |φ| = 0.0043 rad, |φ̇| = 0.0040 rad/s で「収束」
制御の教科書では当たり前の話ですが、私はそこの基礎知識がないので、見事にハマりました。
9. 結果
同じミッションを時変LQR と PID で回した結果です。もう一度動画を示します。
👇動画：左が時変LQR、右が PID
時変LQR
--- carry (t= 8.2) 搬送 x 20 → 10 m
--- settle (t=21.2) 整定待ち
振れ |φ|=0.0043 rad, |φ̇|=0.0040 rad/s で 収束（8.4 s）
--- lower (t=29.6) 巻下げ
--- release (t=49.6) 切り離し
--- retreat (t=49.8) フックを巻き上げて退避
--- done (t=65.8)
PID
--- carry (t= 8.2)
--- settle (t=21.2)
振れ |φ|=0.0407 rad, |φ̇|=0.0661 rad/s で タイムアウト（40.0 s）
--- lower (t=61.2)
--- release (t=81.4) 振れたまま設置することになる
差は 32 秒です。振れが収まらないと荷を置けないので、待たされたぶんがそのまま
工程の遅れになります。しかも PID 側は最後まで収まらないので、
実際には「振れたまま降ろす」か「いつまでも待つ」かの二択になります。
前回は振れ角そのもので比較しましたが、こうして工程に落とすと意味が伝わりやすくなります。
振れ止めは「グラフがきれいになる」話ではなく「作業が早く終わる」話だ、ということです。
9.1 動かし方
cd ~/tower_crane_ws
source install/setup.bash
# 時変LQR（既定）— 振れが収まってから荷を降ろす
ros2 launch tower_crane_control mission_gui.launch.py
# PID — 振れが収まらずタイムアウトしてから降ろす
ros2 launch tower_crane_control mission_gui.launch.py mode:=pid
工程はターミナルに出るので、どこで待たされているか分かります。
主な引数です。
mode lqr_tv | pid 制御則
x_goal 10.0 搬送先のトロリ半径 [m]
carry_duration 12.0 搬送の所要時間 [s]。短くすると振れが大きくなる
duration 150.0 実行の長さ [sim s]
シーケンサを使わず、任意のタイミングで切り離すこともできます。
ros2 service call /release_payload std_srvs/srv/Trigger
10. まとめ
Gazebo Classic にタワークレーンの吊荷の着脱を実装し、搬送から設置までの一連の工程を組みました。
- 実行時に着脱できる関節は標準に無いので、URDF 内の fixed joint を Detach する
モデルプラグインを書いた（110 行） - 荷を別モデルにして実行時に joint を作る方式は、結合はできても
モデルをまたぐ拘束を ODE が解ききれず振り子が止まった - URDF の fixed joint は SDF 変換で畳み込まれて消えるので、抑止タグが要る
- 分離しても合成重心を一致させれば、検証済みのプラントはそのまま使える（差 0.04 %）
- 整定判定は角度だけでなく角速度も見る。振り子はゼロ通過時に角速度が最大になる
- 振れ止めの効果は、振れ角ではなく作業時間の差 32 秒として出た
一連の3記事で、静止モデルだったタワークレーンが、荷を運んで振れを止めて設置する
ところまで動くようになりました。残っているのは論文が扱っていた MoveIt による
衝突回避経路計画と、LiDAR による占有格子です。経由点さえ与えれば軌道生成側は
今のまま使えるので、次回はこれらのことに取り組みたいと思います。
ライセンス / クレジット
本記事で使用している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 ライセンスとします。