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

Authors: Changliang Xu; Zhong Yang; Hao Xu; Qiuyan Zhang; Dongsheng Zhou; Kaiwen Lu; Jiaming Han; Luwei Liao
Journal: Complexity
 (2021)

---

## Abstract

Obstacles of some trees within the electric power transmission line channel are of great threat to the electricity supply. Nowadays, the tasks of clearing threatening tree branches are still mostly operated by hand and simple tools. In this article, an aerial tree-pruning robot with a novel structure is designed to improve the pruning operation efficiency and enhance the safety of the staff. However, the long arm of the pruning tool results in much higher rotational inertia of the robot, which brings difficulties for the robot to remain stable. Therefore, a control scheme based on model predictive control is proposed for the aerial tree-pruning robot and to deal with an uncertain system during the pruning operation period. One of the main contributions is that an ADMM (alternating direction method of multipliers) algorithm that solves the constrained QP (quadratic programming) is adopted to implement the model predictive control on embedded computers with limited computational power. The dynamic model of the pruning robot is firstly presented. Then, the control scheme of MPC for the pruning robot is presented. Moreover, the QP problem of robot control is addressed with ADMM. Finally, simulation experiments of attitude tracking as well as the antidisturbances capability verification have been conducted. Results for the system of aerial tree-pruning robot are given to demonstrate the effectiveness of the developed attitude tracking control scheme using ADMM-based MPC.


---

## Body

## 1. 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. Therefore, 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.The aerial tree-pruning robot would be a promising replacement for the task of trees barrier clean up.The 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.Third, 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.The 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.Therefore, 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-the-art 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. The 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 large-scale 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].MPC has also been applied successfully in aerospace research like quadrotor or fixed-wing UAVs where path planning [18–22] and attitude control problems [23–27] are the two main issues that involve MPC.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–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. The 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, ADMM-based 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. The remainder of this paper is organized as follows. The dynamic model of the pruning robot is presented first. Then, 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.


## 2. 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].The 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 Figure1). This enables the robot DoF to be listed as elevation, sway, surge, roll, pitch, and yaw.Figure 1
The inputs and moments of the aerial tree-pruning robot.
(a)(b)


### 2.1. Robot Kinematics and Dynamics

The coordinate systems of the robot are built with two coordinates, the world inertial coordinateW and the robot body coordinate B as shown in Figure 2. The translational position of the robot body is denoted as world coordinate variable elevation, sway, and surge p=pxpypzT. The robot attitude Θ=ϕθψT is defined in the world coordinate, which is expressed by the Euler angles (roll, pitch, and yaw).Figure 2
The world coordinate systemsW and robot body coordinate system B.In the kinematics representation, transforming coordinates fromW to B is necessary. Therefore, the rotation matrix RWBϕ,θ,ψ is defined as(1)RWBϕ,θ,ψ=RzTψRyTθRxTϕ,where Rxϕ, Ryθ, and Rzψ are the rotation matrixes along their corresponding axis.The center of mass of the robot is described by the vectorp in the inertial coordinate system W. The total thrust produced by the six propellers in Figure 1 is the direct factor that causes the translation and attitude acceleration of the robot. The 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. The dynamics of translation in the inertial frame are described as(2)p¨=RWBϕ,θ,ψf−00g,where g is the acceleration generated by the Earth gravity and f is the vector sum of the rotor forces fi that act to the robot mass m.(3)f=∑i=16fi.The control inputs to the tree-pruning robot are the mass-normalized thrust of the rotor forces sumf, and the desired forces sum f are specified by the desired rotational rates along the body axes of the tree-pruning robotbω=ωxωyωzT, 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:(4)ωxωyωz=10−sinθ0cosϕcosθsinϕ0−sinϕcosθcosϕϕ˙θ˙ψ˙.Not only each rotor of the aerial tree-pruning robot produces a forcefi, i=1,2,…,6 in the positive direction of xb or zb, but also the propellers generate reaction torques Mi, i=1,2,…,6 which are perpendicular to the plane of rotor blade rotation as shown below:(5)Mi=cMiωi2,where cMi is the coefficient of the reaction torque from the ith propeller. ωi is the rotate speed of the ith propeller.As shown in Figure1, Rotors 1 and 3 rotate to the positive zb direction, resulting in moments that act in the direction of the negative zb. Meanwhile, Rotors 2 and 4 rotate to the negative direction, which causes the reaction torques to the positive zb 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 matrixI with respect to the center of mass and the robot body coordinate B, the rotational dynamics of the body-fixed frame could be obtained.(6)Iω˙xω˙yω˙z=L2f1+f2−f3−f4+M5−M6L2f1−f2−f3+f4+LBstf5+f6−M1+M2−M3+M4−ωxωyωz×Iωxωyωz,where L is the distance between two adjacent lifting rotors which are arranged in a square. LBst is the distance between the center of mass and the boosters (Rotors 5 and 6) along the zb axis. I refers to the moment of inertia matrix referenced to the center of mass.


