I Introduction
Automation of heavy machinery, especially hydraulic excavators for its versatility, has been a steady research topic over a few decades. Various types of tasks, for example, soil digging [1, 2], rock moving [3, 4], and truck loading [5], are studied for excavator automation. To perform one of the most fundamental operation among such tasks, soil excavation, trajectory planning complying to operational constraints (e.g. swept volume constraint during digging or bucket tip pose constraint during grading) and physical constraints (e.g. actuator force/torque limit, pump flow rate limit, or power limit) is essential. To achieve such objective of planning a constrained reference trajectory, trajectory optimization has been widely applied [6, 7, 8, 9]. However, since most existing works via trajectory optimization optimize the entire trajectory at once offline, they are vulnerable to instantaneous disturbance during operation. Furthermore, external disturbances to hydraulic excavators such as soilbucket interaction and hydraulic friction are usually intractable and unignorable; accordingly, such unmodelable dynamics could result in suboptimality and even constraint violation. Therefore, a new trajectory planning approach to consider constraints and compensate realtime disturbance is required.
Ia Related work
We classify studies on excavator trajectory planning into three classes: 1) trajectoryoptimizationbased approach, 2) receding horizon optimizationbased approach, and 3) nonoptimizationbased approach.
Given a specified cost function, constraints, and system dynamics, trajectory optimization generates an optimized trajectory in the defined state and input space which satisfies a set of constraints and system dynamics. Thanks to this property of explicit satisfaction of constraints, [6, 7, 9] successfully reflect the system dynamics with force/torque input, operational constraints, and physical constraints while minimizing overall effort or operation time. However, considering that optimization over the entire trajectory entails burdensome computation in general, the trajectories are usually parameterized using relatively simple kernel functions [7, 8]. Additionally, although soil model is employed to consider soilbucket interaction forces [7, 9], the actual soilbucket interaction may not be suitably addressed. There also exist studies regarding trajectory optimization of a new platform, e.g. a walking excavator [10, 11]
. Nevertheless, unlike other studies for conventional hydraulic excavator, they focus on planning and control of locomotion using newly added degrees of freedom (DoF) which is out of our scope.
As a variation of trajectory optimization, receding horizon trajectory optimization or model predictive control (MPC), which optimizes trajectory in a receding horizon manner, is introduced [12, 13]. [12] applies offsetfree MPC to track the reference trajectory even in the presence of soil interaction disturbance while satisfying input box constraints. However, only decoupled dynamics, each with single input and no state constraints, are considered in MPC. In [13], MPC is implemented for trajectory planning of an excavator, but system dynamics is modeled only with a double integrator, and the computation time seems to be insufficient for realtime application ( 3.3 Hz).
Different from trajectory optimization, [14] computes a kinematically feasible reachability map of the bucket pose and linear movements offline. In [1], the authors apply dynamic motion primitives (DMP) to plan a trajectory which emulates experts’ operation while being robust to various soil conditions. However, since these approaches cannot explicitly consider constraints, violation of constraints, especially physical constraints, can occur. To handle such physical constraints, studies with a walking excavator [15, 16] present a hierarchical optimizationbased controller. Nevertheless, feasibility cannot be always guaranteed due to the imposed hierarchy among constraints, and nonlinear constraints such as power constraint may not be directly considered in the optimization. In [15], additional force controller tracking a force reference instead of a position reference is implemented in simulation to adjust to unknown soil conditions.
IB Algorithm overview
To address both operational constraints and physical constraints in the presence of external disturbance during excavation, we present an integrated planner consisting of an online global planner and a realtime local planner as in Fig. 2. Considering that the global planning does not require any measurement feedback, the integrated planner can be executed in realtime if the global planner is computed within the trajectory run time.
In the global planning phase, a smallsized nonlinear optimization problem with a reduced number of constraints is solved. The obtained global trajectory is tracked by the modelpredictivecontrolbased local planner, which plans highfidelity local trajectories. The local planner runs alongside with a disturbance estimator, and takes the estimation results into consideration. To the best of our knowledge, this work is the first realtime trajectory planning framework for autonomous excavation with guarantee of dynamic feasibility, physical constraint satisfaction, and disturbanceawareness. We summarize our main contributions as follows:

Integration of global planner and local planner for considering both operational and physical constraints.

Formulation of the global trajectory optimization problem of the reduced dimension, which permits computation time shorter than a few seconds.

