End-Effector Position Tracking Control for an Aerial Manipulator with a Lightweight Manipulator

An end-eﬀector position tracking control task for an aerial manipulator is usually constituted by two subtasks. The ﬁrst is motion control, and the second is coordinate control so that the end-eﬀector of the aerial manipulator can precisely track the given trajectory. This paper proposes a novel end-eﬀector position tracking control approach for the aerial manipulator with a lightweight manipulator to achieve these two subtasks. The motion control of the aerial manipulator is solved by a partially coupled approach and divided into a multirotor controller and a manipulator controller. The multirotor controller is designed by the adaptive neural network control, while joints of the manipulator are steered by PID controllers. By resort-ing to radial basis function neural networks with adaptive weight estimation laws, the dynamic coupling between the multirotor and the manipulator can be com-pensated in real time. With the support of Lyapunov stability criteria, it is proved that the desired trajectories can be boundedly tracked by the multirotor under the proposed controller. Then, a new coordinate control method is proposed based on the linear model predictive control method. This method ensures that the solution satisﬁes physical limits of the aerial manipulator and can be executed in real time. Simulations demonstrate that the proposed motion controller signiﬁcantly outperforms a baseline nonlinear motion controller in the simulation cases. Besides, comparisons among the proposed coordinate control method and


Introduction
Aerial manipulator integrates capabilities of the multirotor and the manipulator, which can extend the scope of potential applications. Since the payload of the multirotor is limited, the aerial manipulator with a lightweight manipulator has received much attention in recent years. The lightweight manipulator generally equips light servos (e.g., Dynamixel servos [1], Herkulex DRS-0101 [2] and DRS-0602 [3]). Limitations of these servos affect the implementation of control methods in terms of control capabilities, available feedback signals and update rate. In the aerial manipulator, the end-effector is the end part of the manipulator that interacts with the environment. The role of the end-effector position tracking control is significant. Totally, it consists of two subtasks. The first is motion control, which is to steer the multirotor and the manipulator from the current positions to the desired positions under the dynamic coupling. The second is coordinate control, which is to coordinate motions of the multirotor and the manipulator such that the end-effector can precisely track the desired trajectory.
These existing methods to design the motion controller of the aerial manipulator can be classified into decoupled, coupled and partially coupled methods (see [4] and [5] for recent surveys). Decoupled methods treat the multirotor and the manipulator as two subsystems that are independently steered by utilizing the existing control algorithms [6][7][8]. Decoupled methods ignore the influence of the manipulator over the dynamics of the multirotor, which motivates the development of lightweight manipulators [9]. However, as soon as the motion of the manipulator is more demanding on accelerations, decoupled methods may achieve large tracking errors. Coupled methods treat the multirotor and the manipulator as a unique entity [10]. The design of a coupled controller relies on the full dynamic model of the aerial manipulator. Therefore, coupled methods perform better in compensating dynamic couplings and allow better performances in terms of accuracy and stability. Based on this viewpoint, the backstepping control [11], adaptive control [1], the multi-stage model predictive control [12] have been studied to develop the coupled motion controller for the aerial manipulator. Coupled methods become complicated when the number of joints increases. These methods require torque controlled motors that are unfeasible for aerial manipulators usually built with conventional servo actuators [3]. Besides, update rates of light servo actuators are usually lower than 100 Hz [13], which is another barrier to implementing coupled methods. Partially coupled methods treat the multirotor and the manipulator as two subsystems, but the controllers exploit the information of the multirotor and the manipulator to compensate the dynamic coupling [14]. The work in [8] designed a sliding battery box to compensate the dynamic coupling based on the estimate results. The performance of this method is limited by the sliding velocity of the battery box. Besides, this method also needs extra mechanical structure. Motivated by these limitations, researchers have proposed some approaches to compensate the dynamic coupling by the capability of the multirotor. For example, a multi-layer architecture has been proposed in [8] and a variable parameter integral backstepping controller has been designed in [15]. Works in [16] and [17] introduced disturbance observers into the motion controller to compensate the dynamic couplings. However, these observers rely on the mathematical model of the dynamic coupling.
The coordinate control of the aerial manipulator is usually treated as an inverse kinematics problem. For example, the works in [11] and [18] introduced the closed-loop inverse kinematics (CLIK) method into the coordinate control of the aerial manipulator. To improve the performance of the coordinate control, the null space-based method and compound behaviors are used in solving the coordinate control problem [19]. Besides, a multiple task-priority inverse kinematics algorithm is introduced to solve the coordinate controller problem for a dual-arm aerial manipulator [20]. How-ever, these methods solve the coordinate control problem with respect to the current state, and they are not able to consider the future state. The work in [21] introduced a nonlinear model predictive control (NMPC) method and treated the coordinate control problem as a constraint optimization problem. With the predictive model, the solution of the NMPC method can solve the optimization problem with respect to the current information and the prediction of the future. Due to the high computational cost of the NMPC method, it takes quite some time to calculate the solution, which might be a barrier to real-time applications.
To overcome the difficulties of the motion control, we propose a novel partially coupled motion controller based on the adaptive neural network (NN) control method that is widely used in solving uncertain nonlinear system control problems [22,23]. The adaptive radial basis function neural networks (RBFNNs) improve the accuracy of estimation with online training [24]. The proposed method compensates the dynamic coupling in real time and does not rely on the accuracy model of the dynamic coupling. To overcome the difficulties of the coordinate control, we propose a novel coordinate controller based on the linear model predictive control (MPC) method. The physical limitations of the aerial manipulator is encoded as constraints of a quadratic program (QP) problem, which ensures that the solution satisfies these limitations. Besides, this method can solve the coordinate problem in real time with the current information and the prediction of the future.
As stated above, the proposed end-effector position tracking controller has a two-layer architecture. The inner layer is the proposed motion controller and the outer layer is the proposed coordinate controller. The main contributions of this paper are fourfold. First, a partially coupled motion controller based on the aerial NN control method is proposed to solve the motion control of the aerial manipulator. To the best of our knowledge, this is the first time that the adaptive NN control method has been applied on the motion control of the aerial manipulator. Second, dynamic couplings of the aerial manipulator are estimated by adaptive RBFNNs via the proposed adaptive weight estimation law. The weights of the adaptive RBFNNs can be trained online. Third, stability analysis proves that tracking errors of the closed-loop multirotor system converge to a small neighborhood of the origin with assuming that the dynamic couplings are continuous. Fourth, we address the coordinate control of the aerial manipulator as a QP problem. And, a linear MPC method is applied to calculate the solution with respect to the given task. Compared with the NMPC method, the linear MPC needs less computing resources. It can be solved efficiently by existing QP solvers (e.g., CVXGEN [25]), which means it can be executed in some real-time applications. Compared with the CLIK methods, the proposed coordinate method shows better effectiveness and performance. To the best of our knowledge, this is the first time that the linear MPC method has been applied on the coordinate control of the aerial manipulator.
The remainder of this paper is structured as follows. Plant description and preliminaries are given in Section 2. Section 3 proposes the adaptive NN control method to design the multirotor controller. The linear MPC method is introduced to solve the coordinate control problem of the aerial manipulator in Section 4. Section 5 shows the proposed control structure for the endeffector position tracking control. Then, the simulation results for the proposed method are given in Section 6. Conclusions are drawn in Section 7.