### 2.2. 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:(7)x˙1=x2,x˙2=Fx+ΔFx+Gx+ΔGxu+dt,y=x1,where x=x1,x2T, x1=ϕ,θ,ψT, x2=ϕ˙,θ˙,ψ˙T, and u=f1,f2,f3,f4,f5,f6T. ΔF and ΔG are the system uncertainties, and dt is the unknown disturbance.


## 2.1. Robot Kinematics and Dynamics

The coordinate systems of the robot are built with two coordinates, the world inertial coordinateW and the robot body coordinate B as shown in Figure 2. The translational position of the robot body is denoted as world coordinate variable elevation, sway, and surge p=pxpypzT. The robot attitude Θ=ϕθψT is defined in the world coordinate, which is expressed by the Euler angles (roll, pitch, and yaw).Figure 2
The world coordinate systemsW and robot body coordinate system B.In the kinematics representation, transforming coordinates fromW to B is necessary. Therefore, the rotation matrix RWBϕ,θ,ψ is defined as(1)RWBϕ,θ,ψ=RzTψRyTθRxTϕ,where Rxϕ, Ryθ, and Rzψ are the rotation matrixes along their corresponding axis.The center of mass of the robot is described by the vectorp in the inertial coordinate system W. The total thrust produced by the six propellers in Figure 1 is the direct factor that causes the translation and attitude acceleration of the robot. The 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. The dynamics of translation in the inertial frame are described as(2)p¨=RWBϕ,θ,ψf−00g,where g is the acceleration generated by the Earth gravity and f is the vector sum of the rotor forces fi that act to the robot mass m.(3)f=∑i=16fi.The control inputs to the tree-pruning robot are the mass-normalized thrust of the rotor forces sumf, and the desired forces sum f are specified by the desired rotational rates along the body axes of the tree-pruning robotbω=ωxωyωzT, 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:(4)ωxωyωz=10−sinθ0cosϕcosθsinϕ0−sinϕcosθcosϕϕ˙θ˙ψ˙.Not only each rotor of the aerial tree-pruning robot produces a forcefi, i=1,2,…,6 in the positive direction of xb or zb, but also the propellers generate reaction torques Mi, i=1,2,…,6 which are perpendicular to the plane of rotor blade rotation as shown below:(5)Mi=cMiωi2,where cMi is the coefficient of the reaction torque from the ith propeller. ωi is the rotate speed of the ith propeller.As shown in Figure1, Rotors 1 and 3 rotate to the positive zb direction, resulting in moments that act in the direction of the negative zb. Meanwhile, Rotors 2 and 4 rotate to the negative direction, which causes the reaction torques to the positive zb 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 matrixI with respect to the center of mass and the robot body coordinate B, the rotational dynamics of the body-fixed frame could be obtained.(6)Iω˙xω˙yω˙z=L2f1+f2−f3−f4+M5−M6L2f1−f2−f3+f4+LBstf5+f6−M1+M2−M3+M4−ωxωyωz×Iωxωyωz,where L is the distance between two adjacent lifting rotors which are arranged in a square. LBst is the distance between the center of mass and the boosters (Rotors 5 and 6) along the zb axis. I refers to the moment of inertia matrix referenced to the center of mass.


