Model Predictive Control for an Aerial Tree-Pruning Robot Based on Alternating Direction Method of Multipliers

College of Automation Engineering, Nanjing University of Aeronautics and Astronautics, Nanjing 211106, China Key Laboratory of Navigation, Control and Health-Management Technologies of Advanced Aerocraft (Nanjing University of Aeronautics and Astronautics), Ministry of Industry and Information Technology, Nanjing 211106, China Jiangsu Key Laboratory of Internet of +ings and Control Technologies (Nanjing University of Aeronautics and Astronautics), Nanjing 211106, China Electric Power Research Institute of Guizhou Power Grid Co., Ltd., Guiyang 550002, China


Introduction
Aerial robots with task tools conducting contact forces with the environment are of typical complex systems, of which the dynamics analysis and control have become emerging hot topics [1]. And as the nonlinear complexity of robot model grows, the optimization control strategy has shown a huge superiority. Electric power transmission system is the key infrastructure to ensure our living and producing. erefore, the national security would encounter threat once the system encounters a breakdown. Trees and branches beside the high-voltage line are among the most hazardous factors that would cause the fault and accident of electric power transmission. So, the tree barriers clean up is the priority among priorities in the operation and maintenance of the electric power facilities. And most of the work has to be implemented by the close cooperation of human and ground equipment. However, power supply companies have been suffering severely from the low operation efficiency and continuously increasing labor costs. e aerial tree-pruning robot would be a promising replacement for the task of trees barrier clean up. e advantages of the aerial tree-pruning robot utilization are as follows. First, the flying pruning robot could reach the operation spot very rapidly, whereas, during manual work, operators always have to carry up and down by lifting machines. Second, greater working space enable the robot to implement tasks at much higher and distant position, but lifting machines, by contrast, have limited arm length and working space.
ird, the robot is friendlier to the environment as it consumes no fossil fuel. Finally, low cost is one of the most impressive advantages, whereas electric power companies have been suffering a lot from the trees pruning by labor. e most challenging task of tree pruning for the aerial robot would be accomplishing the attitude and position control onto the large yaw-inertia aerial robot with the model uncertainty and external disturbance as the pruning operation causes great and sophisticated contact forces. erefore, to build an efficient control algorithm for the robot is the prior option before implementing the pruning operation for the robot.
As there is very little research on pruning robot [2] and the new robot is a sort of multirotor with special structural configuration, it is well reasoned that we consider the method of the pruning robot control from the state-of-theart literature of the control for quadrotors and aircrafts.
Until now, the control problem of quadrotor has been addressed successfully with various approaches, such as PID control [3], backstepping control [4,5], sliding mode control [6], ADRC [7], dynamic inversion [8], and H∞ control [9]. However, model predictive control (MPC) is the most widespread modern control approach applied in industrial process [10], with PID the most popular classical control method. From a certain perspective, the problem of UAV control can be solved by the idea of optimizing control, and model predictive control is aimed right at control problems with optimization requirements. e success of predictive control in complex industrial processes has fully demonstrated its great potential for handling complex optimization control problems with constrains. People put forward higher and higher requirements for control and are no longer satisfied with the traditional stabilization design but hope that the control system can obtain better performance through optimization and model predictive control could just handle this issue. MPC is competent in not only largescale systems, such as traffic [11], energy [12], environment [13], and material synthesis [14] but also fast dynamic systems, such as power electronic converters [15], electrical motor drives [16], torque-split for hybrid vehicle [17], and so on. For instance, a signal-split control approach with explicit model predictive control (EMPC) has been proposed to reduce the complexity of online optimization in the field of transportation system and to increase the applicability for real transportation system [11].
Concerning the path planning, a fast MPC method is introduced to obtain a UAVs' path, where collision avoidance between UAVs and the change of communication topology among UAVs are taken into account [19]. Moreover, model predictive control with sequential linear quadratic (SLQ) solver has been applied to compute feasible and optimal trajectory and to operate a multirotor with a suspended load in dynamic environments [28]. At the same time, regarding the position and attitude control, Alexis [29][30][31][32][33][34][35] promotes great progresses in MPC-based quadrotor control research for the last decade. a real-time robust model predictive control scheme in various challenging conditions have been proposed and experimentally evaluated to addresses the problem of robust flight control of unmanned rotorcrafts, which aims to capture the demanding nature of the potential requirements for their efficient and safe integration in real-life operations [32]. Also, the hardware implementation of MPC was accomplished based on a Matrice 100 quadrotor from DJI alongside its ROS C++ Software Development Kit (SDK) [36].
MPC involves finding cost function values for optimization; therefore, the optimizing efficiency is one of the most essential issues to be tackled. Also, as our goal of the project of the aerial pruning robot is to cut out the tree branches in the field rather than just implement the robot simulation on desktop, real-time computing on the embedded computer with limited computational power should be guaranteed. However, the majority of the optimization problems are tackled with commercial solvers like Gurobi, SNOPT, MOSEK, and so on, which is not suitable for the MPC application on embedded computers which are installed in the aerial robot and responsible for outputting the right command signals to keep the attitude stability and follow the desired trajectories. e reason includes that commercial solvers are always having large code size, higher demand for working memory, and heavy library dependency.
ADMM (Alternating Direction Method of Multipliers) is an optimal problem-solving method that splits complex problem into some relatively simpler problems that could be solved by computers with limited power like embedded microprocessor. Researchers have been reported applying ADMM in several fields like traffic signal control [37] and microgrids (MGs) [38]. Also, ADMM is adopted to realize parallelism of the distributed model predictive control (DMPC) of waterborne AGVs (automated guided vessels) which is modeled with independent dynamics and objectives but coupling collision avoidance constraints [39]. Furthermore, the ADMM-based distributed model predictive flow control (DMPFC) approach has been utilized to the planning of the cooperative synchromodal freight transport [40].
Nonetheless, to the best of our knowledge, ADMMbased MPC has hardly been used to quadrotor control, not to speak of the aerial tree-pruning robots.
Motivated by above discussion and analysis, in this paper, we will present an ADMM algorithm that solves a constrained QP (quadratic programming) to implement the Model Predictive Control for the aerial tree-pruning robots. e remainder of this paper is organized as follows. e dynamic model of the pruning robot is presented first. en, the control scheme of MPC for the pruning robot is described. Moreover, the QP problem of robot control is addressed with ADMM. Finally, the simulation experimental implementation of the ADMM-based MPC control onto the aerial pruning robot is given.