Realtime constrained trajectory generation considering external disturbance.
Ii Equations of Motion
Iia Kinematics
Let and be the base frame and cabin frame where the base frame’s relative orientation is defined by the ground slope, and the orientation of differs from that of by the amount of swing rotation. The origin and axis of are illustrated in Fig. 3. The configuration of the mechanical system of a hydraulic excavator can be either defined with or where are swing angle, rotation angles of boom, arm, and bucket, and are cylinder displacements of boom, arm, and bucket as shown in Fig. 1. The nonlinear, bijective^{1}^{1}1This is true within a physically feasible region, and the region is considered in the local planning as a cylinder displacement constraint. mapping between and can be derived by solving closedchain kinematics, denoted by
(1)  
where are corresponding Jacobian matrices.
In some parts of this paper, the configuration with fixed is used. For the sake of notational simplicity, we denote this subconfiguration by . We also denote the pose of the bucket tip with respect to the frame by where are the bucket tip’s , directional positions, and is its orientation, i.e., . The components of can be computed from using the forward kinematics. We write them as .
IiB Dynamics
By defining the system state and control input where are swing torque, hydraulic forces of boom, arm, and bucket cylinder, the system dynamics can be derived with the EulerLagrange equation:
(2) 
where , are the mass matrix and combined term for the Corioliscentrifugal and gravitational effects. is a lumped disturbance describing unmodeled dynamics, e.g. friction in hydraulic cylinders, soilbucket interaction, and model uncertainties.
As explained in section III and IV, since several state constraints considered in global and local planning can be expressed as linear constraints with respect to the hydraulic cylinder’s motion, we further modify the system dynamics (2) with the newly defined system state . The equations of motion (2) can be rewritten with respect to and as follows:
(3) 
where , , and . Here, by inserting the results of kinematics (1) into the and , and are defined as and .
Iii Global Planning
A single excavation task of an excavator consists of three phases as follows:

Phase 1: The excavator tip moves from its initial position to the point where it enters the earth surface.

Phase 2: The bucket tip cuts through the soil.

Phase 3: The bucket conveys removed soil to the goal position, e.g., right above the dump box of a truck, without spilling the soil.
Details regarding each phase are described in Fig. 3. Between consecutive phases, to guarantee continuity of control, configuration and its derivative must be matched.
Iiia Global planning for phase 2
In this subsection, we set up the trajectory optimization problem for phase 2. For phase 2, we constrain the cabin part of the excavator from moving, in order to prevent the side plates of the bucket from pushing against the soil. The goal of phase 2 global planning is to minimize the length travelled by the hydraulic cylinders during the excavation task. Unlike phases 1 and 3, the excavator faces large and unpredictable external disturbance which originates from the direct contact between the earth and the bucket. Traditional torque or forceminimizing strategies are therefore not suitable, since their optimality would be degraded due to external forces. The travelled distance of the hydraulic cylinders should preferably serve as a cost measure, as the earth shall always resist the bucket’s motion while in phase 2. At the same time, we should prefer large excavation volume, because digging more volume will naturally reduce the number of repetitive excavation tasks needed to complete a mission. To take the aforementioned considerations into account, we first discretize the phase 2 trajectory using waypoints, , , , and propose the following cost function:
(4) 
where is the volume of the removed soil, and are weights associated with the distance travelled by the cylinder and , respectively. is a positivedefinite weight matrix, where (
) denotes a weighted 2norm of a vector
with a matrix .In phase 2, earth surface and target shape are described using polynomials over , where and are given with respect to the frame (Fig. 3):
(5)  
Within the region of interest , the earth surface should be above the target, i.e., . Now, we consider the following constraints for the bucket tip.

The bucket tip should always be located within the region of interest, above the target surface and below the earth. At initial and final conditions, it must be located on the earth surface.

The rotation angle of the bucket tip must be monotonically increasing; otherwise, the bucket plate pushes the dirt out of the bucket, which is energyinefficient.

The bucket must maintain positive clearance angle () while digging. This ensures that the bucket parts other than the tip do not push against the ground.

In the similar sense, the whole bucket body must move above the tip path.