Plant description and preliminaries
This section presents a mathematical model of the aerial manipulator and some preliminary results that will be used throughout this paper.

Kinematics
The aerial manipulator studied in this paper consists of a multirotor and a manipulator (see Fig. 1). The manipulator consists of a number of rigid links connected with joints. The end-effector is the last link of the manipulator. Let Σ I denote the inertial frame. p i ∈ R 3 represents the position of the i-th link's mass center in Σ I . p = [x, y, z] T ∈ R 3 denotes the position of the multirotor in Σ I . Let Σ b denote the body frame of the multirotor. Let p b i ∈ R 3 represents the position of the i-th link's mass center in the body frame Σ b . Then, we have where R ∈ SO(3) is the rotation matrix mapping vectors expressed in Σ b into the vectors expressed in Σ I . p b i can be obtained by Denavit-Harternberg convention [26].
Let p b e ∈ R 3 denote the position of the end-effector T ∈ R n denotes the joint vector of the manipulator. Therefore, the position of the end-effector expressed in inertial frame Σ I is given as where p e ∈ R 3 denote the position of the end-effector in Σ I . p b e is a function of the manipulator's joint position vector q. Deriving (2) with respect to time yieldṡ where [·] × denotes the skew-symmetric matrix, ω ∈ R 3 is the angular velocity vector of the multirotor in Σ b , P b e ∈ R 3×n is the partial derivative matrix of p b e with respect to q.