Problem Description
To begin with, modelling of aerial robot should be considered when conducting the problem description. In our previous work, a tree-pruning robot was designed, and its modelling and LQR control were also presented. Here we followed the modelling methods of our published paper [41]. e floating base robot with 6 DoF (degrees of freedom), has 3 main modules which are multi-rotor aircraft with thrust-ducted propellers, a robot arm (no joints for now) and operation devices (shown in Figure 1). is enables the robot DoF to be listed as elevation, sway, surge, roll, pitch, and yaw.

Robot Kinematics and Dynamics.
e coordinate systems of the robot are built with two coordinates, the world inertial coordinate W and the robot body coordinate B as shown in Figure 2. e translational position of the robot body is denoted as world coordinate variable elevation, sway, and surge p � p x p y p z T . e robot attitude Θ � ϕ θ ψ T is defined in the world coordinate, which is expressed by the Euler angles (roll, pitch, and yaw).
In the kinematics representation, transforming coordi- where R x (ϕ), R y (θ), and R z (ψ) are the rotation matrixes along their corresponding axis. e center of mass of the robot is described by the vector p in the inertial coordinate system W. e total thrust produced by the six propellers in Figure 1 is the direct factor that causes the translation and attitude acceleration of the robot. e advantage of the new structure of the robot in this paper is that two ducted thrusters are installed under the robot, which could achieve "zero pitch angle" forward and backward translation, completing forward translation and accurately stop without the cooperation of attitude angle. e dynamics of translation in the inertial frame are described as where g is the acceleration generated by the Earth gravity and f is the vector sum of the rotor forces f i that act to the robot mass m.
e control inputs to the tree-pruning robot are the mass-normalized thrust of the rotor forces sum f, and the desired forces sum f are specified by the desired rotational rates along the body axes of the tree-pruning robot b ω � ω x ω y ω z T , which is shown in Figure 1.
Also, there exists a relationship between the angular velocity vector ω of the robot body and the rate of change of the robot attitude: Not only each rotor of the aerial tree-pruning robot produces a force f i , i � 1, 2, . . . , 6 in the positive direction of x b or z b , but also the propellers generate reaction torques M i , i � 1, 2, . . . , 6 which are perpendicular to the plane of rotor blade rotation as shown below: where c Mi is the coefficient of the reaction torque from the ith propeller. ω i is the rotate speed of the ith propeller. As shown in Figure 1, Rotors 1 and 3 rotate to the positive z b direction, resulting in moments that act in the direction of the negative z b . Meanwhile, Rotors 2 and 4 rotate to the negative direction, which causes the reaction torques to the positive z b direction [42]. Also, Rotors 5 and 6 rotate to the negative and positive direction, respectively, and cause the reaction torques, too. However, Rotors 5 and 6 rotate with the same speed. So, their reaction torques sum to 0.
Given the inertia matrix I with respect to the center of mass and the robot body coordinate B, the rotational dynamics of the body-fixed frame could be obtained.
where L is the distance between two adjacent lifting rotors which are arranged in a square. L Bst is the distance between the center of mass and the boosters (Rotors 5 and 6) along the z b axis. I refers to the moment of inertia matrix referenced to the center of mass.