## 2.2. 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:(7)x˙1=x2,x˙2=Fx+ΔFx+Gx+ΔGxu+dt,y=x1,where x=x1,x2T, x1=ϕ,θ,ψT, x2=ϕ˙,θ˙,ψ˙T, and u=f1,f2,f3,f4,f5,f6T. ΔF and ΔG are the system uncertainties, and dt is the unknown disturbance.


## 3. Model Predictive Control of the Aerial Tree-Pruning Robot

### 3.1. Model Predictive Control

In order to keep the research proceed, discretization process is conducted to the state-space model of the aerial pruning robot, which is shown below:(8)Xk+1=AXk+BUk,Yk=CXk,where Xk∈ℝn represents the pruning robot’s state of attitude, and Uk∈ℝ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. The core of the pruning robot MPC is to specify a cost function of the kth time step and find its minimum value. The cost function of the finite horizon optimal control problem is shown as(9)minUXHTLXH+∑p=1H−1XpTMXp+UpTNUp,(10)s.t.Xk+p+1|k=AXk+p|k+BUk+p|k,p=0,…,H−1,Xmin≤Xk+p|k≤Xmax,p=0,…,H−1,Umin≤Uk+p|k≤Umax,p=0,…,H−1,X0=Xt,where L∈ℝn, M∈ℝn, and N∈ℝ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. Xk+p|k denotes the predicted state vector at time k+p and is acquired by result from the sequence of input uk,uk+1,…,uk+p−1. The state maximum Xmax∈ℝn and the state minimum Xmin∈ℝn form the constraint of the state variable X. And, the constraint of the input variable U is similarly defined composed of the input maximum Umax∈ℝm and the input minimum Umin∈ℝm.The main idea of the model predictive control is as follows. A sequence of control inputsuk,uk+1,…,uk+p−1 are generated at the time step k, at which time the state of the pruning robot is Xk. And by assigning Xk 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. This 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.


### 3.2. Alternating Direction Method of Multipliers (ADMM)