Dynamics
Let Θ = [φ, θ, ψ] T ∈ R 3 denote Euler angle vector of the multirotor. The dynamic model of the aerial manipulator is derived by the Euler-Lagrange method [26]. It can be written as follows [27]: where ξ = p T , Θ T , q T T , B(ξ) ∈ R n×n is the symmetric and positive definite inertia matrix, C(ξ,ξ) ∈ R n×n is the Coriolis matrix, g(ξ) ∈ R n is the gravity term, f g ∈ R n is the generalized force vector. Dynamic couplings are determined by the motions of the multirotor and the manipulator [4]. However, servo actuators equiped by the lightweight manipulator generally can not provide enough signals to calculate the dynamic couplings [13]. From the practical point of view, the dynamic model (4) is divided into the multirotor and the manipulator models. The multirotor model with dynamic couplings can be written as follows [28]: where m b represents the mass of the multirotor, m i represents the mass of the i-th link, g ∈ R 3 is the acceleration vector of gravity, f ∈ R 3 is the control input vector of the position loop, g s (ξ) ∈ R 3 denotes the torque induced by the displacement of the manipulator on the multirotor, ∆ v and ∆ ω respectively represent the force and the torque induced by the velocity and the acceleration of the manipulator, The torque g s (ξ) can be obtained by the potential energy of the system. The potential energy of the system consists of the multirotor potential energy and the manipulator potential energy. It can be written as Then, we have

Radial basis function neural network
The RBFNN is usually used for modeling nonlinear systems since it can approximate any continuous function over a compact set.

Lemma 1 ( [29]) For any continuous function
where η ∈ S ⊂ R nη represents the neural network input vector, S represents a compact set, w = [w 1 , w 2 , · · · , w N ] T ∈ R N represent the weight vector, w * is the ideal weight vector, s = [s 1 (η), s 2 (η), · · · , s N (η)] T ∈ R N is the basis function vector of the RBFNN, N represents the node number of the neural network.
The structure of the RBFNN is given in Fig. 2. Gaussian functions are chosen to design basis functions, which can be written as where a i ∈ S is the center of the receptive field and b i is the width of the Gaussian function. The ideal weight vector w * is an artificial quantity required for analytical purposes. w * is defined as the value of w that minimizes |ǫ| for all η ∈ S. Then, we have 3 Adaptive NN controller design In this section, the adaptive NN control method is introduced in designing the position control and the attitude control of the multirotor to compensate dynamic couplings of the aerial manipulator.

Position control
Let p d ∈ R 3 denote the desired position. The position error of the multirotor is then defined asp = p − p d . To depict the desired velocity of the multirotor, we define where K p ∈ R 3×3 is a positive diagonal matrix. Then, the velocity error of the multirotor is defined asṽ = T denote the input vector of the position RBFNNs. Let W p ∈ R N ×3 and S p (η p ) ∈ R N ×3 respectively represent the weight matrix and the basis function matrix of the position RBFNNs. The position controller of the multirotor is defined as where K v ∈ R 3×3 is a positive diagonal matrix. The adaptive weight estimation law of the position RBFNNs is given aṡ where w p,i ∈ R N is the i-th column vector of W p , s p,i (η p ) ∈ R N is the i-th column vector of S p (η p ),ṽ i is the i-th element ofṽ, Γ p ∈ R N ×N is a positive diagonal matrix, µ p ∈ (0, 1) represents the learning rate.
Theorem 1 Assuming that ∆ v is continuous, if the position controller is designed as (14) with the adaptive weight estimation law (15), then the position errorp is bounded.
Proof Letw p,i = w p,i − w * p,i denote the error of the RBFNN weight vectors. A candidate Lyapunov functions is given as where r p = [p T ,ṽ T ,w T p,1 ,w T p,2 ,w T p,3 ] T . Let λ p,m and λ p,M denote the minimum and the maximum eigenvalues of Γ −1 p . Then, we have where k p,m = min(1, λ p,m ) and k p,M = max(1, λ p,M ).
Combining (5), (13) and (14), the time derivative of V p is given aṡ Substituting (10) and (15) into (18) yieldṡ By the completion of squares, we have Using Young's inequality, we havẽ Substituting (20) into (19), we can obtaiṅ where k 1 = min(λ Kp,m , λ Kv,m − 0.5, µ p /2), λ Kp,m is the minimum eigenvalue of K p , λ Kv,m is the minimum eigenvalue of K v . According to (17), (22) is rewritten aṡ where Because ǫ p is bounded, we can obtain σ p ≤ σ p,M . Using the comparison method [30], we can obtain Then, we can obtain According to the definition of r p , it can be obtained that which means that the position error is bounded with the dynamic couplings of the aerial manipulator.
Consequently, the desired total thust f and desired attitude angles can be computed as where f i is the i-th element of f .

