経路計画と制御アルゴリズム
確認済みの地形から経路を検査し、推進指令へ変換します。Gazeboで計算された実際の動きが、次の計画と制御に戻されます。
個人の水中ウェイポイント計画にはミッションの深度制限が適用されます。シミュレーションの上限が拡張されても、以下の地形と機体の通過範囲の検査は維持されます。
処理の流れ
- 1. 目標・計測状態・地形
地理座標の目標をローカル座標へ変換します。確認済みの数値地形タイルを共通グリッドに統合し、未取得部分は未知のまま扱います。
- 2. 地理・沿岸の誘導
陸地を避ける接続と港への進入経路を求めます。地図上で陸を避けるだけでは水深を確認できません。
- 3. 船体の通過範囲を検査
船体に追従余裕を加えます。各検査正方形で海底三角形を切り取り、最も高い底面を求め、陸地、余裕不足、欠測を除外します。最大値フィルターで明らかに深い場所の検査を高速化します。
- 4. 直線、次にA*迂回
まず検査済み直線を試します。通れなければ8近傍格子で、距離コストとユークリッド距離ヒューリスティックによるA*探索を行います。角抜け、半径、距離、時間の制限を守ります。
- 5. 短縮して再検査
近道にも同じ通過範囲検査を適用し、保護された港湾進入点を残します。確認済みウェイポイントと最小余裕を返し、経路がなければエラーにします。
- 6. 速度方針から推進へ
航海の先行艇は曲がり角と針路誤差に応じて減速し、加速ランプ、PI速度制御、差動推力を使います。追従艇は先行艇の実測航跡上を先読みし、傾斜と接近速度で指令を制限します。
- 7. 実測フィードバック
Gazeboが力を適用して動きを返します。進捗、安定性、地形・センサー範囲を監視し、未確認範囲に入る前に待機します。目標や条件の変化に応じて再計画します。
停滞からの復旧が長引くと、LIVE は検査済みの外洋経路を使用できます。移動が再開すると、地図上の海岸へ戻り、海岸沿いに進む経路を定期的に探します。各航行区間には引き続き陸地と数値水深の検査が必要です。固定された港湾進入点は必須で、検査済みの経路で接続します。
LIVE航海では、地図上の沿岸接続を一時的な誘導に使います。実測位置が接続の最後の基準点よりも航路目標に50 m以上近づいた場合、その誘導を解除して先の経路を再計画します。新しい推進通路も、地図上の陸地除外と数値水深の検査を通過する必要があります。
地域復旧を繰り返しても実測の前進が10分間得られない場合、LIVEは16 kmの地形範囲で航路方向へつながる経路を探索します。選択した曲がり角を順にたどり、推進する各区間を現在のネイティブ地形と地図上の陸地に照らして検査します。
計画方式の一覧
| 層 | 方法と適用範囲 |
|---|---|
| 地理経路 | 港の目標、進入ゲート、通過地点、陸地ポリゴンから長距離の誘導を作ります。港カタログの座標と航行可能な到着目標は別です。 |
| 地形場 | 確認済み数値グリッドを共通座標へ統合します。厳密な三角形切取りで最も高い底を求め、SciPy最大値フィルターの上界を使います。未知・範囲外は利用不可のままです。衛星画像の色を水深には使いません。 |
| 通過余裕 | 区間を最大1 m間隔で調べ、検査範囲に間隔の半分を追加します。深度が変わる区間は深い側の端点を使用します。保守的な地形包絡検査であり、将来の6自由度運動全体を予測するものではありません。 |
| 局所探索 | 直線から8近傍A*へ進み、角を保守的に検査します。通常の既定値は格子4 m、半径600 m、経路長2,000 m、計画時間25 sです。呼出し側で変更できます。 |
| 機体の余裕 | 水上:船体半径3.5 m、初期追従余裕4 m、海底余裕1.5 m。水中:それぞれ0.5 m、2 m、1 m。個人WAM-Vは追従余裕を2 mへ狭めても、深さの余裕は維持します。 |
| 結果と監視 | 確認済み点、選択余裕、グリッド・時間枠情報、表現地形上の最小余裕を返します。経路帯・範囲監視で停止する場合があります。計画失敗、追従逸脱、データ不足は別の状態です。 |
余裕は表現された地形に対する値で、水路測量精度の証明ではありません。経路が存在しても、選択した環境で制御が成功する保証にはなりません。AISマーカー、映像中の船、LiDARトピックだけでは、動的障害物回避やCOLREG準拠は成立しません。実験で選ぶコントローラーが担う機能です。
LIVEの読込み済み地形による沿岸復帰
読込み済み数値地形と地図上の陸地除外を使って探索します。通常は岸から1.5 kmの帯が探索自体を制限します。確認済みの続行経路がない場合、有限の迂回を沖合2 kmまで広げ、終点で通常帯に戻れます。
- 終点の前進量:最低500 m。目標まで5 km以内では残距離の10 %。
- 浅水域の除外距離:200 m。
- 表現地形の最低水深:2 m。
- 経路帯の最大長:12 km。
ネイティブ実行は区切った各部分を再検査します。新しい復帰終点は、直近に抜け出した領域を避けます。全体を検査した続行経路はその領域を横切って先へ進めます。前進量50 m未満の経路では、全区間で領域を避けます。この履歴制約は船体の物理的な安全検査を変えません。
制御経路とフィードバック
| 経路 | 入力・実装・出力 | 解釈 |
|---|---|---|
| 個人ArduPilot | 誘導目標・手動入力 → MAVROSとGazebo JSONインターフェース経由のArduRover/ArduSub SITL → アクチュエーターチャンネル。 | 航法源、EKF、モード、アーム、出力割当てが影響します。GNSSと航法を参照。 |
| ROS 2直接制御 | ユーザーノード → 許可トピックと選択した出力割当て → Gazeboの力。 | 制御則はユーザーが用意します。目標や速度の発行は、実際に達成した動きの測定ではありません。 |
| 航海の先行艇 | 確認済み点 → 曲がり角での制動、針路誤差による減速、加速ランプ、積分制限付きPI、差動推力。 | 機体運動の実測値を使用。公称周期0.05 s。ハートビート喪失で指令を抑止します。 |
| 航海の追従艇 | 先行艇の実測航跡、距離、接近率 → 先読み、速度・旋回制限、制動 → 推進。 | 船体原点間の公称間隔38 m。デモ用制御で、船団の衝突回避証明ではありません。 |
| 復帰 | 明示的な方針により、質量・接触・抵抗を保持した制限付き物理曳航を要求できます。 | 通常推進とは別に記録します。先行艇の曳航は最大900 N、要求速度0.8 m/sです。 |
GNSSが設定されたセッションのミッションは、アーミングと航法の安定化後に測定位置から経路を再検査して最初の目的地を送ります。安定化には新しいFCUローカル姿勢・位置と、有効なGNSS融合または受け入れ済みの外部センサー融合が必要です。受信機の生のGPS fixは別の状態として扱います。GNSSと航法を参照してください。
個人用 WAM-V ミッションは、既定で入力整形(GUID_OPTIONS=0)を使用します。方位を明示しない場合、到着半径 0.75 m の外では元の位置目標をシミュレーション時間で 2 秒ごとに更新し、半径内では速度とヨー角速度を 0 にするよう要求します。ビット 64 が有効な保存済みプロファイルでは、ArduRover の S 字曲線ウェイポイントプランナーを維持します。BlueROV2 は ArduSub のウェイポイントコントローラーを使用し、位置目標をシミュレーション時間で 2 秒ごとに更新します。テレメトリの鮮度は実時間で判定し、移動中も航法と地形クリアランスの確認を続けます。
動力船が速度とヨーレートがともにゼロの有効な Guided 目標を受け取ると、同梱の ArduRover は速度 PID を作動させ続け、前後方向の漂流を抑えます。この制御は速度を調整し、指定位置への補正は到着範囲の外で始まります。帆船、有効期限を過ぎた目標、その他の制御モードは、それぞれの停止動作を維持します。
加速度制限後の目標速度がゼロになると、速度制御器は最初に停止を計測した時点で積分値をリセットします。設定された停止速度帯の外では、現在の漂流方向に船を加速させる蓄積積分値もリセットします。漂流に逆らう制動積分値と停止速度帯内の補正は維持します。
個人用の標準プロファイルはWP_SPEED=0.5 m/s、ATC_STOP_SPEED=0.02 m/s、ATC_STR_RAT_I=0を使用します。
SITL プラグインは計算済みの物理状態間でセンサー値を補間します。目標周波数は Rover が 200 Hz、Sub が 1000 Hz です。物理計算の間隔を変更すると、中間サンプル数が変わります。各区間の最後は元の物理状態とタイムスタンプに一致します。物理的な分解能は物理計算の時間刻みによって決まります。
公開WAM-Vデモは入力整形方式(GUID_OPTIONS=0)を選び、シミュレーション時間で2秒ごとに元のウェイポイントを更新します。接近と波の中での定点保持にWP_RADIUS=0.2 m、LOIT_RADIUS=0.1 m、LOIT_SPEED_GAIN=1.0を使います。ミッションの到着判定は引き続き水平誤差0.75 m以内です。個人ワークスペースは選択した制御器パラメーターを維持します。
チェックポイント復元と原点移動も状態・座標を変更します。原点移動は地形、経路、航跡を変換しても物理速度を保ちます。その変化を微分して実移動と解釈してはいけません。追従誤差や消費エネルギーの評価では、復帰、リセット、原点移動のイベントを残してください。
ネイティブの先行・追従制御はLIVE用です。個人セッションは選択したArduPilot/ROS 2経路を使います。個人WAM-Vは船体半径3.5 mに追従余裕4 mを加えて試し、出発点や通路が塞がる場合は追従余裕2 mで再計画します。海底余裕1.5 mは維持し、選択した経路帯を外れるとミッションを停止します。
実装と関連文書
POSIM-Routingが航法、計画、沿岸・港湾接続を実装します。POSIM-ControlのPilot.ccとFollower.cc、Pythonの速度・安定性方針が制御を担当します。WWW-POSIMはセッション、制御方式選択、個人ミッションの条件を管理します。アプリが選択したリビジョンを使用してください。