The alternating direction method of multipliers (ADMM) is a brilliant method that solves the problems of convex optimization. The 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].The standard form of optimizing problems that ADMM could effectively handle could be presented as(11)minfUs.t.U∈W,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,(12)minfX+hYs.t.AX+BY=n,where W has an indicator function h, and the constraint becomes the subtraction of a primal variable AX and a separate variable BY−n. Then, the augmented Lagrangian equation which is an obvious element of ADMM is assigned as(13)LρX,Y,n=fX+hY+ρ2AX−BY+n22,where a scaled dual variable n is introduced and ρ acts as a positive penalty parameter. The 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. Thus, in order to accomplish minimality, ADMM tactfully have the global problem decomposed into two separate optimization subproblems. The mathematical expression of the above process could be shown as(14)Xp+1≔argminfX+ρ2AX+BYp−n+up22,Yp+1≔argminhX+ρ2AXp+1+BY−n+up22,up+1≔up+AXp+1+BYp+1−n.For the beginning, several steps of the robot model, we have the state equations as(15)Xk+1=AXk+BUk,Xk+2=AXk+1+BUk+1=ΑAXk+BUk+BUk+1=A2Xk+ABUk+BUk+1,Xk+3=AXk+2+BUk+2=AA2Xk+ABUk+BUk+1+BUk+2=A3Xk+A2BUk+ABUk+1+BUk+2….So according to a deduction, the robot state at timek+p is could be reckoned as(16)Xk+p=ApXk+∑m=0p−1AmBUk+p−1−m.To transform the objective function into a typical QP form, the elements in theXk+p deduction formula could be expanded as follows:(17)Xk+p=ApXk+∑m=0p−1AmBUk+p−1−m=ApXk+Ap−1BUk+Ap−2BUk+1+⋯+ABUk+H−2+BUk+H−1.LetTp=Ap−1B,Ap−2B,…,B and U=Uk,Uk+1,…,Uk+H−1; then,(18)ApXk+Ap−1BUk+Ap−2BUk+1+⋯+ABUk+H−2+BUk+H−1=TpU.Thus the robot state is obtained as(19)Xk+p=ApXk+TpU.The transposition ofXk+p is(20)Xk+pT=XkTApT+UTTpT.ExpandXk+pTMXk+p using the above equation, we have(21)Xk+pTMXk+p=XkTApT+UTTpTMApXk+TpU=XkTApTM+UTTpTMApXk+TpU=XkTApTMApXk+XkTApTMTpU+UTTpTMApXk+UTTpTMTpU=XkTApTMApXk+2XkTApTMTpU+UTTpTMTpU=XkTrXt+2XkTgU+UTtU,where(22)r=ApTMAp,g=ApTMTp,t=TpTMTp.Similarly,XHTLXH could be rewritten using the Xt+pTMXt+p expansion method above. Therefore, we have the MPC optimization problem transformed to a QP problem whose objective function is presented as(23)JU,Xk=UTTU+2XkTGTU+XkTRXk,where(24)R=∑p=0H−1ApTMAp+AHTLAN,G=∑p=0H−1ApTMTp+AHTLTN,T=∑p=0H−1TpTMTp+TNTLTN+V,V=N⋱N.As for the constraints of the pruning robot’s MPC problem, there exists(25)Xmin≤Xp≤Xmax,p=0,…,H−1,Umin≤Up≤Umax,p=0,…,H−1.Also, a definition is made:(26)Tp=Ap−1B,Ap−2B,…,B,Xk+p=ApXk+TpU,Up=Uk,Uk+1,…,Uk+N−1.Take the equation ofXk+p above into(27)Xmin≤Xk+p≤Xmax,which becomes(28)Xmin≤ApXk+TpU≤Xmax.So by solving the inequation above,U is limited by(29)Tp−1Xmin−ApXk≤U≤Tp−1Xmax−ApXk,which is disassembled as a set of inequalities as(30)Up≤Tp−1Xmax−Tp−1ApXk,−Up≤−Tp−1Xmin+Tp−1ApXk.A combination is taken to the inequation group above, the QP constraints could be described as(31)DU≤E+QXk,where(32)D=1,−1T,E=Tp−1⊗Xmax,XminT,Q=Tp−1Ap,Tp−1ApT.The stop criterion of ADMM reaching convergence is obtained as(33)wp2≤εpri,zp2≤εdual.The algorithm of ADMM is shown as a pseudo code in Algorithm1.Algorithm 1: MPC optimization solving with ADMM.
InitializeX0,Y0,U0;RepeatXp+1:=argminfX+ρ/2AX+BYp−n+up22;Yp+1:=argminhX+ρ/2AXp+1+BY−n+up22;up+1:=up+AXp+1+BYp+1−n;p=p+1;Untilwp2≤εpri;zp2≤εdual;Return: optimal control input U∗;


## 3.1. Model Predictive Control

In order to keep the research proceed, discretization process is conducted to the state-space model of the aerial pruning robot, which is shown below:(8)Xk+1=AXk+BUk,Yk=CXk,where Xk∈ℝn represents the pruning robot’s state of attitude, and Uk∈ℝ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. The core of the pruning robot MPC is to specify a cost function of the kth time step and find its minimum value. The cost function of the finite horizon optimal control problem is shown as(9)minUXHTLXH+∑p=1H−1XpTMXp+UpTNUp,(10)s.t.Xk+p+1|k=AXk+p|k+BUk+p|k,p=0,…,H−1,Xmin≤Xk+p|k≤Xmax,p=0,…,H−1,Umin≤Uk+p|k≤Umax,p=0,…,H−1,X0=Xt,where L∈ℝn, M∈ℝn, and N∈ℝ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. Xk+p|k denotes the predicted state vector at time k+p and is acquired by result from the sequence of input uk,uk+1,…,uk+p−1. The state maximum Xmax∈ℝn and the state minimum Xmin∈ℝn form the constraint of the state variable X. And, the constraint of the input variable U is similarly defined composed of the input maximum Umax∈ℝm and the input minimum Umin∈ℝm.The main idea of the model predictive control is as follows. A sequence of control inputsuk,uk+1,…,uk+p−1 are generated at the time step k, at which time the state of the pruning robot is Xk. And by assigning Xk 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. This 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.