Attitude control
Let Θ d ∈ R 3 denote the desired attitude of the multirotor. LetΘ = Θ − Θ d ∈ R 3 denote the attitude error of the multirotor. To depict the desired angular velocity of the multirotor, we define where K Θ ∈ R 3×3 is a positive diagonal matrix. Then, the angular velocity error of the multirotor is defined asω = ω − ω r . Let η a = [Θ T , ω T , q] T denote the input vector of the attitude RBFNNs. Let W a ∈ R N ×3 and S a (η a ) ∈ R N ×3 respectively represent the weight matrix and the basis function matrix of the attitude RBFNNs. The attitude controller of the multirotor is given as where K ω ∈ R 3×3 is a positive diagonal matrix. The adaptive weight estimation law of the attitude RBFNNs is given aṡ where w a,i ∈ R N is the i-th column vector of W a , s a,i (η a ) ∈ R N is the i-th column vector of S a (η a ),ω i is the i-th element ofω, Γ a ∈ R N ×N is a positive diagonal matrix, µ a ∈ (0, 1) represents the learning rate.
Theorem 2 Assuming that ∆ ω is continuous, if the position controller is designed as (30) with the adaptive weight estimation law (31), then the attitude errorΘ is bounded.
Proof Letw a,i = w a,i − w * a,i denote the error of the attitude RBFNNs weight vectors. A candidate function is given as where r a = [Θ T ,ω T ,w T a,1 ,w T a,2 ,w T a,3 ] T . Let λ a,m and λ a,M respectively denote the minimum and the maximum eigenvalues of Γ −1 a . Then, we have where k a,m = min(1, λ a,m ) and k a,M = max(1, λ a,M ). Combining (6), (29) and (30), the time derivative of V a is given aṡ Substituting (10) and (31) into (34) yieldṡ By the completion of squares, we have Using Young's inequality, we havẽ Substituting (20) into (35), we can obtaiṅ where k 2 = min(λ K Θ ,m , λ Kω,m − 0.5, µ a /2), λ K Θ ,m is the minimum eigenvalue of K Θ , λ Kω,m is the minimum eigenvalue of K ω . According to (33), (38) is rewritten aṡ where σ a = 1 2 ǫ a 2 + µa 2 3 i=1 w * T a,i w * a,i . Because ǫ a is bounded, we can obtain σ a ≤ σ a,M . Using the comparison method [30], we can obtain Then, we can obtain According to the definition of r a , it can be obtained that which means that the attitude error is bounded with the dynamic couplings of the aerial manipulator.

Coordinate control
In this section, we propose a coordinate control method to coordinate motions of the multirotor and the manipulator based on the linear MPC method.