Nonlinear System of the Aerial Tree-Pruning Robot.
During the operation of tree barriers cleaning, the contact force significantly affects the attitude of the flying robot, which could not be ignored when the control system is designed. For the convenience of the control with system uncertainty and time-varying disturbance, it is necessary to formulate the robot model in the MIMO nonlinear system below: ΔF and ΔG are the system uncertainties, and d(t) is the unknown disturbance.

Model Predictive Control.
In order to keep the research proceed, discretization process is conducted to the statespace model of the aerial pruning robot, which is shown below: where X k ∈ R n represents the pruning robot's state of attitude, and U k ∈ R n refers to the robot input generated from the MPC controller, at time k, A and B denote the system matrix and input matrix, respectively. e core of the pruning robot MPC is to specify a cost function of the kth time step and find its minimum value. e cost function of the finite horizon optimal control problem is shown as s.t. X k+p+1|k � AX k+p|k + BU k+p|k , p � 0, . . . , H − 1, where L ∈ R n , M ∈ R n , and N ∈ R n usually are positive semi-definite matrices, namely L ≥ 0, M ≥ 0, and N ≥ 0. Specifically, L determines the penalty degree of the robot state's deviation at the end of the prediction horizon H. And, M represents the penalty degree of the robot state's deviation over the entire horizon. N affects the proportion of the robot control inputs in the cost function. X k+p|k denotes the predicted state vector at time k + p and is acquired by result from the sequence of input u k , u k+1 , . . . , u k+p−1 . e state maximum X max ∈ R n and the state minimum X min ∈ R n form the constraint of the state variable X. And, the 4 Complexity constraint of the input variable U is similarly defined composed of the input maximum U max ∈ R m and the input minimum U min ∈ R m . e main idea of the model predictive control is as follows. A sequence of control inputs u k , u k+1 , . . . , u k+p−1 are generated at the time step k, at which time the state of the pruning robot is X k . And by assigning X k as the initial state to cost function (9) with constraints (10), the sequence of optimal control input could be obtained. Nevertheless, the receding horizon framework refers that not all the generated control input elements of the sequence would be applied to the robot control. Instead, only the first element is adopted, with all the rest ones culled. is comes to an end of the work procedure at time step k, and the new initial state would be obtained after the effect of the optimal input. Similarly, the new selected optimal input at step time k + 1 is acquired.