The bucket rotation must be smaller than : otherwise, the excavator will spill the dirt onto its wrist, which will escalate the risk of equipment malfunctioning. (Fig. 4)
The mentioned constraints are explained in Fig. 5.
Basically, the volume of the removed dirt is geometrically determined using the swept volume between the earth surface and the bucket tip trajectory. However, if the swept volume is greater than the bucket capacity, the excavator shall spill the excess dirt, thus , where
(6)  
is the (approximate) swept volume calculated using the trapezoidal integration rule, , , and . The maximum capacity of the bucket, which is a function of , is modeled as described in Fig. 4.
IiiB Global planning for phases 1 and 3
Unlike phase 2, in phases 1 and 3, the bucket travels a long distance while influenced by fewer constraints. Therefore, in order to reduce the computation burden, we parameterize the trajectory using a preselected set of bases. In this paper, Bernstein polynomials are used to parameterize the trajectory. Bernstein polynomial is written as
(7) 
where . Bernstein bases are used because the convex hull property of the bases allows to incorporate the box constraint for and linear inequality constraints for easily. Furthermore, their timederivatives can be easily calculated, so faster computation speed can be achieved. For phases 1 and 3, we minimize the force cost during travel.
(8)  
where is the th control point for the Bernstein polynomial representation of , and is the time cost for complete traversal. Superscripts and denote lower and upper bounds for the state variables, respectively. The boundary values , , , are chosen such that the transition between any two consecutive phases is smooth. is a positivedefinite weighting matrix. The middle term in the third constraint is the control points for and was derived from the properties of Bernstein bases. For phase 3, to keep the excavator from spilling the dirt, we introduce one additional constraint:
(9) 
where is the bucket rotation angle at which phase 2 ended.
Iv Local Planning
This section describes how local reference trajectory is computed using MPC, which has been widely adopted in robotics as a realtime planner [17, 18, 19] thanks to its optimality and constraintsatisfying nature. The objective of this local recedinghorizon planning is the following three: 1) physical constraint satisfaction, 2) disturbanceawareness, and 3) realtime computation. In addition to state constraints, actuationrelated constraints are explicitly considered in the MPC problem formulation. Furthermore, to handle uncertainties in the dynamical model (3), disturbance estimation is appended to the local planning for disturbanceawareness. Lastly, to guarantee realtime applicability of the proposed local planning, we reformulate the MPC problem using feedback linearization as in [20].
Iva Constraints
Similar to other existing researches for trajectory optimization of a hydraulic excavator [6, 7], we consider four types of physical constraints in the local planning: 1) actuation (force/torque) limit, 2) power limit, 3) cylinder displacement limit, and 4) pump flow rate limit. They are modeled as
(10a)  
(10b)  
(10c)  
(10d) 
where in (10a) are the lower and upper bounds of the control input , and in (10b) is the upper bound of the overall power generated by the hydraulic pump. It is required to impose this overall power limit because of the actuation mechanism of the hydraulic excavator where all four control inputs are powered by the same actuation source. in (10c) are the lower and upper bounds of the hydraulic cylinder displacement . Lastly, and in (10d) are the upper bound of the pump flow rate and crosssection areas corresponding to swing motion, boom, arm, and bucket cylinder displacement, respectively. Because the crosssection area differs depending on the motion of a hydraulic cylinder, either contraction or expansion, is defined to include such effect and modeled as a function of sign of [6]. is used to denote an elementwise absolutevalued vector mapped from a vector . To consider the flow rates of two hydraulic pumps, the subscript is used.
IvB Disturbance estimation
As mentioned in [21, 22], if disturbance is not properly considered in the system dynamics of MPC, constraints, especially state constraints, can be violated even if they were satisfied in the predicted trajectory computed with nominal dynamics. To alleviate such feasibility issue, inspired by successful demonstrations of integration between model predictive control and disturbance estimation in robotics [23, 24], we combined disturbance estimation and model predictive control for disturbanceawareness of the computed local reference trajectory.
We design a realtime disturbance estimation algorithm based on momentumbased disturbance estimation [1, 25]. Considering the system dynamics based on the joint angles of the excavator (2), the momentumbased disturbance estimation algorithm is formulated as follows:
(11)  
where are Coriolis matrix and gravity vector computed from the EulerLagrange equation. Utilizing the fact that , the estimation algorithm can be analyzed to operate as a low pass filter of the actual disturbance as The disturbance estimate of the cylinder displacementbased dynamics (3) then can be computed as
IvC Model predictive control
Based on the system dynamics (3) and constraints (10), the MPC problem is formulated as follows:
(12)  
s.t.  
where is the number of the prediction horizon, is the discretized dynamics of (3), are the inequality constraints (10), and are the number of inequality constraints. , , are positivedefinite weighting matrices. Here, the subscript is omitted in the state , dynamics , and disturbance estimate for brevity. A quadratic objective function is designed to allow the excavator to track the global reference trajectory while minimizing the overall effort . Lastly, similar to the widely adopted assumptions in papers on disturbanceaware model predictive control [22, 24, 23], we assume that the disturbance does not vary significantly over a short time horizon, i.e. .
To solve the formulated MPC problem (12), most optimal control solvers require partial derivatives, either numerical or analytic, of the dynamics (3) and constraints (10) with respect to the state and the input . However, due to the high nonlinearity of the system dynamics (3), especially the matrix inverse of the mass matrix which inheres in the closedchain kinematics, it is difficult to obtain an optimized solution in realtime. Therefore, we simplify the MPC problem by introducing a virtual input and a feedbacklinearizationbased control law through which the original nonlinear dynamics (3) is transformed into a linear dynamics as
(13) 
where are zero and identity matrices. The feedbacklinearizationbased MPC problem is then formulated as follows:
(14)  
s.t.  
where , , are positivedefinite weight matrices, and are constant matrices in the discretized dynamics, corresponding to and in the continuous counterpart (13).
V Simulation
Va Setup
We validate the proposed planning algorithms in simulation. The overall flow chart during simulation is illustrated in Fig. 2. All modules in Fig. 2 are individually implemented in Robot Operating System (ROS) using C++. Excavator dynamics simulator (PLANT in Fig. 2) is based on the derived system dynamics (2) where the external disturbance is modeled with a steadystate modified LuGre friction model [26] for cylinder friction and modified Fundamental Earthmoving Equation (FEE) [27, 7] for soilbucket interaction. Note that friction and soilbucket interaction models are introduced only to construct a realistic simulation environment and are unknown to the planning algorithms. Parameters in the system dynamics and physical constraints are from the Hyundai HX300L model in Fig. 1.
As a motion controller, which computes the control input based on the reference trajectory generated from the local planning module, we adopt a feedback linearizationbased PID controller: . Applying the control input to the system equation (2), inputoutput stability of the closedloop system can be derived [28, Ch. 5] where the input is the estimation error of the external disturbance multiplied by the inverse of the mass matrix , and the output is the state .
The global planner is implemented using ALGLIB [29]
, an opensource nonlinear optimization solver. We use 9 Bernstein control points for phases 1 and 3, respectively, and 20 via points for phase 2. Next, to solve the formulated model predictive control problem (
14), we implement differential dynamic programming (DDP) with augmented Lagrangian method [30], which is an algorithm widely exploited in various robotic application for realtime trajectory optimization [19, 17, 18]. It transforms the original constrained MPC problem into an unconstrained problem using the Lagrangian and quadratic penalty function, which is then solved with the conventional DDP algorithm. For MPC implementation, we employ the time horizon of 1000 ms, control discretization and integration rate of 20 ms. Except for the results of cylinder displacements , which are normalized with constant bias and constant normalization factors, the other results related to physical limits of Hyundai HX300L displayed in Fig. 7 and Fig. 8 are normalized with constant normalization factors, denoted with . Each element of control inputs and the corresponding channel of disturbance estimates are normalized with the same factor for comparison.To validate the proposed framework, we conducted two simulation scenarios on a desktop computer equipped with an Intel Core i79750 CPU at 2.6 GHz, by varying the target excavation depth: 1) shallow excavation and 2) deep excavation. As depicted in Fig. 2, we assume prior knowledge of the environment, i.e. the ground shape, which can be obtained either from onboard sensors like lidars and cameras, or from external sensors like total station in real experiment.
VB Results
VB1 Scenario 1  shallow excavation
Fig. 6a and Fig. 7 describe the simulation result for shallow excavation. In the first scenario, not to penetrate through the target shape, the global planner plans a trajectory that stretches out in a relatively large region in phase 2. The bucket tip well tracks the target shape, and tipgradinglike motion is generated. The global trajectory was generated in 2.1 seconds. As in (a) and (c), even if the global trajectory is intentionally generated to exceed the physical limit of some variables that are not considered in the global planning, physical constraints are still satisfied during local planning phase. The control input trajectory computed from the local planning is displayed in (b) where all the input limits are satisfied. In (d), the hydraulic cylinder friction model can be clearly found at the bucket cylinder (bottomleft subfigure), whereas the effect by the friction model is found to be smaller at the boom cylinder (topright subfigure) and the arm cylinder (bottomleft) due to larger disturbance from the soilbucket interaction. Still, the disturbance estimate (black line) follows the modeled disturbance (red line) in all directions. The average computation time of the local planning is 21.7 ms ( 46 Hz) which we believe is sufficient for realtime application.
VB2 Scenario 2  deep excavation
Simulation results of the second scenario can be found in Fig. 8 and Fig. 6b. In contrast to the first scenario, if the target ground is located deep below the earth surface, the excavator tip does not aim to reach the target shape in phase 2. Instead, it maximizes the excavation volume by generating a triangleshaped phase 2 trajectory. Since the phase 2 trajectory is constrained not to exceed the target shape, such deep target shape can be finally reached by repeating this targetshapeaware excavationvolumemaximizing strategy. The global trajectory was generated in 1.6 seconds. As in the subfigure (d), larger disturbance than that in the first scenario was applied due to deeper penetration of the bucket into the soil. Even in the presence of such larger disturbance, similar to the results of the first scenario, the global reference is well tracked by the local planning as can be found in the subfigure (a). While tracking the global reference, the computed local trajectory is shown to satisfy all the physical constraints as in the subfigures (a), (b), and (c). It took 30.2 ms ( 33 Hz) for local planning computation on average.
Vi Conclusion
In this paper, we presented a realtime planning algorithm for a single digging task of a hydraulic excavator. The algorithm consists of a global and a local planner. In the global planning phase, the trajectory that maximizes excavation volume and minimizes energy cost under the operational constraints is generated online using Bernstein parameterization. The highfidelity local planner tracks the generated global trajectory while satisfying the physical constraints in realtime. The proposed algorithm was validated through a realistic simulation environment with two different operation objectives: deep and shallow soil excavation. The simulation results showed that the proposed online global planner generates an optimized bucket tip trajectory in a few seconds, and the local planner plans disturbanceaware, physically feasible receding horizon trajectories in realtime. Future work may include validation of the proposed algorithm in actual excavators.
Acknowledgment
The authors would like to appreciate Young Bum Kim (Hyundai Construction Equipment) for his kind support in the simulation setup.
References
 [1] B. Son, C. Kim, C. Kim, and D. Lee, “Expertemulating excavation trajectory planning for autonomous robotic industrial excavator,” in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2020, pp. 2656–2662.
 [2] R. J. Sandzimier and H. H. Asada, “A datadriven approach to prediction and optimal bucketfilling control for autonomous excavators,” IEEE Robotics and Automation Letters, vol. 5, no. 2, pp. 2682–2689, 2020.