## 3.2. Alternating Direction Method of Multipliers (ADMM)

The alternating direction method of multipliers (ADMM) is a brilliant method that solves the problems of convex optimization. The 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].The standard form of optimizing problems that ADMM could effectively handle could be presented as(11)minfUs.t.U∈W,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,(12)minfX+hYs.t.AX+BY=n,where W has an indicator function h, and the constraint becomes the subtraction of a primal variable AX and a separate variable BY−n. Then, the augmented Lagrangian equation which is an obvious element of ADMM is assigned as(13)LρX,Y,n=fX+hY+ρ2AX−BY+n22,where a scaled dual variable n is introduced and ρ acts as a positive penalty parameter. The 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. Thus, in order to accomplish minimality, ADMM tactfully have the global problem decomposed into two separate optimization subproblems. The mathematical expression of the above process could be shown as(14)Xp+1≔argminfX+ρ2AX+BYp−n+up22,Yp+1≔argminhX+ρ2AXp+1+BY−n+up22,up+1≔up+AXp+1+BYp+1−n.For the beginning, several steps of the robot model, we have the state equations as(15)Xk+1=AXk+BUk,Xk+2=AXk+1+BUk+1=ΑAXk+BUk+BUk+1=A2Xk+ABUk+BUk+1,Xk+3=AXk+2+BUk+2=AA2Xk+ABUk+BUk+1+BUk+2=A3Xk+A2BUk+ABUk+1+BUk+2….So according to a deduction, the robot state at timek+p is could be reckoned as(16)Xk+p=ApXk+∑m=0p−1AmBUk+p−1−m.To transform the objective function into a typical QP form, the elements in theXk+p deduction formula could be expanded as follows:(17)Xk+p=ApXk+∑m=0p−1AmBUk+p−1−m=ApXk+Ap−1BUk+Ap−2BUk+1+⋯+ABUk+H−2+BUk+H−1.LetTp=Ap−1B,Ap−2B,…,B and U=Uk,Uk+1,…,Uk+H−1; then,(18)ApXk+Ap−1BUk+Ap−2BUk+1+⋯+ABUk+H−2+BUk+H−1=TpU.Thus the robot state is obtained as(19)Xk+p=ApXk+TpU.The transposition ofXk+p is(20)Xk+pT=XkTApT+UTTpT.ExpandXk+pTMXk+p using the above equation, we have(21)Xk+pTMXk+p=XkTApT+UTTpTMApXk+TpU=XkTApTM+UTTpTMApXk+TpU=XkTApTMApXk+XkTApTMTpU+UTTpTMApXk+UTTpTMTpU=XkTApTMApXk+2XkTApTMTpU+UTTpTMTpU=XkTrXt+2XkTgU+UTtU,where(22)r=ApTMAp,g=ApTMTp,t=TpTMTp.Similarly,XHTLXH could be rewritten using the Xt+pTMXt+p expansion method above. Therefore, we have the MPC optimization problem transformed to a QP problem whose objective function is presented as(23)JU,Xk=UTTU+2XkTGTU+XkTRXk,where(24)R=∑p=0H−1ApTMAp+AHTLAN,G=∑p=0H−1ApTMTp+AHTLTN,T=∑p=0H−1TpTMTp+TNTLTN+V,V=N⋱N.As for the constraints of the pruning robot’s MPC problem, there exists(25)Xmin≤Xp≤Xmax,p=0,…,H−1,Umin≤Up≤Umax,p=0,…,H−1.Also, a definition is made:(26)Tp=Ap−1B,Ap−2B,…,B,Xk+p=ApXk+TpU,Up=Uk,Uk+1,…,Uk+N−1.Take the equation ofXk+p above into(27)Xmin≤Xk+p≤Xmax,which becomes(28)Xmin≤ApXk+TpU≤Xmax.So by solving the inequation above,U is limited by(29)Tp−1Xmin−ApXk≤U≤Tp−1Xmax−ApXk,which is disassembled as a set of inequalities as(30)Up≤Tp−1Xmax−Tp−1ApXk,−Up≤−Tp−1Xmin+Tp−1ApXk.A combination is taken to the inequation group above, the QP constraints could be described as(31)DU≤E+QXk,where(32)D=1,−1T,E=Tp−1⊗Xmax,XminT,Q=Tp−1Ap,Tp−1ApT.The stop criterion of ADMM reaching convergence is obtained as(33)wp2≤εpri,zp2≤εdual.The algorithm of ADMM is shown as a pseudo code in Algorithm1.Algorithm 1: MPC optimization solving with ADMM.
InitializeX0,Y0,U0;RepeatXp+1:=argminfX+ρ/2AX+BYp−n+up22;Yp+1:=argminhX+ρ/2AXp+1+BY−n+up22;up+1:=up+AXp+1+BYp+1−n;p=p+1;Untilwp2≤εpri;zp2≤εdual;Return: optimal control input U∗;


