A Collision-Free Sway Damping Model Predictive Controller for Safe and Reactive Forestry Crane Navigation

Forestry cranes operate in dynamic, unstructured outdoor environments where simultaneous collision avoidance and payload sway control are critical for safe navigation. Existing approaches address these challenges separately, either focusing on sway damping with predefined collision-free paths or performing collision avoidance only at the global planning level. We present the first collision-free, sway-damping model predictive controller (MPC) for a forestry crane that unifies both objectives in a single control framework. Our approach integrates LiDAR-based environment mapping directly into the MPC using online Euclidean distance fields (EDF), enabling real-time environmental adaptation. The controller simultaneously enforces collision constraints while damping payload sway, allowing it to (i) replan upon quasi-static environmental changes, (ii) maintain collision-free operation under disturbances, and (iii) provide safe stopping when no bypass exists. Experimental validation on a real forestry crane demonstrates effective sway damping and successful obstacle avoidance. A video can be found at https://youtu.be/tEXDoeLLTxA.

💡 Research Summary

The paper presents a novel Model Predictive Control (MPC) framework that simultaneously guarantees collision avoidance and payload sway damping for a forestry crane operating in dynamic, unstructured outdoor environments. Unlike prior work that treats these two challenges separately—either assuming pre‑computed collision‑free paths for sway control or handling collision avoidance only at a global planning level—the authors embed both objectives directly into a single optimization problem.

The system architecture consists of three modules. The perception module processes LiDAR point clouds, filters out points belonging to the crane itself, builds an OctoMap occupancy grid, and incrementally updates a voxelized Euclidean Distance Field (EDF) using the FIESTA‑EDF algorithm. This EDF provides a continuous signed‑distance measure to the nearest obstacle for any point in space. The global planner, based on the PFRC‑VP‑STO planner from previous work, generates a reference trajectory for the actuated joints (a cubic spline) while respecting hydraulic pump flow constraints, but it does not model the passive pendulum dynamics. The local planner is the core contribution: a collision‑free, sway‑damping MPC that receives the reference trajectory, the EDF map, and the current joint states.

The crane model comprises seven degrees of freedom: five actuated joints and two passive pendulum joints. The pendulum dynamics are expressed by a full nonlinear equation involving the mass matrix, Coriolis terms, and gravity. For the hydraulic actuators, a second‑order velocity model is adopted (instead of the commonly used first‑order model) to reduce oscillations in joint velocities. A pump flow rate constraint Q≤Q_max is also enforced.

In the MPC formulation, the decision variables include the state trajectory, control inputs, and a time‑progress variable τ together with its rate ˙τ. Introducing τ allows the controller to gracefully stop when no feasible collision‑free maneuver exists. The cost function combines five terms: tracking error of the actuated joints to the global reference, a damping term penalizing pendulum angular velocity, velocity and acceleration regularization for the actuated joints, and a progress term encouraging τ to advance at unit speed. Collision avoidance is enforced by approximating each relevant link (boom, arm, gripper) with a set of spheres; for each link the signed distance is defined as the minimum EDF value minus the sphere radius, yielding three inequality constraints. Violations are penalized with a quadratic barrier function parameterized by a safety margin ε and weight µ=10ε, ensuring a constant per‑step penalty.

The MPC runs at 10 Hz on a separate compute node, with a fixed 70 ms solver time budget, guaranteeing deterministic execution. A prediction horizon of N=40 (≈4 s) was selected as a trade‑off between computational load and the ability to plan around obstacles; horizons between 30 and 80 steps were tested without stability issues. Experiments on a real forestry crane equipped with a Livox LiDAR demonstrated three key capabilities: (1) effective sway damping under external rope pulls, both in free space and near obstacles; (2) reactive replanning when unforeseen static obstacles appear, thanks to the online EDF update; and (3) inherent compensation for tracking errors, ensuring collision avoidance even when the global planner’s path is imperfect. A safety margin of at least ε=0.2 m was required for robust operation; smaller margins caused overly aggressive control and instability near obstacles.

Overall, the work delivers the first experimentally validated collision‑free sway‑damping MPC for a large‑scale underactuated hydraulic manipulator. By integrating perception, global planning, and a dynamics‑aware local MPC, the approach achieves real‑time adaptation to environmental changes while maintaining payload stability, marking a significant step toward fully autonomous forestry operations. Future directions include higher‑frequency control loops, multi‑crane coordination, and learning‑based cost tuning to further enhance performance and robustness.