Coordinate control problem
The coordinate control is to calculate desired motions of the multirotor and the manipulation with the given task. The differential kinematics equation (3) describes the relationship between the position of the end-effector and states of the system. In the aerial manipulator system, the multirotor is underactuated. Therefore, not all aerial manipulator states can be controlled. To address this issue, we define h = [p T , q T ] T as the controlled states vector. The coordinate control problem for the aerial manipulator can be formulated as to find reference values for the control vector h. Then, we havė where J = [E 3 , RP b e ] is the actuated Jacobian matrix of the aerial manipulator, E 3 is an identity matrix of order 3 × 3.
Besides, the aerial manipulator has physical limits, e.g., joint position limits, velocity limits and acceleration limits. These limits are also taken into account in solving the coordinate control problem. Generally, the physical limits of the aerial manipulator can be written as where the subscribe [·] m represents the lower boundary of the corresponding variable, the subscribe [·] M represents the upper boundary of the corresponding variable.

Linear MPC Method
The linear MPC method is introduced in solving the coordinate control problem of the aerial manipulator. The MPC is an advanced control technique used to control a plant while satisfying a set of constraints. It determines an optimal control input locally at each control update time, usually via a predictive model, by propagating input and state sequences forwards in time.
The state of the aerial manipulator predictive model is defined as x = [h,ḣ] T . Let y denote the velocity vector of actuated joints, i.e., y =ḣ. Define control input as u =ḧ. Therefore, the predictive model with constant step size ∆t can be formulated as where 0 3+n is a zero matrix of order (3 + n) × (3 + n), E 3+n is an identity matrix of order (3 + n) × (3 + n), ∆t is the time step of the predictive model.
Define position error of the end-effector asp e = p e − p d e , where p d e is the desired position of the endeffector. The coordinate control problem is mathematically formulated as a finite prediction horizon QP problem: x k+i+1|k ∈ U, i = 0, 1, . . . , n p − 1 where n p is the prediction horizon of the MPC method. n c denotes the control horizon, (·) k+i|k denotes predictive result at time t k+i , J k is the actuated Jacobian matrix at time t k , W u is a positive definite weighting matrix that prioritizes the different joints, U = [u T k|k , u T k+1|k , · · · , u T k+nc−1|k ] T denotes the predictive control vector,σ k+i is virtual control vector at time t k +i∆t and can be calculated bẏ where p d e,k+i is the desired position of end-effector at time t k+i , p e,k is the position of the end-effector at time t k .
An advantage of the above formulation is that it can be solved efficiently by existing numerical QP solving methods (e.g., CVXGEN [25]). Due to introducing a predictive model, the solution also ensures that the constraints will be satisfied in the future. At each control update step t k , the QP problem is solved with an input value to obtain U . Although the control horizon of this algorithm n c ≥ 1, only the first control action of the sequence is implemented to the multirotor controller and the manipulator controller. Then, we can obtain the reference values for the multirotor (p d k ,ṗ d k ,p d k ) and the manipulator (q d k ,q d k ,q d k ).

Control architecture
The whole control system consists of two layers (see Fig. 3). The inner layer is the proposed motion controller, and the outer layer is the proposed coordinate controller. Since the motion controller is partially decoupled, the motion controller can be divided into the multirotor and the manipulator controllers. The inputs of the system are the desired position of the end-effector p d e and the yaw angle of the multirotor ψ d . Under the giving p d e , the coordinate controller proposed in section 4 can calculate the multirotor position reference p d and the manipulator joint reference q d .
The adaptive NN multirotor controller proposed in section 3 consists of the position controller and attitude controller. The lightweight manipulator is attached to the multirotor body. Generally, all joints of the manipulator are controlled by servo actuators. Therefore, the manipulator controller discussed in this paper consists of several servo controllers that can steer joints of the manipulator from current angles to desired angles. The PID method is used for designing the servo controllers. Fig. 4 illustrates the structure of the PID servo controller.

Simulation
An aerial manipulator which consists of a DJI F450 multirotor and a manipulator is simulated (see Fig. 5). The manipulator is mounted at the bottom of the multirotor and has 2 DOF (see Fig. 5). Then, the joint position vector is written as q = [q 1 , q 2 ] T .
The aerial manipulator is simulated in Simscape which provides a multibody simulation environment for 3D mechanical systems in MATLAB [31]. Table 1 shows the masses, lengths, and physical bounds of the aerial manipulator.