Alternating Direction Method of Multipliers (ADMM).
e alternating direction method of multipliers (ADMM) is a brilliant method that solves the problems of convex optimization. e main idea of ADMM is that a global optimization problem could be ingeniously broken into some smaller pieces which could be able to be solved relatively much easier [43]. e standard form of optimizing problems that ADMM could effectively handle could be presented as where f and W must be convex. And for the specific application to the pruning robot control, the optimization problem has another form to be rewritten, which is, where W has an indicator function h, and the constraint becomes the subtraction of a primal variable AX and a separate variable BY − n. en, the augmented Lagrangian equation which is an obvious element of ADMM is assigned as where a scaled dual variable n is introduced and ρ acts as a positive penalty parameter. e general method of the augmented Lagrange multiplier could be summarized as finding the minimum of L ρ w.r.t. both AX and BY − n. However, the direct minimizing of L ρ is hard, and the obvious separable character of the objective function is not well utilized.
us, in order to accomplish minimality, ADMM tactfully have the global problem decomposed into two separate optimization subproblems. e mathematical expression of the above process could be shown as For the beginning, several steps of the robot model, we have the state equations as So according to a deduction, the robot state at time k + p is could be reckoned as To transform the objective function into a typical QP form, the elements in the X k+p deduction formula could be expanded as follows: −1 B, A p−2 B, . . . , B] and U � [U k , U k+1 , . . . , U k+H−1 ]; then, Complexity us the robot state is obtained as e transposition of X k+p is Expand X T k+p MX k+p using the above equation, we have where Similarly, X T H LX H could be rewritten using the X T t+p MX t+p expansion method above. erefore, we have the MPC optimization problem transformed to a QP problem whose objective function is presented as where As for the constraints of the pruning robot's MPC problem, there exists X min ≤ X p ≤ X max , p � 0, . . . , H − 1, Also, a definition is made: −1 B, A p−2 B, . . . , B ,

(26)
Take the equation of X k+p above into which becomes So by solving the inequation above, U is limited by which is disassembled as a set of inequalities as A combination is taken to the inequation group above, the QP constraints could be described as where e stop criterion of ADMM reaching convergence is obtained as e algorithm of ADMM is shown as a pseudo code in Algorithm 1.