[3]
F. E. Sotiropoulos and H. H. Asada, “Autonomous excavation of rocks using a gaussian process model and unscented kalman filter,”
IEEE Robotics and Automation Letters, vol. 5, no. 2, pp. 2491–2497, 2020.  [4] R. Mascaro, M. Wermelinger, M. Hutter, and M. Chli, “Towards automating construction tasks: Largescale object mapping, segmentation, and manipulation,” Journal of Field Robotics, 2020.
 [5] A. Stentz, J. Bares, S. Singh, and P. Rowe, “A robotic excavator for autonomous truck loading,” Autonomous Robots, vol. 7, no. 2, pp. 175–186, 1999.
 [6] S. Yoo, C. Park, S. You, and B. Lim, “A dynamicsbased optimal trajectory generation for controlling an automated excavator,” Proceedings of the Institution of Mechanical Engineers, Part C: Journal of Mechanical Engineering Science, vol. 224, no. 10, pp. 2109–2119, 2010.
 [7] Y. B. Kim, J. Ha, H. Kang, P. Y. Kim, J. Park, and F. Park, “Dynamically optimal trajectories for earthmoving excavators,” Automation in Construction, vol. 35, pp. 568–578, 2013.
 [8] Y. Yang, P. Long, X. Song, J. Pan, and L. Zhang, “Optimizationbased framework for excavation trajectory generation,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 1479–1486, 2021.
 [9] Y. Yang, J. Pan, P. Long, X. Song, and L. Zhang, “Time variable minimum torque trajectory optimization for autonomous excavator,” arXiv preprint arXiv:2006.00811, 2020.
 [10] E. Jelavic and M. Hutter, “Wholebody motion planning for walking excavators,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019, pp. 2292–2299.
 [11] E. Jelavic, Y. Berdou, D. Jud, S. Kerscher, and M. Hutter, “Terrainadaptive planning and control of complex motions for walking excavators,” in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2020, pp. 2684–2691.
 [12] F. A. Bender, S. Göltz, T. Bräunl, and O. Sawodny, “Modeling and offsetfree model predictive control of a hydraulic mini excavator,” IEEE Transactions on Automation Science and Engineering, vol. 14, no. 4, pp. 1682–1694, 2017.
 [13] H. Wind, A. Renner, F. A. Bender, and O. Sawodny, “Trajectory generation for a hydraulic mini excavator using nonlinear model predictive control,” in 2020 IEEE International Conference on Industrial Technology (ICIT). IEEE, 2020, pp. 107–112.
 [14] Y. Yang, L. Zhang, X. Cheng, J. Pan, and R. Yang, “Compact reachability map for excavator motion planning,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019, pp. 2308–2313.
 [15] D. Jud, G. Hottiger, P. Leemann, and M. Hutter, “Planning and control for autonomous excavation,” IEEE Robotics and Automation Letters, vol. 2, no. 4, pp. 2151–2158, 2017.
 [16] D. Jud, P. Leemann, S. Kerscher, and M. Hutter, “Autonomous freeform trenching using a walking excavator,” IEEE Robotics and Automation Letters, vol. 4, no. 4, pp. 3208–3215, 2019.
 [17] F. Farshidian, E. Jelavic, A. Satapathy, M. Giftthaler, and J. Buchli, “Realtime motion planning of legged robots: A model predictive control approach,” in 2017 IEEERAS 17th International Conference on Humanoid Robotics (Humanoids). IEEE, 2017, pp. 577–584.
 [18] D. Lee, H. Seo, D. Kim, and H. J. Kim, “Aerial manipulation using model predictive control for opening a hinged door,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 1237–1242.
 [19] C. Y. Son, H. Seo, D. Jang, and H. J. Kim, “Realtime optimal trajectory generation and control of a multirotor with a suspended load for obstacle avoidance,” IEEE Robotics and Automation Letters, vol. 5, no. 2, pp. 1915–1922, 2020.
 [20] W. Van Soest, Q. Chu, and J. Mulder, “Combined feedback linearization and constrained model predictive control for entry flight,” Journal of guidance, control, and dynamics, vol. 29, no. 2, pp. 427–434, 2006.
 [21] D. Q. Mayne, J. B. Rawlings, C. V. Rao, and P. O. Scokaert, “Constrained model predictive control: Stability and optimality,” Automatica, vol. 36, no. 6, pp. 789–814, 2000.
 [22] Z. Li and J. Sun, “Disturbance compensating model predictive control with application to ship heading control,” IEEE transactions on control systems technology, vol. 20, no. 1, pp. 257–265, 2011.
 [23] D. Hentzen, T. Stastny, R. Siegwart, and R. Brockers, “Disturbance estimation and rejection for highprecision multirotor position control,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019, pp. 2797–2804.
 [24] B. B. Kocer, T. Tjahjowidodo, and G. G. L. Seet, “Model predictive uavtool interaction control enhanced by external forces,” Mechatronics, vol. 58, pp. 47–57, 2019.
 [25] T. Tomić, C. Ott, and S. Haddadin, “External wrench estimation, collision detection, and reflex reaction for flying robots,” IEEE Transactions on Robotics, vol. 33, no. 6, pp. 1467–1482, 2017.
 [26] X. B. Tran, N. Hafizah, and H. Yanada, “Modeling of dynamic friction behaviors of hydraulic cylinders,” Mechatronics, vol. 22, no. 1, pp. 65–75, 2012.
 [27] O. Luengo, S. Singh, and H. Cannon, “Modeling and identification of soiltool interaction in automated excavation,” in Proceedings. 1998 IEEE/RSJ International Conference on Intelligent Robots and Systems. Innovations in Theory, Practice and Applications (Cat. No. 98CH36190), vol. 3. IEEE, 1998, pp. 1900–1906.
 [28] H. K. Khalil and J. W. Grizzle, Nonlinear systems. Prentice hall Upper Saddle River, NJ, 2002, vol. 3.
 [29] S. Bochkanov and V. Bystritsky, “Alglib,” Available from: www alglib net, vol. 59, 2013.
 [30] B. Plancher, Z. Manchester, and S. Kuindersma, “Constrained unscented dynamic programming,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017, pp. 5674–5680.