Validation for the motion controller
The greatest challenge for the motion controller is the dynamic coupling between the multirotor and the manipulator. Therefore, simulations are introduced to illustrate the effectiveness of the multirotor controller when the manipulator is moving. In the simulations, the movement of the manipulator is set as Fig. 6.
A position tracking task is simulated to validate the proposed multirotor controller. From Fig. 7, one can conclude that the proposed method performs much better than the baseline controller. The mean position error vector of the baseline controller is (0.39 m, 0.27 m, 0.36 m). The mean position error vector of the proposed controller is (0.15 m, 0.17 m, 0.09 m). These results show the performance and robustness of the proposed method in the simulation.

Validation for the coordinate controller
Three cases are simulated to illustrate the effectiveness of the coordinate controller. The first case demonstrates the proposed method when the end-effector moves from a start point to an end point. The second case demonstrates the proposed method when the end-effector tracks a 3D trajectory. The third case demonstrates the proposed method when the end-effector tracks some complicated trajectories in a plane. To show the performance of the proposed scheme, comparisons among the CLIK method [18], the damped CLIK method [32], and the proposed method are introduced in the simulation cases. The gain matrix in these three methods is set as K e = diag(1.2, 1.2, 1.2). Damped ratios in the damped CLIK is set as 0.70. We let weight matrix W u = diag(1.5, 1.5, 1.5, 0.8, 0.8). The results of these simulation cases are shown as follows.  Fig. 8 shows the result end-effector trajectories of the three methods. The trajectory lengths and arriving times of results are given in Tab. 2. From Fig. 8 and Tab. 2, one can obtain that the results of the proposed method are more smooth than the results of the other two methods. The result trajectory length of the proposed method is 0.29 m. It is shorter than the trajectory lengths of the CLIK method and the damped CLIK method. The duration time from start point to end point of the proposed method is 11.68 s. It is also less than the duration times of the CLIK method and the damped CLIK method.
Case 2: In this case, the end-effector of the aerial manipulator tracks a 3D trajectory. The 3D trajectory     The simulation results of this case are depicted in Fig. 9. The maximum tracking error appears at the beginning of the simulation. And tracking error converges to a very small value after about 10s. We define   Case 3: In this case, the end-effector of the aerial manipulator tracks several plane trajectories. Fig. 10 shows the results of this case. The desired position of the end-effector p d e is given as Fig. 10. The desired position of the multirotor (x d , y d , z d ) and the desired joint angles of the manipulator (q d 1 , q d 2 ) are obtained by the proposed coordinate control method. As stated above, these simulations illustrate the effectiveness and performance of the proposed motion controller and the proposed coordinate controller.

Conclusion
This paper proposed a new approach to design the endeffector position tracking controller of the aerial manipulator with a lightweight manipulator. The whole control system consists of two layers. The inner layer is the motion controller, and the outer layer is the coordinate controller. In addition, this article presented several interesting results.
According to the characters of the lightweight manipulator, a partially coupled motion controller is designed by dividing the motion controller into the multirotor and manipulator controllers. The multirotor controller is designed by the adaptive NN method, and the joints of the manipulator are controlled by the PID controllers. Dynamic couplings of the aerial manipulator are estimated by RBFNNs with an adaptive weight estimation law. The weights of the RBFNNs are trained online. With assuming that the dynamic couplings are continuous, it is proved that the desired multirotor trajectories can be boundedly tracked under the proposed adaptive NN controller. The proposed motion controller is applicable to the aerial manipulator with a lightweight manipulator since this controller does not require torque controlled motors with high update rates and does not rely on the mathematical model of the dynamic couplings. Simulations show that the proposed method performs better than the baseline controller.
The coordinate controller is designed by the linear MPC method. the coordinate control problem is formulated as a QP problem with a finite time horizon. The physical limits of the aerial manipulator are considered as constraints of the QP problem. With the predictive model, the proposed controller can ensure that the solution satisfies the physical limits. Since the MPC method obtains the control signals by minimizing the cost function, solutions of this method show high performance in the simulation cases. Compared with the CLIK method and the damped CLIK method, the solution of this method performs better in terms of trajectory length and duration time. Compared with the NMPC method, this method spends less computation cost and shows more possibility to be executed in realtime applications.
There are several important topics for future research. For example, the multirotor control method presented in this paper may be further utilized as a controller by taking the manipulator model into account. It would be interesting and practical to solve these problems in future research.