Simulation and Experiment Results
e efficiency and efficacy of the control scheme proposed in this paper is investigated using some simulation study whose main idea is to check the trajectory tracking performance under the contact force from saw-tree interaction and system uncertainty. erefore, the following experiments were organized to verify the control algorithm to the robot. Firstly, the robot attitude control performance under ADMM-MPC control is discussed through the pitch, roll, and yaw tracking of desired trajectories. e second part of experiment is designed to check the cutting force effects during the robot operation where 3 continuous forces were added to the robot along 3 difference axis respectively. e simulation results are then shown about the pruning operation where the contact forces generated between the saws and tree branches are significant 6 Complexity disturbances to the aerial robot. Finally, considering the large inertia of the robot pitch and yaw that could challenge our control algorithm, the inertia is then increased to check the regulating performance. Also, the optimization performance is presented as well.
In terms of evaluating the performance of the robot attitude tracking, some parameters must be specified as one of the prerequisites. Table 1 gives the physical information including kinematic variables. e reference trajectories of the robot attitude are specified as Under the developed control scheme, the simulation results for aerial tree-pruning robot are shown in Figures 3-6. Meanwhile, in order to examine the control performance of the robot being impacted by the cutting forces, we specified respectively two pulse disturbances along the robot Y and Z axes which acts from 3 to 3.25 s, as shown in Figure 7. Figures 3-5 show the robot attitude tracking results, where the cyan solid lines indicate the real trajectories, and the blue dashed lines indicate the desired trajectories. Obviously, the tracking performance of system is satisfactory; meanwhile, the tracking errors always be maintained between the prescribed performance bounds. Figure 6 shows the body angular velocities of the aerial tree-pruning robot.
It can be shown from these simulation results of the numerical example that the closed-loop system for the nonlinear system of the aerial tree-pruning robot with input saturation and parametric uncertainties is asymptotically stable. erefore, the conclusion could be given that the proposed robust control method is valid. And the robot has performed satisfactorily during the period of cutting operation, which has proved strong robustness of the control scheme.
In order to demonstrate the optimization performance of the MPC control based on ADMM for the pruning robot. Figure 8 shows the cost function values of each step during the control process. e blue "x" listed in the same column represent the cost function value changes of one control step. Only in the very beginning, the cost value is pretty big, and the values converged exponentially to values around the steady-state value just within 0.5 second.
e running time of optimization in each step is one of the most common variables that could embody the optimization performance. Figure 9 shows the optimization running time of each step during the robot control. In the first several steps, it took relatively longer time for the optimizing completion because much larger number of iteration is needed to find optimized solutions, which is also very common in other algorithm of optimization. Figure 10 shows the iteration number of each step of the simulation. e iteration number of optimization means the number of trial to obtain successively closer approximations to the best control input of the aerial pruning robot. In the Initialize (X 0 , Y 0 , U 0 ); Repeat X p+1 : � arg min(f(X)) + (ρ/2)‖AX + BY p − n + u p ‖ 2 2 ; Y p+1 : � arg min(h(X)) + (ρ/2)‖AX p+1 + BY − n + u p ‖ 2 2 ; u p+1 : � u p + AX p+1 + BY p+1 − n; p � p + 1; Until ‖w p ‖ 2 ≤ ε pri ; ‖z p ‖ 2 ≤ ε dual ; Return: optimal control input U * ; ALGORITHM 1: MPC optimization solving with ADMM.          very beginning of the simulation, up to 40 iterations have been taken to get the optimization result, and the number sharply converges to a relatively steady state remaining less than eight, which means an excellent optimizing capability of the algorithm.
As the aerial pruning robot features much larger inertia in the pitch and yaw channel than that of normal multirotor aircraft, the proposed control algorithm's ability of the large inertia tolerance is necessary to be verified here. We increased the moment of inertia along three axes of the robot by 25% to check whether the control algorithm could still handle the trajectory tracking on the robot with the same hardware and software configuration. e simulation result is shown in Figure 11, which presents the comparison of the attitude trajectory tracking before and after the robot moment of inertia increases. e change of moment of inertia does not cause a failure of the control for the pruning robot, as the robot is still able to reach the desired attitude although in a relatively slower way. As in Figure 11, the tracking performance of the pitch channel looks less satisfying because of the increase of the moment of inertia, in which it is obvious to see that the MPC controller's adjusting time increases. But, it is still considered as a valid tracking control, as the controller could always make the robot attitude tracking the desired values.
However, the moment of inertia could not be increased ceaselessly because of the hardware restriction of the pruning aerial robot.

Conclusion
is paper mainly focuses on the design and validation of model predictive control for the aerial tree-pruning robot. A robust model predictive control scheme has been designed to achieve the trajectory tracking for an aerial tree-pruning robot in this article. Based on ADMM, the optimization solving problem has been issued through the MPC QP resolving. Finally, simulation results have been given to illustrate tracking performance of the proposed attitude and position control approach. e future research is going to consider a more calculation efficient solving method for aerial treepruning robot with operational external disturbances.

Data Availability
All data used to support the findings of this study are included within the article or generated from experiments.

Conflicts of Interest
e authors declare that there are no conflicts of interest.