## 4. Simulation and Experiment Results

The 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. Therefore, 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. The 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. The simulation results are then shown about the pruning operation where the contact forces generated between the saws and tree branches are significant 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. Table1 gives the physical information including kinematic variables. The reference trajectories of the robot attitude are specified as(34)ϕdθdψd=10⋅sint−0.5π10⋅sin2t+0.5π2⋅sint.Table 1
Simulation parameters of the tree-pruning robot system.
SymbolsDescriptionsValuesmThe mass of the robot15.97kgJxMoment of inertia aboutOx axis0.46kg⋅m2JyMoment of inertia aboutOy axis7.35kg⋅m2JzMoment of inertia aboutOz axis7.08kg⋅m2gAcceleration of gravity9.76m/s2kfLift coefficient6.33×10−5N/rad2kmAntitorque coefficient6.1×10−7N⋅m/rad2JPGyro moment coefficient5.1×10−5N⋅m/rad2ρAir density1.293kg/m3cCoefficient of air resistance2.92Under the developed control scheme, the simulation results for aerial tree-pruning robot are shown in Figures3–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.Figure 3
The tracking control result of pitch angle of the aerial tree-pruning robot.Figure 4
The tracking control result of roll angle of the aerial tree-pruning robot.Figure 5
The tracking control result of yaw angle of the aerial tree-pruning robot.Figure 6
The angular velocity of robot body along three axes.Figure 7
The disturbances from cutting forces along three axes. (a) Pulse disturbances from cutting forces. (b) Integral of the disturbances.
(a)(b)Figures3–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. Therefore, 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. Figure8 shows the cost function values of each step during the control process. The 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.Figure 8
The cost function values of each step.The running time of optimization in each step is one of the most common variables that could embody the optimization performance. Figure9 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 9
The running time of each step.Figure10 shows the iteration number of each step of the simulation. The 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 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.Figure 10
The iteration number of each step.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. The simulation result is shown in Figure11, which presents the comparison of the attitude trajectory tracking before and after the robot moment of inertia increases.Figure 11
Tracking performance of increased moment of inertia.
(a)(b)(c)The 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 Figure11, 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.


## 5. Conclusion

This 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. The future research is going to consider a more calculation efficient solving method for aerial tree-pruning robot with operational external disturbances.



---

*Source: 9981123-2021-06-29.xml*